Added functionality to track narrow phase timing stats (#18)

* Added functionality to track narrow phase timing stats
* Removed virtual CastShape function, this has to be done through the collision dispatch now (gets rid of 1 virtual call + makes it possible to create specializations for e.g. capsule vs capsule)

Jolt/Jolt.cmake

@@ -203,6 +203,8 @@ set(JOLT_PHYSICS_SRC_FILES
 	${JOLT_PHYSICS_ROOT}/Physics/Collision/ManifoldBetweenTwoFaces.h
 	${JOLT_PHYSICS_ROOT}/Physics/Collision/NarrowPhaseQuery.cpp
 	${JOLT_PHYSICS_ROOT}/Physics/Collision/NarrowPhaseQuery.h
+	${JOLT_PHYSICS_ROOT}/Physics/Collision/NarrowPhaseStats.cpp
+	${JOLT_PHYSICS_ROOT}/Physics/Collision/NarrowPhaseStats.h
 	${JOLT_PHYSICS_ROOT}/Physics/Collision/ObjectLayer.h
 	${JOLT_PHYSICS_ROOT}/Physics/Collision/PhysicsMaterial.cpp
 	${JOLT_PHYSICS_ROOT}/Physics/Collision/PhysicsMaterial.h

Jolt/Physics/Collision/BroadPhase/QuadTree.h

@@ -312,7 +312,6 @@ private:
 	void						ValidateTree(const BodyVector &inBodies, const TrackingVector &inTracking, uint32 inNodeIndex, uint32 inNumExpectedBodies) const;
 #endif
 
-
 #ifdef JPH_TRACK_BROADPHASE_STATS
 	/// Mutex protecting the various LayerToStats members
 	mutable Mutex				mStatsMutex;

Jolt/Physics/Collision/CastConvexVsTriangles.cpp

@@ -7,6 +7,7 @@
 #include <Physics/Collision/TransformedShape.h>
 #include <Physics/Collision/Shape/ScaleHelpers.h>
 #include <Physics/Collision/ActiveEdges.h>
+#include <Physics/Collision/NarrowPhaseStats.h>
 #include <Geometry/EPAPenetrationDepth.h>
 
 namespace JPH {
@@ -105,6 +106,7 @@ void CastConvexVsTriangles::Cast(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8
 				p = mCenterOfMassTransform2 * p;
 		}
 
+		JPH_IF_TRACK_NARROWPHASE_STATS(TrackNarrowPhaseCollector track;)
 		mCollector.AddHit(result);
 	}
 }

Jolt/Physics/Collision/CollideConvexVsTriangles.cpp

@@ -8,6 +8,7 @@
 #include <Physics/Collision/CollideShape.h>
 #include <Physics/Collision/TransformedShape.h>
 #include <Physics/Collision/ActiveEdges.h>
+#include <Physics/Collision/NarrowPhaseStats.h>
 #include <Core/StatCollector.h>
 #include <Geometry/EPAPenetrationDepth.h>
 #include <Geometry/Plane.h>
@@ -158,6 +159,7 @@ void CollideConvexVsTriangles::Collide(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2,
 	JPH_IF_STAT_COLLECTOR(sNumCollisions++;)
 
 	// Notify the collector
+	JPH_IF_TRACK_NARROWPHASE_STATS(TrackNarrowPhaseCollector track;)
 	mCollector.AddHit(result);
 }
 

Jolt/Physics/Collision/CollisionDispatch.cpp

@@ -8,6 +8,6 @@
 namespace JPH {
 
 CollisionDispatch::CollideShape CollisionDispatch::sCollideShape[NumSubShapeTypes][NumSubShapeTypes];
-CollisionDispatch::CastShape CollisionDispatch::sCastShape[NumSubShapeTypes];
+CollisionDispatch::CastShape CollisionDispatch::sCastShape[NumSubShapeTypes][NumSubShapeTypes];
 
 } // JPH

Jolt/Physics/Collision/CollisionDispatch.h

@@ -7,6 +7,7 @@
 #include <Physics/Collision/Shape/SubShapeID.h>
 #include <Physics/Collision/ShapeCast.h>
 #include <Physics/Collision/ShapeFilter.h>
+#include <Physics/Collision/NarrowPhaseStats.h>
 
 namespace JPH {
 
@@ -29,24 +30,29 @@ public:
 	/// @param ioCollector The collector that receives the results.
 	static inline 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)
 	{
+		JPH_IF_TRACK_NARROWPHASE_STATS(TrackNarrowPhaseStat track(NarrowPhaseStat::sCollideShape[(int)inShape1->GetSubType()][(int)inShape2->GetSubType()]);)
+
 		sCollideShape[(int)inShape1->GetSubType()][(int)inShape2->GetSubType()](inShape1, inShape2, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector);
 	}
 
 	/// Cast a shape againt this shape, passes any hits found to ioCollector.
+	/// Note that the shape cast should be relative to the center of mass of this shape (i.e. inShapeCast.mCenterOfMassStart = Start * Mat44::sTranslation(mShape->GetCenterOfMass()) if you want to cast the shape in the space it was created).
 	/// @param inShapeCast The shape to cast against the other shape and its start and direction
 	/// @param inShapeCastSettings Settings for performing the cast
 	/// @param inShape The shape to cast against.
 	/// @param inScale Local space scale for the shape to cast against.
-	/// @param inShapeFilter Determines if sub shapes of the shape can collide
+	/// @param inShapeFilter allows selectively disabling collisions between pairs of (sub) shapes.
 	/// @param inCenterOfMassTransform2 Is the center of mass transform of shape 2 (excluding scale), this is used to provide a transform to the shape cast result so that local quantities can be transformed into world space.
 	/// @param inSubShapeIDCreator1 Class that tracks the current sub shape ID for the casting shape
 	/// @param inSubShapeIDCreator2 Class that tracks the current sub shape ID for the shape we're casting against
 	/// @param ioCollector The collector that receives the results.
 	static inline void		sCastShapeVsShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
 	{
+		JPH_IF_TRACK_NARROWPHASE_STATS(TrackNarrowPhaseStat track(NarrowPhaseStat::sCastShape[(int)inShapeCast.mShape->GetSubType()][(int)inShape->GetSubType()]);)
+
 		// Only test shape if it passes the shape filter
 		if (inShapeFilter.ShouldCollide(inSubShapeIDCreator1.GetID(), inSubShapeIDCreator2.GetID()))
-			sCastShape[(int)inShapeCast.mShape->GetSubType()](inShapeCast, inShapeCastSettings, inShape, inScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector);
+			sCastShape[(int)inShapeCast.mShape->GetSubType()][(int)inShape->GetSubType()](inShapeCast, inShapeCastSettings, inShape, inScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector);
 	}
 
 	/// Function that collides 2 shapes (see sCollideShapeVsShape) 
@@ -59,11 +65,11 @@ public:
 	static void				sRegisterCollideShape(EShapeSubType inType1, EShapeSubType inType2, CollideShape inFunction)	{ sCollideShape[(int)inType1][(int)inType2] = inFunction; }
 
 	/// Register a cast shape function in the collision table
-	static void				sRegisterCastShape(EShapeSubType inType1, CastShape inFunction)									{ sCastShape[(int)inType1] = inFunction; }
+	static void				sRegisterCastShape(EShapeSubType inType1, EShapeSubType inType2, CastShape inFunction)			{ sCastShape[(int)inType1][(int)inType2] = inFunction; }
 
 private:
 	static CollideShape		sCollideShape[NumSubShapeTypes][NumSubShapeTypes];
-	static CastShape		sCastShape[NumSubShapeTypes];
+	static CastShape		sCastShape[NumSubShapeTypes][NumSubShapeTypes];
 };
 
 } // JPH

