Added functionality to create bodies without ID and assign an ID later (#272)

This allows creating bodies on multiple threads and then later assigning them an ID in a serial manner to ensure a deterministic simulation

Jolt/Physics/Body/BodyInterface.cpp

@@ -17,12 +17,39 @@ JPH_NAMESPACE_BEGIN
 
 Body *BodyInterface::CreateBody(const BodyCreationSettings &inSettings)
 {
-	return mBodyManager->CreateBody(inSettings);
+	Body *body = mBodyManager->AllocateBody(inSettings);
+	if (!mBodyManager->AddBody(body))
+	{
+		mBodyManager->FreeBody(body);
+		return nullptr;
+	}
+	return body;
 }
 
 Body *BodyInterface::CreateBodyWithID(const BodyID &inBodyID, const BodyCreationSettings &inSettings)
 {
-	return mBodyManager->CreateBodyWithID(inBodyID, inSettings);
+	Body *body = mBodyManager->AllocateBody(inSettings);
+	if (!mBodyManager->AddBodyWithCustomID(body, inBodyID))
+	{
+		mBodyManager->FreeBody(body);
+		return nullptr;
+	}
+	return body;
+}
+
+Body *BodyInterface::CreateBodyWithoutID(const BodyCreationSettings &inSettings) const
+{
+	return mBodyManager->AllocateBody(inSettings);
+}
+
+void BodyInterface::DestroyBodyWithoutID(Body *inBody) const
+{
+	mBodyManager->FreeBody(inBody);
+}
+
+bool BodyInterface::AssignBodyID(Body *ioBody, const BodyID &inBodyID)
+{
+	return mBodyManager->AddBodyWithCustomID(ioBody, inBodyID);
 }
 
 void BodyInterface::DestroyBody(const BodyID &inBodyID)

Jolt/Physics/Body/BodyInterface.h

@@ -40,6 +40,17 @@ public:
 	/// @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);
 
+	/// Advanced use only. Creates a body without specifying an ID. This body cannot be added to the world until it has gotten a body ID. A call to CreateBodyWithoutID followed by AssignBodyID is equivalent to calling CreateBodyWithID.
+	/// @return Created body
+	Body *						CreateBodyWithoutID(const BodyCreationSettings &inSettings) const;
+
+	/// Advanced use only. Destroy a body previously created with CreateBodyWithoutID that hasn't gotten an ID yet through the AssignBodyID function. Bodies that did get an ID should be destroyed through DestroyBody.
+	void						DestroyBodyWithoutID(Body *inBody) const;
+
+	/// Advanced use only. Assigns a body ID to a body that was created using CreateBodyWithoutID. After this call, the body can be added to the world like any other body.
+	/// @return false if the body already has an ID or if the ID is not valid.
+	bool						AssignBodyID(Body *ioBody, const BodyID &inBodyID);
+
 	/// Destroy a body
 	void						DestroyBody(const BodyID &inBodyID);
 	

Jolt/Physics/Body/BodyManager.cpp

@@ -120,8 +120,68 @@ BodyManager::BodyStats BodyManager::GetBodyStats() const
 	return stats;
 }
 
