Added Constraint::ResetWarmStart and Ragdoll::ResetWarmStart (#960)

Used to notify the system that the configuration of the bodies and/or constraint has changed enough so that the warm start impulses should not be applied the next frame.
You can use this function for example when repositioning a ragdoll through Ragdoll::SetPose in such a way that the orientation of the bodies completely changes so that
the previous frame impulses are no longer a good approximation of what the impulses will be in the next frame. Calling this function when there are no big changes
will result in the constraints being much 'softer' than usual so they are more easily violated (e.g. a long chain of bodies might sag a bit if you call this every frame).

Jolt/Physics/Constraints/ConeConstraint.cpp

@@ -129,6 +129,12 @@ void ConeConstraint::SetupVelocityConstraint(float inDeltaTime)
 	CalculateRotationConstraintProperties(rotation1, rotation2);
 }
 
+void ConeConstraint::ResetWarmStart()
+{
+	mPointConstraintPart.Deactivate();
+	mAngleConstraintPart.Deactivate();
+}
+
 void ConeConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio)
 {
 	// Warm starting: Apply previous frame impulse

Jolt/Physics/Constraints/ConeConstraint.h

@@ -78,6 +78,7 @@ public:
 	virtual EConstraintSubType	GetSubType() const override					{ return EConstraintSubType::Cone; }
 	virtual void				NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override;
 	virtual void				SetupVelocityConstraint(float inDeltaTime) override;
+	virtual void				ResetWarmStart() override;
 	virtual void				WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override;
 	virtual bool				SolveVelocityConstraint(float inDeltaTime) override;
 	virtual bool				SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override;

Jolt/Physics/Constraints/Constraint.h

@@ -160,6 +160,12 @@ public:
 	/// @param inDeltaCOM The delta of the center of mass of the body (shape->GetCenterOfMass() - shape_before_change->GetCenterOfMass())
 	virtual void				NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) = 0;
 
+	/// Notify the system that the configuration of the bodies and/or constraint has changed enough so that the warm start impulses should not be applied the next frame.
+	/// You can use this function for example when repositioning a ragdoll through Ragdoll::SetPose in such a way that the orientation of the bodies completely changes so that
+	/// the previous frame impulses are no longer a good approximation of what the impulses will be in the next frame. Calling this function when there are no big changes
+	/// will result in the constraints being much 'softer' than usual so they are more easily violated (e.g. a long chain of bodies might sag a bit if you call this every frame).
+	virtual void				ResetWarmStart() = 0;
+
 	///@name Solver interface
 	///@{
 	virtual bool				IsActive() const							{ return mEnabled; }

Jolt/Physics/Constraints/DistanceConstraint.cpp

@@ -161,6 +161,11 @@ void DistanceConstraint::SetupVelocityConstraint(float inDeltaTime)
 	CalculateConstraintProperties(inDeltaTime);
 }
 
+void DistanceConstraint::ResetWarmStart()
+{
+	mAxisConstraint.Deactivate();
+}
+
 void DistanceConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio)
 {
 	mAxisConstraint.WarmStart(*mBody1, *mBody2, mWorldSpaceNormal, inWarmStartImpulseRatio);

Jolt/Physics/Constraints/DistanceConstraint.h

@@ -57,6 +57,7 @@ public:
 	virtual EConstraintSubType	GetSubType() const override									{ return EConstraintSubType::Distance; }
 	virtual void				NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override;
 	virtual void				SetupVelocityConstraint(float inDeltaTime) override;
+	virtual void				ResetWarmStart() override;
 	virtual void				WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override;
 	virtual bool				SolveVelocityConstraint(float inDeltaTime) override;
 	virtual bool				SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override;

Jolt/Physics/Constraints/FixedConstraint.cpp

@@ -130,6 +130,12 @@ void FixedConstraint::SetupVelocityConstraint(float inDeltaTime)
 	mPointConstraintPart.CalculateConstraintProperties(*mBody1, rotation1, mLocalSpacePosition1, *mBody2, rotation2, mLocalSpacePosition2);
 }
 
