Twist limits no longer need to be centered around zero anymore for SixDOFConstraint and SwingTwistConstraints (#805)

Part of #779, see also #777

Docs/ReleaseNotes.md

@@ -5,6 +5,7 @@ For breaking API changes see [this document](https://github.com/jrouwe/JoltPhysi
 ## Unreleased changes
 
 ### New functionality
+* Twist limits no longer need to be centered around zero anymore for SixDOFConstraint and SwingTwistConstraints.
 * Changed the meaning of Constraint::mNumVelocity/PositionStepsOverride. Before the number of steps would be the maximum of all constraints and the default value, now an overridden value of 0 means that the constraint uses the default value, otherwise it will use the value as specified. This means that if all constraints in an island have a lower value than the default, we will now use the lower value instead of the default. This allows simulating an island at a lower precision than the default.
 * Bodies can now also override the default number of solver iterations. This value is used when the body collides with another body and a contact constraint is created (for constraints, the constraint override is always used).
 * Added BodyInterface::SetUseManifoldReduction which will clear the contact cache and ensure that you get consistent contact callbacks in case the body that you're changing already has contacts.

Jolt/Physics/Constraints/ConstraintPart/SwingTwistConstraintPart.h

@@ -32,8 +32,7 @@ public:
 		constexpr float cFreeAngle = DegreesToRadians(179.5f);
 
 		// Assume sane input
-		JPH_ASSERT(inTwistMinAngle <= 0.0f && inTwistMinAngle >= -JPH_PI);
-		JPH_ASSERT(inTwistMaxAngle >= 0.0f && inTwistMaxAngle <= JPH_PI);
+		JPH_ASSERT(inTwistMinAngle <= inTwistMinAngle);
 		JPH_ASSERT(inSwingYHalfAngle >= 0.0f && inSwingYHalfAngle <= JPH_PI);
 		JPH_ASSERT(inSwingZHalfAngle >= 0.0f && inSwingZHalfAngle <= JPH_PI);
 
@@ -99,10 +98,11 @@ public:
 	}
 
 	/// Clamp twist and swing against the constraint limits, returns which parts were clamped (everything assumed in constraint space)
-	inline void					ClampSwingTwist(Quat &ioSwing, bool &outSwingYClamped, bool &outSwingZClamped, Quat &ioTwist, bool &outTwistClamped) const
+	inline void					ClampSwingTwist(Quat &ioSwing, bool &outSwingYClamped, bool &outSwingZClamped, Quat &ioTwist, bool &outTwistClampedToMin, bool &outTwistClampedToMax) const
 	{
 		// Start with not clamped
-		outTwistClamped = false;
+		outTwistClampedToMin = false;
+		outTwistClampedToMax = false;
 		outSwingYClamped = false;
 		outSwingZClamped = false;
 
@@ -125,7 +125,7 @@ public:
 			if (ioTwist.GetX() != 0.0f)
 			{
 				ioTwist = Quat::sIdentity();
-				outTwistClamped = true;
+				outTwistClampedToMin = outTwistClampedToMax = true;
 			}
 		}
 		else if ((mRotationFlags & TwistXFree) == 0)
@@ -147,10 +147,15 @@ public:
 
 				// Pick the twist that corresponds to the smallest delta
 				if (delta_min < delta_max)
+				{
 					ioTwist = Quat(mSinTwistHalfMinAngle, 0, 0, mCosTwistHalfMinAngle);
+					outTwistClampedToMin = true;
+				}
 				else
+				{
 					ioTwist = Quat(mSinTwistHalfMaxAngle, 0, 0, mCosTwistHalfMaxAngle);
-				outTwistClamped = true;
+					outTwistClampedToMax = true;
+				}
 			}
 		}
 
@@ -221,8 +226,8 @@ public:
 
 		// Clamp against joint limits
 		Quat q_clamped_swing = q_swing, q_clamped_twist = q_twist;
