SoftBodyManifold now returns sensor contacts separately (#1276)

Before this change, there was a limit of a single colliding body per soft body vertex. If the closest body happened to be a sensor this effectively disabled the collision with the world and caused artifacts. We can now also detect multiple sensor contacts per soft body and they are returned through a new interface SoftBodyManifold::GetSensorContactBodyID.

* Shape::CollideSoftBodyVertices is no longer using SoftBodyVertex but CollideSoftBodyVertexIterator which can write to other things than a SoftBodyVertex
* Removed delta time and displacement due to gravity from Shape::CollideSoftBodyVertices
* Added soft body vs sensor test that doesn't have a limit of 1 sensor per soft body

Fixes #1016

Jolt/Core/StridedPtr.h

@@ -0,0 +1,63 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2024 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+/// A strided pointer behaves exactly like a normal pointer except that the
+/// elements that the pointer points to can be part of a larger structure.
+/// The stride gives the number of bytes from one element to the next.
+template <class T>
+class JPH_EXPORT StridedPtr
+{
+public:
+	using value_type = T;
+
+	/// Constructors
+							StridedPtr() = default;
+							StridedPtr(const StridedPtr &inRHS) = default;
+							StridedPtr(T *inPtr, int inStride = sizeof(T))			: mPtr(const_cast<uint8 *>(reinterpret_cast<const uint8 *>(inPtr))), mStride(inStride) { }
+
+	/// Assignment
+	inline StridedPtr &		operator = (const StridedPtr &inRHS) = default;
+
+	/// Incrementing / decrementing
+	inline StridedPtr &		operator ++ ()											{ mPtr += mStride; return *this; }
+	inline StridedPtr &		operator -- ()											{ mPtr -= mStride; return *this; }
+	inline StridedPtr		operator ++ (int)										{ StridedPtr old_ptr(*this); mPtr += mStride; return old_ptr; }
+	inline StridedPtr		operator -- (int)										{ StridedPtr old_ptr(*this); mPtr -= mStride; return old_ptr; }
+	inline StridedPtr		operator + (int inOffset) const							{ StridedPtr new_ptr(*this); new_ptr.mPtr += inOffset * mStride; return new_ptr; }
+	inline StridedPtr		operator - (int inOffset) const							{ StridedPtr new_ptr(*this); new_ptr.mPtr -= inOffset * mStride; return new_ptr; }
+	inline void				operator += (int inOffset)								{ mPtr += inOffset * mStride; }
+	inline void				operator -= (int inOffset)								{ mPtr -= inOffset * mStride; }
+
+	/// Distance between two pointers in elements
+	inline int				operator - (const StridedPtr &inRHS) const				{ JPH_ASSERT(inRHS.mStride == mStride); return (mPtr - inRHS.mPtr) / mStride; }
+
+	/// Comparison operators
+	inline bool				operator == (const StridedPtr &inRHS) const				{ return mPtr == inRHS.mPtr; }
+	inline bool				operator != (const StridedPtr &inRHS) const				{ return mPtr != inRHS.mPtr; }
+	inline bool				operator <= (const StridedPtr &inRHS) const				{ return mPtr <= inRHS.mPtr; }
+	inline bool				operator >= (const StridedPtr &inRHS) const				{ return mPtr >= inRHS.mPtr; }
+	inline bool				operator <  (const StridedPtr &inRHS) const				{ return mPtr <  inRHS.mPtr; }
+	inline bool				operator >  (const StridedPtr &inRHS) const				{ return mPtr >  inRHS.mPtr; }
+
+	/// Access value
+	inline T &				operator * () const										{ return *reinterpret_cast<T *>(mPtr); }
+	inline T *				operator -> () const									{ return reinterpret_cast<T *>(mPtr); }
+	inline T &				operator [] (int inOffset) const						{ uint8 *ptr = mPtr + inOffset * mStride; return *reinterpret_cast<T *>(ptr); }
+
+	/// Explicit conversion
+	inline T *				GetPtr() const											{ return reinterpret_cast<T *>(mPtr); }
+
+	/// Get stride in bytes
+	inline int				GetStride() const										{ return mStride; }
+
+private:
+	uint8 *					mPtr = nullptr;											/// Pointer to element
+	int						mStride = 0;											/// Stride (number of bytes) between elements
+};
+
+JPH_NAMESPACE_END

Jolt/Jolt.cmake

@@ -68,6 +68,7 @@ set(JOLT_PHYSICS_SRC_FILES
 	${JOLT_PHYSICS_ROOT}/Core/StreamOut.h
 	${JOLT_PHYSICS_ROOT}/Core/StreamUtils.h
 	${JOLT_PHYSICS_ROOT}/Core/StreamWrapper.h
+	${JOLT_PHYSICS_ROOT}/Core/StridedPtr.h
 	${JOLT_PHYSICS_ROOT}/Core/StringTools.cpp
 	${JOLT_PHYSICS_ROOT}/Core/StringTools.h
 	${JOLT_PHYSICS_ROOT}/Core/TempAllocator.h
@@ -197,6 +198,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/CollideSoftBodyVertexIterator.h
 	${JOLT_PHYSICS_ROOT}/Physics/Collision/CollideSoftBodyVerticesVsTriangles.h
 	${JOLT_PHYSICS_ROOT}/Physics/Collision/CollideSphereVsTriangles.cpp
 	${JOLT_PHYSICS_ROOT}/Physics/Collision/CollideSphereVsTriangles.h

Jolt/Jolt.natvis

@@ -94,4 +94,7 @@
 			</ArrayItems>
 		</Expand>
 	</Type>
+	<Type Name="JPH::StridedPtr&lt;*&gt;">
+		<DisplayString>{(value_type *)mPtr}, stride={mStride}</DisplayString>
+	</Type>
 </AutoVisualizer>

Jolt/Physics/Collision/CollideSoftBodyVertexIterator.h

@@ -0,0 +1,110 @@
+// 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/Core/StridedPtr.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Class that allows iterating over the vertices of a soft body.
+/// It tracks the largest penetration and allows storing the resulting collision in a different structure than the soft body vertex itself.
+class CollideSoftBodyVertexIterator
+{
+public:
+	/// Default constructor
+									CollideSoftBodyVertexIterator() = default;
+									CollideSoftBodyVertexIterator(const CollideSoftBodyVertexIterator &) = default;
+
+	/// Construct using (strided) pointers
+									CollideSoftBodyVertexIterator(const StridedPtr<const Vec3> &inPosition, const StridedPtr<const float> &inInvMass, const StridedPtr<Plane> &inCollisionPlane, const StridedPtr<float> &inLargestPenetration, const StridedPtr<int> &inCollidingShapeIndex) :
+		mPosition(inPosition),
+		mInvMass(inInvMass),
+		mCollisionPlane(inCollisionPlane),
+		mLargestPenetration(inLargestPenetration),
+		mCollidingShapeIndex(inCollidingShapeIndex)
+	{
+	}
+
+	/// Construct using a soft body vertex
+	explicit						CollideSoftBodyVertexIterator(SoftBodyVertex *inVertices) :
+		mPosition(&inVertices->mPosition, sizeof(SoftBodyVertex)),
+		mInvMass(&inVertices->mInvMass, sizeof(SoftBodyVertex)),
+		mCollisionPlane(&inVertices->mCollisionPlane, sizeof(SoftBodyVertex)),
+		mLargestPenetration(&inVertices->mLargestPenetration, sizeof(SoftBodyVertex)),
+		mCollidingShapeIndex(&inVertices->mCollidingShapeIndex, sizeof(SoftBodyVertex))
+	{
+	}
+
+	/// Default assignment
+	CollideSoftBodyVertexIterator &	operator = (const CollideSoftBodyVertexIterator &) = default;
+
+	/// Equality operator.
+	/// Note: Only used to determine end iterator, so we only compare position.
+	bool							operator != (const CollideSoftBodyVertexIterator &inRHS) const
+	{
+		return mPosition != inRHS.mPosition;
+	}
+
+	/// Next vertex
+	CollideSoftBodyVertexIterator &	operator ++ ()
+	{
+		++mPosition;
+		++mInvMass;
+		++mCollisionPlane;
+		++mLargestPenetration;
+		++mCollidingShapeIndex;
+		return *this;
+	}
+
+	/// Add an offset
+	/// Note: Only used to determine end iterator, so we only set position.
+	CollideSoftBodyVertexIterator	operator + (int inOffset) const
+	{
+		return CollideSoftBodyVertexIterator(mPosition + inOffset, StridedPtr<const float>(), StridedPtr<Plane>(), StridedPtr<float>(), StridedPtr<int>());
+	}
+
+	/// Get the position of the current vertex
+	Vec3							GetPosition() const
+	{
+		return *mPosition;
+	}
+
+	/// Get the inverse mass of the current vertex
+	float							GetInvMass() const
+	{
+		return *mInvMass;
+	}
+
+	/// Update penetration of the current vertex
+	/// @return Returns true if the vertex has the largest penetration so far, this means you need to follow up by calling SetCollision
+	bool							UpdatePenetration(float inLargestPenetration) const
+	{
+		float &penetration = *mLargestPenetration;
+		if (penetration >= inLargestPenetration)
+			return false;
+		penetration = inLargestPenetration;
+		return true;
+	}
+
+	/// Update the collision of the current vertex
+	void							SetCollision(const Plane &inCollisionPlane, int inCollidingShapeIndex) const
+	{
+		*mCollisionPlane = inCollisionPlane;
+		*mCollidingShapeIndex = inCollidingShapeIndex;
+	}
+
+private:
+	/// Input data
+	StridedPtr<const Vec3>			mPosition;
+	StridedPtr<const float>			mInvMass;
+
+	/// Output data
+	StridedPtr<Plane>				mCollisionPlane;
+	StridedPtr<float>				mLargestPenetration;
+	StridedPtr<int>					mCollidingShapeIndex;
+};
+
+JPH_NAMESPACE_END

Jolt/Physics/Collision/CollideSoftBodyVerticesVsTriangles.h

@@ -4,7 +4,7 @@
 
 #pragma once
 
-#include <Jolt/Physics/SoftBody/SoftBodyVertex.h>
+#include <Jolt/Physics/Collision/CollideSoftBodyVertexIterator.h>
 #include <Jolt/Geometry/ClosestPoint.h>
 
 JPH_NAMESPACE_BEGIN
@@ -20,9 +20,9 @@ public:
 	{
 	}
 
-	JPH_INLINE void		StartVertex(const SoftBodyVertex &inVertex)
+	JPH_INLINE void		StartVertex(const CollideSoftBodyVertexIterator &inVertex)
 	{
-		mLocalPosition = mInvTransform * inVertex.mPosition;
+		mLocalPosition = mInvTransform * inVertex.GetPosition();
 		mClosestDistanceSq = FLT_MAX;
 	}
 
@@ -43,7 +43,7 @@ public:
 		}
 	}
 
-	JPH_INLINE void		FinishVertex(SoftBodyVertex &ioVertex, int inCollidingShapeIndex) const
+	JPH_INLINE void		FinishVertex(const CollideSoftBodyVertexIterator &ioVertex, int inCollidingShapeIndex) const
 	{
 		if (mClosestDistanceSq < FLT_MAX)
 		{
@@ -57,29 +57,21 @@ public:
 			{
 				// 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;
-				}
+				float penetration = min(triangle_normal.Dot(v0 - ioVertex.GetPosition()), 0.1f);
+				if (ioVertex.UpdatePenetration(penetration))
+					ioVertex.SetCollision(Plane::sFromPointAndNormal(v0, triangle_normal), 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;
+				Vec3 normal = ioVertex.GetPosition() - 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;
-					}
+					if (ioVertex.UpdatePenetration(penetration))
+						ioVertex.SetCollision(Plane::sFromPointAndNormal(closest_point, normal_length > 0.0f? normal / normal_length : triangle_normal), inCollidingShapeIndex);
 				}
 			}
 		}

Jolt/Physics/Collision/Shape/BoxShape.cpp

@@ -11,7 +11,7 @@
 #include <Jolt/Physics/Collision/CastResult.h>
 #include <Jolt/Physics/Collision/CollidePointResult.h>
 #include <Jolt/Physics/Collision/TransformedShape.h>
