Changed the meaning of the number of Constraint::mNumVelocity/PositionStepsOverride (#785)

* Before the number of steps would be the maximum of all constraints and the default value, now an overridden value of 0 means that the constraint uses the default value, otherwise it will use the value as specified. This means that if all constraints in an island have a lower value than the default, we will now use the lower value instead of the default. This allows simulating an island at a lower precision than the default.
* Bodies can now also override the default number of solver iterations. This value is used when the body collides with another body and a contact constraint is created (for constraints, the constraint override is always used).

Jolt/Jolt.cmake

@@ -296,6 +296,7 @@ set(JOLT_PHYSICS_SRC_FILES
 	${JOLT_PHYSICS_ROOT}/Physics/Collision/SortReverseAndStore.h
 	${JOLT_PHYSICS_ROOT}/Physics/Collision/TransformedShape.cpp
 	${JOLT_PHYSICS_ROOT}/Physics/Collision/TransformedShape.h
+	${JOLT_PHYSICS_ROOT}/Physics/Constraints/CalculateSolverSteps.h
 	${JOLT_PHYSICS_ROOT}/Physics/Constraints/ConeConstraint.cpp
 	${JOLT_PHYSICS_ROOT}/Physics/Constraints/ConeConstraint.h
 	${JOLT_PHYSICS_ROOT}/Physics/Constraints/Constraint.cpp

Jolt/Physics/Body/Body.cpp

@@ -345,6 +345,8 @@ BodyCreationSettings Body::GetBodyCreationSettings() const
 	result.mMaxLinearVelocity = mMotionProperties != nullptr? mMotionProperties->GetMaxLinearVelocity() : 0.0f;
 	result.mMaxAngularVelocity = mMotionProperties != nullptr? mMotionProperties->GetMaxAngularVelocity() : 0.0f;
 	result.mGravityFactor = mMotionProperties != nullptr? mMotionProperties->GetGravityFactor() : 1.0f;
+	result.mNumVelocityStepsOverride = mMotionProperties != nullptr? mMotionProperties->GetNumVelocityStepsOverride() : 0;
+	result.mNumPositionStepsOverride = mMotionProperties != nullptr? mMotionProperties->GetNumPositionStepsOverride() : 0;
 	result.mOverrideMassProperties = EOverrideMassProperties::MassAndInertiaProvided;
 	result.mMassPropertiesOverride.mMass = mMotionProperties != nullptr? 1.0f / mMotionProperties->GetInverseMassUnchecked() : FLT_MAX;
 	result.mMassPropertiesOverride.mInertia = mMotionProperties != nullptr? mMotionProperties->GetLocalSpaceInverseInertiaUnchecked().Inversed3x3() : Mat44::sIdentity();

Jolt/Physics/Body/BodyCreationSettings.cpp

@@ -36,6 +36,8 @@ JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(BodyCreationSettings)
 	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mMaxLinearVelocity)
 	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mMaxAngularVelocity)
 	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mGravityFactor)
+	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mNumVelocityStepsOverride)
+	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mNumPositionStepsOverride)
 	JPH_ADD_ENUM_ATTRIBUTE(BodyCreationSettings, mOverrideMassProperties)
 	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mInertiaMultiplier)
 	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mMassPropertiesOverride)
@@ -64,6 +66,8 @@ void BodyCreationSettings::SaveBinaryState(StreamOut &inStream) const
 	inStream.Write(mMaxLinearVelocity);
 	inStream.Write(mMaxAngularVelocity);
 	inStream.Write(mGravityFactor);
+	inStream.Write(mNumVelocityStepsOverride);
+	inStream.Write(mNumPositionStepsOverride);
 	inStream.Write(mOverrideMassProperties);
 	inStream.Write(mInertiaMultiplier);
 	mMassPropertiesOverride.SaveBinaryState(inStream);
@@ -92,6 +96,8 @@ void BodyCreationSettings::RestoreBinaryState(StreamIn &inStream)
 	inStream.Read(mMaxLinearVelocity);
 	inStream.Read(mMaxAngularVelocity);
 	inStream.Read(mGravityFactor);
+	inStream.Read(mNumVelocityStepsOverride);
+	inStream.Read(mNumPositionStepsOverride);
 	inStream.Read(mOverrideMassProperties);
 	inStream.Read(mInertiaMultiplier);
 	mMassPropertiesOverride.RestoreBinaryState(inStream);

Jolt/Physics/Body/BodyCreationSettings.h

@@ -105,6 +105,8 @@ public:
 	float					mMaxLinearVelocity = 500.0f;									///< Maximum linear velocity that this body can reach (m/s)
 	float					mMaxAngularVelocity = 0.25f * JPH_PI * 60.0f;					///< Maximum angular velocity that this body can reach (rad/s)
 	float					mGravityFactor = 1.0f;											///< Value to multiply gravity with for this body
