Ability to split unassigning of a body ID from freeing the body (#291)

Jolt/Physics/Body/BodyInterface.cpp

@@ -47,11 +47,28 @@ void BodyInterface::DestroyBodyWithoutID(Body *inBody) const
 	mBodyManager->FreeBody(inBody);
 }
 
+bool BodyInterface::AssignBodyID(Body *ioBody)
+{
+	return mBodyManager->AddBody(ioBody);
+}
+
 bool BodyInterface::AssignBodyID(Body *ioBody, const BodyID &inBodyID)
 {
 	return mBodyManager->AddBodyWithCustomID(ioBody, inBodyID);
 }
 
+Body *BodyInterface::UnassignBodyID(const BodyID &inBodyID)
+{
+	Body *body = nullptr;
+	mBodyManager->RemoveBodies(&inBodyID, 1, &body);
+	return body;
+}
+
+void BodyInterface::UnassignBodyIDs(const BodyID *inBodyIDs, int inNumber, Body **outBodies)
+{
+	mBodyManager->RemoveBodies(inBodyIDs, inNumber, outBodies);
+}
+
 void BodyInterface::DestroyBody(const BodyID &inBodyID)
 {
 	mBodyManager->DestroyBodies(&inBodyID, 1);

Jolt/Physics/Body/BodyInterface.h

@@ -40,33 +40,49 @@ 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.
+	/// Advanced use only. Creates a body without specifying an ID. This body cannot be added to the physics system until it has been assigned a body ID.
+	/// This can be used to decouple allocation from registering the body. 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.
+	/// Advanced use only. Destroy a body previously created with CreateBodyWithoutID that hasn't gotten an ID yet through the AssignBodyID function,
+	/// or a body that has had its body ID unassigned through UnassignBodyIDs. Bodies that have 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.
+	/// Advanced use only. Assigns the next available body ID to a body that was created using CreateBodyWithoutID. After this call, the body can be added to the physics system.
+	/// @return false if the body already has an ID or out of body ids.
+	bool						AssignBodyID(Body *ioBody);
+
+	/// 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 physics system.
 	/// @return false if the body already has an ID or if the ID is not valid.
 	bool						AssignBodyID(Body *ioBody, const BodyID &inBodyID);
 
+	/// Advanced use only. See UnassignBodyIDs. Unassigns the ID of a single body.
+	Body *						UnassignBodyID(const BodyID &inBodyID);
+
+	/// Advanced use only. Removes a number of body IDs from their bodies and returns the body pointers. Before calling this, the body should have been removed from the physics system.
+	/// The body can be destroyed through DestroyBodyWithoutID. This can be used to decouple deallocation. A call to UnassignBodyIDs followed by calls to DestroyBodyWithoutID is equivalent to calling DestroyBodies.
+	/// @param inBodyIDs A list of body IDs
+	/// @param inNumber Number of bodies in the list
+	/// @param outBodies If not null on input, this will contain a list of body pointers corresponding to inBodyIDs that can be destroyed afterwards (caller assumes ownership over these).
+	void						UnassignBodyIDs(const BodyID *inBodyIDs, int inNumber, Body **outBodies);
+
 	/// Destroy a body
 	void						DestroyBody(const BodyID &inBodyID);
 	
 	/// Destroy multiple bodies
 	void						DestroyBodies(const BodyID *inBodyIDs, int inNumber);
 
-	/// Add body to the world.
+	/// Add body to the physics system.
 	/// Note that if you need to add multiple bodies, use the AddBodiesPrepare/AddBodiesFinalize function.
 	/// Adding many bodies, one at a time, results in a really inefficient broadphase until PhysicsSystem::OptimizeBroadPhase is called or when PhysicsSystem::Update rebuilds the tree!
 	/// After adding, to get a body by ID use the BodyLockRead or BodyLockWrite interface!
 	void						AddBody(const BodyID &inBodyID, EActivation inActivationMode);
 	
-	/// Remove body from the world.
+	/// Remove body from the physics system.
 	void						RemoveBody(const BodyID &inBodyID);
 	
-	/// Check if a body has been added to the world.
+	/// Check if a body has been added to the physics system.
 	bool						IsAdded(const BodyID &inBodyID) const;
 
 	/// Combines CreateBody and AddBody

Jolt/Physics/Body/BodyManager.cpp

@@ -291,6 +291,73 @@ bool BodyManager::AddBodyWithCustomID(Body *ioBody, const BodyID &inBodyID)
 	return true;
 }
 