-#include <Jolt/Physics/SoftBody/SoftBodyVertex.h>
+#include <Jolt/Physics/Collision/CollideSoftBodyVertexIterator.h>
 #include <Jolt/Geometry/RayAABox.h>
 #include <Jolt/ObjectStream/TypeDeclarations.h>
 #include <Jolt/Core/StreamIn.h>
@@ -225,16 +225,16 @@ void BoxShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShape
 		ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator.GetID() });
 }
 
-void BoxShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, [[maybe_unused]] float inDeltaTime, [[maybe_unused]] Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const
+void BoxShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const
 {
 	Mat44 inverse_transform = inCenterOfMassTransform.InversedRotationTranslation();
 	Vec3 half_extent = inScale.Abs() * mHalfExtent;
 
-	for (SoftBodyVertex *v = ioVertices, *sbv_end = ioVertices + inNumVertices; v < sbv_end; ++v)
-		if (v->mInvMass > 0.0f)
+	for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v)
+		if (v.GetInvMass() > 0.0f)
 		{
 			// Convert to local space
-			Vec3 local_pos = inverse_transform * v->mPosition;
+			Vec3 local_pos = inverse_transform * v.GetPosition();
 
 			// Clamp point to inside box
 			Vec3 clamped_point = Vec3::sMax(Vec3::sMin(local_pos, half_extent), -half_extent);
@@ -246,18 +246,15 @@ void BoxShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg
 				Vec3 delta = half_extent - local_pos.Abs();
 				int index = delta.GetLowestComponentIndex();
 				float penetration = delta[index];
-				if (penetration > v->mLargestPenetration)
+				if (v.UpdatePenetration(penetration))
 				{
-					v->mLargestPenetration = penetration;
-
 					// Calculate contact point and normal
 					Vec3 possible_normals[] = { Vec3::sAxisX(), Vec3::sAxisY(), Vec3::sAxisZ() };
 					Vec3 normal = local_pos.GetSign() * possible_normals[index];
 					Vec3 point = normal * half_extent;
 
 					// Store collision
-					v->mCollisionPlane = Plane::sFromPointAndNormal(point, normal).GetTransformed(inCenterOfMassTransform);
-					v->mCollidingShapeIndex = inCollidingShapeIndex;
+					v.SetCollision(Plane::sFromPointAndNormal(point, normal).GetTransformed(inCenterOfMassTransform), inCollidingShapeIndex);
 				}
 			}
 			else
@@ -268,15 +265,12 @@ void BoxShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg
 
 				// Penetration will be negative since we're not penetrating
 				float penetration = -normal_length;
-				if (penetration > v->mLargestPenetration)
+				if (v.UpdatePenetration(penetration))
 				{
 					normal /= normal_length;
 
-					v->mLargestPenetration = penetration;
-
 					// Store collision
-					v->mCollisionPlane = Plane::sFromPointAndNormal(clamped_point, normal).GetTransformed(inCenterOfMassTransform);
-					v->mCollidingShapeIndex = inCollidingShapeIndex;
+					v.SetCollision(Plane::sFromPointAndNormal(clamped_point, normal).GetTransformed(inCenterOfMassTransform), inCollidingShapeIndex);
 				}
 			}
 		}

Jolt/Physics/Collision/Shape/BoxShape.h

@@ -77,7 +77,7 @@ public:
 	virtual void			CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
 
 	// See: Shape::CollideSoftBodyVertices
-	virtual void			CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override;
+	virtual void			CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override;
 
 	// See Shape::GetTrianglesStart
 	virtual void			GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override;

Jolt/Physics/Collision/Shape/CapsuleShape.cpp

@@ -12,7 +12,7 @@
 #include <Jolt/Physics/Collision/CastResult.h>
 #include <Jolt/Physics/Collision/CollidePointResult.h>
 #include <Jolt/Physics/Collision/TransformedShape.h>
-#include <Jolt/Physics/SoftBody/SoftBodyVertex.h>
+#include <Jolt/Physics/Collision/CollideSoftBodyVertexIterator.h>
 #include <Jolt/Geometry/RayCapsule.h>
 #include <Jolt/ObjectStream/TypeDeclarations.h>
 #include <Jolt/Core/StreamIn.h>
@@ -321,7 +321,7 @@ void CapsuleShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubS
 		ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator.GetID() });
 }
 
-void CapsuleShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, [[maybe_unused]] float inDeltaTime, [[maybe_unused]] Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const
+void CapsuleShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const
 {
 	JPH_ASSERT(IsValidScale(inScale));
 
@@ -332,11 +332,11 @@ void CapsuleShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec
 	float half_height_of_cylinder = scale * mHalfHeightOfCylinder;
 	float radius = scale * mRadius;
 
-	for (SoftBodyVertex *v = ioVertices, *sbv_end = ioVertices + inNumVertices; v < sbv_end; ++v)
-		if (v->mInvMass > 0.0f)
+	for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v)
+		if (v.GetInvMass() > 0.0f)
 		{
 			// Calculate penetration
-			Vec3 local_pos = inverse_transform * v->mPosition;
+			Vec3 local_pos = inverse_transform * v.GetPosition();
 			if (abs(local_pos.GetY()) <= half_height_of_cylinder)
 			{
 				// Near cylinder
@@ -344,17 +344,14 @@ void CapsuleShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec
 				normal.SetY(0.0f);
 				float normal_length = normal.Length();
 				float penetration = radius - normal_length;
-				if (penetration > v->mLargestPenetration)
+				if (v.UpdatePenetration(penetration))
 				{
-					v->mLargestPenetration = penetration;
-
 					// Calculate contact point and normal
 					normal = normal_length > 0.0f? normal / normal_length : Vec3::sAxisX();
 					Vec3 point = radius * normal;
 
 					// Store collision
-					v->mCollisionPlane = Plane::sFromPointAndNormal(point, normal).GetTransformed(inCenterOfMassTransform);
-					v->mCollidingShapeIndex = inCollidingShapeIndex;
+					v.SetCollision(Plane::sFromPointAndNormal(point, normal).GetTransformed(inCenterOfMassTransform), inCollidingShapeIndex);
 				}
 			}
 			else
@@ -364,17 +361,14 @@ void CapsuleShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec
 				Vec3 delta = local_pos - center;
 				float distance = delta.Length();
 				float penetration = radius - distance;
-				if (penetration > v->mLargestPenetration)
+				if (v.UpdatePenetration(penetration))
 				{
-					v->mLargestPenetration = penetration;
-
 					// Calculate contact point and normal
 					Vec3 normal = delta / distance;
 					Vec3 point = center + radius * normal;
 
 					// Store collision
-					v->mCollisionPlane = Plane::sFromPointAndNormal(point, normal).GetTransformed(inCenterOfMassTransform);
-					v->mCollidingShapeIndex = inCollidingShapeIndex;
+					v.SetCollision(Plane::sFromPointAndNormal(point, normal).GetTransformed(inCenterOfMassTransform), inCollidingShapeIndex);
 				}
 			}
 		}

Jolt/Physics/Collision/Shape/CapsuleShape.h

@@ -86,7 +86,7 @@ public:
 	virtual void			CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
 
 	// See: Shape::CollideSoftBodyVertices
-	virtual void			CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override;
+	virtual void			CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override;
 
 	// See Shape::GetTrianglesStart
 	virtual void			GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override;

Jolt/Physics/Collision/Shape/CompoundShape.cpp

@@ -262,12 +262,12 @@ void CompoundShape::DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg i
 }
 #endif // JPH_DEBUG_RENDERER
 
-void CompoundShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const
+void CompoundShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const
 {
 	for (const SubShape &shape : mSubShapes)
 	{
 		Mat44 transform = shape.GetLocalTransformNoScale(inScale);
-		shape.mShape->CollideSoftBodyVertices(inCenterOfMassTransform * transform, shape.TransformScale(inScale), ioVertices, inNumVertices, inDeltaTime, inDisplacementDueToGravity, inCollidingShapeIndex);
+		shape.mShape->CollideSoftBodyVertices(inCenterOfMassTransform * transform, shape.TransformScale(inScale), inVertices, inNumVertices, inCollidingShapeIndex);
 	}
 }
 

Jolt/Physics/Collision/Shape/CompoundShape.h

@@ -109,7 +109,7 @@ public:
 #endif // JPH_DEBUG_RENDERER
 
 	// See: Shape::CollideSoftBodyVertices
-	virtual void					CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override;
+	virtual void					CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override;
 
 	// See Shape::TransformShape
 	virtual void					TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const override;

Jolt/Physics/Collision/Shape/ConvexHullShape.cpp

@@ -11,7 +11,7 @@
 #include <Jolt/Physics/Collision/CastResult.h>
 #include <Jolt/Physics/Collision/CollidePointResult.h>
 #include <Jolt/Physics/Collision/TransformedShape.h>
-#include <Jolt/Physics/SoftBody/SoftBodyVertex.h>
+#include <Jolt/Physics/Collision/CollideSoftBodyVertexIterator.h>
 #include <Jolt/Geometry/ConvexHullBuilder.h>
 #include <Jolt/Geometry/ClosestPoint.h>
 #include <Jolt/ObjectStream/TypeDeclarations.h>
@@ -1057,7 +1057,7 @@ void ConvexHullShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inS
 	ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator.GetID() });
 }
 
-void ConvexHullShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, [[maybe_unused]] float inDeltaTime, [[maybe_unused]] Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const
+void ConvexHullShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const
 {
 	Mat44 inverse_transform = inCenterOfMassTransform.InversedRotationTranslation();
 
@@ -1065,10 +1065,10 @@ void ConvexHullShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform,
 	bool is_not_scaled = ScaleHelpers::IsNotScaled(inScale);
 	float scale_flip = ScaleHelpers::IsInsideOut(inScale)? -1.0f : 1.0f;
 
-	for (SoftBodyVertex *v = ioVertices, *sbv_end = ioVertices + inNumVertices; v < sbv_end; ++v)
-		if (v->mInvMass > 0.0f)
+	for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v)
+		if (v.GetInvMass() > 0.0f)
 		{
-			Vec3 local_pos = inverse_transform * v->mPosition;
+			Vec3 local_pos = inverse_transform * v.GetPosition();
 
 			// Find most facing plane
 			float max_distance = -FLT_MAX;
@@ -1150,17 +1150,14 @@ void ConvexHullShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform,
 				penetration = -penetration;
 			else
 				normal = -normal;
-			if (penetration > v->mLargestPenetration)
+			if (v.UpdatePenetration(penetration))
 			{
-				v->mLargestPenetration = penetration;
-
 				// Calculate contact plane
 				normal = normal_length > 0.0f? normal / normal_length : max_plane_normal;
 				Plane plane = Plane::sFromPointAndNormal(closest_point, normal);
 
 				// Store collision
-				v->mCollisionPlane = plane.GetTransformed(inCenterOfMassTransform);
-				v->mCollidingShapeIndex = inCollidingShapeIndex;
+				v.SetCollision(plane.GetTransformed(inCenterOfMassTransform), inCollidingShapeIndex);
 			}
 		}
 }

Jolt/Physics/Collision/Shape/ConvexHullShape.h

@@ -90,7 +90,7 @@ public:
 	virtual void			CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
 
 	// See: Shape::CollideSoftBodyVertices
-	virtual void			CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override;
+	virtual void			CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override;
 
 	// See Shape::GetTrianglesStart
 	virtual void			GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override;

Jolt/Physics/Collision/Shape/CylinderShape.cpp

@@ -11,7 +11,7 @@
 #include <Jolt/Physics/Collision/CastResult.h>
 #include <Jolt/Physics/Collision/CollidePointResult.h>
 #include <Jolt/Physics/Collision/TransformedShape.h>
-#include <Jolt/Physics/SoftBody/SoftBodyVertex.h>
+#include <Jolt/Physics/Collision/CollideSoftBodyVertexIterator.h>
 #include <Jolt/Geometry/RayCylinder.h>
 #include <Jolt/ObjectStream/TypeDeclarations.h>
 #include <Jolt/Core/StreamIn.h>
