Implemented SimShapeFilter for soft bodies (#1365)

* Added a demo of it in SimShapeFilterTest.

Jolt/Jolt.cmake

@@ -291,6 +291,7 @@ set(JOLT_PHYSICS_SRC_FILES
 	${JOLT_PHYSICS_ROOT}/Physics/Collision/ShapeCast.h
 	${JOLT_PHYSICS_ROOT}/Physics/Collision/ShapeFilter.h
 	${JOLT_PHYSICS_ROOT}/Physics/Collision/SimShapeFilter.h
+	${JOLT_PHYSICS_ROOT}/Physics/Collision/SimShapeFilterWrapper.h
 	${JOLT_PHYSICS_ROOT}/Physics/Collision/SortReverseAndStore.h
 	${JOLT_PHYSICS_ROOT}/Physics/Collision/TransformedShape.cpp
 	${JOLT_PHYSICS_ROOT}/Physics/Collision/TransformedShape.h

Jolt/Physics/Collision/SimShapeFilterWrapper.h

@@ -0,0 +1,81 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2024 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/SimShapeFilter.h>
+#include <Jolt/Physics/Body/Body.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Helper class to forward ShapeFilter calls to a SimShapeFilter
+/// INTERNAL CLASS DO NOT USE!
+class SimShapeFilterWrapper : public ShapeFilter
+{
+public:
+	/// Constructor
+							SimShapeFilterWrapper(const SimShapeFilter *inFilter, const Body *inBody1) :
+		mFilter(inFilter),
+		mBody1(inBody1)
+	{
+	}
+
+	/// Forward to the simulation shape filter
+	virtual bool			ShouldCollide(const Shape *inShape1, const SubShapeID &inSubShapeIDOfShape1, const Shape *inShape2, const SubShapeID &inSubShapeIDOfShape2) const override
+	{
+		return mFilter->ShouldCollide(*mBody1, inShape1, inSubShapeIDOfShape1, *mBody2, inShape2, inSubShapeIDOfShape2);
+	}
+
+	/// Forward to the simulation shape filter
+	virtual bool			ShouldCollide(const Shape *inShape2, const SubShapeID &inSubShapeIDOfShape2) const override
+	{
+		return mFilter->ShouldCollide(*mBody1, mBody1->GetShape(), SubShapeID(), *mBody2, inShape2, inSubShapeIDOfShape2);
+	}
+
+	/// Set the body we're colliding against
+	void					SetBody2(const Body *inBody2)
+	{
+		mBody2 = inBody2;
+	}
+
+private:
+	const SimShapeFilter *	mFilter;
+	const Body *			mBody1;
+	const Body *			mBody2;
+};
+
+/// In case we don't have a simulation shape filter, we fall back to using a default shape filter that always returns true
+/// INTERNAL CLASS DO NOT USE!
+union SimShapeFilterWrapperUnion
+{
+public:
+	/// Constructor
+							SimShapeFilterWrapperUnion(const SimShapeFilter *inFilter, const Body *inBody1)
+	{
+		// Dirty trick: if we don't have a filter, placement new a standard ShapeFilter so that we
+		// don't have to check for nullptr in the ShouldCollide function
+		if (inFilter != nullptr)
+			new (&mSimShapeFilterWrapper) SimShapeFilterWrapper(inFilter, inBody1);
+		else
+			new (&mSimShapeFilterWrapper) ShapeFilter();
+	}
+
+	/// Destructor
+							~SimShapeFilterWrapperUnion()
+	{
+		// Doesn't need to be destructed
+	}
+
+	/// Accessor
+	SimShapeFilterWrapper &	GetSimShapeFilterWrapper()
+	{
+		return mSimShapeFilterWrapper;
+	}
+
+private:
+	SimShapeFilterWrapper	mSimShapeFilterWrapper;
+	ShapeFilter				mShapeFilter;
+};
+
+JPH_NAMESPACE_END

Jolt/Physics/PhysicsSystem.cpp

@@ -19,7 +19,7 @@
 #include <Jolt/Physics/Collision/CollideConvexVsTriangles.h>
 #include <Jolt/Physics/Collision/ManifoldBetweenTwoFaces.h>
 #include <Jolt/Physics/Collision/Shape/ConvexShape.h>
-#include <Jolt/Physics/Collision/SimShapeFilter.h>
+#include <Jolt/Physics/Collision/SimShapeFilterWrapper.h>
 #include <Jolt/Physics/Collision/InternalEdgeRemovingCollector.h>
 #include <Jolt/Physics/Constraints/CalculateSolverSteps.h>
 #include <Jolt/Physics/Constraints/ConstraintPart/AxisConstraintPart.h>
@@ -967,74 +967,6 @@ void PhysicsSystem::JobFindCollisions(PhysicsUpdateContext::Step *ioStep, int in
 	}
 }
 
-// Helper class to forward ShapeFilter calls to the simulation shape filter
-class SimShapeFilterWrapper : public ShapeFilter
-{
-public:
-	// Constructor
-							SimShapeFilterWrapper(const SimShapeFilter *inFilter, const Body *inBody1) :
-		mFilter(inFilter),
-		mBody1(inBody1)
-	{
-	}
-
-	// Forward to the simulation shape filter
-	virtual bool			ShouldCollide(const Shape *inShape1, const SubShapeID &inSubShapeIDOfShape1, const Shape *inShape2, const SubShapeID &inSubShapeIDOfShape2) const override
-	{
-		return mFilter->ShouldCollide(*mBody1, inShape1, inSubShapeIDOfShape1, *mBody2, inShape2, inSubShapeIDOfShape2);
-	}
-
-	// Set the body we're colliding against
-	void					SetBody2(const Body *inBody2)
-	{
-		mBody2 = inBody2;
-	}
-
-private:
-	// Dummy implemention to satisfy the ShapeFilter interface
-	virtual bool			ShouldCollide(const Shape *inShape2, const SubShapeID &inSubShapeIDOfShape2) const override
-	{
-		JPH_ASSERT(false); // Should not be called
-		return true;
-	}
-
-	const SimShapeFilter *	mFilter;
-	const Body *			mBody1;
-	const Body *			mBody2;
-};
-
-// In case we don't have a simulation shape filter, we fall back to using a default shape filter that always returns true
-union SimShapeFilterWrapperUnion
-{
-public:
-	// Constructor
-							SimShapeFilterWrapperUnion(const SimShapeFilter *inFilter, const Body *inBody1)
-	{
-		// Dirty trick: if we don't have a filter, placement new a standard ShapeFilter so that we
-		// don't have to check for nullptr in the ShouldCollide function
-		if (inFilter != nullptr)
-			new (&mSimShapeFilterWrapper) SimShapeFilterWrapper(inFilter, inBody1);
-		else
-			new (&mSimShapeFilterWrapper) ShapeFilter();
-	}
-
-	// Destructor
-							~SimShapeFilterWrapperUnion()
-	{
-		// Doesn't need to be destructed
-	}
-
-	// Accessor
-	SimShapeFilterWrapper &	GetSimShapeFilterWrapper()
-	{
-		return mSimShapeFilterWrapper;
-	}
-
-private:
-	SimShapeFilterWrapper	mSimShapeFilterWrapper;
-	ShapeFilter				mShapeFilter;
-};
-
 void PhysicsSystem::ProcessBodyPair(ContactAllocator &ioContactAllocator, const BodyPair &inBodyPair)
 {
 	JPH_PROFILE_FUNCTION();

Jolt/Physics/SoftBody/SoftBodyMotionProperties.cpp

@@ -9,6 +9,7 @@
 #include <Jolt/Physics/SoftBody/SoftBodyContactListener.h>
 #include <Jolt/Physics/SoftBody/SoftBodyManifold.h>
 #include <Jolt/Physics/Collision/CollideSoftBodyVertexIterator.h>
+#include <Jolt/Physics/Collision/SimShapeFilterWrapper.h>
 #include <Jolt/Physics/PhysicsSystem.h>
 #include <Jolt/Physics/Body/BodyManager.h>
 #include <Jolt/Core/ScopeExit.h>
@@ -109,12 +110,14 @@ void SoftBodyMotionProperties::DetermineCollidingShapes(const SoftBodyUpdateCont
 
 	struct Collector : public CollideShapeBodyCollector
 	{
-									Collector(const SoftBodyUpdateContext &inContext, const PhysicsSystem &inSystem, const BodyLockInterface &inBodyLockInterface, Array<CollidingShape> &ioHits, Array<CollidingSensor> &ioSensors) :
+									Collector(const SoftBodyUpdateContext &inContext, const PhysicsSystem &inSystem, const BodyLockInterface &inBodyLockInterface, const AABox &inLocalBounds, SimShapeFilterWrapper &inShapeFilter, Array<CollidingShape> &ioHits, Array<CollidingSensor> &ioSensors) :
 										mContext(inContext),
 										mInverseTransform(inContext.mCenterOfMassTransform.InversedRotationTranslation()),
+										mLocalBounds(inLocalBounds),
 										mBodyLockInterface(inBodyLockInterface),
 										mCombineFriction(inSystem.GetCombineFriction()),
 										mCombineRestitution(inSystem.GetCombineRestitution()),
+										mShapeFilter(inShapeFilter),
 										mHits(ioHits),
 										mSensors(ioSensors)
 		{
@@ -152,12 +155,30 @@ void SoftBodyMotionProperties::DetermineCollidingShapes(const SoftBodyUpdateCont
 							return;
 					}
 
+					// Calculate transform of this body relative to the soft body
 					Mat44 com = (mInverseTransform * body.GetCenterOfMassTransform()).ToMat44();
+
+					// Collect leaf shapes
+					mShapeFilter.SetBody2(&body);
+					struct LeafShapeCollector : public TransformedShapeCollector
+					{
+						virtual void		AddHit(const TransformedShape &inResult) override
+						{
+							mHits.emplace_back(Mat44::sRotationTranslation(inResult.mShapeRotation, Vec3(inResult.mShapePositionCOM)), inResult.GetShapeScale(), inResult.mShape);
+						}
+
+						Array<LeafShape>	mHits;
+					};
+					LeafShapeCollector collector;
+					body.GetShape()->CollectTransformedShapes(mLocalBounds, com.GetTranslation(), com.GetQuaternion(), Vec3::sReplicate(1.0f), SubShapeIDCreator(), collector, mShapeFilter);
+					if (collector.mHits.empty())
+						return;
+
 					if (settings.mIsSensor)
 					{
 						CollidingSensor cs;
 						cs.mCenterOfMassTransform = com;
-						cs.mShape = body.GetShape();
+						cs.mShapes = std::move(collector.mHits);
 						cs.mBodyID = inResult;
 						mSensors.push_back(cs);
 					}
@@ -165,7 +186,7 @@ void SoftBodyMotionProperties::DetermineCollidingShapes(const SoftBodyUpdateCont
 					{
 						CollidingShape cs;
 						cs.mCenterOfMassTransform = com;
-						cs.mShape = body.GetShape();
+						cs.mShapes = std::move(collector.mHits);
 						cs.mBodyID = inResult;
 						cs.mMotionType = body.GetMotionType();
 						cs.mUpdateVelocities = false;
@@ -189,22 +210,32 @@ void SoftBodyMotionProperties::DetermineCollidingShapes(const SoftBodyUpdateCont
 	private:
 		const SoftBodyUpdateContext &mContext;
 		RMat44						mInverseTransform;
+		AABox						mLocalBounds;
 		const BodyLockInterface &	mBodyLockInterface;
 		ContactConstraintManager::CombineFunction mCombineFriction;
 		ContactConstraintManager::CombineFunction mCombineRestitution;
+		SimShapeFilterWrapper &		mShapeFilter;
 		Array<CollidingShape> &		mHits;
 		Array<CollidingSensor> &	mSensors;
 	};
 
-	Collector collector(inContext, inSystem, inBodyLockInterface, mCollidingShapes, mCollidingSensors);
-	AABox bounds = mLocalBounds;
-	bounds.Encapsulate(mLocalPredictedBounds);
-	bounds = bounds.Transformed(inContext.mCenterOfMassTransform);
-	bounds.ExpandBy(Vec3::sReplicate(mSettings->mVertexRadius));
+	// Calculate local bounding box
+	AABox local_bounds = mLocalBounds;
+	local_bounds.Encapsulate(mLocalPredictedBounds);
+	local_bounds.ExpandBy(Vec3::sReplicate(mSettings->mVertexRadius));
+
+	// Calculate world space bounding box
+	AABox world_bounds = local_bounds.Transformed(inContext.mCenterOfMassTransform);
+
+	// Create shape filter
+	SimShapeFilterWrapperUnion shape_filter_union(inContext.mSimShapeFilter, inContext.mBody);
+	SimShapeFilterWrapper &shape_filter = shape_filter_union.GetSimShapeFilterWrapper();
+
+	Collector collector(inContext, inSystem, inBodyLockInterface, local_bounds, shape_filter, mCollidingShapes, mCollidingSensors);
 	ObjectLayer layer = inContext.mBody->GetObjectLayer();
 	DefaultBroadPhaseLayerFilter broadphase_layer_filter = inSystem.GetDefaultBroadPhaseLayerFilter(layer);
 	DefaultObjectLayerFilter object_layer_filter = inSystem.GetDefaultLayerFilter(layer);
-	inSystem.GetBroadPhaseQuery().CollideAABox(bounds, collector, broadphase_layer_filter, object_layer_filter);
+	inSystem.GetBroadPhaseQuery().CollideAABox(world_bounds, collector, broadphase_layer_filter, object_layer_filter);
 }
 
 void SoftBodyMotionProperties::DetermineCollisionPlanes(uint inVertexStart, uint inNumVertices)
@@ -213,7 +244,8 @@ void SoftBodyMotionProperties::DetermineCollisionPlanes(uint inVertexStart, uint
 
 	// Generate collision planes
 	for (const CollidingShape &cs : mCollidingShapes)
-		cs.mShape->CollideSoftBodyVertices(cs.mCenterOfMassTransform, Vec3::sReplicate(1.0f), CollideSoftBodyVertexIterator(mVertices.data() + inVertexStart), inNumVertices, int(&cs - mCollidingShapes.data()));
+		for (const LeafShape &shape : cs.mShapes)
+			shape.mShape->CollideSoftBodyVertices(shape.mTransform, shape.mScale, CollideSoftBodyVertexIterator(mVertices.data() + inVertexStart), inNumVertices, int(&cs - mCollidingShapes.data()));
 }
 
 void SoftBodyMotionProperties::DetermineSensorCollisions(CollidingSensor &ioSensor)
@@ -231,7 +263,8 @@ void SoftBodyMotionProperties::DetermineSensorCollisions(CollidingSensor &ioSens
 		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);
+	for (const LeafShape &shape : ioSensor.mShapes)
+		shape.mShape->CollideSoftBodyVertices(shape.mTransform, shape.mScale, vertex_iterator, uint(mVertices.size()), 0);
 	ioSensor.mHasContact = largest_penetration > 0.0f;
 
 	// We need a contact callback if one of the sensors collided
@@ -798,6 +831,7 @@ void SoftBodyMotionProperties::InitializeUpdateContext(float inDeltaTime, Body &
 	ioContext.mBody = &inSoftBody;
 	ioContext.mMotionProperties = this;
 	ioContext.mContactListener = inSystem.GetSoftBodyContactListener();
+	ioContext.mSimShapeFilter = inSystem.GetSimShapeFilter();
 
 	// Convert gravity to local space
 	ioContext.mCenterOfMassTransform = inSoftBody.GetCenterOfMassTransform();

Jolt/Physics/SoftBody/SoftBodyMotionProperties.h

@@ -7,6 +7,7 @@
 #include <Jolt/Geometry/AABox.h>
 #include <Jolt/Physics/Body/BodyID.h>
 #include <Jolt/Physics/Body/MotionProperties.h>
+#include <Jolt/Physics/Collision/TransformedShape.h>
 #include <Jolt/Physics/SoftBody/SoftBodySharedSettings.h>
 #include <Jolt/Physics/SoftBody/SoftBodyVertex.h>
 #include <Jolt/Physics/SoftBody/SoftBodyUpdateContext.h>
@@ -162,6 +163,17 @@ private:
 	// SoftBodyManifold needs to have access to CollidingShape
 	friend class SoftBodyManifold;
 
+	// Information about a leaf shape that we're colliding with
+	struct LeafShape
+	{
+										LeafShape() = default;
+										LeafShape(Mat44Arg inTransform, Vec3Arg inScale, const Shape *inShape) : mTransform(inTransform), mScale(inScale), mShape(inShape) { }
+
+		Mat44							mTransform;									///< Transform of the shape relative to the soft body
+		Vec3							mScale;										///< Scale of the shape
+		RefConst<Shape>					mShape;										///< Shape
+	};
+
 	// Collect information about the colliding bodies
 	struct CollidingShape
 	{
@@ -172,7 +184,7 @@ private:
 		}
 
 		Mat44							mCenterOfMassTransform;						///< Transform of the body relative to the soft body
-		RefConst<Shape>					mShape;										///< Shape of the body we hit
+		Array<LeafShape>				mShapes;									///< Leaf shapes of the body we hit
 		BodyID							mBodyID;									///< Body ID of the body we hit
 		EMotionType						mMotionType;								///< Motion type of the body we hit
 		float							mInvMass;									///< Inverse mass of the body we hit
@@ -191,7 +203,7 @@ private:
 	struct CollidingSensor
 	{
 		Mat44							mCenterOfMassTransform;						///< Transform of the body relative to the soft body
-		RefConst<Shape>					mShape;										///< Shape of the body we hit
+		Array<LeafShape>				mShapes;									///< Leaf shapes of the body we hit
 		BodyID							mBodyID;									///< Body ID of the body we hit
 		bool							mHasContact;								///< If the sensor collided with the soft body
 	};

Jolt/Physics/SoftBody/SoftBodyUpdateContext.h

@@ -12,6 +12,7 @@ JPH_NAMESPACE_BEGIN
 class Body;
 class SoftBodyMotionProperties;
 class SoftBodyContactListener;
+class SimShapeFilter;
 
 /// Temporary data used by the update of a soft body
 class SoftBodyUpdateContext : public NonCopyable
@@ -24,6 +25,7 @@ public:
 	Body *								mBody;										///< Body that is being updated
 	SoftBodyMotionProperties *			mMotionProperties;							///< Motion properties of that body
 	SoftBodyContactListener *			mContactListener;							///< Contact listener to fire callbacks to
+	const SimShapeFilter *				mSimShapeFilter;							///< Shape filter to use for collision detection
 	RMat44								mCenterOfMassTransform;						///< Transform of the body relative to the soft body
 	Vec3								mGravity;									///< Gravity vector in local space of the soft body
 	Vec3								mDisplacementDueToGravity;					///< Displacement of the center of mass due to gravity in the current time step

Samples/Tests/General/SimShapeFilterTest.cpp

@@ -10,6 +10,8 @@
 #include <Jolt/Physics/Collision/Shape/CapsuleShape.h>
 #include <Jolt/Physics/Collision/Shape/StaticCompoundShape.h>
 #include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Jolt/Physics/SoftBody/SoftBodyCreationSettings.h>
+#include <Utils/SoftBodyCreator.h>
 #include <Layers.h>
 
 JPH_IMPLEMENT_RTTI_VIRTUAL(SimShapeFilterTest)
@@ -44,15 +46,34 @@ void SimShapeFilterTest::Initialize()
 	compound->AddShape(Vec3::sZero(), Quat::sIdentity(), capsule);
 	compound->AddShape(Vec3(0, -2, 0), Quat::sIdentity(), sphere);
 	compound->AddShape(Vec3(0, 2, 0), Quat::sIdentity(), box);
-	mShapeFilter.mCompoundID = mBodyInterface->CreateAndAddBody(BodyCreationSettings(compound, RVec3(0, 15, 0), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING), EActivation::Activate);
+
+	// Create compound above the platform
+	BodyCreationSettings compound_body(compound, RVec3(0, 15, 0), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING);
+	mShapeFilter.mCompoundID[0] = mBodyInterface->CreateAndAddBody(compound_body, EActivation::Activate);
+
+	// Create cloth that's fixated at the corners
+	SoftBodyCreationSettings cloth(SoftBodyCreator::CreateClothWithFixatedCorners(20, 20, 0.2f), RVec3(10, 10.0f, 0), Quat::sIdentity(), Layers::MOVING);
+	mShapeFilter.mClothID = mBodyInterface->CreateAndAddSoftBody(cloth, EActivation::Activate);
+
+	// Create compound above the cloth
+	compound_body.mPosition = RVec3(10, 15, 0);
+	mShapeFilter.mCompoundID[1] = mBodyInterface->CreateAndAddBody(compound_body, EActivation::Activate);
 }
 
 bool SimShapeFilterTest::Filter::ShouldCollide(const Body &inBody1, const Shape *inShape1, const SubShapeID &inSubShapeIDOfShape1, const Body &inBody2, const Shape *inShape2, const SubShapeID &inSubShapeIDOfShape2) const
 {
-	// If the platform is colliding with the compound, filter out collisions where the shape has user data 1
-	if (inBody1.GetID() == mPlatformID && inBody2.GetID() == mCompoundID)
-		return inShape2->GetUserData() != 1;
-	else if (inBody1.GetID() == mCompoundID && inBody2.GetID() == mPlatformID)
-		return inShape1->GetUserData() != 1;
+	// If the platform/cloth is colliding with the compound, filter out collisions where the shape has user data 1
+	if (inBody1.GetID() == mPlatformID || inBody1.GetID() == mClothID)
+	{
+		for (int i = 0; i < 2; ++i)
+			if (inBody2.GetID() == mCompoundID[i])
+				return inShape2->GetUserData() != 1;
+	}
+	else if (inBody2.GetID() == mPlatformID || inBody2.GetID() == mClothID)
+	{
+		for (int i = 0; i < 2; ++i)
+			if (inBody1.GetID() == mCompoundID[i])
+				return inShape1->GetUserData() != 1;
+	}
 	return true;
 }

Samples/Tests/General/SimShapeFilterTest.h

@@ -27,7 +27,8 @@ private:
 		virtual bool	ShouldCollide(const Body &inBody1, const Shape *inShape1, const SubShapeID &inSubShapeIDOfShape1, const Body &inBody2, const Shape *inShape2, const SubShapeID &inSubShapeIDOfShape2) const override;
 
 		BodyID			mPlatformID;
-		BodyID			mCompoundID;
+		BodyID			mClothID;
+		BodyID			mCompoundID[2];
 	};
 
 	Filter				mShapeFilter;