Added support for a vertex radius for soft bodies. (#879)

This keeps the vertices a fixed distance away from the surface which can be used to avoid z-fighting while rendering the soft body.

In order to support this the CollideSoftBodyVertices functions for MeshShape and HeightFieldShape had to be rewritten as the ray cast by sCollideSoftBodyVerticesUsingRayCast was too short when the vertices were not touching the surface. Now we're finding the closest triangle and using that to calculate a collision plane.

See #850

Docs/ReleaseNotes.md

@@ -19,6 +19,7 @@ For breaking API changes see [this document](https://github.com/jrouwe/JoltPhysi
 * Added function to query the bounding box of all bodies in the physics system, see PhysicsSystem::GetBounds.
 * Renamed SensorDetectsStatic to CollideKinematicVsNonDynamic and made it work for non-sensors. This means that kinematic bodies can now get collision callbacks when they collide with other static / kinematic objects.
 * CharacterVirtual will now receive an OnContactAdded callback when it collides with a sensor (but will have no further interaction).
+* Added support for a vertex radius for soft bodies. This keeps the vertices a fixed distance away from the surface which can be used to avoid z-fighting while rendering the soft body.
 
 ### Improvements
 * Multithreading the SetupVelocityConstraints job. This was causing a bottleneck in the case that there are a lot of constraints but very few possible collisions.

Jolt/Geometry/AABox4.h

@@ -189,16 +189,29 @@ JPH_INLINE UVec4 AABox4VsBox(const OrientedBox &inBox, Vec4Arg inBoxMinX, Vec4Ar
 	return AABox4VsBox(inBox.mOrientation, inBox.mHalfExtents, inBoxMinX, inBoxMinY, inBoxMinZ, inBoxMaxX, inBoxMaxY, inBoxMaxZ, inEpsilon);
 }
 
-/// Test 4 AABoxes vs a sphere
-JPH_INLINE UVec4 AABox4VsSphere(Vec4Arg inCenterX, Vec4Arg inCenterY, Vec4Arg inCenterZ, Vec4Arg inRadiusSq, Vec4Arg inBoxMinX, Vec4Arg inBoxMinY, Vec4Arg inBoxMinZ, Vec4Arg inBoxMaxX, Vec4Arg inBoxMaxY, Vec4Arg inBoxMaxZ)
+/// Get the squared distance between 4 AABoxes and a point
+JPH_INLINE Vec4 AABox4DistanceSqToPoint(Vec4Arg inPointX, Vec4Arg inPointY, Vec4Arg inPointZ, Vec4Arg inBoxMinX, Vec4Arg inBoxMinY, Vec4Arg inBoxMinZ, Vec4Arg inBoxMaxX, Vec4Arg inBoxMaxY, Vec4Arg inBoxMaxZ)
 {
 	// Get closest point on box
-	Vec4 closest_x = Vec4::sMin(Vec4::sMax(inCenterX, inBoxMinX), inBoxMaxX);
-	Vec4 closest_y = Vec4::sMin(Vec4::sMax(inCenterY, inBoxMinY), inBoxMaxY);
-	Vec4 closest_z = Vec4::sMin(Vec4::sMax(inCenterZ, inBoxMinZ), inBoxMaxZ);
+	Vec4 closest_x = Vec4::sMin(Vec4::sMax(inPointX, inBoxMinX), inBoxMaxX);
+	Vec4 closest_y = Vec4::sMin(Vec4::sMax(inPointY, inBoxMinY), inBoxMaxY);
+	Vec4 closest_z = Vec4::sMin(Vec4::sMax(inPointZ, inBoxMinZ), inBoxMaxZ);
+
+	// Return the squared distance between the box and point
+	return Square(closest_x - inPointX) + Square(closest_y - inPointY) + Square(closest_z - inPointZ);
+}
+
+/// Get the squared distance between 4 AABoxes and a point
+JPH_INLINE Vec4 AABox4DistanceSqToPoint(Vec3 inPoint, Vec4Arg inBoxMinX, Vec4Arg inBoxMinY, Vec4Arg inBoxMinZ, Vec4Arg inBoxMaxX, Vec4Arg inBoxMaxY, Vec4Arg inBoxMaxZ)
+{
+	return AABox4DistanceSqToPoint(inPoint.SplatX(), inPoint.SplatY(), inPoint.SplatZ(), inBoxMinX, inBoxMinY, inBoxMinZ, inBoxMaxX, inBoxMaxY, inBoxMaxZ);
+}
 
+/// Test 4 AABoxes vs a sphere
+JPH_INLINE UVec4 AABox4VsSphere(Vec4Arg inCenterX, Vec4Arg inCenterY, Vec4Arg inCenterZ, Vec4Arg inRadiusSq, Vec4Arg inBoxMinX, Vec4Arg inBoxMinY, Vec4Arg inBoxMinZ, Vec4Arg inBoxMaxX, Vec4Arg inBoxMaxY, Vec4Arg inBoxMaxZ)
+{
 	// Test the distance from the center of the sphere to the box is smaller than the radius
-	Vec4 distance_sq = Square(closest_x - inCenterX) + Square(closest_y - inCenterY) + Square(closest_z - inCenterZ);
+	Vec4 distance_sq = AABox4DistanceSqToPoint(inCenterX, inCenterY, inCenterZ, inBoxMinX, inBoxMinY, inBoxMinZ, inBoxMaxX, inBoxMaxY, inBoxMaxZ);
 	return Vec4::sLessOrEqual(distance_sq, inRadiusSq);
 }
 

Jolt/Jolt.cmake

@@ -222,6 +222,7 @@ set(JOLT_PHYSICS_SRC_FILES
 	${JOLT_PHYSICS_ROOT}/Physics/Collision/CollideConvexVsTriangles.h
 	${JOLT_PHYSICS_ROOT}/Physics/Collision/CollidePointResult.h
 	${JOLT_PHYSICS_ROOT}/Physics/Collision/CollideShape.h
+	${JOLT_PHYSICS_ROOT}/Physics/Collision/CollideSoftBodyVerticesVsTriangles.h
 	${JOLT_PHYSICS_ROOT}/Physics/Collision/CollideSphereVsTriangles.cpp
 	${JOLT_PHYSICS_ROOT}/Physics/Collision/CollideSphereVsTriangles.h
 	${JOLT_PHYSICS_ROOT}/Physics/Collision/CollisionCollector.h

Jolt/Physics/Collision/CollideSoftBodyVerticesVsTriangles.h

@@ -0,0 +1,98 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2024 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/SoftBody/SoftBodyVertex.h>
+#include <Jolt/Geometry/ClosestPoint.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Collision detection helper that collides soft body vertices vs triangles
+class JPH_EXPORT CollideSoftBodyVerticesVsTriangles
+{
+public:
+						CollideSoftBodyVerticesVsTriangles(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) :
+		mTransform(inCenterOfMassTransform * Mat44::sScale(inScale)),
+		mInvTransform(mTransform.Inversed()),
+		mNormalSign(ScaleHelpers::IsInsideOut(inScale)? -1.0f : 1.0f)
+	{
+	}
+
+	JPH_INLINE void		StartVertex(const SoftBodyVertex &inVertex)
+	{
+		mLocalPosition = mInvTransform * inVertex.mPosition;
+		mClosestDistanceSq = FLT_MAX;
+	}
+
+	JPH_INLINE void		ProcessTriangle(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg 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);
+		float dist_sq = closest_point.LengthSq();
+		if (dist_sq < mClosestDistanceSq)
+		{
+			mV0 = inV0;
+			mV1 = inV1;
+			mV2 = inV2;
+			mClosestPoint = closest_point;
+			mClosestDistanceSq = dist_sq;
+			mSet = set;
+		}
+	}
+
+	JPH_INLINE void		FinishVertex(SoftBodyVertex &ioVertex, int inCollidingShapeIndex) const
+	{
+		if (mClosestDistanceSq < FLT_MAX)
+		{
+			// Convert triangle to world space
+			Vec3 v0 = mTransform * mV0;
+			Vec3 v1 = mTransform * mV1;
+			Vec3 v2 = mTransform * mV2;
+			Vec3 triangle_normal = mNormalSign * (v1 - v0).Cross(v2 - v0).NormalizedOr(Vec3::sAxisY());
+
+			if (mSet == 0b111)
+			{
+				// Closest is interior to the triangle, use plane as collision plane but don't allow more than 0.1 m 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.mPosition), 0.1f);
+				if (penetration > ioVertex.mLargestPenetration)
+				{
+					ioVertex.mLargestPenetration = penetration;
+					ioVertex.mCollisionPlane = Plane::sFromPointAndNormal(v0, triangle_normal);
+					ioVertex.mCollidingShapeIndex = inCollidingShapeIndex;
+				}
+			}
+			else
+			{
+				// Closest point is on an edge or vertex, use closest point as collision plane
+				Vec3 closest_point = mTransform * (mLocalPosition + mClosestPoint);
+				Vec3 normal = ioVertex.mPosition - closest_point;
+				if (normal.Dot(triangle_normal) > 0.0f) // Ignore back facing edges
+				{
+					float normal_length = normal.Length();
+					float penetration = -normal_length;
+					if (penetration > ioVertex.mLargestPenetration)
+					{
+						ioVertex.mLargestPenetration = penetration;
+						ioVertex.mCollisionPlane = Plane::sFromPointAndNormal(closest_point, normal_length > 0.0f? normal / normal_length : triangle_normal);
+						ioVertex.mCollidingShapeIndex = inCollidingShapeIndex;
+					}
+				}
+			}
+		}
+	}
+
+	Mat44				mTransform;
+	Mat44				mInvTransform;
+	Vec3				mLocalPosition;
+	Vec3				mV0, mV1, mV2;
+	Vec3				mClosestPoint;
+	float				mNormalSign;
+	float				mClosestDistanceSq;
+	uint32				mSet;
+};
+
+JPH_NAMESPACE_END

Jolt/Physics/Collision/Shape/HeightFieldShape.cpp

@@ -21,6 +21,7 @@
 #include <Jolt/Physics/Collision/ActiveEdges.h>
 #include <Jolt/Physics/Collision/CollisionDispatch.h>
 #include <Jolt/Physics/Collision/SortReverseAndStore.h>
+#include <Jolt/Physics/Collision/CollideSoftBodyVerticesVsTriangles.h>
 #include <Jolt/Core/Profiler.h>
 #include <Jolt/Core/StringTools.h>
 #include <Jolt/Core/StreamIn.h>
@@ -1796,7 +1797,7 @@ private:
 };
 
 template <class Visitor>
-JPH_INLINE void HeightFieldShape::WalkHeightField(Visitor &ioVisitor) const
+void HeightFieldShape::WalkHeightField(Visitor &ioVisitor) const
 {
 	DecodingContext ctx(this);
 	ctx.WalkHeightField(ioVisitor);
@@ -1942,9 +1943,50 @@ void HeightFieldShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &in
 	// A height field doesn't have volume, so we can't test insideness
 }
 
-void HeightFieldShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const
+void HeightFieldShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, [[maybe_unused]] float inDeltaTime, [[maybe_unused]] Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const
 {
-	sCollideSoftBodyVerticesUsingRayCast(*this, inCenterOfMassTransform, inScale, ioVertices, inNumVertices, inDeltaTime, inDisplacementDueToGravity, inCollidingShapeIndex);
+	JPH_PROFILE_FUNCTION();
+
+	struct Visitor : public CollideSoftBodyVerticesVsTriangles
+	{
+		using CollideSoftBodyVerticesVsTriangles::CollideSoftBodyVerticesVsTriangles;
+
+		JPH_INLINE bool	ShouldAbort() const
+		{
+			return false;
+		}
+
+		JPH_INLINE bool	ShouldVisitRangeBlock([[maybe_unused]] int inStackTop) const
+		{
+			return mDistanceStack[inStackTop] < mClosestDistanceSq;
+		}
+
+		JPH_INLINE int	VisitRangeBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop)
+		{
+			// Get distance to vertex
+			Vec4 dist_sq = AABox4DistanceSqToPoint(mLocalPosition, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
+
+			// 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]);
+		}
+
+		JPH_INLINE void	VisitTriangle([[maybe_unused]] uint inX, [[maybe_unused]] uint inY, [[maybe_unused]] uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2)
+		{
+			ProcessTriangle(inV0, inV1, inV2);
+		}
+
+		float			mDistanceStack[cStackSize];
+	};
+
+	Visitor visitor(inCenterOfMassTransform, inScale);
+
+	for (SoftBodyVertex *v = ioVertices, *sbv_end = ioVertices + inNumVertices; v < sbv_end; ++v)
+		if (v->mInvMass > 0.0f)
+		{
+			visitor.StartVertex(*v);
+			WalkHeightField(visitor);
+			visitor.FinishVertex(*v, inCollidingShapeIndex);
+		}
 }
 
 void HeightFieldShape::sCastConvexVsHeightField(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, [[maybe_unused]] const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)

Jolt/Physics/Collision/Shape/HeightFieldShape.h

@@ -277,8 +277,9 @@ private:
 	static void						sCastSphereVsHeightField(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector);
 
 	/// Visit the entire height field using a visitor pattern
+	/// Note: Used to be inlined but this triggers a bug in MSVC where it will not free the memory allocated by alloca which causes a stack overflow when WalkHeightField is called in a loop (clang does it correct)
 	template <class Visitor>
-	JPH_INLINE void					WalkHeightField(Visitor &ioVisitor) const;
+	void							WalkHeightField(Visitor &ioVisitor) const;
 
 	/// A block of 2x2 ranges used to form a hierarchical grid, ordered left top, right top, left bottom, right bottom
 	struct alignas(16) RangeBlock

Jolt/Physics/Collision/Shape/MeshShape.cpp

@@ -20,6 +20,7 @@
 #include <Jolt/Physics/Collision/ActiveEdges.h>
 #include <Jolt/Physics/Collision/CollisionDispatch.h>
 #include <Jolt/Physics/Collision/SortReverseAndStore.h>
+#include <Jolt/Physics/Collision/CollideSoftBodyVerticesVsTriangles.h>
 #include <Jolt/Core/StringTools.h>
 #include <Jolt/Core/StreamIn.h>
 #include <Jolt/Core/StreamOut.h>
@@ -780,9 +781,50 @@ void MeshShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShap
 	sCollidePointUsingRayCast(*this, inPoint, inSubShapeIDCreator, ioCollector, inShapeFilter);
 }
 
