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

lib/All/JoltPhysics/Samples/Tests/BroadPhase/BroadPhaseCastRayTest.cpp

@@ -0,0 +1,58 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/BroadPhase/BroadPhaseCastRayTest.h>
+#include <Jolt/Physics/Collision/RayCast.h>
+#include <Jolt/Physics/Collision/CastResult.h>
+#include <Jolt/Physics/Collision/CollisionCollectorImpl.h>
+#include <Utils/DebugRendererSP.h>
+#include <random>
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(BroadPhaseCastRayTest)
+{
+	JPH_ADD_BASE_CLASS(BroadPhaseCastRayTest, BroadPhaseTest)
+}
+
+void BroadPhaseCastRayTest::Initialize()
+{
+	BroadPhaseTest::Initialize();
+
+	int num_bodies = int(mBodyManager->GetMaxBodies());
+
+	// Create random boxes
+	CreateBalancedDistribution(mBodyManager, num_bodies);
+
+	// Add all bodies to the broadphase
+	Body **body_vector = mBodyManager->GetBodies().data();
+	BodyID *bodies_to_add = new BodyID [num_bodies];
+	for (int b = 0; b < num_bodies; ++b)
+		bodies_to_add[b] = body_vector[b]->GetID();
+	BroadPhase::AddState add_state = mBroadPhase->AddBodiesPrepare(bodies_to_add, num_bodies);
+	mBroadPhase->AddBodiesFinalize(bodies_to_add, num_bodies, add_state);
+	delete [] bodies_to_add;
+
+	// Optimize the broadphase
+	mBroadPhase->Optimize();
+}
+
+void BroadPhaseCastRayTest::PrePhysicsUpdate(const PreUpdateParams &inParams)
+{
+	// Create ray
+	default_random_engine random;
+	Vec3 from = 1000.0f * Vec3::sRandom(random);
+	RayCast ray { from, -2.0f * from };
+
+	// Raycast before update
+	AllHitCollisionCollector<RayCastBodyCollector> collector;
+	mBroadPhase->CastRay(ray, collector);
+	int num_hits = (int)collector.mHits.size();
+	BroadPhaseCastResult *results = collector.mHits.data();
+
+	// Draw results
+	for (int i = 0; i < num_hits; ++i)
+		DrawMarkerSP(mDebugRenderer, ray.GetPointOnRay(results[i].mFraction), Color::sGreen, 10.0f);
+	DrawLineSP(mDebugRenderer, ray.mOrigin, ray.mOrigin + ray.mDirection, Color::sRed);
+}

lib/All/JoltPhysics/Samples/Tests/BroadPhase/BroadPhaseCastRayTest.h

@@ -0,0 +1,25 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/BroadPhase/BroadPhaseTest.h>
+
+class BroadPhaseCastRayTest : public BroadPhaseTest
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, BroadPhaseCastRayTest)
+
+	// Description of the test
+	virtual const char *GetDescription() const override
+	{
+		return "Simple test that casts a ray through the broadphase.";
+	}
+
+	// Initialize the test
+	virtual void	Initialize() override;
+
+	// Update the test, called before the physics update
+	virtual void	PrePhysicsUpdate(const PreUpdateParams &inParams) override;
+};

lib/All/JoltPhysics/Samples/Tests/BroadPhase/BroadPhaseInsertionTest.cpp

@@ -0,0 +1,151 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/BroadPhase/BroadPhaseInsertionTest.h>
+#include <Jolt/Physics/Collision/RayCast.h>
+#include <Jolt/Physics/Collision/CastResult.h>
+#include <Jolt/Physics/Collision/CollisionCollectorImpl.h>
+#include <Jolt/Geometry/RayAABox.h>
+#include <Utils/Log.h>
+#include <Utils/DebugRendererSP.h>
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(BroadPhaseInsertionTest)
+{
+	JPH_ADD_BASE_CLASS(BroadPhaseInsertionTest, BroadPhaseTest)
+}
+
+void BroadPhaseInsertionTest::Initialize()
+{
+	BroadPhaseTest::Initialize();
+
+	CreateBalancedDistribution(mBodyManager, mBodyManager->GetMaxBodies());
+}
+
+void BroadPhaseInsertionTest::PrePhysicsUpdate(const PreUpdateParams &inParams)
+{
+	// Check if we need to change direction
+	if (mDirection == 1 && mCurrentBody >= mBodyManager->GetMaxBodies())
+		mDirection = -1;
+	if (mDirection == -1 && mCurrentBody == 0)
+		mDirection = 1;
+
+	int num_this_step = int(mBodyManager->GetMaxBodies() / 10);
+
+	if (mDirection < 0)
+		mCurrentBody -= num_this_step;
+
+	Body **body_vector = mBodyManager->GetBodies().data();
+
+	// Randomly move bodies around
+	if (mCurrentBody > 0)
+	{
+		const int cNumBodiesToMove = 100;
+		BodyID *bodies_to_move = new BodyID [cNumBodiesToMove];
+		uniform_int_distribution<int> body_selector(0, (int)mCurrentBody - 1);
+		uniform_real_distribution<float> translation_selector(1.0f, 5.0f);
+		for (int i = 0; i < cNumBodiesToMove; ++i)
+		{
+			Body &body = *body_vector[body_selector(mRandomGenerator)];
+			JPH_ASSERT(body.IsInBroadPhase());
+			body.SetPositionAndRotationInternal(body.GetPosition() + translation_selector(mRandomGenerator) * Vec3::sRandom(mRandomGenerator), Quat::sIdentity());
+			bodies_to_move[i] = body.GetID();
+		}
+		mBroadPhase->NotifyBodiesAABBChanged(bodies_to_move, cNumBodiesToMove);
+		delete [] bodies_to_move;
+	}
+
+	// Create batch of bodies
+	BodyID *bodies_to_add_or_remove = new BodyID [num_this_step];
+	for (int b = 0; b < num_this_step; ++b)
+		bodies_to_add_or_remove[b] = body_vector[mCurrentBody + b]->GetID();
+
+	// Add/remove them
+	if (mDirection == 1)
+	{
+		// Prepare and abort
+		BroadPhase::AddState add_state = mBroadPhase->AddBodiesPrepare(bodies_to_add_or_remove, num_this_step);
+		mBroadPhase->AddBodiesAbort(bodies_to_add_or_remove, num_this_step, add_state);
+
+		// Prepare and add
+		add_state = mBroadPhase->AddBodiesPrepare(bodies_to_add_or_remove, num_this_step);
+		mBroadPhase->AddBodiesFinalize(bodies_to_add_or_remove, num_this_step, add_state);
+	}
+	else
+		mBroadPhase->RemoveBodies(bodies_to_add_or_remove, num_this_step);
+
+	// Delete temp array
+	delete [] bodies_to_add_or_remove;
+
+	// Create ray
+	default_random_engine random;
+	Vec3 from = 1000.0f * Vec3::sRandom(random);
+	RayCast ray { from, -2.0f * from };
+
+	// Raycast before update
+	AllHitCollisionCollector<RayCastBodyCollector> hits_before;
+	mBroadPhase->CastRay(ray, hits_before);
+	int num_before = (int)hits_before.mHits.size();
+	BroadPhaseCastResult *results_before = hits_before.mHits.data();
+	Trace("Before update: %d results found", num_before);
+
+	// Draw results
+	DrawLineSP(mDebugRenderer, ray.mOrigin, ray.mOrigin + ray.mDirection, Color::sRed);
+	for (int i = 0; i < num_before; ++i)
+		DrawMarkerSP(mDebugRenderer, ray.GetPointOnRay(results_before[i].mFraction), Color::sGreen, 10.0f);
+
+	// Update the broadphase
+	mBroadPhase->Optimize();
+
+	// Raycast after update
+	AllHitCollisionCollector<RayCastBodyCollector> hits_after;
+	mBroadPhase->CastRay(ray, hits_after);
+	int num_after = (int)hits_after.mHits.size();
+	BroadPhaseCastResult *results_after = hits_after.mHits.data();
+	Trace("After update: %d results found", num_after);
+
+	// Before update we may have some false hits, check that there are less hits after update than before
+	if (num_after > num_before)
+		FatalError("BroadPhaseInsertionTest: After has more hits than before");
+	for (BroadPhaseCastResult *ra = results_after, *ra_end = results_after + num_after; ra < ra_end; ++ra)
+	{
+		bool found = false;
+		for (BroadPhaseCastResult *rb = results_before, *rb_end = results_before + num_before; rb < rb_end; ++rb)
+			if (ra->mBodyID == rb->mBodyID)
+			{
+				found = true;
+				break;
+			}
+		if (!found)
+			FatalError("BroadPhaseInsertionTest: Result after not found in result before");
+	}
+
+	// Validate with brute force approach
+	for (const Body *b : mBodyManager->GetBodies())
+	{
+		bool found = false;
+		for (BroadPhaseCastResult *r = results_after, *r_end = results_after + num_after; r < r_end; ++r)
+			if (r->mBodyID == b->GetID())
+			{
+				found = true;
+				break;
+			}
+
+		if (b->IsInBroadPhase()
+			&& RayAABoxHits(ray.mOrigin, ray.mDirection, b->GetWorldSpaceBounds().mMin, b->GetWorldSpaceBounds().mMax))
+		{
+			if (!found)
+				FatalError("BroadPhaseInsertionTest: Is intersecting but was not found");
+		}
+		else
+		{
+			if (found)
+				FatalError("BroadPhaseInsertionTest: Is not intersecting but was found");
+		}
+	}
+
+	if (mDirection > 0)
+		mCurrentBody += num_this_step;
+}

lib/All/JoltPhysics/Samples/Tests/BroadPhase/BroadPhaseInsertionTest.h

@@ -0,0 +1,31 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/BroadPhase/BroadPhaseTest.h>
+#include <random>
+
+class BroadPhaseInsertionTest : public BroadPhaseTest
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, BroadPhaseInsertionTest)
+
+	// Description of the test
+	virtual const char *	GetDescription() const override
+	{
+		return "Test that adds/removes objects to/from the broadphase and casts a ray through the boxes to see if the collision results are correct.";
+	}
+
+	// Initialize the test
+	virtual void			Initialize() override;
+
+	// Update the test, called before the physics update
+	virtual void			PrePhysicsUpdate(const PreUpdateParams &inParams) override;
+
+private:
+	default_random_engine	mRandomGenerator;
+	size_t					mCurrentBody = 0;
+	int						mDirection = 1;
+};

lib/All/JoltPhysics/Samples/Tests/BroadPhase/BroadPhaseTest.cpp

@@ -0,0 +1,70 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/BroadPhase/BroadPhaseTest.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Jolt/Physics/Collision/BroadPhase/BroadPhaseBruteForce.h>
+#include <Jolt/Physics/Collision/BroadPhase/BroadPhaseQuadTree.h>
+#include <random>
+
+JPH_IMPLEMENT_RTTI_ABSTRACT(BroadPhaseTest)
+{
+	JPH_ADD_BASE_CLASS(BroadPhaseTest, Test)
+}
+
+#define NUM_BODIES		10000
+
+//#define BROAD_PHASE		BroadPhaseBruteForce()
+#define BROAD_PHASE		BroadPhaseQuadTree()
+
+BroadPhaseTest::~BroadPhaseTest()
+{
+	delete mBroadPhase;
+	delete mBodyManager;
+}
+
+void BroadPhaseTest::CreateBalancedDistribution(BodyManager *inBodyManager, int inNumBodies, float inEnvironmentSize)
+{
+	default_random_engine random(0x1ee7c0de);
+	uniform_real_distribution<float> zero_to_one(0.0f, 1.0f);
+	float n = float(inNumBodies);
+	Vec3 max_box_start = Vec3::sReplicate(inEnvironmentSize * (1.0f - pow(n, -1.0f / 3.0f)));
+	Vec3 min_box_size = Vec3::sReplicate(1.0f / inEnvironmentSize);
+	Vec3 max_box_size = Vec3::sReplicate(inEnvironmentSize * pow(n, -1.0f / 3.0f)) - min_box_size;
+	for (int b = 0; b < inNumBodies; ++b)
+	{
+		AABox box;
+		box.mMin = max_box_start * Vec3(zero_to_one(random), zero_to_one(random), zero_to_one(random)) - Vec3::sReplicate(0.5f * inEnvironmentSize);
+		box.mMax = box.mMin + min_box_size + max_box_size * Vec3(zero_to_one(random), zero_to_one(random), zero_to_one(random));
+
+		BodyCreationSettings s;
+		s.SetShape(new BoxShape(box.GetExtent(), 0.0f));
+		s.mPosition = RVec3(box.GetCenter());
+		s.mRotation = Quat::sIdentity();
+		s.mObjectLayer = (random() % 10) == 0? Layers::MOVING : Layers::NON_MOVING;
+		Body *body = inBodyManager->AllocateBody(s);
+		inBodyManager->AddBody(body);
+	}
+}
+
+void BroadPhaseTest::Initialize()
+{
+	// Create body manager
+	mBodyManager = new BodyManager();
+	mBodyManager->Init(NUM_BODIES, 0, mBroadPhaseLayerInterface);
+
+	// Create broadphase
+	mBroadPhase = new BROAD_PHASE;
+	mBroadPhase->Init(mBodyManager, mBroadPhaseLayerInterface);
+}
+
+void BroadPhaseTest::PostPhysicsUpdate(float inDeltaTime)
+{
+#ifdef JPH_DEBUG_RENDERER
+	mBodyManager->Draw(BodyManager::DrawSettings(), PhysicsSettings(), mDebugRenderer);
+#endif // JPH_DEBUG_RENDERER
+}

lib/All/JoltPhysics/Samples/Tests/BroadPhase/BroadPhaseTest.h

@@ -0,0 +1,35 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/Test.h>
+#include <Jolt/Physics/Body/BodyManager.h>
+#include <Jolt/Physics/Collision/BroadPhase/BroadPhase.h>
+#include <Layers.h>
+
+// Base class for a test involving only the broad phase
+class BroadPhaseTest : public Test
+{
+public:
+	JPH_DECLARE_RTTI_ABSTRACT(JPH_NO_EXPORT, BroadPhaseTest)
+
+	// Destructor
+	virtual					~BroadPhaseTest() override;
+
+	// Initialize the test
+	virtual void			Initialize() override;
+
+	// Update the test, called after the physics update
+	virtual void			PostPhysicsUpdate(float inDeltaTime) override;
+
+protected:
+	// Create bodies according to method outlined in "FAST SOFTWARE FOR BOX INTERSECTIONS by AFRA ZOMORODIAN" section "The balanced distribution"
+	// http://pub.ist.ac.at/~edels/Papers/2002-J-01-FastBoxIntersection.pdf
+	void					CreateBalancedDistribution(BodyManager *inBodyManager, int inNumBodies, float inEnvironmentSize = 512.0f);
+
+	BPLayerInterfaceImpl	mBroadPhaseLayerInterface;
+	BroadPhase *			mBroadPhase = nullptr;
+	BodyManager *			mBodyManager = nullptr;
+};

lib/All/JoltPhysics/Samples/Tests/Character/CharacterBaseTest.cpp

@@ -0,0 +1,855 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/Character/CharacterBaseTest.h>
+#include <Jolt/Physics/PhysicsScene.h>
+#include <Jolt/Physics/Collision/Shape/CapsuleShape.h>
+#include <Jolt/Physics/Collision/Shape/CylinderShape.h>
+#include <Jolt/Physics/Collision/Shape/RotatedTranslatedShape.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/Shape/MeshShape.h>
+#include <Jolt/Physics/Constraints/HingeConstraint.h>
+#include <Jolt/Core/StringTools.h>
+#include <Jolt/ObjectStream/ObjectStreamIn.h>
+#include <Application/DebugUI.h>
+#include <Layers.h>
+#include <Utils/Log.h>
+#include <Utils/AssetStream.h>
+#include <Renderer/DebugRendererImp.h>
+
+JPH_IMPLEMENT_RTTI_ABSTRACT(CharacterBaseTest)
+{
+	JPH_ADD_BASE_CLASS(CharacterBaseTest, Test)
+}
+
+const char *CharacterBaseTest::sScenes[] =
+{
+	"PerlinMesh",
+	"PerlinHeightField",
+	"ObstacleCourse",
+	"InitiallyIntersecting",
+#ifdef JPH_OBJECT_STREAM
+	"Terrain1",
+	"Terrain2",
+#endif // JPH_OBJECT_STREAM
+};
+
+const char *CharacterBaseTest::sSceneName = "ObstacleCourse";
+
+// Scene constants
+static const RVec3 cRotatingPosition(-5, 0.15f, 15);
+static const Quat cRotatingOrientation = Quat::sIdentity();
+static const RVec3 cRotatingWallPosition(5, 1.0f, 25.0f);
+static const Quat cRotatingWallOrientation = Quat::sIdentity();
+static const RVec3 cRotatingAndTranslatingPosition(-10, 0.15f, 27.5f);
+static const Quat cRotatingAndTranslatingOrientation = Quat::sIdentity();
+static const RVec3 cSmoothVerticallyMovingPosition(0, 2.0f, 15);
+static const Quat cSmoothVerticallyMovingOrientation = Quat::sIdentity();
+static const RVec3 cReversingVerticallyMovingPosition(0, 0.15f, 25);
+static const Quat cReversingVerticallyMovingOrientation = Quat::sIdentity();
+static const RVec3 cHorizontallyMovingPosition(5, 1, 15);
+static const Quat cHorizontallyMovingOrientation = Quat::sRotation(Vec3::sAxisZ(), 0.5f * JPH_PI);
+static const RVec3 cConveyorBeltPosition(-10, 0.15f, 15);
+static const RVec3 cRampPosition(15, 2.2f, 15);
+static const Quat cRampOrientation = Quat::sRotation(Vec3::sAxisX(), -0.25f * JPH_PI);
+static const RVec3 cRampBlocksStart = cRampPosition + Vec3(-3.0f, 3.0f, 1.5f);
+static const Vec3 cRampBlocksDelta = Vec3(2.0f, 0, 0);
+static const float cRampBlocksTime = 5.0f;
+static const RVec3 cSmallBumpsPosition(-5.0f, 0, 2.5f);
+static const float cSmallBumpHeight = 0.05f;
+static const float cSmallBumpWidth = 0.01f;
+static const float cSmallBumpDelta = 0.5f;
+static const RVec3 cLargeBumpsPosition(-10.0f, 0, 2.5f);
+static const float cLargeBumpHeight = 0.3f;
+static const float cLargeBumpWidth = 0.1f;
+static const float cLargeBumpDelta = 2.0f;
+static const RVec3 cStairsPosition(-15.0f, 0, 2.5f);
+static const float cStairsStepHeight = 0.3f;
+static const RVec3 cMeshStairsPosition(-20.0f, 0, 2.5f);
+static const RVec3 cNoStairsPosition(-15.0f, 0, 10.0f);
+static const float cNoStairsStepHeight = 0.3f;
+static const float cNoStairsStepDelta = 0.05f;
+static const RVec3 cMeshNoStairsPosition(-20.0f, 0, 10.0f);
+static const RVec3 cMeshWallPosition(-25.0f, 0, -27.0f);
+static const float cMeshWallHeight = 3.0f;
+static const float cMeshWallWidth = 2.0f;
+static const float cMeshWallStepStart = 0.5f;
+static const float cMeshWallStepEnd = 4.0f;
+static const int cMeshWallSegments = 25;
+static const RVec3 cHalfCylinderPosition(5.0f, 0, 8.0f);
+static const RVec3 cMeshBoxPosition(30.0f, 1.5f, 5.0f);
+static const RVec3 cSensorPosition(30, 0.9f, -5);
+static const RVec3 cCharacterPosition(-3.5f, 0, 3.0f);
+static const RVec3 cCharacterVirtualPosition(-5.0f, 0, 3.0f);
+static const RVec3 cCharacterVirtualWithInnerBodyPosition(-6.5f, 0, 3.0f);
+static const Vec3 cCharacterVelocity(0, 0, 2);
+
+CharacterBaseTest::~CharacterBaseTest()
+{
+	if (mAnimatedCharacter != nullptr)
+		mAnimatedCharacter->RemoveFromPhysicsSystem();
+}
+
+void CharacterBaseTest::Initialize()
+{
+	// Create capsule shapes for all stances
+	switch (sShapeType)
+	{
+	case EType::Capsule:
+		mStandingShape = RotatedTranslatedShapeSettings(Vec3(0, 0.5f * cCharacterHeightStanding + cCharacterRadiusStanding, 0), Quat::sIdentity(), new CapsuleShape(0.5f * cCharacterHeightStanding, cCharacterRadiusStanding)).Create().Get();
+		mCrouchingShape = RotatedTranslatedShapeSettings(Vec3(0, 0.5f * cCharacterHeightCrouching + cCharacterRadiusCrouching, 0), Quat::sIdentity(), new CapsuleShape(0.5f * cCharacterHeightCrouching, cCharacterRadiusCrouching)).Create().Get();
+		mInnerStandingShape = RotatedTranslatedShapeSettings(Vec3(0, 0.5f * cCharacterHeightStanding + cCharacterRadiusStanding, 0), Quat::sIdentity(), new CapsuleShape(0.5f * cInnerShapeFraction * cCharacterHeightStanding, cInnerShapeFraction * cCharacterRadiusStanding)).Create().Get();
+		mInnerCrouchingShape = RotatedTranslatedShapeSettings(Vec3(0, 0.5f * cCharacterHeightCrouching + cCharacterRadiusCrouching, 0), Quat::sIdentity(), new CapsuleShape(0.5f * cInnerShapeFraction * cCharacterHeightCrouching, cInnerShapeFraction * cCharacterRadiusCrouching)).Create().Get();
+		break;
+
+	case EType::Cylinder:
+		mStandingShape = RotatedTranslatedShapeSettings(Vec3(0, 0.5f * cCharacterHeightStanding + cCharacterRadiusStanding, 0), Quat::sIdentity(), new CylinderShape(0.5f * cCharacterHeightStanding + cCharacterRadiusStanding, cCharacterRadiusStanding)).Create().Get();
+		mCrouchingShape = RotatedTranslatedShapeSettings(Vec3(0, 0.5f * cCharacterHeightCrouching + cCharacterRadiusCrouching, 0), Quat::sIdentity(), new CylinderShape(0.5f * cCharacterHeightCrouching + cCharacterRadiusCrouching, cCharacterRadiusCrouching)).Create().Get();
+		mInnerStandingShape = RotatedTranslatedShapeSettings(Vec3(0, 0.5f * cCharacterHeightStanding + cCharacterRadiusStanding, 0), Quat::sIdentity(), new CylinderShape(cInnerShapeFraction * (0.5f * cCharacterHeightStanding + cCharacterRadiusStanding), cInnerShapeFraction * cCharacterRadiusStanding)).Create().Get();
+		mInnerCrouchingShape = RotatedTranslatedShapeSettings(Vec3(0, 0.5f * cCharacterHeightCrouching + cCharacterRadiusCrouching, 0), Quat::sIdentity(), new CylinderShape(cInnerShapeFraction * (0.5f * cCharacterHeightCrouching + cCharacterRadiusCrouching), cInnerShapeFraction * cCharacterRadiusCrouching)).Create().Get();
+		break;
+
+	case EType::Box:
+		mStandingShape = RotatedTranslatedShapeSettings(Vec3(0, 0.5f * cCharacterHeightStanding + cCharacterRadiusStanding, 0), Quat::sIdentity(), new BoxShape(Vec3(cCharacterRadiusStanding, 0.5f * cCharacterHeightStanding + cCharacterRadiusStanding, cCharacterRadiusStanding))).Create().Get();
+		mCrouchingShape = RotatedTranslatedShapeSettings(Vec3(0, 0.5f * cCharacterHeightCrouching + cCharacterRadiusCrouching, 0), Quat::sIdentity(), new BoxShape(Vec3(cCharacterRadiusCrouching, 0.5f * cCharacterHeightCrouching + cCharacterRadiusCrouching, cCharacterRadiusCrouching))).Create().Get();
+		mInnerStandingShape = RotatedTranslatedShapeSettings(Vec3(0, 0.5f * cCharacterHeightStanding + cCharacterRadiusStanding, 0), Quat::sIdentity(), new BoxShape(cInnerShapeFraction * Vec3(cCharacterRadiusStanding, 0.5f * cCharacterHeightStanding + cCharacterRadiusStanding, cCharacterRadiusStanding))).Create().Get();
+		mInnerCrouchingShape = RotatedTranslatedShapeSettings(Vec3(0, 0.5f * cCharacterHeightCrouching + cCharacterRadiusCrouching, 0), Quat::sIdentity(), new BoxShape(cInnerShapeFraction * Vec3(cCharacterRadiusCrouching, 0.5f * cCharacterHeightCrouching + cCharacterRadiusCrouching, cCharacterRadiusCrouching))).Create().Get();
+		break;
+
+	case EType::Compound:
+		{
+			StaticCompoundShapeSettings standing_compound;
+			standing_compound.AddShape(Vec3(-0.3f, 0.5f * cCharacterHeightStanding + cCharacterRadiusStanding, 0), Quat::sIdentity(), new CapsuleShape(0.5f * cCharacterHeightStanding, cCharacterRadiusStanding));
+			standing_compound.AddShape(Vec3(0.3f, 0.5f * cCharacterHeightStanding + cCharacterRadiusStanding, 0), Quat::sIdentity(), new BoxShape(Vec3(cCharacterRadiusStanding, 0.5f * cCharacterHeightStanding + cCharacterRadiusStanding, cCharacterRadiusStanding)));
+			mStandingShape = standing_compound.Create().Get();
+
+			StaticCompoundShapeSettings crouching_compound;
+			crouching_compound.AddShape(Vec3(-0.3f, 0.5f * cCharacterHeightCrouching + cCharacterRadiusCrouching, 0), Quat::sIdentity(), new CapsuleShape(0.5f * cCharacterHeightCrouching, cCharacterRadiusCrouching));
+			crouching_compound.AddShape(Vec3(0.3f, 0.5f * cCharacterHeightCrouching + cCharacterRadiusCrouching, 0), Quat::sIdentity(), new BoxShape(Vec3(cCharacterRadiusCrouching, 0.5f * cCharacterHeightCrouching + cCharacterRadiusCrouching, cCharacterRadiusCrouching)));
+			mCrouchingShape = crouching_compound.Create().Get();
+
+			StaticCompoundShapeSettings inner_standing_compound;
+			inner_standing_compound.AddShape(Vec3(-0.3f, 0.5f * cCharacterHeightStanding + cCharacterRadiusStanding, 0), Quat::sIdentity(), new CapsuleShape(0.5f * cInnerShapeFraction * cCharacterHeightStanding, cInnerShapeFraction * cCharacterRadiusStanding));
+			inner_standing_compound.AddShape(Vec3(0.3f, 0.5f * cCharacterHeightStanding + cCharacterRadiusStanding, 0), Quat::sIdentity(), new BoxShape(cInnerShapeFraction * Vec3(cCharacterRadiusStanding, 0.5f * cCharacterHeightStanding + cCharacterRadiusStanding, cCharacterRadiusStanding)));
+			mInnerStandingShape = inner_standing_compound.Create().Get();
+
+			StaticCompoundShapeSettings inner_crouching_compound;
+			inner_crouching_compound.AddShape(Vec3(-0.3f, 0.5f * cCharacterHeightCrouching + cCharacterRadiusCrouching, 0), Quat::sIdentity(), new CapsuleShape(0.5f * cInnerShapeFraction * cCharacterHeightCrouching, cInnerShapeFraction * cCharacterRadiusCrouching));
+			inner_crouching_compound.AddShape(Vec3(0.3f, 0.5f * cCharacterHeightCrouching + cCharacterRadiusCrouching, 0), Quat::sIdentity(), new BoxShape(cInnerShapeFraction * Vec3(cCharacterRadiusCrouching, 0.5f * cCharacterHeightCrouching + cCharacterRadiusCrouching, cCharacterRadiusCrouching)));
+			mInnerCrouchingShape = inner_crouching_compound.Create().Get();
+		}
+		break;
+	}
+
+	if (strcmp(sSceneName, "PerlinMesh") == 0)
+	{
+		// Default terrain
+		CreateMeshTerrain();
+	}
+	else if (strcmp(sSceneName, "PerlinHeightField") == 0)
+	{
+		// Default terrain
+		CreateHeightFieldTerrain();
+	}
+	else if (strcmp(sSceneName, "InitiallyIntersecting") == 0)
+	{
+		CreateFloor();
+
+		// Create a grid of boxes that are initially intersecting with the character
+		RefConst<Shape> box = new BoxShape(Vec3(0.1f, 0.1f, 0.1f));
+		BodyCreationSettings settings(box, RVec3(0, 0.5f, 0), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING);
+		for (int x = 0; x < 4; ++x)
+			for (int y = 0; y <= 10; ++y)
+				for (int z = 0; z <= 10; ++z)
+				{
+					settings.mPosition = RVec3(-0.5f + 0.1f * x, 0.1f + 0.1f * y, -0.5f + 0.1f * z);
+					mBodyInterface->CreateAndAddBody(settings, EActivation::DontActivate);
+				}
+	}
+	else if (strcmp(sSceneName, "ObstacleCourse") == 0)
+	{
+		// Default terrain
+		CreateFloor(350.0f);
+
+		{
+			// Create ramps with different inclinations
+			Ref<Shape> ramp = RotatedTranslatedShapeSettings(Vec3(0, 0, -2.5f), Quat::sIdentity(), new BoxShape(Vec3(1.0f, 0.05f, 2.5f))).Create().Get();
+			for (int angle = 0; angle < 18; ++angle)
+				mBodyInterface->CreateAndAddBody(BodyCreationSettings(ramp, RVec3(-15.0f + angle * 2.0f, 0, -10.0f), Quat::sRotation(Vec3::sAxisX(), DegreesToRadians(10.0f * angle)), EMotionType::Static, Layers::NON_MOVING), EActivation::DontActivate);
+		}
+
+		{
+			// Create ramps with different inclinations intersecting with a steep slope
+			Ref<Shape> ramp = RotatedTranslatedShapeSettings(Vec3(0, 0, -2.5f), Quat::sIdentity(), new BoxShape(Vec3(1.0f, 0.05f, 2.5f))).Create().Get();
+			Ref<Shape> ramp2 = RotatedTranslatedShapeSettings(Vec3(0, 2.0f, 0), Quat::sIdentity(), new BoxShape(Vec3(0.05f, 2.0f, 1.0f))).Create().Get();
+			for (int angle = 0; angle < 9; ++angle)
+			{
+				mBodyInterface->CreateAndAddBody(BodyCreationSettings(ramp, RVec3(-15.0f + angle * 2.0f, 0, -20.0f - angle * 0.1f), Quat::sRotation(Vec3::sAxisX(), DegreesToRadians(10.0f * angle)), EMotionType::Static, Layers::NON_MOVING), EActivation::DontActivate);
+				mBodyInterface->CreateAndAddBody(BodyCreationSettings(ramp2, RVec3(-15.0f + angle * 2.0f, 0, -21.0f), Quat::sRotation(Vec3::sAxisZ(), DegreesToRadians(20.0f)), EMotionType::Static, Layers::NON_MOVING), EActivation::DontActivate);
+			}
+		}
+
+		{
+			// Create wall consisting of vertical pillars
+			// Note: Convex radius 0 because otherwise it will be a bumpy wall
+			Ref<Shape> wall = new BoxShape(Vec3(0.1f, 2.5f, 0.1f), 0.0f);
+			for (int z = 0; z < 30; ++z)
+				mBodyInterface->CreateAndAddBody(BodyCreationSettings(wall, RVec3(0.0f, 2.5f, 2.0f + 0.2f * z), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING), EActivation::DontActivate);
+		}
+
+		{
+			// Kinematic blocks to test interacting with moving objects
+			Ref<Shape> kinematic = new BoxShape(Vec3(1, 0.15f, 3.0f));
+			mRotatingBody = mBodyInterface->CreateAndAddBody(BodyCreationSettings(kinematic, cRotatingPosition, cRotatingOrientation, EMotionType::Kinematic, Layers::MOVING), EActivation::Activate);
+			mRotatingWallBody = mBodyInterface->CreateAndAddBody(BodyCreationSettings(new BoxShape(Vec3(3.0f, 1, 0.15f)), cRotatingWallPosition, cRotatingWallOrientation, EMotionType::Kinematic, Layers::MOVING), EActivation::Activate);
+			mRotatingAndTranslatingBody = mBodyInterface->CreateAndAddBody(BodyCreationSettings(kinematic, cRotatingAndTranslatingPosition, cRotatingAndTranslatingOrientation, EMotionType::Kinematic, Layers::MOVING), EActivation::Activate);
+			mSmoothVerticallyMovingBody = mBodyInterface->CreateAndAddBody(BodyCreationSettings(kinematic, cSmoothVerticallyMovingPosition, cSmoothVerticallyMovingOrientation, EMotionType::Kinematic, Layers::MOVING), EActivation::Activate);
+			mReversingVerticallyMovingBody = mBodyInterface->CreateAndAddBody(BodyCreationSettings(kinematic, cReversingVerticallyMovingPosition, cReversingVerticallyMovingOrientation, EMotionType::Kinematic, Layers::MOVING), EActivation::Activate);
+			mHorizontallyMovingBody = mBodyInterface->CreateAndAddBody(BodyCreationSettings(kinematic, cHorizontallyMovingPosition, cHorizontallyMovingOrientation, EMotionType::Kinematic, Layers::MOVING), EActivation::Activate);
+		}
+
+		{
+			// Conveyor belt (only works with virtual character)
+			mConveyorBeltBody = mBodyInterface->CreateAndAddBody(BodyCreationSettings(new BoxShape(Vec3(1, 0.15f, 3.0f)), cConveyorBeltPosition, Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING), EActivation::Activate);
+		}
+
+		{
+			// A rolling sphere towards the player
+			BodyCreationSettings bcs(new SphereShape(0.2f), RVec3(0.0f, 0.2f, -1.0f), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING);
+			bcs.mLinearVelocity = Vec3(0, 0, 2.0f);
+			bcs.mOverrideMassProperties = EOverrideMassProperties::CalculateInertia;
+			bcs.mMassPropertiesOverride.mMass = 10.0f;
+			mBodyInterface->CreateAndAddBody(bcs, EActivation::Activate);
+		}
+
+		{
+			// Dynamic blocks to test player pushing blocks
+			Ref<Shape> block = new BoxShape(Vec3::sReplicate(0.5f));
+			for (int y = 0; y < 3; ++y)
+			{
+				BodyCreationSettings bcs(block, RVec3(5.0f, 0.5f + float(y), 0.0f), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING);
+				bcs.mOverrideMassProperties = EOverrideMassProperties::CalculateInertia;
+				bcs.mMassPropertiesOverride.mMass = 10.0f;
+				mBodyInterface->CreateAndAddBody(bcs, EActivation::DontActivate);
+			}
+		}
+
+		{
+			// Dynamic block on a static step (to test pushing block on stairs)
+			mBodyInterface->CreateAndAddBody(BodyCreationSettings(new BoxShape(Vec3(0.5f, 0.15f, 0.5f)), RVec3(10.0f, 0.15f, 0.0f), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING), EActivation::DontActivate);
+			BodyCreationSettings bcs(new BoxShape(Vec3::sReplicate(0.5f)), RVec3(10.0f, 0.8f, 0.0f), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING);
+			bcs.mOverrideMassProperties = EOverrideMassProperties::CalculateInertia;
+			bcs.mMassPropertiesOverride.mMass = 10.0f;
+			mBodyInterface->CreateAndAddBody(bcs, EActivation::DontActivate);
+		}
+
+		{
+			// Dynamic spheres to test player pushing stuff you can step on
+			float h = 0.0f;
+			for (int y = 0; y < 3; ++y)
+			{
+				float r = 0.4f - 0.1f * y;
+				h += r;
+				BodyCreationSettings bcs(new SphereShape(r), RVec3(15.0f, h, 0.0f), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING);
+				h += r;
+				bcs.mOverrideMassProperties = EOverrideMassProperties::CalculateInertia;
+				bcs.mMassPropertiesOverride.mMass = 10.0f;
+				mBodyInterface->CreateAndAddBody(bcs, EActivation::DontActivate);
+			}
+		}
+
+		{
+			// A seesaw to test character gravity
+			BodyID b1 = mBodyInterface->CreateAndAddBody(BodyCreationSettings(new BoxShape(Vec3(1.0f, 0.2f, 0.05f)), RVec3(20.0f, 0.2f, 0.0f), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING), EActivation::DontActivate);
+			BodyCreationSettings bcs(new BoxShape(Vec3(1.0f, 0.05f, 5.0f)), RVec3(20.0f, 0.45f, 0.0f), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING);
+			bcs.mOverrideMassProperties = EOverrideMassProperties::CalculateInertia;
+			bcs.mMassPropertiesOverride.mMass = 10.0f;
+			BodyID b2 = mBodyInterface->CreateAndAddBody(bcs, EActivation::Activate);
+
+			// Connect the parts with a hinge
+			HingeConstraintSettings hinge;
+			hinge.mPoint1 = hinge.mPoint2 = RVec3(20.0f, 0.4f, 0.0f);
+			hinge.mHingeAxis1 = hinge.mHingeAxis2 = Vec3::sAxisX();
+			mPhysicsSystem->AddConstraint(mBodyInterface->CreateConstraint(&hinge, b1, b2));
+		}
+
+		{
+			// A board above the character to crouch and jump up against
+			float h = 0.5f * cCharacterHeightCrouching + cCharacterRadiusCrouching + 0.1f;
+			for (int x = 0; x < 2; ++x)
+				mBodyInterface->CreateAndAddBody(BodyCreationSettings(new BoxShape(Vec3(1.0f, h, 0.05f)), RVec3(25.0f, h, x == 0? -0.95f : 0.95f), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING), EActivation::DontActivate);
+			BodyCreationSettings bcs(new BoxShape(Vec3(1.0f, 0.05f, 1.0f)), RVec3(25.0f, 2.0f * h + 0.05f, 0.0f), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING);
+			bcs.mOverrideMassProperties = EOverrideMassProperties::CalculateInertia;
+			bcs.mMassPropertiesOverride.mMass = 10.0f;
+			mBodyInterface->CreateAndAddBody(bcs, EActivation::Activate);
+		}
+
+		{
+			// A floating static block
+			mBodyInterface->CreateAndAddBody(BodyCreationSettings(new BoxShape(Vec3::sReplicate(0.5f)), RVec3(30.0f, 1.5f, 0.0f), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING), EActivation::DontActivate);
+		}
+
+		{
+			// Create ramp
+			BodyCreationSettings ramp(new BoxShape(Vec3(4.0f, 0.1f, 3.0f)), cRampPosition, cRampOrientation, EMotionType::Static, Layers::NON_MOVING);
+			mBodyInterface->CreateAndAddBody(ramp, EActivation::DontActivate);
+
+			// Create blocks on ramp
+			Ref<Shape> block = new BoxShape(Vec3::sReplicate(0.5f));
+			BodyCreationSettings bcs(block, cRampBlocksStart, cRampOrientation, EMotionType::Dynamic, Layers::MOVING);
+			bcs.mOverrideMassProperties = EOverrideMassProperties::CalculateInertia;
+			bcs.mMassPropertiesOverride.mMass = 10.0f;
+			for (int i = 0; i < 4; ++i)
+			{
+				mRampBlocks.emplace_back(mBodyInterface->CreateAndAddBody(bcs, EActivation::Activate));
+				bcs.mPosition += cRampBlocksDelta;
+			}
+		}
+
+		// Create three funnels with walls that are too steep to climb
+		Ref<Shape> funnel = new BoxShape(Vec3(0.1f, 1.0f, 1.0f));
+		for (int i = 0; i < 2; ++i)
+		{
+			Quat rotation = Quat::sRotation(Vec3::sAxisY(), JPH_PI * i);
+			mBodyInterface->CreateAndAddBody(BodyCreationSettings(funnel, RVec3(5.0f, 0.1f, 5.0f) + rotation * Vec3(0.2f, 0, 0), rotation * Quat::sRotation(Vec3::sAxisZ(), -DegreesToRadians(40.0f)), EMotionType::Static, Layers::NON_MOVING), EActivation::DontActivate);
+		}
+		for (int i = 0; i < 3; ++i)
+		{
+			Quat rotation = Quat::sRotation(Vec3::sAxisY(), 2.0f / 3.0f * JPH_PI * i);
+			mBodyInterface->CreateAndAddBody(BodyCreationSettings(funnel, RVec3(7.5f, 0.1f, 5.0f) + rotation * Vec3(0.2f, 0, 0), rotation * Quat::sRotation(Vec3::sAxisZ(), -DegreesToRadians(40.0f)), EMotionType::Static, Layers::NON_MOVING), EActivation::DontActivate);
+		}
+		for (int i = 0; i < 4; ++i)
+		{
+			Quat rotation = Quat::sRotation(Vec3::sAxisY(), 0.5f * JPH_PI * i);
+			mBodyInterface->CreateAndAddBody(BodyCreationSettings(funnel, RVec3(10.0f, 0.1f, 5.0f) + rotation * Vec3(0.2f, 0, 0), rotation * Quat::sRotation(Vec3::sAxisZ(), -DegreesToRadians(40.0f)), EMotionType::Static, Layers::NON_MOVING), EActivation::DontActivate);
+		}
+
+		// Create small bumps
+		{
+			BodyCreationSettings step(new BoxShape(Vec3(2.0f, 0.5f * cSmallBumpHeight, 0.5f * cSmallBumpWidth), 0.0f), RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING);
+			for (int i = 0; i < 10; ++i)
+			{
+				step.mPosition = cSmallBumpsPosition + Vec3(0, 0.5f * cSmallBumpHeight, cSmallBumpDelta * i);
+				mBodyInterface->CreateAndAddBody(step, EActivation::DontActivate);
+			}
+		}
+
+		// Create large bumps
+		{
+			BodyCreationSettings step(new BoxShape(Vec3(2.0f, 0.5f * cLargeBumpHeight, 0.5f * cLargeBumpWidth)), RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING);
+			for (int i = 0; i < 5; ++i)
+			{
+				step.mPosition = cLargeBumpsPosition + Vec3(0, 0.5f * cLargeBumpHeight, cLargeBumpDelta * i);
+				mBodyInterface->CreateAndAddBody(step, EActivation::DontActivate);
+			}
+		}
+
+		// Create stairs
+		{
+			BodyCreationSettings step(new BoxShape(Vec3(2.0f, 0.5f * cStairsStepHeight, 0.5f * cStairsStepHeight)), RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING);
+			for (int i = 0; i < 10; ++i)
+			{
+				step.mPosition = cStairsPosition + Vec3(0, cStairsStepHeight * (0.5f + i), cStairsStepHeight * i);
+				mBodyInterface->CreateAndAddBody(step, EActivation::DontActivate);
+			}
+		}
+
+		// A wall beside the stairs
+		mBodyInterface->CreateAndAddBody(BodyCreationSettings(new BoxShape(Vec3(0.5f, 2.0f, 5.0f * cStairsStepHeight)), cStairsPosition + Vec3(-2.5f, 2.0f, 5.0f * cStairsStepHeight), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING), EActivation::DontActivate);
+
+		// Create stairs from triangles
+		{
+			TriangleList triangles;
+
+			float rear_z = 10 * cStairsStepHeight;
+
+			for (int i = 0; i < 10; ++i)
+			{
+				// Start of step
+				Vec3 base(0, cStairsStepHeight * i, cStairsStepHeight * i);
+
+				// Left side
+				Vec3 b1 = base + Vec3(2.0f, 0, 0);
+				Vec3 s1 = b1 + Vec3(0, cStairsStepHeight, 0);
+				Vec3 p1 = s1 + Vec3(0, 0, cStairsStepHeight);
+
+				// 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));
+
+				// Side of stairs
+				Vec3 rb2 = b2; rb2.SetZ(rear_z);
+				Vec3 rs2 = s2; rs2.SetZ(rear_z);
+
+				triangles.push_back(Triangle(s2, b2, rs2));
+				triangles.push_back(Triangle(rs2, b2, rb2));
+			}
+
+			MeshShapeSettings mesh(triangles);
+			mesh.SetEmbedded();
+			BodyCreationSettings mesh_stairs(&mesh, cMeshStairsPosition, Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING);
+			mBodyInterface->CreateAndAddBody(mesh_stairs, EActivation::DontActivate);
+		}
+
+		// A wall to the side and behind the stairs
+		mBodyInterface->CreateAndAddBody(BodyCreationSettings(new BoxShape(Vec3(0.5f, 2.0f, 0.25f)), cStairsPosition + Vec3(-7.5f, 2.0f, 10.0f * cStairsStepHeight + 0.25f), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING), EActivation::DontActivate);
+
+		// Create stairs with too little space between the steps
+		{
+			BodyCreationSettings step(new BoxShape(Vec3(2.0f, 0.5f * cNoStairsStepHeight, 0.5f * cNoStairsStepHeight)), RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING);
+			for (int i = 0; i < 10; ++i)
+			{
+				step.mPosition = cNoStairsPosition + Vec3(0, cNoStairsStepHeight * (0.5f + i), cNoStairsStepDelta * i);
+				mBodyInterface->CreateAndAddBody(step, EActivation::DontActivate);
+			}
+		}
+
+		// Create stairs with too little space between the steps consisting of triangles
+		{
+			TriangleList triangles;
+
+			for (int i = 0; i < 10; ++i)
+			{
+				// Start of step
+				Vec3 base(0, cStairsStepHeight * i, cNoStairsStepDelta * i);
+
+				// Left side
+				Vec3 b1 = base - Vec3(2.0f, 0, 0);
+				Vec3 s1 = b1 + Vec3(0, cStairsStepHeight, 0);
+				Vec3 p1 = s1 + Vec3(0, 0, cNoStairsStepDelta);
+
+				// 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, s2, b1));
+				triangles.push_back(Triangle(b1, s2, b2));
+				triangles.push_back(Triangle(s1, p1, p2));
+				triangles.push_back(Triangle(s1, p2, s2));
+			}
+
+			MeshShapeSettings mesh(triangles);
+			mesh.SetEmbedded();
+			BodyCreationSettings mesh_stairs(&mesh, cMeshNoStairsPosition, Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING);
+			mBodyInterface->CreateAndAddBody(mesh_stairs, EActivation::DontActivate);
+		}
+
+		// Create mesh with walls at varying angles
+		{
+			TriangleList triangles;
+			Vec3 p1(0.5f * cMeshWallWidth, 0, 0);
+			Vec3 h(0, cMeshWallHeight, 0);
+			for (int i = 0; i < cMeshWallSegments; ++i)
+			{
+				float delta = cMeshWallStepStart + i * (cMeshWallStepEnd - cMeshWallStepStart) / (cMeshWallSegments - 1);
+				Vec3 p2 = Vec3((i & 1)? 0.5f * cMeshWallWidth : -0.5f * cMeshWallWidth, 0, p1.GetZ() + delta);
+				triangles.push_back(Triangle(p1, p1 + h, p2 + h));
+				triangles.push_back(Triangle(p1, p2 + h, p2));
+				p1 = p2;
+			}
+
+			MeshShapeSettings mesh(triangles);
+			mesh.SetEmbedded();
+			BodyCreationSettings wall(&mesh, cMeshWallPosition, Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING);
+			mBodyInterface->CreateAndAddBody(wall, EActivation::DontActivate);
+		}
+
+		// 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 = 512;
+			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 = cCharacterRadiusStanding + 0.05f;
+					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));
+			}
+
+			MeshShapeSettings mesh(std::move(vertices), std::move(triangles));
+			mesh.SetEmbedded();
+			BodyCreationSettings mesh_cylinder(&mesh, cHalfCylinderPosition, Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING);
+			mBodyInterface->CreateAndAddBody(mesh_cylinder, EActivation::DontActivate);
+		}
+
+		// Create a box made out of polygons (character should not get stuck behind back facing side)
+		{
+			VertexList vertices = {
+				Float3(-1,  1, -1),
+				Float3( 1,  1, -1),
+				Float3( 1,  1,  1),
+				Float3(-1,  1,  1),
+				Float3(-1, -1, -1),
+				Float3( 1, -1, -1),
+				Float3( 1, -1,  1),
+				Float3(-1, -1,  1)
+			};
+
+			IndexedTriangleList triangles = {
+				IndexedTriangle(0, 3, 2),
+				IndexedTriangle(0, 2, 1),
+				IndexedTriangle(4, 5, 6),
+				IndexedTriangle(4, 6, 7),
+				IndexedTriangle(0, 4, 3),
+				IndexedTriangle(3, 4, 7),
+				IndexedTriangle(2, 6, 5),
+				IndexedTriangle(2, 5, 1),
+				IndexedTriangle(3, 7, 6),
+				IndexedTriangle(3, 6, 2),
+				IndexedTriangle(0, 1, 5),
+				IndexedTriangle(0, 5, 4)
+			};
+
+			MeshShapeSettings mesh(std::move(vertices), std::move(triangles));
+			mesh.SetEmbedded();
+			BodyCreationSettings box(&mesh, cMeshBoxPosition, Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING);
+			mBodyInterface->CreateAndAddBody(box, EActivation::DontActivate);
+		}
+
+		// Create a sensor
+		{
+			BodyCreationSettings sensor(new BoxShape(Vec3::sOne()), cSensorPosition, Quat::sIdentity(), EMotionType::Kinematic, Layers::SENSOR);
+			sensor.mIsSensor = true;
+			mSensorBody = mBodyInterface->CreateAndAddBody(sensor, EActivation::Activate);
+		}
+
+		// Create Character
+		{
+			CharacterSettings settings;
+			settings.mLayer = Layers::MOVING;
+			settings.mShape = mStandingShape;
+			settings.mSupportingVolume = Plane(Vec3::sAxisY(), -cCharacterRadiusStanding); // Accept contacts that touch the lower sphere of the capsule
+			mAnimatedCharacter = new Character(&settings, cCharacterPosition, Quat::sIdentity(), 0, mPhysicsSystem);
+			mAnimatedCharacter->AddToPhysicsSystem();
+		}
+
+		// Create CharacterVirtual
+		{
+			CharacterVirtualSettings settings;
+			settings.mShape = mStandingShape;
+			settings.mSupportingVolume = Plane(Vec3::sAxisY(), -cCharacterRadiusStanding); // Accept contacts that touch the lower sphere of the capsule
+			mAnimatedCharacterVirtual = new CharacterVirtual(&settings, cCharacterVirtualPosition, Quat::sIdentity(), 0, mPhysicsSystem);
+			mAnimatedCharacterVirtual->SetCharacterVsCharacterCollision(&mCharacterVsCharacterCollision);
+			mCharacterVsCharacterCollision.Add(mAnimatedCharacterVirtual);
+		}
+
+		// Create CharacterVirtual with inner rigid body
+		{
+			CharacterVirtualSettings settings;
+			settings.mShape = mStandingShape;
+			settings.mInnerBodyShape = mInnerStandingShape;
+			settings.mSupportingVolume = Plane(Vec3::sAxisY(), -cCharacterRadiusStanding); // Accept contacts that touch the lower sphere of the capsule
+			mAnimatedCharacterVirtualWithInnerBody = new CharacterVirtual(&settings, cCharacterVirtualWithInnerBodyPosition, Quat::sIdentity(), 0, mPhysicsSystem);
+			mAnimatedCharacterVirtualWithInnerBody->SetCharacterVsCharacterCollision(&mCharacterVsCharacterCollision);
+			mCharacterVsCharacterCollision.Add(mAnimatedCharacterVirtualWithInnerBody);
+		}
+	}
+#ifdef JPH_OBJECT_STREAM
+	else
+	{
+		// Load scene
+		Ref<PhysicsScene> scene;
+		AssetStream stream(String(sSceneName) + ".bof", std::ios::in | std::ios::binary);
+		if (!ObjectStreamIn::sReadObject(stream.Get(), scene))
+			FatalError("Failed to load scene");
+		scene->FixInvalidScales();
+		for (BodyCreationSettings &settings : scene->GetBodies())
+		{
+			settings.mObjectLayer = Layers::NON_MOVING;
+			settings.mFriction = 0.5f;
+		}
+		scene->CreateBodies(mPhysicsSystem);
+	}
+#endif // JPH_OBJECT_STREAM
+}
+
+void CharacterBaseTest::ProcessInput(const ProcessInputParams &inParams)
+{
+	// Determine controller input
+	mControlInput = Vec3::sZero();
+	if (inParams.mKeyboard->IsKeyPressed(EKey::Left))	mControlInput.SetZ(-1);
+	if (inParams.mKeyboard->IsKeyPressed(EKey::Right))	mControlInput.SetZ(1);
+	if (inParams.mKeyboard->IsKeyPressed(EKey::Up))		mControlInput.SetX(1);
+	if (inParams.mKeyboard->IsKeyPressed(EKey::Down))	mControlInput.SetX(-1);
+	if (mControlInput != Vec3::sZero())
+		mControlInput = mControlInput.Normalized();
+
+	// Rotate controls to align with the camera
+	Vec3 cam_fwd = inParams.mCameraState.mForward;
+	cam_fwd.SetY(0.0f);
+	cam_fwd = cam_fwd.NormalizedOr(Vec3::sAxisX());
+	Quat rotation = Quat::sFromTo(Vec3::sAxisX(), cam_fwd);
+	mControlInput = rotation * mControlInput;
+
+	// Check actions
+	mJump = inParams.mKeyboard->IsKeyPressedAndTriggered(EKey::RControl, mWasJump);
+	mSwitchStance = inParams.mKeyboard->IsKeyPressedAndTriggered(EKey::RShift, mWasSwitchStance);
+}
+
+void CharacterBaseTest::PrePhysicsUpdate(const PreUpdateParams &inParams)
+{
+	// Update scene time
+	mTime += inParams.mDeltaTime;
+
+	// Update camera pivot
+	mCameraPivot = GetCharacterPosition();
+
+	// Animate bodies
+	if (!mRotatingBody.IsInvalid())
+		mBodyInterface->MoveKinematic(mRotatingBody, cRotatingPosition, Quat::sRotation(Vec3::sAxisY(), JPH_PI * Sin(mTime)), inParams.mDeltaTime);
+	if (!mRotatingWallBody.IsInvalid())
+		mBodyInterface->MoveKinematic(mRotatingWallBody, cRotatingWallPosition, Quat::sRotation(Vec3::sAxisY(), JPH_PI * Sin(mTime)), inParams.mDeltaTime);
+	if (!mRotatingAndTranslatingBody.IsInvalid())
+		mBodyInterface->MoveKinematic(mRotatingAndTranslatingBody, cRotatingAndTranslatingPosition + 5.0f * Vec3(Sin(JPH_PI * mTime), 0, Cos(JPH_PI * mTime)), Quat::sRotation(Vec3::sAxisY(), JPH_PI * Sin(mTime)), inParams.mDeltaTime);
+	if (!mHorizontallyMovingBody.IsInvalid())
+		mBodyInterface->MoveKinematic(mHorizontallyMovingBody, cHorizontallyMovingPosition + Vec3(3.0f * Sin(mTime), 0, 0), cHorizontallyMovingOrientation, inParams.mDeltaTime);
+	if (!mSmoothVerticallyMovingBody.IsInvalid())
+		mBodyInterface->MoveKinematic(mSmoothVerticallyMovingBody, cSmoothVerticallyMovingPosition + Vec3(0, 1.75f * Sin(mTime), 0), cSmoothVerticallyMovingOrientation, inParams.mDeltaTime);
+	if (!mReversingVerticallyMovingBody.IsInvalid())
+	{
+		RVec3 pos = mBodyInterface->GetPosition(mReversingVerticallyMovingBody);
+		if (pos.GetY() < cReversingVerticallyMovingPosition.GetY())
+			mReversingVerticallyMovingVelocity = 1.0f;
+		else if (pos.GetY() > cReversingVerticallyMovingPosition.GetY() + 5.0f)
+			mReversingVerticallyMovingVelocity = -1.0f;
+		mBodyInterface->MoveKinematic(mReversingVerticallyMovingBody, pos + Vec3(0, mReversingVerticallyMovingVelocity * 3.0f * inParams.mDeltaTime, 0), cReversingVerticallyMovingOrientation, inParams.mDeltaTime);
+	}
+
+	// Animate character
+	if (mAnimatedCharacter != nullptr)
+		mAnimatedCharacter->SetLinearVelocity(Sin(mTime) * cCharacterVelocity);
+
+	// Animate character virtual
+	for (CharacterVirtual *character : { mAnimatedCharacterVirtual, mAnimatedCharacterVirtualWithInnerBody })
+		if (character != nullptr)
+		{
+			// Draw the character
+			DrawPaddedCharacter(character->GetShape(), character->GetCharacterPadding(), character->GetCenterOfMassTransform());
+
+			// Update velocity and apply gravity
+			Vec3 velocity;
+			if (character->GetGroundState() == CharacterVirtual::EGroundState::OnGround)
+				velocity = Vec3::sZero();
+			else
+				velocity = character->GetLinearVelocity() * mAnimatedCharacter->GetUp() + mPhysicsSystem->GetGravity() * inParams.mDeltaTime;
+			velocity += Sin(mTime) * cCharacterVelocity;
+			character->SetLinearVelocity(velocity);
+
+			// Move character
+			CharacterVirtual::ExtendedUpdateSettings update_settings;
+			character->ExtendedUpdate(inParams.mDeltaTime,
+				mPhysicsSystem->GetGravity(),
+				update_settings,
+				mPhysicsSystem->GetDefaultBroadPhaseLayerFilter(Layers::MOVING),
+				mPhysicsSystem->GetDefaultLayerFilter(Layers::MOVING),
+				{ },
+				{ },
+				*mTempAllocator);
+		}
+
+	// Reset ramp blocks
+	mRampBlocksTimeLeft -= inParams.mDeltaTime;
+	if (mRampBlocksTimeLeft < 0.0f)
+	{
+		for (size_t i = 0; i < mRampBlocks.size(); ++i)
+		{
+			mBodyInterface->SetPositionAndRotation(mRampBlocks[i], cRampBlocksStart + float(i) * cRampBlocksDelta, cRampOrientation, EActivation::Activate);
+			mBodyInterface->SetLinearAndAngularVelocity(mRampBlocks[i], Vec3::sZero(), Vec3::sZero());
+		}
+		mRampBlocksTimeLeft = cRampBlocksTime;
+	}
+
+	// Call handle input after new velocities have been set to avoid frame delay
+	HandleInput(mControlInput, mJump, mSwitchStance, inParams.mDeltaTime);
+}
+
+void CharacterBaseTest::CreateSettingsMenu(DebugUI *inUI, UIElement *inSubMenu)
+{
+	inUI->CreateTextButton(inSubMenu, "Select Scene", [this, inUI]() {
+		UIElement *scene_name = inUI->CreateMenu();
+		for (uint i = 0; i < size(sScenes); ++i)
+			inUI->CreateTextButton(scene_name, sScenes[i], [this, i]() { sSceneName = sScenes[i]; RestartTest(); });
+		inUI->ShowMenu(scene_name);
+	});
+
+	inUI->CreateTextButton(inSubMenu, "Character Movement Settings", [this, inUI]() {
+		UIElement *movement_settings = inUI->CreateMenu();
+
+		inUI->CreateCheckBox(movement_settings, "Control Movement During Jump", sControlMovementDuringJump, [](UICheckBox::EState inState) { sControlMovementDuringJump = inState == UICheckBox::STATE_CHECKED; });
+		inUI->CreateSlider(movement_settings, "Character Speed", sCharacterSpeed, 0.1f, 10.0f, 0.1f, [](float inValue) { sCharacterSpeed = inValue; });
+		inUI->CreateSlider(movement_settings, "Character Jump Speed", sJumpSpeed, 0.1f, 10.0f, 0.1f, [](float inValue) { sJumpSpeed = inValue; });
+		AddCharacterMovementSettings(inUI, movement_settings);
+		inUI->ShowMenu(movement_settings);
+	});
+
+	inUI->CreateTextButton(inSubMenu, "Configuration Settings", [this, inUI]() {
+		UIElement *configuration_settings = inUI->CreateMenu();
+
+		inUI->CreateComboBox(configuration_settings, "Shape Type", { "Capsule", "Cylinder", "Box", "Compound" }, (int)sShapeType, [](int inItem) { sShapeType = (EType)inItem; });
+		AddConfigurationSettings(inUI, configuration_settings);
+		inUI->CreateTextButton(configuration_settings, "Accept Changes", [this]() { RestartTest(); });
+		inUI->ShowMenu(configuration_settings);
+	});
+}
+
+void CharacterBaseTest::GetInitialCamera(CameraState& ioState) const
+{
+	// This will become the local space offset, look down the x axis and slightly down
+	ioState.mPos = RVec3::sZero();
+	ioState.mForward = Vec3(10.0f, -2.0f, 0).Normalized();
+}
+
+RMat44 CharacterBaseTest::GetCameraPivot(float inCameraHeading, float inCameraPitch) const
+{
+	// Pivot is center of character + distance behind based on the heading and pitch of the camera
+	Vec3 fwd = Vec3(Cos(inCameraPitch) * Cos(inCameraHeading), Sin(inCameraPitch), Cos(inCameraPitch) * Sin(inCameraHeading));
+	return RMat44::sTranslation(mCameraPivot + Vec3(0, cCharacterHeightStanding + cCharacterRadiusStanding, 0) - 5.0f * fwd);
+}
+
+void CharacterBaseTest::SaveState(StateRecorder &inStream) const
+{
+	inStream.Write(mTime);
+	inStream.Write(mRampBlocksTimeLeft);
+	inStream.Write(mReversingVerticallyMovingVelocity);
+
+	if (mAnimatedCharacterVirtual != nullptr)
+		mAnimatedCharacterVirtual->SaveState(inStream);
+
+	if (mAnimatedCharacterVirtualWithInnerBody != nullptr)
+		mAnimatedCharacterVirtualWithInnerBody->SaveState(inStream);
+}
+
+void CharacterBaseTest::RestoreState(StateRecorder &inStream)
+{
+	inStream.Read(mTime);
+	inStream.Read(mRampBlocksTimeLeft);
+	inStream.Read(mReversingVerticallyMovingVelocity);
+
+	if (mAnimatedCharacterVirtual != nullptr)
+		mAnimatedCharacterVirtual->RestoreState(inStream);
+
+	if (mAnimatedCharacterVirtualWithInnerBody != nullptr)
+		mAnimatedCharacterVirtualWithInnerBody->RestoreState(inStream);
+}
+
+void CharacterBaseTest::SaveInputState(StateRecorder &inStream) const
+{
+	inStream.Write(mControlInput);
+	inStream.Write(mJump);
+	inStream.Write(mSwitchStance);
+}
+
+void CharacterBaseTest::RestoreInputState(StateRecorder &inStream)
+{
+	inStream.Read(mControlInput);
+	inStream.Read(mJump);
+	inStream.Read(mSwitchStance);
+}
+
+void CharacterBaseTest::DrawCharacterState(const CharacterBase *inCharacter, RMat44Arg inCharacterTransform, Vec3Arg inCharacterVelocity)
+{
+	// Draw current location
+	// Drawing prior to update since the physics system state is also that prior to the simulation step (so that all detected collisions etc. make sense)
+	mDebugRenderer->DrawCoordinateSystem(inCharacterTransform, 0.1f);
+
+	// Draw the state of the ground contact
+	CharacterBase::EGroundState ground_state = inCharacter->GetGroundState();
+	if (ground_state != CharacterBase::EGroundState::InAir)
+	{
+		RVec3 ground_position = inCharacter->GetGroundPosition();
+		Vec3 ground_normal = inCharacter->GetGroundNormal();
+		Vec3 ground_velocity = inCharacter->GetGroundVelocity();
+
+		// Draw ground position
+		mDebugRenderer->DrawMarker(ground_position, Color::sRed, 0.1f);
+		mDebugRenderer->DrawArrow(ground_position, ground_position + 2.0f * ground_normal, Color::sGreen, 0.1f);
+
+		// Draw ground velocity
+		if (!ground_velocity.IsNearZero())
+			mDebugRenderer->DrawArrow(ground_position, ground_position + ground_velocity, Color::sBlue, 0.1f);
+	}
+
+	// Draw provided character velocity
+	if (!inCharacterVelocity.IsNearZero())
+		mDebugRenderer->DrawArrow(inCharacterTransform.GetTranslation(), inCharacterTransform.GetTranslation() + inCharacterVelocity, Color::sYellow, 0.1f);
+
+	// Draw text info
+	const PhysicsMaterial *ground_material = inCharacter->GetGroundMaterial();
+	Vec3 horizontal_velocity = inCharacterVelocity;
+	horizontal_velocity.SetY(0);
+	mDebugRenderer->DrawText3D(inCharacterTransform.GetTranslation(), StringFormat("State: %s\nMat: %s\nHorizontal Vel: %.1f m/s\nVertical Vel: %.1f m/s", CharacterBase::sToString(ground_state), ground_material->GetDebugName(), (double)horizontal_velocity.Length(), (double)inCharacterVelocity.GetY()), Color::sWhite, 0.25f);
+}
+
+void CharacterBaseTest::DrawPaddedCharacter(const Shape *inShape, float inPadding, RMat44Arg inCenterOfMass)
+{
+	if (inShape->GetSubType() == EShapeSubType::Capsule)
+	{
+		const CapsuleShape *capsule = static_cast<const CapsuleShape *>(inShape);
+		mDebugRenderer->DrawCapsule(inCenterOfMass, capsule->GetHalfHeightOfCylinder(), capsule->GetRadius() + inPadding, Color::sGrey, DebugRenderer::ECastShadow::Off, DebugRenderer::EDrawMode::Wireframe);
+	}
+	else if (inShape->GetSubType() == EShapeSubType::Cylinder)
+	{
+		// Not correct as the edges should be rounded
+		const CylinderShape *cylinder = static_cast<const CylinderShape *>(inShape);
+		mDebugRenderer->DrawCylinder(inCenterOfMass, cylinder->GetHalfHeight() + inPadding, cylinder->GetRadius() + inPadding, Color::sGrey, DebugRenderer::ECastShadow::Off, DebugRenderer::EDrawMode::Wireframe);
+	}
+	else if (inShape->GetSubType() == EShapeSubType::Box)
+	{
+		// Not correct as the edges should be rounded
+		const BoxShape *box = static_cast<const BoxShape *>(inShape);
+		AABox bounds = box->GetLocalBounds();
+		bounds.ExpandBy(Vec3::sReplicate(inPadding));
+		mDebugRenderer->DrawWireBox(inCenterOfMass, bounds, Color::sGrey);
+	}
+	else if (inShape->GetSubType() == EShapeSubType::RotatedTranslated)
+	{
+		const RotatedTranslatedShape *rt = static_cast<const RotatedTranslatedShape *>(inShape);
+		DrawPaddedCharacter(rt->GetInnerShape(), inPadding, inCenterOfMass);
+	}
+	else if (inShape->GetType() == EShapeType::Compound)
+	{
+		const CompoundShape *compound = static_cast<const CompoundShape *>(inShape);
+		for (const CompoundShape::SubShape &sub_shape : compound->GetSubShapes())
+			DrawPaddedCharacter(sub_shape.mShape, inPadding, inCenterOfMass * sub_shape.GetLocalTransformNoScale(Vec3::sOne()));
+	}
+}

lib/All/JoltPhysics/Samples/Tests/Character/CharacterBaseTest.h

@@ -0,0 +1,145 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/Test.h>
+#include <Jolt/Physics/Character/Character.h>
+#include <Jolt/Physics/Character/CharacterVirtual.h>
+
+// Base class for the character tests, initializes the test scene.
+class CharacterBaseTest : public Test
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, CharacterBaseTest)
+
+	// Destructor
+	virtual					~CharacterBaseTest() override;
+
+	// Number used to scale the terrain and camera movement to the scene
+	virtual float			GetWorldScale() const override								{ return 0.2f; }
+
+	// Initialize the test
+	virtual void			Initialize() override;
+
+	// Process input
+	virtual void			ProcessInput(const ProcessInputParams &inParams) override;
+
+	// Update the test, called before the physics update
+	virtual void			PrePhysicsUpdate(const PreUpdateParams &inParams) override;
+
+	// Override to specify the initial camera state (local to GetCameraPivot)
+	virtual void			GetInitialCamera(CameraState &ioState) const override;
+
+	// Override to specify a camera pivot point and orientation (world space)
+	virtual RMat44			GetCameraPivot(float inCameraHeading, float inCameraPitch) const override;
+
+	// Optional settings menu
+	virtual bool			HasSettingsMenu() const override							{ return true; }
+	virtual void			CreateSettingsMenu(DebugUI *inUI, UIElement *inSubMenu) override;
+
+	// Saving / restoring state for replay
+	virtual void			SaveState(StateRecorder &inStream) const override;
+	virtual void			RestoreState(StateRecorder &inStream) override;
+
+	// Saving / restoring controller input state for replay
+	virtual void			SaveInputState(StateRecorder &inStream) const override;
+	virtual void			RestoreInputState(StateRecorder &inStream) override;
+
+protected:
+	// Get position of the character
+	virtual RVec3			GetCharacterPosition() const = 0;
+
+	// Handle user input to the character
+	virtual void			HandleInput(Vec3Arg inMovementDirection, bool inJump, bool inSwitchStance, float inDeltaTime) = 0;
+
+	// Draw the character state
+	void					DrawCharacterState(const CharacterBase *inCharacter, RMat44Arg inCharacterTransform, Vec3Arg inCharacterVelocity);
+
+	// Add character movement settings
+	virtual void			AddCharacterMovementSettings(DebugUI* inUI, UIElement* inSubMenu) { /* Nothing by default */ }
+
+	// Add test configuration settings
+	virtual void			AddConfigurationSettings(DebugUI *inUI, UIElement *inSubMenu) { /* Nothing by default */ }
+
+	// Draw the character + padding
+	void					DrawPaddedCharacter(const Shape *inShape, float inPadding, RMat44Arg inCenterOfMass);
+
+	// Character size
+	static constexpr float	cCharacterHeightStanding = 1.35f;
+	static constexpr float	cCharacterRadiusStanding = 0.3f;
+	static constexpr float	cCharacterHeightCrouching = 0.8f;
+	static constexpr float	cCharacterRadiusCrouching = 0.3f;
+	static constexpr float	cInnerShapeFraction = 0.9f;
+
+	// Character movement properties
+	inline static bool		sControlMovementDuringJump = true;							///< If false the character cannot change movement direction in mid air
+	inline static float		sCharacterSpeed = 6.0f;
+	inline static float		sJumpSpeed = 4.0f;
+
+	// The different stances for the character
+	RefConst<Shape>			mStandingShape;
+	RefConst<Shape>			mCrouchingShape;
+	RefConst<Shape>			mInnerCrouchingShape;
+	RefConst<Shape>			mInnerStandingShape;
+
+	// List of boxes on ramp
+	Array<BodyID>			mRampBlocks;
+	float					mRampBlocksTimeLeft = 0.0f;
+
+	// Conveyor belt body
+	BodyID					mConveyorBeltBody;
+
+	// Sensor body
+	BodyID					mSensorBody;
+
+	// List of active characters in the scene so they can collide
+	CharacterVsCharacterCollisionSimple mCharacterVsCharacterCollision;
+
+	// Shape types
+	enum class EType
+	{
+		Capsule,
+		Cylinder,
+		Box,
+		Compound
+	};
+
+	// Character shape type
+	static inline EType		sShapeType = EType::Capsule;
+
+private:
+	// List of possible scene names
+	static const char *		sScenes[];
+
+	// Filename of animation to load for this test
+	static const char *		sSceneName;
+
+	// Scene time (for moving bodies)
+	float					mTime = 0.0f;
+
+	// The camera pivot, recorded before the physics update to align with the drawn world
+	RVec3					mCameraPivot = RVec3::sZero();
+
+	// Moving bodies
+	BodyID					mRotatingBody;
+	BodyID					mRotatingWallBody;
+	BodyID					mRotatingAndTranslatingBody;
+	BodyID					mSmoothVerticallyMovingBody;
+	BodyID					mReversingVerticallyMovingBody;
+	float					mReversingVerticallyMovingVelocity = 1.0f;
+	BodyID					mHorizontallyMovingBody;
+
+	// Moving characters
+	Ref<Character>			mAnimatedCharacter;
+	Ref<CharacterVirtual>	mAnimatedCharacterVirtual;
+	Ref<CharacterVirtual>	mAnimatedCharacterVirtualWithInnerBody;
+
+	// Player input
+	Vec3					mControlInput = Vec3::sZero();
+	bool					mJump = false;
+	bool					mWasJump = false;
+	bool					mSwitchStance = false;
+	bool					mWasSwitchStance = false;
+};

lib/All/JoltPhysics/Samples/Tests/Character/CharacterPlanetTest.cpp

@@ -0,0 +1,204 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2024 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/Character/CharacterPlanetTest.h>
+#include <Jolt/Physics/Collision/Shape/CapsuleShape.h>
+#include <Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h>
+#include <Jolt/Physics/Collision/Shape/SphereShape.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Layers.h>
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(CharacterPlanetTest)
+{
+	JPH_ADD_BASE_CLASS(CharacterPlanetTest, Test)
+}
+
+void CharacterPlanetTest::Initialize()
+{
+	// Create planet
+	mBodyInterface->CreateAndAddBody(BodyCreationSettings(new SphereShape(cPlanetRadius), RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING), EActivation::DontActivate);
+
+	// Create spheres
+	BodyCreationSettings sphere(new SphereShape(0.5f), RVec3::sZero(), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING);
+	sphere.mGravityFactor = 0.0f; // We do our own gravity
+	sphere.mOverrideMassProperties = EOverrideMassProperties::CalculateInertia;
+	sphere.mMassPropertiesOverride.mMass = 10.0f;
+	sphere.mAngularDamping = 0.5f;
+	default_random_engine random;
+	for (int i = 0; i < 200; ++i)
+	{
+		uniform_real_distribution<float> theta(0, JPH_PI);
+		uniform_real_distribution<float> phi(0, 2 * JPH_PI);
+		sphere.mPosition = RVec3(1.1f * cPlanetRadius * Vec3::sUnitSpherical(theta(random), phi(random)));
+		mBodyInterface->CreateAndAddBody(sphere, EActivation::Activate);
+	}
+
+	// Register to receive OnStep callbacks to apply gravity
+	mPhysicsSystem->AddStepListener(this);
+
+	// Create 'player' character
+	Ref<CharacterVirtualSettings> settings = new CharacterVirtualSettings();
+	settings->mShape = RotatedTranslatedShapeSettings(Vec3(0, 0.5f * cCharacterHeightStanding + cCharacterRadiusStanding, 0), Quat::sIdentity(), new CapsuleShape(0.5f * cCharacterHeightStanding, cCharacterRadiusStanding)).Create().Get();
+	settings->mSupportingVolume = Plane(Vec3::sAxisY(), -cCharacterRadiusStanding); // Accept contacts that touch the lower sphere of the capsule
+	mCharacter = new CharacterVirtual(settings, RVec3(0, cPlanetRadius, 0), Quat::sIdentity(), 0, mPhysicsSystem);
+	mCharacter->SetListener(this);
+}
+
+void CharacterPlanetTest::ProcessInput(const ProcessInputParams &inParams)
+{
+	// Determine controller input
+	Vec3 control_input = Vec3::sZero();
+	if (inParams.mKeyboard->IsKeyPressed(EKey::Left))	control_input.SetZ(-1);
+	if (inParams.mKeyboard->IsKeyPressed(EKey::Right))	control_input.SetZ(1);
+	if (inParams.mKeyboard->IsKeyPressed(EKey::Up))		control_input.SetX(1);
+	if (inParams.mKeyboard->IsKeyPressed(EKey::Down))	control_input.SetX(-1);
+	if (control_input != Vec3::sZero())
+		control_input = control_input.Normalized();
+
+	// Smooth the player input
+	mDesiredVelocity = 0.25f * control_input * cCharacterSpeed + 0.75f * mDesiredVelocity;
+
+	// Convert player input to world space
+	Vec3 up = mCharacter->GetUp();
+	Vec3 right = inParams.mCameraState.mForward.Cross(up).NormalizedOr(Vec3::sAxisZ());
+	Vec3 forward = up.Cross(right).NormalizedOr(Vec3::sAxisX());
+	mDesiredVelocityWS = right * mDesiredVelocity.GetZ() + forward * mDesiredVelocity.GetX();
+
+	// Check actions
+	mJump = inParams.mKeyboard->IsKeyPressedAndTriggered(EKey::RControl, mWasJump);
+}
+
+void CharacterPlanetTest::PrePhysicsUpdate(const PreUpdateParams &inParams)
+{
+	// Calculate up vector based on position on planet surface
+	Vec3 old_up = mCharacter->GetUp();
+	Vec3 up = Vec3(mCharacter->GetPosition()).Normalized();
+	mCharacter->SetUp(up);
+
+	// Rotate capsule so it points up relative to the planet surface
+	mCharacter->SetRotation((Quat::sFromTo(old_up, up) * mCharacter->GetRotation()).Normalized());
+
+	// Draw character pre update (the sim is also drawn pre update)
+#ifdef JPH_DEBUG_RENDERER
+	mCharacter->GetShape()->Draw(mDebugRenderer, mCharacter->GetCenterOfMassTransform(), Vec3::sOne(), Color::sGreen, false, true);
+#endif // JPH_DEBUG_RENDERER
+
+	// Determine new character velocity
+	Vec3 current_vertical_velocity = mCharacter->GetLinearVelocity().Dot(up) * up;
+	Vec3 ground_velocity = mCharacter->GetGroundVelocity();
+	Vec3 new_velocity;
+	if (mCharacter->GetGroundState() == CharacterVirtual::EGroundState::OnGround // If on ground
+		&& (current_vertical_velocity - ground_velocity).Dot(up) < 0.1f) // And not moving away from ground
+	{
+		// Assume velocity of ground when on ground
+		new_velocity = ground_velocity;
+
+		// Jump
+		if (mJump)
+			new_velocity += cJumpSpeed * up;
+	}
+	else
+		new_velocity = current_vertical_velocity;
+
+	// Apply gravity
+	Vec3 gravity = -up * mPhysicsSystem->GetGravity().Length();
+	new_velocity += gravity * inParams.mDeltaTime;
+
+	// Apply player input
+	new_velocity += mDesiredVelocityWS;
+
+	// Update character velocity
+	mCharacter->SetLinearVelocity(new_velocity);
+
+	// Update the character position
+	CharacterVirtual::ExtendedUpdateSettings update_settings;
+	mCharacter->ExtendedUpdate(inParams.mDeltaTime,
+		gravity,
+		update_settings,
+		mPhysicsSystem->GetDefaultBroadPhaseLayerFilter(Layers::MOVING),
+		mPhysicsSystem->GetDefaultLayerFilter(Layers::MOVING),
+		{ },
+		{ },
+		*mTempAllocator);
+}
+
+void CharacterPlanetTest::GetInitialCamera(CameraState& ioState) const
+{
+	ioState.mPos = RVec3(0, 0.5f, 0);
+	ioState.mForward = Vec3(1, -0.3f, 0).Normalized();
+}
+
+RMat44 CharacterPlanetTest::GetCameraPivot(float inCameraHeading, float inCameraPitch) const
+{
+	// Pivot is center of character + distance behind based on the heading and pitch of the camera.
+	Vec3 fwd = Vec3(Cos(inCameraPitch) * Cos(inCameraHeading), Sin(inCameraPitch), Cos(inCameraPitch) * Sin(inCameraHeading));
+	RVec3 cam_pos = mCharacter->GetPosition() - 5.0f * (mCharacter->GetRotation() * fwd);
+	return RMat44::sRotationTranslation(mCharacter->GetRotation(), cam_pos);
+}
+
+void CharacterPlanetTest::SaveState(StateRecorder &inStream) const
+{
+	mCharacter->SaveState(inStream);
+
+	// Save character up, it's not stored by default but we use it in this case update the rotation of the character
+	inStream.Write(mCharacter->GetUp());
+}
+
+void CharacterPlanetTest::RestoreState(StateRecorder &inStream)
+{
+	mCharacter->RestoreState(inStream);
+
+	Vec3 up = mCharacter->GetUp();
+	inStream.Read(up);
+	mCharacter->SetUp(up);
+}
+
+void CharacterPlanetTest::SaveInputState(StateRecorder &inStream) const
+{
+	inStream.Write(mDesiredVelocity);
+	inStream.Write(mDesiredVelocityWS);
+	inStream.Write(mJump);
+}
+
+void CharacterPlanetTest::RestoreInputState(StateRecorder &inStream)
+{
+	inStream.Read(mDesiredVelocity);
+	inStream.Read(mDesiredVelocityWS);
+	inStream.Read(mJump);
+}
+
+void CharacterPlanetTest::OnStep(const PhysicsStepListenerContext &inContext)
+{
+	// Use the length of the global gravity vector
+	float gravity = inContext.mPhysicsSystem->GetGravity().Length();
+
+	// We don't need to lock the bodies since they're already locked in the OnStep callback.
+	// Note that this means we're responsible for avoiding race conditions with other step listeners while accessing bodies.
+	// We know that this is safe because in this demo there's only one step listener.
+	const BodyLockInterface &body_interface = inContext.mPhysicsSystem->GetBodyLockInterfaceNoLock();
+
+	// Loop over all active bodies
+	BodyIDVector body_ids;
+	inContext.mPhysicsSystem->GetActiveBodies(EBodyType::RigidBody, body_ids);
+	for (const BodyID &id : body_ids)
+	{
+		BodyLockWrite lock(body_interface, id);
+		if (lock.Succeeded())
+		{
+			// Apply gravity towards the center of the planet
+			Body &body = lock.GetBody();
+			RVec3 position = body.GetPosition();
+			float mass = 1.0f / body.GetMotionProperties()->GetInverseMass();
+			body.AddForce(-gravity * mass * Vec3(position).Normalized());
+		}
+	}
+}
+
+void CharacterPlanetTest::OnContactAdded(const CharacterVirtual *inCharacter, const BodyID &inBodyID2, const SubShapeID &inSubShapeID2, RVec3Arg inContactPosition, Vec3Arg inContactNormal, CharacterContactSettings &ioSettings)
+{
+	// We don't want the spheres to push the player character
+	ioSettings.mCanPushCharacter = false;
+}

lib/All/JoltPhysics/Samples/Tests/Character/CharacterPlanetTest.h

@@ -0,0 +1,69 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2024 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/Test.h>
+#include <Jolt/Physics/Character/CharacterVirtual.h>
+#include <Jolt/Physics/PhysicsStepListener.h>
+
+class CharacterPlanetTest : public Test, public PhysicsStepListener, public CharacterContactListener
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, CharacterPlanetTest)
+
+	// Description of the test
+	virtual const char *	GetDescription() const override
+	{
+		return "Demonstrates how to do custom gravity to simulate a character walking on a planet.";
+	}
+
+	// Initialize the test
+	virtual void			Initialize() override;
+
+	// Process input
+	virtual void			ProcessInput(const ProcessInputParams &inParams) override;
+
+	// Update the test, called before the physics update
+	virtual void			PrePhysicsUpdate(const PreUpdateParams &inParams) override;
+
+	// Override to specify the initial camera state (local to GetCameraPivot)
+	virtual void			GetInitialCamera(CameraState &ioState) const override;
+
+	// Override to specify a camera pivot point and orientation (world space)
+	virtual RMat44			GetCameraPivot(float inCameraHeading, float inCameraPitch) const override;
+
+	// Saving / restoring state for replay
+	virtual void			SaveState(StateRecorder &inStream) const override;
+	virtual void			RestoreState(StateRecorder &inStream) override;
+
+	// Saving / restoring controller input state for replay
+	virtual void			SaveInputState(StateRecorder &inStream) const override;
+	virtual void			RestoreInputState(StateRecorder &inStream) override;
+
+	// See: PhysicsStepListener
+	virtual void			OnStep(const PhysicsStepListenerContext &inContext) override;
+
+	// See: CharacterContactListener
+	virtual void			OnContactAdded(const CharacterVirtual *inCharacter, const BodyID &inBodyID2, const SubShapeID &inSubShapeID2, RVec3Arg inContactPosition, Vec3Arg inContactNormal, CharacterContactSettings &ioSettings) override;
+
+private:
+	// Planet size
+	static constexpr float	cPlanetRadius = 20.0f;
+
+	// Character size
+	static constexpr float	cCharacterHeightStanding = 1.35f;
+	static constexpr float	cCharacterRadiusStanding = 0.3f;
+	static constexpr float	cCharacterSpeed = 6.0f;
+	static constexpr float	cJumpSpeed = 4.0f;
+
+	// The 'player' character
+	Ref<CharacterVirtual>	mCharacter;
+
+	// Player input
+	Vec3					mDesiredVelocity = Vec3::sZero();
+	Vec3					mDesiredVelocityWS = Vec3::sZero();
+	bool					mJump = false;
+	bool					mWasJump = false;
+};

lib/All/JoltPhysics/Samples/Tests/Character/CharacterSpaceShipTest.cpp

@@ -0,0 +1,196 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/Character/CharacterSpaceShipTest.h>
+#include <Jolt/Physics/Collision/Shape/CapsuleShape.h>
+#include <Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h>
+#include <Jolt/Physics/Collision/Shape/StaticCompoundShape.h>
+#include <Jolt/Physics/Collision/Shape/CylinderShape.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Layers.h>
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(CharacterSpaceShipTest)
+{
+	JPH_ADD_BASE_CLASS(CharacterSpaceShipTest, Test)
+}
+
+void CharacterSpaceShipTest::Initialize()
+{
+	// Dimensions of our space ship
+	constexpr float cSpaceShipHeight = 2.0f;
+	constexpr float cSpaceShipRingHeight = 0.2f;
+	constexpr float cSpaceShipRadius = 100.0f;
+	const RVec3 cShipInitialPosition(-25, 15, 0);
+
+	// Create floor for reference
+	CreateFloor();
+
+	// Create 'player' character
+	Ref<CharacterVirtualSettings> settings = new CharacterVirtualSettings();
+	settings->mShape = RotatedTranslatedShapeSettings(Vec3(0, 0.5f * cCharacterHeightStanding + cCharacterRadiusStanding, 0), Quat::sIdentity(), new CapsuleShape(0.5f * cCharacterHeightStanding, cCharacterRadiusStanding)).Create().Get();
+	settings->mSupportingVolume = Plane(Vec3::sAxisY(), -cCharacterRadiusStanding); // Accept contacts that touch the lower sphere of the capsule
+	mCharacter = new CharacterVirtual(settings, cShipInitialPosition + Vec3(0, cSpaceShipHeight, 0), Quat::sIdentity(), 0, mPhysicsSystem);
+	mCharacter->SetListener(this);
+
+	// Create the space ship
+	StaticCompoundShapeSettings compound;
+	compound.SetEmbedded();
+	for (float h = cSpaceShipRingHeight; h < cSpaceShipHeight; h += cSpaceShipRingHeight)
+		compound.AddShape(Vec3::sZero(), Quat::sIdentity(), new CylinderShape(h, sqrt(Square(cSpaceShipRadius) - Square(cSpaceShipRadius - cSpaceShipHeight - cSpaceShipRingHeight + h))));
+	mSpaceShip = mBodyInterface->CreateAndAddBody(BodyCreationSettings(&compound, cShipInitialPosition, Quat::sIdentity(), EMotionType::Kinematic, Layers::MOVING), EActivation::Activate);
+	mSpaceShipPrevTransform = mBodyInterface->GetCenterOfMassTransform(mSpaceShip);
+}
+
+void CharacterSpaceShipTest::ProcessInput(const ProcessInputParams &inParams)
+{
+	// Determine controller input
+	Vec3 control_input = Vec3::sZero();
+	if (inParams.mKeyboard->IsKeyPressed(EKey::Left))	control_input.SetZ(-1);
+	if (inParams.mKeyboard->IsKeyPressed(EKey::Right))	control_input.SetZ(1);
+	if (inParams.mKeyboard->IsKeyPressed(EKey::Up))		control_input.SetX(1);
+	if (inParams.mKeyboard->IsKeyPressed(EKey::Down))	control_input.SetX(-1);
+	if (control_input != Vec3::sZero())
+		control_input = control_input.Normalized();
+
+	// Calculate the desired velocity in local space to the ship based on the camera forward
+	RMat44 new_space_ship_transform = mBodyInterface->GetCenterOfMassTransform(mSpaceShip);
+	Vec3 cam_fwd = new_space_ship_transform.GetRotation().Multiply3x3Transposed(inParams.mCameraState.mForward);
+	cam_fwd.SetY(0.0f);
+	cam_fwd = cam_fwd.NormalizedOr(Vec3::sAxisX());
+	Quat rotation = Quat::sFromTo(Vec3::sAxisX(), cam_fwd);
+	control_input = rotation * control_input;
+
+	// Smooth the player input in local space to the ship
+	mDesiredVelocity = 0.25f * control_input * cCharacterSpeed + 0.75f * mDesiredVelocity;
+
+	// Check actions
+	mJump = inParams.mKeyboard->IsKeyPressedAndTriggered(EKey::RControl, mWasJump);
+}
+
+void CharacterSpaceShipTest::PrePhysicsUpdate(const PreUpdateParams &inParams)
+{
+	// Update scene time
+	mTime += inParams.mDeltaTime;
+
+	// Update the character so it stays relative to the space ship
+	RMat44 new_space_ship_transform = mBodyInterface->GetCenterOfMassTransform(mSpaceShip);
+	mCharacter->SetPosition(new_space_ship_transform * mSpaceShipPrevTransform.Inversed() * mCharacter->GetPosition());
+
+	// Update the character rotation and its up vector to match the new up vector of the ship
+	mCharacter->SetUp(new_space_ship_transform.GetAxisY());
+	mCharacter->SetRotation(new_space_ship_transform.GetQuaternion());
+
+	// Draw character pre update (the sim is also drawn pre update)
+	// Note that we have first updated the position so that it matches the new position of the ship
+#ifdef JPH_DEBUG_RENDERER
+	mCharacter->GetShape()->Draw(mDebugRenderer, mCharacter->GetCenterOfMassTransform(), Vec3::sOne(), Color::sGreen, false, true);
+#endif // JPH_DEBUG_RENDERER
+
+	// Determine new character velocity
+	Vec3 current_vertical_velocity = mCharacter->GetLinearVelocity().Dot(mSpaceShipPrevTransform.GetAxisY()) * mCharacter->GetUp();
+	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
+		if (mJump)
+			new_velocity += cJumpSpeed * mCharacter->GetUp();
+	}
+	else
+		new_velocity = current_vertical_velocity;
+
+	// Gravity always acts relative to the ship
+	Vec3 gravity = new_space_ship_transform.Multiply3x3(mPhysicsSystem->GetGravity());
+	new_velocity += gravity * inParams.mDeltaTime;
+
+	// Transform player input to world space
+	new_velocity += new_space_ship_transform.Multiply3x3(mDesiredVelocity);
+
+	// Update character velocity
+	mCharacter->SetLinearVelocity(new_velocity);
+
+	// Update the character position
+	CharacterVirtual::ExtendedUpdateSettings update_settings;
+	mCharacter->ExtendedUpdate(inParams.mDeltaTime,
+		gravity,
+		update_settings,
+		mPhysicsSystem->GetDefaultBroadPhaseLayerFilter(Layers::MOVING),
+		mPhysicsSystem->GetDefaultLayerFilter(Layers::MOVING),
+		{ },
+		{ },
+		*mTempAllocator);
+
+	// Update previous transform
+	mSpaceShipPrevTransform = new_space_ship_transform;
+
+	// Calculate new velocity
+	UpdateShipVelocity();
+}
+
+void CharacterSpaceShipTest::UpdateShipVelocity()
+{
+	// Make it a rocky ride...
+	mSpaceShipLinearVelocity = Vec3(Sin(mTime), 0, Cos(mTime)) * 50.0f;
+	mSpaceShipAngularVelocity = Vec3(Sin(2.0f * mTime), 1, Cos(2.0f * mTime)) * 0.5f;
+
+	mBodyInterface->SetLinearAndAngularVelocity(mSpaceShip, mSpaceShipLinearVelocity, mSpaceShipAngularVelocity);
+}
+
+void CharacterSpaceShipTest::GetInitialCamera(CameraState& ioState) const
+{
+	// This will become the local space offset, look down the x axis and slightly down
+	ioState.mPos = RVec3::sZero();
+	ioState.mForward = Vec3(10.0f, -2.0f, 0).Normalized();
+}
+
+RMat44 CharacterSpaceShipTest::GetCameraPivot(float inCameraHeading, float inCameraPitch) const
+{
+	// Pivot is center of character + distance behind based on the heading and pitch of the camera
+	Vec3 fwd = Vec3(Cos(inCameraPitch) * Cos(inCameraHeading), Sin(inCameraPitch), Cos(inCameraPitch) * Sin(inCameraHeading));
+	return RMat44::sTranslation(mCharacter->GetPosition() + Vec3(0, cCharacterHeightStanding + cCharacterRadiusStanding, 0) - 5.0f * fwd);
+}
+
+void CharacterSpaceShipTest::SaveState(StateRecorder &inStream) const
+{
+	mCharacter->SaveState(inStream);
+
+	inStream.Write(mTime);
+	inStream.Write(mSpaceShipPrevTransform);
+}
+
+void CharacterSpaceShipTest::RestoreState(StateRecorder &inStream)
+{
+	mCharacter->RestoreState(inStream);
+
+	inStream.Read(mTime);
+	inStream.Read(mSpaceShipPrevTransform);
+
+	// Calculate new velocity
+	UpdateShipVelocity();
+}
+
+void CharacterSpaceShipTest::SaveInputState(StateRecorder &inStream) const
+{
+	inStream.Write(mDesiredVelocity);
+	inStream.Write(mJump);
+}
+
+void CharacterSpaceShipTest::RestoreInputState(StateRecorder &inStream)
+{
+	inStream.Read(mDesiredVelocity);
+	inStream.Read(mJump);
+}
+
+void CharacterSpaceShipTest::OnAdjustBodyVelocity(const CharacterVirtual *inCharacter, const Body &inBody2, Vec3 &ioLinearVelocity, Vec3 &ioAngularVelocity)
+{
+	// Cancel out velocity of space ship, we move relative to this which means we don't feel any of the acceleration of the ship (= engage inertial dampeners!)
+	ioLinearVelocity -= mSpaceShipLinearVelocity;
+	ioAngularVelocity -= mSpaceShipAngularVelocity;
+}

lib/All/JoltPhysics/Samples/Tests/Character/CharacterSpaceShipTest.h

@@ -0,0 +1,79 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/Test.h>
+#include <Jolt/Physics/Character/CharacterVirtual.h>
+
+class CharacterSpaceShipTest : public Test, public CharacterContactListener
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, CharacterSpaceShipTest)
+
+	// Description of the test
+	virtual const char *	GetDescription() const override
+	{
+		return	"Demonstrates how a character may walk around a fast moving/accelerating sci-fi space ship that is equipped with inertial dampeners.\n"
+				"Note that this is 'game physics' and not real physics, inertial dampeners only exist in the movies.\n"
+				"You can walk off the ship and remain attached to the ship. A proper implementation would detect this and detach the character.";
+	}
+
+	// Initialize the test
+	virtual void			Initialize() override;
+
+	// Process input
+	virtual void			ProcessInput(const ProcessInputParams &inParams) override;
+
+	// Update the test, called before the physics update
+	virtual void			PrePhysicsUpdate(const PreUpdateParams &inParams) override;
+
+	// Override to specify the initial camera state (local to GetCameraPivot)
+	virtual void			GetInitialCamera(CameraState &ioState) const override;
+
+	// Override to specify a camera pivot point and orientation (world space)
+	virtual RMat44			GetCameraPivot(float inCameraHeading, float inCameraPitch) const override;
+
+	// Saving / restoring state for replay
+	virtual void			SaveState(StateRecorder &inStream) const override;
+	virtual void			RestoreState(StateRecorder &inStream) override;
+
+	// Saving / restoring controller input state for replay
+	virtual void			SaveInputState(StateRecorder &inStream) const override;
+	virtual void			RestoreInputState(StateRecorder &inStream) override;
+
+private:
+	// Calculate new ship velocity
+	void					UpdateShipVelocity();
+
+	/// Callback to adjust the velocity of a body as seen by the character. Can be adjusted to e.g. implement a conveyor belt or an inertial dampener system of a sci-fi space ship.
+	virtual void			OnAdjustBodyVelocity(const CharacterVirtual *inCharacter, const Body &inBody2, Vec3 &ioLinearVelocity, Vec3 &ioAngularVelocity) override;
+
+	// Character size
+	static constexpr float	cCharacterHeightStanding = 1.35f;
+	static constexpr float	cCharacterRadiusStanding = 0.3f;
+	static constexpr float	cCharacterSpeed = 6.0f;
+	static constexpr float	cJumpSpeed = 4.0f;
+
+	// The 'player' character
+	Ref<CharacterVirtual>	mCharacter;
+
+	// The space ship
+	BodyID					mSpaceShip;
+
+	// Previous frame space ship transform
+	RMat44					mSpaceShipPrevTransform;
+
+	// Space ship velocity
+	Vec3					mSpaceShipLinearVelocity;
+	Vec3					mSpaceShipAngularVelocity;
+
+	// Global time
+	float					mTime = 0.0f;
+
+	// Player input
+	Vec3					mDesiredVelocity = Vec3::sZero();
+	bool					mJump = false;
+	bool					mWasJump = false;
+};

lib/All/JoltPhysics/Samples/Tests/Character/CharacterTest.cpp

@@ -0,0 +1,123 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/Character/CharacterTest.h>
+#include <Layers.h>
+#include <Renderer/DebugRendererImp.h>
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(CharacterTest)
+{
+	JPH_ADD_BASE_CLASS(CharacterTest, CharacterBaseTest)
+}
+
+static const float cCollisionTolerance = 0.05f;
+
+CharacterTest::~CharacterTest()
+{
+	mCharacter->RemoveFromPhysicsSystem();
+}
+
+void CharacterTest::Initialize()
+{
+	CharacterBaseTest::Initialize();
+
+	// Create 'player' character
+	Ref<CharacterSettings> settings = new CharacterSettings();
+	settings->mMaxSlopeAngle = DegreesToRadians(45.0f);
+	settings->mLayer = Layers::MOVING;
+	settings->mShape = mStandingShape;
+	settings->mFriction = 0.5f;
+	settings->mSupportingVolume = Plane(Vec3::sAxisY(), -cCharacterRadiusStanding); // Accept contacts that touch the lower sphere of the capsule
+	mCharacter = new Character(settings, RVec3::sZero(), Quat::sIdentity(), 0, mPhysicsSystem);
+	mCharacter->AddToPhysicsSystem(EActivation::Activate);
+}
+
+void CharacterTest::PrePhysicsUpdate(const PreUpdateParams &inParams)
+{
+	CharacterBaseTest::PrePhysicsUpdate(inParams);
+
+	// Draw state of character
+	DrawCharacterState(mCharacter, mCharacter->GetWorldTransform(), mCharacter->GetLinearVelocity());
+}
+
+void CharacterTest::PostPhysicsUpdate(float inDeltaTime)
+{
+	// Fetch the new ground properties
+	mCharacter->PostSimulation(cCollisionTolerance);
+}
+
+void CharacterTest::SaveState(StateRecorder &inStream) const
+{
+	CharacterBaseTest::SaveState(inStream);
+
+	mCharacter->SaveState(inStream);
+
+	bool is_standing = mCharacter->GetShape() == mStandingShape;
+	inStream.Write(is_standing);
+}
+
+void CharacterTest::RestoreState(StateRecorder &inStream)
+{
+	CharacterBaseTest::RestoreState(inStream);
+
+	mCharacter->RestoreState(inStream);
+
+	bool is_standing = mCharacter->GetShape() == mStandingShape; // Initialize variable for validation mode
+	inStream.Read(is_standing);
+	mCharacter->SetShape(is_standing? mStandingShape : mCrouchingShape, FLT_MAX);
+}
+
+void CharacterTest::HandleInput(Vec3Arg inMovementDirection, bool inJump, bool inSwitchStance, float inDeltaTime)
+{
+	// Cancel movement in opposite direction of normal when touching something we can't walk up
+	Vec3 movement_direction = inMovementDirection;
+	Character::EGroundState ground_state = mCharacter->GetGroundState();
+	if (ground_state == Character::EGroundState::OnSteepGround
+		|| ground_state == Character::EGroundState::NotSupported)
+	{
+		Vec3 normal = mCharacter->GetGroundNormal();
+		normal.SetY(0.0f);
+		float dot = normal.Dot(movement_direction);
+		if (dot < 0.0f)
+			movement_direction -= (dot * normal) / normal.LengthSq();
+	}
+
+	// Stance switch
+	if (inSwitchStance)
+		mCharacter->SetShape(mCharacter->GetShape() == mStandingShape? mCrouchingShape : mStandingShape, 1.5f * mPhysicsSystem->GetPhysicsSettings().mPenetrationSlop);
+
+	if (sControlMovementDuringJump || mCharacter->IsSupported())
+	{
+		// Update velocity
+		Vec3 current_velocity = mCharacter->GetLinearVelocity();
+		Vec3 desired_velocity = sCharacterSpeed * movement_direction;
+		if (!desired_velocity.IsNearZero() || current_velocity.GetY() < 0.0f || !mCharacter->IsSupported())
+			desired_velocity.SetY(current_velocity.GetY());
+		Vec3 new_velocity = 0.75f * current_velocity + 0.25f * desired_velocity;
+
+		// Jump
+		if (inJump && ground_state == Character::EGroundState::OnGround)
+			new_velocity += Vec3(0, sJumpSpeed, 0);
+
+		// Update the velocity
+		mCharacter->SetLinearVelocity(new_velocity);
+	}
+}
+
+void CharacterTest::OnContactAdded(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings)
+{
+	// Draw a box around the character when it enters the sensor
+	if (inBody1.GetID() == mSensorBody)
+		mDebugRenderer->DrawBox(inBody2.GetWorldSpaceBounds(), Color::sGreen, DebugRenderer::ECastShadow::Off, DebugRenderer::EDrawMode::Wireframe);
+	else if (inBody2.GetID() == mSensorBody)
+		mDebugRenderer->DrawBox(inBody1.GetWorldSpaceBounds(), Color::sGreen, DebugRenderer::ECastShadow::Off, DebugRenderer::EDrawMode::Wireframe);
+}
+
+void CharacterTest::OnContactPersisted(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings)
+{
+	// Same behavior as contact added
+	OnContactAdded(inBody1, inBody2, inManifold, ioSettings);
+}

lib/All/JoltPhysics/Samples/Tests/Character/CharacterTest.h

@@ -0,0 +1,54 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/Character/CharacterBaseTest.h>
+
+class CharacterTest : public CharacterBaseTest, public ContactListener
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, CharacterTest)
+
+	// Description of the test
+	virtual const char *	GetDescription() const override
+	{
+		return	"Shows the Character class. Move around with the arrow keys, Shift for crouch and Ctrl for jump.\n"
+				"Note that most games should use CharacterVirtual instead of the Character class.";
+	}
+
+	// Destructor
+	virtual					~CharacterTest() override;
+
+	// Initialize the test
+	virtual void			Initialize() override;
+
+	// Update the test, called before the physics update
+	virtual void			PrePhysicsUpdate(const PreUpdateParams &inParams) override;
+
+	// Update the test, called after the physics update
+	virtual void			PostPhysicsUpdate(float inDeltaTime) override;
+
+	// Saving / restoring state for replay
+	virtual void			SaveState(StateRecorder &inStream) const override;
+	virtual void			RestoreState(StateRecorder &inStream) override;
+
+	// If this test implements a contact listener, it should be returned here
+	virtual ContactListener *GetContactListener() override	{ return this; }
+
+	// See: ContactListener
+	virtual void			OnContactAdded(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings) override;
+	virtual void			OnContactPersisted(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings) override;
+
+protected:
+	// Get position of the character
+	virtual RVec3			GetCharacterPosition() const override				{ return mCharacter->GetPosition(); }
+
+	// Handle user input to the character
+	virtual void			HandleInput(Vec3Arg inMovementDirection, bool inJump, bool inSwitchStance, float inDeltaTime) override;
+
+private:
+	// The 'player' character
+	Ref<Character>			mCharacter;
+};

lib/All/JoltPhysics/Samples/Tests/Character/CharacterVirtualTest.cpp

@@ -0,0 +1,388 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/Character/CharacterVirtualTest.h>
+#include <Jolt/Physics/Collision/Shape/CapsuleShape.h>
+#include <Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h>
+#include <Layers.h>
+#include <Utils/Log.h>
+#include <Renderer/DebugRendererImp.h>
+#include <Application/DebugUI.h>
+
+//#define CHARACTER_TRACE_CONTACTS
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(CharacterVirtualTest)
+{
+	JPH_ADD_BASE_CLASS(CharacterVirtualTest, CharacterBaseTest)
+}
+
+void CharacterVirtualTest::Initialize()
+{
+	CharacterBaseTest::Initialize();
+
+	// Create 'player' character
+	Ref<CharacterVirtualSettings> settings = new CharacterVirtualSettings();
+	settings->mMaxSlopeAngle = sMaxSlopeAngle;
+	settings->mMaxStrength = sMaxStrength;
+	settings->mShape = mStandingShape;
+	settings->mBackFaceMode = sBackFaceMode;
+	settings->mCharacterPadding = sCharacterPadding;
+	settings->mPenetrationRecoverySpeed = sPenetrationRecoverySpeed;
+	settings->mPredictiveContactDistance = sPredictiveContactDistance;
+	settings->mSupportingVolume = Plane(Vec3::sAxisY(), -cCharacterRadiusStanding); // Accept contacts that touch the lower sphere of the capsule
+	settings->mEnhancedInternalEdgeRemoval = sEnhancedInternalEdgeRemoval;
+	settings->mInnerBodyShape = sCreateInnerBody? mInnerStandingShape : nullptr;
+	settings->mInnerBodyLayer = Layers::MOVING;
+	mCharacter = new CharacterVirtual(settings, RVec3::sZero(), Quat::sIdentity(), 0, mPhysicsSystem);
+	mCharacter->SetCharacterVsCharacterCollision(&mCharacterVsCharacterCollision);
+	mCharacterVsCharacterCollision.Add(mCharacter);
+
+	// Install contact listener for all characters
+	for (CharacterVirtual *character : mCharacterVsCharacterCollision.mCharacters)
+		character->SetListener(this);
+
+	// Draw labels on ramp blocks
+	for (size_t i = 0; i < mRampBlocks.size(); ++i)
+		SetBodyLabel(mRampBlocks[i], StringFormat("PushesPlayer: %s\nPushable: %s", (i & 1) != 0? "True" : "False", (i & 2) != 0? "True" : "False"));
+}
+
+void CharacterVirtualTest::PrePhysicsUpdate(const PreUpdateParams &inParams)
+{
+	CharacterBaseTest::PrePhysicsUpdate(inParams);
+
+	// Draw character pre update (the sim is also drawn pre update)
+	RMat44 com = mCharacter->GetCenterOfMassTransform();
+	RMat44 world_transform = mCharacter->GetWorldTransform();
+#ifdef JPH_DEBUG_RENDERER
+	mCharacter->GetShape()->Draw(mDebugRenderer, com, Vec3::sOne(), Color::sGreen, false, true);
+#endif // JPH_DEBUG_RENDERER
+
+	DrawPaddedCharacter(mCharacter->GetShape(), mCharacter->GetCharacterPadding(), com);
+
+	// Remember old position
+	RVec3 old_position = mCharacter->GetPosition();
+
+	// Settings for our update function
+	CharacterVirtual::ExtendedUpdateSettings update_settings;
+	if (!sEnableStickToFloor)
+		update_settings.mStickToFloorStepDown = Vec3::sZero();
+	else
+		update_settings.mStickToFloorStepDown = -mCharacter->GetUp() * update_settings.mStickToFloorStepDown.Length();
+	if (!sEnableWalkStairs)
+		update_settings.mWalkStairsStepUp = Vec3::sZero();
+	else
+		update_settings.mWalkStairsStepUp = mCharacter->GetUp() * update_settings.mWalkStairsStepUp.Length();
+
+	// Update the character position
+	mCharacter->ExtendedUpdate(inParams.mDeltaTime,
+		-mCharacter->GetUp() * mPhysicsSystem->GetGravity().Length(),
+		update_settings,
+		mPhysicsSystem->GetDefaultBroadPhaseLayerFilter(Layers::MOVING),
+		mPhysicsSystem->GetDefaultLayerFilter(Layers::MOVING),
+		{ },
+		{ },
+		*mTempAllocator);
+
+#ifdef JPH_ENABLE_ASSERTS
+	// Validate that our contact list is in sync with that of the character
+	// Note that compound shapes could be non convex so we may detect more contacts than have been reported by the character
+	// as the character only reports contacts as it is sliding through the world. If 2 sub shapes hit at the same time then
+	// most likely only one will be reported as it stops the character and prevents the 2nd one from being seen.
+	uint num_contacts = 0;
+	for (const CharacterVirtual::Contact &c : mCharacter->GetActiveContacts())
+		if (c.mHadCollision)
+		{
+			JPH_ASSERT(std::find(mActiveContacts.begin(), mActiveContacts.end(), c) != mActiveContacts.end());
+			num_contacts++;
+		}
+	JPH_ASSERT(sShapeType == EType::Compound? num_contacts >= mActiveContacts.size() : num_contacts == mActiveContacts.size());
+#endif
+
+	// Calculate effective velocity
+	RVec3 new_position = mCharacter->GetPosition();
+	Vec3 velocity = Vec3(new_position - old_position) / inParams.mDeltaTime;
+
+	// Draw state of character
+	DrawCharacterState(mCharacter, world_transform, velocity);
+}
+
+void CharacterVirtualTest::HandleInput(Vec3Arg inMovementDirection, bool inJump, bool inSwitchStance, float inDeltaTime)
+{
+	bool player_controls_horizontal_velocity = sControlMovementDuringJump || mCharacter->IsSupported();
+	if (player_controls_horizontal_velocity)
+	{
+		// Smooth the player input
+		mDesiredVelocity = sEnableCharacterInertia? 0.25f * inMovementDirection * sCharacterSpeed + 0.75f * mDesiredVelocity : inMovementDirection * sCharacterSpeed;
+
+		// True if the player intended to move
+		mAllowSliding = !inMovementDirection.IsNearZero();
+	}
+	else
+	{
+		// While in air we allow sliding
+		mAllowSliding = true;
+	}
+
+	// Update the character rotation and its up vector to match the up vector set by the user settings
+	Quat character_up_rotation = Quat::sEulerAngles(Vec3(sUpRotationX, 0, sUpRotationZ));
+	mCharacter->SetUp(character_up_rotation.RotateAxisY());
+	mCharacter->SetRotation(character_up_rotation);
+
+	// A cheaper way to update the character's ground velocity,
+	// the platforms that the character is standing on may have changed velocity
+	mCharacter->UpdateGroundVelocity();
+
+	// Determine new basic velocity
+	Vec3 current_vertical_velocity = mCharacter->GetLinearVelocity().Dot(mCharacter->GetUp()) * mCharacter->GetUp();
+	Vec3 ground_velocity = mCharacter->GetGroundVelocity();
+	Vec3 new_velocity;
+	bool moving_towards_ground = (current_vertical_velocity.GetY() - ground_velocity.GetY()) < 0.1f;
+	if (mCharacter->GetGroundState() == CharacterVirtual::EGroundState::OnGround	// If on ground
+		&& (sEnableCharacterInertia?
+			moving_towards_ground													// Inertia enabled: And not moving away from ground
+			: !mCharacter->IsSlopeTooSteep(mCharacter->GetGroundNormal())))			// Inertia disabled: And not on a slope that is too steep
+	{
+		// Assume velocity of ground when on ground
+		new_velocity = ground_velocity;
+
+		// Jump
+		if (inJump && moving_towards_ground)
+			new_velocity += sJumpSpeed * mCharacter->GetUp();
+	}
+	else
+		new_velocity = current_vertical_velocity;
+
+	// Gravity
+	new_velocity += (character_up_rotation * mPhysicsSystem->GetGravity()) * inDeltaTime;
+
+	if (player_controls_horizontal_velocity)
+	{
+		// Player input
+		new_velocity += character_up_rotation * mDesiredVelocity;
+	}
+	else
+	{
+		// Preserve horizontal velocity
+		Vec3 current_horizontal_velocity = mCharacter->GetLinearVelocity() - current_vertical_velocity;
+		new_velocity += current_horizontal_velocity;
+	}
+
+	// Update character velocity
+	mCharacter->SetLinearVelocity(new_velocity);
+
+	// Stance switch
+	if (inSwitchStance)
+	{
+		bool is_standing = mCharacter->GetShape() == mStandingShape;
+		const Shape *shape = is_standing? mCrouchingShape : mStandingShape;
+		if (mCharacter->SetShape(shape, 1.5f * mPhysicsSystem->GetPhysicsSettings().mPenetrationSlop, mPhysicsSystem->GetDefaultBroadPhaseLayerFilter(Layers::MOVING), mPhysicsSystem->GetDefaultLayerFilter(Layers::MOVING), { }, { }, *mTempAllocator))
+		{
+			const Shape *inner_shape = is_standing? mInnerCrouchingShape : mInnerStandingShape;
+			mCharacter->SetInnerBodyShape(inner_shape);
+		}
+	}
+}
+
+void CharacterVirtualTest::AddCharacterMovementSettings(DebugUI* inUI, UIElement* inSubMenu)
+{
+	inUI->CreateCheckBox(inSubMenu, "Enable Character Inertia", sEnableCharacterInertia, [](UICheckBox::EState inState) { sEnableCharacterInertia = inState == UICheckBox::STATE_CHECKED; });
+	inUI->CreateCheckBox(inSubMenu, "Player Can Push Other Virtual Characters", sPlayerCanPushOtherCharacters, [](UICheckBox::EState inState) { sPlayerCanPushOtherCharacters = inState == UICheckBox::STATE_CHECKED; });
+	inUI->CreateCheckBox(inSubMenu, "Other Virtual Characters Can Push Player", sOtherCharactersCanPushPlayer, [](UICheckBox::EState inState) { sOtherCharactersCanPushPlayer = inState == UICheckBox::STATE_CHECKED; });
+}
+
+void CharacterVirtualTest::AddConfigurationSettings(DebugUI *inUI, UIElement *inSubMenu)
+{
+	inUI->CreateComboBox(inSubMenu, "Back Face Mode", { "Ignore", "Collide" }, (int)sBackFaceMode, [=](int inItem) { sBackFaceMode = (EBackFaceMode)inItem; });
+	inUI->CreateSlider(inSubMenu, "Up Rotation X (degrees)", RadiansToDegrees(sUpRotationX), -90.0f, 90.0f, 1.0f, [](float inValue) { sUpRotationX = DegreesToRadians(inValue); });
+	inUI->CreateSlider(inSubMenu, "Up Rotation Z (degrees)", RadiansToDegrees(sUpRotationZ), -90.0f, 90.0f, 1.0f, [](float inValue) { sUpRotationZ = DegreesToRadians(inValue); });
+	inUI->CreateSlider(inSubMenu, "Max Slope Angle (degrees)", RadiansToDegrees(sMaxSlopeAngle), 0.0f, 90.0f, 1.0f, [](float inValue) { sMaxSlopeAngle = DegreesToRadians(inValue); });
+	inUI->CreateSlider(inSubMenu, "Max Strength (N)", sMaxStrength, 0.0f, 500.0f, 1.0f, [](float inValue) { sMaxStrength = inValue; });
+	inUI->CreateSlider(inSubMenu, "Character Padding", sCharacterPadding, 0.01f, 0.5f, 0.01f, [](float inValue) { sCharacterPadding = inValue; });
+	inUI->CreateSlider(inSubMenu, "Penetration Recovery Speed", sPenetrationRecoverySpeed, 0.0f, 1.0f, 0.05f, [](float inValue) { sPenetrationRecoverySpeed = inValue; });
+	inUI->CreateSlider(inSubMenu, "Predictive Contact Distance", sPredictiveContactDistance, 0.01f, 1.0f, 0.01f, [](float inValue) { sPredictiveContactDistance = inValue; });
+	inUI->CreateCheckBox(inSubMenu, "Enable Walk Stairs", sEnableWalkStairs, [](UICheckBox::EState inState) { sEnableWalkStairs = inState == UICheckBox::STATE_CHECKED; });
+	inUI->CreateCheckBox(inSubMenu, "Enable Stick To Floor", sEnableStickToFloor, [](UICheckBox::EState inState) { sEnableStickToFloor = inState == UICheckBox::STATE_CHECKED; });
+	inUI->CreateCheckBox(inSubMenu, "Enhanced Internal Edge Removal", sEnhancedInternalEdgeRemoval, [](UICheckBox::EState inState) { sEnhancedInternalEdgeRemoval = inState == UICheckBox::STATE_CHECKED; });
+	inUI->CreateCheckBox(inSubMenu, "Create Inner Body", sCreateInnerBody, [](UICheckBox::EState inState) { sCreateInnerBody = inState == UICheckBox::STATE_CHECKED; });
+}
+
+void CharacterVirtualTest::SaveState(StateRecorder &inStream) const
+{
+	CharacterBaseTest::SaveState(inStream);
+
+	mCharacter->SaveState(inStream);
+
+	bool is_standing = mCharacter->GetShape() == mStandingShape;
+	inStream.Write(is_standing);
+
+	inStream.Write(mAllowSliding);
+	inStream.Write(mDesiredVelocity);
+	inStream.Write(mActiveContacts);
+}
+
+void CharacterVirtualTest::RestoreState(StateRecorder &inStream)
+{
+	CharacterBaseTest::RestoreState(inStream);
+
+	mCharacter->RestoreState(inStream);
+
+	bool is_standing = mCharacter->GetShape() == mStandingShape; // Initialize variable for validation mode
+	inStream.Read(is_standing);
+	const Shape *shape = is_standing? mStandingShape : mCrouchingShape;
+	mCharacter->SetShape(shape, FLT_MAX, { }, { }, { }, { }, *mTempAllocator);
+	const Shape *inner_shape = is_standing? mInnerStandingShape : mInnerCrouchingShape;
+	mCharacter->SetInnerBodyShape(inner_shape);
+
+	inStream.Read(mAllowSliding);
+	inStream.Read(mDesiredVelocity);
+	inStream.Read(mActiveContacts);
+}
+
+void CharacterVirtualTest::OnAdjustBodyVelocity(const CharacterVirtual *inCharacter, const Body &inBody2, Vec3 &ioLinearVelocity, Vec3 &ioAngularVelocity)
+{
+	// Apply artificial velocity to the character when standing on the conveyor belt
+	if (inBody2.GetID() == mConveyorBeltBody)
+		ioLinearVelocity += Vec3(0, 0, 2);
+}
+
+void CharacterVirtualTest::OnContactCommon(const CharacterVirtual *inCharacter, const BodyID &inBodyID2, const SubShapeID &inSubShapeID2, RVec3Arg inContactPosition, Vec3Arg inContactNormal, CharacterContactSettings &ioSettings)
+{
+	// Draw a box around the character when it enters the sensor
+	if (inBodyID2 == mSensorBody)
+	{
+		AABox box = inCharacter->GetShape()->GetWorldSpaceBounds(inCharacter->GetCenterOfMassTransform(), Vec3::sOne());
+		mDebugRenderer->DrawBox(box, Color::sGreen, DebugRenderer::ECastShadow::Off, DebugRenderer::EDrawMode::Wireframe);
+	}
+
+	// Dynamic boxes on the ramp go through all permutations
+	Array<BodyID>::const_iterator i = find(mRampBlocks.begin(), mRampBlocks.end(), inBodyID2);
+	if (i != mRampBlocks.end())
+	{
+		size_t index = i - mRampBlocks.begin();
+		ioSettings.mCanPushCharacter = (index & 1) != 0;
+		ioSettings.mCanReceiveImpulses = (index & 2) != 0;
+	}
+
+	// If we encounter an object that can push the player, enable sliding
+	if (inCharacter == mCharacter
+		&& ioSettings.mCanPushCharacter
+		&& mPhysicsSystem->GetBodyInterface().GetMotionType(inBodyID2) != EMotionType::Static)
+		mAllowSliding = true;
+}
+
+void CharacterVirtualTest::OnContactAdded(const CharacterVirtual *inCharacter, const BodyID &inBodyID2, const SubShapeID &inSubShapeID2, RVec3Arg inContactPosition, Vec3Arg inContactNormal, CharacterContactSettings &ioSettings)
+{
+	OnContactCommon(inCharacter, inBodyID2, inSubShapeID2, inContactPosition, inContactNormal, ioSettings);
+
+	if (inCharacter == mCharacter)
+	{
+	#ifdef CHARACTER_TRACE_CONTACTS
+		Trace("Contact added with body %08x, sub shape %08x", inBodyID2.GetIndexAndSequenceNumber(), inSubShapeID2.GetValue());
+	#endif
+		CharacterVirtual::ContactKey c(inBodyID2, inSubShapeID2);
+		if (std::find(mActiveContacts.begin(), mActiveContacts.end(), c) != mActiveContacts.end())
+			FatalError("Got an add contact that should have been a persisted contact");
+		mActiveContacts.push_back(c);
+	}
+}
+
+void CharacterVirtualTest::OnContactPersisted(const CharacterVirtual *inCharacter, const BodyID &inBodyID2, const SubShapeID &inSubShapeID2, RVec3Arg inContactPosition, Vec3Arg inContactNormal, CharacterContactSettings &ioSettings)
+{
+	OnContactCommon(inCharacter, inBodyID2, inSubShapeID2, inContactPosition, inContactNormal, ioSettings);
+
+	if (inCharacter == mCharacter)
+	{
+	#ifdef CHARACTER_TRACE_CONTACTS
+		Trace("Contact persisted with body %08x, sub shape %08x", inBodyID2.GetIndexAndSequenceNumber(), inSubShapeID2.GetValue());
+	#endif
+		if (std::find(mActiveContacts.begin(), mActiveContacts.end(), CharacterVirtual::ContactKey(inBodyID2, inSubShapeID2)) == mActiveContacts.end())
+			FatalError("Got a persisted contact that should have been an add contact");
+	}
+}
+
+void CharacterVirtualTest::OnContactRemoved(const CharacterVirtual *inCharacter, const BodyID &inBodyID2, const SubShapeID &inSubShapeID2)
+{
+	if (inCharacter == mCharacter)
+	{
+	#ifdef CHARACTER_TRACE_CONTACTS
+		Trace("Contact removed with body %08x, sub shape %08x", inBodyID2.GetIndexAndSequenceNumber(), inSubShapeID2.GetValue());
+	#endif
+		ContactSet::iterator it = std::find(mActiveContacts.begin(), mActiveContacts.end(), CharacterVirtual::ContactKey(inBodyID2, inSubShapeID2));
+		if (it == mActiveContacts.end())
+			FatalError("Got a remove contact that has not been added");
+		mActiveContacts.erase(it);
+	}
+}
+
+void CharacterVirtualTest::OnCharacterContactCommon(const CharacterVirtual *inCharacter, const CharacterVirtual *inOtherCharacter, const SubShapeID &inSubShapeID2, RVec3Arg inContactPosition, Vec3Arg inContactNormal, CharacterContactSettings &ioSettings)
+{
+	// Characters can only be pushed in their own update
+	if (sPlayerCanPushOtherCharacters)
+		ioSettings.mCanPushCharacter = sOtherCharactersCanPushPlayer || inOtherCharacter == mCharacter;
+	else if (sOtherCharactersCanPushPlayer)
+		ioSettings.mCanPushCharacter = inCharacter == mCharacter;
+	else
+		ioSettings.mCanPushCharacter = false;
+
+	// If the player can be pushed by the other virtual character, we allow sliding
+	if (inCharacter == mCharacter && ioSettings.mCanPushCharacter)
+		mAllowSliding = true;
+}
+
+void CharacterVirtualTest::OnCharacterContactAdded(const CharacterVirtual *inCharacter, const CharacterVirtual *inOtherCharacter, const SubShapeID &inSubShapeID2, RVec3Arg inContactPosition, Vec3Arg inContactNormal, CharacterContactSettings &ioSettings)
+{
+	OnCharacterContactCommon(inCharacter, inOtherCharacter, inSubShapeID2, inContactPosition, inContactNormal, ioSettings);
+
+	if (inCharacter == mCharacter)
+	{
+	#ifdef CHARACTER_TRACE_CONTACTS
+		Trace("Contact added with character %08x, sub shape %08x", inOtherCharacter->GetID().GetValue(), inSubShapeID2.GetValue());
+	#endif
+		CharacterVirtual::ContactKey c(inOtherCharacter->GetID(), inSubShapeID2);
+		if (std::find(mActiveContacts.begin(), mActiveContacts.end(), c) != mActiveContacts.end())
+			FatalError("Got an add contact that should have been a persisted contact");
+		mActiveContacts.push_back(c);
+	}
+}
+
+void CharacterVirtualTest::OnCharacterContactPersisted(const CharacterVirtual *inCharacter, const CharacterVirtual *inOtherCharacter, const SubShapeID &inSubShapeID2, RVec3Arg inContactPosition, Vec3Arg inContactNormal, CharacterContactSettings &ioSettings)
+{
+	OnCharacterContactCommon(inCharacter, inOtherCharacter, inSubShapeID2, inContactPosition, inContactNormal, ioSettings);
+
+	if (inCharacter == mCharacter)
+	{
+	#ifdef CHARACTER_TRACE_CONTACTS
+		Trace("Contact persisted with character %08x, sub shape %08x", inOtherCharacter->GetID().GetValue(), inSubShapeID2.GetValue());
+	#endif
+		if (std::find(mActiveContacts.begin(), mActiveContacts.end(), CharacterVirtual::ContactKey(inOtherCharacter->GetID(), inSubShapeID2)) == mActiveContacts.end())
+			FatalError("Got a persisted contact that should have been an add contact");
+	}
+}
+
+void CharacterVirtualTest::OnCharacterContactRemoved(const CharacterVirtual *inCharacter, const CharacterID &inOtherCharacterID, const SubShapeID &inSubShapeID2)
+{
+	if (inCharacter == mCharacter)
+	{
+	#ifdef CHARACTER_TRACE_CONTACTS
+		Trace("Contact removed with character %08x, sub shape %08x", inOtherCharacterID.GetValue(), inSubShapeID2.GetValue());
+	#endif
+		ContactSet::iterator it = std::find(mActiveContacts.begin(), mActiveContacts.end(), CharacterVirtual::ContactKey(inOtherCharacterID, inSubShapeID2));
+		if (it == mActiveContacts.end())
+			FatalError("Got a remove contact that has not been added");
+		mActiveContacts.erase(it);
+	}
+}
+
+void CharacterVirtualTest::OnContactSolve(const CharacterVirtual *inCharacter, const BodyID &inBodyID2, const SubShapeID &inSubShapeID2, RVec3Arg inContactPosition, Vec3Arg inContactNormal, Vec3Arg inContactVelocity, const PhysicsMaterial *inContactMaterial, Vec3Arg inCharacterVelocity, Vec3 &ioNewCharacterVelocity)
+{
+	// Ignore callbacks for other characters than the player
+	if (inCharacter != mCharacter)
+		return;
+
+	// Don't allow the player to slide down static not-too-steep surfaces when not actively moving and when not on a moving platform
+	if (!mAllowSliding && inContactVelocity.IsNearZero() && !inCharacter->IsSlopeTooSteep(inContactNormal))
+		ioNewCharacterVelocity = Vec3::sZero();
+}

lib/All/JoltPhysics/Samples/Tests/Character/CharacterVirtualTest.h

@@ -0,0 +1,103 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/Character/CharacterBaseTest.h>
+
+class CharacterVirtualTest : public CharacterBaseTest, public CharacterContactListener
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, CharacterVirtualTest)
+
+	// Description of the test
+	virtual const char *	GetDescription() const override
+	{
+		return "Shows the CharacterVirtual class. Move around with the arrow keys, Shift for crouch and Ctrl for jump.";
+	}
+
+	// Initialize the test
+	virtual void			Initialize() override;
+
+	// Update the test, called before the physics update
+	virtual void			PrePhysicsUpdate(const PreUpdateParams &inParams) override;
+
+	// Saving / restoring state for replay
+	virtual void			SaveState(StateRecorder &inStream) const override;
+	virtual void			RestoreState(StateRecorder &inStream) override;
+
+	/// Callback to adjust the velocity of a body as seen by the character. Can be adjusted to e.g. implement a conveyor belt or an inertial dampener system of a sci-fi space ship.
+	virtual void			OnAdjustBodyVelocity(const CharacterVirtual *inCharacter, const Body &inBody2, Vec3 &ioLinearVelocity, Vec3 &ioAngularVelocity) override;
+
+	// Called whenever the character collides with a body.
+	virtual void			OnContactAdded(const CharacterVirtual *inCharacter, const BodyID &inBodyID2, const SubShapeID &inSubShapeID2, RVec3Arg inContactPosition, Vec3Arg inContactNormal, CharacterContactSettings &ioSettings) override;
+
+	// Called whenever the character persists colliding with a body.
+	virtual void			OnContactPersisted(const CharacterVirtual *inCharacter, const BodyID &inBodyID2, const SubShapeID &inSubShapeID2, RVec3Arg inContactPosition, Vec3Arg inContactNormal, CharacterContactSettings &ioSettings) override;
+
+	// Called whenever the character loses contact with a body.
+	virtual void			OnContactRemoved(const CharacterVirtual *inCharacter, const BodyID &inBodyID2, const SubShapeID &inSubShapeID2) override;
+
+	// Called whenever the character collides with a virtual character.
+	virtual void			OnCharacterContactAdded(const CharacterVirtual *inCharacter, const CharacterVirtual *inOtherCharacter, const SubShapeID &inSubShapeID2, RVec3Arg inContactPosition, Vec3Arg inContactNormal, CharacterContactSettings &ioSettings) override;
+
+	// Called whenever the character persists colliding with a virtual character.
+	virtual void			OnCharacterContactPersisted(const CharacterVirtual *inCharacter, const CharacterVirtual *inOtherCharacter, const SubShapeID &inSubShapeID2, RVec3Arg inContactPosition, Vec3Arg inContactNormal, CharacterContactSettings &ioSettings) override;
+
+	// Called whenever the character loses contact with a virtual character.
+	virtual void			OnCharacterContactRemoved(const CharacterVirtual *inCharacter, const CharacterID &inOtherCharacterID, const SubShapeID &inSubShapeID2) override;
+
+	// Called whenever the character movement is solved and a constraint is hit. Allows the listener to override the resulting character velocity (e.g. by preventing sliding along certain surfaces).
+	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;
+
+protected:
+	// Common function to be called when contacts are added/persisted
+	void					OnContactCommon(const CharacterVirtual *inCharacter, const BodyID &inBodyID2, const SubShapeID &inSubShapeID2, RVec3Arg inContactPosition, Vec3Arg inContactNormal, CharacterContactSettings &ioSettings);
+	void					OnCharacterContactCommon(const CharacterVirtual *inCharacter, const CharacterVirtual *inOtherCharacter, const SubShapeID &inSubShapeID2, RVec3Arg inContactPosition, Vec3Arg inContactNormal, CharacterContactSettings &ioSettings);
+
+	// Get position of the character
+	virtual RVec3			GetCharacterPosition() const override				{ return mCharacter->GetPosition(); }
+
+	// Handle user input to the character
+	virtual void			HandleInput(Vec3Arg inMovementDirection, bool inJump, bool inSwitchStance, float inDeltaTime) override;
+
+	// Add character movement settings
+	virtual void			AddCharacterMovementSettings(DebugUI* inUI, UIElement* inSubMenu) override;
+
+	// Add test configuration settings
+	virtual void			AddConfigurationSettings(DebugUI *inUI, UIElement *inSubMenu) override;
+
+private:
+	// Character movement settings
+	static inline bool		sEnableCharacterInertia = true;
+
+	// Test configuration settings
+	static inline EBackFaceMode sBackFaceMode = EBackFaceMode::CollideWithBackFaces;
+	static inline float		sUpRotationX = 0;
+	static inline float		sUpRotationZ = 0;
+	static inline float		sMaxSlopeAngle = DegreesToRadians(45.0f);
+	static inline float		sMaxStrength = 100.0f;
+	static inline float		sCharacterPadding = 0.02f;
+	static inline float		sPenetrationRecoverySpeed = 1.0f;
+	static inline float		sPredictiveContactDistance = 0.1f;
+	static inline bool		sEnableWalkStairs = true;
+	static inline bool		sEnableStickToFloor = true;
+	static inline bool		sEnhancedInternalEdgeRemoval = false;
+	static inline bool		sCreateInnerBody = false;
+	static inline bool		sPlayerCanPushOtherCharacters = true;
+	static inline bool		sOtherCharactersCanPushPlayer = true;
+
+	// The 'player' character
+	Ref<CharacterVirtual>	mCharacter;
+
+	// Smoothed value of the player input
+	Vec3					mDesiredVelocity = Vec3::sZero();
+
+	// True when the player is pressing movement controls
+	bool					mAllowSliding = false;
+
+	// Track active contacts for debugging purposes
+	using ContactSet = Array<CharacterVirtual::ContactKey>;
+	ContactSet				mActiveContacts;
+};

lib/All/JoltPhysics/Samples/Tests/Constraints/ConeConstraintTest.cpp

@@ -0,0 +1,63 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/Constraints/ConeConstraintTest.h>
+#include <Jolt/Physics/Collision/Shape/CapsuleShape.h>
+#include <Jolt/Physics/Collision/GroupFilterTable.h>
+#include <Jolt/Physics/Constraints/ConeConstraint.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Layers.h>
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(ConeConstraintTest)
+{
+	JPH_ADD_BASE_CLASS(ConeConstraintTest, Test)
+}
+
+void ConeConstraintTest::Initialize()
+{
+	// Floor
+	CreateFloor();
+
+	float half_cylinder_height = 2.5f;
+
+	const int cChainLength = 5;
+
+	// Build a collision group filter that disables collision between adjacent bodies
+	Ref<GroupFilterTable> group_filter = new GroupFilterTable(cChainLength);
+	for (CollisionGroup::SubGroupID i = 0; i < cChainLength - 1; ++i)
+		group_filter->DisableCollision(i, i + 1);
+
+	// Bodies attached through cone constraints
+	for (int j = 0; j < 2; ++j)
+	{
+		Body *prev = nullptr;
+		Quat rotation = Quat::sRotation(Vec3::sAxisZ(), 0.5f * JPH_PI);
+		RVec3 position(0, 20.0f, 10.0f * j);
+		for (int i = 0; i < cChainLength; ++i)
+		{
+			position += Vec3(2.0f * half_cylinder_height, 0, 0);
+
+			Body &segment = *mBodyInterface->CreateBody(BodyCreationSettings(new CapsuleShape(half_cylinder_height, 1), position, Quat::sRotation(Vec3::sAxisX(), 0.25f * JPH_PI * i) * rotation, i == 0? EMotionType::Static : EMotionType::Dynamic, i == 0? Layers::NON_MOVING : Layers::MOVING));
+			segment.SetCollisionGroup(CollisionGroup(group_filter, CollisionGroup::GroupID(j), CollisionGroup::SubGroupID(i)));
+			mBodyInterface->AddBody(segment.GetID(), EActivation::Activate);
+
+			if (prev != nullptr)
+			{
+				ConeConstraintSettings settings;
+				settings.mPoint1 = settings.mPoint2 = position + Vec3(-half_cylinder_height, 0, 0);
+				settings.mTwistAxis1 = settings.mTwistAxis2 = Vec3(1, 0, 0);
+				if (j == 0)
+					settings.mHalfConeAngle = 0.0f;
+				else
+					settings.mHalfConeAngle = DegreesToRadians(20);
+
+				mPhysicsSystem->AddConstraint(settings.Create(*prev, segment));
+			}
+
+			prev = &segment;
+		}
+	}
+}

lib/All/JoltPhysics/Samples/Tests/Constraints/ConeConstraintTest.h

@@ -0,0 +1,16 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/Test.h>
+
+class ConeConstraintTest : public Test
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, ConeConstraintTest)
+
+	// See: Test
+	virtual void		Initialize() override;
+};

lib/All/JoltPhysics/Samples/Tests/Constraints/ConstraintPriorityTest.cpp

@@ -0,0 +1,56 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/Constraints/ConstraintPriorityTest.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Layers.h>
+#include <Renderer/DebugRendererImp.h>
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(ConstraintPriorityTest)
+{
+	JPH_ADD_BASE_CLASS(ConstraintPriorityTest, Test)
+}
+
+void ConstraintPriorityTest::Initialize()
+{
+	float box_size = 1.0f;
+	RefConst<Shape> box = new BoxShape(Vec3(0.5f * box_size, 0.2f, 0.2f));
+
+	const int num_bodies = 20;
+
+	// Bodies attached through fixed constraints
+	for (int priority = 0; priority < 2; ++priority)
+	{
+		RVec3 position(0, 10.0f, 0.2f * priority);
+		Body &top = *mBodyInterface->CreateBody(BodyCreationSettings(box, position, Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING));
+		mBodyInterface->AddBody(top.GetID(), EActivation::DontActivate);
+
+		Body *prev = &top;
+		for (int i = 1; i < num_bodies; ++i)
+		{
+			position += Vec3(box_size, 0, 0);
+
+			Body &segment = *mBodyInterface->CreateBody(BodyCreationSettings(box, position, Quat::sIdentity(), EMotionType::Dynamic, Layers::NON_MOVING)); // Putting all bodies in the NON_MOVING layer so they won't collide
+			mBodyInterface->AddBody(segment.GetID(), EActivation::Activate);
+
+			FixedConstraintSettings settings;
+			settings.mAutoDetectPoint = true;
+			settings.mConstraintPriority = priority == 0? i : num_bodies - i; // Priority is reversed for one chain compared to the other
+			Ref<Constraint> c = settings.Create(*prev, segment);
+			mPhysicsSystem->AddConstraint(c);
+			mConstraints.push_back(StaticCast<FixedConstraint>(c));
+
+			prev = &segment;
+		}
+	}
+}
+
+void ConstraintPriorityTest::PostPhysicsUpdate(float inDeltaTime)
+{
+	for (FixedConstraint *c : mConstraints)
+		mDebugRenderer->DrawText3D(0.5f * (c->GetBody1()->GetCenterOfMassPosition() + c->GetBody2()->GetCenterOfMassPosition()), StringFormat("Priority: %d", c->GetConstraintPriority()), Color::sWhite, 0.2f);
+}

lib/All/JoltPhysics/Samples/Tests/Constraints/ConstraintPriorityTest.h

@@ -0,0 +1,28 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/Test.h>
+#include <Jolt/Physics/Constraints/FixedConstraint.h>
+
+class ConstraintPriorityTest : public Test
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, ConstraintPriorityTest)
+
+	// Description of the test
+	virtual const char *		GetDescription() const override
+	{
+		return	"Tests constraint priority system to demonstrate that the order of solving can have an effect on the simulation.\n"
+				"Solving the root first will make the system stiffer.";
+	}
+
+	// See: Test
+	virtual void				Initialize() override;
+	virtual void				PostPhysicsUpdate(float inDeltaTime) override;
+
+private:
+	Array<Ref<FixedConstraint>>	mConstraints;
+};

lib/All/JoltPhysics/Samples/Tests/Constraints/ConstraintSingularityTest.cpp

@@ -0,0 +1,104 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/Constraints/ConstraintSingularityTest.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Collision/GroupFilterTable.h>
+#include <Jolt/Physics/Constraints/HingeConstraint.h>
+#include <Jolt/Physics/Constraints/FixedConstraint.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Layers.h>
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(ConstraintSingularityTest)
+{
+	JPH_ADD_BASE_CLASS(ConstraintSingularityTest, Test)
+}
+
+void ConstraintSingularityTest::Initialize()
+{
+	// Floor
+	CreateFloor();
+
+	float box_size = 4.0f;
+	RefConst<Shape> box = new BoxShape(Vec3::sReplicate(0.5f * box_size));
+
+	const int num_constraint_types = 2;
+	const int num_configurations = 4;
+
+	// Create group filter
+	Ref<GroupFilterTable> group_filter = new GroupFilterTable;
+	CollisionGroup::GroupID group_id = 0;
+
+	for (int constraint_type = 0; constraint_type < num_constraint_types; ++constraint_type)
+		for (int configuration = 0; configuration < num_configurations; ++configuration)
+		{
+			RVec3 test_position(10.0f * constraint_type, 10.0f + 10.0f * configuration, 0);
+
+			Body &body1 = *mBodyInterface->CreateBody(BodyCreationSettings(box, test_position, Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING));
+			body1.SetCollisionGroup(CollisionGroup(group_filter, group_id, 0));
+			mBodyInterface->AddBody(body1.GetID(), EActivation::DontActivate);
+
+			Body &body2 = *mBodyInterface->CreateBody(BodyCreationSettings(box, test_position + Vec3(box_size, 0, 0), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING));
+			body2.SetCollisionGroup(CollisionGroup(group_filter, group_id, 0));
+			mBodyInterface->AddBody(body2.GetID(), EActivation::Activate);
+
+			Constraint *constraint;
+			switch (constraint_type)
+			{
+			case 0:
+				{
+					HingeConstraintSettings settings;
+					settings.mPoint1 = settings.mPoint2 = test_position + Vec3(0.5f * box_size, 0, 0.5f * box_size);
+					settings.mHingeAxis1 = settings.mHingeAxis2 = Vec3::sAxisY();
+					settings.mNormalAxis1 = settings.mNormalAxis2 = Vec3::sAxisX();
+					settings.mLimitsMin = -0.01f;
+					settings.mLimitsMax = 0.01f;
+					constraint = settings.Create(body1, body2);
+					break;
+				}
+
+			default:
+				{
+					FixedConstraintSettings settings;
+					settings.mAutoDetectPoint = true;
+					constraint = settings.Create(body1, body2);
+					break;
+				}
+			}
+
+			mPhysicsSystem->AddConstraint(constraint);
+
+			RVec3 position;
+			Quat orientation;
+			switch (configuration)
+			{
+			case 0:
+				position = test_position + Vec3(0, 0, box_size);
+				orientation = Quat::sRotation(Vec3::sAxisY(), DegreesToRadians(180.0f));
+				break;
+
+			case 1:
+				position = test_position + Vec3(0, 0, box_size);
+				orientation = Quat::sRotation(Vec3::sAxisY(), DegreesToRadians(-90.0f)) * Quat::sRotation(Vec3::sAxisX(), DegreesToRadians(180.0f));
+				break;
+
+			case 2:
+				position = test_position + Vec3(box_size, 0, 0);
+				orientation = Quat::sRotation(Vec3::sAxisY(), DegreesToRadians(90.0f)) * Quat::sRotation(Vec3::sAxisZ(), DegreesToRadians(90.0f));
+				break;
+
+			default:
+				JPH_ASSERT(configuration == 3);
+				position = test_position + Vec3(-box_size, 0, 0);
+				orientation = Quat::sRotation(Vec3::sAxisY(), DegreesToRadians(90.0f)) * Quat::sRotation(Vec3::sAxisZ(), DegreesToRadians(90.0f));
+				break;
+			}
+
+			mBodyInterface->SetPositionAndRotation(body2.GetID(), position, orientation, EActivation::DontActivate);
+
+			++group_id;
+		}
+}

lib/All/JoltPhysics/Samples/Tests/Constraints/ConstraintSingularityTest.h

@@ -0,0 +1,22 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/Test.h>
+
+class ConstraintSingularityTest : public Test
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, ConstraintSingularityTest)
+
+	// Description of the test
+	virtual const char *GetDescription() const override
+	{
+		return "Starts constraints in a configuration where there are multiple directions to move in to satisfy the constraint.";
+	}
+
+	// See: Test
+	virtual void		Initialize() override;
+};

lib/All/JoltPhysics/Samples/Tests/Constraints/ConstraintVsCOMChangeTest.cpp

@@ -0,0 +1,130 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/Constraints/ConstraintVsCOMChangeTest.h>
+#include <Jolt/Physics/Collision/Shape/MutableCompoundShape.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Collision/GroupFilterTable.h>
+#include <Jolt/Physics/Constraints/HingeConstraint.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Layers.h>
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(ConstraintVsCOMChangeTest)
+{
+	JPH_ADD_BASE_CLASS(ConstraintVsCOMChangeTest, Test)
+}
+
+void ConstraintVsCOMChangeTest::Initialize()
+{
+	constexpr int cChainLength = 15;
+	constexpr float cMinAngle = DegreesToRadians(-10.0f);
+	constexpr float cMaxAngle = DegreesToRadians(20.0f);
+
+	// Floor
+	CreateFloor();
+
+	// Create box shape
+	mBox = new BoxShape(Vec3::sReplicate(0.5f * cBoxSize));
+
+	// Build a collision group filter that disables collision between adjacent bodies
+	Ref<GroupFilterTable> group_filter = new GroupFilterTable(cChainLength);
+	for (CollisionGroup::SubGroupID i = 0; i < cChainLength - 1; ++i)
+		group_filter->DisableCollision(i, i + 1);
+
+	// Create chain of bodies
+	RVec3 position(0, 25, 0);
+	for (int i = 0; i < cChainLength; ++i)
+	{
+		position += Vec3(cBoxSize, 0, 0);
+		Quat rotation = Quat::sIdentity();
+
+		// Create compound shape specific for this body
+		MutableCompoundShapeSettings compound_shape;
+		compound_shape.SetEmbedded();
+		compound_shape.AddShape(Vec3::sZero(), Quat::sIdentity(), mBox);
+
+		// Create body
+		Body& segment = *mBodyInterface->CreateBody(BodyCreationSettings(&compound_shape, position, rotation, i == 0 ? EMotionType::Static : EMotionType::Dynamic, i == 0 ? Layers::NON_MOVING : Layers::MOVING));
+		segment.SetCollisionGroup(CollisionGroup(group_filter, 0, CollisionGroup::SubGroupID(i)));
+		mBodyInterface->AddBody(segment.GetID(), EActivation::Activate);
+
+		if (i > 0)
+		{
+			// Create hinge
+			HingeConstraintSettings settings;
+			settings.mPoint1 = settings.mPoint2 = position + Vec3(-0.5f * cBoxSize, -0.5f * cBoxSize, 0);
+			settings.mHingeAxis1 = settings.mHingeAxis2 = Vec3::sAxisZ();
+			settings.mNormalAxis1 = settings.mNormalAxis2 = Vec3::sAxisX();
+			settings.mLimitsMin = cMinAngle;
+			settings.mLimitsMax = cMaxAngle;
+			Constraint* constraint = settings.Create(*mBodies.back(), segment);
+			mPhysicsSystem->AddConstraint(constraint);
+
+			mConstraints.push_back(constraint);
+		}
+
+		mBodies.push_back(&segment);
+	}
+}
+
+void ConstraintVsCOMChangeTest::PrePhysicsUpdate(const PreUpdateParams& inParams)
+{
+	// Increment time
+	mTime += inParams.mDeltaTime;
+
+	UpdateShapes();
+}
+
+void ConstraintVsCOMChangeTest::SaveState(StateRecorder &inStream) const
+{
+	inStream.Write(mTime);
+}
+
+void ConstraintVsCOMChangeTest::RestoreState(StateRecorder &inStream)
+{
+	inStream.Read(mTime);
+
+	UpdateShapes();
+}
+
+void ConstraintVsCOMChangeTest::UpdateShapes()
+{
+	// Check if we need to change the configuration
+	int num_shapes = int(mTime) & 1? 2 : 1;
+	if (mNumShapes != num_shapes)
+	{
+		mNumShapes = num_shapes;
+
+		// Change the COM of the bodies
+		for (int i = 1; i < (int)mBodies.size(); i += 2)
+		{
+			Body *b = mBodies[i];
+			MutableCompoundShape *s = static_cast<MutableCompoundShape *>(const_cast<Shape *>(b->GetShape()));
+
+			// Remember the center of mass before the change
+			Vec3 prev_com = s->GetCenterOfMass();
+
+			// First remove all existing shapes
+			for (int j = s->GetNumSubShapes() - 1; j >= 0; --j)
+				s->RemoveShape(j);
+
+			// Then create the desired number of shapes
+			for (int j = 0; j < num_shapes; ++j)
+				s->AddShape(Vec3(0, 0, (1.0f + cBoxSize) * j), Quat::sIdentity(), mBox);
+
+			// Update the center of mass to account for the new box configuration
+			s->AdjustCenterOfMass();
+
+			// Notify the physics system that the shape has changed
+			mBodyInterface->NotifyShapeChanged(b->GetID(), prev_com, true, EActivation::Activate);
+
+			// Notify the constraints that the shape has changed (this could be done more efficient as we know which constraints are affected)
+			Vec3 delta_com = s->GetCenterOfMass() - prev_com;
+			for (Constraint *c : mConstraints)
+				c->NotifyShapeChanged(b->GetID(), delta_com);
+		}
+	}
+}

lib/All/JoltPhysics/Samples/Tests/Constraints/ConstraintVsCOMChangeTest.h

@@ -0,0 +1,38 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/Test.h>
+
+class ConstraintVsCOMChangeTest : public Test
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, ConstraintVsCOMChangeTest)
+
+	// Description of the test
+	virtual const char *		GetDescription() const override
+	{
+		return	"This test demonstrates how to notify a constraint that the center of mass of a body changed.\n"
+				"Constraints store their attachment points in center of mass space.";
+	}
+
+	// See: Test
+	virtual void				Initialize() override;
+	virtual void				PrePhysicsUpdate(const PreUpdateParams& inParams) override;
+	virtual void				SaveState(StateRecorder &inStream) const override;
+	virtual void				RestoreState(StateRecorder &inStream) override;
+
+private:
+	void						UpdateShapes();
+
+	RefConst<Shape>				mBox;
+	Array<Body *>				mBodies;
+	Array<Ref<Constraint>>		mConstraints;
+
+	static constexpr float		cBoxSize = 2.0f;
+
+	float						mTime = 0.0f;
+	int							mNumShapes = -1;
+};

lib/All/JoltPhysics/Samples/Tests/Constraints/DistanceConstraintTest.cpp

@@ -0,0 +1,59 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/Constraints/DistanceConstraintTest.h>
+#include <Jolt/Physics/Collision/Shape/CapsuleShape.h>
+#include <Jolt/Physics/Constraints/DistanceConstraint.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Layers.h>
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(DistanceConstraintTest)
+{
+	JPH_ADD_BASE_CLASS(DistanceConstraintTest, Test)
+}
+
+void DistanceConstraintTest::Initialize()
+{
+	// Floor
+	CreateFloor();
+
+	float half_cylinder_height = 2.5f;
+
+	// Variation 0: Fixed distance
+	// Variation 1: Min/max distance
+	for (int variation = 0; variation < 2; ++variation)
+	{
+		// Bodies attached through distance constraints
+		Quat rotation = Quat::sRotation(Vec3::sAxisZ(), 0.5f * JPH_PI);
+		RVec3 position(0, 75, 10.0f * variation);
+		Body &top = *mBodyInterface->CreateBody(BodyCreationSettings(new CapsuleShape(half_cylinder_height, 1), position, rotation, EMotionType::Static, Layers::NON_MOVING));
+		mBodyInterface->AddBody(top.GetID(), EActivation::DontActivate);
+
+		Body *prev = &top;
+		for (int i = 1; i < 15; ++i)
+		{
+			position += Vec3(5.0f + 2.0f * half_cylinder_height, 0, 0);
+
+			Body &segment = *mBodyInterface->CreateBody(BodyCreationSettings(new CapsuleShape(half_cylinder_height, 1), position, rotation, EMotionType::Dynamic, Layers::MOVING));
+			mBodyInterface->AddBody(segment.GetID(), EActivation::Activate);
+
+			DistanceConstraintSettings settings;
+			settings.mPoint1 = position - Vec3(5.0f + half_cylinder_height, 0, 0);
+			settings.mPoint2 = position - Vec3(half_cylinder_height, 0, 0);
+
+			if (variation == 1)
+			{
+				// Default distance is 5, override min/max range
+				settings.mMinDistance = 4.0f;
+				settings.mMaxDistance = 8.0f;
+			}
+
+			mPhysicsSystem->AddConstraint(settings.Create(*prev, segment));
+
+			prev = &segment;
+		}
+	}
+}

lib/All/JoltPhysics/Samples/Tests/Constraints/DistanceConstraintTest.h

@@ -0,0 +1,16 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/Test.h>
+
+class DistanceConstraintTest : public Test
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, DistanceConstraintTest)
+
+	// See: Test
+	virtual void		Initialize() override;
+};

lib/All/JoltPhysics/Samples/Tests/Constraints/FixedConstraintTest.cpp

@@ -0,0 +1,165 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/Constraints/FixedConstraintTest.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Collision/GroupFilterTable.h>
+#include <Jolt/Physics/Constraints/FixedConstraint.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Layers.h>
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(FixedConstraintTest)
+{
+	JPH_ADD_BASE_CLASS(FixedConstraintTest, Test)
+}
+
+void FixedConstraintTest::Initialize()
+{
+	// Floor
+	CreateFloor();
+
+	float box_size = 4.0f;
+	RefConst<Shape> box = new BoxShape(Vec3::sReplicate(0.5f * box_size));
+
+	const int num_bodies = 10;
+
+	// Build a collision group filter that disables collision between adjacent bodies
+	Ref<GroupFilterTable> group_filter = new GroupFilterTable(num_bodies);
+	for (CollisionGroup::SubGroupID i = 0; i < num_bodies - 1; ++i)
+		group_filter->DisableCollision(i, i + 1);
+
+	// Bodies attached through fixed constraints
+	for (int randomness = 0; randomness < 2; ++randomness)
+	{
+		CollisionGroup::GroupID group_id = CollisionGroup::GroupID(randomness);
+
+		RVec3 position(0, 25.0f, -randomness * 20.0f);
+		Body &top = *mBodyInterface->CreateBody(BodyCreationSettings(box, position, Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING));
+		top.SetCollisionGroup(CollisionGroup(group_filter, group_id, 0));
+		mBodyInterface->AddBody(top.GetID(), EActivation::DontActivate);
+
+		default_random_engine random;
+		uniform_real_distribution<float> displacement(-1.0f, 1.0f);
+
+		Body *prev = &top;
+		for (int i = 1; i < num_bodies; ++i)
+		{
+			Quat rotation;
+			if (randomness == 0)
+			{
+				position += Vec3(box_size, 0, 0);
+				rotation = Quat::sIdentity();
+			}
+			else
+			{
+				position += Vec3(box_size + abs(displacement(random)), displacement(random), displacement(random));
+				rotation = Quat::sRandom(random);
+			}
+
+			Body &segment = *mBodyInterface->CreateBody(BodyCreationSettings(box, position, rotation, EMotionType::Dynamic, Layers::MOVING));
+			segment.SetCollisionGroup(CollisionGroup(group_filter, group_id, CollisionGroup::SubGroupID(i)));
+			mBodyInterface->AddBody(segment.GetID(), EActivation::Activate);
+
+			FixedConstraintSettings settings;
+			settings.mAutoDetectPoint = true;
+			Ref<Constraint> c = settings.Create(*prev, segment);
+			mPhysicsSystem->AddConstraint(c);
+
+			prev = &segment;
+		}
+	}
+
+	{
+		// Two light bodies attached to a heavy body (gives issues if the wrong anchor point is chosen)
+		Body *light1 = mBodyInterface->CreateBody(BodyCreationSettings(new BoxShape(Vec3::sReplicate(0.1f)), RVec3(-5.0f, 7.0f, -5.2f), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING));
+		mBodyInterface->AddBody(light1->GetID(), EActivation::Activate);
+		Body *heavy = mBodyInterface->CreateBody(BodyCreationSettings(new BoxShape(Vec3::sReplicate(5.0f)), RVec3(-5.0f, 7.0f, 0.0f), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING));
+		mBodyInterface->AddBody(heavy->GetID(), EActivation::Activate);
+		Body *light2 = mBodyInterface->CreateBody(BodyCreationSettings(new BoxShape(Vec3::sReplicate(0.1f)), RVec3(-5.0f, 7.0f, 5.2f), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING));
+		mBodyInterface->AddBody(light2->GetID(), EActivation::Activate);
+
+		FixedConstraintSettings light1_heavy;
+		light1_heavy.mAutoDetectPoint = true;
+		mPhysicsSystem->AddConstraint(light1_heavy.Create(*light1, *heavy));
+
+		FixedConstraintSettings heavy_light2;
+		heavy_light2.mAutoDetectPoint = true;
+		mPhysicsSystem->AddConstraint(heavy_light2.Create(*heavy, *light2));
+	}
+
+	{
+		// A tower of beams and crossbeams (note that it is not recommended to make constructs with this many fixed constraints, this is not always stable)
+		RVec3 base_position(0, 25, -40.0f);
+		Quat base_rotation = Quat::sRotation(Vec3::sAxisZ(), -0.5f * JPH_PI);
+
+		Ref<BoxShape> pillar = new BoxShape(Vec3(0.1f, 1.0f, 0.1f), 0.0f);
+		Ref<BoxShape> beam = new BoxShape(Vec3(0.01f, 1.5f, 0.1f), 0.0f);
+
+		Body *prev_pillars[4] = { &Body::sFixedToWorld, &Body::sFixedToWorld, &Body::sFixedToWorld, &Body::sFixedToWorld };
+
+		Vec3 center = Vec3::sZero();
+		for (int y = 0; y < 10; ++y)
+		{
+			// Create pillars
+			Body *pillars[4];
+			for (int i = 0; i < 4; ++i)
+			{
+				Quat rotation = Quat::sRotation(Vec3::sAxisY(), i * 0.5f * JPH_PI);
+
+				pillars[i] = mBodyInterface->CreateBody(BodyCreationSettings(pillar, base_position + base_rotation * (center + rotation * Vec3(1.0f, 1.0f, 1.0f)), base_rotation, EMotionType::Dynamic, Layers::MOVING));
+				pillars[i]->SetCollisionGroup(CollisionGroup(group_filter, 0, 0)); // For convenience, we disable collisions between all objects in the tower
+				mBodyInterface->AddBody(pillars[i]->GetID(), EActivation::Activate);
+			}
+
+			for (int i = 0; i < 4; ++i)
+			{
+				Quat rotation = Quat::sRotation(Vec3::sAxisY(), i * 0.5f * JPH_PI);
+
+				// Create cross beam
+				Body *cross = mBodyInterface->CreateBody(BodyCreationSettings(beam, base_position + base_rotation * (center + rotation * Vec3(1.105f, 1.0f, 0.0f)), base_rotation * rotation * Quat::sRotation(Vec3::sAxisX(), 0.3f * JPH_PI), EMotionType::Dynamic, Layers::MOVING));
+				cross->SetCollisionGroup(CollisionGroup(group_filter, 0, 0));
+				mBodyInterface->AddBody(cross->GetID(), EActivation::Activate);
+
+				// Attach cross beam to pillars
+				for (int j = 0; j < 2; ++j)
+				{
+					FixedConstraintSettings constraint;
+					constraint.mAutoDetectPoint = true;
+					constraint.mNumVelocityStepsOverride = 64; // This structure needs more solver steps to be stable
+					constraint.mNumPositionStepsOverride = JPH_IF_NOT_DEBUG(64) JPH_IF_DEBUG(8); // In debug mode use less steps to preserve framerate (at the cost of stability)
+					mPhysicsSystem->AddConstraint(constraint.Create(*pillars[(i + j) % 4], *cross));
+				}
+
+				// Attach to previous pillar
+				if (prev_pillars[i] != nullptr)
+				{
+					FixedConstraintSettings constraint;
+					constraint.mAutoDetectPoint = true;
+					constraint.mNumVelocityStepsOverride = 64;
+					constraint.mNumPositionStepsOverride = JPH_IF_NOT_DEBUG(64) JPH_IF_DEBUG(8);
+					mPhysicsSystem->AddConstraint(constraint.Create(*prev_pillars[i], *pillars[i]));
+				}
+
+				prev_pillars[i] = pillars[i];
+			}
+
+			center += Vec3(0.0f, 2.0f, 0.0f);
+		}
+
+		// Create top
+		Body *top = mBodyInterface->CreateBody(BodyCreationSettings(new BoxShape(Vec3(1.2f, 0.1f, 1.2f)), base_position + base_rotation * (center + Vec3(0.0f, 0.1f, 0.0f)), base_rotation, EMotionType::Dynamic, Layers::MOVING));
+		top->SetCollisionGroup(CollisionGroup(group_filter, 0, 0));
+		mBodyInterface->AddBody(top->GetID(), EActivation::Activate);
+
+		// Attach top to pillars
+		for (int i = 0; i < 4; ++i)
+		{
+			FixedConstraintSettings constraint;
+			constraint.mAutoDetectPoint = true;
+			mPhysicsSystem->AddConstraint(constraint.Create(*prev_pillars[i], *top));
+		}
+	}
+}

lib/All/JoltPhysics/Samples/Tests/Constraints/FixedConstraintTest.h

@@ -0,0 +1,16 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/Test.h>
+
+class FixedConstraintTest : public Test
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, FixedConstraintTest)
+
+	// See: Test
+	virtual void		Initialize() override;
+};

lib/All/JoltPhysics/Samples/Tests/Constraints/GearConstraintTest.cpp

@@ -0,0 +1,124 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/Constraints/GearConstraintTest.h>
+#include <Jolt/Physics/Collision/Shape/CylinderShape.h>
+#include <Jolt/Physics/Collision/Shape/ConvexHullShape.h>
+#include <Jolt/Physics/Collision/Shape/StaticCompoundShape.h>
+#include <Jolt/Physics/Collision/GroupFilterTable.h>
+#include <Jolt/Physics/Constraints/HingeConstraint.h>
+#include <Jolt/Physics/Constraints/GearConstraint.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Layers.h>
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(GearConstraintTest)
+{
+	JPH_ADD_BASE_CLASS(GearConstraintTest, Test)
+}
+
+void GearConstraintTest::Initialize()
+{
+	// Floor
+	CreateFloor();
+
+	constexpr float cGearHalfWidth = 0.05f;
+
+	constexpr float cGear1Radius = 0.5f;
+	constexpr int cGear1NumTeeth = 100;
+
+	constexpr float cGear2Radius = 2.0f;
+	constexpr int cGear2NumTeeth = int(cGear1NumTeeth * cGear2Radius / cGear1Radius);
+
+	constexpr float cToothThicknessBottom = 0.01f;
+	constexpr float cToothThicknessTop = 0.005f;
+	constexpr float cToothHeight = 0.02f;
+
+	// Create a tooth
+	Array<Vec3> tooth_points = {
+		Vec3(0, cGearHalfWidth, cToothThicknessBottom),
+		Vec3(0, -cGearHalfWidth, cToothThicknessBottom),
+		Vec3(0, cGearHalfWidth, -cToothThicknessBottom),
+		Vec3(0, -cGearHalfWidth, -cToothThicknessBottom),
+		Vec3(cToothHeight, -cGearHalfWidth, cToothThicknessTop),
+		Vec3(cToothHeight, cGearHalfWidth, cToothThicknessTop),
+		Vec3(cToothHeight, -cGearHalfWidth, -cToothThicknessTop),
+		Vec3(cToothHeight, cGearHalfWidth, -cToothThicknessTop),
+	};
+	ConvexHullShapeSettings tooth_settings(tooth_points);
+	tooth_settings.SetEmbedded();
+
+	// Create gear 1
+	CylinderShapeSettings gear1_cylinder(cGearHalfWidth, cGear1Radius);
+	gear1_cylinder.SetEmbedded();
+
+	StaticCompoundShapeSettings gear1_settings;
+	gear1_settings.SetEmbedded();
+
+	gear1_settings.AddShape(Vec3::sZero(), Quat::sIdentity(), &gear1_cylinder);
+	for (int i = 0; i < cGear1NumTeeth; ++i)
+	{
+		Quat rotation = Quat::sRotation(Vec3::sAxisY(), 2.0f * JPH_PI * i / cGear1NumTeeth);
+		gear1_settings.AddShape(rotation * Vec3(cGear1Radius, 0, 0), rotation, &tooth_settings);
+	}
+
+	RVec3 gear1_initial_p(0, 3.0f, 0);
+	Quat gear1_initial_r = Quat::sRotation(Vec3::sAxisX(), 0.5f * JPH_PI);
+	Body *gear1 = mBodyInterface->CreateBody(BodyCreationSettings(&gear1_settings, gear1_initial_p, gear1_initial_r, EMotionType::Dynamic, Layers::MOVING));
+	mBodyInterface->AddBody(gear1->GetID(), EActivation::Activate);
+
+	// Create gear 2
+	CylinderShapeSettings gear2_cylinder(cGearHalfWidth, cGear2Radius);
+	gear2_cylinder.SetEmbedded();
+
+	StaticCompoundShapeSettings gear2_settings;
+	gear2_settings.SetEmbedded();
+
+	gear2_settings.AddShape(Vec3::sZero(), Quat::sIdentity(), &gear2_cylinder);
+	for (int i = 0; i < cGear2NumTeeth; ++i)
+	{
+		Quat rotation = Quat::sRotation(Vec3::sAxisY(), 2.0f * JPH_PI * (i + 0.5f) / cGear2NumTeeth);
+		gear2_settings.AddShape(rotation * Vec3(cGear2Radius, 0, 0), rotation, &tooth_settings);
+	}
+
+	RVec3 gear2_initial_p = gear1_initial_p + Vec3(cGear1Radius + cGear2Radius + cToothHeight, 0, 0);
+	Quat gear2_initial_r = gear1_initial_r;
+	Body *gear2 = mBodyInterface->CreateBody(BodyCreationSettings(&gear2_settings, gear2_initial_p, gear2_initial_r, EMotionType::Dynamic, Layers::MOVING));
+	mBodyInterface->AddBody(gear2->GetID(), EActivation::Activate);
+
+	// Create a hinge for gear 1
+	HingeConstraintSettings hinge1;
+	hinge1.mPoint1 = hinge1.mPoint2 = gear1_initial_p;
+	hinge1.mHingeAxis1 = hinge1.mHingeAxis2 = Vec3::sAxisZ();
+	hinge1.mNormalAxis1 = hinge1.mNormalAxis2 = Vec3::sAxisX();
+	Constraint *hinge1_constraint = hinge1.Create(Body::sFixedToWorld, *gear1);
+	mPhysicsSystem->AddConstraint(hinge1_constraint);
+
+	// Create a hinge for gear 1
+	HingeConstraintSettings hinge2;
+	hinge2.mPoint1 = hinge2.mPoint2 = gear2_initial_p;
+	hinge2.mHingeAxis1 = hinge2.mHingeAxis2 = Vec3::sAxisZ();
+	hinge2.mNormalAxis1 = hinge2.mNormalAxis2 = Vec3::sAxisX();
+	Constraint *hinge2_constraint = hinge2.Create(Body::sFixedToWorld, *gear2);
+	mPhysicsSystem->AddConstraint(hinge2_constraint);
+
+	// Disable collision between gears
+	Ref<GroupFilterTable> group_filter = new GroupFilterTable(2);
+	group_filter->DisableCollision(0, 1);
+	gear1->SetCollisionGroup(CollisionGroup(group_filter, 0, 0));
+	gear2->SetCollisionGroup(CollisionGroup(group_filter, 0, 1));
+
+	// Create gear constraint to constrain the two bodies
+	GearConstraintSettings gear;
+	gear.mHingeAxis1 = hinge1.mHingeAxis1;
+	gear.mHingeAxis2 = hinge2.mHingeAxis1;
+	gear.SetRatio(cGear1NumTeeth, cGear2NumTeeth);
+	GearConstraint *gear_constraint = static_cast<GearConstraint *>(gear.Create(*gear1, *gear2));
+	gear_constraint->SetConstraints(hinge1_constraint, hinge2_constraint);
+	mPhysicsSystem->AddConstraint(gear_constraint);
+
+	// Give the gear a spin
+	gear2->SetAngularVelocity(Vec3(0, 0, 3.0f));
+}

lib/All/JoltPhysics/Samples/Tests/Constraints/GearConstraintTest.h

@@ -0,0 +1,17 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/Test.h>
+
+// This test demonstrates the use of a gear constraint
+class GearConstraintTest : public Test
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, GearConstraintTest)
+
+	// See: Test
+	virtual void		Initialize() override;
+};

lib/All/JoltPhysics/Samples/Tests/Constraints/HingeConstraintTest.cpp

@@ -0,0 +1,126 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/Constraints/HingeConstraintTest.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Collision/GroupFilterTable.h>
+#include <Jolt/Physics/Constraints/HingeConstraint.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Layers.h>
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(HingeConstraintTest)
+{
+	JPH_ADD_BASE_CLASS(HingeConstraintTest, Test)
+}
+
+void HingeConstraintTest::Initialize()
+{
+	// Floor
+	CreateFloor();
+
+	float box_size = 4.0f;
+	RefConst<Shape> box = new BoxShape(Vec3::sReplicate(0.5f * box_size));
+
+	constexpr int cChainLength = 15;
+	constexpr float cMinAngle = DegreesToRadians(-10.0f);
+	constexpr float cMaxAngle = DegreesToRadians(20.0f);
+
+	// Build a collision group filter that disables collision between adjacent bodies
+	Ref<GroupFilterTable> group_filter = new GroupFilterTable(cChainLength);
+	for (CollisionGroup::SubGroupID i = 0; i < cChainLength - 1; ++i)
+		group_filter->DisableCollision(i, i + 1);
+
+	// Bodies attached through hinge constraints
+	for (int randomness = 0; randomness < 2; ++randomness)
+	{
+		CollisionGroup::GroupID group_id = CollisionGroup::GroupID(randomness);
+
+		RVec3 position(0, 50, -randomness * 20.0f);
+		Body &top = *mBodyInterface->CreateBody(BodyCreationSettings(box, position, Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING));
+		top.SetCollisionGroup(CollisionGroup(group_filter, group_id, 0));
+		mBodyInterface->AddBody(top.GetID(), EActivation::DontActivate);
+
+		default_random_engine random;
+		uniform_real_distribution<float> displacement(-1.0f, 1.0f);
+
+		Body *prev = &top;
+		for (int i = 1; i < cChainLength; ++i)
+		{
+			Quat rotation;
+			if (randomness == 0)
+			{
+				position += Vec3(box_size, 0, 0);
+				rotation = Quat::sIdentity();
+			}
+			else
+			{
+				position += Vec3(box_size + abs(displacement(random)), displacement(random), displacement(random));
+				rotation = Quat::sRandom(random);
+			}
+
+			Body &segment = *mBodyInterface->CreateBody(BodyCreationSettings(box, position, rotation, EMotionType::Dynamic, Layers::MOVING));
+			segment.SetCollisionGroup(CollisionGroup(group_filter, group_id, CollisionGroup::SubGroupID(i)));
+			mBodyInterface->AddBody(segment.GetID(), EActivation::Activate);
+
+			HingeConstraintSettings settings;
+			if ((i & 1) == 0)
+			{
+				settings.mPoint1 = settings.mPoint2 = position + Vec3(-0.5f * box_size, 0, 0.5f * box_size);
+				settings.mHingeAxis1 = settings.mHingeAxis2 = Vec3::sAxisY();
+				settings.mNormalAxis1 = settings.mNormalAxis2 = Vec3::sAxisX();
+			}
+			else
+			{
+				settings.mPoint1 = settings.mPoint2 = position + Vec3(-0.5f * box_size, -0.5f * box_size, 0);
+				settings.mHingeAxis1 = settings.mHingeAxis2 = Vec3::sAxisZ();
+				settings.mNormalAxis1 = settings.mNormalAxis2 = Vec3::sAxisX();
+			}
+			settings.mLimitsMin = cMinAngle;
+			settings.mLimitsMax = cMaxAngle;
+			mPhysicsSystem->AddConstraint(settings.Create(*prev, segment));
+
+			prev = &segment;
+		}
+	}
+
+	{
+		// Two bodies connected with a hard hinge
+		Body *body1 = mBodyInterface->CreateBody(BodyCreationSettings(new BoxShape(Vec3::sOne()), RVec3(4, 5, 0), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING));
+		body1->SetCollisionGroup(CollisionGroup(group_filter, 0, 0));
+		mBodyInterface->AddBody(body1->GetID(), EActivation::DontActivate);
+		Body *body2 = mBodyInterface->CreateBody(BodyCreationSettings(new BoxShape(Vec3::sOne()), RVec3(6, 5, 0), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING));
+		body2->SetCollisionGroup(CollisionGroup(group_filter, 0, 1));
+		mBodyInterface->AddBody(body2->GetID(), EActivation::Activate);
+
+		HingeConstraintSettings hinge;
+		hinge.mPoint1 = hinge.mPoint2 = RVec3(5, 4, 0);
+		hinge.mHingeAxis1 = hinge.mHingeAxis2 = Vec3::sAxisZ();
+		hinge.mNormalAxis1 = hinge.mNormalAxis2 = Vec3::sAxisY();
+		hinge.mLimitsMin = DegreesToRadians(-10.0f);
+		hinge.mLimitsMax = DegreesToRadians(110.0f);
+		mPhysicsSystem->AddConstraint(hinge.Create(*body1, *body2));
+	}
+
+	{
+		// Two bodies connected with a soft hinge
+		Body *body1 = mBodyInterface->CreateBody(BodyCreationSettings(new BoxShape(Vec3::sOne()), RVec3(10, 5, 0), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING));
+		body1->SetCollisionGroup(CollisionGroup(group_filter, 0, 0));
+		mBodyInterface->AddBody(body1->GetID(), EActivation::DontActivate);
+		Body *body2 = mBodyInterface->CreateBody(BodyCreationSettings(new BoxShape(Vec3::sOne()), RVec3(12, 5, 0), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING));
+		body2->SetCollisionGroup(CollisionGroup(group_filter, 0, 1));
+		mBodyInterface->AddBody(body2->GetID(), EActivation::Activate);
+
+		HingeConstraintSettings hinge;
+		hinge.mPoint1 = hinge.mPoint2 = RVec3(11, 4, 0);
+		hinge.mHingeAxis1 = hinge.mHingeAxis2 = Vec3::sAxisZ();
+		hinge.mNormalAxis1 = hinge.mNormalAxis2 = Vec3::sAxisY();
+		hinge.mLimitsMin = DegreesToRadians(-10.0f);
+		hinge.mLimitsMax = DegreesToRadians(110.0f);
+		hinge.mLimitsSpringSettings.mFrequency = 1.0f;
+		hinge.mLimitsSpringSettings.mDamping = 0.5f;
+		mPhysicsSystem->AddConstraint(hinge.Create(*body1, *body2));
+	}
+}

lib/All/JoltPhysics/Samples/Tests/Constraints/HingeConstraintTest.h

@@ -0,0 +1,16 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/Test.h>
+
+class HingeConstraintTest : public Test
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, HingeConstraintTest)
+
+	// See: Test
+	virtual void		Initialize() override;
+};

lib/All/JoltPhysics/Samples/Tests/Constraints/PathConstraintTest.cpp

@@ -0,0 +1,134 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/Constraints/PathConstraintTest.h>
+#include <Jolt/Physics/Collision/Shape/CapsuleShape.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Constraints/PathConstraintPathHermite.h>
+#include <Application/DebugUI.h>
+#include <Layers.h>
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(PathConstraintTest)
+{
+	JPH_ADD_BASE_CLASS(PathConstraintTest, Test)
+}
+
+EPathRotationConstraintType PathConstraintTest::sOrientationType = EPathRotationConstraintType::Free;
+
+void PathConstraintTest::Initialize()
+{
+	// Floor
+	CreateFloor();
+
+	{
+		// Create spiral path
+		Ref<PathConstraintPathHermite> path = new PathConstraintPathHermite;
+		Vec3 normal(0, 1, 0);
+		Array<Vec3> positions;
+		for (float a = -0.1f * JPH_PI; a < 4.0f * JPH_PI; a += 0.1f * JPH_PI)
+			positions.push_back(Vec3(5.0f * Cos(a), -a, 5.0f * Sin(a)));
+		for (int i = 1; i < int(positions.size() - 1); ++i)
+		{
+			Vec3 tangent = 0.5f * (positions[i + 1] - positions[i - 1]);
+			path->AddPoint(positions[i], tangent, normal);
+		}
+		mPaths[0] = path;
+
+		// Dynamic base plate to which the path attaches
+		Body &body1 = *mBodyInterface->CreateBody(BodyCreationSettings(new BoxShape(Vec3(5, 0.5f, 5)), RVec3(-10, 1, 0), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING));
+		mBodyInterface->AddBody(body1.GetID(), EActivation::Activate);
+
+		// Dynamic body attached to the path
+		Body &body2 = *mBodyInterface->CreateBody(BodyCreationSettings(new BoxShape(Vec3(0.5f, 1.0f, 2.0f)), RVec3(-5, 15, 0), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING));
+		body2.SetAllowSleeping(false);
+		mBodyInterface->AddBody(body2.GetID(), EActivation::Activate);
+
+		// Create path constraint
+		PathConstraintSettings settings;
+		settings.mPath = path;
+		settings.mPathPosition = Vec3(0, 15, 0);
+		settings.mRotationConstraintType = sOrientationType;
+		mConstraints[0] = static_cast<PathConstraint *>(settings.Create(body1, body2));
+		mPhysicsSystem->AddConstraint(mConstraints[0]);
+	}
+
+	{
+		// Create circular path
+		Ref<PathConstraintPathHermite> path = new PathConstraintPathHermite;
+		path->SetIsLooping(true);
+		Vec3 normal(0, 1, 0);
+		Array<Vec3> positions;
+		for (int i = -1; i < 11; ++i)
+		{
+			float a = 2.0f * JPH_PI * i / 10.0f;
+			positions.push_back(Vec3(5.0f * Cos(a), 0.0f, 5.0f * Sin(a)));
+		}
+		for (int i = 1; i < int(positions.size() - 1); ++i)
+		{
+			Vec3 tangent = 0.5f * (positions[i + 1] - positions[i - 1]);
+			path->AddPoint(positions[i], tangent, normal);
+		}
+		mPaths[1] = path;
+
+		// Dynamic base plate to which the path attaches
+		Body &body1 = *mBodyInterface->CreateBody(BodyCreationSettings(new BoxShape(Vec3(5, 0.5f, 5)), RVec3(10, 1, 0), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING));
+		mBodyInterface->AddBody(body1.GetID(), EActivation::Activate);
+
+		// Dynamic body attached to the path
+		Body &body2 = *mBodyInterface->CreateBody(BodyCreationSettings(new BoxShape(Vec3(0.5f, 1.0f, 2.0f)), RVec3(15, 5, 0), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING));
+		body2.SetAllowSleeping(false);
+		mBodyInterface->AddBody(body2.GetID(), EActivation::Activate);
+
+		// Create path constraint
+		PathConstraintSettings settings;
+		settings.mPath = path;
+		settings.mPathPosition = Vec3(0, 5, 0);
+		settings.mPathRotation = Quat::sRotation(Vec3::sAxisX(), -0.1f * JPH_PI);
+		settings.mRotationConstraintType = sOrientationType;
+		mConstraints[1] = static_cast<PathConstraint *>(settings.Create(body1, body2));
+		mPhysicsSystem->AddConstraint(mConstraints[1]);
+	}
+
+}
+
+void PathConstraintTest::PrePhysicsUpdate(const PreUpdateParams &inParams)
+{
+	for (PathConstraint *c : mConstraints)
+	{
+		MotorSettings &motor_settings = c->GetPositionMotorSettings();
+		motor_settings.SetForceLimit(sMaxMotorAcceleration / c->GetBody2()->GetMotionProperties()->GetInverseMass()); // F = m * a
+		motor_settings.mSpringSettings.mFrequency = sFrequency;
+		motor_settings.mSpringSettings.mDamping = sDamping;
+		c->SetMaxFrictionForce(sMaxFrictionAcceleration / c->GetBody2()->GetMotionProperties()->GetInverseMass());
+	}
+}
+
+void PathConstraintTest::CreateSettingsMenu(DebugUI *inUI, UIElement *inSubMenu)
+{
+	static Array<String> constraint_types = { "Free", "Tangent", "Normal", "Binormal", "Path", "Full" };
+
+	inUI->CreateTextButton(inSubMenu, "Configuration Settings", [this, inUI]() {
+		UIElement *configuration_settings = inUI->CreateMenu();
+		inUI->CreateComboBox(configuration_settings, "Rotation Constraint", constraint_types, (int)sOrientationType, [=](int inItem) { sOrientationType = (EPathRotationConstraintType)inItem; });
+		inUI->CreateTextButton(configuration_settings, "Accept Changes", [this]() { RestartTest(); });
+		inUI->ShowMenu(configuration_settings);
+	});
+
+	inUI->CreateTextButton(inSubMenu, "Runtime Settings", [this, inUI]() {
+		UIElement *runtime_settings = inUI->CreateMenu();
+		inUI->CreateComboBox(runtime_settings, "Motor", { "Off", "Velocity", "Position" }, (int)mConstraints[0]->GetPositionMotorState(), [this](int inItem) { for (PathConstraint *c : mConstraints) c->SetPositionMotorState((EMotorState)inItem); });
+		inUI->CreateSlider(runtime_settings, "Target Velocity (m/s)", mConstraints[0]->GetTargetVelocity(), -10.0f, 10.0f, 0.1f, [this](float inValue) { for (PathConstraint *c : mConstraints) c->SetTargetVelocity(inValue); });
+		inUI->CreateSlider(runtime_settings, "Target Path Fraction", mConstraints[0]->GetTargetPathFraction(), 0, mPaths[0]->GetPathMaxFraction(), 0.1f, [this](float inValue) { for (PathConstraint *c : mConstraints) c->SetTargetPathFraction(inValue); });
+		inUI->CreateSlider(runtime_settings, "Max Acceleration (m/s^2)", sMaxMotorAcceleration, 0.0f, 100.0f, 1.0f, [](float inValue) { sMaxMotorAcceleration = inValue; });
+		inUI->CreateSlider(runtime_settings, "Frequency (Hz)", sFrequency, 0.0f, 20.0f, 0.1f, [](float inValue) { sFrequency = inValue; });
+		inUI->CreateSlider(runtime_settings, "Damping", sDamping, 0.0f, 2.0f, 0.01f, [](float inValue) { sDamping = inValue; });
+		inUI->CreateSlider(runtime_settings, "Max Friction Acceleration (m/s^2)", sMaxFrictionAcceleration, 0.0f, 10.0f, 0.1f, [](float inValue) { sMaxFrictionAcceleration = inValue; });
+		inUI->ShowMenu(runtime_settings);
+	});
+
+	inUI->ShowMenu(inSubMenu);
+}

lib/All/JoltPhysics/Samples/Tests/Constraints/PathConstraintTest.h

@@ -0,0 +1,31 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/Test.h>
+#include <Jolt/Physics/Constraints/PathConstraint.h>
+
+class PathConstraintTest : public Test
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, PathConstraintTest)
+
+	virtual void						Initialize() override;
+
+	virtual void						PrePhysicsUpdate(const PreUpdateParams &inParams) override;
+
+	virtual bool						HasSettingsMenu() const override							{ return true; }
+	virtual void						CreateSettingsMenu(DebugUI *inUI, UIElement *inSubMenu) override;
+
+private:
+	Ref<PathConstraintPath>				mPaths[2];
+	Ref<PathConstraint>					mConstraints[2];
+
+	inline static float					sMaxMotorAcceleration = 20.0f;
+	inline static float					sMaxFrictionAcceleration = 0.0f;
+	inline static float					sFrequency = 2.0f;
+	inline static float					sDamping = 1.0f;
+	static EPathRotationConstraintType	sOrientationType;					///< The orientation constraint type for the path constraint
+};

lib/All/JoltPhysics/Samples/Tests/Constraints/PointConstraintTest.cpp

@@ -0,0 +1,55 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/Constraints/PointConstraintTest.h>
+#include <Jolt/Physics/Collision/Shape/CapsuleShape.h>
+#include <Jolt/Physics/Collision/GroupFilterTable.h>
+#include <Jolt/Physics/Constraints/PointConstraint.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Layers.h>
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(PointConstraintTest)
+{
+	JPH_ADD_BASE_CLASS(PointConstraintTest, Test)
+}
+
+void PointConstraintTest::Initialize()
+{
+	// Floor
+	CreateFloor();
+
+	float half_cylinder_height = 2.5f;
+
+	const int cChainLength = 15;
+
+	// Build a collision group filter that disables collision between adjacent bodies
+	Ref<GroupFilterTable> group_filter = new GroupFilterTable(cChainLength);
+	for (CollisionGroup::SubGroupID i = 0; i < cChainLength - 1; ++i)
+		group_filter->DisableCollision(i, i + 1);
+
+	// Bodies attached through point constraints
+	Quat rotation = Quat::sRotation(Vec3::sAxisZ(), 0.5f * JPH_PI);
+	RVec3 position(0, 50, 0);
+	Body &top = *mBodyInterface->CreateBody(BodyCreationSettings(new CapsuleShape(half_cylinder_height, 1), position, rotation, EMotionType::Static, Layers::NON_MOVING));
+	top.SetCollisionGroup(CollisionGroup(group_filter, 0, 0));
+	mBodyInterface->AddBody(top.GetID(), EActivation::DontActivate);
+
+	Body *prev = &top;
+	for (int i = 1; i < cChainLength; ++i)
+	{
+		position += Vec3(2.0f * half_cylinder_height, 0, 0);
+
+		Body &segment = *mBodyInterface->CreateBody(BodyCreationSettings(new CapsuleShape(half_cylinder_height, 1), position, rotation, EMotionType::Dynamic, Layers::MOVING));
+		segment.SetCollisionGroup(CollisionGroup(group_filter, 0, CollisionGroup::SubGroupID(i)));
+		mBodyInterface->AddBody(segment.GetID(), EActivation::Activate);
+
+		PointConstraintSettings settings;
+		settings.mPoint1 = settings.mPoint2 = position + Vec3(-half_cylinder_height, 0, 0);
+		mPhysicsSystem->AddConstraint(settings.Create(*prev, segment));
+
+		prev = &segment;
+	}
+}

lib/All/JoltPhysics/Samples/Tests/Constraints/PointConstraintTest.h

@@ -0,0 +1,16 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/Test.h>
+
+class PointConstraintTest : public Test
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, PointConstraintTest)
+
+	// See: Test
+	virtual void		Initialize() override;
+};

lib/All/JoltPhysics/Samples/Tests/Constraints/PoweredHingeConstraintTest.cpp

@@ -0,0 +1,82 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/Constraints/PoweredHingeConstraintTest.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Collision/GroupFilterTable.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Application/DebugUI.h>
+#include <Layers.h>
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(PoweredHingeConstraintTest)
+{
+	JPH_ADD_BASE_CLASS(PoweredHingeConstraintTest, Test)
+}
+
+void PoweredHingeConstraintTest::Initialize()
+{
+	// Floor
+	CreateFloor();
+
+	// Create group filter
+	Ref<GroupFilterTable> group_filter = new GroupFilterTable;
+
+	// Create box
+	float box_size = 4.0f;
+	RefConst<Shape> box = new BoxShape(Vec3::sReplicate(0.5f * box_size));
+
+	RVec3 body1_position(0, 10, 0);
+	Body &body1 = *mBodyInterface->CreateBody(BodyCreationSettings(box, body1_position, Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING));
+	body1.SetCollisionGroup(CollisionGroup(group_filter, 0, 0));
+	mBodyInterface->AddBody(body1.GetID(), EActivation::DontActivate);
+
+	RVec3 body2_position = body1_position + Vec3(box_size, 0, 0);
+	Body &body2 = *mBodyInterface->CreateBody(BodyCreationSettings(box, body2_position, Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING));
+	body2.SetCollisionGroup(CollisionGroup(group_filter, 0, 0));
+	body2.GetMotionProperties()->SetLinearDamping(0.0f);
+	body2.GetMotionProperties()->SetAngularDamping(0.0f);
+	body2.SetAllowSleeping(false);
+	mBodyInterface->AddBody(body2.GetID(), EActivation::Activate);
+
+	RVec3 constraint_position = body1_position + Vec3(0.5f * box_size, 0, 0.5f * box_size);
+
+	HingeConstraintSettings settings;
+	settings.mPoint1 = settings.mPoint2 = constraint_position;
+	settings.mHingeAxis1 = settings.mHingeAxis2 = Vec3::sAxisY();
+	settings.mNormalAxis1 = settings.mNormalAxis2 = Vec3::sAxisX();
+	mConstraint = static_cast<HingeConstraint *>(settings.Create(body1, body2));
+	mConstraint->SetMotorState(EMotorState::Velocity);
+	mConstraint->SetTargetAngularVelocity(DegreesToRadians(25));
+	mPhysicsSystem->AddConstraint(mConstraint);
+
+	// Calculate inertia of body 2 as seen from the constraint
+	MassProperties body2_inertia_from_constraint;
+	body2_inertia_from_constraint.mMass = 1.0f / body2.GetMotionProperties()->GetInverseMass();
+	body2_inertia_from_constraint.mInertia = body2.GetMotionProperties()->GetLocalSpaceInverseInertia().Inversed3x3();
+	body2_inertia_from_constraint.Translate(Vec3(body2_position - constraint_position));
+	mInertiaBody2AsSeenFromConstraint = (body2_inertia_from_constraint.mInertia * Vec3::sAxisY()).Length();
+}
+
+void PoweredHingeConstraintTest::PrePhysicsUpdate(const PreUpdateParams &inParams)
+{
+	// Torque = Inertia * Angular Acceleration (alpha)
+	MotorSettings &motor_settings = mConstraint->GetMotorSettings();
+	motor_settings.SetTorqueLimit(mInertiaBody2AsSeenFromConstraint * sMaxAngularAcceleration);
+	motor_settings.mSpringSettings.mFrequency = sFrequency;
+	motor_settings.mSpringSettings.mDamping = sDamping;
+	mConstraint->SetMaxFrictionTorque(mInertiaBody2AsSeenFromConstraint * sMaxFrictionAngularAcceleration);
+}
+
+void PoweredHingeConstraintTest::CreateSettingsMenu(DebugUI *inUI, UIElement *inSubMenu)
+{
+	inUI->CreateComboBox(inSubMenu, "Motor", { "Off", "Velocity", "Position" }, (int)mConstraint->GetMotorState(), [this](int inItem) { mConstraint->SetMotorState((EMotorState)inItem); });
+	inUI->CreateSlider(inSubMenu, "Target Angular Velocity (deg/s)", RadiansToDegrees(mConstraint->GetTargetAngularVelocity()), -90.0f, 90.0f, 1.0f, [this](float inValue) { mConstraint->SetTargetAngularVelocity(DegreesToRadians(inValue)); });
+	inUI->CreateSlider(inSubMenu, "Target Angle (deg)", RadiansToDegrees(mConstraint->GetTargetAngle()), -180.0f, 180.0f, 1.0f, [this](float inValue) { mConstraint->SetTargetAngle(DegreesToRadians(inValue)); });
+	inUI->CreateSlider(inSubMenu, "Max Angular Acceleration (deg/s^2)", RadiansToDegrees(sMaxAngularAcceleration), 0.0f, 3600.0f, 10.0f, [](float inValue) { sMaxAngularAcceleration = DegreesToRadians(inValue); });
+	inUI->CreateSlider(inSubMenu, "Frequency (Hz)", sFrequency, 0.0f, 20.0f, 0.1f, [](float inValue) { sFrequency = inValue; });
+	inUI->CreateSlider(inSubMenu, "Damping", sDamping, 0.0f, 2.0f, 0.01f, [](float inValue) { sDamping = inValue; });
+	inUI->CreateSlider(inSubMenu, "Max Friction Angular Acceleration (deg/s^2)", RadiansToDegrees(sMaxFrictionAngularAcceleration), 0.0f, 90.0f, 1.0f, [](float inValue) { sMaxFrictionAngularAcceleration = DegreesToRadians(inValue); });
+}

lib/All/JoltPhysics/Samples/Tests/Constraints/PoweredHingeConstraintTest.h

@@ -0,0 +1,29 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/Test.h>
+#include <Jolt/Physics/Constraints/HingeConstraint.h>
+
+class PoweredHingeConstraintTest : public Test
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, PoweredHingeConstraintTest)
+
+	virtual void			Initialize() override;
+	virtual void			PrePhysicsUpdate(const PreUpdateParams &inParams) override;
+
+	virtual bool			HasSettingsMenu() const override							{ return true; }
+	virtual void			CreateSettingsMenu(DebugUI *inUI, UIElement *inSubMenu) override;
+
+private:
+	inline static float		sMaxAngularAcceleration = DegreesToRadians(3600.0f);
+	inline static float		sMaxFrictionAngularAcceleration = 0.0f;
+	inline static float		sFrequency = 2.0f;
+	inline static float		sDamping = 1.0f;
+
+	HingeConstraint *		mConstraint = nullptr;
+	float					mInertiaBody2AsSeenFromConstraint;
+};

lib/All/JoltPhysics/Samples/Tests/Constraints/PoweredSliderConstraintTest.cpp

@@ -0,0 +1,74 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/Constraints/PoweredSliderConstraintTest.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Collision/GroupFilterTable.h>
+#include <Jolt/Physics/Constraints/SliderConstraint.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Application/DebugUI.h>
+#include <Layers.h>
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(PoweredSliderConstraintTest)
+{
+	JPH_ADD_BASE_CLASS(PoweredSliderConstraintTest, Test)
+}
+
+void PoweredSliderConstraintTest::Initialize()
+{
+	// Floor
+	CreateFloor();
+
+	// Create group filter
+	Ref<GroupFilterTable> group_filter = new GroupFilterTable;
+
+	// Create box
+	float box_size = 4.0f;
+	RefConst<Shape> box = new BoxShape(Vec3::sReplicate(0.5f * box_size));
+
+	RVec3 position(0, 10, 0);
+	Body &body1 = *mBodyInterface->CreateBody(BodyCreationSettings(box, position, Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING));
+	body1.SetCollisionGroup(CollisionGroup(group_filter, 0, 0));
+	mBodyInterface->AddBody(body1.GetID(), EActivation::DontActivate);
+
+	position += Vec3(box_size + 10.0f, 0, 0);
+
+	mBody2 = mBodyInterface->CreateBody(BodyCreationSettings(box, position, Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING));
+	mBody2->SetCollisionGroup(CollisionGroup(group_filter, 0, 0));
+	mBody2->GetMotionProperties()->SetLinearDamping(0.0f);
+	mBody2->SetAllowSleeping(false);
+	mBodyInterface->AddBody(mBody2->GetID(), EActivation::Activate);
+
+	SliderConstraintSettings settings;
+	settings.mAutoDetectPoint = true;
+	settings.SetSliderAxis(Vec3::sAxisX());
+	settings.mLimitsMin = -5.0f;
+	settings.mLimitsMax = 100.0f;
+	mConstraint = static_cast<SliderConstraint *>(settings.Create(body1, *mBody2));
+	mConstraint->SetMotorState(EMotorState::Velocity);
+	mConstraint->SetTargetVelocity(1);
+	mPhysicsSystem->AddConstraint(mConstraint);
+}
+
+void PoweredSliderConstraintTest::PrePhysicsUpdate(const PreUpdateParams &inParams)
+{
+	MotorSettings &motor_settings = mConstraint->GetMotorSettings();
+	motor_settings.SetForceLimit(sMaxMotorAcceleration / mBody2->GetMotionProperties()->GetInverseMass()); // F = m * a
+	motor_settings.mSpringSettings.mFrequency = sFrequency;
+	motor_settings.mSpringSettings.mDamping = sDamping;
+	mConstraint->SetMaxFrictionForce(sMaxFrictionAcceleration / mBody2->GetMotionProperties()->GetInverseMass());
+}
+
+void PoweredSliderConstraintTest::CreateSettingsMenu(DebugUI *inUI, UIElement *inSubMenu)
+{
+	inUI->CreateComboBox(inSubMenu, "Motor", { "Off", "Velocity", "Position" }, (int)mConstraint->GetMotorState(), [this](int inItem) { mConstraint->SetMotorState((EMotorState)inItem); });
+	inUI->CreateSlider(inSubMenu, "Target Velocity (m/s)", mConstraint->GetTargetVelocity(), -10.0f, 10.0f, 0.1f, [this](float inValue) { mConstraint->SetTargetVelocity(inValue); });
+	inUI->CreateSlider(inSubMenu, "Target Position (m)", mConstraint->GetTargetPosition(), -5.0f, 20.0f, 0.1f, [this](float inValue) { mConstraint->SetTargetPosition(inValue); });
+	inUI->CreateSlider(inSubMenu, "Max Acceleration (m/s^2)", sMaxMotorAcceleration, 0.0f, 250.0f, 1.0f, [](float inValue) { sMaxMotorAcceleration = inValue; });
+	inUI->CreateSlider(inSubMenu, "Frequency (Hz)", sFrequency, 0.0f, 20.0f, 0.1f, [](float inValue) { sFrequency = inValue; });
+	inUI->CreateSlider(inSubMenu, "Damping", sDamping, 0.0f, 2.0f, 0.01f, [](float inValue) { sDamping = inValue; });
+	inUI->CreateSlider(inSubMenu, "Max Friction Acceleration (m/s^2)", sMaxFrictionAcceleration, 0.0f, 10.0f, 0.1f, [](float inValue) { sMaxFrictionAcceleration = inValue; });
+}

lib/All/JoltPhysics/Samples/Tests/Constraints/PoweredSliderConstraintTest.h

@@ -0,0 +1,30 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/Test.h>
+#include <Jolt/Physics/Constraints/SliderConstraint.h>
+
+class PoweredSliderConstraintTest : public Test
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, PoweredSliderConstraintTest)
+
+	// See: Test
+	virtual void			Initialize() override;
+	virtual void			PrePhysicsUpdate(const PreUpdateParams &inParams) override;
+
+	virtual bool			HasSettingsMenu() const override							{ return true; }
+	virtual void			CreateSettingsMenu(DebugUI *inUI, UIElement *inSubMenu) override;
+
+private:
+	inline static float		sMaxMotorAcceleration = 250.0f;
+	inline static float		sMaxFrictionAcceleration = 0.0f;
+	inline static float		sFrequency = 2.0f;
+	inline static float		sDamping = 1.0f;
+
+	Body *					mBody2 = nullptr;
+	SliderConstraint *		mConstraint = nullptr;
+};

lib/All/JoltPhysics/Samples/Tests/Constraints/PoweredSwingTwistConstraintTest.cpp

@@ -0,0 +1,142 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/Constraints/PoweredSwingTwistConstraintTest.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Collision/GroupFilterTable.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Application/DebugUI.h>
+#include <Layers.h>
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(PoweredSwingTwistConstraintTest)
+{
+	JPH_ADD_BASE_CLASS(PoweredSwingTwistConstraintTest, Test)
+}
+
+Vec3 PoweredSwingTwistConstraintTest::sBodyRotation[] = { Vec3::sZero(), Vec3::sZero() };
+
+void PoweredSwingTwistConstraintTest::Initialize()
+{
+	// Floor
+	CreateFloor();
+
+	// Create group filter
+	Ref<GroupFilterTable> group_filter = new GroupFilterTable;
+
+	float half_box_height = 1.5f;
+	RefConst<Shape> box = new BoxShape(Vec3(0.25f, half_box_height, 0.5f));
+	Quat body1_rotation = Quat::sEulerAngles(sBodyRotation[0]);
+	Quat body2_rotation = Quat::sEulerAngles(sBodyRotation[1]) * body1_rotation;
+
+	RVec3 body1_position(0, 20, 0);
+	Body &body1 = *mBodyInterface->CreateBody(BodyCreationSettings(box, body1_position, body1_rotation, EMotionType::Static, Layers::NON_MOVING));
+	body1.SetCollisionGroup(CollisionGroup(group_filter, 0, 0));
+	mBodyInterface->AddBody(body1.GetID(), EActivation::DontActivate);
+
+	RVec3 constraint_position = body1_position + body1_rotation * Vec3(0, -half_box_height, 0);
+
+	RVec3 body2_position = constraint_position + body2_rotation * Vec3(0, -half_box_height, 0);
+	Body &body2 = *mBodyInterface->CreateBody(BodyCreationSettings(box, body2_position, body2_rotation, EMotionType::Dynamic, Layers::MOVING));
+	body2.SetCollisionGroup(CollisionGroup(group_filter, 0, 0));
+	body2.GetMotionProperties()->SetLinearDamping(0.0f);
+	body2.GetMotionProperties()->SetAngularDamping(0.0f);
+	body2.SetAllowSleeping(false);
+	mBodyInterface->AddBody(body2.GetID(), EActivation::Activate);
+
+	Ref<SwingTwistConstraintSettings> settings = new SwingTwistConstraintSettings;
+	settings->mNormalHalfConeAngle = sNormalHalfConeAngle;
+	settings->mPlaneHalfConeAngle = sPlaneHalfConeAngle;
+	settings->mTwistMinAngle = sTwistMinAngle;
+	settings->mTwistMaxAngle = sTwistMaxAngle;
+
+	settings->mPosition1 = settings->mPosition2 = constraint_position;
+	settings->mTwistAxis1 = settings->mTwistAxis2 = -body1_rotation.RotateAxisY();
+	settings->mPlaneAxis1 = settings->mPlaneAxis2 = body1_rotation.RotateAxisX();
+
+	mConstraint = static_cast<SwingTwistConstraint *>(settings->Create(body1, body2));
+	mPhysicsSystem->AddConstraint(mConstraint);
+
+	// Calculate inertia along the axis of the box, so that the acceleration of the motor / friction are correct for twist
+	Mat44 body2_inertia = body2.GetMotionProperties()->GetLocalSpaceInverseInertia().Inversed3x3();
+	mInertiaBody2AsSeenFromConstraint = (body2_inertia * Vec3::sAxisY()).Length();
+}
+
+void PoweredSwingTwistConstraintTest::PrePhysicsUpdate(const PreUpdateParams &inParams)
+{
+	// Torque = Inertia * Angular Acceleration (alpha)
+	mConstraint->SetMaxFrictionTorque(mInertiaBody2AsSeenFromConstraint * sMaxFrictionAngularAcceleration);
+
+	mConstraint->SetNormalHalfConeAngle(sNormalHalfConeAngle);
+	mConstraint->SetPlaneHalfConeAngle(sPlaneHalfConeAngle);
+	mConstraint->SetTwistMinAngle(sTwistMinAngle);
+	mConstraint->SetTwistMaxAngle(sTwistMaxAngle);
+
+	mConstraint->SetSwingMotorState(sSwingMotorState);
+	mConstraint->SetTwistMotorState(sTwistMotorState);
+	mConstraint->SetTargetAngularVelocityCS(sTargetVelocityCS);
+	mConstraint->SetTargetOrientationCS(Quat::sEulerAngles(sTargetOrientationCS));
+
+	MotorSettings &swing = mConstraint->GetSwingMotorSettings();
+	swing.SetTorqueLimit(mInertiaBody2AsSeenFromConstraint * sMaxAngularAcceleration);
+	swing.mSpringSettings.mFrequency = sFrequency;
+	swing.mSpringSettings.mDamping = sDamping;
+
+	MotorSettings &twist = mConstraint->GetTwistMotorSettings();
+	twist.SetTorqueLimit(mInertiaBody2AsSeenFromConstraint * sMaxAngularAcceleration);
+	twist.mSpringSettings.mFrequency = sFrequency;
+	twist.mSpringSettings.mDamping = sDamping;
+}
+
+void PoweredSwingTwistConstraintTest::GetInitialCamera(CameraState &ioState) const
+{
+	ioState.mPos = RVec3(4, 25, 4);
+	ioState.mForward = Vec3(-1, -1, -1).Normalized();
+}
+
+void PoweredSwingTwistConstraintTest::CreateSettingsMenu(DebugUI *inUI, UIElement *inSubMenu)
+{
+	Array<String> axis_label = { "X", "Y", "Z" };
+	Array<String> constraint_label = { "Twist", "Plane", "Normal" };
+
+	inUI->CreateTextButton(inSubMenu, "Configuration Settings", [this, inUI, axis_label]() {
+		UIElement *configuration_settings = inUI->CreateMenu();
+
+		for (int body = 0; body < 2; ++body)
+			for (int axis = 0; axis < 3; ++axis)
+				inUI->CreateSlider(configuration_settings, "Body " + ConvertToString(body + 1) + " Rotation " + axis_label[axis] + " (deg)", RadiansToDegrees(sBodyRotation[body][axis]), -180.0f, 180.0f, 1.0f, [=](float inValue) { sBodyRotation[body].SetComponent(axis, DegreesToRadians(inValue)); });
+
+		inUI->CreateTextButton(configuration_settings, "Accept Changes", [this]() { RestartTest(); });
+
+		inUI->ShowMenu(configuration_settings);
+	});
+
+	inUI->CreateTextButton(inSubMenu, "Runtime Settings", [=]() {
+		UIElement *runtime_settings = inUI->CreateMenu();
+
+		inUI->CreateSlider(runtime_settings, "Min Twist Angle (deg)", RadiansToDegrees(sTwistMinAngle), -180.0f, 180.0f, 1.0f, [=](float inValue) { sTwistMinAngle = DegreesToRadians(inValue); });
+		inUI->CreateSlider(runtime_settings, "Max Twist Angle (deg)", RadiansToDegrees(sTwistMaxAngle), -180.0f, 180.0f, 1.0f, [=](float inValue) { sTwistMaxAngle = DegreesToRadians(inValue); });
+		inUI->CreateSlider(runtime_settings, "Normal Half Cone Angle (deg)", RadiansToDegrees(sNormalHalfConeAngle), 0.0f, 180.0f, 1.0f, [=](float inValue) { sNormalHalfConeAngle = DegreesToRadians(inValue); });
+		inUI->CreateSlider(runtime_settings, "Plane Half Cone Angle (deg)", RadiansToDegrees(sPlaneHalfConeAngle), 0.0f, 180.0f, 1.0f, [=](float inValue) { sPlaneHalfConeAngle = DegreesToRadians(inValue); });
+
+		inUI->CreateComboBox(runtime_settings, "Swing Motor", { "Off", "Velocity", "Position" }, (int)sSwingMotorState, [=](int inItem) { sSwingMotorState = (EMotorState)inItem; });
+		inUI->CreateComboBox(runtime_settings, "Twist Motor", { "Off", "Velocity", "Position" }, (int)sTwistMotorState, [=](int inItem) { sTwistMotorState = (EMotorState)inItem; });
+
+		for (int i = 0; i < 3; ++i)
+			inUI->CreateSlider(runtime_settings, "Target Angular Velocity " + constraint_label[i] + " (deg/s)", RadiansToDegrees(sTargetVelocityCS[i]), -90.0f, 90.0f, 1.0f, [i](float inValue) { sTargetVelocityCS.SetComponent(i, DegreesToRadians(inValue)); });
+
+		for (int i = 0; i < 3; ++i)
+			inUI->CreateSlider(runtime_settings, "Target Angle " + constraint_label[i] + " (deg)", RadiansToDegrees(sTargetOrientationCS[i]), -180, 180.0f, 1.0f, [i](float inValue) {
+				sTargetOrientationCS.SetComponent(i, DegreesToRadians(Clamp(inValue, -179.99f, 179.99f))); // +/- 180 degrees is ambiguous, so add a little bit of a margin
+			});
+
+		inUI->CreateSlider(runtime_settings, "Max Angular Acceleration (deg/s^2)", RadiansToDegrees(sMaxAngularAcceleration), 0.0f, 36000.0f, 100.0f, [=](float inValue) { sMaxAngularAcceleration = DegreesToRadians(inValue); });
+		inUI->CreateSlider(runtime_settings, "Frequency (Hz)", sFrequency, 0.0f, 20.0f, 0.1f, [=](float inValue) { sFrequency = inValue; });
+		inUI->CreateSlider(runtime_settings, "Damping", sDamping, 0.0f, 2.0f, 0.01f, [=](float inValue) { sDamping = inValue; });
+		inUI->CreateSlider(runtime_settings, "Max Friction Angular Acceleration (deg/s^2)", RadiansToDegrees(sMaxFrictionAngularAcceleration), 0.0f, 900.0f, 1.0f, [=](float inValue) { sMaxFrictionAngularAcceleration = DegreesToRadians(inValue); });
+
+		inUI->ShowMenu(runtime_settings);
+	});
+}

lib/All/JoltPhysics/Samples/Tests/Constraints/PoweredSwingTwistConstraintTest.h

@@ -0,0 +1,40 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/Test.h>
+#include <Jolt/Physics/Constraints/SwingTwistConstraint.h>
+
+class PoweredSwingTwistConstraintTest : public Test
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, PoweredSwingTwistConstraintTest)
+
+	virtual void				Initialize() override;
+	virtual void				PrePhysicsUpdate(const PreUpdateParams &inParams) override;
+
+	virtual void				GetInitialCamera(CameraState &ioState) const override;
+
+	virtual bool				HasSettingsMenu() const override							{ return true; }
+	virtual void				CreateSettingsMenu(DebugUI *inUI, UIElement *inSubMenu) override;
+
+private:
+	static Vec3					sBodyRotation[2];
+	inline static EMotorState	sSwingMotorState = EMotorState::Velocity;
+	inline static EMotorState	sTwistMotorState = EMotorState::Velocity;
+	inline static Vec3			sTargetVelocityCS = Vec3(DegreesToRadians(90), 0, 0);
+	inline static Vec3			sTargetOrientationCS = Vec3::sZero();
+	inline static float			sMaxAngularAcceleration = DegreesToRadians(36000.0f);
+	inline static float			sMaxFrictionAngularAcceleration = 0.0f;
+	inline static float			sNormalHalfConeAngle = DegreesToRadians(60);
+	inline static float			sPlaneHalfConeAngle = DegreesToRadians(45);
+	inline static float			sTwistMinAngle = DegreesToRadians(-180);
+	inline static float			sTwistMaxAngle = DegreesToRadians(180);
+	inline static float			sFrequency = 10.0f;
+	inline static float			sDamping = 2.0f;
+
+	SwingTwistConstraint *		mConstraint = nullptr;
+	float						mInertiaBody2AsSeenFromConstraint;
+};

lib/All/JoltPhysics/Samples/Tests/Constraints/PulleyConstraintTest.cpp

@@ -0,0 +1,68 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2022 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/Constraints/PulleyConstraintTest.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Constraints/PulleyConstraint.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Layers.h>
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(PulleyConstraintTest)
+{
+	JPH_ADD_BASE_CLASS(PulleyConstraintTest, Test)
+}
+
+void PulleyConstraintTest::Initialize()
+{
+	// Floor
+	CreateFloor();
+
+	// Variation 0: Max length (rope)
+	// Variation 1: Fixed length (rigid rod)
+	// Variation 2: Min/max length
+	// Variation 3: With ratio (block and tackle)
+	for (int variation = 0; variation < 4; ++variation)
+	{
+		RVec3 position1(-10, 10, -10.0f * variation);
+		Body &body1 = *mBodyInterface->CreateBody(BodyCreationSettings(new BoxShape(Vec3::sReplicate(0.5f)), position1, Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING));
+		mBodyInterface->AddBody(body1.GetID(), EActivation::Activate);
+
+		RVec3 position2(10, 10, -10.0f * variation);
+		Body &body2 = *mBodyInterface->CreateBody(BodyCreationSettings(new BoxShape(Vec3::sReplicate(0.5f)), position2, Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING));
+		mBodyInterface->AddBody(body2.GetID(), EActivation::Activate);
+
+		PulleyConstraintSettings settings;
+		settings.mBodyPoint1 = position1 + Vec3(0, 0.5f, 0); // Connect at the top of the block
+		settings.mBodyPoint2 = position2 + Vec3(0, 0.5f, 0);
+		settings.mFixedPoint1 = settings.mBodyPoint1 + Vec3(0, 10, 0);
+		settings.mFixedPoint2 = settings.mBodyPoint2 + Vec3(0, 10, 0);
+
+		switch (variation)
+		{
+		case 0:
+			// Can't extend but can contract
+			break;
+
+		case 1:
+			// Fixed size
+			settings.mMinLength = settings.mMaxLength = -1;
+			break;
+
+		case 2:
+			// With range
+			settings.mMinLength = 18.0f;
+			settings.mMaxLength = 22.0f;
+			break;
+
+		case 3:
+			// With ratio
+			settings.mRatio = 4.0f;
+			break;
+		}
+
+		mPhysicsSystem->AddConstraint(settings.Create(body1, body2));
+	}
+}

lib/All/JoltPhysics/Samples/Tests/Constraints/PulleyConstraintTest.h

@@ -0,0 +1,17 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2022 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/Test.h>
+
+// Demonstrates the pulley constraint
+class PulleyConstraintTest : public Test
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, PulleyConstraintTest)
+
+	// See: Test
+	virtual void		Initialize() override;
+};

lib/All/JoltPhysics/Samples/Tests/Constraints/RackAndPinionConstraintTest.cpp

@@ -0,0 +1,126 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/Constraints/RackAndPinionConstraintTest.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Collision/Shape/CylinderShape.h>
+#include <Jolt/Physics/Collision/Shape/ConvexHullShape.h>
+#include <Jolt/Physics/Collision/Shape/StaticCompoundShape.h>
+#include <Jolt/Physics/Collision/GroupFilterTable.h>
+#include <Jolt/Physics/Constraints/HingeConstraint.h>
+#include <Jolt/Physics/Constraints/SliderConstraint.h>
+#include <Jolt/Physics/Constraints/RackAndPinionConstraint.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Layers.h>
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(RackAndPinionConstraintTest)
+{
+	JPH_ADD_BASE_CLASS(RackAndPinionConstraintTest, Test)
+}
+
+void RackAndPinionConstraintTest::Initialize()
+{
+	// Floor
+	CreateFloor();
+
+	constexpr float cGearRadius = 0.5f;
+	constexpr float cGearHalfWidth = 0.05f;
+	constexpr int cGearNumTeeth = 100;
+
+	constexpr float cRackLength = 10.0f;
+	constexpr float cRackHalfHeight = 0.1f;
+	constexpr float cRackHalfWidth = 0.05f;
+	constexpr int cRackNumTeeth = int(cRackLength * cGearNumTeeth / (2.0f * JPH_PI * cGearRadius));
+
+	constexpr float cToothThicknessBottom = 0.01f;
+	constexpr float cToothThicknessTop = 0.005f;
+	constexpr float cToothHeight = 0.02f;
+
+	// Create a tooth
+	Array<Vec3> tooth_points = {
+		Vec3(0, cGearHalfWidth, cToothThicknessBottom),
+		Vec3(0, -cGearHalfWidth, cToothThicknessBottom),
+		Vec3(0, cGearHalfWidth, -cToothThicknessBottom),
+		Vec3(0, -cGearHalfWidth, -cToothThicknessBottom),
+		Vec3(cToothHeight, -cGearHalfWidth, cToothThicknessTop),
+		Vec3(cToothHeight, cGearHalfWidth, cToothThicknessTop),
+		Vec3(cToothHeight, -cGearHalfWidth, -cToothThicknessTop),
+		Vec3(cToothHeight, cGearHalfWidth, -cToothThicknessTop),
+	};
+	ConvexHullShapeSettings tooth_settings(tooth_points);
+	tooth_settings.SetEmbedded();
+
+	// Create gear
+	CylinderShapeSettings gear_cylinder(cGearHalfWidth, cGearRadius);
+	gear_cylinder.SetEmbedded();
+
+	StaticCompoundShapeSettings gear_settings;
+	gear_settings.SetEmbedded();
+
+	gear_settings.AddShape(Vec3::sZero(), Quat::sIdentity(), &gear_cylinder);
+	for (int i = 0; i < cGearNumTeeth; ++i)
+	{
+		Quat rotation = Quat::sRotation(Vec3::sAxisY(), 2.0f * JPH_PI * i / cGearNumTeeth);
+		gear_settings.AddShape(rotation * Vec3(cGearRadius, 0, 0), rotation, &tooth_settings);
+	}
+
+	RVec3 gear_initial_p(0, 2.0f, 0);
+	Quat gear_initial_r = Quat::sRotation(Vec3::sAxisX(), 0.5f * JPH_PI);
+	Body *gear = mBodyInterface->CreateBody(BodyCreationSettings(&gear_settings, gear_initial_p, gear_initial_r, EMotionType::Dynamic, Layers::MOVING));
+	mBodyInterface->AddBody(gear->GetID(), EActivation::Activate);
+
+	// Create rack
+	BoxShapeSettings rack_box(Vec3(cRackHalfHeight, cRackHalfWidth, 0.5f * cRackLength), 0.0f);
+	rack_box.SetEmbedded();
+
+	StaticCompoundShapeSettings rack_settings;
+	rack_settings.SetEmbedded();
+
+	rack_settings.AddShape(Vec3::sZero(), Quat::sIdentity(), &rack_box);
+	for (int i = 0; i < cRackNumTeeth; ++i)
+		rack_settings.AddShape(Vec3(cRackHalfHeight, 0, -0.5f * cRackLength + (i + 0.5f) * cRackLength / cRackNumTeeth), Quat::sIdentity(), &tooth_settings);
+
+	RVec3 slider_initial_p = gear_initial_p - Vec3(0, cGearRadius + cRackHalfHeight + cToothHeight, 0);
+	Quat slider_initial_r = Quat::sRotation(Vec3::sAxisZ(), 0.5f * JPH_PI) * gear_initial_r;
+	Body *rack = mBodyInterface->CreateBody(BodyCreationSettings(&rack_settings, slider_initial_p, slider_initial_r, EMotionType::Dynamic, Layers::MOVING));
+	mBodyInterface->AddBody(rack->GetID(), EActivation::Activate);
+
+	// Create a hinge for the gear
+	HingeConstraintSettings hinge;
+	hinge.mPoint1 = hinge.mPoint2 = gear_initial_p;
+	hinge.mHingeAxis1 = hinge.mHingeAxis2 = Vec3::sAxisZ();
+	hinge.mNormalAxis1 = hinge.mNormalAxis2 = Vec3::sAxisX();
+	Constraint *hinge_constraint = hinge.Create(Body::sFixedToWorld, *gear);
+	mPhysicsSystem->AddConstraint(hinge_constraint);
+
+	// Create a slider for the rack
+	SliderConstraintSettings slider;
+	slider.mPoint1 = slider.mPoint2 = gear_initial_p;
+	slider.mSliderAxis1 = slider.mSliderAxis2 = Vec3::sAxisX();
+	slider.mNormalAxis1 = slider.mNormalAxis2 = Vec3::sAxisZ();
+	slider.mLimitsMin = -0.5f * cRackLength;
+	slider.mLimitsMax = 0.5f * cRackLength;
+	Constraint *slider_constraint = slider.Create(Body::sFixedToWorld, *rack);
+	mPhysicsSystem->AddConstraint(slider_constraint);
+
+	// Disable collision between rack and gear (we want the rack and pinion constraint to take care of the relative movement)
+	Ref<GroupFilterTable> group_filter = new GroupFilterTable(2);
+	group_filter->DisableCollision(0, 1);
+	gear->SetCollisionGroup(CollisionGroup(group_filter, 0, 0));
+	rack->SetCollisionGroup(CollisionGroup(group_filter, 0, 1));
+
+	// Create rack and pinion constraint to constrain the two bodies
+	RackAndPinionConstraintSettings randp;
+	randp.mHingeAxis = hinge.mHingeAxis1;
+	randp.mSliderAxis = slider.mSliderAxis2;
+	randp.SetRatio(cRackNumTeeth, cRackLength, cGearNumTeeth);
+	RackAndPinionConstraint *randp_constraint = static_cast<RackAndPinionConstraint *>(randp.Create(*gear, *rack));
+	randp_constraint->SetConstraints(hinge_constraint, slider_constraint);
+	mPhysicsSystem->AddConstraint(randp_constraint);
+
+	// Give the gear a spin
+	gear->SetAngularVelocity(Vec3(0, 0, 6.0f));
+}

lib/All/JoltPhysics/Samples/Tests/Constraints/RackAndPinionConstraintTest.h

@@ -0,0 +1,17 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/Test.h>
+
+// This test demonstrates the use of a rack and pinion constraint
+class RackAndPinionConstraintTest : public Test
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, RackAndPinionConstraintTest)
+
+	// See: Test
+	virtual void		Initialize() override;
+};

lib/All/JoltPhysics/Samples/Tests/Constraints/SixDOFConstraintTest.cpp

@@ -0,0 +1,173 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/Constraints/SixDOFConstraintTest.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Collision/GroupFilterTable.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Application/DebugUI.h>
+#include <Layers.h>
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(SixDOFConstraintTest)
+{
+	JPH_ADD_BASE_CLASS(SixDOFConstraintTest, Test)
+}
+
+float SixDOFConstraintTest::sLimitMin[EAxis::Num] = { 0, 0, 0, 0, 0, 0 };
+float SixDOFConstraintTest::sLimitMax[EAxis::Num] = { 0, 0, 0, 0, 0, 0 };
+bool SixDOFConstraintTest::sEnableLimits[EAxis::Num] = { true, true, true, true, true, true };
+
+SixDOFConstraintTest::SettingsRef SixDOFConstraintTest::sSettings = []() {
+		static SixDOFConstraintSettings settings;
+		settings.SetEmbedded();
+		settings.mAxisX1 = settings.mAxisX2 = -Vec3::sAxisY();
+		settings.mAxisY1 = settings.mAxisY2 = Vec3::sAxisZ();
+		for (int i = 0; i < 6; ++i)
+			settings.mMotorSettings[i] = MotorSettings(10.0f, 2.0f);
+		return &settings;
+	}();
+
+void SixDOFConstraintTest::Initialize()
+{
+	// Floor
+	CreateFloor();
+
+	// Convert limits to settings class
+	for (int i = 0; i < EAxis::Num; ++i)
+		if (sEnableLimits[i])
+			sSettings->SetLimitedAxis((EAxis)i, sLimitMin[i], sLimitMax[i]);
+		else
+			sSettings->MakeFreeAxis((EAxis)i);
+
+	// Create group filter
+	Ref<GroupFilterTable> group_filter = new GroupFilterTable;
+
+	// Create box
+	float half_box_height = 1.5f;
+	RVec3 position(0, 25, 0);
+	RefConst<BoxShape> box = new BoxShape(Vec3(0.5f, half_box_height, 0.25f));
+
+	// Create static body
+	Body &body1 = *mBodyInterface->CreateBody(BodyCreationSettings(box, position, Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING));
+	body1.SetCollisionGroup(CollisionGroup(group_filter, 0, 0));
+	mBodyInterface->AddBody(body1.GetID(), EActivation::DontActivate);
+
+	// Create dynamic body
+	Body &body2 = *mBodyInterface->CreateBody(BodyCreationSettings(box, position - Vec3(0, 2.0f * half_box_height, 0), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING));
+	body2.SetCollisionGroup(CollisionGroup(group_filter, 0, 0));
+	body2.SetAllowSleeping(false);
+	mBodyInterface->AddBody(body2.GetID(), EActivation::Activate);
+
+	// Set constraint position
+	sSettings->mPosition1 = sSettings->mPosition2 = position - Vec3(0, half_box_height, 0);
+
+	// Set force limits
+	const float max_acceleration = 1.0f;
+	for (int i = 0; i < 3; ++i)
+		sSettings->mMotorSettings[i].SetForceLimit(max_acceleration / body2.GetMotionProperties()->GetInverseMass());
+
+	// Create constraint
+	mConstraint = static_cast<SixDOFConstraint *>(sSettings->Create(body1, body2));
+	mPhysicsSystem->AddConstraint(mConstraint);
+}
+
+void SixDOFConstraintTest::GetInitialCamera(CameraState &ioState) const
+{
+	ioState.mPos = RVec3(4, 30, 4);
+	ioState.mForward = Vec3(-1, -1, -1).Normalized();
+}
+
+void SixDOFConstraintTest::CreateSettingsMenu(DebugUI *inUI, UIElement *inSubMenu)
+{
+	Array<String> labels = { "Translation X", "Translation Y", "Translation Z", "Rotation X", "Rotation Y", "Rotation Z" };
+	Array<String> motor_states = { "Off", "Velocity", "Position" };
+	Array<String> swing_types = { "Cone", "Pyramid" };
+
+	inUI->CreateTextButton(inSubMenu, "Configuration Settings (Limits)", [this, inUI, labels, swing_types]() {
+		UIElement *configuration_settings = inUI->CreateMenu();
+
+		inUI->CreateComboBox(configuration_settings, "Swing Type", swing_types, (int)sSettings->mSwingType, [](int inItem) { sSettings->mSwingType = (ESwingType)inItem; });
+
+		for (int i = 0; i < 3; ++i)
+		{
+			inUI->CreateCheckBox(configuration_settings, "Enable Limits " + labels[i], sEnableLimits[i], [=](UICheckBox::EState inState) { sEnableLimits[i] = inState == UICheckBox::STATE_CHECKED; });
+			inUI->CreateSlider(configuration_settings, "Limit Min", sLimitMin[i], -5.0f, 5.0f, 0.1f, [=](float inValue) { sLimitMin[i] = inValue; });
+			inUI->CreateSlider(configuration_settings, "Limit Max", sLimitMax[i], -5.0f, 5.0f, 0.1f, [=](float inValue) { sLimitMax[i] = inValue; });
+			inUI->CreateSlider(configuration_settings, "Limit Frequency (Hz)", sSettings->mLimitsSpringSettings[i].mFrequency, 0.0f, 20.0f, 0.1f, [=](float inValue) { sSettings->mLimitsSpringSettings[i].mFrequency = inValue; });
+			inUI->CreateSlider(configuration_settings, "Limit Damping", sSettings->mLimitsSpringSettings[i].mDamping, 0.0f, 2.0f, 0.01f, [=](float inValue) { sSettings->mLimitsSpringSettings[i].mDamping = inValue; });
+		}
+
+		for (int i = 3; i < 6; ++i)
+		{
+			inUI->CreateCheckBox(configuration_settings, "Enable Limits " + labels[i], sEnableLimits[i], [=](UICheckBox::EState inState) { sEnableLimits[i] = inState == UICheckBox::STATE_CHECKED; });
+			inUI->CreateSlider(configuration_settings, "Limit Min", RadiansToDegrees(sLimitMin[i]), -180.0f, 180.0f, 1.0f, [=](float inValue) { sLimitMin[i] = DegreesToRadians(inValue); });
+			inUI->CreateSlider(configuration_settings, "Limit Max", RadiansToDegrees(sLimitMax[i]), -180.0f, 180.0f, 1.0f, [=](float inValue) { sLimitMax[i] = DegreesToRadians(inValue); });
+		}
+
+		inUI->CreateTextButton(configuration_settings, "Accept Changes", [this]() { RestartTest(); });
+
+		inUI->ShowMenu(configuration_settings);
+	});
+
+	inUI->CreateTextButton(inSubMenu, "Configuration Settings (Other)", [this, inUI, labels]() {
+		UIElement *configuration_settings = inUI->CreateMenu();
+
+		for (int i = 0; i < 6; ++i)
+			inUI->CreateSlider(configuration_settings, "Max Friction " + labels[i], sSettings->mMaxFriction[i], 0.0f, 500.0f, 1.0f, [=](float inValue) { sSettings->mMaxFriction[i] = inValue; });
+
+		for (int i = 0; i < 6; ++i)
+			inUI->CreateSlider(configuration_settings, "Motor Frequency " + labels[i] + " (Hz)", sSettings->mMotorSettings[i].mSpringSettings.mFrequency, 0.0f, 20.0f, 0.1f, [i](float inValue) { sSettings->mMotorSettings[i].mSpringSettings.mFrequency = inValue; });
+
+		for (int i = 0; i < 6; ++i)
+			inUI->CreateSlider(configuration_settings, "Motor Damping " + labels[i], sSettings->mMotorSettings[i].mSpringSettings.mDamping, 0.0f, 2.0f, 0.01f, [i](float inValue) { sSettings->mMotorSettings[i].mSpringSettings.mDamping = inValue; });
+
+		inUI->CreateTextButton(configuration_settings, "Accept Changes", [this]() { RestartTest(); });
+
+		inUI->ShowMenu(configuration_settings);
+	});
+
+	inUI->CreateTextButton(inSubMenu, "Runtime Settings", [this, inUI, labels, motor_states]() {
+		UIElement *runtime_settings = inUI->CreateMenu();
+
+		for (int i = 0; i < 3; ++i)
+		{
+			EAxis axis = EAxis(EAxis::TranslationX + i);
+
+			UIComboBox *combo = inUI->CreateComboBox(runtime_settings, "Motor " + labels[i], motor_states, (int)mConstraint->GetMotorState(axis), [this, axis](int inItem) { mConstraint->SetMotorState(axis, (EMotorState)inItem); });
+			combo->SetDisabled(sSettings->IsFixedAxis(axis));
+
+			UISlider *velocity = inUI->CreateSlider(runtime_settings, "Target Velocity", mConstraint->GetTargetVelocityCS()[i], -10.0f, 10.0f, 0.1f, [this, i](float inValue) {
+				Vec3 v = mConstraint->GetTargetVelocityCS();
+				v.SetComponent(i, inValue);
+				mConstraint->SetTargetVelocityCS(v); });
+			velocity->SetDisabled(sSettings->IsFixedAxis(axis));
+
+			UISlider *position = inUI->CreateSlider(runtime_settings, "Target Position", mConstraint->GetTargetPositionCS()[i], -10.0f, 10.0f, 0.1f, [this, i](float inValue) {
+				Vec3 v = mConstraint->GetTargetPositionCS();
+				v.SetComponent(i, inValue);
+				mConstraint->SetTargetPositionCS(v); });
+			position->SetDisabled(sSettings->IsFixedAxis(axis));
+		}
+
+		for (int i = 0; i < 3; ++i)
+		{
+			EAxis axis = EAxis(EAxis::RotationX + i);
+
+			inUI->CreateComboBox(runtime_settings, "Motor " + labels[axis], motor_states, (int)mConstraint->GetMotorState(axis), [this, axis](int inItem) { mConstraint->SetMotorState(axis, (EMotorState)inItem); });
+
+			inUI->CreateSlider(runtime_settings, "Target Velocity", RadiansToDegrees(mConstraint->GetTargetAngularVelocityCS()[i]), -90.0f, 90.0f, 1.0f, [this, i](float inValue) {
+				Vec3 v = mConstraint->GetTargetAngularVelocityCS();
+				v.SetComponent(i, DegreesToRadians(inValue));
+				mConstraint->SetTargetAngularVelocityCS(v); });
+
+			inUI->CreateSlider(runtime_settings, "Target Position", RadiansToDegrees(mTargetOrientationCS[i]), -180.0f, 180.0f, 1.0f, [this, i](float inValue) {
+				mTargetOrientationCS.SetComponent(i, DegreesToRadians(Clamp(inValue, -179.99f, 179.99f))); // +/- 180 degrees is ambiguous, so add a little bit of a margin
+				mConstraint->SetTargetOrientationCS(Quat::sEulerAngles(mTargetOrientationCS)); });
+		}
+
+		inUI->ShowMenu(runtime_settings);
+	});
+}

lib/All/JoltPhysics/Samples/Tests/Constraints/SixDOFConstraintTest.h

@@ -0,0 +1,34 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/Test.h>
+#include <Jolt/Physics/Constraints/SixDOFConstraint.h>
+
+class SixDOFConstraintTest : public Test
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, SixDOFConstraintTest)
+
+	virtual void			Initialize() override;
+
+	virtual void			GetInitialCamera(CameraState &ioState) const override;
+
+	virtual bool			HasSettingsMenu() const override							{ return true; }
+	virtual void			CreateSettingsMenu(DebugUI *inUI, UIElement *inSubMenu) override;
+
+private:
+	using SettingsRef = Ref<SixDOFConstraintSettings>;
+	using EAxis = SixDOFConstraintSettings::EAxis;
+
+	static float			sLimitMin[EAxis::Num];
+	static float			sLimitMax[EAxis::Num];
+	static bool				sEnableLimits[EAxis::Num];
+	static SettingsRef		sSettings;
+
+	Vec3					mTargetOrientationCS = Vec3::sZero();
+
+	Ref<SixDOFConstraint>	mConstraint;
+};

lib/All/JoltPhysics/Samples/Tests/Constraints/SliderConstraintTest.cpp

@@ -0,0 +1,149 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/Constraints/SliderConstraintTest.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Collision/GroupFilterTable.h>
+#include <Jolt/Physics/Constraints/SliderConstraint.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Layers.h>
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(SliderConstraintTest)
+{
+	JPH_ADD_BASE_CLASS(SliderConstraintTest, Test)
+}
+
+void SliderConstraintTest::Initialize()
+{
+	// Floor
+	CreateFloor();
+
+	const int cChainLength = 10;
+
+	// Create group filter
+	Ref<GroupFilterTable> group_filter = new GroupFilterTable(cChainLength);
+	for (CollisionGroup::SubGroupID i = 0; i < cChainLength - 1; ++i)
+		group_filter->DisableCollision(i, i + 1);
+	CollisionGroup::GroupID group_id = 0;
+
+	// Create box
+	float box_size = 4.0f;
+	RefConst<Shape> box = new BoxShape(Vec3::sReplicate(0.5f * box_size));
+
+	// Bodies attached through slider constraints
+	for (int randomness = 0; randomness < 2; ++randomness)
+	{
+		RVec3 position(0, 25.0f, -randomness * 20.0f);
+		Body &top = *mBodyInterface->CreateBody(BodyCreationSettings(box, position, Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING));
+		top.SetCollisionGroup(CollisionGroup(group_filter, group_id, 0));
+		mBodyInterface->AddBody(top.GetID(), EActivation::DontActivate);
+
+		default_random_engine random;
+		uniform_real_distribution<float> displacement(-1.0f, 1.0f);
+
+		Body *prev = &top;
+		for (int i = 1; i < cChainLength; ++i)
+		{
+			Quat rotation;
+			Vec3 slider_axis;
+			if (randomness == 0)
+			{
+				position += Vec3(box_size, 0, 0);
+				rotation = Quat::sIdentity();
+				slider_axis = Quat::sRotation(Vec3::sAxisZ(), -DegreesToRadians(10)).RotateAxisX();
+			}
+			else
+			{
+				position += Vec3(box_size + abs(displacement(random)), displacement(random), displacement(random));
+				rotation = Quat::sRandom(random);
+				slider_axis = Quat::sRotation(Vec3::sAxisY(), displacement(random) * DegreesToRadians(20)) * Quat::sRotation(Vec3::sAxisZ(), -DegreesToRadians(10)).RotateAxisX();
+			}
+
+			Body &segment = *mBodyInterface->CreateBody(BodyCreationSettings(box, position, rotation, EMotionType::Dynamic, Layers::MOVING));
+			segment.SetCollisionGroup(CollisionGroup(group_filter, group_id, CollisionGroup::SubGroupID(i)));
+			mBodyInterface->AddBody(segment.GetID(), EActivation::Activate);
+
+			SliderConstraintSettings settings;
+			settings.mAutoDetectPoint = true;
+			settings.SetSliderAxis(slider_axis);
+			settings.mLimitsMin = -5.0f;
+			settings.mLimitsMax = 10.0f;
+			mPhysicsSystem->AddConstraint(settings.Create(*prev, segment));
+
+			prev = &segment;
+		}
+
+		group_id++;
+	}
+
+	{
+		// Two light bodies attached to a heavy body (gives issues if the wrong anchor point is chosen)
+		Body *light1 = mBodyInterface->CreateBody(BodyCreationSettings(new BoxShape(Vec3::sReplicate(0.1f)), RVec3(-5.0f, 7.0f, -5.2f), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING));
+		light1->SetCollisionGroup(CollisionGroup(group_filter, group_id, 0));
+		mBodyInterface->AddBody(light1->GetID(), EActivation::Activate);
+		Body *heavy = mBodyInterface->CreateBody(BodyCreationSettings(new BoxShape(Vec3::sReplicate(5.0f)), RVec3(-5.0f, 7.0f, 0.0f), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING));
+		heavy->SetCollisionGroup(CollisionGroup(group_filter, group_id, 1));
+		mBodyInterface->AddBody(heavy->GetID(), EActivation::Activate);
+		Body *light2 = mBodyInterface->CreateBody(BodyCreationSettings(new BoxShape(Vec3::sReplicate(0.1f)), RVec3(-5.0f, 7.0f, 5.2f), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING));
+		light2->SetCollisionGroup(CollisionGroup(group_filter, group_id, 2));
+		mBodyInterface->AddBody(light2->GetID(), EActivation::Activate);
+		++group_id;
+
+		// Note: This violates the recommendation that body 1 is heavier than body 2, therefore this constraint will not work well (the rotation constraint will not be solved accurately)
+		SliderConstraintSettings slider1;
+		slider1.mAutoDetectPoint = true;
+		slider1.SetSliderAxis(Vec3::sAxisZ());
+		slider1.mLimitsMin = 0.0f;
+		slider1.mLimitsMax = 1.0f;
+		mPhysicsSystem->AddConstraint(slider1.Create(*light1, *heavy));
+
+		// This constraint has the heavy body as body 1 so will work fine
+		SliderConstraintSettings slider2;
+		slider2.mAutoDetectPoint = true;
+		slider2.SetSliderAxis(Vec3::sAxisZ());
+		slider2.mLimitsMin = 0.0f;
+		slider2.mLimitsMax = 1.0f;
+		mPhysicsSystem->AddConstraint(slider2.Create(*heavy, *light2));
+	}
+
+	{
+		// Two bodies vertically stacked with a slider constraint
+		Body *vert1 = mBodyInterface->CreateBody(BodyCreationSettings(new BoxShape(Vec3::sOne()), RVec3(5, 9, 0), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING));
+		vert1->SetCollisionGroup(CollisionGroup(group_filter, group_id, 0));
+		mBodyInterface->AddBody(vert1->GetID(), EActivation::Activate);
+		Body *vert2 = mBodyInterface->CreateBody(BodyCreationSettings(new BoxShape(Vec3::sOne()), RVec3(5, 3, 0), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING));
+		vert2->SetCollisionGroup(CollisionGroup(group_filter, group_id, 1));
+		mBodyInterface->AddBody(vert2->GetID(), EActivation::Activate);
+		++group_id;
+
+		SliderConstraintSettings slider;
+		slider.mAutoDetectPoint = true;
+		slider.SetSliderAxis(Vec3::sAxisY());
+		slider.mLimitsMin = 0.0f;
+		slider.mLimitsMax = 2.0f;
+		mPhysicsSystem->AddConstraint(slider.Create(*vert1, *vert2));
+	}
+
+	{
+		// Two bodies vertically stacked with a slider constraint using soft limits
+		Body *vert1 = mBodyInterface->CreateBody(BodyCreationSettings(new BoxShape(Vec3::sOne()), RVec3(10, 9, 0), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING));
+		vert1->SetCollisionGroup(CollisionGroup(group_filter, group_id, 0));
+		mBodyInterface->AddBody(vert1->GetID(), EActivation::Activate);
+		Body *vert2 = mBodyInterface->CreateBody(BodyCreationSettings(new BoxShape(Vec3::sOne()), RVec3(10, 3, 0), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING));
+		vert2->SetCollisionGroup(CollisionGroup(group_filter, group_id, 1));
+		mBodyInterface->AddBody(vert2->GetID(), EActivation::Activate);
+		++group_id;
+
+		SliderConstraintSettings slider;
+		slider.mAutoDetectPoint = true;
+		slider.SetSliderAxis(Vec3::sAxisY());
+		slider.mLimitsMin = 0.0f;
+		slider.mLimitsMax = 2.0f;
+		slider.mLimitsSpringSettings.mFrequency = 1.0f;
+		slider.mLimitsSpringSettings.mDamping = 0.5f;
+		mPhysicsSystem->AddConstraint(slider.Create(*vert1, *vert2));
+	}
+}

lib/All/JoltPhysics/Samples/Tests/Constraints/SliderConstraintTest.h

@@ -0,0 +1,16 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/Test.h>
+
+class SliderConstraintTest : public Test
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, SliderConstraintTest)
+
+	// See: Test
+	virtual void		Initialize() override;
+};

lib/All/JoltPhysics/Samples/Tests/Constraints/SpringTest.cpp

@@ -0,0 +1,94 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/Constraints/SpringTest.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Constraints/DistanceConstraint.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Layers.h>
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(SpringTest)
+{
+	JPH_ADD_BASE_CLASS(SpringTest, Test)
+}
+
+void SpringTest::Initialize()
+{
+	// Floor
+	CreateFloor();
+
+	// Top fixed body
+	RVec3 position(0, 75, 0);
+	Body &top = *mBodyInterface->CreateBody(BodyCreationSettings(new BoxShape(Vec3(100.0f, 1.0f, 1.0f)), position, Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING));
+	mBodyInterface->AddBody(top.GetID(), EActivation::DontActivate);
+
+	// Bodies attached with spring with different string lengths, same frequency and no damping
+	for (int i = 0; i < 10; ++i)
+	{
+		// Create body
+		RVec3 attachment_point = position + Vec3(-100.0f + i * 5.0f, 0, 0);
+		RVec3 body_position = attachment_point - Vec3(0, 10.0f + i * 2.5f, 0);
+		Body &body = *mBodyInterface->CreateBody(BodyCreationSettings(new BoxShape(Vec3::sReplicate(0.75f)), body_position, Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING));
+		body.GetMotionProperties()->SetAngularDamping(0.0f);
+		body.GetMotionProperties()->SetLinearDamping(0.0f);
+		mBodyInterface->AddBody(body.GetID(), EActivation::Activate);
+
+		// Attach spring
+		DistanceConstraintSettings settings;
+		settings.mPoint1 = attachment_point;
+		settings.mPoint2 = body_position;
+		settings.mLimitsSpringSettings.mFrequency = 0.33f;
+		mPhysicsSystem->AddConstraint(settings.Create(top, body));
+
+		// Move the body up so that it can start oscillating
+		mBodyInterface->SetPositionAndRotation(body.GetID(), attachment_point - Vec3(0, 5, 0), Quat::sIdentity(), EActivation::DontActivate);
+	}
+
+	// Bodies attached with spring with different frequency and no damping
+	for (int i = 0; i < 10; ++i)
+	{
+		// Create body
+		RVec3 attachment_point = position + Vec3(-25.0f + i * 5.0f, 0, 0);
+		RVec3 body_position = attachment_point - Vec3(0, 25.0f, 0);
+		Body &body = *mBodyInterface->CreateBody(BodyCreationSettings(new BoxShape(Vec3::sReplicate(0.75f)), body_position, Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING));
+		body.GetMotionProperties()->SetAngularDamping(0.0f);
+		body.GetMotionProperties()->SetLinearDamping(0.0f);
+		mBodyInterface->AddBody(body.GetID(), EActivation::Activate);
+
+		// Attach spring
+		DistanceConstraintSettings settings;
+		settings.mPoint1 = attachment_point;
+		settings.mPoint2 = body_position;
+		settings.mLimitsSpringSettings.mFrequency = 0.1f + 0.1f * i;
+		mPhysicsSystem->AddConstraint(settings.Create(top, body));
+
+		// Move the body up so that it can start oscillating
+		mBodyInterface->SetPositionAndRotation(body.GetID(), attachment_point - Vec3(0, 5, 0), Quat::sIdentity(), EActivation::DontActivate);
+	}
+
+	// Bodies attached with spring with same spring length, same frequency and different damping
+	for (int i = 0; i < 10; ++i)
+	{
+		// Create body
+		RVec3 attachment_point = position + Vec3(50.0f + i * 5.0f, 0, 0);
+		RVec3 body_position = attachment_point - Vec3(0, 25.0f, 0);
+		Body &body = *mBodyInterface->CreateBody(BodyCreationSettings(new BoxShape(Vec3::sReplicate(0.75f)), body_position, Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING));
+		body.GetMotionProperties()->SetAngularDamping(0.0f);
+		body.GetMotionProperties()->SetLinearDamping(0.0f);
+		mBodyInterface->AddBody(body.GetID(), EActivation::Activate);
+
+		// Attach spring
+		DistanceConstraintSettings settings;
+		settings.mPoint1 = attachment_point;
+		settings.mPoint2 = body_position;
+		settings.mLimitsSpringSettings.mFrequency = 0.33f;
+		settings.mLimitsSpringSettings.mDamping = (1.0f / 9.0f) * i;
+		mPhysicsSystem->AddConstraint(settings.Create(top, body));
+
+		// Move the body up so that it can start oscillating
+		mBodyInterface->SetPositionAndRotation(body.GetID(), attachment_point - Vec3(0, 5, 0), Quat::sIdentity(), EActivation::DontActivate);
+	}
+}

lib/All/JoltPhysics/Samples/Tests/Constraints/SpringTest.h

@@ -0,0 +1,16 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/Test.h>
+
+class SpringTest : public Test
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, SpringTest)
+
+	// See: Test
+	virtual void		Initialize() override;
+};

lib/All/JoltPhysics/Samples/Tests/Constraints/SwingTwistConstraintFrictionTest.cpp

@@ -0,0 +1,89 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/Constraints/SwingTwistConstraintFrictionTest.h>
+#include <Jolt/Physics/Collision/Shape/CapsuleShape.h>
+#include <Jolt/Physics/Collision/GroupFilterTable.h>
+#include <Jolt/Physics/Constraints/SwingTwistConstraint.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Layers.h>
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(SwingTwistConstraintFrictionTest)
+{
+	JPH_ADD_BASE_CLASS(SwingTwistConstraintFrictionTest, Test)
+}
+
+void SwingTwistConstraintFrictionTest::Initialize()
+{
+	// Floor
+	CreateFloor();
+
+	// Create group filter
+	Ref<GroupFilterTable> group_filter = new GroupFilterTable;
+
+	float half_cylinder_height = 1.5f;
+	RefConst<Shape> capsule = new CapsuleShape(half_cylinder_height, 0.5f);
+
+	RVec3 body1_position(0, 10, 0);
+	Body &body1 = *mBodyInterface->CreateBody(BodyCreationSettings(capsule, body1_position, Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING));
+	body1.SetCollisionGroup(CollisionGroup(group_filter, 0, 0));
+	mBodyInterface->AddBody(body1.GetID(), EActivation::DontActivate);
+
+	RVec3 body2_position = body1_position + Vec3(0, -2.0f * half_cylinder_height, 0);
+	Body &body2 = *mBodyInterface->CreateBody(BodyCreationSettings(capsule, body2_position, Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING));
+	body2.SetCollisionGroup(CollisionGroup(group_filter, 0, 0));
+	body2.GetMotionProperties()->SetLinearDamping(0.0f);
+	body2.GetMotionProperties()->SetAngularDamping(0.0f);
+	body2.SetAllowSleeping(false);
+	mBodyInterface->AddBody(body2.GetID(), EActivation::Activate);
+
+	SwingTwistConstraintSettings settings;
+	settings.mPosition1 = settings.mPosition2 = body1_position + Vec3(0, -half_cylinder_height, 0);
+	settings.mTwistAxis1 = settings.mTwistAxis2 = Vec3(0, -1, 0);
+	settings.mPlaneAxis1 = settings.mPlaneAxis2 = Vec3::sAxisX();
+	settings.mNormalHalfConeAngle = DegreesToRadians(90);
+	settings.mPlaneHalfConeAngle = DegreesToRadians(90);
+	settings.mTwistMinAngle = -JPH_PI;
+	settings.mTwistMaxAngle = JPH_PI;
+
+	float body2_inertia = (body2.GetMotionProperties()->GetLocalSpaceInverseInertia().Inversed3x3() * Vec3::sAxisY()).Length();
+	constexpr float max_angular_acceleration = DegreesToRadians(90.0f); // rad/s^2
+	settings.mMaxFrictionTorque = body2_inertia * max_angular_acceleration;
+
+	settings.mTwistMotorSettings = settings.mSwingMotorSettings = MotorSettings(10.0f, 2.0f);
+
+	mConstraint = static_cast<SwingTwistConstraint *>(settings.Create(body1, body2));
+	mPhysicsSystem->AddConstraint(mConstraint);
+}
+
+void SwingTwistConstraintFrictionTest::PrePhysicsUpdate(const PreUpdateParams &inParams)
+{
+	mTime += inParams.mDeltaTime;
+
+	bool pause = fmod(mTime, 5.0f) > 2.5f;
+
+	if (pause)
+	{
+		mConstraint->SetSwingMotorState(EMotorState::Off);
+		mConstraint->SetTwistMotorState(EMotorState::Off);
+	}
+	else
+	{
+		mConstraint->SetSwingMotorState(EMotorState::Velocity);
+		mConstraint->SetTwistMotorState(EMotorState::Velocity);
+		mConstraint->SetTargetAngularVelocityCS(Vec3(DegreesToRadians(180.0f), 0, 0));
+	}
+}
+
+void SwingTwistConstraintFrictionTest::SaveState(StateRecorder &inStream) const
+{
+	inStream.Write(mTime);
+}
+
+void SwingTwistConstraintFrictionTest::RestoreState(StateRecorder &inStream)
+{
+	inStream.Read(mTime);
+}

lib/All/JoltPhysics/Samples/Tests/Constraints/SwingTwistConstraintFrictionTest.h

@@ -0,0 +1,24 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/Test.h>
+#include <Jolt/Physics/Constraints/SwingTwistConstraint.h>
+
+class SwingTwistConstraintFrictionTest : public Test
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, SwingTwistConstraintFrictionTest)
+
+	// See: Test
+	virtual void			Initialize() override;
+	virtual void			PrePhysicsUpdate(const PreUpdateParams &inParams) override;
+	virtual void			SaveState(StateRecorder &inStream) const override;
+	virtual void			RestoreState(StateRecorder &inStream) override;
+
+private:
+	float					mTime = 0.0f;
+	SwingTwistConstraint *	mConstraint = nullptr;
+};

lib/All/JoltPhysics/Samples/Tests/Constraints/SwingTwistConstraintTest.cpp

@@ -0,0 +1,85 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/Constraints/SwingTwistConstraintTest.h>
+#include <Jolt/Physics/Collision/Shape/CapsuleShape.h>
+#include <Jolt/Physics/Collision/GroupFilterTable.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Application/DebugUI.h>
+#include <Layers.h>
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(SwingTwistConstraintTest)
+{
+	JPH_ADD_BASE_CLASS(SwingTwistConstraintTest, Test)
+}
+
+void SwingTwistConstraintTest::Initialize()
+{
+	// Floor
+	CreateFloor();
+
+	float half_cylinder_height = 1.5f;
+
+	const int cChainLength = 10;
+
+	// Build a collision group filter that disables collision between adjacent bodies
+	Ref<GroupFilterTable> group_filter = new GroupFilterTable(cChainLength);
+	for (CollisionGroup::SubGroupID i = 0; i < cChainLength - 1; ++i)
+		group_filter->DisableCollision(i, i + 1);
+
+	Body *prev = nullptr;
+	Quat rotation = Quat::sRotation(Vec3::sAxisZ(), 0.5f * JPH_PI);
+	RVec3 position(0, 25, 0);
+	for (int i = 0; i < cChainLength; ++i)
+	{
+		position += Vec3(2.0f * half_cylinder_height, 0, 0);
+
+		Body &segment = *mBodyInterface->CreateBody(BodyCreationSettings(new CapsuleShape(half_cylinder_height, 0.5f), position, Quat::sRotation(Vec3::sAxisX(), 0.25f * JPH_PI * i) * rotation, i == 0? EMotionType::Static : EMotionType::Dynamic, i == 0? Layers::NON_MOVING : Layers::MOVING));
+		segment.SetCollisionGroup(CollisionGroup(group_filter, 0, CollisionGroup::SubGroupID(i)));
+		mBodyInterface->AddBody(segment.GetID(), EActivation::Activate);
+		if (i != 0)
+			segment.SetAllowSleeping(false);
+
+		if (prev != nullptr)
+		{
+			Ref<SwingTwistConstraintSettings> settings = new SwingTwistConstraintSettings;
+			settings->mPosition1 = settings->mPosition2 = position + Vec3(-half_cylinder_height, 0, 0);
+			settings->mTwistAxis1 = settings->mTwistAxis2 = Vec3::sAxisX();
+			settings->mPlaneAxis1 = settings->mPlaneAxis2 = Vec3::sAxisY();
+			settings->mNormalHalfConeAngle = sNormalHalfConeAngle;
+			settings->mPlaneHalfConeAngle = sPlaneHalfConeAngle;
+			settings->mTwistMinAngle = sTwistMinAngle;
+			settings->mTwistMaxAngle = sTwistMaxAngle;
+
+			Ref<SwingTwistConstraint> constraint = static_cast<SwingTwistConstraint *>(settings->Create(*prev, segment));
+			mPhysicsSystem->AddConstraint(constraint);
+			mConstraints.push_back(constraint);
+		}
+
+		prev = &segment;
+	}
+}
+
+void SwingTwistConstraintTest::PrePhysicsUpdate(const PreUpdateParams &inParams)
+{
+	for (SwingTwistConstraint *c : mConstraints)
+	{
+		c->SetNormalHalfConeAngle(sNormalHalfConeAngle);
+		c->SetPlaneHalfConeAngle(sPlaneHalfConeAngle);
+		c->SetTwistMinAngle(sTwistMinAngle);
+		c->SetTwistMaxAngle(sTwistMaxAngle);
+	}
+}
+
+void SwingTwistConstraintTest::CreateSettingsMenu(DebugUI *inUI, UIElement *inSubMenu)
+{
+	inUI->CreateSlider(inSubMenu, "Min Twist Angle (deg)", RadiansToDegrees(sTwistMinAngle), -180.0f, 180.0f, 1.0f, [=](float inValue) { sTwistMinAngle = DegreesToRadians(inValue); });
+	inUI->CreateSlider(inSubMenu, "Max Twist Angle (deg)", RadiansToDegrees(sTwistMaxAngle), -180.0f, 180.0f, 1.0f, [=](float inValue) { sTwistMaxAngle = DegreesToRadians(inValue); });
+	inUI->CreateSlider(inSubMenu, "Normal Half Cone Angle (deg)", RadiansToDegrees(sNormalHalfConeAngle), 0.0f, 180.0f, 1.0f, [=](float inValue) { sNormalHalfConeAngle = DegreesToRadians(inValue); });
+	inUI->CreateSlider(inSubMenu, "Plane Half Cone Angle (deg)", RadiansToDegrees(sPlaneHalfConeAngle), 0.0f, 180.0f, 1.0f, [=](float inValue) { sPlaneHalfConeAngle = DegreesToRadians(inValue); });
+
+	inUI->ShowMenu(inSubMenu);
+}

lib/All/JoltPhysics/Samples/Tests/Constraints/SwingTwistConstraintTest.h

@@ -0,0 +1,29 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/Test.h>
+#include <Jolt/Physics/Constraints/SwingTwistConstraint.h>
+
+class SwingTwistConstraintTest : public Test
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, SwingTwistConstraintTest)
+
+	virtual void			Initialize() override;
+
+	virtual void			PrePhysicsUpdate(const PreUpdateParams &inParams) override;
+
+	virtual bool			HasSettingsMenu() const override							{ return true; }
+	virtual void			CreateSettingsMenu(DebugUI *inUI, UIElement *inSubMenu) override;
+
+private:
+	Array<Ref<SwingTwistConstraint>>	mConstraints;
+
+	inline static float		sNormalHalfConeAngle = DegreesToRadians(60);
+	inline static float		sPlaneHalfConeAngle = DegreesToRadians(20);
+	inline static float		sTwistMinAngle = DegreesToRadians(-10);
+	inline static float		sTwistMaxAngle = DegreesToRadians(20);
+};

lib/All/JoltPhysics/Samples/Tests/ConvexCollision/CapsuleVsBoxTest.cpp

@@ -0,0 +1,72 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/ConvexCollision/CapsuleVsBoxTest.h>
+#include <Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h>
+#include <Jolt/Physics/Collision/Shape/CapsuleShape.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Collision/CollideShape.h>
+#include <Jolt/Physics/Collision/CollisionDispatch.h>
+#include <Jolt/Physics/Collision/CollisionCollectorImpl.h>
+#include <Utils/DebugRendererSP.h>
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(CapsuleVsBoxTest)
+{
+	JPH_ADD_BASE_CLASS(CapsuleVsBoxTest, Test)
+}
+
+void CapsuleVsBoxTest::PrePhysicsUpdate(const PreUpdateParams &inParams)
+{
+	// 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;
+	Quat capsule_compound_rotation(0.499999970f, -0.499999970f, -0.499999970f, 0.499999970f);
+	Ref<RotatedTranslatedShapeSettings> capsule_settings = new RotatedTranslatedShapeSettings(Vec3(0, 0, 75), capsule_compound_rotation, 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);
+
+#ifdef JPH_DEBUG_RENDERER
+	// Draw the shapes
+	box_shape->Draw(mDebugRenderer, RMat44(box_transform), Vec3::sOne(), Color::sWhite, false, false);
+	capsule_shape->Draw(mDebugRenderer, RMat44(capsule_transform), Vec3::sOne(), Color::sWhite, false, false);
+#endif // JPH_DEBUG_RENDERER
+
+	// Draw contact points
+	const CollideShapeResult &hit = collector.mHits[0];
+	DrawMarkerSP(mDebugRenderer, hit.mContactPointOn1, Color::sRed, 1.0f);
+	DrawMarkerSP(mDebugRenderer, hit.mContactPointOn2, Color::sGreen, 1.0f);
+
+	// Draw penetration axis with length of the penetration
+	Vec3 pen_axis = hit.mPenetrationAxis;
+	float pen_axis_len = pen_axis.Length();
+	if (pen_axis_len > 0.0f)
+	{
+		pen_axis *= hit.mPenetrationDepth / pen_axis_len;
+		DrawArrowSP(mDebugRenderer, hit.mContactPointOn2, hit.mContactPointOn2 + pen_axis, Color::sYellow, 0.01f);
+
+#ifdef JPH_DEBUG_RENDERER
+		Mat44 resolved_box = box_transform.PostTranslated(pen_axis);
+		box_shape->Draw(mDebugRenderer, RMat44(resolved_box), Vec3::sOne(), Color::sGreen, false, false);
+#endif // JPH_DEBUG_RENDERER
+	}
+}

lib/All/JoltPhysics/Samples/Tests/ConvexCollision/CapsuleVsBoxTest.h

@@ -0,0 +1,17 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/Test.h>
+
+// Does a very long capsule vs rotated embedded box test, this was a repro for a bug and can be used to test bug regression
+class CapsuleVsBoxTest : public Test
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, CapsuleVsBoxTest)
+
+	// Update the test, called before the physics update
+	virtual void	PrePhysicsUpdate(const PreUpdateParams &inParams) override;
+};

lib/All/JoltPhysics/Samples/Tests/ConvexCollision/ClosestPointTest.cpp

@@ -0,0 +1,254 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/ConvexCollision/ClosestPointTest.h>
+#include <Jolt/Geometry/ClosestPoint.h>
+#include <Renderer/DebugRendererImp.h>
+#include <Utils/DebugRendererSP.h>
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(ClosestPointTest)
+{
+	JPH_ADD_BASE_CLASS(ClosestPointTest, Test)
+}
+
+void ClosestPointTest::PrePhysicsUpdate(const PreUpdateParams &inParams)
+{
+	Vec3 pos(inParams.mCameraState.mPos);
+
+	{
+		// Normal tetrahedron
+		Vec3 a(2, 0, 0);
+		Vec3 b(1, 0, 1);
+		Vec3 c(2, 0, 1);
+		Vec3 d(1, 1, 0);
+
+		TestTetra(pos, a, b, c, d);
+	}
+
+	{
+		// Inside out tetrahedron
+		Vec3 a(2, -2, 0);
+		Vec3 b(1, -2, 1);
+		Vec3 c(2, -2, 1);
+		Vec3 d(1, -3, 0);
+
+		TestTetra(pos, a, b, c, d);
+	}
+
+	{
+		// Degenerate tetrahedron
+		Vec3 a(2, 3, 0);
+		Vec3 b = a;
+		Vec3 c(2, 3, 1);
+		Vec3 d(1, 4, 0);
+
+		TestTetra(pos, a, b, c, d);
+	}
+
+	{
+		// Degenerate tetrahedron
+		Vec3 a(2, 6, 0);
+		Vec3 b(1, 6, 1);
+		Vec3 c = a;
+		Vec3 d(1, 7, 0);
+
+		TestTetra(pos, a, b, c, d);
+	}
+
+	{
+		// Degenerate tetrahedron
+		Vec3 a(2, 9, 0);
+		Vec3 b(1, 9, 1);
+		Vec3 c(2, 9, 1);
+		Vec3 d = a;
+
+		TestTetra(pos, a, b, c, d);
+	}
+
+	{
+		// Degenerate tetrahedron
+		Vec3 a(2, 12, 0);
+		Vec3 b(1, 12, 1);
+		Vec3 c = b;
+		Vec3 d(1, 13, 0);
+
+		TestTetra(pos, a, b, c, d);
+	}
+
+	{
+		// Degenerate tetrahedron
+		Vec3 a(2, 15, 0);
+		Vec3 b(1, 15, 1);
+		Vec3 c(2, 15, 1);
+		Vec3 d = b;
+
+		TestTetra(pos, a, b, c, d);
+	}
+
+	{
+		// Degenerate tetrahedron
+		Vec3 a(2, 18, 0);
+		Vec3 b(1, 18, 1);
+		Vec3 c(2, 18, 1);
+		Vec3 d = c;
+
+		TestTetra(pos, a, b, c, d);
+	}
+
+	{
+		// Normal tri
+		Vec3 a(5, 0, 0);
+		Vec3 b(4, 0, 1);
+		Vec3 c(5, 0, 1);
+
+		TestTri(pos, a, b, c);
+	}
+
+	{
+		// Degenerate tri
+		Vec3 a(5, 3, 0);
+		Vec3 b = a;
+		Vec3 c(5, 3, 1);
+
+		TestTri(pos, a, b, c);
+	}
+
+	{
+		// Degenerate tri
+		Vec3 a(5, 6, 0);
+		Vec3 b(4, 6, 1);
+		Vec3 c = a;
+
+		TestTri(pos, a, b, c);
+	}
+
+	{
+		// Degenerate tri
+		Vec3 a(5, 9, 0);
+		Vec3 b(4, 9, 1);
+		Vec3 c = b;
+
+		TestTri(pos, a, b, c);
+	}
+
+	{
+		// Normal line
+		Vec3 a(10, 0, 0);
+		Vec3 b(9, 0, 1);
+		TestLine(pos, a, b);
+	}
+
+	{
+		// Degenerate line
+		Vec3 a(10, 3, 0);
+		Vec3 b = a;
+		TestLine(pos, a, b);
+	}
+}
+
+void ClosestPointTest::TestLine(Vec3Arg inPosition, Vec3Arg inA, Vec3Arg inB)
+{
+	Vec3 a = inA - inPosition;
+	Vec3 b = inB - inPosition;
+
+	uint32 set;
+	Vec3 closest = ClosestPoint::GetClosestPointOnLine(a, b, set) + inPosition;
+
+	DrawLineSP(mDebugRenderer, inA, inB, Color::sWhite);
+
+	DrawMarkerSP(mDebugRenderer, closest, Color::sRed, 0.1f);
+
+	if (set & 0b0001)
+		DrawMarkerSP(mDebugRenderer, inA, Color::sYellow, 0.5f);
+	if (set & 0b0010)
+		DrawMarkerSP(mDebugRenderer, inB, Color::sYellow, 0.5f);
+
+	Vec3 a2 = inA - closest;
+	Vec3 b2 = inB - closest;
+
+	float u, v;
+	ClosestPoint::GetBaryCentricCoordinates(a2, b2, u, v);
+	DrawWireSphereSP(mDebugRenderer, u * inA + v * inB, 0.05f, Color::sGreen);
+
+	DrawText3DSP(mDebugRenderer, inA, "a");
+	DrawText3DSP(mDebugRenderer, inB, "b");
+}
+
+void ClosestPointTest::TestTri(Vec3Arg inPosition, Vec3Arg inA, Vec3Arg inB, Vec3Arg inC)
+{
+	Vec3 a = inA - inPosition;
+	Vec3 b = inB - inPosition;
+	Vec3 c = inC - inPosition;
+
+	uint32 set;
+	Vec3 closest = ClosestPoint::GetClosestPointOnTriangle(a, b, c, set) + inPosition;
+
+	DrawLineSP(mDebugRenderer, inA, inB, Color::sWhite);
+	DrawLineSP(mDebugRenderer, inA, inC, Color::sWhite);
+	DrawLineSP(mDebugRenderer, inB, inC, Color::sWhite);
+
+	DrawTriangleSP(mDebugRenderer, inA, inB, inC, Color::sGrey);
+
+	DrawMarkerSP(mDebugRenderer, closest, Color::sRed, 0.1f);
+
+	if (set & 0b0001)
+		DrawMarkerSP(mDebugRenderer, inA, Color::sYellow, 0.5f);
+	if (set & 0b0010)
+		DrawMarkerSP(mDebugRenderer, inB, Color::sYellow, 0.5f);
+	if (set & 0b0100)
+		DrawMarkerSP(mDebugRenderer, inC, Color::sYellow, 0.5f);
+
+	Vec3 a2 = inA - closest;
+	Vec3 b2 = inB - closest;
+	Vec3 c2 = inC - closest;
+
+	float u, v, w;
+	ClosestPoint::GetBaryCentricCoordinates(a2, b2, c2, u, v, w);
+	DrawWireSphereSP(mDebugRenderer, u * inA + v * inB + w * inC, 0.05f, Color::sGreen);
+
+	DrawText3DSP(mDebugRenderer, inA, "a");
+	DrawText3DSP(mDebugRenderer, inB, "b");
+	DrawText3DSP(mDebugRenderer, inC, "c");
+}
+
+void ClosestPointTest::TestTetra(Vec3Arg inPosition, Vec3Arg inA, Vec3Arg inB, Vec3Arg inC, Vec3Arg inD)
+{
+	Vec3 a = inA - inPosition;
+	Vec3 b = inB - inPosition;
+	Vec3 c = inC - inPosition;
+	Vec3 d = inD - inPosition;
+
+	uint32 set;
+	Vec3 closest = ClosestPoint::GetClosestPointOnTetrahedron(a, b, c, d, set) + inPosition;
+
+	DrawLineSP(mDebugRenderer, inA, inB, Color::sWhite);
+	DrawLineSP(mDebugRenderer, inA, inC, Color::sWhite);
+	DrawLineSP(mDebugRenderer, inA, inD, Color::sWhite);
+	DrawLineSP(mDebugRenderer, inB, inC, Color::sWhite);
+	DrawLineSP(mDebugRenderer, inB, inD, Color::sWhite);
+	DrawLineSP(mDebugRenderer, inC, inD, Color::sWhite);
+
+	DrawTriangleSP(mDebugRenderer, inA, inC, inB, Color::sGrey);
+	DrawTriangleSP(mDebugRenderer, inA, inD, inC, Color::sGrey);
+	DrawTriangleSP(mDebugRenderer, inA, inB, inD, Color::sGrey);
+	DrawTriangleSP(mDebugRenderer, inB, inC, inD, Color::sGrey);
+
+	DrawMarkerSP(mDebugRenderer, closest, Color::sRed, 0.1f);
+
+	if (set & 0b0001)
+		DrawMarkerSP(mDebugRenderer, inA, Color::sYellow, 0.5f);
+	if (set & 0b0010)
+		DrawMarkerSP(mDebugRenderer, inB, Color::sYellow, 0.5f);
+	if (set & 0b0100)
+		DrawMarkerSP(mDebugRenderer, inC, Color::sYellow, 0.5f);
+	if (set & 0b1000)
+		DrawMarkerSP(mDebugRenderer, inD, Color::sYellow, 0.5f);
+
+	DrawText3DSP(mDebugRenderer, inA, "a");
+	DrawText3DSP(mDebugRenderer, inB, "b");
+	DrawText3DSP(mDebugRenderer, inC, "c");
+	DrawText3DSP(mDebugRenderer, inD, "d");
+}

lib/All/JoltPhysics/Samples/Tests/ConvexCollision/ClosestPointTest.h

@@ -0,0 +1,22 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/Test.h>
+
+// Test that interactively shows the algorithms from the ClosestPoints namespace
+class ClosestPointTest : public Test
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, ClosestPointTest)
+
+	// Update the test, called before the physics update
+	virtual void	PrePhysicsUpdate(const PreUpdateParams &inParams) override;
+
+private:
+	void			TestLine(Vec3Arg inPosition, Vec3Arg inA, Vec3Arg inB);
+	void			TestTri(Vec3Arg inPosition, Vec3Arg inA, Vec3Arg inB, Vec3Arg inC);
+	void			TestTetra(Vec3Arg inPosition, Vec3Arg inA, Vec3Arg inB, Vec3Arg inC, Vec3Arg inD);
+};

lib/All/JoltPhysics/Samples/Tests/ConvexCollision/ConvexHullShrinkTest.cpp

@@ -0,0 +1,176 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/ConvexCollision/ConvexHullShrinkTest.h>
+#include <Utils/Log.h>
+#include <Utils/DebugRendererSP.h>
+#include <Jolt/Geometry/ConvexSupport.h>
+#include <Jolt/Physics/Collision/Shape/ConvexHullShape.h>
+#include <Renderer/DebugRendererImp.h>
+#include <Utils/AssetStream.h>
+
+JPH_SUPPRESS_WARNINGS_STD_BEGIN
+#include <fstream>
+JPH_SUPPRESS_WARNINGS_STD_END
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(ConvexHullShrinkTest)
+{
+	JPH_ADD_BASE_CLASS(ConvexHullShrinkTest, Test)
+}
+
+void ConvexHullShrinkTest::Initialize()
+{
+	// First add a list of shapes that were problematic before
+	mPoints = {
+		{
+			Vec3(1, 1, 1),
+			Vec3(1, 1, -1),
+			Vec3(1, -1, 1),
+			Vec3(1, -1, -1),
+		},
+		{
+			Vec3(1, 1, 1),
+			Vec3(1, 1, -1),
+			Vec3(1, -1, 1),
+			Vec3(1, -1, -1),
+			Vec3(-1, 1, 1),
+			Vec3(-1, 1, -1),
+			Vec3(-1, -1, 1),
+			Vec3(-1, -1, -1),
+		},
+		{
+			Vec3(0.24055352f, 0.42262089f, 0.20811508f),
+			Vec3(0.23034751f, 0.42984104f, -0.21389426f),
+			Vec3(0.21995061f, 0.43724900f, 0.20929135f),
+			Vec3(0.18619442f, 0.44122630f, 0.10257969f),
+			Vec3(-0.22997921f, 0.43706810f, 0.21128670f),
+			Vec3(0.18488347f, -0.44135576f, 0.10415942f),
+			Vec3(-0.20950880f, -0.43603044f, 0.20873074f),
+			Vec3(-0.21230474f, -0.43691945f, -0.20506332f),
+			Vec3(0.23440370f, -0.43392032f, 0.20985059f),
+			Vec3(0.22406587f, -0.43578571f, -0.21132792f),
+			Vec3(0.24845430f, -0.41821426f, -0.21033705f),
+			Vec3(0.24780219f, -0.42262548f, 0.21058462f),
+			Vec3(-0.24866026f, 0.41188520f, 0.20908103f),
+			Vec3(-0.25144735f, 0.41933101f, -0.20718251f),
+			Vec3(-0.24799588f, -0.20490804f, 0.21178717f),
+			Vec3(0.01075744f, -0.41775572f, -0.22181017f),
+			Vec3(-0.18624404f, -0.18736419f, -0.21975047f),
+			Vec3(0.22080457f, 0.01773871f, -0.22080121f),
+			Vec3(-0.17988407f, 0.40095943f, -0.21670545f),
+			Vec3(-0.23094913f, 0.42154532f, 0.21846796f),
+			Vec3(0.23783659f, 0.41114848f, -0.20812420f),
+			Vec3(0.25242796f, 0.00087111f, 0.04875314f),
+			Vec3(0.20976084f, 0.43694448f, -0.20819492f),
+			Vec3(0.21914389f, -0.42215359f, -0.21839635f),
+			Vec3(0.22120973f, 0.42172050f, 0.21581716f),
+			Vec3(0.07287904f, 0.40937370f, 0.21898652f),
+			Vec3(-0.23638439f, 0.42299985f, -0.21391643f),
+			Vec3(0.25210538f, -0.20603905f, 0.20603551f),
+			Vec3(-0.22867783f, -0.43080616f, -0.21309699f),
+			Vec3(-0.22365719f, 0.43650645f, -0.20515810f),
+			Vec3(-0.23701435f, 0.43320888f, -0.20985882f),
+			Vec3(-0.24509817f, 0.42541492f, 0.21352110f),
+			Vec3(0.22803798f, -0.41877448f, 0.21590335f),
+			Vec3(-0.21627685f, -0.41884291f, 0.21908275f),
+			Vec3(-0.24125161f, -0.13299965f, -0.21386964f),
+			Vec3(-0.22310710f, -0.43280768f, 0.21368177f),
+			Vec3(-0.23707944f, -0.41916745f, 0.21170078f),
+			Vec3(-0.23729360f, -0.42400050f, -0.20905880f),
+			Vec3(-0.23056241f, 0.44033193f, -0.00191451f),
+			Vec3(-0.24118152f, -0.41101628f, -0.20855166f),
+			Vec3(0.21646300f, 0.42087674f, -0.21763385f),
+			Vec3(0.25090047f, -0.41023433f, 0.10248772f),
+			Vec3(0.03950108f, -0.43627834f, -0.21231101f),
+			Vec3(-0.22727611f, -0.24993966f, 0.21899925f),
+			Vec3(0.24388977f, -0.07015021f, -0.21204789f)
+		}
+	};
+
+	// Open the external file with hulls
+	// A stream containing predefined convex hulls
+	AssetStream points_asset_stream("convex_hulls.bin", std::ios::in | std::ios::binary);
+	std::istream &points_stream = points_asset_stream.Get();
+	for (;;)
+	{
+		// Read the length of the next point cloud
+		uint32 len = 0;
+		points_stream.read((char *)&len, sizeof(len));
+		if (points_stream.eof())
+			break;
+
+		// Read the points
+		if (len > 0)
+		{
+			Points p;
+			for (uint32 i = 0; i < len; ++i)
+			{
+				Float3 v;
+				points_stream.read((char *)&v, sizeof(v));
+				p.push_back(Vec3(v));
+			}
+			mPoints.push_back(std::move(p));
+		}
+	}
+}
+
+void ConvexHullShrinkTest::PrePhysicsUpdate(const PreUpdateParams &inParams)
+{
+	// Take one of the predefined shapes
+	const Points &points = mIteration < mPoints.size()? mPoints[mIteration] : mPoints.back();
+	mIteration++;
+
+	// Create shape
+	ConvexHullShapeSettings settings(points, cDefaultConvexRadius);
+	Shape::ShapeResult result = settings.Create();
+	if (!result.IsValid())
+	{
+		Trace("%d: %s", mIteration - 1, result.GetError().c_str());
+		return;
+	}
+	RefConst<ConvexHullShape> shape = StaticCast<ConvexHullShape>(result.Get());
+
+	// Shape creation may have reduced the convex radius, fetch the result
+	const float convex_radius = shape->GetConvexRadius();
+	if (convex_radius > 0.0f)
+	{
+		// Get the support function of the shape excluding convex radius and add the convex radius
+		ConvexShape::SupportBuffer buffer;
+		const ConvexShape::Support *support = shape->GetSupportFunction(ConvexShape::ESupportMode::ExcludeConvexRadius, buffer, Vec3::sOne());
+		AddConvexRadius add_cvx(*support, convex_radius);
+
+		// Calculate the error w.r.t. the original hull
+		float max_error = -FLT_MAX;
+		int max_error_plane = 0;
+		Vec3 max_error_support_point = Vec3::sZero();
+		const Array<Plane> &planes = shape->GetPlanes();
+		for (int i = 0; i < (int)planes.size(); ++i)
+		{
+			const Plane &plane = planes[i];
+			Vec3 support_point = add_cvx.GetSupport(plane.GetNormal());
+			float distance = plane.SignedDistance(support_point);
+			if (distance > max_error)
+			{
+				max_error = distance;
+				max_error_support_point = support_point;
+				max_error_plane = i;
+			}
+		}
+		if (max_error > settings.mMaxErrorConvexRadius)
+		{
+			Trace("%d, %f, %f", mIteration - 1, (double)convex_radius, (double)max_error);
+			DrawMarkerSP(mDebugRenderer, max_error_support_point, Color::sPurple, 0.1f);
+			DrawArrowSP(mDebugRenderer, max_error_support_point, max_error_support_point - max_error * planes[max_error_plane].GetNormal(), Color::sPurple, 0.01f);
+		}
+	}
+
+#ifdef JPH_DEBUG_RENDERER
+	// Draw the hulls
+	shape->Draw(DebugRenderer::sInstance, RMat44::sIdentity(), Vec3::sOne(), Color::sRed, false, false);
+	shape->DrawGetSupportFunction(DebugRenderer::sInstance, RMat44::sIdentity(), Vec3::sOne(), Color::sLightGrey, false);
+	shape->DrawShrunkShape(DebugRenderer::sInstance, RMat44::sIdentity(), Vec3::sOne());
+#endif // JPH_DEBUG_RENDERER
+}

lib/All/JoltPhysics/Samples/Tests/ConvexCollision/ConvexHullShrinkTest.h

@@ -0,0 +1,31 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/Test.h>
+
+// Create a convex hull, shrink it with the convex radius and expand it again to check the error
+class ConvexHullShrinkTest : public Test
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, ConvexHullShrinkTest)
+
+	// Initialize the test
+	virtual void			Initialize() override;
+
+	// Update the test, called before the physics update
+	virtual void			PrePhysicsUpdate(const PreUpdateParams &inParams) override;
+
+	// Number used to scale the terrain and camera movement to the scene
+	virtual float			GetWorldScale() const override								{ return 0.2f; }
+
+private:
+	// A list of predefined points to feed the convex hull algorithm
+	using Points = Array<Vec3>;
+	Array<Points>			mPoints;
+
+	// Which index in the list we're currently using
+	size_t					mIteration = 0;
+};

lib/All/JoltPhysics/Samples/Tests/ConvexCollision/ConvexHullTest.cpp

@@ -0,0 +1,628 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/ConvexCollision/ConvexHullTest.h>
+#include <Jolt/Geometry/ConvexHullBuilder.h>
+#include <Utils/Log.h>
+#include <Utils/AssetStream.h>
+#include <Utils/DebugRendererSP.h>
+
+JPH_SUPPRESS_WARNINGS_STD_BEGIN
+#include <fstream>
+JPH_SUPPRESS_WARNINGS_STD_END
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(ConvexHullTest)
+{
+	JPH_ADD_BASE_CLASS(ConvexHullTest, Test)
+}
+
+void ConvexHullTest::Initialize()
+{
+	// First add a list of shapes that were problematic before
+	mPoints = {
+		{
+			Vec3(-1, 0, -1),
+			Vec3(1, 0, -1),
+			Vec3(-1, 0, 1),
+			Vec3(1, 0, 1)
+		},
+		{
+			Vec3(-1, 0, -1),
+			Vec3(1, 0, -1),
+			Vec3(-1, 0, 1),
+			Vec3(-0.5f, 0, -0.5f)
+		},
+		{
+			Vec3(-1, 0, -1),
+			Vec3(1, 0, -1),
+			Vec3(-1, 0, 1),
+			Vec3(1, 0, 1),
+			Vec3(0, 1, 0)
+		},
+		{
+			Vec3(1.25793016f, 0.157113776f, 1.22066617f),
+			Vec3(1.92657053f, 0.157114446f, 0.240761176f),
+			Vec3(1.40259242f, 0.157115221f, -0.834863901f),
+			Vec3(1.94086421f, 0.157113507f, -0.790734947f),
+			Vec3(2.20533752f, 0.157113209f, -0.281754375f),
+			Vec3(0.0426187329f, 0.157113969f, -1.40533638f),
+			Vec3(1.11055744f, 0.157113969f, -1.33626819f),
+			Vec3(0.180490851f, 0.157114655f, 1.16420007f),
+			Vec3(-1.34696794f, 0.157110974f, -0.978962243f),
+			Vec3(-0.981223822f, 0.157110706f, -1.44589376f),
+			Vec3(-1.8200444f, 0.157106474f, 1.05036092f),
+			Vec3(-0.376947045f, 0.15711388f, 1.13544536f),
+			Vec3(-1.37966835f, 0.157109678f, 1.08289516f),
+			Vec3(-1.04599845f, 0.157108605f, 1.54891157f),
+			Vec3(-0.597127378f, 0.157110557f, 1.57243586f),
+			Vec3(-2.09407234f, 0.157106325f, 0.560136259f),
+			Vec3(-1.91857386f, 0.157108605f, 0.0392456949f),
+			Vec3(-2.08503342f, 0.157106936f, -0.506603181f),
+			Vec3(-1.80278254f, 0.157107696f, -0.986931145f),
+			Vec3(0.434835076f, 0.157112151f, 1.62568307f),
+			Vec3(0.917346299f, 0.157111734f, 1.65097046f),
+			Vec3(1.77710009f, 0.157112047f, 1.2388792f),
+			Vec3(2.11432409f, 0.157112464f, 0.780689001f),
+		},
+		{
+			Vec3(1.32055235f, -0.0982032791f, 0.020047307f),
+			Vec3(-0.0175848603f, -0.104957283f, 0.020047307f),
+			Vec3(-0.0175848603f, 0.098285675f, 0.020047307f),
+			Vec3(1.32055235f, 0.098285675f, 0.020047307f),
+			Vec3(1.00427914f, -0.0982032791f, 0.868395209f),
+			Vec3(1.32055235f, -0.0982032791f, 2.63605499f),
+			Vec3(1.00427914f, -0.0982032791f, 1.95698023f),
+			Vec3(1.00427914f, -0.104957283f, 0.511006474f),
+			Vec3(0.00150847435f, -0.104957283f, 0.511006474f),
+			Vec3(0.271511227f, -0.179470509f, 0.868395209f),
+			Vec3(0.00150847435f, -0.179470509f, 0.868395209f),
+			Vec3(0.00150847435f, -0.179470509f, 0.511006474f),
+			Vec3(0.271511227f, -0.179470509f, 0.511006474f),
+			Vec3(1.00427914f, -0.145700991f, 1.95698023f),
+			Vec3(1.00427914f, -0.145700991f, 2.40789247f),
+			Vec3(0.271511227f, -0.179470509f, 2.40789247f),
+			Vec3(0.271511227f, -0.179470509f, 1.95698023f),
+			Vec3(0.00150847435f, -0.104957283f, 2.40789247f),
+			Vec3(1.00427914f, -0.104957283f, 2.40789247f),
+			Vec3(-0.0175848603f, -0.104957283f, 2.63605499f),
+			Vec3(1.32055235f, 0.098285675f, 2.63605499f),
+			Vec3(-0.0175848603f, 0.098285675f, 2.63605499f),
+			Vec3(-0.0175848603f, -0.0929760709f, 1.31891572f),
+			Vec3(-0.0175848603f, 0.0915316716f, 1.31891572f),
+			Vec3(1.00427914f, -0.145700991f, 0.868395209f),
+			Vec3(1.00427914f, -0.145700991f, 0.511006474f),
+			Vec3(0.00150847435f, -0.104957283f, 0.868395209f),
+			Vec3(0.00150847435f, -0.104957283f, 1.95698023f),
+			Vec3(0.00150847435f, -0.179470509f, 1.95698023f),
+			Vec3(0.00150847435f, -0.179470509f, 2.40789247f),
+			Vec3(-0.0175848603f, -0.100129686f, 0.959797204f),
+			Vec3(0.0878298879f, 0.139223307f, 1.04704332f),
+			Vec3(0.122709334f, -0.147821367f, 1.15395057f),
+			Vec3(0.122709334f, 0.139223307f, 1.15395057f),
+			Vec3(0.19671753f, -0.118080139f, 1.15425301f),
+			Vec3(0.0986568928f, -0.147821367f, 1.22612f),
+			Vec3(0.175069571f, -0.118080139f, 1.2711879f),
+			Vec3(-0.0175848603f, -0.147821367f, 0.959797204f),
+			Vec3(0.0767889619f, -0.118080139f, 0.947003484f),
+			Vec3(0.0878298879f, -0.147821367f, 1.04704332f),
+			Vec3(0.18563965f, -0.118080139f, 1.03236175f),
+			Vec3(-0.0175848603f, 0.098285675f, 0.959797204f),
+			Vec3(0.0986568928f, 0.139223307f, 1.22612f),
+			Vec3(0.0897113085f, -0.104957283f, 1.32667887f),
+			Vec3(-0.0175848603f, -0.147821367f, 1.31891572f),
+			Vec3(0.0897113085f, -0.118080139f, 1.32667887f),
+			Vec3(0.175069571f, -0.104957283f, 1.2711879f),
+			Vec3(0.18563965f, -0.104957283f, 1.03236175f),
+			Vec3(0.19671753f, -0.104957283f, 1.15425301f),
+			Vec3(0.0767889619f, -0.104957283f, 0.947003484f),
+			Vec3(1.00427914f, 0.098285675f, 0.868395209f),
+			Vec3(1.00427914f, 0.098285675f, 1.95698023f),
+			Vec3(1.00427914f, 0.098285675f, 0.511006474f),
+			Vec3(0.00150847435f, 0.098285675f, 0.511006474f),
+			Vec3(0.00150847435f, 0.17087248f, 0.511006474f),
+			Vec3(0.00150847435f, 0.17087248f, 0.868395209f),
+			Vec3(0.271511227f, 0.17087248f, 0.868395209f),
+			Vec3(0.271511227f, 0.17087248f, 0.511006474f),
+			Vec3(0.271511227f, 0.17087248f, 2.40789247f),
+			Vec3(1.00427914f, 0.137102962f, 2.40789247f),
+			Vec3(1.00427914f, 0.137102962f, 1.95698023f),
+			Vec3(0.271511227f, 0.17087248f, 1.95698023f),
+			Vec3(0.00150847435f, 0.098285675f, 2.40789247f),
+			Vec3(1.00427914f, 0.098285675f, 2.40789247f),
+			Vec3(1.00427914f, 0.137102962f, 0.868395209f),
+			Vec3(1.00427914f, 0.137102962f, 0.511006474f),
+			Vec3(0.00150847435f, 0.098285675f, 0.868395209f),
+			Vec3(0.00150847435f, 0.098285675f, 1.95698023f),
+			Vec3(0.00150847435f, 0.17087248f, 1.95698023f),
+			Vec3(0.00150847435f, 0.17087248f, 2.40789247f),
+			Vec3(0.19671753f, 0.109482117f, 1.15425301f),
+			Vec3(0.175069571f, 0.109482117f, 1.2711879f),
+			Vec3(-0.0175848603f, 0.139223307f, 0.959797204f),
+			Vec3(0.0767889619f, 0.109482117f, 0.947003484f),
+			Vec3(0.18563965f, 0.109482117f, 1.03236175f),
+			Vec3(0.0897113085f, 0.098285675f, 1.32667887f),
+			Vec3(-0.0175848603f, 0.139223307f, 1.31891572f),
+			Vec3(0.0897113085f, 0.109482117f, 1.32667887f),
+			Vec3(0.175069571f, 0.098285675f, 1.2711879f),
+			Vec3(0.19671753f, 0.098285675f, 1.15425301f),
+			Vec3(0.18563965f, 0.098285675f, 1.03236175f),
+			Vec3(0.0767889619f, 0.098285675f, 0.947003484f)
+		},
+		{
+			Vec3(0.0212580804f, 1.29376173f, 0.0102035152f),
+			Vec3(0.0225791596f, 1.05854928f, 0.0887729526f),
+			Vec3(0.0596007220f, 0.984267414f, 0.0408750288f),
+			Vec3(0.0722020790f, 0.980246127f, -0.0416274220f),
+			Vec3(-0.00376634207f, -0.718282819f, 0.00411359267f),
+			Vec3(-0.00188124576f, -0.718283117f, 0.00229378697f),
+			Vec3(-0.00162511703f, -0.718282461f, 0.00753012672f),
+			Vec3(-0.00118427153f, 1.36079276f, 0.00107491738f),
+			Vec3(-6.78644137e-05f, -0.718282998f, 0.00426622201f),
+			Vec3(0.00102991192f, 1.29927433f, 0.0230795704f),
+			Vec3(0.00699944887f, 1.05855191f, 0.0887731761f),
+			Vec3(-0.00603519706f, 1.04913890f, -0.102404378f),
+			Vec3(-0.0212373994f, 1.31092644f, 0.00530112581f),
+			Vec3(-0.0542707182f, 1.07623804f, 0.0403260253f),
+			Vec3(-0.0946691483f, 1.07357991f, -0.0185115524f),
+			Vec3(-0.0946691483f, 1.07357991f, -0.0185115524f)
+		},
+		{
+			Vec3(0.0283679180f, 0.0443800166f, -0.00569444988f),
+			Vec3(0.0327114500f, -0.0221119970f, 0.0232404359f),
+			Vec3(0.0374971032f, 0.0148781445f, -0.0245264377f),
+			Vec3(0.0439460576f, 0.0126368264f, 0.0197663195f),
+			Vec3(-0.0327170566f, 0.0423904508f, 0.0181609988f),
+			Vec3(-0.0306955911f, 0.0311534479f, -0.0281516202f),
+			Vec3(-0.0262422040f, 0.0248970203f, 0.0450032614f),
+			Vec3(-0.0262093470f, 0.00906597450f, 0.0481815264f),
+			Vec3(-0.0256845430f, -0.00607067533f, -0.0401362479f),
+			Vec3(-0.0179684199f, 0.0266145933f, -0.0394567028f),
+			Vec3(-0.00567848794f, -0.0313231349f, -0.0263656937f),
+			Vec3(-0.00444967486f, -0.0383231938f, 0.0206601117f),
+			Vec3(-0.00329093798f, 0.0464436933f, 0.0343827978f),
+			Vec3(-0.00225042878f, 0.0550651476f, -0.00304153794f),
+			Vec3(0.00310287252f, 0.00219658483f, 0.0542362332f),
+			Vec3(0.00435558241f, 0.00644031307f, -0.0455060303f),
+			Vec3(0.00495047215f, -0.0144955292f, 0.0482611060f),
+			Vec3(0.00510909408f, 0.0300753452f, -0.0415933356f),
+			Vec3(0.00619197031f, 0.0269140154f, 0.0500008501f),
+			Vec3(0.0190936550f, -0.0106478147f, 0.0453430638f),
+			Vec3(0.0202461667f, 0.00821140409f, 0.0500608832f),
+			Vec3(0.0199985132f, 0.0353404805f, 0.0413853638f),
+			Vec3(0.0267947838f, -0.0155944452f, -0.0300960485f),
+			Vec3(0.0274163429f, 0.0318853259f, -0.0288569275f),
+			Vec3(-0.0404368788f, -0.0213200711f, -0.00530833099f),
+			Vec3(-0.0383560173f, -0.0111571737f, 0.0346816145f),
+			Vec3(-0.0453024730f, 0.00178011740f, -0.0218658112f),
+			Vec3(-0.0482929349f, 0.0101582557f, 0.0191618335f)
+		},
+		{
+			Vec3(0.19555497f, 0.06892325f, 0.21078214f),
+			Vec3(0.20527978f, -0.01703966f, -0.09207391f),
+			Vec3(0.21142941f, 0.01785821f, -0.09836373f),
+			Vec3(0.21466828f, 0.05084385f, -0.03549951f),
+			Vec3(-0.20511348f, -0.07018351f, -0.31925454f),
+			Vec3(-0.19310803f, -0.13756239f, -0.33457401f),
+			Vec3(-0.20095457f, -0.09572067f, -0.11383702f),
+			Vec3(-0.18695570f, -0.14865115f, -0.19356145f),
+			Vec3(-0.18073241f, -0.08639215f, -0.35319963f),
+			Vec3(-0.18014188f, -0.15241129f, -0.34185338f),
+			Vec3(-0.18174356f, -0.15312561f, -0.19147469f),
+			Vec3(-0.19579467f, 0.01310298f, -0.00632396f),
+			Vec3(-0.16814114f, -0.05610058f, -0.34890732f),
+			Vec3(-0.16448530f, -0.16787034f, -0.29141789f),
+			Vec3(-0.17525161f, 0.01533679f, 0.08730947f),
+			Vec3(-0.17286175f, 0.08774700f, -0.01591185f),
+			Vec3(-0.17077128f, 0.01983560f, 0.10070839f),
+			Vec3(-0.14615997f, -0.16541340f, -0.37489247f),
+			Vec3(-0.14595763f, -0.16490393f, -0.37515628f),
+			Vec3(-0.16272801f, 0.07975677f, 0.08464866f),
+			Vec3(-0.13369306f, -0.06286648f, -0.37556374f),
+			Vec3(-0.14785704f, 0.14323678f, -0.01563696f),
+			Vec3(-0.12817731f, -0.04268694f, -0.36287897f),
+			Vec3(-0.14112462f, 0.13547241f, 0.05140329f),
+			Vec3(-0.12341158f, -0.17782864f, -0.36954373f),
+			Vec3(-0.12310848f, -0.18070405f, -0.20412853f),
+			Vec3(-0.09967888f, -0.18289816f, -0.38768309f),
+			Vec3(-0.09960851f, 0.14144828f, 0.12903015f),
+			Vec3(-0.08962545f, -0.17236463f, -0.39919903f),
+			Vec3(-0.09338194f, -0.00865331f, 0.23358464f),
+			Vec3(-0.09496998f, 0.17418922f, 0.03730623f),
+			Vec3(-0.09499961f, 0.16077143f, -0.03914160f),
+			Vec3(-0.08221246f, -0.07778487f, -0.39787262f),
+			Vec3(-0.07918695f, -0.14616625f, -0.40242865f),
+			Vec3(-0.08256439f, 0.01469633f, 0.24209134f),
+			Vec3(-0.07199146f, 0.16959090f, 0.11185526f),
+			Vec3(-0.05876892f, -0.18819671f, -0.40239989f),
+			Vec3(-0.05744339f, -0.18692162f, -0.40386000f),
+			Vec3(-0.04441069f, -0.04126521f, -0.37501192f),
+			Vec3(-0.04648328f, 0.18093951f, 0.03905040f),
+			Vec3(-0.03611449f, -0.14904837f, -0.40508240f),
+			Vec3(-0.03163360f, 0.17144355f, 0.13303288f),
+			Vec3(-0.02255749f, -0.01798030f, 0.33883106f),
+			Vec3(-0.01062212f, -0.11764656f, -0.39784804f),
+			Vec3(0.00002799f, -0.18946082f, -0.39155373f),
+			Vec3(0.00190875f, -0.16691279f, -0.40337407f),
+			Vec3(0.02337403f, -0.03170533f, 0.38295418f),
+			Vec3(0.02689898f, -0.03111388f, 0.38642361f),
+			Vec3(0.03513940f, -0.09795553f, -0.38733068f),
+			Vec3(0.04139633f, -0.18845227f, -0.32015734f),
+			Vec3(0.04843888f, 0.12765829f, -0.09677977f),
+			Vec3(0.04454701f, -0.14539991f, -0.38590988f),
+			Vec3(0.04690936f, -0.17584648f, -0.38177087f),
+			Vec3(0.05052238f, -0.18907529f, -0.35411724f),
+			Vec3(0.07129140f, -0.02806735f, 0.41684112f),
+			Vec3(0.07599759f, 0.02516599f, 0.43382310f),
+			Vec3(0.08328492f, -0.18135514f, -0.32588836f),
+			Vec3(0.08443428f, 0.07232403f, 0.37877142f),
+			Vec3(0.09074404f, -0.15272216f, -0.36002999f),
+			Vec3(0.09381036f, -0.04931259f, -0.32999005f),
+			Vec3(0.09348832f, -0.17767928f, -0.33666068f),
+			Vec3(0.09247280f, -0.01328942f, 0.44227284f),
+			Vec3(0.09364306f, 0.03557658f, 0.44191616f),
+			Vec3(0.09611026f, -0.01203391f, 0.44345939f),
+			Vec3(0.09662163f, 0.03456752f, 0.44326156f),
+			Vec3(0.10482377f, 0.12817247f, 0.27224415f),
+			Vec3(0.11271536f, 0.12685699f, 0.26856660f),
+			Vec3(0.10957191f, 0.03837919f, 0.43455946f),
+			Vec3(0.11146642f, -0.01284471f, 0.42120608f),
+			Vec3(0.11088928f, 0.00377234f, 0.44789928f),
+			Vec3(0.11571233f, -0.12474029f, -0.34762913f),
+			Vec3(0.12183426f, -0.16410264f, -0.30295142f),
+			Vec3(0.12211698f, 0.01099167f, 0.44373258f),
+			Vec3(0.12308656f, 0.01315179f, 0.44303578f),
+			Vec3(0.13090495f, -0.15086941f, -0.31031519f),
+			Vec3(0.14427974f, 0.09778974f, 0.30786031f),
+			Vec3(0.14200252f, 0.01419945f, 0.41783332f),
+			Vec3(0.14424091f, 0.06972501f, 0.37377491f),
+			Vec3(0.14422383f, 0.02227210f, 0.41717034f),
+			Vec3(0.15133176f, -0.03861540f, -0.27380293f),
+			Vec3(0.14738929f, 0.06972805f, 0.37101438f),
+			Vec3(0.15116664f, -0.13012324f, -0.26891800f),
+			Vec3(0.15432675f, -0.05065062f, -0.27696538f),
+			Vec3(0.17231981f, 0.09891064f, -0.04109610f),
+			Vec3(0.15486444f, 0.03080789f, 0.39333733f),
+			Vec3(0.16293872f, 0.09977609f, 0.23133035f),
+			Vec3(0.17278114f, 0.05925680f, -0.13166353f),
+			Vec3(0.17344120f, 0.06815492f, 0.29800513f),
+			Vec3(0.18346339f, 0.03002923f, -0.16944433f),
+			Vec3(0.18475264f, -0.03337195f, -0.21144425f),
+			Vec3(0.18153211f, 0.05077920f, 0.29410797f),
+			Vec3(0.18872119f, 0.08419117f, 0.18681980f),
+			Vec3(0.19402013f, 0.03129275f, -0.14645814f),
+			Vec3(0.20299899f, 0.06450803f, -0.05323168f),
+			Vec3(-0.20916573f, -0.14482390f, -0.28754678f),
+			Vec3(-0.21912349f, -0.12297497f, -0.25853595f),
+			Vec3(-0.21891747f, -0.11492035f, -0.30946639f),
+			Vec3(-0.22503024f, -0.09871494f, -0.27031892f),
+			Vec3(-0.22503024f, -0.09871494f, -0.27031892f),
+			Vec3(-0.22503024f, -0.09871494f, -0.27031892f)
+		},
+		{
+			Vec3(0.28483882f, 0.09470236f, 0.11433057f),
+			Vec3(0.30260321f, 0.07340867f, 0.00849266f),
+			Vec3(0.30380272f, 0.05582517f, -0.22405298f),
+			Vec3(0.30670973f, 0.02778204f, -0.22415190f),
+			Vec3(-0.29766368f, -0.06492511f, -0.19135096f),
+			Vec3(-0.28324991f, 0.02856347f, 0.16558051f),
+			Vec3(-0.27339774f, 0.11253071f, -0.13812468f),
+			Vec3(-0.26324614f, -0.03483995f, 0.34903234f),
+			Vec3(-0.27118766f, -0.15035510f, -0.06431498f),
+			Vec3(-0.26041472f, 0.10464326f, -0.20795805f),
+			Vec3(-0.22156618f, -0.00712212f, 0.40348106f),
+			Vec3(-0.20013636f, 0.13795423f, -0.23888915f),
+			Vec3(-0.19368620f, 0.04208890f, 0.42129427f),
+			Vec3(-0.18170905f, -0.10169907f, 0.38139578f),
+			Vec3(-0.18724660f, 0.18995818f, 0.08522552f),
+			Vec3(-0.17479378f, -0.05597380f, 0.41057986f),
+			Vec3(-0.15012621f, 0.08595391f, 0.43914794f),
+			Vec3(-0.11722116f, -0.10298516f, -0.30289822f),
+			Vec3(-0.11217459f, 0.00596011f, 0.44133874f),
+			Vec3(-0.11709289f, 0.23012112f, 0.12055066f),
+			Vec3(-0.10705470f, 0.15775623f, -0.33419770f),
+			Vec3(-0.08655276f, 0.09824081f, 0.43651989f),
+			Vec3(-0.08401379f, 0.08668444f, -0.41111666f),
+			Vec3(-0.08026488f, -0.24695427f, -0.01228247f),
+			Vec3(-0.06294082f, 0.12666735f, -0.39178270f),
+			Vec3(-0.05308891f, -0.07724215f, -0.37346649f),
+			Vec3(-0.04869145f, -0.23846265f, -0.11154356f),
+			Vec3(-0.04377052f, 0.06346821f, 0.44263243f),
+			Vec3(-0.03821557f, 0.05776290f, -0.43330976f),
+			Vec3(-0.01401243f, -0.07849873f, 0.37016886f),
+			Vec3(-0.01267736f, -0.24327334f, -0.09846258f),
+			Vec3(-0.00871999f, -0.24532425f, -0.01158716f),
+			Vec3(0.00610917f, 0.20575316f, -0.32363408f),
+			Vec3(0.01893912f, -0.02637211f, -0.44099009f),
+			Vec3(0.03742292f, 0.25572568f, 0.11976100f),
+			Vec3(0.04572892f, -0.02452080f, 0.37599292f),
+			Vec3(0.04809525f, 0.11413645f, 0.38247618f),
+			Vec3(0.04934106f, -0.01875172f, -0.43612641f),
+			Vec3(0.07854398f, 0.13351599f, 0.34539741f),
+			Vec3(0.11064179f, 0.03347895f, 0.33272063f),
+			Vec3(0.11110801f, 0.04016598f, -0.42360800f),
+			Vec3(0.12390327f, -0.20230874f, -0.01599736f),
+			Vec3(0.13082972f, -0.19843940f, -0.08606190f),
+			Vec3(0.12559986f, -0.02563187f, -0.38013845f),
+			Vec3(0.12924608f, 0.16206453f, -0.34893369f),
+			Vec3(0.15646456f, 0.21451330f, 0.16623015f),
+			Vec3(0.17851203f, -0.14074428f, 0.08427754f),
+			Vec3(0.19401437f, -0.15288332f, -0.03272480f),
+			Vec3(0.20102191f, 0.08705597f, -0.37915167f),
+			Vec3(0.20596674f, 0.06604006f, -0.38868805f),
+			Vec3(0.26085311f, 0.08702713f, -0.32507085f),
+			Vec3(0.27331018f, 0.15497627f, 0.11259682f),
+			Vec3(0.27269470f, 0.03719006f, -0.31962081f),
+			Vec3(0.27288356f, 0.06217747f, -0.33064606f),
+			Vec3(-0.29314118f, -0.18079891f, 0.24351751f),
+			Vec3(-0.30831277f, -0.06952596f, 0.07340523f),
+			Vec3(-0.30126276f, -0.18365636f, 0.22815129f),
+			Vec3(-0.30392047f, -0.17969127f, 0.22713920f),
+			Vec3(-0.30392047f, -0.17969127f, 0.22713920f),
+			Vec3(-0.30392047f, -0.17969127f, 0.22713920f)
+		},
+		{
+			// A really small hull
+			Vec3(-0.00707678869f, 0.00559568405f, -0.0239779726f),
+			Vec3(0.0136205591f, 0.00541752577f, -0.0225500446f),
+			Vec3(0.0135576781f, 0.00559568405f, -0.0224227905f),
+			Vec3(-0.0108219199f, 0.00559568405f, -0.0223935191f),
+			Vec3(0.0137226451f, 0.00559568405f, -0.0220940933f),
+			Vec3(0.00301175844f, -0.0232942104f, -0.0214947499f),
+			Vec3(0.017349612f, 0.00559568405f, 0.0241708681f),
+			Vec3(0.00390899926f, -0.0368074179f, 0.0541367307f),
+			Vec3(-0.0164459459f, 0.00559568405f, 0.0607497096f),
+			Vec3(-0.0169881769f, 0.00559568405f, 0.0608173609f),
+			Vec3(-0.0168782212f, 0.0052883029f, 0.0613293499f),
+			Vec3(-0.00663783913f, 0.00559568405f, -0.024154868f),
+			Vec3(-0.00507298298f, 0.00559568405f, -0.0242112875f),
+			Vec3(-0.00565947127f, 0.00477081537f, -0.0243848339f),
+			Vec3(0.0118075963f, 0.00124305487f, -0.0258472487f),
+			Vec3(0.00860248506f, -0.00697988272f, -0.0276725553f),
+		},
+		{
+			// Nearly co-planar hull (but not enough to go through the 2d hull builder)
+			Vec3(0.129325435f, -0.213319957f, 0.00901593268f),
+			Vec3(0.129251331f, -0.213436425f, 0.00932094082f),
+			Vec3(0.160741553f, -0.171540618f, 0.0494558439f),
+			Vec3(0.160671368f, -0.17165187f, 0.049765937f),
+			Vec3(0.14228563f, 0.432965666f, 0.282429159f),
+			Vec3(0.142746598f, 0.433226734f, 0.283286631f),
+			Vec3(0.296031296f, 0.226935148f, 0.312804461f),
+			Vec3(0.296214104f, 0.227568939f, 0.313606918f),
+			Vec3(-0.00354258716f, -0.180767179f, -0.0762089267f),
+			Vec3(-0.00372517109f, -0.1805875f, -0.0766792595f),
+			Vec3(-0.0157070309f, -0.176182508f, -0.0833940506f),
+			Vec3(-0.0161666721f, -0.175898403f, -0.0840280354f),
+			Vec3(-0.342764735f, 0.0259497911f, -0.244388372f),
+			Vec3(-0.342298329f, 0.0256615728f, -0.24456653f),
+			Vec3(-0.366584063f, 0.0554589033f, -0.250078142f),
+			Vec3(-0.366478682f, 0.0556178838f, -0.250342518f),
+		},
+		{
+			// A hull with a very acute angle that won't properly build when using distance to plane only
+			Vec3(-0.0451235026f, -0.103826642f, -0.0346511155f),
+			Vec3(-0.0194563419f, -0.123563275f, -0.032212317f),
+			Vec3(0.0323024541f, -0.0468643308f, -0.0307639092f),
+			Vec3(0.0412166864f, -0.0884782523f, -0.0288816988f),
+			Vec3(-0.0564572513f, 0.0207469314f, 0.0169318169f),
+			Vec3(0.00537410378f, 0.105688639f, 0.0355164111f),
+			Vec3(0.0209896415f, 0.117749952f, 0.0365252197f),
+			Vec3(0.0211542398f, 0.118546993f, 0.0375355929f),
+		}
+	};
+
+	// Add a cube formed out of a regular grid of vertices, this shows how the algorithm deals
+	// with many coplanar points
+	{
+		Points p;
+		for (int x = 0; x < 10; ++x)
+			for (int y = 0; y < 10; ++y)
+				for (int z = 0; z < 10; ++z)
+					p.push_back(Vec3::sReplicate(-0.5f) * 0.1f * Vec3(float(x), float(y), float(z)));
+		mPoints.push_back(std::move(p));
+	}
+
+	// Add disc of many points
+	{
+		Points p;
+		Mat44 rot = Mat44::sRotationZ(0.25f * JPH_PI);
+		for (float r = 0.0f; r < 2.0f; r += 0.1f)
+			for (float phi = 0.0f; phi <= 2.0f * JPH_PI; phi += 2.0f * JPH_PI / 20.0f)
+				p.push_back(rot * Vec3(r * Cos(phi), r * Sin(phi), 0));
+		mPoints.push_back(std::move(p));
+	}
+
+	// Add wedge shaped disc that is just above the hull tolerance on its widest side and zero on the other side
+	{
+		Points p;
+		for (float phi = 0.0f; phi <= 2.0f * JPH_PI; phi += 2.0f * JPH_PI / 40.0f)
+		{
+			Vec3 pos(2.0f * Cos(phi), 0, 2.0f * Sin(phi));
+			p.push_back(pos);
+			p.push_back(pos + Vec3(0, 2.0e-3f * (2.0f + pos.GetX()) / 4.0f, 0));
+		}
+		mPoints.push_back(std::move(p));
+	}
+
+	// Add a sphere of many points
+	{
+		Points p;
+		for (float theta = 0.0f; theta <= JPH_PI; theta += JPH_PI / 20.0f)
+			for (float phi = 0.0f; phi <= 2.0f * JPH_PI; phi += 2.0f * JPH_PI / 20.0f)
+				p.push_back(Vec3::sUnitSpherical(theta, phi));
+		mPoints.push_back(std::move(p));
+	}
+
+	// Open the external file with hulls
+	// A stream containing predefined convex hulls
+	AssetStream points_asset_stream("convex_hulls.bin", std::ios::in | std::ios::binary);
+	std::istream &points_stream = points_asset_stream.Get();
+	for (;;)
+	{
+		// Read the length of the next point cloud
+		uint32 len = 0;
+		points_stream.read((char *)&len, sizeof(len));
+		if (points_stream.eof())
+			break;
+
+		// Read the points
+		if (len > 0)
+		{
+			Points p;
+			for (uint32 i = 0; i < len; ++i)
+			{
+				Float3 v;
+				points_stream.read((char *)&v, sizeof(v));
+				p.push_back(Vec3(v));
+			}
+			mPoints.push_back(std::move(p));
+		}
+	}
+}
+
+void ConvexHullTest::PrePhysicsUpdate(const PreUpdateParams &inParams)
+{
+	const float display_scale = 10.0f;
+
+	float tolerance = 1.0e-3f;
+
+	Points points;
+	if (mIteration < mPoints.size())
+	{
+		// Take one of the predefined shapes
+		points = mPoints[mIteration];
+	}
+	else
+	{
+		uniform_real_distribution<float> zero_one(0.0f, 1.0f);
+		uniform_real_distribution<float> zero_two(0.0f, 2.0f);
+
+		// Define vertex scale
+		uniform_real_distribution<float> scale_start(0.1f, 0.5f);
+		uniform_real_distribution<float> scale_range(0.1f, 2.0f);
+		float start = scale_start(mRandom);
+		uniform_real_distribution<float> vertex_scale(start, start + scale_range(mRandom));
+
+		// Define shape scale to make shape less sphere like
+		uniform_real_distribution<float> shape_scale(0.1f, 1.0f);
+		Vec3 scale(shape_scale(mRandom), shape_scale(mRandom), shape_scale(mRandom));
+
+		// Add some random points
+		for (int i = 0; i < 100; ++i)
+		{
+			// Add random point
+			Vec3 p1 = vertex_scale(mRandom) * Vec3::sRandom(mRandom) * scale;
+			points.push_back(p1);
+
+			// Point close to p1
+			Vec3 p2 = p1 + tolerance * zero_two(mRandom) * Vec3::sRandom(mRandom);
+			points.push_back(p2);
+
+			// Point on a line to another point
+			float fraction = zero_one(mRandom);
+			Vec3 p3 = fraction * p1 + (1.0f - fraction) * points[mRandom() % points.size()];
+			points.push_back(p3);
+
+			// Point close to p3
+			Vec3 p4 = p3 + tolerance * zero_two(mRandom) * Vec3::sRandom(mRandom);
+			points.push_back(p4);
+		}
+	}
+	mIteration++;
+
+	using Face = ConvexHullBuilder::Face;
+	using Edge = ConvexHullBuilder::Edge;
+	ConvexHullBuilder builder(points);
+
+	// Build the hull
+	const char *error = nullptr;
+	ConvexHullBuilder::EResult result = builder.Initialize(INT_MAX, tolerance, error);
+	if (result != ConvexHullBuilder::EResult::Success && result != ConvexHullBuilder::EResult::MaxVerticesReached)
+	{
+		Trace("Iteration %d: Failed to initialize from positions: %s", mIteration - 1, error);
+		JPH_ASSERT(false);
+		return;
+	}
+
+	// Determine center of mass
+	Vec3 com;
+	float vol;
+	builder.GetCenterOfMassAndVolume(com, vol);
+
+	// Test if all points are inside the hull with the given tolerance
+	float max_error, coplanar_distance;
+	int max_error_point;
+	Face *max_error_face;
+	builder.DetermineMaxError(max_error_face, max_error, max_error_point, coplanar_distance);
+
+	// Check if error is bigger than 4 * the tolerance
+	if (max_error > 4.0f * max(tolerance, coplanar_distance))
+	{
+		Trace("Iteration %d: max_error=%g", mIteration - 1, (double)max_error);
+
+		// Draw point that had the max error
+		Vec3 point = display_scale * (points[max_error_point] - com);
+		DrawMarkerSP(mDebugRenderer, point, Color::sRed, 1.0f);
+		DrawText3DSP(mDebugRenderer, point, StringFormat("%d: %g", max_error_point, (double)max_error), Color::sRed);
+
+		// Length of normal (2x area) for max error face
+		Vec3 centroid = display_scale * (max_error_face->mCentroid - com);
+		Vec3 centroid_plus_normal = centroid + max_error_face->mNormal.Normalized();
+		DrawArrowSP(mDebugRenderer, centroid, centroid_plus_normal, Color::sGreen, 0.1f);
+		DrawText3DSP(mDebugRenderer, centroid_plus_normal, ConvertToString(max_error_face->mNormal.Length()), Color::sGreen);
+
+		// Draw face that had the max error
+		const Edge *e = max_error_face->mFirstEdge;
+		Vec3 prev = display_scale * (points[e->mStartIdx] - com);
+		do
+		{
+			const Edge *next = e->mNextEdge;
+			Vec3 cur = display_scale * (points[next->mStartIdx] - com);
+			DrawArrowSP(mDebugRenderer, prev, cur, Color::sYellow, 0.01f);
+			DrawText3DSP(mDebugRenderer, prev, ConvertToString(e->mStartIdx), Color::sYellow);
+			e = next;
+			prev = cur;
+		} while (e != max_error_face->mFirstEdge);
+
+		JPH_ASSERT(false);
+	}
+
+	// Draw input points around center of mass
+	for (Vec3 v : points)
+		DrawMarkerSP(mDebugRenderer, display_scale * (v - com), Color::sWhite, 0.01f);
+
+	// Draw the hull around center of mass
+	int color_idx = 0;
+	for (Face *f : builder.GetFaces())
+	{
+		Color color = Color::sGetDistinctColor(color_idx++);
+
+		// First point
+		const Edge *e = f->mFirstEdge;
+		Vec3 p1 = display_scale * (points[e->mStartIdx] - com);
+
+		// Second point
+		e = e->mNextEdge;
+		Vec3 p2 = display_scale * (points[e->mStartIdx] - com);
+
+		// First line
+		DrawLineSP(mDebugRenderer, p1, p2, Color::sGrey);
+
+		do
+		{
+			// Third point
+			e = e->mNextEdge;
+			Vec3 p3 = display_scale * (points[e->mStartIdx] - com);
+
+			DrawTriangleSP(mDebugRenderer, p1, p2, p3, color);
+
+			DrawLineSP(mDebugRenderer, p2, p3, Color::sGrey);
+
+			p2 = p3;
+		}
+		while (e != f->mFirstEdge);
+	}
+}

lib/All/JoltPhysics/Samples/Tests/ConvexCollision/ConvexHullTest.h

@@ -0,0 +1,31 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/Test.h>
+
+// Simple test to create a convex hull
+class ConvexHullTest : public Test
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, ConvexHullTest)
+
+	// Initialize the test
+	virtual void			Initialize() override;
+
+	// Update the test, called before the physics update
+	virtual void			PrePhysicsUpdate(const PreUpdateParams &inParams) override;
+
+private:
+	// A list of predefined points to feed the convex hull algorithm
+	using Points = Array<Vec3>;
+	Array<Points>			mPoints;
+
+	// Which index in the list we're currently using
+	size_t					mIteration = 0;
+
+	// If we run out of points, we start creating random points
+	default_random_engine	mRandom { 12345 };
+};

lib/All/JoltPhysics/Samples/Tests/ConvexCollision/EPATest.cpp

@@ -0,0 +1,90 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/ConvexCollision/EPATest.h>
+#include <Jolt/Geometry/Sphere.h>
+#include <Jolt/Geometry/AABox.h>
+#include <Jolt/Geometry/ConvexSupport.h>
+#include <Jolt/Geometry/EPAPenetrationDepth.h>
+#include <Utils/DebugRendererSP.h>
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(EPATest)
+{
+	JPH_ADD_BASE_CLASS(EPATest, Test)
+}
+
+void EPATest::PrePhysicsUpdate(const PreUpdateParams &inParams)
+{
+	AABox box(Vec3(1, 1, -2), Vec3(2, 2, 2));
+	Sphere sphere(Vec3(4, 4, 0), sqrt(8.0f) + 0.01f);
+	Mat44 matrix = Mat44::sRotationTranslation(Quat::sRotation(Vec3(1, 1, 1).Normalized(), 0.25f * JPH_PI), Vec3(1, 2, 3));
+	bool intersecting = CollideBoxSphere(matrix, box, sphere);
+	JPH_ASSERT(intersecting);
+	(void)intersecting; // For when asserts are off
+}
+
+bool EPATest::CollideBoxSphere(Mat44Arg inMatrix, const AABox &inBox, const Sphere &inSphere) const
+{
+	// Draw the box and shere
+	DrawBoxSP(mDebugRenderer, inMatrix, inBox, Color::sGrey);
+	DrawSphereSP(mDebugRenderer, inMatrix * inSphere.GetCenter(), inSphere.GetRadius(), Color::sGrey);
+
+	// Transform the box and sphere according to inMatrix
+	TransformedConvexObject transformed_box(inMatrix, inBox);
+	TransformedConvexObject transformed_sphere(inMatrix, inSphere);
+
+	// Run the EPA algorithm
+	EPAPenetrationDepth epa;
+	Vec3 v1 = Vec3::sAxisX(), pa1, pb1;
+	bool intersect1 = epa.GetPenetrationDepth(transformed_box, transformed_box, 0.0f, transformed_sphere, transformed_sphere, 0.0f, 1.0e-2f, FLT_EPSILON, v1, pa1, pb1);
+
+	// Draw iterative solution
+	if (intersect1)
+	{
+		DrawMarkerSP(mDebugRenderer, pa1, Color::sRed, 1.0f);
+		DrawMarkerSP(mDebugRenderer, pb1, Color::sGreen, 1.0f);
+		DrawArrowSP(mDebugRenderer, pb1 + Vec3(0, 1, 0), pb1 + Vec3(0, 1, 0) + v1, Color::sYellow, 0.1f);
+	}
+
+	// Calculate analytical solution
+	Vec3 pa2 = inBox.GetClosestPoint(inSphere.GetCenter());
+	Vec3 v2 = inSphere.GetCenter() - pa2;
+	bool intersect2 = v2.LengthSq() <= Square(inSphere.GetRadius());
+
+	JPH_ASSERT(intersect1 == intersect2);
+	if (intersect1 && intersect2)
+	{
+		Vec3 pb2 = inSphere.GetCenter() - inSphere.GetRadius() * v2.NormalizedOr(Vec3::sZero());
+
+		// Transform analytical solution
+		v2 = inMatrix.Multiply3x3(v2);
+		pa2 = inMatrix * pa2;
+		pb2 = inMatrix * pb2;
+
+		// Draw analytical solution
+		DrawMarkerSP(mDebugRenderer, pa2, Color::sOrange, 1.0f);
+		DrawMarkerSP(mDebugRenderer, pb2, Color::sYellow, 1.0f);
+
+		// Check angle between v1 and v2
+		float dot = v1.Dot(v2);
+		float len = v1.Length() * v2.Length();
+		float angle = RadiansToDegrees(ACos(dot / len));
+		JPH_ASSERT(angle < 0.1f);
+		Trace("Angle = %.9g", (double)angle);
+
+		// Check delta between contact on A
+		Vec3 dpa = pa2 - pa1;
+		JPH_ASSERT(dpa.IsNearZero(Square(8.0e-4f)));
+		Trace("Delta A = %.9g", (double)dpa.Length());
+
+		// Check delta between contact on B
+		Vec3 dpb = pb2 - pb1;
+		JPH_ASSERT(dpb.IsNearZero(Square(8.0e-4f)));
+		Trace("Delta B = %.9g", (double)dpb.Length());
+	}
+
+	return intersect1;
+}

lib/All/JoltPhysics/Samples/Tests/ConvexCollision/EPATest.h

@@ -0,0 +1,20 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/Test.h>
+
+// Does a single box vs sphere test without convex radius for visually debugging the EPA algorithm
+class EPATest : public Test
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, EPATest)
+
+	// Update the test, called before the physics update
+	virtual void	PrePhysicsUpdate(const PreUpdateParams &inParams) override;
+
+private:
+	bool			CollideBoxSphere(Mat44Arg inMatrix, const AABox &inBox, const Sphere &inSphere) const;
+};

lib/All/JoltPhysics/Samples/Tests/ConvexCollision/InteractivePairsTest.cpp

@@ -0,0 +1,221 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/ConvexCollision/InteractivePairsTest.h>
+#include <Input/Keyboard.h>
+#include <Jolt/Geometry/Sphere.h>
+#include <Jolt/Geometry/AABox.h>
+#include <Jolt/Geometry/ConvexSupport.h>
+#include <Jolt/Geometry/EPAPenetrationDepth.h>
+#include <Utils/DebugRendererSP.h>
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(InteractivePairsTest)
+{
+	JPH_ADD_BASE_CLASS(InteractivePairsTest, Test)
+}
+
+void InteractivePairsTest::ProcessInput(const ProcessInputParams &inParams)
+{
+	// Keyboard controls
+	if (inParams.mKeyboard->IsKeyPressed(EKey::Z))
+	{
+		mKeyboardMode = true;
+		mDistance -= inParams.mDeltaTime;
+	}
+	else if (inParams.mKeyboard->IsKeyPressed(EKey::C))
+	{
+		mKeyboardMode = true;
+		mDistance += inParams.mDeltaTime;
+	}
+	else if (inParams.mKeyboard->IsKeyPressed(EKey::X))
+	{
+		mKeyboardMode = false;
+	}
+
+	// Auto update
+	if (!mKeyboardMode)
+		mDistance -= inParams.mDeltaTime;
+
+	// Clamp distance
+	if (mDistance < -4.0f)
+		mDistance = 4.0f;
+	if (mDistance > 4.0f)
+		mDistance = -4.0f;
+}
+
+void InteractivePairsTest::PrePhysicsUpdate(const PreUpdateParams &inParams)
+{
+	float z = 0.0f;
+
+	const float r1 = 0.25f * JPH_PI;
+	const float r2 = ATan(1.0f / sqrt(2.0f)); // When rotating cube by 45 degrees the one axis becomes sqrt(2) long while the other stays at length 1
+
+	for (int i = 0; i < 2; ++i)
+	{
+		const float cvx_radius = i == 0? 0.0f : 0.1f; // First round without convex radius, second with
+		const float edge_len = 1.0f - cvx_radius;
+		AABox b(Vec3(-edge_len, -edge_len, -edge_len), Vec3(edge_len, edge_len, edge_len));
+
+		// Face vs face
+		TestBoxVsBox(Vec3(0, 0, z), Vec3(0, 0, 0), cvx_radius, b, Vec3(mDistance, 0, z), Vec3(0, 0, 0), cvx_radius, b);
+		z += 4;
+		TestBoxVsBox(Vec3(0, 0, z), Vec3(0, 0, 0), cvx_radius, b, Vec3(mDistance, 0, z), Vec3(r1, 0, 0), cvx_radius, b);
+		z += 4;
+
+		// Face vs edge
+		TestBoxVsBox(Vec3(0, 0, z), Vec3(0, 0, 0), cvx_radius, b, Vec3(mDistance, 0, z), Vec3(0, r1, 0), cvx_radius, b);
+		z += 4;
+		TestBoxVsBox(Vec3(0, 0, z), Vec3(0, 0, 0), cvx_radius, b, Vec3(mDistance, 0, z), Vec3(0, 0, r1), cvx_radius, b);
+		z += 4;
+
+		// Face vs vertex
+		TestBoxVsBox(Vec3(0, 0, z), Vec3(0, 0, 0), cvx_radius, b, Vec3(mDistance, 0, z), Vec3(0, r2, r1), cvx_radius, b);
+		z += 4;
+
+		// Edge vs edge
+		TestBoxVsBox(Vec3(0, 0, z), Vec3(0, r1, 0), cvx_radius, b, Vec3(mDistance, 0, z), Vec3(0, r1, 0), cvx_radius, b);
+		z += 4;
+		TestBoxVsBox(Vec3(0, 0, z), Vec3(0, 0, r1), cvx_radius, b, Vec3(mDistance, 0, z), Vec3(0, r1, 0), cvx_radius, b);
+		z += 4;
+
+		// Edge vs vertex
+		TestBoxVsBox(Vec3(0, 0, z), Vec3(0, r2, r1), cvx_radius, b, Vec3(mDistance, 0, z), Vec3(0, r2, r1), cvx_radius, b);
+		z += 4;
+
+		// Sphere vs face
+		TestSphereVsBox(Vec3(0, 0, z), 1.0f, Vec3(mDistance, 0, z), Vec3(0, 0, 0), cvx_radius, b);
+		z += 4;
+		TestSphereVsBox(Vec3(0, 0, z), 1.0f, Vec3(mDistance, 0, z), Vec3(r1, 0, 0), cvx_radius, b);
+		z += 4;
+
+		// Sphere vs edge
+		TestSphereVsBox(Vec3(0, 0, z), 1.0f, Vec3(mDistance, 0, z), Vec3(0, r1, 0), cvx_radius, b);
+		z += 4;
+		TestSphereVsBox(Vec3(0, 0, z), 1.0f, Vec3(mDistance, 0, z), Vec3(0, 0, r1), cvx_radius, b);
+		z += 4;
+
+		// Sphere vs vertex
+		TestSphereVsBox(Vec3(0, 0, z), 1.0f, Vec3(mDistance, 0, z), Vec3(0, r2, r1), cvx_radius, b);
+		z += 4;
+
+		// Sphere vs sphere
+		TestSphereVsSphere(Vec3(0, 0, z), 1.0f, Vec3(mDistance, 0, z), 1.0f, i == 1);
+		z += 4;
+	}
+}
+
+void InteractivePairsTest::TestBoxVsBox(Vec3Arg inTranslationA, Vec3Arg inRotationA, float inConvexRadiusA, const AABox &inA, Vec3Arg inTranslationB, Vec3Arg inRotationB, float inConvexRadiusB, const AABox &inB)
+{
+	Mat44 mat_a = Mat44::sTranslation(inTranslationA) * Mat44::sRotationX(inRotationA.GetX()) * Mat44::sRotationY(inRotationA.GetY()) * Mat44::sRotationZ(inRotationA.GetZ());
+	TransformedConvexObject a(mat_a, inA);
+
+	Mat44 mat_b = Mat44::sTranslation(inTranslationB) * Mat44::sRotationX(inRotationB.GetX()) * Mat44::sRotationY(inRotationB.GetY()) * Mat44::sRotationZ(inRotationB.GetZ());
+	TransformedConvexObject b(mat_b, inB);
+
+	EPAPenetrationDepth pen_depth;
+	Vec3 v = Vec3::sAxisX(), pa, pb;
+
+	DrawBoxSP(mDebugRenderer, mat_a, inA, Color::sWhite);
+
+	AABox widened_a = inA;
+	widened_a.ExpandBy(Vec3::sReplicate(inConvexRadiusA));
+
+	AABox widened_b = inB;
+	widened_b.ExpandBy(Vec3::sReplicate(inConvexRadiusB));
+
+	DrawBoxSP(mDebugRenderer, mat_a, inA, Color::sWhite);
+	if (inConvexRadiusA > 0.0f)
+		DrawWireBoxSP(mDebugRenderer, mat_a, widened_a, Color::sWhite);
+
+	AddConvexRadius a_inc(a, inConvexRadiusA);
+	AddConvexRadius b_inc(b, inConvexRadiusB);
+
+	if (pen_depth.GetPenetrationDepth(a, a_inc, inConvexRadiusA, b, b_inc, inConvexRadiusB, 1.0e-4f, FLT_EPSILON, v, pa, pb))
+	{
+		DrawBoxSP(mDebugRenderer, mat_b, inB, Color::sRed);
+		if (inConvexRadiusB > 0.0f)
+			DrawWireBoxSP(mDebugRenderer, mat_b, widened_b, Color::sRed);
+		DrawMarkerSP(mDebugRenderer, pa, Color::sYellow, 2.0f);
+		DrawMarkerSP(mDebugRenderer, pb, Color::sCyan, 2.0f);
+	}
+	else
+	{
+		DrawBoxSP(mDebugRenderer, mat_b, inB, Color::sGreen);
+		if (inConvexRadiusB > 0.0f)
+			DrawWireBoxSP(mDebugRenderer, mat_b, widened_b, Color::sGreen);
+	}
+	DrawArrowSP(mDebugRenderer, inTranslationB + Vec3(0, 2, 0), inTranslationB + v + Vec3(0, 2, 0), Color::sOrange, 0.05f);
+}
+
+void InteractivePairsTest::TestSphereVsBox(Vec3Arg inTranslationA, float inRadiusA, Vec3Arg inTranslationB, Vec3Arg inRotationB, float inConvexRadiusB, const AABox &inB)
+{
+	Sphere s(inTranslationA, inRadiusA);
+	Mat44 mat_b = Mat44::sTranslation(inTranslationB) * Mat44::sRotationX(inRotationB.GetX()) * Mat44::sRotationY(inRotationB.GetY()) * Mat44::sRotationZ(inRotationB.GetZ());
+	TransformedConvexObject b(mat_b, inB);
+
+	AABox widened_b = inB;
+	widened_b.ExpandBy(Vec3::sReplicate(inConvexRadiusB));
+
+	EPAPenetrationDepth pen_depth;
+	Vec3 v = Vec3::sAxisX(), pa, pb;
+
+	DrawSphereSP(mDebugRenderer, inTranslationA, inRadiusA, Color::sWhite);
+
+	AddConvexRadius	b_inc(b, inConvexRadiusB);
+
+	if (pen_depth.GetPenetrationDepth(s, s, 0.0f, b, b_inc, inConvexRadiusB, 1.0e-4f, FLT_EPSILON, v, pa, pb))
+	{
+		DrawBoxSP(mDebugRenderer, mat_b, inB, Color::sRed);
+		if (inConvexRadiusB > 0.0f)
+			DrawWireBoxSP(mDebugRenderer, mat_b, widened_b, Color::sRed);
+		DrawMarkerSP(mDebugRenderer, pa, Color::sYellow, 2.0f);
+		DrawMarkerSP(mDebugRenderer, pb, Color::sCyan, 2.0f);
+	}
+	else
+	{
+		DrawBoxSP(mDebugRenderer, mat_b, inB, Color::sGreen);
+		if (inConvexRadiusB > 0.0f)
+			DrawWireBoxSP(mDebugRenderer, mat_b, widened_b, Color::sGreen);
+	}
+	DrawArrowSP(mDebugRenderer, inTranslationB + Vec3(0, 2, 0), inTranslationB + v + Vec3(0, 2, 0), Color::sOrange, 0.05f);
+}
+
+void InteractivePairsTest::TestSphereVsSphere(Vec3Arg inTranslationA, float inRadiusA, Vec3Arg inTranslationB, float inRadiusB, bool inTreatSphereAsPointWithConvexRadius)
+{
+	Sphere s1(inTranslationA, inRadiusA);
+	Sphere s2(inTranslationB, inRadiusB);
+
+	if (inTreatSphereAsPointWithConvexRadius)
+		DrawWireSphereSP(mDebugRenderer, s1.GetCenter(), s1.GetRadius(), Color::sWhite);
+	else
+		DrawSphereSP(mDebugRenderer, s1.GetCenter(), s1.GetRadius(), Color::sWhite);
+
+	bool intersects;
+	EPAPenetrationDepth pen_depth;
+	Vec3 v = Vec3::sAxisX(), pa, pb;
+	if (inTreatSphereAsPointWithConvexRadius)
+		intersects = pen_depth.GetPenetrationDepth(PointConvexSupport { inTranslationA }, s1, inRadiusA, PointConvexSupport { inTranslationB }, s2, inRadiusB, 1.0e-4f, FLT_EPSILON, v, pa, pb);
+	else
+		intersects = pen_depth.GetPenetrationDepth(s1, s1, 0.0f, s2, s2, 0.0f, 1.0e-4f, FLT_EPSILON, v, pa, pb);
+
+	if (intersects)
+	{
+		if (inTreatSphereAsPointWithConvexRadius)
+			DrawWireSphereSP(mDebugRenderer, s2.GetCenter(), s2.GetRadius(), Color::sRed);
+		else
+			DrawSphereSP(mDebugRenderer, s2.GetCenter(), s2.GetRadius(), Color::sRed);
+		DrawMarkerSP(mDebugRenderer, pa, Color::sYellow, 2.0f);
+		DrawMarkerSP(mDebugRenderer, pb, Color::sCyan, 2.0f);
+	}
+	else
+	{
+		if (inTreatSphereAsPointWithConvexRadius)
+			DrawWireSphereSP(mDebugRenderer, s2.GetCenter(), s2.GetRadius(), Color::sGreen);
+		else
+			DrawSphereSP(mDebugRenderer, s2.GetCenter(), s2.GetRadius(), Color::sGreen);
+	}
+	DrawArrowSP(mDebugRenderer, inTranslationB + Vec3(0, 2, 0), inTranslationB + v + Vec3(0, 2, 0), Color::sOrange, 0.05f);
+}

lib/All/JoltPhysics/Samples/Tests/ConvexCollision/InteractivePairsTest.h

@@ -0,0 +1,28 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/Test.h>
+
+// Renders pairs of objects and their collisions. Use Z, X, C keys to control distance.
+class InteractivePairsTest : public Test
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, InteractivePairsTest)
+
+	// Process input
+	virtual void	ProcessInput(const ProcessInputParams &inParams) override;
+
+	// Update the test, called before the physics update
+	virtual void	PrePhysicsUpdate(const PreUpdateParams &inParams) override;
+
+private:
+	void			TestBoxVsBox(Vec3Arg inTranslationA, Vec3Arg inRotationA, float inConvexRadiusA, const AABox &inA, Vec3Arg inTranslationB, Vec3Arg inRotationB, float inConvexRadiusB, const AABox &inB);
+	void			TestSphereVsBox(Vec3Arg inTranslationA, float inRadiusA, Vec3Arg inTranslationB, Vec3Arg inRotationB, float inConvexRadiusB, const AABox &inB);
+	void			TestSphereVsSphere(Vec3Arg inTranslationA, float inRadiusA, Vec3Arg inTranslationB, float inRadiusB, bool inTreatSphereAsPointWithConvexRadius);
+
+	bool			mKeyboardMode = false;
+	float			mDistance = 3.0f;
+};

lib/All/JoltPhysics/Samples/Tests/ConvexCollision/RandomRayTest.cpp

@@ -0,0 +1,208 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/ConvexCollision/RandomRayTest.h>
+#include <Jolt/Geometry/Sphere.h>
+#include <Jolt/Geometry/AABox.h>
+#include <Jolt/Geometry/GJKClosestPoint.h>
+#include <Jolt/Geometry/RayTriangle.h>
+#include <Jolt/Geometry/RaySphere.h>
+#include <Jolt/Geometry/RayAABox.h>
+#include <Jolt/Geometry/RayCapsule.h>
+#include <Jolt/Geometry/RayCylinder.h>
+#include <Jolt/Geometry/ConvexSupport.h>
+#include <Jolt/Physics/Collision/Shape/SphereShape.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Collision/Shape/CapsuleShape.h>
+#include <Jolt/Physics/Collision/Shape/CylinderShape.h>
+#include <Renderer/DebugRendererImp.h>
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(RandomRayTest)
+{
+	JPH_ADD_BASE_CLASS(RandomRayTest, Test)
+}
+
+//-----------------------------------------------------------------------------
+// Tests the CastRay function
+//-----------------------------------------------------------------------------
+template <typename A, typename Context>
+void RandomRayTest::TestRay(const char *inTestName, RVec3Arg inRenderOffset, const A &inA, const Context &inContext, float (*inCompareFunc)(const Context &inContext, Vec3Arg inRayOrigin, Vec3Arg inRayDirection))
+{
+	default_random_engine random(12345);
+	uniform_real_distribution<float> random_scale(-2.0f, 2.0f);
+
+#ifdef JPH_DEBUG
+	const int count = 1000;
+#else
+	const int count = 10000;
+#endif
+
+	int mismatches = 0;
+	int nonzero_hits = 0;
+	int zero_hits = 0;
+	float total_error = 0;
+	int total_error_count = 0;
+	float min_error = FLT_MAX;
+	float max_error = 0;
+
+	GJKClosestPoint gjk;
+
+	Trace("Starting: %s", inTestName);
+
+	for (int i = 0; i < count; ++i)
+	{
+		Vec3 from(random_scale(random), random_scale(random), random_scale(random));
+		Vec3 to(random_scale(random), random_scale(random), random_scale(random));
+		Vec3 direction = to - from;
+
+		// Use GJK to cast a ray
+		float fraction1 = 1.0f + FLT_EPSILON;
+		if (!gjk.CastRay(from, direction, 1.0e-4f, inA, fraction1))
+			fraction1 = FLT_MAX;
+
+		// Use the comparison function
+		float fraction2 = inCompareFunc(inContext, from, direction);
+
+		// The comparison functions work with infinite rays, so a fraction > 1 means a miss
+		if (fraction2 > 1.0f)
+			fraction2 = FLT_MAX;
+
+		float error = abs(fraction1 - fraction2);
+		if (error > 0.005f)
+		{
+			Trace("Mismatch iteration: %d (%f vs %f, diff: %f)", i, (double)fraction1, (double)fraction2, (double)abs(fraction2 - fraction1));
+			++mismatches;
+
+			Color c;
+			if (fraction2 == FLT_MAX)
+			{
+				c = Color::sRed;
+				mDebugRenderer->DrawMarker(inRenderOffset + from + fraction1 * direction, Color::sRed, 0.1f);
+			}
+			else if (fraction1 == FLT_MAX)
+			{
+				c = Color::sBlue;
+				mDebugRenderer->DrawMarker(inRenderOffset + from + fraction2 * direction, Color::sBlue, 0.1f);
+			}
+			else
+			{
+				total_error += abs(fraction2 - fraction1);
+				total_error_count++;
+				c = Color::sGreen;
+				mDebugRenderer->DrawMarker(inRenderOffset + from + fraction1 * direction, Color::sCyan, 0.1f);
+				mDebugRenderer->DrawMarker(inRenderOffset + from + fraction2 * direction, Color::sGreen, 0.1f);
+			}
+			mDebugRenderer->DrawArrow(inRenderOffset + from, inRenderOffset + to, c, 0.1f);
+		}
+		else if (fraction1 != FLT_MAX)
+		{
+			mDebugRenderer->DrawMarker(inRenderOffset + from + fraction1 * direction, Color::sYellow, 0.02f);
+		}
+
+		if (fraction1 != FLT_MAX && fraction2 != FLT_MAX)
+		{
+			total_error += error;
+			total_error_count++;
+			min_error = min(min_error, error);
+			max_error = max(max_error, error);
+		}
+
+		if (fraction2 == 0.0f)
+			++zero_hits;
+		else if (fraction2 > 0 && fraction2 <= 1.0f)
+			++nonzero_hits;
+	}
+
+	Trace("Report for: %s", inTestName);
+	Trace("Mismatches: %d (%.1f%%)", mismatches, 100.0 * mismatches / count);
+	Trace("Hits (fraction = 0): %d (%.1f%%)", zero_hits, 100.0 * zero_hits / count);
+	Trace("Hits (fraction > 0 and fraction <= 1): %d (%.1f%%)", nonzero_hits, 100.0 * nonzero_hits / count);
+	Trace("Fraction error: Avg %f, Min %f, Max %f", total_error_count > 0? double(total_error / total_error_count) : 0.0, (double)min_error, (double)max_error);
+}
+
+void RandomRayTest::PrePhysicsUpdate(const PreUpdateParams &inParams)
+{
+	{
+		RVec3 render_offset(0, 0, 0);
+		Sphere sphere(Vec3(0.1f, 0.2f, 0.3f), 1.1f);
+		mDebugRenderer->DrawSphere(render_offset + sphere.GetCenter(), sphere.GetRadius(), Color::sYellow);
+		TestRay<Sphere, Sphere>("Sphere", render_offset, sphere, sphere, [](const Sphere &inSphere, Vec3Arg inRayOrigin, Vec3Arg inRayDirection) {
+			return RaySphere(inRayOrigin, inRayDirection, inSphere.GetCenter(), inSphere.GetRadius());
+		});
+	}
+
+	{
+		RVec3 render_offset(5, 0, 0);
+		SphereShape sphere_shape(1.1f);
+	#ifdef JPH_DEBUG_RENDERER
+		sphere_shape.Draw(mDebugRenderer, RMat44::sTranslation(render_offset), Vec3::sOne(), Color::sYellow, false, false);
+	#endif // JPH_DEBUG_RENDERER
+		ConvexShape::SupportBuffer buffer;
+		const ConvexShape::Support *support = sphere_shape.GetSupportFunction(ConvexShape::ESupportMode::IncludeConvexRadius, buffer, Vec3::sOne());
+		TestRay<ConvexShape::Support, SphereShape>("Sphere Shape", render_offset, *support, sphere_shape, [](const SphereShape &inSphere, Vec3Arg inRayOrigin, Vec3Arg inRayDirection) {
+			return RaySphere(inRayOrigin, inRayDirection, Vec3::sZero(), inSphere.GetRadius());
+		});
+	}
+
+	{
+		RVec3 render_offset(10, 0, 0);
+		AABox box(Vec3(-0.9f, -1.0f, -1.1f), Vec3(0.8f, 0.9f, 1.0f));
+		mDebugRenderer->DrawBox(box.Transformed(Mat44::sTranslation(Vec3(render_offset))), Color::sYellow);
+		TestRay<AABox, AABox>("Box", render_offset, box, box, [](const AABox &inBox, Vec3Arg inRayOrigin, Vec3Arg inRayDirection) {
+			float fraction = RayAABox(inRayOrigin, RayInvDirection(inRayDirection), inBox.mMin, inBox.mMax);
+			return max(fraction, 0.0f);
+		});
+	}
+
+	{
+		RVec3 render_offset(15, 0, 0);
+		BoxShape box_shape(Vec3(0.9f, 1.0f, 1.1f), 0.0f);
+	#ifdef JPH_DEBUG_RENDERER
+		box_shape.Draw(mDebugRenderer, RMat44::sTranslation(render_offset), Vec3::sOne(), Color::sYellow, false, false);
+	#endif // JPH_DEBUG_RENDERER
+		ConvexShape::SupportBuffer buffer;
+		const ConvexShape::Support *support = box_shape.GetSupportFunction(ConvexShape::ESupportMode::IncludeConvexRadius, buffer, Vec3::sOne());
+		TestRay<ConvexShape::Support, BoxShape>("Box Shape", render_offset, *support, box_shape, [](const BoxShape &inBox, Vec3Arg inRayOrigin, Vec3Arg inRayDirection) {
+			float fraction = RayAABox(inRayOrigin, RayInvDirection(inRayDirection), -inBox.GetHalfExtent(), inBox.GetHalfExtent());
+			return max(fraction, 0.0f);
+		});
+	}
+
+	{
+		RVec3 render_offset(20, 0, 0);
+		CapsuleShape capsule_shape(1.1f, 0.6f);
+	#ifdef JPH_DEBUG_RENDERER
+		capsule_shape.Draw(mDebugRenderer, RMat44::sTranslation(render_offset), Vec3::sOne(), Color::sYellow, false, false);
+	#endif // JPH_DEBUG_RENDERER
+		ConvexShape::SupportBuffer buffer;
+		const ConvexShape::Support *support = capsule_shape.GetSupportFunction(ConvexShape::ESupportMode::IncludeConvexRadius, buffer, Vec3::sOne());
+		TestRay<ConvexShape::Support, CapsuleShape>("Capsule Shape", render_offset, *support, capsule_shape, [](const CapsuleShape &inCapsule, Vec3Arg inRayOrigin, Vec3Arg inRayDirection) {
+			return RayCapsule(inRayOrigin, inRayDirection, inCapsule.GetHalfHeightOfCylinder(), inCapsule.GetRadius());
+		});
+	}
+
+	{
+		RVec3 render_offset(25, 0, 0);
+		CylinderShape cylinder_shape(1.5f, 0.6f, 0.0f);
+	#ifdef JPH_DEBUG_RENDERER
+		cylinder_shape.Draw(mDebugRenderer, RMat44::sTranslation(render_offset), Vec3::sOne(), Color::sYellow, false, false);
+	#endif // JPH_DEBUG_RENDERER
+		ConvexShape::SupportBuffer buffer;
+		const ConvexShape::Support *support = cylinder_shape.GetSupportFunction(ConvexShape::ESupportMode::IncludeConvexRadius, buffer, Vec3::sOne());
+		TestRay<ConvexShape::Support, CylinderShape>("Cylinder Shape", render_offset, *support, cylinder_shape, [](const CylinderShape &inCylinder, Vec3Arg inRayOrigin, Vec3Arg inRayDirection) {
+			return RayCylinder(inRayOrigin, inRayDirection, inCylinder.GetHalfHeight(), inCylinder.GetRadius());
+		});
+	}
+
+	{
+		RVec3 render_offset(30, 0, 0);
+		TriangleConvexSupport triangle(Vec3(0.1f, 0.9f, 0.3f), Vec3(-0.9f, -0.5f, 0.2f), Vec3(0.7f, -0.3f, -0.1f));
+		mDebugRenderer->DrawTriangle(render_offset + triangle.mV1, render_offset + triangle.mV2, render_offset + triangle.mV3, Color::sYellow);
+		TestRay<TriangleConvexSupport, TriangleConvexSupport>("Triangle", render_offset, triangle, triangle, [](const TriangleConvexSupport &inTriangle, Vec3Arg inRayOrigin, Vec3Arg inRayDirection) {
+			return RayTriangle(inRayOrigin, inRayDirection, inTriangle.mV1, inTriangle.mV2, inTriangle.mV3);
+		});
+	}
+}

lib/All/JoltPhysics/Samples/Tests/ConvexCollision/RandomRayTest.h

@@ -0,0 +1,21 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/Test.h>
+
+// Tests a lot of random rays against convex shapes
+class RandomRayTest : public Test
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, RandomRayTest)
+
+	// Update the test, called before the physics update
+	virtual void	PrePhysicsUpdate(const PreUpdateParams &inParams) override;
+
+private:
+	template <typename A, typename Context>
+	void			TestRay(const char *inTestName, RVec3Arg inRenderOffset, const A &inA, const Context &inContext, float (*inCompareFunc)(const Context &inContext, Vec3Arg inRayOrigin, Vec3Arg inRayDirection));
+};

lib/All/JoltPhysics/Samples/Tests/General/ActivateDuringUpdateTest.cpp

@@ -0,0 +1,47 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/General/ActivateDuringUpdateTest.h>
+#include <Layers.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(ActivateDuringUpdateTest)
+{
+	JPH_ADD_BASE_CLASS(ActivateDuringUpdateTest, Test)
+}
+
+void ActivateDuringUpdateTest::Initialize()
+{
+	// Floor
+	CreateFloor();
+
+	BodyCreationSettings settings;
+	settings.SetShape(new BoxShape(Vec3::sReplicate(0.5f)));
+	settings.mMotionType = EMotionType::Dynamic;
+	settings.mObjectLayer = Layers::MOVING;
+
+	const int cNumBodies = 3;
+	const float cPenetrationSlop = mPhysicsSystem->GetPhysicsSettings().mPenetrationSlop;
+
+	for (int i = 0; i < cNumBodies; ++i)
+	{
+		settings.mPosition = RVec3(i * (1.0f - cPenetrationSlop), 2.0f, 0);
+		BodyID body_id = mBodyInterface->CreateBody(settings)->GetID();
+		mBodyInterface->AddBody(body_id, EActivation::DontActivate);
+		if (i == 0)
+			mBodyInterface->SetLinearVelocity(body_id, Vec3(500, 0, 0));
+	}
+
+	for (int i = 0; i < cNumBodies; ++i)
+	{
+		settings.mPosition = RVec3(i * (1.0f - cPenetrationSlop), 2.0f, 2.0f);
+		BodyID body_id = mBodyInterface->CreateBody(settings)->GetID();
+		mBodyInterface->AddBody(body_id, EActivation::DontActivate);
+		if (i == cNumBodies - 1)
+			mBodyInterface->SetLinearVelocity(body_id, Vec3(-500, 0, 0));
+	}
+}

lib/All/JoltPhysics/Samples/Tests/General/ActivateDuringUpdateTest.h

@@ -0,0 +1,23 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/Test.h>
+
+class ActivateDuringUpdateTest : public Test
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, ActivateDuringUpdateTest)
+
+	// Description of the test
+	virtual const char *	GetDescription() const override
+	{
+		return	"Three initially colliding boxes where only 1 is awake and has a high velocity.\n"
+				"The 2nd and 3rd box should wake up at the same time and not pass through each other.";
+	}
+
+	// Initialize the test
+	virtual void			Initialize() override;
+};

lib/All/JoltPhysics/Samples/Tests/General/ActiveEdgesTest.cpp

@@ -0,0 +1,98 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/General/ActiveEdgesTest.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Collision/Shape/MeshShape.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Jolt/Geometry/Triangle.h>
+#include <Layers.h>
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(ActiveEdgesTest)
+{
+	JPH_ADD_BASE_CLASS(ActiveEdgesTest, Test)
+}
+
+void ActiveEdgesTest::Initialize()
+{
+	const float cWidth = 5.0f;
+	const float cLength = 10.0f;
+
+	// Settings for a frictionless box
+	Ref<BoxShape> box_shape = new BoxShape(Vec3(1.0f, 1.0f, 1.0f), cDefaultConvexRadius);
+	BodyCreationSettings box_settings(box_shape, RVec3::sZero(), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING);
+	box_settings.mFriction = 0.0f;
+	box_settings.mLinearDamping = 0.0f;
+	box_settings.mAllowSleeping = false;
+
+	// Create various triangle strips
+	TriangleList triangles;
+	for (int angle = -90; angle <= 90; angle++)
+	{
+		// Under which normal we want to place the block
+		Vec3 desired_normal = angle < 0? Vec3(0, 1, -1).Normalized() : Vec3(0, 1, 0);
+		float best_dot = -FLT_MAX;
+
+		// Place segments
+		float x = cWidth * angle;
+		Vec3 v1(x, 0.0f, -0.5f * cLength);
+		Vec3 v2(x + cWidth, 0.0f, -0.5f * cLength);
+		for (int total_angle = 0, cur_segment = 0; abs(total_angle) <= 90 && cur_segment < 90; total_angle += angle, ++cur_segment)
+		{
+			// Determine positions of end of this segment
+			float total_angle_rad = DegreesToRadians(float(total_angle));
+			Quat rotation = Quat::sRotation(Vec3::sAxisX(), total_angle_rad);
+			Vec3 delta = cLength * rotation.RotateAxisZ();
+			Vec3 v3 = v1 + delta;
+			Vec3 v4 = v2 + delta;
+
+			// Check if this segment is the best segment to place the dynamic block on
+			Vec3 normal = (v3 - v1).Cross(v2 - v1).Normalized();
+			float dot = normal.Dot(desired_normal);
+			if (dot > best_dot)
+			{
+				best_dot = dot;
+				box_settings.mPosition = RVec3((v1 + v2 + v3 + v4) / 4 + normal);
+				box_settings.mRotation = rotation;
+			}
+
+			// Add segment
+			triangles.push_back(Triangle(v1, v3, v4));
+			triangles.push_back(Triangle(v1, v4, v2));
+
+			// Add segment mirrored in Z axis
+			if (cur_segment != 0)
+			{
+				Vec3 flip(1, 1, -1);
+				triangles.push_back(Triangle(flip * v1, flip * v4, flip * v3));
+				triangles.push_back(Triangle(flip * v1, flip * v2, flip * v4));
+			}
+
+			// The end of the segment will be the start for the next iteration
+			v1 = v3;
+			v2 = v4;
+		}
+
+		// Place box on best segment
+		Body &body = *mBodyInterface->CreateBody(box_settings);
+		mBodyInterface->AddBody(body.GetID(), EActivation::Activate);
+
+		// For convex segments give the block a push
+		if (angle >= 0)
+			body.SetLinearVelocity(Vec3(0, 0, 2.0f));
+	}
+
+	// Mesh shape
+	MeshShapeSettings mesh_shape(triangles);
+	mesh_shape.SetEmbedded();
+	mesh_shape.mActiveEdgeCosThresholdAngle = Cos(DegreesToRadians(50.0f));
+
+	// Mesh
+	BodyCreationSettings mesh_settings(&mesh_shape, RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING);
+	// Instead of setting mActiveEdgeCosThresholdAngle you can also set: mesh_settings.mEnhancedInternalEdgeRemoval = true
+	mesh_settings.mFriction = 0.0f;
+	mBodyInterface->CreateAndAddBody(mesh_settings, EActivation::DontActivate);
+}

lib/All/JoltPhysics/Samples/Tests/General/ActiveEdgesTest.h

@@ -0,0 +1,22 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/Test.h>
+
+class ActiveEdgesTest : public Test
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, ActiveEdgesTest)
+
+	// Description of the test
+	virtual const char *GetDescription() const override
+	{
+		return "Boxes sliding over the ramps should not collide with internal triangle edges of the ramp (aka ghost collisions).";
+	}
+
+	// See: Test
+	virtual void		Initialize() override;
+};

lib/All/JoltPhysics/Samples/Tests/General/AllowedDOFsTest.cpp

@@ -0,0 +1,62 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/General/AllowedDOFsTest.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Constraints/DistanceConstraint.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Renderer/DebugRendererImp.h>
+#include <Layers.h>
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(AllowedDOFsTest)
+{
+	JPH_ADD_BASE_CLASS(AllowedDOFsTest, Test)
+}
+
+void AllowedDOFsTest::Initialize()
+{
+	// Floor
+	CreateFloor();
+
+	Vec3 box_size(0.5f, 1.0f, 2.0f);
+	RefConst<Shape> box_shape = new BoxShape(box_size);
+
+	for (int allowed_dofs = 1; allowed_dofs <= 0b111111; ++allowed_dofs)
+	{
+		float x = -35.0f + 10.0f * (allowed_dofs & 0b111);
+		float z = -35.0f + 10.0f * ((allowed_dofs >> 3) & 0b111);
+
+		// Create body
+		BodyCreationSettings bcs(box_shape, RVec3(x, 10, z), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING);
+		bcs.mAllowedDOFs = (EAllowedDOFs)allowed_dofs;
+		BodyID id = mBodyInterface->CreateAndAddBody(bcs, EActivation::Activate);
+		mBodies.push_back(id);
+
+		// Create a constraint
+		DistanceConstraintSettings dcs;
+		dcs.mPoint1 = bcs.mPosition + Vec3(5, 5, 5);
+		dcs.mPoint2 = bcs.mPosition + box_size;
+		dcs.mMinDistance = 0.0f;
+		dcs.mMaxDistance = sqrt(3.0f) * 5.0f + 1.0f;
+		mPhysicsSystem->AddConstraint(mBodyInterface->CreateConstraint(&dcs, BodyID(), id));
+
+		// Draw degrees of freedom
+		String allowed_dofs_str = "";
+		if ((EAllowedDOFs(allowed_dofs) & EAllowedDOFs::TranslationX) == EAllowedDOFs::TranslationX)
+			allowed_dofs_str += "X ";
+		if ((EAllowedDOFs(allowed_dofs) & EAllowedDOFs::TranslationY) == EAllowedDOFs::TranslationY)
+			allowed_dofs_str += "Y ";
+		if ((EAllowedDOFs(allowed_dofs) & EAllowedDOFs::TranslationZ) == EAllowedDOFs::TranslationZ)
+			allowed_dofs_str += "Z ";
+		if ((EAllowedDOFs(allowed_dofs) & EAllowedDOFs::RotationX) == EAllowedDOFs::RotationX)
+			allowed_dofs_str += "RX ";
+		if ((EAllowedDOFs(allowed_dofs) & EAllowedDOFs::RotationY) == EAllowedDOFs::RotationY)
+			allowed_dofs_str += "RY ";
+		if ((EAllowedDOFs(allowed_dofs) & EAllowedDOFs::RotationZ) == EAllowedDOFs::RotationZ)
+			allowed_dofs_str += "RZ ";
+		SetBodyLabel(id, allowed_dofs_str);
+	}
+}

lib/All/JoltPhysics/Samples/Tests/General/AllowedDOFsTest.h

@@ -0,0 +1,26 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/Test.h>
+
+class AllowedDOFsTest : public Test
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, AllowedDOFsTest)
+
+	// Description of the test
+	virtual const char *GetDescription() const override
+	{
+		return	"Shows all permutations of allowed degrees of freedom for a body (see EAllowedDOFs).\n"
+				"The boxes are constrained to the world using a distance constraint, press C to show it.";
+	}
+
+	// See: Test
+	virtual void		Initialize() override;
+
+private:
+	BodyIDVector		mBodies;
+};

lib/All/JoltPhysics/Samples/Tests/General/BigVsSmallTest.cpp

@@ -0,0 +1,30 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/General/BigVsSmallTest.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Collision/Shape/MeshShape.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Jolt/Geometry/Triangle.h>
+#include <Layers.h>
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(BigVsSmallTest)
+{
+	JPH_ADD_BASE_CLASS(BigVsSmallTest, Test)
+}
+
+void BigVsSmallTest::Initialize()
+{
+	// Create a big triangle
+	TriangleList triangles;
+	triangles.push_back(Triangle(Float3(-100, 0, 0), Float3(0, 0, 100), Float3(100, 0, -100)));
+	mBodyInterface->CreateAndAddBody(BodyCreationSettings(new MeshShapeSettings(triangles), RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING), EActivation::DontActivate);
+
+	// A small box
+	Body &body = *mBodyInterface->CreateBody(BodyCreationSettings(new BoxShape(Vec3(0.1f, 0.1f, 0.1f)), RVec3(0, 1.0f, 0), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING));
+	body.SetAllowSleeping(false);
+	mBodyInterface->AddBody(body.GetID(), EActivation::Activate);
+}

lib/All/JoltPhysics/Samples/Tests/General/BigVsSmallTest.h

@@ -0,0 +1,22 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/Test.h>
+
+class BigVsSmallTest : public Test
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, BigVsSmallTest)
+
+	// Description of the test
+	virtual const char *GetDescription() const override
+	{
+		return "A small box falling on a big triangle to test for numerical precision errors.";
+	}
+
+	// See: Test
+	virtual void		Initialize() override;
+};

lib/All/JoltPhysics/Samples/Tests/General/CenterOfMassTest.cpp

@@ -0,0 +1,49 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/General/CenterOfMassTest.h>
+#include <Jolt/Physics/Collision/Shape/StaticCompoundShape.h>
+#include <Jolt/Physics/Collision/Shape/SphereShape.h>
+#include <Jolt/Physics/Collision/Shape/CapsuleShape.h>
+#include <Jolt/Physics/Collision/Shape/ConvexHullShape.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Layers.h>
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(CenterOfMassTest)
+{
+	JPH_ADD_BASE_CLASS(CenterOfMassTest, Test)
+}
+
+void CenterOfMassTest::Initialize()
+{
+	// Floor
+	CreateFloor();
+
+	// Compound shape with center of mass offset
+	Ref<StaticCompoundShapeSettings> compound_shape1 = new StaticCompoundShapeSettings;
+	compound_shape1->AddShape(Vec3(10, 0, 0), Quat::sIdentity(), new SphereShape(2));
+	mBodyInterface->CreateAndAddBody(BodyCreationSettings(compound_shape1, RVec3(0, 10.0f, 0), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING), EActivation::Activate);
+
+	// Create box with center of mass offset
+	Array<Vec3> box;
+	box.push_back(Vec3(10, 10, 10));
+	box.push_back(Vec3(5, 10, 10));
+	box.push_back(Vec3(10, 5, 10));
+	box.push_back(Vec3(5, 5, 10));
+	box.push_back(Vec3(10, 10, 5));
+	box.push_back(Vec3(5, 10, 5));
+	box.push_back(Vec3(10, 5, 5));
+	box.push_back(Vec3(5, 5, 5));
+	mBodyInterface->CreateAndAddBody(BodyCreationSettings(new ConvexHullShapeSettings(box), RVec3(0, 10.0f, 20.0f), Quat::sRotation(Vec3::sAxisX(), 0.25f * JPH_PI), EMotionType::Dynamic, Layers::MOVING), EActivation::Activate);
+
+	// Compound
+	Ref<StaticCompoundShapeSettings> compound_shape2 = new StaticCompoundShapeSettings;
+	Quat rotation = Quat::sRotation(Vec3::sAxisX(), 0.25f * JPH_PI);
+	compound_shape2->AddShape(Vec3(10, 0, 0), rotation, new CapsuleShape(5, 1));
+	compound_shape2->AddShape(rotation * Vec3(10, -5, 0), Quat::sIdentity(), new SphereShape(4));
+	compound_shape2->AddShape(rotation * Vec3(10, 5, 0), Quat::sIdentity(), new SphereShape(2));
+	mBodyInterface->CreateAndAddBody(BodyCreationSettings(compound_shape2, RVec3(0, 10.0f, 40.0f), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING), EActivation::Activate);
+}

lib/All/JoltPhysics/Samples/Tests/General/CenterOfMassTest.h

@@ -0,0 +1,22 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/Test.h>
+
+class CenterOfMassTest : public Test
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, CenterOfMassTest)
+
+	// Description of the test
+	virtual const char *GetDescription() const override
+	{
+		return "Spawns various shapes with the center of mass not in the center of the object.";
+	}
+
+	// See: Test
+	virtual void		Initialize() override;
+};

lib/All/JoltPhysics/Samples/Tests/General/ChangeMotionQualityTest.cpp

@@ -0,0 +1,82 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/General/ChangeMotionQualityTest.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/Body/BodyCreationSettings.h>
+#include <Layers.h>
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(ChangeMotionQualityTest)
+{
+	JPH_ADD_BASE_CLASS(ChangeMotionQualityTest, Test)
+}
+
+void ChangeMotionQualityTest::Initialize()
+{
+	// Floor
+	CreateFloor();
+
+	// Single shape that has 4 walls to surround fast moving sphere
+	BodyCreationSettings enclosing_settings;
+	Ref<BoxShapeSettings> box_shape = new BoxShapeSettings(Vec3(5.0f, 1.0f, 0.1f));
+	Ref<StaticCompoundShapeSettings> enclosing_shape = new StaticCompoundShapeSettings();
+	enclosing_shape->AddShape(Vec3(0, 0, 5), Quat::sIdentity(), box_shape);
+	enclosing_shape->AddShape(Vec3(0, 0, -5), Quat::sIdentity(), box_shape);
+	enclosing_shape->AddShape(Vec3(5, 0, 0), Quat::sRotation(Vec3::sAxisY(), 0.5f * JPH_PI), box_shape);
+	enclosing_shape->AddShape(Vec3(-5, 0, 0), Quat::sRotation(Vec3::sAxisY(), 0.5f * JPH_PI), box_shape);
+	enclosing_settings.SetShapeSettings(enclosing_shape);
+	enclosing_settings.mMotionType = EMotionType::Kinematic;
+	enclosing_settings.mObjectLayer = Layers::MOVING;
+	enclosing_settings.mPosition = RVec3(0, 1, 0);
+	mBodyInterface->CreateAndAddBody(enclosing_settings, EActivation::Activate);
+
+	// Create high speed sphere inside
+	BodyCreationSettings settings;
+	settings.SetShape(new SphereShape(1.0f));
+	settings.mPosition = RVec3(0, 0.5f, 0);
+	settings.mMotionType = EMotionType::Dynamic;
+	settings.mLinearVelocity = Vec3(-240, 0, -120);
+	settings.mFriction = 0.0f;
+	settings.mRestitution = 1.0f;
+	settings.mObjectLayer = Layers::MOVING;
+	mBody = mBodyInterface->CreateBody(settings);
+	mBodyInterface->AddBody(mBody->GetID(), EActivation::Activate);
+
+	UpdateMotionQuality();
+}
+
+void ChangeMotionQualityTest::UpdateMotionQuality()
+{
+	static EMotionQuality qualities[] = { EMotionQuality::LinearCast, EMotionQuality::Discrete };
+	static const char *labels[] = { "LinearCast", "Discrete" };
+
+	// Calculate desired motion quality
+	int idx = int(mTime) & 1;
+	mBodyInterface->SetMotionQuality(mBody->GetID(), qualities[idx]);
+	SetBodyLabel(mBody->GetID(), labels[idx]);
+}
+
+void ChangeMotionQualityTest::PrePhysicsUpdate(const PreUpdateParams &inParams)
+{
+	// Increment time
+	mTime += inParams.mDeltaTime;
+
+	UpdateMotionQuality();
+}
+
+void ChangeMotionQualityTest::SaveState(StateRecorder &inStream) const
+{
+	inStream.Write(mTime);
+}
+
+void ChangeMotionQualityTest::RestoreState(StateRecorder &inStream)
+{
+	inStream.Read(mTime);
+
+	UpdateMotionQuality();
+}

lib/All/JoltPhysics/Samples/Tests/General/ChangeMotionQualityTest.h

@@ -0,0 +1,33 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/Test.h>
+#include <Jolt/Physics/Body/Body.h>
+
+class ChangeMotionQualityTest : public Test
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, ChangeMotionQualityTest)
+
+	// Description of the test
+	virtual const char *GetDescription() const override
+	{
+		return	"Switches a body's motion quality from linear to discrete.\n"
+				"After the switch, the high speed body passes through the wall.";
+	}
+
+	// See: Test
+	virtual void	Initialize() override;
+	virtual void	PrePhysicsUpdate(const PreUpdateParams &inParams) override;
+	virtual void	SaveState(StateRecorder &inStream) const override;
+	virtual void	RestoreState(StateRecorder &inStream) override;
+
+private:
+	void			UpdateMotionQuality();
+
+	Body *			mBody = nullptr;
+	float			mTime = 0.0f;
+};

lib/All/JoltPhysics/Samples/Tests/General/ChangeMotionTypeTest.cpp

@@ -0,0 +1,75 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/General/ChangeMotionTypeTest.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Collision/Shape/SphereShape.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Layers.h>
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(ChangeMotionTypeTest)
+{
+	JPH_ADD_BASE_CLASS(ChangeMotionTypeTest, Test)
+}
+
+void ChangeMotionTypeTest::Initialize()
+{
+	// Floor
+	CreateFloor();
+
+	// Create body as static, but allow to become dynamic
+	BodyCreationSettings settings;
+	settings.SetShape(new BoxShape(Vec3(0.5f, 1.0f, 2.0f)));
+	settings.mPosition = RVec3(0, 10, 0);
+	settings.mMotionType = EMotionType::Static;
+	settings.mObjectLayer = Layers::MOVING; // Put in moving layer, this will result in some overhead when the body is static
+	settings.mAllowDynamicOrKinematic = true;
+	mBody = mBodyInterface->CreateBody(settings);
+	mBodyInterface->AddBody(mBody->GetID(), EActivation::Activate);
+
+	UpdateMotionType();
+}
+
+void ChangeMotionTypeTest::UpdateMotionType()
+{
+	static const EMotionType cycle[] = { EMotionType::Dynamic, EMotionType::Kinematic, EMotionType::Static, EMotionType::Kinematic, EMotionType::Dynamic, EMotionType::Static };
+	static const char *label[] = { "Dynamic", "Kinematic", "Static", "Kinematic", "Dynamic", "Static" };
+
+	// Calculate desired motion type
+	int idx = int(mTime) % size(cycle);
+	EMotionType motion_type = cycle[idx];
+
+	// Update motion type and reactivate the body
+	if (motion_type != mBody->GetMotionType())
+	{
+		mBodyInterface->SetMotionType(mBody->GetID(), motion_type, EActivation::Activate);
+		SetBodyLabel(mBody->GetID(), label[idx]);
+	}
+}
+
+void ChangeMotionTypeTest::PrePhysicsUpdate(const PreUpdateParams &inParams)
+{
+	// Increment time
+	mTime += inParams.mDeltaTime;
+
+	UpdateMotionType();
+
+	// Provide kinematic body a target
+	if (mBody->IsKinematic())
+		mBody->MoveKinematic(RVec3(Sin(mTime), 10, 0), Quat::sRotation(Vec3::sAxisX(), Cos(mTime)), inParams.mDeltaTime);
+}
+
+void ChangeMotionTypeTest::SaveState(StateRecorder &inStream) const
+{
+	inStream.Write(mTime);
+}
+
+void ChangeMotionTypeTest::RestoreState(StateRecorder &inStream)
+{
+	inStream.Read(mTime);
+
+	UpdateMotionType();
+}

lib/All/JoltPhysics/Samples/Tests/General/ChangeMotionTypeTest.h

@@ -0,0 +1,32 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/Test.h>
+#include <Jolt/Physics/Body/Body.h>
+
+class ChangeMotionTypeTest : public Test
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, ChangeMotionTypeTest)
+
+	// Description of the test
+	virtual const char *GetDescription() const override
+	{
+		return "Switches a body's motion type between static, kinematic and dynamic.";
+	}
+
+	// See: Test
+	virtual void	Initialize() override;
+	virtual void	PrePhysicsUpdate(const PreUpdateParams &inParams) override;
+	virtual void	SaveState(StateRecorder &inStream) const override;
+	virtual void	RestoreState(StateRecorder &inStream) override;
+
+private:
+	void			UpdateMotionType();
+
+	Body *			mBody = nullptr;
+	float			mTime = 0.0f;
+};

lib/All/JoltPhysics/Samples/Tests/General/ChangeObjectLayerTest.cpp

@@ -0,0 +1,81 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/General/ChangeObjectLayerTest.h>
+#include <Layers.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(ChangeObjectLayerTest)
+{
+	JPH_ADD_BASE_CLASS(ChangeObjectLayerTest, Test)
+}
+
+void ChangeObjectLayerTest::Initialize()
+{
+	// Floor
+	CreateFloor();
+
+	// A dynamic box in the MOVING layer
+	mMoving = mBodyInterface->CreateAndAddBody(BodyCreationSettings(new BoxShape(Vec3(5, 0.1f, 5)), RVec3(0, 1.5f, 0), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING), EActivation::Activate);
+
+	// Lots of dynamic objects in the DEBRIS layer
+	default_random_engine random;
+	uniform_real_distribution<float> position_variation(-10, 10);
+	for (int i = 0; i < 50; ++i)
+	{
+		RVec3 position(position_variation(random), 2.0f, position_variation(random));
+		Quat rotation = Quat::sRandom(random);
+		mDebris.push_back(mBodyInterface->CreateAndAddBody(BodyCreationSettings(new BoxShape(Vec3::sReplicate(0.1f)), position, rotation, EMotionType::Dynamic, Layers::DEBRIS), EActivation::Activate));
+	}
+}
+
+void ChangeObjectLayerTest::PrePhysicsUpdate(const PreUpdateParams &inParams)
+{
+	const float cSwitchTime = 2.0f;
+
+	// Increment time
+	mTime += inParams.mDeltaTime;
+
+	if (mTime >= cSwitchTime)
+	{
+		mIsDebris = !mIsDebris;
+
+		// Reposition moving object
+		mBodyInterface->SetPosition(mMoving, RVec3(0, 1.5f, 0), EActivation::Activate);
+
+		default_random_engine random;
+		uniform_real_distribution<float> position_variation(-7.5f, 7.5f);
+		for (BodyID id : mDebris)
+		{
+			// Reposition debris
+			RVec3 position(position_variation(random), 2.0f, position_variation(random));
+			Quat rotation = Quat::sRandom(random);
+			mBodyInterface->SetPositionAndRotation(id, position, rotation, EActivation::Activate);
+
+			// And update layer
+			mBodyInterface->SetObjectLayer(id, mIsDebris? Layers::DEBRIS : Layers::MOVING);
+		}
+
+		mTime = 0;
+	}
+}
+
+void ChangeObjectLayerTest::SaveState(StateRecorder &inStream) const
+{
+	inStream.Write(mTime);
+	inStream.Write(mIsDebris);
+}
+
+void ChangeObjectLayerTest::RestoreState(StateRecorder &inStream)
+{
+	inStream.Read(mTime);
+	inStream.Read(mIsDebris);
+
+	// Restore layer
+	for (BodyID id : mDebris)
+		mBodyInterface->SetObjectLayer(id, mIsDebris? Layers::DEBRIS : Layers::MOVING);
+}

lib/All/JoltPhysics/Samples/Tests/General/ChangeObjectLayerTest.h

@@ -0,0 +1,37 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/Test.h>
+#include <Jolt/Physics/Body/BodyID.h>
+
+class ChangeObjectLayerTest : public Test
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, ChangeObjectLayerTest)
+
+	// Description of the test
+	virtual const char *	GetDescription() const override
+	{
+		return	"Demonstrates how to use layers to disable collisions with other objects and how to change layers on the fly.\n"
+				"The small cubes will switch between the MOVING layer and the DEBRIS layer (debris only collides with the static floor).";
+	}
+
+	// Initialize the test
+	virtual void			Initialize() override;
+
+	// Update the test, called before the physics update
+	virtual void			PrePhysicsUpdate(const PreUpdateParams &inParams) override;
+
+	// Saving / restoring state for replay
+	virtual void			SaveState(StateRecorder &inStream) const override;
+	virtual void			RestoreState(StateRecorder &inStream) override;
+
+private:
+	BodyID					mMoving;
+	BodyIDVector			mDebris;
+	bool					mIsDebris = true;
+	float					mTime = 0.0f;
+};

lib/All/JoltPhysics/Samples/Tests/General/ChangeShapeTest.cpp

@@ -0,0 +1,87 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/General/ChangeShapeTest.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/TaperedCapsuleShape.h>
+#include <Jolt/Physics/Collision/Shape/StaticCompoundShape.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Application/DebugUI.h>
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(ChangeShapeTest)
+{
+	JPH_ADD_BASE_CLASS(ChangeShapeTest, Test)
+}
+
+void ChangeShapeTest::Initialize()
+{
+	// Floor
+	CreateFloor();
+
+	// Shapes
+	mShapes.push_back(new BoxShape(Vec3(0.5f, 1.5f, 0.5f)));
+	mShapes.push_back(new SphereShape(0.5f));
+	mShapes.push_back(new CapsuleShape(1.0f, 0.5f));
+	mShapes.push_back(TaperedCapsuleShapeSettings(1.0f, 0.5f, 0.3f).Create().Get());
+
+	// Compound with center of mass shifted (this requires a correction of the position in the body)
+	StaticCompoundShapeSettings compound_settings;
+	compound_settings.AddShape(Vec3(0, 1.5f, 0), Quat::sIdentity(), new CapsuleShape(1.5f, 0.5f));
+	compound_settings.AddShape(Vec3(0, 3, 0), Quat::sIdentity(), new SphereShape(1));
+	RefConst<Shape> compound = compound_settings.Create().Get();
+	mShapes.push_back(compound);
+
+	// Create dynamic body that changes shape
+	BodyCreationSettings settings;
+	settings.SetShape(mShapes[mShapeIdx]);
+	settings.mPosition = RVec3(0, 10, 0);
+	settings.mMotionType = EMotionType::Dynamic;
+	settings.mObjectLayer = Layers::MOVING;
+	mBodyID = mBodyInterface->CreateBody(settings)->GetID();
+	mBodyInterface->AddBody(mBodyID, EActivation::Activate);
+}
+
+void ChangeShapeTest::PrePhysicsUpdate(const PreUpdateParams &inParams)
+{
+	const float cSwitchTime = 3.0f;
+
+	// Increment time
+	mTime += inParams.mDeltaTime;
+
+	// Get new shape
+	int shape_idx = int(mTime / cSwitchTime) % mShapes.size();
+
+	// Change shape
+	if (mShapeIdx != shape_idx)
+	{
+		mShapeIdx = shape_idx;
+		mBodyInterface->SetShape(mBodyID, mShapes[mShapeIdx], true, mActivateAfterSwitch? EActivation::Activate : EActivation::DontActivate);
+	}
+}
+
+void ChangeShapeTest::SaveState(StateRecorder &inStream) const
+{
+	inStream.Write(mTime);
+	inStream.Write(mShapeIdx);
+}
+
+void ChangeShapeTest::RestoreState(StateRecorder &inStream)
+{
+	inStream.Read(mTime);
+	inStream.Read(mShapeIdx);
+
+	// Reset the shape to what was stored
+	mBodyInterface->SetShape(mBodyID, mShapes[mShapeIdx], true, EActivation::DontActivate);
+}
+
+void ChangeShapeTest::CreateSettingsMenu(DebugUI *inUI, UIElement *inSubMenu)
+{
+	inUI->CreateCheckBox(inSubMenu, "Activate Body After Switch", mActivateAfterSwitch, [this](UICheckBox::EState inState) { mActivateAfterSwitch = inState == UICheckBox::STATE_CHECKED; });
+}
+

lib/All/JoltPhysics/Samples/Tests/General/ChangeShapeTest.h

@@ -0,0 +1,42 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/Test.h>
+#include <Jolt/Physics/Body/BodyID.h>
+#include <Jolt/Physics/Collision/Shape/Shape.h>
+
+class ChangeShapeTest : public Test
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, ChangeShapeTest)
+
+	// Description of the test
+	virtual const char *	GetDescription() const override
+	{
+		return "Demonstrates how to dynamically update the shape of a body.";
+	}
+
+	// Initialize the test
+	virtual void			Initialize() override;
+
+	// Update the test, called before the physics update
+	virtual void			PrePhysicsUpdate(const PreUpdateParams &inParams) override;
+
+	// Saving / restoring state for replay
+	virtual void			SaveState(StateRecorder &inStream) const override;
+	virtual void			RestoreState(StateRecorder &inStream) override;
+
+	// Optional settings menu
+	virtual bool			HasSettingsMenu() const override							{ return true; }
+	virtual void			CreateSettingsMenu(DebugUI *inUI, UIElement *inSubMenu) override;
+
+private:
+	bool					mActivateAfterSwitch = true;
+	BodyID					mBodyID;
+	Array<RefConst<Shape>>	mShapes;
+	float					mTime = 0.0f;
+	int						mShapeIdx = 0;
+};

lib/All/JoltPhysics/Samples/Tests/General/ContactListenerTest.cpp

@@ -0,0 +1,151 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/General/ContactListenerTest.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/StaticCompoundShape.h>
+#include <Jolt/Physics/Collision/EstimateCollisionResponse.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Layers.h>
+#include <Renderer/DebugRendererImp.h>
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(ContactListenerTest)
+{
+	JPH_ADD_BASE_CLASS(ContactListenerTest, Test)
+}
+
+void ContactListenerTest::Initialize()
+{
+	// Floor
+	CreateFloor();
+
+	RefConst<Shape> box_shape = new BoxShape(Vec3(0.5f, 1.0f, 2.0f));
+
+	// Dynamic body 1, this body will have restitution 1 for new contacts and restitution 0 for persisting contacts
+	Body &body1 = *mBodyInterface->CreateBody(BodyCreationSettings(box_shape, RVec3(0, 10, 0), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING));
+	body1.SetAllowSleeping(false);
+	mBodyInterface->AddBody(body1.GetID(), EActivation::Activate);
+
+	// Dynamic body 2
+	Body &body2 = *mBodyInterface->CreateBody(BodyCreationSettings(box_shape, RVec3(5, 10, 0), Quat::sRotation(Vec3::sAxisX(), 0.25f * JPH_PI), EMotionType::Dynamic, Layers::MOVING));
+	body2.SetAllowSleeping(false);
+	mBodyInterface->AddBody(body2.GetID(), EActivation::Activate);
+
+	// Dynamic body 3
+	Body &body3 = *mBodyInterface->CreateBody(BodyCreationSettings(new SphereShape(2.0f), RVec3(10, 10, 0), Quat::sRotation(Vec3::sAxisX(), 0.25f * JPH_PI), EMotionType::Dynamic, Layers::MOVING));
+	body3.SetAllowSleeping(false);
+	mBodyInterface->AddBody(body3.GetID(), EActivation::Activate);
+
+	// Dynamic body 4
+	Ref<StaticCompoundShapeSettings> compound_shape = new StaticCompoundShapeSettings;
+	compound_shape->AddShape(Vec3::sZero(), Quat::sIdentity(), new CapsuleShape(5, 1));
+	compound_shape->AddShape(Vec3(0, -5, 0), Quat::sIdentity(), new SphereShape(2));
+	compound_shape->AddShape(Vec3(0, 5, 0), Quat::sIdentity(), new SphereShape(2));
+	Body &body4 = *mBodyInterface->CreateBody(BodyCreationSettings(compound_shape, RVec3(15, 10, 0), Quat::sRotation(Vec3::sAxisX(), 0.25f * JPH_PI), EMotionType::Dynamic, Layers::MOVING));
+	body4.SetAllowSleeping(false);
+	mBodyInterface->AddBody(body4.GetID(), EActivation::Activate);
+
+	// Dynamic body 5, a cube with a bigger cube surrounding it that acts as a sensor
+	Ref<StaticCompoundShapeSettings> compound_shape2 = new StaticCompoundShapeSettings;
+	compound_shape2->AddShape(Vec3::sZero(), Quat::sIdentity(), new BoxShape(Vec3::sReplicate(1)));
+	compound_shape2->AddShape(Vec3::sZero(), Quat::sIdentity(), new BoxShape(Vec3::sReplicate(2))); // This will become a sensor in the contact callback
+	BodyCreationSettings bcs5(compound_shape2, RVec3(20, 10, 0), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING);
+	bcs5.mUseManifoldReduction = false; // Needed in order to prevent the physics system from combining contacts between sensor and non-sensor sub shapes
+	Body &body5 = *mBodyInterface->CreateBody(bcs5);
+	body5.SetAllowSleeping(false);
+	mBodyInterface->AddBody(body5.GetID(), EActivation::Activate);
+
+	// Store bodies for later use
+	mBody[0] = &body1;
+	mBody[1] = &body2;
+	mBody[2] = &body3;
+	mBody[3] = &body4;
+	mBody[4] = &body5;
+}
+
+void ContactListenerTest::PostPhysicsUpdate(float inDeltaTime)
+{
+	// Check that predicted velocities match actual velocities
+	lock_guard lock(mPredictedVelocitiesMutex);
+	for (const PredictedVelocity &v : mPredictedVelocities)
+	{
+		BodyLockRead body_lock(mPhysicsSystem->GetBodyLockInterface(), v.mBodyID);
+		if (body_lock.Succeeded())
+		{
+			const Body &body = body_lock.GetBody();
+			Vec3 linear_velocity = body.GetLinearVelocity();
+			Vec3 angular_velocity = body.GetAngularVelocity();
+			float diff_v = (v.mLinearVelocity - linear_velocity).Length();
+			float diff_w = (v.mAngularVelocity - angular_velocity).Length();
+			if (diff_v > 1.0e-3f || diff_w > 1.0e-3f)
+				Trace("Mispredicted collision for body: %08x, v=%s, w=%s, predicted_v=%s, predicted_w=%s, diff_v=%f, diff_w=%f",
+					body.GetID().GetIndex(),
+					ConvertToString(linear_velocity).c_str(), ConvertToString(angular_velocity).c_str(),
+					ConvertToString(v.mLinearVelocity).c_str(), ConvertToString(v.mAngularVelocity).c_str(),
+					(double)diff_v,
+					(double)diff_w);
+		}
+	}
+	mPredictedVelocities.clear();
+}
+
+ValidateResult ContactListenerTest::OnContactValidate(const Body &inBody1, const Body &inBody2, RVec3Arg inBaseOffset, const CollideShapeResult &inCollisionResult)
+{
+	// Body 1 and 2 should never collide
+	return ((&inBody1 == mBody[0] && &inBody2 == mBody[1]) || (&inBody1 == mBody[1] && &inBody2 == mBody[0]))? ValidateResult::RejectAllContactsForThisBodyPair : ValidateResult::AcceptAllContactsForThisBodyPair;
+}
+
+void ContactListenerTest::MakeBody5PartialSensor(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings)
+{
+	// Make the 2nd shape of body 5 a sensor
+	SubShapeID body5_subshape_1 = StaticCast<CompoundShape>(mBody[4]->GetShape())->GetSubShapeIDFromIndex(1, SubShapeIDCreator()).GetID();
+	if ((&inBody1 == mBody[4] && inManifold.mSubShapeID1 == body5_subshape_1)
+		|| (&inBody2 == mBody[4] && inManifold.mSubShapeID2 == body5_subshape_1))
+	{
+		Trace("Sensor contact detected between body %08x and body %08x", inBody1.GetID().GetIndexAndSequenceNumber(), inBody2.GetID().GetIndexAndSequenceNumber());
+		ioSettings.mIsSensor = true;
+	}
+}
+
+void ContactListenerTest::OnContactAdded(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings)
+{
+	// Make body 1 bounce only when a new contact point is added but not when it is persisted (its restitution is normally 0)
+	if (&inBody1 == mBody[0] || &inBody2 == mBody[0])
+	{
+		JPH_ASSERT(ioSettings.mCombinedRestitution == 0.0f);
+		ioSettings.mCombinedRestitution = 1.0f;
+	}
+
+	MakeBody5PartialSensor(inBody1, inBody2, inManifold, ioSettings);
+
+	// Estimate the contact impulses.
+	CollisionEstimationResult result;
+	EstimateCollisionResponse(inBody1, inBody2, inManifold, result, ioSettings.mCombinedFriction, ioSettings.mCombinedRestitution);
+
+	// Trace the result
+	String impulses_str;
+	for (const CollisionEstimationResult::Impulse &impulse : result.mImpulses)
+		impulses_str += StringFormat("(%f, %f, %f) ", (double)impulse.mContactImpulse, (double)impulse.mFrictionImpulse1, (double)impulse.mFrictionImpulse2);
+
+	Trace("Estimated velocity after collision, body1: %08x, v=%s, w=%s, body2: %08x, v=%s, w=%s, impulses: %s",
+		inBody1.GetID().GetIndex(), ConvertToString(result.mLinearVelocity1).c_str(), ConvertToString(result.mAngularVelocity1).c_str(),
+		inBody2.GetID().GetIndex(), ConvertToString(result.mLinearVelocity2).c_str(), ConvertToString(result.mAngularVelocity2).c_str(),
+		impulses_str.c_str());
+
+	// Log predicted velocities
+	{
+		lock_guard lock(mPredictedVelocitiesMutex);
+		mPredictedVelocities.push_back({ inBody1.GetID(), result.mLinearVelocity1, result.mAngularVelocity1 });
+		mPredictedVelocities.push_back({ inBody2.GetID(), result.mLinearVelocity2, result.mAngularVelocity2 });
+	}
+}
+
+void ContactListenerTest::OnContactPersisted(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings)
+{
+	MakeBody5PartialSensor(inBody1, inBody2, inManifold, ioSettings);
+}

lib/All/JoltPhysics/Samples/Tests/General/ContactListenerTest.h

@@ -0,0 +1,51 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/Test.h>
+#include <Jolt/Physics/Collision/ContactListener.h>
+
+class ContactListenerTest : public Test, public ContactListener
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, ContactListenerTest)
+
+	// Description of the test
+	virtual const char *	GetDescription() const override
+	{
+		return	"Demonstrates how to listen for contact events.\n"
+				"Leftmost box ignores contacts with the 2nd box and overrides the restitution to 1 for non-persisted contacts.\n"
+				"Rightmost box contains an inner and an outer shape, the outer shape acts as a sensor.\n"
+				"The TTY will output estimated post collision velocities.";
+	}
+
+	// See: Test
+	virtual void			Initialize() override;
+	virtual void			PostPhysicsUpdate(float inDeltaTime) override;
+
+	// If this test implements a contact listener, it should be returned here
+	virtual ContactListener *GetContactListener() override		{ return this; }
+
+	// See: ContactListener
+	virtual ValidateResult	OnContactValidate(const Body &inBody1, const Body &inBody2, RVec3Arg inBaseOffset, const CollideShapeResult &inCollisionResult) override;
+	virtual void			OnContactAdded(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings) override;
+	virtual void			OnContactPersisted(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings) override;
+
+private:
+	void					MakeBody5PartialSensor(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings);
+
+	// The 5 bodies that we create
+	Body *					mBody[5];
+
+	// Tracks predicted velocities so we can compare them with the actual velocities after time step
+	struct PredictedVelocity
+	{
+		BodyID				mBodyID;
+		Vec3				mLinearVelocity;
+		Vec3				mAngularVelocity;
+	};
+	Mutex					mPredictedVelocitiesMutex;
+	Array<PredictedVelocity> mPredictedVelocities;
+};

lib/All/JoltPhysics/Samples/Tests/General/ContactManifoldTest.cpp

@@ -0,0 +1,36 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/General/ContactManifoldTest.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Collision/Shape/CapsuleShape.h>
+#include <Layers.h>
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(ContactManifoldTest)
+{
+	JPH_ADD_BASE_CLASS(ContactManifoldTest, Test)
+}
+
+void ContactManifoldTest::Initialize()
+{
+	// Floor
+	CreateFloor();
+
+	RefConst<Shape> big_box = new BoxShape(Vec3(4, 4, 4), 0.0f);
+	RefConst<Shape> capsule = new CapsuleShape(5, 2);
+	RefConst<Shape> long_box = new BoxShape(Vec3(2, 7, 2));
+
+	for (int i = 0; i < 3; ++i)
+		for (int j = 0; j < 2; ++j)
+		{
+			// Create a box
+			mBodyInterface->CreateAndAddBody(BodyCreationSettings(big_box, RVec3(-20.0f + i * 10.0f, 4, -20.0f + j * 40.0f), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING), EActivation::DontActivate);
+
+			// Place a dynamic body on it
+			mBodyInterface->CreateAndAddBody(BodyCreationSettings(j == 0? capsule : long_box, RVec3(-20.0f + i * 10.0f, 12, -5.0f + i * 5.0f - 20.0f + j * 40.0f), Quat::sRotation(Vec3::sAxisY(), 0.1f * JPH_PI) * Quat::sRotation(Vec3::sAxisX(), 0.5f * JPH_PI), EMotionType::Dynamic, Layers::MOVING), EActivation::Activate);
+		}
+}

lib/All/JoltPhysics/Samples/Tests/General/ContactManifoldTest.h

@@ -0,0 +1,22 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/Test.h>
+
+class ContactManifoldTest : public Test
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, ContactManifoldTest)
+
+	// Description of the test
+	virtual const char *GetDescription() const override
+	{
+		return "Spawns objects at an angle to test if the contact manifold is calculated correctly.";
+	}
+
+	// See: Test
+	virtual void		Initialize() override;
+};

lib/All/JoltPhysics/Samples/Tests/General/ConveyorBeltTest.cpp

@@ -0,0 +1,111 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/General/ConveyorBeltTest.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Collision/Shape/CylinderShape.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Layers.h>
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(ConveyorBeltTest)
+{
+	JPH_ADD_BASE_CLASS(ConveyorBeltTest, Test)
+}
+
+void ConveyorBeltTest::Initialize()
+{
+	// Floor
+	CreateFloor();
+
+	// Create conveyor belts
+	const float cBeltWidth = 10.0f;
+	const float cBeltLength = 50.0f;
+	BodyCreationSettings belt_settings(new BoxShape(Vec3(cBeltWidth, 0.1f, cBeltLength)), RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING);
+	for (int i = 0; i < 4; ++i)
+	{
+		belt_settings.mFriction = 0.25f * (i + 1);
+		belt_settings.mRotation = Quat::sRotation(Vec3::sAxisY(), 0.5f * JPH_PI * i) * Quat::sRotation(Vec3::sAxisX(), DegreesToRadians(1.0f));
+		belt_settings.mPosition = RVec3(belt_settings.mRotation * Vec3(cBeltLength, 6.0f, cBeltWidth));
+		mLinearBelts.push_back(mBodyInterface->CreateAndAddBody(belt_settings, EActivation::DontActivate));
+	}
+
+	// Bodies with decreasing friction
+	BodyCreationSettings cargo_settings(new BoxShape(Vec3::sReplicate(2.0f)), RVec3::sZero(), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING);
+	for (int i = 0; i <= 10; ++i)
+	{
+		cargo_settings.mPosition = RVec3(-cBeltLength + i * 10.0f, 10.0f, -cBeltLength);
+		cargo_settings.mFriction = max(0.0f, 1.0f - 0.1f * i);
+		mBodyInterface->CreateAndAddBody(cargo_settings, EActivation::Activate);
+	}
+
+	// Create 2 cylinders
+	BodyCreationSettings cylinder_settings(new CylinderShape(6.0f, 1.0f), RVec3(-25.0f, 1.0f, -20.0f), Quat::sRotation(Vec3::sAxisZ(), 0.5f * JPH_PI), EMotionType::Dynamic, Layers::MOVING);
+	mBodyInterface->CreateAndAddBody(cylinder_settings, EActivation::Activate);
+	cylinder_settings.mPosition.SetZ(20.0f);
+	mBodyInterface->CreateAndAddBody(cylinder_settings, EActivation::Activate);
+
+	// Let a dynamic belt rest on it
+	BodyCreationSettings dynamic_belt(new BoxShape(Vec3(5.0f, 0.1f, 25.0f), 0.0f), RVec3(-25.0f, 3.0f, 0), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING);
+	mLinearBelts.push_back(mBodyInterface->CreateAndAddBody(dynamic_belt, EActivation::Activate));
+
+	// Create cargo on the dynamic belt
+	cargo_settings.mPosition = RVec3(-25.0f, 6.0f, 15.0f);
+	cargo_settings.mFriction = 1.0f;
+	mBodyInterface->CreateAndAddBody(cargo_settings, EActivation::Activate);
+
+	// Create an angular belt
+	BodyCreationSettings angular_belt(new BoxShape(Vec3(20.0f, 0.1f, 20.0f), 0.0f), RVec3(10.0f, 3.0f, 0), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING);
+	mAngularBelt = mBodyInterface->CreateAndAddBody(angular_belt, EActivation::Activate);
+
+	// Bodies with decreasing friction dropping on the angular belt
+	for (int i = 0; i <= 6; ++i)
+	{
+		cargo_settings.mPosition = RVec3(10.0f, 10.0f, -15.0f + 5.0f * i);
+		cargo_settings.mFriction = max(0.0f, 1.0f - 0.1f * i);
+		mBodyInterface->CreateAndAddBody(cargo_settings, EActivation::Activate);
+	}
+}
+
+void ConveyorBeltTest::OnContactAdded(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings)
+{
+	// Linear belts
+	bool body1_linear_belt = std::find(mLinearBelts.begin(), mLinearBelts.end(), inBody1.GetID()) != mLinearBelts.end();
+	bool body2_linear_belt = std::find(mLinearBelts.begin(), mLinearBelts.end(), inBody2.GetID()) != mLinearBelts.end();
+	if (body1_linear_belt || body2_linear_belt)
+	{
+		// Determine the world space surface velocity of both bodies
+		const Vec3 cLocalSpaceVelocity(0, 0, -10.0f);
+		Vec3 body1_linear_surface_velocity = body1_linear_belt? inBody1.GetRotation() * cLocalSpaceVelocity : Vec3::sZero();
+		Vec3 body2_linear_surface_velocity = body2_linear_belt? inBody2.GetRotation() * cLocalSpaceVelocity : Vec3::sZero();
+
+		// Calculate the relative surface velocity
+		ioSettings.mRelativeLinearSurfaceVelocity = body2_linear_surface_velocity - body1_linear_surface_velocity;
+	}
+
+	// Angular belt
+	bool body1_angular = inBody1.GetID() == mAngularBelt;
+	bool body2_angular = inBody2.GetID() == mAngularBelt;
+	if (body1_angular || body2_angular)
+	{
+		// Determine the world space angular surface velocity of both bodies
+		const Vec3 cLocalSpaceAngularVelocity(0, DegreesToRadians(10.0f), 0);
+		Vec3 body1_angular_surface_velocity = body1_angular? inBody1.GetRotation() * cLocalSpaceAngularVelocity : Vec3::sZero();
+		Vec3 body2_angular_surface_velocity = body2_angular? inBody2.GetRotation() * cLocalSpaceAngularVelocity : Vec3::sZero();
+
+		// Note that the angular velocity is the angular velocity around body 1's center of mass, so we need to add the linear velocity of body 2's center of mass
+		Vec3 body2_linear_surface_velocity = body2_angular? body2_angular_surface_velocity.Cross(Vec3(inBody1.GetCenterOfMassPosition() - inBody2.GetCenterOfMassPosition())) : Vec3::sZero();
+
+		// Calculate the relative angular surface velocity
+		ioSettings.mRelativeLinearSurfaceVelocity = body2_linear_surface_velocity;
+		ioSettings.mRelativeAngularSurfaceVelocity = body2_angular_surface_velocity - body1_angular_surface_velocity;
+	}
+}
+
+void ConveyorBeltTest::OnContactPersisted(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings)
+{
+	// Same behavior as contact added
+	OnContactAdded(inBody1, inBody2, inManifold, ioSettings);
+}

lib/All/JoltPhysics/Samples/Tests/General/ConveyorBeltTest.h

@@ -0,0 +1,33 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/Test.h>
+
+class ConveyorBeltTest : public Test, public ContactListener
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, ConveyorBeltTest)
+
+	// Description of the test
+	virtual const char *	GetDescription() const override
+	{
+		return "Demonstrates how to use a contact listener to implement a conveyor belt.";
+	}
+
+	// See: Test
+	virtual void			Initialize() override;
+
+	// If this test implements a contact listener, it should be returned here
+	virtual ContactListener *GetContactListener() override		{ return this; }
+
+	// See: ContactListener
+	virtual void			OnContactAdded(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings) override;
+	virtual void			OnContactPersisted(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings) override;
+
+private:
+	BodyIDVector			mLinearBelts;
+	BodyID					mAngularBelt;
+};

lib/All/JoltPhysics/Samples/Tests/General/DampingTest.cpp

@@ -0,0 +1,44 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/General/DampingTest.h>
+#include <Jolt/Physics/Collision/Shape/SphereShape.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Layers.h>
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(DampingTest)
+{
+	JPH_ADD_BASE_CLASS(DampingTest, Test)
+}
+
+void DampingTest::Initialize()
+{
+	// Floor
+	CreateFloor();
+
+	RefConst<Shape> sphere = new SphereShape(2.0f);
+
+	// Bodies with increasing damping
+	for (int i = 0; i <= 10; ++i)
+	{
+		Body &body = *mBodyInterface->CreateBody(BodyCreationSettings(sphere, RVec3(-50.0f + i * 10.0f, 2.0f, -80.0f), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING));
+		body.GetMotionProperties()->SetAngularDamping(0.0f);
+		body.GetMotionProperties()->SetLinearDamping(0.1f * i);
+		body.SetLinearVelocity(Vec3(0, 0, 10));
+		mBodyInterface->AddBody(body.GetID(), EActivation::Activate);
+		SetBodyLabel(body.GetID(), StringFormat("Linear damping: %.1f", double(body.GetMotionProperties()->GetLinearDamping())));
+	}
+
+	for (int i = 0; i <= 10; ++i)
+	{
+		Body &body = *mBodyInterface->CreateBody(BodyCreationSettings(sphere, RVec3(-50.0f + i * 10.0f, 2.0f, -90.0f), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING));
+		body.GetMotionProperties()->SetLinearDamping(0.0f);
+		body.GetMotionProperties()->SetAngularDamping(0.1f * i);
+		body.SetAngularVelocity(Vec3(0, 10, 0));
+		mBodyInterface->AddBody(body.GetID(), EActivation::Activate);
+		SetBodyLabel(body.GetID(), StringFormat("Angular damping: %.1f", double(body.GetMotionProperties()->GetAngularDamping())));
+	}
+}

lib/All/JoltPhysics/Samples/Tests/General/DampingTest.h

@@ -0,0 +1,22 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/Test.h>
+
+class DampingTest : public Test
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, DampingTest)
+
+	// Description of the test
+	virtual const char *GetDescription() const override
+	{
+		return "Tests various values for linear and angular damping.";
+	}
+
+	// See: Test
+	virtual void		Initialize() override;
+};

lib/All/JoltPhysics/Samples/Tests/General/DynamicMeshTest.cpp

@@ -0,0 +1,46 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/General/DynamicMeshTest.h>
+#include <Jolt/Physics/Collision/Shape/MeshShape.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Layers.h>
+#include <Utils/ShapeCreator.h>
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(DynamicMeshTest)
+{
+	JPH_ADD_BASE_CLASS(DynamicMeshTest, Test)
+}
+
+void DynamicMeshTest::Initialize()
+{
+	// Floor
+	CreateFloor();
+
+	constexpr float cTorusRadius = 3.0f;
+	constexpr float cTubeRadius = 1.0f;
+
+	// Create torus
+	RefConst<Shape> mesh_shape = ShapeCreator::CreateTorusMesh(cTorusRadius, cTubeRadius);
+	BodyCreationSettings settings(mesh_shape, RVec3(0, 10, 0), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING);
+
+	// Mesh cannot calculate its mass, we must provide it
+	settings.mOverrideMassProperties = EOverrideMassProperties::MassAndInertiaProvided;
+	settings.mMassPropertiesOverride.SetMassAndInertiaOfSolidBox(2.0f * Vec3(cTorusRadius, cTubeRadius, cTorusRadius), 1000.0f);
+
+	mBodyInterface->CreateAndAddBody(settings, EActivation::Activate);
+
+	// Wall of blocks
+	RefConst<Shape> box_shape = new BoxShape(Vec3::sReplicate(0.5f));
+	for (int i = 0; i < 7; ++i)
+		for (int j = i / 2; j < 7 - (i + 1) / 2; ++j)
+		{
+			RVec3 position(-3.5f + j * 1.0f + (i & 1? 0.5f : 0.0f), 0.5f + i * 1.0f, 0.0f);
+			Body &wall = *mBodyInterface->CreateBody(BodyCreationSettings(box_shape, position, Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING));
+			mBodyInterface->AddBody(wall.GetID(), EActivation::Activate);
+		}
+}

lib/All/JoltPhysics/Samples/Tests/General/DynamicMeshTest.h

@@ -0,0 +1,23 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/Test.h>
+
+class DynamicMeshTest : public Test
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, DynamicMeshTest)
+
+	// Description of the test
+	virtual const char *GetDescription() const override
+	{
+		return	"Drops a dynamic body with a mesh shape on a pile of boxes.\n"
+				"Note that mesh vs mesh collisions are currently not supported.";
+	}
+
+	// See: Test
+	virtual void		Initialize() override;
+};

lib/All/JoltPhysics/Samples/Tests/General/EnhancedInternalEdgeRemovalTest.cpp

@@ -0,0 +1,236 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2024 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/General/EnhancedInternalEdgeRemovalTest.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Collision/Shape/SphereShape.h>
+#include <Jolt/Physics/Collision/Shape/MeshShape.h>
+#include <Jolt/Physics/Collision/Shape/StaticCompoundShape.h>
+#include <Jolt/Geometry/Triangle.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Layers.h>
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(EnhancedInternalEdgeRemovalTest)
+{
+	JPH_ADD_BASE_CLASS(EnhancedInternalEdgeRemovalTest, Test)
+}
+
+void EnhancedInternalEdgeRemovalTest::CreateSlidingObjects(RVec3Arg inStart)
+{
+	// Slide the shapes over the grid of boxes
+	RVec3 pos = inStart - RVec3(0, 0, 12.0_r);
+	static const char *labels[] = { "Normal", "Enhanced edge removal" };
+	for (int enhanced_removal = 0; enhanced_removal < 2; ++enhanced_removal)
+	{
+		// A box
+		BodyCreationSettings box_bcs(new BoxShape(Vec3::sReplicate(2.0f)), pos, Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING);
+		box_bcs.mLinearVelocity = Vec3(20, 0, 0);
+		box_bcs.mEnhancedInternalEdgeRemoval = enhanced_removal == 1;
+		BodyID id = mBodyInterface->CreateAndAddBody(box_bcs, EActivation::Activate);
+		SetBodyLabel(id, labels[enhanced_removal]);
+		pos += RVec3(0, 0, 5.0_r);
+
+		// A sphere
+		BodyCreationSettings sphere_bcs(new SphereShape(2.0f), pos, Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING);
+		sphere_bcs.mLinearVelocity = Vec3(20, 0, 0);
+		sphere_bcs.mEnhancedInternalEdgeRemoval = enhanced_removal == 1;
+		id = mBodyInterface->CreateAndAddBody(sphere_bcs, EActivation::Activate);
+		SetBodyLabel(id, labels[enhanced_removal]);
+		pos += RVec3(0, 0, 5.0_r);
+
+		// Compound
+		RefConst<Shape> box = new BoxShape(Vec3::sReplicate(0.1f));
+		StaticCompoundShapeSettings compound;
+		compound.SetEmbedded();
+		for (int x = 0; x < 2; ++x)
+			for (int y = 0; y < 2; ++y)
+				for (int z = 0; z < 2; ++z)
+					compound.AddShape(Vec3(x == 0? -1.9f : 1.9f, y == 0? -1.9f : 1.9f, z == 0? -1.9f : 1.9f), Quat::sIdentity(), box);
+		BodyCreationSettings compound_bcs(&compound, pos, Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING);
+		compound_bcs.mLinearVelocity = Vec3(20, 0, 0);
+		compound_bcs.mEnhancedInternalEdgeRemoval = enhanced_removal == 1;
+		id = mBodyInterface->CreateAndAddBody(compound_bcs, EActivation::Activate);
+		SetBodyLabel(id, labels[enhanced_removal]);
+		pos += RVec3(0, 0, 7.0_r);
+	}
+}
+
+void EnhancedInternalEdgeRemovalTest::Initialize()
+{
+	// This test creates a grid of connected boxes and tests that objects don't hit the internal edges
+	{
+		StaticCompoundShapeSettings compound_settings;
+		compound_settings.SetEmbedded();
+		constexpr float size = 2.0f;
+		RefConst<Shape> box_shape = new BoxShape(Vec3::sReplicate(0.5f * size));
+		for (int x = -10; x < 10; ++x)
+			for (int z = -10; z < 10; ++z)
+				compound_settings.AddShape(Vec3(size * x, 0, size * z), Quat::sIdentity(), box_shape);
+		BodyID id = mBodyInterface->CreateAndAddBody(BodyCreationSettings(&compound_settings, RVec3(0, -1, -40), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING), EActivation::DontActivate);
+		SetBodyLabel(id, "Dense grid of boxes");
+
+		CreateSlidingObjects(RVec3(-18, 1.9_r, -40.0_r));
+	}
+
+	// This tests if objects do not collide with internal edges
+	{
+		// Create a dense grid of triangles so that we have a large chance of hitting an internal edge
+		constexpr float size = 2.0f;
+		TriangleList triangles;
+		for (int x = -10; x < 10; ++x)
+			for (int z = -10; z < 10; ++z)
+			{
+				float x1 = size * x;
+				float z1 = size * z;
+				float x2 = x1 + size;
+				float z2 = z1 + size;
+
+				Float3 v1 = Float3(x1, 0, z1);
+				Float3 v2 = Float3(x2, 0, z1);
+				Float3 v3 = Float3(x1, 0, z2);
+				Float3 v4 = Float3(x2, 0, z2);
+
+				triangles.push_back(Triangle(v1, v3, v4));
+				triangles.push_back(Triangle(v1, v4, v2));
+			}
+
+		MeshShapeSettings mesh_settings(triangles);
+		mesh_settings.mActiveEdgeCosThresholdAngle = -1.0f; // Turn off regular active edge determination so that we only rely on the mEnhancedInternalEdgeRemoval flag
+		mesh_settings.SetEmbedded();
+		BodyID id = mBodyInterface->CreateAndAddBody(BodyCreationSettings(&mesh_settings, RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING), EActivation::DontActivate);
+		SetBodyLabel(id, "Dense triangle mesh");
+
+		CreateSlidingObjects(RVec3(-18, 1.9_r, 0));
+	}
+
+	// This test tests that we only ignore edges that are shared with voided triangles
+	{
+		// Create an L shape mesh lying on its back
+		TriangleList triangles;
+		constexpr float height = 0.5f;
+		constexpr float half_width = 5.0f;
+		constexpr float half_length = 2.0f;
+		triangles.push_back(Triangle(Float3(-half_length, 0, half_width), Float3(half_length, 0, -half_width), Float3(-half_length, 0, -half_width)));
+		triangles.push_back(Triangle(Float3(-half_length, 0, half_width), Float3(half_length, 0, half_width), Float3(half_length, 0, -half_width)));
+		triangles.push_back(Triangle(Float3(half_length, height, half_width), Float3(half_length, height, -half_width), Float3(half_length, 0, half_width)));
+		triangles.push_back(Triangle(Float3(half_length, 0, half_width), Float3(half_length, height, -half_width), Float3(half_length, 0, -half_width)));
+		mBodyInterface->CreateAndAddBody(BodyCreationSettings(new MeshShapeSettings(triangles), RVec3(0, 0, 30), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING), EActivation::DontActivate);
+
+		// Roll a sphere towards the edge pointing upwards
+		float z = 28.0f;
+		for (int enhanced_removal = 0; enhanced_removal < 2; ++enhanced_removal)
+		{
+			// A sphere
+			BodyCreationSettings sphere_bcs(new SphereShape(1.0f), RVec3(0, 1, z), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING);
+			sphere_bcs.mLinearVelocity = Vec3(20, 0, 0);
+			sphere_bcs.mEnhancedInternalEdgeRemoval = enhanced_removal == 1;
+			mBodyInterface->CreateAndAddBody(sphere_bcs, EActivation::Activate);
+			z += 4.0f;
+		}
+	}
+
+	// This tests that fast moving spheres rolling over a triangle will not be affected by internal edges
+	{
+		// Create a flat plane
+		MeshShapeSettings plane_mesh({
+			{
+				Float3(-10, 0, -10),
+				Float3(-10, 0, 10),
+				Float3(10, 0, 10)
+			},
+			{
+				Float3(-10, 0, -10),
+				Float3(10, 0, 10),
+				Float3(10, 0, -10)
+			},
+		});
+		plane_mesh.SetEmbedded();
+		BodyCreationSettings level_plane(&plane_mesh, RVec3(-10, 0, 50), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING);
+		level_plane.mFriction = 1;
+		BodyID id = mBodyInterface->CreateAndAddBody(level_plane, EActivation::DontActivate);
+		SetBodyLabel(id, "Dense triangle mesh");
+
+		// Roll a ball over it
+		BodyCreationSettings level_ball(new SphereShape(0.5f), RVec3(-10, 1, 41), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING);
+		level_ball.mEnhancedInternalEdgeRemoval = true;
+		level_ball.mFriction = 1;
+		level_ball.mOverrideMassProperties = EOverrideMassProperties::CalculateInertia;
+		level_ball.mMassPropertiesOverride.mMass = 1;
+		mLevelBall = mBodyInterface->CreateAndAddBody(level_ball, EActivation::Activate);
+
+		// Create a sloped plane
+		BodyCreationSettings slope_plane(&plane_mesh, RVec3(10, 0, 50), Quat::sRotation(Vec3::sAxisX(), DegreesToRadians(45)), EMotionType::Static, Layers::NON_MOVING);
+		slope_plane.mFriction = 1;
+		id = mBodyInterface->CreateAndAddBody(slope_plane, EActivation::DontActivate);
+		SetBodyLabel(id, "Dense triangle mesh");
+
+		// Roll a ball over it
+		BodyCreationSettings slope_ball(new SphereShape(0.5f), RVec3(10, 8, 44), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING);
+		slope_ball.mEnhancedInternalEdgeRemoval = true;
+		slope_ball.mFriction = 1;
+		slope_ball.mOverrideMassProperties = EOverrideMassProperties::CalculateInertia;
+		slope_ball.mMassPropertiesOverride.mMass = 1;
+		mBodyInterface->CreateAndAddBody(slope_ball, EActivation::Activate);
+	}
+
+	// This tests a previous bug where a compound shape will fall through a box because features are voided by accident.
+	// This is because both boxes of the compound shape collide with the top face of the static box. The big box will have a normal
+	// that is aligned with the face so will be processed immediately. This will void the top face of the static box. The small box,
+	// which collides with an edge of the top face will not be processed. This will cause the small box to penetrate the face.
+	{
+		// A box
+		BodyCreationSettings box_bcs(new BoxShape(Vec3::sReplicate(2.5f)), RVec3(0, 0, 70), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING);
+		mBodyInterface->CreateAndAddBody(box_bcs, EActivation::DontActivate);
+
+		// Compound
+		StaticCompoundShapeSettings compound;
+		compound.SetEmbedded();
+		compound.AddShape(Vec3(-2.5f, 0, 0), Quat::sIdentity(), new BoxShape(Vec3(2.5f, 0.1f, 0.1f)));
+		compound.AddShape(Vec3(0.1f, 0, 0), Quat::sIdentity(), new BoxShape(Vec3(0.1f, 1, 1)));
+		BodyCreationSettings compound_bcs(&compound, RVec3(2, 5, 70), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING);
+		compound_bcs.mEnhancedInternalEdgeRemoval = true;
+		mBodyInterface->CreateAndAddBody(compound_bcs, EActivation::Activate);
+	}
+
+	// Create a super dense grid of triangles
+	{
+		constexpr float size = 0.25f;
+		TriangleList triangles;
+		for (int x = -100; x < 100; ++x)
+			for (int z = -5; z < 5; ++z)
+			{
+				float x1 = size * x;
+				float z1 = size * z;
+				float x2 = x1 + size;
+				float z2 = z1 + size;
+
+				Float3 v1 = Float3(x1, 0, z1);
+				Float3 v2 = Float3(x2, 0, z1);
+				Float3 v3 = Float3(x1, 0, z2);
+				Float3 v4 = Float3(x2, 0, z2);
+
+				triangles.push_back(Triangle(v1, v3, v4));
+				triangles.push_back(Triangle(v1, v4, v2));
+			}
+
+		MeshShapeSettings mesh_settings(triangles);
+		mesh_settings.mActiveEdgeCosThresholdAngle = -1.0f; // Turn off regular active edge determination so that we only rely on the mEnhancedInternalEdgeRemoval flag
+		mesh_settings.SetEmbedded();
+		BodyID id = mBodyInterface->CreateAndAddBody(BodyCreationSettings(&mesh_settings, RVec3(0, 0, 80), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING), EActivation::DontActivate);
+		SetBodyLabel(id, "Dense triangle mesh");
+
+		BodyCreationSettings box_bcs(new BoxShape(Vec3::sReplicate(1.0f)), RVec3(-24, 0.9_r, 80), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING);
+		box_bcs.mLinearVelocity = Vec3(20, 0, 0);
+		box_bcs.mEnhancedInternalEdgeRemoval = true;
+		mBodyInterface->CreateAndAddBody(box_bcs, EActivation::Activate);
+	}
+}
+
+void EnhancedInternalEdgeRemovalTest::PrePhysicsUpdate(const PreUpdateParams &inParams)
+{
+	// Increase rotation speed of the ball on the flat plane
+	mBodyInterface->AddTorque(mLevelBall, Vec3(JPH_PI * 4, 0, 0));
+}

lib/All/JoltPhysics/Samples/Tests/General/EnhancedInternalEdgeRemovalTest.h

@@ -0,0 +1,28 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2024 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/Test.h>
+
+class EnhancedInternalEdgeRemovalTest : public Test
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, EnhancedInternalEdgeRemovalTest)
+
+	// Description of the test
+	virtual const char *GetDescription() const override
+	{
+		return "Shows bodies using enhanced edge removal vs bodies that don't.";
+	}
+
+	// See: Test
+	virtual void	Initialize() override;
+	virtual void	PrePhysicsUpdate(const PreUpdateParams &inParams) override;
+
+private:
+	void			CreateSlidingObjects(RVec3Arg inStart);
+
+	BodyID			mLevelBall;
+};