Added ClosestHitPerBodyCollisionCollector (#1465)

* Added overridable CollisionCollector::OnBodyEnd that is called after all hits for a body have been processed when collecting hits through NarrowPhaseQuery.
* Added ClosestHitPerBodyCollisionCollector which will report the closest / deepest hit per body that the collision query collides with.

Fixes #481

Docs/ReleaseNotes.md

@@ -22,6 +22,8 @@ For breaking API changes see [this document](https://github.com/jrouwe/JoltPhysi
 * Added support for CharacterVirtual to override the inner rigid body ID. This can be used to make the simulation deterministic in e.g. client/server setups.
 * Added OnContactPersisted, OnContactRemoved, OnCharacterContactPersisted and OnCharacterContactRemoved functions on CharacterContactListener to better match the interface of ContactListener.
 * Every CharacterVirtual now has a CharacterID. This ID can be used to identify the character after removal and is used to make the simulation deterministic in case a character collides with multiple other virtual characters.
+* Added overridable CollisionCollector::OnBodyEnd that is called after all hits for a body have been processed when collecting hits through NarrowPhaseQuery.
+* Added ClosestHitPerBodyCollisionCollector which will report the closest / deepest hit per body that the collision query collides with.
 
 ### Bug fixes
 

Jolt/Physics/Collision/CollisionCollector.h

@@ -66,6 +66,9 @@ public:
 	/// before AddHit is called (e.g. the user data pointer or the velocity of the body).
 	virtual void			OnBody([[maybe_unused]] const Body &inBody)		{ /* Collects nothing by default */ }
 
+	/// When running a query through the NarrowPhaseQuery class, this will be called after all AddHit calls have been made for a particular body.
+	virtual void			OnBodyEnd()										{ /* Does nothing by default */ }
+
 	/// Set by the collision detection functions to the current TransformedShape that we're colliding against before calling the AddHit function
 	void					SetContext(const TransformedShape *inContext)	{ mContext = inContext; }
 	const TransformedShape *GetContext() const								{ return mContext; }

Jolt/Physics/Collision/CollisionCollectorImpl.h

@@ -89,6 +89,91 @@ private:
 	bool				mHadHit = false;
 };
 
+/// Implementation that collects the closest / deepest hit for each body and optionally sorts them on distance
+template <class CollectorType>
+class ClosestHitPerBodyCollisionCollector : public CollectorType
+{
+public:
+	/// Redeclare ResultType
+	using ResultType = typename CollectorType::ResultType;
+
+	// See: CollectorType::Reset
+	virtual void		Reset() override
+	{
+		CollectorType::Reset();
+
+		mHits.clear();
+		mHadHit = false;
+	}
+
+	// See: CollectorType::OnBody
+	virtual void		OnBody(const Body &inBody) override
+	{
+		// Store the early out fraction so we can restore it after we've collected all hits for this body
+		mPreviousEarlyOutFraction = CollectorType::GetEarlyOutFraction();
+	}
+
+	// See: CollectorType::AddHit
+	virtual void		AddHit(const ResultType &inResult) override
+	{
+		float early_out = inResult.GetEarlyOutFraction();
+		if (!mHadHit || early_out < CollectorType::GetEarlyOutFraction())
+		{
+			// Update early out fraction to avoid spending work on collecting further hits for this body
+			CollectorType::UpdateEarlyOutFraction(early_out);
+
+			if (!mHadHit)
+			{
+				// First time we have a hit we append it to the array
+				mHits.push_back(inResult);
+				mHadHit = true;
+			}
+			else
+			{
+				// Closer hits will override the previous one
+				mHits.back() = inResult;
+			}
+		}
+	}
+
+	// See: CollectorType::OnBodyEnd
+	virtual void		OnBodyEnd() override
+	{
+		if (mHadHit)
+		{
+			// Reset the early out fraction to the configured value so that we will continue
+			// to collect hits at any distance for other bodies
+			JPH_ASSERT(mPreviousEarlyOutFraction != -FLT_MAX); // Check that we got a call to OnBody
+			CollectorType::ResetEarlyOutFraction(mPreviousEarlyOutFraction);
+			mHadHit = false;
+		}
+
+		// For asserting purposes we reset the stored early out fraction so we can detect that OnBody was called
+		JPH_IF_ENABLE_ASSERTS(mPreviousEarlyOutFraction = -FLT_MAX;)
+	}
+
+	/// Order hits on closest first
+	void				Sort()
+	{
+		QuickSort(mHits.begin(), mHits.end(), [](const ResultType &inLHS, const ResultType &inRHS) { return inLHS.GetEarlyOutFraction() < inRHS.GetEarlyOutFraction(); });
+	}
+
+	/// Check if any hits were collected
+	inline bool			HadHit() const
+	{
+		return !mHits.empty();
+	}
+
+	Array<ResultType>	mHits;
+
+private:
+	// Store early out fraction that was initially configured for the collector
+	float				mPreviousEarlyOutFraction = -FLT_MAX;
+
+	// Flag to indicate if we have a hit for the current body
+	bool				mHadHit = false;
+};
+
 /// Simple implementation that collects any hit
 template <class CollectorType>
 class AnyHitCollisionCollector : public CollectorType

Jolt/Physics/Collision/InternalEdgeRemovingCollector.h

@@ -17,6 +17,9 @@ JPH_NAMESPACE_BEGIN
 
 /// Removes internal edges from collision results. Can be used to filter out 'ghost collisions'.
 /// Based on: Contact generation for meshes - Pierre Terdiman (https://www.codercorner.com/MeshContacts.pdf)
+///
+/// Note that this class requires that CollideSettingsBase::mActiveEdgeMode == EActiveEdgeMode::CollideWithAll
+/// and CollideSettingsBase::mCollectFacesMode == ECollectFacesMode::CollectFaces.
 class InternalEdgeRemovingCollector : public CollideShapeCollector
 {
 	static constexpr uint cMaxDelayedResults = 16;
@@ -221,6 +224,13 @@ public:
 		mDelayedResults.clear();
 	}
 
+	// See: CollideShapeCollector::OnBodyEnd
+	virtual void			OnBodyEnd() override
+	{
+		Flush();
+		mChainedCollector.OnBodyEnd();
+	}
+
 	/// Version of CollisionDispatch::sCollideShapeVsShape that removes internal edges
 	static void				sCollideShapeVsShape(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 = { })
 	{

Jolt/Physics/Collision/NarrowPhaseQuery.cpp

@@ -126,6 +126,10 @@ void NarrowPhaseQuery::CastRay(const RRayCast &inRay, const RayCastSettings &inR
 						// Do narrow phase collision check
 						ts.CastRay(mRay, mRayCastSettings, mCollector, mShapeFilter);
 
+						// Notify collector of the end of this body
+						// We do this before updating the early out fraction so that the collector can still modify it
+						mCollector.OnBodyEnd();
+
 						// Update early out fraction based on narrow phase collector
 						UpdateEarlyOutFraction(mCollector.GetEarlyOutFraction());
 					}
@@ -189,6 +193,10 @@ void NarrowPhaseQuery::CollidePoint(RVec3Arg inPoint, CollidePointCollector &ioC
 						// Do narrow phase collision check
 						ts.CollidePoint(mPoint, mCollector, mShapeFilter);
 
+						// Notify collector of the end of this body
+						// We do this before updating the early out fraction so that the collector can still modify it
+						mCollector.OnBodyEnd();
+
 						// Update early out fraction based on narrow phase collector
 						UpdateEarlyOutFraction(mCollector.GetEarlyOutFraction());
 					}
@@ -255,6 +263,10 @@ void NarrowPhaseQuery::CollideShape(const Shape *inShape, Vec3Arg inShapeScale,
 						// Do narrow phase collision check
 						ts.CollideShape(mShape, mShapeScale, mCenterOfMassTransform, mCollideShapeSettings, mBaseOffset, mCollector, mShapeFilter);
 
+						// Notify collector of the end of this body
+						// We do this before updating the early out fraction so that the collector can still modify it
+						mCollector.OnBodyEnd();
+
 						// Update early out fraction based on narrow phase collector
 						UpdateEarlyOutFraction(mCollector.GetEarlyOutFraction());
 					}
@@ -284,82 +296,13 @@ void NarrowPhaseQuery::CollideShape(const Shape *inShape, Vec3Arg inShapeScale,
 
 void NarrowPhaseQuery::CollideShapeWithInternalEdgeRemoval(const Shape *inShape, Vec3Arg inShapeScale, RMat44Arg inCenterOfMassTransform, const CollideShapeSettings &inCollideShapeSettings, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const
 {
-	JPH_PROFILE_FUNCTION();
-
-	class MyCollector : public CollideShapeBodyCollector
-	{
-	public:
-							MyCollector(const Shape *inShape, Vec3Arg inShapeScale, RMat44Arg inCenterOfMassTransform, const CollideShapeSettings &inCollideShapeSettings, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, const BodyLockInterface &inBodyLockInterface, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) :
-			CollideShapeBodyCollector(ioCollector),
-			mShape(inShape),
-			mShapeScale(inShapeScale),
-			mCenterOfMassTransform(inCenterOfMassTransform),
-			mBaseOffset(inBaseOffset),
-			mBodyLockInterface(inBodyLockInterface),
-			mBodyFilter(inBodyFilter),
-			mShapeFilter(inShapeFilter),
-			mCollideShapeSettings(inCollideShapeSettings),
-			mCollector(ioCollector)
-		{
-			// We require these settings for internal edge removal to work
-			mCollideShapeSettings.mActiveEdgeMode = EActiveEdgeMode::CollideWithAll;
-			mCollideShapeSettings.mCollectFacesMode = ECollectFacesMode::CollectFaces;
-		}
-
-		virtual void		AddHit(const ResultType &inResult) override
-		{
-			// Only test shape if it passes the body filter
-			if (mBodyFilter.ShouldCollide(inResult))
-			{
-				// Lock the body
-				BodyLockRead lock(mBodyLockInterface, inResult);
-				if (lock.SucceededAndIsInBroadPhase()) // Race condition: body could have been removed since it has been found in the broadphase, ensures body is in the broadphase while we call the callbacks
-				{
-					const Body &body = lock.GetBody();
-
-					// Check body filter again now that we've locked the body
-					if (mBodyFilter.ShouldCollideLocked(body))
-					{
-						// Collect the transformed shape
-						TransformedShape ts = body.GetTransformedShape();
+	// We require these settings for internal edge removal to work
+	CollideShapeSettings settings = inCollideShapeSettings;
+	settings.mActiveEdgeMode = EActiveEdgeMode::CollideWithAll;
+	settings.mCollectFacesMode = ECollectFacesMode::CollectFaces;
 
-						// Notify collector of new body
-						mCollector.OnBody(body);
-
-						// Release the lock now, we have all the info we need in the transformed shape
-						lock.ReleaseLock();
-
-						// Do narrow phase collision check
-						ts.CollideShape(mShape, mShapeScale, mCenterOfMassTransform, mCollideShapeSettings, mBaseOffset, mCollector, mShapeFilter);
-
-						// After each body, we need to flush the InternalEdgeRemovingCollector because it uses 'ts' as context and it will go out of scope at the end of this block
-						mCollector.Flush();
-
-						// Update early out fraction based on narrow phase collector
-						UpdateEarlyOutFraction(mCollector.GetEarlyOutFraction());
-					}
-				}
-			}
-		}
-
-		const Shape *					mShape;
-		Vec3							mShapeScale;
-		RMat44							mCenterOfMassTransform;
-		RVec3							mBaseOffset;
-		const BodyLockInterface &		mBodyLockInterface;
-		const BodyFilter &				mBodyFilter;
-		const ShapeFilter &				mShapeFilter;
-		CollideShapeSettings			mCollideShapeSettings;
-		InternalEdgeRemovingCollector	mCollector;
-	};
-
-	// Calculate bounds for shape and expand by max separation distance
-	AABox bounds = inShape->GetWorldSpaceBounds(inCenterOfMassTransform, inShapeScale);
-	bounds.ExpandBy(Vec3::sReplicate(inCollideShapeSettings.mMaxSeparationDistance));
-
-	// Do broadphase test
-	MyCollector collector(inShape, inShapeScale, inCenterOfMassTransform, inCollideShapeSettings, inBaseOffset, ioCollector, *mBodyLockInterface, inBodyFilter, inShapeFilter);
-	mBroadPhaseQuery->CollideAABox(bounds, collector, inBroadPhaseLayerFilter, inObjectLayerFilter);
+	InternalEdgeRemovingCollector wrapper(ioCollector);
+	CollideShape(inShape, inShapeScale, inCenterOfMassTransform, settings, inBaseOffset, wrapper, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter);
 }
 
 void NarrowPhaseQuery::CastShape(const RShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, RVec3Arg inBaseOffset, CastShapeCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const
@@ -409,6 +352,10 @@ void NarrowPhaseQuery::CastShape(const RShapeCast &inShapeCast, const ShapeCastS
 						// Do narrow phase collision check
 						ts.CastShape(mShapeCast, mShapeCastSettings, mBaseOffset, mCollector, mShapeFilter);
 
+						// Notify collector of the end of this body
+						// We do this before updating the early out fraction so that the collector can still modify it
+						mCollector.OnBodyEnd();
+
 						// Update early out fraction based on narrow phase collector
 						UpdateEarlyOutFraction(mCollector.GetEarlyOutFraction());
 					}
@@ -471,6 +418,10 @@ void NarrowPhaseQuery::CollectTransformedShapes(const AABox &inBox, TransformedS
 						// Do narrow phase collision check
 						ts.CollectTransformedShapes(mBox, mCollector, mShapeFilter);
 
+						// Notify collector of the end of this body
+						// We do this before updating the early out fraction so that the collector can still modify it
+						mCollector.OnBodyEnd();
+
 						// Update early out fraction based on narrow phase collector
 						UpdateEarlyOutFraction(mCollector.GetEarlyOutFraction());
 					}

Samples/SamplesApp.cpp

@@ -590,6 +590,7 @@ SamplesApp::SamplesApp(const String &inCommandLine) :
 			mDebugUI->CreateCheckBox(probe_options, "Shrunken Shape + Convex Radius", mUseShrunkenShapeAndConvexRadius, [this](UICheckBox::EState inState) { mUseShrunkenShapeAndConvexRadius = inState == UICheckBox::STATE_CHECKED; });
 			mDebugUI->CreateCheckBox(probe_options, "Draw Supporting Face", mDrawSupportingFace, [this](UICheckBox::EState inState) { mDrawSupportingFace = inState == UICheckBox::STATE_CHECKED; });
 			mDebugUI->CreateSlider(probe_options, "Max Hits", float(mMaxHits), 0, 10, 1, [this](float inValue) { mMaxHits = (int)inValue; });
+			mDebugUI->CreateCheckBox(probe_options, "Closest Hit Per Body", mClosestHitPerBody, [this](UICheckBox::EState inState) { mClosestHitPerBody = inState == UICheckBox::STATE_CHECKED; });
 			mDebugUI->ShowMenu(probe_options);
 		});
 		mDebugUI->CreateTextButton(main_menu, "Shoot Object", [this]() {
@@ -1166,6 +1167,15 @@ bool SamplesApp::CastProbe(float inProbeLength, float &outFraction, RVec3 &outPo
 				if (collector.HadHit())
 					hits.push_back(collector.mHit);
 			}
+			else if (mClosestHitPerBody)
+			{
+				ClosestHitPerBodyCollisionCollector<CastRayCollector> collector;
+				mPhysicsSystem->GetNarrowPhaseQuery().CastRay(ray, settings, collector);
+				collector.Sort();
+				hits.insert(hits.end(), collector.mHits.begin(), collector.mHits.end());
+				if ((int)hits.size() > mMaxHits)
+					hits.resize(mMaxHits);
+			}
 			else
 			{
 				AllHitCollisionCollector<CastRayCollector> collector;
@@ -1305,6 +1315,15 @@ bool SamplesApp::CastProbe(float inProbeLength, float &outFraction, RVec3 &outPo
 				if (collector.HadHit())
 					hits.push_back(collector.mHit);
 			}
+			else if (mClosestHitPerBody)
+			{
+				ClosestHitPerBodyCollisionCollector<CollideShapeCollector> collector;
+				(mPhysicsSystem->GetNarrowPhaseQuery().*collide_shape_function)(shape, Vec3::sOne(), shape_transform, settings, base_offset, collector, { }, { }, { }, { });
+				collector.Sort();
+				hits.insert(hits.end(), collector.mHits.begin(), collector.mHits.end());
+				if ((int)hits.size() > mMaxHits)
+					hits.resize(mMaxHits);
+			}
 			else
 			{
 				AllHitCollisionCollector<CollideShapeCollector> collector;
@@ -1396,6 +1415,15 @@ bool SamplesApp::CastProbe(float inProbeLength, float &outFraction, RVec3 &outPo
 				if (collector.HadHit())
 					hits.push_back(collector.mHit);
 			}
+			else if (mClosestHitPerBody)
+			{
+				ClosestHitPerBodyCollisionCollector<CastShapeCollector> collector;
+				mPhysicsSystem->GetNarrowPhaseQuery().CastShape(shape_cast, settings, base_offset, collector);
+				collector.Sort();
+				hits.insert(hits.end(), collector.mHits.begin(), collector.mHits.end());
+				if ((int)hits.size() > mMaxHits)
+					hits.resize(mMaxHits);
+			}
 			else
 			{
 				AllHitCollisionCollector<CastShapeCollector> collector;

Samples/SamplesApp.h

@@ -202,6 +202,7 @@ private:
 	bool					mUseShrunkenShapeAndConvexRadius = false;					// Shrink then expand the shape by the convex radius
 	bool					mDrawSupportingFace = false;								// Draw the result of GetSupportingFace
 	int						mMaxHits = 10;												// The maximum number of hits to request for a collision probe.
+	bool					mClosestHitPerBody = false;									// If we are only interested in the closest hit for every body
 
 	// Which object to shoot
 	enum class EShootObjectShape

UnitTests/Physics/CastShapeTests.cpp

@@ -359,4 +359,86 @@ TEST_SUITE("CastShapeTests")
 		caster.Cast(Vec3(14.5536213f, 10.5973721f, -0.00600051880f), Vec3(14.5536213f, 10.5969315f, -3.18638134f), Vec3(14.5536213f, 10.5969315f, -5.18637228f), 0b111, SubShapeID());
 		CHECK(!collector.HadHit());
 	}
+
+	// Test ClosestHitPerBodyCollisionCollector
+	TEST_CASE("TestClosestHitPerBodyCollisionCollector")
+	{
+		PhysicsTestContext c;
+
+		// Create a 1 by 1 by 1 box consisting of 10 slabs
+		StaticCompoundShapeSettings compound_settings;
+		compound_settings.SetEmbedded();
+		for (int i = 0; i < 10; ++i)
+			compound_settings.AddShape(Vec3(0.1f * i - 0.45f, 0, 0), Quat::sIdentity(), new BoxShape(Vec3(0.05f, 0.5f, 0.5f)));
+
+		// Create 2 instances
+		Body &body1 = c.CreateBody(&compound_settings, RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, EActivation::DontActivate);
+		Body &body2 = c.CreateBody(&compound_settings, RVec3(1.0_r, 0, 0), Quat::sIdentity(), EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, EActivation::DontActivate);
+
+		ShapeCastSettings cast_settings;
+
+		SphereShape sphere(0.1f);
+		sphere.SetEmbedded();
+
+		{
+			RShapeCast shape_cast(&sphere, Vec3::sOne(), RMat44::sTranslation(RVec3(-1, 0, 0)), Vec3(3, 0, 0));
+
+			// Check that the all hit collector finds 20 hits (2 x 10 slabs)
+			AllHitCollisionCollector<CastShapeCollector> all_collector;
+			c.GetSystem()->GetNarrowPhaseQuery().CastShape(shape_cast, cast_settings, RVec3::sZero(), all_collector);
+			all_collector.Sort();
+			CHECK(all_collector.mHits.size() == 20);
+			for (int i = 0; i < 10; ++i)
+			{
+				CHECK(all_collector.mHits[i].mBodyID2 == body1.GetID());
+				CHECK_APPROX_EQUAL(all_collector.mHits[i].mContactPointOn1, Vec3(-0.5f + 0.1f * i, 0, 0));
+			}
+			for (int i = 0; i < 10; ++i)
+			{
+				CHECK(all_collector.mHits[10 + i].mBodyID2 == body2.GetID());
+				CHECK_APPROX_EQUAL(all_collector.mHits[10 + i].mContactPointOn1, Vec3(0.5f + 0.1f * i, 0, 0));
+			}
+
+			// Check that the closest hit per body collector only finds 2
+			ClosestHitPerBodyCollisionCollector<CastShapeCollector> closest_collector;
+			c.GetSystem()->GetNarrowPhaseQuery().CastShape(shape_cast, cast_settings, RVec3::sZero(), closest_collector);
+			closest_collector.Sort();
+			CHECK(closest_collector.mHits.size() == 2);
+			CHECK(closest_collector.mHits[0].mBodyID2 == body1.GetID());
+			CHECK_APPROX_EQUAL(closest_collector.mHits[0].mContactPointOn1, Vec3(-0.5f, 0, 0));
+			CHECK(closest_collector.mHits[1].mBodyID2 == body2.GetID());
+			CHECK_APPROX_EQUAL(closest_collector.mHits[1].mContactPointOn1, Vec3(0.5f, 0, 0));
+		}
+
+		{
+			// Cast in reverse direction
+			RShapeCast shape_cast(&sphere, Vec3::sOne(), RMat44::sTranslation(RVec3(2, 0, 0)), Vec3(-3, 0, 0));
+
+			// Check that the all hit collector finds 20 hits (2 x 10 slabs)
+			AllHitCollisionCollector<CastShapeCollector> all_collector;
+			c.GetSystem()->GetNarrowPhaseQuery().CastShape(shape_cast, cast_settings, RVec3::sZero(), all_collector);
+			all_collector.Sort();
+			CHECK(all_collector.mHits.size() == 20);
+			for (int i = 0; i < 10; ++i)
+			{
+				CHECK(all_collector.mHits[i].mBodyID2 == body2.GetID());
+				CHECK_APPROX_EQUAL(all_collector.mHits[i].mContactPointOn1, Vec3(1.5f - 0.1f * i, 0, 0));
+			}
+			for (int i = 0; i < 10; ++i)
+			{
+				CHECK(all_collector.mHits[10 + i].mBodyID2 == body1.GetID());
+				CHECK_APPROX_EQUAL(all_collector.mHits[10 + i].mContactPointOn1, Vec3(0.5f - 0.1f * i, 0, 0));
+			}
+
+			// Check that the closest hit per body collector only finds 2
+			ClosestHitPerBodyCollisionCollector<CastShapeCollector> closest_collector;
+			c.GetSystem()->GetNarrowPhaseQuery().CastShape(shape_cast, cast_settings, RVec3::sZero(), closest_collector);
+			closest_collector.Sort();
+			CHECK(closest_collector.mHits.size() == 2);
+			CHECK(closest_collector.mHits[0].mBodyID2 == body2.GetID());
+			CHECK_APPROX_EQUAL(closest_collector.mHits[0].mContactPointOn1, Vec3(1.5f, 0, 0));
+			CHECK(closest_collector.mHits[1].mBodyID2 == body1.GetID());
+			CHECK_APPROX_EQUAL(closest_collector.mHits[1].mContactPointOn1, Vec3(0.5f, 0, 0));
+		}
+	}
 }