Ability to override the number of velocity/position steps per constraint (#223)

Jolt/Core/Core.h

@@ -303,8 +303,10 @@ static_assert(sizeof(void *) == (JPH_CPU_ADDRESS_BITS == 64? 8 : 4), "Invalid si
 // Shorthand for #ifdef _DEBUG / #endif
 #ifdef _DEBUG
 	#define JPH_IF_DEBUG(...)	__VA_ARGS__
+	#define JPH_IF_NOT_DEBUG(...)
 #else
 	#define JPH_IF_DEBUG(...)
+	#define JPH_IF_NOT_DEBUG(...) __VA_ARGS__
 #endif
 
 // Shorthand for #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED / #endif

Jolt/Physics/Constraints/Constraint.cpp

@@ -18,6 +18,8 @@ JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(ConstraintSettings)
 
 	JPH_ADD_ATTRIBUTE(ConstraintSettings, mEnabled)
 	JPH_ADD_ATTRIBUTE(ConstraintSettings, mDrawConstraintSize)
+	JPH_ADD_ATTRIBUTE(ConstraintSettings, mNumVelocityStepsOverride)
+	JPH_ADD_ATTRIBUTE(ConstraintSettings, mNumPositionStepsOverride)
 }
 
 void ConstraintSettings::SaveBinaryState(StreamOut &inStream) const
@@ -25,6 +27,8 @@ void ConstraintSettings::SaveBinaryState(StreamOut &inStream) const
 	inStream.Write(GetRTTI()->GetHash());
 	inStream.Write(mEnabled);
 	inStream.Write(mDrawConstraintSize);
+	inStream.Write(mNumVelocityStepsOverride);
+	inStream.Write(mNumPositionStepsOverride);
 }
 
 void ConstraintSettings::RestoreBinaryState(StreamIn &inStream)
@@ -32,6 +36,8 @@ void ConstraintSettings::RestoreBinaryState(StreamIn &inStream)
 	// Type hash read by sRestoreFromBinaryState
 	inStream.Read(mEnabled);
 	inStream.Read(mDrawConstraintSize);
+	inStream.Read(mNumVelocityStepsOverride);
+	inStream.Read(mNumPositionStepsOverride);
 }
 
 ConstraintSettings::ConstraintResult ConstraintSettings::sRestoreFromBinaryState(StreamIn &inStream)
@@ -81,7 +87,8 @@ void Constraint::RestoreState(StateRecorder &inStream)
 void Constraint::ToConstraintSettings(ConstraintSettings &outSettings) const
 {
 	outSettings.mEnabled = mEnabled;
-
+	outSettings.mNumVelocityStepsOverride = mNumVelocityStepsOverride;
+	outSettings.mNumPositionStepsOverride = mNumPositionStepsOverride;
 #ifdef JPH_DEBUG_RENDERER
 	outSettings.mDrawConstraintSize = mDrawConstraintSize;
 #endif // JPH_DEBUG_RENDERER

Jolt/Physics/Constraints/Constraint.h

@@ -73,6 +73,12 @@ public:
 	/// If this constraint is enabled initially. Use Constraint::SetEnabled to toggle after creation.
 	bool						mEnabled = true;
 
+	/// 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;
+
+	/// 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;
+
 	/// Size of constraint when drawing it through the debug renderer
 	float						mDrawConstraintSize = 1.0f;
 
@@ -92,6 +98,8 @@ public:
 #ifdef JPH_DEBUG_RENDERER
 		mDrawConstraintSize(inSettings.mDrawConstraintSize),
 #endif // JPH_DEBUG_RENDERER
+		mNumVelocityStepsOverride(inSettings.mNumVelocityStepsOverride),
+		mNumPositionStepsOverride(inSettings.mNumPositionStepsOverride),
 		mEnabled(inSettings.mEnabled)
 	{
 	}
@@ -105,6 +113,14 @@ public:
 	/// Get the sub type of a constraint
 	virtual EConstraintSubType	GetSubType() const = 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.
+	void						SetNumVelocityStepsOverride(int inN)		{ mNumVelocityStepsOverride = inN; }
+	int							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; }
+
 	/// 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.
 	/// Note that although a disabled constraint will not affect the simulation in any way anymore, it does incur some processing overhead.
@@ -164,6 +180,12 @@ private:
 	/// Index in the mConstraints list of the ConstraintManager for easy finding
 	uint32						mConstraintIndex = cInvalidConstraintIndex;
 