+	uint					mNumVelocityStepsOverride = 0;									///< Used only when this body is dynamic and colliding. Override for the number of solver velocity iterations to run, 0 means use the default in PhysicsSettings::mNumVelocitySteps. The number of iterations to use is the max of all contacts and constraints in the island.
+	uint					mNumPositionStepsOverride = 0;									///< Used only when this body is dynamic and colliding. Override for the number of solver position iterations to run, 0 means use the default in PhysicsSettings::mNumPositionSteps. The number of iterations to use is the max of all contacts and constraints in the island.
 
 	///@name Mass properties of the body (by default calculated by the shape)
 	EOverrideMassProperties	mOverrideMassProperties = EOverrideMassProperties::CalculateMassAndInertia; ///< Determines how mMassPropertiesOverride will be used

Jolt/Physics/Body/BodyManager.cpp

@@ -216,6 +216,8 @@ Body *BodyManager::AllocateBody(const BodyCreationSettings &inBodyCreationSettin
 		mp->SetLinearVelocity(inBodyCreationSettings.mLinearVelocity); // Needs to happen after setting the max linear/angular velocity
 		mp->SetAngularVelocity(inBodyCreationSettings.mAngularVelocity);
 		mp->SetGravityFactor(inBodyCreationSettings.mGravityFactor);
+		mp->SetNumVelocityStepsOverride(inBodyCreationSettings.mNumVelocityStepsOverride);
+		mp->SetNumPositionStepsOverride(inBodyCreationSettings.mNumPositionStepsOverride);
 		mp->mMotionQuality = inBodyCreationSettings.mMotionQuality;
 		mp->mAllowSleeping = inBodyCreationSettings.mAllowSleeping;
 		JPH_IF_ENABLE_ASSERTS(mp->mCachedBodyType = body->mBodyType;)

Jolt/Physics/Body/MotionProperties.h

@@ -148,6 +148,14 @@ public:
 		return Vec3::sAnd(inV, Vec3(allowed_dofs_mask.ReinterpretAsFloat()));
 	}
 
+	/// Used only when this body is dynamic and colliding. Override for the number of solver velocity iterations to run, 0 means use the default in PhysicsSettings::mNumVelocitySteps. The number of iterations to use is the max of all contacts and constraints in the island.
+	void					SetNumVelocityStepsOverride(uint inN)							{ JPH_ASSERT(inN < 256); mNumVelocityStepsOverride = uint8(inN); }
+	uint					GetNumVelocityStepsOverride() const								{ return mNumVelocityStepsOverride; }
+
+	/// Used only when this body is dynamic and colliding. Override for the number of solver position iterations to run, 0 means use the default in PhysicsSettings::mNumPositionSteps. The number of iterations to use is the max of all contacts and constraints in the island.
+	void					SetNumPositionStepsOverride(uint inN)							{ JPH_ASSERT(inN < 256); mNumPositionStepsOverride = uint8(inN); }
+	uint					GetNumPositionStepsOverride() const								{ return mNumPositionStepsOverride; }
+
 	////////////////////////////////////////////////////////////
 	// FUNCTIONS BELOW THIS LINE ARE FOR INTERNAL USE ONLY
 	////////////////////////////////////////////////////////////
@@ -219,6 +227,8 @@ private:
 	EMotionQuality			mMotionQuality;													///< Motion quality, or how well it detects collisions when it has a high velocity
 	bool					mAllowSleeping;													///< If this body can go to sleep
 	EAllowedDOFs			mAllowedDOFs = EAllowedDOFs::All;								///< Allowed degrees of freedom for this body
+	uint8					mNumVelocityStepsOverride = 0;									///< Used only when this body is dynamic and colliding. Override for the number of solver velocity iterations to run, 0 means use the default in PhysicsSettings::mNumVelocitySteps. The number of iterations to use is the max of all contacts and constraints in the island.
+	uint8					mNumPositionStepsOverride = 0;									///< Used only when this body is dynamic and colliding. Override for the number of solver position iterations to run, 0 means use the default in PhysicsSettings::mNumPositionSteps. The number of iterations to use is the max of all contacts and constraints in the island.
 
 	// 3rd cache line (least frequently used)
 	// 4 byte aligned (or 8 byte if running in double precision)

Jolt/Physics/Constraints/CalculateSolverSteps.h

@@ -0,0 +1,66 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/PhysicsSettings.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Class used to calculate the total number of velocity and position steps
+class CalculateSolverSteps
+{
+public:
+	/// Constructor
+	JPH_INLINE explicit			CalculateSolverSteps(const PhysicsSettings &inSettings) : mSettings(inSettings) { }
+
+	/// Combine the number of velocity and position steps for this body/constraint with the current values
+	template <class Type>
+	JPH_INLINE void				operator () (const Type *inObject)
+	{
+		uint num_velocity_steps = inObject->GetNumVelocityStepsOverride();
+		mNumVelocitySteps = max(mNumVelocitySteps, num_velocity_steps);
+		mApplyDefaultVelocity |= num_velocity_steps == 0;
+
+		uint num_position_steps = inObject->GetNumPositionStepsOverride();
+		mNumPositionSteps = max(mNumPositionSteps, num_position_steps);
+		mApplyDefaultPosition |= num_position_steps == 0;
+	}
+
+	/// Must be called after all bodies/constraints have been processed
+	JPH_INLINE void				Finalize()
+	{
+		// If we have a default velocity/position step count, take the max of the default and the overrides
+		if (mApplyDefaultVelocity)
+			mNumVelocitySteps = max(mNumVelocitySteps, mSettings.mNumVelocitySteps);
+		if (mApplyDefaultPosition)
+			mNumPositionSteps = max(mNumPositionSteps, mSettings.mNumPositionSteps);
+	}
+
+	/// Get the results of the calculation
+	JPH_INLINE uint				GetNumPositionSteps() const					{ return mNumPositionSteps; }
+	JPH_INLINE uint				GetNumVelocitySteps() const					{ return mNumVelocitySteps; }
+
+private:
+	const PhysicsSettings &		mSettings;
+
+	uint						mNumVelocitySteps = 0;
+	uint						mNumPositionSteps = 0;
+
+	bool						mApplyDefaultVelocity = false;
+	bool						mApplyDefaultPosition = false;
+};
+
+/// Dummy class to replace the steps calculator when we don't need the result
+class DummyCalculateSolverSteps
+{
+public:
+	template <class Type>
+	JPH_INLINE void				operator () (const Type *) const
+	{
+		/* Nothing to do */
+	}
+};
+
+JPH_NAMESPACE_END

