Implemented SolvePositionConstraint for RackAndPinionConstraint and GearConstraint (#182)

Jolt/Physics/Constraints/GearConstraint.cpp

@@ -4,6 +4,7 @@
 #include <Jolt/Jolt.h>
 
 #include <Jolt/Physics/Constraints/GearConstraint.h>
+#include <Jolt/Physics/Constraints/HingeConstraint.h>
 #include <Jolt/Physics/Body/Body.h>
 #include <Jolt/ObjectStream/TypeDeclarations.h>
 #include <Jolt/Core/StreamIn.h>
@@ -93,8 +94,39 @@ bool GearConstraint::SolveVelocityConstraint(float inDeltaTime)
 
 bool GearConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte)
 {
-	// To be implemented
-	return false;
+	if (mGear1Constraint == nullptr || mGear2Constraint == nullptr)
+		return false;
+
+	float gear1rot;
+	if (mGear1Constraint->GetSubType() == EConstraintSubType::Hinge)
+	{
+		gear1rot = static_cast<const HingeConstraint *>(mGear1Constraint.GetPtr())->GetCurrentAngle();
+	}
+	else
+	{
+		JPH_ASSERT(false, "Unsupported");
+		return false;
+	}
+
+	float gear2rot;
+	if (mGear2Constraint->GetSubType() == EConstraintSubType::Hinge)
+	{
+		gear2rot = static_cast<const HingeConstraint *>(mGear2Constraint.GetPtr())->GetCurrentAngle();
+	}
+	else
+	{		
+		JPH_ASSERT(false, "Unsupported");
+		return false;
+	}
+
+	float error = CenterAngleAroundZero(fmod(gear1rot + mRatio * gear2rot, 2.0f * JPH_PI));
+	if (error == 0.0f)
+		return false;
+
+	Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation());
+	Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation());
+	CalculateConstraintProperties(rotation1, rotation2);
+	return mGearConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, error, inBaumgarte);
 }
 
 #ifdef JPH_DEBUG_RENDERER

Jolt/Physics/Constraints/GearConstraint.h

@@ -21,7 +21,7 @@ public:
 	virtual TwoBodyConstraint *	Create(Body &inBody1, Body &inBody2) const override;
 
 	/// Defines the ratio between the rotation of both gears
-	/// The ratio is defined as: Gear1Rotation(t) = ratio * Gear2Rotation(t)
+	/// The ratio is defined as: Gear1Rotation(t) = -ratio * Gear2Rotation(t)
 	/// @param inNumTeethGear1 Number of teeth that body 1 has
 	/// @param inNumTeethGear2 Number of teeth that body 2 has
 	void						SetRatio(int inNumTeethGear1, int inNumTeethGear2)
@@ -71,6 +71,9 @@ public:
 	virtual Mat44				GetConstraintToBody1Matrix() const override;
 	virtual Mat44				GetConstraintToBody2Matrix() const override;
 
+	/// The constraints that constrain both gears (2 hinges), optional and used to calculate the rotation error and fix numerical drift.
+	void						SetConstraints(const Constraint *inGear1, const Constraint *inGear2)	{ mGear1Constraint = inGear1; mGear2Constraint = inGear2; }
+
 	///@name Get Lagrange multiplier from last physics update (relates to how much force/torque was applied to satisfy the constraint)
 	inline float				GetTotalLambda() const									{ return mGearConstraintPart.GetTotalLambda(); }
 
@@ -89,6 +92,10 @@ private:
 	// Ratio between gear 1 and 2
 	float						mRatio;
 
+	// The constraints that constrain both gears (2 hinges), optional and used to calculate the rotation error and fix numerical drift.
+	RefConst<Constraint>		mGear1Constraint;
+	RefConst<Constraint>		mGear2Constraint;
+
 	// RUN TIME PROPERTIES FOLLOW
 
 	// World space hinge axis for body 1

Jolt/Physics/Constraints/HingeConstraint.cpp

@@ -107,6 +107,14 @@ HingeConstraint::HingeConstraint(Body &inBody1, Body &inBody2, const HingeConstr
 	}
 }
 
