Added ability to lock multiple physics systems without triggering assert (#405)

Fix for issue reported in #403

Docs/Architecture.md

@@ -376,6 +376,22 @@ When synchronizing two simulations via a network, it is possible that a change t
 
 If you wish to share saved state between server and client, you need to ensure that all APIs that modify the state of the world are called in the exact same order. So if the client creates physics objects for player 1 then 2 and the server creates the objects for 2 then 1 you already have a problem (the body IDs will be different, which will render the save state snapshots incompatible). When rolling back a simulation, you'll also need to ensure that the BodyIDs are kept the same, so you need to remove/add the body from/to the physics system instead of destroy/re-create them or you need to create bodies with the same ID on both sides using [BodyInterface::CreateBodyWithID](@ref BodyInterface::CreateBodyWithID).
 
+## Working With Multiple Physics Systems
+
+You can create, simulate and interact with multiple PhysicsSystems at the same time provided that you do not share any objects (bodies, constraints) between the systems. 
+When a Body is created it receives a BodyID that is unique for the PhysicsSystem that it was created for, so it cannot be shared. The only object that can be shared between PhysicsSystems is a Shape. 
+If you want to move a body from one PhysicsSystem to another, use Body::GetBodyCreationSettings to get the settings needed to create the body in the other PhysicsSystem.
+
+PhysicsSystems are not completely independent:
+
+* There is only 1 RTTI factory (Factory::sInstance).
+* There is only 1 default material (PhysicsMaterial::sDefault).
+* There is only 1 debug renderer (DebugRenderer::sInstance) although many functions take a custom DebugRenderer for drawing.
+* Custom shapes and CollisionDispatch functions are shared.
+* The custom memory allocation functions (e.g. Allocate), Trace and AssertFailed functions are shared.
+
+These functions / systems need to be registered in advance.
+
 ## The Simulation Step in Detail
 
 The job graph looks like this:

Jolt/Physics/Body/BodyAccess.h

@@ -24,6 +24,9 @@ public:
 	public:
 		inline							Grant(EAccess inVelocity, EAccess inPosition)
 		{
+			JPH_ASSERT(sVelocityAccess == EAccess::ReadWrite);
+			JPH_ASSERT(sPositionAccess == EAccess::ReadWrite);
+
 			sVelocityAccess = inVelocity;
 			sPositionAccess = inPosition;
 		}

Jolt/Physics/Body/BodyLockInterface.h

@@ -82,25 +82,25 @@ public:
 	virtual SharedMutex *		LockRead(const BodyID &inBodyID) const override
 	{
 		SharedMutex &mutex = mBodyManager.GetMutexForBody(inBodyID);
-		PhysicsLock::sLockShared(mutex, EPhysicsLockTypes::PerBody);
+		PhysicsLock::sLockShared(mutex JPH_IF_ENABLE_ASSERTS(, &mBodyManager, EPhysicsLockTypes::PerBody));
 		return &mutex;
 	}
 
 	virtual void				UnlockRead(SharedMutex *inMutex) const override
 	{
-		PhysicsLock::sUnlockShared(*inMutex, EPhysicsLockTypes::PerBody);
+		PhysicsLock::sUnlockShared(*inMutex JPH_IF_ENABLE_ASSERTS(, &mBodyManager, EPhysicsLockTypes::PerBody));
 	}
 	
 	virtual SharedMutex *		LockWrite(const BodyID &inBodyID) const override
 	{
 		SharedMutex &mutex = mBodyManager.GetMutexForBody(inBodyID);
-		PhysicsLock::sLock(mutex, EPhysicsLockTypes::PerBody);
+		PhysicsLock::sLock(mutex JPH_IF_ENABLE_ASSERTS(, &mBodyManager, EPhysicsLockTypes::PerBody));
 		return &mutex;
 	}
 
 	virtual void				UnlockWrite(SharedMutex *inMutex) const override
 	{
-		PhysicsLock::sUnlock(*inMutex, EPhysicsLockTypes::PerBody);
+		PhysicsLock::sUnlock(*inMutex JPH_IF_ENABLE_ASSERTS(, &mBodyManager, EPhysicsLockTypes::PerBody));
 	}
 
 	///@name Batch locking functions

Jolt/Physics/Body/BodyManager.cpp

@@ -45,7 +45,7 @@ inline void BodyManager::sDeleteBody(Body *inBody)
 
 BodyManager::~BodyManager()
 {
-	UniqueLock lock(mBodiesMutex, EPhysicsLockTypes::BodiesList);
+	UniqueLock lock(mBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::BodiesList));
 
 	// Destroy any bodies that are still alive
 	for (Body *b : mBodies)
@@ -57,7 +57,7 @@ BodyManager::~BodyManager()
 
 void BodyManager::Init(uint inMaxBodies, uint inNumBodyMutexes, const BroadPhaseLayerInterface &inLayerInterface)
 {
-	UniqueLock lock(mBodiesMutex, EPhysicsLockTypes::BodiesList);
+	UniqueLock lock(mBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::BodiesList));
 
 	// Num body mutexes must be a power of two and not bigger than our MutexMask
 	uint num_body_mutexes = Clamp<uint>(GetNextPowerOf2(inNumBodyMutexes == 0? 2 * thread::hardware_concurrency() : inNumBodyMutexes), 1, sizeof(MutexMask) * 8);
@@ -81,14 +81,14 @@ void BodyManager::Init(uint inMaxBodies, uint inNumBodyMutexes, const BroadPhase
 
 uint BodyManager::GetNumBodies() const
 {
-	UniqueLock lock(mBodiesMutex, EPhysicsLockTypes::BodiesList);
+	UniqueLock lock(mBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::BodiesList));
 
 	return mNumBodies;
 }
 
 BodyManager::BodyStats BodyManager::GetBodyStats() const
 {
-	UniqueLock lock(mBodiesMutex, EPhysicsLockTypes::BodiesList);
+	UniqueLock lock(mBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::BodiesList));
 
 	BodyStats stats;
 	stats.mNumBodies = mNumBodies;
@@ -185,7 +185,7 @@ bool BodyManager::AddBody(Body *ioBody)
 	// Determine next free index
 	uint32 idx;
 	{
-		UniqueLock lock(mBodiesMutex, EPhysicsLockTypes::BodiesList);
+		UniqueLock lock(mBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::BodiesList));
 
 		if (mBodyIDFreeListStart != cBodyIDFreeListEnd)
 		{
@@ -228,7 +228,7 @@ bool BodyManager::AddBodyWithCustomID(Body *ioBody, const BodyID &inBodyID)
 		return false;
 
 	{
-		UniqueLock lock(mBodiesMutex, EPhysicsLockTypes::BodiesList);
+		UniqueLock lock(mBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::BodiesList));
 
 		// Check if index is beyond the max body ID
 		uint32 idx = inBodyID.GetIndex();
@@ -331,7 +331,7 @@ void BodyManager::RemoveBodies(const BodyID *inBodyIDs, int inNumber, Body **out
 	if (inNumber <= 0)
 		return;
 
-	UniqueLock lock(mBodiesMutex, EPhysicsLockTypes::BodiesList);
+	UniqueLock lock(mBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::BodiesList));
 
 	// Update cached number of bodies
 	JPH_ASSERT(mNumBodies >= (uint)inNumber);
@@ -364,7 +364,7 @@ void BodyManager::DestroyBodies(const BodyID *inBodyIDs, int inNumber)
 	if (inNumber <= 0)
 		return;
 
-	UniqueLock lock(mBodiesMutex, EPhysicsLockTypes::BodiesList);
+	UniqueLock lock(mBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::BodiesList));
 
 	// Update cached number of bodies
 	JPH_ASSERT(mNumBodies >= (uint)inNumber);
@@ -390,8 +390,8 @@ void BodyManager::ActivateBodies(const BodyID *inBodyIDs, int inNumber)
 	if (inNumber <= 0)
 		return;
 
-	UniqueLock lock(mActiveBodiesMutex, EPhysicsLockTypes::ActiveBodiesList);
-
+	UniqueLock lock(mActiveBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::ActiveBodiesList));
+	
 	JPH_ASSERT(!mActiveBodiesLocked || sOverrideAllowActivation);
 
 	for (const BodyID *b = inBodyIDs, *b_end = inBodyIDs + inNumber; b < b_end; b++)