@@ -300,7 +300,7 @@ void CylinderShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSub
 		ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator.GetID() });
 }
 
-void CylinderShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, [[maybe_unused]] float inDeltaTime, [[maybe_unused]] Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const
+void CylinderShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const
 {
 	JPH_ASSERT(IsValidScale(inScale));
 
@@ -311,10 +311,10 @@ void CylinderShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Ve
 	float half_height = abs_scale.GetY() * mHalfHeight;
 	float radius = abs_scale.GetX() * mRadius;
 
-	for (SoftBodyVertex *v = ioVertices, *sbv_end = ioVertices + inNumVertices; v < sbv_end; ++v)
-		if (v->mInvMass > 0.0f)
+	for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v)
+		if (v.GetInvMass() > 0.0f)
 		{
-			Vec3 local_pos = inverse_transform * v->mPosition;
+			Vec3 local_pos = inverse_transform * v.GetPosition();
 
 			// Calculate penetration into side surface
 			Vec3 side_normal = local_pos;
@@ -348,14 +348,8 @@ void CylinderShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Ve
 			// Calculate penetration
 			Plane plane = Plane::sFromPointAndNormal(point, normal);
 			float penetration = -plane.SignedDistance(local_pos);
-			if (penetration > v->mLargestPenetration)
-			{
-				v->mLargestPenetration = penetration;
-
-				// Store collision
-				v->mCollisionPlane = plane.GetTransformed(inCenterOfMassTransform);
-				v->mCollidingShapeIndex = inCollidingShapeIndex;
-			}
+			if (v.UpdatePenetration(penetration))
+				v.SetCollision(plane.GetTransformed(inCenterOfMassTransform), inCollidingShapeIndex);
 		}
 }
 

Jolt/Physics/Collision/Shape/CylinderShape.h

@@ -81,7 +81,7 @@ public:
 	virtual void			CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
 
 	// See: Shape::CollideSoftBodyVertices
-	virtual void			CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override;
+	virtual void			CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override;
 
 	// See Shape::GetTrianglesStart
 	virtual void			GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override;

Jolt/Physics/Collision/Shape/EmptyShape.h

@@ -50,7 +50,7 @@ public:
 	virtual bool			CastRay([[maybe_unused]] const RayCast &inRay, [[maybe_unused]] const SubShapeIDCreator &inSubShapeIDCreator, [[maybe_unused]] RayCastResult &ioHit) const override { return false; }
 	virtual void			CastRay([[maybe_unused]] const RayCast &inRay, [[maybe_unused]] const RayCastSettings &inRayCastSettings, [[maybe_unused]] const SubShapeIDCreator &inSubShapeIDCreator, [[maybe_unused]] CastRayCollector &ioCollector, [[maybe_unused]] const ShapeFilter &inShapeFilter = { }) const override { /* Do nothing */ }
 	virtual void			CollidePoint([[maybe_unused]] Vec3Arg inPoint, [[maybe_unused]] const SubShapeIDCreator &inSubShapeIDCreator, [[maybe_unused]] CollidePointCollector &ioCollector, [[maybe_unused]] const ShapeFilter &inShapeFilter = { }) const override { /* Do nothing */ }
-	virtual void			CollideSoftBodyVertices([[maybe_unused]] Mat44Arg inCenterOfMassTransform, [[maybe_unused]] Vec3Arg inScale, [[maybe_unused]] SoftBodyVertex *ioVertices, [[maybe_unused]] uint inNumVertices, [[maybe_unused]] float inDeltaTime, [[maybe_unused]] Vec3Arg inDisplacementDueToGravity, [[maybe_unused]] int inCollidingShapeIndex) const override { /* Do nothing */ }
+	virtual void			CollideSoftBodyVertices([[maybe_unused]] Mat44Arg inCenterOfMassTransform, [[maybe_unused]] Vec3Arg inScale, [[maybe_unused]] const CollideSoftBodyVertexIterator &inVertices, [[maybe_unused]] uint inNumVertices, [[maybe_unused]] int inCollidingShapeIndex) const override { /* Do nothing */ }
 	virtual void			GetTrianglesStart([[maybe_unused]] GetTrianglesContext &ioContext, [[maybe_unused]] const AABox &inBox, [[maybe_unused]] Vec3Arg inPositionCOM, [[maybe_unused]] QuatArg inRotation, [[maybe_unused]] Vec3Arg inScale) const override { /* Do nothing */ }
 	virtual int				GetTrianglesNext([[maybe_unused]] GetTrianglesContext &ioContext, [[maybe_unused]] int inMaxTrianglesRequested, [[maybe_unused]] Float3 *outTriangleVertices, [[maybe_unused]] const PhysicsMaterial **outMaterials = nullptr) const override { return 0; }
 	Stats					GetStats() const override										{ return { sizeof(*this), 0 }; }

Jolt/Physics/Collision/Shape/HeightFieldShape.cpp

@@ -2201,7 +2201,7 @@ 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, [[maybe_unused]] float inDeltaTime, [[maybe_unused]] Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const
+void HeightFieldShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const
 {
 	JPH_PROFILE_FUNCTION();
 
@@ -2241,12 +2241,12 @@ void HeightFieldShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform,
 
 	Visitor visitor(inCenterOfMassTransform, inScale);
 
-	for (SoftBodyVertex *v = ioVertices, *sbv_end = ioVertices + inNumVertices; v < sbv_end; ++v)
-		if (v->mInvMass > 0.0f)
+	for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v)
+		if (v.GetInvMass() > 0.0f)
 		{
-			visitor.StartVertex(*v);
+			visitor.StartVertex(v);
 			WalkHeightField(visitor);
-			visitor.FinishVertex(*v, inCollidingShapeIndex);
+			visitor.FinishVertex(v, inCollidingShapeIndex);
 		}
 }
 

Jolt/Physics/Collision/Shape/HeightFieldShape.h

@@ -174,7 +174,7 @@ public:
 	virtual void					CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
 
 	// See: Shape::CollideSoftBodyVertices
-	virtual void					CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override;
+	virtual void					CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override;
 
 	// See Shape::GetTrianglesStart
 	virtual void					GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override;

Jolt/Physics/Collision/Shape/MeshShape.cpp

@@ -793,7 +793,7 @@ 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, [[maybe_unused]] float inDeltaTime, [[maybe_unused]] Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const
+void MeshShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const
 {
 	JPH_PROFILE_FUNCTION();
 
@@ -830,12 +830,12 @@ void MeshShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Ar
 
 	Visitor visitor(inCenterOfMassTransform, inScale);
 
-	for (SoftBodyVertex *v = ioVertices, *sbv_end = ioVertices + inNumVertices; v < sbv_end; ++v)
-		if (v->mInvMass > 0.0f)
+	for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v)
+		if (v.GetInvMass() > 0.0f)
 		{
-			visitor.StartVertex(*v);
+			visitor.StartVertex(v);
 			WalkTreePerTriangle(SubShapeIDCreator(), visitor);
-			visitor.FinishVertex(*v, inCollidingShapeIndex);
+			visitor.FinishVertex(v, inCollidingShapeIndex);
 		}
 }
 

Jolt/Physics/Collision/Shape/MeshShape.h

@@ -125,7 +125,7 @@ public:
 	virtual void					CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
 
 	// See: Shape::CollideSoftBodyVertices
-	virtual void					CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override;
+	virtual void					CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override;
 
 	// See Shape::GetTrianglesStart
 	virtual void					GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override;

Jolt/Physics/Collision/Shape/OffsetCenterOfMassShape.cpp

@@ -127,9 +127,9 @@ void OffsetCenterOfMassShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCrea
 	mInnerShape->CollidePoint(inPoint + mOffset, inSubShapeIDCreator, ioCollector, inShapeFilter);
 }
 
-void OffsetCenterOfMassShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const
+void OffsetCenterOfMassShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const
 {
-	mInnerShape->CollideSoftBodyVertices(inCenterOfMassTransform.PreTranslated(-inScale * mOffset), inScale, ioVertices, inNumVertices, inDeltaTime, inDisplacementDueToGravity, inCollidingShapeIndex);
+	mInnerShape->CollideSoftBodyVertices(inCenterOfMassTransform.PreTranslated(-inScale * mOffset), inScale, inVertices, inNumVertices, inCollidingShapeIndex);
 }
 
 void OffsetCenterOfMassShape::CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const

Jolt/Physics/Collision/Shape/OffsetCenterOfMassShape.h

@@ -97,7 +97,7 @@ public:
 	virtual void					CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
 
 	// See: Shape::CollideSoftBodyVertices
-	virtual void					CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override;
+	virtual void					CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override;
 
 	// See Shape::CollectTransformedShapes
 	virtual void					CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const override;

Jolt/Physics/Collision/Shape/PlaneShape.cpp

@@ -14,7 +14,7 @@
 #include <Jolt/Physics/Collision/CollisionDispatch.h>
 #include <Jolt/Physics/Collision/TransformedShape.h>
 #include <Jolt/Physics/Collision/CollidePointResult.h>
-#include <Jolt/Physics/SoftBody/SoftBodyVertex.h>
+#include <Jolt/Physics/Collision/CollideSoftBodyVertexIterator.h>
 #include <Jolt/Core/Profiler.h>
 #include <Jolt/Core/StreamIn.h>
 #include <Jolt/Core/StreamOut.h>
@@ -246,24 +246,20 @@ void PlaneShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubSha
 		ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator.GetID() });
 }
 
-void PlaneShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, [[maybe_unused]] float inDeltaTime, [[maybe_unused]] Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const
+void PlaneShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const
 {
 	JPH_PROFILE_FUNCTION();
 
 	// Convert plane to world space
 	Plane plane = mPlane.Scaled(inScale).GetTransformed(inCenterOfMassTransform);
 
-	for (SoftBodyVertex *v = ioVertices, *sbv_end = ioVertices + inNumVertices; v < sbv_end; ++v)
-		if (v->mInvMass > 0.0f)
+	for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v)
+		if (v.GetInvMass() > 0.0f)
 		{
 			// Calculate penetration
-			float penetration = -plane.SignedDistance(v->mPosition);
-			if (penetration > v->mLargestPenetration)
-			{
-				v->mLargestPenetration = penetration;
-				v->mCollisionPlane = plane;
-				v->mCollidingShapeIndex = inCollidingShapeIndex;
-			}
+			float penetration = -plane.SignedDistance(v.GetPosition());
+			if (v.UpdatePenetration(penetration))
+				v.SetCollision(plane, inCollidingShapeIndex);
 		}
 }
 

Jolt/Physics/Collision/Shape/PlaneShape.h

@@ -92,7 +92,7 @@ public:
 	virtual void					CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
 
 	// See: Shape::CollideSoftBodyVertices
-	virtual void					CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override;
+	virtual void					CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override;
 
 	// See Shape::GetTrianglesStart
 	virtual void					GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override;

Jolt/Physics/Collision/Shape/RotatedTranslatedShape.cpp

@@ -161,9 +161,9 @@ void RotatedTranslatedShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreat
 	mInnerShape->CollidePoint(transform * inPoint, inSubShapeIDCreator, ioCollector, inShapeFilter);
 }
 
-void RotatedTranslatedShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const
+void RotatedTranslatedShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const
 {
-	mInnerShape->CollideSoftBodyVertices(inCenterOfMassTransform * Mat44::sRotation(mRotation), inScale, ioVertices, inNumVertices, inDeltaTime, inDisplacementDueToGravity, inCollidingShapeIndex);
+	mInnerShape->CollideSoftBodyVertices(inCenterOfMassTransform * Mat44::sRotation(mRotation), inScale, inVertices, inNumVertices, inCollidingShapeIndex);
 }
 
 void RotatedTranslatedShape::CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const

Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h

@@ -98,7 +98,7 @@ public:
 	virtual void					CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
 
 	// See: Shape::CollideSoftBodyVertices
-	virtual void					CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override;
+	virtual void					CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override;
 
 	// See Shape::CollectTransformedShapes
 	virtual void					CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const override;

Jolt/Physics/Collision/Shape/ScaledShape.cpp

