finish SAT optimization, calculate correct old sweep transform, remove unecessary static body transform update

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);
 

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);

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;

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<b3Shape>;
 

src/bounce/dynamics/body.cpp

@@ -179,9 +179,7 @@ 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;
 	

src/bounce/dynamics/contacts/collide/collide_hulls_cache.cpp

@@ -20,6 +20,7 @@
 #include <bounce/dynamics/contacts/collide/clip.h>
 #include <bounce/dynamics/contacts/manifold.h>
 #include <bounce/dynamics/shapes/hull_shape.h>
+#include <bounce/dynamics/body.h>
 #include <bounce/collision/shapes/hull.h>
 
 extern u32 b3_convexCacheHits;
@@ -101,12 +102,45 @@ void b3RebuildFaceContact(b3Manifold& manifold,
 	const b3Hull* hullA = sA->m_hull;
 	const b3Hull* hullB = sB->m_hull;
 
+	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;
+	
+	// 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;
-	
-	// @todo Use heuristic (relative orientation) to increase performance.
 		b3BuildFaceContact(manifold, xfA, hullA, xfB, hullB, query, flipNormal);
 	}
+}
 
 void b3CollideCache(b3Manifold& manifold,
 	const b3Transform& xfA, const b3HullShape* sA,

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;
 

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();
 	}
 }