@@ -430,7 +430,7 @@ void BodyManager::DeactivateBodies(const BodyID *inBodyIDs, int inNumber)
 	if (inNumber <= 0)
 		return;
 
-	UniqueLock lock(mActiveBodiesMutex, EPhysicsLockTypes::ActiveBodiesList);
+	UniqueLock lock(mActiveBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::ActiveBodiesList));
 
 	JPH_ASSERT(!mActiveBodiesLocked || sOverrideAllowDeactivation);
 
@@ -487,7 +487,7 @@ void BodyManager::SetMotionQuality(Body &ioBody, EMotionQuality inMotionQuality)
 	MotionProperties *mp = ioBody.GetMotionPropertiesUnchecked();
 	if (mp != nullptr && mp->GetMotionQuality() != inMotionQuality)
 	{
-		UniqueLock lock(mActiveBodiesMutex, EPhysicsLockTypes::ActiveBodiesList);
+		UniqueLock lock(mActiveBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::ActiveBodiesList));
 
 		JPH_ASSERT(!mActiveBodiesLocked);
 
@@ -506,7 +506,7 @@ void BodyManager::GetActiveBodies(BodyIDVector &outBodyIDs) const
 {
 	JPH_PROFILE_FUNCTION();
 
-	UniqueLock lock(mActiveBodiesMutex, EPhysicsLockTypes::ActiveBodiesList);
+	UniqueLock lock(mActiveBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::ActiveBodiesList));
 
 	outBodyIDs.assign(mActiveBodies, mActiveBodies + mNumActiveBodies);
 }
@@ -515,7 +515,7 @@ void BodyManager::GetBodyIDs(BodyIDVector &outBodies) const
 {
 	JPH_PROFILE_FUNCTION();
 
-	UniqueLock lock(mBodiesMutex, EPhysicsLockTypes::BodiesList);
+	UniqueLock lock(mBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::BodiesList));
 
 	// Reserve space for all bodies
 	outBodies.clear();
