initial commit, 4.5 stable

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

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

@@ -0,0 +1,139 @@
+// 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/SortReverseAndStore.h>
+#include <Jolt/Math/HalfFloat.h>
+
+JPH_NAMESPACE_BEGIN
+
+class CollideShapeSettings;
+class TempAllocator;
+
+/// Class that constructs a StaticCompoundShape. Note that if you only want a compound of 1 shape, use a RotatedTranslatedShape instead.
+class JPH_EXPORT StaticCompoundShapeSettings final : public CompoundShapeSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, StaticCompoundShapeSettings)
+
+public:
+	// See: ShapeSettings
+	virtual ShapeResult				Create() const override;
+
+	/// Specialization of Create() function that allows specifying a temp allocator to avoid temporary memory allocations on the heap
+	ShapeResult						Create(TempAllocator &inTempAllocator) const;
+};
+
+/// A compound shape, sub shapes can be rotated and translated.
+/// Sub shapes cannot be modified once the shape is constructed.
+/// Shifts all child objects so that they're centered around the center of mass.
+class JPH_EXPORT StaticCompoundShape final : public CompoundShape
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+									StaticCompoundShape() : CompoundShape(EShapeSubType::StaticCompound) { }
+									StaticCompoundShape(const StaticCompoundShapeSettings &inSettings, TempAllocator &inTempAllocator, ShapeResult &outResult);
+
+	// 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) + mNodes.size() * sizeof(Node), 0); }
+
+	// 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 GetIntersectingSubShapesVisitorSC : public GetIntersectingSubShapesVisitor<BoxType>
+	{
+		using GetIntersectingSubShapesVisitor<BoxType>::GetIntersectingSubShapesVisitor;
+
+		JPH_INLINE bool				ShouldVisitNode(int inStackTop) const
+		{
+			return true;
+		}
+
+		JPH_INLINE int				VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop)
+		{
+			// Test if point overlaps with box
+			UVec4 collides = GetIntersectingSubShapesVisitor<BoxType>::TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
+			return CountAndSortTrues(collides, ioProperties);
+		}
+	};
+
+	/// Sorts ioBodyIdx spatially into 2 groups. Second groups starts at ioBodyIdx + outMidPoint.
+	/// After the function returns ioBodyIdx and ioBounds will be shuffled
+	static void						sPartition(uint *ioBodyIdx, AABox *ioBounds, int inNumber, int &outMidPoint);
+
+	/// Sorts ioBodyIdx 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 ioBodyIdx and ioBounds will be shuffled
+	static void						sPartition4(uint *ioBodyIdx, AABox *ioBounds, int inBegin, int inEnd, int *outSplit);
+
+	// 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);
+
+	// Maximum size of the stack during tree walk
+	static constexpr int			cStackSize = 128;
+
+	template <class Visitor>
+	JPH_INLINE void					WalkTree(Visitor &ioVisitor) const;						///< Walk the node tree calling the Visitor::VisitNodes for each node encountered and Visitor::VisitShape for each sub shape encountered
+
+	/// Bits used in Node::mNodeProperties
+	enum : uint32
+	{
+		IS_SUBSHAPE					= 0x80000000,											///< If this bit is set, the other bits index in mSubShape, otherwise in mNodes
+		INVALID_NODE				= 0x7fffffff,											///< Signifies an invalid node
+	};
+
+	/// Node structure
+	struct Node
+	{
+		void						SetChildBounds(uint inIndex, const AABox &inBounds);	///< Set bounding box for child inIndex to inBounds
+		void						SetChildInvalid(uint inIndex);							///< Mark the child inIndex as invalid and set its bounding box to invalid
+
+		HalfFloat					mBoundsMinX[4];											///< 4 child bounding boxes
+		HalfFloat					mBoundsMinY[4];
+		HalfFloat					mBoundsMinZ[4];
+		HalfFloat					mBoundsMaxX[4];
+		HalfFloat					mBoundsMaxY[4];
+		HalfFloat					mBoundsMaxZ[4];
+		uint32						mNodeProperties[4];										///< 4 child node properties
+	};
+
+	static_assert(sizeof(Node) == 64, "Node should be 64 bytes");
+
+	using Nodes = Array<Node>;
+
+	Nodes							mNodes;													///< Quad tree node structure
+};
+
+JPH_NAMESPACE_END

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

@@ -0,0 +1,138 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+/// @brief A sub shape id contains a path to an element (usually a triangle or other primitive type) of a compound shape
+///
+/// Each sub shape knows how many bits it needs to encode its ID, so knows how many bits to take from the sub shape ID.
+///
+/// For example:
+/// * We have a CompoundShape A with 5 child shapes (identify sub shape using 3 bits AAA)
+/// * One of its child shapes is CompoundShape B which has 3 child shapes (identify sub shape using 2 bits BB)
+/// * One of its child shapes is MeshShape C which contains enough triangles to need 7 bits to identify a triangle (identify sub shape using 7 bits CCCCCCC, note that MeshShape is block based and sorts triangles spatially, you can't assume that the first triangle will have bit pattern 0000000).
+///
+/// The bit pattern of the sub shape ID to identify a triangle in MeshShape C will then be CCCCCCCBBAAA.
+///
+/// A sub shape ID will become invalid when the structure of the shape changes. For example, if a child shape is removed from a compound shape, the sub shape ID will no longer be valid.
+/// This can be a problem when caching sub shape IDs from one frame to the next. See comments at ContactListener::OnContactPersisted / OnContactRemoved.
+class SubShapeID
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Underlying storage type
+	using Type = uint32;
+
+	/// Type that is bigger than the underlying storage type for operations that would otherwise overflow
+	using BiggerType = uint64;
+
+	static_assert(sizeof(BiggerType) > sizeof(Type), "The calculation below assumes BiggerType is a bigger type than Type");
+
+	/// How many bits we can store in this ID
+	static constexpr uint MaxBits = 8 * sizeof(Type);
+
+	/// Constructor
+						SubShapeID() = default;
+
+	/// Get the next id in the chain of ids (pops parents before children)
+	Type				PopID(uint inBits, SubShapeID &outRemainder) const
+	{
+		Type mask_bits = Type((BiggerType(1) << inBits) - 1);
+		Type fill_bits = Type(BiggerType(cEmpty) << (MaxBits - inBits)); // Fill left side bits with 1 so that if there's no remainder all bits will be set, note that we do this using a BiggerType since on intel 0xffffffff << 32 == 0xffffffff
+		Type v = mValue & mask_bits;
+		outRemainder = SubShapeID(Type(BiggerType(mValue) >> inBits) | fill_bits);
+		return v;
+	}
+
+	/// Get the value of the path to the sub shape ID
+	inline Type			GetValue() const
+	{
+		return mValue;
+	}
+
+	/// Set the value of the sub shape ID (use with care!)
+	inline void			SetValue(Type inValue)
+	{
+		mValue = inValue;
+	}
+
+	/// Check if there is any bits of subshape ID left.
+	/// Note that this is not a 100% guarantee as the subshape ID could consist of all 1 bits. Use for asserts only.
+	inline bool			IsEmpty() const
+	{
+		return mValue == cEmpty;
+	}
+
+	/// Check equal
+	inline bool			operator == (const SubShapeID &inRHS) const
+	{
+		return mValue == inRHS.mValue;
+	}
+
+	/// Check not-equal
+	inline bool			operator != (const SubShapeID &inRHS) const
+	{
+		return mValue != inRHS.mValue;
+	}
+
+private:
+	friend class SubShapeIDCreator;
+
+	/// An empty SubShapeID has all bits set
+	static constexpr Type cEmpty = ~Type(0);
+
+	/// Constructor
+	explicit			SubShapeID(const Type &inValue) : mValue(inValue) { }
+
+	/// Adds an id at a particular position in the chain
+	/// (this should really only be called by the SubShapeIDCreator)
+	void				PushID(Type inValue, uint inFirstBit, uint inBits)
+	{
+		// First clear the bits
+		mValue &= ~(Type((BiggerType(1) << inBits) - 1) << inFirstBit);
+
+		// Then set them to the new value
+		mValue |= inValue << inFirstBit;
+	}
+
+	Type				mValue = cEmpty;
+};
+
+/// A sub shape id creator can be used to create a new sub shape id by recursing through the shape
+/// hierarchy and pushing new ID's onto the chain
+class SubShapeIDCreator
+{
+public:
+	/// Add a new id to the chain of id's and return it
+	SubShapeIDCreator	PushID(uint inValue, uint inBits) const
+	{
+		JPH_ASSERT(inValue < (SubShapeID::BiggerType(1) << inBits));
+		SubShapeIDCreator copy = *this;
+		copy.mID.PushID(inValue, mCurrentBit, inBits);
+		copy.mCurrentBit += inBits;
+		JPH_ASSERT(copy.mCurrentBit <= SubShapeID::MaxBits);
+		return copy;
+	}
+
+	// Get the resulting sub shape ID
+	const SubShapeID &	GetID() const
+	{
+		return mID;
+	}
+
+	/// Get the number of bits that have been written to the sub shape ID so far
+	inline uint			GetNumBitsWritten() const
+	{
+		return mCurrentBit;
+	}
+
+private:
+	SubShapeID			mID;
+	uint				mCurrentBit = 0;
+};
+
+JPH_NAMESPACE_END

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

@@ -0,0 +1,65 @@
+// 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>
+#include <Jolt/Core/HashCombine.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// A pair of bodies and their sub shape ID's. Can be used as a key in a map to find a contact point.
+class SubShapeIDPair
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+							SubShapeIDPair() = default;
+							SubShapeIDPair(const BodyID &inBody1ID, const SubShapeID &inSubShapeID1, const BodyID &inBody2ID, const SubShapeID &inSubShapeID2) : mBody1ID(inBody1ID), mSubShapeID1(inSubShapeID1), mBody2ID(inBody2ID), mSubShapeID2(inSubShapeID2) { }
+	SubShapeIDPair &		operator = (const SubShapeIDPair &) = default;
+							SubShapeIDPair(const SubShapeIDPair &) = default;
+
+	/// Equality operator
+	inline bool				operator == (const SubShapeIDPair &inRHS) const
+	{
+		return UVec4::sLoadInt4(reinterpret_cast<const uint32 *>(this)) == UVec4::sLoadInt4(reinterpret_cast<const uint32 *>(&inRHS));
+	}
+
+	/// Less than operator, used to consistently order contact points for a deterministic simulation
+	inline bool				operator < (const SubShapeIDPair &inRHS) const
+	{
+		if (mBody1ID != inRHS.mBody1ID)
+			return mBody1ID < inRHS.mBody1ID;
+
+		if (mSubShapeID1.GetValue() != inRHS.mSubShapeID1.GetValue())
+			return mSubShapeID1.GetValue() < inRHS.mSubShapeID1.GetValue();
+
+		if (mBody2ID != inRHS.mBody2ID)
+			return mBody2ID < inRHS.mBody2ID;
+
+		return mSubShapeID2.GetValue() < inRHS.mSubShapeID2.GetValue();
+	}
+
+	const BodyID &			GetBody1ID() const				{ return mBody1ID; }
+	const SubShapeID &		GetSubShapeID1() const			{ return mSubShapeID1; }
+	const BodyID &			GetBody2ID() const				{ return mBody2ID; }
+	const SubShapeID &		GetSubShapeID2() const			{ return mSubShapeID2; }
+
+	uint64					GetHash() const					{ return HashBytes(this, sizeof(SubShapeIDPair)); }
+
+private:
+	BodyID					mBody1ID;
+	SubShapeID				mSubShapeID1;
+	BodyID					mBody2ID;
+	SubShapeID				mSubShapeID2;
+};
+
+static_assert(sizeof(SubShapeIDPair) == 16, "Unexpected size");
+static_assert(alignof(SubShapeIDPair) == 4, "Assuming 4 byte aligned");
+
+JPH_NAMESPACE_END
+
+JPH_MAKE_STD_HASH(JPH::SubShapeIDPair)

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

