Improved engine model of wheeled vehicles (#306)

* Implemented better solver for engine vs wheel speed
* Implemented limited slip differential for wheels and between differentials
* Improved behavior of automatic transmission
* Don't allow the vehicle to go to sleep while forward input is given
* Added extra configuration settings to vehicle constraint test
* Fixed bug where left and right wheels were mislabeled in vehicle constraint test
* Added steep test slope test level
* Added unit test for wheeled vehicles

Jolt/Jolt.cmake

@@ -92,6 +92,7 @@ set(JOLT_PHYSICS_SRC_FILES
 	${JOLT_PHYSICS_ROOT}/Jolt.h
 	${JOLT_PHYSICS_ROOT}/Math/DVec3.h
 	${JOLT_PHYSICS_ROOT}/Math/DVec3.inl
+	${JOLT_PHYSICS_ROOT}/Math/DynMatrix.h
 	${JOLT_PHYSICS_ROOT}/Math/EigenValueSymmetric.h
 	${JOLT_PHYSICS_ROOT}/Math/FindRoot.h
 	${JOLT_PHYSICS_ROOT}/Math/Float2.h

Jolt/Math/DynMatrix.h

@@ -0,0 +1,30 @@
+// SPDX-FileCopyrightText: 2022 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+/// Dynamic resizable matrix class
+class [[nodiscard]] DynMatrix
+{
+public:
+	/// Constructor
+					DynMatrix(const DynMatrix &) = default;
+					DynMatrix(uint inRows, uint inCols)			: mRows(inRows), mCols(inCols) { mElements.resize(inRows * inCols); }
+
+	/// Access an element
+	float			operator () (uint inRow, uint inCol) const	{ JPH_ASSERT(inRow < mRows && inCol < mCols); return mElements[inRow * mCols + inCol]; }
+	float &			operator () (uint inRow, uint inCol)		{ JPH_ASSERT(inRow < mRows && inCol < mCols); return mElements[inRow * mCols + inCol]; }
+
+	/// Get dimensions
+	uint			GetCols() const								{ return mCols; }
+	uint			GetRows() const								{ return mRows; }
+
+private:
+	uint			mRows;
+	uint			mCols;
+	Array<float>	mElements;
+};
+
+JPH_NAMESPACE_END

Jolt/Physics/Vehicle/TrackedVehicleController.cpp

@@ -227,15 +227,17 @@ void TrackedVehicleController::PostCollide(float inDeltaTime, PhysicsSystem &inP
 		// Update RPM only if the tracks are connected to the engine
 		if (fastest_wheel_speed > -FLT_MAX && fastest_wheel_speed < FLT_MAX)
 			mEngine.SetCurrentRPM(fastest_wheel_speed * mTransmission.GetCurrentRatio() * VehicleEngine::cAngularVelocityToRPM);
-		mEngine.SetCurrentRPM(Clamp(mEngine.GetCurrentRPM(), mEngine.mMinRPM, mEngine.mMaxRPM));
 	}
 	else
 	{
+		// Update engine with damping
+		mEngine.ApplyDamping(inDeltaTime);
+
 		// In auto transmission mode, don't accelerate the engine when switching gears
 		float forward_input = mTransmission.mMode == ETransmissionMode::Manual? abs(mForwardInput) : 0.0f;
 
 		// Engine not connected to wheels, update RPM based on engine inertia alone
-		mEngine.UpdateRPM(inDeltaTime, forward_input);
+		mEngine.ApplyTorque(mEngine.GetTorque(forward_input), inDeltaTime);
 	}
 
 	// Update transmission

Jolt/Physics/Vehicle/TrackedVehicleController.h

@@ -118,6 +118,7 @@ protected:
 
 	// See: VehicleController
 	virtual Wheel *				ConstructWheel(const WheelSettings &inWheel) const override { JPH_ASSERT(IsKindOf(&inWheel, JPH_RTTI(WheelSettingsTV))); return new WheelTV(static_cast<const WheelSettingsTV &>(inWheel)); }
+	virtual bool				AllowSleep() const override					{ return mForwardInput == 0.0f; }
 	virtual void				PreCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem) override;
 	virtual void				PostCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem) override;
 	virtual bool				SolveLongitudinalAndLateralConstraints(float inDeltaTime) override;

Jolt/Physics/Vehicle/VehicleConstraint.cpp

@@ -212,10 +212,14 @@ void VehicleConstraint::OnStep(float inDeltaTime, PhysicsSystem &inPhysicsSystem
 	mController->PostCollide(inDeltaTime, inPhysicsSystem);
 
 	// If the wheels are rotating, we don't want to go to sleep yet
-	bool allow_sleep = true;
-	for (const Wheel *w : mWheels)
-		if (abs(w->mAngularVelocity) > DegreesToRadians(10.0f))
-			allow_sleep = false;
+	bool allow_sleep = mController->AllowSleep();
+	if (allow_sleep)
+		for (const Wheel *w : mWheels)
+			if (abs(w->mAngularVelocity) > DegreesToRadians(10.0f))
+			{
+				allow_sleep = false;
+				break;
+			}
 	if (mBody->GetAllowSleeping() != allow_sleep)
 		mBody->SetAllowSleeping(allow_sleep);
 }

Jolt/Physics/Vehicle/VehicleController.h

@@ -45,6 +45,9 @@ protected:
 	// Create a new instance of wheel
 	virtual Wheel *				ConstructWheel(const WheelSettings &inWheel) const = 0;
 
+	// If the vehicle is allowed to go to sleep
+	virtual bool				AllowSleep() const = 0;
+
 	// Called before the wheel probes have been done
 	virtual void				PreCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem) = 0;
 

Jolt/Physics/Vehicle/VehicleDifferential.cpp

@@ -14,6 +14,7 @@ JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(VehicleDifferentialSettings)
 	JPH_ADD_ATTRIBUTE(VehicleDifferentialSettings, mRightWheel)
 	JPH_ADD_ATTRIBUTE(VehicleDifferentialSettings, mDifferentialRatio)
 	JPH_ADD_ATTRIBUTE(VehicleDifferentialSettings, mLeftRightSplit)
+	JPH_ADD_ATTRIBUTE(VehicleDifferentialSettings, mLimitedSlipRatio)
 	JPH_ADD_ATTRIBUTE(VehicleDifferentialSettings, mEngineTorqueRatio)
 }
 
@@ -23,6 +24,7 @@ void VehicleDifferentialSettings::SaveBinaryState(StreamOut &inStream) const
 	inStream.Write(mRightWheel);
 	inStream.Write(mDifferentialRatio);
 	inStream.Write(mLeftRightSplit);
+	inStream.Write(mLimitedSlipRatio);
 	inStream.Write(mEngineTorqueRatio);
 }
 
@@ -32,7 +34,47 @@ void VehicleDifferentialSettings::RestoreBinaryState(StreamIn &inStream)
 	inStream.Read(mRightWheel);
 	inStream.Read(mDifferentialRatio);
 	inStream.Read(mLeftRightSplit);
+	inStream.Read(mLimitedSlipRatio);
 	inStream.Read(mEngineTorqueRatio);
 }
 
+void VehicleDifferentialSettings::CalculateTorqueRatio(float inLeftAngularVelocity, float inRightAngularVelocity, float &outLeftTorqueFraction, float &outRightTorqueFraction) const
+{
+	// Start with the default torque ratio
+	outLeftTorqueFraction = 1.0f - mLeftRightSplit;
+	outRightTorqueFraction = mLeftRightSplit;
+
+	if (mLimitedSlipRatio < FLT_MAX)
+	{
+		JPH_ASSERT(mLimitedSlipRatio > 1.0f);
+
+		// This is a limited slip differential, adjust torque ratios according to wheel speeds
+		float omega_l = max(1.0e-3f, abs(inLeftAngularVelocity)); // prevent div by zero by setting a minimum velocity and ignoring that the wheels may be rotating in different directions
+		float omega_r = max(1.0e-3f, abs(inRightAngularVelocity));
+		float omega_min = min(omega_l, omega_r);
+		float omega_max = max(omega_l, omega_r);
+
+		// Map into a value that is 0 when the wheels are turning at an equal rate and 1 when the wheels are turning at mLimitedSlipRotationRatio
+		float alpha = min((omega_max / omega_min - 1.0f) / (mLimitedSlipRatio - 1.0f), 1.0f);
+		JPH_ASSERT(alpha >= 0.0f);
+		float one_min_alpha = 1.0f - alpha;
+
+		if (omega_l < omega_r)
+		{
+			// Redirect more power to the left wheel
+			outLeftTorqueFraction = outLeftTorqueFraction * one_min_alpha + alpha;
+			outRightTorqueFraction = outRightTorqueFraction * one_min_alpha;
+		}
+		else
+		{
+			// Redirect more power to the right wheel
+			outLeftTorqueFraction = outLeftTorqueFraction * one_min_alpha;
+			outRightTorqueFraction = outRightTorqueFraction * one_min_alpha + alpha;
+		}
+	}
+
+	// Assert the values add up to 1
+	JPH_ASSERT(abs(outLeftTorqueFraction + outRightTorqueFraction - 1.0f) < 1.0e-6f);
+}
+
 JPH_NAMESPACE_END

