Vehicle constraint bugfixes (#901)

* Bugfix: Longitudinal friction impulse could become much higher than the calculated max because each iteration it was clamped to the max friction impulse which meant the total friction impulse could be PhysicsSettings::mNumVelocitySteps times too high.
* Properly initializing current RPM to min RPM. When min RPM was lower than the default min RPM the engine would not start at min RPM.
* Added ability to override the max tire impulse calculations and added a temporary workaround to the VehicleConstraintTest to show how to get the old behavior back.

Jolt/Physics/Vehicle/TrackedVehicleController.cpp

@@ -128,6 +128,7 @@ TrackedVehicleController::TrackedVehicleController(const TrackedVehicleControlle
 	static_cast<VehicleEngineSettings &>(mEngine) = inSettings.mEngine;
 	JPH_ASSERT(inSettings.mEngine.mMinRPM >= 0.0f);
 	JPH_ASSERT(inSettings.mEngine.mMinRPM <= inSettings.mEngine.mMaxRPM);
+	mEngine.SetCurrentRPM(mEngine.mMinRPM);
 
 	// Copy transmission settings
 	static_cast<VehicleTransmissionSettings &>(mTransmission) = inSettings.mTransmission;
@@ -388,10 +389,10 @@ bool TrackedVehicleController::SolveLongitudinalAndLateralConstraints(float inDe
 				float linear_impulse = (track.mAngularVelocity - desired_angular_velocity) * track.mInertia / settings->mRadius;
 
 				// Limit the impulse by max track friction
-				min_longitudinal_impulse = max_longitudinal_impulse = w->GetLongitudinalLambda() + Sign(linear_impulse) * min(abs(linear_impulse), max_longitudinal_friction_impulse);
+				float prev_lambda = w->GetLongitudinalLambda();
+				min_longitudinal_impulse = max_longitudinal_impulse = Clamp(prev_lambda + linear_impulse, -max_longitudinal_friction_impulse, max_longitudinal_friction_impulse);
 
 				// Longitudinal impulse
-				float prev_lambda = w->GetLongitudinalLambda();
 				impulse |= w->SolveLongitudinalConstraintPart(mConstraint, min_longitudinal_impulse, max_longitudinal_impulse);
 
 				// Update the angular velocity of the track according to the lambda that was applied

Jolt/Physics/Vehicle/WheeledVehicleController.cpp

@@ -176,6 +176,7 @@ WheeledVehicleController::WheeledVehicleController(const WheeledVehicleControlle
 	static_cast<VehicleEngineSettings &>(mEngine) = inSettings.mEngine;
 	JPH_ASSERT(inSettings.mEngine.mMinRPM >= 0.0f);
 	JPH_ASSERT(inSettings.mEngine.mMinRPM <= inSettings.mEngine.mMaxRPM);
+	mEngine.SetCurrentRPM(mEngine.mMinRPM);
 
 	// Copy transmission settings
 	static_cast<VehicleTransmissionSettings &>(mTransmission) = inSettings.mTransmission;
@@ -652,14 +653,21 @@ bool WheeledVehicleController::SolveLongitudinalAndLateralConstraints(float inDe
 {
 	bool impulse = false;
 
+	float *max_lateral_friction_impulse = (float *)JPH_STACK_ALLOC(mConstraint.GetWheels().size() * sizeof(float));
+
+	uint wheel_index = 0;
 	for (Wheel *w_base : mConstraint.GetWheels())
+	{
 		if (w_base->HasContact())
 		{
 			WheelWV *w = static_cast<WheelWV *>(w_base);
 			const WheelSettingsWV *settings = w->GetSettings();
 
 			// Calculate max impulse that we can apply on the ground
-			float max_longitudinal_friction_impulse = w->mCombinedLongitudinalFriction * w->GetSuspensionLambda();
+			float max_longitudinal_friction_impulse;
+			mTireMaxImpulseCallback(wheel_index,
+				max_longitudinal_friction_impulse, max_lateral_friction_impulse[wheel_index], w->GetSuspensionLambda(),
+				w->mCombinedLongitudinalFriction, w->mCombinedLateralFriction, w->mLongitudinalSlip, w->mLateralSlip, inDeltaTime);
 
 			// Calculate relative velocity between wheel contact point and floor in longitudinal direction
 			Vec3 relative_velocity = mConstraint.GetVehicleBody()->GetPointVelocity(w->GetContactPosition()) - w->GetContactPointVelocity();
@@ -694,26 +702,32 @@ bool WheeledVehicleController::SolveLongitudinalAndLateralConstraints(float inDe
 				float linear_impulse = (w->GetAngularVelocity() - desired_angular_velocity) * settings->mInertia / settings->mRadius;
 
 				// Limit the impulse by max tire friction
-				min_longitudinal_impulse = max_longitudinal_impulse = w->GetLongitudinalLambda() + Sign(linear_impulse) * min(abs(linear_impulse), max_longitudinal_friction_impulse);
+				float prev_lambda = w->GetLongitudinalLambda();
+				min_longitudinal_impulse = max_longitudinal_impulse = Clamp(prev_lambda + linear_impulse, -max_longitudinal_friction_impulse, max_longitudinal_friction_impulse);
 
 				// Longitudinal impulse
-				float prev_lambda = w->GetLongitudinalLambda();
 				impulse |= w->SolveLongitudinalConstraintPart(mConstraint, min_longitudinal_impulse, max_longitudinal_impulse);
 
 				// Update the angular velocity of the wheels according to the lambda that was applied
 				w->SetAngularVelocity(w->GetAngularVelocity() - (w->GetLongitudinalLambda() - prev_lambda) * settings->mRadius / settings->mInertia);
 			}
 		}
+		++wheel_index;
+	}
 
+	wheel_index = 0;
 	for (Wheel *w_base : mConstraint.GetWheels())
+	{
 		if (w_base->HasContact())
 		{
 			WheelWV *w = static_cast<WheelWV *>(w_base);
 
 			// Lateral friction
-			float max_lateral_friction_impulse = w->mCombinedLateralFriction * w->GetSuspensionLambda();
-			impulse |= w->SolveLateralConstraintPart(mConstraint, -max_lateral_friction_impulse, max_lateral_friction_impulse);
+			float max_lateral_impulse = max_lateral_friction_impulse[wheel_index];
+			impulse |= w->SolveLateralConstraintPart(mConstraint, -max_lateral_impulse, max_lateral_impulse);
 		}
+		++wheel_index;
+	}
 
 	return impulse;
 }

Jolt/Physics/Vehicle/WheeledVehicleController.h

@@ -145,6 +145,11 @@ public:
 	/// Get the average wheel speed of all driven wheels (measured at the clutch)
 	float						GetWheelSpeedAtClutch() const;
 
+	/// Calculate max tire impulses by combining friction, slip, and suspension impulse. Note that the actual applied impulse may be lower (e.g. when the vehicle is stationary on a horizontal surface the actual impulse applied will be 0).
+	using TireMaxImpulseCallback = function<void(uint inWheelIndex, float &outLongitudinalImpulse, float &outLateralImpulse, float inSuspensionImpulse, float inLongitudinalFriction, float inLateralFriction, float inLongitudinalSlip, float inLateralSlip, float inDeltaTime)>;
+	const TireMaxImpulseCallback&GetTireMaxImpulseCallback() const			{ return mTireMaxImpulseCallback; }
+	void						SetTireMaxImpulseCallback(const TireMaxImpulseCallback &inTireMaxImpulseCallback)	{ mTireMaxImpulseCallback = inTireMaxImpulseCallback; }
+
 #ifdef JPH_DEBUG_RENDERER
 	/// Debug drawing of RPM meter
 	void						SetRPMMeter(Vec3Arg inPosition, float inSize) { mRPMMeterPosition = inPosition; mRPMMeterSize = inSize; }
@@ -176,6 +181,14 @@ protected:
 	float						mDifferentialLimitedSlipRatio;				///< Ratio max / min average wheel speed of each differential (measured at the clutch).
 	float						mPreviousDeltaTime = 0.0f;					///< Delta time of the last step
 
+	// Callback that calculates the max impulse that the tire can apply to the ground
+	TireMaxImpulseCallback		mTireMaxImpulseCallback =
+		[](uint, float &outLongitudinalImpulse, float &outLateralImpulse, float inSuspensionImpulse, float inLongitudinalFriction, float inLateralFriction, float, float, float)
+		{
+			outLongitudinalImpulse = inLongitudinalFriction * inSuspensionImpulse;
+			outLateralImpulse = inLateralFriction * inSuspensionImpulse;
+		};
+
 #ifdef JPH_DEBUG_RENDERER
 	// Debug settings
 	Vec3						mRPMMeterPosition { 0, 1, 0 };				///< Position (in local space of the body) of the RPM meter when drawing the constraint

Samples/Tests/Vehicle/VehicleConstraintTest.cpp

@@ -152,6 +152,17 @@ void VehicleConstraintTest::Initialize()
 	}
 
 	mVehicleConstraint = new VehicleConstraint(*mCarBody, vehicle);
+
+	// The vehicle settings were tweaked with a buggy implementation of the longitudinal tire impulses, this meant that PhysicsSettings::mNumVelocitySteps times more impulse
+	// could be applied than intended. To keep the behavior of the vehicle the same we increase the max longitudinal impulse by the same factor. In a future version the vehicle
+	// will be retweaked.
+	static_cast<WheeledVehicleController *>(mVehicleConstraint->GetController())->SetTireMaxImpulseCallback(
+		[](uint, float &outLongitudinalImpulse, float &outLateralImpulse, float inSuspensionImpulse, float inLongitudinalFriction, float inLateralFriction, float, float, float)
+		{
+			outLongitudinalImpulse = 10.0f * inLongitudinalFriction * inSuspensionImpulse;
+			outLateralImpulse = inLateralFriction * inSuspensionImpulse;
+		});
+
 	mPhysicsSystem->AddConstraint(mVehicleConstraint);
 	mPhysicsSystem->AddStepListener(mVehicleConstraint);
 }

UnitTests/Physics/WheeledVehicleTests.cpp

@@ -178,7 +178,7 @@ TEST_SUITE("WheeledVehicleTests")
 		// Start driving forward
 		controller->SetDriverInput(1.0f, 0.0f, 0.0f, 0.0f);
 		c.GetBodyInterface().ActivateBody(body->GetID());
-		c.Simulate(1.0f);
+		c.Simulate(2.0f);
 		CheckOnGround(constraint, settings, floor_id);
 		RVec3 pos2 = body->GetPosition();
 		CHECK_APPROX_EQUAL(pos2.GetX(), 0, 1.0e-2_r); // Not moving left/right
@@ -228,7 +228,7 @@ TEST_SUITE("WheeledVehicleTests")
 		// Turn right
 		controller->SetDriverInput(1.0f, 1.0f, 0.0f, 0.0f);
 		c.GetBodyInterface().ActivateBody(body->GetID());
-		c.Simulate(1.0f);
+		c.Simulate(2.0f);
 		CheckOnGround(constraint, settings, floor_id);
 		Vec3 omega = body->GetAngularVelocity();
 		CHECK(omega.GetY() < -0.4f); // Rotating right
@@ -237,7 +237,7 @@ TEST_SUITE("WheeledVehicleTests")
 		// Hand brake
 		controller->SetDriverInput(0.0f, 0.0f, 0.0f, 1.0f);
 		c.GetBodyInterface().ActivateBody(body->GetID());
-		c.Simulate(5.0f);
+		c.Simulate(7.0f);
 		CheckOnGround(constraint, settings, floor_id);
 		CHECK(!body->IsActive()); // Car should have gone to sleep
 		vel = body->GetLinearVelocity();
@@ -246,7 +246,7 @@ TEST_SUITE("WheeledVehicleTests")
 		// Turn left
 		controller->SetDriverInput(1.0f, -1.0f, 0.0f, 0.0f);
 		c.GetBodyInterface().ActivateBody(body->GetID());
-		c.Simulate(1.0f);
+		c.Simulate(2.0f);
 		CheckOnGround(constraint, settings, floor_id);
 		omega = body->GetAngularVelocity();
 		CHECK(omega.GetY() > 0.4f); // Rotating left
@@ -305,6 +305,14 @@ TEST_SUITE("WheeledVehicleTests")
 			Body *body = constraint->GetVehicleBody();
 			WheeledVehicleController *controller = static_cast<WheeledVehicleController *>(constraint->GetController());
 
+			// Give the wheels extra grip
+			controller->SetTireMaxImpulseCallback(
+				[](uint, float &outLongitudinalImpulse, float &outLateralImpulse, float inSuspensionImpulse, float inLongitudinalFriction, float inLateralFriction, float, float, float)
+				{
+					outLongitudinalImpulse = 10.0f * inLongitudinalFriction * inSuspensionImpulse;
+					outLateralImpulse = inLateralFriction * inSuspensionImpulse;
+				});
+
 			// Simulate till vehicle rests on block
 			bool vehicle_on_floor = false;
 			for (float time = 0; time < 2.0f; time += c.GetDeltaTime())