@@ -0,0 +1,453 @@
+// 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/TaperedCapsuleShape.h>
+#include <Jolt/Physics/Collision/Shape/SphereShape.h>
+#include <Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h>
+#include <Jolt/Physics/Collision/Shape/ScaleHelpers.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(TaperedCapsuleShapeSettings)
+{
+	JPH_ADD_BASE_CLASS(TaperedCapsuleShapeSettings, ConvexShapeSettings)
+
+	JPH_ADD_ATTRIBUTE(TaperedCapsuleShapeSettings, mHalfHeightOfTaperedCylinder)
+	JPH_ADD_ATTRIBUTE(TaperedCapsuleShapeSettings, mTopRadius)
+	JPH_ADD_ATTRIBUTE(TaperedCapsuleShapeSettings, mBottomRadius)
+}
+
+bool TaperedCapsuleShapeSettings::IsSphere() const
+{
+	return max(mTopRadius, mBottomRadius) >= 2.0f * mHalfHeightOfTaperedCylinder + min(mTopRadius, mBottomRadius);
+}
+
+ShapeSettings::ShapeResult TaperedCapsuleShapeSettings::Create() const
+{
+	if (mCachedResult.IsEmpty())
+	{
+		Ref<Shape> shape;
+		if (IsValid() && IsSphere())
+		{
+			// Determine sphere center and radius
+			float radius, center;
+			if (mTopRadius > mBottomRadius)
+			{
+				radius = mTopRadius;
+				center = mHalfHeightOfTaperedCylinder;
+			}
+			else
+			{
+				radius = mBottomRadius;
+				center = -mHalfHeightOfTaperedCylinder;
+			}
+
+			// Create sphere
+			shape = new SphereShape(radius, mMaterial);
+
+			// Offset sphere if needed
+			if (abs(center) > 1.0e-6f)
+			{
+				RotatedTranslatedShapeSettings rot_trans(Vec3(0, center, 0), Quat::sIdentity(), shape);
+				mCachedResult = rot_trans.Create();
+			}
+			else
+				mCachedResult.Set(shape);
+		}
+		else
+		{
+			// Normal tapered capsule shape
+			shape = new TaperedCapsuleShape(*this, mCachedResult);
+		}
+	}
+	return mCachedResult;
+}
+
+TaperedCapsuleShapeSettings::TaperedCapsuleShapeSettings(float inHalfHeightOfTaperedCylinder, float inTopRadius, float inBottomRadius, const PhysicsMaterial *inMaterial) :
+	ConvexShapeSettings(inMaterial),
+	mHalfHeightOfTaperedCylinder(inHalfHeightOfTaperedCylinder),
+	mTopRadius(inTopRadius),
+	mBottomRadius(inBottomRadius)
+{
+}
+
+TaperedCapsuleShape::TaperedCapsuleShape(const TaperedCapsuleShapeSettings &inSettings, ShapeResult &outResult) :
+	ConvexShape(EShapeSubType::TaperedCapsule, inSettings, outResult),
+	mTopRadius(inSettings.mTopRadius),
+	mBottomRadius(inSettings.mBottomRadius)
+{
+	if (mTopRadius <= 0.0f)
+	{
+		outResult.SetError("Invalid top radius");
+		return;
+	}
+
+	if (mBottomRadius <= 0.0f)
+	{
+		outResult.SetError("Invalid bottom radius");
+		return;
+	}
+
+	if (inSettings.mHalfHeightOfTaperedCylinder <= 0.0f)
+	{
+		outResult.SetError("Invalid height");
+		return;
+	}
+
+	// If this goes off one of the sphere ends falls totally inside the other and you should use a sphere instead
+	if (inSettings.IsSphere())
+	{
+		outResult.SetError("One sphere embedded in other sphere, please use sphere shape instead");
+		return;
+	}
+
+	// Approximation: The center of mass is exactly half way between the top and bottom cap of the tapered capsule
+	mTopCenter = inSettings.mHalfHeightOfTaperedCylinder + 0.5f * (mBottomRadius - mTopRadius);
+	mBottomCenter = -inSettings.mHalfHeightOfTaperedCylinder + 0.5f * (mBottomRadius - mTopRadius);
+
+	// Calculate center of mass
+	mCenterOfMass = Vec3(0, inSettings.mHalfHeightOfTaperedCylinder - mTopCenter, 0);
+
+	// Calculate convex radius
+	mConvexRadius = min(mTopRadius, mBottomRadius);
+	JPH_ASSERT(mConvexRadius > 0.0f);
+
+	// Calculate the sin and tan of the angle that the cone surface makes with the Y axis
+	// See: TaperedCapsuleShape.gliffy
+	mSinAlpha = (mBottomRadius - mTopRadius) / (mTopCenter - mBottomCenter);
+	JPH_ASSERT(mSinAlpha >= -1.0f && mSinAlpha <= 1.0f);
+	mTanAlpha = Tan(ASin(mSinAlpha));
+
+	outResult.Set(this);
+}
+
+class TaperedCapsuleShape::TaperedCapsule final : public Support
+{
+public:
+					TaperedCapsule(Vec3Arg inTopCenter, Vec3Arg inBottomCenter, float inTopRadius, float inBottomRadius, float inConvexRadius) :
+		mTopCenter(inTopCenter),
+		mBottomCenter(inBottomCenter),
+		mTopRadius(inTopRadius),
+		mBottomRadius(inBottomRadius),
+		mConvexRadius(inConvexRadius)
+	{
+		static_assert(sizeof(TaperedCapsule) <= sizeof(SupportBuffer), "Buffer size too small");
+		JPH_ASSERT(IsAligned(this, alignof(TaperedCapsule)));
+	}
+
+	virtual Vec3	GetSupport(Vec3Arg inDirection) const override
+	{
+		// Check zero vector
+		float len = inDirection.Length();
+		if (len == 0.0f)
+			return mTopCenter + Vec3(0, mTopRadius, 0); // Return top
+
+		// Check if the support of the top sphere or bottom sphere is bigger
+		Vec3 support_top = mTopCenter + (mTopRadius / len) * inDirection;
+		Vec3 support_bottom = mBottomCenter + (mBottomRadius / len) * inDirection;
+		if (support_top.Dot(inDirection) > support_bottom.Dot(inDirection))
+			return support_top;
+		else
+			return support_bottom;
+	}
+
+	virtual float	GetConvexRadius() const override
+	{
+		return mConvexRadius;
+	}
+
+private:
+	Vec3			mTopCenter;
+	Vec3			mBottomCenter;
+	float			mTopRadius;
+	float			mBottomRadius;
+	float			mConvexRadius;
+};
+
+const ConvexShape::Support *TaperedCapsuleShape::GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const
+{
+	JPH_ASSERT(IsValidScale(inScale));
+
+	// Get scaled tapered capsule
+	Vec3 abs_scale = inScale.Abs();
+	float scale_xz = abs_scale.GetX();
+	float scale_y = inScale.GetY(); // The sign of y is important as it flips the tapered capsule
+	Vec3 scaled_top_center = Vec3(0, scale_y * mTopCenter, 0);
+	Vec3 scaled_bottom_center = Vec3(0, scale_y * mBottomCenter, 0);
+	float scaled_top_radius = scale_xz * mTopRadius;
+	float scaled_bottom_radius = scale_xz * mBottomRadius;
+	float scaled_convex_radius = scale_xz * mConvexRadius;
+
+	switch (inMode)
+	{
+	case ESupportMode::IncludeConvexRadius:
+		return new (&inBuffer) TaperedCapsule(scaled_top_center, scaled_bottom_center, scaled_top_radius, scaled_bottom_radius, 0.0f);
+
+	case ESupportMode::ExcludeConvexRadius:
+	case ESupportMode::Default:
+		{
+			// Get radii reduced by convex radius
+			float tr = scaled_top_radius - scaled_convex_radius;
+			float br = scaled_bottom_radius - scaled_convex_radius;
+			JPH_ASSERT(tr >= 0.0f && br >= 0.0f);
+			JPH_ASSERT(tr == 0.0f || br == 0.0f, "Convex radius should be that of the smallest sphere");
+			return new (&inBuffer) TaperedCapsule(scaled_top_center, scaled_bottom_center, tr, br, scaled_convex_radius);
+		}
+	}
+
+	JPH_ASSERT(false);
+	return nullptr;
+}
+
+void TaperedCapsuleShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const
+{
+	JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID");
+	JPH_ASSERT(IsValidScale(inScale));
+
+	// Check zero vector
+	float len = inDirection.Length();
+	if (len == 0.0f)
+		return;
+
+	// Get scaled tapered capsule
+	Vec3 abs_scale = inScale.Abs();
+	float scale_xz = abs_scale.GetX();
+	float scale_y = inScale.GetY(); // The sign of y is important as it flips the tapered capsule
+	Vec3 scaled_top_center = Vec3(0, scale_y * mTopCenter, 0);
+	Vec3 scaled_bottom_center = Vec3(0, scale_y * mBottomCenter, 0);
+	float scaled_top_radius = scale_xz * mTopRadius;
+	float scaled_bottom_radius = scale_xz * mBottomRadius;
+
+	// Get support point for top and bottom sphere in the opposite of inDirection (including convex radius)
+	Vec3 support_top = scaled_top_center - (scaled_top_radius / len) * inDirection;
+	Vec3 support_bottom = scaled_bottom_center - (scaled_bottom_radius / len) * inDirection;
+
+	// Get projection on inDirection
+	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 * len)
+	{
+		outVertices.push_back(inCenterOfMassTransform * support_top);
+		outVertices.push_back(inCenterOfMassTransform * support_bottom);
+	}
+}
+
+MassProperties TaperedCapsuleShape::GetMassProperties() const
+{
+	AABox box = GetInertiaApproximation();
+
+	MassProperties p;
+	p.SetMassAndInertiaOfSolidBox(box.GetSize(), GetDensity());
+	return p;
+}
+
+Vec3 TaperedCapsuleShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const
+{
+	JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID");
+
+	// See: TaperedCapsuleShape.gliffy
+	// We need to calculate ty and by in order to see if the position is on the top or bottom sphere
+	// sin(alpha) = by / br = ty / tr
+	// => by = sin(alpha) * br, ty = sin(alpha) * tr
+
+	if (inLocalSurfacePosition.GetY() > mTopCenter + mSinAlpha * mTopRadius)
+		return (inLocalSurfacePosition - Vec3(0, mTopCenter, 0)).Normalized();
+	else if (inLocalSurfacePosition.GetY() < mBottomCenter + mSinAlpha * mBottomRadius)
+		return (inLocalSurfacePosition - Vec3(0, mBottomCenter, 0)).Normalized();
+	else
+	{
+		// Get perpendicular vector to the surface in the xz plane
+		Vec3 perpendicular = Vec3(inLocalSurfacePosition.GetX(), 0, inLocalSurfacePosition.GetZ()).NormalizedOr(Vec3::sAxisX());
+
+		// We know that the perpendicular has length 1 and that it needs a y component where tan(alpha) = y / 1 in order to align it to the surface
+		perpendicular.SetY(mTanAlpha);
+		return perpendicular.Normalized();
+	}
+}
+
+AABox TaperedCapsuleShape::GetLocalBounds() const
+{
+	float max_radius = max(mTopRadius, mBottomRadius);
+	return AABox(Vec3(-max_radius, mBottomCenter - mBottomRadius, -max_radius), Vec3(max_radius, mTopCenter + mTopRadius, max_radius));
+}
+
+AABox TaperedCapsuleShape::GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const
+{
+	JPH_ASSERT(IsValidScale(inScale));
+
+	Vec3 abs_scale = inScale.Abs();
+	float scale_xz = abs_scale.GetX();
+	float scale_y = inScale.GetY(); // The sign of y is important as it flips the tapered capsule
+	Vec3 bottom_extent = Vec3::sReplicate(scale_xz * mBottomRadius);
+	Vec3 bottom_center = inCenterOfMassTransform * Vec3(0, scale_y * mBottomCenter, 0);
+	Vec3 top_extent = Vec3::sReplicate(scale_xz * mTopRadius);
+	Vec3 top_center = inCenterOfMassTransform * Vec3(0, scale_y * mTopCenter, 0);
+	Vec3 p1 = Vec3::sMin(top_center - top_extent, bottom_center - bottom_extent);
+	Vec3 p2 = Vec3::sMax(top_center + top_extent, bottom_center + bottom_extent);
+	return AABox(p1, p2);
+}
+
+void TaperedCapsuleShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const
+{
+	JPH_ASSERT(IsValidScale(inScale));
+
+	Mat44 inverse_transform = inCenterOfMassTransform.InversedRotationTranslation();
+
+	// Get scaled tapered capsule
+	Vec3 abs_scale = inScale.Abs();
+	float scale_y = abs_scale.GetY();
+	float scale_xz = abs_scale.GetX();
+	Vec3 scale_y_flip(1, Sign(inScale.GetY()), 1);
+	Vec3 scaled_top_center(0, scale_y * mTopCenter, 0);
+	Vec3 scaled_bottom_center(0, scale_y * mBottomCenter, 0);
+	float scaled_top_radius = scale_xz * mTopRadius;
+	float scaled_bottom_radius = scale_xz * mBottomRadius;
+
+	for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v)
+		if (v.GetInvMass() > 0.0f)
+		{
+			Vec3 local_pos = scale_y_flip * (inverse_transform * v.GetPosition());
+
+			Vec3 position, normal;
+
+			// If the vertex is inside the cone starting at the top center pointing along the y-axis with angle PI/2 - alpha then the closest point is on the top sphere
+			// This corresponds to: Dot(y-axis, (local_pos - top_center) / |local_pos - top_center|) >= cos(PI/2 - alpha)
+			// <=> (local_pos - top_center).y >= sin(alpha) * |local_pos - top_center|
+			Vec3 top_center_to_local_pos = local_pos - scaled_top_center;
+			float top_center_to_local_pos_len = top_center_to_local_pos.Length();
+			if (top_center_to_local_pos.GetY() >= mSinAlpha * top_center_to_local_pos_len)
+			{
+				// Top sphere
+				normal = top_center_to_local_pos_len != 0.0f? top_center_to_local_pos / top_center_to_local_pos_len : Vec3::sAxisY();
+				position = scaled_top_center + scaled_top_radius * normal;
+			}
+			else
+			{
+				// If the vertex is outside the cone starting at the bottom center pointing along the y-axis with angle PI/2 - alpha then the closest point is on the bottom sphere
+				// This corresponds to: Dot(y-axis, (local_pos - bottom_center) / |local_pos - bottom_center|) <= cos(PI/2 - alpha)
+				// <=> (local_pos - bottom_center).y <= sin(alpha) * |local_pos - bottom_center|
+				Vec3 bottom_center_to_local_pos = local_pos - scaled_bottom_center;
+				float bottom_center_to_local_pos_len = bottom_center_to_local_pos.Length();
+				if (bottom_center_to_local_pos.GetY() <= mSinAlpha * bottom_center_to_local_pos_len)
+				{
+					// Bottom sphere
+					normal = bottom_center_to_local_pos_len != 0.0f? bottom_center_to_local_pos / bottom_center_to_local_pos_len : -Vec3::sAxisY();
+				}
+				else
+				{
+					// Tapered cylinder
+					normal = Vec3(local_pos.GetX(), 0, local_pos.GetZ()).NormalizedOr(Vec3::sAxisX());
+					normal.SetY(mTanAlpha);
+					normal = normal.NormalizedOr(Vec3::sAxisX());
+				}
+				position = scaled_bottom_center + scaled_bottom_radius * normal;
+			}
+
+			Plane plane = Plane::sFromPointAndNormal(position, normal);
+			float penetration = -plane.SignedDistance(local_pos);
+			if (v.UpdatePenetration(penetration))
+			{
+				// Need to flip the normal's y if capsule is flipped (this corresponds to flipping both the point and the normal around y)
+				plane.SetNormal(scale_y_flip * plane.GetNormal());
+
+				// Store collision
+				v.SetCollision(plane.GetTransformed(inCenterOfMassTransform), inCollidingShapeIndex);
+			}
+		}
+}
+
+#ifdef JPH_DEBUG_RENDERER
+void TaperedCapsuleShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
+{
+	if (mGeometry == nullptr)
+	{
+		SupportBuffer buffer;
+		const Support *support = GetSupportFunction(ESupportMode::IncludeConvexRadius, buffer, Vec3::sOne());
+		mGeometry = inRenderer->CreateTriangleGeometryForConvex([support](Vec3Arg inDirection) { return support->GetSupport(inDirection); });
+	}
+
+	// Preserve flip along y axis but make sure we're not inside out
+	Vec3 scale = ScaleHelpers::IsInsideOut(inScale)? Vec3(-1, 1, 1) * inScale : inScale;
+	RMat44 world_transform = inCenterOfMassTransform * Mat44::sScale(scale);
+
+	AABox bounds = Shape::GetWorldSpaceBounds(inCenterOfMassTransform, inScale);
+
+	float lod_scale_sq = Square(max(mTopRadius, mBottomRadius));
+
+	Color color = inUseMaterialColors? GetMaterial()->GetDebugColor() : inColor;
+
+	DebugRenderer::EDrawMode draw_mode = inDrawWireframe? DebugRenderer::EDrawMode::Wireframe : DebugRenderer::EDrawMode::Solid;
+
+	inRenderer->DrawGeometry(world_transform, bounds, lod_scale_sq, color, mGeometry, DebugRenderer::ECullMode::CullBackFace, DebugRenderer::ECastShadow::On, draw_mode);
+}
+#endif // JPH_DEBUG_RENDERER
+
+AABox TaperedCapsuleShape::GetInertiaApproximation() const
+{
+	// TODO: For now the mass and inertia is that of a box
+	float avg_radius = 0.5f * (mTopRadius + mBottomRadius);
+	return AABox(Vec3(-avg_radius, mBottomCenter - mBottomRadius, -avg_radius), Vec3(avg_radius, mTopCenter + mTopRadius, avg_radius));
+}
+
+void TaperedCapsuleShape::SaveBinaryState(StreamOut &inStream) const
+{
+	ConvexShape::SaveBinaryState(inStream);
+
+	inStream.Write(mCenterOfMass);
+	inStream.Write(mTopRadius);
+	inStream.Write(mBottomRadius);
+	inStream.Write(mTopCenter);
+	inStream.Write(mBottomCenter);
+	inStream.Write(mConvexRadius);
+	inStream.Write(mSinAlpha);
+	inStream.Write(mTanAlpha);
+}
+
+void TaperedCapsuleShape::RestoreBinaryState(StreamIn &inStream)
+{
+	ConvexShape::RestoreBinaryState(inStream);
+
+	inStream.Read(mCenterOfMass);
+	inStream.Read(mTopRadius);
+	inStream.Read(mBottomRadius);
+	inStream.Read(mTopCenter);
+	inStream.Read(mBottomCenter);
+	inStream.Read(mConvexRadius);
+	inStream.Read(mSinAlpha);
+	inStream.Read(mTanAlpha);
+}
+
+bool TaperedCapsuleShape::IsValidScale(Vec3Arg inScale) const
+{
+	return ConvexShape::IsValidScale(inScale) && ScaleHelpers::IsUniformScale(inScale.Abs());
+}
+
+Vec3 TaperedCapsuleShape::MakeScaleValid(Vec3Arg inScale) const
+{
+	Vec3 scale = ScaleHelpers::MakeNonZeroScale(inScale);
+
+	return scale.GetSign() * ScaleHelpers::MakeUniformScale(scale.Abs());
+}
+
+void TaperedCapsuleShape::sRegister()
+{
+	ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::TaperedCapsule);
+	f.mConstruct = []() -> Shape * { return new TaperedCapsuleShape; };
+	f.mColor = Color::sGreen;
+}
+
+JPH_NAMESPACE_END

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