-Body *BodyManager::CreateBody(const BodyCreationSettings &inBodyCreationSettings)
+Body *BodyManager::AllocateBody(const BodyCreationSettings &inBodyCreationSettings) const
 {
+	// Fill in basic properties
+	Body *body;
+	if (inBodyCreationSettings.HasMassProperties())
+	{
+		BodyWithMotionProperties *bmp = new BodyWithMotionProperties;
+		body = bmp;
+		body->mMotionProperties = &bmp->mMotionProperties;
+	}
+	else
+	{
+	 	body = new Body;
+	}
+	body->mShape = inBodyCreationSettings.GetShape();
+	body->mUserData = inBodyCreationSettings.mUserData;
+	body->SetFriction(inBodyCreationSettings.mFriction);
+	body->SetRestitution(inBodyCreationSettings.mRestitution);
+	body->mMotionType = inBodyCreationSettings.mMotionType;
+	if (inBodyCreationSettings.mIsSensor)
+		body->SetIsSensor(true);
+	SetBodyObjectLayerInternal(*body, inBodyCreationSettings.mObjectLayer);
+	body->mObjectLayer = inBodyCreationSettings.mObjectLayer;
+	body->mCollisionGroup = inBodyCreationSettings.mCollisionGroup;
+	
+	if (inBodyCreationSettings.HasMassProperties())
+	{
+		MotionProperties *mp = body->mMotionProperties;
+		mp->SetLinearDamping(inBodyCreationSettings.mLinearDamping);
+		mp->SetAngularDamping(inBodyCreationSettings.mAngularDamping);
+		mp->SetMaxLinearVelocity(inBodyCreationSettings.mMaxLinearVelocity);
+		mp->SetMaxAngularVelocity(inBodyCreationSettings.mMaxAngularVelocity);
+		mp->SetLinearVelocity(inBodyCreationSettings.mLinearVelocity); // Needs to happen after setting the max linear/angular velocity
+		mp->SetAngularVelocity(inBodyCreationSettings.mAngularVelocity);
+		mp->SetGravityFactor(inBodyCreationSettings.mGravityFactor);
+		mp->SetMotionQuality(inBodyCreationSettings.mMotionQuality);
+		mp->mAllowSleeping = inBodyCreationSettings.mAllowSleeping;
+		mp->mIndexInActiveBodies = Body::cInactiveIndex;
+		mp->mIslandIndex = Body::cInactiveIndex;
+		JPH_IF_ENABLE_ASSERTS(mp->mCachedMotionType = body->mMotionType;)
+		mp->SetMassProperties(inBodyCreationSettings.GetMassProperties());
+	}
+
+	// Position body
+	body->SetPositionAndRotationInternal(inBodyCreationSettings.mPosition, inBodyCreationSettings.mRotation);
+
+	return body;
+}
+
+void BodyManager::FreeBody(Body *inBody) const
+{
+	JPH_ASSERT(inBody->GetID().IsInvalid(), "This function should only be called on a body that doesn't have an ID yet, use DestroyBody otherwise");
+
+	sDeleteBody(inBody);
+}
+
+bool BodyManager::AddBody(Body *ioBody)
+{
+	// Return error when body was already added
+	if (!ioBody->GetID().IsInvalid())
+		return false;
+
 	// Determine next free index
 	uint32 idx;
 	{
@@ -134,6 +194,7 @@ Body *BodyManager::CreateBody(const BodyCreationSettings &inBodyCreationSettings
 			idx = uint32(mBodyIDFreeListStart >> cFreedBodyIndexShift);
 			JPH_ASSERT(!sIsValidBodyPointer(mBodies[idx]));
 			mBodyIDFreeListStart = uintptr_t(mBodies[idx]);
+			mBodies[idx] = ioBody;
 		}
 		else
 		{
@@ -141,12 +202,12 @@ Body *BodyManager::CreateBody(const BodyCreationSettings &inBodyCreationSettings
 			{
 				// Allocate a new entry, note that the array should not actually resize since we've reserved it at init time
 				idx = uint32(mBodies.size());
-				mBodies.push_back((Body *)cBodyIDFreeListEnd);
+				mBodies.push_back(ioBody);
 			}
 			else
 			{
 				// Out of bodies
-				return nullptr;
+				return false;
 			}
 		}
 
@@ -154,28 +215,31 @@ Body *BodyManager::CreateBody(const BodyCreationSettings &inBodyCreationSettings
 		mNumBodies++;
 	}
 
-	// Get next sequence number
+	// Get next sequence number and assign the ID
 	uint8 seq_no = GetNextSequenceNumber(idx);
-
-	// Do actual creation
-	return CreateBodyWithIDInternal(BodyID(idx, seq_no), inBodyCreationSettings);
+	ioBody->mID = BodyID(idx, seq_no);
+	return true;
 }
 
-Body *BodyManager::CreateBodyWithID(const BodyID &inBodyID, const BodyCreationSettings &inBodyCreationSettings)
+bool BodyManager::AddBodyWithCustomID(Body *ioBody, const BodyID &inBodyID)
 {
+	// Return error when body was already added
+	if (!ioBody->GetID().IsInvalid())
+		return false;
+
 	{
 		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
+			return false; // 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
+				return false; // Return error
 
 			// Remove the entry from the freelist
 			uintptr_t idx_start = mBodyIDFreeListStart >> cFreedBodyIndexShift;
@@ -199,10 +263,10 @@ Body *BodyManager::CreateBodyWithID(const BodyID &inBodyID, const BodyCreationSe
 					}
 				}
 				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;
 			}
+
+			// Put the body in the slot
+			mBodies[idx] = ioBody;
 		}
 		else
 		{
@@ -214,68 +278,17 @@ Body *BodyManager::CreateBodyWithID(const BodyID &inBodyID, const BodyCreationSe
 				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);
+			// Add the element to the list
+			mBodies.push_back(ioBody);
 		}
 
 		// 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())
-	{
-		BodyWithMotionProperties *bmp = new BodyWithMotionProperties;
-		body = bmp;
-		body->mMotionProperties = &bmp->mMotionProperties;
-	}
-	else
-	{
-	 	body = new Body;
-	}
-	body->mID = inBodyID;
-	body->mShape = inBodyCreationSettings.GetShape();
-	body->mUserData = inBodyCreationSettings.mUserData;
-	body->SetFriction(inBodyCreationSettings.mFriction);
-	body->SetRestitution(inBodyCreationSettings.mRestitution);
-	body->mMotionType = inBodyCreationSettings.mMotionType;
-	if (inBodyCreationSettings.mIsSensor)
-		body->SetIsSensor(true);
-	SetBodyObjectLayerInternal(*body, inBodyCreationSettings.mObjectLayer);
-	body->mObjectLayer = inBodyCreationSettings.mObjectLayer;
-	body->mCollisionGroup = inBodyCreationSettings.mCollisionGroup;
-	
-	if (inBodyCreationSettings.HasMassProperties())
-	{
-		MotionProperties *mp = body->mMotionProperties;
-		mp->SetLinearDamping(inBodyCreationSettings.mLinearDamping);
-		mp->SetAngularDamping(inBodyCreationSettings.mAngularDamping);
-		mp->SetMaxLinearVelocity(inBodyCreationSettings.mMaxLinearVelocity);
-		mp->SetMaxAngularVelocity(inBodyCreationSettings.mMaxAngularVelocity);
-		mp->SetLinearVelocity(inBodyCreationSettings.mLinearVelocity); // Needs to happen after setting the max linear/angular velocity
-		mp->SetAngularVelocity(inBodyCreationSettings.mAngularVelocity);
-		mp->SetGravityFactor(inBodyCreationSettings.mGravityFactor);
-		mp->SetMotionQuality(inBodyCreationSettings.mMotionQuality);
-		mp->mAllowSleeping = inBodyCreationSettings.mAllowSleeping;
-		mp->mIndexInActiveBodies = Body::cInactiveIndex;
-		mp->mIslandIndex = Body::cInactiveIndex;
-		JPH_IF_ENABLE_ASSERTS(mp->mCachedMotionType = body->mMotionType;)
-		mp->SetMassProperties(inBodyCreationSettings.GetMassProperties());
-	}
-
-	// Position body
-	body->SetPositionAndRotationInternal(inBodyCreationSettings.mPosition, inBodyCreationSettings.mRotation);
-
-	// Add body
-	mBodies[inBodyID.GetIndex()] = body;
-	return body;
+	// Assign the ID
+	ioBody->mID = inBodyID;
+	return true;
 }
 
 void BodyManager::DestroyBodies(const BodyID *inBodyIDs, int inNumber)

