Merge pull request #2 from jrouwe/feature/layer_optimization

Feature/layer optimization

Jolt/Physics/Collision/BroadPhase/BroadPhase.cpp

@@ -7,7 +7,7 @@
 
 namespace JPH {
 
-void BroadPhase::Init(BodyManager *inBodyManager, const ObjectToBroadPhaseLayer &inObjectToBroadPhaseLayer)
+void BroadPhase::Init(BodyManager *inBodyManager, const ObjectToBroadPhaseLayer &inObjectToBroadPhaseLayer, BroadPhaseLayerToString inBroadPhaseLayerToString)
 {
 	mBodyManager = inBodyManager;
 }

Jolt/Physics/Collision/BroadPhase/BroadPhase.h

@@ -21,7 +21,7 @@ public:
 	/// @param inBodyManager The body manager singleton
 	/// @param inObjectToBroadPhaseLayer Maps object layer to broadphase layer, @see ObjectToBroadPhaseLayer. 
 	/// Note that the broadphase takes a pointer to the data inside inObjectToBroadPhaseLayer so this object should remain static.
-	virtual void		Init(BodyManager *inBodyManager, const ObjectToBroadPhaseLayer &inObjectToBroadPhaseLayer);
+	virtual void		Init(BodyManager *inBodyManager, const ObjectToBroadPhaseLayer &inObjectToBroadPhaseLayer, BroadPhaseLayerToString inBroadPhaseLayerToString);
 
 	/// Should be called after many objects have been inserted to make the broadphase more efficient, usually done on startup only
 	virtual void		Optimize()															{ }
@@ -79,10 +79,10 @@ public:
 	/// @param ioActiveBodies is a list of bodies for which we need to find colliding pairs (this function can change the order of the ioActiveBodies array). This can be a subset of the set of active bodies in the system.
 	/// @param inNumActiveBodies is the size of the ioActiveBodies array.
 	/// @param inSpeculativeContactDistance Distance at which speculative contact points will be created.
-	/// @param inBroadPhaseLayerPairFilter is the filter that determines if two objects can collide (at broadphase level).
+	/// @param inObjectVsBroadPhaseLayerFilter is the filter that determines if an object can collide with a broadphase layer.
 	/// @param inObjectLayerPairFilter is the filter that determines if two objects can collide.
 	/// @param ioPairCollector receives callbacks for every body pair found.
-	virtual void		FindCollidingPairs(BodyID *ioActiveBodies, int inNumActiveBodies, float inSpeculativeContactDistance, BroadPhaseLayerPairFilter inBroadPhaseLayerPairFilter, ObjectLayerPairFilter inObjectLayerPairFilter, BodyPairCollector &ioPairCollector) const = 0;
+	virtual void		FindCollidingPairs(BodyID *ioActiveBodies, int inNumActiveBodies, float inSpeculativeContactDistance, ObjectVsBroadPhaseLayerFilter inObjectVsBroadPhaseLayerFilter, ObjectLayerPairFilter inObjectLayerPairFilter, BodyPairCollector &ioPairCollector) const = 0;
 
 protected:
 	/// Link to the body manager that manages the bodies in this broadphase

Jolt/Physics/Collision/BroadPhase/BroadPhaseBruteForce.cpp

@@ -252,7 +252,7 @@ void BroadPhaseBruteForce::CastAABox(const AABoxCast &inBox, CastShapeBodyCollec
 	}
 }
 