@@ -0,0 +1,135 @@
+// 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>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+/// Class that constructs a TaperedCapsuleShape
+class JPH_EXPORT TaperedCapsuleShapeSettings final : public ConvexShapeSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, TaperedCapsuleShapeSettings)
+
+public:
+	/// Default constructor for deserialization
+							TaperedCapsuleShapeSettings() = default;
+
+	/// Create a tapered capsule centered around the origin with one sphere cap at (0, -inHalfHeightOfTaperedCylinder, 0) with radius inBottomRadius and the other at (0, inHalfHeightOfTaperedCylinder, 0) with radius inTopRadius
+							TaperedCapsuleShapeSettings(float inHalfHeightOfTaperedCylinder, float inTopRadius, float inBottomRadius, const PhysicsMaterial *inMaterial = nullptr);
+
+	/// Check if the settings are valid
+	bool					IsValid() const															{ return mTopRadius > 0.0f && mBottomRadius > 0.0f && mHalfHeightOfTaperedCylinder >= 0.0f; }
+
+	/// Checks if the settings of this tapered capsule make this shape a sphere
+	bool					IsSphere() const;
+
+	// See: ShapeSettings
+	virtual ShapeResult		Create() const override;
+
+	float					mHalfHeightOfTaperedCylinder = 0.0f;
+	float					mTopRadius = 0.0f;
+	float					mBottomRadius = 0.0f;
+};
+
+/// A capsule with different top and bottom radii
+class JPH_EXPORT TaperedCapsuleShape final : public ConvexShape
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+							TaperedCapsuleShape() : ConvexShape(EShapeSubType::TaperedCapsule) { }
+							TaperedCapsuleShape(const TaperedCapsuleShapeSettings &inSettings, ShapeResult &outResult);
+
+	/// Get top radius of the tapered capsule
+	inline float			GetTopRadius() const													{ return mTopRadius; }
+
+	/// Get bottom radius of the tapered capsule
+	inline float			GetBottomRadius() const													{ return mBottomRadius; }
+
+	/// Get half height between the top and bottom sphere center
+	inline float			GetHalfHeight() const													{ return 0.5f * (mTopCenter - mBottomCenter); }
+
+	// 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 min(mTopRadius, mBottomRadius); }
+
+	// 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::CollideSoftBodyVertices
+	virtual void			CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) 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
+	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 GetLocalBounds().GetVolume(); } // Volume is approximate!
+
+	// 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					TaperedCapsule;
+
+	/// Returns box that approximates the inertia
+	AABox					GetInertiaApproximation() const;
+
+	Vec3					mCenterOfMass = Vec3::sZero();
+	float					mTopRadius = 0.0f;
+	float					mBottomRadius = 0.0f;
+	float					mTopCenter = 0.0f;
+	float					mBottomCenter = 0.0f;
+	float					mConvexRadius = 0.0f;
+	float					mSinAlpha = 0.0f;
+	float					mTanAlpha = 0.0f;
+
+#ifdef JPH_DEBUG_RENDERER
+	mutable DebugRenderer::GeometryRef mGeometry;
+#endif // JPH_DEBUG_RENDERER
+};
+
+JPH_NAMESPACE_END

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