Jolt/Physics/Constraints/Constraint.h

@@ -81,11 +81,11 @@ public:
 	/// Note that if you want a deterministic simulation and you cannot guarantee the order in which constraints are added/removed, you can make the priority for all constraints unique to get a deterministic ordering.
 	uint32						mConstraintPriority = 0;
 
-	/// Override for the number of solver velocity iterations to run, the total amount of iterations is the max of PhysicsSettings::mNumVelocitySteps and this for all constraints in the island.
-	int							mNumVelocityStepsOverride = 0;
+	/// Used only when the constraint is active. Override for the number of solver velocity iterations to run, 0 means use the default in PhysicsSettings::mNumVelocitySteps. The number of iterations to use is the max of all contacts and constraints in the island.
+	uint						mNumVelocityStepsOverride = 0;
 
-	/// Override for the number of position velocity iterations to run, the total amount of iterations is the max of PhysicsSettings::mNumPositionSteps and this for all constraints in the island.
-	int							mNumPositionStepsOverride = 0;
+	/// Used only when the constraint is active. Override for the number of solver position iterations to run, 0 means use the default in PhysicsSettings::mNumPositionSteps. The number of iterations to use is the max of all contacts and constraints in the island.
+	uint						mNumPositionStepsOverride = 0;
 
 	/// Size of constraint when drawing it through the debug renderer
 	float						mDrawConstraintSize = 1.0f;
@@ -110,11 +110,13 @@ public:
 		mDrawConstraintSize(inSettings.mDrawConstraintSize),
 #endif // JPH_DEBUG_RENDERER
 		mConstraintPriority(inSettings.mConstraintPriority),
-		mNumVelocityStepsOverride(inSettings.mNumVelocityStepsOverride),
-		mNumPositionStepsOverride(inSettings.mNumPositionStepsOverride),
+		mNumVelocityStepsOverride(uint8(inSettings.mNumVelocityStepsOverride)),
+		mNumPositionStepsOverride(uint8(inSettings.mNumPositionStepsOverride)),
 		mEnabled(inSettings.mEnabled),
 		mUserData(inSettings.mUserData)
 	{
+		JPH_ASSERT(inSettings.mNumVelocityStepsOverride < 256);
+		JPH_ASSERT(inSettings.mNumPositionStepsOverride < 256);
 	}
 
 	/// Virtual destructor
@@ -131,13 +133,13 @@ public:
 	uint32						GetConstraintPriority() const				{ return mConstraintPriority; }
 	void						SetConstraintPriority(uint32 inPriority)	{ mConstraintPriority = inPriority; }
 
-	/// Override for the number of solver velocity iterations to run, the total amount of iterations is the max of PhysicsSettings::mNumVelocitySteps and this for all constraints in the island.
-	void						SetNumVelocityStepsOverride(int inN)		{ mNumVelocityStepsOverride = inN; }
-	int							GetNumVelocityStepsOverride() const			{ return mNumVelocityStepsOverride; }
+	/// Used only when the constraint is active. Override for the number of solver velocity iterations to run, 0 means use the default in PhysicsSettings::mNumVelocitySteps. The number of iterations to use is the max of all contacts and constraints in the island.
+	void						SetNumVelocityStepsOverride(uint inN)		{ JPH_ASSERT(inN < 256); mNumVelocityStepsOverride = uint8(inN); }
+	uint						GetNumVelocityStepsOverride() const			{ return mNumVelocityStepsOverride; }
 
-	/// Override for the number of position velocity iterations to run, the total amount of iterations is the max of PhysicsSettings::mNumPositionSteps and this for all constraints in the island.
-	void						SetNumPositionStepsOverride(int inN)		{ mNumPositionStepsOverride = inN; }
-	int							GetNumPositionStepsOverride() const			{ return mNumPositionStepsOverride; }
+	/// Used only when the constraint is active. Override for the number of solver position iterations to run, 0 means use the default in PhysicsSettings::mNumPositionSteps. The number of iterations to use is the max of all contacts and constraints in the island.
+	void						SetNumPositionStepsOverride(uint inN)		{ JPH_ASSERT(inN < 256); mNumPositionStepsOverride = uint8(inN); }
+	uint						GetNumPositionStepsOverride() const			{ return mNumPositionStepsOverride; }
 
 	/// Enable / disable this constraint. This can e.g. be used to implement a breakable constraint by detecting that the constraint impulse
 	/// (see e.g. PointConstraint::GetTotalLambdaPosition) went over a certain limit and then disabling the constraint.