-void MeshShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const
+void MeshShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, [[maybe_unused]] float inDeltaTime, [[maybe_unused]] Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const
 {
-	sCollideSoftBodyVerticesUsingRayCast(*this, inCenterOfMassTransform, inScale, ioVertices, inNumVertices, inDeltaTime, inDisplacementDueToGravity, inCollidingShapeIndex);
+	JPH_PROFILE_FUNCTION();
+
+	struct Visitor : public CollideSoftBodyVerticesVsTriangles
+	{
+		using CollideSoftBodyVerticesVsTriangles::CollideSoftBodyVerticesVsTriangles;
+
+		JPH_INLINE bool	ShouldAbort() const
+		{
+			return false;
+		}
+
+		JPH_INLINE bool	ShouldVisitNode([[maybe_unused]] int inStackTop) const
+		{
+			return mDistanceStack[inStackTop] < mClosestDistanceSq;
+		}
+
+		JPH_INLINE int	VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop)
+		{
+			// Get distance to vertex
+			Vec4 dist_sq = AABox4DistanceSqToPoint(mLocalPosition, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
+
+			// 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]);
+		}
+
+		JPH_INLINE void	VisitTriangle(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, [[maybe_unused]] uint8 inActiveEdges, [[maybe_unused]] SubShapeID inSubShapeID2)
+		{
+			ProcessTriangle(inV0, inV1, inV2);
+		}
+
+		float			mDistanceStack[NodeCodec::StackSize];
+	};
+
+	Visitor visitor(inCenterOfMassTransform, inScale);
+
+	for (SoftBodyVertex *v = ioVertices, *sbv_end = ioVertices + inNumVertices; v < sbv_end; ++v)
+		if (v->mInvMass > 0.0f)
+		{
+			visitor.StartVertex(*v);
+			WalkTreePerTriangle(SubShapeIDCreator(), visitor);
+			visitor.FinishVertex(*v, inCollidingShapeIndex);
+		}
 }
 
 void MeshShape::sCastConvexVsMesh(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, [[maybe_unused]] const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)