@@ -532,7 +532,7 @@ void BodyManager::GetBodyIDs(BodyIDVector &outBodies) const
 
 void BodyManager::SetBodyActivationListener(BodyActivationListener *inListener)	
 { 
-	UniqueLock lock(mActiveBodiesMutex, EPhysicsLockTypes::ActiveBodiesList);
+	UniqueLock lock(mActiveBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::ActiveBodiesList));
 
 	mActivationListener = inListener; 
 }
@@ -561,7 +561,7 @@ BodyManager::MutexMask BodyManager::GetMutexMask(const BodyID *inBodies, int inN
 
 void BodyManager::LockRead(MutexMask inMutexMask) const
 {
-	JPH_IF_ENABLE_ASSERTS(PhysicsLock::sCheckLock(EPhysicsLockTypes::PerBody));
+	JPH_IF_ENABLE_ASSERTS(PhysicsLock::sCheckLock(this, EPhysicsLockTypes::PerBody));
 
 	int index = 0;
 	for (MutexMask mask = inMutexMask; mask != 0; mask >>= 1, index++)
@@ -571,7 +571,7 @@ void BodyManager::LockRead(MutexMask inMutexMask) const
 
 void BodyManager::UnlockRead(MutexMask inMutexMask) const
 {
-	JPH_IF_ENABLE_ASSERTS(PhysicsLock::sCheckUnlock(EPhysicsLockTypes::PerBody));
+	JPH_IF_ENABLE_ASSERTS(PhysicsLock::sCheckUnlock(this, EPhysicsLockTypes::PerBody));
 
 	int index = 0;
 	for (MutexMask mask = inMutexMask; mask != 0; mask >>= 1, index++)
@@ -581,7 +581,7 @@ void BodyManager::UnlockRead(MutexMask inMutexMask) const
 
 void BodyManager::LockWrite(MutexMask inMutexMask) const
 {
-	JPH_IF_ENABLE_ASSERTS(PhysicsLock::sCheckLock(EPhysicsLockTypes::PerBody));
+	JPH_IF_ENABLE_ASSERTS(PhysicsLock::sCheckLock(this, EPhysicsLockTypes::PerBody));
 
 	int index = 0;
 	for (MutexMask mask = inMutexMask; mask != 0; mask >>= 1, index++)
@@ -591,7 +591,7 @@ void BodyManager::LockWrite(MutexMask inMutexMask) const
 
 void BodyManager::UnlockWrite(MutexMask inMutexMask) const
 {
-	JPH_IF_ENABLE_ASSERTS(PhysicsLock::sCheckUnlock(EPhysicsLockTypes::PerBody));
+	JPH_IF_ENABLE_ASSERTS(PhysicsLock::sCheckUnlock(this, EPhysicsLockTypes::PerBody));
 
 	int index = 0;
 	for (MutexMask mask = inMutexMask; mask != 0; mask >>= 1, index++)
@@ -601,17 +601,17 @@ void BodyManager::UnlockWrite(MutexMask inMutexMask) const
 
 void BodyManager::LockAllBodies() const						
 { 
-	JPH_IF_ENABLE_ASSERTS(PhysicsLock::sCheckLock(EPhysicsLockTypes::PerBody));
+	JPH_IF_ENABLE_ASSERTS(PhysicsLock::sCheckLock(this, EPhysicsLockTypes::PerBody));
 	mBodyMutexes.LockAll(); 
 
-	PhysicsLock::sLock(mBodiesMutex, EPhysicsLockTypes::BodiesList);
+	PhysicsLock::sLock(mBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::BodiesList));
 }
 
 void BodyManager::UnlockAllBodies() const						
 { 
-	PhysicsLock::sUnlock(mBodiesMutex, EPhysicsLockTypes::BodiesList);
+	PhysicsLock::sUnlock(mBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::BodiesList));
 
-	JPH_IF_ENABLE_ASSERTS(PhysicsLock::sCheckUnlock(EPhysicsLockTypes::PerBody));
+	JPH_IF_ENABLE_ASSERTS(PhysicsLock::sCheckUnlock(this, EPhysicsLockTypes::PerBody));
 	mBodyMutexes.UnlockAll(); 
 }
 
@@ -639,7 +639,7 @@ void BodyManager::SaveState(StateRecorder &inStream) const
 	}
 
 	{
-		UniqueLock lock(mActiveBodiesMutex, EPhysicsLockTypes::ActiveBodiesList);
+		UniqueLock lock(mActiveBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::ActiveBodiesList));
 
 		// Write active bodies, sort because activation can come from multiple threads, so order is not deterministic
 		inStream.Write(mNumActiveBodies);
@@ -689,7 +689,7 @@ bool BodyManager::RestoreState(StateRecorder &inStream)
 	}
 
 	{
-		UniqueLock lock(mActiveBodiesMutex, EPhysicsLockTypes::ActiveBodiesList);
+		UniqueLock lock(mActiveBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::ActiveBodiesList));
 
 		// Mark current active bodies as deactivated
 		for (const BodyID *id = mActiveBodies, *id_end = mActiveBodies + mNumActiveBodies; id < id_end; ++id)
@@ -915,7 +915,7 @@ void BodyManager::ValidateContactCacheForAllBodies()
 #ifdef _DEBUG
 void BodyManager::ValidateActiveBodyBounds()
 {
-	UniqueLock lock(mActiveBodiesMutex, EPhysicsLockTypes::ActiveBodiesList);
+	UniqueLock lock(mActiveBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::ActiveBodiesList));
 
 	for (BodyID *id = mActiveBodies, *id_end = mActiveBodies + mNumActiveBodies; id < id_end; ++id)
 	{

Jolt/Physics/Collision/BroadPhase/BroadPhaseQuadTree.cpp

@@ -6,7 +6,6 @@
 #include <Jolt/Physics/Collision/RayCast.h>
 #include <Jolt/Physics/Collision/AABoxCast.h>
 #include <Jolt/Physics/Collision/CastResult.h>
-#include <Jolt/Physics/PhysicsLock.h>
 #include <Jolt/Core/QuickSort.h>
 
 JPH_NAMESPACE_BEGIN
@@ -25,6 +24,11 @@ void BroadPhaseQuadTree::Init(BodyManager *inBodyManager, const BroadPhaseLayerI
 	mNumLayers = inLayerInterface.GetNumBroadPhaseLayers();
 	JPH_ASSERT(mNumLayers < (BroadPhaseLayer::Type)cBroadPhaseLayerInvalid);
 
+#ifdef JPH_ENABLE_ASSERTS
+	// Store lock context
+	mLockContext = inBodyManager;
+#endif // JPH_ENABLE_ASSERTS
+
 	// Store max bodies
 	mMaxBodies = inBodyManager->GetMaxBodies();
 
@@ -58,7 +62,7 @@ void BroadPhaseQuadTree::FrameSync()
 	// Note that nothing should be locked at this point to avoid risking a lock inversion deadlock.
 	// Note that in other places where we lock this mutex we don't use SharedLock to detect lock inversions. As long as
 	// nothing else is locked this is safe. This is why BroadPhaseQuery should be the highest priority lock.
-	UniqueLock root_lock(mQueryLocks[mQueryLockIdx ^ 1], EPhysicsLockTypes::BroadPhaseQuery);
+	UniqueLock root_lock(mQueryLocks[mQueryLockIdx ^ 1] JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::BroadPhaseQuery));
 
 	for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l)
 		mLayers[l].DiscardOldTree();
@@ -91,7 +95,7 @@ void BroadPhaseQuadTree::Optimize()
 void BroadPhaseQuadTree::LockModifications()
 {
 	// From this point on we prevent modifications to the tree
-	PhysicsLock::sLock(mUpdateMutex, EPhysicsLockTypes::BroadPhaseUpdate);
+	PhysicsLock::sLock(mUpdateMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::BroadPhaseUpdate));
 }
 
 BroadPhase::UpdateState BroadPhaseQuadTree::UpdatePrepare()