@@ -214,11 +216,11 @@ private:
 	/// Priority of the constraint when solving. Higher numbers have are more likely to be solved correctly.
 	uint32						mConstraintPriority = 0;
 
-	/// Override for the number of solver velocity iterations to run, the total amount of iterations is the max of PhysicsSettings::mNumVelocitySteps and this for all constraints in the island.
-	int							mNumVelocityStepsOverride = 0;
+	/// Used only when the constraint is active. Override for the number of solver velocity iterations to run, 0 means use the default in PhysicsSettings::mNumVelocitySteps. The number of iterations to use is the max of all contacts and constraints in the island.
+	uint8						mNumVelocityStepsOverride = 0;
 
-	/// Override for the number of position velocity iterations to run, the total amount of iterations is the max of PhysicsSettings::mNumPositionSteps and this for all constraints in the island.
-	int							mNumPositionStepsOverride = 0;
+	/// Used only when the constraint is active. Override for the number of solver position iterations to run, 0 means use the default in PhysicsSettings::mNumPositionSteps. The number of iterations to use is the max of all contacts and constraints in the island.
+	uint8						mNumPositionStepsOverride = 0;
 
 	/// If this constraint is currently enabled
 	bool						mEnabled = true;

Jolt/Physics/Constraints/ConstraintManager.cpp

@@ -5,6 +5,7 @@
 #include <Jolt/Jolt.h>
 
 #include <Jolt/Physics/Constraints/ConstraintManager.h>
+#include <Jolt/Physics/Constraints/CalculateSolverSteps.h>
 #include <Jolt/Physics/IslandBuilder.h>
 #include <Jolt/Physics/StateRecorder.h>
 #include <Jolt/Physics/PhysicsLock.h>
@@ -122,28 +123,22 @@ void ConstraintManager::sSetupVelocityConstraints(Constraint **inActiveConstrain
 		(*c)->SetupVelocityConstraint(inDeltaTime);
 }
 
-void ConstraintManager::sWarmStartVelocityConstraints(Constraint **inActiveConstraints, const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inWarmStartImpulseRatio)
+template <class ConstraintCallback>
+void ConstraintManager::sWarmStartVelocityConstraints(Constraint **inActiveConstraints, const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inWarmStartImpulseRatio, ConstraintCallback &ioCallback)
 {
 	JPH_PROFILE_FUNCTION();
 
 	for (const uint32 *constraint_idx = inConstraintIdxBegin; constraint_idx < inConstraintIdxEnd; ++constraint_idx)
 	{
 		Constraint *c = inActiveConstraints[*constraint_idx];
+		ioCallback(c);
 		c->WarmStartVelocityConstraint(inWarmStartImpulseRatio);
 	}
 }
 