Jolt/Physics/Vehicle/VehicleDifferential.h

@@ -20,10 +20,18 @@ public:
 	/// Restores the contents in binary form to inStream.
 	void					RestoreBinaryState(StreamIn &inStream);
 
+	/// Calculate the torque ratio between left and right wheel
+	/// @param inLeftAngularVelocity Angular velocity of left wheel (rad / s)
+	/// @param inRightAngularVelocity Angular velocity of right wheel (rad / s)
+	/// @param outLeftTorqueFraction Fraction of torque that should go to the left wheel
+	/// @param outRightTorqueFraction Fraction of torque that should go to the right wheel
+	void					CalculateTorqueRatio(float inLeftAngularVelocity, float inRightAngularVelocity, float &outLeftTorqueFraction, float &outRightTorqueFraction) const;
+
 	int						mLeftWheel = -1;							///< Index (in mWheels) that represents the left wheel of this differential (can be -1 to indicate no wheel)
 	int						mRightWheel = -1;							///< Index (in mWheels) that represents the right wheel of this differential (can be -1 to indicate no wheel)
 	float					mDifferentialRatio = 3.42f;					///< Ratio between rotation speed of gear box and wheels
 	float					mLeftRightSplit = 0.5f;						///< Defines how the engine torque is split across the left and right wheel (0 = left, 0.5 = center, 1 = right)
+	float					mLimitedSlipRatio = 1.4f;					///< Ratio max / min wheel speed. When this ratio is exceeded, all torque gets distributed to the slowest moving wheel. This allows implementing a limited slip differential. Set to FLT_MAX for an open differential. Value should be > 1.
 	float					mEngineTorqueRatio = 1.0f;					///< How much of the engines torque is applied to this differential (0 = none, 1 = full), make sure the sum of all differentials is 1.
 };
 

Jolt/Physics/Vehicle/VehicleEngine.cpp

@@ -43,19 +43,21 @@ void VehicleEngineSettings::RestoreBinaryState(StreamIn &inStream)
 	mNormalizedTorque.RestoreBinaryState(inStream);
 }
 
-void VehicleEngine::UpdateRPM(float inDeltaTime, float inAcceleration)
+void VehicleEngine::ApplyTorque(float inTorque, float inDeltaTime)
+{
+	// Accelerate engine using torque
+	mCurrentRPM += cAngularVelocityToRPM * inTorque * inDeltaTime / mInertia;
+	ClampRPM();
+}
+
+void VehicleEngine::ApplyDamping(float inDeltaTime)
 {
 	// Angular damping: dw/dt = -c * w
 	// Solution: w(t) = w(0) * e^(-c * t) or w2 = w1 * e^(-c * dt)
 	// Taylor expansion of e^(-c * dt) = 1 - c * dt + ...
 	// Since dt is usually in the order of 1/60 and c is a low number too this approximation is good enough
 	mCurrentRPM *= max(0.0f, 1.0f - mAngularDamping * inDeltaTime);
-
-	// Accelerate engine using torque
-	mCurrentRPM += cAngularVelocityToRPM * GetTorque(inAcceleration) * inDeltaTime / mInertia;
-
-	// Clamp RPM
-	mCurrentRPM = Clamp(mCurrentRPM, mMinRPM, mMaxRPM);
+	ClampRPM();
 }
 
 #ifdef JPH_DEBUG_RENDERER

Jolt/Physics/Vehicle/VehicleEngine.h

@@ -34,7 +34,7 @@ public:
 	float					mMinRPM = 1000.0f;							///< Min amount of revolutions per minute (rpm) the engine can produce without stalling
 	float					mMaxRPM = 6000.0f;							///< Max amount of revolutions per minute (rpm) the engine can generate
 	LinearCurve				mNormalizedTorque;							///< Curve that describes a ratio of the max torque the engine can produce vs the fraction of the max RPM of the engine
-	float					mInertia = 2.0f;							///< Moment of inertia (kg m^2) of the engine
+	float					mInertia = 0.5f;							///< Moment of inertia (kg m^2) of the engine
 	float					mAngularDamping = 0.2f;						///< Angular damping factor of the wheel: dw/dt = -c * w
 };
 
@@ -45,20 +45,30 @@ public:
 	/// Multiply an angular velocity (rad/s) with this value to get rounds per minute (RPM)
 	static constexpr float	cAngularVelocityToRPM = 60.0f / (2.0f * JPH_PI);
 
+	/// Clamp the RPM between min and max RPM
+	inline void				ClampRPM()									{ mCurrentRPM = Clamp(mCurrentRPM, mMinRPM, mMaxRPM); }
+
 	/// Current rotation speed of engine in rounds per minute
 	float					GetCurrentRPM() const						{ return mCurrentRPM; }
 
 	/// Update rotation speed of engine in rounds per minute
-	void					SetCurrentRPM(float inRPM)					{ mCurrentRPM = inRPM; }
+	void					SetCurrentRPM(float inRPM)					{ mCurrentRPM = inRPM; ClampRPM(); }
+
+	/// Get current angular velocity of the engine in radians / second
+	inline float			GetAngularVelocity() const					{ return mCurrentRPM / cAngularVelocityToRPM; }
 
 	/// Get the amount of torque (N m) that the engine can supply
 	/// @param inAcceleration How much the gas pedal is pressed [0, 1]
 	float					GetTorque(float inAcceleration) const		{ return inAcceleration * mMaxTorque * mNormalizedTorque.GetValue(mCurrentRPM / mMaxRPM); }
 
-	/// Update the engine RPM assuming the engine is not connected to the wheels
+	/// Apply a torque to the engine rotation speed
+	/// @param inTorque Torque in N m
 	/// @param inDeltaTime Delta time in seconds
-	/// @param inAcceleration How much the gas pedal is pressed [0, 1]
-	void					UpdateRPM(float inDeltaTime, float inAcceleration);
+	void					ApplyTorque(float inTorque, float inDeltaTime);
+
+	/// Update the engine RPM for damping
+	/// @param inDeltaTime Delta time in seconds
+	void					ApplyDamping(float inDeltaTime);
 
 #ifdef JPH_DEBUG_RENDERER
 	/// Debug draw a RPM meter
@@ -70,7 +80,7 @@ public:
 	void					RestoreState(StateRecorder &inStream);
 
 private:
-	float					mCurrentRPM = 1000.0f;						///< Current rotation speed of engine in rounds per minute
+	float					mCurrentRPM = mMinRPM;						///< Current rotation speed of engine in rounds per minute
 };
 
 JPH_NAMESPACE_END

Jolt/Physics/Vehicle/VehicleTransmission.cpp

@@ -18,6 +18,7 @@ JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(VehicleTransmissionSettings)
 	JPH_ADD_ATTRIBUTE(VehicleTransmissionSettings, mSwitchLatency)
 	JPH_ADD_ATTRIBUTE(VehicleTransmissionSettings, mShiftUpRPM)
 	JPH_ADD_ATTRIBUTE(VehicleTransmissionSettings, mShiftDownRPM)
+	JPH_ADD_ATTRIBUTE(VehicleTransmissionSettings, mClutchStrength)
 }
 
 void VehicleTransmissionSettings::SaveBinaryState(StreamOut &inStream) const
@@ -30,6 +31,7 @@ void VehicleTransmissionSettings::SaveBinaryState(StreamOut &inStream) const
 	inStream.Write(mSwitchLatency);
 	inStream.Write(mShiftUpRPM);
 	inStream.Write(mShiftDownRPM);
+	inStream.Write(mClutchStrength);
 }
 
 void VehicleTransmissionSettings::RestoreBinaryState(StreamIn &inStream)
@@ -42,9 +44,10 @@ void VehicleTransmissionSettings::RestoreBinaryState(StreamIn &inStream)
 	inStream.Read(mSwitchLatency);
 	inStream.Read(mShiftUpRPM);
 	inStream.Read(mShiftDownRPM);
+	inStream.Read(mClutchStrength);
 }
 
