Added BodyInterface::CreateBodyWithID to be able to keep BodyIDs consistent between client and server (#247)

Docs/Architecture.md

@@ -329,7 +329,7 @@ When running the Samples Application you can press ESC, Physics Settings and che
 
 When synchronizing two simulations via a network, it is possible that a change that needed to be applied at frame N is received at frame N + M. This will require rolling back the simulation to the state of frame N and repeating the simulation with the new inputs. This can be implemented by saving the physics state using [SaveState](@ref PhysicsSystem::SaveState) at every frame. To roll back, call [RestoreState](@ref PhysicsSystem::RestoreState) with the state at frame N. SaveState only records the state that the physics engine modifies during its update step (positions, velocities etc.), so if you change anything else you need to restore this yourself. E.g. if you did a [SetFriction](@ref Body::SetFriction) on frame N + 2 then, when rewinding, you need to restore the friction to what is was on frame N and update it again on frame N + 2 when you replay. If you start adding/removing objects (e.g. bodies or constraints) during these frames, the RestoreState function will not work. If you added a body on frame N + 1, you'll need to remove it when rewinding and then add it back on frame N + 1 again (with the proper initial position/velocity etc. because it won't be contained in the snapshot at frame N).
 
-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.
+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).
 
 ## The Simulation Step in Detail
 

Jolt/Physics/Body/BodyInterface.cpp

@@ -20,6 +20,11 @@ Body *BodyInterface::CreateBody(const BodyCreationSettings &inSettings)
 	return mBodyManager->CreateBody(inSettings);
 }
 
