initial commit, 4.5 stable

thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhase.cpp

@@ -0,0 +1,16 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Collision/BroadPhase/BroadPhase.h>
+
+JPH_NAMESPACE_BEGIN
+
+void BroadPhase::Init(BodyManager *inBodyManager, const BroadPhaseLayerInterface &inLayerInterface)
+{
+	mBodyManager = inBodyManager;
+}
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhase.h

@@ -0,0 +1,112 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/BroadPhase/BroadPhaseQuery.h>
+#include <Jolt/Physics/Collision/BroadPhase/BroadPhaseLayer.h>
+
+JPH_NAMESPACE_BEGIN
+
+// 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
+
+class BodyManager;
+struct BodyPair;
+
+using BodyPairCollector = CollisionCollector<BodyPair, CollisionCollectorTraitsCollideShape>;
+
+/// Used to do coarse collision detection operations to quickly prune out bodies that will not collide.
+class JPH_EXPORT BroadPhase : public BroadPhaseQuery
+{
+public:
+	/// Initialize the broadphase.
+	/// @param inBodyManager The body manager singleton
+	/// @param inLayerInterface Interface that maps object layers to broadphase layers.
+	/// Note that the broadphase takes a pointer to the data inside inObjectToBroadPhaseLayer so this object should remain static.
+	virtual void		Init(BodyManager *inBodyManager, const BroadPhaseLayerInterface &inLayerInterface);
+
+	/// Should be called after many objects have been inserted to make the broadphase more efficient, usually done on startup only
+	virtual void		Optimize()															{ /* Optionally overridden by implementation */ }
+
+	/// Must be called just before updating the broadphase when none of the body mutexes are locked
+	virtual void		FrameSync()															{ /* Optionally overridden by implementation */ }
+
+	/// Must be called before UpdatePrepare to prevent modifications from being made to the tree
+	virtual void		LockModifications()													{ /* Optionally overridden by implementation */ }
+
+	/// Context used during broadphase update
+	struct UpdateState { void *mData[4]; };
+
+	/// Update the broadphase, needs to be called frequently to update the internal state when bodies have been modified.
+	/// The UpdatePrepare() function can run in a background thread without influencing the broadphase
+	virtual	UpdateState	UpdatePrepare()														{ return UpdateState(); }
+
+	/// Finalizing the update will quickly apply the changes
+	virtual void		UpdateFinalize([[maybe_unused]] const UpdateState &inUpdateState)	{ /* Optionally overridden by implementation */ }
+
+	/// Must be called after UpdateFinalize to allow modifications to the broadphase
+	virtual void		UnlockModifications()												{ /* Optionally overridden by implementation */ }
+
+	/// Handle used during adding bodies to the broadphase
+	using AddState = void *;
+
+	/// Prepare adding inNumber bodies at ioBodies to the broadphase, returns a handle that should be used in AddBodiesFinalize/Abort.
+	/// This can be done on a background thread without influencing the broadphase.
+	/// ioBodies may be shuffled around by this function and should be kept that way until AddBodiesFinalize/Abort is called.
+	virtual AddState	AddBodiesPrepare([[maybe_unused]] BodyID *ioBodies, [[maybe_unused]] int inNumber) { return nullptr; } // By default the broadphase doesn't support this
+
+	/// Finalize adding bodies to the broadphase, supply the return value of AddBodiesPrepare in inAddState.
+	/// Please ensure that the ioBodies array passed to AddBodiesPrepare is unmodified and passed again to this function.
+	virtual void		AddBodiesFinalize(BodyID *ioBodies, int inNumber, AddState inAddState) = 0;
+
+	/// Abort adding bodies to the broadphase, supply the return value of AddBodiesPrepare in inAddState.
+	/// This can be done on a background thread without influencing the broadphase.
+	/// Please ensure that the ioBodies array passed to AddBodiesPrepare is unmodified and passed again to this function.
+	virtual void		AddBodiesAbort([[maybe_unused]] BodyID *ioBodies, [[maybe_unused]] int inNumber, [[maybe_unused]] AddState inAddState)	{ /* By default nothing needs to be done */ }
+
+	/// Remove inNumber bodies in ioBodies from the broadphase.
+	/// ioBodies may be shuffled around by this function.
+	virtual void		RemoveBodies(BodyID *ioBodies, int inNumber) = 0;
+
+	/// Call whenever the aabb of a body changes (can change order of ioBodies array)
+	/// inTakeLock should be false if we're between LockModifications/UnlockModifications, in which case care needs to be taken to not call this between UpdatePrepare/UpdateFinalize
+	virtual void		NotifyBodiesAABBChanged(BodyID *ioBodies, int inNumber, bool inTakeLock = true) = 0;
+
+	/// Call whenever the layer (and optionally the aabb as well) of a body changes (can change order of ioBodies array)
+	virtual void		NotifyBodiesLayerChanged(BodyID *ioBodies, int inNumber) = 0;
+
+	/// Find all colliding pairs between dynamic bodies
+	/// Note that this function is very specifically tailored for the PhysicsSystem::Update function, hence it is not part of the BroadPhaseQuery interface.
+	/// One of the assumptions it can make is that no locking is needed during the query as it will only be called during a very particular part of the update.
+	/// @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 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, const ObjectVsBroadPhaseLayerFilter &inObjectVsBroadPhaseLayerFilter, const ObjectLayerPairFilter &inObjectLayerPairFilter, BodyPairCollector &ioPairCollector) const = 0;
+
+	/// Same as BroadPhaseQuery::CastAABox but can be implemented in a way to take no broad phase locks.
+	virtual void		CastAABoxNoLock(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const = 0;
+
+	/// Get the bounding box of all objects in the broadphase
+	virtual AABox		GetBounds() const = 0;
+
+#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;
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseBruteForce.cpp

@@ -0,0 +1,313 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+#include <Jolt/Physics/Collision/BroadPhase/BroadPhaseBruteForce.h>
+#include <Jolt/Physics/Collision/RayCast.h>
+#include <Jolt/Physics/Collision/AABoxCast.h>
+#include <Jolt/Physics/Collision/CastResult.h>
+#include <Jolt/Physics/Body/BodyManager.h>
+#include <Jolt/Physics/Body/BodyPair.h>
+#include <Jolt/Geometry/RayAABox.h>
+#include <Jolt/Geometry/OrientedBox.h>
+#include <Jolt/Core/QuickSort.h>
+
+JPH_NAMESPACE_BEGIN
+
+void BroadPhaseBruteForce::AddBodiesFinalize(BodyID *ioBodies, int inNumber, AddState inAddState)
+{
+	lock_guard lock(mMutex);
+
+	BodyVector &bodies = mBodyManager->GetBodies();
+
+	// Allocate space
+	uint32 idx = (uint32)mBodyIDs.size();
+	mBodyIDs.resize(idx + inNumber);
+
+	// Add bodies
+	for (const BodyID *b = ioBodies, *b_end = ioBodies + inNumber; b < b_end; ++b)
+	{
+		Body &body = *bodies[b->GetIndex()];
+
+		// Validate that body ID is consistent with array index
+		JPH_ASSERT(body.GetID() == *b);
+		JPH_ASSERT(!body.IsInBroadPhase());
+
+		// Add it to the list
+		mBodyIDs[idx] = body.GetID();
+		++idx;
+
+		// Indicate body is in the broadphase
+		body.SetInBroadPhaseInternal(true);
+	}
+
+	// Resort
+	QuickSort(mBodyIDs.begin(), mBodyIDs.end());
+}
+
+void BroadPhaseBruteForce::RemoveBodies(BodyID *ioBodies, int inNumber)
+{
+	lock_guard lock(mMutex);
+
+	BodyVector &bodies = mBodyManager->GetBodies();
+
+	JPH_ASSERT((int)mBodyIDs.size() >= inNumber);
+
+	// Remove bodies
+	for (const BodyID *b = ioBodies, *b_end = ioBodies + inNumber; b < b_end; ++b)
+	{
+		Body &body = *bodies[b->GetIndex()];
+
+		// Validate that body ID is consistent with array index
+		JPH_ASSERT(body.GetID() == *b);
+		JPH_ASSERT(body.IsInBroadPhase());
+
+		// Find body id
+		Array<BodyID>::const_iterator it = std::lower_bound(mBodyIDs.begin(), mBodyIDs.end(), body.GetID());
+		JPH_ASSERT(it != mBodyIDs.end());
+
+		// Remove element
+		mBodyIDs.erase(it);
+
+		// Indicate body is no longer in the broadphase
+		body.SetInBroadPhaseInternal(false);
+	}
+}
+
+void BroadPhaseBruteForce::NotifyBodiesAABBChanged(BodyID *ioBodies, int inNumber, bool inTakeLock)
+{
+	// Do nothing, we directly reference the body
+}
+
+void BroadPhaseBruteForce::NotifyBodiesLayerChanged(BodyID * ioBodies, int inNumber)
+{
+	// Do nothing, we directly reference the body
+}
+
+void BroadPhaseBruteForce::CastRay(const RayCast &inRay, RayCastBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const
+{
+	shared_lock lock(mMutex);
+
+	// Load ray
+	Vec3 origin(inRay.mOrigin);
+	RayInvDirection inv_direction(inRay.mDirection);
+
+	// For all bodies
+	float early_out_fraction = ioCollector.GetEarlyOutFraction();
+	for (BodyID b : mBodyIDs)
+	{
+		const Body &body = mBodyManager->GetBody(b);
+
+		// Test layer
+		if (inObjectLayerFilter.ShouldCollide(body.GetObjectLayer()))
+		{
+			// Test intersection with ray
+			const AABox &bounds = body.GetWorldSpaceBounds();
+			float fraction = RayAABox(origin, inv_direction, bounds.mMin, bounds.mMax);
+			if (fraction < early_out_fraction)
+			{
+				// Store hit
+				BroadPhaseCastResult result { b, fraction };
+				ioCollector.AddHit(result);
+				if (ioCollector.ShouldEarlyOut())
+					break;
+				early_out_fraction = ioCollector.GetEarlyOutFraction();
+			}
+		}
+	}
+}
+
+void BroadPhaseBruteForce::CollideAABox(const AABox &inBox, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const
+{
+	shared_lock lock(mMutex);
+
+	// For all bodies
+	for (BodyID b : mBodyIDs)
+	{
+		const Body &body = mBodyManager->GetBody(b);
+
+		// Test layer
+		if (inObjectLayerFilter.ShouldCollide(body.GetObjectLayer()))
+		{
+			// Test intersection with box
+			const AABox &bounds = body.GetWorldSpaceBounds();
+			if (bounds.Overlaps(inBox))
+			{
+				// Store hit
+				ioCollector.AddHit(b);
+				if (ioCollector.ShouldEarlyOut())
+					break;
+			}
+		}
+	}
+}
+
+void BroadPhaseBruteForce::CollideSphere(Vec3Arg inCenter, float inRadius, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const
+{
+	shared_lock lock(mMutex);
+
+	float radius_sq = Square(inRadius);
+
+	// For all bodies
+	for (BodyID b : mBodyIDs)
+	{
+		const Body &body = mBodyManager->GetBody(b);
+
+		// Test layer
+		if (inObjectLayerFilter.ShouldCollide(body.GetObjectLayer()))
+		{
+			// Test intersection with box
+			const AABox &bounds = body.GetWorldSpaceBounds();
+			if (bounds.GetSqDistanceTo(inCenter) <= radius_sq)
+			{
+				// Store hit
+				ioCollector.AddHit(b);
+				if (ioCollector.ShouldEarlyOut())
+					break;
+			}
+		}
+	}
+}
+
+void BroadPhaseBruteForce::CollidePoint(Vec3Arg inPoint, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const
+{
+	shared_lock lock(mMutex);
+
+	// For all bodies
+	for (BodyID b : mBodyIDs)
+	{
+		const Body &body = mBodyManager->GetBody(b);
+
+		// Test layer
+		if (inObjectLayerFilter.ShouldCollide(body.GetObjectLayer()))
+		{
+			// Test intersection with box
+			const AABox &bounds = body.GetWorldSpaceBounds();
+			if (bounds.Contains(inPoint))
+			{
+				// Store hit
+				ioCollector.AddHit(b);
+				if (ioCollector.ShouldEarlyOut())
+					break;
+			}
+		}
+	}
+}
+
+void BroadPhaseBruteForce::CollideOrientedBox(const OrientedBox &inBox, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const
+{
+	shared_lock lock(mMutex);
+
+	// For all bodies
+	for (BodyID b : mBodyIDs)
+	{
+		const Body &body = mBodyManager->GetBody(b);
+
+		// Test layer
+		if (inObjectLayerFilter.ShouldCollide(body.GetObjectLayer()))
+		{
+			// Test intersection with box
+			const AABox &bounds = body.GetWorldSpaceBounds();
+			if (inBox.Overlaps(bounds))
+			{
+				// Store hit
+				ioCollector.AddHit(b);
+				if (ioCollector.ShouldEarlyOut())
+					break;
+			}
+		}
+	}
+}
+
+void BroadPhaseBruteForce::CastAABoxNoLock(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const
+{
+	shared_lock lock(mMutex);
+
+	// Load box
+	Vec3 origin(inBox.mBox.GetCenter());
+	Vec3 extent(inBox.mBox.GetExtent());
+	RayInvDirection inv_direction(inBox.mDirection);
+
+	// For all bodies
+	float early_out_fraction = ioCollector.GetPositiveEarlyOutFraction();
+	for (BodyID b : mBodyIDs)
+	{
+		const Body &body = mBodyManager->GetBody(b);
+
+		// Test layer
+		if (inObjectLayerFilter.ShouldCollide(body.GetObjectLayer()))
+		{
+			// Test intersection with ray
+			const AABox &bounds = body.GetWorldSpaceBounds();
+			float fraction = RayAABox(origin, inv_direction, bounds.mMin - extent, bounds.mMax + extent);
+			if (fraction < early_out_fraction)
+			{
+				// Store hit
+				BroadPhaseCastResult result { b, fraction };
+				ioCollector.AddHit(result);
+				if (ioCollector.ShouldEarlyOut())
+					break;
+				early_out_fraction = ioCollector.GetPositiveEarlyOutFraction();
+			}
+		}
+	}
+}
+
+void BroadPhaseBruteForce::CastAABox(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const
+{
+	CastAABoxNoLock(inBox, ioCollector, inBroadPhaseLayerFilter, inObjectLayerFilter);
+}
+
+void BroadPhaseBruteForce::FindCollidingPairs(BodyID *ioActiveBodies, int inNumActiveBodies, float inSpeculativeContactDistance, const ObjectVsBroadPhaseLayerFilter &inObjectVsBroadPhaseLayerFilter, const ObjectLayerPairFilter &inObjectLayerPairFilter, BodyPairCollector &ioPairCollector) const
+{
+	shared_lock lock(mMutex);
+
+	// Loop through all active bodies
+	size_t num_bodies = mBodyIDs.size();
+	for (int b1 = 0; b1 < inNumActiveBodies; ++b1)
+	{
+		BodyID b1_id = ioActiveBodies[b1];
+		const Body &body1 = mBodyManager->GetBody(b1_id);
+		const ObjectLayer layer1 = body1.GetObjectLayer();
+
+		// Expand the bounding box by the speculative contact distance
+		AABox bounds1 = body1.GetWorldSpaceBounds();
+		bounds1.ExpandBy(Vec3::sReplicate(inSpeculativeContactDistance));
+
+		// For all other bodies
+		for (size_t b2 = 0; b2 < num_bodies; ++b2)
+		{
+			// Check if bodies can collide
+			BodyID b2_id = mBodyIDs[b2];
+			const Body &body2 = mBodyManager->GetBody(b2_id);
+			if (!Body::sFindCollidingPairsCanCollide(body1, body2))
+				continue;
+
+			// Check if layers can collide
+			const ObjectLayer layer2 = body2.GetObjectLayer();
+			if (!inObjectLayerPairFilter.ShouldCollide(layer1, layer2))
+				continue;
+
+			// Check if bounds overlap
+			const AABox &bounds2 = body2.GetWorldSpaceBounds();
+			if (!bounds1.Overlaps(bounds2))
+				continue;
+
+			// Store overlapping pair
+			ioPairCollector.AddHit({ b1_id, b2_id });
+		}
+	}
+}
+
+AABox BroadPhaseBruteForce::GetBounds() const
+{
+	shared_lock lock(mMutex);
+
+	AABox bounds;
+	for (BodyID b : mBodyIDs)
+		bounds.Encapsulate(mBodyManager->GetBody(b).GetWorldSpaceBounds());
+	return bounds;
+}
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseBruteForce.h

@@ -0,0 +1,38 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/BroadPhase/BroadPhase.h>
+#include <Jolt/Core/Mutex.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Test BroadPhase implementation that does not do anything to speed up the operations. Can be used as a reference implementation.
+class JPH_EXPORT BroadPhaseBruteForce final : public BroadPhase
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	// Implementing interface of BroadPhase (see BroadPhase for documentation)
+	virtual void		AddBodiesFinalize(BodyID *ioBodies, int inNumber, AddState inAddState) override;
+	virtual void		RemoveBodies(BodyID *ioBodies, int inNumber) override;
+	virtual void		NotifyBodiesAABBChanged(BodyID *ioBodies, int inNumber, bool inTakeLock) override;
+	virtual void		NotifyBodiesLayerChanged(BodyID *ioBodies, int inNumber) override;
+	virtual void		CastRay(const RayCast &inRay, RayCastBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override;
+	virtual void		CollideAABox(const AABox &inBox, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override;
+	virtual void		CollideSphere(Vec3Arg inCenter, float inRadius, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override;
+	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		CastAABoxNoLock(const AABoxCast &inBox, CastShapeBodyCollector &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, const ObjectVsBroadPhaseLayerFilter &inObjectVsBroadPhaseLayerFilter, const ObjectLayerPairFilter &inObjectLayerPairFilter, BodyPairCollector &ioPairCollector) const override;
+	virtual AABox		GetBounds() const override;
+
+private:
+	Array<BodyID>		mBodyIDs;
+	mutable SharedMutex	mMutex;
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseLayer.h

@@ -0,0 +1,148 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/NonCopyable.h>
+#include <Jolt/Physics/Collision/ObjectLayer.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// An object layer can be mapped to a broadphase layer. Objects with the same broadphase layer will end up in the same sub structure (usually a tree) of the broadphase.
+/// When there are many layers, this reduces the total amount of sub structures the broad phase needs to manage. Usually you want objects that don't collide with each other
+/// in different broad phase layers, but there could be exceptions if objects layers only contain a minor amount of objects so it is not beneficial to give each layer its
+/// own sub structure in the broadphase.
+/// Note: This class requires explicit casting from and to Type to avoid confusion with ObjectLayer
+class BroadPhaseLayer
+{
+public:
+	using Type = uint8;
+
+	JPH_INLINE						BroadPhaseLayer() = default;
+	JPH_INLINE explicit constexpr	BroadPhaseLayer(Type inValue) : mValue(inValue) { }
+	JPH_INLINE constexpr			BroadPhaseLayer(const BroadPhaseLayer &) = default;
+	JPH_INLINE BroadPhaseLayer &	operator = (const BroadPhaseLayer &) = default;
+
+	JPH_INLINE constexpr bool		operator == (const BroadPhaseLayer &inRHS) const
+	{
+		return mValue == inRHS.mValue;
+	}
+
+	JPH_INLINE constexpr bool		operator != (const BroadPhaseLayer &inRHS) const
+	{
+		return mValue != inRHS.mValue;
+	}
+
+	JPH_INLINE constexpr bool		operator < (const BroadPhaseLayer &inRHS) const
+	{
+		return mValue < inRHS.mValue;
+	}
+
+	JPH_INLINE explicit constexpr	operator Type() const
+	{
+		return mValue;
+	}
+
+	JPH_INLINE Type					GetValue() const
+	{
+		return mValue;
+	}
+
+private:
+	Type							mValue;
+};
+
+/// Constant value used to indicate an invalid broad phase layer
+static constexpr BroadPhaseLayer cBroadPhaseLayerInvalid(0xff);
+
+/// Interface that the application should implement to allow mapping object layers to broadphase layers
+class JPH_EXPORT BroadPhaseLayerInterface : public NonCopyable
+{
+public:
+	/// Destructor
+	virtual							~BroadPhaseLayerInterface() = default;
+
+	/// Return the number of broadphase layers there are
+	virtual uint					GetNumBroadPhaseLayers() const = 0;
+
+	/// Convert an object layer to the corresponding broadphase layer
+	virtual BroadPhaseLayer			GetBroadPhaseLayer(ObjectLayer inLayer) const = 0;
+
+#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED)
+	/// Get the user readable name of a broadphase layer (debugging purposes)
+	virtual const char *			GetBroadPhaseLayerName(BroadPhaseLayer inLayer) const = 0;
+#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED
+};
+
+/// Class to test if an object can collide with a broadphase layer. Used while finding collision pairs.
+class JPH_EXPORT ObjectVsBroadPhaseLayerFilter : public NonCopyable
+{
+public:
+	/// Destructor
+	virtual							~ObjectVsBroadPhaseLayerFilter() = default;
+
+	/// Returns true if an object layer should collide with a broadphase layer
+	virtual bool					ShouldCollide([[maybe_unused]] ObjectLayer inLayer1, [[maybe_unused]] BroadPhaseLayer inLayer2) const
+	{
+		return true;
+	}
+};
+
+/// Filter class for broadphase layers
+class JPH_EXPORT BroadPhaseLayerFilter : public NonCopyable
+{
+public:
+	/// Destructor
+	virtual							~BroadPhaseLayerFilter() = default;
+
+	/// Function to filter out broadphase layers when doing collision query test (return true to allow testing against objects with this layer)
+	virtual bool					ShouldCollide([[maybe_unused]] BroadPhaseLayer inLayer) const
+	{
+		return true;
+	}
+};
+
+/// Default filter class that uses the pair filter in combination with a specified layer to filter layers
+class JPH_EXPORT DefaultBroadPhaseLayerFilter : public BroadPhaseLayerFilter
+{
+public:
+	/// Constructor
+									DefaultBroadPhaseLayerFilter(const ObjectVsBroadPhaseLayerFilter &inObjectVsBroadPhaseLayerFilter, ObjectLayer inLayer) :
+		mObjectVsBroadPhaseLayerFilter(inObjectVsBroadPhaseLayerFilter),
+		mLayer(inLayer)
+	{
+	}
+
+	// See BroadPhaseLayerFilter::ShouldCollide
+	virtual bool					ShouldCollide(BroadPhaseLayer inLayer) const override
+	{
+		return mObjectVsBroadPhaseLayerFilter.ShouldCollide(mLayer, inLayer);
+	}
+
+private:
+	const ObjectVsBroadPhaseLayerFilter &mObjectVsBroadPhaseLayerFilter;
+	ObjectLayer						mLayer;
+};
+
+/// Allows objects from a specific broad phase layer only
+class JPH_EXPORT SpecifiedBroadPhaseLayerFilter : public BroadPhaseLayerFilter
+{
+public:
+	/// Constructor
+	explicit						SpecifiedBroadPhaseLayerFilter(BroadPhaseLayer inLayer) :
+		mLayer(inLayer)
+	{
+	}
+
+	// See BroadPhaseLayerFilter::ShouldCollide
+	virtual bool					ShouldCollide(BroadPhaseLayer inLayer) const override
+	{
+		return mLayer == inLayer;
+	}
+
+private:
+	BroadPhaseLayer					mLayer;
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseLayerInterfaceMask.h

@@ -0,0 +1,92 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/BroadPhase/BroadPhaseLayer.h>
+#include <Jolt/Physics/Collision/ObjectLayerPairFilterMask.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// BroadPhaseLayerInterface implementation.
+/// This defines a mapping between object and broadphase layers.
+/// This implementation works together with ObjectLayerPairFilterMask and ObjectVsBroadPhaseLayerFilterMask.
+/// A broadphase layer is suitable for an object if its group & inGroupsToInclude is not zero and its group & inGroupsToExclude is zero.
+/// The broadphase layers are iterated from lowest to highest value and the first one that matches is taken. If none match then it takes the last layer.
+class BroadPhaseLayerInterfaceMask : public BroadPhaseLayerInterface
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	explicit				BroadPhaseLayerInterfaceMask(uint inNumBroadPhaseLayers)
+	{
+		JPH_ASSERT(inNumBroadPhaseLayers > 0);
+		mMapping.resize(inNumBroadPhaseLayers);
+
+#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED)
+		mBroadPhaseLayerNames.resize(inNumBroadPhaseLayers, "Undefined");
+#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED
+	}
+
+	// Configures a broadphase layer.
+	void					ConfigureLayer(BroadPhaseLayer inBroadPhaseLayer, uint32 inGroupsToInclude, uint32 inGroupsToExclude)
+	{
+		JPH_ASSERT((BroadPhaseLayer::Type)inBroadPhaseLayer < (uint)mMapping.size());
+		Mapping &m = mMapping[(BroadPhaseLayer::Type)inBroadPhaseLayer];
+		m.mGroupsToInclude = inGroupsToInclude;
+		m.mGroupsToExclude = inGroupsToExclude;
+	}
+
+	virtual uint			GetNumBroadPhaseLayers() const override
+	{
+		return (uint)mMapping.size();
+	}
+
+	virtual BroadPhaseLayer	GetBroadPhaseLayer(ObjectLayer inLayer) const override
+	{
+		// Try to find the first broadphase layer that matches
+		uint32 group = ObjectLayerPairFilterMask::sGetGroup(inLayer);
+		for (const Mapping &m : mMapping)
+			if ((group & m.mGroupsToInclude) != 0 && (group & m.mGroupsToExclude) == 0)
+				return BroadPhaseLayer(BroadPhaseLayer::Type(&m - mMapping.data()));
+
+		// Fall back to the last broadphase layer
+		return BroadPhaseLayer(BroadPhaseLayer::Type(mMapping.size() - 1));
+	}
+
+	/// Returns true if an object layer should collide with a broadphase layer, this function is being called from ObjectVsBroadPhaseLayerFilterMask
+	inline bool				ShouldCollide(ObjectLayer inLayer1, BroadPhaseLayer inLayer2) const
+	{
+		uint32 mask = ObjectLayerPairFilterMask::sGetMask(inLayer1);
+		const Mapping &m = mMapping[(BroadPhaseLayer::Type)inLayer2];
+		return &m == &mMapping.back() // Last layer may collide with anything
+			|| (m.mGroupsToInclude & mask) != 0; // Mask allows it to collide with objects that could reside in this layer
+	}
+
+#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED)
+	void					SetBroadPhaseLayerName(BroadPhaseLayer inLayer, const char *inName)
+	{
+		mBroadPhaseLayerNames[(BroadPhaseLayer::Type)inLayer] = inName;
+	}
+
+	virtual const char *	GetBroadPhaseLayerName(BroadPhaseLayer inLayer) const override
+	{
+		return mBroadPhaseLayerNames[(BroadPhaseLayer::Type)inLayer];
+	}
+#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED
+
+private:
+	struct Mapping
+	{
+		uint32				mGroupsToInclude = 0;
+		uint32				mGroupsToExclude = ~uint32(0);
+	};
+	Array<Mapping>			mMapping;
+
+#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED)
+	Array<const char *>		mBroadPhaseLayerNames;
+#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseLayerInterfaceTable.h

@@ -0,0 +1,64 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/BroadPhase/BroadPhaseLayer.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// BroadPhaseLayerInterface implementation.
+/// This defines a mapping between object and broadphase layers.
+/// This implementation uses a simple table
+class BroadPhaseLayerInterfaceTable : public BroadPhaseLayerInterface
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+							BroadPhaseLayerInterfaceTable(uint inNumObjectLayers, uint inNumBroadPhaseLayers) :
+		mNumBroadPhaseLayers(inNumBroadPhaseLayers)
+	{
+		mObjectToBroadPhase.resize(inNumObjectLayers, BroadPhaseLayer(0));
+#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED)
+		mBroadPhaseLayerNames.resize(inNumBroadPhaseLayers, "Undefined");
+#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED
+	}
+
+	void					MapObjectToBroadPhaseLayer(ObjectLayer inObjectLayer, BroadPhaseLayer inBroadPhaseLayer)
+	{
+		JPH_ASSERT((BroadPhaseLayer::Type)inBroadPhaseLayer < mNumBroadPhaseLayers);
+		mObjectToBroadPhase[inObjectLayer] = inBroadPhaseLayer;
+	}
+
+	virtual uint			GetNumBroadPhaseLayers() const override
+	{
+		return mNumBroadPhaseLayers;
+	}
+
+	virtual BroadPhaseLayer	GetBroadPhaseLayer(ObjectLayer inLayer) const override
+	{
+		return mObjectToBroadPhase[inLayer];
+	}
+
+#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED)
+	void					SetBroadPhaseLayerName(BroadPhaseLayer inLayer, const char *inName)
+	{
+		mBroadPhaseLayerNames[(BroadPhaseLayer::Type)inLayer] = inName;
+	}
+
+	virtual const char *	GetBroadPhaseLayerName(BroadPhaseLayer inLayer) const override
+	{
+		return mBroadPhaseLayerNames[(BroadPhaseLayer::Type)inLayer];
+	}
+#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED
+
+private:
+	uint					mNumBroadPhaseLayers;
+	Array<BroadPhaseLayer>	mObjectToBroadPhase;
+#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED)
+	Array<const char *>		mBroadPhaseLayerNames;
+#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseQuadTree.cpp

@@ -0,0 +1,629 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+#include <Jolt/Physics/Collision/BroadPhase/BroadPhaseQuadTree.h>
+#include <Jolt/Physics/Collision/RayCast.h>
+#include <Jolt/Physics/Collision/AABoxCast.h>
+#include <Jolt/Physics/Collision/CastResult.h>
+#include <Jolt/Core/QuickSort.h>
+
+JPH_NAMESPACE_BEGIN
+
+BroadPhaseQuadTree::~BroadPhaseQuadTree()
+{
+	delete [] mLayers;
+}
+
+void BroadPhaseQuadTree::Init(BodyManager *inBodyManager, const BroadPhaseLayerInterface &inLayerInterface)
+{
+	BroadPhase::Init(inBodyManager, inLayerInterface);
+
+	// Store input parameters
+	mBroadPhaseLayerInterface = &inLayerInterface;
+	mNumLayers = inLayerInterface.GetNumBroadPhaseLayers();
+	JPH_ASSERT(mNumLayers < (BroadPhaseLayer::Type)cBroadPhaseLayerInvalid);
+
+#ifdef JPH_ENABLE_ASSERTS
+	// Store lock context
+	mLockContext = inBodyManager;
+#endif // JPH_ENABLE_ASSERTS
+
+	// Store max bodies
+	mMaxBodies = inBodyManager->GetMaxBodies();
+
+	// Initialize tracking data
+	mTracking.resize(mMaxBodies);
+
+	// Init allocator
+	// Estimate the amount of nodes we're going to need
+	uint32 num_leaves = (uint32)(mMaxBodies + 1) / 2; // Assume 50% fill
+	uint32 num_leaves_plus_internal_nodes = num_leaves + (num_leaves + 2) / 3; // = Sum(num_leaves * 4^-i) with i = [0, Inf].
+	mAllocator.Init(2 * num_leaves_plus_internal_nodes, 256); // We use double the amount of nodes while rebuilding the tree during Update()
+
+	// Init sub trees
+	mLayers = new QuadTree [mNumLayers];
+	for (uint l = 0; l < mNumLayers; ++l)
+	{
+		mLayers[l].Init(mAllocator);
+
+#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED)
+		// Set the name of the layer
+		mLayers[l].SetName(inLayerInterface.GetBroadPhaseLayerName(BroadPhaseLayer(BroadPhaseLayer::Type(l))));
+#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED
+	}
+}
+
+void BroadPhaseQuadTree::FrameSync()
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Take a unique lock on the old query lock so that we know no one is using the old nodes anymore.
+	// Note that nothing should be locked at this point to avoid risking a lock inversion deadlock.
+	// Note that in other places where we lock this mutex we don't use SharedLock to detect lock inversions. As long as
+	// nothing else is locked this is safe. This is why BroadPhaseQuery should be the highest priority lock.
+	UniqueLock root_lock(mQueryLocks[mQueryLockIdx ^ 1] JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::BroadPhaseQuery));
+
+	for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l)
+		mLayers[l].DiscardOldTree();
+}
+
+void BroadPhaseQuadTree::Optimize()
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Free the previous tree so we can create a new optimized tree
+	FrameSync();
+
+	LockModifications();
+
+	for (uint l = 0; l < mNumLayers; ++l)
+	{
+		QuadTree &tree = mLayers[l];
+		if (tree.HasBodies() || tree.IsDirty())
+		{
+			QuadTree::UpdateState update_state;
+			tree.UpdatePrepare(mBodyManager->GetBodies(), mTracking, update_state, true);
+			tree.UpdateFinalize(mBodyManager->GetBodies(), mTracking, update_state);
+		}
+	}
+
+	UnlockModifications();
+
+	// Free the tree from before we created a new optimized tree
+	FrameSync();
+
+	mNextLayerToUpdate = 0;
+}
+
+void BroadPhaseQuadTree::LockModifications()
+{
+	// From this point on we prevent modifications to the tree
+	PhysicsLock::sLock(mUpdateMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::BroadPhaseUpdate));
+}
+
+BroadPhase::UpdateState BroadPhaseQuadTree::UpdatePrepare()
+{
+	// LockModifications should have been called
+	JPH_ASSERT(mUpdateMutex.is_locked());
+
+	// Create update state
+	UpdateState update_state;
+	UpdateStateImpl *update_state_impl = reinterpret_cast<UpdateStateImpl *>(&update_state);
+
+	// Loop until we've seen all layers
+	for (uint iteration = 0; iteration < mNumLayers; ++iteration)
+	{
+		// Get the layer
+		QuadTree &tree = mLayers[mNextLayerToUpdate];
+		mNextLayerToUpdate = (mNextLayerToUpdate + 1) % mNumLayers;
+
+		// If it is dirty we update this one
+		if (tree.HasBodies() && tree.IsDirty() && tree.CanBeUpdated())
+		{
+			update_state_impl->mTree = &tree;
+			tree.UpdatePrepare(mBodyManager->GetBodies(), mTracking, update_state_impl->mUpdateState, false);
+			return update_state;
+		}
+	}
+
+	// Nothing to update
+	update_state_impl->mTree = nullptr;
+	return update_state;
+}
+
+void BroadPhaseQuadTree::UpdateFinalize(const UpdateState &inUpdateState)
+{
+	// LockModifications should have been called
+	JPH_ASSERT(mUpdateMutex.is_locked());
+
+	// Test if a tree was updated
+	const UpdateStateImpl *update_state_impl = reinterpret_cast<const UpdateStateImpl *>(&inUpdateState);
+	if (update_state_impl->mTree == nullptr)
+		return;
+
+	update_state_impl->mTree->UpdateFinalize(mBodyManager->GetBodies(), mTracking, update_state_impl->mUpdateState);
+
+	// Make all queries from now on use the new lock
+	mQueryLockIdx = mQueryLockIdx ^ 1;
+}
+
+void BroadPhaseQuadTree::UnlockModifications()
+{
+	// From this point on we allow modifications to the tree again
+	PhysicsLock::sUnlock(mUpdateMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::BroadPhaseUpdate));
+}
+
+BroadPhase::AddState BroadPhaseQuadTree::AddBodiesPrepare(BodyID *ioBodies, int inNumber)
+{
+	JPH_PROFILE_FUNCTION();
+
+	if (inNumber <= 0)
+		return nullptr;
+
+	const BodyVector &bodies = mBodyManager->GetBodies();
+	JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies());
+
+	LayerState *state = new LayerState [mNumLayers];
+
+	// Sort bodies on layer
+	Body * const * const bodies_ptr = bodies.data(); // C pointer or else sort is incredibly slow in debug mode
+	QuickSort(ioBodies, ioBodies + inNumber, [bodies_ptr](BodyID inLHS, BodyID inRHS) { return bodies_ptr[inLHS.GetIndex()]->GetBroadPhaseLayer() < bodies_ptr[inRHS.GetIndex()]->GetBroadPhaseLayer(); });
+
+	BodyID *b_start = ioBodies, *b_end = ioBodies + inNumber;
+	while (b_start < b_end)
+	{
+		// Get broadphase layer
+		BroadPhaseLayer::Type broadphase_layer = (BroadPhaseLayer::Type)bodies[b_start->GetIndex()]->GetBroadPhaseLayer();
+		JPH_ASSERT(broadphase_layer < mNumLayers);
+
+		// Find first body with different layer
+		BodyID *b_mid = std::upper_bound(b_start, b_end, broadphase_layer, [bodies_ptr](BroadPhaseLayer::Type inLayer, BodyID inBodyID) { return inLayer < (BroadPhaseLayer::Type)bodies_ptr[inBodyID.GetIndex()]->GetBroadPhaseLayer(); });
+
+		// Keep track of state for this layer
+		LayerState &layer_state = state[broadphase_layer];
+		layer_state.mBodyStart = b_start;
+		layer_state.mBodyEnd = b_mid;
+
+		// Insert all bodies of the same layer
+		mLayers[broadphase_layer].AddBodiesPrepare(bodies, mTracking, b_start, int(b_mid - b_start), layer_state.mAddState);
+
+		// Keep track in which tree we placed the object
+		for (const BodyID *b = b_start; b < b_mid; ++b)
+		{
+			uint32 index = b->GetIndex();
+			JPH_ASSERT(bodies[index]->GetID() == *b, "Provided BodyID doesn't match BodyID in body manager");
+			JPH_ASSERT(!bodies[index]->IsInBroadPhase());
+			Tracking &t = mTracking[index];
+			JPH_ASSERT(t.mBroadPhaseLayer == (BroadPhaseLayer::Type)cBroadPhaseLayerInvalid);
+			t.mBroadPhaseLayer = broadphase_layer;
+			JPH_ASSERT(t.mObjectLayer == cObjectLayerInvalid);
+			t.mObjectLayer = bodies[index]->GetObjectLayer();
+		}
+
+		// Repeat
+		b_start = b_mid;
+	}
+
+	return state;
+}
+
+void BroadPhaseQuadTree::AddBodiesFinalize(BodyID *ioBodies, int inNumber, AddState inAddState)
+{
+	JPH_PROFILE_FUNCTION();
+
+	if (inNumber <= 0)
+	{
+		JPH_ASSERT(inAddState == nullptr);
+		return;
+	}
+
+	// This cannot run concurrently with UpdatePrepare()/UpdateFinalize()
+	SharedLock lock(mUpdateMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::BroadPhaseUpdate));
+
+	BodyVector &bodies = mBodyManager->GetBodies();
+	JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies());
+
+	LayerState *state = (LayerState *)inAddState;
+
+	for (BroadPhaseLayer::Type broadphase_layer = 0; broadphase_layer < mNumLayers; broadphase_layer++)
+	{
+		const LayerState &l = state[broadphase_layer];
+		if (l.mBodyStart != nullptr)
+		{
+			// Insert all bodies of the same layer
+			mLayers[broadphase_layer].AddBodiesFinalize(mTracking, int(l.mBodyEnd - l.mBodyStart), l.mAddState);
+
+			// Mark added to broadphase
+			for (const BodyID *b = l.mBodyStart; b < l.mBodyEnd; ++b)
+			{
+				uint32 index = b->GetIndex();
+				JPH_ASSERT(bodies[index]->GetID() == *b, "Provided BodyID doesn't match BodyID in body manager");
+				JPH_ASSERT(mTracking[index].mBroadPhaseLayer == broadphase_layer);
+				JPH_ASSERT(mTracking[index].mObjectLayer == bodies[index]->GetObjectLayer());
+				JPH_ASSERT(!bodies[index]->IsInBroadPhase());
+				bodies[index]->SetInBroadPhaseInternal(true);
+			}
+		}
+	}
+
+	delete [] state;
+}
+
+void BroadPhaseQuadTree::AddBodiesAbort(BodyID *ioBodies, int inNumber, AddState inAddState)
+{
+	JPH_PROFILE_FUNCTION();
+
+	if (inNumber <= 0)
+	{
+		JPH_ASSERT(inAddState == nullptr);
+		return;
+	}
+
+	JPH_IF_ENABLE_ASSERTS(const BodyVector &bodies = mBodyManager->GetBodies();)
+	JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies());
+
+	LayerState *state = (LayerState *)inAddState;
+
+	for (BroadPhaseLayer::Type broadphase_layer = 0; broadphase_layer < mNumLayers; broadphase_layer++)
+	{
+		const LayerState &l = state[broadphase_layer];
+		if (l.mBodyStart != nullptr)
+		{
+			// Insert all bodies of the same layer
+			mLayers[broadphase_layer].AddBodiesAbort(mTracking, l.mAddState);
+
+			// Reset bookkeeping
+			for (const BodyID *b = l.mBodyStart; b < l.mBodyEnd; ++b)
+			{
+				uint32 index = b->GetIndex();
+				JPH_ASSERT(bodies[index]->GetID() == *b, "Provided BodyID doesn't match BodyID in body manager");
+				JPH_ASSERT(!bodies[index]->IsInBroadPhase());
+				Tracking &t = mTracking[index];
+				JPH_ASSERT(t.mBroadPhaseLayer == broadphase_layer);
+				t.mBroadPhaseLayer = (BroadPhaseLayer::Type)cBroadPhaseLayerInvalid;
+				t.mObjectLayer = cObjectLayerInvalid;
+			}
+		}
+	}
+
+	delete [] state;
+}
+
+void BroadPhaseQuadTree::RemoveBodies(BodyID *ioBodies, int inNumber)
+{
+	JPH_PROFILE_FUNCTION();
+
+	if (inNumber <= 0)
+		return;
+
+	// This cannot run concurrently with UpdatePrepare()/UpdateFinalize()
+	SharedLock lock(mUpdateMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::BroadPhaseUpdate));
+
+	BodyVector &bodies = mBodyManager->GetBodies();
+	JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies());
+
+	// Sort bodies on layer
+	Tracking *tracking = mTracking.data(); // C pointer or else sort is incredibly slow in debug mode
+	QuickSort(ioBodies, ioBodies + inNumber, [tracking](BodyID inLHS, BodyID inRHS) { return tracking[inLHS.GetIndex()].mBroadPhaseLayer < tracking[inRHS.GetIndex()].mBroadPhaseLayer; });
+
+	BodyID *b_start = ioBodies, *b_end = ioBodies + inNumber;
+	while (b_start < b_end)
+	{
+		// Get broad phase layer
+		BroadPhaseLayer::Type broadphase_layer = mTracking[b_start->GetIndex()].mBroadPhaseLayer;
+		JPH_ASSERT(broadphase_layer != (BroadPhaseLayer::Type)cBroadPhaseLayerInvalid);
+
+		// Find first body with different layer
+		BodyID *b_mid = std::upper_bound(b_start, b_end, broadphase_layer, [tracking](BroadPhaseLayer::Type inLayer, BodyID inBodyID) { return inLayer < tracking[inBodyID.GetIndex()].mBroadPhaseLayer; });
+
+		// Remove all bodies of the same layer
+		mLayers[broadphase_layer].RemoveBodies(bodies, mTracking, b_start, int(b_mid - b_start));
+
+		for (const BodyID *b = b_start; b < b_mid; ++b)
+		{
+			// Reset bookkeeping
+			uint32 index = b->GetIndex();
+			Tracking &t = tracking[index];
+			t.mBroadPhaseLayer = (BroadPhaseLayer::Type)cBroadPhaseLayerInvalid;
+			t.mObjectLayer = cObjectLayerInvalid;
+
+			// Mark removed from broadphase
+			JPH_ASSERT(bodies[index]->IsInBroadPhase());
+			bodies[index]->SetInBroadPhaseInternal(false);
+		}
+
+		// Repeat
+		b_start = b_mid;
+	}
+}
+
+void BroadPhaseQuadTree::NotifyBodiesAABBChanged(BodyID *ioBodies, int inNumber, bool inTakeLock)
+{
+	JPH_PROFILE_FUNCTION();
+
+	if (inNumber <= 0)
+		return;
+
+	// This cannot run concurrently with UpdatePrepare()/UpdateFinalize()
+	if (inTakeLock)
+		PhysicsLock::sLockShared(mUpdateMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::BroadPhaseUpdate));
+	else
+		JPH_ASSERT(mUpdateMutex.is_locked());
+
+	const BodyVector &bodies = mBodyManager->GetBodies();
+	JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies());
+
+	// Sort bodies on layer
+	const Tracking *tracking = mTracking.data(); // C pointer or else sort is incredibly slow in debug mode
+	QuickSort(ioBodies, ioBodies + inNumber, [tracking](BodyID inLHS, BodyID inRHS) { return tracking[inLHS.GetIndex()].mBroadPhaseLayer < tracking[inRHS.GetIndex()].mBroadPhaseLayer; });
+
+	BodyID *b_start = ioBodies, *b_end = ioBodies + inNumber;
+	while (b_start < b_end)
+	{
+		// Get broadphase layer
+		BroadPhaseLayer::Type broadphase_layer = tracking[b_start->GetIndex()].mBroadPhaseLayer;
+		JPH_ASSERT(broadphase_layer != (BroadPhaseLayer::Type)cBroadPhaseLayerInvalid);
+
+		// Find first body with different layer
+		BodyID *b_mid = std::upper_bound(b_start, b_end, broadphase_layer, [tracking](BroadPhaseLayer::Type inLayer, BodyID inBodyID) { return inLayer < tracking[inBodyID.GetIndex()].mBroadPhaseLayer; });
+
+		// Notify all bodies of the same layer changed
+		mLayers[broadphase_layer].NotifyBodiesAABBChanged(bodies, mTracking, b_start, int(b_mid - b_start));
+
+		// Repeat
+		b_start = b_mid;
+	}
+
+	if (inTakeLock)
+		PhysicsLock::sUnlockShared(mUpdateMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::BroadPhaseUpdate));
+}
+
+void BroadPhaseQuadTree::NotifyBodiesLayerChanged(BodyID *ioBodies, int inNumber)
+{
+	JPH_PROFILE_FUNCTION();
+
+	if (inNumber <= 0)
+		return;
+
+	// First sort the bodies that actually changed layer to beginning of the array
+	const BodyVector &bodies = mBodyManager->GetBodies();
+	JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies());
+	for (BodyID *body_id = ioBodies + inNumber - 1; body_id >= ioBodies; --body_id)
+	{
+		uint32 index = body_id->GetIndex();
+		JPH_ASSERT(bodies[index]->GetID() == *body_id, "Provided BodyID doesn't match BodyID in body manager");
+		const Body *body = bodies[index];
+		BroadPhaseLayer::Type broadphase_layer = (BroadPhaseLayer::Type)body->GetBroadPhaseLayer();
+		JPH_ASSERT(broadphase_layer < mNumLayers);
+		if (mTracking[index].mBroadPhaseLayer == broadphase_layer)
+		{
+			// Update tracking information
+			mTracking[index].mObjectLayer = body->GetObjectLayer();
+
+			// Move the body to the end, layer didn't change
+			std::swap(*body_id, ioBodies[inNumber - 1]);
+			--inNumber;
+		}
+	}
+
+	if (inNumber > 0)
+	{
+		// Changing layer requires us to remove from one tree and add to another, so this is equivalent to removing all bodies first and then adding them again
+		RemoveBodies(ioBodies, inNumber);
+		AddState add_state = AddBodiesPrepare(ioBodies, inNumber);
+		AddBodiesFinalize(ioBodies, inNumber, add_state);
+	}
+}
+
+void BroadPhaseQuadTree::CastRay(const RayCast &inRay, RayCastBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies());
+
+	// Prevent this from running in parallel with node deletion in FrameSync(), see notes there
+	shared_lock lock(mQueryLocks[mQueryLockIdx]);
+
+	// Loop over all layers and test the ones that could hit
+	for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l)
+	{
+		const QuadTree &tree = mLayers[l];
+		if (tree.HasBodies() && inBroadPhaseLayerFilter.ShouldCollide(BroadPhaseLayer(l)))
+		{
+			JPH_PROFILE(tree.GetName());
+			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
+{
+	JPH_PROFILE_FUNCTION();
+
+	JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies());
+
+	// Prevent this from running in parallel with node deletion in FrameSync(), see notes there
+	shared_lock lock(mQueryLocks[mQueryLockIdx]);
+
+	// Loop over all layers and test the ones that could hit
+	for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l)
+	{
+		const QuadTree &tree = mLayers[l];
+		if (tree.HasBodies() && inBroadPhaseLayerFilter.ShouldCollide(BroadPhaseLayer(l)))
+		{
+			JPH_PROFILE(tree.GetName());
+			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
+{
+	JPH_PROFILE_FUNCTION();
+
+	JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies());
+
+	// Prevent this from running in parallel with node deletion in FrameSync(), see notes there
+	shared_lock lock(mQueryLocks[mQueryLockIdx]);
+
+	// Loop over all layers and test the ones that could hit
+	for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l)
+	{
+		const QuadTree &tree = mLayers[l];
+		if (tree.HasBodies() && inBroadPhaseLayerFilter.ShouldCollide(BroadPhaseLayer(l)))
+		{
+			JPH_PROFILE(tree.GetName());
+			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
+{
+	JPH_PROFILE_FUNCTION();
+
+	JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies());
+
+	// Prevent this from running in parallel with node deletion in FrameSync(), see notes there
+	shared_lock lock(mQueryLocks[mQueryLockIdx]);
+
+	// Loop over all layers and test the ones that could hit
+	for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l)
+	{
+		const QuadTree &tree = mLayers[l];
+		if (tree.HasBodies() && inBroadPhaseLayerFilter.ShouldCollide(BroadPhaseLayer(l)))
+		{
+			JPH_PROFILE(tree.GetName());
+			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
+{
+	JPH_PROFILE_FUNCTION();
+
+	JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies());
+
+	// Prevent this from running in parallel with node deletion in FrameSync(), see notes there
+	shared_lock lock(mQueryLocks[mQueryLockIdx]);
+
+	// Loop over all layers and test the ones that could hit
+	for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l)
+	{
+		const QuadTree &tree = mLayers[l];
+		if (tree.HasBodies() && inBroadPhaseLayerFilter.ShouldCollide(BroadPhaseLayer(l)))
+		{
+			JPH_PROFILE(tree.GetName());
+			tree.CollideOrientedBox(inBox, ioCollector, inObjectLayerFilter, mTracking);
+			if (ioCollector.ShouldEarlyOut())
+				break;
+		}
+	}
+}
+
+void BroadPhaseQuadTree::CastAABoxNoLock(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies());
+
+	// Loop over all layers and test the ones that could hit
+	for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l)
+	{
+		const QuadTree &tree = mLayers[l];
+		if (tree.HasBodies() && inBroadPhaseLayerFilter.ShouldCollide(BroadPhaseLayer(l)))
+		{
+			JPH_PROFILE(tree.GetName());
+			tree.CastAABox(inBox, ioCollector, inObjectLayerFilter, mTracking);
+			if (ioCollector.ShouldEarlyOut())
+				break;
+		}
+	}
+}
+
+void BroadPhaseQuadTree::CastAABox(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const
+{
+	// Prevent this from running in parallel with node deletion in FrameSync(), see notes there
+	shared_lock lock(mQueryLocks[mQueryLockIdx]);
+
+	CastAABoxNoLock(inBox, ioCollector, inBroadPhaseLayerFilter, inObjectLayerFilter);
+}
+
+void BroadPhaseQuadTree::FindCollidingPairs(BodyID *ioActiveBodies, int inNumActiveBodies, float inSpeculativeContactDistance, const ObjectVsBroadPhaseLayerFilter &inObjectVsBroadPhaseLayerFilter, const ObjectLayerPairFilter &inObjectLayerPairFilter, BodyPairCollector &ioPairCollector) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	const BodyVector &bodies = mBodyManager->GetBodies();
+	JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies());
+
+	// Note that we don't take any locks at this point. We know that the tree is not going to be swapped or deleted while finding collision pairs due to the way the jobs are scheduled in the PhysicsSystem::Update.
+
+	// Sort bodies on layer
+	const Tracking *tracking = mTracking.data(); // C pointer or else sort is incredibly slow in debug mode
+	QuickSort(ioActiveBodies, ioActiveBodies + inNumActiveBodies, [tracking](BodyID inLHS, BodyID inRHS) { 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
+		ObjectLayer object_layer = tracking[b_start->GetIndex()].mObjectLayer;
+		JPH_ASSERT(object_layer != cObjectLayerInvalid);
+
+		// Find first body with different layer
+		BodyID *b_mid = std::upper_bound(b_start, b_end, object_layer, [tracking](ObjectLayer inLayer, BodyID inBodyID) { 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)
+		{
+			const QuadTree &tree = mLayers[l];
+			if (tree.HasBodies() && inObjectVsBroadPhaseLayerFilter.ShouldCollide(object_layer, BroadPhaseLayer(l)))
+			{
+				JPH_PROFILE(tree.GetName());
+				tree.FindCollidingPairs(bodies, b_start, int(b_mid - b_start), inSpeculativeContactDistance, ioPairCollector, inObjectLayerPairFilter);
+			}
+		}
+
+		// Repeat
+		b_start = b_mid;
+	}
+}
+
+AABox BroadPhaseQuadTree::GetBounds() const
+{
+	// Prevent this from running in parallel with node deletion in FrameSync(), see notes there
+	shared_lock lock(mQueryLocks[mQueryLockIdx]);
+
+	AABox bounds;
+	for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l)
+		bounds.Encapsulate(mLayers[l].GetBounds());
+	return bounds;
+}
+
+#ifdef JPH_TRACK_BROADPHASE_STATS
+
+void BroadPhaseQuadTree::ReportStats()
+{
+	Trace("Query Type, Filter Description, Tree Name, Num Queries, Total Time (%%), Total Time Excl. Collector (%%), Nodes Visited, Bodies Visited, Hits Reported, Hits Reported vs Bodies Visited (%%), Hits Reported vs Nodes Visited");
+
+	uint64 total_ticks = 0;
+	for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l)
+		total_ticks += mLayers[l].GetTicks100Pct();
+
+	for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l)
+		mLayers[l].ReportStats(total_ticks);
+}
+
+#endif // JPH_TRACK_BROADPHASE_STATS
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseQuadTree.h