@@ -139,9 +139,9 @@ void ScaledShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubSh
 	mInnerShape->CollidePoint(inv_scale * inPoint, inSubShapeIDCreator, ioCollector, inShapeFilter);
 }
 
-void ScaledShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const
+void ScaledShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const
 {
-	mInnerShape->CollideSoftBodyVertices(inCenterOfMassTransform, inScale * mScale, ioVertices, inNumVertices, inDeltaTime, inDisplacementDueToGravity, inCollidingShapeIndex);
+	mInnerShape->CollideSoftBodyVertices(inCenterOfMassTransform, inScale * mScale, inVertices, inNumVertices, inCollidingShapeIndex);
 }
 
 void ScaledShape::CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const

Jolt/Physics/Collision/Shape/ScaledShape.h

@@ -95,7 +95,7 @@ public:
 	virtual void					CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
 
 	// See: Shape::CollideSoftBodyVertices
-	virtual void					CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override;
+	virtual void					CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override;
 
 	// See Shape::CollectTransformedShapes
 	virtual void					CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const override;

Jolt/Physics/Collision/Shape/Shape.h

@@ -33,7 +33,7 @@ class SubShapeID;
 class PhysicsMaterial;
 class TransformedShape;
 class Plane;
-class SoftBodyVertex;
+class CollideSoftBodyVertexIterator;
 class Shape;
 class StreamOut;
 class StreamIn;
@@ -323,12 +323,10 @@ public:
 	/// Collides all vertices of a soft body with this shape and updates SoftBodyVertex::mCollisionPlane, SoftBodyVertex::mCollidingShapeIndex and SoftBodyVertex::mLargestPenetration if a collision with more penetration was found.
 	/// @param inCenterOfMassTransform Center of mass transform for this shape relative to the vertices.
 	/// @param inScale Scale in local space of the shape (scales relative to its center of mass)
-	/// @param ioVertices The vertices of the soft body
-	/// @param inNumVertices The number of vertices in ioVertices
-	/// @param inDeltaTime Delta time of this time step (can be used to extrapolate the position using the velocity of the particle)
-	/// @param inDisplacementDueToGravity Displacement due to gravity during this time step
-	/// @param inCollidingShapeIndex Value to store in SoftBodyVertex::mCollidingShapeIndex when a collision was found
-	virtual void					CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const = 0;
+	/// @param inVertices The vertices of the soft body
+	/// @param inNumVertices The number of vertices in inVertices
+	/// @param inCollidingShapeIndex Value to store in CollideSoftBodyVertexIterator::mCollidingShapeIndex when a collision was found
+	virtual void					CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const = 0;
 
 	/// Collect the leaf transformed shapes of all leaf shapes of this shape.
 	/// inBox is the world space axis aligned box which leaf shapes should collide with.

Jolt/Physics/Collision/Shape/SphereShape.cpp

@@ -11,7 +11,7 @@
 #include <Jolt/Physics/Collision/CastResult.h>
 #include <Jolt/Physics/Collision/CollidePointResult.h>
 #include <Jolt/Physics/Collision/TransformedShape.h>
-#include <Jolt/Physics/SoftBody/SoftBodyVertex.h>
+#include <Jolt/Physics/Collision/CollideSoftBodyVertexIterator.h>
 #include <Jolt/Geometry/RaySphere.h>
 #include <Jolt/Geometry/Plane.h>
 #include <Jolt/Core/StreamIn.h>
@@ -276,29 +276,26 @@ void SphereShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubSh
 		ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator.GetID() });
 }
 
-void SphereShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, [[maybe_unused]] float inDeltaTime, [[maybe_unused]] Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const
+void SphereShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const
 {
 	Vec3 center = inCenterOfMassTransform.GetTranslation();
 	float radius = GetScaledRadius(inScale);
 
-	for (SoftBodyVertex *v = ioVertices, *sbv_end = ioVertices + inNumVertices; v < sbv_end; ++v)
-		if (v->mInvMass > 0.0f)
+	for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v)
+		if (v.GetInvMass() > 0.0f)
 		{
 			// Calculate penetration
-			Vec3 delta = v->mPosition - center;
+			Vec3 delta = v.GetPosition() - center;
 			float distance = delta.Length();
 			float penetration = radius - distance;
-			if (penetration > v->mLargestPenetration)
+			if (v.UpdatePenetration(penetration))
 			{
-				v->mLargestPenetration = penetration;
-
 				// Calculate contact point and normal
 				Vec3 normal = distance > 0.0f? delta / distance : Vec3::sAxisY();
 				Vec3 point = center + radius * normal;
 
 				// Store collision
-				v->mCollisionPlane = Plane::sFromPointAndNormal(point, normal);
-				v->mCollidingShapeIndex = inCollidingShapeIndex;
+				v.SetCollision(Plane::sFromPointAndNormal(point, normal), inCollidingShapeIndex);
 			}
 		}
 }

Jolt/Physics/Collision/Shape/SphereShape.h

@@ -81,7 +81,7 @@ public:
 	virtual void			CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
 
 	// See: Shape::CollideSoftBodyVertices
-	virtual void			CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override;
+	virtual void			CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override;
 
 	// See Shape::GetTrianglesStart
 	virtual void			GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override;

Jolt/Physics/Collision/Shape/TaperedCapsuleShape.cpp

@@ -9,7 +9,7 @@
 #include <Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h>
 #include <Jolt/Physics/Collision/Shape/ScaleHelpers.h>
 #include <Jolt/Physics/Collision/TransformedShape.h>
-#include <Jolt/Physics/SoftBody/SoftBodyVertex.h>
+#include <Jolt/Physics/Collision/CollideSoftBodyVertexIterator.h>
 #include <Jolt/Geometry/RayCapsule.h>
 #include <Jolt/ObjectStream/TypeDeclarations.h>
 #include <Jolt/Core/StreamIn.h>
@@ -301,7 +301,7 @@ AABox TaperedCapsuleShape::GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform,
 	return AABox(p1, p2);
 }
 
-void TaperedCapsuleShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, [[maybe_unused]] float inDeltaTime, [[maybe_unused]] Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const
+void TaperedCapsuleShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const
 {
 	JPH_ASSERT(IsValidScale(inScale));
 
@@ -317,10 +317,10 @@ void TaperedCapsuleShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransfo
 	float scaled_top_radius = scale_xz * mTopRadius;
 	float scaled_bottom_radius = scale_xz * mBottomRadius;
 
-	for (SoftBodyVertex *v = ioVertices, *sbv_end = ioVertices + inNumVertices; v < sbv_end; ++v)
-		if (v->mInvMass > 0.0f)
+	for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v)
+		if (v.GetInvMass() > 0.0f)
 		{
-			Vec3 local_pos = scale_y_flip * (inverse_transform * v->mPosition);
+			Vec3 local_pos = scale_y_flip * (inverse_transform * v.GetPosition());
 
 			Vec3 position, normal;
 
@@ -359,16 +359,13 @@ void TaperedCapsuleShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransfo
 
 			Plane plane = Plane::sFromPointAndNormal(position, normal);
 			float penetration = -plane.SignedDistance(local_pos);
-			if (penetration > v->mLargestPenetration)
+			if (v.UpdatePenetration(penetration))
 			{
-				v->mLargestPenetration = penetration;
-
 				// Need to flip the normal's y if capsule is flipped (this corresponds to flipping both the point and the normal around y)
 				plane.SetNormal(scale_y_flip * plane.GetNormal());
 
 				// Store collision
-				v->mCollisionPlane = plane.GetTransformed(inCenterOfMassTransform);
-				v->mCollidingShapeIndex = inCollidingShapeIndex;
+				v.SetCollision(plane.GetTransformed(inCenterOfMassTransform), inCollidingShapeIndex);
 			}
 		}
 }

Jolt/Physics/Collision/Shape/TaperedCapsuleShape.h

@@ -81,7 +81,7 @@ public:
 	virtual const Support *	GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const override;
 
 	// See: Shape::CollideSoftBodyVertices
-	virtual void			CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override;
+	virtual void			CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override;
 
 #ifdef JPH_DEBUG_RENDERER
 	// See Shape::Draw

Jolt/Physics/Collision/Shape/TaperedCylinderShape.cpp

@@ -9,7 +9,7 @@
 #include <Jolt/Physics/Collision/Shape/ScaleHelpers.h>
 #include <Jolt/Physics/Collision/CollidePointResult.h>
 #include <Jolt/Physics/Collision/TransformedShape.h>
-#include <Jolt/Physics/SoftBody/SoftBodyVertex.h>
+#include <Jolt/Physics/Collision/CollideSoftBodyVertexIterator.h>
 #include <Jolt/ObjectStream/TypeDeclarations.h>
 #include <Jolt/Core/StreamIn.h>
 #include <Jolt/Core/StreamOut.h>
@@ -383,7 +383,7 @@ void TaperedCylinderShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator
 		ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator.GetID() });
 }
 
-void TaperedCylinderShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, [[maybe_unused]] float inDeltaTime, [[maybe_unused]] Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const
+void TaperedCylinderShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const
 {
 	JPH_ASSERT(IsValidScale(inScale));
 
@@ -395,10 +395,10 @@ void TaperedCylinderShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransf
 	Vec3 top_3d(0, top, 0);
 	Vec3 bottom_3d(0, bottom, 0);
 
-	for (SoftBodyVertex *v = ioVertices, *sbv_end = ioVertices + inNumVertices; v < sbv_end; ++v)
-		if (v->mInvMass > 0.0f)
+	for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v)
+		if (v.GetInvMass() > 0.0f)
 		{
-			Vec3 local_pos = inverse_transform * v->mPosition;
+			Vec3 local_pos = inverse_transform * v.GetPosition();
 
 			// Calculate penetration into side surface
 			Vec3 normal_xz = sCalculateSideNormalXZ(local_pos);
@@ -500,14 +500,8 @@ void TaperedCylinderShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransf
 			// Calculate penetration
 			Plane plane = Plane::sFromPointAndNormal(point, normal);
 			float penetration = -plane.SignedDistance(local_pos);
-			if (penetration > v->mLargestPenetration)
-			{
-				v->mLargestPenetration = penetration;
-
-				// Store collision
-				v->mCollisionPlane = plane.GetTransformed(inCenterOfMassTransform);
-				v->mCollidingShapeIndex = inCollidingShapeIndex;
-			}
+			if (v.UpdatePenetration(penetration))
+				v.SetCollision(plane.GetTransformed(inCenterOfMassTransform), inCollidingShapeIndex);
 		}
 }
 

Jolt/Physics/Collision/Shape/TaperedCylinderShape.h

@@ -76,7 +76,7 @@ public:
 	virtual void			CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
 
 	// See: Shape::CollideSoftBodyVertices
-	virtual void			CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override;
+	virtual void			CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override;
 
 	// See Shape::GetTrianglesStart
 	virtual void			GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override;

Jolt/Physics/Collision/Shape/TriangleShape.cpp

@@ -269,16 +269,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, [[maybe_unused]] float inDeltaTime, [[maybe_unused]] Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const
+void TriangleShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const
 {
 	CollideSoftBodyVerticesVsTriangles collider(inCenterOfMassTransform, inScale);
 
-	for (SoftBodyVertex *v = ioVertices, *sbv_end = ioVertices + inNumVertices; v < sbv_end; ++v)
-		if (v->mInvMass > 0.0f)
+	for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v)
+		if (v.GetInvMass() > 0.0f)
 		{
-			collider.StartVertex(*v);
+			collider.StartVertex(v);
 			collider.ProcessTriangle(mV1, mV2, mV3);
-			collider.FinishVertex(*v, inCollidingShapeIndex);
+			collider.FinishVertex(v, inCollidingShapeIndex);
 		}
 }
 

Jolt/Physics/Collision/Shape/TriangleShape.h

@@ -90,7 +90,7 @@ public:
 	virtual void			CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
 
 	// See: Shape::CollideSoftBodyVertices
-	virtual void			CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override;
+	virtual void			CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override;
 
 	// See Shape::GetTrianglesStart
 	virtual void			GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override;

Jolt/Physics/SoftBody/SoftBodyManifold.h

