浏览代码

Ability to specify stiffness/damping for springs instead of frequency/damping (#561)

* Motors, DistanceConstraint, SliderConstraint, HingeConstraint and SixDOFConstraint can now switch between these 2 modes
* Created SpringSettings class to reduce code duplication
* Added soft limits from HingeConstraint and SixDOFConstraint
* Added unit tests for springs for DistanceConstraint, SliderConstraint, HingeConstraint and SixDOFConstraint
* Fixed bug in DistanceConstraint and HingeConstraint that caused the spring to behave incorrectly if limit min = limit max
Jorrit Rouwe 2 年之前
父节点
当前提交
3cabc057c1
共有 43 个文件被更改,包括 975 次插入311 次删除
  1. 2 0
      Jolt/Jolt.cmake
  2. 5 1
      Jolt/ObjectStream/SerializableAttributeEnum.h
  3. 5 1
      Jolt/ObjectStream/SerializableAttributeTyped.h
  4. 4 4
      Jolt/Physics/Constraints/ConeConstraint.cpp
  5. 1 1
      Jolt/Physics/Constraints/ConeConstraint.h
  6. 57 10
      Jolt/Physics/Constraints/ConstraintPart/AngleConstraintPart.h
  7. 87 31
      Jolt/Physics/Constraints/ConstraintPart/AxisConstraintPart.h
  8. 88 49
      Jolt/Physics/Constraints/ConstraintPart/SpringPart.h
  9. 10 11
      Jolt/Physics/Constraints/ConstraintPart/SwingTwistConstraintPart.h
  10. 6 8
      Jolt/Physics/Constraints/ContactConstraintManager.cpp
  11. 1 1
      Jolt/Physics/Constraints/ContactConstraintManager.h
  12. 29 29
      Jolt/Physics/Constraints/DistanceConstraint.cpp
  13. 8 16
      Jolt/Physics/Constraints/DistanceConstraint.h
  14. 35 12
      Jolt/Physics/Constraints/HingeConstraint.cpp
  15. 11 0
      Jolt/Physics/Constraints/HingeConstraint.h
  16. 5 6
      Jolt/Physics/Constraints/MotorSettings.cpp
  17. 6 6
      Jolt/Physics/Constraints/MotorSettings.h
  18. 4 4
      Jolt/Physics/Constraints/PathConstraint.cpp
  19. 74 46
      Jolt/Physics/Constraints/SixDOFConstraint.cpp
  20. 15 1
      Jolt/Physics/Constraints/SixDOFConstraint.h
  21. 28 21
      Jolt/Physics/Constraints/SliderConstraint.cpp
  22. 8 14
      Jolt/Physics/Constraints/SliderConstraint.h
  23. 35 0
      Jolt/Physics/Constraints/SpringSettings.cpp
  24. 62 0
      Jolt/Physics/Constraints/SpringSettings.h
  25. 7 7
      Jolt/Physics/Constraints/SwingTwistConstraint.cpp
  26. 3 3
      Jolt/Physics/PhysicsSystem.cpp
  27. 1 1
      Jolt/Physics/Vehicle/MotorcycleController.cpp
  28. 9 9
      Jolt/Physics/Vehicle/VehicleConstraint.cpp
  29. 1 1
      Jolt/Physics/Vehicle/VehicleConstraint.h
  30. 2 2
      Samples/SamplesApp.cpp
  31. 38 0
      Samples/Tests/Constraints/HingeConstraintTest.cpp
  32. 2 2
      Samples/Tests/Constraints/PathConstraintTest.cpp
  33. 2 2
      Samples/Tests/Constraints/PoweredHingeConstraintTest.cpp
  34. 2 2
      Samples/Tests/Constraints/PoweredSliderConstraintTest.cpp
  35. 4 4
      Samples/Tests/Constraints/PoweredSwingTwistConstraintTest.cpp
  36. 2 0
      Samples/Tests/Constraints/SixDOFConstraintTest.cpp
  37. 2 2
      Samples/Tests/Constraints/SliderConstraintTest.cpp
  38. 4 4
      Samples/Tests/Constraints/SpringTest.cpp
  39. 74 0
      UnitTests/Physics/DistanceConstraintTests.cpp
  40. 84 0
      UnitTests/Physics/HingeConstraintTests.cpp
  41. 87 0
      UnitTests/Physics/SixDOFConstraintTests.cpp
  42. 62 0
      UnitTests/Physics/SliderConstraintTests.cpp
  43. 3 0
      UnitTests/UnitTests.cmake

+ 2 - 0
Jolt/Jolt.cmake

@@ -334,6 +334,8 @@ set(JOLT_PHYSICS_SRC_FILES
 	${JOLT_PHYSICS_ROOT}/Physics/Constraints/SixDOFConstraint.h
 	${JOLT_PHYSICS_ROOT}/Physics/Constraints/SixDOFConstraint.h
 	${JOLT_PHYSICS_ROOT}/Physics/Constraints/SliderConstraint.cpp
 	${JOLT_PHYSICS_ROOT}/Physics/Constraints/SliderConstraint.cpp
 	${JOLT_PHYSICS_ROOT}/Physics/Constraints/SliderConstraint.h
 	${JOLT_PHYSICS_ROOT}/Physics/Constraints/SliderConstraint.h
+	${JOLT_PHYSICS_ROOT}/Physics/Constraints/SpringSettings.cpp
+	${JOLT_PHYSICS_ROOT}/Physics/Constraints/SpringSettings.h
 	${JOLT_PHYSICS_ROOT}/Physics/Constraints/SwingTwistConstraint.cpp
 	${JOLT_PHYSICS_ROOT}/Physics/Constraints/SwingTwistConstraint.cpp
 	${JOLT_PHYSICS_ROOT}/Physics/Constraints/SwingTwistConstraint.h
 	${JOLT_PHYSICS_ROOT}/Physics/Constraints/SwingTwistConstraint.h
 	${JOLT_PHYSICS_ROOT}/Physics/Constraints/TwoBodyConstraint.cpp
 	${JOLT_PHYSICS_ROOT}/Physics/Constraints/TwoBodyConstraint.cpp

+ 5 - 1
Jolt/ObjectStream/SerializableAttributeEnum.h

@@ -47,8 +47,12 @@ inline void AddSerializableAttributeEnum(RTTI &inRTTI, uint inOffset, const char
 		}));
 		}));
 }
 }
 
 
+// JPH_ADD_ENUM_ATTRIBUTE_WITH_ALIAS
+#define JPH_ADD_ENUM_ATTRIBUTE_WITH_ALIAS(class_name, member_name, alias_name) \
+	AddSerializableAttributeEnum<decltype(class_name::member_name)>(inRTTI, offsetof(class_name, member_name), alias_name);
+
 // JPH_ADD_ENUM_ATTRIBUTE
 // JPH_ADD_ENUM_ATTRIBUTE
 #define JPH_ADD_ENUM_ATTRIBUTE(class_name, member_name) \
 #define JPH_ADD_ENUM_ATTRIBUTE(class_name, member_name) \
-	AddSerializableAttributeEnum<decltype(class_name::member_name)>(inRTTI, offsetof(class_name, member_name), #member_name);
+	JPH_ADD_ENUM_ATTRIBUTE_WITH_ALIAS(class_name, member_name, #member_name);
 
 
 JPH_NAMESPACE_END
 JPH_NAMESPACE_END

+ 5 - 1
Jolt/ObjectStream/SerializableAttributeTyped.h

@@ -40,8 +40,12 @@ inline void AddSerializableAttributeTyped(RTTI &inRTTI, uint inOffset, const cha
 		}));
 		}));
 }
 }
 
 
+// JPH_ADD_ATTRIBUTE
+#define JPH_ADD_ATTRIBUTE_WITH_ALIAS(class_name, member_name, alias_name) \
+	AddSerializableAttributeTyped<decltype(class_name::member_name)>(inRTTI, offsetof(class_name, member_name), alias_name);
+
 // JPH_ADD_ATTRIBUTE
 // JPH_ADD_ATTRIBUTE
 #define JPH_ADD_ATTRIBUTE(class_name, member_name) \
 #define JPH_ADD_ATTRIBUTE(class_name, member_name) \
-	AddSerializableAttributeTyped<decltype(class_name::member_name)>(inRTTI, offsetof(class_name, member_name), #member_name);
+	JPH_ADD_ATTRIBUTE_WITH_ALIAS(class_name, member_name, #member_name)
 
 
 JPH_NAMESPACE_END
 JPH_NAMESPACE_END

+ 4 - 4
Jolt/Physics/Constraints/ConeConstraint.cpp

@@ -97,7 +97,7 @@ void ConeConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaC
 		mLocalSpacePosition2 -= inDeltaCOM;
 		mLocalSpacePosition2 -= inDeltaCOM;
 }
 }
 
 
-void ConeConstraint::CalculateRotationConstraintProperties(float inDeltaTime, Mat44Arg inRotation1, Mat44Arg inRotation2)
+void ConeConstraint::CalculateRotationConstraintProperties(Mat44Arg inRotation1, Mat44Arg inRotation2)
 {
 {
 	// Rotation is along the cross product of both twist axis
 	// Rotation is along the cross product of both twist axis
 	Vec3 twist1 = inRotation1.Multiply3x3(mLocalSpaceTwistAxis1);
 	Vec3 twist1 = inRotation1.Multiply3x3(mLocalSpaceTwistAxis1);
@@ -115,7 +115,7 @@ void ConeConstraint::CalculateRotationConstraintProperties(float inDeltaTime, Ma
 		if (len > 0.0f)
 		if (len > 0.0f)
 			mWorldSpaceRotationAxis = rot_axis / len;
 			mWorldSpaceRotationAxis = rot_axis / len;
 
 
-		mAngleConstraintPart.CalculateConstraintProperties(inDeltaTime, *mBody1, *mBody2, mWorldSpaceRotationAxis);		
+		mAngleConstraintPart.CalculateConstraintProperties(*mBody1, *mBody2, mWorldSpaceRotationAxis);		
 	}
 	}
 	else
 	else
 		mAngleConstraintPart.Deactivate();
 		mAngleConstraintPart.Deactivate();
@@ -126,7 +126,7 @@ void ConeConstraint::SetupVelocityConstraint(float inDeltaTime)
 	Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation());
 	Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation());
 	Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation());
 	Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation());
 	mPointConstraintPart.CalculateConstraintProperties(*mBody1, rotation1, mLocalSpacePosition1, *mBody2, rotation2, mLocalSpacePosition2);
 	mPointConstraintPart.CalculateConstraintProperties(*mBody1, rotation1, mLocalSpacePosition1, *mBody2, rotation2, mLocalSpacePosition2);
-	CalculateRotationConstraintProperties(inDeltaTime, rotation1, rotation2);
+	CalculateRotationConstraintProperties(rotation1, rotation2);
 }
 }
 
 
 void ConeConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio)
 void ConeConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio)
@@ -153,7 +153,7 @@ bool ConeConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgart
 	bool pos = mPointConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, inBaumgarte);
 	bool pos = mPointConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, inBaumgarte);
 
 
 	bool rot = false;
 	bool rot = false;
-	CalculateRotationConstraintProperties(inDeltaTime, Mat44::sRotation(mBody1->GetRotation()), Mat44::sRotation(mBody2->GetRotation()));
+	CalculateRotationConstraintProperties(Mat44::sRotation(mBody1->GetRotation()), Mat44::sRotation(mBody2->GetRotation()));
 	if (mAngleConstraintPart.IsActive())
 	if (mAngleConstraintPart.IsActive())
 		rot = mAngleConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mCosTheta - mCosHalfConeAngle, inBaumgarte);
 		rot = mAngleConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mCosTheta - mCosHalfConeAngle, inBaumgarte);
 
 

+ 1 - 1
Jolt/Physics/Constraints/ConeConstraint.h

@@ -103,7 +103,7 @@ public:
 
 
 private:
 private:
 	// Internal helper function to calculate the values below
 	// Internal helper function to calculate the values below
-	void						CalculateRotationConstraintProperties(float inDeltaTime, Mat44Arg inRotation1, Mat44Arg inRotation2);
+	void						CalculateRotationConstraintProperties(Mat44Arg inRotation1, Mat44Arg inRotation2);
 
 
 	// CONFIGURATION PROPERTIES FOLLOW
 	// CONFIGURATION PROPERTIES FOLLOW
 
 

+ 57 - 10
Jolt/Physics/Constraints/ConstraintPart/AngleConstraintPart.h

@@ -6,6 +6,7 @@
 
 
 #include <Jolt/Physics/Body/Body.h>
 #include <Jolt/Physics/Body/Body.h>
 #include <Jolt/Physics/Constraints/ConstraintPart/SpringPart.h>
 #include <Jolt/Physics/Constraints/ConstraintPart/SpringPart.h>
+#include <Jolt/Physics/Constraints/SpringSettings.h>
 #include <Jolt/Physics/StateRecorder.h>
 #include <Jolt/Physics/StateRecorder.h>
 
 
 JPH_NAMESPACE_BEGIN
 JPH_NAMESPACE_BEGIN
@@ -57,7 +58,33 @@ class AngleConstraintPart
 		return false;
 		return false;
 	}
 	}
 
 
+	/// Internal helper function to calculate the inverse effective mass
+	JPH_INLINE float				CalculateInverseEffectiveMass(const Body &inBody1, const Body &inBody2, Vec3Arg inWorldSpaceAxis)
+	{
+		JPH_ASSERT(inWorldSpaceAxis.IsNormalized(1.0e-4f));
+
+		// Calculate properties used below
+		mInvI1_Axis = inBody1.IsDynamic()? inBody1.GetMotionProperties()->MultiplyWorldSpaceInverseInertiaByVector(inBody1.GetRotation(), inWorldSpaceAxis) : Vec3::sZero();
+		mInvI2_Axis = inBody2.IsDynamic()? inBody2.GetMotionProperties()->MultiplyWorldSpaceInverseInertiaByVector(inBody2.GetRotation(), inWorldSpaceAxis) : Vec3::sZero();
+	
+		// Calculate inverse effective mass: K = J M^-1 J^T
+		return inWorldSpaceAxis.Dot(mInvI1_Axis + mInvI2_Axis);
+	}
+
 public:
 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 inWorldSpaceAxis The axis of rotation along which the constraint acts (normalized)
+	/// Set the following terms to zero if you don't want to drive the constraint to zero with a spring:
+	/// @param inBias Bias term (b) for the constraint impulse: lambda = J v + b
+	inline void					CalculateConstraintProperties(const Body &inBody1, const Body &inBody2, Vec3Arg inWorldSpaceAxis, float inBias = 0.0f)
+	{
+		mEffectiveMass = 1.0f / CalculateInverseEffectiveMass(inBody1, inBody2, inWorldSpaceAxis);
+
+		mSpringPart.CalculateSpringPropertiesWithBias(inBias);
+	}
+
 	/// Calculate properties used during the functions below
 	/// Calculate properties used during the functions below
 	/// @param inDeltaTime Time step
 	/// @param inDeltaTime Time step
 	/// @param inBody1 The first body that this constraint is attached to
 	/// @param inBody1 The first body that this constraint is attached to
@@ -68,19 +95,39 @@ public:
 	///	@param inC Value of the constraint equation (C)
 	///	@param inC Value of the constraint equation (C)
 	///	@param inFrequency Oscillation frequency (Hz)
 	///	@param inFrequency Oscillation frequency (Hz)
 	///	@param inDamping Damping factor (0 = no damping, 1 = critical damping)
 	///	@param inDamping Damping factor (0 = no damping, 1 = critical damping)
-	inline void					CalculateConstraintProperties(float inDeltaTime, const Body &inBody1, const Body &inBody2, Vec3Arg inWorldSpaceAxis, float inBias = 0.0f, float inC = 0.0f, float inFrequency = 0.0f, float inDamping = 0.0f)
+	inline void					CalculateConstraintPropertiesWithFrequencyAndDamping(float inDeltaTime, const Body &inBody1, const Body &inBody2, Vec3Arg inWorldSpaceAxis, float inBias, float inC, float inFrequency, float inDamping)
 	{
 	{
-		JPH_ASSERT(inWorldSpaceAxis.IsNormalized(1.0e-4f));
+		float inv_effective_mass = CalculateInverseEffectiveMass(inBody1, inBody2, inWorldSpaceAxis);
 
 
-		// Calculate properties used below
-		mInvI1_Axis = inBody1.IsDynamic()? inBody1.GetMotionProperties()->MultiplyWorldSpaceInverseInertiaByVector(inBody1.GetRotation(), inWorldSpaceAxis) : Vec3::sZero();
-		mInvI2_Axis = inBody2.IsDynamic()? inBody2.GetMotionProperties()->MultiplyWorldSpaceInverseInertiaByVector(inBody2.GetRotation(), inWorldSpaceAxis) : Vec3::sZero();
-	
-		// Calculate inverse effective mass: K = J M^-1 J^T
-		float inv_effective_mass = inWorldSpaceAxis.Dot(mInvI1_Axis + mInvI2_Axis);
+		mSpringPart.CalculateSpringPropertiesWithFrequencyAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inFrequency, inDamping, mEffectiveMass);
+	}
+
+	/// Calculate properties used during the functions below
+	/// @param inDeltaTime Time step
+	/// @param inBody1 The first body that this constraint is attached to
+	/// @param inBody2 The second body that this constraint is attached to
+	/// @param inWorldSpaceAxis The axis of rotation along which the constraint acts (normalized)
+	/// Set the following terms to zero if you don't want to drive the constraint to zero with a spring:
+	/// @param inBias Bias term (b) for the constraint impulse: lambda = J v + b
+	///	@param inC Value of the constraint equation (C)
+	///	@param inStiffness Spring stiffness k.
+	///	@param inDamping Spring damping coefficient c.
+	inline void					CalculateConstraintPropertiesWithStiffnessAndDamping(float inDeltaTime, const Body &inBody1, const Body &inBody2, Vec3Arg inWorldSpaceAxis, float inBias, float inC, float inStiffness, float inDamping)
+	{
+		float inv_effective_mass = CalculateInverseEffectiveMass(inBody1, inBody2, inWorldSpaceAxis);
+
+		mSpringPart.CalculateSpringPropertiesWithStiffnessAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inStiffness, inDamping, mEffectiveMass);
+	}
+
+	/// Selects one of the above functions based on the spring settings
+	inline void					CalculateConstraintPropertiesWithSettings(float inDeltaTime, const Body &inBody1, const Body &inBody2, Vec3Arg inWorldSpaceAxis, float inBias, float inC, const SpringSettings &inSpringSettings)
+	{
+		float inv_effective_mass = CalculateInverseEffectiveMass(inBody1, inBody2, inWorldSpaceAxis);
 
 
-		// Calculate effective mass and spring properties
-		mSpringPart.CalculateSpringProperties(inDeltaTime, inv_effective_mass, inBias, inC, inFrequency, inDamping, mEffectiveMass);
+		if (inSpringSettings.mMode == ESpringMode::FrequencyAndDamping)
+			mSpringPart.CalculateSpringPropertiesWithFrequencyAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inSpringSettings.mFrequency, inSpringSettings.mDamping, mEffectiveMass);
+		else
+			mSpringPart.CalculateSpringPropertiesWithStiffnessAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inSpringSettings.mStiffness, inSpringSettings.mDamping, mEffectiveMass);
 	}
 	}
 
 
 	/// Deactivate this constraint
 	/// Deactivate this constraint

+ 87 - 31
Jolt/Physics/Constraints/ConstraintPart/AxisConstraintPart.h

@@ -6,6 +6,7 @@
 
 
 #include <Jolt/Physics/Body/Body.h>
 #include <Jolt/Physics/Body/Body.h>
 #include <Jolt/Physics/Constraints/ConstraintPart/SpringPart.h>
 #include <Jolt/Physics/Constraints/ConstraintPart/SpringPart.h>
+#include <Jolt/Physics/Constraints/SpringSettings.h>
 #include <Jolt/Physics/StateRecorder.h>
 #include <Jolt/Physics/StateRecorder.h>
 #include <Jolt/Physics/DeterminismLog.h>
 #include <Jolt/Physics/DeterminismLog.h>
 
 
@@ -70,10 +71,9 @@ class AxisConstraintPart
 		return false;
 		return false;
 	}
 	}
 
 
-public:
-	/// Templated form of CalculateConstraintProperties with the motion types baked in
+	/// Internal helper function to calculate the inverse effective mass
 	template <EMotionType Type1, EMotionType Type2>
 	template <EMotionType Type1, EMotionType Type2>
-	JPH_INLINE void				TemplatedCalculateConstraintProperties(float inDeltaTime, const MotionProperties *inMotionProperties1, Mat44Arg inInvI1, Vec3Arg inR1PlusU, const MotionProperties *inMotionProperties2, Mat44Arg inInvI2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis, float inBias = 0.0f, float inC = 0.0f, float inFrequency = 0.0f, float inDamping = 0.0f)
+	JPH_INLINE float			TemplatedCalculateInverseEffectiveMass(const MotionProperties *inMotionProperties1, Mat44Arg inInvI1, Vec3Arg inR1PlusU, const MotionProperties *inMotionProperties2, Mat44Arg inInvI2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis)
 	{
 	{
 		JPH_ASSERT(inWorldSpaceAxis.IsNormalized(1.0e-5f));
 		JPH_ASSERT(inWorldSpaceAxis.IsNormalized(1.0e-5f));
 
 
@@ -122,24 +122,11 @@ public:
 			JPH_IF_DEBUG(Vec3::sNaN().StoreFloat3(&mInvI2_R2xAxis);)
 			JPH_IF_DEBUG(Vec3::sNaN().StoreFloat3(&mInvI2_R2xAxis);)
 		}
 		}
 
 
-		// Calculate effective mass and spring properties
-		mSpringPart.CalculateSpringProperties(inDeltaTime, inv_effective_mass, inBias, inC, inFrequency, inDamping, mEffectiveMass);
-
-		JPH_DET_LOG("TemplatedCalculateConstraintProperties: dt: " << inDeltaTime << " invI1: " << inInvI1 << " r1PlusU: " << inR1PlusU << " invI2: " << inInvI2 << " r2: " << inR2 << " bias: " << inBias << ", c: " << inC << ", frequency: " << inFrequency << ", damping: " << inDamping << " r1PlusUxAxis: " << mR1PlusUxAxis << " r2xAxis: " << mR2xAxis << " invI1_R1PlusUxAxis: " << mInvI1_R1PlusUxAxis << " invI2_R2xAxis: " << mInvI2_R2xAxis << " effectiveMass: " << mEffectiveMass << " totalLambda: " << mTotalLambda);
+		return inv_effective_mass;
 	}
 	}
 
 
