Performance optimizations (#17)

- ShapeCast no longer keeps reference on shape (does a lot of addref/remove while traversing the shape hierarchy which is pointless)
- PhysicsSystem uses a non-locking version of CastAABox to avoid taking mutex locks that we know we don't need

Jolt/Physics/Collision/BroadPhase/BroadPhase.h

@@ -31,13 +31,13 @@ public:
 	virtual void		Init(BodyManager *inBodyManager, const ObjectToBroadPhaseLayer &inObjectToBroadPhaseLayer);
 
 	/// Should be called after many objects have been inserted to make the broadphase more efficient, usually done on startup only
-	virtual void		Optimize()															{ }
+	virtual void		Optimize()															{ /* Optionally overridden by implementation */ }
 
 	/// Must be called just before updating the broadphase when none of the body mutexes are locked
-	virtual void		FrameSync()															{ }
+	virtual void		FrameSync()															{ /* Optionally overridden by implementation */ }
 
 	/// Must be called before UpdatePrepare to prevent modifications from being made to the tree
-	virtual void		LockModifications()													{ }
+	virtual void		LockModifications()													{ /* Optionally overridden by implementation */ }
 
 	/// Context used during broadphase update
 	struct UpdateState { void *mData[4]; };
@@ -47,10 +47,10 @@ public:
 	virtual	UpdateState	UpdatePrepare()														{ return UpdateState(); }
 
 	/// Finalizing the update will quickly apply the changes
-	virtual void		UpdateFinalize(UpdateState &inUpdateState)							{ }
+	virtual void		UpdateFinalize(UpdateState &inUpdateState)							{ /* Optionally overridden by implementation */ }
 
 	/// Must be called after UpdateFinalize to allow modifications to the broadphase
-	virtual void		UnlockModifications()												{ }
+	virtual void		UnlockModifications()												{ /* Optionally overridden by implementation */ }
 
 	/// Handle used during adding bodies to the broadphase
 	using AddState = void *;
@@ -67,7 +67,7 @@ public:
 	/// Abort adding bodies to the broadphase, supply the return value of AddBodiesPrepare in inAddState.
 	/// This can be done on a background thread without influencing the broadphase.
 	/// Please ensure that the ioBodies array passed to AddBodiesPrepare is unmodified and passed again to this function.
-	virtual void		AddBodiesAbort(BodyID *ioBodies, int inNumber, AddState inAddState)	{ } // By default nothing needs to be done
+	virtual void		AddBodiesAbort(BodyID *ioBodies, int inNumber, AddState inAddState)	{ /* By default nothing needs to be done */ }
 
 	/// Remove inNumber bodies in ioBodies from the broadphase.
 	/// ioBodies may be shuffled around by this function.
@@ -91,6 +91,9 @@ public:
 	/// @param ioPairCollector receives callbacks for every body pair found.
 	virtual void		FindCollidingPairs(BodyID *ioActiveBodies, int inNumActiveBodies, float inSpeculativeContactDistance, ObjectVsBroadPhaseLayerFilter inObjectVsBroadPhaseLayerFilter, ObjectLayerPairFilter inObjectLayerPairFilter, BodyPairCollector &ioPairCollector) const = 0;
 
+	/// Same as BroadPhaseQuery::CastAABox but can be implemented in a way to take no broad phase locks.
+	virtual void		CastAABoxNoLock(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const = 0; 
+
 #if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED)
 	/// Set function that converts a broadphase layer to a human readable string for debugging purposes
 	virtual void		SetBroadPhaseLayerToString(BroadPhaseLayerToString inBroadPhaseLayerToString) { /* Can be implemented by derived classes */ }

Jolt/Physics/Collision/BroadPhase/BroadPhaseBruteForce.cpp

@@ -24,7 +24,7 @@ void BroadPhaseBruteForce::AddBodiesFinalize(BodyID *ioBodies, int inNumber, Add
 	mBodyIDs.resize(idx + inNumber);
 
 	// Add bodies
-	for (BodyID *b = ioBodies, *b_end = ioBodies + inNumber; b < b_end; ++b)
+	for (const BodyID *b = ioBodies, *b_end = ioBodies + inNumber; b < b_end; ++b)
 	{
 		Body &body = *bodies[b->GetIndex()];
 
@@ -53,7 +53,7 @@ void BroadPhaseBruteForce::RemoveBodies(BodyID *ioBodies, int inNumber)
 	JPH_ASSERT((int)mBodyIDs.size() >= inNumber);
 
 	// Remove bodies
-	for (BodyID *b = ioBodies, *b_end = ioBodies + inNumber; b < b_end; ++b)
+	for (const BodyID *b = ioBodies, *b_end = ioBodies + inNumber; b < b_end; ++b)
 	{
 		Body &body = *bodies[b->GetIndex()];
 
@@ -218,7 +218,7 @@ void BroadPhaseBruteForce::CollideOrientedBox(const OrientedBox &inBox, CollideS
 	}
 }
 
-void BroadPhaseBruteForce::CastAABox(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const
+void BroadPhaseBruteForce::CastAABoxNoLock(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const 
 {
 	shared_lock lock(mMutex);
 
@@ -252,6 +252,11 @@ void BroadPhaseBruteForce::CastAABox(const AABoxCast &inBox, CastShapeBodyCollec
 	}
 }
 
+void BroadPhaseBruteForce::CastAABox(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const
+{
+	CastAABoxNoLock(inBox, ioCollector, inBroadPhaseLayerFilter, inObjectLayerFilter);
+}
+
 void BroadPhaseBruteForce::FindCollidingPairs(BodyID *ioActiveBodies, int inNumActiveBodies, float inSpeculativeContactDistance, ObjectVsBroadPhaseLayerFilter inObjectVsBroadPhaseLayerFilter, ObjectLayerPairFilter inObjectLayerPairFilter, BodyPairCollector &ioPairCollector) const
 {
 	shared_lock lock(mMutex);

Jolt/Physics/Collision/BroadPhase/BroadPhaseBruteForce.h

@@ -22,6 +22,7 @@ public:
 	virtual void		CollideSphere(Vec3Arg inCenter, float inRadius, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override;
 	virtual void		CollidePoint(Vec3Arg inPoint, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override;
 	virtual void		CollideOrientedBox(const OrientedBox &inBox, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override;
+	virtual void		CastAABoxNoLock(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override; 
 	virtual void		CastAABox(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override;
 	virtual void		FindCollidingPairs(BodyID *ioActiveBodies, int inNumActiveBodies, float inSpeculativeContactDistance, ObjectVsBroadPhaseLayerFilter inObjectVsBroadPhaseLayerFilter, ObjectLayerPairFilter inObjectLayerPairFilter, BodyPairCollector &ioPairCollector) const override;
 

Jolt/Physics/Collision/BroadPhase/BroadPhaseQuadTree.cpp

@@ -184,7 +184,7 @@ BroadPhase::AddState BroadPhaseQuadTree::AddBodiesPrepare(BodyID *ioBodies, int
 		mLayers[broadphase_layer].AddBodiesPrepare(bodies, mTracking, b_start, int(b_mid - b_start), layer_state.mAddState);
 
 		// Keep track in which tree we placed the object
-		for (BodyID *b = b_start; b < b_mid; ++b)
+		for (const BodyID *b = b_start; b < b_mid; ++b)
 		{
 			uint32 index = b->GetIndex();
 			JPH_ASSERT(bodies[index]->GetID() == *b, "Provided BodyID doesn't match BodyID in body manager");
@@ -224,7 +224,7 @@ void BroadPhaseQuadTree::AddBodiesFinalize(BodyID *ioBodies, int inNumber, AddSt
 			mLayers[broadphase_layer].AddBodiesFinalize(mTracking, int(l.mBodyEnd - l.mBodyStart), l.mAddState);
 
 			// Mark added to broadphase
-			for (BodyID *b = l.mBodyStart; b < l.mBodyEnd; ++b)
+			for (const BodyID *b = l.mBodyStart; b < l.mBodyEnd; ++b)
 			{
 				uint32 index = b->GetIndex();
 				JPH_ASSERT(bodies[index]->GetID() == *b, "Provided BodyID doesn't match BodyID in body manager");
@@ -257,7 +257,7 @@ void BroadPhaseQuadTree::AddBodiesAbort(BodyID *ioBodies, int inNumber, AddState
 			mLayers[broadphase_layer].AddBodiesAbort(mTracking, l.mAddState);
 
 			// Reset bookkeeping
-			for (BodyID *b = l.mBodyStart; b < l.mBodyEnd; ++b)
+			for (const BodyID *b = l.mBodyStart; b < l.mBodyEnd; ++b)
 			{
 				uint32 index = b->GetIndex();
 				JPH_ASSERT(bodies[index]->GetID() == *b, "Provided BodyID doesn't match BodyID in body manager");
@@ -302,7 +302,7 @@ void BroadPhaseQuadTree::RemoveBodies(BodyID *ioBodies, int inNumber)
 		// Remove all bodies of the same layer
 		mLayers[broadphase_layer].RemoveBodies(bodies, mTracking, b_start, int(b_mid - b_start));
 
-		for (BodyID *b = b_start; b < b_mid; ++b)
+		for (const BodyID *b = b_start; b < b_mid; ++b)
 		{
 			// Reset bookkeeping
 			uint32 index = b->GetIndex();
@@ -336,7 +336,7 @@ void BroadPhaseQuadTree::NotifyBodiesAABBChanged(BodyID *ioBodies, int inNumber,
 	JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies());
 
 	// Sort bodies on layer
-	Tracking *tracking = mTracking.data(); // C pointer or else sort is incredibly slow in debug mode
+	const Tracking *tracking = mTracking.data(); // C pointer or else sort is incredibly slow in debug mode
 	sort(ioBodies, ioBodies + inNumber, [tracking](BodyID inLHS, BodyID inRHS) { return tracking[inLHS.GetIndex()].mBroadPhaseLayer < tracking[inRHS.GetIndex()].mBroadPhaseLayer; });
 
 	BodyID *b_start = ioBodies, *b_end = ioBodies + inNumber;
@@ -512,15 +512,12 @@ void BroadPhaseQuadTree::CollideOrientedBox(const OrientedBox &inBox, CollideSha
 	}
 }
 
-void BroadPhaseQuadTree::CastAABox(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const 
+void BroadPhaseQuadTree::CastAABoxNoLock(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const 
 { 
 	JPH_PROFILE_FUNCTION();
 
 	JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies());	
 
-	// Prevent this from running in parallel with node deletion in FrameSync(), see notes there
-	shared_lock lock(mQueryLocks[mQueryLockIdx]);
-
 	// Loop over all layers and test the ones that could hit
 	for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l)
 	{
@@ -535,6 +532,14 @@ void BroadPhaseQuadTree::CastAABox(const AABoxCast &inBox, CastShapeBodyCollecto
 	}
 }
 
+void BroadPhaseQuadTree::CastAABox(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const 
+{ 
+	// Prevent this from running in parallel with node deletion in FrameSync(), see notes there
+	shared_lock lock(mQueryLocks[mQueryLockIdx]);
+
+	CastAABoxNoLock(inBox, ioCollector, inBroadPhaseLayerFilter, inObjectLayerFilter);
+}
+
 void BroadPhaseQuadTree::FindCollidingPairs(BodyID *ioActiveBodies, int inNumActiveBodies, float inSpeculativeContactDistance, ObjectVsBroadPhaseLayerFilter inObjectVsBroadPhaseLayerFilter, ObjectLayerPairFilter inObjectLayerPairFilter, BodyPairCollector &ioPairCollector) const 
 { 
 	JPH_PROFILE_FUNCTION();

Jolt/Physics/Collision/BroadPhase/BroadPhaseQuadTree.h

@@ -34,6 +34,7 @@ public:
 	virtual void			CollideSphere(Vec3Arg inCenter, float inRadius, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override;
 	virtual void			CollidePoint(Vec3Arg inPoint, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override;
 	virtual void			CollideOrientedBox(const OrientedBox &inBox, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override;
+	virtual void			CastAABoxNoLock(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override; 
 	virtual void			CastAABox(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override;
 	virtual void			FindCollidingPairs(BodyID *ioActiveBodies, int inNumActiveBodies, float inSpeculativeContactDistance, ObjectVsBroadPhaseLayerFilter inObjectVsBroadPhaseLayerFilter, ObjectLayerPairFilter inObjectLayerPairFilter, BodyPairCollector &ioPairCollector) const override;
 #if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED)

Jolt/Physics/Collision/CastConvexVsTriangles.cpp

@@ -57,7 +57,7 @@ void CastConvexVsTriangles::Cast(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8
 		ConvexShape::ESupportMode support_mode = mShapeCastSettings.mUseShrunkenShapeAndConvexRadius? ConvexShape::ESupportMode::ExcludeConvexRadius : ConvexShape::ESupportMode::IncludeConvexRadius;
 
 		// Create support function
-		mSupport = static_cast<const ConvexShape *>(mShapeCast.mShape.GetPtr())->GetSupportFunction(support_mode, mSupportBuffer, mShapeCast.mScale);
+		mSupport = static_cast<const ConvexShape *>(mShapeCast.mShape)->GetSupportFunction(support_mode, mSupportBuffer, mShapeCast.mScale);
 	}
 
 	EPAPenetrationDepth epa;
@@ -90,7 +90,7 @@ void CastConvexVsTriangles::Cast(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8
 			// Get supporting face of shape 1
 			Mat44 transform_1_to_2 = mShapeCast.mCenterOfMassStart;
 			transform_1_to_2.SetTranslation(transform_1_to_2.GetTranslation() + fraction * mShapeCast.mDirection);
-			static_cast<const ConvexShape *>(mShapeCast.mShape.GetPtr())->GetSupportingFace(transform_1_to_2.Multiply3x3Transposed(-contact_normal), mShapeCast.mScale, result.mShape1Face);
+			static_cast<const ConvexShape *>(mShapeCast.mShape)->GetSupportingFace(transform_1_to_2.Multiply3x3Transposed(-contact_normal), mShapeCast.mScale, result.mShape1Face);
 
 			// Convert to world space
 			Mat44 transform_1_to_world = mCenterOfMassTransform2 * transform_1_to_2;

Jolt/Physics/Collision/Shape/CompoundShape.cpp

@@ -243,7 +243,7 @@ void CompoundShape::sCastCompoundVsShape(const ShapeCast &inShapeCast, const Sha
 
 	// Fetch compound shape from cast shape
 	JPH_ASSERT(inShapeCast.mShape->GetType() == EShapeType::Compound);
-	const CompoundShape *compound = static_cast<const CompoundShape *>(inShapeCast.mShape.GetPtr());
+	const CompoundShape *compound = static_cast<const CompoundShape *>(inShapeCast.mShape);
 
 	// Number of sub shapes
 	int n = (int)compound->mSubShapes.size();

Jolt/Physics/Collision/Shape/ConvexShape.cpp

@@ -261,7 +261,7 @@ void ConvexShape::CastShape(const ShapeCast &inShapeCast, const ShapeCastSetting
 
 	// Only supported for convex shapes
 	JPH_ASSERT(inShapeCast.mShape->GetType() == EShapeType::Convex); 
-	const ConvexShape *cast_shape = static_cast<const ConvexShape *>(inShapeCast.mShape.GetPtr());
+	const ConvexShape *cast_shape = static_cast<const ConvexShape *>(inShapeCast.mShape);
 
 	// 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;

Jolt/Physics/Collision/Shape/OffsetCenterOfMassShape.cpp

@@ -151,7 +151,7 @@ void OffsetCenterOfMassShape::sCastOffsetCenterOfMassVsShape(const ShapeCast &in
 {
 	// Fetch offset center of mass shape from cast shape
 	JPH_ASSERT(inShapeCast.mShape->GetSubType() == EShapeSubType::OffsetCenterOfMass);
-	const OffsetCenterOfMassShape *shape1 = static_cast<const OffsetCenterOfMassShape *>(inShapeCast.mShape.GetPtr());
+	const OffsetCenterOfMassShape *shape1 = static_cast<const OffsetCenterOfMassShape *>(inShapeCast.mShape);
 
 	// Transform the shape cast and update the shape
 	ShapeCast shape_cast(shape1->mInnerShape, inShapeCast.mScale, inShapeCast.mCenterOfMassStart * Mat44::sTranslation(-shape1->mOffset), inShapeCast.mDirection);

Jolt/Physics/Collision/Shape/RotatedTranslatedShape.cpp

@@ -182,7 +182,7 @@ void RotatedTranslatedShape::sCastRotatedTranslatedVsShape(const ShapeCast &inSh
 {
 	// Fetch rotated translated shape from cast shape
 	JPH_ASSERT(inShapeCast.mShape->GetSubType() == EShapeSubType::RotatedTranslated);
-	const RotatedTranslatedShape *shape1 = static_cast<const RotatedTranslatedShape *>(inShapeCast.mShape.GetPtr());
+	const RotatedTranslatedShape *shape1 = static_cast<const RotatedTranslatedShape *>(inShapeCast.mShape);
 
 	// Transform the shape cast and update the shape
 	Mat44 transform = inShapeCast.mCenterOfMassStart * Mat44::sRotation(shape1->mRotation);

Jolt/Physics/Collision/Shape/ScaledShape.cpp

@@ -176,7 +176,7 @@ void ScaledShape::sCollideShapeVsScaled(const Shape *inShape1, const Shape *inSh
 void ScaledShape::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)
 {
 	JPH_ASSERT(inShapeCast.mShape->GetSubType() == EShapeSubType::Scaled);
-	const ScaledShape *shape = static_cast<const ScaledShape *>(inShapeCast.mShape.GetPtr());
+	const ScaledShape *shape = static_cast<const ScaledShape *>(inShapeCast.mShape);
 
 	ShapeCast scaled_cast(shape->GetInnerShape(), inShapeCast.mScale * shape->GetScale(), inShapeCast.mCenterOfMassStart, inShapeCast.mDirection);
 	CollisionDispatch::sCastShapeVsShape(scaled_cast, inShapeCastSettings, inShape, inScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector);

Jolt/Physics/Collision/ShapeCast.h

@@ -36,7 +36,7 @@ struct ShapeCast
 		return { mShape, mScale, start, direction };
 	}
 
-	const RefConst<Shape>		mShape;								///< Shape that's being cast (cannot be mesh shape)
+	const Shape *				mShape;								///< Shape that's being cast (cannot be mesh shape). Note that this structure does not assume ownership over the shape for performance reasons.
 	const Vec3					mScale;								///< Scale in local space of the shape being cast
 	const Mat44					mCenterOfMassStart;					///< Start position and orientation of the center of mass of the shape (i.e. mCenterOfMassStart = Start * Mat44::sTranslation(mShape->GetCenterOfMass()) if you want to cast the shape in the space it was created)
 	const Vec3					mDirection;							///< Direction and length of the cast (anything beyond this length will not be reported as a hit)

Jolt/Physics/PhysicsSystem.cpp

@@ -344,9 +344,9 @@ void PhysicsSystem::Update(float inDeltaTime, int inCollisionSteps, int inIntegr
 					step.mBodySetIslandIndex.RemoveDependency();
 				}, num_find_collisions_jobs + 2); // depends on: find collisions, build islands from constraints, finish building jobs
 
-			// Unblock previous jobs (find collisions last since it will use up all CPU cores)
+			// Unblock previous job
+			// Note: technically we could release find collisions here but we don't want to because that could make them run before 'setup velocity constraints' which means that job won't have a thread left
 			step.mBuildIslandsFromConstraints.RemoveDependency();
-			JobHandle::sRemoveDependencies(step.mFindCollisions);
 
 			// This job will call the contact removed callbacks
 			step.mContactRemovedCallbacks = inJobSystem->CreateJob("ContactRemovedCallbacks", cColorContactRemovedCallbacks, [&context, &step]()
@@ -433,8 +433,12 @@ void PhysicsSystem::Update(float inDeltaTime, int inCollisionSteps, int inIntegr
 				// Unblock previous jobs
 				if (is_first_sub_step)
 				{
-					step.mFinalizeIslands.RemoveDependency();
+					// Kick find collisions after setup velocity constraints because the former job will use up all CPU cores
 					step.mSetupVelocityConstraints.RemoveDependency();
+					JobHandle::sRemoveDependencies(step.mFindCollisions);
+
+					// Finalize islands is a dependency on find collisions so it can go last
+					step.mFinalizeIslands.RemoveDependency();
 				}
 				else
 				{
@@ -530,7 +534,7 @@ void PhysicsSystem::Update(float inDeltaTime, int inCollisionSteps, int inIntegr
 		JPH_PROFILE("Build job barrier");
 
 		StaticArray<JobHandle, cMaxPhysicsJobs> handles;
-		for (PhysicsUpdateContext::Step &step : context.mSteps)
+		for (const PhysicsUpdateContext::Step &step : context.mSteps)
 		{
 			if (step.mBroadPhasePrepare.IsValid())
 				handles.push_back(step.mBroadPhasePrepare);
@@ -548,7 +552,7 @@ void PhysicsSystem::Update(float inDeltaTime, int inCollisionSteps, int inIntegr
 			handles.push_back(step.mBuildIslandsFromConstraints);
 			handles.push_back(step.mFinalizeIslands);
 			handles.push_back(step.mBodySetIslandIndex);
-			for (PhysicsUpdateContext::SubStep &sub_step : step.mSubSteps)
+			for (const PhysicsUpdateContext::SubStep &sub_step : step.mSubSteps)
 			{
 				for (const JobHandle &h : sub_step.mSolveVelocityConstraints)
 					handles.push_back(h);
@@ -639,7 +643,7 @@ void PhysicsSystem::JobStepListeners(PhysicsUpdateContext::Step *ioStep)
 	}
 }
 
-void PhysicsSystem::JobDetermineActiveConstraints(PhysicsUpdateContext::Step *ioStep)
+void PhysicsSystem::JobDetermineActiveConstraints(PhysicsUpdateContext::Step *ioStep) const
 {
 #ifdef JPH_ENABLE_ASSERTS
 	// No body access
@@ -672,7 +676,7 @@ void PhysicsSystem::JobDetermineActiveConstraints(PhysicsUpdateContext::Step *io
 	}
 }
 
-void PhysicsSystem::JobApplyGravity(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep)
+void PhysicsSystem::JobApplyGravity(const PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep)
 {
 #ifdef JPH_ENABLE_ASSERTS
 	// We update velocities and need the rotation to do so
@@ -703,7 +707,7 @@ void PhysicsSystem::JobApplyGravity(PhysicsUpdateContext *ioContext, PhysicsUpda
 		// Process the batch
 		while (active_body_idx < active_body_idx_end)
 		{
-			Body &body = mBodyManager.GetBody(active_bodies[active_body_idx]);
+			const Body &body = mBodyManager.GetBody(active_bodies[active_body_idx]);
 			if (body.IsDynamic())
 				body.GetMotionProperties()->ApplyForceTorqueAndDragInternal(body.GetRotation(), mGravity, delta_time);
 			active_body_idx++;
@@ -738,7 +742,7 @@ void PhysicsSystem::JobBuildIslandsFromConstraints(PhysicsUpdateContext *ioConte
 	mConstraintManager.BuildIslands(ioStep->mContext->mActiveConstraints, ioStep->mNumActiveConstraints, mIslandBuilder, mBodyManager);
 }
 
-void PhysicsSystem::TrySpawnJobFindCollisions(PhysicsUpdateContext::Step *ioStep)
+void PhysicsSystem::TrySpawnJobFindCollisions(PhysicsUpdateContext::Step *ioStep) const
 {
 	// Get how many jobs we can spawn and check if we can spawn more
 	uint max_jobs = ioStep->mBodyPairQueues.size();
@@ -862,7 +866,7 @@ void PhysicsSystem::JobFindCollisions(PhysicsUpdateContext::Step *ioStep, int in
 				mBroadPhase->FindCollidingPairs(active_bodies, batch_size, mPhysicsSettings.mSpeculativeContactDistance, mObjectVsBroadPhaseLayerFilter, mObjectLayerPairFilter, add_pair);
 
 				// Check if we have enough pairs in the buffer to start a new job
-				PhysicsUpdateContext::BodyPairQueue &queue = ioStep->mBodyPairQueues[inJobIndex];
+				const PhysicsUpdateContext::BodyPairQueue &queue = ioStep->mBodyPairQueues[inJobIndex];
 				uint32 body_pairs_in_queue = queue.mWriteIdx - queue.mReadIdx;
 				if (body_pairs_in_queue >= cNarrowPhaseBatchSize)
 					TrySpawnJobFindCollisions(ioStep);
@@ -871,7 +875,7 @@ void PhysicsSystem::JobFindCollisions(PhysicsUpdateContext::Step *ioStep, int in
 		else 
 		{
 			// Lockless loop to get the next body pair from the pairs buffer
-			PhysicsUpdateContext *context = ioStep->mContext;
+			const PhysicsUpdateContext *context = ioStep->mContext;
 			int first_read_queue_idx = read_queue_idx;
 			for (;;)
 			{
@@ -903,7 +907,7 @@ void PhysicsSystem::JobFindCollisions(PhysicsUpdateContext::Step *ioStep, int in
 				}
 
 				// Copy the body pair out of the buffer
-				BodyPair bp = context->mBodyPairs[read_queue_idx * ioStep->mMaxBodyPairsPerQueue + pair_idx % ioStep->mMaxBodyPairsPerQueue];
+				const BodyPair bp = context->mBodyPairs[read_queue_idx * ioStep->mMaxBodyPairsPerQueue + pair_idx % ioStep->mMaxBodyPairsPerQueue];
 			
 				// Mark this pair as taken
 				if (queue.mReadIdx.compare_exchange_strong(pair_idx, pair_idx + 1))
@@ -1546,7 +1550,7 @@ inline static PhysicsUpdateContext::SubStep::CCDBody *sGetCCDBody(const Body &in
 	return ccd_body;
 }
 
-void PhysicsSystem::JobFindCCDContacts(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::SubStep *ioSubStep)
+void PhysicsSystem::JobFindCCDContacts(const PhysicsUpdateContext *ioContext, PhysicsUpdateContext::SubStep *ioSubStep)
 {
 #ifdef JPH_ENABLE_ASSERTS
 	// We only read positions, but the validate callback may read body positions and velocities
@@ -1776,10 +1780,10 @@ void PhysicsSystem::JobFindCCDContacts(PhysicsUpdateContext *ioContext, PhysicsU
 			float						mDeltaTime;
 		};
 
-		// Check if we collide with any other body
+		// Check if we collide with any other body. Note that we use the non-locking interface as we know the broadphase cannot be modified at this point.
 		ShapeCast shape_cast(body.GetShape(), Vec3::sReplicate(1.0f), body.GetCenterOfMassTransform(), ccd_body.mDeltaPosition);
 		CCDBroadPhaseCollector bp_collector(ccd_body, body, shape_cast, settings, np_collector, mBodyManager, ioSubStep, ioContext->mSubStepDeltaTime);
-		mBroadPhase->CastAABox({ shape_cast.mShapeWorldBounds, shape_cast.mDirection }, bp_collector, broadphase_layer_filter, object_layer_filter);
+		mBroadPhase->CastAABoxNoLock({ shape_cast.mShapeWorldBounds, shape_cast.mDirection }, bp_collector, broadphase_layer_filter, object_layer_filter);
 
 		// Check if there was a hit
 		if (ccd_body.mFractionPlusSlop < 1.0f)
@@ -2177,7 +2181,7 @@ bool PhysicsSystem::RestoreState(StateRecorder &inStream)
 
 	// Update bounding boxes for all bodies in the broadphase
 	vector<BodyID> bodies;
-	for (Body *b : mBodyManager.GetBodies())
+	for (const Body *b : mBodyManager.GetBodies())
 		if (BodyManager::sIsValidBodyPointer(b) && b->IsInBroadPhase())
 			bodies.push_back(b->GetID());
 	if (!bodies.empty())

Jolt/Physics/PhysicsSystem.h

@@ -33,7 +33,7 @@ public:
 	/// @param inNumBodyMutexes Number of body mutexes to use. Should be a power of 2 in the range [1, 64], use 0 to auto detect.
 	/// @param inMaxBodyPairs Maximum amount of body pairs to process (anything else will fall through the world), this number should generally be much higher than the max amount of contact points as there will be lots of bodies close that are not actually touching
 	/// @param inMaxContactConstraints Maximum amount of contact constraints to process (anything else will fall through the world)
-	/// @param inObjectToBroadPhaseLayer Maps object layer to broadphase layer, @see ObjectToBroadPhaseLayer.
+	/// @param inObjectToBroadPhaseLayer Maps object layer to broadphase layer, see ObjectToBroadPhaseLayer.
 	/// @param inObjectLayerPairFilter Filter callback function that is used to determine if two object layers collide.
 	void						Init(uint inMaxBodies, uint inNumBodyMutexes, uint inMaxBodyPairs, uint inMaxContactConstraints, const ObjectToBroadPhaseLayer &inObjectToBroadPhaseLayer, ObjectVsBroadPhaseLayerFilter inObjectVsBroadPhaseLayerFilter, ObjectLayerPairFilter inObjectLayerPairFilter);
 	
@@ -176,8 +176,8 @@ private:
 
 	// Various job entry points
 	void						JobStepListeners(PhysicsUpdateContext::Step *ioStep);
-	void						JobDetermineActiveConstraints(PhysicsUpdateContext::Step *ioStep);
-	void						JobApplyGravity(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep);	
+	void						JobDetermineActiveConstraints(PhysicsUpdateContext::Step *ioStep) const;
+	void						JobApplyGravity(const PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep);	
 	void						JobSetupVelocityConstraints(float inDeltaTime, PhysicsUpdateContext::Step *ioStep);
 	void						JobBuildIslandsFromConstraints(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep);
 	void						JobFindCollisions(PhysicsUpdateContext::Step *ioStep, int inJobIndex);
@@ -187,13 +187,13 @@ private:
 	void						JobPreIntegrateVelocity(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::SubStep *ioSubStep) const;
 	void						JobIntegrateVelocity(const PhysicsUpdateContext *ioContext, PhysicsUpdateContext::SubStep *ioSubStep);
 	void						JobPostIntegrateVelocity(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::SubStep *ioSubStep) const;
-	void						JobFindCCDContacts(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::SubStep *ioSubStep);
+	void						JobFindCCDContacts(const PhysicsUpdateContext *ioContext, PhysicsUpdateContext::SubStep *ioSubStep);
 	void						JobResolveCCDContacts(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::SubStep *ioSubStep);
 	void						JobContactRemovedCallbacks();
 	void						JobSolvePositionConstraints(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::SubStep *ioSubStep);
 
 	/// Tries to spawn a new FindCollisions job if max concurrency hasn't been reached yet
-	void						TrySpawnJobFindCollisions(PhysicsUpdateContext::Step *ioStep);
+	void						TrySpawnJobFindCollisions(PhysicsUpdateContext::Step *ioStep) const;
 
 	/// Process narrow phase for a single body pair
 	void						ProcessBodyPair(const BodyPair &inBodyPair);

UnitTests/Physics/CastShapeTests.cpp

@@ -185,7 +185,8 @@ TEST_SUITE("CastShapeTests")
 
 		{
 			// Create shape cast in X from -5 to 5
-			ShapeCast shape_cast { new SphereShape(1.0f), Vec3::sReplicate(1.0f), Mat44::sTranslation(Vec3(-5, 0, 0)), Vec3(10, 0, 0) };
+			RefConst<Shape> sphere = new SphereShape(1.0f);
+			ShapeCast shape_cast { sphere, Vec3::sReplicate(1.0f), Mat44::sTranslation(Vec3(-5, 0, 0)), Vec3(10, 0, 0) };
 
 			// We should hit the first body
 			ClosestHitCollisionCollector<CastShapeCollector> collector;
@@ -202,7 +203,8 @@ TEST_SUITE("CastShapeTests")
 
 		{
 			// Create shape cast in X from 5 to -5
-			ShapeCast shape_cast { new SphereShape(1.0f), Vec3::sReplicate(1.0f), Mat44::sTranslation(Vec3(5, 0, 0)), Vec3(-10, 0, 0) };
+			RefConst<Shape> sphere = new SphereShape(1.0f);
+			ShapeCast shape_cast { sphere, Vec3::sReplicate(1.0f), Mat44::sTranslation(Vec3(5, 0, 0)), Vec3(-10, 0, 0) };
 
 			// We should hit the last body
 			ClosestHitCollisionCollector<CastShapeCollector> collector;
@@ -219,7 +221,8 @@ TEST_SUITE("CastShapeTests")
 
 		{
 			// Create shape cast in X from 1.05 to 11, this should intersect with all bodies and have deepest penetration in bodies[5]
-			ShapeCast shape_cast { new SphereShape(1.0f), Vec3::sReplicate(1.0f), Mat44::sTranslation(Vec3(1.05f, 0, 0)), Vec3(10, 0, 0) };
+			RefConst<Shape> sphere = new SphereShape(1.0f);
+			ShapeCast shape_cast { sphere, Vec3::sReplicate(1.0f), Mat44::sTranslation(Vec3(1.05f, 0, 0)), Vec3(10, 0, 0) };
 
 			// We should hit bodies[5]
 			AllHitCollisionCollector<CastShapeCollector> collector;