Jolt/Physics/Collision/Shape/Shape.cpp

@@ -12,7 +12,6 @@
 #include <Jolt/Physics/Collision/RayCast.h>
 #include <Jolt/Physics/Collision/CastResult.h>
 #include <Jolt/Physics/Collision/CollidePointResult.h>
-#include <Jolt/Physics/SoftBody/SoftBodyVertex.h>
 #include <Jolt/Core/StreamIn.h>
 #include <Jolt/Core/StreamOut.h>
 #include <Jolt/Core/Factory.h>
@@ -271,42 +270,6 @@ Shape::ShapeResult Shape::ScaleShape(Vec3Arg inScale) const
 	return compound.Create();
 }
 
-void Shape::sCollideSoftBodyVerticesUsingRayCast(const Shape &inShape, Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex)
-{
-	Mat44 inverse_transform = Mat44::sScale(inScale.Reciprocal()) * inCenterOfMassTransform.InversedRotationTranslation();
-	Mat44 direction_preserving_transform = inverse_transform.Transposed3x3(); // To transform normals: transpose of the inverse
-
-	for (SoftBodyVertex *v = ioVertices, *sbv_end = ioVertices + inNumVertices; v < sbv_end; ++v)
-		if (v->mInvMass > 0.0f)
-		{
-			// Calculate the distance we will move this frame
-			Vec3 movement = v->mVelocity * inDeltaTime + inDisplacementDueToGravity;
-
-			RayCastResult hit;
-			hit.mFraction = 2.0f; // Add a little extra distance in case the particle speeds up
-
-			RayCast ray(v->mPosition - 0.5f * movement, movement); // Start a little early in case we penetrated before
-
-			if (inShape.CastRay(ray.Transformed(inverse_transform), SubShapeIDCreator(), hit))
-			{
-				// Calculate penetration
-				float penetration = (0.5f - hit.mFraction) * movement.Length();
-				if (penetration > v->mLargestPenetration)
-				{
-					v->mLargestPenetration = penetration;
-
-					// Calculate contact point and normal
-					Vec3 point = ray.GetPointOnRay(hit.mFraction);
-					Vec3 normal = direction_preserving_transform.Multiply3x3(inShape.GetSurfaceNormal(hit.mSubShapeID2, inverse_transform * point)).Normalized();
-
-					// Store collision
-					v->mCollisionPlane = Plane::sFromPointAndNormal(point, normal);
-					v->mCollidingShapeIndex = inCollidingShapeIndex;
-				}
-			}
-		}
-}
-
 void Shape::sCollidePointUsingRayCast(const Shape &inShape, Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter)
 {
 	// First test if we're inside our bounding box

Jolt/Physics/Collision/Shape/Shape.h

@@ -434,9 +434,6 @@ protected:
 	/// A fallback version of CollidePoint that uses a ray cast and counts the number of hits to determine if the point is inside the shape. Odd number of hits means inside, even number of hits means outside.
 	static void						sCollidePointUsingRayCast(const Shape &inShape, Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter);
 
-	/// A fallback version of CollideSoftBodyVertices that uses a raycast to collide the vertices with the shape.
-	static void						sCollideSoftBodyVerticesUsingRayCast(const Shape &inShape, Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex);
-
 private:
 	uint64							mUserData = 0;
 	EShapeType						mShapeType;

Jolt/Physics/Collision/Shape/TriangleShape.cpp

@@ -17,7 +17,7 @@
 #include <Jolt/Physics/Collision/CollideConvexVsTriangles.h>
 #include <Jolt/Physics/Collision/CollideSphereVsTriangles.h>
 #include <Jolt/Physics/Collision/CollisionDispatch.h>
-#include <Jolt/Physics/SoftBody/SoftBodyVertex.h>
+#include <Jolt/Physics/Collision/CollideSoftBodyVerticesVsTriangles.h>
 #include <Jolt/Geometry/ConvexSupport.h>
 #include <Jolt/Geometry/RayTriangle.h>
 #include <Jolt/Geometry/ClosestPoint.h>
@@ -259,57 +259,16 @@ void TriangleShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSub
 	// Can't be inside a triangle
 }
 
