QuadTree will now fall back to the heap when running out of stack space during collision queries (#1675)

Previously this would trigger an assert and some collisions would not be detected.

See discussion at: godotengine/godot#107454

Docs/Architecture.md

@@ -41,7 +41,7 @@ If you need to add many bodies at the same time then use the batching functions:
 - BodyInterface::AddBodiesAbort - If you've called AddBodiesPrepare but changed your mind and no longer want to add the bodies to the PhysicsSystem. Useful when streaming in level sections and the player decides to go the other way.
 - BodyInterface::RemoveBodies - Batch remove a lot of bodies from the PhysicsSystem.
 
-Always use the batch adding functions when possible! Adding many bodies, one at a time, results in a really inefficient broadphase and in the worst case can lead to missed collisions (an assert will trigger if this is the case). If you cannot avoid adding many bodies one at a time, use PhysicsSystem::OptimizeBroadPhase to rebuild the tree.
+Always use the batch adding functions when possible! Adding many bodies, one at a time, results in a really inefficient broadphase (a trace will notify when this happens) and in extreme cases may lead to the broadphase running out of internal nodes (std::abort will be called in that case). If you cannot avoid adding many bodies one at a time, use PhysicsSystem::OptimizeBroadPhase to rebuild the tree.
 
 You can call AddBody, RemoveBody, AddBody, RemoveBody to temporarily remove and later reinsert a body into the simulation.
 

Docs/ReleaseNotes.md

@@ -14,6 +14,7 @@ For breaking API changes see [this document](https://github.com/jrouwe/JoltPhysi
 
 ### Bug Fixes
 
+* QuadTree will now fall back to the heap when running out of stack space during collision queries. Previously this would trigger an assert and some collisions would not be detected.
 * Fixed `BodyInterface::MoveKinematic`, `SetLinearVelocity`, `SetAngularVelocity`, `SetLinearAndAngularVelocity`, `AddLinearVelocity`, `AddLinearAndAngularVelocity`, `SetPositionRotationAndVelocity` and `SetMotionType` when body not added to the physics system yet.
 * Fixed UBSAN false positive error that detected a dirty trick in `SimShapeFilter``.
 * WheelSettingsTV and WheelSettingsWV were not serializing their base class members.

Jolt/Physics/Collision/BroadPhase/QuadTree.cpp

@@ -122,6 +122,18 @@ bool QuadTree::Node::EncapsulateChildBounds(int inChildIndex, const AABox &inBou
 
 const AABox QuadTree::cInvalidBounds(Vec3::sReplicate(cLargeFloat), Vec3::sReplicate(-cLargeFloat));
 
+static inline void sQuadTreePerformanceWarning()
+{
+#ifdef JPH_ENABLE_ASSERTS
+	static atomic<bool> triggered_report { false };
+	bool expected = false;
+	if (triggered_report.compare_exchange_strong(expected, true))
+		Trace("QuadTree: Performance warning: Stack full!\n"
+			"This must be a very deep tree. Are you batch adding bodies through BodyInterface::AddBodiesPrepare/AddBodiesFinalize?\n"
+			"If you add lots of bodies through BodyInterface::AddBody you may need to call PhysicsSystem::OptimizeBroadPhase to rebuild the tree.");
+#endif
+}
+
 void QuadTree::GetBodyLocation(const TrackingVector &inTracking, BodyID inBodyID, uint32 &outNodeIdx, uint32 &outChildIdx) const
 {
 	uint32 body_location = inTracking[inBodyID.GetIndex()].mBodyLocation;
@@ -293,14 +305,17 @@ void QuadTree::UpdatePrepare(const BodyVector &inBodies, TrackingVector &ioTrack
 	NodeID *cur_node_id = node_ids;
 
 	// Collect all bodies
-	NodeID node_stack[cStackSize];
-	node_stack[0] = root_node.GetNodeID();
-	JPH_ASSERT(node_stack[0].IsValid());
-	int top = 0;
+	Array<NodeID, STLLocalAllocator<NodeID, cStackSize>> node_stack;
+	node_stack.reserve(cStackSize);
+	node_stack.push_back(root_node.GetNodeID());
+	JPH_ASSERT(node_stack.front().IsValid());
 	do
 	{
+		// Pop node from stack
+		NodeID node_id = node_stack.back();
+		node_stack.pop_back();
+
 		// Check if node is a body
-		NodeID node_id = node_stack[top];
 		if (node_id.IsBody())
 		{
 			// Validate that we're still in the right layer
@@ -330,31 +345,14 @@ void QuadTree::UpdatePrepare(const BodyVector &inBodies, TrackingVector &ioTrack
 				// Node is changed, recurse and get all children
 				for (NodeID child_node_id : node.mChildNodeID)
 					if (child_node_id.IsValid())
-					{
-						if (top < cStackSize)
-						{
-							node_stack[top] = child_node_id;
-							top++;
-						}
-						else
-						{
-							JPH_ASSERT(false, "Stack full!\n"
-								"This must be a very deep tree. Are you batch adding bodies through BodyInterface::AddBodiesPrepare/AddBodiesFinalize?\n"
-								"If you add lots of bodies through BodyInterface::AddBody you may need to call PhysicsSystem::OptimizeBroadPhase to rebuild the tree.");
-
-							// Falling back to adding the node as a whole
-							*cur_node_id = child_node_id;
-							++cur_node_id;
-						}
-					}
+						node_stack.push_back(child_node_id);
 
 				// Mark node to be freed
 				mAllocator->AddObjectToBatch(mFreeNodeBatch, node_idx);
 			}
 		}
-		--top;
 	}
-	while (top >= 0);
+	while (!node_stack.empty());
 
 	// Check that our book keeping matches
 	uint32 num_node_ids = uint32(cur_node_id - node_ids);
@@ -988,7 +986,9 @@ JPH_INLINE void QuadTree::WalkTree(const ObjectLayerFilter &inObjectLayerFilter,
 	uint64 start = GetProcessorTickCount();
 #endif // JPH_TRACK_BROADPHASE_STATS
 
-	NodeID node_stack[cStackSize];
+	Array<NodeID, STLLocalAllocator<NodeID, cStackSize>> node_stack_array;
+	node_stack_array.resize(cStackSize);
+	NodeID *node_stack = node_stack_array.data();
 	node_stack[0] = root_node.GetNodeID();
 	int top = 0;
 	do
@@ -1027,33 +1027,34 @@ JPH_INLINE void QuadTree::WalkTree(const ObjectLayerFilter &inObjectLayerFilter,
 		{
 			JPH_IF_TRACK_BROADPHASE_STATS(++nodes_visited;)
 
-			// Check if stack can hold more nodes
-			if (top + 4 < cStackSize)
+			// Ensure there is space on the stack (falls back to heap if there isn't)
+			if (top + 4 >= (int)node_stack_array.size())
 			{
-				// Process normal node
-				const Node &node = mAllocator->Get(child_node_id.GetNodeIndex());
-				JPH_ASSERT(IsAligned(&node, JPH_CACHE_LINE_SIZE));
-
-				// Load bounds of 4 children
-				Vec4 bounds_minx = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMinX);
-				Vec4 bounds_miny = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMinY);
-				Vec4 bounds_minz = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMinZ);
-				Vec4 bounds_maxx = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMaxX);
-				Vec4 bounds_maxy = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMaxY);
-				Vec4 bounds_maxz = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMaxZ);
-
-				// Load ids for 4 children
-				UVec4 child_ids = UVec4::sLoadInt4Aligned((const uint32 *)&node.mChildNodeID[0]);
-
-				// Check which sub nodes to visit
-				int num_results = ioVisitor.VisitNodes(bounds_minx, bounds_miny, bounds_minz, bounds_maxx, bounds_maxy, bounds_maxz, child_ids, top);
-				child_ids.StoreInt4((uint32 *)&node_stack[top]);
-				top += num_results;
+				sQuadTreePerformanceWarning();
+				node_stack_array.resize(node_stack_array.size() << 1);
+				node_stack = node_stack_array.data();
+				ioVisitor.OnStackResized(node_stack_array.size());
 			}
-			else
-				JPH_ASSERT(false, "Stack full!\n"
-					"This must be a very deep tree. Are you batch adding bodies through BodyInterface::AddBodiesPrepare/AddBodiesFinalize?\n"
-					"If you add lots of bodies through BodyInterface::AddBody you may need to call PhysicsSystem::OptimizeBroadPhase to rebuild the tree.");
+
+			// Process normal node
+			const Node &node = mAllocator->Get(child_node_id.GetNodeIndex());
+			JPH_ASSERT(IsAligned(&node, JPH_CACHE_LINE_SIZE));
+
+			// Load bounds of 4 children
+			Vec4 bounds_minx = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMinX);
+			Vec4 bounds_miny = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMinY);
+			Vec4 bounds_minz = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMinZ);
+			Vec4 bounds_maxx = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMaxX);
+			Vec4 bounds_maxy = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMaxY);
+			Vec4 bounds_maxz = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMaxZ);
+
+			// Load ids for 4 children
+			UVec4 child_ids = UVec4::sLoadInt4Aligned((const uint32 *)&node.mChildNodeID[0]);
+
+			// Check which sub nodes to visit
+			int num_results = ioVisitor.VisitNodes(bounds_minx, bounds_miny, bounds_minz, bounds_maxx, bounds_maxy, bounds_maxz, child_ids, top);
+			child_ids.StoreInt4((uint32 *)&node_stack[top]);
+			top += num_results;
 		}
 
 		// Fetch next node until we find one that the visitor wants to see
@@ -1092,6 +1093,7 @@ void QuadTree::CastRay(const RayCast &inRay, RayCastBodyCollector &ioCollector,
 			mInvDirection(inRay.mDirection),
 			mCollector(ioCollector)
 		{
+			mFractionStack.resize(cStackSize);
 			mFractionStack[0] = -1;
 		}
 
@@ -1125,11 +1127,17 @@ void QuadTree::CastRay(const RayCast &inRay, RayCastBodyCollector &ioCollector,
 			mCollector.AddHit(result);
 		}
 
+		/// Called when the stack is resized, this allows us to resize the fraction stack to match the new stack size
+		JPH_INLINE void			OnStackResized(size_t inNewStackSize)
+		{
+			mFractionStack.resize(inNewStackSize);
+		}
+
 	private:
 		Vec3					mOrigin;
 		RayInvDirection			mInvDirection;
 		RayCastBodyCollector &	mCollector;
-		float					mFractionStack[cStackSize];
+		Array<float, STLLocalAllocator<float, cStackSize>> mFractionStack;
 	};
 
 	Visitor visitor(inRay, ioCollector);
@@ -1175,6 +1183,12 @@ void QuadTree::CollideAABox(const AABox &inBox, CollideShapeBodyCollector &ioCol
 			mCollector.AddHit(inBodyID);
 		}
 
+		/// Called when the stack is resized
+		JPH_INLINE void				OnStackResized([[maybe_unused]] size_t inNewStackSize) const
+		{
+			// Nothing to do
+		}
+
 	private:
 		const AABox &				mBox;
 		CollideShapeBodyCollector &	mCollector;
@@ -1226,7 +1240,13 @@ void QuadTree::CollideSphere(Vec3Arg inCenter, float inRadius, CollideShapeBodyC
 			mCollector.AddHit(inBodyID);
 		}
 
-	private:
+		/// Called when the stack is resized
+		JPH_INLINE void				OnStackResized([[maybe_unused]] size_t inNewStackSize) const
+		{
+			// Nothing to do
+		}
+
+private:
 		Vec4						mCenterX;
 		Vec4						mCenterY;
 		Vec4						mCenterZ;
@@ -1277,6 +1297,12 @@ void QuadTree::CollidePoint(Vec3Arg inPoint, CollideShapeBodyCollector &ioCollec
 			mCollector.AddHit(inBodyID);
 		}
 
+		/// Called when the stack is resized
+		JPH_INLINE void				OnStackResized([[maybe_unused]] size_t inNewStackSize) const
+		{
+			// Nothing to do
+		}
+
 	private:
 		Vec3						mPoint;
 		CollideShapeBodyCollector &	mCollector;
@@ -1325,6 +1351,12 @@ void QuadTree::CollideOrientedBox(const OrientedBox &inBox, CollideShapeBodyColl
 			mCollector.AddHit(inBodyID);
 		}
 
+		/// Called when the stack is resized
+		JPH_INLINE void				OnStackResized([[maybe_unused]] size_t inNewStackSize) const
+		{
+			// Nothing to do
+		}
+
 	private:
 		OrientedBox					mBox;
 		CollideShapeBodyCollector &	mCollector;
@@ -1346,6 +1378,7 @@ void QuadTree::CastAABox(const AABoxCast &inBox, CastShapeBodyCollector &ioColle
 			mInvDirection(inBox.mDirection),
 			mCollector(ioCollector)
 		{
+			mFractionStack.resize(cStackSize);
 			mFractionStack[0] = -1;
 		}
 
@@ -1383,12 +1416,18 @@ void QuadTree::CastAABox(const AABoxCast &inBox, CastShapeBodyCollector &ioColle
 			mCollector.AddHit(result);
 		}
 
+		/// Called when the stack is resized, this allows us to resize the fraction stack to match the new stack size
+		JPH_INLINE void				OnStackResized(size_t inNewStackSize)
+		{
+			mFractionStack.resize(inNewStackSize);
+		}
+
 	private:
 		Vec3						mOrigin;
 		Vec3						mExtent;
 		RayInvDirection				mInvDirection;
 		CastShapeBodyCollector &	mCollector;
-		float						mFractionStack[cStackSize];
+		Array<float, STLLocalAllocator<float, cStackSize>> mFractionStack;
 	};
 
 	Visitor visitor(inBox, ioCollector);
@@ -1406,7 +1445,9 @@ void QuadTree::FindCollidingPairs(const BodyVector &inBodies, const BodyID *inAc
 	JPH_ASSERT(inActiveBodies != nullptr);
 	JPH_ASSERT(inNumActiveBodies > 0);
 
-	NodeID node_stack[cStackSize];
+	Array<NodeID, STLLocalAllocator<NodeID, cStackSize>> node_stack_array;
+	node_stack_array.resize(cStackSize);
+	NodeID *node_stack = node_stack_array.data();
 
 	// Loop over all active bodies
 	for (int b1 = 0; b1 < inNumActiveBodies; ++b1)
@@ -1468,16 +1509,17 @@ void QuadTree::FindCollidingPairs(const BodyVector &inBodies, const BodyID *inAc
 					// Sort so that overlaps are first
 					child_ids = UVec4::sSort4True(overlap, child_ids);
 
-					// Push them onto the stack
-					if (top + 4 < cStackSize)
+					// Ensure there is space on the stack (falls back to heap if there isn't)
+					if (top + 4 >= (int)node_stack_array.size())
 					{
-						child_ids.StoreInt4((uint32 *)&node_stack[top]);
-						top += num_results;
+						sQuadTreePerformanceWarning();
+						node_stack_array.resize(node_stack_array.size() << 1);
+						node_stack = node_stack_array.data();
 					}
-					else
-						JPH_ASSERT(false, "Stack full!\n"
-							"This must be a very deep tree. Are you batch adding bodies through BodyInterface::AddBodiesPrepare/AddBodiesFinalize?\n"
-							"If you add lots of bodies through BodyInterface::AddBody you may need to call PhysicsSystem::OptimizeBroadPhase to rebuild the tree.");
+
+					// Push them onto the stack
+					child_ids.StoreInt4((uint32 *)&node_stack[top]);
+					top += num_results;
 				}
 			}
 			--top;
@@ -1597,6 +1639,7 @@ void QuadTree::DumpTree(const NodeID &inRoot, const char *inFileNamePrefix) cons
 
 	// Iterate the entire tree
 	Array<NodeID, STLLocalAllocator<NodeID, cStackSize>> node_stack;
+	node_stack.reserve(cStackSize);
 	node_stack.push_back(inRoot);
 	JPH_ASSERT(inRoot.IsValid());
 	do

Jolt/Physics/PhysicsSystem.h

@@ -200,6 +200,12 @@ public:
 	/// Returns a locking interface that locks the body so other threads cannot modify it.
 	inline const BodyLockInterfaceLocking &	GetBodyLockInterface() const					{ return mBodyLockInterfaceLocking; }
 
+	/// Broadphase layer filter that decides if two objects can collide, this was passed to the Init function.
+	const ObjectVsBroadPhaseLayerFilter &GetObjectVsBroadPhaseLayerFilter() const			{ return *mObjectVsBroadPhaseLayerFilter; }
+
+	/// Object layer filter that decides if two objects can collide, this was passed to the Init function.
+	const ObjectLayerPairFilter &GetObjectLayerPairFilter() const							{ return *mObjectLayerPairFilter; }
+
 	/// Get an broadphase layer filter that uses the default pair filter and a specified object layer to determine if broadphase layers collide
 	DefaultBroadPhaseLayerFilter GetDefaultBroadPhaseLayerFilter(ObjectLayer inLayer) const	{ return DefaultBroadPhaseLayerFilter(*mObjectVsBroadPhaseLayerFilter, inLayer); }
 

UnitTests/Physics/PhysicsTests.cpp

@@ -11,10 +11,18 @@
 #include <Jolt/Physics/Collision/Shape/SphereShape.h>
 #include <Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h>
 #include <Jolt/Physics/Collision/Shape/StaticCompoundShape.h>
+#include <Jolt/Physics/Collision/CollisionCollectorImpl.h>
+#include <Jolt/Physics/Collision/RayCast.h>
+#include <Jolt/Physics/Collision/CastResult.h>
+#include <Jolt/Physics/Collision/BroadPhase/BroadPhase.h>
 #include <Jolt/Physics/Body/BodyLockMulti.h>
 #include <Jolt/Physics/Constraints/PointConstraint.h>
 #include <Jolt/Physics/StateRecorderImpl.h>
 
+JPH_SUPPRESS_WARNINGS_STD_BEGIN
+#include <cstring>
+JPH_SUPPRESS_WARNINGS_STD_END
+
 TEST_SUITE("PhysicsTests")
 {
 	// Gravity vector
@@ -1495,6 +1503,77 @@ TEST_SUITE("PhysicsTests")
 		bi.DestroyBody(b2->GetID());
 	}
 
+	static int sStackFullMsgs = 0;
+	static int sOtherMsgs = 0;
+	static void sStackFullTrace(const char *inFMT, ...)
+	{
+		if (std::strstr(inFMT, "Stack full") != nullptr)
+			++sStackFullMsgs;
+		else
+			++sOtherMsgs;
+	}
+
+	TEST_CASE("TestAddSingleBodies")
+	{
+		PhysicsTestContext c(1.0f / 60.0f, 1, 0);
+
+		BodyInterface& bi = c.GetBodyInterface();
+		PhysicsSystem &sys = *c.GetSystem();
+
+		const int cMaxBodies = 128;
+
+		// Add individual bodies in a way that will create an inefficient broad phase and will trigger a warning on query
+		RefConst<Shape> sphere = new SphereShape(1.0f);
+		bi.CreateAndAddBody(BodyCreationSettings(sphere, RVec3::sZero(), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING), EActivation::Activate); // Leave this body
+		for (int repeat = 0; repeat < 10; ++repeat)
+		{
+			// Create cMaxBodies - 1 bodies
+			BodyIDVector body_ids;
+			for (int i = 0; i < cMaxBodies - 1; ++i)
+				body_ids.push_back(bi.CreateAndAddBody(BodyCreationSettings(sphere, RVec3::sZero(), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING), EActivation::Activate));
+
+			// In all but the last iteration, remove the bodies again
+			if (repeat < 9)
+				for (BodyID id : body_ids)
+				{
+					bi.DeactivateBody(id);
+					bi.RemoveBody(id);
+					bi.DestroyBody(id);
+				}
+		}
+
+		// Override the trace function to count how many times we get a "Stack full" message
+		TraceFunction old_trace = Trace;
+		sStackFullMsgs = 0;
+		sOtherMsgs = 0;
+		Trace = sStackFullTrace;
+
+		// Cast a ray
+		AllHitCollisionCollector<CastRayCollector> ray_collector;
+		sys.GetNarrowPhaseQuery().CastRay(RRayCast(RVec3(-2, 0, 0), Vec3(2, 0, 0)), {}, ray_collector);
+
+		// Find colliding pairs
+		BodyIDVector active_bodies;
+		sys.GetActiveBodies(EBodyType::RigidBody, active_bodies);
+		AllHitCollisionCollector<BodyPairCollector> body_pair_collector;
+		static_cast<const BroadPhase &>(sys.GetBroadPhaseQuery()).FindCollidingPairs(active_bodies.data(), (int)active_bodies.size(), 0.0f, sys.GetObjectVsBroadPhaseLayerFilter(), sys.GetObjectLayerPairFilter(), body_pair_collector);
+
+		// Restore the old trace function
+		Trace = old_trace;
+
+		// Assert that we got a "Stack full" message when asserts are enabled
+	#ifdef JPH_ENABLE_ASSERTS
+		CHECK(sStackFullMsgs == 1);
+	#else
+		CHECK(sStackFullMsgs == 0);
+	#endif
+		CHECK(sOtherMsgs == 0);
+
+		// Assert that we hit all bodies
+		CHECK(ray_collector.mHits.size() == cMaxBodies);
+		CHECK(body_pair_collector.mHits.size() == cMaxBodies * (cMaxBodies - 1) / 2);
+	}
+
 	TEST_CASE("TestOutOfContactConstraints")
 	{
 		// Create a context with space for 8 constraints