@@ -0,0 +1,703 @@
+// 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/TaperedCylinderShape.h>
+#include <Jolt/Physics/Collision/Shape/CylinderShape.h>
+#include <Jolt/Physics/Collision/Shape/ScaleHelpers.h>
+#include <Jolt/Physics/Collision/CollidePointResult.h>
+#include <Jolt/Physics/Collision/TransformedShape.h>
+#include <Jolt/Physics/Collision/CollideSoftBodyVertexIterator.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
+
+// Approximation of a face of the tapered cylinder
+static const Vec3 cTaperedCylinderFace[] =
+{
+	Vec3(0.0f,			0.0f,	1.0f),
+	Vec3(0.707106769f,	0.0f,	0.707106769f),
+	Vec3(1.0f,			0.0f,	0.0f),
+	Vec3(0.707106769f,	0.0f,	-0.707106769f),
+	Vec3(-0.0f,			0.0f,	-1.0f),
+	Vec3(-0.707106769f,	0.0f,	-0.707106769f),
+	Vec3(-1.0f,			0.0f,	0.0f),
+	Vec3(-0.707106769f,	0.0f,	0.707106769f)
+};
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(TaperedCylinderShapeSettings)
+{
+	JPH_ADD_BASE_CLASS(TaperedCylinderShapeSettings, ConvexShapeSettings)
+
+	JPH_ADD_ATTRIBUTE(TaperedCylinderShapeSettings, mHalfHeight)
+	JPH_ADD_ATTRIBUTE(TaperedCylinderShapeSettings, mTopRadius)
+	JPH_ADD_ATTRIBUTE(TaperedCylinderShapeSettings, mBottomRadius)
+	JPH_ADD_ATTRIBUTE(TaperedCylinderShapeSettings, mConvexRadius)
+}
+
+ShapeSettings::ShapeResult TaperedCylinderShapeSettings::Create() const
+{
+	if (mCachedResult.IsEmpty())
+	{
+		Ref<Shape> shape;
+		if (mTopRadius == mBottomRadius)
+		{
+			// Convert to regular cylinder
+			CylinderShapeSettings settings;
+			settings.mHalfHeight = mHalfHeight;
+			settings.mRadius = mTopRadius;
+			settings.mMaterial = mMaterial;
+			settings.mConvexRadius = mConvexRadius;
+			new CylinderShape(settings, mCachedResult);
+		}
+		else
+		{
+			// Normal tapered cylinder shape
+			new TaperedCylinderShape(*this, mCachedResult);
+		}
+	}
+	return mCachedResult;
+}
+
+TaperedCylinderShapeSettings::TaperedCylinderShapeSettings(float inHalfHeightOfTaperedCylinder, float inTopRadius, float inBottomRadius, float inConvexRadius, const PhysicsMaterial *inMaterial) :
+	ConvexShapeSettings(inMaterial),
+	mHalfHeight(inHalfHeightOfTaperedCylinder),
+	mTopRadius(inTopRadius),
+	mBottomRadius(inBottomRadius),
+	mConvexRadius(inConvexRadius)
+{
+}
+
+TaperedCylinderShape::TaperedCylinderShape(const TaperedCylinderShapeSettings &inSettings, ShapeResult &outResult) :
+	ConvexShape(EShapeSubType::TaperedCylinder, inSettings, outResult),
+	mTopRadius(inSettings.mTopRadius),
+	mBottomRadius(inSettings.mBottomRadius),
+	mConvexRadius(inSettings.mConvexRadius)
+{
+	if (mTopRadius < 0.0f)
+	{
+		outResult.SetError("Invalid top radius");
+		return;
+	}
+
+	if (mBottomRadius < 0.0f)
+	{
+		outResult.SetError("Invalid bottom radius");
+		return;
+	}
+
+	if (inSettings.mHalfHeight <= 0.0f)
+	{
+		outResult.SetError("Invalid height");
+		return;
+	}
+
+	if (inSettings.mConvexRadius < 0.0f)
+	{
+		outResult.SetError("Invalid convex radius");
+		return;
+	}
+
+	if (inSettings.mTopRadius < inSettings.mConvexRadius)
+	{
+		outResult.SetError("Convex radius must be smaller than convex radius");
+		return;
+	}
+
+	if (inSettings.mBottomRadius < inSettings.mConvexRadius)
+	{
+		outResult.SetError("Convex radius must be smaller than bottom radius");
+		return;
+	}
+
+	// Calculate the center of mass (using wxMaxima).
+	// Radius of cross section for tapered cylinder from 0 to h:
+	// r(x):=br+x*(tr-br)/h;
+	// Area:
+	// area(x):=%pi*r(x)^2;
+	// Total volume of cylinder:
+	// volume(h):=integrate(area(x),x,0,h);
+	// Center of mass:
+	// com(br,tr,h):=integrate(x*area(x),x,0,h)/volume(h);
+	// Results:
+	// ratsimp(com(br,tr,h),br,bt);
+	// Non-tapered cylinder should have com = 0.5:
+	// ratsimp(com(r,r,h));
+	// Cone with tip at origin and height h should have com = 3/4 h
+	// ratsimp(com(0,r,h));
+	float h = 2.0f * inSettings.mHalfHeight;
+	float tr = mTopRadius;
+	float tr2 = Square(tr);
+	float br = mBottomRadius;
+	float br2 = Square(br);
+	float com = h * (3 * tr2 + 2 * br * tr + br2) / (4.0f * (tr2 + br * tr + br2));
+	mTop = h - com;
+	mBottom = -com;
+
+	outResult.Set(this);
+}
+
+class TaperedCylinderShape::TaperedCylinder final : public Support
+{
+public:
+					TaperedCylinder(float inTop, float inBottom, float inTopRadius, float inBottomRadius, float inConvexRadius) :
+		mTop(inTop),
+		mBottom(inBottom),
+		mTopRadius(inTopRadius),
+		mBottomRadius(inBottomRadius),
+		mConvexRadius(inConvexRadius)
+	{
+		static_assert(sizeof(TaperedCylinder) <= sizeof(SupportBuffer), "Buffer size too small");
+		JPH_ASSERT(IsAligned(this, alignof(TaperedCylinder)));
+	}
+
+	virtual Vec3	GetSupport(Vec3Arg inDirection) const override
+	{
+		float x = inDirection.GetX(), y = inDirection.GetY(), z = inDirection.GetZ();
+		float o = sqrt(Square(x) + Square(z));
+		if (o > 0.0f)
+		{
+			Vec3 top_support((mTopRadius * x) / o, mTop, (mTopRadius * z) / o);
+			Vec3 bottom_support((mBottomRadius * x) / o, mBottom, (mBottomRadius * z) / o);
+			return inDirection.Dot(top_support) > inDirection.Dot(bottom_support)? top_support : bottom_support;
+		}
+		else
+		{
+			if (y > 0.0f)
+				return Vec3(0, mTop, 0);
+			else
+				return Vec3(0, mBottom, 0);
+		}
+	}
+
+	virtual float	GetConvexRadius() const override
+	{
+		return mConvexRadius;
+	}
+
+private:
+	float			mTop;
+	float			mBottom;
+	float			mTopRadius;
+	float			mBottomRadius;
+	float			mConvexRadius;
+};
+
+JPH_INLINE void TaperedCylinderShape::GetScaled(Vec3Arg inScale, float &outTop, float &outBottom, float &outTopRadius, float &outBottomRadius, float &outConvexRadius) const
+{
+	Vec3 abs_scale = inScale.Abs();
+	float scale_xz = abs_scale.GetX();
+	float scale_y = inScale.GetY();
+
+	outTop = scale_y * mTop;
+	outBottom = scale_y * mBottom;
+	outTopRadius = scale_xz * mTopRadius;
+	outBottomRadius = scale_xz * mBottomRadius;
+	outConvexRadius = min(abs_scale.GetY(), scale_xz) * mConvexRadius;
+
+	// Negative Y-scale flips the top and bottom
+	if (outBottom > outTop)
+	{
+		std::swap(outTop, outBottom);
+		std::swap(outTopRadius, outBottomRadius);
+	}
+}
+
+const ConvexShape::Support *TaperedCylinderShape::GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const
+{
+	JPH_ASSERT(IsValidScale(inScale));
+
+	// Get scaled tapered cylinder
+	float top, bottom, top_radius, bottom_radius, convex_radius;
+	GetScaled(inScale, top, bottom, top_radius, bottom_radius, convex_radius);
+
+	switch (inMode)
+	{
+	case ESupportMode::IncludeConvexRadius:
+	case ESupportMode::Default:
+		return new (&inBuffer) TaperedCylinder(top, bottom, top_radius, bottom_radius, 0.0f);
+
+	case ESupportMode::ExcludeConvexRadius:
+		return new (&inBuffer) TaperedCylinder(top - convex_radius, bottom + convex_radius, top_radius - convex_radius, bottom_radius - convex_radius, convex_radius);
+	}
+
+	JPH_ASSERT(false);
+	return nullptr;
+}
+
+JPH_INLINE static Vec3 sCalculateSideNormalXZ(Vec3Arg inSurfacePosition)
+{
+	return (Vec3(1, 0, 1) * inSurfacePosition).NormalizedOr(Vec3::sAxisX());
+}
+
+JPH_INLINE static Vec3 sCalculateSideNormal(Vec3Arg inNormalXZ, float inTop, float inBottom, float inTopRadius, float inBottomRadius)
+{
+	float tan_alpha = (inBottomRadius - inTopRadius) / (inTop - inBottom);
+	return Vec3(inNormalXZ.GetX(), tan_alpha, inNormalXZ.GetZ()).Normalized();
+}
+
+void TaperedCylinderShape::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 tapered cylinder
+	float top, bottom, top_radius, bottom_radius, convex_radius;
+	GetScaled(inScale, top, bottom, top_radius, bottom_radius, convex_radius);
+
+	// Get the normal of the side of the cylinder
+	Vec3 normal_xz = sCalculateSideNormalXZ(-inDirection);
+	Vec3 normal = sCalculateSideNormal(normal_xz, top, bottom, top_radius, bottom_radius);
+
+	constexpr float cMinRadius = 1.0e-3f;
+
+	// Check if the normal is closer to the side than to the top or bottom
+	if (abs(normal.Dot(inDirection)) > abs(inDirection.GetY()))
+	{
+		// Return the side of the cylinder
+		outVertices.push_back(inCenterOfMassTransform * (normal_xz * top_radius + Vec3(0, top, 0)));
+		outVertices.push_back(inCenterOfMassTransform * (normal_xz * bottom_radius + Vec3(0, bottom, 0)));
+	}
+	else
+	{
+		// 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;
+		Vec4 base_x = Vec4(inDirection.GetX(), 0, inDirection.GetZ(), 0);
+		float xz_sq = base_x.LengthSq();
+		float y_sq = Square(inDirection.GetY());
+		if (xz_sq > 0.00765427f * y_sq)
+		{
+			base_x /= 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));
+		}
+
+		if (inDirection.GetY() < 0.0f)
+		{
+			// Top of the cylinder
+			if (top_radius > cMinRadius)
+			{
+				Vec3 top_3d(0, top, 0);
+				for (Vec3 v : cTaperedCylinderFace)
+					outVertices.push_back(transform * (top_radius * v + top_3d));
+			}
+		}
+		else
+		{
+			// Bottom of the cylinder
+			if (bottom_radius > cMinRadius)
+			{
+				Vec3 bottom_3d(0, bottom, 0);
+				for (const Vec3 *v = cTaperedCylinderFace + std::size(cTaperedCylinderFace) - 1; v >= cTaperedCylinderFace; --v)
+					outVertices.push_back(transform * (bottom_radius * *v + bottom_3d));
+			}
+		}
+	}
+}
+
+MassProperties TaperedCylinderShape::GetMassProperties() const
+{
+	MassProperties p;
+
+	// Calculate mass
+	float density = GetDensity();
+	p.mMass = GetVolume() * density;
+
+	// Calculate inertia of a tapered cylinder (using wxMaxima)
+	// Radius:
+	// r(x):=br+(x-b)*(tr-br)/(t-b);
+	// Where t=top, b=bottom, tr=top radius, br=bottom radius
+	// Area of the cross section of the cylinder at x:
+	// area(x):=%pi*r(x)^2;
+	// Inertia x slice at x (using inertia of a solid disc, see https://en.wikipedia.org/wiki/List_of_moments_of_inertia, note needs to be multiplied by density):
+	// dix(x):=area(x)*r(x)^2/4;
+	// Inertia y slice at y (note needs to be multiplied by density)
+	// diy(x):=area(x)*r(x)^2/2;
+	// Volume:
+	// volume(b,t):=integrate(area(x),x,b,t);
+	// The constant density (note that we have this through GetDensity() so we'll use that instead):
+	// density(b,t):=m/volume(b,t);
+	// Inertia tensor element xx, note that we use the parallel axis theorem to move the inertia: Ixx' = Ixx + m translation^2, also note we multiply by density here:
+	// Ixx(br,tr,b,t):=integrate(dix(x)+area(x)*x^2,x,b,t)*density(b,t);
+	// Inertia tensor element yy:
+	// Iyy(br,tr,b,t):=integrate(diy(x),x,b,t)*density(b,t);
+	// Note that we can simplify Ixx by using:
+	// Ixx_delta(br,tr,b,t):=Ixx(br,tr,b,t)-Iyy(br,tr,b,t)/2;
+	// For a cylinder this formula matches what is listed on the wiki:
+	// factor(Ixx(r,r,-h/2,h/2));
+	// factor(Iyy(r,r,-h/2,h/2));
+	// For a cone with tip at origin too:
+	// factor(Ixx(0,r,0,h));
+	// factor(Iyy(0,r,0,h));
+	// Now for the tapered cylinder:
+	// rat(Ixx(br,tr,b,t),br,bt);
+	// rat(Iyy(br,tr,b,t),br,bt);
+	// rat(Ixx_delta(br,tr,b,t),br,bt);
+	float t = mTop;
+	float t2 = Square(t);
+	float t3 = t * t2;
+
+	float b = mBottom;
+	float b2 = Square(b);
+	float b3 = b * b2;
+
+	float br = mBottomRadius;
+	float br2 = Square(br);
+	float br3 = br * br2;
+	float br4 = Square(br2);
+
+	float tr = mTopRadius;
+	float tr2 = Square(tr);
+	float tr3 = tr * tr2;
+	float tr4 = Square(tr2);
+
+	float inertia_y = (JPH_PI / 10.0f) * density * (t - b) * (br4 + tr * br3 + tr2 * br2 + tr3 * br + tr4);
+	float inertia_x_delta = (JPH_PI / 30.0f) * density * ((t3 + 2 * b * t2 + 3 * b2 * t - 6 * b3) * br2 + (3 * t3 + b * t2 - b2 * t - 3 * b3) * tr * br + (6 * t3 - 3 * b * t2 - 2 * b2 * t - b3) * tr2);
+	float inertia_x = inertia_x_delta + inertia_y / 2;
+	float inertia_z = inertia_x;
+	p.mInertia = Mat44::sScale(Vec3(inertia_x, inertia_y, inertia_z));
+	return p;
+}
+
+Vec3 TaperedCylinderShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const
+{
+	JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID");
+
+	constexpr float cEpsilon = 1.0e-5f;
+
+	if (inLocalSurfacePosition.GetY() > mTop - cEpsilon)
+		return Vec3(0, 1, 0);
+	else if (inLocalSurfacePosition.GetY() < mBottom + cEpsilon)
+		return Vec3(0, -1, 0);
+	else
+		return sCalculateSideNormal(sCalculateSideNormalXZ(inLocalSurfacePosition), mTop, mBottom, mTopRadius, mBottomRadius);
+}
+
+AABox TaperedCylinderShape::GetLocalBounds() const
+{
+	float max_radius = max(mTopRadius, mBottomRadius);
+	return AABox(Vec3(-max_radius, mBottom, -max_radius), Vec3(max_radius, mTop, max_radius));
+}
+
+void TaperedCylinderShape::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 tapered cylinder
+	if (inPoint.GetY() >= mBottom && inPoint.GetY() <= mTop // Within height
+		&& Square(inPoint.GetX()) + Square(inPoint.GetZ()) <= Square(mBottomRadius + (inPoint.GetY() - mBottom) * (mTopRadius - mBottomRadius) / (mTop - mBottom))) // Within the radius
+		ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator.GetID() });
+}
+
+void TaperedCylinderShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const
+{
+	JPH_ASSERT(IsValidScale(inScale));
+
+	Mat44 inverse_transform = inCenterOfMassTransform.InversedRotationTranslation();
+
+	// Get scaled tapered cylinder
+	float top, bottom, top_radius, bottom_radius, convex_radius;
+	GetScaled(inScale, top, bottom, top_radius, bottom_radius, convex_radius);
+	Vec3 top_3d(0, top, 0);
+	Vec3 bottom_3d(0, bottom, 0);
+
+	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 normal_xz = sCalculateSideNormalXZ(local_pos);
+			Vec3 side_normal = sCalculateSideNormal(normal_xz, top, bottom, top_radius, bottom_radius);
+			Vec3 side_support_top = normal_xz * top_radius + top_3d;
+			float side_penetration = (side_support_top - local_pos).Dot(side_normal);
+
+			// Calculate penetration into top and bottom plane
+			float top_penetration = top - local_pos.GetY();
+			float bottom_penetration = local_pos.GetY() - bottom;
+			float min_top_bottom_penetration = min(top_penetration, bottom_penetration);
+
+			Vec3 point, normal;
+			if (side_penetration < 0.0f || min_top_bottom_penetration < 0.0f)
+			{
+				// We're outside the cylinder
+				// Calculate the closest point on the line segment from bottom to top support point:
+				// closest_point = bottom + fraction * (top - bottom) / |top - bottom|^2
+				Vec3 side_support_bottom = normal_xz * bottom_radius + bottom_3d;
+				Vec3 bottom_to_top = side_support_top - side_support_bottom;
+				float fraction = (local_pos - side_support_bottom).Dot(bottom_to_top);
+
+				// Calculate the distance to the axis of the cylinder
+				float distance_to_axis = normal_xz.Dot(local_pos);
+				bool inside_top_radius = distance_to_axis <= top_radius;
+				bool inside_bottom_radius = distance_to_axis <= bottom_radius;
+
+				/*
+					Regions of tapered cylinder (side view):
+
+						_  B |       |
+						 --_ |   A   |
+							 t-------+
+					   C    /         \
+						   /  tapered  \
+					_     /  cylinder   \
+					 --_ /               \
+						b-----------------+
+					 D  |        E        |
+						|                 |
+
+					t = side_support_top, b = side_support_bottom
+					Lines between B and C and C and D are at a 90 degree angle to the line between t and b
+				*/
+				if (fraction >= bottom_to_top.LengthSq() // Region B: Above the line segment
+					&& !inside_top_radius) // Outside the top radius
+				{
+					// Top support point is closest
+					point = side_support_top;
+					normal = (local_pos - point).NormalizedOr(Vec3::sAxisY());
+				}
+				else if (fraction < 0.0f // Region D: Below the line segment
+					&& !inside_bottom_radius) // Outside the bottom radius
+				{
+					// Bottom support point is closest
+					point = side_support_bottom;
+					normal = (local_pos - point).NormalizedOr(Vec3::sAxisY());
+				}
+				else if (top_penetration < 0.0f // Region A: Above the top plane
+					&& inside_top_radius) // Inside the top radius
+				{
+					// Top plane is closest
+					point = top_3d;
+					normal = Vec3(0, 1, 0);
+				}
+				else if (bottom_penetration < 0.0f // Region E: Below the bottom plane
+					&& inside_bottom_radius) // Inside the bottom radius
+				{
+					// Bottom plane is closest
+					point = bottom_3d;
+					normal = Vec3(0, -1, 0);
+				}
+				else // Region C
+				{
+					// Side surface is closest
+					point = side_support_top;
+					normal = side_normal;
+				}
+			}
+			else if (side_penetration < min_top_bottom_penetration)
+			{
+				// Side surface is closest
+				point = side_support_top;
+				normal = side_normal;
+			}
+			else if (top_penetration < bottom_penetration)
+			{
+				// Top plane is closest
+				point = top_3d;
+				normal = Vec3(0, 1, 0);
+			}
+			else
+			{
+				// Bottom plane is closest
+				point = bottom_3d;
+				normal = Vec3(0, -1, 0);
+			}
+
+			// Calculate penetration
+			Plane plane = Plane::sFromPointAndNormal(point, normal);
+			float penetration = -plane.SignedDistance(local_pos);
+			if (v.UpdatePenetration(penetration))
+				v.SetCollision(plane.GetTransformed(inCenterOfMassTransform), inCollidingShapeIndex);
+		}
+}
+
+class TaperedCylinderShape::TCSGetTrianglesContext
+{
+public:
+	explicit	TCSGetTrianglesContext(Mat44Arg inTransform) : mTransform(inTransform) { }
+
+	Mat44		mTransform;
+	uint		mProcessed = 0; // Which elements we processed, bit 0 = top, bit 1 = bottom, bit 2 = side
+};
+
+void TaperedCylinderShape::GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const
+{
+	static_assert(sizeof(TCSGetTrianglesContext) <= sizeof(GetTrianglesContext), "GetTrianglesContext too small");
+	JPH_ASSERT(IsAligned(&ioContext, alignof(TCSGetTrianglesContext)));
+
+	// Make sure the scale is not inside out
+	Vec3 scale = ScaleHelpers::IsInsideOut(inScale)? Vec3(-1, 1, 1) * inScale : inScale;
+
+	// Mark top and bottom processed if their radius is too small
+	TCSGetTrianglesContext *context = new (&ioContext) TCSGetTrianglesContext(Mat44::sRotationTranslation(inRotation, inPositionCOM) * Mat44::sScale(scale));
+	constexpr float cMinRadius = 1.0e-3f;
+	if (mTopRadius < cMinRadius)
+		context->mProcessed |= 0b001;
+	if (mBottomRadius < cMinRadius)
+		context->mProcessed |= 0b010;
+}
+
+int TaperedCylinderShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) const
+{
+	constexpr int cNumVertices = int(std::size(cTaperedCylinderFace));
+
+	static_assert(cGetTrianglesMinTrianglesRequested >= 2 * cNumVertices);
+	JPH_ASSERT(inMaxTrianglesRequested >= cGetTrianglesMinTrianglesRequested);
+
+	TCSGetTrianglesContext &context = (TCSGetTrianglesContext &)ioContext;
+
+	int total_num_triangles = 0;
+
+	// Top cap
+	Vec3 top_3d(0, mTop, 0);
+	if ((context.mProcessed & 0b001) == 0)
+	{
+		Vec3 v0 = context.mTransform * (top_3d + mTopRadius * cTaperedCylinderFace[0]);
+		Vec3 v1 = context.mTransform * (top_3d + mTopRadius * cTaperedCylinderFace[1]);
+
+		for (const Vec3 *v = cTaperedCylinderFace + 2, *v_end = cTaperedCylinderFace + cNumVertices; v < v_end; ++v)
+		{
+			Vec3 v2 = context.mTransform * (top_3d + mTopRadius * *v);
+
+			v0.StoreFloat3(outTriangleVertices++);
+			v1.StoreFloat3(outTriangleVertices++);
+			v2.StoreFloat3(outTriangleVertices++);
+
+			v1 = v2;
+		}
+
+		total_num_triangles = cNumVertices - 2;
+		context.mProcessed |= 0b001;
+	}
+
+	// Bottom cap
+	Vec3 bottom_3d(0, mBottom, 0);
+	if ((context.mProcessed & 0b010) == 0
+		&& total_num_triangles + cNumVertices - 2 < inMaxTrianglesRequested)
+	{
+		Vec3 v0 = context.mTransform * (bottom_3d + mBottomRadius * cTaperedCylinderFace[0]);
+		Vec3 v1 = context.mTransform * (bottom_3d + mBottomRadius * cTaperedCylinderFace[1]);
+
+		for (const Vec3 *v = cTaperedCylinderFace + 2, *v_end = cTaperedCylinderFace + cNumVertices; v < v_end; ++v)
+		{
+			Vec3 v2 = context.mTransform * (bottom_3d + mBottomRadius * *v);
+
+			v0.StoreFloat3(outTriangleVertices++);
+			v2.StoreFloat3(outTriangleVertices++);
+			v1.StoreFloat3(outTriangleVertices++);
+
+			v1 = v2;
+		}
+
+		total_num_triangles += cNumVertices - 2;
+		context.mProcessed |= 0b010;
+	}
+
+	// Side
+	if ((context.mProcessed & 0b100) == 0
+		&& total_num_triangles + 2 * cNumVertices < inMaxTrianglesRequested)
+	{
+		Vec3 v0t = context.mTransform * (top_3d + mTopRadius * cTaperedCylinderFace[cNumVertices - 1]);
+		Vec3 v0b = context.mTransform * (bottom_3d + mBottomRadius * cTaperedCylinderFace[cNumVertices - 1]);
+
+		for (const Vec3 *v = cTaperedCylinderFace, *v_end = cTaperedCylinderFace + cNumVertices; v < v_end; ++v)
+		{
+			Vec3 v1t = context.mTransform * (top_3d + mTopRadius * *v);
+			v0t.StoreFloat3(outTriangleVertices++);
+			v0b.StoreFloat3(outTriangleVertices++);
+			v1t.StoreFloat3(outTriangleVertices++);
+
+			Vec3 v1b = context.mTransform * (bottom_3d + mBottomRadius * *v);
+			v1t.StoreFloat3(outTriangleVertices++);
+			v0b.StoreFloat3(outTriangleVertices++);
+			v1b.StoreFloat3(outTriangleVertices++);
+
+			v0t = v1t;
+			v0b = v1b;
+		}
+
+		total_num_triangles += 2 * cNumVertices;
+		context.mProcessed |= 0b100;
+	}
+
+	// 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;
+}
+
+#ifdef JPH_DEBUG_RENDERER
+void TaperedCylinderShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
+{
+	// Preserve flip along y axis but make sure we're not inside out
+	Vec3 scale = ScaleHelpers::IsInsideOut(inScale)? Vec3(-1, 1, 1) * inScale : inScale;
+	RMat44 world_transform = inCenterOfMassTransform * Mat44::sScale(scale);
+
+	DebugRenderer::EDrawMode draw_mode = inDrawWireframe? DebugRenderer::EDrawMode::Wireframe : DebugRenderer::EDrawMode::Solid;
+	inRenderer->DrawTaperedCylinder(world_transform, mTop, mBottom, mTopRadius, mBottomRadius, inUseMaterialColors? GetMaterial()->GetDebugColor() : inColor, DebugRenderer::ECastShadow::On, draw_mode);
+}
+#endif // JPH_DEBUG_RENDERER
+
+void TaperedCylinderShape::SaveBinaryState(StreamOut &inStream) const
+{
+	ConvexShape::SaveBinaryState(inStream);
+
+	inStream.Write(mTop);
+	inStream.Write(mBottom);
+	inStream.Write(mTopRadius);
+	inStream.Write(mBottomRadius);
+	inStream.Write(mConvexRadius);
+}
+
+void TaperedCylinderShape::RestoreBinaryState(StreamIn &inStream)
+{
+	ConvexShape::RestoreBinaryState(inStream);
+
+	inStream.Read(mTop);
+	inStream.Read(mBottom);
+	inStream.Read(mTopRadius);
+	inStream.Read(mBottomRadius);
+	inStream.Read(mConvexRadius);
+}
+
+float TaperedCylinderShape::GetVolume() const
+{
+	// Volume of a tapered cylinder is: integrate(%pi*(b+x*(t-b)/h)^2,x,0,h) where t is the top radius, b is the bottom radius and h is the height
+	return (JPH_PI / 3.0f) * (mTop - mBottom) * (Square(mTopRadius) + mTopRadius * mBottomRadius + Square(mBottomRadius));
+}
+
+bool TaperedCylinderShape::IsValidScale(Vec3Arg inScale) const
+{
+	return ConvexShape::IsValidScale(inScale) && ScaleHelpers::IsUniformScaleXZ(inScale.Abs());
+}
+
+Vec3 TaperedCylinderShape::MakeScaleValid(Vec3Arg inScale) const
+{
+	Vec3 scale = ScaleHelpers::MakeNonZeroScale(inScale);
+
+	return scale.GetSign() * ScaleHelpers::MakeUniformScaleXZ(scale.Abs());
+}
+
+void TaperedCylinderShape::sRegister()
+{
+	ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::TaperedCylinder);
+	f.mConstruct = []() -> Shape * { return new TaperedCylinderShape; };
+	f.mColor = Color::sGreen;
+}
+
+JPH_NAMESPACE_END

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