-	/// Calculate properties used during the functions below
-	/// @param inDeltaTime Time step
-	/// @param inBody1 The first body that this constraint is attached to
-	/// @param inBody2 The second body that this constraint is attached to
-	/// @param inR1PlusU See equations above (r1 + u)
-	/// @param inR2 See equations above (r2)
-	/// @param inWorldSpaceAxis Axis along which the constraint acts (normalized, pointing from body 1 to 2)
-	/// @param inBias Bias term (b) for the constraint impulse: lambda = J v + b
-	///	@param inC Value of the constraint equation (C). Set to zero if you don't want to drive the constraint to zero with a spring.
-	///	@param inFrequency Oscillation frequency (Hz). Set to zero if you don't want to drive the constraint to zero with a spring.
-	///	@param inDamping Damping factor (0 = no damping, 1 = critical damping). Set to zero if you don't want to drive the constraint to zero with a spring.
-	inline void					CalculateConstraintProperties(float inDeltaTime, const Body &inBody1, Vec3Arg inR1PlusU, const Body &inBody2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis, float inBias = 0.0f, float inC = 0.0f, float inFrequency = 0.0f, float inDamping = 0.0f)
+	/// Internal helper function to calculate the inverse effective mass
+	JPH_INLINE float			CalculateInverseEffectiveMass(const Body &inBody1, Vec3Arg inR1PlusU, const Body &inBody2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis)
 	{
 	{
 		// Dispatch to the correct templated form
 		// Dispatch to the correct templated form
 		switch (inBody1.GetMotionType())
 		switch (inBody1.GetMotionType())
@@ -151,19 +138,15 @@ public:
 				switch (inBody2.GetMotionType())
 				switch (inBody2.GetMotionType())
 				{
 				{
 				case EMotionType::Dynamic:
 				case EMotionType::Dynamic:
-					TemplatedCalculateConstraintProperties<EMotionType::Dynamic, EMotionType::Dynamic>(inDeltaTime, mp1, invi1, inR1PlusU, inBody2.GetMotionPropertiesUnchecked(), inBody2.GetInverseInertia(), inR2, inWorldSpaceAxis, inBias, inC, inFrequency, inDamping);
-					break;
+					return TemplatedCalculateInverseEffectiveMass<EMotionType::Dynamic, EMotionType::Dynamic>(mp1, invi1, inR1PlusU, inBody2.GetMotionPropertiesUnchecked(), inBody2.GetInverseInertia(), inR2, inWorldSpaceAxis);
 
 
 				case EMotionType::Kinematic:
 				case EMotionType::Kinematic:
-					TemplatedCalculateConstraintProperties<EMotionType::Dynamic, EMotionType::Kinematic>(inDeltaTime, mp1, invi1, inR1PlusU, nullptr, Mat44() /* Will not be used */, inR2, inWorldSpaceAxis, inBias, inC, inFrequency, inDamping);
-					break;
+					return TemplatedCalculateInverseEffectiveMass<EMotionType::Dynamic, EMotionType::Kinematic>(mp1, invi1, inR1PlusU, nullptr, Mat44() /* Will not be used */, inR2, inWorldSpaceAxis);
 
 
 				case EMotionType::Static:
 				case EMotionType::Static:
-					TemplatedCalculateConstraintProperties<EMotionType::Dynamic, EMotionType::Static>(inDeltaTime, mp1, invi1, inR1PlusU, nullptr, Mat44() /* Will not be used */, inR2, inWorldSpaceAxis, inBias, inC, inFrequency, inDamping);
-					break;
+					return TemplatedCalculateInverseEffectiveMass<EMotionType::Dynamic, EMotionType::Static>(mp1, invi1, inR1PlusU, nullptr, Mat44() /* Will not be used */, inR2, inWorldSpaceAxis);
 
 
 				default:
 				default:
-					JPH_ASSERT(false);
 					break;
 					break;
 				}
 				}
 				break;
 				break;
@@ -171,18 +154,91 @@ public:
 
 
 		case EMotionType::Kinematic:
 		case EMotionType::Kinematic:
 			JPH_ASSERT(inBody2.IsDynamic());
 			JPH_ASSERT(inBody2.IsDynamic());
-			TemplatedCalculateConstraintProperties<EMotionType::Kinematic, EMotionType::Dynamic>(inDeltaTime, nullptr, Mat44() /* Will not be used */, inR1PlusU, inBody2.GetMotionPropertiesUnchecked(), inBody2.GetInverseInertia(), inR2, inWorldSpaceAxis, inBias, inC, inFrequency, inDamping);
-			break;
+			return TemplatedCalculateInverseEffectiveMass<EMotionType::Kinematic, EMotionType::Dynamic>(nullptr, Mat44() /* Will not be used */, inR1PlusU, inBody2.GetMotionPropertiesUnchecked(), inBody2.GetInverseInertia(), inR2, inWorldSpaceAxis);
 
 
 		case EMotionType::Static:
 		case EMotionType::Static:
 			JPH_ASSERT(inBody2.IsDynamic());
 			JPH_ASSERT(inBody2.IsDynamic());
-			TemplatedCalculateConstraintProperties<EMotionType::Static, EMotionType::Dynamic>(inDeltaTime, nullptr, Mat44() /* Will not be used */, inR1PlusU, inBody2.GetMotionPropertiesUnchecked(), inBody2.GetInverseInertia(), inR2, inWorldSpaceAxis, inBias, inC, inFrequency, inDamping);
-			break;
+			return TemplatedCalculateInverseEffectiveMass<EMotionType::Static, EMotionType::Dynamic>(nullptr, Mat44() /* Will not be used */, inR1PlusU, inBody2.GetMotionPropertiesUnchecked(), inBody2.GetInverseInertia(), inR2, inWorldSpaceAxis);
 
 
 		default:
 		default:
-			JPH_ASSERT(false);
 			break;
 			break;
 		}
 		}
+
+		JPH_ASSERT(false);
+		return 0.0f;
+	}
+
+public:
+	/// Templated form of CalculateConstraintProperties with the motion types baked in
+	template <EMotionType Type1, EMotionType Type2>
+	JPH_INLINE void				TemplatedCalculateConstraintProperties(const MotionProperties *inMotionProperties1, Mat44Arg inInvI1, Vec3Arg inR1PlusU, const MotionProperties *inMotionProperties2, Mat44Arg inInvI2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis, float inBias = 0.0f)
+	{
+		mEffectiveMass = 1.0f / TemplatedCalculateInverseEffectiveMass<Type1, Type2>(inMotionProperties1, inInvI1, inR1PlusU, inMotionProperties2, inInvI2, inR2, inWorldSpaceAxis);
+
+		mSpringPart.CalculateSpringPropertiesWithBias(inBias);
+
+		JPH_DET_LOG("TemplatedCalculateConstraintProperties: invI1: " << inInvI1 << " r1PlusU: " << inR1PlusU << " invI2: " << inInvI2 << " r2: " << inR2 << " bias: " << inBias << " r1PlusUxAxis: " << mR1PlusUxAxis << " r2xAxis: " << mR2xAxis << " invI1_R1PlusUxAxis: " << mInvI1_R1PlusUxAxis << " invI2_R2xAxis: " << mInvI2_R2xAxis << " effectiveMass: " << mEffectiveMass << " totalLambda: " << mTotalLambda);
+	}
+
+	/// 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 inR1PlusU See equations above (r1 + u)
+	/// @param inR2 See equations above (r2)
+	/// @param inWorldSpaceAxis Axis along which the constraint acts (normalized, pointing from body 1 to 2)
+	/// @param inBias Bias term (b) for the constraint impulse: lambda = J v + b
+	inline void					CalculateConstraintProperties(const Body &inBody1, Vec3Arg inR1PlusU, const Body &inBody2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis, float inBias = 0.0f)
+	{
+		mEffectiveMass = 1.0f / CalculateInverseEffectiveMass(inBody1, inR1PlusU, inBody2, inR2, inWorldSpaceAxis);
+
+		mSpringPart.CalculateSpringPropertiesWithBias(inBias);
+	}
+
+	/// Calculate properties used during the functions below
+	/// @param inDeltaTime Time step
+	/// @param inBody1 The first body that this constraint is attached to
+	/// @param inBody2 The second body that this constraint is attached to
+	/// @param inR1PlusU See equations above (r1 + u)
+	/// @param inR2 See equations above (r2)
+	/// @param inWorldSpaceAxis Axis along which the constraint acts (normalized, pointing from body 1 to 2)
+	/// @param inBias Bias term (b) for the constraint impulse: lambda = J v + b
+	///	@param inC Value of the constraint equation (C).
+	///	@param inFrequency Oscillation frequency (Hz).
+	///	@param inDamping Damping factor (0 = no damping, 1 = critical damping).
+	inline void					CalculateConstraintPropertiesWithFrequencyAndDamping(float inDeltaTime, const Body &inBody1, Vec3Arg inR1PlusU, const Body &inBody2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis, float inBias, float inC, float inFrequency, float inDamping)
+	{
+		float inv_effective_mass = CalculateInverseEffectiveMass(inBody1, inR1PlusU, inBody2, inR2, inWorldSpaceAxis);
+
+		mSpringPart.CalculateSpringPropertiesWithFrequencyAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inFrequency, inDamping, mEffectiveMass);
+	}
+
+	/// Calculate properties used during the functions below
+	/// @param inDeltaTime Time step
+	/// @param inBody1 The first body that this constraint is attached to
+	/// @param inBody2 The second body that this constraint is attached to
+	/// @param inR1PlusU See equations above (r1 + u)
+	/// @param inR2 See equations above (r2)
+	/// @param inWorldSpaceAxis Axis along which the constraint acts (normalized, pointing from body 1 to 2)
+	/// @param inBias Bias term (b) for the constraint impulse: lambda = J v + b
+	///	@param inC Value of the constraint equation (C).
+	///	@param inStiffness Spring stiffness k.
+	///	@param inDamping Spring damping coefficient c.
+	inline void					CalculateConstraintPropertiesWithStiffnessAndDamping(float inDeltaTime, const Body &inBody1, Vec3Arg inR1PlusU, const Body &inBody2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis, float inBias, float inC, float inStiffness, float inDamping)
+	{
+		float inv_effective_mass = CalculateInverseEffectiveMass(inBody1, inR1PlusU, inBody2, inR2, inWorldSpaceAxis);
+
+		mSpringPart.CalculateSpringPropertiesWithStiffnessAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inStiffness, inDamping, mEffectiveMass);
+	}
+
+	/// Selects one of the above functions based on the spring settings
+	inline void					CalculateConstraintPropertiesWithSettings(float inDeltaTime, const Body &inBody1, Vec3Arg inR1PlusU, const Body &inBody2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis, float inBias, float inC, const SpringSettings &inSpringSettings)
+	{
+		float inv_effective_mass = CalculateInverseEffectiveMass(inBody1, inR1PlusU, inBody2, inR2, inWorldSpaceAxis);
+
+		if (inSpringSettings.mMode == ESpringMode::FrequencyAndDamping)
+			mSpringPart.CalculateSpringPropertiesWithFrequencyAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inSpringSettings.mFrequency, inSpringSettings.mDamping, mEffectiveMass);
+		else
+			mSpringPart.CalculateSpringPropertiesWithStiffnessAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inSpringSettings.mStiffness, inSpringSettings.mDamping, mEffectiveMass);
 	}
 	}
 
 
 	/// Deactivate this constraint
 	/// Deactivate this constraint

+ 88 - 49
Jolt/Physics/Constraints/ConstraintPart/SpringPart.h

@@ -12,8 +12,69 @@ JPH_MSVC_SUPPRESS_WARNING(4723) // potential divide by 0 - caused by line: outEf
 /// Class used in other constraint parts to calculate the required bias factor in the lagrange multiplier for creating springs
 /// Class used in other constraint parts to calculate the required bias factor in the lagrange multiplier for creating springs
 class SpringPart
 class SpringPart
 {
 {
+private:
+	JPH_INLINE void				CalculateSpringPropertiesHelper(float inDeltaTime, float inInvEffectiveMass, float inBias, float inC, float inStiffness, float inDamping, float &outEffectiveMass)
+	{
+		// Soft constraints as per: Soft Contraints: Reinventing The Spring - Erin Catto - GDC 2011
+
+		// Note that the calculation of beta and gamma below are based on the solution of an implicit Euler integration scheme
+		// This scheme is unconditionally stable but has built in damping, so even when you set the damping ratio to 0 there will still
+		// be damping. See page 16 and 32.
+
+		// Calculate softness (gamma in the slides)
+		// See page 34 and note that the gamma needs to be divided by delta time since we're working with impulses rather than forces:
+		// softness = 1 / (dt * (c + dt * k))
+		// Note that the spring stiffness is k and the spring damping is c
+		mSoftness = 1.0f / (inDeltaTime * (inDamping + inDeltaTime * inStiffness));
+
+		// Calculate bias factor (baumgarte stabilization):
+		// beta = dt * k / (c + dt * k) = dt * k^2 * softness
+		// b = beta / dt * C = dt * k * softness * C
+		mBias = inBias + inDeltaTime * inStiffness * mSoftness * inC;
+			
+		// Update the effective mass, see post by Erin Catto: http://www.bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=4&t=1354
+		// 
+		// Newton's Law: 
+		// M * (v2 - v1) = J^T * lambda 
+		// 
+		// Velocity constraint with softness and Baumgarte: 
+		// J * v2 + softness * lambda + b = 0 
+		// 
+		// where b = beta * C / dt 
+		// 
+		// We know everything except v2 and lambda. 
+		// 
+		// First solve Newton's law for v2 in terms of lambda: 
+		// 
+		// v2 = v1 + M^-1 * J^T * lambda 
+		// 
+		// Substitute this expression into the velocity constraint: 
+		// 
+		// J * (v1 + M^-1 * J^T * lambda) + softness * lambda + b = 0 
+		// 
+		// Now collect coefficients of lambda: 
+		// 
+		// (J * M^-1 * J^T + softness) * lambda = - J * v1 - b 
+		// 
+		// Now we define: 
+		// 
+		// K = J * M^-1 * J^T + softness 
+		// 
+		// So our new effective mass is K^-1 
+		outEffectiveMass = 1.0f / (inInvEffectiveMass + mSoftness);
+	}
+
 public:
 public:
-	/// Calculate spring properties
+	/// Turn off the spring and set a bias only
+	///
+	/// @param inBias Bias term (b) for the constraint impulse: lambda = J v + b
+	inline void					CalculateSpringPropertiesWithBias(float inBias)
+	{
+		mSoftness = 0.0f;
+		mBias = inBias;
+	}
+
+	/// Calculate spring properties based on frequency and damping ratio
 	///
 	///
 	/// @param inDeltaTime Time step
 	/// @param inDeltaTime Time step
 	/// @param inInvEffectiveMass Inverse effective mass K
 	/// @param inInvEffectiveMass Inverse effective mass K
@@ -22,69 +83,47 @@ public:
 	///	@param inFrequency Oscillation frequency (Hz). Set to zero if you don't want to drive the constraint to zero with a spring.
 	///	@param inFrequency Oscillation frequency (Hz). Set to zero if you don't want to drive the constraint to zero with a spring.
 	///	@param inDamping Damping factor (0 = no damping, 1 = critical damping). Set to zero if you don't want to drive the constraint to zero with a spring.
 	///	@param inDamping Damping factor (0 = no damping, 1 = critical damping). Set to zero if you don't want to drive the constraint to zero with a spring.
 	/// @param outEffectiveMass On return, this contains the new effective mass K^-1
 	/// @param outEffectiveMass On return, this contains the new effective mass K^-1
-	inline void					CalculateSpringProperties(float inDeltaTime, float inInvEffectiveMass, float inBias, float inC, float inFrequency, float inDamping, float &outEffectiveMass)
+	inline void					CalculateSpringPropertiesWithFrequencyAndDamping(float inDeltaTime, float inInvEffectiveMass, float inBias, float inC, float inFrequency, float inDamping, float &outEffectiveMass)
 	{
 	{
 		outEffectiveMass = 1.0f / inInvEffectiveMass;
 		outEffectiveMass = 1.0f / inInvEffectiveMass;
 
 
-		// Soft constraints as per: Soft Contraints: Reinventing The Spring - Erin Catto - GDC 2011
 		if (inFrequency > 0.0f)
 		if (inFrequency > 0.0f)
 		{
 		{
 			// Calculate angular frequency
 			// Calculate angular frequency
 			float omega = 2.0f * JPH_PI * inFrequency;
 			float omega = 2.0f * JPH_PI * inFrequency;
 
 
-			// Calculate spring constant k and drag constant c (page 45)
+			// Calculate spring stiffness k and damping constant c (page 45)
 			float k = outEffectiveMass * Square(omega);
 			float k = outEffectiveMass * Square(omega);
 			float c = 2.0f * outEffectiveMass * inDamping * omega;
 			float c = 2.0f * outEffectiveMass * inDamping * omega;
 
 
-			// Note that the calculation of beta and gamma below are based on the solution of an implicit Euler integration scheme
-			// This scheme is unconditionally stable but has built in damping, so even when you set the damping ratio to 0 there will still
-			// be damping. See page 16 and 32.
-
-			// Calculate softness (gamma in the slides)
-			// See page 34 and note that the gamma needs to be divided by delta time since we're working with impulses rather than forces:
-			// softness = 1 / (dt * (c + dt * k))
-			mSoftness = 1.0f / (inDeltaTime * (c + inDeltaTime * k));
+			CalculateSpringPropertiesHelper(inDeltaTime, inInvEffectiveMass, inBias, inC, k, c, outEffectiveMass);
+		}
+		else
+		{
+			CalculateSpringPropertiesWithBias(inBias);
+		}
+	}
 
 
-			// Calculate bias factor (baumgarte stabilization):
-			// beta = dt * k / (c + dt * k) = dt * k^2 * softness
-			// b = beta / dt * C = dt * k * softness * C
-			mBias = inBias + inDeltaTime * k * mSoftness * inC;
-			
-			// Update the effective mass, see post by Erin Catto: http://www.bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=4&t=1354
-			// 
-			// Newton's Law: 
-			// M * (v2 - v1) = J^T * lambda 
-			// 
-			// Velocity constraint with softness and Baumgarte: 
-			// J * v2 + softness * lambda + b = 0 
-			// 
-			// where b = beta * C / dt 
-			// 
-			// We know everything except v2 and lambda. 
-			// 
-			// First solve Newton's law for v2 in terms of lambda: 
-			// 
-			// v2 = v1 + M^-1 * J^T * lambda 
-			// 
-			// Substitute this expression into the velocity constraint: 
-			// 
-			// J * (v1 + M^-1 * J^T * lambda) + softness * lambda + b = 0 
-			// 
-			// Now collect coefficients of lambda: 
-			// 
-			// (J * M^-1 * J^T + softness) * lambda = - J * v1 - b 
-			// 
-			// Now we define: 
-			// 
-			// K = J * M^-1 * J^T + softness 
-			// 
-			// So our new effective mass is K^-1 
-			outEffectiveMass = 1.0f / (inInvEffectiveMass + mSoftness);
+	/// Calculate spring properties with spring Stiffness (k) and damping (c), this is based on the spring equation: F = -k * x - c * v
+	///
+	/// @param inDeltaTime Time step
+	/// @param inInvEffectiveMass Inverse effective mass K
+	/// @param inBias Bias term (b) for the constraint impulse: lambda = J v + b
+	///	@param inC Value of the constraint equation (C). Set to zero if you don't want to drive the constraint to zero with a spring.
+	///	@param inStiffness Spring stiffness k. Set to zero if you don't want to drive the constraint to zero with a spring.
+	///	@param inDamping Spring damping coefficient c. Set to zero if you don't want to drive the constraint to zero with a spring.
+	/// @param outEffectiveMass On return, this contains the new effective mass K^-1
+	inline void					CalculateSpringPropertiesWithStiffnessAndDamping(float inDeltaTime, float inInvEffectiveMass, float inBias, float inC, float inStiffness, float inDamping, float &outEffectiveMass)
+	{
+		if (inStiffness > 0.0f)
+		{
+			CalculateSpringPropertiesHelper(inDeltaTime, inInvEffectiveMass, inBias, inC, inStiffness, inDamping, outEffectiveMass);
 		}
 		}
 		else
 		else
 		{
 		{
-			mSoftness = 0.0f;
-			mBias = inBias;
+			outEffectiveMass = 1.0f / inInvEffectiveMass;
+
+			CalculateSpringPropertiesWithBias(inBias);
 		}
 		}
 	}
 	}
 
 

+ 10 - 11
Jolt/Physics/Constraints/ConstraintPart/SwingTwistConstraintPart.h

@@ -209,12 +209,11 @@ public:
 	}
 	}
 
 
 	/// Calculate properties used during the functions below
 	/// Calculate properties used during the functions below
-	/// @param inDeltaTime Time step
 	/// @param inBody1 The first body that this constraint is attached to
 	/// @param inBody1 The first body that this constraint is attached to
 	/// @param inBody2 The second body that this constraint is attached to
 	/// @param inBody2 The second body that this constraint is attached to
 	/// @param inConstraintRotation The current rotation of the constraint in constraint space
 	/// @param inConstraintRotation The current rotation of the constraint in constraint space
 	/// @param inConstraintToWorld Rotates from constraint space into world space
 	/// @param inConstraintToWorld Rotates from constraint space into world space