@@ -143,7 +147,7 @@ void BroadPhaseQuadTree::UpdateFinalize(const UpdateState &inUpdateState)
 void BroadPhaseQuadTree::UnlockModifications()
 {
 	// From this point on we allow modifications to the tree again
-	PhysicsLock::sUnlock(mUpdateMutex, EPhysicsLockTypes::BroadPhaseUpdate);
+	PhysicsLock::sUnlock(mUpdateMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::BroadPhaseUpdate));
 }
 
 BroadPhase::AddState BroadPhaseQuadTree::AddBodiesPrepare(BodyID *ioBodies, int inNumber) 
@@ -204,7 +208,7 @@ void BroadPhaseQuadTree::AddBodiesFinalize(BodyID *ioBodies, int inNumber, AddSt
 	JPH_PROFILE_FUNCTION();
 
 	// This cannot run concurrently with UpdatePrepare()/UpdateFinalize()
-	SharedLock lock(mUpdateMutex, EPhysicsLockTypes::BroadPhaseUpdate);
+	SharedLock lock(mUpdateMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::BroadPhaseUpdate));
 
 	BodyVector &bodies = mBodyManager->GetBodies();
 	JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies());
@@ -274,7 +278,7 @@ void BroadPhaseQuadTree::RemoveBodies(BodyID *ioBodies, int inNumber)
 	JPH_PROFILE_FUNCTION();
 
 	// This cannot run concurrently with UpdatePrepare()/UpdateFinalize()
-	SharedLock lock(mUpdateMutex, EPhysicsLockTypes::BroadPhaseUpdate);
+	SharedLock lock(mUpdateMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::BroadPhaseUpdate));
 
 	JPH_ASSERT(inNumber > 0);
 
@@ -324,7 +328,7 @@ void BroadPhaseQuadTree::NotifyBodiesAABBChanged(BodyID *ioBodies, int inNumber,
 
 	// This cannot run concurrently with UpdatePrepare()/UpdateFinalize()
 	if (inTakeLock)
-		PhysicsLock::sLockShared(mUpdateMutex, EPhysicsLockTypes::BroadPhaseUpdate);
+		PhysicsLock::sLockShared(mUpdateMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::BroadPhaseUpdate));
 	else
 		JPH_ASSERT(mUpdateMutex.is_locked());
 
