Broadphase related changes

* OptimizeBroadphase will do a full rebuild
* Optimize broadphase should also optimize non-dirty trees
* PhysicsScene::CreateBodies will batch add bodies
* Ability to dump the structure of the tree as a SVG file

Jolt/Physics/Collision/BroadPhase/BroadPhaseQuadTree.cpp

@@ -76,10 +76,10 @@ void BroadPhaseQuadTree::Optimize()
 	for (uint l = 0; l < mNumLayers; ++l)
 	{
 		QuadTree &tree = mLayers[l];
-		if (tree.HasBodies() && tree.IsDirty())
+		if (tree.HasBodies())
 		{
 			QuadTree::UpdateState update_state;
-			tree.UpdatePrepare(mBodyManager->GetBodies(), mTracking, update_state);
+			tree.UpdatePrepare(mBodyManager->GetBodies(), mTracking, update_state, true);
 			tree.UpdateFinalize(mBodyManager->GetBodies(), mTracking, update_state);
 		}
 	}
@@ -115,7 +115,7 @@ BroadPhase::UpdateState BroadPhaseQuadTree::UpdatePrepare()
 		if (tree.HasBodies() && tree.IsDirty() && tree.CanBeUpdated())
 		{
 			update_state_impl->mTree = &tree;
-			tree.UpdatePrepare(mBodyManager->GetBodies(), mTracking, update_state_impl->mUpdateState);
+			tree.UpdatePrepare(mBodyManager->GetBodies(), mTracking, update_state_impl->mUpdateState, false);
 			return update_state;
 		}
 	}

Jolt/Physics/Collision/BroadPhase/QuadTree.cpp

@@ -216,7 +216,7 @@ void QuadTree::DiscardOldTree()
 	}
 }
 