-	inline void					CalculateConstraintProperties(float inDeltaTime, const Body &inBody1, const Body &inBody2, QuatArg inConstraintRotation, QuatArg inConstraintToWorld)
+	inline void					CalculateConstraintProperties(const Body &inBody1, const Body &inBody2, QuatArg inConstraintRotation, QuatArg inConstraintToWorld)
 	{
 	{
 		// Decompose into swing and twist
 		// Decompose into swing and twist
 		Quat q_swing, q_twist;
 		Quat q_swing, q_twist;
@@ -234,18 +233,18 @@ public:
 			if (mRotationFlags & SwingZLocked)
 			if (mRotationFlags & SwingZLocked)
 			{
 			{
 				// Swing fully locked
 				// Swing fully locked
-				mSwingLimitYConstraintPart.CalculateConstraintProperties(inDeltaTime, inBody1, inBody2, mWorldSpaceSwingLimitYRotationAxis);
-				mSwingLimitZConstraintPart.CalculateConstraintProperties(inDeltaTime, inBody1, inBody2, mWorldSpaceSwingLimitZRotationAxis);
+				mSwingLimitYConstraintPart.CalculateConstraintProperties(inBody1, inBody2, mWorldSpaceSwingLimitYRotationAxis);
+				mSwingLimitZConstraintPart.CalculateConstraintProperties(inBody1, inBody2, mWorldSpaceSwingLimitZRotationAxis);
 			}
 			}
 			else
 			else
 			{
 			{
 				// Swing only locked around Y
 				// Swing only locked around Y
-				mSwingLimitYConstraintPart.CalculateConstraintProperties(inDeltaTime, inBody1, inBody2, mWorldSpaceSwingLimitYRotationAxis);
+				mSwingLimitYConstraintPart.CalculateConstraintProperties(inBody1, inBody2, mWorldSpaceSwingLimitYRotationAxis);
 				if (swing_z_clamped)
 				if (swing_z_clamped)
 				{
 				{
 					if (Sign(q_swing.GetW()) * q_swing.GetZ() < 0.0f)
 					if (Sign(q_swing.GetW()) * q_swing.GetZ() < 0.0f)
 						mWorldSpaceSwingLimitZRotationAxis = -mWorldSpaceSwingLimitZRotationAxis; // Flip axis if angle is negative because the impulse limit is going to be between [-FLT_MAX, 0]
 						mWorldSpaceSwingLimitZRotationAxis = -mWorldSpaceSwingLimitZRotationAxis; // Flip axis if angle is negative because the impulse limit is going to be between [-FLT_MAX, 0]
-					mSwingLimitZConstraintPart.CalculateConstraintProperties(inDeltaTime, inBody1, inBody2, mWorldSpaceSwingLimitZRotationAxis);
+					mSwingLimitZConstraintPart.CalculateConstraintProperties(inBody1, inBody2, mWorldSpaceSwingLimitZRotationAxis);
 				}
 				}
 				else
 				else
 					mSwingLimitZConstraintPart.Deactivate();
 					mSwingLimitZConstraintPart.Deactivate();
@@ -262,11 +261,11 @@ public:
 			{
 			{
 				if (Sign(q_swing.GetW()) * q_swing.GetY() < 0.0f)
 				if (Sign(q_swing.GetW()) * q_swing.GetY() < 0.0f)
 					mWorldSpaceSwingLimitYRotationAxis = -mWorldSpaceSwingLimitYRotationAxis; // Flip axis if angle is negative because the impulse limit is going to be between [-FLT_MAX, 0]
 					mWorldSpaceSwingLimitYRotationAxis = -mWorldSpaceSwingLimitYRotationAxis; // Flip axis if angle is negative because the impulse limit is going to be between [-FLT_MAX, 0]
-				mSwingLimitYConstraintPart.CalculateConstraintProperties(inDeltaTime, inBody1, inBody2, mWorldSpaceSwingLimitYRotationAxis);
+				mSwingLimitYConstraintPart.CalculateConstraintProperties(inBody1, inBody2, mWorldSpaceSwingLimitYRotationAxis);
 			}
 			}
 			else
 			else
 				mSwingLimitYConstraintPart.Deactivate();
 				mSwingLimitYConstraintPart.Deactivate();
-			mSwingLimitZConstraintPart.CalculateConstraintProperties(inDeltaTime, inBody1, inBody2, mWorldSpaceSwingLimitZRotationAxis);
+			mSwingLimitZConstraintPart.CalculateConstraintProperties(inBody1, inBody2, mWorldSpaceSwingLimitZRotationAxis);
 		}
 		}
 		else if ((mRotationFlags & SwingYZFree) != SwingYZFree)
 		else if ((mRotationFlags & SwingYZFree) != SwingYZFree)
 		{
 		{
@@ -281,7 +280,7 @@ public:
 				if (len != 0.0f)
 				if (len != 0.0f)
 				{
 				{
 					mWorldSpaceSwingLimitYRotationAxis /= len;
 					mWorldSpaceSwingLimitYRotationAxis /= len;
-					mSwingLimitYConstraintPart.CalculateConstraintProperties(inDeltaTime, inBody1, inBody2, mWorldSpaceSwingLimitYRotationAxis);
+					mSwingLimitYConstraintPart.CalculateConstraintProperties(inBody1, inBody2, mWorldSpaceSwingLimitYRotationAxis);
 				}
 				}
 				else
 				else
 					mSwingLimitYConstraintPart.Deactivate();
 					mSwingLimitYConstraintPart.Deactivate();
@@ -301,7 +300,7 @@ public:
 		{
 		{
 			// Twist locked, always activate constraint
 			// Twist locked, always activate constraint
 			mWorldSpaceTwistLimitRotationAxis = (inConstraintToWorld * q_swing).RotateAxisX();
 			mWorldSpaceTwistLimitRotationAxis = (inConstraintToWorld * q_swing).RotateAxisX();
-			mTwistLimitConstraintPart.CalculateConstraintProperties(inDeltaTime, inBody1, inBody2, mWorldSpaceTwistLimitRotationAxis);
+			mTwistLimitConstraintPart.CalculateConstraintProperties(inBody1, inBody2, mWorldSpaceTwistLimitRotationAxis);
 		}
 		}
 		else if ((mRotationFlags & TwistXFree) == 0)
 		else if ((mRotationFlags & TwistXFree) == 0)
 		{
 		{
@@ -311,7 +310,7 @@ public:
 				mWorldSpaceTwistLimitRotationAxis = (inConstraintToWorld * q_swing).RotateAxisX();				
 				mWorldSpaceTwistLimitRotationAxis = (inConstraintToWorld * q_swing).RotateAxisX();				
 				if (Sign(q_twist.GetW()) * q_twist.GetX() < 0.0f)
 				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]
 					mWorldSpaceTwistLimitRotationAxis = -mWorldSpaceTwistLimitRotationAxis; // Flip axis if angle is negative because the impulse limit is going to be between [-FLT_MAX, 0]
-				mTwistLimitConstraintPart.CalculateConstraintProperties(inDeltaTime, inBody1, inBody2, mWorldSpaceTwistLimitRotationAxis);
+				mTwistLimitConstraintPart.CalculateConstraintProperties(inBody1, inBody2, mWorldSpaceTwistLimitRotationAxis);
 			}
 			}
 			else
 			else
 				mTwistLimitConstraintPart.Deactivate();
 				mTwistLimitConstraintPart.Deactivate();

+ 6 - 8
Jolt/Physics/Constraints/ContactConstraintManager.cpp

@@ -33,14 +33,14 @@ bool ContactConstraintManager::sDrawContactManifolds = false;
 // ContactConstraintManager::WorldContactPoint
 // ContactConstraintManager::WorldContactPoint
 ////////////////////////////////////////////////////////////////////////////////////////////////////////
 ////////////////////////////////////////////////////////////////////////////////////////////////////////
 
 
-void ContactConstraintManager::WorldContactPoint::CalculateNonPenetrationConstraintProperties(float inDeltaTime, const Body &inBody1, const Body &inBody2, RVec3Arg inWorldSpacePosition1, RVec3Arg inWorldSpacePosition2, Vec3Arg inWorldSpaceNormal)
+void ContactConstraintManager::WorldContactPoint::CalculateNonPenetrationConstraintProperties(const Body &inBody1, const Body &inBody2, RVec3Arg inWorldSpacePosition1, RVec3Arg inWorldSpacePosition2, Vec3Arg inWorldSpaceNormal)
 {
 {
 	// Calculate collision points relative to body
 	// Calculate collision points relative to body
 	RVec3 p = 0.5_r * (inWorldSpacePosition1 + inWorldSpacePosition2);
 	RVec3 p = 0.5_r * (inWorldSpacePosition1 + inWorldSpacePosition2);
 	Vec3 r1 = Vec3(p - inBody1.GetCenterOfMassPosition());
 	Vec3 r1 = Vec3(p - inBody1.GetCenterOfMassPosition());
 	Vec3 r2 = Vec3(p - inBody2.GetCenterOfMassPosition());
 	Vec3 r2 = Vec3(p - inBody2.GetCenterOfMassPosition());
 
 
-	mNonPenetrationConstraint.CalculateConstraintProperties(inDeltaTime, inBody1, r1, inBody2, r2, inWorldSpaceNormal);
+	mNonPenetrationConstraint.CalculateConstraintProperties(inBody1, r1, inBody2, r2, inWorldSpaceNormal);
 }
 }
 
 
 template <EMotionType Type1, EMotionType Type2>
 template <EMotionType Type1, EMotionType Type2>
@@ -107,14 +107,14 @@ JPH_INLINE void ContactConstraintManager::WorldContactPoint::CalculateFrictionAn
 		normal_velocity_bias = speculative_contact_velocity_bias;
 		normal_velocity_bias = speculative_contact_velocity_bias;
 	}
 	}
 
 
-	mNonPenetrationConstraint.TemplatedCalculateConstraintProperties<Type1, Type2>(inDeltaTime, mp1, inInvI1, r1, mp2, inInvI2, r2, inWorldSpaceNormal, normal_velocity_bias);
+	mNonPenetrationConstraint.TemplatedCalculateConstraintProperties<Type1, Type2>(mp1, inInvI1, r1, mp2, inInvI2, r2, inWorldSpaceNormal, normal_velocity_bias);
 
 
 	// Calculate friction part
 	// Calculate friction part
 	if (inCombinedFriction > 0.0f)
 	if (inCombinedFriction > 0.0f)
 	{
 	{
 		// Implement friction as 2 AxisContraintParts
 		// Implement friction as 2 AxisContraintParts
-		mFrictionConstraint1.TemplatedCalculateConstraintProperties<Type1, Type2>(inDeltaTime, mp1, inInvI1, r1, mp2, inInvI2, r2, inWorldSpaceTangent1, inSurfaceVelocity1);
-		mFrictionConstraint2.TemplatedCalculateConstraintProperties<Type1, Type2>(inDeltaTime, mp1, inInvI1, r1, mp2, inInvI2, r2, inWorldSpaceTangent2, inSurfaceVelocity2);
+		mFrictionConstraint1.TemplatedCalculateConstraintProperties<Type1, Type2>(mp1, inInvI1, r1, mp2, inInvI2, r2, inWorldSpaceTangent1, inSurfaceVelocity1);
+		mFrictionConstraint2.TemplatedCalculateConstraintProperties<Type1, Type2>(mp1, inInvI1, r1, mp2, inInvI2, r2, inWorldSpaceTangent2, inSurfaceVelocity2);
 	}
 	}
 	else
 	else
 	{
 	{
@@ -1568,8 +1568,6 @@ bool ContactConstraintManager::SolvePositionConstraints(const uint32 *inConstrai
 
 
 	bool any_impulse_applied = false;
 	bool any_impulse_applied = false;
 
 
-	float delta_time = mUpdateContext->mSubStepDeltaTime;
-
 	for (const uint32 *constraint_idx = inConstraintIdxBegin; constraint_idx < inConstraintIdxEnd; ++constraint_idx)
 	for (const uint32 *constraint_idx = inConstraintIdxBegin; constraint_idx < inConstraintIdxEnd; ++constraint_idx)
 	{
 	{
 		ContactConstraint &constraint = mConstraints[*constraint_idx];
 		ContactConstraint &constraint = mConstraints[*constraint_idx];
@@ -1599,7 +1597,7 @@ bool ContactConstraintManager::SolvePositionConstraints(const uint32 *inConstrai
 			if (separation < 0.0f)
 			if (separation < 0.0f)
 			{
 			{
 				// Update constraint properties (bodies may have moved)
 				// Update constraint properties (bodies may have moved)
-				wcp.CalculateNonPenetrationConstraintProperties(delta_time, body1, body2, p1, p2, ws_normal);
+				wcp.CalculateNonPenetrationConstraintProperties(body1, body2, p1, p2, ws_normal);
 
 
 				// Solve position errors
 				// Solve position errors
 				if (wcp.mNonPenetrationConstraint.SolvePositionConstraint(body1, body2, ws_normal, separation, mPhysicsSettings.mBaumgarte))
 				if (wcp.mNonPenetrationConstraint.SolvePositionConstraint(body1, body2, ws_normal, separation, mPhysicsSettings.mBaumgarte))

+ 1 - 1
Jolt/Physics/Constraints/ContactConstraintManager.h

@@ -421,7 +421,7 @@ private:
 	{
 	{
 	public:
 	public:
 		/// Calculate constraint properties below
 		/// Calculate constraint properties below
-		void					CalculateNonPenetrationConstraintProperties(float inDeltaTime, const Body &inBody1, const Body &inBody2, RVec3Arg inWorldSpacePosition1, RVec3Arg inWorldSpacePosition2, Vec3Arg inWorldSpaceNormal);
+		void					CalculateNonPenetrationConstraintProperties(const Body &inBody1, const Body &inBody2, RVec3Arg inWorldSpacePosition1, RVec3Arg inWorldSpacePosition2, Vec3Arg inWorldSpaceNormal);
 		template <EMotionType Type1, EMotionType Type2>
 		template <EMotionType Type1, EMotionType Type2>
 		JPH_INLINE void			CalculateFrictionAndNonPenetrationConstraintProperties(float inDeltaTime, const Body &inBody1, const Body &inBody2, Mat44Arg inInvI1, Mat44Arg inInvI2, RVec3Arg inWorldSpacePosition1, RVec3Arg inWorldSpacePosition2, Vec3Arg inWorldSpaceNormal, Vec3Arg inWorldSpaceTangent1, Vec3Arg inWorldSpaceTangent2, float inCombinedRestitution, float inCombinedFriction, float inMinVelocityForRestitution, float inSurfaceVelocity1, float inSurfaceVelocity2);
 		JPH_INLINE void			CalculateFrictionAndNonPenetrationConstraintProperties(float inDeltaTime, const Body &inBody1, const Body &inBody2, Mat44Arg inInvI1, Mat44Arg inInvI2, RVec3Arg inWorldSpacePosition1, RVec3Arg inWorldSpacePosition2, Vec3Arg inWorldSpaceNormal, Vec3Arg inWorldSpaceTangent1, Vec3Arg inWorldSpaceTangent2, float inCombinedRestitution, float inCombinedFriction, float inMinVelocityForRestitution, float inSurfaceVelocity1, float inSurfaceVelocity2);
 
 

+ 29 - 29
Jolt/Physics/Constraints/DistanceConstraint.cpp

@@ -26,8 +26,9 @@ JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(DistanceConstraintSettings)
 	JPH_ADD_ATTRIBUTE(DistanceConstraintSettings, mPoint2)
 	JPH_ADD_ATTRIBUTE(DistanceConstraintSettings, mPoint2)
 	JPH_ADD_ATTRIBUTE(DistanceConstraintSettings, mMinDistance)
 	JPH_ADD_ATTRIBUTE(DistanceConstraintSettings, mMinDistance)
 	JPH_ADD_ATTRIBUTE(DistanceConstraintSettings, mMaxDistance)
 	JPH_ADD_ATTRIBUTE(DistanceConstraintSettings, mMaxDistance)
-	JPH_ADD_ATTRIBUTE(DistanceConstraintSettings, mFrequency)
-	JPH_ADD_ATTRIBUTE(DistanceConstraintSettings, mDamping)
+	JPH_ADD_ENUM_ATTRIBUTE_WITH_ALIAS(DistanceConstraintSettings, mLimitsSpringSettings.mMode, "mSpringMode")
+	JPH_ADD_ATTRIBUTE_WITH_ALIAS(DistanceConstraintSettings, mLimitsSpringSettings.mFrequency, "mFrequency") // Renaming attributes to stay compatible with old versions of the library
+	JPH_ADD_ATTRIBUTE_WITH_ALIAS(DistanceConstraintSettings, mLimitsSpringSettings.mDamping, "mDamping")
 }
 }
 
 
 void DistanceConstraintSettings::SaveBinaryState(StreamOut &inStream) const
 void DistanceConstraintSettings::SaveBinaryState(StreamOut &inStream) const
@@ -39,8 +40,7 @@ void DistanceConstraintSettings::SaveBinaryState(StreamOut &inStream) const
 	inStream.Write(mPoint2);
 	inStream.Write(mPoint2);
 	inStream.Write(mMinDistance);
 	inStream.Write(mMinDistance);
 	inStream.Write(mMaxDistance);
 	inStream.Write(mMaxDistance);
-	inStream.Write(mFrequency);
-	inStream.Write(mDamping);
+	mLimitsSpringSettings.SaveBinaryState(inStream);
 }
 }
 
 
 void DistanceConstraintSettings::RestoreBinaryState(StreamIn &inStream)
 void DistanceConstraintSettings::RestoreBinaryState(StreamIn &inStream)
@@ -52,8 +52,7 @@ void DistanceConstraintSettings::RestoreBinaryState(StreamIn &inStream)
 	inStream.Read(mPoint2);
 	inStream.Read(mPoint2);
 	inStream.Read(mMinDistance);
 	inStream.Read(mMinDistance);
 	inStream.Read(mMaxDistance);
 	inStream.Read(mMaxDistance);
-	inStream.Read(mFrequency);
-	inStream.Read(mDamping);
+	mLimitsSpringSettings.RestoreBinaryState(inStream);
 }
 }
 
 
 TwoBodyConstraint *DistanceConstraintSettings::Create(Body &inBody1, Body &inBody2) const
 TwoBodyConstraint *DistanceConstraintSettings::Create(Body &inBody1, Body &inBody2) const
@@ -90,9 +89,8 @@ DistanceConstraint::DistanceConstraint(Body &inBody1, Body &inBody2, const Dista
 	// Most likely gravity is going to tear us apart (this is only used when the distance between the points = 0)
 	// Most likely gravity is going to tear us apart (this is only used when the distance between the points = 0)
 	mWorldSpaceNormal = Vec3::sAxisY(); 
 	mWorldSpaceNormal = Vec3::sAxisY(); 
 
 
-	// Store frequency and damping
-	SetFrequency(inSettings.mFrequency);
-	SetDamping(inSettings.mDamping);
+	// Store spring settings
+	SetLimitsSpringSettings(inSettings.mLimitsSpringSettings);
 }
 }
 
 
 void DistanceConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM)
 void DistanceConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM)
@@ -122,15 +120,15 @@ void DistanceConstraint::CalculateConstraintProperties(float inDeltaTime)
 
 
 	if (mMinDistance == mMaxDistance)
 	if (mMinDistance == mMaxDistance)
 	{
 	{
-		mAxisConstraint.CalculateConstraintProperties(inDeltaTime, *mBody1, r1_plus_u, *mBody2, r2, mWorldSpaceNormal, 0.0f, delta_len - mMinDistance, mFrequency, mDamping);
+		mAxisConstraint.CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, r1_plus_u, *mBody2, r2, mWorldSpaceNormal, 0.0f, delta_len - mMinDistance, mLimitsSpringSettings);
 
 
 		// Single distance, allow constraint forces in both directions
 		// Single distance, allow constraint forces in both directions
 		mMinLambda = -FLT_MAX;
 		mMinLambda = -FLT_MAX;
 		mMaxLambda = FLT_MAX;
 		mMaxLambda = FLT_MAX;
 	}
 	}
-	if (delta_len <= mMinDistance)
+	else if (delta_len <= mMinDistance)
 	{
 	{
-		mAxisConstraint.CalculateConstraintProperties(inDeltaTime, *mBody1, r1_plus_u, *mBody2, r2, mWorldSpaceNormal, 0.0f, delta_len - mMinDistance, mFrequency, mDamping);
+		mAxisConstraint.CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, r1_plus_u, *mBody2, r2, mWorldSpaceNormal, 0.0f, delta_len - mMinDistance, mLimitsSpringSettings);
 
 
 		// Allow constraint forces to make distance bigger only
 		// Allow constraint forces to make distance bigger only
 		mMinLambda = 0;
 		mMinLambda = 0;
@@ -138,7 +136,7 @@ void DistanceConstraint::CalculateConstraintProperties(float inDeltaTime)
 	}
 	}
 	else if (delta_len >= mMaxDistance)
 	else if (delta_len >= mMaxDistance)
 	{
 	{
-		mAxisConstraint.CalculateConstraintProperties(inDeltaTime, *mBody1, r1_plus_u, *mBody2, r2, mWorldSpaceNormal, 0.0f, delta_len - mMaxDistance, mFrequency, mDamping);
+		mAxisConstraint.CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, r1_plus_u, *mBody2, r2, mWorldSpaceNormal, 0.0f, delta_len - mMaxDistance, mLimitsSpringSettings);
 
 
 		// Allow constraint forces to make distance smaller only
 		// Allow constraint forces to make distance smaller only
 		mMinLambda = -FLT_MAX;
 		mMinLambda = -FLT_MAX;
@@ -168,21 +166,24 @@ bool DistanceConstraint::SolveVelocityConstraint(float inDeltaTime)
 
 
 bool DistanceConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte)
 bool DistanceConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte)
 {
 {
-	float distance = Vec3(mWorldSpacePosition2 - mWorldSpacePosition1).Dot(mWorldSpaceNormal);
-
-	// Calculate position error
-	float position_error = 0.0f;
-	if (distance < mMinDistance)
-		position_error = distance - mMinDistance;
-	else if (distance > mMaxDistance)
-		position_error = distance - mMaxDistance;
-
-	if (position_error != 0.0f)
+	if (mLimitsSpringSettings.mFrequency <= 0.0f) // When the spring is active, we don't need to solve the position constraint
 	{
 	{
-		// Update constraint properties (bodies may have moved)
-		CalculateConstraintProperties(inDeltaTime);
-
-		return mAxisConstraint.SolvePositionConstraint(*mBody1, *mBody2, mWorldSpaceNormal, position_error, inBaumgarte);
+		float distance = Vec3(mWorldSpacePosition2 - mWorldSpacePosition1).Dot(mWorldSpaceNormal);
+
+		// Calculate position error
+		float position_error = 0.0f;
+		if (distance < mMinDistance)
+			position_error = distance - mMinDistance;
+		else if (distance > mMaxDistance)
+			position_error = distance - mMaxDistance;
+
+		if (position_error != 0.0f)
+		{
+			// Update constraint properties (bodies may have moved)
+			CalculateConstraintProperties(inDeltaTime);
+
+			return mAxisConstraint.SolvePositionConstraint(*mBody1, *mBody2, mWorldSpaceNormal, position_error, inBaumgarte);
+		}
 	}
 	}
 
 
 	return false;
 	return false;
@@ -243,8 +244,7 @@ Ref<ConstraintSettings> DistanceConstraint::GetConstraintSettings() const
 	settings->mPoint2 = RVec3(mLocalSpacePosition2);
 	settings->mPoint2 = RVec3(mLocalSpacePosition2);
 	settings->mMinDistance = mMinDistance;
 	settings->mMinDistance = mMinDistance;
 	settings->mMaxDistance = mMaxDistance;
 	settings->mMaxDistance = mMaxDistance;
-	settings->mFrequency = mFrequency;
-	settings->mDamping = mDamping;
+	settings->mLimitsSpringSettings = mLimitsSpringSettings;
 	return settings;
 	return settings;
 }
 }
 
 