-void ConstraintManager::sWarmStartVelocityConstraints(Constraint **inActiveConstraints, const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inWarmStartImpulseRatio, int &ioNumVelocitySteps)
-{
-	JPH_PROFILE_FUNCTION();
-
-	for (const uint32 *constraint_idx = inConstraintIdxBegin; constraint_idx < inConstraintIdxEnd; ++constraint_idx)
-	{
-		Constraint *c = inActiveConstraints[*constraint_idx];
-		ioNumVelocitySteps = max(ioNumVelocitySteps, c->GetNumVelocityStepsOverride());
-		c->WarmStartVelocityConstraint(inWarmStartImpulseRatio);
-	}
-}
+// Specialize for the two constraint callback types
+template void ConstraintManager::sWarmStartVelocityConstraints<CalculateSolverSteps>(Constraint **inActiveConstraints, const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inWarmStartImpulseRatio, CalculateSolverSteps &ioCallback);
+template void ConstraintManager::sWarmStartVelocityConstraints<DummyCalculateSolverSteps>(Constraint **inActiveConstraints, const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inWarmStartImpulseRatio, DummyCalculateSolverSteps &ioCallback);
 
 bool ConstraintManager::sSolveVelocityConstraints(Constraint **inActiveConstraints, const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inDeltaTime)
 {
@@ -175,22 +170,6 @@ bool ConstraintManager::sSolvePositionConstraints(Constraint **inActiveConstrain
 	return any_impulse_applied;
 }
 
-bool ConstraintManager::sSolvePositionConstraints(Constraint **inActiveConstraints, const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inDeltaTime, float inBaumgarte, int &ioNumPositionSteps)
-{
-	JPH_PROFILE_FUNCTION();
-
-	bool any_impulse_applied = false;
-
-	for (const uint32 *constraint_idx = inConstraintIdxBegin; constraint_idx < inConstraintIdxEnd; ++constraint_idx)
-	{
-		Constraint *c = inActiveConstraints[*constraint_idx];
-		ioNumPositionSteps = max(ioNumPositionSteps, c->GetNumPositionStepsOverride());
-		any_impulse_applied |= c->SolvePositionConstraint(inDeltaTime, inBaumgarte);
-	}
-
-	return any_impulse_applied;
-}
-
 #ifdef JPH_DEBUG_RENDERER
 void ConstraintManager::DrawConstraints(DebugRenderer *inRenderer) const
 {

Jolt/Physics/Constraints/ConstraintManager.h

@@ -58,10 +58,8 @@ public:
 	static void				sSetupVelocityConstraints(Constraint **inActiveConstraints, uint32 inNumActiveConstraints, float inDeltaTime);
 
 	/// Apply last frame's impulses, must be called prior to SolveVelocityConstraints
-	static void				sWarmStartVelocityConstraints(Constraint **inActiveConstraints, const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inWarmStartImpulseRatio);
-
-	/// Same as above but also calculates the number of velocity steps
-	static void				sWarmStartVelocityConstraints(Constraint **inActiveConstraints, const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inWarmStartImpulseRatio, int &ioNumVelocitySteps);
+	template <class ConstraintCallback>
+	static void				sWarmStartVelocityConstraints(Constraint **inActiveConstraints, const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inWarmStartImpulseRatio, ConstraintCallback &ioCallback);
 
 	/// This function is called multiple times to iteratively come to a solution that meets all velocity constraints
 	static bool				sSolveVelocityConstraints(Constraint **inActiveConstraints, const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inDeltaTime);
@@ -69,9 +67,6 @@ public:
 	/// This function is called multiple times to iteratively come to a solution that meets all position constraints
 	static bool				sSolvePositionConstraints(Constraint **inActiveConstraints, const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inDeltaTime, float inBaumgarte);
 
-	/// Same as above but also calculates the number of position steps
-	static bool				sSolvePositionConstraints(Constraint **inActiveConstraints, const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inDeltaTime, float inBaumgarte, int &ioNumPositionSteps);
-
 #ifdef JPH_DEBUG_RENDERER
 	/// Draw all constraints
 	void					DrawConstraints(DebugRenderer *inRenderer) const;

Jolt/Physics/Constraints/ContactConstraintManager.cpp

@@ -5,6 +5,7 @@
 #include <Jolt/Jolt.h>
 
 #include <Jolt/Physics/Constraints/ContactConstraintManager.h>
+#include <Jolt/Physics/Constraints/CalculateSolverSteps.h>
 #include <Jolt/Physics/Body/Body.h>
 #include <Jolt/Physics/PhysicsUpdateContext.h>
 #include <Jolt/Physics/PhysicsSettings.h>
@@ -1457,7 +1458,8 @@ JPH_INLINE void ContactConstraintManager::sWarmStartConstraint(ContactConstraint
 	}
 }
 
-void ContactConstraintManager::WarmStartVelocityConstraints(const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inWarmStartImpulseRatio)
+template <class MotionPropertiesCallback>
+void ContactConstraintManager::WarmStartVelocityConstraints(const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inWarmStartImpulseRatio, MotionPropertiesCallback &ioCallback)
 {
 	JPH_PROFILE_FUNCTION();
 
@@ -1479,18 +1481,31 @@ void ContactConstraintManager::WarmStartVelocityConstraints(const uint32 *inCons
 		if (motion_type1 == EMotionType::Dynamic)
 		{
 			if (motion_type2 == EMotionType::Dynamic)
+			{
 				sWarmStartConstraint<EMotionType::Dynamic, EMotionType::Dynamic>(constraint, motion_properties1, motion_properties2, inWarmStartImpulseRatio);
+
+				ioCallback(motion_properties2);
+			}
 			else
 				sWarmStartConstraint<EMotionType::Dynamic, EMotionType::Static>(constraint, motion_properties1, motion_properties2, inWarmStartImpulseRatio);
+
+			ioCallback(motion_properties1);
 		}
 		else
 		{
 			JPH_ASSERT(motion_type2 == EMotionType::Dynamic);
+
 			sWarmStartConstraint<EMotionType::Static, EMotionType::Dynamic>(constraint, motion_properties1, motion_properties2, inWarmStartImpulseRatio);
+
+			ioCallback(motion_properties2);
 		}
 	}
 }
 
+// Specialize for the two body callback types
+template void ContactConstraintManager::WarmStartVelocityConstraints<CalculateSolverSteps>(const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inWarmStartImpulseRatio, CalculateSolverSteps &ioCallback);
+template void ContactConstraintManager::WarmStartVelocityConstraints<DummyCalculateSolverSteps>(const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inWarmStartImpulseRatio, DummyCalculateSolverSteps &ioCallback);
+
 template <EMotionType Type1, EMotionType Type2>
 JPH_INLINE bool ContactConstraintManager::sSolveVelocityConstraint(ContactConstraint &ioConstraint, MotionProperties *ioMotionProperties1, MotionProperties *ioMotionProperties2)
 {

Jolt/Physics/Constraints/ContactConstraintManager.h

@@ -169,7 +169,8 @@ public:
 	}
 
 	/// Apply last frame's impulses as an initial guess for this frame's impulses
-	void						WarmStartVelocityConstraints(const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inWarmStartImpulseRatio);
+	template <class MotionPropertiesCallback>
+	void						WarmStartVelocityConstraints(const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inWarmStartImpulseRatio, MotionPropertiesCallback &ioCallback);
 
 	/// Solve velocity constraints, when almost nothing changes this should only apply very small impulses
 	/// since we're warm starting with the total impulse applied in the last frame above.

Jolt/Physics/IslandBuilder.cpp

@@ -382,6 +382,8 @@ void IslandBuilder::Finalize(const BodyID *inActiveBodies, uint32 inNumActiveBod
 	BuildConstraintIslands(mConstraintLinks, mNumConstraints, mConstraintIslands, mConstraintIslandEnds, inTempAllocator);
 	BuildConstraintIslands(mContactLinks, mNumContacts, mContactIslands, mContactIslandEnds, inTempAllocator);
 	SortIslands(inTempAllocator);
+
+	mNumPositionSteps = (uint8 *)inTempAllocator->Allocate(mNumIslands * sizeof(uint8));
 }
 
 void IslandBuilder::GetBodiesInIsland(uint32 inIslandIndex, BodyID *&outBodiesBegin, BodyID *&outBodiesEnd) const
@@ -432,6 +434,8 @@ void IslandBuilder::ResetIslands(TempAllocator *inTempAllocator)
 {
 	JPH_PROFILE_FUNCTION();
 
+	inTempAllocator->Free(mNumPositionSteps, mNumIslands * sizeof(uint8));
+
 	if (mIslandsSorted != nullptr)
 	{
 		inTempAllocator->Free(mIslandsSorted, mNumIslands * sizeof(uint32));

Jolt/Physics/IslandBuilder.h

@@ -50,6 +50,10 @@ public:
 	bool					GetConstraintsInIsland(uint32 inIslandIndex, uint32 *&outConstraintsBegin, uint32 *&outConstraintsEnd) const;
 	bool					GetContactsInIsland(uint32 inIslandIndex, uint32 *&outContactsBegin, uint32 *&outContactsEnd) const;
 
+	/// The number of position iterations for each island
+	void					SetNumPositionSteps(uint32 inIslandIndex, uint inNumPositionSteps)	{ JPH_ASSERT(inIslandIndex < mNumIslands); JPH_ASSERT(inNumPositionSteps < 256); mNumPositionSteps[inIslandIndex] = uint8(inNumPositionSteps); }
+	uint					GetNumPositionSteps(uint32 inIslandIndex) const						{ JPH_ASSERT(inIslandIndex < mNumIslands); return mNumPositionSteps[inIslandIndex]; }
+
 	/// After you're done calling the three functions above, call this function to free associated data
 	void					ResetIslands(TempAllocator *inTempAllocator);
 
@@ -95,6 +99,8 @@ private:
 
 	uint32 *				mIslandsSorted = nullptr;						///< A list of island indices in order of most constraints first
 
+	uint8 *					mNumPositionSteps = nullptr;					///< Number of position steps for each island
+
 	// Counters
 	uint32					mMaxActiveBodies;								///< Maximum size of the active bodies list (see BodyManager::mActiveBodies)
 	uint32					mNumActiveBodies = 0;							///< Number of active bodies passed to

Jolt/Physics/LargeIslandSplitter.cpp

@@ -5,6 +5,7 @@
 
 #include <Jolt/Physics/LargeIslandSplitter.h>
 #include <Jolt/Physics/IslandBuilder.h>
+#include <Jolt/Physics/Constraints/CalculateSolverSteps.h>
 #include <Jolt/Physics/Constraints/Constraint.h>
 #include <Jolt/Physics/Constraints/ContactConstraintManager.h>
 #include <Jolt/Physics/Body/BodyManager.h>
@@ -279,7 +280,7 @@ uint LargeIslandSplitter::AssignToNonParallelSplit(const Body *inBody)
 	return cNonParallelSplitIdx;
 }
 
-bool LargeIslandSplitter::SplitIsland(uint32 inIslandIndex, const IslandBuilder &inIslandBuilder, const BodyManager &inBodyManager, const ContactConstraintManager &inContactManager, Constraint **inActiveConstraints, int inNumVelocitySteps, int inNumPositionSteps)
+bool LargeIslandSplitter::SplitIsland(uint32 inIslandIndex, const IslandBuilder &inIslandBuilder, const BodyManager &inBodyManager, const ContactConstraintManager &inContactManager, Constraint **inActiveConstraints, CalculateSolverSteps &ioStepsCalculator)
 {
 	JPH_PROFILE_FUNCTION();
 
@@ -325,6 +326,11 @@ bool LargeIslandSplitter::SplitIsland(uint32 inIslandIndex, const IslandBuilder
 		uint split = AssignSplit(body1, body2);
 		num_contacts_in_split[split]++;
 		*cur_contact_split_idx++ = split;
+
+		if (body1->IsDynamic())
+			ioStepsCalculator(body1->GetMotionPropertiesUnchecked());
+		if (body2->IsDynamic())
+			ioStepsCalculator(body2->GetMotionPropertiesUnchecked());
 	}
 
 	// Assign the constraints to a split
@@ -333,12 +339,14 @@ bool LargeIslandSplitter::SplitIsland(uint32 inIslandIndex, const IslandBuilder
 	{
 		const Constraint *constraint = inActiveConstraints[*c];
 		uint split = constraint->BuildIslandSplits(*this);
-		inNumVelocitySteps = max(inNumVelocitySteps, constraint->GetNumVelocityStepsOverride());
-		inNumPositionSteps = max(inNumPositionSteps, constraint->GetNumPositionStepsOverride());
 		num_constraints_in_split[split]++;
 		*cur_constraint_split_idx++ = split;
+
+		ioStepsCalculator(constraint);
 	}
 
+	ioStepsCalculator.Finalize();
+
 	// Start with 0 splits
 	uint split_remap_table[cNumSplits];
 	uint new_split_idx = mNextSplitIsland.fetch_add(1, memory_order_relaxed);
@@ -346,9 +354,9 @@ bool LargeIslandSplitter::SplitIsland(uint32 inIslandIndex, const IslandBuilder
 	Splits &splits = mSplitIslands[new_split_idx];
 	splits.mIslandIndex = inIslandIndex;
 	splits.mNumSplits = 0;
-	splits.mNumIterations = inNumVelocitySteps + 1; // Iteration 0 is used for warm starting
-	splits.mNumVelocitySteps = inNumVelocitySteps;
-	splits.mNumPositionSteps = inNumPositionSteps;
+	splits.mNumIterations = ioStepsCalculator.GetNumVelocitySteps() + 1; // Iteration 0 is used for warm starting
+	splits.mNumVelocitySteps = ioStepsCalculator.GetNumVelocitySteps();
+	splits.mNumPositionSteps = ioStepsCalculator.GetNumPositionSteps();
 	splits.mItemsProcessed.store(0, memory_order_release);
 
 	// Allocate space to store the sorted constraint and contact indices per split

Jolt/Physics/LargeIslandSplitter.h

@@ -15,6 +15,7 @@ class TempAllocator;
 class Constraint;
 class BodyManager;
 class ContactConstraintManager;
+class CalculateSolverSteps;
 
 /// Assigns bodies in large islands to multiple groups that can run in parallel
 ///
@@ -142,7 +143,7 @@ public:
 	uint					AssignToNonParallelSplit(const Body *inBody);
 
 	/// Splits up an island, the created splits will be added to the list of batches and can be fetched with FetchNextBatch. Returns false if the island did not need splitting.
-	bool					SplitIsland(uint32 inIslandIndex, const IslandBuilder &inIslandBuilder, const BodyManager &inBodyManager, const ContactConstraintManager &inContactManager, Constraint **inActiveConstraints, int inNumVelocitySteps, int inNumPositionSteps);
+	bool					SplitIsland(uint32 inIslandIndex, const IslandBuilder &inIslandBuilder, const BodyManager &inBodyManager, const ContactConstraintManager &inContactManager, Constraint **inActiveConstraints, CalculateSolverSteps &ioStepsCalculator);
 
 	/// Fetch the next batch to process, returns a handle in outSplitIslandIndex that must be provided to MarkBatchProcessed when complete
 	EStatus					FetchNextBatch(uint &outSplitIslandIndex, uint32 *&outConstraintsBegin, uint32 *&outConstraintsEnd, uint32 *&outContactsBegin, uint32 *&outContactsEnd, bool &outFirstIteration);

Jolt/Physics/PhysicsSettings.h

@@ -75,10 +75,10 @@ struct PhysicsSettings
 
 	/// Number of solver velocity iterations to run
 	/// Note that this needs to be >= 2 in order for friction to work (friction is applied using the non-penetration impulse from the previous iteration)
-	int			mNumVelocitySteps = 10;
+	uint		mNumVelocitySteps = 10;
 
 	/// Number of solver position iterations to run
-	int			mNumPositionSteps = 2;
+	uint		mNumPositionSteps = 2;
 
 	/// Minimal velocity needed before a collision can be elastic (unit: m)
 	float		mMinVelocityForRestitution = 1.0f;

Jolt/Physics/PhysicsSystem.cpp

@@ -19,6 +19,7 @@
 #include <Jolt/Physics/Collision/CollideConvexVsTriangles.h>
 #include <Jolt/Physics/Collision/ManifoldBetweenTwoFaces.h>
 #include <Jolt/Physics/Collision/Shape/ConvexShape.h>
+#include <Jolt/Physics/Constraints/CalculateSolverSteps.h>
 #include <Jolt/Physics/Constraints/ConstraintPart/AxisConstraintPart.h>
 #include <Jolt/Physics/DeterminismLog.h>
 #include <Jolt/Physics/SoftBody/SoftBodyMotionProperties.h>
@@ -1258,6 +1259,9 @@ void PhysicsSystem::JobBodySetIslandIndex()
 	}
 }
 
+JPH_SUPPRESS_WARNING_PUSH
+JPH_CLANG_SUPPRESS_WARNING("-Wundefined-func-template") // ConstraintManager::sWarmStartVelocityConstraints / ContactConstraintManager::WarmStartVelocityConstraints is instantiated in the cpp file
+
 void PhysicsSystem::JobSolveVelocityConstraints(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep)
 {
 #ifdef JPH_ENABLE_ASSERTS
@@ -1287,8 +1291,9 @@ void PhysicsSystem::JobSolveVelocityConstraints(PhysicsUpdateContext *ioContext,
 					if (first_iteration)
 					{
 						// Iteration 0 is used to warm start the batch (we added 1 to the number of iterations in LargeIslandSplitter::SplitIsland)
-						ConstraintManager::sWarmStartVelocityConstraints(active_constraints, constraints_begin, constraints_end, warm_start_impulse_ratio);
-						mContactManager.WarmStartVelocityConstraints(contacts_begin, contacts_end, warm_start_impulse_ratio);
+						DummyCalculateSolverSteps dummy;
+						ConstraintManager::sWarmStartVelocityConstraints(active_constraints, constraints_begin, constraints_end, warm_start_impulse_ratio, dummy);
+						mContactManager.WarmStartVelocityConstraints(contacts_begin, contacts_end, warm_start_impulse_ratio, dummy);
 					}
 					else
 					{
@@ -1363,17 +1368,21 @@ void PhysicsSystem::JobSolveVelocityConstraints(PhysicsUpdateContext *ioContext,
 			}
 
 			// Split up large islands
-			int num_velocity_steps = mPhysicsSettings.mNumVelocitySteps;
+			CalculateSolverSteps steps_calculator(mPhysicsSettings);
 			if (mPhysicsSettings.mUseLargeIslandSplitter
-				&& mLargeIslandSplitter.SplitIsland(island_idx, mIslandBuilder, mBodyManager, mContactManager, active_constraints, num_velocity_steps, mPhysicsSettings.mNumPositionSteps))
+				&& mLargeIslandSplitter.SplitIsland(island_idx, mIslandBuilder, mBodyManager, mContactManager, active_constraints, steps_calculator))
 				continue; // Loop again to try to fetch the newly split island
 
 			// We didn't create a split, just run the solver now for this entire island. Begin by warm starting.
-			ConstraintManager::sWarmStartVelocityConstraints(active_constraints, constraints_begin, constraints_end, warm_start_impulse_ratio, num_velocity_steps);
-			mContactManager.WarmStartVelocityConstraints(contacts_begin, contacts_end, warm_start_impulse_ratio);
+			ConstraintManager::sWarmStartVelocityConstraints(active_constraints, constraints_begin, constraints_end, warm_start_impulse_ratio, steps_calculator);
+			mContactManager.WarmStartVelocityConstraints(contacts_begin, contacts_end, warm_start_impulse_ratio, steps_calculator);
+			steps_calculator.Finalize();
+
+			// Store the number of position steps for later
+			mIslandBuilder.SetNumPositionSteps(island_idx, steps_calculator.GetNumPositionSteps());
 
 			// Solve velocity constraints
-			for (int velocity_step = 0; velocity_step < num_velocity_steps; ++velocity_step)
+			for (uint velocity_step = 0; velocity_step < steps_calculator.GetNumVelocitySteps(); ++velocity_step)
 			{
 				bool applied_impulse = ConstraintManager::sSolveVelocityConstraints(active_constraints, constraints_begin, constraints_end, delta_time);
 				applied_impulse |= mContactManager.SolveVelocityConstraints(contacts_begin, contacts_end);
@@ -1394,6 +1403,8 @@ void PhysicsSystem::JobSolveVelocityConstraints(PhysicsUpdateContext *ioContext,
 	while (check_islands || check_split_islands);
 }
 
+JPH_SUPPRESS_WARNING_POP
+
 void PhysicsSystem::JobPreIntegrateVelocity(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep)
 {
 	// Reserve enough space for all bodies that may need a cast
@@ -2301,29 +2312,9 @@ void PhysicsSystem::JobSolvePositionConstraints(PhysicsUpdateContext *ioContext,
 			// Check if this island needs solving
 			if (num_items > 0)
 			{
-				// First iteration
-				int num_position_steps = mPhysicsSettings.mNumPositionSteps;
-				if (num_position_steps > 0)
-				{
-					// In the first iteration also calculate the number of position steps (this way we avoid pulling all constraints into the cache twice)
-					bool applied_impulse = ConstraintManager::sSolvePositionConstraints(active_constraints, constraints_begin, constraints_end, delta_time, baumgarte, num_position_steps);
-					applied_impulse |= mContactManager.SolvePositionConstraints(contacts_begin, contacts_end);
-
-					// If no impulses were applied we can stop, otherwise we already did 1 iteration
-					if (!applied_impulse)
-						num_position_steps = 0;
-					else
-						--num_position_steps;
-				}
-				else
-				{
-					// Iterate the constraints to see if they override the number of position steps
-					for (const uint32 *c = constraints_begin; c < constraints_end; ++c)
-						num_position_steps = max(num_position_steps, active_constraints[*c]->GetNumPositionStepsOverride());
-				}
-
-				// Further iterations
-				for (int position_step = 0; position_step < num_position_steps; ++position_step)
+				// Iterate
+				uint num_position_steps = mIslandBuilder.GetNumPositionSteps(island_idx);
+				for (uint position_step = 0; position_step < num_position_steps; ++position_step)
 				{
 					bool applied_impulse = ConstraintManager::sSolvePositionConstraints(active_constraints, constraints_begin, constraints_end, delta_time, baumgarte);
 					applied_impulse |= mContactManager.SolvePositionConstraints(contacts_begin, contacts_end);