Jolt/Physics/Collision/NarrowPhaseStats.cpp

@@ -0,0 +1,52 @@
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt.h>
+
+#include <Physics/Collision/NarrowPhaseStats.h>
+
+#ifdef JPH_TRACK_NARROWPHASE_STATS
+
+namespace JPH {
+
+NarrowPhaseStat	NarrowPhaseStat::sCollideShape[NumSubShapeTypes][NumSubShapeTypes];
+NarrowPhaseStat	NarrowPhaseStat::sCastShape[NumSubShapeTypes][NumSubShapeTypes];
+
+thread_local TrackNarrowPhaseStat *TrackNarrowPhaseStat::sRoot = nullptr;
+
+void NarrowPhaseStat::ReportStats(const char *inName, EShapeSubType inType1, EShapeSubType inType2) const
+{
+	uint64 ticks_per_sec = GetProcessorTicksPerSecond();
+
+	double total_time = 1000.0 * double(mTotalTicks) / double(ticks_per_sec);
+	double total_time_excl_children = 1000.0 * double(mTotalTicks - mChildTicks) / double(ticks_per_sec);
+
+	stringstream str;
+	str << inName << ", " << sSubShapeTypeNames[(int)inType1] << ", " << sSubShapeTypeNames[(int)inType2] << ", " << mNumQueries << ", " << total_time << ", " << total_time_excl_children << ", " << total_time_excl_children / mNumQueries << ", " << mHitsReported;
+	Trace(str.str().c_str());
+}
+
+void NarrowPhaseStat::sReportStats()
+{
+	Trace("Query Type, Shape Type 1, Shape Type 2, Num Queries, Total Time (ms), Total Time Excl Children (ms), Total Time Excl. Children / Query (ms), Hits Reported");
+
+	for (EShapeSubType t1 : sAllSubShapeTypes)
+		for (EShapeSubType t2 : sAllSubShapeTypes)
+		{
+			const NarrowPhaseStat &stat = sCollideShape[(int)t1][(int)t2];
+			if (stat.mNumQueries > 0)
+				stat.ReportStats("CollideShape", t1, t2);
+		}
+
+	for (EShapeSubType t1 : sAllSubShapeTypes)
+		for (EShapeSubType t2 : sAllSubShapeTypes)
+		{
+			const NarrowPhaseStat &stat = sCastShape[(int)t1][(int)t2];
+			if (stat.mNumQueries > 0)
+				stat.ReportStats("CastShape", t1, t2);
+		}
+}
+
+} // JPH
+
+#endif // JPH_TRACK_NARROWPHASE_STATS

Jolt/Physics/Collision/NarrowPhaseStats.h

@@ -0,0 +1,103 @@
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Core/TickCounter.h>
+#include <Physics/Collision/Shape/Shape.h>
+
+// Shorthand function to ifdef out code if narrow phase stats tracking is off
+#ifdef JPH_TRACK_NARROWPHASE_STATS
+	#define JPH_IF_TRACK_NARROWPHASE_STATS(...) __VA_ARGS__
+#else
+	#define JPH_IF_TRACK_NARROWPHASE_STATS(...)
+#endif // JPH_TRACK_NARROWPHASE_STATS
+
+#ifdef JPH_TRACK_NARROWPHASE_STATS
+
+namespace JPH {
+
+/// Structure that tracks narrow phase timing information for a particular combination of shapes
+class NarrowPhaseStat
+{
+public:
+	/// Trace an individual stat in CSV form.
+	void					ReportStats(const char *inName, EShapeSubType inType1, EShapeSubType inType2) const;
+
+	/// Trace the collected broadphase stats in CSV form.
+	/// This report can be used to judge and tweak the efficiency of the broadphase.
+	static void				sReportStats();
+
+	atomic<uint64>			mNumQueries = 0;
+	atomic<uint64>			mHitsReported = 0;
+	atomic<uint64>			mTotalTicks = 0;
+	atomic<uint64>			mChildTicks = 0;
+
+	static NarrowPhaseStat	sCollideShape[NumSubShapeTypes][NumSubShapeTypes];
+	static NarrowPhaseStat	sCastShape[NumSubShapeTypes][NumSubShapeTypes];
+};
+
+/// Object that tracks the start and end of a narrow phase operation
+class TrackNarrowPhaseStat
+{
+public:
+							TrackNarrowPhaseStat(NarrowPhaseStat &inStat) : 
+		mStat(inStat),
+		mParent(sRoot),
+		mStart(GetProcessorTickCount())
+	{
+		// Make this the new root of the chain
+		sRoot = this;
+	}
+
+							~TrackNarrowPhaseStat()
+	{
+		uint64 delta_ticks = GetProcessorTickCount() - mStart;
+
+		// Notify parent of time spent in child
+		if (mParent != nullptr)
+			mParent->mStat.mChildTicks += delta_ticks;
+
+		// Increment stats at this level
+		mStat.mNumQueries++;
+		mStat.mTotalTicks += delta_ticks;
+
+		// Restore root pointer
+		JPH_ASSERT(sRoot == this);
+		sRoot = mParent;
+	}
+
+	NarrowPhaseStat &		mStat;
+	TrackNarrowPhaseStat *	mParent;
+	uint64					mStart;
+
+	static thread_local TrackNarrowPhaseStat *sRoot;
+};
+
+/// Object that tracks the start and end of a hit being processed by a collision collector
+class TrackNarrowPhaseCollector
+{
+public:
+							TrackNarrowPhaseCollector() : 
+		mStart(GetProcessorTickCount())
+	{
+	}
+
+							~TrackNarrowPhaseCollector()
+	{
+		// Mark time spent in collector as 'child' time for the parent
+		uint64 delta_ticks = GetProcessorTickCount() - mStart;
+		TrackNarrowPhaseStat::sRoot->mStat.mChildTicks += delta_ticks;
+
+		// Notify all parents of a hit
+		for (TrackNarrowPhaseStat *track = TrackNarrowPhaseStat::sRoot; track != nullptr; track = track->mParent)
+			track->mStat.mHitsReported++;
+	}
+
+private:
+	uint64					mStart;
+};
+
+} // JPH
+
+#endif // JPH_TRACK_NARROWPHASE_STATS

