From bd69458750eac2ae3b84c60ebdce15d1281057d1 Mon Sep 17 00:00:00 2001 From: Irlan Date: Tue, 11 Apr 2017 16:35:18 -0300 Subject: [PATCH] rename function, remove indirection, fix bug --- examples/testbed/framework/debug_draw.cpp | 4 +- .../tests/capsule_and_hull_collision_1.h | 4 +- .../tests/capsule_and_hull_collision_2.h | 4 +- examples/testbed/tests/collide_test.h | 4 +- examples/testbed/tests/distance_test.h | 4 +- examples/testbed/tests/shape_stack.h | 1 + include/bounce/common/geometry.h | 27 ---------- include/bounce/common/math/quat.h | 14 ++--- include/bounce/common/math/transform.h | 4 +- include/bounce/dynamics/body.h | 2 +- src/bounce/dynamics/body.cpp | 2 +- .../dynamics/contacts/collide/collide.cpp | 2 +- .../dynamics/contacts/contact_solver.cpp | 8 +-- src/bounce/dynamics/contacts/mesh_contact.cpp | 2 +- src/bounce/dynamics/joints/revolute_joint.cpp | 2 +- src/bounce/dynamics/shapes/mesh_shape.cpp | 53 ++++++++++++++++--- 16 files changed, 74 insertions(+), 63 deletions(-) diff --git a/examples/testbed/framework/debug_draw.cpp b/examples/testbed/framework/debug_draw.cpp index 09f943a..b54d8f3 100644 --- a/examples/testbed/framework/debug_draw.cpp +++ b/examples/testbed/framework/debug_draw.cpp @@ -64,7 +64,7 @@ b3Mat44 Camera::BuildProjectionMatrix() const b3Transform Camera::BuildWorldTransform() const { b3Transform xf; - xf.rotation = b3ConvertQuatToRot(m_q); + xf.rotation = b3ConvertQuatToMat(m_q); xf.position = (m_zoom * xf.rotation.z) - m_center; return xf; } @@ -78,7 +78,7 @@ b3Mat44 Camera::BuildWorldMatrix() const b3Transform Camera::BuildViewTransform() const { b3Transform xf; - xf.rotation = b3ConvertQuatToRot(m_q); + xf.rotation = b3ConvertQuatToMat(m_q); xf.position = (m_zoom * xf.rotation.z) - m_center; return b3Inverse(xf); } diff --git a/examples/testbed/tests/capsule_and_hull_collision_1.h b/examples/testbed/tests/capsule_and_hull_collision_1.h index 6c553b1..34d1243 100644 --- a/examples/testbed/tests/capsule_and_hull_collision_1.h +++ b/examples/testbed/tests/capsule_and_hull_collision_1.h @@ -25,14 +25,14 @@ public: CapsuleAndHullCollision1() { m_xfA.position.Set(0.0f, 0.0f, 0.0f); - m_xfA.rotation = b3ConvertQuatToRot(b3Quat(b3Vec3(0.0f, 0.0f, 1.0f), 0.55f * B3_PI)); + m_xfA.rotation = b3ConvertQuatToMat(b3Quat(b3Vec3(0.0f, 0.0f, 1.0f), 0.55f * B3_PI)); m_sA.m_centers[0].Set(1.0f, -1.0f, 0.0f); m_sA.m_centers[1].Set(0.0f, 1.0f, 0.0f); m_sA.m_radius = 2.0f; m_xfB.position.Set(0.f, 0.0f, 0.0f); - m_xfB.rotation = b3ConvertQuatToRot(b3Quat(b3Vec3(0.0f, 0.0f, 1.0f), 0.0f * B3_PI)); + m_xfB.rotation = b3ConvertQuatToMat(b3Quat(b3Vec3(0.0f, 0.0f, 1.0f), 0.0f * B3_PI)); b3Transform xf; xf.SetIdentity(); diff --git a/examples/testbed/tests/capsule_and_hull_collision_2.h b/examples/testbed/tests/capsule_and_hull_collision_2.h index f1b28b9..efaaee8 100644 --- a/examples/testbed/tests/capsule_and_hull_collision_2.h +++ b/examples/testbed/tests/capsule_and_hull_collision_2.h @@ -25,14 +25,14 @@ public: CapsuleAndHullCollision2() { m_xfA.position.Set(0.0f, 0.0f, 0.0f); - m_xfA.rotation = b3ConvertQuatToRot(b3Quat(b3Vec3(0.0f, 0.0f, 1.0f), 0.55f * B3_PI)); + m_xfA.rotation = b3ConvertQuatToMat(b3Quat(b3Vec3(0.0f, 0.0f, 1.0f), 0.55f * B3_PI)); m_sA.m_centers[0].Set(0.0f, 0.0f, 0.0f); m_sA.m_centers[1].Set(0.0f, 0.0f, 0.0f); m_sA.m_radius = 0.05f; m_xfB.position.Set(0.f, 0.0f, 0.0f); - m_xfB.rotation = b3ConvertQuatToRot(b3Quat(b3Vec3(0.0f, 0.0f, 1.0f), 0.0f * B3_PI)); + m_xfB.rotation = b3ConvertQuatToMat(b3Quat(b3Vec3(0.0f, 0.0f, 1.0f), 0.0f * B3_PI)); b3Transform xf; xf.SetIdentity(); diff --git a/examples/testbed/tests/collide_test.h b/examples/testbed/tests/collide_test.h index 7157dab..351999d 100644 --- a/examples/testbed/tests/collide_test.h +++ b/examples/testbed/tests/collide_test.h @@ -92,7 +92,7 @@ public: if (key == GLFW_KEY_X) { b3Quat qx(b3Vec3(1.0f, 0.0f, 0.0f), 0.05f * B3_PI); - b3Mat33 xfx = b3ConvertQuatToRot(qx); + b3Mat33 xfx = b3ConvertQuatToMat(qx); m_xfB.rotation = m_xfB.rotation * xfx; } @@ -100,7 +100,7 @@ public: if (key == GLFW_KEY_Y) { b3Quat qy(b3Vec3(0.0f, 1.0f, 0.0f), 0.05f * B3_PI); - b3Mat33 xfy = b3ConvertQuatToRot(qy); + b3Mat33 xfy = b3ConvertQuatToMat(qy); m_xfB.rotation = m_xfB.rotation * xfy; } diff --git a/examples/testbed/tests/distance_test.h b/examples/testbed/tests/distance_test.h index 89f0848..ec2cc46 100644 --- a/examples/testbed/tests/distance_test.h +++ b/examples/testbed/tests/distance_test.h @@ -103,7 +103,7 @@ public: if (key == GLFW_KEY_X) { b3Quat qx(b3Vec3(1.0f, 0.0f, 0.0f), 0.05f * B3_PI); - b3Mat33 xfx = b3ConvertQuatToRot(qx); + b3Mat33 xfx = b3ConvertQuatToMat(qx); m_xfB.rotation = m_xfB.rotation * xfx; } @@ -111,7 +111,7 @@ public: if (key == GLFW_KEY_Y) { b3Quat qy(b3Vec3(0.0f, 1.0f, 0.0f), 0.05f * B3_PI); - b3Mat33 xfy = b3ConvertQuatToRot(qy); + b3Mat33 xfy = b3ConvertQuatToMat(qy); m_xfB.rotation = m_xfB.rotation * xfy; } diff --git a/examples/testbed/tests/shape_stack.h b/examples/testbed/tests/shape_stack.h index 85eaf65..a7b4dca 100644 --- a/examples/testbed/tests/shape_stack.h +++ b/examples/testbed/tests/shape_stack.h @@ -26,6 +26,7 @@ public: { { b3BodyDef bd; + bd.orientation.Set(b3Vec3(0.0f, 1.0f, 0.0f), 0.18f * B3_PI); b3Body* ground = m_world.CreateBody(bd); b3MeshShape ms; diff --git a/include/bounce/common/geometry.h b/include/bounce/common/geometry.h index 3a8c45c..98b0b21 100644 --- a/include/bounce/common/geometry.h +++ b/include/bounce/common/geometry.h @@ -100,33 +100,6 @@ inline void b3BarycentricCoordinates(float32 out[3], out[2] = divisor; } -// Convert a point Q from euclidean coordinates to barycentric coordinates (u, v, w) -// with respect to a triangle ABC. -// The last output value is the divisor. -inline void b3BarycentricCoordinates(float32 out[4], - const b3Vec3& A, const b3Vec3& B, const b3Vec3& C, - const b3Vec3& Q) -{ - b3Vec3 AB = B - A; - b3Vec3 AC = C - A; - - b3Vec3 QA = A - Q; - b3Vec3 QB = B - Q; - b3Vec3 QC = C - Q; - - b3Vec3 QB_x_QC = b3Cross(QB, QC); - b3Vec3 QC_x_QA = b3Cross(QC, QA); - b3Vec3 QA_x_QB = b3Cross(QA, QB); - - b3Vec3 AB_x_AC = b3Cross(AB, AC); - float32 divisor = b3Dot(AB_x_AC, AB_x_AC); - - out[0] = b3Dot(QB_x_QC, AB_x_AC); - out[1] = b3Dot(QC_x_QA, AB_x_AC); - out[2] = b3Dot(QA_x_QB, AB_x_AC); - out[3] = divisor; -} - // Project a point onto a segment AB. inline b3Vec3 b3ClosestPointOnSegment(const b3Vec3& P, const b3Vec3& A, const b3Vec3& B) { diff --git a/include/bounce/common/math/quat.h b/include/bounce/common/math/quat.h index 2a3493d..a8d7fa6 100644 --- a/include/bounce/common/math/quat.h +++ b/include/bounce/common/math/quat.h @@ -205,7 +205,7 @@ inline b3Quat b3Conjugate(const b3Quat& q) return b3Quat(-q.x, -q.y, -q.z, q.w); } -// Rotate a vector by an orientation quaternion. +// Rotate a vector. inline b3Vec3 b3Mul(const b3Quat& q, const b3Vec3& v) { b3Vec3 qv(q.x, q.y, q.z); @@ -215,14 +215,14 @@ inline b3Vec3 b3Mul(const b3Quat& q, const b3Vec3& v) return v + qs * t + b3Cross(qv, t); } -// Inverse rotate a vector by an orientation quaternion. +// Inverse rotate a vector. inline b3Vec3 b3MulT(const b3Quat& q, const b3Vec3& v) { return b3Mul(b3Conjugate(q), v); } -// Convert a 3-by-3 rotation matrix to an orientation quaternion. -inline b3Quat b3ConvertRotToQuat(const b3Mat33& m) +// Convert a 3-by-3 rotation matrix to an rotation quaternion. +inline b3Quat b3ConvertMatToQuat(const b3Mat33& m) { // Check the diagonal. float32 trace = m[0][0] + m[1][1] + m[2][2]; @@ -286,8 +286,8 @@ inline b3Quat b3ConvertRotToQuat(const b3Mat33& m) return result; } -// Convert an orientation quaternion to a 3-by-3 rotation matrix. -inline b3Mat33 b3ConvertQuatToRot(const b3Quat& q) +// Convert an rotation quaternion to a 3-by-3 rotation matrix. +inline b3Mat33 b3ConvertQuatToMat(const b3Quat& q) { float32 x = q.x, y = q.y, z = q.z, w = q.w; float32 x2 = x + x, y2 = y + y, z2 = z + z; @@ -301,4 +301,4 @@ inline b3Mat33 b3ConvertQuatToRot(const b3Quat& q) b3Vec3( xz + wy, yz - wx, 1.0f - (xx + yy))); } -#endif +#endif \ No newline at end of file diff --git a/include/bounce/common/math/transform.h b/include/bounce/common/math/transform.h index b4baf2f..a9a80eb 100644 --- a/include/bounce/common/math/transform.h +++ b/include/bounce/common/math/transform.h @@ -35,7 +35,7 @@ struct b3Transform b3Transform(const b3Vec3& p, const b3Quat& q) { position = p; - rotation = b3ConvertQuatToRot(q); + rotation = b3ConvertQuatToMat(q); } // Set this transform to the identity. @@ -75,7 +75,7 @@ inline b3Transform b3Sweep::GetTransform(float32 t) const q.Normalize(); b3Transform xf; - xf.rotation = b3ConvertQuatToRot(q); + xf.rotation = b3ConvertQuatToMat(q); xf.position = c - b3Mul(q, localCenter); return xf; } diff --git a/include/bounce/dynamics/body.h b/include/bounce/dynamics/body.h index e3fff4d..34de0b9 100644 --- a/include/bounce/dynamics/body.h +++ b/include/bounce/dynamics/body.h @@ -385,7 +385,7 @@ inline void b3Body::SetTransform(const b3Vec3& position, const b3Vec3& axis, flo b3Quat q = b3Quat(axis, angle); m_xf.position = position; - m_xf.rotation = b3ConvertQuatToRot(q); + m_xf.rotation = b3ConvertQuatToMat(q); m_sweep.worldCenter = b3Mul(m_xf, m_sweep.localCenter); m_sweep.orientation = q; diff --git a/src/bounce/dynamics/body.cpp b/src/bounce/dynamics/body.cpp index 035f09c..b3c82e9 100644 --- a/src/bounce/dynamics/body.cpp +++ b/src/bounce/dynamics/body.cpp @@ -77,7 +77,7 @@ b3Body::b3Body(const b3BodyDef& def, b3World* world) m_sweep.t0 = 0.0f; m_xf.position = m_sweep.worldCenter; - m_xf.rotation = b3ConvertQuatToRot(m_sweep.orientation); + m_xf.rotation = b3ConvertQuatToMat(m_sweep.orientation); m_gravityScale = def.gravityScale; m_userData = def.userData; diff --git a/src/bounce/dynamics/contacts/collide/collide.cpp b/src/bounce/dynamics/contacts/collide/collide.cpp index 0630b79..8eed610 100644 --- a/src/bounce/dynamics/contacts/collide/collide.cpp +++ b/src/bounce/dynamics/contacts/collide/collide.cpp @@ -184,4 +184,4 @@ void b3CollideShapeAndShape(b3Manifold& manifold, B3_ASSERT(CollideFunc); CollideFunc(manifold, xfA, shapeA, xfB, shapeB, cache); -} +} \ No newline at end of file diff --git a/src/bounce/dynamics/contacts/contact_solver.cpp b/src/bounce/dynamics/contacts/contact_solver.cpp index e80fb47..9ad5ffa 100644 --- a/src/bounce/dynamics/contacts/contact_solver.cpp +++ b/src/bounce/dynamics/contacts/contact_solver.cpp @@ -176,11 +176,11 @@ void b3ContactSolver::InitializeConstraints() b3Vec3 wB = m_velocities[indexB].w; b3Transform xfA; - xfA.rotation = b3ConvertQuatToRot(qA); + xfA.rotation = b3ConvertQuatToMat(qA); xfA.position = xA - b3Mul(xfA.rotation, localCenterA); b3Transform xfB; - xfB.rotation = b3ConvertQuatToRot(qB); + xfB.rotation = b3ConvertQuatToMat(qB); xfB.position = xB - b3Mul(xfB.rotation, localCenterB); for (u32 j = 0; j < manifoldCount; ++j) @@ -532,11 +532,11 @@ bool b3ContactSolver::SolvePositionConstraints() b3PositionConstraintPoint* pcp = pcm->points + k; b3Transform xfA; - xfA.rotation = b3ConvertQuatToRot(qA); + xfA.rotation = b3ConvertQuatToMat(qA); xfA.position = cA - b3Mul(xfA.rotation, localCenterA); b3Transform xfB; - xfB.rotation = b3ConvertQuatToRot(qB); + xfB.rotation = b3ConvertQuatToMat(qB); xfB.position = cB - b3Mul(xfB.rotation, localCenterB); b3ContactPositionSolverPoint cpcp; diff --git a/src/bounce/dynamics/contacts/mesh_contact.cpp b/src/bounce/dynamics/contacts/mesh_contact.cpp index 9b24dea..8a08b4d 100644 --- a/src/bounce/dynamics/contacts/mesh_contact.cpp +++ b/src/bounce/dynamics/contacts/mesh_contact.cpp @@ -72,7 +72,7 @@ void b3MeshContact::SynchronizeShapes() b3Sweep* sweepA = &bodyA->m_sweep; b3Transform xfA0; xfA0.position = sweepA->worldCenter0; - xfA0.rotation = b3ConvertQuatToRot(sweepA->orientation0); + xfA0.rotation = b3ConvertQuatToMat(sweepA->orientation0); // Calculate the displacement of body A using its position at the last // time step and the current position. diff --git a/src/bounce/dynamics/joints/revolute_joint.cpp b/src/bounce/dynamics/joints/revolute_joint.cpp index 8e0b212..1f513ba 100644 --- a/src/bounce/dynamics/joints/revolute_joint.cpp +++ b/src/bounce/dynamics/joints/revolute_joint.cpp @@ -180,7 +180,7 @@ void b3RevoluteJointDef::Initialize(b3Body* bA, b3Body* bB, rotation.y = b3Perp(axis); rotation.x = b3Cross(rotation.y, axis); - b3Quat q = b3ConvertRotToQuat(rotation); + b3Quat q = b3ConvertMatToQuat(rotation); float32 len = q.Normalize(); B3_ASSERT(len > B3_EPSILON); diff --git a/src/bounce/dynamics/shapes/mesh_shape.cpp b/src/bounce/dynamics/shapes/mesh_shape.cpp index 2b1a0f0..6cd57a0 100644 --- a/src/bounce/dynamics/shapes/mesh_shape.cpp +++ b/src/bounce/dynamics/shapes/mesh_shape.cpp @@ -62,7 +62,15 @@ void b3MeshShape::ComputeAABB(b3AABB3* output, const b3Transform& xf) const void b3MeshShape::ComputeAABB(b3AABB3* output, const b3Transform& xf, u32 index) const { - *output = m_mesh->GetTriangleAABB(index); + B3_ASSERT(index < m_mesh->triangleCount); + const b3Triangle* triangle = m_mesh->triangles + index; + b3Vec3 v1 = b3Mul(xf, m_mesh->vertices[triangle->v1]); + b3Vec3 v2 = b3Mul(xf, m_mesh->vertices[triangle->v2]); + b3Vec3 v3 = b3Mul(xf, m_mesh->vertices[triangle->v3]); + + output->m_lower = b3Min(b3Min(v1, v2), v3); + output->m_upper = b3Max(b3Max(v1, v2), v3); + output->Extend(m_radius); } bool b3MeshShape::TestPoint(const b3Vec3& point, const b3Transform& xf) const @@ -98,7 +106,7 @@ bool b3MeshShape::RayCast(b3RayCastOutput* output, const b3RayCastInput& input, float32 t = numerator / denominator; - // Is the intersection point on the segment? + // Is the intersection not on the segment? if (t < 0.0f || input.maxFraction < t) { return false; @@ -106,11 +114,32 @@ bool b3MeshShape::RayCast(b3RayCastOutput* output, const b3RayCastInput& input, b3Vec3 q = p1 + t * d; - float32 w[4]; - b3BarycentricCoordinates(w, v1, v2, v3, q); + // Barycentric coordinates for q + b3Vec3 Q = q; + b3Vec3 A = v1; + b3Vec3 B = v2; + b3Vec3 C = v3; - // Is the intersection point on the triangle? - if (w[0] > 0.0f && w[1] > 0.0f && w[2] > 0.0f) + b3Vec3 AB = B - A; + b3Vec3 AC = C - A; + + b3Vec3 QA = A - Q; + b3Vec3 QB = B - Q; + b3Vec3 QC = C - Q; + + b3Vec3 QB_x_QC = b3Cross(QB, QC); + b3Vec3 QC_x_QA = b3Cross(QC, QA); + b3Vec3 QA_x_QB = b3Cross(QA, QB); + + b3Vec3 AB_x_AC = b3Cross(AB, AC); + float32 den = b3Dot(AB_x_AC, AB_x_AC); + + float32 u = b3Dot(QB_x_QC, AB_x_AC); + float32 v = b3Dot(QC_x_QA, AB_x_AC); + float32 w = b3Dot(QA_x_QB, AB_x_AC); + + // Is the intersection on the triangle? + if (u > 0.0f && v > 0.0f && w > 0.0f) { output->fraction = t; @@ -132,8 +161,10 @@ bool b3MeshShape::RayCast(b3RayCastOutput* output, const b3RayCastInput& input, struct b3MeshRayCastCallback { - float32 Report(const b3RayCastInput& input, u32 proxyId) + float32 Report(const b3RayCastInput& subInput, u32 proxyId) { + B3_NOT_USED(subInput); + u32 childIndex = mesh->m_mesh->tree.GetUserData(proxyId); b3RayCastOutput childOutput; @@ -150,6 +181,7 @@ struct b3MeshRayCastCallback return 1.0f; } + b3RayCastInput input; const b3MeshShape* mesh; b3Transform xf; @@ -160,12 +192,17 @@ struct b3MeshRayCastCallback bool b3MeshShape::RayCast(b3RayCastOutput* output, const b3RayCastInput& input, const b3Transform& xf) const { b3MeshRayCastCallback callback; + callback.input = input; callback.mesh = this; callback.xf = xf; callback.hit = false; callback.output.fraction = B3_MAX_FLOAT; - m_mesh->tree.RayCast(&callback, input); + b3RayCastInput subInput; + subInput.p1 = b3MulT(xf, input.p1); + subInput.p2 = b3MulT(xf, input.p2); + subInput.maxFraction = input.maxFraction; + m_mesh->tree.RayCast(&callback, subInput); output->fraction = callback.output.fraction; output->normal = callback.output.normal;