@@ -353,7 +357,7 @@ void BroadPhaseQuadTree::NotifyBodiesAABBChanged(BodyID *ioBodies, int inNumber,
 	}
 
 	if (inTakeLock)
-		PhysicsLock::sUnlockShared(mUpdateMutex, EPhysicsLockTypes::BroadPhaseUpdate);
+		PhysicsLock::sUnlockShared(mUpdateMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::BroadPhaseUpdate));
 }
 
 void BroadPhaseQuadTree::NotifyBodiesLayerChanged(BodyID *ioBodies, int inNumber)

Jolt/Physics/Collision/BroadPhase/BroadPhaseQuadTree.h

@@ -5,6 +5,7 @@
 
 #include <Jolt/Physics/Collision/BroadPhase/QuadTree.h>
 #include <Jolt/Physics/Collision/BroadPhase/BroadPhase.h>
+#include <Jolt/Physics/PhysicsLock.h>
 
 JPH_NAMESPACE_BEGIN
 
@@ -57,6 +58,11 @@ private:
 	using Tracking = QuadTree::Tracking;
 	using TrackingVector = QuadTree::TrackingVector;
 
+#ifdef JPH_ENABLE_ASSERTS
+	/// Context used to lock a physics lock
+	PhysicsLockContext		mLockContext = nullptr;
+#endif // JPH_ENABLE_ASSERTS
+
 	/// Max amount of bodies we support
 	size_t					mMaxBodies = 0;
 

Jolt/Physics/Constraints/ConstraintManager.cpp

@@ -14,7 +14,7 @@ JPH_NAMESPACE_BEGIN
 
 void ConstraintManager::Add(Constraint **inConstraints, int inNumber)						
 { 
-	UniqueLock lock(mConstraintsMutex, EPhysicsLockTypes::ConstraintsList);
+	UniqueLock lock(mConstraintsMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::ConstraintsList));
 
 	mConstraints.reserve(mConstraints.size() + inNumber);
 
@@ -33,7 +33,7 @@ void ConstraintManager::Add(Constraint **inConstraints, int inNumber)
 
 void ConstraintManager::Remove(Constraint **inConstraints, int inNumber)
 {
-	UniqueLock lock(mConstraintsMutex, EPhysicsLockTypes::ConstraintsList);
+	UniqueLock lock(mConstraintsMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::ConstraintsList));
 
 	for (Constraint **c = inConstraints, **c_end = inConstraints + inNumber; c < c_end; ++c)
 	{
@@ -60,7 +60,7 @@ void ConstraintManager::Remove(Constraint **inConstraints, int inNumber)
 
 Constraints ConstraintManager::GetConstraints() const
 {
-	UniqueLock lock(mConstraintsMutex, EPhysicsLockTypes::ConstraintsList);
+	UniqueLock lock(mConstraintsMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::ConstraintsList));
 
 	Constraints copy = mConstraints;
 	return copy;
@@ -187,7 +187,7 @@ void ConstraintManager::DrawConstraints(DebugRenderer *inRenderer) const
 {
 	JPH_PROFILE_FUNCTION();
 
-	UniqueLock lock(mConstraintsMutex, EPhysicsLockTypes::ConstraintsList);
+	UniqueLock lock(mConstraintsMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::ConstraintsList));
 
 	for (const Ref<Constraint> &c : mConstraints)			
 		c->DrawConstraint(inRenderer);
@@ -197,7 +197,7 @@ void ConstraintManager::DrawConstraintLimits(DebugRenderer *inRenderer) const
 {
 	JPH_PROFILE_FUNCTION();
 
-	UniqueLock lock(mConstraintsMutex, EPhysicsLockTypes::ConstraintsList);
+	UniqueLock lock(mConstraintsMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::ConstraintsList));
 
 	for (const Ref<Constraint> &c : mConstraints)
 		c->DrawConstraintLimits(inRenderer);
@@ -207,7 +207,7 @@ void ConstraintManager::DrawConstraintReferenceFrame(DebugRenderer *inRenderer)
 {
 	JPH_PROFILE_FUNCTION();
 
-	UniqueLock lock(mConstraintsMutex, EPhysicsLockTypes::ConstraintsList);
+	UniqueLock lock(mConstraintsMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::ConstraintsList));
 
 	for (const Ref<Constraint> &c : mConstraints)
 		c->DrawConstraintReferenceFrame(inRenderer);
@@ -216,7 +216,7 @@ void ConstraintManager::DrawConstraintReferenceFrame(DebugRenderer *inRenderer)
 
 void ConstraintManager::SaveState(StateRecorder &inStream) const
 {	
-	UniqueLock lock(mConstraintsMutex, EPhysicsLockTypes::ConstraintsList);
+	UniqueLock lock(mConstraintsMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::ConstraintsList));
 
 	// Write state of constraints
 	size_t num_constraints = mConstraints.size();
@@ -227,7 +227,7 @@ void ConstraintManager::SaveState(StateRecorder &inStream) const
 
 bool ConstraintManager::RestoreState(StateRecorder &inStream)
 {
-	UniqueLock lock(mConstraintsMutex, EPhysicsLockTypes::ConstraintsList);
+	UniqueLock lock(mConstraintsMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::ConstraintsList));
 
 	// Read state of constraints
 	size_t num_constraints = mConstraints.size(); // Initialize to current value for validation

Jolt/Physics/Constraints/ConstraintManager.h

@@ -24,6 +24,11 @@ class ConstraintManager : public NonCopyable
 public:
 	JPH_OVERRIDE_NEW_DELETE
 
+#ifdef JPH_ENABLE_ASSERTS
+	/// Constructor
+							ConstraintManager(PhysicsLockContext inContext) : mLockContext(inContext) { }
+#endif // JPH_ENABLE_ASSERTS
+
 	/// Add a new constraint. This is thread safe.
 	/// Note that the inConstraints array is allowed to have nullptrs, these will be ignored.
 	void					Add(Constraint **inConstraints, int inNumber);
@@ -83,10 +88,13 @@ public:
 	bool					RestoreState(StateRecorder &inStream);
 
 	/// Lock all constraints. This should only be done during PhysicsSystem::Update().
-	void					LockAllConstraints()						{ PhysicsLock::sLock(mConstraintsMutex, EPhysicsLockTypes::ConstraintsList); }
-	void					UnlockAllConstraints()						{ PhysicsLock::sUnlock(mConstraintsMutex, EPhysicsLockTypes::ConstraintsList); }
+	void					LockAllConstraints()						{ PhysicsLock::sLock(mConstraintsMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::ConstraintsList)); }
+	void					UnlockAllConstraints()						{ PhysicsLock::sUnlock(mConstraintsMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::ConstraintsList)); }
 
 private:
