Added unit test for EPA CastShape test and fixed bug that could return a zero length normal when the GJK algorithm terminated in the first iteration (#217)

Jolt/Geometry/EPAPenetrationDepth.h

@@ -420,7 +420,7 @@ public:
 	/// @param ioLambda The max fraction along the sweep, on output updated with the actual collision fraction.
 	///	@param outPointA is the contact point on A
 	///	@param outPointB is the contact point on B
-	/// @param outContactNormal is either the contact normal when the objects are touching or the penetration axis when the objects are penetrating at the start of the sweep (pointing from A to B)
+	/// @param outContactNormal is either the contact normal when the objects are touching or the penetration axis when the objects are penetrating at the start of the sweep (pointing from A to B, length will not be 1)
 	/// 
 	/// @return true if the a hit was found, in which case ioLambda, outPointA, outPointB and outSurfaceNormal are updated.
 	template <typename A, typename B>
@@ -430,10 +430,13 @@ public:
 		if (!mGJK.CastShape(inStart, inDirection, inCollisionTolerance, inA, inB, inConvexRadiusA, inConvexRadiusB, ioLambda, outPointA, outPointB, outContactNormal))
 			return false;
 
+		// When our contact normal is too small, we don't have an accurate result
+		bool contact_normal_invalid = outContactNormal.IsNearZero(Square(inCollisionTolerance));
+		
 		if (inReturnDeepestPoint 
 			&& ioLambda == 0.0f // Only when lambda = 0 we can have the bodies overlap
 			&& (inConvexRadiusA + inConvexRadiusB == 0.0f // When no convex radius was provided we can never trust contact points at lambda = 0
-				|| outContactNormal.LengthSq() <= inCollisionTolerance * inCollisionTolerance)) // When our contact normal is too small, we don't have an accurate result and need to run the EPA algorithm
+				|| contact_normal_invalid))
 		{
 			// If we're initially intersecting, we need to run the EPA algorithm in order to find the deepest contact point
 			AddConvexRadius<A> add_convex_a(inA, inConvexRadiusA);
@@ -442,6 +445,11 @@ public:
 			if (!GetPenetrationDepthStepEPA(transformed_a, add_convex_b, inPenetrationTolerance, outContactNormal, outPointA, outPointB))
 				return false;
 		}
+		else if (contact_normal_invalid)
+		{
+			// If we weren't able to calculate a contact normal, use the cast direction instead
+			outContactNormal = inDirection;
+		}
 
 		return true;
 	}

UnitTests/Geometry/EPATests.cpp

@@ -169,4 +169,44 @@ TEST_SUITE("EPATests")
 		float angle = AngleBetweenVectors(v, pa - pb);
 		CHECK(angle < 1.0e-3f);
 	}
+
+	TEST_CASE("TestEPACastSphereSphereMiss")
+	{
+		Sphere sphere(Vec3(0, 0, 0), 1.0f);
+		EPAPenetrationDepth epa;
+		float lambda = 1.0f + FLT_EPSILON;
+		const Vec3 invalid(-999, -999, -999);
+		Vec3 pa = invalid, pb = invalid, normal = invalid;
+		CHECK(!epa.CastShape(Mat44::sTranslation(Vec3(-10, 2.1f, 0)), Vec3(20, 0, 0), 1.0e-4f, 1.0e-4f, sphere, sphere, 0.0f, 0.0f, true, lambda, pa, pb, normal));
+		CHECK(lambda == 1.0f + FLT_EPSILON); // Check input values didn't change
+		CHECK(pa == invalid);
+		CHECK(pb == invalid);
+		CHECK(normal == invalid);
+	}
+
+	TEST_CASE("TestEPACastSphereSphereInitialOverlap")
+	{
+		Sphere sphere(Vec3(0, 0, 0), 1.0f);
+		EPAPenetrationDepth epa;
+		float lambda = 1.0f + FLT_EPSILON;
+		Vec3 pa, pb, normal;
+		CHECK(epa.CastShape(Mat44::sTranslation(Vec3(-1, 0, 0)), Vec3(10, 0, 0), 1.0e-4f, 1.0e-4f, sphere, sphere, 0.0f, 0.0f, true, lambda, pa, pb, normal));
+		CHECK(lambda == 0.0f);
+		CHECK_APPROX_EQUAL(pa, Vec3::sZero(), 5.0e-3f);
+		CHECK_APPROX_EQUAL(pb, Vec3(-1, 0, 0), 5.0e-3f);
+		CHECK_APPROX_EQUAL(normal.NormalizedOr(Vec3::sZero()), Vec3(1, 0, 0), 1.0e-2f);
+	}
+
+	TEST_CASE("TestEPACastSphereSphereHit")
+	{
+		Sphere sphere(Vec3(0, 0, 0), 1.0f);
+		EPAPenetrationDepth epa;
+		float lambda = 1.0f + FLT_EPSILON;
+		Vec3 pa, pb, normal;
+		CHECK(epa.CastShape(Mat44::sTranslation(Vec3(-10, 0, 0)), Vec3(20, 0, 0), 1.0e-4f, 1.0e-4f, sphere, sphere, 0.0f, 0.0f, true, lambda, pa, pb, normal));
+		CHECK_APPROX_EQUAL(lambda, 8.0f / 20.0f);
+		CHECK_APPROX_EQUAL(pa, Vec3(-1, 0, 0));
+		CHECK_APPROX_EQUAL(pb, Vec3(-1, 0, 0));
+		CHECK_APPROX_EQUAL(normal.NormalizedOr(Vec3::sZero()), Vec3(1, 0, 0));
+	}
 }