moved SAT code into collision module

include/bounce/collision/sat/sat.h

@@ -52,4 +52,57 @@ float32 b3Project(const b3Vec3& P1, const b3Vec3& E1, const b3Vec3& P2, const b3
 b3EdgeQuery b3QueryEdgeSeparation(const b3Transform& xf1, const b3Hull* hull1,
 	const b3Transform& xf2, const b3Hull* hull2);
 
+
+///////////////////////////////////////////////////////////////////////////////////////////////////
+
+enum b3SATCacheType
+{
+	e_separation,
+	e_overlap,
+	e_empty,
+};
+
+enum b3SATFeatureType
+{
+	e_edge1, // an edge on hull 1 and an edge on hull 2
+	e_face1, // a face on hull 1 and a vertex/edge/face on hull 2
+	e_face2, // a face on hull 2 and a vertex/edge/face on hull 1
+};
+
+// A cached feature pair is used to improve the performance 
+// of the SAT when called more than once. 
+struct b3SATFeaturePair
+{
+	b3SATCacheType state; // sat result
+	b3SATFeatureType type; // feature pair type
+	u32 index1; // feature index on hull 1
+	u32 index2; // feature index on hull 2
+};
+
+inline b3SATFeaturePair b3MakeFeaturePair(b3SATCacheType state, b3SATFeatureType type, u32 index1, u32 index2)
+{
+	b3SATFeaturePair pair;
+	pair.state = state;
+	pair.type = type;
+	pair.index1 = index1;
+	pair.index2 = index2;
+	return pair;
+}
+
+struct b3FeatureCache
+{
+	// Read the current state of the cache.
+	// Return e_unkown if neither a separation or penetration was detected.
+	b3SATCacheType ReadState(const b3Transform& xf1, const b3Hull* hull1,
+		const b3Transform& xf2, const b3Hull* hull2, float32 totalRadius);
+
+	b3SATCacheType ReadEdge(const b3Transform& xf1, const b3Hull* hull1,
+		const b3Transform& xf2, const b3Hull* hull2, float32 totalRadius);
+
+	b3SATCacheType ReadFace(const b3Transform& xf1, const b3Hull* hull1,
+		const b3Transform& xf2, const b3Hull* hull2, float32 totalRadius);
+	
+	b3SATFeaturePair m_featurePair;
+};
+
 #endif

include/bounce/dynamics/contacts/collide/collide.h

@@ -33,45 +33,6 @@ class b3MeshShape;
 
 struct b3Manifold;
 
-enum b3SATCacheType
-{
-	e_separation,
-	e_overlap,
-	e_empty,
-};
-
-struct b3SATFeaturePair
-{
-	enum Type
-	{
-		e_edge1, // an edge on hull 1 and an edge on hull 2
-		e_face1, // a face on hull 1 and a vertex/edge/face on hull 2
-		e_face2, // a face on hull 2 and a vertex/edge/face on hull 1
-	};
-
-	b3SATCacheType state; // sat result
-	Type type; // feature pair type
-	u32 index1; // feature index on hull 1
-	u32 index2; // feature index on hull 2
-};
-
-struct b3FeatureCache
-{
-	// Read the current state of the cache.
-	// Return e_unkown if neither a separation or penetration was detected.
-	b3SATCacheType ReadState(const b3Transform& xf1, const b3Hull* hull1,
-		const b3Transform& xf2, const b3Hull* hull2, float32 totalRadius);
-
-	b3SATCacheType ReadEdge(const b3Transform& xf1, const b3Hull* hull1,
-		const b3Transform& xf2, const b3Hull* hull2, float32 totalRadius);
-
-	b3SATCacheType ReadFace(const b3Transform& xf1, const b3Hull* hull1,
-		const b3Transform& xf2, const b3Hull* hull2, float32 totalRadius);
-
-	// We could increase the cache size (e.g. a feature pair of the last two frames).
-	b3SATFeaturePair m_featurePair;
-};
-
 // A convex cache contains information used to exploit temporal 
 // coherence of the contact generation algorithms between two shapes.
 struct b3ConvexCache

src/bounce/collision/sat/sat.cpp

@@ -171,3 +171,108 @@ b3EdgeQuery b3QueryEdgeSeparation(const b3Transform& xf1, const b3Hull* hull1,
 	out.separation = maxSeparation;
 	return out;
 }
+
+///////////////////////////////////////////////////////////////////////////////////////////////////
+
+b3SATCacheType b3FeatureCache::ReadState(
+	const b3Transform& xf1, const b3Hull* hull1,
+	const b3Transform& xf2, const b3Hull* hull2, float32 totalRadius)
+{
+	// If the cache was empty or flushed choose an arbitrary feature pair.
+	if (m_featurePair.state == b3SATCacheType::e_empty)
+	{
+		m_featurePair = b3MakeFeaturePair(b3SATCacheType::e_separation, b3SATFeatureType::e_face1, 0, 0);
+	}
+
+	switch (m_featurePair.type)
+	{
+	case b3SATFeatureType::e_edge1:
+	{
+		return ReadEdge(xf1, hull1, xf2, hull2, totalRadius);
+	}
+	case b3SATFeatureType::e_face1:
+	{
+		return ReadFace(xf1, hull1, xf2, hull2, totalRadius);
+	}
+	case b3SATFeatureType::e_face2:
+	{
+		return ReadFace(xf2, hull2, xf1, hull1, totalRadius);
+	}
+	default:
+	{
+		return b3SATCacheType::e_empty;
+	}
+	}
+}
+
+b3SATCacheType b3FeatureCache::ReadFace(
+	const b3Transform& xf1, const b3Hull* hull1,
+	const b3Transform& xf2, const b3Hull* hull2, float32 totalRadius)
+{
+	// Perform computations in the local space of the second hull.
+	b3Transform xf = b3MulT(xf2, xf1);
+	b3Plane plane = xf * hull1->GetPlane(m_featurePair.index1);
+	float32 separation = b3Project(hull2, plane);
+	if (separation > totalRadius)
+	{
+		return e_separation;
+	}
+	return e_overlap;
+}
+
+b3SATCacheType b3FeatureCache::ReadEdge(
+	const b3Transform& xf1, const b3Hull* hull1,
+	const b3Transform& xf2, const b3Hull* hull2, float32 totalRadius)
+{
+	u32 i = m_featurePair.index1;
+	u32 j = m_featurePair.index2;
+
+	// Query minimum separation distance and axis of the first hull planes.
+	// Perform computations in the local space of the second hull.
+	b3Transform xf = b3MulT(xf2, xf1);
+	b3Vec3 C1 = xf * hull1->centroid;
+
+	const b3HalfEdge* edge1 = hull1->GetEdge(i);
+	const b3HalfEdge* twin1 = hull1->GetEdge(i + 1);
+
+	B3_ASSERT(edge1->twin == i + 1 && twin1->twin == i);
+
+	b3Vec3 P1 = xf * hull1->GetVertex(edge1->origin);
+	b3Vec3 Q1 = xf * hull1->GetVertex(twin1->origin);
+	b3Vec3 E1 = Q1 - P1;
+
+	// The Gauss Map of edge 1.
+	b3Vec3 U1 = xf.rotation * hull1->GetPlane(edge1->face).normal;
+	b3Vec3 V1 = xf.rotation * hull1->GetPlane(twin1->face).normal;
+
+	const b3HalfEdge* edge2 = hull2->GetEdge(j);
+	const b3HalfEdge* twin2 = hull2->GetEdge(j + 1);
+
+	B3_ASSERT(edge2->twin == j + 1 && twin2->twin == j);
+
+	b3Vec3 P2 = hull2->GetVertex(edge2->origin);
+	b3Vec3 Q2 = hull2->GetVertex(twin2->origin);
+	b3Vec3 E2 = Q2 - P2;
+
+	// The Gauss Map of edge 2.
+	b3Vec3 U2 = hull2->GetPlane(edge2->face).normal;
+	b3Vec3 V2 = hull2->GetPlane(twin2->face).normal;
+
+	// Negate the Gauss Map 2 for account for the MD.
+	if (b3IsMinkowskiFace(U1, V1, -E1, -U2, -V2, -E2))
+	{
+		float32 separation = b3Project(P1, E1, P2, E2, C1);
+		if (separation > totalRadius)
+		{
+			return b3SATCacheType::e_separation;
+		}
+		else
+		{
+			return b3SATCacheType::e_overlap;
+		}
+	}
+
+	// We can't determine the cache type 
+	// therefore must run SAT.
+	return b3SATCacheType::e_empty;
+}

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

@@ -236,7 +236,7 @@ void b3CollideHulls(b3Manifold& manifold,
 		return;
 	}
 
-	const float32 kTol = 0.1f * B3_LINEAR_SLOP;
+	const float32 kTol = 0.05f * B3_LINEAR_SLOP;
 	if (edgeQuery.separation > b3Max(faceQuery1.separation, faceQuery2.separation) + kTol)
 	{
 		b3BuildEdgeContact(manifold, xf1, edgeQuery.index1, s1, xf2, edgeQuery.index2, s2);

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

@@ -139,7 +139,7 @@ static void b3RebuildFaceContact(b3Manifold& manifold,
 	// 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;
@@ -162,20 +162,17 @@ void b3CollideCache(b3Manifold& manifold,
 
 	const b3Hull* hull1 = s1->m_hull;
 	float32 r1 = s1->m_radius;
-	
+
 	const b3Hull* hull2 = s2->m_hull;
 	float32 r2 = s2->m_radius;
-	
+
 	float32 totalRadius = r1 + r2;
 
 	b3FaceQuery faceQuery1 = b3QueryFaceSeparation(xf1, hull1, xf2, hull2);
 	if (faceQuery1.separation > totalRadius)
 	{
 		// Write a separation cache.
-		cache->m_featurePair.state = b3SATCacheType::e_separation;
+		cache->m_featurePair = b3MakeFeaturePair(b3SATCacheType::e_separation, b3SATFeatureType::e_face1, faceQuery1.index, faceQuery1.index);
-		cache->m_featurePair.type = b3SATFeaturePair::e_face1;
-		cache->m_featurePair.index1 = faceQuery1.index;
-		cache->m_featurePair.index2 = faceQuery1.index;
 		return;
 	}
 
@@ -183,10 +180,7 @@ void b3CollideCache(b3Manifold& manifold,
 	if (faceQuery2.separation > totalRadius)
 	{
 		// Write a separation cache.
-		cache->m_featurePair.state = b3SATCacheType::e_separation;
+		cache->m_featurePair = b3MakeFeaturePair(b3SATCacheType::e_separation, b3SATFeatureType::e_face2, faceQuery2.index, faceQuery2.index);
-		cache->m_featurePair.type = b3SATFeaturePair::e_face2;
-		cache->m_featurePair.index1 = faceQuery2.index;
-		cache->m_featurePair.index2 = faceQuery2.index;
 		return;
 	}
 
@@ -194,25 +188,19 @@ void b3CollideCache(b3Manifold& manifold,
 	if (edgeQuery.separation > totalRadius)
 	{
 		// Write a separation cache.
-		cache->m_featurePair.state = b3SATCacheType::e_separation;
+		cache->m_featurePair = b3MakeFeaturePair(b3SATCacheType::e_separation, b3SATFeatureType::e_edge1, edgeQuery.index1, edgeQuery.index2);
-		cache->m_featurePair.type = b3SATFeaturePair::e_edge1;
-		cache->m_featurePair.index1 = edgeQuery.index1;
-		cache->m_featurePair.index2 = edgeQuery.index2;
 		return;
 	}
 
-	const float32 kTol = 0.1f * B3_LINEAR_SLOP;
+	const float32 kTol = 0.05f * B3_LINEAR_SLOP;
 
 	if (edgeQuery.separation > b3Max(faceQuery1.separation, faceQuery2.separation) + kTol)
 	{
 		b3BuildEdgeContact(manifold, xf1, edgeQuery.index1, s1, xf2, edgeQuery.index2, s2);
-		if(manifold.pointCount > 0)
+		if (manifold.pointCount > 0)
-		{	
+		{
-			// Write an overlap cache.
+			// Write an overlap cache.		
-			cache->m_featurePair.state = b3SATCacheType::e_overlap;
+			cache->m_featurePair = b3MakeFeaturePair(b3SATCacheType::e_overlap, b3SATFeatureType::e_edge1, edgeQuery.index1, edgeQuery.index2);
-			cache->m_featurePair.type = b3SATFeaturePair::e_edge1;
-			cache->m_featurePair.index1 = edgeQuery.index1;
-			cache->m_featurePair.index2 = edgeQuery.index2;		
 		}
 	}
 	else
@@ -220,136 +208,24 @@ void b3CollideCache(b3Manifold& manifold,
 		if (faceQuery1.separation + kTol > faceQuery2.separation)
 		{
 			b3BuildFaceContact(manifold, xf1, faceQuery1.index, s1, xf2, s2, false);
-			if(manifold.pointCount > 0)
+			if (manifold.pointCount > 0)
 			{
 				// Write an overlap cache.
-				cache->m_featurePair.state = b3SATCacheType::e_overlap;
+				cache->m_featurePair = b3MakeFeaturePair(b3SATCacheType::e_overlap, b3SATFeatureType::e_face1, faceQuery1.index, faceQuery1.index);
-				cache->m_featurePair.type = b3SATFeaturePair::e_face1;
-				cache->m_featurePair.index1 = faceQuery1.index;
-				cache->m_featurePair.index2 = faceQuery1.index;
 			}
 		}
 		else
 		{
 			b3BuildFaceContact(manifold, xf2, faceQuery2.index, s2, xf1, s1, true);
-			if(manifold.pointCount > 0)
+			if (manifold.pointCount > 0)
 			{
 				// Write an overlap cache.
-				cache->m_featurePair.state = b3SATCacheType::e_overlap;
+				cache->m_featurePair = b3MakeFeaturePair(b3SATCacheType::e_overlap, b3SATFeatureType::e_face2, faceQuery2.index, faceQuery2.index);
-				cache->m_featurePair.type = b3SATFeaturePair::e_face2;
-				cache->m_featurePair.index1 = faceQuery2.index;
-				cache->m_featurePair.index2 = faceQuery2.index;
 			}
 		}
 	}
 }
 
-b3SATCacheType b3FeatureCache::ReadState(
-	const b3Transform& xf1, const b3Hull* hull1,
-	const b3Transform& xf2, const b3Hull* hull2, float32 totalRadius)
-{
-	// If the cache was empty or flushed choose an arbitrary feature pair.
-	if (m_featurePair.state == b3SATCacheType::e_empty)
-	{
-		m_featurePair.state = b3SATCacheType::e_separation;
-		m_featurePair.type = b3SATFeaturePair::e_face1;
-		m_featurePair.index1 = 0;
-		m_featurePair.index2 = 0;
-	}
-
-	switch (m_featurePair.type)
-	{
-	case b3SATFeaturePair::e_edge1:
-	{
-		return ReadEdge(xf1, hull1, xf2, hull2, totalRadius);
-	}
-	case b3SATFeaturePair::e_face1:
-	{
-		return ReadFace(xf1, hull1, xf2, hull2, totalRadius);
-	}
-	case b3SATFeaturePair::e_face2:
-	{
-		return ReadFace(xf2, hull2, xf1, hull1, totalRadius);
-	}
-	default:
-	{
-		return b3SATCacheType::e_empty;
-	}
-	}
-}
-
-b3SATCacheType b3FeatureCache::ReadFace(
-	const b3Transform& xf1, const b3Hull* hull1,
-	const b3Transform& xf2, const b3Hull* hull2, float32 totalRadius)
-{
-	// Perform computations in the local space of the second hull.
-	b3Transform xf = b3MulT(xf2, xf1);
-	b3Plane plane = xf * hull1->GetPlane(m_featurePair.index1);
-	float32 separation = b3Project(hull2, plane);
-	if (separation > totalRadius)
-	{
-		return e_separation;
-	}
-	return e_overlap;
-}
-
-b3SATCacheType b3FeatureCache::ReadEdge(
-	const b3Transform& xf1, const b3Hull* hull1,
-	const b3Transform& xf2, const b3Hull* hull2, float32 totalRadius)
-{
-	u32 i = m_featurePair.index1;
-	u32 j = m_featurePair.index2;
-
-	// Query minimum separation distance and axis of the first hull planes.
-	// Perform computations in the local space of the second hull.
-	b3Transform xf = b3MulT(xf2, xf1);
-	b3Vec3 C1 = xf * hull1->centroid;
-
-	const b3HalfEdge* edge1 = hull1->GetEdge(i);
-	const b3HalfEdge* twin1 = hull1->GetEdge(i + 1);
-
-	B3_ASSERT(edge1->twin == i + 1 && twin1->twin == i);
-
-	b3Vec3 P1 = xf * hull1->GetVertex(edge1->origin);
-	b3Vec3 Q1 = xf * hull1->GetVertex(twin1->origin);
-	b3Vec3 E1 = Q1 - P1;
-
-	// The Gauss Map of edge 1.
-	b3Vec3 U1 = xf.rotation * hull1->GetPlane(edge1->face).normal;
-	b3Vec3 V1 = xf.rotation * hull1->GetPlane(twin1->face).normal;
-
-	const b3HalfEdge* edge2 = hull2->GetEdge(j);
-	const b3HalfEdge* twin2 = hull2->GetEdge(j + 1);
-
-	B3_ASSERT(edge2->twin == j + 1 && twin2->twin == j);
-
-	b3Vec3 P2 = hull2->GetVertex(edge2->origin);
-	b3Vec3 Q2 = hull2->GetVertex(twin2->origin);
-	b3Vec3 E2 = Q2 - P2;
-
-	// The Gauss Map of edge 2.
-	b3Vec3 U2 = hull2->GetPlane(edge2->face).normal;
-	b3Vec3 V2 = hull2->GetPlane(twin2->face).normal;
-
-	// Negate the Gauss Map 2 for account for the MD.
-	if (b3IsMinkowskiFace(U1, V1, -E1, -U2, -V2, -E2))
-	{
-		float32 separation = b3Project(P1, E1, P2, E2, C1);
-		if (separation > totalRadius)
-		{
-			return b3SATCacheType::e_separation;
-		}
-		else
-		{
-			return b3SATCacheType::e_overlap;
-		}
-	}
-
-	// We can't determine the cache type 
-	// therefore must run SAT.
-	return b3SATCacheType::e_empty;
-}
-
 extern u32 b3_convexCacheHits;
 
 void b3CollideHulls(b3Manifold& manifold,
@@ -359,16 +235,16 @@ void b3CollideHulls(b3Manifold& manifold,
 {
 	const b3Hull* hull1 = s1->m_hull;
 	float32 r1 = s1->m_radius;
-	
+
 	const b3Hull* hull2 = s2->m_hull;
 	float32 r2 = s2->m_radius;
-	
+
 	float32 totalRadius = r1 + r2;
 
 	// Read cache
 	b3SATCacheType state0 = cache->m_featurePair.state;
 	b3SATCacheType state1 = cache->ReadState(xf1, hull1, xf2, hull2, totalRadius);
-	
+
 	if (state0 == b3SATCacheType::e_separation &&
 		state1 == b3SATCacheType::e_separation)
 	{
@@ -376,23 +252,24 @@ void b3CollideHulls(b3Manifold& manifold,
 		++b3_convexCacheHits;
 		return;
 	}
-	else if (state0 == b3SATCacheType::e_overlap &&
+
-			 state1 == b3SATCacheType::e_overlap)
+	if (state0 == b3SATCacheType::e_overlap &&
+		state1 == b3SATCacheType::e_overlap)
 	{
 		// Try to rebuild or reclip the features.
 		switch (cache->m_featurePair.type)
 		{
-		case b3SATFeaturePair::e_edge1:
+		case b3SATFeatureType::e_edge1:
 		{
 			b3RebuildEdgeContact(manifold, xf1, cache->m_featurePair.index1, s1, xf2, cache->m_featurePair.index2, s2);
 			break;
 		}
-		case b3SATFeaturePair::e_face1:
+		case b3SATFeatureType::e_face1:
 		{
 			b3RebuildFaceContact(manifold, xf1, cache->m_featurePair.index1, s1, xf2, s2, false);
 			break;
 		}
-		case b3SATFeaturePair::e_face2:
+		case b3SATFeatureType::e_face2:
 		{
 			b3RebuildFaceContact(manifold, xf2, cache->m_featurePair.index1, s2, xf1, s1, true);
 			break;