Added support for fixating an axis at non-zero rotation/translation (#812)

* Added support for changing which axis are fixed/limited/free on the fly
* A cone shaped swing limit no longer allows non-symmetrical limits if one of the axis is fixed anymore, switch to a pyramid swing limit for this

Jolt/Physics/Constraints/ConstraintPart/SwingTwistConstraintPart.h

@@ -13,8 +13,8 @@ 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
-	Pyramid,					///< Swing is limited by a pyramid shape, note that this pyramid starts to deform for larger swing angles
+	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
@@ -119,7 +119,7 @@ public:
 			mSinSwingYHalfMaxAngle = swing_s.GetY();
 			mCosSwingYHalfMinAngle = swing_c.GetX();
 			mCosSwingYHalfMaxAngle = swing_c.GetY();
-			JPH_ASSERT(mSinSwingYHalfMinAngle < mSinSwingYHalfMaxAngle);
+			JPH_ASSERT(mSinSwingYHalfMinAngle <= mSinSwingYHalfMaxAngle);
 		}
 
 		if (inSwingZMinAngle > -cLockedAngle && inSwingZMaxAngle < cLockedAngle)
@@ -144,7 +144,7 @@ public:
 			mSinSwingZHalfMaxAngle = swing_s.GetW();
 			mCosSwingZHalfMinAngle = swing_c.GetZ();
 			mCosSwingZHalfMaxAngle = swing_c.GetW();
-			JPH_ASSERT(mSinSwingZHalfMinAngle < mSinSwingZHalfMaxAngle);
+			JPH_ASSERT(mSinSwingZHalfMinAngle <= mSinSwingZHalfMaxAngle);
 		}
 	}
 
@@ -472,14 +472,14 @@ public:
 
 		// Solve swing constraint
 		if (mSwingLimitYConstraintPart.IsActive())
-			impulse |= mSwingLimitYConstraintPart.SolveVelocityConstraint(ioBody1, ioBody2, mWorldSpaceSwingLimitYRotationAxis, -FLT_MAX, (mRotationFlags & SwingYLocked)? FLT_MAX : 0.0f);
+			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, (mRotationFlags & SwingZLocked)? FLT_MAX : 0.0f);
+			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, (mRotationFlags & TwistXLocked)? FLT_MAX : 0.0f);
+			impulse |= mTwistLimitConstraintPart.SolveVelocityConstraint(ioBody1, ioBody2, mWorldSpaceTwistLimitRotationAxis, -FLT_MAX, mSinTwistHalfMinAngle == mSinTwistHalfMaxAngle? FLT_MAX : 0.0f);
 
 		return impulse;
 	}

Jolt/Physics/Constraints/SixDOFConstraint.cpp

@@ -82,22 +82,58 @@ TwoBodyConstraint *SixDOFConstraintSettings::Create(Body &inBody1, Body &inBody2
 	return new SixDOFConstraint(inBody1, inBody2, *this);
 }
 
+void SixDOFConstraint::UpdateTranslationLimits()
+{
+	// Set to zero if the limits are inversed
+	for (int i = EAxis::TranslationX; i <= EAxis::TranslationZ; ++i)
+		if (mLimitMin[i] > mLimitMax[i])
+			mLimitMin[i] = mLimitMax[i] = 0.0f;
+}
+
 void SixDOFConstraint::UpdateRotationLimits()
 {
-	// Make values sensible
-	for (int i = 3; i < 6; ++i)
-		if (IsFixedAxis((EAxis)i))
+	if (mSwingTwistConstraintPart.GetSwingType() == ESwingType::Cone)
+	{
+		// Cone swing upper limit needs to be positive
+		mLimitMax[EAxis::RotationY] = max(0.0f, mLimitMax[EAxis::RotationY]);
+		mLimitMax[EAxis::RotationZ] = max(0.0f, mLimitMax[EAxis::RotationZ]);
+
+		// Cone swing limits only support symmetric ranges
+		mLimitMin[EAxis::RotationY] = -mLimitMax[EAxis::RotationY];
+		mLimitMin[EAxis::RotationZ] = -mLimitMax[EAxis::RotationZ];
+	}
+
+	for (int i = EAxis::RotationX; i <= EAxis::RotationZ; ++i)
+	{
+		// Clamp to [-PI, PI] range
+		mLimitMin[i] = Clamp(mLimitMin[i], -JPH_PI, JPH_PI);
+		mLimitMax[i] = Clamp(mLimitMax[i], -JPH_PI, JPH_PI);
+
+		// Set to zero if the limits are inversed
+		if (mLimitMin[i] > mLimitMax[i])
 			mLimitMin[i] = mLimitMax[i] = 0.0f;
-		else
-		{
-			mLimitMin[i] = max(-JPH_PI, mLimitMin[i]);
-			mLimitMax[i] = min(JPH_PI, mLimitMax[i]);
-		}
+	}
 
 	// Pass limits on to constraint part
 	mSwingTwistConstraintPart.SetLimits(mLimitMin[EAxis::RotationX], mLimitMax[EAxis::RotationX], mLimitMin[EAxis::RotationY], mLimitMax[EAxis::RotationY], mLimitMin[EAxis::RotationZ], mLimitMax[EAxis::RotationZ]);
 }
 
