Slightly safer / cheaper way of invalidating the contact cache of a body

Jolt/Physics/Body/Body.h

@@ -216,8 +216,11 @@ public:
 	/// Flag if body is in the broadphase (should only be called by the BroadPhase)
 	inline void				SetInBroadPhaseInternal(bool inInBroadPhase)					{ if (inInBroadPhase) mFlags.fetch_or(uint8(EFlags::IsInBroadPhase), memory_order_relaxed); else mFlags.fetch_and(uint8(~uint8(EFlags::IsInBroadPhase)), memory_order_relaxed); }
 
-	/// Flag to invalidate the collision cache (should only be called by the BodyManager), will be reset the next simulation step.
-	inline void				InvalidateCollisionCacheInternal(bool inInvalidate)				{ if (inInvalidate) mFlags.fetch_or(uint8(EFlags::InvalidateContactCache), memory_order_relaxed); else mFlags.fetch_and(uint8(~uint8(EFlags::InvalidateContactCache)), memory_order_relaxed); }
+	/// Invalidate the contact cache (should only be called by the BodyManager), will be reset the next simulation step. Returns true if the contact cache was still valid.
+	inline bool				InvalidateContactCacheInternal()								{ return (mFlags.fetch_or(uint8(EFlags::InvalidateContactCache), memory_order_relaxed) & uint8(EFlags::InvalidateContactCache)) == 0; }
+
+	/// Reset the collision cache invalid flag (should only be called by the BodyManager).
+	inline void				ValidateContactCacheInternal()									{ JPH_IF_ENABLE_ASSERTS(uint8 old_val = ) mFlags.fetch_and(uint8(~uint8(EFlags::InvalidateContactCache)), memory_order_relaxed); JPH_ASSERT((old_val & uint8(EFlags::InvalidateContactCache)) != 0); }
 
 	/// Updates world space bounding box (should only be called by the PhysicsSystem)
 	void					CalculateWorldSpaceBoundsInternal();

Jolt/Physics/Body/BodyManager.cpp

@@ -729,17 +729,11 @@ void BodyManager::Draw(const DrawSettings &inDrawSettings, const PhysicsSettings
 
 void BodyManager::InvalidateContactCacheForBody(Body &ioBody)
 {
-	// Check if invalid to avoid taking a mutex lock. Note that this double locking pattern is safe as this flag is only reset during the physics update at which point we never invalidate the contact cache (and all bodies are locked).
-	if (!ioBody.IsCollisionCacheInvalid())
+	// If this is the first time we flip the collision cache invalid flag, we need to add it to an internal list to ensure we reset the flag at the end of the physics update
+	if (ioBody.InvalidateContactCacheInternal())
 	{
 		lock_guard lock(mBodiesCacheInvalidMutex);
-	
-		// Check flag again
-		if (!ioBody.IsCollisionCacheInvalid())
-		{
-			mBodiesCacheInvalid.push_back(ioBody.GetID());
-			ioBody.InvalidateCollisionCacheInternal(true);
-		}
+		mBodiesCacheInvalid.push_back(ioBody.GetID());
 	}
 }
 
@@ -752,7 +746,7 @@ void BodyManager::ValidateContactCacheForAllBodies()
 		// The body may have been removed between the call to InvalidateContactCacheForBody and this call, so check if it still exists
 		Body *body = TryGetBody(b);
 		if (body != nullptr)
-			body->InvalidateCollisionCacheInternal(false);
+			body->ValidateContactCacheInternal();
 	}
 	mBodiesCacheInvalid.clear();
 }