Fixed a bug in ManifoldBetweenTwoFaces (#1488)

This led to incorrect ContactManifold::mRelativeContactPointsOn2 when the contact normal and the face normal were not roughly parallel. Also it led to possibly jitter in the simulation in that case.

.github/workflows/determinism_check.yml

@@ -1,10 +1,10 @@
 name: Determinism Check
 
 env:
-  CONVEX_VS_MESH_HASH: '0x610d538e15420778'
-  RAGDOLL_HASH: '0x275057ded572c916'
-  PYRAMID_HASH: '0x198b8eeaee57e29a'
-  CHARACTER_VIRTUAL_HASH: '0x8fbe7347b302ac43'
+  CONVEX_VS_MESH_HASH: '0x554adfba1815d6af'
+  RAGDOLL_HASH: '0xdf768b4736057c87'
+  PYRAMID_HASH: '0x2e2dda8c1f4eb906'
+  CHARACTER_VIRTUAL_HASH: '0x4ec98831ce0590ff'
   EMSCRIPTEN_VERSION: 3.1.73
   UBUNTU_CLANG_VERSION: clang++-18
   UBUNTU_GCC_VERSION: g++-14

Docs/ReleaseNotes.md

@@ -39,6 +39,7 @@ For breaking API changes see [this document](https://github.com/jrouwe/JoltPhysi
 * Fixed numerical inaccuracy in penetration depth calculation when CollideShapeSettings::mMaxSeparationDistance was set to a really high value (e.g. 1000).
 * Bugfix in Semaphore::Acquire for non-windows platform. The count was updated before waiting, meaning that the counter would become -(number of waiting threads) and the semaphore would not wake up until at least the same amount of releases was done. In practice this meant that the main thread had to do the last (number of threads) jobs, slowing down the simulation a bit.
 * An empty MutableCompoundShape now returns the same local bounding box as EmptyShape (a point at (0, 0, 0)). This prevents floating point overflow exceptions.
+* Fixed a bug in ManifoldBetweenTwoFaces that led to incorrect ContactManifold::mRelativeContactPointsOn2 when the contact normal and the face normal were not roughly parallel. Also it led to possibly jitter in the simulation in that case.
 
 ## v5.2.0
 

Jolt/Physics/Collision/ManifoldBetweenTwoFaces.cpp

@@ -134,7 +134,7 @@ void PruneContactPoints(Vec3Arg inPenetrationAxis, ContactPoints &ioContactPoint
 	ioContactPointsOn2 = points_to_keep_on_2;
 }
 
-void ManifoldBetweenTwoFaces(Vec3Arg inContactPoint1, Vec3Arg inContactPoint2, Vec3Arg inPenetrationAxis, float inMaxContactDistanceSq	, const ConvexShape::SupportingFace &inShape1Face, const ConvexShape::SupportingFace &inShape2Face, ContactPoints &outContactPoints1, ContactPoints &outContactPoints2 JPH_IF_DEBUG_RENDERER(, RVec3Arg inCenterOfMass))
+void ManifoldBetweenTwoFaces(Vec3Arg inContactPoint1, Vec3Arg inContactPoint2, Vec3Arg inPenetrationAxis, float inMaxContactDistance, const ConvexShape::SupportingFace &inShape1Face, const ConvexShape::SupportingFace &inShape2Face, ContactPoints &outContactPoints1, ContactPoints &outContactPoints2 JPH_IF_DEBUG_RENDERER(, RVec3Arg inCenterOfMass))
 {
 #ifdef JPH_DEBUG_RENDERER
 	if (ContactConstraintManager::sDrawContactPoint)
@@ -165,7 +165,7 @@ void ManifoldBetweenTwoFaces(Vec3Arg inContactPoint1, Vec3Arg inContactPoint2, V
 		else if (inShape1Face.size() == 2)
 			ClipPolyVsEdge(inShape2Face, inShape1Face[0], inShape1Face[1], inPenetrationAxis, clipped_face);
 
-		// Project the points back onto the plane of shape 1 face and only keep those that are behind the plane
+		// Determine plane origin and normal for shape 1
 		Vec3 plane_origin = inShape1Face[0];
 		Vec3 plane_normal;
 		Vec3 first_edge = inShape1Face[1] - plane_origin;
@@ -180,20 +180,25 @@ void ManifoldBetweenTwoFaces(Vec3Arg inContactPoint1, Vec3Arg inContactPoint2, V
 			plane_normal = first_edge.Cross(inPenetrationAxis).Cross(first_edge);
 		}
 
-		// Check if the plane normal has any length, if not the clipped shape is so small that we'll just use the contact points
-		float plane_normal_len_sq = plane_normal.LengthSq();
-		if (plane_normal_len_sq > 0.0f)
+		// If penetration axis and plane normal are perpendicular, fall back to the contact points
+		float penetration_axis_dot_plane_normal = inPenetrationAxis.Dot(plane_normal);
+		if (penetration_axis_dot_plane_normal != 0.0f)
 		{
-			// Discard points of faces that are too far away to collide
+			float penetration_axis_len = inPenetrationAxis.Length();
+
 			for (Vec3 p2 : clipped_face)
 			{
-				float distance = (p2 - plane_origin).Dot(plane_normal); // Note should divide by length of plane_normal (unnormalized here)
-				if (distance <= 0.0f || Square(distance) < inMaxContactDistanceSq * plane_normal_len_sq) // Must be close enough to plane, note we correct for not dividing by plane normal length here
+				// Project clipped face back onto the plane of face 1, we do this by solving:
+				// p1 = p2 + distance * penetration_axis / |penetration_axis|
+				// (p1 - plane_origin) . plane_normal = 0
+				// This gives us:
+				// distance = -|penetration_axis| * (p2 - plane_origin) . plane_normal / penetration_axis . plane_normal
+				float distance = (p2 - plane_origin).Dot(plane_normal) / penetration_axis_dot_plane_normal; // note left out -|penetration_axis| term
+
+				// If the point is behind or less then inMaxContactDistance in front of the plane of face 2, add it as a contact point
+				if (distance <= 0.0f || distance * penetration_axis_len < inMaxContactDistance)
 				{
-					// Project point back on shape 1 using the normal, note we correct for not dividing by plane normal length here:
-					// p1 = p2 - (distance / sqrt(plane_normal_len_sq)) * (plane_normal / sqrt(plane_normal_len_sq));
-					Vec3 p1 = p2 - (distance / plane_normal_len_sq) * plane_normal;
-
+					Vec3 p1 = p2 - distance * inPenetrationAxis;
 					outContactPoints1.push_back(p1);
 					outContactPoints2.push_back(p2);
 				}
@@ -213,15 +218,19 @@ void ManifoldBetweenTwoFaces(Vec3Arg inContactPoint1, Vec3Arg inContactPoint2, V
 			DebugRenderer::sInstance->DrawWirePolygon(com, inShape2Face, Color::sGreen, 0.05f);
 
 			// Draw normal
-			if (plane_normal_len_sq > 0.0f)
+			float plane_normal_len = plane_normal.Length();
+			if (plane_normal_len > 0.0f)
 			{
 				RVec3 plane_origin_ws = inCenterOfMass + plane_origin;
-				DebugRenderer::sInstance->DrawArrow(plane_origin_ws, plane_origin_ws + plane_normal / sqrt(plane_normal_len_sq), Color::sYellow, 0.05f);
+				DebugRenderer::sInstance->DrawArrow(plane_origin_ws, plane_origin_ws + plane_normal / plane_normal_len, Color::sYellow, 0.05f);
 			}
 
 			// Draw contact points that remain after distance check
 			for (ContactPoints::size_type p = old_size; p < outContactPoints1.size(); ++p)
+			{
 				DebugRenderer::sInstance->DrawMarker(inCenterOfMass + outContactPoints1[p], Color::sYellow, 0.1f);
+				DebugRenderer::sInstance->DrawMarker(inCenterOfMass + outContactPoints2[p], Color::sOrange, 0.1f);
+			}
 		}
 	#endif // JPH_DEBUG_RENDERER
 	}

Jolt/Physics/PhysicsSettings.h

@@ -55,8 +55,8 @@ struct PhysicsSettings
 	/// Fraction of its inner radius a body may penetrate another body for the LinearCast motion quality
 	float		mLinearCastMaxPenetration = 0.25f;
 
-	/// Max squared distance to use to determine if two points are on the same plane for determining the contact manifold between two shape faces (unit: meter^2)
-	float		mManifoldToleranceSq = 1.0e-6f;
+	/// Max distance to use to determine if two points are on the same plane for determining the contact manifold between two shape faces (unit: meter)
+	float		mManifoldTolerance = 1.0e-3f;
 
 	/// Maximum distance to correct in a single iteration when solving position constraints (unit: meters)
 	float		mMaxPenetrationDistance = 0.2f;

Jolt/Physics/PhysicsSystem.cpp

@@ -1121,7 +1121,7 @@ void PhysicsSystem::ProcessBodyPair(ContactAllocator &ioContactAllocator, const
 
 					// Determine contact points
 					const PhysicsSettings &settings = mSystem->mPhysicsSettings;
-					ManifoldBetweenTwoFaces(inResult.mContactPointOn1, inResult.mContactPointOn2, inResult.mPenetrationAxis, Square(settings.mSpeculativeContactDistance) + settings.mManifoldToleranceSq, inResult.mShape1Face, inResult.mShape2Face, manifold->mRelativeContactPointsOn1, manifold->mRelativeContactPointsOn2 JPH_IF_DEBUG_RENDERER(, mBody1->GetCenterOfMassPosition()));
+					ManifoldBetweenTwoFaces(inResult.mContactPointOn1, inResult.mContactPointOn2, inResult.mPenetrationAxis, settings.mSpeculativeContactDistance + settings.mManifoldTolerance, inResult.mShape1Face, inResult.mShape2Face, manifold->mRelativeContactPointsOn1, manifold->mRelativeContactPointsOn2 JPH_IF_DEBUG_RENDERER(, mBody1->GetCenterOfMassPosition()));
 
 					// Prune if we have more than 32 points (this means we could run out of space in the next iteration)
 					if (manifold->mRelativeContactPointsOn1.size() > 32)
@@ -1207,7 +1207,7 @@ void PhysicsSystem::ProcessBodyPair(ContactAllocator &ioContactAllocator, const
 					ContactManifold manifold;
 					manifold.mBaseOffset = mBody1->GetCenterOfMassPosition();
 					const PhysicsSettings &settings = mSystem->mPhysicsSettings;
-					ManifoldBetweenTwoFaces(inResult.mContactPointOn1, inResult.mContactPointOn2, inResult.mPenetrationAxis, Square(settings.mSpeculativeContactDistance) + settings.mManifoldToleranceSq, inResult.mShape1Face, inResult.mShape2Face, manifold.mRelativeContactPointsOn1, manifold.mRelativeContactPointsOn2 JPH_IF_DEBUG_RENDERER(, manifold.mBaseOffset));
+					ManifoldBetweenTwoFaces(inResult.mContactPointOn1, inResult.mContactPointOn2, inResult.mPenetrationAxis, settings.mSpeculativeContactDistance + settings.mManifoldTolerance, inResult.mShape1Face, inResult.mShape2Face, manifold.mRelativeContactPointsOn1, manifold.mRelativeContactPointsOn2 JPH_IF_DEBUG_RENDERER(, manifold.mBaseOffset));
 
 					// Calculate normal
 					manifold.mWorldSpaceNormal = inResult.mPenetrationAxis.Normalized();
@@ -1929,7 +1929,7 @@ void PhysicsSystem::JobFindCCDContacts(const PhysicsUpdateContext *ioContext, Ph
 			// Determine contact manifold
 			ContactManifold manifold;
 			manifold.mBaseOffset = shape_cast.mCenterOfMassStart.GetTranslation();
-			ManifoldBetweenTwoFaces(cast_shape_result.mContactPointOn1, cast_shape_result.mContactPointOn2, cast_shape_result.mPenetrationAxis, mPhysicsSettings.mManifoldToleranceSq, cast_shape_result.mShape1Face, cast_shape_result.mShape2Face, manifold.mRelativeContactPointsOn1, manifold.mRelativeContactPointsOn2 JPH_IF_DEBUG_RENDERER(, manifold.mBaseOffset));
+			ManifoldBetweenTwoFaces(cast_shape_result.mContactPointOn1, cast_shape_result.mContactPointOn2, cast_shape_result.mPenetrationAxis, mPhysicsSettings.mManifoldTolerance, cast_shape_result.mShape1Face, cast_shape_result.mShape2Face, manifold.mRelativeContactPointsOn1, manifold.mRelativeContactPointsOn2 JPH_IF_DEBUG_RENDERER(, manifold.mBaseOffset));
 			manifold.mSubShapeID1 = cast_shape_result.mSubShapeID1;
 			manifold.mSubShapeID2 = cast_shape_result.mSubShapeID2;
 			manifold.mPenetrationDepth = cast_shape_result.mPenetrationDepth;