+ 8 - 16
Jolt/Physics/Constraints/DistanceConstraint.h

@@ -36,12 +36,8 @@ public:
 	float						mMinDistance = -1.0f;
 	float						mMinDistance = -1.0f;
 	float						mMaxDistance = -1.0f;
 	float						mMaxDistance = -1.0f;
 
 
-	/// If mFrequency > 0 the constraint will be soft and mFrequency specifies the oscillation frequency in Hz and mDamping the damping ratio (0 = no damping, 1 = critical damping).
-	/// 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).
-	/// Note that if you set mDamping = 0, you will not get an infinite oscillation. Because we integrate physics using an explicit Euler scheme, there is always energy loss.
-	/// This is done to keep the simulation from exploding, because with a damping of 0 and even the slightest rounding error, the oscillation could become bigger and bigger until the simluation explodes.
-	float						mFrequency = 0.0f;
-	float						mDamping = 0.0f;
+	/// When enabled, this makes the limits soft. When the constraint exceeds the limits, a spring force will pull it back.
+	SpringSettings				mLimitsSpringSettings;
 
 
 protected:
 protected:
 	// See: ConstraintSettings::RestoreBinaryState
 	// See: ConstraintSettings::RestoreBinaryState
@@ -80,13 +76,10 @@ public:
 	float						GetMinDistance() const										{ return mMinDistance; }
 	float						GetMinDistance() const										{ return mMinDistance; }
 	float						GetMaxDistance() const										{ return mMaxDistance; }
 	float						GetMaxDistance() const										{ return mMaxDistance; }
 
 
-	/// Update the spring frequency for the constraint
-	void						SetFrequency(float inFrequency)								{ JPH_ASSERT(inFrequency >= 0.0f); mFrequency = inFrequency; }
-	float						GetFrequency() const										{ return mFrequency; }
-
-	/// Update the spring damping for the constraint
-	void						SetDamping(float inDamping)									{ JPH_ASSERT(inDamping >= 0.0f); mDamping = inDamping; }
-	float						GetDamping() const											{ return mDamping; }
+	/// Update the limits spring settings
+	const SpringSettings &		GetLimitsSpringSettings() const								{ return mLimitsSpringSettings; }
+	SpringSettings &			GetLimitsSpringSettings()									{ return mLimitsSpringSettings; }
+	void						SetLimitsSpringSettings(const SpringSettings &inLimitsSpringSettings) { mLimitsSpringSettings = inLimitsSpringSettings; }
 
 
 	///@name Get Lagrange multiplier from last physics update (relates to how much force/torque was applied to satisfy the constraint)
 	///@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 mAxisConstraint.GetTotalLambda(); }
 	inline float	 			GetTotalLambdaPosition() const								{ return mAxisConstraint.GetTotalLambda(); }
@@ -105,9 +98,8 @@ private:
 	float						mMinDistance;
 	float						mMinDistance;
 	float						mMaxDistance;
 	float						mMaxDistance;
 
 
-	// Soft constraint properties (see DistanceConstraintSettings)
-	float						mFrequency;
-	float						mDamping;
+	// Soft constraint limits
+	SpringSettings				mLimitsSpringSettings;
 
 
 	// RUN TIME PROPERTIES FOLLOW
 	// RUN TIME PROPERTIES FOLLOW
 
 

+ 35 - 12
Jolt/Physics/Constraints/HingeConstraint.cpp

@@ -29,6 +29,7 @@ JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(HingeConstraintSettings)
 	JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mNormalAxis2)
 	JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mNormalAxis2)
 	JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mLimitsMin)
 	JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mLimitsMin)
 	JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mLimitsMax)
 	JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mLimitsMax)
+	JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mLimitsSpringSettings)
 	JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mMaxFrictionTorque)
 	JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mMaxFrictionTorque)
 	JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mMotorSettings)
 	JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mMotorSettings)
 }
 }
@@ -47,6 +48,7 @@ void HingeConstraintSettings::SaveBinaryState(StreamOut &inStream) const
 	inStream.Write(mLimitsMin);
 	inStream.Write(mLimitsMin);
 	inStream.Write(mLimitsMax);
 	inStream.Write(mLimitsMax);
 	inStream.Write(mMaxFrictionTorque);
 	inStream.Write(mMaxFrictionTorque);
+	mLimitsSpringSettings.SaveBinaryState(inStream);
 	mMotorSettings.SaveBinaryState(inStream);
 	mMotorSettings.SaveBinaryState(inStream);
 }
 }
 
 
@@ -64,6 +66,7 @@ void HingeConstraintSettings::RestoreBinaryState(StreamIn &inStream)
 	inStream.Read(mLimitsMin);
 	inStream.Read(mLimitsMin);
 	inStream.Read(mLimitsMax);
 	inStream.Read(mLimitsMax);
 	inStream.Read(mMaxFrictionTorque);
 	inStream.Read(mMaxFrictionTorque);
+	mLimitsSpringSettings.RestoreBinaryState(inStream);
 	mMotorSettings.RestoreBinaryState(inStream);}
 	mMotorSettings.RestoreBinaryState(inStream);}
 
 
 TwoBodyConstraint *HingeConstraintSettings::Create(Body &inBody1, Body &inBody2) const
 TwoBodyConstraint *HingeConstraintSettings::Create(Body &inBody1, Body &inBody2) const
@@ -77,7 +80,7 @@ HingeConstraint::HingeConstraint(Body &inBody1, Body &inBody2, const HingeConstr
 	mMotorSettings(inSettings.mMotorSettings)
 	mMotorSettings(inSettings.mMotorSettings)
 {
 {
 	// Store limits
 	// Store limits
-	JPH_ASSERT(inSettings.mLimitsMin != inSettings.mLimitsMax, "Better use a fixed constraint in this case");
+	JPH_ASSERT(inSettings.mLimitsMin != inSettings.mLimitsMax || inSettings.mLimitsSpringSettings.mFrequency > 0.0f, "Better use a fixed constraint in this case");
 	SetLimits(inSettings.mLimitsMin, inSettings.mLimitsMax);
 	SetLimits(inSettings.mLimitsMin, inSettings.mLimitsMax);
 
 
 	// Store inverse of initial rotation from body 1 to body 2 in body 1 space
 	// Store inverse of initial rotation from body 1 to body 2 in body 1 space
@@ -110,6 +113,9 @@ HingeConstraint::HingeConstraint(Body &inBody1, Body &inBody2, const HingeConstr
 		mLocalSpaceHingeAxis2 = inSettings.mHingeAxis2;
 		mLocalSpaceHingeAxis2 = inSettings.mHingeAxis2;
 		mLocalSpaceNormalAxis2 = inSettings.mNormalAxis2;
 		mLocalSpaceNormalAxis2 = inSettings.mNormalAxis2;
 	}
 	}
+
+	// Store spring settings
+	SetLimitsSpringSettings(inSettings.mLimitsSpringSettings);
 }
 }
 
 
 void HingeConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM)
 void HingeConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM)
@@ -172,7 +178,7 @@ void HingeConstraint::CalculateRotationLimitsConstraintProperties(float inDeltaT
 {
 {
 	// Apply constraint if outside of limits
 	// Apply constraint if outside of limits
 	if (mHasLimits && (mTheta <= mLimitsMin || mTheta >= mLimitsMax))
 	if (mHasLimits && (mTheta <= mLimitsMin || mTheta >= mLimitsMax))
-		mRotationLimitsConstraintPart.CalculateConstraintProperties(inDeltaTime, *mBody1, *mBody2, mA1);
+		mRotationLimitsConstraintPart.CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, *mBody2, mA1, 0.0f, GetSmallestAngleToLimit(), mLimitsSpringSettings);
 	else
 	else
 		mRotationLimitsConstraintPart.Deactivate();
 		mRotationLimitsConstraintPart.Deactivate();
 }
 }
@@ -183,17 +189,17 @@ void HingeConstraint::CalculateMotorConstraintProperties(float inDeltaTime)
 	{
 	{
 	case EMotorState::Off:
 	case EMotorState::Off:
 		if (mMaxFrictionTorque > 0.0f)
 		if (mMaxFrictionTorque > 0.0f)
-			mMotorConstraintPart.CalculateConstraintProperties(inDeltaTime, *mBody1, *mBody2, mA1);
+			mMotorConstraintPart.CalculateConstraintProperties(*mBody1, *mBody2, mA1);
 		else
 		else
 			mMotorConstraintPart.Deactivate();
 			mMotorConstraintPart.Deactivate();
 		break;
 		break;
 
 
 	case EMotorState::Velocity:
 	case EMotorState::Velocity:
-		mMotorConstraintPart.CalculateConstraintProperties(inDeltaTime, *mBody1, *mBody2, mA1, -mTargetAngularVelocity);
+		mMotorConstraintPart.CalculateConstraintProperties(*mBody1, *mBody2, mA1, -mTargetAngularVelocity);
 		break;
 		break;
 
 
 	case EMotorState::Position:
 	case EMotorState::Position:
-		mMotorConstraintPart.CalculateConstraintProperties(inDeltaTime, *mBody1, *mBody2, mA1, 0.0f, CenterAngleAroundZero(mTheta - mTargetAngle), mMotorSettings.mFrequency, mMotorSettings.mDamping);
+		mMotorConstraintPart.CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, *mBody2, mA1, 0.0f, CenterAngleAroundZero(mTheta - mTargetAngle), mMotorSettings.mSpringSettings);
 		break;
 		break;
 	}	
 	}	
 }
 }
@@ -258,10 +264,23 @@ bool HingeConstraint::SolveVelocityConstraint(float inDeltaTime)
 	bool limit = false;
 	bool limit = false;
 	if (mRotationLimitsConstraintPart.IsActive())
 	if (mRotationLimitsConstraintPart.IsActive())
 	{
 	{
-		if (GetSmallestAngleToLimit() < 0.0f)
-			limit = mRotationLimitsConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mA1, 0, FLT_MAX);
+		float min_lambda, max_lambda;
+		if (mLimitsMin == mLimitsMax)
+		{
+			min_lambda = -FLT_MAX;
+			max_lambda = FLT_MAX;
+		}
+		else if (GetSmallestAngleToLimit() < 0.0f)
+		{
+			min_lambda = 0.0f;
+			max_lambda = FLT_MAX;
+		}
 		else
 		else
-			limit = mRotationLimitsConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mA1, -FLT_MAX, 0);
+		{
+			min_lambda = -FLT_MAX;
+			max_lambda = 0.0f;
+		}
+		limit = mRotationLimitsConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mA1, min_lambda, max_lambda);
 	}
 	}
 
 
 	return motor || pos || rot || limit;
 	return motor || pos || rot || limit;
@@ -283,10 +302,13 @@ bool HingeConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgar
 
 
 	// Solve rotation limits
 	// Solve rotation limits
 	bool limit = false;
 	bool limit = false;
-	CalculateA1AndTheta();
-	CalculateRotationLimitsConstraintProperties(inDeltaTime);
-	if (mRotationLimitsConstraintPart.IsActive())
-		limit = mRotationLimitsConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, GetSmallestAngleToLimit(), inBaumgarte);
+	if (mHasLimits && mLimitsSpringSettings.mFrequency <= 0.0f)
+	{
+		CalculateA1AndTheta();
+		CalculateRotationLimitsConstraintProperties(inDeltaTime);
+		if (mRotationLimitsConstraintPart.IsActive())
+			limit = mRotationLimitsConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, GetSmallestAngleToLimit(), inBaumgarte);
+	}
 
 
 	return pos || rot || limit;
 	return pos || rot || limit;
 }
 }
@@ -365,6 +387,7 @@ Ref<ConstraintSettings> HingeConstraint::GetConstraintSettings() const
 	settings->mNormalAxis2 = mLocalSpaceNormalAxis2;
 	settings->mNormalAxis2 = mLocalSpaceNormalAxis2;
 	settings->mLimitsMin = mLimitsMin;
 	settings->mLimitsMin = mLimitsMin;
 	settings->mLimitsMax = mLimitsMax;
 	settings->mLimitsMax = mLimitsMax;
+	settings->mLimitsSpringSettings = mLimitsSpringSettings;
 	settings->mMaxFrictionTorque = mMaxFrictionTorque;
 	settings->mMaxFrictionTorque = mMaxFrictionTorque;
 	settings->mMotorSettings = mMotorSettings;
 	settings->mMotorSettings = mMotorSettings;
 	return settings;
 	return settings;

+ 11 - 0
Jolt/Physics/Constraints/HingeConstraint.h

@@ -43,6 +43,9 @@ public:
 	float						mLimitsMin = -JPH_PI;
 	float						mLimitsMin = -JPH_PI;
 	float						mLimitsMax = JPH_PI;
 	float						mLimitsMax = JPH_PI;
 
 
+	/// When enabled, this makes the limits soft. When the constraint exceeds the limits, a spring force will pull it back.
+	SpringSettings				mLimitsSpringSettings;
+
 	/// Maximum amount of torque (N m) to apply as friction when the constraint is not powered by a motor
 	/// Maximum amount of torque (N m) to apply as friction when the constraint is not powered by a motor
 	float						mMaxFrictionTorque = 0.0f;
 	float						mMaxFrictionTorque = 0.0f;
 
 
@@ -107,6 +110,11 @@ public:
 	float						GetLimitsMax() const									{ return mLimitsMax; }
 	float						GetLimitsMax() const									{ return mLimitsMax; }
 	bool						HasLimits() const										{ return mHasLimits; }
 	bool						HasLimits() const										{ return mHasLimits; }
 
 
+	/// Update the limits spring settings
+	const SpringSettings &		GetLimitsSpringSettings() const							{ return mLimitsSpringSettings; }
+	SpringSettings &			GetLimitsSpringSettings()								{ return mLimitsSpringSettings; }
+	void						SetLimitsSpringSettings(const SpringSettings &inLimitsSpringSettings) { mLimitsSpringSettings = inLimitsSpringSettings; }
+
 	///@name Get Lagrange multiplier from last physics update (relates to how much force/torque was applied to satisfy the constraint)
 	///@name Get Lagrange multiplier from last physics update (relates to how much force/torque was applied to satisfy the constraint)
 	inline Vec3		 			GetTotalLambdaPosition() const							{ return mPointConstraintPart.GetTotalLambda(); }
 	inline Vec3		 			GetTotalLambdaPosition() const							{ return mPointConstraintPart.GetTotalLambda(); }
 	inline Vector<2>			GetTotalLambdaRotation() const							{ return mRotationConstraintPart.GetTotalLambda(); }
 	inline Vector<2>			GetTotalLambdaRotation() const							{ return mRotationConstraintPart.GetTotalLambda(); }
@@ -142,6 +150,9 @@ private:
 	float						mLimitsMin;
 	float						mLimitsMin;
 	float						mLimitsMax;
 	float						mLimitsMax;
 
 
+	// Soft constraint limits
+	SpringSettings				mLimitsSpringSettings;
+
 	// Friction
 	// Friction
 	float						mMaxFrictionTorque;
 	float						mMaxFrictionTorque;
 
 

+ 5 - 6
Jolt/Physics/Constraints/MotorSettings.cpp