-void TriangleShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const
+void TriangleShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, [[maybe_unused]] float inDeltaTime, [[maybe_unused]] Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const
 {
-	Vec3 v1 = inCenterOfMassTransform * (inScale * mV1);
-	Vec3 v2 = inCenterOfMassTransform * (inScale * mV2);
-	Vec3 v3 = inCenterOfMassTransform * (inScale * mV3);
-
-	if (ScaleHelpers::IsInsideOut(inScale))
-		swap(v1, v2);
-
-	Vec3 triangle_normal = (v2 - v1).Cross(v3 - v1).NormalizedOr(Vec3::sAxisY());
+	CollideSoftBodyVerticesVsTriangles collider(inCenterOfMassTransform, inScale);
 
 	for (SoftBodyVertex *v = ioVertices, *sbv_end = ioVertices + inNumVertices; v < sbv_end; ++v)
 		if (v->mInvMass > 0.0f)
 		{
-			// Get the closest point from the vertex to the triangle
-			uint32 set;
-			Vec3 v1_minus_position = v1 - v->mPosition;
-			Vec3 closest_point = ClosestPoint::GetClosestPointOnTriangle(v1_minus_position, v2 - v->mPosition, v3 - v->mPosition, set);
-
-			if (set == 0b111)
-			{
-				// Closest is interior to the triangle, use plane as collision plane but don't allow more than 10cm 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(v1_minus_position), 0.1f);
-				if (penetration > v->mLargestPenetration)
-				{
-					v->mLargestPenetration = penetration;
-
-					// Store collision
-					v->mCollisionPlane = Plane::sFromPointAndNormal(v1, triangle_normal);
-					v->mCollidingShapeIndex = inCollidingShapeIndex;
-				}
-			}
-			else if (closest_point.Dot(triangle_normal) < 0.0f) // Ignore back facing edges
-			{
-				// Closest point is on an edge or vertex, use closest point as collision plane
-				float closest_point_length = closest_point.Length();
-				float penetration = -closest_point_length;
-				if (penetration > v->mLargestPenetration)
-				{
-					v->mLargestPenetration = penetration;
-
-					// Calculate contact point and normal
-					Vec3 point = v->mPosition + closest_point;
-					Vec3 normal = -closest_point / closest_point_length;
-
-					// Store collision
-					v->mCollisionPlane = Plane::sFromPointAndNormal(point, normal);
-					v->mCollidingShapeIndex = inCollidingShapeIndex;
-				}
-			}
+			collider.StartVertex(*v);
+			collider.ProcessTriangle(mV1, mV2, mV3);
+			collider.FinishVertex(*v, inCollidingShapeIndex);
 		}
 }
 

