Fixed issue in contact normal calculation in EPA algorithm (#683)

If, during the building of the hull, the hull becomes invalid (e.g. triangles facing the origin) there is a chance that the last processed triangle is actually not close to the origin at all (the support function returns a high distance). In order to reduce the chance of using this triangle as the contact point and normal we now compare the before last triangle with the last triangle to try to detect this case.

Fixes #671

Jolt/Geometry/EPAConvexHullBuilder.h

@@ -38,7 +38,7 @@ public:
 	// Constants
 	static constexpr int cMaxEdgeLength = 128;				///< Max number of edges in FindEdge
 	static constexpr float cMinTriangleArea = 1.0e-10f;		///< Minimum area of a triangle before, if smaller than this it will not be added to the priority queue
-	static constexpr float cBarycentricEpsilon = 1.0e-3f;	///< Epsilon value used to determine if a point is in the interior of a triangle 
+	static constexpr float cBarycentricEpsilon = 1.0e-3f;	///< Epsilon value used to determine if a point is in the interior of a triangle
 
 	// Forward declare
 	class Triangle;
@@ -50,7 +50,7 @@ public:
 		/// Information about neighbouring triangle
 		Triangle *		mNeighbourTriangle;					///< Triangle that neighbours this triangle
 		int				mNeighbourEdge;						///< Index in mEdge that specifies edge that this Edge is connected to
-															
+
 		int				mStartIdx;							///< Vertex index in mPositions that indicates the start vertex of this edge
 	};
 
@@ -309,7 +309,7 @@ public:
 
 #ifdef JPH_EPA_CONVEX_BUILDER_DRAW
 		// Draw new support point
-		DrawMarker(pos, Color::sYellow, 1.0f); 
+		DrawMarker(pos, Color::sYellow, 1.0f);
 #endif
 
 #ifdef JPH_EPA_CONVEX_BUILDER_VALIDATE
@@ -337,7 +337,7 @@ public:
 				|| nt->mClosestLenSq < 0.0f)										// For when the origin is not inside the hull yet
 				mTriangleQueue.push_back(nt);
 		}
-		
+
 		// Link edges
 		for (int i = 0; i < num_edges; ++i)
 		{
@@ -555,12 +555,12 @@ private:
 		DrawState();
 #endif
 
-		// When we start with two triangles facing away from each other and adding a point that is on the plane, 
+		// When we start with two triangles facing away from each other and adding a point that is on the plane,
 		// sometimes we consider the point in front of both causing both triangles to be removed resulting in an empty edge list.
 		// In this case we fail to add the point which will result in no collision reported (the shapes are contacting in 1 point so there's 0 penetration)
 		return outEdges.size() >= 3;
 	}
-	
+
 #ifdef JPH_EPA_CONVEX_BUILDER_VALIDATE
 	/// Check consistency of 1 triangle
 	void				ValidateTriangle(const Triangle *inT) const
@@ -649,6 +649,14 @@ public:
 		mOffset += Vec3(max_x - min_x + 0.5f, 0.0f, 0.0f);
 	}
 
+	/// Draw a label to indicate the next stage in the algorithm
+	void				DrawLabel(const string_view &inText)
+	{
+		DebugRenderer::sInstance->DrawText3D(cDrawScale * mOffset, inText, Color::sWhite, 0.1f * cDrawScale);
+
+		mOffset += Vec3(5.0f, 0.0f, 0.0f);
+	}
+
 	/// Draw a triangle for debugging purposes
 	void				DrawWireTriangle(const Triangle &inTriangle, ColorArg inColor)
 	{
@@ -680,11 +688,11 @@ private:
 	TriangleQueue 		mTriangleQueue;						///< List of triangles that are part of the hull that still need to be checked (if !mRemoved)
 
 #if defined(JPH_EPA_CONVEX_BUILDER_VALIDATE) || defined(JPH_EPA_CONVEX_BUILDER_DRAW)
-	Triangles			mTriangles;							///< The list of all triangles in this hull (for debug purposes)	
+	Triangles			mTriangles;							///< The list of all triangles in this hull (for debug purposes)
 #endif
 
 #ifdef JPH_EPA_CONVEX_BUILDER_DRAW
-	int					mIteration;							///< Number of iterations we've had so far (for debug purposes)	
+	int					mIteration;							///< Number of iterations we've had so far (for debug purposes)
 	RVec3				mOffset;							///< Offset to use for state drawing
 #endif
 };