+#ifdef JPH_ENABLE_ASSERTS
+	PhysicsLockContext		mLockContext;
+#endif // JPH_ENABLE_ASSERTS
 	Constraints				mConstraints;
 	mutable Mutex			mConstraintsMutex;
 };

Jolt/Physics/PhysicsLock.cpp

@@ -9,7 +9,7 @@
 
 JPH_NAMESPACE_BEGIN
 
-thread_local uint32 PhysicsLock::sLockedMutexes = 0;
+thread_local PhysicsLock::LockData PhysicsLock::sLocks[4];
 
 JPH_NAMESPACE_END
 

Jolt/Physics/PhysicsLock.h

@@ -7,6 +7,8 @@
 
 JPH_NAMESPACE_BEGIN
 
+#ifdef JPH_ENABLE_ASSERTS
+
 /// This is the list of locks used by the physics engine, they need to be locked in a particular order (from top of the list to bottom of the list) in order to prevent deadlocks
 enum class EPhysicsLockTypes
 {
@@ -18,6 +20,12 @@ enum class EPhysicsLockTypes
 	ActiveBodiesList		= 1 << 5,
 };
 
+/// A token that indicates the context of a lock (we use 1 per physics system and we use the body manager pointer because it's convenient)
+class BodyManager;
+using PhysicsLockContext = const BodyManager *;
+
+#endif // !JPH_ENABLE_ASSERTS
+
 /// Helpers to safely lock the different mutexes that are part of the physics system while preventing deadlock
 /// Class that keeps track per thread which lock are taken and if the order of locking is correct
 class PhysicsLock
@@ -25,51 +33,79 @@ class PhysicsLock
 public:
 #ifdef JPH_ENABLE_ASSERTS
 	/// Call before taking the lock
-	static inline void			sCheckLock(EPhysicsLockTypes inType)
+	static inline void			sCheckLock(PhysicsLockContext inContext, EPhysicsLockTypes inType)
 	{
-		JPH_ASSERT((uint32)inType > sLockedMutexes, "A lock of same or higher priority was already taken, this can create a deadlock!");
-		sLockedMutexes = sLockedMutexes | (uint32)inType;
+		uint32 &mutexes = sGetLockedMutexes(inContext);
+		JPH_ASSERT((uint32)inType > mutexes, "A lock of same or higher priority was already taken, this can create a deadlock!");
+		mutexes = mutexes | (uint32)inType;
 	}
 
 	/// Call after releasing the lock
-	static inline void			sCheckUnlock(EPhysicsLockTypes inType)
+	static inline void			sCheckUnlock(PhysicsLockContext inContext, EPhysicsLockTypes inType)
 	{
-		JPH_ASSERT((sLockedMutexes & (uint32)inType) != 0, "Mutex was not locked!");
-		sLockedMutexes = sLockedMutexes & ~(uint32)inType;
+		uint32 &mutexes = sGetLockedMutexes(inContext);
+		JPH_ASSERT((mutexes & (uint32)inType) != 0, "Mutex was not locked!");
+		mutexes = mutexes & ~(uint32)inType;
 	}
 #endif // !JPH_ENABLE_ASSERTS
 
 	template <class LockType>
-	static inline void			sLock(LockType &inMutex, [[maybe_unused]] EPhysicsLockTypes inType)
+	static inline void			sLock(LockType &inMutex JPH_IF_ENABLE_ASSERTS(, PhysicsLockContext inContext, EPhysicsLockTypes inType))
 	{
-		JPH_IF_ENABLE_ASSERTS(sCheckLock(inType);)
+		JPH_IF_ENABLE_ASSERTS(sCheckLock(inContext, inType);)
 		inMutex.lock();
 	}
 
 	template <class LockType>
