initial commit, 4.5 stable

thirdparty/jolt_physics/Jolt/Geometry/AABox.h

@@ -0,0 +1,313 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Geometry/Triangle.h>
+#include <Jolt/Geometry/IndexedTriangle.h>
+#include <Jolt/Geometry/Plane.h>
+#include <Jolt/Math/Mat44.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Axis aligned box
+class [[nodiscard]] AABox
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+					AABox()												: mMin(Vec3::sReplicate(FLT_MAX)), mMax(Vec3::sReplicate(-FLT_MAX)) { }
+					AABox(Vec3Arg inMin, Vec3Arg inMax)					: mMin(inMin), mMax(inMax) { }
+					AABox(DVec3Arg inMin, DVec3Arg inMax)				: mMin(inMin.ToVec3RoundDown()), mMax(inMax.ToVec3RoundUp()) { }
+					AABox(Vec3Arg inCenter, float inRadius)				: mMin(inCenter - Vec3::sReplicate(inRadius)), mMax(inCenter + Vec3::sReplicate(inRadius)) { }
+
+	/// Create box from 2 points
+	static AABox	sFromTwoPoints(Vec3Arg inP1, Vec3Arg inP2)			{ return AABox(Vec3::sMin(inP1, inP2), Vec3::sMax(inP1, inP2)); }
+
+	/// Create box from indexed triangle
+	static AABox	sFromTriangle(const VertexList &inVertices, const IndexedTriangle &inTriangle)
+	{
+		AABox box = sFromTwoPoints(Vec3(inVertices[inTriangle.mIdx[0]]), Vec3(inVertices[inTriangle.mIdx[1]]));
+		box.Encapsulate(Vec3(inVertices[inTriangle.mIdx[2]]));
+		return box;
+	}
+
+	/// Get bounding box of size FLT_MAX
+	static AABox	sBiggest()
+	{
+		/// Max half extent of AABox is 0.5 * FLT_MAX so that GetSize() remains finite
+		return AABox(Vec3::sReplicate(-0.5f * FLT_MAX), Vec3::sReplicate(0.5f * FLT_MAX));
+	}
+
+	/// Comparison operators
+	bool			operator == (const AABox &inRHS) const				{ return mMin == inRHS.mMin && mMax == inRHS.mMax; }
+	bool			operator != (const AABox &inRHS) const				{ return mMin != inRHS.mMin || mMax != inRHS.mMax; }
+
+	/// Reset the bounding box to an empty bounding box
+	void			SetEmpty()
+	{
+		mMin = Vec3::sReplicate(FLT_MAX);
+		mMax = Vec3::sReplicate(-FLT_MAX);
+	}
+
+	/// Check if the bounding box is valid (max >= min)
+	bool			IsValid() const
+	{
+		return mMin.GetX() <= mMax.GetX() && mMin.GetY() <= mMax.GetY() && mMin.GetZ() <= mMax.GetZ();
+	}
+
+	/// Encapsulate point in bounding box
+	void			Encapsulate(Vec3Arg inPos)
+	{
+		mMin = Vec3::sMin(mMin, inPos);
+		mMax = Vec3::sMax(mMax, inPos);
+	}
+
+	/// Encapsulate bounding box in bounding box
+	void			Encapsulate(const AABox &inRHS)
+	{
+		mMin = Vec3::sMin(mMin, inRHS.mMin);
+		mMax = Vec3::sMax(mMax, inRHS.mMax);
+	}
+
+	/// Encapsulate triangle in bounding box
+	void			Encapsulate(const Triangle &inRHS)
+	{
+		Vec3 v = Vec3::sLoadFloat3Unsafe(inRHS.mV[0]);
+		Encapsulate(v);
+		v = Vec3::sLoadFloat3Unsafe(inRHS.mV[1]);
+		Encapsulate(v);
+		v = Vec3::sLoadFloat3Unsafe(inRHS.mV[2]);
+		Encapsulate(v);
+	}
+
+	/// Encapsulate triangle in bounding box
+	void			Encapsulate(const VertexList &inVertices, const IndexedTriangle &inTriangle)
+	{
+		for (uint32 idx : inTriangle.mIdx)
+			Encapsulate(Vec3(inVertices[idx]));
+	}
+
+	/// Intersect this bounding box with inOther, returns the intersection
+	AABox			Intersect(const AABox &inOther) const
+	{
+		return AABox(Vec3::sMax(mMin, inOther.mMin), Vec3::sMin(mMax, inOther.mMax));
+	}
+
+	/// Make sure that each edge of the bounding box has a minimal length
+	void			EnsureMinimalEdgeLength(float inMinEdgeLength)
+	{
+		Vec3 min_length = Vec3::sReplicate(inMinEdgeLength);
+		mMax = Vec3::sSelect(mMax, mMin + min_length, Vec3::sLess(mMax - mMin, min_length));
+	}
+
+	/// Widen the box on both sides by inVector
+	void			ExpandBy(Vec3Arg inVector)
+	{
+		mMin -= inVector;
+		mMax += inVector;
+	}
+
+	/// Get center of bounding box
+	Vec3			GetCenter() const
+	{
+		return 0.5f * (mMin + mMax);
+	}
+
+	/// Get extent of bounding box (half of the size)
+	Vec3			GetExtent() const
+	{
+		return 0.5f * (mMax - mMin);
+	}
+
+	/// Get size of bounding box
+	Vec3			GetSize() const
+	{
+		return mMax - mMin;
+	}
+
+	/// Get surface area of bounding box
+	float			GetSurfaceArea() const
+	{
+		Vec3 extent = mMax - mMin;
+		return 2.0f * (extent.GetX() * extent.GetY() + extent.GetX() * extent.GetZ() + extent.GetY() * extent.GetZ());
+	}
+
+	/// Get volume of bounding box
+	float			GetVolume() const
+	{
+		Vec3 extent = mMax - mMin;
+		return extent.GetX() * extent.GetY() * extent.GetZ();
+	}
+
+	/// Check if this box contains another box
+	bool			Contains(const AABox &inOther) const
+	{
+		return UVec4::sAnd(Vec3::sLessOrEqual(mMin, inOther.mMin), Vec3::sGreaterOrEqual(mMax, inOther.mMax)).TestAllXYZTrue();
+	}
+
+	/// Check if this box contains a point
+	bool			Contains(Vec3Arg inOther) const
+	{
+		return UVec4::sAnd(Vec3::sLessOrEqual(mMin, inOther), Vec3::sGreaterOrEqual(mMax, inOther)).TestAllXYZTrue();
+	}
+
+	/// Check if this box contains a point
+	bool			Contains(DVec3Arg inOther) const
+	{
+		return Contains(Vec3(inOther));
+	}
+
+	/// Check if this box overlaps with another box
+	bool			Overlaps(const AABox &inOther) const
+	{
+		return !UVec4::sOr(Vec3::sGreater(mMin, inOther.mMax), Vec3::sLess(mMax, inOther.mMin)).TestAnyXYZTrue();
+	}
+
+	/// Check if this box overlaps with a plane
+	bool			Overlaps(const Plane &inPlane) const
+	{
+		Vec3 normal = inPlane.GetNormal();
+		float dist_normal = inPlane.SignedDistance(GetSupport(normal));
+		float dist_min_normal = inPlane.SignedDistance(GetSupport(-normal));
+		return dist_normal * dist_min_normal <= 0.0f; // If both support points are on the same side of the plane we don't overlap
+	}
+
+	/// Translate bounding box
+	void			Translate(Vec3Arg inTranslation)
+	{
+		mMin += inTranslation;
+		mMax += inTranslation;
+	}
+
+	/// Translate bounding box
+	void			Translate(DVec3Arg inTranslation)
+	{
+		mMin = (DVec3(mMin) + inTranslation).ToVec3RoundDown();
+		mMax = (DVec3(mMax) + inTranslation).ToVec3RoundUp();
+	}
+
+	/// Transform bounding box
+	AABox			Transformed(Mat44Arg inMatrix) const
+	{
+		// Start with the translation of the matrix
+		Vec3 new_min, new_max;
+		new_min = new_max = inMatrix.GetTranslation();
+
+		// Now find the extreme points by considering the product of the min and max with each column of inMatrix
+		for (int c = 0; c < 3; ++c)
+		{
+			Vec3 col = inMatrix.GetColumn3(c);
+
+			Vec3 a = col * mMin[c];
+			Vec3 b = col * mMax[c];
+
+			new_min += Vec3::sMin(a, b);
+			new_max += Vec3::sMax(a, b);
+		}
+
+		// Return the new bounding box
+		return AABox(new_min, new_max);
+	}
+
+	/// Transform bounding box
+	AABox			Transformed(DMat44Arg inMatrix) const
+	{
+		AABox transformed = Transformed(inMatrix.GetRotation());
+		transformed.Translate(inMatrix.GetTranslation());
+		return transformed;
+	}
+
+	/// Scale this bounding box, can handle non-uniform and negative scaling
+	AABox			Scaled(Vec3Arg inScale) const
+	{
+		return AABox::sFromTwoPoints(mMin * inScale, mMax * inScale);
+	}
+
+	/// Calculate the support vector for this convex shape.
+	Vec3			GetSupport(Vec3Arg inDirection) const
+	{
+		return Vec3::sSelect(mMax, mMin, Vec3::sLess(inDirection, Vec3::sZero()));
+	}
+
+	/// Get the vertices of the face that faces inDirection the most
+	template <class VERTEX_ARRAY>
+	void			GetSupportingFace(Vec3Arg inDirection, VERTEX_ARRAY &outVertices) const
+	{
+		outVertices.resize(4);
+
+		int axis = inDirection.Abs().GetHighestComponentIndex();
+		if (inDirection[axis] < 0.0f)
+		{
+			switch (axis)
+			{
+			case 0:
+				outVertices[0] = Vec3(mMax.GetX(), mMin.GetY(), mMin.GetZ());
+				outVertices[1] = Vec3(mMax.GetX(), mMax.GetY(), mMin.GetZ());
+				outVertices[2] = Vec3(mMax.GetX(), mMax.GetY(), mMax.GetZ());
+				outVertices[3] = Vec3(mMax.GetX(), mMin.GetY(), mMax.GetZ());
+				break;
+
+			case 1:
+				outVertices[0] = Vec3(mMin.GetX(), mMax.GetY(), mMin.GetZ());
+				outVertices[1] = Vec3(mMin.GetX(), mMax.GetY(), mMax.GetZ());
+				outVertices[2] = Vec3(mMax.GetX(), mMax.GetY(), mMax.GetZ());
+				outVertices[3] = Vec3(mMax.GetX(), mMax.GetY(), mMin.GetZ());
+				break;
+
+			case 2:
+				outVertices[0] = Vec3(mMin.GetX(), mMin.GetY(), mMax.GetZ());
+				outVertices[1] = Vec3(mMax.GetX(), mMin.GetY(), mMax.GetZ());
+				outVertices[2] = Vec3(mMax.GetX(), mMax.GetY(), mMax.GetZ());
+				outVertices[3] = Vec3(mMin.GetX(), mMax.GetY(), mMax.GetZ());
+				break;
+			}
+		}
+		else
+		{
+			switch (axis)
+			{
+			case 0:
+				outVertices[0] = Vec3(mMin.GetX(), mMin.GetY(), mMin.GetZ());
+				outVertices[1] = Vec3(mMin.GetX(), mMin.GetY(), mMax.GetZ());
+				outVertices[2] = Vec3(mMin.GetX(), mMax.GetY(), mMax.GetZ());
+				outVertices[3] = Vec3(mMin.GetX(), mMax.GetY(), mMin.GetZ());
+				break;
+
+			case 1:
+				outVertices[0] = Vec3(mMin.GetX(), mMin.GetY(), mMin.GetZ());
+				outVertices[1] = Vec3(mMax.GetX(), mMin.GetY(), mMin.GetZ());
+				outVertices[2] = Vec3(mMax.GetX(), mMin.GetY(), mMax.GetZ());
+				outVertices[3] = Vec3(mMin.GetX(), mMin.GetY(), mMax.GetZ());
+				break;
+
+			case 2:
+				outVertices[0] = Vec3(mMin.GetX(), mMin.GetY(), mMin.GetZ());
+				outVertices[1] = Vec3(mMin.GetX(), mMax.GetY(), mMin.GetZ());
+				outVertices[2] = Vec3(mMax.GetX(), mMax.GetY(), mMin.GetZ());
+				outVertices[3] = Vec3(mMax.GetX(), mMin.GetY(), mMin.GetZ());
+				break;
+			}
+		}
+	}
+
+	/// Get the closest point on or in this box to inPoint
+	Vec3			GetClosestPoint(Vec3Arg inPoint) const
+	{
+		return Vec3::sMin(Vec3::sMax(inPoint, mMin), mMax);
+	}
+
+	/// Get the squared distance between inPoint and this box (will be 0 if in Point is inside the box)
+	inline float	GetSqDistanceTo(Vec3Arg inPoint) const
+	{
+		return (GetClosestPoint(inPoint) - inPoint).LengthSq();
+	}
+
+	/// Bounding box min and max
+	Vec3			mMin;
+	Vec3			mMax;
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Geometry/AABox4.h

@@ -0,0 +1,224 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Geometry/OrientedBox.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Helper functions that process 4 axis aligned boxes at the same time using SIMD
+/// Test if 4 bounding boxes overlap with 1 bounding box, splat 1 box
+JPH_INLINE UVec4 AABox4VsBox(const AABox &inBox1, Vec4Arg inBox2MinX, Vec4Arg inBox2MinY, Vec4Arg inBox2MinZ, Vec4Arg inBox2MaxX, Vec4Arg inBox2MaxY, Vec4Arg inBox2MaxZ)
+{
+	// Splat values of box 1
+	Vec4 box1_minx = inBox1.mMin.SplatX();
+	Vec4 box1_miny = inBox1.mMin.SplatY();
+	Vec4 box1_minz = inBox1.mMin.SplatZ();
+	Vec4 box1_maxx = inBox1.mMax.SplatX();
+	Vec4 box1_maxy = inBox1.mMax.SplatY();
+	Vec4 box1_maxz = inBox1.mMax.SplatZ();
+
+	// Test separation over each axis
+	UVec4 nooverlapx = UVec4::sOr(Vec4::sGreater(box1_minx, inBox2MaxX), Vec4::sGreater(inBox2MinX, box1_maxx));
+	UVec4 nooverlapy = UVec4::sOr(Vec4::sGreater(box1_miny, inBox2MaxY), Vec4::sGreater(inBox2MinY, box1_maxy));
+	UVec4 nooverlapz = UVec4::sOr(Vec4::sGreater(box1_minz, inBox2MaxZ), Vec4::sGreater(inBox2MinZ, box1_maxz));
+
+	// Return overlap
+	return UVec4::sNot(UVec4::sOr(UVec4::sOr(nooverlapx, nooverlapy), nooverlapz));
+}
+
+/// Scale 4 axis aligned boxes
+JPH_INLINE void AABox4Scale(Vec3Arg inScale, Vec4Arg inBoxMinX, Vec4Arg inBoxMinY, Vec4Arg inBoxMinZ, Vec4Arg inBoxMaxX, Vec4Arg inBoxMaxY, Vec4Arg inBoxMaxZ, Vec4 &outBoundsMinX, Vec4 &outBoundsMinY, Vec4 &outBoundsMinZ, Vec4 &outBoundsMaxX, Vec4 &outBoundsMaxY, Vec4 &outBoundsMaxZ)
+{
+	Vec4 scale_x = inScale.SplatX();
+	Vec4 scaled_min_x = scale_x * inBoxMinX;
+	Vec4 scaled_max_x = scale_x * inBoxMaxX;
+	outBoundsMinX = Vec4::sMin(scaled_min_x, scaled_max_x); // Negative scale can flip min and max
+	outBoundsMaxX = Vec4::sMax(scaled_min_x, scaled_max_x);
+
+	Vec4 scale_y = inScale.SplatY();
+	Vec4 scaled_min_y = scale_y * inBoxMinY;
+	Vec4 scaled_max_y = scale_y * inBoxMaxY;
+	outBoundsMinY = Vec4::sMin(scaled_min_y, scaled_max_y);
+	outBoundsMaxY = Vec4::sMax(scaled_min_y, scaled_max_y);
+
+	Vec4 scale_z = inScale.SplatZ();
+	Vec4 scaled_min_z = scale_z * inBoxMinZ;
+	Vec4 scaled_max_z = scale_z * inBoxMaxZ;
+	outBoundsMinZ = Vec4::sMin(scaled_min_z, scaled_max_z);
+	outBoundsMaxZ = Vec4::sMax(scaled_min_z, scaled_max_z);
+}
+
+/// Enlarge 4 bounding boxes with extent (add to both sides)
+JPH_INLINE void AABox4EnlargeWithExtent(Vec3Arg inExtent, Vec4 &ioBoundsMinX, Vec4 &ioBoundsMinY, Vec4 &ioBoundsMinZ, Vec4 &ioBoundsMaxX, Vec4 &ioBoundsMaxY, Vec4 &ioBoundsMaxZ)
+{
+	Vec4 extent_x = inExtent.SplatX();
+	ioBoundsMinX -= extent_x;
+	ioBoundsMaxX += extent_x;
+
+	Vec4 extent_y = inExtent.SplatY();
+	ioBoundsMinY -= extent_y;
+	ioBoundsMaxY += extent_y;
+
+	Vec4 extent_z = inExtent.SplatZ();
+	ioBoundsMinZ -= extent_z;
+	ioBoundsMaxZ += extent_z;
+}
+
+/// Test if 4 bounding boxes overlap with a point
+JPH_INLINE UVec4 AABox4VsPoint(Vec3Arg inPoint, Vec4Arg inBoxMinX, Vec4Arg inBoxMinY, Vec4Arg inBoxMinZ, Vec4Arg inBoxMaxX, Vec4Arg inBoxMaxY, Vec4Arg inBoxMaxZ)
+{
+	// Splat point to 4 component vectors
+	Vec4 point_x = Vec4(inPoint).SplatX();
+	Vec4 point_y = Vec4(inPoint).SplatY();
+	Vec4 point_z = Vec4(inPoint).SplatZ();
+
+	// Test if point overlaps with box
+	UVec4 overlapx = UVec4::sAnd(Vec4::sGreaterOrEqual(point_x, inBoxMinX), Vec4::sLessOrEqual(point_x, inBoxMaxX));
+	UVec4 overlapy = UVec4::sAnd(Vec4::sGreaterOrEqual(point_y, inBoxMinY), Vec4::sLessOrEqual(point_y, inBoxMaxY));
+	UVec4 overlapz = UVec4::sAnd(Vec4::sGreaterOrEqual(point_z, inBoxMinZ), Vec4::sLessOrEqual(point_z, inBoxMaxZ));
+
+	// Test if all are overlapping
+	return UVec4::sAnd(UVec4::sAnd(overlapx, overlapy), overlapz);
+}
+
+/// Test if 4 bounding boxes overlap with an oriented box
+JPH_INLINE UVec4 AABox4VsBox(Mat44Arg inOrientation, Vec3Arg inHalfExtents, Vec4Arg inBoxMinX, Vec4Arg inBoxMinY, Vec4Arg inBoxMinZ, Vec4Arg inBoxMaxX, Vec4Arg inBoxMaxY, Vec4Arg inBoxMaxZ, float inEpsilon = 1.0e-6f)
+{
+	// Taken from: Real Time Collision Detection - Christer Ericson
+	// Chapter 4.4.1, page 103-105.
+	// Note that the code is swapped around: A is the aabox and B is the oriented box (this saves us from having to invert the orientation of the oriented box)
+
+	// Compute translation vector t (the translation of B in the space of A)
+	Vec4 t[3] {
+		inOrientation.GetTranslation().SplatX() - 0.5f * (inBoxMinX + inBoxMaxX),
+		inOrientation.GetTranslation().SplatY() - 0.5f * (inBoxMinY + inBoxMaxY),
+		inOrientation.GetTranslation().SplatZ() - 0.5f * (inBoxMinZ + inBoxMaxZ) };
+
+	// Compute common subexpressions. Add in an epsilon term to
+	// counteract arithmetic errors when two edges are parallel and
+	// their cross product is (near) null (see text for details)
+	Vec3 epsilon = Vec3::sReplicate(inEpsilon);
+	Vec3 abs_r[3] { inOrientation.GetAxisX().Abs() + epsilon, inOrientation.GetAxisY().Abs() + epsilon, inOrientation.GetAxisZ().Abs() + epsilon };
+
+	// Half extents for a
+	Vec4 a_half_extents[3] {
+		0.5f * (inBoxMaxX - inBoxMinX),
+		0.5f * (inBoxMaxY - inBoxMinY),
+		0.5f * (inBoxMaxZ - inBoxMinZ) };
+
+	// Half extents of b
+	Vec4 b_half_extents_x = inHalfExtents.SplatX();
+	Vec4 b_half_extents_y = inHalfExtents.SplatY();
+	Vec4 b_half_extents_z = inHalfExtents.SplatZ();
+
+	// Each component corresponds to 1 overlapping OBB vs ABB
+	UVec4 overlaps = UVec4(0xffffffff, 0xffffffff, 0xffffffff, 0xffffffff);
+
+	// Test axes L = A0, L = A1, L = A2
+	Vec4 ra, rb;
+	for (int i = 0; i < 3; i++)
+	{
+		ra = a_half_extents[i];
+		rb = b_half_extents_x * abs_r[0][i] + b_half_extents_y * abs_r[1][i] + b_half_extents_z * abs_r[2][i];
+		overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual(t[i].Abs(), ra + rb));
+	}
+
+	// Test axes L = B0, L = B1, L = B2
+	for (int i = 0; i < 3; i++)
+	{
+		ra = a_half_extents[0] * abs_r[i][0] + a_half_extents[1] * abs_r[i][1] + a_half_extents[2] * abs_r[i][2];
+		rb = Vec4::sReplicate(inHalfExtents[i]);
+		overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[0] * inOrientation(0, i) + t[1] * inOrientation(1, i) + t[2] * inOrientation(2, i)).Abs(), ra + rb));
+	}
+
+	// Test axis L = A0 x B0
+	ra = a_half_extents[1] * abs_r[0][2] + a_half_extents[2] * abs_r[0][1];
+	rb = b_half_extents_y * abs_r[2][0] + b_half_extents_z * abs_r[1][0];
+	overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[2] * inOrientation(1, 0) - t[1] * inOrientation(2, 0)).Abs(), ra + rb));
+
+	// Test axis L = A0 x B1
+	ra = a_half_extents[1] * abs_r[1][2] + a_half_extents[2] * abs_r[1][1];
+	rb = b_half_extents_x * abs_r[2][0] + b_half_extents_z * abs_r[0][0];
+	overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[2] * inOrientation(1, 1) - t[1] * inOrientation(2, 1)).Abs(), ra + rb));
+
+	// Test axis L = A0 x B2
+	ra = a_half_extents[1] * abs_r[2][2] + a_half_extents[2] * abs_r[2][1];
+	rb = b_half_extents_x * abs_r[1][0] + b_half_extents_y * abs_r[0][0];
+	overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[2] * inOrientation(1, 2) - t[1] * inOrientation(2, 2)).Abs(), ra + rb));
+
+	// Test axis L = A1 x B0
+	ra = a_half_extents[0] * abs_r[0][2] + a_half_extents[2] * abs_r[0][0];
+	rb = b_half_extents_y * abs_r[2][1] + b_half_extents_z * abs_r[1][1];
+	overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[0] * inOrientation(2, 0) - t[2] * inOrientation(0, 0)).Abs(), ra + rb));
+
+	// Test axis L = A1 x B1
+	ra = a_half_extents[0] * abs_r[1][2] + a_half_extents[2] * abs_r[1][0];
+	rb = b_half_extents_x * abs_r[2][1] + b_half_extents_z * abs_r[0][1];
+	overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[0] * inOrientation(2, 1) - t[2] * inOrientation(0, 1)).Abs(), ra + rb));
+
+	// Test axis L = A1 x B2
+	ra = a_half_extents[0] * abs_r[2][2] + a_half_extents[2] * abs_r[2][0];
+	rb = b_half_extents_x * abs_r[1][1] + b_half_extents_y * abs_r[0][1];
+	overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[0] * inOrientation(2, 2) - t[2] * inOrientation(0, 2)).Abs(), ra + rb));
+
+	// Test axis L = A2 x B0
+	ra = a_half_extents[0] * abs_r[0][1] + a_half_extents[1] * abs_r[0][0];
+	rb = b_half_extents_y * abs_r[2][2] + b_half_extents_z * abs_r[1][2];
+	overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[1] * inOrientation(0, 0) - t[0] * inOrientation(1, 0)).Abs(), ra + rb));
+
+	// Test axis L = A2 x B1
+	ra = a_half_extents[0] * abs_r[1][1] + a_half_extents[1] * abs_r[1][0];
+	rb = b_half_extents_x * abs_r[2][2] + b_half_extents_z * abs_r[0][2];
+	overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[1] * inOrientation(0, 1) - t[0] * inOrientation(1, 1)).Abs(), ra + rb));
+
+	// Test axis L = A2 x B2
+	ra = a_half_extents[0] * abs_r[2][1] + a_half_extents[1] * abs_r[2][0];
+	rb = b_half_extents_x * abs_r[1][2] + b_half_extents_y * abs_r[0][2];
+	overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[1] * inOrientation(0, 2) - t[0] * inOrientation(1, 2)).Abs(), ra + rb));
+
+	// Return if the OBB vs AABBs are intersecting
+	return overlaps;
+}
+
+/// Convenience function that tests 4 AABoxes vs OrientedBox
+JPH_INLINE UVec4 AABox4VsBox(const OrientedBox &inBox, Vec4Arg inBoxMinX, Vec4Arg inBoxMinY, Vec4Arg inBoxMinZ, Vec4Arg inBoxMaxX, Vec4Arg inBoxMaxY, Vec4Arg inBoxMaxZ, float inEpsilon = 1.0e-6f)
+{
+	return AABox4VsBox(inBox.mOrientation, inBox.mHalfExtents, inBoxMinX, inBoxMinY, inBoxMinZ, inBoxMaxX, inBoxMaxY, inBoxMaxZ, inEpsilon);
+}
+
+/// Get the squared distance between 4 AABoxes and a point
+JPH_INLINE Vec4 AABox4DistanceSqToPoint(Vec4Arg inPointX, Vec4Arg inPointY, Vec4Arg inPointZ, Vec4Arg inBoxMinX, Vec4Arg inBoxMinY, Vec4Arg inBoxMinZ, Vec4Arg inBoxMaxX, Vec4Arg inBoxMaxY, Vec4Arg inBoxMaxZ)
+{
+	// Get closest point on box
+	Vec4 closest_x = Vec4::sMin(Vec4::sMax(inPointX, inBoxMinX), inBoxMaxX);
+	Vec4 closest_y = Vec4::sMin(Vec4::sMax(inPointY, inBoxMinY), inBoxMaxY);
+	Vec4 closest_z = Vec4::sMin(Vec4::sMax(inPointZ, inBoxMinZ), inBoxMaxZ);
+
+	// Return the squared distance between the box and point
+	return Square(closest_x - inPointX) + Square(closest_y - inPointY) + Square(closest_z - inPointZ);
+}
+
+/// Get the squared distance between 4 AABoxes and a point
+JPH_INLINE Vec4 AABox4DistanceSqToPoint(Vec3 inPoint, Vec4Arg inBoxMinX, Vec4Arg inBoxMinY, Vec4Arg inBoxMinZ, Vec4Arg inBoxMaxX, Vec4Arg inBoxMaxY, Vec4Arg inBoxMaxZ)
+{
+	return AABox4DistanceSqToPoint(inPoint.SplatX(), inPoint.SplatY(), inPoint.SplatZ(), inBoxMinX, inBoxMinY, inBoxMinZ, inBoxMaxX, inBoxMaxY, inBoxMaxZ);
+}
+
+/// Test 4 AABoxes vs a sphere
+JPH_INLINE UVec4 AABox4VsSphere(Vec4Arg inCenterX, Vec4Arg inCenterY, Vec4Arg inCenterZ, Vec4Arg inRadiusSq, Vec4Arg inBoxMinX, Vec4Arg inBoxMinY, Vec4Arg inBoxMinZ, Vec4Arg inBoxMaxX, Vec4Arg inBoxMaxY, Vec4Arg inBoxMaxZ)
+{
+	// Test the distance from the center of the sphere to the box is smaller than the radius
+	Vec4 distance_sq = AABox4DistanceSqToPoint(inCenterX, inCenterY, inCenterZ, inBoxMinX, inBoxMinY, inBoxMinZ, inBoxMaxX, inBoxMaxY, inBoxMaxZ);
+	return Vec4::sLessOrEqual(distance_sq, inRadiusSq);
+}
+
+/// Test 4 AABoxes vs a sphere
+JPH_INLINE UVec4 AABox4VsSphere(Vec3Arg inCenter, float inRadiusSq, Vec4Arg inBoxMinX, Vec4Arg inBoxMinY, Vec4Arg inBoxMinZ, Vec4Arg inBoxMaxX, Vec4Arg inBoxMaxY, Vec4Arg inBoxMaxZ)
+{
+	return AABox4VsSphere(inCenter.SplatX(), inCenter.SplatY(), inCenter.SplatZ(), Vec4::sReplicate(inRadiusSq), inBoxMinX, inBoxMinY, inBoxMinZ, inBoxMaxX, inBoxMaxY, inBoxMaxZ);
+}
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Geometry/ClipPoly.h

