Fixing flipped normals en EPA penetration depth algorithm (#691)

With a hull consisting of two triangles that are back to back, it is possible that the side on which the origin is is misclassified for one of the two triangles (due to floating point precision issues). The resulting penetration axis (= triangle normal) will be flipped because of that resulting in a penetration axis that pushes the objects together rather than separating them. In order to avoid this, we do an additional check when we detect a failure in building a correct hull to see if the penetration is less in the flipped normal direction and if it is we flip the penetration axis.

Fixes #671

Jolt/Geometry/EPAPenetrationDepth.h

@@ -303,6 +303,9 @@ public:
 		// Remember last good triangle
 		Triangle *last = nullptr;
 
+		// If we want to flip the penetration depth
+		bool flip_v_sign = false;
+
 		// Loop until closest point found
 		do
 		{
@@ -408,6 +411,13 @@ public:
 #ifdef JPH_EPA_PENETRATION_DEPTH_DEBUG
 				Trace("Has defect");
 #endif // JPH_EPA_PENETRATION_DEPTH_DEBUG
+				// When the hull has defects it is possible that the origin has been classified on the wrong side of the triangle
+				// so we do an additional check to see if the penetration in the -triangle normal direction is smaller than
+				// the penetration in the triangle normal direction. If so we must flip the sign of the penetration depth.
+				Vec3 w2 = inAIncludingConvexRadius.GetSupport(-t->mNormal) - inBIncludingConvexRadius.GetSupport(t->mNormal);
+				float dot2 = -t->mNormal.Dot(w2);
+				if (dot2 < dot)
+					flip_v_sign = true;
 				break;
 			}
 		}
@@ -432,6 +442,10 @@ public:
 		if (outV.IsNearZero())
 			return false;
 
+		// Check if we have to flip the sign of the penetration depth
+		if (flip_v_sign)
+			outV = -outV;
+
 		// Use the barycentric coordinates for the closest point to the origin to find the contact points on A and B
 		Vec3 p0 = support_points.mP[last->mEdge[0].mStartIdx];
 		Vec3 p1 = support_points.mP[last->mEdge[1].mStartIdx];

UnitTests/Physics/CollideShapeTests.cpp

@@ -378,8 +378,6 @@ TEST_SUITE("CollideShapeTests")
 		CHECK_APPROX_EQUAL(actual_penetration_depth, expected_penetration_depth);
 	}
 
-	/*
-	// TODO: Re-enable after a better fix is found
 	// A test case of a box and a convex hull that are nearly touching and that should return a contact with correct normal because the collision settings specify a max separation distance. This was producing the wrong normal.
 	TEST_CASE("BoxVsConvexHullNoConvexRadius")
 	{
@@ -425,12 +423,11 @@ TEST_SUITE("CollideShapeTests")
 			// Check that there was a hit and that the contact normal is correct
 			CHECK(collector.HadHit());
 			const CollideShapeResult &hit = collector.mHit;
-			CHECK_APPROX_EQUAL(hit.mContactPointOn1.GetY(), hull_height + box_separation_from_hull, 2.0e-4f);
+			CHECK_APPROX_EQUAL(hit.mContactPointOn1.GetY(), hull_height + box_separation_from_hull, 1.0e-3f);
 			CHECK_APPROX_EQUAL(hit.mContactPointOn2.GetY(), hull_height);
 			CHECK_APPROX_EQUAL(hit.mPenetrationAxis.NormalizedOr(Vec3::sZero()), -Vec3::sAxisY(), 1.0e-3f);
 		}
 
 		CHECK(angle >= 2.0f * JPH_PI);
 	}
-	*/
 }