+float HingeConstraint::GetCurrentAngle() const
+{
+	// See: CalculateA1AndTheta
+	Quat rotation1 = mBody1->GetRotation();
+	Quat diff = mBody2->GetRotation() * mInvInitialOrientation * rotation1.Conjugated();
+	return diff.GetRotationAngle(rotation1 * mLocalSpaceHingeAxis1);
+}
+
 void HingeConstraint::SetLimits(float inLimitsMin, float inLimitsMax)
 {
 	JPH_ASSERT(inLimitsMin <= 0.0f && inLimitsMin >= -JPH_PI);

Jolt/Physics/Constraints/HingeConstraint.h

@@ -78,6 +78,9 @@ public:
 	virtual Mat44				GetConstraintToBody1Matrix() const override;
 	virtual Mat44				GetConstraintToBody2Matrix() const override;
 
+	/// Get the current rotation angle from the rest position
+	float						GetCurrentAngle() const;
+
 	// Friction control
 	void						SetMaxFrictionTorque(float inFrictionTorque)			{ mMaxFrictionTorque = inFrictionTorque; }
 	float						GetMaxFrictionTorque() const							{ return mMaxFrictionTorque; }

Jolt/Physics/Constraints/RackAndPinionConstraint.cpp

@@ -4,6 +4,8 @@
 #include <Jolt/Jolt.h>
 
 #include <Jolt/Physics/Constraints/RackAndPinionConstraint.h>
+#include <Jolt/Physics/Constraints/HingeConstraint.h>
+#include <Jolt/Physics/Constraints/SliderConstraint.h>
 #include <Jolt/Physics/Body/Body.h>
 #include <Jolt/ObjectStream/TypeDeclarations.h>
 #include <Jolt/Core/StreamIn.h>
@@ -93,8 +95,39 @@ bool RackAndPinionConstraint::SolveVelocityConstraint(float inDeltaTime)
 
 bool RackAndPinionConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte)
 {
-	// To be implemented
-	return false;
+	if (mRackConstraint == nullptr || mPinionConstraint == nullptr)
+		return false;
+
+	float rotation;
+	if (mPinionConstraint->GetSubType() == EConstraintSubType::Hinge)
+	{
+		rotation = static_cast<const HingeConstraint *>(mPinionConstraint.GetPtr())->GetCurrentAngle();
+	}
+	else
+	{
+		JPH_ASSERT(false, "Unsupported");
+		return false;
+	}
+
+	float translation;
+	if (mRackConstraint->GetSubType() == EConstraintSubType::Slider)
+	{
+		translation = static_cast<const SliderConstraint *>(mRackConstraint.GetPtr())->GetCurrentPosition();
+	}
+	else
+	{		
+		JPH_ASSERT(false, "Unsupported");
+		return false;
+	}
+
+	float error = CenterAngleAroundZero(fmod(rotation - mRatio * translation, 2.0f * JPH_PI));
+	if (error == 0.0f)
+		return false;
+
+	Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation());
+	Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation());
+	CalculateConstraintProperties(rotation1, rotation2);
+	return mRackAndPinionConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, error, inBaumgarte);
 }
 
 #ifdef JPH_DEBUG_RENDERER

Jolt/Physics/Constraints/RackAndPinionConstraint.h

@@ -57,7 +57,7 @@ public:
 								RackAndPinionConstraint(Body &inBody1, Body &inBody2, const RackAndPinionConstraintSettings &inSettings);
 
 	// Generic interface of a constraint
-	virtual EConstraintSubType	GetSubType() const override								{ return EConstraintSubType::RackAndPinion; }
+	virtual EConstraintSubType	GetSubType() const override												{ return EConstraintSubType::RackAndPinion; }
 	virtual void				SetupVelocityConstraint(float inDeltaTime) override;
 	virtual void				WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override;
 	virtual bool				SolveVelocityConstraint(float inDeltaTime) override;
@@ -73,8 +73,11 @@ public:
 	virtual Mat44				GetConstraintToBody1Matrix() const override;
 	virtual Mat44				GetConstraintToBody2Matrix() const override;
 
+	/// The constraints that constrain the rack and pinion (a slider and a hinge), optional and used to calculate the position error and fix numerical drift.
+	void						SetConstraints(const Constraint *inPinion, const Constraint *inRack)	{ mPinionConstraint = inPinion; mRackConstraint = inRack; }
+
 	///@name Get Lagrange multiplier from last physics update (relates to how much force/torque was applied to satisfy the constraint)