+void SixDOFConstraint::UpdateFixedFreeAxis()
+{
+	// Cache which axis are fixed and which ones are free
+	mFreeAxis = 0;
+	mFixedAxis = 0;
+	for (int a = 0; a < EAxis::Num; ++a)
+	{
+		float limit = a >= EAxis::RotationX? JPH_PI : FLT_MAX;
+
+		if (mLimitMin[a] >= mLimitMax[a])
+			mFixedAxis |= 1 << a;
+		else if (mLimitMin[a] <= -limit && mLimitMax[a] >= limit)
+			mFreeAxis |= 1 << a;
+	}
+}
+
 SixDOFConstraint::SixDOFConstraint(Body &inBody1, Body &inBody2, const SixDOFConstraintSettings &inSettings) :
 	TwoBodyConstraint(inBody1, inBody2, inSettings)
 {
@@ -129,23 +165,13 @@ SixDOFConstraint::SixDOFConstraint(Body &inBody1, Body &inBody2, const SixDOFCon
 		mLocalSpacePosition2 = Vec3(inSettings.mPosition2);
 	}
 
-	// Cache which axis are fixed and which ones are free
-	mFreeAxis = 0;
-	mFixedAxis = 0;
-	for (int a = 0; a < EAxis::Num; ++a)
-	{
-		if (inSettings.IsFixedAxis((EAxis)a))
-			mFixedAxis |= 1 << a;
-
-		if (inSettings.IsFreeAxis((EAxis)a))
-			mFreeAxis |= 1 << a;
-	}
-
 	// Copy translation and rotation limits
 	memcpy(mLimitMin, inSettings.mLimitMin, sizeof(mLimitMin));
 	memcpy(mLimitMax, inSettings.mLimitMax, sizeof(mLimitMax));
 	memcpy(mLimitsSpringSettings, inSettings.mLimitsSpringSettings, sizeof(mLimitsSpringSettings));
+	UpdateTranslationLimits();
 	UpdateRotationLimits();
+	UpdateFixedFreeAxis();
 	CacheHasSpringLimits();
 
 	// Store friction settings
@@ -176,6 +202,9 @@ void SixDOFConstraint::SetTranslationLimits(Vec3Arg inLimitMin, Vec3Arg inLimitM
 	mLimitMax[EAxis::TranslationX] = inLimitMax.GetX();
 	mLimitMax[EAxis::TranslationY] = inLimitMax.GetY();
 	mLimitMax[EAxis::TranslationZ] = inLimitMax.GetZ();
+
+	UpdateTranslationLimits();
+	UpdateFixedFreeAxis();
 }
 
 void SixDOFConstraint::SetRotationLimits(Vec3Arg inLimitMin, Vec3Arg inLimitMax)
@@ -188,6 +217,7 @@ void SixDOFConstraint::SetRotationLimits(Vec3Arg inLimitMin, Vec3Arg inLimitMax)
 	mLimitMax[EAxis::RotationZ] = inLimitMax.GetZ();
 
 	UpdateRotationLimits();
+	UpdateFixedFreeAxis();
 }
 
 void SixDOFConstraint::SetMaxFriction(EAxis inAxis, float inFriction)
