Motor frequency/stiffness of 0 should turn the motor off (#724)

Before this change, the motor would become infinitely stiff instead and drive the constraint to the target position when frequency was 0

Jolt/Physics/Constraints/HingeConstraint.cpp

@@ -199,7 +199,10 @@ void HingeConstraint::CalculateMotorConstraintProperties(float inDeltaTime)
 		break;
 
 	case EMotorState::Position:
-		mMotorConstraintPart.CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, *mBody2, mA1, 0.0f, CenterAngleAroundZero(mTheta - mTargetAngle), mMotorSettings.mSpringSettings);
+		if (mMotorSettings.mSpringSettings.HasStiffness())
+			mMotorConstraintPart.CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, *mBody2, mA1, 0.0f, CenterAngleAroundZero(mTheta - mTargetAngle), mMotorSettings.mSpringSettings);
+		else
+			mMotorConstraintPart.Deactivate();
 		break;
 	}
 }

Jolt/Physics/Constraints/PathConstraint.cpp

@@ -196,6 +196,7 @@ void PathConstraint::CalculateConstraintProperties(float inDeltaTime)
 		break;
 
 	case EMotorState::Position:
+		if (mPositionMotorSettings.mSpringSettings.HasStiffness())
 		{
 			// Calculate constraint value to drive to
 			float c;
@@ -212,8 +213,10 @@ void PathConstraint::CalculateConstraintProperties(float inDeltaTime)
 			else
 				c = mPathFraction - mTargetPathFraction;
 			mPositionMotorConstraintPart.CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, mR1 + mU, *mBody2, mR2, mPathTangent, 0.0f, c, mPositionMotorSettings.mSpringSettings);
-			break;
 		}
+		else
+			mPositionMotorConstraintPart.Deactivate();
+		break;
 	}
 }
 

Jolt/Physics/Constraints/SixDOFConstraint.cpp

@@ -379,8 +379,14 @@ void SixDOFConstraint::SetupVelocityConstraint(float inDeltaTime)
 				break;
 
 			case EMotorState::Position:
-				mMotorTranslationConstraintPart[i].CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, r1_plus_u, *mBody2, r2, translation_axis, 0.0f, translation_axis.Dot(u) - mTargetPosition[i], mMotorSettings[i].mSpringSettings);
-				break;
+				{
+					const SpringSettings &spring_settings = mMotorSettings[i].mSpringSettings;
+					if (spring_settings.HasStiffness())
+						mMotorTranslationConstraintPart[i].CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, r1_plus_u, *mBody2, r2, translation_axis, 0.0f, translation_axis.Dot(u) - mTargetPosition[i], spring_settings);
+					else
+						mMotorTranslationConstraintPart[i].Deactivate();
+					break;
+				}
 			}
 		}
 	}
@@ -504,8 +510,14 @@ void SixDOFConstraint::SetupVelocityConstraint(float inDeltaTime)
 					break;
 
 				case EMotorState::Position:
-					mMotorRotationConstraintPart[i].CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, *mBody2, rotation_axis, 0.0f, rotation_error[i], mMotorSettings[axis].mSpringSettings);
-					break;
+					{
+						const SpringSettings &spring_settings = mMotorSettings[axis].mSpringSettings;
+						if (spring_settings.HasStiffness())
+							mMotorRotationConstraintPart[i].CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, *mBody2, rotation_axis, 0.0f, rotation_error[i], spring_settings);
+						else
+							mMotorRotationConstraintPart[i].Deactivate();
+						break;
+					}
 				}
 			}
 		}

Jolt/Physics/Constraints/SliderConstraint.cpp

@@ -246,7 +246,10 @@ void SliderConstraint::CalculateMotorConstraintProperties(float inDeltaTime)
 		break;
 
 	case EMotorState::Position:
-		mMotorConstraintPart.CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, mR1 + mU, *mBody2, mR2, mWorldSpaceSliderAxis, 0.0f, mD - mTargetPosition, mMotorSettings.mSpringSettings);
+		if (mMotorSettings.mSpringSettings.HasStiffness())
+			mMotorConstraintPart.CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, mR1 + mU, *mBody2, mR2, mWorldSpaceSliderAxis, 0.0f, mD - mTargetPosition, mMotorSettings.mSpringSettings);
+		else
+			mMotorConstraintPart.Deactivate();
 		break;
 	}
 }

Jolt/Physics/Constraints/SpringSettings.h

@@ -36,6 +36,9 @@ public:
 	/// Restores contents from the binary stream inStream.
 	void						RestoreBinaryState(StreamIn &inStream);
 