-	static inline void			sUnlock(LockType &inMutex, [[maybe_unused]] EPhysicsLockTypes inType)
+	static inline void			sUnlock(LockType &inMutex JPH_IF_ENABLE_ASSERTS(, PhysicsLockContext inContext, EPhysicsLockTypes inType))
 	{
-		JPH_IF_ENABLE_ASSERTS(sCheckUnlock(inType);)
+		JPH_IF_ENABLE_ASSERTS(sCheckUnlock(inContext, inType);)
 		inMutex.unlock();
 	}
 
 	template <class LockType>
-	static inline void			sLockShared(LockType &inMutex, [[maybe_unused]] EPhysicsLockTypes inType)
+	static inline void			sLockShared(LockType &inMutex JPH_IF_ENABLE_ASSERTS(, PhysicsLockContext inContext, EPhysicsLockTypes inType))
 	{
-		JPH_IF_ENABLE_ASSERTS(sCheckLock(inType);)
+		JPH_IF_ENABLE_ASSERTS(sCheckLock(inContext, inType);)
 		inMutex.lock_shared();
 	}
 
 	template <class LockType>
-	static inline void			sUnlockShared(LockType &inMutex, [[maybe_unused]] EPhysicsLockTypes inType)
+	static inline void			sUnlockShared(LockType &inMutex JPH_IF_ENABLE_ASSERTS(, PhysicsLockContext inContext, EPhysicsLockTypes inType))
 	{
-		JPH_IF_ENABLE_ASSERTS(sCheckUnlock(inType);)
+		JPH_IF_ENABLE_ASSERTS(sCheckUnlock(inContext, inType);)
 		inMutex.unlock_shared();
 	}
 
 #ifdef JPH_ENABLE_ASSERTS
 private:
-	static thread_local uint32	sLockedMutexes;
+	struct LockData
+	{
+		uint32					mLockedMutexes = 0;
+		PhysicsLockContext 		mContext = nullptr;
+	};
+
+	static thread_local LockData sLocks[4];
+
+	// Helper function to find the locked mutexes for a particular context
+	static uint32 &				sGetLockedMutexes(PhysicsLockContext inContext)
+	{
+		// If we find a matching context we can use it
+		for (LockData &l : sLocks)
+			if (l.mContext == inContext)
+				return l.mLockedMutexes;
+
+		// Otherwise we look for an entry that is not in use
+		for (LockData &l : sLocks)
+			if (l.mLockedMutexes == 0)
+			{
+				l.mContext = inContext;
+				return l.mLockedMutexes;
+			}
+
+		JPH_ASSERT(false, "Too many physics systems locked at the same time!");
+		return sLocks[0].mLockedMutexes;
+	}
 #endif // !JPH_ENABLE_ASSERTS
 };
 
@@ -78,12 +114,27 @@ template <class LockType>
 class UniqueLock : public NonCopyable
 {
 public:
-								UniqueLock(LockType &inLock, EPhysicsLockTypes inType)		: mLock(inLock), mType(inType) { PhysicsLock::sLock(mLock, mType); }
-								~UniqueLock()												{ PhysicsLock::sUnlock(mLock, mType); }
+	explicit					UniqueLock(LockType &inLock JPH_IF_ENABLE_ASSERTS(, PhysicsLockContext inContext, EPhysicsLockTypes inType)) :
+		mLock(inLock)
+#ifdef JPH_ENABLE_ASSERTS
+		, mContext(inContext),
+		mType(inType)
+#endif // JPH_ENABLE_ASSERTS
+	{
+		PhysicsLock::sLock(mLock JPH_IF_ENABLE_ASSERTS(, mContext, mType));
+	}
+								
+								~UniqueLock()
+	{
+		PhysicsLock::sUnlock(mLock JPH_IF_ENABLE_ASSERTS(, mContext, mType));
+	}
 
 private:
 	LockType &					mLock;
+#ifdef JPH_ENABLE_ASSERTS
+	PhysicsLockContext 			mContext;
 	EPhysicsLockTypes			mType;
+#endif // JPH_ENABLE_ASSERTS
 };
 
 /// Helper class that is similar to std::shared_lock
@@ -91,12 +142,27 @@ template <class LockType>
 class SharedLock : public NonCopyable
 {
 public:
-								SharedLock(LockType &inLock, EPhysicsLockTypes inType)		: mLock(inLock), mType(inType) { PhysicsLock::sLockShared(mLock, mType); }
-								~SharedLock()												{ PhysicsLock::sUnlockShared(mLock, mType); }
+	explicit					SharedLock(LockType &inLock JPH_IF_ENABLE_ASSERTS(, PhysicsLockContext inContext, EPhysicsLockTypes inType)) :
+		mLock(inLock)
+#ifdef JPH_ENABLE_ASSERTS
+		, mContext(inContext)
+		, mType(inType)
+#endif // JPH_ENABLE_ASSERTS
+	{
+		PhysicsLock::sLockShared(mLock JPH_IF_ENABLE_ASSERTS(, mContext, mType));
+	}
+								
+								~SharedLock()
+	{
+		PhysicsLock::sUnlockShared(mLock JPH_IF_ENABLE_ASSERTS(, mContext, mType));
+	}
 
 private:
 	LockType &					mLock;
+#ifdef JPH_ENABLE_ASSERTS
+	PhysicsLockContext 			mContext;
 	EPhysicsLockTypes			mType;
+#endif // JPH_ENABLE_ASSERTS
 };
 
 JPH_NAMESPACE_END

