Fixed bug in OptimizeBroadPhase (#1682)

- A tree that had all its bodies removed would never be optimized and never release its empty nodes
- Quad tree nodes were not released until the next PhysicsSystem::Update / OptimizeBroadPhase

See https://github.com/godotengine/godot/issues/107951

Jolt/Physics/Collision/BroadPhase/BroadPhaseQuadTree.cpp

@@ -73,6 +73,7 @@ void BroadPhaseQuadTree::Optimize()
 {
 	JPH_PROFILE_FUNCTION();
 
+	// Free the previous tree so we can create a new optimized tree
 	FrameSync();
 
 	LockModifications();
@@ -80,7 +81,7 @@ void BroadPhaseQuadTree::Optimize()
 	for (uint l = 0; l < mNumLayers; ++l)
 	{
 		QuadTree &tree = mLayers[l];
-		if (tree.HasBodies())
+		if (tree.HasBodies() || tree.IsDirty())
 		{
 			QuadTree::UpdateState update_state;
 			tree.UpdatePrepare(mBodyManager->GetBodies(), mTracking, update_state, true);
@@ -90,6 +91,9 @@ void BroadPhaseQuadTree::Optimize()
 
 	UnlockModifications();
 
+	// Free the tree from before we created a new optimized tree
+	FrameSync();
+
 	mNextLayerToUpdate = 0;
 }
 

Jolt/Physics/Collision/BroadPhase/QuadTree.cpp

@@ -301,7 +301,7 @@ void QuadTree::UpdatePrepare(const BodyVector &inBodies, TrackingVector &ioTrack
 #endif
 
 	// Create space for all body ID's
-	NodeID *node_ids = new NodeID [mNumBodies];
+	NodeID *node_ids = mNumBodies > 0? new NodeID [mNumBodies] : nullptr;
 	NodeID *cur_node_id = node_ids;
 
 	// Collect all bodies

UnitTests/Physics/PhysicsTests.cpp

@@ -2117,4 +2117,40 @@ TEST_SUITE("PhysicsTests")
 		CHECK(contact_listener.Contains(LoggingContactListener::EType::Remove, floor.GetID(), SubShapeID(), body_id, sub_shape_ids[1]));
 		CHECK(contact_listener.Contains(LoggingContactListener::EType::Remove, floor.GetID(), SubShapeID(), body_id, sub_shape_ids[2]));
 	}
+
+	// This tests that we don't run out of nodes if we keep adding removing bodies when using OptimizeBroadPhase
+	TEST_CASE("TestOptimizeBroadPhase")
+	{
+		constexpr uint cMaxBodies = 128;
+		PhysicsTestContext c(1.0f / 60.0f, 1, 0, cMaxBodies);
+		BodyInterface &bi = c.GetBodyInterface();
+
+		// Create max number of bodies
+		BodyIDVector bodies;
+		BodyCreationSettings bcs(new SphereShape(1.0f), RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, Layers::MOVING);
+		for (uint i = 0; i < cMaxBodies; ++i)
+		{
+			Body *b = bi.CreateBody(bcs);
+			CHECK(b != nullptr);
+			bodies.push_back(b->GetID());
+		}
+
+		// Repeatedly add and remove bodies
+		for (int i = 0; i < 10; ++i)
+		{
+			BodyInterface::AddState add_state = bi.AddBodiesPrepare(bodies.data(), (int)bodies.size());
+			for (const BodyID &id : bodies)
+				CHECK(!bi.IsAdded(id));
+			bi.AddBodiesFinalize(bodies.data(), (int)bodies.size(), add_state, EActivation::DontActivate);
+			for (const BodyID &id : bodies)
+				CHECK(bi.IsAdded(id));
+
+			bi.RemoveBodies(bodies.data(), (int)bodies.size());
+			for (const BodyID &id : bodies)
+				CHECK(!bi.IsAdded(id));
+
+			// Optimize the broad phase to recycle quad tree nodes
+			c.GetSystem()->OptimizeBroadPhase();
+		}
+	}
 }