@@ -767,7 +775,7 @@ EPAConvexHullBuilder::Triangle::Triangle(int inIdx0, int inIdx1, int inIdx2, con
 				mLambda[0] = l0;
 				mLambda[1] = l1;
 				mLambdaRelativeTo0 = true;
-	
+
 				// Check if closest point is interior to the triangle. For a convex hull which contains the origin each face must contain the origin, but because
 				// our faces are triangles, we can have multiple coplanar triangles and only 1 will have the origin as an interior point. We want to use this triangle
 				// to calculate the contact points because it gives the most accurate results, so we will only add these triangles to the priority queue.

Jolt/Geometry/EPAPenetrationDepth.h

@@ -9,6 +9,8 @@
 #include <Jolt/Geometry/GJKClosestPoint.h>
 #include <Jolt/Geometry/EPAConvexHullBuilder.h>
 
+//#define JPH_EPA_PENETRATION_DEPTH_DEBUG
+
 JPH_NAMESPACE_BEGIN
 
 /// Implementation of Expanding Polytope Algorithm as described in:
@@ -195,6 +197,12 @@ public:
 		// Create hull out of the initial points
 		JPH_ASSERT(support_points.mY.size() >= 3);
 		EPAConvexHullBuilder hull(support_points.mY);
+#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
+		hull.DrawLabel("Build initial hull");
+#endif
+#ifdef JPH_EPA_PENETRATION_DEPTH_DEBUG
+		Trace("Init: num_points = %d", support_points.mY.size());
+#endif
 		hull.Initialize(0, 1, 2);
 		for (typename Points::size_type i = 3; i < support_points.mY.size(); ++i)
 		{
@@ -212,6 +220,10 @@ public:
 			}
 		}
 
+#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
+		hull.DrawLabel("Ensure origin in hull");
+#endif
+
 		// Loop until we are sure that the origin is inside the hull
 		for (;;)
 		{
@@ -235,6 +247,17 @@ public:
 			if (t->mClosestLenSq >= 0.0f)
 				break;
 
+#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
+			hull.DrawLabel("Next iteration");
+#endif
+#ifdef JPH_EPA_PENETRATION_DEPTH_DEBUG
+			Trace("EncapsulateOrigin: verts = (%d, %d, %d), closest_dist_sq = %g, centroid = (%g, %g, %g), normal = (%g, %g, %g)",
+				t->mEdge[0].mStartIdx, t->mEdge[1].mStartIdx, t->mEdge[2].mStartIdx,
+				t->mClosestLenSq,
+				t->mCentroid.GetX(), t->mCentroid.GetY(), t->mCentroid.GetZ(),
+				t->mNormal.GetX(), t->mNormal.GetY(), t->mNormal.GetZ());
+#endif
+
 			// Remove the triangle from the queue before we start adding new ones (which may result in a new closest triangle at the front of the queue)
 			hull.PopClosestTriangleFromQueue();
 
@@ -263,11 +286,16 @@ public:
 				return false;
 		}
 
+#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
+		hull.DrawLabel("Main algorithm");
+#endif
+
 		// Current closest distance to origin
 		float closest_dist_sq = FLT_MAX;
 
 		// Remember last good triangle
-		Triangle *last = nullptr;
+		Triangle *before_last = nullptr, *last = nullptr;
+		float before_last_dist_sq = FLT_MAX, last_dist_sq = FLT_MAX;
 
 		// Loop until closest point found
 		do
@@ -282,15 +310,20 @@ public:
 				continue;
 			}
 
+#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
+			hull.DrawLabel("Next iteration");
+#endif
+#ifdef JPH_EPA_PENETRATION_DEPTH_DEBUG
+			Trace("FindClosest: verts = (%d, %d, %d), closest_len_sq = %g, centroid = (%g, %g, %g), normal = (%g, %g, %g)",
+				t->mEdge[0].mStartIdx, t->mEdge[1].mStartIdx, t->mEdge[2].mStartIdx,
+				t->mClosestLenSq,
+				t->mCentroid.GetX(), t->mCentroid.GetY(), t->mCentroid.GetZ(),
+				t->mNormal.GetX(), t->mNormal.GetY(), t->mNormal.GetZ());
+#endif
 			// Check if next triangle is further away than closest point, we've found the closest point
 			if (t->mClosestLenSq >= closest_dist_sq)
 				break;
 
