Improved simulation of engine RPM (#596)

* Fix for engine RPM being much higher than wheel RPM when measured at clutch. Before we were ignoring bake and wheel torques in engine RPM calculation. Now they're much closer but this unfortunately means that the simulation of the vehicle has changed and mainly the engine torque and clutch strength need to be re-tweaked.
* Always apply engine damping. Before this was only done when not accelerating. This means that the engine torque needs to be higher than before to overcome the drag.
* Added new test scene of a steep slope to test shifting gears on a slope.

Jolt/Physics/Vehicle/WheeledVehicleController.cpp

@@ -256,7 +256,7 @@ void WheeledVehicleController::PostCollide(float inDeltaTime, PhysicsSystem &inP
 {
 	JPH_PROFILE_FUNCTION();
 
-	// Remember old RPM so we're increasing or decreasing
+	// Remember old RPM so we can detect if we're increasing or decreasing
 	float old_engine_rpm = mEngine.GetCurrentRPM();
 
 	Wheels &wheels = mConstraint.GetWheels();
@@ -273,9 +273,8 @@ void WheeledVehicleController::PostCollide(float inDeltaTime, PhysicsSystem &inP
 	if (mTransmission.mMode == ETransmissionMode::Auto)
 		forward_input *= mTransmission.GetClutchFriction();
 
-	// Apply damping if there is no acceleration
-	if (forward_input < 1.0e-3f)
-		mEngine.ApplyDamping(inDeltaTime);
+	// Apply engine damping
+	mEngine.ApplyDamping(inDeltaTime);
 
 	// Calculate engine torque
 	float engine_torque = mEngine.GetTorque(forward_input);
@@ -360,6 +359,7 @@ void WheeledVehicleController::PostCollide(float inDeltaTime, PhysicsSystem &inP
 		WheelWV *				mWheel;
 		float					mClutchToWheelRatio;
 		float					mClutchToWheelTorqueRatio;
+		float					mEstimatedAngularImpulse;
 	};
 	Array<DrivenWheel> driven_wheels;
 	driven_wheels.reserve(wheels.size());
@@ -382,18 +382,18 @@ void WheeledVehicleController::PostCollide(float inDeltaTime, PhysicsSystem &inP
 			d.CalculateTorqueRatio(wl->GetAngularVelocity(), wr->GetAngularVelocity(), ratio_l, ratio_r);
 
 			// Add both wheels
-			driven_wheels.push_back({ wl, clutch_to_wheel_ratio, dd.mClutchToDifferentialTorqueRatio * ratio_l });
-			driven_wheels.push_back({ wr, clutch_to_wheel_ratio, dd.mClutchToDifferentialTorqueRatio * ratio_r });
+			driven_wheels.push_back({ wl, clutch_to_wheel_ratio, dd.mClutchToDifferentialTorqueRatio * ratio_l, 0.0f });
+			driven_wheels.push_back({ wr, clutch_to_wheel_ratio, dd.mClutchToDifferentialTorqueRatio * ratio_r, 0.0f });
 		}
 		else if (wl != nullptr)
 		{
 			// Only left wheel, all power to left
-			driven_wheels.push_back({ wl, clutch_to_wheel_ratio, dd.mClutchToDifferentialTorqueRatio });
+			driven_wheels.push_back({ wl, clutch_to_wheel_ratio, dd.mClutchToDifferentialTorqueRatio, 0.0f });
 		}
 		else if (wr != nullptr)
 		{
 			// Only right wheel, all power to right
-			driven_wheels.push_back({ wr, clutch_to_wheel_ratio, dd.mClutchToDifferentialTorqueRatio });
+			driven_wheels.push_back({ wr, clutch_to_wheel_ratio, dd.mClutchToDifferentialTorqueRatio, 0.0f });
 		}
 	}
 
@@ -465,8 +465,6 @@ void WheeledVehicleController::PostCollide(float inDeltaTime, PhysicsSystem &inP
 		// a * w(t + dt) = b
 		//
 		// We then invert the matrix to get the new angular velocities.
-		//
-		// Note that currently we set TW(i) = 0 so that the wheels will accelerate as if no external force was applied to them. These external forces are applied later and will slow down the wheel before the end of the time step.
 
 		// Dimension of matrix is N + 1
 		int n = (int)driven_wheels.size() + 1;
@@ -490,19 +488,23 @@ void WheeledVehicleController::PostCollide(float inDeltaTime, PhysicsSystem &inP
 		// dt / Ie
 		float dt_div_ie = inDeltaTime / mEngine.mInertia;
 
+		// Calculate scale factor for impulses based on previous delta time
+		float impulse_scale = mPreviousDeltaTime > 0.0f? inDeltaTime / mPreviousDeltaTime : 0.0f;
+
 		// Iterate the rows for the wheels
 		for (int i = 0; i < (int)driven_wheels.size(); ++i)
 		{
-			const DrivenWheel &w_i = driven_wheels[i];
+			DrivenWheel &w_i = driven_wheels[i];
+			const WheelSettingsWV *settings = w_i.mWheel->GetSettings();
 
-			// dt / Iw
-			float dt_div_iw = inDeltaTime / w_i.mWheel->GetSettings()->mInertia;
+			// Get wheel inertia
+			float inertia = settings->mInertia;
 
 			// S * R(i)
 			float s_r = clutch_strength * w_i.mClutchToWheelRatio;
 
 			// dt * S * R(i) * F(i) / Iw
-			float dt_s_r_f_div_iw = dt_div_iw * s_r * w_i.mClutchToWheelTorqueRatio;
+			float dt_s_r_f_div_iw = inDeltaTime * s_r * w_i.mClutchToWheelTorqueRatio / inertia;
 
 			// Fill in the columns of a for wheel j
 			for (int j = 0; j < (int)driven_wheels.size(); ++j)
@@ -517,8 +519,35 @@ void WheeledVehicleController::PostCollide(float inDeltaTime, PhysicsSystem &inP
 			// Add the column for the engine
 			a(i, engine) = -dt_s_r_f_div_iw;
 
-			// Fill in the constant b
-			b(i, 0) = w_i.mWheel->GetAngularVelocity(); // + dt_div_iw * (brake and tire torques)
+			// Calculate external angular impulse operating on the wheel: TW(i) * dt
+			float dt_tw = 0.0f;
+
+			// Combine brake with hand brake torque
+			float brake_torque = mBrakeInput * settings->mMaxBrakeTorque + mHandBrakeInput * settings->mMaxHandBrakeTorque;
+			if (brake_torque > 0.0f)
+			{
+				// We're braking
+				// Calculate brake angular impulse
+				float sign;
+				if (w_i.mWheel->GetAngularVelocity() != 0.0f)
+					sign = Sign(w_i.mWheel->GetAngularVelocity());
+				else
+					sign = Sign(mTransmission.GetCurrentRatio()); // When wheels have locked up use the transmission ratio to determine the sign
+				dt_tw = sign * inDeltaTime * brake_torque;
+			}
+
+			if (w_i.mWheel->HasContact())
+			{
+				// We have wheel contact with the floor
+				// Note that we don't know the torque due to the ground contact yet, so we use the impulse applied from the last frame to estimate it
+				// Wheel torque TW = force * radius = lambda / dt * radius
+				dt_tw += impulse_scale * w_i.mWheel->GetLongitudinalLambda() * settings->mRadius;
+			}
+
+			w_i.mEstimatedAngularImpulse = dt_tw;
+
+			// Fill in the constant b = ww(i,t)+(dt*TW(i))/Iw(i)
+			b(i, 0) = w_i.mWheel->GetAngularVelocity() - dt_tw / inertia;
 
 			// To avoid looping over the wheels again, we also fill in the wheel columns of the engine row here
 			a(engine, i) = -dt_div_ie * s_r / num_driven_wheels_float;
@@ -534,8 +563,18 @@ void WheeledVehicleController::PostCollide(float inDeltaTime, PhysicsSystem &inP
 			// Update the angular velocities for the wheels
 			for (int i = 0; i < (int)driven_wheels.size(); ++i)
 			{
-				DrivenWheel &dw1 = driven_wheels[i];
-				dw1.mWheel->SetAngularVelocity(b(i, 0));
+				DrivenWheel &w_i = driven_wheels[i];
+				const WheelSettingsWV *settings = w_i.mWheel->GetSettings();
+
+				// Get solved wheel angular velocity
+				float angular_velocity = b(i, 0);
+
+				// We estimated TW and applied it in the equation above, but we haven't actually applied this torque yet so we undo it here.
+				// It will be applied when we solve the actual braking / the constraints with the floor.
+				angular_velocity += w_i.mEstimatedAngularImpulse / settings->mInertia;
+
+				// Update angular velocity
+				w_i.mWheel->SetAngularVelocity(angular_velocity);
 			}
 
 			// Update the engine RPM
@@ -600,6 +639,9 @@ void WheeledVehicleController::PostCollide(float inDeltaTime, PhysicsSystem &inP
 			w->mBrakeImpulse = 0.0f;
 		}
 	}
+
+	// Remember previous delta time so we can scale the impulses correctly
+	mPreviousDeltaTime = inDeltaTime;
 }
 
 bool WheeledVehicleController::SolveLongitudinalAndLateralConstraints(float inDeltaTime) 
@@ -685,11 +727,14 @@ void WheeledVehicleController::Draw(DebugRenderer *inRenderer) const
 	Vec3 rpm_meter_fwd = body->GetRotation() * mConstraint.GetLocalForward();
 	mEngine.DrawRPM(inRenderer, rpm_meter_pos, rpm_meter_fwd, rpm_meter_up, mRPMMeterSize, mTransmission.mShiftDownRPM, mTransmission.mShiftUpRPM);
 
-	// Calculate average wheel speed at clutch
-	float wheel_speed_at_clutch = GetWheelSpeedAtClutch();
+	if (mTransmission.GetCurrentRatio() != 0.0f)
+	{
+		// Calculate average wheel speed at clutch
+		float wheel_speed_at_clutch = GetWheelSpeedAtClutch();
 		
-	// Draw the average wheel speed measured at clutch to compare engine RPM with wheel RPM
-	inRenderer->DrawLine(rpm_meter_pos, rpm_meter_pos + Quat::sRotation(rpm_meter_fwd, mEngine.ConvertRPMToAngle(wheel_speed_at_clutch)) * (rpm_meter_up * 1.1f * mRPMMeterSize), Color::sYellow);
+		// Draw the average wheel speed measured at clutch to compare engine RPM with wheel RPM
+		inRenderer->DrawLine(rpm_meter_pos, rpm_meter_pos + Quat::sRotation(rpm_meter_fwd, mEngine.ConvertRPMToAngle(wheel_speed_at_clutch)) * (rpm_meter_up * 1.1f * mRPMMeterSize), Color::sYellow);
+	}
 
 	// Draw current vehicle state
 	String status = StringFormat("Forward: %.1f, Right: %.1f\nBrake: %.1f, HandBrake: %.1f\n"
@@ -756,6 +801,7 @@ void WheeledVehicleController::SaveState(StateRecorder &inStream) const
 	inStream.Write(mRightInput);
 	inStream.Write(mBrakeInput);
 	inStream.Write(mHandBrakeInput);
+	inStream.Write(mPreviousDeltaTime);
 
 	mEngine.SaveState(inStream);
 	mTransmission.SaveState(inStream);
@@ -767,6 +813,7 @@ void WheeledVehicleController::RestoreState(StateRecorder &inStream)
 	inStream.Read(mRightInput);
 	inStream.Read(mBrakeInput);
 	inStream.Read(mHandBrakeInput);
+	inStream.Read(mPreviousDeltaTime);
 
 	mEngine.RestoreState(inStream);
 	mTransmission.RestoreState(inStream);

Jolt/Physics/Vehicle/WheeledVehicleController.h

@@ -157,6 +157,7 @@ protected:
 	VehicleTransmission			mTransmission;								///< Transmission state of the vehicle
 	Differentials				mDifferentials;								///< Differential states of the vehicle
 	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
 
 #ifdef JPH_DEBUG_RENDERER
 	// Debug settings

Samples/Tests/Vehicle/MotorcycleTest.cpp

@@ -105,13 +105,14 @@ void MotorcycleTest::Initialize()
 	vehicle.mWheels = { front, back };
 
 	MotorcycleControllerSettings *controller = new MotorcycleControllerSettings;
-	controller->mEngine.mMaxTorque = 80.0f;
+	controller->mEngine.mMaxTorque = 150.0f;
 	controller->mEngine.mMinRPM = 1000.0f;
 	controller->mEngine.mMaxRPM = 10000.0f;
 	controller->mTransmission.mShiftDownRPM = 2000.0f;
-	controller->mTransmission.mShiftUpRPM = 9000.0f;
+	controller->mTransmission.mShiftUpRPM = 8000.0f;
 	controller->mTransmission.mGearRatios = { 2.27f, 1.63f, 1.3f, 1.09f, 0.96f, 0.88f }; // From: https://www.blocklayer.com/rpm-gear-bikes
 	controller->mTransmission.mReverseGearRatios = { -4.0f };
+	controller->mTransmission.mClutchStrength = 2.0f;
 	vehicle.mController = controller;
 
 	// Differential (not really applicable to a motorcycle but we need one anyway to drive it)

Samples/Tests/Vehicle/VehicleTest.cpp

@@ -25,6 +25,7 @@ JPH_IMPLEMENT_RTTI_VIRTUAL(VehicleTest)
 const char *VehicleTest::sScenes[] =
 {
 	"Flat",
+	"Flat With Slope",
 	"Steep Slope",
 	"Step",
 	"Playground",
@@ -45,6 +46,25 @@ void VehicleTest::Initialize()
 		// Load a race track to have something to assess speed and steering behavior
 		LoadRaceTrack("Assets/Racetracks/Zandvoort.csv");
 	}
+	else if (strcmp(sSceneName, "Flat With Slope") == 0)
+	{
+		const float cSlopeStartDistance = 100.0f;
+		const float cSlopeLength = 100.0f;
+		const float cSlopeAngle = DegreesToRadians(30.0f);
+
+		// Flat test floor
+		Body &floor = *mBodyInterface->CreateBody(BodyCreationSettings(new BoxShape(Vec3(1000.0f, 1.0f, 1000.0f), 0.0f), RVec3(0.0f, -1.0f, 0.0f), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING));
+		floor.SetFriction(1.0f);
+		mBodyInterface->AddBody(floor.GetID(), EActivation::DontActivate);
+
+		Body &slope_up = *mBodyInterface->CreateBody(BodyCreationSettings(new BoxShape(Vec3(25.0f, 1.0f, cSlopeLength), 0.0f), RVec3(0.0f, cSlopeLength * Sin(cSlopeAngle) - 1.0f, cSlopeStartDistance + cSlopeLength * Cos(cSlopeAngle)), Quat::sRotation(Vec3::sAxisX(), -cSlopeAngle), EMotionType::Static, Layers::NON_MOVING));
+		slope_up.SetFriction(1.0f);
+		mBodyInterface->AddBody(slope_up.GetID(), EActivation::DontActivate);
+
+		Body &slope_down = *mBodyInterface->CreateBody(BodyCreationSettings(new BoxShape(Vec3(25.0f, 1.0f, cSlopeLength), 0.0f), RVec3(0.0f, cSlopeLength * Sin(cSlopeAngle) - 1.0f, cSlopeStartDistance + 3.0f * cSlopeLength * Cos(cSlopeAngle)), Quat::sRotation(Vec3::sAxisX(), cSlopeAngle), EMotionType::Static, Layers::NON_MOVING));
+		slope_down.SetFriction(1.0f);
+		mBodyInterface->AddBody(slope_down.GetID(), EActivation::DontActivate);
+	}
 	else if (strcmp(sSceneName, "Steep Slope") == 0)
 	{
 		// Steep slope test floor (20 degrees = 36% grade)

UnitTests/Physics/WheeledVehicleTests.cpp

@@ -181,21 +181,21 @@ TEST_SUITE("WheeledVehicleTests")
 		c.Simulate(1.0f);
 		CheckOnGround(constraint, settings, floor_id);
 		RVec3 pos2 = body->GetPosition();
-		CHECK_APPROX_EQUAL(pos2.GetX(), 0, 1.0e-3_r); // Not moving left/right
+		CHECK_APPROX_EQUAL(pos2.GetX(), 0, 1.0e-2_r); // Not moving left/right
 		CHECK(pos2.GetZ() > pos1.GetZ() + 1.0f); // Moving in Z direction
 		Vec3 vel = body->GetLinearVelocity();
-		CHECK_APPROX_EQUAL(vel.GetX(), 0, 1.0e-2f); // Not moving left/right
+		CHECK_APPROX_EQUAL(vel.GetX(), 0, 2.0e-2f); // Not moving left/right
 		CHECK(vel.GetZ() > 1.0f); // Moving in Z direction
 		CHECK(controller->GetTransmission().GetCurrentGear() > 0);
 
 		// Brake
 		controller->SetDriverInput(0.0f, 0.0f, 1.0f, 0.0f);
 		c.GetBodyInterface().ActivateBody(body->GetID());
-		c.Simulate(2.0f);
+		c.Simulate(5.0f);
 		CheckOnGround(constraint, settings, floor_id);
 		CHECK(!body->IsActive()); // Car should have gone to sleep
 		RVec3 pos3 = body->GetPosition();
-		CHECK_APPROX_EQUAL(pos3.GetX(), 0, 2.0e-3_r); // Not moving left/right
+		CHECK_APPROX_EQUAL(pos3.GetX(), 0, 2.0e-2_r); // Not moving left/right
 		CHECK(pos3.GetZ() > pos2.GetZ() + 1.0f); // Moving in Z direction while braking
 		vel = body->GetLinearVelocity();
 		CHECK_APPROX_EQUAL(vel, Vec3::sZero(), 1.0e-3f); // Not moving
@@ -206,10 +206,10 @@ TEST_SUITE("WheeledVehicleTests")
 		c.Simulate(2.0f);
 		CheckOnGround(constraint, settings, floor_id);
 		RVec3 pos4 = body->GetPosition();
-		CHECK_APPROX_EQUAL(pos4.GetX(), 0, 1.0e-2_r); // Not moving left/right
+		CHECK_APPROX_EQUAL(pos4.GetX(), 0, 3.0e-2_r); // Not moving left/right
 		CHECK(pos4.GetZ() < pos3.GetZ() - 1.0f); // Moving in -Z direction
 		vel = body->GetLinearVelocity();
-		CHECK_APPROX_EQUAL(vel.GetX(), 0, 1.0e-2f); // Not moving left/right
+		CHECK_APPROX_EQUAL(vel.GetX(), 0, 5.0e-2f); // Not moving left/right
 		CHECK(vel.GetZ() < -1.0f); // Moving in -Z direction
 		CHECK(controller->GetTransmission().GetCurrentGear() < 0);
 
@@ -220,7 +220,7 @@ TEST_SUITE("WheeledVehicleTests")
 		CheckOnGround(constraint, settings, floor_id);
 		CHECK(!body->IsActive()); // Car should have gone to sleep
 		RVec3 pos5 = body->GetPosition();
-		CHECK_APPROX_EQUAL(pos5.GetX(), 0, 1.0e-2_r); // Not moving left/right
+		CHECK_APPROX_EQUAL(pos5.GetX(), 0, 7.0e-2_r); // Not moving left/right
 		CHECK(pos5.GetZ() < pos4.GetZ() - 1.0f); // Moving in -Z direction while braking
 		vel = body->GetLinearVelocity();
 		CHECK_APPROX_EQUAL(vel, Vec3::sZero(), 1.0e-3f); // Not moving
@@ -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(4.0f);
+		c.Simulate(5.0f);
 		CheckOnGround(constraint, settings, floor_id);
 		CHECK(!body->IsActive()); // Car should have gone to sleep
 		vel = body->GetLinearVelocity();