-		bool swing_y_clamped, swing_z_clamped, twist_clamped;
-		ClampSwingTwist(q_clamped_swing, swing_y_clamped, swing_z_clamped, q_clamped_twist, twist_clamped);
+		bool swing_y_clamped, swing_z_clamped, twist_clamped_to_min, twist_clamped_to_max;
+		ClampSwingTwist(q_clamped_swing, swing_y_clamped, swing_z_clamped, q_clamped_twist, twist_clamped_to_min, twist_clamped_to_max);
 
 		if (mRotationFlags & SwingYLocked)
 		{
@@ -305,11 +310,11 @@ public:
 		else if ((mRotationFlags & TwistXFree) == 0)
 		{
 			// Twist has limits
-			if (twist_clamped)
+			if (twist_clamped_to_min || twist_clamped_to_max)
 			{
 				mWorldSpaceTwistLimitRotationAxis = (inConstraintToWorld * q_swing).RotateAxisX();
-				if (Sign(q_twist.GetW()) * q_twist.GetX() < 0.0f)
-					mWorldSpaceTwistLimitRotationAxis = -mWorldSpaceTwistLimitRotationAxis; // Flip axis if angle is negative because the impulse limit is going to be between [-FLT_MAX, 0]
+				if (twist_clamped_to_min)
+					mWorldSpaceTwistLimitRotationAxis = -mWorldSpaceTwistLimitRotationAxis; // Flip axis if hittin min limit because the impulse limit is going to be between [-FLT_MAX, 0]
 				mTwistLimitConstraintPart.CalculateConstraintProperties(inBody1, inBody2, mWorldSpaceTwistLimitRotationAxis);
 			}
 			else
@@ -374,11 +379,11 @@ public:
 		Quat q_swing, q_twist;
 		inConstraintRotation.GetSwingTwist(q_swing, q_twist);
 
-		bool swing_y_clamped, swing_z_clamped, twist_clamped;
-		ClampSwingTwist(q_swing, swing_y_clamped, swing_z_clamped, q_twist, twist_clamped);
+		bool swing_y_clamped, swing_z_clamped, twist_clamped_to_min, twist_clamped_to_max;
+		ClampSwingTwist(q_swing, swing_y_clamped, swing_z_clamped, q_twist, twist_clamped_to_min, twist_clamped_to_max);
 
 		// Solve rotation violations
-		if (swing_y_clamped || swing_z_clamped || twist_clamped)
+		if (swing_y_clamped || swing_z_clamped || twist_clamped_to_min || twist_clamped_to_max)
 		{
 			RotationEulerConstraintPart part;
 			Quat inv_initial_orientation = inConstraintToBody2 * (inConstraintToBody1 * q_swing * q_twist).Conjugated();

Jolt/Physics/Constraints/SixDOFConstraint.cpp

@@ -294,10 +294,10 @@ void SixDOFConstraint::SetTargetOrientationCS(QuatArg inOrientation)
 	Quat q_swing, q_twist;
 	inOrientation.GetSwingTwist(q_swing, q_twist);
 
-	bool twist_clamped, swing_y_clamped, swing_z_clamped;
-	mSwingTwistConstraintPart.ClampSwingTwist(q_swing, swing_y_clamped, swing_z_clamped, q_twist, twist_clamped);
+	bool swing_y_clamped, swing_z_clamped, twist_clamped_to_min, twist_clamped_to_max;
+	mSwingTwistConstraintPart.ClampSwingTwist(q_swing, swing_y_clamped, swing_z_clamped, q_twist, twist_clamped_to_min, twist_clamped_to_max);
 
-	if (twist_clamped || swing_y_clamped || swing_z_clamped)
+	if (swing_y_clamped || swing_z_clamped || twist_clamped_to_min || twist_clamped_to_max)
 		mTargetOrientation = q_swing * q_twist;
 	else
 		mTargetOrientation = inOrientation;

Jolt/Physics/Constraints/SixDOFConstraint.h

@@ -27,9 +27,9 @@ public:
 		TranslationY,
 		TranslationZ,
 
-		RotationX,				///< When limited: MinLimit needs to be [-PI, 0], MaxLimit needs to be [0, PI]
-		RotationY,				///< When limited: MaxLimit between [0, PI]. MinLimit = -MaxLimit. Forms a cone shaped limit with Z.
-		RotationZ,				///< When limited: MaxLimit between [0, PI]. MinLimit = -MaxLimit. Forms a cone shaped limit with Y.
+		RotationX,				///< When limited: Should be \f$\in [-\pi, \pi]\f$. Can by asymmetric.
+		RotationY,				///< When limited: MaxLimit \f$\in [0, \pi]\f$. MinLimit = -MaxLimit. Forms a cone shaped limit with Z.
+		RotationZ,				///< When limited: MaxLimit \f$\in [0, \pi]\f$. MinLimit = -MaxLimit. Forms a cone shaped limit with Y.
 
 		Num,
 		NumTranslation = TranslationZ + 1,
@@ -82,7 +82,7 @@ public:
 	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); JPH_ASSERT(inMin <= 0.0f); JPH_ASSERT(inMax >= 0.0f); mLimitMin[inAxis] = inMin; mLimitMax[inAxis] = inMax; }
+	void						SetLimitedAxis(EAxis inAxis, float inMin, float inMax)		{ JPH_ASSERT(inMin < inMax); mLimitMin[inAxis] = inMin; mLimitMax[inAxis] = inMax; }
 
 	/// Motor settings for each axis
 	MotorSettings				mMotorSettings[EAxis::Num];

Jolt/Physics/Constraints/SwingTwistConstraint.cpp

@@ -182,10 +182,10 @@ void SwingTwistConstraint::SetTargetOrientationCS(QuatArg inOrientation)
 	Quat q_swing, q_twist;
 	inOrientation.GetSwingTwist(q_swing, q_twist);
 
-	bool swing_y_clamped, swing_z_clamped, twist_clamped;
-	mSwingTwistConstraintPart.ClampSwingTwist(q_swing, swing_y_clamped, swing_z_clamped, q_twist, twist_clamped);
+	bool swing_y_clamped, swing_z_clamped, twist_clamped_to_min, twist_clamped_to_max;
+	mSwingTwistConstraintPart.ClampSwingTwist(q_swing, swing_y_clamped, swing_z_clamped, q_twist, twist_clamped_to_min, twist_clamped_to_max);
 
-	if (swing_y_clamped || swing_z_clamped || twist_clamped)
+	if (swing_y_clamped || swing_z_clamped || twist_clamped_to_min || twist_clamped_to_max)
 		mTargetOrientation = q_swing * q_twist;
 	else
 		mTargetOrientation = inOrientation;

Jolt/Physics/Constraints/SwingTwistConstraint.h

@@ -46,8 +46,8 @@ public:
 	float						mPlaneHalfConeAngle = 0.0f;									///< See image at Detailed Description. Angle in radians.
 
 	///@name Twist rotation limits
-	float						mTwistMinAngle = 0.0f;										///< See image at Detailed Description. Angle in radians. Rotation will be limited between [mLimitsMin, mLimitsMax] where mLimitsMin \f$\in [-\pi, 0]\f$ and mLimitsMax \f$\in [0, \pi]\f$
-	float						mTwistMaxAngle = 0.0f;										///< See image at Detailed Description. Angle in radians.
+	float						mTwistMinAngle = 0.0f;										///< See image at Detailed Description. Angle in radians. Should be \f$\in [-\pi, \pi]\f$.
+	float						mTwistMaxAngle = 0.0f;										///< See image at Detailed Description. Angle in radians. Should be \f$\in [-\pi, \pi]\f$.
 
 	///@name Friction
 	float						mMaxFrictionTorque = 0.0f;									///< Maximum amount of torque (N m) to apply as friction when the constraint is not powered by a motor

Samples/Tests/Constraints/PoweredSwingTwistConstraintTest.cpp

@@ -116,8 +116,8 @@ void PoweredSwingTwistConstraintTest::CreateSettingsMenu(DebugUI *inUI, UIElemen
 	inUI->CreateTextButton(inSubMenu, "Runtime Settings", [=]() {
 		UIElement *runtime_settings = inUI->CreateMenu();
 
-		inUI->CreateSlider(runtime_settings, "Min Twist Angle (deg)", RadiansToDegrees(sTwistMinAngle), -180.0f, 0.0f, 1.0f, [=](float inValue) { sTwistMinAngle = DegreesToRadians(inValue); });
-		inUI->CreateSlider(runtime_settings, "Max Twist Angle (deg)", RadiansToDegrees(sTwistMaxAngle), 0.0f, 180.0f, 1.0f, [=](float inValue) { sTwistMaxAngle = DegreesToRadians(inValue); });
+		inUI->CreateSlider(runtime_settings, "Min Twist Angle (deg)", RadiansToDegrees(sTwistMinAngle), -180.0f, 180.0f, 1.0f, [=](float inValue) { sTwistMinAngle = DegreesToRadians(inValue); });
+		inUI->CreateSlider(runtime_settings, "Max Twist Angle (deg)", RadiansToDegrees(sTwistMaxAngle), -180.0f, 180.0f, 1.0f, [=](float inValue) { sTwistMaxAngle = DegreesToRadians(inValue); });
 		inUI->CreateSlider(runtime_settings, "Normal Half Cone Angle (deg)", RadiansToDegrees(sNormalHalfConeAngle), 0.0f, 180.0f, 1.0f, [=](float inValue) { sNormalHalfConeAngle = DegreesToRadians(inValue); });
 		inUI->CreateSlider(runtime_settings, "Plane Half Cone Angle (deg)", RadiansToDegrees(sPlaneHalfConeAngle), 0.0f, 180.0f, 1.0f, [=](float inValue) { sPlaneHalfConeAngle = DegreesToRadians(inValue); });
 

Samples/Tests/Constraints/SixDOFConstraintTest.cpp

@@ -109,8 +109,8 @@ void SixDOFConstraintTest::CreateSettingsMenu(DebugUI *inUI, UIElement *inSubMen
 			inUI->CreateCheckBox(configuration_settings, "Enable Limits " + labels[i], sEnableLimits[i], [=](UICheckBox::EState inState) { sEnableLimits[i] = inState == UICheckBox::STATE_CHECKED; });
 			if (i == 3)
 			{
-				inUI->CreateSlider(configuration_settings, "Limit Min", RadiansToDegrees(sLimitMin[i]), -180.0f, 0.0f, 1.0f, [=](float inValue) { sLimitMin[i] = DegreesToRadians(inValue); });
-				inUI->CreateSlider(configuration_settings, "Limit Max", RadiansToDegrees(sLimitMax[i]), 0.0f, 180.0f, 1.0f, [=](float inValue) { sLimitMax[i] = DegreesToRadians(inValue); });
+				inUI->CreateSlider(configuration_settings, "Limit Min", RadiansToDegrees(sLimitMin[i]), -180.0f, 180.0f, 1.0f, [=](float inValue) { sLimitMin[i] = DegreesToRadians(inValue); });
+				inUI->CreateSlider(configuration_settings, "Limit Max", RadiansToDegrees(sLimitMax[i]), -180.0f, 180.0f, 1.0f, [=](float inValue) { sLimitMax[i] = DegreesToRadians(inValue); });
 			}
 			else
 			{

Samples/Tests/Constraints/SwingTwistConstraintTest.cpp

@@ -76,8 +76,8 @@ void SwingTwistConstraintTest::PrePhysicsUpdate(const PreUpdateParams &inParams)
 
 void SwingTwistConstraintTest::CreateSettingsMenu(DebugUI *inUI, UIElement *inSubMenu)
 {
-	inUI->CreateSlider(inSubMenu, "Min Twist Angle (deg)", RadiansToDegrees(sTwistMinAngle), -180.0f, 0.0f, 1.0f, [=](float inValue) { sTwistMinAngle = DegreesToRadians(inValue); });
-	inUI->CreateSlider(inSubMenu, "Max Twist Angle (deg)", RadiansToDegrees(sTwistMaxAngle), 0.0f, 180.0f, 1.0f, [=](float inValue) { sTwistMaxAngle = DegreesToRadians(inValue); });
+	inUI->CreateSlider(inSubMenu, "Min Twist Angle (deg)", RadiansToDegrees(sTwistMinAngle), -180.0f, 180.0f, 1.0f, [=](float inValue) { sTwistMinAngle = DegreesToRadians(inValue); });
+	inUI->CreateSlider(inSubMenu, "Max Twist Angle (deg)", RadiansToDegrees(sTwistMaxAngle), -180.0f, 180.0f, 1.0f, [=](float inValue) { sTwistMaxAngle = DegreesToRadians(inValue); });
 	inUI->CreateSlider(inSubMenu, "Normal Half Cone Angle (deg)", RadiansToDegrees(sNormalHalfConeAngle), 0.0f, 180.0f, 1.0f, [=](float inValue) { sNormalHalfConeAngle = DegreesToRadians(inValue); });
 	inUI->CreateSlider(inSubMenu, "Plane Half Cone Angle (deg)", RadiansToDegrees(sPlaneHalfConeAngle), 0.0f, 180.0f, 1.0f, [=](float inValue) { sPlaneHalfConeAngle = DegreesToRadians(inValue); });