@@ -39,6 +39,18 @@ public:
 		return inVertex.mHasContact? mCollidingShapes[inVertex.mCollidingShapeIndex].mBodyID : BodyID();
 	}
 
+	/// Get the number of sensors that are in contact with the soft body
+	JPH_INLINE uint					GetNumSensorContacts() const
+	{
+		return (uint)mCollidingSensors.size();
+	}
+
+	/// Get the i-th sensor that is in contact with the soft body
+	JPH_INLINE BodyID				GetSensorContactBodyID(uint inIndex) const
+	{
+		return mCollidingSensors[inIndex].mBodyID;
+	}
+
 private:
 	/// Allow SoftBodyMotionProperties to construct us
 	friend class SoftBodyMotionProperties;
@@ -46,14 +58,17 @@ private:
 	/// Constructor
 	explicit						SoftBodyManifold(const SoftBodyMotionProperties *inMotionProperties) :
 										mVertices(inMotionProperties->mVertices),
-										mCollidingShapes(inMotionProperties->mCollidingShapes)
+										mCollidingShapes(inMotionProperties->mCollidingShapes),
+										mCollidingSensors(inMotionProperties->mCollidingSensors)
 	{
 	}
 
 	using CollidingShape = SoftBodyMotionProperties::CollidingShape;
+	using CollidingSensor = SoftBodyMotionProperties::CollidingSensor;
 
 	const Array<SoftBodyVertex> &	mVertices;
 	const Array<CollidingShape>	&	mCollidingShapes;
+	const Array<CollidingSensor> &	mCollidingSensors;
 };
 
 JPH_NAMESPACE_END

Jolt/Physics/SoftBody/SoftBodyMotionProperties.cpp

@@ -8,6 +8,7 @@
 #include <Jolt/Physics/SoftBody/SoftBodyCreationSettings.h>
 #include <Jolt/Physics/SoftBody/SoftBodyContactListener.h>
 #include <Jolt/Physics/SoftBody/SoftBodyManifold.h>
+#include <Jolt/Physics/Collision/CollideSoftBodyVertexIterator.h>
 #include <Jolt/Physics/PhysicsSystem.h>
 #include <Jolt/Physics/Body/BodyManager.h>
 #include <Jolt/Core/ScopeExit.h>
@@ -71,9 +72,7 @@ void SoftBodyMotionProperties::Initialize(const SoftBodyCreationSettings &inSett
 		Vertex &out_vertex = mVertices[v];
 		out_vertex.mPreviousPosition = out_vertex.mPosition = rotation * Vec3(in_vertex.mPosition);
 		out_vertex.mVelocity = rotation.Multiply3x3(Vec3(in_vertex.mVelocity));
-		out_vertex.mCollidingShapeIndex = -1;
-		out_vertex.mHasContact = false;
-		out_vertex.mLargestPenetration = -FLT_MAX;
+		out_vertex.ResetCollision();
 		out_vertex.mInvMass = in_vertex.mInvMass;
 		mLocalBounds.Encapsulate(out_vertex.mPosition);
 	}
@@ -105,15 +104,19 @@ void SoftBodyMotionProperties::DetermineCollidingShapes(const SoftBodyUpdateCont
 {
 	JPH_PROFILE_FUNCTION();
 
+	// Reset flag prior to collision detection
+	mNeedContactCallback = false;
+
 	struct Collector : public CollideShapeBodyCollector
 	{
-									Collector(const SoftBodyUpdateContext &inContext, const PhysicsSystem &inSystem, const BodyLockInterface &inBodyLockInterface, Array<CollidingShape> &ioHits) :
+									Collector(const SoftBodyUpdateContext &inContext, const PhysicsSystem &inSystem, const BodyLockInterface &inBodyLockInterface, Array<CollidingShape> &ioHits, Array<CollidingSensor> &ioSensors) :
 										mContext(inContext),
 										mInverseTransform(inContext.mCenterOfMassTransform.InversedRotationTranslation()),
 										mBodyLockInterface(inBodyLockInterface),
 										mCombineFriction(inSystem.GetCombineFriction()),
 										mCombineRestitution(inSystem.GetCombineRestitution()),
-										mHits(ioHits)
+										mHits(ioHits),
+										mSensors(ioSensors)
 		{
 		}
 
@@ -149,25 +152,36 @@ void SoftBodyMotionProperties::DetermineCollidingShapes(const SoftBodyUpdateCont
 							return;
 					}
 
-					CollidingShape cs;
-					cs.mCenterOfMassTransform = (mInverseTransform * body.GetCenterOfMassTransform()).ToMat44();
-					cs.mShape = body.GetShape();
-					cs.mBodyID = inResult;
-					cs.mMotionType = body.GetMotionType();
-					cs.mIsSensor = settings.mIsSensor;
-					cs.mUpdateVelocities = false;
-					cs.mFriction = mCombineFriction(soft_body, SubShapeID(), body, SubShapeID());
-					cs.mRestitution = mCombineRestitution(soft_body, SubShapeID(), body, SubShapeID());
-					cs.mSoftBodyInvMassScale = settings.mInvMassScale1;
-					if (cs.mMotionType == EMotionType::Dynamic)
+					Mat44 com = (mInverseTransform * body.GetCenterOfMassTransform()).ToMat44();
+					if (settings.mIsSensor)
+					{
+						CollidingSensor cs;
+						cs.mCenterOfMassTransform = com;
+						cs.mShape = body.GetShape();
+						cs.mBodyID = inResult;
+						mSensors.push_back(cs);
+					}
+					else
 					{
-						const MotionProperties *mp = body.GetMotionProperties();
-						cs.mInvMass = settings.mInvMassScale2 * mp->GetInverseMass();
-						cs.mInvInertia = settings.mInvInertiaScale2 * mp->GetInverseInertiaForRotation(cs.mCenterOfMassTransform.GetRotation());
-						cs.mOriginalLinearVelocity = cs.mLinearVelocity = mInverseTransform.Multiply3x3(mp->GetLinearVelocity());
-						cs.mOriginalAngularVelocity = cs.mAngularVelocity = mInverseTransform.Multiply3x3(mp->GetAngularVelocity());
+						CollidingShape cs;
+						cs.mCenterOfMassTransform = com;
+						cs.mShape = body.GetShape();
+						cs.mBodyID = inResult;
+						cs.mMotionType = body.GetMotionType();
+						cs.mUpdateVelocities = false;
+						cs.mFriction = mCombineFriction(soft_body, SubShapeID(), body, SubShapeID());
+						cs.mRestitution = mCombineRestitution(soft_body, SubShapeID(), body, SubShapeID());
+						cs.mSoftBodyInvMassScale = settings.mInvMassScale1;
+						if (cs.mMotionType == EMotionType::Dynamic)
+						{
+							const MotionProperties *mp = body.GetMotionProperties();
+							cs.mInvMass = settings.mInvMassScale2 * mp->GetInverseMass();
+							cs.mInvInertia = settings.mInvInertiaScale2 * mp->GetInverseInertiaForRotation(cs.mCenterOfMassTransform.GetRotation());
+							cs.mOriginalLinearVelocity = cs.mLinearVelocity = mInverseTransform.Multiply3x3(mp->GetLinearVelocity());
+							cs.mOriginalAngularVelocity = cs.mAngularVelocity = mInverseTransform.Multiply3x3(mp->GetAngularVelocity());
+						}
+						mHits.push_back(cs);
 					}
-					mHits.push_back(cs);
 				}
 			}
 		}
@@ -179,9 +193,10 @@ void SoftBodyMotionProperties::DetermineCollidingShapes(const SoftBodyUpdateCont
 		ContactConstraintManager::CombineFunction mCombineFriction;
 		ContactConstraintManager::CombineFunction mCombineRestitution;
 		Array<CollidingShape> &		mHits;
+		Array<CollidingSensor> &	mSensors;
 	};
 
-	Collector collector(inContext, inSystem, inBodyLockInterface, mCollidingShapes);
+	Collector collector(inContext, inSystem, inBodyLockInterface, mCollidingShapes, mCollidingSensors);
 	AABox bounds = mLocalBounds;
 	bounds.Encapsulate(mLocalPredictedBounds);
 	bounds = bounds.Transformed(inContext.mCenterOfMassTransform);
@@ -192,13 +207,36 @@ void SoftBodyMotionProperties::DetermineCollidingShapes(const SoftBodyUpdateCont
 	inSystem.GetBroadPhaseQuery().CollideAABox(bounds, collector, broadphase_layer_filter, object_layer_filter);
 }
 
-void SoftBodyMotionProperties::DetermineCollisionPlanes(const SoftBodyUpdateContext &inContext, uint inVertexStart, uint inNumVertices)
+void SoftBodyMotionProperties::DetermineCollisionPlanes(uint inVertexStart, uint inNumVertices)
 {
 	JPH_PROFILE_FUNCTION();
 
 	// Generate collision planes
 	for (const CollidingShape &cs : mCollidingShapes)
-		cs.mShape->CollideSoftBodyVertices(cs.mCenterOfMassTransform, Vec3::sReplicate(1.0f), mVertices.data() + inVertexStart, inNumVertices, inContext.mDeltaTime, inContext.mDisplacementDueToGravity, int(&cs - mCollidingShapes.data()));
+		cs.mShape->CollideSoftBodyVertices(cs.mCenterOfMassTransform, Vec3::sReplicate(1.0f), CollideSoftBodyVertexIterator(mVertices.data() + inVertexStart), inNumVertices, int(&cs - mCollidingShapes.data()));
+}
+
+void SoftBodyMotionProperties::DetermineSensorCollisions(CollidingSensor &ioSensor)
+{
+	JPH_PROFILE_FUNCTION();
+
+	Plane collision_plane;
+	float largest_penetration = -FLT_MAX;
+	int colliding_shape_idx = -1;
+
+	// Collide sensor against all vertices
+	CollideSoftBodyVertexIterator vertex_iterator(
+		StridedPtr<const Vec3>(&mVertices[0].mPosition, sizeof(SoftBodyVertex)), // The position and mass come from the soft body vertex
+		StridedPtr<const float>(&mVertices[0].mInvMass, sizeof(SoftBodyVertex)),
+		StridedPtr<Plane>(&collision_plane, 0), // We want all vertices to result in a single collision so we pass stride 0
+		StridedPtr<float>(&largest_penetration, 0),
+		StridedPtr<int>(&colliding_shape_idx, 0));
+	ioSensor.mShape->CollideSoftBodyVertices(ioSensor.mCenterOfMassTransform, Vec3::sReplicate(1.0f), vertex_iterator, uint(mVertices.size()), 0);
+	ioSensor.mHasContact = largest_penetration > 0.0f;
+
+	// We need a contact callback if one of the sensors collided
+	if (ioSensor.mHasContact)
+		mNeedContactCallback = true;
 }
 
 void SoftBodyMotionProperties::ApplyPressure(const SoftBodyUpdateContext &inContext)
