initial commit, 4.5 stable

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

@@ -0,0 +1,20 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Geometry/AABox.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Structure that holds AABox moving linearly through 3d space
+struct AABoxCast
+{
+	JPH_OVERRIDE_NEW_DELETE
+
+	AABox						mBox;						///< Axis aligned box at starting location
+	Vec3						mDirection;					///< Direction and length of the cast (anything beyond this length will not be reported as a hit)
+};
+
+JPH_NAMESPACE_END

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

@@ -0,0 +1,17 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+/// How to treat active/inactive edges.
+/// An active edge is an edge that either has no neighbouring edge or if the angle between the two connecting faces is too large, see: ActiveEdges
+enum class EActiveEdgeMode : uint8
+{
+	CollideOnlyWithActive,								///< Do not collide with inactive edges. For physics simulation, this gives less ghost collisions.
+	CollideWithAll,										///< Collide with all edges. Use this when you're interested in all collisions.
+};
+
+JPH_NAMESPACE_END

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

@@ -0,0 +1,114 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Geometry/ClosestPoint.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// An active edge is an edge that either has no neighbouring edge or if the angle between the two connecting faces is too large.
+namespace ActiveEdges
+{
+	/// Helper function to check if an edge is active or not
+	/// @param inNormal1 Triangle normal of triangle on the left side of the edge (when looking along the edge from the top)
+	/// @param inNormal2 Triangle normal of triangle on the right side of the edge
+	/// @param inEdgeDirection Vector that points along the edge
+	/// @param inCosThresholdAngle Cosine of the threshold angle (if the angle between the two triangles is bigger than this, the edge is active, note that a concave edge is always inactive)
+	inline static bool					IsEdgeActive(Vec3Arg inNormal1, Vec3Arg inNormal2, Vec3Arg inEdgeDirection, float inCosThresholdAngle)
+	{
+		// If normals are opposite the edges are active (the triangles are back to back)
+		float cos_angle_normals = inNormal1.Dot(inNormal2);
+		if (cos_angle_normals < -0.999848f) // cos(179 degrees)
+			return true;
+
+		// Check if concave edge, if so we are not active
+		if (inNormal1.Cross(inNormal2).Dot(inEdgeDirection) < 0.0f)
+			return false;
+
+		// Convex edge, active when angle bigger than threshold
+		return cos_angle_normals < inCosThresholdAngle;
+	}
+
+	/// Replace normal by triangle normal if a hit is hitting an inactive edge
+	/// @param inV0 , inV1 , inV2 form the triangle
+	/// @param inTriangleNormal is the normal of the provided triangle (does not need to be normalized)
+	/// @param inActiveEdges bit 0 = edge v0..v1 is active, bit 1 = edge v1..v2 is active, bit 2 = edge v2..v0 is active
+	/// @param inPoint Collision point on the triangle
+	/// @param inNormal Collision normal on the triangle (does not need to be normalized)
+	/// @param inMovementDirection Can be zero. This gives an indication of in which direction the motion is to determine if when we hit an inactive edge/triangle we should return the triangle normal.
+	/// @return Returns inNormal if an active edge was hit, otherwise returns inTriangleNormal
+	inline static Vec3					FixNormal(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inTriangleNormal, uint8 inActiveEdges, Vec3Arg inPoint, Vec3Arg inNormal, Vec3Arg inMovementDirection)
+	{
+		// Check: All of the edges are active, we have the correct normal already. No need to call this function!
+		JPH_ASSERT(inActiveEdges != 0b111);
+
+		// If inNormal would affect movement less than inTriangleNormal use inNormal
+		// This is done since it is really hard to make a distinction between sliding over a horizontal triangulated grid and hitting an edge (in this case you want to use the triangle normal)
+		// and sliding over a triangulated grid and grazing a vertical triangle with an inactive edge (in this case using the triangle normal will cause the object to bounce back so we want to use the calculated normal).
+		// To solve this we take a movement hint to give an indication of what direction our object is moving. If the edge normal results in less motion difference than the triangle normal we use the edge normal.
+		float normal_length = inNormal.Length();
+		float triangle_normal_length = inTriangleNormal.Length();
+		if (inMovementDirection.Dot(inNormal) * triangle_normal_length < inMovementDirection.Dot(inTriangleNormal) * normal_length)
+			return inNormal;
+
+		// Check: None of the edges are active, we need to use the triangle normal
+		if (inActiveEdges == 0)
+			return inTriangleNormal;
+
+		// Some edges are active.
+		// If normal is parallel to the triangle normal we don't need to check the active edges.
+		if (inTriangleNormal.Dot(inNormal) > 0.999848f * normal_length * triangle_normal_length) // cos(1 degree)
+			return inNormal;
+
+		const float cEpsilon = 1.0e-4f;
+		const float cOneMinusEpsilon = 1.0f - cEpsilon;
+
+		uint colliding_edge;
+
+		// Test where the contact point is in the triangle
+		float u, v, w;
+		ClosestPoint::GetBaryCentricCoordinates(inV0 - inPoint, inV1 - inPoint, inV2 - inPoint, u, v, w);
+		if (u > cOneMinusEpsilon)
+		{
+			// Colliding with v0, edge 0 or 2 needs to be active
+			colliding_edge = 0b101;
+		}
+		else if (v > cOneMinusEpsilon)
+		{
+			// Colliding with v1, edge 0 or 1 needs to be active
+			colliding_edge = 0b011;
+		}
+		else if (w > cOneMinusEpsilon)
+		{
+			// Colliding with v2, edge 1 or 2 needs to be active
+			colliding_edge = 0b110;
+		}
+		else if (u < cEpsilon)
+		{
+			// Colliding with edge v1, v2, edge 1 needs to be active
+			colliding_edge = 0b010;
+		}
+		else if (v < cEpsilon)
+		{
+			// Colliding with edge v0, v2, edge 2 needs to be active
+			colliding_edge = 0b100;
+		}
+		else if (w < cEpsilon)
+		{
+			// Colliding with edge v0, v1, edge 0 needs to be active
+			colliding_edge = 0b001;
+		}
+		else
+		{
+			// Interior hit
+			return inTriangleNormal;
+		}
+
+		// If this edge is active, use the provided normal instead of the triangle normal
+		return (inActiveEdges & colliding_edge) != 0? inNormal : inTriangleNormal;
+	}
+}
+
+JPH_NAMESPACE_END

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

@@ -0,0 +1,16 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+/// How collision detection functions will treat back facing triangles
+enum class EBackFaceMode : uint8
+{
+	IgnoreBackFaces,									///< Ignore collision with back facing surfaces/triangles
+	CollideWithBackFaces,								///< Collide with back facing surfaces/triangles
+};
+
+JPH_NAMESPACE_END

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

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

@@ -0,0 +1,109 @@
+// 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/CastConvexVsTriangles.h>
+#include <Jolt/Physics/Collision/TransformedShape.h>
+#include <Jolt/Physics/Collision/Shape/ScaleHelpers.h>
+#include <Jolt/Physics/Collision/ActiveEdges.h>
+#include <Jolt/Physics/Collision/NarrowPhaseStats.h>
+#include <Jolt/Geometry/EPAPenetrationDepth.h>
+
+JPH_NAMESPACE_BEGIN
+
+CastConvexVsTriangles::CastConvexVsTriangles(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, CastShapeCollector &ioCollector) :
+	mShapeCast(inShapeCast),
+	mShapeCastSettings(inShapeCastSettings),
+	mCenterOfMassTransform2(inCenterOfMassTransform2),
+	mScale(inScale),
+	mSubShapeIDCreator1(inSubShapeIDCreator1),
+	mCollector(ioCollector)
+{
+	JPH_ASSERT(inShapeCast.mShape->GetType() == EShapeType::Convex);
+
+	// Determine if shape is inside out or not
+	mScaleSign = ScaleHelpers::IsInsideOut(inScale)? -1.0f : 1.0f;
+}
+
+void CastConvexVsTriangles::Cast(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, const SubShapeID &inSubShapeID2)
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Scale triangle
+	Vec3 v0 = mScale * inV0;
+	Vec3 v1 = mScale * inV1;
+	Vec3 v2 = mScale * inV2;
+
+	// Calculate triangle normal
+	Vec3 triangle_normal = mScaleSign * (v1 - v0).Cross(v2 - v0);
+
+	// Backface check
+	bool back_facing = triangle_normal.Dot(mShapeCast.mDirection) > 0.0f;
+	if (mShapeCastSettings.mBackFaceModeTriangles == EBackFaceMode::IgnoreBackFaces && back_facing)
+		return;
+
+	// Create triangle support function
+	TriangleConvexSupport triangle { v0, v1, v2 };
+
+	// Check if we already created the cast shape support function
+	if (mSupport == nullptr)
+	{
+		// Determine if we want to use the actual shape or a shrunken shape with convex radius
+		ConvexShape::ESupportMode support_mode = mShapeCastSettings.mUseShrunkenShapeAndConvexRadius? ConvexShape::ESupportMode::ExcludeConvexRadius : ConvexShape::ESupportMode::Default;
+
+		// Create support function
+		mSupport = static_cast<const ConvexShape *>(mShapeCast.mShape)->GetSupportFunction(support_mode, mSupportBuffer, mShapeCast.mScale);
+	}
+
+	EPAPenetrationDepth epa;
+	float fraction = mCollector.GetEarlyOutFraction();
+	Vec3 contact_point_a, contact_point_b, contact_normal;
+	if (epa.CastShape(mShapeCast.mCenterOfMassStart, mShapeCast.mDirection, mShapeCastSettings.mCollisionTolerance, mShapeCastSettings.mPenetrationTolerance, *mSupport, triangle, mSupport->GetConvexRadius(), 0.0f, mShapeCastSettings.mReturnDeepestPoint, fraction, contact_point_a, contact_point_b, contact_normal))
+	{
+		// Check if we have enabled active edge detection
+		if (mShapeCastSettings.mActiveEdgeMode == EActiveEdgeMode::CollideOnlyWithActive && inActiveEdges != 0b111)
+		{
+			// Convert the active edge velocity hint to local space
+			Vec3 active_edge_movement_direction = mCenterOfMassTransform2.Multiply3x3Transposed(mShapeCastSettings.mActiveEdgeMovementDirection);
+
+			// Update the contact normal to account for active edges
+			// Note that we flip the triangle normal as the penetration axis is pointing towards the triangle instead of away
+			contact_normal = ActiveEdges::FixNormal(v0, v1, v2, back_facing? triangle_normal : -triangle_normal, inActiveEdges, contact_point_b, contact_normal, active_edge_movement_direction);
+		}
+
+		// Convert to world space
+		contact_point_a = mCenterOfMassTransform2 * contact_point_a;
+		contact_point_b = mCenterOfMassTransform2 * contact_point_b;
+		Vec3 contact_normal_world = mCenterOfMassTransform2.Multiply3x3(contact_normal);
+
+		// Its a hit, store the sub shape id's
+		ShapeCastResult result(fraction, contact_point_a, contact_point_b, contact_normal_world, back_facing, mSubShapeIDCreator1.GetID(), inSubShapeID2, TransformedShape::sGetBodyID(mCollector.GetContext()));
+
+		// Early out if this hit is deeper than the collector's early out value
+		if (fraction == 0.0f && -result.mPenetrationDepth >= mCollector.GetEarlyOutFraction())
+			return;
+
+		// Gather faces
+		if (mShapeCastSettings.mCollectFacesMode == ECollectFacesMode::CollectFaces)
+		{
+			// Get supporting face of shape 1
+			Mat44 transform_1_to_2 = mShapeCast.mCenterOfMassStart;
+			transform_1_to_2.SetTranslation(transform_1_to_2.GetTranslation() + fraction * mShapeCast.mDirection);
+			static_cast<const ConvexShape *>(mShapeCast.mShape)->GetSupportingFace(SubShapeID(), transform_1_to_2.Multiply3x3Transposed(-contact_normal), mShapeCast.mScale, mCenterOfMassTransform2 * transform_1_to_2, result.mShape1Face);
+
+			// Get face of the triangle
+			triangle.GetSupportingFace(contact_normal, result.mShape2Face);
+
+			// Convert to world space
+			for (Vec3 &p : result.mShape2Face)
+				p = mCenterOfMassTransform2 * p;
+		}
+
+		JPH_IF_TRACK_NARROWPHASE_STATS(TrackNarrowPhaseCollector track;)
+		mCollector.AddHit(result);
+	}
+}
+
+JPH_NAMESPACE_END

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

@@ -0,0 +1,46 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/Shape/ConvexShape.h>
+#include <Jolt/Physics/Collision/ShapeCast.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Collision detection helper that casts a convex object vs one or more triangles
+class JPH_EXPORT CastConvexVsTriangles
+{
+public:
+	/// Constructor
+	/// @param inShapeCast The shape to cast against the triangles and its start and direction
+	/// @param inShapeCastSettings Settings for performing the cast
+	/// @param inScale Local space scale for the shape to cast against (scales relative to its center of mass).
+	/// @param inCenterOfMassTransform2 Is the center of mass transform of shape 2 (excluding scale), this is used to provide a transform to the shape cast result so that local quantities can be transformed into world space.
+	/// @param inSubShapeIDCreator1 Class that tracks the current sub shape ID for the casting shape
+	/// @param ioCollector The collector that receives the results.
+									CastConvexVsTriangles(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, CastShapeCollector &ioCollector);
+
+	/// Cast convex object with a single triangle
+	/// @param inV0 , inV1 , inV2: CCW triangle vertices
+	/// @param inActiveEdges bit 0 = edge v0..v1 is active, bit 1 = edge v1..v2 is active, bit 2 = edge v2..v0 is active
+	/// An active edge is an edge that is not connected to another triangle in such a way that it is impossible to collide with the edge
+	/// @param inSubShapeID2 The sub shape ID for the triangle
+	void							Cast(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, const SubShapeID &inSubShapeID2);
+
+protected:
+	const ShapeCast &				mShapeCast;
+	const ShapeCastSettings &		mShapeCastSettings;
+	const Mat44 &					mCenterOfMassTransform2;
+	Vec3							mScale;
+	SubShapeIDCreator				mSubShapeIDCreator1;
+	CastShapeCollector &			mCollector;
+
+private:
+	ConvexShape::SupportBuffer		mSupportBuffer;						///< Buffer that holds the support function of the cast shape
+	const ConvexShape::Support *	mSupport = nullptr;					///< Support function of the cast shape
+	float							mScaleSign;							///< Sign of the scale, -1 if object is inside out, 1 if not
+};
+
+JPH_NAMESPACE_END

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

@@ -0,0 +1,37 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Body/BodyID.h>
+#include <Jolt/Physics/Collision/Shape/SubShapeID.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Structure that holds a ray cast or other object cast hit
+class BroadPhaseCastResult
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Function required by the CollisionCollector. A smaller fraction is considered to be a 'better hit'. For rays/cast shapes we can just use the collision fraction.
+	inline float	GetEarlyOutFraction() const			{ return mFraction; }
+
+	/// Reset this result so it can be reused for a new cast.
+	inline void		Reset()								{ mBodyID = BodyID(); mFraction = 1.0f + FLT_EPSILON; }
+
+	BodyID			mBodyID;							///< Body that was hit
+	float			mFraction = 1.0f + FLT_EPSILON;		///< Hit fraction of the ray/object [0, 1], HitPoint = Start + mFraction * (End - Start)
+};
+
+/// Specialization of cast result against a shape
+class RayCastResult : public BroadPhaseCastResult
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	SubShapeID		mSubShapeID2;						///< Sub shape ID of shape that we collided against
+};
+
+JPH_NAMESPACE_END

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

@@ -0,0 +1,223 @@
+// 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/CastSphereVsTriangles.h>
+#include <Jolt/Physics/Collision/TransformedShape.h>
+#include <Jolt/Physics/Collision/Shape/ScaleHelpers.h>
+#include <Jolt/Physics/Collision/Shape/SphereShape.h>
+#include <Jolt/Physics/Collision/ActiveEdges.h>
+#include <Jolt/Physics/Collision/NarrowPhaseStats.h>
+#include <Jolt/Geometry/ClosestPoint.h>
+#include <Jolt/Geometry/RaySphere.h>
+#include <Jolt/Core/Profiler.h>
+
+JPH_NAMESPACE_BEGIN
+
+CastSphereVsTriangles::CastSphereVsTriangles(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, CastShapeCollector &ioCollector) :
+	mStart(inShapeCast.mCenterOfMassStart.GetTranslation()),
+	mDirection(inShapeCast.mDirection),
+	mShapeCastSettings(inShapeCastSettings),
+	mCenterOfMassTransform2(inCenterOfMassTransform2),
+	mScale(inScale),
+	mSubShapeIDCreator1(inSubShapeIDCreator1),
+	mCollector(ioCollector)
+{
+	// Cast to sphere shape
+	JPH_ASSERT(inShapeCast.mShape->GetSubType() == EShapeSubType::Sphere);
+	const SphereShape *sphere = static_cast<const SphereShape *>(inShapeCast.mShape);
+
+	// Scale the radius
+	mRadius = sphere->GetRadius() * abs(inShapeCast.mScale.GetX());
+
+	// Determine if shape is inside out or not
+	mScaleSign = ScaleHelpers::IsInsideOut(inScale)? -1.0f : 1.0f;
+}
+
+void CastSphereVsTriangles::AddHit(bool inBackFacing, const SubShapeID &inSubShapeID2, float inFraction, Vec3Arg inContactPointA, Vec3Arg inContactPointB, Vec3Arg inContactNormal)
+{
+	// Convert to world space
+	Vec3 contact_point_a = mCenterOfMassTransform2 * (mStart + inContactPointA);
+	Vec3 contact_point_b = mCenterOfMassTransform2 * (mStart + inContactPointB);
+	Vec3 contact_normal_world = mCenterOfMassTransform2.Multiply3x3(inContactNormal);
+
+	// Its a hit, store the sub shape id's
+	ShapeCastResult result(inFraction, contact_point_a, contact_point_b, contact_normal_world, inBackFacing, mSubShapeIDCreator1.GetID(), inSubShapeID2, TransformedShape::sGetBodyID(mCollector.GetContext()));
+
+	// Note: We don't gather faces here because that's only useful if both shapes have a face. Since the sphere always has only 1 contact point, the manifold is always a point.
+
+	JPH_IF_TRACK_NARROWPHASE_STATS(TrackNarrowPhaseCollector track;)
+	mCollector.AddHit(result);
+}
+
+void CastSphereVsTriangles::AddHitWithActiveEdgeDetection(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, bool inBackFacing, Vec3Arg inTriangleNormal, uint8 inActiveEdges, const SubShapeID &inSubShapeID2, float inFraction, Vec3Arg inContactPointA, Vec3Arg inContactPointB, Vec3Arg inContactNormal)
+{
+	// Check if we have enabled active edge detection
+	Vec3 contact_normal = inContactNormal;
+	if (mShapeCastSettings.mActiveEdgeMode == EActiveEdgeMode::CollideOnlyWithActive && inActiveEdges != 0b111)
+	{
+		// Convert the active edge velocity hint to local space
+		Vec3 active_edge_movement_direction = mCenterOfMassTransform2.Multiply3x3Transposed(mShapeCastSettings.mActiveEdgeMovementDirection);
+
+		// Update the contact normal to account for active edges
+		// Note that we flip the triangle normal as the penetration axis is pointing towards the triangle instead of away
+		contact_normal = ActiveEdges::FixNormal(inV0, inV1, inV2, inBackFacing? inTriangleNormal : -inTriangleNormal, inActiveEdges, inContactPointB, inContactNormal, active_edge_movement_direction);
+	}
+
+	AddHit(inBackFacing, inSubShapeID2, inFraction, inContactPointA, inContactPointB, contact_normal);
+}
+
+// This is a simplified version of the ray cylinder test from: Real Time Collision Detection - Christer Ericson
+// Chapter 5.3.7, page 194-197. Some conditions have been removed as we're not interested in hitting the caps of the cylinder.
+// Note that the ray origin is assumed to be the origin here.
+float CastSphereVsTriangles::RayCylinder(Vec3Arg inRayDirection, Vec3Arg inCylinderA, Vec3Arg inCylinderB, float inRadius) const
+{
+	// Calculate cylinder axis
+	Vec3 axis = inCylinderB - inCylinderA;
+
+	// Make ray start relative to cylinder side A (moving cylinder A to the origin)
+	Vec3 start = -inCylinderA;
+
+	// Test if segment is fully on the A side of the cylinder
+	float start_dot_axis = start.Dot(axis);
+	float direction_dot_axis = inRayDirection.Dot(axis);
+	float end_dot_axis = start_dot_axis + direction_dot_axis;
+	if (start_dot_axis < 0.0f && end_dot_axis < 0.0f)
+		return FLT_MAX;
+
+	// Test if segment is fully on the B side of the cylinder
+	float axis_len_sq = axis.LengthSq();
+	if (start_dot_axis > axis_len_sq && end_dot_axis > axis_len_sq)
+		return FLT_MAX;
+
+	// Calculate a, b and c, the factors for quadratic equation
+	// We're basically solving the ray: x = start + direction * t
+	// The closest point to x on the segment A B is: w = (x . axis) * axis / (axis . axis)
+	// The distance between x and w should be radius: (x - w) . (x - w) = radius^2
+	// Solving this gives the following:
+	float a = axis_len_sq * inRayDirection.LengthSq() - Square(direction_dot_axis);
+	if (abs(a) < 1.0e-6f)
+		return FLT_MAX; // Segment runs parallel to cylinder axis, stop processing, we will either hit at fraction = 0 or we'll hit a vertex
+	float b = axis_len_sq * start.Dot(inRayDirection) - direction_dot_axis * start_dot_axis; // should be multiplied by 2, instead we'll divide a and c by 2 when we solve the quadratic equation
+	float c = axis_len_sq * (start.LengthSq() - Square(inRadius)) - Square(start_dot_axis);
+	float det = Square(b) - a * c; // normally 4 * a * c but since both a and c need to be divided by 2 we lose the 4
+	if (det < 0.0f)
+		return FLT_MAX; // No solution to quadratic equation
+
+	// Solve fraction t where the ray hits the cylinder
+	float t = -(b + sqrt(det)) / a; // normally divided by 2 * a but since a should be divided by 2 we lose the 2
+	if (t < 0.0f || t > 1.0f)
+		return FLT_MAX; // Intersection lies outside segment
+	if (start_dot_axis + t * direction_dot_axis < 0.0f || start_dot_axis + t * direction_dot_axis > axis_len_sq)
+		return FLT_MAX; // Intersection outside the end point of the cylinder, stop processing, we will possibly hit a vertex
+	return t;
+}
+
+void CastSphereVsTriangles::Cast(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, const SubShapeID &inSubShapeID2)
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Scale triangle and make it relative to the start of the cast
+	Vec3 v0 = mScale * inV0 - mStart;
+	Vec3 v1 = mScale * inV1 - mStart;
+	Vec3 v2 = mScale * inV2 - mStart;
+
+	// Calculate triangle normal
+	Vec3 triangle_normal = mScaleSign * (v1 - v0).Cross(v2 - v0);
+	float triangle_normal_len = triangle_normal.Length();
+	if (triangle_normal_len == 0.0f)
+		return; // Degenerate triangle
+	triangle_normal /= triangle_normal_len;
+
+	// Backface check
+	float normal_dot_direction = triangle_normal.Dot(mDirection);
+	bool back_facing = normal_dot_direction > 0.0f;
+	if (mShapeCastSettings.mBackFaceModeTriangles == EBackFaceMode::IgnoreBackFaces && back_facing)
+		return;
+
+	// Test if distance between the sphere and plane of triangle is smaller or equal than the radius
+	if (abs(v0.Dot(triangle_normal)) <= mRadius)
+	{
+		// Check if the sphere intersects at the start of the cast
+		uint32 closest_feature;
+		Vec3 q = ClosestPoint::GetClosestPointOnTriangle(v0, v1, v2, closest_feature);
+		float q_len_sq = q.LengthSq();
+		if (q_len_sq <= Square(mRadius))
+		{
+			// Early out if this hit is deeper than the collector's early out value
+			float q_len = sqrt(q_len_sq);
+			float penetration_depth = mRadius - q_len;
+			if (-penetration_depth >= mCollector.GetEarlyOutFraction())
+				return;
+
+			// Generate contact point
+			Vec3 contact_normal = q_len > 0.0f? q / q_len : Vec3::sAxisY();
+			Vec3 contact_point_a = q + contact_normal * penetration_depth;
+			Vec3 contact_point_b = q;
+			AddHitWithActiveEdgeDetection(v0, v1, v2, back_facing, triangle_normal, inActiveEdges, inSubShapeID2, 0.0f, contact_point_a, contact_point_b, contact_normal);
+			return;
+		}
+	}
+	else
+	{
+		// Check if cast is not parallel to the plane of the triangle
+		float abs_normal_dot_direction = abs(normal_dot_direction);
+		if (abs_normal_dot_direction > 1.0e-6f)
+		{
+			// Calculate the point on the sphere that will hit the triangle's plane first and calculate a fraction where it will do so
+			Vec3 d = Sign(normal_dot_direction) * mRadius * triangle_normal;
+			float plane_intersection = (v0 - d).Dot(triangle_normal) / normal_dot_direction;
+
+			// Check if sphere will hit in the interval that we're interested in
+			if (plane_intersection * abs_normal_dot_direction < -mRadius	// Sphere hits the plane before the sweep, cannot intersect
+				|| plane_intersection >= mCollector.GetEarlyOutFraction())	// Sphere hits the plane after the sweep / early out fraction, cannot intersect
+				return;
+
+			// We can only report an interior hit if we're hitting the plane during our sweep and not before
+			if (plane_intersection >= 0.0f)
+			{
+				// Calculate the point of contact on the plane
+				Vec3 p = d + plane_intersection * mDirection;
+
+				// Check if this is an interior point
+				float u, v, w;
+				if (ClosestPoint::GetBaryCentricCoordinates(v0 - p, v1 - p, v2 - p, u, v, w)
+					&& u >= 0.0f && v >= 0.0f && w >= 0.0f)
+				{
+					// Interior point, we found the collision point. We don't need to check active edges.
+					AddHit(back_facing, inSubShapeID2, plane_intersection, p, p, back_facing? triangle_normal : -triangle_normal);
+					return;
+				}
+			}
+		}
+	}
+
+	// Test 3 edges
+	float fraction = RayCylinder(mDirection, v0, v1, mRadius);
+	fraction = min(fraction, RayCylinder(mDirection, v1, v2, mRadius));
+	fraction = min(fraction, RayCylinder(mDirection, v2, v0, mRadius));
+
+	// Test 3 vertices
+	fraction = min(fraction, RaySphere(Vec3::sZero(), mDirection, v0, mRadius));
+	fraction = min(fraction, RaySphere(Vec3::sZero(), mDirection, v1, mRadius));
+	fraction = min(fraction, RaySphere(Vec3::sZero(), mDirection, v2, mRadius));
+
+	// Check if we have a collision
+	JPH_ASSERT(fraction >= 0.0f);
+	if (fraction < mCollector.GetEarlyOutFraction())
+	{
+		// Calculate the center of the sphere at the point of contact
+		Vec3 p = fraction * mDirection;
+
+		// Get contact point and normal
+		uint32 closest_feature;
+		Vec3 q = ClosestPoint::GetClosestPointOnTriangle(v0 - p, v1 - p, v2 - p, closest_feature);
+		Vec3 contact_normal = q.Normalized();
+		Vec3 contact_point_ab = p + q;
+		AddHitWithActiveEdgeDetection(v0, v1, v2, back_facing, triangle_normal, inActiveEdges, inSubShapeID2, fraction, contact_point_ab, contact_point_ab, contact_normal);
+	}
+}
+
+JPH_NAMESPACE_END

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

@@ -0,0 +1,49 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/ShapeCast.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Collision detection helper that casts a sphere vs one or more triangles
+class JPH_EXPORT CastSphereVsTriangles
+{
+public:
+	/// Constructor
+	/// @param inShapeCast The sphere to cast against the triangles and its start and direction
+	/// @param inShapeCastSettings Settings for performing the cast
+	/// @param inScale Local space scale for the shape to cast against (scales relative to its center of mass).
+	/// @param inCenterOfMassTransform2 Is the center of mass transform of shape 2 (excluding scale), this is used to provide a transform to the shape cast result so that local quantities can be transformed into world space.
+	/// @param inSubShapeIDCreator1 Class that tracks the current sub shape ID for the casting shape
+	/// @param ioCollector The collector that receives the results.
+									CastSphereVsTriangles(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, CastShapeCollector &ioCollector);
+
+	/// Cast sphere with a single triangle
+	/// @param inV0 , inV1 , inV2: CCW triangle vertices
+	/// @param inActiveEdges bit 0 = edge v0..v1 is active, bit 1 = edge v1..v2 is active, bit 2 = edge v2..v0 is active
+	/// An active edge is an edge that is not connected to another triangle in such a way that it is impossible to collide with the edge
+	/// @param inSubShapeID2 The sub shape ID for the triangle
+	void							Cast(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, const SubShapeID &inSubShapeID2);
+
+protected:
+	Vec3							mStart;								///< Starting location of the sphere
+	Vec3							mDirection;							///< Direction and length of movement of sphere
+	float							mRadius;							///< Scaled radius of sphere
+	const ShapeCastSettings &		mShapeCastSettings;
+	const Mat44 &					mCenterOfMassTransform2;
+	Vec3							mScale;
+	SubShapeIDCreator				mSubShapeIDCreator1;
+	CastShapeCollector &			mCollector;
+
+private:
+	void							AddHit(bool inBackFacing, const SubShapeID &inSubShapeID2, float inFraction, Vec3Arg inContactPointA, Vec3Arg inContactPointB, Vec3Arg inContactNormal);
+	void							AddHitWithActiveEdgeDetection(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, bool inBackFacing, Vec3Arg inTriangleNormal, uint8 inActiveEdges, const SubShapeID &inSubShapeID2, float inFraction, Vec3Arg inContactPointA, Vec3Arg inContactPointB, Vec3Arg inContactNormal);
+	float							RayCylinder(Vec3Arg inRayDirection, Vec3Arg inCylinderA, Vec3Arg inCylinderB, float inRadius) const;
+
+	float							mScaleSign;							///< Sign of the scale, -1 if object is inside out, 1 if not
+};
+
+JPH_NAMESPACE_END

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

@@ -0,0 +1,16 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+/// Whether or not to collect faces, used by CastShape and CollideShape
+enum class ECollectFacesMode : uint8
+{
+	CollectFaces,										///< mShape1/2Face is desired
+	NoFaces												///< mShape1/2Face is not desired
+};
+
+JPH_NAMESPACE_END

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

@@ -0,0 +1,157 @@
+// 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/CollideConvexVsTriangles.h>
+#include <Jolt/Physics/Collision/Shape/ScaleHelpers.h>
+#include <Jolt/Physics/Collision/CollideShape.h>
+#include <Jolt/Physics/Collision/TransformedShape.h>
+#include <Jolt/Physics/Collision/ActiveEdges.h>
+#include <Jolt/Physics/Collision/NarrowPhaseStats.h>
+#include <Jolt/Geometry/EPAPenetrationDepth.h>
+#include <Jolt/Geometry/Plane.h>
+
+JPH_NAMESPACE_BEGIN
+
+CollideConvexVsTriangles::CollideConvexVsTriangles(const ConvexShape *inShape1, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeID &inSubShapeID1, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector) :
+	mCollideShapeSettings(inCollideShapeSettings),
+	mCollector(ioCollector),
+	mShape1(inShape1),
+	mScale1(inScale1),
+	mScale2(inScale2),
+	mTransform1(inCenterOfMassTransform1),
+	mSubShapeID1(inSubShapeID1)
+{
+	// Get transforms
+	Mat44 inverse_transform2 = inCenterOfMassTransform2.InversedRotationTranslation();
+	Mat44 transform1_to_2 = inverse_transform2 * inCenterOfMassTransform1;
+	mTransform2To1 = transform1_to_2.InversedRotationTranslation();
+
+	// Calculate bounds
+	mBoundsOf1 = inShape1->GetLocalBounds().Scaled(inScale1);
+	mBoundsOf1.ExpandBy(Vec3::sReplicate(inCollideShapeSettings.mMaxSeparationDistance));
+	mBoundsOf1InSpaceOf2 = mBoundsOf1.Transformed(transform1_to_2);	// Convert bounding box of 1 into space of 2
+
+	// Determine if shape 2 is inside out or not
+	mScaleSign2 = ScaleHelpers::IsInsideOut(inScale2)? -1.0f : 1.0f;
+}
+
+void CollideConvexVsTriangles::Collide(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, const SubShapeID &inSubShapeID2)
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Scale triangle and transform it to the space of 1
+	Vec3 v0 = mTransform2To1 * (mScale2 * inV0);
+	Vec3 v1 = mTransform2To1 * (mScale2 * inV1);
+	Vec3 v2 = mTransform2To1 * (mScale2 * inV2);
+
+	// Calculate triangle normal
+	Vec3 triangle_normal = mScaleSign2 * (v1 - v0).Cross(v2 - v0);
+
+	// Backface check
+	bool back_facing = triangle_normal.Dot(v0) > 0.0f;
+	if (mCollideShapeSettings.mBackFaceMode == EBackFaceMode::IgnoreBackFaces && back_facing)
+		return;
+
+	// Get bounding box for triangle
+	AABox triangle_bbox = AABox::sFromTwoPoints(v0, v1);
+	triangle_bbox.Encapsulate(v2);
+
+	// Get intersection between triangle and shape box, if there is none, we're done
+	if (!triangle_bbox.Overlaps(mBoundsOf1))
+		return;
+
+	// Create triangle support function
+	TriangleConvexSupport triangle(v0, v1, v2);
+
+	// Perform collision detection
+	// Note: As we don't remember the penetration axis from the last iteration, and it is likely that the shape (A) we're colliding the triangle (B) against is in front of the triangle,
+	// and the penetration axis is the shortest distance along to push B out of collision, we use the inverse of the triangle normal as an initial penetration axis. This has been seen
+	// to improve performance by approx. 5% over using a fixed axis like (1, 0, 0).
+	Vec3 penetration_axis = -triangle_normal, point1, point2;
+	EPAPenetrationDepth pen_depth;
+	EPAPenetrationDepth::EStatus status;
+
+	// Get the support function
+	if (mShape1ExCvxRadius == nullptr)
+		mShape1ExCvxRadius = mShape1->GetSupportFunction(ConvexShape::ESupportMode::ExcludeConvexRadius, mBufferExCvxRadius, mScale1);
+
+	// Perform GJK step
+	float max_separation_distance = mCollideShapeSettings.mMaxSeparationDistance;
+	status = pen_depth.GetPenetrationDepthStepGJK(*mShape1ExCvxRadius, mShape1ExCvxRadius->GetConvexRadius() + max_separation_distance, triangle, 0.0f, mCollideShapeSettings.mCollisionTolerance, penetration_axis, point1, point2);
+
+	// Check result of collision detection
+	if (status == EPAPenetrationDepth::EStatus::NotColliding)
+		return;
+	else if (status == EPAPenetrationDepth::EStatus::Indeterminate)
+	{
+		// Need to run expensive EPA algorithm
+
+		// We know we're overlapping at this point, so we can set the max separation distance to 0.
+		// Numerically it is possible that GJK finds that the shapes are overlapping but EPA finds that they're separated.
+		// In order to avoid this, we clamp the max separation distance to 1 so that we don't excessively inflate the shape,
+		// but we still inflate it enough to avoid the case where EPA misses the collision.
+		max_separation_distance = min(max_separation_distance, 1.0f);
+
+		// Get the support function
+		if (mShape1IncCvxRadius == nullptr)
+			mShape1IncCvxRadius = mShape1->GetSupportFunction(ConvexShape::ESupportMode::IncludeConvexRadius, mBufferIncCvxRadius, mScale1);
+
+		// Add convex radius
+		AddConvexRadius shape1_add_max_separation_distance(*mShape1IncCvxRadius, max_separation_distance);
+
+		// Perform EPA step
+		if (!pen_depth.GetPenetrationDepthStepEPA(shape1_add_max_separation_distance, triangle, mCollideShapeSettings.mPenetrationTolerance, penetration_axis, point1, point2))
+			return;
+	}
+
+	// Check if the penetration is bigger than the early out fraction
+	float penetration_depth = (point2 - point1).Length() - max_separation_distance;
+	if (-penetration_depth >= mCollector.GetEarlyOutFraction())
+		return;
+
+	// Correct point1 for the added separation distance
+	float penetration_axis_len = penetration_axis.Length();
+	if (penetration_axis_len > 0.0f)
+		point1 -= penetration_axis * (max_separation_distance / penetration_axis_len);
+
+	// Check if we have enabled active edge detection
+	if (mCollideShapeSettings.mActiveEdgeMode == EActiveEdgeMode::CollideOnlyWithActive && inActiveEdges != 0b111)
+	{
+		// Convert the active edge velocity hint to local space
+		Vec3 active_edge_movement_direction = mTransform1.Multiply3x3Transposed(mCollideShapeSettings.mActiveEdgeMovementDirection);
+
+		// Update the penetration axis to account for active edges
+		// Note that we flip the triangle normal as the penetration axis is pointing towards the triangle instead of away
+		penetration_axis = ActiveEdges::FixNormal(v0, v1, v2, back_facing? triangle_normal : -triangle_normal, inActiveEdges, point2, penetration_axis, active_edge_movement_direction);
+	}
+
+	// Convert to world space
+	point1 = mTransform1 * point1;
+	point2 = mTransform1 * point2;
+	Vec3 penetration_axis_world = mTransform1.Multiply3x3(penetration_axis);
+
+	// Create collision result
+	CollideShapeResult result(point1, point2, penetration_axis_world, penetration_depth, mSubShapeID1, inSubShapeID2, TransformedShape::sGetBodyID(mCollector.GetContext()));
+
+	// Gather faces
+	if (mCollideShapeSettings.mCollectFacesMode == ECollectFacesMode::CollectFaces)
+	{
+		// Get supporting face of shape 1
+		mShape1->GetSupportingFace(SubShapeID(), -penetration_axis, mScale1, mTransform1, result.mShape1Face);
+
+		// Get face of the triangle
+		result.mShape2Face.resize(3);
+		result.mShape2Face[0] = mTransform1 * v0;
+		result.mShape2Face[1] = mTransform1 * v1;
+		result.mShape2Face[2] = mTransform1 * v2;
+	}
+
+	// Notify the collector
+	JPH_IF_TRACK_NARROWPHASE_STATS(TrackNarrowPhaseCollector track;)
+	mCollector.AddHit(result);
+}
+
+JPH_NAMESPACE_END

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

@@ -0,0 +1,56 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Geometry/AABox.h>
+#include <Jolt/Physics/Collision/Shape/Shape.h>
+#include <Jolt/Physics/Collision/Shape/SubShapeID.h>
+#include <Jolt/Physics/Collision/Shape/ConvexShape.h>
+
+JPH_NAMESPACE_BEGIN
+
+class CollideShapeSettings;
+
+/// Collision detection helper that collides a convex object vs one or more triangles
+class JPH_EXPORT CollideConvexVsTriangles
+{
+public:
+	/// Constructor
+	/// @param inShape1 The convex shape to collide against triangles
+	/// @param inScale1 Local space scale for the convex object (scales relative to its center of mass)
+	/// @param inScale2 Local space scale for the triangles
+	/// @param inCenterOfMassTransform1 Transform that takes the center of mass of 1 into world space
+	/// @param inCenterOfMassTransform2 Transform that takes the center of mass of 2 into world space
+	/// @param inSubShapeID1 Sub shape ID of the convex object
+	/// @param inCollideShapeSettings Settings for the collide shape query
+	/// @param ioCollector The collector that will receive the results
+									CollideConvexVsTriangles(const ConvexShape *inShape1, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeID &inSubShapeID1, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector);
+
+	/// Collide convex object with a single triangle
+	/// @param inV0 , inV1 , inV2: CCW triangle vertices
+	/// @param inActiveEdges bit 0 = edge v0..v1 is active, bit 1 = edge v1..v2 is active, bit 2 = edge v2..v0 is active
+	/// An active edge is an edge that is not connected to another triangle in such a way that it is impossible to collide with the edge
+	/// @param inSubShapeID2 The sub shape ID for the triangle
+	void							Collide(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, const SubShapeID &inSubShapeID2);
+
+protected:
+	const CollideShapeSettings &	mCollideShapeSettings;					///< Settings for this collision operation
+	CollideShapeCollector &			mCollector;								///< The collector that will receive the results
+	const ConvexShape *				mShape1;								///< The shape that we're colliding with
+	Vec3							mScale1;								///< The scale of the shape (in shape local space) of the shape we're colliding with
+	Vec3							mScale2;								///< The scale of the shape (in shape local space) of the shape we're colliding against
+	Mat44							mTransform1;							///< Transform of the shape we're colliding with
+	Mat44							mTransform2To1;							///< Transform that takes a point in space of the colliding shape to the shape we're colliding with
+	AABox							mBoundsOf1;								///< Bounds of the colliding shape in local space
+	AABox							mBoundsOf1InSpaceOf2;					///< Bounds of the colliding shape in space of shape we're colliding with
+	SubShapeID						mSubShapeID1;							///< Sub shape ID of colliding shape
+	float							mScaleSign2;							///< Sign of the scale of object 2, -1 if object is inside out, 1 if not
+	ConvexShape::SupportBuffer		mBufferExCvxRadius;						///< Buffer that holds the support function data excluding convex radius
+	ConvexShape::SupportBuffer		mBufferIncCvxRadius;					///< Buffer that holds the support function data including convex radius
+	const ConvexShape::Support *	mShape1ExCvxRadius = nullptr;			///< Actual support function object excluding convex radius
+	const ConvexShape::Support *	mShape1IncCvxRadius = nullptr;			///< Actual support function object including convex radius
+};
+
+JPH_NAMESPACE_END

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

@@ -0,0 +1,25 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Body/BodyID.h>
+#include <Jolt/Physics/Collision/Shape/SubShapeID.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Structure that holds the result of colliding a point against a shape
+class CollidePointResult
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Function required by the CollisionCollector. A smaller fraction is considered to be a 'better hit'. For point queries there is no sensible return value.
+	inline float	GetEarlyOutFraction() const			{ return 0.0f; }
+
+	BodyID			mBodyID;							///< Body that was hit
+	SubShapeID		mSubShapeID2;						///< Sub shape ID of shape that we collided against
+};
+
+JPH_NAMESPACE_END

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

@@ -0,0 +1,106 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/StaticArray.h>
+#include <Jolt/Physics/Collision/BackFaceMode.h>
+#include <Jolt/Physics/Collision/ActiveEdgeMode.h>
+#include <Jolt/Physics/Collision/CollectFacesMode.h>
+#include <Jolt/Physics/Collision/Shape/SubShapeID.h>
+#include <Jolt/Physics/Body/BodyID.h>
+#include <Jolt/Physics/PhysicsSettings.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Class that contains all information of two colliding shapes
+class CollideShapeResult
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Default constructor
+								CollideShapeResult() = default;
+
+	/// Constructor
+								CollideShapeResult(Vec3Arg inContactPointOn1, Vec3Arg inContactPointOn2, Vec3Arg inPenetrationAxis, float inPenetrationDepth, const SubShapeID &inSubShapeID1, const SubShapeID &inSubShapeID2, const BodyID &inBodyID2) :
+		mContactPointOn1(inContactPointOn1),
+		mContactPointOn2(inContactPointOn2),
+		mPenetrationAxis(inPenetrationAxis),
+		mPenetrationDepth(inPenetrationDepth),
+		mSubShapeID1(inSubShapeID1),
+		mSubShapeID2(inSubShapeID2),
+		mBodyID2(inBodyID2)
+	{
+	}
+
+	/// Function required by the CollisionCollector. A smaller fraction is considered to be a 'better hit'. We use -penetration depth to get the hit with the biggest penetration depth
+	inline float				GetEarlyOutFraction() const	{ return -mPenetrationDepth; }
+
+	/// Reverses the hit result, swapping contact point 1 with contact point 2 etc.
+	inline CollideShapeResult	Reversed() const
+	{
+		CollideShapeResult result;
+		result.mContactPointOn2 = mContactPointOn1;
+		result.mContactPointOn1 = mContactPointOn2;
+		result.mPenetrationAxis = -mPenetrationAxis;
+		result.mPenetrationDepth = mPenetrationDepth;
+		result.mSubShapeID2 = mSubShapeID1;
+		result.mSubShapeID1 = mSubShapeID2;
+		result.mBodyID2 = mBodyID2;
+		result.mShape2Face = mShape1Face;
+		result.mShape1Face = mShape2Face;
+		return result;
+	}
+
+	using Face = StaticArray<Vec3, 32>;
+
+	Vec3						mContactPointOn1;			///< Contact point on the surface of shape 1 (in world space or relative to base offset)
+	Vec3						mContactPointOn2;			///< Contact point on the surface of shape 2 (in world space or relative to base offset). If the penetration depth is 0, this will be the same as mContactPointOn1.
+	Vec3						mPenetrationAxis;			///< Direction to move shape 2 out of collision along the shortest path (magnitude is meaningless, in world space). You can use -mPenetrationAxis.Normalized() as contact normal.
+	float						mPenetrationDepth;			///< Penetration depth (move shape 2 by this distance to resolve the collision). If CollideShapeSettings::mMaxSeparationDistance > 0 this number can be negative to indicate that the objects are separated by -mPenetrationDepth. The contact points are the closest points in that case.
+	SubShapeID					mSubShapeID1;				///< Sub shape ID that identifies the face on shape 1
+	SubShapeID					mSubShapeID2;				///< Sub shape ID that identifies the face on shape 2
+	BodyID						mBodyID2;					///< BodyID to which shape 2 belongs to
+	Face						mShape1Face;				///< Colliding face on shape 1 (optional result, in world space or relative to base offset)
+	Face						mShape2Face;				///< Colliding face on shape 2 (optional result, in world space or relative to base offset)
+};
+
+/// Settings to be passed with a collision query
+class CollideSettingsBase
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// How active edges (edges that a moving object should bump into) are handled
+	EActiveEdgeMode				mActiveEdgeMode				= EActiveEdgeMode::CollideOnlyWithActive;
+
+	/// If colliding faces should be collected or only the collision point
+	ECollectFacesMode			mCollectFacesMode			= ECollectFacesMode::NoFaces;
+
+	/// If objects are closer than this distance, they are considered to be colliding (used for GJK) (unit: meter)
+	float						mCollisionTolerance			= cDefaultCollisionTolerance;
+
+	/// A factor that determines the accuracy of the penetration depth calculation. If the change of the squared distance is less than tolerance * current_penetration_depth^2 the algorithm will terminate. (unit: dimensionless)
+	float						mPenetrationTolerance		= cDefaultPenetrationTolerance;
+
+	/// When mActiveEdgeMode is CollideOnlyWithActive a movement direction can be provided. When hitting an inactive edge, the system will select the triangle normal as penetration depth only if it impedes the movement less than with the calculated penetration depth.
+	Vec3						mActiveEdgeMovementDirection = Vec3::sZero();
+};
+
+/// Settings to be passed with a collision query
+class CollideShapeSettings : public CollideSettingsBase
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// When > 0 contacts in the vicinity of the query shape can be found. All nearest contacts that are not further away than this distance will be found.
+	/// Note that in this case CollideShapeResult::mPenetrationDepth can become negative to indicate that objects are not overlapping. (unit: meter)
+	float						mMaxSeparationDistance		= 0.0f;
+
+	/// How backfacing triangles should be treated
+	EBackFaceMode				mBackFaceMode				= EBackFaceMode::IgnoreBackFaces;
+};
+
+JPH_NAMESPACE_END

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

@@ -0,0 +1,93 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2025 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/CollideShape.h>
+#include <Jolt/Physics/Collision/CollisionDispatch.h>
+#include <Jolt/Core/STLLocalAllocator.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Collide 2 shapes and returns at most 1 hit per leaf shape pairs that overlapping. This can be used when not all contacts between the shapes are needed.
+/// E.g. when testing a compound with 2 MeshShapes A and B against a compound with 2 SphereShapes C and D, then at most you'll get 4 collisions: AC, AD, BC, BD.
+/// The default CollisionDispatch::sCollideShapeVsShape function would return all intersecting triangles in A against C, all in B against C etc.
+/// @param inShape1 The first shape
+/// @param inShape2 The second shape
+/// @param inScale1 Local space scale of shape 1 (scales relative to its center of mass)
+/// @param inScale2 Local space scale of shape 2 (scales relative to its center of mass)
+/// @param inCenterOfMassTransform1 Transform to transform center of mass of shape 1 into world space
+/// @param inCenterOfMassTransform2 Transform to transform center of mass of shape 2 into world space
+/// @param inSubShapeIDCreator1 Class that tracks the current sub shape ID for shape 1
+/// @param inSubShapeIDCreator2 Class that tracks the current sub shape ID for shape 2
+/// @param inCollideShapeSettings Options for the CollideShape test
+/// @param ioCollector The collector that receives the results.
+/// @param inShapeFilter allows selectively disabling collisions between pairs of (sub) shapes.
+/// @tparam LeafCollector The type of the collector that will be used to collect hits between leaf pairs. Must be either AnyHitCollisionCollector<CollideShapeCollector> to get any hit (cheapest) or ClosestHitCollisionCollector<CollideShapeCollector> to get the deepest hit (more expensive).
+template <class LeafCollector>
+void CollideShapeVsShapePerLeaf(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter = { })
+{
+	// Tracks information we need about a leaf shape
+	struct LeafShape
+	{
+							LeafShape() = default;
+
+							LeafShape(const AABox &inBounds, Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Shape *inShape, const SubShapeIDCreator &inSubShapeIDCreator) :
+			mBounds(inBounds),
+			mCenterOfMassTransform(inCenterOfMassTransform),
+			mScale(inScale),
+			mShape(inShape),
+			mSubShapeIDCreator(inSubShapeIDCreator)
+		{
+		}
+
+		AABox				mBounds;
+		Mat44				mCenterOfMassTransform;
+		Vec3				mScale;
+		const Shape *		mShape;
+		SubShapeIDCreator	mSubShapeIDCreator;
+	};
+
+	constexpr uint cMaxLocalLeafShapes = 32;
+
+	// A collector that stores the information we need from a leaf shape in an array that is usually on the stack but can fall back to the heap if needed
+	class MyCollector : public TransformedShapeCollector
+	{
+	public:
+							MyCollector()
+		{
+			mHits.reserve(cMaxLocalLeafShapes);
+		}
+
+		void				AddHit(const TransformedShape &inShape) override
+		{
+			mHits.emplace_back(inShape.GetWorldSpaceBounds(), inShape.GetCenterOfMassTransform().ToMat44(), inShape.GetShapeScale(), inShape.mShape, inShape.mSubShapeIDCreator);
+		}
+
+		Array<LeafShape, STLLocalAllocator<LeafShape, cMaxLocalLeafShapes>> mHits;
+	};
+
+	// Get bounds of both shapes
+	AABox bounds1 = inShape1->GetWorldSpaceBounds(inCenterOfMassTransform1, inScale1);
+	AABox bounds2 = inShape2->GetWorldSpaceBounds(inCenterOfMassTransform2, inScale2);
+
+	// Get leaf shapes that overlap with the bounds of the other shape
+	MyCollector leaf_shapes1, leaf_shapes2;
+	inShape1->CollectTransformedShapes(bounds2, inCenterOfMassTransform1.GetTranslation(), inCenterOfMassTransform1.GetQuaternion(), inScale1, inSubShapeIDCreator1, leaf_shapes1, inShapeFilter);
+	inShape2->CollectTransformedShapes(bounds1, inCenterOfMassTransform2.GetTranslation(), inCenterOfMassTransform2.GetQuaternion(), inScale2, inSubShapeIDCreator2, leaf_shapes2, inShapeFilter);
+
+	// Now test each leaf shape against each other leaf
+	for (const LeafShape &leaf1 : leaf_shapes1.mHits)
+		for (const LeafShape &leaf2 : leaf_shapes2.mHits)
+			if (leaf1.mBounds.Overlaps(leaf2.mBounds))
+			{
+				// Use the leaf collector to collect max 1 hit for this pair and pass it on to ioCollector
+				LeafCollector collector;
+				CollisionDispatch::sCollideShapeVsShape(leaf1.mShape, leaf2.mShape, leaf1.mScale, leaf2.mScale, leaf1.mCenterOfMassTransform, leaf2.mCenterOfMassTransform, leaf1.mSubShapeIDCreator, leaf2.mSubShapeIDCreator, inCollideShapeSettings, collector, inShapeFilter);
+				if (collector.HadHit())
+					ioCollector.AddHit(collector.mHit);
+			}
+}
+
+JPH_NAMESPACE_END

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

@@ -0,0 +1,110 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2024 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/SoftBody/SoftBodyVertex.h>
+#include <Jolt/Core/StridedPtr.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Class that allows iterating over the vertices of a soft body.
+/// It tracks the largest penetration and allows storing the resulting collision in a different structure than the soft body vertex itself.
+class CollideSoftBodyVertexIterator
+{
+public:
+	/// Default constructor
+									CollideSoftBodyVertexIterator() = default;
+									CollideSoftBodyVertexIterator(const CollideSoftBodyVertexIterator &) = default;
+
+	/// Construct using (strided) pointers
+									CollideSoftBodyVertexIterator(const StridedPtr<const Vec3> &inPosition, const StridedPtr<const float> &inInvMass, const StridedPtr<Plane> &inCollisionPlane, const StridedPtr<float> &inLargestPenetration, const StridedPtr<int> &inCollidingShapeIndex) :
+		mPosition(inPosition),
+		mInvMass(inInvMass),
+		mCollisionPlane(inCollisionPlane),
+		mLargestPenetration(inLargestPenetration),
+		mCollidingShapeIndex(inCollidingShapeIndex)
+	{
+	}
+
+	/// Construct using a soft body vertex
+	explicit						CollideSoftBodyVertexIterator(SoftBodyVertex *inVertices) :
+		mPosition(&inVertices->mPosition, sizeof(SoftBodyVertex)),
+		mInvMass(&inVertices->mInvMass, sizeof(SoftBodyVertex)),
+		mCollisionPlane(&inVertices->mCollisionPlane, sizeof(SoftBodyVertex)),
+		mLargestPenetration(&inVertices->mLargestPenetration, sizeof(SoftBodyVertex)),
+		mCollidingShapeIndex(&inVertices->mCollidingShapeIndex, sizeof(SoftBodyVertex))
+	{
+	}
+
+	/// Default assignment
+	CollideSoftBodyVertexIterator &	operator = (const CollideSoftBodyVertexIterator &) = default;
+
+	/// Equality operator.
+	/// Note: Only used to determine end iterator, so we only compare position.
+	bool							operator != (const CollideSoftBodyVertexIterator &inRHS) const
+	{
+		return mPosition != inRHS.mPosition;
+	}
+
+	/// Next vertex
+	CollideSoftBodyVertexIterator &	operator ++ ()
+	{
+		++mPosition;
+		++mInvMass;
+		++mCollisionPlane;
+		++mLargestPenetration;
+		++mCollidingShapeIndex;
+		return *this;
+	}
+
+	/// Add an offset
+	/// Note: Only used to determine end iterator, so we only set position.
+	CollideSoftBodyVertexIterator	operator + (int inOffset) const
+	{
+		return CollideSoftBodyVertexIterator(mPosition + inOffset, StridedPtr<const float>(), StridedPtr<Plane>(), StridedPtr<float>(), StridedPtr<int>());
+	}
+
+	/// Get the position of the current vertex
+	Vec3							GetPosition() const
+	{
+		return *mPosition;
+	}
+
+	/// Get the inverse mass of the current vertex
+	float							GetInvMass() const
+	{
+		return *mInvMass;
+	}
+
+	/// Update penetration of the current vertex
+	/// @return Returns true if the vertex has the largest penetration so far, this means you need to follow up by calling SetCollision
+	bool							UpdatePenetration(float inLargestPenetration) const
+	{
+		float &penetration = *mLargestPenetration;
+		if (penetration >= inLargestPenetration)
+			return false;
+		penetration = inLargestPenetration;
+		return true;
+	}
+
+	/// Update the collision of the current vertex
+	void							SetCollision(const Plane &inCollisionPlane, int inCollidingShapeIndex) const
+	{
+		*mCollisionPlane = inCollisionPlane;
+		*mCollidingShapeIndex = inCollidingShapeIndex;
+	}
+
+private:
+	/// Input data
+	StridedPtr<const Vec3>			mPosition;
+	StridedPtr<const float>			mInvMass;
+
+	/// Output data
+	StridedPtr<Plane>				mCollisionPlane;
+	StridedPtr<float>				mLargestPenetration;
+	StridedPtr<int>					mCollidingShapeIndex;
+};
+
+JPH_NAMESPACE_END

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

@@ -0,0 +1,90 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2024 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/CollideSoftBodyVertexIterator.h>
+#include <Jolt/Geometry/ClosestPoint.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Collision detection helper that collides soft body vertices vs triangles
+class JPH_EXPORT CollideSoftBodyVerticesVsTriangles
+{
+public:
+						CollideSoftBodyVerticesVsTriangles(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) :
+		mTransform(inCenterOfMassTransform * Mat44::sScale(inScale)),
+		mInvTransform(mTransform.Inversed()),
+		mNormalSign(ScaleHelpers::IsInsideOut(inScale)? -1.0f : 1.0f)
+	{
+	}
+
+	JPH_INLINE void		StartVertex(const CollideSoftBodyVertexIterator &inVertex)
+	{
+		mLocalPosition = mInvTransform * inVertex.GetPosition();
+		mClosestDistanceSq = FLT_MAX;
+	}
+
+	JPH_INLINE void		ProcessTriangle(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2)
+	{
+		// Get the closest point from the vertex to the triangle
+		uint32 set;
+		Vec3 closest_point = ClosestPoint::GetClosestPointOnTriangle(inV0 - mLocalPosition, inV1 - mLocalPosition, inV2 - mLocalPosition, set);
+		float dist_sq = closest_point.LengthSq();
+		if (dist_sq < mClosestDistanceSq)
+		{
+			mV0 = inV0;
+			mV1 = inV1;
+			mV2 = inV2;
+			mClosestPoint = closest_point;
+			mClosestDistanceSq = dist_sq;
+			mSet = set;
+		}
+	}
+
+	JPH_INLINE void		FinishVertex(const CollideSoftBodyVertexIterator &ioVertex, int inCollidingShapeIndex) const
+	{
+		if (mClosestDistanceSq < FLT_MAX)
+		{
+			// Convert triangle to world space
+			Vec3 v0 = mTransform * mV0;
+			Vec3 v1 = mTransform * mV1;
+			Vec3 v2 = mTransform * mV2;
+			Vec3 triangle_normal = mNormalSign * (v1 - v0).Cross(v2 - v0).NormalizedOr(Vec3::sAxisY());
+
+			if (mSet == 0b111)
+			{
+				// Closest is interior to the triangle, use plane as collision plane but don't allow more than 0.1 m penetration
+				// because otherwise a triangle half a level a way will have a huge penetration if it is back facing
+				float penetration = min(triangle_normal.Dot(v0 - ioVertex.GetPosition()), 0.1f);
+				if (ioVertex.UpdatePenetration(penetration))
+					ioVertex.SetCollision(Plane::sFromPointAndNormal(v0, triangle_normal), inCollidingShapeIndex);
+			}
+			else
+			{
+				// Closest point is on an edge or vertex, use closest point as collision plane
+				Vec3 closest_point = mTransform * (mLocalPosition + mClosestPoint);
+				Vec3 normal = ioVertex.GetPosition() - closest_point;
+				if (normal.Dot(triangle_normal) > 0.0f) // Ignore back facing edges
+				{
+					float normal_length = normal.Length();
+					float penetration = -normal_length;
+					if (ioVertex.UpdatePenetration(penetration))
+						ioVertex.SetCollision(Plane::sFromPointAndNormal(closest_point, normal_length > 0.0f? normal / normal_length : triangle_normal), inCollidingShapeIndex);
+				}
+			}
+		}
+	}
+
+	Mat44				mTransform;
+	Mat44				mInvTransform;
+	Vec3				mLocalPosition;
+	Vec3				mV0, mV1, mV2;
+	Vec3				mClosestPoint;
+	float				mNormalSign;
+	float				mClosestDistanceSq;
+	uint32				mSet;
+};
+
+JPH_NAMESPACE_END

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

@@ -0,0 +1,123 @@
+// 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/CollideSphereVsTriangles.h>
+#include <Jolt/Physics/Collision/Shape/ScaleHelpers.h>
+#include <Jolt/Physics/Collision/CollideShape.h>
+#include <Jolt/Physics/Collision/TransformedShape.h>
+#include <Jolt/Physics/Collision/ActiveEdges.h>
+#include <Jolt/Physics/Collision/NarrowPhaseStats.h>
+#include <Jolt/Core/Profiler.h>
+
+JPH_NAMESPACE_BEGIN
+
+static constexpr uint8 sClosestFeatureToActiveEdgesMask[] = {
+	0b000,		// 0b000: Invalid, guarded by an assert
+	0b101,		// 0b001: Vertex 1 -> edge 1 or 3
+	0b011,		// 0b010: Vertex 2 -> edge 1 or 2
+	0b001,		// 0b011: Vertex 1 & 2 -> edge 1
+	0b110,		// 0b100: Vertex 3 -> edge 2 or 3
+	0b100,		// 0b101: Vertex 1 & 3 -> edge 3
+	0b010,		// 0b110: Vertex 2 & 3 -> edge 2
+	// 0b111: Vertex 1, 2 & 3 -> interior, guarded by an if
+};
+
+CollideSphereVsTriangles::CollideSphereVsTriangles(const SphereShape *inShape1, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeID &inSubShapeID1, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector) :
+	mCollideShapeSettings(inCollideShapeSettings),
+	mCollector(ioCollector),
+	mShape1(inShape1),
+	mScale2(inScale2),
+	mTransform2(inCenterOfMassTransform2),
+	mSubShapeID1(inSubShapeID1)
+{
+	// Calculate the center of the sphere in the space of 2
+	mSphereCenterIn2 = inCenterOfMassTransform2.Multiply3x3Transposed(inCenterOfMassTransform1.GetTranslation() - inCenterOfMassTransform2.GetTranslation());
+
+	// Determine if shape 2 is inside out or not
+	mScaleSign2 = ScaleHelpers::IsInsideOut(inScale2)? -1.0f : 1.0f;
+
+	// Check that the sphere is uniformly scaled
+	JPH_ASSERT(ScaleHelpers::IsUniformScale(inScale1.Abs()));
+	mRadius = abs(inScale1.GetX()) * inShape1->GetRadius();
+	mRadiusPlusMaxSeparationSq = Square(mRadius + inCollideShapeSettings.mMaxSeparationDistance);
+}
+
+void CollideSphereVsTriangles::Collide(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, const SubShapeID &inSubShapeID2)
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Scale triangle and make it relative to the center of the sphere
+	Vec3 v0 = mScale2 * inV0 - mSphereCenterIn2;
+	Vec3 v1 = mScale2 * inV1 - mSphereCenterIn2;
+	Vec3 v2 = mScale2 * inV2 - mSphereCenterIn2;
+
+	// Calculate triangle normal
+	Vec3 triangle_normal = mScaleSign2 * (v1 - v0).Cross(v2 - v0);
+
+	// Backface check
+	bool back_facing = triangle_normal.Dot(v0) > 0.0f;
+	if (mCollideShapeSettings.mBackFaceMode == EBackFaceMode::IgnoreBackFaces && back_facing)
+		return;
+
+	// Check if we collide with the sphere
+	uint32 closest_feature;
+	Vec3 point2 = ClosestPoint::GetClosestPointOnTriangle(v0, v1, v2, closest_feature);
+	float point2_len_sq = point2.LengthSq();
+	if (point2_len_sq > mRadiusPlusMaxSeparationSq)
+		return;
+
+	// Calculate penetration depth
+	float penetration_depth = mRadius - sqrt(point2_len_sq);
+	if (-penetration_depth >= mCollector.GetEarlyOutFraction())
+		return;
+
+	// Calculate penetration axis, direction along which to push 2 to move it out of collision (this is always away from the sphere center)
+	Vec3 penetration_axis = point2.NormalizedOr(Vec3::sAxisY());
+
+	// Calculate the point on the sphere
+	Vec3 point1 = mRadius * penetration_axis;
+
+	// Check if we have enabled active edge detection
+	JPH_ASSERT(closest_feature != 0);
+	if (mCollideShapeSettings.mActiveEdgeMode == EActiveEdgeMode::CollideOnlyWithActive
+		&& closest_feature != 0b111 // For an interior hit we should already have the right normal
+		&& (inActiveEdges & sClosestFeatureToActiveEdgesMask[closest_feature]) == 0) // If we didn't hit an active edge we should take the triangle normal
+	{
+		// Convert the active edge velocity hint to local space
+		Vec3 active_edge_movement_direction = mTransform2.Multiply3x3Transposed(mCollideShapeSettings.mActiveEdgeMovementDirection);
+
+		// See ActiveEdges::FixNormal. If penetration_axis affects the movement less than the triangle normal we keep penetration_axis.
+		Vec3 new_penetration_axis = back_facing? triangle_normal : -triangle_normal;
+		if (active_edge_movement_direction.Dot(penetration_axis) * new_penetration_axis.Length() >= active_edge_movement_direction.Dot(new_penetration_axis))
+			penetration_axis = new_penetration_axis;
+	}
+
+	// Convert to world space
+	point1 = mTransform2 * (mSphereCenterIn2 + point1);
+	point2 = mTransform2 * (mSphereCenterIn2 + point2);
+	Vec3 penetration_axis_world = mTransform2.Multiply3x3(penetration_axis);
+
+	// Create collision result
+	CollideShapeResult result(point1, point2, penetration_axis_world, penetration_depth, mSubShapeID1, inSubShapeID2, TransformedShape::sGetBodyID(mCollector.GetContext()));
+
+	// Gather faces
+	if (mCollideShapeSettings.mCollectFacesMode == ECollectFacesMode::CollectFaces)
+	{
+		// The sphere doesn't have a supporting face
+
+		// Get face of triangle 2
+		result.mShape2Face.resize(3);
+		result.mShape2Face[0] = mTransform2 * (mSphereCenterIn2 + v0);
+		result.mShape2Face[1] = mTransform2 * (mSphereCenterIn2 + v1);
+		result.mShape2Face[2] = mTransform2 * (mSphereCenterIn2 + v2);
+	}
+
+	// Notify the collector
+	JPH_IF_TRACK_NARROWPHASE_STATS(TrackNarrowPhaseCollector track;)
+	mCollector.AddHit(result);
+}
+
+JPH_NAMESPACE_END

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

@@ -0,0 +1,50 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/Shape/Shape.h>
+#include <Jolt/Physics/Collision/Shape/SubShapeID.h>
+#include <Jolt/Physics/Collision/Shape/SphereShape.h>
+
+JPH_NAMESPACE_BEGIN
+
+class CollideShapeSettings;
+
+/// Collision detection helper that collides a sphere vs one or more triangles
+class JPH_EXPORT CollideSphereVsTriangles
+{
+public:
+	/// Constructor
+	/// @param inShape1 The sphere to collide against triangles
+	/// @param inScale1 Local space scale for the sphere (scales relative to its center of mass)
+	/// @param inScale2 Local space scale for the triangles
+	/// @param inCenterOfMassTransform1 Transform that takes the center of mass of 1 into world space
+	/// @param inCenterOfMassTransform2 Transform that takes the center of mass of 2 into world space
+	/// @param inSubShapeID1 Sub shape ID of the convex object
+	/// @param inCollideShapeSettings Settings for the collide shape query
+	/// @param ioCollector The collector that will receive the results
+									CollideSphereVsTriangles(const SphereShape *inShape1, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeID &inSubShapeID1, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector);
+
+	/// Collide sphere with a single triangle
+	/// @param inV0 , inV1 , inV2: CCW triangle vertices
+	/// @param inActiveEdges bit 0 = edge v0..v1 is active, bit 1 = edge v1..v2 is active, bit 2 = edge v2..v0 is active
+	/// An active edge is an edge that is not connected to another triangle in such a way that it is impossible to collide with the edge
+	/// @param inSubShapeID2 The sub shape ID for the triangle
+	void							Collide(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, const SubShapeID &inSubShapeID2);
+
+protected:
+	const CollideShapeSettings &	mCollideShapeSettings;					///< Settings for this collision operation
+	CollideShapeCollector &			mCollector;								///< The collector that will receive the results
+	const SphereShape *				mShape1;								///< The shape that we're colliding with
+	Vec3							mScale2;								///< The scale of the shape (in shape local space) of the shape we're colliding against
+	Mat44							mTransform2;							///< Transform of the shape we're colliding against
+	Vec3							mSphereCenterIn2;						///< The center of the sphere in the space of 2
+	SubShapeID						mSubShapeID1;							///< Sub shape ID of colliding shape
+	float							mScaleSign2;							///< Sign of the scale of object 2, -1 if object is inside out, 1 if not
+	float							mRadius;								///< Radius of the sphere
+	float							mRadiusPlusMaxSeparationSq;				///< (Radius + Max SeparationDistance)^2
+};
+
+JPH_NAMESPACE_END

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

@@ -0,0 +1,109 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+class Body;
+class TransformedShape;
+
+/// Traits to use for CastRay
+class CollisionCollectorTraitsCastRay
+{
+public:
+	/// For rays the early out fraction is the fraction along the line to order hits.
+	static constexpr float InitialEarlyOutFraction = 1.0f + FLT_EPSILON;	///< Furthest hit: Fraction is 1 + epsilon
+	static constexpr float ShouldEarlyOutFraction = 0.0f;					///< Closest hit: Fraction is 0
+};
+
+/// Traits to use for CastShape
+class CollisionCollectorTraitsCastShape
+{
+public:
+	/// For rays the early out fraction is the fraction along the line to order hits.
+	static constexpr float InitialEarlyOutFraction = 1.0f + FLT_EPSILON;	///< Furthest hit: Fraction is 1 + epsilon
+	static constexpr float ShouldEarlyOutFraction = -FLT_MAX;				///< Deepest hit: Penetration is infinite
+};
+
+/// Traits to use for CollideShape
+class CollisionCollectorTraitsCollideShape
+{
+public:
+	/// For shape collisions we use -penetration depth to order hits.
+	static constexpr float InitialEarlyOutFraction = FLT_MAX;				///< Most shallow hit: Separation is infinite
+	static constexpr float ShouldEarlyOutFraction = -FLT_MAX;				///< Deepest hit: Penetration is infinite
+};
+
+/// Traits to use for CollidePoint
+using CollisionCollectorTraitsCollidePoint = CollisionCollectorTraitsCollideShape;
+
+/// Virtual interface that allows collecting multiple collision results
+template <class ResultTypeArg, class TraitsType>
+class CollisionCollector
+{
+public:
+	/// Declare ResultType so that derived classes can use it
+	using ResultType = ResultTypeArg;
+
+	/// Default constructor
+							CollisionCollector() = default;
+
+	/// Constructor to initialize from another collector
+	template <class ResultTypeArg2>
+	explicit				CollisionCollector(const CollisionCollector<ResultTypeArg2, TraitsType> &inRHS) : mEarlyOutFraction(inRHS.GetEarlyOutFraction()), mContext(inRHS.GetContext()) { }
+							CollisionCollector(const CollisionCollector<ResultTypeArg, TraitsType> &inRHS) = default;
+
+	/// Destructor
+	virtual					~CollisionCollector() = default;
+
+	/// If you want to reuse this collector, call Reset()
+	virtual void			Reset()											{ mEarlyOutFraction = TraitsType::InitialEarlyOutFraction; }
+
+	/// When running a query through the NarrowPhaseQuery class, this will be called for every body that is potentially colliding.
+	/// It allows collecting additional information needed by the collision collector implementation from the body under lock protection
+	/// before AddHit is called (e.g. the user data pointer or the velocity of the body).
+	virtual void			OnBody([[maybe_unused]] const Body &inBody)		{ /* Collects nothing by default */ }
+
+	/// When running a query through the NarrowPhaseQuery class, this will be called after all AddHit calls have been made for a particular body.
+	virtual void			OnBodyEnd()										{ /* Does nothing by default */ }
+
+	/// Set by the collision detection functions to the current TransformedShape that we're colliding against before calling the AddHit function.
+	/// Note: Only valid during AddHit! For performance reasons, the pointer is not reset after leaving AddHit so the context may point to freed memory.
+	void					SetContext(const TransformedShape *inContext)	{ mContext = inContext; }
+	const TransformedShape *GetContext() const								{ return mContext; }
+
+	/// This function can be used to set some user data on the collision collector
+	virtual void			SetUserData(uint64 inUserData)					{ /* Does nothing by default */ }
+
+	/// This function will be called for every hit found, it's up to the application to decide how to store the hit
+	virtual void			AddHit(const ResultType &inResult) = 0;
+
+	/// Update the early out fraction (should be lower than before)
+	inline void				UpdateEarlyOutFraction(float inFraction)		{ JPH_ASSERT(inFraction <= mEarlyOutFraction); mEarlyOutFraction = inFraction; }
+
+	/// Reset the early out fraction to a specific value
+	inline void				ResetEarlyOutFraction(float inFraction = TraitsType::InitialEarlyOutFraction) { mEarlyOutFraction = inFraction; }
+
+	/// Force the collision detection algorithm to terminate as soon as possible. Call this from the AddHit function when a satisfying hit is found.
+	inline void				ForceEarlyOut()									{ mEarlyOutFraction = TraitsType::ShouldEarlyOutFraction; }
+
+	/// When true, the collector will no longer accept any additional hits and the collision detection routine should early out as soon as possible
+	inline bool				ShouldEarlyOut() const							{ return mEarlyOutFraction <= TraitsType::ShouldEarlyOutFraction; }
+
+	/// Get the current early out value
+	inline float			GetEarlyOutFraction() const						{ return mEarlyOutFraction; }
+
+	/// Get the current early out value but make sure it's bigger than zero, this is used for shape casting as negative values are used for penetration
+	inline float			GetPositiveEarlyOutFraction() const				{ return max(FLT_MIN, mEarlyOutFraction); }
+
+private:
+	/// The early out fraction determines the fraction below which the collector is still accepting a hit (can be used to reduce the amount of work)
+	float					mEarlyOutFraction = TraitsType::InitialEarlyOutFraction;
+
+	/// Set by the collision detection functions to the current TransformedShape of the body that we're colliding against before calling the AddHit function
+	const TransformedShape *mContext = nullptr;
+};
+
+JPH_NAMESPACE_END

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

@@ -0,0 +1,219 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/CollisionCollector.h>
+#include <Jolt/Core/QuickSort.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Simple implementation that collects all hits and optionally sorts them on distance
+template <class CollectorType>
+class AllHitCollisionCollector : public CollectorType
+{
+public:
+	/// Redeclare ResultType
+	using ResultType = typename CollectorType::ResultType;
+
+	// See: CollectorType::Reset
+	virtual void		Reset() override
+	{
+		CollectorType::Reset();
+
+		mHits.clear();
+	}
+
+	// See: CollectorType::AddHit
+	virtual void		AddHit(const ResultType &inResult) override
+	{
+		mHits.push_back(inResult);
+	}
+
+	/// Order hits on closest first
+	void				Sort()
+	{
+		QuickSort(mHits.begin(), mHits.end(), [](const ResultType &inLHS, const ResultType &inRHS) { return inLHS.GetEarlyOutFraction() < inRHS.GetEarlyOutFraction(); });
+	}
+
+	/// Check if any hits were collected
+	inline bool			HadHit() const
+	{
+		return !mHits.empty();
+	}
+
+	Array<ResultType>	mHits;
+};
+
+/// Simple implementation that collects the closest / deepest hit
+template <class CollectorType>
+class ClosestHitCollisionCollector : public CollectorType
+{
+public:
+	/// Redeclare ResultType
+	using ResultType = typename CollectorType::ResultType;
+
+	// See: CollectorType::Reset
+	virtual void		Reset() override
+	{
+		CollectorType::Reset();
+
+		mHadHit = false;
+	}
+
+	// See: CollectorType::AddHit
+	virtual void		AddHit(const ResultType &inResult) override
+	{
+		float early_out = inResult.GetEarlyOutFraction();
+		if (!mHadHit || early_out < mHit.GetEarlyOutFraction())
+		{
+			// Update early out fraction
+			CollectorType::UpdateEarlyOutFraction(early_out);
+
+			// Store hit
+			mHit = inResult;
+			mHadHit = true;
+		}
+	}
+
+	/// Check if this collector has had a hit
+	inline bool			HadHit() const
+	{
+		return mHadHit;
+	}
+
+	ResultType			mHit;
+
+private:
+	bool				mHadHit = false;
+};
+
+/// Implementation that collects the closest / deepest hit for each body and optionally sorts them on distance
+template <class CollectorType>
+class ClosestHitPerBodyCollisionCollector : public CollectorType
+{
+public:
+	/// Redeclare ResultType
+	using ResultType = typename CollectorType::ResultType;
+
+	// See: CollectorType::Reset
+	virtual void		Reset() override
+	{
+		CollectorType::Reset();
+
+		mHits.clear();
+		mHadHit = false;
+	}
+
+	// See: CollectorType::OnBody
+	virtual void		OnBody(const Body &inBody) override
+	{
+		// Store the early out fraction so we can restore it after we've collected all hits for this body
+		mPreviousEarlyOutFraction = CollectorType::GetEarlyOutFraction();
+	}
+
+	// See: CollectorType::AddHit
+	virtual void		AddHit(const ResultType &inResult) override
+	{
+		float early_out = inResult.GetEarlyOutFraction();
+		if (!mHadHit || early_out < CollectorType::GetEarlyOutFraction())
+		{
+			// Update early out fraction to avoid spending work on collecting further hits for this body
+			CollectorType::UpdateEarlyOutFraction(early_out);
+
+			if (!mHadHit)
+			{
+				// First time we have a hit we append it to the array
+				mHits.push_back(inResult);
+				mHadHit = true;
+			}
+			else
+			{
+				// Closer hits will override the previous one
+				mHits.back() = inResult;
+			}
+		}
+	}
+
+	// See: CollectorType::OnBodyEnd
+	virtual void		OnBodyEnd() override
+	{
+		if (mHadHit)
+		{
+			// Reset the early out fraction to the configured value so that we will continue
+			// to collect hits at any distance for other bodies
+			JPH_ASSERT(mPreviousEarlyOutFraction != -FLT_MAX); // Check that we got a call to OnBody
+			CollectorType::ResetEarlyOutFraction(mPreviousEarlyOutFraction);
+			mHadHit = false;
+		}
+
+		// For asserting purposes we reset the stored early out fraction so we can detect that OnBody was called
+		JPH_IF_ENABLE_ASSERTS(mPreviousEarlyOutFraction = -FLT_MAX;)
+	}
+
+	/// Order hits on closest first
+	void				Sort()
+	{
+		QuickSort(mHits.begin(), mHits.end(), [](const ResultType &inLHS, const ResultType &inRHS) { return inLHS.GetEarlyOutFraction() < inRHS.GetEarlyOutFraction(); });
+	}
+
+	/// Check if any hits were collected
+	inline bool			HadHit() const
+	{
+		return !mHits.empty();
+	}
+
+	Array<ResultType>	mHits;
+
+private:
+	// Store early out fraction that was initially configured for the collector
+	float				mPreviousEarlyOutFraction = -FLT_MAX;
+
+	// Flag to indicate if we have a hit for the current body
+	bool				mHadHit = false;
+};
+
+/// Simple implementation that collects any hit
+template <class CollectorType>
+class AnyHitCollisionCollector : public CollectorType
+{
+public:
+	/// Redeclare ResultType
+	using ResultType = typename CollectorType::ResultType;
+
+	// See: CollectorType::Reset
+	virtual void		Reset() override
+	{
+		CollectorType::Reset();
+
+		mHadHit = false;
+	}
+
+	// See: CollectorType::AddHit
+	virtual void		AddHit(const ResultType &inResult) override
+	{
+		// Test that the collector is not collecting more hits after forcing an early out
+		JPH_ASSERT(!mHadHit);
+
+		// Abort any further testing
+		CollectorType::ForceEarlyOut();
+
+		// Store hit
+		mHit = inResult;
+		mHadHit = true;
+	}
+
+	/// Check if this collector has had a hit
+	inline bool			HadHit() const
+	{
+		return mHadHit;
+	}
+
+	ResultType			mHit;
+
+private:
+	bool				mHadHit = false;
+};
+
+JPH_NAMESPACE_END

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

@@ -0,0 +1,107 @@
+// 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/CollisionDispatch.h>
+#include <Jolt/Physics/Collision/CastResult.h>
+
+JPH_NAMESPACE_BEGIN
+
+CollisionDispatch::CollideShape CollisionDispatch::sCollideShape[NumSubShapeTypes][NumSubShapeTypes];
+CollisionDispatch::CastShape CollisionDispatch::sCastShape[NumSubShapeTypes][NumSubShapeTypes];
+
+void CollisionDispatch::sInit()
+{
+	for (uint i = 0; i < NumSubShapeTypes; ++i)
+		for (uint j = 0; j < NumSubShapeTypes; ++j)
+		{
+			if (sCollideShape[i][j] == nullptr)
+				sCollideShape[i][j] = [](const Shape *, const Shape *, Vec3Arg, Vec3Arg, Mat44Arg, Mat44Arg, const SubShapeIDCreator &, const SubShapeIDCreator &, const CollideShapeSettings &, CollideShapeCollector &, const ShapeFilter &)
+				{
+					JPH_ASSERT(false, "Unsupported shape pair");
+				};
+
+			if (sCastShape[i][j] == nullptr)
+				sCastShape[i][j] = [](const ShapeCast &, const ShapeCastSettings &, const Shape *, Vec3Arg, const ShapeFilter &, Mat44Arg, const SubShapeIDCreator &, const SubShapeIDCreator &, CastShapeCollector &)
+				{
+					JPH_ASSERT(false, "Unsupported shape pair");
+				};
+		}
+}
+
+void CollisionDispatch::sReversedCollideShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter)
+{
+	// A collision collector that flips the collision results
+	class ReversedCollector : public CollideShapeCollector
+	{
+	public:
+		explicit				ReversedCollector(CollideShapeCollector &ioCollector) :
+			CollideShapeCollector(ioCollector),
+			mCollector(ioCollector)
+		{
+		}
+
+		virtual void			AddHit(const CollideShapeResult &inResult) override
+		{
+			// Add the reversed hit
+			mCollector.AddHit(inResult.Reversed());
+
+			// If our chained collector updated its early out fraction, we need to follow
+			UpdateEarlyOutFraction(mCollector.GetEarlyOutFraction());
+		}
+
+	private:
+		CollideShapeCollector &	mCollector;
+	};
+
+	ReversedShapeFilter shape_filter(inShapeFilter);
+	ReversedCollector collector(ioCollector);
+	sCollideShapeVsShape(inShape2, inShape1, inScale2, inScale1, inCenterOfMassTransform2, inCenterOfMassTransform1, inSubShapeIDCreator2, inSubShapeIDCreator1, inCollideShapeSettings, collector, shape_filter);
+}
+
+void CollisionDispatch::sReversedCastShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
+{
+	// A collision collector that flips the collision results
+	class ReversedCollector : public CastShapeCollector
+	{
+	public:
+		explicit				ReversedCollector(CastShapeCollector &ioCollector, Vec3Arg inWorldDirection) :
+			CastShapeCollector(ioCollector),
+			mCollector(ioCollector),
+			mWorldDirection(inWorldDirection)
+		{
+		}
+
+		virtual void			AddHit(const ShapeCastResult &inResult) override
+		{
+			// Add the reversed hit
+			mCollector.AddHit(inResult.Reversed(mWorldDirection));
+
+			// If our chained collector updated its early out fraction, we need to follow
+			UpdateEarlyOutFraction(mCollector.GetEarlyOutFraction());
+		}
+
+	private:
+		CastShapeCollector &	mCollector;
+		Vec3					mWorldDirection;
+	};
+
+	// Reverse the shape cast (shape cast is in local space to shape 2)
+	Mat44 com_start_inv = inShapeCast.mCenterOfMassStart.InversedRotationTranslation();
+	ShapeCast local_shape_cast(inShape, inScale, com_start_inv, -com_start_inv.Multiply3x3(inShapeCast.mDirection));
+
+	// Calculate the center of mass of shape 1 at start of sweep
+	Mat44 shape1_com = inCenterOfMassTransform2 * inShapeCast.mCenterOfMassStart;
+
+	// Calculate the world space direction vector of the shape cast
+	Vec3 world_direction = -inCenterOfMassTransform2.Multiply3x3(inShapeCast.mDirection);
+
+	// Forward the cast
+	ReversedShapeFilter shape_filter(inShapeFilter);
+	ReversedCollector collector(ioCollector, world_direction);
+	sCastShapeVsShapeLocalSpace(local_shape_cast, inShapeCastSettings, inShapeCast.mShape, inShapeCast.mScale, shape_filter, shape1_com, inSubShapeIDCreator2, inSubShapeIDCreator1, collector);
+}
+
+JPH_NAMESPACE_END

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

@@ -0,0 +1,97 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/Shape/Shape.h>
+#include <Jolt/Physics/Collision/Shape/SubShapeID.h>
+#include <Jolt/Physics/Collision/ShapeCast.h>
+#include <Jolt/Physics/Collision/ShapeFilter.h>
+#include <Jolt/Physics/Collision/NarrowPhaseStats.h>
+
+JPH_NAMESPACE_BEGIN
+
+class CollideShapeSettings;
+
+/// Dispatch function, main function to handle collisions between shapes
+class JPH_EXPORT CollisionDispatch
+{
+public:
+	/// Collide 2 shapes and pass any collision on to ioCollector
+	/// @param inShape1 The first shape
+	/// @param inShape2 The second shape
+	/// @param inScale1 Local space scale of shape 1 (scales relative to its center of mass)
+	/// @param inScale2 Local space scale of shape 2 (scales relative to its center of mass)
+	/// @param inCenterOfMassTransform1 Transform to transform center of mass of shape 1 into world space
+	/// @param inCenterOfMassTransform2 Transform to transform center of mass of shape 2 into world space
+	/// @param inSubShapeIDCreator1 Class that tracks the current sub shape ID for shape 1
+	/// @param inSubShapeIDCreator2 Class that tracks the current sub shape ID for shape 2
+	/// @param inCollideShapeSettings Options for the CollideShape test
+	/// @param ioCollector The collector that receives the results.
+	/// @param inShapeFilter allows selectively disabling collisions between pairs of (sub) shapes.
+	static inline void		sCollideShapeVsShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter = { })
+	{
+		JPH_IF_TRACK_NARROWPHASE_STATS(TrackNarrowPhaseStat track(NarrowPhaseStat::sCollideShape[(int)inShape1->GetSubType()][(int)inShape2->GetSubType()]);)
+
+		// Only test shape if it passes the shape filter
+		if (inShapeFilter.ShouldCollide(inShape1, inSubShapeIDCreator1.GetID(), inShape2, inSubShapeIDCreator2.GetID()))
+			sCollideShape[(int)inShape1->GetSubType()][(int)inShape2->GetSubType()](inShape1, inShape2, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter);
+	}
+
+	/// Cast a shape against this shape, passes any hits found to ioCollector.
+	/// Note: This version takes the shape cast in local space relative to the center of mass of inShape, take a look at sCastShapeVsShapeWorldSpace if you have a shape cast in world space.
+	/// @param inShapeCastLocal The shape to cast against the other shape and its start and direction.
+	/// @param inShapeCastSettings Settings for performing the cast
+	/// @param inShape The shape to cast against.
+	/// @param inScale Local space scale for the shape to cast against (scales relative to its center of mass).
+	/// @param inShapeFilter allows selectively disabling collisions between pairs of (sub) shapes.
+	/// @param inCenterOfMassTransform2 Is the center of mass transform of shape 2 (excluding scale), this is used to provide a transform to the shape cast result so that local hit result quantities can be transformed into world space.
+	/// @param inSubShapeIDCreator1 Class that tracks the current sub shape ID for the casting shape
+	/// @param inSubShapeIDCreator2 Class that tracks the current sub shape ID for the shape we're casting against
+	/// @param ioCollector The collector that receives the results.
+	static inline void		sCastShapeVsShapeLocalSpace(const ShapeCast &inShapeCastLocal, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
+	{
+		JPH_IF_TRACK_NARROWPHASE_STATS(TrackNarrowPhaseStat track(NarrowPhaseStat::sCastShape[(int)inShapeCastLocal.mShape->GetSubType()][(int)inShape->GetSubType()]);)
+
+		// Only test shape if it passes the shape filter
+		if (inShapeFilter.ShouldCollide(inShapeCastLocal.mShape, inSubShapeIDCreator1.GetID(), inShape, inSubShapeIDCreator2.GetID()))
+			sCastShape[(int)inShapeCastLocal.mShape->GetSubType()][(int)inShape->GetSubType()](inShapeCastLocal, inShapeCastSettings, inShape, inScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector);
+	}
+
+	/// See: sCastShapeVsShapeLocalSpace.
+	/// The only difference is that the shape cast (inShapeCastWorld) is provided in world space.
+	/// Note: A shape cast contains the center of mass start of the shape, if you have the world transform of the shape you probably want to construct it using ShapeCast::sFromWorldTransform.
+	static inline void		sCastShapeVsShapeWorldSpace(const ShapeCast &inShapeCastWorld, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
+	{
+		ShapeCast local_shape_cast = inShapeCastWorld.PostTransformed(inCenterOfMassTransform2.InversedRotationTranslation());
+		sCastShapeVsShapeLocalSpace(local_shape_cast, inShapeCastSettings, inShape, inScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector);
+	}
+
+	/// Function that collides 2 shapes (see sCollideShapeVsShape)
+	using CollideShape = void (*)(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter);
+
+	/// Function that casts a shape vs another shape (see sCastShapeVsShapeLocalSpace)
+	using CastShape = void (*)(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector);
+
+	/// Initialize all collision functions with a function that asserts and returns no collision
+	static void				sInit();
+
+	/// Register a collide shape function in the collision table
+	static void				sRegisterCollideShape(EShapeSubType inType1, EShapeSubType inType2, CollideShape inFunction)	{ sCollideShape[(int)inType1][(int)inType2] = inFunction; }
+
+	/// Register a cast shape function in the collision table
+	static void				sRegisterCastShape(EShapeSubType inType1, EShapeSubType inType2, CastShape inFunction)			{ sCastShape[(int)inType1][(int)inType2] = inFunction; }
+
+	/// An implementation of CollideShape that swaps inShape1 and inShape2 and swaps the result back, can be registered if the collision function only exists the other way around
+	static void				sReversedCollideShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter);
+
+	/// An implementation of CastShape that swaps inShape1 and inShape2 and swaps the result back, can be registered if the collision function only exists the other way around
+	static void				sReversedCastShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector);
+
+private:
+	static CollideShape		sCollideShape[NumSubShapeTypes][NumSubShapeTypes];
+	static CastShape		sCastShape[NumSubShapeTypes][NumSubShapeTypes];
+};
+
+JPH_NAMESPACE_END

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

@@ -0,0 +1,35 @@
+// 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/CollisionGroup.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(CollisionGroup)
+{
+	JPH_ADD_ATTRIBUTE(CollisionGroup, mGroupFilter)
+	JPH_ADD_ATTRIBUTE(CollisionGroup, mGroupID)
+	JPH_ADD_ATTRIBUTE(CollisionGroup, mSubGroupID)
+}
+
+const CollisionGroup CollisionGroup::sInvalid;
+
+void CollisionGroup::SaveBinaryState(StreamOut &inStream) const
+{
+	inStream.Write(mGroupID);
+	inStream.Write(mSubGroupID);
+}
+
+void CollisionGroup::RestoreBinaryState(StreamIn &inStream)
+{
+	inStream.Read(mGroupID);
+	inStream.Read(mSubGroupID);
+}
+
+JPH_NAMESPACE_END

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

@@ -0,0 +1,97 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/GroupFilter.h>
+#include <Jolt/ObjectStream/SerializableObject.h>
+
+JPH_NAMESPACE_BEGIN
+
+class StreamIn;
+class StreamOut;
+
+/// Two objects collide with each other if:
+/// - Both don't have a group filter
+/// - The first group filter says that the objects can collide
+/// - Or if there's no filter for the first object, the second group filter says the objects can collide
+class JPH_EXPORT CollisionGroup
+{
+	JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, CollisionGroup)
+
+public:
+	using GroupID			= uint32;
+	using SubGroupID		= uint32;
+
+	static const GroupID	cInvalidGroup = ~GroupID(0);
+	static const SubGroupID	cInvalidSubGroup = ~SubGroupID(0);
+
+	/// Default constructor
+							CollisionGroup() = default;
+
+	/// Construct with all properties
+							CollisionGroup(const GroupFilter *inFilter, GroupID inGroupID, SubGroupID inSubGroupID) : mGroupFilter(inFilter), mGroupID(inGroupID), mSubGroupID(inSubGroupID) { }
+
+	/// Set the collision group filter
+	inline void				SetGroupFilter(const GroupFilter *inFilter)
+	{
+		mGroupFilter = inFilter;
+	}
+
+	/// Get the collision group filter
+	inline const GroupFilter *GetGroupFilter() const
+	{
+		return mGroupFilter;
+	}
+
+	/// Set the main group id for this object
+	inline void				SetGroupID(GroupID inID)
+	{
+		mGroupID = inID;
+	}
+
+	inline GroupID			GetGroupID() const
+	{
+		return mGroupID;
+	}
+
+	/// Add this object to a sub group
+	inline void				SetSubGroupID(SubGroupID inID)
+	{
+		mSubGroupID = inID;
+	}
+
+	inline SubGroupID		GetSubGroupID() const
+	{
+		return mSubGroupID;
+	}
+
+	/// Check if this object collides with another object
+	bool					CanCollide(const CollisionGroup &inOther) const
+	{
+		// Call the CanCollide function of the first group filter that's not null
+		if (mGroupFilter != nullptr)
+			return mGroupFilter->CanCollide(*this, inOther);
+		else if (inOther.mGroupFilter != nullptr)
+			return inOther.mGroupFilter->CanCollide(inOther, *this);
+		else
+			return true;
+	}
+
+	/// Saves the state of this object in binary form to inStream. Does not save group filter.
+	void					SaveBinaryState(StreamOut &inStream) const;
+
+	/// Restore the state of this object from inStream. Does not save group filter.
+	void					RestoreBinaryState(StreamIn &inStream);
+
+	/// An invalid collision group
+	static const CollisionGroup	sInvalid;
+
+private:
+	RefConst<GroupFilter>	mGroupFilter;
+	GroupID					mGroupID = cInvalidGroup;
+	SubGroupID				mSubGroupID = cInvalidSubGroup;
+};
+
+JPH_NAMESPACE_END

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

@@ -0,0 +1,119 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/Shape/SubShapeIDPair.h>
+#include <Jolt/Core/StaticArray.h>
+
+JPH_NAMESPACE_BEGIN
+
+class Body;
+class CollideShapeResult;
+
+/// Array of contact points
+using ContactPoints = StaticArray<Vec3, 64>;
+
+/// Manifold class, describes the contact surface between two bodies
+class ContactManifold
+{
+public:
+	/// Swaps shape 1 and 2
+	ContactManifold			SwapShapes() const					{ return { mBaseOffset, -mWorldSpaceNormal, mPenetrationDepth, mSubShapeID2, mSubShapeID1, mRelativeContactPointsOn2, mRelativeContactPointsOn1 }; }
+
+	/// Access to the world space contact positions
+	inline RVec3			GetWorldSpaceContactPointOn1(uint inIndex) const { return mBaseOffset + mRelativeContactPointsOn1[inIndex]; }
+	inline RVec3			GetWorldSpaceContactPointOn2(uint inIndex) const { return mBaseOffset + mRelativeContactPointsOn2[inIndex]; }
+
+	RVec3					mBaseOffset;						///< Offset to which all the contact points are relative
+	Vec3					mWorldSpaceNormal;					///< Normal for this manifold, direction along which to move body 2 out of collision along the shortest path
+	float					mPenetrationDepth;					///< Penetration depth (move shape 2 by this distance to resolve the collision). If this value is negative, this is a speculative contact point and may not actually result in a velocity change as during solving the bodies may not actually collide.
+	SubShapeID				mSubShapeID1;						///< Sub shapes that formed this manifold (note that when multiple manifolds are combined because they're coplanar, we lose some information here because we only keep track of one sub shape pair that we encounter, see description at Body::SetUseManifoldReduction)
+	SubShapeID				mSubShapeID2;
+	ContactPoints			mRelativeContactPointsOn1;			///< Contact points on the surface of shape 1 relative to mBaseOffset.
+	ContactPoints			mRelativeContactPointsOn2;			///< Contact points on the surface of shape 2 relative to mBaseOffset. If there's no penetration, this will be the same as mRelativeContactPointsOn1. If there is penetration they will be different.
+};
+
+/// When a contact point is added or persisted, the callback gets a chance to override certain properties of the contact constraint.
+/// The values are filled in with their defaults by the system so the callback doesn't need to modify anything, but it can if it wants to.
+class ContactSettings
+{
+public:
+	float					mCombinedFriction;					///< Combined friction for the body pair (see: PhysicsSystem::SetCombineFriction)
+	float					mCombinedRestitution;				///< Combined restitution for the body pair (see: PhysicsSystem::SetCombineRestitution)
+	float					mInvMassScale1 = 1.0f;				///< Scale factor for the inverse mass of body 1 (0 = infinite mass, 1 = use original mass, 2 = body has half the mass). For the same contact pair, you should strive to keep the value the same over time.
+	float					mInvInertiaScale1 = 1.0f;			///< Scale factor for the inverse inertia of body 1 (usually same as mInvMassScale1)
+	float					mInvMassScale2 = 1.0f;				///< Scale factor for the inverse mass of body 2 (0 = infinite mass, 1 = use original mass, 2 = body has half the mass). For the same contact pair, you should strive to keep the value the same over time.
+	float					mInvInertiaScale2 = 1.0f;			///< Scale factor for the inverse inertia of body 2 (usually same as mInvMassScale2)
+	bool					mIsSensor;							///< If the contact should be treated as a sensor vs body contact (no collision response)
+	Vec3					mRelativeLinearSurfaceVelocity = Vec3::sZero(); ///< Relative linear surface velocity between the bodies (world space surface velocity of body 2 - world space surface velocity of body 1), can be used to create a conveyor belt effect
+	Vec3					mRelativeAngularSurfaceVelocity = Vec3::sZero(); ///< Relative angular surface velocity between the bodies (world space angular surface velocity of body 2 - world space angular surface velocity of body 1). Note that this angular velocity is relative to the center of mass of body 1, so if you want it relative to body 2's center of mass you need to add body 2 angular velocity x (body 1 world space center of mass - body 2 world space center of mass) to mRelativeLinearSurfaceVelocity.
+};
+
+/// Return value for the OnContactValidate callback. Determines if the contact is being processed or not.
+/// Results are ordered so that the strongest accept has the lowest number and the strongest reject the highest number (which allows for easy combining of results)
+enum class ValidateResult
+{
+	AcceptAllContactsForThisBodyPair,							///< Accept this and any further contact points for this body pair
+	AcceptContact,												///< Accept this contact only (and continue calling this callback for every contact manifold for the same body pair)
+	RejectContact,												///< Reject this contact only (but process any other contact manifolds for the same body pair)
+	RejectAllContactsForThisBodyPair							///< Rejects this and any further contact points for this body pair
+};
+
+/// A listener class that receives collision contact events.
+/// It can be registered with the ContactConstraintManager (or PhysicsSystem).
+/// Note that contact listener callbacks are called from multiple threads at the same time when all bodies are locked, you're only allowed to read from the bodies and you can't change physics state.
+/// During OnContactRemoved you cannot access the bodies at all, see the comments at that function.
+class ContactListener
+{
+public:
+	/// Ensure virtual destructor
+	virtual					~ContactListener() = default;
+
+	/// Called after detecting a collision between a body pair, but before calling OnContactAdded and before adding the contact constraint.
+	/// If the function rejects the contact, the contact will not be added and any other contacts between this body pair will not be processed.
+	/// This function will only be called once per PhysicsSystem::Update per body pair and may not be called again the next update
+	/// if a contact persists and no new contact pairs between sub shapes are found.
+	/// This is a rather expensive time to reject a contact point since a lot of the collision detection has happened already, make sure you
+	/// filter out the majority of undesired body pairs through the ObjectLayerPairFilter that is registered on the PhysicsSystem.
+	/// Note that this callback is called when all bodies are locked, so don't use any locking functions!
+	/// Body 1 will have a motion type that is larger or equal than body 2's motion type (order from large to small: dynamic -> kinematic -> static). When motion types are equal, they are ordered by BodyID.
+	/// The collision result (inCollisionResult) is reported relative to inBaseOffset.
+	virtual ValidateResult	OnContactValidate([[maybe_unused]] const Body &inBody1, [[maybe_unused]] const Body &inBody2, [[maybe_unused]] RVec3Arg inBaseOffset, [[maybe_unused]] const CollideShapeResult &inCollisionResult) { return ValidateResult::AcceptAllContactsForThisBodyPair; }
+
+	/// Called whenever a new contact point is detected.
+	/// Note that this callback is called when all bodies are locked, so don't use any locking functions!
+	/// Body 1 and 2 will be sorted such that body 1 ID < body 2 ID, so body 1 may not be dynamic.
+	/// Note that only active bodies will report contacts, as soon as a body goes to sleep the contacts between that body and all other
+	/// bodies will receive an OnContactRemoved callback, if this is the case then Body::IsActive() will return false during the callback.
+	/// When contacts are added, the constraint solver has not run yet, so the collision impulse is unknown at that point.
+	/// The velocities of inBody1 and inBody2 are the velocities before the contact has been resolved, so you can use this to
+	/// estimate the collision impulse to e.g. determine the volume of the impact sound to play (see: EstimateCollisionResponse).
+	virtual void			OnContactAdded([[maybe_unused]] const Body &inBody1, [[maybe_unused]] const Body &inBody2, [[maybe_unused]] const ContactManifold &inManifold, [[maybe_unused]] ContactSettings &ioSettings) { /* Do nothing */ }
+
+	/// Called whenever a contact is detected that was also detected last update.
+	/// Note that this callback is called when all bodies are locked, so don't use any locking functions!
+	/// Body 1 and 2 will be sorted such that body 1 ID < body 2 ID, so body 1 may not be dynamic.
+	/// If the structure of the shape of a body changes between simulation steps (e.g. by adding/removing a child shape of a compound shape),
+	/// it is possible that the same sub shape ID used to identify the removed child shape is now reused for a different child shape. The physics
+	/// system cannot detect this, so may send a 'contact persisted' callback even though the contact is now on a different child shape. You can
+	/// detect this by keeping the old shape (before adding/removing a part) around until the next PhysicsSystem::Update (when the OnContactPersisted
+	/// callbacks are triggered) and resolving the sub shape ID against both the old and new shape to see if they still refer to the same child shape.
+	virtual void			OnContactPersisted([[maybe_unused]] const Body &inBody1, [[maybe_unused]] const Body &inBody2, [[maybe_unused]] const ContactManifold &inManifold, [[maybe_unused]] ContactSettings &ioSettings) { /* Do nothing */ }
+
+	/// Called whenever a contact was detected last update but is not detected anymore.
+	/// You cannot access the bodies at the time of this callback because:
+	/// - All bodies are locked at the time of this callback.
+	/// - Some properties of the bodies are being modified from another thread at the same time.
+	/// - The body may have been removed and destroyed (you'll receive an OnContactRemoved callback in the PhysicsSystem::Update after the body has been removed).
+	/// Cache what you need in the OnContactAdded and OnContactPersisted callbacks and store it in a separate structure to use during this callback.
+	/// Alternatively, you could just record that the contact was removed and process it after PhysicsSimulation::Update.
+	/// Body 1 and 2 will be sorted such that body 1 ID < body 2 ID, so body 1 may not be dynamic.
+	/// The sub shape ID were created in the previous simulation step too, so if the structure of a shape changes (e.g. by adding/removing a child shape of a compound shape),
+	/// the sub shape ID may not be valid / may not point to the same sub shape anymore.
+	/// If you want to know if this is the last contact between the two bodies, use PhysicsSystem::WereBodiesInContact.
+	virtual void			OnContactRemoved([[maybe_unused]] const SubShapeIDPair &inSubShapePair) { /* Do nothing */ }
+};
+
+JPH_NAMESPACE_END

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

@@ -0,0 +1,213 @@
+// 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/EstimateCollisionResponse.h>
+#include <Jolt/Physics/Body/Body.h>
+
+JPH_NAMESPACE_BEGIN
+
+void EstimateCollisionResponse(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, CollisionEstimationResult &outResult, float inCombinedFriction, float inCombinedRestitution, float inMinVelocityForRestitution, uint inNumIterations)
+{
+	// Note this code is based on AxisConstraintPart, see that class for more comments on the math
+
+	ContactPoints::size_type num_points = inManifold.mRelativeContactPointsOn1.size();
+	JPH_ASSERT(num_points == inManifold.mRelativeContactPointsOn2.size());
+
+	// Start with zero impulses
+	outResult.mImpulses.resize(num_points);
+	memset(outResult.mImpulses.data(), 0, num_points * sizeof(CollisionEstimationResult::Impulse));
+
+	// Calculate friction directions
+	outResult.mTangent1 = inManifold.mWorldSpaceNormal.GetNormalizedPerpendicular();
+	outResult.mTangent2 = inManifold.mWorldSpaceNormal.Cross(outResult.mTangent1);
+
+	// Get body velocities
+	EMotionType motion_type1 = inBody1.GetMotionType();
+	const MotionProperties *motion_properties1 = inBody1.GetMotionPropertiesUnchecked();
+	if (motion_type1 != EMotionType::Static)
+	{
+		outResult.mLinearVelocity1 = motion_properties1->GetLinearVelocity();
+		outResult.mAngularVelocity1 = motion_properties1->GetAngularVelocity();
+	}
+	else
+		outResult.mLinearVelocity1 = outResult.mAngularVelocity1 = Vec3::sZero();
+
+	EMotionType motion_type2 = inBody2.GetMotionType();
+	const MotionProperties *motion_properties2 = inBody2.GetMotionPropertiesUnchecked();
+	if (motion_type2 != EMotionType::Static)
+	{
+		outResult.mLinearVelocity2 = motion_properties2->GetLinearVelocity();
+		outResult.mAngularVelocity2 = motion_properties2->GetAngularVelocity();
+	}
+	else
+		outResult.mLinearVelocity2 = outResult.mAngularVelocity2 = Vec3::sZero();
+
+	// Get inverse mass and inertia
+	float inv_m1, inv_m2;
+	Mat44 inv_i1, inv_i2;
+	if (motion_type1 == EMotionType::Dynamic)
+	{
+		inv_m1 = motion_properties1->GetInverseMass();
+		inv_i1 = inBody1.GetInverseInertia();
+	}
+	else
+	{
+		inv_m1 = 0.0f;
+		inv_i1 = Mat44::sZero();
+	}
+
+	if (motion_type2 == EMotionType::Dynamic)
+	{
+		inv_m2 = motion_properties2->GetInverseMass();
+		inv_i2 = inBody2.GetInverseInertia();
+	}
+	else
+	{
+		inv_m2 = 0.0f;
+		inv_i2 = Mat44::sZero();
+	}
+
+	// Get center of masses relative to the base offset
+	Vec3 com1 = Vec3(inBody1.GetCenterOfMassPosition() - inManifold.mBaseOffset);
+	Vec3 com2 = Vec3(inBody2.GetCenterOfMassPosition() - inManifold.mBaseOffset);
+
+	struct AxisConstraint
+	{
+		inline void		Initialize(Vec3Arg inR1, Vec3Arg inR2, Vec3Arg inWorldSpaceNormal, float inInvM1, float inInvM2, Mat44Arg inInvI1, Mat44Arg inInvI2)
+		{
+			// Calculate effective mass: K^-1 = (J M^-1 J^T)^-1
+			mR1PlusUxAxis = inR1.Cross(inWorldSpaceNormal);
+			mR2xAxis = inR2.Cross(inWorldSpaceNormal);
+			mInvI1_R1PlusUxAxis = inInvI1.Multiply3x3(mR1PlusUxAxis);
+			mInvI2_R2xAxis = inInvI2.Multiply3x3(mR2xAxis);
+			mEffectiveMass = 1.0f / (inInvM1 + mInvI1_R1PlusUxAxis.Dot(mR1PlusUxAxis) + inInvM2 + mInvI2_R2xAxis.Dot(mR2xAxis));
+			mBias = 0.0f;
+		}
+
+		inline float	SolveGetLambda(Vec3Arg inWorldSpaceNormal, const CollisionEstimationResult &inResult) const
+		{
+			// Calculate jacobian multiplied by linear/angular velocity
+			float jv = inWorldSpaceNormal.Dot(inResult.mLinearVelocity1 - inResult.mLinearVelocity2) + mR1PlusUxAxis.Dot(inResult.mAngularVelocity1) - mR2xAxis.Dot(inResult.mAngularVelocity2);
+
+			// Lagrange multiplier is:
+			//
+			// lambda = -K^-1 (J v + b)
+			return mEffectiveMass * (jv - mBias);
+		}
+
+		inline void		SolveApplyLambda(Vec3Arg inWorldSpaceNormal, float inInvM1, float inInvM2, float inLambda, CollisionEstimationResult &ioResult) const
+		{
+			// Apply impulse to body velocities
+			ioResult.mLinearVelocity1 -= (inLambda * inInvM1) * inWorldSpaceNormal;
+			ioResult.mAngularVelocity1 -= inLambda * mInvI1_R1PlusUxAxis;
+			ioResult.mLinearVelocity2 += (inLambda * inInvM2) * inWorldSpaceNormal;
+			ioResult.mAngularVelocity2 += inLambda * mInvI2_R2xAxis;
+		}
+
+		inline void		Solve(Vec3Arg inWorldSpaceNormal, float inInvM1, float inInvM2, float inMinLambda, float inMaxLambda, float &ioTotalLambda, CollisionEstimationResult &ioResult) const
+		{
+			// Calculate new total lambda
+			float total_lambda = ioTotalLambda + SolveGetLambda(inWorldSpaceNormal, ioResult);
+
+			// Clamp impulse
+			total_lambda = Clamp(total_lambda, inMinLambda, inMaxLambda);
+
+			SolveApplyLambda(inWorldSpaceNormal, inInvM1, inInvM2, total_lambda - ioTotalLambda, ioResult);
+
+			ioTotalLambda = total_lambda;
+		}
+
+		Vec3			mR1PlusUxAxis;
+		Vec3			mR2xAxis;
+		Vec3			mInvI1_R1PlusUxAxis;
+		Vec3			mInvI2_R2xAxis;
+		float			mEffectiveMass;
+		float			mBias;
+	};
+
+	struct Constraint
+	{
+		AxisConstraint	mContact;
+		AxisConstraint	mFriction1;
+		AxisConstraint	mFriction2;
+	};
+
+	// Initialize the constraint properties
+	Constraint constraints[ContactPoints::Capacity];
+	for (uint c = 0; c < num_points; ++c)
+	{
+		Constraint &constraint = constraints[c];
+
+		// Calculate contact points relative to body 1 and 2
+		Vec3 p = 0.5f * (inManifold.mRelativeContactPointsOn1[c] + inManifold.mRelativeContactPointsOn2[c]);
+		Vec3 r1 = p - com1;
+		Vec3 r2 = p - com2;
+
+		// Initialize contact constraint
+		constraint.mContact.Initialize(r1, r2, inManifold.mWorldSpaceNormal, inv_m1, inv_m2, inv_i1, inv_i2);
+
+		// Handle elastic collisions
+		if (inCombinedRestitution > 0.0f)
+		{
+			// Calculate velocity of contact point
+			Vec3 relative_velocity = outResult.mLinearVelocity2 + outResult.mAngularVelocity2.Cross(r2) - outResult.mLinearVelocity1 - outResult.mAngularVelocity1.Cross(r1);
+			float normal_velocity = relative_velocity.Dot(inManifold.mWorldSpaceNormal);
+
+			// If it is big enough, apply restitution
+			if (normal_velocity < -inMinVelocityForRestitution)
+				constraint.mContact.mBias = inCombinedRestitution * normal_velocity;
+		}
+
+		if (inCombinedFriction > 0.0f)
+		{
+			// Initialize friction constraints
+			constraint.mFriction1.Initialize(r1, r2, outResult.mTangent1, inv_m1, inv_m2, inv_i1, inv_i2);
+			constraint.mFriction2.Initialize(r1, r2, outResult.mTangent2, inv_m1, inv_m2, inv_i1, inv_i2);
+		}
+	}
+
+	// If there's only 1 contact point, we only need 1 iteration
+	int num_iterations = inCombinedFriction <= 0.0f && num_points == 1? 1 : inNumIterations;
+
+	// Solve iteratively
+	for (int iteration = 0; iteration < num_iterations; ++iteration)
+	{
+		// Solve friction constraints first
+		if (inCombinedFriction > 0.0f && iteration > 0) // For first iteration the contact impulse is zero so there's no point in applying friction
+			for (uint c = 0; c < num_points; ++c)
+			{
+				const Constraint &constraint = constraints[c];
+				CollisionEstimationResult::Impulse &impulse = outResult.mImpulses[c];
+
+				float lambda1 = impulse.mFrictionImpulse1 + constraint.mFriction1.SolveGetLambda(outResult.mTangent1, outResult);
+				float lambda2 = impulse.mFrictionImpulse2 + constraint.mFriction2.SolveGetLambda(outResult.mTangent2, outResult);
+
+				// Calculate max impulse based on contact impulse
+				float max_impulse = inCombinedFriction * impulse.mContactImpulse;
+
+				// If the total lambda that we will apply is too large, scale it back
+				float total_lambda_sq = Square(lambda1) + Square(lambda2);
+				if (total_lambda_sq > Square(max_impulse))
+				{
+					float scale = max_impulse / sqrt(total_lambda_sq);
+					lambda1 *= scale;
+					lambda2 *= scale;
+				}
+
+				constraint.mFriction1.SolveApplyLambda(outResult.mTangent1, inv_m1, inv_m2, lambda1 - impulse.mFrictionImpulse1, outResult);
+				constraint.mFriction2.SolveApplyLambda(outResult.mTangent2, inv_m1, inv_m2, lambda2 - impulse.mFrictionImpulse2, outResult);
+
+				impulse.mFrictionImpulse1 = lambda1;
+				impulse.mFrictionImpulse2 = lambda2;
+			}
+
+		// Solve contact constraints last
+		for (uint c = 0; c < num_points; ++c)
+			constraints[c].mContact.Solve(inManifold.mWorldSpaceNormal, inv_m1, inv_m2, 0.0f, FLT_MAX, outResult.mImpulses[c].mContactImpulse, outResult);
+	}
+}
+
+JPH_NAMESPACE_END

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

@@ -0,0 +1,48 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/ContactListener.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// A structure that contains the estimated contact and friction impulses and the resulting body velocities
+struct CollisionEstimationResult
+{
+	Vec3			mLinearVelocity1;				///< The estimated linear velocity of body 1 after collision
+	Vec3			mAngularVelocity1;				///< The estimated angular velocity of body 1 after collision
+	Vec3			mLinearVelocity2;				///< The estimated linear velocity of body 2 after collision
+	Vec3			mAngularVelocity2;				///< The estimated angular velocity of body 2 after collision
+
+	Vec3			mTangent1;						///< Normalized tangent of contact normal
+	Vec3			mTangent2;						///< Second normalized tangent of contact normal (forms a basis with mTangent1 and mWorldSpaceNormal)
+
+	struct Impulse
+	{
+		float		mContactImpulse;				///< Estimated contact impulses (kg m / s)
+		float		mFrictionImpulse1;				///< Estimated friction impulses in the direction of tangent 1 (kg m / s)
+		float		mFrictionImpulse2;				///< Estimated friction impulses in the direction of tangent 2 (kg m / s)
+	};
+
+	using Impulses = StaticArray<Impulse, ContactPoints::Capacity>;
+
+	Impulses		mImpulses;
+};
+
+/// This function estimates the contact impulses and body velocity changes as a result of a collision.
+/// It can be used in the ContactListener::OnContactAdded to determine the strength of the collision to e.g. play a sound or trigger a particle system.
+/// This function is accurate when two bodies collide but will not be accurate when more than 2 bodies collide at the same time as it does not know about these other collisions.
+///
+/// @param inBody1 Colliding body 1
+/// @param inBody2 Colliding body 2
+/// @param inManifold The collision manifold
+/// @param outResult A structure that contains the estimated contact and friction impulses and the resulting body velocities
+/// @param inCombinedFriction The combined friction of body 1 and body 2 (see ContactSettings::mCombinedFriction)
+/// @param inCombinedRestitution The combined restitution of body 1 and body 2 (see ContactSettings::mCombinedRestitution)
+/// @param inMinVelocityForRestitution Minimal velocity required for restitution to be applied (see PhysicsSettings::mMinVelocityForRestitution)
+/// @param inNumIterations Number of iterations to use for the impulse estimation (see PhysicsSettings::mNumVelocitySteps, note you can probably use a lower number for a decent estimate). If you set the number of iterations to 1 then no friction will be calculated.
+JPH_EXPORT void EstimateCollisionResponse(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, CollisionEstimationResult &outResult, float inCombinedFriction, float inCombinedRestitution, float inMinVelocityForRestitution = 1.0f, uint inNumIterations = 10);
+
+JPH_NAMESPACE_END

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

@@ -0,0 +1,32 @@
+// 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/GroupFilter.h>
+#include <Jolt/Core/StreamUtils.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT_BASE(GroupFilter)
+{
+	JPH_ADD_BASE_CLASS(GroupFilter, SerializableObject)
+}
+
+void GroupFilter::SaveBinaryState(StreamOut &inStream) const
+{
+	inStream.Write(GetRTTI()->GetHash());
+}
+
+void GroupFilter::RestoreBinaryState(StreamIn &inStream)
+{
+	// RTTI hash is read in sRestoreFromBinaryState
+}
+
+GroupFilter::GroupFilterResult GroupFilter::sRestoreFromBinaryState(StreamIn &inStream)
+{
+	return StreamUtils::RestoreObject<GroupFilter>(inStream, &GroupFilter::RestoreBinaryState);
+}
+
+JPH_NAMESPACE_END

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

@@ -0,0 +1,41 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/Result.h>
+#include <Jolt/ObjectStream/SerializableObject.h>
+
+JPH_NAMESPACE_BEGIN
+
+class CollisionGroup;
+class StreamIn;
+class StreamOut;
+
+/// Abstract class that checks if two CollisionGroups collide
+class JPH_EXPORT GroupFilter : public SerializableObject, public RefTarget<GroupFilter>
+{
+	JPH_DECLARE_SERIALIZABLE_ABSTRACT(JPH_EXPORT, GroupFilter)
+
+public:
+	/// Virtual destructor
+	virtual						~GroupFilter() override = default;
+
+	/// Check if two groups collide
+	virtual bool				CanCollide(const CollisionGroup &inGroup1, const CollisionGroup &inGroup2) const = 0;
+
+	/// Saves the contents of the group filter in binary form to inStream.
+	virtual void				SaveBinaryState(StreamOut &inStream) const;
+
+	using GroupFilterResult = Result<Ref<GroupFilter>>;
+
+	/// Creates a GroupFilter of the correct type and restores its contents from the binary stream inStream.
+	static GroupFilterResult	sRestoreFromBinaryState(StreamIn &inStream);
+
+protected:
+	/// This function should not be called directly, it is used by sRestoreFromBinaryState.
+	virtual void				RestoreBinaryState(StreamIn &inStream);
+};
+
+JPH_NAMESPACE_END

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

@@ -0,0 +1,38 @@
+// 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/GroupFilterTable.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(GroupFilterTable)
+{
+	JPH_ADD_BASE_CLASS(GroupFilterTable, GroupFilter)
+
+	JPH_ADD_ATTRIBUTE(GroupFilterTable, mNumSubGroups)
+	JPH_ADD_ATTRIBUTE(GroupFilterTable, mTable)
+}
+
+void GroupFilterTable::SaveBinaryState(StreamOut &inStream) const
+{
+	GroupFilter::SaveBinaryState(inStream);
+
+	inStream.Write(mNumSubGroups);
+	inStream.Write(mTable);
+}
+
+void GroupFilterTable::RestoreBinaryState(StreamIn &inStream)
+{
+	GroupFilter::RestoreBinaryState(inStream);
+
+	inStream.Read(mNumSubGroups);
+	inStream.Read(mTable);
+}
+
+JPH_NAMESPACE_END

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

@@ -0,0 +1,130 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/GroupFilter.h>
+#include <Jolt/Physics/Collision/CollisionGroup.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Implementation of GroupFilter that stores a bit table with one bit per sub shape ID pair to determine if they collide or not
+///
+/// The collision rules:
+/// - If one of the objects is in the cInvalidGroup the objects will collide.
+/// - If the objects are in different groups they will collide.
+/// - If they're in the same group but their collision filter is different they will not collide.
+/// - If they're in the same group and their collision filters match, we'll use the SubGroupID and the table below.
+///
+/// For N = 6 sub groups the table will look like:
+///
+///		            sub group 1 --->
+///		sub group 2 x.....
+///		     |      ox....
+///		     |      oox...
+///		     V      ooox..
+///		            oooox.
+///		            ooooox
+///
+/// * 'x' means sub group 1 == sub group 2 and we define this to never collide.
+/// * 'o' is a bit that we have to store that defines if the sub groups collide or not.
+/// * '.' is a bit we don't need to store because the table is symmetric, we take care that group 2 > group 1 by swapping sub group 1 and sub group 2 if needed.
+///
+/// The total number of bits we need to store is (N * (N - 1)) / 2
+class JPH_EXPORT GroupFilterTable final : public GroupFilter
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, GroupFilterTable)
+
+private:
+	using GroupID = CollisionGroup::GroupID;
+	using SubGroupID = CollisionGroup::SubGroupID;
+
+	/// Get which bit corresponds to the pair (inSubGroup1, inSubGroup2)
+	int						GetBit(SubGroupID inSubGroup1, SubGroupID inSubGroup2) const
+	{
+		JPH_ASSERT(inSubGroup1 != inSubGroup2);
+
+		// We store the lower left half only, so swap the inputs when trying to access the top right half
+		if (inSubGroup1 > inSubGroup2)
+			std::swap(inSubGroup1, inSubGroup2);
+
+		JPH_ASSERT(inSubGroup2 < mNumSubGroups);
+
+		// Calculate at which bit the entry for this pair resides
+		// We use the fact that a row always starts at inSubGroup2 * (inSubGroup2 - 1) / 2
+		// (this is the amount of bits needed to store a table of inSubGroup2 entries)
+		return (inSubGroup2 * (inSubGroup2 - 1)) / 2 + inSubGroup1;
+	}
+
+public:
+	/// Constructs the table with inNumSubGroups subgroups, initially all collision pairs are enabled except when the sub group ID is the same
+	explicit				GroupFilterTable(uint inNumSubGroups = 0) :
+		mNumSubGroups(inNumSubGroups)
+	{
+		// By default everything collides
+		int table_size = ((inNumSubGroups * (inNumSubGroups - 1)) / 2 + 7) / 8;
+		mTable.resize(table_size, 0xff);
+	}
+
+	/// Copy constructor
+							GroupFilterTable(const GroupFilterTable &inRHS) : mNumSubGroups(inRHS.mNumSubGroups), mTable(inRHS.mTable) { }
+
+	/// Disable collision between two sub groups
+	void					DisableCollision(SubGroupID inSubGroup1, SubGroupID inSubGroup2)
+	{
+		int bit = GetBit(inSubGroup1, inSubGroup2);
+		mTable[bit >> 3] &= (0xff ^ (1 << (bit & 0b111)));
+	}
+
+	/// Enable collision between two sub groups
+	void					EnableCollision(SubGroupID inSubGroup1, SubGroupID inSubGroup2)
+	{
+		int bit = GetBit(inSubGroup1, inSubGroup2);
+		mTable[bit >> 3] |= 1 << (bit & 0b111);
+	}
+
+	/// Check if the collision between two subgroups is enabled
+	inline bool				IsCollisionEnabled(SubGroupID inSubGroup1, SubGroupID inSubGroup2) const
+	{
+		// Test if the bit is set for this group pair
+		int bit = GetBit(inSubGroup1, inSubGroup2);
+		return (mTable[bit >> 3] & (1 << (bit & 0b111))) != 0;
+	}
+
+	/// Checks if two CollisionGroups collide
+	virtual bool			CanCollide(const CollisionGroup &inGroup1, const CollisionGroup &inGroup2) const override
+	{
+		// If one of the groups is cInvalidGroup the objects will collide (note that the if following this if will ensure that group2 is not cInvalidGroup)
+		if (inGroup1.GetGroupID() == CollisionGroup::cInvalidGroup)
+			return true;
+
+		// If the objects are in different groups, they collide
+		if (inGroup1.GetGroupID() != inGroup2.GetGroupID())
+			return true;
+
+		// If the collision filters do not match, but they're in the same group we ignore the collision
+		if (inGroup1.GetGroupFilter() != inGroup2.GetGroupFilter())
+			return false;
+
+		// If they are in the same sub group, they don't collide
+		if (inGroup1.GetSubGroupID() == inGroup2.GetSubGroupID())
+			return false;
+
+		// Check the bit table
+		return IsCollisionEnabled(inGroup1.GetSubGroupID(), inGroup2.GetSubGroupID());
+	}
+
+	// See: GroupFilter::SaveBinaryState
+	virtual void			SaveBinaryState(StreamOut &inStream) const override;
+
+protected:
+	// See: GroupFilter::RestoreBinaryState
+	virtual void			RestoreBinaryState(StreamIn &inStream) override;
+
+private:
+	uint					mNumSubGroups;									///< The number of subgroups that this group filter supports
+	Array<uint8>			mTable;											///< The table of bits that indicates which pairs collide
+};
+
+JPH_NAMESPACE_END

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

@@ -0,0 +1,261 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2024 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/QuickSort.h>
+#include <Jolt/Core/STLLocalAllocator.h>
+#include <Jolt/Physics/Collision/CollisionDispatch.h>
+
+//#define JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG
+
+#ifdef JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG
+#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG
+
+JPH_NAMESPACE_BEGIN
+
+/// Removes internal edges from collision results. Can be used to filter out 'ghost collisions'.
+/// Based on: Contact generation for meshes - Pierre Terdiman (https://www.codercorner.com/MeshContacts.pdf)
+///
+/// Note that this class requires that CollideSettingsBase::mActiveEdgeMode == EActiveEdgeMode::CollideWithAll
+/// and CollideSettingsBase::mCollectFacesMode == ECollectFacesMode::CollectFaces.
+class InternalEdgeRemovingCollector : public CollideShapeCollector
+{
+	static constexpr uint cMaxLocalDelayedResults = 32;
+	static constexpr uint cMaxLocalVoidedFeatures = 128;
+
+	/// Check if a vertex is voided
+	inline bool				IsVoided(const SubShapeID &inSubShapeID, Vec3 inV) const
+	{
+		for (const Voided &vf : mVoidedFeatures)
+			if (vf.mSubShapeID == inSubShapeID
+				&& inV.IsClose(Vec3::sLoadFloat3Unsafe(vf.mFeature), 1.0e-8f))
+				return true;
+		return false;
+	}
+
+	/// Add all vertices of a face to the voided features
+	inline void				VoidFeatures(const CollideShapeResult &inResult)
+	{
+		for (const Vec3 &v : inResult.mShape2Face)
+			if (!IsVoided(inResult.mSubShapeID1, v))
+			{
+				Voided vf;
+				v.StoreFloat3(&vf.mFeature);
+				vf.mSubShapeID = inResult.mSubShapeID1;
+				mVoidedFeatures.push_back(vf);
+			}
+	}
+
+	/// Call the chained collector
+	inline void				Chain(const CollideShapeResult &inResult)
+	{
+		// Make sure the chained collector has the same context as we do
+		mChainedCollector.SetContext(GetContext());
+
+		// Forward the hit
+		mChainedCollector.AddHit(inResult);
+
+		// If our chained collector updated its early out fraction, we need to follow
+		UpdateEarlyOutFraction(mChainedCollector.GetEarlyOutFraction());
+	}
+
+	/// Call the chained collector and void all features of inResult
+	inline void				ChainAndVoid(const CollideShapeResult &inResult)
+	{
+		Chain(inResult);
+		VoidFeatures(inResult);
+
+	#ifdef JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG
+		DebugRenderer::sInstance->DrawWirePolygon(RMat44::sIdentity(), inResult.mShape2Face, Color::sGreen);
+		DebugRenderer::sInstance->DrawArrow(RVec3(inResult.mContactPointOn2), RVec3(inResult.mContactPointOn2) + inResult.mPenetrationAxis.NormalizedOr(Vec3::sZero()), Color::sGreen, 0.1f);
+	#endif // JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG
+	}
+
+public:
+	/// Constructor, configures a collector to be called with all the results that do not hit internal edges
+	explicit				InternalEdgeRemovingCollector(CollideShapeCollector &inChainedCollector) :
+		CollideShapeCollector(inChainedCollector),
+		mChainedCollector(inChainedCollector)
+	{
+		// Initialize arrays to full capacity to avoid needless reallocation calls
+		mVoidedFeatures.reserve(cMaxLocalVoidedFeatures);
+		mDelayedResults.reserve(cMaxLocalDelayedResults);
+	}
+
+	// See: CollideShapeCollector::Reset
+	virtual void			Reset() override
+	{
+		CollideShapeCollector::Reset();
+
+		mChainedCollector.Reset();
+
+		mVoidedFeatures.clear();
+		mDelayedResults.clear();
+	}
+
+	// See: CollideShapeCollector::OnBody
+	virtual void			OnBody(const Body &inBody) override
+	{
+		// Just forward the call to our chained collector
+		mChainedCollector.OnBody(inBody);
+	}
+
+	// See: CollideShapeCollector::AddHit
+	virtual void			AddHit(const CollideShapeResult &inResult) override
+	{
+		// We only support welding when the shape is a triangle or has more vertices so that we can calculate a normal
+		if (inResult.mShape2Face.size() < 3)
+			return ChainAndVoid(inResult);
+
+		// Get the triangle normal of shape 2 face
+		Vec3 triangle_normal = (inResult.mShape2Face[1] - inResult.mShape2Face[0]).Cross(inResult.mShape2Face[2] - inResult.mShape2Face[0]);
+		float triangle_normal_len = triangle_normal.Length();
+		if (triangle_normal_len < 1e-6f)
+			return ChainAndVoid(inResult);
+
+		// If the triangle normal matches the contact normal within 1 degree, we can process the contact immediately
+		// We make the assumption here that if the contact normal and the triangle normal align that the we're dealing with a 'face contact'
+		Vec3 contact_normal = -inResult.mPenetrationAxis;
+		float contact_normal_len = inResult.mPenetrationAxis.Length();
+		if (triangle_normal.Dot(contact_normal) > 0.999848f * contact_normal_len * triangle_normal_len) // cos(1 degree)
+			return ChainAndVoid(inResult);
+
+		// Delayed processing
+		mDelayedResults.push_back(inResult);
+	}
+
+	/// After all hits have been added, call this function to process the delayed results
+	void					Flush()
+	{
+		// Sort on biggest penetration depth first
+		Array<uint, STLLocalAllocator<uint, cMaxLocalDelayedResults>> sorted_indices;
+		sorted_indices.resize(mDelayedResults.size());
+		for (uint i = 0; i < uint(mDelayedResults.size()); ++i)
+			sorted_indices[i] = i;
+		QuickSort(sorted_indices.begin(), sorted_indices.end(), [this](uint inLHS, uint inRHS) { return mDelayedResults[inLHS].mPenetrationDepth > mDelayedResults[inRHS].mPenetrationDepth; });
+
+		// Loop over all results
+		for (uint i = 0; i < uint(mDelayedResults.size()); ++i)
+		{
+			const CollideShapeResult &r = mDelayedResults[sorted_indices[i]];
+
+			// Determine which vertex or which edge is the closest to the contact point
+			float best_dist_sq = FLT_MAX;
+			uint best_v1_idx = 0;
+			uint best_v2_idx = 0;
+			uint num_v = uint(r.mShape2Face.size());
+			uint v1_idx = num_v - 1;
+			Vec3 v1 = r.mShape2Face[v1_idx] - r.mContactPointOn2;
+			for (uint v2_idx = 0; v2_idx < num_v; ++v2_idx)
+			{
+				Vec3 v2 = r.mShape2Face[v2_idx] - r.mContactPointOn2;
+				Vec3 v1_v2 = v2 - v1;
+				float denominator = v1_v2.LengthSq();
+				if (denominator < Square(FLT_EPSILON))
+				{
+					// Degenerate, assume v1 is closest, v2 will be tested in a later iteration
+					float v1_len_sq = v1.LengthSq();
+					if (v1_len_sq < best_dist_sq)
+					{
+						best_dist_sq = v1_len_sq;
+						best_v1_idx = v1_idx;
+						best_v2_idx = v1_idx;
+					}
+				}
+				else
+				{
+					// Taken from ClosestPoint::GetBaryCentricCoordinates
+					float fraction = -v1.Dot(v1_v2) / denominator;
+					if (fraction < 1.0e-6f)
+					{
+						// Closest lies on v1
+						float v1_len_sq = v1.LengthSq();
+						if (v1_len_sq < best_dist_sq)
+						{
+							best_dist_sq = v1_len_sq;
+							best_v1_idx = v1_idx;
+							best_v2_idx = v1_idx;
+						}
+					}
+					else if (fraction < 1.0f - 1.0e-6f)
+					{
+						// Closest lies on the line segment v1, v2
+						Vec3 closest = v1 + fraction * v1_v2;
+						float closest_len_sq = closest.LengthSq();
+						if (closest_len_sq < best_dist_sq)
+						{
+							best_dist_sq = closest_len_sq;
+							best_v1_idx = v1_idx;
+							best_v2_idx = v2_idx;
+						}
+					}
+					// else closest is v2, but v2 will be tested in a later iteration
+				}
+
+				v1_idx = v2_idx;
+				v1 = v2;
+			}
+
+			// Check if this vertex/edge is voided
+			bool voided = IsVoided(r.mSubShapeID1, r.mShape2Face[best_v1_idx])
+				&& (best_v1_idx == best_v2_idx || IsVoided(r.mSubShapeID1, r.mShape2Face[best_v2_idx]));
+
+		#ifdef JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG
+			Color color = voided? Color::sRed : Color::sYellow;
+			DebugRenderer::sInstance->DrawText3D(RVec3(r.mContactPointOn2), StringFormat("%d: %g", i, r.mPenetrationDepth), color, 0.1f);
+			DebugRenderer::sInstance->DrawWirePolygon(RMat44::sIdentity(), r.mShape2Face, color);
+			DebugRenderer::sInstance->DrawArrow(RVec3(r.mContactPointOn2), RVec3(r.mContactPointOn2) + r.mPenetrationAxis.NormalizedOr(Vec3::sZero()), color, 0.1f);
+			DebugRenderer::sInstance->DrawMarker(RVec3(r.mShape2Face[best_v1_idx]), IsVoided(r.mSubShapeID1, r.mShape2Face[best_v1_idx])? Color::sRed : Color::sYellow, 0.1f);
+			DebugRenderer::sInstance->DrawMarker(RVec3(r.mShape2Face[best_v2_idx]), IsVoided(r.mSubShapeID1, r.mShape2Face[best_v2_idx])? Color::sRed : Color::sYellow, 0.1f);
+		#endif // JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG
+
+			// No voided features, accept the contact
+			if (!voided)
+				Chain(r);
+
+			// Void the features of this face
+			VoidFeatures(r);
+		}
+
+		// All delayed results have been processed
+		mVoidedFeatures.clear();
+		mDelayedResults.clear();
+	}
+
+	// See: CollideShapeCollector::OnBodyEnd
+	virtual void			OnBodyEnd() override
+	{
+		Flush();
+		mChainedCollector.OnBodyEnd();
+	}
+
+	/// Version of CollisionDispatch::sCollideShapeVsShape that removes internal edges
+	static void				sCollideShapeVsShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter = { })
+	{
+		JPH_ASSERT(inCollideShapeSettings.mActiveEdgeMode == EActiveEdgeMode::CollideWithAll); // Won't work without colliding with all edges
+		JPH_ASSERT(inCollideShapeSettings.mCollectFacesMode == ECollectFacesMode::CollectFaces); // Won't work without collecting faces
+
+		InternalEdgeRemovingCollector wrapper(ioCollector);
+		CollisionDispatch::sCollideShapeVsShape(inShape1, inShape2, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, wrapper, inShapeFilter);
+		wrapper.Flush();
+	}
+
+private:
+	// This algorithm tests a convex shape (shape 1) against a set of polygons (shape 2).
+	// This assumption doesn't hold if the shape we're testing is a compound shape, so we must also
+	// store the sub shape ID and ignore voided features that belong to another sub shape ID.
+	struct Voided
+	{
+		Float3				mFeature;				// Feature that is voided (of shape 2). Read with Vec3::sLoadFloat3Unsafe so must not be the last member.
+		SubShapeID			mSubShapeID;			// Sub shape ID of the shape that is colliding against the feature (of shape 1).
+	};
+
+	CollideShapeCollector &	mChainedCollector;
+	Array<Voided, STLLocalAllocator<Voided, cMaxLocalVoidedFeatures>> mVoidedFeatures;
+	Array<CollideShapeResult, STLLocalAllocator<CollideShapeResult, cMaxLocalDelayedResults>> mDelayedResults;
+};
+
+JPH_NAMESPACE_END

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

@@ -0,0 +1,248 @@
+// 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/ManifoldBetweenTwoFaces.h>
+#include <Jolt/Physics/Constraints/ContactConstraintManager.h>
+#include <Jolt/Geometry/ClipPoly.h>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+void PruneContactPoints(Vec3Arg inPenetrationAxis, ContactPoints &ioContactPointsOn1, ContactPoints &ioContactPointsOn2 JPH_IF_DEBUG_RENDERER(, RVec3Arg inCenterOfMass))
+{
+	// Makes no sense to call this with 4 or less points
+	JPH_ASSERT(ioContactPointsOn1.size() > 4);
+
+	// Both arrays should have the same size
+	JPH_ASSERT(ioContactPointsOn1.size() == ioContactPointsOn2.size());
+
+	// Penetration axis must be normalized
+	JPH_ASSERT(inPenetrationAxis.IsNormalized());
+
+	// We use a heuristic of (distance to center of mass) * (penetration depth) to find the contact point that we should keep
+	// Neither of those two terms should ever become zero, so we clamp against this minimum value
+	constexpr float cMinDistanceSq = 1.0e-6f; // 1 mm
+
+	ContactPoints projected;
+	StaticArray<float, 64> penetration_depth_sq;
+	for (ContactPoints::size_type i = 0; i < ioContactPointsOn1.size(); ++i)
+	{
+		// Project contact points on the plane through inCenterOfMass with normal inPenetrationAxis and center around the center of mass of body 1
+		// (note that since all points are relative to inCenterOfMass we can project onto the plane through the origin)
+		Vec3 v1 = ioContactPointsOn1[i];
+		projected.push_back(v1 - v1.Dot(inPenetrationAxis) * inPenetrationAxis);
+
+		// Calculate penetration depth^2 of each point and clamp against the minimal distance
+		Vec3 v2 = ioContactPointsOn2[i];
+		penetration_depth_sq.push_back(max(cMinDistanceSq, (v2 - v1).LengthSq()));
+	}
+
+	// Find the point that is furthest away from the center of mass (its torque will have the biggest influence)
+	// and the point that has the deepest penetration depth. Use the heuristic (distance to center of mass) * (penetration depth) for this.
+	uint point1 = 0;
+	float val = max(cMinDistanceSq, projected[0].LengthSq()) * penetration_depth_sq[0];
+	for (uint i = 0; i < projected.size(); ++i)
+	{
+		float v = max(cMinDistanceSq, projected[i].LengthSq()) * penetration_depth_sq[i];
+		if (v > val)
+		{
+			val = v;
+			point1 = i;
+		}
+	}
+	Vec3 point1v = projected[point1];
+
+	// Find point furthest from the first point forming a line segment with point1. Again combine this with the heuristic
+	// for deepest point as per above.
+	uint point2 = uint(-1);
+	val = -FLT_MAX;
+	for (uint i = 0; i < projected.size(); ++i)
+		if (i != point1)
+		{
+			float v = max(cMinDistanceSq, (projected[i] - point1v).LengthSq()) * penetration_depth_sq[i];
+			if (v > val)
+			{
+				val = v;
+				point2 = i;
+			}
+		}
+	JPH_ASSERT(point2 != uint(-1));
+	Vec3 point2v = projected[point2];
+
+	// Find furthest points on both sides of the line segment in order to maximize the area
+	uint point3 = uint(-1);
+	uint point4 = uint(-1);
+	float min_val = 0.0f;
+	float max_val = 0.0f;
+	Vec3 perp = (point2v - point1v).Cross(inPenetrationAxis);
+	for (uint i = 0; i < projected.size(); ++i)
+		if (i != point1 && i != point2)
+		{
+			float v = perp.Dot(projected[i] - point1v);
+			if (v < min_val)
+			{
+				min_val = v;
+				point3 = i;
+			}
+			else if (v > max_val)
+			{
+				max_val = v;
+				point4 = i;
+			}
+		}
+
+	// Add points to array (in order so they form a polygon)
+	StaticArray<Vec3, 4> points_to_keep_on_1, points_to_keep_on_2;
+	points_to_keep_on_1.push_back(ioContactPointsOn1[point1]);
+	points_to_keep_on_2.push_back(ioContactPointsOn2[point1]);
+	if (point3 != uint(-1))
+	{
+		points_to_keep_on_1.push_back(ioContactPointsOn1[point3]);
+		points_to_keep_on_2.push_back(ioContactPointsOn2[point3]);
+	}
+	points_to_keep_on_1.push_back(ioContactPointsOn1[point2]);
+	points_to_keep_on_2.push_back(ioContactPointsOn2[point2]);
+	if (point4 != uint(-1))
+	{
+		JPH_ASSERT(point3 != point4);
+		points_to_keep_on_1.push_back(ioContactPointsOn1[point4]);
+		points_to_keep_on_2.push_back(ioContactPointsOn2[point4]);
+	}
+
+#ifdef JPH_DEBUG_RENDERER
+	if (ContactConstraintManager::sDrawContactPointReduction)
+	{
+		// Draw input polygon
+		DebugRenderer::sInstance->DrawWirePolygon(RMat44::sTranslation(inCenterOfMass), ioContactPointsOn1, Color::sOrange, 0.05f);
+
+		// Draw primary axis
+		DebugRenderer::sInstance->DrawArrow(inCenterOfMass + ioContactPointsOn1[point1], inCenterOfMass + ioContactPointsOn1[point2], Color::sRed, 0.05f);
+
+		// Draw contact points we kept
+		for (Vec3 p : points_to_keep_on_1)
+			DebugRenderer::sInstance->DrawMarker(inCenterOfMass + p, Color::sGreen, 0.1f);
+	}
+#endif // JPH_DEBUG_RENDERER
+
+	// Copy the points back to the input buffer
+	ioContactPointsOn1 = points_to_keep_on_1;
+	ioContactPointsOn2 = points_to_keep_on_2;
+}
+
+void ManifoldBetweenTwoFaces(Vec3Arg inContactPoint1, Vec3Arg inContactPoint2, Vec3Arg inPenetrationAxis, float inMaxContactDistance, const ConvexShape::SupportingFace &inShape1Face, const ConvexShape::SupportingFace &inShape2Face, ContactPoints &outContactPoints1, ContactPoints &outContactPoints2 JPH_IF_DEBUG_RENDERER(, RVec3Arg inCenterOfMass))
+{
+	JPH_ASSERT(inMaxContactDistance > 0.0f);
+
+#ifdef JPH_DEBUG_RENDERER
+	if (ContactConstraintManager::sDrawContactPoint)
+	{
+		RVec3 cp1 = inCenterOfMass + inContactPoint1;
+		RVec3 cp2 = inCenterOfMass + inContactPoint2;
+
+		// Draw contact points
+		DebugRenderer::sInstance->DrawMarker(cp1, Color::sRed, 0.1f);
+		DebugRenderer::sInstance->DrawMarker(cp2, Color::sGreen, 0.1f);
+
+		// Draw contact normal
+		DebugRenderer::sInstance->DrawArrow(cp1, cp1 + inPenetrationAxis.Normalized(), Color::sRed, 0.05f);
+	}
+#endif // JPH_DEBUG_RENDERER
+
+	// Remember size before adding new points, to check at the end if we added some
+	ContactPoints::size_type old_size = outContactPoints1.size();
+
+	// Check if both shapes have polygon faces
+	if (inShape1Face.size() >= 2 // The dynamic shape needs to have at least 2 points or else there can never be more than 1 contact point
+		&& inShape2Face.size() >= 3) // The dynamic/static shape needs to have at least 3 points (in the case that it has 2 points only if the edges match exactly you can have 2 contact points, but this situation is unstable anyhow)
+	{
+		// Clip the polygon of face 2 against that of 1
+		ConvexShape::SupportingFace clipped_face;
+		if (inShape1Face.size() >= 3)
+			ClipPolyVsPoly(inShape2Face, inShape1Face, inPenetrationAxis, clipped_face);
+		else if (inShape1Face.size() == 2)
+			ClipPolyVsEdge(inShape2Face, inShape1Face[0], inShape1Face[1], inPenetrationAxis, clipped_face);
+
+		// Determine plane origin and normal for shape 1
+		Vec3 plane_origin = inShape1Face[0];
+		Vec3 plane_normal;
+		Vec3 first_edge = inShape1Face[1] - plane_origin;
+		if (inShape1Face.size() >= 3)
+		{
+			// Three vertices, can just calculate the normal
+			plane_normal = first_edge.Cross(inShape1Face[2] - plane_origin);
+		}
+		else
+		{
+			// Two vertices, first find a perpendicular to the edge and penetration axis and then use the perpendicular together with the edge to form a normal
+			plane_normal = first_edge.Cross(inPenetrationAxis).Cross(first_edge);
+		}
+
+		// If penetration axis and plane normal are perpendicular, fall back to the contact points
+		float penetration_axis_dot_plane_normal = inPenetrationAxis.Dot(plane_normal);
+		if (penetration_axis_dot_plane_normal != 0.0f)
+		{
+			float penetration_axis_len = inPenetrationAxis.Length();
+
+			for (Vec3 p2 : clipped_face)
+			{
+				// Project clipped face back onto the plane of face 1, we do this by solving:
+				// p1 = p2 + distance * penetration_axis / |penetration_axis|
+				// (p1 - plane_origin) . plane_normal = 0
+				// This gives us:
+				// distance = -|penetration_axis| * (p2 - plane_origin) . plane_normal / penetration_axis . plane_normal
+				float distance = (p2 - plane_origin).Dot(plane_normal) / penetration_axis_dot_plane_normal; // note left out -|penetration_axis| term
+
+				// If the point is less than inMaxContactDistance in front of the plane of face 2, add it as a contact point
+				if (distance * penetration_axis_len < inMaxContactDistance)
+				{
+					Vec3 p1 = p2 - distance * inPenetrationAxis;
+					outContactPoints1.push_back(p1);
+					outContactPoints2.push_back(p2);
+				}
+			}
+		}
+
+	#ifdef JPH_DEBUG_RENDERER
+		if (ContactConstraintManager::sDrawSupportingFaces)
+		{
+			RMat44 com = RMat44::sTranslation(inCenterOfMass);
+
+			// Draw clipped poly
+			DebugRenderer::sInstance->DrawWirePolygon(com, clipped_face, Color::sOrange);
+
+			// Draw supporting faces
+			DebugRenderer::sInstance->DrawWirePolygon(com, inShape1Face, Color::sRed, 0.05f);
+			DebugRenderer::sInstance->DrawWirePolygon(com, inShape2Face, Color::sGreen, 0.05f);
+
+			// Draw normal
+			float plane_normal_len = plane_normal.Length();
+			if (plane_normal_len > 0.0f)
+			{
+				RVec3 plane_origin_ws = inCenterOfMass + plane_origin;
+				DebugRenderer::sInstance->DrawArrow(plane_origin_ws, plane_origin_ws + plane_normal / plane_normal_len, Color::sYellow, 0.05f);
+			}
+
+			// Draw contact points that remain after distance check
+			for (ContactPoints::size_type p = old_size; p < outContactPoints1.size(); ++p)
+			{
+				DebugRenderer::sInstance->DrawMarker(inCenterOfMass + outContactPoints1[p], Color::sYellow, 0.1f);
+				DebugRenderer::sInstance->DrawMarker(inCenterOfMass + outContactPoints2[p], Color::sOrange, 0.1f);
+			}
+		}
+	#endif // JPH_DEBUG_RENDERER
+	}
+
+	// If the clipping result is empty, use the contact point itself
+	if (outContactPoints1.size() == old_size)
+	{
+		outContactPoints1.push_back(inContactPoint1);
+		outContactPoints2.push_back(inContactPoint2);
+	}
+}
+
+JPH_NAMESPACE_END

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

@@ -0,0 +1,44 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/Shape/ConvexShape.h>
+#include <Jolt/Physics/Collision/ContactListener.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Remove contact points if there are > 4 (no more than 4 are needed for a stable solution)
+/// @param inPenetrationAxis is the world space penetration axis (must be normalized)
+/// @param ioContactPointsOn1 The contact points on shape 1 relative to inCenterOfMass
+/// @param ioContactPointsOn2 The contact points on shape 2 relative to inCenterOfMass
+/// On output ioContactPointsOn1/2 are reduced to 4 or less points
+#ifdef JPH_DEBUG_RENDERER
+/// @param inCenterOfMass Center of mass position of body 1
+#endif
+JPH_EXPORT void PruneContactPoints(Vec3Arg inPenetrationAxis, ContactPoints &ioContactPointsOn1, ContactPoints &ioContactPointsOn2
+#ifdef JPH_DEBUG_RENDERER
+	, RVec3Arg inCenterOfMass
+#endif
+	);
+
+/// Determine contact points between 2 faces of 2 shapes and return them in outContactPoints 1 & 2
+/// @param inContactPoint1 The contact point on shape 1 relative to inCenterOfMass
+/// @param inContactPoint2 The contact point on shape 2 relative to inCenterOfMass
+/// @param inPenetrationAxis The local space penetration axis in world space
+/// @param inMaxContactDistance After face 2 is clipped against face 1, each remaining point on face 2 is tested against the plane of face 1. If the distance on the positive side of the plane is larger than this distance, the point will be discarded as a contact point.
+/// @param inShape1Face The supporting faces on shape 1 relative to inCenterOfMass
+/// @param inShape2Face The supporting faces on shape 2 relative to inCenterOfMass
+/// @param outContactPoints1 Returns the contact points between the two shapes for shape 1 relative to inCenterOfMass (any existing points in the output array are left as is)
+/// @param outContactPoints2 Returns the contact points between the two shapes for shape 2 relative to inCenterOfMass (any existing points in the output array are left as is)
+#ifdef JPH_DEBUG_RENDERER
+/// @param inCenterOfMass Center of mass position of body 1
+#endif
+JPH_EXPORT void ManifoldBetweenTwoFaces(Vec3Arg inContactPoint1, Vec3Arg inContactPoint2, Vec3Arg inPenetrationAxis, float inMaxContactDistance, const ConvexShape::SupportingFace &inShape1Face, const ConvexShape::SupportingFace &inShape2Face, ContactPoints &outContactPoints1, ContactPoints &outContactPoints2
+#ifdef JPH_DEBUG_RENDERER
+	, RVec3Arg inCenterOfMass
+#endif
+	);
+
+JPH_NAMESPACE_END

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

@@ -0,0 +1,444 @@
+// 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/NarrowPhaseQuery.h>
+#include <Jolt/Physics/Collision/CollisionDispatch.h>
+#include <Jolt/Physics/Collision/RayCast.h>
+#include <Jolt/Physics/Collision/AABoxCast.h>
+#include <Jolt/Physics/Collision/ShapeCast.h>
+#include <Jolt/Physics/Collision/CollideShape.h>
+#include <Jolt/Physics/Collision/CollisionCollectorImpl.h>
+#include <Jolt/Physics/Collision/CastResult.h>
+#include <Jolt/Physics/Collision/InternalEdgeRemovingCollector.h>
+
+JPH_NAMESPACE_BEGIN
+
+bool NarrowPhaseQuery::CastRay(const RRayCast &inRay, RayCastResult &ioHit, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	class MyCollector : public RayCastBodyCollector
+	{
+	public:
+							MyCollector(const RRayCast &inRay, RayCastResult &ioHit, const BodyLockInterface &inBodyLockInterface, const BodyFilter &inBodyFilter) :
+			mRay(inRay),
+			mHit(ioHit),
+			mBodyLockInterface(inBodyLockInterface),
+			mBodyFilter(inBodyFilter)
+		{
+			ResetEarlyOutFraction(ioHit.mFraction);
+		}
+
+		virtual void		AddHit(const ResultType &inResult) override
+		{
+			JPH_ASSERT(inResult.mFraction < mHit.mFraction, "This hit should not have been passed on to the collector");
+
+			// Only test shape if it passes the body filter
+			if (mBodyFilter.ShouldCollide(inResult.mBodyID))
+			{
+				// Lock the body
+				BodyLockRead lock(mBodyLockInterface, inResult.mBodyID);
+				if (lock.SucceededAndIsInBroadPhase()) // Race condition: body could have been removed since it has been found in the broadphase, ensures body is in the broadphase while we call the callbacks
+				{
+					const Body &body = lock.GetBody();
+
+					// Check body filter again now that we've locked the body
+					if (mBodyFilter.ShouldCollideLocked(body))
+					{
+						// Collect the transformed shape
+						TransformedShape ts = body.GetTransformedShape();
+
+						// Release the lock now, we have all the info we need in the transformed shape
+						lock.ReleaseLock();
+
+						// Do narrow phase collision check
+						if (ts.CastRay(mRay, mHit))
+						{
+							// Test that we didn't find a further hit by accident
+							JPH_ASSERT(mHit.mFraction >= 0.0f && mHit.mFraction < GetEarlyOutFraction());
+
+							// Update early out fraction based on narrow phase collector
+							UpdateEarlyOutFraction(mHit.mFraction);
+						}
+					}
+				}
+			}
+		}
+
+		RRayCast					mRay;
+		RayCastResult &				mHit;
+		const BodyLockInterface &	mBodyLockInterface;
+		const BodyFilter &			mBodyFilter;
+	};
+
+	// Do broadphase test, note that the broadphase uses floats so we drop precision here
+	MyCollector collector(inRay, ioHit, *mBodyLockInterface, inBodyFilter);
+	mBroadPhaseQuery->CastRay(RayCast(inRay), collector, inBroadPhaseLayerFilter, inObjectLayerFilter);
+	return ioHit.mFraction <= 1.0f;
+}
+
+void NarrowPhaseQuery::CastRay(const RRayCast &inRay, const RayCastSettings &inRayCastSettings, CastRayCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	class MyCollector : public RayCastBodyCollector
+	{
+	public:
+							MyCollector(const RRayCast &inRay, const RayCastSettings &inRayCastSettings, CastRayCollector &ioCollector, const BodyLockInterface &inBodyLockInterface, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) :
+			RayCastBodyCollector(ioCollector),
+			mRay(inRay),
+			mRayCastSettings(inRayCastSettings),
+			mCollector(ioCollector),
+			mBodyLockInterface(inBodyLockInterface),
+			mBodyFilter(inBodyFilter),
+			mShapeFilter(inShapeFilter)
+		{
+		}
+
+		virtual void		AddHit(const ResultType &inResult) override
+		{
+			JPH_ASSERT(inResult.mFraction < mCollector.GetEarlyOutFraction(), "This hit should not have been passed on to the collector");
+
+			// Only test shape if it passes the body filter
+			if (mBodyFilter.ShouldCollide(inResult.mBodyID))
+			{
+				// Lock the body
+				BodyLockRead lock(mBodyLockInterface, inResult.mBodyID);
+				if (lock.SucceededAndIsInBroadPhase()) // Race condition: body could have been removed since it has been found in the broadphase, ensures body is in the broadphase while we call the callbacks
+				{
+					const Body &body = lock.GetBody();
+
+					// Check body filter again now that we've locked the body
+					if (mBodyFilter.ShouldCollideLocked(body))
+					{
+						// Collect the transformed shape
+						TransformedShape ts = body.GetTransformedShape();
+
+						// Notify collector of new body
+						mCollector.OnBody(body);
+
+						// Release the lock now, we have all the info we need in the transformed shape
+						lock.ReleaseLock();
+
+						// Do narrow phase collision check
+						ts.CastRay(mRay, mRayCastSettings, mCollector, mShapeFilter);
+
+						// Notify collector of the end of this body
+						// We do this before updating the early out fraction so that the collector can still modify it
+						mCollector.OnBodyEnd();
+
+						// Update early out fraction based on narrow phase collector
+						UpdateEarlyOutFraction(mCollector.GetEarlyOutFraction());
+					}
+				}
+			}
+		}
+
+		RRayCast					mRay;
+		RayCastSettings				mRayCastSettings;
+		CastRayCollector &			mCollector;
+		const BodyLockInterface &	mBodyLockInterface;
+		const BodyFilter &			mBodyFilter;
+		const ShapeFilter &			mShapeFilter;
+	};
+
+	// Do broadphase test, note that the broadphase uses floats so we drop precision here
+	MyCollector collector(inRay, inRayCastSettings, ioCollector, *mBodyLockInterface, inBodyFilter, inShapeFilter);
+	mBroadPhaseQuery->CastRay(RayCast(inRay), collector, inBroadPhaseLayerFilter, inObjectLayerFilter);
+}
+
+void NarrowPhaseQuery::CollidePoint(RVec3Arg inPoint, CollidePointCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	class MyCollector : public CollideShapeBodyCollector
+	{
+	public:
+							MyCollector(RVec3Arg inPoint, CollidePointCollector &ioCollector, const BodyLockInterface &inBodyLockInterface, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) :
+			CollideShapeBodyCollector(ioCollector),
+			mPoint(inPoint),
+			mCollector(ioCollector),
+			mBodyLockInterface(inBodyLockInterface),
+			mBodyFilter(inBodyFilter),
+			mShapeFilter(inShapeFilter)
+		{
+		}
+
+		virtual void		AddHit(const ResultType &inResult) override
+		{
+			// Only test shape if it passes the body filter
+			if (mBodyFilter.ShouldCollide(inResult))
+			{
+				// Lock the body
+				BodyLockRead lock(mBodyLockInterface, inResult);
+				if (lock.SucceededAndIsInBroadPhase()) // Race condition: body could have been removed since it has been found in the broadphase, ensures body is in the broadphase while we call the callbacks
+				{
+					const Body &body = lock.GetBody();
+
+					// Check body filter again now that we've locked the body
+					if (mBodyFilter.ShouldCollideLocked(body))
+					{
+						// Collect the transformed shape
+						TransformedShape ts = body.GetTransformedShape();
+
+						// Notify collector of new body
+						mCollector.OnBody(body);
+
+						// Release the lock now, we have all the info we need in the transformed shape
+						lock.ReleaseLock();
+
+						// Do narrow phase collision check
+						ts.CollidePoint(mPoint, mCollector, mShapeFilter);
+
+						// Notify collector of the end of this body
+						// We do this before updating the early out fraction so that the collector can still modify it
+						mCollector.OnBodyEnd();
+
+						// Update early out fraction based on narrow phase collector
+						UpdateEarlyOutFraction(mCollector.GetEarlyOutFraction());
+					}
+				}
+			}
+		}
+
+		RVec3							mPoint;
+		CollidePointCollector &			mCollector;
+		const BodyLockInterface &		mBodyLockInterface;
+		const BodyFilter &				mBodyFilter;
+		const ShapeFilter &				mShapeFilter;
+	};
+
+	// Do broadphase test (note: truncates double to single precision since the broadphase uses single precision)
+	MyCollector collector(inPoint, ioCollector, *mBodyLockInterface, inBodyFilter, inShapeFilter);
+	mBroadPhaseQuery->CollidePoint(Vec3(inPoint), collector, inBroadPhaseLayerFilter, inObjectLayerFilter);
+}
+
+void NarrowPhaseQuery::CollideShape(const Shape *inShape, Vec3Arg inShapeScale, RMat44Arg inCenterOfMassTransform, const CollideShapeSettings &inCollideShapeSettings, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	class MyCollector : public CollideShapeBodyCollector
+	{
+	public:
+							MyCollector(const Shape *inShape, Vec3Arg inShapeScale, RMat44Arg inCenterOfMassTransform, const CollideShapeSettings &inCollideShapeSettings, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, const BodyLockInterface &inBodyLockInterface, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) :
+			CollideShapeBodyCollector(ioCollector),
+			mShape(inShape),
+			mShapeScale(inShapeScale),
+			mCenterOfMassTransform(inCenterOfMassTransform),
+			mCollideShapeSettings(inCollideShapeSettings),
+			mBaseOffset(inBaseOffset),
+			mCollector(ioCollector),
+			mBodyLockInterface(inBodyLockInterface),
+			mBodyFilter(inBodyFilter),
+			mShapeFilter(inShapeFilter)
+		{
+		}
+
+		virtual void		AddHit(const ResultType &inResult) override
+		{
+			// Only test shape if it passes the body filter
+			if (mBodyFilter.ShouldCollide(inResult))
+			{
+				// Lock the body
+				BodyLockRead lock(mBodyLockInterface, inResult);
+				if (lock.SucceededAndIsInBroadPhase()) // Race condition: body could have been removed since it has been found in the broadphase, ensures body is in the broadphase while we call the callbacks
+				{
+					const Body &body = lock.GetBody();
+
+					// Check body filter again now that we've locked the body
+					if (mBodyFilter.ShouldCollideLocked(body))
+					{
+						// Collect the transformed shape
+						TransformedShape ts = body.GetTransformedShape();
+
+						// Notify collector of new body
+						mCollector.OnBody(body);
+
+						// Release the lock now, we have all the info we need in the transformed shape
+						lock.ReleaseLock();
+
+						// Do narrow phase collision check
+						ts.CollideShape(mShape, mShapeScale, mCenterOfMassTransform, mCollideShapeSettings, mBaseOffset, mCollector, mShapeFilter);
+
+						// Notify collector of the end of this body
+						// We do this before updating the early out fraction so that the collector can still modify it
+						mCollector.OnBodyEnd();
+
+						// Update early out fraction based on narrow phase collector
+						UpdateEarlyOutFraction(mCollector.GetEarlyOutFraction());
+					}
+				}
+			}
+		}
+
+		const Shape *					mShape;
+		Vec3							mShapeScale;
+		RMat44							mCenterOfMassTransform;
+		const CollideShapeSettings &	mCollideShapeSettings;
+		RVec3							mBaseOffset;
+		CollideShapeCollector &			mCollector;
+		const BodyLockInterface &		mBodyLockInterface;
+		const BodyFilter &				mBodyFilter;
+		const ShapeFilter &				mShapeFilter;
+	};
+
+	// Calculate bounds for shape and expand by max separation distance
+	AABox bounds = inShape->GetWorldSpaceBounds(inCenterOfMassTransform, inShapeScale);
+	bounds.ExpandBy(Vec3::sReplicate(inCollideShapeSettings.mMaxSeparationDistance));
+
+	// Do broadphase test
+	MyCollector collector(inShape, inShapeScale, inCenterOfMassTransform, inCollideShapeSettings, inBaseOffset, ioCollector, *mBodyLockInterface, inBodyFilter, inShapeFilter);
+	mBroadPhaseQuery->CollideAABox(bounds, collector, inBroadPhaseLayerFilter, inObjectLayerFilter);
+}
+
+void NarrowPhaseQuery::CollideShapeWithInternalEdgeRemoval(const Shape *inShape, Vec3Arg inShapeScale, RMat44Arg inCenterOfMassTransform, const CollideShapeSettings &inCollideShapeSettings, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const
+{
+	// We require these settings for internal edge removal to work
+	CollideShapeSettings settings = inCollideShapeSettings;
+	settings.mActiveEdgeMode = EActiveEdgeMode::CollideWithAll;
+	settings.mCollectFacesMode = ECollectFacesMode::CollectFaces;
+
+	InternalEdgeRemovingCollector wrapper(ioCollector);
+	CollideShape(inShape, inShapeScale, inCenterOfMassTransform, settings, inBaseOffset, wrapper, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter);
+}
+
+void NarrowPhaseQuery::CastShape(const RShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, RVec3Arg inBaseOffset, CastShapeCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	class MyCollector : public CastShapeBodyCollector
+	{
+	public:
+							MyCollector(const RShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, RVec3Arg inBaseOffset, CastShapeCollector &ioCollector, const BodyLockInterface &inBodyLockInterface, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) :
+			CastShapeBodyCollector(ioCollector),
+			mShapeCast(inShapeCast),
+			mShapeCastSettings(inShapeCastSettings),
+			mBaseOffset(inBaseOffset),
+			mCollector(ioCollector),
+			mBodyLockInterface(inBodyLockInterface),
+			mBodyFilter(inBodyFilter),
+			mShapeFilter(inShapeFilter)
+		{
+		}
+
+		virtual void		AddHit(const ResultType &inResult) override
+		{
+			JPH_ASSERT(inResult.mFraction <= max(0.0f, mCollector.GetEarlyOutFraction()), "This hit should not have been passed on to the collector");
+
+			// Only test shape if it passes the body filter
+			if (mBodyFilter.ShouldCollide(inResult.mBodyID))
+			{
+				// Lock the body
+				BodyLockRead lock(mBodyLockInterface, inResult.mBodyID);
+				if (lock.SucceededAndIsInBroadPhase()) // Race condition: body could have been removed since it has been found in the broadphase, ensures body is in the broadphase while we call the callbacks
+				{
+					const Body &body = lock.GetBody();
+
+					// Check body filter again now that we've locked the body
+					if (mBodyFilter.ShouldCollideLocked(body))
+					{
+						// Collect the transformed shape
+						TransformedShape ts = body.GetTransformedShape();
+
+						// Notify collector of new body
+						mCollector.OnBody(body);
+
+						// Release the lock now, we have all the info we need in the transformed shape
+						lock.ReleaseLock();
+
+						// Do narrow phase collision check
+						ts.CastShape(mShapeCast, mShapeCastSettings, mBaseOffset, mCollector, mShapeFilter);
+
+						// Notify collector of the end of this body
+						// We do this before updating the early out fraction so that the collector can still modify it
+						mCollector.OnBodyEnd();
+
+						// Update early out fraction based on narrow phase collector
+						UpdateEarlyOutFraction(mCollector.GetEarlyOutFraction());
+					}
+				}
+			}
+		}
+
+		RShapeCast					mShapeCast;
+		const ShapeCastSettings &	mShapeCastSettings;
+		RVec3						mBaseOffset;
+		CastShapeCollector &		mCollector;
+		const BodyLockInterface &	mBodyLockInterface;
+		const BodyFilter &			mBodyFilter;
+		const ShapeFilter &			mShapeFilter;
+	};
+
+	// Do broadphase test
+	MyCollector collector(inShapeCast, inShapeCastSettings, inBaseOffset, ioCollector, *mBodyLockInterface, inBodyFilter, inShapeFilter);
+	mBroadPhaseQuery->CastAABox({ inShapeCast.mShapeWorldBounds, inShapeCast.mDirection }, collector, inBroadPhaseLayerFilter, inObjectLayerFilter);
+}
+
+void NarrowPhaseQuery::CollectTransformedShapes(const AABox &inBox, TransformedShapeCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const
+{
+	class MyCollector : public CollideShapeBodyCollector
+	{
+	public:
+							MyCollector(const AABox &inBox, TransformedShapeCollector &ioCollector, const BodyLockInterface &inBodyLockInterface, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) :
+			CollideShapeBodyCollector(ioCollector),
+			mBox(inBox),
+			mCollector(ioCollector),
+			mBodyLockInterface(inBodyLockInterface),
+			mBodyFilter(inBodyFilter),
+			mShapeFilter(inShapeFilter)
+		{
+		}
+
+		virtual void		AddHit(const ResultType &inResult) override
+		{
+			// Only test shape if it passes the body filter
+			if (mBodyFilter.ShouldCollide(inResult))
+			{
+				// Lock the body
+				BodyLockRead lock(mBodyLockInterface, inResult);
+				if (lock.SucceededAndIsInBroadPhase()) // Race condition: body could have been removed since it has been found in the broadphase, ensures body is in the broadphase while we call the callbacks
+				{
+					const Body &body = lock.GetBody();
+
+					// Check body filter again now that we've locked the body
+					if (mBodyFilter.ShouldCollideLocked(body))
+					{
+						// Collect the transformed shape
+						TransformedShape ts = body.GetTransformedShape();
+
+						// Notify collector of new body
+						mCollector.OnBody(body);
+
+						// Release the lock now, we have all the info we need in the transformed shape
+						lock.ReleaseLock();
+
+						// Do narrow phase collision check
+						ts.CollectTransformedShapes(mBox, mCollector, mShapeFilter);
+
+						// Notify collector of the end of this body
+						// We do this before updating the early out fraction so that the collector can still modify it
+						mCollector.OnBodyEnd();
+
+						// Update early out fraction based on narrow phase collector
+						UpdateEarlyOutFraction(mCollector.GetEarlyOutFraction());
+					}
+				}
+			}
+		}
+
+		const AABox &					mBox;
+		TransformedShapeCollector &		mCollector;
+		const BodyLockInterface &		mBodyLockInterface;
+		const BodyFilter &				mBodyFilter;
+		const ShapeFilter &				mShapeFilter;
+	};
+
+	// Do broadphase test
+	MyCollector collector(inBox, ioCollector, *mBodyLockInterface, inBodyFilter, inShapeFilter);
+	mBroadPhaseQuery->CollideAABox(inBox, collector, inBroadPhaseLayerFilter, inObjectLayerFilter);
+}
+
+JPH_NAMESPACE_END

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

@@ -0,0 +1,77 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Body/BodyFilter.h>
+#include <Jolt/Physics/Body/BodyLock.h>
+#include <Jolt/Physics/Body/BodyLockInterface.h>
+#include <Jolt/Physics/Collision/ShapeFilter.h>
+#include <Jolt/Physics/Collision/BroadPhase/BroadPhaseQuery.h>
+#include <Jolt/Physics/Collision/BackFaceMode.h>
+
+JPH_NAMESPACE_BEGIN
+
+class Shape;
+class CollideShapeSettings;
+class RayCastResult;
+
+/// Class that provides an interface for doing precise collision detection against the broad and then the narrow phase.
+/// Unlike a BroadPhaseQuery, the NarrowPhaseQuery will test against shapes and will return collision information against triangles, spheres etc.
+class JPH_EXPORT NarrowPhaseQuery : public NonCopyable
+{
+public:
+	/// Initialize the interface (should only be called by PhysicsSystem)
+	void						Init(BodyLockInterface &inBodyLockInterface, BroadPhaseQuery &inBroadPhaseQuery) { mBodyLockInterface = &inBodyLockInterface; mBroadPhaseQuery = &inBroadPhaseQuery; }
+
+	/// Cast a ray and find the closest hit. Returns true if it finds a hit. Hits further than ioHit.mFraction will not be considered and in this case ioHit will remain unmodified (and the function will return false).
+	/// Convex objects will be treated as solid (meaning if the ray starts inside, you'll get a hit fraction of 0) and back face hits against triangles are returned.
+	/// If you want the surface normal of the hit use Body::GetWorldSpaceSurfaceNormal(ioHit.mSubShapeID2, inRay.GetPointOnRay(ioHit.mFraction)) on body with ID ioHit.mBodyID.
+	bool						CastRay(const RRayCast &inRay, RayCastResult &ioHit, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }, const BodyFilter &inBodyFilter = { }) const;
+
+	/// Cast a ray, allows collecting multiple hits. Note that this version is more flexible but also slightly slower than the CastRay function that returns only a single hit.
+	/// If you want the surface normal of the hit use Body::GetWorldSpaceSurfaceNormal(collected sub shape ID, inRay.GetPointOnRay(collected fraction)) on body with collected body ID.
+	void						CastRay(const RRayCast &inRay, const RayCastSettings &inRayCastSettings, CastRayCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }, const BodyFilter &inBodyFilter = { }, const ShapeFilter &inShapeFilter = { }) const;
+
+	/// Check if inPoint is inside any shapes. For this tests all shapes are treated as if they were solid.
+	/// For a mesh shape, this test will only provide sensible information if the mesh is a closed manifold.
+	/// For each shape that collides, ioCollector will receive a hit
+	void						CollidePoint(RVec3Arg inPoint, CollidePointCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }, const BodyFilter &inBodyFilter = { }, const ShapeFilter &inShapeFilter = { }) const;
+
+	/// Collide a shape with the system
+	/// @param inShape Shape to test
+	/// @param inShapeScale Scale in local space of shape
+	/// @param inCenterOfMassTransform Center of mass transform for the shape
+	/// @param inCollideShapeSettings Settings
+	/// @param inBaseOffset All hit results will be returned relative to this offset, can be zero to get results in world position, but when you're testing far from the origin you get better precision by picking a position that's closer e.g. inCenterOfMassTransform.GetTranslation() since floats are most accurate near the origin
+	/// @param ioCollector Collector that receives the hits
+	/// @param inBroadPhaseLayerFilter Filter that filters at broadphase level
+	/// @param inObjectLayerFilter Filter that filters at layer level
+	/// @param inBodyFilter Filter that filters at body level
+	/// @param inShapeFilter Filter that filters at shape level
+	void						CollideShape(const Shape *inShape, Vec3Arg inShapeScale, RMat44Arg inCenterOfMassTransform, const CollideShapeSettings &inCollideShapeSettings, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }, const BodyFilter &inBodyFilter = { }, const ShapeFilter &inShapeFilter = { }) const;
+
+	/// Same as CollideShape, but uses InternalEdgeRemovingCollector to remove internal edges from the collision results (a.k.a. ghost collisions)
+	void						CollideShapeWithInternalEdgeRemoval(const Shape *inShape, Vec3Arg inShapeScale, RMat44Arg inCenterOfMassTransform, const CollideShapeSettings &inCollideShapeSettings, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }, const BodyFilter &inBodyFilter = { }, const ShapeFilter &inShapeFilter = { }) const;
+
+	/// Cast a shape and report any hits to ioCollector
+	/// @param inShapeCast The shape cast and its position and direction
+	/// @param inShapeCastSettings Settings for the shape cast
+	/// @param inBaseOffset All hit results will be returned relative to this offset, can be zero to get results in world position, but when you're testing far from the origin you get better precision by picking a position that's closer e.g. inShapeCast.mCenterOfMassStart.GetTranslation() since floats are most accurate near the origin
+	/// @param ioCollector Collector that receives the hits
+	/// @param inBroadPhaseLayerFilter Filter that filters at broadphase level
+	/// @param inObjectLayerFilter Filter that filters at layer level
+	/// @param inBodyFilter Filter that filters at body level
+	/// @param inShapeFilter Filter that filters at shape level
+	void						CastShape(const RShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, RVec3Arg inBaseOffset, CastShapeCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }, const BodyFilter &inBodyFilter = { }, const ShapeFilter &inShapeFilter = { }) const;
+
+	/// Collect all leaf transformed shapes that fall inside world space box inBox
+	void						CollectTransformedShapes(const AABox &inBox, TransformedShapeCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }, const BodyFilter &inBodyFilter = { }, const ShapeFilter &inShapeFilter = { }) const;
+
+private:
+	BodyLockInterface *			mBodyLockInterface = nullptr;
+	BroadPhaseQuery *			mBroadPhaseQuery = nullptr;
+};
+
+JPH_NAMESPACE_END

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

@@ -0,0 +1,62 @@
+// 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/NarrowPhaseStats.h>
+
+#ifdef JPH_TRACK_NARROWPHASE_STATS
+
+JPH_NAMESPACE_BEGIN
+
+NarrowPhaseStat	NarrowPhaseStat::sCollideShape[NumSubShapeTypes][NumSubShapeTypes];
+NarrowPhaseStat	NarrowPhaseStat::sCastShape[NumSubShapeTypes][NumSubShapeTypes];
+
+thread_local TrackNarrowPhaseStat *TrackNarrowPhaseStat::sRoot = nullptr;
+
+void NarrowPhaseStat::ReportStats(const char *inName, EShapeSubType inType1, EShapeSubType inType2, uint64 inTicks100Pct) const
+{
+	double total_pct = 100.0 * double(mTotalTicks) / double(inTicks100Pct);
+	double total_pct_excl_children = 100.0 * double(mTotalTicks - mChildTicks) / double(inTicks100Pct);
+
+	std::stringstream str;
+	str << inName << ", " << sSubShapeTypeNames[(int)inType1] << ", " << sSubShapeTypeNames[(int)inType2] << ", " << mNumQueries << ", " << total_pct << ", " << total_pct_excl_children << ", " << total_pct_excl_children / mNumQueries << ", " << mHitsReported;
+	Trace(str.str().c_str());
+}
+
+void NarrowPhaseStat::sReportStats()
+{
+	Trace("Query Type, Shape Type 1, Shape Type 2, Num Queries, Total Time (%%), Total Time Excl Children (%%), Total Time Excl. Children / Query (%%), Hits Reported");
+
+	uint64 total_ticks = 0;
+	for (EShapeSubType t1 : sAllSubShapeTypes)
+		for (EShapeSubType t2 : sAllSubShapeTypes)
+		{
+			const NarrowPhaseStat &collide_stat = sCollideShape[(int)t1][(int)t2];
+			total_ticks += collide_stat.mTotalTicks - collide_stat.mChildTicks;
+
+			const NarrowPhaseStat &cast_stat = sCastShape[(int)t1][(int)t2];
+			total_ticks += cast_stat.mTotalTicks - cast_stat.mChildTicks;
+		}
+
+	for (EShapeSubType t1 : sAllSubShapeTypes)
+		for (EShapeSubType t2 : sAllSubShapeTypes)
+		{
+			const NarrowPhaseStat &stat = sCollideShape[(int)t1][(int)t2];
+			if (stat.mNumQueries > 0)
+				stat.ReportStats("CollideShape", t1, t2, total_ticks);
+		}
+
+	for (EShapeSubType t1 : sAllSubShapeTypes)
+		for (EShapeSubType t2 : sAllSubShapeTypes)
+		{
+			const NarrowPhaseStat &stat = sCastShape[(int)t1][(int)t2];
+			if (stat.mNumQueries > 0)
+				stat.ReportStats("CastShape", t1, t2, total_ticks);
+		}
+}
+
+JPH_NAMESPACE_END
+
+#endif // JPH_TRACK_NARROWPHASE_STATS

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

@@ -0,0 +1,110 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/TickCounter.h>
+#include <Jolt/Physics/Collision/Shape/Shape.h>
+
+JPH_SUPPRESS_WARNING_PUSH
+JPH_CLANG_SUPPRESS_WARNING("-Wc++98-compat-pedantic")
+
+// Shorthand function to ifdef out code if narrow phase stats tracking is off
+#ifdef JPH_TRACK_NARROWPHASE_STATS
+	#define JPH_IF_TRACK_NARROWPHASE_STATS(...) __VA_ARGS__
+#else
+	#define JPH_IF_TRACK_NARROWPHASE_STATS(...)
+#endif // JPH_TRACK_NARROWPHASE_STATS
+
+JPH_SUPPRESS_WARNING_POP
+
+#ifdef JPH_TRACK_NARROWPHASE_STATS
+
+JPH_NAMESPACE_BEGIN
+
+/// Structure that tracks narrow phase timing information for a particular combination of shapes
+class NarrowPhaseStat
+{
+public:
+	/// Trace an individual stat in CSV form.
+	void					ReportStats(const char *inName, EShapeSubType inType1, EShapeSubType inType2, uint64 inTicks100Pct) const;
+
+	/// Trace the collected broadphase stats in CSV form.
+	/// This report can be used to judge and tweak the efficiency of the broadphase.
+	static void				sReportStats();
+
+	atomic<uint64>			mNumQueries = 0;
+	atomic<uint64>			mHitsReported = 0;
+	atomic<uint64>			mTotalTicks = 0;
+	atomic<uint64>			mChildTicks = 0;
+
+	static NarrowPhaseStat	sCollideShape[NumSubShapeTypes][NumSubShapeTypes];
+	static NarrowPhaseStat	sCastShape[NumSubShapeTypes][NumSubShapeTypes];
+};
+
+/// Object that tracks the start and end of a narrow phase operation
+class TrackNarrowPhaseStat
+{
+public:
+							TrackNarrowPhaseStat(NarrowPhaseStat &inStat) :
+		mStat(inStat),
+		mParent(sRoot),
+		mStart(GetProcessorTickCount())
+	{
+		// Make this the new root of the chain
+		sRoot = this;
+	}
+
+							~TrackNarrowPhaseStat()
+	{
+		uint64 delta_ticks = GetProcessorTickCount() - mStart;
+
+		// Notify parent of time spent in child
+		if (mParent != nullptr)
+			mParent->mStat.mChildTicks += delta_ticks;
+
+		// Increment stats at this level
+		mStat.mNumQueries++;
+		mStat.mTotalTicks += delta_ticks;
+
+		// Restore root pointer
+		JPH_ASSERT(sRoot == this);
+		sRoot = mParent;
+	}
+
+	NarrowPhaseStat &		mStat;
+	TrackNarrowPhaseStat *	mParent;
+	uint64					mStart;
+
+	static thread_local TrackNarrowPhaseStat *sRoot;
+};
+
+/// Object that tracks the start and end of a hit being processed by a collision collector
+class TrackNarrowPhaseCollector
+{
+public:
+							TrackNarrowPhaseCollector() :
+		mStart(GetProcessorTickCount())
+	{
+	}
+
+							~TrackNarrowPhaseCollector()
+	{
+		// Mark time spent in collector as 'child' time for the parent
+		uint64 delta_ticks = GetProcessorTickCount() - mStart;
+		if (TrackNarrowPhaseStat::sRoot != nullptr)
+			TrackNarrowPhaseStat::sRoot->mStat.mChildTicks += delta_ticks;
+
+		// Notify all parents of a hit
+		for (TrackNarrowPhaseStat *track = TrackNarrowPhaseStat::sRoot; track != nullptr; track = track->mParent)
+			track->mStat.mHitsReported++;
+	}
+
+private:
+	uint64					mStart;
+};
+
+JPH_NAMESPACE_END
+
+#endif // JPH_TRACK_NARROWPHASE_STATS

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

@@ -0,0 +1,111 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/NonCopyable.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Layer that objects can be in, determines which other objects it can collide with
+#ifndef JPH_OBJECT_LAYER_BITS
+	#define JPH_OBJECT_LAYER_BITS 16
+#endif // JPH_OBJECT_LAYER_BITS
+#if JPH_OBJECT_LAYER_BITS == 16
+	using ObjectLayer = uint16;
+#elif JPH_OBJECT_LAYER_BITS == 32
+	using ObjectLayer = uint32;
+#else
+	#error "JPH_OBJECT_LAYER_BITS must be 16 or 32"
+#endif
+
+/// Constant value used to indicate an invalid object layer
+static constexpr ObjectLayer cObjectLayerInvalid = ObjectLayer(~ObjectLayer(0U));
+
+/// Filter class for object layers
+class JPH_EXPORT ObjectLayerFilter : public NonCopyable
+{
+public:
+	/// Destructor
+	virtual					~ObjectLayerFilter() = default;
+
+	/// Function to filter out object layers when doing collision query test (return true to allow testing against objects with this layer)
+	virtual bool			ShouldCollide([[maybe_unused]] ObjectLayer inLayer) const
+	{
+		return true;
+	}
+
+#ifdef JPH_TRACK_BROADPHASE_STATS
+	/// Get a string that describes this filter for stat tracking purposes
+	virtual String			GetDescription() const
+	{
+		return "No Description";
+	}
+#endif // JPH_TRACK_BROADPHASE_STATS
+};
+
+/// Filter class to test if two objects can collide based on their object layer. Used while finding collision pairs.
+class JPH_EXPORT ObjectLayerPairFilter : public NonCopyable
+{
+public:
+	/// Destructor
+	virtual					~ObjectLayerPairFilter() = default;
+
+	/// Returns true if two layers can collide
+	virtual bool			ShouldCollide([[maybe_unused]] ObjectLayer inLayer1, [[maybe_unused]] ObjectLayer inLayer2) const
+	{
+		return true;
+	}
+};
+
+/// Default filter class that uses the pair filter in combination with a specified layer to filter layers
+class JPH_EXPORT DefaultObjectLayerFilter : public ObjectLayerFilter
+{
+public:
+	/// Constructor
+							DefaultObjectLayerFilter(const ObjectLayerPairFilter &inObjectLayerPairFilter, ObjectLayer inLayer) :
+		mObjectLayerPairFilter(inObjectLayerPairFilter),
+		mLayer(inLayer)
+	{
+	}
+
+	/// Copy constructor
+							DefaultObjectLayerFilter(const DefaultObjectLayerFilter &inRHS) :
+		mObjectLayerPairFilter(inRHS.mObjectLayerPairFilter),
+		mLayer(inRHS.mLayer)
+	{
+	}
+
+	// See ObjectLayerFilter::ShouldCollide
+	virtual bool			ShouldCollide(ObjectLayer inLayer) const override
+	{
+		return mObjectLayerPairFilter.ShouldCollide(mLayer, inLayer);
+	}
+
+private:
+	const ObjectLayerPairFilter & mObjectLayerPairFilter;
+	ObjectLayer				mLayer;
+};
+
+/// Allows objects from a specific layer only
+class JPH_EXPORT SpecifiedObjectLayerFilter : public ObjectLayerFilter
+{
+public:
+	/// Constructor
+	explicit				SpecifiedObjectLayerFilter(ObjectLayer inLayer) :
+		mLayer(inLayer)
+	{
+	}
+
+	// See ObjectLayerFilter::ShouldCollide
+	virtual bool			ShouldCollide(ObjectLayer inLayer) const override
+	{
+		return mLayer == inLayer;
+	}
+
+private:
+	ObjectLayer				mLayer;
+};
+
+JPH_NAMESPACE_END

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

@@ -0,0 +1,52 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/ObjectLayer.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Filter class to test if two objects can collide based on their object layer. Used while finding collision pairs.
+/// Uses group bits and mask bits. Two layers can collide if Object1.Group & Object2.Mask is non-zero and Object2.Group & Object1.Mask is non-zero.
+/// The behavior is similar to that in e.g. Bullet.
+/// This implementation works together with BroadPhaseLayerInterfaceMask and ObjectVsBroadPhaseLayerFilterMask
+class ObjectLayerPairFilterMask : public ObjectLayerPairFilter
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Number of bits for the group and mask bits
+	static constexpr uint32 cNumBits = JPH_OBJECT_LAYER_BITS / 2;
+	static constexpr uint32	cMask = (1 << cNumBits) - 1;
+
+	/// Construct an ObjectLayer from a group and mask bits
+	static ObjectLayer		sGetObjectLayer(uint32 inGroup, uint32 inMask = cMask)
+	{
+		JPH_ASSERT((inGroup & ~cMask) == 0);
+		JPH_ASSERT((inMask & ~cMask) == 0);
+		return ObjectLayer((inGroup & cMask) | (inMask << cNumBits));
+	}
+
+	/// Get the group bits from an ObjectLayer
+	static inline uint32	sGetGroup(ObjectLayer inObjectLayer)
+	{
+		return uint32(inObjectLayer) & cMask;
+	}
+
+	/// Get the mask bits from an ObjectLayer
+	static inline uint32	sGetMask(ObjectLayer inObjectLayer)
+	{
+		return uint32(inObjectLayer) >> cNumBits;
+	}
+
+	/// Returns true if two layers can collide
+	virtual bool			ShouldCollide(ObjectLayer inObject1, ObjectLayer inObject2) const override
+	{
+		return (sGetGroup(inObject1) & sGetMask(inObject2)) != 0
+			&& (sGetGroup(inObject2) & sGetMask(inObject1)) != 0;
+	}
+};
+
+JPH_NAMESPACE_END

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

@@ -0,0 +1,78 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/ObjectLayer.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Filter class to test if two objects can collide based on their object layer. Used while finding collision pairs.
+/// This implementation uses a table to determine if two layers can collide.
+class ObjectLayerPairFilterTable : public ObjectLayerPairFilter
+{
+private:
+	/// Get which bit corresponds to the pair (inLayer1, inLayer2)
+	uint					GetBit(ObjectLayer inLayer1, ObjectLayer inLayer2) const
+	{
+		// We store the lower left half only, so swap the inputs when trying to access the top right half
+		if (inLayer1 > inLayer2)
+			std::swap(inLayer1, inLayer2);
+
+		JPH_ASSERT(inLayer2 < mNumObjectLayers);
+
+		// Calculate at which bit the entry for this pair resides
+		// We use the fact that a row always starts at inLayer2 * (inLayer2 + 1) / 2
+		// (this is the amount of bits needed to store a table of inLayer2 entries)
+		return (inLayer2 * (inLayer2 + 1)) / 2 + inLayer1;
+	}
+
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructs the table with inNumObjectLayers Layers, initially all layer pairs are disabled
+	explicit				ObjectLayerPairFilterTable(uint inNumObjectLayers) :
+		mNumObjectLayers(inNumObjectLayers)
+	{
+		// By default nothing collides
+		// For the first layer we only need to store 1 bit, for the second 2 bits, for the third 3 bits, etc.
+		// We use the formula Sum_i=1^N i = N * (N + 1) / 2 to calculate the size of the table
+		int table_size = (inNumObjectLayers * (inNumObjectLayers + 1) / 2 + 7) / 8;
+		mTable.resize(table_size, 0);
+	}
+
+	/// Get the number of object layers
+	uint					GetNumObjectLayers() const
+	{
+		return mNumObjectLayers;
+	}
+
+	/// Disable collision between two object layers
+	void					DisableCollision(ObjectLayer inLayer1, ObjectLayer inLayer2)
+	{
+		uint bit = GetBit(inLayer1, inLayer2);
+		mTable[bit >> 3] &= (0xff ^ (1 << (bit & 0b111)));
+	}
+
+	/// Enable collision between two object layers
+	void					EnableCollision(ObjectLayer inLayer1, ObjectLayer inLayer2)
+	{
+		uint bit = GetBit(inLayer1, inLayer2);
+		mTable[bit >> 3] |= 1 << (bit & 0b111);
+	}
+
+	/// Returns true if two layers can collide
+	virtual bool			ShouldCollide(ObjectLayer inObject1, ObjectLayer inObject2) const override
+	{
+		// Test if the bit is set for this group pair
+		uint bit = GetBit(inObject1, inObject2);
+		return (mTable[bit >> 3] & (1 << (bit & 0b111))) != 0;
+	}
+
+private:
+	uint					mNumObjectLayers;							///< The number of layers that this table supports
+	Array<uint8>			mTable;										///< The table of bits that indicates which layers collide
+};
+
+JPH_NAMESPACE_END

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

@@ -0,0 +1,35 @@
+// 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/PhysicsMaterial.h>
+#include <Jolt/Physics/Collision/PhysicsMaterialSimple.h>
+#include <Jolt/Core/StreamUtils.h>
+
+JPH_NAMESPACE_BEGIN
+
+RefConst<PhysicsMaterial> PhysicsMaterial::sDefault;
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(PhysicsMaterial)
+{
+	JPH_ADD_BASE_CLASS(PhysicsMaterial, SerializableObject)
+}
+
+void PhysicsMaterial::SaveBinaryState(StreamOut &inStream) const
+{
+	inStream.Write(GetRTTI()->GetHash());
+}
+
+void PhysicsMaterial::RestoreBinaryState(StreamIn &inStream)
+{
+	// RTTI hash is read in sRestoreFromBinaryState
+}
+
+PhysicsMaterial::PhysicsMaterialResult PhysicsMaterial::sRestoreFromBinaryState(StreamIn &inStream)
+{
+	return StreamUtils::RestoreObject<PhysicsMaterial>(inStream, &PhysicsMaterial::RestoreBinaryState);
+}
+
+JPH_NAMESPACE_END

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

@@ -0,0 +1,52 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/Reference.h>
+#include <Jolt/Core/Color.h>
+#include <Jolt/Core/Result.h>
+#include <Jolt/ObjectStream/SerializableObject.h>
+
+JPH_NAMESPACE_BEGIN
+
+class StreamIn;
+class StreamOut;
+
+/// This structure describes the surface of (part of) a shape. You should inherit from it to define additional
+/// information that is interesting for the simulation. The 2 materials involved in a contact could be used
+/// to decide which sound or particle effects to play.
+///
+/// If you inherit from this material, don't forget to create a suitable default material in sDefault
+class JPH_EXPORT PhysicsMaterial : public SerializableObject, public RefTarget<PhysicsMaterial>
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, PhysicsMaterial)
+
+public:
+	/// Virtual destructor
+	virtual									~PhysicsMaterial() override = default;
+
+	/// Default material that is used when a shape has no materials defined
+	static RefConst<PhysicsMaterial>		sDefault;
+
+	// Properties
+	virtual const char *					GetDebugName() const			{ return "Unknown"; }
+	virtual Color							GetDebugColor() const			{ return Color::sGrey; }
+
+	/// Saves the contents of the material in binary form to inStream.
+	virtual void							SaveBinaryState(StreamOut &inStream) const;
+
+	using PhysicsMaterialResult = Result<Ref<PhysicsMaterial>>;
+
+	/// Creates a PhysicsMaterial of the correct type and restores its contents from the binary stream inStream.
+	static PhysicsMaterialResult			sRestoreFromBinaryState(StreamIn &inStream);
+
+protected:
+	/// This function should not be called directly, it is used by sRestoreFromBinaryState.
+	virtual void							RestoreBinaryState(StreamIn &inStream);
+};
+
+using PhysicsMaterialList = Array<RefConst<PhysicsMaterial>>;
+
+JPH_NAMESPACE_END

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

@@ -0,0 +1,38 @@
+// 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/PhysicsMaterialSimple.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(PhysicsMaterialSimple)
+{
+	JPH_ADD_BASE_CLASS(PhysicsMaterialSimple, PhysicsMaterial)
+
+	JPH_ADD_ATTRIBUTE(PhysicsMaterialSimple, mDebugName)
+	JPH_ADD_ATTRIBUTE(PhysicsMaterialSimple, mDebugColor)
+}
+
+void PhysicsMaterialSimple::SaveBinaryState(StreamOut &inStream) const
+{
+	PhysicsMaterial::SaveBinaryState(inStream);
+
+	inStream.Write(mDebugName);
+	inStream.Write(mDebugColor);
+}
+
+void PhysicsMaterialSimple::RestoreBinaryState(StreamIn &inStream)
+{
+	PhysicsMaterial::RestoreBinaryState(inStream);
+
+	inStream.Read(mDebugName);
+	inStream.Read(mDebugColor);
+}
+
+JPH_NAMESPACE_END

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

@@ -0,0 +1,37 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/PhysicsMaterial.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Sample implementation of PhysicsMaterial that just holds the needed properties directly
+class JPH_EXPORT PhysicsMaterialSimple : public PhysicsMaterial
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, PhysicsMaterialSimple)
+
+public:
+	/// Constructor
+											PhysicsMaterialSimple() = default;
+											PhysicsMaterialSimple(const string_view &inName, ColorArg inColor) : mDebugName(inName), mDebugColor(inColor) { }
+
+	// Properties
+	virtual const char *					GetDebugName() const override		{ return mDebugName.c_str(); }
+	virtual Color							GetDebugColor() const override		{ return mDebugColor; }
+
+	// See: PhysicsMaterial::SaveBinaryState
+	virtual void							SaveBinaryState(StreamOut &inStream) const override;
+
+protected:
+	// See: PhysicsMaterial::RestoreBinaryState
+	virtual void							RestoreBinaryState(StreamIn &inStream) override;
+
+private:
+	String									mDebugName;							///< Name of the material, used for debugging purposes
+	Color									mDebugColor = Color::sGrey;			///< Color of the material, used to render the shapes
+};
+
+JPH_NAMESPACE_END

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

@@ -0,0 +1,87 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/BackFaceMode.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Structure that holds a single ray cast
+template <class Vec, class Mat, class RayCastType>
+struct RayCastT
+{
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructors
+								RayCastT() = default; // Allow raycast to be created uninitialized
+								RayCastT(typename Vec::ArgType inOrigin, Vec3Arg inDirection) : mOrigin(inOrigin), mDirection(inDirection) { }
+								RayCastT(const RayCastT<Vec, Mat, RayCastType> &) = default;
+
+	/// Transform this ray using inTransform
+	RayCastType					Transformed(typename Mat::ArgType inTransform) const
+	{
+		Vec ray_origin = inTransform * mOrigin;
+		Vec3 ray_direction(inTransform * (mOrigin + mDirection) - ray_origin);
+		return { ray_origin, ray_direction };
+	}
+
+	/// Translate ray using inTranslation
+	RayCastType					Translated(typename Vec::ArgType inTranslation) const
+	{
+		return { inTranslation + mOrigin, mDirection };
+	}
+
+	/// Get point with fraction inFraction on ray (0 = start of ray, 1 = end of ray)
+	inline Vec					GetPointOnRay(float inFraction) const
+	{
+		return mOrigin + inFraction * mDirection;
+	}
+
+	Vec							mOrigin;					///< Origin of the ray
+	Vec3						mDirection;					///< Direction and length of the ray (anything beyond this length will not be reported as a hit)
+};
+
+struct RayCast : public RayCastT<Vec3, Mat44, RayCast>
+{
+	using RayCastT<Vec3, Mat44, RayCast>::RayCastT;
+};
+
+struct RRayCast : public RayCastT<RVec3, RMat44, RRayCast>
+{
+	using RayCastT<RVec3, RMat44, RRayCast>::RayCastT;
+
+	/// Convert from RayCast, converts single to double precision
+	explicit					RRayCast(const RayCast &inRay) :
+		RRayCast(RVec3(inRay.mOrigin), inRay.mDirection)
+	{
+	}
+
+	/// Convert to RayCast, which implies casting from double precision to single precision
+	explicit					operator RayCast() const
+	{
+		return RayCast(Vec3(mOrigin), mDirection);
+	}
+};
+
+/// Settings to be passed with a ray cast
+class RayCastSettings
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Set the backfacing mode for all shapes
+	void						SetBackFaceMode(EBackFaceMode inMode) { mBackFaceModeTriangles = mBackFaceModeConvex = inMode; }
+
+	/// How backfacing triangles should be treated (should we report back facing hits for triangle based shapes, e.g. MeshShape/HeightFieldShape?)
+	EBackFaceMode				mBackFaceModeTriangles		= EBackFaceMode::IgnoreBackFaces;
+
+	/// How backfacing convex objects should be treated (should we report back facing hits for convex shapes?)
+	EBackFaceMode				mBackFaceModeConvex			= EBackFaceMode::IgnoreBackFaces;
+
+	/// If convex shapes should be treated as solid. When true, a ray starting inside a convex shape will generate a hit at fraction 0.
+	bool						mTreatConvexAsSolid			= true;
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/BoxShape.cpp

@@ -0,0 +1,312 @@
+// 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/Shape/BoxShape.h>
+#include <Jolt/Physics/Collision/Shape/ScaleHelpers.h>
+#include <Jolt/Physics/Collision/Shape/GetTrianglesContext.h>
+#include <Jolt/Physics/Collision/RayCast.h>
+#include <Jolt/Physics/Collision/CastResult.h>
+#include <Jolt/Physics/Collision/CollidePointResult.h>
+#include <Jolt/Physics/Collision/TransformedShape.h>
+#include <Jolt/Physics/Collision/CollideSoftBodyVertexIterator.h>
+#include <Jolt/Geometry/RayAABox.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(BoxShapeSettings)
+{
+	JPH_ADD_BASE_CLASS(BoxShapeSettings, ConvexShapeSettings)
+
+	JPH_ADD_ATTRIBUTE(BoxShapeSettings, mHalfExtent)
+	JPH_ADD_ATTRIBUTE(BoxShapeSettings, mConvexRadius)
+}
+
+static const Vec3 sUnitBoxTriangles[] = {
+	Vec3(-1, 1, -1),	Vec3(-1, 1, 1),		Vec3(1, 1, 1),
+	Vec3(-1, 1, -1),	Vec3(1, 1, 1),		Vec3(1, 1, -1),
+	Vec3(-1, -1, -1),	Vec3(1, -1, -1),	Vec3(1, -1, 1),
+	Vec3(-1, -1, -1),	Vec3(1, -1, 1),		Vec3(-1, -1, 1),
+	Vec3(-1, 1, -1),	Vec3(-1, -1, -1),	Vec3(-1, -1, 1),
+	Vec3(-1, 1, -1),	Vec3(-1, -1, 1),	Vec3(-1, 1, 1),
+	Vec3(1, 1, 1),		Vec3(1, -1, 1),		Vec3(1, -1, -1),
+	Vec3(1, 1, 1),		Vec3(1, -1, -1),	Vec3(1, 1, -1),
+	Vec3(-1, 1, 1),		Vec3(-1, -1, 1),	Vec3(1, -1, 1),
+	Vec3(-1, 1, 1),		Vec3(1, -1, 1),		Vec3(1, 1, 1),
+	Vec3(-1, 1, -1),	Vec3(1, 1, -1),		Vec3(1, -1, -1),
+	Vec3(-1, 1, -1),	Vec3(1, -1, -1),	Vec3(-1, -1, -1)
+};
+
+ShapeSettings::ShapeResult BoxShapeSettings::Create() const
+{
+	if (mCachedResult.IsEmpty())
+		Ref<Shape> shape = new BoxShape(*this, mCachedResult);
+	return mCachedResult;
+}
+
+BoxShape::BoxShape(const BoxShapeSettings &inSettings, ShapeResult &outResult) :
+	ConvexShape(EShapeSubType::Box, inSettings, outResult),
+	mHalfExtent(inSettings.mHalfExtent),
+	mConvexRadius(inSettings.mConvexRadius)
+{
+	// Check convex radius
+	if (inSettings.mConvexRadius < 0.0f
+		|| inSettings.mHalfExtent.ReduceMin() < inSettings.mConvexRadius)
+	{
+		outResult.SetError("Invalid convex radius");
+		return;
+	}
+
+	// Result is valid
+	outResult.Set(this);
+}
+
+class BoxShape::Box final : public Support
+{
+public:
+					Box(const AABox &inBox, float inConvexRadius) :
+		mBox(inBox),
+		mConvexRadius(inConvexRadius)
+	{
+		static_assert(sizeof(Box) <= sizeof(SupportBuffer), "Buffer size too small");
+		JPH_ASSERT(IsAligned(this, alignof(Box)));
+	}
+
+	virtual Vec3	GetSupport(Vec3Arg inDirection) const override
+	{
+		return mBox.GetSupport(inDirection);
+	}
+
+	virtual float	GetConvexRadius() const override
+	{
+		return mConvexRadius;
+	}
+
+private:
+	AABox			mBox;
+	float			mConvexRadius;
+};
+
+const ConvexShape::Support *BoxShape::GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const
+{
+	// Scale our half extents
+	Vec3 scaled_half_extent = inScale.Abs() * mHalfExtent;
+
+	switch (inMode)
+	{
+	case ESupportMode::IncludeConvexRadius:
+	case ESupportMode::Default:
+		{
+			// Make box out of our half extents
+			AABox box = AABox(-scaled_half_extent, scaled_half_extent);
+			JPH_ASSERT(box.IsValid());
+			return new (&inBuffer) Box(box, 0.0f);
+		}
+
+	case ESupportMode::ExcludeConvexRadius:
+		{
+			// Reduce the box by our convex radius
+			float convex_radius = ScaleHelpers::ScaleConvexRadius(mConvexRadius, inScale);
+			Vec3 convex_radius3 = Vec3::sReplicate(convex_radius);
+			Vec3 reduced_half_extent = scaled_half_extent - convex_radius3;
+			AABox box = AABox(-reduced_half_extent, reduced_half_extent);
+			JPH_ASSERT(box.IsValid());
+			return new (&inBuffer) Box(box, convex_radius);
+		}
+	}
+
+	JPH_ASSERT(false);
+	return nullptr;
+}
+
+void BoxShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const
+{
+	JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID");
+
+	Vec3 scaled_half_extent = inScale.Abs() * mHalfExtent;
+	AABox box(-scaled_half_extent, scaled_half_extent);
+	box.GetSupportingFace(inDirection, outVertices);
+
+	// Transform to world space
+	for (Vec3 &v : outVertices)
+		v = inCenterOfMassTransform * v;
+}
+
+MassProperties BoxShape::GetMassProperties() const
+{
+	MassProperties p;
+	p.SetMassAndInertiaOfSolidBox(2.0f * mHalfExtent, GetDensity());
+	return p;
+}
+
+Vec3 BoxShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const
+{
+	JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID");
+
+	// Get component that is closest to the surface of the box
+	int index = (inLocalSurfacePosition.Abs() - mHalfExtent).Abs().GetLowestComponentIndex();
+
+	// Calculate normal
+	Vec3 normal = Vec3::sZero();
+	normal.SetComponent(index, inLocalSurfacePosition[index] > 0.0f? 1.0f : -1.0f);
+	return normal;
+}
+
+#ifdef JPH_DEBUG_RENDERER
+void BoxShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
+{
+	DebugRenderer::EDrawMode draw_mode = inDrawWireframe? DebugRenderer::EDrawMode::Wireframe : DebugRenderer::EDrawMode::Solid;
+	inRenderer->DrawBox(inCenterOfMassTransform * Mat44::sScale(inScale.Abs()), GetLocalBounds(), inUseMaterialColors? GetMaterial()->GetDebugColor() : inColor, DebugRenderer::ECastShadow::On, draw_mode);
+}
+#endif // JPH_DEBUG_RENDERER
+
+bool BoxShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const
+{
+	// Test hit against box
+	float fraction = max(RayAABox(inRay.mOrigin, RayInvDirection(inRay.mDirection), -mHalfExtent, mHalfExtent), 0.0f);
+	if (fraction < ioHit.mFraction)
+	{
+		ioHit.mFraction = fraction;
+		ioHit.mSubShapeID2 = inSubShapeIDCreator.GetID();
+		return true;
+	}
+	return false;
+}
+
+void BoxShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	// Test shape filter
+	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
+		return;
+
+	float min_fraction, max_fraction;
+	RayAABox(inRay.mOrigin, RayInvDirection(inRay.mDirection), -mHalfExtent, mHalfExtent, min_fraction, max_fraction);
+	if (min_fraction <= max_fraction // Ray should intersect
+		&& max_fraction >= 0.0f // End of ray should be inside box
+		&& min_fraction < ioCollector.GetEarlyOutFraction()) // Start of ray should be before early out fraction
+	{
+		// Better hit than the current hit
+		RayCastResult hit;
+		hit.mBodyID = TransformedShape::sGetBodyID(ioCollector.GetContext());
+		hit.mSubShapeID2 = inSubShapeIDCreator.GetID();
+
+		// Check front side
+		if (inRayCastSettings.mTreatConvexAsSolid || min_fraction > 0.0f)
+		{
+			hit.mFraction = max(0.0f, min_fraction);
+			ioCollector.AddHit(hit);
+		}
+
+		// Check back side hit
+		if (inRayCastSettings.mBackFaceModeConvex == EBackFaceMode::CollideWithBackFaces
+			&& max_fraction < ioCollector.GetEarlyOutFraction())
+		{
+			hit.mFraction = max_fraction;
+			ioCollector.AddHit(hit);
+		}
+	}
+}
+
+void BoxShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	// Test shape filter
+	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
+		return;
+
+	if (Vec3::sLessOrEqual(inPoint.Abs(), mHalfExtent).TestAllXYZTrue())
+		ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator.GetID() });
+}
+
+void BoxShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const
+{
+	Mat44 inverse_transform = inCenterOfMassTransform.InversedRotationTranslation();
+	Vec3 half_extent = inScale.Abs() * mHalfExtent;
+
+	for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v)
+		if (v.GetInvMass() > 0.0f)
+		{
+			// Convert to local space
+			Vec3 local_pos = inverse_transform * v.GetPosition();
+
+			// Clamp point to inside box
+			Vec3 clamped_point = Vec3::sMax(Vec3::sMin(local_pos, half_extent), -half_extent);
+
+			// Test if point was inside
+			if (clamped_point == local_pos)
+			{
+				// Calculate closest distance to surface
+				Vec3 delta = half_extent - local_pos.Abs();
+				int index = delta.GetLowestComponentIndex();
+				float penetration = delta[index];
+				if (v.UpdatePenetration(penetration))
+				{
+					// Calculate contact point and normal
+					Vec3 possible_normals[] = { Vec3::sAxisX(), Vec3::sAxisY(), Vec3::sAxisZ() };
+					Vec3 normal = local_pos.GetSign() * possible_normals[index];
+					Vec3 point = normal * half_extent;
+
+					// Store collision
+					v.SetCollision(Plane::sFromPointAndNormal(point, normal).GetTransformed(inCenterOfMassTransform), inCollidingShapeIndex);
+				}
+			}
+			else
+			{
+				// Calculate normal
+				Vec3 normal = local_pos - clamped_point;
+				float normal_length = normal.Length();
+
+				// Penetration will be negative since we're not penetrating
+				float penetration = -normal_length;
+				if (v.UpdatePenetration(penetration))
+				{
+					normal /= normal_length;
+
+					// Store collision
+					v.SetCollision(Plane::sFromPointAndNormal(clamped_point, normal).GetTransformed(inCenterOfMassTransform), inCollidingShapeIndex);
+				}
+			}
+		}
+}
+
+void BoxShape::GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const
+{
+	new (&ioContext) GetTrianglesContextVertexList(inPositionCOM, inRotation, inScale, Mat44::sScale(mHalfExtent), sUnitBoxTriangles, std::size(sUnitBoxTriangles), GetMaterial());
+}
+
+int BoxShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) const
+{
+	return ((GetTrianglesContextVertexList &)ioContext).GetTrianglesNext(inMaxTrianglesRequested, outTriangleVertices, outMaterials);
+}
+
+void BoxShape::SaveBinaryState(StreamOut &inStream) const
+{
+	ConvexShape::SaveBinaryState(inStream);
+
+	inStream.Write(mHalfExtent);
+	inStream.Write(mConvexRadius);
+}
+
+void BoxShape::RestoreBinaryState(StreamIn &inStream)
+{
+	ConvexShape::RestoreBinaryState(inStream);
+
+	inStream.Read(mHalfExtent);
+	inStream.Read(mConvexRadius);
+}
+
+void BoxShape::sRegister()
+{
+	ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::Box);
+	f.mConstruct = []() -> Shape * { return new BoxShape; };
+	f.mColor = Color::sGreen;
+}
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/BoxShape.h

@@ -0,0 +1,115 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/Shape/ConvexShape.h>
+#include <Jolt/Physics/PhysicsSettings.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Class that constructs a BoxShape
+class JPH_EXPORT BoxShapeSettings final : public ConvexShapeSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, BoxShapeSettings)
+
+public:
+	/// Default constructor for deserialization
+							BoxShapeSettings() = default;
+
+	/// Create a box with half edge length inHalfExtent and convex radius inConvexRadius.
+	/// (internally the convex radius will be subtracted from the half extent so the total box will not grow with the convex radius).
+							BoxShapeSettings(Vec3Arg inHalfExtent, float inConvexRadius = cDefaultConvexRadius, const PhysicsMaterial *inMaterial = nullptr) : ConvexShapeSettings(inMaterial), mHalfExtent(inHalfExtent), mConvexRadius(inConvexRadius) { }
+
+	// See: ShapeSettings
+	virtual ShapeResult		Create() const override;
+
+	Vec3					mHalfExtent = Vec3::sZero();								///< Half the size of the box (including convex radius)
+	float					mConvexRadius = 0.0f;
+};
+
+/// A box, centered around the origin
+class JPH_EXPORT BoxShape final : public ConvexShape
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+							BoxShape() : ConvexShape(EShapeSubType::Box) { }
+							BoxShape(const BoxShapeSettings &inSettings, ShapeResult &outResult);
+
+	/// Create a box with half edge length inHalfExtent and convex radius inConvexRadius.
+	/// (internally the convex radius will be subtracted from the half extent so the total box will not grow with the convex radius).
+							BoxShape(Vec3Arg inHalfExtent, float inConvexRadius = cDefaultConvexRadius, const PhysicsMaterial *inMaterial = nullptr) : ConvexShape(EShapeSubType::Box, inMaterial), mHalfExtent(inHalfExtent), mConvexRadius(inConvexRadius) { JPH_ASSERT(inConvexRadius >= 0.0f); JPH_ASSERT(inHalfExtent.ReduceMin() >= inConvexRadius); }
+
+	/// Get half extent of box
+	Vec3					GetHalfExtent() const										{ return mHalfExtent; }
+
+	// See Shape::GetLocalBounds
+	virtual AABox			GetLocalBounds() const override								{ return AABox(-mHalfExtent, mHalfExtent); }
+
+	// See Shape::GetInnerRadius
+	virtual float			GetInnerRadius() const override								{ return mHalfExtent.ReduceMin(); }
+
+	// See Shape::GetMassProperties
+	virtual MassProperties	GetMassProperties() const override;
+
+	// See Shape::GetSurfaceNormal
+	virtual Vec3			GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override;
+
+	// See Shape::GetSupportingFace
+	virtual void			GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override;
+
+	// See ConvexShape::GetSupportFunction
+	virtual const Support *	GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const override;
+
+#ifdef JPH_DEBUG_RENDERER
+	// See Shape::Draw
+	virtual void			Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override;
+#endif // JPH_DEBUG_RENDERER
+
+	// See Shape::CastRay
+	virtual bool			CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override;
+	virtual void			CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
+
+	// See: Shape::CollidePoint
+	virtual void			CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
+
+	// See: Shape::CollideSoftBodyVertices
+	virtual void			CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override;
+
+	// See Shape::GetTrianglesStart
+	virtual void			GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override;
+
+	// See Shape::GetTrianglesNext
+	virtual int				GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override;
+
+	// See Shape
+	virtual void			SaveBinaryState(StreamOut &inStream) const override;
+
+	// See Shape::GetStats
+	virtual Stats			GetStats() const override									{ return Stats(sizeof(*this), 12); }
+
+	// See Shape::GetVolume
+	virtual float			GetVolume() const override									{ return GetLocalBounds().GetVolume(); }
+
+	/// Get the convex radius of this box
+	float					GetConvexRadius() const										{ return mConvexRadius; }
+
+	// Register shape functions with the registry
+	static void				sRegister();
+
+protected:
+	// See: Shape::RestoreBinaryState
+	virtual void			RestoreBinaryState(StreamIn &inStream) override;
+
+private:
+	// Class for GetSupportFunction
+	class					Box;
+
+	Vec3					mHalfExtent = Vec3::sZero();								///< Half the size of the box (including convex radius)
+	float					mConvexRadius = 0.0f;
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CapsuleShape.cpp

@@ -0,0 +1,438 @@
+// 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/Shape/CapsuleShape.h>
+#include <Jolt/Physics/Collision/Shape/SphereShape.h>
+#include <Jolt/Physics/Collision/Shape/ScaleHelpers.h>
+#include <Jolt/Physics/Collision/Shape/GetTrianglesContext.h>
+#include <Jolt/Physics/Collision/RayCast.h>
+#include <Jolt/Physics/Collision/CastResult.h>
+#include <Jolt/Physics/Collision/CollidePointResult.h>
+#include <Jolt/Physics/Collision/TransformedShape.h>
+#include <Jolt/Physics/Collision/CollideSoftBodyVertexIterator.h>
+#include <Jolt/Geometry/RayCapsule.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(CapsuleShapeSettings)
+{
+	JPH_ADD_BASE_CLASS(CapsuleShapeSettings, ConvexShapeSettings)
+
+	JPH_ADD_ATTRIBUTE(CapsuleShapeSettings, mRadius)
+	JPH_ADD_ATTRIBUTE(CapsuleShapeSettings, mHalfHeightOfCylinder)
+}
+
+static const int cCapsuleDetailLevel = 2;
+
+static const StaticArray<Vec3, 192> sCapsuleTopTriangles = []() {
+	StaticArray<Vec3, 192> verts;
+	GetTrianglesContextVertexList::sCreateHalfUnitSphereTop(verts, cCapsuleDetailLevel);
+	return verts;
+}();
+
+static const StaticArray<Vec3, 96> sCapsuleMiddleTriangles = []() {
+	StaticArray<Vec3, 96> verts;
+	GetTrianglesContextVertexList::sCreateUnitOpenCylinder(verts, cCapsuleDetailLevel);
+	return verts;
+}();
+
+static const StaticArray<Vec3, 192> sCapsuleBottomTriangles = []() {
+	StaticArray<Vec3, 192> verts;
+	GetTrianglesContextVertexList::sCreateHalfUnitSphereBottom(verts, cCapsuleDetailLevel);
+	return verts;
+}();
+
+ShapeSettings::ShapeResult CapsuleShapeSettings::Create() const
+{
+	if (mCachedResult.IsEmpty())
+	{
+		Ref<Shape> shape;
+		if (IsValid() && IsSphere())
+		{
+			// If the capsule has no height, use a sphere instead
+			shape = new SphereShape(mRadius, mMaterial);
+			mCachedResult.Set(shape);
+		}
+		else
+			shape = new CapsuleShape(*this, mCachedResult);
+	}
+	return mCachedResult;
+}
+
+CapsuleShape::CapsuleShape(const CapsuleShapeSettings &inSettings, ShapeResult &outResult) :
+	ConvexShape(EShapeSubType::Capsule, inSettings, outResult),
+	mRadius(inSettings.mRadius),
+	mHalfHeightOfCylinder(inSettings.mHalfHeightOfCylinder)
+{
+	if (inSettings.mHalfHeightOfCylinder <= 0.0f)
+	{
+		outResult.SetError("Invalid height");
+		return;
+	}
+
+	if (inSettings.mRadius <= 0.0f)
+	{
+		outResult.SetError("Invalid radius");
+		return;
+	}
+
+	outResult.Set(this);
+}
+
+class CapsuleShape::CapsuleNoConvex final : public Support
+{
+public:
+					CapsuleNoConvex(Vec3Arg inHalfHeightOfCylinder, float inConvexRadius) :
+		mHalfHeightOfCylinder(inHalfHeightOfCylinder),
+		mConvexRadius(inConvexRadius)
+	{
+		static_assert(sizeof(CapsuleNoConvex) <= sizeof(SupportBuffer), "Buffer size too small");
+		JPH_ASSERT(IsAligned(this, alignof(CapsuleNoConvex)));
+	}
+
+	virtual Vec3	GetSupport(Vec3Arg inDirection) const override
+	{
+		if (inDirection.GetY() > 0)
+			return mHalfHeightOfCylinder;
+		else
+			return -mHalfHeightOfCylinder;
+	}
+
+	virtual float	GetConvexRadius() const override
+	{
+		return mConvexRadius;
+	}
+
+private:
+	Vec3			mHalfHeightOfCylinder;
+	float			mConvexRadius;
+};
+
+class CapsuleShape::CapsuleWithConvex final : public Support
+{
+public:
+					CapsuleWithConvex(Vec3Arg inHalfHeightOfCylinder, float inRadius) :
+		mHalfHeightOfCylinder(inHalfHeightOfCylinder),
+		mRadius(inRadius)
+	{
+		static_assert(sizeof(CapsuleWithConvex) <= sizeof(SupportBuffer), "Buffer size too small");
+		JPH_ASSERT(IsAligned(this, alignof(CapsuleWithConvex)));
+	}
+
+	virtual Vec3	GetSupport(Vec3Arg inDirection) const override
+	{
+		float len = inDirection.Length();
+		Vec3 radius = len > 0.0f? inDirection * (mRadius / len) : Vec3::sZero();
+
+		if (inDirection.GetY() > 0)
+			return radius + mHalfHeightOfCylinder;
+		else
+			return radius - mHalfHeightOfCylinder;
+	}
+
+	virtual float	GetConvexRadius() const override
+	{
+		return 0.0f;
+	}
+
+private:
+	Vec3			mHalfHeightOfCylinder;
+	float			mRadius;
+};
+
+const ConvexShape::Support *CapsuleShape::GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const
+{
+	JPH_ASSERT(IsValidScale(inScale));
+
+	// Get scaled capsule
+	Vec3 abs_scale = inScale.Abs();
+	float scale = abs_scale.GetX();
+	Vec3 scaled_half_height_of_cylinder = Vec3(0, scale * mHalfHeightOfCylinder, 0);
+	float scaled_radius = scale * mRadius;
+
+	switch (inMode)
+	{
+	case ESupportMode::IncludeConvexRadius:
+		return new (&inBuffer) CapsuleWithConvex(scaled_half_height_of_cylinder, scaled_radius);
+
+	case ESupportMode::ExcludeConvexRadius:
+	case ESupportMode::Default:
+		return new (&inBuffer) CapsuleNoConvex(scaled_half_height_of_cylinder, scaled_radius);
+	}
+
+	JPH_ASSERT(false);
+	return nullptr;
+}
+
+void CapsuleShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const
+{
+	JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID");
+	JPH_ASSERT(IsValidScale(inScale));
+
+	// Get direction in horizontal plane
+	Vec3 direction = inDirection;
+	direction.SetComponent(1, 0.0f);
+
+	// Check zero vector, in this case we're hitting from top/bottom so there's no supporting face
+	float len = direction.Length();
+	if (len == 0.0f)
+		return;
+
+	// Get scaled capsule
+	Vec3 abs_scale = inScale.Abs();
+	float scale = abs_scale.GetX();
+	Vec3 scaled_half_height_of_cylinder = Vec3(0, scale * mHalfHeightOfCylinder, 0);
+	float scaled_radius = scale * mRadius;
+
+	// Get support point for top and bottom sphere in the opposite of 'direction' (including convex radius)
+	Vec3 support = (scaled_radius / len) * direction;
+	Vec3 support_top = scaled_half_height_of_cylinder - support;
+	Vec3 support_bottom = -scaled_half_height_of_cylinder - support;
+
+	// Get projection on inDirection
+	// Note that inDirection is not normalized, so we need to divide by inDirection.Length() to get the actual projection
+	// We've multiplied both sides of the if below with inDirection.Length()
+	float proj_top = support_top.Dot(inDirection);
+	float proj_bottom = support_bottom.Dot(inDirection);
+
+	// If projection is roughly equal then return line, otherwise we return nothing as there's only 1 point
+	if (abs(proj_top - proj_bottom) < cCapsuleProjectionSlop * inDirection.Length())
+	{
+		outVertices.push_back(inCenterOfMassTransform * support_top);
+		outVertices.push_back(inCenterOfMassTransform * support_bottom);
+	}
+}
+
+MassProperties CapsuleShape::GetMassProperties() const
+{
+	MassProperties p;
+
+	float density = GetDensity();
+
+	// Calculate inertia and mass according to:
+	// https://www.gamedev.net/resources/_/technical/math-and-physics/capsule-inertia-tensor-r3856
+	// Note that there is an error in eq 14, H^2/2 should be H^2/4 in Ixx and Izz, eq 12 does contain the correct value
+	float radius_sq = Square(mRadius);
+	float height = 2.0f * mHalfHeightOfCylinder;
+	float cylinder_mass = JPH_PI * height * radius_sq * density;
+	float hemisphere_mass = (2.0f * JPH_PI / 3.0f) * radius_sq * mRadius * density;
+
+	// From cylinder
+	float height_sq = Square(height);
+	float inertia_y = radius_sq * cylinder_mass * 0.5f;
+	float inertia_xz = inertia_y * 0.5f + cylinder_mass * height_sq / 12.0f;
+
+	// From hemispheres
+	float temp = hemisphere_mass * 4.0f * radius_sq / 5.0f;
+	inertia_y += temp;
+	inertia_xz += temp + hemisphere_mass * (0.5f * height_sq + (3.0f / 4.0f) * height * mRadius);
+
+	// Mass is cylinder + hemispheres
+	p.mMass = cylinder_mass + hemisphere_mass * 2.0f;
+
+	// Set inertia
+	p.mInertia = Mat44::sScale(Vec3(inertia_xz, inertia_y, inertia_xz));
+
+	return p;
+}
+
+Vec3 CapsuleShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const
+{
+	JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID");
+
+	if (inLocalSurfacePosition.GetY() > mHalfHeightOfCylinder)
+		return (inLocalSurfacePosition - Vec3(0, mHalfHeightOfCylinder, 0)).Normalized();
+	else if (inLocalSurfacePosition.GetY() < -mHalfHeightOfCylinder)
+		return (inLocalSurfacePosition - Vec3(0, -mHalfHeightOfCylinder, 0)).Normalized();
+	else
+		return Vec3(inLocalSurfacePosition.GetX(), 0, inLocalSurfacePosition.GetZ()).NormalizedOr(Vec3::sAxisX());
+}
+
+AABox CapsuleShape::GetLocalBounds() const
+{
+	Vec3 extent = Vec3::sReplicate(mRadius) + Vec3(0, mHalfHeightOfCylinder, 0);
+	return AABox(-extent, extent);
+}
+
+AABox CapsuleShape::GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const
+{
+	JPH_ASSERT(IsValidScale(inScale));
+
+	Vec3 abs_scale = inScale.Abs();
+	float scale = abs_scale.GetX();
+	Vec3 extent = Vec3::sReplicate(scale * mRadius);
+	Vec3 height = Vec3(0, scale * mHalfHeightOfCylinder, 0);
+	Vec3 p1 = inCenterOfMassTransform * -height;
+	Vec3 p2 = inCenterOfMassTransform * height;
+	return AABox(Vec3::sMin(p1, p2) - extent, Vec3::sMax(p1, p2) + extent);
+}
+
+#ifdef JPH_DEBUG_RENDERER
+void CapsuleShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
+{
+	DebugRenderer::EDrawMode draw_mode = inDrawWireframe? DebugRenderer::EDrawMode::Wireframe : DebugRenderer::EDrawMode::Solid;
+	inRenderer->DrawCapsule(inCenterOfMassTransform * Mat44::sScale(inScale.Abs().GetX()), mHalfHeightOfCylinder, mRadius, inUseMaterialColors? GetMaterial()->GetDebugColor() : inColor, DebugRenderer::ECastShadow::On, draw_mode);
+}
+#endif // JPH_DEBUG_RENDERER
+
+bool CapsuleShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const
+{
+	// Test ray against capsule
+	float fraction = RayCapsule(inRay.mOrigin, inRay.mDirection, mHalfHeightOfCylinder, mRadius);
+	if (fraction < ioHit.mFraction)
+	{
+		ioHit.mFraction = fraction;
+		ioHit.mSubShapeID2 = inSubShapeIDCreator.GetID();
+		return true;
+	}
+	return false;
+}
+
+void CapsuleShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	// Test shape filter
+	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
+		return;
+
+	float radius_sq = Square(mRadius);
+
+	// Get vertical distance to the top/bottom sphere centers
+	float delta_y = abs(inPoint.GetY()) - mHalfHeightOfCylinder;
+
+	// Get distance in horizontal plane
+	float xz_sq = Square(inPoint.GetX()) + Square(inPoint.GetZ());
+
+	// Check if the point is in one of the two spheres
+	bool in_sphere = xz_sq + Square(delta_y) <= radius_sq;
+
+	// Check if the point is in the cylinder in the middle
+	bool in_cylinder = delta_y <= 0.0f && xz_sq <= radius_sq;
+
+	if (in_sphere || in_cylinder)
+		ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator.GetID() });
+}
+
+void CapsuleShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const
+{
+	JPH_ASSERT(IsValidScale(inScale));
+
+	Mat44 inverse_transform = inCenterOfMassTransform.InversedRotationTranslation();
+
+	// Get scaled capsule
+	float scale = abs(inScale.GetX());
+	float half_height_of_cylinder = scale * mHalfHeightOfCylinder;
+	float radius = scale * mRadius;
+
+	for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v)
+		if (v.GetInvMass() > 0.0f)
+		{
+			// Calculate penetration
+			Vec3 local_pos = inverse_transform * v.GetPosition();
+			if (abs(local_pos.GetY()) <= half_height_of_cylinder)
+			{
+				// Near cylinder
+				Vec3 normal = local_pos;
+				normal.SetY(0.0f);
+				float normal_length = normal.Length();
+				float penetration = radius - normal_length;
+				if (v.UpdatePenetration(penetration))
+				{
+					// Calculate contact point and normal
+					normal = normal_length > 0.0f? normal / normal_length : Vec3::sAxisX();
+					Vec3 point = radius * normal;
+
+					// Store collision
+					v.SetCollision(Plane::sFromPointAndNormal(point, normal).GetTransformed(inCenterOfMassTransform), inCollidingShapeIndex);
+				}
+			}
+			else
+			{
+				// Near cap
+				Vec3 center = Vec3(0, Sign(local_pos.GetY()) * half_height_of_cylinder, 0);
+				Vec3 delta = local_pos - center;
+				float distance = delta.Length();
+				float penetration = radius - distance;
+				if (v.UpdatePenetration(penetration))
+				{
+					// Calculate contact point and normal
+					Vec3 normal = delta / distance;
+					Vec3 point = center + radius * normal;
+
+					// Store collision
+					v.SetCollision(Plane::sFromPointAndNormal(point, normal).GetTransformed(inCenterOfMassTransform), inCollidingShapeIndex);
+				}
+			}
+		}
+}
+
+void CapsuleShape::GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const
+{
+	JPH_ASSERT(IsValidScale(inScale));
+
+	Vec3 abs_scale = inScale.Abs();
+	float scale = abs_scale.GetX();
+
+	GetTrianglesContextMultiVertexList *context = new (&ioContext) GetTrianglesContextMultiVertexList(false, GetMaterial());
+
+	Mat44 world_matrix = Mat44::sRotationTranslation(inRotation, inPositionCOM) * Mat44::sScale(scale);
+
+	Mat44 top_matrix = world_matrix * Mat44(Vec4(mRadius, 0, 0, 0), Vec4(0, mRadius, 0, 0), Vec4(0, 0, mRadius, 0), Vec4(0, mHalfHeightOfCylinder, 0, 1));
+	context->AddPart(top_matrix, sCapsuleTopTriangles.data(), sCapsuleTopTriangles.size());
+
+	Mat44 middle_matrix = world_matrix * Mat44::sScale(Vec3(mRadius, mHalfHeightOfCylinder, mRadius));
+	context->AddPart(middle_matrix, sCapsuleMiddleTriangles.data(), sCapsuleMiddleTriangles.size());
+
+	Mat44 bottom_matrix = world_matrix * Mat44(Vec4(mRadius, 0, 0, 0), Vec4(0, mRadius, 0, 0), Vec4(0, 0, mRadius, 0), Vec4(0, -mHalfHeightOfCylinder, 0, 1));
+	context->AddPart(bottom_matrix, sCapsuleBottomTriangles.data(), sCapsuleBottomTriangles.size());
+}
+
+int CapsuleShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) const
+{
+	return ((GetTrianglesContextMultiVertexList &)ioContext).GetTrianglesNext(inMaxTrianglesRequested, outTriangleVertices, outMaterials);
+}
+
+void CapsuleShape::SaveBinaryState(StreamOut &inStream) const
+{
+	ConvexShape::SaveBinaryState(inStream);
+
+	inStream.Write(mRadius);
+	inStream.Write(mHalfHeightOfCylinder);
+}
+
+void CapsuleShape::RestoreBinaryState(StreamIn &inStream)
+{
+	ConvexShape::RestoreBinaryState(inStream);
+
+	inStream.Read(mRadius);
+	inStream.Read(mHalfHeightOfCylinder);
+}
+
+bool CapsuleShape::IsValidScale(Vec3Arg inScale) const
+{
+	return ConvexShape::IsValidScale(inScale) && ScaleHelpers::IsUniformScale(inScale.Abs());
+}
+
+Vec3 CapsuleShape::MakeScaleValid(Vec3Arg inScale) const
+{
+	Vec3 scale = ScaleHelpers::MakeNonZeroScale(inScale);
+
+	return scale.GetSign() * ScaleHelpers::MakeUniformScale(scale.Abs());
+}
+
+void CapsuleShape::sRegister()
+{
+	ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::Capsule);
+	f.mConstruct = []() -> Shape * { return new CapsuleShape; };
+	f.mColor = Color::sGreen;
+}
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CapsuleShape.h

@@ -0,0 +1,129 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/Shape/ConvexShape.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Class that constructs a CapsuleShape
+class JPH_EXPORT CapsuleShapeSettings final : public ConvexShapeSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, CapsuleShapeSettings)
+
+public:
+	/// Default constructor for deserialization
+							CapsuleShapeSettings() = default;
+
+	/// Create a capsule centered around the origin with one sphere cap at (0, -inHalfHeightOfCylinder, 0) and the other at (0, inHalfHeightOfCylinder, 0)
+							CapsuleShapeSettings(float inHalfHeightOfCylinder, float inRadius, const PhysicsMaterial *inMaterial = nullptr) : ConvexShapeSettings(inMaterial), mRadius(inRadius), mHalfHeightOfCylinder(inHalfHeightOfCylinder) { }
+
+	/// Check if this is a valid capsule shape
+	bool					IsValid() const															{ return mRadius > 0.0f && mHalfHeightOfCylinder >= 0.0f; }
+
+	/// Checks if the settings of this capsule make this shape a sphere
+	bool					IsSphere() const														{ return mHalfHeightOfCylinder == 0.0f; }
+
+	// See: ShapeSettings
+	virtual ShapeResult		Create() const override;
+
+	float					mRadius = 0.0f;
+	float					mHalfHeightOfCylinder = 0.0f;
+};
+
+/// A capsule, implemented as a line segment with convex radius
+class JPH_EXPORT CapsuleShape final : public ConvexShape
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+							CapsuleShape() : ConvexShape(EShapeSubType::Capsule) { }
+							CapsuleShape(const CapsuleShapeSettings &inSettings, ShapeResult &outResult);
+
+	/// Create a capsule centered around the origin with one sphere cap at (0, -inHalfHeightOfCylinder, 0) and the other at (0, inHalfHeightOfCylinder, 0)
+							CapsuleShape(float inHalfHeightOfCylinder, float inRadius, const PhysicsMaterial *inMaterial = nullptr) : ConvexShape(EShapeSubType::Capsule, inMaterial), mRadius(inRadius), mHalfHeightOfCylinder(inHalfHeightOfCylinder) { JPH_ASSERT(inHalfHeightOfCylinder > 0.0f); JPH_ASSERT(inRadius > 0.0f); }
+
+	/// Radius of the cylinder
+	float					GetRadius() const														{ return mRadius; }
+
+	/// Get half of the height of the cylinder
+	float					GetHalfHeightOfCylinder() const											{ return mHalfHeightOfCylinder; }
+
+	// See Shape::GetLocalBounds
+	virtual AABox			GetLocalBounds() const override;
+
+	// See Shape::GetWorldSpaceBounds
+	virtual AABox			GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override;
+	using Shape::GetWorldSpaceBounds;
+
+	// See Shape::GetInnerRadius
+	virtual float			GetInnerRadius() const override											{ return mRadius; }
+
+	// See Shape::GetMassProperties
+	virtual MassProperties	GetMassProperties() const override;
+
+	// See Shape::GetSurfaceNormal
+	virtual Vec3			GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override;
+
+	// See Shape::GetSupportingFace
+	virtual void			GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override;
+
+	// See ConvexShape::GetSupportFunction
+	virtual const Support *	GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const override;
+
+#ifdef JPH_DEBUG_RENDERER
+	// See Shape::Draw
+	virtual void			Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override;
+#endif // JPH_DEBUG_RENDERER
+
+	// See Shape::CastRay
+	using ConvexShape::CastRay;
+	virtual bool			CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override;
+
+	// See: Shape::CollidePoint
+	virtual void			CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
+
+	// See: Shape::CollideSoftBodyVertices
+	virtual void			CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override;
+
+	// See Shape::GetTrianglesStart
+	virtual void			GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override;
+
+	// See Shape::GetTrianglesNext
+	virtual int				GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override;
+
+	// See Shape
+	virtual void			SaveBinaryState(StreamOut &inStream) const override;
+
+	// See Shape::GetStats
+	virtual Stats			GetStats() const override												{ return Stats(sizeof(*this), 0); }
+
+	// See Shape::GetVolume
+	virtual float			GetVolume() const override												{ return 4.0f / 3.0f * JPH_PI * Cubed(mRadius) + 2.0f * JPH_PI * mHalfHeightOfCylinder * Square(mRadius); }
+
+	// See Shape::IsValidScale
+	virtual bool			IsValidScale(Vec3Arg inScale) const override;
+
+	// See Shape::MakeScaleValid
+	virtual Vec3			MakeScaleValid(Vec3Arg inScale) const override;
+
+	// Register shape functions with the registry
+	static void				sRegister();
+
+protected:
+	// See: Shape::RestoreBinaryState
+	virtual void			RestoreBinaryState(StreamIn &inStream) override;
+
+private:
+	// Classes for GetSupportFunction
+	class					CapsuleNoConvex;
+	class					CapsuleWithConvex;
+
+	float					mRadius = 0.0f;
+	float					mHalfHeightOfCylinder = 0.0f;
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CompoundShape.cpp

@@ -0,0 +1,433 @@
+// 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/Shape/CompoundShape.h>
+#include <Jolt/Physics/Collision/CollisionDispatch.h>
+#include <Jolt/Physics/Collision/ShapeCast.h>
+#include <Jolt/Physics/Collision/CastResult.h>
+#include <Jolt/Physics/Collision/TransformedShape.h>
+#include <Jolt/Core/Profiler.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT(CompoundShapeSettings)
+{
+	JPH_ADD_BASE_CLASS(CompoundShapeSettings, ShapeSettings)
+
+	JPH_ADD_ATTRIBUTE(CompoundShapeSettings, mSubShapes)
+}
+
+JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(CompoundShapeSettings::SubShapeSettings)
+{
+	JPH_ADD_ATTRIBUTE(CompoundShapeSettings::SubShapeSettings, mShape)
+	JPH_ADD_ATTRIBUTE(CompoundShapeSettings::SubShapeSettings, mPosition)
+	JPH_ADD_ATTRIBUTE(CompoundShapeSettings::SubShapeSettings, mRotation)
+	JPH_ADD_ATTRIBUTE(CompoundShapeSettings::SubShapeSettings, mUserData)
+}
+
+void CompoundShapeSettings::AddShape(Vec3Arg inPosition, QuatArg inRotation, const ShapeSettings *inShape, uint32 inUserData)
+{
+	// Add shape
+	SubShapeSettings shape;
+	shape.mPosition = inPosition;
+	shape.mRotation = inRotation;
+	shape.mShape = inShape;
+	shape.mUserData = inUserData;
+	mSubShapes.push_back(shape);
+}
+
+void CompoundShapeSettings::AddShape(Vec3Arg inPosition, QuatArg inRotation, const Shape *inShape, uint32 inUserData)
+{
+	// Add shape
+	SubShapeSettings shape;
+	shape.mPosition = inPosition;
+	shape.mRotation = inRotation;
+	shape.mShapePtr = inShape;
+	shape.mUserData = inUserData;
+	mSubShapes.push_back(shape);
+}
+
+bool CompoundShape::MustBeStatic() const
+{
+	for (const SubShape &shape : mSubShapes)
+		if (shape.mShape->MustBeStatic())
+			return true;
+
+	return false;
+}
+
+MassProperties CompoundShape::GetMassProperties() const
+{
+	MassProperties p;
+
+	// Calculate mass and inertia
+	p.mMass = 0.0f;
+	p.mInertia = Mat44::sZero();
+	for (const SubShape &shape : mSubShapes)
+	{
+		// Rotate and translate inertia of child into place
+		MassProperties child = shape.mShape->GetMassProperties();
+		child.Rotate(Mat44::sRotation(shape.GetRotation()));
+		child.Translate(shape.GetPositionCOM());
+
+		// Accumulate mass and inertia
+		p.mMass += child.mMass;
+		p.mInertia += child.mInertia;
+	}
+
+	// Ensure that inertia is a 3x3 matrix, adding inertias causes the bottom right element to change
+	p.mInertia.SetColumn4(3, Vec4(0, 0, 0, 1));
+
+	return p;
+}
+
+AABox CompoundShape::GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const
+{
+	if (mSubShapes.empty())
+	{
+		// If there are no sub-shapes, we must return an empty box to avoid overflows in the broadphase
+		return AABox(inCenterOfMassTransform.GetTranslation(), inCenterOfMassTransform.GetTranslation());
+	}
+	else if (mSubShapes.size() <= 10)
+	{
+		AABox bounds;
+		for (const SubShape &shape : mSubShapes)
+		{
+			Mat44 transform = inCenterOfMassTransform * shape.GetLocalTransformNoScale(inScale);
+			bounds.Encapsulate(shape.mShape->GetWorldSpaceBounds(transform, shape.TransformScale(inScale)));
+		}
+		return bounds;
+	}
+	else
+	{
+		// If there are too many shapes, use the base class function (this will result in a slightly wider bounding box)
+		return Shape::GetWorldSpaceBounds(inCenterOfMassTransform, inScale);
+	}
+}
+
+uint CompoundShape::GetSubShapeIDBitsRecursive() const
+{
+	// Add max of child bits to our bits
+	uint child_bits = 0;
+	for (const SubShape &shape : mSubShapes)
+		child_bits = max(child_bits, shape.mShape->GetSubShapeIDBitsRecursive());
+	return child_bits + GetSubShapeIDBits();
+}
+
+const PhysicsMaterial *CompoundShape::GetMaterial(const SubShapeID &inSubShapeID) const
+{
+	// Decode sub shape index
+	SubShapeID remainder;
+	uint32 index = GetSubShapeIndexFromID(inSubShapeID, remainder);
+
+	// Pass call on
+	return mSubShapes[index].mShape->GetMaterial(remainder);
+}
+
+const Shape *CompoundShape::GetLeafShape(const SubShapeID &inSubShapeID, SubShapeID &outRemainder) const
+{
+	// Decode sub shape index
+	SubShapeID remainder;
+	uint32 index = GetSubShapeIndexFromID(inSubShapeID, remainder);
+	if (index >= mSubShapes.size())
+	{
+		// No longer valid index
+		outRemainder = SubShapeID();
+		return nullptr;
+	}
+
+	// Pass call on
+	return mSubShapes[index].mShape->GetLeafShape(remainder, outRemainder);
+}
+
+uint64 CompoundShape::GetSubShapeUserData(const SubShapeID &inSubShapeID) const
+{
+	// Decode sub shape index
+	SubShapeID remainder;
+	uint32 index = GetSubShapeIndexFromID(inSubShapeID, remainder);
+	if (index >= mSubShapes.size())
+		return 0; // No longer valid index
+
+	// Pass call on
+	return mSubShapes[index].mShape->GetSubShapeUserData(remainder);
+}
+
+TransformedShape CompoundShape::GetSubShapeTransformedShape(const SubShapeID &inSubShapeID, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, SubShapeID &outRemainder) const
+{
+	// Get the sub shape
+	const SubShape &sub_shape = mSubShapes[GetSubShapeIndexFromID(inSubShapeID, outRemainder)];
+
+	// Calculate transform for sub shape
+	Vec3 position = inPositionCOM + inRotation * (inScale * sub_shape.GetPositionCOM());
+	Quat rotation = inRotation * sub_shape.GetRotation();
+	Vec3 scale = sub_shape.TransformScale(inScale);
+
+	// Return transformed shape
+	TransformedShape ts(RVec3(position), rotation, sub_shape.mShape, BodyID());
+	ts.SetShapeScale(scale);
+	return ts;
+}
+
+Vec3 CompoundShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const
+{
+	// Decode sub shape index
+	SubShapeID remainder;
+	uint32 index = GetSubShapeIndexFromID(inSubShapeID, remainder);
+
+	// Transform surface position to local space and pass call on
+	const SubShape &shape = mSubShapes[index];
+	Mat44 transform = Mat44::sInverseRotationTranslation(shape.GetRotation(), shape.GetPositionCOM());
+	Vec3 normal = shape.mShape->GetSurfaceNormal(remainder, transform * inLocalSurfacePosition);
+
+	// Transform normal to this shape's space
+	return transform.Multiply3x3Transposed(normal);
+}
+
+void CompoundShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const
+{
+	// Decode sub shape index
+	SubShapeID remainder;
+	uint32 index = GetSubShapeIndexFromID(inSubShapeID, remainder);
+
+	// Apply transform and pass on to sub shape
+	const SubShape &shape = mSubShapes[index];
+	Mat44 transform = shape.GetLocalTransformNoScale(inScale);
+	shape.mShape->GetSupportingFace(remainder, transform.Multiply3x3Transposed(inDirection), shape.TransformScale(inScale), inCenterOfMassTransform * transform, outVertices);
+}
+
+void CompoundShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const
+{
+	outTotalVolume = 0.0f;
+	outSubmergedVolume = 0.0f;
+	outCenterOfBuoyancy = Vec3::sZero();
+
+	for (const SubShape &shape : mSubShapes)
+	{
+		// Get center of mass transform of child
+		Mat44 transform = inCenterOfMassTransform * shape.GetLocalTransformNoScale(inScale);
+
+		// Recurse to child
+		float total_volume, submerged_volume;
+		Vec3 center_of_buoyancy;
+		shape.mShape->GetSubmergedVolume(transform, shape.TransformScale(inScale), inSurface, total_volume, submerged_volume, center_of_buoyancy JPH_IF_DEBUG_RENDERER(, inBaseOffset));
+
+		// Accumulate volumes
+		outTotalVolume += total_volume;
+		outSubmergedVolume += submerged_volume;
+
+		// The center of buoyancy is the weighted average of the center of buoyancy of our child shapes
+		outCenterOfBuoyancy += submerged_volume * center_of_buoyancy;
+	}
+
+	if (outSubmergedVolume > 0.0f)
+		outCenterOfBuoyancy /= outSubmergedVolume;
+
+#ifdef JPH_DEBUG_RENDERER
+	// Draw center of buoyancy
+	if (sDrawSubmergedVolumes)
+		DebugRenderer::sInstance->DrawWireSphere(inBaseOffset + outCenterOfBuoyancy, 0.05f, Color::sRed, 1);
+#endif // JPH_DEBUG_RENDERER
+}
+
+#ifdef JPH_DEBUG_RENDERER
+void CompoundShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
+{
+	for (const SubShape &shape : mSubShapes)
+	{
+		Mat44 transform = shape.GetLocalTransformNoScale(inScale);
+		shape.mShape->Draw(inRenderer, inCenterOfMassTransform * transform, shape.TransformScale(inScale), inColor, inUseMaterialColors, inDrawWireframe);
+	}
+}
+
+void CompoundShape::DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const
+{
+	for (const SubShape &shape : mSubShapes)
+	{
+		Mat44 transform = shape.GetLocalTransformNoScale(inScale);
+		shape.mShape->DrawGetSupportFunction(inRenderer, inCenterOfMassTransform * transform, shape.TransformScale(inScale), inColor, inDrawSupportDirection);
+	}
+}
+
+void CompoundShape::DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const
+{
+	for (const SubShape &shape : mSubShapes)
+	{
+		Mat44 transform = shape.GetLocalTransformNoScale(inScale);
+		shape.mShape->DrawGetSupportingFace(inRenderer, inCenterOfMassTransform * transform, shape.TransformScale(inScale));
+	}
+}
+#endif // JPH_DEBUG_RENDERER
+
+void CompoundShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const
+{
+	for (const SubShape &shape : mSubShapes)
+	{
+		Mat44 transform = shape.GetLocalTransformNoScale(inScale);
+		shape.mShape->CollideSoftBodyVertices(inCenterOfMassTransform * transform, shape.TransformScale(inScale), inVertices, inNumVertices, inCollidingShapeIndex);
+	}
+}
+
+void CompoundShape::TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const
+{
+	for (const SubShape &shape : mSubShapes)
+		shape.mShape->TransformShape(inCenterOfMassTransform * Mat44::sRotationTranslation(shape.GetRotation(), shape.GetPositionCOM()), ioCollector);
+}
+
+void CompoundShape::sCastCompoundVsShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Fetch compound shape from cast shape
+	JPH_ASSERT(inShapeCast.mShape->GetType() == EShapeType::Compound);
+	const CompoundShape *compound = static_cast<const CompoundShape *>(inShapeCast.mShape);
+
+	// Number of sub shapes
+	int n = (int)compound->mSubShapes.size();
+
+	// Determine amount of bits for sub shape
+	uint sub_shape_bits = compound->GetSubShapeIDBits();
+
+	// Recurse to sub shapes
+	for (int i = 0; i < n; ++i)
+	{
+		const SubShape &shape = compound->mSubShapes[i];
+
+		// Create ID for sub shape
+		SubShapeIDCreator shape1_sub_shape_id = inSubShapeIDCreator1.PushID(i, sub_shape_bits);
+
+		// Transform the shape cast and update the shape
+		Mat44 transform = inShapeCast.mCenterOfMassStart * shape.GetLocalTransformNoScale(inShapeCast.mScale);
+		Vec3 scale = shape.TransformScale(inShapeCast.mScale);
+		ShapeCast shape_cast(shape.mShape, scale, transform, inShapeCast.mDirection);
+
+		CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, inShapeCastSettings, inShape, inScale, inShapeFilter, inCenterOfMassTransform2, shape1_sub_shape_id, inSubShapeIDCreator2, ioCollector);
+
+		if (ioCollector.ShouldEarlyOut())
+			break;
+	}
+}
+
+void CompoundShape::SaveBinaryState(StreamOut &inStream) const
+{
+	Shape::SaveBinaryState(inStream);
+
+	inStream.Write(mCenterOfMass);
+	inStream.Write(mLocalBounds.mMin);
+	inStream.Write(mLocalBounds.mMax);
+	inStream.Write(mInnerRadius);
+
+	// Write sub shapes
+	inStream.Write(mSubShapes, [](const SubShape &inElement, StreamOut &inS) {
+		inS.Write(inElement.mUserData);
+		inS.Write(inElement.mPositionCOM);
+		inS.Write(inElement.mRotation);
+	});
+}
+
+void CompoundShape::RestoreBinaryState(StreamIn &inStream)
+{
+	Shape::RestoreBinaryState(inStream);
+
+	inStream.Read(mCenterOfMass);
+	inStream.Read(mLocalBounds.mMin);
+	inStream.Read(mLocalBounds.mMax);
+	inStream.Read(mInnerRadius);
+
+	// Read sub shapes
+	inStream.Read(mSubShapes, [](StreamIn &inS, SubShape &outElement) {
+		inS.Read(outElement.mUserData);
+		inS.Read(outElement.mPositionCOM);
+		inS.Read(outElement.mRotation);
+		outElement.mIsRotationIdentity = outElement.mRotation == Float3(0, 0, 0);
+	});
+}
+
+void CompoundShape::SaveSubShapeState(ShapeList &outSubShapes) const
+{
+	outSubShapes.clear();
+	outSubShapes.reserve(mSubShapes.size());
+	for (const SubShape &shape : mSubShapes)
+		outSubShapes.push_back(shape.mShape);
+}
+
+void CompoundShape::RestoreSubShapeState(const ShapeRefC *inSubShapes, uint inNumShapes)
+{
+	JPH_ASSERT(mSubShapes.size() == inNumShapes);
+	for (uint i = 0; i < inNumShapes; ++i)
+		mSubShapes[i].mShape = inSubShapes[i];
+}
+
+Shape::Stats CompoundShape::GetStatsRecursive(VisitedShapes &ioVisitedShapes) const
+{
+	// Get own stats
+	Stats stats = Shape::GetStatsRecursive(ioVisitedShapes);
+
+	// Add child stats
+	for (const SubShape &shape : mSubShapes)
+	{
+		Stats child_stats = shape.mShape->GetStatsRecursive(ioVisitedShapes);
+		stats.mSizeBytes += child_stats.mSizeBytes;
+		stats.mNumTriangles += child_stats.mNumTriangles;
+	}
+
+	return stats;
+}
+
+float CompoundShape::GetVolume() const
+{
+	float volume = 0.0f;
+	for (const SubShape &shape : mSubShapes)
+		volume += shape.mShape->GetVolume();
+	return volume;
+}
+
+bool CompoundShape::IsValidScale(Vec3Arg inScale) const
+{
+	if (!Shape::IsValidScale(inScale))
+		return false;
+
+	for (const SubShape &shape : mSubShapes)
+	{
+		// Test if the scale is non-uniform and the shape is rotated
+		if (!shape.IsValidScale(inScale))
+			return false;
+
+		// Test the child shape
+		if (!shape.mShape->IsValidScale(shape.TransformScale(inScale)))
+			return false;
+	}
+
+	return true;
+}
+
+Vec3 CompoundShape::MakeScaleValid(Vec3Arg inScale) const
+{
+	Vec3 scale = ScaleHelpers::MakeNonZeroScale(inScale);
+	if (CompoundShape::IsValidScale(scale))
+		return scale;
+
+	Vec3 abs_uniform_scale = ScaleHelpers::MakeUniformScale(scale.Abs());
+	Vec3 uniform_scale = scale.GetSign() * abs_uniform_scale;
+	if (CompoundShape::IsValidScale(uniform_scale))
+		return uniform_scale;
+
+	return Sign(scale.GetX()) * abs_uniform_scale;
+}
+
+void CompoundShape::sRegister()
+{
+	for (EShapeSubType s1 : sCompoundSubShapeTypes)
+		for (EShapeSubType s2 : sAllSubShapeTypes)
+			CollisionDispatch::sRegisterCastShape(s1, s2, sCastCompoundVsShape);
+}
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CompoundShape.h

@@ -0,0 +1,354 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/Shape/Shape.h>
+#include <Jolt/Physics/Collision/Shape/ScaleHelpers.h>
+#include <Jolt/Physics/Collision/Shape/SubShapeID.h>
+
+JPH_NAMESPACE_BEGIN
+
+class CollideShapeSettings;
+class OrientedBox;
+
+/// Base class settings to construct a compound shape
+class JPH_EXPORT CompoundShapeSettings : public ShapeSettings
+{
+	JPH_DECLARE_SERIALIZABLE_ABSTRACT(JPH_EXPORT, CompoundShapeSettings)
+
+public:
+	/// Constructor. Use AddShape to add the parts.
+									CompoundShapeSettings() = default;
+
+	/// Add a shape to the compound.
+	void							AddShape(Vec3Arg inPosition, QuatArg inRotation, const ShapeSettings *inShape, uint32 inUserData = 0);
+
+	/// Add a shape to the compound. Variant that uses a concrete shape, which means this object cannot be serialized.
+	void							AddShape(Vec3Arg inPosition, QuatArg inRotation, const Shape *inShape, uint32 inUserData = 0);
+
+	struct SubShapeSettings
+	{
+		JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, SubShapeSettings)
+
+		RefConst<ShapeSettings>		mShape;													///< Sub shape (either this or mShapePtr needs to be filled up)
+		RefConst<Shape>				mShapePtr;												///< Sub shape (either this or mShape needs to be filled up)
+		Vec3						mPosition;												///< Position of the sub shape
+		Quat						mRotation;												///< Rotation of the sub shape
+
+		/// User data value (can be used by the application for any purpose).
+		/// Note this value can be retrieved through GetSubShape(...).mUserData, not through GetSubShapeUserData(...) as that returns Shape::GetUserData() of the leaf shape.
+		/// Use GetSubShapeIndexFromID get a shape index from a SubShapeID to pass to GetSubShape.
+		uint32						mUserData = 0;
+	};
+
+	using SubShapes = Array<SubShapeSettings>;
+
+	SubShapes						mSubShapes;
+};
+
+/// Base class for a compound shape
+class JPH_EXPORT CompoundShape : public Shape
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+	explicit						CompoundShape(EShapeSubType inSubType) : Shape(EShapeType::Compound, inSubType) { }
+									CompoundShape(EShapeSubType inSubType, const ShapeSettings &inSettings, ShapeResult &outResult) : Shape(EShapeType::Compound, inSubType, inSettings, outResult) { }
+
+	// See Shape::GetCenterOfMass
+	virtual Vec3					GetCenterOfMass() const override						{ return mCenterOfMass; }
+
+	// See Shape::MustBeStatic
+	virtual bool					MustBeStatic() const override;
+
+	// See Shape::GetLocalBounds
+	virtual AABox					GetLocalBounds() const override							{ return mLocalBounds; }
+
+	// See Shape::GetSubShapeIDBitsRecursive
+	virtual uint					GetSubShapeIDBitsRecursive() const override;
+
+	// See Shape::GetWorldSpaceBounds
+	virtual AABox					GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override;
+	using Shape::GetWorldSpaceBounds;
+
+	// See Shape::GetInnerRadius
+	virtual float					GetInnerRadius() const override							{ return mInnerRadius; }
+
+	// See Shape::GetMassProperties
+	virtual MassProperties			GetMassProperties() const override;
+
+	// See Shape::GetMaterial
+	virtual const PhysicsMaterial *	GetMaterial(const SubShapeID &inSubShapeID) const override;
+
+	// See Shape::GetLeafShape
+	virtual const Shape *			GetLeafShape(const SubShapeID &inSubShapeID, SubShapeID &outRemainder) const override;
+
+	// See Shape::GetSubShapeUserData
+	virtual uint64					GetSubShapeUserData(const SubShapeID &inSubShapeID) const override;
+
+	// See Shape::GetSubShapeTransformedShape
+	virtual TransformedShape		GetSubShapeTransformedShape(const SubShapeID &inSubShapeID, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, SubShapeID &outRemainder) const override;
+
+	// See Shape::GetSurfaceNormal
+	virtual Vec3					GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override;
+
+	// See Shape::GetSupportingFace
+	virtual void					GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override;
+
+	// See Shape::GetSubmergedVolume
+	virtual void					GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const override;
+
+#ifdef JPH_DEBUG_RENDERER
+	// See Shape::Draw
+	virtual void					Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override;
+
+	// See Shape::DrawGetSupportFunction
+	virtual void					DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const override;
+
+	// See Shape::DrawGetSupportingFace
+	virtual void					DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override;
+#endif // JPH_DEBUG_RENDERER
+
+	// See: Shape::CollideSoftBodyVertices
+	virtual void					CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override;
+
+	// See Shape::TransformShape
+	virtual void					TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const override;
+
+	// See Shape::GetTrianglesStart
+	virtual void					GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override { JPH_ASSERT(false, "Cannot call on non-leaf shapes, use CollectTransformedShapes to collect the leaves first!"); }
+
+	// See Shape::GetTrianglesNext
+	virtual int						GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override { JPH_ASSERT(false, "Cannot call on non-leaf shapes, use CollectTransformedShapes to collect the leaves first!"); return 0; }
+
+	/// Get which sub shape's bounding boxes overlap with an axis aligned box
+	/// @param inBox The axis aligned box to test against (relative to the center of mass of this shape)
+	/// @param outSubShapeIndices Buffer where to place the indices of the sub shapes that intersect
+	/// @param inMaxSubShapeIndices How many indices will fit in the buffer (normally you'd provide a buffer of GetNumSubShapes() indices)
+	/// @return How many indices were placed in outSubShapeIndices
+	virtual int						GetIntersectingSubShapes(const AABox &inBox, uint *outSubShapeIndices, int inMaxSubShapeIndices) const = 0;
+
+	/// Get which sub shape's bounding boxes overlap with an axis aligned box
+	/// @param inBox The axis aligned box to test against (relative to the center of mass of this shape)
+	/// @param outSubShapeIndices Buffer where to place the indices of the sub shapes that intersect
+	/// @param inMaxSubShapeIndices How many indices will fit in the buffer (normally you'd provide a buffer of GetNumSubShapes() indices)
+	/// @return How many indices were placed in outSubShapeIndices
+	virtual int						GetIntersectingSubShapes(const OrientedBox &inBox, uint *outSubShapeIndices, int inMaxSubShapeIndices) const = 0;
+
+	struct SubShape
+	{
+		/// Initialize sub shape from sub shape settings
+		/// @param inSettings Settings object
+		/// @param outResult Result object, only used in case of error
+		/// @return True on success, false on failure
+		bool						FromSettings(const CompoundShapeSettings::SubShapeSettings &inSettings, ShapeResult &outResult)
+		{
+			if (inSettings.mShapePtr != nullptr)
+			{
+				// Use provided shape
+				mShape = inSettings.mShapePtr;
+			}
+			else
+			{
+				// Create child shape
+				ShapeResult child_result = inSettings.mShape->Create();
+				if (!child_result.IsValid())
+				{
+					outResult = child_result;
+					return false;
+				}
+				mShape = child_result.Get();
+			}
+
+			// Copy user data
+			mUserData = inSettings.mUserData;
+
+			SetTransform(inSettings.mPosition, inSettings.mRotation, Vec3::sZero() /* Center of mass not yet calculated */);
+			return true;
+		}
+
+		/// Update the transform of this sub shape
+		/// @param inPosition New position
+		/// @param inRotation New orientation
+		/// @param inCenterOfMass The center of mass of the compound shape
+		JPH_INLINE void				SetTransform(Vec3Arg inPosition, QuatArg inRotation, Vec3Arg inCenterOfMass)
+		{
+			SetPositionCOM(inPosition - inCenterOfMass + inRotation * mShape->GetCenterOfMass());
+
+			mIsRotationIdentity = inRotation.IsClose(Quat::sIdentity()) || inRotation.IsClose(-Quat::sIdentity());
+			SetRotation(mIsRotationIdentity? Quat::sIdentity() : inRotation);
+		}
+
+		/// Get the local transform for this shape given the scale of the child shape
+		/// The total transform of the child shape will be GetLocalTransformNoScale(inScale) * Mat44::sScaling(TransformScale(inScale))
+		/// @param inScale The scale of the child shape (in local space of this shape)
+		JPH_INLINE Mat44			GetLocalTransformNoScale(Vec3Arg inScale) const
+		{
+			JPH_ASSERT(IsValidScale(inScale));
+			return Mat44::sRotationTranslation(GetRotation(), inScale * GetPositionCOM());
+		}
+
+		/// Test if inScale is valid for this sub shape
+		inline bool					IsValidScale(Vec3Arg inScale) const
+		{
+			// We can always handle uniform scale or identity rotations
+			if (mIsRotationIdentity || ScaleHelpers::IsUniformScale(inScale))
+				return true;
+
+			return ScaleHelpers::CanScaleBeRotated(GetRotation(), inScale);
+		}
+
+		/// Transform the scale to the local space of the child shape
+		inline Vec3					TransformScale(Vec3Arg inScale) const
+		{
+			// We don't need to transform uniform scale or if the rotation is identity
+			if (mIsRotationIdentity || ScaleHelpers::IsUniformScale(inScale))
+				return inScale;
+
+			return ScaleHelpers::RotateScale(GetRotation(), inScale);
+		}
+
+		/// Compress the center of mass position
+		JPH_INLINE void				SetPositionCOM(Vec3Arg inPositionCOM)
+		{
+			inPositionCOM.StoreFloat3(&mPositionCOM);
+		}
+
+		/// Uncompress the center of mass position
+		JPH_INLINE Vec3				GetPositionCOM() const
+		{
+			return Vec3::sLoadFloat3Unsafe(mPositionCOM);
+		}
+
+		/// Compress the rotation
+		JPH_INLINE void				SetRotation(QuatArg inRotation)
+		{
+			inRotation.StoreFloat3(&mRotation);
+		}
+
+		/// Uncompress the rotation
+		JPH_INLINE Quat				GetRotation() const
+		{
+			return mIsRotationIdentity? Quat::sIdentity() : Quat::sLoadFloat3Unsafe(mRotation);
+		}
+
+		RefConst<Shape>				mShape;
+		Float3						mPositionCOM;											///< Note: Position of center of mass of sub shape!
+		Float3						mRotation;												///< Note: X, Y, Z of rotation quaternion - note we read 4 bytes beyond this so make sure there's something there
+		uint32						mUserData;												///< User data value (put here because it falls in padding bytes)
+		bool						mIsRotationIdentity;									///< If mRotation is close to identity (put here because it falls in padding bytes)
+		// 3 padding bytes left
+	};
+
+	static_assert(sizeof(SubShape) == (JPH_CPU_ADDRESS_BITS == 64? 40 : 36), "Compiler added unexpected padding");
+
+	using SubShapes = Array<SubShape>;
+
+	/// Access to the sub shapes of this compound
+	const SubShapes &				GetSubShapes() const									{ return mSubShapes; }
+
+	/// Get the total number of sub shapes
+	uint							GetNumSubShapes() const									{ return uint(mSubShapes.size()); }
+
+	/// Access to a particular sub shape
+	const SubShape &				GetSubShape(uint inIdx) const							{ return mSubShapes[inIdx]; }
+
+	/// Get the user data associated with a shape in this compound
+	uint32							GetCompoundUserData(uint inIdx) const					{ return mSubShapes[inIdx].mUserData; }
+
+	/// Set the user data associated with a shape in this compound
+	void							SetCompoundUserData(uint inIdx, uint32 inUserData)		{ mSubShapes[inIdx].mUserData = inUserData; }
+
+	/// Check if a sub shape ID is still valid for this shape
+	/// @param inSubShapeID Sub shape id that indicates the leaf shape relative to this shape
+	/// @return True if the ID is valid, false if not
+	inline bool						IsSubShapeIDValid(SubShapeID inSubShapeID) const
+	{
+		SubShapeID remainder;
+		return inSubShapeID.PopID(GetSubShapeIDBits(), remainder) < mSubShapes.size();
+	}
+
+	/// Convert SubShapeID to sub shape index
+	/// @param inSubShapeID Sub shape id that indicates the leaf shape relative to this shape
+	/// @param outRemainder This is the sub shape ID for the sub shape of the compound after popping off the index
+	/// @return The index of the sub shape of this compound
+	inline uint32					GetSubShapeIndexFromID(SubShapeID inSubShapeID, SubShapeID &outRemainder) const
+	{
+		uint32 idx = inSubShapeID.PopID(GetSubShapeIDBits(), outRemainder);
+		JPH_ASSERT(idx < mSubShapes.size(), "Invalid SubShapeID");
+		return idx;
+	}
+
+	/// @brief Convert a sub shape index to a sub shape ID
+	/// @param inIdx Index of the sub shape of this compound
+	/// @param inParentSubShapeID Parent SubShapeID (describing the path to the compound shape)
+	/// @return A sub shape ID creator that contains the full path to the sub shape with index inIdx
+	inline SubShapeIDCreator		GetSubShapeIDFromIndex(int inIdx, const SubShapeIDCreator &inParentSubShapeID) const
+	{
+		return inParentSubShapeID.PushID(inIdx, GetSubShapeIDBits());
+	}
+
+	// See Shape
+	virtual void					SaveBinaryState(StreamOut &inStream) const override;
+	virtual void					SaveSubShapeState(ShapeList &outSubShapes) const override;
+	virtual void					RestoreSubShapeState(const ShapeRefC *inSubShapes, uint inNumShapes) override;
+
+	// See Shape::GetStatsRecursive
+	virtual Stats					GetStatsRecursive(VisitedShapes &ioVisitedShapes) const override;
+
+	// See Shape::GetVolume
+	virtual float					GetVolume() const override;
+
+	// See Shape::IsValidScale
+	virtual bool					IsValidScale(Vec3Arg inScale) const override;
+
+	// See Shape::MakeScaleValid
+	virtual Vec3					MakeScaleValid(Vec3Arg inScale) const override;
+
+	// Register shape functions with the registry
+	static void						sRegister();
+
+protected:
+	// See: Shape::RestoreBinaryState
+	virtual void					RestoreBinaryState(StreamIn &inStream) override;
+
+	// Visitors for collision detection
+	struct CastRayVisitor;
+	struct CastRayVisitorCollector;
+	struct CollidePointVisitor;
+	struct CastShapeVisitor;
+	struct CollectTransformedShapesVisitor;
+	struct CollideCompoundVsShapeVisitor;
+	struct CollideShapeVsCompoundVisitor;
+	template <class BoxType> struct GetIntersectingSubShapesVisitor;
+
+	/// Determine amount of bits needed to encode sub shape id
+	inline uint						GetSubShapeIDBits() const
+	{
+		// Ensure we have enough bits to encode our shape [0, n - 1]
+		uint32 n = uint32(mSubShapes.size()) - 1;
+		return 32 - CountLeadingZeros(n);
+	}
+
+	/// Determine the inner radius of this shape
+	inline void						CalculateInnerRadius()
+	{
+		mInnerRadius = FLT_MAX;
+		for (const SubShape &s : mSubShapes)
+			mInnerRadius = min(mInnerRadius, s.mShape->GetInnerRadius());
+	}
+
+	Vec3							mCenterOfMass { Vec3::sZero() };						///< Center of mass of the compound
+	AABox							mLocalBounds { Vec3::sZero(), Vec3::sZero() };
+	SubShapes						mSubShapes;
+	float							mInnerRadius = FLT_MAX;									///< Smallest radius of GetInnerRadius() of child shapes
+
+private:
+	// Helper functions called by CollisionDispatch
+	static void						sCastCompoundVsShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector);
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CompoundShapeVisitors.h

@@ -0,0 +1,460 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/Shape/CompoundShape.h>
+#include <Jolt/Physics/Collision/Shape/SubShapeID.h>
+#include <Jolt/Physics/Collision/RayCast.h>
+#include <Jolt/Physics/Collision/CastResult.h>
+#include <Jolt/Physics/Collision/ShapeCast.h>
+#include <Jolt/Physics/Collision/TransformedShape.h>
+#include <Jolt/Physics/Collision/CollisionDispatch.h>
+#include <Jolt/Geometry/RayAABox.h>
+#include <Jolt/Geometry/AABox4.h>
+#include <Jolt/Geometry/OrientedBox.h>
+
+JPH_NAMESPACE_BEGIN
+
+struct CompoundShape::CastRayVisitor
+{
+	JPH_INLINE			CastRayVisitor(const RayCast &inRay, const CompoundShape *inShape, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) :
+		mRay(inRay),
+		mHit(ioHit),
+		mSubShapeIDCreator(inSubShapeIDCreator),
+		mSubShapeBits(inShape->GetSubShapeIDBits())
+	{
+		// Determine ray properties of cast
+		mInvDirection.Set(inRay.mDirection);
+	}
+
+	/// Returns true when collision detection should abort because it's not possible to find a better hit
+	JPH_INLINE bool		ShouldAbort() const
+	{
+		return mHit.mFraction <= 0.0f;
+	}
+
+	/// Test ray against 4 bounding boxes and returns the distance where the ray enters the bounding box
+	JPH_INLINE Vec4		TestBounds(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const
+	{
+		return RayAABox4(mRay.mOrigin, mInvDirection, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
+	}
+
+	/// Test the ray against a single subshape
+	JPH_INLINE void		VisitShape(const SubShape &inSubShape, uint32 inSubShapeIndex)
+	{
+		// Create ID for sub shape
+		SubShapeIDCreator shape2_sub_shape_id = mSubShapeIDCreator.PushID(inSubShapeIndex, mSubShapeBits);
+
+		// Transform the ray
+		Mat44 transform = Mat44::sInverseRotationTranslation(inSubShape.GetRotation(), inSubShape.GetPositionCOM());
+		RayCast ray = mRay.Transformed(transform);
+		if (inSubShape.mShape->CastRay(ray, shape2_sub_shape_id, mHit))
+			mReturnValue = true;
+	}
+
+	RayInvDirection		mInvDirection;
+	const RayCast &		mRay;
+	RayCastResult &		mHit;
+	SubShapeIDCreator	mSubShapeIDCreator;
+	uint				mSubShapeBits;
+	bool				mReturnValue = false;
+};
+
+struct CompoundShape::CastRayVisitorCollector
+{
+	JPH_INLINE			CastRayVisitorCollector(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const CompoundShape *inShape, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) :
+		mRay(inRay),
+		mCollector(ioCollector),
+		mSubShapeIDCreator(inSubShapeIDCreator),
+		mSubShapeBits(inShape->GetSubShapeIDBits()),
+		mRayCastSettings(inRayCastSettings),
+		mShapeFilter(inShapeFilter)
+	{
+		// Determine ray properties of cast
+		mInvDirection.Set(inRay.mDirection);
+	}
+
+	/// Returns true when collision detection should abort because it's not possible to find a better hit
+	JPH_INLINE bool		ShouldAbort() const
+	{
+		return mCollector.ShouldEarlyOut();
+	}
+
+	/// Test ray against 4 bounding boxes and returns the distance where the ray enters the bounding box
+	JPH_INLINE Vec4		TestBounds(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const
+	{
+		return RayAABox4(mRay.mOrigin, mInvDirection, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
+	}
+
+	/// Test the ray against a single subshape
+	JPH_INLINE void		VisitShape(const SubShape &inSubShape, uint32 inSubShapeIndex)
+	{
+		// Create ID for sub shape
+		SubShapeIDCreator shape2_sub_shape_id = mSubShapeIDCreator.PushID(inSubShapeIndex, mSubShapeBits);
+
+		// Transform the ray
+		Mat44 transform = Mat44::sInverseRotationTranslation(inSubShape.GetRotation(), inSubShape.GetPositionCOM());
+		RayCast ray = mRay.Transformed(transform);
+		inSubShape.mShape->CastRay(ray, mRayCastSettings, shape2_sub_shape_id, mCollector, mShapeFilter);
+	}
+
+	RayInvDirection		mInvDirection;
+	const RayCast &		mRay;
+	CastRayCollector &	mCollector;
+	SubShapeIDCreator	mSubShapeIDCreator;
+	uint				mSubShapeBits;
+	RayCastSettings		mRayCastSettings;
+	const ShapeFilter &	mShapeFilter;
+};
+
+struct CompoundShape::CollidePointVisitor
+{
+	JPH_INLINE			CollidePointVisitor(Vec3Arg inPoint, const CompoundShape *inShape, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) :
+		mPoint(inPoint),
+		mSubShapeIDCreator(inSubShapeIDCreator),
+		mCollector(ioCollector),
+		mSubShapeBits(inShape->GetSubShapeIDBits()),
+		mShapeFilter(inShapeFilter)
+	{
+	}
+
+	/// Returns true when collision detection should abort because it's not possible to find a better hit
+	JPH_INLINE bool		ShouldAbort() const
+	{
+		return mCollector.ShouldEarlyOut();
+	}
+
+	/// Test if point overlaps with 4 boxes, returns true for the ones that do
+	JPH_INLINE UVec4	TestBounds(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const
+	{
+		return AABox4VsPoint(mPoint, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
+	}
+
+	/// Test the point against a single subshape
+	JPH_INLINE void		VisitShape(const SubShape &inSubShape, uint32 inSubShapeIndex)
+	{
+		// Create ID for sub shape
+		SubShapeIDCreator shape2_sub_shape_id = mSubShapeIDCreator.PushID(inSubShapeIndex, mSubShapeBits);
+
+		// Transform the point
+		Mat44 transform = Mat44::sInverseRotationTranslation(inSubShape.GetRotation(), inSubShape.GetPositionCOM());
+		inSubShape.mShape->CollidePoint(transform * mPoint, shape2_sub_shape_id, mCollector, mShapeFilter);
+	}
+
+	Vec3						mPoint;
+	SubShapeIDCreator			mSubShapeIDCreator;
+	CollidePointCollector &		mCollector;
+	uint						mSubShapeBits;
+	const ShapeFilter &			mShapeFilter;
+};
+
+struct CompoundShape::CastShapeVisitor
+{
+	JPH_INLINE			CastShapeVisitor(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const CompoundShape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) :
+		mBoxCenter(inShapeCast.mShapeWorldBounds.GetCenter()),
+		mBoxExtent(inShapeCast.mShapeWorldBounds.GetExtent()),
+		mScale(inScale),
+		mShapeCast(inShapeCast),
+		mShapeCastSettings(inShapeCastSettings),
+		mShapeFilter(inShapeFilter),
+		mCollector(ioCollector),
+		mCenterOfMassTransform2(inCenterOfMassTransform2),
+		mSubShapeIDCreator1(inSubShapeIDCreator1),
+		mSubShapeIDCreator2(inSubShapeIDCreator2),
+		mSubShapeBits(inShape->GetSubShapeIDBits())
+	{
+		// Determine ray properties of cast
+		mInvDirection.Set(inShapeCast.mDirection);
+	}
+
+	/// Returns true when collision detection should abort because it's not possible to find a better hit
+	JPH_INLINE bool		ShouldAbort() const
+	{
+		return mCollector.ShouldEarlyOut();
+	}
+
+	/// Tests the shape cast against 4 bounding boxes, returns the distance along the shape cast where the shape first enters the bounding box
+	JPH_INLINE Vec4		TestBounds(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const
+	{
+		// Scale the bounding boxes
+		Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z;
+		AABox4Scale(mScale, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
+
+		// Enlarge them by the casted shape's box extents
+		AABox4EnlargeWithExtent(mBoxExtent, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
+
+		// Test ray against the bounding boxes
+		return RayAABox4(mBoxCenter, mInvDirection, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
+	}
+
+	/// Test the cast shape against a single subshape
+	JPH_INLINE void		VisitShape(const SubShape &inSubShape, uint32 inSubShapeIndex)
+	{
+		JPH_ASSERT(inSubShape.IsValidScale(mScale));
+
+		// Create ID for sub shape
+		SubShapeIDCreator shape2_sub_shape_id = mSubShapeIDCreator2.PushID(inSubShapeIndex, mSubShapeBits);
+
+		// Calculate the local transform for this sub shape
+		Mat44 local_transform = Mat44::sRotationTranslation(inSubShape.GetRotation(), mScale * inSubShape.GetPositionCOM());
+
+		// Transform the center of mass of 2
+		Mat44 center_of_mass_transform2 = mCenterOfMassTransform2 * local_transform;
+
+		// Transform the shape cast
+		ShapeCast shape_cast = mShapeCast.PostTransformed(local_transform.InversedRotationTranslation());
+
+		CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, mShapeCastSettings, inSubShape.mShape, inSubShape.TransformScale(mScale), mShapeFilter, center_of_mass_transform2, mSubShapeIDCreator1, shape2_sub_shape_id, mCollector);
+	}
+
+	RayInvDirection				mInvDirection;
+	Vec3						mBoxCenter;
+	Vec3						mBoxExtent;
+	Vec3						mScale;
+	const ShapeCast &			mShapeCast;
+	const ShapeCastSettings &	mShapeCastSettings;
+	const ShapeFilter &			mShapeFilter;
+	CastShapeCollector &		mCollector;
+	Mat44						mCenterOfMassTransform2;
+	SubShapeIDCreator			mSubShapeIDCreator1;
+	SubShapeIDCreator			mSubShapeIDCreator2;
+	uint						mSubShapeBits;
+};
+
+struct CompoundShape::CollectTransformedShapesVisitor
+{
+	JPH_INLINE			CollectTransformedShapesVisitor(const AABox &inBox, const CompoundShape *inShape, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) :
+		mBox(inBox),
+		mLocalBox(Mat44::sInverseRotationTranslation(inRotation, inPositionCOM), inBox),
+		mPositionCOM(inPositionCOM),
+		mRotation(inRotation),
+		mScale(inScale),
+		mSubShapeIDCreator(inSubShapeIDCreator),
+		mCollector(ioCollector),
+		mSubShapeBits(inShape->GetSubShapeIDBits()),
+		mShapeFilter(inShapeFilter)
+	{
+	}
+
+	/// Returns true when collision detection should abort because it's not possible to find a better hit
+	JPH_INLINE bool		ShouldAbort() const
+	{
+		return mCollector.ShouldEarlyOut();
+	}
+
+	/// Tests 4 bounding boxes against the query box, returns true for the ones that collide
+	JPH_INLINE UVec4	TestBounds(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const
+	{
+		// Scale the bounding boxes of this node
+		Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z;
+		AABox4Scale(mScale, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
+
+		// Test which nodes collide
+		return AABox4VsBox(mLocalBox, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
+	}
+
+	/// Collect the transformed sub shapes for a single subshape
+	JPH_INLINE void		VisitShape(const SubShape &inSubShape, uint32 inSubShapeIndex)
+	{
+		JPH_ASSERT(inSubShape.IsValidScale(mScale));
+
+		// Create ID for sub shape
+		SubShapeIDCreator sub_shape_id = mSubShapeIDCreator.PushID(inSubShapeIndex, mSubShapeBits);
+
+		// Calculate world transform for sub shape
+		Vec3 position = mPositionCOM + mRotation * (mScale * inSubShape.GetPositionCOM());
+		Quat rotation = mRotation * inSubShape.GetRotation();
+
+		// Recurse to sub shape
+		inSubShape.mShape->CollectTransformedShapes(mBox, position, rotation, inSubShape.TransformScale(mScale), sub_shape_id, mCollector, mShapeFilter);
+	}
+
+	AABox							mBox;
+	OrientedBox						mLocalBox;
+	Vec3							mPositionCOM;
+	Quat							mRotation;
+	Vec3							mScale;
+	SubShapeIDCreator				mSubShapeIDCreator;
+	TransformedShapeCollector &		mCollector;
+	uint							mSubShapeBits;
+	const ShapeFilter &				mShapeFilter;
+};
+
+struct CompoundShape::CollideCompoundVsShapeVisitor
+{
+	JPH_INLINE			CollideCompoundVsShapeVisitor(const CompoundShape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) :
+		mCollideShapeSettings(inCollideShapeSettings),
+		mCollector(ioCollector),
+		mShape2(inShape2),
+		mScale1(inScale1),
+		mScale2(inScale2),
+		mTransform1(inCenterOfMassTransform1),
+		mTransform2(inCenterOfMassTransform2),
+		mSubShapeIDCreator1(inSubShapeIDCreator1),
+		mSubShapeIDCreator2(inSubShapeIDCreator2),
+		mSubShapeBits(inShape1->GetSubShapeIDBits()),
+		mShapeFilter(inShapeFilter)
+	{
+		// Get transform from shape 2 to shape 1
+		Mat44 transform2_to_1 = inCenterOfMassTransform1.InversedRotationTranslation() * inCenterOfMassTransform2;
+
+		// Convert bounding box of 2 into space of 1
+		mBoundsOf2InSpaceOf1 = inShape2->GetLocalBounds().Scaled(inScale2).Transformed(transform2_to_1);
+	}
+
+	/// Returns true when collision detection should abort because it's not possible to find a better hit
+	JPH_INLINE bool		ShouldAbort() const
+	{
+		return mCollector.ShouldEarlyOut();
+	}
+
+	/// Tests the bounds of shape 2 vs 4 bounding boxes, returns true for the ones that intersect
+	JPH_INLINE UVec4	TestBounds(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const
+	{
+		// Scale the bounding boxes
+		Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z;
+		AABox4Scale(mScale1, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
+
+		// Test which boxes collide
+		return AABox4VsBox(mBoundsOf2InSpaceOf1, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
+	}
+
+	/// Test the shape against a single subshape
+	JPH_INLINE void		VisitShape(const SubShape &inSubShape, uint32 inSubShapeIndex)
+	{
+		// Get world transform of 1
+		Mat44 transform1 = mTransform1 * inSubShape.GetLocalTransformNoScale(mScale1);
+
+		// Create ID for sub shape
+		SubShapeIDCreator shape1_sub_shape_id = mSubShapeIDCreator1.PushID(inSubShapeIndex, mSubShapeBits);
+
+		CollisionDispatch::sCollideShapeVsShape(inSubShape.mShape, mShape2, inSubShape.TransformScale(mScale1), mScale2, transform1, mTransform2, shape1_sub_shape_id, mSubShapeIDCreator2, mCollideShapeSettings, mCollector, mShapeFilter);
+	}
+
+	const CollideShapeSettings &	mCollideShapeSettings;
+	CollideShapeCollector &			mCollector;
+	const Shape *					mShape2;
+	Vec3							mScale1;
+	Vec3							mScale2;
+	Mat44							mTransform1;
+	Mat44							mTransform2;
+	AABox							mBoundsOf2InSpaceOf1;
+	SubShapeIDCreator				mSubShapeIDCreator1;
+	SubShapeIDCreator				mSubShapeIDCreator2;
+	uint							mSubShapeBits;
+	const ShapeFilter &				mShapeFilter;
+};
+
+struct CompoundShape::CollideShapeVsCompoundVisitor
+{
+	JPH_INLINE			CollideShapeVsCompoundVisitor(const Shape *inShape1, const CompoundShape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) :
+		mCollideShapeSettings(inCollideShapeSettings),
+		mCollector(ioCollector),
+		mShape1(inShape1),
+		mScale1(inScale1),
+		mScale2(inScale2),
+		mTransform1(inCenterOfMassTransform1),
+		mTransform2(inCenterOfMassTransform2),
+		mSubShapeIDCreator1(inSubShapeIDCreator1),
+		mSubShapeIDCreator2(inSubShapeIDCreator2),
+		mSubShapeBits(inShape2->GetSubShapeIDBits()),
+		mShapeFilter(inShapeFilter)
+	{
+		// Get transform from shape 1 to shape 2
+		Mat44 transform1_to_2 = inCenterOfMassTransform2.InversedRotationTranslation() * inCenterOfMassTransform1;
+
+		// Convert bounding box of 1 into space of 2
+		mBoundsOf1InSpaceOf2 = inShape1->GetLocalBounds().Scaled(inScale1).Transformed(transform1_to_2);
+		mBoundsOf1InSpaceOf2.ExpandBy(Vec3::sReplicate(inCollideShapeSettings.mMaxSeparationDistance));
+	}
+
+	/// Returns true when collision detection should abort because it's not possible to find a better hit
+	JPH_INLINE bool		ShouldAbort() const
+	{
+		return mCollector.ShouldEarlyOut();
+	}
+
+	/// Tests the bounds of shape 1 vs 4 bounding boxes, returns true for the ones that intersect
+	JPH_INLINE UVec4	TestBounds(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const
+	{
+		// Scale the bounding boxes
+		Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z;
+		AABox4Scale(mScale2, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
+
+		// Test which bounding boxes collide
+		return AABox4VsBox(mBoundsOf1InSpaceOf2, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
+	}
+
+	/// Test the shape against a single subshape
+	JPH_INLINE void		VisitShape(const SubShape &inSubShape, uint32 inSubShapeIndex)
+	{
+		// Create ID for sub shape
+		SubShapeIDCreator shape2_sub_shape_id = mSubShapeIDCreator2.PushID(inSubShapeIndex, mSubShapeBits);
+
+		// Get world transform of 2
+		Mat44 transform2 = mTransform2 * inSubShape.GetLocalTransformNoScale(mScale2);
+
+		CollisionDispatch::sCollideShapeVsShape(mShape1, inSubShape.mShape, mScale1, inSubShape.TransformScale(mScale2), mTransform1, transform2, mSubShapeIDCreator1, shape2_sub_shape_id, mCollideShapeSettings, mCollector, mShapeFilter);
+	}
+
+	const CollideShapeSettings &	mCollideShapeSettings;
+	CollideShapeCollector &			mCollector;
+	const Shape *					mShape1;
+	Vec3							mScale1;
+	Vec3							mScale2;
+	Mat44							mTransform1;
+	Mat44							mTransform2;
+	AABox							mBoundsOf1InSpaceOf2;
+	SubShapeIDCreator				mSubShapeIDCreator1;
+	SubShapeIDCreator				mSubShapeIDCreator2;
+	uint							mSubShapeBits;
+	const ShapeFilter &				mShapeFilter;
+};
+
+template <class BoxType>
+struct CompoundShape::GetIntersectingSubShapesVisitor
+{
+	JPH_INLINE			GetIntersectingSubShapesVisitor(const BoxType &inBox, uint *outSubShapeIndices, int inMaxSubShapeIndices) :
+		mBox(inBox),
+		mSubShapeIndices(outSubShapeIndices),
+		mMaxSubShapeIndices(inMaxSubShapeIndices)
+	{
+	}
+
+	/// Returns true when collision detection should abort because the buffer is full
+	JPH_INLINE bool		ShouldAbort() const
+	{
+		return mNumResults >= mMaxSubShapeIndices;
+	}
+
+	/// Tests the box vs 4 bounding boxes, returns true for the ones that intersect
+	JPH_INLINE UVec4	TestBounds(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const
+	{
+		// Test which bounding boxes collide
+		return AABox4VsBox(mBox, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
+	}
+
+	/// Records a hit
+	JPH_INLINE void		VisitShape([[maybe_unused]] const SubShape &inSubShape, uint32 inSubShapeIndex)
+	{
+		JPH_ASSERT(mNumResults < mMaxSubShapeIndices);
+		*mSubShapeIndices++ = inSubShapeIndex;
+		mNumResults++;
+	}
+
+	/// Get the number of indices that were found
+	JPH_INLINE int		GetNumResults() const
+	{
+		return mNumResults;
+	}
+
+private:
+	BoxType				mBox;
+	uint *				mSubShapeIndices;
+	int					mMaxSubShapeIndices;
+	int					mNumResults = 0;
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ConvexHullShape.h

@@ -0,0 +1,202 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/Shape/ConvexShape.h>
+#include <Jolt/Physics/PhysicsSettings.h>
+#include <Jolt/Geometry/Plane.h>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+/// Class that constructs a ConvexHullShape
+class JPH_EXPORT ConvexHullShapeSettings final : public ConvexShapeSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, ConvexHullShapeSettings)
+
+public:
+	/// Default constructor for deserialization
+							ConvexHullShapeSettings() = default;
+
+	/// Create a convex hull from inPoints and maximum convex radius inMaxConvexRadius, the radius is automatically lowered if the hull requires it.
+	/// (internally this will be subtracted so the total size will not grow with the convex radius).
+							ConvexHullShapeSettings(const Vec3 *inPoints, int inNumPoints, float inMaxConvexRadius = cDefaultConvexRadius, const PhysicsMaterial *inMaterial = nullptr) : ConvexShapeSettings(inMaterial), mPoints(inPoints, inPoints + inNumPoints), mMaxConvexRadius(inMaxConvexRadius) { }
+							ConvexHullShapeSettings(const Array<Vec3> &inPoints, float inConvexRadius = cDefaultConvexRadius, const PhysicsMaterial *inMaterial = nullptr) : ConvexShapeSettings(inMaterial), mPoints(inPoints), mMaxConvexRadius(inConvexRadius) { }
+
+	// See: ShapeSettings
+	virtual ShapeResult		Create() const override;
+
+	Array<Vec3>				mPoints;															///< Points to create the hull from. Note that these points don't need to be the vertices of the convex hull, they can contain interior points or points on faces/edges.
+	float					mMaxConvexRadius = 0.0f;											///< Convex radius as supplied by the constructor. Note that during hull creation the convex radius can be made smaller if the value is too big for the hull.
+	float					mMaxErrorConvexRadius = 0.05f;										///< Maximum distance between the shrunk hull + convex radius and the actual hull.
+	float					mHullTolerance = 1.0e-3f;											///< Points are allowed this far outside of the hull (increasing this yields a hull with less vertices). Note that the actual used value can be larger if the points of the hull are far apart.
+};
+
+/// A convex hull
+class JPH_EXPORT ConvexHullShape final : public ConvexShape
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Maximum amount of points supported in a convex hull. Note that while constructing a hull, interior points are discarded so you can provide more points.
+	/// The ConvexHullShapeSettings::Create function will return an error when too many points are provided.
+	static constexpr int	cMaxPointsInHull = 256;
+
+	/// Constructor
+							ConvexHullShape() : ConvexShape(EShapeSubType::ConvexHull) { }
+							ConvexHullShape(const ConvexHullShapeSettings &inSettings, ShapeResult &outResult);
+
+	// See Shape::GetCenterOfMass
+	virtual Vec3			GetCenterOfMass() const override									{ return mCenterOfMass; }
+
+	// See Shape::GetLocalBounds
+	virtual AABox			GetLocalBounds() const override										{ return mLocalBounds; }
+
+	// See Shape::GetInnerRadius
+	virtual float			GetInnerRadius() const override										{ return mInnerRadius; }
+
+	// See Shape::GetMassProperties
+	virtual MassProperties	GetMassProperties() const override;
+
+	// See Shape::GetSurfaceNormal
+	virtual Vec3			GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override;
+
+	// See Shape::GetSupportingFace
+	virtual void			GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override;
+
+	// See ConvexShape::GetSupportFunction
+	virtual const Support *	GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const override;
+
+	// See Shape::GetSubmergedVolume
+	virtual void			GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const override;
+
+#ifdef JPH_DEBUG_RENDERER
+	// See Shape::Draw
+	virtual void			Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override;
+
+	/// Debugging helper draw function that draws how all points are moved when a shape is shrunk by the convex radius
+	void					DrawShrunkShape(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const;
+#endif // JPH_DEBUG_RENDERER
+
+	// See Shape::CastRay
+	virtual bool			CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override;
+	virtual void			CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
+
+	// See: Shape::CollidePoint
+	virtual void			CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
+
+	// See: Shape::CollideSoftBodyVertices
+	virtual void			CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override;
+
+	// See Shape::GetTrianglesStart
+	virtual void			GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override;
+
+	// See Shape::GetTrianglesNext
+	virtual int				GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override;
+
+	// See Shape
+	virtual void			SaveBinaryState(StreamOut &inStream) const override;
+
+	// See Shape::GetStats
+	virtual Stats			GetStats() const override;
+
+	// See Shape::GetVolume
+	virtual float			GetVolume() const override											{ return mVolume; }
+
+	/// Get the convex radius of this convex hull
+	float					GetConvexRadius() const												{ return mConvexRadius; }
+
+	/// Get the planes of this convex hull
+	const Array<Plane> &	GetPlanes() const													{ return mPlanes; }
+
+	/// Get the number of vertices in this convex hull
+	inline uint				GetNumPoints() const												{ return uint(mPoints.size()); }
+
+	/// Get a vertex of this convex hull relative to the center of mass
+	inline Vec3				GetPoint(uint inIndex) const										{ return mPoints[inIndex].mPosition; }
+
+	/// Get the number of faces in this convex hull
+	inline uint				GetNumFaces() const													{ return uint(mFaces.size()); }
+
+	/// Get the number of vertices in a face
+	inline uint				GetNumVerticesInFace(uint inFaceIndex) const						{ return mFaces[inFaceIndex].mNumVertices; }
+
+	/// Get the vertices indices of a face
+	/// @param inFaceIndex Index of the face.
+	/// @param inMaxVertices Maximum number of vertices to return.
+	/// @param outVertices Array of vertices indices, must be at least inMaxVertices in size, the vertices are returned in counter clockwise order and the positions can be obtained using GetPoint(index).
+	/// @return Number of vertices in face, if this is bigger than inMaxVertices, not all vertices were retrieved.
+	inline uint				GetFaceVertices(uint inFaceIndex, uint inMaxVertices, uint *outVertices) const
+	{
+		const Face &face = mFaces[inFaceIndex];
+		const uint8 *first_vertex = mVertexIdx.data() + face.mFirstVertex;
+		uint num_vertices = min<uint>(face.mNumVertices, inMaxVertices);
+		for (uint i = 0; i < num_vertices; ++i)
+			outVertices[i] = first_vertex[i];
+		return face.mNumVertices;
+	}
+
+	// Register shape functions with the registry
+	static void				sRegister();
+
+#ifdef JPH_DEBUG_RENDERER
+	/// Draw the outlines of the faces of the convex hull when drawing the shape
+	inline static bool		sDrawFaceOutlines = false;
+#endif // JPH_DEBUG_RENDERER
+
+protected:
+	// See: Shape::RestoreBinaryState
+	virtual void			RestoreBinaryState(StreamIn &inStream) override;
+
+private:
+	/// Helper function that returns the min and max fraction along the ray that hits the convex hull. Returns false if there is no hit.
+	bool					CastRayHelper(const RayCast &inRay, float &outMinFraction, float &outMaxFraction) const;
+
+	/// Class for GetTrianglesStart/Next
+	class					CHSGetTrianglesContext;
+
+	/// Classes for GetSupportFunction
+	class					HullNoConvex;
+	class					HullWithConvex;
+	class					HullWithConvexScaled;
+
+	struct Face
+	{
+		uint16				mFirstVertex;				///< First index in mVertexIdx to use
+		uint16				mNumVertices = 0;			///< Number of vertices in the mVertexIdx to use
+	};
+
+	static_assert(sizeof(Face) == 4, "Unexpected size");
+	static_assert(alignof(Face) == 2, "Unexpected alignment");
+
+	struct Point
+	{
+		Vec3				mPosition;					///< Position of vertex
+		int					mNumFaces = 0;				///< Number of faces in the face array below
+		int					mFaces[3] = { -1, -1, -1 };	///< Indices of 3 neighboring faces with the biggest difference in normal (used to shift vertices for convex radius)
+	};
+
+	static_assert(sizeof(Point) == 32, "Unexpected size");
+	static_assert(alignof(Point) == JPH_VECTOR_ALIGNMENT, "Unexpected alignment");
+
+	Vec3					mCenterOfMass;				///< Center of mass of this convex hull
+	Mat44					mInertia;					///< Inertia matrix assuming density is 1 (needs to be multiplied by density)
+	AABox					mLocalBounds;				///< Local bounding box for the convex hull
+	Array<Point>			mPoints;					///< Points on the convex hull surface
+	Array<Face>				mFaces;						///< Faces of the convex hull surface
+	Array<Plane>			mPlanes;					///< Planes for the faces (1-on-1 with mFaces array, separate because they need to be 16 byte aligned)
+	Array<uint8>			mVertexIdx;					///< A list of vertex indices (indexing in mPoints) for each of the faces
+	float					mConvexRadius = 0.0f;		///< Convex radius
+	float					mVolume;					///< Total volume of the convex hull
+	float					mInnerRadius = FLT_MAX;		///< Radius of the biggest sphere that fits entirely in the convex hull
+
+#ifdef JPH_DEBUG_RENDERER
+	mutable DebugRenderer::GeometryRef mGeometry;
+#endif // JPH_DEBUG_RENDERER
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ConvexShape.cpp

@@ -0,0 +1,566 @@
+// 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/Shape/ConvexShape.h>
+#include <Jolt/Physics/Collision/RayCast.h>
+#include <Jolt/Physics/Collision/ShapeCast.h>
+#include <Jolt/Physics/Collision/CollideShape.h>
+#include <Jolt/Physics/Collision/CastResult.h>
+#include <Jolt/Physics/Collision/CollidePointResult.h>
+#include <Jolt/Physics/Collision/Shape/ScaleHelpers.h>
+#include <Jolt/Physics/Collision/Shape/GetTrianglesContext.h>
+#include <Jolt/Physics/Collision/Shape/PolyhedronSubmergedVolumeCalculator.h>
+#include <Jolt/Physics/Collision/TransformedShape.h>
+#include <Jolt/Physics/Collision/CollisionDispatch.h>
+#include <Jolt/Physics/Collision/NarrowPhaseStats.h>
+#include <Jolt/Physics/PhysicsSettings.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+#include <Jolt/Geometry/EPAPenetrationDepth.h>
+#include <Jolt/Geometry/OrientedBox.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT(ConvexShapeSettings)
+{
+	JPH_ADD_BASE_CLASS(ConvexShapeSettings, ShapeSettings)
+
+	JPH_ADD_ATTRIBUTE(ConvexShapeSettings, mDensity)
+	JPH_ADD_ATTRIBUTE(ConvexShapeSettings, mMaterial)
+}
+
+const StaticArray<Vec3, 384> ConvexShape::sUnitSphereTriangles = []() {
+	const int level = 2;
+
+	StaticArray<Vec3, 384> verts;
+	GetTrianglesContextVertexList::sCreateHalfUnitSphereTop(verts, level);
+	GetTrianglesContextVertexList::sCreateHalfUnitSphereBottom(verts, level);
+	return verts;
+}();
+
+void ConvexShape::sCollideConvexVsConvex(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, [[maybe_unused]] const ShapeFilter &inShapeFilter)
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Get the shapes
+	JPH_ASSERT(inShape1->GetType() == EShapeType::Convex);
+	JPH_ASSERT(inShape2->GetType() == EShapeType::Convex);
+	const ConvexShape *shape1 = static_cast<const ConvexShape *>(inShape1);
+	const ConvexShape *shape2 = static_cast<const ConvexShape *>(inShape2);
+
+	// Get transforms
+	Mat44 inverse_transform1 = inCenterOfMassTransform1.InversedRotationTranslation();
+	Mat44 transform_2_to_1 = inverse_transform1 * inCenterOfMassTransform2;
+
+	// Get bounding boxes
+	float max_separation_distance = inCollideShapeSettings.mMaxSeparationDistance;
+	AABox shape1_bbox = shape1->GetLocalBounds().Scaled(inScale1);
+	shape1_bbox.ExpandBy(Vec3::sReplicate(max_separation_distance));
+	AABox shape2_bbox = shape2->GetLocalBounds().Scaled(inScale2);
+
+	// Check if they overlap
+	if (!OrientedBox(transform_2_to_1, shape2_bbox).Overlaps(shape1_bbox))
+		return;
+
+	// Note: As we don't remember the penetration axis from the last iteration, and it is likely that shape2 is pushed out of
+	// collision relative to shape1 by comparing their COM's, we use that as an initial penetration axis: shape2.com - shape1.com
+	// This has been seen to improve performance by approx. 1% over using a fixed axis like (1, 0, 0).
+	Vec3 penetration_axis = transform_2_to_1.GetTranslation();
+
+	// Ensure that we do not pass in a near zero penetration axis
+	if (penetration_axis.IsNearZero())
+		penetration_axis = Vec3::sAxisX();
+
+	Vec3 point1, point2;
+	EPAPenetrationDepth pen_depth;
+	EPAPenetrationDepth::EStatus status;
+
+	// Scope to limit lifetime of SupportBuffer
+	{
+		// Create support function
+		SupportBuffer buffer1_excl_cvx_radius, buffer2_excl_cvx_radius;
+		const Support *shape1_excl_cvx_radius = shape1->GetSupportFunction(ConvexShape::ESupportMode::ExcludeConvexRadius, buffer1_excl_cvx_radius, inScale1);
+		const Support *shape2_excl_cvx_radius = shape2->GetSupportFunction(ConvexShape::ESupportMode::ExcludeConvexRadius, buffer2_excl_cvx_radius, inScale2);
+
+		// Transform shape 2 in the space of shape 1
+		TransformedConvexObject transformed2_excl_cvx_radius(transform_2_to_1, *shape2_excl_cvx_radius);
+
+		// Perform GJK step
+		status = pen_depth.GetPenetrationDepthStepGJK(*shape1_excl_cvx_radius, shape1_excl_cvx_radius->GetConvexRadius() + max_separation_distance, transformed2_excl_cvx_radius, shape2_excl_cvx_radius->GetConvexRadius(), inCollideShapeSettings.mCollisionTolerance, penetration_axis, point1, point2);
+	}
+
+	// Check result of collision detection
+	switch (status)
+	{
+	case EPAPenetrationDepth::EStatus::Colliding:
+		break;
+
+	case EPAPenetrationDepth::EStatus::NotColliding:
+		return;
+
+	case EPAPenetrationDepth::EStatus::Indeterminate:
+		{
+			// Need to run expensive EPA algorithm
+
+			// We know we're overlapping at this point, so we can set the max separation distance to 0.
+			// Numerically it is possible that GJK finds that the shapes are overlapping but EPA finds that they're separated.
+			// In order to avoid this, we clamp the max separation distance to 1 so that we don't excessively inflate the shape,
+			// but we still inflate it enough to avoid the case where EPA misses the collision.
+			max_separation_distance = min(max_separation_distance, 1.0f);
+
+			// Create support function
+			SupportBuffer buffer1_incl_cvx_radius, buffer2_incl_cvx_radius;
+			const Support *shape1_incl_cvx_radius = shape1->GetSupportFunction(ConvexShape::ESupportMode::IncludeConvexRadius, buffer1_incl_cvx_radius, inScale1);
+			const Support *shape2_incl_cvx_radius = shape2->GetSupportFunction(ConvexShape::ESupportMode::IncludeConvexRadius, buffer2_incl_cvx_radius, inScale2);
+
+			// Add separation distance
+			AddConvexRadius shape1_add_max_separation_distance(*shape1_incl_cvx_radius, max_separation_distance);
+
+			// Transform shape 2 in the space of shape 1
+			TransformedConvexObject transformed2_incl_cvx_radius(transform_2_to_1, *shape2_incl_cvx_radius);
+
+			// Perform EPA step
+			if (!pen_depth.GetPenetrationDepthStepEPA(shape1_add_max_separation_distance, transformed2_incl_cvx_radius, inCollideShapeSettings.mPenetrationTolerance, penetration_axis, point1, point2))
+				return;
+			break;
+		}
+	}
+
+	// Check if the penetration is bigger than the early out fraction
+	float penetration_depth = (point2 - point1).Length() - max_separation_distance;
+	if (-penetration_depth >= ioCollector.GetEarlyOutFraction())
+		return;
+
+	// Correct point1 for the added separation distance
+	float penetration_axis_len = penetration_axis.Length();
+	if (penetration_axis_len > 0.0f)
+		point1 -= penetration_axis * (max_separation_distance / penetration_axis_len);
+
+	// Convert to world space
+	point1 = inCenterOfMassTransform1 * point1;
+	point2 = inCenterOfMassTransform1 * point2;
+	Vec3 penetration_axis_world = inCenterOfMassTransform1.Multiply3x3(penetration_axis);
+
+	// Create collision result
+	CollideShapeResult result(point1, point2, penetration_axis_world, penetration_depth, inSubShapeIDCreator1.GetID(), inSubShapeIDCreator2.GetID(), TransformedShape::sGetBodyID(ioCollector.GetContext()));
+
+	// Gather faces
+	if (inCollideShapeSettings.mCollectFacesMode == ECollectFacesMode::CollectFaces)
+	{
+		// Get supporting face of shape 1
+		shape1->GetSupportingFace(SubShapeID(), -penetration_axis, inScale1, inCenterOfMassTransform1, result.mShape1Face);
+
+		// Get supporting face of shape 2
+		shape2->GetSupportingFace(SubShapeID(), transform_2_to_1.Multiply3x3Transposed(penetration_axis), inScale2, inCenterOfMassTransform2, result.mShape2Face);
+	}
+
+	// Notify the collector
+	JPH_IF_TRACK_NARROWPHASE_STATS(TrackNarrowPhaseCollector track;)
+	ioCollector.AddHit(result);
+}
+
+bool ConvexShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const
+{
+	// Note: This is a fallback routine, most convex shapes should implement a more performant version!
+
+	JPH_PROFILE_FUNCTION();
+
+	// Create support function
+	SupportBuffer buffer;
+	const Support *support = GetSupportFunction(ConvexShape::ESupportMode::IncludeConvexRadius, buffer, Vec3::sOne());
+
+	// Cast ray
+	GJKClosestPoint gjk;
+	if (gjk.CastRay(inRay.mOrigin, inRay.mDirection, cDefaultCollisionTolerance, *support, ioHit.mFraction))
+	{
+		ioHit.mSubShapeID2 = inSubShapeIDCreator.GetID();
+		return true;
+	}
+
+	return false;
+}
+
+void ConvexShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	// Note: This is a fallback routine, most convex shapes should implement a more performant version!
+
+	// Test shape filter
+	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
+		return;
+
+	// First do a normal raycast, limited to the early out fraction
+	RayCastResult hit;
+	hit.mFraction = ioCollector.GetEarlyOutFraction();
+	if (CastRay(inRay, inSubShapeIDCreator, hit))
+	{
+		// Check front side
+		if (inRayCastSettings.mTreatConvexAsSolid || hit.mFraction > 0.0f)
+		{
+			hit.mBodyID = TransformedShape::sGetBodyID(ioCollector.GetContext());
+			ioCollector.AddHit(hit);
+		}
+
+		// Check if we want back facing hits and the collector still accepts additional hits
+		if (inRayCastSettings.mBackFaceModeConvex == EBackFaceMode::CollideWithBackFaces && !ioCollector.ShouldEarlyOut())
+		{
+			// Invert the ray, going from the early out fraction back to the fraction where we found our forward hit
+			float start_fraction = min(1.0f, ioCollector.GetEarlyOutFraction());
+			float delta_fraction = hit.mFraction - start_fraction;
+			if (delta_fraction < 0.0f)
+			{
+				RayCast inverted_ray { inRay.mOrigin + start_fraction * inRay.mDirection, delta_fraction * inRay.mDirection };
+
+				// Cast another ray
+				RayCastResult inverted_hit;
+				inverted_hit.mFraction = 1.0f;
+				if (CastRay(inverted_ray, inSubShapeIDCreator, inverted_hit)
+					&& inverted_hit.mFraction > 0.0f) // Ignore hits with fraction 0, this means the ray ends inside the object and we don't want to report it as a back facing hit
+				{
+					// Invert fraction and rescale it to the fraction of the original ray
+					inverted_hit.mFraction = hit.mFraction + (inverted_hit.mFraction - 1.0f) * delta_fraction;
+					inverted_hit.mBodyID = TransformedShape::sGetBodyID(ioCollector.GetContext());
+					ioCollector.AddHit(inverted_hit);
+				}
+			}
+		}
+	}
+}
+
+void ConvexShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	// Test shape filter
+	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
+		return;
+
+	// First test bounding box
+	if (GetLocalBounds().Contains(inPoint))
+	{
+		// Create support function
+		SupportBuffer buffer;
+		const Support *support = GetSupportFunction(ConvexShape::ESupportMode::IncludeConvexRadius, buffer, Vec3::sOne());
+
+		// Create support function for point
+		PointConvexSupport point { inPoint };
+
+		// Test intersection
+		GJKClosestPoint gjk;
+		Vec3 v = inPoint;
+		if (gjk.Intersects(*support, point, cDefaultCollisionTolerance, v))
+			ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator.GetID() });
+	}
+}
+
+void ConvexShape::sCastConvexVsConvex(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, [[maybe_unused]] const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Only supported for convex shapes
+	JPH_ASSERT(inShapeCast.mShape->GetType() == EShapeType::Convex);
+	const ConvexShape *cast_shape = static_cast<const ConvexShape *>(inShapeCast.mShape);
+
+	JPH_ASSERT(inShape->GetType() == EShapeType::Convex);
+	const ConvexShape *shape = static_cast<const ConvexShape *>(inShape);
+
+	// Determine if we want to use the actual shape or a shrunken shape with convex radius
+	ConvexShape::ESupportMode support_mode = inShapeCastSettings.mUseShrunkenShapeAndConvexRadius? ConvexShape::ESupportMode::ExcludeConvexRadius : ConvexShape::ESupportMode::Default;
+
+	// Create support function for shape to cast
+	SupportBuffer cast_buffer;
+	const Support *cast_support = cast_shape->GetSupportFunction(support_mode, cast_buffer, inShapeCast.mScale);
+
+	// Create support function for target shape
+	SupportBuffer target_buffer;
+	const Support *target_support = shape->GetSupportFunction(support_mode, target_buffer, inScale);
+
+	// Do a raycast against the result
+	EPAPenetrationDepth epa;
+	float fraction = ioCollector.GetEarlyOutFraction();
+	Vec3 contact_point_a, contact_point_b, contact_normal;
+	if (epa.CastShape(inShapeCast.mCenterOfMassStart, inShapeCast.mDirection, inShapeCastSettings.mCollisionTolerance, inShapeCastSettings.mPenetrationTolerance, *cast_support, *target_support, cast_support->GetConvexRadius(), target_support->GetConvexRadius(), inShapeCastSettings.mReturnDeepestPoint, fraction, contact_point_a, contact_point_b, contact_normal)
+		&& (inShapeCastSettings.mBackFaceModeConvex == EBackFaceMode::CollideWithBackFaces
+			|| contact_normal.Dot(inShapeCast.mDirection) > 0.0f)) // Test if backfacing
+	{
+		// Convert to world space
+		contact_point_a = inCenterOfMassTransform2 * contact_point_a;
+		contact_point_b = inCenterOfMassTransform2 * contact_point_b;
+		Vec3 contact_normal_world = inCenterOfMassTransform2.Multiply3x3(contact_normal);
+
+		ShapeCastResult result(fraction, contact_point_a, contact_point_b, contact_normal_world, false, inSubShapeIDCreator1.GetID(), inSubShapeIDCreator2.GetID(), TransformedShape::sGetBodyID(ioCollector.GetContext()));
+
+		// Early out if this hit is deeper than the collector's early out value
+		if (fraction == 0.0f && -result.mPenetrationDepth >= ioCollector.GetEarlyOutFraction())
+			return;
+
+		// Gather faces
+		if (inShapeCastSettings.mCollectFacesMode == ECollectFacesMode::CollectFaces)
+		{
+			// Get supporting face of shape 1
+			Mat44 transform_1_to_2 = inShapeCast.mCenterOfMassStart;
+			transform_1_to_2.SetTranslation(transform_1_to_2.GetTranslation() + fraction * inShapeCast.mDirection);
+			cast_shape->GetSupportingFace(SubShapeID(), transform_1_to_2.Multiply3x3Transposed(-contact_normal), inShapeCast.mScale, inCenterOfMassTransform2 * transform_1_to_2, result.mShape1Face);
+
+			// Get supporting face of shape 2
+			shape->GetSupportingFace(SubShapeID(), contact_normal, inScale, inCenterOfMassTransform2, result.mShape2Face);
+		}
+
+		JPH_IF_TRACK_NARROWPHASE_STATS(TrackNarrowPhaseCollector track;)
+		ioCollector.AddHit(result);
+	}
+}
+
+class ConvexShape::CSGetTrianglesContext
+{
+public:
+				CSGetTrianglesContext(const ConvexShape *inShape, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) :
+		mLocalToWorld(Mat44::sRotationTranslation(inRotation, inPositionCOM) * Mat44::sScale(inScale)),
+		mIsInsideOut(ScaleHelpers::IsInsideOut(inScale))
+	{
+		mSupport = inShape->GetSupportFunction(ESupportMode::IncludeConvexRadius, mSupportBuffer, Vec3::sOne());
+	}
+
+	SupportBuffer		mSupportBuffer;
+	const Support *		mSupport;
+	Mat44				mLocalToWorld;
+	bool				mIsInsideOut;
+	size_t				mCurrentVertex = 0;
+};
+
+void ConvexShape::GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const
+{
+	static_assert(sizeof(CSGetTrianglesContext) <= sizeof(GetTrianglesContext), "GetTrianglesContext too small");
+	JPH_ASSERT(IsAligned(&ioContext, alignof(CSGetTrianglesContext)));
+
+	new (&ioContext) CSGetTrianglesContext(this, inPositionCOM, inRotation, inScale);
+}
+
+int ConvexShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) const
+{
+	JPH_ASSERT(inMaxTrianglesRequested >= cGetTrianglesMinTrianglesRequested);
+
+	CSGetTrianglesContext &context = (CSGetTrianglesContext &)ioContext;
+
+	int total_num_vertices = min(inMaxTrianglesRequested * 3, int(sUnitSphereTriangles.size() - context.mCurrentVertex));
+
+	if (context.mIsInsideOut)
+	{
+		// Store triangles flipped
+		for (const Vec3 *v = sUnitSphereTriangles.data() + context.mCurrentVertex, *v_end = v + total_num_vertices; v < v_end; v += 3)
+		{
+			(context.mLocalToWorld * context.mSupport->GetSupport(v[0])).StoreFloat3(outTriangleVertices++);
+			(context.mLocalToWorld * context.mSupport->GetSupport(v[2])).StoreFloat3(outTriangleVertices++);
+			(context.mLocalToWorld * context.mSupport->GetSupport(v[1])).StoreFloat3(outTriangleVertices++);
+		}
+	}
+	else
+	{
+		// Store triangles
+		for (const Vec3 *v = sUnitSphereTriangles.data() + context.mCurrentVertex, *v_end = v + total_num_vertices; v < v_end; v += 3)
+		{
+			(context.mLocalToWorld * context.mSupport->GetSupport(v[0])).StoreFloat3(outTriangleVertices++);
+			(context.mLocalToWorld * context.mSupport->GetSupport(v[1])).StoreFloat3(outTriangleVertices++);
+			(context.mLocalToWorld * context.mSupport->GetSupport(v[2])).StoreFloat3(outTriangleVertices++);
+		}
+	}
+
+	context.mCurrentVertex += total_num_vertices;
+	int total_num_triangles = total_num_vertices / 3;
+
+	// Store materials
+	if (outMaterials != nullptr)
+	{
+		const PhysicsMaterial *material = GetMaterial();
+		for (const PhysicsMaterial **m = outMaterials, **m_end = outMaterials + total_num_triangles; m < m_end; ++m)
+			*m = material;
+	}
+
+	return total_num_triangles;
+}
+
+void ConvexShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const
+{
+	// Calculate total volume
+	Vec3 abs_scale = inScale.Abs();
+	Vec3 extent = GetLocalBounds().GetExtent() * abs_scale;
+	outTotalVolume = 8.0f * extent.GetX() * extent.GetY() * extent.GetZ();
+
+	// Points of the bounding box
+	Vec3 points[] =
+	{
+		Vec3(-1, -1, -1),
+		Vec3( 1, -1, -1),
+		Vec3(-1,  1, -1),
+		Vec3( 1,  1, -1),
+		Vec3(-1, -1,  1),
+		Vec3( 1, -1,  1),
+		Vec3(-1,  1,  1),
+		Vec3( 1,  1,  1),
+	};
+
+	// Faces of the bounding box
+	using Face = int[5];
+	#define MAKE_FACE(a, b, c, d) { a, b, c, d, ((1 << a) | (1 << b) | (1 << c) | (1 << d)) } // Last int is a bit mask that indicates which indices are used
+	Face faces[] =
+	{
+		MAKE_FACE(0, 2, 3, 1),
+		MAKE_FACE(4, 6, 2, 0),
+		MAKE_FACE(4, 5, 7, 6),
+		MAKE_FACE(1, 3, 7, 5),
+		MAKE_FACE(2, 6, 7, 3),
+		MAKE_FACE(0, 1, 5, 4),
+	};
+
+	PolyhedronSubmergedVolumeCalculator::Point *buffer = (PolyhedronSubmergedVolumeCalculator::Point *)JPH_STACK_ALLOC(8 * sizeof(PolyhedronSubmergedVolumeCalculator::Point));
+	PolyhedronSubmergedVolumeCalculator submerged_vol_calc(inCenterOfMassTransform * Mat44::sScale(extent), points, sizeof(Vec3), 8, inSurface, buffer JPH_IF_DEBUG_RENDERER(, inBaseOffset));
+
+	if (submerged_vol_calc.AreAllAbove())
+	{
+		// We're above the water
+		outSubmergedVolume = 0.0f;
+		outCenterOfBuoyancy = Vec3::sZero();
+	}
+	else if (submerged_vol_calc.AreAllBelow())
+	{
+		// We're fully submerged
+		outSubmergedVolume = outTotalVolume;
+		outCenterOfBuoyancy = inCenterOfMassTransform.GetTranslation();
+	}
+	else
+	{
+		// Calculate submerged volume
+		int reference_point_bit = 1 << submerged_vol_calc.GetReferencePointIdx();
+		for (const Face &f : faces)
+		{
+			// Test if this face includes the reference point
+			if ((f[4] & reference_point_bit) == 0)
+			{
+				// Triangulate the face (a quad)
+				submerged_vol_calc.AddFace(f[0], f[1], f[2]);
+				submerged_vol_calc.AddFace(f[0], f[2], f[3]);
+			}
+		}
+
+		submerged_vol_calc.GetResult(outSubmergedVolume, outCenterOfBuoyancy);
+	}
+}
+
+#ifdef JPH_DEBUG_RENDERER
+void ConvexShape::DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const
+{
+	// Get the support function with convex radius
+	SupportBuffer buffer;
+	const Support *support = GetSupportFunction(ESupportMode::ExcludeConvexRadius, buffer, inScale);
+	AddConvexRadius add_convex(*support, support->GetConvexRadius());
+
+	// Draw the shape
+	DebugRenderer::GeometryRef geometry = inRenderer->CreateTriangleGeometryForConvex([&add_convex](Vec3Arg inDirection) { return add_convex.GetSupport(inDirection); });
+	AABox bounds = geometry->mBounds.Transformed(inCenterOfMassTransform);
+	float lod_scale_sq = geometry->mBounds.GetExtent().LengthSq();
+	inRenderer->DrawGeometry(inCenterOfMassTransform, bounds, lod_scale_sq, inColor, geometry);
+
+	if (inDrawSupportDirection)
+	{
+		// Iterate on all directions and draw the support point and an arrow in the direction that was sampled to test if the support points make sense
+		for (Vec3 v : Vec3::sUnitSphere)
+		{
+			Vec3 direction = 0.05f * v;
+			Vec3 pos = add_convex.GetSupport(direction);
+			RVec3 from = inCenterOfMassTransform * pos;
+			RVec3 to = inCenterOfMassTransform * (pos + direction);
+			inRenderer->DrawMarker(from, Color::sWhite, 0.001f);
+			inRenderer->DrawArrow(from, to, Color::sWhite, 0.001f);
+		}
+	}
+}
+
+void ConvexShape::DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const
+{
+	// Sample directions and map which faces belong to which directions
+	using FaceToDirection = UnorderedMap<SupportingFace, Array<Vec3>>;
+	FaceToDirection faces;
+	for (Vec3 v : Vec3::sUnitSphere)
+	{
+		Vec3 direction = 0.05f * v;
+
+		SupportingFace face;
+		GetSupportingFace(SubShapeID(), direction, inScale, Mat44::sIdentity(), face);
+
+		if (!face.empty())
+		{
+			JPH_ASSERT(face.size() >= 2, "The GetSupportingFace function should either return nothing or at least an edge");
+			faces[face].push_back(direction);
+		}
+	}
+
+	// Draw each face in a unique color and draw corresponding directions
+	int color_it = 0;
+	for (FaceToDirection::value_type &ftd : faces)
+	{
+		Color color = Color::sGetDistinctColor(color_it++);
+
+		// Create copy of face (key in map is read only)
+		SupportingFace face = ftd.first;
+
+		// Displace the face a little bit forward so it is easier to see
+		Vec3 normal = face.size() >= 3? (face[2] - face[1]).Cross(face[0] - face[1]).NormalizedOr(Vec3::sZero()) : Vec3::sZero();
+		Vec3 displacement = 0.001f * normal;
+
+		// Transform face to world space and calculate center of mass
+		Vec3 com_ls = Vec3::sZero();
+		for (Vec3 &v : face)
+		{
+			v = inCenterOfMassTransform.Multiply3x3(v + displacement);
+			com_ls += v;
+		}
+		RVec3 com = inCenterOfMassTransform.GetTranslation() + com_ls / (float)face.size();
+
+		// Draw the polygon and directions
+		inRenderer->DrawWirePolygon(RMat44::sTranslation(inCenterOfMassTransform.GetTranslation()), face, color, face.size() >= 3? 0.001f : 0.0f);
+		if (face.size() >= 3)
+			inRenderer->DrawArrow(com, com + inCenterOfMassTransform.Multiply3x3(normal), color, 0.01f);
+		for (Vec3 &v : ftd.second)
+			inRenderer->DrawArrow(com, com + inCenterOfMassTransform.Multiply3x3(-v), color, 0.001f);
+	}
+}
+#endif // JPH_DEBUG_RENDERER
+
+void ConvexShape::SaveBinaryState(StreamOut &inStream) const
+{
+	Shape::SaveBinaryState(inStream);
+
+	inStream.Write(mDensity);
+}
+
+void ConvexShape::RestoreBinaryState(StreamIn &inStream)
+{
+	Shape::RestoreBinaryState(inStream);
+
+	inStream.Read(mDensity);
+}
+
+void ConvexShape::SaveMaterialState(PhysicsMaterialList &outMaterials) const
+{
+	outMaterials.clear();
+	outMaterials.push_back(mMaterial);
+}
+
+void ConvexShape::RestoreMaterialState(const PhysicsMaterialRefC *inMaterials, uint inNumMaterials)
+{
+	JPH_ASSERT(inNumMaterials == 1);
+	mMaterial = inMaterials[0];
+}
+
+void ConvexShape::sRegister()
+{
+	for (EShapeSubType s1 : sConvexSubShapeTypes)
+		for (EShapeSubType s2 : sConvexSubShapeTypes)
+		{
+			CollisionDispatch::sRegisterCollideShape(s1, s2, sCollideConvexVsConvex);
+			CollisionDispatch::sRegisterCastShape(s1, s2, sCastConvexVsConvex);
+		}
+}
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ConvexShape.h

@@ -0,0 +1,150 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/StaticArray.h>
+#include <Jolt/Physics/Collision/Shape/Shape.h>
+#include <Jolt/Physics/Collision/Shape/SubShapeID.h>
+#include <Jolt/Physics/Collision/PhysicsMaterial.h>
+
+JPH_NAMESPACE_BEGIN
+
+class CollideShapeSettings;
+
+/// Class that constructs a ConvexShape (abstract)
+class JPH_EXPORT ConvexShapeSettings : public ShapeSettings
+{
+	JPH_DECLARE_SERIALIZABLE_ABSTRACT(JPH_EXPORT, ConvexShapeSettings)
+
+public:
+	/// Constructor
+									ConvexShapeSettings() = default;
+	explicit						ConvexShapeSettings(const PhysicsMaterial *inMaterial)		: mMaterial(inMaterial) { }
+
+	/// Set the density of the object in kg / m^3
+	void							SetDensity(float inDensity)									{ mDensity = inDensity; }
+
+	// Properties
+	RefConst<PhysicsMaterial>		mMaterial;													///< Material assigned to this shape
+	float							mDensity = 1000.0f;											///< Uniform density of the interior of the convex object (kg / m^3)
+};
+
+/// Base class for all convex shapes. Defines a virtual interface.
+class JPH_EXPORT ConvexShape : public Shape
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+	explicit						ConvexShape(EShapeSubType inSubType) : Shape(EShapeType::Convex, inSubType) { }
+									ConvexShape(EShapeSubType inSubType, const ConvexShapeSettings &inSettings, ShapeResult &outResult) : Shape(EShapeType::Convex, inSubType, inSettings, outResult), mMaterial(inSettings.mMaterial), mDensity(inSettings.mDensity) { }
+									ConvexShape(EShapeSubType inSubType, const PhysicsMaterial *inMaterial) : Shape(EShapeType::Convex, inSubType), mMaterial(inMaterial) { }
+
+	// See Shape::GetSubShapeIDBitsRecursive
+	virtual uint					GetSubShapeIDBitsRecursive() const override					{ return 0; } // Convex shapes don't have sub shapes
+
+	// See Shape::GetMaterial
+	virtual const PhysicsMaterial *	GetMaterial([[maybe_unused]] const SubShapeID &inSubShapeID) const override	{ JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); return GetMaterial(); }
+
+	// See Shape::CastRay
+	virtual bool					CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override;
+	virtual void					CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
+
+	// See: Shape::CollidePoint
+	virtual void					CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
+
+	// See Shape::GetTrianglesStart
+	virtual void					GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override;
+
+	// See Shape::GetTrianglesNext
+	virtual int						GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override;
+
+	// See Shape::GetSubmergedVolume
+	virtual void					GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const override;
+
+	/// Function that provides an interface for GJK
+	class Support
+	{
+	public:
+		/// Warning: Virtual destructor will not be called on this object!
+		virtual						~Support() = default;
+
+		/// Calculate the support vector for this convex shape (includes / excludes the convex radius depending on how this was obtained).
+		/// Support vector is relative to the center of mass of the shape.
+		virtual Vec3				GetSupport(Vec3Arg inDirection) const = 0;
+
+		/// Convex radius of shape. Collision detection on penetrating shapes is much more expensive,
+		/// so you can add a radius around objects to increase the shape. This makes it far less likely that they will actually penetrate.
+		virtual float				GetConvexRadius() const = 0;
+	};
+
+	/// Buffer to hold a Support object, used to avoid dynamic memory allocations
+	class alignas(16) SupportBuffer
+	{
+	public:
+		uint8						mData[4160];
+	};
+
+	/// How the GetSupport function should behave
+	enum class ESupportMode
+	{
+		ExcludeConvexRadius,		///< Return the shape excluding the convex radius, Support::GetConvexRadius will return the convex radius if there is one, but adding this radius may not result in the most accurate/efficient representation of shapes with sharp edges
+		IncludeConvexRadius,		///< Return the shape including the convex radius, Support::GetSupport includes the convex radius if there is one, Support::GetConvexRadius will return 0
+		Default,					///< Use both Support::GetSupport add Support::GetConvexRadius to get a support point that matches the original shape as accurately/efficiently as possible
+	};
+
+	/// Returns an object that provides the GetSupport function for this shape.
+	/// inMode determines if this support function includes or excludes the convex radius.
+	/// of the values returned by the GetSupport function. This improves numerical accuracy of the results.
+	/// inScale scales this shape in local space.
+	virtual const Support *			GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const = 0;
+
+	/// Material of the shape
+	void							SetMaterial(const PhysicsMaterial *inMaterial)				{ mMaterial = inMaterial; }
+	const PhysicsMaterial *			GetMaterial() const											{ return mMaterial != nullptr? mMaterial : PhysicsMaterial::sDefault; }
+
+	/// Set density of the shape (kg / m^3)
+	void							SetDensity(float inDensity)									{ mDensity = inDensity; }
+
+	/// Get density of the shape (kg / m^3)
+	float							GetDensity() const											{ return mDensity; }
+
+#ifdef JPH_DEBUG_RENDERER
+	// See Shape::DrawGetSupportFunction
+	virtual void					DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const override;
+
+	// See Shape::DrawGetSupportingFace
+	virtual void					DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override;
+#endif // JPH_DEBUG_RENDERER
+
+	// See Shape
+	virtual void					SaveBinaryState(StreamOut &inStream) const override;
+	virtual void					SaveMaterialState(PhysicsMaterialList &outMaterials) const override;
+	virtual void					RestoreMaterialState(const PhysicsMaterialRefC *inMaterials, uint inNumMaterials) override;
+
+	// Register shape functions with the registry
+	static void						sRegister();
+
+protected:
+	// See: Shape::RestoreBinaryState
+	virtual void					RestoreBinaryState(StreamIn &inStream) override;
+
+	/// Vertex list that forms a unit sphere
+	static const StaticArray<Vec3, 384> sUnitSphereTriangles;
+
+private:
+	// Class for GetTrianglesStart/Next
+	class							CSGetTrianglesContext;
+
+	// Helper functions called by CollisionDispatch
+	static void						sCollideConvexVsConvex(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter);
+	static void						sCastConvexVsConvex(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector);
+
+	// Properties
+	RefConst<PhysicsMaterial>		mMaterial;													///< Material assigned to this shape
+	float							mDensity = 1000.0f;											///< Uniform density of the interior of the convex object (kg / m^3)
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CylinderShape.cpp

@@ -0,0 +1,418 @@
+// 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/Shape/CylinderShape.h>
+#include <Jolt/Physics/Collision/Shape/ScaleHelpers.h>
+#include <Jolt/Physics/Collision/Shape/GetTrianglesContext.h>
+#include <Jolt/Physics/Collision/RayCast.h>
+#include <Jolt/Physics/Collision/CastResult.h>
+#include <Jolt/Physics/Collision/CollidePointResult.h>
+#include <Jolt/Physics/Collision/TransformedShape.h>
+#include <Jolt/Physics/Collision/CollideSoftBodyVertexIterator.h>
+#include <Jolt/Geometry/RayCylinder.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(CylinderShapeSettings)
+{
+	JPH_ADD_BASE_CLASS(CylinderShapeSettings, ConvexShapeSettings)
+
+	JPH_ADD_ATTRIBUTE(CylinderShapeSettings, mHalfHeight)
+	JPH_ADD_ATTRIBUTE(CylinderShapeSettings, mRadius)
+	JPH_ADD_ATTRIBUTE(CylinderShapeSettings, mConvexRadius)
+}
+
+// Approximation of top face with 8 vertices
+static const Vec3 cCylinderTopFace[] =
+{
+	Vec3(0.0f,			1.0f,	1.0f),
+	Vec3(0.707106769f,	1.0f,	0.707106769f),
+	Vec3(1.0f,			1.0f,	0.0f),
+	Vec3(0.707106769f,	1.0f,	-0.707106769f),
+	Vec3(-0.0f,			1.0f,	-1.0f),
+	Vec3(-0.707106769f,	1.0f,	-0.707106769f),
+	Vec3(-1.0f,			1.0f,	0.0f),
+	Vec3(-0.707106769f,	1.0f,	0.707106769f)
+};
+
+static const StaticArray<Vec3, 96> sUnitCylinderTriangles = []() {
+	StaticArray<Vec3, 96> verts;
+
+	const Vec3 bottom_offset(0.0f, -2.0f, 0.0f);
+
+	int num_verts = sizeof(cCylinderTopFace) / sizeof(Vec3);
+	for (int i = 0; i < num_verts; ++i)
+	{
+		Vec3 t1 = cCylinderTopFace[i];
+		Vec3 t2 = cCylinderTopFace[(i + 1) % num_verts];
+		Vec3 b1 = cCylinderTopFace[i] + bottom_offset;
+		Vec3 b2 = cCylinderTopFace[(i + 1) % num_verts] + bottom_offset;
+
+		// Top
+		verts.emplace_back(0.0f, 1.0f, 0.0f);
+		verts.push_back(t1);
+		verts.push_back(t2);
+
+		// Bottom
+		verts.emplace_back(0.0f, -1.0f, 0.0f);
+		verts.push_back(b2);
+		verts.push_back(b1);
+
+		// Side
+		verts.push_back(t1);
+		verts.push_back(b1);
+		verts.push_back(t2);
+
+		verts.push_back(t2);
+		verts.push_back(b1);
+		verts.push_back(b2);
+	}
+
+	return verts;
+}();
+
+ShapeSettings::ShapeResult CylinderShapeSettings::Create() const
+{
+	if (mCachedResult.IsEmpty())
+		Ref<Shape> shape = new CylinderShape(*this, mCachedResult);
+	return mCachedResult;
+}
+
+CylinderShape::CylinderShape(const CylinderShapeSettings &inSettings, ShapeResult &outResult) :
+	ConvexShape(EShapeSubType::Cylinder, inSettings, outResult),
+	mHalfHeight(inSettings.mHalfHeight),
+	mRadius(inSettings.mRadius),
+	mConvexRadius(inSettings.mConvexRadius)
+{
+	if (inSettings.mHalfHeight < inSettings.mConvexRadius)
+	{
+		outResult.SetError("Invalid height");
+		return;
+	}
+
+	if (inSettings.mRadius < inSettings.mConvexRadius)
+	{
+		outResult.SetError("Invalid radius");
+		return;
+	}
+
+	if (inSettings.mConvexRadius < 0.0f)
+	{
+		outResult.SetError("Invalid convex radius");
+		return;
+	}
+
+	outResult.Set(this);
+}
+
+CylinderShape::CylinderShape(float inHalfHeight, float inRadius, float inConvexRadius, const PhysicsMaterial *inMaterial) :
+	ConvexShape(EShapeSubType::Cylinder, inMaterial),
+	mHalfHeight(inHalfHeight),
+	mRadius(inRadius),
+	mConvexRadius(inConvexRadius)
+{
+	JPH_ASSERT(inHalfHeight >= inConvexRadius);
+	JPH_ASSERT(inRadius >= inConvexRadius);
+	JPH_ASSERT(inConvexRadius >= 0.0f);
+}
+
+class CylinderShape::Cylinder final : public Support
+{
+public:
+					Cylinder(float inHalfHeight, float inRadius, float inConvexRadius) :
+		mHalfHeight(inHalfHeight),
+		mRadius(inRadius),
+		mConvexRadius(inConvexRadius)
+	{
+		static_assert(sizeof(Cylinder) <= sizeof(SupportBuffer), "Buffer size too small");
+		JPH_ASSERT(IsAligned(this, alignof(Cylinder)));
+	}
+
+	virtual Vec3	GetSupport(Vec3Arg inDirection) const override
+	{
+		// Support mapping, taken from:
+		// A Fast and Robust GJK Implementation for Collision Detection of Convex Objects - Gino van den Bergen
+		// page 8
+		float x = inDirection.GetX(), y = inDirection.GetY(), z = inDirection.GetZ();
+		float o = sqrt(Square(x) + Square(z));
+		if (o > 0.0f)
+			return Vec3((mRadius * x) / o, Sign(y) * mHalfHeight, (mRadius * z) / o);
+		else
+			return Vec3(0, Sign(y) * mHalfHeight, 0);
+	}
+
+	virtual float	GetConvexRadius() const override
+	{
+		return mConvexRadius;
+	}
+
+private:
+	float			mHalfHeight;
+	float			mRadius;
+	float			mConvexRadius;
+};
+
+const ConvexShape::Support *CylinderShape::GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const
+{
+	JPH_ASSERT(IsValidScale(inScale));
+
+	// Get scaled cylinder
+	Vec3 abs_scale = inScale.Abs();
+	float scale_xz = abs_scale.GetX();
+	float scale_y = abs_scale.GetY();
+	float scaled_half_height = scale_y * mHalfHeight;
+	float scaled_radius = scale_xz * mRadius;
+	float scaled_convex_radius = ScaleHelpers::ScaleConvexRadius(mConvexRadius, inScale);
+
+	switch (inMode)
+	{
+	case ESupportMode::IncludeConvexRadius:
+	case ESupportMode::Default:
+		return new (&inBuffer) Cylinder(scaled_half_height, scaled_radius, 0.0f);
+
+	case ESupportMode::ExcludeConvexRadius:
+		return new (&inBuffer) Cylinder(scaled_half_height - scaled_convex_radius, scaled_radius - scaled_convex_radius, scaled_convex_radius);
+	}
+
+	JPH_ASSERT(false);
+	return nullptr;
+}
+
+void CylinderShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const
+{
+	JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID");
+	JPH_ASSERT(IsValidScale(inScale));
+
+	// Get scaled cylinder
+	Vec3 abs_scale = inScale.Abs();
+	float scale_xz = abs_scale.GetX();
+	float scale_y = abs_scale.GetY();
+	float scaled_half_height = scale_y * mHalfHeight;
+	float scaled_radius = scale_xz * mRadius;
+
+	float x = inDirection.GetX(), y = inDirection.GetY(), z = inDirection.GetZ();
+	float xz_sq = Square(x) + Square(z);
+	float y_sq = Square(y);
+
+	// Check which component is bigger
+	if (xz_sq > y_sq)
+	{
+		// Hitting side
+		float f = -scaled_radius / sqrt(xz_sq);
+		float vx = x * f;
+		float vz = z * f;
+		outVertices.push_back(inCenterOfMassTransform * Vec3(vx, scaled_half_height, vz));
+		outVertices.push_back(inCenterOfMassTransform * Vec3(vx, -scaled_half_height, vz));
+	}
+	else
+	{
+		// Hitting top or bottom
+
+		// When the inDirection is more than 5 degrees from vertical, align the vertices so that 1 of the vertices
+		// points towards inDirection in the XZ plane. This ensures that we always have a vertex towards max penetration depth.
+		Mat44 transform = inCenterOfMassTransform;
+		if (xz_sq > 0.00765427f * y_sq)
+		{
+			Vec4 base_x = Vec4(x, 0, z, 0) / sqrt(xz_sq);
+			Vec4 base_z = base_x.Swizzle<SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_X, SWIZZLE_W>() * Vec4(-1, 0, 1, 0);
+			transform = transform * Mat44(base_x, Vec4(0, 1, 0, 0), base_z, Vec4(0, 0, 0, 1));
+		}
+
+		// Adjust for scale and height
+		Vec3 multiplier = y < 0.0f? Vec3(scaled_radius, scaled_half_height, scaled_radius) : Vec3(-scaled_radius, -scaled_half_height, scaled_radius);
+		transform = transform.PreScaled(multiplier);
+
+		for (const Vec3 &v : cCylinderTopFace)
+			outVertices.push_back(transform * v);
+	}
+}
+
+MassProperties CylinderShape::GetMassProperties() const
+{
+	MassProperties p;
+
+	// Mass is surface of circle * height
+	float radius_sq = Square(mRadius);
+	float height = 2.0f * mHalfHeight;
+	p.mMass = JPH_PI * radius_sq * height * GetDensity();
+
+	// Inertia according to https://en.wikipedia.org/wiki/List_of_moments_of_inertia:
+	float inertia_y = radius_sq * p.mMass * 0.5f;
+	float inertia_x = inertia_y * 0.5f + p.mMass * height * height / 12.0f;
+	float inertia_z = inertia_x;
+
+	// Set inertia
+	p.mInertia = Mat44::sScale(Vec3(inertia_x, inertia_y, inertia_z));
+
+	return p;
+}
+
+Vec3 CylinderShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const
+{
+	JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID");
+
+	// Calculate distance to infinite cylinder surface
+	Vec3 local_surface_position_xz(inLocalSurfacePosition.GetX(), 0, inLocalSurfacePosition.GetZ());
+	float local_surface_position_xz_len = local_surface_position_xz.Length();
+	float distance_to_curved_surface = abs(local_surface_position_xz_len - mRadius);
+
+	// Calculate distance to top or bottom plane
+	float distance_to_top_or_bottom = abs(abs(inLocalSurfacePosition.GetY()) - mHalfHeight);
+
+	// Return normal according to closest surface
+	if (distance_to_curved_surface < distance_to_top_or_bottom)
+		return local_surface_position_xz / local_surface_position_xz_len;
+	else
+		return inLocalSurfacePosition.GetY() > 0.0f? Vec3::sAxisY() : -Vec3::sAxisY();
+}
+
+AABox CylinderShape::GetLocalBounds() const
+{
+	Vec3 extent = Vec3(mRadius, mHalfHeight, mRadius);
+	return AABox(-extent, extent);
+}
+
+#ifdef JPH_DEBUG_RENDERER
+void CylinderShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
+{
+	DebugRenderer::EDrawMode draw_mode = inDrawWireframe? DebugRenderer::EDrawMode::Wireframe : DebugRenderer::EDrawMode::Solid;
+	inRenderer->DrawCylinder(inCenterOfMassTransform * Mat44::sScale(inScale.Abs()), mHalfHeight, mRadius, inUseMaterialColors? GetMaterial()->GetDebugColor() : inColor, DebugRenderer::ECastShadow::On, draw_mode);
+}
+#endif // JPH_DEBUG_RENDERER
+
+bool CylinderShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const
+{
+	// Test ray against capsule
+	float fraction = RayCylinder(inRay.mOrigin, inRay.mDirection, mHalfHeight, mRadius);
+	if (fraction < ioHit.mFraction)
+	{
+		ioHit.mFraction = fraction;
+		ioHit.mSubShapeID2 = inSubShapeIDCreator.GetID();
+		return true;
+	}
+	return false;
+}
+
+void CylinderShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	// Test shape filter
+	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
+		return;
+
+	// Check if the point is in the cylinder
+	if (abs(inPoint.GetY()) <= mHalfHeight											// Within the height
+		&& Square(inPoint.GetX()) + Square(inPoint.GetZ()) <= Square(mRadius))		// Within the radius
+		ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator.GetID() });
+}
+
+void CylinderShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const
+{
+	JPH_ASSERT(IsValidScale(inScale));
+
+	Mat44 inverse_transform = inCenterOfMassTransform.InversedRotationTranslation();
+
+	// Get scaled cylinder
+	Vec3 abs_scale = inScale.Abs();
+	float half_height = abs_scale.GetY() * mHalfHeight;
+	float radius = abs_scale.GetX() * mRadius;
+
+	for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v)
+		if (v.GetInvMass() > 0.0f)
+		{
+			Vec3 local_pos = inverse_transform * v.GetPosition();
+
+			// Calculate penetration into side surface
+			Vec3 side_normal = local_pos;
+			side_normal.SetY(0.0f);
+			float side_normal_length = side_normal.Length();
+			float side_penetration = radius - side_normal_length;
+
+			// Calculate penetration into top or bottom plane
+			float top_penetration = half_height - abs(local_pos.GetY());
+
+			Vec3 point, normal;
+			if (side_penetration < 0.0f && top_penetration < 0.0f)
+			{
+				// We're outside the cylinder height and radius
+				point = side_normal * (radius / side_normal_length) + Vec3(0, half_height * Sign(local_pos.GetY()), 0);
+				normal = (local_pos - point).NormalizedOr(Vec3::sAxisY());
+			}
+			else if (side_penetration < top_penetration)
+			{
+				// Side surface is closest
+				normal = side_normal_length > 0.0f? side_normal / side_normal_length : Vec3::sAxisX();
+				point = radius * normal;
+			}
+			else
+			{
+				// Top or bottom plane is closest
+				normal = Vec3(0, Sign(local_pos.GetY()), 0);
+				point = half_height * normal;
+			}
+
+			// Calculate penetration
+			Plane plane = Plane::sFromPointAndNormal(point, normal);
+			float penetration = -plane.SignedDistance(local_pos);
+			if (v.UpdatePenetration(penetration))
+				v.SetCollision(plane.GetTransformed(inCenterOfMassTransform), inCollidingShapeIndex);
+		}
+}
+
+void CylinderShape::GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const
+{
+	Mat44 unit_cylinder_transform(Vec4(mRadius, 0, 0, 0), Vec4(0, mHalfHeight, 0, 0), Vec4(0, 0, mRadius, 0), Vec4(0, 0, 0, 1));
+	new (&ioContext) GetTrianglesContextVertexList(inPositionCOM, inRotation, inScale, unit_cylinder_transform, sUnitCylinderTriangles.data(), sUnitCylinderTriangles.size(), GetMaterial());
+}
+
+int CylinderShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) const
+{
+	return ((GetTrianglesContextVertexList &)ioContext).GetTrianglesNext(inMaxTrianglesRequested, outTriangleVertices, outMaterials);
+}
+
+void CylinderShape::SaveBinaryState(StreamOut &inStream) const
+{
+	ConvexShape::SaveBinaryState(inStream);
+
+	inStream.Write(mHalfHeight);
+	inStream.Write(mRadius);
+	inStream.Write(mConvexRadius);
+}
+
+void CylinderShape::RestoreBinaryState(StreamIn &inStream)
+{
+	ConvexShape::RestoreBinaryState(inStream);
+
+	inStream.Read(mHalfHeight);
+	inStream.Read(mRadius);
+	inStream.Read(mConvexRadius);
+}
+
+bool CylinderShape::IsValidScale(Vec3Arg inScale) const
+{
+	return ConvexShape::IsValidScale(inScale) && ScaleHelpers::IsUniformScaleXZ(inScale.Abs());
+}
+
+Vec3 CylinderShape::MakeScaleValid(Vec3Arg inScale) const
+{
+	Vec3 scale = ScaleHelpers::MakeNonZeroScale(inScale);
+
+	return scale.GetSign() * ScaleHelpers::MakeUniformScaleXZ(scale.Abs());
+}
+
+void CylinderShape::sRegister()
+{
+	ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::Cylinder);
+	f.mConstruct = []() -> Shape * { return new CylinderShape; };
+	f.mColor = Color::sGreen;
+}
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CylinderShape.h

@@ -0,0 +1,126 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/Shape/ConvexShape.h>
+#include <Jolt/Physics/PhysicsSettings.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Class that constructs a CylinderShape
+class JPH_EXPORT CylinderShapeSettings final : public ConvexShapeSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, CylinderShapeSettings)
+
+public:
+	/// Default constructor for deserialization
+							CylinderShapeSettings() = default;
+
+	/// Create a shape centered around the origin with one top at (0, -inHalfHeight, 0) and the other at (0, inHalfHeight, 0) and radius inRadius.
+	/// (internally the convex radius will be subtracted from the cylinder the total cylinder will not grow with the convex radius, but the edges of the cylinder will be rounded a bit).
+							CylinderShapeSettings(float inHalfHeight, float inRadius, float inConvexRadius = cDefaultConvexRadius, const PhysicsMaterial *inMaterial = nullptr) : ConvexShapeSettings(inMaterial), mHalfHeight(inHalfHeight), mRadius(inRadius), mConvexRadius(inConvexRadius) { }
+
+	// See: ShapeSettings
+	virtual ShapeResult		Create() const override;
+
+	float					mHalfHeight = 0.0f;
+	float					mRadius = 0.0f;
+	float					mConvexRadius = 0.0f;
+};
+
+/// A cylinder
+class JPH_EXPORT CylinderShape final : public ConvexShape
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+							CylinderShape() : ConvexShape(EShapeSubType::Cylinder) { }
+							CylinderShape(const CylinderShapeSettings &inSettings, ShapeResult &outResult);
+
+	/// Create a shape centered around the origin with one top at (0, -inHalfHeight, 0) and the other at (0, inHalfHeight, 0) and radius inRadius.
+	/// (internally the convex radius will be subtracted from the cylinder the total cylinder will not grow with the convex radius, but the edges of the cylinder will be rounded a bit).
+							CylinderShape(float inHalfHeight, float inRadius, float inConvexRadius = cDefaultConvexRadius, const PhysicsMaterial *inMaterial = nullptr);
+
+	/// Get half height of cylinder
+	float					GetHalfHeight() const														{ return mHalfHeight; }
+
+	/// Get radius of cylinder
+	float					GetRadius() const															{ return mRadius; }
+
+	// See Shape::GetLocalBounds
+	virtual AABox			GetLocalBounds() const override;
+
+	// See Shape::GetInnerRadius
+	virtual float			GetInnerRadius() const override												{ return min(mHalfHeight, mRadius); }
+
+	// See Shape::GetMassProperties
+	virtual MassProperties	GetMassProperties() const override;
+
+	// See Shape::GetSurfaceNormal
+	virtual Vec3			GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override;
+
+	// See Shape::GetSupportingFace
+	virtual void			GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override;
+
+	// See ConvexShape::GetSupportFunction
+	virtual const Support *	GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const override;
+
+#ifdef JPH_DEBUG_RENDERER
+	// See Shape::Draw
+	virtual void			Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override;
+#endif // JPH_DEBUG_RENDERER
+
+	// See Shape::CastRay
+	using ConvexShape::CastRay;
+	virtual bool			CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override;
+
+	// See: Shape::CollidePoint
+	virtual void			CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
+
+	// See: Shape::CollideSoftBodyVertices
+	virtual void			CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override;
+
+	// See Shape::GetTrianglesStart
+	virtual void			GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override;
+
+	// See Shape::GetTrianglesNext
+	virtual int				GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override;
+
+	// See Shape
+	virtual void			SaveBinaryState(StreamOut &inStream) const override;
+
+	// See Shape::GetStats
+	virtual Stats			GetStats() const override												{ return Stats(sizeof(*this), 0); }
+
+	// See Shape::GetVolume
+	virtual float			GetVolume() const override												{ return 2.0f * JPH_PI * mHalfHeight * Square(mRadius); }
+
+	/// Get the convex radius of this cylinder
+	float					GetConvexRadius() const													{ return mConvexRadius; }
+
+	// See Shape::IsValidScale
+	virtual bool			IsValidScale(Vec3Arg inScale) const override;
+
+	// See Shape::MakeScaleValid
+	virtual Vec3			MakeScaleValid(Vec3Arg inScale) const override;
+
+	// Register shape functions with the registry
+	static void				sRegister();
+
+protected:
+	// See: Shape::RestoreBinaryState
+	virtual void			RestoreBinaryState(StreamIn &inStream) override;
+
+private:
+	// Class for GetSupportFunction
+	class					Cylinder;
+
+	float					mHalfHeight = 0.0f;
+	float					mRadius = 0.0f;
+	float					mConvexRadius = 0.0f;
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/DecoratedShape.cpp

@@ -0,0 +1,87 @@
+// 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/Shape/DecoratedShape.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT(DecoratedShapeSettings)
+{
+	JPH_ADD_BASE_CLASS(DecoratedShapeSettings, ShapeSettings)
+
+	JPH_ADD_ATTRIBUTE(DecoratedShapeSettings, mInnerShape)
+}
+
+DecoratedShape::DecoratedShape(EShapeSubType inSubType, const DecoratedShapeSettings &inSettings, ShapeResult &outResult) :
+	Shape(EShapeType::Decorated, inSubType, inSettings, outResult)
+{
+	// Check that there's a shape
+	if (inSettings.mInnerShape == nullptr && inSettings.mInnerShapePtr == nullptr)
+	{
+		outResult.SetError("Inner shape is null!");
+		return;
+	}
+
+	if (inSettings.mInnerShapePtr != nullptr)
+	{
+		// Use provided shape
+		mInnerShape = inSettings.mInnerShapePtr;
+	}
+	else
+	{
+		// Create child shape
+		ShapeResult child_result = inSettings.mInnerShape->Create();
+		if (!child_result.IsValid())
+		{
+			outResult = child_result;
+			return;
+		}
+		mInnerShape = child_result.Get();
+	}
+}
+
+const PhysicsMaterial *DecoratedShape::GetMaterial(const SubShapeID &inSubShapeID) const
+{
+	return mInnerShape->GetMaterial(inSubShapeID);
+}
+
+void DecoratedShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const
+{
+	mInnerShape->GetSupportingFace(inSubShapeID, inDirection, inScale, inCenterOfMassTransform, outVertices);
+}
+
+uint64 DecoratedShape::GetSubShapeUserData(const SubShapeID &inSubShapeID) const
+{
+	return mInnerShape->GetSubShapeUserData(inSubShapeID);
+}
+
+void DecoratedShape::SaveSubShapeState(ShapeList &outSubShapes) const
+{
+	outSubShapes.clear();
+	outSubShapes.push_back(mInnerShape);
+}
+
+void DecoratedShape::RestoreSubShapeState(const ShapeRefC *inSubShapes, uint inNumShapes)
+{
+	JPH_ASSERT(inNumShapes == 1);
+	mInnerShape = inSubShapes[0];
+}
+
+Shape::Stats DecoratedShape::GetStatsRecursive(VisitedShapes &ioVisitedShapes) const
+{
+	// Get own stats
+	Stats stats = Shape::GetStatsRecursive(ioVisitedShapes);
+
+	// Add child stats
+	Stats child_stats = mInnerShape->GetStatsRecursive(ioVisitedShapes);
+	stats.mSizeBytes += child_stats.mSizeBytes;
+	stats.mNumTriangles += child_stats.mNumTriangles;
+
+	return stats;
+}
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/DecoratedShape.h

@@ -0,0 +1,80 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/Shape/Shape.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Class that constructs a DecoratedShape
+class JPH_EXPORT DecoratedShapeSettings : public ShapeSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, DecoratedShapeSettings)
+
+public:
+	/// Default constructor for deserialization
+									DecoratedShapeSettings() = default;
+
+	/// Constructor that decorates another shape
+	explicit						DecoratedShapeSettings(const ShapeSettings *inShape)	: mInnerShape(inShape) { }
+	explicit						DecoratedShapeSettings(const Shape *inShape)			: mInnerShapePtr(inShape) { }
+
+	RefConst<ShapeSettings>			mInnerShape;											///< Sub shape (either this or mShapePtr needs to be filled up)
+	RefConst<Shape>					mInnerShapePtr;											///< Sub shape (either this or mShape needs to be filled up)
+};
+
+/// Base class for shapes that decorate another shape with extra functionality (e.g. scale, translation etc.)
+class JPH_EXPORT DecoratedShape : public Shape
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+	explicit						DecoratedShape(EShapeSubType inSubType) : Shape(EShapeType::Decorated, inSubType) { }
+									DecoratedShape(EShapeSubType inSubType, const Shape *inInnerShape) : Shape(EShapeType::Decorated, inSubType), mInnerShape(inInnerShape) { }
+									DecoratedShape(EShapeSubType inSubType, const DecoratedShapeSettings &inSettings, ShapeResult &outResult);
+
+	/// Access to the decorated inner shape
+	const Shape *					GetInnerShape() const									{ return mInnerShape; }
+
+	// See Shape::MustBeStatic
+	virtual bool					MustBeStatic() const override							{ return mInnerShape->MustBeStatic(); }
+
+	// See Shape::GetCenterOfMass
+	virtual Vec3					GetCenterOfMass() const override						{ return mInnerShape->GetCenterOfMass(); }
+
+	// See Shape::GetSubShapeIDBitsRecursive
+	virtual uint					GetSubShapeIDBitsRecursive() const override				{ return mInnerShape->GetSubShapeIDBitsRecursive(); }
+
+	// See Shape::GetLeafShape
+	virtual const Shape *			GetLeafShape(const SubShapeID &inSubShapeID, SubShapeID &outRemainder) const override { return mInnerShape->GetLeafShape(inSubShapeID, outRemainder); }
+
+	// See Shape::GetMaterial
+	virtual const PhysicsMaterial *	GetMaterial(const SubShapeID &inSubShapeID) const override;
+
+	// See Shape::GetSupportingFace
+	virtual void					GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override;
+
+	// See Shape::GetSubShapeUserData
+	virtual uint64					GetSubShapeUserData(const SubShapeID &inSubShapeID) const override;
+
+	// See Shape
+	virtual void					SaveSubShapeState(ShapeList &outSubShapes) const override;
+	virtual void					RestoreSubShapeState(const ShapeRefC *inSubShapes, uint inNumShapes) override;
+
+	// See Shape::GetStatsRecursive
+	virtual Stats					GetStatsRecursive(VisitedShapes &ioVisitedShapes) const override;
+
+	// See Shape::IsValidScale
+	virtual bool					IsValidScale(Vec3Arg inScale) const override			{ return mInnerShape->IsValidScale(inScale); }
+
+	// See Shape::MakeScaleValid
+	virtual Vec3					MakeScaleValid(Vec3Arg inScale) const override			{ return mInnerShape->MakeScaleValid(inScale); }
+
+protected:
+	RefConst<Shape>					mInnerShape;
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/EmptyShape.cpp

@@ -0,0 +1,65 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2024 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Collision/Shape/EmptyShape.h>
+#include <Jolt/Physics/Collision/CollisionDispatch.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(EmptyShapeSettings)
+{
+	JPH_ADD_BASE_CLASS(EmptyShapeSettings, ShapeSettings)
+
+	JPH_ADD_ATTRIBUTE(EmptyShapeSettings, mCenterOfMass)
+}
+
+ShapeSettings::ShapeResult EmptyShapeSettings::Create() const
+{
+	if (mCachedResult.IsEmpty())
+		new EmptyShape(*this, mCachedResult);
+
+	return mCachedResult;
+}
+
+MassProperties EmptyShape::GetMassProperties() const
+{
+	MassProperties mass_properties;
+	mass_properties.mMass = 1.0f;
+	mass_properties.mInertia = Mat44::sIdentity();
+	return mass_properties;
+}
+
+#ifdef JPH_DEBUG_RENDERER
+void EmptyShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, [[maybe_unused]] bool inUseMaterialColors, [[maybe_unused]] bool inDrawWireframe) const
+{
+	inRenderer->DrawMarker(inCenterOfMassTransform.GetTranslation(), inColor, abs(inScale.GetX()) * 0.1f);
+}
+#endif // JPH_DEBUG_RENDERER
+
+void EmptyShape::sRegister()
+{
+	ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::Empty);
+	f.mConstruct = []() -> Shape * { return new EmptyShape; };
+	f.mColor = Color::sBlack;
+
+	auto collide_empty = []([[maybe_unused]] const Shape *inShape1, [[maybe_unused]] const Shape *inShape2, [[maybe_unused]] Vec3Arg inScale1, [[maybe_unused]] Vec3Arg inScale2, [[maybe_unused]] Mat44Arg inCenterOfMassTransform1, [[maybe_unused]] Mat44Arg inCenterOfMassTransform2, [[maybe_unused]] const SubShapeIDCreator &inSubShapeIDCreator1, [[maybe_unused]] const SubShapeIDCreator &inSubShapeIDCreator2, [[maybe_unused]] const CollideShapeSettings &inCollideShapeSettings, [[maybe_unused]] CollideShapeCollector &ioCollector, [[maybe_unused]] const ShapeFilter &inShapeFilter) { /* Do Nothing */ };
+	auto cast_empty = []([[maybe_unused]] const ShapeCast &inShapeCast, [[maybe_unused]] const ShapeCastSettings &inShapeCastSettings, [[maybe_unused]] const Shape *inShape, [[maybe_unused]] Vec3Arg inScale, [[maybe_unused]] const ShapeFilter &inShapeFilter, [[maybe_unused]] Mat44Arg inCenterOfMassTransform2, [[maybe_unused]] const SubShapeIDCreator &inSubShapeIDCreator1, [[maybe_unused]] const SubShapeIDCreator &inSubShapeIDCreator2, [[maybe_unused]] CastShapeCollector &ioCollector) { /* Do nothing */ };
+
+	for (const EShapeSubType s : sAllSubShapeTypes)
+	{
+		CollisionDispatch::sRegisterCollideShape(EShapeSubType::Empty, s, collide_empty);
+		CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::Empty, collide_empty);
+
+		CollisionDispatch::sRegisterCastShape(EShapeSubType::Empty, s, cast_empty);
+		CollisionDispatch::sRegisterCastShape(s, EShapeSubType::Empty, cast_empty);
+	}
+}
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/EmptyShape.h

@@ -0,0 +1,75 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2024 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/Shape/Shape.h>
+#include <Jolt/Physics/Collision/PhysicsMaterial.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Class that constructs an EmptyShape
+class JPH_EXPORT EmptyShapeSettings final : public ShapeSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, EmptyShapeSettings)
+
+public:
+							EmptyShapeSettings() = default;
+	explicit				EmptyShapeSettings(Vec3Arg inCenterOfMass) : mCenterOfMass(inCenterOfMass) { }
+
+	ShapeResult				Create() const override;
+
+	Vec3					mCenterOfMass = Vec3::sZero();									///< Determines the center of mass for this shape
+};
+
+/// An empty shape that has no volume and collides with nothing.
+///
+/// Possible use cases:
+/// - As a placeholder for a shape that will be created later. E.g. if you first need to create a body and only then know what shape it will have.
+/// - If you need a kinematic body to attach a constraint to, but you don't want the body to collide with anything.
+///
+/// Note that, if possible, you should also put your body in an ObjectLayer that doesn't collide with anything.
+/// This ensures that collisions will be filtered out at broad phase level instead of at narrow phase level, this is more efficient.
+class JPH_EXPORT EmptyShape final : public Shape
+{
+public:
+	// Constructor
+							EmptyShape() : Shape(EShapeType::Empty, EShapeSubType::Empty) { }
+	explicit				EmptyShape(Vec3Arg inCenterOfMass) : Shape(EShapeType::Empty, EShapeSubType::Empty), mCenterOfMass(inCenterOfMass) { }
+							EmptyShape(const EmptyShapeSettings &inSettings, ShapeResult &outResult) : Shape(EShapeType::Empty, EShapeSubType::Empty, inSettings, outResult), mCenterOfMass(inSettings.mCenterOfMass) { outResult.Set(this); }
+
+	// See: Shape
+	Vec3					GetCenterOfMass() const override								{ return mCenterOfMass; }
+	AABox					GetLocalBounds() const override									{ return { Vec3::sZero(), Vec3::sZero() }; }
+	uint					GetSubShapeIDBitsRecursive() const override						{ return 0; }
+	float					GetInnerRadius() const override									{ return 0.0f; }
+	MassProperties			GetMassProperties() const override;
+	const PhysicsMaterial *	GetMaterial([[maybe_unused]] const SubShapeID &inSubShapeID) const override { return PhysicsMaterial::sDefault; }
+	virtual Vec3			GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override { return Vec3::sZero(); }
+	virtual void			GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy
+#ifdef JPH_DEBUG_RENDERER // Not using JPH_IF_DEBUG_RENDERER for Doxygen
+		, RVec3Arg inBaseOffset
+#endif
+		) const override																	{ outTotalVolume = 0.0f; outSubmergedVolume = 0.0f; outCenterOfBuoyancy = Vec3::sZero(); }
+#ifdef JPH_DEBUG_RENDERER
+	virtual void			Draw([[maybe_unused]] DebugRenderer *inRenderer, [[maybe_unused]] RMat44Arg inCenterOfMassTransform, [[maybe_unused]] Vec3Arg inScale, [[maybe_unused]] ColorArg inColor, [[maybe_unused]] bool inUseMaterialColors, [[maybe_unused]] bool inDrawWireframe) const override;
+#endif // JPH_DEBUG_RENDERER
+	virtual bool			CastRay([[maybe_unused]] const RayCast &inRay, [[maybe_unused]] const SubShapeIDCreator &inSubShapeIDCreator, [[maybe_unused]] RayCastResult &ioHit) const override { return false; }
+	virtual void			CastRay([[maybe_unused]] const RayCast &inRay, [[maybe_unused]] const RayCastSettings &inRayCastSettings, [[maybe_unused]] const SubShapeIDCreator &inSubShapeIDCreator, [[maybe_unused]] CastRayCollector &ioCollector, [[maybe_unused]] const ShapeFilter &inShapeFilter = { }) const override { /* Do nothing */ }
+	virtual void			CollidePoint([[maybe_unused]] Vec3Arg inPoint, [[maybe_unused]] const SubShapeIDCreator &inSubShapeIDCreator, [[maybe_unused]] CollidePointCollector &ioCollector, [[maybe_unused]] const ShapeFilter &inShapeFilter = { }) const override { /* Do nothing */ }
+	virtual void			CollideSoftBodyVertices([[maybe_unused]] Mat44Arg inCenterOfMassTransform, [[maybe_unused]] Vec3Arg inScale, [[maybe_unused]] const CollideSoftBodyVertexIterator &inVertices, [[maybe_unused]] uint inNumVertices, [[maybe_unused]] int inCollidingShapeIndex) const override { /* Do nothing */ }
+	virtual void			GetTrianglesStart([[maybe_unused]] GetTrianglesContext &ioContext, [[maybe_unused]] const AABox &inBox, [[maybe_unused]] Vec3Arg inPositionCOM, [[maybe_unused]] QuatArg inRotation, [[maybe_unused]] Vec3Arg inScale) const override { /* Do nothing */ }
+	virtual int				GetTrianglesNext([[maybe_unused]] GetTrianglesContext &ioContext, [[maybe_unused]] int inMaxTrianglesRequested, [[maybe_unused]] Float3 *outTriangleVertices, [[maybe_unused]] const PhysicsMaterial **outMaterials = nullptr) const override { return 0; }
+	Stats					GetStats() const override										{ return { sizeof(*this), 0 }; }
+	float					GetVolume() const override										{ return 0.0f; }
+	bool					IsValidScale([[maybe_unused]] Vec3Arg inScale) const override	{ return true; }
+
+	// Register shape functions with the registry
+	static void				sRegister();
+
+private:
+	Vec3					mCenterOfMass = Vec3::sZero();
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/GetTrianglesContext.h

@@ -0,0 +1,248 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/Shape/Shape.h>
+
+JPH_NAMESPACE_BEGIN
+
+class PhysicsMaterial;
+
+/// Implementation of GetTrianglesStart/Next that uses a fixed list of vertices for the triangles. These are transformed into world space when getting the triangles.
+class GetTrianglesContextVertexList
+{
+public:
+	/// Constructor, to be called in GetTrianglesStart
+					GetTrianglesContextVertexList(Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, Mat44Arg inLocalTransform, const Vec3 *inTriangleVertices, size_t inNumTriangleVertices, const PhysicsMaterial *inMaterial) :
+		mLocalToWorld(Mat44::sRotationTranslation(inRotation, inPositionCOM) * Mat44::sScale(inScale) * inLocalTransform),
+		mTriangleVertices(inTriangleVertices),
+		mNumTriangleVertices(inNumTriangleVertices),
+		mMaterial(inMaterial),
+		mIsInsideOut(ScaleHelpers::IsInsideOut(inScale))
+	{
+		static_assert(sizeof(GetTrianglesContextVertexList) <= sizeof(Shape::GetTrianglesContext), "GetTrianglesContext too small");
+		JPH_ASSERT(IsAligned(this, alignof(GetTrianglesContextVertexList)));
+		JPH_ASSERT(inNumTriangleVertices % 3 == 0);
+	}
+
+	/// @see Shape::GetTrianglesNext
+	int				GetTrianglesNext(int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials)
+	{
+		JPH_ASSERT(inMaxTrianglesRequested >= Shape::cGetTrianglesMinTrianglesRequested);
+
+		int total_num_vertices = min(inMaxTrianglesRequested * 3, int(mNumTriangleVertices - mCurrentVertex));
+
+		if (mIsInsideOut)
+		{
+			// Store triangles flipped
+			for (const Vec3 *v = mTriangleVertices + mCurrentVertex, *v_end = v + total_num_vertices; v < v_end; v += 3)
+			{
+				(mLocalToWorld * v[0]).StoreFloat3(outTriangleVertices++);
+				(mLocalToWorld * v[2]).StoreFloat3(outTriangleVertices++);
+				(mLocalToWorld * v[1]).StoreFloat3(outTriangleVertices++);
+			}
+		}
+		else
+		{
+			// Store triangles
+			for (const Vec3 *v = mTriangleVertices + mCurrentVertex, *v_end = v + total_num_vertices; v < v_end; v += 3)
+			{
+				(mLocalToWorld * v[0]).StoreFloat3(outTriangleVertices++);
+				(mLocalToWorld * v[1]).StoreFloat3(outTriangleVertices++);
+				(mLocalToWorld * v[2]).StoreFloat3(outTriangleVertices++);
+			}
+		}
+
+		// Update the current vertex to point to the next vertex to get
+		mCurrentVertex += total_num_vertices;
+		int total_num_triangles = total_num_vertices / 3;
+
+		// Store materials
+		if (outMaterials != nullptr)
+			for (const PhysicsMaterial **m = outMaterials, **m_end = outMaterials + total_num_triangles; m < m_end; ++m)
+				*m = mMaterial;
+
+		return total_num_triangles;
+	}
+
+	/// Helper function that creates a vertex list of a half unit sphere (top part)
+	template <class A>
+	static void		sCreateHalfUnitSphereTop(A &ioVertices, int inDetailLevel)
+	{
+		sCreateUnitSphereHelper(ioVertices,  Vec3::sAxisX(),  Vec3::sAxisY(),  Vec3::sAxisZ(), inDetailLevel);
+		sCreateUnitSphereHelper(ioVertices,  Vec3::sAxisY(), -Vec3::sAxisX(),  Vec3::sAxisZ(), inDetailLevel);
+		sCreateUnitSphereHelper(ioVertices,  Vec3::sAxisY(),  Vec3::sAxisX(), -Vec3::sAxisZ(), inDetailLevel);
+		sCreateUnitSphereHelper(ioVertices, -Vec3::sAxisX(),  Vec3::sAxisY(), -Vec3::sAxisZ(), inDetailLevel);
+	}
+
+	/// Helper function that creates a vertex list of a half unit sphere (bottom part)
+	template <class A>
+	static void		sCreateHalfUnitSphereBottom(A &ioVertices, int inDetailLevel)
+	{
+		sCreateUnitSphereHelper(ioVertices, -Vec3::sAxisX(), -Vec3::sAxisY(),  Vec3::sAxisZ(), inDetailLevel);
+		sCreateUnitSphereHelper(ioVertices, -Vec3::sAxisY(),  Vec3::sAxisX(),  Vec3::sAxisZ(), inDetailLevel);
+		sCreateUnitSphereHelper(ioVertices,  Vec3::sAxisX(), -Vec3::sAxisY(), -Vec3::sAxisZ(), inDetailLevel);
+		sCreateUnitSphereHelper(ioVertices, -Vec3::sAxisY(), -Vec3::sAxisX(), -Vec3::sAxisZ(), inDetailLevel);
+	}
+
+	/// Helper function that creates an open cylinder of half height 1 and radius 1
+	template <class A>
+	static void		sCreateUnitOpenCylinder(A &ioVertices, int inDetailLevel)
+	{
+		const Vec3 bottom_offset(0.0f, -2.0f, 0.0f);
+		int num_verts = 4 * (1 << inDetailLevel);
+		for (int i = 0; i < num_verts; ++i)
+		{
+			float angle1 = 2.0f * JPH_PI * (float(i) / num_verts);
+			float angle2 = 2.0f * JPH_PI * (float(i + 1) / num_verts);
+
+			Vec3 t1(Sin(angle1), 1.0f, Cos(angle1));
+			Vec3 t2(Sin(angle2), 1.0f, Cos(angle2));
+			Vec3 b1 = t1 + bottom_offset;
+			Vec3 b2 = t2 + bottom_offset;
+
+			ioVertices.push_back(t1);
+			ioVertices.push_back(b1);
+			ioVertices.push_back(t2);
+
+			ioVertices.push_back(t2);
+			ioVertices.push_back(b1);
+			ioVertices.push_back(b2);
+		}
+	}
+
+private:
+	/// Recursive helper function for creating a sphere
+	template <class A>
+	static void		sCreateUnitSphereHelper(A &ioVertices, Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3, int inLevel)
+	{
+		Vec3 center1 = (inV1 + inV2).Normalized();
+		Vec3 center2 = (inV2 + inV3).Normalized();
+		Vec3 center3 = (inV3 + inV1).Normalized();
+
+		if (inLevel > 0)
+		{
+			int new_level = inLevel - 1;
+			sCreateUnitSphereHelper(ioVertices, inV1, center1, center3, new_level);
+			sCreateUnitSphereHelper(ioVertices, center1, center2, center3, new_level);
+			sCreateUnitSphereHelper(ioVertices, center1, inV2, center2, new_level);
+			sCreateUnitSphereHelper(ioVertices, center3, center2, inV3, new_level);
+		}
+		else
+		{
+			ioVertices.push_back(inV1);
+			ioVertices.push_back(inV2);
+			ioVertices.push_back(inV3);
+		}
+	}
+
+	Mat44					mLocalToWorld;
+	const Vec3 *			mTriangleVertices;
+	size_t					mNumTriangleVertices;
+	size_t					mCurrentVertex = 0;
+	const PhysicsMaterial *	mMaterial;
+	bool					mIsInsideOut;
+};
+
+/// Implementation of GetTrianglesStart/Next that uses a multiple fixed lists of vertices for the triangles. These are transformed into world space when getting the triangles.
+class GetTrianglesContextMultiVertexList
+{
+public:
+	/// Constructor, to be called in GetTrianglesStart
+					GetTrianglesContextMultiVertexList(bool inIsInsideOut, const PhysicsMaterial *inMaterial) :
+		mMaterial(inMaterial),
+		mIsInsideOut(inIsInsideOut)
+	{
+		static_assert(sizeof(GetTrianglesContextMultiVertexList) <= sizeof(Shape::GetTrianglesContext), "GetTrianglesContext too small");
+		JPH_ASSERT(IsAligned(this, alignof(GetTrianglesContextMultiVertexList)));
+	}
+
+	/// Add a mesh part and its transform
+	void			AddPart(Mat44Arg inLocalToWorld, const Vec3 *inTriangleVertices, size_t inNumTriangleVertices)
+	{
+		JPH_ASSERT(inNumTriangleVertices % 3 == 0);
+
+		mParts.push_back({ inLocalToWorld, inTriangleVertices, inNumTriangleVertices });
+	}
+
+	/// @see Shape::GetTrianglesNext
+	int				GetTrianglesNext(int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials)
+	{
+		JPH_ASSERT(inMaxTrianglesRequested >= Shape::cGetTrianglesMinTrianglesRequested);
+
+		int total_num_vertices = 0;
+		int max_vertices_requested = inMaxTrianglesRequested * 3;
+
+		// Loop over parts
+		for (; mCurrentPart < mParts.size(); ++mCurrentPart)
+		{
+			const Part &part = mParts[mCurrentPart];
+
+			// Calculate how many vertices to take from this part
+			int part_num_vertices = min(max_vertices_requested, int(part.mNumTriangleVertices - mCurrentVertex));
+			if (part_num_vertices == 0)
+				break;
+
+			max_vertices_requested -= part_num_vertices;
+			total_num_vertices += part_num_vertices;
+
+			if (mIsInsideOut)
+			{
+				// Store triangles flipped
+				for (const Vec3 *v = part.mTriangleVertices + mCurrentVertex, *v_end = v + part_num_vertices; v < v_end; v += 3)
+				{
+					(part.mLocalToWorld * v[0]).StoreFloat3(outTriangleVertices++);
+					(part.mLocalToWorld * v[2]).StoreFloat3(outTriangleVertices++);
+					(part.mLocalToWorld * v[1]).StoreFloat3(outTriangleVertices++);
+				}
+			}
+			else
+			{
+				// Store triangles
+				for (const Vec3 *v = part.mTriangleVertices + mCurrentVertex, *v_end = v + part_num_vertices; v < v_end; v += 3)
+				{
+					(part.mLocalToWorld * v[0]).StoreFloat3(outTriangleVertices++);
+					(part.mLocalToWorld * v[1]).StoreFloat3(outTriangleVertices++);
+					(part.mLocalToWorld * v[2]).StoreFloat3(outTriangleVertices++);
+				}
+			}
+
+			// Update the current vertex to point to the next vertex to get
+			mCurrentVertex += part_num_vertices;
+
+			// Check if we completed this part
+			if (mCurrentVertex < part.mNumTriangleVertices)
+				break;
+
+			// Reset current vertex for the next part
+			mCurrentVertex = 0;
+		}
+
+		int total_num_triangles = total_num_vertices / 3;
+
+		// Store materials
+		if (outMaterials != nullptr)
+			for (const PhysicsMaterial **m = outMaterials, **m_end = outMaterials + total_num_triangles; m < m_end; ++m)
+				*m = mMaterial;
+
+		return total_num_triangles;
+	}
+
+private:
+	struct Part
+	{
+		Mat44				mLocalToWorld;
+		const Vec3 *		mTriangleVertices;
+		size_t				mNumTriangleVertices;
+	};
+
+	StaticArray<Part, 3>	mParts;
+	uint					mCurrentPart = 0;
+	size_t					mCurrentVertex = 0;
+	const PhysicsMaterial *	mMaterial;
+	bool					mIsInsideOut;
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/HeightFieldShape.h

@@ -0,0 +1,380 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/Shape/Shape.h>
+#include <Jolt/Physics/Collision/PhysicsMaterial.h>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+class ConvexShape;
+class CollideShapeSettings;
+class TempAllocator;
+
+/// Constants for HeightFieldShape, this was moved out of the HeightFieldShape because of a linker bug
+namespace HeightFieldShapeConstants
+{
+	/// Value used to create gaps in the height field
+	constexpr float					cNoCollisionValue = FLT_MAX;
+
+	/// Stack size to use during WalkHeightField
+	constexpr int					cStackSize = 128;
+
+	/// A position in the hierarchical grid is defined by a level (which grid), x and y position. We encode this in a single uint32 as: level << 28 | y << 14 | x
+	constexpr uint					cNumBitsXY = 14;
+	constexpr uint					cMaskBitsXY = (1 << cNumBitsXY) - 1;
+	constexpr uint					cLevelShift = 2 * cNumBitsXY;
+
+	/// When height samples are converted to 16 bit:
+	constexpr uint16				cNoCollisionValue16 = 0xffff;				///< This is the magic value for 'no collision'
+	constexpr uint16				cMaxHeightValue16 = 0xfffe;					///< This is the maximum allowed height value
+};
+
+/// Class that constructs a HeightFieldShape
+class JPH_EXPORT HeightFieldShapeSettings final : public ShapeSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, HeightFieldShapeSettings)
+
+public:
+	/// Default constructor for deserialization
+									HeightFieldShapeSettings() = default;
+
+	/// Create a height field shape of inSampleCount * inSampleCount vertices.
+	/// The height field is a surface defined by: inOffset + inScale * (x, inSamples[y * inSampleCount + x], y).
+	/// where x and y are integers in the range x and y e [0, inSampleCount - 1].
+	/// inSampleCount: inSampleCount / mBlockSize must be minimally 2 and a power of 2 is the most efficient in terms of performance and storage.
+	/// inSamples: inSampleCount^2 vertices.
+	/// inMaterialIndices: (inSampleCount - 1)^2 indices that index into inMaterialList.
+									HeightFieldShapeSettings(const float *inSamples, Vec3Arg inOffset, Vec3Arg inScale, uint32 inSampleCount, const uint8 *inMaterialIndices = nullptr, const PhysicsMaterialList &inMaterialList = PhysicsMaterialList());
+
+	// See: ShapeSettings
+	virtual ShapeResult				Create() const override;
+
+	/// Determine the minimal and maximal value of mHeightSamples (will ignore cNoCollisionValue)
+	/// @param outMinValue The minimal value of mHeightSamples or FLT_MAX if no samples have collision
+	/// @param outMaxValue The maximal value of mHeightSamples or -FLT_MAX if no samples have collision
+	/// @param outQuantizationScale (value - outMinValue) * outQuantizationScale quantizes a height sample to 16 bits
+	void							DetermineMinAndMaxSample(float &outMinValue, float &outMaxValue, float &outQuantizationScale) const;
+
+	/// Given mBlockSize, mSampleCount and mHeightSamples, calculate the amount of bits needed to stay below absolute error inMaxError
+	/// @param inMaxError Maximum allowed error in mHeightSamples after compression (note that this does not take mScale.Y into account)
+	/// @return Needed bits per sample in the range [1, 8].
+	uint32							CalculateBitsPerSampleForError(float inMaxError) const;
+
+	/// The height field is a surface defined by: mOffset + mScale * (x, mHeightSamples[y * mSampleCount + x], y).
+	/// where x and y are integers in the range x and y e [0, mSampleCount - 1].
+	Vec3							mOffset = Vec3::sZero();
+	Vec3							mScale = Vec3::sOne();
+	uint32							mSampleCount = 0;
+
+	/// Artificial minimal value of mHeightSamples, used for compression and can be used to update the terrain after creating with lower height values. If there are any lower values in mHeightSamples, this value will be ignored.
+	float							mMinHeightValue = cLargeFloat;
+
+	/// Artificial maximum value of mHeightSamples, used for compression and can be used to update the terrain after creating with higher height values. If there are any higher values in mHeightSamples, this value will be ignored.
+	float							mMaxHeightValue = -cLargeFloat;
+
+	/// When bigger than mMaterials.size() the internal material list will be preallocated to support this number of materials.
+	/// This avoids reallocations when calling HeightFieldShape::SetMaterials with new materials later.
+	uint32							mMaterialsCapacity = 0;
+
+	/// The heightfield is divided in blocks of mBlockSize * mBlockSize * 2 triangles and the acceleration structure culls blocks only,
+	/// bigger block sizes reduce memory consumption but also reduce query performance. Sensible values are [2, 8], does not need to be
+	/// a power of 2. Note that at run-time we'll perform one more grid subdivision, so the effective block size is half of what is provided here.
+	uint32							mBlockSize = 2;
+
+	/// How many bits per sample to use to compress the height field. Can be in the range [1, 8].
+	/// Note that each sample is compressed relative to the min/max value of its block of mBlockSize * mBlockSize pixels so the effective precision is higher.
+	/// Also note that increasing mBlockSize saves more memory than reducing the amount of bits per sample.
+	uint32							mBitsPerSample = 8;
+
+	/// An array of mSampleCount^2 height samples. Samples are stored in row major order, so the sample at (x, y) is at index y * mSampleCount + x.
+	Array<float>					mHeightSamples;
+
+	/// An array of (mSampleCount - 1)^2 material indices.
+	Array<uint8>					mMaterialIndices;
+
+	/// The materials of square at (x, y) is: mMaterials[mMaterialIndices[x + y * (mSampleCount - 1)]]
+	PhysicsMaterialList				mMaterials;
+
+	/// Cosine of the threshold angle (if the angle between the two triangles is bigger than this, the edge is active, note that a concave edge is always inactive).
+	/// Setting this value too small can cause ghost collisions with edges, setting it too big can cause depenetration artifacts (objects not depenetrating quickly).
+	/// Valid ranges are between cos(0 degrees) and cos(90 degrees). The default value is cos(5 degrees).
+	float							mActiveEdgeCosThresholdAngle = 0.996195f;	// cos(5 degrees)
+};
+
+/// A height field shape. Cannot be used as a dynamic object.
+///
+/// Note: If you're using HeightFieldShape and are querying data while modifying the shape you'll have a race condition.
+/// In this case it is best to create a new HeightFieldShape using the Clone function. You replace the shape on a body using BodyInterface::SetShape.
+/// If a query is still working on the old shape, it will have taken a reference and keep the old shape alive until the query finishes.
+class JPH_EXPORT HeightFieldShape final : public Shape
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+									HeightFieldShape() : Shape(EShapeType::HeightField, EShapeSubType::HeightField) { }
+									HeightFieldShape(const HeightFieldShapeSettings &inSettings, ShapeResult &outResult);
+	virtual							~HeightFieldShape() override;
+
+	/// Clone this shape. Can be used to avoid race conditions. See the documentation of this class for more information.
+	Ref<HeightFieldShape>			Clone() const;
+
+	// See Shape::MustBeStatic
+	virtual bool					MustBeStatic() const override				{ return true; }
+
+	/// Get the size of the height field. Note that this will always be rounded up to the nearest multiple of GetBlockSize().
+	inline uint						GetSampleCount() const						{ return mSampleCount; }
+
+	/// Get the size of a block
+	inline uint						GetBlockSize() const						{ return mBlockSize; }
+
+	// See Shape::GetLocalBounds
+	virtual AABox					GetLocalBounds() const override;
+
+	// See Shape::GetSubShapeIDBitsRecursive
+	virtual uint					GetSubShapeIDBitsRecursive() const override	{ return GetSubShapeIDBits(); }
+
+	// See Shape::GetInnerRadius
+	virtual float					GetInnerRadius() const override				{ return 0.0f; }
+
+	// See Shape::GetMassProperties
+	virtual MassProperties			GetMassProperties() const override;
+
+	// See Shape::GetMaterial
+	virtual const PhysicsMaterial *	GetMaterial(const SubShapeID &inSubShapeID) const override;
+
+	/// Overload to get the material at a particular location
+	const PhysicsMaterial *			GetMaterial(uint inX, uint inY) const;
+
+	// See Shape::GetSurfaceNormal
+	virtual Vec3					GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override;
+
+	// See Shape::GetSupportingFace
+	virtual void					GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override;
+
+	// See Shape::GetSubmergedVolume
+	virtual void					GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const override { JPH_ASSERT(false, "Not supported"); }
+
+#ifdef JPH_DEBUG_RENDERER
+	// See Shape::Draw
+	virtual void					Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override;
+#endif // JPH_DEBUG_RENDERER
+
+	// See Shape::CastRay
+	virtual bool					CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override;
+	virtual void					CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
+
+	// See: Shape::CollidePoint
+	virtual void					CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
+
+	// See: Shape::CollideSoftBodyVertices
+	virtual void					CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override;
+
+	// See Shape::GetTrianglesStart
+	virtual void					GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override;
+
+	// See Shape::GetTrianglesNext
+	virtual int						GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override;
+
+	/// Get height field position at sampled location (inX, inY).
+	/// where inX and inY are integers in the range inX e [0, mSampleCount - 1] and inY e [0, mSampleCount - 1].
+	Vec3							GetPosition(uint inX, uint inY) const;
+
+	/// Check if height field at sampled location (inX, inY) has collision (has a hole or not)
+	bool							IsNoCollision(uint inX, uint inY) const;
+
+	/// Projects inLocalPosition (a point in the space of the shape) along the Y axis onto the surface and returns it in outSurfacePosition.
+	/// When there is no surface position (because of a hole or because the point is outside the heightfield) the function will return false.
+	bool							ProjectOntoSurface(Vec3Arg inLocalPosition, Vec3 &outSurfacePosition, SubShapeID &outSubShapeID) const;
+
+	/// Returns the coordinates of the triangle that a sub shape ID represents
+	/// @param inSubShapeID The sub shape ID to decode
+	/// @param outX X coordinate of the triangle (in the range [0, mSampleCount - 2])
+	/// @param outY Y coordinate of the triangle (in the range [0, mSampleCount - 2])
+	/// @param outTriangleIndex Triangle within the quad (0 = lower triangle or 1 = upper triangle)
+	void							GetSubShapeCoordinates(const SubShapeID &inSubShapeID, uint &outX, uint &outY, uint &outTriangleIndex) const;
+
+	/// Get the range of height values that this height field can encode. Can be used to determine the allowed range when setting the height values with SetHeights.
+	float							GetMinHeightValue() const					{ return mOffset.GetY(); }
+	float							GetMaxHeightValue() const					{ return mOffset.GetY() + mScale.GetY() * HeightFieldShapeConstants::cMaxHeightValue16; }
+
+	/// Get the height values of a block of data.
+	/// Note that the height values are decompressed so will be slightly different from what the shape was originally created with.
+	/// @param inX Start X position, must be a multiple of mBlockSize and in the range [0, mSampleCount - 1]
+	/// @param inY Start Y position, must be a multiple of mBlockSize and in the range [0, mSampleCount - 1]
+	/// @param inSizeX Number of samples in X direction, must be a multiple of mBlockSize and in the range [0, mSampleCount - inX]
+	/// @param inSizeY Number of samples in Y direction, must be a multiple of mBlockSize and in the range [0, mSampleCount - inY]
+	/// @param outHeights Returned height values, must be at least inSizeX * inSizeY floats. Values are returned in x-major order and can be cNoCollisionValue.
+	/// @param inHeightsStride Stride in floats between two consecutive rows of outHeights (can be negative if the data is upside down).
+	void							GetHeights(uint inX, uint inY, uint inSizeX, uint inSizeY, float *outHeights, intptr_t inHeightsStride) const;
+
+	/// Set the height values of a block of data.
+	/// Note that this requires decompressing and recompressing a border of size mBlockSize in the negative x/y direction so will cause some precision loss.
+	/// Beware this can create a race condition if you're running collision queries in parallel. See class documentation for more information.
+	/// @param inX Start X position, must be a multiple of mBlockSize and in the range [0, mSampleCount - 1]
+	/// @param inY Start Y position, must be a multiple of mBlockSize and in the range [0, mSampleCount - 1]
+	/// @param inSizeX Number of samples in X direction, must be a multiple of mBlockSize and in the range [0, mSampleCount - inX]
+	/// @param inSizeY Number of samples in Y direction, must be a multiple of mBlockSize and in the range [0, mSampleCount - inY]
+	/// @param inHeights The new height values to set, must be an array of inSizeX * inSizeY floats, can be cNoCollisionValue. Values outside of the range [GetMinHeightValue(), GetMaxHeightValue()] will be clamped.
+	/// @param inHeightsStride Stride in floats between two consecutive rows of inHeights (can be negative if the data is upside down).
+	/// @param inAllocator Allocator to use for temporary memory
+	/// @param inActiveEdgeCosThresholdAngle Cosine of the threshold angle (if the angle between the two triangles is bigger than this, the edge is active, note that a concave edge is always inactive).
+	void							SetHeights(uint inX, uint inY, uint inSizeX, uint inSizeY, const float *inHeights, intptr_t inHeightsStride, TempAllocator &inAllocator, float inActiveEdgeCosThresholdAngle = 0.996195f);
+
+	/// Get the current list of materials, the indices returned by GetMaterials() will index into this list.
+	const PhysicsMaterialList &		GetMaterialList() const						{ return mMaterials; }
+
+	/// Get the material indices of a block of data.
+	/// @param inX Start X position, must in the range [0, mSampleCount - 1]
+	/// @param inY Start Y position, must in the range [0, mSampleCount - 1]
+	/// @param inSizeX Number of samples in X direction
+	/// @param inSizeY Number of samples in Y direction
+	/// @param outMaterials Returned material indices, must be at least inSizeX * inSizeY uint8s. Values are returned in x-major order.
+	/// @param inMaterialsStride Stride in uint8s between two consecutive rows of outMaterials (can be negative if the data is upside down).
+	void							GetMaterials(uint inX, uint inY, uint inSizeX, uint inSizeY, uint8 *outMaterials, intptr_t inMaterialsStride) const;
+
+	/// Set the material indices of a block of data.
+	/// Beware this can create a race condition if you're running collision queries in parallel. See class documentation for more information.
+	/// @param inX Start X position, must in the range [0, mSampleCount - 1]
+	/// @param inY Start Y position, must in the range [0, mSampleCount - 1]
+	/// @param inSizeX Number of samples in X direction
+	/// @param inSizeY Number of samples in Y direction
+	/// @param inMaterials The new material indices, must be at least inSizeX * inSizeY uint8s. Values are returned in x-major order.
+	/// @param inMaterialsStride Stride in uint8s between two consecutive rows of inMaterials (can be negative if the data is upside down).
+	/// @param inMaterialList The material list to use for the new material indices or nullptr if the material list should not be updated
+	/// @param inAllocator Allocator to use for temporary memory
+	/// @return True if the material indices were set, false if the total number of materials exceeded 256
+	bool							SetMaterials(uint inX, uint inY, uint inSizeX, uint inSizeY, const uint8 *inMaterials, intptr_t inMaterialsStride, const PhysicsMaterialList *inMaterialList, TempAllocator &inAllocator);
+
+	// See Shape
+	virtual void					SaveBinaryState(StreamOut &inStream) const override;
+	virtual void					SaveMaterialState(PhysicsMaterialList &outMaterials) const override;
+	virtual void					RestoreMaterialState(const PhysicsMaterialRefC *inMaterials, uint inNumMaterials) override;
+
+	// See Shape::GetStats
+	virtual Stats					GetStats() const override;
+
+	// See Shape::GetVolume
+	virtual float					GetVolume() const override					{ return 0; }
+
+#ifdef JPH_DEBUG_RENDERER
+	// Settings
+	static bool						sDrawTriangleOutlines;
+#endif // JPH_DEBUG_RENDERER
+
+	// Register shape functions with the registry
+	static void						sRegister();
+
+protected:
+	// See: Shape::RestoreBinaryState
+	virtual void					RestoreBinaryState(StreamIn &inStream) override;
+
+private:
+	class							DecodingContext;							///< Context class for walking through all nodes of a heightfield
+	struct							HSGetTrianglesContext;						///< Context class for GetTrianglesStart/Next
+
+	/// Calculate commonly used values and store them in the shape
+	void							CacheValues();
+
+	/// Allocate the mRangeBlocks, mHeightSamples and mActiveEdges buffers as a single data block
+	void							AllocateBuffers();
+
+	/// Calculate bit mask for all active edges in the heightfield for a specific region
+	void							CalculateActiveEdges(uint inX, uint inY, uint inSizeX, uint inSizeY, const float *inHeights, uint inHeightsStartX, uint inHeightsStartY, intptr_t inHeightsStride, float inHeightsScale, float inActiveEdgeCosThresholdAngle, TempAllocator &inAllocator);
+
+	/// Calculate bit mask for all active edges in the heightfield
+	void							CalculateActiveEdges(const HeightFieldShapeSettings &inSettings);
+
+	/// Store material indices in the least amount of bits per index possible
+	void							StoreMaterialIndices(const HeightFieldShapeSettings &inSettings);
+
+	/// Get the amount of horizontal/vertical blocks
+	inline uint						GetNumBlocks() const						{ return mSampleCount / mBlockSize; }
+
+	/// Get the maximum level (amount of grids) of the tree
+	static inline uint				sGetMaxLevel(uint inNumBlocks)				{ return 32 - CountLeadingZeros(inNumBlocks - 1); }
+
+	/// Get the range block offset and stride for GetBlockOffsetAndScale
+	static inline void				sGetRangeBlockOffsetAndStride(uint inNumBlocks, uint inMaxLevel, uint &outRangeBlockOffset, uint &outRangeBlockStride);
+
+	/// For block (inBlockX, inBlockY) get the offset and scale needed to decode a uint8 height sample to a uint16
+	inline void						GetBlockOffsetAndScale(uint inBlockX, uint inBlockY, uint inRangeBlockOffset, uint inRangeBlockStride, float &outBlockOffset, float &outBlockScale) const;
+
+	/// Get the height sample at position (inX, inY)
+	inline uint8					GetHeightSample(uint inX, uint inY) const;
+
+	/// Faster version of GetPosition when block offset and scale are already known
+	inline Vec3						GetPosition(uint inX, uint inY, float inBlockOffset, float inBlockScale, bool &outNoCollision) const;
+
+	/// Determine amount of bits needed to encode sub shape id
+	uint							GetSubShapeIDBits() const;
+
+	/// En/decode a sub shape ID. inX and inY specify the coordinate of the triangle. inTriangle == 0 is the lower triangle, inTriangle == 1 is the upper triangle.
+	inline SubShapeID				EncodeSubShapeID(const SubShapeIDCreator &inCreator, uint inX, uint inY, uint inTriangle) const;
+	inline void						DecodeSubShapeID(const SubShapeID &inSubShapeID, uint &outX, uint &outY, uint &outTriangle) const;
+
+	/// Get the edge flags for a triangle
+	inline uint8					GetEdgeFlags(uint inX, uint inY, uint inTriangle) const;
+
+	// Helper functions called by CollisionDispatch
+	static void						sCollideConvexVsHeightField(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter);
+	static void						sCollideSphereVsHeightField(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter);
+	static void						sCastConvexVsHeightField(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector);
+	static void						sCastSphereVsHeightField(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector);
+
+	/// Visit the entire height field using a visitor pattern
+	/// Note: Used to be inlined but this triggers a bug in MSVC where it will not free the memory allocated by alloca which causes a stack overflow when WalkHeightField is called in a loop (clang does it correct)
+	template <class Visitor>
+	void							WalkHeightField(Visitor &ioVisitor) const;
+
+	/// A block of 2x2 ranges used to form a hierarchical grid, ordered left top, right top, left bottom, right bottom
+	struct alignas(16) RangeBlock
+	{
+		uint16						mMin[4];
+		uint16						mMax[4];
+	};
+
+	/// For block (inBlockX, inBlockY) get the range block and the entry in the range block
+	inline void						GetRangeBlock(uint inBlockX, uint inBlockY, uint inRangeBlockOffset, uint inRangeBlockStride, RangeBlock *&outBlock, uint &outIndexInBlock);
+
+	/// Offset of first RangedBlock in grid per level
+	static const uint				sGridOffsets[];
+
+	/// The height field is a surface defined by: mOffset + mScale * (x, mHeightSamples[y * mSampleCount + x], y).
+	/// where x and y are integers in the range x and y e [0, mSampleCount - 1].
+	Vec3							mOffset = Vec3::sZero();
+	Vec3							mScale = Vec3::sOne();
+
+	/// Height data
+	uint32							mSampleCount = 0;							///< See HeightFieldShapeSettings::mSampleCount
+	uint32							mBlockSize = 2;								///< See HeightFieldShapeSettings::mBlockSize
+	uint32							mHeightSamplesSize = 0;						///< Size of mHeightSamples in bytes
+	uint32							mRangeBlocksSize = 0;						///< Size of mRangeBlocks in elements
+	uint32							mActiveEdgesSize = 0;						///< Size of mActiveEdges in bytes
+	uint8							mBitsPerSample = 8;							///< See HeightFieldShapeSettings::mBitsPerSample
+	uint8							mSampleMask = 0xff;							///< All bits set for a sample: (1 << mBitsPerSample) - 1, used to indicate that there's no collision
+	uint16							mMinSample = HeightFieldShapeConstants::cNoCollisionValue16; ///< Min and max value in mHeightSamples quantized to 16 bit, for calculating bounding box
+	uint16							mMaxSample = HeightFieldShapeConstants::cNoCollisionValue16;
+	RangeBlock *					mRangeBlocks = nullptr;						///< Hierarchical grid of range data describing the height variations within 1 block. The grid for level <level> starts at offset sGridOffsets[<level>]
+	uint8 *							mHeightSamples = nullptr;					///< mBitsPerSample-bit height samples. Value [0, mMaxHeightValue] maps to highest detail grid in mRangeBlocks [mMin, mMax]. mNoCollisionValue is reserved to indicate no collision.
+	uint8 *							mActiveEdges = nullptr;						///< (mSampleCount - 1)^2 * 3-bit active edge flags.
+
+	/// Materials
+	PhysicsMaterialList				mMaterials;									///< The materials of square at (x, y) is: mMaterials[mMaterialIndices[x + y * (mSampleCount - 1)]]
+	Array<uint8>					mMaterialIndices;							///< Compressed to the minimum amount of bits per material index (mSampleCount - 1) * (mSampleCount - 1) * mNumBitsPerMaterialIndex bits of data
+	uint32							mNumBitsPerMaterialIndex = 0;				///< Number of bits per material index
+
+#ifdef JPH_DEBUG_RENDERER
+	/// Temporary rendering data
+	mutable Array<DebugRenderer::GeometryRef> mGeometry;
+	mutable bool					mCachedUseMaterialColors = false;			///< This is used to regenerate the triangle batch if the drawing settings change
+#endif // JPH_DEBUG_RENDERER
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/MeshShape.h

@@ -0,0 +1,228 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/Shape/Shape.h>
+#include <Jolt/Physics/Collision/PhysicsMaterial.h>
+#include <Jolt/Core/ByteBuffer.h>
+#include <Jolt/Geometry/Triangle.h>
+#include <Jolt/Geometry/IndexedTriangle.h>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+class ConvexShape;
+class CollideShapeSettings;
+
+/// Class that constructs a MeshShape
+class JPH_EXPORT MeshShapeSettings final : public ShapeSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, MeshShapeSettings)
+
+public:
+	/// Default constructor for deserialization
+									MeshShapeSettings() = default;
+
+	/// Create a mesh shape.
+									MeshShapeSettings(const TriangleList &inTriangles, PhysicsMaterialList inMaterials = PhysicsMaterialList());
+									MeshShapeSettings(VertexList inVertices, IndexedTriangleList inTriangles, PhysicsMaterialList inMaterials = PhysicsMaterialList());
+
+	/// Sanitize the mesh data. Remove duplicate and degenerate triangles. This is called automatically when constructing the MeshShapeSettings with a list of (indexed-) triangles.
+	void							Sanitize();
+
+	// See: ShapeSettings
+	virtual ShapeResult				Create() const override;
+
+	/// Vertices belonging to mIndexedTriangles
+	VertexList						mTriangleVertices;
+
+	/// Original list of indexed triangles (triangles will be reordered internally in the mesh shape).
+	/// Triangles must be provided in counter clockwise order.
+	/// Degenerate triangles will automatically be removed during mesh creation but no other mesh simplifications are performed, use an external library if this is desired.
+	/// For simulation, the triangles are considered to be single sided.
+	/// For ray casts you can choose to make triangles double sided by setting RayCastSettings::mBackFaceMode to EBackFaceMode::CollideWithBackFaces.
+	/// For collide shape tests you can use CollideShapeSettings::mBackFaceMode and for shape casts you can use ShapeCastSettings::mBackFaceModeTriangles.
+	IndexedTriangleList				mIndexedTriangles;
+
+	/// Materials assigned to the triangles. Each triangle specifies which material it uses through its mMaterialIndex
+	PhysicsMaterialList				mMaterials;
+
+	/// Maximum number of triangles in each leaf of the axis aligned box tree. This is a balance between memory and performance. Can be in the range [1, MeshShape::MaxTrianglesPerLeaf].
+	/// Sensible values are between 4 (for better performance) and 8 (for less memory usage).
+	uint							mMaxTrianglesPerLeaf = 8;
+
+	/// Cosine of the threshold angle (if the angle between the two triangles is bigger than this, the edge is active, note that a concave edge is always inactive).
+	/// Setting this value too small can cause ghost collisions with edges, setting it too big can cause depenetration artifacts (objects not depenetrating quickly).
+	/// Valid ranges are between cos(0 degrees) and cos(90 degrees). The default value is cos(5 degrees).
+	/// Negative values will make all edges active and causes EActiveEdgeMode::CollideOnlyWithActive to behave as EActiveEdgeMode::CollideWithAll.
+	/// This speeds up the build process but will require all bodies that can interact with the mesh to use BodyCreationSettings::mEnhancedInternalEdgeRemoval = true.
+	float							mActiveEdgeCosThresholdAngle = 0.996195f;					// cos(5 degrees)
+
+	/// When true, we store the user data coming from Triangle::mUserData or IndexedTriangle::mUserData in the mesh shape.
+	/// This can be used to store additional data like the original index of the triangle in the mesh.
+	/// Can be retrieved using MeshShape::GetTriangleUserData.
+	/// Turning this on increases the memory used by the MeshShape by roughly 25%.
+	bool							mPerTriangleUserData = false;
+
+	enum class EBuildQuality
+	{
+		FavorRuntimePerformance,																///< Favor runtime performance, takes more time to build the MeshShape but performs better
+		FavorBuildSpeed,																		///< Favor build speed, build the tree faster but the MeshShape will be slower
+	};
+
+	/// Determines the quality of the tree building process.
+	EBuildQuality					mBuildQuality = EBuildQuality::FavorRuntimePerformance;
+};
+
+/// A mesh shape, consisting of triangles. Mesh shapes are mostly used for static geometry.
+/// They can be used by dynamic or kinematic objects but only if they don't collide with other mesh or heightfield shapes as those collisions are currently not supported.
+/// Note that if you make a mesh shape a dynamic or kinematic object, you need to provide a mass yourself as mesh shapes don't need to form a closed hull so don't have a well defined volume from which the mass can be calculated.
+class JPH_EXPORT MeshShape final : public Shape
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+									MeshShape() : Shape(EShapeType::Mesh, EShapeSubType::Mesh) { }
+									MeshShape(const MeshShapeSettings &inSettings, ShapeResult &outResult);
+
+	// See Shape::MustBeStatic
+	virtual bool					MustBeStatic() const override								{ return true; }
+
+	// See Shape::GetLocalBounds
+	virtual AABox					GetLocalBounds() const override;
+
+	// See Shape::GetSubShapeIDBitsRecursive
+	virtual uint					GetSubShapeIDBitsRecursive() const override;
+
+	// See Shape::GetInnerRadius
+	virtual float					GetInnerRadius() const override								{ return 0.0f; }
+
+	// See Shape::GetMassProperties
+	virtual MassProperties			GetMassProperties() const override;
+
+	// See Shape::GetMaterial
+	virtual const PhysicsMaterial *	GetMaterial(const SubShapeID &inSubShapeID) const override;
+
+	/// Get the list of all materials
+	const PhysicsMaterialList &		GetMaterialList() const										{ return mMaterials; }
+
+	/// Determine which material index a particular sub shape uses (note that if there are no materials this function will return 0 so check the array size)
+	/// Note: This could for example be used to create a decorator shape around a mesh shape that overrides the GetMaterial call to replace a material with another material.
+	uint							GetMaterialIndex(const SubShapeID &inSubShapeID) const;
+
+	// See Shape::GetSurfaceNormal
+	virtual Vec3					GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override;
+
+	// See Shape::GetSupportingFace
+	virtual void					GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override;
+
+#ifdef JPH_DEBUG_RENDERER
+	// See Shape::Draw
+	virtual void					Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override;
+#endif // JPH_DEBUG_RENDERER
+
+	// See Shape::CastRay
+	virtual bool					CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override;
+	virtual void					CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
+
+	/// See: Shape::CollidePoint
+	/// Note that for CollidePoint to work for a mesh shape, the mesh needs to be closed (a manifold) or multiple non-intersecting manifolds. Triangles may be facing the interior of the manifold.
+	/// Insideness is tested by counting the amount of triangles encountered when casting an infinite ray from inPoint. If the number of hits is odd we're inside, if it's even we're outside.
+	virtual void					CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
+
+	// See: Shape::CollideSoftBodyVertices
+	virtual void					CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override;
+
+	// See Shape::GetTrianglesStart
+	virtual void					GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override;
+
+	// See Shape::GetTrianglesNext
+	virtual int						GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override;
+
+	// See Shape::GetSubmergedVolume
+	virtual void					GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const override { JPH_ASSERT(false, "Not supported"); }
+
+	// See Shape
+	virtual void					SaveBinaryState(StreamOut &inStream) const override;
+	virtual void					SaveMaterialState(PhysicsMaterialList &outMaterials) const override;
+	virtual void					RestoreMaterialState(const PhysicsMaterialRefC *inMaterials, uint inNumMaterials) override;
+
+	// See Shape::GetStats
+	virtual Stats					GetStats() const override;
+
+	// See Shape::GetVolume
+	virtual float					GetVolume() const override									{ return 0; }
+
+	// When MeshShape::mPerTriangleUserData is true, this function can be used to retrieve the user data that was stored in the mesh shape.
+	uint32							GetTriangleUserData(const SubShapeID &inSubShapeID) const;
+
+#ifdef JPH_DEBUG_RENDERER
+	// Settings
+	static bool						sDrawTriangleGroups;
+	static bool						sDrawTriangleOutlines;
+#endif // JPH_DEBUG_RENDERER
+
+	// Register shape functions with the registry
+	static void						sRegister();
+
+protected:
+	// See: Shape::RestoreBinaryState
+	virtual void					RestoreBinaryState(StreamIn &inStream) override;
+
+private:
+	struct							MSGetTrianglesContext;										///< Context class for GetTrianglesStart/Next
+
+	static constexpr int			NumTriangleBits = 3;										///< How many bits to reserve to encode the triangle index
+	static constexpr int			MaxTrianglesPerLeaf = 1 << NumTriangleBits;					///< Number of triangles that are stored max per leaf aabb node
+
+	/// Find and flag active edges
+	static void						sFindActiveEdges(const MeshShapeSettings &inSettings, IndexedTriangleList &ioIndices);
+
+	/// Visit the entire tree using a visitor pattern
+	template <class Visitor>
+	void							WalkTree(Visitor &ioVisitor) const;
+
+	/// Same as above but with a callback per triangle instead of per block of triangles
+	template <class Visitor>
+	void							WalkTreePerTriangle(const SubShapeIDCreator &inSubShapeIDCreator2, Visitor &ioVisitor) const;
+
+	/// Decode a sub shape ID
+	inline void						DecodeSubShapeID(const SubShapeID &inSubShapeID, const void *&outTriangleBlock, uint32 &outTriangleIndex) const;
+
+	// Helper functions called by CollisionDispatch
+	static void						sCollideConvexVsMesh(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter);
+	static void						sCollideSphereVsMesh(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter);
+	static void						sCastConvexVsMesh(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector);
+	static void						sCastSphereVsMesh(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector);
+
+	/// Materials assigned to the triangles. Each triangle specifies which material it uses through its mMaterialIndex
+	PhysicsMaterialList				mMaterials;
+
+	ByteBuffer						mTree;														///< Resulting packed data structure
+
+	/// 8 bit flags stored per triangle
+	enum ETriangleFlags
+	{
+		/// Material index
+		FLAGS_MATERIAL_BITS			= 5,
+		FLAGS_MATERIAL_MASK			= (1 << FLAGS_MATERIAL_BITS) - 1,
+
+		/// Active edge bits
+		FLAGS_ACTIVE_EGDE_SHIFT		= FLAGS_MATERIAL_BITS,
+		FLAGS_ACTIVE_EDGE_BITS		= 3,
+		FLAGS_ACTIVE_EDGE_MASK		= (1 << FLAGS_ACTIVE_EDGE_BITS) - 1
+	};
+
+#ifdef JPH_DEBUG_RENDERER
+	mutable DebugRenderer::GeometryRef	mGeometry;												///< Debug rendering data
+	mutable bool					mCachedTrianglesColoredPerGroup = false;					///< This is used to regenerate the triangle batch if the drawing settings change
+	mutable bool					mCachedUseMaterialColors = false;							///< This is used to regenerate the triangle batch if the drawing settings change
+#endif // JPH_DEBUG_RENDERER
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/MutableCompoundShape.cpp

@@ -0,0 +1,597 @@
+// 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/Shape/MutableCompoundShape.h>
+#include <Jolt/Physics/Collision/Shape/CompoundShapeVisitors.h>
+#include <Jolt/Core/Profiler.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(MutableCompoundShapeSettings)
+{
+	JPH_ADD_BASE_CLASS(MutableCompoundShapeSettings, CompoundShapeSettings)
+}
+
+ShapeSettings::ShapeResult MutableCompoundShapeSettings::Create() const
+{
+	// Build a mutable compound shape
+	if (mCachedResult.IsEmpty())
+		Ref<Shape> shape = new MutableCompoundShape(*this, mCachedResult);
+
+	return mCachedResult;
+}
+
+MutableCompoundShape::MutableCompoundShape(const MutableCompoundShapeSettings &inSettings, ShapeResult &outResult) :
+	CompoundShape(EShapeSubType::MutableCompound, inSettings, outResult)
+{
+	mSubShapes.reserve(inSettings.mSubShapes.size());
+	for (const CompoundShapeSettings::SubShapeSettings &shape : inSettings.mSubShapes)
+	{
+		// Start constructing the runtime sub shape
+		SubShape out_shape;
+		if (!out_shape.FromSettings(shape, outResult))
+			return;
+
+		mSubShapes.push_back(out_shape);
+	}
+
+	AdjustCenterOfMass();
+
+	CalculateSubShapeBounds(0, (uint)mSubShapes.size());
+
+	// Check if we're not exceeding the amount of sub shape id bits
+	if (GetSubShapeIDBitsRecursive() > SubShapeID::MaxBits)
+	{
+		outResult.SetError("Compound hierarchy is too deep and exceeds the amount of available sub shape ID bits");
+		return;
+	}
+
+	outResult.Set(this);
+}
+
+Ref<MutableCompoundShape> MutableCompoundShape::Clone() const
+{
+	Ref<MutableCompoundShape> clone = new MutableCompoundShape();
+	clone->SetUserData(GetUserData());
+
+	clone->mCenterOfMass = mCenterOfMass;
+	clone->mLocalBounds = mLocalBounds;
+	clone->mSubShapes = mSubShapes;
+	clone->mInnerRadius = mInnerRadius;
+	clone->mSubShapeBounds = mSubShapeBounds;
+
+	return clone;
+}
+
+void MutableCompoundShape::AdjustCenterOfMass()
+{
+	// First calculate the delta of the center of mass
+	float mass = 0.0f;
+	Vec3 center_of_mass = Vec3::sZero();
+	for (const CompoundShape::SubShape &sub_shape : mSubShapes)
+	{
+		MassProperties child = sub_shape.mShape->GetMassProperties();
+		mass += child.mMass;
+		center_of_mass += sub_shape.GetPositionCOM() * child.mMass;
+	}
+	if (mass > 0.0f)
+		center_of_mass /= mass;
+
+	// Now adjust all shapes to recenter around center of mass
+	for (CompoundShape::SubShape &sub_shape : mSubShapes)
+		sub_shape.SetPositionCOM(sub_shape.GetPositionCOM() - center_of_mass);
+
+	// Update bounding boxes
+	for (Bounds &bounds : mSubShapeBounds)
+	{
+		Vec4 xxxx = center_of_mass.SplatX();
+		Vec4 yyyy = center_of_mass.SplatY();
+		Vec4 zzzz = center_of_mass.SplatZ();
+		bounds.mMinX -= xxxx;
+		bounds.mMinY -= yyyy;
+		bounds.mMinZ -= zzzz;
+		bounds.mMaxX -= xxxx;
+		bounds.mMaxY -= yyyy;
+		bounds.mMaxZ -= zzzz;
+	}
+	mLocalBounds.Translate(-center_of_mass);
+
+	// And adjust the center of mass for this shape in the opposite direction
+	mCenterOfMass += center_of_mass;
+}
+
+void MutableCompoundShape::CalculateLocalBounds()
+{
+	uint num_blocks = GetNumBlocks();
+	if (num_blocks > 0)
+	{
+		// Initialize min/max for first block
+		const Bounds *bounds = mSubShapeBounds.data();
+		Vec4 min_x = bounds->mMinX;
+		Vec4 min_y = bounds->mMinY;
+		Vec4 min_z = bounds->mMinZ;
+		Vec4 max_x = bounds->mMaxX;
+		Vec4 max_y = bounds->mMaxY;
+		Vec4 max_z = bounds->mMaxZ;
+
+		// Accumulate other blocks
+		const Bounds *bounds_end = bounds + num_blocks;
+		for (++bounds; bounds < bounds_end; ++bounds)
+		{
+			min_x = Vec4::sMin(min_x, bounds->mMinX);
+			min_y = Vec4::sMin(min_y, bounds->mMinY);
+			min_z = Vec4::sMin(min_z, bounds->mMinZ);
+			max_x = Vec4::sMax(max_x, bounds->mMaxX);
+			max_y = Vec4::sMax(max_y, bounds->mMaxY);
+			max_z = Vec4::sMax(max_z, bounds->mMaxZ);
+		}
+
+		// Calculate resulting bounding box
+		mLocalBounds.mMin.SetX(min_x.ReduceMin());
+		mLocalBounds.mMin.SetY(min_y.ReduceMin());
+		mLocalBounds.mMin.SetZ(min_z.ReduceMin());
+		mLocalBounds.mMax.SetX(max_x.ReduceMax());
+		mLocalBounds.mMax.SetY(max_y.ReduceMax());
+		mLocalBounds.mMax.SetZ(max_z.ReduceMax());
+	}
+	else
+	{
+		// There are no subshapes, make the bounding box empty
+		mLocalBounds.mMin = mLocalBounds.mMax = Vec3::sZero();
+	}
+
+	// Cache the inner radius as it can take a while to recursively iterate over all sub shapes
+	CalculateInnerRadius();
+}
+
+void MutableCompoundShape::EnsureSubShapeBoundsCapacity()
+{
+	// Check if we have enough space
+	uint new_capacity = ((uint)mSubShapes.size() + 3) >> 2;
+	if (mSubShapeBounds.size() < new_capacity)
+		mSubShapeBounds.resize(new_capacity);
+}
+
+void MutableCompoundShape::CalculateSubShapeBounds(uint inStartIdx, uint inNumber)
+{
+	// Ensure that we have allocated the required space for mSubShapeBounds
+	EnsureSubShapeBoundsCapacity();
+
+	// Loop over blocks of 4 sub shapes
+	for (uint sub_shape_idx_start = inStartIdx & ~uint(3), sub_shape_idx_end = inStartIdx + inNumber; sub_shape_idx_start < sub_shape_idx_end; sub_shape_idx_start += 4)
+	{
+		Mat44 bounds_min;
+		Mat44 bounds_max;
+
+		AABox sub_shape_bounds;
+		for (uint col = 0; col < 4; ++col)
+		{
+			uint sub_shape_idx = sub_shape_idx_start + col;
+			if (sub_shape_idx < mSubShapes.size()) // else reuse sub_shape_bounds from previous iteration
+			{
+				const SubShape &sub_shape = mSubShapes[sub_shape_idx];
+
+				// Transform the shape's bounds into our local space
+				Mat44 transform = Mat44::sRotationTranslation(sub_shape.GetRotation(), sub_shape.GetPositionCOM());
+
+				// Get the bounding box
+				sub_shape_bounds = sub_shape.mShape->GetWorldSpaceBounds(transform, Vec3::sOne());
+			}
+
+			// Put the bounds as columns in a matrix
+			bounds_min.SetColumn3(col, sub_shape_bounds.mMin);
+			bounds_max.SetColumn3(col, sub_shape_bounds.mMax);
+		}
+
+		// Transpose to go to structure of arrays format
+		Mat44 bounds_min_t = bounds_min.Transposed();
+		Mat44 bounds_max_t = bounds_max.Transposed();
+
+		// Store in our bounds array
+		Bounds &bounds = mSubShapeBounds[sub_shape_idx_start >> 2];
+		bounds.mMinX = bounds_min_t.GetColumn4(0);
+		bounds.mMinY = bounds_min_t.GetColumn4(1);
+		bounds.mMinZ = bounds_min_t.GetColumn4(2);
+		bounds.mMaxX = bounds_max_t.GetColumn4(0);
+		bounds.mMaxY = bounds_max_t.GetColumn4(1);
+		bounds.mMaxZ = bounds_max_t.GetColumn4(2);
+	}
+
+	CalculateLocalBounds();
+}
+
+uint MutableCompoundShape::AddShape(Vec3Arg inPosition, QuatArg inRotation, const Shape *inShape, uint32 inUserData, uint inIndex)
+{
+	SubShape sub_shape;
+	sub_shape.mShape = inShape;
+	sub_shape.mUserData = inUserData;
+	sub_shape.SetTransform(inPosition, inRotation, mCenterOfMass);
+
+	if (inIndex >= mSubShapes.size())
+	{
+		uint shape_idx = uint(mSubShapes.size());
+		mSubShapes.push_back(sub_shape);
+		CalculateSubShapeBounds(shape_idx, 1);
+		return shape_idx;
+	}
+	else
+	{
+		mSubShapes.insert(mSubShapes.begin() + inIndex, sub_shape);
+		CalculateSubShapeBounds(inIndex, uint(mSubShapes.size()) - inIndex);
+		return inIndex;
+	}
+}
+
+void MutableCompoundShape::RemoveShape(uint inIndex)
+{
+	mSubShapes.erase(mSubShapes.begin() + inIndex);
+
+	// We always need to recalculate the bounds of the sub shapes as we test blocks
+	// of 4 sub shapes at a time and removed shapes get their bounds updated
+	// to repeat the bounds of the previous sub shape
+	uint num_bounds = (uint)mSubShapes.size() - inIndex;
+	CalculateSubShapeBounds(inIndex, num_bounds);
+}
+
+void MutableCompoundShape::ModifyShape(uint inIndex, Vec3Arg inPosition, QuatArg inRotation)
+{
+	SubShape &sub_shape = mSubShapes[inIndex];
+	sub_shape.SetTransform(inPosition, inRotation, mCenterOfMass);
+
+	CalculateSubShapeBounds(inIndex, 1);
+}
+
+void MutableCompoundShape::ModifyShape(uint inIndex, Vec3Arg inPosition, QuatArg inRotation, const Shape *inShape)
+{
+	SubShape &sub_shape = mSubShapes[inIndex];
+	sub_shape.mShape = inShape;
+	sub_shape.SetTransform(inPosition, inRotation, mCenterOfMass);
+
+	CalculateSubShapeBounds(inIndex, 1);
+}
+
+void MutableCompoundShape::ModifyShapes(uint inStartIndex, uint inNumber, const Vec3 *inPositions, const Quat *inRotations, uint inPositionStride, uint inRotationStride)
+{
+	JPH_ASSERT(inStartIndex + inNumber <= mSubShapes.size());
+
+	const Vec3 *pos = inPositions;
+	const Quat *rot = inRotations;
+	for (SubShape *dest = &mSubShapes[inStartIndex], *dest_end = dest + inNumber; dest < dest_end; ++dest)
+	{
+		// Update transform
+		dest->SetTransform(*pos, *rot, mCenterOfMass);
+
+		// Advance pointer in position / rotation buffer
+		pos = reinterpret_cast<const Vec3 *>(reinterpret_cast<const uint8 *>(pos) + inPositionStride);
+		rot = reinterpret_cast<const Quat *>(reinterpret_cast<const uint8 *>(rot) + inRotationStride);
+	}
+
+	CalculateSubShapeBounds(inStartIndex, inNumber);
+}
+
+template <class Visitor>
+inline void MutableCompoundShape::WalkSubShapes(Visitor &ioVisitor) const
+{
+	// Loop over all blocks of 4 bounding boxes
+	for (uint block = 0, num_blocks = GetNumBlocks(); block < num_blocks; ++block)
+	{
+		// Test the bounding boxes
+		const Bounds &bounds = mSubShapeBounds[block];
+		typename Visitor::Result result = ioVisitor.TestBlock(bounds.mMinX, bounds.mMinY, bounds.mMinZ, bounds.mMaxX, bounds.mMaxY, bounds.mMaxZ);
+
+		// Check if any of the bounding boxes collided
+		if (ioVisitor.ShouldVisitBlock(result))
+		{
+			// Go through the individual boxes
+			uint sub_shape_start_idx = block << 2;
+			for (uint col = 0, max_col = min<uint>(4, (uint)mSubShapes.size() - sub_shape_start_idx); col < max_col; ++col) // Don't read beyond the end of the subshapes array
+				if (ioVisitor.ShouldVisitSubShape(result, col)) // Because the early out fraction can change, we need to retest every shape
+				{
+					// Test sub shape
+					uint sub_shape_idx = sub_shape_start_idx + col;
+					const SubShape &sub_shape = mSubShapes[sub_shape_idx];
+					ioVisitor.VisitShape(sub_shape, sub_shape_idx);
+
+					// If no better collision is available abort
+					if (ioVisitor.ShouldAbort())
+						break;
+				}
+		}
+	}
+}
+
+bool MutableCompoundShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	struct Visitor : public CastRayVisitor
+	{
+		using CastRayVisitor::CastRayVisitor;
+
+		using Result = Vec4;
+
+		JPH_INLINE Result	TestBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const
+		{
+			return TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
+		}
+
+		JPH_INLINE bool		ShouldVisitBlock(Vec4Arg inResult) const
+		{
+			UVec4 closer = Vec4::sLess(inResult, Vec4::sReplicate(mHit.mFraction));
+			return closer.TestAnyTrue();
+		}
+
+		JPH_INLINE bool		ShouldVisitSubShape(Vec4Arg inResult, uint inIndexInBlock) const
+		{
+			return inResult[inIndexInBlock] < mHit.mFraction;
+		}
+	};
+
+	Visitor visitor(inRay, this, inSubShapeIDCreator, ioHit);
+	WalkSubShapes(visitor);
+	return visitor.mReturnValue;
+}
+
+void MutableCompoundShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Test shape filter
+	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
+		return;
+
+	struct Visitor : public CastRayVisitorCollector
+	{
+		using CastRayVisitorCollector::CastRayVisitorCollector;
+
+		using Result = Vec4;
+
+		JPH_INLINE Result	TestBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const
+		{
+			return TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
+		}
+
+		JPH_INLINE bool		ShouldVisitBlock(Vec4Arg inResult) const
+		{
+			UVec4 closer = Vec4::sLess(inResult, Vec4::sReplicate(mCollector.GetEarlyOutFraction()));
+			return closer.TestAnyTrue();
+		}
+
+		JPH_INLINE bool		ShouldVisitSubShape(Vec4Arg inResult, uint inIndexInBlock) const
+		{
+			return inResult[inIndexInBlock] < mCollector.GetEarlyOutFraction();
+		}
+	};
+
+	Visitor visitor(inRay, inRayCastSettings, this, inSubShapeIDCreator, ioCollector, inShapeFilter);
+	WalkSubShapes(visitor);
+}
+
+void MutableCompoundShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Test shape filter
+	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
+		return;
+
+	struct Visitor : public CollidePointVisitor
+	{
+		using CollidePointVisitor::CollidePointVisitor;
+
+		using Result = UVec4;
+
+		JPH_INLINE Result	TestBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const
+		{
+			return TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
+		}
+
+		JPH_INLINE bool		ShouldVisitBlock(UVec4Arg inResult) const
+		{
+			return inResult.TestAnyTrue();
+		}
+
+		JPH_INLINE bool		ShouldVisitSubShape(UVec4Arg inResult, uint inIndexInBlock) const
+		{
+			return inResult[inIndexInBlock] != 0;
+		}
+	};
+
+	Visitor visitor(inPoint, this, inSubShapeIDCreator, ioCollector, inShapeFilter);
+	WalkSubShapes(visitor);
+}
+
+void MutableCompoundShape::sCastShapeVsCompound(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
+{
+	JPH_PROFILE_FUNCTION();
+
+	struct Visitor : public CastShapeVisitor
+	{
+		using CastShapeVisitor::CastShapeVisitor;
+
+		using Result = Vec4;
+
+		JPH_INLINE Result	TestBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const
+		{
+			return TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
+		}
+
+		JPH_INLINE bool		ShouldVisitBlock(Vec4Arg inResult) const
+		{
+			UVec4 closer = Vec4::sLess(inResult, Vec4::sReplicate(mCollector.GetPositiveEarlyOutFraction()));
+			return closer.TestAnyTrue();
+		}
+
+		JPH_INLINE bool		ShouldVisitSubShape(Vec4Arg inResult, uint inIndexInBlock) const
+		{
+			return inResult[inIndexInBlock] < mCollector.GetPositiveEarlyOutFraction();
+		}
+	};
+
+	JPH_ASSERT(inShape->GetSubType() == EShapeSubType::MutableCompound);
+	const MutableCompoundShape *shape = static_cast<const MutableCompoundShape *>(inShape);
+
+	Visitor visitor(inShapeCast, inShapeCastSettings, shape, inScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector);
+	shape->WalkSubShapes(visitor);
+}
+
+void MutableCompoundShape::CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Test shape filter
+	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
+		return;
+
+	struct Visitor : public CollectTransformedShapesVisitor
+	{
+		using CollectTransformedShapesVisitor::CollectTransformedShapesVisitor;
+
+		using Result = UVec4;
+
+		JPH_INLINE Result	TestBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const
+		{
+			return TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
+		}
+
+		JPH_INLINE bool		ShouldVisitBlock(UVec4Arg inResult) const
+		{
+			return inResult.TestAnyTrue();
+		}
+
+		JPH_INLINE bool		ShouldVisitSubShape(UVec4Arg inResult, uint inIndexInBlock) const
+		{
+			return inResult[inIndexInBlock] != 0;
+		}
+	};
+
+	Visitor visitor(inBox, this, inPositionCOM, inRotation, inScale, inSubShapeIDCreator, ioCollector, inShapeFilter);
+	WalkSubShapes(visitor);
+}
+
+int MutableCompoundShape::GetIntersectingSubShapes(const AABox &inBox, uint *outSubShapeIndices, int inMaxSubShapeIndices) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	GetIntersectingSubShapesVisitorMC<AABox> visitor(inBox, outSubShapeIndices, inMaxSubShapeIndices);
+	WalkSubShapes(visitor);
+	return visitor.GetNumResults();
+}
+
+int MutableCompoundShape::GetIntersectingSubShapes(const OrientedBox &inBox, uint *outSubShapeIndices, int inMaxSubShapeIndices) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	GetIntersectingSubShapesVisitorMC<OrientedBox> visitor(inBox, outSubShapeIndices, inMaxSubShapeIndices);
+	WalkSubShapes(visitor);
+	return visitor.GetNumResults();
+}
+
+void MutableCompoundShape::sCollideCompoundVsShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter)
+{
+	JPH_PROFILE_FUNCTION();
+
+	JPH_ASSERT(inShape1->GetSubType() == EShapeSubType::MutableCompound);
+	const MutableCompoundShape *shape1 = static_cast<const MutableCompoundShape *>(inShape1);
+
+	struct Visitor : public CollideCompoundVsShapeVisitor
+	{
+		using CollideCompoundVsShapeVisitor::CollideCompoundVsShapeVisitor;
+
+		using Result = UVec4;
+
+		JPH_INLINE Result	TestBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const
+		{
+			return TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
+		}
+
+		JPH_INLINE bool		ShouldVisitBlock(UVec4Arg inResult) const
+		{
+			return inResult.TestAnyTrue();
+		}
+
+		JPH_INLINE bool		ShouldVisitSubShape(UVec4Arg inResult, uint inIndexInBlock) const
+		{
+			return inResult[inIndexInBlock] != 0;
+		}
+	};
+
+	Visitor visitor(shape1, inShape2, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter);
+	shape1->WalkSubShapes(visitor);
+}
+
+void MutableCompoundShape::sCollideShapeVsCompound(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter)
+{
+	JPH_PROFILE_FUNCTION();
+
+	JPH_ASSERT(inShape2->GetSubType() == EShapeSubType::MutableCompound);
+	const MutableCompoundShape *shape2 = static_cast<const MutableCompoundShape *>(inShape2);
+
+	struct Visitor : public CollideShapeVsCompoundVisitor
+	{
+		using CollideShapeVsCompoundVisitor::CollideShapeVsCompoundVisitor;
+
+		using Result = UVec4;
+
+		JPH_INLINE Result	TestBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const
+		{
+			return TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
+		}
+
+		JPH_INLINE bool		ShouldVisitBlock(UVec4Arg inResult) const
+		{
+			return inResult.TestAnyTrue();
+		}
+
+		JPH_INLINE bool		ShouldVisitSubShape(UVec4Arg inResult, uint inIndexInBlock) const
+		{
+			return inResult[inIndexInBlock] != 0;
+		}
+	};
+
+	Visitor visitor(inShape1, shape2, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter);
+	shape2->WalkSubShapes(visitor);
+}
+
+void MutableCompoundShape::SaveBinaryState(StreamOut &inStream) const
+{
+	CompoundShape::SaveBinaryState(inStream);
+
+	// Write bounds
+	uint bounds_size = (((uint)mSubShapes.size() + 3) >> 2) * sizeof(Bounds);
+	inStream.WriteBytes(mSubShapeBounds.data(), bounds_size);
+}
+
+void MutableCompoundShape::RestoreBinaryState(StreamIn &inStream)
+{
+	CompoundShape::RestoreBinaryState(inStream);
+
+	// Ensure that we have allocated the required space for mSubShapeBounds
+	EnsureSubShapeBoundsCapacity();
+
+	// Read bounds
+	uint bounds_size = (((uint)mSubShapes.size() + 3) >> 2) * sizeof(Bounds);
+	inStream.ReadBytes(mSubShapeBounds.data(), bounds_size);
+}
+
+void MutableCompoundShape::sRegister()
+{
+	ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::MutableCompound);
+	f.mConstruct = []() -> Shape * { return new MutableCompoundShape; };
+	f.mColor = Color::sDarkOrange;
+
+	for (EShapeSubType s : sAllSubShapeTypes)
+	{
+		CollisionDispatch::sRegisterCollideShape(EShapeSubType::MutableCompound, s, sCollideCompoundVsShape);
+		CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::MutableCompound, sCollideShapeVsCompound);
+		CollisionDispatch::sRegisterCastShape(s, EShapeSubType::MutableCompound, sCastShapeVsCompound);
+	}
+}
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/MutableCompoundShape.h

@@ -0,0 +1,176 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/Shape/CompoundShape.h>
+
+JPH_NAMESPACE_BEGIN
+
+class CollideShapeSettings;
+
+/// Class that constructs a MutableCompoundShape.
+class JPH_EXPORT MutableCompoundShapeSettings final : public CompoundShapeSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, MutableCompoundShapeSettings)
+
+public:
+	// See: ShapeSettings
+	virtual ShapeResult				Create() const override;
+};
+
+/// A compound shape, sub shapes can be rotated and translated.
+/// This shape is optimized for adding / removing and changing the rotation / translation of sub shapes but is less efficient in querying.
+/// Shifts all child objects so that they're centered around the center of mass (which needs to be kept up to date by calling AdjustCenterOfMass).
+///
+/// Note: If you're using MutableCompoundShape and are querying data while modifying the shape you'll have a race condition.
+/// In this case it is best to create a new MutableCompoundShape using the Clone function. You replace the shape on a body using BodyInterface::SetShape.
+/// If a query is still working on the old shape, it will have taken a reference and keep the old shape alive until the query finishes.
+///
+/// When you modify a MutableCompoundShape, beware that the SubShapeIDs of all other shapes can change. So be careful when storing SubShapeIDs.
+class JPH_EXPORT MutableCompoundShape final : public CompoundShape
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+									MutableCompoundShape() : CompoundShape(EShapeSubType::MutableCompound) { }
+									MutableCompoundShape(const MutableCompoundShapeSettings &inSettings, ShapeResult &outResult);
+
+	/// Clone this shape. Can be used to avoid race conditions. See the documentation of this class for more information.
+	Ref<MutableCompoundShape>		Clone() const;
+
+	// See Shape::CastRay
+	virtual bool					CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override;
+	virtual void					CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
+
+	// See: Shape::CollidePoint
+	virtual void					CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
+
+	// See Shape::CollectTransformedShapes
+	virtual void					CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const override;
+
+	// See: CompoundShape::GetIntersectingSubShapes
+	virtual int						GetIntersectingSubShapes(const AABox &inBox, uint *outSubShapeIndices, int inMaxSubShapeIndices) const override;
+
+	// See: CompoundShape::GetIntersectingSubShapes
+	virtual int						GetIntersectingSubShapes(const OrientedBox &inBox, uint *outSubShapeIndices, int inMaxSubShapeIndices) const override;
+
+	// See Shape
+	virtual void					SaveBinaryState(StreamOut &inStream) const override;
+
+	// See Shape::GetStats
+	virtual Stats					GetStats() const override								{ return Stats(sizeof(*this) + mSubShapes.size() * sizeof(SubShape) + mSubShapeBounds.size() * sizeof(Bounds), 0); }
+
+	///@{
+	/// @name Mutating shapes. Note that this is not thread safe, so you need to ensure that any bodies that use this shape are locked at the time of modification using BodyLockWrite. After modification you need to call BodyInterface::NotifyShapeChanged to update the broadphase and collision caches.
+
+	/// Adding a new shape.
+	/// Beware this can create a race condition if you're running collision queries in parallel. See class documentation for more information.
+	/// @param inPosition The position of the new shape
+	/// @param inRotation The orientation of the new shape
+	/// @param inShape The shape to add
+	/// @param inUserData User data that will be stored with the shape and can be retrieved using GetCompoundUserData
+	/// @param inIndex Index where to insert the shape, UINT_MAX to add to the end
+	/// @return The index of the newly added shape
+	uint							AddShape(Vec3Arg inPosition, QuatArg inRotation, const Shape *inShape, uint32 inUserData = 0, uint inIndex = UINT_MAX);
+
+	/// Remove a shape by index.
+	/// Beware this can create a race condition if you're running collision queries in parallel. See class documentation for more information.
+	void							RemoveShape(uint inIndex);
+
+	/// Modify the position / orientation of a shape.
+	/// Beware this can create a race condition if you're running collision queries in parallel. See class documentation for more information.
+	void							ModifyShape(uint inIndex, Vec3Arg inPosition, QuatArg inRotation);
+
+	/// Modify the position / orientation and shape at the same time.
+	/// Beware this can create a race condition if you're running collision queries in parallel. See class documentation for more information.
+	void							ModifyShape(uint inIndex, Vec3Arg inPosition, QuatArg inRotation, const Shape *inShape);
+
+	/// @brief Batch set positions / orientations, this avoids duplicate work due to bounding box calculation.
+	/// Beware this can create a race condition if you're running collision queries in parallel. See class documentation for more information.
+	/// @param inStartIndex Index of first shape to update
+	/// @param inNumber Number of shapes to update
+	/// @param inPositions A list of positions with arbitrary stride
+	/// @param inRotations A list of orientations with arbitrary stride
+	/// @param inPositionStride The position stride (the number of bytes between the first and second element)
+	/// @param inRotationStride The orientation stride (the number of bytes between the first and second element)
+	void							ModifyShapes(uint inStartIndex, uint inNumber, const Vec3 *inPositions, const Quat *inRotations, uint inPositionStride = sizeof(Vec3), uint inRotationStride = sizeof(Quat));
+
+	/// Recalculate the center of mass and shift all objects so they're centered around it
+	/// (this needs to be done of dynamic bodies and if the center of mass changes significantly due to adding / removing / repositioning sub shapes or else the simulation will look unnatural)
+	/// Note that after adjusting the center of mass of an object you need to call BodyInterface::NotifyShapeChanged and Constraint::NotifyShapeChanged on the relevant bodies / constraints.
+	/// Beware this can create a race condition if you're running collision queries in parallel. See class documentation for more information.
+	void							AdjustCenterOfMass();
+
+	///@}
+
+	// Register shape functions with the registry
+	static void						sRegister();
+
+protected:
+	// See: Shape::RestoreBinaryState
+	virtual void					RestoreBinaryState(StreamIn &inStream) override;
+
+private:
+	// Visitor for GetIntersectingSubShapes
+	template <class BoxType>
+	struct GetIntersectingSubShapesVisitorMC : public GetIntersectingSubShapesVisitor<BoxType>
+	{
+		using GetIntersectingSubShapesVisitor<BoxType>::GetIntersectingSubShapesVisitor;
+
+		using Result = UVec4;
+
+		JPH_INLINE Result			TestBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const
+		{
+			return GetIntersectingSubShapesVisitor<BoxType>::TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
+		}
+
+		JPH_INLINE bool				ShouldVisitBlock(UVec4Arg inResult) const
+		{
+			return inResult.TestAnyTrue();
+		}
+
+		JPH_INLINE bool				ShouldVisitSubShape(UVec4Arg inResult, uint inIndexInBlock) const
+		{
+			return inResult[inIndexInBlock] != 0;
+		}
+	};
+
+	/// Get the number of blocks of 4 bounding boxes
+	inline uint						GetNumBlocks() const										{ return ((uint)mSubShapes.size() + 3) >> 2; }
+
+	/// Ensure that the mSubShapeBounds has enough space to store bounding boxes equivalent to the number of shapes in mSubShapes
+	void							EnsureSubShapeBoundsCapacity();
+
+	/// Update mSubShapeBounds
+	/// @param inStartIdx First sub shape to update
+	/// @param inNumber Number of shapes to update
+	void							CalculateSubShapeBounds(uint inStartIdx, uint inNumber);
+
+	/// Calculate mLocalBounds from mSubShapeBounds
+	void							CalculateLocalBounds();
+
+	template <class Visitor>
+	JPH_INLINE void					WalkSubShapes(Visitor &ioVisitor) const;					///< Walk the sub shapes and call Visitor::VisitShape for each sub shape encountered
+
+	// Helper functions called by CollisionDispatch
+	static void						sCollideCompoundVsShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter);
+	static void						sCollideShapeVsCompound(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter);
+	static void						sCastShapeVsCompound(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector);
+
+	struct Bounds
+	{
+		Vec4						mMinX;
+		Vec4						mMinY;
+		Vec4						mMinZ;
+		Vec4						mMaxX;
+		Vec4						mMaxY;
+		Vec4						mMaxZ;
+	};
+
+	Array<Bounds>					mSubShapeBounds;											///< Bounding boxes of all sub shapes in SOA format (in blocks of 4 boxes), MinX 0..3, MinY 0..3, MinZ 0..3, MaxX 0..3, MaxY 0..3, MaxZ 0..3, MinX 4..7, MinY 4..7, ...
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/OffsetCenterOfMassShape.cpp

@@ -0,0 +1,217 @@
+// 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/Shape/OffsetCenterOfMassShape.h>
+#include <Jolt/Physics/Collision/CollisionDispatch.h>
+#include <Jolt/Physics/Collision/RayCast.h>
+#include <Jolt/Physics/Collision/ShapeCast.h>
+#include <Jolt/Physics/Collision/TransformedShape.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(OffsetCenterOfMassShapeSettings)
+{
+	JPH_ADD_BASE_CLASS(OffsetCenterOfMassShapeSettings, DecoratedShapeSettings)
+
+	JPH_ADD_ATTRIBUTE(OffsetCenterOfMassShapeSettings, mOffset)
+}
+
+ShapeSettings::ShapeResult OffsetCenterOfMassShapeSettings::Create() const
+{
+	if (mCachedResult.IsEmpty())
+		Ref<Shape> shape = new OffsetCenterOfMassShape(*this, mCachedResult);
+	return mCachedResult;
+}
+
+OffsetCenterOfMassShape::OffsetCenterOfMassShape(const OffsetCenterOfMassShapeSettings &inSettings, ShapeResult &outResult) :
+	DecoratedShape(EShapeSubType::OffsetCenterOfMass, inSettings, outResult),
+	mOffset(inSettings.mOffset)
+{
+	if (outResult.HasError())
+		return;
+
+	outResult.Set(this);
+}
+
+AABox OffsetCenterOfMassShape::GetLocalBounds() const
+{
+	AABox bounds = mInnerShape->GetLocalBounds();
+	bounds.mMin -= mOffset;
+	bounds.mMax -= mOffset;
+	return bounds;
+}
+
+AABox OffsetCenterOfMassShape::GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const
+{
+	return mInnerShape->GetWorldSpaceBounds(inCenterOfMassTransform.PreTranslated(-inScale * mOffset), inScale);
+}
+
+TransformedShape OffsetCenterOfMassShape::GetSubShapeTransformedShape(const SubShapeID &inSubShapeID, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, SubShapeID &outRemainder) const
+{
+	// We don't use any bits in the sub shape ID
+	outRemainder = inSubShapeID;
+
+	TransformedShape ts(RVec3(inPositionCOM - inRotation * (inScale * mOffset)), inRotation, mInnerShape, BodyID());
+	ts.SetShapeScale(inScale);
+	return ts;
+}
+
+Vec3 OffsetCenterOfMassShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const
+{
+	// Transform surface position to local space and pass call on
+	return mInnerShape->GetSurfaceNormal(inSubShapeID, inLocalSurfacePosition + mOffset);
+}
+
+void OffsetCenterOfMassShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const
+{
+	mInnerShape->GetSupportingFace(inSubShapeID, inDirection, inScale, inCenterOfMassTransform.PreTranslated(-inScale * mOffset), outVertices);
+}
+
+void OffsetCenterOfMassShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const
+{
+	mInnerShape->GetSubmergedVolume(inCenterOfMassTransform.PreTranslated(-inScale * mOffset), inScale, inSurface, outTotalVolume, outSubmergedVolume, outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, inBaseOffset));
+}
+
+#ifdef JPH_DEBUG_RENDERER
+void OffsetCenterOfMassShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
+{
+	mInnerShape->Draw(inRenderer, inCenterOfMassTransform.PreTranslated(-inScale * mOffset), inScale, inColor, inUseMaterialColors, inDrawWireframe);
+}
+
+void OffsetCenterOfMassShape::DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const
+{
+	mInnerShape->DrawGetSupportFunction(inRenderer, inCenterOfMassTransform.PreTranslated(-inScale * mOffset), inScale, inColor, inDrawSupportDirection);
+}
+
+void OffsetCenterOfMassShape::DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const
+{
+	mInnerShape->DrawGetSupportingFace(inRenderer, inCenterOfMassTransform.PreTranslated(-inScale * mOffset), inScale);
+}
+#endif // JPH_DEBUG_RENDERER
+
+bool OffsetCenterOfMassShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const
+{
+	// Transform the ray to local space
+	RayCast ray = inRay;
+	ray.mOrigin += mOffset;
+
+	return mInnerShape->CastRay(ray, inSubShapeIDCreator, ioHit);
+}
+
+void OffsetCenterOfMassShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	// Test shape filter
+	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
+		return;
+
+	// Transform the ray to local space
+	RayCast ray = inRay;
+	ray.mOrigin += mOffset;
+
+	return mInnerShape->CastRay(ray, inRayCastSettings, inSubShapeIDCreator, ioCollector, inShapeFilter);
+}
+
+void OffsetCenterOfMassShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	// Test shape filter
+	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
+		return;
+
+	// Pass the point on to the inner shape in local space
+	mInnerShape->CollidePoint(inPoint + mOffset, inSubShapeIDCreator, ioCollector, inShapeFilter);
+}
+
+void OffsetCenterOfMassShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const
+{
+	mInnerShape->CollideSoftBodyVertices(inCenterOfMassTransform.PreTranslated(-inScale * mOffset), inScale, inVertices, inNumVertices, inCollidingShapeIndex);
+}
+
+void OffsetCenterOfMassShape::CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	// Test shape filter
+	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
+		return;
+
+	mInnerShape->CollectTransformedShapes(inBox, inPositionCOM - inRotation * (inScale * mOffset), inRotation, inScale, inSubShapeIDCreator, ioCollector, inShapeFilter);
+}
+
+void OffsetCenterOfMassShape::TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const
+{
+	mInnerShape->TransformShape(inCenterOfMassTransform.PreTranslated(-mOffset), ioCollector);
+}
+
+void OffsetCenterOfMassShape::sCollideOffsetCenterOfMassVsShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter)
+{
+	JPH_ASSERT(inShape1->GetSubType() == EShapeSubType::OffsetCenterOfMass);
+	const OffsetCenterOfMassShape *shape1 = static_cast<const OffsetCenterOfMassShape *>(inShape1);
+
+	CollisionDispatch::sCollideShapeVsShape(shape1->mInnerShape, inShape2, inScale1, inScale2, inCenterOfMassTransform1.PreTranslated(-inScale1 * shape1->mOffset), inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter);
+}
+
+void OffsetCenterOfMassShape::sCollideShapeVsOffsetCenterOfMass(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter)
+{
+	JPH_ASSERT(inShape2->GetSubType() == EShapeSubType::OffsetCenterOfMass);
+	const OffsetCenterOfMassShape *shape2 = static_cast<const OffsetCenterOfMassShape *>(inShape2);
+
+	CollisionDispatch::sCollideShapeVsShape(inShape1, shape2->mInnerShape, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2.PreTranslated(-inScale2 * shape2->mOffset), inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter);
+}
+
+void OffsetCenterOfMassShape::sCastOffsetCenterOfMassVsShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
+{
+	// Fetch offset center of mass shape from cast shape
+	JPH_ASSERT(inShapeCast.mShape->GetSubType() == EShapeSubType::OffsetCenterOfMass);
+	const OffsetCenterOfMassShape *shape1 = static_cast<const OffsetCenterOfMassShape *>(inShapeCast.mShape);
+
+	// Transform the shape cast and update the shape
+	ShapeCast shape_cast(shape1->mInnerShape, inShapeCast.mScale, inShapeCast.mCenterOfMassStart.PreTranslated(-inShapeCast.mScale * shape1->mOffset), inShapeCast.mDirection);
+
+	CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, inShapeCastSettings, inShape, inScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector);
+}
+
+void OffsetCenterOfMassShape::sCastShapeVsOffsetCenterOfMass(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
+{
+	JPH_ASSERT(inShape->GetSubType() == EShapeSubType::OffsetCenterOfMass);
+	const OffsetCenterOfMassShape *shape = static_cast<const OffsetCenterOfMassShape *>(inShape);
+
+	// Transform the shape cast
+	ShapeCast shape_cast = inShapeCast.PostTransformed(Mat44::sTranslation(inScale * shape->mOffset));
+
+	CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, inShapeCastSettings, shape->mInnerShape, inScale, inShapeFilter, inCenterOfMassTransform2.PreTranslated(-inScale * shape->mOffset), inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector);
+}
+
+void OffsetCenterOfMassShape::SaveBinaryState(StreamOut &inStream) const
+{
+	DecoratedShape::SaveBinaryState(inStream);
+
+	inStream.Write(mOffset);
+}
+
+void OffsetCenterOfMassShape::RestoreBinaryState(StreamIn &inStream)
+{
+	DecoratedShape::RestoreBinaryState(inStream);
+
+	inStream.Read(mOffset);
+}
+
+void OffsetCenterOfMassShape::sRegister()
+{
+	ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::OffsetCenterOfMass);
+	f.mConstruct = []() -> Shape * { return new OffsetCenterOfMassShape; };
+	f.mColor = Color::sCyan;
+
+	for (EShapeSubType s : sAllSubShapeTypes)
+	{
+		CollisionDispatch::sRegisterCollideShape(EShapeSubType::OffsetCenterOfMass, s, sCollideOffsetCenterOfMassVsShape);
+		CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::OffsetCenterOfMass, sCollideShapeVsOffsetCenterOfMass);
+		CollisionDispatch::sRegisterCastShape(EShapeSubType::OffsetCenterOfMass, s, sCastOffsetCenterOfMassVsShape);
+		CollisionDispatch::sRegisterCastShape(s, EShapeSubType::OffsetCenterOfMass, sCastShapeVsOffsetCenterOfMass);
+	}
+}
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/OffsetCenterOfMassShape.h

@@ -0,0 +1,140 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/Shape/DecoratedShape.h>
+
+JPH_NAMESPACE_BEGIN
+
+class CollideShapeSettings;
+
+/// Class that constructs an OffsetCenterOfMassShape
+class JPH_EXPORT OffsetCenterOfMassShapeSettings final : public DecoratedShapeSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, OffsetCenterOfMassShapeSettings)
+
+public:
+	/// Constructor
+									OffsetCenterOfMassShapeSettings() = default;
+
+	/// Construct with shape settings, can be serialized.
+									OffsetCenterOfMassShapeSettings(Vec3Arg inOffset, const ShapeSettings *inShape) : DecoratedShapeSettings(inShape), mOffset(inOffset) { }
+
+	/// Variant that uses a concrete shape, which means this object cannot be serialized.
+									OffsetCenterOfMassShapeSettings(Vec3Arg inOffset, const Shape *inShape): DecoratedShapeSettings(inShape), mOffset(inOffset) { }
+
+	// See: ShapeSettings
+	virtual ShapeResult				Create() const override;
+
+	Vec3							mOffset;												///< Offset to be applied to the center of mass of the child shape
+};
+
+/// This shape will shift the center of mass of a child shape, it can e.g. be used to lower the center of mass of an unstable object like a boat to make it stable
+class JPH_EXPORT OffsetCenterOfMassShape final : public DecoratedShape
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+									OffsetCenterOfMassShape() : DecoratedShape(EShapeSubType::OffsetCenterOfMass) { }
+									OffsetCenterOfMassShape(const OffsetCenterOfMassShapeSettings &inSettings, ShapeResult &outResult);
+									OffsetCenterOfMassShape(const Shape *inShape, Vec3Arg inOffset) : DecoratedShape(EShapeSubType::OffsetCenterOfMass, inShape), mOffset(inOffset) { }
+
+	/// Access the offset that is applied to the center of mass
+	Vec3							GetOffset() const										{ return mOffset; }
+
+	// See Shape::GetCenterOfMass
+	virtual Vec3					GetCenterOfMass() const override						{ return mInnerShape->GetCenterOfMass() + mOffset; }
+
+	// See Shape::GetLocalBounds
+	virtual AABox					GetLocalBounds() const override;
+
+	// See Shape::GetWorldSpaceBounds
+	virtual AABox					GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override;
+	using Shape::GetWorldSpaceBounds;
+
+	// See Shape::GetInnerRadius
+	virtual float					GetInnerRadius() const override							{ return mInnerShape->GetInnerRadius(); }
+
+	// See Shape::GetMassProperties
+	virtual MassProperties			GetMassProperties() const override
+	{
+		MassProperties mp = mInnerShape->GetMassProperties();
+		mp.Translate(mOffset);
+		return mp;
+	}
+
+	// See Shape::GetSubShapeTransformedShape
+	virtual TransformedShape		GetSubShapeTransformedShape(const SubShapeID &inSubShapeID, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, SubShapeID &outRemainder) const override;
+
+	// See Shape::GetSurfaceNormal
+	virtual Vec3					GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override;
+
+	// See Shape::GetSupportingFace
+	virtual void					GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override;
+
+	// See Shape::GetSubmergedVolume
+	virtual void					GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const override;
+
+#ifdef JPH_DEBUG_RENDERER
+	// See Shape::Draw
+	virtual void					Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override;
+
+	// See Shape::DrawGetSupportFunction
+	virtual void					DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const override;
+
+	// See Shape::DrawGetSupportingFace
+	virtual void					DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override;
+#endif // JPH_DEBUG_RENDERER
+
+	// See Shape::CastRay
+	virtual bool					CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override;
+	virtual void					CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
+
+	// See: Shape::CollidePoint
+	virtual void					CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
+
+	// See: Shape::CollideSoftBodyVertices
+	virtual void					CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override;
+
+	// See Shape::CollectTransformedShapes
+	virtual void					CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const override;
+
+	// See Shape::TransformShape
+	virtual void					TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const override;
+
+	// See Shape::GetTrianglesStart
+	virtual void					GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override { JPH_ASSERT(false, "Cannot call on non-leaf shapes, use CollectTransformedShapes to collect the leaves first!"); }
+
+	// See Shape::GetTrianglesNext
+	virtual int						GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override { JPH_ASSERT(false, "Cannot call on non-leaf shapes, use CollectTransformedShapes to collect the leaves first!"); return 0; }
+
+	// See Shape
+	virtual void					SaveBinaryState(StreamOut &inStream) const override;
+
+	// See Shape::GetStats
+	virtual Stats					GetStats() const override								{ return Stats(sizeof(*this), 0); }
+
+	// See Shape::GetVolume
+	virtual float					GetVolume() const override								{ return mInnerShape->GetVolume(); }
+
+	// Register shape functions with the registry
+	static void						sRegister();
+
+protected:
+	// See: Shape::RestoreBinaryState
+	virtual void					RestoreBinaryState(StreamIn &inStream) override;
+
+private:
+	// Helper functions called by CollisionDispatch
+	static void						sCollideOffsetCenterOfMassVsShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter);
+	static void						sCollideShapeVsOffsetCenterOfMass(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter);
+	static void						sCastOffsetCenterOfMassVsShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector);
+	static void						sCastShapeVsOffsetCenterOfMass(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector);
+
+	Vec3							mOffset;												///< Offset of the center of mass
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/PlaneShape.cpp

@@ -0,0 +1,541 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2024 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Collision/Shape/PlaneShape.h>
+#include <Jolt/Physics/Collision/Shape/ConvexShape.h>
+#include <Jolt/Physics/Collision/Shape/ScaleHelpers.h>
+#include <Jolt/Physics/Collision/RayCast.h>
+#include <Jolt/Physics/Collision/ShapeCast.h>
+#include <Jolt/Physics/Collision/ShapeFilter.h>
+#include <Jolt/Physics/Collision/CastResult.h>
+#include <Jolt/Physics/Collision/CollisionDispatch.h>
+#include <Jolt/Physics/Collision/TransformedShape.h>
+#include <Jolt/Physics/Collision/CollidePointResult.h>
+#include <Jolt/Physics/Collision/CollideSoftBodyVertexIterator.h>
+#include <Jolt/Core/Profiler.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+#include <Jolt/Geometry/Plane.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(PlaneShapeSettings)
+{
+	JPH_ADD_BASE_CLASS(PlaneShapeSettings, ShapeSettings)
+
+	JPH_ADD_ATTRIBUTE(PlaneShapeSettings, mPlane)
+	JPH_ADD_ATTRIBUTE(PlaneShapeSettings, mMaterial)
+	JPH_ADD_ATTRIBUTE(PlaneShapeSettings, mHalfExtent)
+}
+
+ShapeSettings::ShapeResult PlaneShapeSettings::Create() const
+{
+	if (mCachedResult.IsEmpty())
+		Ref<Shape> shape = new PlaneShape(*this, mCachedResult);
+	return mCachedResult;
+}
+
+inline static void sPlaneGetOrthogonalBasis(Vec3Arg inNormal, Vec3 &outPerp1, Vec3 &outPerp2)
+{
+	outPerp1 = inNormal.Cross(Vec3::sAxisY()).NormalizedOr(Vec3::sAxisX());
+	outPerp2 = outPerp1.Cross(inNormal).Normalized();
+	outPerp1 = inNormal.Cross(outPerp2);
+}
+
+void PlaneShape::GetVertices(Vec3 *outVertices) const
+{
+	// Create orthogonal basis
+	Vec3 normal = mPlane.GetNormal();
+	Vec3 perp1, perp2;
+	sPlaneGetOrthogonalBasis(normal, perp1, perp2);
+
+	// Scale basis
+	perp1 *= mHalfExtent;
+	perp2 *= mHalfExtent;
+
+	// Calculate corners
+	Vec3 point = -normal * mPlane.GetConstant();
+	outVertices[0] = point + perp1 + perp2;
+	outVertices[1] = point + perp1 - perp2;
+	outVertices[2] = point - perp1 - perp2;
+	outVertices[3] = point - perp1 + perp2;
+}
+
+void PlaneShape::CalculateLocalBounds()
+{
+	// Get the vertices of the plane
+	Vec3 vertices[4];
+	GetVertices(vertices);
+
+	// Encapsulate the vertices and a point mHalfExtent behind the plane
+	mLocalBounds = AABox();
+	Vec3 normal = mPlane.GetNormal();
+	for (const Vec3 &v : vertices)
+	{
+		mLocalBounds.Encapsulate(v);
+		mLocalBounds.Encapsulate(v - mHalfExtent * normal);
+	}
+}
+
+PlaneShape::PlaneShape(const PlaneShapeSettings &inSettings, ShapeResult &outResult) :
+	Shape(EShapeType::Plane, EShapeSubType::Plane, inSettings, outResult),
+	mPlane(inSettings.mPlane),
+	mMaterial(inSettings.mMaterial),
+	mHalfExtent(inSettings.mHalfExtent)
+{
+	if (!mPlane.GetNormal().IsNormalized())
+	{
+		outResult.SetError("Plane normal needs to be normalized!");
+		return;
+	}
+
+	CalculateLocalBounds();
+
+	outResult.Set(this);
+}
+
+MassProperties PlaneShape::GetMassProperties() const
+{
+	// Object should always be static, return default mass properties
+	return MassProperties();
+}
+
+void PlaneShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const
+{
+	// Get the vertices of the plane
+	Vec3 vertices[4];
+	GetVertices(vertices);
+
+	// Reverse if scale is inside out
+	if (ScaleHelpers::IsInsideOut(inScale))
+	{
+		std::swap(vertices[0], vertices[3]);
+		std::swap(vertices[1], vertices[2]);
+	}
+
+	// Transform them to world space
+	outVertices.clear();
+	Mat44 com = inCenterOfMassTransform.PreScaled(inScale);
+	for (const Vec3 &v : vertices)
+		outVertices.push_back(com * v);
+}
+
+#ifdef JPH_DEBUG_RENDERER
+void PlaneShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
+{
+	// Get the vertices of the plane
+	Vec3 local_vertices[4];
+	GetVertices(local_vertices);
+
+	// Reverse if scale is inside out
+	if (ScaleHelpers::IsInsideOut(inScale))
+	{
+		std::swap(local_vertices[0], local_vertices[3]);
+		std::swap(local_vertices[1], local_vertices[2]);
+	}
+
+	// Transform them to world space
+	RMat44 com = inCenterOfMassTransform.PreScaled(inScale);
+	RVec3 vertices[4];
+	for (uint i = 0; i < 4; ++i)
+		vertices[i] = com * local_vertices[i];
+
+	// Determine the color
+	Color color = inUseMaterialColors? GetMaterial(SubShapeID())->GetDebugColor() : inColor;
+
+	// Draw the plane
+	if (inDrawWireframe)
+	{
+		inRenderer->DrawWireTriangle(vertices[0], vertices[1], vertices[2], color);
+		inRenderer->DrawWireTriangle(vertices[0], vertices[2], vertices[3], color);
+	}
+	else
+	{
+		inRenderer->DrawTriangle(vertices[0], vertices[1], vertices[2], color, DebugRenderer::ECastShadow::On);
+		inRenderer->DrawTriangle(vertices[0], vertices[2], vertices[3], color, DebugRenderer::ECastShadow::On);
+	}
+}
+#endif // JPH_DEBUG_RENDERER
+
+bool PlaneShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Test starting inside of negative half space
+	float distance = mPlane.SignedDistance(inRay.mOrigin);
+	if (distance <= 0.0f)
+	{
+		ioHit.mFraction = 0.0f;
+		ioHit.mSubShapeID2 = inSubShapeIDCreator.GetID();
+		return true;
+	}
+
+	// Test ray parallel to plane
+	float dot = inRay.mDirection.Dot(mPlane.GetNormal());
+	if (dot == 0.0f)
+		return false;
+
+	// Calculate hit fraction
+	float fraction = -distance / dot;
+	if (fraction >= 0.0f && fraction < ioHit.mFraction)
+	{
+		ioHit.mFraction = fraction;
+		ioHit.mSubShapeID2 = inSubShapeIDCreator.GetID();
+		return true;
+	}
+
+	return false;
+}
+
+void PlaneShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Test shape filter
+	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
+		return;
+
+	// Inside solid half space?
+	float distance = mPlane.SignedDistance(inRay.mOrigin);
+	if (inRayCastSettings.mTreatConvexAsSolid
+		&& distance <= 0.0f // Inside plane
+		&& ioCollector.GetEarlyOutFraction() > 0.0f) // Willing to accept hits at fraction 0
+	{
+		// Hit at fraction 0
+		RayCastResult hit;
+		hit.mBodyID = TransformedShape::sGetBodyID(ioCollector.GetContext());
+		hit.mFraction = 0.0f;
+		hit.mSubShapeID2 = inSubShapeIDCreator.GetID();
+		ioCollector.AddHit(hit);
+	}
+
+	float dot = inRay.mDirection.Dot(mPlane.GetNormal());
+	if (dot != 0.0f // Parallel ray will not hit plane
+		&& (inRayCastSettings.mBackFaceModeConvex == EBackFaceMode::CollideWithBackFaces || dot < 0.0f)) // Back face culling
+	{
+		// Calculate hit with plane
+		float fraction = -distance / dot;
+		if (fraction >= 0.0f && fraction < ioCollector.GetEarlyOutFraction())
+		{
+			RayCastResult hit;
+			hit.mBodyID = TransformedShape::sGetBodyID(ioCollector.GetContext());
+			hit.mFraction = fraction;
+			hit.mSubShapeID2 = inSubShapeIDCreator.GetID();
+			ioCollector.AddHit(hit);
+		}
+	}
+}
+
+void PlaneShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Test shape filter
+	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
+		return;
+
+	// Check if the point is inside the plane
+	if (mPlane.SignedDistance(inPoint) < 0.0f)
+		ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator.GetID() });
+}
+
+void PlaneShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Convert plane to world space
+	Plane plane = mPlane.Scaled(inScale).GetTransformed(inCenterOfMassTransform);
+
+	for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v)
+		if (v.GetInvMass() > 0.0f)
+		{
+			// Calculate penetration
+			float penetration = -plane.SignedDistance(v.GetPosition());
+			if (v.UpdatePenetration(penetration))
+				v.SetCollision(plane, inCollidingShapeIndex);
+		}
+}
+
+// This is a version of GetSupportingFace that returns a face that is large enough to cover the shape we're colliding with but not as large as the regular GetSupportedFace to avoid numerical precision issues
+inline static void sGetSupportingFace(const ConvexShape *inShape, Vec3Arg inShapeCOM, const Plane &inPlane, Mat44Arg inPlaneToWorld, ConvexShape::SupportingFace &outPlaneFace)
+{
+	// Project COM of shape onto plane
+	Plane world_plane = inPlane.GetTransformed(inPlaneToWorld);
+	Vec3 center = world_plane.ProjectPointOnPlane(inShapeCOM);
+
+	// Create orthogonal basis for the plane
+	Vec3 normal = world_plane.GetNormal();
+	Vec3 perp1, perp2;
+	sPlaneGetOrthogonalBasis(normal, perp1, perp2);
+
+	// Base the size of the face on the bounding box of the shape, ensuring that it is large enough to cover the entire shape
+	float size = inShape->GetLocalBounds().GetSize().Length();
+	perp1 *= size;
+	perp2 *= size;
+
+	// Emit the vertices
+	outPlaneFace.resize(4);
+	outPlaneFace[0] = center + perp1 + perp2;
+	outPlaneFace[1] = center + perp1 - perp2;
+	outPlaneFace[2] = center - perp1 - perp2;
+	outPlaneFace[3] = center - perp1 + perp2;
+}
+
+void PlaneShape::sCastConvexVsPlane(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, [[maybe_unused]] const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Get the shapes
+	JPH_ASSERT(inShapeCast.mShape->GetType() == EShapeType::Convex);
+	JPH_ASSERT(inShape->GetType() == EShapeType::Plane);
+	const ConvexShape *convex_shape = static_cast<const ConvexShape *>(inShapeCast.mShape);
+	const PlaneShape *plane_shape = static_cast<const PlaneShape *>(inShape);
+
+	// Shape cast is provided relative to COM of inShape, so all we need to do is transform our plane with inScale
+	Plane plane = plane_shape->mPlane.Scaled(inScale);
+	Vec3 normal = plane.GetNormal();
+
+	// Get support function
+	ConvexShape::SupportBuffer shape1_support_buffer;
+	const ConvexShape::Support *shape1_support = convex_shape->GetSupportFunction(ConvexShape::ESupportMode::Default, shape1_support_buffer, inShapeCast.mScale);
+
+	// Get the support point of the convex shape in the opposite direction of the plane normal in our local space
+	Vec3 normal_in_convex_shape_space = inShapeCast.mCenterOfMassStart.Multiply3x3Transposed(normal);
+	Vec3 support_point = inShapeCast.mCenterOfMassStart * shape1_support->GetSupport(-normal_in_convex_shape_space);
+	float signed_distance = plane.SignedDistance(support_point);
+	float convex_radius = shape1_support->GetConvexRadius();
+	float penetration_depth = -signed_distance + convex_radius;
+	float dot = inShapeCast.mDirection.Dot(normal);
+
+	// Collision output
+	Mat44 com_hit;
+	Vec3 point1, point2;
+	float fraction;
+
+	// Do we start in collision?
+	if (penetration_depth > 0.0f)
+	{
+		// Back face culling?
+		if (inShapeCastSettings.mBackFaceModeConvex == EBackFaceMode::IgnoreBackFaces && dot > 0.0f)
+			return;
+
+		// Shallower hit?
+		if (penetration_depth <= -ioCollector.GetEarlyOutFraction())
+			return;
+
+		// We're hitting at fraction 0
+		fraction = 0.0f;
+
+		// Get contact point
+		com_hit = inCenterOfMassTransform2;
+		point1 = inCenterOfMassTransform2 * (support_point - normal * convex_radius);
+		point2 = inCenterOfMassTransform2 * (support_point - normal * signed_distance);
+	}
+	else if (dot < 0.0f) // Moving towards the plane?
+	{
+		// Calculate hit fraction
+		fraction = penetration_depth / dot;
+		JPH_ASSERT(fraction >= 0.0f);
+
+		// Further than early out fraction?
+		if (fraction >= ioCollector.GetEarlyOutFraction())
+			return;
+
+		// Get contact point
+		com_hit = inCenterOfMassTransform2.PostTranslated(fraction * inShapeCast.mDirection);
+		point1 = point2 = com_hit * (support_point - normal * convex_radius);
+	}
+	else
+	{
+		// Moving away from the plane
+		return;
+	}
+
+	// Create cast result
+	Vec3 penetration_axis_world = com_hit.Multiply3x3(-normal);
+	bool back_facing = dot > 0.0f;
+	ShapeCastResult result(fraction, point1, point2, penetration_axis_world, back_facing, inSubShapeIDCreator1.GetID(), inSubShapeIDCreator2.GetID(), TransformedShape::sGetBodyID(ioCollector.GetContext()));
+
+	// Gather faces
+	if (inShapeCastSettings.mCollectFacesMode == ECollectFacesMode::CollectFaces)
+	{
+		// Get supporting face of convex shape
+		Mat44 shape_to_world = com_hit * inShapeCast.mCenterOfMassStart;
+		convex_shape->GetSupportingFace(SubShapeID(), normal_in_convex_shape_space, inShapeCast.mScale, shape_to_world, result.mShape1Face);
+
+		// Get supporting face of plane
+		if (!result.mShape1Face.empty())
+			sGetSupportingFace(convex_shape, shape_to_world.GetTranslation(), plane, inCenterOfMassTransform2, result.mShape2Face);
+	}
+
+	// Notify the collector
+	JPH_IF_TRACK_NARROWPHASE_STATS(TrackNarrowPhaseCollector track;)
+	ioCollector.AddHit(result);
+}
+
+struct PlaneShape::PSGetTrianglesContext
+{
+	Float3	mVertices[4];
+	bool	mDone = false;
+};
+
+void PlaneShape::GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const
+{
+	static_assert(sizeof(PSGetTrianglesContext) <= sizeof(GetTrianglesContext), "GetTrianglesContext too small");
+	JPH_ASSERT(IsAligned(&ioContext, alignof(PSGetTrianglesContext)));
+
+	PSGetTrianglesContext *context = new (&ioContext) PSGetTrianglesContext();
+
+	// Get the vertices of the plane
+	Vec3 vertices[4];
+	GetVertices(vertices);
+
+	// Reverse if scale is inside out
+	if (ScaleHelpers::IsInsideOut(inScale))
+	{
+		std::swap(vertices[0], vertices[3]);
+		std::swap(vertices[1], vertices[2]);
+	}
+
+	// Transform them to world space
+	Mat44 com = Mat44::sRotationTranslation(inRotation, inPositionCOM).PreScaled(inScale);
+	for (uint i = 0; i < 4; ++i)
+		(com * vertices[i]).StoreFloat3(&context->mVertices[i]);
+}
+
+int PlaneShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) const
+{
+	static_assert(cGetTrianglesMinTrianglesRequested >= 2, "cGetTrianglesMinTrianglesRequested is too small");
+	JPH_ASSERT(inMaxTrianglesRequested >= cGetTrianglesMinTrianglesRequested);
+
+	// Check if we're done
+	PSGetTrianglesContext &context = (PSGetTrianglesContext &)ioContext;
+	if (context.mDone)
+		return 0;
+	context.mDone = true;
+
+	// 1st triangle
+	outTriangleVertices[0] = context.mVertices[0];
+	outTriangleVertices[1] = context.mVertices[1];
+	outTriangleVertices[2] = context.mVertices[2];
+
+	// 2nd triangle
+	outTriangleVertices[3] = context.mVertices[0];
+	outTriangleVertices[4] = context.mVertices[2];
+	outTriangleVertices[5] = context.mVertices[3];
+
+	if (outMaterials != nullptr)
+	{
+		// Get material
+		const PhysicsMaterial *material = GetMaterial(SubShapeID());
+		outMaterials[0] = material;
+		outMaterials[1] = material;
+	}
+
+	return 2;
+}
+
+void PlaneShape::sCollideConvexVsPlane(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, [[maybe_unused]] const ShapeFilter &inShapeFilter)
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Get the shapes
+	JPH_ASSERT(inShape1->GetType() == EShapeType::Convex);
+	JPH_ASSERT(inShape2->GetType() == EShapeType::Plane);
+	const ConvexShape *shape1 = static_cast<const ConvexShape *>(inShape1);
+	const PlaneShape *shape2 = static_cast<const PlaneShape *>(inShape2);
+
+	// Transform the plane to the space of the convex shape
+	Plane scaled_plane = shape2->mPlane.Scaled(inScale2);
+	Plane plane = scaled_plane.GetTransformed(inCenterOfMassTransform1.InversedRotationTranslation() * inCenterOfMassTransform2);
+	Vec3 normal = plane.GetNormal();
+
+	// Get support function
+	ConvexShape::SupportBuffer shape1_support_buffer;
+	const ConvexShape::Support *shape1_support = shape1->GetSupportFunction(ConvexShape::ESupportMode::Default, shape1_support_buffer, inScale1);
+
+	// Get the support point of the convex shape in the opposite direction of the plane normal
+	Vec3 support_point = shape1_support->GetSupport(-normal);
+	float signed_distance = plane.SignedDistance(support_point);
+	float convex_radius = shape1_support->GetConvexRadius();
+	float penetration_depth = -signed_distance + convex_radius;
+	if (penetration_depth > -inCollideShapeSettings.mMaxSeparationDistance)
+	{
+		// Get contact point
+		Vec3 point1 = inCenterOfMassTransform1 * (support_point - normal * convex_radius);
+		Vec3 point2 = inCenterOfMassTransform1 * (support_point - normal * signed_distance);
+		Vec3 penetration_axis_world = inCenterOfMassTransform1.Multiply3x3(-normal);
+
+		// Create collision result
+		CollideShapeResult result(point1, point2, penetration_axis_world, penetration_depth, inSubShapeIDCreator1.GetID(), inSubShapeIDCreator2.GetID(), TransformedShape::sGetBodyID(ioCollector.GetContext()));
+
+		// Gather faces
+		if (inCollideShapeSettings.mCollectFacesMode == ECollectFacesMode::CollectFaces)
+		{
+			// Get supporting face of shape 1
+			shape1->GetSupportingFace(SubShapeID(), normal, inScale1, inCenterOfMassTransform1, result.mShape1Face);
+
+			// Get supporting face of shape 2
+			if (!result.mShape1Face.empty())
+				sGetSupportingFace(shape1, inCenterOfMassTransform1.GetTranslation(), scaled_plane, inCenterOfMassTransform2, result.mShape2Face);
+		}
+
+		// Notify the collector
+		JPH_IF_TRACK_NARROWPHASE_STATS(TrackNarrowPhaseCollector track;)
+		ioCollector.AddHit(result);
+	}
+}
+
+void PlaneShape::SaveBinaryState(StreamOut &inStream) const
+{
+	Shape::SaveBinaryState(inStream);
+
+	inStream.Write(mPlane);
+	inStream.Write(mHalfExtent);
+}
+
+void PlaneShape::RestoreBinaryState(StreamIn &inStream)
+{
+	Shape::RestoreBinaryState(inStream);
+
+	inStream.Read(mPlane);
+	inStream.Read(mHalfExtent);
+
+	CalculateLocalBounds();
+}
+
+void PlaneShape::SaveMaterialState(PhysicsMaterialList &outMaterials) const
+{
+	outMaterials = { mMaterial };
+}
+
+void PlaneShape::RestoreMaterialState(const PhysicsMaterialRefC *inMaterials, uint inNumMaterials)
+{
+	JPH_ASSERT(inNumMaterials == 1);
+	mMaterial = inMaterials[0];
+}
+
+void PlaneShape::sRegister()
+{
+	ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::Plane);
+	f.mConstruct = []() -> Shape * { return new PlaneShape; };
+	f.mColor = Color::sDarkRed;
+
+	for (EShapeSubType s : sConvexSubShapeTypes)
+	{
+		CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::Plane, sCollideConvexVsPlane);
+		CollisionDispatch::sRegisterCastShape(s, EShapeSubType::Plane, sCastConvexVsPlane);
+
+		CollisionDispatch::sRegisterCastShape(EShapeSubType::Plane, s, CollisionDispatch::sReversedCastShape);
+		CollisionDispatch::sRegisterCollideShape(EShapeSubType::Plane, s, CollisionDispatch::sReversedCollideShape);
+	}
+}
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/PlaneShape.h

@@ -0,0 +1,147 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2024 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/Shape/Shape.h>
+#include <Jolt/Physics/Collision/Shape/SubShapeID.h>
+#include <Jolt/Physics/Collision/PhysicsMaterial.h>
+
+JPH_NAMESPACE_BEGIN
+
+class CollideShapeSettings;
+
+/// Class that constructs a PlaneShape
+class JPH_EXPORT PlaneShapeSettings final : public ShapeSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, PlaneShapeSettings)
+
+public:
+	/// Default constructor for deserialization
+									PlaneShapeSettings() = default;
+
+	/// Create a plane shape.
+									PlaneShapeSettings(const Plane &inPlane, const PhysicsMaterial *inMaterial = nullptr, float inHalfExtent = cDefaultHalfExtent) : mPlane(inPlane), mMaterial(inMaterial), mHalfExtent(inHalfExtent) { }
+
+	// See: ShapeSettings
+	virtual ShapeResult				Create() const override;
+
+	Plane							mPlane;														///< Plane that describes the shape. The negative half space is considered solid.
+
+	RefConst<PhysicsMaterial>		mMaterial;													///< Surface material of the plane
+
+	static constexpr float			cDefaultHalfExtent = 1000.0f;								///< Default half-extent of the plane (total size along 1 axis will be 2 * half-extent)
+
+	float							mHalfExtent = cDefaultHalfExtent;							///< The bounding box of this plane will run from [-half_extent, half_extent]. Keep this as low as possible for better broad phase performance.
+};
+
+/// A plane shape. The negative half space is considered solid. Planes cannot be dynamic objects, only static or kinematic.
+/// The plane is considered an infinite shape, but testing collision outside of its bounding box (defined by the half-extent parameter) will not return a collision result.
+/// At the edge of the bounding box collision with the plane will be inconsistent. If you need something of a well defined size, a box shape may be better.
+class JPH_EXPORT PlaneShape final : public Shape
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+									PlaneShape() : Shape(EShapeType::Plane, EShapeSubType::Plane) { }
+									PlaneShape(const Plane &inPlane, const PhysicsMaterial *inMaterial = nullptr, float inHalfExtent = PlaneShapeSettings::cDefaultHalfExtent) : Shape(EShapeType::Plane, EShapeSubType::Plane), mPlane(inPlane), mMaterial(inMaterial), mHalfExtent(inHalfExtent) { CalculateLocalBounds(); }
+									PlaneShape(const PlaneShapeSettings &inSettings, ShapeResult &outResult);
+
+	/// Get the plane
+	const Plane &					GetPlane() const											{ return mPlane; }
+
+	/// Get the half-extent of the bounding box of the plane
+	float							GetHalfExtent() const										{ return mHalfExtent; }
+
+	// See Shape::MustBeStatic
+	virtual bool					MustBeStatic() const override								{ return true; }
+
+	// See Shape::GetLocalBounds
+	virtual AABox					GetLocalBounds() const override								{ return mLocalBounds; }
+
+	// See Shape::GetSubShapeIDBitsRecursive
+	virtual uint					GetSubShapeIDBitsRecursive() const override					{ return 0; }
+
+	// See Shape::GetInnerRadius
+	virtual float					GetInnerRadius() const override								{ return 0.0f; }
+
+	// See Shape::GetMassProperties
+	virtual MassProperties			GetMassProperties() const override;
+
+	// See Shape::GetMaterial
+	virtual const PhysicsMaterial *	GetMaterial(const SubShapeID &inSubShapeID) const override	{ JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); return GetMaterial(); }
+
+	// See Shape::GetSurfaceNormal
+	virtual Vec3					GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override { JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); return mPlane.GetNormal(); }
+
+	// See Shape::GetSupportingFace
+	virtual void					GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override;
+
+#ifdef JPH_DEBUG_RENDERER
+	// See Shape::Draw
+	virtual void					Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override;
+#endif // JPH_DEBUG_RENDERER
+
+	// See Shape::CastRay
+	virtual bool					CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override;
+	virtual void					CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
+
+	// See: Shape::CollidePoint
+	virtual void					CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
+
+	// See: Shape::CollideSoftBodyVertices
+	virtual void					CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override;
+
+	// See Shape::GetTrianglesStart
+	virtual void					GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override;
+
+	// See Shape::GetTrianglesNext
+	virtual int						GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override;
+
+	// See Shape::GetSubmergedVolume
+	virtual void					GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const override { JPH_ASSERT(false, "Not supported"); }
+
+	// See Shape
+	virtual void					SaveBinaryState(StreamOut &inStream) const override;
+	virtual void					SaveMaterialState(PhysicsMaterialList &outMaterials) const override;
+	virtual void					RestoreMaterialState(const PhysicsMaterialRefC *inMaterials, uint inNumMaterials) override;
+
+	// See Shape::GetStats
+	virtual Stats					GetStats() const override									{ return Stats(sizeof(*this), 0); }
+
+	// See Shape::GetVolume
+	virtual float					GetVolume() const override									{ return 0; }
+
+	/// Material of the shape
+	void							SetMaterial(const PhysicsMaterial *inMaterial)				{ mMaterial = inMaterial; }
+	const PhysicsMaterial *			GetMaterial() const											{ return mMaterial != nullptr? mMaterial : PhysicsMaterial::sDefault; }
+
+	// Register shape functions with the registry
+	static void						sRegister();
+
+protected:
+	// See: Shape::RestoreBinaryState
+	virtual void					RestoreBinaryState(StreamIn &inStream) override;
+
+private:
+	struct							PSGetTrianglesContext;										///< Context class for GetTrianglesStart/Next
+
+	// Get 4 vertices that form the plane
+	void							GetVertices(Vec3 *outVertices) const;
+
+	// Cache the local bounds
+	void							CalculateLocalBounds();
+
+	// Helper functions called by CollisionDispatch
+	static void						sCollideConvexVsPlane(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter);
+	static void						sCastConvexVsPlane(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector);
+
+	Plane							mPlane;
+	RefConst<PhysicsMaterial>		mMaterial;
+	float							mHalfExtent;
+	AABox							mLocalBounds;
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/PolyhedronSubmergedVolumeCalculator.h

@@ -0,0 +1,319 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Geometry/Plane.h>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+/// This class calculates the intersection between a fluid surface and a polyhedron and returns the submerged volume and its center of buoyancy
+/// Construct this class and then one by one add all faces of the polyhedron using the AddFace function. After all faces have been added the result
+/// can be gotten through GetResult.
+class PolyhedronSubmergedVolumeCalculator
+{
+private:
+	// Calculate submerged volume * 6 and center of mass * 4 for a tetrahedron with 4 vertices submerged
+	// inV1 .. inV4 are submerged
+	inline static void	sTetrahedronVolume4(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3, Vec3Arg inV4, float &outVolumeTimes6, Vec3 &outCenterTimes4)
+	{
+		// Calculate center of mass and mass of this tetrahedron,
+		// see: https://en.wikipedia.org/wiki/Tetrahedron#Volume
+		outVolumeTimes6 = max((inV1 - inV4).Dot((inV2 - inV4).Cross(inV3 - inV4)), 0.0f); // All contributions should be positive because we use a reference point that is on the surface of the hull
+		outCenterTimes4 = inV1 + inV2 + inV3 + inV4;
+	}
+
+	// Get the intersection point with a plane.
+	// inV1 is inD1 distance away from the plane, inV2 is inD2 distance away from the plane
+	inline static Vec3	sGetPlaneIntersection(Vec3Arg inV1, float inD1, Vec3Arg inV2, float inD2)
+	{
+		JPH_ASSERT(Sign(inD1) != Sign(inD2), "Assuming both points are on opposite ends of the plane");
+		float delta = inD1 - inD2;
+		if (abs(delta) < 1.0e-6f)
+			return inV1; // Parallel to plane, just pick a point
+		else
+			return inV1 + inD1 * (inV2 - inV1) / delta;
+	}
+
+	// Calculate submerged volume * 6 and center of mass * 4 for a tetrahedron with 1 vertex submerged
+	// inV1 is submerged, inV2 .. inV4 are not
+	// inD1 .. inD4 are the distances from the points to the plane
+	inline JPH_IF_NOT_DEBUG_RENDERER(static) void sTetrahedronVolume1(Vec3Arg inV1, float inD1, Vec3Arg inV2, float inD2, Vec3Arg inV3, float inD3, Vec3Arg inV4, float inD4, float &outVolumeTimes6, Vec3 &outCenterTimes4)
+	{
+		// A tetrahedron with 1 point submerged is cut along 3 edges forming a new tetrahedron
+		Vec3 v2 = sGetPlaneIntersection(inV1, inD1, inV2, inD2);
+		Vec3 v3 = sGetPlaneIntersection(inV1, inD1, inV3, inD3);
+		Vec3 v4 = sGetPlaneIntersection(inV1, inD1, inV4, inD4);
+
+	#ifdef JPH_DEBUG_RENDERER
+		// Draw intersection between tetrahedron and surface
+		if (Shape::sDrawSubmergedVolumes)
+		{
+			RVec3 v2w = mBaseOffset + v2;
+			RVec3 v3w = mBaseOffset + v3;
+			RVec3 v4w = mBaseOffset + v4;
+
+			DebugRenderer::sInstance->DrawTriangle(v4w, v3w, v2w, Color::sGreen);
+			DebugRenderer::sInstance->DrawWireTriangle(v4w, v3w, v2w, Color::sWhite);
+		}
+	#endif // JPH_DEBUG_RENDERER
+
+		sTetrahedronVolume4(inV1, v2, v3, v4, outVolumeTimes6, outCenterTimes4);
+	}
+
+	// Calculate submerged volume * 6 and center of mass * 4 for a tetrahedron with 2 vertices submerged
+	// inV1, inV2 are submerged, inV3, inV4 are not
+	// inD1 .. inD4 are the distances from the points to the plane
+	inline JPH_IF_NOT_DEBUG_RENDERER(static) void sTetrahedronVolume2(Vec3Arg inV1, float inD1, Vec3Arg inV2, float inD2, Vec3Arg inV3, float inD3, Vec3Arg inV4, float inD4, float &outVolumeTimes6, Vec3 &outCenterTimes4)
+	{
+		// A tetrahedron with 2 points submerged is cut along 4 edges forming a quad
+		Vec3 c = sGetPlaneIntersection(inV1, inD1, inV3, inD3);
+		Vec3 d = sGetPlaneIntersection(inV1, inD1, inV4, inD4);
+		Vec3 e = sGetPlaneIntersection(inV2, inD2, inV4, inD4);
+		Vec3 f = sGetPlaneIntersection(inV2, inD2, inV3, inD3);
+
+	#ifdef JPH_DEBUG_RENDERER
+		// Draw intersection between tetrahedron and surface
+		if (Shape::sDrawSubmergedVolumes)
+		{
+			RVec3 cw = mBaseOffset + c;
+			RVec3 dw = mBaseOffset + d;
+			RVec3 ew = mBaseOffset + e;
+			RVec3 fw = mBaseOffset + f;
+
+			DebugRenderer::sInstance->DrawTriangle(cw, ew, dw, Color::sGreen);
+			DebugRenderer::sInstance->DrawTriangle(cw, fw, ew, Color::sGreen);
+			DebugRenderer::sInstance->DrawWireTriangle(cw, ew, dw, Color::sWhite);
+			DebugRenderer::sInstance->DrawWireTriangle(cw, fw, ew, Color::sWhite);
+		}
+	#endif // JPH_DEBUG_RENDERER
+
+		// We pick point c as reference (which is on the cut off surface)
+		// This leaves us with three tetrahedrons to sum up (any faces that are in the same plane as c will have zero volume)
+		Vec3 center1, center2, center3;
+		float volume1, volume2, volume3;
+		sTetrahedronVolume4(e, f, inV2, c, volume1, center1);
+		sTetrahedronVolume4(e, inV1, d, c, volume2, center2);
+		sTetrahedronVolume4(e, inV2, inV1, c, volume3, center3);
+
+		// Tally up the totals
+		outVolumeTimes6 = volume1 + volume2 + volume3;
+		outCenterTimes4 = outVolumeTimes6 > 0.0f? (volume1 * center1 + volume2 * center2 + volume3 * center3) / outVolumeTimes6 : Vec3::sZero();
+	}
+
+	// Calculate submerged volume * 6 and center of mass * 4 for a tetrahedron with 3 vertices submerged
+	// inV1, inV2, inV3 are submerged, inV4 is not
+	// inD1 .. inD4 are the distances from the points to the plane
+	inline JPH_IF_NOT_DEBUG_RENDERER(static) void sTetrahedronVolume3(Vec3Arg inV1, float inD1, Vec3Arg inV2, float inD2, Vec3Arg inV3, float inD3, Vec3Arg inV4, float inD4, float &outVolumeTimes6, Vec3 &outCenterTimes4)
+	{
+		// A tetrahedron with 1 point above the surface is cut along 3 edges forming a new tetrahedron
+		Vec3 v1 = sGetPlaneIntersection(inV1, inD1, inV4, inD4);
+		Vec3 v2 = sGetPlaneIntersection(inV2, inD2, inV4, inD4);
+		Vec3 v3 = sGetPlaneIntersection(inV3, inD3, inV4, inD4);
+
+	#ifdef JPH_DEBUG_RENDERER
+		// Draw intersection between tetrahedron and surface
+		if (Shape::sDrawSubmergedVolumes)
+		{
+			RVec3 v1w = mBaseOffset + v1;
+			RVec3 v2w = mBaseOffset + v2;
+			RVec3 v3w = mBaseOffset + v3;
+
+			DebugRenderer::sInstance->DrawTriangle(v3w, v2w, v1w, Color::sGreen);
+			DebugRenderer::sInstance->DrawWireTriangle(v3w, v2w, v1w, Color::sWhite);
+		}
+	#endif // JPH_DEBUG_RENDERER
+
+		Vec3 dry_center, total_center;
+		float dry_volume, total_volume;
+
+		// We first calculate the part that is above the surface
+		sTetrahedronVolume4(v1, v2, v3, inV4, dry_volume, dry_center);
+
+		// Calculate the total volume
+		sTetrahedronVolume4(inV1, inV2, inV3, inV4, total_volume, total_center);
+
+		// From this we can calculate the center and volume of the submerged part
+		outVolumeTimes6 = max(total_volume - dry_volume, 0.0f);
+		outCenterTimes4 = outVolumeTimes6 > 0.0f? (total_center * total_volume - dry_center * dry_volume) / outVolumeTimes6 : Vec3::sZero();
+	}
+
+public:
+	/// A helper class that contains cached information about a polyhedron vertex
+	class Point
+	{
+	public:
+		Vec3			mPosition;						///< World space position of vertex
+		float			mDistanceToSurface;				///< Signed distance to the surface (> 0 is above, < 0 is below)
+		bool			mAboveSurface;					///< If the point is above the surface (mDistanceToSurface > 0)
+	};
+
+	/// Constructor
+	/// @param inTransform Transform to transform all incoming points with
+	/// @param inPoints Array of points that are part of the polyhedron
+	/// @param inPointStride Amount of bytes between each point (should usually be sizeof(Vec3))
+	/// @param inNumPoints The amount of points
+	/// @param inSurface The plane that forms the fluid surface (normal should point up)
+	/// @param ioBuffer A temporary buffer of Point's that should have inNumPoints entries and should stay alive while this class is alive
+#ifdef JPH_DEBUG_RENDERER
+	/// @param inBaseOffset The offset to transform inTransform to world space (in double precision mode this can be used to shift the whole operation closer to the origin). Only used for debug drawing.
+#endif // JPH_DEBUG_RENDERER
+						PolyhedronSubmergedVolumeCalculator(const Mat44 &inTransform, const Vec3 *inPoints, int inPointStride, int inNumPoints, const Plane &inSurface, Point *ioBuffer
+#ifdef JPH_DEBUG_RENDERER // Not using JPH_IF_DEBUG_RENDERER for Doxygen
+		, RVec3 inBaseOffset
+#endif // JPH_DEBUG_RENDERER
+		) :
+		mPoints(ioBuffer)
+#ifdef JPH_DEBUG_RENDERER
+		, mBaseOffset(inBaseOffset)
+#endif // JPH_DEBUG_RENDERER
+	{
+		// Convert the points to world space and determine the distance to the surface
+		float reference_dist = FLT_MAX;
+		for (int p = 0; p < inNumPoints; ++p)
+		{
+			// Calculate values
+			Vec3 transformed_point = inTransform * *reinterpret_cast<const Vec3 *>(reinterpret_cast<const uint8 *>(inPoints) + p * inPointStride);
+			float dist = inSurface.SignedDistance(transformed_point);
+			bool above = dist >= 0.0f;
+
+			// Keep track if all are above or below
+			mAllAbove &= above;
+			mAllBelow &= !above;
+
+			// Calculate lowest point, we use this to create tetrahedrons out of all faces
+			if (reference_dist > dist)
+			{
+				mReferencePointIdx = p;
+				reference_dist = dist;
+			}
+
+			// Store values
+			ioBuffer->mPosition = transformed_point;
+			ioBuffer->mDistanceToSurface = dist;
+			ioBuffer->mAboveSurface = above;
+			++ioBuffer;
+		}
+	}
+
+	/// Check if all points are above the surface. Should be used as early out.
+	inline bool			AreAllAbove() const
+	{
+		return mAllAbove;
+	}
+
+	/// Check if all points are below the surface. Should be used as early out.
+	inline bool			AreAllBelow() const
+	{
+		return mAllBelow;
+	}
+
+	/// Get the lowest point of the polyhedron. Used to form the 4th vertex to make a tetrahedron out of a polyhedron face.
+	inline int			GetReferencePointIdx() const
+	{
+		return mReferencePointIdx;
+	}
+
+	/// Add a polyhedron face. Supply the indices of the points that form the face (in counter clockwise order).
+	void				AddFace(int inIdx1, int inIdx2, int inIdx3)
+	{
+		JPH_ASSERT(inIdx1 != mReferencePointIdx && inIdx2 != mReferencePointIdx && inIdx3 != mReferencePointIdx, "A face using the reference point will not contribute to the volume");
+
+		// Find the points
+		const Point &ref = mPoints[mReferencePointIdx];
+		const Point &p1 = mPoints[inIdx1];
+		const Point &p2 = mPoints[inIdx2];
+		const Point &p3 = mPoints[inIdx3];
+
+		// Determine which vertices are submerged
+		uint code = (p1.mAboveSurface? 0 : 0b001) | (p2.mAboveSurface? 0 : 0b010) | (p3.mAboveSurface? 0 : 0b100);
+
+		float volume;
+		Vec3 center;
+		switch (code)
+		{
+		case 0b000:
+			// One point submerged
+			sTetrahedronVolume1(ref.mPosition, ref.mDistanceToSurface, p3.mPosition, p3.mDistanceToSurface, p2.mPosition, p2.mDistanceToSurface, p1.mPosition, p1.mDistanceToSurface, volume, center);
+			break;
+
+		case 0b001:
+			// Two points submerged
+			sTetrahedronVolume2(ref.mPosition, ref.mDistanceToSurface, p1.mPosition, p1.mDistanceToSurface, p3.mPosition, p3.mDistanceToSurface, p2.mPosition, p2.mDistanceToSurface, volume, center);
+			break;
+
+		case 0b010:
+			// Two points submerged
+			sTetrahedronVolume2(ref.mPosition, ref.mDistanceToSurface, p2.mPosition, p2.mDistanceToSurface, p1.mPosition, p1.mDistanceToSurface, p3.mPosition, p3.mDistanceToSurface, volume, center);
+			break;
+
+		case 0b100:
+			// Two points submerged
+			sTetrahedronVolume2(ref.mPosition, ref.mDistanceToSurface, p3.mPosition, p3.mDistanceToSurface, p2.mPosition, p2.mDistanceToSurface, p1.mPosition, p1.mDistanceToSurface, volume, center);
+			break;
+
+		case 0b011:
+			// Three points submerged
+			sTetrahedronVolume3(ref.mPosition, ref.mDistanceToSurface, p2.mPosition, p2.mDistanceToSurface, p1.mPosition, p1.mDistanceToSurface, p3.mPosition, p3.mDistanceToSurface, volume, center);
+			break;
+
+		case 0b101:
+			// Three points submerged
+			sTetrahedronVolume3(ref.mPosition, ref.mDistanceToSurface, p1.mPosition, p1.mDistanceToSurface, p3.mPosition, p3.mDistanceToSurface, p2.mPosition, p2.mDistanceToSurface, volume, center);
+			break;
+
+		case 0b110:
+			// Three points submerged
+			sTetrahedronVolume3(ref.mPosition, ref.mDistanceToSurface, p3.mPosition, p3.mDistanceToSurface, p2.mPosition, p2.mDistanceToSurface, p1.mPosition, p1.mDistanceToSurface, volume, center);
+			break;
+
+		case 0b111:
+			// Four points submerged
+			sTetrahedronVolume4(ref.mPosition, p3.mPosition, p2.mPosition, p1.mPosition, volume, center);
+			break;
+
+		default:
+			// Should not be possible
+			JPH_ASSERT(false);
+			volume = 0.0f;
+			center = Vec3::sZero();
+			break;
+		}
+
+		mSubmergedVolume += volume;
+		mCenterOfBuoyancy += volume * center;
+	}
+
+	/// Call after all faces have been added. Returns the submerged volume and the center of buoyancy for the submerged volume.
+	void				GetResult(float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy) const
+	{
+		outCenterOfBuoyancy = mSubmergedVolume > 0.0f? mCenterOfBuoyancy / (4.0f * mSubmergedVolume) : Vec3::sZero(); // Do this before dividing submerged volume by 6 to get correct weight factor
+		outSubmergedVolume = mSubmergedVolume / 6.0f;
+	}
+
+private:
+	// The precalculated points for this polyhedron
+	const Point *		mPoints;
+
+	// If all points are above/below the surface
+	bool				mAllBelow = true;
+	bool				mAllAbove = true;
+
+	// The lowest point
+	int					mReferencePointIdx = 0;
+
+	// Aggregator for submerged volume and center of buoyancy
+	float				mSubmergedVolume = 0.0f;
+	Vec3				mCenterOfBuoyancy = Vec3::sZero();
+
+#ifdef JPH_DEBUG_RENDERER
+	// Base offset used for drawing
+	RVec3				mBaseOffset;
+#endif
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/RotatedTranslatedShape.cpp

@@ -0,0 +1,333 @@
+// 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/Shape/RotatedTranslatedShape.h>
+#include <Jolt/Physics/Collision/CollisionDispatch.h>
+#include <Jolt/Physics/Collision/RayCast.h>
+#include <Jolt/Physics/Collision/ShapeCast.h>
+#include <Jolt/Physics/Collision/TransformedShape.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(RotatedTranslatedShapeSettings)
+{
+	JPH_ADD_BASE_CLASS(RotatedTranslatedShapeSettings, DecoratedShapeSettings)
+
+	JPH_ADD_ATTRIBUTE(RotatedTranslatedShapeSettings, mPosition)
+	JPH_ADD_ATTRIBUTE(RotatedTranslatedShapeSettings, mRotation)
+}
+
+ShapeSettings::ShapeResult RotatedTranslatedShapeSettings::Create() const
+{
+	if (mCachedResult.IsEmpty())
+		Ref<Shape> shape = new RotatedTranslatedShape(*this, mCachedResult);
+	return mCachedResult;
+}
+
+RotatedTranslatedShape::RotatedTranslatedShape(const RotatedTranslatedShapeSettings &inSettings, ShapeResult &outResult) :
+	DecoratedShape(EShapeSubType::RotatedTranslated, inSettings, outResult)
+{
+	if (outResult.HasError())
+		return;
+
+	// Calculate center of mass position
+	mCenterOfMass = inSettings.mPosition + inSettings.mRotation * mInnerShape->GetCenterOfMass();
+
+	// Store rotation (position is always zero because we center around the center of mass)
+	mRotation = inSettings.mRotation;
+	mIsRotationIdentity = mRotation.IsClose(Quat::sIdentity());
+
+	outResult.Set(this);
+}
+
+RotatedTranslatedShape::RotatedTranslatedShape(Vec3Arg inPosition, QuatArg inRotation, const Shape *inShape) :
+	DecoratedShape(EShapeSubType::RotatedTranslated, inShape)
+{
+	// Calculate center of mass position
+	mCenterOfMass = inPosition + inRotation * mInnerShape->GetCenterOfMass();
+
+	// Store rotation (position is always zero because we center around the center of mass)
+	mRotation = inRotation;
+	mIsRotationIdentity = mRotation.IsClose(Quat::sIdentity());
+}
+
+MassProperties RotatedTranslatedShape::GetMassProperties() const
+{
+	// Rotate inertia of child into place
+	MassProperties p = mInnerShape->GetMassProperties();
+	p.Rotate(Mat44::sRotation(mRotation));
+	return p;
+}
+
+AABox RotatedTranslatedShape::GetLocalBounds() const
+{
+	return mInnerShape->GetLocalBounds().Transformed(Mat44::sRotation(mRotation));
+}
+
+AABox RotatedTranslatedShape::GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const
+{
+	Mat44 transform = inCenterOfMassTransform * Mat44::sRotation(mRotation);
+	return mInnerShape->GetWorldSpaceBounds(transform, TransformScale(inScale));
+}
+
+TransformedShape RotatedTranslatedShape::GetSubShapeTransformedShape(const SubShapeID &inSubShapeID, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, SubShapeID &outRemainder) const
+{
+	// We don't use any bits in the sub shape ID
+	outRemainder = inSubShapeID;
+
+	TransformedShape ts(RVec3(inPositionCOM), inRotation * mRotation, mInnerShape, BodyID());
+	ts.SetShapeScale(TransformScale(inScale));
+	return ts;
+}
+
+Vec3 RotatedTranslatedShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const
+{
+	// Transform surface position to local space and pass call on
+	Mat44 transform = Mat44::sRotation(mRotation.Conjugated());
+	Vec3 normal = mInnerShape->GetSurfaceNormal(inSubShapeID, transform * inLocalSurfacePosition);
+
+	// Transform normal to this shape's space
+	return transform.Multiply3x3Transposed(normal);
+}
+
+void RotatedTranslatedShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const
+{
+	Mat44 transform = Mat44::sRotation(mRotation);
+	mInnerShape->GetSupportingFace(inSubShapeID, transform.Multiply3x3Transposed(inDirection), TransformScale(inScale), inCenterOfMassTransform * transform, outVertices);
+}
+
+void RotatedTranslatedShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const
+{
+	// Get center of mass transform of child
+	Mat44 transform = inCenterOfMassTransform * Mat44::sRotation(mRotation);
+
+	// Recurse to child
+	mInnerShape->GetSubmergedVolume(transform, TransformScale(inScale), inSurface, outTotalVolume, outSubmergedVolume, outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, inBaseOffset));
+}
+
+#ifdef JPH_DEBUG_RENDERER
+void RotatedTranslatedShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
+{
+	mInnerShape->Draw(inRenderer, inCenterOfMassTransform * Mat44::sRotation(mRotation), TransformScale(inScale), inColor, inUseMaterialColors, inDrawWireframe);
+}
+
+void RotatedTranslatedShape::DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const
+{
+	mInnerShape->DrawGetSupportFunction(inRenderer, inCenterOfMassTransform * Mat44::sRotation(mRotation), TransformScale(inScale), inColor, inDrawSupportDirection);
+}
+
+void RotatedTranslatedShape::DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const
+{
+	mInnerShape->DrawGetSupportingFace(inRenderer, inCenterOfMassTransform * Mat44::sRotation(mRotation), TransformScale(inScale));
+}
+#endif // JPH_DEBUG_RENDERER
+
+bool RotatedTranslatedShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const
+{
+	// Transform the ray
+	Mat44 transform = Mat44::sRotation(mRotation.Conjugated());
+	RayCast ray = inRay.Transformed(transform);
+
+	return mInnerShape->CastRay(ray, inSubShapeIDCreator, ioHit);
+}
+
+void RotatedTranslatedShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	// Test shape filter
+	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
+		return;
+
+	// Transform the ray
+	Mat44 transform = Mat44::sRotation(mRotation.Conjugated());
+	RayCast ray = inRay.Transformed(transform);
+
+	return mInnerShape->CastRay(ray, inRayCastSettings, inSubShapeIDCreator, ioCollector, inShapeFilter);
+}
+
+void RotatedTranslatedShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	// Test shape filter
+	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
+		return;
+
+	// Transform the point
+	Mat44 transform = Mat44::sRotation(mRotation.Conjugated());
+	mInnerShape->CollidePoint(transform * inPoint, inSubShapeIDCreator, ioCollector, inShapeFilter);
+}
+
+void RotatedTranslatedShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const
+{
+	mInnerShape->CollideSoftBodyVertices(inCenterOfMassTransform * Mat44::sRotation(mRotation), inScale, inVertices, inNumVertices, inCollidingShapeIndex);
+}
+
+void RotatedTranslatedShape::CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	// Test shape filter
+	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
+		return;
+
+	mInnerShape->CollectTransformedShapes(inBox, inPositionCOM, inRotation * mRotation, TransformScale(inScale), inSubShapeIDCreator, ioCollector, inShapeFilter);
+}
+
+void RotatedTranslatedShape::TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const
+{
+	mInnerShape->TransformShape(inCenterOfMassTransform * Mat44::sRotation(mRotation), ioCollector);
+}
+
+void RotatedTranslatedShape::sCollideRotatedTranslatedVsShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter)
+{
+	JPH_ASSERT(inShape1->GetSubType() == EShapeSubType::RotatedTranslated);
+	const RotatedTranslatedShape *shape1 = static_cast<const RotatedTranslatedShape *>(inShape1);
+
+	// Get world transform of 1
+	Mat44 transform1 = inCenterOfMassTransform1 * Mat44::sRotation(shape1->mRotation);
+
+	CollisionDispatch::sCollideShapeVsShape(shape1->mInnerShape, inShape2, shape1->TransformScale(inScale1), inScale2, transform1, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter);
+}
+
+void RotatedTranslatedShape::sCollideShapeVsRotatedTranslated(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter)
+{
+	JPH_ASSERT(inShape2->GetSubType() == EShapeSubType::RotatedTranslated);
+	const RotatedTranslatedShape *shape2 = static_cast<const RotatedTranslatedShape *>(inShape2);
+
+	// Get world transform of 2
+	Mat44 transform2 = inCenterOfMassTransform2 * Mat44::sRotation(shape2->mRotation);
+
+	CollisionDispatch::sCollideShapeVsShape(inShape1, shape2->mInnerShape, inScale1, shape2->TransformScale(inScale2), inCenterOfMassTransform1, transform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter);
+}
+
+void RotatedTranslatedShape::sCollideRotatedTranslatedVsRotatedTranslated(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter)
+{
+	JPH_ASSERT(inShape1->GetSubType() == EShapeSubType::RotatedTranslated);
+	const RotatedTranslatedShape *shape1 = static_cast<const RotatedTranslatedShape *>(inShape1);
+	JPH_ASSERT(inShape2->GetSubType() == EShapeSubType::RotatedTranslated);
+	const RotatedTranslatedShape *shape2 = static_cast<const RotatedTranslatedShape *>(inShape2);
+
+	// Get world transform of 1 and 2
+	Mat44 transform1 = inCenterOfMassTransform1 * Mat44::sRotation(shape1->mRotation);
+	Mat44 transform2 = inCenterOfMassTransform2 * Mat44::sRotation(shape2->mRotation);
+
+	CollisionDispatch::sCollideShapeVsShape(shape1->mInnerShape, shape2->mInnerShape, shape1->TransformScale(inScale1), shape2->TransformScale(inScale2), transform1, transform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter);
+}
+
+void RotatedTranslatedShape::sCastRotatedTranslatedVsShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
+{
+	// Fetch rotated translated shape from cast shape
+	JPH_ASSERT(inShapeCast.mShape->GetSubType() == EShapeSubType::RotatedTranslated);
+	const RotatedTranslatedShape *shape1 = static_cast<const RotatedTranslatedShape *>(inShapeCast.mShape);
+
+	// Transform the shape cast and update the shape
+	Mat44 transform = inShapeCast.mCenterOfMassStart * Mat44::sRotation(shape1->mRotation);
+	Vec3 scale = shape1->TransformScale(inShapeCast.mScale);
+	ShapeCast shape_cast(shape1->mInnerShape, scale, transform, inShapeCast.mDirection);
+
+	CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, inShapeCastSettings, inShape, inScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector);
+}
+
+void RotatedTranslatedShape::sCastShapeVsRotatedTranslated(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
+{
+	JPH_ASSERT(inShape->GetSubType() == EShapeSubType::RotatedTranslated);
+	const RotatedTranslatedShape *shape = static_cast<const RotatedTranslatedShape *>(inShape);
+
+	// Determine the local transform
+	Mat44 local_transform = Mat44::sRotation(shape->mRotation);
+
+	// Transform the shape cast
+	ShapeCast shape_cast = inShapeCast.PostTransformed(local_transform.Transposed3x3());
+
+	CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, inShapeCastSettings, shape->mInnerShape, shape->TransformScale(inScale), inShapeFilter, inCenterOfMassTransform2 * local_transform, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector);
+}
+
+void RotatedTranslatedShape::sCastRotatedTranslatedVsRotatedTranslated(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
+{
+	JPH_ASSERT(inShapeCast.mShape->GetSubType() == EShapeSubType::RotatedTranslated);
+	const RotatedTranslatedShape *shape1 = static_cast<const RotatedTranslatedShape *>(inShapeCast.mShape);
+	JPH_ASSERT(inShape->GetSubType() == EShapeSubType::RotatedTranslated);
+	const RotatedTranslatedShape *shape2 = static_cast<const RotatedTranslatedShape *>(inShape);
+
+	// Determine the local transform of shape 2
+	Mat44 local_transform2 = Mat44::sRotation(shape2->mRotation);
+	Mat44 local_transform2_transposed = local_transform2.Transposed3x3();
+
+	// Transform the shape cast and update the shape
+	Mat44 transform = (local_transform2_transposed * inShapeCast.mCenterOfMassStart) * Mat44::sRotation(shape1->mRotation);
+	Vec3 scale = shape1->TransformScale(inShapeCast.mScale);
+	ShapeCast shape_cast(shape1->mInnerShape, scale, transform, local_transform2_transposed.Multiply3x3(inShapeCast.mDirection));
+
+	CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, inShapeCastSettings, shape2->mInnerShape, shape2->TransformScale(inScale), inShapeFilter, inCenterOfMassTransform2 * local_transform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector);
+}
+
+void RotatedTranslatedShape::SaveBinaryState(StreamOut &inStream) const
+{
+	DecoratedShape::SaveBinaryState(inStream);
+
+	inStream.Write(mCenterOfMass);
+	inStream.Write(mRotation);
+}
+
+void RotatedTranslatedShape::RestoreBinaryState(StreamIn &inStream)
+{
+	DecoratedShape::RestoreBinaryState(inStream);
+
+	inStream.Read(mCenterOfMass);
+	inStream.Read(mRotation);
+	mIsRotationIdentity = mRotation.IsClose(Quat::sIdentity());
+}
+
+bool RotatedTranslatedShape::IsValidScale(Vec3Arg inScale) const
+{
+	if (!Shape::IsValidScale(inScale))
+		return false;
+
+	if (mIsRotationIdentity || ScaleHelpers::IsUniformScale(inScale))
+		return mInnerShape->IsValidScale(inScale);
+
+	if (!ScaleHelpers::CanScaleBeRotated(mRotation, inScale))
+		return false;
+
+	return mInnerShape->IsValidScale(ScaleHelpers::RotateScale(mRotation, inScale));
+}
+
+Vec3 RotatedTranslatedShape::MakeScaleValid(Vec3Arg inScale) const
+{
+	Vec3 scale = ScaleHelpers::MakeNonZeroScale(inScale);
+
+	if (mIsRotationIdentity || ScaleHelpers::IsUniformScale(scale))
+		return mInnerShape->MakeScaleValid(scale);
+
+	if (ScaleHelpers::CanScaleBeRotated(mRotation, scale))
+		return ScaleHelpers::RotateScale(mRotation.Conjugated(), mInnerShape->MakeScaleValid(ScaleHelpers::RotateScale(mRotation, scale)));
+
+	Vec3 abs_uniform_scale = ScaleHelpers::MakeUniformScale(scale.Abs());
+	Vec3 uniform_scale = scale.GetSign() * abs_uniform_scale;
+	if (ScaleHelpers::CanScaleBeRotated(mRotation, uniform_scale))
+		return uniform_scale;
+
+	return Sign(scale.GetX()) * abs_uniform_scale;
+}
+
+void RotatedTranslatedShape::sRegister()
+{
+	ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::RotatedTranslated);
+	f.mConstruct = []() -> Shape * { return new RotatedTranslatedShape; };
+	f.mColor = Color::sBlue;
+
+	for (EShapeSubType s : sAllSubShapeTypes)
+	{
+		CollisionDispatch::sRegisterCollideShape(EShapeSubType::RotatedTranslated, s, sCollideRotatedTranslatedVsShape);
+		CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::RotatedTranslated, sCollideShapeVsRotatedTranslated);
+		CollisionDispatch::sRegisterCastShape(EShapeSubType::RotatedTranslated, s, sCastRotatedTranslatedVsShape);
+		CollisionDispatch::sRegisterCastShape(s, EShapeSubType::RotatedTranslated, sCastShapeVsRotatedTranslated);
+	}
+
+	CollisionDispatch::sRegisterCollideShape(EShapeSubType::RotatedTranslated, EShapeSubType::RotatedTranslated, sCollideRotatedTranslatedVsRotatedTranslated);
+	CollisionDispatch::sRegisterCastShape(EShapeSubType::RotatedTranslated, EShapeSubType::RotatedTranslated, sCastRotatedTranslatedVsRotatedTranslated);
+}
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h

@@ -0,0 +1,161 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/Shape/DecoratedShape.h>
+#include <Jolt/Physics/Collision/Shape/ScaleHelpers.h>
+
+JPH_NAMESPACE_BEGIN
+
+class CollideShapeSettings;
+
+/// Class that constructs a RotatedTranslatedShape
+class JPH_EXPORT RotatedTranslatedShapeSettings final : public DecoratedShapeSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, RotatedTranslatedShapeSettings)
+
+public:
+	/// Constructor
+									RotatedTranslatedShapeSettings() = default;
+
+	/// Construct with shape settings, can be serialized.
+									RotatedTranslatedShapeSettings(Vec3Arg inPosition, QuatArg inRotation, const ShapeSettings *inShape) : DecoratedShapeSettings(inShape), mPosition(inPosition), mRotation(inRotation) { }
+
+	/// Variant that uses a concrete shape, which means this object cannot be serialized.
+									RotatedTranslatedShapeSettings(Vec3Arg inPosition, QuatArg inRotation, const Shape *inShape): DecoratedShapeSettings(inShape), mPosition(inPosition), mRotation(inRotation) { }
+
+	// See: ShapeSettings
+	virtual ShapeResult				Create() const override;
+
+	Vec3							mPosition;												///< Position of the sub shape
+	Quat							mRotation;												///< Rotation of the sub shape
+};
+
+/// A rotated translated shape will rotate and translate a child shape.
+/// Shifts the child object so that it is centered around the center of mass.
+class JPH_EXPORT RotatedTranslatedShape final : public DecoratedShape
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+									RotatedTranslatedShape() : DecoratedShape(EShapeSubType::RotatedTranslated) { }
+									RotatedTranslatedShape(const RotatedTranslatedShapeSettings &inSettings, ShapeResult &outResult);
+									RotatedTranslatedShape(Vec3Arg inPosition, QuatArg inRotation, const Shape *inShape);
+
+	/// Access the rotation that is applied to the inner shape
+	Quat							GetRotation() const										{ return mRotation; }
+
+	/// Access the translation that has been applied to the inner shape
+	Vec3							GetPosition() const										{ return mCenterOfMass - mRotation * mInnerShape->GetCenterOfMass(); }
+
+	// See Shape::GetCenterOfMass
+	virtual Vec3					GetCenterOfMass() const override						{ return mCenterOfMass; }
+
+	// See Shape::GetLocalBounds
+	virtual AABox					GetLocalBounds() const override;
+
+	// See Shape::GetWorldSpaceBounds
+	virtual AABox					GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override;
+	using Shape::GetWorldSpaceBounds;
+
+	// See Shape::GetInnerRadius
+	virtual float					GetInnerRadius() const override							{ return mInnerShape->GetInnerRadius(); }
+
+	// See Shape::GetMassProperties
+	virtual MassProperties			GetMassProperties() const override;
+
+	// See Shape::GetSubShapeTransformedShape
+	virtual TransformedShape		GetSubShapeTransformedShape(const SubShapeID &inSubShapeID, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, SubShapeID &outRemainder) const override;
+
+	// See Shape::GetSurfaceNormal
+	virtual Vec3					GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override;
+
+	// See Shape::GetSupportingFace
+	virtual void					GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override;
+
+	// See Shape::GetSubmergedVolume
+	virtual void					GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const override;
+
+#ifdef JPH_DEBUG_RENDERER
+	// See Shape::Draw
+	virtual void					Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override;
+
+	// See Shape::DrawGetSupportFunction
+	virtual void					DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const override;
+
+	// See Shape::DrawGetSupportingFace
+	virtual void					DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override;
+#endif // JPH_DEBUG_RENDERER
+
+	// See Shape::CastRay
+	virtual bool					CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override;
+	virtual void					CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
+
+	// See: Shape::CollidePoint
+	virtual void					CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
+
+	// See: Shape::CollideSoftBodyVertices
+	virtual void					CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override;
+
+	// See Shape::CollectTransformedShapes
+	virtual void					CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const override;
+
+	// See Shape::TransformShape
+	virtual void					TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const override;
+
+	// See Shape::GetTrianglesStart
+	virtual void					GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override { JPH_ASSERT(false, "Cannot call on non-leaf shapes, use CollectTransformedShapes to collect the leaves first!"); }
+
+	// See Shape::GetTrianglesNext
+	virtual int						GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override { JPH_ASSERT(false, "Cannot call on non-leaf shapes, use CollectTransformedShapes to collect the leaves first!"); return 0; }
+
+	// See Shape
+	virtual void					SaveBinaryState(StreamOut &inStream) const override;
+
+	// See Shape::GetStats
+	virtual Stats					GetStats() const override								{ return Stats(sizeof(*this), 0); }
+
+	// See Shape::GetVolume
+	virtual float					GetVolume() const override								{ return mInnerShape->GetVolume(); }
+
+	// See Shape::IsValidScale
+	virtual bool					IsValidScale(Vec3Arg inScale) const override;
+
+	// See Shape::MakeScaleValid
+	virtual Vec3					MakeScaleValid(Vec3Arg inScale) const override;
+
+	/// Transform the scale to the local space of the child shape
+	inline Vec3						TransformScale(Vec3Arg inScale) const
+	{
+		// We don't need to transform uniform scale or if the rotation is identity
+		if (mIsRotationIdentity || ScaleHelpers::IsUniformScale(inScale))
+			return inScale;
+
+		return ScaleHelpers::RotateScale(mRotation, inScale);
+	}
+
+	// Register shape functions with the registry
+	static void						sRegister();
+
+protected:
+	// See: Shape::RestoreBinaryState
+	virtual void					RestoreBinaryState(StreamIn &inStream) override;
+
+private:
+	// Helper functions called by CollisionDispatch
+	static void						sCollideRotatedTranslatedVsShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter);
+	static void						sCollideShapeVsRotatedTranslated(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter);
+	static void						sCollideRotatedTranslatedVsRotatedTranslated(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter);
+	static void						sCastRotatedTranslatedVsShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector);
+	static void						sCastShapeVsRotatedTranslated(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector);
+	static void						sCastRotatedTranslatedVsRotatedTranslated(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector);
+
+	bool							mIsRotationIdentity;									///< If mRotation is close to identity (put here because it falls in padding bytes)
+	Vec3							mCenterOfMass;											///< Position of the center of mass
+	Quat							mRotation;												///< Rotation of the child shape
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ScaleHelpers.h

@@ -0,0 +1,83 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/PhysicsSettings.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Helper functions to get properties of a scaling vector
+namespace ScaleHelpers
+{
+	/// Minimum valid scale value. This is used to prevent division by zero when scaling a shape with a zero scale.
+	static constexpr float	cMinScale = 1.0e-6f;
+
+	/// The tolerance used to check if components of the scale vector are the same
+	static constexpr float	cScaleToleranceSq = 1.0e-8f;
+
+	/// Test if a scale is identity
+	inline bool				IsNotScaled(Vec3Arg inScale)									{ return inScale.IsClose(Vec3::sOne(), cScaleToleranceSq); }
+
+	/// Test if a scale is uniform
+	inline bool				IsUniformScale(Vec3Arg inScale)									{ return inScale.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_X>().IsClose(inScale, cScaleToleranceSq); }
+
+	/// Test if a scale is uniform in XZ
+	inline bool				IsUniformScaleXZ(Vec3Arg inScale)								{ return inScale.Swizzle<SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_X>().IsClose(inScale, ScaleHelpers::cScaleToleranceSq); }
+
+	/// Scale the convex radius of an object
+	inline float			ScaleConvexRadius(float inConvexRadius, Vec3Arg inScale)		{ return min(inConvexRadius * inScale.Abs().ReduceMin(), cDefaultConvexRadius); }
+
+	/// Test if a scale flips an object inside out (which requires flipping all normals and polygon windings)
+	inline bool				IsInsideOut(Vec3Arg inScale)									{ return (CountBits(Vec3::sLess(inScale, Vec3::sZero()).GetTrues() & 0x7) & 1) != 0; }
+
+	/// Test if any of the components of the scale have a value below cMinScale
+	inline bool				IsZeroScale(Vec3Arg inScale)									{ return Vec3::sLess(inScale.Abs(), Vec3::sReplicate(cMinScale)).TestAnyXYZTrue(); }
+
+	/// Ensure that the scale for each component is at least cMinScale
+	inline Vec3				MakeNonZeroScale(Vec3Arg inScale)								{ return inScale.GetSign() * Vec3::sMax(inScale.Abs(), Vec3::sReplicate(cMinScale)); }
+
+	/// Get the average scale if inScale, used to make the scale uniform when a shape doesn't support non-uniform scale
+	inline Vec3				MakeUniformScale(Vec3Arg inScale)								{ return Vec3::sReplicate((inScale.GetX() + inScale.GetY() + inScale.GetZ()) / 3.0f); }
+
+	/// Average the scale in XZ, used to make the scale uniform when a shape doesn't support non-uniform scale in the XZ plane
+	inline Vec3				MakeUniformScaleXZ(Vec3Arg inScale)								{ return 0.5f * (inScale + inScale.Swizzle<SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_X>()); }
+
+	/// Checks in scale can be rotated to child shape
+	/// @param inRotation Rotation of child shape
+	/// @param inScale Scale in local space of parent shape
+	/// @return True if the scale is valid (no shearing introduced)
+	inline bool				CanScaleBeRotated(QuatArg inRotation, Vec3Arg inScale)
+	{
+		// inScale is a scale in local space of the shape, so the transform for the shape (ignoring translation) is: T = Mat44::sScale(inScale) * mRotation.
+		// when we pass the scale to the child it needs to be local to the child, so we want T = mRotation * Mat44::sScale(ChildScale).
+		// Solving for ChildScale: ChildScale = mRotation^-1 * Mat44::sScale(inScale) * mRotation = mRotation^T * Mat44::sScale(inScale) * mRotation
+		// If any of the off diagonal elements are non-zero, it means the scale / rotation is not compatible.
+		Mat44 r = Mat44::sRotation(inRotation);
+		Mat44 child_scale = r.Multiply3x3LeftTransposed(r.PostScaled(inScale));
+
+		// Get the columns, but zero the diagonal
+		Vec4 zero = Vec4::sZero();
+		Vec4 c0 = Vec4::sSelect(child_scale.GetColumn4(0), zero, UVec4(0xffffffff, 0, 0, 0)).Abs();
+		Vec4 c1 = Vec4::sSelect(child_scale.GetColumn4(1), zero, UVec4(0, 0xffffffff, 0, 0)).Abs();
+		Vec4 c2 = Vec4::sSelect(child_scale.GetColumn4(2), zero, UVec4(0, 0, 0xffffffff, 0)).Abs();
+
+		// Check if all elements are less than epsilon
+		Vec4 epsilon = Vec4::sReplicate(1.0e-6f);
+		return UVec4::sAnd(UVec4::sAnd(Vec4::sLess(c0, epsilon), Vec4::sLess(c1, epsilon)), Vec4::sLess(c2, epsilon)).TestAllTrue();
+	}
+
+	/// Adjust scale for rotated child shape
+	/// @param inRotation Rotation of child shape
+	/// @param inScale Scale in local space of parent shape
+	/// @return Rotated scale
+	inline Vec3				RotateScale(QuatArg inRotation, Vec3Arg inScale)
+	{
+		// Get the diagonal of mRotation^T * Mat44::sScale(inScale) * mRotation (see comment at CanScaleBeRotated)
+		Mat44 r = Mat44::sRotation(inRotation);
+		return r.Multiply3x3LeftTransposed(r.PostScaled(inScale)).GetDiagonal3();
+	}
+}
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ScaledShape.cpp

@@ -0,0 +1,238 @@
+// 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/Shape/ScaledShape.h>
+#include <Jolt/Physics/Collision/Shape/ScaleHelpers.h>
+#include <Jolt/Physics/Collision/RayCast.h>
+#include <Jolt/Physics/Collision/ShapeCast.h>
+#include <Jolt/Physics/Collision/TransformedShape.h>
+#include <Jolt/Physics/Collision/CollisionDispatch.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(ScaledShapeSettings)
+{
+	JPH_ADD_BASE_CLASS(ScaledShapeSettings, DecoratedShapeSettings)
+
+	JPH_ADD_ATTRIBUTE(ScaledShapeSettings, mScale)
+}
+
+ShapeSettings::ShapeResult ScaledShapeSettings::Create() const
+{
+	if (mCachedResult.IsEmpty())
+		Ref<Shape> shape = new ScaledShape(*this, mCachedResult);
+	return mCachedResult;
+}
+
+ScaledShape::ScaledShape(const ScaledShapeSettings &inSettings, ShapeResult &outResult) :
+	DecoratedShape(EShapeSubType::Scaled, inSettings, outResult),
+	mScale(inSettings.mScale)
+{
+	if (outResult.HasError())
+		return;
+
+	if (ScaleHelpers::IsZeroScale(inSettings.mScale))
+	{
+		outResult.SetError("Can't use zero scale!");
+		return;
+	}
+
+	outResult.Set(this);
+}
+
+MassProperties ScaledShape::GetMassProperties() const
+{
+	MassProperties p = mInnerShape->GetMassProperties();
+	p.Scale(mScale);
+	return p;
+}
+
+AABox ScaledShape::GetLocalBounds() const
+{
+	return mInnerShape->GetLocalBounds().Scaled(mScale);
+}
+
+AABox ScaledShape::GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const
+{
+	return mInnerShape->GetWorldSpaceBounds(inCenterOfMassTransform, inScale * mScale);
+}
+
+TransformedShape ScaledShape::GetSubShapeTransformedShape(const SubShapeID &inSubShapeID, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, SubShapeID &outRemainder) const
+{
+	// We don't use any bits in the sub shape ID
+	outRemainder = inSubShapeID;
+
+	TransformedShape ts(RVec3(inPositionCOM), inRotation, mInnerShape, BodyID());
+	ts.SetShapeScale(inScale * mScale);
+	return ts;
+}
+
+Vec3 ScaledShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const
+{
+	// Transform the surface point to local space and pass the query on
+	Vec3 normal = mInnerShape->GetSurfaceNormal(inSubShapeID, inLocalSurfacePosition / mScale);
+
+	// Need to transform the plane normals using inScale
+	// Transforming a direction with matrix M is done through multiplying by (M^-1)^T
+	// In this case M is a diagonal matrix with the scale vector, so we need to multiply our normal by 1 / scale and renormalize afterwards
+	return (normal / mScale).Normalized();
+}
+
+void ScaledShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const
+{
+	mInnerShape->GetSupportingFace(inSubShapeID, inDirection, inScale * mScale, inCenterOfMassTransform, outVertices);
+}
+
+void ScaledShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const
+{
+	mInnerShape->GetSubmergedVolume(inCenterOfMassTransform, inScale * mScale, inSurface, outTotalVolume, outSubmergedVolume, outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, inBaseOffset));
+}
+
+#ifdef JPH_DEBUG_RENDERER
+void ScaledShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
+{
+	mInnerShape->Draw(inRenderer, inCenterOfMassTransform, inScale * mScale, inColor, inUseMaterialColors, inDrawWireframe);
+}
+
+void ScaledShape::DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const
+{
+	mInnerShape->DrawGetSupportFunction(inRenderer, inCenterOfMassTransform, inScale * mScale, inColor, inDrawSupportDirection);
+}
+
+void ScaledShape::DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const
+{
+	mInnerShape->DrawGetSupportingFace(inRenderer, inCenterOfMassTransform, inScale * mScale);
+}
+#endif // JPH_DEBUG_RENDERER
+
+bool ScaledShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const
+{
+	Vec3 inv_scale = mScale.Reciprocal();
+	RayCast scaled_ray { inv_scale * inRay.mOrigin, inv_scale * inRay.mDirection };
+	return mInnerShape->CastRay(scaled_ray, inSubShapeIDCreator, ioHit);
+}
+
+void ScaledShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	// Test shape filter
+	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
+		return;
+
+	Vec3 inv_scale = mScale.Reciprocal();
+	RayCast scaled_ray { inv_scale * inRay.mOrigin, inv_scale * inRay.mDirection };
+	return mInnerShape->CastRay(scaled_ray, inRayCastSettings, inSubShapeIDCreator, ioCollector, inShapeFilter);
+}
+
+void ScaledShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	// Test shape filter
+	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
+		return;
+
+	Vec3 inv_scale = mScale.Reciprocal();
+	mInnerShape->CollidePoint(inv_scale * inPoint, inSubShapeIDCreator, ioCollector, inShapeFilter);
+}
+
+void ScaledShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const
+{
+	mInnerShape->CollideSoftBodyVertices(inCenterOfMassTransform, inScale * mScale, inVertices, inNumVertices, inCollidingShapeIndex);
+}
+
+void ScaledShape::CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	// Test shape filter
+	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
+		return;
+
+	mInnerShape->CollectTransformedShapes(inBox, inPositionCOM, inRotation, inScale * mScale, inSubShapeIDCreator, ioCollector, inShapeFilter);
+}
+
+void ScaledShape::TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const
+{
+	mInnerShape->TransformShape(inCenterOfMassTransform * Mat44::sScale(mScale), ioCollector);
+}
+
+void ScaledShape::SaveBinaryState(StreamOut &inStream) const
+{
+	DecoratedShape::SaveBinaryState(inStream);
+
+	inStream.Write(mScale);
+}
+
+void ScaledShape::RestoreBinaryState(StreamIn &inStream)
+{
+	DecoratedShape::RestoreBinaryState(inStream);
+
+	inStream.Read(mScale);
+}
+
+float ScaledShape::GetVolume() const
+{
+	return abs(mScale.GetX() * mScale.GetY() * mScale.GetZ()) * mInnerShape->GetVolume();
+}
+
+bool ScaledShape::IsValidScale(Vec3Arg inScale) const
+{
+	return mInnerShape->IsValidScale(inScale * mScale);
+}
+
+Vec3 ScaledShape::MakeScaleValid(Vec3Arg inScale) const
+{
+	return mInnerShape->MakeScaleValid(mScale * inScale) / mScale;
+}
+
+void ScaledShape::sCollideScaledVsShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter)
+{
+	JPH_ASSERT(inShape1->GetSubType() == EShapeSubType::Scaled);
+	const ScaledShape *shape1 = static_cast<const ScaledShape *>(inShape1);
+
+	CollisionDispatch::sCollideShapeVsShape(shape1->GetInnerShape(), inShape2, inScale1 * shape1->GetScale(), inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter);
+}
+
+void ScaledShape::sCollideShapeVsScaled(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter)
+{
+	JPH_ASSERT(inShape2->GetSubType() == EShapeSubType::Scaled);
+	const ScaledShape *shape2 = static_cast<const ScaledShape *>(inShape2);
+
+	CollisionDispatch::sCollideShapeVsShape(inShape1, shape2->GetInnerShape(), inScale1, inScale2 * shape2->GetScale(), inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter);
+}
+
+void ScaledShape::sCastScaledVsShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
+{
+	JPH_ASSERT(inShapeCast.mShape->GetSubType() == EShapeSubType::Scaled);
+	const ScaledShape *shape = static_cast<const ScaledShape *>(inShapeCast.mShape);
+
+	ShapeCast scaled_cast(shape->GetInnerShape(), inShapeCast.mScale * shape->GetScale(), inShapeCast.mCenterOfMassStart, inShapeCast.mDirection);
+	CollisionDispatch::sCastShapeVsShapeLocalSpace(scaled_cast, inShapeCastSettings, inShape, inScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector);
+}
+
+void ScaledShape::sCastShapeVsScaled(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
+{
+	JPH_ASSERT(inShape->GetSubType() == EShapeSubType::Scaled);
+	const ScaledShape *shape = static_cast<const ScaledShape *>(inShape);
+
+	CollisionDispatch::sCastShapeVsShapeLocalSpace(inShapeCast, inShapeCastSettings, shape->mInnerShape, inScale * shape->mScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector);
+}
+
+void ScaledShape::sRegister()
+{
+	ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::Scaled);
+	f.mConstruct = []() -> Shape * { return new ScaledShape; };
+	f.mColor = Color::sYellow;
+
+	for (EShapeSubType s : sAllSubShapeTypes)
+	{
+		CollisionDispatch::sRegisterCollideShape(EShapeSubType::Scaled, s, sCollideScaledVsShape);
+		CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::Scaled, sCollideShapeVsScaled);
+		CollisionDispatch::sRegisterCastShape(EShapeSubType::Scaled, s, sCastScaledVsShape);
+		CollisionDispatch::sRegisterCastShape(s, EShapeSubType::Scaled, sCastShapeVsScaled);
+	}
+}
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ScaledShape.h

@@ -0,0 +1,145 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/Shape/DecoratedShape.h>
+#include <Jolt/Physics/Collision/Shape/ScaleHelpers.h>
+
+JPH_NAMESPACE_BEGIN
+
+class SubShapeIDCreator;
+class CollideShapeSettings;
+
+/// Class that constructs a ScaledShape
+class JPH_EXPORT ScaledShapeSettings final : public DecoratedShapeSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, ScaledShapeSettings)
+
+public:
+	/// Default constructor for deserialization
+									ScaledShapeSettings() = default;
+
+	/// Constructor that decorates another shape with a scale
+									ScaledShapeSettings(const ShapeSettings *inShape, Vec3Arg inScale) : DecoratedShapeSettings(inShape), mScale(inScale) { }
+
+	/// Variant that uses a concrete shape, which means this object cannot be serialized.
+									ScaledShapeSettings(const Shape *inShape, Vec3Arg inScale) : DecoratedShapeSettings(inShape), mScale(inScale) { }
+
+	// See: ShapeSettings
+	virtual ShapeResult				Create() const override;
+
+	Vec3							mScale = Vec3(1, 1, 1);
+};
+
+/// A shape that scales a child shape in local space of that shape. The scale can be non-uniform and can even turn it inside out when one or three components of the scale are negative.
+class JPH_EXPORT ScaledShape final : public DecoratedShape
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+									ScaledShape() : DecoratedShape(EShapeSubType::Scaled) { }
+									ScaledShape(const ScaledShapeSettings &inSettings, ShapeResult &outResult);
+
+	/// Constructor that decorates another shape with a scale
+									ScaledShape(const Shape *inShape, Vec3Arg inScale)		: DecoratedShape(EShapeSubType::Scaled, inShape), mScale(inScale) { JPH_ASSERT(!ScaleHelpers::IsZeroScale(mScale)); }
+
+	/// Get the scale
+	Vec3							GetScale() const										{ return mScale; }
+
+	// See Shape::GetCenterOfMass
+	virtual Vec3					GetCenterOfMass() const override						{ return mScale * mInnerShape->GetCenterOfMass(); }
+
+	// See Shape::GetLocalBounds
+	virtual AABox					GetLocalBounds() const override;
+
+	// See Shape::GetWorldSpaceBounds
+	virtual AABox					GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override;
+	using Shape::GetWorldSpaceBounds;
+
+	// See Shape::GetInnerRadius
+	virtual float					GetInnerRadius() const override							{ return mScale.ReduceMin() * mInnerShape->GetInnerRadius(); }
+
+	// See Shape::GetMassProperties
+	virtual MassProperties			GetMassProperties() const override;
+
+	// See Shape::GetSubShapeTransformedShape
+	virtual TransformedShape		GetSubShapeTransformedShape(const SubShapeID &inSubShapeID, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, SubShapeID &outRemainder) const override;
+
+	// See Shape::GetSurfaceNormal
+	virtual Vec3					GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override;
+
+	// See Shape::GetSupportingFace
+	virtual void					GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override;
+
+	// See Shape::GetSubmergedVolume
+	virtual void					GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const override;
+
+#ifdef JPH_DEBUG_RENDERER
+	// See Shape::Draw
+	virtual void					Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override;
+
+	// See Shape::DrawGetSupportFunction
+	virtual void					DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const override;
+
+	// See Shape::DrawGetSupportingFace
+	virtual void					DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override;
+#endif // JPH_DEBUG_RENDERER
+
+	// See Shape::CastRay
+	virtual bool					CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override;
+	virtual void					CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
+
+	// See: Shape::CollidePoint
+	virtual void					CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
+
+	// See: Shape::CollideSoftBodyVertices
+	virtual void					CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override;
+
+	// See Shape::CollectTransformedShapes
+	virtual void					CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const override;
+
+	// See Shape::TransformShape
+	virtual void					TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const override;
+
+	// See Shape::GetTrianglesStart
+	virtual void					GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override { JPH_ASSERT(false, "Cannot call on non-leaf shapes, use CollectTransformedShapes to collect the leaves first!"); }
+
+	// See Shape::GetTrianglesNext
+	virtual int						GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override { JPH_ASSERT(false, "Cannot call on non-leaf shapes, use CollectTransformedShapes to collect the leaves first!"); return 0; }
+
+	// See Shape
+	virtual void					SaveBinaryState(StreamOut &inStream) const override;
+
+	// See Shape::GetStats
+	virtual Stats					GetStats() const override								{ return Stats(sizeof(*this), 0); }
+
+	// See Shape::GetVolume
+	virtual float					GetVolume() const override;
+
+	// See Shape::IsValidScale
+	virtual bool					IsValidScale(Vec3Arg inScale) const override;
+
+	// See Shape::MakeScaleValid
+	virtual Vec3					MakeScaleValid(Vec3Arg inScale) const override;
+
+	// Register shape functions with the registry
+	static void						sRegister();
+
+protected:
+	// See: Shape::RestoreBinaryState
+	virtual void					RestoreBinaryState(StreamIn &inStream) override;
+
+private:
+	// Helper functions called by CollisionDispatch
+	static void						sCollideScaledVsShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter);
+	static void						sCollideShapeVsScaled(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter);
+	static void						sCastScaledVsShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector);
+	static void						sCastShapeVsScaled(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector);
+
+	Vec3							mScale = Vec3(1, 1, 1);
+};
+
+JPH_NAMESPACE_END

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

@@ -0,0 +1,325 @@
+// 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/Shape/Shape.h>
+#include <Jolt/Physics/Collision/Shape/ScaledShape.h>
+#include <Jolt/Physics/Collision/Shape/StaticCompoundShape.h>
+#include <Jolt/Physics/Collision/TransformedShape.h>
+#include <Jolt/Physics/Collision/PhysicsMaterial.h>
+#include <Jolt/Physics/Collision/RayCast.h>
+#include <Jolt/Physics/Collision/CastResult.h>
+#include <Jolt/Physics/Collision/CollidePointResult.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+#include <Jolt/Core/Factory.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT_BASE(ShapeSettings)
+{
+	JPH_ADD_BASE_CLASS(ShapeSettings, SerializableObject)
+
+	JPH_ADD_ATTRIBUTE(ShapeSettings, mUserData)
+}
+
+#ifdef JPH_DEBUG_RENDERER
+bool Shape::sDrawSubmergedVolumes = false;
+#endif // JPH_DEBUG_RENDERER
+
+ShapeFunctions ShapeFunctions::sRegistry[NumSubShapeTypes];
+
+const Shape *Shape::GetLeafShape([[maybe_unused]] const SubShapeID &inSubShapeID, SubShapeID &outRemainder) const
+{
+	outRemainder = inSubShapeID;
+	return this;
+}
+
+TransformedShape Shape::GetSubShapeTransformedShape(const SubShapeID &inSubShapeID, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, SubShapeID &outRemainder) const
+{
+	// We have reached the leaf shape so there is no remainder
+	outRemainder = SubShapeID();
+
+	// Just return the transformed shape for this shape
+	TransformedShape ts(RVec3(inPositionCOM), inRotation, this, BodyID());
+	ts.SetShapeScale(inScale);
+	return ts;
+}
+
+void Shape::CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	// Test shape filter
+	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
+		return;
+
+	TransformedShape ts(RVec3(inPositionCOM), inRotation, this, TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator);
+	ts.SetShapeScale(inScale);
+	ioCollector.AddHit(ts);
+}
+
+void Shape::TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const
+{
+	Vec3 scale;
+	Mat44 transform = inCenterOfMassTransform.Decompose(scale);
+	TransformedShape ts(RVec3(transform.GetTranslation()), transform.GetQuaternion(), this, BodyID(), SubShapeIDCreator());
+	ts.SetShapeScale(MakeScaleValid(scale));
+	ioCollector.AddHit(ts);
+}
+
+void Shape::SaveBinaryState(StreamOut &inStream) const
+{
+	inStream.Write(mShapeSubType);
+	inStream.Write(mUserData);
+}
+
+void Shape::RestoreBinaryState(StreamIn &inStream)
+{
+	// Type hash read by sRestoreFromBinaryState
+	inStream.Read(mUserData);
+}
+
+Shape::ShapeResult Shape::sRestoreFromBinaryState(StreamIn &inStream)
+{
+	ShapeResult result;
+
+	// Read the type of the shape
+	EShapeSubType shape_sub_type;
+	inStream.Read(shape_sub_type);
+	if (inStream.IsEOF() || inStream.IsFailed())
+	{
+		result.SetError("Failed to read type id");
+		return result;
+	}
+
+	// Construct and read the data of the shape
+	Ref<Shape> shape = ShapeFunctions::sGet(shape_sub_type).mConstruct();
+	shape->RestoreBinaryState(inStream);
+	if (inStream.IsEOF() || inStream.IsFailed())
+	{
+		result.SetError("Failed to restore shape");
+		return result;
+	}
+
+	result.Set(shape);
+	return result;
+}
+
+void Shape::SaveWithChildren(StreamOut &inStream, ShapeToIDMap &ioShapeMap, MaterialToIDMap &ioMaterialMap) const
+{
+	ShapeToIDMap::const_iterator shape_id_iter = ioShapeMap.find(this);
+	if (shape_id_iter == ioShapeMap.end())
+	{
+		// Write shape ID of this shape
+		uint32 shape_id = ioShapeMap.size();
+		ioShapeMap[this] = shape_id;
+		inStream.Write(shape_id);
+
+		// Write the shape itself
+		SaveBinaryState(inStream);
+
+		// Write the ID's of all sub shapes
+		ShapeList sub_shapes;
+		SaveSubShapeState(sub_shapes);
+		inStream.Write(uint32(sub_shapes.size()));
+		for (const Shape *shape : sub_shapes)
+		{
+			if (shape == nullptr)
+				inStream.Write(~uint32(0));
+			else
+				shape->SaveWithChildren(inStream, ioShapeMap, ioMaterialMap);
+		}
+
+		// Write the materials
+		PhysicsMaterialList materials;
+		SaveMaterialState(materials);
+		StreamUtils::SaveObjectArray(inStream, materials, &ioMaterialMap);
+	}
+	else
+	{
+		// Known shape, just write the ID
+		inStream.Write(shape_id_iter->second);
+	}
+}
+
+Shape::ShapeResult Shape::sRestoreWithChildren(StreamIn &inStream, IDToShapeMap &ioShapeMap, IDToMaterialMap &ioMaterialMap)
+{
+	ShapeResult result;
+
+	// Read ID of this shape
+	uint32 shape_id;
+	inStream.Read(shape_id);
+	if (inStream.IsEOF() || inStream.IsFailed())
+	{
+		result.SetError("Failed to read shape id");
+		return result;
+	}
+
+	// Check nullptr shape
+	if (shape_id == ~uint32(0))
+	{
+		result.Set(nullptr);
+		return result;
+	}
+
+	// Check if we already read this shape
+	if (shape_id < ioShapeMap.size())
+	{
+		result.Set(ioShapeMap[shape_id]);
+		return result;
+	}
+
+	// Read the shape
+	result = sRestoreFromBinaryState(inStream);
+	if (result.HasError())
+		return result;
+	JPH_ASSERT(ioShapeMap.size() == shape_id); // Assert that this is the next ID in the map
+	ioShapeMap.push_back(result.Get());
+
+	// Read the sub shapes
+	uint32 len;
+	inStream.Read(len);
+	if (inStream.IsEOF() || inStream.IsFailed())
+	{
+		result.SetError("Failed to read stream");
+		return result;
+	}
+	ShapeList sub_shapes;
+	sub_shapes.reserve(len);
+	for (size_t i = 0; i < len; ++i)
+	{
+		ShapeResult sub_shape_result = sRestoreWithChildren(inStream, ioShapeMap, ioMaterialMap);
+		if (sub_shape_result.HasError())
+			return sub_shape_result;
+		sub_shapes.push_back(sub_shape_result.Get());
+	}
+	result.Get()->RestoreSubShapeState(sub_shapes.data(), (uint)sub_shapes.size());
+
+	// Read the materials
+	Result mlresult = StreamUtils::RestoreObjectArray<PhysicsMaterialList>(inStream, ioMaterialMap);
+	if (mlresult.HasError())
+	{
+		result.SetError(mlresult.GetError());
+		return result;
+	}
+	const PhysicsMaterialList &materials = mlresult.Get();
+	result.Get()->RestoreMaterialState(materials.data(), (uint)materials.size());
+
+	return result;
+}
+
+Shape::Stats Shape::GetStatsRecursive(VisitedShapes &ioVisitedShapes) const
+{
+	Stats stats = GetStats();
+
+	// If shape is already visited, don't count its size again
+	if (!ioVisitedShapes.insert(this).second)
+		stats.mSizeBytes = 0;
+
+	return stats;
+}
+
+bool Shape::IsValidScale(Vec3Arg inScale) const
+{
+	return !ScaleHelpers::IsZeroScale(inScale);
+}
+
+Vec3 Shape::MakeScaleValid(Vec3Arg inScale) const
+{
+	return ScaleHelpers::MakeNonZeroScale(inScale);
+}
+
+Shape::ShapeResult Shape::ScaleShape(Vec3Arg inScale) const
+{
+	const Vec3 unit_scale = Vec3::sOne();
+
+	if (inScale.IsNearZero())
+	{
+		ShapeResult result;
+		result.SetError("Can't use zero scale!");
+		return result;
+	}
+
+	// First test if we can just wrap this shape in a scaled shape
+	if (IsValidScale(inScale))
+	{
+		// Test if the scale is near unit
+		ShapeResult result;
+		if (inScale.IsClose(unit_scale))
+			result.Set(const_cast<Shape *>(this));
+		else
+			result.Set(new ScaledShape(this, inScale));
+		return result;
+	}
+
+	// Collect the leaf shapes and their transforms
+	struct Collector : TransformedShapeCollector
+	{
+		virtual void				AddHit(const ResultType &inResult) override
+		{
+			mShapes.push_back(inResult);
+		}
+
+		Array<TransformedShape>		mShapes;
+	};
+	Collector collector;
+	TransformShape(Mat44::sScale(inScale) * Mat44::sTranslation(GetCenterOfMass()), collector);
+
+	// Construct a compound shape
+	StaticCompoundShapeSettings compound;
+	compound.mSubShapes.reserve(collector.mShapes.size());
+	for (const TransformedShape &ts : collector.mShapes)
+	{
+		const Shape *shape = ts.mShape;
+
+		// Construct a scaled shape if scale is not unit
+		Vec3 scale = ts.GetShapeScale();
+		if (!scale.IsClose(unit_scale))
+			shape = new ScaledShape(shape, scale);
+
+		// Add the shape
+		compound.AddShape(Vec3(ts.mShapePositionCOM) - ts.mShapeRotation * shape->GetCenterOfMass(), ts.mShapeRotation, shape);
+	}
+
+	return compound.Create();
+}
+
+void Shape::sCollidePointUsingRayCast(const Shape &inShape, Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter)
+{
+	// First test if we're inside our bounding box
+	AABox bounds = inShape.GetLocalBounds();
+	if (bounds.Contains(inPoint))
+	{
+		// A collector that just counts the number of hits
+		class HitCountCollector : public CastRayCollector
+		{
+		public:
+			virtual void	AddHit(const RayCastResult &inResult) override
+			{
+				// Store the last sub shape ID so that we can provide something to our outer hit collector
+				mSubShapeID = inResult.mSubShapeID2;
+
+				++mHitCount;
+			}
+
+			int				mHitCount = 0;
+			SubShapeID		mSubShapeID;
+		};
+		HitCountCollector collector;
+
+		// Configure the raycast
+		RayCastSettings settings;
+		settings.SetBackFaceMode(EBackFaceMode::CollideWithBackFaces);
+
+		// Cast a ray that's 10% longer than the height of our bounding box
+		inShape.CastRay(RayCast { inPoint, 1.1f * bounds.GetSize().GetY() * Vec3::sAxisY() }, settings, inSubShapeIDCreator, collector, inShapeFilter);
+
+		// Odd amount of hits means inside
+		if ((collector.mHitCount & 1) == 1)
+			ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), collector.mSubShapeID });
+	}
+}
+
+JPH_NAMESPACE_END

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

@@ -0,0 +1,466 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Body/MassProperties.h>
+#include <Jolt/Physics/Collision/BackFaceMode.h>
+#include <Jolt/Physics/Collision/CollisionCollector.h>
+#include <Jolt/Physics/Collision/ShapeFilter.h>
+#include <Jolt/Geometry/AABox.h>
+#include <Jolt/Core/Reference.h>
+#include <Jolt/Core/Color.h>
+#include <Jolt/Core/Result.h>
+#include <Jolt/Core/NonCopyable.h>
+#include <Jolt/Core/UnorderedMap.h>
+#include <Jolt/Core/UnorderedSet.h>
+#include <Jolt/Core/StreamUtils.h>
+#include <Jolt/ObjectStream/SerializableObject.h>
+
+JPH_NAMESPACE_BEGIN
+
+struct RayCast;
+class RayCastSettings;
+struct ShapeCast;
+class ShapeCastSettings;
+class RayCastResult;
+class ShapeCastResult;
+class CollidePointResult;
+class CollideShapeResult;
+class SubShapeIDCreator;
+class SubShapeID;
+class PhysicsMaterial;
+class TransformedShape;
+class Plane;
+class CollideSoftBodyVertexIterator;
+class Shape;
+class StreamOut;
+class StreamIn;
+#ifdef JPH_DEBUG_RENDERER
+class DebugRenderer;
+#endif // JPH_DEBUG_RENDERER
+
+using CastRayCollector = CollisionCollector<RayCastResult, CollisionCollectorTraitsCastRay>;
+using CastShapeCollector = CollisionCollector<ShapeCastResult, CollisionCollectorTraitsCastShape>;
+using CollidePointCollector = CollisionCollector<CollidePointResult, CollisionCollectorTraitsCollidePoint>;
+using CollideShapeCollector = CollisionCollector<CollideShapeResult, CollisionCollectorTraitsCollideShape>;
+using TransformedShapeCollector = CollisionCollector<TransformedShape, CollisionCollectorTraitsCollideShape>;
+
+using ShapeRefC = RefConst<Shape>;
+using ShapeList = Array<ShapeRefC>;
+using PhysicsMaterialRefC = RefConst<PhysicsMaterial>;
+using PhysicsMaterialList = Array<PhysicsMaterialRefC>;
+
+/// Shapes are categorized in groups, each shape can return which group it belongs to through its Shape::GetType function.
+enum class EShapeType : uint8
+{
+	Convex,							///< Used by ConvexShape, all shapes that use the generic convex vs convex collision detection system (box, sphere, capsule, tapered capsule, cylinder, triangle)
+	Compound,						///< Used by CompoundShape
+	Decorated,						///< Used by DecoratedShape
+	Mesh,							///< Used by MeshShape
+	HeightField,					///< Used by HeightFieldShape
+	SoftBody,						///< Used by SoftBodyShape
+
+	// User defined shapes
+	User1,
+	User2,
+	User3,
+	User4,
+
+	Plane,							///< Used by PlaneShape
+	Empty,							///< Used by EmptyShape
+};
+
+/// This enumerates all shape types, each shape can return its type through Shape::GetSubType
+enum class EShapeSubType : uint8
+{
+	// Convex shapes
+	Sphere,
+	Box,
+	Triangle,
+	Capsule,
+	TaperedCapsule,
+	Cylinder,
+	ConvexHull,
+
+	// Compound shapes
+	StaticCompound,
+	MutableCompound,
+
+	// Decorated shapes
+	RotatedTranslated,
+	Scaled,
+	OffsetCenterOfMass,
+
+	// Other shapes
+	Mesh,
+	HeightField,
+	SoftBody,
+
+	// User defined shapes
+	User1,
+	User2,
+	User3,
+	User4,
+	User5,
+	User6,
+	User7,
+	User8,
+
+	// User defined convex shapes
+	UserConvex1,
+	UserConvex2,
+	UserConvex3,
+	UserConvex4,
+	UserConvex5,
+	UserConvex6,
+	UserConvex7,
+	UserConvex8,
+
+	// Other shapes
+	Plane,
+	TaperedCylinder,
+	Empty,
+};
+
+// Sets of shape sub types
+static constexpr EShapeSubType sAllSubShapeTypes[] = { EShapeSubType::Sphere, EShapeSubType::Box, EShapeSubType::Triangle, EShapeSubType::Capsule, EShapeSubType::TaperedCapsule, EShapeSubType::Cylinder, EShapeSubType::ConvexHull, EShapeSubType::StaticCompound, EShapeSubType::MutableCompound, EShapeSubType::RotatedTranslated, EShapeSubType::Scaled, EShapeSubType::OffsetCenterOfMass, EShapeSubType::Mesh, EShapeSubType::HeightField, EShapeSubType::SoftBody, EShapeSubType::User1, EShapeSubType::User2, EShapeSubType::User3, EShapeSubType::User4, EShapeSubType::User5, EShapeSubType::User6, EShapeSubType::User7, EShapeSubType::User8, EShapeSubType::UserConvex1, EShapeSubType::UserConvex2, EShapeSubType::UserConvex3, EShapeSubType::UserConvex4, EShapeSubType::UserConvex5, EShapeSubType::UserConvex6, EShapeSubType::UserConvex7, EShapeSubType::UserConvex8, EShapeSubType::Plane, EShapeSubType::TaperedCylinder, EShapeSubType::Empty };
+static constexpr EShapeSubType sConvexSubShapeTypes[] = { EShapeSubType::Sphere, EShapeSubType::Box, EShapeSubType::Triangle, EShapeSubType::Capsule, EShapeSubType::TaperedCapsule, EShapeSubType::Cylinder, EShapeSubType::ConvexHull, EShapeSubType::TaperedCylinder, EShapeSubType::UserConvex1, EShapeSubType::UserConvex2, EShapeSubType::UserConvex3, EShapeSubType::UserConvex4, EShapeSubType::UserConvex5, EShapeSubType::UserConvex6, EShapeSubType::UserConvex7, EShapeSubType::UserConvex8 };
+static constexpr EShapeSubType sCompoundSubShapeTypes[] = { EShapeSubType::StaticCompound, EShapeSubType::MutableCompound };
+static constexpr EShapeSubType sDecoratorSubShapeTypes[] = { EShapeSubType::RotatedTranslated, EShapeSubType::Scaled, EShapeSubType::OffsetCenterOfMass };
+
+/// How many shape types we support
+static constexpr uint NumSubShapeTypes = uint(std::size(sAllSubShapeTypes));
+
+/// Names of sub shape types
+static constexpr const char *sSubShapeTypeNames[] = { "Sphere", "Box", "Triangle", "Capsule", "TaperedCapsule", "Cylinder", "ConvexHull", "StaticCompound", "MutableCompound", "RotatedTranslated", "Scaled", "OffsetCenterOfMass", "Mesh", "HeightField", "SoftBody", "User1", "User2", "User3", "User4", "User5", "User6", "User7", "User8", "UserConvex1", "UserConvex2", "UserConvex3", "UserConvex4", "UserConvex5", "UserConvex6", "UserConvex7", "UserConvex8", "Plane", "TaperedCylinder", "Empty" };
+static_assert(std::size(sSubShapeTypeNames) == NumSubShapeTypes);
+
+/// Class that can construct shapes and that is serializable using the ObjectStream system.
+/// Can be used to store shape data in 'uncooked' form (i.e. in a form that is still human readable and authorable).
+/// Once the shape has been created using the Create() function, the data will be moved into the Shape class
+/// in a form that is optimized for collision detection. After this, the ShapeSettings object is no longer needed
+/// and can be destroyed. Each shape class has a derived class of the ShapeSettings object to store shape specific
+/// data.
+class JPH_EXPORT ShapeSettings : public SerializableObject, public RefTarget<ShapeSettings>
+{
+	JPH_DECLARE_SERIALIZABLE_ABSTRACT(JPH_EXPORT, ShapeSettings)
+
+public:
+	using ShapeResult = Result<Ref<Shape>>;
+
+	/// Create a shape according to the settings specified by this object.
+	virtual ShapeResult				Create() const = 0;
+
+	/// When creating a shape, the result is cached so that calling Create() again will return the same shape.
+	/// If you make changes to the ShapeSettings you need to call this function to clear the cached result to allow Create() to build a new shape.
+	void							ClearCachedResult()													{ mCachedResult.Clear(); }
+
+	/// User data (to be used freely by the application)
+	uint64							mUserData = 0;
+
+protected:
+	mutable ShapeResult				mCachedResult;
+};
+
+/// Function table for functions on shapes
+class JPH_EXPORT ShapeFunctions
+{
+public:
+	/// Construct a shape
+	Shape *							(*mConstruct)() = nullptr;
+
+	/// Color of the shape when drawing
+	Color							mColor = Color::sBlack;
+
+	/// Get an entry in the registry for a particular sub type
+	static inline ShapeFunctions &	sGet(EShapeSubType inSubType)										{ return sRegistry[int(inSubType)]; }
+
+private:
+	static ShapeFunctions			sRegistry[NumSubShapeTypes];
+};
+
+/// Base class for all shapes (collision volume of a body). Defines a virtual interface for collision detection.
+class JPH_EXPORT Shape : public RefTarget<Shape>, public NonCopyable
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	using ShapeResult = ShapeSettings::ShapeResult;
+
+	/// Constructor
+									Shape(EShapeType inType, EShapeSubType inSubType) : mShapeType(inType), mShapeSubType(inSubType) { }
+									Shape(EShapeType inType, EShapeSubType inSubType, const ShapeSettings &inSettings, [[maybe_unused]] ShapeResult &outResult) : mUserData(inSettings.mUserData), mShapeType(inType), mShapeSubType(inSubType) { }
+
+	/// Destructor
+	virtual							~Shape() = default;
+
+	/// Get type
+	inline EShapeType				GetType() const														{ return mShapeType; }
+	inline EShapeSubType			GetSubType() const													{ return mShapeSubType; }
+
+	/// User data (to be used freely by the application)
+	uint64							GetUserData() const													{ return mUserData; }
+	void							SetUserData(uint64 inUserData)										{ mUserData = inUserData; }
+
+	/// Check if this shape can only be used to create a static body or if it can also be dynamic/kinematic
+	virtual bool					MustBeStatic() const												{ return false; }
+
+	/// All shapes are centered around their center of mass. This function returns the center of mass position that needs to be applied to transform the shape to where it was created.
+	virtual Vec3					GetCenterOfMass() const												{ return Vec3::sZero(); }
+
+	/// Get local bounding box including convex radius, this box is centered around the center of mass rather than the world transform
+	virtual AABox					GetLocalBounds() const = 0;
+
+	/// Get the max number of sub shape ID bits that are needed to be able to address any leaf shape in this shape. Used mainly for checking that it is smaller or equal than SubShapeID::MaxBits.
+	virtual uint					GetSubShapeIDBitsRecursive() const = 0;
+
+	/// Get world space bounds including convex radius.
+	/// This shape is scaled by inScale in local space first.
+	/// This function can be overridden to return a closer fitting world space bounding box, by default it will just transform what GetLocalBounds() returns.
+	virtual AABox					GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const { return GetLocalBounds().Scaled(inScale).Transformed(inCenterOfMassTransform); }
+
+	/// Get world space bounds including convex radius.
+	AABox							GetWorldSpaceBounds(DMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const
+	{
+		// Use single precision version using the rotation only
+		AABox bounds = GetWorldSpaceBounds(inCenterOfMassTransform.GetRotation(), inScale);
+
+		// Apply translation
+		bounds.Translate(inCenterOfMassTransform.GetTranslation());
+
+		return bounds;
+	}
+
+	/// Returns the radius of the biggest sphere that fits entirely in the shape. In case this shape consists of multiple sub shapes, it returns the smallest sphere of the parts.
+	/// This can be used as a measure of how far the shape can be moved without risking going through geometry.
+	virtual float					GetInnerRadius() const = 0;
+
+	/// Calculate the mass and inertia of this shape
+	virtual MassProperties			GetMassProperties() const = 0;
+
+	/// Get the leaf shape for a particular sub shape ID.
+	/// @param inSubShapeID The full sub shape ID that indicates the path to the leaf shape
+	/// @param outRemainder What remains of the sub shape ID after removing the path to the leaf shape (could e.g. refer to a triangle within a MeshShape)
+	/// @return The shape or null if the sub shape ID is invalid
+	virtual const Shape *			GetLeafShape([[maybe_unused]] const SubShapeID &inSubShapeID, SubShapeID &outRemainder) const;
+
+	/// Get the material assigned to a particular sub shape ID
+	virtual const PhysicsMaterial *	GetMaterial(const SubShapeID &inSubShapeID) const = 0;
+
+	/// Get the surface normal of a particular sub shape ID and point on surface (all vectors are relative to center of mass for this shape).
+	/// Note: When you have a CollideShapeResult or ShapeCastResult you should use -mPenetrationAxis.Normalized() as contact normal as GetSurfaceNormal will only return face normals (and not vertex or edge normals).
+	virtual Vec3					GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const = 0;
+
+	/// Type definition for a supporting face
+	using SupportingFace = StaticArray<Vec3, 32>;
+
+	/// Get the vertices of the face that faces inDirection the most (includes any convex radius). Note that this function can only return faces of
+	/// convex shapes or triangles, which is why a sub shape ID to get to that leaf must be provided.
+	/// @param inSubShapeID Sub shape ID of target shape
+	/// @param inDirection Direction that the face should be facing (in local space to this shape)
+	/// @param inCenterOfMassTransform Transform to transform outVertices with
+	/// @param inScale Scale in local space of the shape (scales relative to its center of mass)
+	/// @param outVertices Resulting face. The returned face can be empty if the shape doesn't have polygons to return (e.g. because it's a sphere). The face will be returned in world space.
+	virtual void					GetSupportingFace([[maybe_unused]] const SubShapeID &inSubShapeID, [[maybe_unused]] Vec3Arg inDirection, [[maybe_unused]] Vec3Arg inScale, [[maybe_unused]] Mat44Arg inCenterOfMassTransform, [[maybe_unused]] SupportingFace &outVertices) const { /* Nothing */ }
+
+	/// Get the user data of a particular sub shape ID. Corresponds with the value stored in Shape::GetUserData of the leaf shape pointed to by inSubShapeID.
+	virtual uint64					GetSubShapeUserData([[maybe_unused]] const SubShapeID &inSubShapeID) const			{ return mUserData; }
+
+	/// Get the direct child sub shape and its transform for a sub shape ID.
+	/// @param inSubShapeID Sub shape ID that indicates the path to the leaf shape
+	/// @param inPositionCOM The position of the center of mass of this shape
+	/// @param inRotation The orientation of this shape
+	/// @param inScale Scale in local space of the shape (scales relative to its center of mass)
+	/// @param outRemainder The remainder of the sub shape ID after removing the sub shape
+	/// @return Direct child sub shape and its transform, note that the body ID and sub shape ID will be invalid
+	virtual TransformedShape		GetSubShapeTransformedShape(const SubShapeID &inSubShapeID, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, SubShapeID &outRemainder) const;
+
+	/// Gets the properties needed to do buoyancy calculations for a body using this shape
+	/// @param inCenterOfMassTransform Transform that takes this shape (centered around center of mass) to world space (or a desired other space)
+	/// @param inScale Scale in local space of the shape (scales relative to its center of mass)
+	/// @param inSurface The surface plane of the liquid relative to inCenterOfMassTransform
+	/// @param outTotalVolume On return this contains the total volume of the shape
+	/// @param outSubmergedVolume On return this contains the submerged volume of the shape
+	/// @param outCenterOfBuoyancy On return this contains the world space center of mass of the submerged volume
+#ifdef JPH_DEBUG_RENDERER
+	/// @param inBaseOffset The offset to transform inCenterOfMassTransform to world space (in double precision mode this can be used to shift the whole operation closer to the origin). Only used for debug drawing.
+#endif
+	virtual void					GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy
+#ifdef JPH_DEBUG_RENDERER // Not using JPH_IF_DEBUG_RENDERER for Doxygen
+		, RVec3Arg inBaseOffset
+#endif
+		) const = 0;
+
+#ifdef JPH_DEBUG_RENDERER
+	/// Draw the shape at a particular location with a particular color (debugging purposes)
+	virtual void					Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const = 0;
+
+	/// Draw the results of the GetSupportFunction with the convex radius added back on to show any errors introduced by this process (only relevant for convex shapes)
+	virtual void					DrawGetSupportFunction([[maybe_unused]] DebugRenderer *inRenderer, [[maybe_unused]] RMat44Arg inCenterOfMassTransform, [[maybe_unused]] Vec3Arg inScale, [[maybe_unused]] ColorArg inColor, [[maybe_unused]] bool inDrawSupportDirection) const { /* Only implemented for convex shapes */ }
+
+	/// Draw the results of the GetSupportingFace function to show any errors introduced by this process (only relevant for convex shapes)
+	virtual void					DrawGetSupportingFace([[maybe_unused]] DebugRenderer *inRenderer, [[maybe_unused]] RMat44Arg inCenterOfMassTransform, [[maybe_unused]] Vec3Arg inScale) const { /* Only implemented for convex shapes */ }
+#endif // JPH_DEBUG_RENDERER
+
+	/// Cast a ray against this shape, returns true if it finds a hit closer than ioHit.mFraction and updates that fraction. Otherwise ioHit is left untouched and the function returns false.
+	/// Note that the ray should be relative to the center of mass of this shape (i.e. subtract Shape::GetCenterOfMass() from RayCast::mOrigin if you want to cast against the shape in the space it was created).
+	/// Convex objects will be treated as solid (meaning if the ray starts inside, you'll get a hit fraction of 0) and back face hits against triangles are returned.
+	/// If you want the surface normal of the hit use GetSurfaceNormal(ioHit.mSubShapeID2, inRay.GetPointOnRay(ioHit.mFraction)).
+	virtual bool					CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const = 0;
+
+	/// Cast a ray against this shape. Allows returning multiple hits through ioCollector. Note that this version is more flexible but also slightly slower than the CastRay function that returns only a single hit.
+	/// If you want the surface normal of the hit use GetSurfaceNormal(collected sub shape ID, inRay.GetPointOnRay(collected faction)).
+	virtual void					CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const = 0;
+
+	/// Check if inPoint is inside this shape. For this tests all shapes are treated as if they were solid.
+	/// Note that inPoint should be relative to the center of mass of this shape (i.e. subtract Shape::GetCenterOfMass() from inPoint if you want to test against the shape in the space it was created).
+	/// For a mesh shape, this test will only provide sensible information if the mesh is a closed manifold.
+	/// For each shape that collides, ioCollector will receive a hit.
+	virtual void					CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const = 0;
+
+	/// Collides all vertices of a soft body with this shape and updates SoftBodyVertex::mCollisionPlane, SoftBodyVertex::mCollidingShapeIndex and SoftBodyVertex::mLargestPenetration if a collision with more penetration was found.
+	/// @param inCenterOfMassTransform Center of mass transform for this shape relative to the vertices.
+	/// @param inScale Scale in local space of the shape (scales relative to its center of mass)
+	/// @param inVertices The vertices of the soft body
+	/// @param inNumVertices The number of vertices in inVertices
+	/// @param inCollidingShapeIndex Value to store in CollideSoftBodyVertexIterator::mCollidingShapeIndex when a collision was found
+	virtual void					CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const = 0;
+
+	/// Collect the leaf transformed shapes of all leaf shapes of this shape.
+	/// inBox is the world space axis aligned box which leaf shapes should collide with.
+	/// inPositionCOM/inRotation/inScale describes the transform of this shape.
+	/// inSubShapeIDCreator represents the current sub shape ID of this shape.
+	virtual void					CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const;
+
+	/// Transforms this shape and all of its children with inTransform, resulting shape(s) are passed to ioCollector.
+	/// Note that not all shapes support all transforms (especially true for scaling), the resulting shape will try to match the transform as accurately as possible.
+	/// @param inCenterOfMassTransform The transform (rotation, translation, scale) that the center of mass of the shape should get
+	/// @param ioCollector The transformed shapes will be passed to this collector
+	virtual void					TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const;
+
+	/// Scale this shape. Note that not all shapes support all scales, this will return a shape that matches the scale as accurately as possible. See Shape::IsValidScale for more information.
+	/// @param inScale The scale to use for this shape (note: this scale is applied to the entire shape in the space it was created, most other functions apply the scale in the space of the leaf shapes and from the center of mass!)
+	ShapeResult						ScaleShape(Vec3Arg inScale) const;
+
+	/// An opaque buffer that holds shape specific information during GetTrianglesStart/Next.
+	struct alignas(16)				GetTrianglesContext { uint8 mData[4288]; };
+
+	/// This is the minimum amount of triangles that should be requested through GetTrianglesNext.
+	static constexpr int			cGetTrianglesMinTrianglesRequested = 32;
+
+	/// To start iterating over triangles, call this function first.
+	/// ioContext is a temporary buffer and should remain untouched until the last call to GetTrianglesNext.
+	/// inBox is the world space bounding in which you want to get the triangles.
+	/// inPositionCOM/inRotation/inScale describes the transform of this shape.
+	/// To get the actual triangles call GetTrianglesNext.
+	virtual void					GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const = 0;
+
+	/// Call this repeatedly to get all triangles in the box.
+	/// outTriangleVertices should be large enough to hold 3 * inMaxTriangleRequested entries.
+	/// outMaterials (if it is not null) should contain inMaxTrianglesRequested entries.
+	/// The function returns the amount of triangles that it found (which will be <= inMaxTrianglesRequested), or 0 if there are no more triangles.
+	/// Note that the function can return a value < inMaxTrianglesRequested and still have more triangles to process (triangles can be returned in blocks).
+	/// Note that the function may return triangles outside of the requested box, only coarse culling is performed on the returned triangles.
+	virtual int						GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const = 0;
+
+	///@name Binary serialization of the shape. Note that this saves the 'cooked' shape in a format which will not be backwards compatible for newer library versions.
+	/// In this case you need to recreate the shape from the ShapeSettings object and save it again. The user is expected to call SaveBinaryState followed by SaveMaterialState and SaveSubShapeState.
+	/// The stream should be stored as is and the material and shape list should be saved using the applications own serialization system (e.g. by assigning an ID to each pointer).
+	/// When restoring data, call sRestoreFromBinaryState to get the shape and then call RestoreMaterialState and RestoreSubShapeState to restore the pointers to the external objects.
+	/// Alternatively you can use SaveWithChildren and sRestoreWithChildren to save and restore the shape and all its child shapes and materials in a single stream.
+	///@{
+
+	/// Saves the contents of the shape in binary form to inStream.
+	virtual void					SaveBinaryState(StreamOut &inStream) const;
+
+	/// Creates a Shape of the correct type and restores its contents from the binary stream inStream.
+	static ShapeResult				sRestoreFromBinaryState(StreamIn &inStream);
+
+	/// Outputs the material references that this shape has to outMaterials.
+	virtual void					SaveMaterialState([[maybe_unused]] PhysicsMaterialList &outMaterials) const			{ /* By default do nothing */ }
+
+	/// Restore the material references after calling sRestoreFromBinaryState. Note that the exact same materials need to be provided in the same order as returned by SaveMaterialState.
+	virtual void					RestoreMaterialState([[maybe_unused]] const PhysicsMaterialRefC *inMaterials, [[maybe_unused]] uint inNumMaterials) { JPH_ASSERT(inNumMaterials == 0); }
+
+	/// Outputs the shape references that this shape has to outSubShapes.
+	virtual void					SaveSubShapeState([[maybe_unused]] ShapeList &outSubShapes) const					{ /* By default do nothing */ }
+
+	/// Restore the shape references after calling sRestoreFromBinaryState. Note that the exact same shapes need to be provided in the same order as returned by SaveSubShapeState.
+	virtual void					RestoreSubShapeState([[maybe_unused]] const ShapeRefC *inSubShapes, [[maybe_unused]] uint inNumShapes) { JPH_ASSERT(inNumShapes == 0); }
+
+	using ShapeToIDMap = StreamUtils::ObjectToIDMap<Shape>;
+	using IDToShapeMap = StreamUtils::IDToObjectMap<Shape>;
+	using MaterialToIDMap = StreamUtils::ObjectToIDMap<PhysicsMaterial>;
+	using IDToMaterialMap = StreamUtils::IDToObjectMap<PhysicsMaterial>;
+
+	/// Save this shape, all its children and its materials. Pass in an empty map in ioShapeMap / ioMaterialMap or reuse the same map while saving multiple shapes to the same stream in order to avoid writing duplicates.
+	void							SaveWithChildren(StreamOut &inStream, ShapeToIDMap &ioShapeMap, MaterialToIDMap &ioMaterialMap) const;
+
+	/// Restore a shape, all its children and materials. Pass in an empty map in ioShapeMap / ioMaterialMap or reuse the same map while reading multiple shapes from the same stream in order to restore duplicates.
+	static ShapeResult				sRestoreWithChildren(StreamIn &inStream, IDToShapeMap &ioShapeMap, IDToMaterialMap &ioMaterialMap);
+
+	///@}
+
+	/// Class that holds information about the shape that can be used for logging / data collection purposes
+	struct Stats
+	{
+									Stats(size_t inSizeBytes, uint inNumTriangles) : mSizeBytes(inSizeBytes), mNumTriangles(inNumTriangles) { }
+
+		size_t						mSizeBytes;				///< Amount of memory used by this shape (size in bytes)
+		uint						mNumTriangles;			///< Number of triangles in this shape (when applicable)
+	};
+
+	/// Get stats of this shape. Use for logging / data collection purposes only. Does not add values from child shapes, use GetStatsRecursive for this.
+	virtual Stats					GetStats() const = 0;
+
+	using VisitedShapes = UnorderedSet<const Shape *>;
+
+	/// Get the combined stats of this shape and its children.
+	/// @param ioVisitedShapes is used to track which shapes have already been visited, to avoid calculating the wrong memory size.
+	virtual Stats					GetStatsRecursive(VisitedShapes &ioVisitedShapes) const;
+
+	///< Volume of this shape (m^3). Note that for compound shapes the volume may be incorrect since child shapes can overlap which is not accounted for.
+	virtual float					GetVolume() const = 0;
+
+	/// Test if inScale is a valid scale for this shape. Some shapes can only be scaled uniformly, compound shapes cannot handle shapes
+	/// being rotated and scaled (this would cause shearing), scale can never be zero. When the scale is invalid, the function will return false.
+	///
+	/// Here's a list of supported scales:
+	/// * SphereShape: Scale must be uniform (signs of scale are ignored).
+	/// * BoxShape: Any scale supported (signs of scale are ignored).
+	/// * TriangleShape: Any scale supported when convex radius is zero, otherwise only uniform scale supported.
+	/// * CapsuleShape: Scale must be uniform (signs of scale are ignored).
+	/// * TaperedCapsuleShape: Scale must be uniform (sign of Y scale can be used to flip the capsule).
+	/// * CylinderShape: Scale must be uniform in XZ plane, Y can scale independently (signs of scale are ignored).
+	/// * RotatedTranslatedShape: Scale must not cause shear in the child shape.
+	/// * CompoundShape: Scale must not cause shear in any of the child shapes.
+	virtual bool					IsValidScale(Vec3Arg inScale) const;
+
+	/// This function will make sure that if you wrap this shape in a ScaledShape that the scale is valid.
+	/// Note that this involves discarding components of the scale that are invalid, so the resulting scaled shape may be different than the requested scale.
+	/// Compare the return value of this function with the scale you passed in to detect major inconsistencies and possibly warn the user.
+	/// @param inScale Local space scale for this shape.
+	/// @return Scale that can be used to wrap this shape in a ScaledShape. IsValidScale will return true for this scale.
+	virtual Vec3					MakeScaleValid(Vec3Arg inScale) const;
+
+#ifdef JPH_DEBUG_RENDERER
+	/// Debug helper which draws the intersection between water and the shapes, the center of buoyancy and the submerged volume
+	static bool						sDrawSubmergedVolumes;
+#endif // JPH_DEBUG_RENDERER
+
+protected:
+	/// This function should not be called directly, it is used by sRestoreFromBinaryState.
+	virtual void					RestoreBinaryState(StreamIn &inStream);
+
+	/// A fallback version of CollidePoint that uses a ray cast and counts the number of hits to determine if the point is inside the shape. Odd number of hits means inside, even number of hits means outside.
+	static void						sCollidePointUsingRayCast(const Shape &inShape, Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter);
+
+private:
+	uint64							mUserData = 0;
+	EShapeType						mShapeType;
+	EShapeSubType					mShapeSubType;
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/SphereShape.cpp

@@ -0,0 +1,347 @@
+// 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/Shape/SphereShape.h>
+#include <Jolt/Physics/Collision/Shape/ScaleHelpers.h>
+#include <Jolt/Physics/Collision/Shape/GetTrianglesContext.h>
+#include <Jolt/Physics/Collision/RayCast.h>
+#include <Jolt/Physics/Collision/CastResult.h>
+#include <Jolt/Physics/Collision/CollidePointResult.h>
+#include <Jolt/Physics/Collision/TransformedShape.h>
+#include <Jolt/Physics/Collision/CollideSoftBodyVertexIterator.h>
+#include <Jolt/Geometry/RaySphere.h>
+#include <Jolt/Geometry/Plane.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(SphereShapeSettings)
+{
+	JPH_ADD_BASE_CLASS(SphereShapeSettings, ConvexShapeSettings)
+
+	JPH_ADD_ATTRIBUTE(SphereShapeSettings, mRadius)
+}
+
+ShapeSettings::ShapeResult SphereShapeSettings::Create() const
+{
+	if (mCachedResult.IsEmpty())
+		Ref<Shape> shape = new SphereShape(*this, mCachedResult);
+	return mCachedResult;
+}
+
+SphereShape::SphereShape(const SphereShapeSettings &inSettings, ShapeResult &outResult) :
+	ConvexShape(EShapeSubType::Sphere, inSettings, outResult),
+	mRadius(inSettings.mRadius)
+{
+	if (inSettings.mRadius <= 0.0f)
+	{
+		outResult.SetError("Invalid radius");
+		return;
+	}
+
+	outResult.Set(this);
+}
+
+float SphereShape::GetScaledRadius(Vec3Arg inScale) const
+{
+	JPH_ASSERT(IsValidScale(inScale));
+
+	Vec3 abs_scale = inScale.Abs();
+	return abs_scale.GetX() * mRadius;
+}
+
+AABox SphereShape::GetLocalBounds() const
+{
+	Vec3 half_extent = Vec3::sReplicate(mRadius);
+	return AABox(-half_extent, half_extent);
+}
+
+AABox SphereShape::GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const
+{
+	float scaled_radius = GetScaledRadius(inScale);
+	Vec3 half_extent = Vec3::sReplicate(scaled_radius);
+	AABox bounds(-half_extent, half_extent);
+	bounds.Translate(inCenterOfMassTransform.GetTranslation());
+	return bounds;
+}
+
+class SphereShape::SphereNoConvex final : public Support
+{
+public:
+	explicit		SphereNoConvex(float inRadius) :
+		mRadius(inRadius)
+	{
+		static_assert(sizeof(SphereNoConvex) <= sizeof(SupportBuffer), "Buffer size too small");
+		JPH_ASSERT(IsAligned(this, alignof(SphereNoConvex)));
+	}
+
+	virtual Vec3	GetSupport(Vec3Arg inDirection) const override
+	{
+		return Vec3::sZero();
+	}
+
+	virtual float	GetConvexRadius() const override
+	{
+		return mRadius;
+	}
+
+private:
+	float			mRadius;
+};
+
+class SphereShape::SphereWithConvex final : public Support
+{
+public:
+	explicit		SphereWithConvex(float inRadius) :
+		mRadius(inRadius)
+	{
+		static_assert(sizeof(SphereWithConvex) <= sizeof(SupportBuffer), "Buffer size too small");
+		JPH_ASSERT(IsAligned(this, alignof(SphereWithConvex)));
+	}
+
+	virtual Vec3	GetSupport(Vec3Arg inDirection) const override
+	{
+		float len = inDirection.Length();
+		return len > 0.0f? (mRadius / len) * inDirection : Vec3::sZero();
+	}
+
+	virtual float	GetConvexRadius() const override
+	{
+		return 0.0f;
+	}
+
+private:
+	float			mRadius;
+};
+
+const ConvexShape::Support *SphereShape::GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const
+{
+	float scaled_radius = GetScaledRadius(inScale);
+
+	switch (inMode)
+	{
+	case ESupportMode::IncludeConvexRadius:
+		return new (&inBuffer) SphereWithConvex(scaled_radius);
+
+	case ESupportMode::ExcludeConvexRadius:
+	case ESupportMode::Default:
+		return new (&inBuffer) SphereNoConvex(scaled_radius);
+	}
+
+	JPH_ASSERT(false);
+	return nullptr;
+}
+
+MassProperties SphereShape::GetMassProperties() const
+{
+	MassProperties p;
+
+	// Calculate mass
+	float r2 = mRadius * mRadius;
+	p.mMass = (4.0f / 3.0f * JPH_PI) * mRadius * r2 * GetDensity();
+
+	// Calculate inertia
+	float inertia = (2.0f / 5.0f) * p.mMass * r2;
+	p.mInertia = Mat44::sScale(inertia);
+
+	return p;
+}
+
+Vec3 SphereShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const
+{
+	JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID");
+
+	float len = inLocalSurfacePosition.Length();
+	return len != 0.0f? inLocalSurfacePosition / len : Vec3::sAxisY();
+}
+
+void SphereShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const
+{
+	float scaled_radius = GetScaledRadius(inScale);
+	outTotalVolume = (4.0f / 3.0f * JPH_PI) * Cubed(scaled_radius);
+
+	float distance_to_surface = inSurface.SignedDistance(inCenterOfMassTransform.GetTranslation());
+	if (distance_to_surface >= scaled_radius)
+	{
+		// Above surface
+		outSubmergedVolume = 0.0f;
+		outCenterOfBuoyancy = Vec3::sZero();
+	}
+	else if (distance_to_surface <= -scaled_radius)
+	{
+		// Under surface
+		outSubmergedVolume = outTotalVolume;
+		outCenterOfBuoyancy = inCenterOfMassTransform.GetTranslation();
+	}
+	else
+	{
+		// Intersecting surface
+
+		// Calculate submerged volume, see: https://en.wikipedia.org/wiki/Spherical_cap
+		float h = scaled_radius - distance_to_surface;
+		outSubmergedVolume = (JPH_PI / 3.0f) * Square(h) * (3.0f * scaled_radius - h);
+
+		// Calculate center of buoyancy, see: http://mathworld.wolfram.com/SphericalCap.html (eq 10)
+		float z = (3.0f / 4.0f) * Square(2.0f * scaled_radius - h) / (3.0f * scaled_radius - h);
+		outCenterOfBuoyancy = inCenterOfMassTransform.GetTranslation() - z * inSurface.GetNormal(); // Negative normal since we want the portion under the water
+
+	#ifdef JPH_DEBUG_RENDERER
+		// Draw intersection between sphere and water plane
+		if (sDrawSubmergedVolumes)
+		{
+			Vec3 circle_center = inCenterOfMassTransform.GetTranslation() - distance_to_surface * inSurface.GetNormal();
+			float circle_radius = sqrt(Square(scaled_radius) - Square(distance_to_surface));
+			DebugRenderer::sInstance->DrawPie(inBaseOffset + circle_center, circle_radius, inSurface.GetNormal(), inSurface.GetNormal().GetNormalizedPerpendicular(), -JPH_PI, JPH_PI, Color::sGreen, DebugRenderer::ECastShadow::Off);
+		}
+	#endif // JPH_DEBUG_RENDERER
+	}
+
+#ifdef JPH_DEBUG_RENDERER
+	// Draw center of buoyancy
+	if (sDrawSubmergedVolumes)
+		DebugRenderer::sInstance->DrawWireSphere(inBaseOffset + outCenterOfBuoyancy, 0.05f, Color::sRed, 1);
+#endif // JPH_DEBUG_RENDERER
+}
+
+#ifdef JPH_DEBUG_RENDERER
+void SphereShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
+{
+	DebugRenderer::EDrawMode draw_mode = inDrawWireframe? DebugRenderer::EDrawMode::Wireframe : DebugRenderer::EDrawMode::Solid;
+	inRenderer->DrawUnitSphere(inCenterOfMassTransform * Mat44::sScale(mRadius * inScale.Abs().GetX()), inUseMaterialColors? GetMaterial()->GetDebugColor() : inColor, DebugRenderer::ECastShadow::On, draw_mode);
+}
+#endif // JPH_DEBUG_RENDERER
+
+bool SphereShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const
+{
+	float fraction = RaySphere(inRay.mOrigin, inRay.mDirection, Vec3::sZero(), mRadius);
+	if (fraction < ioHit.mFraction)
+	{
+		ioHit.mFraction = fraction;
+		ioHit.mSubShapeID2 = inSubShapeIDCreator.GetID();
+		return true;
+	}
+	return false;
+}
+
+void SphereShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	// Test shape filter
+	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
+		return;
+
+	float min_fraction, max_fraction;
+	int num_results = RaySphere(inRay.mOrigin, inRay.mDirection, Vec3::sZero(), mRadius, min_fraction, max_fraction);
+	if (num_results > 0 // Ray should intersect
+		&& max_fraction >= 0.0f // End of ray should be inside sphere
+		&& min_fraction < ioCollector.GetEarlyOutFraction()) // Start of ray should be before early out fraction
+	{
+		// Better hit than the current hit
+		RayCastResult hit;
+		hit.mBodyID = TransformedShape::sGetBodyID(ioCollector.GetContext());
+		hit.mSubShapeID2 = inSubShapeIDCreator.GetID();
+
+		// Check front side hit
+		if (inRayCastSettings.mTreatConvexAsSolid || min_fraction > 0.0f)
+		{
+			hit.mFraction = max(0.0f, min_fraction);
+			ioCollector.AddHit(hit);
+		}
+
+		// Check back side hit
+		if (inRayCastSettings.mBackFaceModeConvex == EBackFaceMode::CollideWithBackFaces
+			&& num_results > 1 // Ray should have 2 intersections
+			&& max_fraction < ioCollector.GetEarlyOutFraction()) // End of ray should be before early out fraction
+		{
+			hit.mFraction = max_fraction;
+			ioCollector.AddHit(hit);
+		}
+	}
+}
+
+void SphereShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	// Test shape filter
+	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
+		return;
+
+	if (inPoint.LengthSq() <= Square(mRadius))
+		ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator.GetID() });
+}
+
+void SphereShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const
+{
+	Vec3 center = inCenterOfMassTransform.GetTranslation();
+	float radius = GetScaledRadius(inScale);
+
+	for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v)
+		if (v.GetInvMass() > 0.0f)
+		{
+			// Calculate penetration
+			Vec3 delta = v.GetPosition() - center;
+			float distance = delta.Length();
+			float penetration = radius - distance;
+			if (v.UpdatePenetration(penetration))
+			{
+				// Calculate contact point and normal
+				Vec3 normal = distance > 0.0f? delta / distance : Vec3::sAxisY();
+				Vec3 point = center + radius * normal;
+
+				// Store collision
+				v.SetCollision(Plane::sFromPointAndNormal(point, normal), inCollidingShapeIndex);
+			}
+		}
+}
+
+void SphereShape::GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const
+{
+	float scaled_radius = GetScaledRadius(inScale);
+	new (&ioContext) GetTrianglesContextVertexList(inPositionCOM, inRotation, Vec3::sOne(), Mat44::sScale(scaled_radius), sUnitSphereTriangles.data(), sUnitSphereTriangles.size(), GetMaterial());
+}
+
+int SphereShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) const
+{
+	return ((GetTrianglesContextVertexList &)ioContext).GetTrianglesNext(inMaxTrianglesRequested, outTriangleVertices, outMaterials);
+}
+
+void SphereShape::SaveBinaryState(StreamOut &inStream) const
+{
+	ConvexShape::SaveBinaryState(inStream);
+
+	inStream.Write(mRadius);
+}
+
+void SphereShape::RestoreBinaryState(StreamIn &inStream)
+{
+	ConvexShape::RestoreBinaryState(inStream);
+
+	inStream.Read(mRadius);
+}
+
+bool SphereShape::IsValidScale(Vec3Arg inScale) const
+{
+	return ConvexShape::IsValidScale(inScale) && ScaleHelpers::IsUniformScale(inScale.Abs());
+}
+
+Vec3 SphereShape::MakeScaleValid(Vec3Arg inScale) const
+{
+	Vec3 scale = ScaleHelpers::MakeNonZeroScale(inScale);
+
+	return scale.GetSign() * ScaleHelpers::MakeUniformScale(scale.Abs());
+}
+
+void SphereShape::sRegister()
+{
+	ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::Sphere);
+	f.mConstruct = []() -> Shape * { return new SphereShape; };
+	f.mColor = Color::sGreen;
+}
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/SphereShape.h

@@ -0,0 +1,125 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/Shape/ConvexShape.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Class that constructs a SphereShape
+class JPH_EXPORT SphereShapeSettings final : public ConvexShapeSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, SphereShapeSettings)
+
+public:
+	/// Default constructor for deserialization
+							SphereShapeSettings() = default;
+
+	/// Create a sphere with radius inRadius
+							SphereShapeSettings(float inRadius, const PhysicsMaterial *inMaterial = nullptr)	: ConvexShapeSettings(inMaterial), mRadius(inRadius) { }
+
+	// See: ShapeSettings
+	virtual ShapeResult		Create() const override;
+
+	float					mRadius = 0.0f;
+};
+
+/// A sphere, centered around the origin.
+/// Note that it is implemented as a point with convex radius.
+class JPH_EXPORT SphereShape final : public ConvexShape
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+							SphereShape() : ConvexShape(EShapeSubType::Sphere) { }
+							SphereShape(const SphereShapeSettings &inSettings, ShapeResult &outResult);
+
+	/// Create a sphere with radius inRadius
+							SphereShape(float inRadius, const PhysicsMaterial *inMaterial = nullptr)			: ConvexShape(EShapeSubType::Sphere, inMaterial), mRadius(inRadius) { JPH_ASSERT(inRadius > 0.0f); }
+
+	/// Radius of the sphere
+	float					GetRadius() const																	{ return mRadius; }
+
+	// See Shape::GetLocalBounds
+	virtual AABox			GetLocalBounds() const override;
+
+	// See Shape::GetWorldSpaceBounds
+	virtual AABox			GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override;
+	using Shape::GetWorldSpaceBounds;
+
+	// See Shape::GetInnerRadius
+	virtual float			GetInnerRadius() const override														{ return mRadius; }
+
+	// See Shape::GetMassProperties
+	virtual MassProperties	GetMassProperties() const override;
+
+	// See Shape::GetSurfaceNormal
+	virtual Vec3			GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override;
+
+	// See Shape::GetSupportingFace
+	virtual void			GetSupportingFace([[maybe_unused]] const SubShapeID &inSubShapeID, [[maybe_unused]] Vec3Arg inDirection, [[maybe_unused]] Vec3Arg inScale, [[maybe_unused]] Mat44Arg inCenterOfMassTransform, [[maybe_unused]] SupportingFace &outVertices) const override { /* Hit is always a single point, no point in returning anything */ }
+
+	// See ConvexShape::GetSupportFunction
+	virtual const Support *	GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const override;
+
+	// See Shape::GetSubmergedVolume
+	virtual void			GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const override;
+
+#ifdef JPH_DEBUG_RENDERER
+	// See Shape::Draw
+	virtual void			Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override;
+#endif // JPH_DEBUG_RENDERER
+
+	// See Shape::CastRay
+	virtual bool			CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override;
+	virtual void			CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
+
+	// See: Shape::CollidePoint
+	virtual void			CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
+
+	// See: Shape::CollideSoftBodyVertices
+	virtual void			CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override;
+
+	// See Shape::GetTrianglesStart
+	virtual void			GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override;
+
+	// See Shape::GetTrianglesNext
+	virtual int				GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override;
+
+	// See Shape
+	virtual void			SaveBinaryState(StreamOut &inStream) const override;
+
+	// See Shape::GetStats
+	virtual Stats			GetStats() const override															{ return Stats(sizeof(*this), 0); }
+
+	// See Shape::GetVolume
+	virtual float			GetVolume() const override															{ return 4.0f / 3.0f * JPH_PI * Cubed(mRadius); }
+
+	// See Shape::IsValidScale
+	virtual bool			IsValidScale(Vec3Arg inScale) const override;
+
+	// See Shape::MakeScaleValid
+	virtual Vec3			MakeScaleValid(Vec3Arg inScale) const override;
+
+	// Register shape functions with the registry
+	static void				sRegister();
+
+protected:
+	// See: Shape::RestoreBinaryState
+	virtual void			RestoreBinaryState(StreamIn &inStream) override;
+
+private:
+	// Get the radius of this sphere scaled by inScale
+	inline float			GetScaledRadius(Vec3Arg inScale) const;
+
+	// Classes for GetSupportFunction
+	class					SphereNoConvex;
+	class					SphereWithConvex;
+
+	float					mRadius = 0.0f;
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/StaticCompoundShape.cpp

@@ -0,0 +1,674 @@
+// 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/Shape/StaticCompoundShape.h>
+#include <Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h>
+#include <Jolt/Physics/Collision/Shape/CompoundShapeVisitors.h>
+#include <Jolt/Core/Profiler.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+#include <Jolt/Core/TempAllocator.h>
+#include <Jolt/Core/ScopeExit.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(StaticCompoundShapeSettings)
+{
+	JPH_ADD_BASE_CLASS(StaticCompoundShapeSettings, CompoundShapeSettings)
+}
+
+ShapeSettings::ShapeResult StaticCompoundShapeSettings::Create(TempAllocator &inTempAllocator) const
+{
+	if (mCachedResult.IsEmpty())
+	{
+		if (mSubShapes.size() == 0)
+		{
+			// It's an error to create a compound with no subshapes (the compound cannot encode this)
+			mCachedResult.SetError("Compound needs a sub shape!");
+		}
+		else if (mSubShapes.size() == 1)
+		{
+			// If there's only 1 part we don't need a StaticCompoundShape
+			const SubShapeSettings &s = mSubShapes[0];
+			if (s.mPosition == Vec3::sZero()
+				&& s.mRotation == Quat::sIdentity())
+			{
+				// No rotation or translation, we can use the shape directly
+				if (s.mShapePtr != nullptr)
+					mCachedResult.Set(const_cast<Shape *>(s.mShapePtr.GetPtr()));
+				else if (s.mShape != nullptr)
+					mCachedResult = s.mShape->Create();
+				else
+					mCachedResult.SetError("Sub shape is null!");
+			}
+			else
+			{
+				// We can use a RotatedTranslatedShape instead
+				RotatedTranslatedShapeSettings settings;
+				settings.mPosition = s.mPosition;
+				settings.mRotation = s.mRotation;
+				settings.mInnerShape = s.mShape;
+				settings.mInnerShapePtr = s.mShapePtr;
+				Ref<Shape> shape = new RotatedTranslatedShape(settings, mCachedResult);
+			}
+		}
+		else
+		{
+			// Build a regular compound shape
+			Ref<Shape> shape = new StaticCompoundShape(*this, inTempAllocator, mCachedResult);
+		}
+	}
+	return mCachedResult;
+}
+
+ShapeSettings::ShapeResult StaticCompoundShapeSettings::Create() const
+{
+	TempAllocatorMalloc allocator;
+	return Create(allocator);
+}
+
+void StaticCompoundShape::Node::SetChildInvalid(uint inIndex)
+{
+	// Make this an invalid node
+	mNodeProperties[inIndex] = INVALID_NODE;
+
+	// Make bounding box invalid
+	mBoundsMinX[inIndex] = HALF_FLT_MAX;
+	mBoundsMinY[inIndex] = HALF_FLT_MAX;
+	mBoundsMinZ[inIndex] = HALF_FLT_MAX;
+	mBoundsMaxX[inIndex] = HALF_FLT_MAX;
+	mBoundsMaxY[inIndex] = HALF_FLT_MAX;
+	mBoundsMaxZ[inIndex] = HALF_FLT_MAX;
+}
+
+void StaticCompoundShape::Node::SetChildBounds(uint inIndex, const AABox &inBounds)
+{
+	mBoundsMinX[inIndex] = HalfFloatConversion::FromFloat<HalfFloatConversion::ROUND_TO_NEG_INF>(inBounds.mMin.GetX());
+	mBoundsMinY[inIndex] = HalfFloatConversion::FromFloat<HalfFloatConversion::ROUND_TO_NEG_INF>(inBounds.mMin.GetY());
+	mBoundsMinZ[inIndex] = HalfFloatConversion::FromFloat<HalfFloatConversion::ROUND_TO_NEG_INF>(inBounds.mMin.GetZ());
+	mBoundsMaxX[inIndex] = HalfFloatConversion::FromFloat<HalfFloatConversion::ROUND_TO_POS_INF>(inBounds.mMax.GetX());
+	mBoundsMaxY[inIndex] = HalfFloatConversion::FromFloat<HalfFloatConversion::ROUND_TO_POS_INF>(inBounds.mMax.GetY());
+	mBoundsMaxZ[inIndex] = HalfFloatConversion::FromFloat<HalfFloatConversion::ROUND_TO_POS_INF>(inBounds.mMax.GetZ());
+}
+
+void StaticCompoundShape::sPartition(uint *ioBodyIdx, AABox *ioBounds, int inNumber, int &outMidPoint)
+{
+	// Handle trivial case
+	if (inNumber <= 4)
+	{
+		outMidPoint = inNumber / 2;
+		return;
+	}
+
+	// Calculate bounding box of box centers
+	Vec3 center_min = Vec3::sReplicate(FLT_MAX);
+	Vec3 center_max = Vec3::sReplicate(-FLT_MAX);
+	for (const AABox *b = ioBounds, *b_end = ioBounds + inNumber; b < b_end; ++b)
+	{
+		Vec3 center = b->GetCenter();
+		center_min = Vec3::sMin(center_min, center);
+		center_max = Vec3::sMax(center_max, center);
+	}
+
+	// Calculate split plane
+	int dimension = (center_max - center_min).GetHighestComponentIndex();
+	float split = 0.5f * (center_min + center_max)[dimension];
+
+	// Divide bodies
+	int start = 0, end = inNumber;
+	while (start < end)
+	{
+		// Search for first element that is on the right hand side of the split plane
+		while (start < end && ioBounds[start].GetCenter()[dimension] < split)
+			++start;
+
+		// Search for the first element that is on the left hand side of the split plane
+		while (start < end && ioBounds[end - 1].GetCenter()[dimension] >= split)
+			--end;
+
+		if (start < end)
+		{
+			// Swap the two elements
+			std::swap(ioBodyIdx[start], ioBodyIdx[end - 1]);
+			std::swap(ioBounds[start], ioBounds[end - 1]);
+			++start;
+			--end;
+		}
+	}
+	JPH_ASSERT(start == end);
+
+	if (start > 0 && start < inNumber)
+	{
+		// Success!
+		outMidPoint = start;
+	}
+	else
+	{
+		// Failed to divide bodies
+		outMidPoint = inNumber / 2;
+	}
+}
+
+void StaticCompoundShape::sPartition4(uint *ioBodyIdx, AABox *ioBounds, int inBegin, int inEnd, int *outSplit)
+{
+	uint *body_idx = ioBodyIdx + inBegin;
+	AABox *node_bounds = ioBounds + inBegin;
+	int number = inEnd - inBegin;
+
+	// Partition entire range
+	sPartition(body_idx, node_bounds, number, outSplit[2]);
+
+	// Partition lower half
+	sPartition(body_idx, node_bounds, outSplit[2], outSplit[1]);
+
+	// Partition upper half
+	sPartition(body_idx + outSplit[2], node_bounds + outSplit[2], number - outSplit[2], outSplit[3]);
+
+	// Convert to proper range
+	outSplit[0] = inBegin;
+	outSplit[1] += inBegin;
+	outSplit[2] += inBegin;
+	outSplit[3] += outSplit[2];
+	outSplit[4] = inEnd;
+}
+
+StaticCompoundShape::StaticCompoundShape(const StaticCompoundShapeSettings &inSettings, TempAllocator &inTempAllocator, ShapeResult &outResult) :
+	CompoundShape(EShapeSubType::StaticCompound, inSettings, outResult)
+{
+	// Check that there's at least 1 shape
+	uint num_subshapes = (uint)inSettings.mSubShapes.size();
+	if (num_subshapes < 2)
+	{
+		outResult.SetError("Compound needs at least 2 sub shapes, otherwise you should use a RotatedTranslatedShape!");
+		return;
+	}
+
+	// Keep track of total mass to calculate center of mass
+	float mass = 0.0f;
+
+	mSubShapes.resize(num_subshapes);
+	for (uint i = 0; i < num_subshapes; ++i)
+	{
+		const CompoundShapeSettings::SubShapeSettings &shape = inSettings.mSubShapes[i];
+
+		// Start constructing the runtime sub shape
+		SubShape &out_shape = mSubShapes[i];
+		if (!out_shape.FromSettings(shape, outResult))
+			return;
+
+		// Calculate mass properties of child
+		MassProperties child = out_shape.mShape->GetMassProperties();
+
+		// Accumulate center of mass
+		mass += child.mMass;
+		mCenterOfMass += out_shape.GetPositionCOM() * child.mMass;
+	}
+
+	if (mass > 0.0f)
+		mCenterOfMass /= mass;
+
+	// Cache the inner radius as it can take a while to recursively iterate over all sub shapes
+	CalculateInnerRadius();
+
+	// Temporary storage for the bounding boxes of all shapes
+	uint bounds_size = num_subshapes * sizeof(AABox);
+	AABox *bounds = (AABox *)inTempAllocator.Allocate(bounds_size);
+	JPH_SCOPE_EXIT([&inTempAllocator, bounds, bounds_size]{ inTempAllocator.Free(bounds, bounds_size); });
+
+	// Temporary storage for body indexes (we're shuffling them)
+	uint body_idx_size = num_subshapes * sizeof(uint);
+	uint *body_idx = (uint *)inTempAllocator.Allocate(body_idx_size);
+	JPH_SCOPE_EXIT([&inTempAllocator, body_idx, body_idx_size]{ inTempAllocator.Free(body_idx, body_idx_size); });
+
+	// Shift all shapes so that the center of mass is now at the origin and calculate bounds
+	for (uint i = 0; i < num_subshapes; ++i)
+	{
+		SubShape &shape = mSubShapes[i];
+
+		// Shift the shape so it's centered around our center of mass
+		shape.SetPositionCOM(shape.GetPositionCOM() - mCenterOfMass);
+
+		// Transform the shape's bounds into our local space
+		Mat44 transform = Mat44::sRotationTranslation(shape.GetRotation(), shape.GetPositionCOM());
+		AABox shape_bounds = shape.mShape->GetWorldSpaceBounds(transform, Vec3::sOne());
+
+		// Store bounds and body index for tree construction
+		bounds[i] = shape_bounds;
+		body_idx[i] = i;
+
+		// Update our local bounds
+		mLocalBounds.Encapsulate(shape_bounds);
+	}
+
+	// The algorithm is a recursive tree build, but to avoid the call overhead we keep track of a stack here
+	struct StackEntry
+	{
+		uint32			mNodeIdx;					// Node index of node that is generated
+		int				mChildIdx;					// Index of child that we're currently processing
+		int				mSplit[5];					// Indices where the node ID's have been split to form 4 partitions
+		AABox			mBounds;					// Bounding box of this node
+	};
+	uint stack_size = num_subshapes * sizeof(StackEntry);
+	StackEntry *stack = (StackEntry *)inTempAllocator.Allocate(stack_size);
+	JPH_SCOPE_EXIT([&inTempAllocator, stack, stack_size]{ inTempAllocator.Free(stack, stack_size); });
+	int top = 0;
+
+	// Reserve enough space so that every sub shape gets its own leaf node
+	uint next_node_idx = 0;
+	mNodes.resize(num_subshapes + (num_subshapes + 2) / 3); // = Sum(num_subshapes * 4^-i) with i = [0, Inf].
+
+	// Create root node
+	stack[0].mNodeIdx = next_node_idx++;
+	stack[0].mChildIdx = -1;
+	stack[0].mBounds = AABox();
+	sPartition4(body_idx, bounds, 0, num_subshapes, stack[0].mSplit);
+
+	for (;;)
+	{
+		StackEntry &cur_stack = stack[top];
+
+		// Next child
+		cur_stack.mChildIdx++;
+
+		// Check if all children processed
+		if (cur_stack.mChildIdx >= 4)
+		{
+			// Terminate if there's nothing left to pop
+			if (top <= 0)
+				break;
+
+			// Add our bounds to our parents bounds
+			StackEntry &prev_stack = stack[top - 1];
+			prev_stack.mBounds.Encapsulate(cur_stack.mBounds);
+
+			// Store this node's properties in the parent node
+			Node &parent_node = mNodes[prev_stack.mNodeIdx];
+			parent_node.mNodeProperties[prev_stack.mChildIdx] = cur_stack.mNodeIdx;
+			parent_node.SetChildBounds(prev_stack.mChildIdx, cur_stack.mBounds);
+
+			// Pop entry from stack
+			--top;
+		}
+		else
+		{
+			// Get low and high index to bodies to process
+			int low = cur_stack.mSplit[cur_stack.mChildIdx];
+			int high = cur_stack.mSplit[cur_stack.mChildIdx + 1];
+			int num_bodies = high - low;
+
+			if (num_bodies == 0)
+			{
+				// Mark invalid
+				Node &node = mNodes[cur_stack.mNodeIdx];
+				node.SetChildInvalid(cur_stack.mChildIdx);
+			}
+			else if (num_bodies == 1)
+			{
+				// Get body info
+				uint child_node_idx = body_idx[low];
+				const AABox &child_bounds = bounds[low];
+
+				// Update node
+				Node &node = mNodes[cur_stack.mNodeIdx];
+				node.mNodeProperties[cur_stack.mChildIdx] = child_node_idx | IS_SUBSHAPE;
+				node.SetChildBounds(cur_stack.mChildIdx, child_bounds);
+
+				// Encapsulate bounding box in parent
+				cur_stack.mBounds.Encapsulate(child_bounds);
+			}
+			else
+			{
+				// Allocate new node
+				StackEntry &new_stack = stack[++top];
+				JPH_ASSERT(top < (int)num_subshapes);
+				new_stack.mNodeIdx = next_node_idx++;
+				new_stack.mChildIdx = -1;
+				new_stack.mBounds = AABox();
+				sPartition4(body_idx, bounds, low, high, new_stack.mSplit);
+			}
+		}
+	}
+
+	// Resize nodes to actual size
+	JPH_ASSERT(next_node_idx <= mNodes.size());
+	mNodes.resize(next_node_idx);
+	mNodes.shrink_to_fit();
+
+	// Check if we ran out of bits for addressing a node
+	if (next_node_idx > IS_SUBSHAPE)
+	{
+		outResult.SetError("Compound hierarchy has too many nodes");
+		return;
+	}
+
+	// Check if we're not exceeding the amount of sub shape id bits
+	if (GetSubShapeIDBitsRecursive() > SubShapeID::MaxBits)
+	{
+		outResult.SetError("Compound hierarchy is too deep and exceeds the amount of available sub shape ID bits");
+		return;
+	}
+
+	outResult.Set(this);
+}
+
+template <class Visitor>
+inline void StaticCompoundShape::WalkTree(Visitor &ioVisitor) const
+{
+	uint32 node_stack[cStackSize];
+	node_stack[0] = 0;
+	int top = 0;
+	do
+	{
+		// Test if the node is valid, the node should rarely be invalid but it is possible when testing
+		// a really large box against the tree that the invalid nodes will intersect with the box
+		uint32 node_properties = node_stack[top];
+		if (node_properties != INVALID_NODE)
+		{
+			// Test if node contains triangles
+			bool is_node = (node_properties & IS_SUBSHAPE) == 0;
+			if (is_node)
+			{
+				const Node &node = mNodes[node_properties];
+
+				// Unpack bounds
+				UVec4 bounds_minxy = UVec4::sLoadInt4(reinterpret_cast<const uint32 *>(&node.mBoundsMinX[0]));
+				Vec4 bounds_minx = HalfFloatConversion::ToFloat(bounds_minxy);
+				Vec4 bounds_miny = HalfFloatConversion::ToFloat(bounds_minxy.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_UNUSED, SWIZZLE_UNUSED>());
+
+				UVec4 bounds_minzmaxx = UVec4::sLoadInt4(reinterpret_cast<const uint32 *>(&node.mBoundsMinZ[0]));
+				Vec4 bounds_minz = HalfFloatConversion::ToFloat(bounds_minzmaxx);
+				Vec4 bounds_maxx = HalfFloatConversion::ToFloat(bounds_minzmaxx.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_UNUSED, SWIZZLE_UNUSED>());
+
+				UVec4 bounds_maxyz = UVec4::sLoadInt4(reinterpret_cast<const uint32 *>(&node.mBoundsMaxY[0]));
+				Vec4 bounds_maxy = HalfFloatConversion::ToFloat(bounds_maxyz);
+				Vec4 bounds_maxz = HalfFloatConversion::ToFloat(bounds_maxyz.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_UNUSED, SWIZZLE_UNUSED>());
+
+				// Load properties for 4 children
+				UVec4 properties = UVec4::sLoadInt4(&node.mNodeProperties[0]);
+
+				// Check which sub nodes to visit
+				int num_results = ioVisitor.VisitNodes(bounds_minx, bounds_miny, bounds_minz, bounds_maxx, bounds_maxy, bounds_maxz, properties, top);
+
+				// Push them onto the stack
+				JPH_ASSERT(top + 4 < cStackSize);
+				properties.StoreInt4(&node_stack[top]);
+				top += num_results;
+			}
+			else
+			{
+				// Points to a sub shape
+				uint32 sub_shape_idx = node_properties ^ IS_SUBSHAPE;
+				const SubShape &sub_shape = mSubShapes[sub_shape_idx];
+
+				ioVisitor.VisitShape(sub_shape, sub_shape_idx);
+			}
+
+			// Check if we're done
+			if (ioVisitor.ShouldAbort())
+				break;
+		}
+
+		// Fetch next node until we find one that the visitor wants to see
+		do
+			--top;
+		while (top >= 0 && !ioVisitor.ShouldVisitNode(top));
+	}
+	while (top >= 0);
+}
+
+bool StaticCompoundShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	struct Visitor : public CastRayVisitor
+	{
+		using CastRayVisitor::CastRayVisitor;
+
+		JPH_INLINE bool		ShouldVisitNode(int inStackTop) const
+		{
+			return mDistanceStack[inStackTop] < mHit.mFraction;
+		}
+
+		JPH_INLINE int		VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop)
+		{
+			// Test bounds of 4 children
+			Vec4 distance = TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
+
+			// Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom)
+			return SortReverseAndStore(distance, mHit.mFraction, ioProperties, &mDistanceStack[inStackTop]);
+		}
+
+		float				mDistanceStack[cStackSize];
+	};
+
+	Visitor visitor(inRay, this, inSubShapeIDCreator, ioHit);
+	WalkTree(visitor);
+	return visitor.mReturnValue;
+}
+
+void StaticCompoundShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	// Test shape filter
+	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
+		return;
+
+	JPH_PROFILE_FUNCTION();
+
+	struct Visitor : public CastRayVisitorCollector
+	{
+		using CastRayVisitorCollector::CastRayVisitorCollector;
+
+		JPH_INLINE bool		ShouldVisitNode(int inStackTop) const
+		{
+			return mDistanceStack[inStackTop] < mCollector.GetEarlyOutFraction();
+		}
+
+		JPH_INLINE int		VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop)
+		{
+			// Test bounds of 4 children
+			Vec4 distance = TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
+
+			// Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom)
+			return SortReverseAndStore(distance, mCollector.GetEarlyOutFraction(), ioProperties, &mDistanceStack[inStackTop]);
+		}
+
+		float				mDistanceStack[cStackSize];
+	};
+
+	Visitor visitor(inRay, inRayCastSettings, this, inSubShapeIDCreator, ioCollector, inShapeFilter);
+	WalkTree(visitor);
+}
+
+void StaticCompoundShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	struct Visitor : public CollidePointVisitor
+	{
+		using CollidePointVisitor::CollidePointVisitor;
+
+		JPH_INLINE bool		ShouldVisitNode([[maybe_unused]] int inStackTop) const
+		{
+			return true;
+		}
+
+		JPH_INLINE int		VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, [[maybe_unused]] int inStackTop) const
+		{
+			// Test if point overlaps with box
+			UVec4 collides = TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
+			return CountAndSortTrues(collides, ioProperties);
+		}
+	};
+
+	Visitor visitor(inPoint, this, inSubShapeIDCreator, ioCollector, inShapeFilter);
+	WalkTree(visitor);
+}
+
+void StaticCompoundShape::sCastShapeVsCompound(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
+{
+	JPH_PROFILE_FUNCTION();
+
+	struct Visitor : public CastShapeVisitor
+	{
+		using CastShapeVisitor::CastShapeVisitor;
+
+		JPH_INLINE bool		ShouldVisitNode(int inStackTop) const
+		{
+			return mDistanceStack[inStackTop] < mCollector.GetPositiveEarlyOutFraction();
+		}
+
+		JPH_INLINE int		VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop)
+		{
+			// Test bounds of 4 children
+			Vec4 distance = TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
+
+			// Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom)
+			return SortReverseAndStore(distance, mCollector.GetPositiveEarlyOutFraction(), ioProperties, &mDistanceStack[inStackTop]);
+		}
+
+		float				mDistanceStack[cStackSize];
+	};
+
+	JPH_ASSERT(inShape->GetSubType() == EShapeSubType::StaticCompound);
+	const StaticCompoundShape *shape = static_cast<const StaticCompoundShape *>(inShape);
+
+	Visitor visitor(inShapeCast, inShapeCastSettings, shape, inScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector);
+	shape->WalkTree(visitor);
+}
+
+void StaticCompoundShape::CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Test shape filter
+	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
+		return;
+
+	struct Visitor : public CollectTransformedShapesVisitor
+	{
+		using CollectTransformedShapesVisitor::CollectTransformedShapesVisitor;
+
+		JPH_INLINE bool		ShouldVisitNode([[maybe_unused]] int inStackTop) const
+		{
+			return true;
+		}
+
+		JPH_INLINE int		VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, [[maybe_unused]] int inStackTop) const
+		{
+			// Test which nodes collide
+			UVec4 collides = TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
+			return CountAndSortTrues(collides, ioProperties);
+		}
+	};
+
+	Visitor visitor(inBox, this, inPositionCOM, inRotation, inScale, inSubShapeIDCreator, ioCollector, inShapeFilter);
+	WalkTree(visitor);
+}
+
+int StaticCompoundShape::GetIntersectingSubShapes(const AABox &inBox, uint *outSubShapeIndices, int inMaxSubShapeIndices) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	GetIntersectingSubShapesVisitorSC<AABox> visitor(inBox, outSubShapeIndices, inMaxSubShapeIndices);
+	WalkTree(visitor);
+	return visitor.GetNumResults();
+}
+
+int StaticCompoundShape::GetIntersectingSubShapes(const OrientedBox &inBox, uint *outSubShapeIndices, int inMaxSubShapeIndices) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	GetIntersectingSubShapesVisitorSC<OrientedBox> visitor(inBox, outSubShapeIndices, inMaxSubShapeIndices);
+	WalkTree(visitor);
+	return visitor.GetNumResults();
+}
+
+void StaticCompoundShape::sCollideCompoundVsShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter)
+{
+	JPH_PROFILE_FUNCTION();
+
+	JPH_ASSERT(inShape1->GetSubType() == EShapeSubType::StaticCompound);
+	const StaticCompoundShape *shape1 = static_cast<const StaticCompoundShape *>(inShape1);
+
+	struct Visitor : public CollideCompoundVsShapeVisitor
+	{
+		using CollideCompoundVsShapeVisitor::CollideCompoundVsShapeVisitor;
+
+		JPH_INLINE bool		ShouldVisitNode([[maybe_unused]] int inStackTop) const
+		{
+			return true;
+		}
+
+		JPH_INLINE int		VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, [[maybe_unused]] int inStackTop) const
+		{
+			// Test which nodes collide
+			UVec4 collides = TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
+			return CountAndSortTrues(collides, ioProperties);
+		}
+	};
+
+	Visitor visitor(shape1, inShape2, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter);
+	shape1->WalkTree(visitor);
+}
+
+void StaticCompoundShape::sCollideShapeVsCompound(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter)
+{
+	JPH_PROFILE_FUNCTION();
+
+	struct Visitor : public CollideShapeVsCompoundVisitor
+	{
+		using CollideShapeVsCompoundVisitor::CollideShapeVsCompoundVisitor;
+
+		JPH_INLINE bool		ShouldVisitNode([[maybe_unused]] int inStackTop) const
+		{
+			return true;
+		}
+
+		JPH_INLINE int		VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, [[maybe_unused]] int inStackTop) const
+		{
+			// Test which nodes collide
+			UVec4 collides = TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
+			return CountAndSortTrues(collides, ioProperties);
+		}
+	};
+
+	JPH_ASSERT(inShape2->GetSubType() == EShapeSubType::StaticCompound);
+	const StaticCompoundShape *shape2 = static_cast<const StaticCompoundShape *>(inShape2);
+
+	Visitor visitor(inShape1, shape2, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter);
+	shape2->WalkTree(visitor);
+}
+
+void StaticCompoundShape::SaveBinaryState(StreamOut &inStream) const
+{
+	CompoundShape::SaveBinaryState(inStream);
+
+	inStream.Write(mNodes);
+}
+
+void StaticCompoundShape::RestoreBinaryState(StreamIn &inStream)
+{
+	CompoundShape::RestoreBinaryState(inStream);
+
+	inStream.Read(mNodes);
+}
+
+void StaticCompoundShape::sRegister()
+{
+	ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::StaticCompound);
+	f.mConstruct = []() -> Shape * { return new StaticCompoundShape; };
+	f.mColor = Color::sOrange;
+
+	for (EShapeSubType s : sAllSubShapeTypes)
+	{
+		CollisionDispatch::sRegisterCollideShape(EShapeSubType::StaticCompound, s, sCollideCompoundVsShape);
+		CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::StaticCompound, sCollideShapeVsCompound);
+		CollisionDispatch::sRegisterCastShape(s, EShapeSubType::StaticCompound, sCastShapeVsCompound);
+	}
+}
+
+JPH_NAMESPACE_END