-			// Replace last good with this triangle
-			if (last != nullptr)
-				hull.FreeTriangle(last);
-			last = t;
-
 			// Add support point in direction of normal of the plane
 			// Note that the article uses the closest point between the origin and plane, but this always has the exact same direction as the normal (if the origin is behind the plane)
 			// and this way we do less calculations and lose less precision
@@ -306,8 +339,21 @@ public:
 				return false;
 
 			// Get the distance squared (along normal) to the support point
-			float dist_sq = dot * dot / t->mNormal.LengthSq();
+			float dist_sq = Square(dot) / t->mNormal.LengthSq();
+
+			// Replace last good with this triangle and shift last good to before last
+			if (before_last != nullptr)
+				hull.FreeTriangle(before_last);
+			before_last = last;
+			last = t;
+			before_last_dist_sq = last_dist_sq;
+			last_dist_sq = dist_sq;
 
+#ifdef JPH_EPA_PENETRATION_DEPTH_DEBUG
+			Trace("FindClosest: w = (%g, %g, %g), dot = %g, dist_sq = %g",
+				w.GetX(), w.GetY(), w.GetZ(),
+				dot, dist_sq);
+#endif
 #ifdef JPH_EPA_CONVEX_BUILDER_DRAW
 			// Draw the point that we're adding
 			hull.DrawMarker(w, Color::sPurple, 1.0f);
@@ -317,19 +363,34 @@ public:
 
 			// If the error became small enough, we've converged
 			if (dist_sq - t->mClosestLenSq < t->mClosestLenSq * inTolerance)
+			{
+#ifdef JPH_EPA_PENETRATION_DEPTH_DEBUG
+				Trace("Converged");
+#endif // JPH_EPA_PENETRATION_DEPTH_DEBUG
 				break;
+			}
 
 			// Keep track of the minimum distance
 			closest_dist_sq = min(closest_dist_sq, dist_sq);
 
 			// If the triangle thinks this point is not front facing, we've reached numerical precision and we're done
 			if (!t->IsFacing(w))
+			{
+#ifdef JPH_EPA_PENETRATION_DEPTH_DEBUG
+				Trace("Not facing triangle");
+#endif // JPH_EPA_PENETRATION_DEPTH_DEBUG
 				break;
+			}
 
 			// Add point to hull
 			EPAConvexHullBuilder::NewTriangles new_triangles;
 			if (!hull.AddPoint(t, new_index, closest_dist_sq, new_triangles))
+			{
+#ifdef JPH_EPA_PENETRATION_DEPTH_DEBUG
+				Trace("Could not add point");
+#endif // JPH_EPA_PENETRATION_DEPTH_DEBUG
 				break;
+			}
 
 			// If the hull is starting to form defects then we're reaching numerical precision and we have to stop
 			bool has_defect = false;
@@ -340,7 +401,12 @@ public:
 					break;
 				}
 			if (has_defect)
+			{
+#ifdef JPH_EPA_PENETRATION_DEPTH_DEBUG
+				Trace("Has defect");
+#endif // JPH_EPA_PENETRATION_DEPTH_DEBUG
 				break;
+			}
 		}
 		while (hull.HasNextTriangle() && support_points.mY.size() < cMaxPoints);
 
@@ -348,6 +414,23 @@ public:
 		if (last == nullptr)
 			return false;
 
+		// Fall back to before last triangle if the last triangle is significantly worse.
+		// This fixes an issue that when the hull becomes invalid due to numerical precision issues and we did one step too many.
+		// Note that when colliding curved surfaces the last triangle describes the surface better and results in a better contact point,
+		// that's why we only do this when the last triangle is significantly worse than the before last triangle.
+		if (before_last_dist_sq < 0.81f * last_dist_sq) // 10%
+		{
+			JPH_ASSERT(before_last != nullptr);
+			last = before_last;
+		}
+
+#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
+		hull.DrawLabel("Closest found");
+		hull.DrawWireTriangle(*last, Color::sWhite);
+		hull.DrawArrow(last->mCentroid, last->mCentroid + last->mNormal.NormalizedOr(Vec3::sZero()), Color::sWhite, 0.1f);
+		hull.DrawState();
+#endif
+
 		// Calculate penetration by getting the vector from the origin to the closest point on the triangle:
 		// distance = (centroid - origin) . normal / |normal|, closest = origin + distance * normal / |normal|
 		outV = (last->mCentroid.Dot(last->mNormal) / last->mNormal.LengthSq()) * last->mNormal;

UnitTests/Physics/CollideShapeTests.cpp