@@ -343,7 +373,7 @@ void SixDOFConstraint::SetupVelocityConstraint(float inDeltaTime)
 			if (IsFixedAxis(axis))
 			{
 				// When constraint is fixed it is always active
-				constraint_value = d;
+				constraint_value = d - mLimitMin[i];
 				constraint_active = true;
 			}
 			else if (!IsFreeAxis(axis))
@@ -647,7 +677,8 @@ bool SixDOFConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumga
 		// Definition of initial orientation r0: q2 = q1 r0
 		// Initial rotation (see: GetRotationInConstraintSpace): q2 = q1 c1 c2^-1
 		// So: r0^-1 = (c1 c2^-1)^-1 = c2 * c1^-1
-		Quat inv_initial_orientation = mConstraintToBody2 * mConstraintToBody1.Conjugated();
+		Quat constraint_to_body1 = mConstraintToBody1 * Quat::sEulerAngles(GetRotationLimitsMin());
+		Quat inv_initial_orientation = mConstraintToBody2 * constraint_to_body1.Conjugated();
 
 		// Solve rotation violations
 		mRotationConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(mBody1->GetRotation()), *mBody2, Mat44::sRotation(mBody2->GetRotation()));
@@ -666,7 +697,8 @@ bool SixDOFConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumga
 	if (IsTranslationFullyConstrained())
 	{
 		// Translation locked: Solve point constraint
-		mPointConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(mBody1->GetRotation()), mLocalSpacePosition1, *mBody2, Mat44::sRotation(mBody2->GetRotation()), mLocalSpacePosition2);
+		Vec3 local_space_position1 = mLocalSpacePosition1 + mConstraintToBody1 * GetTranslationLimitsMin();
+		mPointConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(mBody1->GetRotation()), local_space_position1, *mBody2, Mat44::sRotation(mBody2->GetRotation()), mLocalSpacePosition2);
 		impulse |= mPointConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, inBaumgarte);
 	}
 	else if (IsTranslationConstrained())
@@ -695,7 +727,7 @@ bool SixDOFConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumga
 				float error = 0.0f;
 				EAxis axis(EAxis(EAxis::TranslationX + i));
 				if (IsFixedAxis(axis))