-void VehicleTransmission::Update(float inDeltaTime, float inCurrentRPM, float inForwardInput, bool inEngineCanApplyTorque)
+void VehicleTransmission::Update(float inDeltaTime, float inCurrentRPM, float inForwardInput, bool inCanShiftUp)
 {
 	// Update current gear and calculate clutch friction
 	if (mMode == ETransmissionMode::Auto)
@@ -59,7 +62,7 @@ void VehicleTransmission::Update(float inDeltaTime, float inCurrentRPM, float in
 		}
 		else if (mGearSwitchLatencyTimeLeft == 0.0f) // If not in the timout after switching gears
 		{
-			if (inEngineCanApplyTorque && inCurrentRPM > mShiftUpRPM)
+			if (inCanShiftUp && inCurrentRPM > mShiftUpRPM)
 			{
 				if (mCurrentGear < 0)
 				{

Jolt/Physics/Vehicle/VehicleTransmission.h

@@ -37,6 +37,7 @@ public:
 	float					mSwitchLatency = 0.5f;						///< How long to wait after releasing the clutch before another switch is attempted (s), only used in auto mode
 	float					mShiftUpRPM = 4000.0f;						///< If RPM of engine is bigger then this we will shift a gear up, only used in auto mode
 	float					mShiftDownRPM = 2000.0f;					///< If RPM of engine is smaller then this we will shift a gear down, only used in auto mode
+	float					mClutchStrength = 10.0f;					///< Strength of the clutch when fully engaged. Total torque a clutch applies is Torque = ClutchStrength * (Velocity Engine - Avg Velocity Wheels) (units: k m^2 s^-1)
 };
 
 /// Runtime data for transmission
@@ -52,8 +53,8 @@ public:
 	/// @param inDeltaTime Time step delta time in s
 	/// @param inCurrentRPM Current RPM for engine
 	/// @param inForwardInput Hint if the user wants to drive forward (> 0) or backwards (< 0)
-	/// @param inEngineCanApplyTorque Indicates if the engine is connected ultimately to the ground, if not it makes no sense to shift up
-	void					Update(float inDeltaTime, float inCurrentRPM, float inForwardInput, bool inEngineCanApplyTorque);
+	/// @param inCanShiftUp Indicates if we want to allow the transmission to shift up (e.g. pass false if wheels are slipping)
+	void					Update(float inDeltaTime, float inCurrentRPM, float inForwardInput, bool inCanShiftUp);
 
 	/// Current gear, -1 = reverse, 0 = neutral, 1 = 1st gear etc.
 	int						GetCurrentGear() const						{ return mCurrentGear; }

Jolt/Physics/Vehicle/WheeledVehicleController.cpp

@@ -8,6 +8,8 @@
 #include <Jolt/ObjectStream/TypeDeclarations.h>
 #include <Jolt/Core/StreamIn.h>
 #include <Jolt/Core/StreamOut.h>
+#include <Jolt/Math/DynMatrix.h>
+#include <Jolt/Math/GaussianElimination.h>
 #ifdef JPH_DEBUG_RENDERER
 	#include <Jolt/Renderer/DebugRenderer.h>
 #endif // JPH_DEBUG_RENDERER
@@ -21,6 +23,7 @@ JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(WheeledVehicleControllerSettings)
 	JPH_ADD_ATTRIBUTE(WheeledVehicleControllerSettings, mEngine)
 	JPH_ADD_ATTRIBUTE(WheeledVehicleControllerSettings, mTransmission)
 	JPH_ADD_ATTRIBUTE(WheeledVehicleControllerSettings, mDifferentials)
+	JPH_ADD_ATTRIBUTE(WheeledVehicleControllerSettings, mDifferentialLimitedSlipRatio)
 }
 
 JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(WheelSettingsWV)
@@ -104,8 +107,9 @@ void WheelWV::Update(float inDeltaTime, const VehicleConstraint &inConstraint)
 		float relative_longitudinal_velocity = relative_velocity.Dot(mContactLongitudinal);
 
 		// Calculate longitudinal friction based on difference between velocity of rolling wheel and drive surface
-		float longitudinal_slip	= relative_longitudinal_velocity != 0.0f? abs((mAngularVelocity * settings->mRadius - relative_longitudinal_velocity) / relative_longitudinal_velocity) : 0.0f;
-		float longitudinal_slip_friction = settings->mLongitudinalFriction.GetValue(longitudinal_slip);
+		float relative_longitudinal_velocity_denom = Sign(relative_longitudinal_velocity) * max(1.0e-3f, abs(relative_longitudinal_velocity)); // Ensure we don't divide by zero
+		mLongitudinalSlip = abs((mAngularVelocity * settings->mRadius - relative_longitudinal_velocity) / relative_longitudinal_velocity_denom);
+		float longitudinal_slip_friction = settings->mLongitudinalFriction.GetValue(mLongitudinalSlip);
 
 		// Calculate lateral friction based on slip angle
 		float relative_velocity_len = relative_velocity.Length();
@@ -119,6 +123,7 @@ void WheelWV::Update(float inDeltaTime, const VehicleConstraint &inConstraint)
 	else
 	{
 		// No collision
+		mLongitudinalSlip = 0.0f;
 		mCombinedLongitudinalFriction = mCombinedLateralFriction = 0.0f;
 	}
 }
@@ -138,6 +143,8 @@ void WheeledVehicleControllerSettings::SaveBinaryState(StreamOut &inStream) cons
 	inStream.Write(num_differentials);
 	for (const VehicleDifferentialSettings &d : mDifferentials)
 		d.SaveBinaryState(inStream);
+
+	inStream.Write(mDifferentialLimitedSlipRatio);
 }
 
 void WheeledVehicleControllerSettings::RestoreBinaryState(StreamIn &inStream)
@@ -151,6 +158,8 @@ void WheeledVehicleControllerSettings::RestoreBinaryState(StreamIn &inStream)
 	mDifferentials.resize(num_differentials);
 	for (VehicleDifferentialSettings &d : mDifferentials)
 		d.RestoreBinaryState(inStream);
+
+	inStream.Read(mDifferentialLimitedSlipRatio);
 }
 
 WheeledVehicleController::WheeledVehicleController(const WheeledVehicleControllerSettings &inSettings, VehicleConstraint &inConstraint) :
@@ -173,6 +182,7 @@ WheeledVehicleController::WheeledVehicleController(const WheeledVehicleControlle
 	JPH_ASSERT(inSettings.mTransmission.mShiftDownRPM > 0.0f);
 	JPH_ASSERT(inSettings.mTransmission.mMode != ETransmissionMode::Auto || inSettings.mTransmission.mShiftUpRPM < inSettings.mEngine.mMaxRPM);
 	JPH_ASSERT(inSettings.mTransmission.mShiftUpRPM > inSettings.mTransmission.mShiftDownRPM);
+	JPH_ASSERT(inSettings.mTransmission.mClutchStrength > 0.0f);
 
 	// Copy differential settings
 	mDifferentials.resize(inSettings.mDifferentials.size());
@@ -183,7 +193,11 @@ WheeledVehicleController::WheeledVehicleController(const WheeledVehicleControlle
 		JPH_ASSERT(d.mDifferentialRatio > 0.0f);
 		JPH_ASSERT(d.mLeftRightSplit >= 0.0f && d.mLeftRightSplit <= 1.0f);
 		JPH_ASSERT(d.mEngineTorqueRatio >= 0.0f);
+		JPH_ASSERT(d.mLimitedSlipRatio > 1.0f);
 	}
+
+	mDifferentialLimitedSlipRatio = inSettings.mDifferentialLimitedSlipRatio;
+	JPH_ASSERT(mDifferentialLimitedSlipRatio > 1.0f);
 }
 
 void WheeledVehicleController::PreCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem)