@@ -13,8 +13,9 @@ JPH_NAMESPACE_BEGIN
 
 
 JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(MotorSettings)
 JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(MotorSettings)
 {
 {
-	JPH_ADD_ATTRIBUTE(MotorSettings, mFrequency)
-	JPH_ADD_ATTRIBUTE(MotorSettings, mDamping)
+	JPH_ADD_ENUM_ATTRIBUTE_WITH_ALIAS(MotorSettings, mSpringSettings.mMode, "mSpringMode")
+	JPH_ADD_ATTRIBUTE_WITH_ALIAS(MotorSettings, mSpringSettings.mFrequency, "mFrequency") // Renaming attributes to stay compatible with old versions of the library
+	JPH_ADD_ATTRIBUTE_WITH_ALIAS(MotorSettings, mSpringSettings.mDamping, "mDamping")
 	JPH_ADD_ATTRIBUTE(MotorSettings, mMinForceLimit)
 	JPH_ADD_ATTRIBUTE(MotorSettings, mMinForceLimit)
 	JPH_ADD_ATTRIBUTE(MotorSettings, mMaxForceLimit)
 	JPH_ADD_ATTRIBUTE(MotorSettings, mMaxForceLimit)
 	JPH_ADD_ATTRIBUTE(MotorSettings, mMinTorqueLimit)
 	JPH_ADD_ATTRIBUTE(MotorSettings, mMinTorqueLimit)
@@ -23,8 +24,7 @@ JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(MotorSettings)
 
 
 void MotorSettings::SaveBinaryState(StreamOut &inStream) const
 void MotorSettings::SaveBinaryState(StreamOut &inStream) const
 {
 {
-	inStream.Write(mFrequency);
-	inStream.Write(mDamping);
+	mSpringSettings.SaveBinaryState(inStream);
 	inStream.Write(mMinForceLimit);
 	inStream.Write(mMinForceLimit);
 	inStream.Write(mMaxForceLimit);
 	inStream.Write(mMaxForceLimit);
 	inStream.Write(mMinTorqueLimit);
 	inStream.Write(mMinTorqueLimit);
@@ -33,8 +33,7 @@ void MotorSettings::SaveBinaryState(StreamOut &inStream) const
 
 
 void MotorSettings::RestoreBinaryState(StreamIn &inStream)
 void MotorSettings::RestoreBinaryState(StreamIn &inStream)
 {
 {
-	inStream.Read(mFrequency);
-	inStream.Read(mDamping);
+	mSpringSettings.RestoreBinaryState(inStream);
 	inStream.Read(mMinForceLimit);
 	inStream.Read(mMinForceLimit);
 	inStream.Read(mMaxForceLimit);
 	inStream.Read(mMaxForceLimit);
 	inStream.Read(mMinTorqueLimit);
 	inStream.Read(mMinTorqueLimit);

+ 6 - 6
Jolt/Physics/Constraints/MotorSettings.h

@@ -6,6 +6,7 @@
 
 
 #include <Jolt/Core/Reference.h>
 #include <Jolt/Core/Reference.h>
 #include <Jolt/ObjectStream/SerializableObject.h>
 #include <Jolt/ObjectStream/SerializableObject.h>
+#include <Jolt/Physics/Constraints/SpringSettings.h>
 
 
 JPH_NAMESPACE_BEGIN
 JPH_NAMESPACE_BEGIN
 
 
@@ -28,9 +29,9 @@ public:
 
 
 	/// Constructor
 	/// Constructor
 							MotorSettings() = default;
 							MotorSettings() = default;
-							MotorSettings(const MotorSettings &inRHS) = default;
-							MotorSettings(float inFrequency, float inDamping) : mFrequency(inFrequency), mDamping(inDamping) { JPH_ASSERT(IsValid()); }
-							MotorSettings(float inFrequency, float inDamping, float inForceLimit, float inTorqueLimit) : mFrequency(inFrequency), mDamping(inDamping), mMinForceLimit(-inForceLimit), mMaxForceLimit(inForceLimit), mMinTorqueLimit(-inTorqueLimit), mMaxTorqueLimit(inTorqueLimit) { JPH_ASSERT(IsValid()); }
+							MotorSettings(const MotorSettings &) = default;
+							MotorSettings(float inFrequency, float inDamping) : mSpringSettings(ESpringMode::FrequencyAndDamping, inFrequency, inDamping) { JPH_ASSERT(IsValid()); }
+							MotorSettings(float inFrequency, float inDamping, float inForceLimit, float inTorqueLimit) : mSpringSettings(ESpringMode::FrequencyAndDamping, inFrequency, inDamping), mMinForceLimit(-inForceLimit), mMaxForceLimit(inForceLimit), mMinTorqueLimit(-inTorqueLimit), mMaxTorqueLimit(inTorqueLimit) { JPH_ASSERT(IsValid()); }
 
 
 	/// Set asymmetric force limits
 	/// Set asymmetric force limits
 	void					SetForceLimits(float inMin, float inMax)	{ JPH_ASSERT(inMin <= inMax); mMinForceLimit = inMin; mMaxForceLimit = inMax; }
 	void					SetForceLimits(float inMin, float inMax)	{ JPH_ASSERT(inMin <= inMax); mMinForceLimit = inMin; mMaxForceLimit = inMax; }
@@ -45,7 +46,7 @@ public:
 	void					SetTorqueLimit(float inLimit)				{ mMinTorqueLimit = -inLimit; mMaxTorqueLimit = inLimit; }
 	void					SetTorqueLimit(float inLimit)				{ mMinTorqueLimit = -inLimit; mMaxTorqueLimit = inLimit; }
 
 
 	/// Check if settings are valid
 	/// Check if settings are valid
-	bool					IsValid() const								{ return mFrequency >= 0.0f && mDamping >= 0.0f && mMinForceLimit <= mMaxForceLimit && mMinTorqueLimit <= mMaxTorqueLimit; }
+	bool					IsValid() const								{ return mSpringSettings.mFrequency >= 0.0f && mSpringSettings.mDamping >= 0.0f && mMinForceLimit <= mMaxForceLimit && mMinTorqueLimit <= mMaxTorqueLimit; }
 
 
 	/// Saves the contents of the motor settings in binary form to inStream.
 	/// Saves the contents of the motor settings in binary form to inStream.
 	void					SaveBinaryState(StreamOut &inStream) const;
 	void					SaveBinaryState(StreamOut &inStream) const;
@@ -54,8 +55,7 @@ public:
 	void					RestoreBinaryState(StreamIn &inStream);
 	void					RestoreBinaryState(StreamIn &inStream);
 
 
 	// Settings
 	// Settings
-	float					mFrequency = 2.0f;							///< Oscillation frequency when solving position target (Hz). Should be in the range (0, 0.5 * simulation frequency]. When simulating at 60 Hz, 20 is a good value for a strong motor. Only used for position motors.
-	float					mDamping = 1.0f;							///< Damping when solving position target (0 = minimal damping, 1 = critical damping). Only used for position motors.
+	SpringSettings			mSpringSettings { ESpringMode::FrequencyAndDamping, 2.0f, 1.0f }; ///< Settings for the spring that is used to drive to the position target (not used when motor is a velocity motor).
 	float					mMinForceLimit = -FLT_MAX;					///< Minimum force to apply in case of a linear constraint (N). Usually this is -mMaxForceLimit unless you want a motor that can e.g. push but not pull. Not used when motor is an angular motor.
 	float					mMinForceLimit = -FLT_MAX;					///< Minimum force to apply in case of a linear constraint (N). Usually this is -mMaxForceLimit unless you want a motor that can e.g. push but not pull. Not used when motor is an angular motor.
 	float					mMaxForceLimit = FLT_MAX;					///< Maximum force to apply in case of a linear constraint (N). Not used when motor is an angular motor.
 	float					mMaxForceLimit = FLT_MAX;					///< Maximum force to apply in case of a linear constraint (N). Not used when motor is an angular motor.
 	float					mMinTorqueLimit = -FLT_MAX;					///< Minimum torque to apply in case of a angular constraint (N m). Usually this is -mMaxTorqueLimit unless you want a motor that can e.g. push but not pull. Not used when motor is a position motor.
 	float					mMinTorqueLimit = -FLT_MAX;					///< Minimum torque to apply in case of a angular constraint (N m). Usually this is -mMaxTorqueLimit unless you want a motor that can e.g. push but not pull. Not used when motor is a position motor.

+ 4 - 4
Jolt/Physics/Constraints/PathConstraint.cpp

@@ -145,7 +145,7 @@ void PathConstraint::CalculateConstraintProperties(float inDeltaTime)
 
 
 	// Check if closest point is on the boundary of the path and if so apply limit
 	// Check if closest point is on the boundary of the path and if so apply limit
 	if (!mPath->IsLooping() && (mPathFraction <= 0.0f || mPathFraction >= mPath->GetPathMaxFraction()))
 	if (!mPath->IsLooping() && (mPathFraction <= 0.0f || mPathFraction >= mPath->GetPathMaxFraction()))
-		mPositionLimitsConstraintPart.CalculateConstraintProperties(inDeltaTime, *mBody1, mR1 + mU, *mBody2, mR2, mPathTangent);
+		mPositionLimitsConstraintPart.CalculateConstraintProperties(*mBody1, mR1 + mU, *mBody2, mR2, mPathTangent);
 	else
 	else
 		mPositionLimitsConstraintPart.Deactivate();
 		mPositionLimitsConstraintPart.Deactivate();
 
 
@@ -186,13 +186,13 @@ void PathConstraint::CalculateConstraintProperties(float inDeltaTime)
 	{
 	{
 	case EMotorState::Off:
 	case EMotorState::Off:
 		if (mMaxFrictionForce > 0.0f)
 		if (mMaxFrictionForce > 0.0f)
-			mPositionMotorConstraintPart.CalculateConstraintProperties(inDeltaTime, *mBody1, mR1 + mU, *mBody2, mR2, mPathTangent);
+			mPositionMotorConstraintPart.CalculateConstraintProperties(*mBody1, mR1 + mU, *mBody2, mR2, mPathTangent);
 		else
 		else
 			mPositionMotorConstraintPart.Deactivate();
 			mPositionMotorConstraintPart.Deactivate();
 		break;
 		break;
 
 
 	case EMotorState::Velocity:
 	case EMotorState::Velocity:
-		mPositionMotorConstraintPart.CalculateConstraintProperties(inDeltaTime, *mBody1, mR1 + mU, *mBody2, mR2, mPathTangent, -mTargetVelocity);
+		mPositionMotorConstraintPart.CalculateConstraintProperties(*mBody1, mR1 + mU, *mBody2, mR2, mPathTangent, -mTargetVelocity);
 		break;
 		break;
 
 
 	case EMotorState::Position:
 	case EMotorState::Position:
@@ -211,7 +211,7 @@ void PathConstraint::CalculateConstraintProperties(float inDeltaTime)
 			}
 			}
 			else
 			else
 				c = mPathFraction - mTargetPathFraction;
 				c = mPathFraction - mTargetPathFraction;
-			mPositionMotorConstraintPart.CalculateConstraintProperties(inDeltaTime, *mBody1, mR1 + mU, *mBody2, mR2, mPathTangent, 0.0f, c, mPositionMotorSettings.mFrequency, mPositionMotorSettings.mDamping);
+			mPositionMotorConstraintPart.CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, mR1 + mU, *mBody2, mR2, mPathTangent, 0.0f, c, mPositionMotorSettings.mSpringSettings);
 			break;
 			break;
 		}
 		}
 	}	
 	}	

+ 74 - 46
Jolt/Physics/Constraints/SixDOFConstraint.cpp

@@ -30,6 +30,7 @@ JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(SixDOFConstraintSettings)
 	JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mMaxFriction)
 	JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mMaxFriction)
 	JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mLimitMin)
 	JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mLimitMin)
 	JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mLimitMax)
 	JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mLimitMax)
+	JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mLimitsSpringSettings)
 	JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mMotorSettings)
 	JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mMotorSettings)
 }
 }
 
 
@@ -47,6 +48,8 @@ void SixDOFConstraintSettings::SaveBinaryState(StreamOut &inStream) const
 	inStream.Write(mMaxFriction);
 	inStream.Write(mMaxFriction);
 	inStream.Write(mLimitMin);
 	inStream.Write(mLimitMin);
 	inStream.Write(mLimitMax);
 	inStream.Write(mLimitMax);
+	for (const SpringSettings &s : mLimitsSpringSettings)
+		s.SaveBinaryState(inStream);
 	for (const MotorSettings &m : mMotorSettings)
 	for (const MotorSettings &m : mMotorSettings)
 		m.SaveBinaryState(inStream);
 		m.SaveBinaryState(inStream);
 }
 }
@@ -65,6 +68,8 @@ void SixDOFConstraintSettings::RestoreBinaryState(StreamIn &inStream)
 	inStream.Read(mMaxFriction);
 	inStream.Read(mMaxFriction);
 	inStream.Read(mLimitMin);
 	inStream.Read(mLimitMin);
 	inStream.Read(mLimitMax);
 	inStream.Read(mLimitMax);
+	for (SpringSettings &s : mLimitsSpringSettings)
+		s.RestoreBinaryState(inStream);
 	for (MotorSettings &m : mMotorSettings)
 	for (MotorSettings &m : mMotorSettings)
 		m.RestoreBinaryState(inStream);
 		m.RestoreBinaryState(inStream);
 }
 }
@@ -139,9 +144,11 @@ SixDOFConstraint::SixDOFConstraint(Body &inBody1, Body &inBody2, const SixDOFCon
 	}
 	}
 
 
 	// Copy translation and rotation limits
 	// Copy translation and rotation limits
-	memcpy(mLimitMin, inSettings.mLimitMin, sizeof(mLimitMin)); 
-	memcpy(mLimitMax, inSettings.mLimitMax, sizeof(mLimitMax)); 
+	memcpy(mLimitMin, inSettings.mLimitMin, sizeof(mLimitMin));
+	memcpy(mLimitMax, inSettings.mLimitMax, sizeof(mLimitMax));
+	memcpy(mLimitsSpringSettings, inSettings.mLimitsSpringSettings, sizeof(mLimitsSpringSettings));
 	UpdateRotationLimits();
 	UpdateRotationLimits();
+	CacheHasSpringLimits();
 
 
 	// Store friction settings
 	// Store friction settings
 	memcpy(mMaxFriction, inSettings.mMaxFriction, sizeof(mMaxFriction));
 	memcpy(mMaxFriction, inSettings.mMaxFriction, sizeof(mMaxFriction));
@@ -240,6 +247,13 @@ void SixDOFConstraint::CacheRotationMotorActive()
 		|| HasFriction(EAxis::RotationZ);
 		|| HasFriction(EAxis::RotationZ);
 }
 }
 
 
+void SixDOFConstraint::CacheHasSpringLimits()
+{
+	mHasSpringLimits = mLimitsSpringSettings[EAxis::TranslationX].mFrequency > 0.0f
+		|| mLimitsSpringSettings[EAxis::TranslationY].mFrequency > 0.0f
+		|| mLimitsSpringSettings[EAxis::TranslationZ].mFrequency > 0.0f;
+}
+
 void SixDOFConstraint::SetMotorState(EAxis inAxis, EMotorState inState)
 void SixDOFConstraint::SetMotorState(EAxis inAxis, EMotorState inState)
 {
 {
 	JPH_ASSERT(inState == EMotorState::Off || mMotorSettings[inAxis].IsValid());
 	JPH_ASSERT(inState == EMotorState::Off || mMotorSettings[inAxis].IsValid());
@@ -317,23 +331,36 @@ void SixDOFConstraint::SetupVelocityConstraint(float inDeltaTime)
 
 
 			Vec3 translation_axis = mTranslationAxis[i];
 			Vec3 translation_axis = mTranslationAxis[i];
 
 
+			// Calculate displacement along this axis
+			float d = translation_axis.Dot(u);
+			mDisplacement[i] = d; // Store for SolveVelocityConstraint
+
 			// Setup limit constraint
 			// Setup limit constraint
 			bool constraint_active = false;
 			bool constraint_active = false;
+			float constraint_value = 0.0f;
 			if (IsFixedAxis(axis))
 			if (IsFixedAxis(axis))
 			{
 			{
 				// When constraint is fixed it is always active
 				// When constraint is fixed it is always active
+				constraint_value = d;
 				constraint_active = true;
 				constraint_active = true;
 			}
 			}
 			else if (!IsFreeAxis(axis))
 			else if (!IsFreeAxis(axis))
 			{
 			{
 				// When constraint is limited, it is only active when outside of the allowed range
 				// When constraint is limited, it is only active when outside of the allowed range
-				float d = translation_axis.Dot(u);
-				constraint_active = d <= mLimitMin[i] || d >= mLimitMax[i];
-				mDisplacement[i] = d; // Store for SolveVelocityConstraint
+				if (d <= mLimitMin[i])
+				{
+					constraint_value = d - mLimitMin[i];
+					constraint_active = true;
+				}
+				else if (d >= mLimitMax[i])
+				{
+					constraint_value = d - mLimitMax[i];
+					constraint_active = true;
+				}
 			}
 			}
 
 
 			if (constraint_active)
 			if (constraint_active)
-				mTranslationConstraintPart[i].CalculateConstraintProperties(inDeltaTime, *mBody1, r1_plus_u, *mBody2, r2, translation_axis);
+				mTranslationConstraintPart[i].CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, r1_plus_u, *mBody2, r2, translation_axis, 0.0f, constraint_value, mLimitsSpringSettings[i]);
 			else
 			else
 				mTranslationConstraintPart[i].Deactivate();
 				mTranslationConstraintPart[i].Deactivate();
 
 
@@ -342,17 +369,17 @@ void SixDOFConstraint::SetupVelocityConstraint(float inDeltaTime)
 			{
 			{
 			case EMotorState::Off:
 			case EMotorState::Off:
 				if (HasFriction(axis))
 				if (HasFriction(axis))
-					mMotorTranslationConstraintPart[i].CalculateConstraintProperties(inDeltaTime, *mBody1, r1_plus_u, *mBody2, r2, translation_axis);
+					mMotorTranslationConstraintPart[i].CalculateConstraintProperties(*mBody1, r1_plus_u, *mBody2, r2, translation_axis);
 				else
 				else
 					mMotorTranslationConstraintPart[i].Deactivate();
 					mMotorTranslationConstraintPart[i].Deactivate();
 				break;
 				break;
 
 
 			case EMotorState::Velocity:
 			case EMotorState::Velocity:
-				mMotorTranslationConstraintPart[i].CalculateConstraintProperties(inDeltaTime, *mBody1, r1_plus_u, *mBody2, r2, translation_axis, -mTargetVelocity[i]);
+				mMotorTranslationConstraintPart[i].CalculateConstraintProperties(*mBody1, r1_plus_u, *mBody2, r2, translation_axis, -mTargetVelocity[i]);
 				break;
 				break;
 
 
 			case EMotorState::Position:
 			case EMotorState::Position:
-				mMotorTranslationConstraintPart[i].CalculateConstraintProperties(inDeltaTime, *mBody1, r1_plus_u, *mBody2, r2, translation_axis, 0.0f, translation_axis.Dot(u) - mTargetPosition[i], mMotorSettings[i].mFrequency, mMotorSettings[i].mDamping);
+				mMotorTranslationConstraintPart[i].CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, r1_plus_u, *mBody2, r2, translation_axis, 0.0f, translation_axis.Dot(u) - mTargetPosition[i], mMotorSettings[i].mSpringSettings);
 				break;
 				break;
 			}
 			}
 		}
 		}
@@ -372,7 +399,7 @@ void SixDOFConstraint::SetupVelocityConstraint(float inDeltaTime)
 
 
 		// Use swing twist constraint part
 		// Use swing twist constraint part
 		if (IsRotationConstrained())
 		if (IsRotationConstrained())
-			mSwingTwistConstraintPart.CalculateConstraintProperties(inDeltaTime, *mBody1, *mBody2, q, constraint_body1_to_world);
+			mSwingTwistConstraintPart.CalculateConstraintProperties(*mBody1, *mBody2, q, constraint_body1_to_world);
 		else
 		else
 			mSwingTwistConstraintPart.Deactivate();
 			mSwingTwistConstraintPart.Deactivate();
 
 
@@ -467,17 +494,17 @@ void SixDOFConstraint::SetupVelocityConstraint(float inDeltaTime)
 				{
 				{
 				case EMotorState::Off:
 				case EMotorState::Off:
 					if (HasFriction(axis))
 					if (HasFriction(axis))
-						mMotorRotationConstraintPart[i].CalculateConstraintProperties(inDeltaTime, *mBody1, *mBody2, rotation_axis);
+						mMotorRotationConstraintPart[i].CalculateConstraintProperties(*mBody1, *mBody2, rotation_axis);
 					else
 					else
 						mMotorRotationConstraintPart[i].Deactivate();
 						mMotorRotationConstraintPart[i].Deactivate();
 					break;
 					break;
 
 
 				case EMotorState::Velocity:
 				case EMotorState::Velocity:
-					mMotorRotationConstraintPart[i].CalculateConstraintProperties(inDeltaTime, *mBody1, *mBody2, rotation_axis, -mTargetAngularVelocity[i]);
+					mMotorRotationConstraintPart[i].CalculateConstraintProperties(*mBody1, *mBody2, rotation_axis, -mTargetAngularVelocity[i]);
 					break;
 					break;
 
 
 				case EMotorState::Position:
 				case EMotorState::Position:
-					mMotorRotationConstraintPart[i].CalculateConstraintProperties(inDeltaTime, *mBody1, *mBody2, rotation_axis, 0.0f, rotation_error[i], mMotorSettings[axis].mFrequency, mMotorSettings[axis].mDamping);
+					mMotorRotationConstraintPart[i].CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, *mBody2, rotation_axis, 0.0f, rotation_error[i], mMotorSettings[axis].mSpringSettings);
 					break;
 					break;
 				}	
 				}	
 			}
 			}
@@ -632,44 +659,45 @@ bool SixDOFConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumga
 	{
 	{
 		// Translation partially locked: Solve per axis
 		// Translation partially locked: Solve per axis
 		for (int i = 0; i < 3; ++i)
 		for (int i = 0; i < 3; ++i)
-		{
-			// Update world space positions (the bodies may have moved)
-			Vec3 r1_plus_u, r2, u;
-			GetPositionConstraintProperties(r1_plus_u, r2, u);
+			if (mLimitsSpringSettings[i].mFrequency <= 0.0f) // If not soft limit
+			{
+				// Update world space positions (the bodies may have moved)
+				Vec3 r1_plus_u, r2, u;
+				GetPositionConstraintProperties(r1_plus_u, r2, u);
 
 
-			// Quaternion that rotates from body1's constraint space to world space
-			Quat constraint_body1_to_world = mBody1->GetRotation() * mConstraintToBody1;
+				// Quaternion that rotates from body1's constraint space to world space
+				Quat constraint_body1_to_world = mBody1->GetRotation() * mConstraintToBody1;
 
 
-			// Calculate axis
-			Vec3 translation_axis;
-			switch (i)
-			{
-			case 0:							translation_axis = constraint_body1_to_world.RotateAxisX(); break;
-			case 1:							translation_axis = constraint_body1_to_world.RotateAxisY(); break;
-			default:	JPH_ASSERT(i == 2); translation_axis = constraint_body1_to_world.RotateAxisZ(); break;
-			}
+				// Calculate axis
+				Vec3 translation_axis;
+				switch (i)
+				{
+				case 0:							translation_axis = constraint_body1_to_world.RotateAxisX(); break;
+				case 1:							translation_axis = constraint_body1_to_world.RotateAxisY(); break;
+				default:	JPH_ASSERT(i == 2); translation_axis = constraint_body1_to_world.RotateAxisZ(); break;
+				}
 
 
-			// Determine position error
-			float error = 0.0f;
-			EAxis axis(EAxis(EAxis::TranslationX + i));
-			if (IsFixedAxis(axis))
-				error = u.Dot(translation_axis);
-			else if (!IsFreeAxis(axis))
-			{
-				float displacement = u.Dot(translation_axis);
-				if (displacement <= mLimitMin[axis])
-					error = displacement - mLimitMin[axis];
-				else if (displacement >= mLimitMax[axis])
-					error = displacement - mLimitMax[axis];
-			}
+				// Determine position error
+				float error = 0.0f;
+				EAxis axis(EAxis(EAxis::TranslationX + i));
+				if (IsFixedAxis(axis))
+					error = u.Dot(translation_axis);
+				else if (!IsFreeAxis(axis))
+				{
+					float displacement = u.Dot(translation_axis);
+					if (displacement <= mLimitMin[axis])
+						error = displacement - mLimitMin[axis];
+					else if (displacement >= mLimitMax[axis])
+						error = displacement - mLimitMax[axis];
+				}
 
 
-			if (error != 0.0f)
-			{
-				// Setup axis constraint part and solve it
-				mTranslationConstraintPart[i].CalculateConstraintProperties(inDeltaTime, *mBody1, r1_plus_u, *mBody2, r2, translation_axis);
-				impulse |= mTranslationConstraintPart[i].SolvePositionConstraint(*mBody1, *mBody2, translation_axis, error, inBaumgarte);
+				if (error != 0.0f)
+				{
+					// Setup axis constraint part and solve it
+					mTranslationConstraintPart[i].CalculateConstraintProperties(*mBody1, r1_plus_u, *mBody2, r2, translation_axis);
+					impulse |= mTranslationConstraintPart[i].SolvePositionConstraint(*mBody1, *mBody2, translation_axis, error, inBaumgarte);
+				}
 			}
 			}
-		}
 	}
 	}
 
 
 	return impulse;
 	return impulse;

+ 15 - 1
Jolt/Physics/Constraints/SixDOFConstraint.h

@@ -32,6 +32,7 @@ public:
 		RotationZ,				///< When limited: MaxLimit between [0, PI]. MinLimit = -MaxLimit. Forms a cone shaped limit with Y.
 		RotationZ,				///< When limited: MaxLimit between [0, PI]. MinLimit = -MaxLimit. Forms a cone shaped limit with Y.
 
 
 		Num,
 		Num,
+		NumTranslation = TranslationZ + 1,
 	};
 	};
 
 
 	// See: ConstraintSettings::SaveBinaryState
 	// See: ConstraintSettings::SaveBinaryState
@@ -68,6 +69,10 @@ public:
 	float						mLimitMin[EAxis::Num] = { -FLT_MAX, -FLT_MAX, -FLT_MAX, -FLT_MAX, -FLT_MAX, -FLT_MAX };
 	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 };
 	float						mLimitMax[EAxis::Num] = { FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX };
 
 
+	/// When enabled, this makes the limits soft. When the constraint exceeds the limits, a spring force will pull it back.
+	/// Only soft translation limits are supported, soft rotation limits are not currently supported.
+	SpringSettings				mLimitsSpringSettings[EAxis::NumTranslation];
+
 	/// Make axis free (unconstrained)
 	/// Make axis free (unconstrained)
 	void						MakeFreeAxis(EAxis inAxis)									{ mLimitMin[inAxis] = -FLT_MAX; mLimitMax[inAxis] = FLT_MAX; }
 	void						MakeFreeAxis(EAxis inAxis)									{ mLimitMin[inAxis] = -FLT_MAX; mLimitMax[inAxis] = FLT_MAX; }
 	bool						IsFreeAxis(EAxis inAxis) const								{ return mLimitMin[inAxis] == -FLT_MAX && mLimitMax[inAxis] == FLT_MAX; }
 	bool						IsFreeAxis(EAxis inAxis) const								{ return mLimitMin[inAxis] == -FLT_MAX && mLimitMax[inAxis] == FLT_MAX; }
@@ -131,6 +136,10 @@ public:
 	inline bool					IsFixedAxis(EAxis inAxis) const								{ return (mFixedAxis & (1 << inAxis)) != 0; }
 	inline bool					IsFixedAxis(EAxis inAxis) const								{ return (mFixedAxis & (1 << inAxis)) != 0; }
 	inline bool					IsFreeAxis(EAxis inAxis) const								{ return (mFreeAxis & (1 << inAxis)) != 0; }
 	inline bool					IsFreeAxis(EAxis inAxis) const								{ return (mFreeAxis & (1 << inAxis)) != 0; }
 
 