+Body *BodyInterface::CreateBodyWithID(const BodyID &inBodyID, const BodyCreationSettings &inSettings)
+{
+	return mBodyManager->CreateBodyWithID(inBodyID, inSettings);
+}
+
 void BodyInterface::DestroyBody(const BodyID &inBodyID)
 {
 	mBodyManager->DestroyBodies(&inBodyID, 1);

Jolt/Physics/Body/BodyInterface.h

@@ -35,6 +35,11 @@ public:
 	/// @return Created body or null when out of bodies
 	Body *						CreateBody(const BodyCreationSettings &inSettings);
 	
+	/// Create a body with specified ID. This function can be used if a simulation is to run in sync between clients or if a simulation needs to be restored exactly.
+	/// The ID created on the server can be replicated to the client and used to create a deterministic simulation.
+	/// @return Created body or null when the body ID is invalid or a body of the same ID already exists.
+	Body *						CreateBodyWithID(const BodyID &inBodyID, const BodyCreationSettings &inSettings);
+
 	/// Destroy a body
 	void						DestroyBody(const BodyID &inBodyID);
 	

Jolt/Physics/Body/BodyManager.cpp

@@ -157,6 +157,77 @@ Body *BodyManager::CreateBody(const BodyCreationSettings &inBodyCreationSettings
 	// Get next sequence number
 	uint8 seq_no = GetNextSequenceNumber(idx);
 
+	// Do actual creation
+	return CreateBodyWithIDInternal(BodyID(idx, seq_no), inBodyCreationSettings);
+}
+
+Body *BodyManager::CreateBodyWithID(const BodyID &inBodyID, const BodyCreationSettings &inBodyCreationSettings)
+{
+	{
+		UniqueLock lock(mBodiesMutex, EPhysicsLockTypes::BodiesList);
+
+		// Check if index is beyond the max body ID
+		uint32 idx = inBodyID.GetIndex();
+		if (idx >= mBodies.capacity())
+			return nullptr; // Return error
+
+		if (idx < mBodies.size())
+		{
+			// Body array entry has already been allocated, check if there's a free body here
+			if (sIsValidBodyPointer(mBodies[idx]))
+				return nullptr; // Return error
+
+			// Remove the entry from the freelist
+			uintptr_t idx_start = mBodyIDFreeListStart >> cFreedBodyIndexShift;
+			if (idx == idx_start)
+			{
+				// First entry, easy to remove, the start of the list is our next
+				mBodyIDFreeListStart = uintptr_t(mBodies[idx]);
+			}
+			else
+			{
+				// Loop over the freelist and find the entry in the freelist pointing to our index
+				// TODO: This is O(N), see if this becomes a performance problem (don't want to put the freed bodies in a double linked list)
+				uintptr_t cur, next;
+				for (cur = idx_start; cur != cBodyIDFreeListEnd >> cFreedBodyIndexShift; cur = next)
+				{
+					next = uintptr_t(mBodies[cur]) >> cFreedBodyIndexShift;
+					if (next == idx)
+					{
+						mBodies[cur] = mBodies[idx];
+						break;
+					}
+				}
+				JPH_ASSERT(cur != cBodyIDFreeListEnd >> cFreedBodyIndexShift);
+
+				// We're leaving the lock, ensure that we've overwritten this entry (although it's not strictly needed)
+				mBodies[idx] = (Body *)cBodyIDFreeListEnd;
+			}
+		}
+		else
+		{
+			// Ensure that all body IDs up to this body ID have been allocated and added to the free list
+			while (idx > mBodies.size())
+			{
+				// Push the id onto the freelist
+				mBodies.push_back((Body *)mBodyIDFreeListStart);
+				mBodyIDFreeListStart = (uintptr_t(mBodies.size() - 1) << cFreedBodyIndexShift) | cIsFreedBody;
+			}
+
+			// Add the element that we're going to overwrite to the list
+			mBodies.push_back((Body *)cBodyIDFreeListEnd);
+		}
+
+		// Update cached number of bodies
+		mNumBodies++;
+	}
+
+	// Do actual creation
+	return CreateBodyWithIDInternal(inBodyID, inBodyCreationSettings);
+}
+
+Body *BodyManager::CreateBodyWithIDInternal(const BodyID &inBodyID, const BodyCreationSettings &inBodyCreationSettings)
+{
 	// Fill in basic properties
 	Body *body;
 	if (inBodyCreationSettings.HasMassProperties())
@@ -169,7 +240,7 @@ Body *BodyManager::CreateBody(const BodyCreationSettings &inBodyCreationSettings
 	{
 	 	body = new Body;
 	}
-	body->mID = BodyID(idx, seq_no);
+	body->mID = inBodyID;
 	body->mShape = inBodyCreationSettings.GetShape();
 	body->mUserData = inBodyCreationSettings.mUserData;
 	body->SetFriction(inBodyCreationSettings.mFriction);
@@ -203,7 +274,7 @@ Body *BodyManager::CreateBody(const BodyCreationSettings &inBodyCreationSettings
 	body->SetPositionAndRotationInternal(inBodyCreationSettings.mPosition, inBodyCreationSettings.mRotation);
 
 	// Add body
-	mBodies[idx] = body;
+	mBodies[inBodyID.GetIndex()] = body;
 	return body;
 }
 

Jolt/Physics/Body/BodyManager.h

@@ -64,6 +64,10 @@ public:
 	/// This is a thread safe function. Can return null if there are no more bodies available.
 	Body *							CreateBody(const BodyCreationSettings &inBodyCreationSettings);
 
+	/// Helper function to create a body when the ID has been determined
+	/// This is a thread safe function. Can return null if there are no more bodies available or when the body ID is already in use.
+	Body *							CreateBodyWithID(const BodyID &inBodyID, const BodyCreationSettings &inBodyCreationSettings);
+
 	/// Mark a list of bodies for destruction and remove it from this manager.
 	/// This is a thread safe function since the body is not deleted until the next PhysicsSystem::Update() (which will take all locks)
 	void							DestroyBodies(const BodyID *inBodyIDs, int inNumber);
@@ -222,6 +226,9 @@ private:
 #endif
 	inline uint8					GetNextSequenceNumber(int inBodyIndex)		{ return ++mBodySequenceNumbers[inBodyIndex]; }
 
+	/// Helper function to create a body when the ID has been determined
+	Body *							CreateBodyWithIDInternal(const BodyID &inBodyID, const BodyCreationSettings &inBodyCreationSettings);
+
 	/// Helper function to delete a body (which could actually be a BodyWithMotionProperties)
 	inline static void				sDeleteBody(Body *inBody);
 

UnitTests/Physics/PhysicsTests.cpp

@@ -188,6 +188,61 @@ TEST_SUITE("PhysicsTests")
 		bi.DestroyBody(body0_id);
 	}
 
+	TEST_CASE("TestPhysicsBodyIDOverride")
+	{
+		PhysicsTestContext c(1.0f / 60.0f, 1, 1);
+		BodyInterface &bi = c.GetBodyInterface();
+
+		// Dummy creation settings
+		BodyCreationSettings bc(new BoxShape(Vec3::sReplicate(1.0f)), Vec3::sZero(), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING);
+
+		// Create a body
+		Body *b1 = bi.CreateBody(bc);
+		CHECK(b1->GetID() == BodyID(0, 1));
+
+		// Create body with same ID and same sequence number
+		Body *b2 = bi.CreateBodyWithID(BodyID(0, 1), bc);
+		CHECK(b2 == nullptr);
+
+		// Create body with same ID and different sequence number
+		b2 = bi.CreateBodyWithID(BodyID(0, 2), bc);
+		CHECK(b2 == nullptr);
+
+		// Create body with different ID (leave 1 open slot)
+		b2 = bi.CreateBodyWithID(BodyID(2, 1), bc);
+		CHECK(b2 != nullptr);
+		CHECK(b2->GetID() == BodyID(2, 1));
+
+		// Create another body and check that the open slot is returned
+		Body *b3 = bi.CreateBody(bc);
+		CHECK(b3->GetID() == BodyID(1, 1));
+
+		// Create another body and check that we do not hand out the body with specified ID
+		Body *b4 = bi.CreateBody(bc);
+		CHECK(b4->GetID() == BodyID(3, 1));
+
+		// Delete and recreate body 4
+		CHECK(bi.CreateBodyWithID(BodyID(3, 1), bc) == nullptr);
+		bi.DestroyBody(b4->GetID());
+		b4 = bi.CreateBodyWithID(BodyID(3, 1), bc);
+		CHECK(b4 != nullptr);
+		CHECK(b4->GetID() == BodyID(3, 1));
+
+		// Clean up all bodies
+		bi.DestroyBody(b1->GetID());
+		bi.DestroyBody(b2->GetID());
+		bi.DestroyBody(b3->GetID());
+		bi.DestroyBody(b4->GetID());
+
+		// Recreate body 1
+		b1 = bi.CreateBodyWithID(BodyID(0, 1), bc);
+		CHECK(b1 != nullptr);
+		CHECK(b1->GetID() == BodyID(0, 1));
+
+		// Destroy last body
+		bi.DestroyBody(b1->GetID());
+	}
+
 	TEST_CASE("TestPhysicsBodyUserData")
 	{
 		PhysicsTestContext c(1.0f / 60.0f, 1, 1);