@@ -203,6 +217,9 @@ void WheeledVehicleController::PostCollide(float inDeltaTime, PhysicsSystem &inP
 {
 	JPH_PROFILE_FUNCTION();
 
+	// Remember old RPM so we're increasing or decreasing
+	float old_engine_rpm = mEngine.GetCurrentRPM();
+
 	Wheels &wheels = mConstraint.GetWheels();
 
 	// Update wheel angle, do this before applying torque to the wheels (as friction will slow them down again)
@@ -211,97 +228,308 @@ void WheeledVehicleController::PostCollide(float inDeltaTime, PhysicsSystem &inP
 		WheelWV *w = static_cast<WheelWV *>(w_base);
 		w->Update(inDeltaTime, mConstraint);
 	}
-	
-	// First calculate engine speed based on speed of all wheels
-	bool can_engine_apply_torque = false;
-	if (mTransmission.GetCurrentGear() != 0 && mTransmission.GetClutchFriction() > 1.0e-3f)
+
+	// In auto transmission mode, don't accelerate the engine when switching gears
+	float forward_input = abs(mForwardInput);
+	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);
+
+	// Calculate engine torque
+	float engine_torque = mEngine.GetTorque(forward_input);
+
+	// Define a struct that contains information about driven differentials (i.e. that have wheels connected)
+	struct DrivenDifferential
 	{
-		float transmission_ratio = mTransmission.GetCurrentRatio();
-		bool forward = transmission_ratio >= 0.0f;
-		float slowest_wheel_speed = forward? FLT_MAX : -FLT_MAX;
-		for (const VehicleDifferentialSettings &d : mDifferentials)
-			if (d.mEngineTorqueRatio > 0.0f)
+		const VehicleDifferentialSettings *	mDifferential;
+		float								mAngularVelocity;
+		float								mClutchToDifferentialTorqueRatio;
+		float								mTempTorqueFactor;
+	};
+
+	// Collect driven differentials and their speeds
+	Array<DrivenDifferential> driven_differentials;
+	driven_differentials.reserve(mDifferentials.size());
+	float differential_omega_min = FLT_MAX, differential_omega_max = 0.0f;
+	for (const VehicleDifferentialSettings &d : mDifferentials)
+	{
+		float avg_omega = 0.0f;
+		int avg_omega_denom = 0;
+		int indices[] = { d.mLeftWheel, d.mRightWheel };
+		for (int idx : indices)
+			if (idx != -1)
 			{
-				if (d.mLeftWheel != -1 && d.mLeftRightSplit < 1.0f)
-				{
-					const Wheel *w = wheels[d.mLeftWheel];
-					if (forward)
-						slowest_wheel_speed = min(slowest_wheel_speed, w->GetAngularVelocity() * d.mDifferentialRatio);
-					else
-						slowest_wheel_speed = max(slowest_wheel_speed, w->GetAngularVelocity() * d.mDifferentialRatio);
-					can_engine_apply_torque |= w->HasContact();
-				}
-				if (d.mRightWheel != -1 && d.mLeftRightSplit > 0.0f)
-				{
-					const Wheel *w = wheels[d.mRightWheel];
-					if (forward)
-						slowest_wheel_speed = min(slowest_wheel_speed, w->GetAngularVelocity() * d.mDifferentialRatio);
-					else
-						slowest_wheel_speed = max(slowest_wheel_speed, w->GetAngularVelocity() * d.mDifferentialRatio);
-					can_engine_apply_torque |= w->HasContact();
-				}
+				avg_omega += wheels[idx]->GetAngularVelocity();
+				avg_omega_denom++;
 			}
 
-		// Update RPM only if the wheels are connected to the engine
-		if (slowest_wheel_speed > -FLT_MAX && slowest_wheel_speed < FLT_MAX)
-			mEngine.SetCurrentRPM(slowest_wheel_speed * transmission_ratio * VehicleEngine::cAngularVelocityToRPM);
-		mEngine.SetCurrentRPM(Clamp(mEngine.GetCurrentRPM(), mEngine.mMinRPM, mEngine.mMaxRPM));
+		if (avg_omega_denom > 0)
+		{
+			avg_omega = abs(avg_omega * d.mDifferentialRatio / float(avg_omega_denom)); // ignoring that the differentials may be rotating in different directions
+			driven_differentials.push_back({ &d, avg_omega, d.mEngineTorqueRatio, 0 });
+
+			// Remember min and max velocity
+			differential_omega_min = min(differential_omega_min, avg_omega);
+			differential_omega_max = max(differential_omega_max, avg_omega);
+		}
 	}
-	else
+
+	if (mDifferentialLimitedSlipRatio < FLT_MAX					// Limited slip differential needs to be turned on
+		&& differential_omega_max > differential_omega_min)		// There needs to be a velocity difference
 	{
-		// In auto transmission mode, don't accelerate the engine when switching gears
-		float forward_input = mTransmission.mMode == ETransmissionMode::Manual? abs(mForwardInput) : 0.0f;
+		// Calculate factor based on relative speed of a differential
+		float sum_factor = 0.0f;
+		for (DrivenDifferential &d : driven_differentials)
+		{
+			// Differential with max velocity gets factor 0, differential with min velocity 1
+			d.mTempTorqueFactor = (differential_omega_max - d.mAngularVelocity) / (differential_omega_max - differential_omega_min);
+			sum_factor += d.mTempTorqueFactor;
+		}
+
+		// Normalize the result
+		for (DrivenDifferential &d : driven_differentials)
+			d.mTempTorqueFactor /= sum_factor;
+
+		// Prevent div by zero
+		differential_omega_min = max(1.0e-3f, differential_omega_min);
+		differential_omega_max = max(1.0e-3f, differential_omega_max);
 
-		// Engine not connected to wheels, update RPM based on engine inertia alone
-		mEngine.UpdateRPM(inDeltaTime, forward_input);
+		// Map into a value that is 0 when the wheels are turning at an equal rate and 1 when the wheels are turning at mDifferentialLimitedSlipRatio
+		float alpha = min((differential_omega_max / differential_omega_min - 1.0f) / (mDifferentialLimitedSlipRatio - 1.0f), 1.0f);
+		JPH_ASSERT(alpha >= 0.0f);
+		float one_min_alpha = 1.0f - alpha;
+
+		// Update torque ratio for all differentials
+		for (DrivenDifferential &d : driven_differentials)
+			d.mClutchToDifferentialTorqueRatio = one_min_alpha * d.mClutchToDifferentialTorqueRatio + alpha * d.mTempTorqueFactor;
 	}
 
-	// Update transmission
-	mTransmission.Update(inDeltaTime, mEngine.GetCurrentRPM(), mForwardInput, can_engine_apply_torque);
+#ifdef JPH_ENABLE_ASSERTS
+	// Assert the values add up to 1
+	float sum_torque_factors = 0.0f;
+	for (DrivenDifferential &d : driven_differentials)
+		sum_torque_factors += d.mClutchToDifferentialTorqueRatio;
+	JPH_ASSERT(abs(sum_torque_factors - 1.0f) < 1.0e-6f);
+#endif // JPH_ENABLE_ASSERTS
 
-	// Calculate the amount of torque the transmission gives to the differentials
+	// Define a struct that collects information about the wheels that connect to the engine
+	struct DrivenWheel
+	{
+		WheelWV *				mWheel;
+		float					mClutchToWheelRatio;
+		float					mClutchToWheelTorqueRatio;
+	};
+	Array<DrivenWheel> driven_wheels;
+	driven_wheels.reserve(wheels.size());
+
+	// Collect driven wheels
 	float transmission_ratio = mTransmission.GetCurrentRatio();
-	float transmission_torque = mTransmission.GetClutchFriction() * transmission_ratio * mEngine.GetTorque(abs(mForwardInput));
-	if (transmission_torque != 0.0f)
+	for (const DrivenDifferential &dd : driven_differentials)
+	{
+		VehicleDifferentialSettings d = *dd.mDifferential;
+
+		WheelWV *wl = d.mLeftWheel != -1? static_cast<WheelWV *>(wheels[d.mLeftWheel]) : nullptr;
+		WheelWV *wr = d.mRightWheel != -1? static_cast<WheelWV *>(wheels[d.mRightWheel]) : nullptr;
+
+		float clutch_to_wheel_ratio = transmission_ratio * d.mDifferentialRatio;
+
+		if (wl != nullptr && wr != nullptr)
+		{
+			// Calculate torque ratio
+			float ratio_l, ratio_r;
+			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 });
+		}
+		else if (wl != nullptr)
+		{
+			// Only left wheel, all power to left
+			driven_wheels.push_back({ wl, clutch_to_wheel_ratio, dd.mClutchToDifferentialTorqueRatio });
+		}
+		else if (wr != nullptr)
+		{
+			// Only right wheel, all power to right
+			driven_wheels.push_back({ wr, clutch_to_wheel_ratio, dd.mClutchToDifferentialTorqueRatio });
+		}
+	}
+
+	bool solved = false;
+	if (!driven_wheels.empty())
 	{
-		// Calculate the max angular velocity of the differential given current engine RPM
-		// Note this adds 0.1% slop to avoid numerical accuracy issues
-		float differential_max_angular_velocity = mEngine.GetCurrentRPM() / (transmission_ratio * VehicleEngine::cAngularVelocityToRPM) * 1.001f;
+		// Define the torque at the clutch at time t as:
+		// 
+		// tc(t):=S*(we(t)-sum(R(j)*ww(j,t),j,1,N)/N)
+		//
+		// Where:
+		// S is the total strength of clutch (= friction * strength)
+		// we(t) is the engine angular velocity at time t
+		// R(j) is the total gear ratio of clutch to wheel for wheel j
+		// ww(j,t) is the angular velocity of wheel j at time t
+		// N is the amount of wheels
+		//
+		// The torque that increases the engine angular velocity at time t is:
+		// 
+		// te(t):=TE-tc(t)
+		//
+		// Where:
+		// TE is the torque delivered by the engine
+		//
+		// The torque that increases the wheel angular velocity for wheel i at time t is:
+		//
+		// tw(i,t):=TW(i)+R(i)*F(i)*tc(t)
+		//
+		// Where:
+		// TW(i) is the torque applied to the wheel outside of the engine (brake + torque due to friction with the ground)
+		// F(i) is the fraction of the engine torque applied from engine to wheel i
+		//
+		// Because the angular accelaration and torque are connected through: Torque = I * dw/dt
+		//
+		// We have the angular acceleration of the engine at time t:
+		//
+		// ddt_we(t):=te(t)/Ie
+		//
+		// Where:
+		// Ie is the inertia of the engine
+		//
+		// We have the angular acceleration of wheel i at time t:
+		//
+		// ddt_ww(i,t):=tw(i,t)/Iw(i)
+		//
+		// Where:
+		// Iw(i) is the inertia of wheel i
+		//
+		// We could take a simple Euler step to calculate the resulting accelerations but because the system is very stiff this turns out to be unstable, so we need to use implicit Euler instead:
+		//
+		// we(t+dt)=we(t)+dt*ddt_we(t+dt)
+		//
+		// and:
+		//
+		// ww(i,t+dt)=ww(i,t)+dt*ddt_ww(i,t+dt)
+		//
+		// Expanding both equations (the equations above are in wxMaxima format and this can easily be done by expand(%)):
+		//
+		// For wheel:
+		// 
+		// ww(i,t+dt) + (S*dt*F(i)*R(i)*sum(R(j)*ww(j,t+dt),j,1,N))/(N*Iw(i)) - (S*dt*F(i)*R(i)*we(t+dt))/Iw(i) = ww(i,t)+(dt*TW(i))/Iw(i)
+		//
+		// For engine:
+		//
+		// we(t+dt) + (S*dt*we(t+dt))/Ie - (S*dt*sum(R(j)*ww(j,t+dt),j,1,N))/(Ie*N) = we(t)+(TE*dt)/Ie
+		//
+		// Defining a vector w(t) = (ww(1, t), ww(2, t), ..., ww(N, t), we(t)) we can write both equations as a matrix multiplication:
+		//
+		// 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;
+
+		// Last column of w is for the engine angular velocity
+		int engine = n - 1;
+
+		// Define a and b
+		DynMatrix a(n, n);
+		DynMatrix b(n, 1);
+
+		// Get number of driven wheels as a float
+		float num_driven_wheels_float = float(driven_wheels.size());
+	
+		// Angular velocity of engine
+		float w_engine = mEngine.GetAngularVelocity();
+
+		// Calculate the total strength of the clutch
+		float clutch_strength = transmission_ratio != 0.0f? mTransmission.GetClutchFriction() * mTransmission.mClutchStrength : 0.0f;
+
+		// dt / Ie
+		float dt_div_ie = inDeltaTime / mEngine.mInertia;
+
+		// Iterate the rows for the wheels
+		for (int i = 0; i < (int)driven_wheels.size(); ++i)
+		{
+			const DrivenWheel &w_i = driven_wheels[i];
 
-		// Apply the transmission torque to the wheels
-		for (const VehicleDifferentialSettings &d : mDifferentials)
-			if (d.mEngineTorqueRatio > 0.0f)
+			// dt / Iw
+			float dt_div_iw = inDeltaTime / w_i.mWheel->GetSettings()->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;
+
+			// Fill in the columns of a for wheel j
+			for (int j = 0; j < (int)driven_wheels.size(); ++j)
 			{
-				// Calculate torque on this differential
-				float differential_torque = d.mEngineTorqueRatio * d.mDifferentialRatio * transmission_torque;
+				const DrivenWheel &w_j = driven_wheels[j];
+				a(i, j) = dt_s_r_f_div_iw * w_j.mClutchToWheelRatio / num_driven_wheels_float;
+			}
 
-				// Calculate max angular velocity for wheels on this differential
-				float wheel_max_angular_velocity = differential_max_angular_velocity / d.mDifferentialRatio;
+			// Add ww(i, t+dt)
+			a(i, i) += 1.0f;
 
-				// Left wheel
-				if (d.mLeftWheel != -1 && d.mLeftRightSplit < 1.0f)
-				{
-					WheelWV *w = static_cast<WheelWV *>(wheels[d.mLeftWheel]);
-					if (w->GetAngularVelocity() * wheel_max_angular_velocity < 0.0f || abs(w->GetAngularVelocity()) < abs(wheel_max_angular_velocity))
-					{
-						float wheel_torque = differential_torque * (1.0f - d.mLeftRightSplit);
-						w->ApplyTorque(wheel_torque, inDeltaTime);
-					}
-				}
+			// Add the column for the engine
+			a(i, engine) = -dt_s_r_f_div_iw;
 
-				// Right wheel
-				if (d.mRightWheel != -1 && d.mLeftRightSplit > 0.0f)
-				{
-					WheelWV *w = static_cast<WheelWV *>(wheels[d.mRightWheel]);
-					if (w->GetAngularVelocity() * wheel_max_angular_velocity < 0.0f || abs(w->GetAngularVelocity()) < abs(wheel_max_angular_velocity))
-					{
-						float wheel_torque = differential_torque * d.mLeftRightSplit;
-						w->ApplyTorque(wheel_torque, inDeltaTime);
-					}
-				}
+			// Fill in the constant b
+			b(i, 0) = w_i.mWheel->GetAngularVelocity(); // + dt_div_iw * (brake and tire torques)
+
+			// 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;
+		}
+
+		// Finalize the engine row
+		a(engine, engine) = (1.0f + dt_div_ie * clutch_strength);
+		b(engine, 0) = w_engine + dt_div_ie * engine_torque;
+
+		// Solve the linear equation
+		if (GaussianElimination(a, b))
+		{
+			// 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));
 			}
+
+			// Update the engine RPM
+			mEngine.SetCurrentRPM(b(engine, 0) * VehicleEngine::cAngularVelocityToRPM);
+
+			// The speeds have been solved
+			solved = true;
+		}
+		else
+		{
+			JPH_ASSERT(false, "New engine/wheel speeds could not be calculated!");
+		}
+	}
+
+	if (!solved)
+	{
+		// Engine not connected to wheels, apply all torque to engine rotation
+		mEngine.ApplyTorque(engine_torque, inDeltaTime);
 	}
 