@@ -0,0 +1,108 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/BroadPhase/QuadTree.h>
+#include <Jolt/Physics/Collision/BroadPhase/BroadPhase.h>
+#include <Jolt/Physics/PhysicsLock.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Fast SIMD based quad tree BroadPhase that is multithreading aware and tries to do a minimal amount of locking.
+class JPH_EXPORT BroadPhaseQuadTree final : public BroadPhase
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Destructor
+	virtual					~BroadPhaseQuadTree() override;
+
+	// Implementing interface of BroadPhase (see BroadPhase for documentation)
+	virtual void			Init(BodyManager *inBodyManager, const BroadPhaseLayerInterface &inLayerInterface) override;
+	virtual void			Optimize() override;
+	virtual void			FrameSync() override;
+	virtual void			LockModifications() override;
+	virtual	UpdateState		UpdatePrepare() override;
+	virtual void			UpdateFinalize(const UpdateState &inUpdateState) override;
+	virtual void			UnlockModifications() override;
+	virtual AddState		AddBodiesPrepare(BodyID *ioBodies, int inNumber) override;
+	virtual void			AddBodiesFinalize(BodyID *ioBodies, int inNumber, AddState inAddState) override;
+	virtual void			AddBodiesAbort(BodyID *ioBodies, int inNumber, AddState inAddState) override;
+	virtual void			RemoveBodies(BodyID *ioBodies, int inNumber) override;
+	virtual void			NotifyBodiesAABBChanged(BodyID *ioBodies, int inNumber, bool inTakeLock) override;
+	virtual void			NotifyBodiesLayerChanged(BodyID *ioBodies, int inNumber) override;
+	virtual void			CastRay(const RayCast &inRay, RayCastBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override;
+	virtual void			CollideAABox(const AABox &inBox, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override;
+	virtual void			CollideSphere(Vec3Arg inCenter, float inRadius, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override;
+	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			CastAABoxNoLock(const AABoxCast &inBox, CastShapeBodyCollector &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, const ObjectVsBroadPhaseLayerFilter &inObjectVsBroadPhaseLayerFilter, const ObjectLayerPairFilter &inObjectLayerPairFilter, BodyPairCollector &ioPairCollector) const override;
+	virtual AABox			GetBounds() const override;
+#ifdef JPH_TRACK_BROADPHASE_STATS
+	virtual void			ReportStats() override;
+#endif // JPH_TRACK_BROADPHASE_STATS
+
+private:
+	/// Helper struct for AddBodies handle
+	struct LayerState
+	{
+		JPH_OVERRIDE_NEW_DELETE
+
+		BodyID *			mBodyStart = nullptr;
+		BodyID *			mBodyEnd;
+		QuadTree::AddState	mAddState;
+	};
+
+	using Tracking = QuadTree::Tracking;
+	using TrackingVector = QuadTree::TrackingVector;
+
+#ifdef JPH_ENABLE_ASSERTS
+	/// Context used to lock a physics lock
+	PhysicsLockContext		mLockContext = nullptr;
+#endif // JPH_ENABLE_ASSERTS
+
+	/// Max amount of bodies we support
+	size_t					mMaxBodies = 0;
+
+	/// Array that for each BodyID keeps track of where it is located in which tree
+	TrackingVector			mTracking;
+
+	/// Node allocator for all trees
+	QuadTree::Allocator		mAllocator;
+
+	/// Information about broad phase layers
+	const BroadPhaseLayerInterface *mBroadPhaseLayerInterface = nullptr;
+
+	/// One tree per object layer
+	QuadTree *				mLayers;
+	uint					mNumLayers;
+
+	/// UpdateState implementation for this tree used during UpdatePrepare/Finalize()
+	struct UpdateStateImpl
+	{
+		QuadTree *				mTree;
+		QuadTree::UpdateState	mUpdateState;
+	};
+
+	static_assert(sizeof(UpdateStateImpl) <= sizeof(UpdateState));
+	static_assert(alignof(UpdateStateImpl) <= alignof(UpdateState));
+
+	/// Mutex that prevents object modification during UpdatePrepare/Finalize()
+	SharedMutex				mUpdateMutex;
+
+	/// We double buffer all trees so that we can query while building the next one and we destroy the old tree the next physics update.
+	/// This structure ensures that we wait for queries that are still using the old tree.
+	mutable SharedMutex		mQueryLocks[2];
+
+	/// This index indicates which lock is currently active, it alternates between 0 and 1
+	atomic<uint32>			mQueryLockIdx { 0 };
+
+	/// This is the next tree to update in UpdatePrepare()
+	uint32					mNextLayerToUpdate = 0;
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseQuery.h

@@ -0,0 +1,53 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/BroadPhase/BroadPhaseLayer.h>
+#include <Jolt/Physics/Collision/ObjectLayer.h>
+#include <Jolt/Physics/Collision/CollisionCollector.h>
+#include <Jolt/Physics/Body/BodyID.h>
+#include <Jolt/Core/NonCopyable.h>
+
+JPH_NAMESPACE_BEGIN
+
+struct RayCast;
+class BroadPhaseCastResult;
+class AABox;
+class OrientedBox;
+struct AABoxCast;
+
+// Various collector configurations
+using RayCastBodyCollector = CollisionCollector<BroadPhaseCastResult, CollisionCollectorTraitsCastRay>;
+using CastShapeBodyCollector = CollisionCollector<BroadPhaseCastResult, CollisionCollectorTraitsCastShape>;
+using CollideShapeBodyCollector = CollisionCollector<BodyID, CollisionCollectorTraitsCollideShape>;
+
+/// Interface to the broadphase that can perform collision queries. These queries will only test the bounding box of the body to quickly determine a potential set of colliding bodies.
+/// The shapes of the bodies are not tested, if you want this then you should use the NarrowPhaseQuery interface.
+class JPH_EXPORT BroadPhaseQuery : public NonCopyable
+{
+public:
+	/// Virtual destructor
+	virtual				~BroadPhaseQuery() = default;
+
+	/// Cast a ray and add any hits to ioCollector
+	virtual void		CastRay(const RayCast &inRay, RayCastBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }) const = 0;
+
+	/// Get bodies intersecting with inBox and any hits to ioCollector
+	virtual void		CollideAABox(const AABox &inBox, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }) const = 0;
+
+	/// Get bodies intersecting with a sphere and any hits to ioCollector
+	virtual void		CollideSphere(Vec3Arg inCenter, float inRadius, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }) const = 0;
+
+	/// Get bodies intersecting with a point and any hits to ioCollector
+	virtual void		CollidePoint(Vec3Arg inPoint, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }) const = 0;
+
+	/// Get bodies intersecting with an oriented box and any hits to ioCollector
+	virtual void		CollideOrientedBox(const OrientedBox &inBox, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }) const = 0;
+
+	/// Cast a box and add any hits to ioCollector
+	virtual void		CastAABox(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }) const = 0;
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/ObjectVsBroadPhaseLayerFilterMask.h