+void FixedConstraint::ResetWarmStart()
+{
+	mRotationConstraintPart.Deactivate();
+	mPointConstraintPart.Deactivate();
+}
+
 void FixedConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio)
 {
 	// Warm starting: Apply previous frame impulse

Jolt/Physics/Constraints/FixedConstraint.h

@@ -57,6 +57,7 @@ public:
 	virtual EConstraintSubType	GetSubType() const override									{ return EConstraintSubType::Fixed; }
 	virtual void				NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override;
 	virtual void				SetupVelocityConstraint(float inDeltaTime) override;
+	virtual void				ResetWarmStart() override;
 	virtual void				WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override;
 	virtual bool				SolveVelocityConstraint(float inDeltaTime) override;
 	virtual bool				SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override;

Jolt/Physics/Constraints/GearConstraint.cpp

@@ -82,6 +82,11 @@ void GearConstraint::SetupVelocityConstraint(float inDeltaTime)
 	CalculateConstraintProperties(rotation1, rotation2);
 }
 
+void GearConstraint::ResetWarmStart()
+{
+	mGearConstraintPart.Deactivate();
+}
+
 void GearConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio)
 {
 	// Warm starting: Apply previous frame impulse

Jolt/Physics/Constraints/GearConstraint.h

@@ -61,6 +61,7 @@ public:
 	virtual EConstraintSubType	GetSubType() const override								{ return EConstraintSubType::Gear; }
 	virtual void				NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override { /* Do nothing */ }
 	virtual void				SetupVelocityConstraint(float inDeltaTime) override;
+	virtual void				ResetWarmStart() override;
 	virtual void				WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override;
 	virtual bool				SolveVelocityConstraint(float inDeltaTime) override;
 	virtual bool				SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override;

Jolt/Physics/Constraints/HingeConstraint.cpp

@@ -219,6 +219,14 @@ void HingeConstraint::SetupVelocityConstraint(float inDeltaTime)
 	CalculateMotorConstraintProperties(inDeltaTime);
 }
 
+void HingeConstraint::ResetWarmStart()
+{
+	mMotorConstraintPart.Deactivate();
+	mPointConstraintPart.Deactivate();
+	mRotationConstraintPart.Deactivate();
+	mRotationLimitsConstraintPart.Deactivate();
+}
+
 void HingeConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio)
 {
 	// Warm starting: Apply previous frame impulse

Jolt/Physics/Constraints/HingeConstraint.h

@@ -70,6 +70,7 @@ public:
 	virtual EConstraintSubType	GetSubType() const override								{ return EConstraintSubType::Hinge; }
 	virtual void				NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override;
 	virtual void				SetupVelocityConstraint(float inDeltaTime) override;
+	virtual void				ResetWarmStart() override;
 	virtual void				WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override;
 	virtual bool				SolveVelocityConstraint(float inDeltaTime) override;
 	virtual bool				SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override;

Jolt/Physics/Constraints/PathConstraint.cpp

@@ -225,6 +225,15 @@ void PathConstraint::SetupVelocityConstraint(float inDeltaTime)
 	CalculateConstraintProperties(inDeltaTime);
 }
 
+void PathConstraint::ResetWarmStart()
+{
+	mPositionMotorConstraintPart.Deactivate();
+	mPositionConstraintPart.Deactivate();
+	mPositionLimitsConstraintPart.Deactivate();
+	mHingeConstraintPart.Deactivate();
+	mRotationConstraintPart.Deactivate();
+}
+
 void PathConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio)
 {
 	// Warm starting: Apply previous frame impulse

Jolt/Physics/Constraints/PathConstraint.h

@@ -83,6 +83,7 @@ public:
 	virtual EConstraintSubType		GetSubType() const override								{ return EConstraintSubType::Path; }
 	virtual void					NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override;
 	virtual void					SetupVelocityConstraint(float inDeltaTime) override;
+	virtual void					ResetWarmStart() override;
 	virtual void					WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override;
 	virtual bool					SolveVelocityConstraint(float inDeltaTime) override;
 	virtual bool					SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override;

Jolt/Physics/Constraints/PointConstraint.cpp

@@ -97,6 +97,11 @@ void PointConstraint::SetupVelocityConstraint(float inDeltaTime)
 	CalculateConstraintProperties();
 }
 