-	inline float				GetTotalLambda() const									{ return mRackAndPinionConstraintPart.GetTotalLambda(); }
+	inline float				GetTotalLambda() const													{ return mRackAndPinionConstraintPart.GetTotalLambda(); }
 
 private:
 	// Internal helper function to calculate the values below
@@ -91,6 +94,10 @@ private:
 	// Ratio between rack and pinion
 	float						mRatio;
 
+	// The constraints that constrain the rack and pinion (a slider and a hinge), optional and used to calculate the position error and fix numerical drift.
+	RefConst<Constraint>		mPinionConstraint;
+	RefConst<Constraint>		mRackConstraint;
+
 	// RUN TIME PROPERTIES FOLLOW
 
 	// World space hinge axis

Jolt/Physics/Constraints/SliderConstraint.cpp

@@ -144,6 +144,15 @@ SliderConstraint::SliderConstraint(Body &inBody1, Body &inBody2, const SliderCon
 	SetDamping(inSettings.mDamping);
 }
 
+float SliderConstraint::GetCurrentPosition() const
+{
+	// See: CalculateR1R2U and CalculateSlidingAxisAndPosition
+	Vec3 r1 = mBody1->GetRotation() * mLocalSpacePosition1;
+	Vec3 r2 = mBody2->GetRotation() * mLocalSpacePosition2;
+	Vec3 u = mBody2->GetCenterOfMassPosition() + r2 - mBody1->GetCenterOfMassPosition() - r1;
+	return u.Dot(mBody1->GetRotation() * mLocalSpaceSliderAxis1);
+}
+
 void SliderConstraint::SetLimits(float inLimitsMin, float inLimitsMax)
 {
 	JPH_ASSERT(inLimitsMin <= 0.0f);

Jolt/Physics/Constraints/SliderConstraint.h

@@ -89,6 +89,9 @@ public:
 	virtual Mat44				GetConstraintToBody1Matrix() const override;
 	virtual Mat44				GetConstraintToBody2Matrix() const override;
 
+	/// Get the current distance from the rest position
+	float						GetCurrentPosition() const;
+
 	/// Friction control
 	void						SetMaxFrictionForce(float inFrictionForce)				{ mMaxFrictionForce = inFrictionForce; }
 	float						GetMaxFrictionForce() const								{ return mMaxFrictionForce; }

Samples/Tests/Constraints/GearConstraintTest.cpp

@@ -78,7 +78,7 @@ void GearConstraintTest::Initialize()
 	gear2_settings.AddShape(Vec3::sZero(), Quat::sIdentity(), &gear2_cylinder);
 	for (int i = 0; i < cGear2NumTeeth; ++i)
 	{
-		Quat rotation = Quat::sRotation(Vec3::sAxisY(), 2.0f * JPH_PI * i / cGear2NumTeeth);
+		Quat rotation = Quat::sRotation(Vec3::sAxisY(), 2.0f * JPH_PI * (i + 0.5f) / cGear2NumTeeth);
 		gear2_settings.AddShape(rotation * Vec3(cGear2Radius, 0, 0), rotation, &tooth_settings);
 	}
 
@@ -92,14 +92,16 @@ void GearConstraintTest::Initialize()
 	hinge1.mPoint1 = hinge1.mPoint2 = gear1_initial_p;
 	hinge1.mHingeAxis1 = hinge1.mHingeAxis2 = Vec3::sAxisZ();
 	hinge1.mNormalAxis1 = hinge1.mNormalAxis2 = Vec3::sAxisX();
-	mPhysicsSystem->AddConstraint(hinge1.Create(*gear1, Body::sFixedToWorld));
+	Constraint *hinge1_constraint = hinge1.Create(Body::sFixedToWorld, *gear1);
+	mPhysicsSystem->AddConstraint(hinge1_constraint);
 
 	// Create a hinge for gear 1
 	HingeConstraintSettings hinge2;
 	hinge2.mPoint1 = hinge2.mPoint2 = gear2_initial_p;
 	hinge2.mHingeAxis1 = hinge2.mHingeAxis2 = Vec3::sAxisZ();
 	hinge2.mNormalAxis1 = hinge2.mNormalAxis2 = Vec3::sAxisX();
-	mPhysicsSystem->AddConstraint(hinge2.Create(*gear2, Body::sFixedToWorld));
+	Constraint *hinge2_constraint = hinge2.Create(Body::sFixedToWorld, *gear2);
+	mPhysicsSystem->AddConstraint(hinge2_constraint);
 
 	// Disable collision between gears
 	Ref<GroupFilterTable> group_filter = new GroupFilterTable(2);
@@ -112,7 +114,9 @@ void GearConstraintTest::Initialize()
 	gear.mHingeAxis1 = hinge1.mHingeAxis1;
 	gear.mHingeAxis2 = hinge2.mHingeAxis1;
 	gear.SetRatio(cGear1NumTeeth, cGear2NumTeeth);
-	mPhysicsSystem->AddConstraint(gear.Create(*gear1, *gear2));
+	GearConstraint *gear_constraint = static_cast<GearConstraint *>(gear.Create(*gear1, *gear2));
+	gear_constraint->SetConstraints(hinge1_constraint, hinge2_constraint);
+	mPhysicsSystem->AddConstraint(gear_constraint);
 
 	// Give the gear a spin
 	gear2->SetAngularVelocity(Vec3(0, 0, 3.0f));

Samples/Tests/Constraints/RackAndPinionConstraintTest.cpp

@@ -29,7 +29,7 @@ void RackAndPinionConstraintTest::Initialize()
 	constexpr float cGearHalfWidth = 0.05f;
 	constexpr int cGearNumTeeth = 100;
 
-	constexpr float cRackLength = 5.0f;
+	constexpr float cRackLength = 10.0f;
 	constexpr float cRackHalfHeight = 0.1f;
 	constexpr float cRackHalfWidth = 0.05f;
 	constexpr int cRackNumTeeth = int(cRackLength * cGearNumTeeth / (2.0f * JPH_PI * cGearRadius));
@@ -92,7 +92,8 @@ void RackAndPinionConstraintTest::Initialize()
 	hinge.mPoint1 = hinge.mPoint2 = gear_initial_p;
 	hinge.mHingeAxis1 = hinge.mHingeAxis2 = Vec3::sAxisZ();
 	hinge.mNormalAxis1 = hinge.mNormalAxis2 = Vec3::sAxisX();
-	mPhysicsSystem->AddConstraint(hinge.Create(*gear, Body::sFixedToWorld));
+	Constraint *hinge_constraint = hinge.Create(Body::sFixedToWorld, *gear);
+	mPhysicsSystem->AddConstraint(hinge_constraint);
 
 	// Create a slider for the rack
 	SliderConstraintSettings slider;
@@ -101,7 +102,8 @@ void RackAndPinionConstraintTest::Initialize()
 	slider.mNormalAxis1 = slider.mNormalAxis2 = Vec3::sAxisZ();
 	slider.mLimitsMin = -0.5f * cRackLength;
 	slider.mLimitsMax = 0.5f * cRackLength;
-	mPhysicsSystem->AddConstraint(slider.Create(*rack, Body::sFixedToWorld));
+	Constraint *slider_constraint = slider.Create(Body::sFixedToWorld, *rack);
+	mPhysicsSystem->AddConstraint(slider_constraint);
 
 	// Disable collision between rack and gear (we want the rack and pinion constraint to take care of the relative movement)
 	Ref<GroupFilterTable> group_filter = new GroupFilterTable(2);
@@ -114,7 +116,9 @@ void RackAndPinionConstraintTest::Initialize()
 	randp.mHingeAxis = hinge.mHingeAxis1;
 	randp.mSliderAxis = slider.mSliderAxis2;
 	randp.SetRatio(cRackNumTeeth, cRackLength, cGearNumTeeth);
-	mPhysicsSystem->AddConstraint(randp.Create(*gear, *rack));
+	RackAndPinionConstraint *randp_constraint = static_cast<RackAndPinionConstraint *>(randp.Create(*gear, *rack));
+	randp_constraint->SetConstraints(hinge_constraint, slider_constraint);
+	mPhysicsSystem->AddConstraint(randp_constraint);
 
 	// Give the gear a spin
 	gear->SetAngularVelocity(Vec3(0, 0, 6.0f));