@@ -93,9 +93,9 @@ TEST_SUITE("CollideShapeTests")
 		class PositionACollideShapeCollector : public CollideShapeCollector
 		{
 		public:
-							PositionACollideShapeCollector(const Body &inBody2) : 
+							PositionACollideShapeCollector(const Body &inBody2) :
 				mBody2(inBody2)
-			{ 
+			{
 			}
 
 			virtual void	AddHit(const CollideShapeResult &inResult) override
@@ -133,9 +133,9 @@ TEST_SUITE("CollideShapeTests")
 		class PositionBCollideShapeCollector : public CollideShapeCollector
 		{
 		public:
-							PositionBCollideShapeCollector(const Body &inBody2) : 
+							PositionBCollideShapeCollector(const Body &inBody2) :
 				mBody2(inBody2)
-			{ 
+			{
 			}
 
 			virtual void	Reset() override
@@ -257,11 +257,11 @@ TEST_SUITE("CollideShapeTests")
 		// Collide the two shapes
 		AllHitCollisionCollector<CollideShapeCollector> collector;
 		CollisionDispatch::sCollideShapeVsShape(capsule_shape, box_shape, Vec3::sReplicate(1.0f), Vec3::sReplicate(1.0f), capsule_transform, box_transform, SubShapeIDCreator(), SubShapeIDCreator(), settings, collector);
-		
+
 		// Check that there was a hit
 		CHECK(collector.mHits.size() == 1);
 		const CollideShapeResult &result = collector.mHits.front();
-		
+
 		// Now move the box 1% further than the returned penetration depth and check that it is no longer in collision
 		Vec3 distance_to_move_box = result.mPenetrationAxis.Normalized() * result.mPenetrationDepth;
 		collector.Reset();
@@ -377,4 +377,57 @@ 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 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")
+	{
+		const float separation_distance = 0.001f;
+		const float box_separation_from_hull = 0.5f * separation_distance;
+		const float hull_height = 0.25f;
+
+		// Box with no convex radius
+		Ref<BoxShapeSettings> box_settings = new BoxShapeSettings(Vec3(0.25f, 0.75f, 0.375f), 0.0f);
+		Ref<Shape> box_shape = box_settings->Create().Get();
+
+		// Convex hull (also a box) with no convex radius
+		Vec3 hull_points[] =
+		{
+			Vec3(-2.5f, -hull_height, -1.5f),
+			Vec3(-2.5f, hull_height, -1.5f),
+			Vec3(2.5f, -hull_height, -1.5f),
+			Vec3(-2.5f, -hull_height, 1.5f),
+			Vec3(-2.5f, hull_height, 1.5f),
+			Vec3(2.5f, hull_height, -1.5f),
+			Vec3(2.5f, -hull_height, 1.5f),
+			Vec3(2.5f, hull_height, 1.5f)
+		};
+		Ref<ConvexHullShapeSettings> hull_settings = new ConvexHullShapeSettings(hull_points, 8, 0.0f);
+		Ref<Shape> hull_shape = hull_settings->Create().Get();
+
+		float angle = 0.0f;
+		for (int i = 0; i < 481; ++i)
+		{
+			// Slowly rotate both box and convex hull
+			angle += DegreesToRadians(45.0f) / 60.0f;
+			Mat44 hull_transform = Mat44::sRotationY(angle);
+			const Mat44 box_local_translation = Mat44::sTranslation(Vec3(0.1f, 1.0f + box_separation_from_hull, -0.5f));
+			const Mat44 box_local_rotation = Mat44::sRotationY(DegreesToRadians(-45.0f));
+			const Mat44 box_local_transform = box_local_translation * box_local_rotation;
+			const Mat44 box_transform = hull_transform * box_local_transform;
+
+			CollideShapeSettings settings;
+			settings.mMaxSeparationDistance = separation_distance;
+			ClosestHitCollisionCollector<CollideShapeCollector> collector;
+			CollisionDispatch::sCollideShapeVsShape(box_shape, hull_shape, Vec3::sReplicate(1.0f), Vec3::sReplicate(1.0f), box_transform, hull_transform, SubShapeIDCreator(), SubShapeIDCreator(), settings, collector);
+
+			// 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.mContactPointOn2.GetY(), hull_height);
+			CHECK_APPROX_EQUAL(hit.mPenetrationAxis.NormalizedOr(Vec3::sZero()), -Vec3::sAxisY(), 1.0e-3f);
+		}
+
+		CHECK(angle >= 2.0f * JPH_PI);
+	}
 }