@@ -548,99 +586,97 @@ void SoftBodyMotionProperties::ApplyCollisionConstraintsAndUpdateVelocities(cons
 				{
 					// Remember that there was a collision
 					v.mHasContact = true;
-					mHasContact = true;
 
-					// Sensors should not have a collision response
+					// We need a contact callback if one of the vertices collided
+					mNeedContactCallback = true;
+
+					// Note that we already calculated the velocity, so this does not affect the velocity (next iteration starts by setting previous position to current position)
 					CollidingShape &cs = mCollidingShapes[v.mCollidingShapeIndex];
-					if (!cs.mIsSensor)
+					Vec3 contact_normal = v.mCollisionPlane.GetNormal();
+					v.mPosition += contact_normal * projected_distance;
+
+					// Apply friction as described in Detailed Rigid Body Simulation with Extended Position Based Dynamics - Matthias Muller et al.
+					// See section 3.6:
+					// Inverse mass: w1 = 1 / m1, w2 = 1 / m2 + (r2 x n)^T I^-1 (r2 x n) = 0 for a static object
+					// r2 are the contact point relative to the center of mass of body 2
+					// Lagrange multiplier for contact: lambda = -c / (w1 + w2)
+					// Where c is the constraint equation (the distance to the plane, negative because penetrating)
+					// Contact normal force: fn = lambda / dt^2
+					// Delta velocity due to friction dv = -vt / |vt| * min(dt * friction * fn * (w1 + w2), |vt|) = -vt * min(-friction * c / (|vt| * dt), 1)
+					// Note that I think there is an error in the paper, I added a mass term, see: https://github.com/matthias-research/pages/issues/29
+					// Relative velocity: vr = v1 - v2 - omega2 x r2
+					// Normal velocity: vn = vr . contact_normal
+					// Tangential velocity: vt = vr - contact_normal * vn
+					// Impulse: p = dv / (w1 + w2)
+					// Changes in particle velocities:
+					// v1 = v1 + p / m1
+					// v2 = v2 - p / m2 (no change when colliding with a static body)
+					// w2 = w2 - I^-1 (r2 x p) (no change when colliding with a static body)
+					if (cs.mMotionType == EMotionType::Dynamic)
 					{
-						// Note that we already calculated the velocity, so this does not affect the velocity (next iteration starts by setting previous position to current position)
-						Vec3 contact_normal = v.mCollisionPlane.GetNormal();
-						v.mPosition += contact_normal * projected_distance;
-
-						// Apply friction as described in Detailed Rigid Body Simulation with Extended Position Based Dynamics - Matthias Muller et al.
-						// See section 3.6:
-						// Inverse mass: w1 = 1 / m1, w2 = 1 / m2 + (r2 x n)^T I^-1 (r2 x n) = 0 for a static object
-						// r2 are the contact point relative to the center of mass of body 2
-						// Lagrange multiplier for contact: lambda = -c / (w1 + w2)
-						// Where c is the constraint equation (the distance to the plane, negative because penetrating)
-						// Contact normal force: fn = lambda / dt^2
-						// Delta velocity due to friction dv = -vt / |vt| * min(dt * friction * fn * (w1 + w2), |vt|) = -vt * min(-friction * c / (|vt| * dt), 1)
-						// Note that I think there is an error in the paper, I added a mass term, see: https://github.com/matthias-research/pages/issues/29
-						// Relative velocity: vr = v1 - v2 - omega2 x r2
-						// Normal velocity: vn = vr . contact_normal
-						// Tangential velocity: vt = vr - contact_normal * vn
-						// Impulse: p = dv / (w1 + w2)
-						// Changes in particle velocities:
-						// v1 = v1 + p / m1
-						// v2 = v2 - p / m2 (no change when colliding with a static body)
-						// w2 = w2 - I^-1 (r2 x p) (no change when colliding with a static body)
-						if (cs.mMotionType == EMotionType::Dynamic)
-						{
-							// Calculate normal and tangential velocity (equation 30)
-							Vec3 r2 = v.mPosition - cs.mCenterOfMassTransform.GetTranslation();
-							Vec3 v2 = cs.GetPointVelocity(r2);
-							Vec3 relative_velocity = v.mVelocity - v2;
-							Vec3 v_normal = contact_normal * contact_normal.Dot(relative_velocity);
-							Vec3 v_tangential = relative_velocity - v_normal;
-							float v_tangential_length = v_tangential.Length();
-
-							// Calculate resulting inverse mass of vertex
-							float vertex_inv_mass = cs.mSoftBodyInvMassScale * v.mInvMass;
-
-							// Calculate inverse effective mass
-							Vec3 r2_cross_n = r2.Cross(contact_normal);
-							float w2 = cs.mInvMass + r2_cross_n.Dot(cs.mInvInertia * r2_cross_n);
-							float w1_plus_w2 = vertex_inv_mass + w2;
-							if (w1_plus_w2 > 0.0f)
-							{
-								// Calculate delta relative velocity due to friction (modified equation 31)
-								Vec3 dv;
-								if (v_tangential_length > 0.0f)
-									dv = v_tangential * min(cs.mFriction * projected_distance / (v_tangential_length * dt), 1.0f);
-								else
-									dv = Vec3::sZero();
-
-								// Calculate delta relative velocity due to restitution (equation 35)
-								dv += v_normal;
-								float prev_v_normal = (prev_v - v2).Dot(contact_normal);
-								if (prev_v_normal < restitution_treshold)
-									dv += cs.mRestitution * prev_v_normal * contact_normal;
-
-								// Calculate impulse
-								Vec3 p = dv / w1_plus_w2;
-
-								// Apply impulse to particle
-								v.mVelocity -= p * vertex_inv_mass;
-
-								// Apply impulse to rigid body
-								cs.mLinearVelocity += p * cs.mInvMass;
-								cs.mAngularVelocity += cs.mInvInertia * r2.Cross(p);
-
-								// Mark that the velocities of the body we hit need to be updated
-								cs.mUpdateVelocities = true;
-							}
-						}
-						else if (cs.mSoftBodyInvMassScale > 0.0f)
+						// Calculate normal and tangential velocity (equation 30)
+						Vec3 r2 = v.mPosition - cs.mCenterOfMassTransform.GetTranslation();
+						Vec3 v2 = cs.GetPointVelocity(r2);
+						Vec3 relative_velocity = v.mVelocity - v2;
+						Vec3 v_normal = contact_normal * contact_normal.Dot(relative_velocity);
+						Vec3 v_tangential = relative_velocity - v_normal;
+						float v_tangential_length = v_tangential.Length();
+
+						// Calculate resulting inverse mass of vertex
+						float vertex_inv_mass = cs.mSoftBodyInvMassScale * v.mInvMass;
+
+						// Calculate inverse effective mass
+						Vec3 r2_cross_n = r2.Cross(contact_normal);
+						float w2 = cs.mInvMass + r2_cross_n.Dot(cs.mInvInertia * r2_cross_n);
+						float w1_plus_w2 = vertex_inv_mass + w2;
+						if (w1_plus_w2 > 0.0f)
 						{
-							// Body is not movable, equations are simpler
-
-							// Calculate normal and tangential velocity (equation 30)
-							Vec3 v_normal = contact_normal * contact_normal.Dot(v.mVelocity);
-							Vec3 v_tangential = v.mVelocity - v_normal;
-							float v_tangential_length = v_tangential.Length();
-
-							// Apply friction (modified equation 31)
+							// Calculate delta relative velocity due to friction (modified equation 31)
+							Vec3 dv;
 							if (v_tangential_length > 0.0f)
-								v.mVelocity -= v_tangential * min(cs.mFriction * projected_distance / (v_tangential_length * dt), 1.0f);
+								dv = v_tangential * min(cs.mFriction * projected_distance / (v_tangential_length * dt), 1.0f);
+							else
+								dv = Vec3::sZero();
 
-							// Apply restitution (equation 35)
-							v.mVelocity -= v_normal;
-							float prev_v_normal = prev_v.Dot(contact_normal);
+							// Calculate delta relative velocity due to restitution (equation 35)
+							dv += v_normal;
+							float prev_v_normal = (prev_v - v2).Dot(contact_normal);
 							if (prev_v_normal < restitution_treshold)
-								v.mVelocity -= cs.mRestitution * prev_v_normal * contact_normal;
+								dv += cs.mRestitution * prev_v_normal * contact_normal;
+
+							// Calculate impulse
+							Vec3 p = dv / w1_plus_w2;
+
+							// Apply impulse to particle
+							v.mVelocity -= p * vertex_inv_mass;
+
+							// Apply impulse to rigid body
+							cs.mLinearVelocity += p * cs.mInvMass;
+							cs.mAngularVelocity += cs.mInvInertia * r2.Cross(p);
+
+							// Mark that the velocities of the body we hit need to be updated
+							cs.mUpdateVelocities = true;
 						}
 					}
+					else if (cs.mSoftBodyInvMassScale > 0.0f)
+					{
+						// Body is not movable, equations are simpler
+
+						// Calculate normal and tangential velocity (equation 30)
+						Vec3 v_normal = contact_normal * contact_normal.Dot(v.mVelocity);
+						Vec3 v_tangential = v.mVelocity - v_normal;
+						float v_tangential_length = v_tangential.Length();
+
+						// Apply friction (modified equation 31)
+						if (v_tangential_length > 0.0f)
+							v.mVelocity -= v_tangential * min(cs.mFriction * projected_distance / (v_tangential_length * dt), 1.0f);
+
+						// Apply restitution (equation 35)
+						v.mVelocity -= v_normal;
+						float prev_v_normal = prev_v.Dot(contact_normal);
+						if (prev_v_normal < restitution_treshold)
+							v.mVelocity -= cs.mRestitution * prev_v_normal * contact_normal;
+					}
 				}
 			}
 		}