Jolt/Physics/Collision/Shape/CompoundShape.cpp

@@ -377,8 +377,9 @@ bool CompoundShape::IsValidScale(Vec3Arg inScale) const
 
 void CompoundShape::sRegister()
 {
-	for (EShapeSubType s : sCompoundSubShapeTypes)
-		CollisionDispatch::sRegisterCastShape(s, sCastCompoundVsShape);
+	for (EShapeSubType s1 : sCompoundSubShapeTypes)
+		for (EShapeSubType s2 : sAllSubShapeTypes)
+			CollisionDispatch::sRegisterCastShape(s1, s2, sCastCompoundVsShape);
 }
 
 } // JPH

Jolt/Physics/Collision/Shape/CompoundShapeVisitors.h

@@ -210,7 +210,7 @@ struct CompoundShape::CastShapeVisitor
 		// Transform the shape cast
 		ShapeCast shape_cast = mShapeCast.PostTransformed(local_transform.InversedRotationTranslation());
 
-		inSubShape.mShape->CastShape(shape_cast, mShapeCastSettings, inSubShape.TransformScale(mScale), mShapeFilter, center_of_mass_transform2, mSubShapeIDCreator1, shape2_sub_shape_id, mCollector);
+		CollisionDispatch::sCastShapeVsShape(shape_cast, mShapeCastSettings, inSubShape.mShape, inSubShape.TransformScale(mScale), mShapeFilter, center_of_mass_transform2, mSubShapeIDCreator1, shape2_sub_shape_id, mCollector);
 	}
 
 	RayInvDirection				mInvDirection;

Jolt/Physics/Collision/Shape/ConvexShape.cpp

@@ -14,6 +14,7 @@
 #include <Physics/Collision/Shape/PolyhedronSubmergedVolumeCalculator.h>
 #include <Physics/Collision/TransformedShape.h>
 #include <Physics/Collision/CollisionDispatch.h>
+#include <Physics/Collision/NarrowPhaseStats.h>
 #include <Physics/PhysicsSettings.h>
 #include <Core/StatCollector.h>
 #include <Core/StreamIn.h>
@@ -164,14 +165,10 @@ void ConvexShape::sCollideConvexVsConvex(const Shape *inShape1, const Shape *inS
 	JPH_IF_STAT_COLLECTOR(sNumCollisions++;)
 
 	// Notify the collector
+	JPH_IF_TRACK_NARROWPHASE_STATS(TrackNarrowPhaseCollector track;)
 	ioCollector.AddHit(result);
 }
 
-void ConvexShape::sCastConvexVsShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
-{
-	inShape->CastShape(inShapeCast, inShapeCastSettings, inScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector);
-}
-
 bool ConvexShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const
 {
 	// Note: This is a fallback routine, most convex shapes should implement a more performant version!
@@ -255,7 +252,7 @@ void ConvexShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubSh
 	}
 }
 
-void ConvexShape::CastShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) const 
+void ConvexShape::sCastConvexVsConvex(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, [[maybe_unused]] const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
 {
 	JPH_PROFILE_FUNCTION();
 
@@ -263,6 +260,9 @@ void ConvexShape::CastShape(const ShapeCast &inShapeCast, const ShapeCastSetting
 	JPH_ASSERT(inShapeCast.mShape->GetType() == EShapeType::Convex); 
 	const ConvexShape *cast_shape = static_cast<const ConvexShape *>(inShapeCast.mShape);
 
+	JPH_ASSERT(inShape->GetType() == EShapeType::Convex); 
+	const ConvexShape *shape = static_cast<const ConvexShape *>(inShape);
+
 	// Determine if we want to use the actual shape or a shrunken shape with convex radius
 	ConvexShape::ESupportMode support_mode = inShapeCastSettings.mUseShrunkenShapeAndConvexRadius? ConvexShape::ESupportMode::ExcludeConvexRadius : ConvexShape::ESupportMode::IncludeConvexRadius;
 
@@ -272,7 +272,7 @@ void ConvexShape::CastShape(const ShapeCast &inShapeCast, const ShapeCastSetting
 
 	// Create support function for target shape
 	SupportBuffer target_buffer;
-	const Support *target_support = GetSupportFunction(support_mode, target_buffer, inScale);
+	const Support *target_support = shape->GetSupportFunction(support_mode, target_buffer, inScale);
 
 	// Do a raycast against the result
 	EPAPenetrationDepth epa;
@@ -305,13 +305,14 @@ void ConvexShape::CastShape(const ShapeCast &inShapeCast, const ShapeCastSetting
 					p = transform_1_to_world * p;
 
 				// Get supporting face of shape 2
-				GetSupportingFace(contact_normal, inScale, result.mShape2Face);
+				shape->GetSupportingFace(contact_normal, inScale, result.mShape2Face);
 
 				// Convert to world space
 				for (Vec3 &p : result.mShape2Face)
 					p = inCenterOfMassTransform2 * p;
 			}
 
+			JPH_IF_TRACK_NARROWPHASE_STATS(TrackNarrowPhaseCollector track;)
 			ioCollector.AddHit(result);
 		}
 	}
@@ -593,12 +594,11 @@ void ConvexShape::RestoreMaterialState(const PhysicsMaterialRefC *inMaterials, u
 void ConvexShape::sRegister()
 {
 	for (EShapeSubType s1 : sConvexSubShapeTypes)
-	{
 		for (EShapeSubType s2 : sConvexSubShapeTypes)
+		{
 			CollisionDispatch::sRegisterCollideShape(s1, s2, sCollideConvexVsConvex);
-
-		CollisionDispatch::sRegisterCastShape(s1, sCastConvexVsShape);
-	}
+			CollisionDispatch::sRegisterCastShape(s1, s2, sCastConvexVsConvex);
+		}
 }
 
 } // JPH

Jolt/Physics/Collision/Shape/ConvexShape.h

@@ -56,9 +56,6 @@ public:
 	// See: Shape::CollidePoint
 	virtual void					CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector) const override;
 
-	// See Shape::CastShape
-	virtual void					CastShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) const override;
-
 	// See Shape::GetTrianglesStart
 	virtual void					GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override;
 