+Body *BodyManager::RemoveBodyInternal(const BodyID &inBodyID)
+{
+	// Get body
+	uint32 idx = inBodyID.GetIndex();
+	Body *body = mBodies[idx];
+
+	// Validate that it can be removed
+	JPH_ASSERT(body->GetID() == inBodyID);
+	JPH_ASSERT(!body->IsActive());
+	JPH_ASSERT(!body->IsInBroadPhase());
+	
+	// Push the id onto the freelist
+	mBodies[idx] = (Body *)mBodyIDFreeListStart;
+	mBodyIDFreeListStart = (uintptr_t(idx) << cFreedBodyIndexShift) | cIsFreedBody;
+
+	return body;
+}
+
+#if defined(_DEBUG) && defined(JPH_ENABLE_ASSERTS)
+
+void BodyManager::ValidateFreeList() const
+{
+	// Check that the freelist is correct
+	size_t num_freed = 0;
+	for (uintptr_t start = mBodyIDFreeListStart; start != cBodyIDFreeListEnd; start = uintptr_t(mBodies[start >> cFreedBodyIndexShift]))
+	{
+		JPH_ASSERT(start & cIsFreedBody);
+		num_freed++;
+	}
+	JPH_ASSERT(mNumBodies == mBodies.size() - num_freed);
+}
+
+#endif // defined(_DEBUG) && _defined(JPH_ENABLE_ASSERTS)
+
+void BodyManager::RemoveBodies(const BodyID *inBodyIDs, int inNumber, Body **outBodies)
+{
+	// Don't take lock if no bodies are to be destroyed
+	if (inNumber <= 0)
+		return;
+
+	UniqueLock lock(mBodiesMutex, EPhysicsLockTypes::BodiesList);
+
+	// Update cached number of bodies
+	JPH_ASSERT(mNumBodies >= (uint)inNumber);
+	mNumBodies -= inNumber;
+
+	for (const BodyID *b = inBodyIDs, *b_end = inBodyIDs + inNumber; b < b_end; b++)
+	{
+		// Remove body
+		Body *body = RemoveBodyInternal(*b);
+
+		// Clear the ID
+		body->mID = BodyID();
+
+		// Return the body to the caller
+		if (outBodies != nullptr)
+		{
+			*outBodies = body;
+			++outBodies;
+		}
+	}
+
+#if defined(_DEBUG) && defined(JPH_ENABLE_ASSERTS)
+	ValidateFreeList();
+#endif // defined(_DEBUG) && _defined(JPH_ENABLE_ASSERTS)
+}
+
 void BodyManager::DestroyBodies(const BodyID *inBodyIDs, int inNumber)
 {
 	// Don't take lock if no bodies are to be destroyed
@@ -305,33 +372,15 @@ void BodyManager::DestroyBodies(const BodyID *inBodyIDs, int inNumber)
 
 	for (const BodyID *b = inBodyIDs, *b_end = inBodyIDs + inNumber; b < b_end; b++)
 	{
-		// Get body
-		BodyID body_id = *b;
-		uint32 idx = body_id.GetIndex();
-		Body *body = mBodies[idx];
-
-		// Validate that it can be removed
-		JPH_ASSERT(body->GetID() == body_id);
-		JPH_ASSERT(!body->IsActive());
-		JPH_ASSERT(!body->IsInBroadPhase());
-	
-		// Push the id onto the freelist
-		mBodies[idx] = (Body *)mBodyIDFreeListStart;
-		mBodyIDFreeListStart = (uintptr_t(idx) << cFreedBodyIndexShift) | cIsFreedBody;
+		// Remove body
+		Body *body = RemoveBodyInternal(*b);
 
 		// Free the body
 		sDeleteBody(body);
 	}
 
 #if defined(_DEBUG) && defined(JPH_ENABLE_ASSERTS)
-	// Check that the freelist is correct
-	size_t num_freed = 0;
-	for (uintptr_t start = mBodyIDFreeListStart; start != cBodyIDFreeListEnd; start = uintptr_t(mBodies[start >> cFreedBodyIndexShift]))
-	{
-		JPH_ASSERT(start & cIsFreedBody);
-		num_freed++;
-	}
-	JPH_ASSERT(mNumBodies == mBodies.size() - num_freed);
+	ValidateFreeList();
 #endif // defined(_DEBUG) && _defined(JPH_ENABLE_ASSERTS)
 }
 