+void PointConstraint::ResetWarmStart()
+{
+	mPointConstraintPart.Deactivate();
+}
+
 void PointConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio)
 {
 	// Warm starting: Apply previous frame impulse

Jolt/Physics/Constraints/PointConstraint.h

@@ -49,6 +49,7 @@ public:
 	virtual EConstraintSubType	GetSubType() const override									{ return EConstraintSubType::Point; }
 	virtual void				NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override;
 	virtual void				SetupVelocityConstraint(float inDeltaTime) override;
+	virtual void				ResetWarmStart() override;
 	virtual void				WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override;
 	virtual bool				SolveVelocityConstraint(float inDeltaTime) override;
 	virtual bool				SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override;

Jolt/Physics/Constraints/PulleyConstraint.cpp

@@ -156,6 +156,11 @@ void PulleyConstraint::SetupVelocityConstraint(float inDeltaTime)
 		mIndependentAxisConstraintPart.Deactivate();
 }
 
+void PulleyConstraint::ResetWarmStart()
+{
+	mIndependentAxisConstraintPart.Deactivate();
+}
+
 void PulleyConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio)
 {
 	mIndependentAxisConstraintPart.WarmStart(*mBody1, *mBody2, mWorldSpaceNormal1, mWorldSpaceNormal2, mRatio, inWarmStartImpulseRatio);

Jolt/Physics/Constraints/PulleyConstraint.h

@@ -68,6 +68,7 @@ public:
 	virtual EConstraintSubType	GetSubType() const override									{ return EConstraintSubType::Pulley; }
 	virtual void				NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override;
 	virtual void				SetupVelocityConstraint(float inDeltaTime) override;
+	virtual void				ResetWarmStart() override;
 	virtual void				WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override;
 	virtual bool				SolveVelocityConstraint(float inDeltaTime) override;
 	virtual bool				SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override;

Jolt/Physics/Constraints/RackAndPinionConstraint.cpp

@@ -83,6 +83,11 @@ void RackAndPinionConstraint::SetupVelocityConstraint(float inDeltaTime)
 	CalculateConstraintProperties(rotation1, rotation2);
 }
 
+void RackAndPinionConstraint::ResetWarmStart()
+{
+	mRackAndPinionConstraintPart.Deactivate();
+}
+
 void RackAndPinionConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio)
 {
 	// Warm starting: Apply previous frame impulse

Jolt/Physics/Constraints/RackAndPinionConstraint.h

@@ -63,6 +63,7 @@ public:
 	virtual EConstraintSubType	GetSubType() const override												{ return EConstraintSubType::RackAndPinion; }
 	virtual void				NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override { /* Nothing */ }
 	virtual void				SetupVelocityConstraint(float inDeltaTime) override;
+	virtual void				ResetWarmStart() override;
 	virtual void				WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override;
 	virtual bool				SolveVelocityConstraint(float inDeltaTime) override;
 	virtual bool				SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override;

Jolt/Physics/Constraints/SixDOFConstraint.cpp

@@ -573,6 +573,19 @@ void SixDOFConstraint::SetupVelocityConstraint(float inDeltaTime)
 	}
 }
 
+void SixDOFConstraint::ResetWarmStart()
+{
+	for (AxisConstraintPart &c : mMotorTranslationConstraintPart)
+		c.Deactivate();
+	for (AngleConstraintPart &c : mMotorRotationConstraintPart)
+		c.Deactivate();
+	mRotationConstraintPart.Deactivate();
+	mSwingTwistConstraintPart.Deactivate();
+	mPointConstraintPart.Deactivate();
+	for (AxisConstraintPart &c : mTranslationConstraintPart)
+		c.Deactivate();
+}
+
 void SixDOFConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio)
 {
 	// Warm start translation motors

Jolt/Physics/Constraints/SixDOFConstraint.h

@@ -117,6 +117,7 @@ public:
 	virtual EConstraintSubType	GetSubType() const override									{ return EConstraintSubType::SixDOF; }
 	virtual void				NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override;
 	virtual void				SetupVelocityConstraint(float inDeltaTime) override;
+	virtual void				ResetWarmStart() override;
 	virtual void				WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override;
 	virtual bool				SolveVelocityConstraint(float inDeltaTime) override;
 	virtual bool				SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override;

Jolt/Physics/Constraints/SliderConstraint.cpp

@@ -267,6 +267,14 @@ void SliderConstraint::SetupVelocityConstraint(float inDeltaTime)
 	CalculateMotorConstraintProperties(inDeltaTime);
 }
 