+	// Calculate if any of the wheels are slipping, this is used to prevent gear switching
+	bool wheels_slipping = false;
+	for (const DrivenWheel &w : driven_wheels)
+		wheels_slipping |= w.mClutchToWheelTorqueRatio > 0.0f && (!w.mWheel->HasContact() || w.mWheel->mLongitudinalSlip > 0.1f);
+
+	// Only allow shifting up when we're not slipping and we're increasing our RPM.
+	// After a jump, we have a very high engine RPM but once we hit the ground the RPM should be decreasing and we don't want to shift up
+	// during that time.
+	bool can_shift_up = !wheels_slipping && mEngine.GetCurrentRPM() >= old_engine_rpm;
+
+	// Update transmission
+	mTransmission.Update(inDeltaTime, mEngine.GetCurrentRPM(), mForwardInput, can_shift_up);
+	
 	// Braking
 	for (Wheel *w_base : wheels)
 	{
@@ -417,8 +645,8 @@ void WheeledVehicleController::Draw(DebugRenderer *inRenderer) const
 	mEngine.DrawRPM(inRenderer, rpm_meter_pos, rpm_meter_fwd, rpm_meter_up, mRPMMeterSize, mTransmission.mShiftDownRPM, mTransmission.mShiftUpRPM);
 
 	// Draw current vehicle state
-	String status = StringFormat("Forward: %.1f, Right: %.1f, Brake: %.1f, HandBrake: %.1f\n"
-								 "Gear: %d, Clutch: %.1f, EngineRPM: %.0f, V: %.1f km/h", 
+	String status = StringFormat("Forward: %.1f, Right: %.1f\nBrake: %.1f, HandBrake: %.1f\n"
+								 "Gear: %d, Clutch: %.1f\nEngineRPM: %.0f, V: %.1f km/h", 
 								 (double)mForwardInput, (double)mRightInput, (double)mBrakeInput, (double)mHandBrakeInput, 
 								 mTransmission.GetCurrentGear(), (double)mTransmission.GetClutchFriction(), (double)mEngine.GetCurrentRPM(), (double)body->GetLinearVelocity().Length() * 3.6);
 	inRenderer->DrawText3D(body->GetPosition(), status, Color::sWhite, mConstraint.GetDrawConstraintSize());
@@ -439,7 +667,7 @@ void WheeledVehicleController::Draw(DebugRenderer *inRenderer) const
 			inRenderer->DrawLine(w->GetContactPosition(), w->GetContactPosition() + w->GetContactLongitudinal(), Color::sRed);
 			inRenderer->DrawLine(w->GetContactPosition(), w->GetContactPosition() + w->GetContactLateral(), Color::sBlue);
 
-			DebugRenderer::sInstance->DrawText3D(w->GetContactPosition(), StringFormat("W: %.1f, S: %.2f, FrLateral: %.1f, FrLong: %.1f", (double)w->GetAngularVelocity(), (double)w->GetSuspensionLength(), (double)w->mCombinedLateralFriction, (double)w->mCombinedLongitudinalFriction), Color::sWhite, 0.1f);
+			DebugRenderer::sInstance->DrawText3D(w->GetContactPosition(), StringFormat("W: %.1f, S: %.2f\nSlip: %.2f, FrLateral: %.1f, FrLong: %.1f", (double)w->GetAngularVelocity(), (double)w->GetSuspensionLength(), (double)w->mLongitudinalSlip, (double)w->mCombinedLateralFriction, (double)w->mCombinedLongitudinalFriction), Color::sWhite, 0.1f);
 		}
 		else
 		{

Jolt/Physics/Vehicle/WheeledVehicleController.h

@@ -57,6 +57,7 @@ public:
 	/// Update the wheel rotation based on the current angular velocity
 	void						Update(float inDeltaTime, const VehicleConstraint &inConstraint);
 
+	float						mLongitudinalSlip = 0.0f;					///< Velocity difference between ground and wheel relative to ground velocity
 	float						mCombinedLongitudinalFriction = 0.0f;		///< Combined friction coefficient in longitudinal direction (combines terrain and tires)
 	float						mCombinedLateralFriction = 0.0f;			///< Combined friction coefficient in lateral direction (combines terrain and tires)
 	float						mBrakeImpulse = 0.0f;						///< Amount of impulse that the brakes can apply to the floor (excluding friction)
@@ -79,6 +80,7 @@ public:
 	VehicleEngineSettings		mEngine;									///< The properties of the engine
 	VehicleTransmissionSettings	mTransmission;								///< The properties of the transmission (aka gear box)
 	Array<VehicleDifferentialSettings> mDifferentials;						///< List of differentials and their properties
+	float						mDifferentialLimitedSlipRatio = 1.4f;		///< Ratio max / min average wheel speed of each differential (measured at the clutch). When the ratio is exceeded all torque gets distributed to the differential with the minimal average velocity. This allows implementing a limited slip differential between differentials. Set to FLT_MAX for an open differential. Value should be > 1.
 };
 
 /// Runtime controller class
@@ -118,6 +120,10 @@ public:
 	/// Get the differentials this vehicle has (writable interface, allows you to make changes to the configuration which will take effect the next time step)
 	Differentials &				GetDifferentials()							{ return mDifferentials; }
 
+	/// Ratio max / min average wheel speed of each differential (measured at the clutch).
+	float						GetDifferentialLimitedSlipRatio() const		{ return mDifferentialLimitedSlipRatio; }
+	void						SetDifferentialLimitedSlipRatio(float inV)	{ mDifferentialLimitedSlipRatio = inV; }
+
 #ifdef JPH_DEBUG_RENDERER
 	/// Debug drawing of RPM meter
 	void						SetRPMMeter(Vec3Arg inPosition, float inSize) { mRPMMeterPosition = inPosition; mRPMMeterSize = inSize; }
@@ -126,6 +132,7 @@ public:
 protected:
 	// See: VehicleController
 	virtual Wheel *				ConstructWheel(const WheelSettings &inWheel) const override { JPH_ASSERT(IsKindOf(&inWheel, JPH_RTTI(WheelSettingsWV))); return new WheelWV(static_cast<const WheelSettingsWV &>(inWheel)); }
+	virtual bool				AllowSleep() const override					{ return mForwardInput == 0.0f; }
 	virtual void				PreCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem) override;
 	virtual void				PostCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem) override;
 	virtual bool				SolveLongitudinalAndLateralConstraints(float inDeltaTime) override;