Jolt/Physics/Body/BodyManager.h

@@ -60,16 +60,19 @@ public:
 	/// Get stats about the bodies in the body manager (slow, iterates through all bodies)
 	BodyStats						GetBodyStats() const;
 
-	/// Create a body.
-	/// This is a thread safe function. Can return null if there are no more bodies available.
-	Body *							CreateBody(const BodyCreationSettings &inBodyCreationSettings);
+	/// Create a body using creation settings. The returned body will not be part of the body manager yet.
+	Body *							AllocateBody(const BodyCreationSettings &inBodyCreationSettings) const;
 
-	/// 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);
+	/// Free a body that has not been added to the body manager yet (if it has, use DestroyBodies).
+	void							FreeBody(Body *inBody) const;
 
-	/// 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)
+	/// Add a body to the body manager, assigning it the next available ID. Returns false if no more IDs are available.
+	bool							AddBody(Body *ioBody);
+
+	/// Add a body to the body manager, assigning it a custom ID. Returns false if the ID is not valid.
+	bool							AddBodyWithCustomID(Body *ioBody, const BodyID &inBodyID);
+
+	/// Remove a set of bodies from the body manager and destroy them.
 	void							DestroyBodies(const BodyID *inBodyIDs, int inNumber);
 
 	/// Activate a list of bodies.
@@ -226,9 +229,6 @@ 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);
 

Samples/Tests/BroadPhase/BroadPhaseTest.cpp

@@ -45,7 +45,8 @@ void BroadPhaseTest::CreateBalancedDistribution(BodyManager *inBodyManager, int
 		s.mPosition = box.GetCenter();
 		s.mRotation = Quat::sIdentity();
 		s.mObjectLayer = (random() % 10) == 0? Layers::MOVING : Layers::NON_MOVING;
-		inBodyManager->CreateBody(s);
+		Body *body = inBodyManager->AllocateBody(s);
+		inBodyManager->AddBody(body);
 	}
 }
 

UnitTests/Physics/BroadPhaseTests.cpp

@@ -27,7 +27,8 @@ TEST_SUITE("BroadPhaseTests")
 
 		// Create a box
 		BodyCreationSettings settings(new BoxShape(Vec3::sReplicate(1.0f)), Vec3::sZero(), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING);
-		Body &body = *body_manager.CreateBody(settings);
+		Body &body = *body_manager.AllocateBody(settings);
+		body_manager.AddBody(&body);
 
 		// Add it to the broadphase
 		BodyID id = body.GetID();