@@ -158,7 +155,7 @@ private:
 
 	// Helper functions called by CollisionDispatch
 	static void						sCollideConvexVsConvex(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);
-	static void						sCastConvexVsShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector);
+	static void						sCastConvexVsConvex(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector);
 
 #ifdef JPH_STAT_COLLECTOR
 	// Statistics

Jolt/Physics/Collision/Shape/HeightFieldShape.cpp

@@ -1442,7 +1442,7 @@ void HeightFieldShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &in
 	}
 }
 
-void HeightFieldShape::CastShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) const 
+void HeightFieldShape::sCastConvexVsHeightField(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
 {
 	JPH_PROFILE_FUNCTION();
 
@@ -1506,13 +1506,16 @@ void HeightFieldShape::CastShape(const ShapeCast &inShapeCast, const ShapeCastSe
 		float						mDistanceStack[cStackSize];
 	};
 
+	JPH_ASSERT(inShape->GetSubType() == EShapeSubType::HeightField);
+	const HeightFieldShape *shape = static_cast<const HeightFieldShape *>(inShape);
+
 	Visitor visitor(inShapeCast, inShapeCastSettings, inScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, ioCollector);
-	visitor.mShape2 = this;
+	visitor.mShape2 = shape;
 	visitor.mInvDirection.Set(inShapeCast.mDirection);
 	visitor.mBoxCenter = inShapeCast.mShapeWorldBounds.GetCenter();
 	visitor.mBoxExtent = inShapeCast.mShapeWorldBounds.GetExtent();
 	visitor.mSubShapeIDCreator2 = inSubShapeIDCreator2;
-	WalkHeightField(visitor);
+	shape->WalkHeightField(visitor);
 }
 
 struct HeightFieldShape::HSGetTrianglesContext
@@ -1757,7 +1760,10 @@ void HeightFieldShape::sRegister()
 	f.mColor = Color::sPurple;
 
 	for (EShapeSubType s : sConvexSubShapeTypes)
+	{
 		CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::HeightField, sCollideConvexVsHeightField);
+		CollisionDispatch::sRegisterCastShape(s, EShapeSubType::HeightField, sCastConvexVsHeightField);
+	}
 }
 
 } // JPH

Jolt/Physics/Collision/Shape/HeightFieldShape.h

@@ -134,9 +134,6 @@ public:
 	// See: Shape::CollidePoint
 	virtual void					CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector) const override;
 
-	// See Shape::CastShape
-	virtual void					CastShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) const override;
-
 	// See Shape::GetTrianglesStart
 	virtual void					GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override;
 
@@ -220,6 +217,7 @@ private:
 
 	// Helper functions called by CollisionDispatch
 	static void						sCollideConvexVsHeightField(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);
+	static void						sCastConvexVsHeightField(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector);
 
 	/// Visit the entire height field using a visitor pattern
 	template <class Visitor>

Jolt/Physics/Collision/Shape/MeshShape.cpp

@@ -732,7 +732,7 @@ void MeshShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShap
 	}
 }
 
-void MeshShape::CastShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) const 
+void MeshShape::sCastConvexVsMesh(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
 {
 	JPH_PROFILE_FUNCTION();
 
@@ -816,13 +816,16 @@ void MeshShape::CastShape(const ShapeCast &inShapeCast, const ShapeCastSettings
 		float						mDistanceStack[NodeCodec::StackSize];
 	};
 
+	JPH_ASSERT(inShape->GetSubType() == EShapeSubType::Mesh);
+	const MeshShape *shape = static_cast<const MeshShape *>(inShape);
+
 	Visitor visitor(inShapeCast, inShapeCastSettings, inScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, ioCollector);
 	visitor.mInvDirection.Set(inShapeCast.mDirection);
 	visitor.mBoxCenter = inShapeCast.mShapeWorldBounds.GetCenter();
 	visitor.mBoxExtent = inShapeCast.mShapeWorldBounds.GetExtent();
 	visitor.mSubShapeIDCreator2 = inSubShapeIDCreator2;
-	visitor.mTriangleBlockIDBits = NodeCodec::DecodingContext::sTriangleBlockIDBits(mTree);
-	WalkTree(visitor);
+	visitor.mTriangleBlockIDBits = NodeCodec::DecodingContext::sTriangleBlockIDBits(shape->mTree);
+	shape->WalkTree(visitor);
 }
 
 struct MeshShape::MSGetTrianglesContext
@@ -1117,7 +1120,10 @@ void MeshShape::sRegister()
 	f.mColor = Color::sRed;
 
 	for (EShapeSubType s : sConvexSubShapeTypes)
+	{
 		CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::Mesh, sCollideConvexVsMesh);
+		CollisionDispatch::sRegisterCastShape(s, EShapeSubType::Mesh, sCastConvexVsMesh);
+	}
 }
 
 } // JPH

Jolt/Physics/Collision/Shape/MeshShape.h

@@ -85,9 +85,6 @@ public:
 	// See: Shape::CollidePoint
 	virtual void					CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector) const override;
 
-	// See Shape::CastShape
-	virtual void					CastShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) const override;
-
 	// See Shape::GetTrianglesStart
 	virtual void					GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override;
 
@@ -139,6 +136,7 @@ private:
 
 	// Helper functions called by CollisionDispatch
 	static void						sCollideConvexVsMesh(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);
+	static void						sCastConvexVsMesh(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector);
 
 	/// Materials assigned to the triangles. Each triangle specifies which material it uses through its mMaterialIndex
 	PhysicsMaterialList				mMaterials;

Jolt/Physics/Collision/Shape/MutableCompoundShape.cpp

@@ -381,7 +381,7 @@ void MutableCompoundShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator
 	WalkSubShapes(visitor);
 }
 