+void SliderConstraint::ResetWarmStart()
+{
+	mMotorConstraintPart.Deactivate();
+	mPositionConstraintPart.Deactivate();
+	mRotationConstraintPart.Deactivate();
+	mPositionLimitsConstraintPart.Deactivate();
+}
+
 void SliderConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio)
 {
 	// Warm starting: Apply previous frame impulse

Jolt/Physics/Constraints/SliderConstraint.h

@@ -76,6 +76,7 @@ public:
 	virtual EConstraintSubType	GetSubType() const override								{ return EConstraintSubType::Slider; }
 	virtual void				NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override;
 	virtual void				SetupVelocityConstraint(float inDeltaTime) override;
+	virtual void				ResetWarmStart() override;
 	virtual void				WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override;
 	virtual bool				SolveVelocityConstraint(float inDeltaTime) override;
 	virtual bool				SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override;

Jolt/Physics/Constraints/SwingTwistConstraint.cpp

@@ -329,6 +329,14 @@ void SwingTwistConstraint::SetupVelocityConstraint(float inDeltaTime)
 	}
 }
 
+void SwingTwistConstraint::ResetWarmStart()
+{
+	for (AngleConstraintPart &c : mMotorConstraintPart)
+		c.Deactivate();
+	mSwingTwistConstraintPart.Deactivate();
+	mPointConstraintPart.Deactivate();
+}
+
 void SwingTwistConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio)
 {
 	// Warm starting: Apply previous frame impulse

Jolt/Physics/Constraints/SwingTwistConstraint.h

@@ -79,6 +79,7 @@ public:
 	virtual EConstraintSubType	GetSubType() const override									{ return EConstraintSubType::SwingTwist; }
 	virtual void				NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override;
 	virtual void				SetupVelocityConstraint(float inDeltaTime) override;
+	virtual void				ResetWarmStart() override;
 	virtual void				WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override;
 	virtual bool				SolveVelocityConstraint(float inDeltaTime) override;
 	virtual bool				SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override;

Jolt/Physics/Ragdoll/Ragdoll.cpp

@@ -588,6 +588,12 @@ void Ragdoll::GetPose(RVec3 &outRootOffset, Mat44 *outJointMatrices, bool inLock
 	}
 }
 
+void Ragdoll::ResetWarmStart()
+{
+	for (TwoBodyConstraint *c : mConstraints)
+		c->ResetWarmStart();
+}
+
 void Ragdoll::DriveToPoseUsingKinematics(const SkeletonPose &inPose, float inDeltaTime, bool inLockBodies)
 {
 	JPH_ASSERT(inPose.GetSkeleton() == mRagdollSettings->mSkeleton);

Jolt/Physics/Ragdoll/Ragdoll.h

@@ -169,6 +169,9 @@ public:
 	/// Lower level version of GetPose that directly returns the world space joint matrices
 	void								GetPose(RVec3 &outRootOffset, Mat44 *outJointMatrices, bool inLockBodies = true);
 
+	/// This function calls ResetWarmStart on all constraints. It can be used after calling SetPose to reset previous frames impulses. See: Constraint::ResetWarmStart.
+	void								ResetWarmStart();
+
 	/// Drive the ragdoll to a specific pose by setting velocities on each of the bodies so that it will reach inPose in inDeltaTime
 	void								DriveToPoseUsingKinematics(const SkeletonPose &inPose, float inDeltaTime, bool inLockBodies = true);
 

Jolt/Physics/Vehicle/VehicleConstraint.cpp

@@ -506,6 +506,19 @@ void VehicleConstraint::SetupVelocityConstraint(float inDeltaTime)
 	CalculatePitchRollConstraintProperties(body_transform);
 }
 
+void VehicleConstraint::ResetWarmStart()
+{
+	for (Wheel *w : mWheels)
+	{
+		w->mSuspensionPart.Deactivate();
+		w->mSuspensionMaxUpPart.Deactivate();
+		w->mLongitudinalPart.Deactivate();
+		w->mLateralPart.Deactivate();
+	}
+
+	mPitchRollPart.Deactivate();
+}
+
 void VehicleConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio)
 {
 	for (Wheel *w : mWheels)

Jolt/Physics/Vehicle/VehicleConstraint.h

@@ -174,6 +174,7 @@ public:
 	virtual bool				IsActive() const override					{ return mIsActive && Constraint::IsActive(); }
 	virtual void				NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override { /* Do nothing */ }
 	virtual void				SetupVelocityConstraint(float inDeltaTime) override;
+	virtual void				ResetWarmStart() override;
 	virtual void				WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override;
 	virtual bool				SolveVelocityConstraint(float inDeltaTime) override;
 	virtual bool				SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override;