From edd29d729ab045d262657331fd348bcffe33036e Mon Sep 17 00:00:00 2001 From: Irlan Date: Wed, 1 Mar 2017 23:42:48 -0300 Subject: [PATCH] finish SAT optimization, calculate correct old sweep transform, remove unecessary static body transform update --- examples/testbed/tests/multiple_shapes.h | 2 +- include/bounce/common/math/quat.h | 2 +- include/bounce/dynamics/body.h | 2 +- include/bounce/dynamics/shapes/shape.h | 1 + src/bounce/dynamics/body.cpp | 6 +-- .../contacts/collide/collide_hulls_cache.cpp | 42 +++++++++++++++++-- src/bounce/dynamics/contacts/mesh_contact.cpp | 1 + src/bounce/dynamics/world.cpp | 11 +++-- 8 files changed, 53 insertions(+), 14 deletions(-) diff --git a/examples/testbed/tests/multiple_shapes.h b/examples/testbed/tests/multiple_shapes.h index 544d407..76b1b9f 100644 --- a/examples/testbed/tests/multiple_shapes.h +++ b/examples/testbed/tests/multiple_shapes.h @@ -79,7 +79,7 @@ public: { b3BodyDef bd; bd.type = e_dynamicBody; - bd.angularVelocity.Set(0.0f, B3_PI, 0.0f); + bd.angularVelocity.Set(0.0f, 200.0f * B3_PI, 0.0f); b3Body* body = m_world.CreateBody(bd); diff --git a/include/bounce/common/math/quat.h b/include/bounce/common/math/quat.h index 40f1774..aa012f4 100644 --- a/include/bounce/common/math/quat.h +++ b/include/bounce/common/math/quat.h @@ -187,7 +187,7 @@ inline float b3Dot(const b3Quat& a, const b3Quat& b) return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w; } -// Conjugate of a quaternion. +// Conjugate of a quaternion (inverse if the quaternion is unit). inline b3Quat b3Conjugate(const b3Quat& q) { return b3Quat(-q.x, -q.y, -q.z, q.w); diff --git a/include/bounce/dynamics/body.h b/include/bounce/dynamics/body.h index 2b049cf..79a2489 100644 --- a/include/bounce/dynamics/body.h +++ b/include/bounce/dynamics/body.h @@ -267,8 +267,8 @@ private: // Destroy all joints connected to the body. void DestroyJoints(); - void SynchronizeShapes(); void SynchronizeTransform(); + void SynchronizeShapes(); // Check if this body should collide with another. bool ShouldCollide(const b3Body* other) const; diff --git a/include/bounce/dynamics/shapes/shape.h b/include/bounce/dynamics/shapes/shape.h index 6569040..ca1e750 100644 --- a/include/bounce/dynamics/shapes/shape.h +++ b/include/bounce/dynamics/shapes/shape.h @@ -139,6 +139,7 @@ protected: friend class b3Body; friend class b3Contact; friend class b3ContactManager; + friend class b3MeshContact; friend class b3ContactSolver; friend class b3List1; diff --git a/src/bounce/dynamics/body.cpp b/src/bounce/dynamics/body.cpp index e068474..035f09c 100644 --- a/src/bounce/dynamics/body.cpp +++ b/src/bounce/dynamics/body.cpp @@ -179,10 +179,8 @@ void b3Body::SynchronizeTransform() void b3Body::SynchronizeShapes() { - b3Transform xf1; - xf1.position = m_sweep.worldCenter0; - xf1.rotation = b3ConvertQuatToRot(m_sweep.orientation0); - + b3Transform xf1 = m_sweep.GetTransform(0.0f); + b3Transform xf2 = m_xf; b3Vec3 displacement = xf2.position - xf1.position; diff --git a/src/bounce/dynamics/contacts/collide/collide_hulls_cache.cpp b/src/bounce/dynamics/contacts/collide/collide_hulls_cache.cpp index a8e2e5c..5b8d9b5 100644 --- a/src/bounce/dynamics/contacts/collide/collide_hulls_cache.cpp +++ b/src/bounce/dynamics/contacts/collide/collide_hulls_cache.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include extern u32 b3_convexCacheHits; @@ -101,11 +102,44 @@ void b3RebuildFaceContact(b3Manifold& manifold, const b3Hull* hullA = sA->m_hull; const b3Hull* hullB = sB->m_hull; - b3FaceQuery query; - query.index = indexA; + const b3Body* bodyA = sA->GetBody(); + const b3Body* bodyB = sB->GetBody(); + + const b3Sweep& sweepA = bodyA->GetSweep(); + b3Quat q10 = sweepA.orientation0; + b3Quat q1 = sweepA.orientation; + + const b3Sweep& sweepB = bodyB->GetSweep(); + b3Quat q20 = sweepB.orientation0; + b3Quat q2 = sweepB.orientation; + + // Check if the relative orientation has changed. + // Here the second orientation seen by the first orientation. + // dp = p2 - p1 + // dq * q1 = q2 + // dq = inv(q1) * q2 + + // The old relative rotation. + // "q0(2) - q0(1)" + b3Quat dq0 = b3Conjugate(q10) * q20; + + // The new relative rotation. + // "q(2) - q(1)" + b3Quat dq = b3Conjugate(q1) * q2; - // @todo Use heuristic (relative orientation) to increase performance. - b3BuildFaceContact(manifold, xfA, hullA, xfB, hullB, query, flipNormal); + // Relative rotation between the new relative rotation and the old relative rotation. + // "dq(2) - dq0(1)" + b3Quat q = b3Conjugate(dq0) * dq; + + // Check the relative absolute cosine because + // we want to check orientation similarity. + const float32 kTol = 0.995f; + if (b3Abs(q.w) > kTol) + { + b3FaceQuery query; + query.index = indexA; + b3BuildFaceContact(manifold, xfA, hullA, xfB, hullB, query, flipNormal); + } } void b3CollideCache(b3Manifold& manifold, diff --git a/src/bounce/dynamics/contacts/mesh_contact.cpp b/src/bounce/dynamics/contacts/mesh_contact.cpp index 967df8f..40e783a 100644 --- a/src/bounce/dynamics/contacts/mesh_contact.cpp +++ b/src/bounce/dynamics/contacts/mesh_contact.cpp @@ -263,6 +263,7 @@ void b3MeshContact::Collide() b3TriangleHull hullB(v1, v2, v3); b3HullShape shapeB; + shapeB.m_body = bodyB; shapeB.m_hull = &hullB; shapeB.m_radius = B3_HULL_RADIUS; diff --git a/src/bounce/dynamics/world.cpp b/src/bounce/dynamics/world.cpp index df63af5..50a01f1 100644 --- a/src/bounce/dynamics/world.cpp +++ b/src/bounce/dynamics/world.cpp @@ -288,20 +288,25 @@ void b3World::Solve(float32 dt, u32 velocityIterations, u32 positionIterations) for (b3Body* b = m_bodyList.m_head; b; b = b->m_next) { - // If a body didn't participate on a island then it didn't move - // at all. + // If a body didn't participate on a island then it didn't move. if ((b->m_flags & b3Body::e_islandFlag) == 0) { continue; } - // Update shape AABBs. + if (b->m_type == e_staticBody) + { + continue; + } + + // Update shapes for broad-phase. b->SynchronizeShapes(); } // Notify the contacts the AABBs may have been moved. m_contactMan.SynchronizeShapes(); + // Find new contacts. m_contactMan.FindNewContacts(); } }