-void MutableCompoundShape::CastShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) const 
+void MutableCompoundShape::sCastShapeVsCompound(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
 {
 	JPH_PROFILE_FUNCTION();
 
@@ -408,8 +408,11 @@ void MutableCompoundShape::CastShape(const ShapeCast &inShapeCast, const ShapeCa
 		}
 	};
 
-	Visitor visitor(inShapeCast, inShapeCastSettings, this, inScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector);
-	WalkSubShapes(visitor);
+	JPH_ASSERT(inShape->GetSubType() == EShapeSubType::MutableCompound);
+	const MutableCompoundShape *shape = static_cast<const MutableCompoundShape *>(inShape);
+
+	Visitor visitor(inShapeCast, inShapeCastSettings, shape, inScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector);
+	shape->WalkSubShapes(visitor);
 }
 
 void MutableCompoundShape::CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector) const
@@ -559,6 +562,7 @@ void MutableCompoundShape::sRegister()
 	{
 		CollisionDispatch::sRegisterCollideShape(EShapeSubType::MutableCompound, s, sCollideCompoundVsShape);
 		CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::MutableCompound, sCollideShapeVsCompound);
+		CollisionDispatch::sRegisterCastShape(s, EShapeSubType::MutableCompound, sCastShapeVsCompound);
 	}
 }
 

Jolt/Physics/Collision/Shape/MutableCompoundShape.h

@@ -41,9 +41,6 @@ public:
 	// See: Shape::CollidePoint
 	virtual void					CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector) const override;
 
-	// See Shape::CastShape
-	virtual void					CastShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) const override;
-
 	// See Shape::CollectTransformedShapes
 	virtual void					CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector) const override;
 
@@ -142,6 +139,7 @@ private:
 	// Helper functions called by CollisionDispatch
 	static void						sCollideCompoundVsShape(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);
 	static void						sCollideShapeVsCompound(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);
+	static void						sCastShapeVsCompound(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector);
 
 	Vec4 *							mSubShapeBounds[6];											///< Bounding boxes of all sub shapes in SOA format, 0 = MinX, 1 = MinY, 2 = MinZ, 3 = MaxX, 4 = MaxY, 5 = MaxZ
 	uint							mSubShapeBoundsCapacity = 0;								///< Number of bounding boxes that can be stored in the mSubShapeBounds array (will be multiple of 4)

Jolt/Physics/Collision/Shape/OffsetCenterOfMassShape.cpp

@@ -113,14 +113,6 @@ void OffsetCenterOfMassShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCrea
 	mInnerShape->CollidePoint(inPoint + mOffset, inSubShapeIDCreator, ioCollector);
 }
 
-void OffsetCenterOfMassShape::CastShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) const 
-{
-	// Transform the shape cast
-	ShapeCast shape_cast = inShapeCast.PostTransformed(Mat44::sTranslation(mOffset));
-
-	mInnerShape->CastShape(shape_cast, inShapeCastSettings, inScale, inShapeFilter, inCenterOfMassTransform2 * Mat44::sTranslation(-mOffset), inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector);
-}
-
 void OffsetCenterOfMassShape::CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector) const
 {
 	mInnerShape->CollectTransformedShapes(inBox, inPositionCOM - inRotation * mOffset, inRotation, inScale, inSubShapeIDCreator, ioCollector);
@@ -159,6 +151,17 @@ void OffsetCenterOfMassShape::sCastOffsetCenterOfMassVsShape(const ShapeCast &in
 	CollisionDispatch::sCastShapeVsShape(shape_cast, inShapeCastSettings, inShape, inScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector);
 }
 
+void OffsetCenterOfMassShape::sCastShapeVsOffsetCenterOfMass(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
+{
+	JPH_ASSERT(inShape->GetSubType() == EShapeSubType::OffsetCenterOfMass);
+	const OffsetCenterOfMassShape *shape = static_cast<const OffsetCenterOfMassShape *>(inShape);
+
+	// Transform the shape cast
+	ShapeCast shape_cast = inShapeCast.PostTransformed(Mat44::sTranslation(shape->mOffset));
+
+	CollisionDispatch::sCastShapeVsShape(shape_cast, inShapeCastSettings, shape->mInnerShape, inScale, inShapeFilter, inCenterOfMassTransform2 * Mat44::sTranslation(-shape->mOffset), inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector);
+}
+
 void OffsetCenterOfMassShape::SaveBinaryState(StreamOut &inStream) const
 {
 	DecoratedShape::SaveBinaryState(inStream);
@@ -183,9 +186,9 @@ void OffsetCenterOfMassShape::sRegister()
 	{
 		CollisionDispatch::sRegisterCollideShape(EShapeSubType::OffsetCenterOfMass, s, sCollideOffsetCenterOfMassVsShape);
 		CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::OffsetCenterOfMass, sCollideShapeVsOffsetCenterOfMass);
+		CollisionDispatch::sRegisterCastShape(EShapeSubType::OffsetCenterOfMass, s, sCastOffsetCenterOfMassVsShape);
+		CollisionDispatch::sRegisterCastShape(s, EShapeSubType::OffsetCenterOfMass, sCastShapeVsOffsetCenterOfMass);
 	}
-
-	CollisionDispatch::sRegisterCastShape(EShapeSubType::OffsetCenterOfMass, sCastOffsetCenterOfMassVsShape);
 }
 
 } // JPH

Jolt/Physics/Collision/Shape/OffsetCenterOfMassShape.h

@@ -83,9 +83,6 @@ public:
 	// See: Shape::CollidePoint
 	virtual void					CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector) const override;
 
-	// See Shape::CastShape
-	virtual void					CastShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) const override;
-
 	// See Shape::CollectTransformedShapes
 	virtual void					CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector) const override;
 
@@ -122,6 +119,7 @@ private:
 	static void						sCollideOffsetCenterOfMassVsShape(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);
 	static void						sCollideShapeVsOffsetCenterOfMass(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);
 	static void						sCastOffsetCenterOfMassVsShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector);
+	static void						sCastShapeVsOffsetCenterOfMass(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector);
 
 	Vec3							mOffset;												///< Offset of the center of mass
 };

Jolt/Physics/Collision/Shape/RotatedTranslatedShape.cpp

@@ -135,17 +135,6 @@ void RotatedTranslatedShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreat
 	mInnerShape->CollidePoint(transform * inPoint, inSubShapeIDCreator, ioCollector);
 }
 