-					error = u.Dot(translation_axis);
+					error = u.Dot(translation_axis) - mLimitMin[axis];
 				else if (!IsFreeAxis(axis))
 				{
 					float displacement = u.Dot(translation_axis);
@@ -761,9 +793,7 @@ void SixDOFConstraint::DrawConstraintLimits(DebugRenderer *inRenderer) const
 	RMat44 constraint_body1_to_world = RMat44::sRotationTranslation(mBody1->GetRotation() * mConstraintToBody1, mBody1->GetCenterOfMassTransform() * mLocalSpacePosition1);
 
 	// Draw limits
-	if (mSwingTwistConstraintPart.GetSwingType() == ESwingType::Pyramid
-		|| mLimitMin[EAxis::RotationY] >= mLimitMax[EAxis::RotationY]
-		|| mLimitMin[EAxis::RotationZ] >= mLimitMax[EAxis::RotationZ])
+	if (mSwingTwistConstraintPart.GetSwingType() == ESwingType::Pyramid)
 		inRenderer->DrawSwingPyramidLimits(constraint_body1_to_world, mLimitMin[EAxis::RotationY], mLimitMax[EAxis::RotationY], mLimitMin[EAxis::RotationZ], mLimitMax[EAxis::RotationZ], mDrawConstraintSize, Color::sGreen, DebugRenderer::ECastShadow::Off);
 	else
 		inRenderer->DrawSwingConeLimits(constraint_body1_to_world, mLimitMax[EAxis::RotationY], mLimitMax[EAxis::RotationZ], mDrawConstraintSize, Color::sGreen, DebugRenderer::ECastShadow::Off);

Jolt/Physics/Constraints/SixDOFConstraint.h

@@ -27,9 +27,9 @@ public:
 		TranslationY,
 		TranslationZ,
 
-		RotationX,				///< When limited: Should be \f$\in [-\pi, \pi]\f$. Can by asymmetric.
-		RotationY,				///< Forms a pyramid or cone shaped limit with Z. For pyramid, should be \f$\in [-\pi, \pi]\f$, for cone \f$\in [0, \pi]\f$.
-		RotationZ,				///< Forms a pyramid or cone shaped limit with Y. For pyramid, should be \f$\in [-\pi, \pi]\f$, for cone \f$\in [0, \pi]\f$.
+		RotationX,
+		RotationY,
+		RotationZ,
 
 		Num,
 		NumTranslation = TranslationZ + 1,
@@ -69,6 +69,12 @@ public:
 	/// Remove degree of freedom by setting min = FLT_MAX and max = -FLT_MAX. The constraint will be driven to 0 for this axis.
 	///
 	/// Free movement over an axis is allowed when min = -FLT_MAX and max = FLT_MAX.
+	/// 
+	/// Rotation limit around X-Axis: When limited, should be \f$\in [-\pi, \pi]\f$. Can be asymmetric around zero.
+	/// 
+	/// Rotation limit around Y-Z Axis: Forms a pyramid or cone shaped limit:
+	/// * For pyramid, should be \f$\in [-\pi, \pi]\f$ and does not need to be symmetrical around zero.
+	/// * For cone should be \f$\in [0, \pi]\f$ and needs to be symmetrical around zero (min limit is assumed to be -max limit).
 	float						mLimitMin[EAxis::Num] = { -FLT_MAX, -FLT_MAX, -FLT_MAX, -FLT_MAX, -FLT_MAX, -FLT_MAX };
 	float						mLimitMax[EAxis::Num] = { FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX };
 
@@ -84,8 +90,8 @@ public:
 	void						MakeFixedAxis(EAxis inAxis)									{ mLimitMin[inAxis] = FLT_MAX; mLimitMax[inAxis] = -FLT_MAX; }
 	bool						IsFixedAxis(EAxis inAxis) const								{ return mLimitMin[inAxis] >= mLimitMax[inAxis]; }
 
-	/// Set a valid range for the constraint
-	void						SetLimitedAxis(EAxis inAxis, float inMin, float inMax)		{ JPH_ASSERT(inMin < inMax); mLimitMin[inAxis] = inMin; mLimitMax[inAxis] = inMax; }
+	/// Set a valid range for the constraint (if inMax < inMin, the axis will become fixed)
+	void						SetLimitedAxis(EAxis inAxis, float inMin, float inMax)		{ mLimitMin[inAxis] = inMin; mLimitMax[inAxis] = inMax; }
 
 	/// Motor settings for each axis
 	MotorSettings				mMotorSettings[EAxis::Num];
@@ -126,16 +132,21 @@ public:
 	virtual Mat44				GetConstraintToBody1Matrix() const override					{ return Mat44::sRotationTranslation(mConstraintToBody1, mLocalSpacePosition1); }
 	virtual Mat44				GetConstraintToBody2Matrix() const override					{ return Mat44::sRotationTranslation(mConstraintToBody2, mLocalSpacePosition2); }
 
-	/// Update the translation limits for this constraint, note that this won't change if axis are free or not.
+	/// Update the translation limits for this constraint
 	void						SetTranslationLimits(Vec3Arg inLimitMin, Vec3Arg inLimitMax);
 
-	/// Update the rotational limits for this constraint, note that this won't change if axis are free or not.
+	/// Update the rotational limits for this constraint
 	void						SetRotationLimits(Vec3Arg inLimitMin, Vec3Arg inLimitMax);
 
 	/// Get constraint Limits
 	float						GetLimitsMin(EAxis inAxis) const							{ return mLimitMin[inAxis]; }
 	float						GetLimitsMax(EAxis inAxis) const							{ return mLimitMax[inAxis]; }
+	Vec3						GetTranslationLimitsMin() const								{ return Vec3::sLoadFloat3Unsafe(*reinterpret_cast<const Float3 *>(&mLimitMin[EAxis::TranslationX])); }
+	Vec3						GetTranslationLimitsMax() const								{ return Vec3::sLoadFloat3Unsafe(*reinterpret_cast<const Float3 *>(&mLimitMax[EAxis::TranslationX])); }
+	Vec3						GetRotationLimitsMin() const								{ return Vec3::sLoadFloat3Unsafe(*reinterpret_cast<const Float3 *>(&mLimitMin[EAxis::RotationX])); }
+	Vec3						GetRotationLimitsMax() const								{ return Vec3::sLoadFloat3Unsafe(*reinterpret_cast<const Float3 *>(&mLimitMax[EAxis::RotationX])); }
 
+	/// Check which axis are fixed/free
 	inline bool					IsFixedAxis(EAxis inAxis) const								{ return (mFixedAxis & (1 << inAxis)) != 0; }
 	inline bool					IsFreeAxis(EAxis inAxis) const								{ return (mFreeAxis & (1 << inAxis)) != 0; }
 
@@ -190,9 +201,15 @@ private:
 	// Calculate properties needed for the position constraint
 	inline void					GetPositionConstraintProperties(Vec3 &outR1PlusU, Vec3 &outR2, Vec3 &outU) const;
 
+	// Sanitize the translation limits
+	inline void					UpdateTranslationLimits();
+
 	// Propagate the rotation limits to the constraint part
 	inline void					UpdateRotationLimits();
 
+	// Update the cached state of which axis are free and which ones are fixed
+	inline void					UpdateFixedFreeAxis();
+
 	// Cache the state of mTranslationMotorActive
 	void						CacheTranslationMotorActive();
 

Samples/Tests/Constraints/SixDOFConstraintTest.cpp

@@ -38,16 +38,9 @@ void SixDOFConstraintTest::Initialize()
 	// Convert limits to settings class
 	for (int i = 0; i < EAxis::Num; ++i)
 		if (sEnableLimits[i])
-		{
-			if (sLimitMax[i] - sLimitMin[i] < 1.0e-3f)
-				sSettings->MakeFixedAxis((EAxis)i);
-			else
-				sSettings->SetLimitedAxis((EAxis)i, sLimitMin[i], sLimitMax[i]);
-		}
+			sSettings->SetLimitedAxis((EAxis)i, sLimitMin[i], sLimitMax[i]);
 		else
-		{
 			sSettings->MakeFreeAxis((EAxis)i);
-		}
 
 	// Create group filter
 	Ref<GroupFilterTable> group_filter = new GroupFilterTable;
@@ -101,8 +94,8 @@ void SixDOFConstraintTest::CreateSettingsMenu(DebugUI *inUI, UIElement *inSubMen
 		for (int i = 0; i < 3; ++i)
 		{
 			inUI->CreateCheckBox(configuration_settings, "Enable Limits " + labels[i], sEnableLimits[i], [=](UICheckBox::EState inState) { sEnableLimits[i] = inState == UICheckBox::STATE_CHECKED; });
-			inUI->CreateSlider(configuration_settings, "Limit Min", sLimitMin[i], -10.0f, 0.0f, 0.1f, [=](float inValue) { sLimitMin[i] = inValue; });
-			inUI->CreateSlider(configuration_settings, "Limit Max", sLimitMax[i], 0.0f, 10.0f, 0.1f, [=](float inValue) { sLimitMax[i] = inValue; });
+			inUI->CreateSlider(configuration_settings, "Limit Min", sLimitMin[i], -5.0f, 5.0f, 0.1f, [=](float inValue) { sLimitMin[i] = inValue; });
+			inUI->CreateSlider(configuration_settings, "Limit Max", sLimitMax[i], -5.0f, 5.0f, 0.1f, [=](float inValue) { sLimitMax[i] = inValue; });
 			inUI->CreateSlider(configuration_settings, "Limit Frequency (Hz)", sSettings->mLimitsSpringSettings[i].mFrequency, 0.0f, 20.0f, 0.1f, [=](float inValue) { sSettings->mLimitsSpringSettings[i].mFrequency = inValue; });
 			inUI->CreateSlider(configuration_settings, "Limit Damping", sSettings->mLimitsSpringSettings[i].mDamping, 0.0f, 2.0f, 0.01f, [=](float inValue) { sSettings->mLimitsSpringSettings[i].mDamping = inValue; });
 		}