initial commit, 4.5 stable

thirdparty/jolt_physics/Jolt/Physics/Body/AllowedDOFs.h

@@ -0,0 +1,68 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+/// Enum used in BodyCreationSettings and MotionProperties to indicate which degrees of freedom a body has
+enum class EAllowedDOFs : uint8
+{
+	None				= 0b000000,									///< No degrees of freedom are allowed. Note that this is not valid and will crash. Use a static body instead.
+	All					= 0b111111,									///< All degrees of freedom are allowed
+	TranslationX		= 0b000001,									///< Body can move in world space X axis
+	TranslationY		= 0b000010,									///< Body can move in world space Y axis
+	TranslationZ		= 0b000100,									///< Body can move in world space Z axis
+	RotationX			= 0b001000,									///< Body can rotate around world space X axis
+	RotationY			= 0b010000,									///< Body can rotate around world space Y axis
+	RotationZ			= 0b100000,									///< Body can rotate around world space Z axis
+	Plane2D				= TranslationX | TranslationY | RotationZ,	///< Body can only move in X and Y axis and rotate around Z axis
+};
+
+/// Bitwise OR operator for EAllowedDOFs
+constexpr EAllowedDOFs operator | (EAllowedDOFs inLHS, EAllowedDOFs inRHS)
+{
+	return EAllowedDOFs(uint8(inLHS) | uint8(inRHS));
+}
+
+/// Bitwise AND operator for EAllowedDOFs
+constexpr EAllowedDOFs operator & (EAllowedDOFs inLHS, EAllowedDOFs inRHS)
+{
+	return EAllowedDOFs(uint8(inLHS) & uint8(inRHS));
+}
+
+/// Bitwise XOR operator for EAllowedDOFs
+constexpr EAllowedDOFs operator ^ (EAllowedDOFs inLHS, EAllowedDOFs inRHS)
+{
+	return EAllowedDOFs(uint8(inLHS) ^ uint8(inRHS));
+}
+
+/// Bitwise NOT operator for EAllowedDOFs
+constexpr EAllowedDOFs operator ~ (EAllowedDOFs inAllowedDOFs)
+{
+	return EAllowedDOFs(~uint8(inAllowedDOFs));
+}
+
+/// Bitwise OR assignment operator for EAllowedDOFs
+constexpr EAllowedDOFs & operator |= (EAllowedDOFs &ioLHS, EAllowedDOFs inRHS)
+{
+	ioLHS = ioLHS | inRHS;
+	return ioLHS;
+}
+
+/// Bitwise AND assignment operator for EAllowedDOFs
+constexpr EAllowedDOFs & operator &= (EAllowedDOFs &ioLHS, EAllowedDOFs inRHS)
+{
+	ioLHS = ioLHS & inRHS;
+	return ioLHS;
+}
+
+/// Bitwise XOR assignment operator for EAllowedDOFs
+constexpr EAllowedDOFs & operator ^= (EAllowedDOFs &ioLHS, EAllowedDOFs inRHS)
+{
+	ioLHS = ioLHS ^ inRHS;
+	return ioLHS;
+}
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Body/Body.cpp

@@ -0,0 +1,423 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Body/Body.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Jolt/Physics/SoftBody/SoftBodyCreationSettings.h>
+#include <Jolt/Physics/SoftBody/SoftBodyMotionProperties.h>
+#include <Jolt/Physics/PhysicsSettings.h>
+#include <Jolt/Physics/StateRecorder.h>
+#include <Jolt/Physics/Collision/Shape/EmptyShape.h>
+#include <Jolt/Core/StringTools.h>
+#include <Jolt/Core/Profiler.h>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+static const EmptyShape sFixedToWorldShape;
+Body Body::sFixedToWorld(false);
+
+Body::Body(bool) :
+	mPosition(Vec3::sZero()),
+	mRotation(Quat::sIdentity()),
+	mShape(&sFixedToWorldShape), // Dummy shape
+	mFriction(0.0f),
+	mRestitution(0.0f),
+	mObjectLayer(cObjectLayerInvalid),
+	mMotionType(EMotionType::Static)
+{
+	sFixedToWorldShape.SetEmbedded();
+}
+
+void Body::SetMotionType(EMotionType inMotionType)
+{
+	if (mMotionType == inMotionType)
+		return;
+
+	JPH_ASSERT(inMotionType == EMotionType::Static || mMotionProperties != nullptr, "Body needs to be created with mAllowDynamicOrKinematic set to true");
+	JPH_ASSERT(inMotionType != EMotionType::Static || !IsActive(), "Deactivate body first");
+	JPH_ASSERT(inMotionType == EMotionType::Dynamic || !IsSoftBody(), "Soft bodies can only be dynamic, you can make individual vertices kinematic by setting their inverse mass to 0");
+
+	// Store new motion type
+	mMotionType = inMotionType;
+
+	if (mMotionProperties != nullptr)
+	{
+		// Update cache
+		JPH_IF_ENABLE_ASSERTS(mMotionProperties->mCachedMotionType = inMotionType;)
+
+		switch (inMotionType)
+		{
+		case EMotionType::Static:
+			// Stop the object
+			mMotionProperties->mLinearVelocity = Vec3::sZero();
+			mMotionProperties->mAngularVelocity = Vec3::sZero();
+			[[fallthrough]];
+
+		case EMotionType::Kinematic:
+			// Cancel forces
+			mMotionProperties->ResetForce();
+			mMotionProperties->ResetTorque();
+			break;
+
+		case EMotionType::Dynamic:
+			break;
+		}
+	}
+}
+
+void Body::SetAllowSleeping(bool inAllow)
+{
+	mMotionProperties->mAllowSleeping = inAllow;
+	if (inAllow)
+		ResetSleepTimer();
+}
+
+void Body::MoveKinematic(RVec3Arg inTargetPosition, QuatArg inTargetRotation, float inDeltaTime)
+{
+	JPH_ASSERT(IsRigidBody()); // Only valid for rigid bodies
+	JPH_ASSERT(!IsStatic());
+	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::Read));
+
+	// Calculate center of mass at end situation
+	RVec3 new_com = inTargetPosition + inTargetRotation * mShape->GetCenterOfMass();
+
+	// Calculate delta position and rotation
+	Vec3 delta_pos = Vec3(new_com - mPosition);
+	Quat delta_rotation = inTargetRotation * mRotation.Conjugated();
+
+	mMotionProperties->MoveKinematic(delta_pos, delta_rotation, inDeltaTime);
+}
+
+void Body::CalculateWorldSpaceBoundsInternal()
+{
+	mBounds = mShape->GetWorldSpaceBounds(GetCenterOfMassTransform(), Vec3::sOne());
+}
+
+void Body::SetPositionAndRotationInternal(RVec3Arg inPosition, QuatArg inRotation, bool inResetSleepTimer)
+{
+	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::ReadWrite));
+
+	mPosition = inPosition + inRotation * mShape->GetCenterOfMass();
+	mRotation = inRotation;
+
+	// Initialize bounding box
+	CalculateWorldSpaceBoundsInternal();
+
+	// Reset sleeping test
+	if (inResetSleepTimer && mMotionProperties != nullptr)
+		ResetSleepTimer();
+}
+
+void Body::UpdateCenterOfMassInternal(Vec3Arg inPreviousCenterOfMass, bool inUpdateMassProperties)
+{
+	// Update center of mass position so the world position for this body stays the same
+	mPosition += mRotation * (mShape->GetCenterOfMass() - inPreviousCenterOfMass);
+
+	// Recalculate mass and inertia if requested
+	if (inUpdateMassProperties && mMotionProperties != nullptr)
+		mMotionProperties->SetMassProperties(mMotionProperties->GetAllowedDOFs(), mShape->GetMassProperties());
+}
+
+void Body::SetShapeInternal(const Shape *inShape, bool inUpdateMassProperties)
+{
+	JPH_ASSERT(IsRigidBody()); // Only valid for rigid bodies
+	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::ReadWrite));
+
+	// Get the old center of mass
+	Vec3 old_com = mShape->GetCenterOfMass();
+
+	// Update the shape
+	mShape = inShape;
+
+	// Update center of mass
+	UpdateCenterOfMassInternal(old_com, inUpdateMassProperties);
+
+	// Recalculate bounding box
+	CalculateWorldSpaceBoundsInternal();
+}
+
+ECanSleep Body::UpdateSleepStateInternal(float inDeltaTime, float inMaxMovement, float inTimeBeforeSleep)
+{
+	// Check override & sensors will never go to sleep (they would stop detecting collisions with sleeping bodies)
+	if (!mMotionProperties->mAllowSleeping || IsSensor())
+		return ECanSleep::CannotSleep;
+
+	// Get the points to test
+	RVec3 points[3];
+	GetSleepTestPoints(points);
+
+#ifdef JPH_DOUBLE_PRECISION
+	// Get base offset for spheres
+	DVec3 offset = mMotionProperties->GetSleepTestOffset();
+#endif // JPH_DOUBLE_PRECISION
+
+	for (int i = 0; i < 3; ++i)
+	{
+		Sphere &sphere = mMotionProperties->mSleepTestSpheres[i];
+
+		// Make point relative to base offset
+#ifdef JPH_DOUBLE_PRECISION
+		Vec3 p = Vec3(points[i] - offset);
+#else
+		Vec3 p = points[i];
+#endif // JPH_DOUBLE_PRECISION
+
+		// Encapsulate the point in a sphere
+		sphere.EncapsulatePoint(p);
+
+		// Test if it exceeded the max movement
+		if (sphere.GetRadius() > inMaxMovement)
+		{
+			// Body is not sleeping, reset test
+			mMotionProperties->ResetSleepTestSpheres(points);
+			return ECanSleep::CannotSleep;
+		}
+	}
+
+	return mMotionProperties->AccumulateSleepTime(inDeltaTime, inTimeBeforeSleep);
+}
+
+void Body::GetSubmergedVolume(RVec3Arg inSurfacePosition, Vec3Arg inSurfaceNormal, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outRelativeCenterOfBuoyancy) const
+{
+	// For GetSubmergedVolume we transform the surface relative to the body position for increased precision
+	Mat44 rotation = Mat44::sRotation(mRotation);
+	Plane surface_relative_to_body = Plane::sFromPointAndNormal(inSurfacePosition - mPosition, inSurfaceNormal);
+
+	// Calculate amount of volume that is submerged and what the center of buoyancy is
+	mShape->GetSubmergedVolume(rotation, Vec3::sOne(), surface_relative_to_body, outTotalVolume, outSubmergedVolume, outRelativeCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, mPosition));
+}
+
+bool Body::ApplyBuoyancyImpulse(float inTotalVolume, float inSubmergedVolume, Vec3Arg inRelativeCenterOfBuoyancy, float inBuoyancy, float inLinearDrag, float inAngularDrag, Vec3Arg inFluidVelocity, Vec3Arg inGravity, float inDeltaTime)
+{
+	JPH_ASSERT(IsRigidBody()); // Only implemented for rigid bodies currently
+
+	// We follow the approach from 'Game Programming Gems 6' 2.5 Exact Buoyancy for Polyhedra
+	// All quantities below are in world space
+
+	// If we're not submerged, there's no point in doing the rest of the calculations
+	if (inSubmergedVolume > 0.0f)
+	{
+	#ifdef JPH_DEBUG_RENDERER
+		// Draw submerged volume properties
+		if (Shape::sDrawSubmergedVolumes)
+		{
+			RVec3 center_of_buoyancy = mPosition + inRelativeCenterOfBuoyancy;
+			DebugRenderer::sInstance->DrawMarker(center_of_buoyancy, Color::sWhite, 2.0f);
+			DebugRenderer::sInstance->DrawText3D(center_of_buoyancy, StringFormat("%.3f / %.3f", (double)inSubmergedVolume, (double)inTotalVolume));
+		}
+	#endif // JPH_DEBUG_RENDERER
+
+		// When buoyancy is 1 we want neutral buoyancy, this means that the density of the liquid is the same as the density of the body at that point.
+		// Buoyancy > 1 should make the object float, < 1 should make it sink.
+		float inverse_mass = mMotionProperties->GetInverseMass();
+		float fluid_density = inBuoyancy / (inTotalVolume * inverse_mass);
+
+		// Buoyancy force = Density of Fluid * Submerged volume * Magnitude of gravity * Up direction (eq 2.5.1)
+		// Impulse = Force * Delta time
+		// We should apply this at the center of buoyancy (= center of mass of submerged volume)
+		Vec3 buoyancy_impulse = -fluid_density * inSubmergedVolume * mMotionProperties->GetGravityFactor() * inGravity * inDeltaTime;
+
+		// Calculate the velocity of the center of buoyancy relative to the fluid
+		Vec3 linear_velocity = mMotionProperties->GetLinearVelocity();
+		Vec3 angular_velocity = mMotionProperties->GetAngularVelocity();
+		Vec3 center_of_buoyancy_velocity = linear_velocity + angular_velocity.Cross(inRelativeCenterOfBuoyancy);
+		Vec3 relative_center_of_buoyancy_velocity = inFluidVelocity - center_of_buoyancy_velocity;
+
+		// Here we deviate from the article, instead of eq 2.5.14 we use a quadratic drag formula: https://en.wikipedia.org/wiki/Drag_%28physics%29
+		// Drag force = 0.5 * Fluid Density * (Velocity of fluid - Velocity of center of buoyancy)^2 * Linear Drag * Area Facing the Relative Fluid Velocity
+		// Again Impulse = Force * Delta Time
+		// We should apply this at the center of buoyancy (= center of mass for submerged volume with no center of mass offset)
+
+		// Get size of local bounding box
+		Vec3 size = mShape->GetLocalBounds().GetSize();
+
+		// Determine area of the local space bounding box in the direction of the relative velocity between the fluid and the center of buoyancy
+		float area = 0.0f;
+		float relative_center_of_buoyancy_velocity_len_sq = relative_center_of_buoyancy_velocity.LengthSq();
+		if (relative_center_of_buoyancy_velocity_len_sq > 1.0e-12f)
+		{
+			Vec3 local_relative_center_of_buoyancy_velocity = GetRotation().Conjugated() * relative_center_of_buoyancy_velocity;
+			area = local_relative_center_of_buoyancy_velocity.Abs().Dot(size.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_X>() * size.Swizzle<SWIZZLE_Z, SWIZZLE_X, SWIZZLE_Y>()) / sqrt(relative_center_of_buoyancy_velocity_len_sq);
+		}
+
+		// Calculate the impulse
+		Vec3 drag_impulse = (0.5f * fluid_density * inLinearDrag * area * inDeltaTime) * relative_center_of_buoyancy_velocity * relative_center_of_buoyancy_velocity.Length();
+
+		// Clamp magnitude against current linear velocity to prevent overshoot
+		float linear_velocity_len_sq = linear_velocity.LengthSq();
+		float drag_delta_linear_velocity_len_sq = (drag_impulse * inverse_mass).LengthSq();
+		if (drag_delta_linear_velocity_len_sq > linear_velocity_len_sq)
+			drag_impulse *= sqrt(linear_velocity_len_sq / drag_delta_linear_velocity_len_sq);
+
+		// Calculate the resulting delta linear velocity due to buoyancy and drag
+		Vec3 delta_linear_velocity = (drag_impulse + buoyancy_impulse) * inverse_mass;
+		mMotionProperties->AddLinearVelocityStep(delta_linear_velocity);
+
+		// Determine average width of the body (across the three axis)
+		float l = (size.GetX() + size.GetY() + size.GetZ()) / 3.0f;
+
+		// Drag torque = -Angular Drag * Mass * Submerged volume / Total volume * (Average width of body)^2 * Angular velocity (eq 2.5.15)
+		Vec3 drag_angular_impulse = (-inAngularDrag * inSubmergedVolume / inTotalVolume * inDeltaTime * Square(l) / inverse_mass) * angular_velocity;
+		Mat44 inv_inertia = GetInverseInertia();
+		Vec3 drag_delta_angular_velocity = inv_inertia * drag_angular_impulse;
+
+		// Clamp magnitude against the current angular velocity to prevent overshoot
+		float angular_velocity_len_sq = angular_velocity.LengthSq();
+		float drag_delta_angular_velocity_len_sq = drag_delta_angular_velocity.LengthSq();
+		if (drag_delta_angular_velocity_len_sq > angular_velocity_len_sq)
+			drag_delta_angular_velocity *= sqrt(angular_velocity_len_sq / drag_delta_angular_velocity_len_sq);
+
+		// Calculate total delta angular velocity due to drag and buoyancy
+		Vec3 delta_angular_velocity = drag_delta_angular_velocity + inv_inertia * inRelativeCenterOfBuoyancy.Cross(buoyancy_impulse + drag_impulse);
+		mMotionProperties->AddAngularVelocityStep(delta_angular_velocity);
+		return true;
+	}
+
+	return false;
+}
+
+bool Body::ApplyBuoyancyImpulse(RVec3Arg inSurfacePosition, Vec3Arg inSurfaceNormal, float inBuoyancy, float inLinearDrag, float inAngularDrag, Vec3Arg inFluidVelocity, Vec3Arg inGravity, float inDeltaTime)
+{
+	JPH_PROFILE_FUNCTION();
+
+	float total_volume, submerged_volume;
+	Vec3 relative_center_of_buoyancy;
+	GetSubmergedVolume(inSurfacePosition, inSurfaceNormal, total_volume, submerged_volume, relative_center_of_buoyancy);
+
+	return ApplyBuoyancyImpulse(total_volume, submerged_volume, relative_center_of_buoyancy, inBuoyancy, inLinearDrag, inAngularDrag, inFluidVelocity, inGravity, inDeltaTime);
+}
+
+void Body::SaveState(StateRecorder &inStream) const
+{
+	// Only write properties that can change at runtime
+	inStream.Write(mPosition);
+	inStream.Write(mRotation);
+
+	if (mMotionProperties != nullptr)
+	{
+		if (IsSoftBody())
+			static_cast<const SoftBodyMotionProperties *>(mMotionProperties)->SaveState(inStream);
+		else
+			mMotionProperties->SaveState(inStream);
+	}
+}
+
+void Body::RestoreState(StateRecorder &inStream)
+{
+	inStream.Read(mPosition);
+	inStream.Read(mRotation);
+
+	if (mMotionProperties != nullptr)
+	{
+		if (IsSoftBody())
+			static_cast<SoftBodyMotionProperties *>(mMotionProperties)->RestoreState(inStream);
+		else
+			mMotionProperties->RestoreState(inStream);
+
+		JPH_IF_ENABLE_ASSERTS(mMotionProperties->mCachedMotionType = mMotionType);
+	}
+
+	// Initialize bounding box
+	CalculateWorldSpaceBoundsInternal();
+}
+
+BodyCreationSettings Body::GetBodyCreationSettings() const
+{
+	JPH_ASSERT(IsRigidBody());
+
+	BodyCreationSettings result;
+
+	result.mPosition = GetPosition();
+	result.mRotation = GetRotation();
+	result.mLinearVelocity = mMotionProperties != nullptr? mMotionProperties->GetLinearVelocity() : Vec3::sZero();
+	result.mAngularVelocity = mMotionProperties != nullptr? mMotionProperties->GetAngularVelocity() : Vec3::sZero();
+	result.mObjectLayer = GetObjectLayer();
+	result.mUserData = mUserData;
+	result.mCollisionGroup = GetCollisionGroup();
+	result.mMotionType = GetMotionType();
+	result.mAllowedDOFs = mMotionProperties != nullptr? mMotionProperties->GetAllowedDOFs() : EAllowedDOFs::All;
+	result.mAllowDynamicOrKinematic = mMotionProperties != nullptr;
+	result.mIsSensor = IsSensor();
+	result.mCollideKinematicVsNonDynamic = GetCollideKinematicVsNonDynamic();
+	result.mUseManifoldReduction = GetUseManifoldReduction();
+	result.mApplyGyroscopicForce = GetApplyGyroscopicForce();
+	result.mMotionQuality = mMotionProperties != nullptr? mMotionProperties->GetMotionQuality() : EMotionQuality::Discrete;
+	result.mEnhancedInternalEdgeRemoval = GetEnhancedInternalEdgeRemoval();
+	result.mAllowSleeping = mMotionProperties != nullptr? GetAllowSleeping() : true;
+	result.mFriction = GetFriction();
+	result.mRestitution = GetRestitution();
+	result.mLinearDamping = mMotionProperties != nullptr? mMotionProperties->GetLinearDamping() : 0.0f;
+	result.mAngularDamping = mMotionProperties != nullptr? mMotionProperties->GetAngularDamping() : 0.0f;
+	result.mMaxLinearVelocity = mMotionProperties != nullptr? mMotionProperties->GetMaxLinearVelocity() : 0.0f;
+	result.mMaxAngularVelocity = mMotionProperties != nullptr? mMotionProperties->GetMaxAngularVelocity() : 0.0f;
+	result.mGravityFactor = mMotionProperties != nullptr? mMotionProperties->GetGravityFactor() : 1.0f;
+	result.mNumVelocityStepsOverride = mMotionProperties != nullptr? mMotionProperties->GetNumVelocityStepsOverride() : 0;
+	result.mNumPositionStepsOverride = mMotionProperties != nullptr? mMotionProperties->GetNumPositionStepsOverride() : 0;
+	result.mOverrideMassProperties = EOverrideMassProperties::MassAndInertiaProvided;
+
+	// Invert inertia and mass
+	if (mMotionProperties != nullptr)
+	{
+		float inv_mass = mMotionProperties->GetInverseMassUnchecked();
+		Mat44 inv_inertia = mMotionProperties->GetLocalSpaceInverseInertiaUnchecked();
+
+		// Get mass
+		result.mMassPropertiesOverride.mMass = inv_mass != 0.0f? 1.0f / inv_mass : FLT_MAX;
+
+		// Get inertia
+		Mat44 inertia;
+		if (inertia.SetInversed3x3(inv_inertia))
+		{
+			// Inertia was invertible, we can use it
+			result.mMassPropertiesOverride.mInertia = inertia;
+		}
+		else
+		{
+			// Prevent division by zero
+			Vec3 diagonal = Vec3::sMax(inv_inertia.GetDiagonal3(), Vec3::sReplicate(FLT_MIN));
+			result.mMassPropertiesOverride.mInertia = Mat44::sScale(diagonal.Reciprocal());
+		}
+	}
+	else
+	{
+		result.mMassPropertiesOverride.mMass = FLT_MAX;
+		result.mMassPropertiesOverride.mInertia = Mat44::sScale(Vec3::sReplicate(FLT_MAX));
+	}
+
+	result.SetShape(GetShape());
+
+	return result;
+}
+
+SoftBodyCreationSettings Body::GetSoftBodyCreationSettings() const
+{
+	JPH_ASSERT(IsSoftBody());
+
+	SoftBodyCreationSettings result;
+
+	result.mPosition = GetPosition();
+	result.mRotation = GetRotation();
+	result.mUserData = mUserData;
+	result.mObjectLayer = GetObjectLayer();
+	result.mCollisionGroup = GetCollisionGroup();
+	result.mFriction = GetFriction();
+	result.mRestitution = GetRestitution();
+	const SoftBodyMotionProperties *mp = static_cast<const SoftBodyMotionProperties *>(mMotionProperties);
+	result.mNumIterations = mp->GetNumIterations();
+	result.mLinearDamping = mp->GetLinearDamping();
+	result.mMaxLinearVelocity = mp->GetMaxLinearVelocity();
+	result.mGravityFactor = mp->GetGravityFactor();
+	result.mPressure = mp->GetPressure();
+	result.mUpdatePosition = mp->GetUpdatePosition();
+	result.mSettings = mp->GetSettings();
+
+	return result;
+}
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Body/Body.h

@@ -0,0 +1,452 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/NonCopyable.h>
+#include <Jolt/Geometry/AABox.h>
+#include <Jolt/Physics/Collision/Shape/Shape.h>
+#include <Jolt/Physics/Collision/BroadPhase/BroadPhaseLayer.h>
+#include <Jolt/Physics/Collision/ObjectLayer.h>
+#include <Jolt/Physics/Collision/CollisionGroup.h>
+#include <Jolt/Physics/Collision/TransformedShape.h>
+#include <Jolt/Physics/Body/MotionProperties.h>
+#include <Jolt/Physics/Body/BodyID.h>
+#include <Jolt/Physics/Body/BodyAccess.h>
+#include <Jolt/Physics/Body/BodyType.h>
+#include <Jolt/Core/StringTools.h>
+
+JPH_NAMESPACE_BEGIN
+
+class StateRecorder;
+class BodyCreationSettings;
+class SoftBodyCreationSettings;
+
+/// A rigid body that can be simulated using the physics system
+///
+/// Note that internally all properties (position, velocity etc.) are tracked relative to the center of mass of the object to simplify the simulation of the object.
+///
+/// The offset between the position of the body and the center of mass position of the body is GetShape()->GetCenterOfMass().
+/// The functions that get/set the position of the body all indicate if they are relative to the center of mass or to the original position in which the shape was created.
+///
+/// The linear velocity is also velocity of the center of mass, to correct for this: \f$VelocityCOM = Velocity - AngularVelocity \times ShapeCOM\f$.
+class
+#ifndef JPH_PLATFORM_DOXYGEN // Doxygen gets confused here
+	JPH_EXPORT_GCC_BUG_WORKAROUND alignas(JPH_RVECTOR_ALIGNMENT)
+#endif
+	Body : public NonCopyable
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Get the id of this body
+	inline const BodyID &	GetID() const													{ return mID; }
+
+	/// Get the type of body (rigid or soft)
+	inline EBodyType		GetBodyType() const												{ return mBodyType; }
+
+	/// Check if this body is a rigid body
+	inline bool				IsRigidBody() const												{ return mBodyType == EBodyType::RigidBody; }
+
+	/// Check if this body is a soft body
+	inline bool				IsSoftBody() const												{ return mBodyType == EBodyType::SoftBody; }
+
+	// See comment at GetIndexInActiveBodiesInternal for reasoning why TSAN is disabled here
+	JPH_TSAN_NO_SANITIZE
+	/// If this body is currently actively simulating (true) or sleeping (false)
+	inline bool				IsActive() const												{ return mMotionProperties != nullptr && mMotionProperties->mIndexInActiveBodies != cInactiveIndex; }
+
+	/// Check if this body is static (not movable)
+	inline bool				IsStatic() const												{ return mMotionType == EMotionType::Static; }
+
+	/// Check if this body is kinematic (keyframed), which means that it will move according to its current velocity, but forces don't affect it
+	inline bool				IsKinematic() const												{ return mMotionType == EMotionType::Kinematic; }
+
+	/// Check if this body is dynamic, which means that it moves and forces can act on it
+	inline bool				IsDynamic() const												{ return mMotionType == EMotionType::Dynamic; }
+
+	/// Check if a body could be made kinematic or dynamic (if it was created dynamic or with mAllowDynamicOrKinematic set to true)
+	inline bool				CanBeKinematicOrDynamic() const									{ return mMotionProperties != nullptr; }
+
+	/// Change the body to a sensor. A sensor will receive collision callbacks, but will not cause any collision responses and can be used as a trigger volume.
+	/// The cheapest sensor (in terms of CPU usage) is a sensor with motion type Static (they can be moved around using BodyInterface::SetPosition/SetPositionAndRotation).
+	/// These sensors will only detect collisions with active Dynamic or Kinematic bodies. As soon as a body go to sleep, the contact point with the sensor will be lost.
+	/// If you make a sensor Dynamic or Kinematic and activate them, the sensor will be able to detect collisions with sleeping bodies too. An active sensor will never go to sleep automatically.
+	/// When you make a Dynamic or Kinematic sensor, make sure it is in an ObjectLayer that does not collide with Static bodies or other sensors to avoid extra overhead in the broad phase.
+	inline void				SetIsSensor(bool inIsSensor)									{ JPH_ASSERT(IsRigidBody()); if (inIsSensor) mFlags.fetch_or(uint8(EFlags::IsSensor), memory_order_relaxed); else mFlags.fetch_and(uint8(~uint8(EFlags::IsSensor)), memory_order_relaxed); }
+
+	/// Check if this body is a sensor.
+	inline bool				IsSensor() const												{ return (mFlags.load(memory_order_relaxed) & uint8(EFlags::IsSensor)) != 0; }
+
+	/// If kinematic objects can generate contact points against other kinematic or static objects.
+	/// Note that turning this on can be CPU intensive as much more collision detection work will be done without any effect on the simulation (kinematic objects are not affected by other kinematic/static objects).
+	/// This can be used to make sensors detect static objects. Note that the sensor must be kinematic and active for it to detect static objects.
+	inline void				SetCollideKinematicVsNonDynamic(bool inCollide)					{ JPH_ASSERT(IsRigidBody()); if (inCollide) mFlags.fetch_or(uint8(EFlags::CollideKinematicVsNonDynamic), memory_order_relaxed); else mFlags.fetch_and(uint8(~uint8(EFlags::CollideKinematicVsNonDynamic)), memory_order_relaxed); }
+
+	/// Check if kinematic objects can generate contact points against other kinematic or static objects.
+	inline bool				GetCollideKinematicVsNonDynamic() const							{ return (mFlags.load(memory_order_relaxed) & uint8(EFlags::CollideKinematicVsNonDynamic)) != 0; }
+
+	/// If PhysicsSettings::mUseManifoldReduction is true, this allows turning off manifold reduction for this specific body.
+	/// Manifold reduction by default will combine contacts with similar normals that come from different SubShapeIDs (e.g. different triangles in a mesh shape or different compound shapes).
+	/// If the application requires tracking exactly which SubShapeIDs are in contact, you can turn off manifold reduction. Note that this comes at a performance cost.
+	/// Consider using BodyInterface::SetUseManifoldReduction if the body could already be in contact with other bodies to ensure that the contact cache is invalidated and you get the correct contact callbacks.
+	inline void				SetUseManifoldReduction(bool inUseReduction)					{ JPH_ASSERT(IsRigidBody()); if (inUseReduction) mFlags.fetch_or(uint8(EFlags::UseManifoldReduction), memory_order_relaxed); else mFlags.fetch_and(uint8(~uint8(EFlags::UseManifoldReduction)), memory_order_relaxed); }
+
+	/// Check if this body can use manifold reduction.
+	inline bool				GetUseManifoldReduction() const									{ return (mFlags.load(memory_order_relaxed) & uint8(EFlags::UseManifoldReduction)) != 0; }
+
+	/// Checks if the combination of this body and inBody2 should use manifold reduction
+	inline bool				GetUseManifoldReductionWithBody(const Body &inBody2) const		{ return ((mFlags.load(memory_order_relaxed) & inBody2.mFlags.load(memory_order_relaxed)) & uint8(EFlags::UseManifoldReduction)) != 0; }
+
+	/// Set to indicate that the gyroscopic force should be applied to this body (aka Dzhanibekov effect, see https://en.wikipedia.org/wiki/Tennis_racket_theorem)
+	inline void				SetApplyGyroscopicForce(bool inApply)							{ JPH_ASSERT(IsRigidBody()); if (inApply) mFlags.fetch_or(uint8(EFlags::ApplyGyroscopicForce), memory_order_relaxed); else mFlags.fetch_and(uint8(~uint8(EFlags::ApplyGyroscopicForce)), memory_order_relaxed); }
+
+	/// Check if the gyroscopic force is being applied for this body
+	inline bool				GetApplyGyroscopicForce() const									{ return (mFlags.load(memory_order_relaxed) & uint8(EFlags::ApplyGyroscopicForce)) != 0; }
+
+	/// Set to indicate that extra effort should be made to try to remove ghost contacts (collisions with internal edges of a mesh). This is more expensive but makes bodies move smoother over a mesh with convex edges.
+	inline void				SetEnhancedInternalEdgeRemoval(bool inApply)					{ JPH_ASSERT(IsRigidBody()); if (inApply) mFlags.fetch_or(uint8(EFlags::EnhancedInternalEdgeRemoval), memory_order_relaxed); else mFlags.fetch_and(uint8(~uint8(EFlags::EnhancedInternalEdgeRemoval)), memory_order_relaxed); }
+
+	/// Check if enhanced internal edge removal is turned on
+	inline bool				GetEnhancedInternalEdgeRemoval() const							{ return (mFlags.load(memory_order_relaxed) & uint8(EFlags::EnhancedInternalEdgeRemoval)) != 0; }
+
+	/// Checks if the combination of this body and inBody2 should use enhanced internal edge removal
+	inline bool				GetEnhancedInternalEdgeRemovalWithBody(const Body &inBody2) const { return ((mFlags.load(memory_order_relaxed) | inBody2.mFlags.load(memory_order_relaxed)) & uint8(EFlags::EnhancedInternalEdgeRemoval)) != 0; }
+
+	/// Get the bodies motion type.
+	inline EMotionType		GetMotionType() const											{ return mMotionType; }
+
+	/// Set the motion type of this body. Consider using BodyInterface::SetMotionType instead of this function if the body may be active or if it needs to be activated.
+	void					SetMotionType(EMotionType inMotionType);
+
+	/// Get broadphase layer, this determines in which broad phase sub-tree the object is placed
+	inline BroadPhaseLayer	GetBroadPhaseLayer() const										{ return mBroadPhaseLayer; }
+
+	/// Get object layer, this determines which other objects it collides with
+	inline ObjectLayer		GetObjectLayer() const											{ return mObjectLayer; }
+
+	/// Collision group and sub-group ID, determines which other objects it collides with
+	const CollisionGroup &	GetCollisionGroup() const										{ return mCollisionGroup; }
+	CollisionGroup &		GetCollisionGroup()												{ return mCollisionGroup; }
+	void					SetCollisionGroup(const CollisionGroup &inGroup)				{ mCollisionGroup = inGroup; }
+
+	/// If this body can go to sleep. Note that disabling sleeping on a sleeping object will not wake it up.
+	bool					GetAllowSleeping() const										{ return mMotionProperties->mAllowSleeping; }
+	void					SetAllowSleeping(bool inAllow);
+
+	/// Resets the sleep timer. This does not wake up the body if it is sleeping, but allows resetting the system that detects when a body is sleeping.
+	inline void				ResetSleepTimer();
+
+	/// Friction (dimensionless number, usually between 0 and 1, 0 = no friction, 1 = friction force equals force that presses the two bodies together). Note that bodies can have negative friction but the combined friction (see PhysicsSystem::SetCombineFriction) should never go below zero.
+	inline float			GetFriction() const												{ return mFriction; }
+	void					SetFriction(float inFriction)									{ mFriction = inFriction; }
+
+	/// Restitution (dimensionless number, usually between 0 and 1, 0 = completely inelastic collision response, 1 = completely elastic collision response). Note that bodies can have negative restitution but the combined restitution (see PhysicsSystem::SetCombineRestitution) should never go below zero.
+	inline float			GetRestitution() const											{ return mRestitution; }
+	void					SetRestitution(float inRestitution)								{ mRestitution = inRestitution; }
+
+	/// Get world space linear velocity of the center of mass (unit: m/s)
+	inline Vec3				GetLinearVelocity() const										{ return !IsStatic()? mMotionProperties->GetLinearVelocity() : Vec3::sZero(); }
+
+	/// Set world space linear velocity of the center of mass (unit: m/s).
+	/// If you want the body to wake up when it is sleeping, use BodyInterface::SetLinearVelocity instead.
+	void					SetLinearVelocity(Vec3Arg inLinearVelocity)						{ JPH_ASSERT(!IsStatic()); mMotionProperties->SetLinearVelocity(inLinearVelocity); }
+
+	/// Set world space linear velocity of the center of mass, will make sure the value is clamped against the maximum linear velocity.
+	/// If you want the body to wake up when it is sleeping, use BodyInterface::SetLinearVelocity instead.
+	void					SetLinearVelocityClamped(Vec3Arg inLinearVelocity)				{ JPH_ASSERT(!IsStatic()); mMotionProperties->SetLinearVelocityClamped(inLinearVelocity); }
+
+	/// Get world space angular velocity of the center of mass (unit: rad/s)
+	inline Vec3				GetAngularVelocity() const										{ return !IsStatic()? mMotionProperties->GetAngularVelocity() : Vec3::sZero(); }
+
+	/// Set world space angular velocity of the center of mass (unit: rad/s).
+	/// If you want the body to wake up when it is sleeping, use BodyInterface::SetAngularVelocity instead.
+	void					SetAngularVelocity(Vec3Arg inAngularVelocity)					{ JPH_ASSERT(!IsStatic()); mMotionProperties->SetAngularVelocity(inAngularVelocity); }
+
+	/// Set world space angular velocity of the center of mass, will make sure the value is clamped against the maximum angular velocity.
+	/// If you want the body to wake up when it is sleeping, use BodyInterface::SetAngularVelocity instead.
+	void					SetAngularVelocityClamped(Vec3Arg inAngularVelocity)			{ JPH_ASSERT(!IsStatic()); mMotionProperties->SetAngularVelocityClamped(inAngularVelocity); }
+
+	/// Velocity of point inPoint (in center of mass space, e.g. on the surface of the body) of the body (unit: m/s)
+	inline Vec3				GetPointVelocityCOM(Vec3Arg inPointRelativeToCOM) const			{ return !IsStatic()? mMotionProperties->GetPointVelocityCOM(inPointRelativeToCOM) : Vec3::sZero(); }
+
+	/// Velocity of point inPoint (in world space, e.g. on the surface of the body) of the body (unit: m/s)
+	inline Vec3				GetPointVelocity(RVec3Arg inPoint) const						{ JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::Read)); return GetPointVelocityCOM(Vec3(inPoint - mPosition)); }
+
+	/// Add force (unit: N) at center of mass for the next time step, will be reset after the next call to PhysicsSystem::Update.
+	/// If you want the body to wake up when it is sleeping, use BodyInterface::AddForce instead.
+	inline void				AddForce(Vec3Arg inForce)										{ JPH_ASSERT(IsDynamic()); (Vec3::sLoadFloat3Unsafe(mMotionProperties->mForce) + inForce).StoreFloat3(&mMotionProperties->mForce); }
+
+	/// Add force (unit: N) at inPosition for the next time step, will be reset after the next call to PhysicsSystem::Update.
+	/// If you want the body to wake up when it is sleeping, use BodyInterface::AddForce instead.
+	inline void				AddForce(Vec3Arg inForce, RVec3Arg inPosition);
+
+	/// Add torque (unit: N m) for the next time step, will be reset after the next call to PhysicsSystem::Update.
+	/// If you want the body to wake up when it is sleeping, use BodyInterface::AddTorque instead.
+	inline void				AddTorque(Vec3Arg inTorque)										{ JPH_ASSERT(IsDynamic()); (Vec3::sLoadFloat3Unsafe(mMotionProperties->mTorque) + inTorque).StoreFloat3(&mMotionProperties->mTorque); }
+
+	// Get the total amount of force applied to the center of mass this time step (through AddForce calls). Note that it will reset to zero after PhysicsSystem::Update.
+	inline Vec3				GetAccumulatedForce() const										{ JPH_ASSERT(IsDynamic()); return mMotionProperties->GetAccumulatedForce(); }
+
+	// Get the total amount of torque applied to the center of mass this time step (through AddForce/AddTorque calls). Note that it will reset to zero after PhysicsSystem::Update.
+	inline Vec3				GetAccumulatedTorque() const									{ JPH_ASSERT(IsDynamic()); return mMotionProperties->GetAccumulatedTorque(); }
+
+	// Reset the total accumulated force, not that this will be done automatically after every time step.
+	JPH_INLINE void			ResetForce()													{ JPH_ASSERT(IsDynamic()); return mMotionProperties->ResetForce(); }
+
+	// Reset the total accumulated torque, not that this will be done automatically after every time step.
+	JPH_INLINE void			ResetTorque()													{ JPH_ASSERT(IsDynamic()); return mMotionProperties->ResetTorque(); }
+
+	// Reset the current velocity and accumulated force and torque.
+	JPH_INLINE void			ResetMotion()													{ JPH_ASSERT(!IsStatic()); return mMotionProperties->ResetMotion(); }
+
+	/// Get inverse inertia tensor in world space
+	inline Mat44			GetInverseInertia() const;
+
+	/// Add impulse to center of mass (unit: kg m/s).
+	/// If you want the body to wake up when it is sleeping, use BodyInterface::AddImpulse instead.
+	inline void				AddImpulse(Vec3Arg inImpulse);
+
+	/// Add impulse to point in world space (unit: kg m/s).
+	/// If you want the body to wake up when it is sleeping, use BodyInterface::AddImpulse instead.
+	inline void				AddImpulse(Vec3Arg inImpulse, RVec3Arg inPosition);
+
+	/// Add angular impulse in world space (unit: N m s).
+	/// If you want the body to wake up when it is sleeping, use BodyInterface::AddAngularImpulse instead.
+	inline void				AddAngularImpulse(Vec3Arg inAngularImpulse);
+
+	/// Set velocity of body such that it will be positioned at inTargetPosition/Rotation in inDeltaTime seconds.
+	/// If you want the body to wake up when it is sleeping, use BodyInterface::MoveKinematic instead.
+	void					MoveKinematic(RVec3Arg inTargetPosition, QuatArg inTargetRotation, float inDeltaTime);
+
+	/// Gets the properties needed to do buoyancy calculations
+	/// @param inSurfacePosition Position of the fluid surface in world space
+	/// @param inSurfaceNormal Normal of the fluid surface (should point up)
+	/// @param outTotalVolume On return this contains the total volume of the shape
+	/// @param outSubmergedVolume On return this contains the submerged volume of the shape
+	/// @param outRelativeCenterOfBuoyancy On return this contains the center of mass of the submerged volume relative to the center of mass of the body
+	void					GetSubmergedVolume(RVec3Arg inSurfacePosition, Vec3Arg inSurfaceNormal, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outRelativeCenterOfBuoyancy) const;
+
+	/// Applies an impulse to the body that simulates fluid buoyancy and drag.
+	/// If you want the body to wake up when it is sleeping, use BodyInterface::ApplyBuoyancyImpulse instead.
+	/// @param inSurfacePosition Position of the fluid surface in world space
+	/// @param inSurfaceNormal Normal of the fluid surface (should point up)
+	/// @param inBuoyancy The buoyancy factor for the body. 1 = neutral body, < 1 sinks, > 1 floats. Note that we don't use the fluid density since it is harder to configure than a simple number between [0, 2]
+	/// @param inLinearDrag Linear drag factor that slows down the body when in the fluid (approx. 0.5)
+	/// @param inAngularDrag Angular drag factor that slows down rotation when the body is in the fluid (approx. 0.01)
+	/// @param inFluidVelocity The average velocity of the fluid (in m/s) in which the body resides
+	/// @param inGravity The gravity vector (pointing down)
+	/// @param inDeltaTime Delta time of the next simulation step (in s)
+	/// @return true if an impulse was applied, false if the body was not in the fluid
+	bool					ApplyBuoyancyImpulse(RVec3Arg inSurfacePosition, Vec3Arg inSurfaceNormal, float inBuoyancy, float inLinearDrag, float inAngularDrag, Vec3Arg inFluidVelocity, Vec3Arg inGravity, float inDeltaTime);
+
+	/// Applies an impulse to the body that simulates fluid buoyancy and drag.
+	/// If you want the body to wake up when it is sleeping, use BodyInterface::ApplyBuoyancyImpulse instead.
+	/// @param inTotalVolume Total volume of the shape of this body (m^3)
+	/// @param inSubmergedVolume Submerged volume of the shape of this body (m^3)
+	/// @param inRelativeCenterOfBuoyancy The center of mass of the submerged volume relative to the center of mass of the body
+	/// @param inBuoyancy The buoyancy factor for the body. 1 = neutral body, < 1 sinks, > 1 floats. Note that we don't use the fluid density since it is harder to configure than a simple number between [0, 2]
+	/// @param inLinearDrag Linear drag factor that slows down the body when in the fluid (approx. 0.5)
+	/// @param inAngularDrag Angular drag factor that slows down rotation when the body is in the fluid (approx. 0.01)
+	/// @param inFluidVelocity The average velocity of the fluid (in m/s) in which the body resides
+	/// @param inGravity The gravity vector (pointing down)
+	/// @param inDeltaTime Delta time of the next simulation step (in s)
+	/// @return true if an impulse was applied, false if the body was not in the fluid
+	bool					ApplyBuoyancyImpulse(float inTotalVolume, float inSubmergedVolume, Vec3Arg inRelativeCenterOfBuoyancy, float inBuoyancy, float inLinearDrag, float inAngularDrag, Vec3Arg inFluidVelocity, Vec3Arg inGravity, float inDeltaTime);
+
+	/// Check if this body has been added to the physics system
+	inline bool				IsInBroadPhase() const											{ return (mFlags.load(memory_order_relaxed) & uint8(EFlags::IsInBroadPhase)) != 0; }
+
+	/// Check if this body has been changed in such a way that the collision cache should be considered invalid for any body interacting with this body
+	inline bool				IsCollisionCacheInvalid() const									{ return (mFlags.load(memory_order_relaxed) & uint8(EFlags::InvalidateContactCache)) != 0; }
+
+	/// Get the shape of this body
+	inline const Shape *	GetShape() const												{ return mShape; }
+
+	/// World space position of the body
+	inline RVec3			GetPosition() const												{ JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::Read)); return mPosition - mRotation * mShape->GetCenterOfMass(); }
+
+	/// World space rotation of the body
+	inline Quat				GetRotation() const												{ JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::Read)); return mRotation; }
+
+	/// Calculates the transform of this body
+	inline RMat44			GetWorldTransform() const;
+
+	/// Gets the world space position of this body's center of mass
+	inline RVec3			GetCenterOfMassPosition() const									{ JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::Read)); return mPosition; }
+
+	/// Calculates the transform for this body's center of mass
+	inline RMat44			GetCenterOfMassTransform() const;
+
+	/// Calculates the inverse of the transform for this body's center of mass
+	inline RMat44			GetInverseCenterOfMassTransform() const;
+
+	/// Get world space bounding box
+	inline const AABox &	GetWorldSpaceBounds() const										{ return mBounds; }
+
+#ifdef JPH_ENABLE_ASSERTS
+	/// Validate that the cached bounding box of the body matches the actual bounding box of the body.
+	/// If this check fails then there are a number of possible causes:
+	/// 1. Shape is being modified without notifying the system of the change. E.g. if you modify a MutableCompoundShape
+	/// without calling BodyInterface::NotifyShapeChanged then there will be a mismatch between the cached bounding box
+	/// in the broad phase and the bounding box of the Shape.
+	/// 2. You are calling functions postfixed with 'Internal' which are not meant to be called by the application.
+	/// 3. If the actual bounds and cached bounds are very close, it could mean that you have a mismatch in floating
+	/// point unit state between threads. E.g. one thread has flush to zero (FTZ) or denormals are zero (DAZ) set and
+	/// the other thread does not. Or if the rounding mode differs between threads. This can cause small differences
+	/// in floating point calculations. If you are using JobSystemThreadPool you can use JobSystemThreadPool::SetThreadInitFunction
+	/// to initialize the floating point unit state.
+	inline void				ValidateCachedBounds() const
+	{
+		AABox actual_body_bounds = mShape->GetWorldSpaceBounds(GetCenterOfMassTransform(), Vec3::sOne());
+		JPH_ASSERT(actual_body_bounds == mBounds, "Mismatch between cached bounding box and actual bounding box");
+	}
+#endif // JPH_ENABLE_ASSERTS
+
+	/// Access to the motion properties
+	const MotionProperties *GetMotionProperties() const										{ JPH_ASSERT(!IsStatic()); return mMotionProperties; }
+	MotionProperties *		GetMotionProperties()											{ JPH_ASSERT(!IsStatic()); return mMotionProperties; }
+
+	/// Access to the motion properties (version that does not check if the object is kinematic or dynamic)
+	const MotionProperties *GetMotionPropertiesUnchecked() const							{ return mMotionProperties; }
+	MotionProperties *		GetMotionPropertiesUnchecked()									{ return mMotionProperties; }
+
+	/// Access to the user data, can be used for anything by the application
+	uint64					GetUserData() const												{ return mUserData; }
+	void					SetUserData(uint64 inUserData)									{ mUserData = inUserData; }
+
+	/// Get surface normal of a particular sub shape and its world space surface position on this body
+	inline Vec3				GetWorldSpaceSurfaceNormal(const SubShapeID &inSubShapeID, RVec3Arg inPosition) const;
+
+	/// Get the transformed shape of this body, which can be used to do collision detection outside of a body lock
+	inline TransformedShape	GetTransformedShape() const										{ JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::Read)); return TransformedShape(mPosition, mRotation, mShape, mID); }
+
+	/// Debug function to convert a body back to a body creation settings object to be able to save/recreate the body later
+	BodyCreationSettings	GetBodyCreationSettings() const;
+
+	/// Debug function to convert a soft body back to a soft body creation settings object to be able to save/recreate the body later
+	SoftBodyCreationSettings GetSoftBodyCreationSettings() const;
+
+	/// A dummy body that can be used by constraints to attach a constraint to the world instead of another body
+	static Body				sFixedToWorld;
+
+	///@name THESE FUNCTIONS ARE FOR INTERNAL USE ONLY AND SHOULD NOT BE CALLED BY THE APPLICATION
+	///@{
+
+	/// Helper function for BroadPhase::FindCollidingPairs that returns true when two bodies can collide
+	/// It assumes that body 1 is dynamic and active and guarantees that it body 1 collides with body 2 that body 2 will not collide with body 1 in order to avoid finding duplicate collision pairs
+	static inline bool		sFindCollidingPairsCanCollide(const Body &inBody1, const Body &inBody2);
+
+	/// Update position using an Euler step (used during position integrate & constraint solving)
+	inline void				AddPositionStep(Vec3Arg inLinearVelocityTimesDeltaTime)			{ JPH_ASSERT(IsRigidBody()); JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::ReadWrite)); mPosition += mMotionProperties->LockTranslation(inLinearVelocityTimesDeltaTime); JPH_ASSERT(!mPosition.IsNaN()); }
+	inline void				SubPositionStep(Vec3Arg inLinearVelocityTimesDeltaTime)			{ JPH_ASSERT(IsRigidBody()); JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::ReadWrite)); mPosition -= mMotionProperties->LockTranslation(inLinearVelocityTimesDeltaTime); JPH_ASSERT(!mPosition.IsNaN()); }
+
+	/// Update rotation using an Euler step (used during position integrate & constraint solving)
+	inline void				AddRotationStep(Vec3Arg inAngularVelocityTimesDeltaTime);
+	inline void				SubRotationStep(Vec3Arg inAngularVelocityTimesDeltaTime);
+
+	/// Flag if body is in the broadphase (should only be called by the BroadPhase)
+	inline void				SetInBroadPhaseInternal(bool inInBroadPhase)					{ if (inInBroadPhase) mFlags.fetch_or(uint8(EFlags::IsInBroadPhase), memory_order_relaxed); else mFlags.fetch_and(uint8(~uint8(EFlags::IsInBroadPhase)), memory_order_relaxed); }
+
+	/// Invalidate the contact cache (should only be called by the BodyManager), will be reset the next simulation step. Returns true if the contact cache was still valid.
+	inline bool				InvalidateContactCacheInternal()								{ return (mFlags.fetch_or(uint8(EFlags::InvalidateContactCache), memory_order_relaxed) & uint8(EFlags::InvalidateContactCache)) == 0; }
+
+	/// Reset the collision cache invalid flag (should only be called by the BodyManager).
+	inline void				ValidateContactCacheInternal()									{ JPH_IF_ENABLE_ASSERTS(uint8 old_val = ) mFlags.fetch_and(uint8(~uint8(EFlags::InvalidateContactCache)), memory_order_relaxed); JPH_ASSERT((old_val & uint8(EFlags::InvalidateContactCache)) != 0); }
+
+	/// Updates world space bounding box (should only be called by the PhysicsSystem)
+	void					CalculateWorldSpaceBoundsInternal();
+
+	/// Function to update body's position (should only be called by the BodyInterface since it also requires updating the broadphase)
+	void					SetPositionAndRotationInternal(RVec3Arg inPosition, QuatArg inRotation, bool inResetSleepTimer = true);
+
+	/// Updates the center of mass and optionally mass properties after shifting the center of mass or changes to the shape (should only be called by the BodyInterface since it also requires updating the broadphase)
+	/// @param inPreviousCenterOfMass Center of mass of the shape before the alterations
+	/// @param inUpdateMassProperties When true, the mass and inertia tensor is recalculated
+	void					UpdateCenterOfMassInternal(Vec3Arg inPreviousCenterOfMass, bool inUpdateMassProperties);
+
+	/// Function to update a body's shape (should only be called by the BodyInterface since it also requires updating the broadphase)
+	/// @param inShape The new shape for this body
+	/// @param inUpdateMassProperties When true, the mass and inertia tensor is recalculated
+	void					SetShapeInternal(const Shape *inShape, bool inUpdateMassProperties);
+
+	// TSAN detects a race between BodyManager::AddBodyToActiveBodies coming from PhysicsSystem::ProcessBodyPair and Body::GetIndexInActiveBodiesInternal coming from PhysicsSystem::ProcessBodyPair.
+	// When PhysicsSystem::ProcessBodyPair activates a body, it updates mIndexInActiveBodies and then updates BodyManager::mNumActiveBodies with release semantics. PhysicsSystem::ProcessBodyPair will
+	// then finish its loop of active bodies and at the end of the loop it will read BodyManager::mNumActiveBodies with acquire semantics to see if any bodies were activated during the loop.
+	// This means that changes to mIndexInActiveBodies must be visible to the thread, so TSANs report must be a false positive. We suppress the warning here.
+	JPH_TSAN_NO_SANITIZE
+	/// Access to the index in the BodyManager::mActiveBodies list
+	uint32					GetIndexInActiveBodiesInternal() const							{ return mMotionProperties != nullptr? mMotionProperties->mIndexInActiveBodies : cInactiveIndex; }
+
+	/// Update eligibility for sleeping
+	ECanSleep				UpdateSleepStateInternal(float inDeltaTime, float inMaxMovement, float inTimeBeforeSleep);
+
+	/// Saving state for replay
+	void					SaveState(StateRecorder &inStream) const;
+
+	/// Restoring state for replay
+	void					RestoreState(StateRecorder &inStream);
+
+	///@}
+
+	static constexpr uint32	cInactiveIndex = MotionProperties::cInactiveIndex;				///< Constant indicating that body is not active
+
+private:
+	friend class BodyManager;
+	friend class BodyWithMotionProperties;
+	friend class SoftBodyWithMotionPropertiesAndShape;
+
+							Body() = default;												///< Bodies must be created through BodyInterface::CreateBody
+
+	explicit				Body(bool);														///< Alternative constructor that initializes all members
+
+							~Body()															{ JPH_ASSERT(mMotionProperties == nullptr); } ///< Bodies must be destroyed through BodyInterface::DestroyBody
+
+	inline void				GetSleepTestPoints(RVec3 *outPoints) const;						///< Determine points to test for checking if body is sleeping: COM, COM + largest bounding box axis, COM + second largest bounding box axis
+
+	enum class EFlags : uint8
+	{
+		IsSensor						= 1 << 0,											///< If this object is a sensor. A sensor will receive collision callbacks, but will not cause any collision responses and can be used as a trigger volume.
+		CollideKinematicVsNonDynamic	= 1 << 1,											///< If kinematic objects can generate contact points against other kinematic or static objects.
+		IsInBroadPhase					= 1 << 2,											///< Set this bit to indicate that the body is in the broadphase
+		InvalidateContactCache			= 1 << 3,											///< Set this bit to indicate that all collision caches for this body are invalid, will be reset the next simulation step.
+		UseManifoldReduction			= 1 << 4,											///< Set this bit to indicate that this body can use manifold reduction (if PhysicsSettings::mUseManifoldReduction is true)
+		ApplyGyroscopicForce			= 1 << 5,											///< Set this bit to indicate that the gyroscopic force should be applied to this body (aka Dzhanibekov effect, see https://en.wikipedia.org/wiki/Tennis_racket_theorem)
+		EnhancedInternalEdgeRemoval		= 1 << 6,											///< Set this bit to indicate that enhanced internal edge removal should be used for this body (see BodyCreationSettings::mEnhancedInternalEdgeRemoval)
+	};
+
+	// 16 byte aligned
+	RVec3					mPosition;														///< World space position of center of mass
+	Quat					mRotation;														///< World space rotation of center of mass
+	AABox					mBounds;														///< World space bounding box of the body
+
+	// 8 byte aligned
+	RefConst<Shape>			mShape;															///< Shape representing the volume of this body
+	MotionProperties *		mMotionProperties = nullptr;									///< If this is a keyframed or dynamic object, this object holds all information about the movement
+	uint64					mUserData = 0;													///< User data, can be used for anything by the application
+	CollisionGroup			mCollisionGroup;												///< The collision group this body belongs to (determines if two objects can collide)
+
+	// 4 byte aligned
+	float					mFriction;														///< Friction of the body (dimensionless number, usually between 0 and 1, 0 = no friction, 1 = friction force equals force that presses the two bodies together). Note that bodies can have negative friction but the combined friction (see PhysicsSystem::SetCombineFriction) should never go below zero.
+	float					mRestitution;													///< Restitution of body (dimensionless number, usually between 0 and 1, 0 = completely inelastic collision response, 1 = completely elastic collision response). Note that bodies can have negative restitution but the combined restitution (see PhysicsSystem::SetCombineRestitution) should never go below zero.
+	BodyID					mID;															///< ID of the body (index in the bodies array)
+
+	// 2 or 4 bytes aligned
+	ObjectLayer				mObjectLayer;													///< The collision layer this body belongs to (determines if two objects can collide)
+
+	// 1 byte aligned
+	EBodyType				mBodyType;														///< Type of body (rigid or soft)
+	BroadPhaseLayer			mBroadPhaseLayer;												///< The broad phase layer this body belongs to
+	EMotionType				mMotionType;													///< Type of motion (static, dynamic or kinematic)
+	atomic<uint8>			mFlags = 0;														///< See EFlags for possible flags
+
+	// 122 bytes up to here (64-bit mode, single precision, 16-bit ObjectLayer)
+};
+
+static_assert(JPH_CPU_ADDRESS_BITS != 64 || sizeof(Body) == JPH_IF_SINGLE_PRECISION_ELSE(128, 160), "Body size is incorrect");
+static_assert(alignof(Body) == JPH_RVECTOR_ALIGNMENT, "Body should properly align");
+
+JPH_NAMESPACE_END
+
+#include "Body.inl"

thirdparty/jolt_physics/Jolt/Physics/Body/Body.inl

@@ -0,0 +1,197 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+RMat44 Body::GetWorldTransform() const
+{
+	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::Read));
+
+	return RMat44::sRotationTranslation(mRotation, mPosition).PreTranslated(-mShape->GetCenterOfMass());
+}
+
+RMat44 Body::GetCenterOfMassTransform() const
+{
+	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::Read));
+
+	return RMat44::sRotationTranslation(mRotation, mPosition);
+}
+
+RMat44 Body::GetInverseCenterOfMassTransform() const
+{
+	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::Read));
+
+	return RMat44::sInverseRotationTranslation(mRotation, mPosition);
+}
+
+inline bool Body::sFindCollidingPairsCanCollide(const Body &inBody1, const Body &inBody2)
+{
+	// First body should never be a soft body
+	JPH_ASSERT(!inBody1.IsSoftBody());
+
+	// One of these conditions must be true
+	// - We always allow detecting collisions between kinematic and non-dynamic bodies
+	// - One of the bodies must be dynamic to collide
+	// - A kinematic object can collide with a sensor
+	if (!inBody1.GetCollideKinematicVsNonDynamic()
+		&& !inBody2.GetCollideKinematicVsNonDynamic()
+		&& (!inBody1.IsDynamic() && !inBody2.IsDynamic())
+		&& !(inBody1.IsKinematic() && inBody2.IsSensor())
+		&& !(inBody2.IsKinematic() && inBody1.IsSensor()))
+		return false;
+
+	// Check that body 1 is active
+	uint32 body1_index_in_active_bodies = inBody1.GetIndexInActiveBodiesInternal();
+	JPH_ASSERT(!inBody1.IsStatic() && body1_index_in_active_bodies != Body::cInactiveIndex, "This function assumes that Body 1 is active");
+
+	// If the pair A, B collides we need to ensure that the pair B, A does not collide or else we will handle the collision twice.
+	// If A is the same body as B we don't want to collide (1)
+	// If A is dynamic / kinematic and B is static we should collide (2)
+	// If A is dynamic / kinematic and B is dynamic / kinematic we should only collide if
+	//	- A is active and B is not active (3)
+	//	- A is active and B will become active during this simulation step (4)
+	//	- A is active and B is active, we require a condition that makes A, B collide and B, A not (5)
+	//
+	// In order to implement this we use the index in the active body list and make use of the fact that
+	// a body not in the active list has Body.Index = 0xffffffff which is the highest possible value for an uint32.
+	//
+	// Because we know that A is active we know that A.Index != 0xffffffff:
+	// (1) Because A.Index != 0xffffffff, if A.Index = B.Index then A = B, so to collide A.Index != B.Index
+	// (2) A.Index != 0xffffffff, B.Index = 0xffffffff (because it's static and cannot be in the active list), so to collide A.Index != B.Index
+	// (3) A.Index != 0xffffffff, B.Index = 0xffffffff (because it's not yet active), so to collide A.Index != B.Index
+	// (4) A.Index != 0xffffffff, B.Index = 0xffffffff currently. But it can activate during the Broad/NarrowPhase step at which point it
+	//     will be added to the end of the active list which will make B.Index > A.Index (this holds only true when we don't deactivate
+	//     bodies during the Broad/NarrowPhase step), so to collide A.Index < B.Index.
+	// (5) As tie breaker we can use the same condition A.Index < B.Index to collide, this means that if A, B collides then B, A won't
+	static_assert(Body::cInactiveIndex == 0xffffffff, "The algorithm below uses this value");
+	if (!inBody2.IsSoftBody() && body1_index_in_active_bodies >= inBody2.GetIndexInActiveBodiesInternal())
+		return false;
+	JPH_ASSERT(inBody1.GetID() != inBody2.GetID(), "Read the comment above, A and B are the same body which should not be possible!");
+
+	// Check collision group filter
+	if (!inBody1.GetCollisionGroup().CanCollide(inBody2.GetCollisionGroup()))
+		return false;
+
+	return true;
+}
+
+void Body::AddRotationStep(Vec3Arg inAngularVelocityTimesDeltaTime)
+{
+	JPH_ASSERT(IsRigidBody());
+	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::ReadWrite));
+
+	// This used to use the equation: d/dt R(t) = 1/2 * w(t) * R(t) so that R(t + dt) = R(t) + 1/2 * w(t) * R(t) * dt
+	// See: Appendix B of An Introduction to Physically Based Modeling: Rigid Body Simulation II-Nonpenetration Constraints
+	// URL: https://www.cs.cmu.edu/~baraff/sigcourse/notesd2.pdf
+	// But this is a first order approximation and does not work well for kinematic ragdolls that are driven to a new
+	// pose if the poses differ enough. So now we split w(t) * dt into an axis and angle part and create a quaternion with it.
+	// Note that the resulting quaternion is normalized since otherwise numerical drift will eventually make the rotation non-normalized.
+	float len = inAngularVelocityTimesDeltaTime.Length();
+	if (len > 1.0e-6f)
+	{
+		mRotation = (Quat::sRotation(inAngularVelocityTimesDeltaTime / len, len) * mRotation).Normalized();
+		JPH_ASSERT(!mRotation.IsNaN());
+	}
+}
+
+void Body::SubRotationStep(Vec3Arg inAngularVelocityTimesDeltaTime)
+{
+	JPH_ASSERT(IsRigidBody());
+	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::ReadWrite));
+
+	// See comment at Body::AddRotationStep
+	float len = inAngularVelocityTimesDeltaTime.Length();
+	if (len > 1.0e-6f)
+	{
+		mRotation = (Quat::sRotation(inAngularVelocityTimesDeltaTime / len, -len) * mRotation).Normalized();
+		JPH_ASSERT(!mRotation.IsNaN());
+	}
+}
+
+Vec3 Body::GetWorldSpaceSurfaceNormal(const SubShapeID &inSubShapeID, RVec3Arg inPosition) const
+{
+	RMat44 inv_com = GetInverseCenterOfMassTransform();
+	return inv_com.Multiply3x3Transposed(mShape->GetSurfaceNormal(inSubShapeID, Vec3(inv_com * inPosition))).Normalized();
+}
+
+Mat44 Body::GetInverseInertia() const
+{
+	JPH_ASSERT(IsDynamic());
+
+	return GetMotionProperties()->GetInverseInertiaForRotation(Mat44::sRotation(mRotation));
+}
+
+void Body::AddForce(Vec3Arg inForce, RVec3Arg inPosition)
+{
+	AddForce(inForce);
+	AddTorque(Vec3(inPosition - mPosition).Cross(inForce));
+}
+
+void Body::AddImpulse(Vec3Arg inImpulse)
+{
+	JPH_ASSERT(IsDynamic());
+
+	SetLinearVelocityClamped(mMotionProperties->GetLinearVelocity() + inImpulse * mMotionProperties->GetInverseMass());
+}
+
+void Body::AddImpulse(Vec3Arg inImpulse, RVec3Arg inPosition)
+{
+	JPH_ASSERT(IsDynamic());
+
+	SetLinearVelocityClamped(mMotionProperties->GetLinearVelocity() + inImpulse * mMotionProperties->GetInverseMass());
+
+	SetAngularVelocityClamped(mMotionProperties->GetAngularVelocity() + mMotionProperties->MultiplyWorldSpaceInverseInertiaByVector(mRotation, Vec3(inPosition - mPosition).Cross(inImpulse)));
+}
+
+void Body::AddAngularImpulse(Vec3Arg inAngularImpulse)
+{
+	JPH_ASSERT(IsDynamic());
+
+	SetAngularVelocityClamped(mMotionProperties->GetAngularVelocity() + mMotionProperties->MultiplyWorldSpaceInverseInertiaByVector(mRotation, inAngularImpulse));
+}
+
+void Body::GetSleepTestPoints(RVec3 *outPoints) const
+{
+	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::Read));
+
+	// Center of mass is the first position
+	outPoints[0] = mPosition;
+
+	// The second and third position are on the largest axis of the bounding box
+	Vec3 extent = mShape->GetLocalBounds().GetExtent();
+	int lowest_component = extent.GetLowestComponentIndex();
+	Mat44 rotation = Mat44::sRotation(mRotation);
+	switch (lowest_component)
+	{
+	case 0:
+		outPoints[1] = mPosition + extent.GetY() * rotation.GetColumn3(1);
+		outPoints[2] = mPosition + extent.GetZ() * rotation.GetColumn3(2);
+		break;
+
+	case 1:
+		outPoints[1] = mPosition + extent.GetX() * rotation.GetColumn3(0);
+		outPoints[2] = mPosition + extent.GetZ() * rotation.GetColumn3(2);
+		break;
+
+	case 2:
+		outPoints[1] = mPosition + extent.GetX() * rotation.GetColumn3(0);
+		outPoints[2] = mPosition + extent.GetY() * rotation.GetColumn3(1);
+		break;
+
+	default:
+		JPH_ASSERT(false);
+		break;
+	}
+}
+
+void Body::ResetSleepTimer()
+{
+	RVec3 points[3];
+	GetSleepTestPoints(points);
+	mMotionProperties->ResetSleepTestSpheres(points);
+}
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Body/BodyAccess.h

@@ -0,0 +1,68 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#ifdef JPH_ENABLE_ASSERTS
+
+JPH_NAMESPACE_BEGIN
+
+class JPH_EXPORT BodyAccess
+{
+public:
+	/// Access rules, used to detect race conditions during simulation
+	enum class EAccess : uint8
+	{
+		None		= 0,
+		Read		= 1,
+		ReadWrite	= 3,
+	};
+
+	/// Grant a scope specific access rights on the current thread
+	class Grant
+	{
+	public:
+		inline							Grant(EAccess inVelocity, EAccess inPosition)
+		{
+			EAccess &velocity = sVelocityAccess();
+			EAccess &position = sPositionAccess();
+
+			JPH_ASSERT(velocity == EAccess::ReadWrite);
+			JPH_ASSERT(position == EAccess::ReadWrite);
+
+			velocity = inVelocity;
+			position = inPosition;
+		}
+
+		inline							~Grant()
+		{
+			sVelocityAccess() = EAccess::ReadWrite;
+			sPositionAccess() = EAccess::ReadWrite;
+		}
+	};
+
+	/// Check if we have permission
+	static inline bool					sCheckRights(EAccess inRights, EAccess inDesiredRights)
+	{
+		return (uint8(inRights) & uint8(inDesiredRights)) == uint8(inDesiredRights);
+	}
+
+	/// Access to read/write velocities
+	static inline EAccess &				sVelocityAccess()
+	{
+		static thread_local EAccess sAccess = BodyAccess::EAccess::ReadWrite;
+		return sAccess;
+	}
+
+	/// Access to read/write positions
+	static inline EAccess &				sPositionAccess()
+	{
+		static thread_local EAccess sAccess = BodyAccess::EAccess::ReadWrite;
+		return sAccess;
+	}
+};
+
+JPH_NAMESPACE_END
+
+#endif // JPH_ENABLE_ASSERTS

thirdparty/jolt_physics/Jolt/Physics/Body/BodyActivationListener.h

@@ -0,0 +1,28 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+class BodyID;
+
+/// A listener class that receives events when a body activates or deactivates.
+/// It can be registered with the BodyManager (or PhysicsSystem).
+class BodyActivationListener
+{
+public:
+	/// Ensure virtual destructor
+	virtual					~BodyActivationListener() = default;
+
+	/// Called whenever a body activates, note this can be called from any thread so make sure your code is thread safe.
+	/// At the time of the callback the body inBodyID will be locked and no bodies can be written/activated/deactivated from the callback.
+	virtual void			OnBodyActivated(const BodyID &inBodyID, uint64 inBodyUserData) = 0;
+
+	/// Called whenever a body deactivates, note this can be called from any thread so make sure your code is thread safe.
+	/// At the time of the callback the body inBodyID will be locked and no bodies can be written/activated/deactivated from the callback.
+	virtual void			OnBodyDeactivated(const BodyID &inBodyID, uint64 inBodyUserData) = 0;
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Body/BodyCreationSettings.cpp

@@ -0,0 +1,234 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(BodyCreationSettings)
+{
+	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mPosition)
+	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mRotation)
+	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mLinearVelocity)
+	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mAngularVelocity)
+	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mUserData)
+	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mShape)
+	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mCollisionGroup)
+	JPH_ADD_ENUM_ATTRIBUTE(BodyCreationSettings, mObjectLayer)
+	JPH_ADD_ENUM_ATTRIBUTE(BodyCreationSettings, mMotionType)
+	JPH_ADD_ENUM_ATTRIBUTE(BodyCreationSettings, mAllowedDOFs)
+	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mAllowDynamicOrKinematic)
+	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mIsSensor)
+	JPH_ADD_ATTRIBUTE_WITH_ALIAS(BodyCreationSettings, mCollideKinematicVsNonDynamic, "mSensorDetectsStatic") // This is the old name to keep backwards compatibility
+	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mUseManifoldReduction)
+	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mApplyGyroscopicForce)
+	JPH_ADD_ENUM_ATTRIBUTE(BodyCreationSettings, mMotionQuality)
+	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mEnhancedInternalEdgeRemoval)
+	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mAllowSleeping)
+	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mFriction)
+	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mRestitution)
+	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mLinearDamping)
+	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mAngularDamping)
+	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mMaxLinearVelocity)
+	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mMaxAngularVelocity)
+	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mGravityFactor)
+	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mNumVelocityStepsOverride)
+	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mNumPositionStepsOverride)
+	JPH_ADD_ENUM_ATTRIBUTE(BodyCreationSettings, mOverrideMassProperties)
+	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mInertiaMultiplier)
+	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mMassPropertiesOverride)
+}
+
+void BodyCreationSettings::SaveBinaryState(StreamOut &inStream) const
+{
+	inStream.Write(mPosition);
+	inStream.Write(mRotation);
+	inStream.Write(mLinearVelocity);
+	inStream.Write(mAngularVelocity);
+	mCollisionGroup.SaveBinaryState(inStream);
+	inStream.Write(mObjectLayer);
+	inStream.Write(mMotionType);
+	inStream.Write(mAllowedDOFs);
+	inStream.Write(mAllowDynamicOrKinematic);
+	inStream.Write(mIsSensor);
+	inStream.Write(mCollideKinematicVsNonDynamic);
+	inStream.Write(mUseManifoldReduction);
+	inStream.Write(mApplyGyroscopicForce);
+	inStream.Write(mMotionQuality);
+	inStream.Write(mEnhancedInternalEdgeRemoval);
+	inStream.Write(mAllowSleeping);
+	inStream.Write(mFriction);
+	inStream.Write(mRestitution);
+	inStream.Write(mLinearDamping);
+	inStream.Write(mAngularDamping);
+	inStream.Write(mMaxLinearVelocity);
+	inStream.Write(mMaxAngularVelocity);
+	inStream.Write(mGravityFactor);
+	inStream.Write(mNumVelocityStepsOverride);
+	inStream.Write(mNumPositionStepsOverride);
+	inStream.Write(mOverrideMassProperties);
+	inStream.Write(mInertiaMultiplier);
+	mMassPropertiesOverride.SaveBinaryState(inStream);
+}
+
+void BodyCreationSettings::RestoreBinaryState(StreamIn &inStream)
+{
+	inStream.Read(mPosition);
+	inStream.Read(mRotation);
+	inStream.Read(mLinearVelocity);
+	inStream.Read(mAngularVelocity);
+	mCollisionGroup.RestoreBinaryState(inStream);
+	inStream.Read(mObjectLayer);
+	inStream.Read(mMotionType);
+	inStream.Read(mAllowedDOFs);
+	inStream.Read(mAllowDynamicOrKinematic);
+	inStream.Read(mIsSensor);
+	inStream.Read(mCollideKinematicVsNonDynamic);
+	inStream.Read(mUseManifoldReduction);
+	inStream.Read(mApplyGyroscopicForce);
+	inStream.Read(mMotionQuality);
+	inStream.Read(mEnhancedInternalEdgeRemoval);
+	inStream.Read(mAllowSleeping);
+	inStream.Read(mFriction);
+	inStream.Read(mRestitution);
+	inStream.Read(mLinearDamping);
+	inStream.Read(mAngularDamping);
+	inStream.Read(mMaxLinearVelocity);
+	inStream.Read(mMaxAngularVelocity);
+	inStream.Read(mGravityFactor);
+	inStream.Read(mNumVelocityStepsOverride);
+	inStream.Read(mNumPositionStepsOverride);
+	inStream.Read(mOverrideMassProperties);
+	inStream.Read(mInertiaMultiplier);
+	mMassPropertiesOverride.RestoreBinaryState(inStream);
+}
+
+Shape::ShapeResult BodyCreationSettings::ConvertShapeSettings()
+{
+	// If we already have a shape, return it
+	if (mShapePtr != nullptr)
+	{
+		mShape = nullptr;
+
+		Shape::ShapeResult result;
+		result.Set(const_cast<Shape *>(mShapePtr.GetPtr()));
+		return result;
+	}
+
+	// Check if we have shape settings
+	if (mShape == nullptr)
+	{
+		Shape::ShapeResult result;
+		result.SetError("No shape present!");
+		return result;
+	}
+
+	// Create the shape
+	Shape::ShapeResult result = mShape->Create();
+	if (result.IsValid())
+		mShapePtr = result.Get();
+	mShape = nullptr;
+	return result;
+}
+
+const Shape *BodyCreationSettings::GetShape() const
+{
+	// If we already have a shape, return it
+	if (mShapePtr != nullptr)
+		return mShapePtr;
+
+	// Check if we have shape settings
+	if (mShape == nullptr)
+		return nullptr;
+
+	// Create the shape
+	Shape::ShapeResult result = mShape->Create();
+	if (result.IsValid())
+		return result.Get();
+
+	Trace("Error: %s", result.GetError().c_str());
+	JPH_ASSERT(false, "An error occurred during shape creation. Use ConvertShapeSettings() to convert the shape and get the error!");
+	return nullptr;
+}
+
+MassProperties BodyCreationSettings::GetMassProperties() const
+{
+	// Calculate mass properties
+	MassProperties mass_properties;
+	switch (mOverrideMassProperties)
+	{
+	case EOverrideMassProperties::CalculateMassAndInertia:
+		mass_properties = GetShape()->GetMassProperties();
+		mass_properties.mInertia *= mInertiaMultiplier;
+		mass_properties.mInertia(3, 3) = 1.0f;
+		break;
+	case EOverrideMassProperties::CalculateInertia:
+		mass_properties = GetShape()->GetMassProperties();
+		mass_properties.ScaleToMass(mMassPropertiesOverride.mMass);
+		mass_properties.mInertia *= mInertiaMultiplier;
+		mass_properties.mInertia(3, 3) = 1.0f;
+		break;
+	case EOverrideMassProperties::MassAndInertiaProvided:
+		mass_properties = mMassPropertiesOverride;
+		break;
+	}
+	return mass_properties;
+}
+
+void BodyCreationSettings::SaveWithChildren(StreamOut &inStream, ShapeToIDMap *ioShapeMap, MaterialToIDMap *ioMaterialMap, GroupFilterToIDMap *ioGroupFilterMap) const
+{
+	// Save creation settings
+	SaveBinaryState(inStream);
+
+	// Save shape
+	if (ioShapeMap != nullptr && ioMaterialMap != nullptr)
+		GetShape()->SaveWithChildren(inStream, *ioShapeMap, *ioMaterialMap);
+	else
+		inStream.Write(~uint32(0));
+
+	// Save group filter
+	StreamUtils::SaveObjectReference(inStream, mCollisionGroup.GetGroupFilter(), ioGroupFilterMap);
+}
+
+BodyCreationSettings::BCSResult BodyCreationSettings::sRestoreWithChildren(StreamIn &inStream, IDToShapeMap &ioShapeMap, IDToMaterialMap &ioMaterialMap, IDToGroupFilterMap &ioGroupFilterMap)
+{
+	BCSResult result;
+
+	// Read creation settings
+	BodyCreationSettings settings;
+	settings.RestoreBinaryState(inStream);
+	if (inStream.IsEOF() || inStream.IsFailed())
+	{
+		result.SetError("Error reading body creation settings");
+		return result;
+	}
+
+	// Read shape
+	Shape::ShapeResult shape_result = Shape::sRestoreWithChildren(inStream, ioShapeMap, ioMaterialMap);
+	if (shape_result.HasError())
+	{
+		result.SetError(shape_result.GetError());
+		return result;
+	}
+	settings.SetShape(shape_result.Get());
+
+	// Read group filter
+	Result gfresult = StreamUtils::RestoreObjectReference(inStream, ioGroupFilterMap);
+	if (gfresult.HasError())
+	{
+		result.SetError(gfresult.GetError());
+		return result;
+	}
+	settings.mCollisionGroup.SetGroupFilter(gfresult.Get());
+
+	result.Set(settings);
+	return result;
+}
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Body/BodyCreationSettings.h

@@ -0,0 +1,124 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/Shape/Shape.h>
+#include <Jolt/Physics/Collision/ObjectLayer.h>
+#include <Jolt/Physics/Collision/CollisionGroup.h>
+#include <Jolt/Physics/Body/MotionType.h>
+#include <Jolt/Physics/Body/MotionQuality.h>
+#include <Jolt/Physics/Body/AllowedDOFs.h>
+#include <Jolt/ObjectStream/SerializableObject.h>
+#include <Jolt/Core/StreamUtils.h>
+
+JPH_NAMESPACE_BEGIN
+
+class StreamIn;
+class StreamOut;
+
+/// Enum used in BodyCreationSettings to indicate how mass and inertia should be calculated
+enum class EOverrideMassProperties : uint8
+{
+	CalculateMassAndInertia,			///< Tells the system to calculate the mass and inertia based on density
+	CalculateInertia,					///< Tells the system to take the mass from mMassPropertiesOverride and to calculate the inertia based on density of the shapes and to scale it to the provided mass
+	MassAndInertiaProvided				///< Tells the system to take the mass and inertia from mMassPropertiesOverride
+};
+
+/// Settings for constructing a rigid body
+class JPH_EXPORT BodyCreationSettings
+{
+	JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, BodyCreationSettings)
+
+public:
+	/// Constructor
+							BodyCreationSettings() = default;
+							BodyCreationSettings(const ShapeSettings *inShape, RVec3Arg inPosition, QuatArg inRotation, EMotionType inMotionType, ObjectLayer inObjectLayer) : mPosition(inPosition), mRotation(inRotation), mObjectLayer(inObjectLayer), mMotionType(inMotionType), mShape(inShape) { }
+							BodyCreationSettings(const Shape *inShape, RVec3Arg inPosition, QuatArg inRotation, EMotionType inMotionType, ObjectLayer inObjectLayer) : mPosition(inPosition), mRotation(inRotation), mObjectLayer(inObjectLayer), mMotionType(inMotionType), mShapePtr(inShape) { }
+
+	/// Access to the shape settings object. This contains serializable (non-runtime optimized) information about the Shape.
+	const ShapeSettings *	GetShapeSettings() const										{ return mShape; }
+	void					SetShapeSettings(const ShapeSettings *inShape)					{ mShape = inShape; mShapePtr = nullptr; }
+
+	/// Convert ShapeSettings object into a Shape object. This will free the ShapeSettings object and make the object ready for runtime. Serialization is no longer possible after this.
+	Shape::ShapeResult		ConvertShapeSettings();
+
+	/// Access to the run-time shape object. Will convert from ShapeSettings object if needed.
+	const Shape *			GetShape() const;
+	void					SetShape(const Shape *inShape)									{ mShapePtr = inShape; mShape = nullptr; }
+
+	/// Check if the mass properties of this body will be calculated (only relevant for kinematic or dynamic objects that need a MotionProperties object)
+	bool					HasMassProperties() const										{ return mAllowDynamicOrKinematic || mMotionType != EMotionType::Static; }
+
+	/// Calculate (or return when overridden) the mass and inertia for this body
+	MassProperties			GetMassProperties() const;
+
+	/// Saves the state of this object in binary form to inStream. Doesn't store the shape nor the group filter.
+	void					SaveBinaryState(StreamOut &inStream) const;
+
+	/// Restore the state of this object from inStream. Doesn't restore the shape nor the group filter.
+	void					RestoreBinaryState(StreamIn &inStream);
+
+	using GroupFilterToIDMap = StreamUtils::ObjectToIDMap<GroupFilter>;
+	using IDToGroupFilterMap = StreamUtils::IDToObjectMap<GroupFilter>;
+	using ShapeToIDMap = Shape::ShapeToIDMap;
+	using IDToShapeMap = Shape::IDToShapeMap;
+	using MaterialToIDMap = StreamUtils::ObjectToIDMap<PhysicsMaterial>;
+	using IDToMaterialMap = StreamUtils::IDToObjectMap<PhysicsMaterial>;
+
+	/// Save body creation settings, its shape, materials and group filter. Pass in an empty map in ioShapeMap / ioMaterialMap / ioGroupFilterMap or reuse the same map while saving multiple shapes to the same stream in order to avoid writing duplicates.
+	/// Pass nullptr to ioShapeMap and ioMaterial map to skip saving shapes
+	/// Pass nullptr to ioGroupFilterMap to skip saving group filters
+	void					SaveWithChildren(StreamOut &inStream, ShapeToIDMap *ioShapeMap, MaterialToIDMap *ioMaterialMap, GroupFilterToIDMap *ioGroupFilterMap) const;
+
+	using BCSResult = Result<BodyCreationSettings>;
+
+	/// Restore body creation settings, its shape, materials and group filter. Pass in an empty map in ioShapeMap / ioMaterialMap / ioGroupFilterMap or reuse the same map while reading multiple shapes from the same stream in order to restore duplicates.
+	static BCSResult		sRestoreWithChildren(StreamIn &inStream, IDToShapeMap &ioShapeMap, IDToMaterialMap &ioMaterialMap, IDToGroupFilterMap &ioGroupFilterMap);
+
+	RVec3					mPosition = RVec3::sZero();										///< Position of the body (not of the center of mass)
+	Quat					mRotation = Quat::sIdentity();									///< Rotation of the body
+	Vec3					mLinearVelocity = Vec3::sZero();								///< World space linear velocity of the center of mass (m/s)
+	Vec3					mAngularVelocity = Vec3::sZero();								///< World space angular velocity (rad/s)
+
+	/// User data value (can be used by application)
+	uint64					mUserData = 0;
+
+	///@name Collision settings
+	ObjectLayer				mObjectLayer = 0;												///< The collision layer this body belongs to (determines if two objects can collide)
+	CollisionGroup			mCollisionGroup;												///< The collision group this body belongs to (determines if two objects can collide)
+
+	///@name Simulation properties
+	EMotionType				mMotionType = EMotionType::Dynamic;								///< Motion type, determines if the object is static, dynamic or kinematic
+	EAllowedDOFs			mAllowedDOFs = EAllowedDOFs::All;								///< Which degrees of freedom this body has (can be used to limit simulation to 2D)
+	bool					mAllowDynamicOrKinematic = false;								///< When this body is created as static, this setting tells the system to create a MotionProperties object so that the object can be switched to kinematic or dynamic
+	bool					mIsSensor = false;												///< If this body is a sensor. A sensor will receive collision callbacks, but will not cause any collision responses and can be used as a trigger volume. See description at Body::SetIsSensor.
+	bool					mCollideKinematicVsNonDynamic = false;							///< If kinematic objects can generate contact points against other kinematic or static objects. See description at Body::SetCollideKinematicVsNonDynamic.
+	bool					mUseManifoldReduction = true;									///< If this body should use manifold reduction (see description at Body::SetUseManifoldReduction)
+	bool					mApplyGyroscopicForce = false;									///< Set to indicate that the gyroscopic force should be applied to this body (aka Dzhanibekov effect, see https://en.wikipedia.org/wiki/Tennis_racket_theorem)
+	EMotionQuality			mMotionQuality = EMotionQuality::Discrete;						///< Motion quality, or how well it detects collisions when it has a high velocity
+	bool					mEnhancedInternalEdgeRemoval = false;							///< Set to indicate that extra effort should be made to try to remove ghost contacts (collisions with internal edges of a mesh). This is more expensive but makes bodies move smoother over a mesh with convex edges.
+	bool					mAllowSleeping = true;											///< If this body can go to sleep or not
+	float					mFriction = 0.2f;												///< Friction of the body (dimensionless number, usually between 0 and 1, 0 = no friction, 1 = friction force equals force that presses the two bodies together). Note that bodies can have negative friction but the combined friction (see PhysicsSystem::SetCombineFriction) should never go below zero.
+	float					mRestitution = 0.0f;											///< Restitution of body (dimensionless number, usually between 0 and 1, 0 = completely inelastic collision response, 1 = completely elastic collision response). Note that bodies can have negative restitution but the combined restitution (see PhysicsSystem::SetCombineRestitution) should never go below zero.
+	float					mLinearDamping = 0.05f;											///< Linear damping: dv/dt = -c * v. c must be between 0 and 1 but is usually close to 0.
+	float					mAngularDamping = 0.05f;										///< Angular damping: dw/dt = -c * w. c must be between 0 and 1 but is usually close to 0.
+	float					mMaxLinearVelocity = 500.0f;									///< Maximum linear velocity that this body can reach (m/s)
+	float					mMaxAngularVelocity = 0.25f * JPH_PI * 60.0f;					///< Maximum angular velocity that this body can reach (rad/s)
+	float					mGravityFactor = 1.0f;											///< Value to multiply gravity with for this body
+	uint					mNumVelocityStepsOverride = 0;									///< Used only when this body is dynamic and colliding. Override for the number of solver velocity iterations to run, 0 means use the default in PhysicsSettings::mNumVelocitySteps. The number of iterations to use is the max of all contacts and constraints in the island.
+	uint					mNumPositionStepsOverride = 0;									///< Used only when this body is dynamic and colliding. Override for the number of solver position iterations to run, 0 means use the default in PhysicsSettings::mNumPositionSteps. The number of iterations to use is the max of all contacts and constraints in the island.
+
+	///@name Mass properties of the body (by default calculated by the shape)
+	EOverrideMassProperties	mOverrideMassProperties = EOverrideMassProperties::CalculateMassAndInertia; ///< Determines how mMassPropertiesOverride will be used
+	float					mInertiaMultiplier = 1.0f;										///< When calculating the inertia (not when it is provided) the calculated inertia will be multiplied by this value
+	MassProperties			mMassPropertiesOverride;										///< Contains replacement mass settings which override the automatically calculated values
+
+private:
+	/// Collision volume for the body
+	RefConst<ShapeSettings>	mShape;															///< Shape settings, can be serialized. Mutually exclusive with mShapePtr
+	RefConst<Shape>			mShapePtr;														///< Actual shape, cannot be serialized. Mutually exclusive with mShape
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Body/BodyFilter.h

@@ -0,0 +1,130 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/NonCopyable.h>
+#include <Jolt/Physics/Body/BodyID.h>
+
+JPH_NAMESPACE_BEGIN
+
+class Body;
+
+/// Class function to filter out bodies, returns true if test should collide with body
+class JPH_EXPORT BodyFilter : public NonCopyable
+{
+public:
+	/// Destructor
+	virtual					~BodyFilter() = default;
+
+	/// Filter function. Returns true if we should collide with inBodyID
+	virtual bool			ShouldCollide([[maybe_unused]] const BodyID &inBodyID) const
+	{
+		return true;
+	}
+
+	/// Filter function. Returns true if we should collide with inBody (this is called after the body is locked and makes it possible to filter based on body members)
+	virtual bool			ShouldCollideLocked([[maybe_unused]] const Body &inBody) const
+	{
+		return true;
+	}
+};
+
+/// A simple body filter implementation that ignores a single, specified body
+class JPH_EXPORT IgnoreSingleBodyFilter : public BodyFilter
+{
+public:
+	/// Constructor, pass the body you want to ignore
+	explicit				IgnoreSingleBodyFilter(const BodyID &inBodyID) :
+		mBodyID(inBodyID)
+	{
+	}
+
+	/// Filter function. Returns true if we should collide with inBodyID
+	virtual bool			ShouldCollide(const BodyID &inBodyID) const override
+	{
+		return mBodyID != inBodyID;
+	}
+
+private:
+	BodyID					mBodyID;
+};
+
+/// A simple body filter implementation that ignores multiple, specified bodies
+class JPH_EXPORT IgnoreMultipleBodiesFilter : public BodyFilter
+{
+public:
+	/// Remove all bodies from the filter
+	void					Clear()
+	{
+		mBodyIDs.clear();
+	}
+
+	/// Reserve space for inSize body ID's
+	void					Reserve(uint inSize)
+	{
+		mBodyIDs.reserve(inSize);
+	}
+
+	/// Add a body to be ignored
+	void					IgnoreBody(const BodyID &inBodyID)
+	{
+		mBodyIDs.push_back(inBodyID);
+	}
+
+	/// Filter function. Returns true if we should collide with inBodyID
+	virtual bool			ShouldCollide(const BodyID &inBodyID) const override
+	{
+		return std::find(mBodyIDs.begin(), mBodyIDs.end(), inBodyID) == mBodyIDs.end();
+	}
+
+private:
+	Array<BodyID>			mBodyIDs;
+};
+
+/// Ignores a single body and chains the filter to another filter
+class JPH_EXPORT IgnoreSingleBodyFilterChained : public BodyFilter
+{
+public:
+	/// Constructor
+	explicit				IgnoreSingleBodyFilterChained(const BodyID inBodyID, const BodyFilter &inFilter) :
+		mBodyID(inBodyID),
+		mFilter(inFilter)
+	{
+	}
+
+	/// Filter function. Returns true if we should collide with inBodyID
+	virtual bool			ShouldCollide(const BodyID &inBodyID) const override
+	{
+		return inBodyID != mBodyID && mFilter.ShouldCollide(inBodyID);
+	}
+
+	/// Filter function. Returns true if we should collide with inBody (this is called after the body is locked and makes it possible to filter based on body members)
+	virtual bool			ShouldCollideLocked(const Body &inBody) const override
+	{
+		return mFilter.ShouldCollideLocked(inBody);
+	}
+
+private:
+	BodyID					mBodyID;
+	const BodyFilter &		mFilter;
+};
+
+#ifdef JPH_DEBUG_RENDERER
+/// Class function to filter out bodies for debug rendering, returns true if body should be rendered
+class JPH_EXPORT BodyDrawFilter : public NonCopyable
+{
+public:
+	/// Destructor
+	virtual					~BodyDrawFilter() = default;
+
+	/// Filter function. Returns true if inBody should be rendered
+	virtual bool			ShouldDraw([[maybe_unused]] const Body& inBody) const
+	{
+		return true;
+	}
+};
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Body/BodyID.h

@@ -0,0 +1,101 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/HashCombine.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// ID of a body. This is a way of reasoning about bodies in a multithreaded simulation while avoiding race conditions.
+class BodyID
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	static constexpr uint32	cInvalidBodyID = 0xffffffff;	///< The value for an invalid body ID
+	static constexpr uint32	cBroadPhaseBit = 0x80000000;	///< This bit is used by the broadphase
+	static constexpr uint32	cMaxBodyIndex = 0x7fffff;		///< Maximum value for body index (also the maximum amount of bodies supported - 1)
+	static constexpr uint8	cMaxSequenceNumber = 0xff;		///< Maximum value for the sequence number
+	static constexpr uint	cSequenceNumberShift = 23;		///< Number of bits to shift to get the sequence number
+
+	/// Construct invalid body ID
+							BodyID() :
+		mID(cInvalidBodyID)
+	{
+	}
+
+	/// Construct from index and sequence number combined in a single uint32 (use with care!)
+	explicit				BodyID(uint32 inID) :
+		mID(inID)
+	{
+		JPH_ASSERT((inID & cBroadPhaseBit) == 0 || inID == cInvalidBodyID); // Check bit used by broadphase
+	}
+
+	/// Construct from index and sequence number
+	explicit				BodyID(uint32 inID, uint8 inSequenceNumber) :
+		mID((uint32(inSequenceNumber) << cSequenceNumberShift) | inID)
+	{
+		JPH_ASSERT(inID <= cMaxBodyIndex); // Should not overlap with broadphase bit or sequence number
+	}
+
+	/// Get index in body array
+	inline uint32			GetIndex() const
+	{
+		return mID & cMaxBodyIndex;
+	}
+
+	/// Get sequence number of body.
+	/// The sequence number can be used to check if a body ID with the same body index has been reused by another body.
+	/// It is mainly used in multi threaded situations where a body is removed and its body index is immediately reused by a body created from another thread.
+	/// Functions querying the broadphase can (after acquiring a body lock) detect that the body has been removed (we assume that this won't happen more than 128 times in a row).
+	inline uint8			GetSequenceNumber() const
+	{
+		return uint8(mID >> cSequenceNumberShift);
+	}
+
+	/// Returns the index and sequence number combined in an uint32
+	inline uint32			GetIndexAndSequenceNumber() const
+	{
+		return mID;
+	}
+
+	/// Check if the ID is valid
+	inline bool				IsInvalid() const
+	{
+		return mID == cInvalidBodyID;
+	}
+
+	/// Equals check
+	inline bool				operator == (const BodyID &inRHS) const
+	{
+		return mID == inRHS.mID;
+	}
+
+	/// Not equals check
+	inline bool				operator != (const BodyID &inRHS) const
+	{
+		return mID != inRHS.mID;
+	}
+
+	/// Smaller than operator, can be used for sorting bodies
+	inline bool				operator < (const BodyID &inRHS) const
+	{
+		return mID < inRHS.mID;
+	}
+
+	/// Greater than operator, can be used for sorting bodies
+	inline bool				operator > (const BodyID &inRHS) const
+	{
+		return mID > inRHS.mID;
+	}
+
+private:
+	uint32					mID;
+};
+
+JPH_NAMESPACE_END
+
+// Create a std::hash/JPH::Hash for BodyID
+JPH_MAKE_HASHABLE(JPH::BodyID, t.GetIndexAndSequenceNumber())

thirdparty/jolt_physics/Jolt/Physics/Body/BodyInterface.h

@@ -0,0 +1,305 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Body/BodyID.h>
+#include <Jolt/Physics/EActivation.h>
+#include <Jolt/Physics/Collision/ObjectLayer.h>
+#include <Jolt/Physics/Body/MotionType.h>
+#include <Jolt/Physics/Body/MotionQuality.h>
+#include <Jolt/Physics/Body/BodyType.h>
+#include <Jolt/Core/Reference.h>
+
+JPH_NAMESPACE_BEGIN
+
+class Body;
+class BodyCreationSettings;
+class SoftBodyCreationSettings;
+class BodyLockInterface;
+class BroadPhase;
+class BodyManager;
+class TransformedShape;
+class PhysicsMaterial;
+class SubShapeID;
+class Shape;
+class TwoBodyConstraintSettings;
+class TwoBodyConstraint;
+class BroadPhaseLayerFilter;
+class AABox;
+class CollisionGroup;
+
+/// Class that provides operations on bodies using a body ID. Note that if you need to do multiple operations on a single body, it is more efficient to lock the body once and combine the operations.
+/// All quantities are in world space unless otherwise specified.
+class JPH_EXPORT BodyInterface : public NonCopyable
+{
+public:
+	/// Initialize the interface (should only be called by PhysicsSystem)
+	void						Init(BodyLockInterface &inBodyLockInterface, BodyManager &inBodyManager, BroadPhase &inBroadPhase) { mBodyLockInterface = &inBodyLockInterface; mBodyManager = &inBodyManager; mBroadPhase = &inBroadPhase; }
+
+	/// Create a rigid body
+	/// @return Created body or null when out of bodies
+	Body *						CreateBody(const BodyCreationSettings &inSettings);
+
+	/// Create a soft body
+	/// @return Created body or null when out of bodies
+	Body *						CreateSoftBody(const SoftBodyCreationSettings &inSettings);
+
+	/// Create a rigid body with specified ID. This function can be used if a simulation is to run in sync between clients or if a simulation needs to be restored exactly.
+	/// The ID created on the server can be replicated to the client and used to create a deterministic simulation.
+	/// @return Created body or null when the body ID is invalid or a body of the same ID already exists.
+	Body *						CreateBodyWithID(const BodyID &inBodyID, const BodyCreationSettings &inSettings);
+
+	/// Create a soft body with specified ID. See comments at CreateBodyWithID.
+	Body *						CreateSoftBodyWithID(const BodyID &inBodyID, const SoftBodyCreationSettings &inSettings);
+
+	/// Advanced use only. Creates a rigid body without specifying an ID. This body cannot be added to the physics system until it has been assigned a body ID.
+	/// This can be used to decouple allocation from registering the body. A call to CreateBodyWithoutID followed by AssignBodyID is equivalent to calling CreateBodyWithID.
+	/// @return Created body
+	Body *						CreateBodyWithoutID(const BodyCreationSettings &inSettings) const;
+
+	/// Advanced use only. Creates a body without specifying an ID. See comments at CreateBodyWithoutID.
+	Body *						CreateSoftBodyWithoutID(const SoftBodyCreationSettings &inSettings) const;
+
+	/// Advanced use only. Destroy a body previously created with CreateBodyWithoutID that hasn't gotten an ID yet through the AssignBodyID function,
+	/// or a body that has had its body ID unassigned through UnassignBodyIDs. Bodies that have an ID should be destroyed through DestroyBody.
+	void						DestroyBodyWithoutID(Body *inBody) const;
+
+	/// Advanced use only. Assigns the next available body ID to a body that was created using CreateBodyWithoutID. After this call, the body can be added to the physics system.
+	/// @return false if the body already has an ID or out of body ids.
+	bool						AssignBodyID(Body *ioBody);
+
+	/// Advanced use only. Assigns a body ID to a body that was created using CreateBodyWithoutID. After this call, the body can be added to the physics system.
+	/// @return false if the body already has an ID or if the ID is not valid.
+	bool						AssignBodyID(Body *ioBody, const BodyID &inBodyID);
+
+	/// Advanced use only. See UnassignBodyIDs. Unassigns the ID of a single body.
+	Body *						UnassignBodyID(const BodyID &inBodyID);
+
+	/// Advanced use only. Removes a number of body IDs from their bodies and returns the body pointers. Before calling this, the body should have been removed from the physics system.
+	/// The body can be destroyed through DestroyBodyWithoutID. This can be used to decouple deallocation. A call to UnassignBodyIDs followed by calls to DestroyBodyWithoutID is equivalent to calling DestroyBodies.
+	/// @param inBodyIDs A list of body IDs
+	/// @param inNumber Number of bodies in the list
+	/// @param outBodies If not null on input, this will contain a list of body pointers corresponding to inBodyIDs that can be destroyed afterwards (caller assumes ownership over these).
+	void						UnassignBodyIDs(const BodyID *inBodyIDs, int inNumber, Body **outBodies);
+
+	/// Destroy a body.
+	/// Make sure that you remove the body from the physics system using BodyInterface::RemoveBody before calling this function.
+	void						DestroyBody(const BodyID &inBodyID);
+
+	/// Destroy multiple bodies
+	/// Make sure that you remove the bodies from the physics system using BodyInterface::RemoveBody before calling this function.
+	void						DestroyBodies(const BodyID *inBodyIDs, int inNumber);
+
+	/// Add body to the physics system.
+	/// Note that if you need to add multiple bodies, use the AddBodiesPrepare/AddBodiesFinalize function.
+	/// Adding many bodies, one at a time, results in a really inefficient broadphase until PhysicsSystem::OptimizeBroadPhase is called or when PhysicsSystem::Update rebuilds the tree!
+	/// After adding, to get a body by ID use the BodyLockRead or BodyLockWrite interface!
+	void						AddBody(const BodyID &inBodyID, EActivation inActivationMode);
+
+	/// Remove body from the physics system.
+	void						RemoveBody(const BodyID &inBodyID);
+
+	/// Check if a body has been added to the physics system.
+	bool						IsAdded(const BodyID &inBodyID) const;
+
+	/// Combines CreateBody and AddBody
+	/// @return Created body ID or an invalid ID when out of bodies
+	BodyID						CreateAndAddBody(const BodyCreationSettings &inSettings, EActivation inActivationMode);
+
+	/// Combines CreateSoftBody and AddBody
+	/// @return Created body ID or an invalid ID when out of bodies
+	BodyID						CreateAndAddSoftBody(const SoftBodyCreationSettings &inSettings, EActivation inActivationMode);
+
+	/// Add state handle, used to keep track of a batch of bodies while adding them to the PhysicsSystem.
+	using AddState = void *;
+
+	///@name Batch adding interface
+	///@{
+
+	/// Prepare adding inNumber bodies at ioBodies to the PhysicsSystem, returns a handle that should be used in AddBodiesFinalize/Abort.
+	/// This can be done on a background thread without influencing the PhysicsSystem.
+	/// ioBodies may be shuffled around by this function and should be kept that way until AddBodiesFinalize/Abort is called.
+	AddState					AddBodiesPrepare(BodyID *ioBodies, int inNumber);
+
+	/// Finalize adding bodies to the PhysicsSystem, supply the return value of AddBodiesPrepare in inAddState.
+	/// Please ensure that the ioBodies array passed to AddBodiesPrepare is unmodified and passed again to this function.
+	void						AddBodiesFinalize(BodyID *ioBodies, int inNumber, AddState inAddState, EActivation inActivationMode);
+
+	/// Abort adding bodies to the PhysicsSystem, supply the return value of AddBodiesPrepare in inAddState.
+	/// This can be done on a background thread without influencing the PhysicsSystem.
+	/// Please ensure that the ioBodies array passed to AddBodiesPrepare is unmodified and passed again to this function.
+	void						AddBodiesAbort(BodyID *ioBodies, int inNumber, AddState inAddState);
+
+	/// Remove inNumber bodies in ioBodies from the PhysicsSystem.
+	/// ioBodies may be shuffled around by this function.
+	void						RemoveBodies(BodyID *ioBodies, int inNumber);
+	///@}
+
+	///@name Activate / deactivate a body
+	///@{
+	void						ActivateBody(const BodyID &inBodyID);
+	void						ActivateBodies(const BodyID *inBodyIDs, int inNumber);
+	void						ActivateBodiesInAABox(const AABox &inBox, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter);
+	void						DeactivateBody(const BodyID &inBodyID);
+	void						DeactivateBodies(const BodyID *inBodyIDs, int inNumber);
+	bool						IsActive(const BodyID &inBodyID) const;
+	void						ResetSleepTimer(const BodyID &inBodyID);
+	///@}
+
+	/// Create a two body constraint
+	TwoBodyConstraint *			CreateConstraint(const TwoBodyConstraintSettings *inSettings, const BodyID &inBodyID1, const BodyID &inBodyID2);
+
+	/// Activate non-static bodies attached to a constraint
+	void						ActivateConstraint(const TwoBodyConstraint *inConstraint);
+
+	///@name Access to the shape of a body
+	///@{
+
+	/// Get the current shape
+	RefConst<Shape>				GetShape(const BodyID &inBodyID) const;
+
+	/// Set a new shape on the body
+	/// @param inBodyID Body ID of body that had its shape changed
+	/// @param inShape The new shape
+	/// @param inUpdateMassProperties When true, the mass and inertia tensor is recalculated
+	/// @param inActivationMode Whether or not to activate the body
+	void						SetShape(const BodyID &inBodyID, const Shape *inShape, bool inUpdateMassProperties, EActivation inActivationMode) const;
+
+	/// Notify all systems to indicate that a shape has changed (usable for MutableCompoundShapes)
+	/// @param inBodyID Body ID of body that had its shape changed
+	/// @param inPreviousCenterOfMass Center of mass of the shape before the alterations
+	/// @param inUpdateMassProperties When true, the mass and inertia tensor is recalculated
+	/// @param inActivationMode Whether or not to activate the body
+	void						NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inPreviousCenterOfMass, bool inUpdateMassProperties, EActivation inActivationMode) const;
+	///@}
+
+	///@name Object layer of a body
+	///@{
+	void						SetObjectLayer(const BodyID &inBodyID, ObjectLayer inLayer);
+	ObjectLayer					GetObjectLayer(const BodyID &inBodyID) const;
+	///@}
+
+	///@name Position and rotation of a body
+	///@{
+	void						SetPositionAndRotation(const BodyID &inBodyID, RVec3Arg inPosition, QuatArg inRotation, EActivation inActivationMode);
+	void						SetPositionAndRotationWhenChanged(const BodyID &inBodyID, RVec3Arg inPosition, QuatArg inRotation, EActivation inActivationMode); ///< Will only update the position/rotation and activate the body when the difference is larger than a very small number. This avoids updating the broadphase/waking up a body when the resulting position/orientation doesn't really change.
+	void						GetPositionAndRotation(const BodyID &inBodyID, RVec3 &outPosition, Quat &outRotation) const;
+	void						SetPosition(const BodyID &inBodyID, RVec3Arg inPosition, EActivation inActivationMode);
+	RVec3						GetPosition(const BodyID &inBodyID) const;
+	RVec3						GetCenterOfMassPosition(const BodyID &inBodyID) const;
+	void						SetRotation(const BodyID &inBodyID, QuatArg inRotation, EActivation inActivationMode);
+	Quat						GetRotation(const BodyID &inBodyID) const;
+	RMat44						GetWorldTransform(const BodyID &inBodyID) const;
+	RMat44						GetCenterOfMassTransform(const BodyID &inBodyID) const;
+	///@}
+
+	/// Set velocity of body such that it will be positioned at inTargetPosition/Rotation in inDeltaTime seconds (will activate body if needed)
+	void						MoveKinematic(const BodyID &inBodyID, RVec3Arg inTargetPosition, QuatArg inTargetRotation, float inDeltaTime);
+
+	/// Linear or angular velocity (functions will activate body if needed).
+	/// Note that the linear velocity is the velocity of the center of mass, which may not coincide with the position of your object, to correct for this: \f$VelocityCOM = Velocity - AngularVelocity \times ShapeCOM\f$
+	void						SetLinearAndAngularVelocity(const BodyID &inBodyID, Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity);
+	void						GetLinearAndAngularVelocity(const BodyID &inBodyID, Vec3 &outLinearVelocity, Vec3 &outAngularVelocity) const;
+	void						SetLinearVelocity(const BodyID &inBodyID, Vec3Arg inLinearVelocity);
+	Vec3						GetLinearVelocity(const BodyID &inBodyID) const;
+	void						AddLinearVelocity(const BodyID &inBodyID, Vec3Arg inLinearVelocity); ///< Add velocity to current velocity
+	void						AddLinearAndAngularVelocity(const BodyID &inBodyID, Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity); ///< Add linear and angular to current velocities
+	void						SetAngularVelocity(const BodyID &inBodyID, Vec3Arg inAngularVelocity);
+	Vec3						GetAngularVelocity(const BodyID &inBodyID) const;
+	Vec3						GetPointVelocity(const BodyID &inBodyID, RVec3Arg inPoint) const; ///< Velocity of point inPoint (in world space, e.g. on the surface of the body) of the body
+
+	/// Set the complete motion state of a body.
+	/// Note that the linear velocity is the velocity of the center of mass, which may not coincide with the position of your object, to correct for this: \f$VelocityCOM = Velocity - AngularVelocity \times ShapeCOM\f$
+	void						SetPositionRotationAndVelocity(const BodyID &inBodyID, RVec3Arg inPosition, QuatArg inRotation, Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity);
+
+	///@name Add forces to the body
+	///@{
+	void						AddForce(const BodyID &inBodyID, Vec3Arg inForce, EActivation inActivationMode = EActivation::Activate); ///< See Body::AddForce
+	void						AddForce(const BodyID &inBodyID, Vec3Arg inForce, RVec3Arg inPoint, EActivation inActivationMode = EActivation::Activate); ///< Applied at inPoint
+	void						AddTorque(const BodyID &inBodyID, Vec3Arg inTorque, EActivation inActivationMode = EActivation::Activate); ///< See Body::AddTorque
+	void						AddForceAndTorque(const BodyID &inBodyID, Vec3Arg inForce, Vec3Arg inTorque, EActivation inActivationMode = EActivation::Activate); ///< A combination of Body::AddForce and Body::AddTorque
+	///@}
+
+	///@name Add an impulse to the body
+	///@{
+	void						AddImpulse(const BodyID &inBodyID, Vec3Arg inImpulse); ///< Applied at center of mass
+	void						AddImpulse(const BodyID &inBodyID, Vec3Arg inImpulse, RVec3Arg inPoint); ///< Applied at inPoint
+	void						AddAngularImpulse(const BodyID &inBodyID, Vec3Arg inAngularImpulse);
+	bool						ApplyBuoyancyImpulse(const BodyID &inBodyID, RVec3Arg inSurfacePosition, Vec3Arg inSurfaceNormal, float inBuoyancy, float inLinearDrag, float inAngularDrag, Vec3Arg inFluidVelocity, Vec3Arg inGravity, float inDeltaTime);
+	///@}
+
+	///@name Body type
+	///@{
+	EBodyType					GetBodyType(const BodyID &inBodyID) const;
+	///@}
+
+	///@name Body motion type
+	///@{
+	void						SetMotionType(const BodyID &inBodyID, EMotionType inMotionType, EActivation inActivationMode);
+	EMotionType					GetMotionType(const BodyID &inBodyID) const;
+	///@}
+
+	///@name Body motion quality
+	///@{
+	void						SetMotionQuality(const BodyID &inBodyID, EMotionQuality inMotionQuality);
+	EMotionQuality				GetMotionQuality(const BodyID &inBodyID) const;
+	///@}
+
+	/// Get inverse inertia tensor in world space
+	Mat44						GetInverseInertia(const BodyID &inBodyID) const;
+
+	///@name Restitution
+	///@{
+	void						SetRestitution(const BodyID &inBodyID, float inRestitution);
+	float						GetRestitution(const BodyID &inBodyID) const;
+	///@}
+
+	///@name Friction
+	///@{
+	void						SetFriction(const BodyID &inBodyID, float inFriction);
+	float						GetFriction(const BodyID &inBodyID) const;
+	///@}
+
+	///@name Gravity factor
+	///@{
+	void						SetGravityFactor(const BodyID &inBodyID, float inGravityFactor);
+	float						GetGravityFactor(const BodyID &inBodyID) const;
+	///@}
+
+	///@name Manifold reduction
+	///@{
+	void						SetUseManifoldReduction(const BodyID &inBodyID, bool inUseReduction);
+	bool						GetUseManifoldReduction(const BodyID &inBodyID) const;
+	///@}
+
+	///@name Collision group
+	///@{
+	void						SetCollisionGroup(const BodyID &inBodyID, const CollisionGroup &inCollisionGroup);
+	const CollisionGroup &		GetCollisionGroup(const BodyID &inBodyID) const;
+	///@}
+
+	/// Get transform and shape for this body, used to perform collision detection
+	TransformedShape			GetTransformedShape(const BodyID &inBodyID) const;
+
+	/// Get the user data for a body
+	uint64						GetUserData(const BodyID &inBodyID) const;
+	void						SetUserData(const BodyID &inBodyID, uint64 inUserData) const;
+
+	/// Get the material for a particular sub shape
+	const PhysicsMaterial *		GetMaterial(const BodyID &inBodyID, const SubShapeID &inSubShapeID) const;
+
+	/// Set the Body::EFlags::InvalidateContactCache flag for the specified body. This means that the collision cache is invalid for any body pair involving that body until the next physics step.
+	void						InvalidateContactCache(const BodyID &inBodyID);
+
+private:
+	/// Helper function to activate a single body
+	JPH_INLINE void				ActivateBodyInternal(Body &ioBody) const;
+
+	BodyLockInterface *			mBodyLockInterface = nullptr;
+	BodyManager *				mBodyManager = nullptr;
+	BroadPhase *				mBroadPhase = nullptr;
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Body/BodyLock.h

@@ -0,0 +1,111 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Body/BodyLockInterface.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Base class for locking bodies for the duration of the scope of this class (do not use directly)
+template <bool Write, class BodyType>
+class BodyLockBase : public NonCopyable
+{
+public:
+	/// Constructor will lock the body
+								BodyLockBase(const BodyLockInterface &inBodyLockInterface, const BodyID &inBodyID) :
+		mBodyLockInterface(inBodyLockInterface)
+	{
+		if (inBodyID == BodyID())
+		{
+			// Invalid body id
+			mBodyLockMutex = nullptr;
+			mBody = nullptr;
+		}
+		else
+		{
+			// Get mutex
+			mBodyLockMutex = Write? inBodyLockInterface.LockWrite(inBodyID) : inBodyLockInterface.LockRead(inBodyID);
+
+			// Get a reference to the body or nullptr when it is no longer valid
+			mBody = inBodyLockInterface.TryGetBody(inBodyID);
+		}
+	}
+
+	/// Explicitly release the lock (normally this is done in the destructor)
+	inline void					ReleaseLock()
+	{
+		if (mBodyLockMutex != nullptr)
+		{
+			if (Write)
+				mBodyLockInterface.UnlockWrite(mBodyLockMutex);
+			else
+				mBodyLockInterface.UnlockRead(mBodyLockMutex);
+
+			mBodyLockMutex = nullptr;
+			mBody = nullptr;
+		}
+	}
+
+	/// Destructor will unlock the body
+								~BodyLockBase()
+	{
+		ReleaseLock();
+	}
+
+	/// Test if the lock was successful (if the body ID was valid)
+	inline bool					Succeeded() const
+	{
+		return mBody != nullptr;
+	}
+
+	/// Test if the lock was successful (if the body ID was valid) and the body is still in the broad phase
+	inline bool					SucceededAndIsInBroadPhase() const
+	{
+		return mBody != nullptr && mBody->IsInBroadPhase();
+	}
+
+	/// Access the body
+	inline BodyType &			GetBody() const
+	{
+		JPH_ASSERT(mBody != nullptr, "Should check Succeeded() first");
+		return *mBody;
+	}
+
+private:
+	const BodyLockInterface &	mBodyLockInterface;
+	SharedMutex *				mBodyLockMutex;
+	BodyType *					mBody;
+};
+
+/// A body lock takes a body ID and locks the underlying body so that other threads cannot access its members
+///
+/// The common usage pattern is:
+///
+///		BodyLockInterface lock_interface = physics_system.GetBodyLockInterface(); // Or non-locking interface if the lock is already taken
+///		BodyID body_id = ...; // Obtain ID to body
+///
+///		// Scoped lock
+///		{
+///			BodyLockRead lock(lock_interface, body_id);
+///			if (lock.Succeeded()) // body_id may no longer be valid
+///			{
+///				const Body &body = lock.GetBody();
+///
+///				// Do something with body
+///				...
+///			}
+///		}
+class BodyLockRead : public BodyLockBase<false, const Body>
+{
+	using BodyLockBase::BodyLockBase;
+};
+
+/// Specialization that locks a body for writing to. @see BodyLockRead for usage patterns.
+class BodyLockWrite : public BodyLockBase<true, Body>
+{
+	using BodyLockBase::BodyLockBase;
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Body/BodyLockInterface.h

@@ -0,0 +1,134 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+
+#pragma once
+
+#include <Jolt/Physics/Body/Body.h>
+#include <Jolt/Physics/Body/BodyManager.h>
+#include <Jolt/Physics/PhysicsLock.h>
+#include <Jolt/Core/Mutex.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Base class interface for locking a body. Usually you will use BodyLockRead / BodyLockWrite / BodyLockMultiRead / BodyLockMultiWrite instead.
+class BodyLockInterface : public NonCopyable
+{
+public:
+	/// Redefine MutexMask
+	using MutexMask = BodyManager::MutexMask;
+
+	/// Constructor
+	explicit					BodyLockInterface(BodyManager &inBodyManager)		: mBodyManager(inBodyManager) { }
+	virtual						~BodyLockInterface() = default;
+
+	///@name Locking functions
+	///@{
+	virtual SharedMutex *		LockRead(const BodyID &inBodyID) const = 0;
+	virtual void				UnlockRead(SharedMutex *inMutex) const = 0;
+	virtual SharedMutex *		LockWrite(const BodyID &inBodyID) const = 0;
+	virtual void				UnlockWrite(SharedMutex *inMutex) const = 0;
+	///@}
+
+	/// Get the mask needed to lock all bodies
+	inline MutexMask			GetAllBodiesMutexMask() const
+	{
+		return mBodyManager.GetAllBodiesMutexMask();
+	}
+
+	///@name Batch locking functions
+	///@{
+	virtual MutexMask			GetMutexMask(const BodyID *inBodies, int inNumber) const = 0;
+	virtual void				LockRead(MutexMask inMutexMask) const = 0;
+	virtual void				UnlockRead(MutexMask inMutexMask) const = 0;
+	virtual void				LockWrite(MutexMask inMutexMask) const = 0;
+	virtual void				UnlockWrite(MutexMask inMutexMask) const = 0;
+	///@}
+
+	/// Convert body ID to body
+	inline Body *				TryGetBody(const BodyID &inBodyID) const			{ return mBodyManager.TryGetBody(inBodyID); }
+
+protected:
+	BodyManager &				mBodyManager;
+};
+
+/// Implementation that performs no locking (assumes the lock has already been taken)
+class BodyLockInterfaceNoLock final : public BodyLockInterface
+{
+public:
+	using BodyLockInterface::BodyLockInterface;
+
+	///@name Locking functions
+	virtual SharedMutex *		LockRead([[maybe_unused]] const BodyID &inBodyID) const override	{ return nullptr; }
+	virtual void				UnlockRead([[maybe_unused]] SharedMutex *inMutex) const override	{ /* Nothing to do */ }
+	virtual SharedMutex *		LockWrite([[maybe_unused]] const BodyID &inBodyID) const override	{ return nullptr; }
+	virtual void				UnlockWrite([[maybe_unused]] SharedMutex *inMutex) const override	{ /* Nothing to do */ }
+
+	///@name Batch locking functions
+	virtual MutexMask			GetMutexMask([[maybe_unused]] const BodyID *inBodies, [[maybe_unused]] int inNumber) const override { return 0; }
+	virtual void				LockRead([[maybe_unused]] MutexMask inMutexMask) const override		{ /* Nothing to do */ }
+	virtual void				UnlockRead([[maybe_unused]] MutexMask inMutexMask) const override	{ /* Nothing to do */ }
+	virtual void				LockWrite([[maybe_unused]] MutexMask inMutexMask) const override	{ /* Nothing to do */ }
+	virtual void				UnlockWrite([[maybe_unused]] MutexMask inMutexMask) const override	{ /* Nothing to do */ }
+};
+
+/// Implementation that uses the body manager to lock the correct mutex for a body
+class BodyLockInterfaceLocking final : public BodyLockInterface
+{
+public:
+	using BodyLockInterface::BodyLockInterface;
+
+	///@name Locking functions
+	virtual SharedMutex *		LockRead(const BodyID &inBodyID) const override
+	{
+		SharedMutex &mutex = mBodyManager.GetMutexForBody(inBodyID);
+		PhysicsLock::sLockShared(mutex JPH_IF_ENABLE_ASSERTS(, &mBodyManager, EPhysicsLockTypes::PerBody));
+		return &mutex;
+	}
+
+	virtual void				UnlockRead(SharedMutex *inMutex) const override
+	{
+		PhysicsLock::sUnlockShared(*inMutex JPH_IF_ENABLE_ASSERTS(, &mBodyManager, EPhysicsLockTypes::PerBody));
+	}
+
+	virtual SharedMutex *		LockWrite(const BodyID &inBodyID) const override
+	{
+		SharedMutex &mutex = mBodyManager.GetMutexForBody(inBodyID);
+		PhysicsLock::sLock(mutex JPH_IF_ENABLE_ASSERTS(, &mBodyManager, EPhysicsLockTypes::PerBody));
+		return &mutex;
+	}
+
+	virtual void				UnlockWrite(SharedMutex *inMutex) const override
+	{
+		PhysicsLock::sUnlock(*inMutex JPH_IF_ENABLE_ASSERTS(, &mBodyManager, EPhysicsLockTypes::PerBody));
+	}
+
+	///@name Batch locking functions
+	virtual MutexMask			GetMutexMask(const BodyID *inBodies, int inNumber) const override
+	{
+		return mBodyManager.GetMutexMask(inBodies, inNumber);
+	}
+
+	virtual void				LockRead(MutexMask inMutexMask) const override
+	{
+		mBodyManager.LockRead(inMutexMask);
+	}
+
+	virtual void				UnlockRead(MutexMask inMutexMask) const override
+	{
+		mBodyManager.UnlockRead(inMutexMask);
+	}
+
+	virtual void				LockWrite(MutexMask inMutexMask) const override
+	{
+		mBodyManager.LockWrite(inMutexMask);
+	}
+
+	virtual void				UnlockWrite(MutexMask inMutexMask) const override
+	{
+		mBodyManager.UnlockWrite(inMutexMask);
+	}
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Body/BodyLockMulti.h

@@ -0,0 +1,104 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Body/BodyLockInterface.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Base class for locking multiple bodies for the duration of the scope of this class (do not use directly)
+template <bool Write, class BodyType>
+class BodyLockMultiBase : public NonCopyable
+{
+public:
+	/// Redefine MutexMask
+	using MutexMask = BodyLockInterface::MutexMask;
+
+	/// Constructor will lock the bodies
+								BodyLockMultiBase(const BodyLockInterface &inBodyLockInterface, const BodyID *inBodyIDs, int inNumber) :
+		mBodyLockInterface(inBodyLockInterface),
+		mMutexMask(inBodyLockInterface.GetMutexMask(inBodyIDs, inNumber)),
+		mBodyIDs(inBodyIDs),
+		mNumBodyIDs(inNumber)
+	{
+		if (mMutexMask != 0)
+		{
+			// Get mutex
+			if (Write)
+				inBodyLockInterface.LockWrite(mMutexMask);
+			else
+				inBodyLockInterface.LockRead(mMutexMask);
+		}
+	}
+
+	/// Destructor will unlock the bodies
+								~BodyLockMultiBase()
+	{
+		if (mMutexMask != 0)
+		{
+			if (Write)
+				mBodyLockInterface.UnlockWrite(mMutexMask);
+			else
+				mBodyLockInterface.UnlockRead(mMutexMask);
+		}
+	}
+
+	/// Access the body (returns null if body was not properly locked)
+	inline BodyType *			GetBody(int inBodyIndex) const
+	{
+		// Range check
+		JPH_ASSERT(inBodyIndex >= 0 && inBodyIndex < mNumBodyIDs);
+
+		// Get body ID
+		const BodyID &body_id = mBodyIDs[inBodyIndex];
+		if (body_id.IsInvalid())
+			return nullptr;
+
+		// Get a reference to the body or nullptr when it is no longer valid
+		return mBodyLockInterface.TryGetBody(body_id);
+	}
+
+private:
+	const BodyLockInterface &	mBodyLockInterface;
+	MutexMask					mMutexMask;
+	const BodyID *				mBodyIDs;
+	int							mNumBodyIDs;
+};
+
+/// A multi body lock takes a number of body IDs and locks the underlying bodies so that other threads cannot access its members
+///
+/// The common usage pattern is:
+///
+///		BodyLockInterface lock_interface = physics_system.GetBodyLockInterface(); // Or non-locking interface if the lock is already taken
+///		const BodyID *body_id = ...; // Obtain IDs to bodies
+///		int num_body_ids = ...;
+///
+///		// Scoped lock
+///		{
+///			BodyLockMultiRead lock(lock_interface, body_ids, num_body_ids);
+///			for (int i = 0; i < num_body_ids; ++i)
+///			{
+///				const Body *body = lock.GetBody(i);
+///				if (body != nullptr)
+///				{
+///					const Body &body = lock.Body();
+///
+///					// Do something with body
+///					...
+///				}
+///			}
+///		}
+class BodyLockMultiRead : public BodyLockMultiBase<false, const Body>
+{
+	using BodyLockMultiBase::BodyLockMultiBase;
+};
+
+/// Specialization that locks multiple bodies for writing to. @see BodyLockMultiRead for usage patterns.
+class BodyLockMultiWrite : public BodyLockMultiBase<true, Body>
+{
+	using BodyLockMultiBase::BodyLockMultiBase;
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Body/BodyManager.h

@@ -0,0 +1,377 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Body/Body.h>
+#include <Jolt/Core/Mutex.h>
+#include <Jolt/Core/MutexArray.h>
+
+JPH_NAMESPACE_BEGIN
+
+// Classes
+class BodyCreationSettings;
+class SoftBodyCreationSettings;
+class BodyActivationListener;
+class StateRecorderFilter;
+struct PhysicsSettings;
+#ifdef JPH_DEBUG_RENDERER
+class DebugRenderer;
+class BodyDrawFilter;
+#endif // JPH_DEBUG_RENDERER
+
+#ifdef JPH_DEBUG_RENDERER
+
+/// Defines how to color soft body constraints
+enum class ESoftBodyConstraintColor
+{
+	ConstraintType,				/// Draw different types of constraints in different colors
+	ConstraintGroup,			/// Draw constraints in the same group in the same color, non-parallel group will be red
+	ConstraintOrder,			/// Draw constraints in the same group in the same color, non-parallel group will be red, and order within each group will be indicated with gradient
+};
+
+#endif // JPH_DEBUG_RENDERER
+
+/// Array of bodies
+using BodyVector = Array<Body *>;
+
+/// Array of body ID's
+using BodyIDVector = Array<BodyID>;
+
+/// Class that contains all bodies
+class JPH_EXPORT BodyManager : public NonCopyable
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Destructor
+									~BodyManager();
+
+	/// Initialize the manager
+	void							Init(uint inMaxBodies, uint inNumBodyMutexes, const BroadPhaseLayerInterface &inLayerInterface);
+
+	/// Gets the current amount of bodies that are in the body manager
+	uint							GetNumBodies() const;
+
+	/// Gets the max bodies that we can support
+	uint							GetMaxBodies() const						{ return uint(mBodies.capacity()); }
+
+	/// Helper struct that counts the number of bodies of each type
+	struct BodyStats
+	{
+		uint						mNumBodies					= 0;			///< Total number of bodies in the body manager
+		uint						mMaxBodies					= 0;			///< Max allowed number of bodies in the body manager (as configured in Init(...))
+
+		uint						mNumBodiesStatic			= 0;			///< Number of static bodies
+
+		uint						mNumBodiesDynamic			= 0;			///< Number of dynamic bodies
+		uint						mNumActiveBodiesDynamic		= 0;			///< Number of dynamic bodies that are currently active
+
+		uint						mNumBodiesKinematic			= 0;			///< Number of kinematic bodies
+		uint						mNumActiveBodiesKinematic	= 0;			///< Number of kinematic bodies that are currently active
+
+		uint						mNumSoftBodies				= 0;			///< Number of soft bodies
+		uint						mNumActiveSoftBodies		= 0;			///< Number of soft bodies that are currently active
+	};
+
+	/// Get stats about the bodies in the body manager (slow, iterates through all bodies)
+	BodyStats						GetBodyStats() const;
+
+	/// Create a body using creation settings. The returned body will not be part of the body manager yet.
+	Body *							AllocateBody(const BodyCreationSettings &inBodyCreationSettings) const;
+
+	/// Create a soft body using creation settings. The returned body will not be part of the body manager yet.
+	Body *							AllocateSoftBody(const SoftBodyCreationSettings &inSoftBodyCreationSettings) const;
+
+	/// Free a body that has not been added to the body manager yet (if it has, use DestroyBodies).
+	void							FreeBody(Body *inBody) const;
+
+	/// Add a body to the body manager, assigning it the next available ID. Returns false if no more IDs are available.
+	bool							AddBody(Body *ioBody);
+
+	/// Add a body to the body manager, assigning it a custom ID. Returns false if the ID is not valid.
+	bool							AddBodyWithCustomID(Body *ioBody, const BodyID &inBodyID);
+
+	/// Remove a list of bodies from the body manager
+	void							RemoveBodies(const BodyID *inBodyIDs, int inNumber, Body **outBodies);
+
+	/// Remove a set of bodies from the body manager and frees them.
+	void							DestroyBodies(const BodyID *inBodyIDs, int inNumber);
+
+	/// Activate a list of bodies.
+	/// This function should only be called when an exclusive lock for the bodies are held.
+	void							ActivateBodies(const BodyID *inBodyIDs, int inNumber);
+
+	/// Deactivate a list of bodies.
+	/// This function should only be called when an exclusive lock for the bodies are held.
+	void							DeactivateBodies(const BodyID *inBodyIDs, int inNumber);
+
+	/// Update the motion quality for a body
+	void							SetMotionQuality(Body &ioBody, EMotionQuality inMotionQuality);
+
+	/// Get copy of the list of active bodies under protection of a lock.
+	void							GetActiveBodies(EBodyType inType, BodyIDVector &outBodyIDs) const;
+
+	/// Get the list of active bodies. Note: Not thread safe. The active bodies list can change at any moment.
+	const BodyID *					GetActiveBodiesUnsafe(EBodyType inType) const { return mActiveBodies[int(inType)]; }
+
+	/// Get the number of active bodies.
+	uint32							GetNumActiveBodies(EBodyType inType) const	{ return mNumActiveBodies[int(inType)].load(memory_order_acquire); }
+
+	/// Get the number of active bodies that are using continuous collision detection
+	uint32							GetNumActiveCCDBodies() const				{ return mNumActiveCCDBodies; }
+
+	/// Listener that is notified whenever a body is activated/deactivated
+	void							SetBodyActivationListener(BodyActivationListener *inListener);
+	BodyActivationListener *		GetBodyActivationListener() const			{ return mActivationListener; }
+
+	/// Check if this is a valid body pointer. When a body is freed the memory that the pointer occupies is reused to store a freelist.
+	static inline bool				sIsValidBodyPointer(const Body *inBody)		{ return (uintptr_t(inBody) & cIsFreedBody) == 0; }
+
+	/// Get all bodies. Note that this can contain invalid body pointers, call sIsValidBodyPointer to check.
+	const BodyVector &				GetBodies() const							{ return mBodies; }
+
+	/// Get all bodies. Note that this can contain invalid body pointers, call sIsValidBodyPointer to check.
+	BodyVector &					GetBodies()									{ return mBodies; }
+
+	/// Get all body IDs under the protection of a lock
+	void							GetBodyIDs(BodyIDVector &outBodies) const;
+
+	/// Access a body (not protected by lock)
+	const Body &					GetBody(const BodyID &inID) const			{ return *mBodies[inID.GetIndex()]; }
+
+	/// Access a body (not protected by lock)
+	Body &							GetBody(const BodyID &inID)					{ return *mBodies[inID.GetIndex()]; }
+
+	/// Access a body, will return a nullptr if the body ID is no longer valid (not protected by lock)
+	const Body *					TryGetBody(const BodyID &inID) const
+	{
+		uint32 idx = inID.GetIndex();
+		if (idx >= mBodies.size())
+			return nullptr;
+
+		const Body *body = mBodies[idx];
+		if (sIsValidBodyPointer(body) && body->GetID() == inID)
+			return body;
+
+		return nullptr;
+	}
+
+	/// Access a body, will return a nullptr if the body ID is no longer valid (not protected by lock)
+	Body *							TryGetBody(const BodyID &inID)
+	{
+		uint32 idx = inID.GetIndex();
+		if (idx >= mBodies.size())
+			return nullptr;
+
+		Body *body = mBodies[idx];
+		if (sIsValidBodyPointer(body) && body->GetID() == inID)
+			return body;
+
+		return nullptr;
+	}
+
+	/// Access the mutex for a single body
+	SharedMutex &					GetMutexForBody(const BodyID &inID) const	{ return mBodyMutexes.GetMutexByObjectIndex(inID.GetIndex()); }
+
+	/// Bodies are protected using an array of mutexes (so a fixed number, not 1 per body). Each bit in this mask indicates a locked mutex.
+	using MutexMask = uint64;
+
+	///@name Batch body mutex access (do not use directly)
+	///@{
+	MutexMask						GetAllBodiesMutexMask() const				{ return mBodyMutexes.GetNumMutexes() == sizeof(MutexMask) * 8? ~MutexMask(0) : (MutexMask(1) << mBodyMutexes.GetNumMutexes()) - 1; }
+	MutexMask						GetMutexMask(const BodyID *inBodies, int inNumber) const;
+	void							LockRead(MutexMask inMutexMask) const;
+	void							UnlockRead(MutexMask inMutexMask) const;
+	void							LockWrite(MutexMask inMutexMask) const;
+	void							UnlockWrite(MutexMask inMutexMask) const;
+	///@}
+
+	/// Lock all bodies. This should only be done during PhysicsSystem::Update().
+	void							LockAllBodies() const;
+
+	/// Unlock all bodies. This should only be done during PhysicsSystem::Update().
+	void							UnlockAllBodies() const;
+
+	/// Function to update body's layer (should only be called by the BodyInterface since it also requires updating the broadphase)
+	inline void						SetBodyObjectLayerInternal(Body &ioBody, ObjectLayer inLayer) const { ioBody.mObjectLayer = inLayer; ioBody.mBroadPhaseLayer = mBroadPhaseLayerInterface->GetBroadPhaseLayer(inLayer); }
+
+	/// Set the Body::EFlags::InvalidateContactCache flag for the specified body. This means that the collision cache is invalid for any body pair involving that body until the next physics step.
+	void							InvalidateContactCacheForBody(Body &ioBody);
+
+	/// Reset the Body::EFlags::InvalidateContactCache flag for all bodies. All contact pairs in the contact cache will now by valid again.
+	void							ValidateContactCacheForAllBodies();
+
+	/// Saving state for replay
+	void							SaveState(StateRecorder &inStream, const StateRecorderFilter *inFilter) const;
+
+	/// Restoring state for replay. Returns false if failed.
+	bool							RestoreState(StateRecorder &inStream);
+
+	/// Save the state of a single body for replay
+	void							SaveBodyState(const Body &inBody, StateRecorder &inStream) const;
+
+	/// Save the state of a single body for replay
+	void							RestoreBodyState(Body &inBody, StateRecorder &inStream);
+
+#ifdef JPH_DEBUG_RENDERER
+	enum class EShapeColor
+	{
+		InstanceColor,				///< Random color per instance
+		ShapeTypeColor,				///< Convex = green, scaled = yellow, compound = orange, mesh = red
+		MotionTypeColor,			///< Static = grey, keyframed = green, dynamic = random color per instance
+		SleepColor,					///< Static = grey, keyframed = green, dynamic = yellow, sleeping = red
+		IslandColor,				///< Static = grey, active = random color per island, sleeping = light grey
+		MaterialColor,				///< Color as defined by the PhysicsMaterial of the shape
+	};
+
+	/// Draw settings
+	struct DrawSettings
+	{
+		bool						mDrawGetSupportFunction = false;				///< Draw the GetSupport() function, used for convex collision detection
+		bool						mDrawSupportDirection = false;					///< When drawing the support function, also draw which direction mapped to a specific support point
+		bool						mDrawGetSupportingFace = false;					///< Draw the faces that were found colliding during collision detection
+		bool						mDrawShape = true;								///< Draw the shapes of all bodies
+		bool						mDrawShapeWireframe = false;					///< When mDrawShape is true and this is true, the shapes will be drawn in wireframe instead of solid.
+		EShapeColor					mDrawShapeColor = EShapeColor::MotionTypeColor; ///< Coloring scheme to use for shapes
+		bool						mDrawBoundingBox = false;						///< Draw a bounding box per body
+		bool						mDrawCenterOfMassTransform = false;				///< Draw the center of mass for each body
+		bool						mDrawWorldTransform = false;					///< Draw the world transform (which can be different than the center of mass) for each body
+		bool						mDrawVelocity = false;							///< Draw the velocity vector for each body
+		bool						mDrawMassAndInertia = false;					///< Draw the mass and inertia (as the box equivalent) for each body
+		bool						mDrawSleepStats = false;						///< Draw stats regarding the sleeping algorithm of each body
+		bool						mDrawSoftBodyVertices = false;					///< Draw the vertices of soft bodies
+		bool						mDrawSoftBodyVertexVelocities = false;			///< Draw the velocities of the vertices of soft bodies
+		bool						mDrawSoftBodyEdgeConstraints = false;			///< Draw the edge constraints of soft bodies
+		bool						mDrawSoftBodyBendConstraints = false;			///< Draw the bend constraints of soft bodies
+		bool						mDrawSoftBodyVolumeConstraints = false;			///< Draw the volume constraints of soft bodies
+		bool						mDrawSoftBodySkinConstraints = false;			///< Draw the skin constraints of soft bodies
+		bool						mDrawSoftBodyLRAConstraints = false;			///< Draw the LRA constraints of soft bodies
+		bool						mDrawSoftBodyPredictedBounds = false;			///< Draw the predicted bounds of soft bodies
+		ESoftBodyConstraintColor	mDrawSoftBodyConstraintColor = ESoftBodyConstraintColor::ConstraintType; ///< Coloring scheme to use for soft body constraints
+	};
+
+	/// Draw the state of the bodies (debugging purposes)
+	void							Draw(const DrawSettings &inSettings, const PhysicsSettings &inPhysicsSettings, DebugRenderer *inRenderer, const BodyDrawFilter *inBodyFilter = nullptr);
+#endif // JPH_DEBUG_RENDERER
+
+#ifdef JPH_ENABLE_ASSERTS
+	/// Lock the active body list, asserts when Activate/DeactivateBody is called.
+	void							SetActiveBodiesLocked(bool inLocked)		{ mActiveBodiesLocked = inLocked; }
+
+	/// Per thread override of the locked state, to be used by the PhysicsSystem only!
+	class GrantActiveBodiesAccess
+	{
+	public:
+		inline GrantActiveBodiesAccess(bool inAllowActivation, bool inAllowDeactivation)
+		{
+			JPH_ASSERT(!sGetOverrideAllowActivation());
+			sSetOverrideAllowActivation(inAllowActivation);
+
+			JPH_ASSERT(!sGetOverrideAllowDeactivation());
+			sSetOverrideAllowDeactivation(inAllowDeactivation);
+		}
+
+		inline ~GrantActiveBodiesAccess()
+		{
+			sSetOverrideAllowActivation(false);
+			sSetOverrideAllowDeactivation(false);
+		}
+	};
+#endif
+
+#ifdef JPH_DEBUG
+	/// Validate if the cached bounding boxes are correct for all active bodies
+	void							ValidateActiveBodyBounds();
+#endif // JPH_DEBUG
+
+private:
+	/// Increment and get the sequence number of the body
+#ifdef JPH_COMPILER_CLANG
+	__attribute__((no_sanitize("implicit-conversion"))) // We intentionally overflow the uint8 sequence number
+#endif
+	inline uint8					GetNextSequenceNumber(int inBodyIndex)		{ return ++mBodySequenceNumbers[inBodyIndex]; }
+
+	/// Add a single body to mActiveBodies, note doesn't lock the active body mutex!
+	inline void						AddBodyToActiveBodies(Body &ioBody);
+
+	/// Remove a single body from mActiveBodies, note doesn't lock the active body mutex!
+	inline void						RemoveBodyFromActiveBodies(Body &ioBody);
+
+	/// Helper function to remove a body from the manager
+	JPH_INLINE Body *				RemoveBodyInternal(const BodyID &inBodyID);
+
+	/// Helper function to delete a body (which could actually be a BodyWithMotionProperties)
+	inline static void				sDeleteBody(Body *inBody);
+
+#if defined(JPH_DEBUG) && defined(JPH_ENABLE_ASSERTS)
+	/// Function to check that the free list is not corrupted
+	void							ValidateFreeList() const;
+#endif // defined(JPH_DEBUG) && _defined(JPH_ENABLE_ASSERTS)
+
+	/// List of pointers to all bodies. Contains invalid pointers for deleted bodies, check with sIsValidBodyPointer. Note that this array is reserved to the max bodies that is passed in the Init function so that adding bodies will not reallocate the array.
+	BodyVector						mBodies;
+
+	/// Current number of allocated bodies
+	uint							mNumBodies = 0;
+
+	/// Indicates that there are no more freed body IDs
+	static constexpr uintptr_t		cBodyIDFreeListEnd = ~uintptr_t(0);
+
+	/// Bit that indicates a pointer in mBodies is actually the index of the next freed body. We use the lowest bit because we know that Bodies need to be 16 byte aligned so addresses can never end in a 1 bit.
+	static constexpr uintptr_t		cIsFreedBody = uintptr_t(1);
+
+	/// Amount of bits to shift to get an index to the next freed body
+	static constexpr uint			cFreedBodyIndexShift = 1;
+
+	/// Index of first entry in mBodies that is unused
+	uintptr_t						mBodyIDFreeListStart = cBodyIDFreeListEnd;
+
+	/// Protects mBodies array (but not the bodies it points to), mNumBodies and mBodyIDFreeListStart
+	mutable Mutex					mBodiesMutex;
+
+	/// An array of mutexes protecting the bodies in the mBodies array
+	using BodyMutexes = MutexArray<SharedMutex>;
+	mutable BodyMutexes				mBodyMutexes;
+
+	/// List of next sequence number for a body ID
+	Array<uint8>					mBodySequenceNumbers;
+
+	/// Mutex that protects the mActiveBodies array
+	mutable Mutex					mActiveBodiesMutex;
+
+	/// List of all active dynamic bodies (size is equal to max amount of bodies)
+	BodyID *						mActiveBodies[cBodyTypeCount] = { };
+
+	/// How many bodies there are in the list of active bodies
+	atomic<uint32>					mNumActiveBodies[cBodyTypeCount] = { };
+
+	/// How many of the active bodies have continuous collision detection enabled
+	uint32							mNumActiveCCDBodies = 0;
+
+	/// Mutex that protects the mBodiesCacheInvalid array
+	mutable Mutex					mBodiesCacheInvalidMutex;
+
+	/// List of all bodies that should have their cache invalidated
+	BodyIDVector					mBodiesCacheInvalid;
+
+	/// Listener that is notified whenever a body is activated/deactivated
+	BodyActivationListener *		mActivationListener = nullptr;
+
+	/// Cached broadphase layer interface
+	const BroadPhaseLayerInterface *mBroadPhaseLayerInterface = nullptr;
+
+#ifdef JPH_ENABLE_ASSERTS
+	static bool						sGetOverrideAllowActivation();
+	static void						sSetOverrideAllowActivation(bool inValue);
+
+	static bool						sGetOverrideAllowDeactivation();
+	static void						sSetOverrideAllowDeactivation(bool inValue);
+
+	/// Debug system that tries to limit changes to active bodies during the PhysicsSystem::Update()
+	bool							mActiveBodiesLocked = false;
+#endif
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Body/BodyPair.h

@@ -0,0 +1,36 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Body/BodyID.h>
+#include <Jolt/Core/HashCombine.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Structure that holds a body pair
+struct alignas(uint64) BodyPair
+{
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+							BodyPair() = default;
+							BodyPair(BodyID inA, BodyID inB)							: mBodyA(inA), mBodyB(inB) { }
+
+	/// Equals operator
+	bool					operator == (const BodyPair &inRHS) const					{ return *reinterpret_cast<const uint64 *>(this) == *reinterpret_cast<const uint64 *>(&inRHS); }
+
+	/// Smaller than operator, used for consistently ordering body pairs
+	bool					operator < (const BodyPair &inRHS) const					{ return *reinterpret_cast<const uint64 *>(this) < *reinterpret_cast<const uint64 *>(&inRHS); }
+
+	/// Get the hash value of this object
+	uint64					GetHash() const												{ return Hash64(*reinterpret_cast<const uint64 *>(this)); }
+
+	BodyID					mBodyA;
+	BodyID					mBodyB;
+};
+
+static_assert(sizeof(BodyPair) == sizeof(uint64), "Mismatch in class size");
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Body/BodyType.h

@@ -0,0 +1,19 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+/// Type of body
+enum class EBodyType : uint8
+{
+	RigidBody,				///< Rigid body consisting of a rigid shape
+	SoftBody,				///< Soft body consisting of a deformable shape
+};
+
+/// How many types of bodies there are
+static constexpr uint cBodyTypeCount = 2;
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Body/MassProperties.cpp

@@ -0,0 +1,185 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Body/MassProperties.h>
+#include <Jolt/Math/Matrix.h>
+#include <Jolt/Math/Vector.h>
+#include <Jolt/Math/EigenValueSymmetric.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+#include <Jolt/Core/InsertionSort.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(MassProperties)
+{
+	JPH_ADD_ATTRIBUTE(MassProperties, mMass)
+	JPH_ADD_ATTRIBUTE(MassProperties, mInertia)
+}
+
+bool MassProperties::DecomposePrincipalMomentsOfInertia(Mat44 &outRotation, Vec3 &outDiagonal) const
+{
+	// Using eigendecomposition to get the principal components of the inertia tensor
+	// See: https://en.wikipedia.org/wiki/Eigendecomposition_of_a_matrix
+	Matrix<3, 3> inertia;
+	inertia.CopyPart(mInertia, 0, 0, 3, 3, 0, 0);
+	Matrix<3, 3> eigen_vec = Matrix<3, 3>::sIdentity();
+	Vector<3> eigen_val;
+	if (!EigenValueSymmetric(inertia, eigen_vec, eigen_val))
+		return false;
+
+	// Sort so that the biggest value goes first
+	int indices[] = { 0, 1, 2 };
+	InsertionSort(indices, indices + 3, [&eigen_val](int inLeft, int inRight) { return eigen_val[inLeft] > eigen_val[inRight]; });
+
+	// Convert to a regular Mat44 and Vec3
+	outRotation = Mat44::sIdentity();
+	for (int i = 0; i < 3; ++i)
+	{
+		outRotation.SetColumn3(i, Vec3(reinterpret_cast<Float3 &>(eigen_vec.GetColumn(indices[i]))));
+		outDiagonal.SetComponent(i, eigen_val[indices[i]]);
+	}
+
+	// Make sure that the rotation matrix is a right handed matrix
+	if (outRotation.GetAxisX().Cross(outRotation.GetAxisY()).Dot(outRotation.GetAxisZ()) < 0.0f)
+		outRotation.SetAxisZ(-outRotation.GetAxisZ());
+
+#ifdef JPH_ENABLE_ASSERTS
+	// Validate that the solution is correct, for each axis we want to make sure that the difference in inertia is
+	// smaller than some fraction of the inertia itself in that axis
+	Mat44 new_inertia = outRotation * Mat44::sScale(outDiagonal) * outRotation.Inversed();
+	for (int i = 0; i < 3; ++i)
+		JPH_ASSERT(new_inertia.GetColumn3(i).IsClose(mInertia.GetColumn3(i), mInertia.GetColumn3(i).LengthSq() * 1.0e-10f));
+#endif
+
+	return true;
+}
+
+void MassProperties::SetMassAndInertiaOfSolidBox(Vec3Arg inBoxSize, float inDensity)
+{
+	// Calculate mass
+	mMass = inBoxSize.GetX() * inBoxSize.GetY() * inBoxSize.GetZ() * inDensity;
+
+	// Calculate inertia
+	Vec3 size_sq = inBoxSize * inBoxSize;
+	Vec3 scale = (size_sq.Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_X>() + size_sq.Swizzle<SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_Y>()) * (mMass / 12.0f);
+	mInertia = Mat44::sScale(scale);
+}
+
+void MassProperties::ScaleToMass(float inMass)
+{
+	if (mMass > 0.0f)
+	{
+		// Calculate how much we have to scale the inertia tensor
+		float mass_scale = inMass / mMass;
+
+		// Update mass
+		mMass = inMass;
+
+		// Update inertia tensor
+		for (int i = 0; i < 3; ++i)
+			mInertia.SetColumn4(i, mInertia.GetColumn4(i) * mass_scale);
+	}
+	else
+	{
+		// Just set the mass
+		mMass = inMass;
+	}
+}
+
+Vec3 MassProperties::sGetEquivalentSolidBoxSize(float inMass, Vec3Arg inInertiaDiagonal)
+{
+	// Moment of inertia of a solid box has diagonal:
+	// mass / 12 * [size_y^2 + size_z^2, size_x^2 + size_z^2, size_x^2 + size_y^2]
+	// Solving for size_x, size_y and size_y (diagonal and mass are known):
+	Vec3 diagonal = inInertiaDiagonal * (12.0f / inMass);
+	return Vec3(sqrt(0.5f * (-diagonal[0] + diagonal[1] + diagonal[2])), sqrt(0.5f * (diagonal[0] - diagonal[1] + diagonal[2])), sqrt(0.5f * (diagonal[0] + diagonal[1] - diagonal[2])));
+}
+
+void MassProperties::Scale(Vec3Arg inScale)
+{
+	// See: https://en.wikipedia.org/wiki/Moment_of_inertia#Inertia_tensor
+	// The diagonal of the inertia tensor can be calculated like this:
+	// Ixx = sum_{k = 1 to n}(m_k * (y_k^2 + z_k^2))
+	// Iyy = sum_{k = 1 to n}(m_k * (x_k^2 + z_k^2))
+	// Izz = sum_{k = 1 to n}(m_k * (x_k^2 + y_k^2))
+	//
+	// We want to isolate the terms x_k, y_k and z_k:
+	// d = [0.5, 0.5, 0.5].[Ixx, Iyy, Izz]
+	// [sum_{k = 1 to n}(m_k * x_k^2), sum_{k = 1 to n}(m_k * y_k^2), sum_{k = 1 to n}(m_k * z_k^2)] = [d, d, d] - [Ixx, Iyy, Izz]
+	Vec3 diagonal = mInertia.GetDiagonal3();
+	Vec3 xyz_sq = Vec3::sReplicate(Vec3::sReplicate(0.5f).Dot(diagonal)) - diagonal;
+
+	// When scaling a shape these terms change like this:
+	// sum_{k = 1 to n}(m_k * (scale_x * x_k)^2) = scale_x^2 * sum_{k = 1 to n}(m_k * x_k^2)
+	// Same for y_k and z_k
+	// Using these terms we can calculate the new diagonal of the inertia tensor:
+	Vec3 xyz_scaled_sq = inScale * inScale * xyz_sq;
+	float i_xx = xyz_scaled_sq.GetY() + xyz_scaled_sq.GetZ();
+	float i_yy = xyz_scaled_sq.GetX() + xyz_scaled_sq.GetZ();
+	float i_zz = xyz_scaled_sq.GetX() + xyz_scaled_sq.GetY();
+
+	// The off diagonal elements are calculated like:
+	// Ixy = -sum_{k = 1 to n}(x_k y_k)
+	// Ixz = -sum_{k = 1 to n}(x_k z_k)
+	// Iyz = -sum_{k = 1 to n}(y_k z_k)
+	// Scaling these is simple:
+	float i_xy = inScale.GetX() * inScale.GetY() * mInertia(0, 1);
+	float i_xz = inScale.GetX() * inScale.GetZ() * mInertia(0, 2);
+	float i_yz = inScale.GetY() * inScale.GetZ() * mInertia(1, 2);
+
+	// Update inertia tensor
+	mInertia(0, 0) = i_xx;
+	mInertia(0, 1) = i_xy;
+	mInertia(1, 0) = i_xy;
+	mInertia(1, 1) = i_yy;
+	mInertia(0, 2) = i_xz;
+	mInertia(2, 0) = i_xz;
+	mInertia(1, 2) = i_yz;
+	mInertia(2, 1) = i_yz;
+	mInertia(2, 2) = i_zz;
+
+	// Mass scales linear with volume (note that the scaling can be negative and we don't want the mass to become negative)
+	float mass_scale = abs(inScale.GetX() * inScale.GetY() * inScale.GetZ());
+	mMass *= mass_scale;
+
+	// Inertia scales linear with mass. This updates the m_k terms above.
+	mInertia *= mass_scale;
+
+	// Ensure that the bottom right element is a 1 again
+	mInertia(3, 3) = 1.0f;
+}
+
+void MassProperties::Rotate(Mat44Arg inRotation)
+{
+	mInertia = inRotation.Multiply3x3(mInertia).Multiply3x3RightTransposed(inRotation);
+}
+
+void MassProperties::Translate(Vec3Arg inTranslation)
+{
+	// Transform the inertia using the parallel axis theorem: I' = I + m * (translation^2 E - translation translation^T)
+	// Where I is the original body's inertia and E the identity matrix
+	// See: https://en.wikipedia.org/wiki/Parallel_axis_theorem
+	mInertia += mMass * (Mat44::sScale(inTranslation.Dot(inTranslation)) - Mat44::sOuterProduct(inTranslation, inTranslation));
+
+	// Ensure that inertia is a 3x3 matrix, adding inertias causes the bottom right element to change
+	mInertia.SetColumn4(3, Vec4(0, 0, 0, 1));
+}
+
+void MassProperties::SaveBinaryState(StreamOut &inStream) const
+{
+	inStream.Write(mMass);
+	inStream.Write(mInertia);
+}
+
+void MassProperties::RestoreBinaryState(StreamIn &inStream)
+{
+	inStream.Read(mMass);
+	inStream.Read(mInertia);
+}
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Body/MassProperties.h

@@ -0,0 +1,58 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/ObjectStream/SerializableObject.h>
+
+JPH_NAMESPACE_BEGIN
+
+class StreamIn;
+class StreamOut;
+
+/// Describes the mass and inertia properties of a body. Used during body construction only.
+class JPH_EXPORT MassProperties
+{
+	JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, MassProperties)
+
+public:
+	/// Using eigendecomposition, decompose the inertia tensor into a diagonal matrix D and a right-handed rotation matrix R so that the inertia tensor is \f$R \: D \: R^{-1}\f$.
+	/// @see https://en.wikipedia.org/wiki/Moment_of_inertia section 'Principal axes'
+	/// @param outRotation The rotation matrix R
+	/// @param outDiagonal The diagonal of the diagonal matrix D
+	/// @return True if successful, false if failed
+	bool					DecomposePrincipalMomentsOfInertia(Mat44 &outRotation, Vec3 &outDiagonal) const;
+
+	/// Set the mass and inertia of a box with edge size inBoxSize and density inDensity
+	void					SetMassAndInertiaOfSolidBox(Vec3Arg inBoxSize, float inDensity);
+
+	/// Set the mass and scale the inertia tensor to match the mass
+	void					ScaleToMass(float inMass);
+
+	/// Calculates the size of the solid box that has an inertia tensor diagonal inInertiaDiagonal
+	static Vec3				sGetEquivalentSolidBoxSize(float inMass, Vec3Arg inInertiaDiagonal);
+
+	/// Rotate the inertia by 3x3 matrix inRotation
+	void					Rotate(Mat44Arg inRotation);
+
+	/// Translate the inertia by a vector inTranslation
+	void					Translate(Vec3Arg inTranslation);
+
+	/// Scale the mass and inertia by inScale, note that elements can be < 0 to flip the shape
+	void					Scale(Vec3Arg inScale);
+
+	/// Saves the state of this object in binary form to inStream.
+	void					SaveBinaryState(StreamOut &inStream) const;
+
+	/// Restore the state of this object from inStream.
+	void					RestoreBinaryState(StreamIn &inStream);
+
+	/// Mass of the shape (kg)
+	float					mMass = 0.0f;
+
+	/// Inertia tensor of the shape (kg m^2)
+	Mat44					mInertia = Mat44::sZero();
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Body/MotionProperties.cpp

@@ -0,0 +1,92 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Body/MotionProperties.h>
+#include <Jolt/Physics/StateRecorder.h>
+
+JPH_NAMESPACE_BEGIN
+
+void MotionProperties::SetMassProperties(EAllowedDOFs inAllowedDOFs, const MassProperties &inMassProperties)
+{
+	// Store allowed DOFs
+	mAllowedDOFs = inAllowedDOFs;
+
+	// Decompose DOFs
+	uint allowed_translation_axis = uint(inAllowedDOFs) & 0b111;
+	uint allowed_rotation_axis = (uint(inAllowedDOFs) >> 3) & 0b111;
+
+	// Set inverse mass
+	if (allowed_translation_axis == 0)
+	{
+		// No translation possible
+		mInvMass = 0.0f;
+	}
+	else
+	{
+		JPH_ASSERT(inMassProperties.mMass > 0.0f, "Invalid mass. "
+			"Some shapes like MeshShape or TriangleShape cannot calculate mass automatically, "
+			"in this case you need to provide it by setting BodyCreationSettings::mOverrideMassProperties and mMassPropertiesOverride.");
+		mInvMass = 1.0f / inMassProperties.mMass;
+	}
+
+	if (allowed_rotation_axis == 0)
+	{
+		// No rotation possible
+		mInvInertiaDiagonal = Vec3::sZero();
+		mInertiaRotation = Quat::sIdentity();
+	}
+	else
+	{
+		// Set inverse inertia
+		Mat44 rotation;
+		Vec3 diagonal;
+		if (inMassProperties.DecomposePrincipalMomentsOfInertia(rotation, diagonal)
+			&& !diagonal.IsNearZero())
+		{
+			mInvInertiaDiagonal = diagonal.Reciprocal();
+			mInertiaRotation = rotation.GetQuaternion();
+		}
+		else
+		{
+			// Failed! Fall back to inertia tensor of sphere with radius 1.
+			mInvInertiaDiagonal = Vec3::sReplicate(2.5f * mInvMass);
+			mInertiaRotation = Quat::sIdentity();
+		}
+	}
+
+	JPH_ASSERT(mInvMass != 0.0f || mInvInertiaDiagonal != Vec3::sZero(), "Can't lock all axes, use a static body for this. This will crash with a division by zero later!");
+}
+
+void MotionProperties::SaveState(StateRecorder &inStream) const
+{
+	// Only write properties that can change at runtime
+	inStream.Write(mLinearVelocity);
+	inStream.Write(mAngularVelocity);
+	inStream.Write(mForce);
+	inStream.Write(mTorque);
+#ifdef JPH_DOUBLE_PRECISION
+	inStream.Write(mSleepTestOffset);
+#endif // JPH_DOUBLE_PRECISION
+	inStream.Write(mSleepTestSpheres);
+	inStream.Write(mSleepTestTimer);
+	inStream.Write(mAllowSleeping);
+}
+
+void MotionProperties::RestoreState(StateRecorder &inStream)
+{
+	inStream.Read(mLinearVelocity);
+	inStream.Read(mAngularVelocity);
+	inStream.Read(mForce);
+	inStream.Read(mTorque);
+#ifdef JPH_DOUBLE_PRECISION
+	inStream.Read(mSleepTestOffset);
+#endif // JPH_DOUBLE_PRECISION
+	inStream.Read(mSleepTestSpheres);
+	inStream.Read(mSleepTestTimer);
+	inStream.Read(mAllowSleeping);
+}
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Body/MotionProperties.h

@@ -0,0 +1,282 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Geometry/Sphere.h>
+#include <Jolt/Physics/Body/AllowedDOFs.h>
+#include <Jolt/Physics/Body/MotionQuality.h>
+#include <Jolt/Physics/Body/BodyAccess.h>
+#include <Jolt/Physics/Body/MotionType.h>
+#include <Jolt/Physics/Body/BodyType.h>
+#include <Jolt/Physics/Body/MassProperties.h>
+#include <Jolt/Physics/DeterminismLog.h>
+
+JPH_NAMESPACE_BEGIN
+
+class StateRecorder;
+
+/// Enum that determines if an object can go to sleep
+enum class ECanSleep
+{
+	CannotSleep = 0,																		///< Object cannot go to sleep
+	CanSleep = 1,																			///< Object can go to sleep
+};
+
+/// The Body class only keeps track of state for static bodies, the MotionProperties class keeps the additional state needed for a moving Body. It has a 1-on-1 relationship with the body.
+class JPH_EXPORT MotionProperties
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Motion quality, or how well it detects collisions when it has a high velocity
+	EMotionQuality			GetMotionQuality() const										{ return mMotionQuality; }
+
+	/// Get the allowed degrees of freedom that this body has (this can be changed by calling SetMassProperties)
+	inline EAllowedDOFs		GetAllowedDOFs() const											{ return mAllowedDOFs; }
+
+	/// If this body can go to sleep.
+	inline bool				GetAllowSleeping() const										{ return mAllowSleeping; }
+
+	/// Get world space linear velocity of the center of mass
+	inline Vec3				GetLinearVelocity() const										{ JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::Read)); return mLinearVelocity; }
+
+	/// Set world space linear velocity of the center of mass
+	void					SetLinearVelocity(Vec3Arg inLinearVelocity)						{ JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite)); JPH_ASSERT(inLinearVelocity.Length() <= mMaxLinearVelocity); mLinearVelocity = LockTranslation(inLinearVelocity); }
+
+	/// Set world space linear velocity of the center of mass, will make sure the value is clamped against the maximum linear velocity
+	void					SetLinearVelocityClamped(Vec3Arg inLinearVelocity)				{ mLinearVelocity = LockTranslation(inLinearVelocity); ClampLinearVelocity(); }
+
+	/// Get world space angular velocity of the center of mass
+	inline Vec3				GetAngularVelocity() const										{ JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::Read)); return mAngularVelocity; }
+
+	/// Set world space angular velocity of the center of mass
+	void					SetAngularVelocity(Vec3Arg inAngularVelocity)					{ JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite)); JPH_ASSERT(inAngularVelocity.Length() <= mMaxAngularVelocity); mAngularVelocity = LockAngular(inAngularVelocity); }
+
+	/// Set world space angular velocity of the center of mass, will make sure the value is clamped against the maximum angular velocity
+	void					SetAngularVelocityClamped(Vec3Arg inAngularVelocity)			{ mAngularVelocity = LockAngular(inAngularVelocity); ClampAngularVelocity(); }
+
+	/// Set velocity of body such that it will be rotate/translate by inDeltaPosition/Rotation in inDeltaTime seconds.
+	inline void				MoveKinematic(Vec3Arg inDeltaPosition, QuatArg inDeltaRotation, float inDeltaTime);
+
+	///@name Velocity limits
+	///@{
+
+	/// Maximum linear velocity that a body can achieve. Used to prevent the system from exploding.
+	inline float			GetMaxLinearVelocity() const									{ return mMaxLinearVelocity; }
+	inline void				SetMaxLinearVelocity(float inLinearVelocity)					{ JPH_ASSERT(inLinearVelocity >= 0.0f); mMaxLinearVelocity = inLinearVelocity; }
+
+	/// Maximum angular velocity that a body can achieve. Used to prevent the system from exploding.
+	inline float			GetMaxAngularVelocity() const									{ return mMaxAngularVelocity; }
+	inline void				SetMaxAngularVelocity(float inAngularVelocity)					{ JPH_ASSERT(inAngularVelocity >= 0.0f); mMaxAngularVelocity = inAngularVelocity; }
+	///@}
+
+	/// Clamp velocity according to limit
+	inline void				ClampLinearVelocity();
+	inline void				ClampAngularVelocity();
+
+	/// Get linear damping: dv/dt = -c * v. c must be between 0 and 1 but is usually close to 0.
+	inline float			GetLinearDamping() const										{ return mLinearDamping; }
+	void					SetLinearDamping(float inLinearDamping)							{ JPH_ASSERT(inLinearDamping >= 0.0f); mLinearDamping = inLinearDamping; }
+
+	/// Get angular damping: dw/dt = -c * w. c must be between 0 and 1 but is usually close to 0.
+	inline float			GetAngularDamping() const										{ return mAngularDamping; }
+	void					SetAngularDamping(float inAngularDamping)						{ JPH_ASSERT(inAngularDamping >= 0.0f); mAngularDamping = inAngularDamping; }
+
+	/// Get gravity factor (1 = normal gravity, 0 = no gravity)
+	inline float			GetGravityFactor() const										{ return mGravityFactor; }
+	void					SetGravityFactor(float inGravityFactor)							{ mGravityFactor = inGravityFactor; }
+
+	/// Set the mass and inertia tensor
+	void					SetMassProperties(EAllowedDOFs inAllowedDOFs, const MassProperties &inMassProperties);
+
+	/// Get inverse mass (1 / mass). Should only be called on a dynamic object (static or kinematic bodies have infinite mass so should be treated as 1 / mass = 0)
+	inline float			GetInverseMass() const											{ JPH_ASSERT(mCachedMotionType == EMotionType::Dynamic); return mInvMass; }
+	inline float			GetInverseMassUnchecked() const									{ return mInvMass; }
+
+	/// Set the inverse mass (1 / mass).
+	/// Note that mass and inertia are linearly related (e.g. inertia of a sphere with mass m and radius r is \f$2/5 \: m \: r^2\f$).
+	/// If you change mass, inertia should probably change as well. You can use ScaleToMass to update mass and inertia at the same time.
+	/// If all your translation degrees of freedom are restricted, make sure this is zero (see EAllowedDOFs).
+	void					SetInverseMass(float inInverseMass)								{ mInvMass = inInverseMass; }
+
+	/// Diagonal of inverse inertia matrix: D. Should only be called on a dynamic object (static or kinematic bodies have infinite mass so should be treated as D = 0)
+	inline Vec3				GetInverseInertiaDiagonal() const								{ JPH_ASSERT(mCachedMotionType == EMotionType::Dynamic); return mInvInertiaDiagonal; }
+
+	/// Rotation (R) that takes inverse inertia diagonal to local space: \f$I_{body}^{-1} = R \: D \: R^{-1}\f$
+	inline Quat				GetInertiaRotation() const										{ return mInertiaRotation; }
+
+	/// Set the inverse inertia tensor in local space by setting the diagonal and the rotation: \f$I_{body}^{-1} = R \: D \: R^{-1}\f$.
+	/// Note that mass and inertia are linearly related (e.g. inertia of a sphere with mass m and radius r is \f$2/5 \: m \: r^2\f$).
+	/// If you change inertia, mass should probably change as well. You can use ScaleToMass to update mass and inertia at the same time.
+	/// If all your rotation degrees of freedom are restricted, make sure this is zero (see EAllowedDOFs).
+	void					SetInverseInertia(Vec3Arg inDiagonal, QuatArg inRot)			{ mInvInertiaDiagonal = inDiagonal; mInertiaRotation = inRot; }
+
+	/// Sets the mass to inMass and scale the inertia tensor based on the ratio between the old and new mass.
+	/// Note that this only works when the current mass is finite (i.e. the body is dynamic and translational degrees of freedom are not restricted).
+	void					ScaleToMass(float inMass);
+
+	/// Get inverse inertia matrix (\f$I_{body}^{-1}\f$). Will be a matrix of zeros for a static or kinematic object.
+	inline Mat44			GetLocalSpaceInverseInertia() const;
+
+	/// Same as GetLocalSpaceInverseInertia() but doesn't check if the body is dynamic
+	inline Mat44			GetLocalSpaceInverseInertiaUnchecked() const;
+
+	/// Get inverse inertia matrix (\f$I^{-1}\f$) for a given object rotation (translation will be ignored). Zero if object is static or kinematic.
+	inline Mat44			GetInverseInertiaForRotation(Mat44Arg inRotation) const;
+
+	/// Multiply a vector with the inverse world space inertia tensor (\f$I_{world}^{-1}\f$). Zero if object is static or kinematic.
+	JPH_INLINE Vec3			MultiplyWorldSpaceInverseInertiaByVector(QuatArg inBodyRotation, Vec3Arg inV) const;
+
+	/// Velocity of point inPoint (in center of mass space, e.g. on the surface of the body) of the body (unit: m/s)
+	JPH_INLINE Vec3			GetPointVelocityCOM(Vec3Arg inPointRelativeToCOM) const			{ return mLinearVelocity + mAngularVelocity.Cross(inPointRelativeToCOM); }
+
+	// Get the total amount of force applied to the center of mass this time step (through Body::AddForce calls). Note that it will reset to zero after PhysicsSystem::Update.
+	JPH_INLINE Vec3			GetAccumulatedForce() const										{ return Vec3::sLoadFloat3Unsafe(mForce); }
+
+	// Get the total amount of torque applied to the center of mass this time step (through Body::AddForce/Body::AddTorque calls). Note that it will reset to zero after PhysicsSystem::Update.
+	JPH_INLINE Vec3			GetAccumulatedTorque() const									{ return Vec3::sLoadFloat3Unsafe(mTorque); }
+
+	// Reset the total accumulated force, note that this will be done automatically after every time step.
+	JPH_INLINE void			ResetForce()													{ mForce = Float3(0, 0, 0); }
+
+	// Reset the total accumulated torque, note that this will be done automatically after every time step.
+	JPH_INLINE void			ResetTorque()													{ mTorque = Float3(0, 0, 0); }
+
+	// Reset the current velocity and accumulated force and torque.
+	JPH_INLINE void			ResetMotion()
+	{
+		JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite));
+		mLinearVelocity = mAngularVelocity = Vec3::sZero();
+		mForce = mTorque = Float3(0, 0, 0);
+	}
+
+	/// Returns a vector where the linear components that are not allowed by mAllowedDOFs are set to 0 and the rest to 0xffffffff
+	JPH_INLINE UVec4		GetLinearDOFsMask() const
+	{
+		UVec4 mask(uint32(EAllowedDOFs::TranslationX), uint32(EAllowedDOFs::TranslationY), uint32(EAllowedDOFs::TranslationZ), 0);
+		return UVec4::sEquals(UVec4::sAnd(UVec4::sReplicate(uint32(mAllowedDOFs)), mask), mask);
+	}
+
+	/// Takes a translation vector inV and returns a vector where the components that are not allowed by mAllowedDOFs are set to 0
+	JPH_INLINE Vec3			LockTranslation(Vec3Arg inV) const
+	{
+		return Vec3::sAnd(inV, Vec3(GetLinearDOFsMask().ReinterpretAsFloat()));
+	}
+
+	/// Returns a vector where the angular components that are not allowed by mAllowedDOFs are set to 0 and the rest to 0xffffffff
+	JPH_INLINE UVec4		GetAngularDOFsMask() const
+	{
+		UVec4 mask(uint32(EAllowedDOFs::RotationX), uint32(EAllowedDOFs::RotationY), uint32(EAllowedDOFs::RotationZ), 0);
+		return UVec4::sEquals(UVec4::sAnd(UVec4::sReplicate(uint32(mAllowedDOFs)), mask), mask);
+	}
+
+	/// Takes an angular velocity / torque vector inV and returns a vector where the components that are not allowed by mAllowedDOFs are set to 0
+	JPH_INLINE Vec3			LockAngular(Vec3Arg inV) const
+	{
+		return Vec3::sAnd(inV, Vec3(GetAngularDOFsMask().ReinterpretAsFloat()));
+	}
+
+	/// Used only when this body is dynamic and colliding. Override for the number of solver velocity iterations to run, 0 means use the default in PhysicsSettings::mNumVelocitySteps. The number of iterations to use is the max of all contacts and constraints in the island.
+	void					SetNumVelocityStepsOverride(uint inN)							{ JPH_ASSERT(inN < 256); mNumVelocityStepsOverride = uint8(inN); }
+	uint					GetNumVelocityStepsOverride() const								{ return mNumVelocityStepsOverride; }
+
+	/// Used only when this body is dynamic and colliding. Override for the number of solver position iterations to run, 0 means use the default in PhysicsSettings::mNumPositionSteps. The number of iterations to use is the max of all contacts and constraints in the island.
+	void					SetNumPositionStepsOverride(uint inN)							{ JPH_ASSERT(inN < 256); mNumPositionStepsOverride = uint8(inN); }
+	uint					GetNumPositionStepsOverride() const								{ return mNumPositionStepsOverride; }
+
+	////////////////////////////////////////////////////////////
+	// FUNCTIONS BELOW THIS LINE ARE FOR INTERNAL USE ONLY
+	////////////////////////////////////////////////////////////
+
+	///@name Update linear and angular velocity (used during constraint solving)
+	///@{
+	inline void				AddLinearVelocityStep(Vec3Arg inLinearVelocityChange)			{ JPH_DET_LOG("AddLinearVelocityStep: " << inLinearVelocityChange); JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite)); mLinearVelocity = LockTranslation(mLinearVelocity + inLinearVelocityChange); JPH_ASSERT(!mLinearVelocity.IsNaN()); }
+	inline void				SubLinearVelocityStep(Vec3Arg inLinearVelocityChange)			{ JPH_DET_LOG("SubLinearVelocityStep: " << inLinearVelocityChange); JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite)); mLinearVelocity = LockTranslation(mLinearVelocity - inLinearVelocityChange); JPH_ASSERT(!mLinearVelocity.IsNaN()); }
+	inline void				AddAngularVelocityStep(Vec3Arg inAngularVelocityChange)			{ JPH_DET_LOG("AddAngularVelocityStep: " << inAngularVelocityChange); JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite)); mAngularVelocity += inAngularVelocityChange; JPH_ASSERT(!mAngularVelocity.IsNaN()); }
+	inline void				SubAngularVelocityStep(Vec3Arg inAngularVelocityChange)			{ JPH_DET_LOG("SubAngularVelocityStep: " << inAngularVelocityChange); JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite)); mAngularVelocity -= inAngularVelocityChange; JPH_ASSERT(!mAngularVelocity.IsNaN()); }
+	///@}
+
+	/// Apply the gyroscopic force (aka Dzhanibekov effect, see https://en.wikipedia.org/wiki/Tennis_racket_theorem)
+	inline void				ApplyGyroscopicForceInternal(QuatArg inBodyRotation, float inDeltaTime);
+
+	/// Apply all accumulated forces, torques and drag (should only be called by the PhysicsSystem)
+	inline void				ApplyForceTorqueAndDragInternal(QuatArg inBodyRotation, Vec3Arg inGravity, float inDeltaTime);
+
+	/// Access to the island index
+	uint32					GetIslandIndexInternal() const									{ return mIslandIndex; }
+	void					SetIslandIndexInternal(uint32 inIndex)							{ mIslandIndex = inIndex; }
+
+	/// Access to the index in the active bodies array
+	uint32					GetIndexInActiveBodiesInternal() const							{ return mIndexInActiveBodies; }
+
+#ifdef JPH_DOUBLE_PRECISION
+	inline DVec3			GetSleepTestOffset() const										{ return DVec3::sLoadDouble3Unsafe(mSleepTestOffset); }
+#endif // JPH_DOUBLE_PRECISION
+
+	/// Reset spheres to center around inPoints with radius 0
+	inline void				ResetSleepTestSpheres(const RVec3 *inPoints);
+
+	/// Reset the sleep test timer without resetting the sleep test spheres
+	inline void				ResetSleepTestTimer()											{ mSleepTestTimer = 0.0f; }
+
+	/// Accumulate sleep time and return if a body can go to sleep
+	inline ECanSleep		AccumulateSleepTime(float inDeltaTime, float inTimeBeforeSleep);
+
+	/// Saving state for replay
+	void					SaveState(StateRecorder &inStream) const;
+
+	/// Restoring state for replay
+	void					RestoreState(StateRecorder &inStream);
+
+	static constexpr uint32	cInactiveIndex = uint32(-1);									///< Constant indicating that body is not active
+
+private:
+	friend class BodyManager;
+	friend class Body;
+
+	// 1st cache line
+	// 16 byte aligned
+	Vec3					mLinearVelocity { Vec3::sZero() };								///< World space linear velocity of the center of mass (m/s)
+	Vec3					mAngularVelocity { Vec3::sZero() };								///< World space angular velocity (rad/s)
+	Vec3					mInvInertiaDiagonal;											///< Diagonal of inverse inertia matrix: D
+	Quat					mInertiaRotation;												///< Rotation (R) that takes inverse inertia diagonal to local space: Ibody^-1 = R * D * R^-1
+
+	// 2nd cache line
+	// 4 byte aligned
+	Float3					mForce { 0, 0, 0 };												///< Accumulated world space force (N). Note loaded through intrinsics so ensure that the 4 bytes after this are readable!
+	Float3					mTorque { 0, 0, 0 };											///< Accumulated world space torque (N m). Note loaded through intrinsics so ensure that the 4 bytes after this are readable!
+	float					mInvMass;														///< Inverse mass of the object (1/kg)
+	float					mLinearDamping;													///< Linear damping: dv/dt = -c * v. c must be between 0 and 1 but is usually close to 0.
+	float					mAngularDamping;												///< Angular damping: dw/dt = -c * w. c must be between 0 and 1 but is usually close to 0.
+	float					mMaxLinearVelocity;												///< Maximum linear velocity that this body can reach (m/s)
+	float					mMaxAngularVelocity;											///< Maximum angular velocity that this body can reach (rad/s)
+	float					mGravityFactor;													///< Factor to multiply gravity with
+	uint32					mIndexInActiveBodies = cInactiveIndex;							///< If the body is active, this is the index in the active body list or cInactiveIndex if it is not active (note that there are 2 lists, one for rigid and one for soft bodies)
+	uint32					mIslandIndex = cInactiveIndex;									///< Index of the island that this body is part of, when the body has not yet been updated or is not active this is cInactiveIndex
+
+	// 1 byte aligned
+	EMotionQuality			mMotionQuality;													///< Motion quality, or how well it detects collisions when it has a high velocity
+	bool					mAllowSleeping;													///< If this body can go to sleep
+	EAllowedDOFs			mAllowedDOFs = EAllowedDOFs::All;								///< Allowed degrees of freedom for this body
+	uint8					mNumVelocityStepsOverride = 0;									///< Used only when this body is dynamic and colliding. Override for the number of solver velocity iterations to run, 0 means use the default in PhysicsSettings::mNumVelocitySteps. The number of iterations to use is the max of all contacts and constraints in the island.
+	uint8					mNumPositionStepsOverride = 0;									///< Used only when this body is dynamic and colliding. Override for the number of solver position iterations to run, 0 means use the default in PhysicsSettings::mNumPositionSteps. The number of iterations to use is the max of all contacts and constraints in the island.
+
+	// 3rd cache line (least frequently used)
+	// 4 byte aligned (or 8 byte if running in double precision)
+#ifdef JPH_DOUBLE_PRECISION
+	Double3					mSleepTestOffset;												///< mSleepTestSpheres are relative to this offset to prevent floating point inaccuracies. Warning: Loaded using sLoadDouble3Unsafe which will read 8 extra bytes.
+#endif // JPH_DOUBLE_PRECISION
+	Sphere					mSleepTestSpheres[3];											///< Measure motion for 3 points on the body to see if it is resting: COM, COM + largest bounding box axis, COM + second largest bounding box axis
+	float					mSleepTestTimer;												///< How long this body has been within the movement tolerance
+
+#ifdef JPH_ENABLE_ASSERTS
+	EBodyType				mCachedBodyType;												///< Copied from Body::mBodyType and cached for asserting purposes
+	EMotionType				mCachedMotionType;												///< Copied from Body::mMotionType and cached for asserting purposes
+#endif
+};
+
+JPH_NAMESPACE_END
+
+#include "MotionProperties.inl"

thirdparty/jolt_physics/Jolt/Physics/Body/MotionProperties.inl

@@ -0,0 +1,178 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+void MotionProperties::MoveKinematic(Vec3Arg inDeltaPosition, QuatArg inDeltaRotation, float inDeltaTime)
+{
+	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite));
+	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::Read));
+	JPH_ASSERT(mCachedBodyType == EBodyType::RigidBody);
+	JPH_ASSERT(mCachedMotionType != EMotionType::Static);
+
+	// Calculate required linear velocity
+	mLinearVelocity = LockTranslation(inDeltaPosition / inDeltaTime);
+
+	// Calculate required angular velocity
+	Vec3 axis;
+	float angle;
+	inDeltaRotation.GetAxisAngle(axis, angle);
+	mAngularVelocity = LockAngular(axis * (angle / inDeltaTime));
+}
+
+void MotionProperties::ClampLinearVelocity()
+{
+	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite));
+
+	float len_sq = mLinearVelocity.LengthSq();
+	JPH_ASSERT(isfinite(len_sq));
+	if (len_sq > Square(mMaxLinearVelocity))
+		mLinearVelocity *= mMaxLinearVelocity / sqrt(len_sq);
+}
+
+void MotionProperties::ClampAngularVelocity()
+{
+	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite));
+
+	float len_sq = mAngularVelocity.LengthSq();
+	JPH_ASSERT(isfinite(len_sq));
+	if (len_sq > Square(mMaxAngularVelocity))
+		mAngularVelocity *= mMaxAngularVelocity / sqrt(len_sq);
+}
+
+inline Mat44 MotionProperties::GetLocalSpaceInverseInertiaUnchecked() const
+{
+	Mat44 rotation = Mat44::sRotation(mInertiaRotation);
+	Mat44 rotation_mul_scale_transposed(mInvInertiaDiagonal.SplatX() * rotation.GetColumn4(0), mInvInertiaDiagonal.SplatY() * rotation.GetColumn4(1), mInvInertiaDiagonal.SplatZ() * rotation.GetColumn4(2), Vec4(0, 0, 0, 1));
+	return rotation.Multiply3x3RightTransposed(rotation_mul_scale_transposed);
+}
+
+inline void MotionProperties::ScaleToMass(float inMass)
+{
+	JPH_ASSERT(mInvMass > 0.0f, "Body must have finite mass");
+	JPH_ASSERT(inMass > 0.0f, "New mass cannot be zero");
+
+	float new_inv_mass = 1.0f / inMass;
+	mInvInertiaDiagonal *= new_inv_mass / mInvMass;
+	mInvMass = new_inv_mass;
+}
+
+inline Mat44 MotionProperties::GetLocalSpaceInverseInertia() const
+{
+	JPH_ASSERT(mCachedMotionType == EMotionType::Dynamic);
+	return GetLocalSpaceInverseInertiaUnchecked();
+}
+
+Mat44 MotionProperties::GetInverseInertiaForRotation(Mat44Arg inRotation) const
+{
+	JPH_ASSERT(mCachedMotionType == EMotionType::Dynamic);
+
+	Mat44 rotation = inRotation.Multiply3x3(Mat44::sRotation(mInertiaRotation));
+	Mat44 rotation_mul_scale_transposed(mInvInertiaDiagonal.SplatX() * rotation.GetColumn4(0), mInvInertiaDiagonal.SplatY() * rotation.GetColumn4(1), mInvInertiaDiagonal.SplatZ() * rotation.GetColumn4(2), Vec4(0, 0, 0, 1));
+	Mat44 inverse_inertia = rotation.Multiply3x3RightTransposed(rotation_mul_scale_transposed);
+
+	// We need to mask out both the rows and columns of DOFs that are not allowed
+	Vec4 angular_dofs_mask = GetAngularDOFsMask().ReinterpretAsFloat();
+	inverse_inertia.SetColumn4(0, Vec4::sAnd(inverse_inertia.GetColumn4(0), Vec4::sAnd(angular_dofs_mask, angular_dofs_mask.SplatX())));
+	inverse_inertia.SetColumn4(1, Vec4::sAnd(inverse_inertia.GetColumn4(1), Vec4::sAnd(angular_dofs_mask, angular_dofs_mask.SplatY())));
+	inverse_inertia.SetColumn4(2, Vec4::sAnd(inverse_inertia.GetColumn4(2), Vec4::sAnd(angular_dofs_mask, angular_dofs_mask.SplatZ())));
+
+	return inverse_inertia;
+}
+
+Vec3 MotionProperties::MultiplyWorldSpaceInverseInertiaByVector(QuatArg inBodyRotation, Vec3Arg inV) const
+{
+	JPH_ASSERT(mCachedMotionType == EMotionType::Dynamic);
+
+	// Mask out columns of DOFs that are not allowed
+	Vec3 angular_dofs_mask = Vec3(GetAngularDOFsMask().ReinterpretAsFloat());
+	Vec3 v = Vec3::sAnd(inV, angular_dofs_mask);
+
+	// Multiply vector by inverse inertia
+	Mat44 rotation = Mat44::sRotation(inBodyRotation * mInertiaRotation);
+	Vec3 result = rotation.Multiply3x3(mInvInertiaDiagonal * rotation.Multiply3x3Transposed(v));
+
+	// Mask out rows of DOFs that are not allowed
+	return Vec3::sAnd(result, angular_dofs_mask);
+}
+
+void MotionProperties::ApplyGyroscopicForceInternal(QuatArg inBodyRotation, float inDeltaTime)
+{
+	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite));
+	JPH_ASSERT(mCachedBodyType == EBodyType::RigidBody);
+	JPH_ASSERT(mCachedMotionType == EMotionType::Dynamic);
+
+	// Calculate local space inertia tensor (a diagonal in local space)
+	UVec4 is_zero = Vec3::sEquals(mInvInertiaDiagonal, Vec3::sZero());
+	Vec3 denominator = Vec3::sSelect(mInvInertiaDiagonal, Vec3::sOne(), is_zero);
+	Vec3 nominator = Vec3::sSelect(Vec3::sOne(), Vec3::sZero(), is_zero);
+	Vec3 local_inertia = nominator / denominator; // Avoid dividing by zero, inertia in this axis will be zero
+
+	// Calculate local space angular momentum
+	Quat inertia_space_to_world_space = inBodyRotation * mInertiaRotation;
+	Vec3 local_angular_velocity = inertia_space_to_world_space.Conjugated() * mAngularVelocity;
+	Vec3 local_momentum = local_inertia * local_angular_velocity;
+
+	// The gyroscopic force applies a torque: T = -w x I w where w is angular velocity and I the inertia tensor
+	// Calculate the new angular momentum by applying the gyroscopic force and make sure the new magnitude is the same as the old one
+	// to avoid introducing energy into the system due to the Euler step
+	Vec3 new_local_momentum = local_momentum - inDeltaTime * local_angular_velocity.Cross(local_momentum);
+	float new_local_momentum_len_sq = new_local_momentum.LengthSq();
+	new_local_momentum = new_local_momentum_len_sq > 0.0f? new_local_momentum * sqrt(local_momentum.LengthSq() / new_local_momentum_len_sq) : Vec3::sZero();
+
+	// Convert back to world space angular velocity
+	mAngularVelocity = inertia_space_to_world_space * (mInvInertiaDiagonal * new_local_momentum);
+}
+
+void MotionProperties::ApplyForceTorqueAndDragInternal(QuatArg inBodyRotation, Vec3Arg inGravity, float inDeltaTime)
+{
+	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite));
+	JPH_ASSERT(mCachedBodyType == EBodyType::RigidBody);
+	JPH_ASSERT(mCachedMotionType == EMotionType::Dynamic);
+
+	// Update linear velocity
+	mLinearVelocity = LockTranslation(mLinearVelocity + inDeltaTime * (mGravityFactor * inGravity + mInvMass * GetAccumulatedForce()));
+
+	// Update angular velocity
+	mAngularVelocity += inDeltaTime * MultiplyWorldSpaceInverseInertiaByVector(inBodyRotation, GetAccumulatedTorque());
+
+	// Linear damping: dv/dt = -c * v
+	// Solution: v(t) = v(0) * e^(-c * t) or v2 = v1 * e^(-c * dt)
+	// Taylor expansion of e^(-c * dt) = 1 - c * dt + ...
+	// Since dt is usually in the order of 1/60 and c is a low number too this approximation is good enough
+	mLinearVelocity *= max(0.0f, 1.0f - mLinearDamping * inDeltaTime);
+	mAngularVelocity *= max(0.0f, 1.0f - mAngularDamping * inDeltaTime);
+
+	// Clamp velocities
+	ClampLinearVelocity();
+	ClampAngularVelocity();
+}
+
+void MotionProperties::ResetSleepTestSpheres(const RVec3 *inPoints)
+{
+#ifdef JPH_DOUBLE_PRECISION
+	// Make spheres relative to the first point and initialize them to zero radius
+	DVec3 offset = inPoints[0];
+	offset.StoreDouble3(&mSleepTestOffset);
+	mSleepTestSpheres[0] = Sphere(Vec3::sZero(), 0.0f);
+	for (int i = 1; i < 3; ++i)
+		mSleepTestSpheres[i] = Sphere(Vec3(inPoints[i] - offset), 0.0f);
+#else
+	// Initialize the spheres to zero radius around the supplied points
+	for (int i = 0; i < 3; ++i)
+		mSleepTestSpheres[i] = Sphere(inPoints[i], 0.0f);
+#endif
+
+	mSleepTestTimer = 0.0f;
+}
+
+ECanSleep MotionProperties::AccumulateSleepTime(float inDeltaTime, float inTimeBeforeSleep)
+{
+	mSleepTestTimer += inDeltaTime;
+	return mSleepTestTimer >= inTimeBeforeSleep? ECanSleep::CanSleep : ECanSleep::CannotSleep;
+}
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Body/MotionQuality.h

@@ -0,0 +1,31 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+/// Motion quality, or how well it detects collisions when it has a high velocity
+enum class EMotionQuality : uint8
+{
+	/// Update the body in discrete steps. Body will tunnel through thin objects if its velocity is high enough.
+	/// This is the cheapest way of simulating a body.
+	Discrete,
+
+	/// Update the body using linear casting. When stepping the body, its collision shape is cast from
+	/// start to destination using the starting rotation. The body will not be able to tunnel through thin
+	/// objects at high velocity, but tunneling is still possible if the body is long and thin and has high
+	/// angular velocity. Time is stolen from the object (which means it will move up to the first collision
+	/// and will not bounce off the surface until the next integration step). This will make the body appear
+	/// to go slower when it collides with high velocity. In order to not get stuck, the body is always
+	/// allowed to move by a fraction of it's inner radius, which may eventually lead it to pass through geometry.
+	///
+	/// Note that if you're using a collision listener, you can receive contact added/persisted notifications of contacts
+	/// that may in the end not happen. This happens between bodies that are using casting: If bodies A and B collide at t1
+	/// and B and C collide at t2 where t2 < t1 and A and C don't collide. In this case you may receive an incorrect contact
+	/// point added callback between A and B (which will be removed the next frame).
+	LinearCast,
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Body/MotionType.h

@@ -0,0 +1,17 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+/// Motion type of a physics body
+enum class EMotionType : uint8
+{
+	Static,						///< Non movable
+	Kinematic,					///< Movable using velocities only, does not respond to forces
+	Dynamic,					///< Responds to forces as a normal physics object
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Character/Character.cpp

@@ -0,0 +1,333 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Character/Character.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Jolt/Physics/Body/BodyLock.h>
+#include <Jolt/Physics/Collision/CollideShape.h>
+#include <Jolt/Physics/PhysicsSystem.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+
+JPH_NAMESPACE_BEGIN
+
+static inline const BodyLockInterface &sCharacterGetBodyLockInterface(const PhysicsSystem *inSystem, bool inLockBodies)
+{
+	return inLockBodies? static_cast<const BodyLockInterface &>(inSystem->GetBodyLockInterface()) : static_cast<const BodyLockInterface &>(inSystem->GetBodyLockInterfaceNoLock());
+}
+
+static inline BodyInterface &sCharacterGetBodyInterface(PhysicsSystem *inSystem, bool inLockBodies)
+{
+	return inLockBodies? inSystem->GetBodyInterface() : inSystem->GetBodyInterfaceNoLock();
+}
+
+static inline const NarrowPhaseQuery &sCharacterGetNarrowPhaseQuery(const PhysicsSystem *inSystem, bool inLockBodies)
+{
+	return inLockBodies? inSystem->GetNarrowPhaseQuery() : inSystem->GetNarrowPhaseQueryNoLock();
+}
+
+Character::Character(const CharacterSettings *inSettings, RVec3Arg inPosition, QuatArg inRotation, uint64 inUserData, PhysicsSystem *inSystem) :
+	CharacterBase(inSettings, inSystem),
+	mLayer(inSettings->mLayer)
+{
+	// Construct rigid body
+	BodyCreationSettings settings(mShape, inPosition, inRotation, EMotionType::Dynamic, mLayer);
+	settings.mAllowedDOFs = inSettings->mAllowedDOFs;
+	settings.mEnhancedInternalEdgeRemoval = inSettings->mEnhancedInternalEdgeRemoval;
+	settings.mOverrideMassProperties = EOverrideMassProperties::MassAndInertiaProvided;
+	settings.mMassPropertiesOverride.mMass = inSettings->mMass;
+	settings.mFriction = inSettings->mFriction;
+	settings.mGravityFactor = inSettings->mGravityFactor;
+	settings.mUserData = inUserData;
+	const Body *body = mSystem->GetBodyInterface().CreateBody(settings);
+	if (body != nullptr)
+		mBodyID = body->GetID();
+}
+
+Character::~Character()
+{
+	// Destroy the body
+	mSystem->GetBodyInterface().DestroyBody(mBodyID);
+}
+
+void Character::AddToPhysicsSystem(EActivation inActivationMode, bool inLockBodies)
+{
+	sCharacterGetBodyInterface(mSystem, inLockBodies).AddBody(mBodyID, inActivationMode);
+}
+
+void Character::RemoveFromPhysicsSystem(bool inLockBodies)
+{
+	sCharacterGetBodyInterface(mSystem, inLockBodies).RemoveBody(mBodyID);
+}
+
+void Character::Activate(bool inLockBodies)
+{
+	sCharacterGetBodyInterface(mSystem, inLockBodies).ActivateBody(mBodyID);
+}
+
+void Character::CheckCollision(RMat44Arg inCenterOfMassTransform, Vec3Arg inMovementDirection, float inMaxSeparationDistance, const Shape *inShape, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, bool inLockBodies) const
+{
+	// Create query broadphase layer filter
+	DefaultBroadPhaseLayerFilter broadphase_layer_filter = mSystem->GetDefaultBroadPhaseLayerFilter(mLayer);
+
+	// Create query object layer filter
+	DefaultObjectLayerFilter object_layer_filter = mSystem->GetDefaultLayerFilter(mLayer);
+
+	// Ignore sensors and my own body
+	class CharacterBodyFilter : public IgnoreSingleBodyFilter
+	{
+	public:
+		using			IgnoreSingleBodyFilter::IgnoreSingleBodyFilter;
+
+		virtual bool	ShouldCollideLocked(const Body &inBody) const override
+		{
+			return !inBody.IsSensor();
+		}
+	};
+	CharacterBodyFilter body_filter(mBodyID);
+
+	// Settings for collide shape
+	CollideShapeSettings settings;
+	settings.mMaxSeparationDistance = inMaxSeparationDistance;
+	settings.mActiveEdgeMode = EActiveEdgeMode::CollideOnlyWithActive;
+	settings.mActiveEdgeMovementDirection = inMovementDirection;
+	settings.mBackFaceMode = EBackFaceMode::IgnoreBackFaces;
+
+	sCharacterGetNarrowPhaseQuery(mSystem, inLockBodies).CollideShape(inShape, Vec3::sOne(), inCenterOfMassTransform, settings, inBaseOffset, ioCollector, broadphase_layer_filter, object_layer_filter, body_filter);
+}
+
+void Character::CheckCollision(RVec3Arg inPosition, QuatArg inRotation, Vec3Arg inMovementDirection, float inMaxSeparationDistance, const Shape *inShape, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, bool inLockBodies) const
+{
+	// Calculate center of mass transform
+	RMat44 center_of_mass = RMat44::sRotationTranslation(inRotation, inPosition).PreTranslated(inShape->GetCenterOfMass());
+
+	CheckCollision(center_of_mass, inMovementDirection, inMaxSeparationDistance, inShape, inBaseOffset, ioCollector, inLockBodies);
+}
+
+void Character::CheckCollision(const Shape *inShape, float inMaxSeparationDistance, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, bool inLockBodies) const
+{
+	// Determine position and velocity of body
+	RMat44 query_transform;
+	Vec3 velocity;
+	{
+		BodyLockRead lock(sCharacterGetBodyLockInterface(mSystem, inLockBodies), mBodyID);
+		if (!lock.Succeeded())
+			return;
+
+		const Body &body = lock.GetBody();
+
+		// Correct the center of mass transform for the difference between the old and new center of mass shape
+		query_transform = body.GetCenterOfMassTransform().PreTranslated(inShape->GetCenterOfMass() - mShape->GetCenterOfMass());
+		velocity = body.GetLinearVelocity();
+	}
+
+	CheckCollision(query_transform, velocity, inMaxSeparationDistance, inShape, inBaseOffset, ioCollector, inLockBodies);
+}
+
+void Character::PostSimulation(float inMaxSeparationDistance, bool inLockBodies)
+{
+	// Get character position, rotation and velocity
+	RVec3 char_pos;
+	Quat char_rot;
+	Vec3 char_vel;
+	{
+		BodyLockRead lock(sCharacterGetBodyLockInterface(mSystem, inLockBodies), mBodyID);
+		if (!lock.Succeeded())
+			return;
+		const Body &body = lock.GetBody();
+		char_pos = body.GetPosition();
+		char_rot = body.GetRotation();
+		char_vel = body.GetLinearVelocity();
+	}
+
+	// Collector that finds the hit with the normal that is the most 'up'
+	class MyCollector : public CollideShapeCollector
+	{
+	public:
+		// Constructor
+		explicit			MyCollector(Vec3Arg inUp, RVec3 inBaseOffset) : mBaseOffset(inBaseOffset), mUp(inUp) { }
+
+		// See: CollectorType::AddHit
+		virtual void		AddHit(const CollideShapeResult &inResult) override
+		{
+			Vec3 normal = -inResult.mPenetrationAxis.Normalized();
+			float dot = normal.Dot(mUp);
+			if (dot > mBestDot) // Find the hit that is most aligned with the up vector
+			{
+				mGroundBodyID = inResult.mBodyID2;
+				mGroundBodySubShapeID = inResult.mSubShapeID2;
+				mGroundPosition = mBaseOffset + inResult.mContactPointOn2;
+				mGroundNormal = normal;
+				mBestDot = dot;
+			}
+		}
+
+		BodyID				mGroundBodyID;
+		SubShapeID			mGroundBodySubShapeID;
+		RVec3				mGroundPosition = RVec3::sZero();
+		Vec3				mGroundNormal = Vec3::sZero();
+
+	private:
+		RVec3				mBaseOffset;
+		Vec3				mUp;
+		float				mBestDot = -FLT_MAX;
+	};
+
+	// Collide shape
+	MyCollector collector(mUp, char_pos);
+	CheckCollision(char_pos, char_rot, char_vel, inMaxSeparationDistance, mShape, char_pos, collector, inLockBodies);
+
+	// Copy results
+	mGroundBodyID = collector.mGroundBodyID;
+	mGroundBodySubShapeID = collector.mGroundBodySubShapeID;
+	mGroundPosition = collector.mGroundPosition;
+	mGroundNormal = collector.mGroundNormal;
+
+	// Get additional data from body
+	BodyLockRead lock(sCharacterGetBodyLockInterface(mSystem, inLockBodies), mGroundBodyID);
+	if (lock.Succeeded())
+	{
+		const Body &body = lock.GetBody();
+
+		// Update ground state
+		RMat44 inv_transform = RMat44::sInverseRotationTranslation(char_rot, char_pos);
+		if (mSupportingVolume.SignedDistance(Vec3(inv_transform * mGroundPosition)) > 0.0f)
+			mGroundState = EGroundState::NotSupported;
+		else if (IsSlopeTooSteep(mGroundNormal))
+			mGroundState = EGroundState::OnSteepGround;
+		else
+			mGroundState = EGroundState::OnGround;
+
+		// Copy other body properties
+		mGroundMaterial = body.GetShape()->GetMaterial(mGroundBodySubShapeID);
+		mGroundVelocity = body.GetPointVelocity(mGroundPosition);
+		mGroundUserData = body.GetUserData();
+	}
+	else
+	{
+		mGroundState = EGroundState::InAir;
+		mGroundMaterial = PhysicsMaterial::sDefault;
+		mGroundVelocity = Vec3::sZero();
+		mGroundUserData = 0;
+	}
+}
+
+void Character::SetLinearAndAngularVelocity(Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity, bool inLockBodies)
+{
+	sCharacterGetBodyInterface(mSystem, inLockBodies).SetLinearAndAngularVelocity(mBodyID, inLinearVelocity, inAngularVelocity);
+}
+
+Vec3 Character::GetLinearVelocity(bool inLockBodies) const
+{
+	return sCharacterGetBodyInterface(mSystem, inLockBodies).GetLinearVelocity(mBodyID);
+}
+
+void Character::SetLinearVelocity(Vec3Arg inLinearVelocity, bool inLockBodies)
+{
+	sCharacterGetBodyInterface(mSystem, inLockBodies).SetLinearVelocity(mBodyID, inLinearVelocity);
+}
+
+void Character::AddLinearVelocity(Vec3Arg inLinearVelocity, bool inLockBodies)
+{
+	sCharacterGetBodyInterface(mSystem, inLockBodies).AddLinearVelocity(mBodyID, inLinearVelocity);
+}
+
+void Character::AddImpulse(Vec3Arg inImpulse, bool inLockBodies)
+{
+	sCharacterGetBodyInterface(mSystem, inLockBodies).AddImpulse(mBodyID, inImpulse);
+}
+
+void Character::GetPositionAndRotation(RVec3 &outPosition, Quat &outRotation, bool inLockBodies) const
+{
+	sCharacterGetBodyInterface(mSystem, inLockBodies).GetPositionAndRotation(mBodyID, outPosition, outRotation);
+}
+
+void Character::SetPositionAndRotation(RVec3Arg inPosition, QuatArg inRotation, EActivation inActivationMode, bool inLockBodies) const
+{
+	sCharacterGetBodyInterface(mSystem, inLockBodies).SetPositionAndRotation(mBodyID, inPosition, inRotation, inActivationMode);
+}
+
+RVec3 Character::GetPosition(bool inLockBodies) const
+{
+	return sCharacterGetBodyInterface(mSystem, inLockBodies).GetPosition(mBodyID);
+}
+
+void Character::SetPosition(RVec3Arg inPosition, EActivation inActivationMode, bool inLockBodies)
+{
+	sCharacterGetBodyInterface(mSystem, inLockBodies).SetPosition(mBodyID, inPosition, inActivationMode);
+}
+
+Quat Character::GetRotation(bool inLockBodies) const
+{
+	return sCharacterGetBodyInterface(mSystem, inLockBodies).GetRotation(mBodyID);
+}
+
+void Character::SetRotation(QuatArg inRotation, EActivation inActivationMode, bool inLockBodies)
+{
+	sCharacterGetBodyInterface(mSystem, inLockBodies).SetRotation(mBodyID, inRotation, inActivationMode);
+}
+
+RVec3 Character::GetCenterOfMassPosition(bool inLockBodies) const
+{
+	return sCharacterGetBodyInterface(mSystem, inLockBodies).GetCenterOfMassPosition(mBodyID);
+}
+
+RMat44 Character::GetWorldTransform(bool inLockBodies) const
+{
+	return sCharacterGetBodyInterface(mSystem, inLockBodies).GetWorldTransform(mBodyID);
+}
+
+void Character::SetLayer(ObjectLayer inLayer, bool inLockBodies)
+{
+	mLayer = inLayer;
+
+	sCharacterGetBodyInterface(mSystem, inLockBodies).SetObjectLayer(mBodyID, inLayer);
+}
+
+bool Character::SetShape(const Shape *inShape, float inMaxPenetrationDepth, bool inLockBodies)
+{
+	if (inMaxPenetrationDepth < FLT_MAX)
+	{
+		// Collector that checks if there is anything in the way while switching to inShape
+		class MyCollector : public CollideShapeCollector
+		{
+		public:
+			// Constructor
+			explicit			MyCollector(float inMaxPenetrationDepth) : mMaxPenetrationDepth(inMaxPenetrationDepth) { }
+
+			// See: CollectorType::AddHit
+			virtual void		AddHit(const CollideShapeResult &inResult) override
+			{
+				if (inResult.mPenetrationDepth > mMaxPenetrationDepth)
+				{
+					mHadCollision = true;
+					ForceEarlyOut();
+				}
+			}
+
+			float				mMaxPenetrationDepth;
+			bool				mHadCollision = false;
+		};
+
+		// Test if anything is in the way of switching
+		RVec3 char_pos = GetPosition(inLockBodies);
+		MyCollector collector(inMaxPenetrationDepth);
+		CheckCollision(inShape, 0.0f, char_pos, collector, inLockBodies);
+		if (collector.mHadCollision)
+			return false;
+	}
+
+	// Switch the shape
+	mShape = inShape;
+	sCharacterGetBodyInterface(mSystem, inLockBodies).SetShape(mBodyID, mShape, false, EActivation::Activate);
+	return true;
+}
+
+TransformedShape Character::GetTransformedShape(bool inLockBodies) const
+{
+	return sCharacterGetBodyInterface(mSystem, inLockBodies).GetTransformedShape(mBodyID);
+}
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Character/Character.h

@@ -0,0 +1,151 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Character/CharacterBase.h>
+#include <Jolt/Physics/Collision/ObjectLayer.h>
+#include <Jolt/Physics/Collision/TransformedShape.h>
+#include <Jolt/Physics/EActivation.h>
+#include <Jolt/Physics/Body/AllowedDOFs.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Contains the configuration of a character
+class JPH_EXPORT CharacterSettings : public CharacterBaseSettings
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Layer that this character will be added to
+	ObjectLayer							mLayer = 0;
+
+	/// Mass of the character
+	float								mMass = 80.0f;
+
+	/// Friction for the character
+	float								mFriction = 0.2f;
+
+	/// Value to multiply gravity with for this character
+	float								mGravityFactor = 1.0f;
+
+	/// Allowed degrees of freedom for this character
+	EAllowedDOFs						mAllowedDOFs = EAllowedDOFs::TranslationX | EAllowedDOFs::TranslationY | EAllowedDOFs::TranslationZ;
+};
+
+/// Runtime character object.
+/// This object usually represents the player or a humanoid AI. It uses a single rigid body,
+/// usually with a capsule shape to simulate movement and collision for the character.
+/// The character is a keyframed object, the application controls it by setting the velocity.
+class JPH_EXPORT Character : public CharacterBase
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+	/// @param inSettings The settings for the character
+	/// @param inPosition Initial position for the character
+	/// @param inRotation Initial rotation for the character (usually only around Y)
+	/// @param inUserData Application specific value
+	/// @param inSystem Physics system that this character will be added to later
+										Character(const CharacterSettings *inSettings, RVec3Arg inPosition, QuatArg inRotation, uint64 inUserData, PhysicsSystem *inSystem);
+
+	/// Destructor
+	virtual								~Character() override;
+
+	/// Add bodies and constraints to the system and optionally activate the bodies
+	void								AddToPhysicsSystem(EActivation inActivationMode = EActivation::Activate, bool inLockBodies = true);
+
+	/// Remove bodies and constraints from the system
+	void								RemoveFromPhysicsSystem(bool inLockBodies = true);
+
+	/// Wake up the character
+	void								Activate(bool inLockBodies = true);
+
+	/// Needs to be called after every PhysicsSystem::Update
+	/// @param inMaxSeparationDistance Max distance between the floor and the character to still consider the character standing on the floor
+	/// @param inLockBodies If the collision query should use the locking body interface (true) or the non locking body interface (false)
+	void								PostSimulation(float inMaxSeparationDistance, bool inLockBodies = true);
+
+	/// Control the velocity of the character
+	void								SetLinearAndAngularVelocity(Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity, bool inLockBodies = true);
+
+	/// Get the linear velocity of the character (m / s)
+	Vec3								GetLinearVelocity(bool inLockBodies = true) const;
+
+	/// Set the linear velocity of the character (m / s)
+	void								SetLinearVelocity(Vec3Arg inLinearVelocity, bool inLockBodies = true);
+
+	/// Add world space linear velocity to current velocity (m / s)
+	void								AddLinearVelocity(Vec3Arg inLinearVelocity, bool inLockBodies = true);
+
+	/// Add impulse to the center of mass of the character
+	void								AddImpulse(Vec3Arg inImpulse, bool inLockBodies = true);
+
+	/// Get the body associated with this character
+	BodyID								GetBodyID() const										{ return mBodyID; }
+
+	/// Get position / rotation of the body
+	void								GetPositionAndRotation(RVec3 &outPosition, Quat &outRotation, bool inLockBodies = true) const;
+
+	/// Set the position / rotation of the body, optionally activating it.
+	void								SetPositionAndRotation(RVec3Arg inPosition, QuatArg inRotation, EActivation inActivationMode = EActivation::Activate, bool inLockBodies = true) const;
+
+	/// Get the position of the character
+	RVec3								GetPosition(bool inLockBodies = true) const;
+
+	/// Set the position of the character, optionally activating it.
+	void								SetPosition(RVec3Arg inPosition, EActivation inActivationMode = EActivation::Activate, bool inLockBodies = true);
+
+	/// Get the rotation of the character
+	Quat								GetRotation(bool inLockBodies = true) const;
+
+	/// Set the rotation of the character, optionally activating it.
+	void								SetRotation(QuatArg inRotation, EActivation inActivationMode = EActivation::Activate, bool inLockBodies = true);
+
+	/// Position of the center of mass of the underlying rigid body
+	RVec3								GetCenterOfMassPosition(bool inLockBodies = true) const;
+
+	/// Calculate the world transform of the character
+	RMat44								GetWorldTransform(bool inLockBodies = true) const;
+
+	/// Get the layer of the character
+	ObjectLayer							GetLayer() const										{ return mLayer; }
+
+	/// Update the layer of the character
+	void								SetLayer(ObjectLayer inLayer, bool inLockBodies = true);
+
+	/// Switch the shape of the character (e.g. for stance). When inMaxPenetrationDepth is not FLT_MAX, it checks
+	/// if the new shape collides before switching shape. Returns true if the switch succeeded.
+	bool								SetShape(const Shape *inShape, float inMaxPenetrationDepth, bool inLockBodies = true);
+
+	/// Get the transformed shape that represents the volume of the character, can be used for collision checks.
+	TransformedShape					GetTransformedShape(bool inLockBodies = true) const;
+
+	/// @brief Get all contacts for the character at a particular location
+	/// @param inPosition Position to test.
+	/// @param inRotation Rotation at which to test the shape.
+	/// @param inMovementDirection A hint in which direction the character is moving, will be used to calculate a proper normal.
+	/// @param inMaxSeparationDistance How much distance around the character you want to report contacts in (can be 0 to match the character exactly).
+	/// @param inShape Shape to test collision with.
+	/// @param inBaseOffset All hit results will be returned relative to this offset, can be zero to get results in world position, but when you're testing far from the origin you get better precision by picking a position that's closer e.g. GetPosition() since floats are most accurate near the origin
+	/// @param ioCollector Collision collector that receives the collision results.
+	/// @param inLockBodies If the collision query should use the locking body interface (true) or the non locking body interface (false)
+	void								CheckCollision(RVec3Arg inPosition, QuatArg inRotation, Vec3Arg inMovementDirection, float inMaxSeparationDistance, const Shape *inShape, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, bool inLockBodies = true) const;
+
+private:
+	/// Check collisions between inShape and the world using the center of mass transform
+	void								CheckCollision(RMat44Arg inCenterOfMassTransform, Vec3Arg inMovementDirection, float inMaxSeparationDistance, const Shape *inShape, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, bool inLockBodies) const;
+
+	/// Check collisions between inShape and the world using the current position / rotation of the character
+	void								CheckCollision(const Shape *inShape, float inMaxSeparationDistance, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, bool inLockBodies) const;
+
+	/// The body of this character
+	BodyID								mBodyID;
+
+	/// The layer the body is in
+	ObjectLayer							mLayer;
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Character/CharacterBase.cpp

@@ -0,0 +1,59 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Character/CharacterBase.h>
+#include <Jolt/Physics/StateRecorder.h>
+
+JPH_NAMESPACE_BEGIN
+
+CharacterBase::CharacterBase(const CharacterBaseSettings *inSettings, PhysicsSystem *inSystem) :
+	mSystem(inSystem),
+	mShape(inSettings->mShape),
+	mUp(inSettings->mUp),
+	mSupportingVolume(inSettings->mSupportingVolume)
+{
+	// Initialize max slope angle
+	SetMaxSlopeAngle(inSettings->mMaxSlopeAngle);
+}
+
+const char *CharacterBase::sToString(EGroundState inState)
+{
+	switch (inState)
+	{
+	case EGroundState::OnGround:		return "OnGround";
+	case EGroundState::OnSteepGround:	return "OnSteepGround";
+	case EGroundState::NotSupported:	return "NotSupported";
+	case EGroundState::InAir:			return "InAir";
+	}
+
+	JPH_ASSERT(false);
+	return "Unknown";
+}
+
+void CharacterBase::SaveState(StateRecorder &inStream) const
+{
+	inStream.Write(mGroundState);
+	inStream.Write(mGroundBodyID);
+	inStream.Write(mGroundBodySubShapeID);
+	inStream.Write(mGroundPosition);
+	inStream.Write(mGroundNormal);
+	inStream.Write(mGroundVelocity);
+	// Can't save user data (may be a pointer) and material
+}
+
+void CharacterBase::RestoreState(StateRecorder &inStream)
+{
+	inStream.Read(mGroundState);
+	inStream.Read(mGroundBodyID);
+	inStream.Read(mGroundBodySubShapeID);
+	inStream.Read(mGroundPosition);
+	inStream.Read(mGroundNormal);
+	inStream.Read(mGroundVelocity);
+	mGroundUserData = 0; // Cannot restore user data
+	mGroundMaterial = PhysicsMaterial::sDefault; // Cannot restore material
+}
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Character/CharacterBase.h

@@ -0,0 +1,157 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/Reference.h>
+#include <Jolt/Core/NonCopyable.h>
+#include <Jolt/Physics/Body/BodyID.h>
+#include <Jolt/Physics/Collision/Shape/Shape.h>
+#include <Jolt/Physics/Collision/Shape/SubShapeID.h>
+#include <Jolt/Physics/Collision/PhysicsMaterial.h>
+
+JPH_NAMESPACE_BEGIN
+
+class PhysicsSystem;
+class StateRecorder;
+
+/// Base class for configuration of a character
+class JPH_EXPORT CharacterBaseSettings : public RefTarget<CharacterBaseSettings>
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+										CharacterBaseSettings() = default;
+										CharacterBaseSettings(const CharacterBaseSettings &inSettings) = default;
+	CharacterBaseSettings &				operator = (const CharacterBaseSettings &inSettings) = default;
+
+	/// Virtual destructor
+	virtual								~CharacterBaseSettings() = default;
+
+	/// Vector indicating the up direction of the character
+	Vec3								mUp = Vec3::sAxisY();
+
+	/// Plane, defined in local space relative to the character. Every contact behind this plane can support the
+	/// character, every contact in front of this plane is treated as only colliding with the player.
+	/// Default: Accept any contact.
+	Plane								mSupportingVolume { Vec3::sAxisY(), -1.0e10f };
+
+	/// Maximum angle of slope that character can still walk on (radians).
+	float								mMaxSlopeAngle = DegreesToRadians(50.0f);
+
+	/// Set to indicate that extra effort should be made to try to remove ghost contacts (collisions with internal edges of a mesh). This is more expensive but makes bodies move smoother over a mesh with convex edges.
+	bool								mEnhancedInternalEdgeRemoval = false;
+
+	/// Initial shape that represents the character's volume.
+	/// Usually this is a capsule, make sure the shape is made so that the bottom of the shape is at (0, 0, 0).
+	RefConst<Shape>						mShape;
+};
+
+/// Base class for character class
+class JPH_EXPORT CharacterBase : public RefTarget<CharacterBase>, public NonCopyable
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+										CharacterBase(const CharacterBaseSettings *inSettings, PhysicsSystem *inSystem);
+
+	/// Destructor
+	virtual								~CharacterBase() = default;
+
+	/// Set the maximum angle of slope that character can still walk on (radians)
+	void								SetMaxSlopeAngle(float inMaxSlopeAngle)					{ mCosMaxSlopeAngle = Cos(inMaxSlopeAngle); }
+	float								GetCosMaxSlopeAngle() const								{ return mCosMaxSlopeAngle; }
+
+	/// Set the up vector for the character
+	void								SetUp(Vec3Arg inUp)										{ mUp = inUp; }
+	Vec3								GetUp() const											{ return mUp; }
+
+	/// Check if the normal of the ground surface is too steep to walk on
+	bool								IsSlopeTooSteep(Vec3Arg inNormal) const
+	{
+		// If cos max slope angle is close to one the system is turned off,
+		// otherwise check the angle between the up and normal vector
+		return mCosMaxSlopeAngle < cNoMaxSlopeAngle && inNormal.Dot(mUp) < mCosMaxSlopeAngle;
+	}
+
+	/// Get the current shape that the character is using.
+	const Shape *						GetShape() const										{ return mShape; }
+
+	enum class EGroundState
+	{
+		OnGround,						///< Character is on the ground and can move freely.
+		OnSteepGround,					///< Character is on a slope that is too steep and can't climb up any further. The caller should start applying downward velocity if sliding from the slope is desired.
+		NotSupported,					///< Character is touching an object, but is not supported by it and should fall. The GetGroundXXX functions will return information about the touched object.
+		InAir,							///< Character is in the air and is not touching anything.
+	};
+
+	/// Debug function to convert enum values to string
+	static const char *					sToString(EGroundState inState);
+
+	///@name Properties of the ground this character is standing on
+
+	/// Current ground state
+	EGroundState						GetGroundState() const									{ return mGroundState; }
+
+	/// Returns true if the player is supported by normal or steep ground
+	bool								IsSupported() const										{ return mGroundState == EGroundState::OnGround || mGroundState == EGroundState::OnSteepGround; }
+
+	/// Get the contact point with the ground
+	RVec3								GetGroundPosition() const								{ return mGroundPosition; }
+
+	/// Get the contact normal with the ground
+	Vec3								GetGroundNormal() const									{ return mGroundNormal; }
+
+	/// Velocity in world space of ground
+	Vec3								GetGroundVelocity() const								{ return mGroundVelocity; }
+
+	/// Material that the character is standing on
+	const PhysicsMaterial *				GetGroundMaterial() const								{ return mGroundMaterial; }
+
+	/// BodyID of the object the character is standing on. Note may have been removed!
+	BodyID								GetGroundBodyID() const									{ return mGroundBodyID; }
+
+	/// Sub part of the body that we're standing on.
+	SubShapeID							GetGroundSubShapeID() const								{ return mGroundBodySubShapeID; }
+
+	/// User data value of the body that we're standing on
+	uint64								GetGroundUserData() const								{ return mGroundUserData; }
+
+	// Saving / restoring state for replay
+	virtual void						SaveState(StateRecorder &inStream) const;
+	virtual void						RestoreState(StateRecorder &inStream);
+
+protected:
+	// Cached physics system
+	PhysicsSystem *						mSystem;
+
+	// The shape that the body currently has
+	RefConst<Shape>						mShape;
+
+	// The character's world space up axis
+	Vec3								mUp;
+
+	// Every contact behind this plane can support the character
+	Plane								mSupportingVolume;
+
+	// Beyond this value there is no max slope
+	static constexpr float				cNoMaxSlopeAngle = 0.9999f;
+
+	// Cosine of the maximum angle of slope that character can still walk on
+	float								mCosMaxSlopeAngle;
+
+	// Ground properties
+	EGroundState						mGroundState = EGroundState::InAir;
+	BodyID								mGroundBodyID;
+	SubShapeID							mGroundBodySubShapeID;
+	RVec3								mGroundPosition = RVec3::sZero();
+	Vec3								mGroundNormal = Vec3::sZero();
+	Vec3								mGroundVelocity = Vec3::sZero();
+	RefConst<PhysicsMaterial>			mGroundMaterial = PhysicsMaterial::sDefault;
+	uint64								mGroundUserData = 0;
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Character/CharacterID.h

@@ -0,0 +1,98 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2025 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/HashCombine.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// ID of a character. Used primarily to identify deleted characters and to sort deterministically.
+class JPH_EXPORT CharacterID
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	static constexpr uint32	cInvalidCharacterID = 0xffffffff;	///< The value for an invalid character ID
+
+	/// Construct invalid character ID
+							CharacterID() :
+		mID(cInvalidCharacterID)
+	{
+	}
+
+	/// Construct with specific value, make sure you don't use the same value twice!
+	explicit				CharacterID(uint32 inID) :
+		mID(inID)
+	{
+	}
+
+	/// Get the numeric value of the ID
+	inline uint32			GetValue() const
+	{
+		return mID;
+	}
+
+	/// Check if the ID is valid
+	inline bool				IsInvalid() const
+	{
+		return mID == cInvalidCharacterID;
+	}
+
+	/// Equals check
+	inline bool				operator == (const CharacterID &inRHS) const
+	{
+		return mID == inRHS.mID;
+	}
+
+	/// Not equals check
+	inline bool				operator != (const CharacterID &inRHS) const
+	{
+		return mID != inRHS.mID;
+	}
+
+	/// Smaller than operator, can be used for sorting characters
+	inline bool				operator < (const CharacterID &inRHS) const
+	{
+		return mID < inRHS.mID;
+	}
+
+	/// Greater than operator, can be used for sorting characters
+	inline bool				operator > (const CharacterID &inRHS) const
+	{
+		return mID > inRHS.mID;
+	}
+
+	/// Get the hash for this character ID
+	inline uint64			GetHash() const
+	{
+		return Hash<uint32>{} (mID);
+	}
+
+	/// Generate the next available character ID
+	static CharacterID		sNextCharacterID()
+	{
+		for (;;)
+		{
+			uint32 next = sNextID.fetch_add(1, std::memory_order_relaxed);
+			if (next != cInvalidCharacterID)
+				return CharacterID(next);
+		}
+	}
+
+	/// Set the next available character ID, can be used after destroying all character to prepare for a second deterministic run
+	static void				sSetNextCharacterID(uint32 inNextValue = 1)
+	{
+		sNextID.store(inNextValue, std::memory_order_relaxed);
+	}
+
+private:
+	/// Next character ID to be assigned
+	inline static atomic<uint32> sNextID = 1;
+
+	/// ID value
+	uint32					mID;
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Character/CharacterVirtual.h

@@ -0,0 +1,743 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Character/CharacterBase.h>
+#include <Jolt/Physics/Character/CharacterID.h>
+#include <Jolt/Physics/Body/MotionType.h>
+#include <Jolt/Physics/Body/BodyFilter.h>
+#include <Jolt/Physics/Collision/BroadPhase/BroadPhaseLayer.h>
+#include <Jolt/Physics/Collision/ObjectLayer.h>
+#include <Jolt/Physics/Collision/TransformedShape.h>
+#include <Jolt/Core/STLTempAllocator.h>
+
+JPH_NAMESPACE_BEGIN
+
+class CharacterVirtual;
+class CollideShapeSettings;
+
+/// Contains the configuration of a character
+class JPH_EXPORT CharacterVirtualSettings : public CharacterBaseSettings
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// ID to give to this character. This is used for deterministically sorting and as an identifier to represent the character in the contact removal callback.
+	CharacterID							mID = CharacterID::sNextCharacterID();
+
+	/// Character mass (kg). Used to push down objects with gravity when the character is standing on top.
+	float								mMass = 70.0f;
+
+	/// Maximum force with which the character can push other bodies (N).
+	float								mMaxStrength = 100.0f;
+
+	/// An extra offset applied to the shape in local space. This allows applying an extra offset to the shape in local space.
+	Vec3								mShapeOffset = Vec3::sZero();
+
+	///@name Movement settings
+	EBackFaceMode						mBackFaceMode = EBackFaceMode::CollideWithBackFaces;	///< When colliding with back faces, the character will not be able to move through back facing triangles. Use this if you have triangles that need to collide on both sides.
+	float								mPredictiveContactDistance = 0.1f;						///< How far to scan outside of the shape for predictive contacts. A value of 0 will most likely cause the character to get stuck as it cannot properly calculate a sliding direction anymore. A value that's too high will cause ghost collisions.
+	uint								mMaxCollisionIterations = 5;							///< Max amount of collision loops
+	uint								mMaxConstraintIterations = 15;							///< How often to try stepping in the constraint solving
+	float								mMinTimeRemaining = 1.0e-4f;							///< Early out condition: If this much time is left to simulate we are done
+	float								mCollisionTolerance = 1.0e-3f;							///< How far we're willing to penetrate geometry
+	float								mCharacterPadding = 0.02f;								///< How far we try to stay away from the geometry, this ensures that the sweep will hit as little as possible lowering the collision cost and reducing the risk of getting stuck
+	uint								mMaxNumHits = 256;										///< Max num hits to collect in order to avoid excess of contact points collection
+	float								mHitReductionCosMaxAngle = 0.999f;						///< Cos(angle) where angle is the maximum angle between two hits contact normals that are allowed to be merged during hit reduction. Default is around 2.5 degrees. Set to -1 to turn off.
+	float								mPenetrationRecoverySpeed = 1.0f;						///< This value governs how fast a penetration will be resolved, 0 = nothing is resolved, 1 = everything in one update
+
+	/// This character can optionally have an inner rigid body. This rigid body can be used to give the character presence in the world. When set it means that:
+	/// - Regular collision checks (e.g. NarrowPhaseQuery::CastRay) will collide with the rigid body (they cannot collide with CharacterVirtual since it is not added to the broad phase)
+	/// - Regular contact callbacks will be called through the ContactListener (next to the ones that will be passed to the CharacterContactListener)
+	/// - Fast moving objects of motion quality LinearCast will not be able to pass through the CharacterVirtual in 1 time step
+	RefConst<Shape>						mInnerBodyShape;
+
+	/// For a deterministic simulation, it is important to have a deterministic body ID. When set and when mInnerBodyShape is specified,
+	/// the inner body will be created with this specified ID instead of a generated ID.
+	BodyID								mInnerBodyIDOverride;
+
+	/// Layer that the inner rigid body will be added to
+	ObjectLayer							mInnerBodyLayer = 0;
+};
+
+/// This class contains settings that allow you to override the behavior of a character's collision response
+class CharacterContactSettings
+{
+public:
+	/// True when the object can push the virtual character.
+	bool								mCanPushCharacter = true;
+
+	/// True when the virtual character can apply impulses (push) the body.
+	/// Note that this only works against rigid bodies. Other CharacterVirtual objects can only be moved in their own update,
+	/// so you must ensure that in their OnCharacterContactAdded mCanPushCharacter is true.
+	bool								mCanReceiveImpulses = true;
+};
+
+/// This class receives callbacks when a virtual character hits something.
+class JPH_EXPORT CharacterContactListener
+{
+public:
+	/// Destructor
+	virtual								~CharacterContactListener() = default;
+
+	/// 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.
+	/// Note that inBody2 is locked during the callback so you can read its properties freely.
+	virtual void						OnAdjustBodyVelocity(const CharacterVirtual *inCharacter, const Body &inBody2, Vec3 &ioLinearVelocity, Vec3 &ioAngularVelocity) { /* Do nothing, the linear and angular velocity are already filled in */ }
+
+	/// Checks if a character can collide with specified body. Return true if the contact is valid.
+	virtual bool						OnContactValidate(const CharacterVirtual *inCharacter, const BodyID &inBodyID2, const SubShapeID &inSubShapeID2) { return true; }
+
+	/// Same as OnContactValidate but when colliding with a CharacterVirtual
+	virtual bool						OnCharacterContactValidate(const CharacterVirtual *inCharacter, const CharacterVirtual *inOtherCharacter, const SubShapeID &inSubShapeID2) { return true; }
+
+	/// Called whenever the character collides with a body for the first time.
+	/// @param inCharacter Character that is being solved
+	/// @param inBodyID2 Body ID of body that is being hit
+	/// @param inSubShapeID2 Sub shape ID of shape that is being hit
+	/// @param inContactPosition World space contact position
+	/// @param inContactNormal World space contact normal
+	/// @param ioSettings Settings returned by the contact callback to indicate how the character should behave
+	virtual void						OnContactAdded(const CharacterVirtual *inCharacter, const BodyID &inBodyID2, const SubShapeID &inSubShapeID2, RVec3Arg inContactPosition, Vec3Arg inContactNormal, CharacterContactSettings &ioSettings) { /* Default do nothing */ }
+
+	/// Called whenever the character persists colliding with a body.
+	/// @param inCharacter Character that is being solved
+	/// @param inBodyID2 Body ID of body that is being hit
+	/// @param inSubShapeID2 Sub shape ID of shape that is being hit
+	/// @param inContactPosition World space contact position
+	/// @param inContactNormal World space contact normal
+	/// @param ioSettings Settings returned by the contact callback to indicate how the character should behave
+	virtual void						OnContactPersisted(const CharacterVirtual *inCharacter, const BodyID &inBodyID2, const SubShapeID &inSubShapeID2, RVec3Arg inContactPosition, Vec3Arg inContactNormal, CharacterContactSettings &ioSettings) { /* Default do nothing */ }
+
+	/// Called whenever the character loses contact with a body.
+	/// Note that there is no guarantee that the body or its sub shape still exists at this point. The body may have been deleted since the last update.
+	/// @param inCharacter Character that is being solved
+	/// @param inBodyID2 Body ID of body that is being hit
+	/// @param inSubShapeID2 Sub shape ID of shape that is being hit
+	virtual void						OnContactRemoved(const CharacterVirtual *inCharacter, const BodyID &inBodyID2, const SubShapeID &inSubShapeID2) { /* Default do nothing */ }
+
+	/// Same as OnContactAdded but when colliding with a CharacterVirtual
+	virtual void						OnCharacterContactAdded(const CharacterVirtual *inCharacter, const CharacterVirtual *inOtherCharacter, const SubShapeID &inSubShapeID2, RVec3Arg inContactPosition, Vec3Arg inContactNormal, CharacterContactSettings &ioSettings) { /* Default do nothing */ }
+
+	/// Same as OnContactPersisted but when colliding with a CharacterVirtual
+	virtual void						OnCharacterContactPersisted(const CharacterVirtual *inCharacter, const CharacterVirtual *inOtherCharacter, const SubShapeID &inSubShapeID2, RVec3Arg inContactPosition, Vec3Arg inContactNormal, CharacterContactSettings &ioSettings) { /* Default do nothing */ }
+
+	/// Same as OnContactRemoved but when colliding with a CharacterVirtual
+	/// Note that inOtherCharacterID can be the ID of a character that has been deleted. This happens if the character was in contact with this character during the last update, but has been deleted since.
+	virtual void						OnCharacterContactRemoved(const CharacterVirtual *inCharacter, const CharacterID &inOtherCharacterID, const SubShapeID &inSubShapeID2) { /* Default do nothing */ }
+
+	/// Called whenever a contact is being used by the solver. Allows the listener to override the resulting character velocity (e.g. by preventing sliding along certain surfaces).
+	/// @param inCharacter Character that is being solved
+	/// @param inBodyID2 Body ID of body that is being hit
+	/// @param inSubShapeID2 Sub shape ID of shape that is being hit
+	/// @param inContactPosition World space contact position
+	/// @param inContactNormal World space contact normal
+	/// @param inContactVelocity World space velocity of contact point (e.g. for a moving platform)
+	/// @param inContactMaterial Material of contact point
+	/// @param inCharacterVelocity World space velocity of the character prior to hitting this contact
+	/// @param ioNewCharacterVelocity Contains the calculated world space velocity of the character after hitting this contact, this velocity slides along the surface of the contact. Can be modified by the listener to provide an alternative velocity.
+	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) { /* Default do nothing */ }
+
+	/// Same as OnContactSolve but when colliding with a CharacterVirtual
+	virtual void						OnCharacterContactSolve(const CharacterVirtual *inCharacter, const CharacterVirtual *inOtherCharacter, const SubShapeID &inSubShapeID2, RVec3Arg inContactPosition, Vec3Arg inContactNormal, Vec3Arg inContactVelocity, const PhysicsMaterial *inContactMaterial, Vec3Arg inCharacterVelocity, Vec3 &ioNewCharacterVelocity) { /* Default do nothing */ }
+};
+
+/// Interface class that allows a CharacterVirtual to check collision with other CharacterVirtual instances.
+/// Since CharacterVirtual instances are not registered anywhere, it is up to the application to test collision against relevant characters.
+/// The characters could be stored in a tree structure to make this more efficient.
+class JPH_EXPORT CharacterVsCharacterCollision : public NonCopyable
+{
+public:
+	virtual								~CharacterVsCharacterCollision() = default;
+
+	/// Collide a character against other CharacterVirtuals.
+	/// @param inCharacter The character to collide.
+	/// @param inCenterOfMassTransform Center of mass transform for this character.
+	/// @param inCollideShapeSettings Settings for the collision check.
+	/// @param inBaseOffset All hit results will be returned relative to this offset, can be zero to get results in world position, but when you're testing far from the origin you get better precision by picking a position that's closer e.g. GetPosition() since floats are most accurate near the origin
+	/// @param ioCollector Collision collector that receives the collision results.
+	virtual void						CollideCharacter(const CharacterVirtual *inCharacter, RMat44Arg inCenterOfMassTransform, const CollideShapeSettings &inCollideShapeSettings, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector) const = 0;
+
+	/// Cast a character against other CharacterVirtuals.
+	/// @param inCharacter The character to cast.
+	/// @param inCenterOfMassTransform Center of mass transform for this character.
+	/// @param inDirection Direction and length to cast in.
+	/// @param inShapeCastSettings Settings for the shape cast.
+	/// @param inBaseOffset All hit results will be returned relative to this offset, can be zero to get results in world position, but when you're testing far from the origin you get better precision by picking a position that's closer e.g. GetPosition() since floats are most accurate near the origin
+	/// @param ioCollector Collision collector that receives the collision results.
+	virtual void						CastCharacter(const CharacterVirtual *inCharacter, RMat44Arg inCenterOfMassTransform, Vec3Arg inDirection, const ShapeCastSettings &inShapeCastSettings, RVec3Arg inBaseOffset, CastShapeCollector &ioCollector) const = 0;
+};
+
+/// Simple collision checker that loops over all registered characters.
+/// This is a brute force checking algorithm. If you have a lot of characters you may want to store your characters
+/// in a hierarchical structure to make this more efficient.
+/// Note that this is not thread safe, so make sure that only one CharacterVirtual is checking collision at a time.
+class JPH_EXPORT CharacterVsCharacterCollisionSimple : public CharacterVsCharacterCollision
+{
+public:
+	/// Add a character to the list of characters to check collision against.
+	void								Add(CharacterVirtual *inCharacter)						{ mCharacters.push_back(inCharacter); }
+
+	/// Remove a character from the list of characters to check collision against.
+	void								Remove(const CharacterVirtual *inCharacter);
+
+	// See: CharacterVsCharacterCollision
+	virtual void						CollideCharacter(const CharacterVirtual *inCharacter, RMat44Arg inCenterOfMassTransform, const CollideShapeSettings &inCollideShapeSettings, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector) const override;
+	virtual void						CastCharacter(const CharacterVirtual *inCharacter, RMat44Arg inCenterOfMassTransform, Vec3Arg inDirection, const ShapeCastSettings &inShapeCastSettings, RVec3Arg inBaseOffset, CastShapeCollector &ioCollector) const override;
+
+	Array<CharacterVirtual *>			mCharacters;											///< The list of characters to check collision against
+};
+
+/// Runtime character object.
+/// This object usually represents the player. Contrary to the Character class it doesn't use a rigid body but moves doing collision checks only (hence the name virtual).
+/// The advantage of this is that you can determine when the character moves in the frame (usually this has to happen at a very particular point in the frame)
+/// but the downside is that other objects don't see this virtual character. To make a CharacterVirtual visible to the simulation, you can optionally create an inner
+/// rigid body through CharacterVirtualSettings::mInnerBodyShape. A CharacterVirtual is not tracked by the PhysicsSystem so you need to update it yourself. This also means
+/// that a call to PhysicsSystem::SaveState will not save its state, you need to call CharacterVirtual::SaveState yourself.
+class JPH_EXPORT CharacterVirtual : public CharacterBase
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+	/// @param inSettings The settings for the character
+	/// @param inPosition Initial position for the character
+	/// @param inRotation Initial rotation for the character (usually only around the up-axis)
+	/// @param inUserData Application specific value
+	/// @param inSystem Physics system that this character will be added to
+										CharacterVirtual(const CharacterVirtualSettings *inSettings, RVec3Arg inPosition, QuatArg inRotation, uint64 inUserData, PhysicsSystem *inSystem);
+
+	/// Constructor without user data
+										CharacterVirtual(const CharacterVirtualSettings *inSettings, RVec3Arg inPosition, QuatArg inRotation, PhysicsSystem *inSystem) : CharacterVirtual(inSettings, inPosition, inRotation, 0, inSystem) { }
+
+	/// Destructor
+	virtual								~CharacterVirtual() override;
+
+	/// The ID of this character
+	inline const CharacterID &			GetID() const											{ return mID; }
+
+	/// Set the contact listener
+	void								SetListener(CharacterContactListener *inListener)		{ mListener = inListener; }
+
+	/// Get the current contact listener
+	CharacterContactListener *			GetListener() const										{ return mListener; }
+
+	/// Set the character vs character collision interface
+	void								SetCharacterVsCharacterCollision(CharacterVsCharacterCollision *inCharacterVsCharacterCollision) { mCharacterVsCharacterCollision = inCharacterVsCharacterCollision; }
+
+	/// Get the linear velocity of the character (m / s)
+	Vec3								GetLinearVelocity() const								{ return mLinearVelocity; }
+
+	/// Set the linear velocity of the character (m / s)
+	void								SetLinearVelocity(Vec3Arg inLinearVelocity)				{ mLinearVelocity = inLinearVelocity; }
+
+	/// Get the position of the character
+	RVec3								GetPosition() const										{ return mPosition; }
+
+	/// Set the position of the character
+	void								SetPosition(RVec3Arg inPosition)						{ mPosition = inPosition; UpdateInnerBodyTransform(); }
+
+	/// Get the rotation of the character
+	Quat								GetRotation() const										{ return mRotation; }
+
+	/// Set the rotation of the character
+	void								SetRotation(QuatArg inRotation)							{ mRotation = inRotation; UpdateInnerBodyTransform(); }
+
+	// Get the center of mass position of the shape
+	inline RVec3						GetCenterOfMassPosition() const							{ return mPosition + (mRotation * (mShapeOffset + mShape->GetCenterOfMass()) + mCharacterPadding * mUp); }
+
+	/// Calculate the world transform of the character
+	RMat44								GetWorldTransform() const								{ return RMat44::sRotationTranslation(mRotation, mPosition); }
+
+	/// Calculates the transform for this character's center of mass
+	RMat44								GetCenterOfMassTransform() const						{ return GetCenterOfMassTransform(mPosition, mRotation, mShape); }
+
+	/// Character mass (kg)
+	float								GetMass() const											{ return mMass; }
+	void								SetMass(float inMass)									{ mMass = inMass; }
+
+	/// Maximum force with which the character can push other bodies (N)
+	float								GetMaxStrength() const									{ return mMaxStrength; }
+	void								SetMaxStrength(float inMaxStrength)						{ mMaxStrength = inMaxStrength; }
+
+	/// This value governs how fast a penetration will be resolved, 0 = nothing is resolved, 1 = everything in one update
+	float								GetPenetrationRecoverySpeed() const						{ return mPenetrationRecoverySpeed; }
+	void								SetPenetrationRecoverySpeed(float inSpeed)				{ mPenetrationRecoverySpeed = inSpeed; }
+
+	/// Set to indicate that extra effort should be made to try to remove ghost contacts (collisions with internal edges of a mesh). This is more expensive but makes bodies move smoother over a mesh with convex edges.
+	bool								GetEnhancedInternalEdgeRemoval() const					{ return mEnhancedInternalEdgeRemoval; }
+	void								SetEnhancedInternalEdgeRemoval(bool inApply)			{ mEnhancedInternalEdgeRemoval = inApply; }
+
+	/// Character padding
+	float								GetCharacterPadding() const								{ return mCharacterPadding; }
+
+	/// Max num hits to collect in order to avoid excess of contact points collection
+	uint								GetMaxNumHits() const									{ return mMaxNumHits; }
+	void								SetMaxNumHits(uint inMaxHits)							{ mMaxNumHits = inMaxHits; }
+
+	/// Cos(angle) where angle is the maximum angle between two hits contact normals that are allowed to be merged during hit reduction. Default is around 2.5 degrees. Set to -1 to turn off.
+	float								GetHitReductionCosMaxAngle() const						{ return mHitReductionCosMaxAngle; }
+	void								SetHitReductionCosMaxAngle(float inCosMaxAngle)			{ mHitReductionCosMaxAngle = inCosMaxAngle; }
+
+	/// Returns if we exceeded the maximum number of hits during the last collision check and had to discard hits based on distance.
+	/// This can be used to find areas that have too complex geometry for the character to navigate properly.
+	/// To solve you can either increase the max number of hits or simplify the geometry. Note that the character simulation will
+	/// try to do its best to select the most relevant contacts to avoid the character from getting stuck.
+	bool								GetMaxHitsExceeded() const								{ return mMaxHitsExceeded; }
+
+	/// An extra offset applied to the shape in local space. This allows applying an extra offset to the shape in local space. Note that setting it on the fly can cause the shape to teleport into collision.
+	Vec3								GetShapeOffset() const									{ return mShapeOffset; }
+	void								SetShapeOffset(Vec3Arg inShapeOffset)					{ mShapeOffset = inShapeOffset; UpdateInnerBodyTransform(); }
+
+	/// Access to the user data, can be used for anything by the application
+	uint64								GetUserData() const										{ return mUserData; }
+	void								SetUserData(uint64 inUserData);
+
+	/// Optional inner rigid body that proxies the character in the world. Can be used to update body properties.
+	BodyID								GetInnerBodyID() const									{ return mInnerBodyID; }
+
+	/// This function can be called prior to calling Update() to convert a desired velocity into a velocity that won't make the character move further onto steep slopes.
+	/// This velocity can then be set on the character using SetLinearVelocity()
+	/// @param inDesiredVelocity Velocity to clamp against steep walls
+	/// @return A new velocity vector that won't make the character move up steep slopes
+	Vec3								CancelVelocityTowardsSteepSlopes(Vec3Arg inDesiredVelocity) const;
+
+	/// This function is internally called by Update, WalkStairs, StickToFloor and ExtendedUpdate and is responsible for tracking if contacts are added, persisted or removed.
+	/// If you want to do multiple operations on a character (e.g. first Update then WalkStairs), you can surround the code with a StartTrackingContactChanges and FinishTrackingContactChanges pair
+	/// to only receive a single callback per contact on the CharacterContactListener. If you don't do this then you could for example receive a contact added callback during the Update and a
+	/// contact persisted callback during WalkStairs.
+	void								StartTrackingContactChanges();
+
+	/// This call triggers contact removal callbacks and is used in conjunction with StartTrackingContactChanges.
+	void								FinishTrackingContactChanges();
+
+	/// This is the main update function. It moves the character according to its current velocity (the character is similar to a kinematic body in the sense
+	/// that you set the velocity and the character will follow unless collision is blocking the way). Note it's your own responsibility to apply gravity to the character velocity!
+	/// Different surface materials (like ice) can be emulated by getting the ground material and adjusting the velocity and/or the max slope angle accordingly every frame.
+	/// @param inDeltaTime Time step to simulate.
+	/// @param inGravity Gravity vector (m/s^2). This gravity vector is only used when the character is standing on top of another object to apply downward force.
+	/// @param inBroadPhaseLayerFilter Filter that is used to check if the character collides with something in the broadphase.
+	/// @param inObjectLayerFilter Filter that is used to check if a character collides with a layer.
+	/// @param inBodyFilter Filter that is used to check if a character collides with a body.
+	/// @param inShapeFilter Filter that is used to check if a character collides with a subshape.
+	/// @param inAllocator An allocator for temporary allocations. All memory will be freed by the time this function returns.
+	void								Update(float inDeltaTime, Vec3Arg inGravity, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator);
+
+	/// This function will return true if the character has moved into a slope that is too steep (e.g. a vertical wall).
+	/// You would call WalkStairs to attempt to step up stairs.
+	/// @param inLinearVelocity The linear velocity that the player desired. This is used to determine if we're pushing into a step.
+	bool								CanWalkStairs(Vec3Arg inLinearVelocity) const;
+
+	/// When stair walking is needed, you can call the WalkStairs function to cast up, forward and down again to try to find a valid position
+	/// @param inDeltaTime Time step to simulate.
+	/// @param inStepUp The direction and distance to step up (this corresponds to the max step height)
+	/// @param inStepForward The direction and distance to step forward after the step up
+	/// @param inStepForwardTest When running at a high frequency, inStepForward can be very small and it's likely that you hit the side of the stairs on the way down. This could produce a normal that violates the max slope angle. If this happens, we test again using this distance from the up position to see if we find a valid slope.
+	/// @param inStepDownExtra An additional translation that is added when stepping down at the end. Allows you to step further down than up. Set to zero if you don't want this. Should be in the opposite direction of up.
+	/// @param inBroadPhaseLayerFilter Filter that is used to check if the character collides with something in the broadphase.
+	/// @param inObjectLayerFilter Filter that is used to check if a character collides with a layer.
+	/// @param inBodyFilter Filter that is used to check if a character collides with a body.
+	/// @param inShapeFilter Filter that is used to check if a character collides with a subshape.
+	/// @param inAllocator An allocator for temporary allocations. All memory will be freed by the time this function returns.
+	/// @return true if the stair walk was successful
+	bool								WalkStairs(float inDeltaTime, Vec3Arg inStepUp, Vec3Arg inStepForward, Vec3Arg inStepForwardTest, Vec3Arg inStepDownExtra, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator);
+
+	/// This function can be used to artificially keep the character to the floor. Normally when a character is on a small step and starts moving horizontally, the character will
+	/// lose contact with the floor because the initial vertical velocity is zero while the horizontal velocity is quite high. To prevent the character from losing contact with the floor,
+	/// we do an additional collision check downwards and if we find the floor within a certain distance, we project the character onto the floor.
+	/// @param inStepDown Max amount to project the character downwards (if no floor is found within this distance, the function will return false)
+	/// @param inBroadPhaseLayerFilter Filter that is used to check if the character collides with something in the broadphase.
+	/// @param inObjectLayerFilter Filter that is used to check if a character collides with a layer.
+	/// @param inBodyFilter Filter that is used to check if a character collides with a body.
+	/// @param inShapeFilter Filter that is used to check if a character collides with a subshape.
+	/// @param inAllocator An allocator for temporary allocations. All memory will be freed by the time this function returns.
+	/// @return True if the character was successfully projected onto the floor.
+	bool								StickToFloor(Vec3Arg inStepDown, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator);
+
+	/// Settings struct with settings for ExtendedUpdate
+	struct ExtendedUpdateSettings
+	{
+		Vec3							mStickToFloorStepDown { 0, -0.5f, 0 };									///< See StickToFloor inStepDown parameter. Can be zero to turn off.
+		Vec3							mWalkStairsStepUp { 0, 0.4f, 0 };										///< See WalkStairs inStepUp parameter. Can be zero to turn off.
+		float							mWalkStairsMinStepForward { 0.02f };									///< See WalkStairs inStepForward parameter. Note that the parameter only indicates a magnitude, direction is taken from current velocity.
+		float							mWalkStairsStepForwardTest { 0.15f };									///< See WalkStairs inStepForwardTest parameter. Note that the parameter only indicates a magnitude, direction is taken from current velocity.
+		float							mWalkStairsCosAngleForwardContact { Cos(DegreesToRadians(75.0f)) };		///< Cos(angle) where angle is the maximum angle between the ground normal in the horizontal plane and the character forward vector where we're willing to adjust the step forward test towards the contact normal.
+		Vec3							mWalkStairsStepDownExtra { Vec3::sZero() };								///< See WalkStairs inStepDownExtra
+	};
+
+	/// This function combines Update, StickToFloor and WalkStairs. This function serves as an example of how these functions could be combined.
+	/// Before calling, call SetLinearVelocity to update the horizontal/vertical speed of the character, typically this is:
+	/// - When on OnGround and not moving away from ground: velocity = GetGroundVelocity() + horizontal speed as input by player + optional vertical jump velocity + delta time * gravity
+	/// - Else: velocity = current vertical velocity + horizontal speed as input by player + delta time * gravity
+	/// @param inDeltaTime Time step to simulate.
+	/// @param inGravity Gravity vector (m/s^2). This gravity vector is only used when the character is standing on top of another object to apply downward force.
+	/// @param inSettings A structure containing settings for the algorithm.
+	/// @param inBroadPhaseLayerFilter Filter that is used to check if the character collides with something in the broadphase.
+	/// @param inObjectLayerFilter Filter that is used to check if a character collides with a layer.
+	/// @param inBodyFilter Filter that is used to check if a character collides with a body.
+	/// @param inShapeFilter Filter that is used to check if a character collides with a subshape.
+	/// @param inAllocator An allocator for temporary allocations. All memory will be freed by the time this function returns.
+	void								ExtendedUpdate(float inDeltaTime, Vec3Arg inGravity, const ExtendedUpdateSettings &inSettings, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator);
+
+	/// This function can be used after a character has teleported to determine the new contacts with the world.
+	void								RefreshContacts(const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator);
+
+	/// Use the ground body ID to get an updated estimate of the ground velocity. This function can be used if the ground body has moved / changed velocity and you want a new estimate of the ground velocity.
+	/// It will not perform collision detection, so is less accurate than RefreshContacts but a lot faster.
+	void								UpdateGroundVelocity();
+
+	/// Switch the shape of the character (e.g. for stance).
+	/// @param inShape The shape to switch to.
+	/// @param inMaxPenetrationDepth When inMaxPenetrationDepth is not FLT_MAX, it checks if the new shape collides before switching shape. This is the max penetration we're willing to accept after the switch.
+	/// @param inBroadPhaseLayerFilter Filter that is used to check if the character collides with something in the broadphase.
+	/// @param inObjectLayerFilter Filter that is used to check if a character collides with a layer.
+	/// @param inBodyFilter Filter that is used to check if a character collides with a body.
+	/// @param inShapeFilter Filter that is used to check if a character collides with a subshape.
+	/// @param inAllocator An allocator for temporary allocations. All memory will be freed by the time this function returns.
+	/// @return Returns true if the switch succeeded.
+	bool								SetShape(const Shape *inShape, float inMaxPenetrationDepth, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator);
+
+	/// Updates the shape of the inner rigid body. Should be called after a successful call to SetShape.
+	void								SetInnerBodyShape(const Shape *inShape);
+
+	/// Get the transformed shape that represents the volume of the character, can be used for collision checks.
+	TransformedShape					GetTransformedShape() const								{ return TransformedShape(GetCenterOfMassPosition(), mRotation, mShape, mInnerBodyID); }
+
+	/// @brief Get all contacts for the character at a particular location.
+	/// When colliding with another character virtual, this pointer will be provided through CollideShapeCollector::SetUserContext before adding a hit.
+	/// @param inPosition Position to test, note that this position will be corrected for the character padding.
+	/// @param inRotation Rotation at which to test the shape.
+	/// @param inMovementDirection A hint in which direction the character is moving, will be used to calculate a proper normal.
+	/// @param inMaxSeparationDistance How much distance around the character you want to report contacts in (can be 0 to match the character exactly).
+	/// @param inShape Shape to test collision with.
+	/// @param inBaseOffset All hit results will be returned relative to this offset, can be zero to get results in world position, but when you're testing far from the origin you get better precision by picking a position that's closer e.g. GetPosition() since floats are most accurate near the origin
+	/// @param ioCollector Collision collector that receives the collision results.
+	/// @param inBroadPhaseLayerFilter Filter that is used to check if the character collides with something in the broadphase.
+	/// @param inObjectLayerFilter Filter that is used to check if a character collides with a layer.
+	/// @param inBodyFilter Filter that is used to check if a character collides with a body.
+	/// @param inShapeFilter Filter that is used to check if a character collides with a subshape.
+	void								CheckCollision(RVec3Arg inPosition, QuatArg inRotation, Vec3Arg inMovementDirection, float inMaxSeparationDistance, const Shape *inShape, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const;
+
+	// Saving / restoring state for replay
+	virtual void						SaveState(StateRecorder &inStream) const override;
+	virtual void						RestoreState(StateRecorder &inStream) override;
+
+#ifdef JPH_DEBUG_RENDERER
+	static inline bool					sDrawConstraints = false;								///< Draw the current state of the constraints for iteration 0 when creating them
+	static inline bool					sDrawWalkStairs = false;								///< Draw the state of the walk stairs algorithm
+	static inline bool					sDrawStickToFloor = false;								///< Draw the state of the stick to floor algorithm
+#endif
+
+	/// Uniquely identifies a contact between a character and another body or character
+	class ContactKey
+	{
+	public:
+		/// Constructor
+										ContactKey() = default;
+										ContactKey(const ContactKey &inContact) = default;
+										ContactKey(const BodyID &inBodyB, const SubShapeID &inSubShapeID) : mBodyB(inBodyB), mSubShapeIDB(inSubShapeID) { }
+										ContactKey(const CharacterID &inCharacterIDB, const SubShapeID &inSubShapeID) : mCharacterIDB(inCharacterIDB), mSubShapeIDB(inSubShapeID) { }
+		ContactKey &					operator = (const ContactKey &inContact) = default;
+
+		/// Checks if two contacts refer to the same body (or virtual character)
+		inline bool						IsSameBody(const ContactKey &inOther) const				{ return mBodyB == inOther.mBodyB && mCharacterIDB == inOther.mCharacterIDB; }
+
+		/// Equality operator
+		bool							operator == (const ContactKey &inRHS) const
+		{
+			return mBodyB == inRHS.mBodyB && mCharacterIDB == inRHS.mCharacterIDB && mSubShapeIDB == inRHS.mSubShapeIDB;
+		}
+
+		bool							operator != (const ContactKey &inRHS) const
+		{
+			return !(*this == inRHS);
+		}
+
+		/// Hash of this structure
+		uint64							GetHash() const
+		{
+			static_assert(sizeof(BodyID) + sizeof(CharacterID) + sizeof(SubShapeID) == sizeof(ContactKey), "No padding expected");
+			return HashBytes(this, sizeof(ContactKey));
+		}
+
+		// Saving / restoring state for replay
+		void							SaveState(StateRecorder &inStream) const;
+		void							RestoreState(StateRecorder &inStream);
+
+		BodyID							mBodyB;													///< ID of body we're colliding with (if not invalid)
+		CharacterID						mCharacterIDB;											///< Character we're colliding with (if not invalid)
+		SubShapeID						mSubShapeIDB;											///< Sub shape ID of body or character we're colliding with
+	};
+
+	/// Encapsulates a collision contact
+	struct Contact : public ContactKey
+	{
+		// Saving / restoring state for replay
+		void							SaveState(StateRecorder &inStream) const;
+		void							RestoreState(StateRecorder &inStream);
+
+		RVec3							mPosition;												///< Position where the character makes contact
+		Vec3							mLinearVelocity;										///< Velocity of the contact point
+		Vec3							mContactNormal;											///< Contact normal, pointing towards the character
+		Vec3							mSurfaceNormal;											///< Surface normal of the contact
+		float							mDistance;												///< Distance to the contact <= 0 means that it is an actual contact, > 0 means predictive
+		float							mFraction;												///< Fraction along the path where this contact takes place
+		EMotionType						mMotionTypeB;											///< Motion type of B, used to determine the priority of the contact
+		bool							mIsSensorB;												///< If B is a sensor
+		const CharacterVirtual *		mCharacterB = nullptr;									///< Character we're colliding with (if not nullptr). Note that this may be a dangling pointer when accessed through GetActiveContacts(), use mCharacterIDB instead.
+		uint64							mUserData;												///< User data of B
+		const PhysicsMaterial *			mMaterial;												///< Material of B
+		bool							mHadCollision = false;									///< If the character actually collided with the contact (can be false if a predictive contact never becomes a real one)
+		bool							mWasDiscarded = false;									///< If the contact validate callback chose to discard this contact or when the body is a sensor
+		bool							mCanPushCharacter = true;								///< When true, the velocity of the contact point can push the character
+	};
+
+	using TempContactList = Array<Contact, STLTempAllocator<Contact>>;
+	using ContactList = Array<Contact>;
+
+	/// Access to the internal list of contacts that the character has found.
+	/// Note that only contacts that have their mHadCollision flag set are actual contacts.
+	const ContactList &					GetActiveContacts() const								{ return mActiveContacts; }
+
+	/// Check if the character is currently in contact with or has collided with another body in the last operation (e.g. Update or WalkStairs)
+	bool								HasCollidedWith(const BodyID &inBody) const
+	{
+		for (const CharacterVirtual::Contact &c : mActiveContacts)
+			if (c.mHadCollision && c.mBodyB == inBody)
+				return true;
+		return false;
+	}
+
+	/// Check if the character is currently in contact with or has collided with another character in the last time step (e.g. Update or WalkStairs)
+	bool								HasCollidedWith(const CharacterID &inCharacterID) const
+	{
+		for (const CharacterVirtual::Contact &c : mActiveContacts)
+			if (c.mHadCollision && c.mCharacterIDB == inCharacterID)
+				return true;
+		return false;
+	}
+
+	/// Check if the character is currently in contact with or has collided with another character in the last time step (e.g. Update or WalkStairs)
+	bool								HasCollidedWith(const CharacterVirtual *inCharacter) const
+	{
+		return HasCollidedWith(inCharacter->GetID());
+	}
+
+private:
+	// Sorting predicate for making contact order deterministic
+	struct ContactOrderingPredicate
+	{
+		inline bool						operator () (const Contact &inLHS, const Contact &inRHS) const
+		{
+			if (inLHS.mBodyB != inRHS.mBodyB)
+				return inLHS.mBodyB < inRHS.mBodyB;
+
+			if (inLHS.mCharacterIDB != inRHS.mCharacterIDB)
+				return inLHS.mCharacterIDB < inRHS.mCharacterIDB;
+
+			return inLHS.mSubShapeIDB.GetValue() < inRHS.mSubShapeIDB.GetValue();
+		}
+	};
+
+	using IgnoredContactList = Array<ContactKey, STLTempAllocator<ContactKey>>;
+
+	// A constraint that limits the movement of the character
+	struct Constraint
+	{
+		Contact *						mContact;												///< Contact that this constraint was generated from
+		float							mTOI;													///< Calculated time of impact (can be negative if penetrating)
+		float							mProjectedVelocity;										///< Velocity of the contact projected on the contact normal (negative if separating)
+		Vec3							mLinearVelocity;										///< Velocity of the contact (can contain a corrective velocity to resolve penetration)
+		Plane							mPlane;													///< Plane around the origin that describes how far we can displace (from the origin)
+		bool							mIsSteepSlope = false;									///< If this constraint belongs to a steep slope
+	};
+
+	using ConstraintList = Array<Constraint, STLTempAllocator<Constraint>>;
+
+	// Collision collector that collects hits for CollideShape
+	class ContactCollector : public CollideShapeCollector
+	{
+	public:
+										ContactCollector(PhysicsSystem *inSystem, const CharacterVirtual *inCharacter, uint inMaxHits, float inHitReductionCosMaxAngle, Vec3Arg inUp, RVec3Arg inBaseOffset, TempContactList &outContacts) : mBaseOffset(inBaseOffset), mUp(inUp), mSystem(inSystem), mCharacter(inCharacter), mContacts(outContacts), mMaxHits(inMaxHits), mHitReductionCosMaxAngle(inHitReductionCosMaxAngle) { }
+
+		virtual void					SetUserData(uint64 inUserData) override					{ mOtherCharacter = reinterpret_cast<CharacterVirtual *>(inUserData); }
+
+		virtual void					AddHit(const CollideShapeResult &inResult) override;
+
+		RVec3							mBaseOffset;
+		Vec3							mUp;
+		PhysicsSystem *					mSystem;
+		const CharacterVirtual *		mCharacter;
+		CharacterVirtual *				mOtherCharacter = nullptr;
+		TempContactList &				mContacts;
+		uint							mMaxHits;
+		float							mHitReductionCosMaxAngle;
+		bool							mMaxHitsExceeded = false;
+	};
+
+	// A collision collector that collects hits for CastShape
+	class ContactCastCollector : public CastShapeCollector
+	{
+	public:
+										ContactCastCollector(PhysicsSystem *inSystem, const CharacterVirtual *inCharacter, Vec3Arg inDisplacement, Vec3Arg inUp, const IgnoredContactList &inIgnoredContacts, RVec3Arg inBaseOffset, Contact &outContact) : mBaseOffset(inBaseOffset), mDisplacement(inDisplacement), mUp(inUp), mSystem(inSystem), mCharacter(inCharacter), mIgnoredContacts(inIgnoredContacts), mContact(outContact) { }
+
+		virtual void					SetUserData(uint64 inUserData) override					{ mOtherCharacter = reinterpret_cast<CharacterVirtual *>(inUserData); }
+
+		virtual void					AddHit(const ShapeCastResult &inResult) override;
+
+		RVec3							mBaseOffset;
+		Vec3							mDisplacement;
+		Vec3							mUp;
+		PhysicsSystem *					mSystem;
+		const CharacterVirtual *		mCharacter;
+		CharacterVirtual *				mOtherCharacter = nullptr;
+		const IgnoredContactList &		mIgnoredContacts;
+		Contact &						mContact;
+	};
+
+	// Helper function to convert a Jolt collision result into a contact
+	template <class taCollector>
+	inline static void					sFillContactProperties(const CharacterVirtual *inCharacter, Contact &outContact, const Body &inBody, Vec3Arg inUp, RVec3Arg inBaseOffset, const taCollector &inCollector, const CollideShapeResult &inResult);
+	inline static void					sFillCharacterContactProperties(Contact &outContact, const CharacterVirtual *inOtherCharacter, RVec3Arg inBaseOffset, const CollideShapeResult &inResult);
+
+	// Move the shape from ioPosition and try to displace it by inVelocity * inDeltaTime, this will try to slide the shape along the world geometry
+	void								MoveShape(RVec3 &ioPosition, Vec3Arg inVelocity, float inDeltaTime, ContactList *outActiveContacts, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator
+	#ifdef JPH_DEBUG_RENDERER
+		, bool inDrawConstraints = false
+	#endif // JPH_DEBUG_RENDERER
+		);
+
+	// Ask the callback if inContact is a valid contact point
+	bool								ValidateContact(const Contact &inContact) const;
+
+	// Trigger the contact callback for inContact and get the contact settings
+	void								ContactAdded(const Contact &inContact, CharacterContactSettings &ioSettings);
+
+	// Tests the shape for collision around inPosition
+	void								GetContactsAtPosition(RVec3Arg inPosition, Vec3Arg inMovementDirection, const Shape *inShape, TempContactList &outContacts, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const;
+
+	// Remove penetrating contacts with the same body that have conflicting normals, leaving these will make the character mover get stuck
+	void								RemoveConflictingContacts(TempContactList &ioContacts, IgnoredContactList &outIgnoredContacts) const;
+
+	// Convert contacts into constraints. The character is assumed to start at the origin and the constraints are planes around the origin that confine the movement of the character.
+	void								DetermineConstraints(TempContactList &inContacts, float inDeltaTime, ConstraintList &outConstraints) const;
+
+	// Use the constraints to solve the displacement of the character. This will slide the character on the planes around the origin for as far as possible.
+	void								SolveConstraints(Vec3Arg inVelocity, float inDeltaTime, float inTimeRemaining, ConstraintList &ioConstraints, IgnoredContactList &ioIgnoredContacts, float &outTimeSimulated, Vec3 &outDisplacement, TempAllocator &inAllocator
+	#ifdef JPH_DEBUG_RENDERER
+		, bool inDrawConstraints = false
+	#endif // JPH_DEBUG_RENDERER
+		);
+
+	// Get the velocity of a body adjusted by the contact listener
+	void								GetAdjustedBodyVelocity(const Body& inBody, Vec3 &outLinearVelocity, Vec3 &outAngularVelocity) const;
+
+	// Calculate the ground velocity of the character assuming it's standing on an object with specified linear and angular velocity and with specified center of mass.
+	// Note that we don't just take the point velocity because a point on an object with angular velocity traces an arc,
+	// so if you just take point velocity * delta time you get an error that accumulates over time
+	Vec3								CalculateCharacterGroundVelocity(RVec3Arg inCenterOfMass, Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity, float inDeltaTime) const;
+
+	// Handle contact with physics object that we're colliding against
+	bool								HandleContact(Vec3Arg inVelocity, Constraint &ioConstraint, float inDeltaTime);
+
+	// Does a swept test of the shape from inPosition with displacement inDisplacement, returns true if there was a collision
+	bool								GetFirstContactForSweep(RVec3Arg inPosition, Vec3Arg inDisplacement, Contact &outContact, const IgnoredContactList &inIgnoredContacts, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const;
+
+	// Store contacts so that we have proper ground information
+	void								StoreActiveContacts(const TempContactList &inContacts, TempAllocator &inAllocator);
+
+	// This function will determine which contacts are touching the character and will calculate the one that is supporting us
+	void								UpdateSupportingContact(bool inSkipContactVelocityCheck, TempAllocator &inAllocator);
+
+	/// This function can be called after moving the character to a new colliding position
+	void								MoveToContact(RVec3Arg inPosition, const Contact &inContact, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator);
+
+	// This function returns the actual center of mass of the shape, not corrected for the character padding
+	inline RMat44						GetCenterOfMassTransform(RVec3Arg inPosition, QuatArg inRotation, const Shape *inShape) const
+	{
+		return RMat44::sRotationTranslation(inRotation, inPosition).PreTranslated(mShapeOffset + inShape->GetCenterOfMass()).PostTranslated(mCharacterPadding * mUp);
+	}
+
+	// This function returns the position of the inner rigid body
+	inline RVec3						GetInnerBodyPosition() const
+	{
+		return mPosition + (mRotation * mShapeOffset + mCharacterPadding * mUp);
+	}
+
+	// Move the inner rigid body to the current position
+	void								UpdateInnerBodyTransform();
+
+	// ID
+	CharacterID							mID;
+
+	// Our main listener for contacts
+	CharacterContactListener *			mListener = nullptr;
+
+	// Interface to detect collision between characters
+	CharacterVsCharacterCollision *		mCharacterVsCharacterCollision = nullptr;
+
+	// Movement settings
+	EBackFaceMode						mBackFaceMode;											// When colliding with back faces, the character will not be able to move through back facing triangles. Use this if you have triangles that need to collide on both sides.
+	float								mPredictiveContactDistance;								// How far to scan outside of the shape for predictive contacts. A value of 0 will most likely cause the character to get stuck as it cannot properly calculate a sliding direction anymore. A value that's too high will cause ghost collisions.
+	uint								mMaxCollisionIterations;								// Max amount of collision loops
+	uint								mMaxConstraintIterations;								// How often to try stepping in the constraint solving
+	float								mMinTimeRemaining;										// Early out condition: If this much time is left to simulate we are done
+	float								mCollisionTolerance;									// How far we're willing to penetrate geometry
+	float								mCharacterPadding;										// How far we try to stay away from the geometry, this ensures that the sweep will hit as little as possible lowering the collision cost and reducing the risk of getting stuck
+	uint								mMaxNumHits;											// Max num hits to collect in order to avoid excess of contact points collection
+	float								mHitReductionCosMaxAngle;								// Cos(angle) where angle is the maximum angle between two hits contact normals that are allowed to be merged during hit reduction. Default is around 2.5 degrees. Set to -1 to turn off.
+	float								mPenetrationRecoverySpeed;								// This value governs how fast a penetration will be resolved, 0 = nothing is resolved, 1 = everything in one update
+	bool								mEnhancedInternalEdgeRemoval;							// Set to indicate that extra effort should be made to try to remove ghost contacts (collisions with internal edges of a mesh). This is more expensive but makes bodies move smoother over a mesh with convex edges.
+
+	// Character mass (kg)
+	float								mMass;
+
+	// Maximum force with which the character can push other bodies (N)
+	float								mMaxStrength;
+
+	// An extra offset applied to the shape in local space. This allows applying an extra offset to the shape in local space.
+	Vec3								mShapeOffset = Vec3::sZero();
+
+	// Current position (of the base, not the center of mass)
+	RVec3								mPosition = RVec3::sZero();
+
+	// Current rotation (of the base, not of the center of mass)
+	Quat								mRotation = Quat::sIdentity();
+
+	// Current linear velocity
+	Vec3								mLinearVelocity = Vec3::sZero();
+
+	// List of contacts that were active in the last frame
+	ContactList							mActiveContacts;
+
+	// Remembers how often we called StartTrackingContactChanges
+	int									mTrackingContactChanges = 0;
+
+	// View from a contact listener perspective on which contacts have been added/removed
+	struct ListenerContactValue
+	{
+										ListenerContactValue() = default;
+		explicit						ListenerContactValue(const CharacterContactSettings &inSettings) : mSettings(inSettings) { }
+
+		CharacterContactSettings		mSettings;
+		int								mCount = 0;
+	};
+
+	using ListenerContacts = UnorderedMap<ContactKey, ListenerContactValue>;
+	ListenerContacts					mListenerContacts;
+
+	// Remembers the delta time of the last update
+	float								mLastDeltaTime = 1.0f / 60.0f;
+
+	// Remember if we exceeded the maximum number of hits and had to remove similar contacts
+	mutable bool						mMaxHitsExceeded = false;
+
+	// User data, can be used for anything by the application
+	uint64								mUserData = 0;
+
+	// The inner rigid body that proxies the character in the world
+	BodyID								mInnerBodyID;
+};
+
+JPH_NAMESPACE_END

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

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

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

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

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

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

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

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

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

@@ -0,0 +1,16 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Collision/BroadPhase/BroadPhase.h>
+
+JPH_NAMESPACE_BEGIN
+
+void BroadPhase::Init(BodyManager *inBodyManager, const BroadPhaseLayerInterface &inLayerInterface)
+{
+	mBodyManager = inBodyManager;
+}
+
+JPH_NAMESPACE_END

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

@@ -0,0 +1,112 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/BroadPhase/BroadPhaseQuery.h>
+#include <Jolt/Physics/Collision/BroadPhase/BroadPhaseLayer.h>
+
+JPH_NAMESPACE_BEGIN
+
+// Shorthand function to ifdef out code if broadphase stats tracking is off
+#ifdef JPH_TRACK_BROADPHASE_STATS
+	#define JPH_IF_TRACK_BROADPHASE_STATS(...) __VA_ARGS__
+#else
+	#define JPH_IF_TRACK_BROADPHASE_STATS(...)
+#endif // JPH_TRACK_BROADPHASE_STATS
+
+class BodyManager;
+struct BodyPair;
+
+using BodyPairCollector = CollisionCollector<BodyPair, CollisionCollectorTraitsCollideShape>;
+
+/// Used to do coarse collision detection operations to quickly prune out bodies that will not collide.
+class JPH_EXPORT BroadPhase : public BroadPhaseQuery
+{
+public:
+	/// Initialize the broadphase.
+	/// @param inBodyManager The body manager singleton
+	/// @param inLayerInterface Interface that maps object layers to broadphase layers.
+	/// Note that the broadphase takes a pointer to the data inside inObjectToBroadPhaseLayer so this object should remain static.
+	virtual void		Init(BodyManager *inBodyManager, const BroadPhaseLayerInterface &inLayerInterface);
+
+	/// Should be called after many objects have been inserted to make the broadphase more efficient, usually done on startup only
+	virtual void		Optimize()															{ /* Optionally overridden by implementation */ }
+
+	/// Must be called just before updating the broadphase when none of the body mutexes are locked
+	virtual void		FrameSync()															{ /* Optionally overridden by implementation */ }
+
+	/// Must be called before UpdatePrepare to prevent modifications from being made to the tree
+	virtual void		LockModifications()													{ /* Optionally overridden by implementation */ }
+
+	/// Context used during broadphase update
+	struct UpdateState { void *mData[4]; };
+
+	/// Update the broadphase, needs to be called frequently to update the internal state when bodies have been modified.
+	/// The UpdatePrepare() function can run in a background thread without influencing the broadphase
+	virtual	UpdateState	UpdatePrepare()														{ return UpdateState(); }
+
+	/// Finalizing the update will quickly apply the changes
+	virtual void		UpdateFinalize([[maybe_unused]] const UpdateState &inUpdateState)	{ /* Optionally overridden by implementation */ }
+
+	/// Must be called after UpdateFinalize to allow modifications to the broadphase
+	virtual void		UnlockModifications()												{ /* Optionally overridden by implementation */ }
+
+	/// Handle used during adding bodies to the broadphase
+	using AddState = void *;
+
+	/// Prepare adding inNumber bodies at ioBodies to the broadphase, returns a handle that should be used in AddBodiesFinalize/Abort.
+	/// This can be done on a background thread without influencing the broadphase.
+	/// ioBodies may be shuffled around by this function and should be kept that way until AddBodiesFinalize/Abort is called.
+	virtual AddState	AddBodiesPrepare([[maybe_unused]] BodyID *ioBodies, [[maybe_unused]] int inNumber) { return nullptr; } // By default the broadphase doesn't support this
+
+	/// Finalize adding bodies to the broadphase, supply the return value of AddBodiesPrepare in inAddState.
+	/// Please ensure that the ioBodies array passed to AddBodiesPrepare is unmodified and passed again to this function.
+	virtual void		AddBodiesFinalize(BodyID *ioBodies, int inNumber, AddState inAddState) = 0;
+
+	/// Abort adding bodies to the broadphase, supply the return value of AddBodiesPrepare in inAddState.
+	/// This can be done on a background thread without influencing the broadphase.
+	/// Please ensure that the ioBodies array passed to AddBodiesPrepare is unmodified and passed again to this function.
+	virtual void		AddBodiesAbort([[maybe_unused]] BodyID *ioBodies, [[maybe_unused]] int inNumber, [[maybe_unused]] AddState inAddState)	{ /* By default nothing needs to be done */ }
+
+	/// Remove inNumber bodies in ioBodies from the broadphase.
+	/// ioBodies may be shuffled around by this function.
+	virtual void		RemoveBodies(BodyID *ioBodies, int inNumber) = 0;
+
+	/// Call whenever the aabb of a body changes (can change order of ioBodies array)
+	/// inTakeLock should be false if we're between LockModifications/UnlockModifications, in which case care needs to be taken to not call this between UpdatePrepare/UpdateFinalize
+	virtual void		NotifyBodiesAABBChanged(BodyID *ioBodies, int inNumber, bool inTakeLock = true) = 0;
+
+	/// Call whenever the layer (and optionally the aabb as well) of a body changes (can change order of ioBodies array)
+	virtual void		NotifyBodiesLayerChanged(BodyID *ioBodies, int inNumber) = 0;
+
+	/// Find all colliding pairs between dynamic bodies
+	/// Note that this function is very specifically tailored for the PhysicsSystem::Update function, hence it is not part of the BroadPhaseQuery interface.
+	/// One of the assumptions it can make is that no locking is needed during the query as it will only be called during a very particular part of the update.
+	/// @param ioActiveBodies is a list of bodies for which we need to find colliding pairs (this function can change the order of the ioActiveBodies array). This can be a subset of the set of active bodies in the system.
+	/// @param inNumActiveBodies is the size of the ioActiveBodies array.
+	/// @param inSpeculativeContactDistance Distance at which speculative contact points will be created.
+	/// @param inObjectVsBroadPhaseLayerFilter is the filter that determines if an object can collide with a broadphase layer.
+	/// @param inObjectLayerPairFilter is the filter that determines if two objects can collide.
+	/// @param ioPairCollector receives callbacks for every body pair found.
+	virtual void		FindCollidingPairs(BodyID *ioActiveBodies, int inNumActiveBodies, float inSpeculativeContactDistance, const ObjectVsBroadPhaseLayerFilter &inObjectVsBroadPhaseLayerFilter, const ObjectLayerPairFilter &inObjectLayerPairFilter, BodyPairCollector &ioPairCollector) const = 0;
+
+	/// Same as BroadPhaseQuery::CastAABox but can be implemented in a way to take no broad phase locks.
+	virtual void		CastAABoxNoLock(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const = 0;
+
+	/// Get the bounding box of all objects in the broadphase
+	virtual AABox		GetBounds() const = 0;
+
+#ifdef JPH_TRACK_BROADPHASE_STATS
+	/// Trace the collected broadphase stats in CSV form.
+	/// This report can be used to judge and tweak the efficiency of the broadphase.
+	virtual void		ReportStats()														{ /* Can be implemented by derived classes */ }
+#endif // JPH_TRACK_BROADPHASE_STATS
+
+protected:
+	/// Link to the body manager that manages the bodies in this broadphase
+	BodyManager *		mBodyManager = nullptr;
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseBruteForce.cpp

@@ -0,0 +1,313 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+#include <Jolt/Physics/Collision/BroadPhase/BroadPhaseBruteForce.h>
+#include <Jolt/Physics/Collision/RayCast.h>
+#include <Jolt/Physics/Collision/AABoxCast.h>
+#include <Jolt/Physics/Collision/CastResult.h>
+#include <Jolt/Physics/Body/BodyManager.h>
+#include <Jolt/Physics/Body/BodyPair.h>
+#include <Jolt/Geometry/RayAABox.h>
+#include <Jolt/Geometry/OrientedBox.h>
+#include <Jolt/Core/QuickSort.h>
+
+JPH_NAMESPACE_BEGIN
+
+void BroadPhaseBruteForce::AddBodiesFinalize(BodyID *ioBodies, int inNumber, AddState inAddState)
+{
+	lock_guard lock(mMutex);
+
+	BodyVector &bodies = mBodyManager->GetBodies();
+
+	// Allocate space
+	uint32 idx = (uint32)mBodyIDs.size();
+	mBodyIDs.resize(idx + inNumber);
+
+	// Add bodies
+	for (const BodyID *b = ioBodies, *b_end = ioBodies + inNumber; b < b_end; ++b)
+	{
+		Body &body = *bodies[b->GetIndex()];
+
+		// Validate that body ID is consistent with array index
+		JPH_ASSERT(body.GetID() == *b);
+		JPH_ASSERT(!body.IsInBroadPhase());
+
+		// Add it to the list
+		mBodyIDs[idx] = body.GetID();
+		++idx;
+
+		// Indicate body is in the broadphase
+		body.SetInBroadPhaseInternal(true);
+	}
+
+	// Resort
+	QuickSort(mBodyIDs.begin(), mBodyIDs.end());
+}
+
+void BroadPhaseBruteForce::RemoveBodies(BodyID *ioBodies, int inNumber)
+{
+	lock_guard lock(mMutex);
+
+	BodyVector &bodies = mBodyManager->GetBodies();
+
+	JPH_ASSERT((int)mBodyIDs.size() >= inNumber);
+
+	// Remove bodies
+	for (const BodyID *b = ioBodies, *b_end = ioBodies + inNumber; b < b_end; ++b)
+	{
+		Body &body = *bodies[b->GetIndex()];
+
+		// Validate that body ID is consistent with array index
+		JPH_ASSERT(body.GetID() == *b);
+		JPH_ASSERT(body.IsInBroadPhase());
+
+		// Find body id
+		Array<BodyID>::const_iterator it = std::lower_bound(mBodyIDs.begin(), mBodyIDs.end(), body.GetID());
+		JPH_ASSERT(it != mBodyIDs.end());
+
+		// Remove element
+		mBodyIDs.erase(it);
+
+		// Indicate body is no longer in the broadphase
+		body.SetInBroadPhaseInternal(false);
+	}
+}
+
+void BroadPhaseBruteForce::NotifyBodiesAABBChanged(BodyID *ioBodies, int inNumber, bool inTakeLock)
+{
+	// Do nothing, we directly reference the body
+}
+
+void BroadPhaseBruteForce::NotifyBodiesLayerChanged(BodyID * ioBodies, int inNumber)
+{
+	// Do nothing, we directly reference the body
+}
+
+void BroadPhaseBruteForce::CastRay(const RayCast &inRay, RayCastBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const
+{
+	shared_lock lock(mMutex);
+
+	// Load ray
+	Vec3 origin(inRay.mOrigin);
+	RayInvDirection inv_direction(inRay.mDirection);
+
+	// For all bodies
+	float early_out_fraction = ioCollector.GetEarlyOutFraction();
+	for (BodyID b : mBodyIDs)
+	{
+		const Body &body = mBodyManager->GetBody(b);
+
+		// Test layer
+		if (inObjectLayerFilter.ShouldCollide(body.GetObjectLayer()))
+		{
+			// Test intersection with ray
+			const AABox &bounds = body.GetWorldSpaceBounds();
+			float fraction = RayAABox(origin, inv_direction, bounds.mMin, bounds.mMax);
+			if (fraction < early_out_fraction)
+			{
+				// Store hit
+				BroadPhaseCastResult result { b, fraction };
+				ioCollector.AddHit(result);
+				if (ioCollector.ShouldEarlyOut())
+					break;
+				early_out_fraction = ioCollector.GetEarlyOutFraction();
+			}
+		}
+	}
+}
+
+void BroadPhaseBruteForce::CollideAABox(const AABox &inBox, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const
+{
+	shared_lock lock(mMutex);
+
+	// For all bodies
+	for (BodyID b : mBodyIDs)
+	{
+		const Body &body = mBodyManager->GetBody(b);
+
+		// Test layer
+		if (inObjectLayerFilter.ShouldCollide(body.GetObjectLayer()))
+		{
+			// Test intersection with box
+			const AABox &bounds = body.GetWorldSpaceBounds();
+			if (bounds.Overlaps(inBox))
+			{
+				// Store hit
+				ioCollector.AddHit(b);
+				if (ioCollector.ShouldEarlyOut())
+					break;
+			}
+		}
+	}
+}
+
+void BroadPhaseBruteForce::CollideSphere(Vec3Arg inCenter, float inRadius, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const
+{
+	shared_lock lock(mMutex);
+
+	float radius_sq = Square(inRadius);
+
+	// For all bodies
+	for (BodyID b : mBodyIDs)
+	{
+		const Body &body = mBodyManager->GetBody(b);
+
+		// Test layer
+		if (inObjectLayerFilter.ShouldCollide(body.GetObjectLayer()))
+		{
+			// Test intersection with box
+			const AABox &bounds = body.GetWorldSpaceBounds();
+			if (bounds.GetSqDistanceTo(inCenter) <= radius_sq)
+			{
+				// Store hit
+				ioCollector.AddHit(b);
+				if (ioCollector.ShouldEarlyOut())
+					break;
+			}
+		}
+	}
+}
+
+void BroadPhaseBruteForce::CollidePoint(Vec3Arg inPoint, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const
+{
+	shared_lock lock(mMutex);
+
+	// For all bodies
+	for (BodyID b : mBodyIDs)
+	{
+		const Body &body = mBodyManager->GetBody(b);
+
+		// Test layer
+		if (inObjectLayerFilter.ShouldCollide(body.GetObjectLayer()))
+		{
+			// Test intersection with box
+			const AABox &bounds = body.GetWorldSpaceBounds();
+			if (bounds.Contains(inPoint))
+			{
+				// Store hit
+				ioCollector.AddHit(b);
+				if (ioCollector.ShouldEarlyOut())
+					break;
+			}
+		}
+	}
+}
+
+void BroadPhaseBruteForce::CollideOrientedBox(const OrientedBox &inBox, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const
+{
+	shared_lock lock(mMutex);
+
+	// For all bodies
+	for (BodyID b : mBodyIDs)
+	{
+		const Body &body = mBodyManager->GetBody(b);
+
+		// Test layer
+		if (inObjectLayerFilter.ShouldCollide(body.GetObjectLayer()))
+		{
+			// Test intersection with box
+			const AABox &bounds = body.GetWorldSpaceBounds();
+			if (inBox.Overlaps(bounds))
+			{
+				// Store hit
+				ioCollector.AddHit(b);
+				if (ioCollector.ShouldEarlyOut())
+					break;
+			}
+		}
+	}
+}
+
+void BroadPhaseBruteForce::CastAABoxNoLock(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const
+{
+	shared_lock lock(mMutex);
+
+	// Load box
+	Vec3 origin(inBox.mBox.GetCenter());
+	Vec3 extent(inBox.mBox.GetExtent());
+	RayInvDirection inv_direction(inBox.mDirection);
+
+	// For all bodies
+	float early_out_fraction = ioCollector.GetPositiveEarlyOutFraction();
+	for (BodyID b : mBodyIDs)
+	{
+		const Body &body = mBodyManager->GetBody(b);
+
+		// Test layer
+		if (inObjectLayerFilter.ShouldCollide(body.GetObjectLayer()))
+		{
+			// Test intersection with ray
+			const AABox &bounds = body.GetWorldSpaceBounds();
+			float fraction = RayAABox(origin, inv_direction, bounds.mMin - extent, bounds.mMax + extent);
+			if (fraction < early_out_fraction)
+			{
+				// Store hit
+				BroadPhaseCastResult result { b, fraction };
+				ioCollector.AddHit(result);
+				if (ioCollector.ShouldEarlyOut())
+					break;
+				early_out_fraction = ioCollector.GetPositiveEarlyOutFraction();
+			}
+		}
+	}
+}
+
+void BroadPhaseBruteForce::CastAABox(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const
+{
+	CastAABoxNoLock(inBox, ioCollector, inBroadPhaseLayerFilter, inObjectLayerFilter);
+}
+
+void BroadPhaseBruteForce::FindCollidingPairs(BodyID *ioActiveBodies, int inNumActiveBodies, float inSpeculativeContactDistance, const ObjectVsBroadPhaseLayerFilter &inObjectVsBroadPhaseLayerFilter, const ObjectLayerPairFilter &inObjectLayerPairFilter, BodyPairCollector &ioPairCollector) const
+{
+	shared_lock lock(mMutex);
+
+	// Loop through all active bodies
+	size_t num_bodies = mBodyIDs.size();
+	for (int b1 = 0; b1 < inNumActiveBodies; ++b1)
+	{
+		BodyID b1_id = ioActiveBodies[b1];
+		const Body &body1 = mBodyManager->GetBody(b1_id);
+		const ObjectLayer layer1 = body1.GetObjectLayer();
+
+		// Expand the bounding box by the speculative contact distance
+		AABox bounds1 = body1.GetWorldSpaceBounds();
+		bounds1.ExpandBy(Vec3::sReplicate(inSpeculativeContactDistance));
+
+		// For all other bodies
+		for (size_t b2 = 0; b2 < num_bodies; ++b2)
+		{
+			// Check if bodies can collide
+			BodyID b2_id = mBodyIDs[b2];
+			const Body &body2 = mBodyManager->GetBody(b2_id);
+			if (!Body::sFindCollidingPairsCanCollide(body1, body2))
+				continue;
+
+			// Check if layers can collide
+			const ObjectLayer layer2 = body2.GetObjectLayer();
+			if (!inObjectLayerPairFilter.ShouldCollide(layer1, layer2))
+				continue;
+
+			// Check if bounds overlap
+			const AABox &bounds2 = body2.GetWorldSpaceBounds();
+			if (!bounds1.Overlaps(bounds2))
+				continue;
+
+			// Store overlapping pair
+			ioPairCollector.AddHit({ b1_id, b2_id });
+		}
+	}
+}
+
+AABox BroadPhaseBruteForce::GetBounds() const
+{
+	shared_lock lock(mMutex);
+
+	AABox bounds;
+	for (BodyID b : mBodyIDs)
+		bounds.Encapsulate(mBodyManager->GetBody(b).GetWorldSpaceBounds());
+	return bounds;
+}
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseBruteForce.h

@@ -0,0 +1,38 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/BroadPhase/BroadPhase.h>
+#include <Jolt/Core/Mutex.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Test BroadPhase implementation that does not do anything to speed up the operations. Can be used as a reference implementation.
+class JPH_EXPORT BroadPhaseBruteForce final : public BroadPhase
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	// Implementing interface of BroadPhase (see BroadPhase for documentation)
+	virtual void		AddBodiesFinalize(BodyID *ioBodies, int inNumber, AddState inAddState) override;
+	virtual void		RemoveBodies(BodyID *ioBodies, int inNumber) override;
+	virtual void		NotifyBodiesAABBChanged(BodyID *ioBodies, int inNumber, bool inTakeLock) override;
+	virtual void		NotifyBodiesLayerChanged(BodyID *ioBodies, int inNumber) override;
+	virtual void		CastRay(const RayCast &inRay, RayCastBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override;
+	virtual void		CollideAABox(const AABox &inBox, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override;
+	virtual void		CollideSphere(Vec3Arg inCenter, float inRadius, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override;
+	virtual void		CollidePoint(Vec3Arg inPoint, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override;
+	virtual void		CollideOrientedBox(const OrientedBox &inBox, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override;
+	virtual void		CastAABoxNoLock(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override;
+	virtual void		CastAABox(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override;
+	virtual void		FindCollidingPairs(BodyID *ioActiveBodies, int inNumActiveBodies, float inSpeculativeContactDistance, const ObjectVsBroadPhaseLayerFilter &inObjectVsBroadPhaseLayerFilter, const ObjectLayerPairFilter &inObjectLayerPairFilter, BodyPairCollector &ioPairCollector) const override;
+	virtual AABox		GetBounds() const override;
+
+private:
+	Array<BodyID>		mBodyIDs;
+	mutable SharedMutex	mMutex;
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseLayer.h

@@ -0,0 +1,148 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/NonCopyable.h>
+#include <Jolt/Physics/Collision/ObjectLayer.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// An object layer can be mapped to a broadphase layer. Objects with the same broadphase layer will end up in the same sub structure (usually a tree) of the broadphase.
+/// When there are many layers, this reduces the total amount of sub structures the broad phase needs to manage. Usually you want objects that don't collide with each other
+/// in different broad phase layers, but there could be exceptions if objects layers only contain a minor amount of objects so it is not beneficial to give each layer its
+/// own sub structure in the broadphase.
+/// Note: This class requires explicit casting from and to Type to avoid confusion with ObjectLayer
+class BroadPhaseLayer
+{
+public:
+	using Type = uint8;
+
+	JPH_INLINE						BroadPhaseLayer() = default;
+	JPH_INLINE explicit constexpr	BroadPhaseLayer(Type inValue) : mValue(inValue) { }
+	JPH_INLINE constexpr			BroadPhaseLayer(const BroadPhaseLayer &) = default;
+	JPH_INLINE BroadPhaseLayer &	operator = (const BroadPhaseLayer &) = default;
+
+	JPH_INLINE constexpr bool		operator == (const BroadPhaseLayer &inRHS) const
+	{
+		return mValue == inRHS.mValue;
+	}
+
+	JPH_INLINE constexpr bool		operator != (const BroadPhaseLayer &inRHS) const
+	{
+		return mValue != inRHS.mValue;
+	}
+
+	JPH_INLINE constexpr bool		operator < (const BroadPhaseLayer &inRHS) const
+	{
+		return mValue < inRHS.mValue;
+	}
+
+	JPH_INLINE explicit constexpr	operator Type() const
+	{
+		return mValue;
+	}
+
+	JPH_INLINE Type					GetValue() const
+	{
+		return mValue;
+	}
+
+private:
+	Type							mValue;
+};
+
+/// Constant value used to indicate an invalid broad phase layer
+static constexpr BroadPhaseLayer cBroadPhaseLayerInvalid(0xff);
+
+/// Interface that the application should implement to allow mapping object layers to broadphase layers
+class JPH_EXPORT BroadPhaseLayerInterface : public NonCopyable
+{
+public:
+	/// Destructor
+	virtual							~BroadPhaseLayerInterface() = default;
+
+	/// Return the number of broadphase layers there are
+	virtual uint					GetNumBroadPhaseLayers() const = 0;
+
+	/// Convert an object layer to the corresponding broadphase layer
+	virtual BroadPhaseLayer			GetBroadPhaseLayer(ObjectLayer inLayer) const = 0;
+
+#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED)
+	/// Get the user readable name of a broadphase layer (debugging purposes)
+	virtual const char *			GetBroadPhaseLayerName(BroadPhaseLayer inLayer) const = 0;
+#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED
+};
+
+/// Class to test if an object can collide with a broadphase layer. Used while finding collision pairs.
+class JPH_EXPORT ObjectVsBroadPhaseLayerFilter : public NonCopyable
+{
+public:
+	/// Destructor
+	virtual							~ObjectVsBroadPhaseLayerFilter() = default;
+
+	/// Returns true if an object layer should collide with a broadphase layer
+	virtual bool					ShouldCollide([[maybe_unused]] ObjectLayer inLayer1, [[maybe_unused]] BroadPhaseLayer inLayer2) const
+	{
+		return true;
+	}
+};
+
+/// Filter class for broadphase layers
+class JPH_EXPORT BroadPhaseLayerFilter : public NonCopyable
+{
+public:
+	/// Destructor
+	virtual							~BroadPhaseLayerFilter() = default;
+
+	/// Function to filter out broadphase layers when doing collision query test (return true to allow testing against objects with this layer)
+	virtual bool					ShouldCollide([[maybe_unused]] BroadPhaseLayer inLayer) const
+	{
+		return true;
+	}
+};
+
+/// Default filter class that uses the pair filter in combination with a specified layer to filter layers
+class JPH_EXPORT DefaultBroadPhaseLayerFilter : public BroadPhaseLayerFilter
+{
+public:
+	/// Constructor
+									DefaultBroadPhaseLayerFilter(const ObjectVsBroadPhaseLayerFilter &inObjectVsBroadPhaseLayerFilter, ObjectLayer inLayer) :
+		mObjectVsBroadPhaseLayerFilter(inObjectVsBroadPhaseLayerFilter),
+		mLayer(inLayer)
+	{
+	}
+
+	// See BroadPhaseLayerFilter::ShouldCollide
+	virtual bool					ShouldCollide(BroadPhaseLayer inLayer) const override
+	{
+		return mObjectVsBroadPhaseLayerFilter.ShouldCollide(mLayer, inLayer);
+	}
+
+private:
+	const ObjectVsBroadPhaseLayerFilter &mObjectVsBroadPhaseLayerFilter;
+	ObjectLayer						mLayer;
+};
+
+/// Allows objects from a specific broad phase layer only
+class JPH_EXPORT SpecifiedBroadPhaseLayerFilter : public BroadPhaseLayerFilter
+{
+public:
+	/// Constructor
+	explicit						SpecifiedBroadPhaseLayerFilter(BroadPhaseLayer inLayer) :
+		mLayer(inLayer)
+	{
+	}
+
+	// See BroadPhaseLayerFilter::ShouldCollide
+	virtual bool					ShouldCollide(BroadPhaseLayer inLayer) const override
+	{
+		return mLayer == inLayer;
+	}
+
+private:
+	BroadPhaseLayer					mLayer;
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseLayerInterfaceMask.h

@@ -0,0 +1,92 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/BroadPhase/BroadPhaseLayer.h>
+#include <Jolt/Physics/Collision/ObjectLayerPairFilterMask.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// BroadPhaseLayerInterface implementation.
+/// This defines a mapping between object and broadphase layers.
+/// This implementation works together with ObjectLayerPairFilterMask and ObjectVsBroadPhaseLayerFilterMask.
+/// A broadphase layer is suitable for an object if its group & inGroupsToInclude is not zero and its group & inGroupsToExclude is zero.
+/// The broadphase layers are iterated from lowest to highest value and the first one that matches is taken. If none match then it takes the last layer.
+class BroadPhaseLayerInterfaceMask : public BroadPhaseLayerInterface
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	explicit				BroadPhaseLayerInterfaceMask(uint inNumBroadPhaseLayers)
+	{
+		JPH_ASSERT(inNumBroadPhaseLayers > 0);
+		mMapping.resize(inNumBroadPhaseLayers);
+
+#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED)
+		mBroadPhaseLayerNames.resize(inNumBroadPhaseLayers, "Undefined");
+#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED
+	}
+
+	// Configures a broadphase layer.
+	void					ConfigureLayer(BroadPhaseLayer inBroadPhaseLayer, uint32 inGroupsToInclude, uint32 inGroupsToExclude)
+	{
+		JPH_ASSERT((BroadPhaseLayer::Type)inBroadPhaseLayer < (uint)mMapping.size());
+		Mapping &m = mMapping[(BroadPhaseLayer::Type)inBroadPhaseLayer];
+		m.mGroupsToInclude = inGroupsToInclude;
+		m.mGroupsToExclude = inGroupsToExclude;
+	}
+
+	virtual uint			GetNumBroadPhaseLayers() const override
+	{
+		return (uint)mMapping.size();
+	}
+
+	virtual BroadPhaseLayer	GetBroadPhaseLayer(ObjectLayer inLayer) const override
+	{
+		// Try to find the first broadphase layer that matches
+		uint32 group = ObjectLayerPairFilterMask::sGetGroup(inLayer);
+		for (const Mapping &m : mMapping)
+			if ((group & m.mGroupsToInclude) != 0 && (group & m.mGroupsToExclude) == 0)
+				return BroadPhaseLayer(BroadPhaseLayer::Type(&m - mMapping.data()));
+
+		// Fall back to the last broadphase layer
+		return BroadPhaseLayer(BroadPhaseLayer::Type(mMapping.size() - 1));
+	}
+
+	/// Returns true if an object layer should collide with a broadphase layer, this function is being called from ObjectVsBroadPhaseLayerFilterMask
+	inline bool				ShouldCollide(ObjectLayer inLayer1, BroadPhaseLayer inLayer2) const
+	{
+		uint32 mask = ObjectLayerPairFilterMask::sGetMask(inLayer1);
+		const Mapping &m = mMapping[(BroadPhaseLayer::Type)inLayer2];
+		return &m == &mMapping.back() // Last layer may collide with anything
+			|| (m.mGroupsToInclude & mask) != 0; // Mask allows it to collide with objects that could reside in this layer
+	}
+
+#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED)
+	void					SetBroadPhaseLayerName(BroadPhaseLayer inLayer, const char *inName)
+	{
+		mBroadPhaseLayerNames[(BroadPhaseLayer::Type)inLayer] = inName;
+	}
+
+	virtual const char *	GetBroadPhaseLayerName(BroadPhaseLayer inLayer) const override
+	{
+		return mBroadPhaseLayerNames[(BroadPhaseLayer::Type)inLayer];
+	}
+#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED
+
+private:
+	struct Mapping
+	{
+		uint32				mGroupsToInclude = 0;
+		uint32				mGroupsToExclude = ~uint32(0);
+	};
+	Array<Mapping>			mMapping;
+
+#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED)
+	Array<const char *>		mBroadPhaseLayerNames;
+#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseLayerInterfaceTable.h

@@ -0,0 +1,64 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/BroadPhase/BroadPhaseLayer.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// BroadPhaseLayerInterface implementation.
+/// This defines a mapping between object and broadphase layers.
+/// This implementation uses a simple table
+class BroadPhaseLayerInterfaceTable : public BroadPhaseLayerInterface
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+							BroadPhaseLayerInterfaceTable(uint inNumObjectLayers, uint inNumBroadPhaseLayers) :
+		mNumBroadPhaseLayers(inNumBroadPhaseLayers)
+	{
+		mObjectToBroadPhase.resize(inNumObjectLayers, BroadPhaseLayer(0));
+#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED)
+		mBroadPhaseLayerNames.resize(inNumBroadPhaseLayers, "Undefined");
+#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED
+	}
+
+	void					MapObjectToBroadPhaseLayer(ObjectLayer inObjectLayer, BroadPhaseLayer inBroadPhaseLayer)
+	{
+		JPH_ASSERT((BroadPhaseLayer::Type)inBroadPhaseLayer < mNumBroadPhaseLayers);
+		mObjectToBroadPhase[inObjectLayer] = inBroadPhaseLayer;
+	}
+
+	virtual uint			GetNumBroadPhaseLayers() const override
+	{
+		return mNumBroadPhaseLayers;
+	}
+
+	virtual BroadPhaseLayer	GetBroadPhaseLayer(ObjectLayer inLayer) const override
+	{
+		return mObjectToBroadPhase[inLayer];
+	}
+
+#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED)
+	void					SetBroadPhaseLayerName(BroadPhaseLayer inLayer, const char *inName)
+	{
+		mBroadPhaseLayerNames[(BroadPhaseLayer::Type)inLayer] = inName;
+	}
+
+	virtual const char *	GetBroadPhaseLayerName(BroadPhaseLayer inLayer) const override
+	{
+		return mBroadPhaseLayerNames[(BroadPhaseLayer::Type)inLayer];
+	}
+#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED
+
+private:
+	uint					mNumBroadPhaseLayers;
+	Array<BroadPhaseLayer>	mObjectToBroadPhase;
+#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED)
+	Array<const char *>		mBroadPhaseLayerNames;
+#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseQuadTree.cpp

@@ -0,0 +1,629 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+#include <Jolt/Physics/Collision/BroadPhase/BroadPhaseQuadTree.h>
+#include <Jolt/Physics/Collision/RayCast.h>
+#include <Jolt/Physics/Collision/AABoxCast.h>
+#include <Jolt/Physics/Collision/CastResult.h>
+#include <Jolt/Core/QuickSort.h>
+
+JPH_NAMESPACE_BEGIN
+
+BroadPhaseQuadTree::~BroadPhaseQuadTree()
+{
+	delete [] mLayers;
+}
+
+void BroadPhaseQuadTree::Init(BodyManager *inBodyManager, const BroadPhaseLayerInterface &inLayerInterface)
+{
+	BroadPhase::Init(inBodyManager, inLayerInterface);
+
+	// Store input parameters
+	mBroadPhaseLayerInterface = &inLayerInterface;
+	mNumLayers = inLayerInterface.GetNumBroadPhaseLayers();
+	JPH_ASSERT(mNumLayers < (BroadPhaseLayer::Type)cBroadPhaseLayerInvalid);
+
+#ifdef JPH_ENABLE_ASSERTS
+	// Store lock context
+	mLockContext = inBodyManager;
+#endif // JPH_ENABLE_ASSERTS
+
+	// Store max bodies
+	mMaxBodies = inBodyManager->GetMaxBodies();
+
+	// Initialize tracking data
+	mTracking.resize(mMaxBodies);
+
+	// Init allocator
+	// Estimate the amount of nodes we're going to need
+	uint32 num_leaves = (uint32)(mMaxBodies + 1) / 2; // Assume 50% fill
+	uint32 num_leaves_plus_internal_nodes = num_leaves + (num_leaves + 2) / 3; // = Sum(num_leaves * 4^-i) with i = [0, Inf].
+	mAllocator.Init(2 * num_leaves_plus_internal_nodes, 256); // We use double the amount of nodes while rebuilding the tree during Update()
+
+	// Init sub trees
+	mLayers = new QuadTree [mNumLayers];
+	for (uint l = 0; l < mNumLayers; ++l)
+	{
+		mLayers[l].Init(mAllocator);
+
+#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED)
+		// Set the name of the layer
+		mLayers[l].SetName(inLayerInterface.GetBroadPhaseLayerName(BroadPhaseLayer(BroadPhaseLayer::Type(l))));
+#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED
+	}
+}
+
+void BroadPhaseQuadTree::FrameSync()
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Take a unique lock on the old query lock so that we know no one is using the old nodes anymore.
+	// Note that nothing should be locked at this point to avoid risking a lock inversion deadlock.
+	// Note that in other places where we lock this mutex we don't use SharedLock to detect lock inversions. As long as
+	// nothing else is locked this is safe. This is why BroadPhaseQuery should be the highest priority lock.
+	UniqueLock root_lock(mQueryLocks[mQueryLockIdx ^ 1] JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::BroadPhaseQuery));
+
+	for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l)
+		mLayers[l].DiscardOldTree();
+}
+
+void BroadPhaseQuadTree::Optimize()
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Free the previous tree so we can create a new optimized tree
+	FrameSync();
+
+	LockModifications();
+
+	for (uint l = 0; l < mNumLayers; ++l)
+	{
+		QuadTree &tree = mLayers[l];
+		if (tree.HasBodies() || tree.IsDirty())
+		{
+			QuadTree::UpdateState update_state;
+			tree.UpdatePrepare(mBodyManager->GetBodies(), mTracking, update_state, true);
+			tree.UpdateFinalize(mBodyManager->GetBodies(), mTracking, update_state);
+		}
+	}
+
+	UnlockModifications();
+
+	// Free the tree from before we created a new optimized tree
+	FrameSync();
+
+	mNextLayerToUpdate = 0;
+}
+
+void BroadPhaseQuadTree::LockModifications()
+{
+	// From this point on we prevent modifications to the tree
+	PhysicsLock::sLock(mUpdateMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::BroadPhaseUpdate));
+}
+
+BroadPhase::UpdateState BroadPhaseQuadTree::UpdatePrepare()
+{
+	// LockModifications should have been called
+	JPH_ASSERT(mUpdateMutex.is_locked());
+
+	// Create update state
+	UpdateState update_state;
+	UpdateStateImpl *update_state_impl = reinterpret_cast<UpdateStateImpl *>(&update_state);
+
+	// Loop until we've seen all layers
+	for (uint iteration = 0; iteration < mNumLayers; ++iteration)
+	{
+		// Get the layer
+		QuadTree &tree = mLayers[mNextLayerToUpdate];
+		mNextLayerToUpdate = (mNextLayerToUpdate + 1) % mNumLayers;
+
+		// If it is dirty we update this one
+		if (tree.HasBodies() && tree.IsDirty() && tree.CanBeUpdated())
+		{
+			update_state_impl->mTree = &tree;
+			tree.UpdatePrepare(mBodyManager->GetBodies(), mTracking, update_state_impl->mUpdateState, false);
+			return update_state;
+		}
+	}
+
+	// Nothing to update
+	update_state_impl->mTree = nullptr;
+	return update_state;
+}
+
+void BroadPhaseQuadTree::UpdateFinalize(const UpdateState &inUpdateState)
+{
+	// LockModifications should have been called
+	JPH_ASSERT(mUpdateMutex.is_locked());
+
+	// Test if a tree was updated
+	const UpdateStateImpl *update_state_impl = reinterpret_cast<const UpdateStateImpl *>(&inUpdateState);
+	if (update_state_impl->mTree == nullptr)
+		return;
+
+	update_state_impl->mTree->UpdateFinalize(mBodyManager->GetBodies(), mTracking, update_state_impl->mUpdateState);
+
+	// Make all queries from now on use the new lock
+	mQueryLockIdx = mQueryLockIdx ^ 1;
+}
+
+void BroadPhaseQuadTree::UnlockModifications()
+{
+	// From this point on we allow modifications to the tree again
+	PhysicsLock::sUnlock(mUpdateMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::BroadPhaseUpdate));
+}
+
+BroadPhase::AddState BroadPhaseQuadTree::AddBodiesPrepare(BodyID *ioBodies, int inNumber)
+{
+	JPH_PROFILE_FUNCTION();
+
+	if (inNumber <= 0)
+		return nullptr;
+
+	const BodyVector &bodies = mBodyManager->GetBodies();
+	JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies());
+
+	LayerState *state = new LayerState [mNumLayers];
+
+	// Sort bodies on layer
+	Body * const * const bodies_ptr = bodies.data(); // C pointer or else sort is incredibly slow in debug mode
+	QuickSort(ioBodies, ioBodies + inNumber, [bodies_ptr](BodyID inLHS, BodyID inRHS) { return bodies_ptr[inLHS.GetIndex()]->GetBroadPhaseLayer() < bodies_ptr[inRHS.GetIndex()]->GetBroadPhaseLayer(); });
+
+	BodyID *b_start = ioBodies, *b_end = ioBodies + inNumber;
+	while (b_start < b_end)
+	{
+		// Get broadphase layer
+		BroadPhaseLayer::Type broadphase_layer = (BroadPhaseLayer::Type)bodies[b_start->GetIndex()]->GetBroadPhaseLayer();
+		JPH_ASSERT(broadphase_layer < mNumLayers);
+
+		// Find first body with different layer
+		BodyID *b_mid = std::upper_bound(b_start, b_end, broadphase_layer, [bodies_ptr](BroadPhaseLayer::Type inLayer, BodyID inBodyID) { return inLayer < (BroadPhaseLayer::Type)bodies_ptr[inBodyID.GetIndex()]->GetBroadPhaseLayer(); });
+
+		// Keep track of state for this layer
+		LayerState &layer_state = state[broadphase_layer];
+		layer_state.mBodyStart = b_start;
+		layer_state.mBodyEnd = b_mid;
+
+		// Insert all bodies of the same layer
+		mLayers[broadphase_layer].AddBodiesPrepare(bodies, mTracking, b_start, int(b_mid - b_start), layer_state.mAddState);
+
+		// Keep track in which tree we placed the object
+		for (const BodyID *b = b_start; b < b_mid; ++b)
+		{
+			uint32 index = b->GetIndex();
+			JPH_ASSERT(bodies[index]->GetID() == *b, "Provided BodyID doesn't match BodyID in body manager");
+			JPH_ASSERT(!bodies[index]->IsInBroadPhase());
+			Tracking &t = mTracking[index];
+			JPH_ASSERT(t.mBroadPhaseLayer == (BroadPhaseLayer::Type)cBroadPhaseLayerInvalid);
+			t.mBroadPhaseLayer = broadphase_layer;
+			JPH_ASSERT(t.mObjectLayer == cObjectLayerInvalid);
+			t.mObjectLayer = bodies[index]->GetObjectLayer();
+		}
+
+		// Repeat
+		b_start = b_mid;
+	}
+
+	return state;
+}
+
+void BroadPhaseQuadTree::AddBodiesFinalize(BodyID *ioBodies, int inNumber, AddState inAddState)
+{
+	JPH_PROFILE_FUNCTION();
+
+	if (inNumber <= 0)
+	{
+		JPH_ASSERT(inAddState == nullptr);
+		return;
+	}
+
+	// This cannot run concurrently with UpdatePrepare()/UpdateFinalize()
+	SharedLock lock(mUpdateMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::BroadPhaseUpdate));
+
+	BodyVector &bodies = mBodyManager->GetBodies();
+	JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies());
+
+	LayerState *state = (LayerState *)inAddState;
+
+	for (BroadPhaseLayer::Type broadphase_layer = 0; broadphase_layer < mNumLayers; broadphase_layer++)
+	{
+		const LayerState &l = state[broadphase_layer];
+		if (l.mBodyStart != nullptr)
+		{
+			// Insert all bodies of the same layer
+			mLayers[broadphase_layer].AddBodiesFinalize(mTracking, int(l.mBodyEnd - l.mBodyStart), l.mAddState);
+
+			// Mark added to broadphase
+			for (const BodyID *b = l.mBodyStart; b < l.mBodyEnd; ++b)
+			{
+				uint32 index = b->GetIndex();
+				JPH_ASSERT(bodies[index]->GetID() == *b, "Provided BodyID doesn't match BodyID in body manager");
+				JPH_ASSERT(mTracking[index].mBroadPhaseLayer == broadphase_layer);
+				JPH_ASSERT(mTracking[index].mObjectLayer == bodies[index]->GetObjectLayer());
+				JPH_ASSERT(!bodies[index]->IsInBroadPhase());
+				bodies[index]->SetInBroadPhaseInternal(true);
+			}
+		}
+	}
+
+	delete [] state;
+}
+
+void BroadPhaseQuadTree::AddBodiesAbort(BodyID *ioBodies, int inNumber, AddState inAddState)
+{
+	JPH_PROFILE_FUNCTION();
+
+	if (inNumber <= 0)
+	{
+		JPH_ASSERT(inAddState == nullptr);
+		return;
+	}
+
+	JPH_IF_ENABLE_ASSERTS(const BodyVector &bodies = mBodyManager->GetBodies();)
+	JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies());
+
+	LayerState *state = (LayerState *)inAddState;
+
+	for (BroadPhaseLayer::Type broadphase_layer = 0; broadphase_layer < mNumLayers; broadphase_layer++)
+	{
+		const LayerState &l = state[broadphase_layer];
+		if (l.mBodyStart != nullptr)
+		{
+			// Insert all bodies of the same layer
+			mLayers[broadphase_layer].AddBodiesAbort(mTracking, l.mAddState);
+
+			// Reset bookkeeping
+			for (const BodyID *b = l.mBodyStart; b < l.mBodyEnd; ++b)
+			{
+				uint32 index = b->GetIndex();
+				JPH_ASSERT(bodies[index]->GetID() == *b, "Provided BodyID doesn't match BodyID in body manager");
+				JPH_ASSERT(!bodies[index]->IsInBroadPhase());
+				Tracking &t = mTracking[index];
+				JPH_ASSERT(t.mBroadPhaseLayer == broadphase_layer);
+				t.mBroadPhaseLayer = (BroadPhaseLayer::Type)cBroadPhaseLayerInvalid;
+				t.mObjectLayer = cObjectLayerInvalid;
+			}
+		}
+	}
+
+	delete [] state;
+}
+
+void BroadPhaseQuadTree::RemoveBodies(BodyID *ioBodies, int inNumber)
+{
+	JPH_PROFILE_FUNCTION();
+
+	if (inNumber <= 0)
+		return;
+
+	// This cannot run concurrently with UpdatePrepare()/UpdateFinalize()
+	SharedLock lock(mUpdateMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::BroadPhaseUpdate));
+
+	BodyVector &bodies = mBodyManager->GetBodies();
+	JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies());
+
+	// Sort bodies on layer
+	Tracking *tracking = mTracking.data(); // C pointer or else sort is incredibly slow in debug mode
+	QuickSort(ioBodies, ioBodies + inNumber, [tracking](BodyID inLHS, BodyID inRHS) { return tracking[inLHS.GetIndex()].mBroadPhaseLayer < tracking[inRHS.GetIndex()].mBroadPhaseLayer; });
+
+	BodyID *b_start = ioBodies, *b_end = ioBodies + inNumber;
+	while (b_start < b_end)
+	{
+		// Get broad phase layer
+		BroadPhaseLayer::Type broadphase_layer = mTracking[b_start->GetIndex()].mBroadPhaseLayer;
+		JPH_ASSERT(broadphase_layer != (BroadPhaseLayer::Type)cBroadPhaseLayerInvalid);
+
+		// Find first body with different layer
+		BodyID *b_mid = std::upper_bound(b_start, b_end, broadphase_layer, [tracking](BroadPhaseLayer::Type inLayer, BodyID inBodyID) { return inLayer < tracking[inBodyID.GetIndex()].mBroadPhaseLayer; });
+
+		// Remove all bodies of the same layer
+		mLayers[broadphase_layer].RemoveBodies(bodies, mTracking, b_start, int(b_mid - b_start));
+
+		for (const BodyID *b = b_start; b < b_mid; ++b)
+		{
+			// Reset bookkeeping
+			uint32 index = b->GetIndex();
+			Tracking &t = tracking[index];
+			t.mBroadPhaseLayer = (BroadPhaseLayer::Type)cBroadPhaseLayerInvalid;
+			t.mObjectLayer = cObjectLayerInvalid;
+
+			// Mark removed from broadphase
+			JPH_ASSERT(bodies[index]->IsInBroadPhase());
+			bodies[index]->SetInBroadPhaseInternal(false);
+		}
+
+		// Repeat
+		b_start = b_mid;
+	}
+}
+
+void BroadPhaseQuadTree::NotifyBodiesAABBChanged(BodyID *ioBodies, int inNumber, bool inTakeLock)
+{
+	JPH_PROFILE_FUNCTION();
+
+	if (inNumber <= 0)
+		return;
+
+	// This cannot run concurrently with UpdatePrepare()/UpdateFinalize()
+	if (inTakeLock)
+		PhysicsLock::sLockShared(mUpdateMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::BroadPhaseUpdate));
+	else
+		JPH_ASSERT(mUpdateMutex.is_locked());
+
+	const BodyVector &bodies = mBodyManager->GetBodies();
+	JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies());
+
+	// Sort bodies on layer
+	const Tracking *tracking = mTracking.data(); // C pointer or else sort is incredibly slow in debug mode
+	QuickSort(ioBodies, ioBodies + inNumber, [tracking](BodyID inLHS, BodyID inRHS) { return tracking[inLHS.GetIndex()].mBroadPhaseLayer < tracking[inRHS.GetIndex()].mBroadPhaseLayer; });
+
+	BodyID *b_start = ioBodies, *b_end = ioBodies + inNumber;
+	while (b_start < b_end)
+	{
+		// Get broadphase layer
+		BroadPhaseLayer::Type broadphase_layer = tracking[b_start->GetIndex()].mBroadPhaseLayer;
+		JPH_ASSERT(broadphase_layer != (BroadPhaseLayer::Type)cBroadPhaseLayerInvalid);
+
+		// Find first body with different layer
+		BodyID *b_mid = std::upper_bound(b_start, b_end, broadphase_layer, [tracking](BroadPhaseLayer::Type inLayer, BodyID inBodyID) { return inLayer < tracking[inBodyID.GetIndex()].mBroadPhaseLayer; });
+
+		// Notify all bodies of the same layer changed
+		mLayers[broadphase_layer].NotifyBodiesAABBChanged(bodies, mTracking, b_start, int(b_mid - b_start));
+
+		// Repeat
+		b_start = b_mid;
+	}
+
+	if (inTakeLock)
+		PhysicsLock::sUnlockShared(mUpdateMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::BroadPhaseUpdate));
+}
+
+void BroadPhaseQuadTree::NotifyBodiesLayerChanged(BodyID *ioBodies, int inNumber)
+{
+	JPH_PROFILE_FUNCTION();
+
+	if (inNumber <= 0)
+		return;
+
+	// First sort the bodies that actually changed layer to beginning of the array
+	const BodyVector &bodies = mBodyManager->GetBodies();
+	JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies());
+	for (BodyID *body_id = ioBodies + inNumber - 1; body_id >= ioBodies; --body_id)
+	{
+		uint32 index = body_id->GetIndex();
+		JPH_ASSERT(bodies[index]->GetID() == *body_id, "Provided BodyID doesn't match BodyID in body manager");
+		const Body *body = bodies[index];
+		BroadPhaseLayer::Type broadphase_layer = (BroadPhaseLayer::Type)body->GetBroadPhaseLayer();
+		JPH_ASSERT(broadphase_layer < mNumLayers);
+		if (mTracking[index].mBroadPhaseLayer == broadphase_layer)
+		{
+			// Update tracking information
+			mTracking[index].mObjectLayer = body->GetObjectLayer();
+
+			// Move the body to the end, layer didn't change
+			std::swap(*body_id, ioBodies[inNumber - 1]);
+			--inNumber;
+		}
+	}
+
+	if (inNumber > 0)
+	{
+		// Changing layer requires us to remove from one tree and add to another, so this is equivalent to removing all bodies first and then adding them again
+		RemoveBodies(ioBodies, inNumber);
+		AddState add_state = AddBodiesPrepare(ioBodies, inNumber);
+		AddBodiesFinalize(ioBodies, inNumber, add_state);
+	}
+}
+
+void BroadPhaseQuadTree::CastRay(const RayCast &inRay, RayCastBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies());
+
+	// Prevent this from running in parallel with node deletion in FrameSync(), see notes there
+	shared_lock lock(mQueryLocks[mQueryLockIdx]);
+
+	// Loop over all layers and test the ones that could hit
+	for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l)
+	{
+		const QuadTree &tree = mLayers[l];
+		if (tree.HasBodies() && inBroadPhaseLayerFilter.ShouldCollide(BroadPhaseLayer(l)))
+		{
+			JPH_PROFILE(tree.GetName());
+			tree.CastRay(inRay, ioCollector, inObjectLayerFilter, mTracking);
+			if (ioCollector.ShouldEarlyOut())
+				break;
+		}
+	}
+}
+
+void BroadPhaseQuadTree::CollideAABox(const AABox &inBox, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies());
+
+	// Prevent this from running in parallel with node deletion in FrameSync(), see notes there
+	shared_lock lock(mQueryLocks[mQueryLockIdx]);
+
+	// Loop over all layers and test the ones that could hit
+	for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l)
+	{
+		const QuadTree &tree = mLayers[l];
+		if (tree.HasBodies() && inBroadPhaseLayerFilter.ShouldCollide(BroadPhaseLayer(l)))
+		{
+			JPH_PROFILE(tree.GetName());
+			tree.CollideAABox(inBox, ioCollector, inObjectLayerFilter, mTracking);
+			if (ioCollector.ShouldEarlyOut())
+				break;
+		}
+	}
+}
+
+void BroadPhaseQuadTree::CollideSphere(Vec3Arg inCenter, float inRadius, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies());
+
+	// Prevent this from running in parallel with node deletion in FrameSync(), see notes there
+	shared_lock lock(mQueryLocks[mQueryLockIdx]);
+
+	// Loop over all layers and test the ones that could hit
+	for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l)
+	{
+		const QuadTree &tree = mLayers[l];
+		if (tree.HasBodies() && inBroadPhaseLayerFilter.ShouldCollide(BroadPhaseLayer(l)))
+		{
+			JPH_PROFILE(tree.GetName());
+			tree.CollideSphere(inCenter, inRadius, ioCollector, inObjectLayerFilter, mTracking);
+			if (ioCollector.ShouldEarlyOut())
+				break;
+		}
+	}
+}
+
+void BroadPhaseQuadTree::CollidePoint(Vec3Arg inPoint, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies());
+
+	// Prevent this from running in parallel with node deletion in FrameSync(), see notes there
+	shared_lock lock(mQueryLocks[mQueryLockIdx]);
+
+	// Loop over all layers and test the ones that could hit
+	for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l)
+	{
+		const QuadTree &tree = mLayers[l];
+		if (tree.HasBodies() && inBroadPhaseLayerFilter.ShouldCollide(BroadPhaseLayer(l)))
+		{
+			JPH_PROFILE(tree.GetName());
+			tree.CollidePoint(inPoint, ioCollector, inObjectLayerFilter, mTracking);
+			if (ioCollector.ShouldEarlyOut())
+				break;
+		}
+	}
+}
+
+void BroadPhaseQuadTree::CollideOrientedBox(const OrientedBox &inBox, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies());
+
+	// Prevent this from running in parallel with node deletion in FrameSync(), see notes there
+	shared_lock lock(mQueryLocks[mQueryLockIdx]);
+
+	// Loop over all layers and test the ones that could hit
+	for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l)
+	{
+		const QuadTree &tree = mLayers[l];
+		if (tree.HasBodies() && inBroadPhaseLayerFilter.ShouldCollide(BroadPhaseLayer(l)))
+		{
+			JPH_PROFILE(tree.GetName());
+			tree.CollideOrientedBox(inBox, ioCollector, inObjectLayerFilter, mTracking);
+			if (ioCollector.ShouldEarlyOut())
+				break;
+		}
+	}
+}
+
+void BroadPhaseQuadTree::CastAABoxNoLock(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies());
+
+	// Loop over all layers and test the ones that could hit
+	for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l)
+	{
+		const QuadTree &tree = mLayers[l];
+		if (tree.HasBodies() && inBroadPhaseLayerFilter.ShouldCollide(BroadPhaseLayer(l)))
+		{
+			JPH_PROFILE(tree.GetName());
+			tree.CastAABox(inBox, ioCollector, inObjectLayerFilter, mTracking);
+			if (ioCollector.ShouldEarlyOut())
+				break;
+		}
+	}
+}
+
+void BroadPhaseQuadTree::CastAABox(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const
+{
+	// Prevent this from running in parallel with node deletion in FrameSync(), see notes there
+	shared_lock lock(mQueryLocks[mQueryLockIdx]);
+
+	CastAABoxNoLock(inBox, ioCollector, inBroadPhaseLayerFilter, inObjectLayerFilter);
+}
+
+void BroadPhaseQuadTree::FindCollidingPairs(BodyID *ioActiveBodies, int inNumActiveBodies, float inSpeculativeContactDistance, const ObjectVsBroadPhaseLayerFilter &inObjectVsBroadPhaseLayerFilter, const ObjectLayerPairFilter &inObjectLayerPairFilter, BodyPairCollector &ioPairCollector) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	const BodyVector &bodies = mBodyManager->GetBodies();
+	JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies());
+
+	// Note that we don't take any locks at this point. We know that the tree is not going to be swapped or deleted while finding collision pairs due to the way the jobs are scheduled in the PhysicsSystem::Update.
+
+	// Sort bodies on layer
+	const Tracking *tracking = mTracking.data(); // C pointer or else sort is incredibly slow in debug mode
+	QuickSort(ioActiveBodies, ioActiveBodies + inNumActiveBodies, [tracking](BodyID inLHS, BodyID inRHS) { return tracking[inLHS.GetIndex()].mObjectLayer < tracking[inRHS.GetIndex()].mObjectLayer; });
+
+	BodyID *b_start = ioActiveBodies, *b_end = ioActiveBodies + inNumActiveBodies;
+	while (b_start < b_end)
+	{
+		// Get broadphase layer
+		ObjectLayer object_layer = tracking[b_start->GetIndex()].mObjectLayer;
+		JPH_ASSERT(object_layer != cObjectLayerInvalid);
+
+		// Find first body with different layer
+		BodyID *b_mid = std::upper_bound(b_start, b_end, object_layer, [tracking](ObjectLayer inLayer, BodyID inBodyID) { return inLayer < tracking[inBodyID.GetIndex()].mObjectLayer; });
+
+		// Loop over all layers and test the ones that could hit
+		for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l)
+		{
+			const QuadTree &tree = mLayers[l];
+			if (tree.HasBodies() && inObjectVsBroadPhaseLayerFilter.ShouldCollide(object_layer, BroadPhaseLayer(l)))
+			{
+				JPH_PROFILE(tree.GetName());
+				tree.FindCollidingPairs(bodies, b_start, int(b_mid - b_start), inSpeculativeContactDistance, ioPairCollector, inObjectLayerPairFilter);
+			}
+		}
+
+		// Repeat
+		b_start = b_mid;
+	}
+}
+
+AABox BroadPhaseQuadTree::GetBounds() const
+{
+	// Prevent this from running in parallel with node deletion in FrameSync(), see notes there
+	shared_lock lock(mQueryLocks[mQueryLockIdx]);
+
+	AABox bounds;
+	for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l)
+		bounds.Encapsulate(mLayers[l].GetBounds());
+	return bounds;
+}
+
+#ifdef JPH_TRACK_BROADPHASE_STATS
+
+void BroadPhaseQuadTree::ReportStats()
+{
+	Trace("Query Type, Filter Description, Tree Name, Num Queries, Total Time (%%), Total Time Excl. Collector (%%), Nodes Visited, Bodies Visited, Hits Reported, Hits Reported vs Bodies Visited (%%), Hits Reported vs Nodes Visited");
+
+	uint64 total_ticks = 0;
+	for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l)
+		total_ticks += mLayers[l].GetTicks100Pct();
+
+	for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l)
+		mLayers[l].ReportStats(total_ticks);
+}
+
+#endif // JPH_TRACK_BROADPHASE_STATS
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseQuadTree.h

@@ -0,0 +1,108 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/BroadPhase/QuadTree.h>
+#include <Jolt/Physics/Collision/BroadPhase/BroadPhase.h>
+#include <Jolt/Physics/PhysicsLock.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Fast SIMD based quad tree BroadPhase that is multithreading aware and tries to do a minimal amount of locking.
+class JPH_EXPORT BroadPhaseQuadTree final : public BroadPhase
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Destructor
+	virtual					~BroadPhaseQuadTree() override;
+
+	// Implementing interface of BroadPhase (see BroadPhase for documentation)
+	virtual void			Init(BodyManager *inBodyManager, const BroadPhaseLayerInterface &inLayerInterface) override;
+	virtual void			Optimize() override;
+	virtual void			FrameSync() override;
+	virtual void			LockModifications() override;
+	virtual	UpdateState		UpdatePrepare() override;
+	virtual void			UpdateFinalize(const UpdateState &inUpdateState) override;
+	virtual void			UnlockModifications() override;
+	virtual AddState		AddBodiesPrepare(BodyID *ioBodies, int inNumber) override;
+	virtual void			AddBodiesFinalize(BodyID *ioBodies, int inNumber, AddState inAddState) override;
+	virtual void			AddBodiesAbort(BodyID *ioBodies, int inNumber, AddState inAddState) override;
+	virtual void			RemoveBodies(BodyID *ioBodies, int inNumber) override;
+	virtual void			NotifyBodiesAABBChanged(BodyID *ioBodies, int inNumber, bool inTakeLock) override;
+	virtual void			NotifyBodiesLayerChanged(BodyID *ioBodies, int inNumber) override;
+	virtual void			CastRay(const RayCast &inRay, RayCastBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override;
+	virtual void			CollideAABox(const AABox &inBox, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override;
+	virtual void			CollideSphere(Vec3Arg inCenter, float inRadius, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override;
+	virtual void			CollidePoint(Vec3Arg inPoint, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override;
+	virtual void			CollideOrientedBox(const OrientedBox &inBox, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override;
+	virtual void			CastAABoxNoLock(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override;
+	virtual void			CastAABox(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override;
+	virtual void			FindCollidingPairs(BodyID *ioActiveBodies, int inNumActiveBodies, float inSpeculativeContactDistance, const ObjectVsBroadPhaseLayerFilter &inObjectVsBroadPhaseLayerFilter, const ObjectLayerPairFilter &inObjectLayerPairFilter, BodyPairCollector &ioPairCollector) const override;
+	virtual AABox			GetBounds() const override;
+#ifdef JPH_TRACK_BROADPHASE_STATS
+	virtual void			ReportStats() override;
+#endif // JPH_TRACK_BROADPHASE_STATS
+
+private:
+	/// Helper struct for AddBodies handle
+	struct LayerState
+	{
+		JPH_OVERRIDE_NEW_DELETE
+
+		BodyID *			mBodyStart = nullptr;
+		BodyID *			mBodyEnd;
+		QuadTree::AddState	mAddState;
+	};
+
+	using Tracking = QuadTree::Tracking;
+	using TrackingVector = QuadTree::TrackingVector;
+
+#ifdef JPH_ENABLE_ASSERTS
+	/// Context used to lock a physics lock
+	PhysicsLockContext		mLockContext = nullptr;
+#endif // JPH_ENABLE_ASSERTS
+
+	/// Max amount of bodies we support
+	size_t					mMaxBodies = 0;
+
+	/// Array that for each BodyID keeps track of where it is located in which tree
+	TrackingVector			mTracking;
+
+	/// Node allocator for all trees
+	QuadTree::Allocator		mAllocator;
+
+	/// Information about broad phase layers
+	const BroadPhaseLayerInterface *mBroadPhaseLayerInterface = nullptr;
+
+	/// One tree per object layer
+	QuadTree *				mLayers;
+	uint					mNumLayers;
+
+	/// UpdateState implementation for this tree used during UpdatePrepare/Finalize()
+	struct UpdateStateImpl
+	{
+		QuadTree *				mTree;
+		QuadTree::UpdateState	mUpdateState;
+	};
+
+	static_assert(sizeof(UpdateStateImpl) <= sizeof(UpdateState));
+	static_assert(alignof(UpdateStateImpl) <= alignof(UpdateState));
+
+	/// Mutex that prevents object modification during UpdatePrepare/Finalize()
+	SharedMutex				mUpdateMutex;
+
+	/// We double buffer all trees so that we can query while building the next one and we destroy the old tree the next physics update.
+	/// This structure ensures that we wait for queries that are still using the old tree.
+	mutable SharedMutex		mQueryLocks[2];
+
+	/// This index indicates which lock is currently active, it alternates between 0 and 1
+	atomic<uint32>			mQueryLockIdx { 0 };
+
+	/// This is the next tree to update in UpdatePrepare()
+	uint32					mNextLayerToUpdate = 0;
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseQuery.h

@@ -0,0 +1,53 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/BroadPhase/BroadPhaseLayer.h>
+#include <Jolt/Physics/Collision/ObjectLayer.h>
+#include <Jolt/Physics/Collision/CollisionCollector.h>
+#include <Jolt/Physics/Body/BodyID.h>
+#include <Jolt/Core/NonCopyable.h>
+
+JPH_NAMESPACE_BEGIN
+
+struct RayCast;
+class BroadPhaseCastResult;
+class AABox;
+class OrientedBox;
+struct AABoxCast;
+
+// Various collector configurations
+using RayCastBodyCollector = CollisionCollector<BroadPhaseCastResult, CollisionCollectorTraitsCastRay>;
+using CastShapeBodyCollector = CollisionCollector<BroadPhaseCastResult, CollisionCollectorTraitsCastShape>;
+using CollideShapeBodyCollector = CollisionCollector<BodyID, CollisionCollectorTraitsCollideShape>;
+
+/// Interface to the broadphase that can perform collision queries. These queries will only test the bounding box of the body to quickly determine a potential set of colliding bodies.
+/// The shapes of the bodies are not tested, if you want this then you should use the NarrowPhaseQuery interface.
+class JPH_EXPORT BroadPhaseQuery : public NonCopyable
+{
+public:
+	/// Virtual destructor
+	virtual				~BroadPhaseQuery() = default;
+
+	/// Cast a ray and add any hits to ioCollector
+	virtual void		CastRay(const RayCast &inRay, RayCastBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }) const = 0;
+
+	/// Get bodies intersecting with inBox and any hits to ioCollector
+	virtual void		CollideAABox(const AABox &inBox, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }) const = 0;
+
+	/// Get bodies intersecting with a sphere and any hits to ioCollector
+	virtual void		CollideSphere(Vec3Arg inCenter, float inRadius, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }) const = 0;
+
+	/// Get bodies intersecting with a point and any hits to ioCollector
+	virtual void		CollidePoint(Vec3Arg inPoint, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }) const = 0;
+
+	/// Get bodies intersecting with an oriented box and any hits to ioCollector
+	virtual void		CollideOrientedBox(const OrientedBox &inBox, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }) const = 0;
+
+	/// Cast a box and add any hits to ioCollector
+	virtual void		CastAABox(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }) const = 0;
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/ObjectVsBroadPhaseLayerFilterMask.h

@@ -0,0 +1,35 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/BroadPhase/BroadPhaseLayerInterfaceMask.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Class that determines if an object layer can collide with a broadphase layer.
+/// This implementation works together with BroadPhaseLayerInterfaceMask and ObjectLayerPairFilterMask
+class ObjectVsBroadPhaseLayerFilterMask : public ObjectVsBroadPhaseLayerFilter
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+/// Constructor
+					ObjectVsBroadPhaseLayerFilterMask(const BroadPhaseLayerInterfaceMask &inBroadPhaseLayerInterface) :
+		mBroadPhaseLayerInterface(inBroadPhaseLayerInterface)
+	{
+	}
+
+	/// Returns true if an object layer should collide with a broadphase layer
+	virtual bool	ShouldCollide(ObjectLayer inLayer1, BroadPhaseLayer inLayer2) const override
+	{
+		// Just defer to BroadPhaseLayerInterface
+		return mBroadPhaseLayerInterface.ShouldCollide(inLayer1, inLayer2);
+	}
+
+private:
+	const BroadPhaseLayerInterfaceMask &mBroadPhaseLayerInterface;
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/ObjectVsBroadPhaseLayerFilterTable.h

@@ -0,0 +1,66 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/BroadPhase/BroadPhaseLayer.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Class that determines if an object layer can collide with a broadphase layer.
+/// This implementation uses a table and constructs itself from an ObjectLayerPairFilter and a BroadPhaseLayerInterface.
+class ObjectVsBroadPhaseLayerFilterTable : public ObjectVsBroadPhaseLayerFilter
+{
+private:
+	/// Get which bit corresponds to the pair (inLayer1, inLayer2)
+	uint					GetBit(ObjectLayer inLayer1, BroadPhaseLayer inLayer2) const
+	{
+		// Calculate at which bit the entry for this pair resides
+		return inLayer1 * mNumBroadPhaseLayers + (BroadPhaseLayer::Type)inLayer2;
+	}
+
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Construct the table
+	/// @param inBroadPhaseLayerInterface The broad phase layer interface that maps object layers to broad phase layers
+	/// @param inNumBroadPhaseLayers Number of broad phase layers
+	/// @param inObjectLayerPairFilter The object layer pair filter that determines which object layers can collide
+	/// @param inNumObjectLayers Number of object layers
+							ObjectVsBroadPhaseLayerFilterTable(const BroadPhaseLayerInterface &inBroadPhaseLayerInterface, uint inNumBroadPhaseLayers, const ObjectLayerPairFilter &inObjectLayerPairFilter, uint inNumObjectLayers) :
+		mNumBroadPhaseLayers(inNumBroadPhaseLayers)
+	{
+		// Resize table and set all entries to false
+		mTable.resize((inNumBroadPhaseLayers * inNumObjectLayers + 7) / 8, 0);
+
+		// Loop over all object layer pairs
+		for (ObjectLayer o1 = 0; o1 < inNumObjectLayers; ++o1)
+			for (ObjectLayer o2 = 0; o2 < inNumObjectLayers; ++o2)
+			{
+				// Get the broad phase layer for the second object layer
+				BroadPhaseLayer b2 = inBroadPhaseLayerInterface.GetBroadPhaseLayer(o2);
+				JPH_ASSERT((BroadPhaseLayer::Type)b2 < inNumBroadPhaseLayers);
+
+				// If the object layers collide then so should the object and broadphase layer
+				if (inObjectLayerPairFilter.ShouldCollide(o1, o2))
+				{
+					uint bit = GetBit(o1, b2);
+					mTable[bit >> 3] |= 1 << (bit & 0b111);
+				}
+			}
+	}
+
+	/// Returns true if an object layer should collide with a broadphase layer
+	virtual bool			ShouldCollide(ObjectLayer inLayer1, BroadPhaseLayer inLayer2) const override
+	{
+		uint bit = GetBit(inLayer1, inLayer2);
+		return (mTable[bit >> 3] & (1 << (bit & 0b111))) != 0;
+	}
+
+private:
+	uint					mNumBroadPhaseLayers;						///< The total number of broadphase layers
+	Array<uint8>			mTable;										///< The table of bits that indicates which layers collide
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/QuadTree.h

@@ -0,0 +1,389 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/FixedSizeFreeList.h>
+#include <Jolt/Core/Atomics.h>
+#include <Jolt/Core/NonCopyable.h>
+#include <Jolt/Physics/Body/BodyManager.h>
+#include <Jolt/Physics/Collision/BroadPhase/BroadPhase.h>
+
+//#define JPH_DUMP_BROADPHASE_TREE
+
+JPH_NAMESPACE_BEGIN
+
+/// Internal tree structure in broadphase, is essentially a quad AABB tree.
+/// Tree is lockless (except for UpdatePrepare/Finalize() function), modifying objects in the tree will widen the aabbs of parent nodes to make the node fit.
+/// During the UpdatePrepare/Finalize() call the tree is rebuilt to achieve a tight fit again.
+class JPH_EXPORT QuadTree : public NonCopyable
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+private:
+	// Forward declare
+	class AtomicNodeID;
+
+	/// Class that points to either a body or a node in the tree
+	class NodeID
+	{
+	public:
+		JPH_OVERRIDE_NEW_DELETE
+
+		/// Default constructor does not initialize
+		inline					NodeID() = default;
+
+		/// Construct a node ID
+		static inline NodeID	sInvalid()							{ return NodeID(cInvalidNodeIndex); }
+		static inline NodeID	sFromBodyID(BodyID inID)			{ NodeID node_id(inID.GetIndexAndSequenceNumber()); JPH_ASSERT(node_id.IsBody()); return node_id; }
+		static inline NodeID	sFromNodeIndex(uint32 inIdx)		{ JPH_ASSERT((inIdx & cIsNode) == 0); return NodeID(inIdx | cIsNode); }
+
+		/// Check what type of ID it is
+		inline bool				IsValid() const						{ return mID != cInvalidNodeIndex; }
+		inline bool				IsBody() const						{ return (mID & cIsNode) == 0; }
+		inline bool				IsNode() const						{ return (mID & cIsNode) != 0; }
+
+		/// Get body or node index
+		inline BodyID			GetBodyID() const					{ JPH_ASSERT(IsBody()); return BodyID(mID); }
+		inline uint32			GetNodeIndex() const				{ JPH_ASSERT(IsNode()); return mID & ~cIsNode; }
+
+		/// Comparison
+		inline bool				operator == (const BodyID &inRHS) const { return mID == inRHS.GetIndexAndSequenceNumber(); }
+		inline bool				operator == (const NodeID &inRHS) const	{ return mID == inRHS.mID; }
+
+	private:
+		friend class AtomicNodeID;
+
+		inline explicit			NodeID(uint32 inID)					: mID(inID) { }
+
+		static const uint32		cIsNode = BodyID::cBroadPhaseBit;	///< If this bit is set it means that the ID refers to a node, otherwise it refers to a body
+
+		uint32					mID;
+	};
+
+	static_assert(sizeof(NodeID) == sizeof(BodyID), "Body id's should have the same size as NodeIDs");
+
+	/// A NodeID that uses atomics to store the value
+	class AtomicNodeID
+	{
+	public:
+		/// Constructor
+								AtomicNodeID() = default;
+		explicit				AtomicNodeID(const NodeID &inRHS)			: mID(inRHS.mID) { }
+
+		/// Assignment
+		inline void				operator = (const NodeID &inRHS)			{ mID = inRHS.mID; }
+
+		/// Getting the value
+		inline					operator NodeID () const					{ return NodeID(mID); }
+
+		/// Check if the ID is valid
+		inline bool				IsValid() const								{ return mID != cInvalidNodeIndex; }
+
+		/// Comparison
+		inline bool				operator == (const BodyID &inRHS) const		{ return mID == inRHS.GetIndexAndSequenceNumber(); }
+		inline bool				operator == (const NodeID &inRHS) const		{ return mID == inRHS.mID; }
+
+		/// Atomically compare and swap value. Expects inOld value, replaces with inNew value or returns false
+		inline bool				CompareExchange(NodeID inOld, NodeID inNew)	{ return mID.compare_exchange_strong(inOld.mID, inNew.mID); }
+
+	private:
+		atomic<uint32>			mID;
+	};
+
+	/// Class that represents a node in the tree
+	class Node
+	{
+	public:
+		/// Construct node
+		explicit				Node(bool inIsChanged);
+
+		/// Get bounding box encapsulating all children
+		void					GetNodeBounds(AABox &outBounds) const;
+
+		/// Get bounding box in a consistent way with the functions below (check outBounds.IsValid() before using the box)
+		void					GetChildBounds(int inChildIndex, AABox &outBounds) const;
+
+		/// Set the bounds in such a way that other threads will either see a fully correct bounding box or a bounding box with no volume
+		void					SetChildBounds(int inChildIndex, const AABox &inBounds);
+
+		/// Invalidate bounding box in such a way that other threads will not temporarily see a very large bounding box
+		void					InvalidateChildBounds(int inChildIndex);
+
+		/// Encapsulate inBounds in node bounds, returns true if there were changes
+		bool					EncapsulateChildBounds(int inChildIndex, const AABox &inBounds);
+
+		/// Bounding box for child nodes or bodies (all initially set to invalid so no collision test will ever traverse to the leaf)
+		atomic<float>			mBoundsMinX[4];
+		atomic<float>			mBoundsMinY[4];
+		atomic<float>			mBoundsMinZ[4];
+		atomic<float>			mBoundsMaxX[4];
+		atomic<float>			mBoundsMaxY[4];
+		atomic<float>			mBoundsMaxZ[4];
+
+		/// Index of child node or body ID.
+		AtomicNodeID			mChildNodeID[4];
+
+		/// Index of the parent node.
+		/// Note: This value is unreliable during the UpdatePrepare/Finalize() function as a node may be relinked to the newly built tree.
+		atomic<uint32>			mParentNodeIndex = cInvalidNodeIndex;
+
+		/// If this part of the tree has changed, if not, we will treat this sub tree as a single body during the UpdatePrepare/Finalize().
+		/// If any changes are made to an object inside this sub tree then the direct path from the body to the top of the tree will become changed.
+		atomic<uint32>			mIsChanged;
+
+		// Padding to align to 124 bytes
+		uint32					mPadding = 0;
+	};
+
+	// Maximum size of the stack during tree walk
+	static constexpr int		cStackSize = 128;
+
+	static_assert(sizeof(atomic<float>) == 4, "Assuming that an atomic doesn't add any additional storage");
+	static_assert(sizeof(atomic<uint32>) == 4, "Assuming that an atomic doesn't add any additional storage");
+	static_assert(std::is_trivially_destructible<Node>(), "Assuming that we don't have a destructor");
+
+public:
+	/// Class that allocates tree nodes, can be shared between multiple trees
+	using Allocator = FixedSizeFreeList<Node>;
+
+	static_assert(Allocator::ObjectStorageSize == 128, "Node should be 128 bytes");
+
+	/// Data to track location of a Body in the tree
+	struct Tracking
+	{
+		/// Constructor to satisfy the vector class
+								Tracking() = default;
+								Tracking(const Tracking &inRHS) : mBroadPhaseLayer(inRHS.mBroadPhaseLayer.load()), mObjectLayer(inRHS.mObjectLayer.load()), mBodyLocation(inRHS.mBodyLocation.load()) { }
+
+		/// Invalid body location identifier
+		static const uint32		cInvalidBodyLocation = 0xffffffff;
+
+		atomic<BroadPhaseLayer::Type> mBroadPhaseLayer = (BroadPhaseLayer::Type)cBroadPhaseLayerInvalid;
+		atomic<ObjectLayer>		mObjectLayer = cObjectLayerInvalid;
+		atomic<uint32>			mBodyLocation { cInvalidBodyLocation };
+	};
+
+	using TrackingVector = Array<Tracking>;
+
+	/// Destructor
+								~QuadTree();
+
+#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED)
+	/// Name of the tree for debugging purposes
+	void						SetName(const char *inName)			{ mName = inName; }
+	inline const char *			GetName() const						{ return mName; }
+#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED
+
+	/// Check if there is anything in the tree
+	inline bool					HasBodies() const					{ return mNumBodies != 0; }
+
+	/// Check if the tree needs an UpdatePrepare/Finalize()
+	inline bool					IsDirty() const						{ return mIsDirty; }
+
+	/// Check if this tree can get an UpdatePrepare/Finalize() or if it needs a DiscardOldTree() first
+	inline bool					CanBeUpdated() const				{ return mFreeNodeBatch.mNumObjects == 0; }
+
+	/// Initialization
+	void						Init(Allocator &inAllocator);
+
+	struct UpdateState
+	{
+		NodeID					mRootNodeID;						///< This will be the new root node id
+	};
+
+	/// Will throw away the previous frame's nodes so that we can start building a new tree in the background
+	void						DiscardOldTree();
+
+	/// Get the bounding box for this tree
+	AABox						GetBounds() const;
+
+	/// Update the broadphase, needs to be called regularly to achieve a tight fit of the tree when bodies have been modified.
+	/// UpdatePrepare() will build the tree, UpdateFinalize() will lock the root of the tree shortly and swap the trees and afterwards clean up temporary data structures.
+	void						UpdatePrepare(const BodyVector &inBodies, TrackingVector &ioTracking, UpdateState &outUpdateState, bool inFullRebuild);
+	void						UpdateFinalize(const BodyVector &inBodies, const TrackingVector &inTracking, const UpdateState &inUpdateState);
+
+	/// Temporary data structure to pass information between AddBodiesPrepare and AddBodiesFinalize/Abort
+	struct AddState
+	{
+		NodeID					mLeafID = NodeID::sInvalid();
+		AABox					mLeafBounds;
+	};
+
+	/// Prepare adding inNumber bodies at ioBodyIDs to the quad tree, returns the state in outState that should be used in AddBodiesFinalize.
+	/// This can be done on a background thread without influencing the broadphase.
+	/// ioBodyIDs may be shuffled around by this function.
+	void						AddBodiesPrepare(const BodyVector &inBodies, TrackingVector &ioTracking, BodyID *ioBodyIDs, int inNumber, AddState &outState);
+
+	/// Finalize adding bodies to the quadtree, supply the same number of bodies as in AddBodiesPrepare.
+	void						AddBodiesFinalize(TrackingVector &ioTracking, int inNumberBodies, const AddState &inState);
+
+	/// Abort adding bodies to the quadtree, supply the same bodies and state as in AddBodiesPrepare.
+	/// This can be done on a background thread without influencing the broadphase.
+	void						AddBodiesAbort(TrackingVector &ioTracking, const AddState &inState);
+
+	/// Remove inNumber bodies in ioBodyIDs from the quadtree.
+	void						RemoveBodies(const BodyVector &inBodies, TrackingVector &ioTracking, const BodyID *ioBodyIDs, int inNumber);
+
+	/// Call whenever the aabb of a body changes.
+	void						NotifyBodiesAABBChanged(const BodyVector &inBodies, const TrackingVector &inTracking, const BodyID *ioBodyIDs, int inNumber);
+
+	/// Cast a ray and get the intersecting bodies in ioCollector.
+	void						CastRay(const RayCast &inRay, RayCastBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const;
+
+	/// Get bodies intersecting with inBox in ioCollector
+	void						CollideAABox(const AABox &inBox, CollideShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const;
+
+	/// Get bodies intersecting with a sphere in ioCollector
+	void						CollideSphere(Vec3Arg inCenter, float inRadius, CollideShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const;
+
+	/// Get bodies intersecting with a point and any hits to ioCollector
+	void						CollidePoint(Vec3Arg inPoint, CollideShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const;
+
+	/// Get bodies intersecting with an oriented box and any hits to ioCollector
+	void						CollideOrientedBox(const OrientedBox &inBox, CollideShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const;
+
+	/// Cast a box and get intersecting bodies in ioCollector
+	void						CastAABox(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const;
+
+	/// Find all colliding pairs between dynamic bodies, calls ioPairCollector for every pair found
+	void						FindCollidingPairs(const BodyVector &inBodies, const BodyID *inActiveBodies, int inNumActiveBodies, float inSpeculativeContactDistance, BodyPairCollector &ioPairCollector, const ObjectLayerPairFilter &inObjectLayerPairFilter) const;
+
+#ifdef JPH_TRACK_BROADPHASE_STATS
+	/// Sum up all the ticks spent in the various layers
+	uint64						GetTicks100Pct() const;
+
+	/// Trace the stats of this tree to the TTY
+	void						ReportStats(uint64 inTicks100Pct) const;
+#endif // JPH_TRACK_BROADPHASE_STATS
+
+private:
+	/// Constants
+	static constexpr uint32		cInvalidNodeIndex = 0xffffffff;		///< Value used to indicate node index is invalid
+	static const AABox			cInvalidBounds;						///< Invalid bounding box using cLargeFloat
+
+	/// We alternate between two trees in order to let collision queries complete in parallel to adding/removing objects to the tree
+	struct RootNode
+	{
+		/// Get the ID of the root node
+		inline NodeID			GetNodeID() const					{ return NodeID::sFromNodeIndex(mIndex); }
+
+		/// Index of the root node of the tree (this is always a node, never a body id)
+		atomic<uint32>			mIndex { cInvalidNodeIndex };
+	};
+
+	/// Caches location of body inBodyID in the tracker, body can be found in mNodes[inNodeIdx].mChildNodeID[inChildIdx]
+	void						GetBodyLocation(const TrackingVector &inTracking, BodyID inBodyID, uint32 &outNodeIdx, uint32 &outChildIdx) const;
+	void						SetBodyLocation(TrackingVector &ioTracking, BodyID inBodyID, uint32 inNodeIdx, uint32 inChildIdx) const;
+	static void					sInvalidateBodyLocation(TrackingVector &ioTracking, BodyID inBodyID);
+
+	/// Get the current root of the tree
+	JPH_INLINE const RootNode &	GetCurrentRoot() const				{ return mRootNode[mRootNodeIndex]; }
+	JPH_INLINE RootNode &		GetCurrentRoot()					{ return mRootNode[mRootNodeIndex]; }
+
+	/// Depending on if inNodeID is a body or tree node return the bounding box
+	inline AABox				GetNodeOrBodyBounds(const BodyVector &inBodies, NodeID inNodeID) const;
+
+	/// Mark node and all of its parents as changed
+	inline void					MarkNodeAndParentsChanged(uint32 inNodeIndex);
+
+	/// Widen parent bounds of node inNodeIndex to encapsulate inNewBounds, also mark node and all of its parents as changed
+	inline void					WidenAndMarkNodeAndParentsChanged(uint32 inNodeIndex, const AABox &inNewBounds);
+
+	/// Allocate a new node
+	inline uint32				AllocateNode(bool inIsChanged);
+
+	/// Try to insert a new leaf to the tree at inNodeIndex
+	inline bool					TryInsertLeaf(TrackingVector &ioTracking, int inNodeIndex, NodeID inLeafID, const AABox &inLeafBounds, int inLeafNumBodies);
+
+	/// Try to replace the existing root with a new root that contains both the existing root and the new leaf
+	inline bool					TryCreateNewRoot(TrackingVector &ioTracking, atomic<uint32> &ioRootNodeIndex, NodeID inLeafID, const AABox &inLeafBounds, int inLeafNumBodies);
+
+	/// Build a tree for ioBodyIDs, returns the NodeID of the root (which will be the ID of a single body if inNumber = 1). All tree levels up to inMaxDepthMarkChanged will be marked as 'changed'.
+	NodeID						BuildTree(const BodyVector &inBodies, TrackingVector &ioTracking, NodeID *ioNodeIDs, int inNumber, uint inMaxDepthMarkChanged, AABox &outBounds);
+
+	/// Sorts ioNodeIDs spatially into 2 groups. Second groups starts at ioNodeIDs + outMidPoint.
+	/// After the function returns ioNodeIDs and ioNodeCenters will be shuffled
+	static void					sPartition(NodeID *ioNodeIDs, Vec3 *ioNodeCenters, int inNumber, int &outMidPoint);
+
+	/// Sorts ioNodeIDs from inBegin to (but excluding) inEnd spatially into 4 groups.
+	/// outSplit needs to be 5 ints long, when the function returns each group runs from outSplit[i] to (but excluding) outSplit[i + 1]
+	/// After the function returns ioNodeIDs and ioNodeCenters will be shuffled
+	static void					sPartition4(NodeID *ioNodeIDs, Vec3 *ioNodeCenters, int inBegin, int inEnd, int *outSplit);
+
+#ifdef JPH_DEBUG
+	/// Validate that the tree is consistent.
+	/// Note: This function only works if the tree is not modified while we're traversing it.
+	void						ValidateTree(const BodyVector &inBodies, const TrackingVector &inTracking, uint32 inNodeIndex, uint32 inNumExpectedBodies) const;
+#endif
+
+#ifdef JPH_DUMP_BROADPHASE_TREE
+	/// Dump the tree in DOT format (see: https://graphviz.org/)
+	void						DumpTree(const NodeID &inRoot, const char *inFileNamePrefix) const;
+#endif
+
+	/// Allocator that controls adding / freeing nodes
+	Allocator *					mAllocator = nullptr;
+
+	/// This is a list of nodes that must be deleted after the trees are swapped and the old tree is no longer in use
+	Allocator::Batch			mFreeNodeBatch;
+
+	/// Number of bodies currently in the tree
+	/// This is aligned to be in a different cache line from the `Allocator` pointer to prevent cross-thread syncs
+	/// when reading nodes.
+	alignas(JPH_CACHE_LINE_SIZE) atomic<uint32> mNumBodies { 0 };
+
+	/// We alternate between two tree root nodes. When updating, we activate the new tree and we keep the old tree alive.
+	/// for queries that are in progress until the next time DiscardOldTree() is called.
+	RootNode					mRootNode[2];
+	atomic<uint32>				mRootNodeIndex { 0 };
+
+	/// Flag to keep track of changes to the broadphase, if false, we don't need to UpdatePrepare/Finalize()
+	atomic<bool>				mIsDirty = false;
+
+#ifdef JPH_TRACK_BROADPHASE_STATS
+	/// Mutex protecting the various LayerToStats members
+	mutable Mutex				mStatsMutex;
+
+	struct Stat
+	{
+		uint64					mNumQueries = 0;
+		uint64					mNodesVisited = 0;
+		uint64					mBodiesVisited = 0;
+		uint64					mHitsReported = 0;
+		uint64					mTotalTicks = 0;
+		uint64					mCollectorTicks = 0;
+	};
+
+	using LayerToStats = UnorderedMap<String, Stat>;
+
+	/// Sum up all the ticks in a layer
+	uint64						GetTicks100Pct(const LayerToStats &inLayer) const;
+
+	/// Trace the stats of a single query type to the TTY
+	void						ReportStats(const char *inName, const LayerToStats &inLayer, uint64 inTicks100Pct) const;
+
+	mutable LayerToStats		mCastRayStats;
+	mutable LayerToStats		mCollideAABoxStats;
+	mutable LayerToStats		mCollideSphereStats;
+	mutable LayerToStats		mCollidePointStats;
+	mutable LayerToStats		mCollideOrientedBoxStats;
+	mutable LayerToStats		mCastAABoxStats;
+#endif // JPH_TRACK_BROADPHASE_STATS
+
+	/// Debug function to get the depth of the tree from node inNodeID
+	uint						GetMaxTreeDepth(const NodeID &inNodeID) const;
+
+	/// Walk the node tree calling the Visitor::VisitNodes for each node encountered and Visitor::VisitBody for each body encountered
+	template <class Visitor>
+	JPH_INLINE void				WalkTree(const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking, Visitor &ioVisitor JPH_IF_TRACK_BROADPHASE_STATS(, LayerToStats &ioStats)) const;
+
+#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED)
+	/// Name of this tree for debugging purposes
+	const char *				mName = "Layer";
+#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED
+};
+
+JPH_NAMESPACE_END

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

@@ -0,0 +1,35 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Collision/CollisionGroup.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(CollisionGroup)
+{
+	JPH_ADD_ATTRIBUTE(CollisionGroup, mGroupFilter)
+	JPH_ADD_ATTRIBUTE(CollisionGroup, mGroupID)
+	JPH_ADD_ATTRIBUTE(CollisionGroup, mSubGroupID)
+}
+
+const CollisionGroup CollisionGroup::sInvalid;
+
+void CollisionGroup::SaveBinaryState(StreamOut &inStream) const
+{
+	inStream.Write(mGroupID);
+	inStream.Write(mSubGroupID);
+}
+
+void CollisionGroup::RestoreBinaryState(StreamIn &inStream)
+{
+	inStream.Read(mGroupID);
+	inStream.Read(mSubGroupID);
+}
+
+JPH_NAMESPACE_END

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

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

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

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

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

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

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

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

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

@@ -0,0 +1,32 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Collision/GroupFilter.h>
+#include <Jolt/Core/StreamUtils.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT_BASE(GroupFilter)
+{
+	JPH_ADD_BASE_CLASS(GroupFilter, SerializableObject)
+}
+
+void GroupFilter::SaveBinaryState(StreamOut &inStream) const
+{
+	inStream.Write(GetRTTI()->GetHash());
+}
+
+void GroupFilter::RestoreBinaryState(StreamIn &inStream)
+{
+	// RTTI hash is read in sRestoreFromBinaryState
+}
+
+GroupFilter::GroupFilterResult GroupFilter::sRestoreFromBinaryState(StreamIn &inStream)
+{
+	return StreamUtils::RestoreObject<GroupFilter>(inStream, &GroupFilter::RestoreBinaryState);
+}
+
+JPH_NAMESPACE_END

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

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

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

@@ -0,0 +1,38 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Collision/GroupFilterTable.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(GroupFilterTable)
+{
+	JPH_ADD_BASE_CLASS(GroupFilterTable, GroupFilter)
+
+	JPH_ADD_ATTRIBUTE(GroupFilterTable, mNumSubGroups)
+	JPH_ADD_ATTRIBUTE(GroupFilterTable, mTable)
+}
+
+void GroupFilterTable::SaveBinaryState(StreamOut &inStream) const
+{
+	GroupFilter::SaveBinaryState(inStream);
+
+	inStream.Write(mNumSubGroups);
+	inStream.Write(mTable);
+}
+
+void GroupFilterTable::RestoreBinaryState(StreamIn &inStream)
+{
+	GroupFilter::RestoreBinaryState(inStream);
+
+	inStream.Read(mNumSubGroups);
+	inStream.Read(mTable);
+}
+
+JPH_NAMESPACE_END

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

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

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

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

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

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

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

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

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

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

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

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

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

@@ -0,0 +1,62 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Collision/NarrowPhaseStats.h>
+
+#ifdef JPH_TRACK_NARROWPHASE_STATS
+
+JPH_NAMESPACE_BEGIN
+
+NarrowPhaseStat	NarrowPhaseStat::sCollideShape[NumSubShapeTypes][NumSubShapeTypes];
+NarrowPhaseStat	NarrowPhaseStat::sCastShape[NumSubShapeTypes][NumSubShapeTypes];
+
+thread_local TrackNarrowPhaseStat *TrackNarrowPhaseStat::sRoot = nullptr;
+
+void NarrowPhaseStat::ReportStats(const char *inName, EShapeSubType inType1, EShapeSubType inType2, uint64 inTicks100Pct) const
+{
+	double total_pct = 100.0 * double(mTotalTicks) / double(inTicks100Pct);
+	double total_pct_excl_children = 100.0 * double(mTotalTicks - mChildTicks) / double(inTicks100Pct);
+
+	std::stringstream str;
+	str << inName << ", " << sSubShapeTypeNames[(int)inType1] << ", " << sSubShapeTypeNames[(int)inType2] << ", " << mNumQueries << ", " << total_pct << ", " << total_pct_excl_children << ", " << total_pct_excl_children / mNumQueries << ", " << mHitsReported;
+	Trace(str.str().c_str());
+}
+
+void NarrowPhaseStat::sReportStats()
+{
+	Trace("Query Type, Shape Type 1, Shape Type 2, Num Queries, Total Time (%%), Total Time Excl Children (%%), Total Time Excl. Children / Query (%%), Hits Reported");
+
+	uint64 total_ticks = 0;
+	for (EShapeSubType t1 : sAllSubShapeTypes)
+		for (EShapeSubType t2 : sAllSubShapeTypes)
+		{
+			const NarrowPhaseStat &collide_stat = sCollideShape[(int)t1][(int)t2];
+			total_ticks += collide_stat.mTotalTicks - collide_stat.mChildTicks;
+
+			const NarrowPhaseStat &cast_stat = sCastShape[(int)t1][(int)t2];
+			total_ticks += cast_stat.mTotalTicks - cast_stat.mChildTicks;
+		}
+
+	for (EShapeSubType t1 : sAllSubShapeTypes)
+		for (EShapeSubType t2 : sAllSubShapeTypes)
+		{
+			const NarrowPhaseStat &stat = sCollideShape[(int)t1][(int)t2];
+			if (stat.mNumQueries > 0)
+				stat.ReportStats("CollideShape", t1, t2, total_ticks);
+		}
+
+	for (EShapeSubType t1 : sAllSubShapeTypes)
+		for (EShapeSubType t2 : sAllSubShapeTypes)
+		{
+			const NarrowPhaseStat &stat = sCastShape[(int)t1][(int)t2];
+			if (stat.mNumQueries > 0)
+				stat.ReportStats("CastShape", t1, t2, total_ticks);
+		}
+}
+
+JPH_NAMESPACE_END
+
+#endif // JPH_TRACK_NARROWPHASE_STATS

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

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

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

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

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

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

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

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

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

@@ -0,0 +1,35 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Collision/PhysicsMaterial.h>
+#include <Jolt/Physics/Collision/PhysicsMaterialSimple.h>
+#include <Jolt/Core/StreamUtils.h>
+
+JPH_NAMESPACE_BEGIN
+
+RefConst<PhysicsMaterial> PhysicsMaterial::sDefault;
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(PhysicsMaterial)
+{
+	JPH_ADD_BASE_CLASS(PhysicsMaterial, SerializableObject)
+}
+
+void PhysicsMaterial::SaveBinaryState(StreamOut &inStream) const
+{
+	inStream.Write(GetRTTI()->GetHash());
+}
+
+void PhysicsMaterial::RestoreBinaryState(StreamIn &inStream)
+{
+	// RTTI hash is read in sRestoreFromBinaryState
+}
+
+PhysicsMaterial::PhysicsMaterialResult PhysicsMaterial::sRestoreFromBinaryState(StreamIn &inStream)
+{
+	return StreamUtils::RestoreObject<PhysicsMaterial>(inStream, &PhysicsMaterial::RestoreBinaryState);
+}
+
+JPH_NAMESPACE_END

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

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

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

@@ -0,0 +1,38 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Collision/PhysicsMaterialSimple.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(PhysicsMaterialSimple)
+{
+	JPH_ADD_BASE_CLASS(PhysicsMaterialSimple, PhysicsMaterial)
+
+	JPH_ADD_ATTRIBUTE(PhysicsMaterialSimple, mDebugName)
+	JPH_ADD_ATTRIBUTE(PhysicsMaterialSimple, mDebugColor)
+}
+
+void PhysicsMaterialSimple::SaveBinaryState(StreamOut &inStream) const
+{
+	PhysicsMaterial::SaveBinaryState(inStream);
+
+	inStream.Write(mDebugName);
+	inStream.Write(mDebugColor);
+}
+
+void PhysicsMaterialSimple::RestoreBinaryState(StreamIn &inStream)
+{
+	PhysicsMaterial::RestoreBinaryState(inStream);
+
+	inStream.Read(mDebugName);
+	inStream.Read(mDebugColor);
+}
+
+JPH_NAMESPACE_END

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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