-void RotatedTranslatedShape::CastShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) const 
-{
-	// Determine the local transform
-	Mat44 local_transform = Mat44::sRotation(mRotation);
-
-	// Transform the shape cast
-	ShapeCast shape_cast = inShapeCast.PostTransformed(local_transform.Transposed3x3());
-
-	mInnerShape->CastShape(shape_cast, inShapeCastSettings, TransformScale(inScale), inShapeFilter, inCenterOfMassTransform2 * local_transform, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector);
-}
-
 void RotatedTranslatedShape::CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector) const
 {
 	mInnerShape->CollectTransformedShapes(inBox, inPositionCOM, inRotation * mRotation, TransformScale(inScale), inSubShapeIDCreator, ioCollector);
@@ -192,6 +181,20 @@ void RotatedTranslatedShape::sCastRotatedTranslatedVsShape(const ShapeCast &inSh
 	CollisionDispatch::sCastShapeVsShape(shape_cast, inShapeCastSettings, inShape, inScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector);
 }
 
+void RotatedTranslatedShape::sCastShapeVsRotatedTranslated(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
+{
+	JPH_ASSERT(inShape->GetSubType() == EShapeSubType::RotatedTranslated);
+	const RotatedTranslatedShape *shape = static_cast<const RotatedTranslatedShape *>(inShape);
+
+	// Determine the local transform
+	Mat44 local_transform = Mat44::sRotation(shape->mRotation);
+
+	// Transform the shape cast
+	ShapeCast shape_cast = inShapeCast.PostTransformed(local_transform.Transposed3x3());
+
+	CollisionDispatch::sCastShapeVsShape(shape_cast, inShapeCastSettings, shape->mInnerShape, shape->TransformScale(inScale), inShapeFilter, inCenterOfMassTransform2 * local_transform, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector);
+}
+
 void RotatedTranslatedShape::SaveBinaryState(StreamOut &inStream) const
 {
 	DecoratedShape::SaveBinaryState(inStream);
@@ -233,9 +236,9 @@ void RotatedTranslatedShape::sRegister()
 	{
 		CollisionDispatch::sRegisterCollideShape(EShapeSubType::RotatedTranslated, s, sCollideRotatedTranslatedVsShape);
 		CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::RotatedTranslated, sCollideShapeVsRotatedTranslated);
+		CollisionDispatch::sRegisterCastShape(EShapeSubType::RotatedTranslated, s, sCastRotatedTranslatedVsShape);
+		CollisionDispatch::sRegisterCastShape(s, EShapeSubType::RotatedTranslated, sCastShapeVsRotatedTranslated);
 	}
-
-	CollisionDispatch::sRegisterCastShape(EShapeSubType::RotatedTranslated, sCastRotatedTranslatedVsShape);
 }
 
 } // JPH

Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h

@@ -89,9 +89,6 @@ public:
 	// See: Shape::CollidePoint
 	virtual void					CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector) const override;
 
-	// See Shape::CastShape
-	virtual void					CastShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) const override;
-
 	// See Shape::CollectTransformedShapes
 	virtual void					CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector) const override;
 
@@ -128,6 +125,7 @@ private:
 	static void						sCollideRotatedTranslatedVsShape(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);
 	static void						sCollideShapeVsRotatedTranslated(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);
 	static void						sCastRotatedTranslatedVsShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector);
+	static void						sCastShapeVsRotatedTranslated(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector);
 
 	/// Transform the scale to the local space of the child shape
 	inline Vec3						TransformScale(Vec3Arg inScale) const

Jolt/Physics/Collision/Shape/ScaledShape.cpp

@@ -118,11 +118,6 @@ void ScaledShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubSh
 	mInnerShape->CollidePoint(inv_scale * inPoint, inSubShapeIDCreator, ioCollector);
 }
 
-void ScaledShape::CastShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) const 
-{
-	mInnerShape->CastShape(inShapeCast, inShapeCastSettings, inScale * mScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector);
-}
-
 void ScaledShape::CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector) const 
 {
 	mInnerShape->CollectTransformedShapes(inBox, inPositionCOM, inRotation, inScale * mScale, inSubShapeIDCreator, ioCollector);
@@ -182,6 +177,14 @@ void ScaledShape::sCastScaledVsShape(const ShapeCast &inShapeCast, const ShapeCa
 	CollisionDispatch::sCastShapeVsShape(scaled_cast, inShapeCastSettings, inShape, inScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector);
 }
 
+void ScaledShape::sCastShapeVsScaled(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
+{
+	JPH_ASSERT(inShape->GetSubType() == EShapeSubType::Scaled);
+	const ScaledShape *shape = static_cast<const ScaledShape *>(inShape);
+
+	CollisionDispatch::sCastShapeVsShape(inShapeCast, inShapeCastSettings, shape->mInnerShape, inScale * shape->mScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector);
+}
+
 void ScaledShape::sRegister()
 {
 	ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::Scaled);
@@ -192,9 +195,9 @@ void ScaledShape::sRegister()
 	{
 		CollisionDispatch::sRegisterCollideShape(EShapeSubType::Scaled, s, sCollideScaledVsShape);
 		CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::Scaled, sCollideShapeVsScaled);
+		CollisionDispatch::sRegisterCastShape(EShapeSubType::Scaled, s, sCastScaledVsShape);
+		CollisionDispatch::sRegisterCastShape(s, EShapeSubType::Scaled, sCastShapeVsScaled);
 	}
-
-	CollisionDispatch::sRegisterCastShape(EShapeSubType::Scaled, sCastScaledVsShape);
 }
 
 } // JPH

Jolt/Physics/Collision/Shape/ScaledShape.h

@@ -83,9 +83,6 @@ public:
 	// See: Shape::CollidePoint
 	virtual void					CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector) const override;
 
-	// See Shape::CastShape
-	virtual void					CastShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) const override;
-
 	// See Shape::CollectTransformedShapes
 	virtual void					CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector) const override;
 
@@ -122,6 +119,7 @@ private:
 	static void						sCollideScaledVsShape(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);
 	static void						sCollideShapeVsScaled(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);
 	static void						sCastScaledVsShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector);
+	static void						sCastShapeVsScaled(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector);
 
 	Vec3							mScale = Vec3(1, 1, 1);
 };

Jolt/Physics/Collision/Shape/Shape.h