+	/// Check if the spring has a valid frequency / stiffness, if not the spring will be hard
+	inline bool					HasStiffness() const							{ return mFrequency > 0.0f; }
+
 	/// Selects the way in which the spring is defined
 	/// If the mode is StiffnessAndDamping then mFrequency becomes the stiffness (k) and mDamping becomes the damping ratio (c) in the spring equation F = -k * x - c * v. Otherwise the properties are as documented.
 	ESpringMode 				mMode = ESpringMode::FrequencyAndDamping;
@@ -44,12 +47,12 @@ public:
 	{
 		/// Valid when mSpringMode = ESpringMode::FrequencyAndDamping.
 		/// If mFrequency > 0 the constraint will be soft and mFrequency specifies the oscillation frequency in Hz.
-		/// If mFrequency <= 0, mDamping is ignored and the distance constraint will have hard limits (as hard as the time step / the number of velocity / position solver steps allows).
+		/// If mFrequency <= 0, mDamping is ignored and the constraint will have hard limits (as hard as the time step / the number of velocity / position solver steps allows).
 		float					mFrequency = 0.0f;
 
 		/// Valid when mSpringMode = ESpringMode::StiffnessAndDamping.
 		/// If mStiffness > 0 the constraint will be soft and mStiffness specifies the stiffness (k) in the spring equation F = -k * x - c * v for a linear or T = -k * theta - c * w for an angular spring.
-		/// If mStiffness <= 0, mDamping is ignored and the distance constraint will have hard limits (as hard as the time step / the number of velocity / position solver steps allows).
+		/// If mStiffness <= 0, mDamping is ignored and the constraint will have hard limits (as hard as the time step / the number of velocity / position solver steps allows).
 		float					mStiffness;
 	};
 

Jolt/Physics/Constraints/SwingTwistConstraint.cpp

@@ -272,8 +272,16 @@ void SwingTwistConstraint::SetupVelocityConstraint(float inDeltaTime)
 
 		case EMotorState::Position:
 			// Use motor to drive rotation error to zero
-			for (int i = 1; i < 3; ++i)
-				mMotorConstraintPart[i].CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, *mBody2, mWorldSpaceMotorAxis[i], 0.0f, rotation_error[i], mSwingMotorSettings.mSpringSettings);
+			if (mSwingMotorSettings.mSpringSettings.HasStiffness())
+			{
+				for (int i = 1; i < 3; ++i)
+					mMotorConstraintPart[i].CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, *mBody2, mWorldSpaceMotorAxis[i], 0.0f, rotation_error[i], mSwingMotorSettings.mSpringSettings);
+			}
+			else
+			{
+				for (int i = 1; i < 3; ++i)
+					mMotorConstraintPart[i].Deactivate();
+			}
 			break;
 		}
 
@@ -300,7 +308,10 @@ void SwingTwistConstraint::SetupVelocityConstraint(float inDeltaTime)
 
 		case EMotorState::Position:
 			// Use motor to drive rotation error to zero
-			mMotorConstraintPart[0].CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, *mBody2, mWorldSpaceMotorAxis[0], 0.0f, rotation_error[0], mTwistMotorSettings.mSpringSettings);
+			if (mTwistMotorSettings.mSpringSettings.HasStiffness())
+				mMotorConstraintPart[0].CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, *mBody2, mWorldSpaceMotorAxis[0], 0.0f, rotation_error[0], mTwistMotorSettings.mSpringSettings);
+			else
+				mMotorConstraintPart[0].Deactivate();
 			break;
 		}
 	}

Samples/Tests/Constraints/SixDOFConstraintTest.cpp

@@ -121,6 +121,12 @@ void SixDOFConstraintTest::CreateSettingsMenu(DebugUI *inUI, UIElement *inSubMen
 		for (int i = 0; i < 6; ++i)
 			inUI->CreateSlider(configuration_settings, "Max Friction " + labels[i], sSettings->mMaxFriction[i], 0.0f, 500.0f, 1.0f, [=](float inValue) { sSettings->mMaxFriction[i] = inValue; });
 
+		for (int i = 0; i < 6; ++i)
+			inUI->CreateSlider(configuration_settings, "Motor Frequency " + labels[i] + " (Hz)", sSettings->mMotorSettings[i].mSpringSettings.mFrequency, 0.0f, 20.0f, 0.1f, [i](float inValue) { sSettings->mMotorSettings[i].mSpringSettings.mFrequency = inValue; });
+
+		for (int i = 0; i < 6; ++i)
+			inUI->CreateSlider(configuration_settings, "Motor Damping " + labels[i], sSettings->mMotorSettings[i].mSpringSettings.mDamping, 0.0f, 2.0f, 0.01f, [i](float inValue) { sSettings->mMotorSettings[i].mSpringSettings.mDamping = inValue; });
+
 		inUI->CreateTextButton(configuration_settings, "Accept Changes", [=]() { RestartTest(); });
 
 		inUI->ShowMenu(configuration_settings);