initial commit, 4.5 stable

thirdparty/jolt_physics/Jolt/Physics/Constraints/CalculateSolverSteps.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/PhysicsSettings.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Class used to calculate the total number of velocity and position steps
+class CalculateSolverSteps
+{
+public:
+	/// Constructor
+	JPH_INLINE explicit			CalculateSolverSteps(const PhysicsSettings &inSettings) : mSettings(inSettings) { }
+
+	/// Combine the number of velocity and position steps for this body/constraint with the current values
+	template <class Type>
+	JPH_INLINE void				operator () (const Type *inObject)
+	{
+		uint num_velocity_steps = inObject->GetNumVelocityStepsOverride();
+		mNumVelocitySteps = max(mNumVelocitySteps, num_velocity_steps);
+		mApplyDefaultVelocity |= num_velocity_steps == 0;
+
+		uint num_position_steps = inObject->GetNumPositionStepsOverride();
+		mNumPositionSteps = max(mNumPositionSteps, num_position_steps);
+		mApplyDefaultPosition |= num_position_steps == 0;
+	}
+
+	/// Must be called after all bodies/constraints have been processed
+	JPH_INLINE void				Finalize()
+	{
+		// If we have a default velocity/position step count, take the max of the default and the overrides
+		if (mApplyDefaultVelocity)
+			mNumVelocitySteps = max(mNumVelocitySteps, mSettings.mNumVelocitySteps);
+		if (mApplyDefaultPosition)
+			mNumPositionSteps = max(mNumPositionSteps, mSettings.mNumPositionSteps);
+	}
+
+	/// Get the results of the calculation
+	JPH_INLINE uint				GetNumPositionSteps() const					{ return mNumPositionSteps; }
+	JPH_INLINE uint				GetNumVelocitySteps() const					{ return mNumVelocitySteps; }
+
+private:
+	const PhysicsSettings &		mSettings;
+
+	uint						mNumVelocitySteps = 0;
+	uint						mNumPositionSteps = 0;
+
+	bool						mApplyDefaultVelocity = false;
+	bool						mApplyDefaultPosition = false;
+};
+
+/// Dummy class to replace the steps calculator when we don't need the result
+class DummyCalculateSolverSteps
+{
+public:
+	template <class Type>
+	JPH_INLINE void				operator () (const Type *) const
+	{
+		/* Nothing to do */
+	}
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Constraints/ConeConstraint.cpp

@@ -0,0 +1,246 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Constraints/ConeConstraint.h>
+#include <Jolt/Physics/Body/Body.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(ConeConstraintSettings)
+{
+	JPH_ADD_BASE_CLASS(ConeConstraintSettings, TwoBodyConstraintSettings)
+
+	JPH_ADD_ENUM_ATTRIBUTE(ConeConstraintSettings, mSpace)
+	JPH_ADD_ATTRIBUTE(ConeConstraintSettings, mPoint1)
+	JPH_ADD_ATTRIBUTE(ConeConstraintSettings, mTwistAxis1)
+	JPH_ADD_ATTRIBUTE(ConeConstraintSettings, mPoint2)
+	JPH_ADD_ATTRIBUTE(ConeConstraintSettings, mTwistAxis2)
+	JPH_ADD_ATTRIBUTE(ConeConstraintSettings, mHalfConeAngle)
+}
+
+void ConeConstraintSettings::SaveBinaryState(StreamOut &inStream) const
+{
+	ConstraintSettings::SaveBinaryState(inStream);
+
+	inStream.Write(mSpace);
+	inStream.Write(mPoint1);
+	inStream.Write(mTwistAxis1);
+	inStream.Write(mPoint2);
+	inStream.Write(mTwistAxis2);
+	inStream.Write(mHalfConeAngle);
+}
+
+void ConeConstraintSettings::RestoreBinaryState(StreamIn &inStream)
+{
+	ConstraintSettings::RestoreBinaryState(inStream);
+
+	inStream.Read(mSpace);
+	inStream.Read(mPoint1);
+	inStream.Read(mTwistAxis1);
+	inStream.Read(mPoint2);
+	inStream.Read(mTwistAxis2);
+	inStream.Read(mHalfConeAngle);
+}
+
+TwoBodyConstraint *ConeConstraintSettings::Create(Body &inBody1, Body &inBody2) const
+{
+	return new ConeConstraint(inBody1, inBody2, *this);
+}
+
+ConeConstraint::ConeConstraint(Body &inBody1, Body &inBody2, const ConeConstraintSettings &inSettings) :
+	TwoBodyConstraint(inBody1, inBody2, inSettings)
+{
+	// Store limits
+	SetHalfConeAngle(inSettings.mHalfConeAngle);
+
+	// Initialize rotation axis to perpendicular of twist axis in case the angle between the twist axis is 0 in the first frame
+	mWorldSpaceRotationAxis = inSettings.mTwistAxis1.GetNormalizedPerpendicular();
+
+	if (inSettings.mSpace == EConstraintSpace::WorldSpace)
+	{
+		// If all properties were specified in world space, take them to local space now
+		RMat44 inv_transform1 = inBody1.GetInverseCenterOfMassTransform();
+		mLocalSpacePosition1 = Vec3(inv_transform1 * inSettings.mPoint1);
+		mLocalSpaceTwistAxis1 = inv_transform1.Multiply3x3(inSettings.mTwistAxis1);
+
+		RMat44 inv_transform2 = inBody2.GetInverseCenterOfMassTransform();
+		mLocalSpacePosition2 = Vec3(inv_transform2 * inSettings.mPoint2);
+		mLocalSpaceTwistAxis2 = inv_transform2.Multiply3x3(inSettings.mTwistAxis2);
+	}
+	else
+	{
+		// Properties already in local space
+		mLocalSpacePosition1 = Vec3(inSettings.mPoint1);
+		mLocalSpacePosition2 = Vec3(inSettings.mPoint2);
+		mLocalSpaceTwistAxis1 = inSettings.mTwistAxis1;
+		mLocalSpaceTwistAxis2 = inSettings.mTwistAxis2;
+
+		// If they were in local space, we need to take the initial rotation axis to world space
+		mWorldSpaceRotationAxis = inBody1.GetRotation() * mWorldSpaceRotationAxis;
+	}
+}
+
+void ConeConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM)
+{
+	if (mBody1->GetID() == inBodyID)
+		mLocalSpacePosition1 -= inDeltaCOM;
+	else if (mBody2->GetID() == inBodyID)
+		mLocalSpacePosition2 -= inDeltaCOM;
+}
+
+void ConeConstraint::CalculateRotationConstraintProperties(Mat44Arg inRotation1, Mat44Arg inRotation2)
+{
+	// Rotation is along the cross product of both twist axis
+	Vec3 twist1 = inRotation1.Multiply3x3(mLocalSpaceTwistAxis1);
+	Vec3 twist2 = inRotation2.Multiply3x3(mLocalSpaceTwistAxis2);
+
+	// Calculate dot product between twist axis, if it's smaller than the cone angle we need to correct
+	mCosTheta = twist1.Dot(twist2);
+	if (mCosTheta < mCosHalfConeAngle)
+	{
+		// Rotation axis is defined by the two twist axis
+		Vec3 rot_axis = twist2.Cross(twist1);
+
+		// If we can't find a rotation axis because the twist is too small, we'll use last frame's rotation axis
+		float len = rot_axis.Length();
+		if (len > 0.0f)
+			mWorldSpaceRotationAxis = rot_axis / len;
+
+		mAngleConstraintPart.CalculateConstraintProperties(*mBody1, *mBody2, mWorldSpaceRotationAxis);
+	}
+	else
+		mAngleConstraintPart.Deactivate();
+}
+
+void ConeConstraint::SetupVelocityConstraint(float inDeltaTime)
+{
+	Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation());
+	Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation());
+	mPointConstraintPart.CalculateConstraintProperties(*mBody1, rotation1, mLocalSpacePosition1, *mBody2, rotation2, mLocalSpacePosition2);
+	CalculateRotationConstraintProperties(rotation1, rotation2);
+}
+
+void ConeConstraint::ResetWarmStart()
+{
+	mPointConstraintPart.Deactivate();
+	mAngleConstraintPart.Deactivate();
+}
+
+void ConeConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio)
+{
+	// Warm starting: Apply previous frame impulse
+	mPointConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio);
+	mAngleConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio);
+}
+
+bool ConeConstraint::SolveVelocityConstraint(float inDeltaTime)
+{
+	bool pos = mPointConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2);
+
+	bool rot = false;
+	if (mAngleConstraintPart.IsActive())
+		rot = mAngleConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mWorldSpaceRotationAxis, 0, FLT_MAX);
+
+	return pos || rot;
+}
+
+bool ConeConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte)
+{
+	mPointConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(mBody1->GetRotation()), mLocalSpacePosition1, *mBody2, Mat44::sRotation(mBody2->GetRotation()), mLocalSpacePosition2);
+	bool pos = mPointConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, inBaumgarte);
+
+	bool rot = false;
+	CalculateRotationConstraintProperties(Mat44::sRotation(mBody1->GetRotation()), Mat44::sRotation(mBody2->GetRotation()));
+	if (mAngleConstraintPart.IsActive())
+		rot = mAngleConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mCosTheta - mCosHalfConeAngle, inBaumgarte);
+
+	return pos || rot;
+}
+
+#ifdef JPH_DEBUG_RENDERER
+void ConeConstraint::DrawConstraint(DebugRenderer *inRenderer) const
+{
+	RMat44 transform1 = mBody1->GetCenterOfMassTransform();
+	RMat44 transform2 = mBody2->GetCenterOfMassTransform();
+
+	RVec3 p1 = transform1 * mLocalSpacePosition1;
+	RVec3 p2 = transform2 * mLocalSpacePosition2;
+
+	// Draw constraint
+	inRenderer->DrawMarker(p1, Color::sRed, 0.1f);
+	inRenderer->DrawMarker(p2, Color::sGreen, 0.1f);
+
+	// Draw twist axis
+	inRenderer->DrawLine(p1, p1 + mDrawConstraintSize * transform1.Multiply3x3(mLocalSpaceTwistAxis1), Color::sRed);
+	inRenderer->DrawLine(p2, p2 + mDrawConstraintSize * transform2.Multiply3x3(mLocalSpaceTwistAxis2), Color::sGreen);
+}
+
+void ConeConstraint::DrawConstraintLimits(DebugRenderer *inRenderer) const
+{
+	// Get constraint properties in world space
+	RMat44 transform1 = mBody1->GetCenterOfMassTransform();
+	RVec3 position1 = transform1 * mLocalSpacePosition1;
+	Vec3 twist_axis1 = transform1.Multiply3x3(mLocalSpaceTwistAxis1);
+	Vec3 normal_axis1 = transform1.Multiply3x3(mLocalSpaceTwistAxis1.GetNormalizedPerpendicular());
+
+	inRenderer->DrawOpenCone(position1, twist_axis1, normal_axis1, ACos(mCosHalfConeAngle), mDrawConstraintSize * mCosHalfConeAngle, Color::sPurple, DebugRenderer::ECastShadow::Off);
+}
+#endif // JPH_DEBUG_RENDERER
+
+void ConeConstraint::SaveState(StateRecorder &inStream) const
+{
+	TwoBodyConstraint::SaveState(inStream);
+
+	mPointConstraintPart.SaveState(inStream);
+	mAngleConstraintPart.SaveState(inStream);
+	inStream.Write(mWorldSpaceRotationAxis); // When twist is too small, the rotation is used from last frame so we need to store it
+}
+
+void ConeConstraint::RestoreState(StateRecorder &inStream)
+{
+	TwoBodyConstraint::RestoreState(inStream);
+
+	mPointConstraintPart.RestoreState(inStream);
+	mAngleConstraintPart.RestoreState(inStream);
+	inStream.Read(mWorldSpaceRotationAxis);
+}
+
+Ref<ConstraintSettings> ConeConstraint::GetConstraintSettings() const
+{
+	ConeConstraintSettings *settings = new ConeConstraintSettings;
+	ToConstraintSettings(*settings);
+	settings->mSpace = EConstraintSpace::LocalToBodyCOM;
+	settings->mPoint1 = RVec3(mLocalSpacePosition1);
+	settings->mTwistAxis1 = mLocalSpaceTwistAxis1;
+	settings->mPoint2 = RVec3(mLocalSpacePosition2);
+	settings->mTwistAxis2 = mLocalSpaceTwistAxis2;
+	settings->mHalfConeAngle = ACos(mCosHalfConeAngle);
+	return settings;
+}
+
+Mat44 ConeConstraint::GetConstraintToBody1Matrix() const
+{
+	Vec3 perp = mLocalSpaceTwistAxis1.GetNormalizedPerpendicular();
+	Vec3 perp2 = mLocalSpaceTwistAxis1.Cross(perp);
+	return Mat44(Vec4(mLocalSpaceTwistAxis1, 0), Vec4(perp, 0), Vec4(perp2, 0), Vec4(mLocalSpacePosition1, 1));
+}
+
+Mat44 ConeConstraint::GetConstraintToBody2Matrix() const
+{
+	// Note: Incorrect in rotation around the twist axis (the perpendicular does not match that of body 1),
+	// this should not matter as we're not limiting rotation around the twist axis.
+	Vec3 perp = mLocalSpaceTwistAxis2.GetNormalizedPerpendicular();
+	Vec3 perp2 = mLocalSpaceTwistAxis2.Cross(perp);
+	return Mat44(Vec4(mLocalSpaceTwistAxis2, 0), Vec4(perp, 0), Vec4(perp2, 0), Vec4(mLocalSpacePosition2, 1));
+}
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Constraints/ConeConstraint.h

@@ -0,0 +1,133 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Constraints/TwoBodyConstraint.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/PointConstraintPart.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/AngleConstraintPart.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Cone constraint settings, used to create a cone constraint
+class JPH_EXPORT ConeConstraintSettings final : public TwoBodyConstraintSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, ConeConstraintSettings)
+
+public:
+	// See: ConstraintSettings::SaveBinaryState
+	virtual void				SaveBinaryState(StreamOut &inStream) const override;
+
+	/// Create an instance of this constraint
+	virtual TwoBodyConstraint *	Create(Body &inBody1, Body &inBody2) const override;
+
+	/// This determines in which space the constraint is setup, all properties below should be in the specified space
+	EConstraintSpace			mSpace = EConstraintSpace::WorldSpace;
+
+	/// Body 1 constraint reference frame (space determined by mSpace)
+	RVec3						mPoint1 = RVec3::sZero();
+	Vec3						mTwistAxis1 = Vec3::sAxisX();
+
+	/// Body 2 constraint reference frame (space determined by mSpace)
+	RVec3						mPoint2 = RVec3::sZero();
+	Vec3						mTwistAxis2 = Vec3::sAxisX();
+
+	/// Half of maximum angle between twist axis of body 1 and 2
+	float						mHalfConeAngle = 0.0f;
+
+protected:
+	// See: ConstraintSettings::RestoreBinaryState
+	virtual void				RestoreBinaryState(StreamIn &inStream) override;
+};
+
+/// A cone constraint constraints 2 bodies to a single point and limits the swing between the twist axis within a cone:
+///
+/// t1 . t2 <= cos(theta)
+///
+/// Where:
+///
+/// t1 = twist axis of body 1.
+/// t2 = twist axis of body 2.
+/// theta = half cone angle (angle from the principal axis of the cone to the edge).
+///
+/// Calculating the Jacobian:
+///
+/// Constraint equation:
+///
+/// C = t1 . t2 - cos(theta)
+///
+/// Derivative:
+///
+/// d/dt C = d/dt (t1 . t2) = (d/dt t1) . t2 + t1 . (d/dt t2) = (w1 x t1) . t2 + t1 . (w2 x t2) = (t1 x t2) . w1 + (t2 x t1) . w2
+///
+/// d/dt C = J v = [0, -t2 x t1, 0, t2 x t1] [v1, w1, v2, w2]
+///
+/// Where J is the Jacobian.
+///
+/// Note that this is the exact same equation as used in AngleConstraintPart if we use t2 x t1 as the world space axis
+class JPH_EXPORT ConeConstraint final : public TwoBodyConstraint
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Construct cone constraint
+								ConeConstraint(Body &inBody1, Body &inBody2, const ConeConstraintSettings &inSettings);
+
+	// Generic interface of a constraint
+	virtual EConstraintSubType	GetSubType() const override					{ return EConstraintSubType::Cone; }
+	virtual void				NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override;
+	virtual void				SetupVelocityConstraint(float inDeltaTime) override;
+	virtual void				ResetWarmStart() override;
+	virtual void				WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override;
+	virtual bool				SolveVelocityConstraint(float inDeltaTime) override;
+	virtual bool				SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override;
+#ifdef JPH_DEBUG_RENDERER
+	virtual void				DrawConstraint(DebugRenderer *inRenderer) const override;
+	virtual void				DrawConstraintLimits(DebugRenderer *inRenderer) const override;
+#endif // JPH_DEBUG_RENDERER
+	virtual void				SaveState(StateRecorder &inStream) const override;
+	virtual void				RestoreState(StateRecorder &inStream) override;
+	virtual Ref<ConstraintSettings> GetConstraintSettings() const override;
+
+	// See: TwoBodyConstraint
+	virtual Mat44				GetConstraintToBody1Matrix() const override;
+	virtual Mat44				GetConstraintToBody2Matrix() const override;
+
+	/// Update maximum angle between body 1 and 2 (see ConeConstraintSettings)
+	void						SetHalfConeAngle(float inHalfConeAngle)		{ JPH_ASSERT(inHalfConeAngle >= 0.0f && inHalfConeAngle <= JPH_PI); mCosHalfConeAngle = Cos(inHalfConeAngle); }
+	float						GetCosHalfConeAngle() const					{ return mCosHalfConeAngle; }
+
+	///@name Get Lagrange multiplier from last physics update (the linear/angular impulse applied to satisfy the constraint)
+	inline Vec3					GetTotalLambdaPosition() const				{ return mPointConstraintPart.GetTotalLambda(); }
+	inline float				GetTotalLambdaRotation() const				{ return mAngleConstraintPart.GetTotalLambda(); }
+
+private:
+	// Internal helper function to calculate the values below
+	void						CalculateRotationConstraintProperties(Mat44Arg inRotation1, Mat44Arg inRotation2);
+
+	// CONFIGURATION PROPERTIES FOLLOW
+
+	// Local space constraint positions
+	Vec3						mLocalSpacePosition1;
+	Vec3						mLocalSpacePosition2;
+
+	// Local space constraint axis
+	Vec3						mLocalSpaceTwistAxis1;
+	Vec3						mLocalSpaceTwistAxis2;
+
+	// Angular limits
+	float						mCosHalfConeAngle;
+
+	// RUN TIME PROPERTIES FOLLOW
+
+	// Axis and angle of rotation between the two bodies
+	Vec3						mWorldSpaceRotationAxis;
+	float						mCosTheta;
+
+	// The constraint parts
+	PointConstraintPart			mPointConstraintPart;
+	AngleConstraintPart			mAngleConstraintPart;
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Constraints/Constraint.cpp

@@ -0,0 +1,73 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Constraints/Constraint.h>
+#include <Jolt/Physics/StateRecorder.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#include <Jolt/Core/StreamUtils.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(ConstraintSettings)
+{
+	JPH_ADD_BASE_CLASS(ConstraintSettings, SerializableObject)
+
+	JPH_ADD_ATTRIBUTE(ConstraintSettings, mEnabled)
+	JPH_ADD_ATTRIBUTE(ConstraintSettings, mDrawConstraintSize)
+	JPH_ADD_ATTRIBUTE(ConstraintSettings, mConstraintPriority)
+	JPH_ADD_ATTRIBUTE(ConstraintSettings, mNumVelocityStepsOverride)
+	JPH_ADD_ATTRIBUTE(ConstraintSettings, mNumPositionStepsOverride)
+	JPH_ADD_ATTRIBUTE(ConstraintSettings, mUserData)
+}
+
+void ConstraintSettings::SaveBinaryState(StreamOut &inStream) const
+{
+	inStream.Write(GetRTTI()->GetHash());
+	inStream.Write(mEnabled);
+	inStream.Write(mDrawConstraintSize);
+	inStream.Write(mConstraintPriority);
+	inStream.Write(mNumVelocityStepsOverride);
+	inStream.Write(mNumPositionStepsOverride);
+}
+
+void ConstraintSettings::RestoreBinaryState(StreamIn &inStream)
+{
+	// Type hash read by sRestoreFromBinaryState
+	inStream.Read(mEnabled);
+	inStream.Read(mDrawConstraintSize);
+	inStream.Read(mConstraintPriority);
+	inStream.Read(mNumVelocityStepsOverride);
+	inStream.Read(mNumPositionStepsOverride);
+}
+
+ConstraintSettings::ConstraintResult ConstraintSettings::sRestoreFromBinaryState(StreamIn &inStream)
+{
+	return StreamUtils::RestoreObject<ConstraintSettings>(inStream, &ConstraintSettings::RestoreBinaryState);
+}
+
+void Constraint::SaveState(StateRecorder &inStream) const
+{
+	inStream.Write(mEnabled);
+}
+
+void Constraint::RestoreState(StateRecorder &inStream)
+{
+	inStream.Read(mEnabled);
+}
+
+void Constraint::ToConstraintSettings(ConstraintSettings &outSettings) const
+{
+	outSettings.mEnabled = mEnabled;
+	outSettings.mConstraintPriority = mConstraintPriority;
+	outSettings.mNumVelocityStepsOverride = mNumVelocityStepsOverride;
+	outSettings.mNumPositionStepsOverride = mNumPositionStepsOverride;
+	outSettings.mUserData = mUserData;
+#ifdef JPH_DEBUG_RENDERER
+	outSettings.mDrawConstraintSize = mDrawConstraintSize;
+#endif // JPH_DEBUG_RENDERER
+}
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Constraints/Constraint.h

@@ -0,0 +1,238 @@
+// 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/Core/Result.h>
+#include <Jolt/ObjectStream/SerializableObject.h>
+
+JPH_NAMESPACE_BEGIN
+
+class BodyID;
+class IslandBuilder;
+class LargeIslandSplitter;
+class BodyManager;
+class StateRecorder;
+class StreamIn;
+class StreamOut;
+#ifdef JPH_DEBUG_RENDERER
+class DebugRenderer;
+#endif // JPH_DEBUG_RENDERER
+
+/// Enum to identify constraint type
+enum class EConstraintType
+{
+	Constraint,
+	TwoBodyConstraint,
+};
+
+/// Enum to identify constraint sub type
+enum class EConstraintSubType
+{
+	Fixed,
+	Point,
+	Hinge,
+	Slider,
+	Distance,
+	Cone,
+	SwingTwist,
+	SixDOF,
+	Path,
+	Vehicle,
+	RackAndPinion,
+	Gear,
+	Pulley,
+
+	/// User defined constraint types start here
+	User1,
+	User2,
+	User3,
+	User4
+};
+
+/// Certain constraints support setting them up in local or world space. This governs what is used.
+enum class EConstraintSpace
+{
+	LocalToBodyCOM,				///< All constraint properties are specified in local space to center of mass of the bodies that are being constrained (so e.g. 'constraint position 1' will be local to body 1 COM, 'constraint position 2' will be local to body 2 COM). Note that this means you need to subtract Shape::GetCenterOfMass() from positions!
+	WorldSpace,					///< All constraint properties are specified in world space
+};
+
+/// Class used to store the configuration of a constraint. Allows run-time creation of constraints.
+class JPH_EXPORT ConstraintSettings : public SerializableObject, public RefTarget<ConstraintSettings>
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, ConstraintSettings)
+
+public:
+	using ConstraintResult = Result<Ref<ConstraintSettings>>;
+
+	/// Saves the contents of the constraint settings in binary form to inStream.
+	virtual void				SaveBinaryState(StreamOut &inStream) const;
+
+	/// Creates a constraint of the correct type and restores its contents from the binary stream inStream.
+	static ConstraintResult		sRestoreFromBinaryState(StreamIn &inStream);
+
+	/// If this constraint is enabled initially. Use Constraint::SetEnabled to toggle after creation.
+	bool						mEnabled = true;
+
+	/// Priority of the constraint when solving. Higher numbers have are more likely to be solved correctly.
+	/// Note that if you want a deterministic simulation and you cannot guarantee the order in which constraints are added/removed, you can make the priority for all constraints unique to get a deterministic ordering.
+	uint32						mConstraintPriority = 0;
+
+	/// Used only when the constraint is active. 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						mNumVelocityStepsOverride = 0;
+
+	/// Used only when the constraint is active. 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.
+	uint						mNumPositionStepsOverride = 0;
+
+	/// Size of constraint when drawing it through the debug renderer
+	float						mDrawConstraintSize = 1.0f;
+
+	/// User data value (can be used by application)
+	uint64						mUserData = 0;
+
+protected:
+	/// This function should not be called directly, it is used by sRestoreFromBinaryState.
+	virtual void				RestoreBinaryState(StreamIn &inStream);
+};
+
+/// Base class for all physics constraints. A constraint removes one or more degrees of freedom for a rigid body.
+class JPH_EXPORT Constraint : public RefTarget<Constraint>, public NonCopyable
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+	explicit					Constraint(const ConstraintSettings &inSettings) :
+#ifdef JPH_DEBUG_RENDERER
+		mDrawConstraintSize(inSettings.mDrawConstraintSize),
+#endif // JPH_DEBUG_RENDERER
+		mConstraintPriority(inSettings.mConstraintPriority),
+		mNumVelocityStepsOverride(uint8(inSettings.mNumVelocityStepsOverride)),
+		mNumPositionStepsOverride(uint8(inSettings.mNumPositionStepsOverride)),
+		mEnabled(inSettings.mEnabled),
+		mUserData(inSettings.mUserData)
+	{
+		JPH_ASSERT(inSettings.mNumVelocityStepsOverride < 256);
+		JPH_ASSERT(inSettings.mNumPositionStepsOverride < 256);
+	}
+
+	/// Virtual destructor
+	virtual						~Constraint() = default;
+
+	/// Get the type of a constraint
+	virtual EConstraintType		GetType() const								{ return EConstraintType::Constraint; }
+
+	/// Get the sub type of a constraint
+	virtual EConstraintSubType	GetSubType() const = 0;
+
+	/// Priority of the constraint when solving. Higher numbers have are more likely to be solved correctly.
+	/// Note that if you want a deterministic simulation and you cannot guarantee the order in which constraints are added/removed, you can make the priority for all constraints unique to get a deterministic ordering.
+	uint32						GetConstraintPriority() const				{ return mConstraintPriority; }
+	void						SetConstraintPriority(uint32 inPriority)	{ mConstraintPriority = inPriority; }
+
+	/// Used only when the constraint is active. 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 the constraint is active. 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; }
+
+	/// Enable / disable this constraint. This can e.g. be used to implement a breakable constraint by detecting that the constraint impulse
+	/// (see e.g. PointConstraint::GetTotalLambdaPosition) went over a certain limit and then disabling the constraint.
+	/// Note that although a disabled constraint will not affect the simulation in any way anymore, it does incur some processing overhead.
+	/// Alternatively you can remove a constraint from the constraint manager (which may be more costly if you want to disable the constraint for a short while).
+	void						SetEnabled(bool inEnabled)					{ mEnabled = inEnabled; }
+
+	/// Test if a constraint is enabled.
+	bool						GetEnabled() const							{ return mEnabled; }
+
+	/// Access to the user data, can be used for anything by the application
+	uint64						GetUserData() const							{ return mUserData; }
+	void						SetUserData(uint64 inUserData)				{ mUserData = inUserData; }
+
+	/// Notify the constraint that the shape of a body has changed and that its center of mass has moved by inDeltaCOM.
+	/// Bodies don't know which constraints are connected to them so the user is responsible for notifying the relevant constraints when a body changes.
+	/// @param inBodyID ID of the body that has changed
+	/// @param inDeltaCOM The delta of the center of mass of the body (shape->GetCenterOfMass() - shape_before_change->GetCenterOfMass())
+	virtual void				NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) = 0;
+
+	/// Notify the system that the configuration of the bodies and/or constraint has changed enough so that the warm start impulses should not be applied the next frame.
+	/// You can use this function for example when repositioning a ragdoll through Ragdoll::SetPose in such a way that the orientation of the bodies completely changes so that
+	/// the previous frame impulses are no longer a good approximation of what the impulses will be in the next frame. Calling this function when there are no big changes
+	/// will result in the constraints being much 'softer' than usual so they are more easily violated (e.g. a long chain of bodies might sag a bit if you call this every frame).
+	virtual void				ResetWarmStart() = 0;
+
+	///@name Solver interface
+	///@{
+	virtual bool				IsActive() const							{ return mEnabled; }
+	virtual void				SetupVelocityConstraint(float inDeltaTime) = 0;
+	virtual void				WarmStartVelocityConstraint(float inWarmStartImpulseRatio) = 0;
+	virtual bool				SolveVelocityConstraint(float inDeltaTime) = 0;
+	virtual bool				SolvePositionConstraint(float inDeltaTime, float inBaumgarte) = 0;
+	///@}
+
+	/// Link bodies that are connected by this constraint in the island builder
+	virtual void				BuildIslands(uint32 inConstraintIndex, IslandBuilder &ioBuilder, BodyManager &inBodyManager) = 0;
+
+	/// Link bodies that are connected by this constraint in the same split. Returns the split index.
+	virtual uint				BuildIslandSplits(LargeIslandSplitter &ioSplitter) const = 0;
+
+#ifdef JPH_DEBUG_RENDERER
+	// Drawing interface
+	virtual void				DrawConstraint(DebugRenderer *inRenderer) const = 0;
+	virtual void				DrawConstraintLimits([[maybe_unused]] DebugRenderer *inRenderer) const { }
+	virtual void				DrawConstraintReferenceFrame([[maybe_unused]] DebugRenderer *inRenderer) const { }
+
+	/// Size of constraint when drawing it through the debug renderer
+	float						GetDrawConstraintSize() const				{ return mDrawConstraintSize; }
+	void						SetDrawConstraintSize(float inSize)			{ mDrawConstraintSize = inSize; }
+#endif // JPH_DEBUG_RENDERER
+
+	/// Saving state for replay
+	virtual void				SaveState(StateRecorder &inStream) const;
+
+	/// Restoring state for replay
+	virtual void				RestoreState(StateRecorder &inStream);
+
+	/// Debug function to convert a constraint to its settings, note that this will not save to which bodies the constraint is connected to
+	virtual Ref<ConstraintSettings> GetConstraintSettings() const = 0;
+
+protected:
+	/// Helper function to copy settings back to constraint settings for this base class
+	void						ToConstraintSettings(ConstraintSettings &outSettings) const;
+
+#ifdef JPH_DEBUG_RENDERER
+	/// Size of constraint when drawing it through the debug renderer
+	float						mDrawConstraintSize;
+#endif // JPH_DEBUG_RENDERER
+
+private:
+	friend class ConstraintManager;
+
+	/// Index that indicates this constraint is not in the constraint manager
+	static constexpr uint32		cInvalidConstraintIndex = 0xffffffff;
+
+	/// Index in the mConstraints list of the ConstraintManager for easy finding
+	uint32						mConstraintIndex = cInvalidConstraintIndex;
+
+	/// Priority of the constraint when solving. Higher numbers have are more likely to be solved correctly.
+	uint32						mConstraintPriority = 0;
+
+	/// Used only when the constraint is active. 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						mNumVelocityStepsOverride = 0;
+
+	/// Used only when the constraint is active. 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.
+	uint8						mNumPositionStepsOverride = 0;
+
+	/// If this constraint is currently enabled
+	bool						mEnabled = true;
+
+	/// User data value (can be used by application)
+	uint64						mUserData;
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintManager.cpp

@@ -0,0 +1,289 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Constraints/ConstraintManager.h>
+#include <Jolt/Physics/Constraints/CalculateSolverSteps.h>
+#include <Jolt/Physics/IslandBuilder.h>
+#include <Jolt/Physics/StateRecorder.h>
+#include <Jolt/Physics/PhysicsLock.h>
+#include <Jolt/Core/Profiler.h>
+#include <Jolt/Core/QuickSort.h>
+
+JPH_NAMESPACE_BEGIN
+
+void ConstraintManager::Add(Constraint **inConstraints, int inNumber)
+{
+	UniqueLock lock(mConstraintsMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::ConstraintsList));
+
+	mConstraints.reserve(mConstraints.size() + inNumber);
+
+	for (Constraint **c = inConstraints, **c_end = inConstraints + inNumber; c < c_end; ++c)
+	{
+		Constraint *constraint = *c;
+
+		// Assume this constraint has not been added yet
+		JPH_ASSERT(constraint->mConstraintIndex == Constraint::cInvalidConstraintIndex);
+
+		// Add to the list
+		constraint->mConstraintIndex = uint32(mConstraints.size());
+		mConstraints.push_back(constraint);
+	}
+}
+
+void ConstraintManager::Remove(Constraint **inConstraints, int inNumber)
+{
+	UniqueLock lock(mConstraintsMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::ConstraintsList));
+
+	for (Constraint **c = inConstraints, **c_end = inConstraints + inNumber; c < c_end; ++c)
+	{
+		Constraint *constraint = *c;
+
+		// Reset constraint index for this constraint
+		uint32 this_constraint_idx = constraint->mConstraintIndex;
+		constraint->mConstraintIndex = Constraint::cInvalidConstraintIndex;
+		JPH_ASSERT(this_constraint_idx != Constraint::cInvalidConstraintIndex);
+
+		// Check if this constraint is somewhere in the middle of the constraints, in this case we need to move the last constraint to this position
+		uint32 last_constraint_idx = uint32(mConstraints.size() - 1);
+		if (this_constraint_idx < last_constraint_idx)
+		{
+			Constraint *last_constraint = mConstraints[last_constraint_idx];
+			last_constraint->mConstraintIndex = this_constraint_idx;
+			mConstraints[this_constraint_idx] = last_constraint;
+		}
+
+		// Pop last constraint
+		mConstraints.pop_back();
+	}
+}
+
+Constraints ConstraintManager::GetConstraints() const
+{
+	UniqueLock lock(mConstraintsMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::ConstraintsList));
+
+	Constraints copy = mConstraints;
+	return copy;
+}
+
+void ConstraintManager::GetActiveConstraints(uint32 inStartConstraintIdx, uint32 inEndConstraintIdx, Constraint **outActiveConstraints, uint32 &outNumActiveConstraints) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	JPH_ASSERT(inEndConstraintIdx <= mConstraints.size());
+
+	uint32 num_active_constraints = 0;
+	for (uint32 constraint_idx = inStartConstraintIdx; constraint_idx < inEndConstraintIdx; ++constraint_idx)
+	{
+		Constraint *c = mConstraints[constraint_idx];
+		JPH_ASSERT(c->mConstraintIndex == constraint_idx);
+		if (c->IsActive())
+		{
+			*(outActiveConstraints++) = c;
+			num_active_constraints++;
+		}
+	}
+
+	outNumActiveConstraints = num_active_constraints;
+}
+
+void ConstraintManager::sBuildIslands(Constraint **inActiveConstraints, uint32 inNumActiveConstraints, IslandBuilder &ioBuilder, BodyManager &inBodyManager)
+{
+	JPH_PROFILE_FUNCTION();
+
+	for (uint32 constraint_idx = 0; constraint_idx < inNumActiveConstraints; ++constraint_idx)
+	{
+		Constraint *c = inActiveConstraints[constraint_idx];
+		c->BuildIslands(constraint_idx, ioBuilder, inBodyManager);
+	}
+}
+
+void ConstraintManager::sSortConstraints(Constraint **inActiveConstraints, uint32 *inConstraintIdxBegin, uint32 *inConstraintIdxEnd)
+{
+	JPH_PROFILE_FUNCTION();
+
+	QuickSort(inConstraintIdxBegin, inConstraintIdxEnd, [inActiveConstraints](uint32 inLHS, uint32 inRHS) {
+		const Constraint *lhs = inActiveConstraints[inLHS];
+		const Constraint *rhs = inActiveConstraints[inRHS];
+
+		if (lhs->GetConstraintPriority() != rhs->GetConstraintPriority())
+			return lhs->GetConstraintPriority() < rhs->GetConstraintPriority();
+
+		return lhs->mConstraintIndex < rhs->mConstraintIndex;
+	});
+}
+
+void ConstraintManager::sSetupVelocityConstraints(Constraint **inActiveConstraints, uint32 inNumActiveConstraints, float inDeltaTime)
+{
+	JPH_PROFILE_FUNCTION();
+
+	for (Constraint **c = inActiveConstraints, **c_end = inActiveConstraints + inNumActiveConstraints; c < c_end; ++c)
+		(*c)->SetupVelocityConstraint(inDeltaTime);
+}
+
+template <class ConstraintCallback>
+void ConstraintManager::sWarmStartVelocityConstraints(Constraint **inActiveConstraints, const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inWarmStartImpulseRatio, ConstraintCallback &ioCallback)
+{
+	JPH_PROFILE_FUNCTION();
+
+	for (const uint32 *constraint_idx = inConstraintIdxBegin; constraint_idx < inConstraintIdxEnd; ++constraint_idx)
+	{
+		Constraint *c = inActiveConstraints[*constraint_idx];
+		ioCallback(c);
+		c->WarmStartVelocityConstraint(inWarmStartImpulseRatio);
+	}
+}
+
+// Specialize for the two constraint callback types
+template void ConstraintManager::sWarmStartVelocityConstraints<CalculateSolverSteps>(Constraint **inActiveConstraints, const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inWarmStartImpulseRatio, CalculateSolverSteps &ioCallback);
+template void ConstraintManager::sWarmStartVelocityConstraints<DummyCalculateSolverSteps>(Constraint **inActiveConstraints, const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inWarmStartImpulseRatio, DummyCalculateSolverSteps &ioCallback);
+
+bool ConstraintManager::sSolveVelocityConstraints(Constraint **inActiveConstraints, const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inDeltaTime)
+{
+	JPH_PROFILE_FUNCTION();
+
+	bool any_impulse_applied = false;
+
+	for (const uint32 *constraint_idx = inConstraintIdxBegin; constraint_idx < inConstraintIdxEnd; ++constraint_idx)
+	{
+		Constraint *c = inActiveConstraints[*constraint_idx];
+		any_impulse_applied |= c->SolveVelocityConstraint(inDeltaTime);
+	}
+
+	return any_impulse_applied;
+}
+
+bool ConstraintManager::sSolvePositionConstraints(Constraint **inActiveConstraints, const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inDeltaTime, float inBaumgarte)
+{
+	JPH_PROFILE_FUNCTION();
+
+	bool any_impulse_applied = false;
+
+	for (const uint32 *constraint_idx = inConstraintIdxBegin; constraint_idx < inConstraintIdxEnd; ++constraint_idx)
+	{
+		Constraint *c = inActiveConstraints[*constraint_idx];
+		any_impulse_applied |= c->SolvePositionConstraint(inDeltaTime, inBaumgarte);
+	}
+
+	return any_impulse_applied;
+}
+
+#ifdef JPH_DEBUG_RENDERER
+void ConstraintManager::DrawConstraints(DebugRenderer *inRenderer) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	UniqueLock lock(mConstraintsMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::ConstraintsList));
+
+	for (const Ref<Constraint> &c : mConstraints)
+		c->DrawConstraint(inRenderer);
+}
+
+void ConstraintManager::DrawConstraintLimits(DebugRenderer *inRenderer) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	UniqueLock lock(mConstraintsMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::ConstraintsList));
+
+	for (const Ref<Constraint> &c : mConstraints)
+		c->DrawConstraintLimits(inRenderer);
+}
+
+void ConstraintManager::DrawConstraintReferenceFrame(DebugRenderer *inRenderer) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	UniqueLock lock(mConstraintsMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::ConstraintsList));
+
+	for (const Ref<Constraint> &c : mConstraints)
+		c->DrawConstraintReferenceFrame(inRenderer);
+}
+#endif // JPH_DEBUG_RENDERER
+
+void ConstraintManager::SaveState(StateRecorder &inStream, const StateRecorderFilter *inFilter) const
+{
+	UniqueLock lock(mConstraintsMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::ConstraintsList));
+
+	// Write state of constraints
+	if (inFilter != nullptr)
+	{
+		// Determine which constraints to save
+		Array<Constraint *> constraints;
+		constraints.reserve(mConstraints.size());
+		for (const Ref<Constraint> &c : mConstraints)
+			if (inFilter->ShouldSaveConstraint(*c))
+				constraints.push_back(c);
+
+		// Save them
+		uint32 num_constraints = (uint32)constraints.size();
+		inStream.Write(num_constraints);
+		for (const Constraint *c : constraints)
+		{
+			inStream.Write(c->mConstraintIndex);
+			c->SaveState(inStream);
+		}
+	}
+	else
+	{
+		// Save all constraints
+		uint32 num_constraints = (uint32)mConstraints.size();
+		inStream.Write(num_constraints);
+		for (const Ref<Constraint> &c : mConstraints)
+		{
+			inStream.Write(c->mConstraintIndex);
+			c->SaveState(inStream);
+		}
+	}
+}
+
+bool ConstraintManager::RestoreState(StateRecorder &inStream)
+{
+	UniqueLock lock(mConstraintsMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::ConstraintsList));
+
+	if (inStream.IsValidating())
+	{
+		// Read state of constraints
+		uint32 num_constraints = (uint32)mConstraints.size(); // Initialize to current value for validation
+		inStream.Read(num_constraints);
+		if (num_constraints != mConstraints.size())
+		{
+			JPH_ASSERT(false, "Cannot handle adding/removing constraints");
+			return false;
+		}
+		for (const Ref<Constraint> &c : mConstraints)
+		{
+			uint32 constraint_index = c->mConstraintIndex;
+			inStream.Read(constraint_index);
+			if (constraint_index != c->mConstraintIndex)
+			{
+				JPH_ASSERT(false, "Unexpected constraint index");
+				return false;
+			}
+			c->RestoreState(inStream);
+		}
+	}
+	else
+	{
+		// Not validating, use more flexible reading, read number of constraints
+		uint32 num_constraints = 0;
+		inStream.Read(num_constraints);
+
+		for (uint32 idx = 0; idx < num_constraints; ++idx)
+		{
+			uint32 constraint_index;
+			inStream.Read(constraint_index);
+			if (mConstraints.size() <= constraint_index)
+			{
+				JPH_ASSERT(false, "Restoring state for non-existing constraint");
+				return false;
+			}
+			mConstraints[constraint_index]->RestoreState(inStream);
+		}
+	}
+
+	return true;
+}
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintManager.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/Constraints/Constraint.h>
+#include <Jolt/Physics/PhysicsLock.h>
+#include <Jolt/Core/Mutex.h>
+
+JPH_NAMESPACE_BEGIN
+
+class IslandBuilder;
+class BodyManager;
+class StateRecorderFilter;
+#ifdef JPH_DEBUG_RENDERER
+class DebugRenderer;
+#endif // JPH_DEBUG_RENDERER
+
+/// A list of constraints
+using Constraints = Array<Ref<Constraint>>;
+
+/// A constraint manager manages all constraints of the same type
+class JPH_EXPORT ConstraintManager : public NonCopyable
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+#ifdef JPH_ENABLE_ASSERTS
+	/// Constructor
+							ConstraintManager(PhysicsLockContext inContext) : mLockContext(inContext) { }
+#endif // JPH_ENABLE_ASSERTS
+
+	/// Add a new constraint. This is thread safe.
+	void					Add(Constraint **inConstraints, int inNumber);
+
+	/// Remove a constraint. This is thread safe.
+	void					Remove(Constraint **inConstraint, int inNumber);
+
+	/// Get a list of all constraints
+	Constraints				GetConstraints() const;
+
+	/// Get total number of constraints
+	inline uint32			GetNumConstraints() const					{ return uint32(mConstraints.size()); }
+
+	/// Determine the active constraints of a subset of the constraints
+	void					GetActiveConstraints(uint32 inStartConstraintIdx, uint32 inEndConstraintIdx, Constraint **outActiveConstraints, uint32 &outNumActiveConstraints) const;
+
+	/// Link bodies to form islands
+	static void				sBuildIslands(Constraint **inActiveConstraints, uint32 inNumActiveConstraints, IslandBuilder &ioBuilder, BodyManager &inBodyManager);
+
+	/// In order to have a deterministic simulation, we need to sort the constraints of an island before solving them
+	static void				sSortConstraints(Constraint **inActiveConstraints, uint32 *inConstraintIdxBegin, uint32 *inConstraintIdxEnd);
+
+	/// Prior to solving the velocity constraints, you must call SetupVelocityConstraints once to precalculate values that are independent of velocity
+	static void				sSetupVelocityConstraints(Constraint **inActiveConstraints, uint32 inNumActiveConstraints, float inDeltaTime);
+
+	/// Apply last frame's impulses, must be called prior to SolveVelocityConstraints
+	template <class ConstraintCallback>
+	static void				sWarmStartVelocityConstraints(Constraint **inActiveConstraints, const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inWarmStartImpulseRatio, ConstraintCallback &ioCallback);
+
+	/// This function is called multiple times to iteratively come to a solution that meets all velocity constraints
+	static bool				sSolveVelocityConstraints(Constraint **inActiveConstraints, const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inDeltaTime);
+
+	/// This function is called multiple times to iteratively come to a solution that meets all position constraints
+	static bool				sSolvePositionConstraints(Constraint **inActiveConstraints, const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inDeltaTime, float inBaumgarte);
+
+#ifdef JPH_DEBUG_RENDERER
+	/// Draw all constraints
+	void					DrawConstraints(DebugRenderer *inRenderer) const;
+
+	/// Draw all constraint limits
+	void					DrawConstraintLimits(DebugRenderer *inRenderer) const;
+
+	/// Draw all constraint reference frames
+	void					DrawConstraintReferenceFrame(DebugRenderer *inRenderer) const;
+#endif // JPH_DEBUG_RENDERER
+
+	/// Save state of constraints
+	void					SaveState(StateRecorder &inStream, const StateRecorderFilter *inFilter) const;
+
+	/// Restore the state of constraints. Returns false if failed.
+	bool					RestoreState(StateRecorder &inStream);
+
+	/// Lock all constraints. This should only be done during PhysicsSystem::Update().
+	void					LockAllConstraints()						{ PhysicsLock::sLock(mConstraintsMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::ConstraintsList)); }
+	void					UnlockAllConstraints()						{ PhysicsLock::sUnlock(mConstraintsMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::ConstraintsList)); }
+
+private:
+#ifdef JPH_ENABLE_ASSERTS
+	PhysicsLockContext		mLockContext;
+#endif // JPH_ENABLE_ASSERTS
+	Constraints				mConstraints;
+	mutable Mutex			mConstraintsMutex;
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/AngleConstraintPart.h

@@ -0,0 +1,257 @@
+// 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/Constraints/ConstraintPart/SpringPart.h>
+#include <Jolt/Physics/Constraints/SpringSettings.h>
+#include <Jolt/Physics/StateRecorder.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Constraint that constrains rotation along 1 axis
+///
+/// Based on: "Constraints Derivation for Rigid Body Simulation in 3D" - Daniel Chappuis, see section 2.4.5
+///
+/// Constraint equation (eq 108):
+///
+/// \f[C = \theta(t) - \theta_{min}\f]
+///
+/// Jacobian (eq 109):
+///
+/// \f[J = \begin{bmatrix}0 & -a^T & 0 & a^T\end{bmatrix}\f]
+///
+/// Used terms (here and below, everything in world space):\n
+/// a = axis around which rotation is constrained (normalized).\n
+/// x1, x2 = center of mass for the bodies.\n
+/// v = [v1, w1, v2, w2].\n
+/// v1, v2 = linear velocity of body 1 and 2.\n
+/// w1, w2 = angular velocity of body 1 and 2.\n
+/// M = mass matrix, a diagonal matrix of the mass and inertia with diagonal [m1, I1, m2, I2].\n
+/// \f$K^{-1} = \left( J M^{-1} J^T \right)^{-1}\f$ = effective mass.\n
+/// b = velocity bias.\n
+/// \f$\beta\f$ = baumgarte constant.
+class AngleConstraintPart
+{
+	/// Internal helper function to update velocities of bodies after Lagrange multiplier is calculated
+	JPH_INLINE bool				ApplyVelocityStep(Body &ioBody1, Body &ioBody2, float inLambda) const
+	{
+		// Apply impulse if delta is not zero
+		if (inLambda != 0.0f)
+		{
+			// Calculate velocity change due to constraint
+			//
+			// Impulse:
+			// P = J^T lambda
+			//
+			// Euler velocity integration:
+			// v' = v + M^-1 P
+			if (ioBody1.IsDynamic())
+				ioBody1.GetMotionProperties()->SubAngularVelocityStep(inLambda * mInvI1_Axis);
+			if (ioBody2.IsDynamic())
+				ioBody2.GetMotionProperties()->AddAngularVelocityStep(inLambda * mInvI2_Axis);
+			return true;
+		}
+
+		return false;
+	}
+
+	/// Internal helper function to calculate the inverse effective mass
+	JPH_INLINE float				CalculateInverseEffectiveMass(const Body &inBody1, const Body &inBody2, Vec3Arg inWorldSpaceAxis)
+	{
+		JPH_ASSERT(inWorldSpaceAxis.IsNormalized(1.0e-4f));
+
+		// Calculate properties used below
+		mInvI1_Axis = inBody1.IsDynamic()? inBody1.GetMotionProperties()->MultiplyWorldSpaceInverseInertiaByVector(inBody1.GetRotation(), inWorldSpaceAxis) : Vec3::sZero();
+		mInvI2_Axis = inBody2.IsDynamic()? inBody2.GetMotionProperties()->MultiplyWorldSpaceInverseInertiaByVector(inBody2.GetRotation(), inWorldSpaceAxis) : Vec3::sZero();
+
+		// Calculate inverse effective mass: K = J M^-1 J^T
+		return inWorldSpaceAxis.Dot(mInvI1_Axis + mInvI2_Axis);
+	}
+
+public:
+	/// Calculate properties used during the functions below
+	/// @param inBody1 The first body that this constraint is attached to
+	/// @param inBody2 The second body that this constraint is attached to
+	/// @param inWorldSpaceAxis The axis of rotation along which the constraint acts (normalized)
+	/// Set the following terms to zero if you don't want to drive the constraint to zero with a spring:
+	/// @param inBias Bias term (b) for the constraint impulse: lambda = J v + b
+	inline void					CalculateConstraintProperties(const Body &inBody1, const Body &inBody2, Vec3Arg inWorldSpaceAxis, float inBias = 0.0f)
+	{
+		float inv_effective_mass = CalculateInverseEffectiveMass(inBody1, inBody2, inWorldSpaceAxis);
+
+		if (inv_effective_mass == 0.0f)
+			Deactivate();
+		else
+		{
+			mEffectiveMass = 1.0f / inv_effective_mass;
+			mSpringPart.CalculateSpringPropertiesWithBias(inBias);
+		}
+	}
+
+	/// Calculate properties used during the functions below
+	/// @param inDeltaTime Time step
+	/// @param inBody1 The first body that this constraint is attached to
+	/// @param inBody2 The second body that this constraint is attached to
+	/// @param inWorldSpaceAxis The axis of rotation along which the constraint acts (normalized)
+	/// Set the following terms to zero if you don't want to drive the constraint to zero with a spring:
+	/// @param inBias Bias term (b) for the constraint impulse: lambda = J v + b
+	///	@param inC Value of the constraint equation (C)
+	///	@param inFrequency Oscillation frequency (Hz)
+	///	@param inDamping Damping factor (0 = no damping, 1 = critical damping)
+	inline void					CalculateConstraintPropertiesWithFrequencyAndDamping(float inDeltaTime, const Body &inBody1, const Body &inBody2, Vec3Arg inWorldSpaceAxis, float inBias, float inC, float inFrequency, float inDamping)
+	{
+		float inv_effective_mass = CalculateInverseEffectiveMass(inBody1, inBody2, inWorldSpaceAxis);
+
+		if (inv_effective_mass == 0.0f)
+			Deactivate();
+		else
+			mSpringPart.CalculateSpringPropertiesWithFrequencyAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inFrequency, inDamping, mEffectiveMass);
+	}
+
+	/// Calculate properties used during the functions below
+	/// @param inDeltaTime Time step
+	/// @param inBody1 The first body that this constraint is attached to
+	/// @param inBody2 The second body that this constraint is attached to
+	/// @param inWorldSpaceAxis The axis of rotation along which the constraint acts (normalized)
+	/// Set the following terms to zero if you don't want to drive the constraint to zero with a spring:
+	/// @param inBias Bias term (b) for the constraint impulse: lambda = J v + b
+	///	@param inC Value of the constraint equation (C)
+	///	@param inStiffness Spring stiffness k.
+	///	@param inDamping Spring damping coefficient c.
+	inline void					CalculateConstraintPropertiesWithStiffnessAndDamping(float inDeltaTime, const Body &inBody1, const Body &inBody2, Vec3Arg inWorldSpaceAxis, float inBias, float inC, float inStiffness, float inDamping)
+	{
+		float inv_effective_mass = CalculateInverseEffectiveMass(inBody1, inBody2, inWorldSpaceAxis);
+
+		if (inv_effective_mass == 0.0f)
+			Deactivate();
+		else
+			mSpringPart.CalculateSpringPropertiesWithStiffnessAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inStiffness, inDamping, mEffectiveMass);
+	}
+
+	/// Selects one of the above functions based on the spring settings
+	inline void					CalculateConstraintPropertiesWithSettings(float inDeltaTime, const Body &inBody1, const Body &inBody2, Vec3Arg inWorldSpaceAxis, float inBias, float inC, const SpringSettings &inSpringSettings)
+	{
+		float inv_effective_mass = CalculateInverseEffectiveMass(inBody1, inBody2, inWorldSpaceAxis);
+
+		if (inv_effective_mass == 0.0f)
+			Deactivate();
+		else if (inSpringSettings.mMode == ESpringMode::FrequencyAndDamping)
+			mSpringPart.CalculateSpringPropertiesWithFrequencyAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inSpringSettings.mFrequency, inSpringSettings.mDamping, mEffectiveMass);
+		else
+			mSpringPart.CalculateSpringPropertiesWithStiffnessAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inSpringSettings.mStiffness, inSpringSettings.mDamping, mEffectiveMass);
+	}
+
+	/// Deactivate this constraint
+	inline void					Deactivate()
+	{
+		mEffectiveMass = 0.0f;
+		mTotalLambda = 0.0f;
+	}
+
+	/// Check if constraint is active
+	inline bool					IsActive() const
+	{
+		return mEffectiveMass != 0.0f;
+	}
+
+	/// Must be called from the WarmStartVelocityConstraint call to apply the previous frame's impulses
+	/// @param ioBody1 The first body that this constraint is attached to
+	/// @param ioBody2 The second body that this constraint is attached to
+	/// @param inWarmStartImpulseRatio Ratio of new step to old time step (dt_new / dt_old) for scaling the lagrange multiplier of the previous frame
+	inline void					WarmStart(Body &ioBody1, Body &ioBody2, float inWarmStartImpulseRatio)
+	{
+		mTotalLambda *= inWarmStartImpulseRatio;
+		ApplyVelocityStep(ioBody1, ioBody2, mTotalLambda);
+	}
+
+	/// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation.
+	/// @param ioBody1 The first body that this constraint is attached to
+	/// @param ioBody2 The second body that this constraint is attached to
+	/// @param inWorldSpaceAxis The axis of rotation along which the constraint acts (normalized)
+	/// @param inMinLambda Minimum angular impulse to apply (N m s)
+	/// @param inMaxLambda Maximum angular impulse to apply (N m s)
+	inline bool					SolveVelocityConstraint(Body &ioBody1, Body &ioBody2, Vec3Arg inWorldSpaceAxis, float inMinLambda, float inMaxLambda)
+	{
+		// Lagrange multiplier is:
+		//
+		// lambda = -K^-1 (J v + b)
+		float lambda = mEffectiveMass * (inWorldSpaceAxis.Dot(ioBody1.GetAngularVelocity() - ioBody2.GetAngularVelocity()) - mSpringPart.GetBias(mTotalLambda));
+		float new_lambda = Clamp(mTotalLambda + lambda, inMinLambda, inMaxLambda); // Clamp impulse
+		lambda = new_lambda - mTotalLambda; // Lambda potentially got clamped, calculate the new impulse to apply
+		mTotalLambda = new_lambda; // Store accumulated impulse
+
+		return ApplyVelocityStep(ioBody1, ioBody2, lambda);
+	}
+
+	/// Return lagrange multiplier
+	float						GetTotalLambda() const
+	{
+		return mTotalLambda;
+	}
+
+	/// Iteratively update the position constraint. Makes sure C(...) == 0.
+	/// @param ioBody1 The first body that this constraint is attached to
+	/// @param ioBody2 The second body that this constraint is attached to
+	/// @param inC Value of the constraint equation (C)
+	/// @param inBaumgarte Baumgarte constant (fraction of the error to correct)
+	inline bool					SolvePositionConstraint(Body &ioBody1, Body &ioBody2, float inC, float inBaumgarte) const
+	{
+		// Only apply position constraint when the constraint is hard, otherwise the velocity bias will fix the constraint
+		if (inC != 0.0f && !mSpringPart.IsActive())
+		{
+			// Calculate lagrange multiplier (lambda) for Baumgarte stabilization:
+			//
+			// lambda = -K^-1 * beta / dt * C
+			//
+			// We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out
+			float lambda = -mEffectiveMass * inBaumgarte * inC;
+
+			// Directly integrate velocity change for one time step
+			//
+			// Euler velocity integration:
+			// dv = M^-1 P
+			//
+			// Impulse:
+			// P = J^T lambda
+			//
+			// Euler position integration:
+			// x' = x + dv * dt
+			//
+			// Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and
+			// Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte
+			// stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity
+			// integrate + a position integrate and then discard the velocity change.
+			if (ioBody1.IsDynamic())
+				ioBody1.SubRotationStep(lambda * mInvI1_Axis);
+			if (ioBody2.IsDynamic())
+				ioBody2.AddRotationStep(lambda * mInvI2_Axis);
+			return true;
+		}
+
+		return false;
+	}
+
+	/// Save state of this constraint part
+	void						SaveState(StateRecorder &inStream) const
+	{
+		inStream.Write(mTotalLambda);
+	}
+
+	/// Restore state of this constraint part
+	void						RestoreState(StateRecorder &inStream)
+	{
+		inStream.Read(mTotalLambda);
+	}
+
+private:
+	Vec3						mInvI1_Axis;
+	Vec3						mInvI2_Axis;
+	float						mEffectiveMass = 0.0f;
+	SpringPart					mSpringPart;
+	float						mTotalLambda = 0.0f;
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/AxisConstraintPart.h

@@ -0,0 +1,682 @@
+// 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/Constraints/ConstraintPart/SpringPart.h>
+#include <Jolt/Physics/Constraints/SpringSettings.h>
+#include <Jolt/Physics/StateRecorder.h>
+#include <Jolt/Physics/DeterminismLog.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Constraint that constrains motion along 1 axis
+///
+/// @see "Constraints Derivation for Rigid Body Simulation in 3D" - Daniel Chappuis, section 2.1.1
+/// (we're not using the approximation of eq 27 but instead add the U term as in eq 55)
+///
+/// Constraint equation (eq 25):
+///
+/// \f[C = (p_2 - p_1) \cdot n\f]
+///
+/// Jacobian (eq 28):
+///
+/// \f[J = \begin{bmatrix} -n^T & (-(r_1 + u) \times n)^T & n^T & (r_2 \times n)^T \end{bmatrix}\f]
+///
+/// Used terms (here and below, everything in world space):\n
+/// n = constraint axis (normalized).\n
+/// p1, p2 = constraint points.\n
+/// r1 = p1 - x1.\n
+/// r2 = p2 - x2.\n
+/// u = x2 + r2 - x1 - r1 = p2 - p1.\n
+/// x1, x2 = center of mass for the bodies.\n
+/// v = [v1, w1, v2, w2].\n
+/// v1, v2 = linear velocity of body 1 and 2.\n
+/// w1, w2 = angular velocity of body 1 and 2.\n
+/// M = mass matrix, a diagonal matrix of the mass and inertia with diagonal [m1, I1, m2, I2].\n
+/// \f$K^{-1} = \left( J M^{-1} J^T \right)^{-1}\f$ = effective mass.\n
+/// b = velocity bias.\n
+/// \f$\beta\f$ = baumgarte constant.
+class AxisConstraintPart
+{
+	/// Internal helper function to update velocities of bodies after Lagrange multiplier is calculated
+	template <EMotionType Type1, EMotionType Type2>
+	JPH_INLINE bool				ApplyVelocityStep(MotionProperties *ioMotionProperties1, float inInvMass1, MotionProperties *ioMotionProperties2, float inInvMass2, Vec3Arg inWorldSpaceAxis, float inLambda) const
+	{
+		// Apply impulse if delta is not zero
+		if (inLambda != 0.0f)
+		{
+			// Calculate velocity change due to constraint
+			//
+			// Impulse:
+			// P = J^T lambda
+			//
+			// Euler velocity integration:
+			// v' = v + M^-1 P
+			if constexpr (Type1 == EMotionType::Dynamic)
+			{
+				ioMotionProperties1->SubLinearVelocityStep((inLambda * inInvMass1) * inWorldSpaceAxis);
+				ioMotionProperties1->SubAngularVelocityStep(inLambda * Vec3::sLoadFloat3Unsafe(mInvI1_R1PlusUxAxis));
+			}
+			if constexpr (Type2 == EMotionType::Dynamic)
+			{
+				ioMotionProperties2->AddLinearVelocityStep((inLambda * inInvMass2) * inWorldSpaceAxis);
+				ioMotionProperties2->AddAngularVelocityStep(inLambda * Vec3::sLoadFloat3Unsafe(mInvI2_R2xAxis));
+			}
+			return true;
+		}
+
+		return false;
+	}
+
+	/// Internal helper function to calculate the inverse effective mass
+	template <EMotionType Type1, EMotionType Type2>
+	JPH_INLINE float			TemplatedCalculateInverseEffectiveMass(float inInvMass1, Mat44Arg inInvI1, Vec3Arg inR1PlusU, float inInvMass2, Mat44Arg inInvI2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis)
+	{
+		JPH_ASSERT(inWorldSpaceAxis.IsNormalized(1.0e-5f));
+
+		// Calculate properties used below
+		Vec3 r1_plus_u_x_axis;
+		if constexpr (Type1 != EMotionType::Static)
+		{
+			r1_plus_u_x_axis = inR1PlusU.Cross(inWorldSpaceAxis);
+			r1_plus_u_x_axis.StoreFloat3(&mR1PlusUxAxis);
+		}
+		else
+		{
+		#ifdef JPH_DEBUG
+			Vec3::sNaN().StoreFloat3(&mR1PlusUxAxis);
+		#endif
+		}
+
+		Vec3 r2_x_axis;
+		if constexpr (Type2 != EMotionType::Static)
+		{
+			r2_x_axis = inR2.Cross(inWorldSpaceAxis);
+			r2_x_axis.StoreFloat3(&mR2xAxis);
+		}
+		else
+		{
+		#ifdef JPH_DEBUG
+			Vec3::sNaN().StoreFloat3(&mR2xAxis);
+		#endif
+		}
+
+		// Calculate inverse effective mass: K = J M^-1 J^T
+		float inv_effective_mass;
+
+		if constexpr (Type1 == EMotionType::Dynamic)
+		{
+			Vec3 invi1_r1_plus_u_x_axis = inInvI1.Multiply3x3(r1_plus_u_x_axis);
+			invi1_r1_plus_u_x_axis.StoreFloat3(&mInvI1_R1PlusUxAxis);
+			inv_effective_mass = inInvMass1 + invi1_r1_plus_u_x_axis.Dot(r1_plus_u_x_axis);
+		}
+		else
+		{
+			(void)r1_plus_u_x_axis; // Fix compiler warning: Not using this (it's not calculated either)
+			JPH_IF_DEBUG(Vec3::sNaN().StoreFloat3(&mInvI1_R1PlusUxAxis);)
+			inv_effective_mass = 0.0f;
+		}
+
+		if constexpr (Type2 == EMotionType::Dynamic)
+		{
+			Vec3 invi2_r2_x_axis = inInvI2.Multiply3x3(r2_x_axis);
+			invi2_r2_x_axis.StoreFloat3(&mInvI2_R2xAxis);
+			inv_effective_mass += inInvMass2 + invi2_r2_x_axis.Dot(r2_x_axis);
+		}
+		else
+		{
+			(void)r2_x_axis; // Fix compiler warning: Not using this (it's not calculated either)
+			JPH_IF_DEBUG(Vec3::sNaN().StoreFloat3(&mInvI2_R2xAxis);)
+		}
+
+		return inv_effective_mass;
+	}
+
+	/// Internal helper function to calculate the inverse effective mass
+	JPH_INLINE float			CalculateInverseEffectiveMass(const Body &inBody1, Vec3Arg inR1PlusU, const Body &inBody2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis)
+	{
+		// Dispatch to the correct templated form
+		switch (inBody1.GetMotionType())
+		{
+		case EMotionType::Dynamic:
+			{
+				const MotionProperties *mp1 = inBody1.GetMotionPropertiesUnchecked();
+				float inv_m1 = mp1->GetInverseMass();
+				Mat44 inv_i1 = inBody1.GetInverseInertia();
+				switch (inBody2.GetMotionType())
+				{
+				case EMotionType::Dynamic:
+					return TemplatedCalculateInverseEffectiveMass<EMotionType::Dynamic, EMotionType::Dynamic>(inv_m1, inv_i1, inR1PlusU, inBody2.GetMotionPropertiesUnchecked()->GetInverseMass(), inBody2.GetInverseInertia(), inR2, inWorldSpaceAxis);
+
+				case EMotionType::Kinematic:
+					return TemplatedCalculateInverseEffectiveMass<EMotionType::Dynamic, EMotionType::Kinematic>(inv_m1, inv_i1, inR1PlusU, 0 /* Will not be used */, Mat44() /* Will not be used */, inR2, inWorldSpaceAxis);
+
+				case EMotionType::Static:
+					return TemplatedCalculateInverseEffectiveMass<EMotionType::Dynamic, EMotionType::Static>(inv_m1, inv_i1, inR1PlusU, 0 /* Will not be used */, Mat44() /* Will not be used */, inR2, inWorldSpaceAxis);
+
+				default:
+					break;
+				}
+				break;
+			}
+
+		case EMotionType::Kinematic:
+			JPH_ASSERT(inBody2.IsDynamic());
+			return TemplatedCalculateInverseEffectiveMass<EMotionType::Kinematic, EMotionType::Dynamic>(0 /* Will not be used */, Mat44() /* Will not be used */, inR1PlusU, inBody2.GetMotionPropertiesUnchecked()->GetInverseMass(), inBody2.GetInverseInertia(), inR2, inWorldSpaceAxis);
+
+		case EMotionType::Static:
+			JPH_ASSERT(inBody2.IsDynamic());
+			return TemplatedCalculateInverseEffectiveMass<EMotionType::Static, EMotionType::Dynamic>(0 /* Will not be used */, Mat44() /* Will not be used */, inR1PlusU, inBody2.GetMotionPropertiesUnchecked()->GetInverseMass(), inBody2.GetInverseInertia(), inR2, inWorldSpaceAxis);
+
+		default:
+			break;
+		}
+
+		JPH_ASSERT(false);
+		return 0.0f;
+	}
+
+	/// Internal helper function to calculate the inverse effective mass, version that supports mass scaling
+	JPH_INLINE float			CalculateInverseEffectiveMassWithMassOverride(const Body &inBody1, float inInvMass1, float inInvInertiaScale1, Vec3Arg inR1PlusU, const Body &inBody2, float inInvMass2, float inInvInertiaScale2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis)
+	{
+		// Dispatch to the correct templated form
+		switch (inBody1.GetMotionType())
+		{
+		case EMotionType::Dynamic:
+			{
+				Mat44 inv_i1 = inInvInertiaScale1 * inBody1.GetInverseInertia();
+				switch (inBody2.GetMotionType())
+				{
+				case EMotionType::Dynamic:
+					return TemplatedCalculateInverseEffectiveMass<EMotionType::Dynamic, EMotionType::Dynamic>(inInvMass1, inv_i1, inR1PlusU, inInvMass2, inInvInertiaScale2 * inBody2.GetInverseInertia(), inR2, inWorldSpaceAxis);
+
+				case EMotionType::Kinematic:
+					return TemplatedCalculateInverseEffectiveMass<EMotionType::Dynamic, EMotionType::Kinematic>(inInvMass1, inv_i1, inR1PlusU, 0 /* Will not be used */, Mat44() /* Will not be used */, inR2, inWorldSpaceAxis);
+
+				case EMotionType::Static:
+					return TemplatedCalculateInverseEffectiveMass<EMotionType::Dynamic, EMotionType::Static>(inInvMass1, inv_i1, inR1PlusU, 0 /* Will not be used */, Mat44() /* Will not be used */, inR2, inWorldSpaceAxis);
+
+				default:
+					break;
+				}
+				break;
+			}
+
+		case EMotionType::Kinematic:
+			JPH_ASSERT(inBody2.IsDynamic());
+			return TemplatedCalculateInverseEffectiveMass<EMotionType::Kinematic, EMotionType::Dynamic>(0 /* Will not be used */, Mat44() /* Will not be used */, inR1PlusU, inInvMass2, inInvInertiaScale2 * inBody2.GetInverseInertia(), inR2, inWorldSpaceAxis);
+
+		case EMotionType::Static:
+			JPH_ASSERT(inBody2.IsDynamic());
+			return TemplatedCalculateInverseEffectiveMass<EMotionType::Static, EMotionType::Dynamic>(0 /* Will not be used */, Mat44() /* Will not be used */, inR1PlusU, inInvMass2, inInvInertiaScale2 * inBody2.GetInverseInertia(), inR2, inWorldSpaceAxis);
+
+		default:
+			break;
+		}
+
+		JPH_ASSERT(false);
+		return 0.0f;
+	}
+
+public:
+	/// Templated form of CalculateConstraintProperties with the motion types baked in
+	template <EMotionType Type1, EMotionType Type2>
+	JPH_INLINE void				TemplatedCalculateConstraintProperties(float inInvMass1, Mat44Arg inInvI1, Vec3Arg inR1PlusU, float inInvMass2, Mat44Arg inInvI2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis, float inBias = 0.0f)
+	{
+		float inv_effective_mass = TemplatedCalculateInverseEffectiveMass<Type1, Type2>(inInvMass1, inInvI1, inR1PlusU, inInvMass2, inInvI2, inR2, inWorldSpaceAxis);
+
+		if (inv_effective_mass == 0.0f)
+			Deactivate();
+		else
+		{
+			mEffectiveMass = 1.0f / inv_effective_mass;
+			mSpringPart.CalculateSpringPropertiesWithBias(inBias);
+		}
+
+		JPH_DET_LOG("TemplatedCalculateConstraintProperties: invM1: " << inInvMass1 << " invI1: " << inInvI1 << " r1PlusU: " << inR1PlusU << " invM2: " << inInvMass2 << " invI2: " << inInvI2 << " r2: " << inR2 << " bias: " << inBias << " r1PlusUxAxis: " << mR1PlusUxAxis << " r2xAxis: " << mR2xAxis << " invI1_R1PlusUxAxis: " << mInvI1_R1PlusUxAxis << " invI2_R2xAxis: " << mInvI2_R2xAxis << " effectiveMass: " << mEffectiveMass << " totalLambda: " << mTotalLambda);
+	}
+
+	/// Calculate properties used during the functions below
+	/// @param inBody1 The first body that this constraint is attached to
+	/// @param inBody2 The second body that this constraint is attached to
+	/// @param inR1PlusU See equations above (r1 + u)
+	/// @param inR2 See equations above (r2)
+	/// @param inWorldSpaceAxis Axis along which the constraint acts (normalized, pointing from body 1 to 2)
+	/// @param inBias Bias term (b) for the constraint impulse: lambda = J v + b
+	inline void					CalculateConstraintProperties(const Body &inBody1, Vec3Arg inR1PlusU, const Body &inBody2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis, float inBias = 0.0f)
+	{
+		float inv_effective_mass = CalculateInverseEffectiveMass(inBody1, inR1PlusU, inBody2, inR2, inWorldSpaceAxis);
+
+		if (inv_effective_mass == 0.0f)
+			Deactivate();
+		else
+		{
+			mEffectiveMass = 1.0f / inv_effective_mass;
+			mSpringPart.CalculateSpringPropertiesWithBias(inBias);
+		}
+	}
+
+	/// Calculate properties used during the functions below, version that supports mass scaling
+	/// @param inBody1 The first body that this constraint is attached to
+	/// @param inBody2 The second body that this constraint is attached to
+	/// @param inInvMass1 The inverse mass of body 1 (only used when body 1 is dynamic)
+	/// @param inInvMass2 The inverse mass of body 2 (only used when body 2 is dynamic)
+	/// @param inInvInertiaScale1 Scale factor for the inverse inertia of body 1
+	/// @param inInvInertiaScale2 Scale factor for the inverse inertia of body 2
+	/// @param inR1PlusU See equations above (r1 + u)
+	/// @param inR2 See equations above (r2)
+	/// @param inWorldSpaceAxis Axis along which the constraint acts (normalized, pointing from body 1 to 2)
+	/// @param inBias Bias term (b) for the constraint impulse: lambda = J v + b
+	inline void					CalculateConstraintPropertiesWithMassOverride(const Body &inBody1, float inInvMass1, float inInvInertiaScale1, Vec3Arg inR1PlusU, const Body &inBody2, float inInvMass2, float inInvInertiaScale2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis, float inBias = 0.0f)
+	{
+		float inv_effective_mass = CalculateInverseEffectiveMassWithMassOverride(inBody1, inInvMass1, inInvInertiaScale1, inR1PlusU, inBody2, inInvMass2, inInvInertiaScale2, inR2, inWorldSpaceAxis);
+
+		if (inv_effective_mass == 0.0f)
+			Deactivate();
+		else
+		{
+			mEffectiveMass = 1.0f / inv_effective_mass;
+			mSpringPart.CalculateSpringPropertiesWithBias(inBias);
+		}
+	}
+
+	/// Calculate properties used during the functions below
+	/// @param inDeltaTime Time step
+	/// @param inBody1 The first body that this constraint is attached to
+	/// @param inBody2 The second body that this constraint is attached to
+	/// @param inR1PlusU See equations above (r1 + u)
+	/// @param inR2 See equations above (r2)
+	/// @param inWorldSpaceAxis Axis along which the constraint acts (normalized, pointing from body 1 to 2)
+	/// @param inBias Bias term (b) for the constraint impulse: lambda = J v + b
+	///	@param inC Value of the constraint equation (C).
+	///	@param inFrequency Oscillation frequency (Hz).
+	///	@param inDamping Damping factor (0 = no damping, 1 = critical damping).
+	inline void					CalculateConstraintPropertiesWithFrequencyAndDamping(float inDeltaTime, const Body &inBody1, Vec3Arg inR1PlusU, const Body &inBody2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis, float inBias, float inC, float inFrequency, float inDamping)
+	{
+		float inv_effective_mass = CalculateInverseEffectiveMass(inBody1, inR1PlusU, inBody2, inR2, inWorldSpaceAxis);
+
+		if (inv_effective_mass == 0.0f)
+			Deactivate();
+		else
+			mSpringPart.CalculateSpringPropertiesWithFrequencyAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inFrequency, inDamping, mEffectiveMass);
+	}
+
+	/// Calculate properties used during the functions below
+	/// @param inDeltaTime Time step
+	/// @param inBody1 The first body that this constraint is attached to
+	/// @param inBody2 The second body that this constraint is attached to
+	/// @param inR1PlusU See equations above (r1 + u)
+	/// @param inR2 See equations above (r2)
+	/// @param inWorldSpaceAxis Axis along which the constraint acts (normalized, pointing from body 1 to 2)
+	/// @param inBias Bias term (b) for the constraint impulse: lambda = J v + b
+	///	@param inC Value of the constraint equation (C).
+	///	@param inStiffness Spring stiffness k.
+	///	@param inDamping Spring damping coefficient c.
+	inline void					CalculateConstraintPropertiesWithStiffnessAndDamping(float inDeltaTime, const Body &inBody1, Vec3Arg inR1PlusU, const Body &inBody2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis, float inBias, float inC, float inStiffness, float inDamping)
+	{
+		float inv_effective_mass = CalculateInverseEffectiveMass(inBody1, inR1PlusU, inBody2, inR2, inWorldSpaceAxis);
+
+		if (inv_effective_mass == 0.0f)
+			Deactivate();
+		else
+			mSpringPart.CalculateSpringPropertiesWithStiffnessAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inStiffness, inDamping, mEffectiveMass);
+	}
+
+	/// Selects one of the above functions based on the spring settings
+	inline void					CalculateConstraintPropertiesWithSettings(float inDeltaTime, const Body &inBody1, Vec3Arg inR1PlusU, const Body &inBody2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis, float inBias, float inC, const SpringSettings &inSpringSettings)
+	{
+		float inv_effective_mass = CalculateInverseEffectiveMass(inBody1, inR1PlusU, inBody2, inR2, inWorldSpaceAxis);
+
+		if (inv_effective_mass == 0.0f)
+			Deactivate();
+		else if (inSpringSettings.mMode == ESpringMode::FrequencyAndDamping)
+			mSpringPart.CalculateSpringPropertiesWithFrequencyAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inSpringSettings.mFrequency, inSpringSettings.mDamping, mEffectiveMass);
+		else
+			mSpringPart.CalculateSpringPropertiesWithStiffnessAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inSpringSettings.mStiffness, inSpringSettings.mDamping, mEffectiveMass);
+	}
+
+	/// Deactivate this constraint
+	inline void					Deactivate()
+	{
+		mEffectiveMass = 0.0f;
+		mTotalLambda = 0.0f;
+	}
+
+	/// Check if constraint is active
+	inline bool					IsActive() const
+	{
+		return mEffectiveMass != 0.0f;
+	}
+
+	/// Templated form of WarmStart with the motion types baked in
+	template <EMotionType Type1, EMotionType Type2>
+	inline void					TemplatedWarmStart(MotionProperties *ioMotionProperties1, float inInvMass1, MotionProperties *ioMotionProperties2, float inInvMass2, Vec3Arg inWorldSpaceAxis, float inWarmStartImpulseRatio)
+	{
+		mTotalLambda *= inWarmStartImpulseRatio;
+
+		ApplyVelocityStep<Type1, Type2>(ioMotionProperties1, inInvMass1, ioMotionProperties2, inInvMass2, inWorldSpaceAxis, mTotalLambda);
+	}
+
+	/// Must be called from the WarmStartVelocityConstraint call to apply the previous frame's impulses
+	/// @param ioBody1 The first body that this constraint is attached to
+	/// @param ioBody2 The second body that this constraint is attached to
+	/// @param inWorldSpaceAxis Axis along which the constraint acts (normalized)
+	/// @param inWarmStartImpulseRatio Ratio of new step to old time step (dt_new / dt_old) for scaling the lagrange multiplier of the previous frame
+	inline void					WarmStart(Body &ioBody1, Body &ioBody2, Vec3Arg inWorldSpaceAxis, float inWarmStartImpulseRatio)
+	{
+		EMotionType motion_type1 = ioBody1.GetMotionType();
+		MotionProperties *motion_properties1 = ioBody1.GetMotionPropertiesUnchecked();
+
+		EMotionType motion_type2 = ioBody2.GetMotionType();
+		MotionProperties *motion_properties2 = ioBody2.GetMotionPropertiesUnchecked();
+
+		// Dispatch to the correct templated form
+		// Note: Warm starting doesn't differentiate between kinematic/static bodies so we handle both as static bodies
+		if (motion_type1 == EMotionType::Dynamic)
+		{
+			if (motion_type2 == EMotionType::Dynamic)
+				TemplatedWarmStart<EMotionType::Dynamic, EMotionType::Dynamic>(motion_properties1, motion_properties1->GetInverseMass(), motion_properties2, motion_properties2->GetInverseMass(), inWorldSpaceAxis, inWarmStartImpulseRatio);
+			else
+				TemplatedWarmStart<EMotionType::Dynamic, EMotionType::Static>(motion_properties1, motion_properties1->GetInverseMass(), motion_properties2, 0.0f /* Unused */, inWorldSpaceAxis, inWarmStartImpulseRatio);
+		}
+		else
+		{
+			JPH_ASSERT(motion_type2 == EMotionType::Dynamic);
+			TemplatedWarmStart<EMotionType::Static, EMotionType::Dynamic>(motion_properties1, 0.0f /* Unused */, motion_properties2, motion_properties2->GetInverseMass(), inWorldSpaceAxis, inWarmStartImpulseRatio);
+		}
+	}
+
+	/// Templated form of SolveVelocityConstraint with the motion types baked in, part 1: get the total lambda
+	template <EMotionType Type1, EMotionType Type2>
+	JPH_INLINE float			TemplatedSolveVelocityConstraintGetTotalLambda(const MotionProperties *ioMotionProperties1, const MotionProperties *ioMotionProperties2, Vec3Arg inWorldSpaceAxis) const
+	{
+		// Calculate jacobian multiplied by linear velocity
+		float jv;
+		if constexpr (Type1 != EMotionType::Static && Type2 != EMotionType::Static)
+			jv = inWorldSpaceAxis.Dot(ioMotionProperties1->GetLinearVelocity() - ioMotionProperties2->GetLinearVelocity());
+		else if constexpr (Type1 != EMotionType::Static)
+			jv = inWorldSpaceAxis.Dot(ioMotionProperties1->GetLinearVelocity());
+		else if constexpr (Type2 != EMotionType::Static)
+			jv = inWorldSpaceAxis.Dot(-ioMotionProperties2->GetLinearVelocity());
+		else
+			JPH_ASSERT(false); // Static vs static is nonsensical!
+
+		// Calculate jacobian multiplied by angular velocity
+		if constexpr (Type1 != EMotionType::Static)
+			jv += Vec3::sLoadFloat3Unsafe(mR1PlusUxAxis).Dot(ioMotionProperties1->GetAngularVelocity());
+		if constexpr (Type2 != EMotionType::Static)
+			jv -= Vec3::sLoadFloat3Unsafe(mR2xAxis).Dot(ioMotionProperties2->GetAngularVelocity());
+
+		// Lagrange multiplier is:
+		//
+		// lambda = -K^-1 (J v + b)
+		float lambda = mEffectiveMass * (jv - mSpringPart.GetBias(mTotalLambda));
+
+		// Return the total accumulated lambda
+		return mTotalLambda + lambda;
+	}
+
+	/// Templated form of SolveVelocityConstraint with the motion types baked in, part 2: apply new lambda
+	template <EMotionType Type1, EMotionType Type2>
+	JPH_INLINE bool				TemplatedSolveVelocityConstraintApplyLambda(MotionProperties *ioMotionProperties1, float inInvMass1, MotionProperties *ioMotionProperties2, float inInvMass2, Vec3Arg inWorldSpaceAxis, float inTotalLambda)
+	{
+		float delta_lambda = inTotalLambda - mTotalLambda; // Calculate change in lambda
+		mTotalLambda = inTotalLambda; // Store accumulated impulse
+
+		return ApplyVelocityStep<Type1, Type2>(ioMotionProperties1, inInvMass1, ioMotionProperties2, inInvMass2, inWorldSpaceAxis, delta_lambda);
+	}
+
+	/// Templated form of SolveVelocityConstraint with the motion types baked in
+	template <EMotionType Type1, EMotionType Type2>
+	inline bool					TemplatedSolveVelocityConstraint(MotionProperties *ioMotionProperties1, float inInvMass1, MotionProperties *ioMotionProperties2, float inInvMass2, Vec3Arg inWorldSpaceAxis, float inMinLambda, float inMaxLambda)
+	{
+		float total_lambda = TemplatedSolveVelocityConstraintGetTotalLambda<Type1, Type2>(ioMotionProperties1, ioMotionProperties2, inWorldSpaceAxis);
+
+		// Clamp impulse to specified range
+		total_lambda = Clamp(total_lambda, inMinLambda, inMaxLambda);
+
+		return TemplatedSolveVelocityConstraintApplyLambda<Type1, Type2>(ioMotionProperties1, inInvMass1, ioMotionProperties2, inInvMass2, inWorldSpaceAxis, total_lambda);
+	}
+
+	/// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation.
+	/// @param ioBody1 The first body that this constraint is attached to
+	/// @param ioBody2 The second body that this constraint is attached to
+	/// @param inWorldSpaceAxis Axis along which the constraint acts (normalized)
+	/// @param inMinLambda Minimum value of constraint impulse to apply (N s)
+	/// @param inMaxLambda Maximum value of constraint impulse to apply (N s)
+	inline bool					SolveVelocityConstraint(Body &ioBody1, Body &ioBody2, Vec3Arg inWorldSpaceAxis, float inMinLambda, float inMaxLambda)
+	{
+		EMotionType motion_type1 = ioBody1.GetMotionType();
+		MotionProperties *motion_properties1 = ioBody1.GetMotionPropertiesUnchecked();
+
+		EMotionType motion_type2 = ioBody2.GetMotionType();
+		MotionProperties *motion_properties2 = ioBody2.GetMotionPropertiesUnchecked();
+
+		// Dispatch to the correct templated form
+		switch (motion_type1)
+		{
+		case EMotionType::Dynamic:
+			switch (motion_type2)
+			{
+			case EMotionType::Dynamic:
+				return TemplatedSolveVelocityConstraint<EMotionType::Dynamic, EMotionType::Dynamic>(motion_properties1, motion_properties1->GetInverseMass(), motion_properties2, motion_properties2->GetInverseMass(), inWorldSpaceAxis, inMinLambda, inMaxLambda);
+
+			case EMotionType::Kinematic:
+				return TemplatedSolveVelocityConstraint<EMotionType::Dynamic, EMotionType::Kinematic>(motion_properties1, motion_properties1->GetInverseMass(), motion_properties2, 0.0f /* Unused */, inWorldSpaceAxis, inMinLambda, inMaxLambda);
+
+			case EMotionType::Static:
+				return TemplatedSolveVelocityConstraint<EMotionType::Dynamic, EMotionType::Static>(motion_properties1, motion_properties1->GetInverseMass(), motion_properties2, 0.0f /* Unused */, inWorldSpaceAxis, inMinLambda, inMaxLambda);
+
+			default:
+				JPH_ASSERT(false);
+				break;
+			}
+			break;
+
+		case EMotionType::Kinematic:
+			JPH_ASSERT(motion_type2 == EMotionType::Dynamic);
+			return TemplatedSolveVelocityConstraint<EMotionType::Kinematic, EMotionType::Dynamic>(motion_properties1, 0.0f /* Unused */, motion_properties2, motion_properties2->GetInverseMass(), inWorldSpaceAxis, inMinLambda, inMaxLambda);
+
+		case EMotionType::Static:
+			JPH_ASSERT(motion_type2 == EMotionType::Dynamic);
+			return TemplatedSolveVelocityConstraint<EMotionType::Static, EMotionType::Dynamic>(motion_properties1, 0.0f /* Unused */, motion_properties2, motion_properties2->GetInverseMass(), inWorldSpaceAxis, inMinLambda, inMaxLambda);
+
+		default:
+			JPH_ASSERT(false);
+			break;
+		}
+
+		return false;
+	}
+
+	/// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation.
+	/// @param ioBody1 The first body that this constraint is attached to
+	/// @param ioBody2 The second body that this constraint is attached to
+	/// @param inInvMass1 The inverse mass of body 1 (only used when body 1 is dynamic)
+	/// @param inInvMass2 The inverse mass of body 2 (only used when body 2 is dynamic)
+	/// @param inWorldSpaceAxis Axis along which the constraint acts (normalized)
+	/// @param inMinLambda Minimum value of constraint impulse to apply (N s)
+	/// @param inMaxLambda Maximum value of constraint impulse to apply (N s)
+	inline bool					SolveVelocityConstraintWithMassOverride(Body &ioBody1, float inInvMass1, Body &ioBody2, float inInvMass2, Vec3Arg inWorldSpaceAxis, float inMinLambda, float inMaxLambda)
+	{
+		EMotionType motion_type1 = ioBody1.GetMotionType();
+		MotionProperties *motion_properties1 = ioBody1.GetMotionPropertiesUnchecked();
+
+		EMotionType motion_type2 = ioBody2.GetMotionType();
+		MotionProperties *motion_properties2 = ioBody2.GetMotionPropertiesUnchecked();
+
+		// Dispatch to the correct templated form
+		switch (motion_type1)
+		{
+		case EMotionType::Dynamic:
+			switch (motion_type2)
+			{
+			case EMotionType::Dynamic:
+				return TemplatedSolveVelocityConstraint<EMotionType::Dynamic, EMotionType::Dynamic>(motion_properties1, inInvMass1, motion_properties2, inInvMass2, inWorldSpaceAxis, inMinLambda, inMaxLambda);
+
+			case EMotionType::Kinematic:
+				return TemplatedSolveVelocityConstraint<EMotionType::Dynamic, EMotionType::Kinematic>(motion_properties1, inInvMass1, motion_properties2, 0.0f /* Unused */, inWorldSpaceAxis, inMinLambda, inMaxLambda);
+
+			case EMotionType::Static:
+				return TemplatedSolveVelocityConstraint<EMotionType::Dynamic, EMotionType::Static>(motion_properties1, inInvMass1, motion_properties2, 0.0f /* Unused */, inWorldSpaceAxis, inMinLambda, inMaxLambda);
+
+			default:
+				JPH_ASSERT(false);
+				break;
+			}
+			break;
+
+		case EMotionType::Kinematic:
+			JPH_ASSERT(motion_type2 == EMotionType::Dynamic);
+			return TemplatedSolveVelocityConstraint<EMotionType::Kinematic, EMotionType::Dynamic>(motion_properties1, 0.0f /* Unused */, motion_properties2, inInvMass2, inWorldSpaceAxis, inMinLambda, inMaxLambda);
+
+		case EMotionType::Static:
+			JPH_ASSERT(motion_type2 == EMotionType::Dynamic);
+			return TemplatedSolveVelocityConstraint<EMotionType::Static, EMotionType::Dynamic>(motion_properties1, 0.0f /* Unused */, motion_properties2, inInvMass2, inWorldSpaceAxis, inMinLambda, inMaxLambda);
+
+		default:
+			JPH_ASSERT(false);
+			break;
+		}
+
+		return false;
+	}
+
+	/// Iteratively update the position constraint. Makes sure C(...) = 0.
+	/// @param ioBody1 The first body that this constraint is attached to
+	/// @param ioBody2 The second body that this constraint is attached to
+	/// @param inWorldSpaceAxis Axis along which the constraint acts (normalized)
+	/// @param inC Value of the constraint equation (C)
+	/// @param inBaumgarte Baumgarte constant (fraction of the error to correct)
+	inline bool					SolvePositionConstraint(Body &ioBody1, Body &ioBody2, Vec3Arg inWorldSpaceAxis, float inC, float inBaumgarte) const
+	{
+		// Only apply position constraint when the constraint is hard, otherwise the velocity bias will fix the constraint
+		if (inC != 0.0f && !mSpringPart.IsActive())
+		{
+			// Calculate lagrange multiplier (lambda) for Baumgarte stabilization:
+			//
+			// lambda = -K^-1 * beta / dt * C
+			//
+			// We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out
+			float lambda = -mEffectiveMass * inBaumgarte * inC;
+
+			// Directly integrate velocity change for one time step
+			//
+			// Euler velocity integration:
+			// dv = M^-1 P
+			//
+			// Impulse:
+			// P = J^T lambda
+			//
+			// Euler position integration:
+			// x' = x + dv * dt
+			//
+			// Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and
+			// Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte
+			// stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity
+			// integrate + a position integrate and then discard the velocity change.
+			if (ioBody1.IsDynamic())
+			{
+				ioBody1.SubPositionStep((lambda * ioBody1.GetMotionProperties()->GetInverseMass()) * inWorldSpaceAxis);
+				ioBody1.SubRotationStep(lambda * Vec3::sLoadFloat3Unsafe(mInvI1_R1PlusUxAxis));
+			}
+			if (ioBody2.IsDynamic())
+			{
+				ioBody2.AddPositionStep((lambda * ioBody2.GetMotionProperties()->GetInverseMass()) * inWorldSpaceAxis);
+				ioBody2.AddRotationStep(lambda * Vec3::sLoadFloat3Unsafe(mInvI2_R2xAxis));
+			}
+			return true;
+		}
+
+		return false;
+	}
+
+	/// Iteratively update the position constraint. Makes sure C(...) = 0.
+	/// @param ioBody1 The first body that this constraint is attached to
+	/// @param ioBody2 The second body that this constraint is attached to
+	/// @param inInvMass1 The inverse mass of body 1 (only used when body 1 is dynamic)
+	/// @param inInvMass2 The inverse mass of body 2 (only used when body 2 is dynamic)
+	/// @param inWorldSpaceAxis Axis along which the constraint acts (normalized)
+	/// @param inC Value of the constraint equation (C)
+	/// @param inBaumgarte Baumgarte constant (fraction of the error to correct)
+	inline bool					SolvePositionConstraintWithMassOverride(Body &ioBody1, float inInvMass1, Body &ioBody2, float inInvMass2, Vec3Arg inWorldSpaceAxis, float inC, float inBaumgarte) const
+	{
+		// Only apply position constraint when the constraint is hard, otherwise the velocity bias will fix the constraint
+		if (inC != 0.0f && !mSpringPart.IsActive())
+		{
+			// Calculate lagrange multiplier (lambda) for Baumgarte stabilization:
+			//
+			// lambda = -K^-1 * beta / dt * C
+			//
+			// We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out
+			float lambda = -mEffectiveMass * inBaumgarte * inC;
+
+			// Directly integrate velocity change for one time step
+			//
+			// Euler velocity integration:
+			// dv = M^-1 P
+			//
+			// Impulse:
+			// P = J^T lambda
+			//
+			// Euler position integration:
+			// x' = x + dv * dt
+			//
+			// Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and
+			// Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte
+			// stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity
+			// integrate + a position integrate and then discard the velocity change.
+			if (ioBody1.IsDynamic())
+			{
+				ioBody1.SubPositionStep((lambda * inInvMass1) * inWorldSpaceAxis);
+				ioBody1.SubRotationStep(lambda * Vec3::sLoadFloat3Unsafe(mInvI1_R1PlusUxAxis));
+			}
+			if (ioBody2.IsDynamic())
+			{
+				ioBody2.AddPositionStep((lambda * inInvMass2) * inWorldSpaceAxis);
+				ioBody2.AddRotationStep(lambda * Vec3::sLoadFloat3Unsafe(mInvI2_R2xAxis));
+			}
+			return true;
+		}
+
+		return false;
+	}
+
+	/// Override total lagrange multiplier, can be used to set the initial value for warm starting
+	inline void					SetTotalLambda(float inLambda)
+	{
+		mTotalLambda = inLambda;
+	}
+
+	/// Return lagrange multiplier
+	inline float				GetTotalLambda() const
+	{
+		return mTotalLambda;
+	}
+
+	/// Save state of this constraint part
+	void						SaveState(StateRecorder &inStream) const
+	{
+		inStream.Write(mTotalLambda);
+	}
+
+	/// Restore state of this constraint part
+	void						RestoreState(StateRecorder &inStream)
+	{
+		inStream.Read(mTotalLambda);
+	}
+
+private:
+	Float3						mR1PlusUxAxis;
+	Float3						mR2xAxis;
+	Float3						mInvI1_R1PlusUxAxis;
+	Float3						mInvI2_R2xAxis;
+	float						mEffectiveMass = 0.0f;
+	SpringPart					mSpringPart;
+	float						mTotalLambda = 0.0f;
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/DualAxisConstraintPart.h

@@ -0,0 +1,276 @@
+// 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/StateRecorder.h>
+#include <Jolt/Math/Vector.h>
+#include <Jolt/Math/Matrix.h>
+
+JPH_NAMESPACE_BEGIN
+
+/**
+	Constrains movement on 2 axis
+
+	@see "Constraints Derivation for Rigid Body Simulation in 3D" - Daniel Chappuis, section 2.3.1
+
+	Constraint equation (eq 51):
+
+	\f[C = \begin{bmatrix} (p_2 - p_1) \cdot n_1 \\ (p_2 - p_1) \cdot n_2\end{bmatrix}\f]
+
+	Jacobian (transposed) (eq 55):
+
+	\f[J^T = \begin{bmatrix}
+	-n_1					& -n_2					\\
+	-(r_1 + u) \times n_1	& -(r_1 + u) \times n_2	\\
+	n_1						& n_2					\\
+	r_2 \times n_1			& r_2 \times n_2
+	\end{bmatrix}\f]
+
+	Used terms (here and below, everything in world space):\n
+	n1, n2 = constraint axis (normalized).\n
+	p1, p2 = constraint points.\n
+	r1 = p1 - x1.\n
+	r2 = p2 - x2.\n
+	u = x2 + r2 - x1 - r1 = p2 - p1.\n
+	x1, x2 = center of mass for the bodies.\n
+	v = [v1, w1, v2, w2].\n
+	v1, v2 = linear velocity of body 1 and 2.\n
+	w1, w2 = angular velocity of body 1 and 2.\n
+	M = mass matrix, a diagonal matrix of the mass and inertia with diagonal [m1, I1, m2, I2].\n
+	\f$K^{-1} = \left( J M^{-1} J^T \right)^{-1}\f$ = effective mass.\n
+	b = velocity bias.\n
+	\f$\beta\f$ = baumgarte constant.
+**/
+class DualAxisConstraintPart
+{
+public:
+	using Vec2 = Vector<2>;
+	using Mat22 = Matrix<2, 2>;
+
+private:
+	/// Internal helper function to update velocities of bodies after Lagrange multiplier is calculated
+	JPH_INLINE bool				ApplyVelocityStep(Body &ioBody1, Body &ioBody2, Vec3Arg inN1, Vec3Arg inN2, const Vec2 &inLambda) const
+	{
+		// Apply impulse if delta is not zero
+		if (!inLambda.IsZero())
+		{
+			// Calculate velocity change due to constraint
+			//
+			// Impulse:
+			// P = J^T lambda
+			//
+			// Euler velocity integration:
+			// v' = v + M^-1 P
+			Vec3 impulse = inN1 * inLambda[0] + inN2 * inLambda[1];
+			if (ioBody1.IsDynamic())
+			{
+				MotionProperties *mp1 = ioBody1.GetMotionProperties();
+				mp1->SubLinearVelocityStep(mp1->GetInverseMass() * impulse);
+				mp1->SubAngularVelocityStep(mInvI1_R1PlusUxN1 * inLambda[0] + mInvI1_R1PlusUxN2 * inLambda[1]);
+			}
+			if (ioBody2.IsDynamic())
+			{
+				MotionProperties *mp2 = ioBody2.GetMotionProperties();
+				mp2->AddLinearVelocityStep(mp2->GetInverseMass() * impulse);
+				mp2->AddAngularVelocityStep(mInvI2_R2xN1 * inLambda[0] + mInvI2_R2xN2 * inLambda[1]);
+			}
+			return true;
+		}
+
+		return false;
+	}
+
+	/// Internal helper function to calculate the lagrange multiplier
+	inline void					CalculateLagrangeMultiplier(const Body &inBody1, const Body &inBody2, Vec3Arg inN1, Vec3Arg inN2, Vec2 &outLambda) const
+	{
+		// Calculate lagrange multiplier:
+		//
+		// lambda = -K^-1 (J v + b)
+		Vec3 delta_lin = inBody1.GetLinearVelocity() - inBody2.GetLinearVelocity();
+		Vec2 jv;
+		jv[0] = inN1.Dot(delta_lin) + mR1PlusUxN1.Dot(inBody1.GetAngularVelocity()) - mR2xN1.Dot(inBody2.GetAngularVelocity());
+		jv[1] = inN2.Dot(delta_lin) + mR1PlusUxN2.Dot(inBody1.GetAngularVelocity()) - mR2xN2.Dot(inBody2.GetAngularVelocity());
+		outLambda = mEffectiveMass * jv;
+	}
+
+public:
+	/// Calculate properties used during the functions below
+	/// All input vectors are in world space
+	inline void					CalculateConstraintProperties(const Body &inBody1, Mat44Arg inRotation1, Vec3Arg inR1PlusU, const Body &inBody2, Mat44Arg inRotation2, Vec3Arg inR2, Vec3Arg inN1, Vec3Arg inN2)
+	{
+		JPH_ASSERT(inN1.IsNormalized(1.0e-5f));
+		JPH_ASSERT(inN2.IsNormalized(1.0e-5f));
+
+		// Calculate properties used during constraint solving
+		mR1PlusUxN1 = inR1PlusU.Cross(inN1);
+		mR1PlusUxN2 = inR1PlusU.Cross(inN2);
+		mR2xN1 = inR2.Cross(inN1);
+		mR2xN2 = inR2.Cross(inN2);
+
+		// Calculate effective mass: K^-1 = (J M^-1 J^T)^-1, eq 59
+		Mat22 inv_effective_mass;
+		if (inBody1.IsDynamic())
+		{
+			const MotionProperties *mp1 = inBody1.GetMotionProperties();
+			Mat44 inv_i1 = mp1->GetInverseInertiaForRotation(inRotation1);
+			mInvI1_R1PlusUxN1 = inv_i1.Multiply3x3(mR1PlusUxN1);
+			mInvI1_R1PlusUxN2 = inv_i1.Multiply3x3(mR1PlusUxN2);
+
+			inv_effective_mass(0, 0) = mp1->GetInverseMass() + mR1PlusUxN1.Dot(mInvI1_R1PlusUxN1);
+			inv_effective_mass(0, 1) = mR1PlusUxN1.Dot(mInvI1_R1PlusUxN2);
+			inv_effective_mass(1, 0) = mR1PlusUxN2.Dot(mInvI1_R1PlusUxN1);
+			inv_effective_mass(1, 1) = mp1->GetInverseMass() + mR1PlusUxN2.Dot(mInvI1_R1PlusUxN2);
+		}
+		else
+		{
+			JPH_IF_DEBUG(mInvI1_R1PlusUxN1 = Vec3::sNaN();)
+			JPH_IF_DEBUG(mInvI1_R1PlusUxN2 = Vec3::sNaN();)
+
+			inv_effective_mass = Mat22::sZero();
+		}
+
+		if (inBody2.IsDynamic())
+		{
+			const MotionProperties *mp2 = inBody2.GetMotionProperties();
+			Mat44 inv_i2 = mp2->GetInverseInertiaForRotation(inRotation2);
+			mInvI2_R2xN1 = inv_i2.Multiply3x3(mR2xN1);
+			mInvI2_R2xN2 = inv_i2.Multiply3x3(mR2xN2);
+
+			inv_effective_mass(0, 0) += mp2->GetInverseMass() + mR2xN1.Dot(mInvI2_R2xN1);
+			inv_effective_mass(0, 1) += mR2xN1.Dot(mInvI2_R2xN2);
+			inv_effective_mass(1, 0) += mR2xN2.Dot(mInvI2_R2xN1);
+			inv_effective_mass(1, 1) += mp2->GetInverseMass() + mR2xN2.Dot(mInvI2_R2xN2);
+		}
+		else
+		{
+			JPH_IF_DEBUG(mInvI2_R2xN1 = Vec3::sNaN();)
+			JPH_IF_DEBUG(mInvI2_R2xN2 = Vec3::sNaN();)
+		}
+
+		if (!mEffectiveMass.SetInversed(inv_effective_mass))
+			Deactivate();
+	}
+
+	/// Deactivate this constraint
+	inline void					Deactivate()
+	{
+		mEffectiveMass.SetZero();
+		mTotalLambda.SetZero();
+	}
+
+	/// Check if constraint is active
+	inline bool					IsActive() const
+	{
+		return !mEffectiveMass.IsZero();
+	}
+
+	/// Must be called from the WarmStartVelocityConstraint call to apply the previous frame's impulses
+	/// All input vectors are in world space
+	inline void					WarmStart(Body &ioBody1, Body &ioBody2, Vec3Arg inN1, Vec3Arg inN2, float inWarmStartImpulseRatio)
+	{
+		mTotalLambda *= inWarmStartImpulseRatio;
+		ApplyVelocityStep(ioBody1, ioBody2, inN1, inN2, mTotalLambda);
+	}
+
+	/// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation.
+	/// All input vectors are in world space
+	inline bool					SolveVelocityConstraint(Body &ioBody1, Body &ioBody2, Vec3Arg inN1, Vec3Arg inN2)
+	{
+		Vec2 lambda;
+		CalculateLagrangeMultiplier(ioBody1, ioBody2, inN1, inN2, lambda);
+
+		// Store accumulated lambda
+		mTotalLambda += lambda;
+
+		return ApplyVelocityStep(ioBody1, ioBody2, inN1, inN2, lambda);
+	}
+
+	/// Iteratively update the position constraint. Makes sure C(...) = 0.
+	/// All input vectors are in world space
+	inline bool					SolvePositionConstraint(Body &ioBody1, Body &ioBody2, Vec3Arg inU, Vec3Arg inN1, Vec3Arg inN2, float inBaumgarte) const
+	{
+		Vec2 c;
+		c[0] = inU.Dot(inN1);
+		c[1] = inU.Dot(inN2);
+		if (!c.IsZero())
+		{
+			// Calculate lagrange multiplier (lambda) for Baumgarte stabilization:
+			//
+			// lambda = -K^-1 * beta / dt * C
+			//
+			// We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out
+			Vec2 lambda = -inBaumgarte * (mEffectiveMass * c);
+
+			// Directly integrate velocity change for one time step
+			//
+			// Euler velocity integration:
+			// dv = M^-1 P
+			//
+			// Impulse:
+			// P = J^T lambda
+			//
+			// Euler position integration:
+			// x' = x + dv * dt
+			//
+			// Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and
+			// Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte
+			// stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity
+			// integrate + a position integrate and then discard the velocity change.
+			Vec3 impulse = inN1 * lambda[0] + inN2 * lambda[1];
+			if (ioBody1.IsDynamic())
+			{
+				ioBody1.SubPositionStep(ioBody1.GetMotionProperties()->GetInverseMass() * impulse);
+				ioBody1.SubRotationStep(mInvI1_R1PlusUxN1 * lambda[0] + mInvI1_R1PlusUxN2 * lambda[1]);
+			}
+			if (ioBody2.IsDynamic())
+			{
+				ioBody2.AddPositionStep(ioBody2.GetMotionProperties()->GetInverseMass() * impulse);
+				ioBody2.AddRotationStep(mInvI2_R2xN1 * lambda[0] + mInvI2_R2xN2 * lambda[1]);
+			}
+			return true;
+		}
+
+		return false;
+	}
+
+	/// Override total lagrange multiplier, can be used to set the initial value for warm starting
+	inline void					SetTotalLambda(const Vec2 &inLambda)
+	{
+		mTotalLambda = inLambda;
+	}
+
+	/// Return lagrange multiplier
+	inline const Vec2 &			GetTotalLambda() const
+	{
+		return mTotalLambda;
+	}
+
+	/// Save state of this constraint part
+	void						SaveState(StateRecorder &inStream) const
+	{
+		inStream.Write(mTotalLambda);
+	}
+
+	/// Restore state of this constraint part
+	void						RestoreState(StateRecorder &inStream)
+	{
+		inStream.Read(mTotalLambda);
+	}
+
+private:
+	Vec3						mR1PlusUxN1;
+	Vec3						mR1PlusUxN2;
+	Vec3						mR2xN1;
+	Vec3						mR2xN2;
+	Vec3						mInvI1_R1PlusUxN1;
+	Vec3						mInvI1_R1PlusUxN2;
+	Vec3						mInvI2_R2xN1;
+	Vec3						mInvI2_R2xN2;
+	Mat22						mEffectiveMass;
+	Vec2						mTotalLambda { Vec2::sZero() };
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/GearConstraintPart.h

@@ -0,0 +1,195 @@
+// 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/StateRecorder.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Constraint that constrains two rotations using a gear (rotating in opposite direction)
+///
+/// Constraint equation:
+///
+/// C = Rotation1(t) + r Rotation2(t)
+///
+/// Derivative:
+///
+/// d/dt C = 0
+/// <=> w1 . a + r w2 . b = 0
+///
+/// Jacobian:
+///
+/// \f[J = \begin{bmatrix}0 & a^T & 0 & r b^T\end{bmatrix}\f]
+///
+/// Used terms (here and below, everything in world space):\n
+/// a = axis around which body 1 rotates (normalized).\n
+/// b = axis along which body 2 slides (normalized).\n
+/// Rotation1(t) = rotation around a of body 1.\n
+/// Rotation2(t) = rotation around b of body 2.\n
+/// r = ratio between rotation for body 1 and 2.\n
+/// v = [v1, w1, v2, w2].\n
+/// v1, v2 = linear velocity of body 1 and 2.\n
+/// w1, w2 = angular velocity of body 1 and 2.\n
+/// M = mass matrix, a diagonal matrix of the mass and inertia with diagonal [m1, I1, m2, I2].\n
+/// \f$K^{-1} = \left( J M^{-1} J^T \right)^{-1}\f$ = effective mass.\n
+/// \f$\beta\f$ = baumgarte constant.
+class GearConstraintPart
+{
+	/// Internal helper function to update velocities of bodies after Lagrange multiplier is calculated
+	JPH_INLINE bool				ApplyVelocityStep(Body &ioBody1, Body &ioBody2, float inLambda) const
+	{
+		// Apply impulse if delta is not zero
+		if (inLambda != 0.0f)
+		{
+			// Calculate velocity change due to constraint
+			//
+			// Impulse:
+			// P = J^T lambda
+			//
+			// Euler velocity integration:
+			// v' = v + M^-1 P
+			ioBody1.GetMotionProperties()->AddAngularVelocityStep(inLambda * mInvI1_A);
+			ioBody2.GetMotionProperties()->AddAngularVelocityStep(inLambda * mInvI2_B);
+			return true;
+		}
+
+		return false;
+	}
+
+public:
+	/// Calculate properties used during the functions below
+	/// @param inBody1 The first body that this constraint is attached to
+	/// @param inBody2 The second body that this constraint is attached to
+	/// @param inWorldSpaceHingeAxis1 The axis around which body 1 rotates
+	/// @param inWorldSpaceHingeAxis2 The axis around which body 2 rotates
+	/// @param inRatio The ratio between rotation and translation
+	inline void					CalculateConstraintProperties(const Body &inBody1, Vec3Arg inWorldSpaceHingeAxis1, const Body &inBody2, Vec3Arg inWorldSpaceHingeAxis2, float inRatio)
+	{
+		JPH_ASSERT(inWorldSpaceHingeAxis1.IsNormalized(1.0e-4f));
+		JPH_ASSERT(inWorldSpaceHingeAxis2.IsNormalized(1.0e-4f));
+
+		// Calculate: I1^-1 a
+		mInvI1_A = inBody1.GetMotionProperties()->MultiplyWorldSpaceInverseInertiaByVector(inBody1.GetRotation(), inWorldSpaceHingeAxis1);
+
+		// Calculate: I2^-1 b
+		mInvI2_B = inBody2.GetMotionProperties()->MultiplyWorldSpaceInverseInertiaByVector(inBody2.GetRotation(), inWorldSpaceHingeAxis2);
+
+		// K^-1 = 1 / (J M^-1 J^T) = 1 / (a^T I1^-1 a + r^2 * b^T I2^-1 b)
+		float inv_effective_mass = (inWorldSpaceHingeAxis1.Dot(mInvI1_A) + inWorldSpaceHingeAxis2.Dot(mInvI2_B) * Square(inRatio));
+		if (inv_effective_mass == 0.0f)
+			Deactivate();
+		else
+			mEffectiveMass = 1.0f / inv_effective_mass;
+	}
+
+	/// Deactivate this constraint
+	inline void					Deactivate()
+	{
+		mEffectiveMass = 0.0f;
+		mTotalLambda = 0.0f;
+	}
+
+	/// Check if constraint is active
+	inline bool					IsActive() const
+	{
+		return mEffectiveMass != 0.0f;
+	}
+
+	/// Must be called from the WarmStartVelocityConstraint call to apply the previous frame's impulses
+	/// @param ioBody1 The first body that this constraint is attached to
+	/// @param ioBody2 The second body that this constraint is attached to
+	/// @param inWarmStartImpulseRatio Ratio of new step to old time step (dt_new / dt_old) for scaling the lagrange multiplier of the previous frame
+	inline void					WarmStart(Body &ioBody1, Body &ioBody2, float inWarmStartImpulseRatio)
+	{
+		mTotalLambda *= inWarmStartImpulseRatio;
+		ApplyVelocityStep(ioBody1, ioBody2, mTotalLambda);
+	}
+
+	/// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation.
+	/// @param ioBody1 The first body that this constraint is attached to
+	/// @param ioBody2 The second body that this constraint is attached to
+	/// @param inWorldSpaceHingeAxis1 The axis around which body 1 rotates
+	/// @param inWorldSpaceHingeAxis2 The axis around which body 2 rotates
+	/// @param inRatio The ratio between rotation and translation
+	inline bool					SolveVelocityConstraint(Body &ioBody1, Vec3Arg inWorldSpaceHingeAxis1, Body &ioBody2, Vec3Arg inWorldSpaceHingeAxis2, float inRatio)
+	{
+		// Lagrange multiplier is:
+		//
+		// lambda = -K^-1 (J v + b)
+		float lambda = -mEffectiveMass * (inWorldSpaceHingeAxis1.Dot(ioBody1.GetAngularVelocity()) + inRatio * inWorldSpaceHingeAxis2.Dot(ioBody2.GetAngularVelocity()));
+		mTotalLambda += lambda; // Store accumulated impulse
+
+		return ApplyVelocityStep(ioBody1, ioBody2, lambda);
+	}
+
+	/// Return lagrange multiplier
+	float						GetTotalLambda() const
+	{
+		return mTotalLambda;
+	}
+
+	/// Iteratively update the position constraint. Makes sure C(...) == 0.
+	/// @param ioBody1 The first body that this constraint is attached to
+	/// @param ioBody2 The second body that this constraint is attached to
+	/// @param inC Value of the constraint equation (C)
+	/// @param inBaumgarte Baumgarte constant (fraction of the error to correct)
+	inline bool					SolvePositionConstraint(Body &ioBody1, Body &ioBody2, float inC, float inBaumgarte) const
+	{
+		// Only apply position constraint when the constraint is hard, otherwise the velocity bias will fix the constraint
+		if (inC != 0.0f)
+		{
+			// Calculate lagrange multiplier (lambda) for Baumgarte stabilization:
+			//
+			// lambda = -K^-1 * beta / dt * C
+			//
+			// We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out
+			float lambda = -mEffectiveMass * inBaumgarte * inC;
+
+			// Directly integrate velocity change for one time step
+			//
+			// Euler velocity integration:
+			// dv = M^-1 P
+			//
+			// Impulse:
+			// P = J^T lambda
+			//
+			// Euler position integration:
+			// x' = x + dv * dt
+			//
+			// Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and
+			// Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte
+			// stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity
+			// integrate + a position integrate and then discard the velocity change.
+			if (ioBody1.IsDynamic())
+				ioBody1.AddRotationStep(lambda * mInvI1_A);
+			if (ioBody2.IsDynamic())
+				ioBody2.AddRotationStep(lambda * mInvI2_B);
+			return true;
+		}
+
+		return false;
+	}
+
+	/// Save state of this constraint part
+	void						SaveState(StateRecorder &inStream) const
+	{
+		inStream.Write(mTotalLambda);
+	}
+
+	/// Restore state of this constraint part
+	void						RestoreState(StateRecorder &inStream)
+	{
+		inStream.Read(mTotalLambda);
+	}
+
+private:
+	Vec3						mInvI1_A;
+	Vec3						mInvI2_B;
+	float						mEffectiveMass = 0.0f;
+	float						mTotalLambda = 0.0f;
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/HingeRotationConstraintPart.h

@@ -0,0 +1,222 @@
+// 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/StateRecorder.h>
+#include <Jolt/Math/Vector.h>
+#include <Jolt/Math/Matrix.h>
+
+JPH_NAMESPACE_BEGIN
+
+/**
+	Constrains rotation around 2 axis so that it only allows rotation around 1 axis
+
+	Based on: "Constraints Derivation for Rigid Body Simulation in 3D" - Daniel Chappuis, section 2.4.1
+
+	Constraint equation (eq 87):
+
+	\f[C = \begin{bmatrix}a_1 \cdot b_2 \\ a_1 \cdot c_2\end{bmatrix}\f]
+
+	Jacobian (eq 90):
+
+	\f[J = \begin{bmatrix}
+	0	& -b_2 \times a_1	& 0		& b_2 \times a_1	\\
+	0	& -c_2 \times a_1	& 0		& c2 \times a_1
+	\end{bmatrix}\f]
+
+	Used terms (here and below, everything in world space):\n
+	a1 = hinge axis on body 1.\n
+	b2, c2 = axis perpendicular to hinge axis on body 2.\n
+	x1, x2 = center of mass for the bodies.\n
+	v = [v1, w1, v2, w2].\n
+	v1, v2 = linear velocity of body 1 and 2.\n
+	w1, w2 = angular velocity of body 1 and 2.\n
+	M = mass matrix, a diagonal matrix of the mass and inertia with diagonal [m1, I1, m2, I2].\n
+	\f$K^{-1} = \left( J M^{-1} J^T \right)^{-1}\f$ = effective mass.\n
+	b = velocity bias.\n
+	\f$\beta\f$ = baumgarte constant.\n
+	E = identity matrix.
+**/
+class HingeRotationConstraintPart
+{
+public:
+	using Vec2 = Vector<2>;
+	using Mat22 = Matrix<2, 2>;
+
+private:
+	/// Internal helper function to update velocities of bodies after Lagrange multiplier is calculated
+	JPH_INLINE bool				ApplyVelocityStep(Body &ioBody1, Body &ioBody2, const Vec2 &inLambda) const
+	{
+		// Apply impulse if delta is not zero
+		if (!inLambda.IsZero())
+		{
+			// Calculate velocity change due to constraint
+			//
+			// Impulse:
+			// P = J^T lambda
+			//
+			// Euler velocity integration:
+			// v' = v + M^-1 P
+			Vec3 impulse = mB2xA1 * inLambda[0] + mC2xA1 * inLambda[1];
+			if (ioBody1.IsDynamic())
+				ioBody1.GetMotionProperties()->SubAngularVelocityStep(mInvI1.Multiply3x3(impulse));
+			if (ioBody2.IsDynamic())
+				ioBody2.GetMotionProperties()->AddAngularVelocityStep(mInvI2.Multiply3x3(impulse));
+			return true;
+		}
+
+		return false;
+	}
+
+public:
+	/// Calculate properties used during the functions below
+	inline void					CalculateConstraintProperties(const Body &inBody1, Mat44Arg inRotation1, Vec3Arg inWorldSpaceHingeAxis1, const Body &inBody2, Mat44Arg inRotation2, Vec3Arg inWorldSpaceHingeAxis2)
+	{
+		JPH_ASSERT(inWorldSpaceHingeAxis1.IsNormalized(1.0e-5f));
+		JPH_ASSERT(inWorldSpaceHingeAxis2.IsNormalized(1.0e-5f));
+
+		// Calculate hinge axis in world space
+		mA1 = inWorldSpaceHingeAxis1;
+		Vec3 a2 = inWorldSpaceHingeAxis2;
+		float dot = mA1.Dot(a2);
+		if (dot <= 1.0e-3f)
+		{
+			// World space axes are more than 90 degrees apart, get a perpendicular vector in the plane formed by mA1 and a2 as hinge axis until the rotation is less than 90 degrees
+			Vec3 perp = a2 - dot * mA1;
+			if (perp.LengthSq() < 1.0e-6f)
+			{
+				// mA1 ~ -a2, take random perpendicular
+				perp = mA1.GetNormalizedPerpendicular();
+			}
+
+			// Blend in a little bit from mA1 so we're less than 90 degrees apart
+			a2 = (0.99f * perp.Normalized() + 0.01f * mA1).Normalized();
+		}
+		mB2 = a2.GetNormalizedPerpendicular();
+		mC2 = a2.Cross(mB2);
+
+		// Calculate properties used during constraint solving
+		mInvI1 = inBody1.IsDynamic()? inBody1.GetMotionProperties()->GetInverseInertiaForRotation(inRotation1) : Mat44::sZero();
+		mInvI2 = inBody2.IsDynamic()? inBody2.GetMotionProperties()->GetInverseInertiaForRotation(inRotation2) : Mat44::sZero();
+		mB2xA1 = mB2.Cross(mA1);
+		mC2xA1 = mC2.Cross(mA1);
+
+		// Calculate effective mass: K^-1 = (J M^-1 J^T)^-1
+		Mat44 summed_inv_inertia = mInvI1 + mInvI2;
+		Mat22 inv_effective_mass;
+		inv_effective_mass(0, 0) = mB2xA1.Dot(summed_inv_inertia.Multiply3x3(mB2xA1));
+		inv_effective_mass(0, 1) = mB2xA1.Dot(summed_inv_inertia.Multiply3x3(mC2xA1));
+		inv_effective_mass(1, 0) = mC2xA1.Dot(summed_inv_inertia.Multiply3x3(mB2xA1));
+		inv_effective_mass(1, 1) = mC2xA1.Dot(summed_inv_inertia.Multiply3x3(mC2xA1));
+		if (!mEffectiveMass.SetInversed(inv_effective_mass))
+			Deactivate();
+	}
+
+	/// Deactivate this constraint
+	inline void					Deactivate()
+	{
+		mEffectiveMass.SetZero();
+		mTotalLambda.SetZero();
+	}
+
+	/// Must be called from the WarmStartVelocityConstraint call to apply the previous frame's impulses
+	inline void					WarmStart(Body &ioBody1, Body &ioBody2, float inWarmStartImpulseRatio)
+	{
+		mTotalLambda *= inWarmStartImpulseRatio;
+		ApplyVelocityStep(ioBody1, ioBody2, mTotalLambda);
+	}
+
+	/// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation.
+	inline bool					SolveVelocityConstraint(Body &ioBody1, Body &ioBody2)
+	{
+		// Calculate lagrange multiplier:
+		//
+		// lambda = -K^-1 (J v + b)
+		Vec3 delta_ang = ioBody1.GetAngularVelocity() - ioBody2.GetAngularVelocity();
+		Vec2 jv;
+		jv[0] = mB2xA1.Dot(delta_ang);
+		jv[1] = mC2xA1.Dot(delta_ang);
+		Vec2 lambda = mEffectiveMass * jv;
+
+		// Store accumulated lambda
+		mTotalLambda += lambda;
+
+		return ApplyVelocityStep(ioBody1, ioBody2, lambda);
+	}
+
+	/// Iteratively update the position constraint. Makes sure C(...) = 0.
+	inline bool					SolvePositionConstraint(Body &ioBody1, Body &ioBody2, float inBaumgarte) const
+	{
+		// Constraint needs Axis of body 1 perpendicular to both B and C from body 2 (which are both perpendicular to the Axis of body 2)
+		Vec2 c;
+		c[0] = mA1.Dot(mB2);
+		c[1] = mA1.Dot(mC2);
+		if (!c.IsZero())
+		{
+			// Calculate lagrange multiplier (lambda) for Baumgarte stabilization:
+			//
+			// lambda = -K^-1 * beta / dt * C
+			//
+			// We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out
+			Vec2 lambda = -inBaumgarte * (mEffectiveMass * c);
+
+			// Directly integrate velocity change for one time step
+			//
+			// Euler velocity integration:
+			// dv = M^-1 P
+			//
+			// Impulse:
+			// P = J^T lambda
+			//
+			// Euler position integration:
+			// x' = x + dv * dt
+			//
+			// Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and
+			// Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte
+			// stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity
+			// integrate + a position integrate and then discard the velocity change.
+			Vec3 impulse = mB2xA1 * lambda[0] + mC2xA1 * lambda[1];
+			if (ioBody1.IsDynamic())
+				ioBody1.SubRotationStep(mInvI1.Multiply3x3(impulse));
+			if (ioBody2.IsDynamic())
+				ioBody2.AddRotationStep(mInvI2.Multiply3x3(impulse));
+			return true;
+		}
+
+		return false;
+	}
+
+	/// Return lagrange multiplier
+	const Vec2 &				GetTotalLambda() const
+	{
+		return mTotalLambda;
+	}
+
+	/// Save state of this constraint part
+	void						SaveState(StateRecorder &inStream) const
+	{
+		inStream.Write(mTotalLambda);
+	}
+
+	/// Restore state of this constraint part
+	void						RestoreState(StateRecorder &inStream)
+	{
+		inStream.Read(mTotalLambda);
+	}
+
+private:
+	Vec3						mA1;						///< World space hinge axis for body 1
+	Vec3						mB2;						///< World space perpendiculars of hinge axis for body 2
+	Vec3						mC2;
+	Mat44						mInvI1;
+	Mat44						mInvI2;
+	Vec3						mB2xA1;
+	Vec3						mC2xA1;
+	Mat22						mEffectiveMass;
+	Vec2						mTotalLambda { Vec2::sZero() };
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/IndependentAxisConstraintPart.h

@@ -0,0 +1,246 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2022 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Body/Body.h>
+#include <Jolt/Physics/StateRecorder.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Constraint part to an AxisConstraintPart but both bodies have an independent axis on which the force is applied.
+///
+/// Constraint equation:
+///
+/// \f[C = (x_1 + r_1 - f_1) . n_1 + r (x_2 + r_2 - f_2) \cdot n_2\f]
+///
+/// Calculating the Jacobian:
+///
+/// \f[dC/dt = (v_1 + w_1 \times r_1) \cdot n_1 + (x_1 + r_1 - f_1) \cdot d n_1/dt + r (v_2 + w_2 \times r_2) \cdot n_2 + r (x_2 + r_2 - f_2) \cdot d n_2/dt\f]
+///
+/// Assuming that d n1/dt and d n2/dt are small this becomes:
+///
+/// \f[(v_1 + w_1 \times r_1) \cdot n_1 + r (v_2 + w_2 \times r_2) \cdot n_2\f]
+/// \f[= v_1 \cdot n_1 + r_1 \times n_1 \cdot w_1 + r v_2 \cdot n_2 + r r_2 \times n_2 \cdot w_2\f]
+///
+/// Jacobian:
+///
+/// \f[J = \begin{bmatrix}n_1 & r_1 \times n_1 & r n_2 & r r_2 \times n_2\end{bmatrix}\f]
+///
+/// Effective mass:
+///
+/// \f[K = m_1^{-1} + r_1 \times n_1 I_1^{-1} r_1 \times n_1 + r^2 m_2^{-1} + r^2 r_2 \times n_2 I_2^{-1} r_2 \times n_2\f]
+///
+/// Used terms (here and below, everything in world space):\n
+/// n1 = (x1 + r1 - f1) / |x1 + r1 - f1|, axis along which the force is applied for body 1\n
+/// n2 = (x2 + r2 - f2) / |x2 + r2 - f2|, axis along which the force is applied for body 2\n
+/// r = ratio how forces are applied between bodies.\n
+/// x1, x2 = center of mass for the bodies.\n
+/// v = [v1, w1, v2, w2].\n
+/// v1, v2 = linear velocity of body 1 and 2.\n
+/// w1, w2 = angular velocity of body 1 and 2.\n
+/// M = mass matrix, a diagonal matrix of the mass and inertia with diagonal [m1, I1, m2, I2].\n
+/// \f$K^{-1} = \left( J M^{-1} J^T \right)^{-1}\f$ = effective mass.\n
+/// b = velocity bias.\n
+/// \f$\beta\f$ = baumgarte constant.
+class IndependentAxisConstraintPart
+{
+	/// Internal helper function to update velocities of bodies after Lagrange multiplier is calculated
+	JPH_INLINE bool				ApplyVelocityStep(Body &ioBody1, Body &ioBody2, Vec3Arg inN1, Vec3Arg inN2, float inRatio, float inLambda) const
+	{
+		// Apply impulse if delta is not zero
+		if (inLambda != 0.0f)
+		{
+			// Calculate velocity change due to constraint
+			//
+			// Impulse:
+			// P = J^T lambda
+			//
+			// Euler velocity integration:
+			// v' = v + M^-1 P
+			if (ioBody1.IsDynamic())
+			{
+				MotionProperties *mp1 = ioBody1.GetMotionProperties();
+				mp1->AddLinearVelocityStep((mp1->GetInverseMass() * inLambda) * inN1);
+				mp1->AddAngularVelocityStep(mInvI1_R1xN1 * inLambda);
+			}
+			if (ioBody2.IsDynamic())
+			{
+				MotionProperties *mp2 = ioBody2.GetMotionProperties();
+				mp2->AddLinearVelocityStep((inRatio * mp2->GetInverseMass() * inLambda) * inN2);
+				mp2->AddAngularVelocityStep(mInvI2_RatioR2xN2 * inLambda);
+			}
+			return true;
+		}
+
+		return false;
+	}
+
+public:
+	/// Calculate properties used during the functions below
+	/// @param inBody1 The first body that this constraint is attached to
+	/// @param inBody2 The second body that this constraint is attached to
+	/// @param inR1 The position on which the constraint operates on body 1 relative to COM
+	/// @param inN1 The world space normal in which the constraint operates for body 1
+	/// @param inR2 The position on which the constraint operates on body 1 relative to COM
+	/// @param inN2 The world space normal in which the constraint operates for body 2
+	/// @param inRatio The ratio how forces are applied between bodies
+	inline void					CalculateConstraintProperties(const Body &inBody1, const Body &inBody2, Vec3Arg inR1, Vec3Arg inN1, Vec3Arg inR2, Vec3Arg inN2, float inRatio)
+	{
+		JPH_ASSERT(inN1.IsNormalized(1.0e-4f) && inN2.IsNormalized(1.0e-4f));
+
+		float inv_effective_mass = 0.0f;
+
+		if (!inBody1.IsStatic())
+		{
+			const MotionProperties *mp1 = inBody1.GetMotionProperties();
+
+			mR1xN1 = inR1.Cross(inN1);
+			mInvI1_R1xN1 = mp1->MultiplyWorldSpaceInverseInertiaByVector(inBody1.GetRotation(), mR1xN1);
+
+			inv_effective_mass += mp1->GetInverseMass() + mInvI1_R1xN1.Dot(mR1xN1);
+		}
+
+		if (!inBody2.IsStatic())
+		{
+			const MotionProperties *mp2 = inBody2.GetMotionProperties();
+
+			mRatioR2xN2 = inRatio * inR2.Cross(inN2);
+			mInvI2_RatioR2xN2 = mp2->MultiplyWorldSpaceInverseInertiaByVector(inBody2.GetRotation(), mRatioR2xN2);
+
+			inv_effective_mass += Square(inRatio) * mp2->GetInverseMass() + mInvI2_RatioR2xN2.Dot(mRatioR2xN2);
+		}
+
+		// Calculate inverse effective mass: K = J M^-1 J^T
+		if (inv_effective_mass == 0.0f)
+			Deactivate();
+		else
+			mEffectiveMass = 1.0f / inv_effective_mass;
+	}
+
+	/// Deactivate this constraint
+	inline void					Deactivate()
+	{
+		mEffectiveMass = 0.0f;
+		mTotalLambda = 0.0f;
+	}
+
+	/// Check if constraint is active
+	inline bool					IsActive() const
+	{
+		return mEffectiveMass != 0.0f;
+	}
+
+	/// Must be called from the WarmStartVelocityConstraint call to apply the previous frame's impulses
+	/// @param ioBody1 The first body that this constraint is attached to
+	/// @param ioBody2 The second body that this constraint is attached to
+	/// @param inN1 The world space normal in which the constraint operates for body 1
+	/// @param inN2 The world space normal in which the constraint operates for body 2
+	/// @param inRatio The ratio how forces are applied between bodies
+	/// @param inWarmStartImpulseRatio Ratio of new step to old time step (dt_new / dt_old) for scaling the lagrange multiplier of the previous frame
+	inline void					WarmStart(Body &ioBody1, Body &ioBody2, Vec3Arg inN1, Vec3Arg inN2, float inRatio, float inWarmStartImpulseRatio)
+	{
+		mTotalLambda *= inWarmStartImpulseRatio;
+		ApplyVelocityStep(ioBody1, ioBody2, inN1, inN2, inRatio, mTotalLambda);
+	}
+
+	/// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation.
+	/// @param ioBody1 The first body that this constraint is attached to
+	/// @param ioBody2 The second body that this constraint is attached to
+	/// @param inN1 The world space normal in which the constraint operates for body 1
+	/// @param inN2 The world space normal in which the constraint operates for body 2
+	/// @param inRatio The ratio how forces are applied between bodies
+	/// @param inMinLambda Minimum angular impulse to apply (N m s)
+	/// @param inMaxLambda Maximum angular impulse to apply (N m s)
+	inline bool					SolveVelocityConstraint(Body &ioBody1, Body &ioBody2, Vec3Arg inN1, Vec3Arg inN2, float inRatio, float inMinLambda, float inMaxLambda)
+	{
+		// Lagrange multiplier is:
+		//
+		// lambda = -K^-1 (J v + b)
+		float lambda = -mEffectiveMass * (inN1.Dot(ioBody1.GetLinearVelocity()) + mR1xN1.Dot(ioBody1.GetAngularVelocity()) + inRatio * inN2.Dot(ioBody2.GetLinearVelocity()) + mRatioR2xN2.Dot(ioBody2.GetAngularVelocity()));
+		float new_lambda = Clamp(mTotalLambda + lambda, inMinLambda, inMaxLambda); // Clamp impulse
+		lambda = new_lambda - mTotalLambda; // Lambda potentially got clamped, calculate the new impulse to apply
+		mTotalLambda = new_lambda; // Store accumulated impulse
+
+		return ApplyVelocityStep(ioBody1, ioBody2, inN1, inN2, inRatio, lambda);
+	}
+
+	/// Return lagrange multiplier
+	float						GetTotalLambda() const
+	{
+		return mTotalLambda;
+	}
+
+	/// Iteratively update the position constraint. Makes sure C(...) == 0.
+	/// @param ioBody1 The first body that this constraint is attached to
+	/// @param ioBody2 The second body that this constraint is attached to
+	/// @param inN1 The world space normal in which the constraint operates for body 1
+	/// @param inN2 The world space normal in which the constraint operates for body 2
+	/// @param inRatio The ratio how forces are applied between bodies
+	/// @param inC Value of the constraint equation (C)
+	/// @param inBaumgarte Baumgarte constant (fraction of the error to correct)
+	inline bool					SolvePositionConstraint(Body &ioBody1, Body &ioBody2, Vec3Arg inN1, Vec3Arg inN2, float inRatio, float inC, float inBaumgarte) const
+	{
+		if (inC != 0.0f)
+		{
+			// Calculate lagrange multiplier (lambda) for Baumgarte stabilization:
+			//
+			// lambda = -K^-1 * beta / dt * C
+			//
+			// We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out
+			float lambda = -mEffectiveMass * inBaumgarte * inC;
+
+			// Directly integrate velocity change for one time step
+			//
+			// Euler velocity integration:
+			// dv = M^-1 P
+			//
+			// Impulse:
+			// P = J^T lambda
+			//
+			// Euler position integration:
+			// x' = x + dv * dt
+			//
+			// Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and
+			// Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte
+			// stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity
+			// integrate + a position integrate and then discard the velocity change.
+			if (ioBody1.IsDynamic())
+			{
+				ioBody1.AddPositionStep((lambda * ioBody1.GetMotionPropertiesUnchecked()->GetInverseMass()) * inN1);
+				ioBody1.AddRotationStep(lambda * mInvI1_R1xN1);
+			}
+			if (ioBody2.IsDynamic())
+			{
+				ioBody2.AddPositionStep((lambda * inRatio * ioBody2.GetMotionPropertiesUnchecked()->GetInverseMass()) * inN2);
+				ioBody2.AddRotationStep(lambda * mInvI2_RatioR2xN2);
+			}
+			return true;
+		}
+
+		return false;
+	}
+
+	/// Save state of this constraint part
+	void						SaveState(StateRecorder &inStream) const
+	{
+		inStream.Write(mTotalLambda);
+	}
+
+	/// Restore state of this constraint part
+	void						RestoreState(StateRecorder &inStream)
+	{
+		inStream.Read(mTotalLambda);
+	}
+
+private:
+	Vec3						mR1xN1;
+	Vec3						mInvI1_R1xN1;
+	Vec3						mRatioR2xN2;
+	Vec3						mInvI2_RatioR2xN2;
+	float						mEffectiveMass = 0.0f;
+	float						mTotalLambda = 0.0f;
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/PointConstraintPart.h

@@ -0,0 +1,239 @@
+// 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/StateRecorder.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Constrains movement along 3 axis
+///
+/// @see "Constraints Derivation for Rigid Body Simulation in 3D" - Daniel Chappuis, section 2.2.1
+///
+/// Constraint equation (eq 45):
+///
+/// \f[C = p_2 - p_1\f]
+///
+/// Jacobian (transposed) (eq 47):
+///
+/// \f[J^T = \begin{bmatrix}-E & r1x & E & -r2x^T\end{bmatrix}
+/// = \begin{bmatrix}-E^T \\ r1x^T \\ E^T \\ -r2x^T\end{bmatrix}
+/// = \begin{bmatrix}-E \\ -r1x \\ E \\ r2x\end{bmatrix}\f]
+///
+/// Used terms (here and below, everything in world space):\n
+/// p1, p2 = constraint points.\n
+/// r1 = p1 - x1.\n
+/// r2 = p2 - x2.\n
+/// r1x = 3x3 matrix for which r1x v = r1 x v (cross product).\n
+/// x1, x2 = center of mass for the bodies.\n
+/// v = [v1, w1, v2, w2].\n
+/// v1, v2 = linear velocity of body 1 and 2.\n
+/// w1, w2 = angular velocity of body 1 and 2.\n
+/// M = mass matrix, a diagonal matrix of the mass and inertia with diagonal [m1, I1, m2, I2].\n
+/// \f$K^{-1} = \left( J M^{-1} J^T \right)^{-1}\f$ = effective mass.\n
+/// b = velocity bias.\n
+/// \f$\beta\f$ = baumgarte constant.\n
+/// E = identity matrix.
+class PointConstraintPart
+{
+	JPH_INLINE bool				ApplyVelocityStep(Body &ioBody1, Body &ioBody2, Vec3Arg inLambda) const
+	{
+		// Apply impulse if delta is not zero
+		if (inLambda != Vec3::sZero())
+		{
+			// Calculate velocity change due to constraint
+			//
+			// Impulse:
+			// P = J^T lambda
+			//
+			// Euler velocity integration:
+			// v' = v + M^-1 P
+			if (ioBody1.IsDynamic())
+			{
+				MotionProperties *mp1 = ioBody1.GetMotionProperties();
+				mp1->SubLinearVelocityStep(mp1->GetInverseMass() * inLambda);
+				mp1->SubAngularVelocityStep(mInvI1_R1X * inLambda);
+			}
+			if (ioBody2.IsDynamic())
+			{
+				MotionProperties *mp2 = ioBody2.GetMotionProperties();
+				mp2->AddLinearVelocityStep(mp2->GetInverseMass() * inLambda);
+				mp2->AddAngularVelocityStep(mInvI2_R2X * inLambda);
+			}
+			return true;
+		}
+
+		return false;
+	}
+
+public:
+	/// Calculate properties used during the functions below
+	/// @param inBody1 The first body that this constraint is attached to
+	/// @param inBody2 The second body that this constraint is attached to
+	/// @param inRotation1 The 3x3 rotation matrix for body 1 (translation part is ignored)
+	/// @param inRotation2 The 3x3 rotation matrix for body 2 (translation part is ignored)
+	/// @param inR1 Local space vector from center of mass to constraint point for body 1
+	/// @param inR2 Local space vector from center of mass to constraint point for body 2
+	inline void					CalculateConstraintProperties(const Body &inBody1, Mat44Arg inRotation1, Vec3Arg inR1, const Body &inBody2, Mat44Arg inRotation2, Vec3Arg inR2)
+	{
+		// Positions where the point constraint acts on (middle point between center of masses) in world space
+		mR1 = inRotation1.Multiply3x3(inR1);
+		mR2 = inRotation2.Multiply3x3(inR2);
+
+		// Calculate effective mass: K^-1 = (J M^-1 J^T)^-1
+		// Using: I^-1 = R * Ibody^-1 * R^T
+		float summed_inv_mass;
+		Mat44 inv_effective_mass;
+		if (inBody1.IsDynamic())
+		{
+			const MotionProperties *mp1 = inBody1.GetMotionProperties();
+			Mat44 inv_i1 = mp1->GetInverseInertiaForRotation(inRotation1);
+			summed_inv_mass = mp1->GetInverseMass();
+
+			Mat44 r1x = Mat44::sCrossProduct(mR1);
+			mInvI1_R1X = inv_i1.Multiply3x3(r1x);
+			inv_effective_mass = r1x.Multiply3x3(inv_i1).Multiply3x3RightTransposed(r1x);
+		}
+		else
+		{
+			JPH_IF_DEBUG(mInvI1_R1X = Mat44::sNaN();)
+
+			summed_inv_mass = 0.0f;
+			inv_effective_mass = Mat44::sZero();
+		}
+
+		if (inBody2.IsDynamic())
+		{
+			const MotionProperties *mp2 = inBody2.GetMotionProperties();
+			Mat44 inv_i2 = mp2->GetInverseInertiaForRotation(inRotation2);
+			summed_inv_mass += mp2->GetInverseMass();
+
+			Mat44 r2x = Mat44::sCrossProduct(mR2);
+			mInvI2_R2X = inv_i2.Multiply3x3(r2x);
+			inv_effective_mass += r2x.Multiply3x3(inv_i2).Multiply3x3RightTransposed(r2x);
+		}
+		else
+		{
+			JPH_IF_DEBUG(mInvI2_R2X = Mat44::sNaN();)
+		}
+
+		inv_effective_mass += Mat44::sScale(summed_inv_mass);
+		if (!mEffectiveMass.SetInversed3x3(inv_effective_mass))
+			Deactivate();
+	}
+
+	/// Deactivate this constraint
+	inline void					Deactivate()
+	{
+		mEffectiveMass = Mat44::sZero();
+		mTotalLambda = Vec3::sZero();
+	}
+
+	/// Check if constraint is active
+	inline bool					IsActive() const
+	{
+		return mEffectiveMass(3, 3) != 0.0f;
+	}
+
+	/// Must be called from the WarmStartVelocityConstraint call to apply the previous frame's impulses
+	/// @param ioBody1 The first body that this constraint is attached to
+	/// @param ioBody2 The second body that this constraint is attached to
+	/// @param inWarmStartImpulseRatio Ratio of new step to old time step (dt_new / dt_old) for scaling the lagrange multiplier of the previous frame
+	inline void					WarmStart(Body &ioBody1, Body &ioBody2, float inWarmStartImpulseRatio)
+	{
+		mTotalLambda *= inWarmStartImpulseRatio;
+		ApplyVelocityStep(ioBody1, ioBody2, mTotalLambda);
+	}
+
+	/// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation.
+	/// @param ioBody1 The first body that this constraint is attached to
+	/// @param ioBody2 The second body that this constraint is attached to
+	inline bool					SolveVelocityConstraint(Body &ioBody1, Body &ioBody2)
+	{
+		// Calculate lagrange multiplier:
+		//
+		// lambda = -K^-1 (J v + b)
+		Vec3 lambda = mEffectiveMass * (ioBody1.GetLinearVelocity() - mR1.Cross(ioBody1.GetAngularVelocity()) - ioBody2.GetLinearVelocity() + mR2.Cross(ioBody2.GetAngularVelocity()));
+		mTotalLambda += lambda; // Store accumulated lambda
+		return ApplyVelocityStep(ioBody1, ioBody2, lambda);
+	}
+
+	/// Iteratively update the position constraint. Makes sure C(...) = 0.
+	/// @param ioBody1 The first body that this constraint is attached to
+	/// @param ioBody2 The second body that this constraint is attached to
+	/// @param inBaumgarte Baumgarte constant (fraction of the error to correct)
+	inline bool					SolvePositionConstraint(Body &ioBody1, Body &ioBody2, float inBaumgarte) const
+	{
+		Vec3 separation = (Vec3(ioBody2.GetCenterOfMassPosition() - ioBody1.GetCenterOfMassPosition()) + mR2 - mR1);
+		if (separation != Vec3::sZero())
+		{
+			// Calculate lagrange multiplier (lambda) for Baumgarte stabilization:
+			//
+			// lambda = -K^-1 * beta / dt * C
+			//
+			// We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out
+			Vec3 lambda = mEffectiveMass * -inBaumgarte * separation;
+
+			// Directly integrate velocity change for one time step
+			//
+			// Euler velocity integration:
+			// dv = M^-1 P
+			//
+			// Impulse:
+			// P = J^T lambda
+			//
+			// Euler position integration:
+			// x' = x + dv * dt
+			//
+			// Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and
+			// Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte
+			// stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity
+			// integrate + a position integrate and then discard the velocity change.
+			if (ioBody1.IsDynamic())
+			{
+				ioBody1.SubPositionStep(ioBody1.GetMotionProperties()->GetInverseMass() * lambda);
+				ioBody1.SubRotationStep(mInvI1_R1X * lambda);
+			}
+			if (ioBody2.IsDynamic())
+			{
+				ioBody2.AddPositionStep(ioBody2.GetMotionProperties()->GetInverseMass() * lambda);
+				ioBody2.AddRotationStep(mInvI2_R2X * lambda);
+			}
+
+			return true;
+		}
+
+		return false;
+	}
+
+	/// Return lagrange multiplier
+	Vec3						GetTotalLambda() const
+	{
+		return mTotalLambda;
+	}
+
+	/// Save state of this constraint part
+	void						SaveState(StateRecorder &inStream) const
+	{
+		inStream.Write(mTotalLambda);
+	}
+
+	/// Restore state of this constraint part
+	void						RestoreState(StateRecorder &inStream)
+	{
+		inStream.Read(mTotalLambda);
+	}
+
+private:
+	Vec3						mR1;
+	Vec3						mR2;
+	Mat44						mInvI1_R1X;
+	Mat44						mInvI2_R2X;
+	Mat44						mEffectiveMass;
+	Vec3						mTotalLambda { Vec3::sZero() };
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/RackAndPinionConstraintPart.h

@@ -0,0 +1,196 @@
+// 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/StateRecorder.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Constraint that constrains a rotation to a translation
+///
+/// Constraint equation:
+///
+/// C = Theta(t) - r d(t)
+///
+/// Derivative:
+///
+/// d/dt C = 0
+/// <=> w1 . a - r v2 . b = 0
+///
+/// Jacobian:
+///
+/// \f[J = \begin{bmatrix}0 & a^T & -r b^T & 0\end{bmatrix}\f]
+///
+/// Used terms (here and below, everything in world space):\n
+/// a = axis around which body 1 rotates (normalized).\n
+/// b = axis along which body 2 slides (normalized).\n
+/// Theta(t) = rotation around a of body 1.\n
+/// d(t) = distance body 2 slides.\n
+/// r = ratio between rotation and translation.\n
+/// v = [v1, w1, v2, w2].\n
+/// v1, v2 = linear velocity of body 1 and 2.\n
+/// w1, w2 = angular velocity of body 1 and 2.\n
+/// M = mass matrix, a diagonal matrix of the mass and inertia with diagonal [m1, I1, m2, I2].\n
+/// \f$K^{-1} = \left( J M^{-1} J^T \right)^{-1}\f$ = effective mass.\n
+/// \f$\beta\f$ = baumgarte constant.
+class RackAndPinionConstraintPart
+{
+	/// Internal helper function to update velocities of bodies after Lagrange multiplier is calculated
+	JPH_INLINE bool				ApplyVelocityStep(Body &ioBody1, Body &ioBody2, float inLambda) const
+	{
+		// Apply impulse if delta is not zero
+		if (inLambda != 0.0f)
+		{
+			// Calculate velocity change due to constraint
+			//
+			// Impulse:
+			// P = J^T lambda
+			//
+			// Euler velocity integration:
+			// v' = v + M^-1 P
+			ioBody1.GetMotionProperties()->AddAngularVelocityStep(inLambda * mInvI1_A);
+			ioBody2.GetMotionProperties()->SubLinearVelocityStep(inLambda * mRatio_InvM2_B);
+			return true;
+		}
+
+		return false;
+	}
+
+public:
+	/// Calculate properties used during the functions below
+	/// @param inBody1 The first body that this constraint is attached to
+	/// @param inBody2 The second body that this constraint is attached to
+	/// @param inWorldSpaceHingeAxis The axis around which body 1 rotates
+	/// @param inWorldSpaceSliderAxis The axis along which body 2 slides
+	/// @param inRatio The ratio between rotation and translation
+	inline void					CalculateConstraintProperties(const Body &inBody1, Vec3Arg inWorldSpaceHingeAxis, const Body &inBody2, Vec3Arg inWorldSpaceSliderAxis, float inRatio)
+	{
+		JPH_ASSERT(inWorldSpaceHingeAxis.IsNormalized(1.0e-4f));
+		JPH_ASSERT(inWorldSpaceSliderAxis.IsNormalized(1.0e-4f));
+
+		// Calculate: I1^-1 a
+		mInvI1_A = inBody1.GetMotionProperties()->MultiplyWorldSpaceInverseInertiaByVector(inBody1.GetRotation(), inWorldSpaceHingeAxis);
+
+		// Calculate: r/m2 b
+		float inv_m2 = inBody2.GetMotionProperties()->GetInverseMass();
+		mRatio_InvM2_B = inRatio * inv_m2 * inWorldSpaceSliderAxis;
+
+		// K^-1 = 1 / (J M^-1 J^T) = 1 / (a^T I1^-1 a + 1/m2 * r^2 * b . b)
+		float inv_effective_mass = (inWorldSpaceHingeAxis.Dot(mInvI1_A) + inv_m2 * Square(inRatio));
+		if (inv_effective_mass == 0.0f)
+			Deactivate();
+		else
+			mEffectiveMass = 1.0f / inv_effective_mass;
+	}
+
+	/// Deactivate this constraint
+	inline void					Deactivate()
+	{
+		mEffectiveMass = 0.0f;
+		mTotalLambda = 0.0f;
+	}
+
+	/// Check if constraint is active
+	inline bool					IsActive() const
+	{
+		return mEffectiveMass != 0.0f;
+	}
+
+	/// Must be called from the WarmStartVelocityConstraint call to apply the previous frame's impulses
+	/// @param ioBody1 The first body that this constraint is attached to
+	/// @param ioBody2 The second body that this constraint is attached to
+	/// @param inWarmStartImpulseRatio Ratio of new step to old time step (dt_new / dt_old) for scaling the lagrange multiplier of the previous frame
+	inline void					WarmStart(Body &ioBody1, Body &ioBody2, float inWarmStartImpulseRatio)
+	{
+		mTotalLambda *= inWarmStartImpulseRatio;
+		ApplyVelocityStep(ioBody1, ioBody2, mTotalLambda);
+	}
+
+	/// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation.
+	/// @param ioBody1 The first body that this constraint is attached to
+	/// @param ioBody2 The second body that this constraint is attached to
+	/// @param inWorldSpaceHingeAxis The axis around which body 1 rotates
+	/// @param inWorldSpaceSliderAxis The axis along which body 2 slides
+	/// @param inRatio The ratio between rotation and translation
+	inline bool					SolveVelocityConstraint(Body &ioBody1, Vec3Arg inWorldSpaceHingeAxis, Body &ioBody2, Vec3Arg inWorldSpaceSliderAxis, float inRatio)
+	{
+		// Lagrange multiplier is:
+		//
+		// lambda = -K^-1 (J v + b)
+		float lambda = mEffectiveMass * (inRatio * inWorldSpaceSliderAxis.Dot(ioBody2.GetLinearVelocity()) - inWorldSpaceHingeAxis.Dot(ioBody1.GetAngularVelocity()));
+		mTotalLambda += lambda; // Store accumulated impulse
+
+		return ApplyVelocityStep(ioBody1, ioBody2, lambda);
+	}
+
+	/// Return lagrange multiplier
+	float						GetTotalLambda() const
+	{
+		return mTotalLambda;
+	}
+
+	/// Iteratively update the position constraint. Makes sure C(...) == 0.
+	/// @param ioBody1 The first body that this constraint is attached to
+	/// @param ioBody2 The second body that this constraint is attached to
+	/// @param inC Value of the constraint equation (C)
+	/// @param inBaumgarte Baumgarte constant (fraction of the error to correct)
+	inline bool					SolvePositionConstraint(Body &ioBody1, Body &ioBody2, float inC, float inBaumgarte) const
+	{
+		// Only apply position constraint when the constraint is hard, otherwise the velocity bias will fix the constraint
+		if (inC != 0.0f)
+		{
+			// Calculate lagrange multiplier (lambda) for Baumgarte stabilization:
+			//
+			// lambda = -K^-1 * beta / dt * C
+			//
+			// We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out
+			float lambda = -mEffectiveMass * inBaumgarte * inC;
+
+			// Directly integrate velocity change for one time step
+			//
+			// Euler velocity integration:
+			// dv = M^-1 P
+			//
+			// Impulse:
+			// P = J^T lambda
+			//
+			// Euler position integration:
+			// x' = x + dv * dt
+			//
+			// Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and
+			// Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte
+			// stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity
+			// integrate + a position integrate and then discard the velocity change.
+			if (ioBody1.IsDynamic())
+				ioBody1.AddRotationStep(lambda * mInvI1_A);
+			if (ioBody2.IsDynamic())
+				ioBody2.SubPositionStep(lambda * mRatio_InvM2_B);
+			return true;
+		}
+
+		return false;
+	}
+
+	/// Save state of this constraint part
+	void						SaveState(StateRecorder &inStream) const
+	{
+		inStream.Write(mTotalLambda);
+	}
+
+	/// Restore state of this constraint part
+	void						RestoreState(StateRecorder &inStream)
+	{
+		inStream.Read(mTotalLambda);
+	}
+
+private:
+	Vec3						mInvI1_A;
+	Vec3						mRatio_InvM2_B;
+	float						mEffectiveMass = 0.0f;
+	float						mTotalLambda = 0.0f;
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/RotationEulerConstraintPart.h

@@ -0,0 +1,270 @@
+// 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/StateRecorder.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Constrains rotation around all axis so that only translation is allowed
+///
+/// Based on: "Constraints Derivation for Rigid Body Simulation in 3D" - Daniel Chappuis, section 2.5.1
+///
+/// Constraint equation (eq 129):
+///
+/// \f[C = \begin{bmatrix}\Delta\theta_x, \Delta\theta_y, \Delta\theta_z\end{bmatrix}\f]
+///
+/// Jacobian (eq 131):
+///
+/// \f[J = \begin{bmatrix}0 & -E & 0 & E\end{bmatrix}\f]
+///
+/// Used terms (here and below, everything in world space):\n
+/// delta_theta_* = difference in rotation between initial rotation of bodies 1 and 2.\n
+/// x1, x2 = center of mass for the bodies.\n
+/// v = [v1, w1, v2, w2].\n
+/// v1, v2 = linear velocity of body 1 and 2.\n
+/// w1, w2 = angular velocity of body 1 and 2.\n
+/// M = mass matrix, a diagonal matrix of the mass and inertia with diagonal [m1, I1, m2, I2].\n
+/// \f$K^{-1} = \left( J M^{-1} J^T \right)^{-1}\f$ = effective mass.\n
+/// b = velocity bias.\n
+/// \f$\beta\f$ = baumgarte constant.\n
+/// E = identity matrix.\n
+class RotationEulerConstraintPart
+{
+private:
+	/// Internal helper function to update velocities of bodies after Lagrange multiplier is calculated
+	JPH_INLINE bool				ApplyVelocityStep(Body &ioBody1, Body &ioBody2, Vec3Arg inLambda) const
+	{
+		// Apply impulse if delta is not zero
+		if (inLambda != Vec3::sZero())
+		{
+			// Calculate velocity change due to constraint
+			//
+			// Impulse:
+			// P = J^T lambda
+			//
+			// Euler velocity integration:
+			// v' = v + M^-1 P
+			if (ioBody1.IsDynamic())
+				ioBody1.GetMotionProperties()->SubAngularVelocityStep(mInvI1.Multiply3x3(inLambda));
+			if (ioBody2.IsDynamic())
+				ioBody2.GetMotionProperties()->AddAngularVelocityStep(mInvI2.Multiply3x3(inLambda));
+			return true;
+		}
+
+		return false;
+	}
+
+public:
+	/// Return inverse of initial rotation from body 1 to body 2 in body 1 space
+	static Quat					sGetInvInitialOrientation(const Body &inBody1, const Body &inBody2)
+	{
+		// q20 = q10 r0
+		// <=> r0 = q10^-1 q20
+		// <=> r0^-1 = q20^-1 q10
+		//
+		// where:
+		//
+		// q20 = initial orientation of body 2
+		// q10 = initial orientation of body 1
+		// r0 = initial rotation from body 1 to body 2
+		return inBody2.GetRotation().Conjugated() * inBody1.GetRotation();
+	}
+
+	/// @brief Return inverse of initial rotation from body 1 to body 2 in body 1 space
+	/// @param inAxisX1 Reference axis X for body 1
+	/// @param inAxisY1 Reference axis Y for body 1
+	/// @param inAxisX2 Reference axis X for body 2
+	/// @param inAxisY2 Reference axis Y for body 2
+	static Quat					sGetInvInitialOrientationXY(Vec3Arg inAxisX1, Vec3Arg inAxisY1, Vec3Arg inAxisX2, Vec3Arg inAxisY2)
+	{
+		// Store inverse of initial rotation from body 1 to body 2 in body 1 space:
+		//
+		// q20 = q10 r0
+		// <=> r0 = q10^-1 q20
+		// <=> r0^-1 = q20^-1 q10
+		//
+		// where:
+		//
+		// q10, q20 = world space initial orientation of body 1 and 2
+		// r0 = initial rotation from body 1 to body 2 in local space of body 1
+		//
+		// We can also write this in terms of the constraint matrices:
+		//
+		// q20 c2 = q10 c1
+		// <=> q20 = q10 c1 c2^-1
+		// => r0 = c1 c2^-1
+		// <=> r0^-1 = c2 c1^-1
+		//
+		// where:
+		//
+		// c1, c2 = matrix that takes us from body 1 and 2 COM to constraint space 1 and 2
+		if (inAxisX1 == inAxisX2 && inAxisY1 == inAxisY2)
+		{
+			// Axis are the same -> identity transform
+			return Quat::sIdentity();
+		}
+		else
+		{
+			Mat44 constraint1(Vec4(inAxisX1, 0), Vec4(inAxisY1, 0), Vec4(inAxisX1.Cross(inAxisY1), 0), Vec4(0, 0, 0, 1));
+			Mat44 constraint2(Vec4(inAxisX2, 0), Vec4(inAxisY2, 0), Vec4(inAxisX2.Cross(inAxisY2), 0), Vec4(0, 0, 0, 1));
+			return constraint2.GetQuaternion() * constraint1.GetQuaternion().Conjugated();
+		}
+	}
+
+	/// @brief Return inverse of initial rotation from body 1 to body 2 in body 1 space
+	/// @param inAxisX1 Reference axis X for body 1
+	/// @param inAxisZ1 Reference axis Z for body 1
+	/// @param inAxisX2 Reference axis X for body 2
+	/// @param inAxisZ2 Reference axis Z for body 2
+	static Quat					sGetInvInitialOrientationXZ(Vec3Arg inAxisX1, Vec3Arg inAxisZ1, Vec3Arg inAxisX2, Vec3Arg inAxisZ2)
+	{
+		// See comment at sGetInvInitialOrientationXY
+		if (inAxisX1 == inAxisX2 && inAxisZ1 == inAxisZ2)
+		{
+			return Quat::sIdentity();
+		}
+		else
+		{
+			Mat44 constraint1(Vec4(inAxisX1, 0), Vec4(inAxisZ1.Cross(inAxisX1), 0), Vec4(inAxisZ1, 0), Vec4(0, 0, 0, 1));
+			Mat44 constraint2(Vec4(inAxisX2, 0), Vec4(inAxisZ2.Cross(inAxisX2), 0), Vec4(inAxisZ2, 0), Vec4(0, 0, 0, 1));
+			return constraint2.GetQuaternion() * constraint1.GetQuaternion().Conjugated();
+		}
+	}
+
+	/// Calculate properties used during the functions below
+	inline void					CalculateConstraintProperties(const Body &inBody1, Mat44Arg inRotation1, const Body &inBody2, Mat44Arg inRotation2)
+	{
+		// Calculate properties used during constraint solving
+		mInvI1 = inBody1.IsDynamic()? inBody1.GetMotionProperties()->GetInverseInertiaForRotation(inRotation1) : Mat44::sZero();
+		mInvI2 = inBody2.IsDynamic()? inBody2.GetMotionProperties()->GetInverseInertiaForRotation(inRotation2) : Mat44::sZero();
+
+		// Calculate effective mass: K^-1 = (J M^-1 J^T)^-1
+		if (!mEffectiveMass.SetInversed3x3(mInvI1 + mInvI2))
+			Deactivate();
+	}
+
+	/// Deactivate this constraint
+	inline void					Deactivate()
+	{
+		mEffectiveMass = Mat44::sZero();
+		mTotalLambda = Vec3::sZero();
+	}
+
+	/// Check if constraint is active
+	inline bool					IsActive() const
+	{
+		return mEffectiveMass(3, 3) != 0.0f;
+	}
+
+	/// Must be called from the WarmStartVelocityConstraint call to apply the previous frame's impulses
+	inline void					WarmStart(Body &ioBody1, Body &ioBody2, float inWarmStartImpulseRatio)
+	{
+		mTotalLambda *= inWarmStartImpulseRatio;
+		ApplyVelocityStep(ioBody1, ioBody2, mTotalLambda);
+	}
+
+	/// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation.
+	inline bool					SolveVelocityConstraint(Body &ioBody1, Body &ioBody2)
+	{
+		// Calculate lagrange multiplier:
+		//
+		// lambda = -K^-1 (J v + b)
+		Vec3 lambda = mEffectiveMass.Multiply3x3(ioBody1.GetAngularVelocity() - ioBody2.GetAngularVelocity());
+		mTotalLambda += lambda;
+		return ApplyVelocityStep(ioBody1, ioBody2, lambda);
+	}
+
+	/// Iteratively update the position constraint. Makes sure C(...) = 0.
+	inline bool					SolvePositionConstraint(Body &ioBody1, Body &ioBody2, QuatArg inInvInitialOrientation, float inBaumgarte) const
+	{
+		// Calculate difference in rotation
+		//
+		// The rotation should be:
+		//
+		// q2 = q1 r0
+		//
+		// But because of drift the actual rotation is
+		//
+		// q2 = diff q1 r0
+		// <=> diff = q2 r0^-1 q1^-1
+		//
+		// Where:
+		// q1 = current rotation of body 1
+		// q2 = current rotation of body 2
+		// diff = error that needs to be reduced to zero
+		Quat diff = ioBody2.GetRotation() * inInvInitialOrientation * ioBody1.GetRotation().Conjugated();
+
+		// A quaternion can be seen as:
+		//
+		// q = [sin(theta / 2) * v, cos(theta/2)]
+		//
+		// Where:
+		// v = rotation vector
+		// theta = rotation angle
+		//
+		// If we assume theta is small (error is small) then sin(x) = x so an approximation of the error angles is:
+		Vec3 error = 2.0f * diff.EnsureWPositive().GetXYZ();
+		if (error != Vec3::sZero())
+		{
+			// Calculate lagrange multiplier (lambda) for Baumgarte stabilization:
+			//
+			// lambda = -K^-1 * beta / dt * C
+			//
+			// We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out
+			Vec3 lambda = -inBaumgarte * mEffectiveMass * error;
+
+			// Directly integrate velocity change for one time step
+			//
+			// Euler velocity integration:
+			// dv = M^-1 P
+			//
+			// Impulse:
+			// P = J^T lambda
+			//
+			// Euler position integration:
+			// x' = x + dv * dt
+			//
+			// Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and
+			// Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte
+			// stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity
+			// integrate + a position integrate and then discard the velocity change.
+			if (ioBody1.IsDynamic())
+				ioBody1.SubRotationStep(mInvI1.Multiply3x3(lambda));
+			if (ioBody2.IsDynamic())
+				ioBody2.AddRotationStep(mInvI2.Multiply3x3(lambda));
+			return true;
+		}
+
+		return false;
+	}
+
+	/// Return lagrange multiplier
+	Vec3						GetTotalLambda() const
+	{
+		return mTotalLambda;
+	}
+
+	/// Save state of this constraint part
+	void						SaveState(StateRecorder &inStream) const
+	{
+		inStream.Write(mTotalLambda);
+	}
+
+	/// Restore state of this constraint part
+	void						RestoreState(StateRecorder &inStream)
+	{
+		inStream.Read(mTotalLambda);
+	}
+
+private:
+	Mat44						mInvI1;
+	Mat44						mInvI2;
+	Mat44						mEffectiveMass;
+	Vec3						mTotalLambda { Vec3::sZero() };
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/RotationQuatConstraintPart.h

@@ -0,0 +1,246 @@
+// 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/StateRecorder.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Quaternion based constraint that constrains rotation around all axis so that only translation is allowed.
+///
+/// NOTE: This constraint part is more expensive than the RotationEulerConstraintPart and slightly more correct since
+/// RotationEulerConstraintPart::SolvePositionConstraint contains an approximation. In practice the difference
+/// is small, so the RotationEulerConstraintPart is probably the better choice.
+///
+/// Rotation is fixed between bodies like this:
+///
+/// q2 = q1 r0
+///
+/// Where:
+/// q1, q2 = world space quaternions representing rotation of body 1 and 2.
+/// r0 = initial rotation between bodies in local space of body 1, this can be calculated by:
+///
+/// q20 = q10 r0
+/// <=> r0 = q10^* q20
+///
+/// Where:
+/// q10, q20 = initial world space rotations of body 1 and 2.
+/// q10^* = conjugate of quaternion q10 (which is the same as the inverse for a unit quaternion)
+///
+/// We exclusively use the conjugate below:
+///
+/// r0^* = q20^* q10
+///
+/// The error in the rotation is (in local space of body 1):
+///
+/// q2 = q1 error r0
+/// <=> error = q1^* q2 r0^*
+///
+/// The imaginary part of the quaternion represents the rotation axis * sin(angle / 2). The real part of the quaternion
+/// does not add any additional information (we know the quaternion in normalized) and we're removing 3 degrees of freedom
+/// so we want 3 parameters. Therefore we define the constraint equation like:
+///
+/// C = A q1^* q2 r0^* = 0
+///
+/// Where (if you write a quaternion as [real-part, i-part, j-part, k-part]):
+///
+///		    [0, 1, 0, 0]
+///		A = [0, 0, 1, 0]
+///		    [0, 0, 0, 1]
+///
+/// or in our case since we store a quaternion like [i-part, j-part, k-part, real-part]:
+///
+///		    [1, 0, 0, 0]
+///		A = [0, 1, 0, 0]
+///		    [0, 0, 1, 0]
+///
+/// Time derivative:
+///
+/// d/dt C = A (q1^* d/dt(q2) + d/dt(q1^*) q2) r0^*
+/// = A (q1^* (1/2 W2 q2) + (1/2 W1 q1)^* q2) r0^*
+/// = 1/2 A (q1^* W2 q2 + q1^* W1^* q2) r0^*
+/// = 1/2 A (q1^* W2 q2 - q1^* W1 * q2) r0^*
+/// = 1/2 A ML(q1^*) MR(q2 r0^*) (W2 - W1)
+/// = 1/2 A ML(q1^*) MR(q2 r0^*) A^T (w2 - w1)
+///
+/// Where:
+/// W1 = [0, w1], W2 = [0, w2] (converting angular velocity to imaginary part of quaternion).
+/// w1, w2 = angular velocity of body 1 and 2.
+/// d/dt(q) = 1/2 W q (time derivative of a quaternion).
+/// W^* = -W (conjugate negates angular velocity as quaternion).
+/// ML(q): 4x4 matrix so that q * p = ML(q) * p, where q and p are quaternions.
+/// MR(p): 4x4 matrix so that q * p = MR(p) * q, where q and p are quaternions.
+/// A^T: Transpose of A.
+///
+/// Jacobian:
+///
+/// J = [0, -1/2 A ML(q1^*) MR(q2 r0^*) A^T, 0, 1/2 A ML(q1^*) MR(q2 r0^*) A^T]
+/// = [0, -JP, 0, JP]
+///
+/// Suggested reading:
+/// - 3D Constraint Derivations for Impulse Solvers - Marijn Tamis
+/// - Game Physics Pearls - Section 9 - Quaternion Based Constraints - Claude Lacoursiere
+class RotationQuatConstraintPart
+{
+private:
+	/// Internal helper function to update velocities of bodies after Lagrange multiplier is calculated
+	JPH_INLINE bool				ApplyVelocityStep(Body &ioBody1, Body &ioBody2, Vec3Arg inLambda) const
+	{
+		// Apply impulse if delta is not zero
+		if (inLambda != Vec3::sZero())
+		{
+			// Calculate velocity change due to constraint
+			//
+			// Impulse:
+			// P = J^T lambda
+			//
+			// Euler velocity integration:
+			// v' = v + M^-1 P
+			if (ioBody1.IsDynamic())
+				ioBody1.GetMotionProperties()->SubAngularVelocityStep(mInvI1_JPT.Multiply3x3(inLambda));
+			if (ioBody2.IsDynamic())
+				ioBody2.GetMotionProperties()->AddAngularVelocityStep(mInvI2_JPT.Multiply3x3(inLambda));
+			return true;
+		}
+
+		return false;
+	}
+
+public:
+	/// Return inverse of initial rotation from body 1 to body 2 in body 1 space
+	static Quat					sGetInvInitialOrientation(const Body &inBody1, const Body &inBody2)
+	{
+		// q20 = q10 r0
+		// <=> r0 = q10^-1 q20
+		// <=> r0^-1 = q20^-1 q10
+		//
+		// where:
+		//
+		// q20 = initial orientation of body 2
+		// q10 = initial orientation of body 1
+		// r0 = initial rotation from body 1 to body 2
+		return inBody2.GetRotation().Conjugated() * inBody1.GetRotation();
+	}
+
+	/// Calculate properties used during the functions below
+	inline void					CalculateConstraintProperties(const Body &inBody1, Mat44Arg inRotation1, const Body &inBody2, Mat44Arg inRotation2, QuatArg inInvInitialOrientation)
+	{
+		// Calculate: JP = 1/2 A ML(q1^*) MR(q2 r0^*) A^T
+		Mat44 jp = (Mat44::sQuatLeftMultiply(0.5f * inBody1.GetRotation().Conjugated()) * Mat44::sQuatRightMultiply(inBody2.GetRotation() * inInvInitialOrientation)).GetRotationSafe();
+
+		// Calculate properties used during constraint solving
+		Mat44 inv_i1 = inBody1.IsDynamic()? inBody1.GetMotionProperties()->GetInverseInertiaForRotation(inRotation1) : Mat44::sZero();
+		Mat44 inv_i2 = inBody2.IsDynamic()? inBody2.GetMotionProperties()->GetInverseInertiaForRotation(inRotation2) : Mat44::sZero();
+		mInvI1_JPT = inv_i1.Multiply3x3RightTransposed(jp);
+		mInvI2_JPT = inv_i2.Multiply3x3RightTransposed(jp);
+
+		// Calculate effective mass: K^-1 = (J M^-1 J^T)^-1
+		// = (JP * I1^-1 * JP^T + JP * I2^-1 * JP^T)^-1
+		// = (JP * (I1^-1 + I2^-1) * JP^T)^-1
+		if (!mEffectiveMass.SetInversed3x3(jp.Multiply3x3(inv_i1 + inv_i2).Multiply3x3RightTransposed(jp)))
+			Deactivate();
+		else
+			mEffectiveMass_JP = mEffectiveMass.Multiply3x3(jp);
+	}
+
+	/// Deactivate this constraint
+	inline void					Deactivate()
+	{
+		mEffectiveMass = Mat44::sZero();
+		mEffectiveMass_JP = Mat44::sZero();
+		mTotalLambda = Vec3::sZero();
+	}
+
+	/// Check if constraint is active
+	inline bool					IsActive() const
+	{
+		return mEffectiveMass(3, 3) != 0.0f;
+	}
+
+	/// Must be called from the WarmStartVelocityConstraint call to apply the previous frame's impulses
+	inline void					WarmStart(Body &ioBody1, Body &ioBody2, float inWarmStartImpulseRatio)
+	{
+		mTotalLambda *= inWarmStartImpulseRatio;
+		ApplyVelocityStep(ioBody1, ioBody2, mTotalLambda);
+	}
+
+	/// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation.
+	inline bool					SolveVelocityConstraint(Body &ioBody1, Body &ioBody2)
+	{
+		// Calculate lagrange multiplier:
+		//
+		// lambda = -K^-1 (J v + b)
+		Vec3 lambda = mEffectiveMass_JP.Multiply3x3(ioBody1.GetAngularVelocity() - ioBody2.GetAngularVelocity());
+		mTotalLambda += lambda;
+		return ApplyVelocityStep(ioBody1, ioBody2, lambda);
+	}
+
+	/// Iteratively update the position constraint. Makes sure C(...) = 0.
+	inline bool					SolvePositionConstraint(Body &ioBody1, Body &ioBody2, QuatArg inInvInitialOrientation, float inBaumgarte) const
+	{
+		// Calculate constraint equation
+		Vec3 c = (ioBody1.GetRotation().Conjugated() * ioBody2.GetRotation() * inInvInitialOrientation).GetXYZ();
+		if (c != Vec3::sZero())
+		{
+			// Calculate lagrange multiplier (lambda) for Baumgarte stabilization:
+			//
+			// lambda = -K^-1 * beta / dt * C
+			//
+			// We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out
+			Vec3 lambda = -inBaumgarte * mEffectiveMass * c;
+
+			// Directly integrate velocity change for one time step
+			//
+			// Euler velocity integration:
+			// dv = M^-1 P
+			//
+			// Impulse:
+			// P = J^T lambda
+			//
+			// Euler position integration:
+			// x' = x + dv * dt
+			//
+			// Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and
+			// Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte
+			// stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity
+			// integrate + a position integrate and then discard the velocity change.
+			if (ioBody1.IsDynamic())
+				ioBody1.SubRotationStep(mInvI1_JPT.Multiply3x3(lambda));
+			if (ioBody2.IsDynamic())
+				ioBody2.AddRotationStep(mInvI2_JPT.Multiply3x3(lambda));
+			return true;
+		}
+
+		return false;
+	}
+
+	/// Return lagrange multiplier
+	Vec3						GetTotalLambda() const
+	{
+		return mTotalLambda;
+	}
+
+	/// Save state of this constraint part
+	void						SaveState(StateRecorder &inStream) const
+	{
+		inStream.Write(mTotalLambda);
+	}
+
+	/// Restore state of this constraint part
+	void						RestoreState(StateRecorder &inStream)
+	{
+		inStream.Read(mTotalLambda);
+	}
+
+private:
+	Mat44						mInvI1_JPT;
+	Mat44						mInvI2_JPT;
+	Mat44						mEffectiveMass;
+	Mat44						mEffectiveMass_JP;
+	Vec3						mTotalLambda { Vec3::sZero() };
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/SpringPart.h

@@ -0,0 +1,169 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+#ifndef JPH_PLATFORM_DOXYGEN // Somehow Doxygen gets confused and thinks the parameters to CalculateSpringProperties belong to this macro
+JPH_MSVC_SUPPRESS_WARNING(4723) // potential divide by 0 - caused by line: outEffectiveMass = 1.0f / inInvEffectiveMass, note that JPH_NAMESPACE_BEGIN already pushes the warning state
+#endif // !JPH_PLATFORM_DOXYGEN
+
+/// Class used in other constraint parts to calculate the required bias factor in the lagrange multiplier for creating springs
+class SpringPart
+{
+private:
+	JPH_INLINE void				CalculateSpringPropertiesHelper(float inDeltaTime, float inInvEffectiveMass, float inBias, float inC, float inStiffness, float inDamping, float &outEffectiveMass)
+	{
+		// Soft constraints as per: Soft Constraints: Reinventing The Spring - Erin Catto - GDC 2011
+
+		// Note that the calculation of beta and gamma below are based on the solution of an implicit Euler integration scheme
+		// This scheme is unconditionally stable but has built in damping, so even when you set the damping ratio to 0 there will still
+		// be damping. See page 16 and 32.
+
+		// Calculate softness (gamma in the slides)
+		// See page 34 and note that the gamma needs to be divided by delta time since we're working with impulses rather than forces:
+		// softness = 1 / (dt * (c + dt * k))
+		// Note that the spring stiffness is k and the spring damping is c
+		mSoftness = 1.0f / (inDeltaTime * (inDamping + inDeltaTime * inStiffness));
+
+		// Calculate bias factor (baumgarte stabilization):
+		// beta = dt * k / (c + dt * k) = dt * k^2 * softness
+		// b = beta / dt * C = dt * k * softness * C
+		mBias = inBias + inDeltaTime * inStiffness * mSoftness * inC;
+
+		// Update the effective mass, see post by Erin Catto: http://www.bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=4&t=1354
+		//
+		// Newton's Law:
+		// M * (v2 - v1) = J^T * lambda
+		//
+		// Velocity constraint with softness and Baumgarte:
+		// J * v2 + softness * lambda + b = 0
+		//
+		// where b = beta * C / dt
+		//
+		// We know everything except v2 and lambda.
+		//
+		// First solve Newton's law for v2 in terms of lambda:
+		//
+		// v2 = v1 + M^-1 * J^T * lambda
+		//
+		// Substitute this expression into the velocity constraint:
+		//
+		// J * (v1 + M^-1 * J^T * lambda) + softness * lambda + b = 0
+		//
+		// Now collect coefficients of lambda:
+		//
+		// (J * M^-1 * J^T + softness) * lambda = - J * v1 - b
+		//
+		// Now we define:
+		//
+		// K = J * M^-1 * J^T + softness
+		//
+		// So our new effective mass is K^-1
+		outEffectiveMass = 1.0f / (inInvEffectiveMass + mSoftness);
+	}
+
+public:
+	/// Turn off the spring and set a bias only
+	///
+	/// @param inBias Bias term (b) for the constraint impulse: lambda = J v + b
+	inline void					CalculateSpringPropertiesWithBias(float inBias)
+	{
+		mSoftness = 0.0f;
+		mBias = inBias;
+	}
+
+	/// Calculate spring properties based on frequency and damping ratio
+	///
+	/// @param inDeltaTime Time step
+	/// @param inInvEffectiveMass Inverse effective mass K
+	/// @param inBias Bias term (b) for the constraint impulse: lambda = J v + b
+	///	@param inC Value of the constraint equation (C). Set to zero if you don't want to drive the constraint to zero with a spring.
+	///	@param inFrequency Oscillation frequency (Hz). Set to zero if you don't want to drive the constraint to zero with a spring.
+	///	@param inDamping Damping factor (0 = no damping, 1 = critical damping). Set to zero if you don't want to drive the constraint to zero with a spring.
+	/// @param outEffectiveMass On return, this contains the new effective mass K^-1
+	inline void					CalculateSpringPropertiesWithFrequencyAndDamping(float inDeltaTime, float inInvEffectiveMass, float inBias, float inC, float inFrequency, float inDamping, float &outEffectiveMass)
+	{
+		outEffectiveMass = 1.0f / inInvEffectiveMass;
+
+		if (inFrequency > 0.0f)
+		{
+			// Calculate angular frequency
+			float omega = 2.0f * JPH_PI * inFrequency;
+
+			// Calculate spring stiffness k and damping constant c (page 45)
+			float k = outEffectiveMass * Square(omega);
+			float c = 2.0f * outEffectiveMass * inDamping * omega;
+
+			CalculateSpringPropertiesHelper(inDeltaTime, inInvEffectiveMass, inBias, inC, k, c, outEffectiveMass);
+		}
+		else
+		{
+			CalculateSpringPropertiesWithBias(inBias);
+		}
+	}
+
+	/// Calculate spring properties with spring Stiffness (k) and damping (c), this is based on the spring equation: F = -k * x - c * v
+	///
+	/// @param inDeltaTime Time step
+	/// @param inInvEffectiveMass Inverse effective mass K
+	/// @param inBias Bias term (b) for the constraint impulse: lambda = J v + b
+	///	@param inC Value of the constraint equation (C). Set to zero if you don't want to drive the constraint to zero with a spring.
+	///	@param inStiffness Spring stiffness k. Set to zero if you don't want to drive the constraint to zero with a spring.
+	///	@param inDamping Spring damping coefficient c. Set to zero if you don't want to drive the constraint to zero with a spring.
+	/// @param outEffectiveMass On return, this contains the new effective mass K^-1
+	inline void					CalculateSpringPropertiesWithStiffnessAndDamping(float inDeltaTime, float inInvEffectiveMass, float inBias, float inC, float inStiffness, float inDamping, float &outEffectiveMass)
+	{
+		if (inStiffness > 0.0f)
+		{
+			CalculateSpringPropertiesHelper(inDeltaTime, inInvEffectiveMass, inBias, inC, inStiffness, inDamping, outEffectiveMass);
+		}
+		else
+		{
+			outEffectiveMass = 1.0f / inInvEffectiveMass;
+
+			CalculateSpringPropertiesWithBias(inBias);
+		}
+	}
+
+	/// Returns if this spring is active
+	inline bool					IsActive() const
+	{
+		return mSoftness != 0.0f;
+	}
+
+	/// Get total bias b, including supplied bias and bias for spring: lambda = J v + b
+	inline float				GetBias(float inTotalLambda) const
+	{
+		// Remainder of post by Erin Catto: http://www.bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=4&t=1354
+		//
+		// Each iteration we are not computing the whole impulse, we are computing an increment to the impulse and we are updating the velocity.
+		// Also, as we solve each constraint we get a perfect v2, but then some other constraint will come along and mess it up.
+		// So we want to patch up the constraint while acknowledging the accumulated impulse and the damaged velocity.
+		// To help with that we use P for the accumulated impulse and lambda as the update. Mathematically we have:
+		//
+		// M * (v2new - v2damaged) = J^T * lambda
+		// J * v2new + softness * (total_lambda + lambda) + b = 0
+		//
+		// If we solve this we get:
+		//
+		// v2new = v2damaged + M^-1 * J^T * lambda
+		// J * (v2damaged + M^-1 * J^T * lambda) + softness * total_lambda + softness * lambda + b = 0
+		//
+		// (J * M^-1 * J^T + softness) * lambda = -(J * v2damaged + softness * total_lambda + b)
+		//
+		// So our lagrange multiplier becomes:
+		//
+		// lambda = -K^-1 (J v + softness * total_lambda + b)
+		//
+		// So we return the bias: softness * total_lambda + b
+		return mSoftness * inTotalLambda + mBias;
+	}
+
+private:
+	float						mBias  = 0.0f;
+	float						mSoftness  = 0.0f;
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/SwingTwistConstraintPart.h

@@ -0,0 +1,597 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Geometry/Ellipse.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/RotationEulerConstraintPart.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/AngleConstraintPart.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// How the swing limit behaves
+enum class ESwingType : uint8
+{
+	Cone,						///< Swing is limited by a cone shape, note that this cone starts to deform for larger swing angles. Cone limits only support limits that are symmetric around 0.
+	Pyramid,					///< Swing is limited by a pyramid shape, note that this pyramid starts to deform for larger swing angles.
+};
+
+/// Quaternion based constraint that decomposes the rotation in constraint space in swing and twist: q = q_swing * q_twist
+/// where q_swing.x = 0 and where q_twist.y = q_twist.z = 0
+///
+/// - Rotation around the twist (x-axis) is within [inTwistMinAngle, inTwistMaxAngle].
+/// - Rotation around the swing axis (y and z axis) are limited to an ellipsoid in quaternion space formed by the equation:
+///
+/// (q_swing.y / sin(inSwingYHalfAngle / 2))^2 + (q_swing.z / sin(inSwingZHalfAngle / 2))^2 <= 1
+///
+/// Which roughly corresponds to an elliptic cone shape with major axis (inSwingYHalfAngle, inSwingZHalfAngle).
+///
+/// In case inSwingYHalfAngle = 0, the rotation around Y will be constrained to 0 and the rotation around Z
+/// will be constrained between [-inSwingZHalfAngle, inSwingZHalfAngle]. Vice versa if inSwingZHalfAngle = 0.
+class SwingTwistConstraintPart
+{
+public:
+	/// Override the swing type
+	void						SetSwingType(ESwingType inSwingType)
+	{
+		mSwingType = inSwingType;
+	}
+
+	/// Get the swing type for this part
+	ESwingType					GetSwingType() const
+	{
+		return mSwingType;
+	}
+
+	/// Set limits for this constraint (see description above for parameters)
+	void						SetLimits(float inTwistMinAngle, float inTwistMaxAngle, float inSwingYMinAngle, float inSwingYMaxAngle, float inSwingZMinAngle, float inSwingZMaxAngle)
+	{
+		constexpr float cLockedAngle = DegreesToRadians(0.5f);
+		constexpr float cFreeAngle = DegreesToRadians(179.5f);
+
+		// Assume sane input
+		JPH_ASSERT(inTwistMinAngle <= inTwistMaxAngle);
+		JPH_ASSERT(inSwingYMinAngle <= inSwingYMaxAngle);
+		JPH_ASSERT(inSwingZMinAngle <= inSwingZMaxAngle);
+		JPH_ASSERT(inSwingYMinAngle >= -JPH_PI && inSwingYMaxAngle <= JPH_PI);
+		JPH_ASSERT(inSwingZMinAngle >= -JPH_PI && inSwingZMaxAngle <= JPH_PI);
+
+		// Calculate the sine and cosine of the half angles
+		Vec4 half_twist = 0.5f * Vec4(inTwistMinAngle, inTwistMaxAngle, 0, 0);
+		Vec4 twist_s, twist_c;
+		half_twist.SinCos(twist_s, twist_c);
+		Vec4 half_swing = 0.5f * Vec4(inSwingYMinAngle, inSwingYMaxAngle, inSwingZMinAngle, inSwingZMaxAngle);
+		Vec4 swing_s, swing_c;
+		half_swing.SinCos(swing_s, swing_c);
+
+		// Store half angles for pyramid limit
+		mSwingYHalfMinAngle = half_swing.GetX();
+		mSwingYHalfMaxAngle = half_swing.GetY();
+		mSwingZHalfMinAngle = half_swing.GetZ();
+		mSwingZHalfMaxAngle = half_swing.GetW();
+
+		// Store axis flags which are used at runtime to quickly decided which constraints to apply
+		mRotationFlags = 0;
+		if (inTwistMinAngle > -cLockedAngle && inTwistMaxAngle < cLockedAngle)
+		{
+			mRotationFlags |= TwistXLocked;
+			mSinTwistHalfMinAngle = 0.0f;
+			mSinTwistHalfMaxAngle = 0.0f;
+			mCosTwistHalfMinAngle = 1.0f;
+			mCosTwistHalfMaxAngle = 1.0f;
+		}
+		else if (inTwistMinAngle < -cFreeAngle && inTwistMaxAngle > cFreeAngle)
+		{
+			mRotationFlags |= TwistXFree;
+			mSinTwistHalfMinAngle = -1.0f;
+			mSinTwistHalfMaxAngle = 1.0f;
+			mCosTwistHalfMinAngle = 0.0f;
+			mCosTwistHalfMaxAngle = 0.0f;
+		}
+		else
+		{
+			mSinTwistHalfMinAngle = twist_s.GetX();
+			mSinTwistHalfMaxAngle = twist_s.GetY();
+			mCosTwistHalfMinAngle = twist_c.GetX();
+			mCosTwistHalfMaxAngle = twist_c.GetY();
+		}
+
+		if (inSwingYMinAngle > -cLockedAngle && inSwingYMaxAngle < cLockedAngle)
+		{
+			mRotationFlags |= SwingYLocked;
+			mSinSwingYHalfMinAngle = 0.0f;
+			mSinSwingYHalfMaxAngle = 0.0f;
+			mCosSwingYHalfMinAngle = 1.0f;
+			mCosSwingYHalfMaxAngle = 1.0f;
+		}
+		else if (inSwingYMinAngle < -cFreeAngle && inSwingYMaxAngle > cFreeAngle)
+		{
+			mRotationFlags |= SwingYFree;
+			mSinSwingYHalfMinAngle = -1.0f;
+			mSinSwingYHalfMaxAngle = 1.0f;
+			mCosSwingYHalfMinAngle = 0.0f;
+			mCosSwingYHalfMaxAngle = 0.0f;
+		}
+		else
+		{
+			mSinSwingYHalfMinAngle = swing_s.GetX();
+			mSinSwingYHalfMaxAngle = swing_s.GetY();
+			mCosSwingYHalfMinAngle = swing_c.GetX();
+			mCosSwingYHalfMaxAngle = swing_c.GetY();
+			JPH_ASSERT(mSinSwingYHalfMinAngle <= mSinSwingYHalfMaxAngle);
+		}
+
+		if (inSwingZMinAngle > -cLockedAngle && inSwingZMaxAngle < cLockedAngle)
+		{
+			mRotationFlags |= SwingZLocked;
+			mSinSwingZHalfMinAngle = 0.0f;
+			mSinSwingZHalfMaxAngle = 0.0f;
+			mCosSwingZHalfMinAngle = 1.0f;
+			mCosSwingZHalfMaxAngle = 1.0f;
+		}
+		else if (inSwingZMinAngle < -cFreeAngle && inSwingZMaxAngle > cFreeAngle)
+		{
+			mRotationFlags |= SwingZFree;
+			mSinSwingZHalfMinAngle = -1.0f;
+			mSinSwingZHalfMaxAngle = 1.0f;
+			mCosSwingZHalfMinAngle = 0.0f;
+			mCosSwingZHalfMaxAngle = 0.0f;
+		}
+		else
+		{
+			mSinSwingZHalfMinAngle = swing_s.GetZ();
+			mSinSwingZHalfMaxAngle = swing_s.GetW();
+			mCosSwingZHalfMinAngle = swing_c.GetZ();
+			mCosSwingZHalfMaxAngle = swing_c.GetW();
+			JPH_ASSERT(mSinSwingZHalfMinAngle <= mSinSwingZHalfMaxAngle);
+		}
+	}
+
+	/// Flags to indicate which axis got clamped by ClampSwingTwist
+	static constexpr uint		cClampedTwistMin = 1 << 0;
+	static constexpr uint		cClampedTwistMax = 1 << 1;
+	static constexpr uint		cClampedSwingYMin = 1 << 2;
+	static constexpr uint		cClampedSwingYMax = 1 << 3;
+	static constexpr uint		cClampedSwingZMin = 1 << 4;
+	static constexpr uint		cClampedSwingZMax = 1 << 5;
+
+	/// Helper function to determine if we're clamped against the min or max limit
+	static JPH_INLINE bool		sDistanceToMinShorter(float inDeltaMin, float inDeltaMax)
+	{
+		// We're outside of the limits, get actual delta to min/max range
+		// Note that a swing/twist of -1 and 1 represent the same angle, so if the difference is bigger than 1, the shortest angle is the other way around (2 - difference)
+		// We should actually be working with angles rather than sin(angle / 2). When the difference is small the approximation is accurate, but
+		// when working with extreme values the calculation is off and e.g. when the limit is between 0 and 180 a value of approx -60 will clamp
+		// to 180 rather than 0 (you'd expect anything > -90 to go to 0).
+		inDeltaMin = abs(inDeltaMin);
+		if (inDeltaMin > 1.0f) inDeltaMin = 2.0f - inDeltaMin;
+		inDeltaMax = abs(inDeltaMax);
+		if (inDeltaMax > 1.0f) inDeltaMax = 2.0f - inDeltaMax;
+		return inDeltaMin < inDeltaMax;
+	}
+
+	/// Clamp twist and swing against the constraint limits, returns which parts were clamped (everything assumed in constraint space)
+	inline void					ClampSwingTwist(Quat &ioSwing, Quat &ioTwist, uint &outClampedAxis) const
+	{
+		// Start with not clamped
+		outClampedAxis = 0;
+
+		// Check that swing and twist quaternions don't contain rotations around the wrong axis
+		JPH_ASSERT(ioSwing.GetX() == 0.0f);
+		JPH_ASSERT(ioTwist.GetY() == 0.0f);
+		JPH_ASSERT(ioTwist.GetZ() == 0.0f);
+
+		// Ensure quaternions have w > 0
+		bool negate_swing = ioSwing.GetW() < 0.0f;
+		if (negate_swing)
+			ioSwing = -ioSwing;
+		bool negate_twist = ioTwist.GetW() < 0.0f;
+		if (negate_twist)
+			ioTwist = -ioTwist;
+
+		if (mRotationFlags & TwistXLocked)
+		{
+			// Twist axis is locked, clamp whenever twist is not identity
+			outClampedAxis |= ioTwist.GetX() != 0.0f? (cClampedTwistMin | cClampedTwistMax) : 0;
+			ioTwist = Quat::sIdentity();
+		}
+		else if ((mRotationFlags & TwistXFree) == 0)
+		{
+			// Twist axis has limit, clamp whenever out of range
+			float delta_min = mSinTwistHalfMinAngle - ioTwist.GetX();
+			float delta_max = ioTwist.GetX() - mSinTwistHalfMaxAngle;
+			if (delta_min > 0.0f || delta_max > 0.0f)
+			{
+				// Pick the twist that corresponds to the smallest delta
+				if (sDistanceToMinShorter(delta_min, delta_max))
+				{
+					ioTwist = Quat(mSinTwistHalfMinAngle, 0, 0, mCosTwistHalfMinAngle);
+					outClampedAxis |= cClampedTwistMin;
+				}
+				else
+				{
+					ioTwist = Quat(mSinTwistHalfMaxAngle, 0, 0, mCosTwistHalfMaxAngle);
+					outClampedAxis |= cClampedTwistMax;
+				}
+			}
+		}
+
+		// Clamp swing
+		if (mRotationFlags & SwingYLocked)
+		{
+			if (mRotationFlags & SwingZLocked)
+			{
+				// Both swing Y and Z are disabled, no degrees of freedom in swing
+				outClampedAxis |= ioSwing.GetY() != 0.0f? (cClampedSwingYMin | cClampedSwingYMax) : 0;
+				outClampedAxis |= ioSwing.GetZ() != 0.0f? (cClampedSwingZMin | cClampedSwingZMax) : 0;
+				ioSwing = Quat::sIdentity();
+			}
+			else
+			{
+				// Swing Y angle disabled, only 1 degree of freedom in swing
+				outClampedAxis |= ioSwing.GetY() != 0.0f? (cClampedSwingYMin | cClampedSwingYMax) : 0;
+				float delta_min = mSinSwingZHalfMinAngle - ioSwing.GetZ();
+				float delta_max = ioSwing.GetZ() - mSinSwingZHalfMaxAngle;
+				if (delta_min > 0.0f || delta_max > 0.0f)
+				{
+					// Pick the swing that corresponds to the smallest delta
+					if (sDistanceToMinShorter(delta_min, delta_max))
+					{
+						ioSwing = Quat(0, 0, mSinSwingZHalfMinAngle, mCosSwingZHalfMinAngle);
+						outClampedAxis |= cClampedSwingZMin;
+					}
+					else
+					{
+						ioSwing = Quat(0, 0, mSinSwingZHalfMaxAngle, mCosSwingZHalfMaxAngle);
+						outClampedAxis |= cClampedSwingZMax;
+					}
+				}
+				else if ((outClampedAxis & cClampedSwingYMin) != 0)
+				{
+					float z = ioSwing.GetZ();
+					ioSwing = Quat(0, 0, z, sqrt(1.0f - Square(z)));
+				}
+			}
+		}
+		else if (mRotationFlags & SwingZLocked)
+		{
+			// Swing Z angle disabled, only 1 degree of freedom in swing
+			outClampedAxis |= ioSwing.GetZ() != 0.0f? (cClampedSwingZMin | cClampedSwingZMax) : 0;
+			float delta_min = mSinSwingYHalfMinAngle - ioSwing.GetY();
+			float delta_max = ioSwing.GetY() - mSinSwingYHalfMaxAngle;
+			if (delta_min > 0.0f || delta_max > 0.0f)
+			{
+				// Pick the swing that corresponds to the smallest delta
+				if (sDistanceToMinShorter(delta_min, delta_max))
+				{
+					ioSwing = Quat(0, mSinSwingYHalfMinAngle, 0, mCosSwingYHalfMinAngle);
+					outClampedAxis |= cClampedSwingYMin;
+				}
+				else
+				{
+					ioSwing = Quat(0, mSinSwingYHalfMaxAngle, 0, mCosSwingYHalfMaxAngle);
+					outClampedAxis |= cClampedSwingYMax;
+				}
+			}
+			else if ((outClampedAxis & cClampedSwingZMin) != 0)
+			{
+				float y = ioSwing.GetY();
+				ioSwing = Quat(0, y, 0, sqrt(1.0f - Square(y)));
+			}
+		}
+		else
+		{
+			// Two degrees of freedom
+			if (mSwingType == ESwingType::Cone)
+			{
+				// Use ellipse to solve limits
+				Ellipse ellipse(mSinSwingYHalfMaxAngle, mSinSwingZHalfMaxAngle);
+				Float2 point(ioSwing.GetY(), ioSwing.GetZ());
+				if (!ellipse.IsInside(point))
+				{
+					Float2 closest = ellipse.GetClosestPoint(point);
+					ioSwing = Quat(0, closest.x, closest.y, sqrt(max(0.0f, 1.0f - Square(closest.x) - Square(closest.y))));
+					outClampedAxis |= cClampedSwingYMin | cClampedSwingYMax | cClampedSwingZMin | cClampedSwingZMax; // We're not using the flags on which side we got clamped here
+				}
+			}
+			else
+			{
+				// Use pyramid to solve limits
+				// The quaternion rotating by angle y around the Y axis then rotating by angle z around the Z axis is:
+				// q = Quat::sRotation(Vec3::sAxisZ(), z) * Quat::sRotation(Vec3::sAxisY(), y)
+				// [q.x, q.y, q.z, q.w] = [-sin(y / 2) * sin(z / 2), sin(y / 2) * cos(z / 2), cos(y / 2) * sin(z / 2), cos(y / 2) * cos(z / 2)]
+				// So we can calculate y / 2 = atan2(q.y, q.w) and z / 2 = atan2(q.z, q.w)
+				Vec4 half_angle = Vec4::sATan2(ioSwing.GetXYZW().Swizzle<SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_Z>(), ioSwing.GetXYZW().SplatW());
+				Vec4 min_half_angle(mSwingYHalfMinAngle, mSwingYHalfMinAngle, mSwingZHalfMinAngle, mSwingZHalfMinAngle);
+				Vec4 max_half_angle(mSwingYHalfMaxAngle, mSwingYHalfMaxAngle, mSwingZHalfMaxAngle, mSwingZHalfMaxAngle);
+				Vec4 clamped_half_angle = Vec4::sMin(Vec4::sMax(half_angle, min_half_angle), max_half_angle);
+				UVec4 unclamped = Vec4::sEquals(half_angle, clamped_half_angle);
+				if (!unclamped.TestAllTrue())
+				{
+					// We now calculate the quaternion again using the formula for q above,
+					// but we leave out the x component in order to not introduce twist
+					Vec4 s, c;
+					clamped_half_angle.SinCos(s, c);
+					ioSwing = Quat(0, s.GetY() * c.GetZ(), c.GetY() * s.GetZ(), c.GetY() * c.GetZ()).Normalized();
+					outClampedAxis |= cClampedSwingYMin | cClampedSwingYMax | cClampedSwingZMin | cClampedSwingZMax; // We're not using the flags on which side we got clamped here
+				}
+			}
+		}
+
+		// Flip sign back
+		if (negate_swing)
+			ioSwing = -ioSwing;
+		if (negate_twist)
+			ioTwist = -ioTwist;
+
+		JPH_ASSERT(ioSwing.IsNormalized());
+		JPH_ASSERT(ioTwist.IsNormalized());
+	}
+
+	/// Calculate properties used during the functions below
+	/// @param inBody1 The first body that this constraint is attached to
+	/// @param inBody2 The second body that this constraint is attached to
+	/// @param inConstraintRotation The current rotation of the constraint in constraint space
+	/// @param inConstraintToWorld Rotates from constraint space into world space
+	inline void					CalculateConstraintProperties(const Body &inBody1, const Body &inBody2, QuatArg inConstraintRotation, QuatArg inConstraintToWorld)
+	{
+		// Decompose into swing and twist
+		Quat q_swing, q_twist;
+		inConstraintRotation.GetSwingTwist(q_swing, q_twist);
+
+		// Clamp against joint limits
+		Quat q_clamped_swing = q_swing, q_clamped_twist = q_twist;
+		uint clamped_axis;
+		ClampSwingTwist(q_clamped_swing, q_clamped_twist, clamped_axis);
+
+		if (mRotationFlags & SwingYLocked)
+		{
+			Quat twist_to_world = inConstraintToWorld * q_swing;
+			mWorldSpaceSwingLimitYRotationAxis = twist_to_world.RotateAxisY();
+			mWorldSpaceSwingLimitZRotationAxis = twist_to_world.RotateAxisZ();
+
+			if (mRotationFlags & SwingZLocked)
+			{
+				// Swing fully locked
+				mSwingLimitYConstraintPart.CalculateConstraintProperties(inBody1, inBody2, mWorldSpaceSwingLimitYRotationAxis);
+				mSwingLimitZConstraintPart.CalculateConstraintProperties(inBody1, inBody2, mWorldSpaceSwingLimitZRotationAxis);
+			}
+			else
+			{
+				// Swing only locked around Y
+				mSwingLimitYConstraintPart.CalculateConstraintProperties(inBody1, inBody2, mWorldSpaceSwingLimitYRotationAxis);
+				if ((clamped_axis & (cClampedSwingZMin | cClampedSwingZMax)) != 0)
+				{
+					if ((clamped_axis & cClampedSwingZMin) != 0)
+						mWorldSpaceSwingLimitZRotationAxis = -mWorldSpaceSwingLimitZRotationAxis; // Flip axis if hitting min limit because the impulse limit is going to be between [-FLT_MAX, 0]
+					mSwingLimitZConstraintPart.CalculateConstraintProperties(inBody1, inBody2, mWorldSpaceSwingLimitZRotationAxis);
+				}
+				else
+					mSwingLimitZConstraintPart.Deactivate();
+			}
+		}
+		else if (mRotationFlags & SwingZLocked)
+		{
+			// Swing only locked around Z
+			Quat twist_to_world = inConstraintToWorld * q_swing;
+			mWorldSpaceSwingLimitYRotationAxis = twist_to_world.RotateAxisY();
+			mWorldSpaceSwingLimitZRotationAxis = twist_to_world.RotateAxisZ();
+
+			if ((clamped_axis & (cClampedSwingYMin | cClampedSwingYMax)) != 0)
+			{
+				if ((clamped_axis & cClampedSwingYMin) != 0)
+					mWorldSpaceSwingLimitYRotationAxis = -mWorldSpaceSwingLimitYRotationAxis; // Flip axis if hitting min limit because the impulse limit is going to be between [-FLT_MAX, 0]
+				mSwingLimitYConstraintPart.CalculateConstraintProperties(inBody1, inBody2, mWorldSpaceSwingLimitYRotationAxis);
+			}
+			else
+				mSwingLimitYConstraintPart.Deactivate();
+			mSwingLimitZConstraintPart.CalculateConstraintProperties(inBody1, inBody2, mWorldSpaceSwingLimitZRotationAxis);
+		}
+		else if ((mRotationFlags & SwingYZFree) != SwingYZFree)
+		{
+			// Swing has limits around Y and Z
+			if ((clamped_axis & (cClampedSwingYMin | cClampedSwingYMax | cClampedSwingZMin | cClampedSwingZMax)) != 0)
+			{
+				// Calculate axis of rotation from clamped swing to swing
+				Vec3 current = (inConstraintToWorld * q_swing).RotateAxisX();
+				Vec3 desired = (inConstraintToWorld * q_clamped_swing).RotateAxisX();
+				mWorldSpaceSwingLimitYRotationAxis = desired.Cross(current);
+				float len = mWorldSpaceSwingLimitYRotationAxis.Length();
+				if (len != 0.0f)
+				{
+					mWorldSpaceSwingLimitYRotationAxis /= len;
+					mSwingLimitYConstraintPart.CalculateConstraintProperties(inBody1, inBody2, mWorldSpaceSwingLimitYRotationAxis);
+				}
+				else
+					mSwingLimitYConstraintPart.Deactivate();
+			}
+			else
+				mSwingLimitYConstraintPart.Deactivate();
+			mSwingLimitZConstraintPart.Deactivate();
+		}
+		else
+		{
+			// No swing limits
+			mSwingLimitYConstraintPart.Deactivate();
+			mSwingLimitZConstraintPart.Deactivate();
+		}
+
+		if (mRotationFlags & TwistXLocked)
+		{
+			// Twist locked, always activate constraint
+			mWorldSpaceTwistLimitRotationAxis = (inConstraintToWorld * q_swing).RotateAxisX();
+			mTwistLimitConstraintPart.CalculateConstraintProperties(inBody1, inBody2, mWorldSpaceTwistLimitRotationAxis);
+		}
+		else if ((mRotationFlags & TwistXFree) == 0)
+		{
+			// Twist has limits
+			if ((clamped_axis & (cClampedTwistMin | cClampedTwistMax)) != 0)
+			{
+				mWorldSpaceTwistLimitRotationAxis = (inConstraintToWorld * q_swing).RotateAxisX();
+				if ((clamped_axis & cClampedTwistMin) != 0)
+					mWorldSpaceTwistLimitRotationAxis = -mWorldSpaceTwistLimitRotationAxis; // Flip axis if hitting min limit because the impulse limit is going to be between [-FLT_MAX, 0]
+				mTwistLimitConstraintPart.CalculateConstraintProperties(inBody1, inBody2, mWorldSpaceTwistLimitRotationAxis);
+			}
+			else
+				mTwistLimitConstraintPart.Deactivate();
+		}
+		else
+		{
+			// No twist limits
+			mTwistLimitConstraintPart.Deactivate();
+		}
+	}
+
+	/// Deactivate this constraint
+	void						Deactivate()
+	{
+		mSwingLimitYConstraintPart.Deactivate();
+		mSwingLimitZConstraintPart.Deactivate();
+		mTwistLimitConstraintPart.Deactivate();
+	}
+
+	/// Check if constraint is active
+	inline bool					IsActive() const
+	{
+		return mSwingLimitYConstraintPart.IsActive() || mSwingLimitZConstraintPart.IsActive() || mTwistLimitConstraintPart.IsActive();
+	}
+
+	/// Must be called from the WarmStartVelocityConstraint call to apply the previous frame's impulses
+	inline void					WarmStart(Body &ioBody1, Body &ioBody2, float inWarmStartImpulseRatio)
+	{
+		mSwingLimitYConstraintPart.WarmStart(ioBody1, ioBody2, inWarmStartImpulseRatio);
+		mSwingLimitZConstraintPart.WarmStart(ioBody1, ioBody2, inWarmStartImpulseRatio);
+		mTwistLimitConstraintPart.WarmStart(ioBody1, ioBody2, inWarmStartImpulseRatio);
+	}
+
+	/// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation.
+	inline bool					SolveVelocityConstraint(Body &ioBody1, Body &ioBody2)
+	{
+		bool impulse = false;
+
+		// Solve swing constraint
+		if (mSwingLimitYConstraintPart.IsActive())
+			impulse |= mSwingLimitYConstraintPart.SolveVelocityConstraint(ioBody1, ioBody2, mWorldSpaceSwingLimitYRotationAxis, -FLT_MAX, mSinSwingYHalfMinAngle == mSinSwingYHalfMaxAngle? FLT_MAX : 0.0f);
+
+		if (mSwingLimitZConstraintPart.IsActive())
+			impulse |= mSwingLimitZConstraintPart.SolveVelocityConstraint(ioBody1, ioBody2, mWorldSpaceSwingLimitZRotationAxis, -FLT_MAX, mSinSwingZHalfMinAngle == mSinSwingZHalfMaxAngle? FLT_MAX : 0.0f);
+
+		// Solve twist constraint
+		if (mTwistLimitConstraintPart.IsActive())
+			impulse |= mTwistLimitConstraintPart.SolveVelocityConstraint(ioBody1, ioBody2, mWorldSpaceTwistLimitRotationAxis, -FLT_MAX, mSinTwistHalfMinAngle == mSinTwistHalfMaxAngle? FLT_MAX : 0.0f);
+
+		return impulse;
+	}
+
+	/// Iteratively update the position constraint. Makes sure C(...) = 0.
+	/// @param ioBody1 The first body that this constraint is attached to
+	/// @param ioBody2 The second body that this constraint is attached to
+	/// @param inConstraintRotation The current rotation of the constraint in constraint space
+	/// @param inConstraintToBody1 , inConstraintToBody2 Rotates from constraint space to body 1/2 space
+	/// @param inBaumgarte Baumgarte constant (fraction of the error to correct)
+	inline bool					SolvePositionConstraint(Body &ioBody1, Body &ioBody2, QuatArg inConstraintRotation, QuatArg inConstraintToBody1, QuatArg inConstraintToBody2, float inBaumgarte) const
+	{
+		Quat q_swing, q_twist;
+		inConstraintRotation.GetSwingTwist(q_swing, q_twist);
+
+		uint clamped_axis;
+		ClampSwingTwist(q_swing, q_twist, clamped_axis);
+
+		// Solve rotation violations
+		if (clamped_axis != 0)
+		{
+			RotationEulerConstraintPart part;
+			Quat inv_initial_orientation = inConstraintToBody2 * (inConstraintToBody1 * q_swing * q_twist).Conjugated();
+			part.CalculateConstraintProperties(ioBody1, Mat44::sRotation(ioBody1.GetRotation()), ioBody2, Mat44::sRotation(ioBody2.GetRotation()));
+			return part.SolvePositionConstraint(ioBody1, ioBody2, inv_initial_orientation, inBaumgarte);
+		}
+
+		return false;
+	}
+
+	/// Return lagrange multiplier for swing
+	inline float				GetTotalSwingYLambda() const
+	{
+		return mSwingLimitYConstraintPart.GetTotalLambda();
+	}
+
+	inline float				GetTotalSwingZLambda() const
+	{
+		return mSwingLimitZConstraintPart.GetTotalLambda();
+	}
+
+	/// Return lagrange multiplier for twist
+	inline float				GetTotalTwistLambda() const
+	{
+		return mTwistLimitConstraintPart.GetTotalLambda();
+	}
+
+	/// Save state of this constraint part
+	void						SaveState(StateRecorder &inStream) const
+	{
+		mSwingLimitYConstraintPart.SaveState(inStream);
+		mSwingLimitZConstraintPart.SaveState(inStream);
+		mTwistLimitConstraintPart.SaveState(inStream);
+	}
+
+	/// Restore state of this constraint part
+	void						RestoreState(StateRecorder &inStream)
+	{
+		mSwingLimitYConstraintPart.RestoreState(inStream);
+		mSwingLimitZConstraintPart.RestoreState(inStream);
+		mTwistLimitConstraintPart.RestoreState(inStream);
+	}
+
+private:
+	// CONFIGURATION PROPERTIES FOLLOW
+
+	enum ERotationFlags
+	{
+		/// Indicates that axis is completely locked (cannot rotate around this axis)
+		TwistXLocked			= 1 << 0,
+		SwingYLocked			= 1 << 1,
+		SwingZLocked			= 1 << 2,
+
+		/// Indicates that axis is completely free (can rotate around without limits)
+		TwistXFree				= 1 << 3,
+		SwingYFree				= 1 << 4,
+		SwingZFree				= 1 << 5,
+		SwingYZFree				= SwingYFree | SwingZFree
+	};
+
+	uint8						mRotationFlags;
+
+	// Constants
+	ESwingType					mSwingType = ESwingType::Cone;
+	float						mSinTwistHalfMinAngle;
+	float						mSinTwistHalfMaxAngle;
+	float						mCosTwistHalfMinAngle;
+	float						mCosTwistHalfMaxAngle;
+	float						mSwingYHalfMinAngle;
+	float						mSwingYHalfMaxAngle;
+	float						mSwingZHalfMinAngle;
+	float						mSwingZHalfMaxAngle;
+	float						mSinSwingYHalfMinAngle;
+	float						mSinSwingYHalfMaxAngle;
+	float						mSinSwingZHalfMinAngle;
+	float						mSinSwingZHalfMaxAngle;
+	float						mCosSwingYHalfMinAngle;
+	float						mCosSwingYHalfMaxAngle;
+	float						mCosSwingZHalfMinAngle;
+	float						mCosSwingZHalfMaxAngle;
+
+	// RUN TIME PROPERTIES FOLLOW
+
+	/// Rotation axis for the angle constraint parts
+	Vec3						mWorldSpaceSwingLimitYRotationAxis;
+	Vec3						mWorldSpaceSwingLimitZRotationAxis;
+	Vec3						mWorldSpaceTwistLimitRotationAxis;
+
+	/// The constraint parts
+	AngleConstraintPart			mSwingLimitYConstraintPart;
+	AngleConstraintPart			mSwingLimitZConstraintPart;
+	AngleConstraintPart			mTwistLimitConstraintPart;
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.h

@@ -0,0 +1,521 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/StaticArray.h>
+#include <Jolt/Core/LockFreeHashMap.h>
+#include <Jolt/Physics/EPhysicsUpdateError.h>
+#include <Jolt/Physics/Body/BodyPair.h>
+#include <Jolt/Physics/Collision/Shape/SubShapeIDPair.h>
+#include <Jolt/Physics/Collision/ManifoldBetweenTwoFaces.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/AxisConstraintPart.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/DualAxisConstraintPart.h>
+#include <Jolt/Core/HashCombine.h>
+#include <Jolt/Core/NonCopyable.h>
+
+JPH_SUPPRESS_WARNINGS_STD_BEGIN
+#include <atomic>
+JPH_SUPPRESS_WARNINGS_STD_END
+
+JPH_NAMESPACE_BEGIN
+
+struct PhysicsSettings;
+class PhysicsUpdateContext;
+
+class JPH_EXPORT ContactConstraintManager : public NonCopyable
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+	explicit					ContactConstraintManager(const PhysicsSettings &inPhysicsSettings);
+								~ContactConstraintManager();
+
+	/// Initialize the system.
+	/// @param inMaxBodyPairs Maximum amount of body pairs to process (anything else will fall through the world), this number should generally be much higher than the max amount of contact points as there will be lots of bodies close that are not actually touching
+	/// @param inMaxContactConstraints Maximum amount of contact constraints to process (anything else will fall through the world)
+	void						Init(uint inMaxBodyPairs, uint inMaxContactConstraints);
+
+	/// Listener that is notified whenever a contact point between two bodies is added/updated/removed
+	void						SetContactListener(ContactListener *inListener)						{ mContactListener = inListener; }
+	ContactListener *			GetContactListener() const											{ return mContactListener; }
+
+	/// Callback function to combine the restitution or friction of two bodies
+	/// Note that when merging manifolds (when PhysicsSettings::mUseManifoldReduction is true) you will only get a callback for the merged manifold.
+	/// It is not possible in that case to get all sub shape ID pairs that were colliding, you'll get the first encountered pair.
+	using CombineFunction = float (*)(const Body &inBody1, const SubShapeID &inSubShapeID1, const Body &inBody2, const SubShapeID &inSubShapeID2);
+
+	/// Set the function that combines the friction of two bodies and returns it
+	/// Default method is the geometric mean: sqrt(friction1 * friction2).
+	void						SetCombineFriction(CombineFunction inCombineFriction)				{ mCombineFriction = inCombineFriction; }
+	CombineFunction				GetCombineFriction() const											{ return mCombineFriction; }
+
+	/// Set the function that combines the restitution of two bodies and returns it
+	/// Default method is max(restitution1, restitution1)
+	void						SetCombineRestitution(CombineFunction inCombineRestitution)			{ mCombineRestitution = inCombineRestitution; }
+	CombineFunction				GetCombineRestitution() const										{ return mCombineRestitution; }
+
+	/// Get the max number of contact constraints that are allowed
+	uint32						GetMaxConstraints() const											{ return mMaxConstraints; }
+
+	/// Check with the listener if inBody1 and inBody2 could collide, returns false if not
+	inline ValidateResult		ValidateContactPoint(const Body &inBody1, const Body &inBody2, RVec3Arg inBaseOffset, const CollideShapeResult &inCollisionResult) const
+	{
+		if (mContactListener == nullptr)
+			return ValidateResult::AcceptAllContactsForThisBodyPair;
+
+		return mContactListener->OnContactValidate(inBody1, inBody2, inBaseOffset, inCollisionResult);
+	}
+
+	/// Sets up the constraint buffer. Should be called before starting collision detection.
+	void						PrepareConstraintBuffer(PhysicsUpdateContext *inContext);
+
+	/// Max 4 contact points are needed for a stable manifold
+	static const int			MaxContactPoints = 4;
+
+	/// Contacts are allocated in a lock free hash map
+	class ContactAllocator : public LFHMAllocatorContext
+	{
+	public:
+		using LFHMAllocatorContext::LFHMAllocatorContext;
+
+		uint					mNumBodyPairs = 0;													///< Total number of body pairs added using this allocator
+		uint					mNumManifolds = 0;													///< Total number of manifolds added using this allocator
+		EPhysicsUpdateError		mErrors = EPhysicsUpdateError::None;								///< Errors reported on this allocator
+	};
+
+	/// Get a new allocator context for storing contacts. Note that you should call this once and then add multiple contacts using the context.
+	ContactAllocator			GetContactAllocator()												{ return mCache[mCacheWriteIdx].GetContactAllocator(); }
+
+	/// Check if the contact points from the previous frame are reusable and if so copy them.
+	/// When the cache was usable and the pair has been handled: outPairHandled = true.
+	/// When a contact constraint was produced: outConstraintCreated = true.
+	void						GetContactsFromCache(ContactAllocator &ioContactAllocator, Body &inBody1, Body &inBody2, bool &outPairHandled, bool &outConstraintCreated);
+
+	/// Handle used to keep track of the current body pair
+	using BodyPairHandle = void *;
+
+	/// Create a handle for a colliding body pair so that contact constraints can be added between them.
+	/// Needs to be called once per body pair per frame before calling AddContactConstraint.
+	BodyPairHandle				AddBodyPair(ContactAllocator &ioContactAllocator, const Body &inBody1, const Body &inBody2);
+
+	/// Add a contact constraint for this frame.
+	///
+	/// @param ioContactAllocator The allocator that reserves memory for the contacts
+	/// @param inBodyPair The handle for the contact cache for this body pair
+	/// @param inBody1 The first body that is colliding
+	/// @param inBody2 The second body that is colliding
+	/// @param inManifold The manifold that describes the collision
+	/// @return true if a contact constraint was created (can be false in the case of a sensor)
+	///
+	/// This is using the approach described in 'Modeling and Solving Constraints' by Erin Catto presented at GDC 2009 (and later years with slight modifications).
+	/// We're using the formulas from slide 50 - 53 combined.
+	///
+	/// Euler velocity integration:
+	///
+	/// v1' = v1 + M^-1 P
+	///
+	/// Impulse:
+	///
+	/// P = J^T lambda
+	///
+	/// Constraint force:
+	///
+	/// lambda = -K^-1 J v1
+	///
+	/// Inverse effective mass:
+	///
+	/// K = J M^-1 J^T
+	///
+	/// Constraint equation (limits movement in 1 axis):
+	///
+	/// C = (p2 - p1) . n
+	///
+	/// Jacobian (for position constraint)
+	///
+	/// J = [-n, -r1 x n, n, r2 x n]
+	///
+	/// n = contact normal (pointing away from body 1).
+	/// p1, p2 = positions of collision on body 1 and 2.
+	/// r1, r2 = contact point relative to center of mass of body 1 and body 2 (r1 = p1 - x1, r2 = p2 - x2).
+	/// v1, v2 = (linear velocity, angular velocity): 6 vectors containing linear and angular velocity for body 1 and 2.
+	/// M = mass matrix, a diagonal matrix of the mass and inertia with diagonal [m1, I1, m2, I2].
+	bool						AddContactConstraint(ContactAllocator &ioContactAllocator, BodyPairHandle inBodyPair, Body &inBody1, Body &inBody2, const ContactManifold &inManifold);
+
+	/// Finalizes the contact cache, the contact cache that was generated during the calls to AddContactConstraint in this update
+	/// will be used from now on to read from. After finalizing the contact cache, the contact removed callbacks will be called.
+	/// inExpectedNumBodyPairs / inExpectedNumManifolds are the amount of body pairs / manifolds found in the previous step and is
+	/// used to determine the amount of buckets the contact cache hash map will use in the next update.
+	void						FinalizeContactCacheAndCallContactPointRemovedCallbacks(uint inExpectedNumBodyPairs, uint inExpectedNumManifolds);
+
+	/// Check if 2 bodies were in contact during the last simulation step. Since contacts are only detected between active bodies, at least one of the bodies must be active.
+	/// Uses the read collision cache to determine if 2 bodies are in contact.
+	bool						WereBodiesInContact(const BodyID &inBody1ID, const BodyID &inBody2ID) const;
+
+	/// Get the number of contact constraints that were found
+	uint32						GetNumConstraints() const											{ return min<uint32>(mNumConstraints, mMaxConstraints); }
+
+	/// Sort contact constraints deterministically
+	void						SortContacts(uint32 *inConstraintIdxBegin, uint32 *inConstraintIdxEnd) const;
+
+	/// Get the affected bodies for a given constraint
+	inline void					GetAffectedBodies(uint32 inConstraintIdx, const Body *&outBody1, const Body *&outBody2) const
+	{
+		const ContactConstraint &constraint = mConstraints[inConstraintIdx];
+		outBody1 = constraint.mBody1;
+		outBody2 = constraint.mBody2;
+	}
+
+	/// Apply last frame's impulses as an initial guess for this frame's impulses
+	template <class MotionPropertiesCallback>
+	void						WarmStartVelocityConstraints(const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inWarmStartImpulseRatio, MotionPropertiesCallback &ioCallback);
+
+	/// Solve velocity constraints, when almost nothing changes this should only apply very small impulses
+	/// since we're warm starting with the total impulse applied in the last frame above.
+	///
+	/// Friction wise we're using the Coulomb friction model which says that:
+	///
+	/// |F_T| <= mu |F_N|
+	///
+	/// Where F_T is the tangential force, F_N is the normal force and mu is the friction coefficient
+	///
+	/// In impulse terms this becomes:
+	///
+	/// |lambda_T| <= mu |lambda_N|
+	///
+	/// And the constraint that needs to be applied is exactly the same as a non penetration constraint
+	/// except that we use a tangent instead of a normal. The tangent should point in the direction of the
+	/// tangential velocity of the point:
+	///
+	/// J = [-T, -r1 x T, T, r2 x T]
+	///
+	/// Where T is the tangent.
+	///
+	/// See slide 42 and 43.
+	///
+	/// Restitution is implemented as a velocity bias (see slide 41):
+	///
+	/// b = e v_n^-
+	///
+	/// e = the restitution coefficient, v_n^- is the normal velocity prior to the collision
+	///
+	/// Restitution is only applied when v_n^- is large enough and the points are moving towards collision
+	bool						SolveVelocityConstraints(const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd);
+
+	/// Save back the lambdas to the contact cache for the next warm start
+	void						StoreAppliedImpulses(const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd) const;
+
+	/// Solve position constraints.
+	/// This is using the approach described in 'Modeling and Solving Constraints' by Erin Catto presented at GDC 2007.
+	/// On slide 78 it is suggested to split up the Baumgarte stabilization for positional drift so that it does not
+	/// actually add to the momentum. We combine an Euler velocity integrate + a position integrate and then discard the velocity
+	/// change.
+	///
+	/// Constraint force:
+	///
+	/// lambda = -K^-1 b
+	///
+	/// Baumgarte stabilization:
+	///
+	/// b = beta / dt C
+	///
+	/// beta = baumgarte stabilization factor.
+	/// dt = delta time.
+	bool						SolvePositionConstraints(const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd);
+
+	/// Recycle the constraint buffer. Should be called between collision simulation steps.
+	void						RecycleConstraintBuffer();
+
+	/// Terminate the constraint buffer. Should be called after simulation ends.
+	void						FinishConstraintBuffer();
+
+	/// Called by continuous collision detection to notify the contact listener that a contact was added
+	/// @param ioContactAllocator The allocator that reserves memory for the contacts
+	/// @param inBody1 The first body that is colliding
+	/// @param inBody2 The second body that is colliding
+	/// @param inManifold The manifold that describes the collision
+	/// @param outSettings The calculated contact settings (may be overridden by the contact listener)
+	void						OnCCDContactAdded(ContactAllocator &ioContactAllocator, const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &outSettings);
+
+#ifdef JPH_DEBUG_RENDERER
+	// Drawing properties
+	static bool					sDrawContactPoint;
+	static bool					sDrawSupportingFaces;
+	static bool					sDrawContactPointReduction;
+	static bool					sDrawContactManifolds;
+#endif // JPH_DEBUG_RENDERER
+
+	/// Saving state for replay
+	void						SaveState(StateRecorder &inStream, const StateRecorderFilter *inFilter) const;
+
+	/// Restoring state for replay. Returns false when failed.
+	bool						RestoreState(StateRecorder &inStream, const StateRecorderFilter *inFilter);
+
+private:
+	/// Local space contact point, used for caching impulses
+	class CachedContactPoint
+	{
+	public:
+		/// Saving / restoring state for replay
+		void					SaveState(StateRecorder &inStream) const;
+		void					RestoreState(StateRecorder &inStream);
+
+		/// Local space positions on body 1 and 2.
+		/// Note: these values are read through sLoadFloat3Unsafe.
+		Float3					mPosition1;
+		Float3					mPosition2;
+
+		/// Total applied impulse during the last update that it was used
+		float					mNonPenetrationLambda;
+		Vector<2>				mFrictionLambda;
+	};
+
+	static_assert(sizeof(CachedContactPoint) == 36, "Unexpected size");
+	static_assert(alignof(CachedContactPoint) == 4, "Assuming 4 byte aligned");
+
+	/// A single cached manifold
+	class CachedManifold
+	{
+	public:
+		/// Calculate size in bytes needed beyond the size of the class to store inNumContactPoints
+		static int				sGetRequiredExtraSize(int inNumContactPoints)						{ return max(0, inNumContactPoints - 1) * sizeof(CachedContactPoint); }
+
+		/// Calculate total class size needed for storing inNumContactPoints
+		static int				sGetRequiredTotalSize(int inNumContactPoints)						{ return sizeof(CachedManifold) + sGetRequiredExtraSize(inNumContactPoints); }
+
+		/// Saving / restoring state for replay
+		void					SaveState(StateRecorder &inStream) const;
+		void					RestoreState(StateRecorder &inStream);
+
+		/// Handle to next cached contact points in ManifoldCache::mCachedManifolds for the same body pair
+		uint32					mNextWithSameBodyPair;
+
+		/// Contact normal in the space of 2.
+		/// Note: this value is read through sLoadFloat3Unsafe.
+		Float3					mContactNormal;
+
+		/// Flags for this cached manifold
+		enum class EFlags : uint16
+		{
+			ContactPersisted	= 1,																///< If this cache entry was reused in the next simulation update
+			CCDContact			= 2																	///< This is a cached manifold reported by continuous collision detection and was only used to create a contact callback
+		};
+
+		/// @see EFlags
+		mutable atomic<uint16>	mFlags { 0 };
+
+		/// Number of contact points in the array below
+		uint16					mNumContactPoints;
+
+		/// Contact points that this manifold consists of
+		CachedContactPoint		mContactPoints[1];
+	};
+
+	static_assert(sizeof(CachedManifold) == 56, "This structure is expect to not contain any waste due to alignment");
+	static_assert(alignof(CachedManifold) == 4, "Assuming 4 byte aligned");
+
+	/// Define a map that maps SubShapeIDPair -> manifold
+	using ManifoldMap = LockFreeHashMap<SubShapeIDPair, CachedManifold>;
+	using MKeyValue = ManifoldMap::KeyValue;
+	using MKVAndCreated = std::pair<MKeyValue *, bool>;
+
+	/// Start of list of contact points for a particular pair of bodies
+	class CachedBodyPair
+	{
+	public:
+		/// Saving / restoring state for replay
+		void					SaveState(StateRecorder &inStream) const;
+		void					RestoreState(StateRecorder &inStream);
+
+		/// Local space position difference from Body A to Body B.
+		/// Note: this value is read through sLoadFloat3Unsafe
+		Float3					mDeltaPosition;
+
+		/// Local space rotation difference from Body A to Body B, fourth component of quaternion is not stored but is guaranteed >= 0.
+		/// Note: this value is read through sLoadFloat3Unsafe
+		Float3					mDeltaRotation;
+
+		/// Handle to first manifold in ManifoldCache::mCachedManifolds
+		uint32					mFirstCachedManifold;
+	};
+
+	static_assert(sizeof(CachedBodyPair) == 28, "Unexpected size");
+	static_assert(alignof(CachedBodyPair) == 4, "Assuming 4 byte aligned");
+
+	/// Define a map that maps BodyPair -> CachedBodyPair
+	using BodyPairMap = LockFreeHashMap<BodyPair, CachedBodyPair>;
+	using BPKeyValue = BodyPairMap::KeyValue;
+
+	/// Holds all caches that are needed to quickly find cached body pairs / manifolds
+	class ManifoldCache
+	{
+	public:
+		/// Initialize the cache
+		void					Init(uint inMaxBodyPairs, uint inMaxContactConstraints, uint inCachedManifoldsSize);
+
+		/// Reset all entries from the cache
+		void					Clear();
+
+		/// Prepare cache before creating new contacts.
+		/// inExpectedNumBodyPairs / inExpectedNumManifolds are the amount of body pairs / manifolds found in the previous step and is used to determine the amount of buckets the contact cache hash map will use.
+		void					Prepare(uint inExpectedNumBodyPairs, uint inExpectedNumManifolds);
+
+		/// Get a new allocator context for storing contacts. Note that you should call this once and then add multiple contacts using the context.
+		ContactAllocator		GetContactAllocator()						{ return ContactAllocator(mAllocator, cAllocatorBlockSize); }
+
+		/// Find / create cached entry for SubShapeIDPair -> CachedManifold
+		const MKeyValue *		Find(const SubShapeIDPair &inKey, uint64 inKeyHash) const;
+		MKeyValue *				Create(ContactAllocator &ioContactAllocator, const SubShapeIDPair &inKey, uint64 inKeyHash, int inNumContactPoints);
+		MKVAndCreated			FindOrCreate(ContactAllocator &ioContactAllocator, const SubShapeIDPair &inKey, uint64 inKeyHash, int inNumContactPoints);
+		uint32					ToHandle(const MKeyValue *inKeyValue) const;
+		const MKeyValue *		FromHandle(uint32 inHandle) const;
+
+		/// Find / create entry for BodyPair -> CachedBodyPair
+		const BPKeyValue *		Find(const BodyPair &inKey, uint64 inKeyHash) const;
+		BPKeyValue *			Create(ContactAllocator &ioContactAllocator, const BodyPair &inKey, uint64 inKeyHash);
+		void					GetAllBodyPairsSorted(Array<const BPKeyValue *> &outAll) const;
+		void					GetAllManifoldsSorted(const CachedBodyPair &inBodyPair, Array<const MKeyValue *> &outAll) const;
+		void					GetAllCCDManifoldsSorted(Array<const MKeyValue *> &outAll) const;
+		void					ContactPointRemovedCallbacks(ContactListener *inListener);
+
+#ifdef JPH_ENABLE_ASSERTS
+		/// Get the amount of manifolds in the cache
+		uint					GetNumManifolds() const						{ return mCachedManifolds.GetNumKeyValues(); }
+
+		/// Get the amount of body pairs in the cache
+		uint					GetNumBodyPairs() const						{ return mCachedBodyPairs.GetNumKeyValues(); }
+
+		/// Before a cache is finalized you can only do Create(), after only Find() or Clear()
+		void					Finalize();
+#endif
+
+		/// Saving / restoring state for replay
+		void					SaveState(StateRecorder &inStream, const StateRecorderFilter *inFilter) const;
+		bool					RestoreState(const ManifoldCache &inReadCache, StateRecorder &inStream, const StateRecorderFilter *inFilter);
+
+	private:
+		/// Block size used when allocating new blocks in the contact cache
+		static constexpr uint32	cAllocatorBlockSize = 4096;
+
+		/// Allocator used by both mCachedManifolds and mCachedBodyPairs, this makes it more likely that a body pair and its manifolds are close in memory
+		LFHMAllocator			mAllocator;
+
+		/// Simple hash map for SubShapeIDPair -> CachedManifold
+		ManifoldMap				mCachedManifolds { mAllocator };
+
+		/// Simple hash map for BodyPair -> CachedBodyPair
+		BodyPairMap				mCachedBodyPairs { mAllocator };
+
+#ifdef JPH_ENABLE_ASSERTS
+		bool					mIsFinalized = false;						///< Marks if this buffer is complete
+#endif
+	};
+
+	ManifoldCache				mCache[2];									///< We have one cache to read from and one to write to
+	int							mCacheWriteIdx = 0;							///< Which cache we're currently writing to
+
+	/// World space contact point, used for solving penetrations
+	class WorldContactPoint
+	{
+	public:
+		/// Calculate constraint properties below
+		void					CalculateNonPenetrationConstraintProperties(const Body &inBody1, float inInvMass1, float inInvInertiaScale1, const Body &inBody2, float inInvMass2, float inInvInertiaScale2, RVec3Arg inWorldSpacePosition1, RVec3Arg inWorldSpacePosition2, Vec3Arg inWorldSpaceNormal);
+
+		template <EMotionType Type1, EMotionType Type2>
+		JPH_INLINE void			TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(float inDeltaTime, float inGravityDeltaTimeDotNormal, const Body &inBody1, const Body &inBody2, float inInvM1, float inInvM2, Mat44Arg inInvI1, Mat44Arg inInvI2, RVec3Arg inWorldSpacePosition1, RVec3Arg inWorldSpacePosition2, Vec3Arg inWorldSpaceNormal, Vec3Arg inWorldSpaceTangent1, Vec3Arg inWorldSpaceTangent2, const ContactSettings &inSettings, float inMinVelocityForRestitution);
+
+		/// The constraint parts
+		AxisConstraintPart		mNonPenetrationConstraint;
+		AxisConstraintPart		mFrictionConstraint1;
+		AxisConstraintPart		mFrictionConstraint2;
+
+		/// Contact cache
+		CachedContactPoint *	mContactPoint;
+	};
+
+	using WorldContactPoints = StaticArray<WorldContactPoint, MaxContactPoints>;
+
+	/// Contact constraint class, used for solving penetrations
+	class ContactConstraint
+	{
+	public:
+	#ifdef JPH_DEBUG_RENDERER
+		/// Draw the state of the contact constraint
+		void					Draw(DebugRenderer *inRenderer, ColorArg inManifoldColor) const;
+	#endif // JPH_DEBUG_RENDERER
+
+		/// Convert the world space normal to a Vec3
+		JPH_INLINE Vec3			GetWorldSpaceNormal() const
+		{
+			return Vec3::sLoadFloat3Unsafe(mWorldSpaceNormal);
+		}
+
+		/// Get the tangents for this contact constraint
+		JPH_INLINE void			GetTangents(Vec3 &outTangent1, Vec3 &outTangent2) const
+		{
+			Vec3 ws_normal = GetWorldSpaceNormal();
+			outTangent1 = ws_normal.GetNormalizedPerpendicular();
+			outTangent2 = ws_normal.Cross(outTangent1);
+		}
+
+		Body *					mBody1;
+		Body *					mBody2;
+		uint64					mSortKey;
+		Float3					mWorldSpaceNormal;
+		float					mCombinedFriction;
+		float					mInvMass1;
+		float					mInvInertiaScale1;
+		float					mInvMass2;
+		float					mInvInertiaScale2;
+		WorldContactPoints		mContactPoints;
+	};
+
+public:
+	/// The maximum value that can be passed to Init for inMaxContactConstraints. Note you should really use a lower value, using this value will cost a lot of memory!
+	static constexpr uint		cMaxContactConstraintsLimit = ~uint(0) / sizeof(ContactConstraint);
+
+	/// The maximum value that can be passed to Init for inMaxBodyPairs. Note you should really use a lower value, using this value will cost a lot of memory!
+	static constexpr uint		cMaxBodyPairsLimit = ~uint(0) / sizeof(BodyPairMap::KeyValue);
+
+private:
+	/// Internal helper function to calculate the friction and non-penetration constraint properties. Templated to the motion type to reduce the amount of branches and calculations.
+	template <EMotionType Type1, EMotionType Type2>
+	JPH_INLINE void				TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(ContactConstraint &ioConstraint, const ContactSettings &inSettings, float inDeltaTime, Vec3Arg inGravityDeltaTime, RMat44Arg inTransformBody1, RMat44Arg inTransformBody2, const Body &inBody1, const Body &inBody2);
+
+	/// Internal helper function to calculate the friction and non-penetration constraint properties.
+	inline void					CalculateFrictionAndNonPenetrationConstraintProperties(ContactConstraint &ioConstraint, const ContactSettings &inSettings, float inDeltaTime, Vec3Arg inGravityDeltaTime, RMat44Arg inTransformBody1, RMat44Arg inTransformBody2, const Body &inBody1, const Body &inBody2);
+
+	/// Internal helper function to add a contact constraint. Templated to the motion type to reduce the amount of branches and calculations.
+	template <EMotionType Type1, EMotionType Type2>
+	bool						TemplatedAddContactConstraint(ContactAllocator &ioContactAllocator, BodyPairHandle inBodyPairHandle, Body &inBody1, Body &inBody2, const ContactManifold &inManifold);
+
+	/// Internal helper function to warm start contact constraint. Templated to the motion type to reduce the amount of branches and calculations.
+	template <EMotionType Type1, EMotionType Type2>
+	JPH_INLINE static void		sWarmStartConstraint(ContactConstraint &ioConstraint, MotionProperties *ioMotionProperties1, MotionProperties *ioMotionProperties2, float inWarmStartImpulseRatio);
+
+	/// Internal helper function to solve a single contact constraint. Templated to the motion type to reduce the amount of branches and calculations.
+	template <EMotionType Type1, EMotionType Type2>
+	JPH_INLINE static bool		sSolveVelocityConstraint(ContactConstraint &ioConstraint, MotionProperties *ioMotionProperties1, MotionProperties *ioMotionProperties2);
+
+	/// The main physics settings instance
+	const PhysicsSettings &		mPhysicsSettings;
+
+	/// Listener that is notified whenever a contact point between two bodies is added/updated/removed
+	ContactListener *			mContactListener = nullptr;
+
+	/// Functions that are used to combine friction and restitution of 2 bodies
+	CombineFunction				mCombineFriction = [](const Body &inBody1, const SubShapeID &, const Body &inBody2, const SubShapeID &) { return sqrt(inBody1.GetFriction() * inBody2.GetFriction()); };
+	CombineFunction				mCombineRestitution = [](const Body &inBody1, const SubShapeID &, const Body &inBody2, const SubShapeID &) { return max(inBody1.GetRestitution(), inBody2.GetRestitution()); };
+
+	/// The constraints that were added this frame
+	ContactConstraint *			mConstraints = nullptr;
+	uint32						mMaxConstraints = 0;
+	atomic<uint32>				mNumConstraints { 0 };
+
+	/// Context used for this physics update
+	PhysicsUpdateContext *		mUpdateContext;
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Constraints/DistanceConstraint.cpp

@@ -0,0 +1,266 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Constraints/DistanceConstraint.h>
+#include <Jolt/Physics/Body/Body.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
+
+using namespace literals;
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(DistanceConstraintSettings)
+{
+	JPH_ADD_BASE_CLASS(DistanceConstraintSettings, TwoBodyConstraintSettings)
+
+	JPH_ADD_ENUM_ATTRIBUTE(DistanceConstraintSettings, mSpace)
+	JPH_ADD_ATTRIBUTE(DistanceConstraintSettings, mPoint1)
+	JPH_ADD_ATTRIBUTE(DistanceConstraintSettings, mPoint2)
+	JPH_ADD_ATTRIBUTE(DistanceConstraintSettings, mMinDistance)
+	JPH_ADD_ATTRIBUTE(DistanceConstraintSettings, mMaxDistance)
+	JPH_ADD_ENUM_ATTRIBUTE_WITH_ALIAS(DistanceConstraintSettings, mLimitsSpringSettings.mMode, "mSpringMode")
+	JPH_ADD_ATTRIBUTE_WITH_ALIAS(DistanceConstraintSettings, mLimitsSpringSettings.mFrequency, "mFrequency") // Renaming attributes to stay compatible with old versions of the library
+	JPH_ADD_ATTRIBUTE_WITH_ALIAS(DistanceConstraintSettings, mLimitsSpringSettings.mDamping, "mDamping")
+}
+
+void DistanceConstraintSettings::SaveBinaryState(StreamOut &inStream) const
+{
+	ConstraintSettings::SaveBinaryState(inStream);
+
+	inStream.Write(mSpace);
+	inStream.Write(mPoint1);
+	inStream.Write(mPoint2);
+	inStream.Write(mMinDistance);
+	inStream.Write(mMaxDistance);
+	mLimitsSpringSettings.SaveBinaryState(inStream);
+}
+
+void DistanceConstraintSettings::RestoreBinaryState(StreamIn &inStream)
+{
+	ConstraintSettings::RestoreBinaryState(inStream);
+
+	inStream.Read(mSpace);
+	inStream.Read(mPoint1);
+	inStream.Read(mPoint2);
+	inStream.Read(mMinDistance);
+	inStream.Read(mMaxDistance);
+	mLimitsSpringSettings.RestoreBinaryState(inStream);
+}
+
+TwoBodyConstraint *DistanceConstraintSettings::Create(Body &inBody1, Body &inBody2) const
+{
+	return new DistanceConstraint(inBody1, inBody2, *this);
+}
+
+DistanceConstraint::DistanceConstraint(Body &inBody1, Body &inBody2, const DistanceConstraintSettings &inSettings) :
+	TwoBodyConstraint(inBody1, inBody2, inSettings),
+	mMinDistance(inSettings.mMinDistance),
+	mMaxDistance(inSettings.mMaxDistance)
+{
+	if (inSettings.mSpace == EConstraintSpace::WorldSpace)
+	{
+		// If all properties were specified in world space, take them to local space now
+		mLocalSpacePosition1 = Vec3(inBody1.GetInverseCenterOfMassTransform() * inSettings.mPoint1);
+		mLocalSpacePosition2 = Vec3(inBody2.GetInverseCenterOfMassTransform() * inSettings.mPoint2);
+		mWorldSpacePosition1 = inSettings.mPoint1;
+		mWorldSpacePosition2 = inSettings.mPoint2;
+	}
+	else
+	{
+		// If properties were specified in local space, we need to calculate world space positions
+		mLocalSpacePosition1 = Vec3(inSettings.mPoint1);
+		mLocalSpacePosition2 = Vec3(inSettings.mPoint2);
+		mWorldSpacePosition1 = inBody1.GetCenterOfMassTransform() * inSettings.mPoint1;
+		mWorldSpacePosition2 = inBody2.GetCenterOfMassTransform() * inSettings.mPoint2;
+	}
+
+	// Store distance we want to keep between the world space points
+	float distance = Vec3(mWorldSpacePosition2 - mWorldSpacePosition1).Length();
+	float min_distance, max_distance;
+	if (mMinDistance < 0.0f && mMaxDistance < 0.0f)
+	{
+		min_distance = max_distance = distance;
+	}
+	else
+	{
+		min_distance = mMinDistance < 0.0f? min(distance, mMaxDistance) : mMinDistance;
+		max_distance = mMaxDistance < 0.0f? max(distance, mMinDistance) : mMaxDistance;
+	}
+	SetDistance(min_distance, max_distance);
+
+	// Most likely gravity is going to tear us apart (this is only used when the distance between the points = 0)
+	mWorldSpaceNormal = Vec3::sAxisY();
+
+	// Store spring settings
+	SetLimitsSpringSettings(inSettings.mLimitsSpringSettings);
+}
+
+void DistanceConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM)
+{
+	if (mBody1->GetID() == inBodyID)
+		mLocalSpacePosition1 -= inDeltaCOM;
+	else if (mBody2->GetID() == inBodyID)
+		mLocalSpacePosition2 -= inDeltaCOM;
+}
+
+void DistanceConstraint::CalculateConstraintProperties(float inDeltaTime)
+{
+	// Update world space positions (the bodies may have moved)
+	mWorldSpacePosition1 = mBody1->GetCenterOfMassTransform() * mLocalSpacePosition1;
+	mWorldSpacePosition2 = mBody2->GetCenterOfMassTransform() * mLocalSpacePosition2;
+
+	// Calculate world space normal
+	Vec3 delta = Vec3(mWorldSpacePosition2 - mWorldSpacePosition1);
+	float delta_len = delta.Length();
+	if (delta_len > 0.0f)
+		mWorldSpaceNormal = delta / delta_len;
+
+	// Calculate points relative to body
+	// r1 + u = (p1 - x1) + (p2 - p1) = p2 - x1
+	Vec3 r1_plus_u = Vec3(mWorldSpacePosition2 - mBody1->GetCenterOfMassPosition());
+	Vec3 r2 = Vec3(mWorldSpacePosition2 - mBody2->GetCenterOfMassPosition());
+
+	if (mMinDistance == mMaxDistance)
+	{
+		mAxisConstraint.CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, r1_plus_u, *mBody2, r2, mWorldSpaceNormal, 0.0f, delta_len - mMinDistance, mLimitsSpringSettings);
+
+		// Single distance, allow constraint forces in both directions
+		mMinLambda = -FLT_MAX;
+		mMaxLambda = FLT_MAX;
+	}
+	else if (delta_len <= mMinDistance)
+	{
+		mAxisConstraint.CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, r1_plus_u, *mBody2, r2, mWorldSpaceNormal, 0.0f, delta_len - mMinDistance, mLimitsSpringSettings);
+
+		// Allow constraint forces to make distance bigger only
+		mMinLambda = 0;
+		mMaxLambda = FLT_MAX;
+	}
+	else if (delta_len >= mMaxDistance)
+	{
+		mAxisConstraint.CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, r1_plus_u, *mBody2, r2, mWorldSpaceNormal, 0.0f, delta_len - mMaxDistance, mLimitsSpringSettings);
+
+		// Allow constraint forces to make distance smaller only
+		mMinLambda = -FLT_MAX;
+		mMaxLambda = 0;
+	}
+	else
+		mAxisConstraint.Deactivate();
+}
+
+void DistanceConstraint::SetupVelocityConstraint(float inDeltaTime)
+{
+	CalculateConstraintProperties(inDeltaTime);
+}
+
+void DistanceConstraint::ResetWarmStart()
+{
+	mAxisConstraint.Deactivate();
+}
+
+void DistanceConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio)
+{
+	mAxisConstraint.WarmStart(*mBody1, *mBody2, mWorldSpaceNormal, inWarmStartImpulseRatio);
+}
+
+bool DistanceConstraint::SolveVelocityConstraint(float inDeltaTime)
+{
+	if (mAxisConstraint.IsActive())
+		return mAxisConstraint.SolveVelocityConstraint(*mBody1, *mBody2, mWorldSpaceNormal, mMinLambda, mMaxLambda);
+	else
+		return false;
+}
+
+bool DistanceConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte)
+{
+	if (mLimitsSpringSettings.mFrequency <= 0.0f) // When the spring is active, we don't need to solve the position constraint
+	{
+		float distance = Vec3(mWorldSpacePosition2 - mWorldSpacePosition1).Dot(mWorldSpaceNormal);
+
+		// Calculate position error
+		float position_error = 0.0f;
+		if (distance < mMinDistance)
+			position_error = distance - mMinDistance;
+		else if (distance > mMaxDistance)
+			position_error = distance - mMaxDistance;
+
+		if (position_error != 0.0f)
+		{
+			// Update constraint properties (bodies may have moved)
+			CalculateConstraintProperties(inDeltaTime);
+
+			return mAxisConstraint.SolvePositionConstraint(*mBody1, *mBody2, mWorldSpaceNormal, position_error, inBaumgarte);
+		}
+	}
+
+	return false;
+}
+
+#ifdef JPH_DEBUG_RENDERER
+void DistanceConstraint::DrawConstraint(DebugRenderer *inRenderer) const
+{
+	// Draw constraint
+	Vec3 delta = Vec3(mWorldSpacePosition2 - mWorldSpacePosition1);
+	float len = delta.Length();
+	if (len < mMinDistance)
+	{
+		RVec3 real_end_pos = mWorldSpacePosition1 + (len > 0.0f? delta * mMinDistance / len : Vec3(0, len, 0));
+		inRenderer->DrawLine(mWorldSpacePosition1, mWorldSpacePosition2, Color::sGreen);
+		inRenderer->DrawLine(mWorldSpacePosition2, real_end_pos, Color::sYellow);
+	}
+	else if (len > mMaxDistance)
+	{
+		RVec3 real_end_pos = mWorldSpacePosition1 + (len > 0.0f? delta * mMaxDistance / len : Vec3(0, len, 0));
+		inRenderer->DrawLine(mWorldSpacePosition1, real_end_pos, Color::sGreen);
+		inRenderer->DrawLine(real_end_pos, mWorldSpacePosition2, Color::sRed);
+	}
+	else
+		inRenderer->DrawLine(mWorldSpacePosition1, mWorldSpacePosition2, Color::sGreen);
+
+	// Draw constraint end points
+	inRenderer->DrawMarker(mWorldSpacePosition1, Color::sWhite, 0.1f);
+	inRenderer->DrawMarker(mWorldSpacePosition2, Color::sWhite, 0.1f);
+
+	// Draw current length
+	inRenderer->DrawText3D(0.5_r * (mWorldSpacePosition1 + mWorldSpacePosition2), StringFormat("%.2f", (double)len));
+}
+#endif // JPH_DEBUG_RENDERER
+
+void DistanceConstraint::SaveState(StateRecorder &inStream) const
+{
+	TwoBodyConstraint::SaveState(inStream);
+
+	mAxisConstraint.SaveState(inStream);
+	inStream.Write(mWorldSpaceNormal); // When distance = 0, the normal is used from last frame so we need to store it
+}
+
+void DistanceConstraint::RestoreState(StateRecorder &inStream)
+{
+	TwoBodyConstraint::RestoreState(inStream);
+
+	mAxisConstraint.RestoreState(inStream);
+	inStream.Read(mWorldSpaceNormal);
+}
+
+Ref<ConstraintSettings> DistanceConstraint::GetConstraintSettings() const
+{
+	DistanceConstraintSettings *settings = new DistanceConstraintSettings;
+	ToConstraintSettings(*settings);
+	settings->mSpace = EConstraintSpace::LocalToBodyCOM;
+	settings->mPoint1 = RVec3(mLocalSpacePosition1);
+	settings->mPoint2 = RVec3(mLocalSpacePosition2);
+	settings->mMinDistance = mMinDistance;
+	settings->mMaxDistance = mMaxDistance;
+	settings->mLimitsSpringSettings = mLimitsSpringSettings;
+	return settings;
+}
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Constraints/DistanceConstraint.h

@@ -0,0 +1,120 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Constraints/TwoBodyConstraint.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/AxisConstraintPart.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Distance constraint settings, used to create a distance constraint
+class JPH_EXPORT DistanceConstraintSettings final : public TwoBodyConstraintSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, DistanceConstraintSettings)
+
+public:
+	// See: ConstraintSettings::SaveBinaryState
+	virtual void				SaveBinaryState(StreamOut &inStream) const override;
+
+	/// Create an instance of this constraint
+	virtual TwoBodyConstraint *	Create(Body &inBody1, Body &inBody2) const override;
+
+	/// This determines in which space the constraint is setup, all properties below should be in the specified space
+	EConstraintSpace			mSpace = EConstraintSpace::WorldSpace;
+
+	/// Body 1 constraint reference frame (space determined by mSpace).
+	/// Constraint will keep mPoint1 (a point on body 1) and mPoint2 (a point on body 2) at the same distance.
+	/// Note that this constraint can be used as a cheap PointConstraint by setting mPoint1 = mPoint2 (but this removes only 1 degree of freedom instead of 3).
+	RVec3						mPoint1 = RVec3::sZero();
+
+	/// Body 2 constraint reference frame (space determined by mSpace)
+	RVec3						mPoint2 = RVec3::sZero();
+
+	/// Ability to override the distance range at which the two points are kept apart. If the value is negative, it will be replaced by the distance between mPoint1 and mPoint2 (works only if mSpace is world space).
+	float						mMinDistance = -1.0f;
+	float						mMaxDistance = -1.0f;
+
+	/// When enabled, this makes the limits soft. When the constraint exceeds the limits, a spring force will pull it back.
+	SpringSettings				mLimitsSpringSettings;
+
+protected:
+	// See: ConstraintSettings::RestoreBinaryState
+	virtual void				RestoreBinaryState(StreamIn &inStream) override;
+};
+
+/// This constraint is a stiff spring that holds 2 points at a fixed distance from each other
+class JPH_EXPORT DistanceConstraint final : public TwoBodyConstraint
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Construct distance constraint
+								DistanceConstraint(Body &inBody1, Body &inBody2, const DistanceConstraintSettings &inSettings);
+
+	// Generic interface of a constraint
+	virtual EConstraintSubType	GetSubType() const override									{ return EConstraintSubType::Distance; }
+	virtual void				NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override;
+	virtual void				SetupVelocityConstraint(float inDeltaTime) override;
+	virtual void				ResetWarmStart() override;
+	virtual void				WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override;
+	virtual bool				SolveVelocityConstraint(float inDeltaTime) override;
+	virtual bool				SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override;
+#ifdef JPH_DEBUG_RENDERER
+	virtual void				DrawConstraint(DebugRenderer *inRenderer) const override;
+#endif // JPH_DEBUG_RENDERER
+	virtual void				SaveState(StateRecorder &inStream) const override;
+	virtual void				RestoreState(StateRecorder &inStream) override;
+	virtual Ref<ConstraintSettings> GetConstraintSettings() const override;
+
+	// See: TwoBodyConstraint
+	virtual Mat44				GetConstraintToBody1Matrix() const override					{ return Mat44::sTranslation(mLocalSpacePosition1); }
+	virtual Mat44				GetConstraintToBody2Matrix() const override					{ return Mat44::sTranslation(mLocalSpacePosition2); } // Note: Incorrect rotation as we don't track the original rotation difference, should not matter though as the constraint is not limiting rotation.
+
+	/// Update the minimum and maximum distance for the constraint
+	void						SetDistance(float inMinDistance, float inMaxDistance)		{ JPH_ASSERT(inMinDistance <= inMaxDistance); mMinDistance = inMinDistance; mMaxDistance = inMaxDistance; }
+	float						GetMinDistance() const										{ return mMinDistance; }
+	float						GetMaxDistance() const										{ return mMaxDistance; }
+
+	/// Update the limits spring settings
+	const SpringSettings &		GetLimitsSpringSettings() const								{ return mLimitsSpringSettings; }
+	SpringSettings &			GetLimitsSpringSettings()									{ return mLimitsSpringSettings; }
+	void						SetLimitsSpringSettings(const SpringSettings &inLimitsSpringSettings) { mLimitsSpringSettings = inLimitsSpringSettings; }
+
+	///@name Get Lagrange multiplier from last physics update (the linear impulse applied to satisfy the constraint)
+	inline float				GetTotalLambdaPosition() const								{ return mAxisConstraint.GetTotalLambda(); }
+
+private:
+	// Internal helper function to calculate the values below
+	void						CalculateConstraintProperties(float inDeltaTime);
+
+	// CONFIGURATION PROPERTIES FOLLOW
+
+	// Local space constraint positions
+	Vec3						mLocalSpacePosition1;
+	Vec3						mLocalSpacePosition2;
+
+	// Min/max distance that must be kept between the world space points
+	float						mMinDistance;
+	float						mMaxDistance;
+
+	// Soft constraint limits
+	SpringSettings				mLimitsSpringSettings;
+
+	// RUN TIME PROPERTIES FOLLOW
+
+	// World space positions and normal
+	RVec3						mWorldSpacePosition1;
+	RVec3						mWorldSpacePosition2;
+	Vec3						mWorldSpaceNormal;
+
+	// Depending on if the distance < min or distance > max we can apply forces to prevent further violations
+	float						mMinLambda;
+	float						mMaxLambda;
+
+	// The constraint part
+	AxisConstraintPart			mAxisConstraint;
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Constraints/FixedConstraint.cpp

@@ -0,0 +1,215 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Constraints/FixedConstraint.h>
+#include <Jolt/Physics/Body/Body.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+using namespace literals;
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(FixedConstraintSettings)
+{
+	JPH_ADD_BASE_CLASS(FixedConstraintSettings, TwoBodyConstraintSettings)
+
+	JPH_ADD_ENUM_ATTRIBUTE(FixedConstraintSettings, mSpace)
+	JPH_ADD_ATTRIBUTE(FixedConstraintSettings, mAutoDetectPoint)
+	JPH_ADD_ATTRIBUTE(FixedConstraintSettings, mPoint1)
+	JPH_ADD_ATTRIBUTE(FixedConstraintSettings, mAxisX1)
+	JPH_ADD_ATTRIBUTE(FixedConstraintSettings, mAxisY1)
+	JPH_ADD_ATTRIBUTE(FixedConstraintSettings, mPoint2)
+	JPH_ADD_ATTRIBUTE(FixedConstraintSettings, mAxisX2)
+	JPH_ADD_ATTRIBUTE(FixedConstraintSettings, mAxisY2)
+}
+
+void FixedConstraintSettings::SaveBinaryState(StreamOut &inStream) const
+{
+	ConstraintSettings::SaveBinaryState(inStream);
+
+	inStream.Write(mSpace);
+	inStream.Write(mAutoDetectPoint);
+	inStream.Write(mPoint1);
+	inStream.Write(mAxisX1);
+	inStream.Write(mAxisY1);
+	inStream.Write(mPoint2);
+	inStream.Write(mAxisX2);
+	inStream.Write(mAxisY2);
+}
+
+void FixedConstraintSettings::RestoreBinaryState(StreamIn &inStream)
+{
+	ConstraintSettings::RestoreBinaryState(inStream);
+
+	inStream.Read(mSpace);
+	inStream.Read(mAutoDetectPoint);
+	inStream.Read(mPoint1);
+	inStream.Read(mAxisX1);
+	inStream.Read(mAxisY1);
+	inStream.Read(mPoint2);
+	inStream.Read(mAxisX2);
+	inStream.Read(mAxisY2);
+}
+
+TwoBodyConstraint *FixedConstraintSettings::Create(Body &inBody1, Body &inBody2) const
+{
+	return new FixedConstraint(inBody1, inBody2, *this);
+}
+
+FixedConstraint::FixedConstraint(Body &inBody1, Body &inBody2, const FixedConstraintSettings &inSettings) :
+	TwoBodyConstraint(inBody1, inBody2, inSettings)
+{
+	// Store inverse of initial rotation from body 1 to body 2 in body 1 space
+	mInvInitialOrientation = RotationEulerConstraintPart::sGetInvInitialOrientationXY(inSettings.mAxisX1, inSettings.mAxisY1, inSettings.mAxisX2, inSettings.mAxisY2);
+
+	if (inSettings.mSpace == EConstraintSpace::WorldSpace)
+	{
+		if (inSettings.mAutoDetectPoint)
+		{
+			// Determine anchor point: If any of the bodies can never be dynamic use the other body as anchor point
+			RVec3 anchor;
+			if (!inBody1.CanBeKinematicOrDynamic())
+				anchor = inBody2.GetCenterOfMassPosition();
+			else if (!inBody2.CanBeKinematicOrDynamic())
+				anchor = inBody1.GetCenterOfMassPosition();
+			else
+			{
+				// Otherwise use weighted anchor point towards the lightest body
+				Real inv_m1 = Real(inBody1.GetMotionPropertiesUnchecked()->GetInverseMassUnchecked());
+				Real inv_m2 = Real(inBody2.GetMotionPropertiesUnchecked()->GetInverseMassUnchecked());
+				Real total_inv_mass = inv_m1 + inv_m2;
+				if (total_inv_mass != 0.0_r)
+					anchor = (inv_m1 * inBody1.GetCenterOfMassPosition() + inv_m2 * inBody2.GetCenterOfMassPosition()) / (inv_m1 + inv_m2);
+				else
+					anchor = inBody1.GetCenterOfMassPosition();
+			}
+
+			// Store local positions
+			mLocalSpacePosition1 = Vec3(inBody1.GetInverseCenterOfMassTransform() * anchor);
+			mLocalSpacePosition2 = Vec3(inBody2.GetInverseCenterOfMassTransform() * anchor);
+		}
+		else
+		{
+			// Store local positions
+			mLocalSpacePosition1 = Vec3(inBody1.GetInverseCenterOfMassTransform() * inSettings.mPoint1);
+			mLocalSpacePosition2 = Vec3(inBody2.GetInverseCenterOfMassTransform() * inSettings.mPoint2);
+		}
+
+		// Constraints were specified in world space, so we should have replaced c1 with q10^-1 c1 and c2 with q20^-1 c2
+		// => r0^-1 = (q20^-1 c2) (q10^-1 c1)^1 = q20^-1 (c2 c1^-1) q10
+		mInvInitialOrientation = inBody2.GetRotation().Conjugated() * mInvInitialOrientation * inBody1.GetRotation();
+	}
+	else
+	{
+		// Store local positions
+		mLocalSpacePosition1 = Vec3(inSettings.mPoint1);
+		mLocalSpacePosition2 = Vec3(inSettings.mPoint2);
+	}
+}
+
+void FixedConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM)
+{
+	if (mBody1->GetID() == inBodyID)
+		mLocalSpacePosition1 -= inDeltaCOM;
+	else if (mBody2->GetID() == inBodyID)
+		mLocalSpacePosition2 -= inDeltaCOM;
+}
+
+void FixedConstraint::SetupVelocityConstraint(float inDeltaTime)
+{
+	// Calculate constraint values that don't change when the bodies don't change position
+	Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation());
+	Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation());
+	mRotationConstraintPart.CalculateConstraintProperties(*mBody1, rotation1, *mBody2, rotation2);
+	mPointConstraintPart.CalculateConstraintProperties(*mBody1, rotation1, mLocalSpacePosition1, *mBody2, rotation2, mLocalSpacePosition2);
+}
+
+void FixedConstraint::ResetWarmStart()
+{
+	mRotationConstraintPart.Deactivate();
+	mPointConstraintPart.Deactivate();
+}
+
+void FixedConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio)
+{
+	// Warm starting: Apply previous frame impulse
+	mRotationConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio);
+	mPointConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio);
+}
+
+bool FixedConstraint::SolveVelocityConstraint(float inDeltaTime)
+{
+	// Solve rotation constraint
+	bool rot = mRotationConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2);
+
+	// Solve position constraint
+	bool pos = mPointConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2);
+
+	return rot || pos;
+}
+
+bool FixedConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte)
+{
+	// Solve rotation constraint
+	mRotationConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(mBody1->GetRotation()), *mBody2, Mat44::sRotation(mBody2->GetRotation()));
+	bool rot = mRotationConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mInvInitialOrientation, inBaumgarte);
+
+	// Solve position constraint
+	mPointConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(mBody1->GetRotation()), mLocalSpacePosition1, *mBody2, Mat44::sRotation(mBody2->GetRotation()), mLocalSpacePosition2);
+	bool pos = mPointConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, inBaumgarte);
+
+	return rot || pos;
+}
+
+#ifdef JPH_DEBUG_RENDERER
+void FixedConstraint::DrawConstraint(DebugRenderer *inRenderer) const
+{
+	RMat44 com1 = mBody1->GetCenterOfMassTransform();
+	RMat44 com2 = mBody2->GetCenterOfMassTransform();
+
+	RVec3 anchor1 = com1 * mLocalSpacePosition1;
+	RVec3 anchor2 = com2 * mLocalSpacePosition2;
+
+	// Draw constraint
+	inRenderer->DrawLine(com1.GetTranslation(), anchor1, Color::sGreen);
+	inRenderer->DrawLine(com2.GetTranslation(), anchor2, Color::sBlue);
+}
+#endif // JPH_DEBUG_RENDERER
+
+void FixedConstraint::SaveState(StateRecorder &inStream) const
+{
+	TwoBodyConstraint::SaveState(inStream);
+
+	mRotationConstraintPart.SaveState(inStream);
+	mPointConstraintPart.SaveState(inStream);
+}
+
+void FixedConstraint::RestoreState(StateRecorder &inStream)
+{
+	TwoBodyConstraint::RestoreState(inStream);
+
+	mRotationConstraintPart.RestoreState(inStream);
+	mPointConstraintPart.RestoreState(inStream);
+}
+
+Ref<ConstraintSettings> FixedConstraint::GetConstraintSettings() const
+{
+	FixedConstraintSettings *settings = new FixedConstraintSettings;
+	ToConstraintSettings(*settings);
+	settings->mSpace = EConstraintSpace::LocalToBodyCOM;
+	settings->mPoint1 = RVec3(mLocalSpacePosition1);
+	settings->mAxisX1 = Vec3::sAxisX();
+	settings->mAxisY1 = Vec3::sAxisY();
+	settings->mPoint2 = RVec3(mLocalSpacePosition2);
+	settings->mAxisX2 = mInvInitialOrientation.RotateAxisX();
+	settings->mAxisY2 = mInvInitialOrientation.RotateAxisY();
+	return settings;
+}
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Constraints/FixedConstraint.h

@@ -0,0 +1,96 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Constraints/TwoBodyConstraint.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/RotationEulerConstraintPart.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/PointConstraintPart.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Fixed constraint settings, used to create a fixed constraint
+class JPH_EXPORT FixedConstraintSettings final : public TwoBodyConstraintSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, FixedConstraintSettings)
+
+public:
+	// See: ConstraintSettings::SaveBinaryState
+	virtual void				SaveBinaryState(StreamOut &inStream) const override;
+
+	/// Create an instance of this constraint
+	virtual TwoBodyConstraint *	Create(Body &inBody1, Body &inBody2) const override;
+
+	/// This determines in which space the constraint is setup, all properties below should be in the specified space
+	EConstraintSpace			mSpace = EConstraintSpace::WorldSpace;
+
+	/// When mSpace is WorldSpace mPoint1 and mPoint2 can be automatically calculated based on the positions of the bodies when the constraint is created (they will be fixated in their current relative position/orientation). Set this to false if you want to supply the attachment points yourself.
+	bool						mAutoDetectPoint = false;
+
+	/// Body 1 constraint reference frame (space determined by mSpace)
+	RVec3						mPoint1 = RVec3::sZero();
+	Vec3						mAxisX1 = Vec3::sAxisX();
+	Vec3						mAxisY1 = Vec3::sAxisY();
+
+	/// Body 2 constraint reference frame (space determined by mSpace)
+	RVec3						mPoint2 = RVec3::sZero();
+	Vec3						mAxisX2 = Vec3::sAxisX();
+	Vec3						mAxisY2 = Vec3::sAxisY();
+
+protected:
+	// See: ConstraintSettings::RestoreBinaryState
+	virtual void				RestoreBinaryState(StreamIn &inStream) override;
+};
+
+/// A fixed constraint welds two bodies together removing all degrees of freedom between them.
+/// This variant uses Euler angles for the rotation constraint.
+class JPH_EXPORT FixedConstraint final : public TwoBodyConstraint
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+								FixedConstraint(Body &inBody1, Body &inBody2, const FixedConstraintSettings &inSettings);
+
+	// Generic interface of a constraint
+	virtual EConstraintSubType	GetSubType() const override									{ return EConstraintSubType::Fixed; }
+	virtual void				NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override;
+	virtual void				SetupVelocityConstraint(float inDeltaTime) override;
+	virtual void				ResetWarmStart() override;
+	virtual void				WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override;
+	virtual bool				SolveVelocityConstraint(float inDeltaTime) override;
+	virtual bool				SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override;
+#ifdef JPH_DEBUG_RENDERER
+	virtual void				DrawConstraint(DebugRenderer *inRenderer) const override;
+#endif // JPH_DEBUG_RENDERER
+	virtual void				SaveState(StateRecorder &inStream) const override;
+	virtual void				RestoreState(StateRecorder &inStream) override;
+	virtual Ref<ConstraintSettings> GetConstraintSettings() const override;
+
+	// See: TwoBodyConstraint
+	virtual Mat44				GetConstraintToBody1Matrix() const override					{ return Mat44::sTranslation(mLocalSpacePosition1); }
+	virtual Mat44				GetConstraintToBody2Matrix() const override					{ return Mat44::sRotationTranslation(mInvInitialOrientation, mLocalSpacePosition2); }
+
+	///@name Get Lagrange multiplier from last physics update (the linear/angular impulse applied to satisfy the constraint)
+	inline Vec3					GetTotalLambdaPosition() const								{ return mPointConstraintPart.GetTotalLambda(); }
+	inline Vec3					GetTotalLambdaRotation() const								{ return mRotationConstraintPart.GetTotalLambda(); }
+
+private:
+	// CONFIGURATION PROPERTIES FOLLOW
+
+	// Local space constraint positions
+	Vec3						mLocalSpacePosition1;
+	Vec3						mLocalSpacePosition2;
+
+	// Inverse of initial rotation from body 1 to body 2 in body 1 space
+	Quat						mInvInitialOrientation;
+
+	// RUN TIME PROPERTIES FOLLOW
+
+	// The constraint parts
+	RotationEulerConstraintPart	mRotationConstraintPart;
+	PointConstraintPart			mPointConstraintPart;
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Constraints/GearConstraint.cpp

@@ -0,0 +1,188 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Constraints/GearConstraint.h>
+#include <Jolt/Physics/Constraints/HingeConstraint.h>
+#include <Jolt/Physics/Body/Body.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(GearConstraintSettings)
+{
+	JPH_ADD_BASE_CLASS(GearConstraintSettings, TwoBodyConstraintSettings)
+
+	JPH_ADD_ENUM_ATTRIBUTE(GearConstraintSettings, mSpace)
+	JPH_ADD_ATTRIBUTE(GearConstraintSettings, mHingeAxis1)
+	JPH_ADD_ATTRIBUTE(GearConstraintSettings, mHingeAxis2)
+	JPH_ADD_ATTRIBUTE(GearConstraintSettings, mRatio)
+}
+
+void GearConstraintSettings::SaveBinaryState(StreamOut &inStream) const
+{
+	ConstraintSettings::SaveBinaryState(inStream);
+
+	inStream.Write(mSpace);
+	inStream.Write(mHingeAxis1);
+	inStream.Write(mHingeAxis2);
+	inStream.Write(mRatio);
+}
+
+void GearConstraintSettings::RestoreBinaryState(StreamIn &inStream)
+{
+	ConstraintSettings::RestoreBinaryState(inStream);
+
+	inStream.Read(mSpace);
+	inStream.Read(mHingeAxis1);
+	inStream.Read(mHingeAxis2);
+	inStream.Read(mRatio);
+}
+
+TwoBodyConstraint *GearConstraintSettings::Create(Body &inBody1, Body &inBody2) const
+{
+	return new GearConstraint(inBody1, inBody2, *this);
+}
+
+GearConstraint::GearConstraint(Body &inBody1, Body &inBody2, const GearConstraintSettings &inSettings) :
+	TwoBodyConstraint(inBody1, inBody2, inSettings),
+	mLocalSpaceHingeAxis1(inSettings.mHingeAxis1),
+	mLocalSpaceHingeAxis2(inSettings.mHingeAxis2),
+	mRatio(inSettings.mRatio)
+{
+	if (inSettings.mSpace == EConstraintSpace::WorldSpace)
+	{
+		// If all properties were specified in world space, take them to local space now
+		mLocalSpaceHingeAxis1 = inBody1.GetInverseCenterOfMassTransform().Multiply3x3(mLocalSpaceHingeAxis1).Normalized();
+		mLocalSpaceHingeAxis2 = inBody2.GetInverseCenterOfMassTransform().Multiply3x3(mLocalSpaceHingeAxis2).Normalized();
+	}
+}
+
+void GearConstraint::CalculateConstraintProperties(Mat44Arg inRotation1, Mat44Arg inRotation2)
+{
+	// Calculate world space normals
+	mWorldSpaceHingeAxis1 = inRotation1 * mLocalSpaceHingeAxis1;
+	mWorldSpaceHingeAxis2 = inRotation2 * mLocalSpaceHingeAxis2;
+
+	mGearConstraintPart.CalculateConstraintProperties(*mBody1, mWorldSpaceHingeAxis1, *mBody2, mWorldSpaceHingeAxis2, mRatio);
+}
+
+void GearConstraint::SetupVelocityConstraint(float inDeltaTime)
+{
+	// Calculate constraint properties that are constant while bodies don't move
+	Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation());
+	Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation());
+	CalculateConstraintProperties(rotation1, rotation2);
+}
+
+void GearConstraint::ResetWarmStart()
+{
+	mGearConstraintPart.Deactivate();
+}
+
+void GearConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio)
+{
+	// Warm starting: Apply previous frame impulse
+	mGearConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio);
+}
+
+bool GearConstraint::SolveVelocityConstraint(float inDeltaTime)
+{
+	return mGearConstraintPart.SolveVelocityConstraint(*mBody1, mWorldSpaceHingeAxis1, *mBody2, mWorldSpaceHingeAxis2, mRatio);
+}
+
+bool GearConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte)
+{
+	if (mGear1Constraint == nullptr || mGear2Constraint == nullptr)
+		return false;
+
+	float gear1rot;
+	if (mGear1Constraint->GetSubType() == EConstraintSubType::Hinge)
+	{
+		gear1rot = StaticCast<HingeConstraint>(mGear1Constraint)->GetCurrentAngle();
+	}
+	else
+	{
+		JPH_ASSERT(false, "Unsupported");
+		return false;
+	}
+
+	float gear2rot;
+	if (mGear2Constraint->GetSubType() == EConstraintSubType::Hinge)
+	{
+		gear2rot = StaticCast<HingeConstraint>(mGear2Constraint)->GetCurrentAngle();
+	}
+	else
+	{
+		JPH_ASSERT(false, "Unsupported");
+		return false;
+	}
+
+	float error = CenterAngleAroundZero(fmod(gear1rot + mRatio * gear2rot, 2.0f * JPH_PI));
+	if (error == 0.0f)
+		return false;
+
+	Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation());
+	Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation());
+	CalculateConstraintProperties(rotation1, rotation2);
+	return mGearConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, error, inBaumgarte);
+}
+
+#ifdef JPH_DEBUG_RENDERER
+void GearConstraint::DrawConstraint(DebugRenderer *inRenderer) const
+{
+	RMat44 transform1 = mBody1->GetCenterOfMassTransform();
+	RMat44 transform2 = mBody2->GetCenterOfMassTransform();
+
+	// Draw constraint axis
+	inRenderer->DrawArrow(transform1.GetTranslation(), transform1 * mLocalSpaceHingeAxis1, Color::sGreen, 0.01f);
+	inRenderer->DrawArrow(transform2.GetTranslation(), transform2 * mLocalSpaceHingeAxis2, Color::sBlue, 0.01f);
+}
+
+#endif // JPH_DEBUG_RENDERER
+
+void GearConstraint::SaveState(StateRecorder &inStream) const
+{
+	TwoBodyConstraint::SaveState(inStream);
+
+	mGearConstraintPart.SaveState(inStream);
+}
+
+void GearConstraint::RestoreState(StateRecorder &inStream)
+{
+	TwoBodyConstraint::RestoreState(inStream);
+
+	mGearConstraintPart.RestoreState(inStream);
+}
+
+Ref<ConstraintSettings> GearConstraint::GetConstraintSettings() const
+{
+	GearConstraintSettings *settings = new GearConstraintSettings;
+	ToConstraintSettings(*settings);
+	settings->mSpace = EConstraintSpace::LocalToBodyCOM;
+	settings->mHingeAxis1 = mLocalSpaceHingeAxis1;
+	settings->mHingeAxis2 = mLocalSpaceHingeAxis2;
+	settings->mRatio = mRatio;
+	return settings;
+}
+
+Mat44 GearConstraint::GetConstraintToBody1Matrix() const
+{
+	Vec3 perp = mLocalSpaceHingeAxis1.GetNormalizedPerpendicular();
+	return Mat44(Vec4(mLocalSpaceHingeAxis1, 0), Vec4(perp, 0), Vec4(mLocalSpaceHingeAxis1.Cross(perp), 0), Vec4(0, 0, 0, 1));
+}
+
+Mat44 GearConstraint::GetConstraintToBody2Matrix() const
+{
+	Vec3 perp = mLocalSpaceHingeAxis2.GetNormalizedPerpendicular();
+	return Mat44(Vec4(mLocalSpaceHingeAxis2, 0), Vec4(perp, 0), Vec4(mLocalSpaceHingeAxis2.Cross(perp), 0), Vec4(0, 0, 0, 1));
+}
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Constraints/GearConstraint.h

@@ -0,0 +1,116 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Constraints/TwoBodyConstraint.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/GearConstraintPart.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Gear constraint settings
+class JPH_EXPORT GearConstraintSettings final : public TwoBodyConstraintSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, GearConstraintSettings)
+
+public:
+	// See: ConstraintSettings::SaveBinaryState
+	virtual void				SaveBinaryState(StreamOut &inStream) const override;
+
+	/// Create an instance of this constraint.
+	virtual TwoBodyConstraint *	Create(Body &inBody1, Body &inBody2) const override;
+
+	/// Defines the ratio between the rotation of both gears
+	/// The ratio is defined as: Gear1Rotation(t) = -ratio * Gear2Rotation(t)
+	/// @param inNumTeethGear1 Number of teeth that body 1 has
+	/// @param inNumTeethGear2 Number of teeth that body 2 has
+	void						SetRatio(int inNumTeethGear1, int inNumTeethGear2)
+	{
+		mRatio = float(inNumTeethGear2) / float(inNumTeethGear1);
+	}
+
+	/// This determines in which space the constraint is setup, all properties below should be in the specified space
+	EConstraintSpace			mSpace = EConstraintSpace::WorldSpace;
+
+	/// Body 1 constraint reference frame (space determined by mSpace).
+	Vec3						mHingeAxis1 = Vec3::sAxisX();
+
+	/// Body 2 constraint reference frame (space determined by mSpace)
+	Vec3						mHingeAxis2 = Vec3::sAxisX();
+
+	/// Ratio between both gears, see SetRatio.
+	float						mRatio = 1.0f;
+
+protected:
+	// See: ConstraintSettings::RestoreBinaryState
+	virtual void				RestoreBinaryState(StreamIn &inStream) override;
+};
+
+/// A gear constraint constrains the rotation of body1 to the rotation of body 2 using a gear.
+/// Note that this constraint needs to be used in conjunction with a two hinge constraints.
+class JPH_EXPORT GearConstraint final : public TwoBodyConstraint
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Construct gear constraint
+								GearConstraint(Body &inBody1, Body &inBody2, const GearConstraintSettings &inSettings);
+
+	// Generic interface of a constraint
+	virtual EConstraintSubType	GetSubType() const override								{ return EConstraintSubType::Gear; }
+	virtual void				NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override { /* Do nothing */ }
+	virtual void				SetupVelocityConstraint(float inDeltaTime) override;
+	virtual void				ResetWarmStart() override;
+	virtual void				WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override;
+	virtual bool				SolveVelocityConstraint(float inDeltaTime) override;
+	virtual bool				SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override;
+#ifdef JPH_DEBUG_RENDERER
+	virtual void				DrawConstraint(DebugRenderer *inRenderer) const override;
+#endif // JPH_DEBUG_RENDERER
+	virtual void				SaveState(StateRecorder &inStream) const override;
+	virtual void				RestoreState(StateRecorder &inStream) override;
+	virtual Ref<ConstraintSettings> GetConstraintSettings() const override;
+
+	// See: TwoBodyConstraint
+	virtual Mat44				GetConstraintToBody1Matrix() const override;
+	virtual Mat44				GetConstraintToBody2Matrix() const override;
+
+	/// The constraints that constrain both gears (2 hinges), optional and used to calculate the rotation error and fix numerical drift.
+	void						SetConstraints(const Constraint *inGear1, const Constraint *inGear2)	{ mGear1Constraint = inGear1; mGear2Constraint = inGear2; }
+
+	///@name Get Lagrange multiplier from last physics update (the angular impulse applied to satisfy the constraint)
+	inline float				GetTotalLambda() const									{ return mGearConstraintPart.GetTotalLambda(); }
+
+private:
+	// Internal helper function to calculate the values below
+	void						CalculateConstraintProperties(Mat44Arg inRotation1, Mat44Arg inRotation2);
+
+	// CONFIGURATION PROPERTIES FOLLOW
+
+	// Local space hinge axis for body 1
+	Vec3						mLocalSpaceHingeAxis1;
+
+	// Local space hinge axis for body 2
+	Vec3						mLocalSpaceHingeAxis2;
+
+	// Ratio between gear 1 and 2
+	float						mRatio;
+
+	// The constraints that constrain both gears (2 hinges), optional and used to calculate the rotation error and fix numerical drift.
+	RefConst<Constraint>		mGear1Constraint;
+	RefConst<Constraint>		mGear2Constraint;
+
+	// RUN TIME PROPERTIES FOLLOW
+
+	// World space hinge axis for body 1
+	Vec3						mWorldSpaceHingeAxis1;
+
+	// World space hinge axis for body 2
+	Vec3						mWorldSpaceHingeAxis2;
+
+	// The constraint parts
+	GearConstraintPart			mGearConstraintPart;
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Constraints/HingeConstraint.cpp

@@ -0,0 +1,424 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Constraints/HingeConstraint.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/RotationEulerConstraintPart.h>
+#include <Jolt/Physics/Body/Body.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(HingeConstraintSettings)
+{
+	JPH_ADD_BASE_CLASS(HingeConstraintSettings, TwoBodyConstraintSettings)
+
+	JPH_ADD_ENUM_ATTRIBUTE(HingeConstraintSettings, mSpace)
+	JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mPoint1)
+	JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mHingeAxis1)
+	JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mNormalAxis1)
+	JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mPoint2)
+	JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mHingeAxis2)
+	JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mNormalAxis2)
+	JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mLimitsMin)
+	JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mLimitsMax)
+	JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mLimitsSpringSettings)
+	JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mMaxFrictionTorque)
+	JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mMotorSettings)
+}
+
+void HingeConstraintSettings::SaveBinaryState(StreamOut &inStream) const
+{
+	ConstraintSettings::SaveBinaryState(inStream);
+
+	inStream.Write(mSpace);
+	inStream.Write(mPoint1);
+	inStream.Write(mHingeAxis1);
+	inStream.Write(mNormalAxis1);
+	inStream.Write(mPoint2);
+	inStream.Write(mHingeAxis2);
+	inStream.Write(mNormalAxis2);
+	inStream.Write(mLimitsMin);
+	inStream.Write(mLimitsMax);
+	inStream.Write(mMaxFrictionTorque);
+	mLimitsSpringSettings.SaveBinaryState(inStream);
+	mMotorSettings.SaveBinaryState(inStream);
+}
+
+void HingeConstraintSettings::RestoreBinaryState(StreamIn &inStream)
+{
+	ConstraintSettings::RestoreBinaryState(inStream);
+
+	inStream.Read(mSpace);
+	inStream.Read(mPoint1);
+	inStream.Read(mHingeAxis1);
+	inStream.Read(mNormalAxis1);
+	inStream.Read(mPoint2);
+	inStream.Read(mHingeAxis2);
+	inStream.Read(mNormalAxis2);
+	inStream.Read(mLimitsMin);
+	inStream.Read(mLimitsMax);
+	inStream.Read(mMaxFrictionTorque);
+	mLimitsSpringSettings.RestoreBinaryState(inStream);
+	mMotorSettings.RestoreBinaryState(inStream);}
+
+TwoBodyConstraint *HingeConstraintSettings::Create(Body &inBody1, Body &inBody2) const
+{
+	return new HingeConstraint(inBody1, inBody2, *this);
+}
+
+HingeConstraint::HingeConstraint(Body &inBody1, Body &inBody2, const HingeConstraintSettings &inSettings) :
+	TwoBodyConstraint(inBody1, inBody2, inSettings),
+	mMaxFrictionTorque(inSettings.mMaxFrictionTorque),
+	mMotorSettings(inSettings.mMotorSettings)
+{
+	// Store limits
+	JPH_ASSERT(inSettings.mLimitsMin != inSettings.mLimitsMax || inSettings.mLimitsSpringSettings.mFrequency > 0.0f, "Better use a fixed constraint in this case");
+	SetLimits(inSettings.mLimitsMin, inSettings.mLimitsMax);
+
+	// Store inverse of initial rotation from body 1 to body 2 in body 1 space
+	mInvInitialOrientation = RotationEulerConstraintPart::sGetInvInitialOrientationXZ(inSettings.mNormalAxis1, inSettings.mHingeAxis1, inSettings.mNormalAxis2, inSettings.mHingeAxis2);
+
+	if (inSettings.mSpace == EConstraintSpace::WorldSpace)
+	{
+		// If all properties were specified in world space, take them to local space now
+		RMat44 inv_transform1 = inBody1.GetInverseCenterOfMassTransform();
+		mLocalSpacePosition1 = Vec3(inv_transform1 * inSettings.mPoint1);
+		mLocalSpaceHingeAxis1 = inv_transform1.Multiply3x3(inSettings.mHingeAxis1).Normalized();
+		mLocalSpaceNormalAxis1 = inv_transform1.Multiply3x3(inSettings.mNormalAxis1).Normalized();
+
+		RMat44 inv_transform2 = inBody2.GetInverseCenterOfMassTransform();
+		mLocalSpacePosition2 = Vec3(inv_transform2 * inSettings.mPoint2);
+		mLocalSpaceHingeAxis2 = inv_transform2.Multiply3x3(inSettings.mHingeAxis2).Normalized();
+		mLocalSpaceNormalAxis2 = inv_transform2.Multiply3x3(inSettings.mNormalAxis2).Normalized();
+
+		// Constraints were specified in world space, so we should have replaced c1 with q10^-1 c1 and c2 with q20^-1 c2
+		// => r0^-1 = (q20^-1 c2) (q10^-1 c1)^1 = q20^-1 (c2 c1^-1) q10
+		mInvInitialOrientation = inBody2.GetRotation().Conjugated() * mInvInitialOrientation * inBody1.GetRotation();
+	}
+	else
+	{
+		mLocalSpacePosition1 = Vec3(inSettings.mPoint1);
+		mLocalSpaceHingeAxis1 = inSettings.mHingeAxis1;
+		mLocalSpaceNormalAxis1 = inSettings.mNormalAxis1;
+
+		mLocalSpacePosition2 = Vec3(inSettings.mPoint2);
+		mLocalSpaceHingeAxis2 = inSettings.mHingeAxis2;
+		mLocalSpaceNormalAxis2 = inSettings.mNormalAxis2;
+	}
+
+	// Store spring settings
+	SetLimitsSpringSettings(inSettings.mLimitsSpringSettings);
+}
+
+void HingeConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM)
+{
+	if (mBody1->GetID() == inBodyID)
+		mLocalSpacePosition1 -= inDeltaCOM;
+	else if (mBody2->GetID() == inBodyID)
+		mLocalSpacePosition2 -= inDeltaCOM;
+}
+
+float HingeConstraint::GetCurrentAngle() const
+{
+	// See: CalculateA1AndTheta
+	Quat rotation1 = mBody1->GetRotation();
+	Quat diff = mBody2->GetRotation() * mInvInitialOrientation * rotation1.Conjugated();
+	return diff.GetRotationAngle(rotation1 * mLocalSpaceHingeAxis1);
+}
+
+void HingeConstraint::SetLimits(float inLimitsMin, float inLimitsMax)
+{
+	JPH_ASSERT(inLimitsMin <= 0.0f && inLimitsMin >= -JPH_PI);
+	JPH_ASSERT(inLimitsMax >= 0.0f && inLimitsMax <= JPH_PI);
+	mLimitsMin = inLimitsMin;
+	mLimitsMax = inLimitsMax;
+	mHasLimits = mLimitsMin > -JPH_PI || mLimitsMax < JPH_PI;
+}
+
+void HingeConstraint::CalculateA1AndTheta()
+{
+	if (mHasLimits || mMotorState != EMotorState::Off || mMaxFrictionTorque > 0.0f)
+	{
+		Quat rotation1 = mBody1->GetRotation();
+
+		// Calculate relative rotation in world space
+		//
+		// The rest rotation is:
+		//
+		// q2 = q1 r0
+		//
+		// But the actual rotation is
+		//
+		// q2 = diff q1 r0
+		// <=> diff = q2 r0^-1 q1^-1
+		//
+		// Where:
+		// q1 = current rotation of body 1
+		// q2 = current rotation of body 2
+		// diff = relative rotation in world space
+		Quat diff = mBody2->GetRotation() * mInvInitialOrientation * rotation1.Conjugated();
+
+		// Calculate hinge axis in world space
+		mA1 = rotation1 * mLocalSpaceHingeAxis1;
+
+		// Get rotation angle around the hinge axis
+		mTheta = diff.GetRotationAngle(mA1);
+	}
+}
+
+void HingeConstraint::CalculateRotationLimitsConstraintProperties(float inDeltaTime)
+{
+	// Apply constraint if outside of limits
+	if (mHasLimits && (mTheta <= mLimitsMin || mTheta >= mLimitsMax))
+		mRotationLimitsConstraintPart.CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, *mBody2, mA1, 0.0f, GetSmallestAngleToLimit(), mLimitsSpringSettings);
+	else
+		mRotationLimitsConstraintPart.Deactivate();
+}
+
+void HingeConstraint::CalculateMotorConstraintProperties(float inDeltaTime)
+{
+	switch (mMotorState)
+	{
+	case EMotorState::Off:
+		if (mMaxFrictionTorque > 0.0f)
+			mMotorConstraintPart.CalculateConstraintProperties(*mBody1, *mBody2, mA1);
+		else
+			mMotorConstraintPart.Deactivate();
+		break;
+
+	case EMotorState::Velocity:
+		mMotorConstraintPart.CalculateConstraintProperties(*mBody1, *mBody2, mA1, -mTargetAngularVelocity);
+		break;
+
+	case EMotorState::Position:
+		if (mMotorSettings.mSpringSettings.HasStiffness())
+			mMotorConstraintPart.CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, *mBody2, mA1, 0.0f, CenterAngleAroundZero(mTheta - mTargetAngle), mMotorSettings.mSpringSettings);
+		else
+			mMotorConstraintPart.Deactivate();
+		break;
+	}
+}
+
+void HingeConstraint::SetupVelocityConstraint(float inDeltaTime)
+{
+	// Cache constraint values that are valid until the bodies move
+	Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation());
+	Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation());
+	mPointConstraintPart.CalculateConstraintProperties(*mBody1, rotation1, mLocalSpacePosition1, *mBody2, rotation2, mLocalSpacePosition2);
+	mRotationConstraintPart.CalculateConstraintProperties(*mBody1, rotation1, rotation1.Multiply3x3(mLocalSpaceHingeAxis1), *mBody2, rotation2, rotation2.Multiply3x3(mLocalSpaceHingeAxis2));
+	CalculateA1AndTheta();
+	CalculateRotationLimitsConstraintProperties(inDeltaTime);
+	CalculateMotorConstraintProperties(inDeltaTime);
+}
+
+void HingeConstraint::ResetWarmStart()
+{
+	mMotorConstraintPart.Deactivate();
+	mPointConstraintPart.Deactivate();
+	mRotationConstraintPart.Deactivate();
+	mRotationLimitsConstraintPart.Deactivate();
+}
+
+void HingeConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio)
+{
+	// Warm starting: Apply previous frame impulse
+	mMotorConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio);
+	mPointConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio);
+	mRotationConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio);
+	mRotationLimitsConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio);
+}
+
+float HingeConstraint::GetSmallestAngleToLimit() const
+{
+	float dist_to_min = CenterAngleAroundZero(mTheta - mLimitsMin);
+	float dist_to_max = CenterAngleAroundZero(mTheta - mLimitsMax);
+	return abs(dist_to_min) < abs(dist_to_max)? dist_to_min : dist_to_max;
+}
+
+bool HingeConstraint::IsMinLimitClosest() const
+{
+	float dist_to_min = CenterAngleAroundZero(mTheta - mLimitsMin);
+	float dist_to_max = CenterAngleAroundZero(mTheta - mLimitsMax);
+	return abs(dist_to_min) < abs(dist_to_max);
+}
+
+bool HingeConstraint::SolveVelocityConstraint(float inDeltaTime)
+{
+	// Solve motor
+	bool motor = false;
+	if (mMotorConstraintPart.IsActive())
+	{
+		switch (mMotorState)
+		{
+		case EMotorState::Off:
+			{
+				float max_lambda = mMaxFrictionTorque * inDeltaTime;
+				motor = mMotorConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mA1, -max_lambda, max_lambda);
+				break;
+			}
+
+		case EMotorState::Velocity:
+		case EMotorState::Position:
+			motor = mMotorConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mA1, inDeltaTime * mMotorSettings.mMinTorqueLimit, inDeltaTime * mMotorSettings.mMaxTorqueLimit);
+			break;
+		}
+	}
+
+	// Solve point constraint
+	bool pos = mPointConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2);
+
+	// Solve rotation constraint
+	bool rot = mRotationConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2);
+
+	// Solve rotation limits
+	bool limit = false;
+	if (mRotationLimitsConstraintPart.IsActive())
+	{
+		float min_lambda, max_lambda;
+		if (mLimitsMin == mLimitsMax)
+		{
+			min_lambda = -FLT_MAX;
+			max_lambda = FLT_MAX;
+		}
+		else if (IsMinLimitClosest())
+		{
+			min_lambda = 0.0f;
+			max_lambda = FLT_MAX;
+		}
+		else
+		{
+			min_lambda = -FLT_MAX;
+			max_lambda = 0.0f;
+		}
+		limit = mRotationLimitsConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mA1, min_lambda, max_lambda);
+	}
+
+	return motor || pos || rot || limit;
+}
+
+bool HingeConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte)
+{
+	// Motor operates on velocities only, don't call SolvePositionConstraint
+
+	// Solve point constraint
+	mPointConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(mBody1->GetRotation()), mLocalSpacePosition1, *mBody2, Mat44::sRotation(mBody2->GetRotation()), mLocalSpacePosition2);
+	bool pos = mPointConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, inBaumgarte);
+
+	// Solve rotation constraint
+	Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation()); // Note that previous call to GetRotation() is out of date since the rotation has changed
+	Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation());
+	mRotationConstraintPart.CalculateConstraintProperties(*mBody1, rotation1, rotation1.Multiply3x3(mLocalSpaceHingeAxis1), *mBody2, rotation2, rotation2.Multiply3x3(mLocalSpaceHingeAxis2));
+	bool rot = mRotationConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, inBaumgarte);
+
+	// Solve rotation limits
+	bool limit = false;
+	if (mHasLimits && mLimitsSpringSettings.mFrequency <= 0.0f)
+	{
+		CalculateA1AndTheta();
+		CalculateRotationLimitsConstraintProperties(inDeltaTime);
+		if (mRotationLimitsConstraintPart.IsActive())
+			limit = mRotationLimitsConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, GetSmallestAngleToLimit(), inBaumgarte);
+	}
+
+	return pos || rot || limit;
+}
+
+#ifdef JPH_DEBUG_RENDERER
+void HingeConstraint::DrawConstraint(DebugRenderer *inRenderer) const
+{
+	RMat44 transform1 = mBody1->GetCenterOfMassTransform();
+	RMat44 transform2 = mBody2->GetCenterOfMassTransform();
+
+	// Draw constraint
+	RVec3 constraint_pos1 = transform1 * mLocalSpacePosition1;
+	inRenderer->DrawMarker(constraint_pos1, Color::sRed, 0.1f);
+	inRenderer->DrawLine(constraint_pos1, transform1 * (mLocalSpacePosition1 + mDrawConstraintSize * mLocalSpaceHingeAxis1), Color::sRed);
+
+	RVec3 constraint_pos2 = transform2 * mLocalSpacePosition2;
+	inRenderer->DrawMarker(constraint_pos2, Color::sGreen, 0.1f);
+	inRenderer->DrawLine(constraint_pos2, transform2 * (mLocalSpacePosition2 + mDrawConstraintSize * mLocalSpaceHingeAxis2), Color::sGreen);
+	inRenderer->DrawLine(constraint_pos2, transform2 * (mLocalSpacePosition2 + mDrawConstraintSize * mLocalSpaceNormalAxis2), Color::sWhite);
+}
+
+void HingeConstraint::DrawConstraintLimits(DebugRenderer *inRenderer) const
+{
+	if (mHasLimits && mLimitsMax > mLimitsMin)
+	{
+		// Get constraint properties in world space
+		RMat44 transform1 = mBody1->GetCenterOfMassTransform();
+		RVec3 position1 = transform1 * mLocalSpacePosition1;
+		Vec3 hinge_axis1 = transform1.Multiply3x3(mLocalSpaceHingeAxis1);
+		Vec3 normal_axis1 = transform1.Multiply3x3(mLocalSpaceNormalAxis1);
+
+		inRenderer->DrawPie(position1, mDrawConstraintSize, hinge_axis1, normal_axis1, mLimitsMin, mLimitsMax, Color::sPurple, DebugRenderer::ECastShadow::Off);
+	}
+}
+#endif // JPH_DEBUG_RENDERER
+
+void HingeConstraint::SaveState(StateRecorder &inStream) const
+{
+	TwoBodyConstraint::SaveState(inStream);
+
+	mMotorConstraintPart.SaveState(inStream);
+	mRotationConstraintPart.SaveState(inStream);
+	mPointConstraintPart.SaveState(inStream);
+	mRotationLimitsConstraintPart.SaveState(inStream);
+
+	inStream.Write(mMotorState);
+	inStream.Write(mTargetAngularVelocity);
+	inStream.Write(mTargetAngle);
+}
+
+void HingeConstraint::RestoreState(StateRecorder &inStream)
+{
+	TwoBodyConstraint::RestoreState(inStream);
+
+	mMotorConstraintPart.RestoreState(inStream);
+	mRotationConstraintPart.RestoreState(inStream);
+	mPointConstraintPart.RestoreState(inStream);
+	mRotationLimitsConstraintPart.RestoreState(inStream);
+
+	inStream.Read(mMotorState);
+	inStream.Read(mTargetAngularVelocity);
+	inStream.Read(mTargetAngle);
+}
+
+
+Ref<ConstraintSettings> HingeConstraint::GetConstraintSettings() const
+{
+	HingeConstraintSettings *settings = new HingeConstraintSettings;
+	ToConstraintSettings(*settings);
+	settings->mSpace = EConstraintSpace::LocalToBodyCOM;
+	settings->mPoint1 = RVec3(mLocalSpacePosition1);
+	settings->mHingeAxis1 = mLocalSpaceHingeAxis1;
+	settings->mNormalAxis1 = mLocalSpaceNormalAxis1;
+	settings->mPoint2 = RVec3(mLocalSpacePosition2);
+	settings->mHingeAxis2 = mLocalSpaceHingeAxis2;
+	settings->mNormalAxis2 = mLocalSpaceNormalAxis2;
+	settings->mLimitsMin = mLimitsMin;
+	settings->mLimitsMax = mLimitsMax;
+	settings->mLimitsSpringSettings = mLimitsSpringSettings;
+	settings->mMaxFrictionTorque = mMaxFrictionTorque;
+	settings->mMotorSettings = mMotorSettings;
+	return settings;
+}
+
+Mat44 HingeConstraint::GetConstraintToBody1Matrix() const
+{
+	return Mat44(Vec4(mLocalSpaceHingeAxis1, 0), Vec4(mLocalSpaceNormalAxis1, 0), Vec4(mLocalSpaceHingeAxis1.Cross(mLocalSpaceNormalAxis1), 0), Vec4(mLocalSpacePosition1, 1));
+}
+
+Mat44 HingeConstraint::GetConstraintToBody2Matrix() const
+{
+	return Mat44(Vec4(mLocalSpaceHingeAxis2, 0), Vec4(mLocalSpaceNormalAxis2, 0), Vec4(mLocalSpaceHingeAxis2.Cross(mLocalSpaceNormalAxis2), 0), Vec4(mLocalSpacePosition2, 1));
+}
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Constraints/HingeConstraint.h

@@ -0,0 +1,200 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Constraints/TwoBodyConstraint.h>
+#include <Jolt/Physics/Constraints/MotorSettings.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/PointConstraintPart.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/HingeRotationConstraintPart.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/AngleConstraintPart.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Hinge constraint settings, used to create a hinge constraint
+class JPH_EXPORT HingeConstraintSettings final : public TwoBodyConstraintSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, HingeConstraintSettings)
+
+public:
+	// See: ConstraintSettings::SaveBinaryState
+	virtual void				SaveBinaryState(StreamOut &inStream) const override;
+
+	/// Create an instance of this constraint
+	virtual TwoBodyConstraint *	Create(Body &inBody1, Body &inBody2) const override;
+
+	/// This determines in which space the constraint is setup, all properties below should be in the specified space
+	EConstraintSpace			mSpace = EConstraintSpace::WorldSpace;
+
+	/// Body 1 constraint reference frame (space determined by mSpace).
+	/// Hinge axis is the axis where rotation is allowed.
+	/// When the normal axis of both bodies align in world space, the hinge angle is defined to be 0.
+	/// mHingeAxis1 and mNormalAxis1 should be perpendicular. mHingeAxis2 and mNormalAxis2 should also be perpendicular.
+	/// If you configure the joint in world space and create both bodies with a relative rotation you want to be defined as zero,
+	/// you can simply set mHingeAxis1 = mHingeAxis2 and mNormalAxis1 = mNormalAxis2.
+	RVec3						mPoint1 = RVec3::sZero();
+	Vec3						mHingeAxis1 = Vec3::sAxisY();
+	Vec3						mNormalAxis1 = Vec3::sAxisX();
+
+	/// Body 2 constraint reference frame (space determined by mSpace)
+	RVec3						mPoint2 = RVec3::sZero();
+	Vec3						mHingeAxis2 = Vec3::sAxisY();
+	Vec3						mNormalAxis2 = Vec3::sAxisX();
+
+	/// Rotation around the hinge axis will be limited between [mLimitsMin, mLimitsMax] where mLimitsMin e [-pi, 0] and mLimitsMax e [0, pi].
+	/// Both angles are in radians.
+	float						mLimitsMin = -JPH_PI;
+	float						mLimitsMax = JPH_PI;
+
+	/// When enabled, this makes the limits soft. When the constraint exceeds the limits, a spring force will pull it back.
+	SpringSettings				mLimitsSpringSettings;
+
+	/// Maximum amount of torque (N m) to apply as friction when the constraint is not powered by a motor
+	float						mMaxFrictionTorque = 0.0f;
+
+	/// In case the constraint is powered, this determines the motor settings around the hinge axis
+	MotorSettings				mMotorSettings;
+
+protected:
+	// See: ConstraintSettings::RestoreBinaryState
+	virtual void				RestoreBinaryState(StreamIn &inStream) override;
+};
+
+/// A hinge constraint constrains 2 bodies on a single point and allows only a single axis of rotation
+class JPH_EXPORT HingeConstraint final : public TwoBodyConstraint
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Construct hinge constraint
+								HingeConstraint(Body &inBody1, Body &inBody2, const HingeConstraintSettings &inSettings);
+
+	// Generic interface of a constraint
+	virtual EConstraintSubType	GetSubType() const override								{ return EConstraintSubType::Hinge; }
+	virtual void				NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override;
+	virtual void				SetupVelocityConstraint(float inDeltaTime) override;
+	virtual void				ResetWarmStart() override;
+	virtual void				WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override;
+	virtual bool				SolveVelocityConstraint(float inDeltaTime) override;
+	virtual bool				SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override;
+#ifdef JPH_DEBUG_RENDERER
+	virtual void				DrawConstraint(DebugRenderer *inRenderer) const override;
+	virtual void				DrawConstraintLimits(DebugRenderer *inRenderer) const override;
+#endif // JPH_DEBUG_RENDERER
+	virtual void				SaveState(StateRecorder &inStream) const override;
+	virtual void				RestoreState(StateRecorder &inStream) override;
+	virtual Ref<ConstraintSettings> GetConstraintSettings() const override;
+
+	// See: TwoBodyConstraint
+	virtual Mat44				GetConstraintToBody1Matrix() const override;
+	virtual Mat44				GetConstraintToBody2Matrix() const override;
+
+	/// Get the attachment point for body 1 relative to body 1 COM (transform by Body::GetCenterOfMassTransform to take to world space)
+	inline Vec3					GetLocalSpacePoint1() const								{ return mLocalSpacePosition1; }
+
+	/// Get the attachment point for body 2 relative to body 2 COM (transform by Body::GetCenterOfMassTransform to take to world space)
+	inline Vec3					GetLocalSpacePoint2() const								{ return mLocalSpacePosition2; }
+
+	// Local space hinge directions (transform direction by Body::GetCenterOfMassTransform to take to world space)
+	Vec3						GetLocalSpaceHingeAxis1() const							{ return mLocalSpaceHingeAxis1; }
+	Vec3						GetLocalSpaceHingeAxis2() const							{ return mLocalSpaceHingeAxis2; }
+
+	// Local space normal directions (transform direction by Body::GetCenterOfMassTransform to take to world space)
+	Vec3						GetLocalSpaceNormalAxis1() const						{ return mLocalSpaceNormalAxis1; }
+	Vec3						GetLocalSpaceNormalAxis2() const						{ return mLocalSpaceNormalAxis2; }
+
+	/// Get the current rotation angle from the rest position
+	float						GetCurrentAngle() const;
+
+	// Friction control
+	void						SetMaxFrictionTorque(float inFrictionTorque)			{ mMaxFrictionTorque = inFrictionTorque; }
+	float						GetMaxFrictionTorque() const							{ return mMaxFrictionTorque; }
+
+	// Motor settings
+	MotorSettings &				GetMotorSettings()										{ return mMotorSettings; }
+	const MotorSettings &		GetMotorSettings() const								{ return mMotorSettings; }
+
+	// Motor controls
+	void						SetMotorState(EMotorState inState)						{ JPH_ASSERT(inState == EMotorState::Off || mMotorSettings.IsValid()); mMotorState = inState; }
+	EMotorState					GetMotorState() const									{ return mMotorState; }
+	void						SetTargetAngularVelocity(float inAngularVelocity)		{ mTargetAngularVelocity = inAngularVelocity; } ///< rad/s
+	float						GetTargetAngularVelocity() const						{ return mTargetAngularVelocity; }
+	void						SetTargetAngle(float inAngle)							{ mTargetAngle = mHasLimits? Clamp(inAngle, mLimitsMin, mLimitsMax) : inAngle; } ///< rad
+	float						GetTargetAngle() const									{ return mTargetAngle; }
+
+	/// Update the rotation limits of the hinge, value in radians (see HingeConstraintSettings)
+	void						SetLimits(float inLimitsMin, float inLimitsMax);
+	float						GetLimitsMin() const									{ return mLimitsMin; }
+	float						GetLimitsMax() const									{ return mLimitsMax; }
+	bool						HasLimits() const										{ return mHasLimits; }
+
+	/// Update the limits spring settings
+	const SpringSettings &		GetLimitsSpringSettings() const							{ return mLimitsSpringSettings; }
+	SpringSettings &			GetLimitsSpringSettings()								{ return mLimitsSpringSettings; }
+	void						SetLimitsSpringSettings(const SpringSettings &inLimitsSpringSettings) { mLimitsSpringSettings = inLimitsSpringSettings; }
+
+	///@name Get Lagrange multiplier from last physics update (the linear/angular impulse applied to satisfy the constraint)
+	inline Vec3					GetTotalLambdaPosition() const							{ return mPointConstraintPart.GetTotalLambda(); }
+	inline Vector<2>			GetTotalLambdaRotation() const							{ return mRotationConstraintPart.GetTotalLambda(); }
+	inline float				GetTotalLambdaRotationLimits() const					{ return mRotationLimitsConstraintPart.GetTotalLambda(); }
+	inline float				GetTotalLambdaMotor() const								{ return mMotorConstraintPart.GetTotalLambda(); }
+
+private:
+	// Internal helper function to calculate the values below
+	void						CalculateA1AndTheta();
+	void						CalculateRotationLimitsConstraintProperties(float inDeltaTime);
+	void						CalculateMotorConstraintProperties(float inDeltaTime);
+	inline float				GetSmallestAngleToLimit() const;
+	inline bool					IsMinLimitClosest() const;
+
+	// CONFIGURATION PROPERTIES FOLLOW
+
+	// Local space constraint positions
+	Vec3						mLocalSpacePosition1;
+	Vec3						mLocalSpacePosition2;
+
+	// Local space hinge directions
+	Vec3						mLocalSpaceHingeAxis1;
+	Vec3						mLocalSpaceHingeAxis2;
+
+	// Local space normal direction (direction relative to which to draw constraint limits)
+	Vec3						mLocalSpaceNormalAxis1;
+	Vec3						mLocalSpaceNormalAxis2;
+
+	// Inverse of initial relative orientation between bodies (which defines hinge angle = 0)
+	Quat						mInvInitialOrientation;
+
+	// Hinge limits
+	bool						mHasLimits;
+	float						mLimitsMin;
+	float						mLimitsMax;
+
+	// Soft constraint limits
+	SpringSettings				mLimitsSpringSettings;
+
+	// Friction
+	float						mMaxFrictionTorque;
+
+	// Motor controls
+	MotorSettings				mMotorSettings;
+	EMotorState					mMotorState = EMotorState::Off;
+	float						mTargetAngularVelocity = 0.0f;
+	float						mTargetAngle = 0.0f;
+
+	// RUN TIME PROPERTIES FOLLOW
+
+	// Current rotation around the hinge axis
+	float						mTheta = 0.0f;
+
+	// World space hinge axis for body 1
+	Vec3						mA1;
+
+	// The constraint parts
+	PointConstraintPart			mPointConstraintPart;
+	HingeRotationConstraintPart mRotationConstraintPart;
+	AngleConstraintPart			mRotationLimitsConstraintPart;
+	AngleConstraintPart			mMotorConstraintPart;
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Constraints/MotorSettings.cpp

@@ -0,0 +1,43 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Constraints/MotorSettings.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(MotorSettings)
+{
+	JPH_ADD_ENUM_ATTRIBUTE_WITH_ALIAS(MotorSettings, mSpringSettings.mMode, "mSpringMode")
+	JPH_ADD_ATTRIBUTE_WITH_ALIAS(MotorSettings, mSpringSettings.mFrequency, "mFrequency") // Renaming attributes to stay compatible with old versions of the library
+	JPH_ADD_ATTRIBUTE_WITH_ALIAS(MotorSettings, mSpringSettings.mDamping, "mDamping")
+	JPH_ADD_ATTRIBUTE(MotorSettings, mMinForceLimit)
+	JPH_ADD_ATTRIBUTE(MotorSettings, mMaxForceLimit)
+	JPH_ADD_ATTRIBUTE(MotorSettings, mMinTorqueLimit)
+	JPH_ADD_ATTRIBUTE(MotorSettings, mMaxTorqueLimit)
+}
+
+void MotorSettings::SaveBinaryState(StreamOut &inStream) const
+{
+	mSpringSettings.SaveBinaryState(inStream);
+	inStream.Write(mMinForceLimit);
+	inStream.Write(mMaxForceLimit);
+	inStream.Write(mMinTorqueLimit);
+	inStream.Write(mMaxTorqueLimit);
+}
+
+void MotorSettings::RestoreBinaryState(StreamIn &inStream)
+{
+	mSpringSettings.RestoreBinaryState(inStream);
+	inStream.Read(mMinForceLimit);
+	inStream.Read(mMaxForceLimit);
+	inStream.Read(mMinTorqueLimit);
+	inStream.Read(mMaxTorqueLimit);
+}
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Constraints/MotorSettings.h

@@ -0,0 +1,66 @@
+// 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/ObjectStream/SerializableObject.h>
+#include <Jolt/Physics/Constraints/SpringSettings.h>
+
+JPH_NAMESPACE_BEGIN
+
+class StreamIn;
+class StreamOut;
+
+enum class EMotorState
+{
+	Off,																///< Motor is off
+	Velocity,															///< Motor will drive to target velocity
+	Position															///< Motor will drive to target position
+};
+
+/// Class that contains the settings for a constraint motor.
+/// See the main page of the API documentation for more information on how to configure a motor.
+class JPH_EXPORT MotorSettings
+{
+	JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, MotorSettings)
+
+public:
+	/// Constructor
+							MotorSettings() = default;
+							MotorSettings(const MotorSettings &) = default;
+	MotorSettings &			operator = (const MotorSettings &) = default;
+							MotorSettings(float inFrequency, float inDamping) : mSpringSettings(ESpringMode::FrequencyAndDamping, inFrequency, inDamping) { JPH_ASSERT(IsValid()); }
+							MotorSettings(float inFrequency, float inDamping, float inForceLimit, float inTorqueLimit) : mSpringSettings(ESpringMode::FrequencyAndDamping, inFrequency, inDamping), mMinForceLimit(-inForceLimit), mMaxForceLimit(inForceLimit), mMinTorqueLimit(-inTorqueLimit), mMaxTorqueLimit(inTorqueLimit) { JPH_ASSERT(IsValid()); }
+
+	/// Set asymmetric force limits
+	void					SetForceLimits(float inMin, float inMax)	{ JPH_ASSERT(inMin <= inMax); mMinForceLimit = inMin; mMaxForceLimit = inMax; }
+
+	/// Set asymmetric torque limits
+	void					SetTorqueLimits(float inMin, float inMax)	{ JPH_ASSERT(inMin <= inMax); mMinTorqueLimit = inMin; mMaxTorqueLimit = inMax; }
+
+	/// Set symmetric force limits
+	void					SetForceLimit(float inLimit)				{ mMinForceLimit = -inLimit; mMaxForceLimit = inLimit; }
+
+	/// Set symmetric torque limits
+	void					SetTorqueLimit(float inLimit)				{ mMinTorqueLimit = -inLimit; mMaxTorqueLimit = inLimit; }
+
+	/// Check if settings are valid
+	bool					IsValid() const								{ return mSpringSettings.mFrequency >= 0.0f && mSpringSettings.mDamping >= 0.0f && mMinForceLimit <= mMaxForceLimit && mMinTorqueLimit <= mMaxTorqueLimit; }
+
+	/// Saves the contents of the motor settings in binary form to inStream.
+	void					SaveBinaryState(StreamOut &inStream) const;
+
+	/// Restores contents from the binary stream inStream.
+	void					RestoreBinaryState(StreamIn &inStream);
+
+	// Settings
+	SpringSettings			mSpringSettings { ESpringMode::FrequencyAndDamping, 2.0f, 1.0f }; ///< Settings for the spring that is used to drive to the position target (not used when motor is a velocity motor).
+	float					mMinForceLimit = -FLT_MAX;					///< Minimum force to apply in case of a linear constraint (N). Usually this is -mMaxForceLimit unless you want a motor that can e.g. push but not pull. Not used when motor is an angular motor.
+	float					mMaxForceLimit = FLT_MAX;					///< Maximum force to apply in case of a linear constraint (N). Not used when motor is an angular motor.
+	float					mMinTorqueLimit = -FLT_MAX;					///< Minimum torque to apply in case of a angular constraint (N m). Usually this is -mMaxTorqueLimit unless you want a motor that can e.g. push but not pull. Not used when motor is a position motor.
+	float					mMaxTorqueLimit = FLT_MAX;					///< Maximum torque to apply in case of a angular constraint (N m). Not used when motor is a position motor.
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraint.cpp

@@ -0,0 +1,458 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Constraints/PathConstraint.h>
+#include <Jolt/Physics/Body/Body.h>
+#include <Jolt/Core/StringTools.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(PathConstraintSettings)
+{
+	JPH_ADD_BASE_CLASS(PathConstraintSettings, TwoBodyConstraintSettings)
+
+	JPH_ADD_ATTRIBUTE(PathConstraintSettings, mPath)
+	JPH_ADD_ATTRIBUTE(PathConstraintSettings, mPathPosition)
+	JPH_ADD_ATTRIBUTE(PathConstraintSettings, mPathRotation)
+	JPH_ADD_ATTRIBUTE(PathConstraintSettings, mPathFraction)
+	JPH_ADD_ATTRIBUTE(PathConstraintSettings, mMaxFrictionForce)
+	JPH_ADD_ATTRIBUTE(PathConstraintSettings, mPositionMotorSettings)
+	JPH_ADD_ENUM_ATTRIBUTE(PathConstraintSettings, mRotationConstraintType)
+}
+
+void PathConstraintSettings::SaveBinaryState(StreamOut &inStream) const
+{
+	ConstraintSettings::SaveBinaryState(inStream);
+
+	mPath->SaveBinaryState(inStream);
+	inStream.Write(mPathPosition);
+	inStream.Write(mPathRotation);
+	inStream.Write(mPathFraction);
+	inStream.Write(mMaxFrictionForce);
+	inStream.Write(mRotationConstraintType);
+	mPositionMotorSettings.SaveBinaryState(inStream);
+}
+
+void PathConstraintSettings::RestoreBinaryState(StreamIn &inStream)
+{
+	ConstraintSettings::RestoreBinaryState(inStream);
+
+	PathConstraintPath::PathResult result = PathConstraintPath::sRestoreFromBinaryState(inStream);
+	if (!result.HasError())
+		mPath = result.Get();
+	inStream.Read(mPathPosition);
+	inStream.Read(mPathRotation);
+	inStream.Read(mPathFraction);
+	inStream.Read(mMaxFrictionForce);
+	inStream.Read(mRotationConstraintType);
+	mPositionMotorSettings.RestoreBinaryState(inStream);
+}
+
+TwoBodyConstraint *PathConstraintSettings::Create(Body &inBody1, Body &inBody2) const
+{
+	return new PathConstraint(inBody1, inBody2, *this);
+}
+
+PathConstraint::PathConstraint(Body &inBody1, Body &inBody2, const PathConstraintSettings &inSettings) :
+	TwoBodyConstraint(inBody1, inBody2, inSettings),
+	mRotationConstraintType(inSettings.mRotationConstraintType),
+	mMaxFrictionForce(inSettings.mMaxFrictionForce),
+	mPositionMotorSettings(inSettings.mPositionMotorSettings)
+{
+	// Calculate transform that takes us from the path start to center of mass space of body 1
+	mPathToBody1 = Mat44::sRotationTranslation(inSettings.mPathRotation, inSettings.mPathPosition - inBody1.GetShape()->GetCenterOfMass());
+
+	SetPath(inSettings.mPath, inSettings.mPathFraction);
+}
+
+void PathConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM)
+{
+	if (mBody1->GetID() == inBodyID)
+		mPathToBody1.SetTranslation(mPathToBody1.GetTranslation() - inDeltaCOM);
+	else if (mBody2->GetID() == inBodyID)
+		mPathToBody2.SetTranslation(mPathToBody2.GetTranslation() - inDeltaCOM);
+}
+
+void PathConstraint::SetPath(const PathConstraintPath *inPath, float inPathFraction)
+{
+	mPath = inPath;
+	mPathFraction = inPathFraction;
+
+	if (mPath != nullptr)
+	{
+		// Get the point on the path for this fraction
+		Vec3 path_point, path_tangent, path_normal, path_binormal;
+		mPath->GetPointOnPath(mPathFraction, path_point, path_tangent, path_normal, path_binormal);
+
+		// Construct the matrix that takes us from the closest point on the path to body 2 center of mass space
+		Mat44 closest_point_to_path(Vec4(path_tangent, 0), Vec4(path_binormal, 0), Vec4(path_normal, 0), Vec4(path_point, 1));
+		Mat44 cp_to_body1 = mPathToBody1 * closest_point_to_path;
+		mPathToBody2 = (mBody2->GetInverseCenterOfMassTransform() * mBody1->GetCenterOfMassTransform()).ToMat44() * cp_to_body1;
+
+		// Calculate initial orientation
+		if (mRotationConstraintType == EPathRotationConstraintType::FullyConstrained)
+			mInvInitialOrientation = RotationEulerConstraintPart::sGetInvInitialOrientation(*mBody1, *mBody2);
+	}
+}
+
+void PathConstraint::CalculateConstraintProperties(float inDeltaTime)
+{
+	// Get transforms of body 1 and 2
+	RMat44 transform1 = mBody1->GetCenterOfMassTransform();
+	RMat44 transform2 = mBody2->GetCenterOfMassTransform();
+
+	// Get the transform of the path transform as seen from body 1 in world space
+	RMat44 path_to_world_1 = transform1 * mPathToBody1;
+
+	// Get the transform of from the point on path that body 2 is attached to in world space
+	RMat44 path_to_world_2 = transform2 * mPathToBody2;
+
+	// Calculate new closest point on path
+	RVec3 position2 = path_to_world_2.GetTranslation();
+	Vec3 position2_local_to_path = Vec3(path_to_world_1.InversedRotationTranslation() * position2);
+	mPathFraction = mPath->GetClosestPoint(position2_local_to_path, mPathFraction);
+
+	// Get the point on the path for this fraction
+	Vec3 path_point, path_tangent, path_normal, path_binormal;
+	mPath->GetPointOnPath(mPathFraction, path_point, path_tangent, path_normal, path_binormal);
+
+	// Calculate R1 and R2
+	RVec3 path_point_ws = path_to_world_1 * path_point;
+	mR1 = Vec3(path_point_ws - mBody1->GetCenterOfMassPosition());
+	mR2 = Vec3(position2 - mBody2->GetCenterOfMassPosition());
+
+	// Calculate U = X2 + R2 - X1 - R1
+	mU = Vec3(position2 - path_point_ws);
+
+	// Calculate world space normals
+	mPathNormal = path_to_world_1.Multiply3x3(path_normal);
+	mPathBinormal = path_to_world_1.Multiply3x3(path_binormal);
+
+	// Calculate slide axis
+	mPathTangent = path_to_world_1.Multiply3x3(path_tangent);
+
+	// Prepare constraint part for position constraint to slide along the path
+	mPositionConstraintPart.CalculateConstraintProperties(*mBody1, transform1.GetRotation(), mR1 + mU, *mBody2, transform2.GetRotation(), mR2, mPathNormal, mPathBinormal);
+
+	// Check if closest point is on the boundary of the path and if so apply limit
+	if (!mPath->IsLooping() && (mPathFraction <= 0.0f || mPathFraction >= mPath->GetPathMaxFraction()))
+		mPositionLimitsConstraintPart.CalculateConstraintProperties(*mBody1, mR1 + mU, *mBody2, mR2, mPathTangent);
+	else
+		mPositionLimitsConstraintPart.Deactivate();
+
+	// Prepare rotation constraint part
+	switch (mRotationConstraintType)
+	{
+	case EPathRotationConstraintType::Free:
+		// No rotational limits
+		break;
+
+	case EPathRotationConstraintType::ConstrainAroundTangent:
+		mHingeConstraintPart.CalculateConstraintProperties(*mBody1, transform1.GetRotation(), mPathTangent, *mBody2, transform2.GetRotation(), path_to_world_2.GetAxisX());
+		break;
+
+	case EPathRotationConstraintType::ConstrainAroundNormal:
+		mHingeConstraintPart.CalculateConstraintProperties(*mBody1, transform1.GetRotation(), mPathNormal, *mBody2, transform2.GetRotation(), path_to_world_2.GetAxisZ());
+		break;
+
+	case EPathRotationConstraintType::ConstrainAroundBinormal:
+		mHingeConstraintPart.CalculateConstraintProperties(*mBody1, transform1.GetRotation(), mPathBinormal, *mBody2, transform2.GetRotation(), path_to_world_2.GetAxisY());
+		break;
+
+	case EPathRotationConstraintType::ConstrainToPath:
+		// We need to calculate the inverse of the rotation from body 1 to body 2 for the current path position (see: RotationEulerConstraintPart::sGetInvInitialOrientation)
+		// RotationBody2 = RotationBody1 * InitialOrientation <=> InitialOrientation^-1 = RotationBody2^-1 * RotationBody1
+		// We can express RotationBody2 in terms of RotationBody1: RotationBody2 = RotationBody1 * PathToBody1 * RotationClosestPointOnPath * PathToBody2^-1
+		// Combining these two: InitialOrientation^-1 = PathToBody2 * (PathToBody1 * RotationClosestPointOnPath)^-1
+		mInvInitialOrientation = mPathToBody2.Multiply3x3RightTransposed(mPathToBody1.Multiply3x3(Mat44(Vec4(path_tangent, 0), Vec4(path_binormal, 0), Vec4(path_normal, 0), Vec4::sZero()))).GetQuaternion();
+		[[fallthrough]];
+
+	case EPathRotationConstraintType::FullyConstrained:
+		mRotationConstraintPart.CalculateConstraintProperties(*mBody1, transform1.GetRotation(), *mBody2, transform2.GetRotation());
+		break;
+	}
+
+	// Motor properties
+	switch (mPositionMotorState)
+	{
+	case EMotorState::Off:
+		if (mMaxFrictionForce > 0.0f)
+			mPositionMotorConstraintPart.CalculateConstraintProperties(*mBody1, mR1 + mU, *mBody2, mR2, mPathTangent);
+		else
+			mPositionMotorConstraintPart.Deactivate();
+		break;
+
+	case EMotorState::Velocity:
+		mPositionMotorConstraintPart.CalculateConstraintProperties(*mBody1, mR1 + mU, *mBody2, mR2, mPathTangent, -mTargetVelocity);
+		break;
+
+	case EMotorState::Position:
+		if (mPositionMotorSettings.mSpringSettings.HasStiffness())
+		{
+			// Calculate constraint value to drive to
+			float c;
+			if (mPath->IsLooping())
+			{
+				float max_fraction = mPath->GetPathMaxFraction();
+				c = fmod(mPathFraction - mTargetPathFraction, max_fraction);
+				float half_max_fraction = 0.5f * max_fraction;
+				if (c > half_max_fraction)
+					c -= max_fraction;
+				else if (c < -half_max_fraction)
+					c += max_fraction;
+			}
+			else
+				c = mPathFraction - mTargetPathFraction;
+			mPositionMotorConstraintPart.CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, mR1 + mU, *mBody2, mR2, mPathTangent, 0.0f, c, mPositionMotorSettings.mSpringSettings);
+		}
+		else
+			mPositionMotorConstraintPart.Deactivate();
+		break;
+	}
+}
+
+void PathConstraint::SetupVelocityConstraint(float inDeltaTime)
+{
+	CalculateConstraintProperties(inDeltaTime);
+}
+
+void PathConstraint::ResetWarmStart()
+{
+	mPositionMotorConstraintPart.Deactivate();
+	mPositionConstraintPart.Deactivate();
+	mPositionLimitsConstraintPart.Deactivate();
+	mHingeConstraintPart.Deactivate();
+	mRotationConstraintPart.Deactivate();
+}
+
+void PathConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio)
+{
+	// Warm starting: Apply previous frame impulse
+	mPositionMotorConstraintPart.WarmStart(*mBody1, *mBody2, mPathTangent, inWarmStartImpulseRatio);
+	mPositionConstraintPart.WarmStart(*mBody1, *mBody2, mPathNormal, mPathBinormal, inWarmStartImpulseRatio);
+	mPositionLimitsConstraintPart.WarmStart(*mBody1, *mBody2, mPathTangent, inWarmStartImpulseRatio);
+
+	switch (mRotationConstraintType)
+	{
+	case EPathRotationConstraintType::Free:
+		// No rotational limits
+		break;
+
+	case EPathRotationConstraintType::ConstrainAroundTangent:
+	case EPathRotationConstraintType::ConstrainAroundNormal:
+	case EPathRotationConstraintType::ConstrainAroundBinormal:
+		mHingeConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio);
+		break;
+
+	case EPathRotationConstraintType::ConstrainToPath:
+	case EPathRotationConstraintType::FullyConstrained:
+		mRotationConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio);
+		break;
+	}
+}
+
+bool PathConstraint::SolveVelocityConstraint(float inDeltaTime)
+{
+	// Solve motor
+	bool motor = false;
+	if (mPositionMotorConstraintPart.IsActive())
+	{
+		switch (mPositionMotorState)
+		{
+		case EMotorState::Off:
+			{
+				float max_lambda = mMaxFrictionForce * inDeltaTime;
+				motor = mPositionMotorConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mPathTangent, -max_lambda, max_lambda);
+				break;
+			}
+
+		case EMotorState::Velocity:
+		case EMotorState::Position:
+			motor = mPositionMotorConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mPathTangent, inDeltaTime * mPositionMotorSettings.mMinForceLimit, inDeltaTime * mPositionMotorSettings.mMaxForceLimit);
+			break;
+		}
+	}
+
+	// Solve position constraint along 2 axis
+	bool pos = mPositionConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mPathNormal, mPathBinormal);
+
+	// Solve limits along path axis
+	bool limit = false;
+	if (mPositionLimitsConstraintPart.IsActive())
+	{
+		if (mPathFraction <= 0.0f)
+			limit = mPositionLimitsConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mPathTangent, 0, FLT_MAX);
+		else
+		{
+			JPH_ASSERT(mPathFraction >= mPath->GetPathMaxFraction());
+			limit = mPositionLimitsConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mPathTangent, -FLT_MAX, 0);
+		}
+	}
+
+	// Solve rotational constraint
+	// Note, this is not entirely correct, we should apply a velocity constraint so that the body will actually follow the path
+	// by looking at the derivative of the tangent, normal or binormal but we don't. This means the position constraint solver
+	// will need to correct the orientation error that builds up, which in turn means that the simulation is not physically correct.
+	bool rot = false;
+	switch (mRotationConstraintType)
+	{
+	case EPathRotationConstraintType::Free:
+		// No rotational limits
+		break;
+
+	case EPathRotationConstraintType::ConstrainAroundTangent:
+	case EPathRotationConstraintType::ConstrainAroundNormal:
+	case EPathRotationConstraintType::ConstrainAroundBinormal:
+		rot = mHingeConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2);
+		break;
+
+	case EPathRotationConstraintType::ConstrainToPath:
+	case EPathRotationConstraintType::FullyConstrained:
+		rot = mRotationConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2);
+		break;
+	}
+
+	return motor || pos || limit || rot;
+}
+
+bool PathConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte)
+{
+	// Update constraint properties (bodies may have moved)
+	CalculateConstraintProperties(inDeltaTime);
+
+	// Solve position constraint along 2 axis
+	bool pos = mPositionConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mU, mPathNormal, mPathBinormal, inBaumgarte);
+
+	// Solve limits along path axis
+	bool limit = false;
+	if (mPositionLimitsConstraintPart.IsActive())
+	{
+		if (mPathFraction <= 0.0f)
+			limit = mPositionLimitsConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mPathTangent, mU.Dot(mPathTangent), inBaumgarte);
+		else
+		{
+			JPH_ASSERT(mPathFraction >= mPath->GetPathMaxFraction());
+			limit = mPositionLimitsConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mPathTangent, mU.Dot(mPathTangent), inBaumgarte);
+		}
+	}
+
+	// Solve rotational constraint
+	bool rot = false;
+	switch (mRotationConstraintType)
+	{
+	case EPathRotationConstraintType::Free:
+		// No rotational limits
+		break;
+
+	case EPathRotationConstraintType::ConstrainAroundTangent:
+	case EPathRotationConstraintType::ConstrainAroundNormal:
+	case EPathRotationConstraintType::ConstrainAroundBinormal:
+		rot = mHingeConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, inBaumgarte);
+		break;
+
+	case EPathRotationConstraintType::ConstrainToPath:
+	case EPathRotationConstraintType::FullyConstrained:
+		rot = mRotationConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mInvInitialOrientation, inBaumgarte);
+		break;
+	}
+
+	return pos || limit || rot;
+}
+
+#ifdef JPH_DEBUG_RENDERER
+void PathConstraint::DrawConstraint(DebugRenderer *inRenderer) const
+{
+	if (mPath != nullptr)
+	{
+		// Draw the path in world space
+		RMat44 path_to_world = mBody1->GetCenterOfMassTransform() * mPathToBody1;
+		mPath->DrawPath(inRenderer, path_to_world);
+
+		// Draw anchor point of both bodies in world space
+		RVec3 x1 = mBody1->GetCenterOfMassPosition() + mR1;
+		RVec3 x2 = mBody2->GetCenterOfMassPosition() + mR2;
+		inRenderer->DrawMarker(x1, Color::sYellow, 0.1f);
+		inRenderer->DrawMarker(x2, Color::sYellow, 0.1f);
+		inRenderer->DrawArrow(x1, x1 + mPathTangent, Color::sBlue, 0.1f);
+		inRenderer->DrawArrow(x1, x1 + mPathNormal, Color::sRed, 0.1f);
+		inRenderer->DrawArrow(x1, x1 + mPathBinormal, Color::sGreen, 0.1f);
+		inRenderer->DrawText3D(x1, StringFormat("%.1f", (double)mPathFraction));
+
+		// Draw motor
+		switch (mPositionMotorState)
+		{
+		case EMotorState::Position:
+			{
+				// Draw target marker
+				Vec3 position, tangent, normal, binormal;
+				mPath->GetPointOnPath(mTargetPathFraction, position, tangent, normal, binormal);
+				inRenderer->DrawMarker(path_to_world * position, Color::sYellow, 1.0f);
+				break;
+			}
+
+		case EMotorState::Velocity:
+			{
+				RVec3 position = mBody2->GetCenterOfMassPosition() + mR2;
+				inRenderer->DrawArrow(position, position + mPathTangent * mTargetVelocity, Color::sRed, 0.1f);
+				break;
+			}
+
+		case EMotorState::Off:
+			break;
+		}
+	}
+}
+#endif // JPH_DEBUG_RENDERER
+
+void PathConstraint::SaveState(StateRecorder &inStream) const
+{
+	TwoBodyConstraint::SaveState(inStream);
+
+	mPositionConstraintPart.SaveState(inStream);
+	mPositionLimitsConstraintPart.SaveState(inStream);
+	mPositionMotorConstraintPart.SaveState(inStream);
+	mHingeConstraintPart.SaveState(inStream);
+	mRotationConstraintPart.SaveState(inStream);
+
+	inStream.Write(mMaxFrictionForce);
+	inStream.Write(mPositionMotorSettings);
+	inStream.Write(mPositionMotorState);
+	inStream.Write(mTargetVelocity);
+	inStream.Write(mTargetPathFraction);
+	inStream.Write(mPathFraction);
+}
+
+void PathConstraint::RestoreState(StateRecorder &inStream)
+{
+	TwoBodyConstraint::RestoreState(inStream);
+
+	mPositionConstraintPart.RestoreState(inStream);
+	mPositionLimitsConstraintPart.RestoreState(inStream);
+	mPositionMotorConstraintPart.RestoreState(inStream);
+	mHingeConstraintPart.RestoreState(inStream);
+	mRotationConstraintPart.RestoreState(inStream);
+
+	inStream.Read(mMaxFrictionForce);
+	inStream.Read(mPositionMotorSettings);
+	inStream.Read(mPositionMotorState);
+	inStream.Read(mTargetVelocity);
+	inStream.Read(mTargetPathFraction);
+	inStream.Read(mPathFraction);
+}
+
+Ref<ConstraintSettings> PathConstraint::GetConstraintSettings() const
+{
+	JPH_ASSERT(false); // Not implemented yet
+	return nullptr;
+}
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraint.h

@@ -0,0 +1,191 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Constraints/TwoBodyConstraint.h>
+#include <Jolt/Physics/Constraints/PathConstraintPath.h>
+#include <Jolt/Physics/Constraints/MotorSettings.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/AxisConstraintPart.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/DualAxisConstraintPart.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/HingeRotationConstraintPart.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/RotationEulerConstraintPart.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_SUPPRESS_WARNING_PUSH
+JPH_GCC_SUPPRESS_WARNING("-Wshadow") // GCC complains about the 'Free' value conflicting with the 'Free' method
+
+/// How to constrain the rotation of the body to a PathConstraint
+enum class EPathRotationConstraintType
+{
+	Free,							///< Do not constrain the rotation of the body at all
+	ConstrainAroundTangent,			///< Only allow rotation around the tangent vector (following the path)
+	ConstrainAroundNormal,			///< Only allow rotation around the normal vector (perpendicular to the path)
+	ConstrainAroundBinormal,		///< Only allow rotation around the binormal vector (perpendicular to the path)
+	ConstrainToPath,				///< Fully constrain the rotation of body 2 to the path (following the tangent and normal of the path)
+	FullyConstrained,				///< Fully constrain the rotation of the body 2 to the rotation of body 1
+};
+
+JPH_SUPPRESS_WARNING_POP
+
+/// Path constraint settings, used to constrain the degrees of freedom between two bodies to a path
+///
+/// The requirements of the path are that:
+/// * Tangent, normal and bi-normal form an orthonormal basis with: tangent cross bi-normal = normal
+/// * The path points along the tangent vector
+/// * The path is continuous so doesn't contain any sharp corners
+///
+/// The reason for all this is that the constraint acts like a slider constraint with the sliding axis being the tangent vector (the assumption here is that delta time will be small enough so that the path is linear for that delta time).
+class JPH_EXPORT PathConstraintSettings final : public TwoBodyConstraintSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, PathConstraintSettings)
+
+public:
+	// See: ConstraintSettings::SaveBinaryState
+	virtual void					SaveBinaryState(StreamOut &inStream) const override;
+
+	/// Create an instance of this constraint
+	virtual TwoBodyConstraint *		Create(Body &inBody1, Body &inBody2) const override;
+
+	/// The path that constrains the two bodies
+	RefConst<PathConstraintPath>	mPath;
+
+	/// The position of the path start relative to world transform of body 1
+	Vec3							mPathPosition = Vec3::sZero();
+
+	/// The rotation of the path start relative to world transform of body 1
+	Quat							mPathRotation = Quat::sIdentity();
+
+	/// The fraction along the path that corresponds to the initial position of body 2. Usually this is 0, the beginning of the path. But if you want to start an object halfway the path you can calculate this with mPath->GetClosestPoint(point on path to attach body to).
+	float							mPathFraction = 0.0f;
+
+	/// Maximum amount of friction force to apply (N) when not driven by a motor.
+	float							mMaxFrictionForce = 0.0f;
+
+	/// In case the constraint is powered, this determines the motor settings along the path
+	MotorSettings					mPositionMotorSettings;
+
+	/// How to constrain the rotation of the body to the path
+	EPathRotationConstraintType		mRotationConstraintType = EPathRotationConstraintType::Free;
+
+protected:
+	// See: ConstraintSettings::RestoreBinaryState
+	virtual void					RestoreBinaryState(StreamIn &inStream) override;
+};
+
+/// Path constraint, used to constrain the degrees of freedom between two bodies to a path
+class JPH_EXPORT PathConstraint final : public TwoBodyConstraint
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Construct point constraint
+									PathConstraint(Body &inBody1, Body &inBody2, const PathConstraintSettings &inSettings);
+
+	// Generic interface of a constraint
+	virtual EConstraintSubType		GetSubType() const override								{ return EConstraintSubType::Path; }
+	virtual void					NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override;
+	virtual void					SetupVelocityConstraint(float inDeltaTime) override;
+	virtual void					ResetWarmStart() override;
+	virtual void					WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override;
+	virtual bool					SolveVelocityConstraint(float inDeltaTime) override;
+	virtual bool					SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override;
+#ifdef JPH_DEBUG_RENDERER
+	virtual void					DrawConstraint(DebugRenderer *inRenderer) const override;
+#endif // JPH_DEBUG_RENDERER
+	virtual void					SaveState(StateRecorder &inStream) const override;
+	virtual void					RestoreState(StateRecorder &inStream) override;
+	virtual bool					IsActive() const override								{ return TwoBodyConstraint::IsActive() && mPath != nullptr; }
+	virtual Ref<ConstraintSettings> GetConstraintSettings() const override;
+
+	// See: TwoBodyConstraint
+	virtual Mat44					GetConstraintToBody1Matrix() const override				{ return mPathToBody1; }
+	virtual Mat44					GetConstraintToBody2Matrix() const override				{ return mPathToBody2; }
+
+	/// Update the path for this constraint
+	void							SetPath(const PathConstraintPath *inPath, float inPathFraction);
+
+	/// Access to the current path
+	const PathConstraintPath *		GetPath() const											{ return mPath; }
+
+	/// Access to the current fraction along the path e [0, GetPath()->GetMaxPathFraction()]
+	float							GetPathFraction() const									{ return mPathFraction; }
+
+	/// Friction control
+	void							SetMaxFrictionForce(float inFrictionForce)				{ mMaxFrictionForce = inFrictionForce; }
+	float							GetMaxFrictionForce() const								{ return mMaxFrictionForce; }
+
+	/// Position motor settings
+	MotorSettings &					GetPositionMotorSettings()								{ return mPositionMotorSettings; }
+	const MotorSettings &			GetPositionMotorSettings() const						{ return mPositionMotorSettings; }
+
+	// Position motor controls (drives body 2 along the path)
+	void							SetPositionMotorState(EMotorState inState)				{ JPH_ASSERT(inState == EMotorState::Off || mPositionMotorSettings.IsValid()); mPositionMotorState = inState; }
+	EMotorState						GetPositionMotorState() const							{ return mPositionMotorState; }
+	void							SetTargetVelocity(float inVelocity)						{ mTargetVelocity = inVelocity; }
+	float							GetTargetVelocity() const								{ return mTargetVelocity; }
+	void							SetTargetPathFraction(float inFraction)					{ JPH_ASSERT(mPath->IsLooping() || (inFraction >= 0.0f && inFraction <= mPath->GetPathMaxFraction())); mTargetPathFraction = inFraction; }
+	float							GetTargetPathFraction() const							{ return mTargetPathFraction; }
+
+	///@name Get Lagrange multiplier from last physics update (the linear/angular impulse applied to satisfy the constraint)
+	inline Vector<2>				GetTotalLambdaPosition() const							{ return mPositionConstraintPart.GetTotalLambda(); }
+	inline float					GetTotalLambdaPositionLimits() const					{ return mPositionLimitsConstraintPart.GetTotalLambda(); }
+	inline float					GetTotalLambdaMotor() const								{ return mPositionMotorConstraintPart.GetTotalLambda(); }
+	inline Vector<2>				GetTotalLambdaRotationHinge() const						{ return mHingeConstraintPart.GetTotalLambda(); }
+	inline Vec3						GetTotalLambdaRotation() const							{ return mRotationConstraintPart.GetTotalLambda(); }
+
+private:
+	// Internal helper function to calculate the values below
+	void							CalculateConstraintProperties(float inDeltaTime);
+
+	// CONFIGURATION PROPERTIES FOLLOW
+
+	RefConst<PathConstraintPath>	mPath;													///< The path that attaches the two bodies
+	Mat44							mPathToBody1;											///< Transform that takes a quantity from path space to body 1 center of mass space
+	Mat44							mPathToBody2;											///< Transform that takes a quantity from path space to body 2 center of mass space
+	EPathRotationConstraintType		mRotationConstraintType;								///< How to constrain the rotation of the path
+
+	// Friction
+	float							mMaxFrictionForce;
+
+	// Motor controls
+	MotorSettings					mPositionMotorSettings;
+	EMotorState						mPositionMotorState = EMotorState::Off;
+	float							mTargetVelocity = 0.0f;
+	float							mTargetPathFraction = 0.0f;
+
+	// RUN TIME PROPERTIES FOLLOW
+
+	// Positions where the point constraint acts on in world space
+	Vec3							mR1;
+	Vec3							mR2;
+
+	// X2 + R2 - X1 - R1
+	Vec3							mU;
+
+	// World space path tangent
+	Vec3							mPathTangent;
+
+	// Normals to the path tangent
+	Vec3							mPathNormal;
+	Vec3							mPathBinormal;
+
+	// Inverse of initial rotation from body 1 to body 2 in body 1 space (only used when rotation constraint type is FullyConstrained)
+	Quat							mInvInitialOrientation;
+
+	// Current fraction along the path where body 2 is attached
+	float							mPathFraction = 0.0f;
+
+	// Translation constraint parts
+	DualAxisConstraintPart			mPositionConstraintPart;								///< Constraint part that keeps the movement along the tangent of the path
+	AxisConstraintPart				mPositionLimitsConstraintPart;							///< Constraint part that prevents movement beyond the beginning and end of the path
+	AxisConstraintPart				mPositionMotorConstraintPart;							///< Constraint to drive the object along the path or to apply friction
+
+	// Rotation constraint parts
+	HingeRotationConstraintPart		mHingeConstraintPart;									///< Constraint part that removes 2 degrees of rotation freedom
+	RotationEulerConstraintPart		mRotationConstraintPart;								///< Constraint part that removes all rotational freedom
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraintPath.cpp

@@ -0,0 +1,85 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Constraints/PathConstraintPath.h>
+#include <Jolt/Core/StreamUtils.h>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT(PathConstraintPath)
+{
+	JPH_ADD_BASE_CLASS(PathConstraintPath, SerializableObject)
+}
+
+#ifdef JPH_DEBUG_RENDERER
+// Helper function to transform the results of GetPointOnPath to world space
+static inline void sTransformPathPoint(RMat44Arg inTransform, Vec3Arg inPosition, RVec3 &outPosition, Vec3 &ioNormal, Vec3 &ioBinormal)
+{
+	outPosition = inTransform * inPosition;
+	ioNormal = inTransform.Multiply3x3(ioNormal);
+	ioBinormal = inTransform.Multiply3x3(ioBinormal);
+}
+
+// Helper function to draw a path segment
+static inline void sDrawPathSegment(DebugRenderer *inRenderer, RVec3Arg inPrevPosition, RVec3Arg inPosition, Vec3Arg inNormal, Vec3Arg inBinormal)
+{
+	inRenderer->DrawLine(inPrevPosition, inPosition, Color::sWhite);
+	inRenderer->DrawArrow(inPosition, inPosition + 0.1f * inNormal, Color::sRed, 0.02f);
+	inRenderer->DrawArrow(inPosition, inPosition + 0.1f * inBinormal, Color::sGreen, 0.02f);
+}
+
+void PathConstraintPath::DrawPath(DebugRenderer *inRenderer, RMat44Arg inBaseTransform) const
+{
+	// Calculate first point
+	Vec3 lfirst_pos, first_tangent, first_normal, first_binormal;
+	GetPointOnPath(0.0f, lfirst_pos, first_tangent, first_normal, first_binormal);
+	RVec3 first_pos;
+	sTransformPathPoint(inBaseTransform, lfirst_pos, first_pos, first_normal, first_binormal);
+
+	float t_max = GetPathMaxFraction();
+
+	// Draw the segments
+	RVec3 prev_pos = first_pos;
+	for (float t = 0.1f; t < t_max; t += 0.1f)
+	{
+		Vec3 lpos, tangent, normal, binormal;
+		GetPointOnPath(t, lpos, tangent, normal, binormal);
+		RVec3 pos;
+		sTransformPathPoint(inBaseTransform, lpos, pos, normal, binormal);
+		sDrawPathSegment(inRenderer, prev_pos, pos, normal, binormal);
+		prev_pos = pos;
+	}
+
+	// Draw last point
+	Vec3 lpos, tangent, normal, binormal;
+	GetPointOnPath(t_max, lpos, tangent, normal, binormal);
+	RVec3 pos;
+	sTransformPathPoint(inBaseTransform, lpos, pos, normal, binormal);
+	sDrawPathSegment(inRenderer, prev_pos, pos, normal, binormal);
+}
+#endif // JPH_DEBUG_RENDERER
+
+void PathConstraintPath::SaveBinaryState(StreamOut &inStream) const
+{
+	inStream.Write(GetRTTI()->GetHash());
+	inStream.Write(mIsLooping);
+}
+
+void PathConstraintPath::RestoreBinaryState(StreamIn &inStream)
+{
+	// Type hash read by sRestoreFromBinaryState
+	inStream.Read(mIsLooping);
+}
+
+PathConstraintPath::PathResult PathConstraintPath::sRestoreFromBinaryState(StreamIn &inStream)
+{
+	return StreamUtils::RestoreObject<PathConstraintPath>(inStream, &PathConstraintPath::RestoreBinaryState);
+}
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraintPath.h

@@ -0,0 +1,71 @@
+// 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/Result.h>
+#include <Jolt/ObjectStream/SerializableObject.h>
+
+JPH_NAMESPACE_BEGIN
+
+class StreamIn;
+class StreamOut;
+#ifdef JPH_DEBUG_RENDERER
+class DebugRenderer;
+#endif // JPH_DEBUG_RENDERER
+
+/// The path for a path constraint. It allows attaching two bodies to each other while giving the second body the freedom to move along a path relative to the first.
+class JPH_EXPORT PathConstraintPath : public SerializableObject, public RefTarget<PathConstraintPath>
+{
+	JPH_DECLARE_SERIALIZABLE_ABSTRACT(JPH_EXPORT, PathConstraintPath)
+
+public:
+	using PathResult = Result<Ref<PathConstraintPath>>;
+
+	/// Virtual destructor to ensure that derived types get their destructors called
+	virtual				~PathConstraintPath() override = default;
+
+	/// Gets the max fraction along the path. I.e. sort of the length of the path.
+	virtual float		GetPathMaxFraction() const = 0;
+
+	/// Get the globally closest point on the curve (Could be slow!)
+	/// @param inPosition Position to find closest point for
+	/// @param inFractionHint Last known fraction along the path (can be used to speed up the search)
+	/// @return Fraction of closest point along the path
+	virtual float		GetClosestPoint(Vec3Arg inPosition, float inFractionHint) const = 0;
+
+	/// Given the fraction along the path, get the point, tangent and normal.
+	/// @param inFraction Fraction along the path [0, GetPathMaxFraction()].
+	/// @param outPathPosition Returns the closest position to inSearchPosition on the path.
+	/// @param outPathTangent Returns the tangent to the path at outPathPosition (the vector that follows the direction of the path)
+	/// @param outPathNormal Return the normal to the path at outPathPosition (a vector that's perpendicular to outPathTangent)
+	/// @param outPathBinormal Returns the binormal to the path at outPathPosition (a vector so that normal cross tangent = binormal)
+	virtual void		GetPointOnPath(float inFraction, Vec3 &outPathPosition, Vec3 &outPathTangent, Vec3 &outPathNormal, Vec3 &outPathBinormal) const = 0;
+
+	/// If the path is looping or not. If a path is looping, the first and last point are automatically connected to each other. They should not be the same points.
+	void				SetIsLooping(bool inIsLooping)						{ mIsLooping = inIsLooping; }
+	bool				IsLooping() const									{ return mIsLooping; }
+
+#ifdef JPH_DEBUG_RENDERER
+	/// Draw the path relative to inBaseTransform. Used for debug purposes.
+	void				DrawPath(DebugRenderer *inRenderer, RMat44Arg inBaseTransform) const;
+#endif // JPH_DEBUG_RENDERER
+
+	/// Saves the contents of the path in binary form to inStream.
+	virtual void		SaveBinaryState(StreamOut &inStream) const;
+
+	/// Creates a Shape of the correct type and restores its contents from the binary stream inStream.
+	static PathResult	sRestoreFromBinaryState(StreamIn &inStream);
+
+protected:
+	/// This function should not be called directly, it is used by sRestoreFromBinaryState.
+	virtual void		RestoreBinaryState(StreamIn &inStream);
+
+private:
+	/// If the path is looping or not. If a path is looping, the first and last point are automatically connected to each other. They should not be the same points.
+	bool				mIsLooping = false;
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraintPathHermite.cpp

@@ -0,0 +1,308 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Constraints/PathConstraintPathHermite.h>
+#include <Jolt/Core/Profiler.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(PathConstraintPathHermite::Point)
+{
+	JPH_ADD_ATTRIBUTE(PathConstraintPathHermite::Point, mPosition)
+	JPH_ADD_ATTRIBUTE(PathConstraintPathHermite::Point, mTangent)
+	JPH_ADD_ATTRIBUTE(PathConstraintPathHermite::Point, mNormal)
+}
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(PathConstraintPathHermite)
+{
+	JPH_ADD_BASE_CLASS(PathConstraintPathHermite, PathConstraintPath)
+
+	JPH_ADD_ATTRIBUTE(PathConstraintPathHermite, mPoints)
+}
+
+// Calculate position and tangent for a Cubic Hermite Spline segment
+static inline void sCalculatePositionAndTangent(Vec3Arg inP1, Vec3Arg inM1, Vec3Arg inP2, Vec3Arg inM2, float inT, Vec3 &outPosition, Vec3 &outTangent)
+{
+	// Calculate factors for Cubic Hermite Spline
+	// See: https://en.wikipedia.org/wiki/Cubic_Hermite_spline
+	float t2 = inT * inT;
+	float t3 = inT * t2;
+	float h00 = 2.0f * t3 - 3.0f * t2 + 1.0f;
+	float h10 = t3 - 2.0f * t2 + inT;
+	float h01 = -2.0f * t3 + 3.0f * t2;
+	float h11 = t3 - t2;
+
+	// Calculate d/dt for factors to calculate the tangent
+	float ddt_h00 = 6.0f * (t2 - inT);
+	float ddt_h10 = 3.0f * t2 - 4.0f * inT + 1.0f;
+	float ddt_h01 = -ddt_h00;
+	float ddt_h11 = 3.0f * t2 - 2.0f * inT;
+
+	outPosition = h00 * inP1 + h10 * inM1 + h01 * inP2 + h11 * inM2;
+	outTangent = ddt_h00 * inP1 + ddt_h10 * inM1 + ddt_h01 * inP2 + ddt_h11 * inM2;
+}
+
+// Calculate the closest point to the origin for a Cubic Hermite Spline segment
+// This is used to get an estimate for the interval in which the closest point can be found,
+// the interval [0, 1] is too big for Newton Raphson to work on because it is solving a 5th degree polynomial which may
+// have multiple local minima that are not the root. This happens especially when the path is straight (tangents aligned with inP2 - inP1).
+// Based on the bisection method: https://en.wikipedia.org/wiki/Bisection_method
+static inline void sCalculateClosestPointThroughBisection(Vec3Arg inP1, Vec3Arg inM1, Vec3Arg inP2, Vec3Arg inM2, float &outTMin, float &outTMax)
+{
+	outTMin = 0.0f;
+	outTMax = 1.0f;
+
+	// To get the closest point of the curve to the origin we need to solve:
+	// d/dt P(t) . P(t) = 0 for t, where P(t) is the point on the curve segment
+	// Using d/dt (a(t) . b(t)) = d/dt a(t) . b(t) + a(t) . d/dt b(t)
+	// See: https://proofwiki.org/wiki/Derivative_of_Dot_Product_of_Vector-Valued_Functions
+	// d/dt P(t) . P(t) = 2 P(t) d/dt P(t) = 2 P(t) . Tangent(t)
+
+	// Calculate the derivative at t = 0, we know P(0) = inP1 and Tangent(0) = inM1
+	float ddt_min = inP1.Dot(inM1); // Leaving out factor 2, we're only interested in the root
+	if (abs(ddt_min) < 1.0e-6f)
+	{
+		// Derivative is near zero, we found our root
+		outTMax = 0.0f;
+		return;
+	}
+	bool ddt_min_negative = ddt_min < 0.0f;
+
+	// Calculate derivative at t = 1, we know P(1) = inP2 and Tangent(1) = inM2
+	float ddt_max = inP2.Dot(inM2);
+	if (abs(ddt_max) < 1.0e-6f)
+	{
+		// Derivative is near zero, we found our root
+		outTMin = 1.0f;
+		return;
+	}
+	bool ddt_max_negative = ddt_max < 0.0f;
+
+	// If the signs of the derivative are not different, this algorithm can't find the root
+	if (ddt_min_negative == ddt_max_negative)
+		return;
+
+	// With 4 iterations we'll get a result accurate to 1 / 2^4 = 0.0625
+	for (int iteration = 0; iteration < 4; ++iteration)
+	{
+		float t_mid = 0.5f * (outTMin + outTMax);
+		Vec3 position, tangent;
+		sCalculatePositionAndTangent(inP1, inM1, inP2, inM2, t_mid, position, tangent);
+		float ddt_mid = position.Dot(tangent);
+		if (abs(ddt_mid) < 1.0e-6f)
+		{
+			// Derivative is near zero, we found our root
+			outTMin = outTMax = t_mid;
+			return;
+		}
+		bool ddt_mid_negative = ddt_mid < 0.0f;
+
+		// Update the search interval so that the signs of the derivative at both ends of the interval are still different
+		if (ddt_mid_negative == ddt_min_negative)
+			outTMin = t_mid;
+		else
+			outTMax = t_mid;
+	}
+}
+
+// Calculate the closest point to the origin for a Cubic Hermite Spline segment
+// Only considers the range t e [inTMin, inTMax] and will stop as soon as the closest point falls outside of that range
+static inline float sCalculateClosestPointThroughNewtonRaphson(Vec3Arg inP1, Vec3Arg inM1, Vec3Arg inP2, Vec3Arg inM2, float inTMin, float inTMax, float &outDistanceSq)
+{
+	// This is the closest position on the curve to the origin that we found
+	Vec3 position;
+
+	// Calculate the size of the interval
+	float interval = inTMax - inTMin;
+
+	// Start in the middle of the interval
+	float t = 0.5f * (inTMin + inTMax);
+
+	// Do max 10 iterations to prevent taking too much CPU time
+	for (int iteration = 0; iteration < 10; ++iteration)
+	{
+		// Calculate derivative at t, see comment at sCalculateClosestPointThroughBisection for derivation of the equations
+		Vec3 tangent;
+		sCalculatePositionAndTangent(inP1, inM1, inP2, inM2, t, position, tangent);
+		float ddt = position.Dot(tangent); // Leaving out factor 2, we're only interested in the root
+
+		// Calculate derivative of ddt: d^2/dt P(t) . P(t) = d/dt (2 P(t) . Tangent(t))
+		// = 2 (d/dt P(t)) . Tangent(t) + P(t) . d/dt Tangent(t)) = 2 (Tangent(t) . Tangent(t) + P(t) . d/dt Tangent(t))
+		float d2dt_h00 = 12.0f * t - 6.0f;
+		float d2dt_h10 = 6.0f * t - 4.0f;
+		float d2dt_h01 = -d2dt_h00;
+		float d2dt_h11 = 6.0f * t - 2.0f;
+		Vec3 ddt_tangent = d2dt_h00 * inP1 + d2dt_h10 * inM1 + d2dt_h01 * inP2 + d2dt_h11 * inM2;
+		float d2dt = tangent.Dot(tangent) + position.Dot(ddt_tangent); // Leaving out factor 2, because we left it out above too
+
+		// If d2dt is zero, the curve is flat and there are multiple t's for which we are closest to the origin, stop now
+		if (d2dt == 0.0f)
+			break;
+
+		// Do a Newton Raphson step
+		// See: https://en.wikipedia.org/wiki/Newton%27s_method
+		// Clamp against [-interval, interval] to avoid overshooting too much, we're not interested outside the interval
+		float delta = Clamp(-ddt / d2dt, -interval, interval);
+
+		// If we're stepping away further from t e [inTMin, inTMax] stop now
+		if ((t > inTMax && delta > 0.0f) || (t < inTMin && delta < 0.0f))
+			break;
+
+		// If we've converged, stop now
+		t += delta;
+		if (abs(delta) < 1.0e-4f)
+			break;
+	}
+
+	// Calculate the distance squared for the origin to the curve
+	outDistanceSq = position.LengthSq();
+	return t;
+}
+
+void PathConstraintPathHermite::GetIndexAndT(float inFraction, int &outIndex, float &outT) const
+{
+	int num_points = int(mPoints.size());
+
+	// Start by truncating the fraction to get the index and storing the remainder in t
+	int index = int(trunc(inFraction));
+	float t = inFraction - float(index);
+
+	if (IsLooping())
+	{
+		JPH_ASSERT(!mPoints.front().mPosition.IsClose(mPoints.back().mPosition), "A looping path should have a different first and last point!");
+
+		// Make sure index is positive by adding a multiple of num_points
+		if (index < 0)
+			index += (-index / num_points + 1) * num_points;
+
+		// Index needs to be modulo num_points
+		index = index % num_points;
+	}
+	else
+	{
+		// Clamp against range of points
+		if (index < 0)
+		{
+			index = 0;
+			t = 0.0f;
+		}
+		else if (index >= num_points - 1)
+		{
+			index = num_points - 2;
+			t = 1.0f;
+		}
+	}
+
+	outIndex = index;
+	outT = t;
+}
+
+float PathConstraintPathHermite::GetClosestPoint(Vec3Arg inPosition, float inFractionHint) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	int num_points = int(mPoints.size());
+
+	// Start with last point on the path, in the non-looping case we won't be visiting this point
+	float best_dist_sq = (mPoints[num_points - 1].mPosition - inPosition).LengthSq();
+	float best_t = float(num_points - 1);
+
+	// Loop over all points
+	for (int i = 0, max_i = IsLooping()? num_points : num_points - 1; i < max_i; ++i)
+	{
+		const Point &p1 = mPoints[i];
+		const Point &p2 = mPoints[(i + 1) % num_points];
+
+		// Make the curve relative to inPosition
+		Vec3 p1_pos = p1.mPosition - inPosition;
+		Vec3 p2_pos = p2.mPosition - inPosition;
+
+		// Get distance to p1
+		float dist_sq = p1_pos.LengthSq();
+		if (dist_sq < best_dist_sq)
+		{
+			best_t = float(i);
+			best_dist_sq = dist_sq;
+		}
+
+		// First find an interval for the closest point so that we can start doing Newton Raphson steps
+		float t_min, t_max;
+		sCalculateClosestPointThroughBisection(p1_pos, p1.mTangent, p2_pos, p2.mTangent, t_min, t_max);
+
+		if (t_min == t_max)
+		{
+			// If the function above returned no interval then it found the root already and we can just calculate the distance
+			Vec3 position, tangent;
+			sCalculatePositionAndTangent(p1_pos, p1.mTangent, p2_pos, p2.mTangent, t_min, position, tangent);
+			dist_sq = position.LengthSq();
+			if (dist_sq < best_dist_sq)
+			{
+				best_t = float(i) + t_min;
+				best_dist_sq = dist_sq;
+			}
+		}
+		else
+		{
+			// Get closest distance along curve segment
+			float t = sCalculateClosestPointThroughNewtonRaphson(p1_pos, p1.mTangent, p2_pos, p2.mTangent, t_min, t_max, dist_sq);
+			if (t >= 0.0f && t <= 1.0f && dist_sq < best_dist_sq)
+			{
+				best_t = float(i) + t;
+				best_dist_sq = dist_sq;
+			}
+		}
+	}
+
+	return best_t;
+}
+
+void PathConstraintPathHermite::GetPointOnPath(float inFraction, Vec3 &outPathPosition, Vec3 &outPathTangent, Vec3 &outPathNormal, Vec3 &outPathBinormal) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Determine which hermite spline segment we need
+	int index;
+	float t;
+	GetIndexAndT(inFraction, index, t);
+
+	// Get the points on the segment
+	const Point &p1 = mPoints[index];
+	const Point &p2 = mPoints[(index + 1) % int(mPoints.size())];
+
+	// Calculate the position and tangent on the path
+	Vec3 tangent;
+	sCalculatePositionAndTangent(p1.mPosition, p1.mTangent, p2.mPosition, p2.mTangent, t, outPathPosition, tangent);
+	outPathTangent = tangent.Normalized();
+
+	// Just linearly interpolate the normal
+	Vec3 normal = (1.0f - t) * p1.mNormal + t * p2.mNormal;
+
+	// Calculate binormal
+	outPathBinormal = normal.Cross(outPathTangent).Normalized();
+
+	// Recalculate normal so it is perpendicular to both (linear interpolation will cause it not to be)
+	outPathNormal = outPathTangent.Cross(outPathBinormal);
+	JPH_ASSERT(outPathNormal.IsNormalized());
+}
+
+void PathConstraintPathHermite::SaveBinaryState(StreamOut &inStream) const
+{
+	PathConstraintPath::SaveBinaryState(inStream);
+
+	inStream.Write(mPoints);
+}
+
+void PathConstraintPathHermite::RestoreBinaryState(StreamIn &inStream)
+{
+	PathConstraintPath::RestoreBinaryState(inStream);
+
+	inStream.Read(mPoints);
+}
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraintPathHermite.h

@@ -0,0 +1,54 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Constraints/PathConstraintPath.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// A path that follows a Hermite spline
+class JPH_EXPORT PathConstraintPathHermite final : public PathConstraintPath
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, PathConstraintPathHermite)
+
+public:
+	// See PathConstraintPath::GetPathMaxFraction
+	virtual float		GetPathMaxFraction() const override									{ return float(IsLooping()? mPoints.size() : mPoints.size() - 1); }
+
+	// See PathConstraintPath::GetClosestPoint
+	virtual float		GetClosestPoint(Vec3Arg inPosition, float inFractionHint) const override;
+
+	// See PathConstraintPath::GetPointOnPath
+	virtual void		GetPointOnPath(float inFraction, Vec3 &outPathPosition, Vec3 &outPathTangent, Vec3 &outPathNormal, Vec3 &outPathBinormal) const override;
+
+	/// Adds a point to the path
+	void				AddPoint(Vec3Arg inPosition, Vec3Arg inTangent, Vec3Arg inNormal)	{ mPoints.push_back({ inPosition, inTangent, inNormal}); }
+
+	// See: PathConstraintPath::SaveBinaryState
+	virtual void		SaveBinaryState(StreamOut &inStream) const override;
+
+	struct Point
+	{
+		JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, Point)
+
+		Vec3			mPosition;															///< Position on the path
+		Vec3			mTangent;															///< Tangent of the path, does not need to be normalized (in the direction of the path)
+		Vec3			mNormal;															///< Normal of the path (together with the tangent along the curve this forms a basis for the constraint)
+	};
+
+protected:
+	// See: PathConstraintPath::RestoreBinaryState
+	virtual void		RestoreBinaryState(StreamIn &inStream) override;
+
+private:
+	/// Helper function that returns the index of the path segment and the fraction t on the path segment based on the full path fraction
+	inline void			GetIndexAndT(float inFraction, int &outIndex, float &outT) const;
+
+	using Points = Array<Point>;
+
+	Points				mPoints;															///< Points on the Hermite spline
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Constraints/PointConstraint.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/Constraints/PointConstraint.h>
+#include <Jolt/Physics/Body/Body.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(PointConstraintSettings)
+{
+	JPH_ADD_BASE_CLASS(PointConstraintSettings, TwoBodyConstraintSettings)
+
+	JPH_ADD_ENUM_ATTRIBUTE(PointConstraintSettings, mSpace)
+	JPH_ADD_ATTRIBUTE(PointConstraintSettings, mPoint1)
+	JPH_ADD_ATTRIBUTE(PointConstraintSettings, mPoint2)
+}
+
+void PointConstraintSettings::SaveBinaryState(StreamOut &inStream) const
+{
+	ConstraintSettings::SaveBinaryState(inStream);
+
+	inStream.Write(mSpace);
+	inStream.Write(mPoint1);
+	inStream.Write(mPoint2);
+}
+
+void PointConstraintSettings::RestoreBinaryState(StreamIn &inStream)
+{
+	ConstraintSettings::RestoreBinaryState(inStream);
+
+	inStream.Read(mSpace);
+	inStream.Read(mPoint1);
+	inStream.Read(mPoint2);
+}
+
+TwoBodyConstraint *PointConstraintSettings::Create(Body &inBody1, Body &inBody2) const
+{
+	return new PointConstraint(inBody1, inBody2, *this);
+}
+
+PointConstraint::PointConstraint(Body &inBody1, Body &inBody2, const PointConstraintSettings &inSettings) :
+	TwoBodyConstraint(inBody1, inBody2, inSettings)
+{
+	if (inSettings.mSpace == EConstraintSpace::WorldSpace)
+	{
+		// If all properties were specified in world space, take them to local space now
+		mLocalSpacePosition1 = Vec3(inBody1.GetInverseCenterOfMassTransform() * inSettings.mPoint1);
+		mLocalSpacePosition2 = Vec3(inBody2.GetInverseCenterOfMassTransform() * inSettings.mPoint2);
+	}
+	else
+	{
+		mLocalSpacePosition1 = Vec3(inSettings.mPoint1);
+		mLocalSpacePosition2 = Vec3(inSettings.mPoint2);
+	}
+}
+
+void PointConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM)
+{
+	if (mBody1->GetID() == inBodyID)
+		mLocalSpacePosition1 -= inDeltaCOM;
+	else if (mBody2->GetID() == inBodyID)
+		mLocalSpacePosition2 -= inDeltaCOM;
+}
+
+void PointConstraint::SetPoint1(EConstraintSpace inSpace, RVec3Arg inPoint1)
+{
+	if (inSpace == EConstraintSpace::WorldSpace)
+		mLocalSpacePosition1 = Vec3(mBody1->GetInverseCenterOfMassTransform() * inPoint1);
+	else
+		mLocalSpacePosition1 = Vec3(inPoint1);
+}
+
+void PointConstraint::SetPoint2(EConstraintSpace inSpace, RVec3Arg inPoint2)
+{
+	if (inSpace == EConstraintSpace::WorldSpace)
+		mLocalSpacePosition2 = Vec3(mBody2->GetInverseCenterOfMassTransform() * inPoint2);
+	else
+		mLocalSpacePosition2 = Vec3(inPoint2);
+}
+
+void PointConstraint::CalculateConstraintProperties()
+{
+	mPointConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(mBody1->GetRotation()), mLocalSpacePosition1, *mBody2, Mat44::sRotation(mBody2->GetRotation()), mLocalSpacePosition2);
+}
+
+void PointConstraint::SetupVelocityConstraint(float inDeltaTime)
+{
+	CalculateConstraintProperties();
+}
+
+void PointConstraint::ResetWarmStart()
+{
+	mPointConstraintPart.Deactivate();
+}
+
+void PointConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio)
+{
+	// Warm starting: Apply previous frame impulse
+	mPointConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio);
+}
+
+bool PointConstraint::SolveVelocityConstraint(float inDeltaTime)
+{
+	return mPointConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2);
+}
+
+bool PointConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte)
+{
+	// Update constraint properties (bodies may have moved)
+	CalculateConstraintProperties();
+
+	return mPointConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, inBaumgarte);
+}
+
+#ifdef JPH_DEBUG_RENDERER
+void PointConstraint::DrawConstraint(DebugRenderer *inRenderer) const
+{
+	// Draw constraint
+	inRenderer->DrawMarker(mBody1->GetCenterOfMassTransform() * mLocalSpacePosition1, Color::sRed, 0.1f);
+	inRenderer->DrawMarker(mBody2->GetCenterOfMassTransform() * mLocalSpacePosition2, Color::sGreen, 0.1f);
+}
+#endif // JPH_DEBUG_RENDERER
+
+void PointConstraint::SaveState(StateRecorder &inStream) const
+{
+	TwoBodyConstraint::SaveState(inStream);
+
+	mPointConstraintPart.SaveState(inStream);
+}
+
+void PointConstraint::RestoreState(StateRecorder &inStream)
+{
+	TwoBodyConstraint::RestoreState(inStream);
+
+	mPointConstraintPart.RestoreState(inStream);
+}
+
+Ref<ConstraintSettings> PointConstraint::GetConstraintSettings() const
+{
+	PointConstraintSettings *settings = new PointConstraintSettings;
+	ToConstraintSettings(*settings);
+	settings->mSpace = EConstraintSpace::LocalToBodyCOM;
+	settings->mPoint1 = RVec3(mLocalSpacePosition1);
+	settings->mPoint2 = RVec3(mLocalSpacePosition2);
+	return settings;
+}
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Constraints/PointConstraint.h

@@ -0,0 +1,94 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Constraints/TwoBodyConstraint.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/PointConstraintPart.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Point constraint settings, used to create a point constraint
+class JPH_EXPORT PointConstraintSettings final : public TwoBodyConstraintSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, PointConstraintSettings)
+
+public:
+	// See: ConstraintSettings::SaveBinaryState
+	virtual void				SaveBinaryState(StreamOut &inStream) const override;
+
+	/// Create an instance of this constraint
+	virtual TwoBodyConstraint *	Create(Body &inBody1, Body &inBody2) const override;
+
+	/// This determines in which space the constraint is setup, all properties below should be in the specified space
+	EConstraintSpace			mSpace = EConstraintSpace::WorldSpace;
+
+	/// Body 1 constraint position (space determined by mSpace).
+	RVec3						mPoint1 = RVec3::sZero();
+
+	/// Body 2 constraint position (space determined by mSpace).
+	/// Note: Normally you would set mPoint1 = mPoint2 if the bodies are already placed how you want to constrain them (if mSpace = world space).
+	RVec3						mPoint2 = RVec3::sZero();
+
+protected:
+	// See: ConstraintSettings::RestoreBinaryState
+	virtual void				RestoreBinaryState(StreamIn &inStream) override;
+};
+
+/// A point constraint constrains 2 bodies on a single point (removing 3 degrees of freedom)
+class JPH_EXPORT PointConstraint final : public TwoBodyConstraint
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Construct point constraint
+								PointConstraint(Body &inBody1, Body &inBody2, const PointConstraintSettings &inSettings);
+
+	// Generic interface of a constraint
+	virtual EConstraintSubType	GetSubType() const override									{ return EConstraintSubType::Point; }
+	virtual void				NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override;
+	virtual void				SetupVelocityConstraint(float inDeltaTime) override;
+	virtual void				ResetWarmStart() override;
+	virtual void				WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override;
+	virtual bool				SolveVelocityConstraint(float inDeltaTime) override;
+	virtual bool				SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override;
+#ifdef JPH_DEBUG_RENDERER
+	virtual void				DrawConstraint(DebugRenderer *inRenderer) const override;
+#endif // JPH_DEBUG_RENDERER
+	virtual void				SaveState(StateRecorder &inStream) const override;
+	virtual void				RestoreState(StateRecorder &inStream) override;
+	virtual Ref<ConstraintSettings> GetConstraintSettings() const override;
+
+	/// Update the attachment point for body 1
+	void						SetPoint1(EConstraintSpace inSpace, RVec3Arg inPoint1);
+
+	/// Update the attachment point for body 2
+	void						SetPoint2(EConstraintSpace inSpace, RVec3Arg inPoint2);
+
+	/// Get the attachment point for body 1 relative to body 1 COM (transform by Body::GetCenterOfMassTransform to take to world space)
+	inline Vec3					GetLocalSpacePoint1() const									{ return mLocalSpacePosition1; }
+
+	/// Get the attachment point for body 2 relative to body 2 COM (transform by Body::GetCenterOfMassTransform to take to world space)
+	inline Vec3					GetLocalSpacePoint2() const									{ return mLocalSpacePosition2; }
+
+	// See: TwoBodyConstraint
+	virtual Mat44				GetConstraintToBody1Matrix() const override					{ return Mat44::sTranslation(mLocalSpacePosition1); }
+	virtual Mat44				GetConstraintToBody2Matrix() const override					{ return Mat44::sTranslation(mLocalSpacePosition2); } // Note: Incorrect rotation as we don't track the original rotation difference, should not matter though as the constraint is not limiting rotation.
+
+	///@name Get Lagrange multiplier from last physics update (the linear impulse applied to satisfy the constraint)
+	inline Vec3					GetTotalLambdaPosition() const								{ return mPointConstraintPart.GetTotalLambda(); }
+
+private:
+	// Internal helper function to calculate the values below
+	void						CalculateConstraintProperties();
+
+	// Local space constraint positions
+	Vec3						mLocalSpacePosition1;
+	Vec3						mLocalSpacePosition2;
+
+	// The constraint part
+	PointConstraintPart			mPointConstraintPart;
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Constraints/PulleyConstraint.cpp

@@ -0,0 +1,253 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2022 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Constraints/PulleyConstraint.h>
+#include <Jolt/Physics/Body/Body.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
+
+using namespace literals;
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(PulleyConstraintSettings)
+{
+	JPH_ADD_BASE_CLASS(PulleyConstraintSettings, TwoBodyConstraintSettings)
+
+	JPH_ADD_ENUM_ATTRIBUTE(PulleyConstraintSettings, mSpace)
+	JPH_ADD_ATTRIBUTE(PulleyConstraintSettings, mBodyPoint1)
+	JPH_ADD_ATTRIBUTE(PulleyConstraintSettings, mFixedPoint1)
+	JPH_ADD_ATTRIBUTE(PulleyConstraintSettings, mBodyPoint2)
+	JPH_ADD_ATTRIBUTE(PulleyConstraintSettings, mFixedPoint2)
+	JPH_ADD_ATTRIBUTE(PulleyConstraintSettings, mRatio)
+	JPH_ADD_ATTRIBUTE(PulleyConstraintSettings, mMinLength)
+	JPH_ADD_ATTRIBUTE(PulleyConstraintSettings, mMaxLength)
+}
+
+void PulleyConstraintSettings::SaveBinaryState(StreamOut &inStream) const
+{
+	ConstraintSettings::SaveBinaryState(inStream);
+
+	inStream.Write(mSpace);
+	inStream.Write(mBodyPoint1);
+	inStream.Write(mFixedPoint1);
+	inStream.Write(mBodyPoint2);
+	inStream.Write(mFixedPoint2);
+	inStream.Write(mRatio);
+	inStream.Write(mMinLength);
+	inStream.Write(mMaxLength);
+}
+
+void PulleyConstraintSettings::RestoreBinaryState(StreamIn &inStream)
+{
+	ConstraintSettings::RestoreBinaryState(inStream);
+
+	inStream.Read(mSpace);
+	inStream.Read(mBodyPoint1);
+	inStream.Read(mFixedPoint1);
+	inStream.Read(mBodyPoint2);
+	inStream.Read(mFixedPoint2);
+	inStream.Read(mRatio);
+	inStream.Read(mMinLength);
+	inStream.Read(mMaxLength);
+}
+
+TwoBodyConstraint *PulleyConstraintSettings::Create(Body &inBody1, Body &inBody2) const
+{
+	return new PulleyConstraint(inBody1, inBody2, *this);
+}
+
+PulleyConstraint::PulleyConstraint(Body &inBody1, Body &inBody2, const PulleyConstraintSettings &inSettings) :
+	TwoBodyConstraint(inBody1, inBody2, inSettings),
+	mFixedPosition1(inSettings.mFixedPoint1),
+	mFixedPosition2(inSettings.mFixedPoint2),
+	mRatio(inSettings.mRatio),
+	mMinLength(inSettings.mMinLength),
+	mMaxLength(inSettings.mMaxLength)
+{
+	if (inSettings.mSpace == EConstraintSpace::WorldSpace)
+	{
+		// If all properties were specified in world space, take them to local space now
+		mLocalSpacePosition1 = Vec3(inBody1.GetInverseCenterOfMassTransform() * inSettings.mBodyPoint1);
+		mLocalSpacePosition2 = Vec3(inBody2.GetInverseCenterOfMassTransform() * inSettings.mBodyPoint2);
+		mWorldSpacePosition1 = inSettings.mBodyPoint1;
+		mWorldSpacePosition2 = inSettings.mBodyPoint2;
+	}
+	else
+	{
+		// If properties were specified in local space, we need to calculate world space positions
+		mLocalSpacePosition1 = Vec3(inSettings.mBodyPoint1);
+		mLocalSpacePosition2 = Vec3(inSettings.mBodyPoint2);
+		mWorldSpacePosition1 = inBody1.GetCenterOfMassTransform() * inSettings.mBodyPoint1;
+		mWorldSpacePosition2 = inBody2.GetCenterOfMassTransform() * inSettings.mBodyPoint2;
+	}
+
+	// Calculate min/max length if it was not provided
+	float current_length = GetCurrentLength();
+	if (mMinLength < 0.0f)
+		mMinLength = current_length;
+	if (mMaxLength < 0.0f)
+		mMaxLength = current_length;
+
+	// Initialize the normals to a likely valid axis in case the fixed points overlap with the attachment points (most likely the fixed points are above both bodies)
+	mWorldSpaceNormal1 = mWorldSpaceNormal2 = -Vec3::sAxisY();
+}
+
+void PulleyConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM)
+{
+	if (mBody1->GetID() == inBodyID)
+		mLocalSpacePosition1 -= inDeltaCOM;
+	else if (mBody2->GetID() == inBodyID)
+		mLocalSpacePosition2 -= inDeltaCOM;
+}
+
+float PulleyConstraint::CalculatePositionsNormalsAndLength()
+{
+	// Update world space positions (the bodies may have moved)
+	mWorldSpacePosition1 = mBody1->GetCenterOfMassTransform() * mLocalSpacePosition1;
+	mWorldSpacePosition2 = mBody2->GetCenterOfMassTransform() * mLocalSpacePosition2;
+
+	// Calculate world space normals
+	Vec3 delta1 = Vec3(mWorldSpacePosition1 - mFixedPosition1);
+	float delta1_len = delta1.Length();
+	if (delta1_len > 0.0f)
+		mWorldSpaceNormal1 = delta1 / delta1_len;
+
+	Vec3 delta2 = Vec3(mWorldSpacePosition2 - mFixedPosition2);
+	float delta2_len = delta2.Length();
+	if (delta2_len > 0.0f)
+		mWorldSpaceNormal2 = delta2 / delta2_len;
+
+	// Calculate length
+	return delta1_len + mRatio * delta2_len;
+}
+
+void PulleyConstraint::CalculateConstraintProperties()
+{
+	// Calculate attachment points relative to COM
+	Vec3 r1 = Vec3(mWorldSpacePosition1 - mBody1->GetCenterOfMassPosition());
+	Vec3 r2 = Vec3(mWorldSpacePosition2 - mBody2->GetCenterOfMassPosition());
+
+	mIndependentAxisConstraintPart.CalculateConstraintProperties(*mBody1, *mBody2, r1, mWorldSpaceNormal1, r2, mWorldSpaceNormal2, mRatio);
+}
+
+void PulleyConstraint::SetupVelocityConstraint(float inDeltaTime)
+{
+	// Determine if the constraint is active
+	float current_length = CalculatePositionsNormalsAndLength();
+	bool min_length_violation = current_length <= mMinLength;
+	bool max_length_violation = current_length >= mMaxLength;
+	if (min_length_violation || max_length_violation)
+	{
+		// Determine max lambda based on if the length is too big or small
+		mMinLambda = max_length_violation? -FLT_MAX : 0.0f;
+		mMaxLambda = min_length_violation? FLT_MAX : 0.0f;
+
+		CalculateConstraintProperties();
+	}
+	else
+		mIndependentAxisConstraintPart.Deactivate();
+}
+
+void PulleyConstraint::ResetWarmStart()
+{
+	mIndependentAxisConstraintPart.Deactivate();
+}
+
+void PulleyConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio)
+{
+	mIndependentAxisConstraintPart.WarmStart(*mBody1, *mBody2, mWorldSpaceNormal1, mWorldSpaceNormal2, mRatio, inWarmStartImpulseRatio);
+}
+
+bool PulleyConstraint::SolveVelocityConstraint(float inDeltaTime)
+{
+	if (mIndependentAxisConstraintPart.IsActive())
+		return mIndependentAxisConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mWorldSpaceNormal1, mWorldSpaceNormal2, mRatio, mMinLambda, mMaxLambda);
+	else
+		return false;
+}
+
+bool PulleyConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte)
+{
+	// Calculate new length (bodies may have changed)
+	float current_length = CalculatePositionsNormalsAndLength();
+
+	float position_error = 0.0f;
+	if (current_length < mMinLength)
+		position_error = current_length - mMinLength;
+	else if (current_length > mMaxLength)
+		position_error = current_length - mMaxLength;
+
+	if (position_error != 0.0f)
+	{
+		// Update constraint properties (bodies may have moved)
+		CalculateConstraintProperties();
+
+		return mIndependentAxisConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mWorldSpaceNormal1, mWorldSpaceNormal2, mRatio, position_error, inBaumgarte);
+	}
+
+	return false;
+}
+
+#ifdef JPH_DEBUG_RENDERER
+void PulleyConstraint::DrawConstraint(DebugRenderer *inRenderer) const
+{
+	// Color according to length vs min/max length
+	float current_length = GetCurrentLength();
+	Color color = Color::sGreen;
+	if (current_length < mMinLength)
+		color = Color::sYellow;
+	else if (current_length > mMaxLength)
+		color = Color::sRed;
+
+	// Draw constraint
+	inRenderer->DrawLine(mWorldSpacePosition1, mFixedPosition1, color);
+	inRenderer->DrawLine(mFixedPosition1, mFixedPosition2, color);
+	inRenderer->DrawLine(mFixedPosition2, mWorldSpacePosition2, color);
+
+	// Draw current length
+	inRenderer->DrawText3D(0.5_r * (mFixedPosition1 + mFixedPosition2), StringFormat("%.2f", (double)current_length));
+}
+#endif // JPH_DEBUG_RENDERER
+
+void PulleyConstraint::SaveState(StateRecorder &inStream) const
+{
+	TwoBodyConstraint::SaveState(inStream);
+
+	mIndependentAxisConstraintPart.SaveState(inStream);
+	inStream.Write(mWorldSpaceNormal1); // When distance to fixed point = 0, the normal is used from last frame so we need to store it
+	inStream.Write(mWorldSpaceNormal2);
+}
+
+void PulleyConstraint::RestoreState(StateRecorder &inStream)
+{
+	TwoBodyConstraint::RestoreState(inStream);
+
+	mIndependentAxisConstraintPart.RestoreState(inStream);
+	inStream.Read(mWorldSpaceNormal1);
+	inStream.Read(mWorldSpaceNormal2);
+}
+
+Ref<ConstraintSettings> PulleyConstraint::GetConstraintSettings() const
+{
+	PulleyConstraintSettings *settings = new PulleyConstraintSettings;
+	ToConstraintSettings(*settings);
+	settings->mSpace = EConstraintSpace::LocalToBodyCOM;
+	settings->mBodyPoint1 = RVec3(mLocalSpacePosition1);
+	settings->mFixedPoint1 = mFixedPosition1;
+	settings->mBodyPoint2 = RVec3(mLocalSpacePosition2);
+	settings->mFixedPoint2 = mFixedPosition2;
+	settings->mRatio = mRatio;
+	settings->mMinLength = mMinLength;
+	settings->mMaxLength = mMaxLength;
+	return settings;
+}
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Constraints/PulleyConstraint.h

@@ -0,0 +1,137 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2022 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Constraints/TwoBodyConstraint.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/IndependentAxisConstraintPart.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Pulley constraint settings, used to create a pulley constraint.
+/// A pulley connects two bodies via two fixed world points to each other similar to a distance constraint.
+/// We define Length1 = |BodyPoint1 - FixedPoint1| where Body1 is a point on body 1 in world space and FixedPoint1 a fixed point in world space
+/// Length2 = |BodyPoint2 - FixedPoint2|
+/// The constraint keeps the two line segments constrained so that
+/// MinDistance <= Length1 + Ratio * Length2 <= MaxDistance
+class JPH_EXPORT PulleyConstraintSettings final : public TwoBodyConstraintSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, PulleyConstraintSettings)
+
+public:
+	// See: ConstraintSettings::SaveBinaryState
+	virtual void				SaveBinaryState(StreamOut &inStream) const override;
+
+	/// Create an instance of this constraint
+	virtual TwoBodyConstraint *	Create(Body &inBody1, Body &inBody2) const override;
+
+	/// This determines in which space the constraint is setup, specified properties below should be in the specified space
+	EConstraintSpace			mSpace = EConstraintSpace::WorldSpace;
+
+	/// Body 1 constraint attachment point (space determined by mSpace).
+	RVec3						mBodyPoint1 = RVec3::sZero();
+
+	/// Fixed world point to which body 1 is connected (always world space)
+	RVec3						mFixedPoint1 = RVec3::sZero();
+
+	/// Body 2 constraint attachment point (space determined by mSpace)
+	RVec3						mBodyPoint2 = RVec3::sZero();
+
+	/// Fixed world point to which body 2 is connected (always world space)
+	RVec3						mFixedPoint2 = RVec3::sZero();
+
+	/// Ratio between the two line segments (see formula above), can be used to create a block and tackle
+	float						mRatio = 1.0f;
+
+	/// The minimum length of the line segments (see formula above), use -1 to calculate the length based on the positions of the objects when the constraint is created.
+	float						mMinLength = 0.0f;
+
+	/// The maximum length of the line segments (see formula above), use -1 to calculate the length based on the positions of the objects when the constraint is created.
+	float						mMaxLength = -1.0f;
+
+protected:
+	// See: ConstraintSettings::RestoreBinaryState
+	virtual void				RestoreBinaryState(StreamIn &inStream) override;
+};
+
+/// A pulley constraint.
+class JPH_EXPORT PulleyConstraint final : public TwoBodyConstraint
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Construct distance constraint
+								PulleyConstraint(Body &inBody1, Body &inBody2, const PulleyConstraintSettings &inSettings);
+
+	// Generic interface of a constraint
+	virtual EConstraintSubType	GetSubType() const override									{ return EConstraintSubType::Pulley; }
+	virtual void				NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override;
+	virtual void				SetupVelocityConstraint(float inDeltaTime) override;
+	virtual void				ResetWarmStart() override;
+	virtual void				WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override;
+	virtual bool				SolveVelocityConstraint(float inDeltaTime) override;
+	virtual bool				SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override;
+#ifdef JPH_DEBUG_RENDERER
+	virtual void				DrawConstraint(DebugRenderer *inRenderer) const override;
+#endif // JPH_DEBUG_RENDERER
+	virtual void				SaveState(StateRecorder &inStream) const override;
+	virtual void				RestoreState(StateRecorder &inStream) override;
+	virtual Ref<ConstraintSettings> GetConstraintSettings() const override;
+
+	// See: TwoBodyConstraint
+	virtual Mat44				GetConstraintToBody1Matrix() const override					{ return Mat44::sTranslation(mLocalSpacePosition1); }
+	virtual Mat44				GetConstraintToBody2Matrix() const override					{ return Mat44::sTranslation(mLocalSpacePosition2); } // Note: Incorrect rotation as we don't track the original rotation difference, should not matter though as the constraint is not limiting rotation.
+
+	/// Update the minimum and maximum length for the constraint
+	void						SetLength(float inMinLength, float inMaxLength)				{ JPH_ASSERT(inMinLength >= 0.0f && inMinLength <= inMaxLength); mMinLength = inMinLength; mMaxLength = inMaxLength; }
+	float						GetMinLength() const										{ return mMinLength; }
+	float						GetMaxLength() const										{ return mMaxLength; }
+
+	/// Get the current length of both segments (multiplied by the ratio for segment 2)
+	float						GetCurrentLength() const									{ return Vec3(mWorldSpacePosition1 - mFixedPosition1).Length() + mRatio * Vec3(mWorldSpacePosition2 - mFixedPosition2).Length(); }
+
+	///@name Get Lagrange multiplier from last physics update (the linear impulse applied to satisfy the constraint)
+	inline float				GetTotalLambdaPosition() const								{ return mIndependentAxisConstraintPart.GetTotalLambda(); }
+
+private:
+	// Calculates world positions and normals and returns current length
+	float						CalculatePositionsNormalsAndLength();
+
+	// Internal helper function to calculate the values below
+	void						CalculateConstraintProperties();
+
+	// CONFIGURATION PROPERTIES FOLLOW
+
+	// Local space constraint positions on the bodies
+	Vec3						mLocalSpacePosition1;
+	Vec3						mLocalSpacePosition2;
+
+	// World space fixed positions
+	RVec3						mFixedPosition1;
+	RVec3						mFixedPosition2;
+
+	/// Ratio between the two line segments
+	float						mRatio;
+
+	// The minimum/maximum length of the line segments
+	float						mMinLength;
+	float						mMaxLength;
+
+	// RUN TIME PROPERTIES FOLLOW
+
+	// World space positions and normal
+	RVec3						mWorldSpacePosition1;
+	RVec3						mWorldSpacePosition2;
+	Vec3						mWorldSpaceNormal1;
+	Vec3						mWorldSpaceNormal2;
+
+	// Depending on if the length < min or length > max we can apply forces to prevent further violations
+	float						mMinLambda;
+	float						mMaxLambda;
+
+	// The constraint part
+	IndependentAxisConstraintPart mIndependentAxisConstraintPart;
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Constraints/RackAndPinionConstraint.cpp

@@ -0,0 +1,189 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Constraints/RackAndPinionConstraint.h>
+#include <Jolt/Physics/Constraints/HingeConstraint.h>
+#include <Jolt/Physics/Constraints/SliderConstraint.h>
+#include <Jolt/Physics/Body/Body.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(RackAndPinionConstraintSettings)
+{
+	JPH_ADD_BASE_CLASS(RackAndPinionConstraintSettings, TwoBodyConstraintSettings)
+
+	JPH_ADD_ENUM_ATTRIBUTE(RackAndPinionConstraintSettings, mSpace)
+	JPH_ADD_ATTRIBUTE(RackAndPinionConstraintSettings, mHingeAxis)
+	JPH_ADD_ATTRIBUTE(RackAndPinionConstraintSettings, mSliderAxis)
+	JPH_ADD_ATTRIBUTE(RackAndPinionConstraintSettings, mRatio)
+}
+
+void RackAndPinionConstraintSettings::SaveBinaryState(StreamOut &inStream) const
+{
+	ConstraintSettings::SaveBinaryState(inStream);
+
+	inStream.Write(mSpace);
+	inStream.Write(mHingeAxis);
+	inStream.Write(mSliderAxis);
+	inStream.Write(mRatio);
+}
+
+void RackAndPinionConstraintSettings::RestoreBinaryState(StreamIn &inStream)
+{
+	ConstraintSettings::RestoreBinaryState(inStream);
+
+	inStream.Read(mSpace);
+	inStream.Read(mHingeAxis);
+	inStream.Read(mSliderAxis);
+	inStream.Read(mRatio);
+}
+
+TwoBodyConstraint *RackAndPinionConstraintSettings::Create(Body &inBody1, Body &inBody2) const
+{
+	return new RackAndPinionConstraint(inBody1, inBody2, *this);
+}
+
+RackAndPinionConstraint::RackAndPinionConstraint(Body &inBody1, Body &inBody2, const RackAndPinionConstraintSettings &inSettings) :
+	TwoBodyConstraint(inBody1, inBody2, inSettings),
+	mLocalSpaceHingeAxis(inSettings.mHingeAxis),
+	mLocalSpaceSliderAxis(inSettings.mSliderAxis),
+	mRatio(inSettings.mRatio)
+{
+	if (inSettings.mSpace == EConstraintSpace::WorldSpace)
+	{
+		// If all properties were specified in world space, take them to local space now
+		mLocalSpaceHingeAxis = inBody1.GetInverseCenterOfMassTransform().Multiply3x3(mLocalSpaceHingeAxis).Normalized();
+		mLocalSpaceSliderAxis = inBody2.GetInverseCenterOfMassTransform().Multiply3x3(mLocalSpaceSliderAxis).Normalized();
+	}
+}
+
+void RackAndPinionConstraint::CalculateConstraintProperties(Mat44Arg inRotation1, Mat44Arg inRotation2)
+{
+	// Calculate world space normals
+	mWorldSpaceHingeAxis = inRotation1 * mLocalSpaceHingeAxis;
+	mWorldSpaceSliderAxis = inRotation2 * mLocalSpaceSliderAxis;
+
+	mRackAndPinionConstraintPart.CalculateConstraintProperties(*mBody1, mWorldSpaceHingeAxis, *mBody2, mWorldSpaceSliderAxis, mRatio);
+}
+
+void RackAndPinionConstraint::SetupVelocityConstraint(float inDeltaTime)
+{
+	// Calculate constraint properties that are constant while bodies don't move
+	Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation());
+	Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation());
+	CalculateConstraintProperties(rotation1, rotation2);
+}
+
+void RackAndPinionConstraint::ResetWarmStart()
+{
+	mRackAndPinionConstraintPart.Deactivate();
+}
+
+void RackAndPinionConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio)
+{
+	// Warm starting: Apply previous frame impulse
+	mRackAndPinionConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio);
+}
+
+bool RackAndPinionConstraint::SolveVelocityConstraint(float inDeltaTime)
+{
+	return mRackAndPinionConstraintPart.SolveVelocityConstraint(*mBody1, mWorldSpaceHingeAxis, *mBody2, mWorldSpaceSliderAxis, mRatio);
+}
+
+bool RackAndPinionConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte)
+{
+	if (mRackConstraint == nullptr || mPinionConstraint == nullptr)
+		return false;
+
+	float rotation;
+	if (mPinionConstraint->GetSubType() == EConstraintSubType::Hinge)
+	{
+		rotation = StaticCast<HingeConstraint>(mPinionConstraint)->GetCurrentAngle();
+	}
+	else
+	{
+		JPH_ASSERT(false, "Unsupported");
+		return false;
+	}
+
+	float translation;
+	if (mRackConstraint->GetSubType() == EConstraintSubType::Slider)
+	{
+		translation = StaticCast<SliderConstraint>(mRackConstraint)->GetCurrentPosition();
+	}
+	else
+	{
+		JPH_ASSERT(false, "Unsupported");
+		return false;
+	}
+
+	float error = CenterAngleAroundZero(fmod(rotation - mRatio * translation, 2.0f * JPH_PI));
+	if (error == 0.0f)
+		return false;
+
+	Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation());
+	Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation());
+	CalculateConstraintProperties(rotation1, rotation2);
+	return mRackAndPinionConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, error, inBaumgarte);
+}
+
+#ifdef JPH_DEBUG_RENDERER
+void RackAndPinionConstraint::DrawConstraint(DebugRenderer *inRenderer) const
+{
+	RMat44 transform1 = mBody1->GetCenterOfMassTransform();
+	RMat44 transform2 = mBody2->GetCenterOfMassTransform();
+
+	// Draw constraint axis
+	inRenderer->DrawArrow(transform1.GetTranslation(), transform1 * mLocalSpaceHingeAxis, Color::sGreen, 0.01f);
+	inRenderer->DrawArrow(transform2.GetTranslation(), transform2 * mLocalSpaceSliderAxis, Color::sBlue, 0.01f);
+}
+
+#endif // JPH_DEBUG_RENDERER
+
+void RackAndPinionConstraint::SaveState(StateRecorder &inStream) const
+{
+	TwoBodyConstraint::SaveState(inStream);
+
+	mRackAndPinionConstraintPart.SaveState(inStream);
+}
+
+void RackAndPinionConstraint::RestoreState(StateRecorder &inStream)
+{
+	TwoBodyConstraint::RestoreState(inStream);
+
+	mRackAndPinionConstraintPart.RestoreState(inStream);
+}
+
+Ref<ConstraintSettings> RackAndPinionConstraint::GetConstraintSettings() const
+{
+	RackAndPinionConstraintSettings *settings = new RackAndPinionConstraintSettings;
+	ToConstraintSettings(*settings);
+	settings->mSpace = EConstraintSpace::LocalToBodyCOM;
+	settings->mHingeAxis = mLocalSpaceHingeAxis;
+	settings->mSliderAxis = mLocalSpaceSliderAxis;
+	settings->mRatio = mRatio;
+	return settings;
+}
+
+Mat44 RackAndPinionConstraint::GetConstraintToBody1Matrix() const
+{
+	Vec3 perp = mLocalSpaceHingeAxis.GetNormalizedPerpendicular();
+	return Mat44(Vec4(mLocalSpaceHingeAxis, 0), Vec4(perp, 0), Vec4(mLocalSpaceHingeAxis.Cross(perp), 0), Vec4(0, 0, 0, 1));
+}
+
+Mat44 RackAndPinionConstraint::GetConstraintToBody2Matrix() const
+{
+	Vec3 perp = mLocalSpaceSliderAxis.GetNormalizedPerpendicular();
+	return Mat44(Vec4(mLocalSpaceSliderAxis, 0), Vec4(perp, 0), Vec4(mLocalSpaceSliderAxis.Cross(perp), 0), Vec4(0, 0, 0, 1));
+}
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Constraints/RackAndPinionConstraint.h

@@ -0,0 +1,118 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Constraints/TwoBodyConstraint.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/RackAndPinionConstraintPart.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Rack and pinion constraint (slider & gear) settings
+class JPH_EXPORT RackAndPinionConstraintSettings final : public TwoBodyConstraintSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, RackAndPinionConstraintSettings)
+
+public:
+	// See: ConstraintSettings::SaveBinaryState
+	virtual void				SaveBinaryState(StreamOut &inStream) const override;
+
+	/// Create an instance of this constraint.
+	/// Body1 should be the pinion (gear) and body 2 the rack (slider).
+	virtual TwoBodyConstraint *	Create(Body &inBody1, Body &inBody2) const override;
+
+	/// Defines the ratio between the rotation of the pinion and the translation of the rack.
+	/// The ratio is defined as: PinionRotation(t) = ratio * RackTranslation(t)
+	/// @param inNumTeethRack Number of teeth that the rack has
+	/// @param inRackLength Length of the rack
+	/// @param inNumTeethPinion Number of teeth the pinion has
+	void						SetRatio(int inNumTeethRack, float inRackLength, int inNumTeethPinion)
+	{
+		mRatio = 2.0f * JPH_PI * inNumTeethRack / (inRackLength * inNumTeethPinion);
+	}
+
+	/// This determines in which space the constraint is setup, all properties below should be in the specified space
+	EConstraintSpace			mSpace = EConstraintSpace::WorldSpace;
+
+	/// Body 1 (pinion) constraint reference frame (space determined by mSpace).
+	Vec3						mHingeAxis = Vec3::sAxisX();
+
+	/// Body 2 (rack) constraint reference frame (space determined by mSpace)
+	Vec3						mSliderAxis = Vec3::sAxisX();
+
+	/// Ratio between the rack and pinion, see SetRatio.
+	float						mRatio = 1.0f;
+
+protected:
+	// See: ConstraintSettings::RestoreBinaryState
+	virtual void				RestoreBinaryState(StreamIn &inStream) override;
+};
+
+/// A rack and pinion constraint constrains the rotation of body1 to the translation of body 2.
+/// Note that this constraint needs to be used in conjunction with a hinge constraint for body 1 and a slider constraint for body 2.
+class JPH_EXPORT RackAndPinionConstraint final : public TwoBodyConstraint
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Construct gear constraint
+								RackAndPinionConstraint(Body &inBody1, Body &inBody2, const RackAndPinionConstraintSettings &inSettings);
+
+	// Generic interface of a constraint
+	virtual EConstraintSubType	GetSubType() const override												{ return EConstraintSubType::RackAndPinion; }
+	virtual void				NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override { /* Nothing */ }
+	virtual void				SetupVelocityConstraint(float inDeltaTime) override;
+	virtual void				ResetWarmStart() override;
+	virtual void				WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override;
+	virtual bool				SolveVelocityConstraint(float inDeltaTime) override;
+	virtual bool				SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override;
+#ifdef JPH_DEBUG_RENDERER
+	virtual void				DrawConstraint(DebugRenderer *inRenderer) const override;
+#endif // JPH_DEBUG_RENDERER
+	virtual void				SaveState(StateRecorder &inStream) const override;
+	virtual void				RestoreState(StateRecorder &inStream) override;
+	virtual Ref<ConstraintSettings> GetConstraintSettings() const override;
+
+	// See: TwoBodyConstraint
+	virtual Mat44				GetConstraintToBody1Matrix() const override;
+	virtual Mat44				GetConstraintToBody2Matrix() const override;
+
+	/// The constraints that constrain the rack and pinion (a slider and a hinge), optional and used to calculate the position error and fix numerical drift.
+	void						SetConstraints(const Constraint *inPinion, const Constraint *inRack)	{ mPinionConstraint = inPinion; mRackConstraint = inRack; }
+
+	///@name Get Lagrange multiplier from last physics update (the linear/angular impulse applied to satisfy the constraint)
+	inline float				GetTotalLambda() const													{ return mRackAndPinionConstraintPart.GetTotalLambda(); }
+
+private:
+	// Internal helper function to calculate the values below
+	void						CalculateConstraintProperties(Mat44Arg inRotation1, Mat44Arg inRotation2);
+
+	// CONFIGURATION PROPERTIES FOLLOW
+
+	// Local space hinge axis
+	Vec3						mLocalSpaceHingeAxis;
+
+	// Local space sliding direction
+	Vec3						mLocalSpaceSliderAxis;
+
+	// Ratio between rack and pinion
+	float						mRatio;
+
+	// The constraints that constrain the rack and pinion (a slider and a hinge), optional and used to calculate the position error and fix numerical drift.
+	RefConst<Constraint>		mPinionConstraint;
+	RefConst<Constraint>		mRackConstraint;
+
+	// RUN TIME PROPERTIES FOLLOW
+
+	// World space hinge axis
+	Vec3						mWorldSpaceHingeAxis;
+
+	// World space sliding direction
+	Vec3						mWorldSpaceSliderAxis;
+
+	// The constraint parts
+	RackAndPinionConstraintPart	mRackAndPinionConstraintPart;
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Constraints/SixDOFConstraint.cpp

@@ -0,0 +1,900 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Constraints/SixDOFConstraint.h>
+#include <Jolt/Physics/Body/Body.h>
+#include <Jolt/Geometry/Ellipse.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(SixDOFConstraintSettings)
+{
+	JPH_ADD_BASE_CLASS(SixDOFConstraintSettings, TwoBodyConstraintSettings)
+
+	JPH_ADD_ENUM_ATTRIBUTE(SixDOFConstraintSettings, mSpace)
+	JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mPosition1)
+	JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mAxisX1)
+	JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mAxisY1)
+	JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mPosition2)
+	JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mAxisX2)
+	JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mAxisY2)
+	JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mMaxFriction)
+	JPH_ADD_ENUM_ATTRIBUTE(SixDOFConstraintSettings, mSwingType)
+	JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mLimitMin)
+	JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mLimitMax)
+	JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mLimitsSpringSettings)
+	JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mMotorSettings)
+}
+
+void SixDOFConstraintSettings::SaveBinaryState(StreamOut &inStream) const
+{
+	ConstraintSettings::SaveBinaryState(inStream);
+
+	inStream.Write(mSpace);
+	inStream.Write(mPosition1);
+	inStream.Write(mAxisX1);
+	inStream.Write(mAxisY1);
+	inStream.Write(mPosition2);
+	inStream.Write(mAxisX2);
+	inStream.Write(mAxisY2);
+	inStream.Write(mMaxFriction);
+	inStream.Write(mSwingType);
+	inStream.Write(mLimitMin);
+	inStream.Write(mLimitMax);
+	for (const SpringSettings &s : mLimitsSpringSettings)
+		s.SaveBinaryState(inStream);
+	for (const MotorSettings &m : mMotorSettings)
+		m.SaveBinaryState(inStream);
+}
+
+void SixDOFConstraintSettings::RestoreBinaryState(StreamIn &inStream)
+{
+	ConstraintSettings::RestoreBinaryState(inStream);
+
+	inStream.Read(mSpace);
+	inStream.Read(mPosition1);
+	inStream.Read(mAxisX1);
+	inStream.Read(mAxisY1);
+	inStream.Read(mPosition2);
+	inStream.Read(mAxisX2);
+	inStream.Read(mAxisY2);
+	inStream.Read(mMaxFriction);
+	inStream.Read(mSwingType);
+	inStream.Read(mLimitMin);
+	inStream.Read(mLimitMax);
+	for (SpringSettings &s : mLimitsSpringSettings)
+		s.RestoreBinaryState(inStream);
+	for (MotorSettings &m : mMotorSettings)
+		m.RestoreBinaryState(inStream);
+}
+
+TwoBodyConstraint *SixDOFConstraintSettings::Create(Body &inBody1, Body &inBody2) const
+{
+	return new SixDOFConstraint(inBody1, inBody2, *this);
+}
+
+void SixDOFConstraint::UpdateTranslationLimits()
+{
+	// Set to zero if the limits are inversed
+	for (int i = EAxis::TranslationX; i <= EAxis::TranslationZ; ++i)
+		if (mLimitMin[i] > mLimitMax[i])
+			mLimitMin[i] = mLimitMax[i] = 0.0f;
+}
+
+void SixDOFConstraint::UpdateRotationLimits()
+{
+	if (mSwingTwistConstraintPart.GetSwingType() == ESwingType::Cone)
+	{
+		// Cone swing upper limit needs to be positive
+		mLimitMax[EAxis::RotationY] = max(0.0f, mLimitMax[EAxis::RotationY]);
+		mLimitMax[EAxis::RotationZ] = max(0.0f, mLimitMax[EAxis::RotationZ]);
+
+		// Cone swing limits only support symmetric ranges
+		mLimitMin[EAxis::RotationY] = -mLimitMax[EAxis::RotationY];
+		mLimitMin[EAxis::RotationZ] = -mLimitMax[EAxis::RotationZ];
+	}
+
+	for (int i = EAxis::RotationX; i <= EAxis::RotationZ; ++i)
+	{
+		// Clamp to [-PI, PI] range
+		mLimitMin[i] = Clamp(mLimitMin[i], -JPH_PI, JPH_PI);
+		mLimitMax[i] = Clamp(mLimitMax[i], -JPH_PI, JPH_PI);
+
+		// Set to zero if the limits are inversed
+		if (mLimitMin[i] > mLimitMax[i])
+			mLimitMin[i] = mLimitMax[i] = 0.0f;
+	}
+
+	// Pass limits on to constraint part
+	mSwingTwistConstraintPart.SetLimits(mLimitMin[EAxis::RotationX], mLimitMax[EAxis::RotationX], mLimitMin[EAxis::RotationY], mLimitMax[EAxis::RotationY], mLimitMin[EAxis::RotationZ], mLimitMax[EAxis::RotationZ]);
+}
+
+void SixDOFConstraint::UpdateFixedFreeAxis()
+{
+	uint8 old_free_axis = mFreeAxis;
+	uint8 old_fixed_axis = mFixedAxis;
+
+	// Cache which axis are fixed and which ones are free
+	mFreeAxis = 0;
+	mFixedAxis = 0;
+	for (int a = 0; a < EAxis::Num; ++a)
+	{
+		float limit = a >= EAxis::RotationX? JPH_PI : FLT_MAX;
+
+		if (mLimitMin[a] >= mLimitMax[a])
+			mFixedAxis |= 1 << a;
+		else if (mLimitMin[a] <= -limit && mLimitMax[a] >= limit)
+			mFreeAxis |= 1 << a;
+	}
+
+	// On change we deactivate all constraints to reset warm starting
+	if (old_free_axis != mFreeAxis || old_fixed_axis != mFixedAxis)
+	{
+		for (AxisConstraintPart &c : mTranslationConstraintPart)
+			c.Deactivate();
+		mPointConstraintPart.Deactivate();
+		mSwingTwistConstraintPart.Deactivate();
+		mRotationConstraintPart.Deactivate();
+		for (AxisConstraintPart &c : mMotorTranslationConstraintPart)
+			c.Deactivate();
+		for (AngleConstraintPart &c : mMotorRotationConstraintPart)
+			c.Deactivate();
+	}
+}
+
+SixDOFConstraint::SixDOFConstraint(Body &inBody1, Body &inBody2, const SixDOFConstraintSettings &inSettings) :
+	TwoBodyConstraint(inBody1, inBody2, inSettings)
+{
+	// Override swing type
+	mSwingTwistConstraintPart.SetSwingType(inSettings.mSwingType);
+
+	// Calculate rotation needed to go from constraint space to body1 local space
+	Vec3 axis_z1 = inSettings.mAxisX1.Cross(inSettings.mAxisY1);
+	Mat44 c_to_b1(Vec4(inSettings.mAxisX1, 0), Vec4(inSettings.mAxisY1, 0), Vec4(axis_z1, 0), Vec4(0, 0, 0, 1));
+	mConstraintToBody1 = c_to_b1.GetQuaternion();
+
+	// Calculate rotation needed to go from constraint space to body2 local space
+	Vec3 axis_z2 = inSettings.mAxisX2.Cross(inSettings.mAxisY2);
+	Mat44 c_to_b2(Vec4(inSettings.mAxisX2, 0), Vec4(inSettings.mAxisY2, 0), Vec4(axis_z2, 0), Vec4(0, 0, 0, 1));
+	mConstraintToBody2 = c_to_b2.GetQuaternion();
+
+	if (inSettings.mSpace == EConstraintSpace::WorldSpace)
+	{
+		// If all properties were specified in world space, take them to local space now
+		mLocalSpacePosition1 = Vec3(inBody1.GetInverseCenterOfMassTransform() * inSettings.mPosition1);
+		mConstraintToBody1 = inBody1.GetRotation().Conjugated() * mConstraintToBody1;
+
+		mLocalSpacePosition2 = Vec3(inBody2.GetInverseCenterOfMassTransform() * inSettings.mPosition2);
+		mConstraintToBody2 = inBody2.GetRotation().Conjugated() * mConstraintToBody2;
+	}
+	else
+	{
+		mLocalSpacePosition1 = Vec3(inSettings.mPosition1);
+		mLocalSpacePosition2 = Vec3(inSettings.mPosition2);
+	}
+
+	// Copy translation and rotation limits
+	memcpy(mLimitMin, inSettings.mLimitMin, sizeof(mLimitMin));
+	memcpy(mLimitMax, inSettings.mLimitMax, sizeof(mLimitMax));
+	memcpy(mLimitsSpringSettings, inSettings.mLimitsSpringSettings, sizeof(mLimitsSpringSettings));
+	UpdateTranslationLimits();
+	UpdateRotationLimits();
+	UpdateFixedFreeAxis();
+	CacheHasSpringLimits();
+
+	// Store friction settings
+	memcpy(mMaxFriction, inSettings.mMaxFriction, sizeof(mMaxFriction));
+
+	// Store motor settings
+	for (int i = 0; i < EAxis::Num; ++i)
+		mMotorSettings[i] = inSettings.mMotorSettings[i];
+
+	// Cache if motors are active (motors are off initially, but we may have friction)
+	CacheTranslationMotorActive();
+	CacheRotationMotorActive();
+}
+
+void SixDOFConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM)
+{
+	if (mBody1->GetID() == inBodyID)
+		mLocalSpacePosition1 -= inDeltaCOM;
+	else if (mBody2->GetID() == inBodyID)
+		mLocalSpacePosition2 -= inDeltaCOM;
+}
+
+void SixDOFConstraint::SetTranslationLimits(Vec3Arg inLimitMin, Vec3Arg inLimitMax)
+{
+	mLimitMin[EAxis::TranslationX] = inLimitMin.GetX();
+	mLimitMin[EAxis::TranslationY] = inLimitMin.GetY();
+	mLimitMin[EAxis::TranslationZ] = inLimitMin.GetZ();
+	mLimitMax[EAxis::TranslationX] = inLimitMax.GetX();
+	mLimitMax[EAxis::TranslationY] = inLimitMax.GetY();
+	mLimitMax[EAxis::TranslationZ] = inLimitMax.GetZ();
+
+	UpdateTranslationLimits();
+	UpdateFixedFreeAxis();
+}
+
+void SixDOFConstraint::SetRotationLimits(Vec3Arg inLimitMin, Vec3Arg inLimitMax)
+{
+	mLimitMin[EAxis::RotationX] = inLimitMin.GetX();
+	mLimitMin[EAxis::RotationY] = inLimitMin.GetY();
+	mLimitMin[EAxis::RotationZ] = inLimitMin.GetZ();
+	mLimitMax[EAxis::RotationX] = inLimitMax.GetX();
+	mLimitMax[EAxis::RotationY] = inLimitMax.GetY();
+	mLimitMax[EAxis::RotationZ] = inLimitMax.GetZ();
+
+	UpdateRotationLimits();
+	UpdateFixedFreeAxis();
+}
+
+void SixDOFConstraint::SetMaxFriction(EAxis inAxis, float inFriction)
+{
+	mMaxFriction[inAxis] = inFriction;
+
+	if (inAxis >= EAxis::TranslationX && inAxis <= EAxis::TranslationZ)
+		CacheTranslationMotorActive();
+	else
+		CacheRotationMotorActive();
+}
+
+void SixDOFConstraint::GetPositionConstraintProperties(Vec3 &outR1PlusU, Vec3 &outR2, Vec3 &outU) const
+{
+	RVec3 p1 = mBody1->GetCenterOfMassTransform() * mLocalSpacePosition1;
+	RVec3 p2 = mBody2->GetCenterOfMassTransform() * mLocalSpacePosition2;
+	outR1PlusU = Vec3(p2 - mBody1->GetCenterOfMassPosition()); // r1 + u = (p1 - x1) + (p2 - p1) = p2 - x1
+	outR2 = Vec3(p2 - mBody2->GetCenterOfMassPosition());
+	outU = Vec3(p2 - p1);
+}
+
+Quat SixDOFConstraint::GetRotationInConstraintSpace() const
+{
+	// Let b1, b2 be the center of mass transform of body1 and body2 (For body1 this is mBody1->GetCenterOfMassTransform())
+	// Let c1, c2 be the transform that takes a vector from constraint space to local space of body1 and body2 (For body1 this is Mat44::sRotationTranslation(mConstraintToBody1, mLocalSpacePosition1))
+	// Let q be the rotation of the constraint in constraint space
+	// b2 takes a vector from the local space of body2 to world space
+	// To express this in terms of b1: b2 = b1 * c1 * q * c2^-1
+	// c2^-1 goes from local body 2 space to constraint space
+	// q rotates the constraint
+	// c1 goes from constraint space to body 1 local space
+	// b1 goes from body 1 local space to world space
+	// So when the body rotations are given, q = (b1 * c1)^-1 * b2 c2
+	// Or: q = (q1 * c1)^-1 * (q2 * c2) if we're only interested in rotations
+	return (mBody1->GetRotation() * mConstraintToBody1).Conjugated() * mBody2->GetRotation() * mConstraintToBody2;
+}
+
+void SixDOFConstraint::CacheTranslationMotorActive()
+{
+	mTranslationMotorActive = mMotorState[EAxis::TranslationX] != EMotorState::Off
+		|| mMotorState[EAxis::TranslationY] != EMotorState::Off
+		|| mMotorState[EAxis::TranslationZ] != EMotorState::Off
+		|| HasFriction(EAxis::TranslationX)
+		|| HasFriction(EAxis::TranslationY)
+		|| HasFriction(EAxis::TranslationZ);
+}
+
+void SixDOFConstraint::CacheRotationMotorActive()
+{
+	mRotationMotorActive = mMotorState[EAxis::RotationX] != EMotorState::Off
+		|| mMotorState[EAxis::RotationY] != EMotorState::Off
+		|| mMotorState[EAxis::RotationZ] != EMotorState::Off
+		|| HasFriction(EAxis::RotationX)
+		|| HasFriction(EAxis::RotationY)
+		|| HasFriction(EAxis::RotationZ);
+}
+
+void SixDOFConstraint::CacheRotationPositionMotorActive()
+{
+	mRotationPositionMotorActive = 0;
+	for (int i = 0; i < 3; ++i)
+		if (mMotorState[EAxis::RotationX + i] == EMotorState::Position)
+			mRotationPositionMotorActive |= 1 << i;
+}
+
+void SixDOFConstraint::CacheHasSpringLimits()
+{
+	mHasSpringLimits = mLimitsSpringSettings[EAxis::TranslationX].mFrequency > 0.0f
+		|| mLimitsSpringSettings[EAxis::TranslationY].mFrequency > 0.0f
+		|| mLimitsSpringSettings[EAxis::TranslationZ].mFrequency > 0.0f;
+}
+
+void SixDOFConstraint::SetMotorState(EAxis inAxis, EMotorState inState)
+{
+	JPH_ASSERT(inState == EMotorState::Off || mMotorSettings[inAxis].IsValid());
+
+	if (mMotorState[inAxis] != inState)
+	{
+		mMotorState[inAxis] = inState;
+
+		// Ensure that warm starting next frame doesn't apply any impulses (motor parts are repurposed for different modes)
+		if (inAxis >= EAxis::TranslationX && inAxis <= EAxis::TranslationZ)
+		{
+			mMotorTranslationConstraintPart[inAxis - EAxis::TranslationX].Deactivate();
+
+			CacheTranslationMotorActive();
+		}
+		else
+		{
+			JPH_ASSERT(inAxis >= EAxis::RotationX && inAxis <= EAxis::RotationZ);
+
+			mMotorRotationConstraintPart[inAxis - EAxis::RotationX].Deactivate();
+
+			CacheRotationMotorActive();
+			CacheRotationPositionMotorActive();
+		}
+	}
+}
+
+void SixDOFConstraint::SetTargetOrientationCS(QuatArg inOrientation)
+{
+	Quat q_swing, q_twist;
+	inOrientation.GetSwingTwist(q_swing, q_twist);
+
+	uint clamped_axis;
+	mSwingTwistConstraintPart.ClampSwingTwist(q_swing, q_twist, clamped_axis);
+
+	if (clamped_axis != 0)
+		mTargetOrientation = q_swing * q_twist;
+	else
+		mTargetOrientation = inOrientation;
+}
+
+void SixDOFConstraint::SetupVelocityConstraint(float inDeltaTime)
+{
+	// Get body rotations
+	Quat rotation1 = mBody1->GetRotation();
+	Quat rotation2 = mBody2->GetRotation();
+
+	// Quaternion that rotates from body1's constraint space to world space
+	Quat constraint_body1_to_world = rotation1 * mConstraintToBody1;
+
+	// Store world space axis of constraint space
+	Mat44 translation_axis_mat = Mat44::sRotation(constraint_body1_to_world);
+	for (int i = 0; i < 3; ++i)
+		mTranslationAxis[i] = translation_axis_mat.GetColumn3(i);
+
+	if (IsTranslationFullyConstrained())
+	{
+		// All translation locked: Setup point constraint
+		mPointConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(rotation1), mLocalSpacePosition1, *mBody2, Mat44::sRotation(rotation2), mLocalSpacePosition2);
+	}
+	else if (IsTranslationConstrained() || mTranslationMotorActive)
+	{
+		// Update world space positions (the bodies may have moved)
+		Vec3 r1_plus_u, r2, u;
+		GetPositionConstraintProperties(r1_plus_u, r2, u);
+
+		// Setup axis constraint parts
+		for (int i = 0; i < 3; ++i)
+		{
+			EAxis axis = EAxis(EAxis::TranslationX + i);
+
+			Vec3 translation_axis = mTranslationAxis[i];
+
+			// Calculate displacement along this axis
+			float d = translation_axis.Dot(u);
+			mDisplacement[i] = d; // Store for SolveVelocityConstraint
+
+			// Setup limit constraint
+			bool constraint_active = false;
+			float constraint_value = 0.0f;
+			if (IsFixedAxis(axis))
+			{
+				// When constraint is fixed it is always active
+				constraint_value = d - mLimitMin[i];
+				constraint_active = true;
+			}
+			else if (!IsFreeAxis(axis))
+			{
+				// When constraint is limited, it is only active when outside of the allowed range
+				if (d <= mLimitMin[i])
+				{
+					constraint_value = d - mLimitMin[i];
+					constraint_active = true;
+				}
+				else if (d >= mLimitMax[i])
+				{
+					constraint_value = d - mLimitMax[i];
+					constraint_active = true;
+				}
+			}
+
+			if (constraint_active)
+				mTranslationConstraintPart[i].CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, r1_plus_u, *mBody2, r2, translation_axis, 0.0f, constraint_value, mLimitsSpringSettings[i]);
+			else
+				mTranslationConstraintPart[i].Deactivate();
+
+			// Setup motor constraint
+			switch (mMotorState[i])
+			{
+			case EMotorState::Off:
+				if (HasFriction(axis))
+					mMotorTranslationConstraintPart[i].CalculateConstraintProperties(*mBody1, r1_plus_u, *mBody2, r2, translation_axis);
+				else
+					mMotorTranslationConstraintPart[i].Deactivate();
+				break;
+
+			case EMotorState::Velocity:
+				mMotorTranslationConstraintPart[i].CalculateConstraintProperties(*mBody1, r1_plus_u, *mBody2, r2, translation_axis, -mTargetVelocity[i]);
+				break;
+
+			case EMotorState::Position:
+				{
+					const SpringSettings &spring_settings = mMotorSettings[i].mSpringSettings;
+					if (spring_settings.HasStiffness())
+						mMotorTranslationConstraintPart[i].CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, r1_plus_u, *mBody2, r2, translation_axis, 0.0f, translation_axis.Dot(u) - mTargetPosition[i], spring_settings);
+					else
+						mMotorTranslationConstraintPart[i].Deactivate();
+					break;
+				}
+			}
+		}
+	}
+
+	// Setup rotation constraints
+	if (IsRotationFullyConstrained())
+	{
+		// All rotation locked: Setup rotation constraint
+		mRotationConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(mBody1->GetRotation()), *mBody2, Mat44::sRotation(mBody2->GetRotation()));
+	}
+	else if (IsRotationConstrained() || mRotationMotorActive)
+	{
+		// GetRotationInConstraintSpace without redoing the calculation of constraint_body1_to_world
+		Quat constraint_body2_to_world = mBody2->GetRotation() * mConstraintToBody2;
+		Quat q = constraint_body1_to_world.Conjugated() * constraint_body2_to_world;
+
+		// Use swing twist constraint part
+		if (IsRotationConstrained())
+			mSwingTwistConstraintPart.CalculateConstraintProperties(*mBody1, *mBody2, q, constraint_body1_to_world);
+		else
+			mSwingTwistConstraintPart.Deactivate();
+
+		if (mRotationMotorActive)
+		{
+			// Calculate rotation motor axis
+			Mat44 ws_axis = Mat44::sRotation(constraint_body2_to_world);
+			for (int i = 0; i < 3; ++i)
+				mRotationAxis[i] = ws_axis.GetColumn3(i);
+
+			// Get target orientation along the shortest path from q
+			Quat target_orientation = q.Dot(mTargetOrientation) > 0.0f? mTargetOrientation : -mTargetOrientation;
+
+			// The definition of the constraint rotation q:
+			// R2 * ConstraintToBody2 = R1 * ConstraintToBody1 * q (1)
+			//
+			// R2' is the rotation of body 2 when reaching the target_orientation:
+			// R2' * ConstraintToBody2 = R1 * ConstraintToBody1 * target_orientation (2)
+			//
+			// The difference in body 2 space:
+			// R2' = R2 * diff_body2 (3)
+			//
+			// We want to specify the difference in the constraint space of body 2:
+			// diff_body2 = ConstraintToBody2 * diff * ConstraintToBody2^* (4)
+			//
+			// Extracting R2' from 2: R2' = R1 * ConstraintToBody1 * target_orientation * ConstraintToBody2^* (5)
+			// Combining 3 & 4: R2' = R2 * ConstraintToBody2 * diff * ConstraintToBody2^* (6)
+			// Combining 1 & 6: R2' = R1 * ConstraintToBody1 * q * diff * ConstraintToBody2^* (7)
+			// Combining 5 & 7: R1 * ConstraintToBody1 * target_orientation * ConstraintToBody2^* = R1 * ConstraintToBody1 * q * diff * ConstraintToBody2^*
+			// <=> target_orientation = q * diff
+			// <=> diff = q^* * target_orientation
+			Quat diff = q.Conjugated() * target_orientation;
+
+			// Project diff so that only rotation around axis that have a position motor are remaining
+			Quat projected_diff;
+			switch (mRotationPositionMotorActive)
+			{
+			case 0b001:
+				// Keep only rotation around X
+				projected_diff = diff.GetTwist(Vec3::sAxisX());
+				break;
+
+			case 0b010:
+				// Keep only rotation around Y
+				projected_diff = diff.GetTwist(Vec3::sAxisY());
+				break;
+
+			case 0b100:
+				// Keep only rotation around Z
+				projected_diff = diff.GetTwist(Vec3::sAxisZ());
+				break;
+
+			case 0b011:
+				// Remove rotation around Z
+				// q = swing_xy * twist_z <=> swing_xy = q * twist_z^*
+				projected_diff = diff * diff.GetTwist(Vec3::sAxisZ()).Conjugated();
+				break;
+
+			case 0b101:
+				// Remove rotation around Y
+				// q = swing_xz * twist_y <=> swing_xz = q * twist_y^*
+				projected_diff = diff * diff.GetTwist(Vec3::sAxisY()).Conjugated();
+				break;
+
+			case 0b110:
+				// Remove rotation around X
+				// q = swing_yz * twist_x <=> swing_yz = q * twist_x^*
+				projected_diff = diff * diff.GetTwist(Vec3::sAxisX()).Conjugated();
+				break;
+
+			case 0b111:
+			default: // All motors off is handled here but the results are unused
+				// Keep entire rotation
+				projected_diff = diff;
+				break;
+			}
+
+			// Approximate error angles
+			// The imaginary part of a quaternion is rotation_axis * sin(angle / 2)
+			// If angle is small, sin(x) = x so angle[i] ~ 2.0f * rotation_axis[i]
+			// We'll be making small time steps, so if the angle is not small at least the sign will be correct and we'll move in the right direction
+			Vec3 rotation_error = -2.0f * projected_diff.GetXYZ();
+
+			// Setup motors
+			for (int i = 0; i < 3; ++i)
+			{
+				EAxis axis = EAxis(EAxis::RotationX + i);
+
+				Vec3 rotation_axis = mRotationAxis[i];
+
+				switch (mMotorState[axis])
+				{
+				case EMotorState::Off:
+					if (HasFriction(axis))
+						mMotorRotationConstraintPart[i].CalculateConstraintProperties(*mBody1, *mBody2, rotation_axis);
+					else
+						mMotorRotationConstraintPart[i].Deactivate();
+					break;
+
+				case EMotorState::Velocity:
+					mMotorRotationConstraintPart[i].CalculateConstraintProperties(*mBody1, *mBody2, rotation_axis, -mTargetAngularVelocity[i]);
+					break;
+
+				case EMotorState::Position:
+					{
+						const SpringSettings &spring_settings = mMotorSettings[axis].mSpringSettings;
+						if (spring_settings.HasStiffness())
+							mMotorRotationConstraintPart[i].CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, *mBody2, rotation_axis, 0.0f, rotation_error[i], spring_settings);
+						else
+							mMotorRotationConstraintPart[i].Deactivate();
+						break;
+					}
+				}
+			}
+		}
+	}
+}
+
+void SixDOFConstraint::ResetWarmStart()
+{
+	for (AxisConstraintPart &c : mMotorTranslationConstraintPart)
+		c.Deactivate();
+	for (AngleConstraintPart &c : mMotorRotationConstraintPart)
+		c.Deactivate();
+	mRotationConstraintPart.Deactivate();
+	mSwingTwistConstraintPart.Deactivate();
+	mPointConstraintPart.Deactivate();
+	for (AxisConstraintPart &c : mTranslationConstraintPart)
+		c.Deactivate();
+}
+
+void SixDOFConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio)
+{
+	// Warm start translation motors
+	if (mTranslationMotorActive)
+		for (int i = 0; i < 3; ++i)
+			if (mMotorTranslationConstraintPart[i].IsActive())
+				mMotorTranslationConstraintPart[i].WarmStart(*mBody1, *mBody2, mTranslationAxis[i], inWarmStartImpulseRatio);
+
+	// Warm start rotation motors
+	if (mRotationMotorActive)
+		for (AngleConstraintPart &c : mMotorRotationConstraintPart)
+			if (c.IsActive())
+				c.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio);
+
+	// Warm start rotation constraints
+	if (IsRotationFullyConstrained())
+		mRotationConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio);
+	else if (IsRotationConstrained())
+		mSwingTwistConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio);
+
+	// Warm start translation constraints
+	if (IsTranslationFullyConstrained())
+		mPointConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio);
+	else if (IsTranslationConstrained())
+		for (int i = 0; i < 3; ++i)
+			if (mTranslationConstraintPart[i].IsActive())
+				mTranslationConstraintPart[i].WarmStart(*mBody1, *mBody2, mTranslationAxis[i], inWarmStartImpulseRatio);
+}
+
+bool SixDOFConstraint::SolveVelocityConstraint(float inDeltaTime)
+{
+	bool impulse = false;
+
+	// Solve translation motor
+	if (mTranslationMotorActive)
+		for (int i = 0; i < 3; ++i)
+			if (mMotorTranslationConstraintPart[i].IsActive())
+				switch (mMotorState[i])
+				{
+				case EMotorState::Off:
+				{
+					// Apply friction only
+					float max_lambda = mMaxFriction[i] * inDeltaTime;
+					impulse |= mMotorTranslationConstraintPart[i].SolveVelocityConstraint(*mBody1, *mBody2, mTranslationAxis[i], -max_lambda, max_lambda);
+					break;
+				}
+
+				case EMotorState::Velocity:
+				case EMotorState::Position:
+					// Drive motor
+					impulse |= mMotorTranslationConstraintPart[i].SolveVelocityConstraint(*mBody1, *mBody2, mTranslationAxis[i], inDeltaTime * mMotorSettings[i].mMinForceLimit, inDeltaTime * mMotorSettings[i].mMaxForceLimit);
+					break;
+				}
+
+	// Solve rotation motor
+	if (mRotationMotorActive)
+		for (int i = 0; i < 3; ++i)
+		{
+			EAxis axis = EAxis(EAxis::RotationX + i);
+			if (mMotorRotationConstraintPart[i].IsActive())
+				switch (mMotorState[axis])
+				{
+				case EMotorState::Off:
+				{
+					// Apply friction only
+					float max_lambda = mMaxFriction[axis] * inDeltaTime;
+					impulse |= mMotorRotationConstraintPart[i].SolveVelocityConstraint(*mBody1, *mBody2, mRotationAxis[i], -max_lambda, max_lambda);
+					break;
+				}
+
+				case EMotorState::Velocity:
+				case EMotorState::Position:
+					// Drive motor
+					impulse |= mMotorRotationConstraintPart[i].SolveVelocityConstraint(*mBody1, *mBody2, mRotationAxis[i], inDeltaTime * mMotorSettings[axis].mMinTorqueLimit, inDeltaTime * mMotorSettings[axis].mMaxTorqueLimit);
+					break;
+				}
+		}
+
+	// Solve rotation constraint
+	if (IsRotationFullyConstrained())
+		impulse |= mRotationConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2);
+	else if (IsRotationConstrained())
+		impulse |= mSwingTwistConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2);
+
+	// Solve position constraint
+	if (IsTranslationFullyConstrained())
+		impulse |= mPointConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2);
+	else if (IsTranslationConstrained())
+		for (int i = 0; i < 3; ++i)
+			if (mTranslationConstraintPart[i].IsActive())
+			{
+				// If the axis is not fixed it must be limited (or else the constraint would not be active)
+				// Calculate the min and max constraint force based on on which side we're limited
+				float limit_min = -FLT_MAX, limit_max = FLT_MAX;
+				if (!IsFixedAxis(EAxis(EAxis::TranslationX + i)))
+				{
+					JPH_ASSERT(!IsFreeAxis(EAxis(EAxis::TranslationX + i)));
+					if (mDisplacement[i] <= mLimitMin[i])
+						limit_min = 0;
+					else if (mDisplacement[i] >= mLimitMax[i])
+						limit_max = 0;
+				}
+
+				impulse |= mTranslationConstraintPart[i].SolveVelocityConstraint(*mBody1, *mBody2, mTranslationAxis[i], limit_min, limit_max);
+			}
+
+	return impulse;
+}
+
+bool SixDOFConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte)
+{
+	bool impulse = false;
+
+	if (IsRotationFullyConstrained())
+	{
+		// Rotation locked: Solve rotation constraint
+
+		// Inverse of initial rotation from body 1 to body 2 in body 1 space
+		// Definition of initial orientation r0: q2 = q1 r0
+		// Initial rotation (see: GetRotationInConstraintSpace): q2 = q1 c1 c2^-1
+		// So: r0^-1 = (c1 c2^-1)^-1 = c2 * c1^-1
+		Quat constraint_to_body1 = mConstraintToBody1 * Quat::sEulerAngles(GetRotationLimitsMin());
+		Quat inv_initial_orientation = mConstraintToBody2 * constraint_to_body1.Conjugated();
+
+		// Solve rotation violations
+		mRotationConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(mBody1->GetRotation()), *mBody2, Mat44::sRotation(mBody2->GetRotation()));
+		impulse |= mRotationConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, inv_initial_orientation, inBaumgarte);
+	}
+	else if (IsRotationConstrained())
+	{
+		// Rotation partially constraint
+
+		// Solve rotation violations
+		Quat q = GetRotationInConstraintSpace();
+		impulse |= mSwingTwistConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, q, mConstraintToBody1, mConstraintToBody2, inBaumgarte);
+	}
+
+	// Solve position violations
+	if (IsTranslationFullyConstrained())
+	{
+		// Translation locked: Solve point constraint
+		Vec3 local_space_position1 = mLocalSpacePosition1 + mConstraintToBody1 * GetTranslationLimitsMin();
+		mPointConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(mBody1->GetRotation()), local_space_position1, *mBody2, Mat44::sRotation(mBody2->GetRotation()), mLocalSpacePosition2);
+		impulse |= mPointConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, inBaumgarte);
+	}
+	else if (IsTranslationConstrained())
+	{
+		// Translation partially locked: Solve per axis
+		for (int i = 0; i < 3; ++i)
+			if (mLimitsSpringSettings[i].mFrequency <= 0.0f) // If not soft limit
+			{
+				// Update world space positions (the bodies may have moved)
+				Vec3 r1_plus_u, r2, u;
+				GetPositionConstraintProperties(r1_plus_u, r2, u);
+
+				// Quaternion that rotates from body1's constraint space to world space
+				Quat constraint_body1_to_world = mBody1->GetRotation() * mConstraintToBody1;
+
+				// Calculate axis
+				Vec3 translation_axis;
+				switch (i)
+				{
+				case 0:							translation_axis = constraint_body1_to_world.RotateAxisX(); break;
+				case 1:							translation_axis = constraint_body1_to_world.RotateAxisY(); break;
+				default:	JPH_ASSERT(i == 2); translation_axis = constraint_body1_to_world.RotateAxisZ(); break;
+				}
+
+				// Determine position error
+				float error = 0.0f;
+				EAxis axis(EAxis(EAxis::TranslationX + i));
+				if (IsFixedAxis(axis))
+					error = u.Dot(translation_axis) - mLimitMin[axis];
+				else if (!IsFreeAxis(axis))
+				{
+					float displacement = u.Dot(translation_axis);
+					if (displacement <= mLimitMin[axis])
+						error = displacement - mLimitMin[axis];
+					else if (displacement >= mLimitMax[axis])
+						error = displacement - mLimitMax[axis];
+				}
+
+				if (error != 0.0f)
+				{
+					// Setup axis constraint part and solve it
+					mTranslationConstraintPart[i].CalculateConstraintProperties(*mBody1, r1_plus_u, *mBody2, r2, translation_axis);
+					impulse |= mTranslationConstraintPart[i].SolvePositionConstraint(*mBody1, *mBody2, translation_axis, error, inBaumgarte);
+				}
+			}
+	}
+
+	return impulse;
+}
+
+#ifdef JPH_DEBUG_RENDERER
+void SixDOFConstraint::DrawConstraint(DebugRenderer *inRenderer) const
+{
+	// Get constraint properties in world space
+	RVec3 position1 = mBody1->GetCenterOfMassTransform() * mLocalSpacePosition1;
+	Quat rotation1 = mBody1->GetRotation() * mConstraintToBody1;
+	Quat rotation2 = mBody2->GetRotation() * mConstraintToBody2;
+
+	// Draw constraint orientation
+	inRenderer->DrawCoordinateSystem(RMat44::sRotationTranslation(rotation1, position1), mDrawConstraintSize);
+
+	if ((IsRotationConstrained() || mRotationPositionMotorActive != 0) && !IsRotationFullyConstrained())
+	{
+		// Draw current swing and twist
+		Quat q = GetRotationInConstraintSpace();
+		Quat q_swing, q_twist;
+		q.GetSwingTwist(q_swing, q_twist);
+		inRenderer->DrawLine(position1, position1 + mDrawConstraintSize * (rotation1 * q_twist).RotateAxisY(), Color::sWhite);
+		inRenderer->DrawLine(position1, position1 + mDrawConstraintSize * (rotation1 * q_swing).RotateAxisX(), Color::sWhite);
+	}
+
+	// Draw target rotation
+	Quat m_swing, m_twist;
+	mTargetOrientation.GetSwingTwist(m_swing, m_twist);
+	if (mMotorState[EAxis::RotationX] == EMotorState::Position)
+		inRenderer->DrawLine(position1, position1 + mDrawConstraintSize * (rotation1 * m_twist).RotateAxisY(), Color::sYellow);
+	if (mMotorState[EAxis::RotationY] == EMotorState::Position || mMotorState[EAxis::RotationZ] == EMotorState::Position)
+		inRenderer->DrawLine(position1, position1 + mDrawConstraintSize * (rotation1 * m_swing).RotateAxisX(), Color::sYellow);
+
+	// Draw target angular velocity
+	Vec3 target_angular_velocity = Vec3::sZero();
+	for (int i = 0; i < 3; ++i)
+		if (mMotorState[EAxis::RotationX + i] == EMotorState::Velocity)
+			target_angular_velocity.SetComponent(i, mTargetAngularVelocity[i]);
+	if (target_angular_velocity != Vec3::sZero())
+		inRenderer->DrawArrow(position1, position1 + rotation2 * target_angular_velocity, Color::sRed, 0.1f);
+}
+
+void SixDOFConstraint::DrawConstraintLimits(DebugRenderer *inRenderer) const
+{
+	// Get matrix that transforms from constraint space to world space
+	RMat44 constraint_body1_to_world = RMat44::sRotationTranslation(mBody1->GetRotation() * mConstraintToBody1, mBody1->GetCenterOfMassTransform() * mLocalSpacePosition1);
+
+	// Draw limits
+	if (mSwingTwistConstraintPart.GetSwingType() == ESwingType::Pyramid)
+		inRenderer->DrawSwingPyramidLimits(constraint_body1_to_world, mLimitMin[EAxis::RotationY], mLimitMax[EAxis::RotationY], mLimitMin[EAxis::RotationZ], mLimitMax[EAxis::RotationZ], mDrawConstraintSize, Color::sGreen, DebugRenderer::ECastShadow::Off);
+	else
+		inRenderer->DrawSwingConeLimits(constraint_body1_to_world, mLimitMax[EAxis::RotationY], mLimitMax[EAxis::RotationZ], mDrawConstraintSize, Color::sGreen, DebugRenderer::ECastShadow::Off);
+	inRenderer->DrawPie(constraint_body1_to_world.GetTranslation(), mDrawConstraintSize, constraint_body1_to_world.GetAxisX(), constraint_body1_to_world.GetAxisY(), mLimitMin[EAxis::RotationX], mLimitMax[EAxis::RotationX], Color::sPurple, DebugRenderer::ECastShadow::Off);
+}
+#endif // JPH_DEBUG_RENDERER
+
+void SixDOFConstraint::SaveState(StateRecorder &inStream) const
+{
+	TwoBodyConstraint::SaveState(inStream);
+
+	for (const AxisConstraintPart &c : mTranslationConstraintPart)
+		c.SaveState(inStream);
+	mPointConstraintPart.SaveState(inStream);
+	mSwingTwistConstraintPart.SaveState(inStream);
+	mRotationConstraintPart.SaveState(inStream);
+	for (const AxisConstraintPart &c : mMotorTranslationConstraintPart)
+		c.SaveState(inStream);
+	for (const AngleConstraintPart &c : mMotorRotationConstraintPart)
+		c.SaveState(inStream);
+
+	inStream.Write(mMotorState);
+	inStream.Write(mTargetVelocity);
+	inStream.Write(mTargetAngularVelocity);
+	inStream.Write(mTargetPosition);
+	inStream.Write(mTargetOrientation);
+}
+
+void SixDOFConstraint::RestoreState(StateRecorder &inStream)
+{
+	TwoBodyConstraint::RestoreState(inStream);
+
+	for (AxisConstraintPart &c : mTranslationConstraintPart)
+		c.RestoreState(inStream);
+	mPointConstraintPart.RestoreState(inStream);
+	mSwingTwistConstraintPart.RestoreState(inStream);
+	mRotationConstraintPart.RestoreState(inStream);
+	for (AxisConstraintPart &c : mMotorTranslationConstraintPart)
+		c.RestoreState(inStream);
+	for (AngleConstraintPart &c : mMotorRotationConstraintPart)
+		c.RestoreState(inStream);
+
+	inStream.Read(mMotorState);
+	inStream.Read(mTargetVelocity);
+	inStream.Read(mTargetAngularVelocity);
+	inStream.Read(mTargetPosition);
+	inStream.Read(mTargetOrientation);
+
+	CacheTranslationMotorActive();
+	CacheRotationMotorActive();
+	CacheRotationPositionMotorActive();
+}
+
+Ref<ConstraintSettings> SixDOFConstraint::GetConstraintSettings() const
+{
+	SixDOFConstraintSettings *settings = new SixDOFConstraintSettings;
+	ToConstraintSettings(*settings);
+	settings->mSpace = EConstraintSpace::LocalToBodyCOM;
+	settings->mPosition1 = RVec3(mLocalSpacePosition1);
+	settings->mAxisX1 = mConstraintToBody1.RotateAxisX();
+	settings->mAxisY1 = mConstraintToBody1.RotateAxisY();
+	settings->mPosition2 = RVec3(mLocalSpacePosition2);
+	settings->mAxisX2 = mConstraintToBody2.RotateAxisX();
+	settings->mAxisY2 = mConstraintToBody2.RotateAxisY();
+	settings->mSwingType = mSwingTwistConstraintPart.GetSwingType();
+	memcpy(settings->mLimitMin, mLimitMin, sizeof(mLimitMin));
+	memcpy(settings->mLimitMax, mLimitMax, sizeof(mLimitMax));
+	memcpy(settings->mMaxFriction, mMaxFriction, sizeof(mMaxFriction));
+	for (int i = 0; i < EAxis::Num; ++i)
+		settings->mMotorSettings[i] = mMotorSettings[i];
+	return settings;
+}
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Constraints/SixDOFConstraint.h

@@ -0,0 +1,289 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Constraints/TwoBodyConstraint.h>
+#include <Jolt/Physics/Constraints/MotorSettings.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/PointConstraintPart.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/AxisConstraintPart.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/AngleConstraintPart.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/RotationEulerConstraintPart.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/SwingTwistConstraintPart.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// 6 Degree Of Freedom Constraint setup structure. Allows control over each of the 6 degrees of freedom.
+class JPH_EXPORT SixDOFConstraintSettings final : public TwoBodyConstraintSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, SixDOFConstraintSettings)
+
+public:
+	/// Constraint is split up into translation/rotation around X, Y and Z axis.
+	enum EAxis
+	{
+		TranslationX,
+		TranslationY,
+		TranslationZ,
+
+		RotationX,
+		RotationY,
+		RotationZ,
+
+		Num,
+		NumTranslation = TranslationZ + 1,
+	};
+
+	// See: ConstraintSettings::SaveBinaryState
+	virtual void				SaveBinaryState(StreamOut &inStream) const override;
+
+	/// Create an instance of this constraint
+	virtual TwoBodyConstraint *	Create(Body &inBody1, Body &inBody2) const override;
+
+	/// This determines in which space the constraint is setup, all properties below should be in the specified space
+	EConstraintSpace			mSpace = EConstraintSpace::WorldSpace;
+
+	/// Body 1 constraint reference frame (space determined by mSpace)
+	RVec3						mPosition1 = RVec3::sZero();
+	Vec3						mAxisX1 = Vec3::sAxisX();
+	Vec3						mAxisY1 = Vec3::sAxisY();
+
+	/// Body 2 constraint reference frame (space determined by mSpace)
+	RVec3						mPosition2 = RVec3::sZero();
+	Vec3						mAxisX2 = Vec3::sAxisX();
+	Vec3						mAxisY2 = Vec3::sAxisY();
+
+	/// Friction settings.
+	/// For translation: Max friction force in N. 0 = no friction.
+	/// For rotation: Max friction torque in Nm. 0 = no friction.
+	float						mMaxFriction[EAxis::Num] = { 0, 0, 0, 0, 0, 0 };
+
+	/// The type of swing constraint that we want to use.
+	ESwingType					mSwingType = ESwingType::Cone;
+
+	/// Limits.
+	/// For translation: Min and max linear limits in m (0 is frame of body 1 and 2 coincide).
+	/// For rotation: Min and max angular limits in rad (0 is frame of body 1 and 2 coincide). See comments at Axis enum for limit ranges.
+	///
+	/// Remove degree of freedom by setting min = FLT_MAX and max = -FLT_MAX. The constraint will be driven to 0 for this axis.
+	///
+	/// Free movement over an axis is allowed when min = -FLT_MAX and max = FLT_MAX.
+	///
+	/// Rotation limit around X-Axis: When limited, should be \f$\in [-\pi, \pi]\f$. Can be asymmetric around zero.
+	///
+	/// Rotation limit around Y-Z Axis: Forms a pyramid or cone shaped limit:
+	/// * For pyramid, should be \f$\in [-\pi, \pi]\f$ and does not need to be symmetrical around zero.
+	/// * For cone should be \f$\in [0, \pi]\f$ and needs to be symmetrical around zero (min limit is assumed to be -max limit).
+	float						mLimitMin[EAxis::Num] = { -FLT_MAX, -FLT_MAX, -FLT_MAX, -FLT_MAX, -FLT_MAX, -FLT_MAX };
+	float						mLimitMax[EAxis::Num] = { FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX };
+
+	/// When enabled, this makes the limits soft. When the constraint exceeds the limits, a spring force will pull it back.
+	/// Only soft translation limits are supported, soft rotation limits are not currently supported.
+	SpringSettings				mLimitsSpringSettings[EAxis::NumTranslation];
+
+	/// Make axis free (unconstrained)
+	void						MakeFreeAxis(EAxis inAxis)									{ mLimitMin[inAxis] = -FLT_MAX; mLimitMax[inAxis] = FLT_MAX; }
+	bool						IsFreeAxis(EAxis inAxis) const								{ return mLimitMin[inAxis] == -FLT_MAX && mLimitMax[inAxis] == FLT_MAX; }
+
+	/// Make axis fixed (fixed at value 0)
+	void						MakeFixedAxis(EAxis inAxis)									{ mLimitMin[inAxis] = FLT_MAX; mLimitMax[inAxis] = -FLT_MAX; }
+	bool						IsFixedAxis(EAxis inAxis) const								{ return mLimitMin[inAxis] >= mLimitMax[inAxis]; }
+
+	/// Set a valid range for the constraint (if inMax < inMin, the axis will become fixed)
+	void						SetLimitedAxis(EAxis inAxis, float inMin, float inMax)		{ mLimitMin[inAxis] = inMin; mLimitMax[inAxis] = inMax; }
+
+	/// Motor settings for each axis
+	MotorSettings				mMotorSettings[EAxis::Num];
+
+protected:
+	// See: ConstraintSettings::RestoreBinaryState
+	virtual void				RestoreBinaryState(StreamIn &inStream) override;
+};
+
+/// 6 Degree Of Freedom Constraint. Allows control over each of the 6 degrees of freedom.
+class JPH_EXPORT SixDOFConstraint final : public TwoBodyConstraint
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Get Axis from settings class
+	using EAxis = SixDOFConstraintSettings::EAxis;
+
+	/// Construct six DOF constraint
+								SixDOFConstraint(Body &inBody1, Body &inBody2, const SixDOFConstraintSettings &inSettings);
+
+	/// Generic interface of a constraint
+	virtual EConstraintSubType	GetSubType() const override									{ return EConstraintSubType::SixDOF; }
+	virtual void				NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override;
+	virtual void				SetupVelocityConstraint(float inDeltaTime) override;
+	virtual void				ResetWarmStart() override;
+	virtual void				WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override;
+	virtual bool				SolveVelocityConstraint(float inDeltaTime) override;
+	virtual bool				SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override;
+#ifdef JPH_DEBUG_RENDERER
+	virtual void				DrawConstraint(DebugRenderer *inRenderer) const override;
+	virtual void				DrawConstraintLimits(DebugRenderer *inRenderer) const override;
+#endif // JPH_DEBUG_RENDERER
+	virtual void				SaveState(StateRecorder &inStream) const override;
+	virtual void				RestoreState(StateRecorder &inStream) override;
+	virtual Ref<ConstraintSettings> GetConstraintSettings() const override;
+
+	// See: TwoBodyConstraint
+	virtual Mat44				GetConstraintToBody1Matrix() const override					{ return Mat44::sRotationTranslation(mConstraintToBody1, mLocalSpacePosition1); }
+	virtual Mat44				GetConstraintToBody2Matrix() const override					{ return Mat44::sRotationTranslation(mConstraintToBody2, mLocalSpacePosition2); }
+
+	/// Update the translation limits for this constraint
+	void						SetTranslationLimits(Vec3Arg inLimitMin, Vec3Arg inLimitMax);
+
+	/// Update the rotational limits for this constraint
+	void						SetRotationLimits(Vec3Arg inLimitMin, Vec3Arg inLimitMax);
+
+	/// Get constraint Limits
+	float						GetLimitsMin(EAxis inAxis) const							{ return mLimitMin[inAxis]; }
+	float						GetLimitsMax(EAxis inAxis) const							{ return mLimitMax[inAxis]; }
+	Vec3						GetTranslationLimitsMin() const								{ return Vec3::sLoadFloat3Unsafe(*reinterpret_cast<const Float3 *>(&mLimitMin[EAxis::TranslationX])); }
+	Vec3						GetTranslationLimitsMax() const								{ return Vec3::sLoadFloat3Unsafe(*reinterpret_cast<const Float3 *>(&mLimitMax[EAxis::TranslationX])); }
+	Vec3						GetRotationLimitsMin() const								{ return Vec3::sLoadFloat3Unsafe(*reinterpret_cast<const Float3 *>(&mLimitMin[EAxis::RotationX])); }
+	Vec3						GetRotationLimitsMax() const								{ return Vec3::sLoadFloat3Unsafe(*reinterpret_cast<const Float3 *>(&mLimitMax[EAxis::RotationX])); }
+
+	/// Check which axis are fixed/free
+	inline bool					IsFixedAxis(EAxis inAxis) const								{ return (mFixedAxis & (1 << inAxis)) != 0; }
+	inline bool					IsFreeAxis(EAxis inAxis) const								{ return (mFreeAxis & (1 << inAxis)) != 0; }
+
+	/// Update the limits spring settings
+	const SpringSettings &		GetLimitsSpringSettings(EAxis inAxis) const					{ JPH_ASSERT(inAxis < EAxis::NumTranslation); return mLimitsSpringSettings[inAxis]; }
+	void						SetLimitsSpringSettings(EAxis inAxis, const SpringSettings& inLimitsSpringSettings) { JPH_ASSERT(inAxis < EAxis::NumTranslation); mLimitsSpringSettings[inAxis] = inLimitsSpringSettings; CacheHasSpringLimits(); }
+
+	/// Set the max friction for each axis
+	void						SetMaxFriction(EAxis inAxis, float inFriction);
+	float						GetMaxFriction(EAxis inAxis) const							{ return mMaxFriction[inAxis]; }
+
+	/// Get rotation of constraint in constraint space
+	Quat						GetRotationInConstraintSpace() const;
+
+	/// Motor settings
+	MotorSettings &				GetMotorSettings(EAxis inAxis)								{ return mMotorSettings[inAxis]; }
+	const MotorSettings &		GetMotorSettings(EAxis inAxis) const						{ return mMotorSettings[inAxis]; }
+
+	/// Motor controls.
+	/// Translation motors work in constraint space of body 1.
+	/// Rotation motors work in constraint space of body 2 (!).
+	void						SetMotorState(EAxis inAxis, EMotorState inState);
+	EMotorState					GetMotorState(EAxis inAxis) const							{ return mMotorState[inAxis]; }
+
+	/// Set the target velocity in body 1 constraint space
+	Vec3						GetTargetVelocityCS() const									{ return mTargetVelocity; }
+	void						SetTargetVelocityCS(Vec3Arg inVelocity)						{ mTargetVelocity = inVelocity; }
+
+	/// Set the target angular velocity in body 2 constraint space (!)
+	void						SetTargetAngularVelocityCS(Vec3Arg inAngularVelocity)		{ mTargetAngularVelocity = inAngularVelocity; }
+	Vec3						GetTargetAngularVelocityCS() const							{ return mTargetAngularVelocity; }
+
+	/// Set the target position in body 1 constraint space
+	Vec3						GetTargetPositionCS() const									{ return mTargetPosition; }
+	void						SetTargetPositionCS(Vec3Arg inPosition)						{ mTargetPosition = inPosition; }
+
+	/// Set the target orientation in body 1 constraint space
+	void						SetTargetOrientationCS(QuatArg inOrientation);
+	Quat						GetTargetOrientationCS() const								{ return mTargetOrientation; }
+
+	/// Set the target orientation in body space (R2 = R1 * inOrientation, where R1 and R2 are the world space rotations for body 1 and 2).
+	/// Solve: R2 * ConstraintToBody2 = R1 * ConstraintToBody1 * q (see SwingTwistConstraint::GetSwingTwist) and R2 = R1 * inOrientation for q.
+	void						SetTargetOrientationBS(QuatArg inOrientation)				{ SetTargetOrientationCS(mConstraintToBody1.Conjugated() * inOrientation * mConstraintToBody2); }
+
+	///@name Get Lagrange multiplier from last physics update (the linear/angular impulse applied to satisfy the constraint)
+	inline Vec3					GetTotalLambdaPosition() const								{ return IsTranslationFullyConstrained()? mPointConstraintPart.GetTotalLambda() : Vec3(mTranslationConstraintPart[0].GetTotalLambda(), mTranslationConstraintPart[1].GetTotalLambda(), mTranslationConstraintPart[2].GetTotalLambda()); }
+	inline Vec3					GetTotalLambdaRotation() const								{ return IsRotationFullyConstrained()? mRotationConstraintPart.GetTotalLambda() : Vec3(mSwingTwistConstraintPart.GetTotalTwistLambda(), mSwingTwistConstraintPart.GetTotalSwingYLambda(), mSwingTwistConstraintPart.GetTotalSwingZLambda()); }
+	inline Vec3					GetTotalLambdaMotorTranslation() const						{ return Vec3(mMotorTranslationConstraintPart[0].GetTotalLambda(), mMotorTranslationConstraintPart[1].GetTotalLambda(), mMotorTranslationConstraintPart[2].GetTotalLambda()); }
+	inline Vec3					GetTotalLambdaMotorRotation() const							{ return Vec3(mMotorRotationConstraintPart[0].GetTotalLambda(), mMotorRotationConstraintPart[1].GetTotalLambda(), mMotorRotationConstraintPart[2].GetTotalLambda()); }
+
+private:
+	// Calculate properties needed for the position constraint
+	inline void					GetPositionConstraintProperties(Vec3 &outR1PlusU, Vec3 &outR2, Vec3 &outU) const;
+
+	// Sanitize the translation limits
+	inline void					UpdateTranslationLimits();
+
+	// Propagate the rotation limits to the constraint part
+	inline void					UpdateRotationLimits();
+
+	// Update the cached state of which axis are free and which ones are fixed
+	inline void					UpdateFixedFreeAxis();
+
+	// Cache the state of mTranslationMotorActive
+	void						CacheTranslationMotorActive();
+
+	// Cache the state of mRotationMotorActive
+	void						CacheRotationMotorActive();
+
+	// Cache the state of mRotationPositionMotorActive
+	void						CacheRotationPositionMotorActive();
+
+	/// Cache the state of mHasSpringLimits
+	void						CacheHasSpringLimits();
+
+	// Constraint settings helper functions
+	inline bool					IsTranslationConstrained() const							{ return (mFreeAxis & 0b111) != 0b111; }
+	inline bool					IsTranslationFullyConstrained() const						{ return (mFixedAxis & 0b111) == 0b111 && !mHasSpringLimits; }
+	inline bool					IsRotationConstrained() const								{ return (mFreeAxis & 0b111000) != 0b111000; }
+	inline bool					IsRotationFullyConstrained() const							{ return (mFixedAxis & 0b111000) == 0b111000; }
+	inline bool					HasFriction(EAxis inAxis) const								{ return !IsFixedAxis(inAxis) && mMaxFriction[inAxis] > 0.0f; }
+
+	// CONFIGURATION PROPERTIES FOLLOW
+
+	// Local space constraint positions
+	Vec3						mLocalSpacePosition1;
+	Vec3						mLocalSpacePosition2;
+
+	// Transforms from constraint space to body space
+	Quat						mConstraintToBody1;
+	Quat						mConstraintToBody2;
+
+	// Limits
+	uint8						mFreeAxis = 0;												// Bitmask of free axis (bit 0 = TranslationX)
+	uint8						mFixedAxis = 0;												// Bitmask of fixed axis (bit 0 = TranslationX)
+	bool						mTranslationMotorActive = false;							// If any of the translational frictions / motors are active
+	bool						mRotationMotorActive = false;								// If any of the rotational frictions / motors are active
+	uint8						mRotationPositionMotorActive = 0;							// Bitmask of axis that have position motor active (bit 0 = RotationX)
+	bool						mHasSpringLimits = false;									// If any of the limit springs have a non-zero frequency/stiffness
+	float						mLimitMin[EAxis::Num];
+	float						mLimitMax[EAxis::Num];
+	SpringSettings				mLimitsSpringSettings[EAxis::NumTranslation];
+
+	// Motor settings for each axis
+	MotorSettings				mMotorSettings[EAxis::Num];
+
+	// Friction settings for each axis
+	float						mMaxFriction[EAxis::Num];
+
+	// Motor controls
+	EMotorState					mMotorState[EAxis::Num] = { EMotorState::Off, EMotorState::Off, EMotorState::Off, EMotorState::Off, EMotorState::Off, EMotorState::Off };
+	Vec3						mTargetVelocity = Vec3::sZero();
+	Vec3						mTargetAngularVelocity = Vec3::sZero();
+	Vec3						mTargetPosition = Vec3::sZero();
+	Quat						mTargetOrientation = Quat::sIdentity();
+
+	// RUN TIME PROPERTIES FOLLOW
+
+	// Constraint space axis in world space
+	Vec3						mTranslationAxis[3];
+	Vec3						mRotationAxis[3];
+
+	// Translation displacement (valid when translation axis has a range limit)
+	float						mDisplacement[3];
+
+	// Individual constraint parts for translation, or a combined point constraint part if all axis are fixed
+	AxisConstraintPart			mTranslationConstraintPart[3];
+	PointConstraintPart			mPointConstraintPart;
+
+	// Individual constraint parts for rotation or a combined constraint part if rotation is fixed
+	SwingTwistConstraintPart	mSwingTwistConstraintPart;
+	RotationEulerConstraintPart	mRotationConstraintPart;
+
+	// Motor or friction constraints
+	AxisConstraintPart			mMotorTranslationConstraintPart[3];
+	AngleConstraintPart			mMotorRotationConstraintPart[3];
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Constraints/SliderConstraint.cpp

@@ -0,0 +1,501 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Constraints/SliderConstraint.h>
+#include <Jolt/Physics/Body/Body.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
+
+using namespace literals;
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(SliderConstraintSettings)
+{
+	JPH_ADD_BASE_CLASS(SliderConstraintSettings, TwoBodyConstraintSettings)
+
+	JPH_ADD_ENUM_ATTRIBUTE(SliderConstraintSettings, mSpace)
+	JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mAutoDetectPoint)
+	JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mPoint1)
+	JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mSliderAxis1)
+	JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mNormalAxis1)
+	JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mPoint2)
+	JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mSliderAxis2)
+	JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mNormalAxis2)
+	JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mLimitsMin)
+	JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mLimitsMax)
+	JPH_ADD_ENUM_ATTRIBUTE_WITH_ALIAS(SliderConstraintSettings, mLimitsSpringSettings.mMode, "mSpringMode")
+	JPH_ADD_ATTRIBUTE_WITH_ALIAS(SliderConstraintSettings, mLimitsSpringSettings.mFrequency, "mFrequency") // Renaming attributes to stay compatible with old versions of the library
+	JPH_ADD_ATTRIBUTE_WITH_ALIAS(SliderConstraintSettings, mLimitsSpringSettings.mDamping, "mDamping")
+	JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mMaxFrictionForce)
+	JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mMotorSettings)
+}
+
+void SliderConstraintSettings::SetSliderAxis(Vec3Arg inSliderAxis)
+{
+	JPH_ASSERT(mSpace == EConstraintSpace::WorldSpace);
+
+	mSliderAxis1 = mSliderAxis2 = inSliderAxis;
+	mNormalAxis1 = mNormalAxis2 = inSliderAxis.GetNormalizedPerpendicular();
+}
+
+void SliderConstraintSettings::SaveBinaryState(StreamOut &inStream) const
+{
+	ConstraintSettings::SaveBinaryState(inStream);
+
+	inStream.Write(mSpace);
+	inStream.Write(mAutoDetectPoint);
+	inStream.Write(mPoint1);
+	inStream.Write(mSliderAxis1);
+	inStream.Write(mNormalAxis1);
+	inStream.Write(mPoint2);
+	inStream.Write(mSliderAxis2);
+	inStream.Write(mNormalAxis2);
+	inStream.Write(mLimitsMin);
+	inStream.Write(mLimitsMax);
+	inStream.Write(mMaxFrictionForce);
+	mLimitsSpringSettings.SaveBinaryState(inStream);
+	mMotorSettings.SaveBinaryState(inStream);
+}
+
+void SliderConstraintSettings::RestoreBinaryState(StreamIn &inStream)
+{
+	ConstraintSettings::RestoreBinaryState(inStream);
+
+	inStream.Read(mSpace);
+	inStream.Read(mAutoDetectPoint);
+	inStream.Read(mPoint1);
+	inStream.Read(mSliderAxis1);
+	inStream.Read(mNormalAxis1);
+	inStream.Read(mPoint2);
+	inStream.Read(mSliderAxis2);
+	inStream.Read(mNormalAxis2);
+	inStream.Read(mLimitsMin);
+	inStream.Read(mLimitsMax);
+	inStream.Read(mMaxFrictionForce);
+	mLimitsSpringSettings.RestoreBinaryState(inStream);
+	mMotorSettings.RestoreBinaryState(inStream);
+}
+
+TwoBodyConstraint *SliderConstraintSettings::Create(Body &inBody1, Body &inBody2) const
+{
+	return new SliderConstraint(inBody1, inBody2, *this);
+}
+
+SliderConstraint::SliderConstraint(Body &inBody1, Body &inBody2, const SliderConstraintSettings &inSettings) :
+	TwoBodyConstraint(inBody1, inBody2, inSettings),
+	mMaxFrictionForce(inSettings.mMaxFrictionForce),
+	mMotorSettings(inSettings.mMotorSettings)
+{
+	// Store inverse of initial rotation from body 1 to body 2 in body 1 space
+	mInvInitialOrientation = RotationEulerConstraintPart::sGetInvInitialOrientationXY(inSettings.mSliderAxis1, inSettings.mNormalAxis1, inSettings.mSliderAxis2, inSettings.mNormalAxis2);
+
+	if (inSettings.mSpace == EConstraintSpace::WorldSpace)
+	{
+		RMat44 inv_transform1 = inBody1.GetInverseCenterOfMassTransform();
+		RMat44 inv_transform2 = inBody2.GetInverseCenterOfMassTransform();
+
+		if (inSettings.mAutoDetectPoint)
+		{
+			// Determine anchor point: If any of the bodies can never be dynamic use the other body as anchor point
+			RVec3 anchor;
+			if (!inBody1.CanBeKinematicOrDynamic())
+				anchor = inBody2.GetCenterOfMassPosition();
+			else if (!inBody2.CanBeKinematicOrDynamic())
+				anchor = inBody1.GetCenterOfMassPosition();
+			else
+			{
+				// Otherwise use weighted anchor point towards the lightest body
+				Real inv_m1 = Real(inBody1.GetMotionPropertiesUnchecked()->GetInverseMassUnchecked());
+				Real inv_m2 = Real(inBody2.GetMotionPropertiesUnchecked()->GetInverseMassUnchecked());
+				Real total_inv_mass = inv_m1 + inv_m2;
+				if (total_inv_mass != 0.0_r)
+					anchor = (inv_m1 * inBody1.GetCenterOfMassPosition() + inv_m2 * inBody2.GetCenterOfMassPosition()) / total_inv_mass;
+				else
+					anchor = inBody1.GetCenterOfMassPosition();
+			}
+
+			// Store local positions
+			mLocalSpacePosition1 = Vec3(inv_transform1 * anchor);
+			mLocalSpacePosition2 = Vec3(inv_transform2 * anchor);
+		}
+		else
+		{
+			// Store local positions
+			mLocalSpacePosition1 = Vec3(inv_transform1 * inSettings.mPoint1);
+			mLocalSpacePosition2 = Vec3(inv_transform2 * inSettings.mPoint2);
+		}
+
+		// If all properties were specified in world space, take them to local space now
+		mLocalSpaceSliderAxis1 = inv_transform1.Multiply3x3(inSettings.mSliderAxis1).Normalized();
+		mLocalSpaceNormal1 = inv_transform1.Multiply3x3(inSettings.mNormalAxis1).Normalized();
+
+		// Constraints were specified in world space, so we should have replaced c1 with q10^-1 c1 and c2 with q20^-1 c2
+		// => r0^-1 = (q20^-1 c2) (q10^-1 c1)^1 = q20^-1 (c2 c1^-1) q10
+		mInvInitialOrientation = inBody2.GetRotation().Conjugated() * mInvInitialOrientation * inBody1.GetRotation();
+	}
+	else
+	{
+		// Store local positions
+		mLocalSpacePosition1 = Vec3(inSettings.mPoint1);
+		mLocalSpacePosition2 = Vec3(inSettings.mPoint2);
+
+		// Store local space axis
+		mLocalSpaceSliderAxis1 = inSettings.mSliderAxis1;
+		mLocalSpaceNormal1 = inSettings.mNormalAxis1;
+	}
+
+	// Calculate 2nd local space normal
+	mLocalSpaceNormal2 = mLocalSpaceSliderAxis1.Cross(mLocalSpaceNormal1);
+
+	// Store limits
+	JPH_ASSERT(inSettings.mLimitsMin != inSettings.mLimitsMax || inSettings.mLimitsSpringSettings.mFrequency > 0.0f, "Better use a fixed constraint");
+	SetLimits(inSettings.mLimitsMin, inSettings.mLimitsMax);
+
+	// Store spring settings
+	SetLimitsSpringSettings(inSettings.mLimitsSpringSettings);
+}
+
+void SliderConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM)
+{
+	if (mBody1->GetID() == inBodyID)
+		mLocalSpacePosition1 -= inDeltaCOM;
+	else if (mBody2->GetID() == inBodyID)
+		mLocalSpacePosition2 -= inDeltaCOM;
+}
+
+float SliderConstraint::GetCurrentPosition() const
+{
+	// See: CalculateR1R2U and CalculateSlidingAxisAndPosition
+	Vec3 r1 = mBody1->GetRotation() * mLocalSpacePosition1;
+	Vec3 r2 = mBody2->GetRotation() * mLocalSpacePosition2;
+	Vec3 u = Vec3(mBody2->GetCenterOfMassPosition() - mBody1->GetCenterOfMassPosition()) + r2 - r1;
+	return u.Dot(mBody1->GetRotation() * mLocalSpaceSliderAxis1);
+}
+
+void SliderConstraint::SetLimits(float inLimitsMin, float inLimitsMax)
+{
+	JPH_ASSERT(inLimitsMin <= 0.0f);
+	JPH_ASSERT(inLimitsMax >= 0.0f);
+	mLimitsMin = inLimitsMin;
+	mLimitsMax = inLimitsMax;
+	mHasLimits = mLimitsMin != -FLT_MAX || mLimitsMax != FLT_MAX;
+}
+
+void SliderConstraint::CalculateR1R2U(Mat44Arg inRotation1, Mat44Arg inRotation2)
+{
+	// Calculate points relative to body
+	mR1 = inRotation1 * mLocalSpacePosition1;
+	mR2 = inRotation2 * mLocalSpacePosition2;
+
+	// Calculate X2 + R2 - X1 - R1
+	mU = Vec3(mBody2->GetCenterOfMassPosition() - mBody1->GetCenterOfMassPosition()) + mR2 - mR1;
+}
+
+void SliderConstraint::CalculatePositionConstraintProperties(Mat44Arg inRotation1, Mat44Arg inRotation2)
+{
+	// Calculate world space normals
+	mN1 = inRotation1 * mLocalSpaceNormal1;
+	mN2 = inRotation1 * mLocalSpaceNormal2;
+
+	mPositionConstraintPart.CalculateConstraintProperties(*mBody1, inRotation1, mR1 + mU, *mBody2, inRotation2, mR2, mN1, mN2);
+}
+
+void SliderConstraint::CalculateSlidingAxisAndPosition(Mat44Arg inRotation1)
+{
+	if (mHasLimits || mMotorState != EMotorState::Off || mMaxFrictionForce > 0.0f)
+	{
+		// Calculate world space slider axis
+		mWorldSpaceSliderAxis = inRotation1 * mLocalSpaceSliderAxis1;
+
+		// Calculate slide distance along axis
+		mD = mU.Dot(mWorldSpaceSliderAxis);
+	}
+}
+
+void SliderConstraint::CalculatePositionLimitsConstraintProperties(float inDeltaTime)
+{
+	// Check if distance is within limits
+	bool below_min = mD <= mLimitsMin;
+	if (mHasLimits && (below_min || mD >= mLimitsMax))
+		mPositionLimitsConstraintPart.CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, mR1 + mU, *mBody2, mR2, mWorldSpaceSliderAxis, 0.0f, mD - (below_min? mLimitsMin : mLimitsMax), mLimitsSpringSettings);
+	else
+		mPositionLimitsConstraintPart.Deactivate();
+}
+
+void SliderConstraint::CalculateMotorConstraintProperties(float inDeltaTime)
+{
+	switch (mMotorState)
+	{
+	case EMotorState::Off:
+		if (mMaxFrictionForce > 0.0f)
+			mMotorConstraintPart.CalculateConstraintProperties(*mBody1, mR1 + mU, *mBody2, mR2, mWorldSpaceSliderAxis);
+		else
+			mMotorConstraintPart.Deactivate();
+		break;
+
+	case EMotorState::Velocity:
+		mMotorConstraintPart.CalculateConstraintProperties(*mBody1, mR1 + mU, *mBody2, mR2, mWorldSpaceSliderAxis, -mTargetVelocity);
+		break;
+
+	case EMotorState::Position:
+		if (mMotorSettings.mSpringSettings.HasStiffness())
+			mMotorConstraintPart.CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, mR1 + mU, *mBody2, mR2, mWorldSpaceSliderAxis, 0.0f, mD - mTargetPosition, mMotorSettings.mSpringSettings);
+		else
+			mMotorConstraintPart.Deactivate();
+		break;
+	}
+}
+
+void SliderConstraint::SetupVelocityConstraint(float inDeltaTime)
+{
+	// Calculate constraint properties that are constant while bodies don't move
+	Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation());
+	Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation());
+	CalculateR1R2U(rotation1, rotation2);
+	CalculatePositionConstraintProperties(rotation1, rotation2);
+	mRotationConstraintPart.CalculateConstraintProperties(*mBody1, rotation1, *mBody2, rotation2);
+	CalculateSlidingAxisAndPosition(rotation1);
+	CalculatePositionLimitsConstraintProperties(inDeltaTime);
+	CalculateMotorConstraintProperties(inDeltaTime);
+}
+
+void SliderConstraint::ResetWarmStart()
+{
+	mMotorConstraintPart.Deactivate();
+	mPositionConstraintPart.Deactivate();
+	mRotationConstraintPart.Deactivate();
+	mPositionLimitsConstraintPart.Deactivate();
+}
+
+void SliderConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio)
+{
+	// Warm starting: Apply previous frame impulse
+	mMotorConstraintPart.WarmStart(*mBody1, *mBody2, mWorldSpaceSliderAxis, inWarmStartImpulseRatio);
+	mPositionConstraintPart.WarmStart(*mBody1, *mBody2, mN1, mN2, inWarmStartImpulseRatio);
+	mRotationConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio);
+	mPositionLimitsConstraintPart.WarmStart(*mBody1, *mBody2, mWorldSpaceSliderAxis, inWarmStartImpulseRatio);
+}
+
+bool SliderConstraint::SolveVelocityConstraint(float inDeltaTime)
+{
+	// Solve motor
+	bool motor = false;
+	if (mMotorConstraintPart.IsActive())
+	{
+		switch (mMotorState)
+		{
+		case EMotorState::Off:
+			{
+				float max_lambda = mMaxFrictionForce * inDeltaTime;
+				motor = mMotorConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mWorldSpaceSliderAxis, -max_lambda, max_lambda);
+				break;
+			}
+
+		case EMotorState::Velocity:
+		case EMotorState::Position:
+			motor = mMotorConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mWorldSpaceSliderAxis, inDeltaTime * mMotorSettings.mMinForceLimit, inDeltaTime * mMotorSettings.mMaxForceLimit);
+			break;
+		}
+	}
+
+	// Solve position constraint along 2 axis
+	bool pos = mPositionConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mN1, mN2);
+
+	// Solve rotation constraint
+	bool rot = mRotationConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2);
+
+	// Solve limits along slider axis
+	bool limit = false;
+	if (mPositionLimitsConstraintPart.IsActive())
+	{
+		float min_lambda, max_lambda;
+		if (mLimitsMin == mLimitsMax)
+		{
+			min_lambda = -FLT_MAX;
+			max_lambda = FLT_MAX;
+		}
+		else if (mD <= mLimitsMin)
+		{
+			min_lambda = 0.0f;
+			max_lambda = FLT_MAX;
+		}
+		else
+		{
+			min_lambda = -FLT_MAX;
+			max_lambda = 0.0f;
+		}
+		limit = mPositionLimitsConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mWorldSpaceSliderAxis, min_lambda, max_lambda);
+	}
+
+	return motor || pos || rot || limit;
+}
+
+bool SliderConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte)
+{
+	// Motor operates on velocities only, don't call SolvePositionConstraint
+
+	// Solve position constraint along 2 axis
+	Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation());
+	Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation());
+	CalculateR1R2U(rotation1, rotation2);
+	CalculatePositionConstraintProperties(rotation1, rotation2);
+	bool pos = mPositionConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mU, mN1, mN2, inBaumgarte);
+
+	// Solve rotation constraint
+	mRotationConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(mBody1->GetRotation()), *mBody2, Mat44::sRotation(mBody2->GetRotation()));
+	bool rot = mRotationConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mInvInitialOrientation, inBaumgarte);
+
+	// Solve limits along slider axis
+	bool limit = false;
+	if (mHasLimits && mLimitsSpringSettings.mFrequency <= 0.0f)
+	{
+		rotation1 = Mat44::sRotation(mBody1->GetRotation());
+		rotation2 = Mat44::sRotation(mBody2->GetRotation());
+		CalculateR1R2U(rotation1, rotation2);
+		CalculateSlidingAxisAndPosition(rotation1);
+		CalculatePositionLimitsConstraintProperties(inDeltaTime);
+		if (mPositionLimitsConstraintPart.IsActive())
+		{
+			if (mD <= mLimitsMin)
+				limit = mPositionLimitsConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mWorldSpaceSliderAxis, mD - mLimitsMin, inBaumgarte);
+			else
+			{
+				JPH_ASSERT(mD >= mLimitsMax);
+				limit = mPositionLimitsConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mWorldSpaceSliderAxis, mD - mLimitsMax, inBaumgarte);
+			}
+		}
+	}
+
+	return pos || rot || limit;
+}
+
+#ifdef JPH_DEBUG_RENDERER
+void SliderConstraint::DrawConstraint(DebugRenderer *inRenderer) const
+{
+	RMat44 transform1 = mBody1->GetCenterOfMassTransform();
+	RMat44 transform2 = mBody2->GetCenterOfMassTransform();
+
+	// Transform the local positions into world space
+	Vec3 slider_axis = transform1.Multiply3x3(mLocalSpaceSliderAxis1);
+	RVec3 position1 = transform1 * mLocalSpacePosition1;
+	RVec3 position2 = transform2 * mLocalSpacePosition2;
+
+	// Draw constraint
+	inRenderer->DrawMarker(position1, Color::sRed, 0.1f);
+	inRenderer->DrawMarker(position2, Color::sGreen, 0.1f);
+	inRenderer->DrawLine(position1, position2, Color::sGreen);
+
+	// Draw motor
+	switch (mMotorState)
+	{
+	case EMotorState::Position:
+		inRenderer->DrawMarker(position1 + mTargetPosition * slider_axis, Color::sYellow, 1.0f);
+		break;
+
+	case EMotorState::Velocity:
+		{
+			Vec3 cur_vel = (mBody2->GetLinearVelocity() - mBody1->GetLinearVelocity()).Dot(slider_axis) * slider_axis;
+			inRenderer->DrawLine(position2, position2 + cur_vel, Color::sBlue);
+			inRenderer->DrawArrow(position2 + cur_vel, position2 + mTargetVelocity * slider_axis, Color::sRed, 0.1f);
+			break;
+		}
+
+	case EMotorState::Off:
+		break;
+	}
+}
+
+void SliderConstraint::DrawConstraintLimits(DebugRenderer *inRenderer) const
+{
+	if (mHasLimits)
+	{
+		RMat44 transform1 = mBody1->GetCenterOfMassTransform();
+		RMat44 transform2 = mBody2->GetCenterOfMassTransform();
+
+		// Transform the local positions into world space
+		Vec3 slider_axis = transform1.Multiply3x3(mLocalSpaceSliderAxis1);
+		RVec3 position1 = transform1 * mLocalSpacePosition1;
+		RVec3 position2 = transform2 * mLocalSpacePosition2;
+
+		// Calculate the limits in world space
+		RVec3 limits_min = position1 + mLimitsMin * slider_axis;
+		RVec3 limits_max = position1 + mLimitsMax * slider_axis;
+
+		inRenderer->DrawLine(limits_min, position1, Color::sWhite);
+		inRenderer->DrawLine(position2, limits_max, Color::sWhite);
+
+		inRenderer->DrawMarker(limits_min, Color::sWhite, 0.1f);
+		inRenderer->DrawMarker(limits_max, Color::sWhite, 0.1f);
+	}
+}
+#endif // JPH_DEBUG_RENDERER
+
+void SliderConstraint::SaveState(StateRecorder &inStream) const
+{
+	TwoBodyConstraint::SaveState(inStream);
+
+	mMotorConstraintPart.SaveState(inStream);
+	mPositionConstraintPart.SaveState(inStream);
+	mRotationConstraintPart.SaveState(inStream);
+	mPositionLimitsConstraintPart.SaveState(inStream);
+
+	inStream.Write(mMotorState);
+	inStream.Write(mTargetVelocity);
+	inStream.Write(mTargetPosition);
+}
+
+void SliderConstraint::RestoreState(StateRecorder &inStream)
+{
+	TwoBodyConstraint::RestoreState(inStream);
+
+	mMotorConstraintPart.RestoreState(inStream);
+	mPositionConstraintPart.RestoreState(inStream);
+	mRotationConstraintPart.RestoreState(inStream);
+	mPositionLimitsConstraintPart.RestoreState(inStream);
+
+	inStream.Read(mMotorState);
+	inStream.Read(mTargetVelocity);
+	inStream.Read(mTargetPosition);
+}
+
+Ref<ConstraintSettings> SliderConstraint::GetConstraintSettings() const
+{
+	SliderConstraintSettings *settings = new SliderConstraintSettings;
+	ToConstraintSettings(*settings);
+	settings->mSpace = EConstraintSpace::LocalToBodyCOM;
+	settings->mPoint1 = RVec3(mLocalSpacePosition1);
+	settings->mSliderAxis1 = mLocalSpaceSliderAxis1;
+	settings->mNormalAxis1 = mLocalSpaceNormal1;
+	settings->mPoint2 = RVec3(mLocalSpacePosition2);
+	Mat44 inv_initial_rotation = Mat44::sRotation(mInvInitialOrientation);
+	settings->mSliderAxis2 = inv_initial_rotation.Multiply3x3(mLocalSpaceSliderAxis1);
+	settings->mNormalAxis2 = inv_initial_rotation.Multiply3x3(mLocalSpaceNormal1);
+	settings->mLimitsMin = mLimitsMin;
+	settings->mLimitsMax = mLimitsMax;
+	settings->mLimitsSpringSettings = mLimitsSpringSettings;
+	settings->mMaxFrictionForce = mMaxFrictionForce;
+	settings->mMotorSettings = mMotorSettings;
+	return settings;
+}
+
+Mat44 SliderConstraint::GetConstraintToBody1Matrix() const
+{
+	return Mat44(Vec4(mLocalSpaceSliderAxis1, 0), Vec4(mLocalSpaceNormal1, 0), Vec4(mLocalSpaceNormal2, 0), Vec4(mLocalSpacePosition1, 1));
+}
+
+Mat44 SliderConstraint::GetConstraintToBody2Matrix() const
+{
+	Mat44 mat = Mat44::sRotation(mInvInitialOrientation).Multiply3x3(Mat44(Vec4(mLocalSpaceSliderAxis1, 0), Vec4(mLocalSpaceNormal1, 0), Vec4(mLocalSpaceNormal2, 0), Vec4(0, 0, 0, 1)));
+	mat.SetTranslation(mLocalSpacePosition2);
+	return mat;
+}
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Constraints/SliderConstraint.h

@@ -0,0 +1,198 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Constraints/TwoBodyConstraint.h>
+#include <Jolt/Physics/Constraints/MotorSettings.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/DualAxisConstraintPart.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/RotationEulerConstraintPart.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/AxisConstraintPart.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Slider constraint settings, used to create a slider constraint
+class JPH_EXPORT SliderConstraintSettings final : public TwoBodyConstraintSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, SliderConstraintSettings)
+
+public:
+	// See: ConstraintSettings::SaveBinaryState
+	virtual void				SaveBinaryState(StreamOut &inStream) const override;
+
+	/// Create an instance of this constraint.
+	/// Note that the rotation constraint will be solved from body 1. This means that if body 1 and body 2 have different masses / inertias (kinematic body = infinite mass / inertia), body 1 should be the heaviest body.
+	virtual TwoBodyConstraint *	Create(Body &inBody1, Body &inBody2) const override;
+
+	/// Simple way of setting the slider and normal axis in world space (assumes the bodies are already oriented correctly when the constraint is created)
+	void						SetSliderAxis(Vec3Arg inSliderAxis);
+
+	/// This determines in which space the constraint is setup, all properties below should be in the specified space
+	EConstraintSpace			mSpace = EConstraintSpace::WorldSpace;
+
+	/// When mSpace is WorldSpace mPoint1 and mPoint2 can be automatically calculated based on the positions of the bodies when the constraint is created (the current relative position/orientation is chosen as the '0' position). Set this to false if you want to supply the attachment points yourself.
+	bool						mAutoDetectPoint = false;
+
+	/// Body 1 constraint reference frame (space determined by mSpace).
+	/// Slider axis is the axis along which movement is possible (direction), normal axis is a perpendicular vector to define the frame.
+	RVec3						mPoint1 = RVec3::sZero();
+	Vec3						mSliderAxis1 = Vec3::sAxisX();
+	Vec3						mNormalAxis1 = Vec3::sAxisY();
+
+	/// Body 2 constraint reference frame (space determined by mSpace)
+	RVec3						mPoint2 = RVec3::sZero();
+	Vec3						mSliderAxis2 = Vec3::sAxisX();
+	Vec3						mNormalAxis2 = Vec3::sAxisY();
+
+	/// When the bodies move so that mPoint1 coincides with mPoint2 the slider position is defined to be 0, movement will be limited between [mLimitsMin, mLimitsMax] where mLimitsMin e [-inf, 0] and mLimitsMax e [0, inf]
+	float						mLimitsMin = -FLT_MAX;
+	float						mLimitsMax = FLT_MAX;
+
+	/// When enabled, this makes the limits soft. When the constraint exceeds the limits, a spring force will pull it back.
+	SpringSettings				mLimitsSpringSettings;
+
+	/// Maximum amount of friction force to apply (N) when not driven by a motor.
+	float						mMaxFrictionForce = 0.0f;
+
+	/// In case the constraint is powered, this determines the motor settings around the sliding axis
+	MotorSettings				mMotorSettings;
+
+protected:
+	// See: ConstraintSettings::RestoreBinaryState
+	virtual void				RestoreBinaryState(StreamIn &inStream) override;
+};
+
+/// A slider constraint allows movement in only 1 axis (and no rotation). Also known as a prismatic constraint.
+class JPH_EXPORT SliderConstraint final : public TwoBodyConstraint
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Construct slider constraint
+								SliderConstraint(Body &inBody1, Body &inBody2, const SliderConstraintSettings &inSettings);
+
+	// Generic interface of a constraint
+	virtual EConstraintSubType	GetSubType() const override								{ return EConstraintSubType::Slider; }
+	virtual void				NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override;
+	virtual void				SetupVelocityConstraint(float inDeltaTime) override;
+	virtual void				ResetWarmStart() override;
+	virtual void				WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override;
+	virtual bool				SolveVelocityConstraint(float inDeltaTime) override;
+	virtual bool				SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override;
+#ifdef JPH_DEBUG_RENDERER
+	virtual void				DrawConstraint(DebugRenderer *inRenderer) const override;
+	virtual void				DrawConstraintLimits(DebugRenderer *inRenderer) const override;
+#endif // JPH_DEBUG_RENDERER
+	virtual void				SaveState(StateRecorder &inStream) const override;
+	virtual void				RestoreState(StateRecorder &inStream) override;
+	virtual Ref<ConstraintSettings> GetConstraintSettings() const override;
+
+	// See: TwoBodyConstraint
+	virtual Mat44				GetConstraintToBody1Matrix() const override;
+	virtual Mat44				GetConstraintToBody2Matrix() const override;
+
+	/// Get the current distance from the rest position
+	float						GetCurrentPosition() const;
+
+	/// Friction control
+	void						SetMaxFrictionForce(float inFrictionForce)				{ mMaxFrictionForce = inFrictionForce; }
+	float						GetMaxFrictionForce() const								{ return mMaxFrictionForce; }
+
+	/// Motor settings
+	MotorSettings &				GetMotorSettings()										{ return mMotorSettings; }
+	const MotorSettings &		GetMotorSettings() const								{ return mMotorSettings; }
+
+	// Motor controls
+	void						SetMotorState(EMotorState inState)						{ JPH_ASSERT(inState == EMotorState::Off || mMotorSettings.IsValid()); mMotorState = inState; }
+	EMotorState					GetMotorState() const									{ return mMotorState; }
+	void						SetTargetVelocity(float inVelocity)						{ mTargetVelocity = inVelocity; }
+	float						GetTargetVelocity() const								{ return mTargetVelocity; }
+	void						SetTargetPosition(float inPosition)						{ mTargetPosition = mHasLimits? Clamp(inPosition, mLimitsMin, mLimitsMax) : inPosition; }
+	float						GetTargetPosition() const								{ return mTargetPosition; }
+
+	/// Update the limits of the slider constraint (see SliderConstraintSettings)
+	void						SetLimits(float inLimitsMin, float inLimitsMax);
+	float						GetLimitsMin() const									{ return mLimitsMin; }
+	float						GetLimitsMax() const									{ return mLimitsMax; }
+	bool						HasLimits() const										{ return mHasLimits; }
+
+	/// Update the limits spring settings
+	const SpringSettings &		GetLimitsSpringSettings() const							{ return mLimitsSpringSettings; }
+	SpringSettings &			GetLimitsSpringSettings()								{ return mLimitsSpringSettings; }
+	void						SetLimitsSpringSettings(const SpringSettings &inLimitsSpringSettings) { mLimitsSpringSettings = inLimitsSpringSettings; }
+
+	///@name Get Lagrange multiplier from last physics update (the linear/angular impulse applied to satisfy the constraint)
+	inline Vector<2>			GetTotalLambdaPosition() const							{ return mPositionConstraintPart.GetTotalLambda(); }
+	inline float				GetTotalLambdaPositionLimits() const					{ return mPositionLimitsConstraintPart.GetTotalLambda(); }
+	inline Vec3					GetTotalLambdaRotation() const							{ return mRotationConstraintPart.GetTotalLambda(); }
+	inline float				GetTotalLambdaMotor() const								{ return mMotorConstraintPart.GetTotalLambda(); }
+
+private:
+	// Internal helper function to calculate the values below
+	void						CalculateR1R2U(Mat44Arg inRotation1, Mat44Arg inRotation2);
+	void						CalculateSlidingAxisAndPosition(Mat44Arg inRotation1);
+	void						CalculatePositionConstraintProperties(Mat44Arg inRotation1, Mat44Arg inRotation2);
+	void						CalculatePositionLimitsConstraintProperties(float inDeltaTime);
+	void						CalculateMotorConstraintProperties(float inDeltaTime);
+
+	// CONFIGURATION PROPERTIES FOLLOW
+
+	// Local space constraint positions
+	Vec3						mLocalSpacePosition1;
+	Vec3						mLocalSpacePosition2;
+
+	// Local space sliding direction
+	Vec3						mLocalSpaceSliderAxis1;
+
+	// Local space normals to the sliding direction (in body 1 space)
+	Vec3						mLocalSpaceNormal1;
+	Vec3						mLocalSpaceNormal2;
+
+	// Inverse of initial rotation from body 1 to body 2 in body 1 space
+	Quat						mInvInitialOrientation;
+
+	// Slider limits
+	bool						mHasLimits;
+	float						mLimitsMin;
+	float						mLimitsMax;
+
+	// Soft constraint limits
+	SpringSettings				mLimitsSpringSettings;
+
+	// Friction
+	float						mMaxFrictionForce;
+
+	// Motor controls
+	MotorSettings				mMotorSettings;
+	EMotorState					mMotorState = EMotorState::Off;
+	float						mTargetVelocity = 0.0f;
+	float						mTargetPosition = 0.0f;
+
+	// RUN TIME PROPERTIES FOLLOW
+
+	// Positions where the point constraint acts on (middle point between center of masses)
+	Vec3						mR1;
+	Vec3						mR2;
+
+	// X2 + R2 - X1 - R1
+	Vec3						mU;
+
+	// World space sliding direction
+	Vec3						mWorldSpaceSliderAxis;
+
+	// Normals to the slider axis
+	Vec3						mN1;
+	Vec3						mN2;
+
+	// Distance along the slide axis
+	float						mD = 0.0f;
+
+	// The constraint parts
+	DualAxisConstraintPart		mPositionConstraintPart;
+	RotationEulerConstraintPart	mRotationConstraintPart;
+	AxisConstraintPart			mPositionLimitsConstraintPart;
+	AxisConstraintPart			mMotorConstraintPart;
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Constraints/SpringSettings.cpp

@@ -0,0 +1,35 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Constraints/SpringSettings.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SpringSettings)
+{
+	JPH_ADD_ENUM_ATTRIBUTE(SpringSettings, mMode)
+	JPH_ADD_ATTRIBUTE(SpringSettings, mFrequency)
+	JPH_ADD_ATTRIBUTE(SpringSettings, mDamping)
+}
+
+void SpringSettings::SaveBinaryState(StreamOut &inStream) const
+{
+	inStream.Write(mMode);
+	inStream.Write(mFrequency);
+	inStream.Write(mDamping);
+}
+
+void SpringSettings::RestoreBinaryState(StreamIn &inStream)
+{
+	inStream.Read(mMode);
+	inStream.Read(mFrequency);
+	inStream.Read(mDamping);
+}
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Constraints/SpringSettings.h

@@ -0,0 +1,70 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/ObjectStream/SerializableObject.h>
+
+JPH_NAMESPACE_BEGIN
+
+class StreamIn;
+class StreamOut;
+
+/// Enum used by constraints to specify how the spring is defined
+enum class ESpringMode : uint8
+{
+	FrequencyAndDamping,		///< Frequency and damping are specified
+	StiffnessAndDamping,		///< Stiffness and damping are specified
+};
+
+/// Settings for a linear or angular spring
+class JPH_EXPORT SpringSettings
+{
+	JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, SpringSettings)
+
+public:
+	/// Constructor
+								SpringSettings() = default;
+								SpringSettings(const SpringSettings &) = default;
+	SpringSettings &			operator = (const SpringSettings &) = default;
+								SpringSettings(ESpringMode inMode, float inFrequencyOrStiffness, float inDamping) : mMode(inMode), mFrequency(inFrequencyOrStiffness), mDamping(inDamping) { }
+
+	/// Saves the contents of the spring settings in binary form to inStream.
+	void						SaveBinaryState(StreamOut &inStream) const;
+
+	/// Restores contents from the binary stream inStream.
+	void						RestoreBinaryState(StreamIn &inStream);
+
+	/// Check if the spring has a valid frequency / stiffness, if not the spring will be hard
+	inline bool					HasStiffness() const							{ return mFrequency > 0.0f; }
+
+	/// Selects the way in which the spring is defined
+	/// If the mode is StiffnessAndDamping then mFrequency becomes the stiffness (k) and mDamping becomes the damping ratio (c) in the spring equation F = -k * x - c * v. Otherwise the properties are as documented.
+	ESpringMode					mMode = ESpringMode::FrequencyAndDamping;
+
+	union
+	{
+		/// Valid when mSpringMode = ESpringMode::FrequencyAndDamping.
+		/// If mFrequency > 0 the constraint will be soft and mFrequency specifies the oscillation frequency in Hz.
+		/// If mFrequency <= 0, mDamping is ignored and the constraint will have hard limits (as hard as the time step / the number of velocity / position solver steps allows).
+		float					mFrequency = 0.0f;
+
+		/// Valid when mSpringMode = ESpringMode::StiffnessAndDamping.
+		/// If mStiffness > 0 the constraint will be soft and mStiffness specifies the stiffness (k) in the spring equation F = -k * x - c * v for a linear or T = -k * theta - c * w for an angular spring.
+		/// If mStiffness <= 0, mDamping is ignored and the constraint will have hard limits (as hard as the time step / the number of velocity / position solver steps allows).
+		///
+		/// Note that stiffness values are large numbers. To calculate a ballpark value for the needed stiffness you can use:
+		/// force = stiffness * delta_spring_length = mass * gravity <=> stiffness = mass * gravity / delta_spring_length.
+		/// So if your object weighs 1500 kg and the spring compresses by 2 meters, you need a stiffness in the order of 1500 * 9.81 / 2 ~ 7500 N/m.
+		float					mStiffness;
+	};
+
+	/// When mSpringMode = ESpringMode::FrequencyAndDamping mDamping is the damping ratio (0 = no damping, 1 = critical damping).
+	/// When mSpringMode = ESpringMode::StiffnessAndDamping mDamping is the damping (c) in the spring equation F = -k * x - c * v for a linear or T = -k * theta - c * w for an angular spring.
+	/// Note that if you set mDamping = 0, you will not get an infinite oscillation. Because we integrate physics using an explicit Euler scheme, there is always energy loss.
+	/// This is done to keep the simulation from exploding, because with a damping of 0 and even the slightest rounding error, the oscillation could become bigger and bigger until the simulation explodes.
+	float						mDamping = 0.0f;
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Constraints/SwingTwistConstraint.cpp

@@ -0,0 +1,524 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Constraints/SwingTwistConstraint.h>
+#include <Jolt/Physics/Body/Body.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(SwingTwistConstraintSettings)
+{
+	JPH_ADD_BASE_CLASS(SwingTwistConstraintSettings, TwoBodyConstraintSettings)
+
+	JPH_ADD_ENUM_ATTRIBUTE(SwingTwistConstraintSettings, mSpace)
+	JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mPosition1)
+	JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mTwistAxis1)
+	JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mPlaneAxis1)
+	JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mPosition2)
+	JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mTwistAxis2)
+	JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mPlaneAxis2)
+	JPH_ADD_ENUM_ATTRIBUTE(SwingTwistConstraintSettings, mSwingType)
+	JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mNormalHalfConeAngle)
+	JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mPlaneHalfConeAngle)
+	JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mTwistMinAngle)
+	JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mTwistMaxAngle)
+	JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mMaxFrictionTorque)
+	JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mSwingMotorSettings)
+	JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mTwistMotorSettings)
+}
+
+void SwingTwistConstraintSettings::SaveBinaryState(StreamOut &inStream) const
+{
+	ConstraintSettings::SaveBinaryState(inStream);
+
+	inStream.Write(mSpace);
+	inStream.Write(mPosition1);
+	inStream.Write(mTwistAxis1);
+	inStream.Write(mPlaneAxis1);
+	inStream.Write(mPosition2);
+	inStream.Write(mTwistAxis2);
+	inStream.Write(mPlaneAxis2);
+	inStream.Write(mSwingType);
+	inStream.Write(mNormalHalfConeAngle);
+	inStream.Write(mPlaneHalfConeAngle);
+	inStream.Write(mTwistMinAngle);
+	inStream.Write(mTwistMaxAngle);
+	inStream.Write(mMaxFrictionTorque);
+	mSwingMotorSettings.SaveBinaryState(inStream);
+	mTwistMotorSettings.SaveBinaryState(inStream);
+}
+
+void SwingTwistConstraintSettings::RestoreBinaryState(StreamIn &inStream)
+{
+	ConstraintSettings::RestoreBinaryState(inStream);
+
+	inStream.Read(mSpace);
+	inStream.Read(mPosition1);
+	inStream.Read(mTwistAxis1);
+	inStream.Read(mPlaneAxis1);
+	inStream.Read(mPosition2);
+	inStream.Read(mTwistAxis2);
+	inStream.Read(mPlaneAxis2);
+	inStream.Read(mSwingType);
+	inStream.Read(mNormalHalfConeAngle);
+	inStream.Read(mPlaneHalfConeAngle);
+	inStream.Read(mTwistMinAngle);
+	inStream.Read(mTwistMaxAngle);
+	inStream.Read(mMaxFrictionTorque);
+	mSwingMotorSettings.RestoreBinaryState(inStream);
+	mTwistMotorSettings.RestoreBinaryState(inStream);
+}
+
+TwoBodyConstraint *SwingTwistConstraintSettings::Create(Body &inBody1, Body &inBody2) const
+{
+	return new SwingTwistConstraint(inBody1, inBody2, *this);
+}
+
+void SwingTwistConstraint::UpdateLimits()
+{
+	// Pass limits on to swing twist constraint part
+	mSwingTwistConstraintPart.SetLimits(mTwistMinAngle, mTwistMaxAngle, -mPlaneHalfConeAngle, mPlaneHalfConeAngle, -mNormalHalfConeAngle, mNormalHalfConeAngle);
+}
+
+SwingTwistConstraint::SwingTwistConstraint(Body &inBody1, Body &inBody2, const SwingTwistConstraintSettings &inSettings) :
+	TwoBodyConstraint(inBody1, inBody2, inSettings),
+	mNormalHalfConeAngle(inSettings.mNormalHalfConeAngle),
+	mPlaneHalfConeAngle(inSettings.mPlaneHalfConeAngle),
+	mTwistMinAngle(inSettings.mTwistMinAngle),
+	mTwistMaxAngle(inSettings.mTwistMaxAngle),
+	mMaxFrictionTorque(inSettings.mMaxFrictionTorque),
+	mSwingMotorSettings(inSettings.mSwingMotorSettings),
+	mTwistMotorSettings(inSettings.mTwistMotorSettings)
+{
+	// Override swing type
+	mSwingTwistConstraintPart.SetSwingType(inSettings.mSwingType);
+
+	// Calculate rotation needed to go from constraint space to body1 local space
+	Vec3 normal_axis1 = inSettings.mPlaneAxis1.Cross(inSettings.mTwistAxis1);
+	Mat44 c_to_b1(Vec4(inSettings.mTwistAxis1, 0), Vec4(normal_axis1, 0), Vec4(inSettings.mPlaneAxis1, 0), Vec4(0, 0, 0, 1));
+	mConstraintToBody1 = c_to_b1.GetQuaternion();
+
+	// Calculate rotation needed to go from constraint space to body2 local space
+	Vec3 normal_axis2 = inSettings.mPlaneAxis2.Cross(inSettings.mTwistAxis2);
+	Mat44 c_to_b2(Vec4(inSettings.mTwistAxis2, 0), Vec4(normal_axis2, 0), Vec4(inSettings.mPlaneAxis2, 0), Vec4(0, 0, 0, 1));
+	mConstraintToBody2 = c_to_b2.GetQuaternion();
+
+	if (inSettings.mSpace == EConstraintSpace::WorldSpace)
+	{
+		// If all properties were specified in world space, take them to local space now
+		mLocalSpacePosition1 = Vec3(inBody1.GetInverseCenterOfMassTransform() * inSettings.mPosition1);
+		mConstraintToBody1 = inBody1.GetRotation().Conjugated() * mConstraintToBody1;
+
+		mLocalSpacePosition2 = Vec3(inBody2.GetInverseCenterOfMassTransform() * inSettings.mPosition2);
+		mConstraintToBody2 = inBody2.GetRotation().Conjugated() * mConstraintToBody2;
+	}
+	else
+	{
+		mLocalSpacePosition1 = Vec3(inSettings.mPosition1);
+		mLocalSpacePosition2 = Vec3(inSettings.mPosition2);
+	}
+
+	UpdateLimits();
+}
+
+void SwingTwistConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM)
+{
+	if (mBody1->GetID() == inBodyID)
+		mLocalSpacePosition1 -= inDeltaCOM;
+	else if (mBody2->GetID() == inBodyID)
+		mLocalSpacePosition2 -= inDeltaCOM;
+}
+
+Quat SwingTwistConstraint::GetRotationInConstraintSpace() const
+{
+	// Let b1, b2 be the center of mass transform of body1 and body2 (For body1 this is mBody1->GetCenterOfMassTransform())
+	// Let c1, c2 be the transform that takes a vector from constraint space to local space of body1 and body2 (For body1 this is Mat44::sRotationTranslation(mConstraintToBody1, mLocalSpacePosition1))
+	// Let q be the rotation of the constraint in constraint space
+	// b2 takes a vector from the local space of body2 to world space
+	// To express this in terms of b1: b2 = b1 * c1 * q * c2^-1
+	// c2^-1 goes from local body 2 space to constraint space
+	// q rotates the constraint
+	// c1 goes from constraint space to body 1 local space
+	// b1 goes from body 1 local space to world space
+	// So when the body rotations are given, q = (b1 * c1)^-1 * b2 c2
+	// Or: q = (q1 * c1)^-1 * (q2 * c2) if we're only interested in rotations
+	Quat constraint_body1_to_world = mBody1->GetRotation() * mConstraintToBody1;
+	Quat constraint_body2_to_world = mBody2->GetRotation() * mConstraintToBody2;
+	return constraint_body1_to_world.Conjugated() * constraint_body2_to_world;
+}
+
+void SwingTwistConstraint::SetSwingMotorState(EMotorState inState)
+{
+	JPH_ASSERT(inState == EMotorState::Off || mSwingMotorSettings.IsValid());
+
+	if (mSwingMotorState != inState)
+	{
+		mSwingMotorState = inState;
+
+		// Ensure that warm starting next frame doesn't apply any impulses (motor parts are repurposed for different modes)
+		for (AngleConstraintPart &c : mMotorConstraintPart)
+			c.Deactivate();
+	}
+}
+
+void SwingTwistConstraint::SetTwistMotorState(EMotorState inState)
+{
+	JPH_ASSERT(inState == EMotorState::Off || mTwistMotorSettings.IsValid());
+
+	if (mTwistMotorState != inState)
+	{
+		mTwistMotorState = inState;
+
+		// Ensure that warm starting next frame doesn't apply any impulses (motor parts are repurposed for different modes)
+		mMotorConstraintPart[0].Deactivate();
+	}
+}
+
+void SwingTwistConstraint::SetTargetOrientationCS(QuatArg inOrientation)
+{
+	Quat q_swing, q_twist;
+	inOrientation.GetSwingTwist(q_swing, q_twist);
+
+	uint clamped_axis;
+	mSwingTwistConstraintPart.ClampSwingTwist(q_swing, q_twist, clamped_axis);
+
+	if (clamped_axis != 0)
+		mTargetOrientation = q_swing * q_twist;
+	else
+		mTargetOrientation = inOrientation;
+}
+
+void SwingTwistConstraint::SetupVelocityConstraint(float inDeltaTime)
+{
+	// Setup point constraint
+	Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation());
+	Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation());
+	mPointConstraintPart.CalculateConstraintProperties(*mBody1, rotation1, mLocalSpacePosition1, *mBody2, rotation2, mLocalSpacePosition2);
+
+	// GetRotationInConstraintSpace written out since we reuse the sub expressions
+	Quat constraint_body1_to_world = mBody1->GetRotation() * mConstraintToBody1;
+	Quat constraint_body2_to_world = mBody2->GetRotation() * mConstraintToBody2;
+	Quat q = constraint_body1_to_world.Conjugated() * constraint_body2_to_world;
+
+	// Calculate constraint properties for the swing twist limit
+	mSwingTwistConstraintPart.CalculateConstraintProperties(*mBody1, *mBody2, q, constraint_body1_to_world);
+
+	if (mSwingMotorState != EMotorState::Off || mTwistMotorState != EMotorState::Off || mMaxFrictionTorque > 0.0f)
+	{
+		// Calculate rotation motor axis
+		Mat44 ws_axis = Mat44::sRotation(constraint_body2_to_world);
+		for (int i = 0; i < 3; ++i)
+			mWorldSpaceMotorAxis[i] = ws_axis.GetColumn3(i);
+
+		Vec3 rotation_error;
+		if (mSwingMotorState == EMotorState::Position || mTwistMotorState == EMotorState::Position)
+		{
+			// Get target orientation along the shortest path from q
+			Quat target_orientation = q.Dot(mTargetOrientation) > 0.0f? mTargetOrientation : -mTargetOrientation;
+
+			// The definition of the constraint rotation q:
+			// R2 * ConstraintToBody2 = R1 * ConstraintToBody1 * q (1)
+			//
+			// R2' is the rotation of body 2 when reaching the target_orientation:
+			// R2' * ConstraintToBody2 = R1 * ConstraintToBody1 * target_orientation (2)
+			//
+			// The difference in body 2 space:
+			// R2' = R2 * diff_body2 (3)
+			//
+			// We want to specify the difference in the constraint space of body 2:
+			// diff_body2 = ConstraintToBody2 * diff * ConstraintToBody2^* (4)
+			//
+			// Extracting R2' from 2: R2' = R1 * ConstraintToBody1 * target_orientation * ConstraintToBody2^* (5)
+			// Combining 3 & 4: R2' = R2 * ConstraintToBody2 * diff * ConstraintToBody2^* (6)
+			// Combining 1 & 6: R2' = R1 * ConstraintToBody1 * q * diff * ConstraintToBody2^* (7)
+			// Combining 5 & 7: R1 * ConstraintToBody1 * target_orientation * ConstraintToBody2^* = R1 * ConstraintToBody1 * q * diff * ConstraintToBody2^*
+			// <=> target_orientation = q * diff
+			// <=> diff = q^* * target_orientation
+			Quat diff = q.Conjugated() * target_orientation;
+
+			// Approximate error angles
+			// The imaginary part of a quaternion is rotation_axis * sin(angle / 2)
+			// If angle is small, sin(x) = x so angle[i] ~ 2.0f * rotation_axis[i]
+			// We'll be making small time steps, so if the angle is not small at least the sign will be correct and we'll move in the right direction
+			rotation_error = -2.0f * diff.GetXYZ();
+		}
+
+		// Swing motor
+		switch (mSwingMotorState)
+		{
+		case EMotorState::Off:
+			if (mMaxFrictionTorque > 0.0f)
+			{
+				// Enable friction
+				for (int i = 1; i < 3; ++i)
+					mMotorConstraintPart[i].CalculateConstraintProperties(*mBody1, *mBody2, mWorldSpaceMotorAxis[i], 0.0f);
+			}
+			else
+			{
+				// Disable friction
+				for (AngleConstraintPart &c : mMotorConstraintPart)
+					c.Deactivate();
+			}
+			break;
+
+		case EMotorState::Velocity:
+			// Use motor to create angular velocity around desired axis
+			for (int i = 1; i < 3; ++i)
+				mMotorConstraintPart[i].CalculateConstraintProperties(*mBody1, *mBody2, mWorldSpaceMotorAxis[i], -mTargetAngularVelocity[i]);
+			break;
+
+		case EMotorState::Position:
+			// Use motor to drive rotation error to zero
+			if (mSwingMotorSettings.mSpringSettings.HasStiffness())
+			{
+				for (int i = 1; i < 3; ++i)
+					mMotorConstraintPart[i].CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, *mBody2, mWorldSpaceMotorAxis[i], 0.0f, rotation_error[i], mSwingMotorSettings.mSpringSettings);
+			}
+			else
+			{
+				for (int i = 1; i < 3; ++i)
+					mMotorConstraintPart[i].Deactivate();
+			}
+			break;
+		}
+
+		// Twist motor
+		switch (mTwistMotorState)
+		{
+		case EMotorState::Off:
+			if (mMaxFrictionTorque > 0.0f)
+			{
+				// Enable friction
+				mMotorConstraintPart[0].CalculateConstraintProperties(*mBody1, *mBody2, mWorldSpaceMotorAxis[0], 0.0f);
+			}
+			else
+			{
+				// Disable friction
+				mMotorConstraintPart[0].Deactivate();
+			}
+			break;
+
+		case EMotorState::Velocity:
+			// Use motor to create angular velocity around desired axis
+			mMotorConstraintPart[0].CalculateConstraintProperties(*mBody1, *mBody2, mWorldSpaceMotorAxis[0], -mTargetAngularVelocity[0]);
+			break;
+
+		case EMotorState::Position:
+			// Use motor to drive rotation error to zero
+			if (mTwistMotorSettings.mSpringSettings.HasStiffness())
+				mMotorConstraintPart[0].CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, *mBody2, mWorldSpaceMotorAxis[0], 0.0f, rotation_error[0], mTwistMotorSettings.mSpringSettings);
+			else
+				mMotorConstraintPart[0].Deactivate();
+			break;
+		}
+	}
+	else
+	{
+		// Disable rotation motor
+		for (AngleConstraintPart &c : mMotorConstraintPart)
+			c.Deactivate();
+	}
+}
+
+void SwingTwistConstraint::ResetWarmStart()
+{
+	for (AngleConstraintPart &c : mMotorConstraintPart)
+		c.Deactivate();
+	mSwingTwistConstraintPart.Deactivate();
+	mPointConstraintPart.Deactivate();
+}
+
+void SwingTwistConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio)
+{
+	// Warm starting: Apply previous frame impulse
+	for (AngleConstraintPart &c : mMotorConstraintPart)
+		c.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio);
+	mSwingTwistConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio);
+	mPointConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio);
+}
+
+bool SwingTwistConstraint::SolveVelocityConstraint(float inDeltaTime)
+{
+	bool impulse = false;
+
+	// Solve twist rotation motor
+	if (mMotorConstraintPart[0].IsActive())
+	{
+		// Twist limits
+		float min_twist_limit, max_twist_limit;
+		if (mTwistMotorState == EMotorState::Off)
+		{
+			max_twist_limit = inDeltaTime * mMaxFrictionTorque;
+			min_twist_limit = -max_twist_limit;
+		}
+		else
+		{
+			min_twist_limit = inDeltaTime * mTwistMotorSettings.mMinTorqueLimit;
+			max_twist_limit = inDeltaTime * mTwistMotorSettings.mMaxTorqueLimit;
+		}
+
+		impulse |= mMotorConstraintPart[0].SolveVelocityConstraint(*mBody1, *mBody2, mWorldSpaceMotorAxis[0], min_twist_limit, max_twist_limit);
+	}
+
+	// Solve swing rotation motor
+	if (mMotorConstraintPart[1].IsActive())
+	{
+		// Swing parts should turn on / off together
+		JPH_ASSERT(mMotorConstraintPart[2].IsActive());
+
+		// Swing limits
+		float min_swing_limit, max_swing_limit;
+		if (mSwingMotorState == EMotorState::Off)
+		{
+			max_swing_limit = inDeltaTime * mMaxFrictionTorque;
+			min_swing_limit = -max_swing_limit;
+		}
+		else
+		{
+			min_swing_limit = inDeltaTime * mSwingMotorSettings.mMinTorqueLimit;
+			max_swing_limit = inDeltaTime * mSwingMotorSettings.mMaxTorqueLimit;
+		}
+
+		for (int i = 1; i < 3; ++i)
+			impulse |= mMotorConstraintPart[i].SolveVelocityConstraint(*mBody1, *mBody2, mWorldSpaceMotorAxis[i], min_swing_limit, max_swing_limit);
+	}
+	else
+	{
+		// Swing parts should turn on / off together
+		JPH_ASSERT(!mMotorConstraintPart[2].IsActive());
+	}
+
+	// Solve rotation limits
+	impulse |= mSwingTwistConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2);
+
+	// Solve position constraint
+	impulse |= mPointConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2);
+
+	return impulse;
+}
+
+bool SwingTwistConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte)
+{
+	bool impulse = false;
+
+	// Solve rotation violations
+	Quat q = GetRotationInConstraintSpace();
+	impulse |= mSwingTwistConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, q, mConstraintToBody1, mConstraintToBody2, inBaumgarte);
+
+	// Solve position violations
+	mPointConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(mBody1->GetRotation()), mLocalSpacePosition1, *mBody2, Mat44::sRotation(mBody2->GetRotation()), mLocalSpacePosition2);
+	impulse |= mPointConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, inBaumgarte);
+
+	return impulse;
+}
+
+#ifdef JPH_DEBUG_RENDERER
+void SwingTwistConstraint::DrawConstraint(DebugRenderer *inRenderer) const
+{
+	// Get constraint properties in world space
+	RMat44 transform1 = mBody1->GetCenterOfMassTransform();
+	RVec3 position1 = transform1 * mLocalSpacePosition1;
+	Quat rotation1 = mBody1->GetRotation() * mConstraintToBody1;
+	Quat rotation2 = mBody2->GetRotation() * mConstraintToBody2;
+
+	// Draw constraint orientation
+	inRenderer->DrawCoordinateSystem(RMat44::sRotationTranslation(rotation1, position1), mDrawConstraintSize);
+
+	// Draw current swing and twist
+	Quat q = GetRotationInConstraintSpace();
+	Quat q_swing, q_twist;
+	q.GetSwingTwist(q_swing, q_twist);
+	inRenderer->DrawLine(position1, position1 + mDrawConstraintSize * (rotation1 * q_twist).RotateAxisY(), Color::sWhite);
+	inRenderer->DrawLine(position1, position1 + mDrawConstraintSize * (rotation1 * q_swing).RotateAxisX(), Color::sWhite);
+
+	if (mSwingMotorState == EMotorState::Velocity || mTwistMotorState == EMotorState::Velocity)
+	{
+		// Draw target angular velocity
+		inRenderer->DrawArrow(position1, position1 + rotation2 * mTargetAngularVelocity, Color::sRed, 0.1f);
+	}
+	if (mSwingMotorState == EMotorState::Position || mTwistMotorState == EMotorState::Position)
+	{
+		// Draw motor swing and twist
+		Quat swing, twist;
+		mTargetOrientation.GetSwingTwist(swing, twist);
+		inRenderer->DrawLine(position1, position1 + mDrawConstraintSize * (rotation1 * twist).RotateAxisY(), Color::sYellow);
+		inRenderer->DrawLine(position1, position1 + mDrawConstraintSize * (rotation1 * swing).RotateAxisX(), Color::sCyan);
+	}
+}
+
+void SwingTwistConstraint::DrawConstraintLimits(DebugRenderer *inRenderer) const
+{
+	// Get matrix that transforms from constraint space to world space
+	RMat44 constraint_to_world = RMat44::sRotationTranslation(mBody1->GetRotation() * mConstraintToBody1, mBody1->GetCenterOfMassTransform() * mLocalSpacePosition1);
+
+	// Draw limits
+	if (mSwingTwistConstraintPart.GetSwingType() == ESwingType::Pyramid)
+		inRenderer->DrawSwingPyramidLimits(constraint_to_world, -mPlaneHalfConeAngle, mPlaneHalfConeAngle, -mNormalHalfConeAngle, mNormalHalfConeAngle, mDrawConstraintSize, Color::sGreen, DebugRenderer::ECastShadow::Off);
+	else
+		inRenderer->DrawSwingConeLimits(constraint_to_world, mPlaneHalfConeAngle, mNormalHalfConeAngle, mDrawConstraintSize, Color::sGreen, DebugRenderer::ECastShadow::Off);
+	inRenderer->DrawPie(constraint_to_world.GetTranslation(), mDrawConstraintSize, constraint_to_world.GetAxisX(), constraint_to_world.GetAxisY(), mTwistMinAngle, mTwistMaxAngle, Color::sPurple, DebugRenderer::ECastShadow::Off);
+}
+#endif // JPH_DEBUG_RENDERER
+
+void SwingTwistConstraint::SaveState(StateRecorder &inStream) const
+{
+	TwoBodyConstraint::SaveState(inStream);
+
+	mPointConstraintPart.SaveState(inStream);
+	mSwingTwistConstraintPart.SaveState(inStream);
+	for (const AngleConstraintPart &c : mMotorConstraintPart)
+		c.SaveState(inStream);
+
+	inStream.Write(mSwingMotorState);
+	inStream.Write(mTwistMotorState);
+	inStream.Write(mTargetAngularVelocity);
+	inStream.Write(mTargetOrientation);
+}
+
+void SwingTwistConstraint::RestoreState(StateRecorder &inStream)
+{
+	TwoBodyConstraint::RestoreState(inStream);
+
+	mPointConstraintPart.RestoreState(inStream);
+	mSwingTwistConstraintPart.RestoreState(inStream);
+	for (AngleConstraintPart &c : mMotorConstraintPart)
+		c.RestoreState(inStream);
+
+	inStream.Read(mSwingMotorState);
+	inStream.Read(mTwistMotorState);
+	inStream.Read(mTargetAngularVelocity);
+	inStream.Read(mTargetOrientation);
+}
+
+Ref<ConstraintSettings> SwingTwistConstraint::GetConstraintSettings() const
+{
+	SwingTwistConstraintSettings *settings = new SwingTwistConstraintSettings;
+	ToConstraintSettings(*settings);
+	settings->mSpace = EConstraintSpace::LocalToBodyCOM;
+	settings->mPosition1 = RVec3(mLocalSpacePosition1);
+	settings->mTwistAxis1 = mConstraintToBody1.RotateAxisX();
+	settings->mPlaneAxis1 = mConstraintToBody1.RotateAxisZ();
+	settings->mPosition2 = RVec3(mLocalSpacePosition2);
+	settings->mTwistAxis2 = mConstraintToBody2.RotateAxisX();
+	settings->mPlaneAxis2 = mConstraintToBody2.RotateAxisZ();
+	settings->mSwingType = mSwingTwistConstraintPart.GetSwingType();
+	settings->mNormalHalfConeAngle = mNormalHalfConeAngle;
+	settings->mPlaneHalfConeAngle = mPlaneHalfConeAngle;
+	settings->mTwistMinAngle = mTwistMinAngle;
+	settings->mTwistMaxAngle = mTwistMaxAngle;
+	settings->mMaxFrictionTorque = mMaxFrictionTorque;
+	settings->mSwingMotorSettings = mSwingMotorSettings;
+	settings->mTwistMotorSettings = mTwistMotorSettings;
+	return settings;
+}
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Constraints/SwingTwistConstraint.h

@@ -0,0 +1,197 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Constraints/TwoBodyConstraint.h>
+#include <Jolt/Physics/Constraints/MotorSettings.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/PointConstraintPart.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/AngleConstraintPart.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/SwingTwistConstraintPart.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Swing twist constraint settings, used to create a swing twist constraint
+/// All values in this structure are copied to the swing twist constraint and the settings object is no longer needed afterwards.
+///
+/// This image describes the limit settings:
+/// @image html Docs/SwingTwistConstraint.png
+class JPH_EXPORT SwingTwistConstraintSettings final : public TwoBodyConstraintSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, SwingTwistConstraintSettings)
+
+public:
+	// See: ConstraintSettings::SaveBinaryState
+	virtual void				SaveBinaryState(StreamOut &inStream) const override;
+
+	/// Create an instance of this constraint
+	virtual TwoBodyConstraint *	Create(Body &inBody1, Body &inBody2) const override;
+
+	/// This determines in which space the constraint is setup, all properties below should be in the specified space
+	EConstraintSpace			mSpace = EConstraintSpace::WorldSpace;
+
+	///@name Body 1 constraint reference frame (space determined by mSpace)
+	RVec3						mPosition1 = RVec3::sZero();
+	Vec3						mTwistAxis1 = Vec3::sAxisX();
+	Vec3						mPlaneAxis1 = Vec3::sAxisY();
+
+	///@name Body 2 constraint reference frame (space determined by mSpace)
+	RVec3						mPosition2 = RVec3::sZero();
+	Vec3						mTwistAxis2 = Vec3::sAxisX();
+	Vec3						mPlaneAxis2 = Vec3::sAxisY();
+
+	/// The type of swing constraint that we want to use.
+	ESwingType					mSwingType = ESwingType::Cone;
+
+	///@name Swing rotation limits
+	float						mNormalHalfConeAngle = 0.0f;								///< See image at Detailed Description. Angle in radians.
+	float						mPlaneHalfConeAngle = 0.0f;									///< See image at Detailed Description. Angle in radians.
+
+	///@name Twist rotation limits
+	float						mTwistMinAngle = 0.0f;										///< See image at Detailed Description. Angle in radians. Should be \f$\in [-\pi, \pi]\f$.
+	float						mTwistMaxAngle = 0.0f;										///< See image at Detailed Description. Angle in radians. Should be \f$\in [-\pi, \pi]\f$.
+
+	///@name Friction
+	float						mMaxFrictionTorque = 0.0f;									///< Maximum amount of torque (N m) to apply as friction when the constraint is not powered by a motor
+
+	///@name In case the constraint is powered, this determines the motor settings around the swing and twist axis
+	MotorSettings				mSwingMotorSettings;
+	MotorSettings				mTwistMotorSettings;
+
+protected:
+	// See: ConstraintSettings::RestoreBinaryState
+	virtual void				RestoreBinaryState(StreamIn &inStream) override;
+};
+
+/// A swing twist constraint is a specialized constraint for humanoid ragdolls that allows limited rotation only
+///
+/// @see SwingTwistConstraintSettings for a description of the limits
+class JPH_EXPORT SwingTwistConstraint final : public TwoBodyConstraint
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Construct swing twist constraint
+								SwingTwistConstraint(Body &inBody1, Body &inBody2, const SwingTwistConstraintSettings &inSettings);
+
+	///@name Generic interface of a constraint
+	virtual EConstraintSubType	GetSubType() const override									{ return EConstraintSubType::SwingTwist; }
+	virtual void				NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override;
+	virtual void				SetupVelocityConstraint(float inDeltaTime) override;
+	virtual void				ResetWarmStart() override;
+	virtual void				WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override;
+	virtual bool				SolveVelocityConstraint(float inDeltaTime) override;
+	virtual bool				SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override;
+#ifdef JPH_DEBUG_RENDERER
+	virtual void				DrawConstraint(DebugRenderer *inRenderer) const override;
+	virtual void				DrawConstraintLimits(DebugRenderer *inRenderer) const override;
+#endif // JPH_DEBUG_RENDERER
+	virtual void				SaveState(StateRecorder &inStream) const override;
+	virtual void				RestoreState(StateRecorder &inStream) override;
+	virtual Ref<ConstraintSettings> GetConstraintSettings() const override;
+
+	// See: TwoBodyConstraint
+	virtual Mat44				GetConstraintToBody1Matrix() const override					{ return Mat44::sRotationTranslation(mConstraintToBody1, mLocalSpacePosition1); }
+	virtual Mat44				GetConstraintToBody2Matrix() const override					{ return Mat44::sRotationTranslation(mConstraintToBody2, mLocalSpacePosition2); }
+
+	///@name Constraint reference frame
+	inline Vec3					GetLocalSpacePosition1() const								{ return mLocalSpacePosition1; }
+	inline Vec3					GetLocalSpacePosition2() const								{ return mLocalSpacePosition2; }
+	inline Quat					GetConstraintToBody1() const								{ return mConstraintToBody1; }
+	inline Quat					GetConstraintToBody2() const								{ return mConstraintToBody2; }
+
+	///@name Constraint limits
+	inline float				GetNormalHalfConeAngle() const								{ return mNormalHalfConeAngle; }
+	inline void					SetNormalHalfConeAngle(float inAngle)						{ mNormalHalfConeAngle = inAngle; UpdateLimits(); }
+	inline float				GetPlaneHalfConeAngle() const								{ return mPlaneHalfConeAngle; }
+	inline void					SetPlaneHalfConeAngle(float inAngle)						{ mPlaneHalfConeAngle = inAngle; UpdateLimits(); }
+	inline float				GetTwistMinAngle() const									{ return mTwistMinAngle; }
+	inline void					SetTwistMinAngle(float inAngle)								{ mTwistMinAngle = inAngle; UpdateLimits(); }
+	inline float				GetTwistMaxAngle() const									{ return mTwistMaxAngle; }
+	inline void					SetTwistMaxAngle(float inAngle)								{ mTwistMaxAngle = inAngle; UpdateLimits(); }
+
+	///@name Motor settings
+	const MotorSettings &		GetSwingMotorSettings() const								{ return mSwingMotorSettings; }
+	MotorSettings &				GetSwingMotorSettings()										{ return mSwingMotorSettings; }
+	const MotorSettings &		GetTwistMotorSettings() const								{ return mTwistMotorSettings; }
+	MotorSettings &				GetTwistMotorSettings()										{ return mTwistMotorSettings; }
+
+	///@name Friction control
+	void						SetMaxFrictionTorque(float inFrictionTorque)				{ mMaxFrictionTorque = inFrictionTorque; }
+	float						GetMaxFrictionTorque() const								{ return mMaxFrictionTorque; }
+
+	///@name Motor controls
+
+	/// Controls if the motors are on or off
+	void						SetSwingMotorState(EMotorState inState);
+	EMotorState					GetSwingMotorState() const									{ return mSwingMotorState; }
+	void						SetTwistMotorState(EMotorState inState);
+	EMotorState					GetTwistMotorState() const									{ return mTwistMotorState; }
+
+	/// Set the target angular velocity of body 2 in constraint space of body 2
+	void						SetTargetAngularVelocityCS(Vec3Arg inAngularVelocity)		{ mTargetAngularVelocity = inAngularVelocity; }
+	Vec3						GetTargetAngularVelocityCS() const							{ return mTargetAngularVelocity; }
+
+	/// Set the target orientation in constraint space (drives constraint to: GetRotationInConstraintSpace() == inOrientation)
+	void						SetTargetOrientationCS(QuatArg inOrientation);
+	Quat						GetTargetOrientationCS() const								{ return mTargetOrientation; }
+
+	/// Set the target orientation in body space (R2 = R1 * inOrientation, where R1 and R2 are the world space rotations for body 1 and 2).
+	/// Solve: R2 * ConstraintToBody2 = R1 * ConstraintToBody1 * q (see SwingTwistConstraint::GetSwingTwist) and R2 = R1 * inOrientation for q.
+	void						SetTargetOrientationBS(QuatArg inOrientation)				{ SetTargetOrientationCS(mConstraintToBody1.Conjugated() * inOrientation * mConstraintToBody2); }
+
+	/// Get current rotation of constraint in constraint space.
+	/// Solve: R2 * ConstraintToBody2 = R1 * ConstraintToBody1 * q for q.
+	Quat						GetRotationInConstraintSpace() const;
+
+	///@name Get Lagrange multiplier from last physics update (the linear/angular impulse applied to satisfy the constraint)
+	inline Vec3					GetTotalLambdaPosition() const								{ return mPointConstraintPart.GetTotalLambda(); }
+	inline float				GetTotalLambdaTwist() const									{ return mSwingTwistConstraintPart.GetTotalTwistLambda(); }
+	inline float				GetTotalLambdaSwingY() const								{ return mSwingTwistConstraintPart.GetTotalSwingYLambda(); }
+	inline float				GetTotalLambdaSwingZ() const								{ return mSwingTwistConstraintPart.GetTotalSwingZLambda(); }
+	inline Vec3					GetTotalLambdaMotor() const									{ return Vec3(mMotorConstraintPart[0].GetTotalLambda(), mMotorConstraintPart[1].GetTotalLambda(), mMotorConstraintPart[2].GetTotalLambda()); }
+
+private:
+	// Update the limits in the swing twist constraint part
+	void						UpdateLimits();
+
+	// CONFIGURATION PROPERTIES FOLLOW
+
+	// Local space constraint positions
+	Vec3						mLocalSpacePosition1;
+	Vec3						mLocalSpacePosition2;
+
+	// Transforms from constraint space to body space
+	Quat						mConstraintToBody1;
+	Quat						mConstraintToBody2;
+
+	// Limits
+	float						mNormalHalfConeAngle;
+	float						mPlaneHalfConeAngle;
+	float						mTwistMinAngle;
+	float						mTwistMaxAngle;
+
+	// Friction
+	float						mMaxFrictionTorque;
+
+	// Motor controls
+	MotorSettings				mSwingMotorSettings;
+	MotorSettings				mTwistMotorSettings;
+	EMotorState					mSwingMotorState = EMotorState::Off;
+	EMotorState					mTwistMotorState = EMotorState::Off;
+	Vec3						mTargetAngularVelocity = Vec3::sZero();
+	Quat						mTargetOrientation = Quat::sIdentity();
+
+	// RUN TIME PROPERTIES FOLLOW
+
+	// Rotation axis for motor constraint parts
+	Vec3						mWorldSpaceMotorAxis[3];
+
+	// The constraint parts
+	PointConstraintPart			mPointConstraintPart;
+	SwingTwistConstraintPart	mSwingTwistConstraintPart;
+	AngleConstraintPart			mMotorConstraintPart[3];
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Constraints/TwoBodyConstraint.cpp

@@ -0,0 +1,56 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Constraints/TwoBodyConstraint.h>
+#include <Jolt/Physics/IslandBuilder.h>
+#include <Jolt/Physics/LargeIslandSplitter.h>
+#include <Jolt/Physics/Body/BodyManager.h>
+
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT(TwoBodyConstraintSettings)
+{
+	JPH_ADD_BASE_CLASS(TwoBodyConstraintSettings, ConstraintSettings)
+}
+
+void TwoBodyConstraint::BuildIslands(uint32 inConstraintIndex, IslandBuilder &ioBuilder, BodyManager &inBodyManager)
+{
+	// Activate bodies
+	BodyID body_ids[2];
+	int num_bodies = 0;
+	if (mBody1->IsDynamic() && !mBody1->IsActive())
+		body_ids[num_bodies++] = mBody1->GetID();
+	if (mBody2->IsDynamic() && !mBody2->IsActive())
+		body_ids[num_bodies++] = mBody2->GetID();
+	if (num_bodies > 0)
+		inBodyManager.ActivateBodies(body_ids, num_bodies);
+
+	// Link the bodies into the same island
+	ioBuilder.LinkConstraint(inConstraintIndex, mBody1->GetIndexInActiveBodiesInternal(), mBody2->GetIndexInActiveBodiesInternal());
+}
+
+uint TwoBodyConstraint::BuildIslandSplits(LargeIslandSplitter &ioSplitter) const
+{
+	return ioSplitter.AssignSplit(mBody1, mBody2);
+}
+
+#ifdef JPH_DEBUG_RENDERER
+
+void TwoBodyConstraint::DrawConstraintReferenceFrame(DebugRenderer *inRenderer) const
+{
+	RMat44 transform1 = mBody1->GetCenterOfMassTransform() * GetConstraintToBody1Matrix();
+	RMat44 transform2 = mBody2->GetCenterOfMassTransform() * GetConstraintToBody2Matrix();
+	inRenderer->DrawCoordinateSystem(transform1, 1.1f * mDrawConstraintSize);
+	inRenderer->DrawCoordinateSystem(transform2, mDrawConstraintSize);
+}
+
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Physics/Constraints/TwoBodyConstraint.h

@@ -0,0 +1,65 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Constraints/Constraint.h>
+#include <Jolt/Physics/Body/Body.h>
+
+JPH_NAMESPACE_BEGIN
+
+class TwoBodyConstraint;
+
+/// Base class for settings for all constraints that involve 2 bodies
+class JPH_EXPORT TwoBodyConstraintSettings : public ConstraintSettings
+{
+	JPH_DECLARE_SERIALIZABLE_ABSTRACT(JPH_EXPORT, TwoBodyConstraintSettings)
+
+public:
+	/// Create an instance of this constraint
+	/// You can use Body::sFixedToWorld for inBody1 if you want to attach inBody2 to the world
+	virtual TwoBodyConstraint *	Create(Body &inBody1, Body &inBody2) const = 0;
+};
+
+/// Base class for all constraints that involve 2 bodies. Body1 is usually considered the parent, Body2 the child.
+class JPH_EXPORT TwoBodyConstraint : public Constraint
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+								TwoBodyConstraint(Body &inBody1, Body &inBody2, const TwoBodyConstraintSettings &inSettings) : Constraint(inSettings), mBody1(&inBody1), mBody2(&inBody2) { }
+
+	/// Get the type of a constraint
+	virtual EConstraintType		GetType() const override				{ return EConstraintType::TwoBodyConstraint; }
+
+	/// Solver interface
+	virtual bool				IsActive() const override				{ return Constraint::IsActive() && (mBody1->IsActive() || mBody2->IsActive()) && (mBody2->IsDynamic() || mBody1->IsDynamic()); }
+#ifdef JPH_DEBUG_RENDERER
+	virtual void				DrawConstraintReferenceFrame(DebugRenderer *inRenderer) const override;
+#endif // JPH_DEBUG_RENDERER
+
+	/// Access to the connected bodies
+	Body *						GetBody1() const						{ return mBody1; }
+	Body *						GetBody2() const						{ return mBody2; }
+
+	/// Calculates the transform that transforms from constraint space to body 1 space. The first column of the matrix is the primary constraint axis (e.g. the hinge axis / slider direction), second column the secondary etc.
+	virtual Mat44				GetConstraintToBody1Matrix() const = 0;
+
+	/// Calculates the transform that transforms from constraint space to body 2 space. The first column of the matrix is the primary constraint axis (e.g. the hinge axis / slider direction), second column the secondary etc.
+	virtual Mat44				GetConstraintToBody2Matrix() const = 0;
+
+	/// Link bodies that are connected by this constraint in the island builder
+	virtual void				BuildIslands(uint32 inConstraintIndex, IslandBuilder &ioBuilder, BodyManager &inBodyManager) override;
+
+	/// Link bodies that are connected by this constraint in the same split. Returns the split index.
+	virtual uint				BuildIslandSplits(LargeIslandSplitter &ioSplitter) const override;
+
+protected:
+	/// The two bodies involved
+	Body *						mBody1;
+	Body *						mBody2;
+};
+
+JPH_NAMESPACE_END