@@ -107,6 +107,10 @@ static constexpr EShapeSubType sDecoratorSubShapeTypes[] = { EShapeSubType::Rota
 /// How many shape types we support
 static constexpr uint NumSubShapeTypes = (uint)size(sAllSubShapeTypes);
 
+/// Names of sub shape types
+static constexpr const char *sSubShapeTypeNames[] = { "Sphere", "Box", "Triangle", "Capsule", "TaperedCapsule", "Cylinder", "ConvexHull", "StaticCompound", "MutableCompound", "RotatedTranslated", "Scaled", "OffsetCenterOfMass", "Mesh", "HeightField", "User1", "User2", "User3", "User4", "User5", "User6", "User7", "User8" };
+static_assert(size(sSubShapeTypeNames) == NumSubShapeTypes);
+
 /// Class that can construct shapes and that is serializable using the ObjectStream system.
 /// Can be used to store shape data in 'uncooked' form (i.e. in a form that is still human readable and authorable).
 /// Once the shape has been created using the Create() function, the data will be moved into the Shape class
@@ -244,15 +248,6 @@ public:
 	/// For each shape that collides, ioCollector will receive a hit.
 	virtual void					CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector) const = 0;
 
-	/// Cast a shape againt this shape, passes any hits found to ioCollector.
-	/// Note that the shape cast should be relative to the center of mass of this shape (i.e. inShapeCast.mCenterOfMassStart = Mat44::sTranslation(-GetCenterOfMass()) * inShapeCast.mCenterOfMassStart if you want to cast against the shape in the space it was created).
-	/// This shape will be scaled in its local space with inScale before performing the cast.
-	/// inShapeFilter allows selectively disabling collisions between pairs of (sub) shapes.
-	/// inCenterOfMassTransform2 Is the center of mass transform of shape 2 (excluding scale), this is used to provide a transform to the shape cast result so that local quantities can be transformed into world space.
-	/// inSubShapeIDCreator1 represents the current sub shape ID of the cast shape.
-	/// inSubShapeIDCreator2 represents the current sub shape ID of the shape that is being cast against.
-	virtual void					CastShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) const = 0;
-
 	/// Collect the leaf transformed shapes of all leaf shapes of this shape.
 	/// inBox is the world space axis aligned box which leaf shapes should collide with.
 	/// inPositionCOM/inRotation/inScale describes the transform of this shape.

Jolt/Physics/Collision/Shape/StaticCompoundShape.cpp

@@ -515,7 +515,7 @@ void StaticCompoundShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator
 	WalkTree(visitor);
 }
 
-void StaticCompoundShape::CastShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) const 
+void StaticCompoundShape::sCastShapeVsCompound(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
 {
 	JPH_PROFILE_FUNCTION();
 
@@ -551,8 +551,11 @@ void StaticCompoundShape::CastShape(const ShapeCast &inShapeCast, const ShapeCas
 		float				mDistanceStack[cStackSize];
 	};
 
-	Visitor visitor(inShapeCast, inShapeCastSettings, this, inScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector);
-	WalkTree(visitor);
+	JPH_ASSERT(inShape->GetSubType() == EShapeSubType::StaticCompound);
+	const StaticCompoundShape *shape = static_cast<const StaticCompoundShape *>(inShape);
+
+	Visitor visitor(inShapeCast, inShapeCastSettings, shape, inScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector);
+	shape->WalkTree(visitor);
 }
 
 void StaticCompoundShape::CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector) const
@@ -693,6 +696,7 @@ void StaticCompoundShape::sRegister()
 	{
 		CollisionDispatch::sRegisterCollideShape(EShapeSubType::StaticCompound, s, sCollideCompoundVsShape);
 		CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::StaticCompound, sCollideShapeVsCompound);
+		CollisionDispatch::sRegisterCastShape(s, EShapeSubType::StaticCompound, sCastShapeVsCompound);
 	}
 }
 

Jolt/Physics/Collision/Shape/StaticCompoundShape.h

@@ -41,9 +41,6 @@ public:
 	// See: Shape::CollidePoint
 	virtual void					CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector) const override;
 
-	// See Shape::CastShape
-	virtual void					CastShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) const override;
-
 	// See Shape::CollectTransformedShapes
 	virtual void					CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector) const override;
 
@@ -102,6 +99,7 @@ private:
 	// Helper functions called by CollisionDispatch
 	static void						sCollideCompoundVsShape(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);
 	static void						sCollideShapeVsCompound(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);
+	static void						sCastShapeVsCompound(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector);
 
 	// Maximum size of the stack during tree walk
 	static constexpr int			cStackSize = 128;

Jolt/Physics/Collision/Shape/TriangleShape.cpp

@@ -12,6 +12,7 @@
 #include <Physics/Collision/CollidePointResult.h>
 #include <Physics/Collision/TransformedShape.h>
 #include <Physics/Collision/CastConvexVsTriangles.h>
+#include <Physics/Collision/CollisionDispatch.h>
 #include <Geometry/ConvexSupport.h>
 #include <Geometry/RayTriangle.h>
 #include <ObjectStream/TypeDeclarations.h>
@@ -233,10 +234,13 @@ void TriangleShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSub
 	// Can't be inside a triangle
 }
 
-void TriangleShape::CastShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) const 
+void TriangleShape::sCastConvexVsTriangle(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
 {
+	JPH_ASSERT(inShape->GetSubType() == EShapeSubType::Triangle);
+	const TriangleShape *shape = static_cast<const TriangleShape *>(inShape);
+
 	CastConvexVsTriangles caster(inShapeCast, inShapeCastSettings, inScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, ioCollector);
-	caster.Cast(mV1, mV2, mV3, 0b111, inSubShapeIDCreator2.GetID());
+	caster.Cast(shape->mV1, shape->mV2, shape->mV3, 0b111, inSubShapeIDCreator2.GetID());
 }
 
 void TriangleShape::TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const
@@ -324,6 +328,9 @@ void TriangleShape::sRegister()
 	ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::Triangle);
 	f.mConstruct = []() -> Shape * { return new TriangleShape; };
 	f.mColor = Color::sGreen;
+
+	for (EShapeSubType s : sConvexSubShapeTypes)
+		CollisionDispatch::sRegisterCastShape(s, EShapeSubType::Triangle, sCastConvexVsTriangle);
 }
 
 } // JPH

Jolt/Physics/Collision/Shape/TriangleShape.h

@@ -80,9 +80,6 @@ public:
 	// See: Shape::CollidePoint
 	virtual void			CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector) const override;
 
-	// See Shape::CastShape
-	virtual void			CastShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) const override;
-
 	// See Shape::TransformShape
 	virtual void			TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const override;
 