Jolt/Physics/Body/BodyManager.h

@@ -72,7 +72,10 @@ public:
 	/// 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.
+	/// Remove a list of bodies from the body manager
+	void							RemoveBodies(const BodyID *inBodyIDs, int inNumber, Body **outBodies);
+
+	/// Remove a set of bodies from the body manager and frees them.
 	void							DestroyBodies(const BodyID *inBodyIDs, int inNumber);
 
 	/// Activate a list of bodies.
@@ -229,9 +232,17 @@ private:
 #endif
 	inline uint8					GetNextSequenceNumber(int inBodyIndex)		{ return ++mBodySequenceNumbers[inBodyIndex]; }
 
+	/// Helper function to remove a body from the manager
+	JPH_INLINE Body *				RemoveBodyInternal(const BodyID &inBodyID);
+
 	/// Helper function to delete a body (which could actually be a BodyWithMotionProperties)
 	inline static void				sDeleteBody(Body *inBody);
 
+#if defined(_DEBUG) && defined(JPH_ENABLE_ASSERTS)
+	/// Function to check that the free list is not corrupted
+	void							ValidateFreeList() const;
+#endif // defined(_DEBUG) && _defined(JPH_ENABLE_ASSERTS)
+
 	/// List of pointers to all bodies. Contains invalid pointers for deleted bodies, check with sIsValidBodyPointer. Note that this array is reserved to the max bodies that is passed in the Init function so that adding bodies will not reallocate the array.
 	BodyVector						mBodies;
 

UnitTests/Physics/PhysicsTests.cpp

@@ -209,8 +209,10 @@ TEST_SUITE("PhysicsTests")
 		CHECK(b2 == nullptr);
 
 		// Create body with different ID (leave 1 open slot)
-		b2 = bi.CreateBodyWithID(BodyID(2, 1), bc);
+		b2 = bi.CreateBodyWithoutID(bc); // Using syntax that allows separation of allocation and assigning an ID
 		CHECK(b2 != nullptr);
+		CHECK(b2->GetID().IsInvalid());
+		bi.AssignBodyID(b2, BodyID(2, 1));
 		CHECK(b2->GetID() == BodyID(2, 1));
 
 		// Create another body and check that the open slot is returned
@@ -228,8 +230,12 @@ TEST_SUITE("PhysicsTests")
 		CHECK(b4 != nullptr);
 		CHECK(b4->GetID() == BodyID(3, 1));
 
-		// Clean up all bodies
-		bi.DestroyBody(b1->GetID());
+		// Destroy 1st body
+		CHECK(bi.UnassignBodyID(b1->GetID()) == b1); // Use syntax that allows separation of unassigning and deallocation
+		CHECK(b1->GetID().IsInvalid());
+		bi.DestroyBodyWithoutID(b1);
+
+		// Clean up remaining bodies
 		bi.DestroyBody(b2->GetID());
 		bi.DestroyBody(b3->GetID());
 		bi.DestroyBody(b4->GetID());