Jolt/Physics/SoftBody/SoftBodyMotionProperties.cpp

@@ -150,6 +150,7 @@ void SoftBodyMotionProperties::DetermineCollidingShapes(const SoftBodyUpdateCont
 	AABox bounds = mLocalBounds;
 	bounds.Encapsulate(mLocalPredictedBounds);
 	bounds = bounds.Transformed(inContext.mCenterOfMassTransform);
+	bounds.ExpandBy(Vec3::sReplicate(mSettings->mVertexRadius));
 	ObjectLayer layer = inContext.mBody->GetObjectLayer();
 	DefaultBroadPhaseLayerFilter broadphase_layer_filter = inSystem.GetDefaultBroadPhaseLayerFilter(layer);
 	DefaultObjectLayerFilter object_layer_filter = inSystem.GetDefaultLayerFilter(layer);
@@ -308,6 +309,7 @@ void SoftBodyMotionProperties::ApplyCollisionConstraintsAndUpdateVelocities(cons
 
 	float dt = inContext.mSubStepDeltaTime;
 	float restitution_treshold = -2.0f * inContext.mGravity.Length() * dt;
+	float vertex_radius = mSettings->mVertexRadius;
 	for (Vertex &v : mVertices)
 		if (v.mInvMass > 0.0f)
 		{
@@ -321,7 +323,7 @@ void SoftBodyMotionProperties::ApplyCollisionConstraintsAndUpdateVelocities(cons
 			if (v.mCollidingShapeIndex >= 0)
 			{
 				// Check if there is a collision
-				float projected_distance = -v.mCollisionPlane.SignedDistance(v.mPosition);
+				float projected_distance = -v.mCollisionPlane.SignedDistance(v.mPosition) + vertex_radius;
 				if (projected_distance > 0.0f)
 				{
 					// Note that we already calculated the velocity, so this does not affect the velocity (next iteration starts by setting previous position to current position)

Jolt/Physics/SoftBody/SoftBodySharedSettings.cpp

@@ -48,6 +48,7 @@ JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings)
 	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mEdgeGroupEndIndices)
 	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mVolumeConstraints)
 	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mMaterials)
+	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mVertexRadius)
 }
 
 void SoftBodySharedSettings::CalculateEdgeLengths()