-void BroadPhaseBruteForce::FindCollidingPairs(BodyID *ioActiveBodies, int inNumActiveBodies, float inSpeculativeContactDistance, BroadPhaseLayerPairFilter inBroadPhaseLayerPairFilter, ObjectLayerPairFilter inObjectLayerPairFilter, BodyPairCollector &ioPairCollector) const
+void BroadPhaseBruteForce::FindCollidingPairs(BodyID *ioActiveBodies, int inNumActiveBodies, float inSpeculativeContactDistance, ObjectVsBroadPhaseLayerFilter inObjectVsBroadPhaseLayerFilter, ObjectLayerPairFilter inObjectLayerPairFilter, BodyPairCollector &ioPairCollector) const
 {
 	shared_lock<SharedMutex> lock(mMutex);
 

Jolt/Physics/Collision/BroadPhase/BroadPhaseBruteForce.h

@@ -23,7 +23,7 @@ public:
 	virtual void		CollidePoint(Vec3Arg inPoint, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override;
 	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, BroadPhaseLayerPairFilter inBroadPhaseLayerPairFilter, ObjectLayerPairFilter inObjectLayerPairFilter, BodyPairCollector &ioPairCollector) const override;
+	virtual void		FindCollidingPairs(BodyID *ioActiveBodies, int inNumActiveBodies, float inSpeculativeContactDistance, ObjectVsBroadPhaseLayerFilter inObjectVsBroadPhaseLayerFilter, ObjectLayerPairFilter inObjectLayerPairFilter, BodyPairCollector &ioPairCollector) const override;
 
 private:
 	vector<BodyID>		mBodyIDs;

Jolt/Physics/Collision/BroadPhase/BroadPhaseLayer.h

@@ -4,6 +4,7 @@
 #pragma once
 
 #include <Core/NonCopyable.h>
+#include <Physics/Collision/ObjectLayer.h>
 
 namespace JPH {
 
@@ -59,8 +60,11 @@ static constexpr BroadPhaseLayer cBroadPhaseLayerInvalid(0xff);
 /// packed, i.e. the lowest value should be 0 and the amount of sub structures that are created in the broadphase is max(inObjectToBroadPhaseLayer).
 using ObjectToBroadPhaseLayer = vector<BroadPhaseLayer>;
 
-/// Function to test if two objects can collide based on their object layer. Used while finding collision pairs.
-using BroadPhaseLayerPairFilter = bool (*)(BroadPhaseLayer inLayer1, BroadPhaseLayer inLayer2);
+/// Function to test if an object can collide with a broadphase layer. Used while finding collision pairs.
+using ObjectVsBroadPhaseLayerFilter = bool (*)(ObjectLayer inLayer1, BroadPhaseLayer inLayer2);
+
+/// Function to convert a broadphase layer to a string for debugging purposes
+using BroadPhaseLayerToString = const char * (*)(BroadPhaseLayer inLayer);
 
 /// Filter class for broadphase layers
 class BroadPhaseLayerFilter : public NonCopyable
@@ -81,15 +85,15 @@ class DefaultBroadPhaseLayerFilter : public BroadPhaseLayerFilter
 {
 public:
 	/// Constructor
-									DefaultBroadPhaseLayerFilter(BroadPhaseLayerPairFilter inObjectLayerPairFilter, BroadPhaseLayer inLayer) :
-		mBroadPhaseLayerPairFilter(inObjectLayerPairFilter),
+									DefaultBroadPhaseLayerFilter(ObjectVsBroadPhaseLayerFilter inObjectVsBroadPhaseLayerFilter, ObjectLayer inLayer) :
+		mObjectVsBroadPhaseLayerFilter(inObjectVsBroadPhaseLayerFilter),
 		mLayer(inLayer)
 	{
 	}
 
 	/// Copy constructor
 									DefaultBroadPhaseLayerFilter(const DefaultBroadPhaseLayerFilter &inRHS) :
-		mBroadPhaseLayerPairFilter(inRHS.mBroadPhaseLayerPairFilter),
+		mObjectVsBroadPhaseLayerFilter(inRHS.mObjectVsBroadPhaseLayerFilter),
 		mLayer(inRHS.mLayer)
 	{
 	}
@@ -97,12 +101,12 @@ public:
 	// See BroadPhaseLayerFilter::ShouldCollide
 	virtual bool					ShouldCollide(BroadPhaseLayer inLayer) const override
 	{
-		return mBroadPhaseLayerPairFilter(mLayer, inLayer);
+		return mObjectVsBroadPhaseLayerFilter(mLayer, inLayer);
 	}
 
 private:
-	BroadPhaseLayerPairFilter		mBroadPhaseLayerPairFilter;
-	BroadPhaseLayer					mLayer;
+	ObjectVsBroadPhaseLayerFilter	mObjectVsBroadPhaseLayerFilter;
+	ObjectLayer						mLayer;
 };
 
 /// Allows objects from a specific broad phase layer only

Jolt/Physics/Collision/BroadPhase/BroadPhaseQuadTree.cpp

@@ -15,9 +15,13 @@ BroadPhaseQuadTree::~BroadPhaseQuadTree()
 	delete [] mLayers;
 }
 
-void BroadPhaseQuadTree::Init(BodyManager* inBodyManager, const ObjectToBroadPhaseLayer &inObjectToBroadPhaseLayer)
+void BroadPhaseQuadTree::Init(BodyManager* inBodyManager, const ObjectToBroadPhaseLayer &inObjectToBroadPhaseLayer, BroadPhaseLayerToString inBroadPhaseLayerToString)
 {
-	BroadPhase::Init(inBodyManager, inObjectToBroadPhaseLayer);
+	BroadPhase::Init(inBodyManager, inObjectToBroadPhaseLayer, inBroadPhaseLayerToString);
+
+	// Store input parameters
+	mObjectToBroadPhaseLayer = inObjectToBroadPhaseLayer;
+	mBroadPhaseLayerToString = inBroadPhaseLayerToString;
 
 	// Store max bodies
 	mMaxBodies = inBodyManager->GetMaxBodies();
@@ -42,10 +46,6 @@ void BroadPhaseQuadTree::Init(BodyManager* inBodyManager, const ObjectToBroadPha
 	JPH_ASSERT(max_layer != (BroadPhaseLayer::Type)cBroadPhaseLayerInvalid); // Assume the invalid layer is unused
 	mNumLayers = max_layer + 1;
 
-	// Store reference to mapping table
-	mObjectToBroadPhaseLayer = inObjectToBroadPhaseLayer.data();
-	mNumObjectLayers = (uint)inObjectToBroadPhaseLayer.size();
-
 	// Init sub trees
 	mLayers = new QuadTree [mNumLayers];
 	for (uint l = 0; l < mNumLayers; ++l)
@@ -70,12 +70,12 @@ void BroadPhaseQuadTree::Optimize()
 
 	for (uint l = 0; l < mNumLayers; ++l)
 	{
-		QuadTree &t = mLayers[l];
-		if (t.HasBodies() && t.IsDirty())
+		QuadTree &tree = mLayers[l];
+		if (tree.HasBodies() && tree.IsDirty())
 		{
 			QuadTree::UpdateState update_state;
-			t.UpdatePrepare(mBodyManager->GetBodies(), mTracking, update_state);
-			t.UpdateFinalize(mBodyManager->GetBodies(), mTracking, update_state);
+			tree.UpdatePrepare(mBodyManager->GetBodies(), mTracking, update_state);
+			tree.UpdateFinalize(mBodyManager->GetBodies(), mTracking, update_state);
 		}
 	}
 
@@ -103,14 +103,14 @@ BroadPhase::UpdateState BroadPhaseQuadTree::UpdatePrepare()
 	for (uint iteration = 0; iteration < mNumLayers; ++iteration)
 	{
 		// Get the layer
-		QuadTree &t = mLayers[mNextLayerToUpdate];
+		QuadTree &tree = mLayers[mNextLayerToUpdate];
 		mNextLayerToUpdate = (mNextLayerToUpdate + 1) % mNumLayers;
 
 		// If it is dirty we update this one
-		if (t.HasBodies() && t.IsDirty() && t.CanBeUpdated())
+		if (tree.HasBodies() && tree.IsDirty() && tree.CanBeUpdated())
 		{
-			update_state_impl->mTree = &t;
-			t.UpdatePrepare(mBodyManager->GetBodies(), mTracking, update_state_impl->mUpdateState);
+			update_state_impl->mTree = &tree;
+			tree.UpdatePrepare(mBodyManager->GetBodies(), mTracking, update_state_impl->mUpdateState);
 			return update_state;
 		}
 	}
@@ -151,7 +151,7 @@ BroadPhase::AddState BroadPhaseQuadTree::AddBodiesPrepare(BodyID *ioBodies, int
 	LayerState *state = new LayerState [mNumLayers];
 
 	// Sort bodies on layer
-	const BroadPhaseLayer *object_to_broadphase = mObjectToBroadPhaseLayer;
+	const BroadPhaseLayer *object_to_broadphase = mObjectToBroadPhaseLayer.data();
 	Body * const * const bodies_ptr = bodies.data(); // C pointer or else sort is incredibly slow in debug mode
 	sort(ioBodies, ioBodies + inNumber, [bodies_ptr, object_to_broadphase](BodyID inLHS, BodyID inRHS) -> bool { return object_to_broadphase[bodies_ptr[inLHS.GetIndex()]->GetObjectLayer()] < object_to_broadphase[bodies_ptr[inRHS.GetIndex()]->GetObjectLayer()]; });
 
@@ -160,7 +160,7 @@ BroadPhase::AddState BroadPhaseQuadTree::AddBodiesPrepare(BodyID *ioBodies, int
 	{
 		// Get broadphase layer
 		ObjectLayer first_body_object_layer = bodies[b_start->GetIndex()]->GetObjectLayer();
-		JPH_ASSERT(first_body_object_layer < mNumObjectLayers);
+		JPH_ASSERT(first_body_object_layer < mObjectToBroadPhaseLayer.size());
 		BroadPhaseLayer::Type broadphase_layer = (BroadPhaseLayer::Type)object_to_broadphase[first_body_object_layer];
 		JPH_ASSERT(broadphase_layer < mNumLayers);
 
@@ -366,7 +366,7 @@ void BroadPhaseQuadTree::NotifyBodiesLayerChanged(BodyID *ioBodies, int inNumber
 		uint32 index = body_id->GetIndex();
 		JPH_ASSERT(bodies[index]->GetID() == *body_id, "Provided BodyID doesn't match BodyID in body manager");
 		ObjectLayer object_layer = bodies[index]->GetObjectLayer();
-		JPH_ASSERT(object_layer < mNumObjectLayers);
+		JPH_ASSERT(object_layer < mObjectToBroadPhaseLayer.size());
 		BroadPhaseLayer::Type broadphase_layer = (BroadPhaseLayer::Type)mObjectToBroadPhaseLayer[object_layer];
 		JPH_ASSERT(broadphase_layer < mNumLayers);
 		if (mTracking[index].mBroadPhaseLayer == broadphase_layer)
@@ -397,12 +397,16 @@ void BroadPhaseQuadTree::CastRay(const RayCast &inRay, RayCastBodyCollector &ioC
 
 	// Loop over all layers and test the ones that could hit
 	for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l)
-		if (inBroadPhaseLayerFilter.ShouldCollide(BroadPhaseLayer(l)))
+	{
+		QuadTree &tree = mLayers[l];
+		if (tree.HasBodies() && inBroadPhaseLayerFilter.ShouldCollide(BroadPhaseLayer(l)))
 		{
-			mLayers[l].CastRay(inRay, ioCollector, inObjectLayerFilter, mTracking);
+			JPH_PROFILE(mBroadPhaseLayerToString != nullptr? mBroadPhaseLayerToString(BroadPhaseLayer(l)) : "QuadTree");
+			tree.CastRay(inRay, ioCollector, inObjectLayerFilter, mTracking);
 			if (ioCollector.ShouldEarlyOut())
 				break;
 		}
+	}
 }
 
 void BroadPhaseQuadTree::CollideAABox(const AABox &inBox, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const 
@@ -413,12 +417,16 @@ void BroadPhaseQuadTree::CollideAABox(const AABox &inBox, CollideShapeBodyCollec
 
 	// Loop over all layers and test the ones that could hit
 	for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l)
-		if (inBroadPhaseLayerFilter.ShouldCollide(BroadPhaseLayer(l)))
+	{
+		QuadTree &tree = mLayers[l];
+		if (tree.HasBodies() && inBroadPhaseLayerFilter.ShouldCollide(BroadPhaseLayer(l)))
 		{
-			mLayers[l].CollideAABox(inBox, ioCollector, inObjectLayerFilter, mTracking);
+			JPH_PROFILE(mBroadPhaseLayerToString != nullptr? mBroadPhaseLayerToString(BroadPhaseLayer(l)) : "QuadTree");
+			tree.CollideAABox(inBox, ioCollector, inObjectLayerFilter, mTracking);
 			if (ioCollector.ShouldEarlyOut())
 				break;
 		}
+	}
 }
 
 void BroadPhaseQuadTree::CollideSphere(Vec3Arg inCenter, float inRadius, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const
@@ -429,12 +437,16 @@ void BroadPhaseQuadTree::CollideSphere(Vec3Arg inCenter, float inRadius, Collide
 
 	// Loop over all layers and test the ones that could hit
 	for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l)
-		if (inBroadPhaseLayerFilter.ShouldCollide(BroadPhaseLayer(l)))
+	{
+		QuadTree &tree = mLayers[l];
+		if (tree.HasBodies() && inBroadPhaseLayerFilter.ShouldCollide(BroadPhaseLayer(l)))
 		{
-			mLayers[l].CollideSphere(inCenter, inRadius, ioCollector, inObjectLayerFilter, mTracking);
+			JPH_PROFILE(mBroadPhaseLayerToString != nullptr? mBroadPhaseLayerToString(BroadPhaseLayer(l)) : "QuadTree");
+			tree.CollideSphere(inCenter, inRadius, ioCollector, inObjectLayerFilter, mTracking);
 			if (ioCollector.ShouldEarlyOut())
 				break;
 		}
+	}
 }
 
 void BroadPhaseQuadTree::CollidePoint(Vec3Arg inPoint, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const
@@ -445,12 +457,16 @@ void BroadPhaseQuadTree::CollidePoint(Vec3Arg inPoint, CollideShapeBodyCollector
 
 	// Loop over all layers and test the ones that could hit
 	for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l)
-		if (inBroadPhaseLayerFilter.ShouldCollide(BroadPhaseLayer(l)))
+	{
+		QuadTree &tree = mLayers[l];
+		if (tree.HasBodies() && inBroadPhaseLayerFilter.ShouldCollide(BroadPhaseLayer(l)))
 		{
-			mLayers[l].CollidePoint(inPoint, ioCollector, inObjectLayerFilter, mTracking);
+			JPH_PROFILE(mBroadPhaseLayerToString != nullptr? mBroadPhaseLayerToString(BroadPhaseLayer(l)) : "QuadTree");
+			tree.CollidePoint(inPoint, ioCollector, inObjectLayerFilter, mTracking);
 			if (ioCollector.ShouldEarlyOut())
 				break;
 		}
+	}
 }
 
 void BroadPhaseQuadTree::CollideOrientedBox(const OrientedBox &inBox, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const
@@ -461,12 +477,16 @@ void BroadPhaseQuadTree::CollideOrientedBox(const OrientedBox &inBox, CollideSha
 
 	// Loop over all layers and test the ones that could hit
 	for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l)
-		if (inBroadPhaseLayerFilter.ShouldCollide(BroadPhaseLayer(l)))
+	{
+		QuadTree &tree = mLayers[l];
+		if (tree.HasBodies() && inBroadPhaseLayerFilter.ShouldCollide(BroadPhaseLayer(l)))
 		{
-			mLayers[l].CollideOrientedBox(inBox, ioCollector, inObjectLayerFilter, mTracking);
+			JPH_PROFILE(mBroadPhaseLayerToString != nullptr? mBroadPhaseLayerToString(BroadPhaseLayer(l)) : "QuadTree");
+			tree.CollideOrientedBox(inBox, ioCollector, inObjectLayerFilter, mTracking);
 			if (ioCollector.ShouldEarlyOut())
 				break;
 		}
+	}
 }
 
 void BroadPhaseQuadTree::CastAABox(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const 
@@ -477,15 +497,19 @@ void BroadPhaseQuadTree::CastAABox(const AABoxCast &inBox, CastShapeBodyCollecto
 
 	// Loop over all layers and test the ones that could hit
 	for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l)
-		if (inBroadPhaseLayerFilter.ShouldCollide(BroadPhaseLayer(l)))
+	{
+		QuadTree &tree = mLayers[l];
+		if (tree.HasBodies() && inBroadPhaseLayerFilter.ShouldCollide(BroadPhaseLayer(l)))
 		{
-			mLayers[l].CastAABox(inBox, ioCollector, inObjectLayerFilter, mTracking);
+			JPH_PROFILE(mBroadPhaseLayerToString != nullptr? mBroadPhaseLayerToString(BroadPhaseLayer(l)) : "QuadTree");
+			tree.CastAABox(inBox, ioCollector, inObjectLayerFilter, mTracking);
 			if (ioCollector.ShouldEarlyOut())
 				break;
 		}
+	}
 }
 
-void BroadPhaseQuadTree::FindCollidingPairs(BodyID *ioActiveBodies, int inNumActiveBodies, float inSpeculativeContactDistance, BroadPhaseLayerPairFilter inBroadPhaseLayerPairFilter, ObjectLayerPairFilter inObjectLayerPairFilter, BodyPairCollector &ioPairCollector) const 
+void BroadPhaseQuadTree::FindCollidingPairs(BodyID *ioActiveBodies, int inNumActiveBodies, float inSpeculativeContactDistance, ObjectVsBroadPhaseLayerFilter inObjectVsBroadPhaseLayerFilter, ObjectLayerPairFilter inObjectLayerPairFilter, BodyPairCollector &ioPairCollector) const 
 { 
 	JPH_PROFILE_FUNCTION();
 
@@ -494,22 +518,28 @@ void BroadPhaseQuadTree::FindCollidingPairs(BodyID *ioActiveBodies, int inNumAct
 
 	// Sort bodies on layer
 	const Tracking *tracking = mTracking.data(); // C pointer or else sort is incredibly slow in debug mode
-	sort(ioActiveBodies, ioActiveBodies + inNumActiveBodies, [tracking](BodyID inLHS, BodyID inRHS) -> bool { return tracking[inLHS.GetIndex()].mBroadPhaseLayer < tracking[inRHS.GetIndex()].mBroadPhaseLayer; });
+	sort(ioActiveBodies, ioActiveBodies + inNumActiveBodies, [tracking](BodyID inLHS, BodyID inRHS) -> bool { return tracking[inLHS.GetIndex()].mObjectLayer < tracking[inRHS.GetIndex()].mObjectLayer; });
 
 	BodyID *b_start = ioActiveBodies, *b_end = ioActiveBodies + inNumActiveBodies;
 	while (b_start < b_end)
 	{
 		// Get broadphase layer
-		BroadPhaseLayer::Type broadphase_layer = tracking[b_start->GetIndex()].mBroadPhaseLayer;
-		JPH_ASSERT(broadphase_layer != (BroadPhaseLayer::Type)cBroadPhaseLayerInvalid);
+		ObjectLayer object_layer = tracking[b_start->GetIndex()].mObjectLayer;
+		JPH_ASSERT(object_layer != cObjectLayerInvalid);
 
 		// Find first body with different layer
-		BodyID *b_mid = upper_bound(b_start, b_end, broadphase_layer, [tracking](BroadPhaseLayer::Type inLayer, BodyID inBodyID) -> bool { return inLayer < tracking[inBodyID.GetIndex()].mBroadPhaseLayer; });
+		BodyID *b_mid = upper_bound(b_start, b_end, object_layer, [tracking](ObjectLayer inLayer, BodyID inBodyID) -> bool { return inLayer < tracking[inBodyID.GetIndex()].mObjectLayer; });
 
 		// Loop over all layers and test the ones that could hit
 		for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l)
-			if (inBroadPhaseLayerPairFilter(BroadPhaseLayer(broadphase_layer), BroadPhaseLayer(l)))
-				mLayers[l].FindCollidingPairs(bodies, b_start, int(b_mid - b_start), inSpeculativeContactDistance, ioPairCollector, inObjectLayerPairFilter);
+		{
+			QuadTree &tree = mLayers[l];
+			if (tree.HasBodies() && inObjectVsBroadPhaseLayerFilter(object_layer, BroadPhaseLayer(l)))
+			{
+				JPH_PROFILE(mBroadPhaseLayerToString != nullptr? mBroadPhaseLayerToString(BroadPhaseLayer(l)) : "QuadTree");
+				tree.FindCollidingPairs(bodies, b_start, int(b_mid - b_start), inSpeculativeContactDistance, ioPairCollector, inObjectLayerPairFilter);
+			}
+		}
 
 		// Repeat
 		b_start = b_mid;

Jolt/Physics/Collision/BroadPhase/BroadPhaseQuadTree.h

@@ -16,7 +16,7 @@ public:
 	virtual					~BroadPhaseQuadTree() override;
 
 	// Implementing interface of BroadPhase (see BroadPhase for documentation)
-	virtual void			Init(BodyManager *inBodyManager, const ObjectToBroadPhaseLayer &inObjectToBroadPhaseLayer) override;
+	virtual void			Init(BodyManager *inBodyManager, const ObjectToBroadPhaseLayer &inObjectToBroadPhaseLayer, BroadPhaseLayerToString inBroadPhaseLayerToString) override;
 	virtual void			Optimize() override;
 	virtual void			FrameSync() override;
 	virtual void			LockModifications() override;
@@ -35,7 +35,7 @@ public:
 	virtual void			CollidePoint(Vec3Arg inPoint, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override;
 	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, BroadPhaseLayerPairFilter inBroadPhaseLayerPairFilter, ObjectLayerPairFilter inObjectLayerPairFilter, BodyPairCollector &ioPairCollector) const override;
+	virtual void			FindCollidingPairs(BodyID *ioActiveBodies, int inNumActiveBodies, float inSpeculativeContactDistance, ObjectVsBroadPhaseLayerFilter inObjectVsBroadPhaseLayerFilter, ObjectLayerPairFilter inObjectLayerPairFilter, BodyPairCollector &ioPairCollector) const override;
 
 private:
 	/// Helper struct for AddBodies handle
@@ -59,8 +59,10 @@ private:
 	QuadTree::Allocator		mAllocator;
 
 	/// Mapping table that maps from Object Layer to tree
-	const BroadPhaseLayer *	mObjectToBroadPhaseLayer = nullptr;
-	uint					mNumObjectLayers;
+	ObjectToBroadPhaseLayer	mObjectToBroadPhaseLayer;
+
+	/// Debug function to convert a broadphase layer to a string
+	BroadPhaseLayerToString	mBroadPhaseLayerToString = nullptr;
 
 	/// One tree per object layer
 	QuadTree *				mLayers;

Jolt/Physics/PhysicsSystem.cpp

@@ -58,18 +58,17 @@ PhysicsSystem::~PhysicsSystem()
 	delete mBroadPhase;
 }
 
-void PhysicsSystem::Init(uint inMaxBodies, uint inMaxBodyPairs, uint inMaxContactConstraints, const ObjectToBroadPhaseLayer &inObjectToBroadPhaseLayer, BroadPhaseLayerPairFilter inBroadPhaseLayerPairFilter, ObjectLayerPairFilter inObjectLayerPairFilter)
+void PhysicsSystem::Init(uint inMaxBodies, uint inMaxBodyPairs, uint inMaxContactConstraints, const ObjectToBroadPhaseLayer &inObjectToBroadPhaseLayer, ObjectVsBroadPhaseLayerFilter inObjectVsBroadPhaseLayerFilter, ObjectLayerPairFilter inObjectLayerPairFilter, BroadPhaseLayerToString inBroadPhaseLayerToString)
 { 
-	mBroadPhaseLayerPairFilter = inBroadPhaseLayerPairFilter;
+	mObjectVsBroadPhaseLayerFilter = inObjectVsBroadPhaseLayerFilter;
 	mObjectLayerPairFilter = inObjectLayerPairFilter;
-	mObjectToBroadPhaseLayer = inObjectToBroadPhaseLayer;
 
 	// Initialize body manager
 	mBodyManager.Init(inMaxBodies); 
 
 	// Create broadphase
 	mBroadPhase = new BROAD_PHASE();
-	mBroadPhase->Init(&mBodyManager, mObjectToBroadPhaseLayer);
+	mBroadPhase->Init(&mBodyManager, inObjectToBroadPhaseLayer, inBroadPhaseLayerToString);
 
 	// Init contact constraint manager
 	mContactManager.Init(inMaxBodyPairs, inMaxContactConstraints);
@@ -828,7 +827,7 @@ void PhysicsSystem::JobFindCollisions(PhysicsUpdateContext::Step *ioStep, int in
 				memcpy(active_bodies, mBodyManager.GetActiveBodiesUnsafe() + active_bodies_read_idx, batch_size * sizeof(BodyID));
 
 				// Find pairs in the broadphase
-				mBroadPhase->FindCollidingPairs(active_bodies, batch_size, mPhysicsSettings.mSpeculativeContactDistance, mBroadPhaseLayerPairFilter, mObjectLayerPairFilter, add_pair);
+				mBroadPhase->FindCollidingPairs(active_bodies, batch_size, mPhysicsSettings.mSpeculativeContactDistance, mObjectVsBroadPhaseLayerFilter, mObjectLayerPairFilter, add_pair);
 
 				// Check if we have enough pairs in the buffer to start a new job
 				PhysicsUpdateContext::BodyPairQueue &queue = ioStep->mBodyPairQueues[inJobIndex];

Jolt/Physics/PhysicsSystem.h

@@ -34,7 +34,7 @@ public:
 	/// @param inMaxContactConstraints Maximum amount of contact constraints to process (anything else will fall through the world)
 	/// @param inObjectToBroadPhaseLayer Maps object layer to broadphase layer, @see ObjectToBroadPhaseLayer.
 	/// @param inObjectLayerPairFilter Filter callback function that is used to determine if two object layers collide.
-	void						Init(uint inMaxBodies, uint inMaxBodyPairs, uint inMaxContactConstraints, const ObjectToBroadPhaseLayer &inObjectToBroadPhaseLayer, BroadPhaseLayerPairFilter inBroadPhaseLayerPairFilter, ObjectLayerPairFilter inObjectLayerPairFilter);
+	void						Init(uint inMaxBodies, uint inMaxBodyPairs, uint inMaxContactConstraints, const ObjectToBroadPhaseLayer &inObjectToBroadPhaseLayer, ObjectVsBroadPhaseLayerFilter inObjectVsBroadPhaseLayerFilter, ObjectLayerPairFilter inObjectLayerPairFilter, BroadPhaseLayerToString inBroadPhaseLayerToString = nullptr);
 	
 	/// Listener that is notified whenever a body is activated/deactivated
 	void						SetBodyActivationListener(BodyActivationListener *inListener) { mBodyManager.SetBodyActivationListener(inListener); }
@@ -132,7 +132,7 @@ public:
 	inline const BodyLockInterfaceLocking &	GetBodyLockInterface() const					{ return mBodyLockInterfaceLocking; }
 
 	/// 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(mBroadPhaseLayerPairFilter, mObjectToBroadPhaseLayer[inLayer]); }
+	DefaultBroadPhaseLayerFilter GetDefaultBroadPhaseLayerFilter(ObjectLayer inLayer) const	{ return DefaultBroadPhaseLayerFilter(mObjectVsBroadPhaseLayerFilter, inLayer); }
 
 	/// Get an object layer filter that uses the default pair filter and a specified layer to determine if layers collide
 	DefaultObjectLayerFilter	GetDefaultLayerFilter(ObjectLayer inLayer) const			{ return DefaultObjectLayerFilter(mObjectLayerPairFilter, inLayer); }
@@ -200,11 +200,8 @@ private:
 	/// Number of continuous collision shape casts that need to be queued before another job is started
 	static constexpr int		cNumCCDBodiesPerJob = 4;
 
-	/// Mapping table that maps from Object Layer to tree
-	ObjectToBroadPhaseLayer		mObjectToBroadPhaseLayer;
-
 	/// Broadphase layer filter that decides if two objects can collide
-	BroadPhaseLayerPairFilter	mBroadPhaseLayerPairFilter = nullptr;
+	ObjectVsBroadPhaseLayerFilter mObjectVsBroadPhaseLayerFilter = nullptr;
 
 	/// Object layer filter that decides if two objects can collide
 	ObjectLayerPairFilter		mObjectLayerPairFilter = nullptr;

Samples/Layers.h

@@ -29,7 +29,7 @@ inline bool ObjectCanCollide(ObjectLayer inObject1, ObjectLayer inObject2)
 	case Layers::NON_MOVING:
 		return inObject2 == Layers::MOVING;
 	case Layers::MOVING:
-		return true;
+		return inObject2 == Layers::NON_MOVING || inObject2 == Layers::MOVING;
 	default:
 		JPH_ASSERT(false);
 		return false;
@@ -45,15 +45,17 @@ namespace BroadPhaseLayers
 };
 
 /// Function that determines if two broadphase layers can collide
-inline bool BroadPhaseCanCollide(BroadPhaseLayer inLayer1, BroadPhaseLayer inLayer2)
+inline bool BroadPhaseCanCollide(ObjectLayer inLayer1, BroadPhaseLayer inLayer2)
 {
-	switch ((BroadPhaseLayer::Type)inLayer1)
+	switch (inLayer1)
 	{
-	case (BroadPhaseLayer::Type)BroadPhaseLayers::NON_MOVING:
+	case Layers::NON_MOVING:
 		return inLayer2 == BroadPhaseLayers::MOVING;
-	case (BroadPhaseLayer::Type)BroadPhaseLayers::MOVING:
-		return true;
-	case (BroadPhaseLayer::Type)BroadPhaseLayers::UNUSED:
+	case Layers::MOVING:
+		return inLayer2 == BroadPhaseLayers::NON_MOVING || inLayer2 == BroadPhaseLayers::MOVING;
+	case Layers::UNUSED1:
+	case Layers::UNUSED2:
+	case Layers::UNUSED3:
 		return false;			
 	default:
 		JPH_ASSERT(false);
@@ -72,4 +74,16 @@ inline ObjectToBroadPhaseLayer GetObjectToBroadPhaseLayer()
 	object_to_broadphase[Layers::NON_MOVING] = BroadPhaseLayers::NON_MOVING;
 	object_to_broadphase[Layers::MOVING] = BroadPhaseLayers::MOVING;
 	return object_to_broadphase;
+}
+
+/// Get name of broadphase layer for debugging purposes
+inline const char *GetBroadPhaseLayerName(BroadPhaseLayer inLayer)
+{
+	switch ((BroadPhaseLayer::Type)inLayer)
+	{
+	case (BroadPhaseLayer::Type)BroadPhaseLayers::NON_MOVING:	return "NON_MOVING";
+	case (BroadPhaseLayer::Type)BroadPhaseLayers::MOVING:		return "MOVING";
+	case (BroadPhaseLayer::Type)BroadPhaseLayers::UNUSED:		return "UNUSED";
+	default:													JPH_ASSERT(false); return "INVALID";
+	}
 }

Samples/SamplesApp.cpp

@@ -502,7 +502,7 @@ void SamplesApp::StartTest(const RTTI *inRTTI)
 
 	// Create physics system
 	mPhysicsSystem = new PhysicsSystem();
-	mPhysicsSystem->Init(cNumBodies, cMaxBodyPairs, cMaxContactConstraints, GetObjectToBroadPhaseLayer(), BroadPhaseCanCollide, ObjectCanCollide);
+	mPhysicsSystem->Init(cNumBodies, cMaxBodyPairs, cMaxContactConstraints, GetObjectToBroadPhaseLayer(), BroadPhaseCanCollide, ObjectCanCollide, GetBroadPhaseLayerName);
 	mPhysicsSystem->SetPhysicsSettings(mPhysicsSettings);
 
 	// Restore gravity

Samples/Tests/BroadPhase/BroadPhaseTest.cpp

@@ -59,7 +59,7 @@ void BroadPhaseTest::Initialize()
 	// Crate broadphase
 	mObjectToBroadPhaseLayer = GetObjectToBroadPhaseLayer();
 	mBroadPhase = new BROAD_PHASE;
-	mBroadPhase->Init(mBodyManager, mObjectToBroadPhaseLayer);
+	mBroadPhase->Init(mBodyManager, mObjectToBroadPhaseLayer, nullptr);
 }
 
 void BroadPhaseTest::PostPhysicsUpdate(float inDeltaTime)

UnitTests/Layers.h

@@ -29,7 +29,7 @@ inline bool ObjectCanCollide(ObjectLayer inObject1, ObjectLayer inObject2)
 	case Layers::NON_MOVING:
 		return inObject2 == Layers::MOVING;
 	case Layers::MOVING:
-		return true;
+		return inObject2 == Layers::NON_MOVING || inObject2 == Layers::MOVING;
 	default:
 		JPH_ASSERT(false);
 		return false;
@@ -45,15 +45,17 @@ namespace BroadPhaseLayers
 };
 
 /// Function that determines if two broadphase layers can collide
-inline bool BroadPhaseCanCollide(BroadPhaseLayer inLayer1, BroadPhaseLayer inLayer2)
+inline bool BroadPhaseCanCollide(ObjectLayer inLayer1, BroadPhaseLayer inLayer2)
 {
-	switch ((BroadPhaseLayer::Type)inLayer1)
+	switch (inLayer1)
 	{
-	case (BroadPhaseLayer::Type)BroadPhaseLayers::NON_MOVING:
+	case Layers::NON_MOVING:
 		return inLayer2 == BroadPhaseLayers::MOVING;
-	case (BroadPhaseLayer::Type)BroadPhaseLayers::MOVING:
-		return true;
-	case (BroadPhaseLayer::Type)BroadPhaseLayers::UNUSED:
+	case Layers::MOVING:
+		return inLayer2 == BroadPhaseLayers::NON_MOVING || inLayer2 == BroadPhaseLayers::MOVING;
+	case Layers::UNUSED1:
+	case Layers::UNUSED2:
+	case Layers::UNUSED3:
 		return false;			
 	default:
 		JPH_ASSERT(false);

UnitTests/Physics/BroadPhaseTests.cpp

@@ -22,7 +22,7 @@ TEST_SUITE("BroadPhaseTests")
 		// Create quad tree
 		BroadPhaseQuadTree broadphase;
 		ObjectToBroadPhaseLayer obj_to_bp = GetObjectToBroadPhaseLayer();
-		broadphase.Init(&body_manager, obj_to_bp);
+		broadphase.Init(&body_manager, obj_to_bp, nullptr);
 
 		// Create a box
 		BodyCreationSettings settings(new BoxShape(Vec3::sReplicate(1.0f)), Vec3::sZero(), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING);

UnitTests/Physics/RayShapeTests.cpp

@@ -274,7 +274,7 @@ TEST_SUITE("RayShapeTests")
 		ObjectToBroadPhaseLayer object_to_broadphase;
 		object_to_broadphase.push_back(BroadPhaseLayer(0));
 		PhysicsSystem system;
-		system.Init(1, 4, 4, object_to_broadphase, [](BroadPhaseLayer, BroadPhaseLayer) { return true; }, [](ObjectLayer, ObjectLayer) { return true; });
+		system.Init(1, 4, 4, object_to_broadphase, [](ObjectLayer, BroadPhaseLayer) { return true; }, [](ObjectLayer, ObjectLayer) { return true; });
 		system.GetBodyInterface().CreateAndAddBody(BodyCreationSettings(inShape, cShapePosition, cShapeRotation, EMotionType::Static, 0), EActivation::DontActivate);