@@ -0,0 +1,132 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2024 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 TaperedCylinderShape
+class JPH_EXPORT TaperedCylinderShapeSettings final : public ConvexShapeSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, TaperedCylinderShapeSettings)
+
+public:
+	/// Default constructor for deserialization
+							TaperedCylinderShapeSettings() = default;
+
+	/// Create a tapered cylinder centered around the origin with bottom at (0, -inHalfHeightOfTaperedCylinder, 0) with radius inBottomRadius and top at (0, inHalfHeightOfTaperedCylinder, 0) with radius inTopRadius
+							TaperedCylinderShapeSettings(float inHalfHeightOfTaperedCylinder, float inTopRadius, float inBottomRadius, float inConvexRadius = cDefaultConvexRadius, const PhysicsMaterial *inMaterial = nullptr);
+
+	// See: ShapeSettings
+	virtual ShapeResult		Create() const override;
+
+	float					mHalfHeight = 0.0f;
+	float					mTopRadius = 0.0f;
+	float					mBottomRadius = 0.0f;
+	float					mConvexRadius = 0.0f;
+};
+
+/// A cylinder with different top and bottom radii
+class JPH_EXPORT TaperedCylinderShape final : public ConvexShape
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+							TaperedCylinderShape() : ConvexShape(EShapeSubType::TaperedCylinder) { }
+							TaperedCylinderShape(const TaperedCylinderShapeSettings &inSettings, ShapeResult &outResult);
+
+	/// Get top radius of the tapered cylinder
+	inline float			GetTopRadius() const													{ return mTopRadius; }
+
+	/// Get bottom radius of the tapered cylinder
+	inline float			GetBottomRadius() const													{ return mBottomRadius; }
+
+	/// Get convex radius of the tapered cylinder
+	inline float			GetConvexRadius() const													{ return mConvexRadius; }
+
+	/// Get half height of the tapered cylinder
+	inline float			GetHalfHeight() const													{ return 0.5f * (mTop - mBottom); }
+
+	// See Shape::GetCenterOfMass
+	virtual Vec3			GetCenterOfMass() const override										{ return Vec3(0, -0.5f * (mTop + mBottom), 0); }
+
+	// See Shape::GetLocalBounds
+	virtual AABox			GetLocalBounds() const override;
+
+	// See Shape::GetInnerRadius
+	virtual float			GetInnerRadius() const override											{ return min(mTopRadius, mBottomRadius); }
+
+	// 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::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;
+
+#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
+	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:
+	// Class for GetSupportFunction
+	class					TaperedCylinder;
+
+	// Class for GetTrianglesTart
+	class					TCSGetTrianglesContext;
+
+	// Scale the cylinder
+	JPH_INLINE void			GetScaled(Vec3Arg inScale, float &outTop, float &outBottom, float &outTopRadius, float &outBottomRadius, float &outConvexRadius) const;
+
+	float					mTop = 0.0f;
+	float					mBottom = 0.0f;
+	float					mTopRadius = 0.0f;
+	float					mBottomRadius = 0.0f;
+	float					mConvexRadius = 0.0f;
+};
+
+JPH_NAMESPACE_END

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

