Added pulley constraint type (#229)

Jolt/Jolt.cmake

@@ -283,6 +283,7 @@ set(JOLT_PHYSICS_SRC_FILES
 	${JOLT_PHYSICS_ROOT}/Physics/Constraints/ConstraintPart/DualAxisConstraintPart.h
 	${JOLT_PHYSICS_ROOT}/Physics/Constraints/ConstraintPart/GearConstraintPart.h
 	${JOLT_PHYSICS_ROOT}/Physics/Constraints/ConstraintPart/HingeRotationConstraintPart.h
+	${JOLT_PHYSICS_ROOT}/Physics/Constraints/ConstraintPart/IndependentAxisConstraintPart.h
 	${JOLT_PHYSICS_ROOT}/Physics/Constraints/ConstraintPart/PointConstraintPart.h
 	${JOLT_PHYSICS_ROOT}/Physics/Constraints/ConstraintPart/RackAndPinionConstraintPart.h
 	${JOLT_PHYSICS_ROOT}/Physics/Constraints/ConstraintPart/RotationEulerConstraintPart.h
@@ -309,6 +310,8 @@ set(JOLT_PHYSICS_SRC_FILES
 	${JOLT_PHYSICS_ROOT}/Physics/Constraints/PathConstraintPathHermite.h
 	${JOLT_PHYSICS_ROOT}/Physics/Constraints/PointConstraint.cpp
 	${JOLT_PHYSICS_ROOT}/Physics/Constraints/PointConstraint.h
+	${JOLT_PHYSICS_ROOT}/Physics/Constraints/PulleyConstraint.cpp
+	${JOLT_PHYSICS_ROOT}/Physics/Constraints/PulleyConstraint.h
 	${JOLT_PHYSICS_ROOT}/Physics/Constraints/RackAndPinionConstraint.cpp
 	${JOLT_PHYSICS_ROOT}/Physics/Constraints/RackAndPinionConstraint.h
 	${JOLT_PHYSICS_ROOT}/Physics/Constraints/SixDOFConstraint.cpp

Jolt/Physics/Constraints/Constraint.h

@@ -41,6 +41,7 @@ enum class EConstraintSubType
 	Vehicle,
 	RackAndPinion,
 	Gear,
+	Pulley,
 
 	/// User defined constraint types start here
 	User1,

Jolt/Physics/Constraints/ConstraintPart/IndependentAxisConstraintPart.h

@@ -0,0 +1,242 @@
+// 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 & n_2 & 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 + m_2^{-1} + 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
+		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

Jolt/Physics/Constraints/PulleyConstraint.cpp

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

Jolt/Physics/Constraints/PulleyConstraint.h

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

Jolt/RegisterTypes.cpp

@@ -56,6 +56,7 @@ JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH, VehicleConstraintSettings)
 JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH, WheeledVehicleControllerSettings)
 JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH, RackAndPinionConstraintSettings)
 JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH, GearConstraintSettings)
+JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH, PulleyConstraintSettings)
 JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH, MotorSettings)
 JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH, PhysicsScene)
 JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH, PhysicsMaterial)
@@ -134,6 +135,7 @@ void RegisterTypes()
 		JPH_RTTI(PathConstraintPathHermite),
 		JPH_RTTI(RackAndPinionConstraintSettings),
 		JPH_RTTI(GearConstraintSettings),
+		JPH_RTTI(PulleyConstraintSettings),
 		JPH_RTTI(MotorSettings),
 		JPH_RTTI(PhysicsScene),
 		JPH_RTTI(PhysicsMaterial),

README.md

@@ -57,6 +57,7 @@ For more information see the [Architecture and API documentation](https://jrouwe
 	* Cone.
 	* Rack and Pinion.
 	* Gear.
+	* Pulley.
 	* Smooth spline paths.
 	* Swing-twist (for humanoid shoulders).
 	* 6 DOF.

Samples/Samples.cmake

@@ -41,6 +41,8 @@ set(SAMPLES_SRC_FILES
 	${SAMPLES_ROOT}/Tests/Constraints/PoweredSwingTwistConstraintTest.h
 	${SAMPLES_ROOT}/Tests/Constraints/PoweredSliderConstraintTest.cpp
 	${SAMPLES_ROOT}/Tests/Constraints/PoweredSliderConstraintTest.h
+	${SAMPLES_ROOT}/Tests/Constraints/PulleyConstraintTest.cpp
+	${SAMPLES_ROOT}/Tests/Constraints/PulleyConstraintTest.h
 	${SAMPLES_ROOT}/Tests/Constraints/RackAndPinionConstraintTest.cpp
 	${SAMPLES_ROOT}/Tests/Constraints/RackAndPinionConstraintTest.h
 	${SAMPLES_ROOT}/Tests/Constraints/SwingTwistConstraintFrictionTest.cpp

Samples/SamplesApp.cpp

@@ -33,6 +33,7 @@
 #include <Jolt/Physics/Collision/Shape/ScaledShape.h>
 #include <Jolt/Physics/Collision/NarrowPhaseStats.h>
 #include <Jolt/Physics/Constraints/DistanceConstraint.h>
+#include <Jolt/Physics/Constraints/PulleyConstraint.h>
 #include <Jolt/Physics/Character/CharacterVirtual.h>
 #include <Utils/Log.h>
 #include <Utils/ShapeCreator.h>
@@ -135,6 +136,7 @@ JPH_DECLARE_RTTI_FOR_FACTORY(SwingTwistConstraintFrictionTest)
 JPH_DECLARE_RTTI_FOR_FACTORY(PathConstraintTest)
 JPH_DECLARE_RTTI_FOR_FACTORY(RackAndPinionConstraintTest)
 JPH_DECLARE_RTTI_FOR_FACTORY(GearConstraintTest)
+JPH_DECLARE_RTTI_FOR_FACTORY(PulleyConstraintTest)
 
 static TestNameAndRTTI sConstraintTests[] =
 {
@@ -153,6 +155,7 @@ static TestNameAndRTTI sConstraintTests[] =
 	{ "Path Constraint",					JPH_RTTI(PathConstraintTest) },
 	{ "Rack And Pinion Constraint",			JPH_RTTI(RackAndPinionConstraintTest) },
 	{ "Gear Constraint",					JPH_RTTI(GearConstraintTest) },
+	{ "Pulley Constraint",					JPH_RTTI(PulleyConstraintTest) },
 	{ "Spring",								JPH_RTTI(SpringTest) },
 	{ "Constraint Singularity",				JPH_RTTI(ConstraintSingularityTest) },
 };

Samples/Tests/Constraints/PulleyConstraintTest.cpp

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

Samples/Tests/Constraints/PulleyConstraintTest.h

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