@@ -0,0 +1,35 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/BroadPhase/BroadPhaseLayerInterfaceMask.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Class that determines if an object layer can collide with a broadphase layer.
+/// This implementation works together with BroadPhaseLayerInterfaceMask and ObjectLayerPairFilterMask
+class ObjectVsBroadPhaseLayerFilterMask : public ObjectVsBroadPhaseLayerFilter
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+/// Constructor
+					ObjectVsBroadPhaseLayerFilterMask(const BroadPhaseLayerInterfaceMask &inBroadPhaseLayerInterface) :
+		mBroadPhaseLayerInterface(inBroadPhaseLayerInterface)
+	{
+	}
+
+	/// Returns true if an object layer should collide with a broadphase layer
+	virtual bool	ShouldCollide(ObjectLayer inLayer1, BroadPhaseLayer inLayer2) const override
+	{
+		// Just defer to BroadPhaseLayerInterface
+		return mBroadPhaseLayerInterface.ShouldCollide(inLayer1, inLayer2);
+	}
+
+private:
+	const BroadPhaseLayerInterfaceMask &mBroadPhaseLayerInterface;
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/ObjectVsBroadPhaseLayerFilterTable.h

@@ -0,0 +1,66 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/BroadPhase/BroadPhaseLayer.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Class that determines if an object layer can collide with a broadphase layer.
+/// This implementation uses a table and constructs itself from an ObjectLayerPairFilter and a BroadPhaseLayerInterface.
+class ObjectVsBroadPhaseLayerFilterTable : public ObjectVsBroadPhaseLayerFilter
+{
+private:
+	/// Get which bit corresponds to the pair (inLayer1, inLayer2)
+	uint					GetBit(ObjectLayer inLayer1, BroadPhaseLayer inLayer2) const
+	{
+		// Calculate at which bit the entry for this pair resides
+		return inLayer1 * mNumBroadPhaseLayers + (BroadPhaseLayer::Type)inLayer2;
+	}
+
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Construct the table
+	/// @param inBroadPhaseLayerInterface The broad phase layer interface that maps object layers to broad phase layers
+	/// @param inNumBroadPhaseLayers Number of broad phase layers
+	/// @param inObjectLayerPairFilter The object layer pair filter that determines which object layers can collide
+	/// @param inNumObjectLayers Number of object layers
+							ObjectVsBroadPhaseLayerFilterTable(const BroadPhaseLayerInterface &inBroadPhaseLayerInterface, uint inNumBroadPhaseLayers, const ObjectLayerPairFilter &inObjectLayerPairFilter, uint inNumObjectLayers) :
+		mNumBroadPhaseLayers(inNumBroadPhaseLayers)
+	{
+		// Resize table and set all entries to false
+		mTable.resize((inNumBroadPhaseLayers * inNumObjectLayers + 7) / 8, 0);
+
+		// Loop over all object layer pairs
+		for (ObjectLayer o1 = 0; o1 < inNumObjectLayers; ++o1)
+			for (ObjectLayer o2 = 0; o2 < inNumObjectLayers; ++o2)
+			{
+				// Get the broad phase layer for the second object layer
+				BroadPhaseLayer b2 = inBroadPhaseLayerInterface.GetBroadPhaseLayer(o2);
+				JPH_ASSERT((BroadPhaseLayer::Type)b2 < inNumBroadPhaseLayers);
+
+				// If the object layers collide then so should the object and broadphase layer
+				if (inObjectLayerPairFilter.ShouldCollide(o1, o2))
+				{
+					uint bit = GetBit(o1, b2);
+					mTable[bit >> 3] |= 1 << (bit & 0b111);
+				}
+			}
+	}
+
+	/// Returns true if an object layer should collide with a broadphase layer
+	virtual bool			ShouldCollide(ObjectLayer inLayer1, BroadPhaseLayer inLayer2) const override
+	{
+		uint bit = GetBit(inLayer1, inLayer2);
+		return (mTable[bit >> 3] & (1 << (bit & 0b111))) != 0;
+	}
+
+private:
+	uint					mNumBroadPhaseLayers;						///< The total number of broadphase layers
+	Array<uint8>			mTable;										///< The table of bits that indicates which layers collide
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/QuadTree.h

@@ -0,0 +1,389 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/FixedSizeFreeList.h>
+#include <Jolt/Core/Atomics.h>
+#include <Jolt/Core/NonCopyable.h>
+#include <Jolt/Physics/Body/BodyManager.h>
+#include <Jolt/Physics/Collision/BroadPhase/BroadPhase.h>
+
+//#define JPH_DUMP_BROADPHASE_TREE
+
+JPH_NAMESPACE_BEGIN
+
+/// Internal tree structure in broadphase, is essentially a quad AABB tree.
+/// Tree is lockless (except for UpdatePrepare/Finalize() function), modifying objects in the tree will widen the aabbs of parent nodes to make the node fit.
+/// During the UpdatePrepare/Finalize() call the tree is rebuilt to achieve a tight fit again.
+class JPH_EXPORT QuadTree : public NonCopyable
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+private:
+	// Forward declare
+	class AtomicNodeID;
+
+	/// Class that points to either a body or a node in the tree
+	class NodeID
+	{
+	public:
+		JPH_OVERRIDE_NEW_DELETE
+
+		/// Default constructor does not initialize
+		inline					NodeID() = default;
+
+		/// Construct a node ID
+		static inline NodeID	sInvalid()							{ return NodeID(cInvalidNodeIndex); }
+		static inline NodeID	sFromBodyID(BodyID inID)			{ NodeID node_id(inID.GetIndexAndSequenceNumber()); JPH_ASSERT(node_id.IsBody()); return node_id; }
+		static inline NodeID	sFromNodeIndex(uint32 inIdx)		{ JPH_ASSERT((inIdx & cIsNode) == 0); return NodeID(inIdx | cIsNode); }
+
+		/// Check what type of ID it is
+		inline bool				IsValid() const						{ return mID != cInvalidNodeIndex; }
+		inline bool				IsBody() const						{ return (mID & cIsNode) == 0; }
+		inline bool				IsNode() const						{ return (mID & cIsNode) != 0; }
+
+		/// Get body or node index
+		inline BodyID			GetBodyID() const					{ JPH_ASSERT(IsBody()); return BodyID(mID); }
+		inline uint32			GetNodeIndex() const				{ JPH_ASSERT(IsNode()); return mID & ~cIsNode; }
+
+		/// Comparison
+		inline bool				operator == (const BodyID &inRHS) const { return mID == inRHS.GetIndexAndSequenceNumber(); }
+		inline bool				operator == (const NodeID &inRHS) const	{ return mID == inRHS.mID; }
+
+	private:
+		friend class AtomicNodeID;
+
+		inline explicit			NodeID(uint32 inID)					: mID(inID) { }
+
+		static const uint32		cIsNode = BodyID::cBroadPhaseBit;	///< If this bit is set it means that the ID refers to a node, otherwise it refers to a body
+
+		uint32					mID;
+	};
+
+	static_assert(sizeof(NodeID) == sizeof(BodyID), "Body id's should have the same size as NodeIDs");
+
+	/// A NodeID that uses atomics to store the value
+	class AtomicNodeID
+	{
+	public:
+		/// Constructor
+								AtomicNodeID() = default;
+		explicit				AtomicNodeID(const NodeID &inRHS)			: mID(inRHS.mID) { }
+
+		/// Assignment
+		inline void				operator = (const NodeID &inRHS)			{ mID = inRHS.mID; }
+
+		/// Getting the value
+		inline					operator NodeID () const					{ return NodeID(mID); }
+
+		/// Check if the ID is valid
+		inline bool				IsValid() const								{ return mID != cInvalidNodeIndex; }
+
+		/// Comparison
+		inline bool				operator == (const BodyID &inRHS) const		{ return mID == inRHS.GetIndexAndSequenceNumber(); }
+		inline bool				operator == (const NodeID &inRHS) const		{ return mID == inRHS.mID; }
+
+		/// Atomically compare and swap value. Expects inOld value, replaces with inNew value or returns false
+		inline bool				CompareExchange(NodeID inOld, NodeID inNew)	{ return mID.compare_exchange_strong(inOld.mID, inNew.mID); }
+
+	private:
+		atomic<uint32>			mID;
+	};
+
+	/// Class that represents a node in the tree
+	class Node
+	{
+	public:
+		/// Construct node
+		explicit				Node(bool inIsChanged);
+
+		/// Get bounding box encapsulating all children
+		void					GetNodeBounds(AABox &outBounds) const;
+
+		/// Get bounding box in a consistent way with the functions below (check outBounds.IsValid() before using the box)
+		void					GetChildBounds(int inChildIndex, AABox &outBounds) const;
+
+		/// Set the bounds in such a way that other threads will either see a fully correct bounding box or a bounding box with no volume
+		void					SetChildBounds(int inChildIndex, const AABox &inBounds);
+
+		/// Invalidate bounding box in such a way that other threads will not temporarily see a very large bounding box
+		void					InvalidateChildBounds(int inChildIndex);
+
+		/// Encapsulate inBounds in node bounds, returns true if there were changes
+		bool					EncapsulateChildBounds(int inChildIndex, const AABox &inBounds);
+
+		/// Bounding box for child nodes or bodies (all initially set to invalid so no collision test will ever traverse to the leaf)
+		atomic<float>			mBoundsMinX[4];
+		atomic<float>			mBoundsMinY[4];
+		atomic<float>			mBoundsMinZ[4];
+		atomic<float>			mBoundsMaxX[4];
+		atomic<float>			mBoundsMaxY[4];
+		atomic<float>			mBoundsMaxZ[4];
+
+		/// Index of child node or body ID.
+		AtomicNodeID			mChildNodeID[4];
+
+		/// Index of the parent node.
+		/// Note: This value is unreliable during the UpdatePrepare/Finalize() function as a node may be relinked to the newly built tree.
+		atomic<uint32>			mParentNodeIndex = cInvalidNodeIndex;
+
+		/// If this part of the tree has changed, if not, we will treat this sub tree as a single body during the UpdatePrepare/Finalize().
+		/// If any changes are made to an object inside this sub tree then the direct path from the body to the top of the tree will become changed.
+		atomic<uint32>			mIsChanged;
+
+		// Padding to align to 124 bytes
+		uint32					mPadding = 0;
+	};
+
+	// Maximum size of the stack during tree walk
+	static constexpr int		cStackSize = 128;
+
+	static_assert(sizeof(atomic<float>) == 4, "Assuming that an atomic doesn't add any additional storage");
+	static_assert(sizeof(atomic<uint32>) == 4, "Assuming that an atomic doesn't add any additional storage");
+	static_assert(std::is_trivially_destructible<Node>(), "Assuming that we don't have a destructor");
+
+public:
+	/// Class that allocates tree nodes, can be shared between multiple trees
+	using Allocator = FixedSizeFreeList<Node>;
+
+	static_assert(Allocator::ObjectStorageSize == 128, "Node should be 128 bytes");
+
+	/// Data to track location of a Body in the tree
+	struct Tracking
+	{
+		/// Constructor to satisfy the vector class
+								Tracking() = default;
+								Tracking(const Tracking &inRHS) : mBroadPhaseLayer(inRHS.mBroadPhaseLayer.load()), mObjectLayer(inRHS.mObjectLayer.load()), mBodyLocation(inRHS.mBodyLocation.load()) { }
+
+		/// Invalid body location identifier
+		static const uint32		cInvalidBodyLocation = 0xffffffff;
+
+		atomic<BroadPhaseLayer::Type> mBroadPhaseLayer = (BroadPhaseLayer::Type)cBroadPhaseLayerInvalid;
+		atomic<ObjectLayer>		mObjectLayer = cObjectLayerInvalid;
+		atomic<uint32>			mBodyLocation { cInvalidBodyLocation };
+	};
+
+	using TrackingVector = Array<Tracking>;
+
+	/// 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; }
+
+	/// Check if the tree needs an UpdatePrepare/Finalize()
+	inline bool					IsDirty() const						{ return mIsDirty; }
+
+	/// Check if this tree can get an UpdatePrepare/Finalize() or if it needs a DiscardOldTree() first
+	inline bool					CanBeUpdated() const				{ return mFreeNodeBatch.mNumObjects == 0; }
+
+	/// Initialization
+	void						Init(Allocator &inAllocator);
+
+	struct UpdateState
+	{
+		NodeID					mRootNodeID;						///< This will be the new root node id
+	};
+
+	/// Will throw away the previous frame's nodes so that we can start building a new tree in the background
+	void						DiscardOldTree();
+
+	/// Get the bounding box for this tree
+	AABox						GetBounds() const;
+
+	/// Update the broadphase, needs to be called regularly to achieve a tight fit of the tree when bodies have been modified.
+	/// UpdatePrepare() will build the tree, UpdateFinalize() will lock the root of the tree shortly and swap the trees and afterwards clean up temporary data structures.
+	void						UpdatePrepare(const BodyVector &inBodies, TrackingVector &ioTracking, UpdateState &outUpdateState, bool inFullRebuild);
+	void						UpdateFinalize(const BodyVector &inBodies, const TrackingVector &inTracking, const UpdateState &inUpdateState);
+
+	/// Temporary data structure to pass information between AddBodiesPrepare and AddBodiesFinalize/Abort
+	struct AddState
+	{
+		NodeID					mLeafID = NodeID::sInvalid();
+		AABox					mLeafBounds;
+	};
+
+	/// Prepare adding inNumber bodies at ioBodyIDs to the quad tree, returns the state in outState that should be used in AddBodiesFinalize.
+	/// This can be done on a background thread without influencing the broadphase.
+	/// ioBodyIDs may be shuffled around by this function.
+	void						AddBodiesPrepare(const BodyVector &inBodies, TrackingVector &ioTracking, BodyID *ioBodyIDs, int inNumber, AddState &outState);
+
+	/// Finalize adding bodies to the quadtree, supply the same number of bodies as in AddBodiesPrepare.
+	void						AddBodiesFinalize(TrackingVector &ioTracking, int inNumberBodies, const AddState &inState);
+
+	/// Abort adding bodies to the quadtree, supply the same bodies and state as in AddBodiesPrepare.
+	/// This can be done on a background thread without influencing the broadphase.
+	void						AddBodiesAbort(TrackingVector &ioTracking, const AddState &inState);
+
+	/// Remove inNumber bodies in ioBodyIDs from the quadtree.
+	void						RemoveBodies(const BodyVector &inBodies, TrackingVector &ioTracking, const BodyID *ioBodyIDs, int inNumber);
+
+	/// Call whenever the aabb of a body changes.
+	void						NotifyBodiesAABBChanged(const BodyVector &inBodies, const TrackingVector &inTracking, const BodyID *ioBodyIDs, int inNumber);
+
+	/// Cast a ray and get the intersecting bodies in ioCollector.
+	void						CastRay(const RayCast &inRay, RayCastBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const;
+
+	/// Get bodies intersecting with inBox in ioCollector
+	void						CollideAABox(const AABox &inBox, CollideShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const;
+
+	/// Get bodies intersecting with a sphere in ioCollector
+	void						CollideSphere(Vec3Arg inCenter, float inRadius, CollideShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const;
+
+	/// Get bodies intersecting with a point and any hits to ioCollector
+	void						CollidePoint(Vec3Arg inPoint, CollideShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const;
+
+	/// Get bodies intersecting with an oriented box and any hits to ioCollector
+	void						CollideOrientedBox(const OrientedBox &inBox, CollideShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const;
+
+	/// Cast a box and get intersecting bodies in ioCollector
+	void						CastAABox(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const;
+
+	/// 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, const ObjectLayerPairFilter &inObjectLayerPairFilter) const;
+
+#ifdef JPH_TRACK_BROADPHASE_STATS
+	/// Sum up all the ticks spent in the various layers
+	uint64						GetTicks100Pct() const;
+
+	/// Trace the stats of this tree to the TTY
+	void						ReportStats(uint64 inTicks100Pct) const;
+#endif // JPH_TRACK_BROADPHASE_STATS
+
+private:
+	/// Constants
+	static constexpr uint32		cInvalidNodeIndex = 0xffffffff;		///< Value used to indicate node index is invalid
+	static const AABox			cInvalidBounds;						///< Invalid bounding box using cLargeFloat
+
+	/// We alternate between two trees in order to let collision queries complete in parallel to adding/removing objects to the tree
+	struct RootNode
+	{
+		/// Get the ID of the root node
+		inline NodeID			GetNodeID() const					{ return NodeID::sFromNodeIndex(mIndex); }
+
+		/// Index of the root node of the tree (this is always a node, never a body id)
+		atomic<uint32>			mIndex { cInvalidNodeIndex };
+	};
+
+	/// Caches location of body inBodyID in the tracker, body can be found in mNodes[inNodeIdx].mChildNodeID[inChildIdx]
+	void						GetBodyLocation(const TrackingVector &inTracking, BodyID inBodyID, uint32 &outNodeIdx, uint32 &outChildIdx) const;
+	void						SetBodyLocation(TrackingVector &ioTracking, BodyID inBodyID, uint32 inNodeIdx, uint32 inChildIdx) const;
+	static void					sInvalidateBodyLocation(TrackingVector &ioTracking, BodyID inBodyID);
+
+	/// Get the current root of the tree
+	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;
+
+	/// Mark node and all of its parents as changed
+	inline void					MarkNodeAndParentsChanged(uint32 inNodeIndex);
+
+	/// Widen parent bounds of node inNodeIndex to encapsulate inNewBounds, also mark node and all of its parents as changed
+	inline void					WidenAndMarkNodeAndParentsChanged(uint32 inNodeIndex, const AABox &inNewBounds);
+
+	/// Allocate a new node
+	inline uint32				AllocateNode(bool inIsChanged);
+
+	/// Try to insert a new leaf to the tree at inNodeIndex
+	inline bool					TryInsertLeaf(TrackingVector &ioTracking, int inNodeIndex, NodeID inLeafID, const AABox &inLeafBounds, int inLeafNumBodies);
+
+	/// Try to replace the existing root with a new root that contains both the existing root and the new leaf
+	inline bool					TryCreateNewRoot(TrackingVector &ioTracking, atomic<uint32> &ioRootNodeIndex, NodeID inLeafID, const AABox &inLeafBounds, int inLeafNumBodies);
+
+	/// Build a tree for ioBodyIDs, returns the NodeID of the root (which will be the ID of a single body if inNumber = 1). All tree levels up to inMaxDepthMarkChanged will be marked as 'changed'.
+	NodeID						BuildTree(const BodyVector &inBodies, TrackingVector &ioTracking, NodeID *ioNodeIDs, int inNumber, uint inMaxDepthMarkChanged, AABox &outBounds);
+
+	/// Sorts ioNodeIDs spatially into 2 groups. Second groups starts at ioNodeIDs + outMidPoint.
+	/// After the function returns ioNodeIDs and ioNodeCenters will be shuffled
+	static void					sPartition(NodeID *ioNodeIDs, Vec3 *ioNodeCenters, int inNumber, int &outMidPoint);
+
+	/// Sorts ioNodeIDs from inBegin to (but excluding) inEnd spatially into 4 groups.
+	/// outSplit needs to be 5 ints long, when the function returns each group runs from outSplit[i] to (but excluding) outSplit[i + 1]
+	/// After the function returns ioNodeIDs and ioNodeCenters will be shuffled
+	static void					sPartition4(NodeID *ioNodeIDs, Vec3 *ioNodeCenters, int inBegin, int inEnd, int *outSplit);
+
+#ifdef JPH_DEBUG
+	/// Validate that the tree is consistent.
+	/// Note: This function only works if the tree is not modified while we're traversing it.
+	void						ValidateTree(const BodyVector &inBodies, const TrackingVector &inTracking, uint32 inNodeIndex, uint32 inNumExpectedBodies) const;
+#endif
+
+#ifdef JPH_DUMP_BROADPHASE_TREE
+	/// Dump the tree in DOT format (see: https://graphviz.org/)
+	void						DumpTree(const NodeID &inRoot, const char *inFileNamePrefix) const;
+#endif
+
+	/// Allocator that controls adding / freeing nodes
+	Allocator *					mAllocator = nullptr;
+
+	/// This is a list of nodes that must be deleted after the trees are swapped and the old tree is no longer in use
+	Allocator::Batch			mFreeNodeBatch;
+
+	/// Number of bodies currently in the tree
+	/// This is aligned to be in a different cache line from the `Allocator` pointer to prevent cross-thread syncs
+	/// when reading nodes.
+	alignas(JPH_CACHE_LINE_SIZE) atomic<uint32> mNumBodies { 0 };
+
+	/// We alternate between two tree root nodes. When updating, we activate the new tree and we keep the old tree alive.
+	/// for queries that are in progress until the next time DiscardOldTree() is called.
+	RootNode					mRootNode[2];
+	atomic<uint32>				mRootNodeIndex { 0 };
+
+	/// Flag to keep track of changes to the broadphase, if false, we don't need to UpdatePrepare/Finalize()
+	atomic<bool>				mIsDirty = false;
+
+#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 = UnorderedMap<String, Stat>;
+
+	/// Sum up all the ticks in a layer
+	uint64						GetTicks100Pct(const LayerToStats &inLayer) const;
+
+	/// Trace the stats of a single query type to the TTY
+	void						ReportStats(const char *inName, const LayerToStats &inLayer, uint64 inTicks100Pct) const;
+
+	mutable LayerToStats		mCastRayStats;
+	mutable LayerToStats		mCollideAABoxStats;
+	mutable LayerToStats		mCollideSphereStats;
+	mutable LayerToStats		mCollidePointStats;
+	mutable LayerToStats		mCollideOrientedBoxStats;
+	mutable LayerToStats		mCastAABoxStats;
+#endif // JPH_TRACK_BROADPHASE_STATS
+
+	/// Debug function to get the depth of the tree from node inNodeID
+	uint						GetMaxTreeDepth(const NodeID &inNodeID) const;
+
+	/// 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
+};
+
+JPH_NAMESPACE_END