Jolt/Physics/PhysicsSystem.cpp

@@ -801,8 +801,11 @@ void PhysicsSystem::TrySpawnJobFindCollisions(PhysicsUpdateContext::Step *ioStep
 void PhysicsSystem::JobFindCollisions(PhysicsUpdateContext::Step *ioStep, int inJobIndex)
 {
 #ifdef JPH_ENABLE_ASSERTS
-	// We only read positions
-	BodyAccess::Grant grant(BodyAccess::EAccess::None, BodyAccess::EAccess::Read);
+	// We read positions and read velocities (for elastic collisions)
+	BodyAccess::Grant grant(BodyAccess::EAccess::Read, BodyAccess::EAccess::Read);
+
+	// Can only activate bodies
+	BodyManager::GrantActiveBodiesAccess grant_active(true, false);
 #endif
 
 	// Allocation context for allocating new contact points
@@ -933,14 +936,6 @@ void PhysicsSystem::ProcessBodyPair(ContactAllocator &ioContactAllocator, const
 {
 	JPH_PROFILE_FUNCTION();
 
-#ifdef JPH_ENABLE_ASSERTS
-	// We read positions and read velocities (for elastic collisions)
-	BodyAccess::Grant grant(BodyAccess::EAccess::Read, BodyAccess::EAccess::Read);
-
-	// Can only activate bodies
-	BodyManager::GrantActiveBodiesAccess grant_active(true, false);
-#endif
-
 	// Fetch body pair
 	Body *body1 = &mBodyManager.GetBody(inBodyPair.mBodyA);
 	Body *body2 = &mBodyManager.GetBody(inBodyPair.mBodyB);

Jolt/Physics/PhysicsSystem.h

@@ -28,7 +28,7 @@ public:
 	JPH_OVERRIDE_NEW_DELETE
 
 	/// Constructor / Destructor
-								PhysicsSystem()												: mContactManager(mPhysicsSettings) { }
+								PhysicsSystem()												: mContactManager(mPhysicsSettings) JPH_IF_ENABLE_ASSERTS(, mConstraintManager(&mBodyManager)) { }
 								~PhysicsSystem();
 
 	/// Initialize the system.

UnitTests/Physics/PhysicsTests.cpp

@@ -1260,4 +1260,45 @@ TEST_SUITE("PhysicsTests")
 		CHECK_APPROX_EQUAL(lq_debris1.GetPosition(), RVec3(0, 0.5f, 0), slop);
 		CHECK_APPROX_EQUAL(lq_debris2.GetPosition(), RVec3(0, 0.5f, 0), slop);
 	}
+
+	TEST_CASE("TestMultiplePhysicsSystems")
+	{
+		PhysicsTestContext c1(1.0f / 60.0f, 1, 1);
+		c1.ZeroGravity();
+		PhysicsTestContext c2(1.0f / 60.0f, 1, 1);
+		c2.ZeroGravity();
+
+		const RVec3 cBox1Position(1.0f, 2.0f, 3.0f);
+		Body &box1 = c1.CreateBox(cBox1Position, Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3::sReplicate(1.0f), EActivation::Activate);
+
+		const RVec3 cBox2Position(4.0f, 5.0f, 6.0f);
+		Body& box2 = c2.CreateBox(cBox2Position, Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3::sReplicate(1.0f), EActivation::Activate);
+
+		const Vec3 cBox1Velocity(1.0f, 0, 0);
+		const Vec3 cBox2Velocity(2.0f, 0, 0);
+		{
+			// This tests if we can lock bodies from multiple physics systems (normally locking 2 bodies at the same time without using BodyLockMultiWrite would trigger an assert)
+			BodyLockWrite lock1(c1.GetSystem()->GetBodyLockInterface(), box1.GetID());
+			BodyLockWrite lock2(c2.GetSystem()->GetBodyLockInterface(), box2.GetID());
+
+			CHECK(lock1.GetBody().GetPosition() == cBox1Position);
+			CHECK(lock2.GetBody().GetPosition() == cBox2Position);
+
+			lock1.GetBody().SetLinearVelocity(cBox1Velocity);
+			lock2.GetBody().SetLinearVelocity(cBox2Velocity);
+		}
+
+		const float cTime = 1.0f;
+		c1.Simulate(cTime);
+		c2.Simulate(cTime);
+
+		{
+			BodyLockRead lock1(c1.GetSystem()->GetBodyLockInterface(), box1.GetID());
+			BodyLockRead lock2(c2.GetSystem()->GetBodyLockInterface(), box2.GetID());
+
+			// Check that the bodies in the different systems updated correctly
+			CHECK_APPROX_EQUAL(lock1.GetBody().GetPosition(), cBox1Position + cBox1Velocity * cTime, 1.0e-5f);
+			CHECK_APPROX_EQUAL(lock2.GetBody().GetPosition(), cBox2Position + cBox2Velocity * cTime, 1.0e-5f);
+		}		
+	}
 }