Ajout de Jolt Physics + 1ere version des factory entitecomposants - camera, transform, rigidbody, collider, renderer

lib/All/JoltPhysics/UnitTests/Physics/ActiveEdgesTests.cpp

@@ -0,0 +1,406 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include "PhysicsTestContext.h"
+#include "Layers.h"
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Collision/Shape/CapsuleShape.h>
+#include <Jolt/Physics/Collision/Shape/SphereShape.h>
+#include <Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h>
+#include <Jolt/Physics/Collision/Shape/MeshShape.h>
+#include <Jolt/Physics/Collision/Shape/HeightFieldShape.h>
+#include <Jolt/Physics/Collision/Shape/ScaledShape.h>
+#include <Jolt/Physics/Collision/CollisionCollectorImpl.h>
+#include <Jolt/Physics/Collision/CollideShape.h>
+#include <Jolt/Physics/Collision/ShapeCast.h>
+#include <Jolt/Physics/Collision/CollisionDispatch.h>
+
+TEST_SUITE("ActiveEdgesTest")
+{
+	static const float cCapsuleProbeOffset = 0.1f; // How much to offset the probe from y = 0 in order to avoid hitting a back instead of a front face
+	static const float cCapsuleRadius = 0.1f;
+
+	// Create a capsule as our probe
+	static Ref<Shape> sCreateProbeCapsule()
+	{
+		// Ensure capsule is long enough so that when active edges mode is on, we will always get a horizontal penetration axis rather than a vertical one
+		CapsuleShapeSettings capsule(1.0f, cCapsuleRadius);
+		capsule.SetEmbedded();
+		return capsule.Create().Get();
+	}
+
+	// Create a flat mesh shape consisting of 7 x 7 quads, we know that only the outer edges of this shape are active
+	static Ref<ShapeSettings> sCreateMeshShape()
+	{
+		TriangleList triangles;
+		for (int z = 0; z < 7; ++z)
+			for (int x = 0; x < 7; ++x)
+			{
+				float fx = (float)x - 3.5f, fz = (float)z - 3.5f;
+				triangles.push_back(Triangle(Vec3(fx, 0, fz), Vec3(fx, 0, fz + 1), Vec3(fx + 1, 0, fz + 1)));
+				triangles.push_back(Triangle(Vec3(fx, 0, fz), Vec3(fx + 1, 0, fz + 1), Vec3(fx + 1, 0, fz)));
+			}
+
+		return new MeshShapeSettings(triangles);
+	}
+
+	// Create a flat height field shape that has the same properties as the mesh shape
+	static Ref<ShapeSettings> sCreateHeightFieldShape()
+	{
+		float samples[8*8];
+		memset(samples, 0, sizeof(samples));
+		return new HeightFieldShapeSettings(samples, Vec3(-3.5f, 0, -3.5f), Vec3::sOne(), 8);
+	}
+
+	// This struct indicates what we hope to find as hit
+	struct ExpectedHit
+	{
+		Vec3		mPosition;
+		Vec3		mPenetrationAxis;
+	};
+
+	// Compare expected hits with returned hits
+	template <class ResultType>
+	static void sCheckMatch(const Array<ResultType> &inResult, const Array<ExpectedHit> &inExpectedHits, float inAccuracySq)
+	{
+		CHECK(inResult.size() == inExpectedHits.size());
+
+		for (const ExpectedHit &hit : inExpectedHits)
+		{
+			bool found = false;
+			for (const ResultType &result : inResult)
+				if (result.mContactPointOn2.IsClose(hit.mPosition, inAccuracySq)
+					&& result.mPenetrationAxis.Normalized().IsClose(hit.mPenetrationAxis, inAccuracySq))
+				{
+					found = true;
+					break;
+				}
+			CHECK(found);
+		}
+	}
+
+	// Collide our probe against the test shape and validate the hit results
+	static void sTestCollideShape(Shape *inProbeShape, Shape *inTestShape, Vec3Arg inTestShapeScale, const CollideShapeSettings &inSettings, Vec3Arg inProbeShapePos, const Array<ExpectedHit> &inExpectedHits)
+	{
+		AllHitCollisionCollector<CollideShapeCollector> collector;
+		CollisionDispatch::sCollideShapeVsShape(inProbeShape, inTestShape, Vec3::sOne(), inTestShapeScale, Mat44::sTranslation(inProbeShapePos), Mat44::sIdentity(), SubShapeIDCreator(), SubShapeIDCreator(), inSettings, collector);
+
+		sCheckMatch(collector.mHits, inExpectedHits, 1.0e-8f);
+	}
+
+	// Collide a probe shape against our test shape in various locations to verify active edge behavior
+	static void sTestCollideShape(const ShapeSettings *inTestShape, Vec3Arg inTestShapeScale, bool inActiveEdgesOnly)
+	{
+		CollideShapeSettings settings;
+		settings.mActiveEdgeMode = inActiveEdgesOnly? EActiveEdgeMode::CollideOnlyWithActive : EActiveEdgeMode::CollideWithAll;
+
+		Ref<Shape> test_shape = inTestShape->Create().Get();
+		Ref<Shape> capsule = sCreateProbeCapsule();
+
+		// Test hitting all active edges
+		sTestCollideShape(capsule, test_shape, inTestShapeScale, settings, Vec3(-3.5f, cCapsuleProbeOffset, 0), { { Vec3(-3.5f, 0, 0), Vec3(1, 0, 0) } });
+		sTestCollideShape(capsule, test_shape, inTestShapeScale, settings, Vec3(3.5f, cCapsuleProbeOffset, 0), { { Vec3(3.5f, 0, 0), Vec3(-1, 0, 0) } });
+		sTestCollideShape(capsule, test_shape, inTestShapeScale, settings, Vec3(0, cCapsuleProbeOffset, -3.5f), { { Vec3(0, 0, -3.5f), Vec3(0, 0, 1) } });
+		sTestCollideShape(capsule, test_shape, inTestShapeScale, settings, Vec3(0, cCapsuleProbeOffset, 3.5f), { { Vec3(0, 0, 3.5f), Vec3(0, 0, -1) } });
+
+		// Test hitting internal edges, this should return two hits
+		sTestCollideShape(capsule, test_shape, inTestShapeScale, settings, Vec3(-2.5f, cCapsuleProbeOffset, 0), { { Vec3(-2.5f, 0, 0), inActiveEdgesOnly? Vec3(0, -1, 0) : Vec3(-1, 0, 0) }, { Vec3(-2.5f, 0, 0), inActiveEdgesOnly? Vec3(0, -1, 0) : Vec3(1, 0, 0) } });
+		sTestCollideShape(capsule, test_shape, inTestShapeScale, settings, Vec3(0, cCapsuleProbeOffset, -2.5f), { { Vec3(0, 0, -2.5f), inActiveEdgesOnly? Vec3(0, -1, 0) : Vec3(0, 0, -1) }, { Vec3(0, 0, -2.5f), inActiveEdgesOnly? Vec3(0, -1, 0) : Vec3(0, 0, -1) } });
+
+		// Test hitting an interior diagonal, this should return two hits
+		sTestCollideShape(capsule, test_shape, inTestShapeScale, settings, Vec3(-3.0f, cCapsuleProbeOffset, 0), { { Vec3(-3.0f, 0, 0), inActiveEdgesOnly? Vec3(0, -1, 0) : (inTestShapeScale * Vec3(1, 0, -1)).Normalized() }, { Vec3(-3.0f, 0, 0), inActiveEdgesOnly? Vec3(0, -1, 0) : (inTestShapeScale * Vec3(-1, 0, 1)).Normalized() } });
+	}
+
+	TEST_CASE("CollideShapeMesh")
+	{
+		Ref<ShapeSettings> shape = sCreateMeshShape();
+
+		sTestCollideShape(shape, Vec3::sOne(), false);
+
+		sTestCollideShape(shape, Vec3::sOne(), true);
+
+		sTestCollideShape(shape, Vec3(-1, 1, 1), false);
+
+		sTestCollideShape(shape, Vec3(-1, 1, 1), true);
+	}
+
+	TEST_CASE("CollideShapeHeightField")
+	{
+		Ref<ShapeSettings> shape = sCreateHeightFieldShape();
+
+		sTestCollideShape(shape, Vec3::sOne(), false);
+
+		sTestCollideShape(shape, Vec3::sOne(), true);
+
+		sTestCollideShape(shape, Vec3(-1, 1, 1), false);
+
+		sTestCollideShape(shape, Vec3(-1, 1, 1), true);
+	}
+
+	// Cast our probe against the test shape and validate the hit results
+	static void sTestCastShape(Shape *inProbeShape, Shape *inTestShape, Vec3Arg inTestShapeScale, const ShapeCastSettings &inSettings, Vec3Arg inProbeShapePos, Vec3Arg inProbeShapeDirection, const Array<ExpectedHit> &inExpectedHits)
+	{
+		AllHitCollisionCollector<CastShapeCollector> collector;
+		ShapeCast shape_cast(inProbeShape, Vec3::sOne(), Mat44::sTranslation(inProbeShapePos), inProbeShapeDirection);
+		CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, inSettings, inTestShape, inTestShapeScale, ShapeFilter(), Mat44::sIdentity(), SubShapeIDCreator(), SubShapeIDCreator(), collector);
+
+		sCheckMatch(collector.mHits, inExpectedHits, 1.0e-6f);
+	}
+
+	// Cast a probe shape against our test shape in various locations to verify active edge behavior
+	static void sTestCastShape(const ShapeSettings *inTestShape, Vec3Arg inTestShapeScale, bool inActiveEdgesOnly)
+	{
+		ShapeCastSettings settings;
+		settings.mActiveEdgeMode = inActiveEdgesOnly? EActiveEdgeMode::CollideOnlyWithActive : EActiveEdgeMode::CollideWithAll;
+		settings.mReturnDeepestPoint = true;
+
+		Ref<Shape> test_shape = inTestShape->Create().Get();
+		Ref<Shape> capsule = sCreateProbeCapsule();
+
+		// Test hitting all active edges
+		sTestCastShape(capsule, test_shape, inTestShapeScale, settings, Vec3(-4, cCapsuleProbeOffset, 0), Vec3(0.5f, 0, 0), { { Vec3(-3.5f, 0, 0), Vec3(1, 0, 0) } });
+		sTestCastShape(capsule, test_shape, inTestShapeScale, settings, Vec3(4, cCapsuleProbeOffset, 0), Vec3(-0.5f, 0, 0), { { Vec3(3.5f, 0, 0), Vec3(-1, 0, 0) } });
+		sTestCastShape(capsule, test_shape, inTestShapeScale, settings, Vec3(0, cCapsuleProbeOffset, -4), Vec3(0, 0, 0.5f), { { Vec3(0, 0, -3.5f), Vec3(0, 0, 1) } });
+		sTestCastShape(capsule, test_shape, inTestShapeScale, settings, Vec3(0, cCapsuleProbeOffset, 4), Vec3(0, 0, -0.5f), { { Vec3(0, 0, 3.5f), Vec3(0, 0, -1) } });
+
+		// Test hitting internal edges, this should return two hits
+		sTestCastShape(capsule, test_shape, inTestShapeScale, settings, Vec3(-2.5f - 1.1f * cCapsuleRadius, cCapsuleProbeOffset, 0), Vec3(0.2f * cCapsuleRadius, 0, 0), { { Vec3(-2.5f, 0, 0), inActiveEdgesOnly? Vec3(0, -1, 0) : Vec3(-1, 0, 0) }, { Vec3(-2.5f, 0, 0), inActiveEdgesOnly? Vec3(0, -1, 0) : Vec3(1, 0, 0) } });
+		sTestCastShape(capsule, test_shape, inTestShapeScale, settings, Vec3(0, cCapsuleProbeOffset, -2.5f - 1.1f * cCapsuleRadius), Vec3(0, 0, 0.2f * cCapsuleRadius), { { Vec3(0, 0, -2.5f), inActiveEdgesOnly? Vec3(0, -1, 0) : Vec3(0, 0, -1) }, { Vec3(0, 0, -2.5f), inActiveEdgesOnly? Vec3(0, -1, 0) : Vec3(0, 0, -1) } });
+	}
+
+	TEST_CASE("CastShapeMesh")
+	{
+		Ref<ShapeSettings> shape = sCreateMeshShape();
+
+		sTestCastShape(shape, Vec3::sOne(), false);
+
+		sTestCastShape(shape, Vec3::sOne(), true);
+
+		sTestCastShape(shape, Vec3(-1, 1, 1), false);
+
+		sTestCastShape(shape, Vec3(-1, 1, 1), true);
+	}
+
+	TEST_CASE("CastShapeHeightField")
+	{
+		Ref<ShapeSettings> shape = sCreateHeightFieldShape();
+
+		sTestCastShape(shape, Vec3::sOne(), false);
+
+		sTestCastShape(shape, Vec3::sOne(), true);
+
+		sTestCastShape(shape, Vec3(-1, 1, 1), false);
+
+		sTestCastShape(shape, Vec3(-1, 1, 1), true);
+	}
+
+	// Tests a discrete cube sliding over a mesh / heightfield shape
+	static void sDiscreteCubeSlide(Ref<ShapeSettings> inShape, bool inCheckActiveEdges)
+	{
+		PhysicsTestContext c;
+
+		const float cPenetrationSlop = c.GetSystem()->GetPhysicsSettings().mPenetrationSlop;
+
+		// Set simulation settings
+		PhysicsSettings settings;
+		settings.mCheckActiveEdges = inCheckActiveEdges;
+		c.GetSystem()->SetPhysicsSettings(settings);
+
+		// Create frictionless floor
+		Body &floor = c.CreateBody(inShape, RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, EActivation::DontActivate);
+		floor.SetFriction(0.0f);
+
+		// Create box sliding over the floor
+		RVec3 initial_position(-3, 0.1f - cPenetrationSlop, 0);
+		Vec3 initial_velocity(3, 0, 0);
+		Body &box = c.CreateBox(initial_position, Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3::sReplicate(0.1f));
+		box.SetLinearVelocity(initial_velocity);
+		box.SetFriction(0.0f);
+		box.GetMotionProperties()->SetLinearDamping(0.0f);
+
+		const float cSimulationTime = 2.0f;
+		c.Simulate(cSimulationTime);
+
+		RVec3 expected_position = initial_position + cSimulationTime * initial_velocity;
+		if (inCheckActiveEdges)
+		{
+			// Box should have slided frictionless over the plane without encountering any collisions
+			CHECK_APPROX_EQUAL(box.GetPosition(), expected_position, 1.0e-3f);
+			CHECK_APPROX_EQUAL(box.GetLinearVelocity(), initial_velocity, 2.0e-3f);
+		}
+		else
+		{
+			// Box should have bumped into an internal edge and not reached its target
+			CHECK(box.GetPosition().GetX() < expected_position.GetX() - 1.0f);
+		}
+	}
+
+	TEST_CASE("DiscreteCubeSlideMesh")
+	{
+		Ref<ShapeSettings> shape = sCreateMeshShape();
+
+		sDiscreteCubeSlide(shape, false);
+
+		sDiscreteCubeSlide(shape, true);
+
+		Ref<ShapeSettings> scaled_shape = new ScaledShapeSettings(shape, Vec3(-1, 1, 1));
+
+		sDiscreteCubeSlide(scaled_shape, false);
+
+		sDiscreteCubeSlide(scaled_shape, true);
+	}
+
+	TEST_CASE("DiscreteCubeSlideHeightField")
+	{
+		Ref<ShapeSettings> shape = sCreateHeightFieldShape();
+
+		sDiscreteCubeSlide(shape, false);
+
+		sDiscreteCubeSlide(shape, true);
+
+		Ref<ShapeSettings> scaled_shape = new ScaledShapeSettings(shape, Vec3(-1, 1, 1));
+
+		sDiscreteCubeSlide(scaled_shape, false);
+
+		sDiscreteCubeSlide(scaled_shape, true);
+	}
+
+	// Tests a linear cast cube sliding over a mesh / heightfield shape
+	static void sLinearCastCubeSlide(Ref<ShapeSettings> inShape, bool inCheckActiveEdges)
+	{
+		PhysicsTestContext c;
+
+		const float cPenetrationSlop = c.GetSystem()->GetPhysicsSettings().mPenetrationSlop;
+
+		// Set simulation settings
+		PhysicsSettings settings;
+		settings.mCheckActiveEdges = inCheckActiveEdges;
+		c.GetSystem()->SetPhysicsSettings(settings);
+
+		// Create frictionless floor
+		Body &floor = c.CreateBody(inShape, RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, EActivation::DontActivate);
+		floor.SetFriction(0.0f);
+
+		// Create box starting a little bit above the floor and ending 0.5 * cPenetrationSlop below the floor so that if no internal edges are hit the motion should not be stopped
+		// Note that we need the vertical velocity or else back face culling will ignore the face
+		RVec3 initial_position(-3, 0.1f + cPenetrationSlop, 0);
+		Vec3 initial_velocity(6 * 60, -1.5f * cPenetrationSlop * 60, 0);
+		Body &box = c.CreateBox(initial_position, Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::LinearCast, Layers::MOVING, Vec3::sReplicate(0.1f));
+		box.SetLinearVelocity(initial_velocity);
+		box.SetFriction(0.0f);
+		box.GetMotionProperties()->SetLinearDamping(0.0f);
+
+		// To avoid extra vertical velocity being picked up in 1 step, zero gravity
+		c.ZeroGravity();
+
+		c.SimulateSingleStep();
+
+		RVec3 expected_position = initial_position + initial_velocity / 60.0f;
+		if (inCheckActiveEdges)
+		{
+			// Box should stepped in one frame over the plane without encountering any linear cast collisions
+			CHECK_APPROX_EQUAL(box.GetPosition(), expected_position, 1.0e-4f);
+			CHECK_APPROX_EQUAL(box.GetLinearVelocity(), initial_velocity, 1.0e-4f);
+		}
+		else
+		{
+			// Box should have bumped into an internal edge and not reached its target
+			CHECK(box.GetPosition().GetX() < expected_position.GetX() - 1.0f);
+		}
+	}
+
+	TEST_CASE("LinearCastCubeSlideMesh")
+	{
+		Ref<ShapeSettings> shape = sCreateMeshShape();
+
+		sLinearCastCubeSlide(shape, false);
+
+		sLinearCastCubeSlide(shape, true);
+
+		Ref<ShapeSettings> scaled_shape = new ScaledShapeSettings(shape, Vec3(-1, 1, 1));
+
+		sLinearCastCubeSlide(scaled_shape, false);
+
+		sLinearCastCubeSlide(scaled_shape, true);
+	}
+
+	TEST_CASE("LinearCastCubeSlideHeightField")
+	{
+		Ref<ShapeSettings> shape = sCreateHeightFieldShape();
+
+		sLinearCastCubeSlide(shape, false);
+
+		sLinearCastCubeSlide(shape, true);
+
+		Ref<ShapeSettings> scaled_shape = new ScaledShapeSettings(shape, Vec3(-1, 1, 1));
+
+		sLinearCastCubeSlide(scaled_shape, false);
+
+		sLinearCastCubeSlide(scaled_shape, true);
+	}
+
+	TEST_CASE("TestNonManifoldMesh")
+	{
+		// Test 3 triangles in a plane that all share the same edge
+		// Normally the shared edge would not be active, but since the mesh is non-manifold we expect all of them to be active
+		TriangleList triangles;
+		triangles.push_back(Triangle(Float3(0, 0, -1), Float3(0, 0, 1), Float3(1, 0, 0), 0));
+		triangles.push_back(Triangle(Float3(0, 0, 1), Float3(0, 0, -1), Float3(-1, 0, 0), 0));
+		triangles.push_back(Triangle(Float3(0, 0, 1), Float3(0, 0, -1), Float3(-0.5f, 0, 0), 0));
+
+		ShapeRefC shape = MeshShapeSettings(triangles).Create().Get();
+
+		ShapeRefC sphere = new SphereShape(0.1f);
+
+		CollideShapeSettings settings;
+		settings.mActiveEdgeMode = EActiveEdgeMode::CollideOnlyWithActive;
+
+		// Collide a sphere on both sides of the active edge so that a 45 degree normal will be found then the edge is active.
+		// An inactive edge will return a normal that is perpendicular to the plane.
+		{
+			AllHitCollisionCollector<CollideShapeCollector> collector;
+			CollisionDispatch::sCollideShapeVsShape(sphere, shape, Vec3::sOne(), Vec3::sOne(), Mat44::sTranslation(Vec3(0.05f, 0.05f, 0)), Mat44::sIdentity(), SubShapeIDCreator(), SubShapeIDCreator(), settings, collector);
+			CHECK(collector.mHits.size() == 3);
+
+			// We expect one interior hit because the sphere is above the triangle and 2 active edge hits that provide a normal pointing towards the sphere
+			int num_interior = 0, num_on_shared_edge = 0;
+			for (const CollideShapeResult &r : collector.mHits)
+				if (r.mContactPointOn2.IsClose(Vec3(0.05f, 0.0f, 0.0f)))
+				{
+					CHECK_APPROX_EQUAL(r.mPenetrationAxis.Normalized(), Vec3(0, -1, 0));
+					++num_interior;
+				}
+				else if (r.mContactPointOn2.IsNearZero())
+				{
+					CHECK_APPROX_EQUAL(r.mPenetrationAxis.Normalized(), Vec3(-1, -1, 0).Normalized(), 1.0e-5f);
+					++num_on_shared_edge;
+				}
+			CHECK(num_interior == 1);
+			CHECK(num_on_shared_edge == 2);
+		}
+
+		{
+			AllHitCollisionCollector<CollideShapeCollector> collector;
+			CollisionDispatch::sCollideShapeVsShape(sphere, shape, Vec3::sOne(), Vec3::sOne(), Mat44::sTranslation(Vec3(-0.05f, 0.05f, 0)), Mat44::sIdentity(), SubShapeIDCreator(), SubShapeIDCreator(), settings, collector);
+			CHECK(collector.mHits.size() == 3);
+
+			// We expect 2 interior hits because the sphere is above the triangle and 1 active edge hit that provide a normal pointing towards the sphere
+			int num_interior = 0, num_on_shared_edge = 0;
+			for (const CollideShapeResult &r : collector.mHits)
+				if (r.mContactPointOn2.IsClose(Vec3(-0.05f, 0.0f, 0.0f)))
+				{
+					CHECK_APPROX_EQUAL(r.mPenetrationAxis.Normalized(), Vec3(0, -1, 0));
+					++num_interior;
+				}
+				else if (r.mContactPointOn2.IsNearZero())
+				{
+					CHECK_APPROX_EQUAL(r.mPenetrationAxis.Normalized(), Vec3(1, -1, 0).Normalized(), 1.0e-5f);
+					++num_on_shared_edge;
+				}
+			CHECK(num_interior == 2);
+			CHECK(num_on_shared_edge == 1);
+		}
+	}
+}

lib/All/JoltPhysics/UnitTests/Physics/BroadPhaseTests.cpp

@@ -0,0 +1,115 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include <Jolt/Physics/Collision/BroadPhase/BroadPhaseQuadTree.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Collision/CollisionCollectorImpl.h>
+#include <Jolt/Physics/Collision/RayCast.h>
+#include <Jolt/Physics/Collision/CastResult.h>
+#include <Jolt/Physics/Body/BodyManager.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include "Layers.h"
+
+TEST_SUITE("BroadPhaseTests")
+{
+	TEST_CASE("TestBroadPhaseOptimize")
+	{
+		BPLayerInterfaceImpl broad_phase_layer_interface;
+
+		// Create body manager
+		BodyManager body_manager;
+		body_manager.Init(1, 0, broad_phase_layer_interface);
+
+		// Create quad tree
+		BroadPhaseQuadTree broadphase;
+		broadphase.Init(&body_manager, broad_phase_layer_interface);
+
+		// Create a box
+		BodyCreationSettings settings(new BoxShape(Vec3::sOne()), RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING);
+		Body &body = *body_manager.AllocateBody(settings);
+		body_manager.AddBody(&body);
+
+		// Add it to the broadphase
+		BodyID id = body.GetID();
+		BroadPhase::AddState add_state = broadphase.AddBodiesPrepare(&id, 1);
+		broadphase.AddBodiesFinalize(&id, 1, add_state);
+
+		// Test that we hit the box at its current location and not where we're going to move it to
+		AllHitCollisionCollector<RayCastBodyCollector> collector;
+		broadphase.CastRay({ Vec3(0, 2, 0), Vec3(0, -2, 0) }, collector, BroadPhaseLayerFilter(), ObjectLayerFilter());
+		CHECK(collector.mHits.size() == 1);
+		CHECK(collector.mHits[0].mBodyID == id);
+		CHECK_APPROX_EQUAL(collector.mHits[0].mFraction, 0.5f);
+		collector.Reset();
+		broadphase.CastRay({ Vec3(2, 2, 0), Vec3(0, -2, 0) }, collector, BroadPhaseLayerFilter(), ObjectLayerFilter());
+		CHECK(collector.mHits.empty());
+		broadphase.CastRay({ Vec3(4, 2, 0), Vec3(0, -2, 0) }, collector, BroadPhaseLayerFilter(), ObjectLayerFilter());
+		CHECK(collector.mHits.empty());
+
+		// Move the body
+		body.SetPositionAndRotationInternal(RVec3(2, 0, 0), Quat::sIdentity());
+		broadphase.NotifyBodiesAABBChanged(&id, 1, true);
+
+		// Test that we hit the box at its previous and current location
+		broadphase.CastRay({ Vec3(0, 2, 0), Vec3(0, -2, 0) }, collector, BroadPhaseLayerFilter(), ObjectLayerFilter());
+		CHECK(collector.mHits.size() == 1);
+		CHECK(collector.mHits[0].mBodyID == id);
+		CHECK_APPROX_EQUAL(collector.mHits[0].mFraction, 0.5f);
+		collector.Reset();
+		broadphase.CastRay({ Vec3(2, 2, 0), Vec3(0, -2, 0) }, collector, BroadPhaseLayerFilter(), ObjectLayerFilter());
+		CHECK(collector.mHits.size() == 1);
+		CHECK(collector.mHits[0].mBodyID == id);
+		CHECK_APPROX_EQUAL(collector.mHits[0].mFraction, 0.5f);
+		collector.Reset();
+		broadphase.CastRay({ Vec3(4, 2, 0), Vec3(0, -2, 0) }, collector, BroadPhaseLayerFilter(), ObjectLayerFilter());
+		CHECK(collector.mHits.empty());
+
+		// Optimize the broadphase
+		broadphase.Optimize();
+
+		// Test that we hit the box only at the new location
+		broadphase.CastRay({ Vec3(0, 2, 0), Vec3(0, -2, 0) }, collector, BroadPhaseLayerFilter(), ObjectLayerFilter());
+		CHECK(collector.mHits.empty());
+		broadphase.CastRay({ Vec3(2, 2, 0), Vec3(0, -2, 0) }, collector, BroadPhaseLayerFilter(), ObjectLayerFilter());
+		CHECK(collector.mHits.size() == 1);
+		CHECK(collector.mHits[0].mBodyID == id);
+		CHECK_APPROX_EQUAL(collector.mHits[0].mFraction, 0.5f);
+		collector.Reset();
+		broadphase.CastRay({ Vec3(4, 2, 0), Vec3(0, -2, 0) }, collector, BroadPhaseLayerFilter(), ObjectLayerFilter());
+		CHECK(collector.mHits.empty());
+
+		// Move the body again (so that for the next optimize we'll have to discard a tree)
+		body.SetPositionAndRotationInternal(RVec3(4, 0, 0), Quat::sIdentity());
+		broadphase.NotifyBodiesAABBChanged(&id, 1, true);
+
+		// Test that we hit the box at its previous and current location
+		broadphase.CastRay({ Vec3(0, 2, 0), Vec3(0, -2, 0) }, collector, BroadPhaseLayerFilter(), ObjectLayerFilter());
+		CHECK(collector.mHits.empty());
+		broadphase.CastRay({ Vec3(2, 2, 0), Vec3(0, -2, 0) }, collector, BroadPhaseLayerFilter(), ObjectLayerFilter());
+		CHECK(collector.mHits.size() == 1);
+		CHECK(collector.mHits[0].mBodyID == id);
+		CHECK_APPROX_EQUAL(collector.mHits[0].mFraction, 0.5f);
+		collector.Reset();
+		broadphase.CastRay({ Vec3(4, 2, 0), Vec3(0, -2, 0) }, collector, BroadPhaseLayerFilter(), ObjectLayerFilter());
+		CHECK(collector.mHits.size() == 1);
+		CHECK(collector.mHits[0].mBodyID == id);
+		CHECK_APPROX_EQUAL(collector.mHits[0].mFraction, 0.5f);
+		collector.Reset();
+
+		// Optimize the broadphase (this will internally have to discard a tree)
+		broadphase.Optimize();
+
+		// Test that we hit the box only at the new location
+		broadphase.CastRay({ Vec3(0, 2, 0), Vec3(0, -2, 0) }, collector, BroadPhaseLayerFilter(), ObjectLayerFilter());
+		CHECK(collector.mHits.empty());
+		broadphase.CastRay({ Vec3(2, 2, 0), Vec3(0, -2, 0) }, collector, BroadPhaseLayerFilter(), ObjectLayerFilter());
+		CHECK(collector.mHits.empty());
+		broadphase.CastRay({ Vec3(4, 2, 0), Vec3(0, -2, 0) }, collector, BroadPhaseLayerFilter(), ObjectLayerFilter());
+		CHECK(collector.mHits.size() == 1);
+		CHECK(collector.mHits[0].mBodyID == id);
+		CHECK_APPROX_EQUAL(collector.mHits[0].mFraction, 0.5f);
+		collector.Reset();
+	}
+}

lib/All/JoltPhysics/UnitTests/Physics/CastShapeTests.cpp

@@ -0,0 +1,514 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include <Jolt/Physics/Collision/ShapeCast.h>
+#include <Jolt/Physics/Collision/CastResult.h>
+#include <Jolt/Physics/Collision/CollisionCollectorImpl.h>
+#include <Jolt/Physics/Collision/Shape/SphereShape.h>
+#include <Jolt/Physics/Collision/Shape/StaticCompoundShape.h>
+#include <Jolt/Physics/Collision/Shape/CapsuleShape.h>
+#include <Jolt/Physics/Collision/Shape/TriangleShape.h>
+#include <Jolt/Physics/Collision/Shape/MeshShape.h>
+#include <Jolt/Physics/Collision/Shape/ScaledShape.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Collision/Shape/ConvexHullShape.h>
+#include <Jolt/Physics/Collision/ShapeFilter.h>
+#include <Jolt/Physics/Collision/CollisionDispatch.h>
+#include <Jolt/Physics/Collision/CastSphereVsTriangles.h>
+#include "PhysicsTestContext.h"
+#include "Layers.h"
+
+TEST_SUITE("CastShapeTests")
+{
+	/// Helper function that tests a sphere against a triangle
+	static void sTestCastSphereVertexOrEdge(const Shape *inSphere, Vec3Arg inPosition, Vec3Arg inDirection, const Shape *inTriangle)
+	{
+		ShapeCast shape_cast(inSphere, Vec3::sOne(), Mat44::sTranslation(inPosition - inDirection), inDirection);
+		ShapeCastSettings cast_settings;
+		cast_settings.mBackFaceModeTriangles = EBackFaceMode::CollideWithBackFaces;
+		cast_settings.mBackFaceModeConvex = EBackFaceMode::CollideWithBackFaces;
+		AllHitCollisionCollector<CastShapeCollector> collector;
+		CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, cast_settings, inTriangle, Vec3::sOne(), ShapeFilter(), Mat44::sIdentity(), SubShapeIDCreator(), SubShapeIDCreator(), collector);
+		CHECK(collector.mHits.size() == 1);
+		const ShapeCastResult &result = collector.mHits.back();
+		CHECK_APPROX_EQUAL(result.mFraction, 1.0f - 0.2f / inDirection.Length(), 1.0e-4f);
+		CHECK_APPROX_EQUAL(result.mPenetrationAxis.Normalized(), inDirection.Normalized(), 1.0e-3f);
+		CHECK_APPROX_EQUAL(result.mPenetrationDepth, 0.0f, 1.0e-3f);
+		CHECK_APPROX_EQUAL(result.mContactPointOn1, inPosition, 1.0e-3f);
+		CHECK_APPROX_EQUAL(result.mContactPointOn2, inPosition, 1.0e-3f);
+	}
+
+	/// Helper function that tests a sphere against a triangle centered on the origin with normal Z
+	static void sTestCastSphereTriangle(const Shape *inTriangle)
+	{
+		// Create sphere
+		Ref<Shape> sphere = SphereShapeSettings(0.2f).Create().Get();
+
+		{
+			// Hit front face
+			ShapeCast shape_cast(sphere, Vec3::sOne(), Mat44::sTranslation(Vec3(0, 0, 15)), Vec3(0, 0, -30));
+			ShapeCastSettings cast_settings;
+			cast_settings.mBackFaceModeTriangles = EBackFaceMode::IgnoreBackFaces;
+			cast_settings.mBackFaceModeConvex = EBackFaceMode::IgnoreBackFaces;
+			cast_settings.mReturnDeepestPoint = false;
+			AllHitCollisionCollector<CastShapeCollector> collector;
+			CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, cast_settings, inTriangle, Vec3::sOne(), ShapeFilter(), Mat44::sIdentity(), SubShapeIDCreator(), SubShapeIDCreator(), collector);
+			CHECK(collector.mHits.size() == 1);
+			const ShapeCastResult &result = collector.mHits.back();
+			CHECK_APPROX_EQUAL(result.mFraction, (15.0f - 0.2f) / 30.0f, 1.0e-4f);
+			CHECK_APPROX_EQUAL(result.mPenetrationAxis.Normalized(), Vec3(0, 0, -1), 1.0e-3f);
+			CHECK(result.mPenetrationDepth == 0.0f);
+			CHECK_APPROX_EQUAL(result.mContactPointOn1, Vec3::sZero(), 1.0e-3f);
+			CHECK_APPROX_EQUAL(result.mContactPointOn2, Vec3::sZero(), 1.0e-3f);
+			CHECK(!result.mIsBackFaceHit);
+		}
+
+		{
+			// Hit back face -> ignored
+			ShapeCast shape_cast(sphere, Vec3::sOne(), Mat44::sTranslation(Vec3(0, 0, -15)), Vec3(0, 0, 30));
+			ShapeCastSettings cast_settings;
+			cast_settings.mBackFaceModeTriangles = EBackFaceMode::IgnoreBackFaces;
+			cast_settings.mBackFaceModeConvex = EBackFaceMode::IgnoreBackFaces;
+			cast_settings.mReturnDeepestPoint = false;
+			AllHitCollisionCollector<CastShapeCollector> collector;
+			CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, cast_settings, inTriangle, Vec3::sOne(), ShapeFilter(), Mat44::sIdentity(), SubShapeIDCreator(), SubShapeIDCreator(), collector);
+			CHECK(collector.mHits.empty());
+
+			// Hit back face -> collision
+			cast_settings.mBackFaceModeTriangles = EBackFaceMode::CollideWithBackFaces;
+			cast_settings.mBackFaceModeConvex = EBackFaceMode::CollideWithBackFaces;
+			CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, cast_settings, inTriangle, Vec3::sOne(), ShapeFilter(), Mat44::sIdentity(), SubShapeIDCreator(), SubShapeIDCreator(), collector);
+			CHECK(collector.mHits.size() == 1);
+			const ShapeCastResult &result = collector.mHits.back();
+			CHECK_APPROX_EQUAL(result.mFraction, (15.0f - 0.2f) / 30.0f, 1.0e-4f);
+			CHECK_APPROX_EQUAL(result.mPenetrationAxis.Normalized(), Vec3(0, 0, 1), 1.0e-3f);
+			CHECK(result.mPenetrationDepth == 0.0f);
+			CHECK_APPROX_EQUAL(result.mContactPointOn1, Vec3::sZero(), 1.0e-3f);
+			CHECK_APPROX_EQUAL(result.mContactPointOn2, Vec3::sZero(), 1.0e-3f);
+			CHECK(result.mIsBackFaceHit);
+		}
+
+		{
+			// Hit back face while starting in collision -> ignored
+			ShapeCast shape_cast(sphere, Vec3::sOne(), Mat44::sTranslation(Vec3(0, 0, -0.1f)), Vec3(0, 0, 15));
+			ShapeCastSettings cast_settings;
+			cast_settings.mBackFaceModeTriangles = EBackFaceMode::IgnoreBackFaces;
+			cast_settings.mBackFaceModeConvex = EBackFaceMode::IgnoreBackFaces;
+			cast_settings.mReturnDeepestPoint = true;
+			AllHitCollisionCollector<CastShapeCollector> collector;
+			CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, cast_settings, inTriangle, Vec3::sOne(), ShapeFilter(), Mat44::sIdentity(), SubShapeIDCreator(), SubShapeIDCreator(), collector);
+			CHECK(collector.mHits.empty());
+
+			// Hit back face while starting in collision -> collision
+			cast_settings.mBackFaceModeTriangles = EBackFaceMode::CollideWithBackFaces;
+			cast_settings.mBackFaceModeConvex = EBackFaceMode::CollideWithBackFaces;
+			CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, cast_settings, inTriangle, Vec3::sOne(), ShapeFilter(), Mat44::sIdentity(), SubShapeIDCreator(), SubShapeIDCreator(), collector);
+			CHECK(collector.mHits.size() == 1);
+			const ShapeCastResult &result = collector.mHits.back();
+			CHECK_APPROX_EQUAL(result.mFraction, 0.0f);
+			CHECK_APPROX_EQUAL(result.mPenetrationAxis.Normalized(), Vec3(0, 0, 1), 1.0e-3f);
+			CHECK_APPROX_EQUAL(result.mPenetrationDepth, 0.1f, 1.0e-3f);
+			CHECK_APPROX_EQUAL(result.mContactPointOn1, Vec3(0, 0, 0.1f), 1.0e-3f);
+			CHECK_APPROX_EQUAL(result.mContactPointOn2, Vec3::sZero(), 1.0e-3f);
+			CHECK(result.mIsBackFaceHit);
+		}
+
+		// Hit vertex 1, 2 and 3
+		sTestCastSphereVertexOrEdge(sphere, Vec3(50, 25, 0), Vec3(-10, -10, 0), inTriangle);
+		sTestCastSphereVertexOrEdge(sphere, Vec3(-50, 25, 0), Vec3(10, -10, 0), inTriangle);
+		sTestCastSphereVertexOrEdge(sphere, Vec3(0, -25, 0), Vec3(0, 10, 0), inTriangle);
+
+		// Hit edge 1, 2 and 3
+		sTestCastSphereVertexOrEdge(sphere, Vec3(0, 25, 0), Vec3(0, -10, 0), inTriangle); // Edge: Vec3(50, 25, 0), Vec3(-50, 25, 0)
+		sTestCastSphereVertexOrEdge(sphere, Vec3(-25, 0, 0), Vec3(10, 10, 0), inTriangle); // Edge: Vec3(-50, 25, 0), Vec3(0,-25, 0)
+		sTestCastSphereVertexOrEdge(sphere, Vec3(25, 0, 0), Vec3(-10, 10, 0), inTriangle); // Edge: Float3(0,-25, 0), Float3(50, 25, 0)
+	}
+
+	TEST_CASE("TestCastSphereTriangle")
+	{
+		// Create triangle
+		Ref<Shape> triangle = TriangleShapeSettings(Vec3(50, 25, 0), Vec3(-50, 25, 0), Vec3(0,-25, 0)).Create().Get();
+		sTestCastSphereTriangle(triangle);
+
+		// Create a triangle mesh shape
+		Ref<Shape> triangle_mesh = MeshShapeSettings({ Triangle(Float3(50, 25, 0), Float3(-50, 25, 0), Float3(0,-25, 0)) }).Create().Get();
+		sTestCastSphereTriangle(triangle_mesh);
+	}
+
+	// Test CastShape for a (scaled) sphere vs box
+	TEST_CASE("TestCastShapeSphereVsBox")
+	{
+		PhysicsTestContext c;
+
+		// Create box to collide against (shape 2)
+		// The box is scaled up by a factor 10 in the X axis and then rotated so that the X axis is up
+		BoxShapeSettings box(Vec3::sOne());
+		box.SetEmbedded();
+		ScaledShapeSettings scaled_box(&box, Vec3(10, 1, 1));
+		scaled_box.SetEmbedded();
+		Body &body2 = c.CreateBody(&scaled_box, RVec3(0, 1, 0), Quat::sRotation(Vec3::sAxisZ(), 0.5f * JPH_PI), EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, EActivation::DontActivate);
+
+		// Set settings
+		ShapeCastSettings settings;
+		settings.mReturnDeepestPoint = true;
+		settings.mBackFaceModeTriangles = EBackFaceMode::CollideWithBackFaces;
+		settings.mBackFaceModeConvex = EBackFaceMode::CollideWithBackFaces;
+
+		{
+			// Create shape cast
+			Ref<Shape> normal_sphere = new SphereShape(1.0f);
+			RShapeCast shape_cast { normal_sphere, Vec3::sOne(), RMat44::sTranslation(RVec3(0, 11, 0)), Vec3(0, 1, 0) };
+
+			// Shape is intersecting at the start
+			AllHitCollisionCollector<CastShapeCollector> collector;
+			c.GetSystem()->GetNarrowPhaseQuery().CastShape(shape_cast, settings, RVec3::sZero(), collector);
+			CHECK(collector.mHits.size() == 1);
+			const ShapeCastResult &result = collector.mHits.front();
+			CHECK(result.mBodyID2 == body2.GetID());
+			CHECK_APPROX_EQUAL(result.mFraction, 0.0f);
+			CHECK_APPROX_EQUAL(result.mPenetrationAxis.Normalized(), Vec3(0, -1, 0), 1.0e-3f);
+			CHECK_APPROX_EQUAL(result.mPenetrationDepth, 1.0f, 1.0e-5f);
+			CHECK_APPROX_EQUAL(result.mContactPointOn1, Vec3(0, 10, 0), 1.0e-3f);
+			CHECK_APPROX_EQUAL(result.mContactPointOn2, Vec3(0, 11, 0), 1.0e-3f);
+			CHECK(!result.mIsBackFaceHit);
+		}
+
+		{
+			// This repeats the same test as above but uses scaling at all levels and validate that the penetration depth is still correct
+			Ref<Shape> scaled_sphere = new ScaledShape(new SphereShape(0.1f), Vec3::sReplicate(5.0f));
+			RShapeCast shape_cast { scaled_sphere, Vec3::sReplicate(2.0f), RMat44::sTranslation(RVec3(0, 11, 0)), Vec3(0, 1, 0) };
+
+			// Shape is intersecting at the start
+			AllHitCollisionCollector<CastShapeCollector> collector;
+			c.GetSystem()->GetNarrowPhaseQuery().CastShape(shape_cast, settings, RVec3::sZero(), collector);
+			CHECK(collector.mHits.size() == 1);
+			const ShapeCastResult &result = collector.mHits.front();
+			CHECK(result.mBodyID2 == body2.GetID());
+			CHECK_APPROX_EQUAL(result.mFraction, 0.0f);
+			CHECK_APPROX_EQUAL(result.mPenetrationAxis.Normalized(), Vec3(0, -1, 0), 1.0e-3f);
+			CHECK_APPROX_EQUAL(result.mPenetrationDepth, 1.0f, 1.0e-5f);
+			CHECK_APPROX_EQUAL(result.mContactPointOn1, Vec3(0, 10, 0), 1.0e-3f);
+			CHECK_APPROX_EQUAL(result.mContactPointOn2, Vec3(0, 11, 0), 1.0e-3f);
+			CHECK(!result.mIsBackFaceHit);
+		}
+	}
+
+	// Test CastShape ordering according to penetration depth
+	TEST_CASE("TestCastShapePenetrationDepthOrdering")
+	{
+		PhysicsTestContext c;
+
+		// Create box to collide against (shape 2)
+		BoxShapeSettings box(Vec3(0.1f, 2.0f, 2.0f));
+		box.SetEmbedded();
+
+		// Create 10 boxes that are 0.2 thick in the X axis and 4 in Y and Z, put them all next to each other on the X axis starting from X = 0 going to X = 2
+		Array<Body *> bodies;
+		for (int i = 0; i < 10; ++i)
+			bodies.push_back(&c.CreateBody(&box, RVec3(0.1f + 0.2f * i, 0, 0), Quat::sIdentity(), EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, EActivation::DontActivate));
+
+		// Set settings
+		ShapeCastSettings settings;
+		settings.mReturnDeepestPoint = true;
+		settings.mBackFaceModeTriangles = EBackFaceMode::CollideWithBackFaces;
+		settings.mBackFaceModeConvex = EBackFaceMode::CollideWithBackFaces;
+		settings.mCollisionTolerance = 1.0e-5f; // Increased precision
+		settings.mPenetrationTolerance = 1.0e-5f;
+
+		{
+			// Create shape cast in X from -5 to 5
+			RefConst<Shape> sphere = new SphereShape(1.0f);
+			RShapeCast shape_cast { sphere, Vec3::sOne(), RMat44::sTranslation(RVec3(-5, 0, 0)), Vec3(10, 0, 0) };
+
+			// We should hit the first body
+			ClosestHitCollisionCollector<CastShapeCollector> collector;
+			c.GetSystem()->GetNarrowPhaseQuery().CastShape(shape_cast, settings, RVec3::sZero(), collector);
+			CHECK(collector.HadHit());
+			CHECK(collector.mHit.mBodyID2 == bodies.front()->GetID());
+			CHECK_APPROX_EQUAL(collector.mHit.mFraction, 4.0f / 10.0f);
+			CHECK(collector.mHit.mPenetrationAxis.Normalized().Dot(Vec3(1, 0, 0)) > Cos(DegreesToRadians(1.0f)));
+			CHECK_APPROX_EQUAL(collector.mHit.mPenetrationDepth, 0.0f);
+			CHECK_APPROX_EQUAL(collector.mHit.mContactPointOn1, Vec3(0, 0, 0), 2.0e-3f);
+			CHECK_APPROX_EQUAL(collector.mHit.mContactPointOn2, Vec3(0, 0, 0), 2.0e-3f);
+			CHECK(!collector.mHit.mIsBackFaceHit);
+		}
+
+		{
+			// Create shape cast in X from 5 to -5
+			RefConst<Shape> sphere = new SphereShape(1.0f);
+			RShapeCast shape_cast { sphere, Vec3::sOne(), RMat44::sTranslation(RVec3(5, 0, 0)), Vec3(-10, 0, 0) };
+
+			// We should hit the last body
+			ClosestHitCollisionCollector<CastShapeCollector> collector;
+			c.GetSystem()->GetNarrowPhaseQuery().CastShape(shape_cast, settings, RVec3::sZero(), collector);
+			CHECK(collector.HadHit());
+			CHECK(collector.mHit.mBodyID2 == bodies.back()->GetID());
+			CHECK_APPROX_EQUAL(collector.mHit.mFraction, 2.0f / 10.0f, 1.0e-4f);
+			CHECK(collector.mHit.mPenetrationAxis.Normalized().Dot(Vec3(-1, 0, 0)) > Cos(DegreesToRadians(1.0f)));
+			CHECK_APPROX_EQUAL(collector.mHit.mPenetrationDepth, 0.0f);
+			CHECK_APPROX_EQUAL(collector.mHit.mContactPointOn1, Vec3(2, 0, 0), 4.0e-4f);
+			CHECK_APPROX_EQUAL(collector.mHit.mContactPointOn2, Vec3(2, 0, 0), 4.0e-4f);
+			CHECK(!collector.mHit.mIsBackFaceHit);
+		}
+
+		{
+			// Create shape cast in X from 1.05 to 11, this should intersect with all bodies and have deepest penetration in bodies[5]
+			RefConst<Shape> sphere = new SphereShape(1.0f);
+			RShapeCast shape_cast { sphere, Vec3::sOne(), RMat44::sTranslation(RVec3(1.05_r, 0, 0)), Vec3(10, 0, 0) };
+
+			// We should hit bodies[5]
+			AllHitCollisionCollector<CastShapeCollector> collector;
+			c.GetSystem()->GetNarrowPhaseQuery().CastShape(shape_cast, settings, RVec3::sZero(), collector);
+			collector.Sort();
+			CHECK(collector.mHits.size() == 10);
+			const ShapeCastResult &result = collector.mHits.front();
+			CHECK(result.mBodyID2 == bodies[5]->GetID());
+			CHECK_APPROX_EQUAL(result.mFraction, 0.0f);
+			CHECK(result.mPenetrationAxis.Normalized().Dot(Vec3(1, 0, 0)) > Cos(DegreesToRadians(1.0f)));
+			CHECK_APPROX_EQUAL(result.mPenetrationDepth, 1.05f);
+			CHECK_APPROX_EQUAL(result.mContactPointOn1, Vec3(2.05f, 0, 0), 2.0e-5f); // Box starts at 1.0, center of sphere adds 0.05, radius of sphere is 1
+			CHECK_APPROX_EQUAL(result.mContactPointOn2, Vec3(1.0f, 0, 0), 2.0e-5f); // Box starts at 1.0
+			CHECK(!result.mIsBackFaceHit);
+		}
+	}
+
+	// Test casting a capsule against a mesh that is intersecting at fraction 0 and test that it returns the deepest penetration
+	TEST_CASE("TestDeepestPenetrationAtFraction0")
+	{
+		// Create an n x n grid of triangles
+		const int n = 10;
+		const float s = 0.1f;
+		TriangleList triangles;
+		for (int z = 0; z < n; ++z)
+			for (int x = 0; x < n; ++x)
+			{
+				float fx = s * x - s * n / 2, fz = s * z - s * n / 2;
+				triangles.push_back(Triangle(Vec3(fx, 0, fz), Vec3(fx, 0, fz + s), Vec3(fx + s, 0, fz + s)));
+				triangles.push_back(Triangle(Vec3(fx, 0, fz), Vec3(fx + s, 0, fz + s), Vec3(fx + s, 0, fz)));
+			}
+		MeshShapeSettings mesh_settings(triangles);
+		mesh_settings.SetEmbedded();
+
+		// Create a compound shape with two copies of the mesh
+		StaticCompoundShapeSettings compound_settings;
+		compound_settings.AddShape(Vec3::sZero(), Quat::sIdentity(), &mesh_settings);
+		compound_settings.AddShape(Vec3(0, -0.01f, 0), Quat::sIdentity(), &mesh_settings); // This will not result in the deepest penetration
+		compound_settings.SetEmbedded();
+
+		// Add it to the scene
+		PhysicsTestContext c;
+		c.CreateBody(&compound_settings, RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, EActivation::DontActivate);
+
+		// Add the same compound a little bit lower (this will not result in the deepest penetration)
+		c.CreateBody(&compound_settings, RVec3(0, -0.1_r, 0), Quat::sIdentity(), EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, EActivation::DontActivate);
+
+		// We want the deepest hit
+		ShapeCastSettings cast_settings;
+		cast_settings.mReturnDeepestPoint = true;
+
+		// Create capsule to test
+		const float capsule_half_height = 2.0f;
+		const float capsule_radius = 1.0f;
+		RefConst<Shape> cast_shape = new CapsuleShape(capsule_half_height, capsule_radius);
+
+		// Cast the shape starting inside the mesh with a long distance so that internally in the mesh shape the RayAABox4 test will return a low negative fraction.
+		// This used to be confused with the penetration depth and would cause an early out and return the wrong result.
+		const float capsule_offset = 0.1f;
+		RShapeCast shape_cast(cast_shape, Vec3::sOne(), RMat44::sTranslation(RVec3(0, capsule_half_height + capsule_offset, 0)), Vec3(0, -100, 0));
+
+		// Cast first using the closest hit collector
+		ClosestHitCollisionCollector<CastShapeCollector> collector;
+		c.GetSystem()->GetNarrowPhaseQuery().CastShape(shape_cast, cast_settings, RVec3::sZero(), collector);
+
+		// Check that it indeed found a hit at fraction 0 with the deepest penetration of all triangles
+		CHECK(collector.HadHit());
+		CHECK(collector.mHit.mFraction == 0.0f);
+		CHECK_APPROX_EQUAL(collector.mHit.mPenetrationDepth, capsule_radius - capsule_offset, 1.0e-4f);
+		CHECK_APPROX_EQUAL(collector.mHit.mPenetrationAxis.Normalized(), Vec3(0, -1, 0));
+		CHECK_APPROX_EQUAL(collector.mHit.mContactPointOn2, Vec3::sZero());
+
+		// Cast again while triggering a force early out after the first hit
+		class MyCollector : public CastShapeCollector
+		{
+		public:
+			virtual void	AddHit(const ShapeCastResult &inResult) override
+			{
+				++mNumHits;
+				ForceEarlyOut();
+			}
+
+			int				mNumHits = 0;
+		};
+		MyCollector collector2;
+		c.GetSystem()->GetNarrowPhaseQuery().CastShape(shape_cast, cast_settings, RVec3::sZero(), collector2);
+
+		// Ensure that we indeed stopped after the first hit
+		CHECK(collector2.mNumHits == 1);
+	}
+
+	// Test a problem case where a sphere cast would incorrectly hit a degenerate triangle (see: https://github.com/jrouwe/JoltPhysics/issues/886)
+	TEST_CASE("TestCastSphereVsDegenerateTriangle")
+	{
+		AllHitCollisionCollector<CastShapeCollector> collector;
+		SphereShape sphere(0.2f);
+		sphere.SetEmbedded();
+		ShapeCast cast(&sphere, Vec3::sOne(), Mat44::sTranslation(Vec3(14.8314590f, 8.19055080f, -4.30825043f)), Vec3(-0.0988006592f, 5.96046448e-08f, 0.000732421875f));
+		ShapeCastSettings settings;
+		CastSphereVsTriangles caster(cast, settings, Vec3::sOne(), Mat44::sIdentity(), { }, collector);
+		caster.Cast(Vec3(14.5536213f, 10.5973721f, -0.00600051880f), Vec3(14.5536213f, 10.5969315f, -3.18638134f), Vec3(14.5536213f, 10.5969315f, -5.18637228f), 0b111, SubShapeID());
+		CHECK(!collector.HadHit());
+	}
+
+	// Test ClosestHitPerBodyCollisionCollector
+	TEST_CASE("TestClosestHitPerBodyCollisionCollector")
+	{
+		PhysicsTestContext c;
+
+		// Create a 1 by 1 by 1 box consisting of 10 slabs
+		StaticCompoundShapeSettings compound_settings;
+		compound_settings.SetEmbedded();
+		for (int i = 0; i < 10; ++i)
+			compound_settings.AddShape(Vec3(0.1f * i - 0.45f, 0, 0), Quat::sIdentity(), new BoxShape(Vec3(0.05f, 0.5f, 0.5f)));
+
+		// Create 2 instances
+		Body &body1 = c.CreateBody(&compound_settings, RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, EActivation::DontActivate);
+		Body &body2 = c.CreateBody(&compound_settings, RVec3(1.0_r, 0, 0), Quat::sIdentity(), EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, EActivation::DontActivate);
+
+		ShapeCastSettings cast_settings;
+
+		SphereShape sphere(0.1f);
+		sphere.SetEmbedded();
+
+		// Override ClosestHitPerBodyCollisionCollector so that we can count the number of calls to AddHit
+		class MyClosestHitPerBodyCollisionCollector : public ClosestHitPerBodyCollisionCollector<CastShapeCollector>
+		{
+		public:
+			virtual void		AddHit(const ResultType &inResult) override
+			{
+				ClosestHitPerBodyCollisionCollector<CastShapeCollector>::AddHit(inResult);
+
+				++mNumCalls;
+			}
+
+			int					mNumCalls = 0;
+		};
+
+		{
+			RShapeCast shape_cast(&sphere, Vec3::sOne(), RMat44::sTranslation(RVec3(-1, 0, 0)), Vec3(3, 0, 0));
+
+			// Check that the all hit collector finds 20 hits (2 x 10 slabs)
+			AllHitCollisionCollector<CastShapeCollector> all_collector;
+			c.GetSystem()->GetNarrowPhaseQuery().CastShape(shape_cast, cast_settings, RVec3::sZero(), all_collector);
+			all_collector.Sort();
+			CHECK(all_collector.mHits.size() == 20);
+			for (int i = 0; i < 10; ++i)
+			{
+				CHECK(all_collector.mHits[i].mBodyID2 == body1.GetID());
+				CHECK_APPROX_EQUAL(all_collector.mHits[i].mContactPointOn1, Vec3(-0.5f + 0.1f * i, 0, 0));
+			}
+			for (int i = 0; i < 10; ++i)
+			{
+				CHECK(all_collector.mHits[10 + i].mBodyID2 == body2.GetID());
+				CHECK_APPROX_EQUAL(all_collector.mHits[10 + i].mContactPointOn1, Vec3(0.5f + 0.1f * i, 0, 0));
+			}
+
+			// Check that the closest hit per body collector only finds 2
+			MyClosestHitPerBodyCollisionCollector closest_collector;
+			c.GetSystem()->GetNarrowPhaseQuery().CastShape(shape_cast, cast_settings, RVec3::sZero(), closest_collector);
+			CHECK(closest_collector.mNumCalls == 2); // Spatial ordering by the broad phase and compound shape and the early out value should have resulted in only 2 calls to AddHit
+			closest_collector.Sort();
+			CHECK(closest_collector.mHits.size() == 2);
+			CHECK(closest_collector.mHits[0].mBodyID2 == body1.GetID());
+			CHECK_APPROX_EQUAL(closest_collector.mHits[0].mContactPointOn1, Vec3(-0.5f, 0, 0));
+			CHECK(closest_collector.mHits[1].mBodyID2 == body2.GetID());
+			CHECK_APPROX_EQUAL(closest_collector.mHits[1].mContactPointOn1, Vec3(0.5f, 0, 0));
+		}
+
+		{
+			// Cast in reverse direction
+			RShapeCast shape_cast(&sphere, Vec3::sOne(), RMat44::sTranslation(RVec3(2, 0, 0)), Vec3(-3, 0, 0));
+
+			// Check that the all hit collector finds 20 hits (2 x 10 slabs)
+			AllHitCollisionCollector<CastShapeCollector> all_collector;
+			c.GetSystem()->GetNarrowPhaseQuery().CastShape(shape_cast, cast_settings, RVec3::sZero(), all_collector);
+			all_collector.Sort();
+			CHECK(all_collector.mHits.size() == 20);
+			for (int i = 0; i < 10; ++i)
+			{
+				CHECK(all_collector.mHits[i].mBodyID2 == body2.GetID());
+				CHECK_APPROX_EQUAL(all_collector.mHits[i].mContactPointOn1, Vec3(1.5f - 0.1f * i, 0, 0));
+			}
+			for (int i = 0; i < 10; ++i)
+			{
+				CHECK(all_collector.mHits[10 + i].mBodyID2 == body1.GetID());
+				CHECK_APPROX_EQUAL(all_collector.mHits[10 + i].mContactPointOn1, Vec3(0.5f - 0.1f * i, 0, 0));
+			}
+
+			// Check that the closest hit per body collector only finds 2
+			MyClosestHitPerBodyCollisionCollector closest_collector;
+			c.GetSystem()->GetNarrowPhaseQuery().CastShape(shape_cast, cast_settings, RVec3::sZero(), closest_collector);
+			CHECK(closest_collector.mNumCalls == 2); // Spatial ordering by the broad phase and compound shape and the early out value should have resulted in only 2 calls to AddHit
+			closest_collector.Sort();
+			CHECK(closest_collector.mHits.size() == 2);
+			CHECK(closest_collector.mHits[0].mBodyID2 == body2.GetID());
+			CHECK_APPROX_EQUAL(closest_collector.mHits[0].mContactPointOn1, Vec3(1.5f, 0, 0));
+			CHECK(closest_collector.mHits[1].mBodyID2 == body1.GetID());
+			CHECK_APPROX_EQUAL(closest_collector.mHits[1].mContactPointOn1, Vec3(0.5f, 0, 0));
+		}
+	}
+
+	// Test 2D shape cast against a box
+	TEST_CASE("TestCast2DBoxVsBox")
+	{
+		RefConst<Shape> box_shape;
+		{
+			float size = 5.0f;
+			float thickness = 1.0f;
+			Array<Vec3> points = {
+				Vec3(-size, -size, thickness),
+				Vec3(size, -size, thickness),
+				Vec3(size, size, thickness),
+				Vec3(-size, size, thickness),
+				Vec3(-size, -size, -thickness),
+				Vec3(size, -size, -thickness),
+				Vec3(size, size, -thickness),
+				Vec3(-size, size, -thickness),
+			};
+			ConvexHullShapeSettings box_shape_settings(points);
+			box_shape_settings.SetEmbedded();
+			box_shape_settings.mMaxConvexRadius = 0.0f;
+			box_shape = box_shape_settings.Create().Get();
+		}
+
+		RefConst<Shape> cast_shape;
+		{
+			float size = 1.0f;
+			Array<Vec3> points = {
+				Vec3(-size, -size, 0),
+				Vec3(size, -size, 0),
+				Vec3(size, size, 0),
+				Vec3(-size, size, 0),
+			};
+			ConvexHullShapeSettings cast_shape_settings(points);
+			cast_shape_settings.SetEmbedded();
+			cast_shape_settings.mMaxConvexRadius = 0.0f;
+			cast_shape = cast_shape_settings.Create().Get();
+		}
+
+		// The 2d box cast touches the surface of the box at the start and moves into it
+		ShapeCastSettings settings;
+		settings.mReturnDeepestPoint = true;
+		ShapeCast shape_cast(cast_shape, Vec3::sOne(), Mat44::sTranslation(Vec3(0, 0, 1)), Vec3(0, 0, -10));
+		ClosestHitCollisionCollector<CastShapeCollector> cast_shape_collector;
+		CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, settings, box_shape, Vec3::sOne(), ShapeFilter(), Mat44::sIdentity(), SubShapeIDCreator(), SubShapeIDCreator(), cast_shape_collector);
+
+		CHECK(cast_shape_collector.HadHit());
+		CHECK(cast_shape_collector.mHit.mFraction == 0.0f);
+		CHECK_APPROX_EQUAL(cast_shape_collector.mHit.mPenetrationAxis.Normalized(), Vec3(0, 0, -1));
+		CHECK_APPROX_EQUAL(cast_shape_collector.mHit.mPenetrationDepth, 0.0f);
+		CHECK_APPROX_EQUAL(cast_shape_collector.mHit.mContactPointOn1, Vec3(0, 0, 1), 1.0e-4f);
+		CHECK_APPROX_EQUAL(cast_shape_collector.mHit.mContactPointOn2, Vec3(0, 0, 1), 1.0e-4f);
+	}
+}

lib/All/JoltPhysics/UnitTests/Physics/CharacterVirtualTests.cpp

@@ -0,0 +1,873 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2022 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include "PhysicsTestContext.h"
+#include "LoggingCharacterContactListener.h"
+#include <Jolt/Physics/Collision/Shape/CapsuleShape.h>
+#include <Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h>
+#include <Jolt/Physics/Collision/Shape/MeshShape.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Character/CharacterVirtual.h>
+#include "Layers.h"
+
+TEST_SUITE("CharacterVirtualTests")
+{
+	class Character : public CharacterContactListener
+	{
+	public:
+		// Construct
+								Character(PhysicsTestContext &ioContext) : mContext(ioContext) { }
+
+		// Create the character
+		void					Create()
+		{
+			// Create capsule
+			Ref<Shape> capsule = new CapsuleShape(0.5f * mHeightStanding, mRadiusStanding);
+			mCharacterSettings.mShape = RotatedTranslatedShapeSettings(Vec3(0, 0.5f * mHeightStanding + mRadiusStanding, 0), Quat::sIdentity(), capsule).Create().Get();
+
+			// Configure supporting volume
+			mCharacterSettings.mSupportingVolume = Plane(Vec3::sAxisY(), -mHeightStanding); // Accept contacts that touch the lower sphere of the capsule
+
+			// Create character
+			mCharacter = new CharacterVirtual(&mCharacterSettings, mInitialPosition, Quat::sIdentity(), 0, mContext.GetSystem());
+			mCharacter->SetListener(this);
+			mCharacter->SetCharacterVsCharacterCollision(&mCharacterVsCharacter);
+		}
+
+		// Step the character and the world
+		void					Step()
+		{
+			// Step the world
+			mContext.SimulateSingleStep();
+
+			// Determine new basic velocity
+			Vec3 current_vertical_velocity = Vec3(0, mCharacter->GetLinearVelocity().GetY(), 0);
+			Vec3 ground_velocity = mCharacter->GetGroundVelocity();
+			Vec3 new_velocity;
+			if (mCharacter->GetGroundState() == CharacterVirtual::EGroundState::OnGround // If on ground
+				&& (current_vertical_velocity.GetY() - ground_velocity.GetY()) < 0.1f) // And not moving away from ground
+			{
+				// Assume velocity of ground when on ground
+				new_velocity = ground_velocity;
+
+				// Jump
+				new_velocity += Vec3(0, mJumpSpeed, 0);
+				mJumpSpeed = 0.0f;
+			}
+			else
+				new_velocity = current_vertical_velocity;
+
+			// Gravity
+			PhysicsSystem *system = mContext.GetSystem();
+			float delta_time = mContext.GetDeltaTime();
+			new_velocity += system->GetGravity() * delta_time;
+
+			// Player input
+			new_velocity += mHorizontalSpeed;
+
+			// Update character velocity
+			mCharacter->SetLinearVelocity(new_velocity);
+
+			RVec3 start_pos = GetPosition();
+
+			// Update the character position
+			TempAllocatorMalloc allocator;
+			mCharacter->ExtendedUpdate(delta_time,
+				system->GetGravity(),
+				mUpdateSettings,
+				system->GetDefaultBroadPhaseLayerFilter(Layers::MOVING),
+				system->GetDefaultLayerFilter(Layers::MOVING),
+				{ },
+				{ },
+				allocator);
+
+			// Calculate effective velocity in this step
+			mEffectiveVelocity = Vec3(GetPosition() - start_pos) / delta_time;
+		}
+
+		// Simulate a longer period of time
+		void					Simulate(float inTime)
+		{
+			int num_steps = (int)round(inTime / mContext.GetDeltaTime());
+			for (int step = 0; step < num_steps; ++step)
+				Step();
+		}
+
+		// Get the number of active contacts
+		size_t					GetNumContacts() const
+		{
+			return mCharacter->GetActiveContacts().size();
+		}
+
+		// Check if the character is in contact with another body
+		bool					HasCollidedWith(const BodyID &inBody) const
+		{
+			return mCharacter->HasCollidedWith(inBody);
+		}
+
+		// Check if the character is in contact with another character
+		bool					HasCollidedWith(const CharacterVirtual *inCharacter) const
+		{
+			return mCharacter->HasCollidedWith(inCharacter);
+		}
+
+		// Get position of character
+		RVec3					GetPosition() const
+		{
+			return mCharacter->GetPosition();
+		}
+
+		// Configuration
+		RVec3					mInitialPosition = RVec3::sZero();
+		float					mHeightStanding = 1.35f;
+		float					mRadiusStanding = 0.3f;
+		CharacterVirtualSettings mCharacterSettings;
+		CharacterVirtual::ExtendedUpdateSettings mUpdateSettings;
+
+		// Character movement settings (update to control the movement of the character)
+		Vec3					mHorizontalSpeed = Vec3::sZero();
+		float					mJumpSpeed = 0.0f; // Character will jump when not 0, will auto reset
+
+		// The character
+		Ref<CharacterVirtual>	mCharacter;
+
+		// Character vs character
+		CharacterVsCharacterCollisionSimple mCharacterVsCharacter;
+
+		// Calculated effective velocity after a step
+		Vec3					mEffectiveVelocity = Vec3::sZero();
+
+		// Log of contact events
+		LoggingCharacterContactListener mContactLog;
+
+	private:
+		// CharacterContactListener callback
+		virtual bool			OnContactValidate(const CharacterVirtual *inCharacter, const BodyID &inBodyID2, const SubShapeID &inSubShapeID2) override
+		{
+			return mContactLog.OnContactValidate(inCharacter, inBodyID2, inSubShapeID2);
+		}
+
+		virtual bool			OnCharacterContactValidate(const CharacterVirtual *inCharacter, const CharacterVirtual *inOtherCharacter, const SubShapeID &inSubShapeID2) override
+		{
+			return mContactLog.OnCharacterContactValidate(inCharacter, inOtherCharacter, inSubShapeID2);
+		}
+
+		virtual void			OnContactAdded(const CharacterVirtual *inCharacter, const BodyID &inBodyID2, const SubShapeID &inSubShapeID2, RVec3Arg inContactPosition, Vec3Arg inContactNormal, CharacterContactSettings &ioSettings) override
+		{
+			mContactLog.OnContactAdded(inCharacter, inBodyID2, inSubShapeID2, inContactPosition, inContactNormal, ioSettings);
+		}
+
+		virtual void			OnContactPersisted(const CharacterVirtual *inCharacter, const BodyID &inBodyID2, const SubShapeID &inSubShapeID2, RVec3Arg inContactPosition, Vec3Arg inContactNormal, CharacterContactSettings &ioSettings) override
+		{
+			mContactLog.OnContactPersisted(inCharacter, inBodyID2, inSubShapeID2, inContactPosition, inContactNormal, ioSettings);
+		}
+
+		virtual void			OnContactRemoved(const CharacterVirtual *inCharacter, const BodyID &inBodyID2, const SubShapeID &inSubShapeID2) override
+		{
+			mContactLog.OnContactRemoved(inCharacter, inBodyID2, inSubShapeID2);
+		}
+
+		virtual void			OnCharacterContactAdded(const CharacterVirtual *inCharacter, const CharacterVirtual *inOtherCharacter, const SubShapeID &inSubShapeID2, RVec3Arg inContactPosition, Vec3Arg inContactNormal, CharacterContactSettings &ioSettings) override
+		{
+			mContactLog.OnCharacterContactAdded(inCharacter, inOtherCharacter, inSubShapeID2, inContactPosition, inContactNormal, ioSettings);
+		}
+
+		virtual void			OnCharacterContactPersisted(const CharacterVirtual *inCharacter, const CharacterVirtual *inOtherCharacter, const SubShapeID &inSubShapeID2, RVec3Arg inContactPosition, Vec3Arg inContactNormal, CharacterContactSettings &ioSettings) override
+		{
+			mContactLog.OnCharacterContactPersisted(inCharacter, inOtherCharacter, inSubShapeID2, inContactPosition, inContactNormal, ioSettings);
+		}
+
+		virtual void			OnCharacterContactRemoved(const CharacterVirtual *inCharacter, const CharacterID &inOtherCharacterID, const SubShapeID &inSubShapeID2) override
+		{
+			mContactLog.OnCharacterContactRemoved(inCharacter, inOtherCharacterID, inSubShapeID2);
+		}
+
+		virtual void			OnContactSolve(const CharacterVirtual *inCharacter, const BodyID &inBodyID2, const SubShapeID &inSubShapeID2, RVec3Arg inContactPosition, Vec3Arg inContactNormal, Vec3Arg inContactVelocity, const PhysicsMaterial *inContactMaterial, Vec3Arg inCharacterVelocity, Vec3 &ioNewCharacterVelocity) override
+		{
+			// Don't allow sliding if the character doesn't want to move
+			if (mHorizontalSpeed.IsNearZero() && inContactVelocity.IsNearZero() && !inCharacter->IsSlopeTooSteep(inContactNormal))
+				ioNewCharacterVelocity = Vec3::sZero();
+		}
+
+		PhysicsTestContext &	mContext;
+	};
+
+	TEST_CASE("TestFallingAndJumping")
+	{
+		// Create floor
+		PhysicsTestContext c;
+		c.CreateFloor();
+
+		// Create character
+		Character character(c);
+		character.mInitialPosition = RVec3(0, 2, 0);
+		character.Create();
+
+		// After 1 step we should still be in air
+		character.Step();
+		CHECK(character.mCharacter->GetGroundState() == CharacterBase::EGroundState::InAir);
+
+		// After some time we should be on the floor
+		character.Simulate(1.0f);
+		CHECK(character.mCharacter->GetGroundState() == CharacterBase::EGroundState::OnGround);
+		CHECK_APPROX_EQUAL(character.GetPosition(), RVec3::sZero());
+		CHECK_APPROX_EQUAL(character.mEffectiveVelocity, Vec3::sZero());
+
+		// Jump
+		character.mJumpSpeed = 1.0f;
+		character.Step();
+		Vec3 velocity(0, 1.0f + c.GetDeltaTime() * c.GetSystem()->GetGravity().GetY(), 0);
+		CHECK_APPROX_EQUAL(character.GetPosition(), RVec3(velocity * c.GetDeltaTime()));
+		CHECK_APPROX_EQUAL(character.mEffectiveVelocity, velocity);
+		CHECK(character.mCharacter->GetGroundState() == CharacterBase::EGroundState::InAir);
+
+		// After some time we should be on the floor again
+		character.Simulate(1.0f);
+		CHECK(character.mCharacter->GetGroundState() == CharacterBase::EGroundState::OnGround);
+		CHECK_APPROX_EQUAL(character.GetPosition(), RVec3::sZero());
+		CHECK_APPROX_EQUAL(character.mEffectiveVelocity, Vec3::sZero());
+	}
+
+	TEST_CASE("TestMovingOnSlope")
+	{
+		constexpr float cFloorHalfHeight = 1.0f;
+		constexpr float cMovementTime = 1.5f;
+
+		// Iterate various slope angles
+		for (float slope_angle = DegreesToRadians(5.0f); slope_angle < DegreesToRadians(85.0f); slope_angle += DegreesToRadians(10.0f))
+		{
+			// Create sloped floor
+			PhysicsTestContext c;
+			Quat slope_rotation = Quat::sRotation(Vec3::sAxisZ(), slope_angle);
+			c.CreateBox(RVec3::sZero(), slope_rotation, EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, Vec3(100.0f, cFloorHalfHeight, 100.0f));
+
+			// Create character so that it is touching the slope
+			Character character(c);
+			float radius_and_padding = character.mRadiusStanding + character.mCharacterSettings.mCharacterPadding;
+			character.mInitialPosition = RVec3(0, (radius_and_padding + cFloorHalfHeight) / Cos(slope_angle) - radius_and_padding, 0);
+			character.Create();
+
+			// Determine if the slope is too steep for the character
+			bool too_steep = slope_angle > character.mCharacterSettings.mMaxSlopeAngle;
+			CharacterBase::EGroundState expected_ground_state = (too_steep? CharacterBase::EGroundState::OnSteepGround : CharacterBase::EGroundState::OnGround);
+
+			Vec3 gravity = c.GetSystem()->GetGravity();
+			float time_step = c.GetDeltaTime();
+			Vec3 slope_normal = slope_rotation.RotateAxisY();
+
+			// Calculate expected position after 1 time step
+			RVec3 position_after_1_step = character.mInitialPosition;
+			if (too_steep)
+			{
+				// Apply 1 frame of gravity and cancel movement in the slope normal direction
+				Vec3 velocity = gravity * time_step;
+				velocity -= velocity.Dot(slope_normal) * slope_normal;
+				position_after_1_step += velocity * time_step;
+			}
+
+			// After 1 step we should be on the slope
+			character.Step();
+			CHECK(character.mCharacter->GetGroundState() == expected_ground_state);
+			CHECK_APPROX_EQUAL(character.GetPosition(), position_after_1_step, 2.0e-6f);
+
+			// Cancel any velocity to make the calculation below easier (otherwise we have to take gravity for 1 time step into account)
+			character.mCharacter->SetLinearVelocity(Vec3::sZero());
+
+			RVec3 start_pos = character.GetPosition();
+
+			// Start moving in X direction
+			character.mHorizontalSpeed = Vec3(2.0f, 0, 0);
+			character.Simulate(cMovementTime);
+			CHECK(character.mCharacter->GetGroundState() == expected_ground_state);
+
+			// Calculate resulting translation
+			Vec3 translation = Vec3(character.GetPosition() - start_pos);
+
+			// Calculate expected translation
+			Vec3 expected_translation;
+			if (too_steep)
+			{
+				// If too steep, we're just falling. Integrate using an Euler integrator.
+				Vec3 velocity = Vec3::sZero();
+				expected_translation = Vec3::sZero();
+				int num_steps = (int)round(cMovementTime / time_step);
+				for (int i = 0; i < num_steps; ++i)
+				{
+					velocity += gravity * time_step;
+					expected_translation += velocity * time_step;
+				}
+			}
+			else
+			{
+				// Every frame we apply 1 delta time * gravity which gets reset on the next update, add this to the horizontal speed
+				expected_translation = (character.mHorizontalSpeed + gravity * time_step) * cMovementTime;
+			}
+
+			// Cancel movement in slope direction
+			expected_translation -= expected_translation.Dot(slope_normal) * slope_normal;
+
+			// Check that we traveled the right amount
+			CHECK_APPROX_EQUAL(translation, expected_translation, 1.0e-4f);
+		}
+	}
+
+	TEST_CASE("TestStickToFloor")
+	{
+		constexpr float cFloorHalfHeight = 1.0f;
+		constexpr float cSlopeAngle = DegreesToRadians(45.0f);
+		constexpr float cMovementTime = 1.5f;
+
+		for (int mode = 0; mode < 2; ++mode)
+		{
+			// If this run is with 'stick to floor' enabled
+			bool stick_to_floor = mode == 0;
+
+			// Create sloped floor
+			PhysicsTestContext c;
+			Quat slope_rotation = Quat::sRotation(Vec3::sAxisZ(), cSlopeAngle);
+			Body &floor = c.CreateBox(RVec3::sZero(), slope_rotation, EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, Vec3(100.0f, cFloorHalfHeight, 100.0f));
+
+			// Create character so that it is touching the slope
+			Character character(c);
+			float radius_and_padding = character.mRadiusStanding + character.mCharacterSettings.mCharacterPadding;
+			character.mInitialPosition = RVec3(0, (radius_and_padding + cFloorHalfHeight) / Cos(cSlopeAngle) - radius_and_padding, 0);
+			character.mUpdateSettings.mStickToFloorStepDown = stick_to_floor? Vec3(0, -0.5f, 0) : Vec3::sZero();
+			character.Create();
+
+			// After 1 step we should be on the slope
+			character.Step();
+			CHECK(character.mCharacter->GetGroundState() == CharacterBase::EGroundState::OnGround);
+			CHECK(character.mContactLog.GetEntryCount() == 2);
+			CHECK(character.mContactLog.Contains(LoggingCharacterContactListener::EType::ValidateBody, character.mCharacter, floor.GetID()));
+			CHECK(character.mContactLog.Contains(LoggingCharacterContactListener::EType::AddBody, character.mCharacter, floor.GetID()));
+			character.mContactLog.Clear();
+
+			// Cancel any velocity to make the calculation below easier (otherwise we have to take gravity for 1 time step into account)
+			character.mCharacter->SetLinearVelocity(Vec3::sZero());
+
+			RVec3 start_pos = character.GetPosition();
+
+			float time_step = c.GetDeltaTime();
+			int num_steps = (int)round(cMovementTime / time_step);
+
+			for (int i = 0; i < num_steps; ++i)
+			{
+				// Start moving down the slope at a speed high enough so that gravity will not keep us on the floor
+				character.mHorizontalSpeed = Vec3(-10.0f, 0, 0);
+				character.Step();
+
+				if (stick_to_floor)
+				{
+					// Should stick to floor
+					CHECK(character.mCharacter->GetGroundState() == CharacterBase::EGroundState::OnGround);
+
+					// Should have received callbacks
+					CHECK(character.mContactLog.GetEntryCount() == 2);
+					CHECK(character.mContactLog.Contains(LoggingCharacterContactListener::EType::ValidateBody, character.mCharacter, floor.GetID()));
+					CHECK(character.mContactLog.Contains(LoggingCharacterContactListener::EType::PersistBody, character.mCharacter, floor.GetID()));
+					character.mContactLog.Clear();
+				}
+				else
+				{
+					// Should be off ground
+					CHECK(character.mCharacter->GetGroundState() == CharacterBase::EGroundState::InAir);
+
+					// Remove callbacks
+					CHECK(character.mContactLog.GetEntryCount() == 1);
+					CHECK(character.mContactLog.Contains(LoggingCharacterContactListener::EType::RemoveBody, character.mCharacter, floor.GetID()));
+				}
+			}
+
+			// Calculate resulting translation
+			Vec3 translation = Vec3(character.GetPosition() - start_pos);
+
+			// Calculate expected translation
+			Vec3 expected_translation;
+			if (stick_to_floor)
+			{
+				// We should stick to the floor, so the vertical translation follows the slope perfectly
+				expected_translation = character.mHorizontalSpeed * cMovementTime;
+				expected_translation.SetY(expected_translation.GetX() * Tan(cSlopeAngle));
+			}
+			else
+			{
+				// If too steep, we're just falling. Integrate using an Euler integrator.
+				Vec3 velocity = character.mHorizontalSpeed;
+				expected_translation = Vec3::sZero();
+				Vec3 gravity = c.GetSystem()->GetGravity();
+				for (int i = 0; i < num_steps; ++i)
+				{
+					velocity += gravity * time_step;
+					expected_translation += velocity * time_step;
+				}
+			}
+
+			// Check that we traveled the right amount
+			CHECK_APPROX_EQUAL(translation, expected_translation, 1.0e-4f);
+		}
+	}
+
+	TEST_CASE("TestWalkStairs")
+	{
+		const float cStepHeight = 0.3f;
+		const int cNumSteps = 10;
+
+		// Create stairs from triangles
+		TriangleList triangles;
+		for (int i = 0; i < cNumSteps; ++i)
+		{
+			// Start of step
+			Vec3 base(0, cStepHeight * i, cStepHeight * i);
+
+			// Left side
+			Vec3 b1 = base + Vec3(2.0f, 0, 0);
+			Vec3 s1 = b1 + Vec3(0, cStepHeight, 0);
+			Vec3 p1 = s1 + Vec3(0, 0, cStepHeight);
+
+			// Right side
+			Vec3 width(-4.0f, 0, 0);
+			Vec3 b2 = b1 + width;
+			Vec3 s2 = s1 + width;
+			Vec3 p2 = p1 + width;
+
+			triangles.push_back(Triangle(s1, b1, s2));
+			triangles.push_back(Triangle(b1, b2, s2));
+			triangles.push_back(Triangle(s1, p2, p1));
+			triangles.push_back(Triangle(s1, s2, p2));
+		}
+
+		MeshShapeSettings mesh(triangles);
+		mesh.SetEmbedded();
+		BodyCreationSettings mesh_stairs(&mesh, RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING);
+
+		// Stair stepping is very delta time sensitive, so test various update frequencies
+		float frequencies[] = { 60.0f, 120.0f, 240.0f, 360.0f };
+		for (float frequency : frequencies)
+		{
+			float time_step = 1.0f / frequency;
+
+			PhysicsTestContext c(time_step);
+			c.CreateFloor();
+			c.GetBodyInterface().CreateAndAddBody(mesh_stairs, EActivation::DontActivate);
+
+			// Create character so that it is touching the slope
+			Character character(c);
+			character.mInitialPosition = RVec3(0, 0, -2.0f); // Start in front of the stairs
+			character.mUpdateSettings.mWalkStairsStepUp = Vec3::sZero(); // No stair walking
+			character.Create();
+
+			// Start moving towards the stairs
+			character.mHorizontalSpeed = Vec3(0, 0, 4.0f);
+			character.Simulate(1.0f);
+
+			// We should have gotten stuck at the start of the stairs (can't move up)
+			CHECK(character.mCharacter->GetGroundState() == CharacterBase::EGroundState::OnGround);
+			float radius_and_padding = character.mRadiusStanding + character.mCharacterSettings.mCharacterPadding;
+			CHECK_APPROX_EQUAL(character.GetPosition(), RVec3(0, 0, -radius_and_padding), 1.1e-2f);
+
+			// Enable stair walking
+			character.mUpdateSettings.mWalkStairsStepUp = Vec3(0, 0.4f, 0);
+
+			// Calculate time it should take to move up the stairs at constant speed
+			float movement_time = (cNumSteps * cStepHeight + radius_and_padding) / character.mHorizontalSpeed.GetZ();
+			int max_steps = int(1.5f * round(movement_time / time_step)); // In practice there is a bit of slowdown while stair stepping, so add a bit of slack
+
+			// Step until we reach the top of the stairs
+			RVec3 last_position = character.GetPosition();
+			bool reached_goal = false;
+			for (int i = 0; i < max_steps; ++i)
+			{
+				character.Step();
+
+				// We should always be on the floor during stair stepping
+				CHECK(character.mCharacter->GetGroundState() == CharacterBase::EGroundState::OnGround);
+
+				// Check position progression
+				RVec3 position = character.GetPosition();
+				CHECK_APPROX_EQUAL(position.GetX(), 0); // No movement in X
+				CHECK(position.GetZ() > last_position.GetZ()); // Always moving forward
+				CHECK(position.GetZ() < cNumSteps * cStepHeight); // No movement beyond stairs
+				if (position.GetY() > cNumSteps * cStepHeight - 1.0e-3f)
+				{
+					reached_goal = true;
+					break;
+				}
+
+				last_position = position;
+			}
+			CHECK(reached_goal);
+		}
+	}
+
+	TEST_CASE("TestRotatingPlatform")
+	{
+		constexpr float cFloorHalfHeight = 1.0f;
+		constexpr float cFloorHalfWidth = 10.0f;
+		constexpr float cCharacterPosition = 0.9f * cFloorHalfWidth;
+		constexpr float cAngularVelocity = 2.0f * JPH_PI;
+
+		PhysicsTestContext c;
+
+		// Create box
+		Body &box = c.CreateBox(RVec3::sZero(), Quat::sIdentity(), EMotionType::Kinematic, EMotionQuality::Discrete, Layers::MOVING, Vec3(cFloorHalfWidth, cFloorHalfHeight, cFloorHalfWidth));
+		box.SetAllowSleeping(false);
+
+		// Create character so that it is touching the box at the
+		Character character(c);
+		character.mInitialPosition = RVec3(cCharacterPosition, cFloorHalfHeight, 0);
+		character.Create();
+
+		// Step to ensure the character is on the box
+		character.Step();
+		CHECK(character.mCharacter->GetGroundState() == CharacterBase::EGroundState::OnGround);
+
+		// Set the box to rotate a full circle per second
+		box.SetAngularVelocity(Vec3(0, cAngularVelocity, 0));
+
+		// Rotate and check that character stays on the box
+		for (int t = 0; t < 60; ++t)
+		{
+			character.Step();
+			CHECK(character.mCharacter->GetGroundState() == CharacterBase::EGroundState::OnGround);
+
+			// Note that the character moves according to the ground velocity and the ground velocity is updated at the end of the step
+			// so the character is always 1 time step behind the platform. This is why we use t and not t + 1 to calculate the expected position.
+			RVec3 expected_position = RMat44::sRotation(Quat::sRotation(Vec3::sAxisY(), float(t) * c.GetDeltaTime() * cAngularVelocity)) * character.mInitialPosition;
+			CHECK_APPROX_EQUAL(character.GetPosition(), expected_position, 1.0e-4f);
+		}
+	}
+
+	TEST_CASE("TestMovingPlatformUp")
+	{
+		constexpr float cFloorHalfHeight = 1.0f;
+		constexpr float cFloorHalfWidth = 10.0f;
+		constexpr float cLinearVelocity = 0.5f;
+
+		PhysicsTestContext c;
+
+		// Create box
+		Body &box = c.CreateBox(RVec3::sZero(), Quat::sIdentity(), EMotionType::Kinematic, EMotionQuality::Discrete, Layers::MOVING, Vec3(cFloorHalfWidth, cFloorHalfHeight, cFloorHalfWidth));
+		box.SetAllowSleeping(false);
+
+		// Create character so that it is touching the box at the
+		Character character(c);
+		character.mInitialPosition = RVec3(0, cFloorHalfHeight, 0);
+		character.Create();
+
+		// Step to ensure the character is on the box
+		character.Step();
+		CHECK(character.mCharacter->GetGroundState() == CharacterBase::EGroundState::OnGround);
+
+		// Set the box to move up
+		box.SetLinearVelocity(Vec3(0, cLinearVelocity, 0));
+
+		// Check that character stays on the box
+		for (int t = 0; t < 60; ++t)
+		{
+			character.Step();
+			CHECK(character.mCharacter->GetGroundState() == CharacterBase::EGroundState::OnGround);
+			RVec3 expected_position = box.GetPosition() + character.mInitialPosition;
+			CHECK_APPROX_EQUAL(character.GetPosition(), expected_position, 1.0e-2f);
+		}
+
+		// Stop box
+		box.SetLinearVelocity(Vec3::sZero());
+		character.Simulate(0.5f);
+
+		// Set the box to move down
+		box.SetLinearVelocity(Vec3(0, -cLinearVelocity, 0));
+
+		// Check that character stays on the box
+		for (int t = 0; t < 60; ++t)
+		{
+			character.Step();
+			CHECK(character.mCharacter->GetGroundState() == CharacterBase::EGroundState::OnGround);
+			RVec3 expected_position = box.GetPosition() + character.mInitialPosition;
+			CHECK_APPROX_EQUAL(character.GetPosition(), expected_position, 1.0e-2f);
+		}
+	}
+
+	TEST_CASE("TestContactPointLimit")
+	{
+		PhysicsTestContext ctx;
+		Body &floor = ctx.CreateFloor();
+
+		// Create character at the origin
+		Character character(ctx);
+		character.mInitialPosition = RVec3(0, 1, 0);
+		character.mUpdateSettings.mStickToFloorStepDown = Vec3::sZero();
+		character.mUpdateSettings.mWalkStairsStepUp = Vec3::sZero();
+		character.Create();
+
+		// Radius including padding
+		const float character_radius = character.mRadiusStanding + character.mCharacterSettings.mCharacterPadding;
+
+		// Create a half cylinder with caps for testing contact point limit
+		VertexList vertices;
+		IndexedTriangleList triangles;
+
+		// The half cylinder
+		const int cPosSegments = 2;
+		const int cAngleSegments = 768;
+		const float cCylinderLength = 2.0f;
+		for (int pos = 0; pos < cPosSegments; ++pos)
+			for (int angle = 0; angle < cAngleSegments; ++angle)
+			{
+				uint32 start = (uint32)vertices.size();
+
+				float radius = character_radius + 0.01f;
+				float angle_rad = (-0.5f + float(angle) / cAngleSegments) * JPH_PI;
+				float s = Sin(angle_rad);
+				float c = Cos(angle_rad);
+				float x = cCylinderLength * (-0.5f + float(pos) / (cPosSegments - 1));
+				float y = angle == 0 || angle == cAngleSegments - 1? 0.5f : (1.0f - c) * radius;
+				float z = s * radius;
+				vertices.push_back(Float3(x, y, z));
+
+				if (pos > 0 && angle > 0)
+				{
+					triangles.push_back(IndexedTriangle(start, start - 1, start - cAngleSegments));
+					triangles.push_back(IndexedTriangle(start - 1, start - cAngleSegments - 1, start - cAngleSegments));
+				}
+			}
+
+		// Add end caps
+		uint32 end = cAngleSegments * (cPosSegments - 1);
+		for (int angle = 0; angle < cAngleSegments - 1; ++angle)
+		{
+			triangles.push_back(IndexedTriangle(0, angle + 1, angle));
+			triangles.push_back(IndexedTriangle(end, end + angle, end + angle + 1));
+		}
+
+		// Create test body
+		MeshShapeSettings mesh(vertices, triangles);
+		mesh.SetEmbedded();
+		BodyCreationSettings mesh_cylinder(&mesh, character.mInitialPosition, Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING);
+		BodyID cylinder_id = ctx.GetBodyInterface().CreateAndAddBody(mesh_cylinder, EActivation::DontActivate);
+
+		// End positions that can be reached by character
+		RVec3 pos_end(0.5_r * cCylinderLength - character_radius, 1, 0);
+		RVec3 neg_end(-0.5_r * cCylinderLength + character_radius, 1, 0);
+
+		// Move towards positive cap and test if we hit the end
+		character.mHorizontalSpeed = Vec3(cCylinderLength, 0, 0);
+		for (int t = 0; t < 60; ++t)
+		{
+			character.Step();
+			CHECK(character.mCharacter->GetMaxHitsExceeded());
+			CHECK(character.GetNumContacts() <= character.mCharacter->GetMaxNumHits());
+			CHECK(character.mCharacter->GetGroundBodyID() == cylinder_id);
+			CHECK(character.mCharacter->GetGroundNormal().Dot(Vec3::sAxisY()) > 0.999f);
+		}
+		CHECK_APPROX_EQUAL(character.GetPosition(), pos_end, 1.0e-4f);
+
+		// Move towards negative cap and test if we hit the end
+		character.mHorizontalSpeed = Vec3(-cCylinderLength, 0, 0);
+		for (int t = 0; t < 60; ++t)
+		{
+			character.Step();
+			CHECK(character.mCharacter->GetMaxHitsExceeded());
+			CHECK(character.GetNumContacts() <= character.mCharacter->GetMaxNumHits());
+			CHECK(character.mCharacter->GetGroundBodyID() == cylinder_id);
+			CHECK(character.mCharacter->GetGroundNormal().Dot(Vec3::sAxisY()) > 0.999f);
+		}
+		CHECK_APPROX_EQUAL(character.GetPosition(), neg_end, 1.0e-4f);
+
+		// Turn off contact point reduction
+		character.mCharacter->SetHitReductionCosMaxAngle(-1.0f);
+
+		// Move towards positive cap and test that we did not reach the end
+		character.mHorizontalSpeed = Vec3(cCylinderLength, 0, 0);
+		for (int t = 0; t < 60; ++t)
+		{
+			character.Step();
+			CHECK(character.mCharacter->GetMaxHitsExceeded());
+			CHECK(character.GetNumContacts() == character.mCharacter->GetMaxNumHits());
+		}
+		RVec3 cur_pos = character.GetPosition();
+		CHECK((pos_end - cur_pos).Length() > 0.01_r);
+
+		// Move towards negative cap and test that we got stuck
+		character.mHorizontalSpeed = Vec3(-cCylinderLength, 0, 0);
+		for (int t = 0; t < 60; ++t)
+		{
+			character.Step();
+			CHECK(character.mCharacter->GetMaxHitsExceeded());
+			CHECK(character.GetNumContacts() == character.mCharacter->GetMaxNumHits());
+		}
+		CHECK(cur_pos.IsClose(character.GetPosition(), 1.0e-6f));
+
+		// Now teleport the character next to the half cylinder
+		character.mCharacter->SetPosition(RVec3(0, 0, 1));
+
+		// Move in positive X and check that we did not exceed max hits and that we were able to move unimpeded
+		character.mHorizontalSpeed = Vec3(cCylinderLength, 0, 0);
+		for (int t = 0; t < 60; ++t)
+		{
+			character.Step();
+			CHECK(!character.mCharacter->GetMaxHitsExceeded());
+			CHECK(character.GetNumContacts() == 1); // We should only hit the floor
+			CHECK(character.mCharacter->GetGroundBodyID() == floor.GetID());
+			CHECK(character.mCharacter->GetGroundNormal().Dot(Vec3::sAxisY()) > 0.999f);
+		}
+		CHECK_APPROX_EQUAL(character.GetPosition(), RVec3(cCylinderLength, 0, 1), 1.0e-4f);
+	}
+
+	TEST_CASE("TestStairWalkAlongWall")
+	{
+		// Stair stepping is very delta time sensitive, so test various update frequencies
+		float frequencies[] = { 60.0f, 120.0f, 240.0f, 360.0f };
+		for (float frequency : frequencies)
+		{
+			float time_step = 1.0f / frequency;
+
+			PhysicsTestContext c(time_step);
+			c.CreateFloor();
+
+			// Create character
+			Character character(c);
+			character.Create();
+
+			// Create a wall
+			const float cWallHalfThickness = 0.05f;
+			c.GetBodyInterface().CreateAndAddBody(BodyCreationSettings(new BoxShape(Vec3(50.0f, 1.0f, cWallHalfThickness)), RVec3(0, 1.0_r, Real(-character.mRadiusStanding - character.mCharacter->GetCharacterPadding() - cWallHalfThickness)), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING), EActivation::DontActivate);
+
+			// Start moving along the wall, if the stair stepping algorithm is working correctly it should not trigger and not apply extra speed to the character
+			character.mHorizontalSpeed = Vec3(5.0f, 0, -1.0f);
+			character.Simulate(1.0f);
+
+			// We should have moved along the wall at the desired speed
+			CHECK(character.mCharacter->GetGroundState() == CharacterBase::EGroundState::OnGround);
+			CHECK_APPROX_EQUAL(character.GetPosition(), RVec3(5.0f, 0, 0), 1.0e-2f);
+		}
+	}
+
+	TEST_CASE("TestInitiallyIntersecting")
+	{
+		PhysicsTestContext c;
+		c.CreateFloor();
+
+		// Create box that is intersecting with the character
+		c.CreateBox(RVec3(-0.5f, 0.5f, 0), Quat::sIdentity(), EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, Vec3::sReplicate(0.5f));
+
+		// Try various penetration recovery values
+		for (float penetration_recovery : { 0.0f, 0.5f, 0.75f, 1.0f })
+		{
+			// Create character
+			Character character(c);
+			character.mCharacterSettings.mPenetrationRecoverySpeed = penetration_recovery;
+			character.Create();
+			CHECK_APPROX_EQUAL(character.GetPosition(), RVec3::sZero());
+
+			// Total radius of character
+			float radius_and_padding = character.mRadiusStanding + character.mCharacterSettings.mCharacterPadding;
+
+			float x = 0.0f;
+			for (int step = 0; step < 3; ++step)
+			{
+				// Calculate expected position
+				x += penetration_recovery * (radius_and_padding - x);
+
+				// Step character and check that it matches expected recovery
+				character.Step();
+				CHECK_APPROX_EQUAL(character.GetPosition(), RVec3(x, 0, 0));
+			}
+		}
+	}
+
+	TEST_CASE("TestCharacterVsCharacter")
+	{
+		PhysicsTestContext c;
+		BodyID floor_id = c.CreateFloor().GetID();
+
+		// Create characters with different radii and padding
+		Character character1(c);
+		character1.mInitialPosition = RVec3::sZero();
+		character1.mRadiusStanding = 0.2f;
+		character1.mCharacterSettings.mCharacterPadding = 0.04f;
+		character1.Create();
+
+		Character character2(c);
+		character2.mInitialPosition = RVec3(1, 0, 0);
+		character2.mRadiusStanding = 0.3f;
+		character2.mCharacterSettings.mCharacterPadding = 0.03f;
+		character2.Create();
+
+		// Make both collide
+		character1.mCharacterVsCharacter.Add(character2.mCharacter);
+		character2.mCharacterVsCharacter.Add(character1.mCharacter);
+
+		// Add a box behind character 2, we should never hit this
+		Vec3 box_extent(0.1f, 1.0f, 1.0f);
+		c.CreateBox(RVec3(1.5f, 0, 0), Quat::sIdentity(), EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, box_extent, EActivation::DontActivate);
+
+		// Move character 1 towards character 2 so that in 1 step it will hit both character 2 and the box
+		character1.mHorizontalSpeed = Vec3(600.0f, 0, 0);
+		character1.Step();
+
+		// Character 1 should have stopped at character 2
+		float character1_radius = character1.mRadiusStanding + character1.mCharacterSettings.mCharacterPadding;
+		float character2_radius = character2.mRadiusStanding + character2.mCharacterSettings.mCharacterPadding;
+		float separation = character1_radius + character2_radius;
+		RVec3 expected_colliding_with_character = character2.mInitialPosition - Vec3(separation, 0, 0);
+		CHECK_APPROX_EQUAL(character1.GetPosition(), expected_colliding_with_character, 1.0e-3f);
+		CHECK(character1.GetNumContacts() == 2);
+		CHECK(character1.HasCollidedWith(floor_id));
+		CHECK(character1.HasCollidedWith(character2.mCharacter));
+
+		// Move character 1 back to its initial position
+		character1.mCharacter->SetPosition(character1.mInitialPosition);
+		character1.mCharacter->SetLinearVelocity(Vec3::sZero());
+
+		// Now move slowly so that we will detect the collision during the normal collide shape step
+		character1.mHorizontalSpeed = Vec3(1.0f, 0, 0);
+		character1.Step();
+		CHECK(character1.GetNumContacts() == 1);
+		CHECK(character1.HasCollidedWith(floor_id));
+		character1.Simulate(1.0f);
+
+		// Character 1 should have stopped at character 2
+		CHECK_APPROX_EQUAL(character1.GetPosition(), expected_colliding_with_character, 1.0e-3f);
+		CHECK(character1.GetNumContacts() == 2);
+		CHECK(character1.HasCollidedWith(floor_id));
+		CHECK(character1.HasCollidedWith(character2.mCharacter));
+
+		// Move character 1 back to its initial position
+		character1.mCharacter->SetPosition(character1.mInitialPosition);
+		character1.mCharacter->SetLinearVelocity(Vec3::sZero());
+
+		// Add a box in between the characters
+		RVec3 box_position(0.5f, 0, 0);
+		BodyID box_id = c.CreateBox(box_position, Quat::sIdentity(), EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, box_extent, EActivation::DontActivate).GetID();
+
+		// Move character 1 so that it will step through both the box and the character in 1 time step
+		character1.mHorizontalSpeed = Vec3(600.0f, 0, 0);
+		character1.Step();
+
+		// Expect that it ends up at the box
+		RVec3 expected_colliding_with_box = box_position - Vec3(character1_radius + box_extent.GetX(), 0, 0);
+		CHECK_APPROX_EQUAL(character1.GetPosition(), expected_colliding_with_box, 1.0e-3f);
+		CHECK(character1.GetNumContacts() == 2);
+		CHECK(character1.HasCollidedWith(floor_id));
+		CHECK(character1.HasCollidedWith(box_id));
+
+		// Move character 1 back to its initial position
+		character1.mCharacter->SetPosition(character1.mInitialPosition);
+		character1.mCharacter->SetLinearVelocity(Vec3::sZero());
+
+		// Now move slowly so that we will detect the collision during the normal collide shape step
+		character1.mHorizontalSpeed = Vec3(1.0f, 0, 0);
+		character1.Step();
+		CHECK(character1.GetNumContacts() == 1);
+		CHECK(character1.HasCollidedWith(floor_id));
+		character1.Simulate(1.0f);
+
+		// Expect that it ends up at the box
+		CHECK_APPROX_EQUAL(character1.GetPosition(), expected_colliding_with_box, 1.0e-3f);
+		CHECK(character1.GetNumContacts() == 2);
+		CHECK(character1.HasCollidedWith(floor_id));
+		CHECK(character1.HasCollidedWith(box_id));
+	}
+}

lib/All/JoltPhysics/UnitTests/Physics/CollidePointTests.cpp

@@ -0,0 +1,433 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include "PhysicsTestContext.h"
+#include "Layers.h"
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Collision/Shape/SphereShape.h>
+#include <Jolt/Physics/Collision/Shape/CapsuleShape.h>
+#include <Jolt/Physics/Collision/Shape/CylinderShape.h>
+#include <Jolt/Physics/Collision/Shape/TaperedCapsuleShape.h>
+#include <Jolt/Physics/Collision/Shape/ConvexHullShape.h>
+#include <Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h>
+#include <Jolt/Physics/Collision/Shape/ScaledShape.h>
+#include <Jolt/Physics/Collision/Shape/OffsetCenterOfMassShape.h>
+#include <Jolt/Physics/Collision/Shape/StaticCompoundShape.h>
+#include <Jolt/Physics/Collision/Shape/MutableCompoundShape.h>
+#include <Jolt/Physics/Collision/Shape/MeshShape.h>
+#include <Jolt/Physics/Collision/CollisionCollectorImpl.h>
+#include <Jolt/Physics/Collision/CollidePointResult.h>
+
+TEST_SUITE("CollidePointTests")
+{
+	// Probe directions in the direction of the faces
+	static Vec3 cube_probes[] = {
+		Vec3(-1.0f, 0, 0),
+		Vec3(1.0f, 0, 0),
+		Vec3(0, -1.0f, 0),
+		Vec3(0, 1.0f, 0),
+		Vec3(0, 0, -1.0f),
+		Vec3(0, 0, 1.0f)
+	};
+
+	// Probe directions in the direction of the faces
+	static Vec3 cube_and_zero_probes[] = {
+		Vec3(0, 0, 0),
+		Vec3(-1.0f, 0, 0),
+		Vec3(1.0f, 0, 0),
+		Vec3(0, -1.0f, 0),
+		Vec3(0, 1.0f, 0),
+		Vec3(0, 0, -1.0f),
+		Vec3(0, 0, 1.0f)
+	};
+
+	// Probes in the xy-plane
+	static Vec3 xy_probes[] = {
+		Vec3(-1.0f, 0, 0),
+		Vec3(1.0f, 0, 0),
+		Vec3(0, 0, -1.0f),
+		Vec3(0, 0, 1.0f)
+	};
+
+	// Probes in the xy-plane and zero
+	static Vec3 xy_and_zero_probes[] = {
+		Vec3(0, 0, 0),
+		Vec3(-1.0f, 0, 0),
+		Vec3(1.0f, 0, 0),
+		Vec3(0, 0, -1.0f),
+		Vec3(0, 0, 1.0f)
+	};
+
+	// Vertices of a cube
+	static Vec3 cube_vertices[] = {
+		Vec3(-1.0f, -1.0f, -1.0f),
+		Vec3( 1.0f, -1.0f, -1.0f),
+		Vec3(-1.0f, -1.0f,  1.0f),
+		Vec3( 1.0f, -1.0f,  1.0f),
+		Vec3(-1.0f,  1.0f, -1.0f),
+		Vec3( 1.0f,  1.0f, -1.0f),
+		Vec3(-1.0f,  1.0f,  1.0f),
+		Vec3( 1.0f,  1.0f,  1.0f)
+	};
+
+	static void sTestHit(const Shape *inShape, Vec3Arg inPosition)
+	{
+		AllHitCollisionCollector<CollidePointCollector> collector;
+		inShape->CollidePoint(inPosition - inShape->GetCenterOfMass(), SubShapeIDCreator(), collector);
+		CHECK(collector.mHits.size() == 1);
+	}
+
+	static void sTestHit(const NarrowPhaseQuery &inNarrowPhase, RVec3Arg inPosition, const BodyID &inBodyID)
+	{
+		AllHitCollisionCollector<CollidePointCollector> collector;
+		inNarrowPhase.CollidePoint(inPosition, collector);
+		CHECK(collector.mHits.size() == 1);
+		CHECK(collector.mHits[0].mBodyID == inBodyID);
+	}
+
+	static void sTestMiss(const Shape *inShape, Vec3Arg inPosition)
+	{
+		AllHitCollisionCollector<CollidePointCollector> collector;
+		inShape->CollidePoint(inPosition - inShape->GetCenterOfMass(), SubShapeIDCreator(), collector);
+		CHECK(collector.mHits.empty());
+	}
+
+	static void sTestMiss(const NarrowPhaseQuery &inNarrowPhase, RVec3Arg inPosition)
+	{
+		AllHitCollisionCollector<CollidePointCollector> collector;
+		inNarrowPhase.CollidePoint(inPosition, collector);
+		CHECK(collector.mHits.empty());
+	}
+
+	TEST_CASE("TestCollidePointVsBox")
+	{
+		Vec3 half_box_size(0.1f, 0.2f, 0.3f);
+		ShapeRefC shape = new BoxShape(half_box_size);
+
+		// Hits
+		for (Vec3 probe : cube_and_zero_probes)
+			sTestHit(shape, 0.99f * half_box_size * probe);
+
+		// Misses
+		for (Vec3 probe : cube_probes)
+			sTestMiss(shape, 1.01f * half_box_size * probe);
+	}
+
+	TEST_CASE("TestCollidePointVsSphere")
+	{
+		const float radius = 0.1f;
+		ShapeRefC shape = new SphereShape(radius);
+
+		// Hits
+		for (Vec3 probe : cube_and_zero_probes)
+			sTestHit(shape, 0.99f * Vec3::sReplicate(radius) * probe);
+
+		// Misses
+		for (Vec3 probe : cube_probes)
+			sTestMiss(shape, 1.01f * Vec3::sReplicate(radius) * probe);
+	}
+
+	TEST_CASE("TestCollidePointVsCapsule")
+	{
+		const float half_height = 0.2f;
+		const float radius = 0.1f;
+		ShapeRefC shape = new CapsuleShape(half_height, radius);
+
+		// Top hits
+		for (Vec3 probe : xy_and_zero_probes)
+			sTestHit(shape, 0.99f * radius * probe + Vec3(0, half_height, 0));
+
+		// Center hit
+		sTestHit(shape, Vec3::sZero());
+
+		// Bottom hits
+		for (Vec3 probe : xy_and_zero_probes)
+			sTestHit(shape, 0.99f * radius * probe + Vec3(0, -half_height, 0));
+
+		// Misses
+		for (Vec3 probe : cube_probes)
+			sTestMiss(shape, 1.01f * Vec3(radius, half_height + radius, radius) * probe);
+	}
+
+	TEST_CASE("TestCollidePointVsTaperedCapsule")
+	{
+		const float half_height = 0.4f;
+		const float top_radius = 0.1f;
+		const float bottom_radius = 0.2f;
+		TaperedCapsuleShapeSettings settings(half_height, top_radius, bottom_radius);
+		ShapeRefC shape = settings.Create().Get();
+
+		// Top hits
+		for (Vec3 probe : xy_and_zero_probes)
+			sTestHit(shape, 0.99f * top_radius * probe + Vec3(0, half_height, 0));
+
+		// Center hit
+		sTestHit(shape, Vec3::sZero());
+
+		// Bottom hits
+		for (Vec3 probe : xy_and_zero_probes)
+			sTestHit(shape, 0.99f * bottom_radius * probe + Vec3(0, -half_height, 0));
+
+		// Top misses
+		sTestMiss(shape, Vec3(0, half_height + top_radius + 0.01f, 0));
+		for (Vec3 probe : xy_probes)
+			sTestMiss(shape, 1.01f * top_radius * probe + Vec3(0, half_height, 0));
+
+		// Bottom misses
+		sTestMiss(shape, Vec3(0, -half_height - bottom_radius - 0.01f, 0));
+		for (Vec3 probe : xy_probes)
+			sTestMiss(shape, 1.01f * bottom_radius * probe + Vec3(0, -half_height, 0));
+	}
+
+	TEST_CASE("TestCollidePointVsCylinder")
+	{
+		const float half_height = 0.2f;
+		const float radius = 0.1f;
+		ShapeRefC shape = new CylinderShape(half_height, radius);
+
+		// Top hits
+		for (Vec3 probe : xy_and_zero_probes)
+			sTestHit(shape, 0.99f * (radius * probe + Vec3(0, half_height, 0)));
+
+		// Center hit
+		sTestHit(shape, Vec3::sZero());
+
+		// Bottom hits
+		for (Vec3 probe : xy_and_zero_probes)
+			sTestHit(shape, 0.99f * (radius * probe + Vec3(0, -half_height, 0)));
+
+		// Misses
+		for (Vec3 probe : cube_probes)
+			sTestMiss(shape, 1.01f * Vec3(radius, half_height, radius) * probe);
+	}
+
+	TEST_CASE("TestCollidePointVsConvexHull")
+	{
+		Vec3 half_box_size(0.1f, 0.2f, 0.3f);
+		Vec3 offset(10.0f, 11.0f, 12.0f);
+
+		ConvexHullShapeSettings settings;
+		for (uint i = 0; i < size(cube_vertices); ++i)
+			settings.mPoints.push_back(offset + cube_vertices[i] * half_box_size);
+		ShapeRefC shape = settings.Create().Get();
+
+		// Hits
+		for (Vec3 probe : cube_and_zero_probes)
+			sTestHit(shape, offset + 0.99f * half_box_size * probe);
+
+		// Misses
+		for (Vec3 probe : cube_probes)
+			sTestMiss(shape, offset + 1.01f * half_box_size * probe);
+	}
+
+	TEST_CASE("TestCollidePointVsRotatedTranslated")
+	{
+		Vec3 translation(10.0f, 11.0f, 12.0f);
+		Quat rotation = Quat::sRotation(Vec3(1, 2, 3).Normalized(), 0.3f * JPH_PI);
+		Mat44 transform = Mat44::sRotationTranslation(rotation, translation);
+
+		Vec3 half_box_size(0.1f, 0.2f, 0.3f);
+		RotatedTranslatedShapeSettings settings(translation, rotation, new BoxShape(half_box_size));
+		ShapeRefC shape = settings.Create().Get();
+
+		// Hits
+		for (Vec3 probe : cube_and_zero_probes)
+			sTestHit(shape, transform * (0.99f * half_box_size * probe));
+
+		// Misses
+		for (Vec3 probe : cube_probes)
+			sTestMiss(shape, transform * (1.01f * half_box_size * probe));
+	}
+
+	TEST_CASE("TestCollidePointVsScaled")
+	{
+		Vec3 scale(2.0f, 3.0f, -4.0f);
+		Vec3 half_box_size(0.1f, 0.2f, 0.3f);
+		ShapeRefC shape = new ScaledShape(new BoxShape(half_box_size), scale);
+
+		// Hits
+		for (Vec3 probe : cube_and_zero_probes)
+			sTestHit(shape, scale * (0.99f * half_box_size * probe));
+
+		// Misses
+		for (Vec3 probe : cube_probes)
+			sTestMiss(shape, scale * (1.01f * half_box_size * probe));
+	}
+
+	TEST_CASE("TestCollidePointVsOffsetCenterOfMass")
+	{
+		Vec3 offset(10.0f, 11.0f, 12.0f);
+		Vec3 half_box_size(0.1f, 0.2f, 0.3f);
+		OffsetCenterOfMassShapeSettings settings(offset, new BoxShape(half_box_size));
+		ShapeRefC shape = settings.Create().Get();
+
+		// Hits
+		for (Vec3 probe : cube_and_zero_probes)
+			sTestHit(shape, 0.99f * half_box_size * probe);
+
+		// Misses
+		for (Vec3 probe : cube_probes)
+			sTestMiss(shape, 1.01f * half_box_size * probe);
+	}
+
+	TEST_CASE("TestCollidePointVsStaticCompound")
+	{
+		Vec3 translation1(10.0f, 11.0f, 12.0f);
+		Quat rotation1 = Quat::sRotation(Vec3(1, 2, 3).Normalized(), 0.3f * JPH_PI);
+		Mat44 transform1 = Mat44::sRotationTranslation(rotation1, translation1);
+
+		Vec3 translation2(-1.0f, -2.0f, -3.0f);
+		Quat rotation2 = Quat::sRotation(Vec3(4, 5, 6).Normalized(), 0.2f * JPH_PI);
+		Mat44 transform2 = Mat44::sRotationTranslation(rotation2, translation2);
+
+		Vec3 half_box_size(0.1f, 0.2f, 0.3f);
+		ShapeRefC box = new BoxShape(half_box_size);
+
+		StaticCompoundShapeSettings settings;
+		settings.AddShape(translation1, rotation1, box);
+		settings.AddShape(translation2, rotation2, box);
+		ShapeRefC shape = settings.Create().Get();
+
+		// Hits
+		for (Vec3 probe : cube_and_zero_probes)
+		{
+			Vec3 point = 0.99f * half_box_size * probe;
+			sTestHit(shape, transform1 * point);
+			sTestHit(shape, transform2 * point);
+		}
+
+		// Misses
+		for (Vec3 probe : cube_probes)
+		{
+			Vec3 point = 1.01f * half_box_size * probe;
+			sTestMiss(shape, transform1 * point);
+			sTestMiss(shape, transform2 * point);
+		}
+	}
+
+	TEST_CASE("TestCollidePointVsMutableCompound")
+	{
+		Vec3 translation1(10.0f, 11.0f, 12.0f);
+		Quat rotation1 = Quat::sRotation(Vec3(1, 2, 3).Normalized(), 0.3f * JPH_PI);
+		Mat44 transform1 = Mat44::sRotationTranslation(rotation1, translation1);
+
+		Vec3 translation2(-1.0f, -2.0f, -3.0f);
+		Quat rotation2 = Quat::sRotation(Vec3(4, 5, 6).Normalized(), 0.2f * JPH_PI);
+		Mat44 transform2 = Mat44::sRotationTranslation(rotation2, translation2);
+
+		Vec3 half_box_size(0.1f, 0.2f, 0.3f);
+		ShapeRefC box = new BoxShape(half_box_size);
+
+		MutableCompoundShapeSettings settings;
+		settings.AddShape(translation1, rotation1, box);
+		settings.AddShape(translation2, rotation2, box);
+		ShapeRefC shape = settings.Create().Get();
+
+		// Hits
+		for (Vec3 probe : cube_and_zero_probes)
+		{
+			Vec3 point = 0.99f * half_box_size * probe;
+			sTestHit(shape, transform1 * point);
+			sTestHit(shape, transform2 * point);
+		}
+
+		// Misses
+		for (Vec3 probe : cube_probes)
+		{
+			Vec3 point = 1.01f * half_box_size * probe;
+			sTestMiss(shape, transform1 * point);
+			sTestMiss(shape, transform2 * point);
+		}
+	}
+
+	TEST_CASE("TestCollidePointVsMesh")
+	{
+		// Face indices of a cube
+		int indices[][3] = {
+			{ 0, 1, 3 },
+			{ 0, 3, 2 },
+			{ 4, 7, 5 },
+			{ 4, 6, 7 },
+			{ 2, 3, 6 },
+			{ 3, 7, 6 },
+			{ 1, 0, 4 },
+			{ 1, 4, 5 },
+			{ 1, 7, 3 },
+			{ 1, 5, 7 },
+			{ 0, 2, 6 },
+			{ 0, 6, 4 }
+		};
+
+		const int grid_size = 2;
+
+		UnitTestRandom random;
+		uniform_real_distribution<float> range(0.1f, 0.3f);
+
+		// Create a grid of closed shapes
+		MeshShapeSettings settings;
+		settings.SetEmbedded();
+		int num_cubes = Cubed(2 * grid_size + 1);
+		settings.mTriangleVertices.reserve(num_cubes * size(cube_vertices));
+		settings.mIndexedTriangles.reserve(num_cubes * size(indices));
+		for (int x = -grid_size; x <= grid_size; ++x)
+			for (int y = -grid_size; y <= grid_size; ++y)
+				for (int z = -grid_size; z <= grid_size; ++z)
+				{
+					Vec3 center((float)x, (float)y, (float)z);
+
+					// Create vertices with randomness
+					uint vtx = (uint)settings.mTriangleVertices.size();
+					settings.mTriangleVertices.resize(vtx + size(cube_vertices));
+					for (uint i = 0; i < size(cube_vertices); ++i)
+					{
+						Vec3 vertex(center + cube_vertices[i] * Vec3(range(random), range(random), range(random)));
+						vertex.StoreFloat3(&settings.mTriangleVertices[vtx + i]);
+					}
+
+					// Flip inside out? (inside out shapes should act the same as normal shapes for CollidePoint)
+					bool flip = (y & 1) == 0;
+
+					// Create face indices
+					uint idx = (uint)settings.mIndexedTriangles.size();
+					settings.mIndexedTriangles.resize(idx + size(indices));
+					for (uint i = 0; i < size(indices); ++i)
+						settings.mIndexedTriangles[idx + i] = IndexedTriangle(vtx + indices[i][0], vtx + indices[i][flip? 2 : 1], vtx + indices[i][flip? 1 : 2]);
+				}
+
+		// Create body with random orientation
+		PhysicsTestContext context;
+		Body &mesh_body = context.CreateBody(&settings, RVec3(Vec3::sRandom(random)), Quat::sRandom(random), EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, EActivation::DontActivate);
+
+		// Get the shape
+		ShapeRefC mesh_shape = mesh_body.GetShape();
+
+		// Get narrow phase
+		const NarrowPhaseQuery &narrow_phase = context.GetSystem()->GetNarrowPhaseQuery();
+
+		// Get transform
+		RMat44 body_transform = mesh_body.GetWorldTransform();
+		CHECK(body_transform != RMat44::sIdentity());
+
+		// Test points
+		for (int x = -grid_size; x <= grid_size; ++x)
+			for (int y = -grid_size; y <= grid_size; ++y)
+				for (int z = -grid_size; z <= grid_size; ++z)
+				{
+					Vec3 center((float)x, (float)y, (float)z);
+
+					// The center point should hit
+					sTestHit(mesh_shape, center);
+					sTestHit(narrow_phase, body_transform * center, mesh_body.GetID());
+
+					// Points outside the hull should not hit
+					for (Vec3 probe : cube_probes)
+					{
+						Vec3 point = center + 0.4f * probe;
+						sTestMiss(mesh_shape, point);
+						sTestMiss(narrow_phase, body_transform * point);
+					}
+				}
+	}
+}

lib/All/JoltPhysics/UnitTests/Physics/CollideShapeTests.cpp

@@ -0,0 +1,560 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include "PhysicsTestContext.h"
+#include <Jolt/Physics/Collision/Shape/SphereShape.h>
+#include <Jolt/Physics/Collision/Shape/ScaledShape.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h>
+#include <Jolt/Physics/Collision/Shape/CapsuleShape.h>
+#include <Jolt/Physics/Collision/Shape/ConvexHullShape.h>
+#include <Jolt/Physics/Collision/Shape/TriangleShape.h>
+#include <Jolt/Physics/Collision/Shape/CylinderShape.h>
+#include <Jolt/Physics/Collision/CollideShape.h>
+#include <Jolt/Physics/Collision/CollisionCollectorImpl.h>
+#include <Jolt/Physics/Collision/CollisionDispatch.h>
+#include <Jolt/Physics/Collision/CollideConvexVsTriangles.h>
+#include <Jolt/Geometry/EPAPenetrationDepth.h>
+#include "Layers.h"
+
+TEST_SUITE("CollideShapeTests")
+{
+	// Compares CollideShapeResult for two spheres with given positions and radii
+	static void sCompareCollideShapeResultSphere(Vec3Arg inPosition1, float inRadius1, Vec3Arg inPosition2, float inRadius2, const CollideShapeResult &inResult)
+	{
+		// Test if spheres overlap
+		Vec3 delta = inPosition2 - inPosition1;
+		float len = delta.Length();
+		CHECK(len > 0.0f);
+		CHECK(len <= inRadius1 + inRadius2);
+
+		// Calculate points on surface + vector that will push 2 out of collision
+		Vec3 expected_point1 = inPosition1 + delta * (inRadius1 / len);
+		Vec3 expected_point2 = inPosition2 - delta * (inRadius2 / len);
+		Vec3 expected_penetration_axis = delta / len;
+
+		// Get actual results
+		Vec3 penetration_axis = inResult.mPenetrationAxis.Normalized();
+
+		// Compare
+		CHECK_APPROX_EQUAL(expected_point1, inResult.mContactPointOn1);
+		CHECK_APPROX_EQUAL(expected_point2, inResult.mContactPointOn2);
+		CHECK_APPROX_EQUAL(expected_penetration_axis, penetration_axis);
+	}
+
+	// Test CollideShape function for spheres
+	TEST_CASE("TestCollideShapeSphere")
+	{
+		// Locations of test sphere
+		static const RVec3 cPosition1A(10.0f, 11.0f, 12.0f);
+		static const RVec3 cPosition1B(10.0f, 21.0f, 12.0f);
+		static const float cRadius1 = 2.0f;
+
+		// Locations of sphere in the physics system
+		static const RVec3 cPosition2A(13.0f, 11.0f, 12.0f);
+		static const RVec3 cPosition2B(13.0f, 22.0f, 12.0f);
+		static const float cRadius2 = 1.5f;
+
+		// Create sphere to test with (shape 1)
+		Ref<Shape> shape1 = new SphereShape(cRadius1);
+		Mat44 shape1_com = Mat44::sTranslation(shape1->GetCenterOfMass());
+		RMat44 shape1_transform = RMat44::sTranslation(cPosition1A) * Mat44::sRotationX(0.1f * JPH_PI) * shape1_com;
+
+		// Create sphere to collide against (shape 2)
+		PhysicsTestContext c;
+		Body &body2 = c.CreateSphere(cPosition2A, cRadius2, EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING);
+
+		// Filters
+		SpecifiedBroadPhaseLayerFilter broadphase_moving_filter(BroadPhaseLayers::MOVING);
+		SpecifiedBroadPhaseLayerFilter broadphase_non_moving_filter(BroadPhaseLayers::NON_MOVING);
+		SpecifiedObjectLayerFilter object_moving_filter(Layers::MOVING);
+		SpecifiedObjectLayerFilter object_non_moving_filter(Layers::NON_MOVING);
+
+		// Collector that fails the test
+		class FailCollideShapeCollector : public CollideShapeCollector
+		{
+		public:
+			virtual void	AddHit(const CollideShapeResult &inResult) override
+			{
+				FAIL("Callback should not be called");
+			}
+		};
+		FailCollideShapeCollector fail_collector;
+
+		// Set settings
+		CollideShapeSettings settings;
+		settings.mActiveEdgeMode = EActiveEdgeMode::CollideWithAll;
+		settings.mBackFaceMode = EBackFaceMode::CollideWithBackFaces;
+
+		// Test against wrong layer
+		c.GetSystem()->GetNarrowPhaseQuery().CollideShape(shape1, Vec3::sOne(), shape1_transform, settings, RVec3::sZero(), fail_collector, broadphase_moving_filter, object_moving_filter);
+
+		// Collector that tests that collision happens at position A
+		class PositionACollideShapeCollector : public CollideShapeCollector
+		{
+		public:
+							PositionACollideShapeCollector(const Body &inBody2) :
+				mBody2(inBody2)
+			{
+			}
+
+			virtual void	AddHit(const CollideShapeResult &inResult) override
+			{
+				CHECK(mBody2.GetID() == GetContext()->mBodyID);
+				sCompareCollideShapeResultSphere(Vec3(cPosition1A), cRadius1, Vec3(cPosition2A), cRadius2, inResult);
+				mWasHit = true;
+			}
+
+			bool			mWasHit = false;
+
+		private:
+			const Body &	mBody2;
+		};
+		PositionACollideShapeCollector position_a_collector(body2);
+
+		// Test collision against correct layer
+		CHECK(!position_a_collector.mWasHit);
+		c.GetSystem()->GetNarrowPhaseQuery().CollideShape(shape1, Vec3::sOne(), shape1_transform, settings, RVec3::sZero(), position_a_collector, broadphase_non_moving_filter, object_non_moving_filter);
+		CHECK(position_a_collector.mWasHit);
+
+		// Now move body to position B
+		c.GetSystem()->GetBodyInterface().SetPositionAndRotation(body2.GetID(), cPosition2B, Quat::sRotation(Vec3::sAxisY(), 0.2f * JPH_PI), EActivation::DontActivate);
+
+		// Test that original position doesn't collide anymore
+		c.GetSystem()->GetNarrowPhaseQuery().CollideShape(shape1, Vec3::sOne(), shape1_transform, settings, RVec3::sZero(), fail_collector, broadphase_non_moving_filter, object_non_moving_filter);
+
+		// Move test shape to position B
+		shape1_transform = RMat44::sTranslation(cPosition1B) * Mat44::sRotationZ(0.3f * JPH_PI) * shape1_com;
+
+		// Test against wrong layer
+		c.GetSystem()->GetNarrowPhaseQuery().CollideShape(shape1, Vec3::sOne(), shape1_transform, settings, RVec3::sZero(), fail_collector, broadphase_moving_filter, object_moving_filter);
+
+		// Callback that tests that collision happens at position B
+		class PositionBCollideShapeCollector : public CollideShapeCollector
+		{
+		public:
+							PositionBCollideShapeCollector(const Body &inBody2) :
+				mBody2(inBody2)
+			{
+			}
+
+			virtual void	Reset() override
+			{
+				CollideShapeCollector::Reset();
+
+				mWasHit = false;
+			}
+
+			virtual void	AddHit(const CollideShapeResult &inResult) override
+			{
+				CHECK(mBody2.GetID() == GetContext()->mBodyID);
+				sCompareCollideShapeResultSphere(Vec3(cPosition1B), cRadius1, Vec3(cPosition2B), cRadius2, inResult);
+				mWasHit = true;
+			}
+
+			bool			mWasHit = false;
+
+		private:
+			const Body &	mBody2;
+		};
+		PositionBCollideShapeCollector position_b_collector(body2);
+
+		// Test collision
+		CHECK(!position_b_collector.mWasHit);
+		c.GetSystem()->GetNarrowPhaseQuery().CollideShape(shape1, Vec3::sOne(), shape1_transform, settings, RVec3::sZero(), position_b_collector, broadphase_non_moving_filter, object_non_moving_filter);
+		CHECK(position_b_collector.mWasHit);
+
+		// Update the physics system (optimizes the broadphase)
+		c.Simulate(c.GetDeltaTime());
+
+		// Test against wrong layer
+		c.GetSystem()->GetNarrowPhaseQuery().CollideShape(shape1, Vec3::sOne(), shape1_transform, settings, RVec3::sZero(), fail_collector, broadphase_moving_filter, object_moving_filter);
+
+		// Test collision again
+		position_b_collector.Reset();
+		CHECK(!position_b_collector.mWasHit);
+		c.GetSystem()->GetNarrowPhaseQuery().CollideShape(shape1, Vec3::sOne(), shape1_transform, settings, RVec3::sZero(), position_b_collector, broadphase_non_moving_filter, object_non_moving_filter);
+		CHECK(position_b_collector.mWasHit);
+	}
+
+	// Test CollideShape function for a (scaled) sphere vs box
+	TEST_CASE("TestCollideShapeSphereVsBox")
+	{
+		PhysicsTestContext c;
+
+		// Create box to collide against (shape 2)
+		// The box is scaled up by a factor 10 in the X axis and then rotated so that the X axis is up
+		BoxShapeSettings box(Vec3::sOne());
+		box.SetEmbedded();
+		ScaledShapeSettings scaled_box(&box, Vec3(10, 1, 1));
+		scaled_box.SetEmbedded();
+		Body &body2 = c.CreateBody(&scaled_box, RVec3(0, 1, 0), Quat::sRotation(Vec3::sAxisZ(), 0.5f * JPH_PI), EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, EActivation::DontActivate);
+
+		// Set settings
+		CollideShapeSettings settings;
+		settings.mActiveEdgeMode = EActiveEdgeMode::CollideWithAll;
+		settings.mBackFaceMode = EBackFaceMode::CollideWithBackFaces;
+
+		{
+			// Create sphere
+			Ref<Shape> normal_sphere = new SphereShape(1.0f);
+
+			// Collect hit with normal sphere
+			AllHitCollisionCollector<CollideShapeCollector> collector;
+			c.GetSystem()->GetNarrowPhaseQuery().CollideShape(normal_sphere, Vec3::sOne(), RMat44::sTranslation(RVec3(0, 11, 0)), settings, RVec3::sZero(), collector);
+			CHECK(collector.mHits.size() == 1);
+			const CollideShapeResult &result = collector.mHits.front();
+			CHECK(result.mBodyID2 == body2.GetID());
+			CHECK_APPROX_EQUAL(result.mContactPointOn1, Vec3(0, 10, 0), 1.0e-4f);
+			CHECK_APPROX_EQUAL(result.mContactPointOn2, Vec3(0, 11, 0), 1.0e-4f);
+			Vec3 pen_axis = result.mPenetrationAxis.Normalized();
+			CHECK_APPROX_EQUAL(pen_axis, Vec3(0, -1, 0), 1.0e-4f);
+			CHECK_APPROX_EQUAL(result.mPenetrationDepth, 1.0f, 1.0e-5f);
+		}
+
+		{
+			// This repeats the same test as above but uses scaling at all levels
+			Ref<Shape> scaled_sphere = new ScaledShape(new SphereShape(0.1f), Vec3::sReplicate(5.0f));
+
+			// Collect hit with scaled sphere
+			AllHitCollisionCollector<CollideShapeCollector> collector;
+			c.GetSystem()->GetNarrowPhaseQuery().CollideShape(scaled_sphere, Vec3::sReplicate(2.0f), RMat44::sTranslation(RVec3(0, 11, 0)), settings, RVec3::sZero(), collector);
+			CHECK(collector.mHits.size() == 1);
+			const CollideShapeResult &result = collector.mHits.front();
+			CHECK(result.mBodyID2 == body2.GetID());
+			CHECK_APPROX_EQUAL(result.mContactPointOn1, Vec3(0, 10, 0), 1.0e-4f);
+			CHECK_APPROX_EQUAL(result.mContactPointOn2, Vec3(0, 11, 0), 1.0e-4f);
+			Vec3 pen_axis = result.mPenetrationAxis.Normalized();
+			CHECK_APPROX_EQUAL(pen_axis, Vec3(0, -1, 0), 1.0e-4f);
+			CHECK_APPROX_EQUAL(result.mPenetrationDepth, 1.0f, 1.0e-5f);
+		}
+	}
+
+	// Test colliding a very long capsule vs a box that is intersecting with the line segment inside the capsule
+	// This particular config reported the wrong penetration due to accuracy problems before
+	TEST_CASE("TestCollideShapeLongCapsuleVsEmbeddedBox")
+	{
+		// Create box
+		Vec3 box_min(-1.0f, -2.0f, 0.5f);
+		Vec3 box_max(2.0f, -0.5f, 3.0f);
+		Ref<RotatedTranslatedShapeSettings> box_settings = new RotatedTranslatedShapeSettings(0.5f * (box_min + box_max), Quat::sIdentity(), new BoxShapeSettings(0.5f * (box_max - box_min)));
+		Ref<Shape> box_shape = box_settings->Create().Get();
+		Mat44 box_transform(Vec4(0.516170502f, -0.803887904f, -0.295520246f, 0.0f), Vec4(0.815010250f, 0.354940295f, 0.458012700f, 0.0f), Vec4(-0.263298869f, -0.477264702f, 0.838386655f, 0.0f), Vec4(-10.2214508f, -18.6808319f, 40.7468987f, 1.0f));
+
+		// Create capsule
+		float capsule_half_height = 75.0f;
+		float capsule_radius = 1.5f;
+		Ref<RotatedTranslatedShapeSettings> capsule_settings = new RotatedTranslatedShapeSettings(Vec3(0, 0, 75), Quat(0.499999970f, -0.499999970f, -0.499999970f, 0.499999970f), new CapsuleShapeSettings(capsule_half_height, capsule_radius));
+		Ref<Shape> capsule_shape = capsule_settings->Create().Get();
+		Mat44 capsule_transform = Mat44::sTranslation(Vec3(-9.68538570f, -18.0328083f, 41.3212280f));
+
+		// Collision settings
+		CollideShapeSettings settings;
+		settings.mActiveEdgeMode = EActiveEdgeMode::CollideWithAll;
+		settings.mBackFaceMode = EBackFaceMode::CollideWithBackFaces;
+		settings.mCollectFacesMode = ECollectFacesMode::NoFaces;
+
+		// Collide the two shapes
+		AllHitCollisionCollector<CollideShapeCollector> collector;
+		CollisionDispatch::sCollideShapeVsShape(capsule_shape, box_shape, Vec3::sOne(), Vec3::sOne(), capsule_transform, box_transform, SubShapeIDCreator(), SubShapeIDCreator(), settings, collector);
+
+		// Check that there was a hit
+		CHECK(collector.mHits.size() == 1);
+		const CollideShapeResult &result = collector.mHits.front();
+
+		// Now move the box 1% further than the returned penetration depth and check that it is no longer in collision
+		Vec3 distance_to_move_box = result.mPenetrationAxis.Normalized() * result.mPenetrationDepth;
+		collector.Reset();
+		CHECK(!collector.HadHit());
+		CollisionDispatch::sCollideShapeVsShape(capsule_shape, box_shape, Vec3::sOne(), Vec3::sOne(), capsule_transform, Mat44::sTranslation(1.01f * distance_to_move_box) * box_transform, SubShapeIDCreator(), SubShapeIDCreator(), settings, collector);
+		CHECK(!collector.HadHit());
+
+		// Now check that moving 1% less than the penetration distance makes the shapes still overlap
+		CollisionDispatch::sCollideShapeVsShape(capsule_shape, box_shape, Vec3::sOne(), Vec3::sOne(), capsule_transform, Mat44::sTranslation(0.99f * distance_to_move_box) * box_transform, SubShapeIDCreator(), SubShapeIDCreator(), settings, collector);
+		CHECK(collector.mHits.size() == 1);
+	}
+
+	// Another test case found in practice of a very large oriented box (convex hull) vs a small triangle outside the hull. This should not report a collision
+	TEST_CASE("TestCollideShapeSmallTriangleVsLargeBox")
+	{
+		// Triangle vertices
+		Vec3 v0(-81.5637589f, -126.987244f, -146.771729f);
+		Vec3 v1(-81.8749924f, -127.270691f, -146.544403f);
+		Vec3 v2(-81.6972275f, -127.383545f, -146.773254f);
+
+		// Oriented box vertices
+		Array<Vec3> obox_points = {
+			Vec3(125.932892f, -374.712250f, 364.192169f),
+			Vec3(319.492218f, -73.2614441f, 475.009613f),
+			Vec3(-122.277550f, -152.200287f, 192.441437f),
+			Vec3(71.2817841f, 149.250519f, 303.258881f),
+			Vec3(-77.8921967f, -359.410797f, 678.579712f),
+			Vec3(115.667137f, -57.9600067f, 789.397095f),
+			Vec3(-326.102631f, -136.898834f, 506.828949f),
+			Vec3(-132.543304f, 164.551971f, 617.646362f)
+		};
+		ConvexHullShapeSettings hull_settings(obox_points, 0.0f);
+		RefConst<ConvexShape> convex_hull = StaticCast<ConvexShape>(hull_settings.Create().Get());
+
+		// Create triangle support function
+		TriangleConvexSupport triangle(v0, v1, v2);
+
+		// Create the convex hull support function
+		ConvexShape::SupportBuffer buffer;
+		const ConvexShape::Support *support = convex_hull->GetSupportFunction(ConvexShape::ESupportMode::IncludeConvexRadius, buffer, Vec3::sOne());
+
+		// Triangle is close enough to make GJK report indeterminate
+		Vec3 penetration_axis = Vec3::sAxisX(), point1, point2;
+		EPAPenetrationDepth pen_depth;
+		EPAPenetrationDepth::EStatus status = pen_depth.GetPenetrationDepthStepGJK(*support, support->GetConvexRadius(), triangle, 0.0f, cDefaultCollisionTolerance, penetration_axis, point1, point2);
+		CHECK(status == EPAPenetrationDepth::EStatus::Indeterminate);
+
+		// But there should not be an actual collision
+		CHECK(!pen_depth.GetPenetrationDepthStepEPA(*support, triangle, cDefaultPenetrationTolerance, penetration_axis, point1, point2));
+	}
+
+	// A test case of a triangle that's nearly parallel to a capsule and penetrating it. This one was causing numerical issues.
+	TEST_CASE("TestCollideParallelTriangleVsCapsule")
+	{
+		Vec3 v1(-0.479988575f, -1.36185002f, 0.269966960f);
+		Vec3 v2(-0.104996204f, 0.388152480f, 0.269967079f);
+		Vec3 v3(-0.104996204f, -1.36185002f, 0.269966960f);
+		TriangleShape triangle(v1, v2, v3);
+		triangle.SetEmbedded();
+
+		float capsule_radius = 0.37f;
+		float capsule_half_height = 0.5f;
+		CapsuleShape capsule(capsule_half_height, capsule_radius);
+		capsule.SetEmbedded();
+
+		CollideShapeSettings settings;
+		AllHitCollisionCollector<CollideShapeCollector> collector;
+		CollisionDispatch::sCollideShapeVsShape(&triangle, &capsule, Vec3::sOne(), Vec3::sOne(), Mat44::sIdentity(), Mat44::sIdentity(), SubShapeIDCreator(), SubShapeIDCreator(), settings, collector);
+
+		// The capsule's center is closest to the triangle's edge v2 v3
+		Vec3 capsule_center_to_triangle_v2_v3 = v3;
+		capsule_center_to_triangle_v2_v3.SetY(0); // The penetration axis will be in x, z only because the triangle is parallel to the capsule axis
+		float capsule_center_to_triangle_v2_v3_len = capsule_center_to_triangle_v2_v3.Length();
+		Vec3 expected_penetration_axis = -capsule_center_to_triangle_v2_v3 / capsule_center_to_triangle_v2_v3_len;
+		float expected_penetration_depth = capsule_radius - capsule_center_to_triangle_v2_v3_len;
+
+		CHECK(collector.mHits.size() == 1);
+		const CollideShapeResult &hit = collector.mHits[0];
+		Vec3 actual_penetration_axis = hit.mPenetrationAxis.Normalized();
+		float actual_penetration_depth = hit.mPenetrationDepth;
+
+		CHECK_APPROX_EQUAL(actual_penetration_axis, expected_penetration_axis);
+		CHECK_APPROX_EQUAL(actual_penetration_depth, expected_penetration_depth);
+	}
+
+	// A test case of a triangle that's nearly parallel to a capsule and penetrating it. This one was causing numerical issues.
+	TEST_CASE("TestCollideParallelTriangleVsCapsule2")
+	{
+		Vec3 v1(-0.0904417038f, -4.72410202f, 0.307858467f);
+		Vec3 v2(-0.0904417038f, 5.27589798f, 0.307857513f);
+		Vec3 v3(9.90955830f, 5.27589798f, 0.307864189f);
+		TriangleShape triangle(v1, v2, v3);
+		triangle.SetEmbedded();
+
+		float capsule_radius = 0.42f;
+		float capsule_half_height = 0.675f;
+		CapsuleShape capsule(capsule_half_height, capsule_radius);
+		capsule.SetEmbedded();
+
+		CollideShapeSettings settings;
+		AllHitCollisionCollector<CollideShapeCollector> collector;
+		CollisionDispatch::sCollideShapeVsShape(&triangle, &capsule, Vec3::sOne(), Vec3::sOne(), Mat44::sIdentity(), Mat44::sIdentity(), SubShapeIDCreator(), SubShapeIDCreator(), settings, collector);
+
+		// The capsule intersects with the triangle and the closest point is in the interior of the triangle
+		Vec3 expected_penetration_axis = Vec3(0, 0, -1); // Triangle is in the XY plane so the normal is Z
+		float expected_penetration_depth = capsule_radius - v1.GetZ();
+
+		CHECK(collector.mHits.size() == 1);
+		const CollideShapeResult &hit = collector.mHits[0];
+		Vec3 actual_penetration_axis = hit.mPenetrationAxis.Normalized();
+		float actual_penetration_depth = hit.mPenetrationDepth;
+
+		CHECK_APPROX_EQUAL(actual_penetration_axis, expected_penetration_axis);
+		CHECK_APPROX_EQUAL(actual_penetration_depth, expected_penetration_depth);
+	}
+
+	// A test case of a triangle that's nearly parallel to a capsule and almost penetrating it. This one was causing numerical issues.
+	TEST_CASE("TestCollideParallelTriangleVsCapsule3")
+	{
+		Vec3 v1(-0.474807739f, 17.2921791f, 0.212532043f);
+		Vec3 v2(-0.474807739f, -2.70782185f, 0.212535858f);
+		Vec3 v3(-0.857490540f, -2.70782185f, -0.711341858f);
+		TriangleShape triangle(v1, v2, v3);
+		triangle.SetEmbedded();
+
+		float capsule_radius = 0.5f;
+		float capsule_half_height = 0.649999976f;
+		CapsuleShape capsule(capsule_half_height, capsule_radius);
+		capsule.SetEmbedded();
+
+		CollideShapeSettings settings;
+		settings.mMaxSeparationDistance = 0.120000005f;
+		ClosestHitCollisionCollector<CollideShapeCollector> collector;
+		CollisionDispatch::sCollideShapeVsShape(&capsule, &triangle, Vec3::sOne(), Vec3::sOne(), Mat44::sIdentity(), Mat44::sIdentity(), SubShapeIDCreator(), SubShapeIDCreator(), settings, collector);
+
+		CHECK(collector.HadHit());
+		Vec3 expected_normal = (v2 - v1).Cross(v3 - v1).Normalized();
+		Vec3 actual_normal = -collector.mHit.mPenetrationAxis.Normalized();
+		CHECK_APPROX_EQUAL(actual_normal, expected_normal, 1.0e-6f);
+		float expected_penetration_depth = capsule.GetRadius() + v1.Dot(expected_normal);
+		CHECK_APPROX_EQUAL(collector.mHit.mPenetrationDepth, expected_penetration_depth, 1.0e-6f);
+	}
+
+	// A test case of a triangle that's nearly parallel to a cylinder and is just penetrating it. This one was causing numerical issues. See issue #1008.
+	TEST_CASE("TestCollideParallelTriangleVsCylinder")
+	{
+		CylinderShape cylinder(0.85f, 0.25f, 0.02f);
+		cylinder.SetEmbedded();
+
+		Mat44 cylinder_transform = Mat44::sTranslation(Vec3(-42.8155518f, -4.32299995f, 12.1734285f));
+
+		CollideShapeSettings settings;
+		settings.mMaxSeparationDistance = 0.001f;
+		ClosestHitCollisionCollector<CollideShapeCollector> collector;
+		CollideConvexVsTriangles c(&cylinder, Vec3::sOne(), Vec3::sOne(), cylinder_transform, Mat44::sIdentity(), SubShapeID(), settings, collector);
+
+		Vec3 v0(-42.7954292f, -0.647318780f, 12.4227943f);
+		Vec3 v1(-29.9111290f, -0.647318780f, 12.4227943f);
+		Vec3 v2(-42.7954292f, -4.86970234f, 12.4227943f);
+		c.Collide(v0, v1, v2, 0, SubShapeID());
+
+		// Check there was a hit
+		CHECK(collector.HadHit());
+		CHECK(collector.mHit.mPenetrationDepth < 1.0e-4f);
+		CHECK(collector.mHit.mPenetrationAxis.Normalized().IsClose(Vec3::sAxisZ()));
+	}
+
+	// A test case of a box and a convex hull that are nearly touching and that should return a contact with correct normal because the collision settings specify a max separation distance. This was producing the wrong normal.
+	TEST_CASE("BoxVsConvexHullNoConvexRadius")
+	{
+		const float separation_distance = 0.001f;
+		const float box_separation_from_hull = 0.5f * separation_distance;
+		const float hull_height = 0.25f;
+
+		// Box with no convex radius
+		Ref<BoxShapeSettings> box_settings = new BoxShapeSettings(Vec3(0.25f, 0.75f, 0.375f), 0.0f);
+		Ref<Shape> box_shape = box_settings->Create().Get();
+
+		// Convex hull (also a box) with no convex radius
+		Vec3 hull_points[] =
+		{
+			Vec3(-2.5f, -hull_height, -1.5f),
+			Vec3(-2.5f, hull_height, -1.5f),
+			Vec3(2.5f, -hull_height, -1.5f),
+			Vec3(-2.5f, -hull_height, 1.5f),
+			Vec3(-2.5f, hull_height, 1.5f),
+			Vec3(2.5f, hull_height, -1.5f),
+			Vec3(2.5f, -hull_height, 1.5f),
+			Vec3(2.5f, hull_height, 1.5f)
+		};
+		Ref<ConvexHullShapeSettings> hull_settings = new ConvexHullShapeSettings(hull_points, 8, 0.0f);
+		Ref<Shape> hull_shape = hull_settings->Create().Get();
+
+		float angle = 0.0f;
+		for (int i = 0; i < 481; ++i)
+		{
+			// Slowly rotate both box and convex hull
+			angle += DegreesToRadians(45.0f) / 60.0f;
+			Mat44 hull_transform = Mat44::sRotationY(angle);
+			const Mat44 box_local_translation = Mat44::sTranslation(Vec3(0.1f, 1.0f + box_separation_from_hull, -0.5f));
+			const Mat44 box_local_rotation = Mat44::sRotationY(DegreesToRadians(-45.0f));
+			const Mat44 box_local_transform = box_local_translation * box_local_rotation;
+			const Mat44 box_transform = hull_transform * box_local_transform;
+
+			CollideShapeSettings settings;
+			settings.mMaxSeparationDistance = separation_distance;
+			ClosestHitCollisionCollector<CollideShapeCollector> collector;
+			CollisionDispatch::sCollideShapeVsShape(box_shape, hull_shape, Vec3::sOne(), Vec3::sOne(), box_transform, hull_transform, SubShapeIDCreator(), SubShapeIDCreator(), settings, collector);
+
+			// Check that there was a hit and that the contact normal is correct
+			CHECK(collector.HadHit());
+			const CollideShapeResult &hit = collector.mHit;
+			CHECK_APPROX_EQUAL(hit.mContactPointOn1.GetY(), hull_height + box_separation_from_hull, 1.0e-3f);
+			CHECK_APPROX_EQUAL(hit.mContactPointOn2.GetY(), hull_height);
+			CHECK_APPROX_EQUAL(hit.mPenetrationAxis.NormalizedOr(Vec3::sZero()), -Vec3::sAxisY(), 1.0e-3f);
+		}
+
+		CHECK(angle >= 2.0f * JPH_PI);
+	}
+
+	// This test checks extreme values of the max separation distance and how it affects ConvexShape::sCollideConvexVsConvex
+	// See: https://github.com/jrouwe/JoltPhysics/discussions/1379
+	TEST_CASE("TestBoxVsSphereLargeSeparationDistance")
+	{
+		constexpr float cRadius = 1.0f;
+		constexpr float cHalfExtent = 10.0f;
+		RefConst<Shape> sphere_shape = new SphereShape(cRadius);
+		RefConst<Shape> box_shape = new BoxShape(Vec3::sReplicate(cHalfExtent));
+		float distances[] = { 0.0f, 0.5f, 1.0f, 5.0f, 10.0f, 50.0f, 100.0f, 500.0f, 1000.0f, 5000.0f, 10000.0f };
+		for (float x : distances)
+			for (float max_separation : distances)
+			{
+				CollideShapeSettings collide_settings;
+				collide_settings.mMaxSeparationDistance = max_separation;
+				ClosestHitCollisionCollector<CollideShapeCollector> collector;
+				CollisionDispatch::sCollideShapeVsShape(box_shape, sphere_shape, Vec3::sOne(), Vec3::sOne(), Mat44::sIdentity(), Mat44::sTranslation(Vec3(x, 0, 0)), SubShapeIDCreator(), SubShapeIDCreator(), collide_settings, collector);
+
+				float expected_penetration = cHalfExtent - (x - cRadius);
+				if (collector.HadHit())
+					CHECK_APPROX_EQUAL(expected_penetration, collector.mHit.mPenetrationDepth, 1.0e-3f);
+				else
+					CHECK(expected_penetration < -max_separation);
+			}
+	}
+
+	// This test case checks extreme values of the max separation distance and how it affects CollideConvexVsTriangles::Collide
+	// See: https://github.com/jrouwe/JoltPhysics/discussions/1379
+	TEST_CASE("TestTriangleVsBoxLargeSeparationDistance")
+	{
+		constexpr float cTriangleX = -0.1f;
+		constexpr float cHalfExtent = 10.0f;
+		RefConst<Shape> triangle_shape = new TriangleShape(Vec3(cTriangleX, -10, 10), Vec3(cTriangleX, -10, -10), Vec3(cTriangleX, 10, 0));
+		RefConst<Shape> box_shape = new BoxShape(Vec3::sReplicate(cHalfExtent));
+		float distances[] = { 0.0f, 0.5f, 1.0f, 5.0f, 10.0f, 50.0f, 100.0f, 500.0f, 1000.0f, 5000.0f, 10000.0f };
+		for (float x : distances)
+			for (float max_separation : distances)
+			{
+				CollideShapeSettings collide_settings;
+				collide_settings.mMaxSeparationDistance = max_separation;
+				ClosestHitCollisionCollector<CollideShapeCollector> collector;
+				CollisionDispatch::sCollideShapeVsShape(triangle_shape, box_shape, Vec3::sOne(), Vec3::sOne(), Mat44::sIdentity(), Mat44::sTranslation(Vec3(x, 0, 0)), SubShapeIDCreator(), SubShapeIDCreator(), collide_settings, collector);
+
+				float expected_penetration = cTriangleX - (x - cHalfExtent);
+				if (collector.HadHit())
+					CHECK_APPROX_EQUAL(expected_penetration, collector.mHit.mPenetrationDepth, 1.0e-3f);
+				else
+				{
+					CHECK(expected_penetration < -max_separation);
+					CHECK_APPROX_EQUAL(collector.mHit.mPenetrationAxis.NormalizedOr(Vec3::sZero()), Vec3::sAxisX(), 1.0e-5f);
+				}
+			}
+	}
+
+	TEST_CASE("TestCollideTriangleVsTriangle")
+	{
+		constexpr float cPenetration = 0.01f;
+
+		// A triangle centered around the origin in the XZ plane
+		RefConst<Shape> t1 = new TriangleShape(Vec3(-1, 0, 1), Vec3(1, 0, 1), Vec3(0, 0, -1));
+
+		// A triangle in the XY plane with its tip just pointing in the origin
+		RefConst<Shape> t2 = new TriangleShape(Vec3(-1, 1, 0), Vec3(1, 1, 0), Vec3(0, -cPenetration, 0));
+
+		CollideShapeSettings collide_settings;
+		ClosestHitCollisionCollector<CollideShapeCollector> collector;
+		CollisionDispatch::sCollideShapeVsShape(t1, t2, Vec3::sOne(), Vec3::sOne(), Mat44::sIdentity(), Mat44::sIdentity(), SubShapeIDCreator(), SubShapeIDCreator(), collide_settings, collector);
+
+		CHECK(collector.HadHit());
+		CHECK_APPROX_EQUAL(collector.mHit.mContactPointOn1, Vec3::sZero());
+		CHECK_APPROX_EQUAL(collector.mHit.mContactPointOn2, Vec3(0, -cPenetration, 0));
+		CHECK_APPROX_EQUAL(collector.mHit.mPenetrationDepth, cPenetration);
+		CHECK_APPROX_EQUAL(collector.mHit.mPenetrationAxis.Normalized(), Vec3(0, 1, 0));
+	}
+}

lib/All/JoltPhysics/UnitTests/Physics/CollisionGroupTests.cpp

@@ -0,0 +1,85 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include <Jolt/Physics/Collision/GroupFilterTable.h>
+#include "Layers.h"
+
+TEST_SUITE("CollisionGroupTests")
+{
+	TEST_CASE("TestCollisionGroup1")
+	{
+		// Test group filter with no sub groups
+		Ref<GroupFilterTable> group_filter = new GroupFilterTable;
+
+		// Check that doesn't collide with self
+		CollisionGroup g1(group_filter, 0, 0);
+		CHECK(!g1.CanCollide(g1));
+
+		// Check that collides with other group
+		CollisionGroup g2(group_filter, 1, 0);
+		CHECK(g1.CanCollide(g2));
+		CHECK(g2.CanCollide(g1));
+	}
+
+	TEST_CASE("TestCollisionGroup2")
+	{
+		// Test group filter with no sub groups
+		Ref<GroupFilterTable> group_filter1 = new GroupFilterTable(10);
+		Ref<GroupFilterTable> group_filter2 = new GroupFilterTable(10);
+
+		// Disable some pairs
+		using SubGroupPair = pair<CollisionGroup::SubGroupID, CollisionGroup::SubGroupID>;
+		Array<SubGroupPair> pairs = {
+			SubGroupPair(CollisionGroup::SubGroupID(1), CollisionGroup::SubGroupID(2)),
+			SubGroupPair(CollisionGroup::SubGroupID(9), CollisionGroup::SubGroupID(5)),
+			SubGroupPair(CollisionGroup::SubGroupID(3), CollisionGroup::SubGroupID(7)),
+			SubGroupPair(CollisionGroup::SubGroupID(6), CollisionGroup::SubGroupID(1)),
+			SubGroupPair(CollisionGroup::SubGroupID(8), CollisionGroup::SubGroupID(1))
+		};
+		for (const SubGroupPair &p : pairs)
+		{
+			group_filter1->DisableCollision(p.first, p.second);
+			group_filter2->DisableCollision(p.first, p.second);
+		}
+
+		for (CollisionGroup::SubGroupID i = 0; i < 10; ++i)
+			for (CollisionGroup::SubGroupID j = 0; j < 10; ++j)
+			{
+				// Check that doesn't collide with self
+				CollisionGroup g1(group_filter1, 0, i);
+				CHECK(!g1.CanCollide(g1));
+
+				// Same filter, same group, check if pairs collide
+				CollisionGroup g2(group_filter1, 0, j);
+				if (i == j
+					|| find(pairs.begin(), pairs.end(), SubGroupPair(i, j)) != pairs.end()
+					|| find(pairs.begin(), pairs.end(), SubGroupPair(j, i)) != pairs.end())
+				{
+					CHECK(!g1.CanCollide(g2));
+					CHECK(!g2.CanCollide(g1));
+				}
+				else
+				{
+					CHECK(g1.CanCollide(g2));
+					CHECK(g2.CanCollide(g1));
+				}
+
+				// Using different group always collides
+				CollisionGroup g3(group_filter1, 1, j);
+				CHECK(g1.CanCollide(g3));
+				CHECK(g3.CanCollide(g1));
+
+				// Using different filter with equal group should not collide
+				CollisionGroup g4(group_filter2, 0, j);
+				CHECK(!g1.CanCollide(g4));
+				CHECK(!g4.CanCollide(g1));
+
+				// Using different filter with non-equal group should collide
+				CollisionGroup g5(group_filter2, 1, j);
+				CHECK(g1.CanCollide(g5));
+				CHECK(g5.CanCollide(g1));
+			}
+	}
+}

lib/All/JoltPhysics/UnitTests/Physics/ContactListenerTests.cpp

@@ -0,0 +1,725 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include "PhysicsTestContext.h"
+#include "Layers.h"
+#include "LoggingContactListener.h"
+#include <Jolt/Physics/Collision/Shape/StaticCompoundShape.h>
+#include <Jolt/Physics/Collision/Shape/SphereShape.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+
+TEST_SUITE("ContactListenerTests")
+{
+	// Gravity vector
+	const Vec3 cGravity = Vec3(0.0f, -9.81f, 0.0f);
+
+	using LogEntry = LoggingContactListener::LogEntry;
+	using EType = LoggingContactListener::EType;
+
+	// Let a sphere bounce on the floor with restitution = 1
+	TEST_CASE("TestContactListenerElastic")
+	{
+		PhysicsTestContext c;
+
+		const float cSimulationTime = 1.0f;
+		const RVec3 cDistanceTraveled = c.PredictPosition(RVec3::sZero(), Vec3::sZero(), cGravity, cSimulationTime);
+		const float cFloorHitEpsilon = 1.0e-4f; // Apply epsilon so that we're sure that the collision algorithm will find a collision
+		const RVec3 cFloorHitPos(0.0f, 1.0f - cFloorHitEpsilon, 0.0f); // Sphere with radius 1 will hit floor when 1 above the floor
+		const RVec3 cInitialPos = cFloorHitPos - cDistanceTraveled;
+		const float cPenetrationSlop = c.GetSystem()->GetPhysicsSettings().mPenetrationSlop;
+
+		// Register listener
+		LoggingContactListener listener;
+		c.GetSystem()->SetContactListener(&listener);
+
+		// Create sphere
+		Body &floor = c.CreateFloor();
+		Body &body = c.CreateSphere(cInitialPos, 1.0f, EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING);
+		body.SetRestitution(1.0f);
+		CHECK(floor.GetID() < body.GetID());
+
+		// Simulate until at floor
+		c.Simulate(cSimulationTime);
+
+		// Assert collision not yet processed
+		CHECK(listener.GetEntryCount() == 0);
+
+		// Simulate one more step to process the collision
+		c.Simulate(c.GetDeltaTime());
+
+		// We expect a validate and a contact point added message
+		CHECK(listener.GetEntryCount() == 2);
+		if (listener.GetEntryCount() == 2)
+		{
+			// Check validate callback
+			const LogEntry &validate = listener.GetEntry(0);
+			CHECK(validate.mType == EType::Validate);
+			CHECK(validate.mBody1 == body.GetID()); // Dynamic body should always be the 1st
+			CHECK(validate.mBody2 == floor.GetID());
+
+			// Check add contact callback
+			const LogEntry &add_contact = listener.GetEntry(1);
+			CHECK(add_contact.mType == EType::Add);
+			CHECK(add_contact.mBody1 == floor.GetID()); // Lowest ID should be first
+			CHECK(add_contact.mManifold.mSubShapeID1.GetValue() == SubShapeID().GetValue()); // Floor doesn't have any sub shapes
+			CHECK(add_contact.mBody2 == body.GetID()); // Highest ID should be second
+			CHECK(add_contact.mManifold.mSubShapeID2.GetValue() == SubShapeID().GetValue()); // Sphere doesn't have any sub shapes
+			CHECK_APPROX_EQUAL(add_contact.mManifold.mWorldSpaceNormal, Vec3::sAxisY()); // Normal should move body 2 out of collision
+			CHECK(add_contact.mManifold.mRelativeContactPointsOn1.size() == 1);
+			CHECK(add_contact.mManifold.mRelativeContactPointsOn2.size() == 1);
+			CHECK(add_contact.mManifold.GetWorldSpaceContactPointOn1(0).IsClose(RVec3::sZero(), Square(cPenetrationSlop)));
+			CHECK(add_contact.mManifold.GetWorldSpaceContactPointOn2(0).IsClose(RVec3::sZero(), Square(cPenetrationSlop)));
+		}
+		listener.Clear();
+
+		// Simulate same time, with a fully elastic body we should reach the initial position again
+		c.Simulate(cSimulationTime);
+
+		// We should only have a remove contact point
+		CHECK(listener.GetEntryCount() == 1);
+		if (listener.GetEntryCount() == 1)
+		{
+			// Check remove contact callback
+			const LogEntry &remove = listener.GetEntry(0);
+			CHECK(remove.mType == EType::Remove);
+			CHECK(remove.mBody1 == floor.GetID()); // Lowest ID should be first
+			CHECK(remove.mBody2 == body.GetID()); // Highest ID should be second
+		}
+	}
+
+	// Let a sphere fall on the floor with restitution = 0, then give it horizontal velocity, then take it away from the floor
+	TEST_CASE("TestContactListenerInelastic")
+	{
+		PhysicsTestContext c;
+
+		const float cSimulationTime = 1.0f;
+		const RVec3 cDistanceTraveled = c.PredictPosition(RVec3::sZero(), Vec3::sZero(), cGravity, cSimulationTime);
+		const float cFloorHitEpsilon = 1.0e-4f; // Apply epsilon so that we're sure that the collision algorithm will find a collision
+		const RVec3 cFloorHitPos(0.0f, 1.0f - cFloorHitEpsilon, 0.0f); // Sphere with radius 1 will hit floor when 1 above the floor
+		const RVec3 cInitialPos = cFloorHitPos - cDistanceTraveled;
+		const float cPenetrationSlop = c.GetSystem()->GetPhysicsSettings().mPenetrationSlop;
+
+		// Register listener
+		LoggingContactListener listener;
+		c.GetSystem()->SetContactListener(&listener);
+
+		// Create sphere
+		Body &floor = c.CreateFloor();
+		Body &body = c.CreateSphere(cInitialPos, 1.0f, EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING);
+		body.SetRestitution(0.0f);
+		body.SetAllowSleeping(false);
+		CHECK(floor.GetID() < body.GetID());
+
+		// Simulate until at floor
+		c.Simulate(cSimulationTime);
+
+		// Assert collision not yet processed
+		CHECK(listener.GetEntryCount() == 0);
+
+		// Simulate one more step to process the collision
+		c.Simulate(c.GetDeltaTime());
+		CHECK_APPROX_EQUAL(body.GetPosition(), cFloorHitPos, cPenetrationSlop);
+
+		// We expect a validate and a contact point added message
+		CHECK(listener.GetEntryCount() == 2);
+		if (listener.GetEntryCount() == 2)
+		{
+			// Check validate callback
+			const LogEntry &validate = listener.GetEntry(0);
+			CHECK(validate.mType == EType::Validate);
+			CHECK(validate.mBody1 == body.GetID()); // Dynamic body should always be the 1st
+			CHECK(validate.mBody2 == floor.GetID());
+
+			// Check add contact callback
+			const LogEntry &add_contact = listener.GetEntry(1);
+			CHECK(add_contact.mType == EType::Add);
+			CHECK(add_contact.mBody1 == floor.GetID()); // Lowest ID first
+			CHECK(add_contact.mManifold.mSubShapeID1.GetValue() == SubShapeID().GetValue()); // Floor doesn't have any sub shapes
+			CHECK(add_contact.mBody2 == body.GetID()); // Highest ID second
+			CHECK(add_contact.mManifold.mSubShapeID2.GetValue() == SubShapeID().GetValue()); // Sphere doesn't have any sub shapes
+			CHECK_APPROX_EQUAL(add_contact.mManifold.mWorldSpaceNormal, Vec3::sAxisY()); // Normal should move body 2 out of collision
+			CHECK(add_contact.mManifold.mRelativeContactPointsOn1.size() == 1);
+			CHECK(add_contact.mManifold.mRelativeContactPointsOn2.size() == 1);
+			CHECK(add_contact.mManifold.GetWorldSpaceContactPointOn1(0).IsClose(RVec3::sZero(), Square(cPenetrationSlop)));
+			CHECK(add_contact.mManifold.GetWorldSpaceContactPointOn2(0).IsClose(RVec3::sZero(), Square(cPenetrationSlop)));
+		}
+		listener.Clear();
+
+		// Simulate 10 steps
+		c.Simulate(10 * c.GetDeltaTime());
+		CHECK_APPROX_EQUAL(body.GetPosition(), cFloorHitPos, cPenetrationSlop);
+
+		// We're not moving, we should have persisted contacts only
+		CHECK(listener.GetEntryCount() == 10);
+		for (size_t i = 0; i < listener.GetEntryCount(); ++i)
+		{
+			// Check persist callback
+			const LogEntry &persist_contact = listener.GetEntry(i);
+			CHECK(persist_contact.mType == EType::Persist);
+			CHECK(persist_contact.mBody1 == floor.GetID()); // Lowest ID first
+			CHECK(persist_contact.mManifold.mSubShapeID1.GetValue() == SubShapeID().GetValue()); // Floor doesn't have any sub shapes
+			CHECK(persist_contact.mBody2 == body.GetID()); // Highest ID second
+			CHECK(persist_contact.mManifold.mSubShapeID2.GetValue() == SubShapeID().GetValue()); // Sphere doesn't have any sub shapes
+			CHECK_APPROX_EQUAL(persist_contact.mManifold.mWorldSpaceNormal, Vec3::sAxisY()); // Normal should move body 2 out of collision
+			CHECK(persist_contact.mManifold.mRelativeContactPointsOn1.size() == 1);
+			CHECK(persist_contact.mManifold.mRelativeContactPointsOn2.size() == 1);
+			CHECK(persist_contact.mManifold.GetWorldSpaceContactPointOn1(0).IsClose(RVec3::sZero(), Square(cPenetrationSlop)));
+			CHECK(persist_contact.mManifold.GetWorldSpaceContactPointOn2(0).IsClose(RVec3::sZero(), Square(cPenetrationSlop)));
+		}
+		listener.Clear();
+
+		// Make the body able to go to sleep
+		body.SetAllowSleeping(true);
+
+		// Let the body go to sleep
+		c.Simulate(1.0f);
+		CHECK_APPROX_EQUAL(body.GetPosition(), cFloorHitPos, cPenetrationSlop);
+
+		// Check it went to sleep and that we received a contact removal callback
+		CHECK(!body.IsActive());
+		CHECK(listener.GetEntryCount() > 0);
+		for (size_t i = 0; i < listener.GetEntryCount(); ++i)
+		{
+			// Check persist / removed callbacks
+			const LogEntry &entry = listener.GetEntry(i);
+			CHECK(entry.mBody1 == floor.GetID());
+			CHECK(entry.mBody2 == body.GetID());
+			CHECK(entry.mType == ((i == listener.GetEntryCount() - 1)? EType::Remove : EType::Persist)); // The last entry should remove the contact as the body went to sleep
+		}
+		listener.Clear();
+
+		// Wake the body up again
+		c.GetBodyInterface().ActivateBody(body.GetID());
+		CHECK(body.IsActive());
+
+		// Simulate 1 time step to detect the collision with the floor again
+		c.SimulateSingleStep();
+
+		// Check that the contact got readded
+		CHECK(listener.GetEntryCount() == 2);
+		CHECK(listener.Contains(EType::Validate, floor.GetID(), body.GetID()));
+		CHECK(listener.Contains(EType::Add, floor.GetID(), body.GetID()));
+		listener.Clear();
+
+		// Prevent body from going to sleep again
+		body.SetAllowSleeping(false);
+
+		// Make the sphere move horizontal
+		body.SetLinearVelocity(Vec3::sAxisX());
+
+		// Simulate 10 steps
+		c.Simulate(10 * c.GetDeltaTime());
+
+		// We should have 10 persisted contacts events
+		int validate = 0;
+		int persisted = 0;
+		for (size_t i = 0; i < listener.GetEntryCount(); ++i)
+		{
+			const LogEntry &entry = listener.GetEntry(i);
+			switch (entry.mType)
+			{
+			case EType::Validate:
+				++validate;
+				break;
+
+			case EType::Persist:
+				// Check persist callback
+				CHECK(entry.mBody1 == floor.GetID()); // Lowest ID first
+				CHECK(entry.mManifold.mSubShapeID1.GetValue() == SubShapeID().GetValue()); // Floor doesn't have any sub shapes
+				CHECK(entry.mBody2 == body.GetID()); // Highest ID second
+				CHECK(entry.mManifold.mSubShapeID2.GetValue() == SubShapeID().GetValue()); // Sphere doesn't have any sub shapes
+				CHECK_APPROX_EQUAL(entry.mManifold.mWorldSpaceNormal, Vec3::sAxisY()); // Normal should move body 2 out of collision
+				CHECK(entry.mManifold.mRelativeContactPointsOn1.size() == 1);
+				CHECK(entry.mManifold.mRelativeContactPointsOn2.size() == 1);
+				CHECK(abs(entry.mManifold.GetWorldSpaceContactPointOn1(0).GetY()) < cPenetrationSlop);
+				CHECK(abs(entry.mManifold.GetWorldSpaceContactPointOn2(0).GetY()) < cPenetrationSlop);
+				++persisted;
+				break;
+
+			case EType::Add:
+			case EType::Remove:
+			default:
+				CHECK(false); // Unexpected event
+			}
+		}
+		CHECK(validate <= 10); // We may receive extra validate callbacks when the object is moving
+		CHECK(persisted == 10);
+		listener.Clear();
+
+		// Move the sphere away from the floor
+		c.GetBodyInterface().SetPosition(body.GetID(), cInitialPos, EActivation::Activate);
+
+		// Simulate 10 steps
+		c.Simulate(10 * c.GetDeltaTime());
+
+		// We should only have a remove contact point
+		CHECK(listener.GetEntryCount() == 1);
+		if (listener.GetEntryCount() == 1)
+		{
+			// Check remove contact callback
+			const LogEntry &remove = listener.GetEntry(0);
+			CHECK(remove.mType == EType::Remove);
+			CHECK(remove.mBody1 == floor.GetID()); // Lowest ID first
+			CHECK(remove.mBody2 == body.GetID()); // Highest ID second
+		}
+	}
+
+	TEST_CASE("TestWereBodiesInContact")
+	{
+		for (int sign = -1; sign <= 1; sign += 2)
+		{
+			PhysicsTestContext c;
+
+			PhysicsSystem *s = c.GetSystem();
+			BodyInterface &bi = c.GetBodyInterface();
+
+			Body &floor = c.CreateFloor();
+
+			// Two spheres at a distance so that when one sphere leaves the floor the body can still be touching the floor with the other sphere
+			Ref<StaticCompoundShapeSettings> compound_shape = new StaticCompoundShapeSettings;
+			compound_shape->AddShape(Vec3(-2, 0, 0), Quat::sIdentity(), new SphereShape(1));
+			compound_shape->AddShape(Vec3(2, 0, 0), Quat::sIdentity(), new SphereShape(1));
+			Body &body = *bi.CreateBody(BodyCreationSettings(compound_shape, RVec3(0, 0.999f, 0), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING));
+			bi.AddBody(body.GetID(), EActivation::Activate);
+
+			class ContactListenerImpl : public ContactListener
+			{
+			public:
+								ContactListenerImpl(PhysicsSystem *inSystem) : mSystem(inSystem) { }
+
+				virtual void	OnContactAdded(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings) override
+				{
+					++mAdded;
+				}
+
+				virtual void	OnContactRemoved(const SubShapeIDPair &inSubShapePair) override
+				{
+					++mRemoved;
+					mWasInContact = mSystem->WereBodiesInContact(inSubShapePair.GetBody1ID(), inSubShapePair.GetBody2ID());
+					CHECK(mWasInContact == mSystem->WereBodiesInContact(inSubShapePair.GetBody2ID(), inSubShapePair.GetBody1ID())); // Returned value should be the same regardless of order
+				}
+
+				int				GetAddCount() const
+				{
+					return mAdded - mRemoved;
+				}
+
+				void			Reset()
+				{
+					mAdded = 0;
+					mRemoved = 0;
+					mWasInContact = false;
+				}
+
+				PhysicsSystem *	mSystem;
+
+				int				mAdded = 0;
+
+				int				mRemoved = 0;
+				bool			mWasInContact = false;
+			};
+
+			// Set listener
+			ContactListenerImpl listener(s);
+			s->SetContactListener(&listener);
+
+			// If the simulation hasn't run yet, we can't be in contact
+			CHECK(!s->WereBodiesInContact(floor.GetID(), body.GetID()));
+
+			// Step the simulation to allow detecting the contact
+			c.SimulateSingleStep();
+
+			// Should be in contact now
+			CHECK(s->WereBodiesInContact(floor.GetID(), body.GetID()));
+			CHECK(s->WereBodiesInContact(body.GetID(), floor.GetID()));
+			CHECK(listener.GetAddCount() == 1);
+			listener.Reset();
+
+			// Impulse on one side
+			bi.AddImpulse(body.GetID(), Vec3(0, 10000, 0), RVec3(Real(-sign * 2), 0, 0));
+			c.SimulateSingleStep(); // One step to detach from the ground (but starts penetrating so will not send a remove callback)
+			CHECK(listener.GetAddCount() == 0);
+			c.SimulateSingleStep(); // One step to get contact remove callback
+
+			// Should still be in contact
+			// Note that we may get a remove and an add callback because manifold reduction has combined the collision with both spheres into 1 contact manifold.
+			// At that point it has to select one of the sub shapes for the contact and if that sub shape no longer collides we get a remove for this sub shape and then an add callback for the other sub shape.
+			CHECK(s->WereBodiesInContact(floor.GetID(), body.GetID()));
+			CHECK(s->WereBodiesInContact(body.GetID(), floor.GetID()));
+			CHECK(listener.GetAddCount() == 0);
+			CHECK((listener.mRemoved == 0 || listener.mWasInContact));
+			listener.Reset();
+
+			// Impulse on the other side
+			bi.AddImpulse(body.GetID(), Vec3(0, 10000, 0), RVec3(Real(sign * 2), 0, 0));
+			c.SimulateSingleStep(); // One step to detach from the ground (but starts penetrating so will not send a remove callback)
+			CHECK(listener.GetAddCount() == 0);
+			c.SimulateSingleStep(); // One step to get contact remove callback
+
+			// Should no longer be in contact
+			CHECK(!s->WereBodiesInContact(floor.GetID(), body.GetID()));
+			CHECK(!s->WereBodiesInContact(body.GetID(), floor.GetID()));
+			CHECK(listener.GetAddCount() == -1);
+			CHECK((listener.mRemoved == 1 && !listener.mWasInContact));
+		}
+	}
+
+	TEST_CASE("TestSurfaceVelocity")
+	{
+		PhysicsTestContext c;
+
+		Body &floor = c.CreateBox(RVec3(0, -1, 0), Quat::sRotation(Vec3::sAxisY(), DegreesToRadians(10.0f)), EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, Vec3(100.0f, 1.0f, 100.0f));
+		floor.SetFriction(1.0f);
+
+		for (int iteration = 0; iteration < 2; ++iteration)
+		{
+			Body &box = c.CreateBox(RVec3(0, 0.999f, 0), Quat::sRotation(Vec3::sAxisY(), DegreesToRadians(30.0f)), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3::sOne());
+			box.SetFriction(1.0f);
+
+			// Contact listener sets a constant surface velocity
+			class ContactListenerImpl : public ContactListener
+			{
+			public:
+								ContactListenerImpl(Body &inFloor, Body &inBox) : mFloor(inFloor), mBox(inBox) { }
+
+				virtual void	OnContactAdded(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings) override
+				{
+					// Ensure that the body order is as expected
+					JPH_ASSERT(inBody1.GetID() == mFloor.GetID() || inBody2.GetID() == mBox.GetID());
+
+					// Calculate the relative surface velocity
+					ioSettings.mRelativeLinearSurfaceVelocity = -(inBody1.GetRotation() * mLocalSpaceLinearVelocity);
+					ioSettings.mRelativeAngularSurfaceVelocity = -(inBody1.GetRotation() * mLocalSpaceAngularVelocity);
+				}
+
+				virtual void	OnContactPersisted(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings) override
+				{
+					OnContactAdded(inBody1, inBody2, inManifold, ioSettings);
+				}
+
+				Body &			mFloor;
+				Body &			mBox;
+				Vec3			mLocalSpaceLinearVelocity;
+				Vec3			mLocalSpaceAngularVelocity;
+			};
+
+			// Set listener
+			ContactListenerImpl listener(floor, box);
+			c.GetSystem()->SetContactListener(&listener);
+
+			// Set linear velocity or angular velocity depending on the iteration
+			listener.mLocalSpaceLinearVelocity = iteration == 0? Vec3(0, 0, -2.0f) : Vec3::sZero();
+			listener.mLocalSpaceAngularVelocity = iteration == 0? Vec3::sZero() : Vec3(0, DegreesToRadians(30.0f), 0);
+
+			// Simulate
+			c.Simulate(5.0f);
+
+			// Check that the box is moving with the correct linear/angular velocity
+			CHECK_APPROX_EQUAL(box.GetLinearVelocity(), floor.GetRotation() * listener.mLocalSpaceLinearVelocity, 0.005f);
+			CHECK_APPROX_EQUAL(box.GetAngularVelocity(), floor.GetRotation() * listener.mLocalSpaceAngularVelocity, 1.0e-4f);
+		}
+	}
+
+	static float sGetInvMassScale(const Body &inBody)
+	{
+		uint64 ud = inBody.GetUserData();
+		int index = ((ud & 1) != 0? (ud >> 1) : (ud >> 3)) & 0b11;
+		float mass_overrides[] = { 1.0f, 0.0f, 0.5f, 2.0f };
+		return mass_overrides[index];
+	}
+
+	TEST_CASE("TestMassOverride")
+	{
+		for (EMotionType m1 = EMotionType::Static; m1 <= EMotionType::Dynamic; m1 = EMotionType((int)m1 + 1))
+			for (EMotionType m2 = EMotionType::Static; m2 <= EMotionType::Dynamic; m2 = EMotionType((int)m2 + 1))
+				if (m1 != EMotionType::Static || m2 != EMotionType::Static)
+					for (int i = 0; i < 16; ++i)
+					{
+						PhysicsTestContext c;
+						c.ZeroGravity();
+
+						const float cInitialVelocity1 = m1 != EMotionType::Static? 3.0f : 0.0f;
+						const float cInitialVelocity2 = m2 != EMotionType::Static? -4.0f : 0.0f;
+
+						// Create two spheres on a collision course
+						BodyCreationSettings bcs(new SphereShape(1.0f), RVec3::sZero(), Quat::sIdentity(), m1, m1 != EMotionType::Static? Layers::MOVING : Layers::NON_MOVING);
+						bcs.mOverrideMassProperties = EOverrideMassProperties::CalculateInertia;
+						bcs.mMassPropertiesOverride.mMass = 1.0f;
+						bcs.mRestitution = 1.0f;
+						bcs.mLinearDamping = 0.0f;
+						bcs.mPosition = RVec3(-2, 0, 0);
+						bcs.mLinearVelocity = Vec3(cInitialVelocity1, 0, 0);
+						bcs.mUserData = i << 1;
+						Body &body1 = *c.GetBodyInterface().CreateBody(bcs);
+						c.GetBodyInterface().AddBody(body1.GetID(), EActivation::Activate);
+
+						bcs.mMotionType = m2;
+						bcs.mObjectLayer = m2 != EMotionType::Static? Layers::MOVING : Layers::NON_MOVING;
+						bcs.mMassPropertiesOverride.mMass = 2.0f;
+						bcs.mPosition = RVec3(2, 0, 0);
+						bcs.mLinearVelocity = Vec3(cInitialVelocity2, 0, 0);
+						bcs.mUserData++;
+						Body &body2 = *c.GetBodyInterface().CreateBody(bcs);
+						c.GetBodyInterface().AddBody(body2.GetID(), EActivation::Activate);
+
+						// Contact listener that modifies mass
+						class ContactListenerImpl : public ContactListener
+						{
+						public:
+							virtual void	OnContactAdded(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings) override
+							{
+								// Override the mass of body 1
+								float scale1 = sGetInvMassScale(inBody1);
+								ioSettings.mInvMassScale1 = scale1;
+								ioSettings.mInvInertiaScale1 = scale1;
+
+								// Override the mass of body 2
+								float scale2 = sGetInvMassScale(inBody2);
+								ioSettings.mInvMassScale2 = scale2;
+								ioSettings.mInvInertiaScale2 = scale2;
+							}
+
+							virtual void	OnContactPersisted(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings) override
+							{
+								OnContactAdded(inBody1, inBody2, inManifold, ioSettings);
+							}
+						};
+
+						// Set listener
+						ContactListenerImpl listener;
+						c.GetSystem()->SetContactListener(&listener);
+
+						// Simulate
+						c.Simulate(1.0f);
+
+						// Calculate resulting inverse mass
+						float inv_m1 = body1.GetMotionType() == EMotionType::Dynamic? sGetInvMassScale(body1) * body1.GetMotionProperties()->GetInverseMass() : 0.0f;
+						float inv_m2 = body2.GetMotionType() == EMotionType::Dynamic? sGetInvMassScale(body2) * body2.GetMotionProperties()->GetInverseMass() : 0.0f;
+
+						float v1, v2;
+						if (inv_m1 == 0.0f && inv_m2 == 0.0f)
+						{
+							// If both bodies became kinematic they will pass through each other
+							v1 = cInitialVelocity1;
+							v2 = cInitialVelocity2;
+						}
+						else
+						{
+							// Calculate resulting velocity using conservation of momentum and energy
+							// See: https://en.wikipedia.org/wiki/Elastic_collision where m1 = 1 / inv_m1 and m2 = 1 / inv_m2
+							v1 = (2.0f * inv_m1 * cInitialVelocity2 + (inv_m2 - inv_m1) * cInitialVelocity1) / (inv_m1 + inv_m2);
+							v2 = (2.0f * inv_m2 * cInitialVelocity1 + (inv_m1 - inv_m2) * cInitialVelocity2) / (inv_m1 + inv_m2);
+						}
+
+						// Check that the spheres move according to their overridden masses
+						CHECK_APPROX_EQUAL(body1.GetLinearVelocity(), Vec3(v1, 0, 0));
+						CHECK_APPROX_EQUAL(body2.GetLinearVelocity(), Vec3(v2, 0, 0));
+					}
+	}
+
+	TEST_CASE("TestInfiniteMassOverride")
+	{
+		for (bool do_swap : { false, true })
+			for (EMotionQuality quality : { EMotionQuality::Discrete, EMotionQuality::LinearCast })
+			{
+				// A contact listener that makes a body have infinite mass
+				class ContactListenerImpl : public ContactListener
+				{
+				public:
+									ContactListenerImpl(const BodyID &inBodyID) : mBodyID(inBodyID) { }
+
+					virtual void	OnContactAdded(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings) override
+					{
+						if (mBodyID == inBody1.GetID())
+						{
+							ioSettings.mInvInertiaScale1 = 0.0f;
+							ioSettings.mInvMassScale1 = 0.0f;
+						}
+						else if (mBodyID == inBody2.GetID())
+						{
+							ioSettings.mInvInertiaScale2 = 0.0f;
+							ioSettings.mInvMassScale2 = 0.0f;
+						}
+					}
+
+					virtual void	OnContactPersisted(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings) override
+					{
+						OnContactAdded(inBody1, inBody2, inManifold, ioSettings);
+					}
+
+				private:
+					BodyID			mBodyID;
+				};
+
+				PhysicsTestContext c;
+				c.ZeroGravity();
+
+				// Create a box
+				const RVec3 cInitialBoxPos(0, 2, 0);
+				BodyCreationSettings box_settings(new BoxShape(Vec3::sReplicate(2)), cInitialBoxPos, Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING);
+				box_settings.mOverrideMassProperties = EOverrideMassProperties::CalculateInertia;
+				box_settings.mMassPropertiesOverride.mMass = 1.0f;
+
+				// Create a sphere
+				BodyCreationSettings sphere_settings(new SphereShape(2), RVec3(30, 2, 0), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING);
+				sphere_settings.mLinearVelocity = Vec3(-100, 0, 0);
+				sphere_settings.mOverrideMassProperties = EOverrideMassProperties::CalculateInertia;
+				sphere_settings.mMassPropertiesOverride.mMass = 10.0f;
+				sphere_settings.mRestitution = 0.1f;
+				sphere_settings.mLinearDamping = 0.0f;
+				sphere_settings.mMotionQuality = quality;
+
+				BodyID box_id, sphere_id;
+				if (do_swap)
+				{
+					// Swap the bodies so that the contact listener will receive the bodies in the opposite order
+					sphere_id = c.GetBodyInterface().CreateAndAddBody(sphere_settings, EActivation::Activate);
+					box_id = c.GetBodyInterface().CreateAndAddBody(box_settings, EActivation::Activate);
+				}
+				else
+				{
+					box_id = c.GetBodyInterface().CreateAndAddBody(box_settings, EActivation::Activate);
+					sphere_id = c.GetBodyInterface().CreateAndAddBody(sphere_settings, EActivation::Activate);
+				}
+
+				// Add listener that will make the box have infinite mass
+				ContactListenerImpl listener(box_id);
+				c.GetSystem()->SetContactListener(&listener);
+
+				// Simulate
+				const float cSimulationTime = 0.3f;
+				c.Simulate(cSimulationTime);
+
+				// Check that the box didn't move
+				BodyInterface &bi = c.GetBodyInterface();
+				CHECK(bi.GetPosition(box_id) == cInitialBoxPos);
+				CHECK(bi.GetLinearVelocity(box_id) == Vec3::sZero());
+				CHECK(bi.GetAngularVelocity(box_id) == Vec3::sZero());
+
+				// Check that the sphere bounced off the box
+				CHECK_APPROX_EQUAL(bi.GetLinearVelocity(sphere_id), -sphere_settings.mLinearVelocity * sphere_settings.mRestitution);
+			}
+		}
+
+	TEST_CASE("TestCollideKinematicVsNonDynamic")
+	{
+		for (EMotionType m1 = EMotionType::Static; m1 <= EMotionType::Dynamic; m1 = EMotionType((int)m1 + 1))
+			for (int allow1 = 0; allow1 < 2; ++allow1)
+				for (int active1 = 0; active1 < 2; ++active1)
+					for (EMotionType m2 = EMotionType::Static; m2 <= EMotionType::Dynamic; m2 = EMotionType((int)m2 + 1))
+						for (int allow2 = 0; allow2 < 2; ++allow2)
+							for (int active2 = 0; active2 < 2; ++active2)
+								if ((m1 != EMotionType::Static && active1) || (m2 != EMotionType::Static && active2))
+								{
+									PhysicsTestContext c;
+									c.ZeroGravity();
+
+									const Vec3 cInitialVelocity1(m1 != EMotionType::Static && active1 != 0? 1.0f : 0.0f, 0, 0);
+									const Vec3 cInitialVelocity2(m2 != EMotionType::Static && active2 != 0? -1.0f : 0.0f, 0, 0);
+
+									// Create two spheres that are colliding initially
+									BodyCreationSettings bcs(new SphereShape(1.0f), RVec3::sZero(), Quat::sIdentity(), m1, m1 != EMotionType::Static? Layers::MOVING : Layers::NON_MOVING);
+									bcs.mPosition = RVec3(-0.5_r, 0, 0);
+									bcs.mLinearVelocity = cInitialVelocity1;
+									bcs.mCollideKinematicVsNonDynamic = allow1 != 0;
+									Body &body1 = *c.GetBodyInterface().CreateBody(bcs);
+									c.GetBodyInterface().AddBody(body1.GetID(), active1 != 0? EActivation::Activate : EActivation::DontActivate);
+
+									bcs.mMotionType = m2;
+									bcs.mObjectLayer = m2 != EMotionType::Static? Layers::MOVING : Layers::NON_MOVING;
+									bcs.mPosition = RVec3(0.5_r, 0, 0);
+									bcs.mLinearVelocity = cInitialVelocity2;
+									bcs.mCollideKinematicVsNonDynamic = allow2 != 0;
+									Body &body2 = *c.GetBodyInterface().CreateBody(bcs);
+									c.GetBodyInterface().AddBody(body2.GetID(), active2 != 0? EActivation::Activate : EActivation::DontActivate);
+
+									// Set listener
+									LoggingContactListener listener;
+									c.GetSystem()->SetContactListener(&listener);
+
+									// Step
+									c.SimulateSingleStep();
+
+									if ((allow1 || allow2) // In this case we always get a callback
+										|| (m1 == EMotionType::Dynamic || m2 == EMotionType::Dynamic)) // Otherwise we only get a callback when one of the bodies is dynamic
+									{
+										// Check that we received a callback
+										CHECK(listener.GetEntryCount() == 2);
+										CHECK(listener.Contains(EType::Validate, body1.GetID(), body2.GetID()));
+										CHECK(listener.Contains(EType::Add, body1.GetID(), body2.GetID()));
+									}
+									else
+									{
+										// No collision events should have been received
+										CHECK(listener.GetEntryCount() == 0);
+									}
+
+									// Velocities should only change if the body is dynamic
+									if (m1 == EMotionType::Dynamic)
+									{
+										CHECK(body1.GetLinearVelocity() != cInitialVelocity1);
+										CHECK(body1.IsActive());
+									}
+									else
+										CHECK(body1.GetLinearVelocity() == cInitialVelocity1);
+
+									if (m2 == EMotionType::Dynamic)
+									{
+										CHECK(body2.GetLinearVelocity() != cInitialVelocity2);
+										CHECK(body2.IsActive());
+									}
+									else
+										CHECK(body2.GetLinearVelocity() == cInitialVelocity2);
+								}
+	}
+
+	// Test that an update with zero delta time doesn't generate contact callbacks
+	TEST_CASE("TestZeroDeltaTime")
+	{
+		PhysicsTestContext c;
+
+		// Register listener
+		LoggingContactListener listener;
+		c.GetSystem()->SetContactListener(&listener);
+
+		// Create a sphere that intersects with the floor
+		Body &floor = c.CreateFloor();
+		Body &body1 = c.CreateSphere(RVec3::sZero(), 1.0f, EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING);
+
+		// Step with zero delta time
+		c.GetSystem()->Update(0.0f, 1, c.GetTempAllocator(), c.GetJobSystem());
+
+		// No callbacks should trigger when delta time is zero
+		CHECK(listener.GetEntryCount() == 0);
+
+		// Simulate for 1 step
+		c.SimulateSingleStep();
+
+		// We expect a validate and a contact point added message
+		CHECK(listener.GetEntryCount() == 2);
+		CHECK(listener.Contains(EType::Validate, floor.GetID(), body1.GetID()));
+		CHECK(listener.Contains(EType::Add, floor.GetID(), body1.GetID()));
+		listener.Clear();
+
+		// Create a 2nd sphere that intersects with the floor
+		Body &body2 = c.CreateSphere(RVec3(4, 0, 0), 1.0f, EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING);
+
+		// Step with zero delta time
+		c.GetSystem()->Update(0.0f, 1, c.GetTempAllocator(), c.GetJobSystem());
+
+		// No callbacks should trigger when delta time is zero
+		CHECK(listener.GetEntryCount() == 0);
+
+		// Simulate for 1 step
+		c.SimulateSingleStep();
+
+		// We expect callbacks for both bodies now
+		CHECK(listener.GetEntryCount() == 4);
+		CHECK(listener.Contains(EType::Validate, floor.GetID(), body1.GetID()));
+		CHECK(listener.Contains(EType::Persist, floor.GetID(), body1.GetID()));
+		CHECK(listener.Contains(EType::Validate, floor.GetID(), body2.GetID()));
+		CHECK(listener.Contains(EType::Add, floor.GetID(), body2.GetID()));
+	}
+}

lib/All/JoltPhysics/UnitTests/Physics/ConvexVsTrianglesTest.cpp

@@ -0,0 +1,361 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include "PhysicsTestContext.h"
+#include <Jolt/Physics/Collision/Shape/SphereShape.h>
+#include <Jolt/Physics/Collision/Shape/TriangleShape.h>
+#include <Jolt/Physics/Collision/Shape/MeshShape.h>
+#include <Jolt/Physics/Collision/CollideShape.h>
+#include <Jolt/Physics/Collision/CollisionCollectorImpl.h>
+#include <Jolt/Physics/Collision/CollideConvexVsTriangles.h>
+#include <Jolt/Physics/Collision/CollideSphereVsTriangles.h>
+#include "Layers.h"
+
+TEST_SUITE("ConvexVsTrianglesTest")
+{
+	static constexpr float cEdgeLength = 4.0f;
+
+	template <class Collider>
+	static void sCheckCollisionNoHit(const CollideShapeSettings &inSettings, Vec3Arg inCenter, float inRadius, uint8 inActiveEdges)
+	{
+		// Our sphere
+		Ref<SphereShape> sphere = new SphereShape(inRadius);
+
+		// Our default triangle
+		Vec3 v1(0, 0, 0);
+		Vec3 v2(0, 0, cEdgeLength);
+		Vec3 v3(cEdgeLength, 0, 0);
+
+		{
+			// Collide sphere
+			AllHitCollisionCollector<CollideShapeCollector> collector;
+			Collider collider(sphere, Vec3::sOne(), Vec3::sOne(), Mat44::sTranslation(inCenter), Mat44::sIdentity(), SubShapeID(), inSettings, collector);
+			collider.Collide(v1, v2, v3, inActiveEdges, SubShapeID());
+			CHECK(!collector.HadHit());
+		}
+
+		// A triangle shape has all edges active, so only test if all edges are active
+		if (inActiveEdges == 0b111)
+		{
+			// Create the triangle shape
+			PhysicsTestContext context;
+			context.CreateBody(new TriangleShapeSettings(v1, v2, v3), RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, EActivation::DontActivate);
+
+			// Collide sphere
+			AllHitCollisionCollector<CollideShapeCollector> collector;
+			context.GetSystem()->GetNarrowPhaseQuery().CollideShape(sphere, Vec3::sOne(), RMat44::sTranslation(RVec3(inCenter)), inSettings, RVec3::sZero(), collector);
+			CHECK(!collector.HadHit());
+		}
+
+		// A mesh shape with a single triangle has all edges active, so only test if all edges are active
+		if (inActiveEdges == 0b111)
+		{
+			// Create a mesh with a single triangle
+			TriangleList triangles;
+			triangles.push_back(Triangle(v1, v2, v3));
+			PhysicsTestContext context;
+			context.CreateBody(new MeshShapeSettings(triangles), RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, EActivation::DontActivate);
+
+			// Collide sphere
+			AllHitCollisionCollector<CollideShapeCollector> collector;
+			context.GetSystem()->GetNarrowPhaseQuery().CollideShape(sphere, Vec3::sOne(), RMat44::sTranslation(RVec3(inCenter)), inSettings, RVec3::sZero(), collector);
+			CHECK(!collector.HadHit());
+		}
+	}
+
+	template <class Collider>
+	static void sCheckCollision(const CollideShapeSettings &inSettings, Vec3Arg inCenter, float inRadius, uint8 inActiveEdges, Vec3Arg inExpectedContactOn1, Vec3Arg inExpectedContactOn2, Vec3Arg inExpectedPenetrationAxis, float inExpectedPenetrationDepth)
+	{
+		// Our sphere
+		Ref<SphereShape> sphere = new SphereShape(inRadius);
+
+		// Our default triangle
+		Vec3 v1(0, 0, 0);
+		Vec3 v2(0, 0, cEdgeLength);
+		Vec3 v3(cEdgeLength, 0, 0);
+
+		// A semi random transform for the triangle
+		Vec3 translation = Vec3(1, 2, 3);
+		Quat rotation = Quat::sRotation(Vec3::sAxisX(), 0.25f * JPH_PI);
+		Mat44 transform = Mat44::sRotationTranslation(rotation, translation);
+		Mat44 inv_transform = transform.InversedRotationTranslation();
+
+		// The transform for the sphere
+		Mat44 sphere_transform = transform * Mat44::sTranslation(inCenter);
+
+		// Transform incoming settings
+		CollideShapeSettings settings = inSettings;
+		settings.mActiveEdgeMovementDirection = transform.Multiply3x3(inSettings.mActiveEdgeMovementDirection);
+
+		// Test the specified collider
+		{
+			SubShapeID sub_shape_id1, sub_shape_id2;
+			sub_shape_id1.SetValue(123);
+			sub_shape_id2.SetValue(456);
+
+			// Collide sphere
+			AllHitCollisionCollector<CollideShapeCollector> collector;
+			Collider collider(sphere, Vec3::sOne(), Vec3::sOne(), sphere_transform, transform, sub_shape_id1, settings, collector);
+			collider.Collide(v1, v2, v3, inActiveEdges, sub_shape_id2);
+
+			// Test result
+			CHECK(collector.mHits.size() == 1);
+			const CollideShapeResult &hit = collector.mHits[0];
+			CHECK(hit.mBodyID2 == BodyID());
+			CHECK(hit.mSubShapeID1.GetValue() == sub_shape_id1.GetValue());
+			CHECK(hit.mSubShapeID2.GetValue() == sub_shape_id2.GetValue());
+			Vec3 contact1 = inv_transform * hit.mContactPointOn1;
+			Vec3 contact2 = inv_transform * hit.mContactPointOn2;
+			Vec3 pen_axis = transform.Multiply3x3Transposed(hit.mPenetrationAxis).Normalized();
+			Vec3 expected_pen_axis = inExpectedPenetrationAxis.Normalized();
+			CHECK_APPROX_EQUAL(contact1, inExpectedContactOn1, 1.0e-4f);
+			CHECK_APPROX_EQUAL(contact2, inExpectedContactOn2, 1.0e-4f);
+			CHECK_APPROX_EQUAL(pen_axis, expected_pen_axis, 1.0e-4f);
+			CHECK_APPROX_EQUAL(hit.mPenetrationDepth, inExpectedPenetrationDepth, 1.0e-4f);
+		}
+
+		// A triangle shape has all edges active, so only test if all edges are active
+		if (inActiveEdges == 0b111)
+		{
+			// Create the triangle shape
+			PhysicsTestContext context;
+			Body &body = context.CreateBody(new TriangleShapeSettings(v1, v2, v3), RVec3(translation), rotation, EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, EActivation::DontActivate);
+
+			// Collide sphere
+			AllHitCollisionCollector<CollideShapeCollector> collector;
+			context.GetSystem()->GetNarrowPhaseQuery().CollideShape(sphere, Vec3::sOne(), RMat44(sphere_transform), settings, RVec3::sZero(), collector);
+
+			// Test result
+			CHECK(collector.mHits.size() == 1);
+			const CollideShapeResult &hit = collector.mHits[0];
+			CHECK(hit.mBodyID2 == body.GetID());
+			CHECK(hit.mSubShapeID1.GetValue() == SubShapeID().GetValue());
+			CHECK(hit.mSubShapeID2.GetValue() == SubShapeID().GetValue());
+			Vec3 contact1 = inv_transform * hit.mContactPointOn1;
+			Vec3 contact2 = inv_transform * hit.mContactPointOn2;
+			Vec3 pen_axis = transform.Multiply3x3Transposed(hit.mPenetrationAxis).Normalized();
+			Vec3 expected_pen_axis = inExpectedPenetrationAxis.Normalized();
+			CHECK_APPROX_EQUAL(contact1, inExpectedContactOn1, 1.0e-4f);
+			CHECK_APPROX_EQUAL(contact2, inExpectedContactOn2, 1.0e-4f);
+			CHECK_APPROX_EQUAL(pen_axis, expected_pen_axis, 1.0e-4f);
+			CHECK_APPROX_EQUAL(hit.mPenetrationDepth, inExpectedPenetrationDepth, 1.0e-4f);
+		}
+
+		// A mesh shape with a single triangle has all edges active, so only test if all edges are active
+		if (inActiveEdges == 0b111)
+		{
+			// Create a mesh with a single triangle
+			TriangleList triangles;
+			triangles.push_back(Triangle(v1, v2, v3));
+			PhysicsTestContext context;
+			Body &body = context.CreateBody(new MeshShapeSettings(triangles), RVec3(translation), rotation, EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, EActivation::DontActivate);
+
+			// Collide sphere
+			AllHitCollisionCollector<CollideShapeCollector> collector;
+			context.GetSystem()->GetNarrowPhaseQuery().CollideShape(sphere, Vec3::sOne(), RMat44(sphere_transform), settings, RVec3::sZero(), collector);
+
+			// Test result
+			CHECK(collector.mHits.size() == 1);
+			const CollideShapeResult &hit = collector.mHits[0];
+			CHECK(hit.mBodyID2 == body.GetID());
+			CHECK(hit.mSubShapeID1.GetValue() == SubShapeID().GetValue());
+			CHECK(hit.mSubShapeID2.GetValue() != SubShapeID().GetValue()); // We don't really know what SubShapeID a triangle in the mesh will get, but it should not be invalid
+			Vec3 contact1 = inv_transform * hit.mContactPointOn1;
+			Vec3 contact2 = inv_transform * hit.mContactPointOn2;
+			Vec3 pen_axis = transform.Multiply3x3Transposed(hit.mPenetrationAxis).Normalized();
+			Vec3 expected_pen_axis = inExpectedPenetrationAxis.Normalized();
+			CHECK_APPROX_EQUAL(contact1, inExpectedContactOn1, 1.0e-4f);
+			CHECK_APPROX_EQUAL(contact2, inExpectedContactOn2, 1.0e-4f);
+			CHECK_APPROX_EQUAL(pen_axis, expected_pen_axis, 1.0e-4f);
+			CHECK_APPROX_EQUAL(hit.mPenetrationDepth, inExpectedPenetrationDepth, 1.0e-4f);
+		}
+	}
+
+	// Compares CollideShapeResult for two spheres with given positions and radii
+	template <class Collider>
+	static void sTestConvexVsTriangles()
+	{
+		const float cRadius = 0.5f;
+		const float cRadiusRS2 = cRadius / sqrt(2.0f);
+		const float cDistanceToTriangle = 0.1f;
+		const float cDistanceToTriangleRS2 = cDistanceToTriangle / sqrt(2.0f);
+		const float cEpsilon = 1.0e-6f; // A small epsilon to ensure we hit the front side
+		const float cMaxSeparationDistance = 0.5f;
+		const float cSeparationDistance = 0.1f;
+
+		// Loop over all possible active edge combinations
+		for (uint8 active_edges = 0; active_edges <= 0b111; ++active_edges)
+		{
+			// Create settings
+			CollideShapeSettings settings;
+			settings.mBackFaceMode = EBackFaceMode::CollideWithBackFaces;
+
+			// Settings with ignore back faces
+			CollideShapeSettings settings_no_bf;
+			settings_no_bf.mBackFaceMode = EBackFaceMode::IgnoreBackFaces;
+
+			// Settings with max separation distance
+			CollideShapeSettings settings_max_distance;
+			settings_max_distance.mBackFaceMode = EBackFaceMode::CollideWithBackFaces;
+			settings_max_distance.mMaxSeparationDistance = cMaxSeparationDistance;
+
+			{
+				// There should be no hit in front of the triangle
+				Vec3 sphere_center(0.25f * cEdgeLength, cRadius + cSeparationDistance, 0.25f * cEdgeLength);
+				sCheckCollisionNoHit<Collider>(settings, sphere_center, cRadius, active_edges);
+
+				// But if there's a max separation distance there should be
+				Vec3 expected1 = sphere_center + Vec3(0, -cRadius, 0);
+				Vec3 expected2(0.25f * cEdgeLength, 0, 0.25f * cEdgeLength);
+				Vec3 pen_axis(0, -1, 0);
+				float pen_depth = -cSeparationDistance;
+				sCheckCollision<Collider>(settings_max_distance, sphere_center, cRadius, active_edges, expected1, expected2, pen_axis, pen_depth);
+			}
+
+			{
+				// But if we go beyond the separation distance we should again have no hit
+				Vec3 sphere_center(0.25f * cEdgeLength, cRadius + cMaxSeparationDistance + cSeparationDistance, 0.25f * cEdgeLength);
+				sCheckCollisionNoHit<Collider>(settings_max_distance, sphere_center, cRadius, active_edges);
+			}
+
+			{
+				// There should be no hit in behind the triangle
+				Vec3 sphere_center(0.25f * cEdgeLength, -cRadius - cSeparationDistance, 0.25f * cEdgeLength);
+				sCheckCollisionNoHit<Collider>(settings, sphere_center, cRadius, active_edges);
+
+				// But if there's a max separation distance there should be
+				Vec3 expected1 = sphere_center + Vec3(0, cRadius, 0);
+				Vec3 expected2(0.25f * cEdgeLength, 0, 0.25f * cEdgeLength);
+				Vec3 pen_axis(0, 1, 0);
+				float pen_depth = -cSeparationDistance;
+				sCheckCollision<Collider>(settings_max_distance, sphere_center, cRadius, active_edges, expected1, expected2, pen_axis, pen_depth);
+			}
+
+			{
+				// But if we go beyond the separation distance we should again have no hit
+				Vec3 sphere_center(0.25f * cEdgeLength, -cRadius - cMaxSeparationDistance - cSeparationDistance, 0.25f * cEdgeLength);
+				sCheckCollisionNoHit<Collider>(settings_max_distance, sphere_center, cRadius, active_edges);
+			}
+
+			{
+				// Hit interior from front side
+				Vec3 expected2(0.25f * cEdgeLength, 0, 0.25f * cEdgeLength);
+				Vec3 sphere_center = expected2 + Vec3(0, cDistanceToTriangle, 0);
+				Vec3 expected1 = sphere_center + Vec3(0, -cRadius, 0);
+				Vec3 pen_axis(0, -1, 0);
+				float pen_depth = cRadius - cDistanceToTriangle;
+				sCheckCollision<Collider>(settings, sphere_center, cRadius, active_edges, expected1, expected2, pen_axis, pen_depth);
+
+				// Ignore back faces should not matter
+				sCheckCollision<Collider>(settings_no_bf, sphere_center, cRadius, active_edges, expected1, expected2, pen_axis, pen_depth);
+			}
+
+			{
+				// Hit interior from back side
+				Vec3 expected2(0.25f * cEdgeLength, 0, 0.25f * cEdgeLength);
+				Vec3 sphere_center = expected2 + Vec3(0, -cDistanceToTriangle, 0);
+				Vec3 expected1 = sphere_center + Vec3(0, cRadius, 0);
+				Vec3 pen_axis(0, 1, 0);
+				float pen_depth = cRadius - cDistanceToTriangle;
+				sCheckCollision<Collider>(settings, sphere_center, cRadius, active_edges, expected1, expected2, pen_axis, pen_depth);
+
+				// Back face hit should be filtered
+				sCheckCollisionNoHit<Collider>(settings_no_bf, sphere_center, cRadius, active_edges);
+			}
+
+			// Loop over possible active edge movement direction permutations
+			for (int movement_direction = 0; movement_direction < 3; ++movement_direction)
+			{
+				switch (movement_direction)
+				{
+				case 0:
+					// Disable the system
+					settings.mActiveEdgeMovementDirection = Vec3::sZero();
+					break;
+
+				case 1:
+					// Move into the triangle, this should always give us the normal from the edge
+					settings.mActiveEdgeMovementDirection = Vec3(0, -1, 0);
+					break;
+
+				case 2:
+					// Move out of the triangle, we should always get the normal of the triangle
+					settings.mActiveEdgeMovementDirection = Vec3(0, 1, 0);
+					break;
+				}
+
+				{
+					// Hit edge 1
+					Vec3 expected2(0, 0, 0.5f * cEdgeLength);
+					Vec3 sphere_center = expected2 + Vec3(-cDistanceToTriangle, cEpsilon, 0);
+					Vec3 expected1 = sphere_center + Vec3(cRadius, 0, 0);
+					Vec3 pen_axis = (active_edges & 0b001) != 0 || movement_direction == 1? Vec3(1, 0, 0) : Vec3(0, -1, 0);
+					float pen_depth = cRadius - cDistanceToTriangle;
+					sCheckCollision<Collider>(settings, sphere_center, cRadius, active_edges, expected1, expected2, pen_axis, pen_depth);
+				}
+
+				{
+					// Hit edge 2
+					Vec3 expected2(0.5f * cEdgeLength, 0, 0.5f * cEdgeLength);
+					Vec3 sphere_center = expected2 + Vec3(cDistanceToTriangleRS2, cEpsilon, cDistanceToTriangleRS2);
+					Vec3 expected1 = sphere_center - Vec3(cRadiusRS2, 0, cRadiusRS2);
+					Vec3 pen_axis = (active_edges & 0b010) != 0 || movement_direction == 1? Vec3(-1, 0, -1) : Vec3(0, -1, 0);
+					float pen_depth = cRadius - cDistanceToTriangle;
+					sCheckCollision<Collider>(settings, sphere_center, cRadius, active_edges, expected1, expected2, pen_axis, pen_depth);
+				}
+
+				{
+					// Hit edge 3
+					Vec3 expected2(0.5f * cEdgeLength, 0, 0);
+					Vec3 sphere_center = expected2 + Vec3(0, cEpsilon, -cDistanceToTriangle);
+					Vec3 expected1 = sphere_center + Vec3(0, 0, cRadius);
+					Vec3 pen_axis = (active_edges & 0b100) != 0 || movement_direction == 1? Vec3(0, 0, 1) : Vec3(0, -1, 0);
+					float pen_depth = cRadius - cDistanceToTriangle;
+					sCheckCollision<Collider>(settings, sphere_center, cRadius, active_edges, expected1, expected2, pen_axis, pen_depth);
+				}
+
+				{
+					// Hit vertex 1
+					Vec3 expected2(0, 0, 0);
+					Vec3 sphere_center = expected2 + Vec3(-cDistanceToTriangleRS2, cEpsilon, -cDistanceToTriangleRS2);
+					Vec3 expected1 = sphere_center + Vec3(cRadiusRS2, 0, cRadiusRS2);
+					Vec3 pen_axis = (active_edges & 0b101) != 0 || movement_direction == 1? Vec3(1, 0, 1) : Vec3(0, -1, 0);
+					float pen_depth = cRadius - cDistanceToTriangle;
+					sCheckCollision<Collider>(settings, sphere_center, cRadius, active_edges, expected1, expected2, pen_axis, pen_depth);
+				}
+
+				{
+					// Hit vertex 2
+					Vec3 expected2(0, 0, cEdgeLength);
+					Vec3 sphere_center = expected2 + Vec3(-cDistanceToTriangleRS2, cEpsilon, cDistanceToTriangleRS2);
+					Vec3 expected1 = sphere_center + Vec3(cRadiusRS2, 0, -cRadiusRS2);
+					Vec3 pen_axis = (active_edges & 0b011) != 0 || movement_direction == 1? Vec3(1, 0, -1) : Vec3(0, -1, 0);
+					float pen_depth = cRadius - cDistanceToTriangle;
+					sCheckCollision<Collider>(settings, sphere_center, cRadius, active_edges, expected1, expected2, pen_axis, pen_depth);
+				}
+
+				{
+					// Hit vertex 3
+					Vec3 expected2(cEdgeLength, 0, 0);
+					Vec3 sphere_center = expected2 + Vec3(cDistanceToTriangleRS2, cEpsilon, -cDistanceToTriangleRS2);
+					Vec3 expected1 = sphere_center + Vec3(-cRadiusRS2, 0, cRadiusRS2);
+					Vec3 pen_axis = (active_edges & 0b110) != 0 || movement_direction == 1? Vec3(-1, 0, 1) : Vec3(0, -1, 0);
+					float pen_depth = cRadius - cDistanceToTriangle;
+					sCheckCollision<Collider>(settings, sphere_center, cRadius, active_edges, expected1, expected2, pen_axis, pen_depth);
+				}
+			}
+		}
+	}
+
+	TEST_CASE("TestConvexVsTriangles")
+	{
+		sTestConvexVsTriangles<CollideConvexVsTriangles>();
+	}
+
+	TEST_CASE("TestSphereVsTriangles")
+	{
+		sTestConvexVsTriangles<CollideSphereVsTriangles>();
+	}
+}

lib/All/JoltPhysics/UnitTests/Physics/DistanceConstraintTests.cpp

@@ -0,0 +1,74 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include "PhysicsTestContext.h"
+#include <Jolt/Physics/Constraints/DistanceConstraint.h>
+#include "Layers.h"
+
+TEST_SUITE("DistanceConstraintTests")
+{
+	// Test if the distance constraint can be used to create a spring
+	TEST_CASE("TestDistanceSpring")
+	{
+		// Configuration of the spring
+		const RVec3 cInitialPosition(10, 0, 0);
+		const float cFrequency = 2.0f;
+		const float cDamping = 0.1f;
+
+		for (int mode = 0; mode < 2; ++mode)
+		{
+			// Create a sphere
+			PhysicsTestContext context;
+			context.ZeroGravity();
+			Body &body = context.CreateSphere(cInitialPosition, 0.5f, EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING);
+			body.GetMotionProperties()->SetLinearDamping(0.0f);
+
+			// Calculate stiffness and damping of spring
+			float m = 1.0f / body.GetMotionProperties()->GetInverseMass();
+			float omega = 2.0f * JPH_PI * cFrequency;
+			float k = m * Square(omega);
+			float c = 2.0f * m * cDamping * omega;
+
+			// Create spring
+			DistanceConstraintSettings constraint;
+			constraint.mPoint2 = cInitialPosition;
+			if (mode == 0)
+			{
+				// First iteration use stiffness and damping
+				constraint.mLimitsSpringSettings.mMode = ESpringMode::StiffnessAndDamping;
+				constraint.mLimitsSpringSettings.mStiffness = k;
+				constraint.mLimitsSpringSettings.mDamping = c;
+			}
+			else
+			{
+				// Second iteration use frequency and damping
+				constraint.mLimitsSpringSettings.mMode = ESpringMode::FrequencyAndDamping;
+				constraint.mLimitsSpringSettings.mFrequency = cFrequency;
+				constraint.mLimitsSpringSettings.mDamping = cDamping;
+			}
+			constraint.mMinDistance = constraint.mMaxDistance = 0.0f;
+			context.CreateConstraint<DistanceConstraint>(Body::sFixedToWorld, body, constraint);
+
+			// Simulate spring
+			Real x = cInitialPosition.GetX();
+			float v = 0.0f;
+			float dt = context.GetDeltaTime();
+			for (int i = 0; i < 120; ++i)
+			{
+				// Using the equations from page 32 of Soft Constraints: Reinventing The Spring - Erin Catto - GDC 2011 for an implicit euler spring damper
+				v = (v - dt * k / m * float(x)) / (1.0f + dt * c / m + Square(dt) * k / m);
+				x += v * dt;
+
+				// Run physics simulation
+				context.SimulateSingleStep();
+
+				// Test if simulation matches prediction
+				CHECK_APPROX_EQUAL(x, body.GetPosition().GetX(), 5.0e-6_r);
+				CHECK(body.GetPosition().GetY() == 0);
+				CHECK(body.GetPosition().GetZ() == 0);
+			}
+		}
+	}
+}

lib/All/JoltPhysics/UnitTests/Physics/EstimateCollisionResponseTest.cpp

@@ -0,0 +1,77 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include <Jolt/Physics/Collision/EstimateCollisionResponse.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include "PhysicsTestContext.h"
+#include "Layers.h"
+
+TEST_SUITE("EstimateCollisionResponseTests")
+{
+	// Test CastShape ordering according to penetration depth
+	TEST_CASE("TestEstimateCollisionResponse")
+	{
+		PhysicsTestContext c;
+		c.ZeroGravity();
+
+		const Vec3 cBox1HalfExtents(0.1f, 1, 2);
+		const Vec3 cBox2HalfExtents(0.2f, 3, 4);
+
+		// Test different motion types, restitution, positions and angular velocities
+		for (EMotionType mt : { EMotionType::Static, EMotionType::Kinematic, EMotionType::Dynamic })
+			for (float restitution : { 0.0f, 0.3f, 1.0f })
+				for (float friction : { 0.0f, 0.3f, 1.0f })
+					for (float y : { 0.0f, 0.5f, cBox2HalfExtents.GetY() })
+						for (float z : { 0.0f, 0.5f, cBox2HalfExtents.GetZ() })
+							for (float w : { 0.0f, -1.0f, 1.0f })
+							{
+								// Install a listener that predicts the collision response
+								class MyListener : public ContactListener
+								{
+								public:
+									virtual void	OnContactAdded(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings) override
+									{
+										EstimateCollisionResponse(inBody1, inBody2, inManifold, mResult, ioSettings.mCombinedFriction, ioSettings.mCombinedRestitution);
+									}
+
+									CollisionEstimationResult mResult;
+								};
+
+								MyListener listener;
+								c.GetSystem()->SetContactListener(&listener);
+
+								const RVec3 cBaseOffset(1, 2, 3);
+								const Real cEpsilon = 0.0001_r;
+
+								Body &box1 = c.CreateBox(cBaseOffset, Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, cBox1HalfExtents);
+								box1.SetFriction(friction);
+								box1.SetRestitution(restitution);
+								box1.SetLinearVelocity(Vec3(1, 1, 0));
+								box1.SetAngularVelocity(Vec3(0, w, 0));
+
+								Body &box2 = c.CreateBox(cBaseOffset + RVec3(cBox1HalfExtents.GetX() + cBox2HalfExtents.GetX() - cEpsilon, y, z), Quat::sIdentity(), mt, EMotionQuality::Discrete, mt == EMotionType::Static? Layers::NON_MOVING : Layers::MOVING, cBox2HalfExtents);
+								box2.SetFriction(friction);
+								box2.SetRestitution(restitution);
+								if (mt != EMotionType::Static)
+									box2.SetLinearVelocity(Vec3(-1, 0, 0));
+
+								// Step the simulation
+								c.SimulateSingleStep();
+
+								// Check that the predicted velocities are correct
+								CHECK_APPROX_EQUAL(listener.mResult.mLinearVelocity1, box1.GetLinearVelocity());
+								CHECK_APPROX_EQUAL(listener.mResult.mAngularVelocity1, box1.GetAngularVelocity());
+								CHECK_APPROX_EQUAL(listener.mResult.mLinearVelocity2, box2.GetLinearVelocity());
+								CHECK_APPROX_EQUAL(listener.mResult.mAngularVelocity2, box2.GetAngularVelocity());
+
+								// Remove the bodies in reverse order
+								BodyInterface &bi = c.GetBodyInterface();
+								bi.RemoveBody(box2.GetID());
+								bi.RemoveBody(box1.GetID());
+								bi.DestroyBody(box2.GetID());
+								bi.DestroyBody(box1.GetID());
+							}
+	}
+}

lib/All/JoltPhysics/UnitTests/Physics/HeightFieldShapeTests.cpp

@@ -0,0 +1,464 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include "PhysicsTestContext.h"
+#include <Jolt/Physics/Collision/RayCast.h>
+#include <Jolt/Physics/Collision/CastResult.h>
+#include <Jolt/Physics/Collision/Shape/HeightFieldShape.h>
+#include <Jolt/Physics/Collision/PhysicsMaterialSimple.h>
+
+TEST_SUITE("HeightFieldShapeTests")
+{
+	static void sRandomizeMaterials(HeightFieldShapeSettings &ioSettings, uint inMaxMaterials)
+	{
+		// Create materials
+		for (uint i = 0; i < inMaxMaterials; ++i)
+			ioSettings.mMaterials.push_back(new PhysicsMaterialSimple("Material " + ConvertToString(i), Color::sGetDistinctColor(i)));
+
+		if (inMaxMaterials > 1)
+		{
+			// Make random material indices
+			UnitTestRandom random;
+			uniform_int_distribution<uint> index_distribution(0, inMaxMaterials - 1);
+			ioSettings.mMaterialIndices.resize(Square(ioSettings.mSampleCount - 1));
+			for (uint y = 0; y < ioSettings.mSampleCount - 1; ++y)
+				for (uint x = 0; x < ioSettings.mSampleCount - 1; ++x)
+					ioSettings.mMaterialIndices[y * (ioSettings.mSampleCount - 1) + x] = uint8(index_distribution(random));
+		}
+	}
+
+	static Ref<HeightFieldShape> sValidateGetPosition(const HeightFieldShapeSettings &inSettings, float inMaxError)
+	{
+		// Create shape
+		Ref<HeightFieldShape> shape = StaticCast<HeightFieldShape>(inSettings.Create().Get());
+
+		// Validate it
+		float max_diff = -1.0f;
+		for (uint y = 0; y < inSettings.mSampleCount; ++y)
+			for (uint x = 0; x < inSettings.mSampleCount; ++x)
+			{
+				// Perform a raycast from above the height field on this location
+				RayCast ray { inSettings.mOffset + inSettings.mScale * Vec3((float)x, 100.0f, (float)y), inSettings.mScale.GetY() * Vec3(0, -200, 0) };
+				RayCastResult hit;
+				shape->CastRay(ray, SubShapeIDCreator(), hit);
+
+				// Get original (unscaled) height
+				float height = inSettings.mHeightSamples[y * inSettings.mSampleCount + x];
+				if (height != HeightFieldShapeConstants::cNoCollisionValue)
+				{
+					// Check there is collision
+					CHECK(!shape->IsNoCollision(x, y));
+
+					// Calculate position
+					Vec3 original_pos = inSettings.mOffset + inSettings.mScale * Vec3((float)x, height, (float)y);
+
+					// Calculate position from the shape
+					Vec3 shape_pos = shape->GetPosition(x, y);
+
+					// Calculate delta
+					float diff = (original_pos - shape_pos).Length();
+					max_diff = max(max_diff, diff);
+
+					// Materials are defined on the triangle, not on the sample points
+					if (x < inSettings.mSampleCount - 1 && y < inSettings.mSampleCount - 1)
+					{
+						const PhysicsMaterial *m1 = PhysicsMaterial::sDefault;
+						if (!inSettings.mMaterialIndices.empty())
+							m1 = inSettings.mMaterials[inSettings.mMaterialIndices[y * (inSettings.mSampleCount - 1) + x]];
+						else if (!inSettings.mMaterials.empty())
+							m1 = inSettings.mMaterials.front();
+
+						const PhysicsMaterial *m2 = shape->GetMaterial(x, y);
+						CHECK(m1 == m2);
+					}
+
+					// Don't test borders, the ray may or may not hit
+					if (x > 0 && y > 0 && x < inSettings.mSampleCount - 1 && y < inSettings.mSampleCount - 1)
+					{
+						// Check that the ray hit the height field
+						Vec3 hit_pos = ray.GetPointOnRay(hit.mFraction);
+						CHECK_APPROX_EQUAL(hit_pos, shape_pos, 1.0e-3f);
+					}
+				}
+				else
+				{
+					// Should be no collision here
+					CHECK(shape->IsNoCollision(x, y));
+
+					// Ray should not have given a hit
+					CHECK(hit.mFraction > 1.0f);
+				}
+			}
+
+		// Check error
+		CHECK(max_diff <= inMaxError);
+
+		return shape;
+	}
+
+	TEST_CASE("TestPlane")
+	{
+		// Create flat plane with offset and scale
+		HeightFieldShapeSettings settings;
+		settings.mOffset = Vec3(3, 5, 7);
+		settings.mScale = Vec3(9, 13, 17);
+		settings.mSampleCount = 32;
+		settings.mBitsPerSample = 1;
+		settings.mBlockSize = 4;
+		settings.mHeightSamples.resize(Square(settings.mSampleCount));
+		for (float &h : settings.mHeightSamples)
+			h = 1.0f;
+
+		// Make some random holes
+		UnitTestRandom random;
+		uniform_int_distribution<uint> index_distribution(0, (uint)settings.mHeightSamples.size() - 1);
+		for (int i = 0; i < 10; ++i)
+			settings.mHeightSamples[index_distribution(random)] = HeightFieldShapeConstants::cNoCollisionValue;
+
+		// We should be able to encode a flat plane in 1 bit
+		CHECK(settings.CalculateBitsPerSampleForError(0.0f) == 1);
+
+		sRandomizeMaterials(settings, 256);
+		sValidateGetPosition(settings, 0.0f);
+	}
+
+	TEST_CASE("TestPlaneCloseToOrigin")
+	{
+		// Create flat plane very close to origin, this tests that we don't introduce a quantization error on a flat plane
+		HeightFieldShapeSettings settings;
+		settings.mSampleCount = 32;
+		settings.mBitsPerSample = 1;
+		settings.mBlockSize = 4;
+		settings.mHeightSamples.resize(Square(settings.mSampleCount));
+		for (float &h : settings.mHeightSamples)
+			h = 1.0e-6f;
+
+		// We should be able to encode a flat plane in 1 bit
+		CHECK(settings.CalculateBitsPerSampleForError(0.0f) == 1);
+
+		sRandomizeMaterials(settings, 50);
+		sValidateGetPosition(settings, 0.0f);
+	}
+
+	TEST_CASE("TestRandomHeightField")
+	{
+		const float cMinHeight = -5.0f;
+		const float cMaxHeight = 10.0f;
+
+		UnitTestRandom random;
+		uniform_real_distribution<float> height_distribution(cMinHeight, cMaxHeight);
+
+		// Create height field with random samples
+		HeightFieldShapeSettings settings;
+		settings.mOffset = Vec3(0.3f, 0.5f, 0.7f);
+		settings.mScale = Vec3(1.1f, 1.2f, 1.3f);
+		settings.mSampleCount = 32;
+		settings.mBitsPerSample = 8;
+		settings.mBlockSize = 4;
+		settings.mHeightSamples.resize(Square(settings.mSampleCount));
+		for (float &h : settings.mHeightSamples)
+			h = height_distribution(random);
+
+		// Check if bits per sample is ok
+		for (uint32 bits_per_sample = 1; bits_per_sample <= 8; ++bits_per_sample)
+		{
+			// Calculate maximum error you can get if you quantize using bits_per_sample.
+			// We ignore the fact that we have range blocks that give much better compression, although
+			// with random input data there shouldn't be much benefit of that.
+			float max_error = 0.5f * (cMaxHeight - cMinHeight) / ((1 << bits_per_sample) - 1);
+			uint32 calculated_bits_per_sample = settings.CalculateBitsPerSampleForError(max_error);
+			CHECK(calculated_bits_per_sample <= bits_per_sample);
+		}
+
+		sRandomizeMaterials(settings, 1);
+		sValidateGetPosition(settings, settings.mScale.GetY() * (cMaxHeight - cMinHeight) / ((1 << settings.mBitsPerSample) - 1));
+	}
+
+	TEST_CASE("TestEmptyHeightField")
+	{
+		// Create height field with no collision
+		HeightFieldShapeSettings settings;
+		settings.mSampleCount = 32;
+		settings.mHeightSamples.resize(Square(settings.mSampleCount));
+		for (float &h : settings.mHeightSamples)
+			h = HeightFieldShapeConstants::cNoCollisionValue;
+
+		// This should use the minimum amount of bits
+		CHECK(settings.CalculateBitsPerSampleForError(0.0f) == 1);
+
+		sRandomizeMaterials(settings, 50);
+		Ref<HeightFieldShape> shape = sValidateGetPosition(settings, 0.0f);
+
+		// Check that we allocated the minimum amount of memory
+		Shape::Stats stats = shape->GetStats();
+		CHECK(stats.mNumTriangles == 0);
+		CHECK(stats.mSizeBytes == sizeof(HeightFieldShape));
+	}
+
+	TEST_CASE("TestGetHeights")
+	{
+		const float cMinHeight = -5.0f;
+		const float cMaxHeight = 10.0f;
+		const uint cSampleCount = 32;
+		const uint cNoCollisionIndex = 10;
+
+		UnitTestRandom random;
+		uniform_real_distribution<float> height_distribution(cMinHeight, cMaxHeight);
+
+		// Create height field with random samples
+		HeightFieldShapeSettings settings;
+		settings.mOffset = Vec3(0.3f, 0.5f, 0.7f);
+		settings.mScale = Vec3(1.1f, 1.2f, 1.3f);
+		settings.mSampleCount = cSampleCount;
+		settings.mBitsPerSample = 8;
+		settings.mBlockSize = 4;
+		settings.mHeightSamples.resize(Square(cSampleCount));
+		for (float &h : settings.mHeightSamples)
+			h = height_distribution(random);
+
+		// Add 1 sample that has no collision
+		settings.mHeightSamples[cNoCollisionIndex] = HeightFieldShapeConstants::cNoCollisionValue;
+
+		// Create shape
+		ShapeRefC shape = settings.Create().Get();
+		const HeightFieldShape *height_field = StaticCast<HeightFieldShape>(shape);
+
+		{
+			// Check that the GetHeights function returns the same values as the original height samples
+			Array<float> sampled_heights;
+			sampled_heights.resize(Square(cSampleCount));
+			height_field->GetHeights(0, 0, cSampleCount, cSampleCount, sampled_heights.data(), cSampleCount);
+			for (uint i = 0; i < Square(cSampleCount); ++i)
+				if (i == cNoCollisionIndex)
+					CHECK(sampled_heights[i] == HeightFieldShapeConstants::cNoCollisionValue);
+				else
+					CHECK_APPROX_EQUAL(sampled_heights[i], settings.mOffset.GetY() + settings.mScale.GetY() * settings.mHeightSamples[i], 0.05f);
+		}
+
+		{
+			// With a random height field the max error is going to be limited by the amount of bits we have per sample as we will not get any benefit from a reduced range per block
+			float tolerance = (cMaxHeight - cMinHeight) / ((1 << settings.mBitsPerSample) - 2);
+
+			// Check a sub rect of the height field
+			uint sx = 4, sy = 8, cx = 16, cy = 8;
+			Array<float> sampled_heights;
+			sampled_heights.resize(cx * cy);
+			height_field->GetHeights(sx, sy, cx, cy, sampled_heights.data(), cx);
+			for (uint y = 0; y < cy; ++y)
+				for (uint x = 0; x < cx; ++x)
+					CHECK_APPROX_EQUAL(sampled_heights[y * cx + x], settings.mOffset.GetY() + settings.mScale.GetY() * settings.mHeightSamples[(sy + y) * cSampleCount + sx + x], tolerance);
+		}
+	}
+
+	TEST_CASE("TestSetHeights")
+	{
+		const float cMinHeight = -5.0f;
+		const float cMaxHeight = 10.0f;
+		const uint cSampleCount = 32;
+
+		UnitTestRandom random;
+		uniform_real_distribution<float> height_distribution(cMinHeight, cMaxHeight);
+
+		// Create height field with random samples
+		HeightFieldShapeSettings settings;
+		settings.mOffset = Vec3(0.3f, 0.5f, 0.7f);
+		settings.mScale = Vec3(1.1f, 1.2f, 1.3f);
+		settings.mSampleCount = cSampleCount;
+		settings.mBitsPerSample = 8;
+		settings.mBlockSize = 4;
+		settings.mHeightSamples.resize(Square(cSampleCount));
+		settings.mMinHeightValue = cMinHeight;
+		settings.mMaxHeightValue = cMaxHeight;
+		for (float &h : settings.mHeightSamples)
+			h = height_distribution(random);
+
+		// Create shape
+		Ref<Shape> shape = settings.Create().Get();
+		HeightFieldShape *height_field = StaticCast<HeightFieldShape>(shape);
+
+		// Get the original (quantized) heights
+		Array<float> original_heights;
+		original_heights.resize(Square(cSampleCount));
+		height_field->GetHeights(0, 0, cSampleCount, cSampleCount, original_heights.data(), cSampleCount);
+
+		// Create new data for height field
+		Array<float> patched_heights;
+		uint sx = 4, sy = 16, cx = 16, cy = 8;
+		patched_heights.resize(cx * cy);
+		for (uint y = 0; y < cy; ++y)
+			for (uint x = 0; x < cx; ++x)
+				patched_heights[y * cx + x] = height_distribution(random);
+
+		// Add 1 sample that has no collision
+		uint no_collision_idx = (sy + 1) * cSampleCount + sx + 2;
+		patched_heights[1 * cx + 2] = HeightFieldShapeConstants::cNoCollisionValue;
+
+		// Update the height field
+		TempAllocatorMalloc temp_allocator;
+		height_field->SetHeights(sx, sy, cx, cy, patched_heights.data(), cx, temp_allocator);
+
+		// With a random height field the max error is going to be limited by the amount of bits we have per sample as we will not get any benefit from a reduced range per block
+		float tolerance = (cMaxHeight - cMinHeight) / ((1 << settings.mBitsPerSample) - 2);
+
+		// Check a sub rect of the height field
+		Array<float> verify_heights;
+		verify_heights.resize(cSampleCount * cSampleCount);
+		height_field->GetHeights(0, 0, cSampleCount, cSampleCount, verify_heights.data(), cSampleCount);
+		for (uint y = 0; y < cSampleCount; ++y)
+			for (uint x = 0; x < cSampleCount; ++x)
+			{
+				uint idx = y * cSampleCount + x;
+				if (idx == no_collision_idx)
+					CHECK(verify_heights[idx] == HeightFieldShapeConstants::cNoCollisionValue);
+				else if (x >= sx && x < sx + cx && y >= sy && y < sy + cy)
+					CHECK_APPROX_EQUAL(verify_heights[y * cSampleCount + x], patched_heights[(y - sy) * cx + x - sx], tolerance);
+				else if (x >= sx - settings.mBlockSize && x < sx + cx && y >= sy - settings.mBlockSize && y < sy + cy)
+					CHECK_APPROX_EQUAL(verify_heights[idx], original_heights[idx], tolerance); // We didn't modify this but it has been quantized again
+				else
+					CHECK(verify_heights[idx] == original_heights[idx]); // We didn't modify this and it is outside of the affected range
+			}
+	}
+
+	TEST_CASE("TestSetMaterials")
+	{
+		constexpr uint cSampleCount = 32;
+
+		PhysicsMaterialRefC material_0 = new PhysicsMaterialSimple("Material 0", Color::sGetDistinctColor(0));
+		PhysicsMaterialRefC material_1 = new PhysicsMaterialSimple("Material 1", Color::sGetDistinctColor(1));
+		PhysicsMaterialRefC material_2 = new PhysicsMaterialSimple("Material 2", Color::sGetDistinctColor(2));
+		PhysicsMaterialRefC material_3 = new PhysicsMaterialSimple("Material 3", Color::sGetDistinctColor(3));
+		PhysicsMaterialRefC material_4 = new PhysicsMaterialSimple("Material 4", Color::sGetDistinctColor(4));
+		PhysicsMaterialRefC material_5 = new PhysicsMaterialSimple("Material 5", Color::sGetDistinctColor(5));
+
+		// Create height field with a single material
+		HeightFieldShapeSettings settings;
+		settings.mSampleCount = cSampleCount;
+		settings.mBitsPerSample = 8;
+		settings.mBlockSize = 4;
+		settings.mHeightSamples.resize(Square(cSampleCount));
+		for (float &h : settings.mHeightSamples)
+			h = 0.0f;
+		settings.mMaterials.push_back(material_0);
+		settings.mMaterialIndices.resize(Square(cSampleCount - 1));
+		for (uint8 &m : settings.mMaterialIndices)
+			m = 0;
+
+		// Store the current state
+		Array<const PhysicsMaterial *> current_state;
+		current_state.resize(Square(cSampleCount - 1));
+		for (const PhysicsMaterial *&m : current_state)
+			m = material_0;
+
+		// Create shape
+		Ref<Shape> shape = settings.Create().Get();
+		HeightFieldShape *height_field = StaticCast<HeightFieldShape>(shape);
+
+		// Check that the material is set
+		auto check_materials = [height_field, &current_state]() {
+			const PhysicsMaterialList &material_list = height_field->GetMaterialList();
+
+			uint sample_count_min_1 = height_field->GetSampleCount() - 1;
+
+			Array<uint8> material_indices;
+			material_indices.resize(Square(sample_count_min_1));
+			height_field->GetMaterials(0, 0, sample_count_min_1, sample_count_min_1, material_indices.data(), sample_count_min_1);
+
+			for (uint i = 0; i < (uint)current_state.size(); ++i)
+				CHECK(current_state[i] == material_list[material_indices[i]]);
+		};
+		check_materials();
+
+		// Function to randomize materials
+		auto update_materials = [height_field, &current_state](uint inStartX, uint inStartY, uint inSizeX, uint inSizeY, const PhysicsMaterialList *inMaterialList) {
+			TempAllocatorMalloc temp_allocator;
+
+			const PhysicsMaterialList &material_list = inMaterialList != nullptr? *inMaterialList : height_field->GetMaterialList();
+
+			UnitTestRandom random;
+			uniform_int_distribution<uint> index_distribution(0, uint(material_list.size()) - 1);
+
+			uint sample_count_min_1 = height_field->GetSampleCount() - 1;
+
+			Array<uint8> patched_materials;
+			patched_materials.resize(inSizeX * inSizeY);
+			for (uint y = 0; y < inSizeY; ++y)
+				for (uint x = 0; x < inSizeX; ++x)
+				{
+					// Initialize the patch
+					uint8 index = uint8(index_distribution(random));
+					patched_materials[y * inSizeX + x] = index;
+
+					// Update reference state
+					current_state[(inStartY + y) * sample_count_min_1 + inStartX + x] = material_list[index];
+				}
+			CHECK(height_field->SetMaterials(inStartX, inStartY, inSizeX, inSizeY, patched_materials.data(), inSizeX, inMaterialList, temp_allocator));
+		};
+
+		{
+			// Add material 1
+			PhysicsMaterialList patched_materials_list;
+			patched_materials_list.push_back(material_1);
+			patched_materials_list.push_back(material_0);
+			update_materials(4, 16, 16, 8, &patched_materials_list);
+			check_materials();
+		}
+
+		{
+			// Add material 2
+			PhysicsMaterialList patched_materials_list;
+			patched_materials_list.push_back(material_0);
+			patched_materials_list.push_back(material_2);
+			update_materials(8, 16, 16, 8, &patched_materials_list);
+			check_materials();
+		}
+
+		{
+			// Add material 3
+			PhysicsMaterialList patched_materials_list;
+			patched_materials_list.push_back(material_0);
+			patched_materials_list.push_back(material_1);
+			patched_materials_list.push_back(material_2);
+			patched_materials_list.push_back(material_3);
+			update_materials(8, 8, 16, 8, &patched_materials_list);
+			check_materials();
+		}
+
+		{
+			// Add material 4
+			PhysicsMaterialList patched_materials_list;
+			patched_materials_list.push_back(material_0);
+			patched_materials_list.push_back(material_1);
+			patched_materials_list.push_back(material_4);
+			patched_materials_list.push_back(material_2);
+			patched_materials_list.push_back(material_3);
+			update_materials(0, 0, 30, 30, &patched_materials_list);
+			check_materials();
+		}
+
+		{
+			// Add material 5
+			PhysicsMaterialList patched_materials_list;
+			patched_materials_list.push_back(material_4);
+			patched_materials_list.push_back(material_3);
+			patched_materials_list.push_back(material_0);
+			patched_materials_list.push_back(material_1);
+			patched_materials_list.push_back(material_2);
+			patched_materials_list.push_back(material_5);
+			update_materials(1, 1, 30, 30, &patched_materials_list);
+			check_materials();
+		}
+
+		{
+			// Update materials without new material list
+			update_materials(2, 5, 10, 15, nullptr);
+			check_materials();
+		}
+
+		// Check materials using GetMaterial call
+		for (uint y = 0; y < cSampleCount - 1; ++y)
+			for (uint x = 0; x < cSampleCount - 1; ++x)
+				CHECK(height_field->GetMaterial(x, y) == current_state[y * (cSampleCount - 1) + x]);
+	}
+}

lib/All/JoltPhysics/UnitTests/Physics/HingeConstraintTests.cpp

@@ -0,0 +1,84 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include "PhysicsTestContext.h"
+#include <Jolt/Physics/Constraints/HingeConstraint.h>
+#include <Jolt/Physics/Collision/Shape/SphereShape.h>
+#include "Layers.h"
+
+TEST_SUITE("HingeConstraintTests")
+{
+	// Test if the hinge constraint can be used to create a spring
+	TEST_CASE("TestHingeSpring")
+	{
+		// Configuration of the spring
+		const float cInitialAngle = DegreesToRadians(100.0f);
+		const float cFrequency = 2.0f;
+		const float cDamping = 0.1f;
+
+		for (int mode = 0; mode < 2; ++mode)
+		{
+			// Create a sphere
+			PhysicsTestContext context;
+			Body &body = context.CreateBody(new SphereShapeSettings(0.5f), RVec3::sZero(), Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, EActivation::Activate);
+			body.GetMotionProperties()->SetAngularDamping(0.0f);
+			body.SetAllowSleeping(false);
+
+			// Calculate stiffness and damping of spring
+			float inertia = body.GetMotionProperties()->GetInverseInertiaForRotation(Mat44::sIdentity()).Inversed3x3().GetAxisY().Length();
+			float omega = 2.0f * JPH_PI * cFrequency;
+			float k = inertia * Square(omega);
+			float c = 2.0f * inertia * cDamping * omega;
+
+			// Create spring
+			HingeConstraintSettings constraint;
+			if (mode == 0)
+			{
+				// First iteration use stiffness and damping
+				constraint.mLimitsSpringSettings.mMode = ESpringMode::StiffnessAndDamping;
+				constraint.mLimitsSpringSettings.mStiffness = k;
+				constraint.mLimitsSpringSettings.mDamping = c;
+			}
+			else
+			{
+				// Second iteration use frequency and damping
+				constraint.mLimitsSpringSettings.mMode = ESpringMode::FrequencyAndDamping;
+				constraint.mLimitsSpringSettings.mFrequency = cFrequency;
+				constraint.mLimitsSpringSettings.mDamping = cDamping;
+			}
+			constraint.mLimitsMin = constraint.mLimitsMax = 0.0f;
+			context.CreateConstraint<HingeConstraint>(Body::sFixedToWorld, body, constraint);
+
+			// Rotate the body to the initial angle
+			context.GetBodyInterface().SetRotation(body.GetID(), Quat::sRotation(Vec3::sAxisY(), cInitialAngle), EActivation::Activate);
+
+			// Simulate angular spring
+			float angle = cInitialAngle;
+			float angular_v = 0.0f;
+			float dt = context.GetDeltaTime();
+			for (int i = 0; i < 120; ++i)
+			{
+				// Using the equations from page 32 of Soft Constraints: Reinventing The Spring - Erin Catto - GDC 2011 for an implicit euler spring damper
+				angular_v = (angular_v - dt * k / inertia * angle) / (1.0f + dt * c / inertia + Square(dt) * k / inertia);
+				angle += angular_v * dt;
+
+				// Run physics simulation
+				context.SimulateSingleStep();
+
+				// Decompose body rotation
+				Vec3 actual_axis;
+				float actual_angle;
+				body.GetRotation().GetAxisAngle(actual_axis, actual_angle);
+				if (actual_axis.GetY() < 0.0f)
+					actual_angle = -actual_angle;
+
+				// Test if simulation matches prediction
+				CHECK_APPROX_EQUAL(angle, actual_angle, DegreesToRadians(0.1f));
+				CHECK_APPROX_EQUAL(actual_axis.GetX(), 0);
+				CHECK_APPROX_EQUAL(actual_axis.GetZ(), 0);
+			}
+		}
+	}
+}

lib/All/JoltPhysics/UnitTests/Physics/MotionQualityLinearCastTests.cpp

@@ -0,0 +1,379 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include "PhysicsTestContext.h"
+#include "Layers.h"
+#include "LoggingContactListener.h"
+#include "LoggingBodyActivationListener.h"
+
+TEST_SUITE("MotionQualityLinearCastTests")
+{
+	static const float cBoxExtent = 0.5f;
+	static const float cFrequency = 60.0f;
+	static const Vec3 cVelocity(2.0f * cFrequency, 0, 0); // High enough velocity to step 2 meters in a single simulation step
+	static const RVec3 cPos1(-1, 0, 0);
+	static const RVec3 cPos2(1, 0, 0);
+
+	// Two boxes colliding in the center, each has enough velocity to tunnel though in 1 step
+	TEST_CASE("TestDiscreteBoxVsDiscreteBox")
+	{
+		PhysicsTestContext c(1.0f / cFrequency, 1);
+		c.ZeroGravity();
+
+		// Register listener
+		LoggingContactListener listener;
+		c.GetSystem()->SetContactListener(&listener);
+
+		Body &box1 = c.CreateBox(cPos1, Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3::sReplicate(cBoxExtent));
+		box1.SetLinearVelocity(cVelocity);
+
+		// Test that the inner radius of the box makes sense (used internally by linear cast)
+		CHECK_APPROX_EQUAL(box1.GetShape()->GetInnerRadius(), cBoxExtent);
+
+		Body &box2 = c.CreateBox(cPos2, Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3::sReplicate(cBoxExtent));
+		box2.SetLinearVelocity(-cVelocity);
+
+		c.SimulateSingleStep();
+
+		// No collisions should be reported and the bodies should have moved according to their velocity (tunneling through each other)
+		CHECK(listener.GetEntryCount() == 0);
+		CHECK_APPROX_EQUAL(box1.GetPosition(), cPos1 + cVelocity / cFrequency);
+		CHECK_APPROX_EQUAL(box1.GetLinearVelocity(), cVelocity);
+		CHECK_APPROX_EQUAL(box1.GetAngularVelocity(), Vec3::sZero());
+		CHECK_APPROX_EQUAL(box2.GetPosition(), cPos2 - cVelocity / cFrequency);
+		CHECK_APPROX_EQUAL(box2.GetLinearVelocity(), -cVelocity);
+		CHECK_APPROX_EQUAL(box2.GetAngularVelocity(), Vec3::sZero());
+	}
+
+	// Two boxes colliding in the center, each has enough velocity to step over the other in 1 step, restitution = 1
+	TEST_CASE("TestLinearCastBoxVsLinearCastBoxElastic")
+	{
+		PhysicsTestContext c(1.0f / cFrequency, 1);
+		c.ZeroGravity();
+
+		const float cPenetrationSlop = c.GetSystem()->GetPhysicsSettings().mPenetrationSlop;
+
+		// Register listener
+		LoggingContactListener listener;
+		c.GetSystem()->SetContactListener(&listener);
+
+		Body &box1 = c.CreateBox(cPos1, Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::LinearCast, Layers::MOVING, Vec3::sReplicate(cBoxExtent));
+		box1.SetLinearVelocity(cVelocity);
+		box1.SetRestitution(1.0f);
+
+		Body &box2 = c.CreateBox(cPos2, Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::LinearCast, Layers::MOVING, Vec3::sReplicate(cBoxExtent));
+		box2.SetLinearVelocity(-cVelocity);
+		box2.SetRestitution(1.0f);
+
+		c.SimulateSingleStep();
+
+		// The bodies should have collided and the velocities reversed
+		CHECK(listener.GetEntryCount() == 2);
+		CHECK(listener.Contains(LoggingContactListener::EType::Validate, box1.GetID(), box2.GetID()));
+		CHECK(listener.Contains(LoggingContactListener::EType::Add, box1.GetID(), box2.GetID()));
+		CHECK_APPROX_EQUAL(box1.GetPosition(), RVec3(-cBoxExtent, 0, 0), cPenetrationSlop);
+		CHECK_APPROX_EQUAL(box1.GetLinearVelocity(), -cVelocity);
+		CHECK_APPROX_EQUAL(box1.GetAngularVelocity(), Vec3::sZero());
+		CHECK_APPROX_EQUAL(box2.GetPosition(), RVec3(cBoxExtent, 0, 0), cPenetrationSlop);
+		CHECK_APPROX_EQUAL(box2.GetLinearVelocity(), cVelocity);
+		CHECK_APPROX_EQUAL(box2.GetAngularVelocity(), Vec3::sZero());
+
+		listener.Clear();
+		c.SimulateSingleStep();
+
+		// In the second step the bodies should have moved away, but since they were initially overlapping we should have a contact persist callback
+		CHECK(listener.GetEntryCount() == 2);
+		CHECK(listener.Contains(LoggingContactListener::EType::Validate, box1.GetID(), box2.GetID()));
+		CHECK(listener.Contains(LoggingContactListener::EType::Persist, box1.GetID(), box2.GetID()));
+		CHECK_APPROX_EQUAL(box1.GetPosition(), RVec3(-cBoxExtent, 0, 0) - cVelocity / cFrequency, cPenetrationSlop);
+		CHECK_APPROX_EQUAL(box1.GetLinearVelocity(), -cVelocity);
+		CHECK_APPROX_EQUAL(box1.GetAngularVelocity(), Vec3::sZero());
+		CHECK_APPROX_EQUAL(box2.GetPosition(), RVec3(cBoxExtent, 0, 0) + cVelocity / cFrequency, cPenetrationSlop);
+		CHECK_APPROX_EQUAL(box2.GetLinearVelocity(), cVelocity);
+		CHECK_APPROX_EQUAL(box2.GetAngularVelocity(), Vec3::sZero());
+
+		listener.Clear();
+		c.SimulateSingleStep();
+
+		// In the third step the bodies have separated and a contact remove callback should have been received
+		CHECK(listener.GetEntryCount() == 1);
+		CHECK(listener.Contains(LoggingContactListener::EType::Remove, box1.GetID(), box2.GetID()));
+		CHECK_APPROX_EQUAL(box1.GetPosition(), RVec3(-cBoxExtent, 0, 0) - 2.0f * cVelocity / cFrequency, cPenetrationSlop);
+		CHECK_APPROX_EQUAL(box1.GetLinearVelocity(), -cVelocity);
+		CHECK_APPROX_EQUAL(box1.GetAngularVelocity(), Vec3::sZero());
+		CHECK_APPROX_EQUAL(box2.GetPosition(), RVec3(cBoxExtent, 0, 0) + 2.0f * cVelocity / cFrequency, cPenetrationSlop);
+		CHECK_APPROX_EQUAL(box2.GetLinearVelocity(), cVelocity);
+		CHECK_APPROX_EQUAL(box2.GetAngularVelocity(), Vec3::sZero());
+	}
+
+	// Two boxes colliding in the center, each has enough velocity to step over the other in 1 step, restitution = 0
+	TEST_CASE("TestLinearCastBoxVsLinearCastBoxInelastic")
+	{
+		PhysicsTestContext c(1.0f / cFrequency, 1);
+		c.ZeroGravity();
+
+		const float cPenetrationSlop = c.GetSystem()->GetPhysicsSettings().mPenetrationSlop;
+
+		// Register listener
+		LoggingContactListener listener;
+		c.GetSystem()->SetContactListener(&listener);
+
+		Body &box1 = c.CreateBox(cPos1, Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::LinearCast, Layers::MOVING, Vec3::sReplicate(cBoxExtent));
+		box1.SetLinearVelocity(cVelocity);
+
+		Body &box2 = c.CreateBox(cPos2, Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::LinearCast, Layers::MOVING, Vec3::sReplicate(cBoxExtent));
+		box2.SetLinearVelocity(-cVelocity);
+
+		c.SimulateSingleStep();
+
+		// The bodies should have collided and both are stopped
+		CHECK(listener.GetEntryCount() == 2);
+		CHECK(listener.Contains(LoggingContactListener::EType::Validate, box1.GetID(), box2.GetID()));
+		CHECK(listener.Contains(LoggingContactListener::EType::Add, box1.GetID(), box2.GetID()));
+		CHECK_APPROX_EQUAL(box1.GetPosition(), RVec3(-cBoxExtent, 0, 0), cPenetrationSlop);
+		CHECK_APPROX_EQUAL(box1.GetLinearVelocity(), Vec3::sZero());
+		CHECK_APPROX_EQUAL(box1.GetAngularVelocity(), Vec3::sZero());
+		CHECK_APPROX_EQUAL(box2.GetPosition(), RVec3(cBoxExtent, 0, 0), cPenetrationSlop);
+		CHECK_APPROX_EQUAL(box2.GetLinearVelocity(), Vec3::sZero());
+		CHECK_APPROX_EQUAL(box2.GetAngularVelocity(), Vec3::sZero());
+
+		// The bodies should persist to contact as they are not moving
+		for (int i = 0; i < 10; ++i)
+		{
+			listener.Clear();
+			c.SimulateSingleStep();
+
+			if (i == 0)
+			{
+				// Only in the first step we will receive a validate callback since after this step the contact cache will be used
+				CHECK(listener.GetEntryCount() == 2);
+				CHECK(listener.Contains(LoggingContactListener::EType::Validate, box1.GetID(), box2.GetID()));
+			}
+			else
+				CHECK(listener.GetEntryCount() == 1);
+			CHECK(listener.Contains(LoggingContactListener::EType::Persist, box1.GetID(), box2.GetID()));
+			CHECK_APPROX_EQUAL(box1.GetPosition(), RVec3(-cBoxExtent, 0, 0), cPenetrationSlop);
+			CHECK_APPROX_EQUAL(box1.GetLinearVelocity(), Vec3::sZero());
+			CHECK_APPROX_EQUAL(box1.GetAngularVelocity(), Vec3::sZero());
+			CHECK_APPROX_EQUAL(box2.GetPosition(), RVec3(cBoxExtent, 0, 0), cPenetrationSlop);
+			CHECK_APPROX_EQUAL(box2.GetLinearVelocity(), Vec3::sZero());
+			CHECK_APPROX_EQUAL(box2.GetAngularVelocity(), Vec3::sZero());
+		}
+	}
+
+	// Two boxes colliding in the center, linear cast vs inactive linear cast
+	TEST_CASE("TestLinearCastBoxVsInactiveLinearCastBox")
+	{
+		PhysicsTestContext c(1.0f / cFrequency, 1);
+		c.ZeroGravity();
+
+		const float cPenetrationSlop = c.GetSystem()->GetPhysicsSettings().mPenetrationSlop;
+
+		// Register listener
+		LoggingContactListener listener;
+		c.GetSystem()->SetContactListener(&listener);
+		LoggingBodyActivationListener activation;
+		c.GetSystem()->SetBodyActivationListener(&activation);
+
+		Body &box1 = c.CreateBox(cPos1, Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::LinearCast, Layers::MOVING, Vec3::sReplicate(cBoxExtent));
+		box1.SetLinearVelocity(cVelocity);
+
+		Body &box2 = c.CreateBox(cPos2, Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::LinearCast, Layers::MOVING, Vec3::sReplicate(cBoxExtent), EActivation::DontActivate);
+		CHECK(!box2.IsActive());
+
+		c.SimulateSingleStep();
+
+		// The bodies should have collided and body 2 should be activated, have velocity, but not moved in this step
+		CHECK(listener.GetEntryCount() == 2);
+		CHECK(listener.Contains(LoggingContactListener::EType::Validate, box1.GetID(), box2.GetID()));
+		CHECK(listener.Contains(LoggingContactListener::EType::Add, box1.GetID(), box2.GetID()));
+		Vec3 new_velocity = 0.5f * cVelocity;
+		CHECK_APPROX_EQUAL(box1.GetPosition(), cPos2 - Vec3(2.0f * cBoxExtent, 0, 0), cPenetrationSlop);
+		CHECK_APPROX_EQUAL(box1.GetLinearVelocity(), new_velocity);
+		CHECK_APPROX_EQUAL(box1.GetAngularVelocity(), Vec3::sZero());
+		CHECK_APPROX_EQUAL(box2.GetPosition(), cPos2);
+		CHECK_APPROX_EQUAL(box2.GetLinearVelocity(), new_velocity);
+		CHECK_APPROX_EQUAL(box2.GetAngularVelocity(), Vec3::sZero());
+		CHECK(box2.IsActive());
+		CHECK(activation.Contains(LoggingBodyActivationListener::EType::Activated, box2.GetID()));
+
+		listener.Clear();
+		c.SimulateSingleStep();
+
+		// In the next step body 2 should have started to move
+		CHECK(listener.GetEntryCount() == 2);
+		CHECK(listener.Contains(LoggingContactListener::EType::Validate, box1.GetID(), box2.GetID()));
+		CHECK(listener.Contains(LoggingContactListener::EType::Persist, box1.GetID(), box2.GetID()));
+		CHECK_APPROX_EQUAL(box1.GetPosition(), cPos2 - Vec3(2.0f * cBoxExtent, 0, 0) + new_velocity / cFrequency, cPenetrationSlop);
+		CHECK_APPROX_EQUAL(box1.GetLinearVelocity(), new_velocity);
+		CHECK_APPROX_EQUAL(box1.GetAngularVelocity(), Vec3::sZero());
+		CHECK_APPROX_EQUAL(box2.GetPosition(), cPos2 + new_velocity / cFrequency);
+		CHECK_APPROX_EQUAL(box2.GetLinearVelocity(), new_velocity);
+		CHECK_APPROX_EQUAL(box2.GetAngularVelocity(), Vec3::sZero());
+	}
+
+	// Two boxes colliding in the center, linear cast vs inactive discrete
+	TEST_CASE("TestLinearCastBoxVsInactiveDiscreteBox")
+	{
+		PhysicsTestContext c(1.0f / cFrequency, 1);
+		c.ZeroGravity();
+
+		const float cPenetrationSlop = c.GetSystem()->GetPhysicsSettings().mPenetrationSlop;
+
+		// Register listener
+		LoggingContactListener listener;
+		c.GetSystem()->SetContactListener(&listener);
+		LoggingBodyActivationListener activation;
+		c.GetSystem()->SetBodyActivationListener(&activation);
+
+		Body &box1 = c.CreateBox(cPos1, Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::LinearCast, Layers::MOVING, Vec3::sReplicate(cBoxExtent));
+		box1.SetLinearVelocity(cVelocity);
+
+		Body &box2 = c.CreateBox(cPos2, Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3::sReplicate(cBoxExtent), EActivation::DontActivate);
+		CHECK(!box2.IsActive());
+
+		c.SimulateSingleStep();
+
+		// The bodies should have collided and body 2 should be activated, have velocity, but not moved in this step
+		CHECK(listener.GetEntryCount() == 2);
+		CHECK(listener.Contains(LoggingContactListener::EType::Validate, box1.GetID(), box2.GetID()));
+		CHECK(listener.Contains(LoggingContactListener::EType::Add, box1.GetID(), box2.GetID()));
+		Vec3 new_velocity = 0.5f * cVelocity;
+		CHECK_APPROX_EQUAL(box1.GetPosition(), cPos2 - Vec3(2.0f * cBoxExtent, 0, 0), cPenetrationSlop);
+		CHECK_APPROX_EQUAL(box1.GetLinearVelocity(), new_velocity);
+		CHECK_APPROX_EQUAL(box1.GetAngularVelocity(), Vec3::sZero());
+		CHECK_APPROX_EQUAL(box2.GetPosition(), cPos2);
+		CHECK_APPROX_EQUAL(box2.GetLinearVelocity(), new_velocity);
+		CHECK_APPROX_EQUAL(box2.GetAngularVelocity(), Vec3::sZero());
+		CHECK(box2.IsActive());
+		CHECK(activation.Contains(LoggingBodyActivationListener::EType::Activated, box2.GetID()));
+
+		listener.Clear();
+		c.SimulateSingleStep();
+
+		// In the next step body 2 should have started to move
+		CHECK(listener.GetEntryCount() == 2);
+		CHECK(listener.Contains(LoggingContactListener::EType::Validate, box1.GetID(), box2.GetID()));
+		CHECK(listener.Contains(LoggingContactListener::EType::Persist, box1.GetID(), box2.GetID()));
+		CHECK_APPROX_EQUAL(box1.GetPosition(), cPos2 - Vec3(2.0f * cBoxExtent, 0, 0) + new_velocity / cFrequency, cPenetrationSlop);
+		CHECK_APPROX_EQUAL(box1.GetLinearVelocity(), new_velocity);
+		CHECK_APPROX_EQUAL(box1.GetAngularVelocity(), Vec3::sZero());
+		CHECK_APPROX_EQUAL(box2.GetPosition(), cPos2 + new_velocity / cFrequency);
+		CHECK_APPROX_EQUAL(box2.GetLinearVelocity(), new_velocity);
+		CHECK_APPROX_EQUAL(box2.GetAngularVelocity(), Vec3::sZero());
+	}
+
+	// Two boxes colliding under an angle, linear cast vs inactive discrete
+	TEST_CASE("TestLinearCastBoxVsInactiveDiscreteBoxAngled")
+	{
+		const Vec3 cAngledOffset1(1, 0, -2);
+		const Vec3 cAngledVelocity = -cFrequency * 2 * cAngledOffset1;
+
+		PhysicsTestContext c(1.0f / cFrequency, 1);
+		c.ZeroGravity();
+
+		const float cPenetrationSlop = c.GetSystem()->GetPhysicsSettings().mPenetrationSlop;
+
+		// Register listener
+		LoggingContactListener listener;
+		c.GetSystem()->SetContactListener(&listener);
+		LoggingBodyActivationListener activation;
+		c.GetSystem()->SetBodyActivationListener(&activation);
+
+		// Make sure box1 exactly hits the face of box2 in the center
+		RVec3 pos1 = RVec3(2.0f * cBoxExtent, 0, 0) + cAngledOffset1;
+		Body &box1 = c.CreateBox(pos1, Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::LinearCast, Layers::MOVING, Vec3::sReplicate(cBoxExtent));
+		box1.SetLinearVelocity(cAngledVelocity);
+		box1.SetRestitution(1.0f);
+		box1.SetFriction(0.0f);
+
+		Body &box2 = c.CreateBox(RVec3::sZero(), Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3::sReplicate(cBoxExtent), EActivation::DontActivate);
+		box2.SetRestitution(1.0f);
+		box2.SetFriction(0.0f);
+		CHECK(!box2.IsActive());
+
+		c.SimulateSingleStep();
+
+		// The bodies should have collided and body 2 should be activated, have inherited the x velocity of body 1, but not moved in this step. Body 1 should have lost all of its velocity in x direction.
+		CHECK(listener.GetEntryCount() == 2);
+		CHECK(listener.Contains(LoggingContactListener::EType::Validate, box1.GetID(), box2.GetID()));
+		CHECK(listener.Contains(LoggingContactListener::EType::Add, box1.GetID(), box2.GetID()));
+		Vec3 new_velocity1 = Vec3(0, 0, cAngledVelocity.GetZ());
+		Vec3 new_velocity2 = Vec3(cAngledVelocity.GetX(), 0, 0);
+		CHECK_APPROX_EQUAL(box1.GetPosition(), RVec3(2.0f * cBoxExtent, 0, 0), 2.3f * cPenetrationSlop); // We're moving 2x as fast in the z direction and the slop is allowed in x direction: sqrt(1^2 + 2^2) ~ 2.3
+		CHECK_APPROX_EQUAL(box1.GetLinearVelocity(), new_velocity1, 1.0e-4f);
+		CHECK_APPROX_EQUAL(box1.GetAngularVelocity(), Vec3::sZero(), 2.0e-4f);
+		CHECK_APPROX_EQUAL(box2.GetPosition(), RVec3::sZero());
+		CHECK_APPROX_EQUAL(box2.GetLinearVelocity(), new_velocity2, 1.0e-4f);
+		CHECK_APPROX_EQUAL(box2.GetAngularVelocity(), Vec3::sZero(), 2.0e-4f);
+		CHECK(box2.IsActive());
+		CHECK(activation.Contains(LoggingBodyActivationListener::EType::Activated, box2.GetID()));
+	}
+
+	// Two boxes colliding in the center, linear cast vs fast moving discrete, should tunnel through because all discrete bodies are moved before linear cast bodies are tested
+	TEST_CASE("TestLinearCastBoxVsFastDiscreteBox")
+	{
+		PhysicsTestContext c(1.0f / cFrequency, 1);
+		c.ZeroGravity();
+
+		// Register listener
+		LoggingContactListener listener;
+		c.GetSystem()->SetContactListener(&listener);
+
+		Body &box1 = c.CreateBox(cPos1, Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::LinearCast, Layers::MOVING, Vec3::sReplicate(cBoxExtent));
+		box1.SetLinearVelocity(cVelocity);
+
+		Body &box2 = c.CreateBox(cPos2, Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3::sReplicate(cBoxExtent));
+		box2.SetLinearVelocity(-cVelocity);
+
+		c.SimulateSingleStep();
+
+		// No collisions should be reported and the bodies should have moved according to their velocity (tunneling through each other)
+		CHECK(listener.GetEntryCount() == 0);
+		CHECK_APPROX_EQUAL(box1.GetPosition(), cPos1 + cVelocity / cFrequency);
+		CHECK_APPROX_EQUAL(box1.GetLinearVelocity(), cVelocity);
+		CHECK_APPROX_EQUAL(box1.GetAngularVelocity(), Vec3::sZero());
+		CHECK_APPROX_EQUAL(box2.GetPosition(), cPos2 - cVelocity / cFrequency);
+		CHECK_APPROX_EQUAL(box2.GetLinearVelocity(), -cVelocity);
+		CHECK_APPROX_EQUAL(box2.GetAngularVelocity(), Vec3::sZero());
+	}
+
+	// Two boxes colliding in the center, linear cast vs moving discrete, discrete is slow enough not to tunnel through linear cast body
+	TEST_CASE("TestLinearCastBoxVsSlowDiscreteBox")
+	{
+		PhysicsTestContext c(1.0f / cFrequency, 1);
+		c.ZeroGravity();
+
+		const float cPenetrationSlop = c.GetSystem()->GetPhysicsSettings().mPenetrationSlop;
+
+		// Register listener
+		LoggingContactListener listener;
+		c.GetSystem()->SetContactListener(&listener);
+
+		Body &box1 = c.CreateBox(cPos1, Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::LinearCast, Layers::MOVING, Vec3::sReplicate(cBoxExtent));
+		box1.SetLinearVelocity(cVelocity);
+
+		// In 1 step it should move -0.1 meter on the X axis
+		const Vec3 cBox2Velocity = Vec3(-0.1f * cFrequency, 0, 0);
+
+		Body &box2 = c.CreateBox(cPos2, Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3::sReplicate(cBoxExtent));
+		box2.SetLinearVelocity(cBox2Velocity);
+
+		c.SimulateSingleStep();
+
+		// The bodies should have collided and body 2 should have moved according to its discrete step
+		CHECK(listener.GetEntryCount() == 2);
+		CHECK(listener.Contains(LoggingContactListener::EType::Validate, box1.GetID(), box2.GetID()));
+		CHECK(listener.Contains(LoggingContactListener::EType::Add, box1.GetID(), box2.GetID()));
+		RVec3 new_pos2 = cPos2 + cBox2Velocity / cFrequency;
+		Vec3 new_velocity = 0.5f * (cVelocity + cBox2Velocity);
+		CHECK_APPROX_EQUAL(box1.GetPosition(), new_pos2 - Vec3(2.0f * cBoxExtent, 0, 0), cPenetrationSlop);
+		CHECK_APPROX_EQUAL(box1.GetLinearVelocity(), new_velocity);
+		CHECK_APPROX_EQUAL(box1.GetAngularVelocity(), Vec3::sZero());
+		CHECK_APPROX_EQUAL(box2.GetPosition(), new_pos2);
+		CHECK_APPROX_EQUAL(box2.GetLinearVelocity(), new_velocity);
+		CHECK_APPROX_EQUAL(box2.GetAngularVelocity(), Vec3::sZero());
+	}
+}

lib/All/JoltPhysics/UnitTests/Physics/MutableCompoundShapeTests.cpp

@@ -0,0 +1,209 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2024 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include "PhysicsTestContext.h"
+#include <Jolt/Physics/Collision/Shape/SphereShape.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Collision/Shape/MutableCompoundShape.h>
+#include <Jolt/Physics/Collision/CollisionCollectorImpl.h>
+#include <Jolt/Physics/Collision/CollidePointResult.h>
+#include <Jolt/Physics/Collision/CollideShape.h>
+
+TEST_SUITE("MutableCompoundShapeTests")
+{
+	TEST_CASE("TestMutableCompoundShapeAddRemove")
+	{
+		MutableCompoundShapeSettings settings;
+		Ref<Shape> sphere1 = new SphereShape(1.0f);
+		settings.AddShape(Vec3::sZero(), Quat::sIdentity(), sphere1);
+		Ref<MutableCompoundShape> shape = StaticCast<MutableCompoundShape>(settings.Create().Get());
+
+		auto check_shape_hit = [shape] (Vec3Arg inPosition) {
+			AllHitCollisionCollector<CollidePointCollector> collector;
+			shape->CollidePoint(inPosition - shape->GetCenterOfMass(), SubShapeIDCreator(), collector);
+			SubShapeID remainder;
+			CHECK(collector.mHits.size() <= 1);
+			return !collector.mHits.empty()? shape->GetSubShape(shape->GetSubShapeIndexFromID(collector.mHits[0].mSubShapeID2, remainder)).mShape : nullptr;
+		};
+
+		CHECK(shape->GetNumSubShapes() == 1);
+		CHECK(shape->GetSubShape(0).mShape == sphere1);
+		CHECK(shape->GetLocalBounds() == AABox(Vec3(-1, -1, -1), Vec3(1, 1, 1)));
+		CHECK(check_shape_hit(Vec3::sZero()) == sphere1);
+
+		Ref<Shape> sphere2 = new SphereShape(2.0f);
+		shape->AddShape(Vec3(10, 0, 0), Quat::sIdentity(), sphere2, 0, 0); // Insert at the start
+		CHECK(shape->GetNumSubShapes() == 2);
+		CHECK(shape->GetSubShape(0).mShape == sphere2);
+		CHECK(shape->GetSubShape(1).mShape == sphere1);
+		CHECK(shape->GetLocalBounds() == AABox(Vec3(-1, -2, -2), Vec3(12, 2, 2)));
+		CHECK(check_shape_hit(Vec3::sZero()) == sphere1);
+		CHECK(check_shape_hit(Vec3(10, 0, 0)) == sphere2);
+
+		Ref<Shape> sphere3 = new SphereShape(3.0f);
+		shape->AddShape(Vec3(20, 0, 0), Quat::sIdentity(), sphere3, 0, 2); // Insert at the end
+		CHECK(shape->GetNumSubShapes() == 3);
+		CHECK(shape->GetSubShape(0).mShape == sphere2);
+		CHECK(shape->GetSubShape(1).mShape == sphere1);
+		CHECK(shape->GetSubShape(2).mShape == sphere3);
+		CHECK(shape->GetLocalBounds() == AABox(Vec3(-1, -3, -3), Vec3(23, 3, 3)));
+		CHECK(check_shape_hit(Vec3::sZero()) == sphere1);
+		CHECK(check_shape_hit(Vec3(10, 0, 0)) == sphere2);
+		CHECK(check_shape_hit(Vec3(20, 0, 0)) == sphere3);
+
+		shape->RemoveShape(1);
+		CHECK(shape->GetNumSubShapes() == 2);
+		CHECK(shape->GetSubShape(0).mShape == sphere2);
+		CHECK(shape->GetSubShape(1).mShape == sphere3);
+		CHECK(shape->GetLocalBounds() == AABox(Vec3(8, -3, -3), Vec3(23, 3, 3)));
+		CHECK(check_shape_hit(Vec3(0, 0, 0)) == nullptr);
+		CHECK(check_shape_hit(Vec3(10, 0, 0)) == sphere2);
+		CHECK(check_shape_hit(Vec3(20, 0, 0)) == sphere3);
+
+		Ref<Shape> sphere4 = new SphereShape(4.0f);
+		shape->AddShape(Vec3(0, 0, 0), Quat::sIdentity(), sphere4, 0); // Insert at the end
+		CHECK(shape->GetNumSubShapes() == 3);
+		CHECK(shape->GetSubShape(0).mShape == sphere2);
+		CHECK(shape->GetSubShape(1).mShape == sphere3);
+		CHECK(shape->GetSubShape(2).mShape == sphere4);
+		CHECK(shape->GetLocalBounds() == AABox(Vec3(-4, -4, -4), Vec3(23, 4, 4)));
+		CHECK(check_shape_hit(Vec3::sZero()) == sphere4);
+		CHECK(check_shape_hit(Vec3(10, 0, 0)) == sphere2);
+		CHECK(check_shape_hit(Vec3(20, 0, 0)) == sphere3);
+
+		Ref<Shape> sphere5 = new SphereShape(1.0f);
+		shape->AddShape(Vec3(15, 0, 0), Quat::sIdentity(), sphere5, 0, 1); // Insert in the middle
+		CHECK(shape->GetNumSubShapes() == 4);
+		CHECK(shape->GetSubShape(0).mShape == sphere2);
+		CHECK(shape->GetSubShape(1).mShape == sphere5);
+		CHECK(shape->GetSubShape(2).mShape == sphere3);
+		CHECK(shape->GetSubShape(3).mShape == sphere4);
+		CHECK(shape->GetLocalBounds() == AABox(Vec3(-4, -4, -4), Vec3(23, 4, 4)));
+		CHECK(check_shape_hit(Vec3::sZero()) == sphere4);
+		CHECK(check_shape_hit(Vec3(10, 0, 0)) == sphere2);
+		CHECK(check_shape_hit(Vec3(15, 0, 0)) == sphere5);
+		CHECK(check_shape_hit(Vec3(20, 0, 0)) == sphere3);
+
+		shape->RemoveShape(3);
+		CHECK(shape->GetNumSubShapes() == 3);
+		CHECK(shape->GetSubShape(0).mShape == sphere2);
+		CHECK(shape->GetSubShape(1).mShape == sphere5);
+		CHECK(shape->GetSubShape(2).mShape == sphere3);
+		CHECK(shape->GetLocalBounds() == AABox(Vec3(8, -3, -3), Vec3(23, 3, 3)));
+		CHECK(check_shape_hit(Vec3::sZero()) == nullptr);
+		CHECK(check_shape_hit(Vec3(10, 0, 0)) == sphere2);
+		CHECK(check_shape_hit(Vec3(15, 0, 0)) == sphere5);
+		CHECK(check_shape_hit(Vec3(20, 0, 0)) == sphere3);
+
+		shape->RemoveShape(1);
+		CHECK(shape->GetNumSubShapes() == 2);
+		CHECK(shape->GetSubShape(0).mShape == sphere2);
+		CHECK(shape->GetSubShape(1).mShape == sphere3);
+		CHECK(shape->GetLocalBounds() == AABox(Vec3(8, -3, -3), Vec3(23, 3, 3)));
+		CHECK(check_shape_hit(Vec3::sZero()) == nullptr);
+		CHECK(check_shape_hit(Vec3(10, 0, 0)) == sphere2);
+		CHECK(check_shape_hit(Vec3(15, 0, 0)) == nullptr);
+		CHECK(check_shape_hit(Vec3(20, 0, 0)) == sphere3);
+
+		shape->RemoveShape(1);
+		CHECK(shape->GetNumSubShapes() == 1);
+		CHECK(shape->GetSubShape(0).mShape == sphere2);
+		CHECK(shape->GetLocalBounds() == AABox(Vec3(8, -2, -2), Vec3(12, 2, 2)));
+		CHECK(check_shape_hit(Vec3::sZero()) == nullptr);
+		CHECK(check_shape_hit(Vec3(10, 0, 0)) == sphere2);
+		CHECK(check_shape_hit(Vec3(15, 0, 0)) == nullptr);
+		CHECK(check_shape_hit(Vec3(20, 0, 0)) == nullptr);
+
+		shape->RemoveShape(0);
+		CHECK(shape->GetNumSubShapes() == 0);
+		CHECK(shape->GetLocalBounds() == AABox(Vec3::sZero(), Vec3::sZero()));
+		CHECK(check_shape_hit(Vec3::sZero()) == nullptr);
+		CHECK(check_shape_hit(Vec3(10, 0, 0)) == nullptr);
+		CHECK(check_shape_hit(Vec3(15, 0, 0)) == nullptr);
+		CHECK(check_shape_hit(Vec3(20, 0, 0)) == nullptr);
+	}
+
+	TEST_CASE("TestMutableCompoundShapeAdjustCenterOfMass")
+	{
+		// Start with a box at (-1 0 0)
+		MutableCompoundShapeSettings settings;
+		Ref<Shape> box_shape1 = new BoxShape(Vec3::sOne());
+		box_shape1->SetUserData(1);
+		settings.AddShape(Vec3(-1.0f, 0.0f, 0.0f), Quat::sIdentity(), box_shape1);
+		Ref<MutableCompoundShape> shape = StaticCast<MutableCompoundShape>(settings.Create().Get());
+		CHECK(shape->GetCenterOfMass() == Vec3(-1.0f, 0.0f, 0.0f));
+		CHECK(shape->GetLocalBounds() == AABox(Vec3::sReplicate(-1.0f), Vec3::sOne()));
+
+		// Check that we can hit the box
+		AllHitCollisionCollector<CollidePointCollector> collector;
+		shape->CollidePoint(Vec3(-0.5f, 0.0f, 0.0f) - shape->GetCenterOfMass(), SubShapeIDCreator(), collector);
+		CHECK((collector.mHits.size() == 1 && shape->GetSubShapeUserData(collector.mHits[0].mSubShapeID2) == 1));
+		collector.Reset();
+		CHECK(collector.mHits.empty());
+
+		// Now add another box at (1 0 0)
+		Ref<Shape> box_shape2 = new BoxShape(Vec3::sOne());
+		box_shape2->SetUserData(2);
+		shape->AddShape(Vec3(1.0f, 0.0f, 0.0f), Quat::sIdentity(), box_shape2);
+		CHECK(shape->GetCenterOfMass() == Vec3(-1.0f, 0.0f, 0.0f));
+		CHECK(shape->GetLocalBounds() == AABox(Vec3(-1.0f, -1.0f, -1.0f), Vec3(3.0f, 1.0f, 1.0f)));
+
+		// Check that we can hit both boxes
+		shape->CollidePoint(Vec3(-0.5f, 0.0f, 0.0f) - shape->GetCenterOfMass(), SubShapeIDCreator(), collector);
+		CHECK((collector.mHits.size() == 1 && shape->GetSubShapeUserData(collector.mHits[0].mSubShapeID2) == 1));
+		collector.Reset();
+		shape->CollidePoint(Vec3(0.5f, 0.0f, 0.0f) - shape->GetCenterOfMass(), SubShapeIDCreator(), collector);
+		CHECK((collector.mHits.size() == 1 && shape->GetSubShapeUserData(collector.mHits[0].mSubShapeID2) == 2));
+		collector.Reset();
+
+		// Adjust the center of mass
+		shape->AdjustCenterOfMass();
+		CHECK(shape->GetCenterOfMass() == Vec3::sZero());
+		CHECK(shape->GetLocalBounds() == AABox(Vec3(-2.0f, -1.0f, -1.0f), Vec3(2.0f, 1.0f, 1.0f)));
+
+		// Check that we can hit both boxes
+		shape->CollidePoint(Vec3(-0.5f, 0.0f, 0.0f) - shape->GetCenterOfMass(), SubShapeIDCreator(), collector);
+		CHECK((collector.mHits.size() == 1 && shape->GetSubShapeUserData(collector.mHits[0].mSubShapeID2) == 1));
+		collector.Reset();
+		shape->CollidePoint(Vec3(0.5f, 0.0f, 0.0f) - shape->GetCenterOfMass(), SubShapeIDCreator(), collector);
+		CHECK((collector.mHits.size() == 1 && shape->GetSubShapeUserData(collector.mHits[0].mSubShapeID2) == 2));
+		collector.Reset();
+	}
+
+	TEST_CASE("TestEmptyMutableCompoundShape")
+	{
+		// Create an empty compound shape
+		PhysicsTestContext c;
+		MutableCompoundShapeSettings settings;
+		Ref<MutableCompoundShape> shape = StaticCast<MutableCompoundShape>(settings.Create().Get());
+		BodyCreationSettings bcs(shape, RVec3::sZero(), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING);
+		bcs.mLinearDamping = 0.0f;
+		bcs.mOverrideMassProperties = EOverrideMassProperties::MassAndInertiaProvided;
+		bcs.mMassPropertiesOverride.mMass = 1.0f;
+		bcs.mMassPropertiesOverride.mInertia = Mat44::sIdentity();
+		BodyID body_id = c.GetBodyInterface().CreateAndAddBody(bcs, EActivation::Activate);
+
+		// Simulate with empty shape
+		c.Simulate(1.0f);
+		RVec3 expected_pos = c.PredictPosition(RVec3::sZero(), Vec3::sZero(), c.GetSystem()->GetGravity(), 1.0f);
+		CHECK_APPROX_EQUAL(c.GetBodyInterface().GetPosition(body_id), expected_pos);
+
+		// Check that we can't hit the shape
+		Ref<Shape> box_shape = new BoxShape(Vec3::sReplicate(10000));
+		AllHitCollisionCollector<CollideShapeCollector> collector;
+		c.GetSystem()->GetNarrowPhaseQuery().CollideShape(box_shape, Vec3::sOne(), RMat44::sIdentity(), CollideShapeSettings(), RVec3::sZero(), collector);
+		CHECK(collector.mHits.empty());
+
+		// Add a box to the compound shape
+		Vec3 com = shape->GetCenterOfMass();
+		shape->AddShape(Vec3::sZero(), Quat::sIdentity(), new BoxShape(Vec3::sOne()));
+		c.GetBodyInterface().NotifyShapeChanged(body_id, com, false, EActivation::DontActivate);
+
+		// Check that we can now hit the shape
+		c.GetSystem()->GetNarrowPhaseQuery().CollideShape(box_shape, Vec3::sOne(), RMat44::sIdentity(), CollideShapeSettings(), RVec3::sZero(), collector);
+		CHECK(collector.mHits.size() == 1);
+		CHECK(collector.mHits[0].mBodyID2 == body_id);
+	}
+}

lib/All/JoltPhysics/UnitTests/Physics/ObjectLayerPairFilterMaskTests.cpp

@@ -0,0 +1,161 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include "Layers.h"
+#include "LoggingContactListener.h"
+#include <Jolt/Physics/Collision/BroadPhase/BroadPhaseLayerInterfaceMask.h>
+#include <Jolt/Physics/Collision/BroadPhase/ObjectVsBroadPhaseLayerFilterMask.h>
+#include <Jolt/Physics/Collision/ObjectLayerPairFilterMask.h>
+#include <Jolt/Physics/PhysicsSystem.h>
+#include <Jolt/Core/JobSystemSingleThreaded.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+
+TEST_SUITE("ObjectLayerPairFilterMaskTests")
+{
+	TEST_CASE("ObjectLayerPairFilterMaskTest")
+	{
+		// Some example layers
+		constexpr uint32 FilterDefault = 1;
+		constexpr uint32 FilterStatic = 2;
+		constexpr uint32 FilterDebris = 4;
+		constexpr uint32 FilterSensor = 8;
+		constexpr uint32 FilterAll = FilterDefault | FilterStatic | FilterDebris | FilterSensor;
+
+		ObjectLayerPairFilterMask pair_filter;
+
+		ObjectLayer layer1 = ObjectLayerPairFilterMask::sGetObjectLayer(FilterDefault, FilterAll);
+		ObjectLayer layer2 = ObjectLayerPairFilterMask::sGetObjectLayer(FilterStatic, FilterAll);
+		CHECK(pair_filter.ShouldCollide(layer1, layer2));
+		CHECK(pair_filter.ShouldCollide(layer2, layer1));
+
+		layer1 = ObjectLayerPairFilterMask::sGetObjectLayer(FilterDefault, FilterStatic);
+		layer2 = ObjectLayerPairFilterMask::sGetObjectLayer(FilterStatic, FilterDefault);
+		CHECK(pair_filter.ShouldCollide(layer1, layer2));
+		CHECK(pair_filter.ShouldCollide(layer2, layer1));
+
+		layer1 = ObjectLayerPairFilterMask::sGetObjectLayer(FilterDefault, FilterDefault);
+		layer2 = ObjectLayerPairFilterMask::sGetObjectLayer(FilterStatic, FilterDefault);
+		CHECK(!pair_filter.ShouldCollide(layer1, layer2));
+		CHECK(!pair_filter.ShouldCollide(layer2, layer1));
+
+		layer1 = ObjectLayerPairFilterMask::sGetObjectLayer(FilterDefault, FilterStatic);
+		layer2 = ObjectLayerPairFilterMask::sGetObjectLayer(FilterStatic, FilterStatic);
+		CHECK(!pair_filter.ShouldCollide(layer1, layer2));
+		CHECK(!pair_filter.ShouldCollide(layer2, layer1));
+
+		layer1 = ObjectLayerPairFilterMask::sGetObjectLayer(FilterDefault | FilterDebris, FilterAll);
+		layer2 = ObjectLayerPairFilterMask::sGetObjectLayer(FilterStatic, FilterStatic);
+		CHECK(!pair_filter.ShouldCollide(layer1, layer2));
+		CHECK(!pair_filter.ShouldCollide(layer2, layer1));
+
+		BroadPhaseLayerInterfaceMask bp_interface(4);
+		bp_interface.ConfigureLayer(BroadPhaseLayer(0), FilterDefault, 0); // Default goes to 0
+		bp_interface.ConfigureLayer(BroadPhaseLayer(1), FilterStatic, FilterSensor); // Static but not sensor goes to 1
+		bp_interface.ConfigureLayer(BroadPhaseLayer(2), FilterStatic, 0); // Everything else static goes to 2
+		// Last layer is for everything else
+
+		CHECK(bp_interface.GetBroadPhaseLayer(ObjectLayerPairFilterMask::sGetObjectLayer(FilterDefault)) == BroadPhaseLayer(0));
+		CHECK(bp_interface.GetBroadPhaseLayer(ObjectLayerPairFilterMask::sGetObjectLayer(FilterAll)) == BroadPhaseLayer(0));
+		CHECK(bp_interface.GetBroadPhaseLayer(ObjectLayerPairFilterMask::sGetObjectLayer(FilterStatic)) == BroadPhaseLayer(1));
+		CHECK(bp_interface.GetBroadPhaseLayer(ObjectLayerPairFilterMask::sGetObjectLayer(FilterStatic | FilterSensor)) == BroadPhaseLayer(2));
+		CHECK(bp_interface.GetBroadPhaseLayer(ObjectLayerPairFilterMask::sGetObjectLayer(FilterDebris)) == BroadPhaseLayer(3));
+
+		ObjectVsBroadPhaseLayerFilterMask bp_filter(bp_interface);
+
+		CHECK(bp_filter.ShouldCollide(ObjectLayerPairFilterMask::sGetObjectLayer(FilterAll, FilterDefault), BroadPhaseLayer(0)));
+		CHECK(!bp_filter.ShouldCollide(ObjectLayerPairFilterMask::sGetObjectLayer(FilterAll, FilterDefault), BroadPhaseLayer(1)));
+		CHECK(!bp_filter.ShouldCollide(ObjectLayerPairFilterMask::sGetObjectLayer(FilterAll, FilterDefault), BroadPhaseLayer(2)));
+		CHECK(bp_filter.ShouldCollide(ObjectLayerPairFilterMask::sGetObjectLayer(FilterAll, FilterDefault), BroadPhaseLayer(3)));
+
+		CHECK(!bp_filter.ShouldCollide(ObjectLayerPairFilterMask::sGetObjectLayer(FilterAll, FilterStatic), BroadPhaseLayer(0)));
+		CHECK(bp_filter.ShouldCollide(ObjectLayerPairFilterMask::sGetObjectLayer(FilterAll, FilterStatic), BroadPhaseLayer(1)));
+		CHECK(bp_filter.ShouldCollide(ObjectLayerPairFilterMask::sGetObjectLayer(FilterAll, FilterStatic), BroadPhaseLayer(2)));
+		CHECK(bp_filter.ShouldCollide(ObjectLayerPairFilterMask::sGetObjectLayer(FilterAll, FilterStatic), BroadPhaseLayer(3)));
+
+		CHECK(!bp_filter.ShouldCollide(ObjectLayerPairFilterMask::sGetObjectLayer(FilterAll, FilterSensor), BroadPhaseLayer(0)));
+		CHECK(!bp_filter.ShouldCollide(ObjectLayerPairFilterMask::sGetObjectLayer(FilterAll, FilterSensor), BroadPhaseLayer(1)));
+		CHECK(!bp_filter.ShouldCollide(ObjectLayerPairFilterMask::sGetObjectLayer(FilterAll, FilterSensor), BroadPhaseLayer(2)));
+		CHECK(bp_filter.ShouldCollide(ObjectLayerPairFilterMask::sGetObjectLayer(FilterAll, FilterSensor), BroadPhaseLayer(3)));
+	}
+
+	TEST_CASE("ThreeFloorTest")
+	{
+		// Define the group bits
+		constexpr uint32 GROUP_STATIC = 1;
+		constexpr uint32 GROUP_FLOOR1 = 2;
+		constexpr uint32 GROUP_FLOOR2 = 4;
+		constexpr uint32 GROUP_FLOOR3 = 8;
+		constexpr uint32 GROUP_ALL = GROUP_STATIC | GROUP_FLOOR1 | GROUP_FLOOR2 | GROUP_FLOOR3;
+
+		ObjectLayerPairFilterMask pair_filter;
+
+		constexpr uint NUM_BROAD_PHASE_LAYERS = 2;
+		BroadPhaseLayer BP_LAYER_STATIC(0);
+		BroadPhaseLayer BP_LAYER_DYNAMIC(1);
+		BroadPhaseLayerInterfaceMask bp_interface(NUM_BROAD_PHASE_LAYERS);
+		bp_interface.ConfigureLayer(BP_LAYER_STATIC, GROUP_STATIC, 0); // Anything that has the static bit set goes into the static broadphase layer
+		bp_interface.ConfigureLayer(BP_LAYER_DYNAMIC, GROUP_FLOOR1 | GROUP_FLOOR2 | GROUP_FLOOR3, 0); // Anything that has one of the floor bits set goes into the dynamic broadphase layer
+
+		ObjectVsBroadPhaseLayerFilterMask bp_filter(bp_interface);
+
+		PhysicsSystem system;
+		system.Init(1024, 0, 1024, 1024, bp_interface, bp_filter, pair_filter);
+		BodyInterface &body_interface = system.GetBodyInterface();
+
+		// Create 3 floors, each colliding with a different group
+		RefConst<Shape> floor_shape = new BoxShape(Vec3(10, 0.1f, 10));
+		BodyID ground = body_interface.CreateAndAddBody(BodyCreationSettings(new BoxShape(Vec3(20, 0.1f, 20)), RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, ObjectLayerPairFilterMask::sGetObjectLayer(GROUP_STATIC, GROUP_FLOOR1)), EActivation::DontActivate);
+		BodyID floor1 = body_interface.CreateAndAddBody(BodyCreationSettings(floor_shape, RVec3(0, 2, 0), Quat::sIdentity(), EMotionType::Static, ObjectLayerPairFilterMask::sGetObjectLayer(GROUP_STATIC, GROUP_FLOOR1)), EActivation::DontActivate);
+		BodyID floor2 = body_interface.CreateAndAddBody(BodyCreationSettings(floor_shape, RVec3(0, 4, 0), Quat::sIdentity(), EMotionType::Static, ObjectLayerPairFilterMask::sGetObjectLayer(GROUP_STATIC, GROUP_FLOOR2)), EActivation::DontActivate);
+		BodyID floor3 = body_interface.CreateAndAddBody(BodyCreationSettings(floor_shape, RVec3(0, 6, 0), Quat::sIdentity(), EMotionType::Static, ObjectLayerPairFilterMask::sGetObjectLayer(GROUP_STATIC, GROUP_FLOOR3)), EActivation::DontActivate);
+
+		// Create dynamic bodies, each colliding with a different floor
+		RefConst<Shape> box_shape = new BoxShape(Vec3::sReplicate(0.5f));
+		BodyID dynamic_floor1 = body_interface.CreateAndAddBody(BodyCreationSettings(box_shape, RVec3(0, 8, 0), Quat::sIdentity(), EMotionType::Dynamic, ObjectLayerPairFilterMask::sGetObjectLayer(GROUP_FLOOR1, GROUP_ALL)), EActivation::Activate);
+		BodyID dynamic_floor2 = body_interface.CreateAndAddBody(BodyCreationSettings(box_shape, RVec3(0, 9, 0), Quat::sIdentity(), EMotionType::Dynamic, ObjectLayerPairFilterMask::sGetObjectLayer(GROUP_FLOOR2, GROUP_ALL)), EActivation::Activate);
+		BodyID dynamic_floor3 = body_interface.CreateAndAddBody(BodyCreationSettings(box_shape, RVec3(0, 10, 0), Quat::sIdentity(), EMotionType::Dynamic, ObjectLayerPairFilterMask::sGetObjectLayer(GROUP_FLOOR3, GROUP_ALL)), EActivation::Activate);
+		BodyID dynamic_ground = body_interface.CreateAndAddBody(BodyCreationSettings(box_shape, RVec3(15, 8, 0), Quat::sIdentity(), EMotionType::Dynamic, ObjectLayerPairFilterMask::sGetObjectLayer(GROUP_FLOOR1, GROUP_ALL)), EActivation::Activate);
+
+		// Start listening to collision events
+		LoggingContactListener listener;
+		system.SetContactListener(&listener);
+
+		// Simulate long enough for all objects to fall on the ground
+		TempAllocatorImpl allocator(4 * 1024 * 1024);
+		JobSystemSingleThreaded job_system(cMaxPhysicsJobs);
+		for (int i = 0; i < 100; ++i)
+			system.Update(1.0f/ 60.0f, 1, &allocator, &job_system);
+
+		// Dynamic 1 should rest on floor 1
+		CHECK(listener.Contains(LoggingContactListener::EType::Add, dynamic_floor1, floor1));
+		CHECK(!listener.Contains(LoggingContactListener::EType::Add, dynamic_floor1, floor2));
+		CHECK(!listener.Contains(LoggingContactListener::EType::Add, dynamic_floor1, floor3));
+		CHECK(!listener.Contains(LoggingContactListener::EType::Add, dynamic_floor1, ground));
+		float tolerance = 1.1f * system.GetPhysicsSettings().mPenetrationSlop;
+		CHECK_APPROX_EQUAL(body_interface.GetPosition(dynamic_floor1), RVec3(0, 2.6_r, 0), tolerance);
+
+		// Dynamic 2 should rest on floor 2
+		CHECK(!listener.Contains(LoggingContactListener::EType::Add, dynamic_floor2, floor1));
+		CHECK(listener.Contains(LoggingContactListener::EType::Add, dynamic_floor2, floor2));
+		CHECK(!listener.Contains(LoggingContactListener::EType::Add, dynamic_floor2, floor3));
+		CHECK(!listener.Contains(LoggingContactListener::EType::Add, dynamic_floor2, ground));
+		CHECK_APPROX_EQUAL(body_interface.GetPosition(dynamic_floor2), RVec3(0, 4.6_r, 0), tolerance);
+
+		// Dynamic 3 should rest on floor 3
+		CHECK(!listener.Contains(LoggingContactListener::EType::Add, dynamic_floor3, floor1));
+		CHECK(!listener.Contains(LoggingContactListener::EType::Add, dynamic_floor3, floor2));
+		CHECK(listener.Contains(LoggingContactListener::EType::Add, dynamic_floor3, floor3));
+		CHECK(!listener.Contains(LoggingContactListener::EType::Add, dynamic_floor3, ground));
+		CHECK_APPROX_EQUAL(body_interface.GetPosition(dynamic_floor3), RVec3(0, 6.6_r, 0), tolerance);
+
+		// Dynamic 4 should rest on the ground floor
+		CHECK(!listener.Contains(LoggingContactListener::EType::Add, dynamic_ground, floor1));
+		CHECK(!listener.Contains(LoggingContactListener::EType::Add, dynamic_ground, floor2));
+		CHECK(!listener.Contains(LoggingContactListener::EType::Add, dynamic_ground, floor3));
+		CHECK(listener.Contains(LoggingContactListener::EType::Add, dynamic_ground, ground));
+		CHECK_APPROX_EQUAL(body_interface.GetPosition(dynamic_ground), RVec3(15, 0.6_r, 0), tolerance);
+	}
+}

lib/All/JoltPhysics/UnitTests/Physics/ObjectLayerPairFilterTableTests.cpp

@@ -0,0 +1,133 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include "Layers.h"
+#include <Jolt/Physics/Collision/BroadPhase/BroadPhaseLayerInterfaceTable.h>
+#include <Jolt/Physics/Collision/BroadPhase/ObjectVsBroadPhaseLayerFilterTable.h>
+#include <Jolt/Physics/Collision/ObjectLayerPairFilterTable.h>
+
+TEST_SUITE("ObjectLayerPairFilterTableTests")
+{
+	TEST_CASE("ObjectLayerPairFilterTableTest")
+	{
+		// Init object layers
+		ObjectLayerPairFilterTable obj_vs_obj_filter(Layers::NUM_LAYERS);
+		obj_vs_obj_filter.EnableCollision(Layers::MOVING, Layers::NON_MOVING);
+		obj_vs_obj_filter.EnableCollision(Layers::MOVING, Layers::MOVING);
+		obj_vs_obj_filter.EnableCollision(Layers::MOVING, Layers::SENSOR);
+		obj_vs_obj_filter.EnableCollision(Layers::LQ_DEBRIS, Layers::NON_MOVING);
+		obj_vs_obj_filter.EnableCollision(Layers::HQ_DEBRIS, Layers::NON_MOVING);
+		obj_vs_obj_filter.EnableCollision(Layers::HQ_DEBRIS, Layers::MOVING);
+
+		// Check collision pairs
+		CHECK(!obj_vs_obj_filter.ShouldCollide(Layers::NON_MOVING, Layers::NON_MOVING));
+		CHECK(obj_vs_obj_filter.ShouldCollide(Layers::NON_MOVING, Layers::MOVING));
+		CHECK(obj_vs_obj_filter.ShouldCollide(Layers::NON_MOVING, Layers::HQ_DEBRIS));
+		CHECK(obj_vs_obj_filter.ShouldCollide(Layers::NON_MOVING, Layers::LQ_DEBRIS));
+		CHECK(!obj_vs_obj_filter.ShouldCollide(Layers::NON_MOVING, Layers::SENSOR));
+
+		CHECK(obj_vs_obj_filter.ShouldCollide(Layers::MOVING, Layers::NON_MOVING));
+		CHECK(obj_vs_obj_filter.ShouldCollide(Layers::MOVING, Layers::MOVING));
+		CHECK(obj_vs_obj_filter.ShouldCollide(Layers::MOVING, Layers::HQ_DEBRIS));
+		CHECK(!obj_vs_obj_filter.ShouldCollide(Layers::MOVING, Layers::LQ_DEBRIS));
+		CHECK(obj_vs_obj_filter.ShouldCollide(Layers::MOVING, Layers::SENSOR));
+
+		CHECK(obj_vs_obj_filter.ShouldCollide(Layers::HQ_DEBRIS, Layers::NON_MOVING));
+		CHECK(obj_vs_obj_filter.ShouldCollide(Layers::HQ_DEBRIS, Layers::MOVING));
+		CHECK(!obj_vs_obj_filter.ShouldCollide(Layers::HQ_DEBRIS, Layers::HQ_DEBRIS));
+		CHECK(!obj_vs_obj_filter.ShouldCollide(Layers::HQ_DEBRIS, Layers::LQ_DEBRIS));
+		CHECK(!obj_vs_obj_filter.ShouldCollide(Layers::HQ_DEBRIS, Layers::SENSOR));
+
+		CHECK(obj_vs_obj_filter.ShouldCollide(Layers::LQ_DEBRIS, Layers::NON_MOVING));
+		CHECK(!obj_vs_obj_filter.ShouldCollide(Layers::LQ_DEBRIS, Layers::MOVING));
+		CHECK(!obj_vs_obj_filter.ShouldCollide(Layers::LQ_DEBRIS, Layers::HQ_DEBRIS));
+		CHECK(!obj_vs_obj_filter.ShouldCollide(Layers::LQ_DEBRIS, Layers::LQ_DEBRIS));
+		CHECK(!obj_vs_obj_filter.ShouldCollide(Layers::LQ_DEBRIS, Layers::SENSOR));
+
+		CHECK(!obj_vs_obj_filter.ShouldCollide(Layers::SENSOR, Layers::NON_MOVING));
+		CHECK(obj_vs_obj_filter.ShouldCollide(Layers::SENSOR, Layers::MOVING));
+		CHECK(!obj_vs_obj_filter.ShouldCollide(Layers::SENSOR, Layers::HQ_DEBRIS));
+		CHECK(!obj_vs_obj_filter.ShouldCollide(Layers::SENSOR, Layers::LQ_DEBRIS));
+		CHECK(!obj_vs_obj_filter.ShouldCollide(Layers::SENSOR, Layers::SENSOR));
+
+		// Init broad phase layers
+		BroadPhaseLayerInterfaceTable bp_layer_interface(Layers::NUM_LAYERS, BroadPhaseLayers::NUM_LAYERS);
+		bp_layer_interface.MapObjectToBroadPhaseLayer(Layers::NON_MOVING, BroadPhaseLayers::NON_MOVING);
+		bp_layer_interface.MapObjectToBroadPhaseLayer(Layers::MOVING, BroadPhaseLayers::MOVING);
+		bp_layer_interface.MapObjectToBroadPhaseLayer(Layers::HQ_DEBRIS, BroadPhaseLayers::MOVING);
+		bp_layer_interface.MapObjectToBroadPhaseLayer(Layers::LQ_DEBRIS, BroadPhaseLayers::LQ_DEBRIS);
+		bp_layer_interface.MapObjectToBroadPhaseLayer(Layers::SENSOR, BroadPhaseLayers::SENSOR);
+
+	#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED)
+		// Set layer names
+		bp_layer_interface.SetBroadPhaseLayerName(BroadPhaseLayers::NON_MOVING, "NON_MOVING");
+		bp_layer_interface.SetBroadPhaseLayerName(BroadPhaseLayers::MOVING, "MOVING");
+		bp_layer_interface.SetBroadPhaseLayerName(BroadPhaseLayers::LQ_DEBRIS, "LQ_DEBRIS");
+		bp_layer_interface.SetBroadPhaseLayerName(BroadPhaseLayers::SENSOR, "SENSOR");
+
+		// Check layer name interface
+		CHECK(strcmp(bp_layer_interface.GetBroadPhaseLayerName(BroadPhaseLayers::NON_MOVING), "NON_MOVING") == 0);
+		CHECK(strcmp(bp_layer_interface.GetBroadPhaseLayerName(BroadPhaseLayers::MOVING), "MOVING") == 0);
+		CHECK(strcmp(bp_layer_interface.GetBroadPhaseLayerName(BroadPhaseLayers::LQ_DEBRIS), "LQ_DEBRIS") == 0);
+		CHECK(strcmp(bp_layer_interface.GetBroadPhaseLayerName(BroadPhaseLayers::SENSOR), "SENSOR") == 0);
+	#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED
+
+		// Init object vs broad phase layer filter
+		ObjectVsBroadPhaseLayerFilterTable obj_vs_bp_filter(bp_layer_interface, BroadPhaseLayers::NUM_LAYERS, obj_vs_obj_filter, Layers::NUM_LAYERS);
+
+		// Check collision pairs
+		CHECK(!obj_vs_bp_filter.ShouldCollide(Layers::NON_MOVING, BroadPhaseLayers::NON_MOVING));
+		CHECK(obj_vs_bp_filter.ShouldCollide(Layers::NON_MOVING, BroadPhaseLayers::MOVING));
+		CHECK(obj_vs_bp_filter.ShouldCollide(Layers::NON_MOVING, BroadPhaseLayers::LQ_DEBRIS));
+		CHECK(!obj_vs_bp_filter.ShouldCollide(Layers::NON_MOVING, BroadPhaseLayers::SENSOR));
+
+		CHECK(obj_vs_bp_filter.ShouldCollide(Layers::MOVING, BroadPhaseLayers::NON_MOVING));
+		CHECK(obj_vs_bp_filter.ShouldCollide(Layers::MOVING, BroadPhaseLayers::MOVING));
+		CHECK(!obj_vs_bp_filter.ShouldCollide(Layers::MOVING, BroadPhaseLayers::LQ_DEBRIS));
+		CHECK(obj_vs_bp_filter.ShouldCollide(Layers::MOVING, BroadPhaseLayers::SENSOR));
+
+		CHECK(obj_vs_bp_filter.ShouldCollide(Layers::HQ_DEBRIS, BroadPhaseLayers::NON_MOVING));
+		CHECK(obj_vs_bp_filter.ShouldCollide(Layers::HQ_DEBRIS, BroadPhaseLayers::MOVING));
+		CHECK(!obj_vs_bp_filter.ShouldCollide(Layers::HQ_DEBRIS, BroadPhaseLayers::LQ_DEBRIS));
+		CHECK(!obj_vs_bp_filter.ShouldCollide(Layers::HQ_DEBRIS, BroadPhaseLayers::SENSOR));
+
+		CHECK(obj_vs_bp_filter.ShouldCollide(Layers::LQ_DEBRIS, BroadPhaseLayers::NON_MOVING));
+		CHECK(!obj_vs_bp_filter.ShouldCollide(Layers::LQ_DEBRIS, BroadPhaseLayers::MOVING));
+		CHECK(!obj_vs_bp_filter.ShouldCollide(Layers::LQ_DEBRIS, BroadPhaseLayers::LQ_DEBRIS));
+		CHECK(!obj_vs_bp_filter.ShouldCollide(Layers::LQ_DEBRIS, BroadPhaseLayers::SENSOR));
+
+		CHECK(!obj_vs_bp_filter.ShouldCollide(Layers::SENSOR, BroadPhaseLayers::NON_MOVING));
+		CHECK(obj_vs_bp_filter.ShouldCollide(Layers::SENSOR, BroadPhaseLayers::MOVING));
+		CHECK(!obj_vs_bp_filter.ShouldCollide(Layers::SENSOR, BroadPhaseLayers::LQ_DEBRIS));
+		CHECK(!obj_vs_bp_filter.ShouldCollide(Layers::SENSOR, BroadPhaseLayers::SENSOR));
+	}
+
+	TEST_CASE("ObjectLayerPairFilterTableTest2")
+	{
+		const int n = 10;
+
+		std::pair<ObjectLayer, ObjectLayer> pairs[] = {
+			{ ObjectLayer(0), ObjectLayer(0) },
+			{ ObjectLayer(9), ObjectLayer(9) },
+			{ ObjectLayer(1), ObjectLayer(3) },
+			{ ObjectLayer(3), ObjectLayer(1) },
+			{ ObjectLayer(5), ObjectLayer(7) },
+			{ ObjectLayer(7), ObjectLayer(5) }
+		};
+
+		for (auto &p : pairs)
+		{
+			ObjectLayerPairFilterTable obj_vs_obj_filter(n);
+			obj_vs_obj_filter.EnableCollision(p.first, p.second);
+
+			for (ObjectLayer i = 0; i < n; ++i)
+				for (ObjectLayer j = 0; j < n; ++j)
+				{
+					bool should_collide = (i == p.first && j == p.second) || (i == p.second && j == p.first);
+					CHECK(obj_vs_obj_filter.ShouldCollide(i, j) == should_collide);
+				}
+		}
+	}
+}

lib/All/JoltPhysics/UnitTests/Physics/OffsetCenterOfMassShapeTests.cpp

@@ -0,0 +1,154 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include "PhysicsTestContext.h"
+#include "Layers.h"
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Collision/Shape/OffsetCenterOfMassShape.h>
+
+TEST_SUITE("OffsetCenterOfMassShapeTests")
+{
+	TEST_CASE("TestAddAngularImpulseCOMZero")
+	{
+		PhysicsTestContext c;
+		c.ZeroGravity();
+
+		// Create box
+		const Vec3 cHalfExtent = Vec3(0.5f, 1.0f, 1.5f);
+		BoxShapeSettings box(cHalfExtent);
+		box.SetEmbedded();
+
+		// Create body with COM offset 0
+		OffsetCenterOfMassShapeSettings com(Vec3::sZero(), &box);
+		com.SetEmbedded();
+		Body &body = c.CreateBody(&com, RVec3::sZero(), Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, EActivation::DontActivate);
+
+		// Check mass and inertia calculated correctly
+		float mass = (8.0f * cHalfExtent.GetX() * cHalfExtent.GetY() * cHalfExtent.GetZ()) * box.mDensity;
+		CHECK_APPROX_EQUAL(body.GetMotionProperties()->GetInverseMass(), 1.0f / mass);
+		float inertia_y = mass / 12.0f * (Square(2.0f * cHalfExtent.GetX()) + Square(2.0f * cHalfExtent.GetZ())); // See: https://en.wikipedia.org/wiki/List_of_moments_of_inertia
+		CHECK_APPROX_EQUAL(body.GetMotionProperties()->GetInverseInertiaForRotation(Mat44::sIdentity())(1, 1), 1.0f / inertia_y);
+
+		// Add impulse
+		Vec3 cImpulse(0, 10000, 0);
+		CHECK(!body.IsActive());
+		c.GetBodyInterface().AddAngularImpulse(body.GetID(), cImpulse);
+		CHECK(body.IsActive());
+
+		// Check resulting velocity change
+		// dv = I^-1 * L
+		float delta_v = (1.0f / inertia_y) * cImpulse.GetY();
+		CHECK_APPROX_EQUAL(body.GetLinearVelocity(), Vec3::sZero());
+		CHECK_APPROX_EQUAL(body.GetAngularVelocity(), Vec3(0, delta_v, 0));
+	}
+
+	TEST_CASE("TestAddAngularImpulseCOMOffset")
+	{
+		PhysicsTestContext c;
+		c.ZeroGravity();
+
+		// Create box
+		const Vec3 cHalfExtent = Vec3(0.5f, 1.0f, 1.5f);
+		BoxShapeSettings box(cHalfExtent);
+		box.SetEmbedded();
+
+		// Create body with COM offset
+		const Vec3 cCOMOffset(5.0f, 0, 0);
+		OffsetCenterOfMassShapeSettings com(cCOMOffset, &box);
+		com.SetEmbedded();
+		Body &body = c.CreateBody(&com, RVec3::sZero(), Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, EActivation::DontActivate);
+
+		// Check mass and inertia calculated correctly
+		float mass = (8.0f * cHalfExtent.GetX() * cHalfExtent.GetY() * cHalfExtent.GetZ()) * box.mDensity;
+		CHECK_APPROX_EQUAL(body.GetMotionProperties()->GetInverseMass(), 1.0f / mass);
+		float inertia_y = mass / 12.0f * (Square(2.0f * cHalfExtent.GetX()) + Square(2.0f * cHalfExtent.GetZ())) + mass * Square(cCOMOffset.GetX()); // See: https://en.wikipedia.org/wiki/List_of_moments_of_inertia & https://en.wikipedia.org/wiki/Parallel_axis_theorem
+		CHECK_APPROX_EQUAL(body.GetMotionProperties()->GetInverseInertiaForRotation(Mat44::sIdentity())(1, 1), 1.0f / inertia_y);
+
+		// Add impulse
+		Vec3 cImpulse(0, 10000, 0);
+		CHECK(!body.IsActive());
+		c.GetBodyInterface().AddAngularImpulse(body.GetID(), cImpulse);
+		CHECK(body.IsActive());
+
+		// Check resulting velocity change
+		// dv = I^-1 * L
+		float delta_v = (1.0f / inertia_y) * cImpulse.GetY();
+		CHECK_APPROX_EQUAL(body.GetLinearVelocity(), Vec3::sZero());
+		CHECK_APPROX_EQUAL(body.GetAngularVelocity(), Vec3(0, delta_v, 0));
+	}
+
+	TEST_CASE("TestAddTorqueCOMZero")
+	{
+		PhysicsTestContext c;
+		c.ZeroGravity();
+
+		// Create box
+		const Vec3 cHalfExtent = Vec3(0.5f, 1.0f, 1.5f);
+		BoxShapeSettings box(cHalfExtent);
+		box.SetEmbedded();
+
+		// Create body with COM offset 0
+		OffsetCenterOfMassShapeSettings com(Vec3::sZero(), &box);
+		com.SetEmbedded();
+		Body &body = c.CreateBody(&com, RVec3::sZero(), Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, EActivation::DontActivate);
+
+		// Check mass and inertia calculated correctly
+		float mass = (8.0f * cHalfExtent.GetX() * cHalfExtent.GetY() * cHalfExtent.GetZ()) * box.mDensity;
+		CHECK_APPROX_EQUAL(body.GetMotionProperties()->GetInverseMass(), 1.0f / mass);
+		float inertia_y = mass / 12.0f * (Square(2.0f * cHalfExtent.GetX()) + Square(2.0f * cHalfExtent.GetZ())); // See: https://en.wikipedia.org/wiki/List_of_moments_of_inertia
+		CHECK_APPROX_EQUAL(body.GetMotionProperties()->GetInverseInertiaForRotation(Mat44::sIdentity())(1, 1), 1.0f / inertia_y);
+
+		// Add torque
+		Vec3 cTorque(0, 100000, 0);
+		CHECK(!body.IsActive());
+		c.GetBodyInterface().AddTorque(body.GetID(), cTorque);
+		CHECK(body.IsActive());
+		CHECK(body.GetAngularVelocity() == Vec3::sZero()); // Angular velocity change should come after the next time step
+		c.SimulateSingleStep();
+
+		// Check resulting velocity change
+		// dv = I^-1 * T * dt
+		float delta_v = (1.0f / inertia_y) * cTorque.GetY() * c.GetDeltaTime();
+		CHECK_APPROX_EQUAL(body.GetLinearVelocity(), Vec3::sZero());
+		CHECK_APPROX_EQUAL(body.GetAngularVelocity(), Vec3(0, delta_v, 0));
+	}
+
+	TEST_CASE("TestAddTorqueCOMOffset")
+	{
+		PhysicsTestContext c;
+		c.ZeroGravity();
+
+		// Create box
+		const Vec3 cHalfExtent = Vec3(0.5f, 1.0f, 1.5f);
+		BoxShapeSettings box(cHalfExtent);
+		box.SetEmbedded();
+
+		// Create body with COM offset
+		const Vec3 cCOMOffset(5.0f, 0, 0);
+		OffsetCenterOfMassShapeSettings com(cCOMOffset, &box);
+		com.SetEmbedded();
+		Body &body = c.CreateBody(&com, RVec3::sZero(), Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, EActivation::DontActivate);
+
+		// Check mass and inertia calculated correctly
+		float mass = (8.0f * cHalfExtent.GetX() * cHalfExtent.GetY() * cHalfExtent.GetZ()) * box.mDensity;
+		CHECK_APPROX_EQUAL(body.GetMotionProperties()->GetInverseMass(), 1.0f / mass);
+		float inertia_y = mass / 12.0f * (Square(2.0f * cHalfExtent.GetX()) + Square(2.0f * cHalfExtent.GetZ())) + mass * Square(cCOMOffset.GetX()); // See: https://en.wikipedia.org/wiki/List_of_moments_of_inertia & https://en.wikipedia.org/wiki/Parallel_axis_theorem
+		CHECK_APPROX_EQUAL(body.GetMotionProperties()->GetInverseInertiaForRotation(Mat44::sIdentity())(1, 1), 1.0f / inertia_y);
+
+		// Add torque
+		Vec3 cTorque(0, 100000, 0);
+		CHECK(!body.IsActive());
+		c.GetBodyInterface().AddTorque(body.GetID(), cTorque);
+		CHECK(body.IsActive());
+		CHECK(body.GetAngularVelocity() == Vec3::sZero()); // Angular velocity change should come after the next time step
+		c.SimulateSingleStep();
+
+		// Check resulting velocity change
+		// dv = I^-1 * T * dt
+		float delta_v = (1.0f / inertia_y) * cTorque.GetY() * c.GetDeltaTime();
+		CHECK_APPROX_EQUAL(body.GetLinearVelocity(), Vec3::sZero());
+		CHECK_APPROX_EQUAL(body.GetAngularVelocity(), Vec3(0, delta_v, 0));
+	}
+}

lib/All/JoltPhysics/UnitTests/Physics/PathConstraintTests.cpp

@@ -0,0 +1,50 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include "PhysicsTestContext.h"
+#include <Jolt/Physics/Constraints/PathConstraintPathHermite.h>
+#include <Jolt/Physics/Constraints/PathConstraintPath.h>
+#include "Layers.h"
+
+TEST_SUITE("PathConstraintTests")
+{
+	// Test a straight line using a hermite spline.
+	TEST_CASE("TestPathConstraintPathHermite")
+	{
+		// A straight spline
+		// This has e.g. for t = 0.1 a local minimum at 0.7 which breaks the Newton Raphson root finding if not doing the bisection algorithm first.
+		Vec3 p1 = Vec3(1424.96313f, 468.565399f, 483.655975f);
+		Vec3 t1 = Vec3(61.4222832f, 42.8926392f, -1.70530257e-13f);
+		Vec3 n1 = Vec3(0, 0, 1);
+		Vec3 p2 = Vec3(1445.20105f, 482.364319f, 483.655975f);
+		Vec3 t2 = Vec3(20.2380009f, 13.7989082f, -5.68434189e-14f);
+		Vec3 n2 = Vec3(0, 0, 1);
+
+		// Construct path
+		Ref<PathConstraintPathHermite> path = new PathConstraintPathHermite;
+		path->AddPoint(p1, t1, n1);
+		path->AddPoint(p2, t2, n2);
+
+		// Test that positions before and after the line return 0 and 1
+		float before_start = path->GetClosestPoint(p1 - 0.01f * t1, 0.0f);
+		CHECK(before_start == 0.0f);
+		float after_end = path->GetClosestPoint(p2 + 0.01f * t2, 0.0f);
+		CHECK(after_end == 1.0f);
+
+		for (int i = 0; i <= 10; ++i)
+		{
+			// Get point on the curve
+			float fraction = 0.1f * i;
+			Vec3 pos, tgt, nrm, bin;
+			path->GetPointOnPath(fraction, pos, tgt, nrm, bin);
+
+			// Let the path determine the fraction of the closest point
+			float closest_fraction = path->GetClosestPoint(pos, 0.0f);
+
+			// Validate that it is equal to what we put in
+			CHECK_APPROX_EQUAL(fraction, closest_fraction, 1.0e-4f);
+		}
+	}
+}

lib/All/JoltPhysics/UnitTests/Physics/PhysicsDeterminismTests.cpp

@@ -0,0 +1,195 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include "PhysicsTestContext.h"
+#include "Layers.h"
+#include <Jolt/Physics/Constraints/SwingTwistConstraint.h>
+#include <Jolt/Physics/Collision/GroupFilterTable.h>
+
+TEST_SUITE("PhysicsDeterminismTests")
+{
+	struct BodyProperties
+	{
+		RVec3	mPositionCOM;
+		Quat	mRotation;
+		Vec3	mLinearVelocity;
+		Vec3	mAngularVelocity;
+		AABox	mBounds;
+		bool	mIsActive;
+	};
+
+	/// Extract all relevant properties of a body for the test
+	static void GetBodyProperties(PhysicsTestContext &ioContext, const BodyID &inBodyID, BodyProperties &outProperties)
+	{
+		BodyLockRead lock(ioContext.GetSystem()->GetBodyLockInterface(), inBodyID);
+		if (lock.SucceededAndIsInBroadPhase())
+		{
+			const Body &body = lock.GetBody();
+			outProperties.mIsActive = body.IsActive();
+			outProperties.mPositionCOM = body.GetCenterOfMassPosition();
+			outProperties.mRotation = body.GetRotation();
+			outProperties.mLinearVelocity = body.GetLinearVelocity();
+			outProperties.mAngularVelocity = body.GetAngularVelocity();
+			outProperties.mBounds = body.GetWorldSpaceBounds();
+		}
+		else
+		{
+			CHECK(false);
+		}
+	}
+
+	/// Step two physics simulations for inTotalTime and check after each step that the simulations are identical
+	static void CompareSimulations(PhysicsTestContext &ioContext1, PhysicsTestContext &ioContext2, float inTotalTime)
+	{
+		CHECK(ioContext1.GetDeltaTime() == ioContext2.GetDeltaTime());
+
+		// Step until we've stepped for inTotalTime
+		for (float t = 0; t <= inTotalTime; t += ioContext1.GetDeltaTime())
+		{
+			// Step the simulation
+			ioContext1.SimulateSingleStep();
+			ioContext2.SimulateSingleStep();
+
+			// Get all bodies
+			BodyIDVector bodies1, bodies2;
+			ioContext1.GetSystem()->GetBodies(bodies1);
+			ioContext2.GetSystem()->GetBodies(bodies2);
+			CHECK(bodies1.size() == bodies2.size());
+
+			// Loop over all bodies
+			for (size_t b = 0; b < min(bodies1.size(), bodies2.size()); ++b)
+			{
+				// Check that the body ID's match
+				BodyID b1_id = bodies1[b];
+				BodyID b2_id = bodies2[b];
+				CHECK(b1_id == b2_id);
+
+				// Get the properties of the body
+				BodyProperties properties1, properties2;
+				GetBodyProperties(ioContext1, b1_id, properties1);
+				GetBodyProperties(ioContext2, b2_id, properties2);
+				CHECK(properties1.mIsActive == properties2.mIsActive);
+				CHECK(properties1.mPositionCOM == properties2.mPositionCOM);
+				CHECK(properties1.mRotation == properties2.mRotation);
+				CHECK(properties1.mLinearVelocity == properties2.mLinearVelocity);
+				CHECK(properties1.mAngularVelocity == properties2.mAngularVelocity);
+				CHECK(properties1.mBounds.mMin == properties2.mBounds.mMin);
+				CHECK(properties1.mBounds.mMax == properties2.mBounds.mMax);
+			}
+		}
+	}
+
+	static void CreateGridOfBoxesDiscrete(PhysicsTestContext &ioContext)
+	{
+		UnitTestRandom random;
+		uniform_real_distribution<float> restitution(0.0f, 1.0f);
+
+		ioContext.CreateFloor();
+
+		for (int x = 0; x < 5; ++x)
+			for (int z = 0; z < 5; ++z)
+			{
+				Body &body = ioContext.CreateBox(RVec3(float(x), 5.0f, float(z)), Quat::sRandom(random), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3::sReplicate(0.1f));
+				body.SetRestitution(restitution(random));
+				body.SetLinearVelocity(Vec3::sRandom(random));
+			}
+	}
+
+	TEST_CASE("TestGridOfBoxesDiscrete")
+	{
+		PhysicsTestContext c1(1.0f / 60.0f, 1, 0);
+		CreateGridOfBoxesDiscrete(c1);
+
+		PhysicsTestContext c2(1.0f / 60.0f, 1, 15);
+		CreateGridOfBoxesDiscrete(c2);
+
+		CompareSimulations(c1, c2, 5.0f);
+	}
+
+	static void CreateGridOfBoxesLinearCast(PhysicsTestContext &ioContext)
+	{
+		UnitTestRandom random;
+		uniform_real_distribution<float> restitution(0.0f, 1.0f);
+
+		ioContext.CreateFloor();
+
+		for (int x = 0; x < 5; ++x)
+			for (int z = 0; z < 5; ++z)
+			{
+				Body &body = ioContext.CreateBox(RVec3(float(x), 5.0f, float(z)), Quat::sRandom(random), EMotionType::Dynamic, EMotionQuality::LinearCast, Layers::MOVING, Vec3::sReplicate(0.1f));
+				body.SetRestitution(restitution(random));
+				body.SetLinearVelocity(Vec3::sRandom(random) - Vec3(0, -5.0f, 0));
+			}
+	}
+
+	TEST_CASE("TestGridOfBoxesLinearCast")
+	{
+		PhysicsTestContext c1(1.0f / 60.0f, 1, 0);
+		CreateGridOfBoxesLinearCast(c1);
+
+		PhysicsTestContext c2(1.0f / 60.0f, 1, 15);
+		CreateGridOfBoxesLinearCast(c2);
+
+		CompareSimulations(c1, c2, 5.0f);
+	}
+
+	static void CreateGridOfBoxesConstrained(PhysicsTestContext &ioContext)
+	{
+		UnitTestRandom random;
+		uniform_real_distribution<float> restitution(0.0f, 1.0f);
+
+		ioContext.CreateFloor();
+
+		const int cNumPerAxis = 5;
+
+		// Build a collision group filter that disables collision between adjacent bodies
+		Ref<GroupFilterTable> group_filter = new GroupFilterTable(cNumPerAxis);
+		for (CollisionGroup::SubGroupID i = 0; i < cNumPerAxis - 1; ++i)
+			group_filter->DisableCollision(i, i + 1);
+
+		// Create a number of chains
+		for (int x = 0; x < cNumPerAxis; ++x)
+		{
+			// Create a chain of bodies connected with swing twist constraints
+			Body *prev_body = nullptr;
+			for (int z = 0; z < cNumPerAxis; ++z)
+			{
+				RVec3 body_pos(float(x), 5.0f, 0.2f * float(z));
+				Body &body = ioContext.CreateBox(body_pos, Quat::sRandom(random), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3::sReplicate(0.1f));
+				body.SetRestitution(restitution(random));
+				body.SetLinearVelocity(Vec3::sRandom(random));
+				body.SetCollisionGroup(CollisionGroup(group_filter, CollisionGroup::GroupID(x), CollisionGroup::SubGroupID(z)));
+
+				// Constrain the body to the previous body
+				if (prev_body != nullptr)
+				{
+					SwingTwistConstraintSettings st;
+					st.mPosition1 = st.mPosition2 = body_pos - Vec3(0, 0, 0.1f);
+					st.mTwistAxis1 = st.mTwistAxis2 = Vec3::sAxisZ();
+					st.mPlaneAxis1 = st.mPlaneAxis2 = Vec3::sAxisX();
+					st.mNormalHalfConeAngle = DegreesToRadians(45.0f);
+					st.mPlaneHalfConeAngle = DegreesToRadians(30.0f);
+					st.mTwistMinAngle = DegreesToRadians(-15.0f);
+					st.mTwistMaxAngle = DegreesToRadians(15.0f);
+					Ref<Constraint> constraint = st.Create(*prev_body, body);
+					ioContext.GetSystem()->AddConstraint(constraint);
+				}
+
+				prev_body = &body;
+			}
+		}
+	}
+
+	TEST_CASE("TestGridOfBoxesConstrained")
+	{
+		PhysicsTestContext c1(1.0f / 60.0f, 1, 0);
+		CreateGridOfBoxesConstrained(c1);
+
+		PhysicsTestContext c2(1.0f / 60.0f, 1, 15);
+		CreateGridOfBoxesConstrained(c2);
+
+		CompareSimulations(c1, c2, 5.0f);
+	}
+}

lib/All/JoltPhysics/UnitTests/Physics/PhysicsStepListenerTests.cpp

@@ -0,0 +1,141 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include "PhysicsTestContext.h"
+#include "Layers.h"
+#include <Jolt/Physics/PhysicsStepListener.h>
+
+TEST_SUITE("StepListenerTest")
+{
+	// Custom step listener that keeps track how often it has been called
+	class TestStepListener : public PhysicsStepListener
+	{
+	public:
+		virtual void			OnStep(const PhysicsStepListenerContext &inContext) override
+		{
+			CHECK(inContext.mDeltaTime == mExpectedDeltaTime);
+			CHECK(inContext.mIsFirstStep == ((mCount % mExpectedSteps) == 0));
+			int new_count = mCount.fetch_add(1) + 1;
+			CHECK(inContext.mIsLastStep == ((new_count % mExpectedSteps) == 0));
+		}
+
+		atomic<int>				mCount = 0;
+		int						mExpectedSteps;
+		float					mExpectedDeltaTime = 0.0f;
+	};
+
+	// Perform the actual listener test with a variable amount of collision steps
+	static void DoTest(int inCollisionSteps)
+	{
+		PhysicsTestContext c(1.0f / 60.0f, inCollisionSteps);
+
+		// Initialize and add listeners
+		TestStepListener listeners[10];
+		for (TestStepListener &l : listeners)
+		{
+			l.mExpectedDeltaTime = 1.0f / 60.0f / inCollisionSteps;
+			l.mExpectedSteps = inCollisionSteps;
+		}
+		for (TestStepListener &l : listeners)
+			c.GetSystem()->AddStepListener(&l);
+
+		// Stepping without delta time should not trigger step listeners
+		c.SimulateNoDeltaTime();
+		for (TestStepListener &l : listeners)
+			CHECK(l.mCount == 0);
+
+		// Stepping with delta time should call the step listeners as they can activate bodies
+		c.SimulateSingleStep();
+		for (TestStepListener &l : listeners)
+			CHECK(l.mCount == inCollisionSteps);
+
+		// Adding an active body should have no effect, step listeners should still be called
+		c.CreateBox(RVec3::sZero(), Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3::sOne());
+
+		// Step the simulation
+		c.SimulateSingleStep();
+		for (TestStepListener &l : listeners)
+			CHECK(l.mCount == 2 * inCollisionSteps);
+
+		// Unregister all listeners
+		for (TestStepListener &l : listeners)
+			c.GetSystem()->RemoveStepListener(&l);
+
+		// Step the simulation
+		c.SimulateSingleStep();
+
+		// Check that no further callbacks were triggered
+		for (TestStepListener &l : listeners)
+			CHECK(l.mCount == 2 * inCollisionSteps);
+	}
+
+	// Test the step listeners with a single collision step
+	TEST_CASE("TestStepListener1")
+	{
+		DoTest(1);
+	}
+
+	// Test the step listeners with two collision steps
+	TEST_CASE("TestStepListener2")
+	{
+		DoTest(2);
+	}
+
+	// Test the step listeners with four collision steps
+	TEST_CASE("TestStepListener4")
+	{
+		DoTest(4);
+	}
+
+	// Activate a body in a step listener
+	TEST_CASE("TestActivateInStepListener")
+	{
+		PhysicsTestContext c(1.0f / 60.0f, 2);
+		c.ZeroGravity();
+
+		// Create a box
+		Body &body = c.CreateBox(RVec3::sZero(), Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3::sOne(), EActivation::DontActivate);
+		body.GetMotionProperties()->SetLinearDamping(0.0f);
+		BodyID body_id = body.GetID();
+
+		static const Vec3 cVelocity(10.0f, 0, 0);
+
+		class MyStepListener : public PhysicsStepListener
+		{
+		public:
+								MyStepListener(const BodyID &inBodyID, BodyInterface &inBodyInterface) : mBodyInterface(inBodyInterface), mBodyID(inBodyID) { }
+
+			virtual void		OnStep(const PhysicsStepListenerContext &inContext) override
+			{
+				if (inContext.mIsFirstStep)
+				{
+					// We activate the body and set a velocity in the first step
+					CHECK(!mBodyInterface.IsActive(mBodyID));
+					mBodyInterface.SetLinearVelocity(mBodyID, cVelocity);
+					CHECK(mBodyInterface.IsActive(mBodyID));
+				}
+				else
+				{
+					// In the second step, the body should already have been activated
+					CHECK(mBodyInterface.IsActive(mBodyID));
+				}
+			}
+
+		private:
+			BodyInterface &		mBodyInterface;
+			BodyID				mBodyID;
+		};
+
+		MyStepListener listener(body_id, c.GetSystem()->GetBodyInterfaceNoLock());
+		c.GetSystem()->AddStepListener(&listener);
+
+		c.SimulateSingleStep();
+
+		BodyInterface &bi = c.GetBodyInterface();
+		CHECK(bi.IsActive(body_id));
+		CHECK(bi.GetLinearVelocity(body_id) == cVelocity);
+		CHECK(bi.GetPosition(body_id) == RVec3(c.GetDeltaTime() * cVelocity));
+	}
+}

lib/All/JoltPhysics/UnitTests/Physics/RayShapeTests.cpp

@@ -0,0 +1,593 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include <Jolt/Physics/Collision/RayCast.h>
+#include <Jolt/Physics/Collision/CastResult.h>
+#include <Jolt/Physics/Collision/CollisionCollectorImpl.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Collision/Shape/SphereShape.h>
+#include <Jolt/Physics/Collision/Shape/ConvexHullShape.h>
+#include <Jolt/Physics/Collision/Shape/CapsuleShape.h>
+#include <Jolt/Physics/Collision/Shape/TaperedCapsuleShape.h>
+#include <Jolt/Physics/Collision/Shape/CylinderShape.h>
+#include <Jolt/Physics/Collision/Shape/TaperedCylinderShape.h>
+#include <Jolt/Physics/Collision/Shape/ScaledShape.h>
+#include <Jolt/Physics/Collision/Shape/StaticCompoundShape.h>
+#include <Jolt/Physics/Collision/Shape/MutableCompoundShape.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Jolt/Physics/PhysicsSystem.h>
+#include <Layers.h>
+
+TEST_SUITE("RayShapeTests")
+{
+	// Function that does the actual ray cast test, inExpectedFraction1/2 should be FLT_MAX if no hit expected
+	using TestFunction = function<void(const RayCast &inRay, float inExpectedFraction1, float inExpectedFraction2)>;
+
+	// Test ray against inShape with lines going through inHitA and inHitB (which should be surface positions of the shape)
+	static void TestRayHelperInternal(Vec3Arg inHitA, Vec3Arg inHitB, TestFunction inTestFunction)
+	{
+		// Determine points before and after the surface on both sides
+		Vec3 delta = inHitB - inHitA;
+		Vec3 l1 = inHitA - 2.0f * delta;
+		Vec3 l2 = inHitA - 0.1f * delta;
+		Vec3 i1 = inHitA + 0.1f * delta;
+		Vec3 i2 = inHitB - 0.1f * delta;
+		Vec3 r1 = inHitB + 0.1f * delta;
+		Vec3 r2 = inHitB + 2.0f * delta;
+
+		// -O---->-|--------|--------
+		inTestFunction(RayCast { l1, l2 - l1 }, FLT_MAX, FLT_MAX);
+
+		// -----O>-|--------|--------
+		inTestFunction(RayCast { l2, Vec3::sZero() }, FLT_MAX, FLT_MAX);
+
+		// ------O-|->------|--------
+		inTestFunction(RayCast { l2, i1 - l2 }, 0.5f, FLT_MAX);
+
+		// ------O-|--------|->------
+		inTestFunction(RayCast { l2, r1 - l2 }, 0.1f / 1.2f, 1.1f / 1.2f);
+
+		// --------|-----O>-|--------
+		inTestFunction(RayCast { i2, Vec3::sZero() }, 0.0f, FLT_MAX);
+
+		// --------|------O-|->------
+		inTestFunction(RayCast { i2, r1 - i2 }, 0.0f, 0.5f);
+
+		// --------|--------|-O---->-
+		inTestFunction(RayCast { r1, r2 - l1 }, FLT_MAX, FLT_MAX);
+	}
+
+	static void TestRayHelper(const Shape *inShape, Vec3Arg inHitA, Vec3Arg inHitB)
+	{
+		//////////////////////////////////////////////////////////////////////////////////////////////////
+		// Test function that directly tests against a shape
+		//////////////////////////////////////////////////////////////////////////////////////////////////
+		TestFunction TestShapeRay = [inShape](const RayCast &inRay, float inExpectedFraction1, float inExpectedFraction2)
+		{
+			// CastRay works relative to center of mass, so transform the ray
+			RayCast ray = inRay;
+			ray.mOrigin -= inShape->GetCenterOfMass();
+
+			RayCastResult hit;
+			SubShapeIDCreator id_creator;
+			if (inExpectedFraction1 != FLT_MAX)
+			{
+				CHECK(inShape->CastRay(ray, id_creator, hit));
+				CHECK_APPROX_EQUAL(hit.mFraction, inExpectedFraction1, 1.0e-5f);
+			}
+			else
+			{
+				CHECK_FALSE(inShape->CastRay(ray, id_creator, hit));
+			}
+		};
+
+		// Test normal ray
+		TestRayHelperInternal(inHitA, inHitB, TestShapeRay);
+
+		// Test inverse ray
+		TestRayHelperInternal(inHitB, inHitA, TestShapeRay);
+
+
+		//////////////////////////////////////////////////////////////////////////////////////////////////
+		// Test function that directly tests against a shape allowing multiple hits but no back facing hits, treating convex objects as solids
+		//////////////////////////////////////////////////////////////////////////////////////////////////
+		TestFunction TestShapeRayMultiHitIgnoreBackFace = [inShape](const RayCast &inRay, float inExpectedFraction1, float inExpectedFraction2)
+		{
+			// CastRay works relative to center of mass, so transform the ray
+			RayCast ray = inRay;
+			ray.mOrigin -= inShape->GetCenterOfMass();
+
+			// Ray cast settings
+			RayCastSettings settings;
+			settings.SetBackFaceMode(EBackFaceMode::IgnoreBackFaces);
+			settings.mTreatConvexAsSolid = true;
+
+			AllHitCollisionCollector<CastRayCollector> collector;
+			SubShapeIDCreator id_creator;
+			inShape->CastRay(ray, settings, id_creator, collector);
+
+			if (inExpectedFraction1 != FLT_MAX)
+			{
+				CHECK(collector.mHits.size() == 1);
+				CHECK_APPROX_EQUAL(collector.mHits[0].mFraction, inExpectedFraction1, 1.0e-5f);
+			}
+			else
+			{
+				CHECK(collector.mHits.empty());
+			}
+		};
+
+		// Test normal ray
+		TestRayHelperInternal(inHitA, inHitB, TestShapeRayMultiHitIgnoreBackFace);
+
+		// Test inverse ray
+		TestRayHelperInternal(inHitB, inHitA, TestShapeRayMultiHitIgnoreBackFace);
+
+
+		//////////////////////////////////////////////////////////////////////////////////////////////////
+		// Test function that directly tests against a shape allowing multiple hits and back facing hits, treating convex objects as solids
+		//////////////////////////////////////////////////////////////////////////////////////////////////
+		TestFunction TestShapeRayMultiHitWithBackFace = [inShape](const RayCast &inRay, float inExpectedFraction1, float inExpectedFraction2)
+		{
+			// CastRay works relative to center of mass, so transform the ray
+			RayCast ray = inRay;
+			ray.mOrigin -= inShape->GetCenterOfMass();
+
+			// Ray cast settings
+			RayCastSettings settings;
+			settings.SetBackFaceMode(EBackFaceMode::CollideWithBackFaces);
+			settings.mTreatConvexAsSolid = true;
+
+			AllHitCollisionCollector<CastRayCollector> collector;
+			SubShapeIDCreator id_creator;
+			inShape->CastRay(ray, settings, id_creator, collector);
+
+			if (inExpectedFraction1 != FLT_MAX)
+			{
+				CHECK(collector.mHits.size() >= 1);
+				CHECK_APPROX_EQUAL(collector.mHits[0].mFraction, inExpectedFraction1, 1.0e-5f);
+			}
+			else
+			{
+				JPH_ASSERT(inExpectedFraction2 == FLT_MAX);
+				CHECK(collector.mHits.empty());
+			}
+
+			if (inExpectedFraction2 != FLT_MAX)
+			{
+				CHECK(collector.mHits.size() >= 2);
+				CHECK_APPROX_EQUAL(collector.mHits[1].mFraction, inExpectedFraction2, 1.0e-5f);
+			}
+			else
+			{
+				CHECK(collector.mHits.size() < 2);
+			}
+		};
+
+		// Test normal ray
+		TestRayHelperInternal(inHitA, inHitB, TestShapeRayMultiHitWithBackFace);
+
+		// Test inverse ray
+		TestRayHelperInternal(inHitB, inHitA, TestShapeRayMultiHitWithBackFace);
+
+
+		//////////////////////////////////////////////////////////////////////////////////////////////////
+		// Test function that directly tests against a shape allowing multiple hits but no back facing hits, treating convex object as non-solids
+		//////////////////////////////////////////////////////////////////////////////////////////////////
+		TestFunction TestShapeRayMultiHitIgnoreBackFaceNonSolid = [inShape](const RayCast &inRay, float inExpectedFraction1, float inExpectedFraction2)
+		{
+			// CastRay works relative to center of mass, so transform the ray
+			RayCast ray = inRay;
+			ray.mOrigin -= inShape->GetCenterOfMass();
+
+			// Ray cast settings
+			RayCastSettings settings;
+			settings.SetBackFaceMode(EBackFaceMode::IgnoreBackFaces);
+			settings.mTreatConvexAsSolid = false;
+
+			AllHitCollisionCollector<CastRayCollector> collector;
+			SubShapeIDCreator id_creator;
+			inShape->CastRay(ray, settings, id_creator, collector);
+
+			// A fraction of 0 means that the ray starts in solid, we treat this as a non-hit
+			if (inExpectedFraction1 != 0.0f && inExpectedFraction1 != FLT_MAX)
+			{
+				CHECK(collector.mHits.size() == 1);
+				CHECK_APPROX_EQUAL(collector.mHits[0].mFraction, inExpectedFraction1, 1.0e-5f);
+			}
+			else
+			{
+				CHECK(collector.mHits.empty());
+			}
+		};
+
+		// Test normal ray
+		TestRayHelperInternal(inHitA, inHitB, TestShapeRayMultiHitIgnoreBackFaceNonSolid);
+
+		// Test inverse ray
+		TestRayHelperInternal(inHitB, inHitA, TestShapeRayMultiHitIgnoreBackFaceNonSolid);
+
+
+		//////////////////////////////////////////////////////////////////////////////////////////////////
+		// Test function that directly tests against a shape allowing multiple hits and back facing hits, treating convex object as non-solids
+		//////////////////////////////////////////////////////////////////////////////////////////////////
+		TestFunction TestShapeRayMultiHitWithBackFaceNonSolid = [inShape](const RayCast &inRay, float inExpectedFraction1, float inExpectedFraction2)
+		{
+			// CastRay works relative to center of mass, so transform the ray
+			RayCast ray = inRay;
+			ray.mOrigin -= inShape->GetCenterOfMass();
+
+			// Ray cast settings
+			RayCastSettings settings;
+			settings.SetBackFaceMode(EBackFaceMode::CollideWithBackFaces);
+			settings.mTreatConvexAsSolid = false;
+
+			AllHitCollisionCollector<CastRayCollector> collector;
+			SubShapeIDCreator id_creator;
+			inShape->CastRay(ray, settings, id_creator, collector);
+
+			// A fraction of 0 means that the ray starts in solid, we treat this as a non-hit
+			if (inExpectedFraction1 == 0.0f)
+			{
+				inExpectedFraction1 = inExpectedFraction2;
+				inExpectedFraction2 = FLT_MAX;
+			}
+
+			if (inExpectedFraction1 != FLT_MAX)
+			{
+				CHECK(collector.mHits.size() >= 1);
+				CHECK_APPROX_EQUAL(collector.mHits[0].mFraction, inExpectedFraction1, 1.0e-5f);
+			}
+			else
+			{
+				JPH_ASSERT(inExpectedFraction2 == FLT_MAX);
+				CHECK(collector.mHits.empty());
+			}
+
+			if (inExpectedFraction2 != FLT_MAX)
+			{
+				CHECK(collector.mHits.size() >= 2);
+				CHECK_APPROX_EQUAL(collector.mHits[1].mFraction, inExpectedFraction2, 1.0e-5f);
+			}
+			else
+			{
+				CHECK(collector.mHits.size() < 2);
+			}
+		};
+
+		// Test normal ray
+		TestRayHelperInternal(inHitA, inHitB, TestShapeRayMultiHitWithBackFaceNonSolid);
+
+		// Test inverse ray
+		TestRayHelperInternal(inHitB, inHitA, TestShapeRayMultiHitWithBackFaceNonSolid);
+
+
+		//////////////////////////////////////////////////////////////////////////////////////////////////
+		// Insert the shape into the world
+		//////////////////////////////////////////////////////////////////////////////////////////////////
+
+		// A non-zero test position for the shape
+		const Vec3 cShapePosition(2, 3, 4);
+		const Quat cShapeRotation = Quat::sRotation(Vec3::sAxisX(), 0.25f * JPH_PI);
+		const Mat44 cShapeMatrix = Mat44::sRotationTranslation(cShapeRotation, cShapePosition);
+
+		// Make the shape part of a body and insert it into the physics system
+		BPLayerInterfaceImpl broad_phase_layer_interface;
+		ObjectVsBroadPhaseLayerFilter object_vs_broadphase_layer_filter;
+		ObjectLayerPairFilter object_vs_object_layer_filter;
+		PhysicsSystem system;
+		system.Init(1, 0, 4, 4, broad_phase_layer_interface, object_vs_broadphase_layer_filter, object_vs_object_layer_filter);
+		system.GetBodyInterface().CreateAndAddBody(BodyCreationSettings(inShape, RVec3(cShapePosition), cShapeRotation, EMotionType::Static, 0), EActivation::DontActivate);
+
+
+		//////////////////////////////////////////////////////////////////////////////////////////////////
+		// Test a ray against a shape through a physics system
+		//////////////////////////////////////////////////////////////////////////////////////////////////
+		TestFunction TestSystemRay = [&system, cShapeMatrix](const RayCast &inRay, float inExpectedFraction1, float inExpectedFraction2)
+		{
+			// inRay is relative to shape, transform it into world space
+			RayCast ray = inRay.Transformed(cShapeMatrix);
+
+			RayCastResult hit;
+			if (inExpectedFraction1 != FLT_MAX)
+			{
+				CHECK(system.GetNarrowPhaseQuery().CastRay(RRayCast(ray), hit));
+				CHECK_APPROX_EQUAL(hit.mFraction, inExpectedFraction1, 2.5e-5f);
+			}
+			else
+			{
+				CHECK_FALSE(system.GetNarrowPhaseQuery().CastRay(RRayCast(ray), hit));
+			}
+		};
+
+		// Test normal ray
+		TestRayHelperInternal(inHitA, inHitB, TestSystemRay);
+
+		// Test inverse ray
+		TestRayHelperInternal(inHitB, inHitA, TestSystemRay);
+
+
+		//////////////////////////////////////////////////////////////////////////////////////////////////
+		// Test a ray against a shape through a physics system allowing multiple hits but no back facing hits
+		//////////////////////////////////////////////////////////////////////////////////////////////////
+		TestFunction TestSystemRayMultiHitIgnoreBackFace = [&system, cShapeMatrix](const RayCast &inRay, float inExpectedFraction1, float inExpectedFraction2)
+		{
+			// inRay is relative to shape, transform it into world space
+			RayCast ray = inRay.Transformed(cShapeMatrix);
+
+			// Ray cast settings
+			RayCastSettings settings;
+			settings.SetBackFaceMode(EBackFaceMode::IgnoreBackFaces);
+			settings.mTreatConvexAsSolid = true;
+
+			AllHitCollisionCollector<CastRayCollector> collector;
+			system.GetNarrowPhaseQuery().CastRay(RRayCast(ray), settings, collector);
+
+			if (inExpectedFraction1 != FLT_MAX)
+			{
+				CHECK(collector.mHits.size() == 1);
+				CHECK_APPROX_EQUAL(collector.mHits[0].mFraction, inExpectedFraction1, 2.5e-5f);
+			}
+			else
+			{
+				CHECK(collector.mHits.empty());
+			}
+		};
+
+		// Test normal ray
+		TestRayHelperInternal(inHitA, inHitB, TestSystemRayMultiHitIgnoreBackFace);
+
+		// Test inverse ray
+		TestRayHelperInternal(inHitB, inHitA, TestSystemRayMultiHitIgnoreBackFace);
+
+
+		//////////////////////////////////////////////////////////////////////////////////////////////////
+		// Test a ray against a shape through a physics system allowing multiple hits and back facing hits
+		//////////////////////////////////////////////////////////////////////////////////////////////////
+		TestFunction TestSystemRayMultiHitWithBackFace = [&system, cShapeMatrix](const RayCast &inRay, float inExpectedFraction1, float inExpectedFraction2)
+		{
+			// inRay is relative to shape, transform it into world space
+			RayCast ray = inRay.Transformed(cShapeMatrix);
+
+			// Ray cast settings
+			RayCastSettings settings;
+			settings.SetBackFaceMode(EBackFaceMode::CollideWithBackFaces);
+			settings.mTreatConvexAsSolid = true;
+
+			AllHitCollisionCollector<CastRayCollector> collector;
+			system.GetNarrowPhaseQuery().CastRay(RRayCast(ray), settings, collector);
+			collector.Sort();
+
+			if (inExpectedFraction1 != FLT_MAX)
+			{
+				CHECK(collector.mHits.size() >= 1);
+				CHECK_APPROX_EQUAL(collector.mHits[0].mFraction, inExpectedFraction1, 2.5e-5f);
+			}
+			else
+			{
+				JPH_ASSERT(inExpectedFraction2 == FLT_MAX);
+				CHECK(collector.mHits.empty());
+			}
+
+			if (inExpectedFraction2 != FLT_MAX)
+			{
+				CHECK(collector.mHits.size() >= 2);
+				CHECK_APPROX_EQUAL(collector.mHits[1].mFraction, inExpectedFraction2, 2.5e-5f);
+			}
+			else
+			{
+				CHECK(collector.mHits.size() < 2);
+			}
+		};
+
+		// Test normal ray
+		TestRayHelperInternal(inHitA, inHitB, TestSystemRayMultiHitWithBackFace);
+
+		// Test inverse ray
+		TestRayHelperInternal(inHitB, inHitA, TestSystemRayMultiHitWithBackFace);
+	}
+
+	/// Helper function to check that a ray misses a shape
+	static void TestRayMiss(const Shape *inShape, Vec3Arg inOrigin, Vec3Arg inDirection)
+	{
+		RayCastResult hit;
+		CHECK(!inShape->CastRay({ inOrigin - inShape->GetCenterOfMass(), inDirection }, SubShapeIDCreator(), hit));
+	}
+
+	TEST_CASE("TestBoxShapeRay")
+	{
+		// Create box shape
+		BoxShape box(Vec3(2, 3, 4)); // Allocate on the stack to test embedded refcounted structs
+		box.SetEmbedded();
+		Ref<Shape> shape = &box; // Add a reference to see if we don't hit free() of a stack allocated struct
+
+		TestRayHelper(shape, Vec3(-2, 0, 0), Vec3(2, 0, 0));
+		TestRayHelper(shape, Vec3(0, -3, 0), Vec3(0, 3, 0));
+		TestRayHelper(shape, Vec3(0, 0, -4), Vec3(0, 0, 4));
+	}
+
+	TEST_CASE("TestSphereShapeRay")
+	{
+		// Create sphere shape
+		Ref<Shape> shape = new SphereShape(2);
+
+		TestRayHelper(shape, Vec3(-2, 0, 0), Vec3(2, 0, 0));
+		TestRayHelper(shape, Vec3(0, -2, 0), Vec3(0, 2, 0));
+		TestRayHelper(shape, Vec3(0, 0, -2), Vec3(0, 0, 2));
+	}
+
+	TEST_CASE("TestConvexHullShapeRay")
+	{
+		// Create convex hull shape of a box (off center so the center of mass is not zero)
+		Array<Vec3> box;
+		box.push_back(Vec3(-2, -4, -6));
+		box.push_back(Vec3(-2, -4, 7));
+		box.push_back(Vec3(-2, 5, -6));
+		box.push_back(Vec3(-2, 5, 7));
+		box.push_back(Vec3(3, -4, -6));
+		box.push_back(Vec3(3, -4, 7));
+		box.push_back(Vec3(3, 5, -6));
+		box.push_back(Vec3(3, 5, 7));
+		RefConst<Shape> shape = ConvexHullShapeSettings(box).Create().Get();
+
+		TestRayHelper(shape, Vec3(-2, 0, 0), Vec3(3, 0, 0));
+		TestRayHelper(shape, Vec3(0, -4, 0), Vec3(0, 5, 0));
+		TestRayHelper(shape, Vec3(0, 0, -6), Vec3(0, 0, 7));
+
+		TestRayMiss(shape, Vec3(-3, -5, 0), Vec3(0, 1, 0));
+		TestRayMiss(shape, Vec3(-3, 0, 0), Vec3(0, 1, 0));
+		TestRayMiss(shape, Vec3(-3, 6, 0), Vec3(0, 1, 0));
+	}
+
+	TEST_CASE("TestCapsuleShapeRay")
+	{
+		// Create capsule shape
+		Ref<Shape> shape = new CapsuleShape(4, 2);
+
+		TestRayHelper(shape, Vec3(-2, 0, 0), Vec3(2, 0, 0));
+		TestRayHelper(shape, Vec3(0, -6, 0), Vec3(0, 6, 0));
+		TestRayHelper(shape, Vec3(0, 0, -2), Vec3(0, 0, 2));
+	}
+
+	TEST_CASE("TestTaperedCapsuleShapeRay")
+	{
+		// Create tapered capsule shape
+		RefConst<Shape> shape = TaperedCapsuleShapeSettings(3, 4, 2).Create().Get();
+
+		TestRayHelper(shape, Vec3(0, 7, 0), Vec3(0, -5, 0)); // Top to bottom
+		TestRayHelper(shape, Vec3(-4, 3, 0), Vec3(4, 3, 0)); // Top sphere
+		TestRayHelper(shape, Vec3(0, 3, -4), Vec3(0, 3, 4)); // Top sphere
+	}
+
+	TEST_CASE("TestCylinderShapeRay")
+	{
+		// Create cylinder shape
+		Ref<Shape> shape = new CylinderShape(4, 2);
+
+		TestRayHelper(shape, Vec3(-2, 0, 0), Vec3(2, 0, 0));
+		TestRayHelper(shape, Vec3(0, -4, 0), Vec3(0, 4, 0));
+		TestRayHelper(shape, Vec3(0, 0, -2), Vec3(0, 0, 2));
+	}
+
+	TEST_CASE("TestTaperedCylinderShapeRay")
+	{
+		// Create tapered cylinder shape
+		Ref<Shape> shape = TaperedCylinderShapeSettings(4, 1, 3).Create().Get();
+
+		// Ray through origin
+		TestRayHelper(shape, Vec3(-2, 0, 0), Vec3(2, 0, 0));
+		TestRayHelper(shape, Vec3(0, -4, 0), Vec3(0, 4, 0));
+		TestRayHelper(shape, Vec3(0, 0, -2), Vec3(0, 0, 2));
+
+		// Ray halfway to the top
+		TestRayHelper(shape, Vec3(-1.5f, 2, 0), Vec3(1.5f, 2, 0));
+		TestRayHelper(shape, Vec3(0, 2, -1.5f), Vec3(0, 2, 1.5f));
+
+		// Ray halfway to the bottom
+		TestRayHelper(shape, Vec3(-2.5f, -2, 0), Vec3(2.5f, -2, 0));
+		TestRayHelper(shape, Vec3(0, -2, -2.5f), Vec3(0, -2, 2.5f));
+	}
+
+	TEST_CASE("TestScaledShapeRay")
+	{
+		// Create convex hull shape of a box (off center so the center of mass is not zero)
+		Array<Vec3> box;
+		box.push_back(Vec3(-2, -4, -6));
+		box.push_back(Vec3(-2, -4, 7));
+		box.push_back(Vec3(-2, 5, -6));
+		box.push_back(Vec3(-2, 5, 7));
+		box.push_back(Vec3(3, -4, -6));
+		box.push_back(Vec3(3, -4, 7));
+		box.push_back(Vec3(3, 5, -6));
+		box.push_back(Vec3(3, 5, 7));
+		RefConst<Shape> hull = ConvexHullShapeSettings(box).Create().Get();
+
+		// Scale the hull
+		Ref<Shape> shape1 = new ScaledShape(hull, Vec3(2, 3, 4));
+
+		TestRayHelper(shape1, Vec3(-4, 0, 0), Vec3(6, 0, 0));
+		TestRayHelper(shape1, Vec3(0, -12, 0), Vec3(0, 15, 0));
+		TestRayHelper(shape1, Vec3(0, 0, -24), Vec3(0, 0, 28));
+
+		// Scale the hull (and flip it inside out)
+		Ref<Shape> shape2 = new ScaledShape(hull, Vec3(-2, 3, 4));
+
+		TestRayHelper(shape2, Vec3(-6, 0, 0), Vec3(4, 0, 0));
+		TestRayHelper(shape2, Vec3(0, -12, 0), Vec3(0, 15, 0));
+		TestRayHelper(shape2, Vec3(0, 0, -24), Vec3(0, 0, 28));
+	}
+
+	TEST_CASE("TestStaticCompoundShapeRay")
+	{
+		// Create convex hull shape of a box (off center so the center of mass is not zero)
+		Array<Vec3> box;
+		box.push_back(Vec3(-2, -4, -6));
+		box.push_back(Vec3(-2, -4, 7));
+		box.push_back(Vec3(-2, 5, -6));
+		box.push_back(Vec3(-2, 5, 7));
+		box.push_back(Vec3(3, -4, -6));
+		box.push_back(Vec3(3, -4, 7));
+		box.push_back(Vec3(3, 5, -6));
+		box.push_back(Vec3(3, 5, 7));
+		RefConst<ShapeSettings> hull = new ConvexHullShapeSettings(box);
+
+		// Translate/rotate the shape through a compound (off center to force center of mass not zero)
+		const Vec3 cShape1Position(10, 20, 30);
+		const Quat cShape1Rotation = Quat::sRotation(Vec3::sAxisX(), 0.1f * JPH_PI) * Quat::sRotation(Vec3::sAxisY(), 0.2f * JPH_PI);
+		const Vec3 cShape2Position(40, 50, 60);
+		const Quat cShape2Rotation = Quat::sRotation(Vec3::sAxisZ(), 0.3f * JPH_PI);
+
+		StaticCompoundShapeSettings compound_settings;
+		compound_settings.AddShape(cShape1Position, cShape1Rotation, hull); // Shape 1
+		compound_settings.AddShape(cShape2Position, cShape2Rotation, hull); // Shape 2
+		RefConst<Shape> compound = compound_settings.Create().Get();
+
+		// Hitting shape 1
+		TestRayHelper(compound, cShape1Position + cShape1Rotation * Vec3(-2, 0, 0), cShape1Position + cShape1Rotation * Vec3(3, 0, 0));
+		TestRayHelper(compound, cShape1Position + cShape1Rotation * Vec3(0, -4, 0), cShape1Position + cShape1Rotation * Vec3(0, 5, 0));
+		TestRayHelper(compound, cShape1Position + cShape1Rotation * Vec3(0, 0, -6), cShape1Position + cShape1Rotation * Vec3(0, 0, 7));
+
+		// Hitting shape 2
+		TestRayHelper(compound, cShape2Position + cShape2Rotation * Vec3(-2, 0, 0), cShape2Position + cShape2Rotation * Vec3(3, 0, 0));
+		TestRayHelper(compound, cShape2Position + cShape2Rotation * Vec3(0, -4, 0), cShape2Position + cShape2Rotation * Vec3(0, 5, 0));
+		TestRayHelper(compound, cShape2Position + cShape2Rotation * Vec3(0, 0, -6), cShape2Position + cShape2Rotation * Vec3(0, 0, 7));
+	}
+
+	TEST_CASE("TestMutableCompoundShapeRay")
+	{
+		// Create convex hull shape of a box (off center so the center of mass is not zero)
+		Array<Vec3> box;
+		box.push_back(Vec3(-2, -4, -6));
+		box.push_back(Vec3(-2, -4, 7));
+		box.push_back(Vec3(-2, 5, -6));
+		box.push_back(Vec3(-2, 5, 7));
+		box.push_back(Vec3(3, -4, -6));
+		box.push_back(Vec3(3, -4, 7));
+		box.push_back(Vec3(3, 5, -6));
+		box.push_back(Vec3(3, 5, 7));
+		RefConst<ShapeSettings> hull = new ConvexHullShapeSettings(box);
+
+		// Translate/rotate the shape through a compound (off center to force center of mass not zero)
+		const Vec3 cShape1Position(10, 20, 30);
+		const Quat cShape1Rotation = Quat::sRotation(Vec3::sAxisX(), 0.1f * JPH_PI) * Quat::sRotation(Vec3::sAxisY(), 0.2f * JPH_PI);
+		const Vec3 cShape2Position(40, 50, 60);
+		const Quat cShape2Rotation = Quat::sRotation(Vec3::sAxisZ(), 0.3f * JPH_PI);
+
+		MutableCompoundShapeSettings compound_settings;
+		compound_settings.AddShape(cShape1Position, cShape1Rotation, hull); // Shape 1
+		compound_settings.AddShape(cShape2Position, cShape2Rotation, hull); // Shape 2
+		RefConst<Shape> compound = compound_settings.Create().Get();
+
+		// Hitting shape 1
+		TestRayHelper(compound, cShape1Position + cShape1Rotation * Vec3(-2, 0, 0), cShape1Position + cShape1Rotation * Vec3(3, 0, 0));
+		TestRayHelper(compound, cShape1Position + cShape1Rotation * Vec3(0, -4, 0), cShape1Position + cShape1Rotation * Vec3(0, 5, 0));
+		TestRayHelper(compound, cShape1Position + cShape1Rotation * Vec3(0, 0, -6), cShape1Position + cShape1Rotation * Vec3(0, 0, 7));
+
+		// Hitting shape 2
+		TestRayHelper(compound, cShape2Position + cShape2Rotation * Vec3(-2, 0, 0), cShape2Position + cShape2Rotation * Vec3(3, 0, 0));
+		TestRayHelper(compound, cShape2Position + cShape2Rotation * Vec3(0, -4, 0), cShape2Position + cShape2Rotation * Vec3(0, 5, 0));
+		TestRayHelper(compound, cShape2Position + cShape2Rotation * Vec3(0, 0, -6), cShape2Position + cShape2Rotation * Vec3(0, 0, 7));
+	}
+}

lib/All/JoltPhysics/UnitTests/Physics/SensorTests.cpp

@@ -0,0 +1,764 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include "PhysicsTestContext.h"
+#include "Layers.h"
+#include "LoggingContactListener.h"
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Collision/Shape/MeshShape.h>
+#include <Jolt/Physics/Collision/Shape/StaticCompoundShape.h>
+#include <Jolt/Physics/Collision/CollideShape.h>
+#include <Jolt/Physics/Collision/CollideShapeVsShapePerLeaf.h>
+#include <Jolt/Physics/Collision/CollisionCollectorImpl.h>
+
+TEST_SUITE("SensorTests")
+{
+	using LogEntry = LoggingContactListener::LogEntry;
+	using EType = LoggingContactListener::EType;
+
+	TEST_CASE("TestDynamicVsSensor")
+	{
+		PhysicsTestContext c;
+		c.ZeroGravity();
+
+		// Register listener
+		LoggingContactListener listener;
+		c.GetSystem()->SetContactListener(&listener);
+
+		// Sensor
+		BodyCreationSettings sensor_settings(new BoxShape(Vec3::sReplicate(1)), RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, Layers::SENSOR);
+		sensor_settings.mIsSensor = true;
+		BodyID sensor_id = c.GetBodyInterface().CreateAndAddBody(sensor_settings, EActivation::DontActivate);
+
+		// Dynamic body moving downwards
+		Body &dynamic = c.CreateBox(RVec3(0, 2, 0), Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3::sReplicate(0.5f));
+		dynamic.SetLinearVelocity(Vec3(0, -1, 0));
+
+		// After a single step the dynamic object should not have touched the sensor yet
+		c.SimulateSingleStep();
+		CHECK(listener.GetEntryCount() == 0);
+
+		// After half a second and one step we should be touching the sensor
+		c.Simulate(0.5f + c.GetStepDeltaTime());
+		CHECK(listener.Contains(EType::Add, dynamic.GetID(), sensor_id));
+		listener.Clear();
+
+		// The next step we require that the contact persists
+		c.SimulateSingleStep();
+		CHECK(listener.Contains(EType::Persist, dynamic.GetID(), sensor_id));
+		CHECK(!listener.Contains(EType::Remove, dynamic.GetID(), sensor_id));
+		listener.Clear();
+
+		// After 3 more seconds we should have left the sensor at the bottom side
+		c.Simulate(3.0f);
+		CHECK(listener.Contains(EType::Remove, dynamic.GetID(), sensor_id));
+		CHECK_APPROX_EQUAL(dynamic.GetPosition(), RVec3(0, -1.5f - 3.0f * c.GetDeltaTime(), 0), 1.0e-4f);
+	}
+
+	TEST_CASE("TestKinematicVsSensor")
+	{
+		PhysicsTestContext c;
+
+		// Register listener
+		LoggingContactListener listener;
+		c.GetSystem()->SetContactListener(&listener);
+
+		// Sensor
+		BodyCreationSettings sensor_settings(new BoxShape(Vec3::sReplicate(1)), RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, Layers::SENSOR);
+		sensor_settings.mIsSensor = true;
+		BodyID sensor_id = c.GetBodyInterface().CreateAndAddBody(sensor_settings, EActivation::DontActivate);
+
+		// Kinematic body moving downwards
+		Body &kinematic = c.CreateBox(RVec3(0, 2, 0), Quat::sIdentity(), EMotionType::Kinematic, EMotionQuality::Discrete, Layers::MOVING, Vec3::sReplicate(0.5f));
+		kinematic.SetLinearVelocity(Vec3(0, -1, 0));
+
+		// After a single step the kinematic object should not have touched the sensor yet
+		c.SimulateSingleStep();
+		CHECK(listener.GetEntryCount() == 0);
+
+		// After half a second and one step we should be touching the sensor
+		c.Simulate(0.5f + c.GetStepDeltaTime());
+		CHECK(listener.Contains(EType::Add, kinematic.GetID(), sensor_id));
+		listener.Clear();
+
+		// The next step we require that the contact persists
+		c.SimulateSingleStep();
+		CHECK(listener.Contains(EType::Persist, kinematic.GetID(), sensor_id));
+		CHECK(!listener.Contains(EType::Remove, kinematic.GetID(), sensor_id));
+		listener.Clear();
+
+		// After 3 more seconds we should have left the sensor at the bottom side
+		c.Simulate(3.0f);
+		CHECK(listener.Contains(EType::Remove, kinematic.GetID(), sensor_id));
+		CHECK_APPROX_EQUAL(kinematic.GetPosition(), RVec3(0, -1.5f - 3.0f * c.GetDeltaTime(), 0), 1.0e-4f);
+	}
+
+	TEST_CASE("TestKinematicVsKinematicSensor")
+	{
+		// Same as TestKinematicVsSensor but with the sensor being an active kinematic body
+
+		PhysicsTestContext c;
+
+		// Register listener
+		LoggingContactListener listener;
+		c.GetSystem()->SetContactListener(&listener);
+
+		// Kinematic sensor
+		BodyCreationSettings sensor_settings(new BoxShape(Vec3::sReplicate(1)), RVec3::sZero(), Quat::sIdentity(), EMotionType::Kinematic, Layers::SENSOR);
+		sensor_settings.mIsSensor = true;
+		BodyID sensor_id = c.GetBodyInterface().CreateAndAddBody(sensor_settings, EActivation::Activate);
+
+		// Kinematic body moving downwards
+		Body &kinematic = c.CreateBox(RVec3(0, 2, 0), Quat::sIdentity(), EMotionType::Kinematic, EMotionQuality::Discrete, Layers::MOVING, Vec3::sReplicate(0.5f));
+		kinematic.SetLinearVelocity(Vec3(0, -1, 0));
+
+		// After a single step the kinematic object should not have touched the sensor yet
+		c.SimulateSingleStep();
+		CHECK(listener.GetEntryCount() == 0);
+
+		// After half a second and one step we should be touching the sensor
+		c.Simulate(0.5f + c.GetStepDeltaTime());
+		CHECK(listener.Contains(EType::Add, kinematic.GetID(), sensor_id));
+		listener.Clear();
+
+		// The next step we require that the contact persists
+		c.SimulateSingleStep();
+		CHECK(listener.Contains(EType::Persist, kinematic.GetID(), sensor_id));
+		CHECK(!listener.Contains(EType::Remove, kinematic.GetID(), sensor_id));
+		listener.Clear();
+
+		// After 3 more seconds we should have left the sensor at the bottom side
+		c.Simulate(3.0f);
+		CHECK(listener.Contains(EType::Remove, kinematic.GetID(), sensor_id));
+		CHECK_APPROX_EQUAL(kinematic.GetPosition(), RVec3(0, -1.5f - 3.0f * c.GetDeltaTime(), 0), 1.0e-4f);
+	}
+
+	TEST_CASE("TestKinematicVsKinematicSensorReversed")
+	{
+		// Same as TestKinematicVsKinematicSensor but with bodies created in reverse order (this matters for Body::sFindCollidingPairsCanCollide because MotionProperties::mIndexInActiveBodies is swapped between the bodies)
+
+		PhysicsTestContext c;
+
+		// Register listener
+		LoggingContactListener listener;
+		c.GetSystem()->SetContactListener(&listener);
+
+		// Kinematic body moving downwards
+		Body &kinematic = c.CreateBox(RVec3(0, 2, 0), Quat::sIdentity(), EMotionType::Kinematic, EMotionQuality::Discrete, Layers::MOVING, Vec3::sReplicate(0.5f));
+		kinematic.SetLinearVelocity(Vec3(0, -1, 0));
+
+		// Kinematic sensor
+		BodyCreationSettings sensor_settings(new BoxShape(Vec3::sReplicate(1)), RVec3::sZero(), Quat::sIdentity(), EMotionType::Kinematic, Layers::SENSOR);
+		sensor_settings.mIsSensor = true;
+		BodyID sensor_id = c.GetBodyInterface().CreateAndAddBody(sensor_settings, EActivation::Activate);
+
+		// After a single step the kinematic object should not have touched the sensor yet
+		c.SimulateSingleStep();
+		CHECK(listener.GetEntryCount() == 0);
+
+		// After half a second and one step we should be touching the sensor
+		c.Simulate(0.5f + c.GetStepDeltaTime());
+		CHECK(listener.Contains(EType::Add, kinematic.GetID(), sensor_id));
+		listener.Clear();
+
+		// The next step we require that the contact persists
+		c.SimulateSingleStep();
+		CHECK(listener.Contains(EType::Persist, kinematic.GetID(), sensor_id));
+		CHECK(!listener.Contains(EType::Remove, kinematic.GetID(), sensor_id));
+		listener.Clear();
+
+		// After 3 more seconds we should have left the sensor at the bottom side
+		c.Simulate(3.0f);
+		CHECK(listener.Contains(EType::Remove, kinematic.GetID(), sensor_id));
+		CHECK_APPROX_EQUAL(kinematic.GetPosition(), RVec3(0, -1.5f - 3.0f * c.GetDeltaTime(), 0), 1.0e-4f);
+	}
+
+	TEST_CASE("TestDynamicSleepingVsStaticSensor")
+	{
+		PhysicsTestContext c;
+
+		// Register listener
+		LoggingContactListener listener;
+		c.GetSystem()->SetContactListener(&listener);
+
+		// Sensor
+		BodyCreationSettings sensor_settings(new BoxShape(Vec3::sReplicate(1)), RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, Layers::SENSOR);
+		sensor_settings.mIsSensor = true;
+		Body &sensor = *c.GetBodyInterface().CreateBody(sensor_settings);
+		c.GetBodyInterface().AddBody(sensor.GetID(), EActivation::DontActivate);
+
+		// Floor
+		Body &floor = c.CreateFloor();
+
+		// Dynamic body on floor (make them penetrate)
+		Body &dynamic = c.CreateBox(RVec3(0, 0.5f - c.GetSystem()->GetPhysicsSettings().mMaxPenetrationDistance, 0), Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3::sReplicate(0.5f), EActivation::DontActivate);
+
+		// After a single step (because the object is sleeping) there should not be a contact
+		c.SimulateSingleStep();
+		CHECK(listener.GetEntryCount() == 0);
+
+		// The dynamic object should not be part of an island
+		CHECK(!sensor.IsActive());
+		CHECK(dynamic.GetMotionProperties()->GetIslandIndexInternal() == Body::cInactiveIndex);
+
+		// Activate the body
+		c.GetBodyInterface().ActivateBody(dynamic.GetID());
+
+		// After a single step we should have detected the collision with the floor and the sensor
+		c.SimulateSingleStep();
+		CHECK(listener.GetEntryCount() == 4);
+		CHECK(listener.Contains(EType::Validate, floor.GetID(), dynamic.GetID()));
+		CHECK(listener.Contains(EType::Add, floor.GetID(), dynamic.GetID()));
+		CHECK(listener.Contains(EType::Validate, sensor.GetID(), dynamic.GetID()));
+		CHECK(listener.Contains(EType::Add, sensor.GetID(), dynamic.GetID()));
+		listener.Clear();
+
+		// The dynamic object should be part of an island now
+		CHECK(!sensor.IsActive());
+		CHECK(dynamic.GetMotionProperties()->GetIslandIndexInternal() != Body::cInactiveIndex);
+
+		// After a second the body should have gone to sleep and the contacts should have been removed
+		c.Simulate(1.0f);
+		CHECK(!dynamic.IsActive());
+		CHECK(listener.Contains(EType::Remove, floor.GetID(), dynamic.GetID()));
+		CHECK(listener.Contains(EType::Remove, sensor.GetID(), dynamic.GetID()));
+
+		// The dynamic object should not be part of an island
+		CHECK(!sensor.IsActive());
+		CHECK(dynamic.GetMotionProperties()->GetIslandIndexInternal() == Body::cInactiveIndex);
+	}
+
+	TEST_CASE("TestDynamicSleepingVsKinematicSensor")
+	{
+		PhysicsTestContext c;
+
+		// Register listener
+		LoggingContactListener listener;
+		c.GetSystem()->SetContactListener(&listener);
+
+		// Kinematic sensor that is active (so will keep detecting contacts with sleeping bodies)
+		BodyCreationSettings sensor_settings(new BoxShape(Vec3::sReplicate(1)), RVec3::sZero(), Quat::sIdentity(), EMotionType::Kinematic, Layers::SENSOR);
+		sensor_settings.mIsSensor = true;
+		Body &sensor = *c.GetBodyInterface().CreateBody(sensor_settings);
+		c.GetBodyInterface().AddBody(sensor.GetID(), EActivation::Activate);
+
+		// Floor
+		Body &floor = c.CreateFloor();
+
+		// Dynamic body on floor (make them penetrate)
+		Body &dynamic = c.CreateBox(RVec3(0, 0.5f - c.GetSystem()->GetPhysicsSettings().mMaxPenetrationDistance, 0), Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3::sReplicate(0.5f), EActivation::DontActivate);
+
+		// After a single step, there should be a contact with the sensor only (the sensor is active)
+		c.SimulateSingleStep();
+		CHECK(listener.GetEntryCount() == 2);
+		CHECK(listener.Contains(EType::Validate, sensor.GetID(), dynamic.GetID()));
+		CHECK(listener.Contains(EType::Add, sensor.GetID(), dynamic.GetID()));
+		listener.Clear();
+
+		// The sensor should be in its own island
+		CHECK(sensor.GetMotionProperties()->GetIslandIndexInternal() != Body::cInactiveIndex);
+		CHECK(dynamic.GetMotionProperties()->GetIslandIndexInternal() == Body::cInactiveIndex);
+
+		// The second step, the contact with the sensor should have persisted
+		c.SimulateSingleStep();
+		CHECK(listener.GetEntryCount() == 1);
+		CHECK(listener.Contains(EType::Persist, sensor.GetID(), dynamic.GetID()));
+		listener.Clear();
+
+		// The sensor should still be in its own island
+		CHECK(sensor.GetMotionProperties()->GetIslandIndexInternal() != Body::cInactiveIndex);
+		CHECK(dynamic.GetMotionProperties()->GetIslandIndexInternal() == Body::cInactiveIndex);
+
+		// Activate the body
+		c.GetBodyInterface().ActivateBody(dynamic.GetID());
+
+		// After a single step we should have detected collision with the floor and the collision with the sensor should have persisted
+		c.SimulateSingleStep();
+		CHECK(listener.GetEntryCount() == 3);
+		CHECK(listener.Contains(EType::Validate, floor.GetID(), dynamic.GetID()));
+		CHECK(listener.Contains(EType::Add, floor.GetID(), dynamic.GetID()));
+		CHECK(listener.Contains(EType::Persist, sensor.GetID(), dynamic.GetID()));
+		listener.Clear();
+
+		// The sensor should not be part of the same island as the dynamic body (they won't interact, so this is not needed)
+		CHECK(sensor.GetMotionProperties()->GetIslandIndexInternal() != Body::cInactiveIndex);
+		CHECK(dynamic.GetMotionProperties()->GetIslandIndexInternal() != Body::cInactiveIndex);
+		CHECK(sensor.GetMotionProperties()->GetIslandIndexInternal() != dynamic.GetMotionProperties()->GetIslandIndexInternal());
+
+		// After another step we should have persisted the collision with the floor and sensor
+		c.SimulateSingleStep();
+		CHECK(listener.GetEntryCount() >= 2); // Depending on if we used the contact cache or not there will be validate callbacks too
+		CHECK(listener.Contains(EType::Persist, floor.GetID(), dynamic.GetID()));
+		CHECK(!listener.Contains(EType::Remove, floor.GetID(), dynamic.GetID()));
+		CHECK(listener.Contains(EType::Persist, sensor.GetID(), dynamic.GetID()));
+		CHECK(!listener.Contains(EType::Remove, sensor.GetID(), dynamic.GetID()));
+		listener.Clear();
+
+		// The same islands as the previous step should have been created
+		CHECK(sensor.GetMotionProperties()->GetIslandIndexInternal() != Body::cInactiveIndex);
+		CHECK(dynamic.GetMotionProperties()->GetIslandIndexInternal() != Body::cInactiveIndex);
+		CHECK(sensor.GetMotionProperties()->GetIslandIndexInternal() != dynamic.GetMotionProperties()->GetIslandIndexInternal());
+
+		// After a second the body should have gone to sleep and the contacts with the floor should have been removed, but not with the sensor
+		c.Simulate(1.0f);
+		CHECK(!dynamic.IsActive());
+		CHECK(listener.Contains(EType::Remove, floor.GetID(), dynamic.GetID()));
+		CHECK(!listener.Contains(EType::Remove, sensor.GetID(), dynamic.GetID()));
+	}
+
+	TEST_CASE("TestSensorVsSensor")
+	{
+		for (int test = 0; test < 2; ++test)
+		{
+			bool sensor_detects_sensor = test == 1;
+
+			PhysicsTestContext c;
+
+			// Register listener
+			LoggingContactListener listener;
+			c.GetSystem()->SetContactListener(&listener);
+
+			// Depending on the iteration we either place the sensor in the moving layer which means it will collide with other sensors
+			// or we put it in the sensor layer which means it won't collide with other sensors
+			ObjectLayer layer = sensor_detects_sensor? Layers::MOVING : Layers::SENSOR;
+
+			// Sensor 1
+			BodyCreationSettings sensor_settings(new BoxShape(Vec3::sReplicate(1)), RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, layer);
+			sensor_settings.mIsSensor = true;
+			BodyID sensor_id1 = c.GetBodyInterface().CreateAndAddBody(sensor_settings, EActivation::DontActivate);
+
+			// Sensor 2 moving downwards
+			sensor_settings.mMotionType = EMotionType::Kinematic;
+			sensor_settings.mPosition = RVec3(0, 3, 0);
+			sensor_settings.mIsSensor = true;
+			sensor_settings.mLinearVelocity = Vec3(0, -2, 0);
+			BodyID sensor_id2 = c.GetBodyInterface().CreateAndAddBody(sensor_settings, EActivation::Activate);
+
+			// After a single step the sensors should not touch yet
+			c.SimulateSingleStep();
+			CHECK(listener.GetEntryCount() == 0);
+
+			// After half a second and one step the sensors should be touching
+			c.Simulate(0.5f + c.GetDeltaTime());
+			if (sensor_detects_sensor)
+				CHECK(listener.Contains(EType::Add, sensor_id1, sensor_id2));
+			else
+				CHECK(listener.GetEntryCount() == 0);
+			listener.Clear();
+
+			// The next step we require that the contact persists
+			c.SimulateSingleStep();
+			if (sensor_detects_sensor)
+			{
+				CHECK(listener.Contains(EType::Persist, sensor_id1, sensor_id2));
+				CHECK(!listener.Contains(EType::Remove, sensor_id1, sensor_id2));
+			}
+			else
+				CHECK(listener.GetEntryCount() == 0);
+			listener.Clear();
+
+			// After 2 more seconds we should have left the sensor at the bottom side
+			c.Simulate(2.0f);
+			if (sensor_detects_sensor)
+				CHECK(listener.Contains(EType::Remove, sensor_id1, sensor_id2));
+			else
+				CHECK(listener.GetEntryCount() == 0);
+			CHECK_APPROX_EQUAL(c.GetBodyInterface().GetPosition(sensor_id2), sensor_settings.mPosition + sensor_settings.mLinearVelocity * (2.5f + 3.0f * c.GetDeltaTime()), 1.0e-4f);
+		}
+	}
+
+	TEST_CASE("TestContactListenerMakesSensor")
+	{
+		PhysicsTestContext c;
+		c.ZeroGravity();
+
+		class SensorOverridingListener : public LoggingContactListener
+		{
+		public:
+			virtual void		OnContactAdded(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings) override
+			{
+				LoggingContactListener::OnContactAdded(inBody1, inBody2, inManifold, ioSettings);
+
+				JPH_ASSERT(ioSettings.mIsSensor == false);
+				if (inBody1.GetID() == mBodyThatSeesSensorID || inBody2.GetID() == mBodyThatSeesSensorID)
+					ioSettings.mIsSensor = true;
+			}
+
+			virtual void		OnContactPersisted(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings) override
+			{
+				LoggingContactListener::OnContactPersisted(inBody1, inBody2, inManifold, ioSettings);
+
+				JPH_ASSERT(ioSettings.mIsSensor == false);
+				if (inBody1.GetID() == mBodyThatSeesSensorID || inBody2.GetID() == mBodyThatSeesSensorID)
+					ioSettings.mIsSensor = true;
+			}
+
+			BodyID				mBodyThatSeesSensorID;
+		};
+
+		// Register listener
+		SensorOverridingListener listener;
+		c.GetSystem()->SetContactListener(&listener);
+
+		// Body that will appear as a sensor to one object and as static to another
+		BodyID static_id = c.GetBodyInterface().CreateAndAddBody(BodyCreationSettings(new BoxShape(Vec3(5, 1, 5)), RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING), EActivation::DontActivate);
+
+		// Dynamic body moving down that will do a normal collision
+		Body &dynamic1 = c.CreateBox(RVec3(-2, 2, 0), Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3::sReplicate(0.5f));
+		dynamic1.SetAllowSleeping(false);
+		dynamic1.SetLinearVelocity(Vec3(0, -1, 0));
+
+		// Dynamic body moving down that will only see the static object as a sensor
+		Body &dynamic2 = c.CreateBox(RVec3(2, 2, 0), Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3::sReplicate(0.5f));
+		dynamic2.SetAllowSleeping(false);
+		dynamic2.SetLinearVelocity(Vec3(0, -1, 0));
+		listener.mBodyThatSeesSensorID = dynamic2.GetID();
+
+		// After a single step the dynamic object should not have touched the sensor yet
+		c.SimulateSingleStep();
+		CHECK(listener.GetEntryCount() == 0);
+
+		// After half a second both bodies should be touching the sensor
+		c.Simulate(0.5f);
+		CHECK(listener.Contains(EType::Add, dynamic1.GetID(), static_id));
+		CHECK(listener.Contains(EType::Add, dynamic2.GetID(), static_id));
+		listener.Clear();
+
+		// The next step we require that the contact persists
+		c.SimulateSingleStep();
+		CHECK(listener.Contains(EType::Persist, dynamic1.GetID(), static_id));
+		CHECK(!listener.Contains(EType::Remove, dynamic1.GetID(), static_id));
+		CHECK(listener.Contains(EType::Persist, dynamic2.GetID(), static_id));
+		CHECK(!listener.Contains(EType::Remove, dynamic2.GetID(), static_id));
+		listener.Clear();
+
+		// After 3 more seconds one body should be resting on the static body, the other should have fallen through
+		c.Simulate(3.0f + c.GetDeltaTime());
+		CHECK(listener.Contains(EType::Persist, dynamic1.GetID(), static_id));
+		CHECK(!listener.Contains(EType::Remove, dynamic1.GetID(), static_id));
+		CHECK(listener.Contains(EType::Remove, dynamic2.GetID(), static_id));
+		CHECK_APPROX_EQUAL(dynamic1.GetPosition(), RVec3(-2, 1.5f, 0), 5.0e-3f);
+		CHECK_APPROX_EQUAL(dynamic2.GetPosition(), RVec3(2, -1.5f - 3.0f * c.GetDeltaTime(), 0), 1.0e-4f);
+	}
+
+	TEST_CASE("TestContactListenerMakesSensorCCD")
+	{
+		PhysicsTestContext c;
+		c.ZeroGravity();
+
+		const float cPenetrationSlop = c.GetSystem()->GetPhysicsSettings().mPenetrationSlop;
+
+		class SensorOverridingListener : public LoggingContactListener
+		{
+		public:
+			virtual void		OnContactAdded(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings) override
+			{
+				LoggingContactListener::OnContactAdded(inBody1, inBody2, inManifold, ioSettings);
+
+				JPH_ASSERT(ioSettings.mIsSensor == false);
+				if (inBody1.GetID() == mBodyThatBecomesSensor || inBody2.GetID() == mBodyThatBecomesSensor)
+					ioSettings.mIsSensor = true;
+			}
+
+			virtual void		OnContactPersisted(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings) override
+			{
+				LoggingContactListener::OnContactPersisted(inBody1, inBody2, inManifold, ioSettings);
+
+				JPH_ASSERT(ioSettings.mIsSensor == false);
+				if (inBody1.GetID() == mBodyThatBecomesSensor || inBody2.GetID() == mBodyThatBecomesSensor)
+					ioSettings.mIsSensor = true;
+			}
+
+			BodyID				mBodyThatBecomesSensor;
+		};
+
+		// Register listener
+		SensorOverridingListener listener;
+		c.GetSystem()->SetContactListener(&listener);
+
+		// Body that blocks the path
+		BodyID static_id = c.GetBodyInterface().CreateAndAddBody(BodyCreationSettings(new BoxShape(Vec3(0.1f, 10, 10)), RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING), EActivation::DontActivate);
+
+		// Dynamic body moving to the static object that will do a normal CCD collision
+		RVec3 dynamic1_pos(-0.5f, 2, 0);
+		Vec3 initial_velocity(500, 0, 0);
+		Body &dynamic1 = c.CreateBox(dynamic1_pos, Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::LinearCast, Layers::MOVING, Vec3::sReplicate(0.1f));
+		dynamic1.SetAllowSleeping(false);
+		dynamic1.SetLinearVelocity(initial_velocity);
+		dynamic1.SetRestitution(1.0f);
+
+		// Dynamic body moving through the static object that will become a sensor an thus pass through
+		RVec3 dynamic2_pos(-0.5f, -2, 0);
+		Body &dynamic2 = c.CreateBox(dynamic2_pos, Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::LinearCast, Layers::MOVING, Vec3::sReplicate(0.1f));
+		dynamic2.SetAllowSleeping(false);
+		dynamic2.SetLinearVelocity(initial_velocity);
+		dynamic2.SetRestitution(1.0f);
+		listener.mBodyThatBecomesSensor = dynamic2.GetID();
+
+		// After a single step the we should have contact added callbacks for both bodies
+		c.SimulateSingleStep();
+		CHECK(listener.Contains(EType::Add, dynamic1.GetID(), static_id));
+		CHECK(listener.Contains(EType::Add, dynamic2.GetID(), static_id));
+		listener.Clear();
+		CHECK_APPROX_EQUAL(dynamic1.GetPosition(), dynamic1_pos + RVec3(0.3f + cPenetrationSlop, 0, 0), 1.0e-4f); // Dynamic 1 should have moved to the surface of the static body
+		CHECK_APPROX_EQUAL(dynamic2.GetPosition(), dynamic2_pos + initial_velocity * c.GetDeltaTime(), 1.0e-4f); // Dynamic 2 should have passed through the static body because it became a sensor
+
+		// The next step the sensor should have its contact removed and the CCD body should have its contact persisted because it starts penetrating
+		c.SimulateSingleStep();
+		CHECK(listener.Contains(EType::Persist, dynamic1.GetID(), static_id));
+		CHECK(listener.Contains(EType::Remove, dynamic2.GetID(), static_id));
+		listener.Clear();
+
+		// The next step all contacts have been removed
+		c.SimulateSingleStep();
+		CHECK(listener.Contains(EType::Remove, dynamic1.GetID(), static_id));
+		listener.Clear();
+	}
+
+	TEST_CASE("TestSensorVsSubShapes")
+	{
+		PhysicsTestContext c;
+		BodyInterface &bi = c.GetBodyInterface();
+
+		// Register listener
+		LoggingContactListener listener;
+		c.GetSystem()->SetContactListener(&listener);
+
+		// Create sensor
+		BodyCreationSettings sensor_settings(new BoxShape(Vec3::sReplicate(5.0f)), RVec3(0, 10, 0), Quat::sIdentity(), EMotionType::Static, Layers::SENSOR);
+		sensor_settings.mIsSensor = true;
+		BodyID sensor_id = bi.CreateAndAddBody(sensor_settings, EActivation::DontActivate);
+
+		// We will be testing if we receive callbacks from the individual sub shapes
+		enum class EUserData
+		{
+			Bottom,
+			Middle,
+			Top,
+		};
+
+		// Create compound with 3 sub shapes
+		Ref<StaticCompoundShapeSettings> shape_settings = new StaticCompoundShapeSettings();
+		Ref<BoxShapeSettings> shape1 = new BoxShapeSettings(Vec3::sReplicate(0.4f));
+		shape1->mUserData = (uint64)EUserData::Bottom;
+		Ref<BoxShapeSettings> shape2 = new BoxShapeSettings(Vec3::sReplicate(0.4f));
+		shape2->mUserData = (uint64)EUserData::Middle;
+		Ref<BoxShapeSettings> shape3 = new BoxShapeSettings(Vec3::sReplicate(0.4f));
+		shape3->mUserData = (uint64)EUserData::Top;
+		shape_settings->AddShape(Vec3(0, -1.0f, 0), Quat::sIdentity(), shape1);
+		shape_settings->AddShape(Vec3(0, 0.0f, 0), Quat::sIdentity(), shape2);
+		shape_settings->AddShape(Vec3(0, 1.0f, 0), Quat::sIdentity(), shape3);
+		BodyCreationSettings compound_body_settings(shape_settings, RVec3(0, 20, 0), Quat::sIdentity(), JPH::EMotionType::Dynamic, Layers::MOVING);
+		compound_body_settings.mUseManifoldReduction = false; // Turn off manifold reduction for this body so that we can get proper callbacks for individual sub shapes
+		JPH::BodyID compound_body = bi.CreateAndAddBody(compound_body_settings, JPH::EActivation::Activate);
+
+		// Simulate until the body passes the origin
+		while (bi.GetPosition(compound_body).GetY() > 0.0f)
+			c.SimulateSingleStep();
+
+		// The expected events
+		struct Expected
+		{
+
+			EType		mType;
+			EUserData	mUserData;
+		};
+		const Expected expected[] = {
+			{ EType::Add, EUserData::Bottom },
+			{ EType::Add, EUserData::Middle },
+			{ EType::Add, EUserData::Top },
+			{ EType::Remove, EUserData::Bottom },
+			{ EType::Remove, EUserData::Middle },
+			{ EType::Remove, EUserData::Top }
+		};
+		const Expected *next = expected;
+		const Expected *end = expected + size(expected);
+
+		// Loop over events that we received
+		for (size_t e = 0; e < listener.GetEntryCount(); ++e)
+		{
+			const LoggingContactListener::LogEntry &entry = listener.GetEntry(e);
+
+			// Only interested in adds/removes
+			if (entry.mType != EType::Add && entry.mType != EType::Remove)
+				continue;
+
+			// Check if we have more expected events
+			if (next >= end)
+			{
+				CHECK(false);
+				break;
+			}
+
+			// Check if it is of expected type
+			CHECK(entry.mType == next->mType);
+			CHECK(entry.mBody1 == sensor_id);
+			CHECK(entry.mManifold.mSubShapeID1 == SubShapeID());
+			CHECK(entry.mBody2 == compound_body);
+			EUserData user_data = (EUserData)bi.GetShape(compound_body)->GetSubShapeUserData(entry.mManifold.mSubShapeID2);
+			CHECK(user_data == next->mUserData);
+
+			// Next expected event
+			++next;
+		}
+
+		// Check all expected events received
+		CHECK(next == end);
+	}
+
+	TEST_CASE("TestSensorVsStatic")
+	{
+		PhysicsTestContext c;
+
+		// Register listener
+		LoggingContactListener listener;
+		c.GetSystem()->SetContactListener(&listener);
+
+		// Static body 1
+		Body &static1 = c.CreateSphere(RVec3::sZero(), 1.0f, EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, EActivation::DontActivate);
+
+		// Sensor
+		BodyCreationSettings sensor_settings(new BoxShape(Vec3::sReplicate(1)), RVec3::sZero(), Quat::sIdentity(), EMotionType::Kinematic, Layers::MOVING); // Put in layer that collides with static
+		sensor_settings.mIsSensor = true;
+		Body &sensor = *c.GetBodyInterface().CreateBody(sensor_settings);
+		BodyID sensor_id = sensor.GetID();
+		c.GetBodyInterface().AddBody(sensor_id, EActivation::Activate);
+
+		// Static body 2 (created after sensor to force higher body ID)
+		Body &static2 = c.CreateSphere(RVec3::sZero(), 1.0f, EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, EActivation::DontActivate);
+
+		// After a step we should not detect the static bodies
+		c.SimulateSingleStep();
+		CHECK(listener.GetEntryCount() == 0);
+		listener.Clear();
+
+		// Start detecting static
+		sensor.SetCollideKinematicVsNonDynamic(true);
+
+		// After a single step we should detect both static bodies
+		c.SimulateSingleStep();
+		CHECK(listener.GetEntryCount() == 4); // Should also contain validates
+		CHECK(listener.Contains(EType::Add, static1.GetID(), sensor_id));
+		CHECK(listener.Contains(EType::Add, static2.GetID(), sensor_id));
+		listener.Clear();
+
+		// Stop detecting static
+		sensor.SetCollideKinematicVsNonDynamic(false);
+
+		// After a single step we should stop detecting both static bodies
+		c.SimulateSingleStep();
+		CHECK(listener.GetEntryCount() == 2);
+		CHECK(listener.Contains(EType::Remove, static1.GetID(), sensor_id));
+		CHECK(listener.Contains(EType::Remove, static2.GetID(), sensor_id));
+		listener.Clear();
+	}
+
+	static void sCollideBodyVsBodyAll(const Body &inBody1, const Body &inBody2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, CollideShapeSettings &ioCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter)
+	{
+		// Override the back face mode so we get hits with all triangles
+		ioCollideShapeSettings.mBackFaceMode = EBackFaceMode::CollideWithBackFaces;
+		PhysicsSystem::sDefaultSimCollideBodyVsBody(inBody1, inBody2, inCenterOfMassTransform1, inCenterOfMassTransform2, ioCollideShapeSettings, ioCollector, inShapeFilter);
+	}
+
+	static void sCollideBodyVsBodyPerBody(const Body &inBody1, const Body &inBody2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, CollideShapeSettings &ioCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter)
+	{
+		// Max 1 hit per body pair
+		AnyHitCollisionCollector<CollideShapeCollector> collector;
+		SubShapeIDCreator part1, part2;
+		CollisionDispatch::sCollideShapeVsShape(inBody1.GetShape(), inBody2.GetShape(), Vec3::sOne(), Vec3::sOne(), inCenterOfMassTransform1, inCenterOfMassTransform2, part1, part2, ioCollideShapeSettings, collector);
+		if (collector.HadHit())
+			ioCollector.AddHit(collector.mHit);
+	}
+
+	static void sCollideBodyVsBodyPerLeaf(const Body &inBody1, const Body &inBody2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, CollideShapeSettings &ioCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter)
+	{
+		// Max 1 hit per leaf shape pair
+		SubShapeIDCreator part1, part2;
+		CollideShapeVsShapePerLeaf<AnyHitCollisionCollector<CollideShapeCollector>>(inBody1.GetShape(), inBody2.GetShape(), Vec3::sOne(), Vec3::sOne(), inCenterOfMassTransform1, inCenterOfMassTransform2, part1, part2, ioCollideShapeSettings, ioCollector, inShapeFilter);
+	}
+
+	TEST_CASE("TestSimCollideBodyVsBody")
+	{
+		// Create pyramid with flat top
+		MeshShapeSettings pyramid;
+		pyramid.mTriangleVertices = { Float3(1, 0, 1), Float3(1, 0, -1), Float3(-1, 0, -1), Float3(-1, 0, 1), Float3(0.1f, 1, 0.1f), Float3(0.1f, 1, -0.1f), Float3(-0.1f, 1, -0.1f), Float3(-0.1f, 1, 0.1f) };
+		pyramid.mIndexedTriangles = { IndexedTriangle(0, 1, 4), IndexedTriangle(4, 1, 5), IndexedTriangle(1, 2, 5), IndexedTriangle(2, 6, 5), IndexedTriangle(2, 3, 6), IndexedTriangle(3, 7, 6), IndexedTriangle(3, 0, 7), IndexedTriangle(0, 4, 7), IndexedTriangle(4, 5, 6), IndexedTriangle(4, 6, 7) };
+		pyramid.SetEmbedded();
+
+		// Create floor of many pyramids
+		StaticCompoundShapeSettings compound;
+		for (int x = -10; x <= 10; ++x)
+			for (int z = -10; z <= 10; ++z)
+				compound.AddShape(Vec3(x * 2.0f, 0, z * 2.0f), Quat::sIdentity(), &pyramid);
+		compound.SetEmbedded();
+
+		for (int type = 0; type < 3; ++type)
+		{
+			PhysicsTestContext c;
+
+			// Register listener
+			LoggingContactListener listener;
+			c.GetSystem()->SetContactListener(&listener);
+
+			// Create floor
+			BodyID floor_id = c.GetBodyInterface().CreateAndAddBody(BodyCreationSettings(&compound, RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING), EActivation::DontActivate);
+
+			// A kinematic sensor that also detects static bodies
+			// Note that the size has been picked so that it is slightly smaller than 11 x 11 pyramids and incorporates all triangles in those pyramids
+			BodyCreationSettings sensor_settings(new BoxShape(Vec3::sReplicate(10.99f)), RVec3(0, 5, 0), Quat::sIdentity(), EMotionType::Kinematic, Layers::MOVING); // Put in a layer that collides with static
+			sensor_settings.mIsSensor = true;
+			sensor_settings.mCollideKinematicVsNonDynamic = true;
+			sensor_settings.mUseManifoldReduction = false;
+			BodyID sensor_id = c.GetBodyInterface().CreateAndAddBody(sensor_settings, EActivation::Activate);
+
+			// Select the body vs body collision function
+			switch (type)
+			{
+			case 0:
+				c.GetSystem()->SetSimCollideBodyVsBody(sCollideBodyVsBodyAll);
+				break;
+
+			case 1:
+				c.GetSystem()->SetSimCollideBodyVsBody(sCollideBodyVsBodyPerLeaf);
+				break;
+
+			case 2:
+				c.GetSystem()->SetSimCollideBodyVsBody(sCollideBodyVsBodyPerBody);
+				break;
+			}
+
+			// Trigger collision callbacks
+			c.SimulateSingleStep();
+
+			// Count the number of hits that were detected
+			size_t count = 0;
+			for (size_t i = 0; i < listener.GetEntryCount(); ++i)
+			{
+				const LoggingContactListener::LogEntry &entry = listener.GetEntry(i);
+				if (entry.mType == EType::Add && entry.mBody1 == floor_id && entry.mBody2 == sensor_id)
+					++count;
+			}
+
+			// Check that we received the number of hits that we expected
+			switch (type)
+			{
+			case 0:
+				// All hits
+				CHECK(count == 11 * 11 * pyramid.mIndexedTriangles.size());
+				break;
+
+			case 1:
+				// Only 1 per sub shape
+				CHECK(count == 11 * 11);
+				break;
+
+			case 2:
+				// Only 1 per body pair
+				CHECK(count == 1);
+				break;
+			}
+		}
+	}
+}

lib/All/JoltPhysics/UnitTests/Physics/ShapeFilterTests.cpp

@@ -0,0 +1,93 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2024 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include "PhysicsTestContext.h"
+#include "Layers.h"
+#include "LoggingContactListener.h"
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Collision/Shape/SphereShape.h>
+#include <Jolt/Physics/Collision/Shape/StaticCompoundShape.h>
+#include <Jolt/Physics/Collision/SimShapeFilter.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+
+TEST_SUITE("ShapeFilterTests")
+{
+	// Tests two spheres in one simulated body, one collides with a static platform, the other doesn't
+	TEST_CASE("TestSimShapeFilter")
+	{
+		// Test once per motion quality type
+		for (int q = 0; q < 2; ++q)
+		{
+			PhysicsTestContext c;
+
+			// Log contacts
+			LoggingContactListener contact_listener;
+			c.GetSystem()->SetContactListener(&contact_listener);
+
+			// Install simulation shape filter
+			class Filter : public SimShapeFilter
+			{
+			public:
+				virtual bool	ShouldCollide(const Body &inBody1, const Shape *inShape1, const SubShapeID &inSubShapeIDOfShape1, const Body &inBody2, const Shape *inShape2, const SubShapeID &inSubShapeIDOfShape2) const override
+				{
+					// If the platform is colliding with the compound, filter out collisions where the shape has user data 1
+					if (inBody1.GetID() == mPlatformID && inBody2.GetID() == mCompoundID)
+						return inShape2->GetUserData() != 1;
+					else if (inBody1.GetID() == mCompoundID && inBody2.GetID() == mPlatformID)
+						return inShape1->GetUserData() != 1;
+					return true;
+				}
+
+				BodyID			mPlatformID;
+				BodyID			mCompoundID;
+			};
+			Filter shape_filter;
+			c.GetSystem()->SetSimShapeFilter(&shape_filter);
+
+			// Floor
+			BodyID floor_id = c.CreateFloor().GetID();
+
+			// Platform
+			BodyInterface &bi = c.GetBodyInterface();
+			shape_filter.mPlatformID = bi.CreateAndAddBody(BodyCreationSettings(new BoxShape(Vec3(10, 0.5f, 10)), RVec3(0, 3.5f, 0), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING), EActivation::DontActivate);
+
+			// Compound shape that starts above platform
+			Ref<Shape> sphere = new SphereShape(0.5f);
+			sphere->SetUserData(1); // Don't want sphere to collide with the platform
+			Ref<Shape> sphere2 = new SphereShape(0.5f);
+			Ref<StaticCompoundShapeSettings> compound_settings = new StaticCompoundShapeSettings;
+			compound_settings->AddShape(Vec3(0, -2, 0), Quat::sIdentity(), sphere);
+			compound_settings->AddShape(Vec3(0, 2, 0), Quat::sIdentity(), sphere2);
+			Ref<StaticCompoundShape> compound = StaticCast<StaticCompoundShape>(compound_settings->Create().Get());
+			BodyCreationSettings bcs(compound, RVec3(0, 7, 0), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING);
+			if (q == 1)
+			{
+				// For the 2nd iteration activate CCD
+				bcs.mMotionQuality = EMotionQuality::LinearCast;
+				bcs.mLinearVelocity = Vec3(0, -50, 0);
+			}
+			shape_filter.mCompoundID = bi.CreateAndAddBody(bcs, EActivation::Activate);
+
+			// Get sub shape IDs
+			SubShapeID sphere_id = compound->GetSubShapeIDFromIndex(0, SubShapeIDCreator()).GetID();
+			SubShapeID sphere2_id = compound->GetSubShapeIDFromIndex(1, SubShapeIDCreator()).GetID();
+
+			// Simulate for 2 seconds
+			c.Simulate(2.0f);
+
+			// The compound should now be resting with sphere on the platform and the sphere2 on the floor
+			CHECK_APPROX_EQUAL(bi.GetPosition(shape_filter.mCompoundID), RVec3(0, 2.5f, 0), 1.01f * c.GetSystem()->GetPhysicsSettings().mPenetrationSlop);
+			CHECK_APPROX_EQUAL(bi.GetRotation(shape_filter.mCompoundID), Quat::sIdentity());
+
+			// Check that sphere2 collided with the platform but sphere did not
+			CHECK(contact_listener.Contains(LoggingContactListener::EType::Add, shape_filter.mPlatformID, SubShapeID(), shape_filter.mCompoundID, sphere2_id));
+			CHECK(!contact_listener.Contains(LoggingContactListener::EType::Add, shape_filter.mPlatformID, SubShapeID(), shape_filter.mCompoundID, sphere_id));
+
+			// Check that sphere2 didn't collide with the floor but that the sphere did
+			CHECK(contact_listener.Contains(LoggingContactListener::EType::Add, floor_id, SubShapeID(), shape_filter.mCompoundID, sphere_id));
+			CHECK(!contact_listener.Contains(LoggingContactListener::EType::Add, floor_id, SubShapeID(), shape_filter.mCompoundID, sphere2_id));
+		}
+	}
+}

lib/All/JoltPhysics/UnitTests/Physics/SixDOFConstraintTests.cpp

@@ -0,0 +1,127 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include "PhysicsTestContext.h"
+#include <Jolt/Physics/Constraints/SixDOFConstraint.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include "Layers.h"
+
+TEST_SUITE("SixDOFConstraintTests")
+{
+	// Test if the 6DOF constraint can be used to create a spring
+	TEST_CASE("TestSixDOFSpring")
+	{
+		// Configuration of the spring
+		const float cFrequency = 2.0f;
+		const float cDamping = 0.1f;
+
+		// Test all permutations of axis
+		for (uint spring_axis = 0b001; spring_axis <= 0b111; ++spring_axis)
+		{
+			// Test all spring modes
+			for (int mode = 0; mode < 2; ++mode)
+			{
+				const RVec3 cInitialPosition(10.0f * (spring_axis & 1), 8.0f * (spring_axis & 2), 6.0f * (spring_axis & 4));
+
+				// Create a sphere
+				PhysicsTestContext context;
+				context.ZeroGravity();
+				Body& body = context.CreateSphere(cInitialPosition, 0.5f, EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING);
+				body.GetMotionProperties()->SetLinearDamping(0.0f);
+
+				// Calculate stiffness and damping of spring
+				float m = 1.0f / body.GetMotionProperties()->GetInverseMass();
+				float omega = 2.0f * JPH_PI * cFrequency;
+				float k = m * Square(omega);
+				float c = 2.0f * m * cDamping * omega;
+
+				// Create spring
+				SixDOFConstraintSettings constraint;
+				constraint.mPosition2 = cInitialPosition;
+				for (int axis = 0; axis < 3; ++axis)
+				{
+					// Check if this axis is supposed to be a spring
+					if (((1 << axis) & spring_axis) != 0)
+					{
+						if (mode == 0)
+						{
+							// First iteration use stiffness and damping
+							constraint.mLimitsSpringSettings[axis].mMode = ESpringMode::StiffnessAndDamping;
+							constraint.mLimitsSpringSettings[axis].mStiffness = k;
+							constraint.mLimitsSpringSettings[axis].mDamping = c;
+						}
+						else
+						{
+							// Second iteration use frequency and damping
+							constraint.mLimitsSpringSettings[axis].mMode = ESpringMode::FrequencyAndDamping;
+							constraint.mLimitsSpringSettings[axis].mFrequency = cFrequency;
+							constraint.mLimitsSpringSettings[axis].mDamping = cDamping;
+						}
+						constraint.mLimitMin[axis] = constraint.mLimitMax[axis] = 0.0f;
+					}
+				}
+				context.CreateConstraint<SixDOFConstraint>(Body::sFixedToWorld, body, constraint);
+
+				// Simulate spring
+				RVec3 x = cInitialPosition;
+				Vec3 v = Vec3::sZero();
+				float dt = context.GetDeltaTime();
+				for (int i = 0; i < 120; ++i)
+				{
+					// Using the equations from page 32 of Soft Constraints: Reinventing The Spring - Erin Catto - GDC 2011 for an implicit euler spring damper
+					for (int axis = 0; axis < 3; ++axis)
+						if (((1 << axis) & spring_axis) != 0) // Only update velocity for axis where there is a spring
+							v.SetComponent(axis, (v[axis] - dt * k / m * float(x[axis])) / (1.0f + dt * c / m + Square(dt) * k / m));
+					x += v * dt;
+
+					// Run physics simulation
+					context.SimulateSingleStep();
+
+					// Test if simulation matches prediction
+					CHECK_APPROX_EQUAL(x, body.GetPosition(), 1.0e-5_r);
+				}
+			}
+		}
+	}
+
+	// Test combination of locked rotation axis with a 6DOF constraint
+	TEST_CASE("TestSixDOFLockedRotation")
+	{
+		PhysicsTestContext context;
+		BodyInterface &bi = context.GetBodyInterface();
+		PhysicsSystem *system = context.GetSystem();
+
+		RefConst<Shape> box_shape = new BoxShape(Vec3::sReplicate(1.0f));
+
+		// Static 'anchor' body
+		BodyCreationSettings settings1(box_shape, RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING);
+		Body &body1 = *bi.CreateBody(settings1);
+		bi.AddBody(body1.GetID(), EActivation::Activate);
+
+		// Dynamic body that cannot rotate around X and Y
+		const RVec3 position2(3, 0, 0);
+		const Quat rotation2 = Quat::sIdentity();
+		BodyCreationSettings settings2(box_shape, position2, rotation2, EMotionType::Dynamic, Layers::MOVING);
+		settings2.mAllowedDOFs = EAllowedDOFs::RotationZ | EAllowedDOFs::TranslationX | EAllowedDOFs::TranslationY | EAllowedDOFs::TranslationZ;
+		Body &body2 = *bi.CreateBody(settings2);
+		bi.AddBody(body2.GetID(), EActivation::Activate);
+
+		// Lock all 6 axis with a 6DOF constraint
+		SixDOFConstraintSettings six_dof;
+		six_dof.MakeFixedAxis(SixDOFConstraintSettings::EAxis::TranslationX);
+		six_dof.MakeFixedAxis(SixDOFConstraintSettings::EAxis::TranslationY);
+		six_dof.MakeFixedAxis(SixDOFConstraintSettings::EAxis::TranslationZ);
+		six_dof.MakeFixedAxis(SixDOFConstraintSettings::EAxis::RotationX);
+		six_dof.MakeFixedAxis(SixDOFConstraintSettings::EAxis::RotationY);
+		six_dof.MakeFixedAxis(SixDOFConstraintSettings::EAxis::RotationZ);
+		system->AddConstraint(six_dof.Create(body1, body2));
+
+		context.Simulate(1.0f);
+
+		// Check that body didn't rotate
+		CHECK_APPROX_EQUAL(body2.GetPosition(), position2, 5.0e-3f);
+		CHECK_APPROX_EQUAL(body2.GetRotation(), rotation2, 5.0e-3f);
+	}
+}

lib/All/JoltPhysics/UnitTests/Physics/SliderConstraintTests.cpp

@@ -0,0 +1,507 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include "PhysicsTestContext.h"
+#include <Jolt/Physics/Constraints/SliderConstraint.h>
+#include <Jolt/Physics/Collision/GroupFilterTable.h>
+#include "Layers.h"
+
+TEST_SUITE("SliderConstraintTests")
+{
+	// Test a box attached to a slider constraint, test that the body doesn't move beyond the min limit
+	TEST_CASE("TestSliderConstraintLimitMin")
+	{
+		const RVec3 cInitialPos(3.0f, 0, 0);
+		const float cLimitMin = -7.0f;
+
+		// Create group filter
+		Ref<GroupFilterTable> group_filter = new GroupFilterTable;
+
+		// Create two boxes
+		PhysicsTestContext c;
+		Body &body1 = c.CreateBox(RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, Vec3(1, 1, 1));
+		Body &body2 = c.CreateBox(cInitialPos, Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3(1, 1, 1));
+
+		// Give body 2 velocity towards min limit (and ensure that it arrives well before 1 second)
+		body2.SetLinearVelocity(-Vec3(10.0f, 0, 0));
+
+		// Bodies will go through each other, make sure they don't collide
+		body1.SetCollisionGroup(CollisionGroup(group_filter, 0, 0));
+		body2.SetCollisionGroup(CollisionGroup(group_filter, 0, 0));
+
+		// Create slider constraint
+		SliderConstraintSettings s;
+		s.mAutoDetectPoint = true;
+		s.SetSliderAxis(Vec3::sAxisX());
+		s.mLimitsMin = cLimitMin;
+		s.mLimitsMax = 0.0f;
+		c.CreateConstraint<SliderConstraint>(body1, body2, s);
+
+		// Simulate
+		c.Simulate(1.0f);
+
+		// Test resulting velocity
+		CHECK_APPROX_EQUAL(Vec3::sZero(), body2.GetLinearVelocity(), 1.0e-4f);
+
+		// Test resulting position
+		CHECK_APPROX_EQUAL(cInitialPos + cLimitMin * s.mSliderAxis1, body2.GetPosition(), 1.0e-4f);
+	}
+
+	// Test a box attached to a slider constraint, test that the body doesn't move beyond the max limit
+	TEST_CASE("TestSliderConstraintLimitMax")
+	{
+		const RVec3 cInitialPos(3.0f, 0, 0);
+		const float cLimitMax = 7.0f;
+
+		// Create two boxes
+		PhysicsTestContext c;
+		Body &body1 = c.CreateBox(RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, Vec3(1, 1, 1));
+		Body &body2 = c.CreateBox(cInitialPos, Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3(1, 1, 1));
+
+		// Give body 2 velocity towards max limit (and ensure that it arrives well before 1 second)
+		body2.SetLinearVelocity(Vec3(10.0f, 0, 0));
+
+		// Create slider constraint
+		SliderConstraintSettings s;
+		s.mAutoDetectPoint = true;
+		s.SetSliderAxis(Vec3::sAxisX());
+		s.mLimitsMin = 0.0f;
+		s.mLimitsMax = cLimitMax;
+		c.CreateConstraint<SliderConstraint>(body1, body2, s);
+
+		// Simulate
+		c.Simulate(1.0f);
+
+		// Test resulting velocity
+		CHECK_APPROX_EQUAL(Vec3::sZero(), body2.GetLinearVelocity(), 1.0e-4f);
+
+		// Test resulting position
+		CHECK_APPROX_EQUAL(cInitialPos + cLimitMax * s.mSliderAxis1, body2.GetPosition(), 1.0e-4f);
+	}
+
+	// Test a box attached to a slider constraint, test that a motor can drive it to a specific velocity
+	TEST_CASE("TestSliderConstraintDriveVelocityStaticVsDynamic")
+	{
+		const RVec3 cInitialPos(3.0f, 0, 0);
+		const float cMotorAcceleration = 2.0f;
+
+		// Create two boxes
+		PhysicsTestContext c;
+		Body &body1 = c.CreateBox(RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, Vec3(1, 1, 1));
+		Body &body2 = c.CreateBox(cInitialPos, Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3(1, 1, 1));
+
+		// Create slider constraint
+		SliderConstraintSettings s;
+		s.mAutoDetectPoint = true;
+		s.SetSliderAxis(Vec3::sAxisX());
+		constexpr float mass = Cubed(2.0f) * 1000.0f; // Density * Volume
+		s.mMotorSettings = MotorSettings(0.0f, 0.0f, mass * cMotorAcceleration, 0.0f);
+		SliderConstraint &constraint = c.CreateConstraint<SliderConstraint>(body1, body2, s);
+		constraint.SetMotorState(EMotorState::Velocity);
+		constraint.SetTargetVelocity(1.5f * cMotorAcceleration);
+
+		// Simulate
+		c.Simulate(1.0f);
+
+		// Test resulting velocity
+		Vec3 expected_vel = cMotorAcceleration * s.mSliderAxis1;
+		CHECK_APPROX_EQUAL(expected_vel, body2.GetLinearVelocity(), 1.0e-4f);
+
+		// Simulate (after 0.5 seconds it should reach the target velocity)
+		c.Simulate(1.0f);
+
+		// Test resulting velocity
+		expected_vel = 1.5f * cMotorAcceleration * s.mSliderAxis1;
+		CHECK_APPROX_EQUAL(expected_vel, body2.GetLinearVelocity(), 1.0e-4f);
+
+		// Test resulting position (1.5s of acceleration + 0.5s of constant speed)
+		RVec3 expected_pos = c.PredictPosition(cInitialPos, Vec3::sZero(), cMotorAcceleration * s.mSliderAxis1, 1.5f) + 0.5f * expected_vel;
+		CHECK_APPROX_EQUAL(expected_pos, body2.GetPosition(), 1.0e-4f);
+	}
+
+	// Test 2 dynamic boxes attached to a slider constraint, test that a motor can drive it to a specific velocity
+	TEST_CASE("TestSliderConstraintDriveVelocityDynamicVsDynamic")
+	{
+		const RVec3 cInitialPos(3.0f, 0, 0);
+		const float cMotorAcceleration = 2.0f;
+
+		// Create two boxes
+		PhysicsTestContext c;
+		c.ZeroGravity();
+		Body &body1 = c.CreateBox(RVec3::sZero(), Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3(1, 1, 1));
+		Body &body2 = c.CreateBox(cInitialPos, Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3(1, 1, 1));
+
+		// Create slider constraint
+		SliderConstraintSettings s;
+		s.mAutoDetectPoint = true;
+		s.SetSliderAxis(Vec3::sAxisX());
+		constexpr float mass = Cubed(2.0f) * 1000.0f; // Density * Volume
+		s.mMotorSettings = MotorSettings(0.0f, 0.0f, mass * cMotorAcceleration, 0.0f);
+		SliderConstraint &constraint = c.CreateConstraint<SliderConstraint>(body1, body2, s);
+		constraint.SetMotorState(EMotorState::Velocity);
+		constraint.SetTargetVelocity(3.0f * cMotorAcceleration);
+
+		// Simulate
+		c.Simulate(1.0f);
+
+		// Test resulting velocity (both boxes move in opposite directions with the same force, so the resulting velocity difference is 2x as big as the previous test)
+		Vec3 expected_vel = cMotorAcceleration * s.mSliderAxis1;
+		CHECK_APPROX_EQUAL(-expected_vel, body1.GetLinearVelocity(), 1.0e-4f);
+		CHECK_APPROX_EQUAL(expected_vel, body2.GetLinearVelocity(), 1.0e-4f);
+
+		// Simulate (after 0.5 seconds it should reach the target velocity)
+		c.Simulate(1.0f);
+
+		// Test resulting velocity
+		expected_vel = 1.5f * cMotorAcceleration * s.mSliderAxis1;
+		CHECK_APPROX_EQUAL(-expected_vel, body1.GetLinearVelocity(), 1.0e-4f);
+		CHECK_APPROX_EQUAL(expected_vel, body2.GetLinearVelocity(), 1.0e-4f);
+
+		// Test resulting position (1.5s of acceleration + 0.5s of constant speed)
+		RVec3 expected_pos1 = c.PredictPosition(RVec3::sZero(), Vec3::sZero(), -cMotorAcceleration * s.mSliderAxis1, 1.5f) - 0.5f * expected_vel;
+		RVec3 expected_pos2 = c.PredictPosition(cInitialPos, Vec3::sZero(), cMotorAcceleration * s.mSliderAxis1, 1.5f) + 0.5f * expected_vel;
+		CHECK_APPROX_EQUAL(expected_pos1, body1.GetPosition(), 1.0e-4f);
+		CHECK_APPROX_EQUAL(expected_pos2, body2.GetPosition(), 1.0e-4f);
+	}
+
+	// Test a box attached to a slider constraint, test that a motor can drive it to a specific position
+	TEST_CASE("TestSliderConstraintDrivePosition")
+	{
+		const RVec3 cInitialPos(3.0f, 0, 0);
+		const RVec3 cMotorPos(10.0f, 0, 0);
+
+		// Create two boxes
+		PhysicsTestContext c;
+		Body &body1 = c.CreateBox(RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, Vec3(1, 1, 1));
+		Body &body2 = c.CreateBox(cInitialPos, Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3(1, 1, 1));
+
+		// Create slider constraint
+		SliderConstraintSettings s;
+		s.mAutoDetectPoint = true;
+		s.SetSliderAxis(Vec3::sAxisX());
+		SliderConstraint &constraint = c.CreateConstraint<SliderConstraint>(body1, body2, s);
+		constraint.SetMotorState(EMotorState::Position);
+		constraint.SetTargetPosition(Vec3(cMotorPos - cInitialPos).Dot(s.mSliderAxis1));
+
+		// Simulate
+		c.Simulate(2.0f);
+
+		// Test resulting velocity
+		CHECK_APPROX_EQUAL(Vec3::sZero(), body2.GetLinearVelocity(), 1.0e-4f);
+
+		// Test resulting position
+		CHECK_APPROX_EQUAL(cMotorPos, body2.GetPosition(), 1.0e-4f);
+	}
+
+	// Test a box attached to a slider constraint, give it initial velocity and test that the friction provides the correct deceleration
+	TEST_CASE("TestSliderConstraintFriction")
+	{
+		const RVec3 cInitialPos(3.0f, 0, 0);
+		const Vec3 cInitialVelocity(10.0f, 0, 0);
+		const float cFrictionAcceleration = 2.0f;
+		const float cSimulationTime = 2.0f;
+
+		// Create two boxes
+		PhysicsTestContext c;
+		Body &body1 = c.CreateBox(RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, Vec3(1, 1, 1));
+		Body &body2 = c.CreateBox(cInitialPos, Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3(1, 1, 1));
+		body2.SetLinearVelocity(cInitialVelocity);
+
+		// Create slider constraint
+		SliderConstraintSettings s;
+		s.mAutoDetectPoint = true;
+		s.SetSliderAxis(Vec3::sAxisX());
+		constexpr float mass = Cubed(2.0f) * 1000.0f; // Density * Volume
+		s.mMaxFrictionForce = mass * cFrictionAcceleration;
+		c.CreateConstraint<SliderConstraint>(body1, body2, s);
+
+		// Simulate while applying friction
+		c.Simulate(cSimulationTime);
+
+		// Test resulting velocity
+		Vec3 expected_vel = cInitialVelocity - cFrictionAcceleration * cSimulationTime * s.mSliderAxis1;
+		CHECK_APPROX_EQUAL(expected_vel, body2.GetLinearVelocity(), 1.0e-4f);
+
+		// Test resulting position
+		RVec3 expected_pos = c.PredictPosition(cInitialPos, cInitialVelocity, -cFrictionAcceleration * s.mSliderAxis1, cSimulationTime);
+		CHECK_APPROX_EQUAL(expected_pos, body2.GetPosition(), 1.0e-4f);
+	}
+
+	// Test if a slider constraint wakes up connected bodies
+	TEST_CASE("TestSliderStaticVsKinematic")
+	{
+		// Create two boxes far away enough so they are not touching
+		PhysicsTestContext c;
+		Body &body1 = c.CreateBox(RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, Vec3(1, 1, 1), EActivation::DontActivate);
+		Body &body2 = c.CreateBox(RVec3(10, 0, 0), Quat::sIdentity(), EMotionType::Kinematic, EMotionQuality::Discrete, Layers::MOVING, Vec3(1, 1, 1), EActivation::DontActivate);
+
+		// Create slider constraint
+		SliderConstraintSettings s;
+		s.mAutoDetectPoint = true;
+		s.SetSliderAxis(Vec3::sAxisX());
+		c.CreateConstraint<SliderConstraint>(body1, body2, s);
+
+		// Verify they're not active
+		CHECK(!body1.IsActive());
+		CHECK(!body2.IsActive());
+
+		// After a physics step, the bodies should still not be active
+		c.SimulateSingleStep();
+		CHECK(!body1.IsActive());
+		CHECK(!body2.IsActive());
+
+		// Activate the kinematic body
+		c.GetSystem()->GetBodyInterface().ActivateBody(body2.GetID());
+		CHECK(!body1.IsActive());
+		CHECK(body2.IsActive());
+
+		// The static body should not become active (it can't)
+		c.SimulateSingleStep();
+		CHECK(!body1.IsActive());
+		CHECK(body2.IsActive());
+	}
+
+	// Test if a slider constraint wakes up connected bodies
+	TEST_CASE("TestSliderStaticVsDynamic")
+	{
+		// Create two boxes far away enough so they are not touching
+		PhysicsTestContext c;
+		Body &body1 = c.CreateBox(RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, Vec3(1, 1, 1), EActivation::DontActivate);
+		Body &body2 = c.CreateBox(RVec3(10, 0, 0), Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3(1, 1, 1), EActivation::DontActivate);
+
+		// Create slider constraint
+		SliderConstraintSettings s;
+		s.mAutoDetectPoint = true;
+		s.SetSliderAxis(Vec3::sAxisX());
+		c.CreateConstraint<SliderConstraint>(body1, body2, s);
+
+		// Verify they're not active
+		CHECK(!body1.IsActive());
+		CHECK(!body2.IsActive());
+
+		// After a physics step, the bodies should still not be active
+		c.SimulateSingleStep();
+		CHECK(!body1.IsActive());
+		CHECK(!body2.IsActive());
+
+		// Activate the dynamic body
+		c.GetSystem()->GetBodyInterface().ActivateBody(body2.GetID());
+		CHECK(!body1.IsActive());
+		CHECK(body2.IsActive());
+
+		// The static body should not become active (it can't)
+		c.SimulateSingleStep();
+		CHECK(!body1.IsActive());
+		CHECK(body2.IsActive());
+	}
+
+	// Test if a slider constraint wakes up connected bodies
+	TEST_CASE("TestSliderKinematicVsDynamic")
+	{
+		// Create two boxes far away enough so they are not touching
+		PhysicsTestContext c;
+		Body &body1 = c.CreateBox(RVec3::sZero(), Quat::sIdentity(), EMotionType::Kinematic, EMotionQuality::Discrete, Layers::MOVING, Vec3(1, 1, 1), EActivation::DontActivate);
+		Body &body2 = c.CreateBox(RVec3(10, 0, 0), Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3(1, 1, 1), EActivation::DontActivate);
+
+		// Create slider constraint
+		SliderConstraintSettings s;
+		s.mAutoDetectPoint = true;
+		s.SetSliderAxis(Vec3::sAxisX());
+		c.CreateConstraint<SliderConstraint>(body1, body2, s);
+
+		// Verify they're not active
+		CHECK(!body1.IsActive());
+		CHECK(!body2.IsActive());
+
+		// After a physics step, the bodies should still not be active
+		c.SimulateSingleStep();
+		CHECK(!body1.IsActive());
+		CHECK(!body2.IsActive());
+
+		// Activate the keyframed body
+		c.GetSystem()->GetBodyInterface().ActivateBody(body1.GetID());
+		CHECK(body1.IsActive());
+		CHECK(!body2.IsActive());
+
+		// After a physics step, both bodies should be active now
+		c.SimulateSingleStep();
+		CHECK(body1.IsActive());
+		CHECK(body2.IsActive());
+	}
+
+	// Test if a slider constraint wakes up connected bodies
+	TEST_CASE("TestSliderKinematicVsKinematic")
+	{
+		// Create two boxes far away enough so they are not touching
+		PhysicsTestContext c;
+		Body &body1 = c.CreateBox(RVec3::sZero(), Quat::sIdentity(), EMotionType::Kinematic, EMotionQuality::Discrete, Layers::MOVING, Vec3(1, 1, 1), EActivation::DontActivate);
+		Body &body2 = c.CreateBox(RVec3(10, 0, 0), Quat::sIdentity(), EMotionType::Kinematic, EMotionQuality::Discrete, Layers::MOVING, Vec3(1, 1, 1), EActivation::DontActivate);
+
+		// Create slider constraint
+		SliderConstraintSettings s;
+		s.mAutoDetectPoint = true;
+		s.SetSliderAxis(Vec3::sAxisX());
+		c.CreateConstraint<SliderConstraint>(body1, body2, s);
+
+		// Verify they're not active
+		CHECK(!body1.IsActive());
+		CHECK(!body2.IsActive());
+
+		// After a physics step, the bodies should still not be active
+		c.SimulateSingleStep();
+		CHECK(!body1.IsActive());
+		CHECK(!body2.IsActive());
+
+		// Activate the first keyframed body
+		c.GetSystem()->GetBodyInterface().ActivateBody(body1.GetID());
+		CHECK(body1.IsActive());
+		CHECK(!body2.IsActive());
+
+		// After a physics step, the second keyframed body should not be woken up
+		c.SimulateSingleStep();
+		CHECK(body1.IsActive());
+		CHECK(!body2.IsActive());
+	}
+
+	// Test if a slider constraint wakes up connected bodies
+	TEST_CASE("TestSliderDynamicVsDynamic")
+	{
+		// Create two boxes far away enough so they are not touching
+		PhysicsTestContext c;
+		Body &body1 = c.CreateBox(RVec3::sZero(), Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3(1, 1, 1), EActivation::DontActivate);
+		Body &body2 = c.CreateBox(RVec3(10, 0, 0), Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3(1, 1, 1), EActivation::DontActivate);
+
+		// Create slider constraint
+		SliderConstraintSettings s;
+		s.mAutoDetectPoint = true;
+		s.SetSliderAxis(Vec3::sAxisX());
+		c.CreateConstraint<SliderConstraint>(body1, body2, s);
+
+		// Verify they're not active
+		CHECK(!body1.IsActive());
+		CHECK(!body2.IsActive());
+
+		// After a physics step, the bodies should still not be active
+		c.SimulateSingleStep();
+		CHECK(!body1.IsActive());
+		CHECK(!body2.IsActive());
+
+		// Activate the first dynamic body
+		c.GetSystem()->GetBodyInterface().ActivateBody(body1.GetID());
+		CHECK(body1.IsActive());
+		CHECK(!body2.IsActive());
+
+		// After a physics step, both bodies should be active now
+		c.SimulateSingleStep();
+		CHECK(body1.IsActive());
+		CHECK(body2.IsActive());
+	}
+
+	// Test that when a reference frame is provided, the slider constraint is correctly constructed
+	TEST_CASE("TestSliderReferenceFrame")
+	{
+		// Create two boxes in semi random position/orientation
+		PhysicsTestContext c;
+		Body &body1 = c.CreateBox(RVec3(1, 2, 3), Quat::sRotation(Vec3(1, 1, 1).Normalized(), 0.1f * JPH_PI), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3(1, 1, 1), EActivation::Activate);
+		Body &body2 = c.CreateBox(RVec3(-3, -2, -1), Quat::sRotation(Vec3(1, 0, 1).Normalized(), 0.2f * JPH_PI), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, Vec3(1, 1, 1), EActivation::Activate);
+
+		// Disable collision between the boxes
+		GroupFilterTable *group_filter = new GroupFilterTable(2);
+		group_filter->DisableCollision(0, 1);
+		body1.SetCollisionGroup(CollisionGroup(group_filter, 0, 0));
+		body2.SetCollisionGroup(CollisionGroup(group_filter, 0, 1));
+
+		// Get their transforms
+		RMat44 t1 = body1.GetCenterOfMassTransform();
+		RMat44 t2 = body2.GetCenterOfMassTransform();
+
+		// Create slider constraint so that slider connects the bodies at their center of mass and rotated XY -> YZ
+		SliderConstraintSettings s;
+		s.mPoint1 = t1.GetTranslation();
+		s.mSliderAxis1 = t1.GetColumn3(0);
+		s.mNormalAxis1 = t1.GetColumn3(1);
+		s.mPoint2 = t2.GetTranslation();
+		s.mSliderAxis2 = t2.GetColumn3(1);
+		s.mNormalAxis2 = t2.GetColumn3(2);
+		SliderConstraint &constraint = c.CreateConstraint<SliderConstraint>(body1, body2, s);
+
+		// Activate the motor to drive to 0
+		constraint.SetMotorState(EMotorState::Position);
+		constraint.SetTargetPosition(0);
+
+		// Simulate for a second
+		c.Simulate(1.0f);
+
+		// Now the bodies should have aligned so their COM is at the same position and they're rotated XY -> YZ
+		t1 = body1.GetCenterOfMassTransform();
+		t2 = body2.GetCenterOfMassTransform();
+		CHECK_APPROX_EQUAL(t1.GetColumn3(0), t2.GetColumn3(1), 1.0e-4f);
+		CHECK_APPROX_EQUAL(t1.GetColumn3(1), t2.GetColumn3(2), 1.0e-4f);
+		CHECK_APPROX_EQUAL(t1.GetColumn3(2), t2.GetColumn3(0), 1.0e-4f);
+		CHECK_APPROX_EQUAL(t1.GetTranslation(), t2.GetTranslation(), 1.0e-2f);
+	}
+
+	// Test if the slider constraint can be used to create a spring
+	TEST_CASE("TestSliderSpring")
+	{
+		// Configuration of the spring
+		const RVec3 cInitialPosition(10, 0, 0);
+		const float cFrequency = 2.0f;
+		const float cDamping = 0.1f;
+
+		for (int mode = 0; mode < 2; ++mode)
+		{
+			// Create a sphere
+			PhysicsTestContext context;
+			Body &body = context.CreateSphere(cInitialPosition, 0.5f, EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING);
+			body.GetMotionProperties()->SetLinearDamping(0.0f);
+
+			// Calculate stiffness and damping of spring
+			float m = 1.0f / body.GetMotionProperties()->GetInverseMass();
+			float omega = 2.0f * JPH_PI * cFrequency;
+			float k = m * Square(omega);
+			float c = 2.0f * m * cDamping * omega;
+
+			// Create spring
+			SliderConstraintSettings constraint;
+			constraint.mPoint2 = cInitialPosition;
+			if (mode == 0)
+			{
+				// First iteration use stiffness and damping
+				constraint.mLimitsSpringSettings.mMode = ESpringMode::StiffnessAndDamping;
+				constraint.mLimitsSpringSettings.mStiffness = k;
+				constraint.mLimitsSpringSettings.mDamping = c;
+			}
+			else
+			{
+				// Second iteration use frequency and damping
+				constraint.mLimitsSpringSettings.mMode = ESpringMode::FrequencyAndDamping;
+				constraint.mLimitsSpringSettings.mFrequency = cFrequency;
+				constraint.mLimitsSpringSettings.mDamping = cDamping;
+			}
+			constraint.mLimitsMin = constraint.mLimitsMax = 0.0f;
+			context.CreateConstraint<SliderConstraint>(Body::sFixedToWorld, body, constraint);
+
+			// Simulate spring
+			Real x = cInitialPosition.GetX();
+			float v = 0.0f;
+			float dt = context.GetDeltaTime();
+			for (int i = 0; i < 120; ++i)
+			{
+				// Using the equations from page 32 of Soft Constraints: Reinventing The Spring - Erin Catto - GDC 2011 for an implicit euler spring damper
+				v = (v - dt * k / m * float(x)) / (1.0f + dt * c / m + Square(dt) * k / m);
+				x += v * dt;
+
+				// Run physics simulation
+				context.SimulateSingleStep();
+
+				// Test if simulation matches prediction
+				CHECK_APPROX_EQUAL(x, body.GetPosition().GetX(), 5.0e-6_r);
+				CHECK_APPROX_EQUAL(body.GetPosition().GetY(), 0);
+				CHECK_APPROX_EQUAL(body.GetPosition().GetZ(), 0);
+			}
+		}
+	}
+}

lib/All/JoltPhysics/UnitTests/Physics/SoftBodyTests.cpp

@@ -0,0 +1,148 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2024 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include "PhysicsTestContext.h"
+#include "Layers.h"
+#include <Jolt/Physics/SoftBody/SoftBodySharedSettings.h>
+#include <Jolt/Physics/SoftBody/SoftBodyCreationSettings.h>
+#include <Jolt/Physics/SoftBody/SoftBodyMotionProperties.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+
+TEST_SUITE("SoftBodyTests")
+{
+	TEST_CASE("TestBendConstraint")
+	{
+		// Possible values for x3
+		const Float3 x3_values[] = {
+			Float3(0, 0, 1),	// forming flat plane
+			Float3(0, 0, -1),	// overlapping
+			Float3(0, 1, 0),	// 90 degrees concave
+			Float3(0, -1, 0),	// 90 degrees convex
+			Float3(0, 1, 1),	// 45 degrees concave
+			Float3(0, -1, -1)	// 135 degrees convex
+		};
+
+		for (const Float3 &x3 : x3_values)
+		{
+			PhysicsTestContext c;
+			PhysicsSystem *s = c.GetSystem();
+			BodyInterface &bi = s->GetBodyInterface();
+
+			// Create settings
+			Ref<SoftBodySharedSettings> shared_settings = new SoftBodySharedSettings;
+
+			/* Create two triangles with a shared edge, x3 = free, the rest is locked
+			   x2
+			e1/  \e3
+			 /    \
+			x0----x1
+			 \ e0 /
+			e2\  /e4
+			   x3
+			*/
+			SoftBodySharedSettings::Vertex v;
+			v.mPosition = Float3(-1, 0, 0);
+			v.mInvMass = 0;
+			shared_settings->mVertices.push_back(v);
+			v.mPosition = Float3(1, 0, 0);
+			shared_settings->mVertices.push_back(v);
+			v.mPosition = Float3(0, 0, -1);
+			shared_settings->mVertices.push_back(v);
+			v.mPosition = x3;
+			v.mInvMass = 1;
+			shared_settings->mVertices.push_back(v);
+
+			// Create the 2 triangles
+			shared_settings->AddFace(SoftBodySharedSettings::Face(0, 1, 2));
+			shared_settings->AddFace(SoftBodySharedSettings::Face(0, 3, 1));
+
+			// Create edge and dihedral constraints
+			SoftBodySharedSettings::VertexAttributes va;
+			va.mShearCompliance = FLT_MAX;
+			va.mBendCompliance = 0;
+			shared_settings->CreateConstraints(&va, 1, SoftBodySharedSettings::EBendType::Dihedral);
+
+			// Optimize the settings
+			shared_settings->Optimize();
+
+			// Create the soft body
+			SoftBodyCreationSettings sb_settings(shared_settings, RVec3::sZero(), Quat::sIdentity(), Layers::MOVING);
+			sb_settings.mGravityFactor = 0.0f;
+			sb_settings.mAllowSleeping = false;
+			sb_settings.mUpdatePosition = false;
+			Body &body = *bi.CreateSoftBody(sb_settings);
+			bi.AddBody(body.GetID(), EActivation::Activate);
+			SoftBodyMotionProperties *mp = static_cast<SoftBodyMotionProperties *>(body.GetMotionProperties());
+
+			// Test 4 angles to see if there are singularities (the dot product between the triangles has the same value for 2 configurations)
+			for (float angle : { 0.0f, 90.0f, 180.0f, 270.0f })
+			{
+				// Perturb x3
+				Vec3 perturbed_x3(x3);
+				mp->GetVertex(3).mPosition = 0.5f * (Mat44::sRotationX(DegreesToRadians(angle)) * perturbed_x3);
+
+				// Simulate
+				c.Simulate(0.25f);
+
+				// Should return to the original position
+				CHECK_APPROX_EQUAL(mp->GetVertex(3).mPosition, Vec3(x3), 1.0e-3f);
+			}
+		}
+	}
+
+	// Test that applying a force to a soft body and rigid body of the same mass has the same effect
+	TEST_CASE("TestApplyForce")
+	{
+		PhysicsTestContext c;
+		PhysicsSystem *s = c.GetSystem();
+		BodyInterface &bi = s->GetBodyInterface();
+
+		// Soft body cube
+		SoftBodyCreationSettings sb_box_settings(SoftBodySharedSettings::sCreateCube(6, 0.2f), RVec3::sZero(), Quat::sIdentity(), Layers::MOVING);
+		sb_box_settings.mGravityFactor = 0.0f;
+		sb_box_settings.mLinearDamping = 0.0f;
+		Body &sb_box = *bi.CreateSoftBody(sb_box_settings);
+		BodyID sb_id = sb_box.GetID();
+		bi.AddBody(sb_id, EActivation::Activate);
+		constexpr float cMass = 216; // 6 * 6 * 6 * 1 kg
+		CHECK_APPROX_EQUAL(sb_box.GetMotionProperties()->GetInverseMass(), 1.0f / cMass);
+
+		// Rigid body cube of same size and mass
+		const RVec3 cRBBoxPos(0, 2, 0);
+		BodyCreationSettings rb_box_settings(new BoxShape(Vec3::sReplicate(0.5f)), cRBBoxPos, Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING);
+		rb_box_settings.mGravityFactor = 0.0f;
+		rb_box_settings.mLinearDamping = 0.0f;
+		rb_box_settings.mOverrideMassProperties = EOverrideMassProperties::CalculateInertia;
+		rb_box_settings.mMassPropertiesOverride.mMass = cMass;
+		Body &rb_box = *bi.CreateBody(rb_box_settings);
+		BodyID rb_id = rb_box.GetID();
+		bi.AddBody(rb_id, EActivation::Activate);
+
+		// Simulate for 3 seconds while applying the same force
+		constexpr int cNumSteps = 180;
+		const Vec3 cForce(10000.0f, 0, 0);
+		for (int i = 0; i < cNumSteps; ++i)
+		{
+			bi.AddForce(sb_id, cForce, EActivation::Activate);
+			bi.AddForce(rb_id, cForce, EActivation::Activate);
+			c.SimulateSingleStep();
+		}
+
+		// Check that the rigid body moved as expected
+		const float cTotalTime = cNumSteps * c.GetStepDeltaTime();
+		const Vec3 cAcceleration = cForce / cMass;
+		const RVec3 cExpectedPos = c.PredictPosition(cRBBoxPos, Vec3::sZero(), cAcceleration, cTotalTime);
+		CHECK_APPROX_EQUAL(rb_box.GetPosition(), cExpectedPos);
+		const Vec3 cExpectedVel = cAcceleration * cTotalTime;
+		CHECK_APPROX_EQUAL(rb_box.GetLinearVelocity(), cExpectedVel, 1.0e-3f);
+		CHECK_APPROX_EQUAL(rb_box.GetAngularVelocity(), Vec3::sZero());
+
+		// Check that the soft body moved within 1% of that
+		const RVec3 cExpectedPosSB = cExpectedPos - cRBBoxPos;
+		CHECK_APPROX_EQUAL(sb_box.GetPosition(), cExpectedPosSB, 0.01f * cExpectedPosSB.Length());
+		CHECK_APPROX_EQUAL(sb_box.GetLinearVelocity(), cExpectedVel, 2.0e-3f);
+		CHECK_APPROX_EQUAL(sb_box.GetAngularVelocity(), Vec3::sZero(), 0.01f);
+	}
+}

lib/All/JoltPhysics/UnitTests/Physics/SubShapeIDTest.cpp

@@ -0,0 +1,75 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include <Jolt/Physics/Collision/Shape/SubShapeID.h>
+
+TEST_SUITE("SubShapeIDTest")
+{
+	struct SSPair
+	{
+		uint32		mValue;
+		uint		mNumBits;
+	};
+
+	using SSPairs = Array<SSPair>;
+
+	// Helper function that pushes sub shape ID's on the creator and checks that they come out again
+	static void TestPushPop(const SSPairs &inPairs)
+	{
+		// Push all id's on the creator
+		SubShapeIDCreator creator;
+		int total_bits = 0;
+		for (const SSPair &p : inPairs)
+		{
+			creator = creator.PushID(p.mValue, p.mNumBits);
+			total_bits += p.mNumBits;
+		}
+		CHECK(creator.GetNumBitsWritten() == total_bits);
+
+		// Now pop all parts
+		SubShapeID id = creator.GetID();
+		for (const SSPair &p : inPairs)
+		{
+			// There should be data (note there is a possibility of a false positive if the bit pattern is all 1's)
+			CHECK(!id.IsEmpty());
+
+			// Pop the part
+			SubShapeID remainder;
+			uint32 value = id.PopID(p.mNumBits, remainder);
+
+			// Check value
+			CHECK(value == p.mValue);
+
+			// Continue with the remainder
+			id = remainder;
+		}
+
+		CHECK(id.IsEmpty());
+	}
+
+	TEST_CASE("SubShapeIDTest")
+	{
+		// Test storing some values
+		TestPushPop({ { 0b110101010, 9 }, { 0b0101010101, 10 }, { 0b10110101010, 11 } });
+
+		// Test storing some values with a different pattern
+		TestPushPop({ { 0b001010101, 9 }, { 0b1010101010, 10 }, { 0b01001010101, 11 } });
+
+		// Test storing up to 32 bits
+		TestPushPop({ { 0b10, 2 }, { 0b1110101010, 10 }, { 0b0101010101, 10 }, { 0b1010101010, 10 } });
+
+		// Test storing up to 32 bits with a different pattern
+		TestPushPop({ { 0b0001010101, 10 }, { 0b1010101010, 10 }, { 0b0101010101, 10 }, { 0b01, 2 } });
+
+		// Test storing 0 bits
+		TestPushPop({ { 0b10, 2 }, { 0b1110101010, 10 }, { 0, 0 }, { 0b0101010101, 10 }, { 0, 0 }, { 0b1010101010, 10 } });
+
+		// Test 32 bits at once
+		TestPushPop({ { 0b10101010101010101010101010101010, 32 } });
+
+		// Test 32 bits at once with a different pattern
+		TestPushPop({ { 0b01010101010101010101010101010101, 32 } });
+	}
+}

lib/All/JoltPhysics/UnitTests/Physics/TaperedCylinderShapeTests.cpp

@@ -0,0 +1,104 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2024 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include "PhysicsTestContext.h"
+#include <Jolt/Physics/Collision/Shape/TaperedCylinderShape.h>
+#include <Jolt/Physics/Collision/CollisionCollectorImpl.h>
+#include <Jolt/Physics/Collision/CollidePointResult.h>
+
+TEST_SUITE("TaperedCylinderShapeTests")
+{
+	TEST_CASE("TestMassAndInertia")
+	{
+		const float cDensity = 3.0f;
+		const float cRadius = 5.0f;
+		const float cHeight = 7.0f;
+
+		TaperedCylinderShapeSettings settings1(0.5f * cHeight, cRadius, 0.0f, 0.0f);
+		settings1.SetDensity(cDensity);
+
+		TaperedCylinderShapeSettings settings2(0.5f * cHeight, 0.0f, cRadius, 0.0f);
+		settings2.SetDensity(cDensity);
+
+		RefConst<TaperedCylinderShape> cylinder1 = StaticCast<TaperedCylinderShape>(settings1.Create().Get());
+		RefConst<TaperedCylinderShape> cylinder2 = StaticCast<TaperedCylinderShape>(settings2.Create().Get());
+
+		// Check accessors
+		CHECK(cylinder1->GetTopRadius() == cRadius);
+		CHECK(cylinder1->GetBottomRadius() == 0.0f);
+		CHECK(cylinder1->GetConvexRadius() == 0.0f);
+		CHECK_APPROX_EQUAL(cylinder1->GetHalfHeight(), 0.5f * cHeight);
+
+		MassProperties m1 = cylinder1->GetMassProperties();
+		MassProperties m2 = cylinder2->GetMassProperties();
+
+		// Mass/inertia is the same for both shapes because they are mirrored versions (inertia is calculated from COM)
+		CHECK_APPROX_EQUAL(m1.mMass, m2.mMass);
+		CHECK_APPROX_EQUAL(m1.mInertia, m2.mInertia);
+
+		// Center of mass for a cone is at 1/4 h (if cone runs from -h/2 to h/2)
+		// See: https://www.miniphysics.com/uy1-centre-of-mass-of-a-cone.html
+		Vec3 expected_com1(0, cHeight / 4.0f, 0);
+		Vec3 expected_com2 = -expected_com1;
+		CHECK_APPROX_EQUAL(cylinder1->GetCenterOfMass(), expected_com1);
+		CHECK_APPROX_EQUAL(cylinder2->GetCenterOfMass(), expected_com2);
+
+		// Mass of cone
+		float expected_mass = cDensity * JPH_PI * Square(cRadius) * cHeight / 3.0f;
+		CHECK_APPROX_EQUAL(expected_mass, m1.mMass);
+
+		// Inertia of cone (according to https://en.wikipedia.org/wiki/List_of_moments_of_inertia)
+		float expected_inertia_xx = expected_mass * (3.0f / 20.0f * Square(cRadius) + 3.0f / 80.0f * Square(cHeight));
+		float expected_inertia_yy = expected_mass * (3.0f / 10.0f * Square(cRadius));
+		CHECK_APPROX_EQUAL(expected_inertia_xx, m1.mInertia(0, 0), 1.0e-3f);
+		CHECK_APPROX_EQUAL(expected_inertia_yy, m1.mInertia(1, 1), 1.0e-3f);
+		CHECK_APPROX_EQUAL(expected_inertia_xx, m1.mInertia(2, 2), 1.0e-3f);
+	}
+
+	TEST_CASE("TestCollidePoint")
+	{
+		const float cTopRadius = 3.0f;
+		const float cBottomRadius = 5.0f;
+		const float cHalfHeight = 3.5f;
+
+		RefConst<Shape> shape = TaperedCylinderShapeSettings(cHalfHeight, cTopRadius, cBottomRadius).Create().Get();
+
+		auto test_inside = [shape](Vec3Arg inPoint)
+		{
+			AllHitCollisionCollector<CollidePointCollector> collector;
+			shape->CollidePoint(inPoint - shape->GetCenterOfMass(), SubShapeIDCreator(), collector);
+			CHECK(collector.mHits.size() == 1);
+		};
+
+		auto test_outside = [shape](Vec3Arg inPoint)
+		{
+			AllHitCollisionCollector<CollidePointCollector> collector;
+			shape->CollidePoint(inPoint - shape->GetCenterOfMass(), SubShapeIDCreator(), collector);
+			CHECK(collector.mHits.size() == 0);
+		};
+
+		constexpr float cEpsilon = 1.0e-3f;
+
+		test_inside(Vec3::sZero());
+
+		// Top plane
+		test_inside(Vec3(0, cHalfHeight - cEpsilon, 0));
+		test_outside(Vec3(0, cHalfHeight + cEpsilon, 0));
+
+		// Bottom plane
+		test_inside(Vec3(0, -cHalfHeight + cEpsilon, 0));
+		test_outside(Vec3(0, -cHalfHeight - cEpsilon, 0));
+
+		// COM plane
+		test_inside(Vec3(0.5f * (cTopRadius + cBottomRadius) - cEpsilon, 0, 0));
+		test_outside(Vec3(0.5f * (cTopRadius + cBottomRadius) + cEpsilon, 0, 0));
+
+		// At quarter h above COM plane
+		float h = 0.5f * cHalfHeight;
+		float r = cBottomRadius + (cTopRadius - cBottomRadius) * (h + cHalfHeight) / (2.0f * cHalfHeight);
+		test_inside(Vec3(0, h, r - cEpsilon));
+		test_outside(Vec3(0, h, r + cEpsilon));
+	}
+}

lib/All/JoltPhysics/UnitTests/Physics/TransformedShapeTests.cpp

@@ -0,0 +1,105 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include "PhysicsTestContext.h"
+#include <Jolt/Physics/Collision/Shape/ScaledShape.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h>
+#include <Jolt/Physics/Collision/TransformedShape.h>
+#include <Jolt/Physics/Collision/CollisionCollectorImpl.h>
+#include <Jolt/Physics/Collision/RayCast.h>
+#include <Jolt/Physics/Collision/CastResult.h>
+#include <Jolt/Physics/Collision/PhysicsMaterialSimple.h>
+
+TEST_SUITE("TransformedShapeTests")
+{
+	TEST_CASE("TestTransformedShape")
+	{
+		const Vec3 half_extents(0.5f, 1.0f, 1.5f);
+		const Vec3 scale(-2, 3, 4);
+		const Vec3 rtshape_translation(1, 3, 5);
+		const Quat rtshape_rotation = Quat::sRotation(Vec3(1, 2, 3).Normalized(), 0.25f * JPH_PI);
+		const RVec3 translation(13, 9, 7);
+		const Quat rotation = Quat::sRotation(Vec3::sAxisY(), 0.5f * JPH_PI); // A rotation of 90 degrees in order to not shear the shape
+
+		PhysicsMaterialSimple *material = new PhysicsMaterialSimple("Test Material", Color::sRed);
+
+		// Create a scaled, rotated and translated box
+		BoxShapeSettings box_settings(half_extents, 0.0f, material);
+		box_settings.SetEmbedded();
+		ScaledShapeSettings scale_settings(&box_settings, scale);
+		scale_settings.SetEmbedded();
+		RotatedTranslatedShapeSettings rtshape_settings(rtshape_translation, rtshape_rotation, &scale_settings);
+		rtshape_settings.SetEmbedded();
+
+		// Create a body with this shape
+		PhysicsTestContext c;
+		Body &body = c.CreateBody(&rtshape_settings, translation, rotation, EMotionType::Static, EMotionQuality::Discrete, 0, EActivation::DontActivate);
+
+		// Collect the leaf shape transform
+		AllHitCollisionCollector<TransformedShapeCollector> collector;
+		c.GetSystem()->GetNarrowPhaseQuery().CollectTransformedShapes(AABox::sBiggest(), collector);
+
+		// Check that there is exactly 1 shape
+		CHECK(collector.mHits.size() == 1);
+		TransformedShape &ts = collector.mHits.front();
+
+		// Check that we got the leaf shape: box
+		CHECK(ts.mShape == box_settings.Create().Get());
+
+		// Check that its transform matches the transform that we provided
+		RMat44 calc_transform = RMat44::sRotationTranslation(rotation, translation) * Mat44::sRotationTranslation(rtshape_rotation, rtshape_translation) * RMat44::sScale(scale);
+		CHECK_APPROX_EQUAL(calc_transform, ts.GetWorldTransform());
+
+		// Check that all corner points are in the bounding box
+		AABox aabox = ts.GetWorldSpaceBounds();
+		Vec3 corners[] = {
+			Vec3(-0.99f, -0.99f, -0.99f) * half_extents,
+			Vec3( 0.99f, -0.99f, -0.99f) * half_extents,
+			Vec3(-0.99f,  0.99f, -0.99f) * half_extents,
+			Vec3( 0.99f,  0.99f, -0.99f) * half_extents,
+			Vec3(-0.99f, -0.99f,  0.99f) * half_extents,
+			Vec3( 0.99f, -0.99f,  0.99f) * half_extents,
+			Vec3(-0.99f,  0.99f,  0.99f) * half_extents,
+			Vec3( 0.99f,  0.99f,  0.99f) * half_extents
+		};
+		for (Vec3 corner : corners)
+		{
+			CHECK(aabox.Contains(calc_transform * corner));
+			CHECK(!aabox.Contains(calc_transform * (2 * corner))); // Check that points twice as far away are not in the box
+		}
+
+		// Now pick a point on the box near the edge in local space, determine a raycast that hits it
+		const Vec3 point_on_box(half_extents.GetX() - 0.01f, half_extents.GetY() - 0.01f, half_extents.GetZ());
+		const Vec3 normal_on_box(0, 0, 1);
+		const Vec3 ray_direction_local(1, 1, -1);
+
+		// Transform to world space and do the raycast
+		Vec3 ray_start_local = point_on_box - ray_direction_local;
+		Vec3 ray_end_local = point_on_box + ray_direction_local;
+		RVec3 ray_start_world = calc_transform * ray_start_local;
+		RVec3 ray_end_world = calc_transform * ray_end_local;
+		Vec3 ray_direction_world = Vec3(ray_end_world - ray_start_world);
+		RRayCast ray_in_world { ray_start_world, ray_direction_world };
+		RayCastResult hit;
+		ts.CastRay(ray_in_world, hit);
+
+		// Check the hit result
+		CHECK_APPROX_EQUAL(hit.mFraction, 0.5f);
+		CHECK(hit.mBodyID == body.GetID());
+		CHECK(ts.GetMaterial(hit.mSubShapeID2) == material);
+		Vec3 world_space_normal = ts.GetWorldSpaceSurfaceNormal(hit.mSubShapeID2, ray_in_world.GetPointOnRay(hit.mFraction));
+		Vec3 expected_normal = (calc_transform.GetDirectionPreservingMatrix() * normal_on_box).Normalized();
+		CHECK_APPROX_EQUAL(world_space_normal, expected_normal);
+
+		// Reset the transform to identity and check that it worked
+		ts.SetWorldTransform(RMat44::sIdentity());
+		CHECK_APPROX_EQUAL(ts.GetWorldTransform(), RMat44::sIdentity());
+
+		// Set the calculated world transform again to see if getting/setting a transform is symmetric
+		ts.SetWorldTransform(calc_transform);
+		CHECK_APPROX_EQUAL(calc_transform, ts.GetWorldTransform());
+	}
+}

lib/All/JoltPhysics/UnitTests/Physics/WheeledVehicleTests.cpp

@@ -0,0 +1,347 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2022 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include "PhysicsTestContext.h"
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Collision/Shape/OffsetCenterOfMassShape.h>
+#include <Jolt/Physics/Vehicle/WheeledVehicleController.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include "Layers.h"
+
+TEST_SUITE("WheeledVehicleTests")
+{
+	enum
+	{
+		FL_WHEEL,
+		FR_WHEEL,
+		BL_WHEEL,
+		BR_WHEEL
+	};
+
+	// Simplified vehicle settings
+	struct VehicleSettings
+	{
+		RVec3		mPosition { 0, 2, 0 };
+		bool		mUseCastSphere = true;
+		float		mWheelRadius = 0.3f;
+		float		mWheelWidth = 0.1f;
+		float		mHalfVehicleLength = 2.0f;
+		float		mHalfVehicleWidth = 0.9f;
+		float		mHalfVehicleHeight = 0.2f;
+		float		mWheelOffsetHorizontal = 1.4f;
+		float		mWheelOffsetVertical = 0.18f;
+		float		mSuspensionMinLength = 0.3f;
+		float		mSuspensionMaxLength = 0.5f;
+		float		mMaxSteeringAngle = DegreesToRadians(30);
+		bool		mFourWheelDrive = false;
+		float		mFrontBackLimitedSlipRatio = 1.4f;
+		float		mLeftRightLimitedSlipRatio = 1.4f;
+		bool		mAntiRollbar = true;
+	};
+
+	// Helper function to create a vehicle
+	static VehicleConstraint *AddVehicle(PhysicsTestContext &inContext, VehicleSettings &inSettings)
+	{
+		// Create vehicle body
+		RefConst<Shape> car_shape = OffsetCenterOfMassShapeSettings(Vec3(0, -inSettings.mHalfVehicleHeight, 0), new BoxShape(Vec3(inSettings.mHalfVehicleWidth, inSettings.mHalfVehicleHeight, inSettings.mHalfVehicleLength))).Create().Get();
+		BodyCreationSettings car_body_settings(car_shape, inSettings.mPosition, Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING);
+		car_body_settings.mOverrideMassProperties = EOverrideMassProperties::CalculateInertia;
+		car_body_settings.mMassPropertiesOverride.mMass = 1500.0f;
+		Body *car_body = inContext.GetBodyInterface().CreateBody(car_body_settings);
+		inContext.GetBodyInterface().AddBody(car_body->GetID(), EActivation::Activate);
+
+		// Create vehicle constraint
+		VehicleConstraintSettings vehicle;
+		vehicle.mDrawConstraintSize = 0.1f;
+		vehicle.mMaxPitchRollAngle = DegreesToRadians(60.0f);
+
+		// Wheels
+		WheelSettingsWV *fl = new WheelSettingsWV;
+		fl->mPosition = Vec3(inSettings.mHalfVehicleWidth, -inSettings.mWheelOffsetVertical, inSettings.mWheelOffsetHorizontal);
+		fl->mMaxSteerAngle = inSettings.mMaxSteeringAngle;
+		fl->mMaxHandBrakeTorque = 0.0f; // Front wheel doesn't have hand brake
+
+		WheelSettingsWV *fr = new WheelSettingsWV;
+		fr->mPosition = Vec3(-inSettings.mHalfVehicleWidth, -inSettings.mWheelOffsetVertical, inSettings.mWheelOffsetHorizontal);
+		fr->mMaxSteerAngle = inSettings.mMaxSteeringAngle;
+		fr->mMaxHandBrakeTorque = 0.0f; // Front wheel doesn't have hand brake
+
+		WheelSettingsWV *bl = new WheelSettingsWV;
+		bl->mPosition = Vec3(inSettings.mHalfVehicleWidth, -inSettings.mWheelOffsetVertical, -inSettings.mWheelOffsetHorizontal);
+		bl->mMaxSteerAngle = 0.0f;
+
+		WheelSettingsWV *br = new WheelSettingsWV;
+		br->mPosition = Vec3(-inSettings.mHalfVehicleWidth, -inSettings.mWheelOffsetVertical, -inSettings.mWheelOffsetHorizontal);
+		br->mMaxSteerAngle = 0.0f;
+
+		vehicle.mWheels.resize(4);
+		vehicle.mWheels[FL_WHEEL] = fl;
+		vehicle.mWheels[FR_WHEEL] = fr;
+		vehicle.mWheels[BL_WHEEL] = bl;
+		vehicle.mWheels[BR_WHEEL] = br;
+
+		for (WheelSettings *w : vehicle.mWheels)
+		{
+			w->mRadius = inSettings.mWheelRadius;
+			w->mWidth = inSettings.mWheelWidth;
+			w->mSuspensionMinLength = inSettings.mSuspensionMinLength;
+			w->mSuspensionMaxLength = inSettings.mSuspensionMaxLength;
+		}
+
+		WheeledVehicleControllerSettings *controller = new WheeledVehicleControllerSettings;
+		vehicle.mController = controller;
+
+		// Differential
+		controller->mDifferentials.resize(inSettings.mFourWheelDrive? 2 : 1);
+		controller->mDifferentials[0].mLeftWheel = FL_WHEEL;
+		controller->mDifferentials[0].mRightWheel = FR_WHEEL;
+		controller->mDifferentials[0].mLimitedSlipRatio = inSettings.mLeftRightLimitedSlipRatio;
+		controller->mDifferentialLimitedSlipRatio = inSettings.mFrontBackLimitedSlipRatio;
+		if (inSettings.mFourWheelDrive)
+		{
+			controller->mDifferentials[1].mLeftWheel = BL_WHEEL;
+			controller->mDifferentials[1].mRightWheel = BR_WHEEL;
+			controller->mDifferentials[1].mLimitedSlipRatio = inSettings.mLeftRightLimitedSlipRatio;
+
+			// Split engine torque
+			controller->mDifferentials[0].mEngineTorqueRatio = controller->mDifferentials[1].mEngineTorqueRatio = 0.5f;
+		}
+
+		// Anti rollbars
+		if (inSettings.mAntiRollbar)
+		{
+			vehicle.mAntiRollBars.resize(2);
+			vehicle.mAntiRollBars[0].mLeftWheel = FL_WHEEL;
+			vehicle.mAntiRollBars[0].mRightWheel = FR_WHEEL;
+			vehicle.mAntiRollBars[1].mLeftWheel = BL_WHEEL;
+			vehicle.mAntiRollBars[1].mRightWheel = BR_WHEEL;
+		}
+
+		// Create the constraint
+		VehicleConstraint *constraint = new VehicleConstraint(*car_body, vehicle);
+
+		// Create collision tester
+		RefConst<VehicleCollisionTester> tester;
+		if (inSettings.mUseCastSphere)
+			tester = new VehicleCollisionTesterCastSphere(Layers::MOVING, 0.5f * inSettings.mWheelWidth);
+		else
+			tester = new VehicleCollisionTesterRay(Layers::MOVING);
+		constraint->SetVehicleCollisionTester(tester);
+
+		// Add to the world
+		inContext.GetSystem()->AddConstraint(constraint);
+		inContext.GetSystem()->AddStepListener(constraint);
+		return constraint;
+	}
+
+	static void CheckOnGround(VehicleConstraint *inConstraint, const VehicleSettings &inSettings, const BodyID &inGroundID)
+	{
+		// Between min and max suspension length
+		RVec3 pos = inConstraint->GetVehicleBody()->GetPosition();
+		CHECK(pos.GetY() > inSettings.mSuspensionMinLength + inSettings.mWheelOffsetVertical + inSettings.mHalfVehicleHeight);
+		CHECK(pos.GetY() < inSettings.mSuspensionMaxLength + inSettings.mWheelOffsetVertical + inSettings.mHalfVehicleHeight);
+
+		// Wheels touching ground
+		for (const Wheel *w : inConstraint->GetWheels())
+			CHECK(w->GetContactBodyID() == inGroundID);
+	}
+
+	TEST_CASE("TestBasicWheeledVehicle")
+	{
+		PhysicsTestContext c;
+		BodyID floor_id = c.CreateFloor().GetID();
+
+		VehicleSettings settings;
+		VehicleConstraint *constraint = AddVehicle(c, settings);
+		Body *body = constraint->GetVehicleBody();
+		WheeledVehicleController *controller = static_cast<WheeledVehicleController *>(constraint->GetController());
+
+		// Should start at specified position
+		CHECK_APPROX_EQUAL(body->GetPosition(), settings.mPosition);
+
+		// After 1 step we should not be at ground yet
+		c.SimulateSingleStep();
+		for (const Wheel *w : constraint->GetWheels())
+			CHECK(w->GetContactBodyID().IsInvalid());
+		CHECK(controller->GetTransmission().GetCurrentGear() == 0);
+
+		// After 1 second we should be on ground but not moving horizontally
+		c.Simulate(1.0f);
+		CheckOnGround(constraint, settings, floor_id);
+		RVec3 pos1 = body->GetPosition();
+		CHECK_APPROX_EQUAL(pos1.GetX(), 0); // Not moving horizontally
+		CHECK_APPROX_EQUAL(pos1.GetZ(), 0);
+		CHECK(controller->GetTransmission().GetCurrentGear() == 0);
+
+		// Start driving forward
+		controller->SetDriverInput(1.0f, 0.0f, 0.0f, 0.0f);
+		c.GetBodyInterface().ActivateBody(body->GetID());
+		c.Simulate(2.0f);
+		CheckOnGround(constraint, settings, floor_id);
+		RVec3 pos2 = body->GetPosition();
+		CHECK_APPROX_EQUAL(pos2.GetX(), 0, 1.0e-2_r); // Not moving left/right
+		CHECK(pos2.GetZ() > pos1.GetZ() + 1.0f); // Moving in Z direction
+		Vec3 vel = body->GetLinearVelocity();
+		CHECK_APPROX_EQUAL(vel.GetX(), 0, 2.0e-2f); // Not moving left/right
+		CHECK(vel.GetZ() > 1.0f); // Moving in Z direction
+		CHECK(controller->GetTransmission().GetCurrentGear() > 0);
+
+		// Brake
+		controller->SetDriverInput(0.0f, 0.0f, 1.0f, 0.0f);
+		c.GetBodyInterface().ActivateBody(body->GetID());
+		c.Simulate(5.0f);
+		CheckOnGround(constraint, settings, floor_id);
+		CHECK(!body->IsActive()); // Car should have gone to sleep
+		RVec3 pos3 = body->GetPosition();
+		CHECK_APPROX_EQUAL(pos3.GetX(), 0, 2.0e-2_r); // Not moving left/right
+		CHECK(pos3.GetZ() > pos2.GetZ() + 1.0f); // Moving in Z direction while braking
+		vel = body->GetLinearVelocity();
+		CHECK_APPROX_EQUAL(vel, Vec3::sZero(), 1.0e-3f); // Not moving
+
+		// Start driving backwards
+		controller->SetDriverInput(-1.0f, 0.0f, 0.0f, 0.0f);
+		c.GetBodyInterface().ActivateBody(body->GetID());
+		c.Simulate(2.0f);
+		CheckOnGround(constraint, settings, floor_id);
+		RVec3 pos4 = body->GetPosition();
+		CHECK_APPROX_EQUAL(pos4.GetX(), 0, 3.0e-2_r); // Not moving left/right
+		CHECK(pos4.GetZ() < pos3.GetZ() - 1.0f); // Moving in -Z direction
+		vel = body->GetLinearVelocity();
+		CHECK_APPROX_EQUAL(vel.GetX(), 0, 5.0e-2f); // Not moving left/right
+		CHECK(vel.GetZ() < -1.0f); // Moving in -Z direction
+		CHECK(controller->GetTransmission().GetCurrentGear() < 0);
+
+		// Brake
+		controller->SetDriverInput(0.0f, 0.0f, 1.0f, 0.0f);
+		c.GetBodyInterface().ActivateBody(body->GetID());
+		c.Simulate(5.0f);
+		CheckOnGround(constraint, settings, floor_id);
+		CHECK(!body->IsActive()); // Car should have gone to sleep
+		RVec3 pos5 = body->GetPosition();
+		CHECK_APPROX_EQUAL(pos5.GetX(), 0, 7.0e-2_r); // Not moving left/right
+		CHECK(pos5.GetZ() < pos4.GetZ() - 1.0f); // Moving in -Z direction while braking
+		vel = body->GetLinearVelocity();
+		CHECK_APPROX_EQUAL(vel, Vec3::sZero(), 1.0e-3f); // Not moving
+
+		// Turn right
+		controller->SetDriverInput(1.0f, 1.0f, 0.0f, 0.0f);
+		c.GetBodyInterface().ActivateBody(body->GetID());
+		c.Simulate(2.0f);
+		CheckOnGround(constraint, settings, floor_id);
+		Vec3 omega = body->GetAngularVelocity();
+		CHECK(omega.GetY() < -0.4f); // Rotating right
+		CHECK(controller->GetTransmission().GetCurrentGear() > 0);
+
+		// Hand brake
+		controller->SetDriverInput(0.0f, 0.0f, 0.0f, 1.0f);
+		c.GetBodyInterface().ActivateBody(body->GetID());
+		c.Simulate(7.0f);
+		CheckOnGround(constraint, settings, floor_id);
+		CHECK(!body->IsActive()); // Car should have gone to sleep
+		vel = body->GetLinearVelocity();
+		CHECK_APPROX_EQUAL(vel, Vec3::sZero(), 1.0e-3f); // Not moving
+
+		// Turn left
+		controller->SetDriverInput(1.0f, -1.0f, 0.0f, 0.0f);
+		c.GetBodyInterface().ActivateBody(body->GetID());
+		c.Simulate(2.0f);
+		CheckOnGround(constraint, settings, floor_id);
+		omega = body->GetAngularVelocity();
+		CHECK(omega.GetY() > 0.4f); // Rotating left
+		CHECK(controller->GetTransmission().GetCurrentGear() > 0);
+	}
+
+	TEST_CASE("TestLSDifferential")
+	{
+		struct Test
+		{
+			RVec3	mBlockPosition;		// Location of the box under the vehicle
+			bool	mFourWheelDrive;	// 4WD or not
+			float	mFBLSRatio;			// Limited slip ratio front-back
+			float	mLRLSRatio;			// Limited slip ratio left-right
+			bool	mFLHasContactPre;	// Which wheels should be in contact with the ground prior to the test
+			bool	mFRHasContactPre;
+			bool	mBLHasContactPre;
+			bool	mBRHasContactPre;
+			bool	mShouldMove;		// If the vehicle should be able to drive off the block
+		};
+
+		Test tests[] = {
+			// Block Position,			4WD,	FBSlip,		LRSlip		FLPre,	FRPre,	BLPre,	BRPre,	ShouldMove
+			{ RVec3(1, 0.5f, 0),		true,	FLT_MAX,	FLT_MAX,	false,	true,	false,	true,	false	},		// Block left, no limited slip -> vehicle can't move
+			{ RVec3(1, 0.5f, 0),		true,	1.4f,		FLT_MAX,	false,	true,	false,	true,	false	},		// Block left, only FB limited slip -> vehicle can't move
+			{ RVec3(1, 0.5f, 0),		true,	1.4f,		1.4f,		false,	true,	false,	true,	true	},		// Block left, limited slip -> vehicle drives off
+			{ RVec3(-1, 0.5f, 0),		true,	FLT_MAX,	FLT_MAX,	true,	false,	true,	false,	false	},		// Block right, no limited slip -> vehicle can't move
+			{ RVec3(-1, 0.5f, 0),		true,	1.4f,		FLT_MAX,	true,	false,	true,	false,	false	},		// Block right, only FB limited slip -> vehicle can't move
+			{ RVec3(-1, 0.5f, 0),		true,	1.4f,		1.4f,		true,	false,	true,	false,	true	},		// Block right, limited slip -> vehicle drives off
+			{ RVec3(0, 0.5f, 1.5f),		true,	FLT_MAX,	FLT_MAX,	false,	false,	true,	true,	false	},		// Block front, no limited slip -> vehicle can't move
+			{ RVec3(0, 0.5f, 1.5f),		true,	1.4f,		FLT_MAX,	false,	false,	true,	true,	true	},		// Block front, only FB limited slip -> vehicle drives off
+			{ RVec3(0, 0.5f, 1.5f),		true,	1.4f,		1.4f,		false,	false,	true,	true,	true	},		// Block front, limited slip -> vehicle drives off
+			{ RVec3(0, 0.5f, 1.5f),		false,	1.4f,		1.4f,		false,	false,	true,	true,	false	},		// Block front, limited slip, 2WD -> vehicle can't move
+			{ RVec3(0, 0.5f, -1.5f),	true,	FLT_MAX,	FLT_MAX,	true,	true,	false,	false,	false	},		// Block back, no limited slip -> vehicle can't move
+			{ RVec3(0, 0.5f, -1.5f),	true,	1.4f,		FLT_MAX,	true,	true,	false,	false,	true	},		// Block back, only FB limited slip -> vehicle drives off
+			{ RVec3(0, 0.5f, -1.5f),	true,	1.4f,		1.4f,		true,	true,	false,	false,	true	},		// Block back, limited slip -> vehicle drives off
+			{ RVec3(0, 0.5f, -1.5f),	false,	1.4f,		1.4f,		true,	true,	false,	false,	true	},		// Block back, limited slip, 2WD -> vehicle drives off
+		};
+
+		for (Test &t : tests)
+		{
+			PhysicsTestContext c;
+			BodyID floor_id = c.CreateFloor().GetID();
+
+			// Box under left side of the vehicle, left wheels won't be touching the ground
+			Body &box = c.CreateBox(t.mBlockPosition, Quat::sIdentity(), EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, Vec3::sReplicate(0.5f));
+			box.SetFriction(1.0f);
+
+			// Create vehicle
+			VehicleSettings settings;
+			settings.mFourWheelDrive = t.mFourWheelDrive;
+			settings.mFrontBackLimitedSlipRatio = t.mFBLSRatio;
+			settings.mLeftRightLimitedSlipRatio = t.mLRLSRatio;
+
+			VehicleConstraint *constraint = AddVehicle(c, settings);
+			Body *body = constraint->GetVehicleBody();
+			WheeledVehicleController *controller = static_cast<WheeledVehicleController *>(constraint->GetController());
+
+			// Give the wheels extra grip
+			controller->SetTireMaxImpulseCallback(
+				[](uint, float &outLongitudinalImpulse, float &outLateralImpulse, float inSuspensionImpulse, float inLongitudinalFriction, float inLateralFriction, float, float, float)
+				{
+					outLongitudinalImpulse = 10.0f * inLongitudinalFriction * inSuspensionImpulse;
+					outLateralImpulse = inLateralFriction * inSuspensionImpulse;
+				});
+
+			// Simulate till vehicle rests on block
+			bool vehicle_on_floor = false;
+			for (float time = 0; time < 2.0f; time += c.GetDeltaTime())
+			{
+				c.SimulateSingleStep();
+
+				// Check pre condition
+				if ((constraint->GetWheel(FL_WHEEL)->GetContactBodyID() == (t.mFLHasContactPre? floor_id : BodyID()))
+					&& (constraint->GetWheel(FR_WHEEL)->GetContactBodyID() == (t.mFRHasContactPre? floor_id : BodyID()))
+					&& (constraint->GetWheel(BL_WHEEL)->GetContactBodyID() == (t.mBLHasContactPre? floor_id : BodyID()))
+					&& (constraint->GetWheel(BR_WHEEL)->GetContactBodyID() == (t.mBRHasContactPre? floor_id : BodyID())))
+				{
+					vehicle_on_floor = true;
+					break;
+				}
+			}
+			CHECK(vehicle_on_floor);
+			CHECK_APPROX_EQUAL(body->GetPosition().GetZ(), 0, 0.03_r);
+
+			// Start driving
+			controller->SetDriverInput(1.0f, 0, 0, 0);
+			c.GetBodyInterface().ActivateBody(body->GetID());
+			c.Simulate(2.0f);
+
+			// Check if vehicle had traction
+			if (t.mShouldMove)
+				CHECK(body->GetPosition().GetZ() > 0.5f);
+			else
+				CHECK_APPROX_EQUAL(body->GetPosition().GetZ(), 0, 0.06_r);
+		}
+	}
+}