@@ -0,0 +1,426 @@
+// 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/TriangleShape.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/ShapeCast.h>
+#include <Jolt/Physics/Collision/CastResult.h>
+#include <Jolt/Physics/Collision/CollidePointResult.h>
+#include <Jolt/Physics/Collision/TransformedShape.h>
+#include <Jolt/Physics/Collision/CastConvexVsTriangles.h>
+#include <Jolt/Physics/Collision/CastSphereVsTriangles.h>
+#include <Jolt/Physics/Collision/CollideConvexVsTriangles.h>
+#include <Jolt/Physics/Collision/CollideSphereVsTriangles.h>
+#include <Jolt/Physics/Collision/CollisionDispatch.h>
+#include <Jolt/Physics/Collision/CollideSoftBodyVerticesVsTriangles.h>
+#include <Jolt/Geometry/ConvexSupport.h>
+#include <Jolt/Geometry/RayTriangle.h>
+#include <Jolt/Geometry/ClosestPoint.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(TriangleShapeSettings)
+{
+	JPH_ADD_BASE_CLASS(TriangleShapeSettings, ConvexShapeSettings)
+
+	JPH_ADD_ATTRIBUTE(TriangleShapeSettings, mV1)
+	JPH_ADD_ATTRIBUTE(TriangleShapeSettings, mV2)
+	JPH_ADD_ATTRIBUTE(TriangleShapeSettings, mV3)
+	JPH_ADD_ATTRIBUTE(TriangleShapeSettings, mConvexRadius)
+}
+
+ShapeSettings::ShapeResult TriangleShapeSettings::Create() const
+{
+	if (mCachedResult.IsEmpty())
+		Ref<Shape> shape = new TriangleShape(*this, mCachedResult);
+	return mCachedResult;
+}
+
+TriangleShape::TriangleShape(const TriangleShapeSettings &inSettings, ShapeResult &outResult) :
+	ConvexShape(EShapeSubType::Triangle, inSettings, outResult),
+	mV1(inSettings.mV1),
+	mV2(inSettings.mV2),
+	mV3(inSettings.mV3),
+	mConvexRadius(inSettings.mConvexRadius)
+{
+	if (inSettings.mConvexRadius < 0.0f)
+	{
+		outResult.SetError("Invalid convex radius");
+		return;
+	}
+
+	outResult.Set(this);
+}
+
+AABox TriangleShape::GetLocalBounds() const
+{
+	AABox bounds(mV1, mV1);
+	bounds.Encapsulate(mV2);
+	bounds.Encapsulate(mV3);
+	bounds.ExpandBy(Vec3::sReplicate(mConvexRadius));
+	return bounds;
+}
+
+AABox TriangleShape::GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const
+{
+	JPH_ASSERT(IsValidScale(inScale));
+
+	Vec3 v1 = inCenterOfMassTransform * (inScale * mV1);
+	Vec3 v2 = inCenterOfMassTransform * (inScale * mV2);
+	Vec3 v3 = inCenterOfMassTransform * (inScale * mV3);
+
+	AABox bounds(v1, v1);
+	bounds.Encapsulate(v2);
+	bounds.Encapsulate(v3);
+	bounds.ExpandBy(inScale * mConvexRadius);
+	return bounds;
+}
+
+class TriangleShape::TriangleNoConvex final : public Support
+{
+public:
+							TriangleNoConvex(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3) :
+		mTriangleSupport(inV1, inV2, inV3)
+	{
+		static_assert(sizeof(TriangleNoConvex) <= sizeof(SupportBuffer), "Buffer size too small");
+		JPH_ASSERT(IsAligned(this, alignof(TriangleNoConvex)));
+	}
+
+	virtual Vec3			GetSupport(Vec3Arg inDirection) const override
+	{
+		return mTriangleSupport.GetSupport(inDirection);
+	}
+
+	virtual float			GetConvexRadius() const override
+	{
+		return 0.0f;
+	}
+
+private:
+	TriangleConvexSupport	mTriangleSupport;
+};
+
+class TriangleShape::TriangleWithConvex final : public Support
+{
+public:
+							TriangleWithConvex(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3, float inConvexRadius) :
+		mConvexRadius(inConvexRadius),
+		mTriangleSupport(inV1, inV2, inV3)
+	{
+		static_assert(sizeof(TriangleWithConvex) <= sizeof(SupportBuffer), "Buffer size too small");
+		JPH_ASSERT(IsAligned(this, alignof(TriangleWithConvex)));
+	}
+
+	virtual Vec3			GetSupport(Vec3Arg inDirection) const override
+	{
+		Vec3 support = mTriangleSupport.GetSupport(inDirection);
+		float len = inDirection.Length();
+		if (len > 0.0f)
+			support += (mConvexRadius / len) * inDirection;
+		return support;
+	}
+
+	virtual float			GetConvexRadius() const override
+	{
+		return mConvexRadius;
+	}
+
+private:
+	float					mConvexRadius;
+	TriangleConvexSupport	mTriangleSupport;
+};
+
+const ConvexShape::Support *TriangleShape::GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const
+{
+	switch (inMode)
+	{
+	case ESupportMode::IncludeConvexRadius:
+	case ESupportMode::Default:
+		if (mConvexRadius > 0.0f)
+			return new (&inBuffer) TriangleWithConvex(inScale * mV1, inScale * mV2, inScale * mV3, mConvexRadius);
+		[[fallthrough]];
+
+	case ESupportMode::ExcludeConvexRadius:
+		return new (&inBuffer) TriangleNoConvex(inScale * mV1, inScale * mV2, inScale * mV3);
+	}
+
+	JPH_ASSERT(false);
+	return nullptr;
+}
+
+void TriangleShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const
+{
+	JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID");
+
+	// Calculate transform with scale
+	Mat44 transform = inCenterOfMassTransform.PreScaled(inScale);
+
+	// Flip triangle if scaled inside out
+	if (ScaleHelpers::IsInsideOut(inScale))
+	{
+		outVertices.push_back(transform * mV1);
+		outVertices.push_back(transform * mV3);
+		outVertices.push_back(transform * mV2);
+	}
+	else
+	{
+		outVertices.push_back(transform * mV1);
+		outVertices.push_back(transform * mV2);
+		outVertices.push_back(transform * mV3);
+	}
+}
+
+MassProperties TriangleShape::GetMassProperties() const
+{
+	// We cannot calculate the volume for a triangle, so we return invalid mass properties.
+	// If you want your triangle to be dynamic, then you should provide the mass properties yourself when
+	// creating a Body:
+	//
+	// BodyCreationSettings::mOverrideMassProperties = EOverrideMassProperties::MassAndInertiaProvided;
+	// BodyCreationSettings::mMassPropertiesOverride.SetMassAndInertiaOfSolidBox(Vec3::sOne(), 1000.0f);
+	//
+	// Note that this makes the triangle shape behave the same as a mesh shape with a single triangle.
+	// In practice there is very little use for a dynamic triangle shape as back side collisions will be ignored
+	// so if the triangle falls the wrong way it will sink through the floor.
+	return MassProperties();
+}
+
+Vec3 TriangleShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const
+{
+	JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID");
+
+	Vec3 cross = (mV2 - mV1).Cross(mV3 - mV1);
+	float len = cross.Length();
+	return len != 0.0f? cross / len : Vec3::sAxisY();
+}
+
+void TriangleShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const
+{
+	// A triangle has no volume
+	outTotalVolume = outSubmergedVolume = 0.0f;
+	outCenterOfBuoyancy = Vec3::sZero();
+}
+
+#ifdef JPH_DEBUG_RENDERER
+void TriangleShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
+{
+	RVec3 v1 = inCenterOfMassTransform * (inScale * mV1);
+	RVec3 v2 = inCenterOfMassTransform * (inScale * mV2);
+	RVec3 v3 = inCenterOfMassTransform * (inScale * mV3);
+
+	if (ScaleHelpers::IsInsideOut(inScale))
+		std::swap(v1, v2);
+
+	if (inDrawWireframe)
+		inRenderer->DrawWireTriangle(v1, v2, v3, inUseMaterialColors? GetMaterial()->GetDebugColor() : inColor);
+	else
+		inRenderer->DrawTriangle(v1, v2, v3, inUseMaterialColors? GetMaterial()->GetDebugColor() : inColor);
+}
+#endif // JPH_DEBUG_RENDERER
+
+bool TriangleShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const
+{
+	float fraction = RayTriangle(inRay.mOrigin, inRay.mDirection, mV1, mV2, mV3);
+	if (fraction < ioHit.mFraction)
+	{
+		ioHit.mFraction = fraction;
+		ioHit.mSubShapeID2 = inSubShapeIDCreator.GetID();
+		return true;
+	}
+	return false;
+}
+
+void TriangleShape::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;
+
+	// Back facing check
+	if (inRayCastSettings.mBackFaceModeTriangles == EBackFaceMode::IgnoreBackFaces && (mV2 - mV1).Cross(mV3 - mV1).Dot(inRay.mDirection) > 0.0f)
+		return;
+
+	// Test ray against triangle
+	float fraction = RayTriangle(inRay.mOrigin, inRay.mDirection, mV1, mV2, mV3);
+	if (fraction < ioCollector.GetEarlyOutFraction())
+	{
+		// Better hit than the current hit
+		RayCastResult hit;
+		hit.mBodyID = TransformedShape::sGetBodyID(ioCollector.GetContext());
+		hit.mFraction = fraction;
+		hit.mSubShapeID2 = inSubShapeIDCreator.GetID();
+		ioCollector.AddHit(hit);
+	}
+}
+
+void TriangleShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	// Can't be inside a triangle
+}
+
+void TriangleShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const
+{
+	CollideSoftBodyVerticesVsTriangles collider(inCenterOfMassTransform, inScale);
+
+	for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v)
+		if (v.GetInvMass() > 0.0f)
+		{
+			collider.StartVertex(v);
+			collider.ProcessTriangle(mV1, mV2, mV3);
+			collider.FinishVertex(v, inCollidingShapeIndex);
+		}
+}
+
+void TriangleShape::sCollideConvexVsTriangle(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_ASSERT(inShape1->GetType() == EShapeType::Convex);
+	const ConvexShape *shape1 = static_cast<const ConvexShape *>(inShape1);
+	JPH_ASSERT(inShape2->GetSubType() == EShapeSubType::Triangle);
+	const TriangleShape *shape2 = static_cast<const TriangleShape *>(inShape2);
+
+	CollideConvexVsTriangles collider(shape1, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1.GetID(), inCollideShapeSettings, ioCollector);
+	collider.Collide(shape2->mV1, shape2->mV2, shape2->mV3, 0b111, inSubShapeIDCreator2.GetID());
+}
+
+void TriangleShape::sCollideSphereVsTriangle(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_ASSERT(inShape1->GetSubType() == EShapeSubType::Sphere);
+	const SphereShape *shape1 = static_cast<const SphereShape *>(inShape1);
+	JPH_ASSERT(inShape2->GetSubType() == EShapeSubType::Triangle);
+	const TriangleShape *shape2 = static_cast<const TriangleShape *>(inShape2);
+
+	CollideSphereVsTriangles collider(shape1, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1.GetID(), inCollideShapeSettings, ioCollector);
+	collider.Collide(shape2->mV1, shape2->mV2, shape2->mV3, 0b111, inSubShapeIDCreator2.GetID());
+}
+
+void TriangleShape::sCastConvexVsTriangle(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_ASSERT(inShape->GetSubType() == EShapeSubType::Triangle);
+	const TriangleShape *shape = static_cast<const TriangleShape *>(inShape);
+
+	CastConvexVsTriangles caster(inShapeCast, inShapeCastSettings, inScale, inCenterOfMassTransform2, inSubShapeIDCreator1, ioCollector);
+	caster.Cast(shape->mV1, shape->mV2, shape->mV3, 0b111, inSubShapeIDCreator2.GetID());
+}
+
+void TriangleShape::sCastSphereVsTriangle(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_ASSERT(inShape->GetSubType() == EShapeSubType::Triangle);
+	const TriangleShape *shape = static_cast<const TriangleShape *>(inShape);
+
+	CastSphereVsTriangles caster(inShapeCast, inShapeCastSettings, inScale, inCenterOfMassTransform2, inSubShapeIDCreator1, ioCollector);
+	caster.Cast(shape->mV1, shape->mV2, shape->mV3, 0b111, inSubShapeIDCreator2.GetID());
+}
+
+class TriangleShape::TSGetTrianglesContext
+{
+public:
+					TSGetTrianglesContext(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3) : mV1(inV1), mV2(inV2), mV3(inV3) { }
+
+	Vec3			mV1;
+	Vec3			mV2;
+	Vec3			mV3;
+
+	bool			mIsDone = false;
+};
+
+void TriangleShape::GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const
+{
+	static_assert(sizeof(TSGetTrianglesContext) <= sizeof(GetTrianglesContext), "GetTrianglesContext too small");
+	JPH_ASSERT(IsAligned(&ioContext, alignof(TSGetTrianglesContext)));
+
+	Mat44 m = Mat44::sRotationTranslation(inRotation, inPositionCOM) * Mat44::sScale(inScale);
+
+	new (&ioContext) TSGetTrianglesContext(m * mV1, m * mV2, m * mV3);
+}
+
+int TriangleShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) const
+{
+	static_assert(cGetTrianglesMinTrianglesRequested >= 3, "cGetTrianglesMinTrianglesRequested is too small");
+	JPH_ASSERT(inMaxTrianglesRequested >= cGetTrianglesMinTrianglesRequested);
+
+	TSGetTrianglesContext &context = (TSGetTrianglesContext &)ioContext;
+
+	// Only return the triangle the 1st time
+	if (context.mIsDone)
+		return 0;
+	context.mIsDone = true;
+
+	// Store triangle
+	context.mV1.StoreFloat3(outTriangleVertices);
+	context.mV2.StoreFloat3(outTriangleVertices + 1);
+	context.mV3.StoreFloat3(outTriangleVertices + 2);
+
+	// Store material
+	if (outMaterials != nullptr)
+		*outMaterials = GetMaterial();
+
+	return 1;
+}
+
+void TriangleShape::SaveBinaryState(StreamOut &inStream) const
+{
+	ConvexShape::SaveBinaryState(inStream);
+
+	inStream.Write(mV1);
+	inStream.Write(mV2);
+	inStream.Write(mV3);
+	inStream.Write(mConvexRadius);
+}
+
+void TriangleShape::RestoreBinaryState(StreamIn &inStream)
+{
+	ConvexShape::RestoreBinaryState(inStream);
+
+	inStream.Read(mV1);
+	inStream.Read(mV2);
+	inStream.Read(mV3);
+	inStream.Read(mConvexRadius);
+}
+
+bool TriangleShape::IsValidScale(Vec3Arg inScale) const
+{
+	return ConvexShape::IsValidScale(inScale) && (mConvexRadius == 0.0f || ScaleHelpers::IsUniformScale(inScale.Abs()));
+}
+
+Vec3 TriangleShape::MakeScaleValid(Vec3Arg inScale) const
+{
+	Vec3 scale = ScaleHelpers::MakeNonZeroScale(inScale);
+
+	if (mConvexRadius == 0.0f)
+		return scale;
+
+	return scale.GetSign() * ScaleHelpers::MakeUniformScale(scale.Abs());
+}
+
+void TriangleShape::sRegister()
+{
+	ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::Triangle);
+	f.mConstruct = []() -> Shape * { return new TriangleShape; };
+	f.mColor = Color::sGreen;
+
+	for (EShapeSubType s : sConvexSubShapeTypes)
+	{
+		CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::Triangle, sCollideConvexVsTriangle);
+		CollisionDispatch::sRegisterCastShape(s, EShapeSubType::Triangle, sCastConvexVsTriangle);
+
+		CollisionDispatch::sRegisterCollideShape(EShapeSubType::Triangle, s, CollisionDispatch::sReversedCollideShape);
+		CollisionDispatch::sRegisterCastShape(EShapeSubType::Triangle, s, CollisionDispatch::sReversedCastShape);
+	}
+
+	// Specialized collision functions
+	CollisionDispatch::sRegisterCollideShape(EShapeSubType::Sphere, EShapeSubType::Triangle, sCollideSphereVsTriangle);
+	CollisionDispatch::sRegisterCastShape(EShapeSubType::Sphere, EShapeSubType::Triangle, sCastSphereVsTriangle);
+}
+
+JPH_NAMESPACE_END

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