@@ -0,0 +1,200 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Geometry/AABox.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Clip inPolygonToClip against the positive halfspace of plane defined by inPlaneOrigin and inPlaneNormal.
+/// inPlaneNormal does not need to be normalized.
+template <class VERTEX_ARRAY>
+void ClipPolyVsPlane(const VERTEX_ARRAY &inPolygonToClip, Vec3Arg inPlaneOrigin, Vec3Arg inPlaneNormal, VERTEX_ARRAY &outClippedPolygon)
+{
+	JPH_ASSERT(inPolygonToClip.size() >= 2);
+	JPH_ASSERT(outClippedPolygon.empty());
+
+	// Determine state of last point
+	Vec3 e1 = inPolygonToClip[inPolygonToClip.size() - 1];
+	float prev_num = (inPlaneOrigin - e1).Dot(inPlaneNormal);
+	bool prev_inside = prev_num < 0.0f;
+
+	// Loop through all vertices
+	for (typename VERTEX_ARRAY::size_type j = 0; j < inPolygonToClip.size(); ++j)
+	{
+		// Check if second point is inside
+		Vec3Arg e2 = inPolygonToClip[j];
+		float num = (inPlaneOrigin - e2).Dot(inPlaneNormal);
+		bool cur_inside = num < 0.0f;
+
+		// In -> Out or Out -> In: Add point on clipping plane
+		if (cur_inside != prev_inside)
+		{
+			// Solve: (X - inPlaneOrigin) . inPlaneNormal = 0 and X = e1 + t * (e2 - e1) for X
+			Vec3 e12 = e2 - e1;
+			float denom = e12.Dot(inPlaneNormal);
+			if (denom != 0.0f)
+				outClippedPolygon.push_back(e1 + (prev_num / denom) * e12);
+			else
+				cur_inside = prev_inside; // Edge is parallel to plane, treat point as if it were on the same side as the last point
+		}
+
+		// Point inside, add it
+		if (cur_inside)
+			outClippedPolygon.push_back(e2);
+
+		// Update previous state
+		prev_num = num;
+		prev_inside = cur_inside;
+		e1 = e2;
+	}
+}
+
+/// Clip polygon versus polygon.
+/// Both polygons are assumed to be in counter clockwise order.
+/// @param inClippingPolygonNormal is used to create planes of all edges in inClippingPolygon against which inPolygonToClip is clipped, inClippingPolygonNormal does not need to be normalized
+/// @param inClippingPolygon is the polygon which inClippedPolygon is clipped against
+/// @param inPolygonToClip is the polygon that is clipped
+/// @param outClippedPolygon will contain clipped polygon when function returns
+template <class VERTEX_ARRAY>
+void ClipPolyVsPoly(const VERTEX_ARRAY &inPolygonToClip, const VERTEX_ARRAY &inClippingPolygon, Vec3Arg inClippingPolygonNormal, VERTEX_ARRAY &outClippedPolygon)
+{
+	JPH_ASSERT(inPolygonToClip.size() >= 2);
+	JPH_ASSERT(inClippingPolygon.size() >= 3);
+
+	VERTEX_ARRAY tmp_vertices[2];
+	int tmp_vertices_idx = 0;
+
+	for (typename VERTEX_ARRAY::size_type i = 0; i < inClippingPolygon.size(); ++i)
+	{
+		// Get edge to clip against
+		Vec3 clip_e1 = inClippingPolygon[i];
+		Vec3 clip_e2 = inClippingPolygon[(i + 1) % inClippingPolygon.size()];
+		Vec3 clip_normal = inClippingPolygonNormal.Cross(clip_e2 - clip_e1); // Pointing inward to the clipping polygon
+
+		// Get source and target polygon
+		const VERTEX_ARRAY &src_polygon = (i == 0)? inPolygonToClip : tmp_vertices[tmp_vertices_idx];
+		tmp_vertices_idx ^= 1;
+		VERTEX_ARRAY &tgt_polygon = (i == inClippingPolygon.size() - 1)? outClippedPolygon : tmp_vertices[tmp_vertices_idx];
+		tgt_polygon.clear();
+
+		// Clip against the edge
+		ClipPolyVsPlane(src_polygon, clip_e1, clip_normal, tgt_polygon);
+
+		// Break out if no polygon left
+		if (tgt_polygon.size() < 3)
+		{
+			outClippedPolygon.clear();
+			break;
+		}
+	}
+}
+
+/// Clip inPolygonToClip against an edge, the edge is projected on inPolygonToClip using inClippingEdgeNormal.
+/// The positive half space (the side on the edge in the direction of inClippingEdgeNormal) is cut away.
+template <class VERTEX_ARRAY>
+void ClipPolyVsEdge(const VERTEX_ARRAY &inPolygonToClip, Vec3Arg inEdgeVertex1, Vec3Arg inEdgeVertex2, Vec3Arg inClippingEdgeNormal, VERTEX_ARRAY &outClippedPolygon)
+{
+	JPH_ASSERT(inPolygonToClip.size() >= 3);
+	JPH_ASSERT(outClippedPolygon.empty());
+
+	// Get normal that is perpendicular to the edge and the clipping edge normal
+	Vec3 edge = inEdgeVertex2 - inEdgeVertex1;
+	Vec3 edge_normal = inClippingEdgeNormal.Cross(edge);
+
+	// Project vertices of edge on inPolygonToClip
+	Vec3 polygon_normal = (inPolygonToClip[2] - inPolygonToClip[0]).Cross(inPolygonToClip[1] - inPolygonToClip[0]);
+	float polygon_normal_len_sq = polygon_normal.LengthSq();
+	Vec3 v1 = inEdgeVertex1 + polygon_normal.Dot(inPolygonToClip[0] - inEdgeVertex1) * polygon_normal / polygon_normal_len_sq;
+	Vec3 v2 = inEdgeVertex2 + polygon_normal.Dot(inPolygonToClip[0] - inEdgeVertex2) * polygon_normal / polygon_normal_len_sq;
+	Vec3 v12 = v2 - v1;
+	float v12_len_sq = v12.LengthSq();
+
+	// Determine state of last point
+	Vec3 e1 = inPolygonToClip[inPolygonToClip.size() - 1];
+	float prev_num = (inEdgeVertex1 - e1).Dot(edge_normal);
+	bool prev_inside = prev_num < 0.0f;
+
+	// Loop through all vertices
+	for (typename VERTEX_ARRAY::size_type j = 0; j < inPolygonToClip.size(); ++j)
+	{
+		// Check if second point is inside
+		Vec3 e2 = inPolygonToClip[j];
+		float num = (inEdgeVertex1 - e2).Dot(edge_normal);
+		bool cur_inside = num < 0.0f;
+
+		// In -> Out or Out -> In: Add point on clipping plane
+		if (cur_inside != prev_inside)
+		{
+			// Solve: (inEdgeVertex1 - X) . edge_normal = 0 and X = e1 + t * (e2 - e1) for X
+			Vec3 e12 = e2 - e1;
+			float denom = e12.Dot(edge_normal);
+			Vec3 clipped_point = denom != 0.0f? e1 + (prev_num / denom) * e12 : e1;
+
+			// Project point on line segment v1, v2 so see if it falls outside if the edge
+			float projection = (clipped_point - v1).Dot(v12);
+			if (projection < 0.0f)
+				outClippedPolygon.push_back(v1);
+			else if (projection > v12_len_sq)
+				outClippedPolygon.push_back(v2);
+			else
+				outClippedPolygon.push_back(clipped_point);
+		}
+
+		// Update previous state
+		prev_num = num;
+		prev_inside = cur_inside;
+		e1 = e2;
+	}
+}
+
+/// Clip polygon vs axis aligned box, inPolygonToClip is assume to be in counter clockwise order.
+/// Output will be stored in outClippedPolygon. Everything inside inAABox will be kept.
+template <class VERTEX_ARRAY>
+void ClipPolyVsAABox(const VERTEX_ARRAY &inPolygonToClip, const AABox &inAABox, VERTEX_ARRAY &outClippedPolygon)
+{
+	JPH_ASSERT(inPolygonToClip.size() >= 2);
+
+	VERTEX_ARRAY tmp_vertices[2];
+	int tmp_vertices_idx = 0;
+
+	for (int coord = 0; coord < 3; ++coord)
+		for (int side = 0; side < 2; ++side)
+		{
+			// Get plane to clip against
+			Vec3 origin = Vec3::sZero(), normal = Vec3::sZero();
+			if (side == 0)
+			{
+				normal.SetComponent(coord, 1.0f);
+				origin.SetComponent(coord, inAABox.mMin[coord]);
+			}
+			else
+			{
+				normal.SetComponent(coord, -1.0f);
+				origin.SetComponent(coord, inAABox.mMax[coord]);
+			}
+
+			// Get source and target polygon
+			const VERTEX_ARRAY &src_polygon = tmp_vertices_idx == 0? inPolygonToClip : tmp_vertices[tmp_vertices_idx & 1];
+			tmp_vertices_idx++;
+			VERTEX_ARRAY &tgt_polygon = tmp_vertices_idx == 6? outClippedPolygon : tmp_vertices[tmp_vertices_idx & 1];
+			tgt_polygon.clear();
+
+			// Clip against the edge
+			ClipPolyVsPlane(src_polygon, origin, normal, tgt_polygon);
+
+			// Break out if no polygon left
+			if (tgt_polygon.size() < 3)
+			{
+				outClippedPolygon.clear();
+				return;
+			}
+
+			// Flip normal
+			normal = -normal;
+		}
+}
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Geometry/ClosestPoint.h

@@ -0,0 +1,498 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+// Turn off fused multiply add instruction because it makes the equations of the form a * b - c * d inaccurate below
+JPH_PRECISE_MATH_ON
+
+/// Helper utils to find the closest point to a line segment, triangle or tetrahedron
+namespace ClosestPoint
+{
+	/// Compute barycentric coordinates of closest point to origin for infinite line defined by (inA, inB)
+	/// Point can then be computed as inA * outU + inB * outV
+	/// Returns false if the points inA, inB do not form a line (are at the same point)
+	inline bool GetBaryCentricCoordinates(Vec3Arg inA, Vec3Arg inB, float &outU, float &outV)
+	{
+		Vec3 ab = inB - inA;
+		float denominator = ab.LengthSq();
+		if (denominator < Square(FLT_EPSILON))
+		{
+			// Degenerate line segment, fallback to points
+			if (inA.LengthSq() < inB.LengthSq())
+			{
+				// A closest
+				outU = 1.0f;
+				outV = 0.0f;
+			}
+			else
+			{
+				// B closest
+				outU = 0.0f;
+				outV = 1.0f;
+			}
+			return false;
+		}
+		else
+		{
+			outV = -inA.Dot(ab) / denominator;
+			outU = 1.0f - outV;
+		}
+		return true;
+	}
+
+	/// Compute barycentric coordinates of closest point to origin for plane defined by (inA, inB, inC)
+	/// Point can then be computed as inA * outU + inB * outV + inC * outW
+	/// Returns false if the points inA, inB, inC do not form a plane (are on the same line or at the same point)
+	inline bool GetBaryCentricCoordinates(Vec3Arg inA, Vec3Arg inB, Vec3Arg inC, float &outU, float &outV, float &outW)
+	{
+		// Taken from: Real-Time Collision Detection - Christer Ericson (Section: Barycentric Coordinates)
+		// With p = 0
+		// Adjusted to always include the shortest edge of the triangle in the calculation to improve numerical accuracy
+
+		// First calculate the three edges
+		Vec3 v0 = inB - inA;
+		Vec3 v1 = inC - inA;
+		Vec3 v2 = inC - inB;
+
+		// Make sure that the shortest edge is included in the calculation to keep the products a * b - c * d as small as possible to preserve accuracy
+		float d00 = v0.LengthSq();
+		float d11 = v1.LengthSq();
+		float d22 = v2.LengthSq();
+		if (d00 <= d22)
+		{
+			// Use v0 and v1 to calculate barycentric coordinates
+			float d01 = v0.Dot(v1);
+
+			// Denominator must be positive:
+			// |v0|^2 * |v1|^2 - (v0 . v1)^2 = |v0|^2 * |v1|^2 * (1 - cos(angle)^2) >= 0
+			float denominator = d00 * d11 - d01 * d01;
+			if (denominator < 1.0e-12f)
+			{
+				// Degenerate triangle, return coordinates along longest edge
+				if (d00 > d11)
+				{
+					GetBaryCentricCoordinates(inA, inB, outU, outV);
+					outW = 0.0f;
+				}
+				else
+				{
+					GetBaryCentricCoordinates(inA, inC, outU, outW);
+					outV = 0.0f;
+				}
+				return false;
+			}
+			else
+			{
+				float a0 = inA.Dot(v0);
+				float a1 = inA.Dot(v1);
+				outV = (d01 * a1 - d11 * a0) / denominator;
+				outW = (d01 * a0 - d00 * a1) / denominator;
+				outU = 1.0f - outV - outW;
+			}
+		}
+		else
+		{
+			// Use v1 and v2 to calculate barycentric coordinates
+			float d12 = v1.Dot(v2);
+
+			float denominator = d11 * d22 - d12 * d12;
+			if (denominator < 1.0e-12f)
+			{
+				// Degenerate triangle, return coordinates along longest edge
+				if (d11 > d22)
+				{
+					GetBaryCentricCoordinates(inA, inC, outU, outW);
+					outV = 0.0f;
+				}
+				else
+				{
+					GetBaryCentricCoordinates(inB, inC, outV, outW);
+					outU = 0.0f;
+				}
+				return false;
+			}
+			else
+			{
+				float c1 = inC.Dot(v1);
+				float c2 = inC.Dot(v2);
+				outU = (d22 * c1 - d12 * c2) / denominator;
+				outV = (d11 * c2 - d12 * c1) / denominator;
+				outW = 1.0f - outU - outV;
+			}
+		}
+		return true;
+	}
+
+	/// Get the closest point to the origin of line (inA, inB)
+	/// outSet describes which features are closest: 1 = a, 2 = b, 3 = line segment ab
+	inline Vec3	GetClosestPointOnLine(Vec3Arg inA, Vec3Arg inB, uint32 &outSet)
+	{
+		float u, v;
+		GetBaryCentricCoordinates(inA, inB, u, v);
+		if (v <= 0.0f)
+		{
+			// inA is closest point
+			outSet = 0b0001;
+			return inA;
+		}
+		else if (u <= 0.0f)
+		{
+			// inB is closest point
+			outSet = 0b0010;
+			return inB;
+		}
+		else
+		{
+			// Closest point lies on line inA inB
+			outSet = 0b0011;
+			return u * inA + v * inB;
+		}
+	}
+
+	/// Get the closest point to the origin of triangle (inA, inB, inC)
+	/// outSet describes which features are closest: 1 = a, 2 = b, 4 = c, 5 = line segment ac, 7 = triangle interior etc.
+	/// If MustIncludeC is true, the function assumes that C is part of the closest feature (vertex, edge, face) and does less work, if the assumption is not true then a closest point to the other features is returned.
+	template <bool MustIncludeC = false>
+	inline Vec3	GetClosestPointOnTriangle(Vec3Arg inA, Vec3Arg inB, Vec3Arg inC, uint32 &outSet)
+	{
+		// Taken from: Real-Time Collision Detection - Christer Ericson (Section: Closest Point on Triangle to Point)
+		// With p = 0
+
+		// The most accurate normal is calculated by using the two shortest edges
+		// See: https://box2d.org/posts/2014/01/troublesome-triangle/
+		// The difference in normals is most pronounced when one edge is much smaller than the others (in which case the other 2 must have roughly the same length).
+		// Therefore we can suffice by just picking the shortest from 2 edges and use that with the 3rd edge to calculate the normal.
+		// We first check which of the edges is shorter and if bc is shorter than ac then we swap a with c to a is always on the shortest edge
+		UVec4 swap_ac;
+		{
+			Vec3 ac = inC - inA;
+			Vec3 bc = inC - inB;
+			swap_ac = Vec4::sLess(bc.DotV4(bc), ac.DotV4(ac));
+		}
+		Vec3 a = Vec3::sSelect(inA, inC, swap_ac);
+		Vec3 c = Vec3::sSelect(inC, inA, swap_ac);
+
+		// Calculate normal
+		Vec3 ab = inB - a;
+		Vec3 ac = c - a;
+		Vec3 n = ab.Cross(ac);
+		float n_len_sq = n.LengthSq();
+
+		// Check degenerate
+		if (n_len_sq < 1.0e-10f) // Square(FLT_EPSILON) was too small and caused numerical problems, see test case TestCollideParallelTriangleVsCapsule
+		{
+			// Degenerate, fallback to vertices and edges
+
+			// Start with vertex C being the closest
+			uint32 closest_set = 0b0100;
+			Vec3 closest_point = inC;
+			float best_dist_sq = inC.LengthSq();
+
+			// If the closest point must include C then A or B cannot be closest
+			// Note that we test vertices first because we want to prefer a closest vertex over a closest edge (this results in an outSet with fewer bits set)
+			if constexpr (!MustIncludeC)
+			{
+				// Try vertex A
+				float a_len_sq = inA.LengthSq();
+				if (a_len_sq < best_dist_sq)
+				{
+					closest_set = 0b0001;
+					closest_point = inA;
+					best_dist_sq = a_len_sq;
+				}
+
+				// Try vertex B
+				float b_len_sq = inB.LengthSq();
+				if (b_len_sq < best_dist_sq)
+				{
+					closest_set = 0b0010;
+					closest_point = inB;
+					best_dist_sq = b_len_sq;
+				}
+			}
+
+			// Edge AC
+			float ac_len_sq = ac.LengthSq();
+			if (ac_len_sq > Square(FLT_EPSILON))
+			{
+				float v = Clamp(-a.Dot(ac) / ac_len_sq, 0.0f, 1.0f);
+				Vec3 q = a + v * ac;
+				float dist_sq = q.LengthSq();
+				if (dist_sq < best_dist_sq)
+				{
+					closest_set = 0b0101;
+					closest_point = q;
+					best_dist_sq = dist_sq;
+				}
+			}
+
+			// Edge BC
+			Vec3 bc = inC - inB;
+			float bc_len_sq = bc.LengthSq();
+			if (bc_len_sq > Square(FLT_EPSILON))
+			{
+				float v = Clamp(-inB.Dot(bc) / bc_len_sq, 0.0f, 1.0f);
+				Vec3 q = inB + v * bc;
+				float dist_sq = q.LengthSq();
+				if (dist_sq < best_dist_sq)
+				{
+					closest_set = 0b0110;
+					closest_point = q;
+					best_dist_sq = dist_sq;
+				}
+			}
+
+			// If the closest point must include C then AB cannot be closest
+			if constexpr (!MustIncludeC)
+			{
+				// Edge AB
+				ab = inB - inA;
+				float ab_len_sq = ab.LengthSq();
+				if (ab_len_sq > Square(FLT_EPSILON))
+				{
+					float v = Clamp(-inA.Dot(ab) / ab_len_sq, 0.0f, 1.0f);
+					Vec3 q = inA + v * ab;
+					float dist_sq = q.LengthSq();
+					if (dist_sq < best_dist_sq)
+					{
+						closest_set = 0b0011;
+						closest_point = q;
+						best_dist_sq = dist_sq;
+					}
+				}
+			}
+
+			outSet = closest_set;
+			return closest_point;
+		}
+
+		// Check if P in vertex region outside A
+		Vec3 ap = -a;
+		float d1 = ab.Dot(ap);
+		float d2 = ac.Dot(ap);
+		if (d1 <= 0.0f && d2 <= 0.0f)
+		{
+			outSet = swap_ac.GetX()? 0b0100 : 0b0001;
+			return a; // barycentric coordinates (1,0,0)
+		}
+
+		// Check if P in vertex region outside B
+		Vec3 bp = -inB;
+		float d3 = ab.Dot(bp);
+		float d4 = ac.Dot(bp);
+		if (d3 >= 0.0f && d4 <= d3)
+		{
+			outSet = 0b0010;
+			return inB; // barycentric coordinates (0,1,0)
+		}
+
+		// Check if P in edge region of AB, if so return projection of P onto AB
+		if (d1 * d4 <= d3 * d2 && d1 >= 0.0f && d3 <= 0.0f)
+		{
+			float v = d1 / (d1 - d3);
+			outSet = swap_ac.GetX()? 0b0110 : 0b0011;
+			return a + v * ab; // barycentric coordinates (1-v,v,0)
+		}
+
+		// Check if P in vertex region outside C
+		Vec3 cp = -c;
+		float d5 = ab.Dot(cp);
+		float d6 = ac.Dot(cp);
+		if (d6 >= 0.0f && d5 <= d6)
+		{
+			outSet = swap_ac.GetX()? 0b0001 : 0b0100;
+			return c; // barycentric coordinates (0,0,1)
+		}
+
+		// Check if P in edge region of AC, if so return projection of P onto AC
+		if (d5 * d2 <= d1 * d6 && d2 >= 0.0f && d6 <= 0.0f)
+		{
+			float w = d2 / (d2 - d6);
+			outSet = 0b0101;
+			return a + w * ac; // barycentric coordinates (1-w,0,w)
+		}
+
+		// Check if P in edge region of BC, if so return projection of P onto BC
+		float d4_d3 = d4 - d3;
+		float d5_d6 = d5 - d6;
+		if (d3 * d6 <= d5 * d4 && d4_d3 >= 0.0f && d5_d6 >= 0.0f)
+		{
+			float w = d4_d3 / (d4_d3 + d5_d6);
+			outSet = swap_ac.GetX()? 0b0011 : 0b0110;
+			return inB + w * (c - inB); // barycentric coordinates (0,1-w,w)
+		}
+
+		// P inside face region.
+		// Here we deviate from Christer Ericson's article to improve accuracy.
+		// Determine distance between triangle and origin: distance = (centroid - origin) . normal / |normal|
+		// Closest point to origin is then: distance . normal / |normal|
+		// Note that this way of calculating the closest point is much more accurate than first calculating barycentric coordinates
+		// and then calculating the closest point based on those coordinates.
+		outSet = 0b0111;
+		return n * (a + inB + c).Dot(n) / (3.0f * n_len_sq);
+	}
+
+	/// Check if the origin is outside the plane of triangle (inA, inB, inC). inD specifies the front side of the plane.
+	inline bool OriginOutsideOfPlane(Vec3Arg inA, Vec3Arg inB, Vec3Arg inC, Vec3Arg inD)
+	{
+		// Taken from: Real-Time Collision Detection - Christer Ericson (Section: Closest Point on Tetrahedron to Point)
+		// With p = 0
+
+		// Test if point p and d lie on opposite sides of plane through abc
+		Vec3 n = (inB - inA).Cross(inC - inA);
+		float signp = inA.Dot(n); // [AP AB AC]
+		float signd = (inD - inA).Dot(n); // [AD AB AC]
+
+		// Points on opposite sides if expression signs are the same
+		// Note that we left out the minus sign in signp so we need to check > 0 instead of < 0 as in Christer's book
+		// We compare against a small negative value to allow for a little bit of slop in the calculations
+		return signp * signd > -FLT_EPSILON;
+	}
+
+	/// Returns for each of the planes of the tetrahedron if the origin is inside it
+	/// Roughly equivalent to:
+	///	[OriginOutsideOfPlane(inA, inB, inC, inD),
+	///	 OriginOutsideOfPlane(inA, inC, inD, inB),
+	///	 OriginOutsideOfPlane(inA, inD, inB, inC),
+	///	 OriginOutsideOfPlane(inB, inD, inC, inA)]
+	inline UVec4 OriginOutsideOfTetrahedronPlanes(Vec3Arg inA, Vec3Arg inB, Vec3Arg inC, Vec3Arg inD)
+	{
+		Vec3 ab = inB - inA;
+		Vec3 ac = inC - inA;
+		Vec3 ad = inD - inA;
+		Vec3 bd = inD - inB;
+		Vec3 bc = inC - inB;
+
+		Vec3 ab_cross_ac = ab.Cross(ac);
+		Vec3 ac_cross_ad = ac.Cross(ad);
+		Vec3 ad_cross_ab = ad.Cross(ab);
+		Vec3 bd_cross_bc = bd.Cross(bc);
+
+		// For each plane get the side on which the origin is
+		float signp0 = inA.Dot(ab_cross_ac); // ABC
+		float signp1 = inA.Dot(ac_cross_ad); // ACD
+		float signp2 = inA.Dot(ad_cross_ab); // ADB
+		float signp3 = inB.Dot(bd_cross_bc); // BDC
+		Vec4 signp(signp0, signp1, signp2, signp3);
+
+		// For each plane get the side that is outside (determined by the 4th point)
+		float signd0 = ad.Dot(ab_cross_ac);  // D
+		float signd1 = ab.Dot(ac_cross_ad);  // B
+		float signd2 = ac.Dot(ad_cross_ab);  // C
+		float signd3 = -ab.Dot(bd_cross_bc); // A
+		Vec4 signd(signd0, signd1, signd2, signd3);
+
+		// The winding of all triangles has been chosen so that signd should have the
+		// same sign for all components. If this is not the case the tetrahedron
+		// is degenerate and we return that the origin is in front of all sides
+		int sign_bits = signd.GetSignBits();
+		switch (sign_bits)
+		{
+		case 0:
+			// All positive
+			return Vec4::sGreaterOrEqual(signp, Vec4::sReplicate(-FLT_EPSILON));
+
+		case 0xf:
+			// All negative
+			return Vec4::sLessOrEqual(signp, Vec4::sReplicate(FLT_EPSILON));
+
+		default:
+			// Mixed signs, degenerate tetrahedron
+			return UVec4::sReplicate(0xffffffff);
+		}
+	}
+
+	/// Get the closest point between tetrahedron (inA, inB, inC, inD) to the origin
+	/// outSet specifies which feature was closest, 1 = a, 2 = b, 4 = c, 8 = d. Edges have 2 bits set, triangles 3 and if the point is in the interior 4 bits are set.
+	/// If MustIncludeD is true, the function assumes that D is part of the closest feature (vertex, edge, face, tetrahedron) and does less work, if the assumption is not true then a closest point to the other features is returned.
+	template <bool MustIncludeD = false>
+	inline Vec3	GetClosestPointOnTetrahedron(Vec3Arg inA, Vec3Arg inB, Vec3Arg inC, Vec3Arg inD, uint32 &outSet)
+	{
+		// Taken from: Real-Time Collision Detection - Christer Ericson (Section: Closest Point on Tetrahedron to Point)
+		// With p = 0
+
+		// Start out assuming point inside all halfspaces, so closest to itself
+		uint32 closest_set = 0b1111;
+		Vec3 closest_point = Vec3::sZero();
+		float best_dist_sq = FLT_MAX;
+
+		// Determine for each of the faces of the tetrahedron if the origin is in front of the plane
+		UVec4 origin_out_of_planes = OriginOutsideOfTetrahedronPlanes(inA, inB, inC, inD);
+
+		// If point outside face abc then compute closest point on abc
+		if (origin_out_of_planes.GetX()) // OriginOutsideOfPlane(inA, inB, inC, inD)
+		{
+			if constexpr (MustIncludeD)
+			{
+				// If the closest point must include D then ABC cannot be closest but the closest point
+				// cannot be an interior point either so we return A as closest point
+				closest_set = 0b0001;
+				closest_point = inA;
+			}
+			else
+			{
+				// Test the face normally
+				closest_point = GetClosestPointOnTriangle<false>(inA, inB, inC, closest_set);
+			}
+			best_dist_sq = closest_point.LengthSq();
+		}
+
+		// Repeat test for face acd
+		if (origin_out_of_planes.GetY()) // OriginOutsideOfPlane(inA, inC, inD, inB)
+		{
+			uint32 set;
+			Vec3 q = GetClosestPointOnTriangle<MustIncludeD>(inA, inC, inD, set);
+			float dist_sq = q.LengthSq();
+			if (dist_sq < best_dist_sq)
+			{
+				best_dist_sq = dist_sq;
+				closest_point = q;
+				closest_set = (set & 0b0001) + ((set & 0b0110) << 1);
+			}
+		}
+
+		// Repeat test for face adb
+		if (origin_out_of_planes.GetZ()) // OriginOutsideOfPlane(inA, inD, inB, inC)
+		{
+			// Keep original vertex order, it doesn't matter if the triangle is facing inward or outward
+			// and it improves consistency for GJK which will always add a new vertex D and keep the closest
+			// feature from the previous iteration in ABC
+			uint32 set;
+			Vec3 q = GetClosestPointOnTriangle<MustIncludeD>(inA, inB, inD, set);
+			float dist_sq = q.LengthSq();
+			if (dist_sq < best_dist_sq)
+			{
+				best_dist_sq = dist_sq;
+				closest_point = q;
+				closest_set = (set & 0b0011) + ((set & 0b0100) << 1);
+			}
+		}
+
+		// Repeat test for face bdc
+		if (origin_out_of_planes.GetW()) // OriginOutsideOfPlane(inB, inD, inC, inA)
+		{
+			// Keep original vertex order, it doesn't matter if the triangle is facing inward or outward
+			// and it improves consistency for GJK which will always add a new vertex D and keep the closest
+			// feature from the previous iteration in ABC
+			uint32 set;
+			Vec3 q = GetClosestPointOnTriangle<MustIncludeD>(inB, inC, inD, set);
+			float dist_sq = q.LengthSq();
+			if (dist_sq < best_dist_sq)
+			{
+				closest_point = q;
+				closest_set = set << 1;
+			}
+		}
+
+		outSet = closest_set;
+		return closest_point;
+	}
+};
+
+JPH_PRECISE_MATH_OFF
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Geometry/ConvexHullBuilder.h