@@ -112,6 +109,9 @@ protected:
 	virtual void			RestoreBinaryState(StreamIn &inStream) override;
 
 private:
+	// Helper functions called by CollisionDispatch
+	static void				sCastConvexVsTriangle(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector);
+
 	// Context for GetTrianglesStart/Next
 	class					TSGetTrianglesContext;
 

Samples/SamplesApp.cpp

@@ -30,6 +30,7 @@
 #include <Physics/Collision/Shape/StaticCompoundShape.h>
 #include <Physics/Collision/Shape/MutableCompoundShape.h>
 #include <Physics/Collision/Shape/ScaledShape.h>
+#include <Physics/Collision/NarrowPhaseStats.h>
 #include <Physics/Constraints/DistanceConstraint.h>
 #include <Layers.h>
 #include <Utils/Log.h>
@@ -2079,6 +2080,11 @@ void SamplesApp::StepPhysics()
 		mPhysicsSystem->ReportBroadphaseStats();
 #endif // JPH_TRACK_BROADPHASE_STATS
 
+#ifdef JPH_TRACK_NARROWPHASE_STATS
+	if (mStepNumber % 600 == 0)
+		NarrowPhaseStat::sReportStats();
+#endif // JPH_TRACK_NARROWPHASE_STATS
+
 	{
 		// Post update
 		JPH_PROFILE("PostPhysicsUpdate");

UnitTests/Physics/CastShapeTests.cpp

@@ -11,6 +11,7 @@
 #include <Physics/Collision/Shape/ScaledShape.h>
 #include <Physics/Collision/Shape/BoxShape.h>
 #include <Physics/Collision/ShapeFilter.h>
+#include <Physics/Collision/CollisionDispatch.h>
 #include "PhysicsTestContext.h"
 #include "Layers.h"
 
@@ -30,7 +31,7 @@ TEST_SUITE("CastShapeTests")
 			cast_settings.mBackFaceModeConvex = EBackFaceMode::IgnoreBackFaces;
 			cast_settings.mReturnDeepestPoint = false;
 			AllHitCollisionCollector<CastShapeCollector> collector;
-			inTriangle->CastShape(shape_cast, cast_settings, Vec3::sReplicate(1.0f), ShapeFilter(), Mat44::sIdentity(), SubShapeIDCreator(), SubShapeIDCreator(), collector);
+			CollisionDispatch::sCastShapeVsShape(shape_cast, cast_settings, inTriangle, Vec3::sReplicate(1.0f), ShapeFilter(), Mat44::sIdentity(), SubShapeIDCreator(), SubShapeIDCreator(), collector);
 			CHECK(collector.mHits.size() == 1);
 			const ShapeCastResult &result = collector.mHits.back();
 			CHECK_APPROX_EQUAL(result.mFraction, (15.0f - 0.2f) / 30.0f, 1.0e-4f);
@@ -49,13 +50,13 @@ TEST_SUITE("CastShapeTests")
 			cast_settings.mBackFaceModeConvex = EBackFaceMode::IgnoreBackFaces;
 			cast_settings.mReturnDeepestPoint = false;
 			AllHitCollisionCollector<CastShapeCollector> collector;
-			inTriangle->CastShape(shape_cast, cast_settings, Vec3::sReplicate(1.0f), ShapeFilter(), Mat44::sIdentity(), SubShapeIDCreator(), SubShapeIDCreator(), collector);
+			CollisionDispatch::sCastShapeVsShape(shape_cast, cast_settings, inTriangle, Vec3::sReplicate(1.0f), ShapeFilter(), Mat44::sIdentity(), SubShapeIDCreator(), SubShapeIDCreator(), collector);
 			CHECK(collector.mHits.empty());
 
 			// Hit back face -> collision
 			cast_settings.mBackFaceModeTriangles = EBackFaceMode::CollideWithBackFaces;
 			cast_settings.mBackFaceModeConvex = EBackFaceMode::CollideWithBackFaces;
-			inTriangle->CastShape(shape_cast, cast_settings, Vec3::sReplicate(1.0f), ShapeFilter(), Mat44::sIdentity(), SubShapeIDCreator(), SubShapeIDCreator(), collector);
+			CollisionDispatch::sCastShapeVsShape(shape_cast, cast_settings, inTriangle, Vec3::sReplicate(1.0f), ShapeFilter(), Mat44::sIdentity(), SubShapeIDCreator(), SubShapeIDCreator(), collector);
 			CHECK(collector.mHits.size() == 1);
 			const ShapeCastResult &result = collector.mHits.back();
 			CHECK_APPROX_EQUAL(result.mFraction, (15.0f - 0.2f) / 30.0f, 1.0e-4f);
@@ -74,13 +75,13 @@ TEST_SUITE("CastShapeTests")
 			cast_settings.mBackFaceModeConvex = EBackFaceMode::IgnoreBackFaces;
 			cast_settings.mReturnDeepestPoint = true;
 			AllHitCollisionCollector<CastShapeCollector> collector;
-			inTriangle->CastShape(shape_cast, cast_settings, Vec3::sReplicate(1.0f), ShapeFilter(), Mat44::sIdentity(), SubShapeIDCreator(), SubShapeIDCreator(), collector);
+			CollisionDispatch::sCastShapeVsShape(shape_cast, cast_settings, inTriangle, Vec3::sReplicate(1.0f), ShapeFilter(), Mat44::sIdentity(), SubShapeIDCreator(), SubShapeIDCreator(), collector);
 			CHECK(collector.mHits.empty());
 
 			// Hit back face while starting in collision -> collision
 			cast_settings.mBackFaceModeTriangles = EBackFaceMode::CollideWithBackFaces;
 			cast_settings.mBackFaceModeConvex = EBackFaceMode::CollideWithBackFaces;
-			inTriangle->CastShape(shape_cast, cast_settings, Vec3::sReplicate(1.0f), ShapeFilter(), Mat44::sIdentity(), SubShapeIDCreator(), SubShapeIDCreator(), collector);
+			CollisionDispatch::sCastShapeVsShape(shape_cast, cast_settings, inTriangle, Vec3::sReplicate(1.0f), ShapeFilter(), Mat44::sIdentity(), SubShapeIDCreator(), SubShapeIDCreator(), collector);
 			CHECK(collector.mHits.size() == 1);
 			const ShapeCastResult &result = collector.mHits.back();
 			CHECK_APPROX_EQUAL(result.mFraction, 0.0f);