Added unit test for SimCollideBodyVsBody override (#1495)

Moved helper function from SimCollideBodyVsBodyTest to CollideShapeVsShapePerLeaf.h

Jolt/Jolt.cmake

@@ -209,6 +209,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/CollideShapeVsShapePerLeaf.h
 	${JOLT_PHYSICS_ROOT}/Physics/Collision/CollideSoftBodyVertexIterator.h
 	${JOLT_PHYSICS_ROOT}/Physics/Collision/CollideSoftBodyVerticesVsTriangles.h
 	${JOLT_PHYSICS_ROOT}/Physics/Collision/CollideSphereVsTriangles.cpp

Jolt/Physics/Collision/CollideShapeVsShapePerLeaf.h

@@ -0,0 +1,86 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2025 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/CollideShape.h>
+#include <Jolt/Physics/Collision/CollisionDispatch.h>
+#include <Jolt/Core/STLLocalAllocator.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Collide 2 shapes and returns at most 1 hit per leaf shape pairs that overlapping. This can be used when not all contacts between the shapes are needed.
+/// E.g. when testing a compound with 2 MeshShapes A and B against a compound with 2 SphereShapes C and D, then at most you'll get 4 collisions: AC, AD, BC, BD.
+/// The default CollisionDispatch::sCollideShapeVsShape function would return all intersecting triangles in A against C, all in B against C etc.
+/// @param inShape1 The first shape
+/// @param inShape2 The second shape
+/// @param inScale1 Local space scale of shape 1 (scales relative to its center of mass)
+/// @param inScale2 Local space scale of shape 2 (scales relative to its center of mass)
+/// @param inCenterOfMassTransform1 Transform to transform center of mass of shape 1 into world space
+/// @param inCenterOfMassTransform2 Transform to transform center of mass of shape 2 into world space
+/// @param inSubShapeIDCreator1 Class that tracks the current sub shape ID for shape 1
+/// @param inSubShapeIDCreator2 Class that tracks the current sub shape ID for shape 2
+/// @param inCollideShapeSettings Options for the CollideShape test
+/// @param ioCollector The collector that receives the results.
+/// @param inShapeFilter allows selectively disabling collisions between pairs of (sub) shapes.
+/// @tparam LeafCollector The type of the collector that will be used to collect hits between leaf pairs. Must be either AnyHitCollisionCollector<CollideShapeCollector> to get any hit (cheapest) or ClosestHitCollisionCollector<CollideShapeCollector> to get the deepest hit (more expensive).
+template <class LeafCollector>
+void CollideShapeVsShapePerLeaf(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter = { })
+{
+	// Tracks information we need about a leaf shape
+	struct LeafShape
+	{
+							LeafShape() = default;
+
+							LeafShape(const AABox &inBounds, Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Shape *inShape, const SubShapeIDCreator &inSubShapeIDCreator) :
+			mBounds(inBounds),
+			mCenterOfMassTransform(inCenterOfMassTransform),
+			mScale(inScale),
+			mShape(inShape),
+			mSubShapeIDCreator(inSubShapeIDCreator)
+		{
+		}
+
+		AABox				mBounds;
+		Mat44				mCenterOfMassTransform;
+		Vec3				mScale;
+		const Shape *		mShape;
+		SubShapeIDCreator	mSubShapeIDCreator;
+	};
+
+	// A collector that stores the information we need from a leaf shape in an array that is usually on the stack but can fall back to the heap if needed
+	class MyCollector : public TransformedShapeCollector
+	{
+	public:
+		void				AddHit(const TransformedShape &inShape) override
+		{
+			mHits.emplace_back(inShape.GetWorldSpaceBounds(), inShape.GetCenterOfMassTransform().ToMat44(), inShape.GetShapeScale(), inShape.mShape, inShape.mSubShapeIDCreator);
+		}
+
+		Array<LeafShape, STLLocalAllocator<LeafShape, 32>> mHits;
+	};
+
+	// Get bounds of both shapes
+	AABox bounds1 = inShape1->GetWorldSpaceBounds(inCenterOfMassTransform1, inScale1);
+	AABox bounds2 = inShape2->GetWorldSpaceBounds(inCenterOfMassTransform2, inScale2);
+
+	// Get leaf shapes that overlap with the bounds of the other shape
+	MyCollector leaf_shapes1, leaf_shapes2;
+	inShape1->CollectTransformedShapes(bounds2, inCenterOfMassTransform1.GetTranslation(), inCenterOfMassTransform1.GetQuaternion(), inScale1, inSubShapeIDCreator1, leaf_shapes1, inShapeFilter);
+	inShape2->CollectTransformedShapes(bounds1, inCenterOfMassTransform2.GetTranslation(), inCenterOfMassTransform2.GetQuaternion(), inScale2, inSubShapeIDCreator2, leaf_shapes2, inShapeFilter);
+
+	// Now test each leaf shape against each other leaf
+	for (const LeafShape &leaf1 : leaf_shapes1.mHits)
+		for (const LeafShape &leaf2 : leaf_shapes2.mHits)
+			if (leaf1.mBounds.Overlaps(leaf2.mBounds))
+			{
+				// Use the leaf collector to collect max 1 hit for this pair and pass it on to ioCollector
+				LeafCollector collector;
+				CollisionDispatch::sCollideShapeVsShape(leaf1.mShape, leaf2.mShape, leaf1.mScale, leaf2.mScale, leaf1.mCenterOfMassTransform, leaf2.mCenterOfMassTransform, leaf1.mSubShapeIDCreator, leaf2.mSubShapeIDCreator, inCollideShapeSettings, collector, inShapeFilter);
+				if (collector.HadHit())
+					ioCollector.AddHit(collector.mHit);
+			}
+}
+
+JPH_NAMESPACE_END

Jolt/Physics/PhysicsSystem.h

@@ -88,8 +88,8 @@ public:
 
 	/// Advanced use only: Set the function that will be used to collide two bodies during simulation.
 	/// This function is expected to eventually call CollideShapeCollector::AddHit all contact points between the shapes of body 1 and 2 in their given transforms.
-	void						SetSimCollideBodyVsBody(SimCollideBodyVsBody inBodyVsBody)	{ mSimCollideBodyVsBody = inBodyVsBody; }
-	SimCollideBodyVsBody		GetSimCollideBodyVsBody() const								{ return mSimCollideBodyVsBody; }
+	void						SetSimCollideBodyVsBody(const SimCollideBodyVsBody &inBodyVsBody) { mSimCollideBodyVsBody = inBodyVsBody; }
+	const SimCollideBodyVsBody &GetSimCollideBodyVsBody() const								{ return mSimCollideBodyVsBody; }
 
 	/// Advanced use only: Default function that is used to collide two bodies during simulation.
 	static void					sDefaultSimCollideBodyVsBody(const Body &inBody1, const Body &inBody2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, CollideShapeSettings &ioCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter);

Samples/Tests/General/SimCollideBodyVsBodyTest.cpp

@@ -10,8 +10,8 @@
 #include <Jolt/Physics/Collision/Shape/StaticCompoundShape.h>
 #include <Jolt/Physics/Collision/CollisionCollectorImpl.h>
 #include <Jolt/Physics/Collision/CollisionDispatch.h>
+#include <Jolt/Physics/Collision/CollideShapeVsShapePerLeaf.h>
 #include <Jolt/Physics/Body/BodyCreationSettings.h>
-#include <Jolt/Core/STLLocalAllocator.h>
 #include <Layers.h>
 #include <Renderer/DebugRendererImp.h>
 
@@ -25,6 +25,7 @@ static void sCollideBodyVsBodyPerBody(const Body &inBody1, const Body &inBody2,
 {
 	if (inBody1.IsSensor() || inBody2.IsSensor())
 	{
+		// A sensor will return max 1 hit per body pair
 		LeafCollector collector;
 		SubShapeIDCreator part1, part2;
 		CollisionDispatch::sCollideShapeVsShape(inBody1.GetShape(), inBody2.GetShape(), Vec3::sOne(), Vec3::sOne(), inCenterOfMassTransform1, inCenterOfMassTransform2, part1, part2, ioCollideShapeSettings, collector);
@@ -43,59 +44,9 @@ static void sCollideBodyVsBodyPerLeaf(const Body &inBody1, const Body &inBody2,
 {
 	if (inBody1.IsSensor() || inBody2.IsSensor())
 	{
-		// Tracks information we need about a leaf shape
-		struct LeafShape
-		{
-								LeafShape() = default;
-
-								LeafShape(const AABox &inBounds, Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Shape *inShape, const SubShapeIDCreator &inSubShapeIDCreator) :
-				mBounds(inBounds),
-				mCenterOfMassTransform(inCenterOfMassTransform),
-				mScale(inScale),
-				mShape(inShape),
-				mSubShapeIDCreator(inSubShapeIDCreator)
-			{
-			}
-
-			AABox				mBounds;
-			Mat44				mCenterOfMassTransform;
-			Vec3				mScale;
-			const Shape *		mShape;
-			SubShapeIDCreator	mSubShapeIDCreator;
-		};
-
-		// A collector that stores the information we need from a leaf shape in an array that is usually on the stack but can fall back to the heap if needed
-		class MyCollector : public TransformedShapeCollector
-		{
-		public:
-			void				AddHit(const TransformedShape &inShape) override
-			{
-				mHits.emplace_back(inShape.GetWorldSpaceBounds(), inShape.GetCenterOfMassTransform().ToMat44(), inShape.GetShapeScale(), inShape.mShape, inShape.mSubShapeIDCreator);
-			}
-
-			Array<LeafShape, STLLocalAllocator<LeafShape, 32>> mHits;
-		};
-
-		// Get bounds of both shapes
-		AABox bounds1 = inBody1.GetShape()->GetWorldSpaceBounds(inCenterOfMassTransform1, Vec3::sOne());
-		AABox bounds2 = inBody2.GetShape()->GetWorldSpaceBounds(inCenterOfMassTransform2, Vec3::sOne());
-
-		// Get leaf shapes that overlap with the bounds of the other shape
+		// A sensor will return 1 hit per leaf shape pair
 		SubShapeIDCreator part1, part2;
-		MyCollector leaf_shapes1, leaf_shapes2;
-		inBody1.GetShape()->CollectTransformedShapes(bounds2, inCenterOfMassTransform1.GetTranslation(), inCenterOfMassTransform1.GetQuaternion(), Vec3::sOne(), part1, leaf_shapes1, inShapeFilter);
-		inBody2.GetShape()->CollectTransformedShapes(bounds1, inCenterOfMassTransform2.GetTranslation(), inCenterOfMassTransform2.GetQuaternion(), Vec3::sOne(), part2, leaf_shapes2, inShapeFilter);
-
-		// Now test each leaf shape against each other leaf
-		for (const LeafShape &leaf1 : leaf_shapes1.mHits)
-			for (const LeafShape &leaf2 : leaf_shapes2.mHits)
-				if (leaf1.mBounds.Overlaps(leaf2.mBounds))
-				{
-					LeafCollector collector;
-					CollisionDispatch::sCollideShapeVsShape(leaf1.mShape, leaf2.mShape, leaf1.mScale, leaf2.mScale, leaf1.mCenterOfMassTransform, leaf2.mCenterOfMassTransform, leaf1.mSubShapeIDCreator, leaf2.mSubShapeIDCreator, ioCollideShapeSettings, collector, inShapeFilter);
-					if (collector.HadHit())
-						ioCollector.AddHit(collector.mHit);
-				}
+		CollideShapeVsShapePerLeaf<LeafCollector>(inBody1.GetShape(), inBody2.GetShape(), Vec3::sOne(), Vec3::sOne(), inCenterOfMassTransform1, inCenterOfMassTransform2, part1, part2, ioCollideShapeSettings, ioCollector, inShapeFilter);
 	}
 	else
 	{

UnitTests/Physics/SensorTests.cpp

@@ -7,7 +7,11 @@
 #include "Layers.h"
 #include "LoggingContactListener.h"
 #include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Collision/Shape/MeshShape.h>
 #include <Jolt/Physics/Collision/Shape/StaticCompoundShape.h>
+#include <Jolt/Physics/Collision/CollideShape.h>
+#include <Jolt/Physics/Collision/CollideShapeVsShapePerLeaf.h>
+#include <Jolt/Physics/Collision/CollisionCollectorImpl.h>
 
 TEST_SUITE("SensorTests")
 {
@@ -650,4 +654,111 @@ TEST_SUITE("SensorTests")
 		CHECK(listener.Contains(EType::Remove, static2.GetID(), sensor_id));
 		listener.Clear();
 	}
+
+	static void sCollideBodyVsBodyAll(const Body &inBody1, const Body &inBody2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, CollideShapeSettings &ioCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter)
+	{
+		// Override the back face mode so we get hits with all triangles
+		ioCollideShapeSettings.mBackFaceMode = EBackFaceMode::CollideWithBackFaces;
+		PhysicsSystem::sDefaultSimCollideBodyVsBody(inBody1, inBody2, inCenterOfMassTransform1, inCenterOfMassTransform2, ioCollideShapeSettings, ioCollector, inShapeFilter);
+	}
+
+	static void sCollideBodyVsBodyPerBody(const Body &inBody1, const Body &inBody2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, CollideShapeSettings &ioCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter)
+	{
+		// Max 1 hit per body pair
+		AnyHitCollisionCollector<CollideShapeCollector> collector;
+		SubShapeIDCreator part1, part2;
+		CollisionDispatch::sCollideShapeVsShape(inBody1.GetShape(), inBody2.GetShape(), Vec3::sOne(), Vec3::sOne(), inCenterOfMassTransform1, inCenterOfMassTransform2, part1, part2, ioCollideShapeSettings, collector);
+		if (collector.HadHit())
+			ioCollector.AddHit(collector.mHit);
+	}
+
+	static void sCollideBodyVsBodyPerLeaf(const Body &inBody1, const Body &inBody2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, CollideShapeSettings &ioCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter)
+	{
+		// Max 1 hit per leaf shape pair
+		SubShapeIDCreator part1, part2;
+		CollideShapeVsShapePerLeaf<AnyHitCollisionCollector<CollideShapeCollector>>(inBody1.GetShape(), inBody2.GetShape(), Vec3::sOne(), Vec3::sOne(), inCenterOfMassTransform1, inCenterOfMassTransform2, part1, part2, ioCollideShapeSettings, ioCollector, inShapeFilter);
+	}
+
+	TEST_CASE("TestSimCollideBodyVsBody")
+	{
+		// Create pyramid with flat top
+		MeshShapeSettings pyramid;
+		pyramid.mTriangleVertices = { Float3(1, 0, 1), Float3(1, 0, -1), Float3(-1, 0, -1), Float3(-1, 0, 1), Float3(0.1f, 1, 0.1f), Float3(0.1f, 1, -0.1f), Float3(-0.1f, 1, -0.1f), Float3(-0.1f, 1, 0.1f) };
+		pyramid.mIndexedTriangles = { IndexedTriangle(0, 1, 4), IndexedTriangle(4, 1, 5), IndexedTriangle(1, 2, 5), IndexedTriangle(2, 6, 5), IndexedTriangle(2, 3, 6), IndexedTriangle(3, 7, 6), IndexedTriangle(3, 0, 7), IndexedTriangle(0, 4, 7), IndexedTriangle(4, 5, 6), IndexedTriangle(4, 6, 7) };
+		pyramid.SetEmbedded();
+
+		// Create floor of many pyramids
+		StaticCompoundShapeSettings compound;
+		for (int x = -10; x <= 10; ++x)
+			for (int z = -10; z <= 10; ++z)
+				compound.AddShape(Vec3(x * 2.0f, 0, z * 2.0f), Quat::sIdentity(), &pyramid);
+		compound.SetEmbedded();
+
+		for (int type = 0; type < 3; ++type)
+		{
+			PhysicsTestContext c;
+
+			// Register listener
+			LoggingContactListener listener;
+			c.GetSystem()->SetContactListener(&listener);
+
+			// Create floor
+			BodyID floor_id = c.GetBodyInterface().CreateAndAddBody(BodyCreationSettings(&compound, RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING), EActivation::DontActivate);
+
+			// A kinematic sensor that also detects static bodies
+			// Note that the size has been picked so that it is slightly smaller than 11 x 11 pyramids and incorporates all triangles in those pyramids
+			BodyCreationSettings sensor_settings(new BoxShape(Vec3::sReplicate(10.99f)), RVec3(0, 5, 0), Quat::sIdentity(), EMotionType::Kinematic, Layers::MOVING); // Put in a layer that collides with static
+			sensor_settings.mIsSensor = true;
+			sensor_settings.mCollideKinematicVsNonDynamic = true;
+			sensor_settings.mUseManifoldReduction = false;
+			BodyID sensor_id = c.GetBodyInterface().CreateAndAddBody(sensor_settings, EActivation::Activate);
+
+			// Select the body vs body collision function
+			switch (type)
+			{
+			case 0:
+				c.GetSystem()->SetSimCollideBodyVsBody(sCollideBodyVsBodyAll);
+				break;
+
+			case 1:
+				c.GetSystem()->SetSimCollideBodyVsBody(sCollideBodyVsBodyPerLeaf);
+				break;
+
+			case 2:
+				c.GetSystem()->SetSimCollideBodyVsBody(sCollideBodyVsBodyPerBody);
+				break;
+			}
+
+			// Trigger collision callbacks
+			c.SimulateSingleStep();
+
+			// Count the number of hits that were detected
+			size_t count = 0;
+			for (size_t i = 0; i < listener.GetEntryCount(); ++i)
+			{
+				const LoggingContactListener::LogEntry &entry = listener.GetEntry(i);
+				if (entry.mType == EType::Add && entry.mBody1 == floor_id && entry.mBody2 == sensor_id)
+					++count;
+			}
+
+			// Check that we received the number of hits that we expected
+			switch (type)
+			{
+			case 0:
+				// All hits
+				CHECK(count == 11 * 11 * pyramid.mIndexedTriangles.size());
+				break;
+
+			case 1:
+				// Only 1 per sub shape
+				CHECK(count == 11 * 11);
+				break;
+
+			case 2:
+				// Only 1 per body pair
+				CHECK(count == 1);
+				break;
+			}
+		}
+	}
 }