@@ -145,6 +152,7 @@ protected:
 	VehicleEngine				mEngine;									///< Engine state of the vehicle
 	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).
 
 #ifdef JPH_DEBUG_RENDERER
 	// Debug settings

Samples/Tests/Character/CharacterBaseTest.cpp

@@ -464,7 +464,7 @@ void CharacterBaseTest::CreateSettingsMenu(DebugUI *inUI, UIElement *inSubMenu)
 	inUI->CreateTextButton(inSubMenu, "Configuration Settings", [=]() {
 		UIElement *configuration_settings = inUI->CreateMenu();
 
-		inUI->CreateComboBox(configuration_settings, "Shape Type", { "Capsule", "Cylinder", "Box" }, (int)sShapeType, [this](int inItem) { sShapeType = (EType)inItem; });
+		inUI->CreateComboBox(configuration_settings, "Shape Type", { "Capsule", "Cylinder", "Box" }, (int)sShapeType, [](int inItem) { sShapeType = (EType)inItem; });
 		AddConfigurationSettings(inUI, configuration_settings);
 		inUI->CreateTextButton(configuration_settings, "Accept Changes", [=]() { RestartTest(); });
 		inUI->ShowMenu(configuration_settings);

Samples/Tests/Vehicle/VehicleConstraintTest.cpp

@@ -17,8 +17,6 @@ JPH_IMPLEMENT_RTTI_VIRTUAL(VehicleConstraintTest)
 	JPH_ADD_BASE_CLASS(VehicleConstraintTest, VehicleTest) 
 }
 
-int VehicleConstraintTest::sCollisionMode = 1;
-
 VehicleConstraintTest::~VehicleConstraintTest()
 {
 	mPhysicsSystem->RemoveStepListener(mVehicleConstraint);
@@ -57,21 +55,21 @@ void VehicleConstraintTest::Initialize()
 
 	// Wheels
 	WheelSettingsWV *w1 = new WheelSettingsWV;
-	w1->mPosition = Vec3(-half_vehicle_width, -0.9f * half_vehicle_height, half_vehicle_length - 2.0f * wheel_radius);
+	w1->mPosition = Vec3(half_vehicle_width, -0.9f * half_vehicle_height, half_vehicle_length - 2.0f * wheel_radius);
 	w1->mMaxSteerAngle = max_steering_angle;
 	w1->mMaxHandBrakeTorque = 0.0f; // Front wheel doesn't have hand brake
 
 	WheelSettingsWV *w2 = new WheelSettingsWV;
-	w2->mPosition = Vec3(half_vehicle_width, -0.9f * half_vehicle_height, half_vehicle_length - 2.0f * wheel_radius);
+	w2->mPosition = Vec3(-half_vehicle_width, -0.9f * half_vehicle_height, half_vehicle_length - 2.0f * wheel_radius);
 	w2->mMaxSteerAngle = max_steering_angle;
 	w2->mMaxHandBrakeTorque = 0.0f; // Front wheel doesn't have hand brake
 
 	WheelSettingsWV *w3 = new WheelSettingsWV;
-	w3->mPosition = Vec3(-half_vehicle_width, -0.9f * half_vehicle_height, -half_vehicle_length + 2.0f * wheel_radius);
+	w3->mPosition = Vec3(half_vehicle_width, -0.9f * half_vehicle_height, -half_vehicle_length + 2.0f * wheel_radius);
 	w3->mMaxSteerAngle = 0.0f;
 
 	WheelSettingsWV *w4 = new WheelSettingsWV;
-	w4->mPosition = Vec3(half_vehicle_width, -0.9f * half_vehicle_height, -half_vehicle_length + 2.0f * wheel_radius);
+	w4->mPosition = Vec3(-half_vehicle_width, -0.9f * half_vehicle_height, -half_vehicle_length + 2.0f * wheel_radius);
 	w4->mMaxSteerAngle = 0.0f;
 
 	vehicle.mWheels = { w1, w2, w3, w4 };
@@ -88,16 +86,27 @@ void VehicleConstraintTest::Initialize()
 	vehicle.mController = controller;
 
 	// Differential
-	controller->mDifferentials.resize(1);
+	controller->mDifferentials.resize(sFourWheelDrive? 2 : 1);
 	controller->mDifferentials[0].mLeftWheel = 0;
 	controller->mDifferentials[0].mRightWheel = 1;
+	if (sFourWheelDrive)
+	{
+		controller->mDifferentials[1].mLeftWheel = 2;
+		controller->mDifferentials[1].mRightWheel = 3;
+
+		// Split engine torque
+		controller->mDifferentials[0].mEngineTorqueRatio = controller->mDifferentials[1].mEngineTorqueRatio = 0.5f;
+	}
 
 	// Anti rollbars
-	vehicle.mAntiRollBars.resize(2);
-	vehicle.mAntiRollBars[0].mLeftWheel = 0;
-	vehicle.mAntiRollBars[0].mRightWheel = 1;
-	vehicle.mAntiRollBars[1].mLeftWheel = 2;
-	vehicle.mAntiRollBars[1].mRightWheel = 3;
+	if (sAntiRollbar)
+	{
+		vehicle.mAntiRollBars.resize(2);
+		vehicle.mAntiRollBars[0].mLeftWheel = 0;
+		vehicle.mAntiRollBars[0].mRightWheel = 1;
+		vehicle.mAntiRollBars[1].mLeftWheel = 2;
+		vehicle.mAntiRollBars[1].mRightWheel = 3;
+	}
 
 	mVehicleConstraint = new VehicleConstraint(*mCarBody, vehicle);
 	mPhysicsSystem->AddConstraint(mVehicleConstraint);
@@ -148,8 +157,20 @@ void VehicleConstraintTest::PrePhysicsUpdate(const PreUpdateParams &inParams)
 	if (right != 0.0f || forward != 0.0f || brake != 0.0f || hand_brake != 0.0f)
 		mBodyInterface->ActivateBody(mCarBody->GetID());
 
+	WheeledVehicleController *controller = static_cast<WheeledVehicleController *>(mVehicleConstraint->GetController());
+
+	// Update vehicle statistics
+	controller->GetEngine().mMaxTorque = sMaxEngineTorque;
+	controller->GetTransmission().mClutchStrength = sClutchStrength;
+
+	// Set slip ratios to the same for everything
+	float limited_slip_ratio = sLimitedSlipDifferentials? 1.4f : FLT_MAX;
+	controller->SetDifferentialLimitedSlipRatio(limited_slip_ratio);
+	for (VehicleDifferentialSettings &d : controller->GetDifferentials())
+		d.mLimitedSlipRatio = limited_slip_ratio;
+
 	// Pass the input on to the constraint
-	static_cast<WheeledVehicleController *>(mVehicleConstraint->GetController())->SetDriverInput(forward, right, brake, hand_brake);
+	controller->SetDriverInput(forward, right, brake, hand_brake);
 
 	// Set the collision tester
 	mVehicleConstraint->SetVehicleCollisionTester(mTesters[sCollisionMode]);
@@ -191,4 +212,9 @@ void VehicleConstraintTest::CreateSettingsMenu(DebugUI *inUI, UIElement *inSubMe
 	VehicleTest::CreateSettingsMenu(inUI, inSubMenu);
 
 	inUI->CreateComboBox(inSubMenu, "Collision Mode", { "Ray", "Cast Sphere" }, sCollisionMode, [](int inItem) { sCollisionMode = inItem; });
+	inUI->CreateCheckBox(inSubMenu, "4 Wheel Drive", sFourWheelDrive, [this](UICheckBox::EState inState) { sFourWheelDrive = inState == UICheckBox::STATE_CHECKED; RestartTest(); });
+	inUI->CreateCheckBox(inSubMenu, "Anti Rollbars", sAntiRollbar, [this](UICheckBox::EState inState) { sAntiRollbar = inState == UICheckBox::STATE_CHECKED; RestartTest(); });
+	inUI->CreateCheckBox(inSubMenu, "Limited Slip Differentials", sLimitedSlipDifferentials, [](UICheckBox::EState inState) { sLimitedSlipDifferentials = inState == UICheckBox::STATE_CHECKED; });
+	inUI->CreateSlider(inSubMenu, "Max Engine Torque", float(sMaxEngineTorque), 100.0f, 2000.0f, 10.0f, [](float inValue) { sMaxEngineTorque = inValue; });
+	inUI->CreateSlider(inSubMenu, "Clutch Strength", float(sClutchStrength), 1.0f, 40.0f, 1.0f, [](float inValue) { sClutchStrength = inValue; });
 }

Samples/Tests/Vehicle/VehicleConstraintTest.h

@@ -25,7 +25,12 @@ public:
 	virtual void				CreateSettingsMenu(DebugUI *inUI, UIElement *inSubMenu) override;
 
 private:
-	static int					sCollisionMode;
+	static inline int			sCollisionMode = 1;
+	static inline bool			sFourWheelDrive = false;
+	static inline bool			sAntiRollbar = true;
+	static inline bool			sLimitedSlipDifferentials = true;
+	static inline float			sMaxEngineTorque = 500.0f;
+	static inline float			sClutchStrength = 10.0f;
 
 	Body *						mCarBody;									///< The vehicle
 	Ref<VehicleConstraint>		mVehicleConstraint;							///< The vehicle constraint

Samples/Tests/Vehicle/VehicleTest.cpp

@@ -23,6 +23,7 @@ JPH_IMPLEMENT_RTTI_VIRTUAL(VehicleTest)
 const char *VehicleTest::sScenes[] =
 {
 	"Flat",
+	"Steep Slope",
 	"Playground",
 	"Terrain1",
 };
@@ -38,6 +39,13 @@ void VehicleTest::Initialize()
 		floor.SetFriction(1.0f);
 		mBodyInterface->AddBody(floor.GetID(), EActivation::DontActivate);
 	}
+	else if (strcmp(sSceneName, "Steep Slope") == 0)
+	{
+		// Steep slope test floor (20 degrees = 36% grade)
+		Body &floor = *mBodyInterface->CreateBody(BodyCreationSettings(new BoxShape(Vec3(1000.0f, 1.0f, 1000.0f), 0.0f), Vec3(0.0f, -1.0f, 0.0f), Quat::sRotation(Vec3::sAxisX(), DegreesToRadians(-20.0f)), EMotionType::Static, Layers::NON_MOVING));
+		floor.SetFriction(1.0f);
+		mBodyInterface->AddBody(floor.GetID(), EActivation::DontActivate);
+	}
 	else if (strcmp(sSceneName, "Playground") == 0)
 	{
 		// Scene with hilly terrain and some objects to drive into

UnitTests/Physics/WheeledVehicleTests.cpp

@@ -0,0 +1,237 @@
+// SPDX-FileCopyrightText: 2022 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include "PhysicsTestContext.h"
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Collision/Shape/OffsetCenterOfMassShape.h>
+#include <Jolt/Physics/Vehicle/WheeledVehicleController.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include "Layers.h"
+
+TEST_SUITE("WheeledVehicleTests")
+{
+	// Simplified vehicle settings
+	struct VehicleSettings
+	{
+		Vec3		mPosition { 0, 2, 0 };
+		bool		mUseCastSphere = true;
+		float		mWheelRadius = 0.3f;
+		float		mWheelWidth = 0.1f;
+		float		mHalfVehicleLength = 2.0f;
+		float		mHalfVehicleWidth = 0.9f;
+		float		mHalfVehicleHeight = 0.2f;
+		float		mWheelOffsetHorizontal = 1.4f;
+		float		mWheelOffsetVertical = 0.18f;
+		float		mSuspensionMinLength = 0.3f;
+		float		mSuspensionMaxLength = 0.5f;
+		float		mMaxSteeringAngle = DegreesToRadians(30);
+		bool		mFourWheelDrive = false;
+		bool		mAntiRollbar = true;
+	};
+
+	// Helper function to create a vehicle
+	static VehicleConstraint *AddVehicle(PhysicsTestContext &inContext, VehicleSettings &inSettings)
+	{
+		// Create vehicle body
+		RefConst<Shape> car_shape = OffsetCenterOfMassShapeSettings(Vec3(0, -inSettings.mHalfVehicleHeight, 0), new BoxShape(Vec3(inSettings.mHalfVehicleWidth, inSettings.mHalfVehicleHeight, inSettings.mHalfVehicleLength))).Create().Get();
+		BodyCreationSettings car_body_settings(car_shape, inSettings.mPosition, Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING);
+		car_body_settings.mOverrideMassProperties = EOverrideMassProperties::CalculateInertia;
+		car_body_settings.mMassPropertiesOverride.mMass = 1500.0f;
+		Body *car_body = inContext.GetBodyInterface().CreateBody(car_body_settings);
+		inContext.GetBodyInterface().AddBody(car_body->GetID(), EActivation::Activate);
+
+		// Create vehicle constraint
+		VehicleConstraintSettings vehicle;
+		vehicle.mDrawConstraintSize = 0.1f;
+		vehicle.mMaxPitchRollAngle = DegreesToRadians(60.0f);
+
+		// Wheels
+		WheelSettingsWV *w1 = new WheelSettingsWV;
+		w1->mPosition = Vec3(inSettings.mHalfVehicleWidth, -inSettings.mWheelOffsetVertical, inSettings.mWheelOffsetHorizontal);
+		w1->mMaxSteerAngle = inSettings.mMaxSteeringAngle;
+		w1->mMaxHandBrakeTorque = 0.0f; // Front wheel doesn't have hand brake
+
+		WheelSettingsWV *w2 = new WheelSettingsWV;
+		w2->mPosition = Vec3(-inSettings.mHalfVehicleWidth, -inSettings.mWheelOffsetVertical, inSettings.mWheelOffsetHorizontal);
+		w2->mMaxSteerAngle = inSettings.mMaxSteeringAngle;
+		w2->mMaxHandBrakeTorque = 0.0f; // Front wheel doesn't have hand brake
+
+		WheelSettingsWV *w3 = new WheelSettingsWV;
+		w3->mPosition = Vec3(inSettings.mHalfVehicleWidth, -inSettings.mWheelOffsetVertical, -inSettings.mWheelOffsetHorizontal);
+		w3->mMaxSteerAngle = 0.0f;
+
+		WheelSettingsWV *w4 = new WheelSettingsWV;
+		w4->mPosition = Vec3(-inSettings.mHalfVehicleWidth, -inSettings.mWheelOffsetVertical, -inSettings.mWheelOffsetHorizontal);
+		w4->mMaxSteerAngle = 0.0f;
+
+		vehicle.mWheels = { w1, w2, w3, w4 };
+	
+		for (WheelSettings *w : vehicle.mWheels)
+		{
+			w->mRadius = inSettings.mWheelRadius;
+			w->mWidth = inSettings.mWheelWidth;
+			w->mSuspensionMinLength = inSettings.mSuspensionMinLength;
+			w->mSuspensionMaxLength = inSettings.mSuspensionMaxLength;
+		}
+
+		WheeledVehicleControllerSettings *controller = new WheeledVehicleControllerSettings;
+		vehicle.mController = controller;
+
+		// Differential
+		controller->mDifferentials.resize(inSettings.mFourWheelDrive? 2 : 1);
+		controller->mDifferentials[0].mLeftWheel = 0;
+		controller->mDifferentials[0].mRightWheel = 1;
+		if (inSettings.mFourWheelDrive)
+		{
+			controller->mDifferentials[1].mLeftWheel = 2;
+			controller->mDifferentials[1].mRightWheel = 3;
+
+			// Split engine torque
+			controller->mDifferentials[0].mEngineTorqueRatio = controller->mDifferentials[1].mEngineTorqueRatio = 0.5f;
+		}
+
+		// Anti rollbars
+		if (inSettings.mAntiRollbar)
+		{
+			vehicle.mAntiRollBars.resize(2);
+			vehicle.mAntiRollBars[0].mLeftWheel = 0;
+			vehicle.mAntiRollBars[0].mRightWheel = 1;
+			vehicle.mAntiRollBars[1].mLeftWheel = 2;
+			vehicle.mAntiRollBars[1].mRightWheel = 3;
+		}
+
+		// Create the constraint
+		VehicleConstraint *constraint = new VehicleConstraint(*car_body, vehicle);
+
+		// Create collision tester
+		RefConst<VehicleCollisionTester> tester;
+		if (inSettings.mUseCastSphere)
+			tester = new VehicleCollisionTesterCastSphere(Layers::MOVING, 0.5f * inSettings.mWheelWidth);
+		else
+			tester = new VehicleCollisionTesterRay(Layers::MOVING);
+		constraint->SetVehicleCollisionTester(tester);
+
+		// Add to the world
+		inContext.GetSystem()->AddConstraint(constraint);
+		inContext.GetSystem()->AddStepListener(constraint);
+		return constraint;
+	}
+
+	static void CheckOnGround(VehicleConstraint *inConstraint, const VehicleSettings &inSettings, const BodyID &inGroundID)
+	{
+		// Between min and max suspension length
+		Vec3 pos = inConstraint->GetVehicleBody()->GetPosition();
+		CHECK(pos.GetY() > inSettings.mSuspensionMinLength + inSettings.mWheelOffsetVertical + inSettings.mHalfVehicleHeight); 
+		CHECK(pos.GetY() < inSettings.mSuspensionMaxLength + inSettings.mWheelOffsetVertical + inSettings.mHalfVehicleHeight);
+
+		// Wheels touching ground
+		for (const Wheel *w : inConstraint->GetWheels()) 
+			CHECK(w->GetContactBodyID() == inGroundID);
+	}
+
+	TEST_CASE("TestBasicWheeledVehicle")
+	{
+		PhysicsTestContext c;
+		BodyID floor_id = c.CreateFloor().GetID();
+
+		VehicleSettings settings;
+		VehicleConstraint *constraint = AddVehicle(c, settings);
+		Body *body = constraint->GetVehicleBody();
+		WheeledVehicleController *controller = static_cast<WheeledVehicleController *>(constraint->GetController());
+
+		// Should start at specified position
+		CHECK_APPROX_EQUAL(body->GetPosition(), settings.mPosition);
+
+		// After 1 step we should not be at ground yet
+		c.SimulateSingleStep();
+		for (const Wheel *w : constraint->GetWheels())
+			CHECK(w->GetContactBodyID().IsInvalid());
+		CHECK(controller->GetTransmission().GetCurrentGear() == 0);
+
+		// After 1 second we should be on ground but not moving horizontally
+		c.Simulate(1.0f);
+		CheckOnGround(constraint, settings, floor_id);
+		Vec3 pos1 = body->GetPosition();
+		CHECK_APPROX_EQUAL(pos1.GetX(), 0); // Not moving horizontally
+		CHECK_APPROX_EQUAL(pos1.GetZ(), 0);
+		CHECK(controller->GetTransmission().GetCurrentGear() == 0);
+
+		// Start driving forward
+		controller->SetDriverInput(1.0f, 0.0f, 0.0f, 0.0f);
+		c.GetBodyInterface().ActivateBody(body->GetID());
+		c.Simulate(1.0f);
+		CheckOnGround(constraint, settings, floor_id);
+		Vec3 pos2 = body->GetPosition();
+		CHECK_APPROX_EQUAL(pos2.GetX(), 0, 1.0e-3f); // 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(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);
+		CheckOnGround(constraint, settings, floor_id);
+		CHECK(!body->IsActive()); // Car should have gone sleeping
+		Vec3 pos3 = body->GetPosition();
+		CHECK_APPROX_EQUAL(pos3.GetX(), 0, 1.0e-3f); // 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
+
+		// Start driving backwards
+		controller->SetDriverInput(-1.0f, 0.0f, 0.0f, 0.0f);
+		c.GetBodyInterface().ActivateBody(body->GetID());
+		c.Simulate(2.0f);
+		CheckOnGround(constraint, settings, floor_id);
+		Vec3 pos4 = body->GetPosition();
+		CHECK_APPROX_EQUAL(pos4.GetX(), 0, 1.0e-2f); // 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(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(3.0f);
+		CheckOnGround(constraint, settings, floor_id);
+		CHECK(!body->IsActive()); // Car should have gone sleeping
+		Vec3 pos5 = body->GetPosition();
+		CHECK_APPROX_EQUAL(pos5.GetX(), 0, 1.0e-2f); // 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
+
+		// Turn right
+		controller->SetDriverInput(1.0f, 1.0f, 0.0f, 0.0f);
+		c.GetBodyInterface().ActivateBody(body->GetID());
+		c.Simulate(1.0f);
+		CheckOnGround(constraint, settings, floor_id);
+		Vec3 omega = body->GetAngularVelocity();
+		CHECK(omega.GetY() < -0.4f); // Rotating right
+		CHECK(controller->GetTransmission().GetCurrentGear() > 0);
+
+		// Hand brake
+		controller->SetDriverInput(0.0f, 0.0f, 0.0f, 1.0f);
+		c.GetBodyInterface().ActivateBody(body->GetID());
+		c.Simulate(4.0f);
+		CheckOnGround(constraint, settings, floor_id);
+		CHECK(!body->IsActive()); // Car should have gone sleeping
+		vel = body->GetLinearVelocity();
+		CHECK_APPROX_EQUAL(vel, Vec3::sZero(), 1.0e-3f); // Not moving
+
+		// Turn left
+		controller->SetDriverInput(1.0f, -1.0f, 0.0f, 0.0f);
+		c.GetBodyInterface().ActivateBody(body->GetID());
+		c.Simulate(1.0f);
+		CheckOnGround(constraint, settings, floor_id);
+		omega = body->GetAngularVelocity();
+		CHECK(omega.GetY() > 0.4f); // Rotating left
+		CHECK(controller->GetTransmission().GetCurrentGear() > 0);
+	}
+}

UnitTests/UnitTests.cmake

@@ -50,6 +50,7 @@ set(UNIT_TESTS_SRC_FILES
 	${UNIT_TESTS_ROOT}/Physics/SliderConstraintTests.cpp
 	${UNIT_TESTS_ROOT}/Physics/SubShapeIDTest.cpp
 	${UNIT_TESTS_ROOT}/Physics/TransformedShapeTests.cpp
+	${UNIT_TESTS_ROOT}/Physics/WheeledVehicleTests.cpp
 	${UNIT_TESTS_ROOT}/PhysicsTestContext.cpp
 	${UNIT_TESTS_ROOT}/PhysicsTestContext.h
 	${UNIT_TESTS_ROOT}/UnitTestFramework.cpp