Fixed numerical inaccuracy in penetration depth calculation when Coll…

…ideShapeSettings::mMaxSeparationDistance was set to a really high value (e.g. 1000). (#1451)

* Also fixed missing collision registrations in TriangleShape and SoftBodyShape
* Some sonar fixes

Fixes #1379

Docs/ReleaseNotes.md

@@ -34,6 +34,7 @@ For breaking API changes see [this document](https://github.com/jrouwe/JoltPhysi
 * VehicleConstraint would override Body::SetAllowSleeping every frame, making it impossible for client code to configure a vehicle that cannot go to sleep.
 * Fixed CharacterVirtual::Contact::mIsSensorB not being persisted in SaveState.
 * Fixed CharacterVirtual::Contact::mHadContact not being true for collisions with sensors. They will still be marked as mWasDiscarded to prevent any further interaction.
+* Fixed numerical inaccuracy in penetration depth calculation when CollideShapeSettings::mMaxSeparationDistance was set to a really high value (e.g. 1000).
 
 ## v5.2.0
 

Jolt/Geometry/EPAPenetrationDepth.h

@@ -540,9 +540,9 @@ class EPAPenetrationDepth
 				|| 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);
-			AddConvexRadius<B> add_convex_b(inB, inConvexRadiusB);
-			TransformedConvexObject<AddConvexRadius<A>> transformed_a(inStart, add_convex_a);
+			AddConvexRadius add_convex_a(inA, inConvexRadiusA);
+			AddConvexRadius add_convex_b(inB, inConvexRadiusB);
+			TransformedConvexObject transformed_a(inStart, add_convex_a);
 			if (!GetPenetrationDepthStepEPA(transformed_a, add_convex_b, inPenetrationTolerance, outContactNormal, outPointA, outPointB))
 				return false;
 		}

Jolt/Physics/Collision/CollideConvexVsTriangles.cpp

