- Added define JPH_TRACK_BROADPHASE_STATS that allow you to collect stats about the broadphase performance (do not enable by default!) (#8)

- Added WalkTree function for broadphase and implemented all collision function using it to avoid duplicating too much code

Jolt/Core/Core.h

@@ -35,6 +35,7 @@
 	#pragma clang diagnostic ignored "-Winvalid-offsetof"
 	#pragma clang diagnostic ignored "-Wgnu-zero-variadic-macro-arguments"
 	#pragma clang diagnostic ignored "-Wdocumentation-unknown-command"
+	#pragma clang diagnostic ignored "-Wctad-maybe-unsupported"
 	#ifndef JPH_PLATFORM_ANDROID
 		#pragma clang diagnostic ignored "-Wimplicit-int-float-conversion"
 	#endif

Jolt/Physics/Collision/BroadPhase/BroadPhase.h

@@ -6,6 +6,13 @@
 #include <Physics/Collision/BroadPhase/BroadPhaseQuery.h>
 #include <Physics/Collision/BroadPhase/BroadPhaseLayer.h>
 
+// Shorthand function to ifdef out code if broadphase stats tracking is off
+#ifdef JPH_TRACK_BROADPHASE_STATS
+	#define JPH_IF_TRACK_BROADPHASE_STATS(...) __VA_ARGS__
+#else
+	#define JPH_IF_TRACK_BROADPHASE_STATS(...)
+#endif // JPH_TRACK_BROADPHASE_STATS
+
 namespace JPH {
 
 class BodyManager;
@@ -86,17 +93,18 @@ public:
 
 #if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED)
 	/// Set function that converts a broadphase layer to a human readable string for debugging purposes
-	void				SetBroadPhaseLayerToString(BroadPhaseLayerToString inBroadPhaseLayerToString) { JPH_ASSERT(inBroadPhaseLayerToString != nullptr); mBroadPhaseLayerToString = inBroadPhaseLayerToString; }
+	virtual void		SetBroadPhaseLayerToString(BroadPhaseLayerToString inBroadPhaseLayerToString) { /* Can be implemented by derived classes */ }
 #endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED
 
+#ifdef JPH_TRACK_BROADPHASE_STATS
+	/// Trace the collected broadphase stats in CSV form.
+	/// This report can be used to judge and tweak the efficiency of the broadphase.
+	virtual void		ReportStats()														{ /* Can be implemented by derived classes */ }
+#endif // JPH_TRACK_BROADPHASE_STATS
+
 protected:
 	/// Link to the body manager that manages the bodies in this broadphase
 	BodyManager *		mBodyManager = nullptr;
-
-#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED)
-	/// Debug function to convert a broadphase layer to a string
-	BroadPhaseLayerToString mBroadPhaseLayerToString = [](BroadPhaseLayer) { return "Layer"; };
-#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED
 };
 
 } // JPH

Jolt/Physics/Collision/BroadPhase/BroadPhaseQuadTree.cpp

