Bugfix: CastShape had incorrect early out condition which could cause it to miss the deepest penetration

* When a shape cast collides at the start location it uses -penetration_depth as early out fraction to allow ordering deeper penetrations. The early out fraction was compared to the fraction of a ray cast in some places which caused it to early out when it shouldn't.
* Added unit test to check this behavior
* Added a few missing early outs that could cause hits to be reported that were larger than the early out fraction of the collector.

Jolt/Physics/Collision/BroadPhase/BroadPhaseBruteForce.cpp

@@ -230,7 +230,7 @@ void BroadPhaseBruteForce::CastAABoxNoLock(const AABoxCast &inBox, CastShapeBody
 	RayInvDirection inv_direction(inBox.mDirection);
 
 	// For all bodies
-	float early_out_fraction = ioCollector.GetEarlyOutFraction();
+	float early_out_fraction = ioCollector.GetPositiveEarlyOutFraction();
 	for (BodyID b : mBodyIDs)
 	{
 		const Body &body = mBodyManager->GetBody(b);
@@ -248,7 +248,7 @@ void BroadPhaseBruteForce::CastAABoxNoLock(const AABoxCast &inBox, CastShapeBody
 				ioCollector.AddHit(result);
 				if (ioCollector.ShouldEarlyOut())
 					break;
-				early_out_fraction = ioCollector.GetEarlyOutFraction();
+				early_out_fraction = ioCollector.GetPositiveEarlyOutFraction();
 			}
 		}
 	}

Jolt/Physics/Collision/BroadPhase/QuadTree.cpp

