Performance optimization and improved accuracy of GetClosestPointOnTetrahedron (#417)

This should fix #419 and #363.

.github/workflows/determinism_check.yml

@@ -1,8 +1,8 @@
 name: Determinism Check
 
 env:
-    CONVEX_VS_MESH_HASH: '0xd956e5d4fe63b095'
-    RAGDOLL_HASH: '0xb874a5f0c92ba9c3'
+    CONVEX_VS_MESH_HASH: '0x4a775b24d5f4699'
+    RAGDOLL_HASH: '0x7f60101bb58d2677'
 
 on:
   push:

Jolt/Geometry/ClosestPoint.h

@@ -145,6 +145,8 @@ namespace ClosestPoint
 
 	/// Get the closest point to the origin of triangle (inA, inB, inC)
 	/// outSet describes which features are closest: 1 = a, 2 = b, 4 = c, 5 = line segment ac, 7 = triangle interior etc.
+	/// If MustIncludeC is true, the function assumes that C is part of the closest feature (vertex, edge, face) and does less work, if the assumption is not true then a closest point to the other features is returned.
+	template <bool MustIncludeC = false>
 	inline Vec3	GetClosestPointOnTriangle(Vec3Arg inA, Vec3Arg inB, Vec3Arg inC, uint32 &outSet)
 	{
 		// Taken from: Real-Time Collision Detection - Christer Ericson (Section: Closest Point on Triangle to Point)
@@ -171,33 +173,81 @@ namespace ClosestPoint
 		float n_len_sq = n.LengthSq();
 
 		// Check degenerate
-		if (n_len_sq < 1.0e-13f) // Square(FLT_EPSILON) was too small and caused numerical problems, see test case TestCollideParallelTriangleVsCapsule
+		if (n_len_sq < 1.0e-11f) // Square(FLT_EPSILON) was too small and caused numerical problems, see test case TestCollideParallelTriangleVsCapsule
 		{
-			// Degenerate, fallback to edges
+			// Degenerate, fallback to vertices and edges
 
-			// Edge AB
-			uint32 closest_set;
-			Vec3 closest_point = GetClosestPointOnLine(inA, inB, closest_set);
-			float best_dist_sq = closest_point.LengthSq();
+			// Start with vertex C being the closest
+			uint32 closest_set = 0b0100;
+			Vec3 closest_point = inC;
+			float best_dist_sq = inC.LengthSq();
+
+			// If the closest point must include C then A or B cannot be closest
+			if constexpr (!MustIncludeC)
+			{
+				// Try vertex A
+				float a_len_sq = inA.LengthSq();
+				if (a_len_sq < best_dist_sq)
+				{
+					closest_set = 0b0001;
+					closest_point = inA;
+					best_dist_sq = a_len_sq;
+				}
+
+				// Try vertex B
+				float b_len_sq = inB.LengthSq();
+				if (b_len_sq < best_dist_sq)
+				{
+					closest_set = 0b0010;
+					closest_point = inB;
+					best_dist_sq = b_len_sq;
+				}
+
+				// Edge AB
+				float ab_len_sq = ab.LengthSq();
+				if (ab_len_sq > Square(FLT_EPSILON))
+				{
+					float v = Clamp(-a.Dot(ab) / ab_len_sq, 0.0f, 1.0f);
+					Vec3 q = a + v * ab;
+					float dist_sq = q.LengthSq();
+					if (dist_sq < best_dist_sq)
+					{
+						closest_set = swap_ac.GetX()? 0b0110 : 0b0011;
+						closest_point = q;
+						best_dist_sq = dist_sq;
+					}
+				}
+			}
 
 			// Edge AC
-			uint32 set;
-			Vec3 q = GetClosestPointOnLine(inA, inC, set);
-			float dist_sq = q.LengthSq();
-			if (dist_sq < best_dist_sq)
+			float ac_len_sq = ac.LengthSq();
+			if (ac_len_sq > Square(FLT_EPSILON))
 			{
-				closest_point = q;
-				best_dist_sq = dist_sq;
-				closest_set = (set & 0b0001) + ((set & 0b0010) << 1);
+				float v = Clamp(-a.Dot(ac) / ac_len_sq, 0.0f, 1.0f);
+				Vec3 q = a + v * ac;
+				float dist_sq = q.LengthSq();
+				if (dist_sq < best_dist_sq)
+				{
+					closest_set = 0b0101;
+					closest_point = q;
+					best_dist_sq = dist_sq;
+				}
 			}
 
 			// Edge BC
-			q = GetClosestPointOnLine(inB, inC, set);
-			dist_sq = q.LengthSq();
-			if (dist_sq < best_dist_sq)
+			Vec3 bc = c - inB;
+			float bc_len_sq = bc.LengthSq();
+			if (bc_len_sq > Square(FLT_EPSILON))
 			{
-				closest_point = q;
-				closest_set = set << 1;
+				float v = Clamp(-inB.Dot(bc) / bc_len_sq, 0.0f, 1.0f);
+				Vec3 q = inB + v * bc;
+				float dist_sq = q.LengthSq();
+				if (dist_sq < best_dist_sq)
+				{
+					closest_set = swap_ac.GetX()? 0b0011 : 0b0110;
+					closest_point = q;
+					best_dist_sq = dist_sq;
+				}
 			}
 
 			outSet = closest_set;
@@ -342,6 +392,8 @@ namespace ClosestPoint
 
 	/// Get the closest point between tetrahedron (inA, inB, inC, inD) to the origin
 	/// outSet specifies which feature was closest, 1 = a, 2 = b, 4 = c, 8 = d. Edges have 2 bits set, triangles 3 and if the point is in the interior 4 bits are set.
+	/// If MustIncludeD is true, the function assumes that D is part of the closest feature (vertex, edge, face, tetrahedron) and does less work, if the assumption is not true then a closest point to the other features is returned.
+	template <bool MustIncludeD = false>
 	inline Vec3	GetClosestPointOnTetrahedron(Vec3Arg inA, Vec3Arg inB, Vec3Arg inC, Vec3Arg inD, uint32 &outSet)
 	{
 		// Taken from: Real-Time Collision Detection - Christer Ericson (Section: Closest Point on Tetrahedron to Point)
@@ -357,23 +409,27 @@ namespace ClosestPoint
 
 		// If point outside face abc then compute closest point on abc 
 		if (origin_out_of_planes.GetX()) // OriginOutsideOfPlane(inA, inB, inC, inD)
-		{ 
-			Vec3 q = GetClosestPointOnTriangle(inA, inB, inC, closest_set); 
-			float dist_sq = q.LengthSq(); 
-			
-			// Update best closest point if (squared) distance is less than current best 
-			if (dist_sq < best_dist_sq) 
+		{
+			if constexpr (MustIncludeD)
 			{
-				best_dist_sq = dist_sq;
-				closest_point = q; 
+				// If the closest point must include D then ABC cannot be closest but the closest point
+				// cannot be an interior point either so we return A as closest point
+				closest_set = 0b0001;
+				closest_point = inA;
 			}
-		} 
+			else
+			{
+				// Test the face normally
+				closest_point = GetClosestPointOnTriangle<false>(inA, inB, inC, closest_set); 
+			}
+			best_dist_sq = closest_point.LengthSq();
+		}
 		
 		// Repeat test for face acd 
 		if (origin_out_of_planes.GetY()) // OriginOutsideOfPlane(inA, inC, inD, inB)
 		{ 
 			uint32 set;
-			Vec3 q = GetClosestPointOnTriangle(inA, inC, inD, set); 
+			Vec3 q = GetClosestPointOnTriangle<MustIncludeD>(inA, inC, inD, set); 
 			float dist_sq = q.LengthSq(); 
 			if (dist_sq < best_dist_sq) 
 			{
@@ -386,27 +442,33 @@ namespace ClosestPoint
 		// Repeat test for face adb 
 		if (origin_out_of_planes.GetZ()) // OriginOutsideOfPlane(inA, inD, inB, inC)
 		{
+			// Keep original vertex order, it doesn't matter if the triangle is facing inward or outward
+			// and it improves consistency for GJK which will always add a new vertex D and keep the closest
+			// feature from the previous iteration in ABC
 			uint32 set;
-			Vec3 q = GetClosestPointOnTriangle(inA, inD, inB, set); 
+			Vec3 q = GetClosestPointOnTriangle<MustIncludeD>(inA, inB, inD, set); 
 			float dist_sq = q.LengthSq(); 
 			if (dist_sq < best_dist_sq) 
 			{
 				best_dist_sq = dist_sq;
 				closest_point = q;
-				closest_set = (set & 0b0001) + ((set & 0b0010) << 2) + ((set & 0b0100) >> 1); 
+				closest_set = (set & 0b0011) + ((set & 0b0100) << 1); 
 			}
 		} 
 		
 		// Repeat test for face bdc 
 		if (origin_out_of_planes.GetW()) // OriginOutsideOfPlane(inB, inD, inC, inA)
 		{ 
+			// Keep original vertex order, it doesn't matter if the triangle is facing inward or outward
+			// and it improves consistency for GJK which will always add a new vertex D and keep the closest
+			// feature from the previous iteration in ABC
 			uint32 set;
-			Vec3 q = GetClosestPointOnTriangle(inB, inD, inC, set); 
+			Vec3 q = GetClosestPointOnTriangle<MustIncludeD>(inB, inC, inD, set); 
 			float dist_sq = q.LengthSq(); 
 			if (dist_sq < best_dist_sq) 
 			{
 				closest_point = q;
-				closest_set = ((set & 0b0001) << 1) + ((set & 0b0010) << 2) + (set & 0b0100); 
+				closest_set = set << 1;
 			}
 		} 
 	

Jolt/Geometry/GJKClosestPoint.h

@@ -27,9 +27,12 @@ private:
 	/// @param outV Closest point
 	/// @param outVLenSq |outV|^2
 	/// @param outSet Set of points that form the new simplex closest to the origin (bit 1 = mY[0], bit 2 = mY[1], ...)
+	/// 
+	/// If LastPointPartOfClosestFeature is true then the last point added will be assumed to be part of the closest feature and the function will do less work.
 	///
 	/// @return True if new closest point was found.
 	/// False if the function failed, in this case the output variables are not modified
+	template <bool LastPointPartOfClosestFeature>
 	bool		GetClosest(float inPrevVLenSq, Vec3 &outV, float &outVLenSq, uint32 &outSet) const
 	{
 #ifdef JPH_GJK_DEBUG
@@ -55,12 +58,12 @@ private:
 
 		case 3:
 			// Triangle
-			v = ClosestPoint::GetClosestPointOnTriangle(mY[0], mY[1], mY[2], set);
+			v = ClosestPoint::GetClosestPointOnTriangle<LastPointPartOfClosestFeature>(mY[0], mY[1], mY[2], set);
 			break;
 
 		case 4:
 			// Tetrahedron
-			v = ClosestPoint::GetClosestPointOnTetrahedron(mY[0], mY[1], mY[2], mY[3], set);
+			v = ClosestPoint::GetClosestPointOnTetrahedron<LastPointPartOfClosestFeature>(mY[0], mY[1], mY[2], mY[3], set);
 			break;
 
 		default:
@@ -256,7 +259,7 @@ public:
 			// Determine the new closest point
 			float v_len_sq;			// Length^2 of v
 			uint32 set;				// Set of points that form the new simplex
-			if (!GetClosest(prev_v_len_sq, ioV, v_len_sq, set))
+			if (!GetClosest<true>(prev_v_len_sq, ioV, v_len_sq, set))
 				return false;
 
 			// If there are 4 points, the origin is inside the tetrahedron and we're done
@@ -407,7 +410,7 @@ public:
 #endif
 
 			uint32 set;
-			if (!GetClosest(prev_v_len_sq, ioV, v_len_sq, set))
+			if (!GetClosest<true>(prev_v_len_sq, ioV, v_len_sq, set))
 			{
 				--mNumPoints; // Undo add last point
 				break;
@@ -596,7 +599,7 @@ public:
 			// Determine the new closest point from Y to origin
 			bool needs_restart = false;
 			uint32 set;						// Set of points that form the new simplex
-			if (!GetClosest(v_len_sq, v, v_len_sq, set))
+			if (!GetClosest<false>(v_len_sq, v, v_len_sq, set))
 			{
 #ifdef JPH_GJK_DEBUG
 				Trace("Failed to converge");
@@ -805,7 +808,7 @@ public:
 			// Determine the new closest point from Y to origin
 			bool needs_restart = false;
 			uint32 set;						// Set of points that form the new simplex
-			if (!GetClosest(v_len_sq, v, v_len_sq, set))
+			if (!GetClosest<false>(v_len_sq, v, v_len_sq, set))
 			{
 #ifdef JPH_GJK_DEBUG
 				Trace("Failed to converge");

UnitTests/Physics/CastShapeTests.cpp

@@ -223,7 +223,7 @@ TEST_SUITE("CastShapeTests")
 			CHECK(collector.HadHit());
 			CHECK(collector.mHit.mBodyID2 == bodies.front()->GetID());
 			CHECK_APPROX_EQUAL(collector.mHit.mFraction, 4.0f / 10.0f);
-			CHECK_APPROX_EQUAL(collector.mHit.mPenetrationAxis.Normalized(), Vec3(1, 0, 0), 4.0e-3f);
+			CHECK(collector.mHit.mPenetrationAxis.Normalized().Dot(Vec3(1, 0, 0)) > Cos(DegreesToRadians(1.0f)));
 			CHECK_APPROX_EQUAL(collector.mHit.mPenetrationDepth, 0.0f);
 			CHECK_APPROX_EQUAL(collector.mHit.mContactPointOn1, Vec3(0, 0, 0), 2.0e-3f);
 			CHECK_APPROX_EQUAL(collector.mHit.mContactPointOn2, Vec3(0, 0, 0), 2.0e-3f);
@@ -241,7 +241,7 @@ TEST_SUITE("CastShapeTests")
 			CHECK(collector.HadHit());
 			CHECK(collector.mHit.mBodyID2 == bodies.back()->GetID());
 			CHECK_APPROX_EQUAL(collector.mHit.mFraction, 2.0f / 10.0f, 1.0e-4f);
-			CHECK_APPROX_EQUAL(collector.mHit.mPenetrationAxis.Normalized(), Vec3(-1, 0, 0), 2.0e-3f);
+			CHECK(collector.mHit.mPenetrationAxis.Normalized().Dot(Vec3(-1, 0, 0)) > Cos(DegreesToRadians(1.0f)));
 			CHECK_APPROX_EQUAL(collector.mHit.mPenetrationDepth, 0.0f);
 			CHECK_APPROX_EQUAL(collector.mHit.mContactPointOn1, Vec3(2, 0, 0), 4.0e-4f);
 			CHECK_APPROX_EQUAL(collector.mHit.mContactPointOn2, Vec3(2, 0, 0), 4.0e-4f);
@@ -261,7 +261,7 @@ TEST_SUITE("CastShapeTests")
 			const ShapeCastResult &result = collector.mHits.front();
 			CHECK(result.mBodyID2 == bodies[5]->GetID());
 			CHECK_APPROX_EQUAL(result.mFraction, 0.0f);
-			CHECK_APPROX_EQUAL(result.mPenetrationAxis.Normalized(), Vec3(1, 0, 0), 1.0e-3f);
+			CHECK(result.mPenetrationAxis.Normalized().Dot(Vec3(1, 0, 0)) > Cos(DegreesToRadians(1.0f)));
 			CHECK_APPROX_EQUAL(result.mPenetrationDepth, 1.05f);
 			CHECK_APPROX_EQUAL(result.mContactPointOn1, Vec3(2.05f, 0, 0), 1.0e-5f); // Box starts at 1.0, center of sphere adds 0.05, radius of sphere is 1
 			CHECK_APPROX_EQUAL(result.mContactPointOn2, Vec3(1.0f, 0, 0), 1.0e-5f); // Box starts at 1.0

UnitTests/Physics/CollideShapeTests.cpp

@@ -345,4 +345,35 @@ TEST_SUITE("CollideShapeTests")
 		CHECK_APPROX_EQUAL(actual_penetration_axis, expected_penetration_axis);
 		CHECK_APPROX_EQUAL(actual_penetration_depth, expected_penetration_depth);
 	}
+
+	// A test case of a triangle that's nearly parallel to a capsule and penetrating it. This one was causing numerical issues.
+	TEST_CASE("TestCollideParallelTriangleVsCapsule2")
+	{
+		Vec3 v1(-0.0904417038f, -4.72410202f, 0.307858467f);
+		Vec3 v2(-0.0904417038f, 5.27589798f, 0.307857513f);
+		Vec3 v3(9.90955830f, 5.27589798f, 0.307864189f);
+		TriangleShape triangle(v1, v2, v3);
+		triangle.SetEmbedded();
+
+		float capsule_radius = 0.42f;
+		float capsule_half_height = 0.675f;
+		CapsuleShape capsule(capsule_half_height, capsule_radius);
+		capsule.SetEmbedded();
+
+		CollideShapeSettings settings;
+		AllHitCollisionCollector<CollideShapeCollector> collector;
+		CollisionDispatch::sCollideShapeVsShape(&triangle, &capsule, Vec3::sReplicate(1.0f), Vec3::sReplicate(1.0f), Mat44::sIdentity(), Mat44::sIdentity(), SubShapeIDCreator(), SubShapeIDCreator(), settings, collector);
+
+		// The capsule intersects with the triangle and the closest point is in the interior of the triangle
+		Vec3 expected_penetration_axis = Vec3(0, 0, -1); // Triangle is in the XY plane so the normal is Z
+		float expected_penetration_depth = capsule_radius - v1.GetZ();
+
+		CHECK(collector.mHits.size() == 1);
+		const CollideShapeResult &hit = collector.mHits[0];
+		Vec3 actual_penetration_axis = hit.mPenetrationAxis.Normalized();
+		float actual_penetration_depth = hit.mPenetrationDepth;
+
+		CHECK_APPROX_EQUAL(actual_penetration_axis, expected_penetration_axis);
+		CHECK_APPROX_EQUAL(actual_penetration_depth, expected_penetration_depth);
+	}
 }