@@ -142,6 +143,7 @@ void SoftBodySharedSettings::SaveBinaryState(StreamOut &inStream) const
 	inStream.Write(mEdgeConstraints);
 	inStream.Write(mEdgeGroupEndIndices);
 	inStream.Write(mVolumeConstraints);
+	inStream.Write(mVertexRadius);
 }
 
 void SoftBodySharedSettings::RestoreBinaryState(StreamIn &inStream)
@@ -151,6 +153,7 @@ void SoftBodySharedSettings::RestoreBinaryState(StreamIn &inStream)
 	inStream.Read(mEdgeConstraints);
 	inStream.Read(mEdgeGroupEndIndices);
 	inStream.Read(mVolumeConstraints);
+	inStream.Read(mVertexRadius);
 }
 
 void SoftBodySharedSettings::SaveWithMaterials(StreamOut &inStream, SharedSettingsToIDMap &ioSettingsMap, MaterialToIDMap &ioMaterialMap) const

Jolt/Physics/SoftBody/SoftBodySharedSettings.h

@@ -125,6 +125,7 @@ public:
 	Array<uint>			mEdgeGroupEndIndices;						///< The start index of each group of edges that can be solved in parallel
 	Array<Volume>		mVolumeConstraints;							///< The list of volume constraints of the body that keep the volume of tetrahedra in the soft body constant
 	PhysicsMaterialList mMaterials { PhysicsMaterial::sDefault };	///< The materials of the faces of the body, referenced by Face::mMaterialIndex