@@ -1312,7 +1312,7 @@ void QuadTree::CastAABox(const AABoxCast &inBox, CastShapeBodyCollector &ioColle
 		/// Returns true if this node / body should be visited, false if no hit can be generated
 		JPH_INLINE bool				ShouldVisitNode(int inStackTop) const
 		{
-			return mFractionStack[inStackTop] < mCollector.GetEarlyOutFraction();
+			return mFractionStack[inStackTop] < mCollector.GetPositiveEarlyOutFraction();
 		}
 
 		/// Visit nodes, returns number of hits found and sorts ioChildNodeIDs so that they are at the beginning of the vector.
@@ -1325,7 +1325,7 @@ void QuadTree::CastAABox(const AABoxCast &inBox, CastShapeBodyCollector &ioColle
 			Vec4 fraction = RayAABox4(mOrigin, mInvDirection, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
 
 			// Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom)
-			return SortReverseAndStore(fraction, mCollector.GetEarlyOutFraction(), ioChildNodeIDs, &mFractionStack[inStackTop]);
+			return SortReverseAndStore(fraction, mCollector.GetPositiveEarlyOutFraction(), ioChildNodeIDs, &mFractionStack[inStackTop]);
 		}
 
 		/// Visit a body, returns false if the algorithm should terminate because no hits can be generated anymore

Jolt/Physics/Collision/CastConvexVsTriangles.cpp

@@ -81,6 +81,10 @@ void CastConvexVsTriangles::Cast(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8
 		// Its a hit, store the sub shape id's
 		ShapeCastResult result(fraction, contact_point_a, contact_point_b, contact_normal_world, back_facing, mSubShapeIDCreator1.GetID(), inSubShapeID2, TransformedShape::sGetBodyID(mCollector.GetContext()));
 
+		// Early out if this hit is deeper than the collector's early out value
+		if (fraction == 0.0f && -result.mPenetrationDepth >= mCollector.GetEarlyOutFraction())
+			return;
+
 		// Gather faces
 		if (mShapeCastSettings.mCollectFacesMode == ECollectFacesMode::CollectFaces)
 		{

Jolt/Physics/Collision/CastSphereVsTriangles.cpp

@@ -142,10 +142,15 @@ void CastSphereVsTriangles::Cast(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8
 		float q_len_sq = q.LengthSq();
 		if (q_len_sq <= Square(mRadius))
 		{
-			// Yes it does, generate contacts now
+			// Early out if this hit is deeper than the collector's early out value
 			float q_len = sqrt(q_len_sq);
+			float penetration_depth = mRadius - q_len;
+			if (-penetration_depth >= mCollector.GetEarlyOutFraction())
+				return;
+
+			// Generate contact point
 			Vec3 contact_normal = q_len > 0.0f? q / q_len : Vec3::sAxisY();
-			Vec3 contact_point_a = q + contact_normal * (mRadius - q_len);
+			Vec3 contact_point_a = q + contact_normal * penetration_depth;
 			Vec3 contact_point_b = q;
 			AddHitWithActiveEdgeDetection(v0, v1, v2, back_facing, triangle_normal, inActiveEdges, inSubShapeID2, 0.0f, contact_point_a, contact_point_b, contact_normal);
 			return;

Jolt/Physics/Collision/CollisionCollector.h

@@ -47,6 +47,13 @@ public:
 	/// Declare ResultType so that derived classes can use it
 	using ResultType = ResultTypeArg;
 
+	/// Default constructor
+							CollisionCollector() = default;
+
+	/// Constructor to initialize from another collector
+	template <class ResultTypeArg2>
+	explicit				CollisionCollector(const CollisionCollector<ResultTypeArg2, TraitsType> &inRHS) : mEarlyOutFraction(inRHS.GetEarlyOutFraction()), mContext(inRHS.GetContext()) { }
+
 	/// Destructor
 	virtual					~CollisionCollector() = default;
 
@@ -80,6 +87,9 @@ public:
 	/// Get the current early out value
 	inline float			GetEarlyOutFraction() const						{ return mEarlyOutFraction; }
 
+	/// Get the current early out value but make sure it's bigger than zero, this is used for shape casting as negative values are used for penetration
+	inline float			GetPositiveEarlyOutFraction() const				{ return max(FLT_MIN, mEarlyOutFraction); }
+
 private:
 	/// The early out fraction determines the fraction below which the collector is still accepting a hit (can be used to reduce the amount of work)
 	float					mEarlyOutFraction = TraitsType::InitialEarlyOutFraction;

Jolt/Physics/Collision/CollisionDispatch.cpp

@@ -38,9 +38,9 @@ void CollisionDispatch::sReversedCollideShape(const Shape *inShape1, const Shape
 	{
 	public:
 		explicit				ReversedCollector(CollideShapeCollector &ioCollector) :
+			CollideShapeCollector(ioCollector),
 			mCollector(ioCollector)
 		{
-			SetContext(ioCollector.GetContext());
 		}
 
 		virtual void			AddHit(const CollideShapeResult &inResult) override
@@ -68,10 +68,10 @@ void CollisionDispatch::sReversedCastShape(const ShapeCast &inShapeCast, const S
 	{
 	public:
 		explicit				ReversedCollector(CastShapeCollector &ioCollector, Vec3Arg inWorldDirection) :
+			CastShapeCollector(ioCollector),
 			mCollector(ioCollector),
 			mWorldDirection(inWorldDirection)
 		{
-			SetContext(ioCollector.GetContext());
 		}
 
 		virtual void			AddHit(const ShapeCastResult &inResult) override

Jolt/Physics/Collision/NarrowPhaseQuery.cpp

@@ -87,6 +87,7 @@ void NarrowPhaseQuery::CastRay(const RRayCast &inRay, const RayCastSettings &inR
 	{
 	public:
 							MyCollector(const RRayCast &inRay, const RayCastSettings &inRayCastSettings, CastRayCollector &ioCollector, const BodyLockInterface &inBodyLockInterface, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) :
+			RayCastBodyCollector(ioCollector),
 			mRay(inRay),
 			mRayCastSettings(inRayCastSettings),
 			mCollector(ioCollector),
@@ -94,7 +95,6 @@ void NarrowPhaseQuery::CastRay(const RRayCast &inRay, const RayCastSettings &inR
 			mBodyFilter(inBodyFilter),
 			mShapeFilter(inShapeFilter)
 		{
-			UpdateEarlyOutFraction(ioCollector.GetEarlyOutFraction());
 		}
 
 		virtual void		AddHit(const ResultType &inResult) override
@@ -153,6 +153,7 @@ void NarrowPhaseQuery::CollidePoint(RVec3Arg inPoint, CollidePointCollector &ioC
 	{
 	public:
 							MyCollector(RVec3Arg inPoint, CollidePointCollector &ioCollector, const BodyLockInterface &inBodyLockInterface, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) :
+			CollideShapeBodyCollector(ioCollector),
 			mPoint(inPoint),
 			mCollector(ioCollector),
 			mBodyLockInterface(inBodyLockInterface),
@@ -214,6 +215,7 @@ void NarrowPhaseQuery::CollideShape(const Shape *inShape, Vec3Arg inShapeScale,
 	{
 	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),
@@ -285,19 +287,9 @@ void NarrowPhaseQuery::CastShape(const RShapeCast &inShapeCast, const ShapeCastS
 
 	class MyCollector : public CastShapeBodyCollector
 	{
-	private:
-			/// Update early out fraction based on narrow phase collector
-			inline void		PropagateEarlyOutFraction()
-			{
-				// The CastShapeCollector uses negative values for penetration depth so we want to clamp to the smallest positive number to keep receiving deeper hits
-				if (mCollector.ShouldEarlyOut())
-					ForceEarlyOut();
-				else
-					UpdateEarlyOutFraction(max(FLT_MIN, mCollector.GetEarlyOutFraction()));
-			}
-
 	public:
 							MyCollector(const RShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, RVec3Arg inBaseOffset, CastShapeCollector &ioCollector, const BodyLockInterface &inBodyLockInterface, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) :
+			CastShapeBodyCollector(ioCollector),
 			mShapeCast(inShapeCast),
 			mShapeCastSettings(inShapeCastSettings),
 			mBaseOffset(inBaseOffset),
@@ -306,7 +298,6 @@ void NarrowPhaseQuery::CastShape(const RShapeCast &inShapeCast, const ShapeCastS
 			mBodyFilter(inBodyFilter),
 			mShapeFilter(inShapeFilter)
 		{
-			PropagateEarlyOutFraction();
 		}
 
 		virtual void		AddHit(const ResultType &inResult) override
@@ -338,7 +329,7 @@ void NarrowPhaseQuery::CastShape(const RShapeCast &inShapeCast, const ShapeCastS
 						ts.CastShape(mShapeCast, mShapeCastSettings, mBaseOffset, mCollector, mShapeFilter);
 
 						// Update early out fraction based on narrow phase collector
-						PropagateEarlyOutFraction();
+						UpdateEarlyOutFraction(mCollector.GetEarlyOutFraction());
 					}
 				}
 			}
@@ -364,6 +355,7 @@ void NarrowPhaseQuery::CollectTransformedShapes(const AABox &inBox, TransformedS
 	{
 	public:
 							MyCollector(const AABox &inBox, TransformedShapeCollector &ioCollector, const BodyLockInterface &inBodyLockInterface, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) :
+			CollideShapeBodyCollector(ioCollector),
 			mBox(inBox),
 			mCollector(ioCollector),
 			mBodyLockInterface(inBodyLockInterface),

Jolt/Physics/Collision/Shape/ConvexShape.cpp

@@ -284,6 +284,10 @@ void ConvexShape::sCastConvexVsConvex(const ShapeCast &inShapeCast, const ShapeC
 
 		ShapeCastResult result(fraction, contact_point_a, contact_point_b, contact_normal_world, false, inSubShapeIDCreator1.GetID(), inSubShapeIDCreator2.GetID(), TransformedShape::sGetBodyID(ioCollector.GetContext()));
 
+		// Early out if this hit is deeper than the collector's early out value
+		if (fraction == 0.0f && -result.mPenetrationDepth >= ioCollector.GetEarlyOutFraction())
+			return;
+
 		// Gather faces
 		if (inShapeCastSettings.mCollectFacesMode == ECollectFacesMode::CollectFaces)
 		{

Jolt/Physics/Collision/Shape/HeightFieldShape.cpp

@@ -1559,7 +1559,7 @@ void HeightFieldShape::sCastConvexVsHeightField(const ShapeCast &inShapeCast, co
 
 		JPH_INLINE bool				ShouldVisitRangeBlock(int inStackTop) const
 		{
-			return mDistanceStack[inStackTop] < mCollector.GetEarlyOutFraction();
+			return mDistanceStack[inStackTop] < mCollector.GetPositiveEarlyOutFraction();
 		}
 
 		JPH_INLINE int				VisitRangeBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) 
@@ -1575,7 +1575,7 @@ void HeightFieldShape::sCastConvexVsHeightField(const ShapeCast &inShapeCast, co
 			Vec4 distance = RayAABox4(mBoxCenter, mInvDirection, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
 	
 			// Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom)
-			return SortReverseAndStore(distance, mCollector.GetEarlyOutFraction(), ioProperties, &mDistanceStack[inStackTop]);
+			return SortReverseAndStore(distance, mCollector.GetPositiveEarlyOutFraction(), ioProperties, &mDistanceStack[inStackTop]);
 		}
 
 		JPH_INLINE void				VisitTriangle(uint inX, uint inY, uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2)
@@ -1624,7 +1624,7 @@ void HeightFieldShape::sCastSphereVsHeightField(const ShapeCast &inShapeCast, co
 
 		JPH_INLINE bool				ShouldVisitRangeBlock(int inStackTop) const
 		{
-			return mDistanceStack[inStackTop] < mCollector.GetEarlyOutFraction();
+			return mDistanceStack[inStackTop] < mCollector.GetPositiveEarlyOutFraction();
 		}
 
 		JPH_INLINE int				VisitRangeBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) 
@@ -1640,7 +1640,7 @@ void HeightFieldShape::sCastSphereVsHeightField(const ShapeCast &inShapeCast, co
 			Vec4 distance = RayAABox4(mStart, mInvDirection, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
 	
 			// Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom)
-			return SortReverseAndStore(distance, mCollector.GetEarlyOutFraction(), ioProperties, &mDistanceStack[inStackTop]);
+			return SortReverseAndStore(distance, mCollector.GetPositiveEarlyOutFraction(), ioProperties, &mDistanceStack[inStackTop]);
 		}
 
 		JPH_INLINE void				VisitTriangle(uint inX, uint inY, uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2)

Jolt/Physics/Collision/Shape/MeshShape.cpp

@@ -824,7 +824,7 @@ void MeshShape::sCastConvexVsMesh(const ShapeCast &inShapeCast, const ShapeCastS
 
 		JPH_INLINE bool		ShouldVisitNode(int inStackTop) const
 		{
-			return mDistanceStack[inStackTop] < mCollector.GetEarlyOutFraction();
+			return mDistanceStack[inStackTop] < mCollector.GetPositiveEarlyOutFraction();
 		}
 
 		JPH_INLINE int		VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) 
@@ -840,7 +840,7 @@ void MeshShape::sCastConvexVsMesh(const ShapeCast &inShapeCast, const ShapeCastS
 			Vec4 distance = RayAABox4(mBoxCenter, mInvDirection, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
 	
 			// Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom)
-			return SortReverseAndStore(distance, mCollector.GetEarlyOutFraction(), ioProperties, &mDistanceStack[inStackTop]);
+			return SortReverseAndStore(distance, mCollector.GetPositiveEarlyOutFraction(), ioProperties, &mDistanceStack[inStackTop]);
 		}
 
 		JPH_INLINE void		VisitTriangle(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, SubShapeID inSubShapeID2) 
@@ -879,7 +879,7 @@ void MeshShape::sCastSphereVsMesh(const ShapeCast &inShapeCast, const ShapeCastS
 
 		JPH_INLINE bool		ShouldVisitNode(int inStackTop) const
 		{
-			return mDistanceStack[inStackTop] < mCollector.GetEarlyOutFraction();
+			return mDistanceStack[inStackTop] < mCollector.GetPositiveEarlyOutFraction();
 		}
 
 		JPH_INLINE int		VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) 
@@ -895,7 +895,7 @@ void MeshShape::sCastSphereVsMesh(const ShapeCast &inShapeCast, const ShapeCastS
 			Vec4 distance = RayAABox4(mStart, mInvDirection, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
 	
 			// Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom)
-			return SortReverseAndStore(distance, mCollector.GetEarlyOutFraction(), ioProperties, &mDistanceStack[inStackTop]);
+			return SortReverseAndStore(distance, mCollector.GetPositiveEarlyOutFraction(), ioProperties, &mDistanceStack[inStackTop]);
 		}
 
 		JPH_INLINE void		VisitTriangle(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, SubShapeID inSubShapeID2) 

Jolt/Physics/Collision/Shape/MutableCompoundShape.cpp

@@ -387,13 +387,13 @@ void MutableCompoundShape::sCastShapeVsCompound(const ShapeCast &inShapeCast, co
 
 		JPH_INLINE bool		ShouldVisitBlock(Vec4Arg inResult) const
 		{
-			UVec4 closer = Vec4::sLess(inResult, Vec4::sReplicate(mCollector.GetEarlyOutFraction()));
+			UVec4 closer = Vec4::sLess(inResult, Vec4::sReplicate(mCollector.GetPositiveEarlyOutFraction()));
 			return closer.TestAnyTrue();
 		}
 
 		JPH_INLINE bool		ShouldVisitSubShape(Vec4Arg inResult, uint inIndexInBlock) const
 		{
-			return inResult[inIndexInBlock] < mCollector.GetEarlyOutFraction();
+			return inResult[inIndexInBlock] < mCollector.GetPositiveEarlyOutFraction();
 		}
 	};
 

Jolt/Physics/Collision/Shape/StaticCompoundShape.cpp

@@ -504,7 +504,7 @@ void StaticCompoundShape::sCastShapeVsCompound(const ShapeCast &inShapeCast, con
 
 		JPH_INLINE bool		ShouldVisitNode(int inStackTop) const
 		{
-			return mDistanceStack[inStackTop] < mCollector.GetEarlyOutFraction();
+			return mDistanceStack[inStackTop] < mCollector.GetPositiveEarlyOutFraction();
 		}
 
 		JPH_INLINE int		VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) 
@@ -513,7 +513,7 @@ void StaticCompoundShape::sCastShapeVsCompound(const ShapeCast &inShapeCast, con
 			Vec4 distance = TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
 	
 			// Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom)
-			return SortReverseAndStore(distance, mCollector.GetEarlyOutFraction(), ioProperties, &mDistanceStack[inStackTop]);
+			return SortReverseAndStore(distance, mCollector.GetPositiveEarlyOutFraction(), ioProperties, &mDistanceStack[inStackTop]);
 		}
 
 		float				mDistanceStack[cStackSize];

Jolt/Physics/Collision/TransformedShape.cpp

@@ -120,11 +120,10 @@ void TransformedShape::CollectTransformedShapes(const AABox &inBox, TransformedS
 		struct MyCollector : public TransformedShapeCollector
 		{
 										MyCollector(TransformedShapeCollector &ioCollector, RVec3 inShapePositionCOM) :
+				TransformedShapeCollector(ioCollector),
 				mCollector(ioCollector),
 				mShapePositionCOM(inShapePositionCOM)
 			{
-				UpdateEarlyOutFraction(ioCollector.GetEarlyOutFraction());
-				SetContext(ioCollector.GetContext());
 			}
 
 			virtual void				AddHit(const TransformedShape &inResult) override
@@ -136,7 +135,8 @@ void TransformedShape::CollectTransformedShapes(const AABox &inBox, TransformedS
 				// Pass hit on to child collector
 				mCollector.AddHit(ts);
 
-				mCollector.UpdateEarlyOutFraction(GetEarlyOutFraction());
+				// Update early out fraction based on child collector
+				UpdateEarlyOutFraction(mCollector.GetEarlyOutFraction());
 			}
 
 			TransformedShapeCollector &	mCollector;

Jolt/Physics/PhysicsSystem.cpp

@@ -1788,7 +1788,7 @@ void PhysicsSystem::JobFindCCDContacts(const PhysicsUpdateContext *ioContext, Ph
 							}
 
 							// Update early out fraction
-							CastShapeCollector::UpdateEarlyOutFraction(fraction_plus_slop);
+							UpdateEarlyOutFraction(fraction_plus_slop);
 						}
 					}
 				}
@@ -1863,7 +1863,7 @@ void PhysicsSystem::JobFindCCDContacts(const PhysicsUpdateContext *ioContext, Ph
 				bounds.mMin -= mBody1Extent;
 				bounds.mMax += mBody1Extent;
 				float hit_fraction = RayAABox(Vec3(mShapeCast.mCenterOfMassStart.GetTranslation()), RayInvDirection(direction), bounds.mMin, bounds.mMax);
-				if (hit_fraction > max(FLT_MIN, GetEarlyOutFraction())) // If early out fraction <= 0, we have the possibility of finding a deeper hit so we need to clamp the early out fraction
+				if (hit_fraction > GetPositiveEarlyOutFraction()) // If early out fraction <= 0, we have the possibility of finding a deeper hit so we need to clamp the early out fraction
 					return;
 
 				// Reset collector (this is a new body pair)

UnitTests/Physics/CastShapeTests.cpp

@@ -7,6 +7,8 @@
 #include <Jolt/Physics/Collision/CastResult.h>
 #include <Jolt/Physics/Collision/CollisionCollectorImpl.h>
 #include <Jolt/Physics/Collision/Shape/SphereShape.h>
+#include <Jolt/Physics/Collision/Shape/StaticCompoundShape.h>
+#include <Jolt/Physics/Collision/Shape/CapsuleShape.h>
 #include <Jolt/Physics/Collision/Shape/TriangleShape.h>
 #include <Jolt/Physics/Collision/Shape/MeshShape.h>
 #include <Jolt/Physics/Collision/Shape/ScaledShape.h>
@@ -269,4 +271,78 @@ TEST_SUITE("CastShapeTests")
 			CHECK(!result.mIsBackFaceHit);
 		}
 	}
+
+	// Test casting a capsule against a mesh that is interecting at fraction 0 and test that it returns the deepest penetration
+	TEST_CASE("TestDeepestPenetrationAtFraction0")
+	{
+		// Create an n x n grid of triangles
+		const int n = 10;
+		const float s = 0.1f;
+		TriangleList triangles;
+		for (int z = 0; z < n; ++z)
+			for (int x = 0; x < n; ++x)
+			{
+				float fx = s * x - s * n / 2, fz = s * z - s * n / 2;
+				triangles.push_back(Triangle(Vec3(fx, 0, fz), Vec3(fx, 0, fz + s), Vec3(fx + s, 0, fz + s)));
+				triangles.push_back(Triangle(Vec3(fx, 0, fz), Vec3(fx + s, 0, fz + s), Vec3(fx + s, 0, fz)));
+			}
+		MeshShapeSettings mesh_settings(triangles);
+		mesh_settings.SetEmbedded();
+
+		// Create a compound shape with two copies of the mesh
+		StaticCompoundShapeSettings compound_settings;
+		compound_settings.AddShape(Vec3::sZero(), Quat::sIdentity(), &mesh_settings);
+		compound_settings.AddShape(Vec3(0, -0.01f, 0), Quat::sIdentity(), &mesh_settings); // This will not result in the deepest penetration
+		compound_settings.SetEmbedded();
+
+		// Add it to the scene
+		PhysicsTestContext c;
+		c.CreateBody(&compound_settings, RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, EActivation::DontActivate);
+
+		// Add the same compound a little bit lower (this will not result in the deepest penetration)
+		c.CreateBody(&compound_settings, RVec3(0, -0.1_r, 0), Quat::sIdentity(), EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, EActivation::DontActivate);
+
+		// We want the deepest hit
+		ShapeCastSettings cast_settings;
+		cast_settings.mReturnDeepestPoint = true;
+
+		// Create capsule to test
+		const float capsule_half_height = 2.0f;
+		const float capsule_radius = 1.0f;
+		RefConst<Shape> cast_shape = new CapsuleShape(capsule_half_height, capsule_radius);
+
+		// Cast the shape starting inside the mesh with a long distance so that internally in the mesh shape the RayAABox4 test will return a low negative fraction.
+		// This used to be confused with the penetration depth and would cause an early out and return the wrong result.
+		const float capsule_offset = 0.1f;
+		RShapeCast shape_cast(cast_shape, Vec3::sReplicate(1.0f), RMat44::sTranslation(RVec3(0, capsule_half_height + capsule_offset, 0)), Vec3(0, -100, 0));
+
+		// Cast first using the closest hit collector
+		ClosestHitCollisionCollector<CastShapeCollector> collector;
+		c.GetSystem()->GetNarrowPhaseQuery().CastShape(shape_cast, cast_settings, RVec3::sZero(), collector);
+
+		// Check that it indeed found a hit at fraction 0 with the deepest penetration of all triangles
+		CHECK(collector.HadHit());
+		CHECK(collector.mHit.mFraction == 0.0f);
+		CHECK_APPROX_EQUAL(collector.mHit.mPenetrationDepth, capsule_radius - capsule_offset, 1.0e-4f);
+		CHECK_APPROX_EQUAL(collector.mHit.mPenetrationAxis.Normalized(), Vec3(0, -1, 0));
+		CHECK_APPROX_EQUAL(collector.mHit.mContactPointOn2, Vec3::sZero());
+
+		// Cast again while triggering a force early out after the first hit
+		class MyCollector : public CastShapeCollector
+		{
+		public:
+			virtual void	AddHit(const ShapeCastResult &inResult) override
+			{
+				++mNumHits;
+				ForceEarlyOut();
+			}
+
+			int				mNumHits = 0;
+		};
+		MyCollector collector2;
+		c.GetSystem()->GetNarrowPhaseQuery().CastShape(shape_cast, cast_settings, RVec3::sZero(), collector2);
+
+		// Ensure that we indeed stopped after the first hit
+		CHECK(collector2.mNumHits == 1);
+	}
 }