+	/// Update the limits spring settings
+	const SpringSettings &		GetLimitsSpringSettings(EAxis inAxis) const					{ JPH_ASSERT(inAxis < EAxis::NumTranslation); return mLimitsSpringSettings[inAxis]; }
+	void						SetLimitsSpringSettings(EAxis inAxis, const SpringSettings& inLimitsSpringSettings) { JPH_ASSERT(inAxis < EAxis::NumTranslation); mLimitsSpringSettings[inAxis] = inLimitsSpringSettings; CacheHasSpringLimits(); }
+
 	/// Set the max friction for each axis
 	/// Set the max friction for each axis
 	void						SetMaxFriction(EAxis inAxis, float inFriction);
 	void						SetMaxFriction(EAxis inAxis, float inFriction);
 	float						GetMaxFriction(EAxis inAxis) const							{ return mMaxFriction[inAxis]; }
 	float						GetMaxFriction(EAxis inAxis) const							{ return mMaxFriction[inAxis]; }
@@ -187,9 +196,12 @@ private:
 	// Cache the state of mRotationMotorActive
 	// Cache the state of mRotationMotorActive
 	void						CacheRotationMotorActive();
 	void						CacheRotationMotorActive();
 
 
+	/// Cache the state of mHasSpringLimits
+	void						CacheHasSpringLimits();
+
 	// Constraint settings helper functions
 	// Constraint settings helper functions
 	inline bool					IsTranslationConstrained() const							{ return (mFreeAxis & 0b111) != 0b111; }
 	inline bool					IsTranslationConstrained() const							{ return (mFreeAxis & 0b111) != 0b111; }
-	inline bool					IsTranslationFullyConstrained() const						{ return (mFixedAxis & 0b111) == 0b111; }
+	inline bool					IsTranslationFullyConstrained() const						{ return (mFixedAxis & 0b111) == 0b111 && !mHasSpringLimits; }
 	inline bool					IsRotationConstrained() const								{ return (mFreeAxis & 0b111000) != 0b111000; }
 	inline bool					IsRotationConstrained() const								{ return (mFreeAxis & 0b111000) != 0b111000; }
 	inline bool					IsRotationFullyConstrained() const							{ return (mFixedAxis & 0b111000) == 0b111000; }
 	inline bool					IsRotationFullyConstrained() const							{ return (mFixedAxis & 0b111000) == 0b111000; }
 	inline bool					HasFriction(EAxis inAxis) const								{ return !IsFixedAxis(inAxis) && mMaxFriction[inAxis] > 0.0f; }
 	inline bool					HasFriction(EAxis inAxis) const								{ return !IsFixedAxis(inAxis) && mMaxFriction[inAxis] > 0.0f; }
@@ -210,8 +222,10 @@ private:
 	bool						mTranslationMotorActive = false;							// If any of the translational frictions / motors are active
 	bool						mTranslationMotorActive = false;							// If any of the translational frictions / motors are active
 	bool						mRotationMotorActive = false;								// If any of the rotational frictions / motors are active
 	bool						mRotationMotorActive = false;								// If any of the rotational frictions / motors are active
 	uint8						mRotationPositionMotorActive = 0;							// Bitmask of axis that have position motor active (bit 0 = RotationX)
 	uint8						mRotationPositionMotorActive = 0;							// Bitmask of axis that have position motor active (bit 0 = RotationX)
+	bool						mHasSpringLimits = false;									// If any of the limit springs have a non-zero frequency/stiffness
 	float						mLimitMin[EAxis::Num];
 	float						mLimitMin[EAxis::Num];
 	float						mLimitMax[EAxis::Num];
 	float						mLimitMax[EAxis::Num];
+	SpringSettings				mLimitsSpringSettings[EAxis::NumTranslation];
 
 
 	// Motor settings for each axis
 	// Motor settings for each axis
 	MotorSettings				mMotorSettings[EAxis::Num];
 	MotorSettings				mMotorSettings[EAxis::Num];

+ 28 - 21
Jolt/Physics/Constraints/SliderConstraint.cpp

@@ -29,8 +29,9 @@ JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(SliderConstraintSettings)
 	JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mNormalAxis2)
 	JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mNormalAxis2)
 	JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mLimitsMin)
 	JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mLimitsMin)
 	JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mLimitsMax)
 	JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mLimitsMax)
-	JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mFrequency)
-	JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mDamping)
+	JPH_ADD_ENUM_ATTRIBUTE_WITH_ALIAS(SliderConstraintSettings, mLimitsSpringSettings.mMode, "mSpringMode")
+	JPH_ADD_ATTRIBUTE_WITH_ALIAS(SliderConstraintSettings, mLimitsSpringSettings.mFrequency, "mFrequency") // Renaming attributes to stay compatible with old versions of the library
+	JPH_ADD_ATTRIBUTE_WITH_ALIAS(SliderConstraintSettings, mLimitsSpringSettings.mDamping, "mDamping")
 	JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mMaxFrictionForce)
 	JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mMaxFrictionForce)
 	JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mMotorSettings)
 	JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mMotorSettings)
 }
 }
@@ -57,9 +58,8 @@ void SliderConstraintSettings::SaveBinaryState(StreamOut &inStream) const
 	inStream.Write(mNormalAxis2);
 	inStream.Write(mNormalAxis2);
 	inStream.Write(mLimitsMin);
 	inStream.Write(mLimitsMin);
 	inStream.Write(mLimitsMax);
 	inStream.Write(mLimitsMax);
-	inStream.Write(mFrequency);
-	inStream.Write(mDamping);
 	inStream.Write(mMaxFrictionForce);
 	inStream.Write(mMaxFrictionForce);
+	mLimitsSpringSettings.SaveBinaryState(inStream);
 	mMotorSettings.SaveBinaryState(inStream);
 	mMotorSettings.SaveBinaryState(inStream);
 }
 }
 
 
@@ -77,9 +77,8 @@ void SliderConstraintSettings::RestoreBinaryState(StreamIn &inStream)
 	inStream.Read(mNormalAxis2);
 	inStream.Read(mNormalAxis2);
 	inStream.Read(mLimitsMin);
 	inStream.Read(mLimitsMin);
 	inStream.Read(mLimitsMax);
 	inStream.Read(mLimitsMax);
-	inStream.Read(mFrequency);
-	inStream.Read(mDamping);
 	inStream.Read(mMaxFrictionForce);
 	inStream.Read(mMaxFrictionForce);
+	mLimitsSpringSettings.RestoreBinaryState(inStream);
 	mMotorSettings.RestoreBinaryState(inStream);
 	mMotorSettings.RestoreBinaryState(inStream);
 }
 }
 
 
@@ -151,12 +150,11 @@ SliderConstraint::SliderConstraint(Body &inBody1, Body &inBody2, const SliderCon
 	mLocalSpaceNormal2 = mLocalSpaceSliderAxis1.Cross(mLocalSpaceNormal1);
 	mLocalSpaceNormal2 = mLocalSpaceSliderAxis1.Cross(mLocalSpaceNormal1);
 
 
 	// Store limits
 	// Store limits
-	JPH_ASSERT(inSettings.mLimitsMin != inSettings.mLimitsMax || inSettings.mFrequency > 0.0f, "Better use a fixed constraint");
+	JPH_ASSERT(inSettings.mLimitsMin != inSettings.mLimitsMax || inSettings.mLimitsSpringSettings.mFrequency > 0.0f, "Better use a fixed constraint");
 	SetLimits(inSettings.mLimitsMin, inSettings.mLimitsMax);
 	SetLimits(inSettings.mLimitsMin, inSettings.mLimitsMax);
 
 
-	// Store frequency and damping
-	SetFrequency(inSettings.mFrequency);
-	SetDamping(inSettings.mDamping);
+	// Store spring settings
+	SetLimitsSpringSettings(inSettings.mLimitsSpringSettings);
 }
 }
 
 
 void SliderConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM)
 void SliderConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM)
@@ -221,7 +219,7 @@ void SliderConstraint::CalculatePositionLimitsConstraintProperties(float inDelta
 	// Check if distance is within limits
 	// Check if distance is within limits
 	bool below_min = mD <= mLimitsMin;
 	bool below_min = mD <= mLimitsMin;
 	if (mHasLimits && (below_min || mD >= mLimitsMax))
 	if (mHasLimits && (below_min || mD >= mLimitsMax))
-		mPositionLimitsConstraintPart.CalculateConstraintProperties(inDeltaTime, *mBody1, mR1 + mU, *mBody2, mR2, mWorldSpaceSliderAxis, 0.0f, mD - (below_min? mLimitsMin : mLimitsMax), mFrequency, mDamping);
+		mPositionLimitsConstraintPart.CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, mR1 + mU, *mBody2, mR2, mWorldSpaceSliderAxis, 0.0f, mD - (below_min? mLimitsMin : mLimitsMax), mLimitsSpringSettings);
 	else
 	else
 		mPositionLimitsConstraintPart.Deactivate();
 		mPositionLimitsConstraintPart.Deactivate();
 }
 }
@@ -232,17 +230,17 @@ void SliderConstraint::CalculateMotorConstraintProperties(float inDeltaTime)
 	{
 	{
 	case EMotorState::Off:
 	case EMotorState::Off:
 		if (mMaxFrictionForce > 0.0f)
 		if (mMaxFrictionForce > 0.0f)
-			mMotorConstraintPart.CalculateConstraintProperties(inDeltaTime, *mBody1, mR1 + mU, *mBody2, mR2, mWorldSpaceSliderAxis);
+			mMotorConstraintPart.CalculateConstraintProperties(*mBody1, mR1 + mU, *mBody2, mR2, mWorldSpaceSliderAxis);
 		else
 		else
 			mMotorConstraintPart.Deactivate();
 			mMotorConstraintPart.Deactivate();
 		break;
 		break;
 
 
 	case EMotorState::Velocity:
 	case EMotorState::Velocity:
-		mMotorConstraintPart.CalculateConstraintProperties(inDeltaTime, *mBody1, mR1 + mU, *mBody2, mR2, mWorldSpaceSliderAxis, -mTargetVelocity);
+		mMotorConstraintPart.CalculateConstraintProperties(*mBody1, mR1 + mU, *mBody2, mR2, mWorldSpaceSliderAxis, -mTargetVelocity);
 		break;
 		break;
 
 
 	case EMotorState::Position:
 	case EMotorState::Position:
-		mMotorConstraintPart.CalculateConstraintProperties(inDeltaTime, *mBody1, mR1 + mU, *mBody2, mR2, mWorldSpaceSliderAxis, 0.0f, mD - mTargetPosition, mMotorSettings.mFrequency, mMotorSettings.mDamping);
+		mMotorConstraintPart.CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, mR1 + mU, *mBody2, mR2, mWorldSpaceSliderAxis, 0.0f, mD - mTargetPosition, mMotorSettings.mSpringSettings);
 		break;
 		break;
 	}	
 	}	
 }
 }
@@ -301,13 +299,23 @@ bool SliderConstraint::SolveVelocityConstraint(float inDeltaTime)
 	bool limit = false;
 	bool limit = false;
 	if (mPositionLimitsConstraintPart.IsActive())
 	if (mPositionLimitsConstraintPart.IsActive())
 	{
 	{
-		if (mD <= mLimitsMin)
-			limit = mPositionLimitsConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mWorldSpaceSliderAxis, 0, FLT_MAX);
+		float min_lambda, max_lambda;
+		if (mLimitsMin == mLimitsMax)
+		{
+			min_lambda = -FLT_MAX;
+			max_lambda = FLT_MAX;
+		}
+		else if (mD <= mLimitsMin)
+		{
+			min_lambda = 0.0f;
+			max_lambda = FLT_MAX;
+		}
 		else
 		else
 		{
 		{
-			JPH_ASSERT(mD >= mLimitsMax);
-			limit = mPositionLimitsConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mWorldSpaceSliderAxis, -FLT_MAX, 0);
+			min_lambda = -FLT_MAX;
+			max_lambda = 0.0f;
 		}
 		}
+		limit = mPositionLimitsConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mWorldSpaceSliderAxis, min_lambda, max_lambda);
 	}
 	}
 
 
 	return motor || pos || rot || limit;
 	return motor || pos || rot || limit;
@@ -330,7 +338,7 @@ bool SliderConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumga
 
 
 	// Solve limits along slider axis
 	// Solve limits along slider axis
 	bool limit = false;
 	bool limit = false;
-	if (mHasLimits && mFrequency <= 0.0f)
+	if (mHasLimits && mLimitsSpringSettings.mFrequency <= 0.0f)
 	{
 	{
 		rotation1 = Mat44::sRotation(mBody1->GetRotation());
 		rotation1 = Mat44::sRotation(mBody1->GetRotation());
 		rotation2 = Mat44::sRotation(mBody2->GetRotation());
 		rotation2 = Mat44::sRotation(mBody2->GetRotation());
@@ -455,8 +463,7 @@ Ref<ConstraintSettings> SliderConstraint::GetConstraintSettings() const
 	settings->mNormalAxis2 = inv_initial_rotation.Multiply3x3(mLocalSpaceNormal1);
 	settings->mNormalAxis2 = inv_initial_rotation.Multiply3x3(mLocalSpaceNormal1);
 	settings->mLimitsMin = mLimitsMin;
 	settings->mLimitsMin = mLimitsMin;
 	settings->mLimitsMax = mLimitsMax;
 	settings->mLimitsMax = mLimitsMax;
-	settings->mFrequency = mFrequency;
-	settings->mDamping = mDamping;
+	settings->mLimitsSpringSettings = mLimitsSpringSettings;
 	settings->mMaxFrictionForce = mMaxFrictionForce;
 	settings->mMaxFrictionForce = mMaxFrictionForce;
 	settings->mMotorSettings = mMotorSettings;
 	settings->mMotorSettings = mMotorSettings;
 	return settings;
 	return settings;

+ 8 - 14
Jolt/Physics/Constraints/SliderConstraint.h

@@ -49,10 +49,8 @@ public:
 	float						mLimitsMin = -FLT_MAX;
 	float						mLimitsMin = -FLT_MAX;
 	float						mLimitsMax = FLT_MAX;
 	float						mLimitsMax = FLT_MAX;
 
 
-	/// If mFrequency > 0 the constraint limits will be soft and mFrequency specifies the oscillation frequency in Hz and mDamping the damping ratio (0 = no damping, 1 = critical damping).
-	/// If mFrequency <= 0, mDamping is ignored and the limits will be hard.
-	float						mFrequency = 0.0f;
-	float						mDamping = 0.0f;
+	/// When enabled, this makes the limits soft. When the constraint exceeds the limits, a spring force will pull it back.
+	SpringSettings				mLimitsSpringSettings;
 
 
 	/// Maximum amount of friction force to apply (N) when not driven by a motor.
 	/// Maximum amount of friction force to apply (N) when not driven by a motor.
 	float						mMaxFrictionForce = 0.0f;
 	float						mMaxFrictionForce = 0.0f;
@@ -118,13 +116,10 @@ public:
 	float						GetLimitsMax() const									{ return mLimitsMax; }
 	float						GetLimitsMax() const									{ return mLimitsMax; }
 	bool						HasLimits() const										{ return mHasLimits; }
 	bool						HasLimits() const										{ return mHasLimits; }
 
 
-	/// Update the spring frequency for the limits constraint
-	void						SetFrequency(float inFrequency)							{ JPH_ASSERT(inFrequency >= 0.0f); mFrequency = inFrequency; }
-	float						GetFrequency() const									{ return mFrequency; }
-
-	/// Update the spring damping for the limits constraint
-	void						SetDamping(float inDamping)								{ JPH_ASSERT(inDamping >= 0.0f); mDamping = inDamping; }
-	float						GetDamping() const										{ return mDamping; }
+	/// Update the limits spring settings
+	const SpringSettings &		GetLimitsSpringSettings() const							{ return mLimitsSpringSettings; }
+	SpringSettings &			GetLimitsSpringSettings()								{ return mLimitsSpringSettings; }
+	void						SetLimitsSpringSettings(const SpringSettings &inLimitsSpringSettings) { mLimitsSpringSettings = inLimitsSpringSettings; }
 
 
 	///@name Get Lagrange multiplier from last physics update (relates to how much force/torque was applied to satisfy the constraint)
 	///@name Get Lagrange multiplier from last physics update (relates to how much force/torque was applied to satisfy the constraint)
 	inline Vector<2> 			GetTotalLambdaPosition() const							{ return mPositionConstraintPart.GetTotalLambda(); }
 	inline Vector<2> 			GetTotalLambdaPosition() const							{ return mPositionConstraintPart.GetTotalLambda(); }
@@ -161,9 +156,8 @@ private:
 	float						mLimitsMin;
 	float						mLimitsMin;
 	float						mLimitsMax;
 	float						mLimitsMax;
 
 
-	// Soft slider limits
-	float						mFrequency;
-	float						mDamping;
+	// Soft constraint limits
+	SpringSettings				mLimitsSpringSettings;
 
 
 	// Friction
 	// Friction
 	float						mMaxFrictionForce;
 	float						mMaxFrictionForce;

+ 35 - 0
Jolt/Physics/Constraints/SpringSettings.cpp

@@ -0,0 +1,35 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Constraints/SpringSettings.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SpringSettings)
+{
+	JPH_ADD_ENUM_ATTRIBUTE(SpringSettings, mMode)
+	JPH_ADD_ATTRIBUTE(SpringSettings, mFrequency)
+	JPH_ADD_ATTRIBUTE(SpringSettings, mDamping)
+}
+
+void SpringSettings::SaveBinaryState(StreamOut &inStream) const
+{
+	inStream.Write(mMode);
+	inStream.Write(mFrequency);
+	inStream.Write(mDamping);
+}
+
+void SpringSettings::RestoreBinaryState(StreamIn &inStream)
+{
+	inStream.Read(mMode);
+	inStream.Read(mFrequency);
+	inStream.Read(mDamping);
+}
+
+JPH_NAMESPACE_END

+ 62 - 0
Jolt/Physics/Constraints/SpringSettings.h

@@ -0,0 +1,62 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/ObjectStream/SerializableObject.h>
+
+JPH_NAMESPACE_BEGIN
+
+class StreamIn;
+class StreamOut;
+
+/// Enum used by constraints to specify how the spring is defined
+enum class ESpringMode : uint8
+{
+	FrequencyAndDamping,		///< Frequency and damping are specified
+	StiffnessAndDamping,		///< Stiffness and damping are specified
+};
+
+/// Settings for a linear or angular spring
+class JPH_EXPORT SpringSettings
+{
+public:
+	JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, SpringSettings)
+
+	/// Constructor
+								SpringSettings() = default;
+								SpringSettings(const SpringSettings &) = default;
+								SpringSettings(ESpringMode inMode, float inFrequencyOrStiffness, float inDamping) : mMode(inMode), mFrequency(inFrequencyOrStiffness), mDamping(inDamping) { }
+
+	/// Saves the contents of the spring settings in binary form to inStream.
+	void						SaveBinaryState(StreamOut &inStream) const;
+
+	/// Restores contents from the binary stream inStream.
+	void						RestoreBinaryState(StreamIn &inStream);
+
+	/// 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;
+
+	union
+	{
+		/// 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).
+		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).
+		float					mStiffness;
+	};
+
+	/// When mSpringMode = ESpringMode::FrequencyAndDamping mDamping is the damping ratio (0 = no damping, 1 = critical damping).
+	/// When mSpringMode = ESpringMode::StiffnessAndDamping mDamping is the damping (c) in the spring equation F = -k * x - c * v for a linear or T = -k * theta - c * w for an angular spring.
+	/// Note that if you set mDamping = 0, you will not get an infinite oscillation. Because we integrate physics using an explicit Euler scheme, there is always energy loss.
+	/// This is done to keep the simulation from exploding, because with a damping of 0 and even the slightest rounding error, the oscillation could become bigger and bigger until the simluation explodes.
+	float						mDamping = 0.0f;
+};
+
+JPH_NAMESPACE_END

+ 7 - 7
Jolt/Physics/Constraints/SwingTwistConstraint.cpp

@@ -204,7 +204,7 @@ void SwingTwistConstraint::SetupVelocityConstraint(float inDeltaTime)
 	Quat q = constraint_body1_to_world.Conjugated() * constraint_body2_to_world;
 	Quat q = constraint_body1_to_world.Conjugated() * constraint_body2_to_world;
 
 
 	// Calculate constraint properties for the swing twist limit
 	// Calculate constraint properties for the swing twist limit
