initial commit, 4.5 stable

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