-void QuadTree::UpdatePrepare(const BodyVector &inBodies, TrackingVector &ioTracking, UpdateState &outUpdateState)
+void QuadTree::UpdatePrepare(const BodyVector &inBodies, TrackingVector &ioTracking, UpdateState &outUpdateState, bool inFullRebuild)
 {
 #ifdef JPH_ENABLE_ASSERTS
 	// We only read positions
@@ -232,6 +232,10 @@ void QuadTree::UpdatePrepare(const BodyVector &inBodies, TrackingVector &ioTrack
 	// Get the current root node
 	RootNode &root_node = GetCurrentRoot();
 
+#ifdef JPH_DUMP_BROADPHASE_TREE
+	DumpTree(root_node.GetNodeID(), StringFormat("%s_PRE", mName).c_str());
+#endif
+
 	// Assert sane data
 #ifdef _DEBUG
 	ValidateTree(inBodies, ioTracking, root_node.mIndex, mNumBodies);
@@ -268,7 +272,7 @@ void QuadTree::UpdatePrepare(const BodyVector &inBodies, TrackingVector &ioTrack
 			uint32 node_idx = node_id.GetNodeIndex();
 			const Node &node = mAllocator->Get(node_idx);
 
-			if (!node.mIsChanged)
+			if (!node.mIsChanged && !inFullRebuild)
 			{
 				// Node is unchanged, treat it as a whole
 				*cur_node_id = node_id;
@@ -298,7 +302,7 @@ void QuadTree::UpdatePrepare(const BodyVector &inBodies, TrackingVector &ioTrack
 
 	// Check that our book keeping matches
 	uint32 num_node_ids = uint32(cur_node_id - node_ids);
-	JPH_ASSERT(num_node_ids <= mNumBodies);
+	JPH_ASSERT(inFullRebuild? num_node_ids == mNumBodies : num_node_ids <= mNumBodies);
 
 	// This will be the new root node id
 	NodeID root_node_id;
@@ -349,6 +353,10 @@ void QuadTree::UpdateFinalize([[maybe_unused]] const BodyVector &inBodies, [[may
 	// All queries that start from now on will use this new tree
 	mRootNodeIndex = new_root_idx;
 
+#ifdef JPH_DUMP_BROADPHASE_TREE
+	DumpTree(new_root_node.GetNodeID(), StringFormat("%s_POST", mName).c_str());
+#endif
+
 #ifdef _DEBUG
 	ValidateTree(inBodies, inTracking, new_root_node.mIndex, mNumBodies);
 #endif
@@ -1513,6 +1521,83 @@ void QuadTree::ValidateTree(const BodyVector &inBodies, const TrackingVector &in
 
 #endif
 
+#ifdef JPH_DUMP_BROADPHASE_TREE
+
+void QuadTree::DumpTree(const NodeID &inRoot, const char *inFileNamePrefix) const
+{
+	// Open DOT file
+	ofstream f;
+	f.open(StringFormat("%s.dot", inFileNamePrefix), ofstream::out | ofstream::trunc);
+	if (!f.is_open())
+		return;
+
+	// Write header
+	f << "digraph {\n";
+
+	// Iterate the entire tree
+	NodeID node_stack[cStackSize];
+	node_stack[0] = inRoot;
+	JPH_ASSERT(node_stack[0].IsValid());
+	int top = 0;
+	do
+	{
+		// Check if node is a body
+		NodeID node_id = node_stack[top];
+		if (node_id.IsBody())
+		{
+			// Output body
+			string body_id = ConvertToString(node_id.GetBodyID().GetIndex());
+			f << "body" << body_id << "[label = \"Body " << body_id << "\"]\n";
+		}
+		else
+		{
+			// Process normal node
+			uint32 node_idx = node_id.GetNodeIndex();
+			const Node &node = mAllocator->Get(node_idx);
+
+			// Get bounding box
+			AABox bounds;
+			node.GetNodeBounds(bounds);
+
+			// Output node
+			string node_str = ConvertToString(node_idx);
+			f << "node" << node_str << "[label = \"Node " << node_str << "\nVolume: " << ConvertToString(bounds.GetVolume()) << "\" color=" << (node.mIsChanged? "red" : "black") << "]\n";
+
+			// Recurse and get all children
+			for (int i = 0; i < 4; ++i)
+			{
+				NodeID child_node_id = node.mChildNodeID[i];
+				if (child_node_id.IsValid())
+				{
+					JPH_ASSERT(top < cStackSize);
+					node_stack[top] = child_node_id;
+					top++;
+
+					// Output link
+					f << "node" << node_str << " -> ";
+					if (child_node_id.IsBody())
+						f << "body" << ConvertToString(child_node_id.GetBodyID().GetIndex());
+					else
+						f << "node" << ConvertToString(child_node_id.GetNodeIndex());
+					f << "\n";
+				}
+			}
+		}
+		--top;
+	} 
+	while (top >= 0);
+
+	// Finish DOT file
+	f << "}\n";
+	f.close();
+
+	// Convert to svg file
+	string cmd = StringFormat("dot %s.dot -Tsvg -o %s.svg", inFileNamePrefix, inFileNamePrefix);
+	system(cmd.c_str());
+}
+
+#endif // JPH_DUMP_BROADPHASE_TREE
+
 #ifdef JPH_TRACK_BROADPHASE_STATS
 
 void QuadTree::ReportStats(const char *inName, const LayerToStats &inLayer) const

Jolt/Physics/Collision/BroadPhase/QuadTree.h

@@ -13,6 +13,8 @@
 	#include <map>
 #endif // JPH_TRACK_BROADPHASE_STATS
 
+//#define JPH_DUMP_BROADPHASE_TREE
+
 namespace JPH {
 
 /// Internal tree structure in broadphase, is essentially a quad AABB tree.
@@ -195,7 +197,7 @@ public:
 
 	/// Update the broadphase, needs to be called regularly to achieve a tight fit of the tree when bodies have been modified.
 	/// UpdatePrepare() will build the tree, UpdateFinalize() will lock the root of the tree shortly and swap the trees and afterwards clean up temporary data structures.
-	void						UpdatePrepare(const BodyVector &inBodies, TrackingVector &ioTracking, UpdateState &outUpdateState);
+	void						UpdatePrepare(const BodyVector &inBodies, TrackingVector &ioTracking, UpdateState &outUpdateState, bool inFullRebuild);
 	void						UpdateFinalize(const BodyVector &inBodies, const TrackingVector &inTracking, const UpdateState &inUpdateState);
 
 	/// Temporary data structure to pass information between AddBodiesPrepare and AddBodiesFinalize/Abort
@@ -311,6 +313,11 @@ private:
 	void						ValidateTree(const BodyVector &inBodies, const TrackingVector &inTracking, uint32 inNodeIndex, uint32 inNumExpectedBodies) const;
 #endif
 
+#ifdef JPH_DUMP_BROADPHASE_TREE
+	/// Dump the tree in DOT format (see: https://graphviz.org/)
+	void						DumpTree(const NodeID &inRoot, const char *inFileNamePrefix) const;
+#endif
+
 #ifdef JPH_TRACK_BROADPHASE_STATS
 	/// Mutex protecting the various LayerToStats members
 	mutable Mutex				mStatsMutex;

Jolt/Physics/PhysicsScene.cpp

@@ -41,11 +41,27 @@ bool PhysicsScene::FixInvalidScales()
 	return success;
 }
 
-void PhysicsScene::CreateBodies(PhysicsSystem *inSystem) const
+bool PhysicsScene::CreateBodies(PhysicsSystem *inSystem) const
 {
 	BodyInterface &bi = inSystem->GetBodyInterface();
+
+	// Create bodies
+	BodyIDVector body_ids;
+	body_ids.reserve(mBodies.size());
 	for (const BodyCreationSettings &b : mBodies)
-		bi.CreateAndAddBody(b, EActivation::Activate);
+	{
+		const Body *body = bi.CreateBody(b);
+		if (body == nullptr)
+			break; // Out of bodies
+		body_ids.push_back(body->GetID());
+	}
+
+	// Batch add bodies
+	BodyInterface::AddState add_state = bi.AddBodiesPrepare(body_ids.data(), (int)body_ids.size());
+	bi.AddBodiesFinalize(body_ids.data(), (int)body_ids.size(), add_state, EActivation::Activate);
+
+	// Return true if all bodies were added
+	return body_ids.size() == mBodies.size();
 }
 
 void PhysicsScene::SaveBinaryState(StreamOut &inStream, bool inSaveShapes, bool inSaveGroupFilter) const

Jolt/Physics/PhysicsScene.h

@@ -26,8 +26,8 @@ public:
 	const vector<BodyCreationSettings> &	GetBodies() const								{ return mBodies; }
 	vector<BodyCreationSettings> &			GetBodies()										{ return mBodies; }
 
-	/// Instantiate all bodies
-	void									CreateBodies(PhysicsSystem *inSystem) const;
+	/// Instantiate all bodies, returns false if not all bodies could be created
+	bool									CreateBodies(PhysicsSystem *inSystem) const;
 
 	/// Go through all body creation settings and fix shapes that are scaled incorrectly (note this will change the scene a bit).
 	/// @return False when not all scales could be fixed.