@@ -651,8 +687,18 @@ void SoftBodyMotionProperties::UpdateSoftBodyState(SoftBodyUpdateContext &ioCont
 	JPH_PROFILE_FUNCTION();
 
 	// Contact callback
-	if (mHasContact && ioContext.mContactListener != nullptr)
+	if (mNeedContactCallback && ioContext.mContactListener != nullptr)
+	{
+		// Remove non-colliding sensors from the list
+		for (int i = int(mCollidingSensors.size()) - 1; i >= 0; --i)
+			if (!mCollidingSensors[i].mHasContact)
+			{			
+				mCollidingSensors[i] = std::move(mCollidingSensors.back());
+				mCollidingSensors.pop_back();
+			}
+
 		ioContext.mContactListener->OnSoftBodyContactAdded(*ioContext.mBody, SoftBodyManifold(this));
+	}
 
 	// Loop through vertices once more to update the global state
 	float dt = ioContext.mDeltaTime;
@@ -660,7 +706,6 @@ void SoftBodyMotionProperties::UpdateSoftBodyState(SoftBodyUpdateContext &ioCont
 	float max_v_sq = 0.0f;
 	Vec3 linear_velocity = Vec3::sZero(), angular_velocity = Vec3::sZero();
 	mLocalPredictedBounds = mLocalBounds = { };
-	mHasContact = false;
 	for (Vertex &v : mVertices)
 	{
 		// Calculate max square velocity
@@ -682,9 +727,7 @@ void SoftBodyMotionProperties::UpdateSoftBodyState(SoftBodyUpdateContext &ioCont
 		mLocalPredictedBounds.Encapsulate(v.mPosition + v.mVelocity * dt + ioContext.mDisplacementDueToGravity);
 
 		// Reset collision data for the next iteration
-		v.mCollidingShapeIndex = -1;
-		v.mHasContact = false;
-		v.mLargestPenetration = -FLT_MAX;
+		v.ResetCollision();
 	}
 
 	// Calculate linear/angular velocity of the body by averaging all vertices and bringing the value to world space
@@ -742,8 +785,9 @@ void SoftBodyMotionProperties::UpdateRigidBodyVelocities(const SoftBodyUpdateCon
 		if (cs.mUpdateVelocities)
 			inBodyInterface.AddLinearAndAngularVelocity(cs.mBodyID, inContext.mCenterOfMassTransform.Multiply3x3(cs.mLinearVelocity - cs.mOriginalLinearVelocity), inContext.mCenterOfMassTransform.Multiply3x3(cs.mAngularVelocity - cs.mOriginalAngularVelocity));
 
-	// Clear colliding shapes to avoid hanging on to references to shapes
+	// Clear colliding shapes/sensors to avoid hanging on to references to shapes
 	mCollidingShapes.clear();
+	mCollidingSensors.clear();
 }
 
 void SoftBodyMotionProperties::InitializeUpdateContext(float inDeltaTime, Body &inSoftBody, const PhysicsSystem &inSystem, SoftBodyUpdateContext &ioContext)
@@ -777,6 +821,15 @@ void SoftBodyMotionProperties::StartNextIteration(const SoftBodyUpdateContext &i
 	IntegratePositions(ioContext);
 }
 
+void SoftBodyMotionProperties::StartFirstIteration(SoftBodyUpdateContext &ioContext)
+{
+	// Start the first iteration
+	JPH_IF_ENABLE_ASSERTS(uint iteration =) ioContext.mNextIteration.fetch_add(1, memory_order_relaxed);
+	JPH_ASSERT(iteration == 0);
+	StartNextIteration(ioContext);
+	ioContext.mState.store(SoftBodyUpdateContext::EState::ApplyConstraints, memory_order_release);
+}
+
 SoftBodyMotionProperties::EStatus SoftBodyMotionProperties::ParallelDetermineCollisionPlanes(SoftBodyUpdateContext &ioContext)
 {
 	// Do a relaxed read first to see if there is any work to do (this prevents us from doing expensive atomic operations and also prevents us from continuously incrementing the counter and overflowing it)
@@ -789,19 +842,44 @@ SoftBodyMotionProperties::EStatus SoftBodyMotionProperties::ParallelDetermineCol
 		{
 			// Process collision planes
 			uint num_vertices_to_process = min(SoftBodyUpdateContext::cVertexCollisionBatch, num_vertices - next_vertex);
-			DetermineCollisionPlanes(ioContext, next_vertex, num_vertices_to_process);
+			DetermineCollisionPlanes(next_vertex, num_vertices_to_process);
 			uint vertices_processed = ioContext.mNumCollisionVerticesProcessed.fetch_add(SoftBodyUpdateContext::cVertexCollisionBatch, memory_order_release) + num_vertices_to_process;
 			if (vertices_processed >= num_vertices)
 			{
-				// Start the first iteration
-				JPH_IF_ENABLE_ASSERTS(uint iteration =) ioContext.mNextIteration.fetch_add(1, memory_order_relaxed);
-				JPH_ASSERT(iteration == 0);
-				StartNextIteration(ioContext);
-				ioContext.mState.store(SoftBodyUpdateContext::EState::ApplyConstraints, memory_order_release);
+				// Determine next state
+				if (mCollidingSensors.empty())
+					StartFirstIteration(ioContext);
+				else
+					ioContext.mState.store(SoftBodyUpdateContext::EState::DetermineSensorCollisions, memory_order_release);
 			}
 			return EStatus::DidWork;
 		}
 	}
+
+	return EStatus::NoWork;
+}
+
+SoftBodyMotionProperties::EStatus SoftBodyMotionProperties::ParallelDetermineSensorCollisions(SoftBodyUpdateContext &ioContext)
+{
+	// Do a relaxed read to see if there are more sensors to process
+	uint num_sensors = (uint)mCollidingSensors.size();
+	if (ioContext.mNextSensorIndex.load(memory_order_relaxed) < num_sensors)
+	{
+		// Fetch next sensor to process
+		uint sensor_index = ioContext.mNextSensorIndex.fetch_add(1, memory_order_acquire);
+		if (sensor_index < num_sensors)
+		{
+			// Process this sensor
+			DetermineSensorCollisions(mCollidingSensors[sensor_index]);
+
+			// Determine next state
+			uint sensors_processed = ioContext.mNumSensorsProcessed.fetch_add(1, memory_order_release) + 1;
+			if (sensors_processed >= num_sensors)
+				StartFirstIteration(ioContext);
+			return EStatus::DidWork;
+		}
+	}
+
 	return EStatus::NoWork;
 }
 
@@ -895,6 +973,9 @@ SoftBodyMotionProperties::EStatus SoftBodyMotionProperties::ParallelUpdate(SoftB
 	case SoftBodyUpdateContext::EState::DetermineCollisionPlanes:
 		return ParallelDetermineCollisionPlanes(ioContext);
 
+	case SoftBodyUpdateContext::EState::DetermineSensorCollisions:
+		return ParallelDetermineSensorCollisions(ioContext);
+
 	case SoftBodyUpdateContext::EState::ApplyConstraints:
 		return ParallelApplyConstraints(ioContext, inPhysicsSettings);
 

Jolt/Physics/SoftBody/SoftBodyMotionProperties.h

@@ -175,7 +175,6 @@ private:
 		RefConst<Shape>					mShape;										///< Shape of the body we hit
 		BodyID							mBodyID;									///< Body ID of the body we hit
 		EMotionType						mMotionType;								///< Motion type of the body we hit
-		bool							mIsSensor;									///< If the contact should be treated as a sensor vs body contact (no collision response)
 		float							mInvMass;									///< Inverse mass of the body we hit
 		float							mFriction;									///< Combined friction of the two bodies
 		float							mRestitution;								///< Combined restitution of the two bodies
@@ -188,6 +187,15 @@ private:
 		Vec3							mOriginalAngularVelocity;					///< Angular velocity of the body in local space to the soft body at start
 	};
 
+	// Collect information about the colliding sensors
+	struct CollidingSensor
+	{
+		Mat44							mCenterOfMassTransform;						///< Transform of the body relative to the soft body
+		RefConst<Shape>					mShape;										///< Shape of the body we hit
+		BodyID							mBodyID;									///< Body ID of the body we hit
+		bool							mHasContact;								///< If the sensor collided with the soft body
+	};
+
 	// Information about the state of all skinned vertices
 	struct SkinState
 	{
@@ -197,7 +205,10 @@ private:
 	};
 
 	/// Do a narrow phase check and determine the closest feature that we can collide with
-	void								DetermineCollisionPlanes(const SoftBodyUpdateContext &inContext, uint inVertexStart, uint inNumVertices);
+	void								DetermineCollisionPlanes(uint inVertexStart, uint inNumVertices);
+
+	/// Do a narrow phase check between a single sensor and the soft body
+	void								DetermineSensorCollisions(CollidingSensor &ioSensor);
 
 	/// Apply pressure force and update the vertex velocities
 	void								ApplyPressure(const SoftBodyUpdateContext &inContext);
@@ -226,12 +237,18 @@ private:
 	/// Update the state of the soft body (position, velocity, bounds)
 	void								UpdateSoftBodyState(SoftBodyUpdateContext &ioContext, const PhysicsSettings &inPhysicsSettings);
 
+	/// Start the first solver iteration
+	void								StartFirstIteration(SoftBodyUpdateContext &ioContext);
+
 	/// Executes tasks that need to run on the start of an iteration (i.e. the stuff that can't run in parallel)
 	void								StartNextIteration(const SoftBodyUpdateContext &ioContext);
 
 	/// Helper function for ParallelUpdate that works on batches of collision planes
 	EStatus								ParallelDetermineCollisionPlanes(SoftBodyUpdateContext &ioContext);
 
+	/// Helper function for ParallelUpdate that works on sensor collisions
+	EStatus								ParallelDetermineSensorCollisions(SoftBodyUpdateContext &ioContext);
+
 	/// Helper function for ParallelUpdate that works on batches of constraints
 	EStatus								ParallelApplyConstraints(SoftBodyUpdateContext &ioContext, const PhysicsSettings &inPhysicsSettings);
 
@@ -252,6 +269,7 @@ private:
 	RefConst<SoftBodySharedSettings>	mSettings;									///< Configuration of the particles and constraints
 	Array<Vertex>						mVertices;									///< Current state of all vertices in the simulation
 	Array<CollidingShape>				mCollidingShapes;							///< List of colliding shapes retrieved during the last update
+	Array<CollidingSensor>				mCollidingSensors;							///< List of colliding sensors retrieved during the last update
 	Array<SkinState>					mSkinState;									///< List of skinned positions (1-on-1 with mVertices but only those that are used by the skinning constraints are filled in)
 	AABox								mLocalBounds;								///< Bounding box of all vertices
 	AABox								mLocalPredictedBounds;						///< Predicted bounding box for all vertices using extrapolation of velocity by last step delta time
@@ -259,7 +277,7 @@ private:
 	float								mPressure;									///< n * R * T, amount of substance * ideal gas constant * absolute temperature, see https://en.wikipedia.org/wiki/Pressure
 	float								mSkinnedMaxDistanceMultiplier = 1.0f;		///< Multiplier applied to Skinned::mMaxDistance to allow tightening or loosening of the skin constraints
 	bool								mUpdatePosition;							///< Update the position of the body while simulating (set to false for something that is attached to the static world)
-	bool								mHasContact = false;						///< True if the soft body has collided with anything in the last update
+	bool								mNeedContactCallback = false;						///< True if the soft body has collided with anything in the last update
 	bool								mEnableSkinConstraints = true;				///< If skin constraints are enabled
 	bool								mSkinStatePreviousPositionValid = false;	///< True if the skinning was updated in the last update so that the previous position of the skin state is valid
 };

Jolt/Physics/SoftBody/SoftBodyShape.cpp

@@ -114,7 +114,7 @@ void SoftBodyShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSub
 	sCollidePointUsingRayCast(*this, inPoint, inSubShapeIDCreator, ioCollector, inShapeFilter);
 }
 
-void SoftBodyShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const
+void SoftBodyShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const
 {
 	/* Not implemented */
 }

Jolt/Physics/SoftBody/SoftBodyShape.h

@@ -47,7 +47,7 @@ public:
 	virtual bool					CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override;
 	virtual void					CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
 	virtual void					CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
-	virtual void					CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override;
+	virtual void					CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override;
 	virtual void					GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override;
 	virtual int						GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override;
 	virtual Stats					GetStats() const override;

Jolt/Physics/SoftBody/SoftBodyUpdateContext.h

@@ -34,6 +34,7 @@ public:
 	enum class EState
 	{
 		DetermineCollisionPlanes,													///< Determine collision planes for vertices in parallel
+		DetermineSensorCollisions,													///< Determine collisions with sensors in parallel
 		ApplyConstraints,															///< Apply constraints in parallel
 		Done																		///< Update is finished
 	};
@@ -41,7 +42,9 @@ public:
 	// State of the update
 	atomic<EState>						mState { EState::DetermineCollisionPlanes };///< Current state of the update
 	atomic<uint>						mNextCollisionVertex { 0 };					///< Next vertex to process for DetermineCollisionPlanes
-	atomic<uint>						mNumCollisionVerticesProcessed { 0 };		///< Number of vertices processed by DetermineCollisionPlanes, used to determine if we can start simulating
+	atomic<uint>						mNumCollisionVerticesProcessed { 0 };		///< Number of vertices processed by DetermineCollisionPlanes, used to determine if we can go to the next step
+	atomic<uint>						mNextSensorIndex { 0 };						///< Next sensor to process for DetermineCollisionPlanes
+	atomic<uint>						mNumSensorsProcessed { 0 };					///< Number of sensors processed by DetermineSensorCollisions, used to determine if we can go to the next step
 	atomic<uint>						mNextIteration { 0 };						///< Next simulation iteration to process
 	atomic<uint>						mNextConstraintGroup { 0 };					///< Next constraint group to process
 	atomic<uint>						mNumConstraintGroupsProcessed { 0 };		///< Number of groups processed, used to determine if we can go to the next iteration

Jolt/Physics/SoftBody/SoftBodyVertex.h

@@ -15,13 +15,21 @@ JPH_NAMESPACE_BEGIN
 class SoftBodyVertex
 {
 public:
-	Vec3			mPreviousPosition;					///< Position at the previous time step
+	/// Reset collision information to prepare for a new collision check
+	inline void		ResetCollision()
+	{
+		mLargestPenetration = -FLT_MAX;
+		mCollidingShapeIndex = -1;
+		mHasContact = false;
+	}
+
+	Vec3			mPreviousPosition;					///< Internal use only. Position at the previous time step
 	Vec3			mPosition;							///< Position, relative to the center of mass of the soft body
 	Vec3			mVelocity;							///< Velocity, relative to the center of mass of the soft body
-	Plane			mCollisionPlane;					///< Nearest collision plane, relative to the center of mass of the soft body
-	int				mCollidingShapeIndex;				///< Index in the colliding shapes list of the body we may collide with
+	Plane			mCollisionPlane;					///< Internal use only. Nearest collision plane, relative to the center of mass of the soft body
+	int				mCollidingShapeIndex;				///< Internal use only. Index in the colliding shapes list of the body we may collide with
 	bool			mHasContact;						///< True if the vertex has collided with anything in the last update
-	float			mLargestPenetration;				///< Used while finding the collision plane, stores the largest penetration found so far
+	float			mLargestPenetration;				///< Internal use only. Used while finding the collision plane, stores the largest penetration found so far
 	float			mInvMass;							///< Inverse mass (1 / mass)
 };
 

Samples/Samples.cmake