@@ -412,7 +412,7 @@ void BroadPhaseQuadTree::CastRay(const RayCast &inRay, RayCastBodyCollector &ioC
 		const QuadTree &tree = mLayers[l];
 		if (tree.HasBodies() && inBroadPhaseLayerFilter.ShouldCollide(BroadPhaseLayer(l)))
 		{
-			JPH_PROFILE(mBroadPhaseLayerToString(BroadPhaseLayer(l)));
+			JPH_PROFILE(tree.GetName());
 			tree.CastRay(inRay, ioCollector, inObjectLayerFilter, mTracking);
 			if (ioCollector.ShouldEarlyOut())
 				break;
@@ -435,7 +435,7 @@ void BroadPhaseQuadTree::CollideAABox(const AABox &inBox, CollideShapeBodyCollec
 		const QuadTree &tree = mLayers[l];
 		if (tree.HasBodies() && inBroadPhaseLayerFilter.ShouldCollide(BroadPhaseLayer(l)))
 		{
-			JPH_PROFILE(mBroadPhaseLayerToString(BroadPhaseLayer(l)));
+			JPH_PROFILE(tree.GetName());
 			tree.CollideAABox(inBox, ioCollector, inObjectLayerFilter, mTracking);
 			if (ioCollector.ShouldEarlyOut())
 				break;
@@ -458,7 +458,7 @@ void BroadPhaseQuadTree::CollideSphere(Vec3Arg inCenter, float inRadius, Collide
 		const QuadTree &tree = mLayers[l];
 		if (tree.HasBodies() && inBroadPhaseLayerFilter.ShouldCollide(BroadPhaseLayer(l)))
 		{
-			JPH_PROFILE(mBroadPhaseLayerToString(BroadPhaseLayer(l)));
+			JPH_PROFILE(tree.GetName());
 			tree.CollideSphere(inCenter, inRadius, ioCollector, inObjectLayerFilter, mTracking);
 			if (ioCollector.ShouldEarlyOut())
 				break;
@@ -481,7 +481,7 @@ void BroadPhaseQuadTree::CollidePoint(Vec3Arg inPoint, CollideShapeBodyCollector
 		const QuadTree &tree = mLayers[l];
 		if (tree.HasBodies() && inBroadPhaseLayerFilter.ShouldCollide(BroadPhaseLayer(l)))
 		{
-			JPH_PROFILE(mBroadPhaseLayerToString(BroadPhaseLayer(l)));
+			JPH_PROFILE(tree.GetName());
 			tree.CollidePoint(inPoint, ioCollector, inObjectLayerFilter, mTracking);
 			if (ioCollector.ShouldEarlyOut())
 				break;
@@ -504,7 +504,7 @@ void BroadPhaseQuadTree::CollideOrientedBox(const OrientedBox &inBox, CollideSha
 		const QuadTree &tree = mLayers[l];
 		if (tree.HasBodies() && inBroadPhaseLayerFilter.ShouldCollide(BroadPhaseLayer(l)))
 		{
-			JPH_PROFILE(mBroadPhaseLayerToString(BroadPhaseLayer(l)));
+			JPH_PROFILE(tree.GetName());
 			tree.CollideOrientedBox(inBox, ioCollector, inObjectLayerFilter, mTracking);
 			if (ioCollector.ShouldEarlyOut())
 				break;
@@ -527,7 +527,7 @@ void BroadPhaseQuadTree::CastAABox(const AABoxCast &inBox, CastShapeBodyCollecto
 		const QuadTree &tree = mLayers[l];
 		if (tree.HasBodies() && inBroadPhaseLayerFilter.ShouldCollide(BroadPhaseLayer(l)))
 		{
-			JPH_PROFILE(mBroadPhaseLayerToString(BroadPhaseLayer(l)));
+			JPH_PROFILE(tree.GetName());
 			tree.CastAABox(inBox, ioCollector, inObjectLayerFilter, mTracking);
 			if (ioCollector.ShouldEarlyOut())
 				break;
@@ -564,7 +564,7 @@ void BroadPhaseQuadTree::FindCollidingPairs(BodyID *ioActiveBodies, int inNumAct
 			const QuadTree &tree = mLayers[l];
 			if (tree.HasBodies() && inObjectVsBroadPhaseLayerFilter(object_layer, BroadPhaseLayer(l)))
 			{
-				JPH_PROFILE(mBroadPhaseLayerToString(BroadPhaseLayer(l)));
+				JPH_PROFILE(tree.GetName());
 				tree.FindCollidingPairs(bodies, b_start, int(b_mid - b_start), inSpeculativeContactDistance, ioPairCollector, inObjectLayerPairFilter);
 			}
 		}
@@ -574,4 +574,26 @@ void BroadPhaseQuadTree::FindCollidingPairs(BodyID *ioActiveBodies, int inNumAct
 	}
 }
 
+#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED)
+
+void BroadPhaseQuadTree::SetBroadPhaseLayerToString(BroadPhaseLayerToString inBroadPhaseLayerToString)
+{
+	// Set the name on the sub trees
+	for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l)
+		mLayers[l].SetName(inBroadPhaseLayerToString(BroadPhaseLayer(l)));
+}
+
+#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED
+
+#ifdef JPH_TRACK_BROADPHASE_STATS
+
+void BroadPhaseQuadTree::ReportStats()
+{
+	Trace("Query Type, Filter Description, Tree Name, Num Queries, Total Time (ms), Nodes Visited, Bodies Visited, Hits Reported, Hits Reported vs Bodies Visited (%%), Hits Reported vs Nodes Visited");
+	for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l)
+		mLayers[l].ReportStats();
+}
+
+#endif // JPH_TRACK_BROADPHASE_STATS
+
 } // JPH

Jolt/Physics/Collision/BroadPhase/BroadPhaseQuadTree.h

@@ -36,6 +36,12 @@ public:
 	virtual void			CollideOrientedBox(const OrientedBox &inBox, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override;
 	virtual void			CastAABox(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override;
 	virtual void			FindCollidingPairs(BodyID *ioActiveBodies, int inNumActiveBodies, float inSpeculativeContactDistance, ObjectVsBroadPhaseLayerFilter inObjectVsBroadPhaseLayerFilter, ObjectLayerPairFilter inObjectLayerPairFilter, BodyPairCollector &ioPairCollector) const override;
+#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED)
+	virtual void			SetBroadPhaseLayerToString(BroadPhaseLayerToString inBroadPhaseLayerToString) override;
+#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED
+#ifdef JPH_TRACK_BROADPHASE_STATS
+	virtual void			ReportStats() override;
+#endif // JPH_TRACK_BROADPHASE_STATS
 
 private:
 	/// Helper struct for AddBodies handle

Jolt/Physics/Collision/BroadPhase/QuadTree.cpp

@@ -931,50 +931,64 @@ void QuadTree::NotifyBodiesAABBChanged(const BodyVector &inBodies, const Trackin
 	}
 }
 
-void QuadTree::CastRay(const RayCast &inRay, RayCastBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const
+template <class Visitor>
+JPH_INLINE void QuadTree::WalkTree(const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking, Visitor &ioVisitor JPH_IF_TRACK_BROADPHASE_STATS(, LayerToStats &ioStats)) const
 {
 	// Get the root
 	const RootNode &root_node = GetCurrentRoot();
 
-	// Properties of ray
-	Vec3 origin(inRay.mOrigin);
-	RayInvDirection inv_direction(inRay.mDirection);
+#ifdef JPH_TRACK_BROADPHASE_STATS
+	// Start tracking stats
+	int bodies_visited = 0;
+	int hits_collected = 0;
+	int nodes_visited = 0;
+	uint64 collector_ticks = 0;
+
+	uint64 start = GetProcessorTickCount();
+#endif // JPH_TRACK_BROADPHASE_STATS
 
 	NodeID node_stack[cStackSize];
-	float fraction_stack[cStackSize];
 	node_stack[0] = root_node.GetNodeID();
-	fraction_stack[0] = -1;
 	int top = 0;
-	float early_out_fraction = ioCollector.GetEarlyOutFraction();
 	do
 	{
 		// Check if node is a body
 		NodeID child_node_id = node_stack[top];
 		if (child_node_id.IsBody())
 		{
-			float fraction = fraction_stack[top];
-			if (fraction < early_out_fraction)
+			// Track amount of bodies visited
+			JPH_IF_TRACK_BROADPHASE_STATS(++bodies_visited;)
+
+			BodyID body_id = child_node_id.GetBodyID();
+			ObjectLayer object_layer = inTracking[body_id.GetIndex()].mObjectLayer; // We're not taking a lock on the body, so it may be in the process of being removed so check if the object layer is invalid
+			if (object_layer != cObjectLayerInvalid && inObjectLayerFilter.ShouldCollide(object_layer))
 			{
-				BodyID body_id = child_node_id.GetBodyID();
-				ObjectLayer object_layer = inTracking[body_id.GetIndex()].mObjectLayer; // We're not taking a lock on the body, so it may be in the process of being removed so check if the object layer is invalid
-				if (object_layer != cObjectLayerInvalid && inObjectLayerFilter.ShouldCollide(object_layer))
-				{
-					// Store potential hit with body
-					BroadPhaseCastResult result { body_id, fraction };
-					ioCollector.AddHit(result);
-					if (ioCollector.ShouldEarlyOut())
-						break;
-					early_out_fraction = ioCollector.GetEarlyOutFraction();
-				}
+				// Track amount of hits
+				JPH_IF_TRACK_BROADPHASE_STATS(++hits_collected;)
+
+				// Start track time the collector takes
+				JPH_IF_TRACK_BROADPHASE_STATS(uint64 collector_start = GetProcessorTickCount();)
+
+				// We found a body we collide with, call our visitor
+				ioVisitor.VisitBody(body_id, top);
+
+				// End track time the collector takes
+				JPH_IF_TRACK_BROADPHASE_STATS(collector_ticks += GetProcessorTickCount() - collector_start;)
+
+				// Check if we're done
+				if (ioVisitor.ShouldAbort())
+					break;
 			}
 		}
 		else if (child_node_id.IsValid())
 		{
+			JPH_IF_TRACK_BROADPHASE_STATS(++nodes_visited;)
+
 			// Process normal node
 			const Node &node = mAllocator->Get(child_node_id.GetNodeIndex());
 			JPH_ASSERT(IsAligned(&node, JPH_CACHE_LINE_SIZE));
 
-			// Test bounds of 4 children
+			// 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);
@@ -982,27 +996,16 @@ void QuadTree::CastRay(const RayCast &inRay, RayCastBodyCollector &ioCollector,
 			Vec4 bounds_maxy = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMaxY);
 			Vec4 bounds_maxz = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMaxZ);
 
-			Vec4 fraction = RayAABox4(origin, inv_direction, bounds_minx, bounds_miny, bounds_minz, bounds_maxx, bounds_maxy, bounds_maxz);
+			// Load ids for 4 children
+			UVec4 child_ids = UVec4::sLoadInt4Aligned((const uint32 *)&node.mChildNodeID[0]);
 
-			// Count how many results are hitting
-			UVec4 hitting = Vec4::sLess(fraction, Vec4::sReplicate(early_out_fraction));
-			int num_results = hitting.CountTrues();
+			// 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);
 			if (num_results > 0)
 			{
-				// Load ids for 4 children
-				UVec4 child_ids = UVec4::sLoadInt4Aligned((const uint32 *)&node.mChildNodeID[0]);
-
-				// Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom)
-				Vec4::sSort4Reverse(fraction, child_ids);
-
-				// Shift the results so that only the hitting ones remain
-				fraction = fraction.ReinterpretAsInt().ShiftComponents4Minus(num_results).ReinterpretAsFloat();
-				child_ids = child_ids.ShiftComponents4Minus(num_results);
-
 				// Push them onto the stack
 				if (top + 4 < cStackSize)
 				{
-					fraction.StoreFloat4((Float4 *)&fraction_stack[top]);
 					child_ids.StoreInt4((uint32 *)&node_stack[top]);
 					top += num_results;
 				}
@@ -1010,373 +1013,417 @@ void QuadTree::CastRay(const RayCast &inRay, RayCastBodyCollector &ioCollector,
 					JPH_ASSERT(false, "Stack full!");
 			}
 		}
-		--top;
+
+		// Fetch next node until we find one that the visitor wants to see
+		do 
+			--top;
+		while (top >= 0 && !ioVisitor.ShouldVisitNode(top));
 	} 
 	while (top >= 0);
+
+#ifdef JPH_TRACK_BROADPHASE_STATS
+	// Calculate total time the broadphase walk took
+	uint64 total_ticks = GetProcessorTickCount() - start;
+
+	// Update stats under lock protection (slow!)
+	{
+		unique_lock lock(mStatsMutex);
+		Stat &s = ioStats[inObjectLayerFilter.GetDescription()];
+		s.mNumQueries++;
+		s.mNodesVisited += nodes_visited;
+		s.mBodiesVisited += bodies_visited;
+		s.mHitsReported += hits_collected;
+		s.mTotalTicks += total_ticks;
+		s.mCollectorTicks += collector_ticks;
+	}
+#endif // JPH_TRACK_BROADPHASE_STATS
 }
 
-void QuadTree::CollideAABox(const AABox &inBox, CollideShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const
+void QuadTree::CastRay(const RayCast &inRay, RayCastBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const
 {
-	// Get the root
-	const RootNode &root_node = GetCurrentRoot();
-
-	NodeID node_stack[cStackSize];
-	node_stack[0] = root_node.GetNodeID();
-	int top = 0;
-	do
+	class Visitor
 	{
-		// Check if node is a body
-		NodeID child_node_id = node_stack[top];
-		if (child_node_id.IsBody())
+	public:
+		/// Constructor
+		JPH_INLINE				Visitor(const RayCast &inRay, RayCastBodyCollector &ioCollector) :
+			mOrigin(inRay.mOrigin),
+			mInvDirection(inRay.mDirection),
+			mCollector(ioCollector)
 		{
-			BodyID body_id = child_node_id.GetBodyID();
-			ObjectLayer object_layer = inTracking[body_id.GetIndex()].mObjectLayer; // We're not taking a lock on the body, so it may be in the process of being removed so check if the object layer is invalid
-			if (object_layer != cObjectLayerInvalid && inObjectLayerFilter.ShouldCollide(object_layer))
-			{
-				// Store potential hit with body
-				ioCollector.AddHit(body_id);
-				if (ioCollector.ShouldEarlyOut())
-					break;
-			}
+			mFractionStack[0] = -1;
 		}
-		else if (child_node_id.IsValid())
+
+		/// Returns true if further processing of the tree should be aborted
+		JPH_INLINE bool			ShouldAbort() const
 		{
-			// Process normal node
-			const Node &node = mAllocator->Get(child_node_id.GetNodeIndex());
-			JPH_ASSERT(IsAligned(&node, JPH_CACHE_LINE_SIZE));
+			return mCollector.ShouldEarlyOut();
+		}
 
-			// Test 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);
+		/// Returns true if this node / body should be visited, false if no hit can be generated
+		JPH_INLINE bool			ShouldVisitNode(int inStackTop) const
+		{
+			return mFractionStack[inStackTop] < mCollector.GetEarlyOutFraction();
+		}
 
-			UVec4 hitting = AABox4VsBox(inBox, bounds_minx, bounds_miny, bounds_minz, bounds_maxx, bounds_maxy, bounds_maxz);
+		/// Visit nodes, returns number of hits found and sorts ioChildNodeIDs so that they are at the beginning of the vector.
+		JPH_INLINE int			VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioChildNodeIDs, int inStackTop) 
+		{
+			// Test the ray against 4 bounding boxes
+			Vec4 fraction = RayAABox4(mOrigin, mInvDirection, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
 
 			// Count how many results are hitting
+			UVec4 hitting = Vec4::sLess(fraction, Vec4::sReplicate(mCollector.GetEarlyOutFraction()));
 			int num_results = hitting.CountTrues();
 			if (num_results > 0)
 			{
-				// Load ids for 4 children
-				UVec4 child_ids = UVec4::sLoadInt4Aligned((const uint32 *)&node.mChildNodeID[0]);
+				// Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom)
+				Vec4::sSort4Reverse(fraction, ioChildNodeIDs);
 
-				// Sort trues first
-				UVec4::sSort4True(hitting, child_ids);
+				// Shift the results so that only the hitting ones remain
+				ioChildNodeIDs = ioChildNodeIDs.ShiftComponents4Minus(num_results);
+				fraction = fraction.ReinterpretAsInt().ShiftComponents4Minus(num_results).ReinterpretAsFloat();
 
 				// Push them onto the stack
-				if (top + 4 < cStackSize)
-				{
-					child_ids.StoreInt4((uint32 *)&node_stack[top]);
-					top += num_results;
-				}
-				else
-					JPH_ASSERT(false, "Stack full!");
+				if (inStackTop + 4 < cStackSize)
+					fraction.StoreFloat4((Float4 *)&mFractionStack[inStackTop]);
 			}
+
+			return num_results;
 		}
-		--top;
-	} 
-	while (top >= 0);
+
+		/// Visit a body, returns false if the algorithm should terminate because no hits can be generated anymore
+		JPH_INLINE void			VisitBody(const BodyID &inBodyID, int inStackTop)
+		{
+			// Store potential hit with body
+			BroadPhaseCastResult result { inBodyID, mFractionStack[inStackTop] };
+			mCollector.AddHit(result);
+		}
+
+	private:
+		Vec3					mOrigin;
+		RayInvDirection			mInvDirection;
+		RayCastBodyCollector &	mCollector;
+		float					mFractionStack[cStackSize];
+	};
+
+	Visitor visitor(inRay, ioCollector);
+	WalkTree(inObjectLayerFilter, inTracking, visitor JPH_IF_TRACK_BROADPHASE_STATS(, mCastRayStats));
 }
 
-void QuadTree::CollideSphere(Vec3Arg inCenter, float inRadius, CollideShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const
+void QuadTree::CollideAABox(const AABox &inBox, CollideShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const
 {
-	// Get the root
-	const RootNode &root_node = GetCurrentRoot();
+	class Visitor
+	{
+	public:
+		/// Constructor
+		JPH_INLINE					Visitor(const AABox &inBox, CollideShapeBodyCollector &ioCollector) :
+			mBox(inBox),
+			mCollector(ioCollector)
+		{
+		}
 
-	// Splat sphere to 4 component vectors
-	Vec4 center_x = Vec4(inCenter).SplatX();
-	Vec4 center_y = Vec4(inCenter).SplatY();
-	Vec4 center_z = Vec4(inCenter).SplatZ();
-	Vec4 radius_sq = Vec4::sReplicate(Square(inRadius));
+		/// Returns true if further processing of the tree should be aborted
+		JPH_INLINE bool				ShouldAbort() const
+		{
+			return mCollector.ShouldEarlyOut();
+		}
 
-	NodeID node_stack[cStackSize];
-	node_stack[0] = root_node.GetNodeID();
-	int top = 0;
-	do
-	{
-		// Check if node is a body
-		NodeID child_node_id = node_stack[top];
-		if (child_node_id.IsBody())
+		/// Returns true if this node / body should be visited, false if no hit can be generated
+		JPH_INLINE bool				ShouldVisitNode(int inStackTop) const
 		{
-			BodyID body_id = child_node_id.GetBodyID();
-			ObjectLayer object_layer = inTracking[body_id.GetIndex()].mObjectLayer; // We're not taking a lock on the body, so it may be in the process of being removed so check if the object layer is invalid
-			if (object_layer != cObjectLayerInvalid && inObjectLayerFilter.ShouldCollide(object_layer))
+			return true;
+		}
+
+		/// Visit nodes, returns number of hits found and sorts ioChildNodeIDs so that they are at the beginning of the vector.
+		JPH_INLINE int				VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioChildNodeIDs, int inStackTop) 
+		{
+			// Test the box vs 4 boxes
+			UVec4 hitting = AABox4VsBox(mBox, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
+
+			// Count how many results are hitting
+			int num_results = hitting.CountTrues();
+			if (num_results > 0)
 			{
-				// Store potential hit with body
-				ioCollector.AddHit(body_id);
-				if (ioCollector.ShouldEarlyOut())
-					break;
+				// Sort trues first
+				UVec4::sSort4True(hitting, ioChildNodeIDs);
 			}
+
+			return num_results;
 		}
-		else if (child_node_id.IsValid())
+
+		/// Visit a body, returns false if the algorithm should terminate because no hits can be generated anymore
+		JPH_INLINE void				VisitBody(const BodyID &inBodyID, int inStackTop)
 		{
-			// Process normal node
-			const Node &node = mAllocator->Get(child_node_id.GetNodeIndex());
-			JPH_ASSERT(IsAligned(&node, JPH_CACHE_LINE_SIZE));
+			// Store potential hit with body
+			mCollector.AddHit(inBodyID);
+		}
 
-			// Test 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);
+	private:
+		const AABox &				mBox;
+		CollideShapeBodyCollector &	mCollector;
+	};
+
+	Visitor visitor(inBox, ioCollector);
+	WalkTree(inObjectLayerFilter, inTracking, visitor JPH_IF_TRACK_BROADPHASE_STATS(, mCollideAABoxStats));
+}
+
+void QuadTree::CollideSphere(Vec3Arg inCenter, float inRadius, CollideShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const
+{
+	class Visitor
+	{
+	public:
+		/// Constructor
+		JPH_INLINE					Visitor(Vec3Arg inCenter, float inRadius, CollideShapeBodyCollector &ioCollector) :
+			mCenterX(inCenter.SplatX()),
+			mCenterY(inCenter.SplatY()),
+			mCenterZ(inCenter.SplatZ()),
+			mRadiusSq(Vec4::sReplicate(Square(inRadius))),
+			mCollector(ioCollector)
+		{
+		}
+
+		/// Returns true if further processing of the tree should be aborted
+		JPH_INLINE bool				ShouldAbort() const
+		{
+			return mCollector.ShouldEarlyOut();
+		}
 
+		/// Returns true if this node / body should be visited, false if no hit can be generated
+		JPH_INLINE bool				ShouldVisitNode(int inStackTop) const
+		{
+			return true;
+		}
+
+		/// Visit nodes, returns number of hits found and sorts ioChildNodeIDs so that they are at the beginning of the vector.
+		JPH_INLINE int				VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioChildNodeIDs, int inStackTop) 
+		{
 			// Get closest point on box
-			Vec4 closest_x = Vec4::sMin(Vec4::sMax(center_x, bounds_minx), bounds_maxx);
-			Vec4 closest_y = Vec4::sMin(Vec4::sMax(center_y, bounds_miny), bounds_maxy);
-			Vec4 closest_z = Vec4::sMin(Vec4::sMax(center_z, bounds_minz), bounds_maxz);
+			Vec4 closest_x = Vec4::sMin(Vec4::sMax(mCenterX, inBoundsMinX), inBoundsMaxX);
+			Vec4 closest_y = Vec4::sMin(Vec4::sMax(mCenterY, inBoundsMinY), inBoundsMaxY);
+			Vec4 closest_z = Vec4::sMin(Vec4::sMax(mCenterZ, inBoundsMinZ), inBoundsMaxZ);
 
 			// Test the distance from the center of the sphere to the box is smaller than the radius
-			Vec4 distance_sq = Square(closest_x - center_x) + Square(closest_y - center_y) + Square(closest_z - center_z);
-			UVec4 hitting = Vec4::sLessOrEqual(distance_sq, radius_sq);
+			Vec4 distance_sq = Square(closest_x - mCenterX) + Square(closest_y - mCenterY) + Square(closest_z - mCenterZ);
+			UVec4 hitting = Vec4::sLessOrEqual(distance_sq, mRadiusSq);
 
 			// Count how many results are hitting
 			int num_results = hitting.CountTrues();
 			if (num_results > 0)
 			{
-				// Load ids for 4 children
-				UVec4 child_ids = UVec4::sLoadInt4Aligned((const uint32 *)&node.mChildNodeID[0]);
-
 				// Sort trues first
-				UVec4::sSort4True(hitting, child_ids);
-
-				// Push them onto the stack
-				if (top + 4 < cStackSize)
-				{
-					child_ids.StoreInt4((uint32 *)&node_stack[top]);
-					top += num_results;
-				}
-				else
-					JPH_ASSERT(false, "Stack full!");
+				UVec4::sSort4True(hitting, ioChildNodeIDs);
 			}
+
+			return num_results;
 		}
-		--top;
-	} 
-	while (top >= 0);
+
+		/// Visit a body, returns false if the algorithm should terminate because no hits can be generated anymore
+		JPH_INLINE void				VisitBody(const BodyID &inBodyID, int inStackTop)
+		{
+			// Store potential hit with body
+			mCollector.AddHit(inBodyID);
+		}
+
+	private:
+		Vec4						mCenterX;
+		Vec4						mCenterY;
+		Vec4						mCenterZ;
+		Vec4						mRadiusSq;
+		CollideShapeBodyCollector &	mCollector;
+	};
+
+	Visitor visitor(inCenter, inRadius, ioCollector);
+	WalkTree(inObjectLayerFilter, inTracking, visitor JPH_IF_TRACK_BROADPHASE_STATS(, mCollideSphereStats));
 }
 
 void QuadTree::CollidePoint(Vec3Arg inPoint, CollideShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const
 {
-	// Get the root
-	const RootNode &root_node = GetCurrentRoot();
-
-	NodeID node_stack[cStackSize];
-	node_stack[0] = root_node.GetNodeID();
-	int top = 0;
-	do
+	class Visitor
 	{
-		// Check if node is a body
-		NodeID child_node_id = node_stack[top];
-		if (child_node_id.IsBody())
+	public:
+		/// Constructor
+		JPH_INLINE					Visitor(Vec3Arg inPoint, CollideShapeBodyCollector &ioCollector) :
+			mPoint(inPoint),
+			mCollector(ioCollector)
 		{
-			BodyID body_id = child_node_id.GetBodyID();
-			ObjectLayer object_layer = inTracking[body_id.GetIndex()].mObjectLayer; // We're not taking a lock on the body, so it may be in the process of being removed so check if the object layer is invalid
-			if (object_layer != cObjectLayerInvalid && inObjectLayerFilter.ShouldCollide(object_layer))
-			{
-				// Store potential hit with body
-				ioCollector.AddHit(body_id);
-				if (ioCollector.ShouldEarlyOut())
-					break;
-			}
 		}
-		else if (child_node_id.IsValid())
+
+		/// Returns true if further processing of the tree should be aborted
+		JPH_INLINE bool				ShouldAbort() const
 		{
-			// Process normal node
-			const Node &node = mAllocator->Get(child_node_id.GetNodeIndex());
-			JPH_ASSERT(IsAligned(&node, JPH_CACHE_LINE_SIZE));
+			return mCollector.ShouldEarlyOut();
+		}
 
-			// Test 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);
+		/// Returns true if this node / body should be visited, false if no hit can be generated
+		JPH_INLINE bool				ShouldVisitNode(int inStackTop) const
+		{
+			return true;
+		}
 
+		/// Visit nodes, returns number of hits found and sorts ioChildNodeIDs so that they are at the beginning of the vector.
+		JPH_INLINE int				VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioChildNodeIDs, int inStackTop) 
+		{
 			// Test if point overlaps with box
-			UVec4 hitting = AABox4VsPoint(inPoint, bounds_minx, bounds_miny, bounds_minz, bounds_maxx, bounds_maxy, bounds_maxz);
+			UVec4 hitting = AABox4VsPoint(mPoint, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
 
 			// Count how many results are hitting
 			int num_results = hitting.CountTrues();
 			if (num_results > 0)
 			{
-				// Load ids for 4 children
-				UVec4 child_ids = UVec4::sLoadInt4Aligned((const uint32 *)&node.mChildNodeID[0]);
-
 				// Sort trues first
-				UVec4::sSort4True(hitting, child_ids);
-
-				// Push them onto the stack
-				if (top + 4 < cStackSize)
-				{
-					child_ids.StoreInt4((uint32 *)&node_stack[top]);
-					top += num_results;
-				}
-				else
-					JPH_ASSERT(false, "Stack full!");
+				UVec4::sSort4True(hitting, ioChildNodeIDs);
 			}
+
+			return num_results;
 		}
-		--top;
-	} 
-	while (top >= 0);
+
+		/// Visit a body, returns false if the algorithm should terminate because no hits can be generated anymore
+		JPH_INLINE void				VisitBody(const BodyID &inBodyID, int inStackTop)
+		{
+			// Store potential hit with body
+			mCollector.AddHit(inBodyID);
+		}
+
+	private:
+		Vec3						mPoint;
+		CollideShapeBodyCollector &	mCollector;
+	};
+
+	Visitor visitor(inPoint, ioCollector);
+	WalkTree(inObjectLayerFilter, inTracking, visitor JPH_IF_TRACK_BROADPHASE_STATS(, mCollidePointStats));
 }
 
 void QuadTree::CollideOrientedBox(const OrientedBox &inBox, CollideShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const
 {
-	// Get the root
-	const RootNode &root_node = GetCurrentRoot();
-
-	NodeID node_stack[cStackSize];
-	node_stack[0] = root_node.GetNodeID();
-	int top = 0;
-	do
+	class Visitor
 	{
-		// Check if node is a body
-		NodeID child_node_id = node_stack[top];
-		if (child_node_id.IsBody())
+	public:
+		/// Constructor
+		JPH_INLINE					Visitor(const OrientedBox &inBox, CollideShapeBodyCollector &ioCollector) :
+			mBox(inBox),
+			mCollector(ioCollector)
 		{
-			BodyID body_id = child_node_id.GetBodyID();
-			ObjectLayer object_layer = inTracking[body_id.GetIndex()].mObjectLayer; // We're not taking a lock on the body, so it may be in the process of being removed so check if the object layer is invalid
-			if (object_layer != cObjectLayerInvalid && inObjectLayerFilter.ShouldCollide(object_layer))
-			{
-				// Store potential hit with body
-				ioCollector.AddHit(body_id);
-				if (ioCollector.ShouldEarlyOut())
-					break;
-			}
 		}
-		else if (child_node_id.IsValid())
+
+		/// Returns true if further processing of the tree should be aborted
+		JPH_INLINE bool				ShouldAbort() const
 		{
-			// Process normal node
-			const Node &node = mAllocator->Get(child_node_id.GetNodeIndex());
-			JPH_ASSERT(IsAligned(&node, JPH_CACHE_LINE_SIZE));
+			return mCollector.ShouldEarlyOut();
+		}
 
-			// Test 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);
+		/// Returns true if this node / body should be visited, false if no hit can be generated
+		JPH_INLINE bool				ShouldVisitNode(int inStackTop) const
+		{
+			return true;
+		}
 
+		/// Visit nodes, returns number of hits found and sorts ioChildNodeIDs so that they are at the beginning of the vector.
+		JPH_INLINE int				VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioChildNodeIDs, int inStackTop) 
+		{
 			// Test if point overlaps with box
-			UVec4 hitting = AABox4VsBox(inBox, bounds_minx, bounds_miny, bounds_minz, bounds_maxx, bounds_maxy, bounds_maxz);
+			UVec4 hitting = AABox4VsBox(mBox, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
 
 			// Count how many results are hitting
 			int num_results = hitting.CountTrues();
 			if (num_results > 0)
 			{
-				// Load ids for 4 children
-				UVec4 child_ids = UVec4::sLoadInt4Aligned((const uint32 *)&node.mChildNodeID[0]);
-
 				// Sort trues first
-				UVec4::sSort4True(hitting, child_ids);
-
-				// Push them onto the stack
-				if (top + 4 < cStackSize)
-				{
-					child_ids.StoreInt4((uint32 *)&node_stack[top]);
-					top += num_results;
-				}
-				else
-					JPH_ASSERT(false, "Stack full!");
+				UVec4::sSort4True(hitting, ioChildNodeIDs);
 			}
+
+			return num_results;
 		}
-		--top;
-	} 
-	while (top >= 0);
+
+		/// Visit a body, returns false if the algorithm should terminate because no hits can be generated anymore
+		JPH_INLINE void				VisitBody(const BodyID &inBodyID, int inStackTop)
+		{
+			// Store potential hit with body
+			mCollector.AddHit(inBodyID);
+		}
+
+	private:
+		OrientedBox					mBox;
+		CollideShapeBodyCollector &	mCollector;
+	};
+
+	Visitor visitor(inBox, ioCollector);
+	WalkTree(inObjectLayerFilter, inTracking, visitor JPH_IF_TRACK_BROADPHASE_STATS(, mCollideOrientedBoxStats));
 }
 
 void QuadTree::CastAABox(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const
 {
-	// Get the root
-	const RootNode &root_node = GetCurrentRoot();
-
-	// Properties of ray
-	Vec3 origin(inBox.mBox.GetCenter());
-	Vec3 extent(inBox.mBox.GetExtent());
-	RayInvDirection inv_direction(inBox.mDirection);
-
-	NodeID node_stack[cStackSize];
-	float fraction_stack[cStackSize];
-	node_stack[0] = root_node.GetNodeID();
-	fraction_stack[0] = -1;
-	int top = 0;
-	float early_out_fraction = ioCollector.GetEarlyOutFraction();
-	do
+	class Visitor
 	{
-		// Check if node is a body
-		NodeID child_node_id = node_stack[top];
-		if (child_node_id.IsBody())
+	public:
+		/// Constructor
+		JPH_INLINE					Visitor(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector) :
+			mOrigin(inBox.mBox.GetCenter()),
+			mExtent(inBox.mBox.GetExtent()),
+			mInvDirection(inBox.mDirection),
+			mCollector(ioCollector)
 		{
-			float fraction = fraction_stack[top];
-			if (fraction < early_out_fraction)
-			{
-				BodyID body_id = child_node_id.GetBodyID();
-				ObjectLayer object_layer = inTracking[body_id.GetIndex()].mObjectLayer; // We're not taking a lock on the body, so it may be in the process of being removed so check if the object layer is invalid
-				if (object_layer != cObjectLayerInvalid && inObjectLayerFilter.ShouldCollide(object_layer))
-				{
-					// Store potential hit with body
-					BroadPhaseCastResult result { body_id, fraction };
-					ioCollector.AddHit(result);
-					if (ioCollector.ShouldEarlyOut())
-						break;
-					early_out_fraction = ioCollector.GetEarlyOutFraction();
-				}
-			}
+			mFractionStack[0] = -1;
 		}
-		else if (child_node_id.IsValid())
+
+		/// Returns true if further processing of the tree should be aborted
+		JPH_INLINE bool				ShouldAbort() const
 		{
-			// Process normal node
-			const Node &node = mAllocator->Get(child_node_id.GetNodeIndex());
-			JPH_ASSERT(IsAligned(&node, JPH_CACHE_LINE_SIZE));
+			return mCollector.ShouldEarlyOut();
+		}
 
-			// Get 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);
+		/// Returns true if this node / body should be visited, false if no hit can be generated
+		JPH_INLINE bool				ShouldVisitNode(int inStackTop) const
+		{
+			return mFractionStack[inStackTop] < mCollector.GetEarlyOutFraction();
+		}
 
+		/// Visit nodes, returns number of hits found and sorts ioChildNodeIDs so that they are at the beginning of the vector.
+		JPH_INLINE int				VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioChildNodeIDs, int inStackTop) 
+		{
 			// Enlarge them by the casted aabox extents
-			AABox4EnlargeWithExtent(extent, bounds_minx, bounds_miny, bounds_minz, bounds_maxx, bounds_maxy, bounds_maxz);
+			AABox4EnlargeWithExtent(mExtent, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
 
 			// Test 4 children
-			Vec4 fraction = RayAABox4(origin, inv_direction, bounds_minx, bounds_miny, bounds_minz, bounds_maxx, bounds_maxy, bounds_maxz);
+			Vec4 fraction = RayAABox4(mOrigin, mInvDirection, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
 
 			// Count how many results are hitting
-			UVec4 hitting = Vec4::sLess(fraction, Vec4::sReplicate(early_out_fraction));
+			UVec4 hitting = Vec4::sLess(fraction, Vec4::sReplicate(mCollector.GetEarlyOutFraction()));
 			int num_results = hitting.CountTrues();
 			if (num_results > 0)
 			{
-				// Load ids for 4 children
-				UVec4 child_ids = UVec4::sLoadInt4Aligned((const uint32 *)&node.mChildNodeID[0]);
-
 				// Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom)
-				Vec4::sSort4Reverse(fraction, child_ids);
+				Vec4::sSort4Reverse(fraction, ioChildNodeIDs);
 
 				// Shift the results so that only the hitting ones remain
+				ioChildNodeIDs = ioChildNodeIDs.ShiftComponents4Minus(num_results);
 				fraction = fraction.ReinterpretAsInt().ShiftComponents4Minus(num_results).ReinterpretAsFloat();
-				child_ids = child_ids.ShiftComponents4Minus(num_results);
 
 				// Push them onto the stack
-				if (top + 4 < cStackSize)
-				{
-					fraction.StoreFloat4((Float4 *)&fraction_stack[top]);
-					child_ids.StoreInt4((uint32 *)&node_stack[top]);
-					top += num_results;
-				}
-				else
-					JPH_ASSERT(false, "Stack full!");
+				if (inStackTop + 4 < cStackSize)
+					fraction.StoreFloat4((Float4 *)&mFractionStack[inStackTop]);
 			}
+
+			return num_results;
 		}
-		--top;
-	} 
-	while (top >= 0);
+
+		/// Visit a body, returns false if the algorithm should terminate because no hits can be generated anymore
+		JPH_INLINE void				VisitBody(const BodyID &inBodyID, int inStackTop)
+		{
+			// Store potential hit with body
+			BroadPhaseCastResult result { inBodyID, mFractionStack[inStackTop] };
+			mCollector.AddHit(result);
+		}
+
+	private:
+		Vec3						mOrigin;
+		Vec3						mExtent;
+		RayInvDirection				mInvDirection;
+		CastShapeBodyCollector &	mCollector;
+		float						mFractionStack[cStackSize];
+	};
+
+	Visitor visitor(inBox, ioCollector);
+	WalkTree(inObjectLayerFilter, inTracking, visitor JPH_IF_TRACK_BROADPHASE_STATS(, mCastAABoxStats));
 }
 
 void QuadTree::FindCollidingPairs(const BodyVector &inBodies, const BodyID *inActiveBodies, int inNumActiveBodies, float inSpeculativeContactDistance, BodyPairCollector &ioPairCollector, ObjectLayerPairFilter inObjectLayerPairFilter) const
@@ -1565,4 +1612,35 @@ void QuadTree::ValidateTree(const BodyVector &inBodies, const TrackingVector &in
 
 #endif
 
+#ifdef JPH_TRACK_BROADPHASE_STATS
+
+void QuadTree::ReportStats(const char *inName, const LayerToStats &inLayer) const
+{
+	uint64 ticks_per_sec = GetProcessorTicksPerSecond();
+
+	for (const LayerToStats::value_type &kv : inLayer)
+	{
+		double total_time = 1000.0 * double(kv.second.mTotalTicks) / double(ticks_per_sec);
+		double hits_reported_vs_bodies_visited = kv.second.mBodiesVisited > 0? 100.0 * double(kv.second.mHitsReported) / double(kv.second.mBodiesVisited) : 100.0;
+		double hits_reported_vs_nodes_visited = kv.second.mNodesVisited > 0? double(kv.second.mHitsReported) / double(kv.second.mNodesVisited) : -1.0f;
+
+		stringstream str;
+		str << inName << ", " << kv.first << ", " << mName << ", " << kv.second.mNumQueries << ", " << total_time << ", " << kv.second.mNodesVisited << ", " << kv.second.mBodiesVisited << ", " << kv.second.mHitsReported << ", " << hits_reported_vs_bodies_visited << ", " << hits_reported_vs_nodes_visited;
+		Trace(str.str().c_str());
+	}
+}
+
+void QuadTree::ReportStats() const
+{
+	unique_lock lock(mStatsMutex);
+	ReportStats("RayCast", mCastRayStats);
+	ReportStats("CollideAABox", mCollideAABoxStats);
+	ReportStats("CollideSphere", mCollideSphereStats);
+	ReportStats("CollidePoint", mCollidePointStats);
+	ReportStats("CollideOrientedBox", mCollideOrientedBoxStats);
+	ReportStats("CastAABox", mCastAABoxStats);
+}
+
+#endif // JPH_TRACK_BROADPHASE_STATS
+
 } // JPH

Jolt/Physics/Collision/BroadPhase/QuadTree.h

@@ -9,6 +9,8 @@
 #include <Physics/Body/BodyManager.h>
 #include <Physics/Collision/BroadPhase/BroadPhase.h>
 
+#include <map>
+
 namespace JPH {
 
 /// Internal tree structure in broadphase, is essentially a quad AABB tree.
@@ -163,6 +165,12 @@ public:
 	/// Destructor
 								~QuadTree();
 
+#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED)
+	/// Name of the tree for debugging purposes
+	void						SetName(const char *inName)			{ mName = inName; }
+	inline const char *			GetName() const						{ return mName; }
+#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED
+
 	/// Check if there is anything in the tree
 	inline bool					HasBodies() const					{ return mNumBodies != 0; }
 
@@ -236,6 +244,11 @@ public:
 	/// Find all colliding pairs between dynamic bodies, calls ioPairCollector for every pair found
 	void						FindCollidingPairs(const BodyVector &inBodies, const BodyID *inActiveBodies, int inNumActiveBodies, float inSpeculativeContactDistance, BodyPairCollector &ioPairCollector, ObjectLayerPairFilter inObjectLayerPairFilter) const;
 
+#ifdef JPH_TRACK_BROADPHASE_STATS
+	/// Trace the stats of this tree to the TTY
+	void						ReportStats() const;
+#endif // JPH_TRACK_BROADPHASE_STATS
+
 private:
 	/// Constants
 	static const uint32			cInvalidNodeIndex = 0xffffffff;		///< Value used to indicate node index is invalid
@@ -258,8 +271,8 @@ private:
 	void						InvalidateBodyLocation(TrackingVector &ioTracking, BodyID inBodyID);
 
 	/// Get the current root of the tree
-	inline const RootNode &		GetCurrentRoot() const				{ return mRootNode[mRootNodeIndex]; }
-	inline RootNode &			GetCurrentRoot()					{ return mRootNode[mRootNodeIndex]; }
+	JPH_INLINE const RootNode &	GetCurrentRoot() const				{ return mRootNode[mRootNodeIndex]; }
+	JPH_INLINE RootNode &		GetCurrentRoot()					{ return mRootNode[mRootNodeIndex]; }
 
 	/// Depending on if inNodeID is a body or tree node return the bounding box
 	inline AABox				GetNodeOrBodyBounds(const BodyVector &inBodies, NodeID inNodeID) const;
@@ -297,6 +310,43 @@ private:
 	void						ValidateTree(const BodyVector &inBodies, const TrackingVector &inTracking, uint32 inNodeIndex, uint32 inNumExpectedBodies) const;
 #endif
 
+
+#ifdef JPH_TRACK_BROADPHASE_STATS
+	/// Mutex protecting the various LayerToStats members
+	mutable Mutex				mStatsMutex;
+
+	struct Stat
+	{
+		uint64					mNumQueries = 0;
+		uint64					mNodesVisited = 0;
+		uint64					mBodiesVisited = 0;
+		uint64					mHitsReported = 0;
+		uint64					mTotalTicks = 0;
+		uint64					mCollectorTicks = 0;
+	};
+	
+	using LayerToStats = map<string, Stat>;
+
+	/// Trace the stats of a single query type to the TTY
+	void						ReportStats(const char *inName, const LayerToStats &inLayer) const;
+	
+	mutable LayerToStats		mCastRayStats;
+	mutable LayerToStats		mCollideAABoxStats;
+	mutable LayerToStats		mCollideSphereStats;
+	mutable LayerToStats		mCollidePointStats;
+	mutable LayerToStats		mCollideOrientedBoxStats;
+	mutable LayerToStats		mCastAABoxStats;
+#endif // JPH_TRACK_BROADPHASE_STATS
+
+	/// Walk the node tree calling the Visitor::VisitNodes for each node encountered and Visitor::VisitBody for each body encountered
+	template <class Visitor>
+	JPH_INLINE void				WalkTree(const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking, Visitor &ioVisitor JPH_IF_TRACK_BROADPHASE_STATS(, LayerToStats &ioStats)) const;
+
+#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED)
+	/// Name of this tree for debugging purposes
+	const char *				mName = "Layer";
+#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED
+
 	/// Number of bodies currently in the tree
 	atomic<uint32>				mNumBodies { 0 };
 

Jolt/Physics/Collision/ObjectLayer.h

@@ -25,6 +25,14 @@ public:
 	{
 		return true;
 	}
+
+#ifdef JPH_TRACK_BROADPHASE_STATS
+	/// Get a string that describes this filter for stat tracking purposes
+	virtual string			GetDescription() const
+	{
+		return "No Description";
+	}
+#endif // JPH_TRACK_BROADPHASE_STATS
 };
 
 /// Function to test if two objects can collide based on their object layer. Used while finding collision pairs.

Jolt/Physics/PhysicsSystem.h

@@ -165,6 +165,11 @@ public:
 	/// @param outBodyIDs On return, this will contain the list of BodyIDs
 	void						GetActiveBodies(BodyIDVector &outBodyIDs) const				{ return mBodyManager.GetActiveBodies(outBodyIDs); }
 
+#ifdef JPH_TRACK_BROADPHASE_STATS
+	/// Trace the accumulated broadphase stats to the TTY
+	void						ReportBroadphaseStats()										{ mBroadPhase->ReportStats(); }
+#endif // JPH_TRACK_BROADPHASE_STATS
+
 private:
 	using CCDBody = PhysicsUpdateContext::SubStep::CCDBody;
 

Samples/SamplesApp.cpp

@@ -2073,6 +2073,11 @@ void SamplesApp::StepPhysics()
 		mTotalTime = 0;
 	}
 
+#ifdef JPH_TRACK_BROADPHASE_STATS
+	if (mStepNumber % 600 == 0)
+		mPhysicsSystem->ReportBroadphaseStats();
+#endif // JPH_TRACK_BROADPHASE_STATS
+
 	{
 		// Post update
 		JPH_PROFILE("PostPhysicsUpdate");

UnitTests/UnitTestFramework.cpp

@@ -22,7 +22,7 @@ using namespace JPH;
 using namespace doctest;
 
 // Callback for traces
-void TraceImpl(const char *inFMT, ...)
+static void TraceImpl(const char *inFMT, ...)
 { 
 	// Format the message
 	va_list list;