@@ -79,7 +79,8 @@ void CollideConvexVsTriangles::Collide(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2,
 		mShape1ExCvxRadius = mShape1->GetSupportFunction(ConvexShape::ESupportMode::ExcludeConvexRadius, mBufferExCvxRadius, mScale1);
 
 	// Perform GJK step
-	status = pen_depth.GetPenetrationDepthStepGJK(*mShape1ExCvxRadius, mShape1ExCvxRadius->GetConvexRadius() + mCollideShapeSettings.mMaxSeparationDistance, triangle, 0.0f, mCollideShapeSettings.mCollisionTolerance, penetration_axis, point1, point2);
+	float max_separation_distance = mCollideShapeSettings.mMaxSeparationDistance;
+	status = pen_depth.GetPenetrationDepthStepGJK(*mShape1ExCvxRadius, mShape1ExCvxRadius->GetConvexRadius() + max_separation_distance, triangle, 0.0f, mCollideShapeSettings.mCollisionTolerance, penetration_axis, point1, point2);
 
 	// Check result of collision detection
 	if (status == EPAPenetrationDepth::EStatus::NotColliding)
@@ -88,27 +89,33 @@ void CollideConvexVsTriangles::Collide(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2,
 	{
 		// Need to run expensive EPA algorithm
 
+		// We know we're overlapping at this point, so we can set the max separation distance to 0.
+		// Numerically it is possible that GJK finds that the shapes are overlapping but EPA finds that they're separated.
+		// In order to avoid this, we clamp the max separation distance to 1 so that we don't excessively inflate the shape,
+		// but we still inflate it enough to avoid the case where EPA misses the collision.
+		max_separation_distance = min(max_separation_distance, 1.0f);
+
 		// Get the support function
 		if (mShape1IncCvxRadius == nullptr)
 			mShape1IncCvxRadius = mShape1->GetSupportFunction(ConvexShape::ESupportMode::IncludeConvexRadius, mBufferIncCvxRadius, mScale1);
 
 		// Add convex radius
-		AddConvexRadius<ConvexShape::Support> shape1_add_max_separation_distance(*mShape1IncCvxRadius, mCollideShapeSettings.mMaxSeparationDistance);
+		AddConvexRadius shape1_add_max_separation_distance(*mShape1IncCvxRadius, max_separation_distance);
 
 		// Perform EPA step
 		if (!pen_depth.GetPenetrationDepthStepEPA(shape1_add_max_separation_distance, triangle, mCollideShapeSettings.mPenetrationTolerance, penetration_axis, point1, point2))
 			return;
 	}
 
 	// Check if the penetration is bigger than the early out fraction
-	float penetration_depth = (point2 - point1).Length() - mCollideShapeSettings.mMaxSeparationDistance;
+	float penetration_depth = (point2 - point1).Length() - max_separation_distance;
 	if (-penetration_depth >= mCollector.GetEarlyOutFraction())
 		return;
 
 	// Correct point1 for the added separation distance
 	float penetration_axis_len = penetration_axis.Length();
 	if (penetration_axis_len > 0.0f)
-		point1 -= penetration_axis * (mCollideShapeSettings.mMaxSeparationDistance / penetration_axis_len);
+		point1 -= penetration_axis * (max_separation_distance / penetration_axis_len);
 
 	// Check if we have enabled active edge detection
 	if (mCollideShapeSettings.mActiveEdgeMode == EActiveEdgeMode::CollideOnlyWithActive && inActiveEdges != 0b111)

Jolt/Physics/Collision/Shape/ConvexShape.cpp

@@ -57,8 +57,9 @@ void ConvexShape::sCollideConvexVsConvex(const Shape *inShape1, const Shape *inS
 	Mat44 transform_2_to_1 = inverse_transform1 * inCenterOfMassTransform2;
 
 	// Get bounding boxes
+	float max_separation_distance = inCollideShapeSettings.mMaxSeparationDistance;
 	AABox shape1_bbox = shape1->GetLocalBounds().Scaled(inScale1);
-	shape1_bbox.ExpandBy(Vec3::sReplicate(inCollideShapeSettings.mMaxSeparationDistance));
+	shape1_bbox.ExpandBy(Vec3::sReplicate(max_separation_distance));
 	AABox shape2_bbox = shape2->GetLocalBounds().Scaled(inScale2);
 
 	// Check if they overlap
@@ -86,10 +87,10 @@ void ConvexShape::sCollideConvexVsConvex(const Shape *inShape1, const Shape *inS
 		const Support *shape2_excl_cvx_radius = shape2->GetSupportFunction(ConvexShape::ESupportMode::ExcludeConvexRadius, buffer2_excl_cvx_radius, inScale2);
 
 		// Transform shape 2 in the space of shape 1
-		TransformedConvexObject<Support> transformed2_excl_cvx_radius(transform_2_to_1, *shape2_excl_cvx_radius);
+		TransformedConvexObject transformed2_excl_cvx_radius(transform_2_to_1, *shape2_excl_cvx_radius);
 
 		// Perform GJK step
-		status = pen_depth.GetPenetrationDepthStepGJK(*shape1_excl_cvx_radius, shape1_excl_cvx_radius->GetConvexRadius() + inCollideShapeSettings.mMaxSeparationDistance, transformed2_excl_cvx_radius, shape2_excl_cvx_radius->GetConvexRadius(), inCollideShapeSettings.mCollisionTolerance, penetration_axis, point1, point2);
+		status = pen_depth.GetPenetrationDepthStepGJK(*shape1_excl_cvx_radius, shape1_excl_cvx_radius->GetConvexRadius() + max_separation_distance, transformed2_excl_cvx_radius, shape2_excl_cvx_radius->GetConvexRadius(), inCollideShapeSettings.mCollisionTolerance, penetration_axis, point1, point2);
 	}
 
 	// Check result of collision detection
@@ -105,16 +106,22 @@ void ConvexShape::sCollideConvexVsConvex(const Shape *inShape1, const Shape *inS
 		{
 			// Need to run expensive EPA algorithm
 
+			// We know we're overlapping at this point, so we can set the max separation distance to 0.
+			// Numerically it is possible that GJK finds that the shapes are overlapping but EPA finds that they're separated.
+			// In order to avoid this, we clamp the max separation distance to 1 so that we don't excessively inflate the shape,
+			// but we still inflate it enough to avoid the case where EPA misses the collision.
+			max_separation_distance = min(max_separation_distance, 1.0f);
+
 			// Create support function
 			SupportBuffer buffer1_incl_cvx_radius, buffer2_incl_cvx_radius;
 			const Support *shape1_incl_cvx_radius = shape1->GetSupportFunction(ConvexShape::ESupportMode::IncludeConvexRadius, buffer1_incl_cvx_radius, inScale1);
 			const Support *shape2_incl_cvx_radius = shape2->GetSupportFunction(ConvexShape::ESupportMode::IncludeConvexRadius, buffer2_incl_cvx_radius, inScale2);
 
 			// Add separation distance
-			AddConvexRadius<Support> shape1_add_max_separation_distance(*shape1_incl_cvx_radius, inCollideShapeSettings.mMaxSeparationDistance);
+			AddConvexRadius shape1_add_max_separation_distance(*shape1_incl_cvx_radius, max_separation_distance);
 
 			// Transform shape 2 in the space of shape 1
-			TransformedConvexObject<Support> transformed2_incl_cvx_radius(transform_2_to_1, *shape2_incl_cvx_radius);
+			TransformedConvexObject transformed2_incl_cvx_radius(transform_2_to_1, *shape2_incl_cvx_radius);
 
 			// Perform EPA step
 			if (!pen_depth.GetPenetrationDepthStepEPA(shape1_add_max_separation_distance, transformed2_incl_cvx_radius, inCollideShapeSettings.mPenetrationTolerance, penetration_axis, point1, point2))
@@ -124,14 +131,14 @@ void ConvexShape::sCollideConvexVsConvex(const Shape *inShape1, const Shape *inS
 	}
 
 	// Check if the penetration is bigger than the early out fraction
-	float penetration_depth = (point2 - point1).Length() - inCollideShapeSettings.mMaxSeparationDistance;
+	float penetration_depth = (point2 - point1).Length() - max_separation_distance;
 	if (-penetration_depth >= ioCollector.GetEarlyOutFraction())
 		return;
 
 	// Correct point1 for the added separation distance
 	float penetration_axis_len = penetration_axis.Length();
 	if (penetration_axis_len > 0.0f)
-		point1 -= penetration_axis * (inCollideShapeSettings.mMaxSeparationDistance / penetration_axis_len);
+		point1 -= penetration_axis * (max_separation_distance / penetration_axis_len);
 
 	// Convert to world space
 	point1 = inCenterOfMassTransform1 * point1;
@@ -446,7 +453,7 @@ void ConvexShape::DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg in
 	// Get the support function with convex radius
 	SupportBuffer buffer;
 	const Support *support = GetSupportFunction(ESupportMode::ExcludeConvexRadius, buffer, inScale);
-	AddConvexRadius<Support> add_convex(*support, support->GetConvexRadius());
+	AddConvexRadius add_convex(*support, support->GetConvexRadius());
 
 	// Draw the shape
 	DebugRenderer::GeometryRef geometry = inRenderer->CreateTriangleGeometryForConvex([&add_convex](Vec3Arg inDirection) { return add_convex.GetSupport(inDirection); });

Jolt/Physics/Collision/Shape/TriangleShape.cpp

@@ -413,6 +413,9 @@ void TriangleShape::sRegister()
 	{
 		CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::Triangle, sCollideConvexVsTriangle);
 		CollisionDispatch::sRegisterCastShape(s, EShapeSubType::Triangle, sCastConvexVsTriangle);
+
+		CollisionDispatch::sRegisterCollideShape(EShapeSubType::Triangle, s, CollisionDispatch::sReversedCollideShape);
+		CollisionDispatch::sRegisterCastShape(EShapeSubType::Triangle, s, CollisionDispatch::sReversedCastShape);
 	}
 
 	// Specialized collision functions

Jolt/Physics/SoftBody/SoftBodyShape.cpp

@@ -328,6 +328,9 @@ void SoftBodyShape::sRegister()
 	{
 		CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::SoftBody, sCollideConvexVsSoftBody);
 		CollisionDispatch::sRegisterCastShape(s, EShapeSubType::SoftBody, sCastConvexVsSoftBody);
+
+		CollisionDispatch::sRegisterCollideShape(EShapeSubType::SoftBody, s, CollisionDispatch::sReversedCollideShape);
+		CollisionDispatch::sRegisterCastShape(EShapeSubType::SoftBody, s, CollisionDispatch::sReversedCastShape);
 	}
 
 	// Specialized collision functions

Samples/Tests/ConvexCollision/ConvexHullShrinkTest.cpp

@@ -141,7 +141,7 @@ void ConvexHullShrinkTest::PrePhysicsUpdate(const PreUpdateParams &inParams)
 		// Get the support function of the shape excluding convex radius and add the convex radius
 		ConvexShape::SupportBuffer buffer;
 		const ConvexShape::Support *support = shape->GetSupportFunction(ConvexShape::ESupportMode::ExcludeConvexRadius, buffer, Vec3::sReplicate(1.0f));
-		AddConvexRadius<ConvexShape::Support> add_cvx(*support, convex_radius);
+		AddConvexRadius add_cvx(*support, convex_radius);
 
 		// Calculate the error w.r.t. the original hull
 		float max_error = -FLT_MAX;

Samples/Tests/ConvexCollision/EPATest.cpp

@@ -33,8 +33,8 @@ bool EPATest::CollideBoxSphere(Mat44Arg inMatrix, const AABox &inBox, const Sphe
 	DrawSphereSP(mDebugRenderer, inMatrix * inSphere.GetCenter(), inSphere.GetRadius(), Color::sGrey);
 
 	// Transform the box and sphere according to inMatrix
-	TransformedConvexObject<AABox> transformed_box(inMatrix, inBox);
-	TransformedConvexObject<Sphere> transformed_sphere(inMatrix, inSphere);
+	TransformedConvexObject transformed_box(inMatrix, inBox);
+	TransformedConvexObject transformed_sphere(inMatrix, inSphere);
 
 	// Run the EPA algorithm
 	EPAPenetrationDepth epa;

Samples/Tests/ConvexCollision/InteractivePairsTest.cpp

@@ -110,10 +110,10 @@ void InteractivePairsTest::PrePhysicsUpdate(const PreUpdateParams &inParams)
 void InteractivePairsTest::TestBoxVsBox(Vec3Arg inTranslationA, Vec3Arg inRotationA, float inConvexRadiusA, const AABox &inA, Vec3Arg inTranslationB, Vec3Arg inRotationB, float inConvexRadiusB, const AABox &inB)
 {
 	Mat44 mat_a = Mat44::sTranslation(inTranslationA) * Mat44::sRotationX(inRotationA.GetX()) * Mat44::sRotationY(inRotationA.GetY()) * Mat44::sRotationZ(inRotationA.GetZ());
-	TransformedConvexObject<AABox> a(mat_a, inA);
+	TransformedConvexObject a(mat_a, inA);
 
 	Mat44 mat_b = Mat44::sTranslation(inTranslationB) * Mat44::sRotationX(inRotationB.GetX()) * Mat44::sRotationY(inRotationB.GetY()) * Mat44::sRotationZ(inRotationB.GetZ());
-	TransformedConvexObject<AABox> b(mat_b, inB);
+	TransformedConvexObject b(mat_b, inB);
 
 	EPAPenetrationDepth pen_depth;
 	Vec3 v = Vec3::sAxisX(), pa, pb;
@@ -130,8 +130,8 @@ void InteractivePairsTest::TestBoxVsBox(Vec3Arg inTranslationA, Vec3Arg inRotati
 	if (inConvexRadiusA > 0.0f)
 		DrawWireBoxSP(mDebugRenderer, mat_a, widened_a, Color::sWhite);
 
-	AddConvexRadius<TransformedConvexObject<AABox>> a_inc(a, inConvexRadiusA);
-	AddConvexRadius<TransformedConvexObject<AABox>> b_inc(b, inConvexRadiusB);
+	AddConvexRadius a_inc(a, inConvexRadiusA);
+	AddConvexRadius b_inc(b, inConvexRadiusB);
 
 	if (pen_depth.GetPenetrationDepth(a, a_inc, inConvexRadiusA, b, b_inc, inConvexRadiusB, 1.0e-4f, FLT_EPSILON, v, pa, pb))
 	{
@@ -154,7 +154,7 @@ void InteractivePairsTest::TestSphereVsBox(Vec3Arg inTranslationA, float inRadiu
 {
 	Sphere s(inTranslationA, inRadiusA);
 	Mat44 mat_b = Mat44::sTranslation(inTranslationB) * Mat44::sRotationX(inRotationB.GetX()) * Mat44::sRotationY(inRotationB.GetY()) * Mat44::sRotationZ(inRotationB.GetZ());
-	TransformedConvexObject<AABox> b(mat_b, inB);
+	TransformedConvexObject b(mat_b, inB);
 
 	AABox widened_b = inB;
 	widened_b.ExpandBy(Vec3::sReplicate(inConvexRadiusB));
@@ -164,7 +164,7 @@ void InteractivePairsTest::TestSphereVsBox(Vec3Arg inTranslationA, float inRadiu
 
 	DrawSphereSP(mDebugRenderer, inTranslationA, inRadiusA, Color::sWhite);
 
-	AddConvexRadius<TransformedConvexObject<AABox>> b_inc(b, inConvexRadiusB);
+	AddConvexRadius	b_inc(b, inConvexRadiusB);
 
 	if (pen_depth.GetPenetrationDepth(s, s, 0.0f, b, b_inc, inConvexRadiusB, 1.0e-4f, FLT_EPSILON, v, pa, pb))
 	{

UnitTests/Geometry/EPATests.cpp

@@ -27,8 +27,8 @@ TEST_SUITE("EPATests")
 	/// @return If a collision was detected
 	static bool CollideBoxSphere(Mat44Arg inMatrix, const AABox &inBox, const Sphere &inSphere)
 	{
-		TransformedConvexObject<AABox> transformed_box(inMatrix, inBox);
-		TransformedConvexObject<Sphere> transformed_sphere(inMatrix, inSphere);
+		TransformedConvexObject transformed_box(inMatrix, inBox);
+		TransformedConvexObject transformed_sphere(inMatrix, inSphere);
 
 		// Use EPA algorithm. Don't use convex radius to avoid EPA being skipped because the inner hulls are not touching.
 		EPAPenetrationDepth epa;

UnitTests/Physics/CollideShapeTests.cpp

@@ -483,4 +483,57 @@ TEST_SUITE("CollideShapeTests")
 
 		CHECK(angle >= 2.0f * JPH_PI);
 	}
+
+	// This test checks extreme values of the max separation distance and how it affects ConvexShape::sCollideConvexVsConvex
+	// See: https://github.com/jrouwe/JoltPhysics/discussions/1379
+	TEST_CASE("TestBoxVsSphereLargeSeparationDistance")
+	{
+		constexpr float cRadius = 1.0f;
+		constexpr float cHalfExtent = 10.0f;
+		RefConst<Shape> sphere_shape = new SphereShape(cRadius);
+		RefConst<Shape> box_shape = new BoxShape(Vec3::sReplicate(cHalfExtent));
+		float distances[] = { 0.0f, 0.5f, 1.0f, 5.0f, 10.0f, 50.0f, 100.0f, 500.0f, 1000.0f, 5000.0f, 10000.0f };
+		for (float x : distances)
+			for (float max_separation : distances)
+			{
+				CollideShapeSettings collide_settings;
+				collide_settings.mMaxSeparationDistance = max_separation;
+				ClosestHitCollisionCollector<CollideShapeCollector> collector;
+				CollisionDispatch::sCollideShapeVsShape(box_shape, sphere_shape, Vec3::sReplicate(1.0f), Vec3::sReplicate(1.0f), Mat44::sIdentity(), Mat44::sTranslation(Vec3(x, 0, 0)), SubShapeIDCreator(), SubShapeIDCreator(), collide_settings, collector);
+
+				float expected_penetration = cHalfExtent - (x - cRadius);
+				if (collector.HadHit())
+					CHECK_APPROX_EQUAL(expected_penetration, collector.mHit.mPenetrationDepth, 1.0e-3f);
+				else
+					CHECK(expected_penetration < -max_separation);
+			}
+	}
+
+	// This test case checks extreme values of the max separation distance and how it affects CollideConvexVsTriangles::Collide
+	// See: https://github.com/jrouwe/JoltPhysics/discussions/1379
+	TEST_CASE("TestTriangleVsBoxLargeSeparationDistance")
+	{
+		constexpr float cTriangleX = -0.1f;
+		constexpr float cHalfExtent = 10.0f;
+		RefConst<Shape> triangle_shape = new TriangleShape(Vec3(cTriangleX, -10, 10), Vec3(cTriangleX, -10, -10), Vec3(cTriangleX, 10, 0));
+		RefConst<Shape> box_shape = new BoxShape(Vec3::sReplicate(cHalfExtent));
+		float distances[] = { 0.0f, 0.5f, 1.0f, 5.0f, 10.0f, 50.0f, 100.0f, 500.0f, 1000.0f, 5000.0f, 10000.0f };
+		for (float x : distances)
+			for (float max_separation : distances)
+			{
+				CollideShapeSettings collide_settings;
+				collide_settings.mMaxSeparationDistance = max_separation;
+				ClosestHitCollisionCollector<CollideShapeCollector> collector;
+				CollisionDispatch::sCollideShapeVsShape(triangle_shape, box_shape, Vec3::sReplicate(1.0f), Vec3::sReplicate(1.0f), Mat44::sIdentity(), Mat44::sTranslation(Vec3(x, 0, 0)), SubShapeIDCreator(), SubShapeIDCreator(), collide_settings, collector);
+
+				float expected_penetration = cTriangleX - (x - cHalfExtent);
+				if (collector.HadHit())
+					CHECK_APPROX_EQUAL(expected_penetration, collector.mHit.mPenetrationDepth, 1.0e-3f);
+				else
+				{
+					CHECK(expected_penetration < -max_separation);
+					CHECK_APPROX_EQUAL(collector.mHit.mPenetrationAxis.NormalizedOr(Vec3::sZero()), Vec3::sAxisX(), 1.0e-5f);
+				}
+			}
+	}
 }