@@ -177,6 +177,8 @@ set(SAMPLES_SRC_FILES
 	${SAMPLES_ROOT}/Tests/SoftBody/SoftBodyShapesTest.h
 	${SAMPLES_ROOT}/Tests/SoftBody/SoftBodySkinnedConstraintTest.cpp
 	${SAMPLES_ROOT}/Tests/SoftBody/SoftBodySkinnedConstraintTest.h
+	${SAMPLES_ROOT}/Tests/SoftBody/SoftBodySensorTest.cpp
+	${SAMPLES_ROOT}/Tests/SoftBody/SoftBodySensorTest.h
 	${SAMPLES_ROOT}/Tests/SoftBody/SoftBodyStressTest.cpp
 	${SAMPLES_ROOT}/Tests/SoftBody/SoftBodyStressTest.h
 	${SAMPLES_ROOT}/Tests/SoftBody/SoftBodyUpdatePositionTest.cpp

Samples/SamplesApp.cpp

@@ -41,6 +41,7 @@
 #include <Jolt/Physics/Collision/Shape/ScaledShape.h>
 #include <Jolt/Physics/Collision/Shape/EmptyShape.h>
 #include <Jolt/Physics/Collision/NarrowPhaseStats.h>
+#include <Jolt/Physics/Collision/CollideSoftBodyVertexIterator.h>
 #include <Jolt/Physics/Constraints/DistanceConstraint.h>
 #include <Jolt/Physics/Constraints/PulleyConstraint.h>
 #include <Jolt/Physics/Character/CharacterVirtual.h>
@@ -355,6 +356,7 @@ JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, SoftBodyCustomUpdateTest)
 JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, SoftBodyLRAConstraintTest)
 JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, SoftBodyBendConstraintTest)
 JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, SoftBodySkinnedConstraintTest)
+JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, SoftBodySensorTest)
 
 static TestNameAndRTTI sSoftBodyTests[] =
 {
@@ -373,7 +375,8 @@ static TestNameAndRTTI sSoftBodyTests[] =
 	{ "Soft Body Custom Update",		JPH_RTTI(SoftBodyCustomUpdateTest) },
 	{ "Soft Body LRA Constraint",		JPH_RTTI(SoftBodyLRAConstraintTest) },
 	{ "Soft Body Bend Constraint",		JPH_RTTI(SoftBodyBendConstraintTest) },
-	{ "Soft Body Skinned Constraint",	JPH_RTTI(SoftBodySkinnedConstraintTest) }
+	{ "Soft Body Skinned Constraint",	JPH_RTTI(SoftBodySkinnedConstraintTest) },
+	{ "Soft Body vs Sensor",			JPH_RTTI(SoftBodySensorTest) }
 };
 
 JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, BroadPhaseCastRayTest)
@@ -1466,31 +1469,32 @@ bool SamplesApp::CastProbe(float inProbeLength, float &outFraction, RVec3 &outPo
 
 	case EProbeMode::CollideSoftBody:
 		{
-			// Create a soft body vertex
 			const float fraction = 0.2f;
 			const float max_distance = 10.0f;
-			SoftBodyVertex vertex;
-			vertex.mInvMass = 1.0f;
-			vertex.mPosition = fraction * direction;
-			vertex.mVelocity = 10.0f * direction;
-			vertex.mCollidingShapeIndex = -1;
-			vertex.mLargestPenetration = -FLT_MAX;
+
+			// Create a soft body vertex iterator
+			const float inv_mass = 1.0f;
+			const Vec3 position = fraction * direction;
+			Plane largest_penetration_collision_plane;
+			float largest_penetration = -FLT_MAX;
+			int largest_penetration_colliding_shape_idx = -1;
+			CollideSoftBodyVertexIterator vertex_iterator(&position, &inv_mass, &largest_penetration_collision_plane, &largest_penetration, &largest_penetration_colliding_shape_idx);
 
 			// Get shapes in a large radius around the start position
-			AABox box(Vec3(start + vertex.mPosition), max_distance);
+			AABox box(Vec3(start + position), max_distance);
 			AllHitCollisionCollector<TransformedShapeCollector> collector;
 			mPhysicsSystem->GetNarrowPhaseQuery().CollectTransformedShapes(box, collector);
 
 			// Closest point found using CollideShape, position relative to 'start'
-			Vec3 closest_point = vertex.mPosition;
+			Vec3 closest_point = position;
 			float closest_point_penetration = 0;
 
 			// Test against each shape
 			for (const TransformedShape &ts : collector.mHits)
 			{
 				int colliding_shape_index = int(&ts - collector.mHits.data());
-				ts.mShape->CollideSoftBodyVertices((RMat44::sTranslation(-start) * ts.GetCenterOfMassTransform()).ToMat44(), ts.GetShapeScale(), &vertex, 1, 1.0f / 60.0f, Vec3::sZero(), colliding_shape_index);
-				if (vertex.mCollidingShapeIndex == colliding_shape_index)
+				ts.mShape->CollideSoftBodyVertices((RMat44::sTranslation(-start) * ts.GetCenterOfMassTransform()).ToMat44(), ts.GetShapeScale(), vertex_iterator, 1, colliding_shape_index);
+				if (largest_penetration_colliding_shape_idx == colliding_shape_index)
 				{
 					// To draw a plane, we need a point but CollideSoftBodyVertices doesn't provide one, so we use CollideShape with a tiny sphere to get the closest point and then project that onto the plane to draw the plane
 					SphereShape point_sphere(1.0e-6f);
@@ -1498,7 +1502,7 @@ bool SamplesApp::CastProbe(float inProbeLength, float &outFraction, RVec3 &outPo
 					CollideShapeSettings settings;
 					settings.mMaxSeparationDistance = sqrt(3.0f) * max_distance; // Box is extended in all directions by max_distance
 					ClosestHitCollisionCollector<CollideShapeCollector> collide_shape_collector;
-					ts.CollideShape(&point_sphere, Vec3::sReplicate(1.0f), RMat44::sTranslation(start + vertex.mPosition), settings, start, collide_shape_collector);
+					ts.CollideShape(&point_sphere, Vec3::sReplicate(1.0f), RMat44::sTranslation(start + position), settings, start, collide_shape_collector);
 					if (collide_shape_collector.HadHit())
 					{
 						closest_point = collide_shape_collector.mHit.mContactPointOn2;
@@ -1508,19 +1512,19 @@ bool SamplesApp::CastProbe(float inProbeLength, float &outFraction, RVec3 &outPo
 			}
 
 			// Draw test point
-			mDebugRenderer->DrawMarker(start + vertex.mPosition, Color::sYellow, 0.1f);
+			mDebugRenderer->DrawMarker(start + position, Color::sYellow, 0.1f);
 			mDebugRenderer->DrawMarker(start + closest_point, Color::sRed, 0.1f);
 
 			// Draw collision plane
-			if (vertex.mCollidingShapeIndex != -1)
+			if (largest_penetration_colliding_shape_idx != -1)
 			{
-				RVec3 plane_point = start + vertex.mPosition - vertex.mCollisionPlane.GetNormal() * vertex.mCollisionPlane.SignedDistance(vertex.mPosition);
-				mDebugRenderer->DrawPlane(plane_point, vertex.mCollisionPlane.GetNormal(), Color::sGreen, 2.0f);
+				RVec3 plane_point = start + position - largest_penetration_collision_plane.GetNormal() * largest_penetration_collision_plane.SignedDistance(position);
+				mDebugRenderer->DrawPlane(plane_point, largest_penetration_collision_plane.GetNormal(), Color::sGreen, 2.0f);
 
-				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));
+				if (abs(closest_point_penetration - largest_penetration) > 0.001f)
+					mDebugRenderer->DrawText3D(plane_point, StringFormat("Pen %f (exp %f)", (double)largest_penetration, (double)closest_point_penetration));
 				else
-					mDebugRenderer->DrawText3D(plane_point, StringFormat("Pen %f", (double)vertex.mLargestPenetration));
+					mDebugRenderer->DrawText3D(plane_point, StringFormat("Pen %f", (double)largest_penetration));
 			}
 		}
 		break;

Samples/Tests/SoftBody/SoftBodySensorTest.cpp

@@ -0,0 +1,69 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2024 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/SoftBody/SoftBodySensorTest.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Jolt/Physics/SoftBody/SoftBodyCreationSettings.h>
+#include <Jolt/Physics/Collision/Shape/SphereShape.h>
+#include <Jolt/Physics/Collision/Shape/TaperedCylinderShape.h>
+#include <Jolt/Physics/SoftBody/SoftBodyManifold.h>
+#include <Utils/SoftBodyCreator.h>
+#include <Renderer/DebugRendererImp.h>
+#include <Layers.h>
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(SoftBodySensorTest)
+{
+	JPH_ADD_BASE_CLASS(SoftBodySensorTest, Test)
+}
+
+void SoftBodySensorTest::Initialize()
+{
+	// Install contact listener for soft bodies
+	mPhysicsSystem->SetSoftBodyContactListener(this);
+
+	// Floor
+	CreateFloor();
+
+	// Create cloth that's fixated at the corners
+	SoftBodyCreationSettings cloth(SoftBodyCreator::CreateClothWithFixatedCorners(), RVec3(0, 10.0f, 0), Quat::sIdentity(), Layers::MOVING);
+	mBodyInterface->CreateAndAddSoftBody(cloth, EActivation::Activate);
+
+	// Some sensors to detect the cloth
+	BodyCreationSettings cylinder_sensor(new TaperedCylinderShapeSettings(4.0f, 1.0f, 2.0f), RVec3(0, 6, 0), Quat::sRotation(Vec3::sAxisX(), 0.5f * JPH_PI), EMotionType::Static, Layers::SENSOR);
+	cylinder_sensor.mIsSensor = true;
+	mBodyInterface->CreateAndAddBody(cylinder_sensor, EActivation::DontActivate);
+
+	BodyCreationSettings sphere_sensor(new SphereShape(4.0f), RVec3(4, 5, 0), Quat::sIdentity(), EMotionType::Static, Layers::SENSOR);
+	sphere_sensor.mIsSensor = true;
+	mBodyInterface->CreateAndAddBody(sphere_sensor, EActivation::DontActivate);
+
+	// Sphere that falls on the cloth to check that we don't ignore this collision
+	BodyCreationSettings bcs(new SphereShape(1.0f), RVec3(0, 15, 0), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING);
+	bcs.mOverrideMassProperties = EOverrideMassProperties::CalculateInertia;
+	bcs.mMassPropertiesOverride.mMass = 500.0f;
+	mBodyInterface->CreateAndAddBody(bcs, EActivation::Activate);
+}
+
+void SoftBodySensorTest::OnSoftBodyContactAdded(const Body &inSoftBody, const SoftBodyManifold &inManifold)
+{
+	// Draw the vertices that are in contact
+	RMat44 com = inSoftBody.GetCenterOfMassTransform();
+	for (const SoftBodyVertex &v : inManifold.GetVertices())
+		if (inManifold.HasContact(v))
+			DebugRenderer::sInstance->DrawMarker(com * v.mPosition, Color::sGreen, 0.1f);
+
+	// Draw the sensors that are in contact with the soft body
+	for (uint i = 0; i < inManifold.GetNumSensorContacts(); ++i)
+	{
+		BodyID sensor_id = inManifold.GetSensorContactBodyID(i);
+		BodyLockRead lock(mPhysicsSystem->GetBodyLockInterfaceNoLock(), sensor_id); // Can't lock in a callback
+		if (lock.SucceededAndIsInBroadPhase())
+		{
+			AABox bounds = lock.GetBody().GetWorldSpaceBounds();
+			DebugRenderer::sInstance->DrawWireBox(bounds, Color::sGreen);
+		}
+	}
+}

Samples/Tests/SoftBody/SoftBodySensorTest.h

@@ -0,0 +1,21 @@
+// 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/SoftBodyContactListener.h>
+
+// This test shows interaction between a soft body and a sensor
+class SoftBodySensorTest : public Test, public SoftBodyContactListener
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, SoftBodySensorTest)
+
+	// See: Test
+	virtual void		Initialize() override;
+
+	// See: SoftBodyContactListener
+	virtual void		OnSoftBodyContactAdded(const Body &inSoftBody, const SoftBodyManifold &inManifold) override;
+};