-	mSwingTwistConstraintPart.CalculateConstraintProperties(inDeltaTime, *mBody1, *mBody2, q, constraint_body1_to_world);
+	mSwingTwistConstraintPart.CalculateConstraintProperties(*mBody1, *mBody2, q, constraint_body1_to_world);
 
 
 	if (mSwingMotorState != EMotorState::Off || mTwistMotorState != EMotorState::Off || mMaxFrictionTorque > 0.0f)
 	if (mSwingMotorState != EMotorState::Off || mTwistMotorState != EMotorState::Off || mMaxFrictionTorque > 0.0f)
 	{
 	{
@@ -254,7 +254,7 @@ void SwingTwistConstraint::SetupVelocityConstraint(float inDeltaTime)
 			{
 			{
 				// Enable friction
 				// Enable friction
 				for (int i = 1; i < 3; ++i)
 				for (int i = 1; i < 3; ++i)
-					mMotorConstraintPart[i].CalculateConstraintProperties(inDeltaTime, *mBody1, *mBody2, mWorldSpaceMotorAxis[i], 0.0f);
+					mMotorConstraintPart[i].CalculateConstraintProperties(*mBody1, *mBody2, mWorldSpaceMotorAxis[i], 0.0f);
 			}
 			}
 			else
 			else
 			{
 			{
@@ -267,13 +267,13 @@ void SwingTwistConstraint::SetupVelocityConstraint(float inDeltaTime)
 		case EMotorState::Velocity:
 		case EMotorState::Velocity:
 			// Use motor to create angular velocity around desired axis
 			// Use motor to create angular velocity around desired axis
 			for (int i = 1; i < 3; ++i)
 			for (int i = 1; i < 3; ++i)
-				mMotorConstraintPart[i].CalculateConstraintProperties(inDeltaTime, *mBody1, *mBody2, mWorldSpaceMotorAxis[i], -mTargetAngularVelocity[i]);
+				mMotorConstraintPart[i].CalculateConstraintProperties(*mBody1, *mBody2, mWorldSpaceMotorAxis[i], -mTargetAngularVelocity[i]);
 			break;
 			break;
 
 
 		case EMotorState::Position:
 		case EMotorState::Position:
 			// Use motor to drive rotation error to zero
 			// Use motor to drive rotation error to zero
 			for (int i = 1; i < 3; ++i)
 			for (int i = 1; i < 3; ++i)
-				mMotorConstraintPart[i].CalculateConstraintProperties(inDeltaTime, *mBody1, *mBody2, mWorldSpaceMotorAxis[i], 0.0f, rotation_error[i], mSwingMotorSettings.mFrequency, mSwingMotorSettings.mDamping);
+				mMotorConstraintPart[i].CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, *mBody2, mWorldSpaceMotorAxis[i], 0.0f, rotation_error[i], mSwingMotorSettings.mSpringSettings);
 			break;
 			break;
 		}	
 		}	
 
 
@@ -284,7 +284,7 @@ void SwingTwistConstraint::SetupVelocityConstraint(float inDeltaTime)
 			if (mMaxFrictionTorque > 0.0f)
 			if (mMaxFrictionTorque > 0.0f)
 			{
 			{
 				// Enable friction
 				// Enable friction
-				mMotorConstraintPart[0].CalculateConstraintProperties(inDeltaTime, *mBody1, *mBody2, mWorldSpaceMotorAxis[0], 0.0f);
+				mMotorConstraintPart[0].CalculateConstraintProperties(*mBody1, *mBody2, mWorldSpaceMotorAxis[0], 0.0f);
 			}
 			}
 			else
 			else
 			{
 			{
@@ -295,12 +295,12 @@ void SwingTwistConstraint::SetupVelocityConstraint(float inDeltaTime)
 
 
 		case EMotorState::Velocity:
 		case EMotorState::Velocity:
 			// Use motor to create angular velocity around desired axis
 			// Use motor to create angular velocity around desired axis
-			mMotorConstraintPart[0].CalculateConstraintProperties(inDeltaTime, *mBody1, *mBody2, mWorldSpaceMotorAxis[0], -mTargetAngularVelocity[0]);
+			mMotorConstraintPart[0].CalculateConstraintProperties(*mBody1, *mBody2, mWorldSpaceMotorAxis[0], -mTargetAngularVelocity[0]);
 			break;
 			break;
 
 
 		case EMotorState::Position:
 		case EMotorState::Position:
 			// Use motor to drive rotation error to zero
 			// Use motor to drive rotation error to zero
-			mMotorConstraintPart[0].CalculateConstraintProperties(inDeltaTime, *mBody1, *mBody2, mWorldSpaceMotorAxis[0], 0.0f, rotation_error[0], mTwistMotorSettings.mFrequency, mTwistMotorSettings.mDamping);
+			mMotorConstraintPart[0].CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, *mBody2, mWorldSpaceMotorAxis[0], 0.0f, rotation_error[0], mTwistMotorSettings.mSpringSettings);
 			break;
 			break;
 		}	
 		}	
 	}
 	}

+ 3 - 3
Jolt/Physics/PhysicsSystem.cpp

@@ -2037,7 +2037,7 @@ void PhysicsSystem::JobResolveCCDContacts(PhysicsUpdateContext *ioContext, Physi
 
 
 					// Solve contact constraint
 					// Solve contact constraint
 					AxisConstraintPart contact_constraint;
 					AxisConstraintPart contact_constraint;
-					contact_constraint.CalculateConstraintProperties(ioContext->mSubStepDeltaTime, body1, r1_plus_u, body2, r2, ccd_body->mContactNormal, normal_velocity_bias);
+					contact_constraint.CalculateConstraintProperties(body1, r1_plus_u, body2, r2, ccd_body->mContactNormal, normal_velocity_bias);
 					contact_constraint.SolveVelocityConstraint(body1, body2, ccd_body->mContactNormal, -FLT_MAX, FLT_MAX);
 					contact_constraint.SolveVelocityConstraint(body1, body2, ccd_body->mContactNormal, -FLT_MAX, FLT_MAX);
 
 
 					// Apply friction
 					// Apply friction
@@ -2049,11 +2049,11 @@ void PhysicsSystem::JobResolveCCDContacts(PhysicsUpdateContext *ioContext, Physi
 						float max_lambda_f = ccd_body->mContactSettings.mCombinedFriction * contact_constraint.GetTotalLambda();
 						float max_lambda_f = ccd_body->mContactSettings.mCombinedFriction * contact_constraint.GetTotalLambda();
 
 
 						AxisConstraintPart friction1;
 						AxisConstraintPart friction1;
-						friction1.CalculateConstraintProperties(ioContext->mSubStepDeltaTime, body1, r1_plus_u, body2, r2, tangent1, 0.0f);
+						friction1.CalculateConstraintProperties(body1, r1_plus_u, body2, r2, tangent1);
 						friction1.SolveVelocityConstraint(body1, body2, tangent1, -max_lambda_f, max_lambda_f);
 						friction1.SolveVelocityConstraint(body1, body2, tangent1, -max_lambda_f, max_lambda_f);
 
 
 						AxisConstraintPart friction2;
 						AxisConstraintPart friction2;
-						friction2.CalculateConstraintProperties(ioContext->mSubStepDeltaTime, body1, r1_plus_u, body2, r2, tangent2, 0.0f);
+						friction2.CalculateConstraintProperties(body1, r1_plus_u, body2, r2, tangent2);
 						friction2.SolveVelocityConstraint(body1, body2, tangent2, -max_lambda_f, max_lambda_f);
 						friction2.SolveVelocityConstraint(body1, body2, tangent2, -max_lambda_f, max_lambda_f);
 					}
 					}
 
 

+ 1 - 1
Jolt/Physics/Vehicle/MotorcycleController.cpp

@@ -111,7 +111,7 @@ void MotorcycleController::PreCollide(float inDeltaTime, PhysicsSystem &inPhysic
 		mTargetLean = world_up;
 		mTargetLean = world_up;
 	}
 	}
 
 
-	JPH_DET_LOG("WheeledVehicleController::PreCollide: target_lean: " << target_lean << " mTargetLean: " << mTargetLean);
+	JPH_DET_LOG("WheeledVehicleController::PreCollide: mTargetLean: " << mTargetLean);
 
 
 	// Calculate max steering angle based on the max lean angle we're willing to take
 	// Calculate max steering angle based on the max lean angle we're willing to take
 	// See: https://en.wikipedia.org/wiki/Bicycle_and_motorcycle_dynamics#Leaning
 	// See: https://en.wikipedia.org/wiki/Bicycle_and_motorcycle_dynamics#Leaning

+ 9 - 9
Jolt/Physics/Vehicle/VehicleConstraint.cpp

@@ -321,7 +321,7 @@ void VehicleConstraint::CalculateWheelContactPoint(const Wheel &inWheel, Vec3 &o
 	outR2 = Vec3(inWheel.mContactPosition - inWheel.mContactBody->GetCenterOfMassPosition());
 	outR2 = Vec3(inWheel.mContactPosition - inWheel.mContactBody->GetCenterOfMassPosition());
 }
 }
 
 
-void VehicleConstraint::CalculatePitchRollConstraintProperties(float inDeltaTime, RMat44Arg inBodyTransform)
+void VehicleConstraint::CalculatePitchRollConstraintProperties(RMat44Arg inBodyTransform)
 {
 {
 	// Check if a limit was specified
 	// Check if a limit was specified
 	if (mCosMaxPitchRollAngle > -1.0f)
 	if (mCosMaxPitchRollAngle > -1.0f)
@@ -337,7 +337,7 @@ void VehicleConstraint::CalculatePitchRollConstraintProperties(float inDeltaTime
 			if (len > 0.0f)
 			if (len > 0.0f)
 				mPitchRollRotationAxis = rotation_axis / len;
 				mPitchRollRotationAxis = rotation_axis / len;
 
 
-			mPitchRollPart.CalculateConstraintProperties(inDeltaTime, *mBody, Body::sFixedToWorld, mPitchRollRotationAxis);
+			mPitchRollPart.CalculateConstraintProperties(*mBody, Body::sFixedToWorld, mPitchRollRotationAxis);
 		}
 		}
 		else
 		else
 			mPitchRollPart.Deactivate();
 			mPitchRollPart.Deactivate();
@@ -412,20 +412,20 @@ void VehicleConstraint::SetupVelocityConstraint(float inDeltaTime)
 				// Get the value of the constraint equation
 				// Get the value of the constraint equation
 				float c = w->mSuspensionLength - settings->mSuspensionMaxLength - settings->mSuspensionPreloadLength;
 				float c = w->mSuspensionLength - settings->mSuspensionMaxLength - settings->mSuspensionPreloadLength;
 
 
-				w->mSuspensionPart.CalculateConstraintProperties(inDeltaTime, *mBody, r1_plus_u, *w->mContactBody, r2, neg_contact_normal, w->mAntiRollBarImpulse, c, frequency, damping);
+				w->mSuspensionPart.CalculateConstraintPropertiesWithFrequencyAndDamping(inDeltaTime, *mBody, r1_plus_u, *w->mContactBody, r2, neg_contact_normal, w->mAntiRollBarImpulse, c, frequency, damping);
 			}
 			}
 			else
 			else
 				w->mSuspensionPart.Deactivate();
 				w->mSuspensionPart.Deactivate();
 
 
 			// Check if we reached the 'max up' position and if so add a hard velocity constraint that stops any further movement in the normal direction
 			// Check if we reached the 'max up' position and if so add a hard velocity constraint that stops any further movement in the normal direction
 			if (w->mSuspensionLength < settings->mSuspensionMinLength)
 			if (w->mSuspensionLength < settings->mSuspensionMinLength)
-				w->mSuspensionMaxUpPart.CalculateConstraintProperties(inDeltaTime, *mBody, r1_plus_u, *w->mContactBody, r2, neg_contact_normal);
+				w->mSuspensionMaxUpPart.CalculateConstraintProperties(*mBody, r1_plus_u, *w->mContactBody, r2, neg_contact_normal);
 			else
 			else
 				w->mSuspensionMaxUpPart.Deactivate();
 				w->mSuspensionMaxUpPart.Deactivate();
 			
 			
 			// Friction and propulsion
 			// Friction and propulsion
-			w->mLongitudinalPart.CalculateConstraintProperties(inDeltaTime, *mBody, r1_plus_u, *w->mContactBody, r2, -w->mContactLongitudinal, 0.0f, 0.0f, 0.0f, 0.0f);
-			w->mLateralPart.CalculateConstraintProperties(inDeltaTime, *mBody, r1_plus_u, *w->mContactBody, r2, -w->mContactLateral, 0.0f, 0.0f, 0.0f, 0.0f);
+			w->mLongitudinalPart.CalculateConstraintProperties(*mBody, r1_plus_u, *w->mContactBody, r2, -w->mContactLongitudinal);
+			w->mLateralPart.CalculateConstraintProperties(*mBody, r1_plus_u, *w->mContactBody, r2, -w->mContactLateral);
 		}
 		}
 		else
 		else
 		{
 		{
@@ -436,7 +436,7 @@ void VehicleConstraint::SetupVelocityConstraint(float inDeltaTime)
 			w->mLateralPart.Deactivate();
 			w->mLateralPart.Deactivate();
 		}
 		}
 
 
-	CalculatePitchRollConstraintProperties(inDeltaTime, body_transform);
+	CalculatePitchRollConstraintProperties(body_transform);
 }
 }
 
 
 void VehicleConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio) 
 void VehicleConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio) 
@@ -510,14 +510,14 @@ bool VehicleConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumg
 				// Recalculate constraint properties since the body may have moved
 				// Recalculate constraint properties since the body may have moved
 				Vec3 r1_plus_u, r2;
 				Vec3 r1_plus_u, r2;
 				CalculateWheelContactPoint(*w, r1_plus_u, r2);
 				CalculateWheelContactPoint(*w, r1_plus_u, r2);
-				w->mSuspensionMaxUpPart.CalculateConstraintProperties(inDeltaTime, *mBody, r1_plus_u, *w->mContactBody, r2, neg_contact_normal, 0.0f, max_up_error);
+				w->mSuspensionMaxUpPart.CalculateConstraintProperties(*mBody, r1_plus_u, *w->mContactBody, r2, neg_contact_normal);
 
 
 				impulse |= w->mSuspensionMaxUpPart.SolvePositionConstraint(*mBody, *w->mContactBody, neg_contact_normal, max_up_error, inBaumgarte);
 				impulse |= w->mSuspensionMaxUpPart.SolvePositionConstraint(*mBody, *w->mContactBody, neg_contact_normal, max_up_error, inBaumgarte);
 			}
 			}
 		}
 		}
 
 
 	// Apply the pitch / roll constraint to avoid the vehicle from toppling over
 	// Apply the pitch / roll constraint to avoid the vehicle from toppling over
-	CalculatePitchRollConstraintProperties(inDeltaTime, body_transform);
+	CalculatePitchRollConstraintProperties(body_transform);
 	if (mPitchRollPart.IsActive())
 	if (mPitchRollPart.IsActive())
 		impulse |= mPitchRollPart.SolvePositionConstraint(*mBody, Body::sFixedToWorld, mCosPitchRollAngle - mCosMaxPitchRollAngle, inBaumgarte);
 		impulse |= mPitchRollPart.SolvePositionConstraint(*mBody, Body::sFixedToWorld, mCosPitchRollAngle - mCosMaxPitchRollAngle, inBaumgarte);
 
 

+ 1 - 1
Jolt/Physics/Vehicle/VehicleConstraint.h

@@ -158,7 +158,7 @@ private:
 	void						CalculateWheelContactPoint(const Wheel &inWheel, Vec3 &outR1PlusU, Vec3 &outR2) const;
 	void						CalculateWheelContactPoint(const Wheel &inWheel, Vec3 &outR1PlusU, Vec3 &outR2) const;
 
 
 	// Calculate the constraint properties for mPitchRollPart
 	// Calculate the constraint properties for mPitchRollPart
-	void						CalculatePitchRollConstraintProperties(float inDeltaTime, RMat44Arg inBodyTransform);
+	void						CalculatePitchRollConstraintProperties(RMat44Arg inBodyTransform);
 
 
 	// Simluation information
 	// Simluation information
 	Body *						mBody;										///< Body of the vehicle
 	Body *						mBody;										///< Body of the vehicle

+ 2 - 2
Samples/SamplesApp.cpp

@@ -1687,8 +1687,8 @@ void SamplesApp::UpdateDebug()
 						// Create constraint to drag body
 						// Create constraint to drag body
 						DistanceConstraintSettings settings;
 						DistanceConstraintSettings settings;
 						settings.mPoint1 = settings.mPoint2 = hit_position;
 						settings.mPoint1 = settings.mPoint2 = hit_position;
-						settings.mFrequency = 2.0f / GetWorldScale();
-						settings.mDamping = 1.0f;
+						settings.mLimitsSpringSettings.mFrequency = 2.0f / GetWorldScale();
+						settings.mLimitsSpringSettings.mDamping = 1.0f;
 
 
 						// Construct fixed body for the mouse constraint
 						// Construct fixed body for the mouse constraint
 						// Note that we don't add it to the world since we don't want anything to collide with it, we just
 						// Note that we don't add it to the world since we don't want anything to collide with it, we just

+ 38 - 0
Samples/Tests/Constraints/HingeConstraintTest.cpp

@@ -85,4 +85,42 @@ void HingeConstraintTest::Initialize()
 			prev = &segment;
 			prev = &segment;
 		}
 		}
 	}
 	}
+
+	{
+		// Two bodies connected with a hard hinge
+		Body *body1 = mBodyInterface->CreateBody(BodyCreationSettings(new BoxShape(Vec3::sReplicate(1.0f)), RVec3(4, 5, 0), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING));
+		body1->SetCollisionGroup(CollisionGroup(group_filter, 0, 0));
+		mBodyInterface->AddBody(body1->GetID(), EActivation::DontActivate);
+		Body *body2 = mBodyInterface->CreateBody(BodyCreationSettings(new BoxShape(Vec3::sReplicate(1.0f)), RVec3(6, 5, 0), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING));
+		body2->SetCollisionGroup(CollisionGroup(group_filter, 0, 1));
+		mBodyInterface->AddBody(body2->GetID(), EActivation::Activate);
+
+		HingeConstraintSettings hinge;
+		hinge.mPoint1 = hinge.mPoint2 = RVec3(5, 4, 0);
+		hinge.mHingeAxis1 = hinge.mHingeAxis2 = Vec3::sAxisZ();
+		hinge.mNormalAxis1 = hinge.mNormalAxis2 = Vec3::sAxisY();
+		hinge.mLimitsMin = DegreesToRadians(-10.0f);
+		hinge.mLimitsMax = DegreesToRadians(110.0f);
+		mPhysicsSystem->AddConstraint(hinge.Create(*body1, *body2));
+	}
+
+	{
+		// Two bodies connected with a soft hinge
+		Body *body1 = mBodyInterface->CreateBody(BodyCreationSettings(new BoxShape(Vec3::sReplicate(1.0f)), RVec3(10, 5, 0), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING));
+		body1->SetCollisionGroup(CollisionGroup(group_filter, 0, 0));
+		mBodyInterface->AddBody(body1->GetID(), EActivation::DontActivate);
+		Body *body2 = mBodyInterface->CreateBody(BodyCreationSettings(new BoxShape(Vec3::sReplicate(1.0f)), RVec3(12, 5, 0), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING));
+		body2->SetCollisionGroup(CollisionGroup(group_filter, 0, 1));
+		mBodyInterface->AddBody(body2->GetID(), EActivation::Activate);
+
+		HingeConstraintSettings hinge;
+		hinge.mPoint1 = hinge.mPoint2 = RVec3(11, 4, 0);
+		hinge.mHingeAxis1 = hinge.mHingeAxis2 = Vec3::sAxisZ();
+		hinge.mNormalAxis1 = hinge.mNormalAxis2 = Vec3::sAxisY();
+		hinge.mLimitsMin = DegreesToRadians(-10.0f);
+		hinge.mLimitsMax = DegreesToRadians(110.0f);
+		hinge.mLimitsSpringSettings.mFrequency = 1.0f;
+		hinge.mLimitsSpringSettings.mDamping = 0.5f;
+		mPhysicsSystem->AddConstraint(hinge.Create(*body1, *body2));
+	}
 }
 }

+ 2 - 2
Samples/Tests/Constraints/PathConstraintTest.cpp

@@ -101,8 +101,8 @@ void PathConstraintTest::PrePhysicsUpdate(const PreUpdateParams &inParams)
 	{
 	{
 		MotorSettings &motor_settings = c->GetPositionMotorSettings();
 		MotorSettings &motor_settings = c->GetPositionMotorSettings();
 		motor_settings.SetForceLimit(sMaxMotorAcceleration / c->GetBody2()->GetMotionProperties()->GetInverseMass()); // F = m * a
 		motor_settings.SetForceLimit(sMaxMotorAcceleration / c->GetBody2()->GetMotionProperties()->GetInverseMass()); // F = m * a
-		motor_settings.mFrequency = sFrequency;
-		motor_settings.mDamping = sDamping;
+		motor_settings.mSpringSettings.mFrequency = sFrequency;
+		motor_settings.mSpringSettings.mDamping = sDamping;
 		c->SetMaxFrictionForce(sMaxFrictionAcceleration / c->GetBody2()->GetMotionProperties()->GetInverseMass());
 		c->SetMaxFrictionForce(sMaxFrictionAcceleration / c->GetBody2()->GetMotionProperties()->GetInverseMass());
 	}
 	}
 }
 }

+ 2 - 2
Samples/Tests/Constraints/PoweredHingeConstraintTest.cpp

@@ -65,8 +65,8 @@ void PoweredHingeConstraintTest::PrePhysicsUpdate(const PreUpdateParams &inParam
 	// Torque = Inertia * Angular Acceleration (alpha)
 	// Torque = Inertia * Angular Acceleration (alpha)
 	MotorSettings &motor_settings = mConstraint->GetMotorSettings();
 	MotorSettings &motor_settings = mConstraint->GetMotorSettings();
 	motor_settings.SetTorqueLimit(mInertiaBody2AsSeenFromConstraint * sMaxAngularAcceleration);
 	motor_settings.SetTorqueLimit(mInertiaBody2AsSeenFromConstraint * sMaxAngularAcceleration);
-	motor_settings.mFrequency = sFrequency;
-	motor_settings.mDamping = sDamping;
+	motor_settings.mSpringSettings.mFrequency = sFrequency;
+	motor_settings.mSpringSettings.mDamping = sDamping;
 	mConstraint->SetMaxFrictionTorque(mInertiaBody2AsSeenFromConstraint * sMaxFrictionAngularAcceleration);
 	mConstraint->SetMaxFrictionTorque(mInertiaBody2AsSeenFromConstraint * sMaxFrictionAngularAcceleration);
 }
 }
 
 

+ 2 - 2
Samples/Tests/Constraints/PoweredSliderConstraintTest.cpp

