Fixed collision between soft body and TriangleShape/MeshShape/HeightFieldShape (#1825)

* This would not find the closest collision point in case the shape was scaled. It would also make the triangles much thicker than intended causing collisions with back facing triangles that were very far away.
* Added ability to configure the thickness of triangles when colliding with soft bodies through CollideSoftBodyVerticesVsTriangles::sTriangleThickness.

Docs/ReleaseNotes.md

@@ -9,6 +9,7 @@ For breaking API changes see [this document](https://github.com/jrouwe/JoltPhysi
 * Added new define `JPH_TRACK_SIMULATION_STATS` which tracks simulation statistics on a per body basis. This can be used to figure out bodies are the most expensive to simulate.
 * Added `RagdollSettings::CalculateConstraintPriorities` which calculates constraint priorities that boost the priority of joints towards the root of the ragdoll.
 * BoxShape, CylinderShape and TaperedCylinderShape will now automatically reduce the convex radius if the specified value is too big for the shape (instead of erroring out).
+* Added ability to configure the thickness of triangles when colliding with soft bodies through `CollideSoftBodyVerticesVsTriangles::sTriangleThickness`.
 
 ### Bug Fixes
 
@@ -19,6 +20,7 @@ For breaking API changes see [this document](https://github.com/jrouwe/JoltPhysi
 * Fixed cast shape possibly not returning a hit when a shape cast starts touching (but not intersecting) another shape and requesting the deepest hit.
 * Fixed division by zero when doing a really long (6 KM) sphere cast against a triangle. In this case the floating point accuracy became low enough so that the distance between the sphere center and the triangle (which should be 'radius') became zero instead.
 * Fixed memory leak when providing invalid parameters to TaperedCylinderShapeSettings and creating the shape.
+* Fixed collision between soft body and `TriangleShape`/`MeshShape`/`HeightFieldShape`. This would not find the closest collision point in case the shape was scaled. It would also make the triangles much thicker than intended causing collisions with back facing triangles that were very far away.
 
 ## v5.4.0
 

Jolt/Physics/Collision/CollideSoftBodyVerticesVsTriangles.h

@@ -14,8 +14,9 @@ class JPH_EXPORT CollideSoftBodyVerticesVsTriangles
 {
 public:
 						CollideSoftBodyVerticesVsTriangles(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) :
-		mTransform(inCenterOfMassTransform * Mat44::sScale(inScale)),
-		mInvTransform(mTransform.Inversed()),
+		mTransform(inCenterOfMassTransform),
+		mInvTransform(mTransform.InversedRotationTranslation()),
+		mScale(inScale),
 		mNormalSign(ScaleHelpers::IsInsideOut(inScale)? -1.0f : 1.0f)
 	{
 	}
@@ -28,15 +29,20 @@ public:
 
 	JPH_INLINE void		ProcessTriangle(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2)
 	{
+		// Apply the scale to the triangle
+		Vec3 v0 = mScale * inV0;
+		Vec3 v1 = mScale * inV1;
+		Vec3 v2 = mScale * inV2;
+
 		// Get the closest point from the vertex to the triangle
 		uint32 set;
-		Vec3 closest_point = ClosestPoint::GetClosestPointOnTriangle(inV0 - mLocalPosition, inV1 - mLocalPosition, inV2 - mLocalPosition, set);
+		Vec3 closest_point = ClosestPoint::GetClosestPointOnTriangle(v0 - mLocalPosition, v1 - mLocalPosition, v2 - mLocalPosition, set);
 		float dist_sq = closest_point.LengthSq();
 		if (dist_sq < mClosestDistanceSq)
 		{
-			mV0 = inV0;
-			mV1 = inV1;
-			mV2 = inV2;
+			mV0 = v0;
+			mV1 = v1;
+			mV2 = v2;
 			mClosestPoint = closest_point;
 			mClosestDistanceSq = dist_sq;
 			mSet = set;
@@ -55,10 +61,10 @@ public:
 
 			if (mSet == 0b111)
 			{
-				// Closest is interior to the triangle, use plane as collision plane but don't allow more than 0.1 m penetration
+				// Closest is interior to the triangle, use plane as collision plane but don't allow more than sTriangleThickness penetration
 				// because otherwise a triangle half a level a way will have a huge penetration if it is back facing
-				float penetration = min(triangle_normal.Dot(v0 - ioVertex.GetPosition()), 0.1f);
-				if (ioVertex.UpdatePenetration(penetration))
+				float penetration = triangle_normal.Dot(v0 - ioVertex.GetPosition());
+				if (penetration < sTriangleThickness && ioVertex.UpdatePenetration(penetration))
 					ioVertex.SetCollision(Plane::sFromPointAndNormal(v0, triangle_normal), inCollidingShapeIndex);
 			}
 			else
@@ -77,8 +83,14 @@ public:
 		}
 	}
 
+	/// Triangles are considered to have some thickness. This thickness extends backwards along the negative triangle normal.
+	/// Make this value smaller than the smallest 'wall thickness' so that the back side of the triangle doesn't protrude through the other side.
+	/// Make this value too small and tunneling is more likely to occur.
+	static inline float	sTriangleThickness = 0.1f;
+
 	Mat44				mTransform;
 	Mat44				mInvTransform;
+	Vec3				mScale;
 	Vec3				mLocalPosition;
 	Vec3				mV0, mV1, mV2;
 	Vec3				mClosestPoint;

Jolt/Physics/Collision/Shape/HeightFieldShape.cpp

@@ -2257,8 +2257,12 @@ void HeightFieldShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform,
 
 		JPH_INLINE int	VisitRangeBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop)
 		{
+			// Scale the bounding boxes of this node
+			Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z;
+			AABox4Scale(mScale, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
+
 			// Get distance to vertex
-			Vec4 dist_sq = AABox4DistanceSqToPoint(mLocalPosition, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
+			Vec4 dist_sq = AABox4DistanceSqToPoint(mLocalPosition, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
 
 			// Clear distance for invalid bounds
 			dist_sq = Vec4::sSelect(Vec4::sReplicate(FLT_MAX), dist_sq, Vec4::sLessOrEqual(inBoundsMinY, inBoundsMaxY));

Jolt/Physics/Collision/Shape/MeshShape.cpp

@@ -849,8 +849,12 @@ void MeshShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Ar
 
 		JPH_INLINE int	VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop)
 		{
+			// Scale the bounding boxes of this node
+			Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z;
+			AABox4Scale(mScale, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
+
 			// Get distance to vertex
-			Vec4 dist_sq = AABox4DistanceSqToPoint(mLocalPosition, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
+			Vec4 dist_sq = AABox4DistanceSqToPoint(mLocalPosition, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
 
 			// Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom)
 			return SortReverseAndStore(dist_sq, mClosestDistanceSq, ioProperties, &mDistanceStack[inStackTop]);