@@ -0,0 +1,143 @@
+// 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 TriangleShape
+class JPH_EXPORT TriangleShapeSettings final : public ConvexShapeSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, TriangleShapeSettings)
+
+public:
+	/// Default constructor for deserialization
+							TriangleShapeSettings() = default;
+
+	/// Create a triangle with points (inV1, inV2, inV3) (counter clockwise) and convex radius inConvexRadius.
+	/// Note that the convex radius is currently only used for shape vs shape collision, for all other purposes the triangle is infinitely thin.
+							TriangleShapeSettings(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3, float inConvexRadius = 0.0f, const PhysicsMaterial *inMaterial = nullptr) : ConvexShapeSettings(inMaterial), mV1(inV1), mV2(inV2), mV3(inV3), mConvexRadius(inConvexRadius) { }
+
+	// See: ShapeSettings
+	virtual ShapeResult		Create() const override;
+
+	Vec3					mV1;
+	Vec3					mV2;
+	Vec3					mV3;
+	float					mConvexRadius = 0.0f;
+};
+
+/// A single triangle, not the most efficient way of creating a world filled with triangles but can be used as a query shape for example.
+class JPH_EXPORT TriangleShape final : public ConvexShape
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+							TriangleShape() : ConvexShape(EShapeSubType::Triangle) { }
+							TriangleShape(const TriangleShapeSettings &inSettings, ShapeResult &outResult);
+
+	/// Create a triangle with points (inV1, inV2, inV3) (counter clockwise) and convex radius inConvexRadius.
+	/// Note that the convex radius is currently only used for shape vs shape collision, for all other purposes the triangle is infinitely thin.
+							TriangleShape(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3, float inConvexRadius = 0.0f, const PhysicsMaterial *inMaterial = nullptr) : ConvexShape(EShapeSubType::Triangle, inMaterial), mV1(inV1), mV2(inV2), mV3(inV3), mConvexRadius(inConvexRadius) { JPH_ASSERT(inConvexRadius >= 0.0f); }
+
+	/// Get the vertices of the triangle
+	inline Vec3				GetVertex1() const																	{ return mV1; }
+	inline Vec3				GetVertex2() const																	{ return mV2; }
+	inline Vec3				GetVertex3() const																	{ return mV3; }
+
+	/// Convex radius
+	float					GetConvexRadius() const																{ return mConvexRadius; }
+
+	// 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 mConvexRadius; }
+
+	// 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;
+#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), 1); }
+
+	// See Shape::GetVolume
+	virtual float			GetVolume() const override															{ return 0; }
+
+	// 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				sCollideConvexVsTriangle(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				sCollideSphereVsTriangle(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				sCastConvexVsTriangle(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				sCastSphereVsTriangle(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector);
+
+	// Context for GetTrianglesStart/Next
+	class					TSGetTrianglesContext;
+
+	// Classes for GetSupportFunction
+	class					TriangleNoConvex;
+	class					TriangleWithConvex;
+
+	Vec3					mV1;
+	Vec3					mV2;
+	Vec3					mV3;
+	float					mConvexRadius = 0.0f;
+};
+
+JPH_NAMESPACE_END