@@ -57,8 +57,8 @@ void PoweredSliderConstraintTest::PrePhysicsUpdate(const PreUpdateParams &inPara
 { 
 { 
 	MotorSettings &motor_settings = mConstraint->GetMotorSettings();
 	MotorSettings &motor_settings = mConstraint->GetMotorSettings();
 	motor_settings.SetForceLimit(sMaxMotorAcceleration / mBody2->GetMotionProperties()->GetInverseMass()); // F = m * a
 	motor_settings.SetForceLimit(sMaxMotorAcceleration / mBody2->GetMotionProperties()->GetInverseMass()); // F = m * a
-	motor_settings.mFrequency = sFrequency;
-	motor_settings.mDamping = sDamping;
+	motor_settings.mSpringSettings.mFrequency = sFrequency;
+	motor_settings.mSpringSettings.mDamping = sDamping;
 	mConstraint->SetMaxFrictionForce(sMaxFrictionAcceleration / mBody2->GetMotionProperties()->GetInverseMass());
 	mConstraint->SetMaxFrictionForce(sMaxFrictionAcceleration / mBody2->GetMotionProperties()->GetInverseMass());
 }
 }
 
 

+ 4 - 4
Samples/Tests/Constraints/PoweredSwingTwistConstraintTest.cpp

@@ -81,13 +81,13 @@ void PoweredSwingTwistConstraintTest::PrePhysicsUpdate(const PreUpdateParams &in
 
 
 	MotorSettings &swing = mConstraint->GetSwingMotorSettings();
 	MotorSettings &swing = mConstraint->GetSwingMotorSettings();
 	swing.SetTorqueLimit(mInertiaBody2AsSeenFromConstraint * sMaxAngularAcceleration);
 	swing.SetTorqueLimit(mInertiaBody2AsSeenFromConstraint * sMaxAngularAcceleration);
-	swing.mFrequency = sFrequency;
-	swing.mDamping = sDamping;
+	swing.mSpringSettings.mFrequency = sFrequency;
+	swing.mSpringSettings.mDamping = sDamping;
 
 
 	MotorSettings &twist = mConstraint->GetTwistMotorSettings();
 	MotorSettings &twist = mConstraint->GetTwistMotorSettings();
 	twist.SetTorqueLimit(mInertiaBody2AsSeenFromConstraint * sMaxAngularAcceleration);
 	twist.SetTorqueLimit(mInertiaBody2AsSeenFromConstraint * sMaxAngularAcceleration);
-	twist.mFrequency = sFrequency;
-	twist.mDamping = sDamping;
+	twist.mSpringSettings.mFrequency = sFrequency;
+	twist.mSpringSettings.mDamping = sDamping;
 }
 }
 
 
 void PoweredSwingTwistConstraintTest::GetInitialCamera(CameraState &ioState) const 
 void PoweredSwingTwistConstraintTest::GetInitialCamera(CameraState &ioState) const 

+ 2 - 0
Samples/Tests/Constraints/SixDOFConstraintTest.cpp

@@ -100,6 +100,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; });
 			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 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 Max", sLimitMax[i], 0.0f, 10.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; });
 		}
 		}
 
 
 		for (int i = 3; i < 6; ++i)
 		for (int i = 3; i < 6; ++i)

+ 2 - 2
Samples/Tests/Constraints/SliderConstraintTest.cpp

@@ -142,8 +142,8 @@ void SliderConstraintTest::Initialize()
 		slider.SetSliderAxis(Vec3::sAxisY());
 		slider.SetSliderAxis(Vec3::sAxisY());
 		slider.mLimitsMin = 0.0f;
 		slider.mLimitsMin = 0.0f;
 		slider.mLimitsMax = 2.0f;
 		slider.mLimitsMax = 2.0f;
-		slider.mFrequency = 1.0f;
-		slider.mDamping = 0.5f;
+		slider.mLimitsSpringSettings.mFrequency = 1.0f;
+		slider.mLimitsSpringSettings.mDamping = 0.5f;
 		mPhysicsSystem->AddConstraint(slider.Create(*vert1, *vert2));
 		mPhysicsSystem->AddConstraint(slider.Create(*vert1, *vert2));
 	}
 	}
 }
 }

+ 4 - 4
Samples/Tests/Constraints/SpringTest.cpp

@@ -40,7 +40,7 @@ void SpringTest::Initialize()
 		DistanceConstraintSettings settings;
 		DistanceConstraintSettings settings;
 		settings.mPoint1 = attachment_point;
 		settings.mPoint1 = attachment_point;
 		settings.mPoint2 = body_position;
 		settings.mPoint2 = body_position;
-		settings.mFrequency = 0.33f;
+		settings.mLimitsSpringSettings.mFrequency = 0.33f;
 		mPhysicsSystem->AddConstraint(settings.Create(top, body));
 		mPhysicsSystem->AddConstraint(settings.Create(top, body));
 
 
 		// Move the body up so that it can start oscillating
 		// Move the body up so that it can start oscillating
@@ -62,7 +62,7 @@ void SpringTest::Initialize()
 		DistanceConstraintSettings settings;
 		DistanceConstraintSettings settings;
 		settings.mPoint1 = attachment_point;
 		settings.mPoint1 = attachment_point;
 		settings.mPoint2 = body_position;
 		settings.mPoint2 = body_position;
-		settings.mFrequency = 0.1f + 0.1f * i;
+		settings.mLimitsSpringSettings.mFrequency = 0.1f + 0.1f * i;
 		mPhysicsSystem->AddConstraint(settings.Create(top, body));
 		mPhysicsSystem->AddConstraint(settings.Create(top, body));
 
 
 		// Move the body up so that it can start oscillating
 		// Move the body up so that it can start oscillating
@@ -84,8 +84,8 @@ void SpringTest::Initialize()
 		DistanceConstraintSettings settings;
 		DistanceConstraintSettings settings;
 		settings.mPoint1 = attachment_point;
 		settings.mPoint1 = attachment_point;
 		settings.mPoint2 = body_position;
 		settings.mPoint2 = body_position;
-		settings.mFrequency = 0.33f;
-		settings.mDamping = (1.0f / 9.0f) * i;
+		settings.mLimitsSpringSettings.mFrequency = 0.33f;
+		settings.mLimitsSpringSettings.mDamping = (1.0f / 9.0f) * i;
 		mPhysicsSystem->AddConstraint(settings.Create(top, body));
 		mPhysicsSystem->AddConstraint(settings.Create(top, body));
 
 
 		// Move the body up so that it can start oscillating
 		// Move the body up so that it can start oscillating

+ 74 - 0
UnitTests/Physics/DistanceConstraintTests.cpp

@@ -0,0 +1,74 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include "PhysicsTestContext.h"
+#include <Jolt/Physics/Constraints/DistanceConstraint.h>
+#include "Layers.h"
+
+TEST_SUITE("DistanceConstraintTests")
+{
+	// Test if the distance constraint can be used to create a spring
+	TEST_CASE("TestDistanceSpring")
+	{
+		// Configuration of the spring
+		const RVec3 cInitialPosition(10, 0, 0);
+		const float cFrequency = 2.0f;
+		const float cDamping = 0.1f;
+
+		for (int mode = 0; mode < 2; ++mode)
+		{
+			// Create a sphere
+			PhysicsTestContext context;
+			context.ZeroGravity();
+			Body &body = context.CreateSphere(cInitialPosition, 0.5f, EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING);
+			body.GetMotionProperties()->SetLinearDamping(0.0f);
+
+			// Calculate stiffness and damping of spring
+			float m = 1.0f / body.GetMotionProperties()->GetInverseMass();
+			float omega = 2.0f * JPH_PI * cFrequency;
+			float k = m * Square(omega);
+			float c = 2.0f * m * cDamping * omega;
+
+			// Create spring
+			DistanceConstraintSettings constraint;
+			constraint.mPoint2 = cInitialPosition;
+			if (mode == 0)
+			{
+				// First iteration use stiffness and damping
+				constraint.mLimitsSpringSettings.mMode = ESpringMode::StiffnessAndDamping;
+				constraint.mLimitsSpringSettings.mStiffness = k;
+				constraint.mLimitsSpringSettings.mDamping = c;
+			}
+			else
+			{
+				// Second iteration use frequency and damping
+				constraint.mLimitsSpringSettings.mMode = ESpringMode::FrequencyAndDamping;
+				constraint.mLimitsSpringSettings.mFrequency = cFrequency;
+				constraint.mLimitsSpringSettings.mDamping = cDamping;
+			}
+			constraint.mMinDistance = constraint.mMaxDistance = 0.0f;
+			context.CreateConstraint<DistanceConstraint>(Body::sFixedToWorld, body, constraint);
+
+			// Simulate spring
+			Real x = cInitialPosition.GetX();
+			float v = 0.0f;
+			float dt = context.GetDeltaTime();
+			for (int i = 0; i < 120; ++i)
+			{
+				// Using the equations from page 32 of Soft Contraints: Reinventing The Spring - Erin Catto - GDC 2011 for an implicit euler spring damper
+				v = (v - dt * k / m * float(x)) / (1.0f + dt * c / m + Square(dt) * k / m);
+				x += v * dt;
+
+				// Run physics simulation
+				context.SimulateSingleStep();
+
+				// Test if simulation matches prediction
+				CHECK_APPROX_EQUAL(x, body.GetPosition().GetX(), 5.0e-6_r);
+				CHECK(body.GetPosition().GetY() == 0);
+				CHECK(body.GetPosition().GetZ() == 0);
+			}
+		}
+	}
+}

+ 84 - 0
UnitTests/Physics/HingeConstraintTests.cpp

@@ -0,0 +1,84 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include "PhysicsTestContext.h"
+#include <Jolt/Physics/Constraints/HingeConstraint.h>
+#include <Jolt/Physics/Collision/Shape/SphereShape.h>
+#include "Layers.h"
+
+TEST_SUITE("HingeConstraintTests")
+{
+	// Test if the hinge constraint can be used to create a spring
+	TEST_CASE("TestHingeSpring")
+	{
+		// Configuration of the spring
+		const float cInitialAngle = DegreesToRadians(100.0f);
+		const float cFrequency = 2.0f;
+		const float cDamping = 0.1f;
+
+		for (int mode = 0; mode < 2; ++mode)
+		{
+			// Create a sphere
+			PhysicsTestContext context;
+			Body &body = context.CreateBody(new SphereShapeSettings(0.5f), RVec3::sZero(), Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, EActivation::Activate);
+			body.GetMotionProperties()->SetAngularDamping(0.0f);
+			body.SetAllowSleeping(false);
+
+			// Calculate stiffness and damping of spring
+			float inertia = body.GetMotionProperties()->GetInverseInertiaForRotation(Mat44::sIdentity()).Inversed3x3().GetAxisY().Length();
+			float omega = 2.0f * JPH_PI * cFrequency;
+			float k = inertia * Square(omega);
+			float c = 2.0f * inertia * cDamping * omega;
+
+			// Create spring
+			HingeConstraintSettings constraint;
+			if (mode == 0)
+			{
+				// First iteration use stiffness and damping
+				constraint.mLimitsSpringSettings.mMode = ESpringMode::StiffnessAndDamping;
+				constraint.mLimitsSpringSettings.mStiffness = k;
+				constraint.mLimitsSpringSettings.mDamping = c;
+			}
+			else
+			{
+				// Second iteration use frequency and damping
+				constraint.mLimitsSpringSettings.mMode = ESpringMode::FrequencyAndDamping;
+				constraint.mLimitsSpringSettings.mFrequency = cFrequency;
+				constraint.mLimitsSpringSettings.mDamping = cDamping;
+			}
+			constraint.mLimitsMin = constraint.mLimitsMax = 0.0f;
+			context.CreateConstraint<HingeConstraint>(Body::sFixedToWorld, body, constraint);
+
+			// Rotate the body to the initial angle
+			context.GetBodyInterface().SetRotation(body.GetID(), Quat::sRotation(Vec3::sAxisY(), cInitialAngle), EActivation::Activate);
+
+			// Simulate angular spring
+			float angle = cInitialAngle;
+			float angular_v = 0.0f;
+			float dt = context.GetDeltaTime();
+			for (int i = 0; i < 120; ++i)
+			{
+				// Using the equations from page 32 of Soft Contraints: Reinventing The Spring - Erin Catto - GDC 2011 for an implicit euler spring damper
+				angular_v = (angular_v - dt * k / inertia * angle) / (1.0f + dt * c / inertia + Square(dt) * k / inertia);
+				angle += angular_v * dt;
+
+				// Run physics simulation
+				context.SimulateSingleStep();
+
+				// Decompose body rotation
+				Vec3 actual_axis;
+				float actual_angle;
+				body.GetRotation().GetAxisAngle(actual_axis, actual_angle);
+				if (actual_axis.GetY() < 0.0f)
+					actual_angle = -actual_angle;
+
+				// Test if simulation matches prediction
+				CHECK_APPROX_EQUAL(angle, actual_angle, DegreesToRadians(0.1f));
+				CHECK_APPROX_EQUAL(actual_axis.GetX(), 0);
+				CHECK_APPROX_EQUAL(actual_axis.GetZ(), 0);
+			}
+		}
+	}
+}

+ 87 - 0
UnitTests/Physics/SixDOFConstraintTests.cpp

@@ -0,0 +1,87 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include "PhysicsTestContext.h"
+#include <Jolt/Physics/Constraints/SixDOFConstraint.h>
+#include "Layers.h"
+
+TEST_SUITE("SixDOFConstraintTests")
+{
+	// Test if the 6DOF constraint can be used to create a spring
+	TEST_CASE("TestSixDOFSpring")
+	{
+		// Configuration of the spring
+		const float cFrequency = 2.0f;
+		const float cDamping = 0.1f;
+
+		// Test all permutations of axis
+		for (uint spring_axis = 0b001; spring_axis <= 0b111; ++spring_axis)
+		{
+			// Test all spring modes
+			for (int mode = 0; mode < 2; ++mode)
+			{
+				const RVec3 cInitialPosition(10.0f * (spring_axis & 1), 8.0f * (spring_axis & 2), 6.0f * (spring_axis & 4));
+
+				// Create a sphere
+				PhysicsTestContext context;
+				context.ZeroGravity();
+				Body& body = context.CreateSphere(cInitialPosition, 0.5f, EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING);
+				body.GetMotionProperties()->SetLinearDamping(0.0f);
+
+				// Calculate stiffness and damping of spring
+				float m = 1.0f / body.GetMotionProperties()->GetInverseMass();
+				float omega = 2.0f * JPH_PI * cFrequency;
+				float k = m * Square(omega);
+				float c = 2.0f * m * cDamping * omega;
+
+				// Create spring
+				SixDOFConstraintSettings constraint;
+				constraint.mPosition2 = cInitialPosition;
+				for (int axis = 0; axis < 3; ++axis)
+				{
+					// Check if this axis is supposed to be a spring
+					if (((1 << axis) & spring_axis) != 0)
+					{
+						if (mode == 0)
+						{
+							// First iteration use stiffness and damping
+							constraint.mLimitsSpringSettings[axis].mMode = ESpringMode::StiffnessAndDamping;
+							constraint.mLimitsSpringSettings[axis].mStiffness = k;
+							constraint.mLimitsSpringSettings[axis].mDamping = c;
+						}
+						else
+						{
+							// Second iteration use frequency and damping
+							constraint.mLimitsSpringSettings[axis].mMode = ESpringMode::FrequencyAndDamping;
+							constraint.mLimitsSpringSettings[axis].mFrequency = cFrequency;
+							constraint.mLimitsSpringSettings[axis].mDamping = cDamping;
+						}
+						constraint.mLimitMin[axis] = constraint.mLimitMax[axis] = 0.0f;
+					}
+				}
+				context.CreateConstraint<SixDOFConstraint>(Body::sFixedToWorld, body, constraint);
+
+				// Simulate spring
+				RVec3 x = cInitialPosition;
+				Vec3 v = Vec3::sZero();
+				float dt = context.GetDeltaTime();
+				for (int i = 0; i < 120; ++i)
+				{
+					// Using the equations from page 32 of Soft Contraints: Reinventing The Spring - Erin Catto - GDC 2011 for an implicit euler spring damper
+					for (int axis = 0; axis < 3; ++axis)
+						if (((1 << axis) & spring_axis) != 0) // Only update velocity for axis where there is a spring
+							v.SetComponent(axis, (v[axis] - dt * k / m * float(x[axis])) / (1.0f + dt * c / m + Square(dt) * k / m));
+					x += v * dt;
+
+					// Run physics simulation
+					context.SimulateSingleStep();
+
+					// Test if simulation matches prediction
+					CHECK_APPROX_EQUAL(x, body.GetPosition(), 1.0e-5_r);
+				}
+			}
+		}
+	}
+}

+ 62 - 0
UnitTests/Physics/SliderConstraintTests.cpp

@@ -442,4 +442,66 @@ TEST_SUITE("SliderConstraintTests")
 		CHECK_APPROX_EQUAL(t1.GetColumn3(2), t2.GetColumn3(0), 1.0e-4f);
 		CHECK_APPROX_EQUAL(t1.GetColumn3(2), t2.GetColumn3(0), 1.0e-4f);
 		CHECK_APPROX_EQUAL(t1.GetTranslation(), t2.GetTranslation(), 1.0e-2f);
 		CHECK_APPROX_EQUAL(t1.GetTranslation(), t2.GetTranslation(), 1.0e-2f);
 	}
 	}
+
+	// Test if the slider constraint can be used to create a spring
+	TEST_CASE("TestSliderSpring")
+	{
+		// Configuration of the spring
+		const RVec3 cInitialPosition(10, 0, 0);
+		const float cFrequency = 2.0f;
+		const float cDamping = 0.1f;
+
+		for (int mode = 0; mode < 2; ++mode)
+		{
+			// Create a sphere
+			PhysicsTestContext context;
+			Body &body = context.CreateSphere(cInitialPosition, 0.5f, EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING);
+			body.GetMotionProperties()->SetLinearDamping(0.0f);
+
+			// Calculate stiffness and damping of spring
+			float m = 1.0f / body.GetMotionProperties()->GetInverseMass();
+			float omega = 2.0f * JPH_PI * cFrequency;
+			float k = m * Square(omega);
+			float c = 2.0f * m * cDamping * omega;
+
+			// Create spring
+			SliderConstraintSettings constraint;
+			constraint.mPoint2 = cInitialPosition;
+			if (mode == 0)
+			{
+				// First iteration use stiffness and damping
+				constraint.mLimitsSpringSettings.mMode = ESpringMode::StiffnessAndDamping;
+				constraint.mLimitsSpringSettings.mStiffness = k;
+				constraint.mLimitsSpringSettings.mDamping = c;
+			}
+			else
+			{
+				// Second iteration use frequency and damping
+				constraint.mLimitsSpringSettings.mMode = ESpringMode::FrequencyAndDamping;
+				constraint.mLimitsSpringSettings.mFrequency = cFrequency;
+				constraint.mLimitsSpringSettings.mDamping = cDamping;
+			}
+			constraint.mLimitsMin = constraint.mLimitsMax = 0.0f;
+			context.CreateConstraint<SliderConstraint>(Body::sFixedToWorld, body, constraint);
+
+			// Simulate spring
+			Real x = cInitialPosition.GetX();
+			float v = 0.0f;
+			float dt = context.GetDeltaTime();
+			for (int i = 0; i < 120; ++i)
+			{
+				// Using the equations from page 32 of Soft Contraints: Reinventing The Spring - Erin Catto - GDC 2011 for an implicit euler spring damper
+				v = (v - dt * k / m * float(x)) / (1.0f + dt * c / m + Square(dt) * k / m);
+				x += v * dt;
+
+				// Run physics simulation
+				context.SimulateSingleStep();
+
+				// Test if simulation matches prediction
+				CHECK_APPROX_EQUAL(x, body.GetPosition().GetX(), 5.0e-6_r);
+				CHECK_APPROX_EQUAL(body.GetPosition().GetY(), 0);
+				CHECK_APPROX_EQUAL(body.GetPosition().GetZ(), 0);
+			}
+		}
+	}
 }
 }

+ 3 - 0
UnitTests/UnitTests.cmake

@@ -41,8 +41,10 @@ set(UNIT_TESTS_SRC_FILES
 	${UNIT_TESTS_ROOT}/Physics/CollisionGroupTests.cpp
 	${UNIT_TESTS_ROOT}/Physics/CollisionGroupTests.cpp
 	${UNIT_TESTS_ROOT}/Physics/ContactListenerTests.cpp
 	${UNIT_TESTS_ROOT}/Physics/ContactListenerTests.cpp
 	${UNIT_TESTS_ROOT}/Physics/ConvexVsTrianglesTest.cpp
 	${UNIT_TESTS_ROOT}/Physics/ConvexVsTrianglesTest.cpp
+	${UNIT_TESTS_ROOT}/Physics/DistanceConstraintTests.cpp
 	${UNIT_TESTS_ROOT}/Physics/EstimateCollisionResponseTest.cpp
 	${UNIT_TESTS_ROOT}/Physics/EstimateCollisionResponseTest.cpp
 	${UNIT_TESTS_ROOT}/Physics/HeightFieldShapeTests.cpp
 	${UNIT_TESTS_ROOT}/Physics/HeightFieldShapeTests.cpp
+	${UNIT_TESTS_ROOT}/Physics/HingeConstraintTests.cpp
 	${UNIT_TESTS_ROOT}/Physics/MotionQualityLinearCastTests.cpp
 	${UNIT_TESTS_ROOT}/Physics/MotionQualityLinearCastTests.cpp
 	${UNIT_TESTS_ROOT}/Physics/OffsetCenterOfMassShapeTests.cpp
 	${UNIT_TESTS_ROOT}/Physics/OffsetCenterOfMassShapeTests.cpp
 	${UNIT_TESTS_ROOT}/Physics/PathConstraintTests.cpp
 	${UNIT_TESTS_ROOT}/Physics/PathConstraintTests.cpp
@@ -52,6 +54,7 @@ set(UNIT_TESTS_SRC_FILES
 	${UNIT_TESTS_ROOT}/Physics/RayShapeTests.cpp
 	${UNIT_TESTS_ROOT}/Physics/RayShapeTests.cpp
 	${UNIT_TESTS_ROOT}/Physics/SensorTests.cpp
 	${UNIT_TESTS_ROOT}/Physics/SensorTests.cpp
 	${UNIT_TESTS_ROOT}/Physics/ShapeTests.cpp
 	${UNIT_TESTS_ROOT}/Physics/ShapeTests.cpp
+	${UNIT_TESTS_ROOT}/Physics/SixDOFConstraintTests.cpp
 	${UNIT_TESTS_ROOT}/Physics/SliderConstraintTests.cpp
 	${UNIT_TESTS_ROOT}/Physics/SliderConstraintTests.cpp
 	${UNIT_TESTS_ROOT}/Physics/SubShapeIDTest.cpp
 	${UNIT_TESTS_ROOT}/Physics/SubShapeIDTest.cpp
 	${UNIT_TESTS_ROOT}/Physics/TransformedShapeTests.cpp
 	${UNIT_TESTS_ROOT}/Physics/TransformedShapeTests.cpp