+	/// 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;
+
+	/// 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;
+
 	/// If this constraint is currently enabled
 	bool						mEnabled = true;
 };

Jolt/Physics/Constraints/ConstraintManager.cpp

@@ -124,13 +124,14 @@ void ConstraintManager::sSetupVelocityConstraints(Constraint **inActiveConstrain
 	}
 }
 
-void ConstraintManager::sWarmStartVelocityConstraints(Constraint **inActiveConstraints, const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float 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);
 	}
 }
@@ -165,6 +166,22 @@ 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

@@ -54,7 +54,7 @@ public:
 	static void				sSetupVelocityConstraints(Constraint **inActiveConstraints, const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, 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);
+	static void				sWarmStartVelocityConstraints(Constraint **inActiveConstraints, const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inWarmStartImpulseRatio, int &ioNumVelocitySteps);
 
 	/// 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);
@@ -62,6 +62,9 @@ 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/PhysicsSystem.cpp

@@ -1324,11 +1324,12 @@ void PhysicsSystem::JobSolveVelocityConstraints(PhysicsUpdateContext *ioContext,
 		}
 
 		// Warm start
-		ConstraintManager::sWarmStartVelocityConstraints(active_constraints, constraints_begin, constraints_end, warm_start_impulse_ratio);
+		int num_velocity_steps = mPhysicsSettings.mNumVelocitySteps;
+		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);
 
 		// Solve
-		for (int velocity_step = 0; velocity_step < mPhysicsSettings.mNumVelocitySteps; ++velocity_step)
+		for (int velocity_step = 0; velocity_step < num_velocity_steps; ++velocity_step)
 		{
 			bool constraint_impulse = ConstraintManager::sSolveVelocityConstraints(active_constraints, constraints_begin, constraints_end, delta_time);
 			bool contact_impulse = mContactManager.SolveVelocityConstraints(contacts_begin, contacts_end);
@@ -2086,7 +2087,30 @@ void PhysicsSystem::JobSolvePositionConstraints(PhysicsUpdateContext *ioContext,
 		if (has_contacts || has_constraints)
 		{
 			float baumgarte = mPhysicsSettings.mBaumgarte;
-			for (int position_step = 0; position_step < mPhysicsSettings.mNumPositionSteps; ++position_step)
+
+			// 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 constraint_impulse = ConstraintManager::sSolvePositionConstraints(active_constraints, constraints_begin, constraints_end, delta_time, baumgarte, num_position_steps);
+				bool contact_impulse = mContactManager.SolvePositionConstraints(contacts_begin, contacts_end);
+
+				// If no impulses were applied we can stop, otherwise we already did 1 iteration
+				if (!constraint_impulse && !contact_impulse)
+					num_position_steps = 0;
+				else
+					--num_position_steps;
+			}
+			else
+			{
+				// Iterate the constraints to see if they override the amount 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)
 			{
 				bool constraint_impulse = ConstraintManager::sSolvePositionConstraints(active_constraints, constraints_begin, constraints_end, delta_time, baumgarte);
 				bool contact_impulse = mContactManager.SolvePositionConstraints(contacts_begin, contacts_end);

Samples/Tests/Constraints/FixedConstraintTest.cpp

@@ -127,6 +127,8 @@ void FixedConstraintTest::Initialize()
 				{
 					FixedConstraintSettings constraint;
 					constraint.mAutoDetectPoint = true;
+					constraint.mNumVelocityStepsOverride = 64; // This structure needs more solver steps to be stable
+					constraint.mNumPositionStepsOverride = JPH_IF_NOT_DEBUG(64) JPH_IF_DEBUG(8); // In debug mode use less steps to preserve framerate (at the cost of stability)
 					mPhysicsSystem->AddConstraint(constraint.Create(*pillars[(i + j) % 4], *cross));
 				}
 
@@ -135,6 +137,8 @@ void FixedConstraintTest::Initialize()
 				{
 					FixedConstraintSettings constraint;
 					constraint.mAutoDetectPoint = true;
+					constraint.mNumVelocityStepsOverride = 64;
+					constraint.mNumPositionStepsOverride = JPH_IF_NOT_DEBUG(64) JPH_IF_DEBUG(8);
 					mPhysicsSystem->AddConstraint(constraint.Create(*prev_pillars[i], *pillars[i]));
 				}