Added ClosestHitPerBodyCollisionCollector which will report the close…

…st / deepest hit per body that the collision query collides with.

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/CollisionCollectorImpl.h

@@ -89,6 +89,91 @@ class ClosestHitCollisionCollector : public CollectorType
 	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 = GetEarlyOutFraction();
+	}
+
+	// See: CollectorType::AddHit
+	virtual void		AddHit(const ResultType &inResult) override
+	{
+		float early_out = inResult.GetEarlyOutFraction();
+		if (!mHadHit || early_out < 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
+			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/NarrowPhaseQuery.cpp

@@ -127,6 +127,7 @@ void NarrowPhaseQuery::CastRay(const RRayCast &inRay, const RayCastSettings &inR
 						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
@@ -193,6 +194,7 @@ void NarrowPhaseQuery::CollidePoint(RVec3Arg inPoint, CollidePointCollector &ioC
 						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
@@ -262,6 +264,7 @@ void NarrowPhaseQuery::CollideShape(const Shape *inShape, Vec3Arg inShapeScale,
 						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
@@ -350,6 +353,7 @@ void NarrowPhaseQuery::CastShape(const RShapeCast &inShapeCast, const ShapeCastS
 						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
@@ -415,6 +419,7 @@ void NarrowPhaseQuery::CollectTransformedShapes(const AABox &inBox, TransformedS
 						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

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 @@ class SamplesApp : public Application
 	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