+	float				mVertexRadius = 0.0f;						///< How big the particles are, can be used to push the vertices a little bit away from the surface of other bodies to prevent z-fighting
 };
 
 JPH_NAMESPACE_END

Samples/Samples.cmake

@@ -181,6 +181,8 @@ set(SAMPLES_SRC_FILES
 	${SAMPLES_ROOT}/Tests/SoftBody/SoftBodyStressTest.h
 	${SAMPLES_ROOT}/Tests/SoftBody/SoftBodyUpdatePositionTest.cpp
 	${SAMPLES_ROOT}/Tests/SoftBody/SoftBodyUpdatePositionTest.h
+	${SAMPLES_ROOT}/Tests/SoftBody/SoftBodyVertexRadiusTest.cpp
+	${SAMPLES_ROOT}/Tests/SoftBody/SoftBodyVertexRadiusTest.h
 	${SAMPLES_ROOT}/Tests/SoftBody/SoftBodyVsFastMovingTest.cpp
 	${SAMPLES_ROOT}/Tests/SoftBody/SoftBodyVsFastMovingTest.h
 	${SAMPLES_ROOT}/Tests/Test.cpp

Samples/SamplesApp.cpp

@@ -317,6 +317,7 @@ JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, SoftBodyKinematicTest)
 JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, SoftBodyUpdatePositionTest)
 JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, SoftBodyStressTest)
 JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, SoftBodyVsFastMovingTest)
+JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, SoftBodyVertexRadiusTest)
 
 static TestNameAndRTTI sSoftBodyTests[] =
 {
@@ -329,6 +330,7 @@ static TestNameAndRTTI sSoftBodyTests[] =
 	{ "Soft Body Kinematic",			JPH_RTTI(SoftBodyKinematicTest) },
 	{ "Soft Body Update Position",		JPH_RTTI(SoftBodyUpdatePositionTest) },
 	{ "Soft Body Stress Test",			JPH_RTTI(SoftBodyStressTest) },
+	{ "Soft Body Vertex Radius Test",	JPH_RTTI(SoftBodyVertexRadiusTest) },
 };
 
 JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, BroadPhaseCastRayTest)