@@ -0,0 +1,276 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+//#define JPH_CONVEX_BUILDER_DEBUG
+//#define JPH_CONVEX_BUILDER_DUMP_SHAPE
+
+#ifdef JPH_CONVEX_BUILDER_DEBUG
+	#include <Jolt/Core/Color.h>
+#endif
+
+#include <Jolt/Core/StaticArray.h>
+#include <Jolt/Core/NonCopyable.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// A convex hull builder that tries to create hulls as accurately as possible. Used for offline processing.
+class JPH_EXPORT ConvexHullBuilder : public NonCopyable
+{
+public:
+	// Forward declare
+	class Face;
+
+	/// Class that holds the information of an edge
+	class Edge : public NonCopyable
+	{
+	public:
+		JPH_OVERRIDE_NEW_DELETE
+
+		/// Constructor
+						Edge(Face *inFace, int inStartIdx)	: mFace(inFace), mStartIdx(inStartIdx) { }
+
+		/// Get the previous edge
+		inline Edge *	GetPreviousEdge()
+		{
+			Edge *prev_edge = this;
+			while (prev_edge->mNextEdge != this)
+				prev_edge = prev_edge->mNextEdge;
+			return prev_edge;
+		}
+
+		Face *			mFace;								///< Face that this edge belongs to
+		Edge *			mNextEdge = nullptr;				///< Next edge of this face
+		Edge *			mNeighbourEdge = nullptr;			///< Edge that this edge is connected to
+		int				mStartIdx;							///< Vertex index in mPositions that indicates the start vertex of this edge
+	};
+
+	using ConflictList = Array<int>;
+
+	/// Class that holds the information of one face
+	class Face : public NonCopyable
+	{
+	public:
+		JPH_OVERRIDE_NEW_DELETE
+
+		/// Destructor
+						~Face();
+
+		/// Initialize a face with three indices
+		void			Initialize(int inIdx0, int inIdx1, int inIdx2, const Vec3 *inPositions);
+
+		/// Calculates the centroid and normal for this face
+		void			CalculateNormalAndCentroid(const Vec3 *inPositions);
+
+		/// Check if face inFace is facing inPosition
+		inline bool		IsFacing(Vec3Arg inPosition) const
+		{
+			JPH_ASSERT(!mRemoved);
+			return mNormal.Dot(inPosition - mCentroid) > 0.0f;
+		}
+
+		Vec3			mNormal;							///< Normal of this face, length is 2 times area of face
+		Vec3			mCentroid;							///< Center of the face
+		ConflictList	mConflictList;						///< Positions associated with this edge (that are closest to this edge). The last position in the list is the point that is furthest away from the face.
+		Edge *			mFirstEdge = nullptr;				///< First edge of this face
+		float			mFurthestPointDistanceSq = 0.0f;	///< Squared distance of furthest point from the conflict list to the face
+		bool			mRemoved = false;					///< Flag that indicates that face has been removed (face will be freed later)
+#ifdef JPH_CONVEX_BUILDER_DEBUG
+		int				mIteration;							///< Iteration that this face was created
+#endif
+	};
+
+	// Typedefs
+	using Positions = Array<Vec3>;
+	using Faces = Array<Face *>;
+
+	/// Constructor
+	explicit			ConvexHullBuilder(const Positions &inPositions);
+
+	/// Destructor
+						~ConvexHullBuilder()				{ FreeFaces(); }
+
+	/// Result enum that indicates how the hull got created
+	enum class EResult
+	{
+		Success,											///< Hull building finished successfully
+		MaxVerticesReached,									///< Hull building finished successfully, but the desired accuracy was not reached because the max vertices limit was reached
+		TooFewPoints,										///< Too few points to create a hull
+		TooFewFaces,										///< Too few faces in the created hull (signifies precision errors during building)
+		Degenerate,											///< Degenerate hull detected
+	};
+
+	/// Takes all positions as provided by the constructor and use them to build a hull
+	/// Any points that are closer to the hull than inTolerance will be discarded
+	/// @param inMaxVertices Max vertices to allow in the hull. Specify INT_MAX if there is no limit.
+	/// @param inTolerance Max distance that a point is allowed to be outside of the hull
+	/// @param outError Error message when building fails
+	/// @return Status code that reports if the hull was created or not
+	EResult				Initialize(int inMaxVertices, float inTolerance, const char *&outError);
+
+	/// Returns the amount of vertices that are currently used by the hull
+	int					GetNumVerticesUsed() const;
+
+	/// Returns true if the hull contains a polygon with inIndices (counter clockwise indices in mPositions)
+	bool				ContainsFace(const Array<int> &inIndices) const;
+
+	/// Calculate the center of mass and the volume of the current convex hull
+	void				GetCenterOfMassAndVolume(Vec3 &outCenterOfMass, float &outVolume) const;
+
+	/// Determines the point that is furthest outside of the hull and reports how far it is outside of the hull (which indicates a failure during hull building)
+	/// @param outFaceWithMaxError The face that caused the error
+	/// @param outMaxError The maximum distance of a point to the hull
+	/// @param outMaxErrorPositionIdx The index of the point that had this distance
+	/// @param outCoplanarDistance Points that are less than this distance from the hull are considered on the hull. This should be used as a lowerbound for the allowed error.
+	void				DetermineMaxError(Face *&outFaceWithMaxError, float &outMaxError, int &outMaxErrorPositionIdx, float &outCoplanarDistance) const;
+
+	/// Access to the created faces. Memory is owned by the convex hull builder.
+	const Faces &		GetFaces() const					{ return mFaces; }
+
+private:
+	/// Minimal square area of a triangle (used for merging and checking if a triangle is degenerate)
+	static constexpr float cMinTriangleAreaSq = 1.0e-12f;
+
+#ifdef JPH_CONVEX_BUILDER_DEBUG
+	/// Factor to scale convex hull when debug drawing the construction process
+	static constexpr Real cDrawScale = 10;
+#endif
+
+	/// Class that holds an edge including start and end index
+	class FullEdge
+	{
+	public:
+		Edge *			mNeighbourEdge;						///< Edge that this edge is connected to
+		int				mStartIdx;							///< Vertex index in mPositions that indicates the start vertex of this edge
+		int				mEndIdx;							///< Vertex index in mPosition that indicates the end vertex of this edge
+	};
+
+	// Private typedefs
+	using FullEdges = Array<FullEdge>;
+
+	// Determine a suitable tolerance for detecting that points are coplanar
+	float				DetermineCoplanarDistance() const;
+
+	/// Find the face for which inPoint is furthest to the front
+	/// @param inPoint Point to test
+	/// @param inFaces List of faces to test
+	/// @param outFace Returns the best face
+	/// @param outDistSq Returns the squared distance how much inPoint is in front of the plane of the face
+	void				GetFaceForPoint(Vec3Arg inPoint, const Faces &inFaces, Face *&outFace, float &outDistSq) const;
+
+	/// @brief Calculates the distance between inPoint and inFace
+	/// @param inFace Face to test
+	/// @param inPoint Point to test
+	/// @return If the projection of the point on the plane is interior to the face 0, otherwise the squared distance to the closest edge
+	float				GetDistanceToEdgeSq(Vec3Arg inPoint, const Face *inFace) const;
+
+	/// Assigns a position to one of the supplied faces based on which face is closest.
+	/// @param inPositionIdx Index of the position to add
+	/// @param inFaces List of faces to consider
+	/// @param inToleranceSq Tolerance of the hull, if the point is closer to the face than this, we ignore it
+	/// @return True if point was assigned, false if it was discarded or added to the coplanar list
+	bool				AssignPointToFace(int inPositionIdx, const Faces &inFaces, float inToleranceSq);
+
+	/// Add a new point to the convex hull
+	void				AddPoint(Face *inFacingFace, int inIdx, float inToleranceSq, Faces &outNewFaces);
+
+	/// Remove all faces that have been marked 'removed' from mFaces list
+	void				GarbageCollectFaces();
+
+	/// Create a new face
+	Face *				CreateFace();
+
+	/// Create a new triangle
+	Face *				CreateTriangle(int inIdx1, int inIdx2, int inIdx3);
+
+	/// Delete a face (checking that it is not connected to any other faces)
+	void				FreeFace(Face *inFace);
+
+	/// Release all faces and edges
+	void				FreeFaces();
+
+	/// Link face edge to other face edge
+	static void			sLinkFace(Edge *inEdge1, Edge *inEdge2);
+
+	/// Unlink this face from all of its neighbours
+	static void			sUnlinkFace(Face *inFace);
+
+	/// Given one face that faces inVertex, find the edges of the faces that are not facing inVertex.
+	/// Will flag all those faces for removal.
+	void				FindEdge(Face *inFacingFace, Vec3Arg inVertex, FullEdges &outEdges) const;
+
+	/// Merges the two faces that share inEdge into the face inEdge->mFace
+	void				MergeFaces(Edge *inEdge);
+
+	/// Merges inFace with a neighbour if it is degenerate (a sliver)
+	void				MergeDegenerateFace(Face *inFace, Faces &ioAffectedFaces);
+
+	/// Merges any coplanar as well as neighbours that form a non-convex edge into inFace.
+	/// Faces are considered coplanar if the distance^2 of the other face's centroid is smaller than inToleranceSq.
+	void				MergeCoplanarOrConcaveFaces(Face *inFace, float inToleranceSq, Faces &ioAffectedFaces);
+
+	/// Mark face as affected if it is not already in the list
+	static void			sMarkAffected(Face *inFace, Faces &ioAffectedFaces);
+
+	/// Removes all invalid edges.
+	/// 1. Merges inFace with faces that share two edges with it since this means inFace or the other face cannot be convex or the edge is colinear.
+	/// 2. Removes edges that are interior to inFace (that have inFace on both sides)
+	/// Any faces that need to be checked for validity will be added to ioAffectedFaces.
+	void				RemoveInvalidEdges(Face *inFace, Faces &ioAffectedFaces);
+
+	/// Removes inFace if it consists of only 2 edges, linking its neighbouring faces together
+	/// Any faces that need to be checked for validity will be added to ioAffectedFaces.
+	/// @return True if face was removed.
+	bool				RemoveTwoEdgeFace(Face *inFace, Faces &ioAffectedFaces) const;
+
+#ifdef JPH_ENABLE_ASSERTS
+	/// Dumps the text representation of a face to the TTY
+	void				DumpFace(const Face *inFace) const;
+
+	/// Dumps the text representation of all faces to the TTY
+	void				DumpFaces() const;
+
+	/// Check consistency of 1 face
+	void				ValidateFace(const Face *inFace) const;
+
+	/// Check consistency of all faces
+	void				ValidateFaces() const;
+#endif
+
+#ifdef JPH_CONVEX_BUILDER_DEBUG
+	/// Draw state of algorithm
+	void				DrawState(bool inDrawConflictList = false) const;
+
+	/// Draw a face for debugging purposes
+	void				DrawWireFace(const Face *inFace, ColorArg inColor) const;
+
+	/// Draw an edge for debugging purposes
+	void				DrawEdge(const Edge *inEdge, ColorArg inColor) const;
+#endif
+
+#ifdef JPH_CONVEX_BUILDER_DUMP_SHAPE
+	void				DumpShape() const;
+#endif
+
+	const Positions &	mPositions;							///< List of positions (some of them are part of the hull)
+	Faces				mFaces;								///< List of faces that are part of the hull (if !mRemoved)
+
+	struct Coplanar
+	{
+		int				mPositionIdx;						///< Index in mPositions
+		float			mDistanceSq;						///< Distance to the edge of closest face (should be > 0)
+	};
+	using CoplanarList = Array<Coplanar>;
+
+	CoplanarList		mCoplanarList;						///< List of positions that are coplanar to a face but outside of the face, these are added to the hull at the end
+
+#ifdef JPH_CONVEX_BUILDER_DEBUG
+	int					mIteration;							///< Number of iterations we've had so far (for debug purposes)
+	mutable RVec3		mOffset;							///< Offset to use for state drawing
+	Vec3				mDelta;								///< Delta offset between next states
+#endif
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Geometry/ConvexHullBuilder2D.cpp

@@ -0,0 +1,335 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Geometry/ConvexHullBuilder2D.h>
+
+#ifdef JPH_CONVEX_BUILDER_2D_DEBUG
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif
+
+JPH_NAMESPACE_BEGIN
+
+void ConvexHullBuilder2D::Edge::CalculateNormalAndCenter(const Vec3 *inPositions)
+{
+	Vec3 p1 = inPositions[mStartIdx];
+	Vec3 p2 = inPositions[mNextEdge->mStartIdx];
+
+	// Center of edge
+	mCenter = 0.5f * (p1 + p2);
+
+	// Create outward pointing normal.
+	// We have two choices for the normal (which satisfies normal . edge = 0):
+	// normal1 = (-edge.y, edge.x, 0)
+	// normal2 = (edge.y, -edge.x, 0)
+	// We want (normal x edge).z > 0 so that the normal points out of the polygon. Only normal2 satisfies this condition.
+	Vec3 edge = p2 - p1;
+	mNormal = Vec3(edge.GetY(), -edge.GetX(), 0);
+}
+
+ConvexHullBuilder2D::ConvexHullBuilder2D(const Positions &inPositions) :
+	mPositions(inPositions)
+{
+#ifdef JPH_CONVEX_BUILDER_2D_DEBUG
+	// Center the drawing of the first hull around the origin and calculate the delta offset between states
+	mOffset = RVec3::sZero();
+	if (mPositions.empty())
+	{
+		// No hull will be generated
+		mDelta = Vec3::sZero();
+	}
+	else
+	{
+		Vec3 maxv = Vec3::sReplicate(-FLT_MAX), minv = Vec3::sReplicate(FLT_MAX);
+		for (Vec3 v : mPositions)
+		{
+			minv = Vec3::sMin(minv, v);
+			maxv = Vec3::sMax(maxv, v);
+			mOffset -= v;
+		}
+		mOffset /= Real(mPositions.size());
+		mDelta = Vec3((maxv - minv).GetX() + 0.5f, 0, 0);
+		mOffset += mDelta; // Don't start at origin, we're already drawing the final hull there
+	}
+#endif
+}
+
+ConvexHullBuilder2D::~ConvexHullBuilder2D()
+{
+	FreeEdges();
+}
+
+void ConvexHullBuilder2D::FreeEdges()
+{
+	if (mFirstEdge == nullptr)
+		return;
+
+	Edge *edge = mFirstEdge;
+	do
+	{
+		Edge *next = edge->mNextEdge;
+		delete edge;
+		edge = next;
+	} while (edge != mFirstEdge);
+
+	mFirstEdge = nullptr;
+	mNumEdges = 0;
+}
+
+#ifdef JPH_ENABLE_ASSERTS
+
+void ConvexHullBuilder2D::ValidateEdges() const
+{
+	if (mFirstEdge == nullptr)
+	{
+		JPH_ASSERT(mNumEdges == 0);
+		return;
+	}
+
+	int count = 0;
+
+	Edge *edge = mFirstEdge;
+	do
+	{
+		// Validate connectivity
+		JPH_ASSERT(edge->mNextEdge->mPrevEdge == edge);
+		JPH_ASSERT(edge->mPrevEdge->mNextEdge == edge);
+
+		++count;
+		edge = edge->mNextEdge;
+	} while (edge != mFirstEdge);
+
+	// Validate that count matches
+	JPH_ASSERT(count == mNumEdges);
+}
+
+#endif // JPH_ENABLE_ASSERTS
+
+void ConvexHullBuilder2D::AssignPointToEdge(int inPositionIdx, const Array<Edge *> &inEdges) const
+{
+	Vec3 point = mPositions[inPositionIdx];
+
+	Edge *best_edge = nullptr;
+	float best_dist_sq = 0.0f;
+
+	// Test against all edges
+	for (Edge *edge : inEdges)
+	{
+		// Determine distance to edge
+		float dot = edge->mNormal.Dot(point - edge->mCenter);
+		if (dot > 0.0f)
+		{
+			float dist_sq = dot * dot / edge->mNormal.LengthSq();
+			if (dist_sq > best_dist_sq)
+			{
+				best_edge = edge;
+				best_dist_sq = dist_sq;
+			}
+		}
+	}
+
+	// If this point is in front of the edge, add it to the conflict list
+	if (best_edge != nullptr)
+	{
+		if (best_dist_sq > best_edge->mFurthestPointDistanceSq)
+		{
+			// This point is further away than any others, update the distance and add point as last point
+			best_edge->mFurthestPointDistanceSq = best_dist_sq;
+			best_edge->mConflictList.push_back(inPositionIdx);
+		}
+		else
+		{
+			// Not the furthest point, add it as the before last point
+			best_edge->mConflictList.insert(best_edge->mConflictList.begin() + best_edge->mConflictList.size() - 1, inPositionIdx);
+		}
+	}
+}
+
+ConvexHullBuilder2D::EResult ConvexHullBuilder2D::Initialize(int inIdx1, int inIdx2, int inIdx3, int inMaxVertices, float inTolerance, Edges &outEdges)
+{
+	// Clear any leftovers
+	FreeEdges();
+	outEdges.clear();
+
+	// Reset flag
+	EResult result = EResult::Success;
+
+	// Determine a suitable tolerance for detecting that points are colinear
+	// Formula as per: Implementing Quickhull - Dirk Gregorius.
+	Vec3 vmax = Vec3::sZero();
+	for (Vec3 v : mPositions)
+		vmax = Vec3::sMax(vmax, v.Abs());
+	float colinear_tolerance_sq = Square(2.0f * FLT_EPSILON * (vmax.GetX() + vmax.GetY()));
+
+	// Increase desired tolerance if accuracy doesn't allow it
+	float tolerance_sq = max(colinear_tolerance_sq, Square(inTolerance));
+
+	// Start with the initial indices in counter clockwise order
+	float z = (mPositions[inIdx2] - mPositions[inIdx1]).Cross(mPositions[inIdx3] - mPositions[inIdx1]).GetZ();
+	if (z < 0.0f)
+		std::swap(inIdx1, inIdx2);
+
+	// Create and link edges
+	Edge *e1 = new Edge(inIdx1);
+	Edge *e2 = new Edge(inIdx2);
+	Edge *e3 = new Edge(inIdx3);
+	e1->mNextEdge = e2;
+	e1->mPrevEdge = e3;
+	e2->mNextEdge = e3;
+	e2->mPrevEdge = e1;
+	e3->mNextEdge = e1;
+	e3->mPrevEdge = e2;
+	mFirstEdge = e1;
+	mNumEdges = 3;
+
+	// Build the initial conflict lists
+	Array<Edge *> edges { e1, e2, e3 };
+	for (Edge *edge : edges)
+		edge->CalculateNormalAndCenter(mPositions.data());
+	for (int idx = 0; idx < (int)mPositions.size(); ++idx)
+		if (idx != inIdx1 && idx != inIdx2 && idx != inIdx3)
+			AssignPointToEdge(idx, edges);
+
+	JPH_IF_ENABLE_ASSERTS(ValidateEdges();)
+#ifdef JPH_CONVEX_BUILDER_2D_DEBUG
+	DrawState();
+#endif
+
+	// Add the remaining points to the hull
+	for (;;)
+	{
+		// Check if we've reached the max amount of vertices that are allowed
+		if (mNumEdges >= inMaxVertices)
+		{
+			result = EResult::MaxVerticesReached;
+			break;
+		}
+
+		// Find the edge with the furthest point on it
+		Edge *edge_with_furthest_point = nullptr;
+		float furthest_dist_sq = 0.0f;
+		Edge *edge = mFirstEdge;
+		do
+		{
+			if (edge->mFurthestPointDistanceSq > furthest_dist_sq)
+			{
+				furthest_dist_sq = edge->mFurthestPointDistanceSq;
+				edge_with_furthest_point = edge;
+			}
+			edge = edge->mNextEdge;
+		} while (edge != mFirstEdge);
+
+		// If there is none closer than our tolerance value, we're done
+		if (edge_with_furthest_point == nullptr || furthest_dist_sq < tolerance_sq)
+			break;
+
+		// Take the furthest point
+		int furthest_point_idx = edge_with_furthest_point->mConflictList.back();
+		edge_with_furthest_point->mConflictList.pop_back();
+		Vec3 furthest_point = mPositions[furthest_point_idx];
+
+		// Find the horizon of edges that need to be removed
+		Edge *first_edge = edge_with_furthest_point;
+		do
+		{
+			Edge *prev = first_edge->mPrevEdge;
+			if (!prev->IsFacing(furthest_point))
+				break;
+			first_edge = prev;
+		} while (first_edge != edge_with_furthest_point);
+
+		Edge *last_edge = edge_with_furthest_point;
+		do
+		{
+			Edge *next = last_edge->mNextEdge;
+			if (!next->IsFacing(furthest_point))
+				break;
+			last_edge = next;
+		} while (last_edge != edge_with_furthest_point);
+
+		// Create new edges
+		e1 = new Edge(first_edge->mStartIdx);
+		e2 = new Edge(furthest_point_idx);
+		e1->mNextEdge = e2;
+		e1->mPrevEdge = first_edge->mPrevEdge;
+		e2->mPrevEdge = e1;
+		e2->mNextEdge = last_edge->mNextEdge;
+		e1->mPrevEdge->mNextEdge = e1;
+		e2->mNextEdge->mPrevEdge = e2;
+		mFirstEdge = e1; // We could delete mFirstEdge so just update it to the newly created edge
+		mNumEdges += 2;
+
+		// Calculate normals
+		Array<Edge *> new_edges { e1, e2 };
+		for (Edge *new_edge : new_edges)
+			new_edge->CalculateNormalAndCenter(mPositions.data());
+
+		// Delete the old edges
+		for (;;)
+		{
+			Edge *next = first_edge->mNextEdge;
+
+			// Redistribute points in conflict list
+			for (int idx : first_edge->mConflictList)
+				AssignPointToEdge(idx, new_edges);
+
+			// Delete the old edge
+			delete first_edge;
+			--mNumEdges;
+
+			if (first_edge == last_edge)
+				break;
+			first_edge = next;
+		}
+
+		JPH_IF_ENABLE_ASSERTS(ValidateEdges();)
+	#ifdef JPH_CONVEX_BUILDER_2D_DEBUG
+		DrawState();
+	#endif
+	}
+
+	// Convert the edge list to a list of indices
+	outEdges.reserve(mNumEdges);
+	Edge *edge = mFirstEdge;
+	do
+	{
+		outEdges.push_back(edge->mStartIdx);
+		edge = edge->mNextEdge;
+	} while (edge != mFirstEdge);
+
+	return result;
+}
+
+#ifdef JPH_CONVEX_BUILDER_2D_DEBUG
+
+void ConvexHullBuilder2D::DrawState()
+{
+	int color_idx = 0;
+
+	const Edge *edge = mFirstEdge;
+	do
+	{
+		const Edge *next = edge->mNextEdge;
+
+		// Get unique color per edge
+		Color color = Color::sGetDistinctColor(color_idx++);
+
+		// Draw edge and normal
+		DebugRenderer::sInstance->DrawArrow(cDrawScale * (mOffset + mPositions[edge->mStartIdx]), cDrawScale * (mOffset + mPositions[next->mStartIdx]), color, 0.1f);
+		DebugRenderer::sInstance->DrawArrow(cDrawScale * (mOffset + edge->mCenter), cDrawScale * (mOffset + edge->mCenter) + edge->mNormal.NormalizedOr(Vec3::sZero()), Color::sGreen, 0.1f);
+
+		// Draw points that belong to this edge in the same color
+		for (int idx : edge->mConflictList)
+			DebugRenderer::sInstance->DrawMarker(cDrawScale * (mOffset + mPositions[idx]), color, 0.05f);
+
+		edge = next;
+	} while (edge != mFirstEdge);
+
+	mOffset += mDelta;
+}
+
+#endif
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Geometry/ConvexHullBuilder2D.h

@@ -0,0 +1,105 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/NonCopyable.h>
+
+//#define JPH_CONVEX_BUILDER_2D_DEBUG
+
+JPH_NAMESPACE_BEGIN
+
+/// A convex hull builder that tries to create 2D hulls as accurately as possible. Used for offline processing.
+class JPH_EXPORT ConvexHullBuilder2D : public NonCopyable
+{
+public:
+	using Positions = Array<Vec3>;
+	using Edges = Array<int>;
+
+	/// Constructor
+	/// @param inPositions Positions used to make the hull. Uses X and Y component of Vec3 only!
+	explicit			ConvexHullBuilder2D(const Positions &inPositions);
+
+	/// Destructor
+						~ConvexHullBuilder2D();
+
+	/// Result enum that indicates how the hull got created
+	enum class EResult
+	{
+		Success,													///< Hull building finished successfully
+		MaxVerticesReached,											///< Hull building finished successfully, but the desired accuracy was not reached because the max vertices limit was reached
+	};
+
+	/// Takes all positions as provided by the constructor and use them to build a hull
+	/// Any points that are closer to the hull than inTolerance will be discarded
+	/// @param inIdx1 , inIdx2 , inIdx3 The indices to use as initial hull (in any order)
+	/// @param inMaxVertices Max vertices to allow in the hull. Specify INT_MAX if there is no limit.
+	/// @param inTolerance Max distance that a point is allowed to be outside of the hull
+	/// @param outEdges On success this will contain the list of indices that form the hull (counter clockwise)
+	/// @return Status code that reports if the hull was created or not
+	EResult				Initialize(int inIdx1, int inIdx2, int inIdx3, int inMaxVertices, float inTolerance, Edges &outEdges);
+
+private:
+#ifdef JPH_CONVEX_BUILDER_2D_DEBUG
+	/// Factor to scale convex hull when debug drawing the construction process
+	static constexpr Real cDrawScale = 10;
+#endif
+
+	class Edge;
+
+	/// Frees all edges
+	void				FreeEdges();
+
+	/// Assigns a position to one of the supplied edges based on which edge is closest.
+	/// @param inPositionIdx Index of the position to add
+	/// @param inEdges List of edges to consider
+	void				AssignPointToEdge(int inPositionIdx, const Array<Edge *> &inEdges) const;
+
+#ifdef JPH_CONVEX_BUILDER_2D_DEBUG
+	/// Draw state of algorithm
+	void				DrawState();
+#endif
+
+#ifdef JPH_ENABLE_ASSERTS
+	/// Validate that the edge structure is intact
+	void				ValidateEdges() const;
+#endif
+
+	using ConflictList = Array<int>;
+
+	/// Linked list of edges
+	class Edge
+	{
+	public:
+		JPH_OVERRIDE_NEW_DELETE
+
+		/// Constructor
+		explicit		Edge(int inStartIdx)						: mStartIdx(inStartIdx) { }
+
+		/// Calculate the center of the edge and the edge normal
+		void			CalculateNormalAndCenter(const Vec3 *inPositions);
+
+		/// Check if this edge is facing inPosition
+		inline bool		IsFacing(Vec3Arg inPosition) const			{ return mNormal.Dot(inPosition - mCenter) > 0.0f; }
+
+		Vec3			mNormal;									///< Normal of the edge (not normalized)
+		Vec3			mCenter;									///< Center of the edge
+		ConflictList	mConflictList;								///< Positions associated with this edge (that are closest to this edge). Last entry is the one furthest away from the edge, remainder is unsorted.
+		Edge *			mPrevEdge = nullptr;						///< Previous edge in circular list
+		Edge *			mNextEdge = nullptr;						///< Next edge in circular list
+		int				mStartIdx;									///< Position index of start of this edge
+		float			mFurthestPointDistanceSq = 0.0f;			///< Squared distance of furthest point from the conflict list to the edge
+	};
+
+	const Positions &	mPositions;									///< List of positions (some of them are part of the hull)
+	Edge *				mFirstEdge = nullptr;						///< First edge of the hull
+	int					mNumEdges = 0;								///< Number of edges in hull
+
+#ifdef JPH_CONVEX_BUILDER_2D_DEBUG
+	RVec3				mOffset;									///< Offset to use for state drawing
+	Vec3				mDelta;										///< Delta offset between next states
+#endif
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Geometry/ConvexSupport.h

@@ -0,0 +1,188 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Math/Mat44.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Helper functions to get the support point for a convex object
+/// Structure that transforms a convex object (supports only uniform scaling)
+template <typename ConvexObject>
+struct TransformedConvexObject
+{
+	/// Create transformed convex object.
+	TransformedConvexObject(Mat44Arg inTransform, const ConvexObject &inObject) :
+		mTransform(inTransform),
+		mObject(inObject)
+	{
+	}
+
+	/// Calculate the support vector for this convex shape.
+	Vec3					GetSupport(Vec3Arg inDirection) const
+	{
+		return mTransform * mObject.GetSupport(mTransform.Multiply3x3Transposed(inDirection));
+	}
+
+	/// Get the vertices of the face that faces inDirection the most
+	template <class VERTEX_ARRAY>
+	void					GetSupportingFace(Vec3Arg inDirection, VERTEX_ARRAY &outVertices) const
+	{
+		mObject.GetSupportingFace(mTransform.Multiply3x3Transposed(inDirection), outVertices);
+
+		for (Vec3 &v : outVertices)
+			v = mTransform * v;
+	}
+
+	Mat44					mTransform;
+	const ConvexObject &	mObject;
+};
+
+/// Structure that adds a convex radius
+template <typename ConvexObject>
+struct AddConvexRadius
+{
+	AddConvexRadius(const ConvexObject &inObject, float inRadius) :
+		mObject(inObject),
+		mRadius(inRadius)
+	{
+	}
+
+	/// Calculate the support vector for this convex shape.
+	Vec3					GetSupport(Vec3Arg inDirection) const
+	{
+		float length = inDirection.Length();
+		return length > 0.0f ? mObject.GetSupport(inDirection) + (mRadius / length) * inDirection : mObject.GetSupport(inDirection);
+	}
+
+	const ConvexObject &	mObject;
+	float					mRadius;
+};
+
+/// Structure that performs a Minkowski difference A - B
+template <typename ConvexObjectA, typename ConvexObjectB>
+struct MinkowskiDifference
+{
+	MinkowskiDifference(const ConvexObjectA &inObjectA, const ConvexObjectB &inObjectB) :
+		mObjectA(inObjectA),
+		mObjectB(inObjectB)
+	{
+	}
+
+	/// Calculate the support vector for this convex shape.
+	Vec3					GetSupport(Vec3Arg inDirection) const
+	{
+		return mObjectA.GetSupport(inDirection) - mObjectB.GetSupport(-inDirection);
+	}
+
+	const ConvexObjectA &	mObjectA;
+	const ConvexObjectB &	mObjectB;
+};
+
+/// Class that wraps a point so that it can be used with convex collision detection
+struct PointConvexSupport
+{
+	/// Calculate the support vector for this convex shape.
+	Vec3					GetSupport([[maybe_unused]] Vec3Arg inDirection) const
+	{
+		return mPoint;
+	}
+
+	Vec3					mPoint;
+};
+
+/// Class that wraps a triangle so that it can used with convex collision detection
+struct TriangleConvexSupport
+{
+	/// Constructor
+							TriangleConvexSupport(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3) :
+		mV1(inV1),
+		mV2(inV2),
+		mV3(inV3)
+	{
+	}
+
+	/// Calculate the support vector for this convex shape.
+	Vec3					GetSupport(Vec3Arg inDirection) const
+	{
+		// Project vertices on inDirection
+		float d1 = mV1.Dot(inDirection);
+		float d2 = mV2.Dot(inDirection);
+		float d3 = mV3.Dot(inDirection);
+
+		// Return vertex with biggest projection
+		if (d1 > d2)
+		{
+			if (d1 > d3)
+				return mV1;
+			else
+				return mV3;
+		}
+		else
+		{
+			if (d2 > d3)
+				return mV2;
+			else
+				return mV3;
+		}
+	}
+
+	/// Get the vertices of the face that faces inDirection the most
+	template <class VERTEX_ARRAY>
+	void					GetSupportingFace([[maybe_unused]] Vec3Arg inDirection, VERTEX_ARRAY &outVertices) const
+	{
+		outVertices.push_back(mV1);
+		outVertices.push_back(mV2);
+		outVertices.push_back(mV3);
+	}
+
+	/// The three vertices of the triangle
+	Vec3					mV1;
+	Vec3					mV2;
+	Vec3					mV3;
+};
+
+/// Class that wraps a polygon so that it can used with convex collision detection
+template <class VERTEX_ARRAY>
+struct PolygonConvexSupport
+{
+	/// Constructor
+	explicit				PolygonConvexSupport(const VERTEX_ARRAY &inVertices) :
+		mVertices(inVertices)
+	{
+	}
+
+	/// Calculate the support vector for this convex shape.
+	Vec3					GetSupport(Vec3Arg inDirection) const
+	{
+		Vec3 support_point = mVertices[0];
+		float best_dot = mVertices[0].Dot(inDirection);
+
+		for (typename VERTEX_ARRAY::const_iterator v = mVertices.begin() + 1; v < mVertices.end(); ++v)
+		{
+			float dot = v->Dot(inDirection);
+			if (dot > best_dot)
+			{
+				best_dot = dot;
+				support_point = *v;
+			}
+		}
+
+		return support_point;
+	}
+
+	/// Get the vertices of the face that faces inDirection the most
+	template <class VERTEX_ARRAY_ARG>
+	void					GetSupportingFace([[maybe_unused]] Vec3Arg inDirection, VERTEX_ARRAY_ARG &outVertices) const
+	{
+		for (Vec3 v : mVertices)
+			outVertices.push_back(v);
+	}
+
+	/// The vertices of the polygon
+	const VERTEX_ARRAY &	mVertices;
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Geometry/EPAConvexHullBuilder.h

@@ -0,0 +1,845 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+// Define to validate the integrity of the hull structure
+//#define JPH_EPA_CONVEX_BUILDER_VALIDATE
+
+// Define to draw the building of the hull for debugging purposes
+//#define JPH_EPA_CONVEX_BUILDER_DRAW
+
+#include <Jolt/Core/NonCopyable.h>
+#include <Jolt/Core/BinaryHeap.h>
+
+#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
+	#include <Jolt/Renderer/DebugRenderer.h>
+	#include <Jolt/Core/StringTools.h>
+#endif
+
+JPH_NAMESPACE_BEGIN
+
+/// A convex hull builder specifically made for the EPA penetration depth calculation. It trades accuracy for speed and will simply abort of the hull forms defects due to numerical precision problems.
+class EPAConvexHullBuilder : public NonCopyable
+{
+private:
+#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
+	/// Factor to scale convex hull when debug drawing the construction process
+	static constexpr Real cDrawScale = 10;
+#endif
+
+public:
+	// Due to the Euler characteristic (https://en.wikipedia.org/wiki/Euler_characteristic) we know that Vertices - Edges + Faces = 2
+	// In our case we only have triangles and they are always fully connected, so each edge is shared exactly between 2 faces: Edges = Faces * 3 / 2
+	// Substituting: Vertices = Faces / 2 + 2 which is approximately Faces / 2.
+	static constexpr int cMaxTriangles = 256;				///< Max triangles in hull
+	static constexpr int cMaxPoints = cMaxTriangles / 2;	///< Max number of points in hull
+
+	// Constants
+	static constexpr int cMaxEdgeLength = 128;				///< Max number of edges in FindEdge
+	static constexpr float cMinTriangleArea = 1.0e-10f;		///< Minimum area of a triangle before, if smaller than this it will not be added to the priority queue
+	static constexpr float cBarycentricEpsilon = 1.0e-3f;	///< Epsilon value used to determine if a point is in the interior of a triangle
+
+	// Forward declare
+	class Triangle;
+
+	/// Class that holds the information of an edge
+	class Edge
+	{
+	public:
+		/// Information about neighbouring triangle
+		Triangle *		mNeighbourTriangle;					///< Triangle that neighbours this triangle
+		int				mNeighbourEdge;						///< Index in mEdge that specifies edge that this Edge is connected to
+
+		int				mStartIdx;							///< Vertex index in mPositions that indicates the start vertex of this edge
+	};
+
+	using Edges = StaticArray<Edge, cMaxEdgeLength>;
+	using NewTriangles = StaticArray<Triangle *, cMaxEdgeLength>;
+
+	/// Class that holds the information of one triangle
+	class Triangle : public NonCopyable
+	{
+	public:
+		/// Constructor
+		inline			Triangle(int inIdx0, int inIdx1, int inIdx2, const Vec3 *inPositions);
+
+		/// Check if triangle is facing inPosition
+		inline bool		IsFacing(Vec3Arg inPosition) const
+		{
+			JPH_ASSERT(!mRemoved);
+			return mNormal.Dot(inPosition - mCentroid) > 0.0f;
+		}
+
+		/// Check if triangle is facing the origin
+		inline bool		IsFacingOrigin() const
+		{
+			JPH_ASSERT(!mRemoved);
+			return mNormal.Dot(mCentroid) < 0.0f;
+		}
+
+		/// Get the next edge of edge inIndex
+		inline const Edge & GetNextEdge(int inIndex) const
+		{
+			return mEdge[(inIndex + 1) % 3];
+		}
+
+		Edge			mEdge[3];							///< 3 edges of this triangle
+		Vec3			mNormal;							///< Normal of this triangle, length is 2 times area of triangle
+		Vec3			mCentroid;							///< Center of the triangle
+		float			mClosestLenSq = FLT_MAX;			///< Closest distance^2 from origin to triangle
+		float			mLambda[2];							///< Barycentric coordinates of closest point to origin on triangle
+		bool			mLambdaRelativeTo0;					///< How to calculate the closest point, true: y0 + l0 * (y1 - y0) + l1 * (y2 - y0), false: y1 + l0 * (y0 - y1) + l1 * (y2 - y1)
+		bool			mClosestPointInterior = false;		///< Flag that indicates that the closest point from this triangle to the origin is an interior point
+		bool			mRemoved = false;					///< Flag that indicates that triangle has been removed
+		bool			mInQueue = false;					///< Flag that indicates that this triangle was placed in the sorted heap (stays true after it is popped because the triangle is freed by the main EPA algorithm loop)
+#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
+		int				mIteration;							///< Iteration that this triangle was created
+#endif
+	};
+
+	/// Factory that creates triangles in a fixed size buffer
+	class TriangleFactory : public NonCopyable
+	{
+	private:
+		/// Struct that stores both a triangle or a next pointer in case the triangle is unused
+		union alignas(Triangle) Block
+		{
+			uint8		mTriangle[sizeof(Triangle)];
+			Block *		mNextFree;
+		};
+
+		/// Storage for triangle data
+		Block			mTriangles[cMaxTriangles];			///< Storage for triangles
+		Block *			mNextFree = nullptr;				///< List of free triangles
+		int				mHighWatermark = 0;					///< High water mark for used triangles (if mNextFree == nullptr we can take one from here)
+
+	public:
+		/// Return all triangles to the free pool
+		void			Clear()
+		{
+			mNextFree = nullptr;
+			mHighWatermark = 0;
+		}
+
+		/// Allocate a new triangle with 3 indexes
+		Triangle *		CreateTriangle(int inIdx0, int inIdx1, int inIdx2, const Vec3 *inPositions)
+		{
+			Triangle *t;
+			if (mNextFree != nullptr)
+			{
+				// Entry available from the free list
+				t = reinterpret_cast<Triangle *>(&mNextFree->mTriangle);
+				mNextFree = mNextFree->mNextFree;
+			}
+			else
+			{
+				// Allocate from never used before triangle store
+				if (mHighWatermark >= cMaxTriangles)
+					return nullptr; // Buffer full
+				t = reinterpret_cast<Triangle *>(&mTriangles[mHighWatermark].mTriangle);
+				++mHighWatermark;
+			}
+
+			// Call constructor
+			new (t) Triangle(inIdx0, inIdx1, inIdx2, inPositions);
+
+			return t;
+		}
+
+		/// Free a triangle
+		void			FreeTriangle(Triangle *inT)
+		{
+			// Destruct triangle
+			inT->~Triangle();
+#ifdef JPH_DEBUG
+			memset(inT, 0xcd, sizeof(Triangle));
+#endif
+
+			// Add triangle to the free list
+			Block *tu = reinterpret_cast<Block *>(inT);
+			tu->mNextFree = mNextFree;
+			mNextFree = tu;
+		}
+	};
+
+	// Typedefs
+	using PointsBase = StaticArray<Vec3, cMaxPoints>;
+	using Triangles = StaticArray<Triangle *, cMaxTriangles>;
+
+	/// Specialized points list that allows direct access to the size
+	class Points : public PointsBase
+	{
+	public:
+		size_type &		GetSizeRef()
+		{
+			return mSize;
+		}
+	};
+
+	/// Specialized triangles list that keeps them sorted on closest distance to origin
+	class TriangleQueue : public Triangles
+	{
+	public:
+		/// Function to sort triangles on closest distance to origin
+		static bool		sTriangleSorter(const Triangle *inT1, const Triangle *inT2)
+		{
+			return inT1->mClosestLenSq > inT2->mClosestLenSq;
+		}
+
+		/// Add triangle to the list
+		void			push_back(Triangle *inT)
+		{
+			// Add to base
+			Triangles::push_back(inT);
+
+			// Mark in queue
+			inT->mInQueue = true;
+
+			// Resort heap
+			BinaryHeapPush(begin(), end(), sTriangleSorter);
+		}
+
+		/// Peek the next closest triangle without removing it
+		Triangle *		PeekClosest()
+		{
+			return front();
+		}
+
+		/// Get next closest triangle
+		Triangle *		PopClosest()
+		{
+			// Move closest to end
+			BinaryHeapPop(begin(), end(), sTriangleSorter);
+
+			// Remove last triangle
+			Triangle *t = back();
+			pop_back();
+			return t;
+		}
+	};
+
+	/// Constructor
+	explicit			EPAConvexHullBuilder(const Points &inPositions) :
+		mPositions(inPositions)
+	{
+#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
+		mIteration = 0;
+		mOffset = RVec3::sZero();
+#endif
+	}
+
+	/// Initialize the hull with 3 points
+	void				Initialize(int inIdx1, int inIdx2, int inIdx3)
+	{
+		// Release triangles
+		mFactory.Clear();
+
+		// Create triangles (back to back)
+		Triangle *t1 = CreateTriangle(inIdx1, inIdx2, inIdx3);
+		Triangle *t2 = CreateTriangle(inIdx1, inIdx3, inIdx2);
+
+		// Link triangles edges
+		sLinkTriangle(t1, 0, t2, 2);
+		sLinkTriangle(t1, 1, t2, 1);
+		sLinkTriangle(t1, 2, t2, 0);
+
+		// Always add both triangles to the priority queue
+		mTriangleQueue.push_back(t1);
+		mTriangleQueue.push_back(t2);
+
+#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
+		// Draw current state
+		DrawState();
+
+		// Increment iteration counter
+		++mIteration;
+#endif
+	}
+
+	/// Check if there's another triangle to process from the queue
+	bool				HasNextTriangle() const
+	{
+		return !mTriangleQueue.empty();
+	}
+
+	/// Access to the next closest triangle to the origin (won't remove it from the queue).
+	Triangle *			PeekClosestTriangleInQueue()
+	{
+		return mTriangleQueue.PeekClosest();
+	}
+
+	/// Access to the next closest triangle to the origin and remove it from the queue.
+	Triangle *			PopClosestTriangleFromQueue()
+	{
+		return mTriangleQueue.PopClosest();
+	}
+
+	/// Find the triangle on which inPosition is the furthest to the front
+	/// Note this function works as long as all points added have been added with AddPoint(..., FLT_MAX).
+	Triangle *			FindFacingTriangle(Vec3Arg inPosition, float &outBestDistSq)
+	{
+		Triangle *best = nullptr;
+		float best_dist_sq = 0.0f;
+
+		for (Triangle *t : mTriangleQueue)
+			if (!t->mRemoved)
+			{
+				float dot = t->mNormal.Dot(inPosition - t->mCentroid);
+				if (dot > 0.0f)
+				{
+					float dist_sq = dot * dot / t->mNormal.LengthSq();
+					if (dist_sq > best_dist_sq)
+					{
+						best = t;
+						best_dist_sq = dist_sq;
+					}
+				}
+			}
+
+		outBestDistSq = best_dist_sq;
+		return best;
+	}
+
+	/// Add a new point to the convex hull
+	bool				AddPoint(Triangle *inFacingTriangle, int inIdx, float inClosestDistSq, NewTriangles &outTriangles)
+	{
+		// Get position
+		Vec3 pos = mPositions[inIdx];
+
+#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
+		// Draw new support point
+		DrawMarker(pos, Color::sYellow, 1.0f);
+#endif
+
+#ifdef JPH_EPA_CONVEX_BUILDER_VALIDATE
+		// Check if structure is intact
+		ValidateTriangles();
+#endif
+
+		// Find edge of convex hull of triangles that are not facing the new vertex w
+		Edges edges;
+		if (!FindEdge(inFacingTriangle, pos, edges))
+			return false;
+
+		// Create new triangles
+		int num_edges = edges.size();
+		for (int i = 0; i < num_edges; ++i)
+		{
+			// Create new triangle
+			Triangle *nt = CreateTriangle(edges[i].mStartIdx, edges[(i + 1) % num_edges].mStartIdx, inIdx);
+			if (nt == nullptr)
+				return false;
+			outTriangles.push_back(nt);
+
+			// Check if we need to put this triangle in the priority queue
+			if ((nt->mClosestPointInterior && nt->mClosestLenSq < inClosestDistSq)	// For the main algorithm
+				|| nt->mClosestLenSq < 0.0f)										// For when the origin is not inside the hull yet
+				mTriangleQueue.push_back(nt);
+		}
+
+		// Link edges
+		for (int i = 0; i < num_edges; ++i)
+		{
+			sLinkTriangle(outTriangles[i], 0, edges[i].mNeighbourTriangle, edges[i].mNeighbourEdge);
+			sLinkTriangle(outTriangles[i], 1, outTriangles[(i + 1) % num_edges], 2);
+		}
+
+#ifdef JPH_EPA_CONVEX_BUILDER_VALIDATE
+		// Check if structure is intact
+		ValidateTriangles();
+#endif
+
+#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
+		// Draw state of the hull
+		DrawState();
+
+		// Increment iteration counter
+		++mIteration;
+#endif
+
+		return true;
+	}
+
+	/// Free a triangle
+	void				FreeTriangle(Triangle *inT)
+	{
+#ifdef JPH_ENABLE_ASSERTS
+		// Make sure that this triangle is not connected
+		JPH_ASSERT(inT->mRemoved);
+		for (const Edge &e : inT->mEdge)
+			JPH_ASSERT(e.mNeighbourTriangle == nullptr);
+#endif
+
+#if defined(JPH_EPA_CONVEX_BUILDER_VALIDATE) || defined(JPH_EPA_CONVEX_BUILDER_DRAW)
+		// Remove from list of all triangles
+		Triangles::iterator i = std::find(mTriangles.begin(), mTriangles.end(), inT);
+		JPH_ASSERT(i != mTriangles.end());
+		mTriangles.erase(i);
+#endif
+
+		mFactory.FreeTriangle(inT);
+	}
+
+private:
+	/// Create a new triangle
+	Triangle *			CreateTriangle(int inIdx1, int inIdx2, int inIdx3)
+	{
+		// Call provider to create triangle
+		Triangle *t = mFactory.CreateTriangle(inIdx1, inIdx2, inIdx3, mPositions.data());
+		if (t == nullptr)
+			return nullptr;
+
+#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
+		// Remember iteration counter
+		t->mIteration = mIteration;
+#endif
+
+#if defined(JPH_EPA_CONVEX_BUILDER_VALIDATE) || defined(JPH_EPA_CONVEX_BUILDER_DRAW)
+		// Add to list of triangles for debugging purposes
+		mTriangles.push_back(t);
+#endif
+
+		return t;
+	}
+
+	/// Link triangle edge to other triangle edge
+	static void			sLinkTriangle(Triangle *inT1, int inEdge1, Triangle *inT2, int inEdge2)
+	{
+		JPH_ASSERT(inEdge1 >= 0 && inEdge1 < 3);
+		JPH_ASSERT(inEdge2 >= 0 && inEdge2 < 3);
+		Edge &e1 = inT1->mEdge[inEdge1];
+		Edge &e2 = inT2->mEdge[inEdge2];
+
+		// Check not connected yet
+		JPH_ASSERT(e1.mNeighbourTriangle == nullptr);
+		JPH_ASSERT(e2.mNeighbourTriangle == nullptr);
+
+		// Check vertices match
+		JPH_ASSERT(e1.mStartIdx == inT2->GetNextEdge(inEdge2).mStartIdx);
+		JPH_ASSERT(e2.mStartIdx == inT1->GetNextEdge(inEdge1).mStartIdx);
+
+		// Link up
+		e1.mNeighbourTriangle = inT2;
+		e1.mNeighbourEdge = inEdge2;
+		e2.mNeighbourTriangle = inT1;
+		e2.mNeighbourEdge = inEdge1;
+	}
+
+	/// Unlink this triangle
+	void				UnlinkTriangle(Triangle *inT)
+	{
+		// Unlink from neighbours
+		for (int i = 0; i < 3; ++i)
+		{
+			Edge &edge = inT->mEdge[i];
+			if (edge.mNeighbourTriangle != nullptr)
+			{
+				Edge &neighbour_edge = edge.mNeighbourTriangle->mEdge[edge.mNeighbourEdge];
+
+				// Validate that neighbour points to us
+				JPH_ASSERT(neighbour_edge.mNeighbourTriangle == inT);
+				JPH_ASSERT(neighbour_edge.mNeighbourEdge == i);
+
+				// Unlink
+				neighbour_edge.mNeighbourTriangle = nullptr;
+				edge.mNeighbourTriangle = nullptr;
+			}
+		}
+
+		// If this triangle is not in the priority queue, we can delete it now
+		if (!inT->mInQueue)
+			FreeTriangle(inT);
+	}
+
+	/// Given one triangle that faces inVertex, find the edges of the triangles that are not facing inVertex.
+	/// Will flag all those triangles for removal.
+	bool				FindEdge(Triangle *inFacingTriangle, Vec3Arg inVertex, Edges &outEdges)
+	{
+		// Assert that we were given an empty array
+		JPH_ASSERT(outEdges.empty());
+
+		// Should start with a facing triangle
+		JPH_ASSERT(inFacingTriangle->IsFacing(inVertex));
+
+		// Flag as removed
+		inFacingTriangle->mRemoved = true;
+
+		// Instead of recursing, we build our own stack with the information we need
+		struct StackEntry
+		{
+			Triangle *	mTriangle;
+			int			mEdge;
+			int			mIter;
+		};
+		StackEntry stack[cMaxEdgeLength];
+		int cur_stack_pos = 0;
+
+		// Start with the triangle / edge provided
+		stack[0].mTriangle = inFacingTriangle;
+		stack[0].mEdge = 0;
+		stack[0].mIter = -1; // Start with edge 0 (is incremented below before use)
+
+		// Next index that we expect to find, if we don't then there are 'islands'
+		int next_expected_start_idx = -1;
+
+		for (;;)
+		{
+			StackEntry &cur_entry = stack[cur_stack_pos];
+
+			// Next iteration
+			if (++cur_entry.mIter >= 3)
+			{
+				// This triangle needs to be removed, unlink it now
+				UnlinkTriangle(cur_entry.mTriangle);
+
+				// Pop from stack
+				if (--cur_stack_pos < 0)
+					break;
+			}
+			else
+			{
+				// Visit neighbour
+				Edge &e = cur_entry.mTriangle->mEdge[(cur_entry.mEdge + cur_entry.mIter) % 3];
+				Triangle *n = e.mNeighbourTriangle;
+				if (n != nullptr && !n->mRemoved)
+				{
+					// Check if vertex is on the front side of this triangle
+					if (n->IsFacing(inVertex))
+					{
+						// Vertex on front, this triangle needs to be removed
+						n->mRemoved = true;
+
+						// Add element to the stack of elements to visit
+						cur_stack_pos++;
+						JPH_ASSERT(cur_stack_pos < cMaxEdgeLength);
+						StackEntry &new_entry = stack[cur_stack_pos];
+						new_entry.mTriangle = n;
+						new_entry.mEdge = e.mNeighbourEdge;
+						new_entry.mIter = 0; // Is incremented before use, we don't need to test this edge again since we came from it
+					}
+					else
+					{
+						// Detect if edge doesn't connect to previous edge, if this happens we have found and 'island' which means
+						// the newly added point is so close to the triangles of the hull that we classified some (nearly) coplanar
+						// triangles as before and some behind the point. At this point we just abort adding the point because
+						// we've reached numerical precision.
+						// Note that we do not need to test if the first and last edge connect, since when there are islands
+						// there should be at least 2 disconnects.
+						if (e.mStartIdx != next_expected_start_idx && next_expected_start_idx != -1)
+							return false;
+
+						// Next expected index is the start index of our neighbour's edge
+						next_expected_start_idx = n->mEdge[e.mNeighbourEdge].mStartIdx;
+
+						// Vertex behind, keep edge
+						outEdges.push_back(e);
+					}
+				}
+			}
+		}
+
+		// Assert that we have a fully connected loop
+		JPH_ASSERT(outEdges.empty() || outEdges[0].mStartIdx == next_expected_start_idx);
+
+#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
+		// Draw edge of facing triangles
+		for (int i = 0; i < (int)outEdges.size(); ++i)
+		{
+			RVec3 edge_start = cDrawScale * (mOffset + mPositions[outEdges[i].mStartIdx]);
+			DebugRenderer::sInstance->DrawArrow(edge_start, cDrawScale * (mOffset + mPositions[outEdges[(i + 1) % outEdges.size()].mStartIdx]), Color::sYellow, 0.01f);
+			DebugRenderer::sInstance->DrawText3D(edge_start, ConvertToString(outEdges[i].mStartIdx), Color::sWhite);
+		}
+
+		// Draw the state with the facing triangles removed
+		DrawState();
+#endif
+
+		// When we start with two triangles facing away from each other and adding a point that is on the plane,
+		// sometimes we consider the point in front of both causing both triangles to be removed resulting in an empty edge list.
+		// In this case we fail to add the point which will result in no collision reported (the shapes are contacting in 1 point so there's 0 penetration)
+		return outEdges.size() >= 3;
+	}
+
+#ifdef JPH_EPA_CONVEX_BUILDER_VALIDATE
+	/// Check consistency of 1 triangle
+	void				ValidateTriangle(const Triangle *inT) const
+	{
+		if (inT->mRemoved)
+		{
+			// Validate that removed triangles are not connected to anything
+			for (const Edge &my_edge : inT->mEdge)
+				JPH_ASSERT(my_edge.mNeighbourTriangle == nullptr);
+		}
+		else
+		{
+			for (int i = 0; i < 3; ++i)
+			{
+				const Edge &my_edge = inT->mEdge[i];
+
+				// Assert that we have a neighbour
+				const Triangle *nb = my_edge.mNeighbourTriangle;
+				JPH_ASSERT(nb != nullptr);
+
+				if (nb != nullptr)
+				{
+					// Assert that our neighbours edge points to us
+					const Edge &nb_edge = nb->mEdge[my_edge.mNeighbourEdge];
+					JPH_ASSERT(nb_edge.mNeighbourTriangle == inT);
+					JPH_ASSERT(nb_edge.mNeighbourEdge == i);
+
+					// Assert that the next edge of the neighbour points to the same vertex as this edge's vertex
+					const Edge &nb_next_edge = nb->GetNextEdge(my_edge.mNeighbourEdge);
+					JPH_ASSERT(nb_next_edge.mStartIdx == my_edge.mStartIdx);
+
+					// Assert that my next edge points to the same vertex as my neighbours vertex
+					const Edge &my_next_edge = inT->GetNextEdge(i);
+					JPH_ASSERT(my_next_edge.mStartIdx == nb_edge.mStartIdx);
+				}
+			}
+		}
+	}
+
+	/// Check consistency of all triangles
+	void				ValidateTriangles() const
+	{
+		for (const Triangle *t : mTriangles)
+			ValidateTriangle(t);
+	}
+#endif
+
+#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
+public:
+	/// Draw state of algorithm
+	void				DrawState()
+	{
+		// Draw origin
+		DebugRenderer::sInstance->DrawCoordinateSystem(RMat44::sTranslation(cDrawScale * mOffset), 1.0f);
+
+		// Draw triangles
+		for (const Triangle *t : mTriangles)
+			if (!t->mRemoved)
+			{
+				// Calculate the triangle vertices
+				RVec3 p1 = cDrawScale * (mOffset + mPositions[t->mEdge[0].mStartIdx]);
+				RVec3 p2 = cDrawScale * (mOffset + mPositions[t->mEdge[1].mStartIdx]);
+				RVec3 p3 = cDrawScale * (mOffset + mPositions[t->mEdge[2].mStartIdx]);
+
+				// Draw triangle
+				DebugRenderer::sInstance->DrawTriangle(p1, p2, p3, Color::sGetDistinctColor(t->mIteration));
+				DebugRenderer::sInstance->DrawWireTriangle(p1, p2, p3, Color::sGrey);
+
+				// Draw normal
+				RVec3 centroid = cDrawScale * (mOffset + t->mCentroid);
+				float len = t->mNormal.Length();
+				if (len > 0.0f)
+					DebugRenderer::sInstance->DrawArrow(centroid, centroid + t->mNormal / len, Color::sDarkGreen, 0.01f);
+			}
+
+		// Determine max position
+		float min_x = FLT_MAX;
+		float max_x = -FLT_MAX;
+		for (Vec3 p : mPositions)
+		{
+			min_x = min(min_x, p.GetX());
+			max_x = max(max_x, p.GetX());
+		}
+
+		// Offset to the right
+		mOffset += Vec3(max_x - min_x + 0.5f, 0.0f, 0.0f);
+	}
+
+	/// Draw a label to indicate the next stage in the algorithm
+	void				DrawLabel(const string_view &inText)
+	{
+		DebugRenderer::sInstance->DrawText3D(cDrawScale * mOffset, inText, Color::sWhite, 0.1f * cDrawScale);
+
+		mOffset += Vec3(5.0f, 0.0f, 0.0f);
+	}
+
+	/// Draw geometry for debugging purposes
+	void				DrawGeometry(const DebugRenderer::GeometryRef &inGeometry, ColorArg inColor)
+	{
+		RMat44 origin = RMat44::sScale(Vec3::sReplicate(cDrawScale)) * RMat44::sTranslation(mOffset);
+		DebugRenderer::sInstance->DrawGeometry(origin, inGeometry->mBounds.Transformed(origin), inGeometry->mBounds.GetExtent().LengthSq(), inColor, inGeometry);
+
+		mOffset += Vec3(inGeometry->mBounds.GetSize().GetX(), 0, 0);
+	}
+
+	/// Draw a triangle for debugging purposes
+	void				DrawWireTriangle(const Triangle &inTriangle, ColorArg inColor)
+	{
+		RVec3 prev = cDrawScale * (mOffset + mPositions[inTriangle.mEdge[2].mStartIdx]);
+		for (const Edge &edge : inTriangle.mEdge)
+		{
+			RVec3 cur = cDrawScale * (mOffset + mPositions[edge.mStartIdx]);
+			DebugRenderer::sInstance->DrawArrow(prev, cur, inColor, 0.01f);
+			prev = cur;
+		}
+	}
+
+	/// Draw a marker for debugging purposes
+	void				DrawMarker(Vec3Arg inPosition, ColorArg inColor, float inSize)
+	{
+		DebugRenderer::sInstance->DrawMarker(cDrawScale * (mOffset + inPosition), inColor, inSize);
+	}
+
+	/// Draw an arrow for debugging purposes
+	void				DrawArrow(Vec3Arg inFrom, Vec3Arg inTo, ColorArg inColor, float inArrowSize)
+	{
+		DebugRenderer::sInstance->DrawArrow(cDrawScale * (mOffset + inFrom), cDrawScale * (mOffset + inTo), inColor, inArrowSize);
+	}
+#endif
+
+private:
+	TriangleFactory		mFactory;							///< Factory to create new triangles and remove old ones
+	const Points &		mPositions;							///< List of positions (some of them are part of the hull)
+	TriangleQueue		mTriangleQueue;						///< List of triangles that are part of the hull that still need to be checked (if !mRemoved)
+
+#if defined(JPH_EPA_CONVEX_BUILDER_VALIDATE) || defined(JPH_EPA_CONVEX_BUILDER_DRAW)
+	Triangles			mTriangles;							///< The list of all triangles in this hull (for debug purposes)
+#endif
+
+#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
+	int					mIteration;							///< Number of iterations we've had so far (for debug purposes)
+	RVec3				mOffset;							///< Offset to use for state drawing
+#endif
+};
+
+// The determinant that is calculated in the Triangle constructor is really sensitive
+// to numerical round off, disable the fmadd instructions to maintain precision.
+JPH_PRECISE_MATH_ON
+
+EPAConvexHullBuilder::Triangle::Triangle(int inIdx0, int inIdx1, int inIdx2, const Vec3 *inPositions)
+{
+	// Fill in indexes
+	JPH_ASSERT(inIdx0 != inIdx1 && inIdx0 != inIdx2 && inIdx1 != inIdx2);
+	mEdge[0].mStartIdx = inIdx0;
+	mEdge[1].mStartIdx = inIdx1;
+	mEdge[2].mStartIdx = inIdx2;
+
+	// Clear links
+	mEdge[0].mNeighbourTriangle = nullptr;
+	mEdge[1].mNeighbourTriangle = nullptr;
+	mEdge[2].mNeighbourTriangle = nullptr;
+
+	// Get vertex positions
+	Vec3 y0 = inPositions[inIdx0];
+	Vec3 y1 = inPositions[inIdx1];
+	Vec3 y2 = inPositions[inIdx2];
+
+	// Calculate centroid
+	mCentroid = (y0 + y1 + y2) / 3.0f;
+
+	// Calculate edges
+	Vec3 y10 = y1 - y0;
+	Vec3 y20 = y2 - y0;
+	Vec3 y21 = y2 - y1;
+
+	// The most accurate normal is calculated by using the two shortest edges
+	// See: https://box2d.org/posts/2014/01/troublesome-triangle/
+	// The difference in normals is most pronounced when one edge is much smaller than the others (in which case the other 2 must have roughly the same length).
+	// Therefore we can suffice by just picking the shortest from 2 edges and use that with the 3rd edge to calculate the normal.
+	// We first check which of the edges is shorter.
+	float y20_dot_y20 = y20.Dot(y20);
+	float y21_dot_y21 = y21.Dot(y21);
+	if (y20_dot_y20 < y21_dot_y21)
+	{
+		// We select the edges y10 and y20
+		mNormal = y10.Cross(y20);
+
+		// Check if triangle is degenerate
+		float normal_len_sq = mNormal.LengthSq();
+		if (normal_len_sq > cMinTriangleArea)
+		{
+			// Determine distance between triangle and origin: distance = (centroid - origin) . normal / |normal|
+			// Note that this way of calculating the closest point is much more accurate than first calculating barycentric coordinates and then calculating the closest
+			// point based on those coordinates. Note that we preserve the sign of the distance to check on which side the origin is.
+			float c_dot_n = mCentroid.Dot(mNormal);
+			mClosestLenSq = abs(c_dot_n) * c_dot_n / normal_len_sq;
+
+			// Calculate closest point to origin using barycentric coordinates:
+			//
+			// v = y0 + l0 * (y1 - y0) + l1 * (y2 - y0)
+			// v . (y1 - y0) = 0
+			// v . (y2 - y0) = 0
+			//
+			// Written in matrix form:
+			//
+			// | y10.y10  y20.y10 | | l0 | = | -y0.y10 |
+			// | y10.y20  y20.y20 | | l1 |   | -y0.y20 |
+			//
+			// (y10 = y1 - y0 etc.)
+			//
+			// Cramers rule to invert matrix:
+			float y10_dot_y10 = y10.LengthSq();
+			float y10_dot_y20 = y10.Dot(y20);
+			float determinant = y10_dot_y10 * y20_dot_y20 - y10_dot_y20 * y10_dot_y20;
+			if (determinant > 0.0f) // If determinant == 0 then the system is linearly dependent and the triangle is degenerate, since y10.10 * y20.y20 > y10.y20^2 it should also be > 0
+			{
+				float y0_dot_y10 = y0.Dot(y10);
+				float y0_dot_y20 = y0.Dot(y20);
+				float l0 = (y10_dot_y20 * y0_dot_y20 - y20_dot_y20 * y0_dot_y10) / determinant;
+				float l1 = (y10_dot_y20 * y0_dot_y10 - y10_dot_y10 * y0_dot_y20) / determinant;
+				mLambda[0] = l0;
+				mLambda[1] = l1;
+				mLambdaRelativeTo0 = true;
+
+				// Check if closest point is interior to the triangle. For a convex hull which contains the origin each face must contain the origin, but because
+				// our faces are triangles, we can have multiple coplanar triangles and only 1 will have the origin as an interior point. We want to use this triangle
+				// to calculate the contact points because it gives the most accurate results, so we will only add these triangles to the priority queue.
+				if (l0 > -cBarycentricEpsilon && l1 > -cBarycentricEpsilon && l0 + l1 < 1.0f + cBarycentricEpsilon)
+					mClosestPointInterior = true;
+			}
+		}
+	}
+	else
+	{
+		// We select the edges y10 and y21
+		mNormal = y10.Cross(y21);
+
+		// Check if triangle is degenerate
+		float normal_len_sq = mNormal.LengthSq();
+		if (normal_len_sq > cMinTriangleArea)
+		{
+			// Again calculate distance between triangle and origin
+			float c_dot_n = mCentroid.Dot(mNormal);
+			mClosestLenSq = abs(c_dot_n) * c_dot_n / normal_len_sq;
+
+			// Calculate closest point to origin using barycentric coordinates but this time using y1 as the reference vertex
+			//
+			// v = y1 + l0 * (y0 - y1) + l1 * (y2 - y1)
+			// v . (y0 - y1) = 0
+			// v . (y2 - y1) = 0
+			//
+			// Written in matrix form:
+			//
+			// | y10.y10  -y21.y10 | | l0 | = | y1.y10 |
+			// | -y10.y21  y21.y21 | | l1 |   | -y1.y21 |
+			//
+			// Cramers rule to invert matrix:
+			float y10_dot_y10 = y10.LengthSq();
+			float y10_dot_y21 = y10.Dot(y21);
+			float determinant = y10_dot_y10 * y21_dot_y21 - y10_dot_y21 * y10_dot_y21;
+			if (determinant > 0.0f)
+			{
+				float y1_dot_y10 = y1.Dot(y10);
+				float y1_dot_y21 = y1.Dot(y21);
+				float l0 = (y21_dot_y21 * y1_dot_y10 - y10_dot_y21 * y1_dot_y21) / determinant;
+				float l1 = (y10_dot_y21 * y1_dot_y10 - y10_dot_y10 * y1_dot_y21) / determinant;
+				mLambda[0] = l0;
+				mLambda[1] = l1;
+				mLambdaRelativeTo0 = false;
+
+				// Again check if the closest point is inside the triangle
+				if (l0 > -cBarycentricEpsilon && l1 > -cBarycentricEpsilon && l0 + l1 < 1.0f + cBarycentricEpsilon)
+					mClosestPointInterior = true;
+			}
+		}
+	}
+}
+
+JPH_PRECISE_MATH_OFF
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Geometry/EPAPenetrationDepth.h

@@ -0,0 +1,559 @@
+// 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/Core/Profiler.h>
+#include <Jolt/Geometry/GJKClosestPoint.h>
+#include <Jolt/Geometry/EPAConvexHullBuilder.h>
+
+//#define JPH_EPA_PENETRATION_DEPTH_DEBUG
+
+JPH_NAMESPACE_BEGIN
+
+/// Implementation of Expanding Polytope Algorithm as described in:
+///
+/// Proximity Queries and Penetration Depth Computation on 3D Game Objects - Gino van den Bergen
+///
+/// The implementation of this algorithm does not completely follow the article, instead of splitting
+/// triangles at each edge as in fig. 7 in the article, we build a convex hull (removing any triangles that
+/// are facing the new point, thereby avoiding the problem of getting really oblong triangles as mentioned in
+/// the article).
+///
+/// The algorithm roughly works like:
+///
+/// - Start with a simplex of the Minkowski sum (difference) of two objects that was calculated by GJK
+/// - This simplex should contain the origin (or else GJK would have reported: no collision)
+/// - In cases where the simplex consists of 1 - 3 points, find some extra support points (of the Minkowski sum) to get to at least 4 points
+/// - Convert this into a convex hull with non-zero volume (which includes the origin)
+/// - A: Calculate the closest point to the origin for all triangles of the hull and take the closest one
+/// - Calculate a new support point (of the Minkowski sum) in this direction and add this point to the convex hull
+/// - This will remove all faces that are facing the new point and will create new triangles to fill up the hole
+/// - Loop to A until no closer point found
+/// - The closest point indicates the position / direction of least penetration
+class EPAPenetrationDepth
+{
+private:
+	// Typedefs
+	static constexpr int cMaxPoints = EPAConvexHullBuilder::cMaxPoints;
+	static constexpr int cMaxPointsToIncludeOriginInHull = 32;
+	static_assert(cMaxPointsToIncludeOriginInHull < cMaxPoints);
+
+	using Triangle = EPAConvexHullBuilder::Triangle;
+	using Points = EPAConvexHullBuilder::Points;
+
+	/// The GJK algorithm, used to start the EPA algorithm
+	GJKClosestPoint		mGJK;
+
+#ifdef JPH_ENABLE_ASSERTS
+	/// Tolerance as passed to the GJK algorithm, used for asserting.
+	float				mGJKTolerance = 0.0f;
+#endif // JPH_ENABLE_ASSERTS
+
+	/// A list of support points for the EPA algorithm
+	class SupportPoints
+	{
+	public:
+		/// List of support points
+		Points			mY;
+		Vec3			mP[cMaxPoints];
+		Vec3			mQ[cMaxPoints];
+
+		/// Calculate and add new support point to the list of points
+		template <typename A, typename B>
+		Vec3			Add(const A &inA, const B &inB, Vec3Arg inDirection, int &outIndex)
+		{
+			// Get support point of the minkowski sum A - B
+			Vec3 p = inA.GetSupport(inDirection);
+			Vec3 q = inB.GetSupport(-inDirection);
+			Vec3 w = p - q;
+
+			// Store new point
+			outIndex = mY.size();
+			mY.push_back(w);
+			mP[outIndex] = p;
+			mQ[outIndex] = q;
+
+			return w;
+		}
+	};
+
+public:
+	/// Return code for GetPenetrationDepthStepGJK
+	enum class EStatus
+	{
+		NotColliding,		///< Returned if the objects don't collide, in this case outPointA/outPointB are invalid
+		Colliding,			///< Returned if the objects penetrate
+		Indeterminate		///< Returned if the objects penetrate further than the convex radius. In this case you need to call GetPenetrationDepthStepEPA to get the actual penetration depth.
+	};
+
+	/// Calculates penetration depth between two objects, first step of two (the GJK step)
+	///
+	/// @param inAExcludingConvexRadius Object A without convex radius.
+	/// @param inBExcludingConvexRadius Object B without convex radius.
+	/// @param inConvexRadiusA Convex radius for A.
+	/// @param inConvexRadiusB Convex radius for B.
+	/// @param ioV Pass in previously returned value or (1, 0, 0). On return this value is changed to direction to move B out of collision along the shortest path (magnitude is meaningless).
+	/// @param inTolerance Minimal distance before A and B are considered colliding.
+	/// @param outPointA Position on A that has the least amount of penetration.
+	/// @param outPointB Position on B that has the least amount of penetration.
+	/// Use |outPointB - outPointA| to get the distance of penetration.
+	template <typename AE, typename BE>
+	EStatus				GetPenetrationDepthStepGJK(const AE &inAExcludingConvexRadius, float inConvexRadiusA, const BE &inBExcludingConvexRadius, float inConvexRadiusB, float inTolerance, Vec3 &ioV, Vec3 &outPointA, Vec3 &outPointB)
+	{
+		JPH_PROFILE_FUNCTION();
+
+		JPH_IF_ENABLE_ASSERTS(mGJKTolerance = inTolerance;)
+
+		// Don't supply a zero ioV, we only want to get points on the hull of the Minkowsky sum and not internal points.
+		//
+		// Note that if the assert below triggers, it is very likely that you have a MeshShape that contains a degenerate triangle (e.g. a sliver).
+		// Go up a couple of levels in the call stack to see if we're indeed testing a triangle and if it is degenerate.
+		// If this is the case then fix the triangles you supply to the MeshShape.
+		JPH_ASSERT(!ioV.IsNearZero());
+
+		// Get closest points
+		float combined_radius = inConvexRadiusA + inConvexRadiusB;
+		float combined_radius_sq = combined_radius * combined_radius;
+		float closest_points_dist_sq = mGJK.GetClosestPoints(inAExcludingConvexRadius, inBExcludingConvexRadius, inTolerance, combined_radius_sq, ioV, outPointA, outPointB);
+		if (closest_points_dist_sq > combined_radius_sq)
+		{
+			// No collision
+			return EStatus::NotColliding;
+		}
+		if (closest_points_dist_sq > 0.0f)
+		{
+			// Collision within convex radius, adjust points for convex radius
+			float v_len = sqrt(closest_points_dist_sq); // GetClosestPoints function returns |ioV|^2 when return value < FLT_MAX
+			outPointA += ioV * (inConvexRadiusA / v_len);
+			outPointB -= ioV * (inConvexRadiusB / v_len);
+			return EStatus::Colliding;
+		}
+
+		return EStatus::Indeterminate;
+	}
+
+	/// Calculates penetration depth between two objects, second step (the EPA step)
+	///
+	/// @param inAIncludingConvexRadius Object A with convex radius
+	/// @param inBIncludingConvexRadius Object B with convex radius
+	/// @param inTolerance A factor that determines the accuracy of the result. If the change of the squared distance is less than inTolerance * current_penetration_depth^2 the algorithm will terminate. Should be bigger or equal to FLT_EPSILON.
+	/// @param outV Direction to move B out of collision along the shortest path (magnitude is meaningless)
+	/// @param outPointA Position on A that has the least amount of penetration
+	/// @param outPointB Position on B that has the least amount of penetration
+	/// Use |outPointB - outPointA| to get the distance of penetration
+	///
+	/// @return False if the objects don't collide, in this case outPointA/outPointB are invalid.
+	/// True if the objects penetrate
+	template <typename AI, typename BI>
+	bool				GetPenetrationDepthStepEPA(const AI &inAIncludingConvexRadius, const BI &inBIncludingConvexRadius, float inTolerance, Vec3 &outV, Vec3 &outPointA, Vec3 &outPointB)
+	{
+		JPH_PROFILE_FUNCTION();
+
+		// Check that the tolerance makes sense (smaller value than this will just result in needless iterations)
+		JPH_ASSERT(inTolerance >= FLT_EPSILON);
+
+		// Fetch the simplex from GJK algorithm
+		SupportPoints support_points;
+		mGJK.GetClosestPointsSimplex(support_points.mY.data(), support_points.mP, support_points.mQ, support_points.mY.GetSizeRef());
+
+		// Fill up the amount of support points to 4
+		switch (support_points.mY.size())
+		{
+		case 1:
+			{
+				// 1 vertex, which must be at the origin, which is useless for our purpose
+				JPH_ASSERT(support_points.mY[0].IsNearZero(Square(mGJKTolerance)));
+				support_points.mY.pop_back();
+
+				// Add support points in 4 directions to form a tetrahedron around the origin
+				int p1, p2, p3, p4;
+				(void)support_points.Add(inAIncludingConvexRadius, inBIncludingConvexRadius, Vec3(0, 1, 0), p1);
+				(void)support_points.Add(inAIncludingConvexRadius, inBIncludingConvexRadius, Vec3(-1, -1, -1), p2);
+				(void)support_points.Add(inAIncludingConvexRadius, inBIncludingConvexRadius, Vec3(1, -1, -1), p3);
+				(void)support_points.Add(inAIncludingConvexRadius, inBIncludingConvexRadius, Vec3(0, -1, 1), p4);
+				JPH_ASSERT(p1 == 0);
+				JPH_ASSERT(p2 == 1);
+				JPH_ASSERT(p3 == 2);
+				JPH_ASSERT(p4 == 3);
+				break;
+			}
+
+		case 2:
+			{
+				// Two vertices, create 3 extra by taking perpendicular axis and rotating it around in 120 degree increments
+				Vec3 axis = (support_points.mY[1] - support_points.mY[0]).Normalized();
+				Mat44 rotation = Mat44::sRotation(axis, DegreesToRadians(120.0f));
+				Vec3 dir1 = axis.GetNormalizedPerpendicular();
+				Vec3 dir2 = rotation * dir1;
+				Vec3 dir3 = rotation * dir2;
+				int p1, p2, p3;
+				(void)support_points.Add(inAIncludingConvexRadius, inBIncludingConvexRadius, dir1, p1);
+				(void)support_points.Add(inAIncludingConvexRadius, inBIncludingConvexRadius, dir2, p2);
+				(void)support_points.Add(inAIncludingConvexRadius, inBIncludingConvexRadius, dir3, p3);
+				JPH_ASSERT(p1 == 2);
+				JPH_ASSERT(p2 == 3);
+				JPH_ASSERT(p3 == 4);
+				break;
+			}
+
+		case 3:
+		case 4:
+			// We already have enough points
+			break;
+		}
+
+		// Create hull out of the initial points
+		JPH_ASSERT(support_points.mY.size() >= 3);
+		EPAConvexHullBuilder hull(support_points.mY);
+#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
+		hull.DrawLabel("Build initial hull");
+#endif
+#ifdef JPH_EPA_PENETRATION_DEPTH_DEBUG
+		Trace("Init: num_points = %u", (uint)support_points.mY.size());
+#endif
+		hull.Initialize(0, 1, 2);
+		for (typename Points::size_type i = 3; i < support_points.mY.size(); ++i)
+		{
+			float dist_sq;
+			Triangle *t = hull.FindFacingTriangle(support_points.mY[i], dist_sq);
+			if (t != nullptr)
+			{
+				EPAConvexHullBuilder::NewTriangles new_triangles;
+				if (!hull.AddPoint(t, i, FLT_MAX, new_triangles))
+				{
+					// We can't recover from a failure to add a point to the hull because the old triangles have been unlinked already.
+					// Assume no collision. This can happen if the shapes touch in 1 point (or plane) in which case the hull is degenerate.
+					return false;
+				}
+			}
+		}
+
+#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
+		hull.DrawLabel("Complete hull");
+
+		// Generate the hull of the Minkowski difference for visualization
+		MinkowskiDifference diff(inAIncludingConvexRadius, inBIncludingConvexRadius);
+		DebugRenderer::GeometryRef geometry = DebugRenderer::sInstance->CreateTriangleGeometryForConvex([&diff](Vec3Arg inDirection) { return diff.GetSupport(inDirection); });
+		hull.DrawGeometry(geometry, Color::sYellow);
+
+		hull.DrawLabel("Ensure origin in hull");
+#endif
+
+		// Loop until we are sure that the origin is inside the hull
+		for (;;)
+		{
+			// Get the next closest triangle
+			Triangle *t = hull.PeekClosestTriangleInQueue();
+
+			// Don't process removed triangles, just free them (because they're in a heap we don't remove them earlier since we would have to rebuild the sorted heap)
+			if (t->mRemoved)
+			{
+				hull.PopClosestTriangleFromQueue();
+
+				// If we run out of triangles, we couldn't include the origin in the hull so there must be very little penetration and we report no collision.
+				if (!hull.HasNextTriangle())
+					return false;
+
+				hull.FreeTriangle(t);
+				continue;
+			}
+
+			// If the closest to the triangle is zero or positive, the origin is in the hull and we can proceed to the main algorithm
+			if (t->mClosestLenSq >= 0.0f)
+				break;
+
+#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
+			hull.DrawLabel("Next iteration");
+#endif
+#ifdef JPH_EPA_PENETRATION_DEPTH_DEBUG
+			Trace("EncapsulateOrigin: verts = (%d, %d, %d), closest_dist_sq = %g, centroid = (%g, %g, %g), normal = (%g, %g, %g)",
+				t->mEdge[0].mStartIdx, t->mEdge[1].mStartIdx, t->mEdge[2].mStartIdx,
+				t->mClosestLenSq,
+				t->mCentroid.GetX(), t->mCentroid.GetY(), t->mCentroid.GetZ(),
+				t->mNormal.GetX(), t->mNormal.GetY(), t->mNormal.GetZ());
+#endif
+
+			// Remove the triangle from the queue before we start adding new ones (which may result in a new closest triangle at the front of the queue)
+			hull.PopClosestTriangleFromQueue();
+
+			// Add a support point to get the origin inside the hull
+			int new_index;
+			Vec3 w = support_points.Add(inAIncludingConvexRadius, inBIncludingConvexRadius, t->mNormal, new_index);
+
+#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
+			// Draw the point that we're adding
+			hull.DrawMarker(w, Color::sRed, 1.0f);
+			hull.DrawWireTriangle(*t, Color::sRed);
+			hull.DrawState();
+#endif
+
+			// Add the point to the hull, if we fail we terminate and report no collision
+			EPAConvexHullBuilder::NewTriangles new_triangles;
+			if (!t->IsFacing(w) || !hull.AddPoint(t, new_index, FLT_MAX, new_triangles))
+				return false;
+
+			// The triangle is facing the support point "w" and can now be safely removed
+			JPH_ASSERT(t->mRemoved);
+			hull.FreeTriangle(t);
+
+			// If we run out of triangles or points, we couldn't include the origin in the hull so there must be very little penetration and we report no collision.
+			if (!hull.HasNextTriangle() || support_points.mY.size() >= cMaxPointsToIncludeOriginInHull)
+				return false;
+		}
+
+#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
+		hull.DrawLabel("Main algorithm");
+#endif
+
+		// Current closest distance to origin
+		float closest_dist_sq = FLT_MAX;
+
+		// Remember last good triangle
+		Triangle *last = nullptr;
+
+		// If we want to flip the penetration depth
+		bool flip_v_sign = false;
+
+		// Loop until closest point found
+		do
+		{
+			// Get closest triangle to the origin
+			Triangle *t = hull.PopClosestTriangleFromQueue();
+
+			// Don't process removed triangles, just free them (because they're in a heap we don't remove them earlier since we would have to rebuild the sorted heap)
+			if (t->mRemoved)
+			{
+				hull.FreeTriangle(t);
+				continue;
+			}
+
+#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
+			hull.DrawLabel("Next iteration");
+#endif
+#ifdef JPH_EPA_PENETRATION_DEPTH_DEBUG
+			Trace("FindClosest: verts = (%d, %d, %d), closest_len_sq = %g, centroid = (%g, %g, %g), normal = (%g, %g, %g)",
+				t->mEdge[0].mStartIdx, t->mEdge[1].mStartIdx, t->mEdge[2].mStartIdx,
+				t->mClosestLenSq,
+				t->mCentroid.GetX(), t->mCentroid.GetY(), t->mCentroid.GetZ(),
+				t->mNormal.GetX(), t->mNormal.GetY(), t->mNormal.GetZ());
+#endif
+			// Check if next triangle is further away than closest point, we've found the closest point
+			if (t->mClosestLenSq >= closest_dist_sq)
+				break;
+
+			// Replace last good with this triangle
+			if (last != nullptr)
+				hull.FreeTriangle(last);
+			last = t;
+
+			// Add support point in direction of normal of the plane
+			// Note that the article uses the closest point between the origin and plane, but this always has the exact same direction as the normal (if the origin is behind the plane)
+			// and this way we do less calculations and lose less precision
+			int new_index;
+			Vec3 w = support_points.Add(inAIncludingConvexRadius, inBIncludingConvexRadius, t->mNormal, new_index);
+
+			// Project w onto the triangle normal
+			float dot = t->mNormal.Dot(w);
+
+			// Check if we just found a separating axis. This can happen if the shape shrunk by convex radius and then expanded by
+			// convex radius is bigger then the original shape due to inaccuracies in the shrinking process.
+			if (dot < 0.0f)
+				return false;
+
+			// Get the distance squared (along normal) to the support point
+			float dist_sq = Square(dot) / t->mNormal.LengthSq();
+
+#ifdef JPH_EPA_PENETRATION_DEPTH_DEBUG
+			Trace("FindClosest: w = (%g, %g, %g), dot = %g, dist_sq = %g",
+				w.GetX(), w.GetY(), w.GetZ(),
+				dot, dist_sq);
+#endif
+#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
+			// Draw the point that we're adding
+			hull.DrawMarker(w, Color::sPurple, 1.0f);
+			hull.DrawWireTriangle(*t, Color::sPurple);
+			hull.DrawState();
+#endif
+
+			// If the error became small enough, we've converged
+			if (dist_sq - t->mClosestLenSq < t->mClosestLenSq * inTolerance)
+			{
+#ifdef JPH_EPA_PENETRATION_DEPTH_DEBUG
+				Trace("Converged");
+#endif // JPH_EPA_PENETRATION_DEPTH_DEBUG
+				break;
+			}
+
+			// Keep track of the minimum distance
+			closest_dist_sq = min(closest_dist_sq, dist_sq);
+
+			// If the triangle thinks this point is not front facing, we've reached numerical precision and we're done
+			if (!t->IsFacing(w))
+			{
+#ifdef JPH_EPA_PENETRATION_DEPTH_DEBUG
+				Trace("Not facing triangle");
+#endif // JPH_EPA_PENETRATION_DEPTH_DEBUG
+				break;
+			}
+
+			// Add point to hull
+			EPAConvexHullBuilder::NewTriangles new_triangles;
+			if (!hull.AddPoint(t, new_index, closest_dist_sq, new_triangles))
+			{
+#ifdef JPH_EPA_PENETRATION_DEPTH_DEBUG
+				Trace("Could not add point");
+#endif // JPH_EPA_PENETRATION_DEPTH_DEBUG
+				break;
+			}
+
+			// If the hull is starting to form defects then we're reaching numerical precision and we have to stop
+			bool has_defect = false;
+			for (const Triangle *nt : new_triangles)
+				if (nt->IsFacingOrigin())
+				{
+					has_defect = true;
+					break;
+				}
+			if (has_defect)
+			{
+#ifdef JPH_EPA_PENETRATION_DEPTH_DEBUG
+				Trace("Has defect");
+#endif // JPH_EPA_PENETRATION_DEPTH_DEBUG
+				// When the hull has defects it is possible that the origin has been classified on the wrong side of the triangle
+				// so we do an additional check to see if the penetration in the -triangle normal direction is smaller than
+				// the penetration in the triangle normal direction. If so we must flip the sign of the penetration depth.
+				Vec3 w2 = inAIncludingConvexRadius.GetSupport(-t->mNormal) - inBIncludingConvexRadius.GetSupport(t->mNormal);
+				float dot2 = -t->mNormal.Dot(w2);
+				if (dot2 < dot)
+					flip_v_sign = true;
+				break;
+			}
+		}
+		while (hull.HasNextTriangle() && support_points.mY.size() < cMaxPoints);
+
+		// Determine closest points, if last == null it means the hull was a plane so there's no penetration
+		if (last == nullptr)
+			return false;
+
+#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
+		hull.DrawLabel("Closest found");
+		hull.DrawWireTriangle(*last, Color::sWhite);
+		hull.DrawArrow(last->mCentroid, last->mCentroid + last->mNormal.NormalizedOr(Vec3::sZero()), Color::sWhite, 0.1f);
+		hull.DrawState();
+#endif
+
+		// Calculate penetration by getting the vector from the origin to the closest point on the triangle:
+		// distance = (centroid - origin) . normal / |normal|, closest = origin + distance * normal / |normal|
+		outV = (last->mCentroid.Dot(last->mNormal) / last->mNormal.LengthSq()) * last->mNormal;
+
+		// If penetration is near zero, treat this as a non collision since we cannot find a good normal
+		if (outV.IsNearZero())
+			return false;
+
+		// Check if we have to flip the sign of the penetration depth
+		if (flip_v_sign)
+			outV = -outV;
+
+		// Use the barycentric coordinates for the closest point to the origin to find the contact points on A and B
+		Vec3 p0 = support_points.mP[last->mEdge[0].mStartIdx];
+		Vec3 p1 = support_points.mP[last->mEdge[1].mStartIdx];
+		Vec3 p2 = support_points.mP[last->mEdge[2].mStartIdx];
+
+		Vec3 q0 = support_points.mQ[last->mEdge[0].mStartIdx];
+		Vec3 q1 = support_points.mQ[last->mEdge[1].mStartIdx];
+		Vec3 q2 = support_points.mQ[last->mEdge[2].mStartIdx];
+
+		if (last->mLambdaRelativeTo0)
+		{
+			// y0 was the reference vertex
+			outPointA = p0 + last->mLambda[0] * (p1 - p0) + last->mLambda[1] * (p2 - p0);
+			outPointB = q0 + last->mLambda[0] * (q1 - q0) + last->mLambda[1] * (q2 - q0);
+		}
+		else
+		{
+			// y1 was the reference vertex
+			outPointA = p1 + last->mLambda[0] * (p0 - p1) + last->mLambda[1] * (p2 - p1);
+			outPointB = q1 + last->mLambda[0] * (q0 - q1) + last->mLambda[1] * (q2 - q1);
+		}
+
+		return true;
+	}
+
+	/// This function combines the GJK and EPA steps and is provided as a convenience function.
+	/// Note: less performant since you're providing all support functions in one go
+	/// Note 2: You need to initialize ioV, see documentation at GetPenetrationDepthStepGJK!
+	template <typename AE, typename AI, typename BE, typename BI>
+	bool				GetPenetrationDepth(const AE &inAExcludingConvexRadius, const AI &inAIncludingConvexRadius, float inConvexRadiusA, const BE &inBExcludingConvexRadius, const BI &inBIncludingConvexRadius, float inConvexRadiusB, float inCollisionToleranceSq, float inPenetrationTolerance, Vec3 &ioV, Vec3 &outPointA, Vec3 &outPointB)
+	{
+		// Check result of collision detection
+		switch (GetPenetrationDepthStepGJK(inAExcludingConvexRadius, inConvexRadiusA, inBExcludingConvexRadius, inConvexRadiusB, inCollisionToleranceSq, ioV, outPointA, outPointB))
+		{
+		case EPAPenetrationDepth::EStatus::Colliding:
+			return true;
+
+		case EPAPenetrationDepth::EStatus::NotColliding:
+			return false;
+
+		case EPAPenetrationDepth::EStatus::Indeterminate:
+			return GetPenetrationDepthStepEPA(inAIncludingConvexRadius, inBIncludingConvexRadius, inPenetrationTolerance, ioV, outPointA, outPointB);
+		}
+
+		JPH_ASSERT(false);
+		return false;
+	}
+
+	/// Test if a cast shape inA moving from inStart to lambda * inStart.GetTranslation() + inDirection where lambda e [0, ioLambda> intersects inB
+	///
+	/// @param inStart Start position and orientation of the convex object
+	/// @param inDirection Direction of the sweep (ioLambda * inDirection determines length)
+	///	@param inCollisionTolerance The minimal distance between A and B before they are considered colliding
+	/// @param inPenetrationTolerance A factor that determines the accuracy of the result. If the change of the squared distance is less than inTolerance * current_penetration_depth^2 the algorithm will terminate. Should be bigger or equal to FLT_EPSILON.
+	/// @param inA The convex object A, must support the GetSupport(Vec3) function.
+	/// @param inB The convex object B, must support the GetSupport(Vec3) function.
+	/// @param inConvexRadiusA The convex radius of A, this will be added on all sides to pad A.
+	/// @param inConvexRadiusB The convex radius of B, this will be added on all sides to pad B.
+	/// @param inReturnDeepestPoint If the shapes are initially intersecting this determines if the EPA algorithm will run to find the deepest point
+	/// @param ioLambda The max fraction along the sweep, on output updated with the actual collision fraction.
+	///	@param outPointA is the contact point on A
+	///	@param outPointB is the contact point on B
+	/// @param outContactNormal is either the contact normal when the objects are touching or the penetration axis when the objects are penetrating at the start of the sweep (pointing from A to B, length will not be 1)
+	///
+	/// @return true if the a hit was found, in which case ioLambda, outPointA, outPointB and outSurfaceNormal are updated.
+	template <typename A, typename B>
+	bool				CastShape(Mat44Arg inStart, Vec3Arg inDirection, float inCollisionTolerance, float inPenetrationTolerance, const A &inA, const B &inB, float inConvexRadiusA, float inConvexRadiusB, bool inReturnDeepestPoint, float &ioLambda, Vec3 &outPointA, Vec3 &outPointB, Vec3 &outContactNormal)
+	{
+		JPH_IF_ENABLE_ASSERTS(mGJKTolerance = inCollisionTolerance;)
+
+		// First determine if there's a collision at all
+		if (!mGJK.CastShape(inStart, inDirection, inCollisionTolerance, inA, inB, inConvexRadiusA, inConvexRadiusB, ioLambda, outPointA, outPointB, outContactNormal))
+			return false;
+
+		// When our contact normal is too small, we don't have an accurate result
+		bool contact_normal_invalid = outContactNormal.IsNearZero(Square(inCollisionTolerance));
+
+		if (inReturnDeepestPoint
+			&& ioLambda == 0.0f // Only when lambda = 0 we can have the bodies overlap
+			&& (inConvexRadiusA + inConvexRadiusB == 0.0f // When no convex radius was provided we can never trust contact points at lambda = 0
+				|| contact_normal_invalid))
+		{
+			// If we're initially intersecting, we need to run the EPA algorithm in order to find the deepest contact point
+			AddConvexRadius add_convex_a(inA, inConvexRadiusA);
+			AddConvexRadius add_convex_b(inB, inConvexRadiusB);
+			TransformedConvexObject transformed_a(inStart, add_convex_a);
+			if (!GetPenetrationDepthStepEPA(transformed_a, add_convex_b, inPenetrationTolerance, outContactNormal, outPointA, outPointB))
+				return false;
+		}
+		else if (contact_normal_invalid)
+		{
+			// If we weren't able to calculate a contact normal, use the cast direction instead
+			outContactNormal = inDirection;
+		}
+
+		return true;
+	}
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Geometry/Ellipse.h

@@ -0,0 +1,77 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Math/Float2.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Ellipse centered around the origin
+/// @see https://en.wikipedia.org/wiki/Ellipse
+class Ellipse
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Construct ellipse with radius A along the X-axis and B along the Y-axis
+					Ellipse(float inA, float inB) : mA(inA), mB(inB) { JPH_ASSERT(inA > 0.0f); JPH_ASSERT(inB > 0.0f); }
+
+	/// Check if inPoint is inside the ellipse
+	bool			IsInside(const Float2 &inPoint) const
+	{
+		return Square(inPoint.x / mA) + Square(inPoint.y / mB) <= 1.0f;
+	}
+
+	/// Get the closest point on the ellipse to inPoint
+	/// Assumes inPoint is outside the ellipse
+	/// @see Rotation Joint Limits in Quaternion Space by Gino van den Bergen, section 10.1 in Game Engine Gems 3.
+	Float2			GetClosestPoint(const Float2 &inPoint) const
+	{
+		float a_sq = Square(mA);
+		float b_sq = Square(mB);
+
+		// Equation of ellipse: f(x, y) = (x/a)^2 + (y/b)^2 - 1 = 0											[1]
+		// Normal on surface: (df/dx, df/dy) = (2 x / a^2, 2 y / b^2)
+		// Closest point (x', y') on ellipse to point (x, y): (x', y') + t (x / a^2, y / b^2) = (x, y)
+		// <=> (x', y') = (a^2 x / (t + a^2), b^2 y / (t + b^2))
+		// Requiring point to be on ellipse (substituting into [1]): g(t) = (a x / (t + a^2))^2 + (b y / (t + b^2))^2 - 1 = 0
+
+		// Newton Raphson iteration, starting at t = 0
+		float t = 0.0f;
+		for (;;)
+		{
+			// Calculate g(t)
+			float t_plus_a_sq = t + a_sq;
+			float t_plus_b_sq = t + b_sq;
+			float gt = Square(mA * inPoint.x / t_plus_a_sq) + Square(mB * inPoint.y / t_plus_b_sq) - 1.0f;
+
+			// Check if g(t) it is close enough to zero
+			if (abs(gt) < 1.0e-6f)
+				return Float2(a_sq * inPoint.x / t_plus_a_sq, b_sq * inPoint.y / t_plus_b_sq);
+
+			// Get derivative dg/dt = g'(t) = -2 (b^2 y^2 / (t + b^2)^3 + a^2 x^2 / (t + a^2)^3)
+			float gt_accent = -2.0f *
+				(a_sq * Square(inPoint.x) / Cubed(t_plus_a_sq)
+				+ b_sq * Square(inPoint.y) / Cubed(t_plus_b_sq));
+
+			// Calculate t for next iteration: tn+1 = tn - g(t) / g'(t)
+			float tn = t - gt / gt_accent;
+			t = tn;
+		}
+	}
+
+	/// Get normal at point inPoint (non-normalized vector)
+	Float2			GetNormal(const Float2 &inPoint) const
+	{
+		// Calculated by [d/dx f(x, y), d/dy f(x, y)], where f(x, y) is the ellipse equation from above
+		return Float2(inPoint.x / Square(mA), inPoint.y / Square(mB));
+	}
+
+private:
+	float			mA;				///< Radius along X-axis
+	float			mB;				///< Radius along Y-axis
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Geometry/GJKClosestPoint.h

@@ -0,0 +1,945 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/NonCopyable.h>
+#include <Jolt/Geometry/ClosestPoint.h>
+#include <Jolt/Geometry/ConvexSupport.h>
+
+//#define JPH_GJK_DEBUG
+#ifdef JPH_GJK_DEBUG
+	#include <Jolt/Core/StringTools.h>
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif
+
+JPH_NAMESPACE_BEGIN
+
+/// Convex vs convex collision detection
+/// Based on: A Fast and Robust GJK Implementation for Collision Detection of Convex Objects - Gino van den Bergen
+class GJKClosestPoint : public NonCopyable
+{
+private:
+	/// Get new closest point to origin given simplex mY of mNumPoints points
+	///
+	/// @param inPrevVLenSq Length of |outV|^2 from the previous iteration, used as a maximum value when selecting a new closest point.
+	/// @param outV Closest point
+	/// @param outVLenSq |outV|^2
+	/// @param outSet Set of points that form the new simplex closest to the origin (bit 1 = mY[0], bit 2 = mY[1], ...)
+	///
+	/// If LastPointPartOfClosestFeature is true then the last point added will be assumed to be part of the closest feature and the function will do less work.
+	///
+	/// @return True if new closest point was found.
+	/// False if the function failed, in this case the output variables are not modified
+	template <bool LastPointPartOfClosestFeature>
+	bool		GetClosest(float inPrevVLenSq, Vec3 &outV, float &outVLenSq, uint32 &outSet) const
+	{
+#ifdef JPH_GJK_DEBUG
+		for (int i = 0; i < mNumPoints; ++i)
+			Trace("y[%d] = [%s], |y[%d]| = %g", i, ConvertToString(mY[i]).c_str(), i, (double)mY[i].Length());
+#endif
+
+		uint32 set;
+		Vec3 v;
+
+		switch (mNumPoints)
+		{
+		case 1:
+			// Single point
+			set = 0b0001;
+			v = mY[0];
+			break;
+
+		case 2:
+			// Line segment
+			v = ClosestPoint::GetClosestPointOnLine(mY[0], mY[1], set);
+			break;
+
+		case 3:
+			// Triangle
+			v = ClosestPoint::GetClosestPointOnTriangle<LastPointPartOfClosestFeature>(mY[0], mY[1], mY[2], set);
+			break;
+
+		case 4:
+			// Tetrahedron
+			v = ClosestPoint::GetClosestPointOnTetrahedron<LastPointPartOfClosestFeature>(mY[0], mY[1], mY[2], mY[3], set);
+			break;
+
+		default:
+			JPH_ASSERT(false);
+			return false;
+		}
+
+#ifdef JPH_GJK_DEBUG
+		Trace("GetClosest: set = 0b%s, v = [%s], |v| = %g", NibbleToBinary(set), ConvertToString(v).c_str(), (double)v.Length());
+#endif
+
+		float v_len_sq = v.LengthSq();
+		if (v_len_sq < inPrevVLenSq) // Note, comparison order important: If v_len_sq is NaN then this expression will be false so we will return false
+		{
+			// Return closest point
+			outV = v;
+			outVLenSq = v_len_sq;
+			outSet = set;
+			return true;
+		}
+
+		// No better match found
+#ifdef JPH_GJK_DEBUG
+		Trace("New closer point is further away, failed to converge");
+#endif
+		return false;
+	}
+
+	// Get max(|Y_0|^2 .. |Y_n|^2)
+	float		GetMaxYLengthSq() const
+	{
+		float y_len_sq = mY[0].LengthSq();
+		for (int i = 1; i < mNumPoints; ++i)
+			y_len_sq = max(y_len_sq, mY[i].LengthSq());
+		return y_len_sq;
+	}
+
+	// Remove points that are not in the set, only updates mY
+	void		UpdatePointSetY(uint32 inSet)
+	{
+		int num_points = 0;
+		for (int i = 0; i < mNumPoints; ++i)
+			if ((inSet & (1 << i)) != 0)
+			{
+				mY[num_points] = mY[i];
+				++num_points;
+			}
+		mNumPoints = num_points;
+	}
+
+	// Remove points that are not in the set, only updates mP
+	void		UpdatePointSetP(uint32 inSet)
+	{
+		int num_points = 0;
+		for (int i = 0; i < mNumPoints; ++i)
+			if ((inSet & (1 << i)) != 0)
+			{
+				mP[num_points] = mP[i];
+				++num_points;
+			}
+		mNumPoints = num_points;
+	}
+
+	// Remove points that are not in the set, only updates mP and mQ
+	void		UpdatePointSetPQ(uint32 inSet)
+	{
+		int num_points = 0;
+		for (int i = 0; i < mNumPoints; ++i)
+			if ((inSet & (1 << i)) != 0)
+			{
+				mP[num_points] = mP[i];
+				mQ[num_points] = mQ[i];
+				++num_points;
+			}
+		mNumPoints = num_points;
+	}
+
+	// Remove points that are not in the set, updates mY, mP and mQ
+	void		UpdatePointSetYPQ(uint32 inSet)
+	{
+		int num_points = 0;
+		for (int i = 0; i < mNumPoints; ++i)
+			if ((inSet & (1 << i)) != 0)
+			{
+				mY[num_points] = mY[i];
+				mP[num_points] = mP[i];
+				mQ[num_points] = mQ[i];
+				++num_points;
+			}
+		mNumPoints = num_points;
+	}
+
+	// Calculate closest points on A and B
+	void		CalculatePointAAndB(Vec3 &outPointA, Vec3 &outPointB) const
+	{
+		switch (mNumPoints)
+		{
+		case 1:
+			outPointA = mP[0];
+			outPointB = mQ[0];
+			break;
+
+		case 2:
+			{
+				float u, v;
+				ClosestPoint::GetBaryCentricCoordinates(mY[0], mY[1], u, v);
+				outPointA = u * mP[0] + v * mP[1];
+				outPointB = u * mQ[0] + v * mQ[1];
+			}
+			break;
+
+		case 3:
+			{
+				float u, v, w;
+				ClosestPoint::GetBaryCentricCoordinates(mY[0], mY[1], mY[2], u, v, w);
+				outPointA = u * mP[0] + v * mP[1] + w * mP[2];
+				outPointB = u * mQ[0] + v * mQ[1] + w * mQ[2];
+			}
+			break;
+
+		case 4:
+		#ifdef JPH_DEBUG
+			memset(&outPointA, 0xcd, sizeof(outPointA));
+			memset(&outPointB, 0xcd, sizeof(outPointB));
+		#endif
+			break;
+		}
+	}
+
+public:
+	/// Test if inA and inB intersect
+	///
+	/// @param inA The convex object A, must support the GetSupport(Vec3) function.
+	/// @param inB The convex object B, must support the GetSupport(Vec3) function.
+	///	@param inTolerance Minimal distance between objects when the objects are considered to be colliding
+	///	@param ioV is used as initial separating axis (provide a zero vector if you don't know yet)
+	///
+	///	@return True if they intersect (in which case ioV = (0, 0, 0)).
+	///	False if they don't intersect in which case ioV is a separating axis in the direction from A to B (magnitude is meaningless)
+	template <typename A, typename B>
+	bool		Intersects(const A &inA, const B &inB, float inTolerance, Vec3 &ioV)
+	{
+		float tolerance_sq = Square(inTolerance);
+
+		// Reset state
+		mNumPoints = 0;
+
+#ifdef JPH_GJK_DEBUG
+		for (int i = 0; i < 4; ++i)
+			mY[i] = Vec3::sZero();
+#endif
+
+		// Previous length^2 of v
+		float prev_v_len_sq = FLT_MAX;
+
+		for (;;)
+		{
+#ifdef JPH_GJK_DEBUG
+			Trace("v = [%s], num_points = %d", ConvertToString(ioV).c_str(), mNumPoints);
+#endif
+
+			// Get support points for shape A and B in the direction of v
+			Vec3 p = inA.GetSupport(ioV);
+			Vec3 q = inB.GetSupport(-ioV);
+
+			// Get support point of the minkowski sum A - B of v
+			Vec3 w = p - q;
+
+			// If the support point sA-B(v) is in the opposite direction as v, then we have found a separating axis and there is no intersection
+			if (ioV.Dot(w) < 0.0f)
+			{
+				// Separating axis found
+#ifdef JPH_GJK_DEBUG
+				Trace("Separating axis");
+#endif
+				return false;
+			}
+
+			// Store the point for later use
+			mY[mNumPoints] = w;
+			++mNumPoints;
+
+#ifdef JPH_GJK_DEBUG
+			Trace("w = [%s]", ConvertToString(w).c_str());
+#endif
+
+			// Determine the new closest point
+			float v_len_sq;			// Length^2 of v
+			uint32 set;				// Set of points that form the new simplex
+			if (!GetClosest<true>(prev_v_len_sq, ioV, v_len_sq, set))
+				return false;
+
+			// If there are 4 points, the origin is inside the tetrahedron and we're done
+			if (set == 0xf)
+			{
+#ifdef JPH_GJK_DEBUG
+				Trace("Full simplex");
+#endif
+				ioV = Vec3::sZero();
+				return true;
+			}
+
+			// If v is very close to zero, we consider this a collision
+			if (v_len_sq <= tolerance_sq)
+			{
+#ifdef JPH_GJK_DEBUG
+				Trace("Distance zero");
+#endif
+				ioV = Vec3::sZero();
+				return true;
+			}
+
+			// If v is very small compared to the length of y, we also consider this a collision
+			if (v_len_sq <= FLT_EPSILON * GetMaxYLengthSq())
+			{
+#ifdef JPH_GJK_DEBUG
+				Trace("Machine precision reached");
+#endif
+				ioV = Vec3::sZero();
+				return true;
+			}
+
+			// The next separation axis to test is the negative of the closest point of the Minkowski sum to the origin
+			// Note: This must be done before terminating as converged since the separating axis is -v
+			ioV = -ioV;
+
+			// If the squared length of v is not changing enough, we've converged and there is no collision
+			JPH_ASSERT(prev_v_len_sq >= v_len_sq);
+			if (prev_v_len_sq - v_len_sq <= FLT_EPSILON * prev_v_len_sq)
+			{
+				// v is a separating axis
+#ifdef JPH_GJK_DEBUG
+				Trace("Converged");
+#endif
+				return false;
+			}
+			prev_v_len_sq = v_len_sq;
+
+			// Update the points of the simplex
+			UpdatePointSetY(set);
+		}
+	}
+
+	/// Get closest points between inA and inB
+	///
+	/// @param inA The convex object A, must support the GetSupport(Vec3) function.
+	/// @param inB The convex object B, must support the GetSupport(Vec3) function.
+	///	@param inTolerance The minimal distance between A and B before the objects are considered colliding and processing is terminated.
+	///	@param inMaxDistSq The maximum squared distance between A and B before the objects are considered infinitely far away and processing is terminated.
+	///	@param ioV Initial guess for the separating axis. Start with any non-zero vector if you don't know.
+	///		If return value is 0, ioV = (0, 0, 0).
+	///		If the return value is bigger than 0 but smaller than FLT_MAX, ioV will be the separating axis in the direction from A to B and its length the squared distance between A and B.
+	///		If the return value is FLT_MAX, ioV will be the separating axis in the direction from A to B and the magnitude of the vector is meaningless.
+	///	@param outPointA , outPointB
+	///		If the return value is 0 the points are invalid.
+	///		If the return value is bigger than 0 but smaller than FLT_MAX these will contain the closest point on A and B.
+	///		If the return value is FLT_MAX the points are invalid.
+	///
+	///	@return The squared distance between A and B or FLT_MAX when they are further away than inMaxDistSq.
+	template <typename A, typename B>
+	float		GetClosestPoints(const A &inA, const B &inB, float inTolerance, float inMaxDistSq, Vec3 &ioV, Vec3 &outPointA, Vec3 &outPointB)
+	{
+		float tolerance_sq = Square(inTolerance);
+
+		// Reset state
+		mNumPoints = 0;
+
+#ifdef JPH_GJK_DEBUG
+		// Generate the hull of the Minkowski difference for visualization
+		MinkowskiDifference diff(inA, inB);
+		mGeometry = DebugRenderer::sInstance->CreateTriangleGeometryForConvex([&diff](Vec3Arg inDirection) { return diff.GetSupport(inDirection); });
+
+		for (int i = 0; i < 4; ++i)
+		{
+			mY[i] = Vec3::sZero();
+			mP[i] = Vec3::sZero();
+			mQ[i] = Vec3::sZero();
+		}
+#endif
+
+		// Length^2 of v
+		float v_len_sq = ioV.LengthSq();
+
+		// Previous length^2 of v
+		float prev_v_len_sq = FLT_MAX;
+
+		for (;;)
+		{
+#ifdef JPH_GJK_DEBUG
+			Trace("v = [%s], num_points = %d", ConvertToString(ioV).c_str(), mNumPoints);
+#endif
+
+			// Get support points for shape A and B in the direction of v
+			Vec3 p = inA.GetSupport(ioV);
+			Vec3 q = inB.GetSupport(-ioV);
+
+			// Get support point of the minkowski sum A - B of v
+			Vec3 w = p - q;
+
+			float dot = ioV.Dot(w);
+
+#ifdef JPH_GJK_DEBUG
+			// Draw -ioV to show the closest point to the origin from the previous simplex
+			DebugRenderer::sInstance->DrawArrow(mOffset, mOffset - ioV, Color::sOrange, 0.05f);
+
+			// Draw ioV to show where we're probing next
+			DebugRenderer::sInstance->DrawArrow(mOffset, mOffset + ioV, Color::sCyan, 0.05f);
+
+			// Draw w, the support point
+			DebugRenderer::sInstance->DrawArrow(mOffset, mOffset + w, Color::sGreen, 0.05f);
+			DebugRenderer::sInstance->DrawMarker(mOffset + w, Color::sGreen, 1.0f);
+
+			// Draw the simplex and the Minkowski difference around it
+			DrawState();
+#endif
+
+			// Test if we have a separation of more than inMaxDistSq, in which case we terminate early
+			if (dot < 0.0f && dot * dot > v_len_sq * inMaxDistSq)
+			{
+#ifdef JPH_GJK_DEBUG
+				Trace("Distance bigger than max");
+#endif
+#ifdef JPH_DEBUG
+				memset(&outPointA, 0xcd, sizeof(outPointA));
+				memset(&outPointB, 0xcd, sizeof(outPointB));
+#endif
+				return FLT_MAX;
+			}
+
+			// Store the point for later use
+			mY[mNumPoints] = w;
+			mP[mNumPoints] = p;
+			mQ[mNumPoints] = q;
+			++mNumPoints;
+
+#ifdef JPH_GJK_DEBUG
+			Trace("w = [%s]", ConvertToString(w).c_str());
+#endif
+
+			uint32 set;
+			if (!GetClosest<true>(prev_v_len_sq, ioV, v_len_sq, set))
+			{
+				--mNumPoints; // Undo add last point
+				break;
+			}
+
+			// If there are 4 points, the origin is inside the tetrahedron and we're done
+			if (set == 0xf)
+			{
+#ifdef JPH_GJK_DEBUG
+				Trace("Full simplex");
+#endif
+				ioV = Vec3::sZero();
+				v_len_sq = 0.0f;
+				break;
+			}
+
+			// Update the points of the simplex
+			UpdatePointSetYPQ(set);
+
+			// If v is very close to zero, we consider this a collision
+			if (v_len_sq <= tolerance_sq)
+			{
+#ifdef JPH_GJK_DEBUG
+				Trace("Distance zero");
+#endif
+				ioV = Vec3::sZero();
+				v_len_sq = 0.0f;
+				break;
+			}
+
+			// If v is very small compared to the length of y, we also consider this a collision
+#ifdef JPH_GJK_DEBUG
+			Trace("Check v small compared to y: %g <= %g", (double)v_len_sq, (double)(FLT_EPSILON * GetMaxYLengthSq()));
+#endif
+			if (v_len_sq <= FLT_EPSILON * GetMaxYLengthSq())
+			{
+#ifdef JPH_GJK_DEBUG
+				Trace("Machine precision reached");
+#endif
+				ioV = Vec3::sZero();
+				v_len_sq = 0.0f;
+				break;
+			}
+
+			// The next separation axis to test is the negative of the closest point of the Minkowski sum to the origin
+			// Note: This must be done before terminating as converged since the separating axis is -v
+			ioV = -ioV;
+
+			// If the squared length of v is not changing enough, we've converged and there is no collision
+#ifdef JPH_GJK_DEBUG
+			Trace("Check v not changing enough: %g <= %g", (double)(prev_v_len_sq - v_len_sq), (double)(FLT_EPSILON * prev_v_len_sq));
+#endif
+			JPH_ASSERT(prev_v_len_sq >= v_len_sq);
+			if (prev_v_len_sq - v_len_sq <= FLT_EPSILON * prev_v_len_sq)
+			{
+				// v is a separating axis
+#ifdef JPH_GJK_DEBUG
+				Trace("Converged");
+#endif
+				break;
+			}
+			prev_v_len_sq = v_len_sq;
+		}
+
+		// Get the closest points
+		CalculatePointAAndB(outPointA, outPointB);
+
+#ifdef JPH_GJK_DEBUG
+		Trace("Return: v = [%s], |v| = %g", ConvertToString(ioV).c_str(), (double)ioV.Length());
+
+		// Draw -ioV to show the closest point to the origin from the previous simplex
+		DebugRenderer::sInstance->DrawArrow(mOffset, mOffset - ioV, Color::sOrange, 0.05f);
+
+		// Draw the closest points
+		DebugRenderer::sInstance->DrawMarker(mOffset + outPointA, Color::sGreen, 1.0f);
+		DebugRenderer::sInstance->DrawMarker(mOffset + outPointB, Color::sPurple, 1.0f);
+
+		// Draw the simplex and the Minkowski difference around it
+		DrawState();
+#endif
+
+		JPH_ASSERT(ioV.LengthSq() == v_len_sq);
+		return v_len_sq;
+	}
+
+	/// Get the resulting simplex after the GetClosestPoints algorithm finishes.
+	/// If it returned a squared distance of 0, the origin will be contained in the simplex.
+	void		GetClosestPointsSimplex(Vec3 *outY, Vec3 *outP, Vec3 *outQ, uint &outNumPoints) const
+	{
+		uint size = sizeof(Vec3) * mNumPoints;
+		memcpy(outY, mY, size);
+		memcpy(outP, mP, size);
+		memcpy(outQ, mQ, size);
+		outNumPoints = mNumPoints;
+	}
+
+	/// Test if a ray inRayOrigin + lambda * inRayDirection for lambda e [0, ioLambda> intersects inA
+	///
+	/// Code based upon: Ray Casting against General Convex Objects with Application to Continuous Collision Detection - Gino van den Bergen
+	///
+	/// @param inRayOrigin Origin of the ray
+	/// @param inRayDirection Direction of the ray (ioLambda * inDirection determines length)
+	///	@param inTolerance The minimal distance between the ray and A before it is considered colliding
+	/// @param inA A convex object that has the GetSupport(Vec3) function
+	/// @param ioLambda The max fraction along the ray, on output updated with the actual collision fraction.
+	///
+	///	@return true if a hit was found, ioLambda is the solution for lambda.
+	template <typename A>
+	bool		CastRay(Vec3Arg inRayOrigin, Vec3Arg inRayDirection, float inTolerance, const A &inA, float &ioLambda)
+	{
+		float tolerance_sq = Square(inTolerance);
+
+		// Reset state
+		mNumPoints = 0;
+
+		float lambda = 0.0f;
+		Vec3 x = inRayOrigin;
+		Vec3 v = x - inA.GetSupport(Vec3::sZero());
+		float v_len_sq = FLT_MAX;
+		bool allow_restart = false;
+
+		for (;;)
+		{
+#ifdef JPH_GJK_DEBUG
+			Trace("v = [%s], num_points = %d", ConvertToString(v).c_str(), mNumPoints);
+#endif
+
+			// Get new support point
+			Vec3 p = inA.GetSupport(v);
+			Vec3 w = x - p;
+
+#ifdef JPH_GJK_DEBUG
+			Trace("w = [%s]", ConvertToString(w).c_str());
+#endif
+
+			float v_dot_w = v.Dot(w);
+#ifdef JPH_GJK_DEBUG
+			Trace("v . w = %g", (double)v_dot_w);
+#endif
+			if (v_dot_w > 0.0f)
+			{
+				// If ray and normal are in the same direction, we've passed A and there's no collision
+				float v_dot_r = v.Dot(inRayDirection);
+#ifdef JPH_GJK_DEBUG
+				Trace("v . r = %g", (double)v_dot_r);
+#endif
+				if (v_dot_r >= 0.0f)
+					return false;
+
+				// Update the lower bound for lambda
+				float delta = v_dot_w / v_dot_r;
+				float old_lambda = lambda;
+				lambda -= delta;
+#ifdef JPH_GJK_DEBUG
+				Trace("lambda = %g, delta = %g", (double)lambda, (double)delta);
+#endif
+
+				// If lambda didn't change, we cannot converge any further and we assume a hit
+				if (old_lambda == lambda)
+					break;
+
+				// If lambda is bigger or equal than max, we don't have a hit
+				if (lambda >= ioLambda)
+					return false;
+
+				// Update x to new closest point on the ray
+				x = inRayOrigin + lambda * inRayDirection;
+
+				// We've shifted x, so reset v_len_sq so that it is not used as early out for GetClosest
+				v_len_sq = FLT_MAX;
+
+				// We allow rebuilding the simplex once after x changes because the simplex was built
+				// for another x and numerical round off builds up as you keep adding points to an
+				// existing simplex
+				allow_restart = true;
+			}
+
+			// Add p to set P: P = P U {p}
+			mP[mNumPoints] = p;
+			++mNumPoints;
+
+			// Calculate Y = {x} - P
+			for (int i = 0; i < mNumPoints; ++i)
+				mY[i] = x - mP[i];
+
+			// Determine the new closest point from Y to origin
+			uint32 set;						// Set of points that form the new simplex
+			if (!GetClosest<false>(v_len_sq, v, v_len_sq, set))
+			{
+#ifdef JPH_GJK_DEBUG
+				Trace("Failed to converge");
+#endif
+
+				// Only allow 1 restart, if we still can't get a closest point
+				// we're so close that we return this as a hit
+				if (!allow_restart)
+					break;
+
+				// If we fail to converge, we start again with the last point as simplex
+#ifdef JPH_GJK_DEBUG
+				Trace("Restarting");
+#endif
+				allow_restart = false;
+				mP[0] = p;
+				mNumPoints = 1;
+				v = x - p;
+				v_len_sq = FLT_MAX;
+				continue;
+			}
+			else if (set == 0xf)
+			{
+#ifdef JPH_GJK_DEBUG
+				Trace("Full simplex");
+#endif
+
+				// We're inside the tetrahedron, we have a hit (verify that length of v is 0)
+				JPH_ASSERT(v_len_sq == 0.0f);
+				break;
+			}
+
+			// Update the points P to form the new simplex
+			// Note: We're not updating Y as Y will shift with x so we have to calculate it every iteration
+			UpdatePointSetP(set);
+
+			// Check if x is close enough to inA
+			if (v_len_sq <= tolerance_sq)
+			{
+#ifdef JPH_GJK_DEBUG
+				Trace("Converged");
+#endif
+				break;
+			}
+		}
+
+		// Store hit fraction
+		ioLambda = lambda;
+		return true;
+	}
+
+	/// Test if a cast shape inA moving from inStart to lambda * inStart.GetTranslation() + inDirection where lambda e [0, ioLambda> intersects inB
+	///
+	/// @param inStart Start position and orientation of the convex object
+	/// @param inDirection Direction of the sweep (ioLambda * inDirection determines length)
+	///	@param inTolerance The minimal distance between A and B before they are considered colliding
+	/// @param inA The convex object A, must support the GetSupport(Vec3) function.
+	/// @param inB The convex object B, must support the GetSupport(Vec3) function.
+	/// @param ioLambda The max fraction along the sweep, on output updated with the actual collision fraction.
+	///
+	/// @return true if a hit was found, ioLambda is the solution for lambda.
+	template <typename A, typename B>
+	bool		CastShape(Mat44Arg inStart, Vec3Arg inDirection, float inTolerance, const A &inA, const B &inB, float &ioLambda)
+	{
+		// Transform the shape to be cast to the starting position
+		TransformedConvexObject transformed_a(inStart, inA);
+
+		// Calculate the minkowski difference inB - inA
+		// inA is moving, so we need to add the back side of inB to the front side of inA
+		MinkowskiDifference difference(inB, transformed_a);
+
+		// Do a raycast against the Minkowski difference
+		return CastRay(Vec3::sZero(), inDirection, inTolerance, difference, ioLambda);
+	}
+
+	/// Test if a cast shape inA moving from inStart to lambda * inStart.GetTranslation() + inDirection where lambda e [0, ioLambda> intersects inB
+	///
+	/// @param inStart Start position and orientation of the convex object
+	/// @param inDirection Direction of the sweep (ioLambda * inDirection determines length)
+	///	@param inTolerance The minimal distance between A and B before they are considered colliding
+	/// @param inA The convex object A, must support the GetSupport(Vec3) function.
+	/// @param inB The convex object B, must support the GetSupport(Vec3) function.
+	/// @param inConvexRadiusA The convex radius of A, this will be added on all sides to pad A.
+	/// @param inConvexRadiusB The convex radius of B, this will be added on all sides to pad B.
+	/// @param ioLambda The max fraction along the sweep, on output updated with the actual collision fraction.
+	///	@param outPointA is the contact point on A (if outSeparatingAxis is near zero, this may not be not the deepest point)
+	///	@param outPointB is the contact point on B (if outSeparatingAxis is near zero, this may not be not the deepest point)
+	/// @param outSeparatingAxis On return this will contain a vector that points from A to B along the smallest distance of separation.
+	/// The length of this vector indicates the separation of A and B without their convex radius.
+	/// If it is near zero, the direction may not be accurate as the bodies may overlap when lambda = 0.
+	///
+	///	@return true if a hit was found, ioLambda is the solution for lambda and outPoint and outSeparatingAxis are valid.
+	template <typename A, typename B>
+	bool		CastShape(Mat44Arg inStart, Vec3Arg inDirection, float inTolerance, const A &inA, const B &inB, float inConvexRadiusA, float inConvexRadiusB, float &ioLambda, Vec3 &outPointA, Vec3 &outPointB, Vec3 &outSeparatingAxis)
+	{
+		float tolerance_sq = Square(inTolerance);
+
+		// Calculate how close A and B (without their convex radius) need to be to each other in order for us to consider this a collision
+		float sum_convex_radius = inConvexRadiusA + inConvexRadiusB;
+
+		// Transform the shape to be cast to the starting position
+		TransformedConvexObject transformed_a(inStart, inA);
+
+		// Reset state
+		mNumPoints = 0;
+
+		float lambda = 0.0f;
+		Vec3 x = Vec3::sZero(); // Since A is already transformed we can start the cast from zero
+		Vec3 v = -inB.GetSupport(Vec3::sZero()) + transformed_a.GetSupport(Vec3::sZero()); // See CastRay: v = x - inA.GetSupport(Vec3::sZero()) where inA is the Minkowski difference inB - transformed_a (see CastShape above) and x is zero
+		float v_len_sq = FLT_MAX;
+		bool allow_restart = false;
+
+		// Keeps track of separating axis of the previous iteration.
+		// Initialized at zero as we don't know if our first v is actually a separating axis.
+		Vec3 prev_v = Vec3::sZero();
+
+		for (;;)
+		{
+#ifdef JPH_GJK_DEBUG
+			Trace("v = [%s], num_points = %d", ConvertToString(v).c_str(), mNumPoints);
+#endif
+
+			// Calculate the minkowski difference inB - inA
+			// inA is moving, so we need to add the back side of inB to the front side of inA
+			// Keep the support points on A and B separate so that in the end we can calculate a contact point
+			Vec3 p = transformed_a.GetSupport(-v);
+			Vec3 q = inB.GetSupport(v);
+			Vec3 w = x - (q - p);
+
+#ifdef JPH_GJK_DEBUG
+			Trace("w = [%s]", ConvertToString(w).c_str());
+#endif
+
+			// Difference from article to this code:
+			// We did not include the convex radius in p and q in order to be able to calculate a good separating axis at the end of the algorithm.
+			// However when moving forward along inDirection we do need to take this into account so that we keep A and B separated by the sum of their convex radii.
+			// From p we have to subtract: inConvexRadiusA * v / |v|
+			// To q we have to add: inConvexRadiusB * v / |v|
+			// This means that to w we have to add: -(inConvexRadiusA + inConvexRadiusB) * v / |v|
+			// So to v . w we have to add: v . (-(inConvexRadiusA + inConvexRadiusB) * v / |v|) = -(inConvexRadiusA + inConvexRadiusB) * |v|
+			float v_dot_w = v.Dot(w) - sum_convex_radius * v.Length();
+#ifdef JPH_GJK_DEBUG
+			Trace("v . w = %g", (double)v_dot_w);
+#endif
+			if (v_dot_w > 0.0f)
+			{
+				// If ray and normal are in the same direction, we've passed A and there's no collision
+				float v_dot_r = v.Dot(inDirection);
+#ifdef JPH_GJK_DEBUG
+				Trace("v . r = %g", (double)v_dot_r);
+#endif
+				if (v_dot_r >= 0.0f)
+					return false;
+
+				// Update the lower bound for lambda
+				float delta = v_dot_w / v_dot_r;
+				float old_lambda = lambda;
+				lambda -= delta;
+#ifdef JPH_GJK_DEBUG
+				Trace("lambda = %g, delta = %g", (double)lambda, (double)delta);
+#endif
+
+				// If lambda didn't change, we cannot converge any further and we assume a hit
+				if (old_lambda == lambda)
+					break;
+
+				// If lambda is bigger or equal than max, we don't have a hit
+				if (lambda >= ioLambda)
+					return false;
+
+				// Update x to new closest point on the ray
+				x = lambda * inDirection;
+
+				// We've shifted x, so reset v_len_sq so that it is not used as early out when GetClosest returns false
+				v_len_sq = FLT_MAX;
+
+				// Now that we've moved, we know that A and B are not intersecting at lambda = 0, so we can update our tolerance to stop iterating
+				// as soon as A and B are inConvexRadiusA + inConvexRadiusB apart
+				tolerance_sq = Square(inTolerance + sum_convex_radius);
+
+				// We allow rebuilding the simplex once after x changes because the simplex was built
+				// for another x and numerical round off builds up as you keep adding points to an
+				// existing simplex
+				allow_restart = true;
+			}
+
+			// Add p to set P, q to set Q: P = P U {p}, Q = Q U {q}
+			mP[mNumPoints] = p;
+			mQ[mNumPoints] = q;
+			++mNumPoints;
+
+			// Calculate Y = {x} - (Q - P)
+			for (int i = 0; i < mNumPoints; ++i)
+				mY[i] = x - (mQ[i] - mP[i]);
+
+			// Determine the new closest point from Y to origin
+			uint32 set;						// Set of points that form the new simplex
+			if (!GetClosest<false>(v_len_sq, v, v_len_sq, set))
+			{
+#ifdef JPH_GJK_DEBUG
+				Trace("Failed to converge");
+#endif
+
+				// Only allow 1 restart, if we still can't get a closest point
+				// we're so close that we return this as a hit
+				if (!allow_restart)
+					break;
+
+				// If we fail to converge, we start again with the last point as simplex
+#ifdef JPH_GJK_DEBUG
+				Trace("Restarting");
+#endif
+				allow_restart = false;
+				mP[0] = p;
+				mQ[0] = q;
+				mNumPoints = 1;
+				v = x - q;
+				v_len_sq = FLT_MAX;
+				continue;
+			}
+			else if (set == 0xf)
+			{
+#ifdef JPH_GJK_DEBUG
+				Trace("Full simplex");
+#endif
+
+				// We're inside the tetrahedron, we have a hit (verify that length of v is 0)
+				JPH_ASSERT(v_len_sq == 0.0f);
+				break;
+			}
+
+			// Update the points P and Q to form the new simplex
+			// Note: We're not updating Y as Y will shift with x so we have to calculate it every iteration
+			UpdatePointSetPQ(set);
+
+			// Check if A and B are touching according to our tolerance
+			if (v_len_sq <= tolerance_sq)
+			{
+#ifdef JPH_GJK_DEBUG
+				Trace("Converged");
+#endif
+				break;
+			}
+
+			// Store our v to return as separating axis
+			prev_v = v;
+		}
+
+		// Calculate Y = {x} - (Q - P) again so we can calculate the contact points
+		for (int i = 0; i < mNumPoints; ++i)
+			mY[i] = x - (mQ[i] - mP[i]);
+
+		// Calculate the offset we need to apply to A and B to correct for the convex radius
+		Vec3 normalized_v = v.NormalizedOr(Vec3::sZero());
+		Vec3 convex_radius_a = inConvexRadiusA * normalized_v;
+		Vec3 convex_radius_b = inConvexRadiusB * normalized_v;
+
+		// Get the contact point
+		// Note that A and B will coincide when lambda > 0. In this case we calculate only B as it is more accurate as it contains less terms.
+		switch (mNumPoints)
+		{
+		case 1:
+			outPointB = mQ[0] + convex_radius_b;
+			outPointA = lambda > 0.0f? outPointB : mP[0] - convex_radius_a;
+			break;
+
+		case 2:
+			{
+				float bu, bv;
+				ClosestPoint::GetBaryCentricCoordinates(mY[0], mY[1], bu, bv);
+				outPointB = bu * mQ[0] + bv * mQ[1] + convex_radius_b;
+				outPointA = lambda > 0.0f? outPointB : bu * mP[0] + bv * mP[1] - convex_radius_a;
+			}
+			break;
+
+		case 3:
+		case 4: // A full simplex, we can't properly determine a contact point! As contact point we take the closest point of the previous iteration.
+			{
+				float bu, bv, bw;
+				ClosestPoint::GetBaryCentricCoordinates(mY[0], mY[1], mY[2], bu, bv, bw);
+				outPointB = bu * mQ[0] + bv * mQ[1] + bw * mQ[2] + convex_radius_b;
+				outPointA = lambda > 0.0f? outPointB : bu * mP[0] + bv * mP[1] + bw * mP[2] - convex_radius_a;
+			}
+			break;
+		}
+
+		// Store separating axis, in case we have a convex radius we can just return v,
+		// otherwise v will be very small and we resort to returning previous v as an approximation.
+		outSeparatingAxis = sum_convex_radius > 0.0f? -v : -prev_v;
+
+		// Store hit fraction
+		ioLambda = lambda;
+		return true;
+	}
+
+private:
+#ifdef JPH_GJK_DEBUG
+	/// Draw state of algorithm
+	void		DrawState()
+	{
+		RMat44 origin = RMat44::sTranslation(mOffset);
+
+		// Draw origin
+		DebugRenderer::sInstance->DrawCoordinateSystem(origin, 1.0f);
+
+		// Draw the hull
+		DebugRenderer::sInstance->DrawGeometry(origin, mGeometry->mBounds.Transformed(origin), mGeometry->mBounds.GetExtent().LengthSq(), Color::sYellow, mGeometry);
+
+		// Draw Y
+		for (int i = 0; i < mNumPoints; ++i)
+		{
+			// Draw support point
+			RVec3 y_i = origin * mY[i];
+			DebugRenderer::sInstance->DrawMarker(y_i, Color::sRed, 1.0f);
+			for (int j = i + 1; j < mNumPoints; ++j)
+			{
+				// Draw edge
+				RVec3 y_j = origin * mY[j];
+				DebugRenderer::sInstance->DrawLine(y_i, y_j, Color::sRed);
+				for (int k = j + 1; k < mNumPoints; ++k)
+				{
+					// Make sure triangle faces the origin
+					RVec3 y_k = origin * mY[k];
+					RVec3 center = (y_i + y_j + y_k) / Real(3);
+					RVec3 normal = (y_j - y_i).Cross(y_k - y_i);
+					if (normal.Dot(center) < Real(0))
+						DebugRenderer::sInstance->DrawTriangle(y_i, y_j, y_k, Color::sLightGrey);
+					else
+						DebugRenderer::sInstance->DrawTriangle(y_i, y_k, y_j, Color::sLightGrey);
+				}
+			}
+		}
+
+		// Offset to the right
+		mOffset += Vec3(mGeometry->mBounds.GetSize().GetX() + 2.0f, 0, 0);
+	}
+#endif // JPH_GJK_DEBUG
+
+	Vec3		mY[4];						///< Support points on A - B
+	Vec3		mP[4];						///< Support point on A
+	Vec3		mQ[4];						///< Support point on B
+	int			mNumPoints = 0;				///< Number of points in mY, mP and mQ that are valid
+
+#ifdef JPH_GJK_DEBUG
+	DebugRenderer::GeometryRef	mGeometry;	///< A visualization of the minkowski difference for state drawing
+	RVec3		mOffset = RVec3::sZero();	///< Offset to use for state drawing
+#endif
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Geometry/IndexedTriangle.h

@@ -0,0 +1,130 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/HashCombine.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Triangle with 32-bit indices
+class IndexedTriangleNoMaterial
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+					IndexedTriangleNoMaterial() = default;
+	constexpr		IndexedTriangleNoMaterial(uint32 inI1, uint32 inI2, uint32 inI3) : mIdx { inI1, inI2, inI3 } { }
+
+	/// Check if two triangles are identical
+	bool			operator == (const IndexedTriangleNoMaterial &inRHS) const
+	{
+		return mIdx[0] == inRHS.mIdx[0] && mIdx[1] == inRHS.mIdx[1] && mIdx[2] == inRHS.mIdx[2];
+	}
+
+	/// Check if two triangles are equivalent (using the same vertices)
+	bool			IsEquivalent(const IndexedTriangleNoMaterial &inRHS) const
+	{
+		return (mIdx[0] == inRHS.mIdx[0] && mIdx[1] == inRHS.mIdx[1] && mIdx[2] == inRHS.mIdx[2])
+			|| (mIdx[0] == inRHS.mIdx[1] && mIdx[1] == inRHS.mIdx[2] && mIdx[2] == inRHS.mIdx[0])
+			|| (mIdx[0] == inRHS.mIdx[2] && mIdx[1] == inRHS.mIdx[0] && mIdx[2] == inRHS.mIdx[1]);
+	}
+
+	/// Check if two triangles are opposite (using the same vertices but in opposing order)
+	bool			IsOpposite(const IndexedTriangleNoMaterial &inRHS) const
+	{
+		return (mIdx[0] == inRHS.mIdx[0] && mIdx[1] == inRHS.mIdx[2] && mIdx[2] == inRHS.mIdx[1])
+			|| (mIdx[0] == inRHS.mIdx[1] && mIdx[1] == inRHS.mIdx[0] && mIdx[2] == inRHS.mIdx[2])
+			|| (mIdx[0] == inRHS.mIdx[2] && mIdx[1] == inRHS.mIdx[1] && mIdx[2] == inRHS.mIdx[0]);
+	}
+
+	/// Check if triangle is degenerate
+	bool			IsDegenerate(const VertexList &inVertices) const
+	{
+		Vec3 v0(inVertices[mIdx[0]]);
+		Vec3 v1(inVertices[mIdx[1]]);
+		Vec3 v2(inVertices[mIdx[2]]);
+
+		return (v1 - v0).Cross(v2 - v0).IsNearZero();
+	}
+
+	/// Rotate the vertices so that the second vertex becomes first etc. This does not change the represented triangle.
+	void			Rotate()
+	{
+		uint32 tmp = mIdx[0];
+		mIdx[0] = mIdx[1];
+		mIdx[1] = mIdx[2];
+		mIdx[2] = tmp;
+	}
+
+	/// Get center of triangle
+	Vec3			GetCentroid(const VertexList &inVertices) const
+	{
+		return (Vec3(inVertices[mIdx[0]]) + Vec3(inVertices[mIdx[1]]) + Vec3(inVertices[mIdx[2]])) / 3.0f;
+	}
+
+	/// Get the hash value of this structure
+	uint64			GetHash() const
+	{
+		static_assert(sizeof(IndexedTriangleNoMaterial) == 3 * sizeof(uint32), "Class should have no padding");
+		return HashBytes(this, sizeof(IndexedTriangleNoMaterial));
+	}
+
+	uint32			mIdx[3];
+};
+
+/// Triangle with 32-bit indices and material index
+class IndexedTriangle : public IndexedTriangleNoMaterial
+{
+public:
+	using IndexedTriangleNoMaterial::IndexedTriangleNoMaterial;
+
+	/// Constructor
+	constexpr		IndexedTriangle(uint32 inI1, uint32 inI2, uint32 inI3, uint32 inMaterialIndex, uint inUserData = 0) : IndexedTriangleNoMaterial(inI1, inI2, inI3), mMaterialIndex(inMaterialIndex), mUserData(inUserData) { }
+
+	/// Check if two triangles are identical
+	bool			operator == (const IndexedTriangle &inRHS) const
+	{
+		return mMaterialIndex == inRHS.mMaterialIndex && mUserData == inRHS.mUserData && IndexedTriangleNoMaterial::operator==(inRHS);
+	}
+
+	/// Rotate the vertices so that the lowest vertex becomes the first. This does not change the represented triangle.
+	IndexedTriangle	GetLowestIndexFirst() const
+	{
+		if (mIdx[0] < mIdx[1])
+		{
+			if (mIdx[0] < mIdx[2])
+				return IndexedTriangle(mIdx[0], mIdx[1], mIdx[2], mMaterialIndex, mUserData); // 0 is smallest
+			else
+				return IndexedTriangle(mIdx[2], mIdx[0], mIdx[1], mMaterialIndex, mUserData); // 2 is smallest
+		}
+		else
+		{
+			if (mIdx[1] < mIdx[2])
+				return IndexedTriangle(mIdx[1], mIdx[2], mIdx[0], mMaterialIndex, mUserData); // 1 is smallest
+			else
+				return IndexedTriangle(mIdx[2], mIdx[0], mIdx[1], mMaterialIndex, mUserData); // 2 is smallest
+		}
+	}
+
+	/// Get the hash value of this structure
+	uint64			GetHash() const
+	{
+		static_assert(sizeof(IndexedTriangle) == 5 * sizeof(uint32), "Class should have no padding");
+		return HashBytes(this, sizeof(IndexedTriangle));
+	}
+
+	uint32			mMaterialIndex = 0;
+	uint32			mUserData = 0;				///< User data that can be used for anything by the application, e.g. for tracking the original index of the triangle
+};
+
+using IndexedTriangleNoMaterialList = Array<IndexedTriangleNoMaterial>;
+using IndexedTriangleList = Array<IndexedTriangle>;
+
+JPH_NAMESPACE_END
+
+// Create a std::hash for IndexedTriangleNoMaterial and IndexedTriangle
+JPH_MAKE_STD_HASH(JPH::IndexedTriangleNoMaterial)
+JPH_MAKE_STD_HASH(JPH::IndexedTriangle)

thirdparty/jolt_physics/Jolt/Geometry/Indexify.cpp

@@ -0,0 +1,222 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Geometry/Indexify.h>
+#include <Jolt/Geometry/AABox.h>
+
+JPH_NAMESPACE_BEGIN
+
+static JPH_INLINE const Float3 &sIndexifyGetFloat3(const TriangleList &inTriangles, uint32 inVertexIndex)
+{
+	return inTriangles[inVertexIndex / 3].mV[inVertexIndex % 3];
+}
+
+static JPH_INLINE Vec3 sIndexifyGetVec3(const TriangleList &inTriangles, uint32 inVertexIndex)
+{
+	return Vec3::sLoadFloat3Unsafe(sIndexifyGetFloat3(inTriangles, inVertexIndex));
+}
+
+static void sIndexifyVerticesBruteForce(const TriangleList &inTriangles, const uint32 *inVertexIndices, const uint32 *inVertexIndicesEnd, Array<uint32> &ioWeldedVertices, float inVertexWeldDistance)
+{
+	float weld_dist_sq = Square(inVertexWeldDistance);
+
+	// Compare every vertex
+	for (const uint32 *v1_idx = inVertexIndices; v1_idx < inVertexIndicesEnd; ++v1_idx)
+	{
+		Vec3 v1 = sIndexifyGetVec3(inTriangles, *v1_idx);
+
+		// with every other vertex...
+		for (const uint32 *v2_idx = v1_idx + 1; v2_idx < inVertexIndicesEnd; ++v2_idx)
+		{
+			Vec3 v2 = sIndexifyGetVec3(inTriangles, *v2_idx);
+
+			// If they're weldable
+			if ((v2 - v1).LengthSq() <= weld_dist_sq)
+			{
+				// Find the lowest indices both indices link to
+				uint32 idx1 = *v1_idx;
+				for (;;)
+				{
+					uint32 new_idx1 = ioWeldedVertices[idx1];
+					if (new_idx1 >= idx1)
+						break;
+					idx1 = new_idx1;
+				}
+				uint32 idx2 = *v2_idx;
+				for (;;)
+				{
+					uint32 new_idx2 = ioWeldedVertices[idx2];
+					if (new_idx2 >= idx2)
+						break;
+					idx2 = new_idx2;
+				}
+
+				// Order the vertices
+				uint32 lowest = min(idx1, idx2);
+				uint32 highest = max(idx1, idx2);
+
+				// Link highest to lowest
+				ioWeldedVertices[highest] = lowest;
+
+				// Also update the vertices we started from to avoid creating long chains
+				ioWeldedVertices[*v1_idx] = lowest;
+				ioWeldedVertices[*v2_idx] = lowest;
+				break;
+			}
+		}
+	}
+}
+
+static void sIndexifyVerticesRecursively(const TriangleList &inTriangles, uint32 *ioVertexIndices, uint inNumVertices, uint32 *ioScratch, Array<uint32> &ioWeldedVertices, float inVertexWeldDistance, uint inMaxRecursion)
+{
+	// Check if we have few enough vertices to do a brute force search
+	// Or if we've recursed too deep (this means we chipped off a few vertices each iteration because all points are very close)
+	if (inNumVertices <= 8 || inMaxRecursion == 0)
+	{
+		sIndexifyVerticesBruteForce(inTriangles, ioVertexIndices, ioVertexIndices + inNumVertices, ioWeldedVertices, inVertexWeldDistance);
+		return;
+	}
+
+	// Calculate bounds
+	AABox bounds;
+	for (const uint32 *v = ioVertexIndices, *v_end = ioVertexIndices + inNumVertices; v < v_end; ++v)
+		bounds.Encapsulate(sIndexifyGetVec3(inTriangles, *v));
+
+	// Determine split plane
+	int split_axis = bounds.GetExtent().GetHighestComponentIndex();
+	float split_value = bounds.GetCenter()[split_axis];
+
+	// Partition vertices
+	uint32 *v_read = ioVertexIndices, *v_write = ioVertexIndices, *v_end = ioVertexIndices + inNumVertices;
+	uint32 *scratch = ioScratch;
+	while (v_read < v_end)
+	{
+		// Calculate distance to plane
+		float distance_to_split_plane = sIndexifyGetFloat3(inTriangles, *v_read)[split_axis] - split_value;
+		if (distance_to_split_plane < -inVertexWeldDistance)
+		{
+			// Vertex is on the right side
+			*v_write = *v_read;
+			++v_read;
+			++v_write;
+		}
+		else if (distance_to_split_plane > inVertexWeldDistance)
+		{
+			// Vertex is on the wrong side, swap with the last vertex
+			--v_end;
+			std::swap(*v_read, *v_end);
+		}
+		else
+		{
+			// Vertex is too close to the split plane, it goes on both sides
+			*scratch++ = *v_read++;
+		}
+	}
+
+	// Check if we made any progress
+	uint num_vertices_on_both_sides = (uint)(scratch - ioScratch);
+	if (num_vertices_on_both_sides == inNumVertices)
+	{
+		sIndexifyVerticesBruteForce(inTriangles, ioVertexIndices, ioVertexIndices + inNumVertices, ioWeldedVertices, inVertexWeldDistance);
+		return;
+	}
+
+	// Calculate how we classified the vertices
+	uint num_vertices_left = (uint)(v_write - ioVertexIndices);
+	uint num_vertices_right = (uint)(ioVertexIndices + inNumVertices - v_end);
+	JPH_ASSERT(num_vertices_left + num_vertices_right + num_vertices_on_both_sides == inNumVertices);
+	memcpy(v_write, ioScratch, num_vertices_on_both_sides * sizeof(uint32));
+
+	// Recurse
+	uint max_recursion = inMaxRecursion - 1;
+	sIndexifyVerticesRecursively(inTriangles, ioVertexIndices, num_vertices_left + num_vertices_on_both_sides, ioScratch, ioWeldedVertices, inVertexWeldDistance, max_recursion);
+	sIndexifyVerticesRecursively(inTriangles, ioVertexIndices + num_vertices_left, num_vertices_right + num_vertices_on_both_sides, ioScratch, ioWeldedVertices, inVertexWeldDistance, max_recursion);
+}
+
+void Indexify(const TriangleList &inTriangles, VertexList &outVertices, IndexedTriangleList &outTriangles, float inVertexWeldDistance)
+{
+	uint num_triangles = (uint)inTriangles.size();
+	uint num_vertices = num_triangles * 3;
+
+	// Create a list of all vertex indices
+	Array<uint32> vertex_indices;
+	vertex_indices.resize(num_vertices);
+	for (uint i = 0; i < num_vertices; ++i)
+		vertex_indices[i] = i;
+
+	// Link each vertex to itself
+	Array<uint32> welded_vertices;
+	welded_vertices.resize(num_vertices);
+	for (uint i = 0; i < num_vertices; ++i)
+		welded_vertices[i] = i;
+
+	// A scope to free memory used by the scratch array
+	{
+		// Some scratch memory, used for the vertices that fall in both partitions
+		Array<uint32> scratch;
+		scratch.resize(num_vertices);
+
+		// Recursively split the vertices
+		sIndexifyVerticesRecursively(inTriangles, vertex_indices.data(), num_vertices, scratch.data(), welded_vertices, inVertexWeldDistance, 32);
+	}
+
+	// Do a pass to complete the welding, linking each vertex to the vertex it is welded to
+	// (and since we're going from 0 to N we can be sure that the vertex we're linking to is already linked to the lowest vertex)
+	uint num_resulting_vertices = 0;
+	for (uint i = 0; i < num_vertices; ++i)
+	{
+		JPH_ASSERT(welded_vertices[welded_vertices[i]] <= welded_vertices[i]);
+		welded_vertices[i] = welded_vertices[welded_vertices[i]];
+		if (welded_vertices[i] == i)
+			++num_resulting_vertices;
+	}
+
+	// Collect the vertices
+	outVertices.clear();
+	outVertices.reserve(num_resulting_vertices);
+	for (uint i = 0; i < num_vertices; ++i)
+		if (welded_vertices[i] == i)
+		{
+			// New vertex
+			welded_vertices[i] = (uint32)outVertices.size();
+			outVertices.push_back(sIndexifyGetFloat3(inTriangles, i));
+		}
+		else
+		{
+			// Reused vertex, remap index
+			welded_vertices[i] = welded_vertices[welded_vertices[i]];
+		}
+
+	// Create indexed triangles
+	outTriangles.clear();
+	outTriangles.reserve(num_triangles);
+	for (uint t = 0; t < num_triangles; ++t)
+	{
+		IndexedTriangle it;
+		it.mMaterialIndex = inTriangles[t].mMaterialIndex;
+		it.mUserData = inTriangles[t].mUserData;
+		for (int v = 0; v < 3; ++v)
+			it.mIdx[v] = welded_vertices[t * 3 + v];
+		if (!it.IsDegenerate(outVertices))
+			outTriangles.push_back(it);
+	}
+}
+
+void Deindexify(const VertexList &inVertices, const IndexedTriangleList &inTriangles, TriangleList &outTriangles)
+{
+	outTriangles.resize(inTriangles.size());
+	for (size_t t = 0; t < inTriangles.size(); ++t)
+	{
+		const IndexedTriangle &in = inTriangles[t];
+		Triangle &out = outTriangles[t];
+		out.mMaterialIndex = in.mMaterialIndex;
+		out.mUserData = in.mUserData;
+		for (int v = 0; v < 3; ++v)
+			out.mV[v] = inVertices[in.mIdx[v]];
+	}
+}
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Geometry/Indexify.h

@@ -0,0 +1,19 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Geometry/Triangle.h>
+#include <Jolt/Geometry/IndexedTriangle.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Take a list of triangles and get the unique set of vertices and use them to create indexed triangles.
+/// Vertices that are less than inVertexWeldDistance apart will be combined to a single vertex.
+JPH_EXPORT void Indexify(const TriangleList &inTriangles, VertexList &outVertices, IndexedTriangleList &outTriangles, float inVertexWeldDistance = 1.0e-4f);
+
+/// Take a list of indexed triangles and unpack them
+JPH_EXPORT void Deindexify(const VertexList &inVertices, const IndexedTriangleList &inTriangles, TriangleList &outTriangles);
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Geometry/MortonCode.h

@@ -0,0 +1,40 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Geometry/AABox.h>
+
+JPH_NAMESPACE_BEGIN
+
+class MortonCode
+{
+public:
+	/// First converts a floating point value in the range [0, 1] to a 10 bit fixed point integer.
+	/// Then expands a 10-bit integer into 30 bits by inserting 2 zeros after each bit.
+	static uint32 sExpandBits(float inV)
+	{
+		JPH_ASSERT(inV >= 0.0f && inV <= 1.0f);
+		uint32 v = uint32(inV * 1023.0f + 0.5f);
+		JPH_ASSERT(v < 1024);
+		v = (v * 0x00010001u) & 0xFF0000FFu;
+		v = (v * 0x00000101u) & 0x0F00F00Fu;
+		v = (v * 0x00000011u) & 0xC30C30C3u;
+		v = (v * 0x00000005u) & 0x49249249u;
+		return v;
+	}
+
+	/// Calculate the morton code for inVector, given that all vectors lie in inVectorBounds
+	static uint32 sGetMortonCode(Vec3Arg inVector, const AABox &inVectorBounds)
+	{
+		// Convert to 10 bit fixed point
+		Vec3 scaled = (inVector - inVectorBounds.mMin) / inVectorBounds.GetSize();
+		uint x = sExpandBits(scaled.GetX());
+		uint y = sExpandBits(scaled.GetY());
+		uint z = sExpandBits(scaled.GetZ());
+		return (x << 2) + (y << 1) + z;
+	}
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Geometry/OrientedBox.cpp

@@ -0,0 +1,178 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Geometry/OrientedBox.h>
+#include <Jolt/Geometry/AABox.h>
+
+JPH_NAMESPACE_BEGIN
+
+bool OrientedBox::Overlaps(const AABox &inBox, float inEpsilon) const
+{
+	// Taken from: Real Time Collision Detection - Christer Ericson
+	// Chapter 4.4.1, page 103-105.
+	// Note that the code is swapped around: A is the aabox and B is the oriented box (this saves us from having to invert the orientation of the oriented box)
+
+	// Convert AABox to center / extent representation
+	Vec3 a_center = inBox.GetCenter();
+	Vec3 a_half_extents = inBox.GetExtent();
+
+	// Compute rotation matrix expressing b in a's coordinate frame
+	Mat44 rot(mOrientation.GetColumn4(0), mOrientation.GetColumn4(1), mOrientation.GetColumn4(2), mOrientation.GetColumn4(3) - Vec4(a_center, 0));
+
+	// Compute common subexpressions. Add in an epsilon term to
+	// counteract arithmetic errors when two edges are parallel and
+	// their cross product is (near) null (see text for details)
+	Vec3 epsilon = Vec3::sReplicate(inEpsilon);
+	Vec3 abs_r[3] { rot.GetAxisX().Abs() + epsilon, rot.GetAxisY().Abs() + epsilon, rot.GetAxisZ().Abs() + epsilon };
+
+	// Test axes L = A0, L = A1, L = A2
+	float ra, rb;
+	for (int i = 0; i < 3; i++)
+	{
+		ra = a_half_extents[i];
+		rb = mHalfExtents[0] * abs_r[0][i] + mHalfExtents[1] * abs_r[1][i] + mHalfExtents[2] * abs_r[2][i];
+		if (abs(rot(i, 3)) > ra + rb) return false;
+	}
+
+	// Test axes L = B0, L = B1, L = B2
+	for (int i = 0; i < 3; i++)
+	{
+		ra = a_half_extents.Dot(abs_r[i]);
+		rb = mHalfExtents[i];
+		if (abs(rot.GetTranslation().Dot(rot.GetColumn3(i))) > ra + rb) return false;
+	}
+
+	// Test axis L = A0 x B0
+	ra = a_half_extents[1] * abs_r[0][2] + a_half_extents[2] * abs_r[0][1];
+	rb = mHalfExtents[1] * abs_r[2][0] + mHalfExtents[2] * abs_r[1][0];
+	if (abs(rot(2, 3) * rot(1, 0) - rot(1, 3) * rot(2, 0)) > ra + rb) return false;
+
+	// Test axis L = A0 x B1
+	ra = a_half_extents[1] * abs_r[1][2] + a_half_extents[2] * abs_r[1][1];
+	rb = mHalfExtents[0] * abs_r[2][0] + mHalfExtents[2] * abs_r[0][0];
+	if (abs(rot(2, 3) * rot(1, 1) - rot(1, 3) * rot(2, 1)) > ra + rb) return false;
+
+	// Test axis L = A0 x B2
+	ra = a_half_extents[1] * abs_r[2][2] + a_half_extents[2] * abs_r[2][1];
+	rb = mHalfExtents[0] * abs_r[1][0] + mHalfExtents[1] * abs_r[0][0];
+	if (abs(rot(2, 3) * rot(1, 2) - rot(1, 3) * rot(2, 2)) > ra + rb) return false;
+
+	// Test axis L = A1 x B0
+	ra = a_half_extents[0] * abs_r[0][2] + a_half_extents[2] * abs_r[0][0];
+	rb = mHalfExtents[1] * abs_r[2][1] + mHalfExtents[2] * abs_r[1][1];
+	if (abs(rot(0, 3) * rot(2, 0) - rot(2, 3) * rot(0, 0)) > ra + rb) return false;
+
+	// Test axis L = A1 x B1
+	ra = a_half_extents[0] * abs_r[1][2] + a_half_extents[2] * abs_r[1][0];
+	rb = mHalfExtents[0] * abs_r[2][1] + mHalfExtents[2] * abs_r[0][1];
+	if (abs(rot(0, 3) * rot(2, 1) - rot(2, 3) * rot(0, 1)) > ra + rb) return false;
+
+	// Test axis L = A1 x B2
+	ra = a_half_extents[0] * abs_r[2][2] + a_half_extents[2] * abs_r[2][0];
+	rb = mHalfExtents[0] * abs_r[1][1] + mHalfExtents[1] * abs_r[0][1];
+	if (abs(rot(0, 3) * rot(2, 2) - rot(2, 3) * rot(0, 2)) > ra + rb) return false;
+
+	// Test axis L = A2 x B0
+	ra = a_half_extents[0] * abs_r[0][1] + a_half_extents[1] * abs_r[0][0];
+	rb = mHalfExtents[1] * abs_r[2][2] + mHalfExtents[2] * abs_r[1][2];
+	if (abs(rot(1, 3) * rot(0, 0) - rot(0, 3) * rot(1, 0)) > ra + rb) return false;
+
+	// Test axis L = A2 x B1
+	ra = a_half_extents[0] * abs_r[1][1] + a_half_extents[1] * abs_r[1][0];
+	rb = mHalfExtents[0] * abs_r[2][2] + mHalfExtents[2] * abs_r[0][2];
+	if (abs(rot(1, 3) * rot(0, 1) - rot(0, 3) * rot(1, 1)) > ra + rb) return false;
+
+	// Test axis L = A2 x B2
+	ra = a_half_extents[0] * abs_r[2][1] + a_half_extents[1] * abs_r[2][0];
+	rb = mHalfExtents[0] * abs_r[1][2] + mHalfExtents[1] * abs_r[0][2];
+	if (abs(rot(1, 3) * rot(0, 2) - rot(0, 3) * rot(1, 2)) > ra + rb) return false;
+
+	// Since no separating axis is found, the OBB and AAB must be intersecting
+	return true;
+}
+
+bool OrientedBox::Overlaps(const OrientedBox &inBox, float inEpsilon) const
+{
+	// Taken from: Real Time Collision Detection - Christer Ericson
+	// Chapter 4.4.1, page 103-105.
+	// Note that A is this, B is inBox
+
+	// Compute rotation matrix expressing b in a's coordinate frame
+	Mat44 rot = mOrientation.InversedRotationTranslation() * inBox.mOrientation;
+
+	// Compute common subexpressions. Add in an epsilon term to
+	// counteract arithmetic errors when two edges are parallel and
+	// their cross product is (near) null (see text for details)
+	Vec3 epsilon = Vec3::sReplicate(inEpsilon);
+	Vec3 abs_r[3] { rot.GetAxisX().Abs() + epsilon, rot.GetAxisY().Abs() + epsilon, rot.GetAxisZ().Abs() + epsilon };
+
+	// Test axes L = A0, L = A1, L = A2
+	float ra, rb;
+	for (int i = 0; i < 3; i++)
+	{
+		ra = mHalfExtents[i];
+		rb = inBox.mHalfExtents[0] * abs_r[0][i] + inBox.mHalfExtents[1] * abs_r[1][i] + inBox.mHalfExtents[2] * abs_r[2][i];
+		if (abs(rot(i, 3)) > ra + rb) return false;
+	}
+
+	// Test axes L = B0, L = B1, L = B2
+	for (int i = 0; i < 3; i++)
+	{
+		ra = mHalfExtents.Dot(abs_r[i]);
+		rb = inBox.mHalfExtents[i];
+		if (abs(rot.GetTranslation().Dot(rot.GetColumn3(i))) > ra + rb) return false;
+	}
+
+	// Test axis L = A0 x B0
+	ra = mHalfExtents[1] * abs_r[0][2] + mHalfExtents[2] * abs_r[0][1];
+	rb = inBox.mHalfExtents[1] * abs_r[2][0] + inBox.mHalfExtents[2] * abs_r[1][0];
+	if (abs(rot(2, 3) * rot(1, 0) - rot(1, 3) * rot(2, 0)) > ra + rb) return false;
+
+	// Test axis L = A0 x B1
+	ra = mHalfExtents[1] * abs_r[1][2] + mHalfExtents[2] * abs_r[1][1];
+	rb = inBox.mHalfExtents[0] * abs_r[2][0] + inBox.mHalfExtents[2] * abs_r[0][0];
+	if (abs(rot(2, 3) * rot(1, 1) - rot(1, 3) * rot(2, 1)) > ra + rb) return false;
+
+	// Test axis L = A0 x B2
+	ra = mHalfExtents[1] * abs_r[2][2] + mHalfExtents[2] * abs_r[2][1];
+	rb = inBox.mHalfExtents[0] * abs_r[1][0] + inBox.mHalfExtents[1] * abs_r[0][0];
+	if (abs(rot(2, 3) * rot(1, 2) - rot(1, 3) * rot(2, 2)) > ra + rb) return false;
+
+	// Test axis L = A1 x B0
+	ra = mHalfExtents[0] * abs_r[0][2] + mHalfExtents[2] * abs_r[0][0];
+	rb = inBox.mHalfExtents[1] * abs_r[2][1] + inBox.mHalfExtents[2] * abs_r[1][1];
+	if (abs(rot(0, 3) * rot(2, 0) - rot(2, 3) * rot(0, 0)) > ra + rb) return false;
+
+	// Test axis L = A1 x B1
+	ra = mHalfExtents[0] * abs_r[1][2] + mHalfExtents[2] * abs_r[1][0];
+	rb = inBox.mHalfExtents[0] * abs_r[2][1] + inBox.mHalfExtents[2] * abs_r[0][1];
+	if (abs(rot(0, 3) * rot(2, 1) - rot(2, 3) * rot(0, 1)) > ra + rb) return false;
+
+	// Test axis L = A1 x B2
+	ra = mHalfExtents[0] * abs_r[2][2] + mHalfExtents[2] * abs_r[2][0];
+	rb = inBox.mHalfExtents[0] * abs_r[1][1] + inBox.mHalfExtents[1] * abs_r[0][1];
+	if (abs(rot(0, 3) * rot(2, 2) - rot(2, 3) * rot(0, 2)) > ra + rb) return false;
+
+	// Test axis L = A2 x B0
+	ra = mHalfExtents[0] * abs_r[0][1] + mHalfExtents[1] * abs_r[0][0];
+	rb = inBox.mHalfExtents[1] * abs_r[2][2] + inBox.mHalfExtents[2] * abs_r[1][2];
+	if (abs(rot(1, 3) * rot(0, 0) - rot(0, 3) * rot(1, 0)) > ra + rb) return false;
+
+	// Test axis L = A2 x B1
+	ra = mHalfExtents[0] * abs_r[1][1] + mHalfExtents[1] * abs_r[1][0];
+	rb = inBox.mHalfExtents[0] * abs_r[2][2] + inBox.mHalfExtents[2] * abs_r[0][2];
+	if (abs(rot(1, 3) * rot(0, 1) - rot(0, 3) * rot(1, 1)) > ra + rb) return false;
+
+	// Test axis L = A2 x B2
+	ra = mHalfExtents[0] * abs_r[2][1] + mHalfExtents[1] * abs_r[2][0];
+	rb = inBox.mHalfExtents[0] * abs_r[1][2] + inBox.mHalfExtents[1] * abs_r[0][2];
+	if (abs(rot(1, 3) * rot(0, 2) - rot(0, 3) * rot(1, 2)) > ra + rb) return false;
+
+	// Since no separating axis is found, the OBBs must be intersecting
+	return true;
+}
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Geometry/OrientedBox.h

@@ -0,0 +1,39 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Geometry/Triangle.h>
+#include <Jolt/Geometry/IndexedTriangle.h>
+#include <Jolt/Geometry/AABox.h>
+#include <Jolt/Math/Mat44.h>
+
+JPH_NAMESPACE_BEGIN
+
+class AABox;
+
+/// Oriented box
+class JPH_EXPORT_GCC_BUG_WORKAROUND [[nodiscard]] OrientedBox
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+					OrientedBox() = default;
+					OrientedBox(Mat44Arg inOrientation, Vec3Arg inHalfExtents)			: mOrientation(inOrientation), mHalfExtents(inHalfExtents) { }
+
+	/// Construct from axis aligned box and transform. Only works for rotation/translation matrix (no scaling / shearing).
+					OrientedBox(Mat44Arg inOrientation, const AABox &inBox)				: OrientedBox(inOrientation.PreTranslated(inBox.GetCenter()), inBox.GetExtent()) { }
+
+	/// Test if oriented box overlaps with axis aligned box each other
+	bool			Overlaps(const AABox &inBox, float inEpsilon = 1.0e-6f) const;
+
+	/// Test if two oriented boxes overlap each other
+	bool			Overlaps(const OrientedBox &inBox, float inEpsilon = 1.0e-6f) const;
+
+	Mat44			mOrientation;														///< Transform that positions and rotates the local space axis aligned box into world space
+	Vec3			mHalfExtents;														///< Half extents (half the size of the edge) of the local space axis aligned box
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Geometry/Plane.h

@@ -0,0 +1,101 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+/// An infinite plane described by the formula X . Normal + Constant = 0.
+class [[nodiscard]] Plane
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+					Plane() = default;
+	explicit		Plane(Vec4Arg inNormalAndConstant)										: mNormalAndConstant(inNormalAndConstant) { }
+					Plane(Vec3Arg inNormal, float inConstant)								: mNormalAndConstant(inNormal, inConstant) { }
+
+	/// Create from point and normal
+	static Plane	sFromPointAndNormal(Vec3Arg inPoint, Vec3Arg inNormal)					{ return Plane(Vec4(inNormal, -inNormal.Dot(inPoint))); }
+
+	/// Create from point and normal, double precision version that more accurately calculates the plane constant
+	static Plane	sFromPointAndNormal(DVec3Arg inPoint, Vec3Arg inNormal)					{ return Plane(Vec4(inNormal, -float(DVec3(inNormal).Dot(inPoint)))); }
+
+	/// Create from 3 counter clockwise points
+	static Plane	sFromPointsCCW(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3)				{ return sFromPointAndNormal(inV1, (inV2 - inV1).Cross(inV3 - inV1).Normalized()); }
+
+	// Properties
+	Vec3			GetNormal() const														{ return Vec3(mNormalAndConstant); }
+	void			SetNormal(Vec3Arg inNormal)												{ mNormalAndConstant = Vec4(inNormal, mNormalAndConstant.GetW()); }
+	float			GetConstant() const														{ return mNormalAndConstant.GetW(); }
+	void			SetConstant(float inConstant)											{ mNormalAndConstant.SetW(inConstant); }
+
+	/// Offset the plane (positive value means move it in the direction of the plane normal)
+	Plane			Offset(float inDistance) const											{ return Plane(mNormalAndConstant - Vec4(Vec3::sZero(), inDistance)); }
+
+	/// Transform the plane by a matrix
+	inline Plane	GetTransformed(Mat44Arg inTransform) const
+	{
+		Vec3 transformed_normal = inTransform.Multiply3x3(GetNormal());
+		return Plane(transformed_normal, GetConstant() - inTransform.GetTranslation().Dot(transformed_normal));
+	}
+
+	/// Scale the plane, can handle non-uniform and negative scaling
+	inline Plane	Scaled(Vec3Arg inScale) const
+	{
+		Vec3 scaled_normal = GetNormal() / inScale;
+		float scaled_normal_length = scaled_normal.Length();
+		return Plane(scaled_normal / scaled_normal_length, GetConstant() / scaled_normal_length);
+	}
+
+	/// Distance point to plane
+	float			SignedDistance(Vec3Arg inPoint) const									{ return inPoint.Dot(GetNormal()) + GetConstant(); }
+
+	/// Project inPoint onto the plane
+	Vec3			ProjectPointOnPlane(Vec3Arg inPoint) const								{ return inPoint - GetNormal() * SignedDistance(inPoint); }
+
+	/// Returns intersection point between 3 planes
+	static bool		sIntersectPlanes(const Plane &inP1, const Plane &inP2, const Plane &inP3, Vec3 &outPoint)
+	{
+		// We solve the equation:
+		// |ax, ay, az, aw|   | x |   | 0 |
+		// |bx, by, bz, bw| * | y | = | 0 |
+		// |cx, cy, cz, cw|   | z |   | 0 |
+		// | 0,	 0,	 0,	 1|   | 1 |   | 1 |
+		// Where normal of plane 1 = (ax, ay, az), plane constant of 1 = aw, normal of plane 2 = (bx, by, bz) etc.
+		// This involves inverting the matrix and multiplying it with [0, 0, 0, 1]
+
+		// Fetch the normals and plane constants for the three planes
+		Vec4 a = inP1.mNormalAndConstant;
+		Vec4 b = inP2.mNormalAndConstant;
+		Vec4 c = inP3.mNormalAndConstant;
+
+		// Result is a vector that we have to divide by:
+		float denominator = Vec3(a).Dot(Vec3(b).Cross(Vec3(c)));
+		if (denominator == 0.0f)
+			return false;
+
+		// The numerator is:
+		// [aw*(bz*cy-by*cz)+ay*(bw*cz-bz*cw)+az*(by*cw-bw*cy)]
+		// [aw*(bx*cz-bz*cx)+ax*(bz*cw-bw*cz)+az*(bw*cx-bx*cw)]
+		// [aw*(by*cx-bx*cy)+ax*(bw*cy-by*cw)+ay*(bx*cw-bw*cx)]
+		Vec4 numerator =
+			a.SplatW() * (b.Swizzle<SWIZZLE_Z, SWIZZLE_X, SWIZZLE_Y, SWIZZLE_UNUSED>() * c.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_X, SWIZZLE_UNUSED>() - b.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_X, SWIZZLE_UNUSED>() * c.Swizzle<SWIZZLE_Z, SWIZZLE_X, SWIZZLE_Y, SWIZZLE_UNUSED>())
+			+ a.Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_X, SWIZZLE_UNUSED>() * (b.Swizzle<SWIZZLE_W, SWIZZLE_Z, SWIZZLE_W, SWIZZLE_UNUSED>() * c.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_Y, SWIZZLE_UNUSED>() - b.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_Y, SWIZZLE_UNUSED>() * c.Swizzle<SWIZZLE_W, SWIZZLE_Z, SWIZZLE_W, SWIZZLE_UNUSED>())
+			+ a.Swizzle<SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_UNUSED>() * (b.Swizzle<SWIZZLE_Y, SWIZZLE_W, SWIZZLE_X, SWIZZLE_UNUSED>() * c.Swizzle<SWIZZLE_W, SWIZZLE_X, SWIZZLE_W, SWIZZLE_UNUSED>() - b.Swizzle<SWIZZLE_W, SWIZZLE_X, SWIZZLE_W, SWIZZLE_UNUSED>() * c.Swizzle<SWIZZLE_Y, SWIZZLE_W, SWIZZLE_X, SWIZZLE_UNUSED>());
+
+		outPoint = Vec3(numerator) / denominator;
+		return true;
+	}
+
+private:
+#ifdef JPH_OBJECT_STREAM
+	friend void		CreateRTTIPlane(class RTTI &);										// For JPH_IMPLEMENT_SERIALIZABLE_OUTSIDE_CLASS
+#endif
+
+	Vec4			mNormalAndConstant;													///< XYZ = normal, W = constant, plane: x . normal + constant = 0
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Geometry/RayAABox.h

@@ -0,0 +1,241 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+/// Helper structure holding the reciprocal of a ray for Ray vs AABox testing
+class RayInvDirection
+{
+public:
+	/// Constructors
+	inline			RayInvDirection() = default;
+	inline explicit	RayInvDirection(Vec3Arg inDirection) { Set(inDirection); }
+
+	/// Set reciprocal from ray direction
+	inline void		Set(Vec3Arg inDirection)
+	{
+		// if (abs(inDirection) <= Epsilon) the ray is nearly parallel to the slab.
+		mIsParallel = Vec3::sLessOrEqual(inDirection.Abs(), Vec3::sReplicate(1.0e-20f));
+
+		// Calculate 1 / direction while avoiding division by zero
+		mInvDirection = Vec3::sSelect(inDirection, Vec3::sOne(), mIsParallel).Reciprocal();
+	}
+
+	Vec3			mInvDirection;					///< 1 / ray direction
+	UVec4			mIsParallel;					///< for each component if it is parallel to the coordinate axis
+};
+
+/// Intersect AABB with ray, returns minimal distance along ray or FLT_MAX if no hit
+/// Note: Can return negative value if ray starts in box
+JPH_INLINE float RayAABox(Vec3Arg inOrigin, const RayInvDirection &inInvDirection, Vec3Arg inBoundsMin, Vec3Arg inBoundsMax)
+{
+	// Constants
+	Vec3 flt_min = Vec3::sReplicate(-FLT_MAX);
+	Vec3 flt_max = Vec3::sReplicate(FLT_MAX);
+
+	// Test against all three axes simultaneously.
+	Vec3 t1 = (inBoundsMin - inOrigin) * inInvDirection.mInvDirection;
+	Vec3 t2 = (inBoundsMax - inOrigin) * inInvDirection.mInvDirection;
+
+	// Compute the max of min(t1,t2) and the min of max(t1,t2) ensuring we don't
+	// use the results from any directions parallel to the slab.
+	Vec3 t_min = Vec3::sSelect(Vec3::sMin(t1, t2), flt_min, inInvDirection.mIsParallel);
+	Vec3 t_max = Vec3::sSelect(Vec3::sMax(t1, t2), flt_max, inInvDirection.mIsParallel);
+
+	// t_min.xyz = maximum(t_min.x, t_min.y, t_min.z);
+	t_min = Vec3::sMax(t_min, t_min.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_X>());
+	t_min = Vec3::sMax(t_min, t_min.Swizzle<SWIZZLE_Z, SWIZZLE_X, SWIZZLE_Y>());
+
+	// t_max.xyz = minimum(t_max.x, t_max.y, t_max.z);
+	t_max = Vec3::sMin(t_max, t_max.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_X>());
+	t_max = Vec3::sMin(t_max, t_max.Swizzle<SWIZZLE_Z, SWIZZLE_X, SWIZZLE_Y>());
+
+	// if (t_min > t_max) return FLT_MAX;
+	UVec4 no_intersection = Vec3::sGreater(t_min, t_max);
+
+	// if (t_max < 0.0f) return FLT_MAX;
+	no_intersection = UVec4::sOr(no_intersection, Vec3::sLess(t_max, Vec3::sZero()));
+
+	// if (inInvDirection.mIsParallel && !(Min <= inOrigin && inOrigin <= Max)) return FLT_MAX; else return t_min;
+	UVec4 no_parallel_overlap = UVec4::sOr(Vec3::sLess(inOrigin, inBoundsMin), Vec3::sGreater(inOrigin, inBoundsMax));
+	no_intersection = UVec4::sOr(no_intersection, UVec4::sAnd(inInvDirection.mIsParallel, no_parallel_overlap));
+	no_intersection = UVec4::sOr(no_intersection, no_intersection.SplatY());
+	no_intersection = UVec4::sOr(no_intersection, no_intersection.SplatZ());
+	return Vec3::sSelect(t_min, flt_max, no_intersection).GetX();
+}
+
+/// Intersect 4 AABBs with ray, returns minimal distance along ray or FLT_MAX if no hit
+/// Note: Can return negative value if ray starts in box
+JPH_INLINE Vec4 RayAABox4(Vec3Arg inOrigin, const RayInvDirection &inInvDirection, Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ)
+{
+	// Constants
+	Vec4 flt_min = Vec4::sReplicate(-FLT_MAX);
+	Vec4 flt_max = Vec4::sReplicate(FLT_MAX);
+
+	// Origin
+	Vec4 originx = inOrigin.SplatX();
+	Vec4 originy = inOrigin.SplatY();
+	Vec4 originz = inOrigin.SplatZ();
+
+	// Parallel
+	UVec4 parallelx = inInvDirection.mIsParallel.SplatX();
+	UVec4 parallely = inInvDirection.mIsParallel.SplatY();
+	UVec4 parallelz = inInvDirection.mIsParallel.SplatZ();
+
+	// Inverse direction
+	Vec4 invdirx = inInvDirection.mInvDirection.SplatX();
+	Vec4 invdiry = inInvDirection.mInvDirection.SplatY();
+	Vec4 invdirz = inInvDirection.mInvDirection.SplatZ();
+
+	// Test against all three axes simultaneously.
+	Vec4 t1x = (inBoundsMinX - originx) * invdirx;
+	Vec4 t1y = (inBoundsMinY - originy) * invdiry;
+	Vec4 t1z = (inBoundsMinZ - originz) * invdirz;
+	Vec4 t2x = (inBoundsMaxX - originx) * invdirx;
+	Vec4 t2y = (inBoundsMaxY - originy) * invdiry;
+	Vec4 t2z = (inBoundsMaxZ - originz) * invdirz;
+
+	// Compute the max of min(t1,t2) and the min of max(t1,t2) ensuring we don't
+	// use the results from any directions parallel to the slab.
+	Vec4 t_minx = Vec4::sSelect(Vec4::sMin(t1x, t2x), flt_min, parallelx);
+	Vec4 t_miny = Vec4::sSelect(Vec4::sMin(t1y, t2y), flt_min, parallely);
+	Vec4 t_minz = Vec4::sSelect(Vec4::sMin(t1z, t2z), flt_min, parallelz);
+	Vec4 t_maxx = Vec4::sSelect(Vec4::sMax(t1x, t2x), flt_max, parallelx);
+	Vec4 t_maxy = Vec4::sSelect(Vec4::sMax(t1y, t2y), flt_max, parallely);
+	Vec4 t_maxz = Vec4::sSelect(Vec4::sMax(t1z, t2z), flt_max, parallelz);
+
+	// t_min.xyz = maximum(t_min.x, t_min.y, t_min.z);
+	Vec4 t_min = Vec4::sMax(Vec4::sMax(t_minx, t_miny), t_minz);
+
+	// t_max.xyz = minimum(t_max.x, t_max.y, t_max.z);
+	Vec4 t_max = Vec4::sMin(Vec4::sMin(t_maxx, t_maxy), t_maxz);
+
+	// if (t_min > t_max) return FLT_MAX;
+	UVec4 no_intersection = Vec4::sGreater(t_min, t_max);
+
+	// if (t_max < 0.0f) return FLT_MAX;
+	no_intersection = UVec4::sOr(no_intersection, Vec4::sLess(t_max, Vec4::sZero()));
+
+	// if bounds are invalid return FLOAT_MAX;
+	UVec4 bounds_invalid = UVec4::sOr(UVec4::sOr(Vec4::sGreater(inBoundsMinX, inBoundsMaxX), Vec4::sGreater(inBoundsMinY, inBoundsMaxY)), Vec4::sGreater(inBoundsMinZ, inBoundsMaxZ));
+	no_intersection = UVec4::sOr(no_intersection, bounds_invalid);
+
+	// if (inInvDirection.mIsParallel && !(Min <= inOrigin && inOrigin <= Max)) return FLT_MAX; else return t_min;
+	UVec4 no_parallel_overlapx = UVec4::sAnd(parallelx, UVec4::sOr(Vec4::sLess(originx, inBoundsMinX), Vec4::sGreater(originx, inBoundsMaxX)));
+	UVec4 no_parallel_overlapy = UVec4::sAnd(parallely, UVec4::sOr(Vec4::sLess(originy, inBoundsMinY), Vec4::sGreater(originy, inBoundsMaxY)));
+	UVec4 no_parallel_overlapz = UVec4::sAnd(parallelz, UVec4::sOr(Vec4::sLess(originz, inBoundsMinZ), Vec4::sGreater(originz, inBoundsMaxZ)));
+	no_intersection = UVec4::sOr(no_intersection, UVec4::sOr(UVec4::sOr(no_parallel_overlapx, no_parallel_overlapy), no_parallel_overlapz));
+	return Vec4::sSelect(t_min, flt_max, no_intersection);
+}
+
+/// Intersect AABB with ray, returns minimal and maximal distance along ray or FLT_MAX, -FLT_MAX if no hit
+/// Note: Can return negative value for outMin if ray starts in box
+JPH_INLINE void RayAABox(Vec3Arg inOrigin, const RayInvDirection &inInvDirection, Vec3Arg inBoundsMin, Vec3Arg inBoundsMax, float &outMin, float &outMax)
+{
+	// Constants
+	Vec3 flt_min = Vec3::sReplicate(-FLT_MAX);
+	Vec3 flt_max = Vec3::sReplicate(FLT_MAX);
+
+	// Test against all three axes simultaneously.
+	Vec3 t1 = (inBoundsMin - inOrigin) * inInvDirection.mInvDirection;
+	Vec3 t2 = (inBoundsMax - inOrigin) * inInvDirection.mInvDirection;
+
+	// Compute the max of min(t1,t2) and the min of max(t1,t2) ensuring we don't
+	// use the results from any directions parallel to the slab.
+	Vec3 t_min = Vec3::sSelect(Vec3::sMin(t1, t2), flt_min, inInvDirection.mIsParallel);
+	Vec3 t_max = Vec3::sSelect(Vec3::sMax(t1, t2), flt_max, inInvDirection.mIsParallel);
+
+	// t_min.xyz = maximum(t_min.x, t_min.y, t_min.z);
+	t_min = Vec3::sMax(t_min, t_min.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_X>());
+	t_min = Vec3::sMax(t_min, t_min.Swizzle<SWIZZLE_Z, SWIZZLE_X, SWIZZLE_Y>());
+
+	// t_max.xyz = minimum(t_max.x, t_max.y, t_max.z);
+	t_max = Vec3::sMin(t_max, t_max.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_X>());
+	t_max = Vec3::sMin(t_max, t_max.Swizzle<SWIZZLE_Z, SWIZZLE_X, SWIZZLE_Y>());
+
+	// if (t_min > t_max) return FLT_MAX;
+	UVec4 no_intersection = Vec3::sGreater(t_min, t_max);
+
+	// if (t_max < 0.0f) return FLT_MAX;
+	no_intersection = UVec4::sOr(no_intersection, Vec3::sLess(t_max, Vec3::sZero()));
+
+	// if (inInvDirection.mIsParallel && !(Min <= inOrigin && inOrigin <= Max)) return FLT_MAX; else return t_min;
+	UVec4 no_parallel_overlap = UVec4::sOr(Vec3::sLess(inOrigin, inBoundsMin), Vec3::sGreater(inOrigin, inBoundsMax));
+	no_intersection = UVec4::sOr(no_intersection, UVec4::sAnd(inInvDirection.mIsParallel, no_parallel_overlap));
+	no_intersection = UVec4::sOr(no_intersection, no_intersection.SplatY());
+	no_intersection = UVec4::sOr(no_intersection, no_intersection.SplatZ());
+	outMin = Vec3::sSelect(t_min, flt_max, no_intersection).GetX();
+	outMax = Vec3::sSelect(t_max, flt_min, no_intersection).GetX();
+}
+
+/// Intersect AABB with ray, returns true if there is a hit closer than inClosest
+JPH_INLINE bool RayAABoxHits(Vec3Arg inOrigin, const RayInvDirection &inInvDirection, Vec3Arg inBoundsMin, Vec3Arg inBoundsMax, float inClosest)
+{
+	// Constants
+	Vec3 flt_min = Vec3::sReplicate(-FLT_MAX);
+	Vec3 flt_max = Vec3::sReplicate(FLT_MAX);
+
+	// Test against all three axes simultaneously.
+	Vec3 t1 = (inBoundsMin - inOrigin) * inInvDirection.mInvDirection;
+	Vec3 t2 = (inBoundsMax - inOrigin) * inInvDirection.mInvDirection;
+
+	// Compute the max of min(t1,t2) and the min of max(t1,t2) ensuring we don't
+	// use the results from any directions parallel to the slab.
+	Vec3 t_min = Vec3::sSelect(Vec3::sMin(t1, t2), flt_min, inInvDirection.mIsParallel);
+	Vec3 t_max = Vec3::sSelect(Vec3::sMax(t1, t2), flt_max, inInvDirection.mIsParallel);
+
+	// t_min.xyz = maximum(t_min.x, t_min.y, t_min.z);
+	t_min = Vec3::sMax(t_min, t_min.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_X>());
+	t_min = Vec3::sMax(t_min, t_min.Swizzle<SWIZZLE_Z, SWIZZLE_X, SWIZZLE_Y>());
+
+	// t_max.xyz = minimum(t_max.x, t_max.y, t_max.z);
+	t_max = Vec3::sMin(t_max, t_max.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_X>());
+	t_max = Vec3::sMin(t_max, t_max.Swizzle<SWIZZLE_Z, SWIZZLE_X, SWIZZLE_Y>());
+
+	// if (t_min > t_max) return false;
+	UVec4 no_intersection = Vec3::sGreater(t_min, t_max);
+
+	// if (t_max < 0.0f) return false;
+	no_intersection = UVec4::sOr(no_intersection, Vec3::sLess(t_max, Vec3::sZero()));
+
+	// if (t_min > inClosest) return false;
+	no_intersection = UVec4::sOr(no_intersection, Vec3::sGreater(t_min, Vec3::sReplicate(inClosest)));
+
+	// if (inInvDirection.mIsParallel && !(Min <= inOrigin && inOrigin <= Max)) return false; else return true;
+	UVec4 no_parallel_overlap = UVec4::sOr(Vec3::sLess(inOrigin, inBoundsMin), Vec3::sGreater(inOrigin, inBoundsMax));
+	no_intersection = UVec4::sOr(no_intersection, UVec4::sAnd(inInvDirection.mIsParallel, no_parallel_overlap));
+
+	return !no_intersection.TestAnyXYZTrue();
+}
+
+/// Intersect AABB with ray without hit fraction, based on separating axis test
+/// @see http://www.codercorner.com/RayAABB.cpp
+JPH_INLINE bool RayAABoxHits(Vec3Arg inOrigin, Vec3Arg inDirection, Vec3Arg inBoundsMin, Vec3Arg inBoundsMax)
+{
+	Vec3 extents = inBoundsMax - inBoundsMin;
+
+	Vec3 diff = 2.0f * inOrigin - inBoundsMin - inBoundsMax;
+	Vec3 abs_diff = diff.Abs();
+
+	UVec4 no_intersection = UVec4::sAnd(Vec3::sGreater(abs_diff, extents), Vec3::sGreaterOrEqual(diff * inDirection, Vec3::sZero()));
+
+	Vec3 abs_dir = inDirection.Abs();
+	Vec3 abs_dir_yzz = abs_dir.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_Z>();
+	Vec3 abs_dir_xyx = abs_dir.Swizzle<SWIZZLE_X, SWIZZLE_Y, SWIZZLE_X>();
+
+	Vec3 extents_yzz = extents.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_Z>();
+	Vec3 extents_xyx = extents.Swizzle<SWIZZLE_X, SWIZZLE_Y, SWIZZLE_X>();
+
+	Vec3 diff_yzx = diff.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_X>();
+
+	Vec3 dir_yzx = inDirection.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_X>();
+
+	no_intersection = UVec4::sOr(no_intersection, Vec3::sGreater((inDirection * diff_yzx - dir_yzx * diff).Abs(), extents_xyx * abs_dir_yzz + extents_yzz * abs_dir_xyx));
+
+	return !no_intersection.TestAnyXYZTrue();
+}
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Geometry/RayCapsule.h

@@ -0,0 +1,37 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Geometry/RayCylinder.h>
+#include <Jolt/Geometry/RaySphere.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Tests a ray starting at inRayOrigin and extending infinitely in inRayDirection
+/// against a capsule centered around the origin with its axis along the Y axis and half height specified.
+/// @return FLT_MAX if there is no intersection, otherwise the fraction along the ray.
+/// @param inRayDirection Ray direction. Does not need to be normalized.
+/// @param inRayOrigin Origin of the ray. If the ray starts inside the capsule, the returned fraction will be 0.
+/// @param inCapsuleHalfHeight Distance from the origin to the center of the top sphere (or that of the bottom)
+/// @param inCapsuleRadius Radius of the top/bottom sphere
+JPH_INLINE float RayCapsule(Vec3Arg inRayOrigin, Vec3Arg inRayDirection, float inCapsuleHalfHeight, float inCapsuleRadius)
+{
+	// Test infinite cylinder
+	float cylinder = RayCylinder(inRayOrigin, inRayDirection, inCapsuleRadius);
+	if (cylinder == FLT_MAX)
+		return FLT_MAX;
+
+	// If this hit is in the finite cylinder we have our fraction
+	if (abs(inRayOrigin.GetY() + cylinder * inRayDirection.GetY()) <= inCapsuleHalfHeight)
+		return cylinder;
+
+	// Test upper and lower sphere
+	Vec3 sphere_center(0, inCapsuleHalfHeight, 0);
+	float upper = RaySphere(inRayOrigin, inRayDirection, sphere_center, inCapsuleRadius);
+	float lower = RaySphere(inRayOrigin, inRayDirection, -sphere_center, inCapsuleRadius);
+	return min(upper, lower);
+}
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Geometry/RayCylinder.h

@@ -0,0 +1,101 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Math/FindRoot.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Tests a ray starting at inRayOrigin and extending infinitely in inRayDirection
+/// against an infinite cylinder centered along the Y axis
+/// @return FLT_MAX if there is no intersection, otherwise the fraction along the ray.
+/// @param inRayDirection Direction of the ray. Does not need to be normalized.
+/// @param inRayOrigin Origin of the ray. If the ray starts inside the cylinder, the returned fraction will be 0.
+/// @param inCylinderRadius Radius of the infinite cylinder
+JPH_INLINE float RayCylinder(Vec3Arg inRayOrigin, Vec3Arg inRayDirection, float inCylinderRadius)
+{
+	// Remove Y component of ray to see of ray intersects with infinite cylinder
+	UVec4 mask_y = UVec4(0, 0xffffffff, 0, 0);
+	Vec3 origin_xz = Vec3::sSelect(inRayOrigin, Vec3::sZero(), mask_y);
+	float origin_xz_len_sq = origin_xz.LengthSq();
+	float r_sq = Square(inCylinderRadius);
+	if (origin_xz_len_sq > r_sq)
+	{
+		// Ray starts outside of the infinite cylinder
+		// Solve: |RayOrigin_xz + fraction * RayDirection_xz|^2 = r^2 to find fraction
+		Vec3 direction_xz = Vec3::sSelect(inRayDirection, Vec3::sZero(), mask_y);
+		float a = direction_xz.LengthSq();
+		float b = 2.0f * origin_xz.Dot(direction_xz);
+		float c = origin_xz_len_sq - r_sq;
+		float fraction1, fraction2;
+		if (FindRoot(a, b, c, fraction1, fraction2) == 0)
+			return FLT_MAX; // No intersection with infinite cylinder
+
+		// Get fraction corresponding to the ray entering the circle
+		float fraction = min(fraction1, fraction2);
+		if (fraction >= 0.0f)
+			return fraction;
+	}
+	else
+	{
+		// Ray starts inside the infinite cylinder
+		return 0.0f;
+	}
+
+	// No collision
+	return FLT_MAX;
+}
+
+/// Test a ray against a cylinder centered around the origin with its axis along the Y axis and half height specified.
+/// @return FLT_MAX if there is no intersection, otherwise the fraction along the ray.
+/// @param inRayDirection Ray direction. Does not need to be normalized.
+/// @param inRayOrigin Origin of the ray. If the ray starts inside the cylinder, the returned fraction will be 0.
+/// @param inCylinderRadius Radius of the cylinder
+/// @param inCylinderHalfHeight Distance from the origin to the top (or bottom) of the cylinder
+JPH_INLINE float RayCylinder(Vec3Arg inRayOrigin, Vec3Arg inRayDirection, float inCylinderHalfHeight, float inCylinderRadius)
+{
+	// Test infinite cylinder
+	float fraction = RayCylinder(inRayOrigin, inRayDirection, inCylinderRadius);
+	if (fraction == FLT_MAX)
+		return FLT_MAX;
+
+	// If this hit is in the finite cylinder we have our fraction
+	if (abs(inRayOrigin.GetY() + fraction * inRayDirection.GetY()) <= inCylinderHalfHeight)
+		return fraction;
+
+	// Check if ray could hit the top or bottom plane of the cylinder
+	float direction_y = inRayDirection.GetY();
+	if (direction_y != 0.0f)
+	{
+		// Solving line equation: x = ray_origin + fraction * ray_direction
+		// and plane equation: plane_normal . x + plane_constant = 0
+		// fraction = (-plane_constant - plane_normal . ray_origin) / (plane_normal . ray_direction)
+		// when the ray_direction.y < 0:
+		// plane_constant = -cylinder_half_height, plane_normal = (0, 1, 0)
+		// else
+		// plane_constant = -cylinder_half_height, plane_normal = (0, -1, 0)
+		float origin_y = inRayOrigin.GetY();
+		float plane_fraction;
+		if (direction_y < 0.0f)
+			plane_fraction = (inCylinderHalfHeight - origin_y) / direction_y;
+		else
+			plane_fraction = -(inCylinderHalfHeight + origin_y) / direction_y;
+
+		// Check if the hit is in front of the ray
+		if (plane_fraction >= 0.0f)
+		{
+			// Test if this hit is inside the cylinder
+			Vec3 point = inRayOrigin + plane_fraction * inRayDirection;
+			float dist_sq = Square(point.GetX()) + Square(point.GetZ());
+			if (dist_sq <= Square(inCylinderRadius))
+				return plane_fraction;
+		}
+	}
+
+	// No collision
+	return FLT_MAX;
+}
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Geometry/RaySphere.h

@@ -0,0 +1,96 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Math/FindRoot.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Tests a ray starting at inRayOrigin and extending infinitely in inRayDirection against a sphere,
+/// @return FLT_MAX if there is no intersection, otherwise the fraction along the ray.
+/// @param inRayOrigin Ray origin. If the ray starts inside the sphere, the returned fraction will be 0.
+/// @param inRayDirection Ray direction. Does not need to be normalized.
+/// @param inSphereCenter Position of the center of the sphere
+/// @param inSphereRadius Radius of the sphere
+JPH_INLINE float RaySphere(Vec3Arg inRayOrigin, Vec3Arg inRayDirection, Vec3Arg inSphereCenter, float inSphereRadius)
+{
+	// Solve: |RayOrigin + fraction * RayDirection - SphereCenter|^2 = SphereRadius^2 for fraction
+	Vec3 center_origin = inRayOrigin - inSphereCenter;
+	float a = inRayDirection.LengthSq();
+	float b = 2.0f * inRayDirection.Dot(center_origin);
+	float c = center_origin.LengthSq() - inSphereRadius * inSphereRadius;
+	float fraction1, fraction2;
+	if (FindRoot(a, b, c, fraction1, fraction2) == 0)
+		return c <= 0.0f? 0.0f : FLT_MAX; // Return if origin is inside the sphere
+
+	// Sort so that the smallest is first
+	if (fraction1 > fraction2)
+		std::swap(fraction1, fraction2);
+
+	// Test solution with lowest fraction, this will be the ray entering the sphere
+	if (fraction1 >= 0.0f)
+		return fraction1; // Sphere is before the ray start
+
+	// Test solution with highest fraction, this will be the ray leaving the sphere
+	if (fraction2 >= 0.0f)
+		return 0.0f; // We start inside the sphere
+
+	// No solution
+	return FLT_MAX;
+}
+
+/// Tests a ray starting at inRayOrigin and extending infinitely in inRayDirection against a sphere.
+/// Outputs entry and exit points (outMinFraction and outMaxFraction) along the ray (which could be negative if the hit point is before the start of the ray).
+/// @param inRayOrigin Ray origin. If the ray starts inside the sphere, the returned fraction will be 0.
+/// @param inRayDirection Ray direction. Does not need to be normalized.
+/// @param inSphereCenter Position of the center of the sphere.
+/// @param inSphereRadius Radius of the sphere.
+/// @param outMinFraction Returned lowest intersection fraction
+/// @param outMaxFraction Returned highest intersection fraction
+/// @return The amount of intersections with the sphere.
+/// If 1 intersection is returned outMinFraction will be equal to outMaxFraction
+JPH_INLINE int RaySphere(Vec3Arg inRayOrigin, Vec3Arg inRayDirection, Vec3Arg inSphereCenter, float inSphereRadius, float &outMinFraction, float &outMaxFraction)
+{
+	// Solve: |RayOrigin + fraction * RayDirection - SphereCenter|^2 = SphereRadius^2 for fraction
+	Vec3 center_origin = inRayOrigin - inSphereCenter;
+	float a = inRayDirection.LengthSq();
+	float b = 2.0f * inRayDirection.Dot(center_origin);
+	float c = center_origin.LengthSq() - inSphereRadius * inSphereRadius;
+	float fraction1, fraction2;
+	switch (FindRoot(a, b, c, fraction1, fraction2))
+	{
+	case 0:
+		if (c <= 0.0f)
+		{
+			// Origin inside sphere
+			outMinFraction = outMaxFraction = 0.0f;
+			return 1;
+		}
+		else
+		{
+			// Origin outside of the sphere
+			return 0;
+		}
+		break;
+
+	case 1:
+		// Ray is touching the sphere
+		outMinFraction = outMaxFraction = fraction1;
+		return 1;
+
+	default:
+		// Ray enters and exits the sphere
+
+		// Sort so that the smallest is first
+		if (fraction1 > fraction2)
+			std::swap(fraction1, fraction2);
+
+		outMinFraction = fraction1;
+		outMaxFraction = fraction2;
+		return 2;
+	}
+}
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Geometry/RayTriangle.h

@@ -0,0 +1,158 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+/// Intersect ray with triangle, returns closest point or FLT_MAX if no hit (branch less version)
+/// Adapted from: http://en.wikipedia.org/wiki/M%C3%B6ller%E2%80%93Trumbore_intersection_algorithm
+JPH_INLINE float RayTriangle(Vec3Arg inOrigin, Vec3Arg inDirection, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2)
+{
+	// Epsilon
+	Vec3 epsilon = Vec3::sReplicate(1.0e-12f);
+
+	// Zero & one
+	Vec3 zero = Vec3::sZero();
+	Vec3 one = Vec3::sOne();
+
+	// Find vectors for two edges sharing inV0
+	Vec3 e1 = inV1 - inV0;
+	Vec3 e2 = inV2 - inV0;
+
+	// Begin calculating determinant - also used to calculate u parameter
+	Vec3 p = inDirection.Cross(e2);
+
+	// if determinant is near zero, ray lies in plane of triangle
+	Vec3 det = Vec3::sReplicate(e1.Dot(p));
+
+	// Check if determinant is near zero
+	UVec4 det_near_zero = Vec3::sLess(det.Abs(), epsilon);
+
+	// When the determinant is near zero, set it to one to avoid dividing by zero
+	det = Vec3::sSelect(det, Vec3::sOne(), det_near_zero);
+
+	// Calculate distance from inV0 to ray origin
+	Vec3 s = inOrigin - inV0;
+
+	// Calculate u parameter
+	Vec3 u = Vec3::sReplicate(s.Dot(p)) / det;
+
+	// Prepare to test v parameter
+	Vec3 q = s.Cross(e1);
+
+	// Calculate v parameter
+	Vec3 v = Vec3::sReplicate(inDirection.Dot(q)) / det;
+
+	// Get intersection point
+	Vec3 t = Vec3::sReplicate(e2.Dot(q)) / det;
+
+	// Check if there is an intersection
+	UVec4 no_intersection =
+		UVec4::sOr
+		(
+			UVec4::sOr
+			(
+				UVec4::sOr
+				(
+					det_near_zero,
+					Vec3::sLess(u, zero)
+				),
+				UVec4::sOr
+				(
+					Vec3::sLess(v, zero),
+					Vec3::sGreater(u + v, one)
+				)
+			),
+			Vec3::sLess(t, zero)
+		);
+
+	// Select intersection point or FLT_MAX based on if there is an intersection or not
+	return Vec3::sSelect(t, Vec3::sReplicate(FLT_MAX), no_intersection).GetX();
+}
+
+/// Intersect ray with 4 triangles in SOA format, returns 4 vector of closest points or FLT_MAX if no hit (uses bit tricks to do less divisions)
+JPH_INLINE Vec4 RayTriangle4(Vec3Arg inOrigin, Vec3Arg inDirection, Vec4Arg inV0X, Vec4Arg inV0Y, Vec4Arg inV0Z, Vec4Arg inV1X, Vec4Arg inV1Y, Vec4Arg inV1Z, Vec4Arg inV2X, Vec4Arg inV2Y, Vec4Arg inV2Z)
+{
+	// Epsilon
+	Vec4 epsilon = Vec4::sReplicate(1.0e-12f);
+
+	// Zero
+	Vec4 zero = Vec4::sZero();
+
+	// Find vectors for two edges sharing inV0
+	Vec4 e1x = inV1X - inV0X;
+	Vec4 e1y = inV1Y - inV0Y;
+	Vec4 e1z = inV1Z - inV0Z;
+	Vec4 e2x = inV2X - inV0X;
+	Vec4 e2y = inV2Y - inV0Y;
+	Vec4 e2z = inV2Z - inV0Z;
+
+	// Get direction vector components
+	Vec4 dx = inDirection.SplatX();
+	Vec4 dy = inDirection.SplatY();
+	Vec4 dz = inDirection.SplatZ();
+
+	// Begin calculating determinant - also used to calculate u parameter
+	Vec4 px = dy * e2z - dz * e2y;
+	Vec4 py = dz * e2x - dx * e2z;
+	Vec4 pz = dx * e2y - dy * e2x;
+
+	// if determinant is near zero, ray lies in plane of triangle
+	Vec4 det = e1x * px + e1y * py + e1z * pz;
+
+	// Get sign bit for determinant and make positive
+	Vec4 det_sign = Vec4::sAnd(det, UVec4::sReplicate(0x80000000).ReinterpretAsFloat());
+	det = Vec4::sXor(det, det_sign);
+
+	// Check which determinants are near zero
+	UVec4 det_near_zero = Vec4::sLess(det, epsilon);
+
+	// Set components of the determinant to 1 that are near zero to avoid dividing by zero
+	det = Vec4::sSelect(det, Vec4::sOne(), det_near_zero);
+
+	// Calculate distance from inV0 to ray origin
+	Vec4 sx = inOrigin.SplatX() - inV0X;
+	Vec4 sy = inOrigin.SplatY() - inV0Y;
+	Vec4 sz = inOrigin.SplatZ() - inV0Z;
+
+	// Calculate u parameter and flip sign if determinant was negative
+	Vec4 u = Vec4::sXor(sx * px + sy * py + sz * pz, det_sign);
+
+	// Prepare to test v parameter
+	Vec4 qx = sy * e1z - sz * e1y;
+	Vec4 qy = sz * e1x - sx * e1z;
+	Vec4 qz = sx * e1y - sy * e1x;
+
+	// Calculate v parameter and flip sign if determinant was negative
+	Vec4 v = Vec4::sXor(dx * qx + dy * qy + dz * qz, det_sign);
+
+	// Get intersection point and flip sign if determinant was negative
+	Vec4 t = Vec4::sXor(e2x * qx + e2y * qy + e2z * qz, det_sign);
+
+	// Check if there is an intersection
+	UVec4 no_intersection =
+		UVec4::sOr
+		(
+			UVec4::sOr
+			(
+				UVec4::sOr
+				(
+					det_near_zero,
+					Vec4::sLess(u, zero)
+				),
+				UVec4::sOr
+				(
+					Vec4::sLess(v, zero),
+					Vec4::sGreater(u + v, det)
+				)
+			),
+			Vec4::sLess(t, zero)
+		);
+
+	// Select intersection point or FLT_MAX based on if there is an intersection or not
+	return Vec4::sSelect(t / det, Vec4::sReplicate(FLT_MAX), no_intersection);
+}
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Geometry/Sphere.h

@@ -0,0 +1,72 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Geometry/AABox.h>
+
+JPH_NAMESPACE_BEGIN
+
+class [[nodiscard]] Sphere
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+	inline				Sphere() = default;
+	inline				Sphere(const Float3 &inCenter, float inRadius)			: mCenter(inCenter), mRadius(inRadius) { }
+	inline				Sphere(Vec3Arg inCenter, float inRadius)				: mRadius(inRadius) { inCenter.StoreFloat3(&mCenter); }
+
+	/// Calculate the support vector for this convex shape.
+	inline Vec3			GetSupport(Vec3Arg inDirection) const
+	{
+		float length = inDirection.Length();
+		return length > 0.0f ? Vec3::sLoadFloat3Unsafe(mCenter) + (mRadius/ length) * inDirection : Vec3::sLoadFloat3Unsafe(mCenter);
+	}
+
+	// Properties
+	inline Vec3			GetCenter() const										{ return Vec3::sLoadFloat3Unsafe(mCenter); }
+	inline float		GetRadius() const										{ return mRadius; }
+
+	/// Test if two spheres overlap
+	inline bool			Overlaps(const Sphere &inB) const
+	{
+		return (Vec3::sLoadFloat3Unsafe(mCenter) - Vec3::sLoadFloat3Unsafe(inB.mCenter)).LengthSq() <= Square(mRadius + inB.mRadius);
+	}
+
+	/// Check if this sphere overlaps with a box
+	inline bool			Overlaps(const AABox &inOther) const
+	{
+		return inOther.GetSqDistanceTo(GetCenter()) <= Square(mRadius);
+	}
+
+	/// Create the minimal sphere that encapsulates this sphere and inPoint
+	inline void			EncapsulatePoint(Vec3Arg inPoint)
+	{
+		// Calculate distance between point and center
+		Vec3 center = GetCenter();
+		Vec3 d_vec = inPoint - center;
+		float d_sq = d_vec.LengthSq();
+		if (d_sq > Square(mRadius))
+		{
+			// It is further away than radius, we need to widen the sphere
+			// The diameter of the new sphere is radius + d, so the new radius is half of that
+			float d = sqrt(d_sq);
+			float radius = 0.5f * (mRadius + d);
+
+			// The center needs to shift by new radius - old radius in the direction of d
+			center += (radius - mRadius) / d * d_vec;
+
+			// Store new sphere
+			center.StoreFloat3(&mCenter);
+			mRadius = radius;
+		}
+	}
+
+private:
+	Float3				mCenter;
+	float				mRadius;
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Geometry/Triangle.h

@@ -0,0 +1,34 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+/// A simple triangle and its material
+class Triangle
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+					Triangle() = default;
+					Triangle(const Float3 &inV1, const Float3 &inV2, const Float3 &inV3, uint32 inMaterialIndex = 0, uint32 inUserData = 0) : mV { inV1, inV2, inV3 }, mMaterialIndex(inMaterialIndex), mUserData(inUserData) { }
+					Triangle(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3, uint32 inMaterialIndex = 0, uint32 inUserData = 0) : mMaterialIndex(inMaterialIndex), mUserData(inUserData) { inV1.StoreFloat3(&mV[0]); inV2.StoreFloat3(&mV[1]); inV3.StoreFloat3(&mV[2]); }
+
+	/// Get center of triangle
+	Vec3			GetCentroid() const
+	{
+		return (Vec3::sLoadFloat3Unsafe(mV[0]) + Vec3::sLoadFloat3Unsafe(mV[1]) + Vec3::sLoadFloat3Unsafe(mV[2])) * (1.0f / 3.0f);
+	}
+
+	/// Vertices
+	Float3			mV[3];
+	uint32			mMaterialIndex = 0;			///< Follows mV[3] so that we can read mV as 4 vectors
+	uint32			mUserData = 0;				///< User data that can be used for anything by the application, e.g. for tracking the original index of the triangle
+};
+
+using TriangleList = Array<Triangle>;
+
+JPH_NAMESPACE_END