@@ -1479,6 +1481,8 @@ bool SamplesApp::CastProbe(float inProbeLength, float &outFraction, RVec3 &outPo
 
 				if (abs(closest_point_penetration - vertex.mLargestPenetration) > 0.001f)
 					mDebugRenderer->DrawText3D(plane_point, StringFormat("Pen %f (exp %f)", (double)vertex.mLargestPenetration, (double)closest_point_penetration));
+				else
+					mDebugRenderer->DrawText3D(plane_point, StringFormat("Pen %f", (double)vertex.mLargestPenetration));
 			}
 		}
 		break;

Samples/Tests/SoftBody/SoftBodyVertexRadiusTest.cpp

@@ -0,0 +1,38 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2024 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/SoftBody/SoftBodyVertexRadiusTest.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Jolt/Physics/Collision/Shape/SphereShape.h>
+#include <Jolt/Physics/SoftBody/SoftBodyCreationSettings.h>
+#include <Utils/SoftBodyCreator.h>
+#include <Layers.h>
+#include <Application/DebugUI.h>
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(SoftBodyVertexRadiusTest)
+{
+	JPH_ADD_BASE_CLASS(SoftBodyVertexRadiusTest, Test)
+}
+
+void SoftBodyVertexRadiusTest::Initialize()
+{
+	// Floor
+	CreateFloor();
+
+	// Create sphere
+	mBodyInterface->CreateAndAddBody(BodyCreationSettings(new SphereShape(2.0f), RVec3(0, 0, 0), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING), EActivation::DontActivate);
+
+	// Create cloth with specified vertex radius
+	mSharedSettings = SoftBodyCreator::CreateCloth(30, 0.5f, false);
+	mSharedSettings->mVertexRadius = sVertexRadius;
+	SoftBodyCreationSettings cloth(mSharedSettings, RVec3(0, 5, 0), Quat::sRotation(Vec3::sAxisY(), 0.25f * JPH_PI), Layers::MOVING);
+	mBodyInterface->CreateAndAddSoftBody(cloth, EActivation::Activate);
+}
+
+void SoftBodyVertexRadiusTest::CreateSettingsMenu(DebugUI *inUI, UIElement *inSubMenu)
+{
+	inUI->CreateSlider(inSubMenu, "Vertex Radius", sVertexRadius, 0.0f, 0.5f, 0.01f, [this](float inValue) { sVertexRadius = inValue; mSharedSettings->mVertexRadius = inValue; });
+}

Samples/Tests/SoftBody/SoftBodyVertexRadiusTest.h

@@ -0,0 +1,27 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2024 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/Test.h>
+#include <Jolt/Physics/SoftBody/SoftBodySharedSettings.h>
+
+// This test shows how you can use the vertex radius of a soft body to prevent z-fighting while rendering it
+class SoftBodyVertexRadiusTest : public Test
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, SoftBodyVertexRadiusTest)
+
+	// See: Test
+	virtual void			Initialize() override;
+
+	// Optional settings menu
+	virtual bool			HasSettingsMenu() const override							{ return true; }
+	virtual void			CreateSettingsMenu(DebugUI *inUI, UIElement *inSubMenu) override;
+
+private:
+	Ref<SoftBodySharedSettings>	mSharedSettings;
+
+	static inline float		sVertexRadius = 0.01f;
+};

Samples/Utils/SoftBodyCreator.cpp

@@ -236,6 +236,9 @@ Ref<SoftBodySharedSettings> CreateSphere(float inRadius, uint inNumTheta, uint i
 	SoftBodySharedSettings *settings = new SoftBodySharedSettings;
 
 	// Create vertices
+	// NOTE: This is not how you should create a soft body sphere, we explicitly use polar coordinates to make the vertices unevenly distributed.
+	// Doing it this way tests the pressure algorithm as it receives non-uniform triangles. Better is to use uniform triangles,
+	// see the use of DebugRenderer::Create8thSphere for an example.
 	SoftBodySharedSettings::Vertex v;
 	(inRadius * Vec3::sUnitSpherical(0, 0)).StoreFloat3(&v.mPosition);
 	settings->mVertices.push_back(v);