Added support for less than 1 collision test per simulation step for vehicle wheels. (#817)

This behavior can be configured differently when the vehicle is active / inactive. This can be used for LODding vehicles.
Also added vehicle stress test level and test for a vehicle on a dynamic block to check sleeping behavior.

Docs/ReleaseNotes.md

@@ -14,6 +14,7 @@ For breaking API changes see [this document](https://github.com/jrouwe/JoltPhysi
 * Added BodyInterface::SetUseManifoldReduction which will clear the contact cache and ensure that you get consistent contact callbacks in case the body that you're changing already has contacts.
 * Created implementations of BroadPhaseLayerInterface, ObjectVsBroadPhaseLayerFilter and ObjectLayerPairFilter that use a bit table internally. These make it easier to define ObjectLayers which object layers collide.
 * Support for compiling with ninja on Windows.
+* Added support for less than 1 collision test per simulation step for vehicle wheels. This behavior can be configured differently when the vehicle is active / inactive. This can be used for LODding vehicles.
 * Added wheel index and friction direction to VehicleConstraint::CombineFunction friction callback so you can have more differentiation between wheels.
 * Added ability to disable the lean steering limit for the motorcycle, turning this off makes the motorcycle more unstable, but gives you more control over the final steering angle.
 * Added function to query the bounding box of all bodies in the physics system, see PhysicsSystem::GetBounds.
@@ -32,8 +33,8 @@ For breaking API changes see [this document](https://github.com/jrouwe/JoltPhysi
 * Ability to stop overriding CMAKE_CXX_FLAGS_DEBUG/CMAKE_CXX_FLAGS_RELEASE which is important for Android as it uses a lot of extra flags. Set the OVERRIDE_CXX_FLAGS=NO cmake flag to enable this.
 * Reduced size of a contact constraint which saves a bit of memory during simulation.
 * Can now build a linux shared library using GCC.
-### Bug fixes
 
+### Bug fixes
 * Fixed mass scaling (as provided by the ContactListener) not applied correctly to CCD objects & during solve position constraints. This led to kinematic objects being pushed by dynamic objects.
 * Workaround for MSVC 17.8, limits.h doesn't include corecrt.h and triggers an error that \_\_STDC_WANT_SECURE_LIB\_\_ is not defined.
 * Fixed bug in MustIncludeC logic in GetClosestPointOnTriangle.

Jolt/Physics/Vehicle/VehicleCollisionTester.cpp

@@ -102,6 +102,26 @@ bool VehicleCollisionTesterRay::Collide(PhysicsSystem &inPhysicsSystem, const Ve
 	return true;
 }
 
+void VehicleCollisionTesterRay::PredictContactProperties(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&ioBody, SubShapeID &ioSubShapeID, RVec3 &ioContactPosition, Vec3 &ioContactNormal, float &ioSuspensionLength) const
+{
+	// Recalculate the contact points assuming the contact point is on an infinite plane
+	const WheelSettings *wheel_settings = inVehicleConstraint.GetWheel(inWheelIndex)->GetSettings();
+	float d_dot_n = inDirection.Dot(ioContactNormal);
+	if (d_dot_n < -1.0e-6f)
+	{
+		// Reproject the contact position using the suspension ray and the plane formed by the contact position and normal
+		ioContactPosition = inOrigin + Vec3(ioContactPosition - inOrigin).Dot(ioContactNormal) / d_dot_n * inDirection;
+
+		// The suspension length is simply the distance between the contact position and the suspension origin excluding the wheel radius
+		ioSuspensionLength = Clamp(Vec3(ioContactPosition - inOrigin).Dot(inDirection) - wheel_settings->mRadius, 0.0f, wheel_settings->mSuspensionMaxLength);
+	}
+	else
+	{
+		// If the normal is pointing away we assume there's no collision anymore
+		ioSuspensionLength = wheel_settings->mSuspensionMaxLength;
+	}
+}
+
 bool VehicleCollisionTesterCastSphere::Collide(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&outBody, SubShapeID &outSubShapeID, RVec3 &outContactPosition, Vec3 &outContactNormal, float &outSuspensionLength) const
 {
 	const DefaultBroadPhaseLayerFilter default_broadphase_layer_filter = inPhysicsSystem.GetDefaultBroadPhaseLayerFilter(mObjectLayer);
@@ -195,6 +215,29 @@ bool VehicleCollisionTesterCastSphere::Collide(PhysicsSystem &inPhysicsSystem, c
 	return true;
 }
 
+void VehicleCollisionTesterCastSphere::PredictContactProperties(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&ioBody, SubShapeID &ioSubShapeID, RVec3 &ioContactPosition, Vec3 &ioContactNormal, float &ioSuspensionLength) const
+{
+	// Recalculate the contact points assuming the contact point is on an infinite plane
+	const WheelSettings *wheel_settings = inVehicleConstraint.GetWheel(inWheelIndex)->GetSettings();
+	float d_dot_n = inDirection.Dot(ioContactNormal);
+	if (d_dot_n < -1.0e-6f)
+	{
+		// Reproject the contact position using the suspension cast sphere and the plane formed by the contact position and normal
+		// This solves x = inOrigin + fraction * inDirection and (x - ioContactPosition) . ioContactNormal = mRadius for fraction
+		float oc_dot_n = Vec3(ioContactPosition - inOrigin).Dot(ioContactNormal);
+		float fraction = (mRadius + oc_dot_n) / d_dot_n;
+		ioContactPosition = inOrigin + fraction * inDirection - mRadius * ioContactNormal;
+
+		// Calculate the new suspension length in the same way as the cast sphere normally does
+		ioSuspensionLength = Clamp(fraction + mRadius - wheel_settings->mRadius, 0.0f, wheel_settings->mSuspensionMaxLength);
+	}
+	else
+	{
+		// If the normal is pointing away we assume there's no collision anymore
+		ioSuspensionLength = wheel_settings->mSuspensionMaxLength;
+	}
+}
+
 bool VehicleCollisionTesterCastCylinder::Collide(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&outBody, SubShapeID &outSubShapeID, RVec3 &outContactPosition, Vec3 &outContactNormal, float &outSuspensionLength) const
 {
 	const DefaultBroadPhaseLayerFilter default_broadphase_layer_filter = inPhysicsSystem.GetDefaultBroadPhaseLayerFilter(mObjectLayer);
@@ -285,4 +328,28 @@ bool VehicleCollisionTesterCastCylinder::Collide(PhysicsSystem &inPhysicsSystem,
 	return true;
 }
 
+void VehicleCollisionTesterCastCylinder::PredictContactProperties(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&ioBody, SubShapeID &ioSubShapeID, RVec3 &ioContactPosition, Vec3 &ioContactNormal, float &ioSuspensionLength) const
+{
+	// Recalculate the contact points assuming the contact point is on an infinite plane
+	const WheelSettings *wheel_settings = inVehicleConstraint.GetWheel(inWheelIndex)->GetSettings();
+	float d_dot_n = inDirection.Dot(ioContactNormal);
+	if (d_dot_n < -1.0e-6f)
+	{
+		// Reproject the contact position using the suspension cast sphere and the plane formed by the contact position and normal
+		// This solves x = inOrigin + fraction * inDirection and (x - ioContactPosition) . ioContactNormal = wheel_radius for fraction
+		float wheel_radius = wheel_settings->mRadius;
+		float oc_dot_n = Vec3(ioContactPosition - inOrigin).Dot(ioContactNormal);
+		float fraction = (wheel_radius + oc_dot_n) / d_dot_n;
+		ioContactPosition = inOrigin + fraction * inDirection - wheel_radius * ioContactNormal;
+
+		// We're treating the wheel as a sphere instead of a cylinder to calculate the new suspension length
+		ioSuspensionLength = Clamp(fraction, 0.0f, wheel_settings->mSuspensionMaxLength);
+	}
+	else
+	{
+		// If the normal is pointing away we assume there's no collision anymore
+		ioSuspensionLength = wheel_settings->mSuspensionMaxLength;
+	}
+}
+
 JPH_NAMESPACE_END

Jolt/Physics/Vehicle/VehicleCollisionTester.h

@@ -59,6 +59,19 @@ public:
 	/// @return True when collision found, false if not
 	virtual bool					Collide(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&outBody, SubShapeID &outSubShapeID, RVec3 &outContactPosition, Vec3 &outContactNormal, float &outSuspensionLength) const = 0;
 
+	/// Do a cheap contact properties prediction based on the contact properties from the last collision test (provided as input parameters)
+	/// @param inPhysicsSystem The physics system that should be tested against
+	/// @param inVehicleConstraint The vehicle constraint
+	/// @param inWheelIndex Index of the wheel that we're testing collision for
+	/// @param inOrigin Origin for the test, corresponds to the world space position for the suspension attachment point
+	/// @param inDirection Direction for the test (unit vector, world space)
+	/// @param ioBody Body that the wheel previously collided with
+	/// @param ioSubShapeID Sub shape ID that the wheel collided with during the last check
+	/// @param ioContactPosition Contact point between wheel and floor during the last check, in world space
+	/// @param ioContactNormal Contact normal between wheel and floor during the last check, pointing away from the floor
+	/// @param ioSuspensionLength New length of the suspension [0, inSuspensionMaxLength]
+	virtual void					PredictContactProperties(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&ioBody, SubShapeID &ioSubShapeID, RVec3 &ioContactPosition, Vec3 &ioContactNormal, float &ioSuspensionLength) const = 0;
+
 protected:
 	const BroadPhaseLayerFilter	*	mBroadPhaseLayerFilter = nullptr;
 	const ObjectLayerFilter *		mObjectLayerFilter = nullptr;
@@ -80,6 +93,7 @@ public:
 
 	// See: VehicleCollisionTester
 	virtual bool					Collide(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&outBody, SubShapeID &outSubShapeID, RVec3 &outContactPosition, Vec3 &outContactNormal, float &outSuspensionLength) const override;
+	virtual void					PredictContactProperties(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&ioBody, SubShapeID &ioSubShapeID, RVec3 &ioContactPosition, Vec3 &ioContactNormal, float &ioSuspensionLength) const override;
 
 private:
 	Vec3							mUp;
@@ -101,6 +115,7 @@ public:
 
 	// See: VehicleCollisionTester
 	virtual bool					Collide(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&outBody, SubShapeID &outSubShapeID, RVec3 &outContactPosition, Vec3 &outContactNormal, float &outSuspensionLength) const override;
+	virtual void					PredictContactProperties(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&ioBody, SubShapeID &ioSubShapeID, RVec3 &ioContactPosition, Vec3 &ioContactNormal, float &ioSuspensionLength) const override;
 
 private:
 	float							mRadius;
@@ -121,6 +136,7 @@ public:
 
 	// See: VehicleCollisionTester
 	virtual bool					Collide(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&outBody, SubShapeID &outSubShapeID, RVec3 &outContactPosition, Vec3 &outContactNormal, float &outSuspensionLength) const override;
+	virtual void					PredictContactProperties(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&ioBody, SubShapeID &ioSubShapeID, RVec3 &ioContactPosition, Vec3 &ioContactNormal, float &ioSuspensionLength) const override;
 
 private:
 	float							mConvexRadiusFraction;

Jolt/Physics/Vehicle/VehicleConstraint.cpp

@@ -106,6 +106,9 @@ VehicleConstraint::VehicleConstraint(Body &inVehicleBody, const VehicleConstrain
 	mWheels.resize(inSettings.mWheels.size());
 	for (uint i = 0; i < mWheels.size(); ++i)
 		mWheels[i] = mController->ConstructWheel(*inSettings.mWheels[i]);
+
+	// Use the body ID as a seed for the step counter so that not all vehicles will update at the same time
+	mCurrentStep = uint32(Hash64(inVehicleBody.GetID().GetIndex()));
 }
 
 VehicleConstraint::~VehicleConstraint()
@@ -171,6 +174,9 @@ void VehicleConstraint::OnStep(float inDeltaTime, PhysicsSystem &inPhysicsSystem
 	// Calculate if this constraint is active by checking if our main vehicle body is active or any of the bodies we touch are active
 	mIsActive = mBody->IsActive();
 
+	// Test how often we need to update the wheels
+	uint num_steps_between_collisions = mIsActive? mNumStepsBetweenCollisionTestActive : mNumStepsBetweenCollisionTestInactive;
+
 	RMat44 body_transform = mBody->GetWorldTransform();
 
 	// Test collision for wheels
@@ -179,20 +185,51 @@ void VehicleConstraint::OnStep(float inDeltaTime, PhysicsSystem &inPhysicsSystem
 		Wheel *w = mWheels[wheel_index];
 		const WheelSettings *settings = w->mSettings;
 
-		// Reset contact
-		w->mContactBodyID = BodyID();
-		w->mContactBody = nullptr;
-		w->mContactSubShapeID = SubShapeID();
-		w->mSuspensionLength = settings->mSuspensionMaxLength;
-
-		// Test collision to find the floor
+		// Calculate suspension origin and direction
 		RVec3 ws_origin = body_transform * settings->mPosition;
 		Vec3 ws_direction = body_transform.Multiply3x3(settings->mSuspensionDirection);
-		if (mVehicleCollisionTester->Collide(inPhysicsSystem, *this, wheel_index, ws_origin, ws_direction, mBody->GetID(), w->mContactBody, w->mContactSubShapeID, w->mContactPosition, w->mContactNormal, w->mSuspensionLength))
+
+		// Test if we need to update this wheel
+		if (num_steps_between_collisions == 0
+			|| (mCurrentStep + wheel_index) % num_steps_between_collisions != 0)
+		{
+			// Simplified wheel contact test
+			if (!w->mContactBodyID.IsInvalid())
+			{
+				// Test if the body is still valid
+				w->mContactBody = inPhysicsSystem.GetBodyLockInterfaceNoLock().TryGetBody(w->mContactBodyID);
+				if (w->mContactBody == nullptr)
+				{
+					// It's not, forget the contact
+					w->mContactBodyID = BodyID();
+					w->mContactSubShapeID = SubShapeID();
+					w->mSuspensionLength = settings->mSuspensionMaxLength;
+				}
+				else
+				{
+					// Extrapolate the wheel contact properties
+					mVehicleCollisionTester->PredictContactProperties(inPhysicsSystem, *this, wheel_index, ws_origin, ws_direction, mBody->GetID(), w->mContactBody, w->mContactSubShapeID, w->mContactPosition, w->mContactNormal, w->mSuspensionLength);
+				}
+			}
+		}
+		else
 		{
-			// Store ID (pointer is not valid outside of the simulation step)
-			w->mContactBodyID = w->mContactBody->GetID();
+			// Full wheel contact test, start by resetting the contact data
+			w->mContactBodyID = BodyID();
+			w->mContactBody = nullptr;
+			w->mContactSubShapeID = SubShapeID();
+			w->mSuspensionLength = settings->mSuspensionMaxLength;
+
+			// Test collision to find the floor
+			if (mVehicleCollisionTester->Collide(inPhysicsSystem, *this, wheel_index, ws_origin, ws_direction, mBody->GetID(), w->mContactBody, w->mContactSubShapeID, w->mContactPosition, w->mContactNormal, w->mSuspensionLength))
+			{
+				// Store ID (pointer is not valid outside of the simulation step)
+				w->mContactBodyID = w->mContactBody->GetID();
+			}
+		}
 
+		if (w->mContactBody != nullptr)
+		{
 			// Store contact velocity, cache this as the contact body may be removed
 			w->mContactPointVelocity = w->mContactBody->GetPointVelocity(w->mContactPosition);
 
@@ -266,6 +303,9 @@ void VehicleConstraint::OnStep(float inDeltaTime, PhysicsSystem &inPhysicsSystem
 			}
 	if (mBody->GetAllowSleeping() != allow_sleep)
 		mBody->SetAllowSleeping(allow_sleep);
+
+	// Increment step counter
+	++mCurrentStep;
 }
 
 void VehicleConstraint::BuildIslands(uint32 inConstraintIndex, IslandBuilder &ioBuilder, BodyManager &inBodyManager)
@@ -575,8 +615,10 @@ void VehicleConstraint::SaveState(StateRecorder &inStream) const
 		inStream.Write(w->mAngularVelocity);
 		inStream.Write(w->mAngle);
 		inStream.Write(w->mContactBodyID); // Used by MotorcycleController::PreCollide
+		inStream.Write(w->mContactPosition); // Used by VehicleCollisionTester::PredictContactProperties
 		inStream.Write(w->mContactNormal); // Used by MotorcycleController::PreCollide
 		inStream.Write(w->mContactLateral); // Used by MotorcycleController::PreCollide
+		inStream.Write(w->mSuspensionLength); // Used by VehicleCollisionTester::PredictContactProperties
 
 		w->mSuspensionPart.SaveState(inStream);
 		w->mSuspensionMaxUpPart.SaveState(inStream);
@@ -586,6 +628,7 @@ void VehicleConstraint::SaveState(StateRecorder &inStream) const
 
 	inStream.Write(mPitchRollRotationAxis); // When rotation is too small we use last frame so we need to store it
 	mPitchRollPart.SaveState(inStream);
+	inStream.Write(mCurrentStep);
 }
 
 void VehicleConstraint::RestoreState(StateRecorder &inStream)
@@ -599,8 +642,10 @@ void VehicleConstraint::RestoreState(StateRecorder &inStream)
 		inStream.Read(w->mAngularVelocity);
 		inStream.Read(w->mAngle);
 		inStream.Read(w->mContactBodyID);
+		inStream.Read(w->mContactPosition);
 		inStream.Read(w->mContactNormal);
 		inStream.Read(w->mContactLateral);
+		inStream.Read(w->mSuspensionLength);
 		w->mContactBody = nullptr; // No longer valid
 
 		w->mSuspensionPart.RestoreState(inStream);
@@ -611,6 +656,7 @@ void VehicleConstraint::RestoreState(StateRecorder &inStream)
 
 	inStream.Read(mPitchRollRotationAxis);
 	mPitchRollPart.RestoreState(inStream);
+	inStream.Read(mCurrentStep);
 }
 
 Ref<ConstraintSettings> VehicleConstraint::GetConstraintSettings() const

Jolt/Physics/Vehicle/VehicleConstraint.h

@@ -161,6 +161,21 @@ public:
 	/// @param inWheelUp Unit vector that indicates up in model space of the wheel
 	RMat44						GetWheelWorldTransform(uint inWheelIndex, Vec3Arg inWheelRight, Vec3Arg inWheelUp) const;
 
+	/// Number of simulation steps between wheel collision tests when the vehicle is active. Default is 1. 0 = never, 1 = every step, 2 = every other step, etc.
+	/// Note that if a vehicle has multiple wheels and the number of steps > 1, the wheels will be tested in a round robin fashion.
+	/// If there are multiple vehicles, the tests will be spread out based on the BodyID of the vehicle.
+	/// If you set this to test less than every step, you may see simulation artifacts. This setting can be used to reduce the cost of simulating vehicles in the distance.
+	void						SetNumStepsBetweenCollisionTestActive(uint inSteps) { mNumStepsBetweenCollisionTestActive = inSteps; }
+	uint						GetNumStepsBetweenCollisionTestActive() const { return mNumStepsBetweenCollisionTestActive; }
+
+	/// Number of simulation steps between wheel collision tests when the vehicle is inactive. Default is 1. 0 = never, 1 = every step, 2 = every other step, etc.
+	/// Note that if a vehicle has multiple wheels and the number of steps > 1, the wheels will be tested in a round robin fashion.
+	/// If there are multiple vehicles, the tests will be spread out based on the BodyID of the vehicle.
+	/// This number can be lower than the number of steps when the vehicle is active as the only purpose of this test is
+	/// to allow the vehicle to wake up in response to bodies moving into the wheels but not touching the body of the vehicle.
+	void						SetNumStepsBetweenCollisionTestInactive(uint inSteps) { mNumStepsBetweenCollisionTestInactive = inSteps; }
+	uint						GetNumStepsBetweenCollisionTestInactive() const { return mNumStepsBetweenCollisionTestInactive; }
+
 	// Generic interface of a constraint
 	virtual bool				IsActive() const override					{ return mIsActive && Constraint::IsActive(); }
 	virtual void				NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override { /* Do nothing */ }
@@ -197,6 +212,9 @@ private:
 	Array<VehicleAntiRollBar>	mAntiRollBars;								///< Anti rollbars of the vehicle
 	VehicleController *			mController;								///< Controls the acceleration / declerration of the vehicle
 	bool						mIsActive = false;							///< If this constraint is active
+	uint						mNumStepsBetweenCollisionTestActive = 1;	///< Number of simulation steps between wheel collision tests when the vehicle is active
+	uint						mNumStepsBetweenCollisionTestInactive = 1;	///< Number of simulation steps between wheel collision tests when the vehicle is inactive
+	uint						mCurrentStep = 0;							///< Current step number, used to determine when to test a wheel
 
 	// Prevent vehicle from toppling over
 	float						mCosMaxPitchRollAngle;						///< Cos of the max pitch/roll angle

Samples/Samples.cmake

@@ -245,6 +245,8 @@ set(SAMPLES_SRC_FILES
 	${SAMPLES_ROOT}/Tests/Vehicle/VehicleConstraintTest.h
 	${SAMPLES_ROOT}/Tests/Vehicle/VehicleSixDOFTest.cpp
 	${SAMPLES_ROOT}/Tests/Vehicle/VehicleSixDOFTest.h
+	${SAMPLES_ROOT}/Tests/Vehicle/VehicleStressTest.cpp
+	${SAMPLES_ROOT}/Tests/Vehicle/VehicleStressTest.h
 	${SAMPLES_ROOT}/Tests/Vehicle/VehicleTest.cpp
 	${SAMPLES_ROOT}/Tests/Vehicle/VehicleTest.h
 	${SAMPLES_ROOT}/Tests/Water/WaterShapeTest.cpp

Samples/SamplesApp.cpp

@@ -297,6 +297,7 @@ JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, VehicleSixDOFTest)
 JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, VehicleConstraintTest)
 JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, MotorcycleTest)
 JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, TankTest)
+JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, VehicleStressTest)
 
 static TestNameAndRTTI sVehicleTests[] =
 {
@@ -304,6 +305,7 @@ static TestNameAndRTTI sVehicleTests[] =
 	{ "Motorcycle (VehicleConstraint)",		JPH_RTTI(MotorcycleTest) },
 	{ "Tank (VehicleConstraint)",			JPH_RTTI(TankTest) },
 	{ "Car (SixDOFConstraint)",				JPH_RTTI(VehicleSixDOFTest) },
+	{ "Vehicle Stress Test",				JPH_RTTI(VehicleStressTest) },
 };
 
 JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, SoftBodyShapesTest)

Samples/Tests/Vehicle/VehicleStressTest.cpp

@@ -0,0 +1,174 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/Vehicle/VehicleStressTest.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Vehicle/WheeledVehicleController.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Layers.h>
+#include <Renderer/DebugRendererImp.h>
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(VehicleStressTest)
+{
+	JPH_ADD_BASE_CLASS(VehicleStressTest, VehicleTest)
+}
+
+VehicleStressTest::~VehicleStressTest()
+{
+	for (Ref<VehicleConstraint> &c : mVehicles)
+		mPhysicsSystem->RemoveStepListener(c);
+}
+
+void VehicleStressTest::Initialize()
+{
+	CreateMeshTerrain();
+
+	// Create walls so the vehicles don't fall off
+	mBodyInterface->CreateAndAddBody(BodyCreationSettings(new BoxShape(Vec3(50.0f, 5.0f, 0.5f)), RVec3(0, 0, -50), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING), EActivation::DontActivate);
+	mBodyInterface->CreateAndAddBody(BodyCreationSettings(new BoxShape(Vec3(50.0f, 5.0f, 0.5f)), RVec3(0, 0, 50), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING), EActivation::DontActivate);
+	mBodyInterface->CreateAndAddBody(BodyCreationSettings(new BoxShape(Vec3(0.5f, 5.0f, 50.0f)), RVec3(-50, 0, 0), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING), EActivation::DontActivate);
+	mBodyInterface->CreateAndAddBody(BodyCreationSettings(new BoxShape(Vec3(0.5f, 5.0f, 50.0f)), RVec3(50, 0, 0), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING), EActivation::DontActivate);
+
+	const float wheel_radius = 0.3f;
+	const float wheel_width = 0.1f;
+	const float half_vehicle_length = 2.0f;
+	const float half_vehicle_width = 0.9f;
+	const float half_vehicle_height = 0.2f;
+	const float max_steering_angle = DegreesToRadians(30.0f);
+
+	// Create vehicle body
+	RefConst<Shape> car_shape = new BoxShape(Vec3(half_vehicle_width, half_vehicle_height, half_vehicle_length));
+	BodyCreationSettings car_body_settings(car_shape, RVec3::sZero(), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING);
+	car_body_settings.mOverrideMassProperties = EOverrideMassProperties::CalculateInertia;
+	car_body_settings.mMassPropertiesOverride.mMass = 1500.0f;
+
+	// Create vehicle constraint
+	VehicleConstraintSettings vehicle;
+
+	// Wheels, left front
+	WheelSettingsWV *w1 = new WheelSettingsWV;
+	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
+
+	// Right front
+	WheelSettingsWV *w2 = new WheelSettingsWV;
+	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
+
+	// Left rear
+	WheelSettingsWV *w3 = new WheelSettingsWV;
+	w3->mPosition = Vec3(half_vehicle_width, -0.9f * half_vehicle_height, -half_vehicle_length + 2.0f * wheel_radius);
+	w3->mMaxSteerAngle = 0.0f;
+
+	// Right rear
+	WheelSettingsWV *w4 = new WheelSettingsWV;
+	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 };
+
+	for (WheelSettings *w : vehicle.mWheels)
+	{
+		w->mRadius = wheel_radius;
+		w->mWidth = wheel_width;
+	}
+
+	// Controller
+	WheeledVehicleControllerSettings *controller = new WheeledVehicleControllerSettings;
+	vehicle.mController = controller;
+	vehicle.mMaxPitchRollAngle = DegreesToRadians(60.0f);
+
+	// Differential
+	controller->mDifferentials.resize(1);
+	controller->mDifferentials[0].mLeftWheel = 0;
+	controller->mDifferentials[0].mRightWheel = 1;
+
+	for (int x = 0; x < 15; ++x)
+		for (int y = 0; y < 15; ++y)
+		{
+			// Create body
+			car_body_settings.mPosition = RVec3(-28.0f + x * 4.0f, 2.0f, -35.0f + y * 5.0f);
+			Body *car_body = mBodyInterface->CreateBody(car_body_settings);
+			mBodyInterface->AddBody(car_body->GetID(), EActivation::Activate);
+
+			// Create constraint
+			VehicleConstraint *c = new VehicleConstraint(*car_body, vehicle);
+			c->SetNumStepsBetweenCollisionTestActive(2); // Only test collision every other step to speed up simulation
+			c->SetNumStepsBetweenCollisionTestInactive(0); // Disable collision testing when inactive
+
+			// Set the collision tester
+			VehicleCollisionTester *tester = new VehicleCollisionTesterRay(Layers::MOVING);
+			c->SetVehicleCollisionTester(tester);
+
+			// Add the vehicle
+			mPhysicsSystem->AddConstraint(c);
+			mPhysicsSystem->AddStepListener(c);
+			mVehicles.push_back(c);
+		}
+}
+
+void VehicleStressTest::ProcessInput(const ProcessInputParams &inParams)
+{
+	// Determine acceleration and brake
+	mForward = 0.0f;
+	if (inParams.mKeyboard->IsKeyPressed(DIK_UP))
+		mForward = 1.0f;
+	else if (inParams.mKeyboard->IsKeyPressed(DIK_DOWN))
+		mForward = -1.0f;
+
+	// Steering
+	mRight = 0.0f;
+	if (inParams.mKeyboard->IsKeyPressed(DIK_LEFT))
+		mRight = -1.0f;
+	else if (inParams.mKeyboard->IsKeyPressed(DIK_RIGHT))
+		mRight = 1.0f;
+
+	// Hand brake will cancel gas pedal
+	mHandBrake = 0.0f;
+	if (inParams.mKeyboard->IsKeyPressed(DIK_Z))
+	{
+		mForward = 0.0f;
+		mHandBrake = 1.0f;
+	}
+}
+
+void VehicleStressTest::PrePhysicsUpdate(const PreUpdateParams &inParams)
+{
+	for (VehicleConstraint *c : mVehicles)
+	{
+		// On user input, assure that the car is active
+		if (mRight != 0.0f || mForward != 0.0f)
+			mBodyInterface->ActivateBody(c->GetVehicleBody()->GetID());
+
+		// Pass the input on to the constraint
+		WheeledVehicleController *controller = static_cast<WheeledVehicleController *>(c->GetController());
+		controller->SetDriverInput(mForward, mRight, 0.0f, mHandBrake);
+
+		// Draw our wheels (this needs to be done in the pre update since we draw the bodies too in the state before the step)
+		for (uint w = 0; w < 4; ++w)
+		{
+			const WheelSettings *settings = c->GetWheels()[w]->GetSettings();
+			RMat44 wheel_transform = c->GetWheelWorldTransform(w, Vec3::sAxisY(), Vec3::sAxisX()); // The cyclinder we draw is aligned with Y so we specify that as rotational axis
+			mDebugRenderer->DrawCylinder(wheel_transform, 0.5f * settings->mWidth, settings->mRadius, Color::sGreen);
+		}
+	}
+}
+
+void VehicleStressTest::SaveInputState(StateRecorder &inStream) const
+{
+	inStream.Write(mForward);
+	inStream.Write(mRight);
+	inStream.Write(mHandBrake);
+}
+
+void VehicleStressTest::RestoreInputState(StateRecorder &inStream)
+{
+	inStream.Read(mForward);
+	inStream.Read(mRight);
+	inStream.Read(mHandBrake);
+}

Samples/Tests/Vehicle/VehicleStressTest.h

@@ -0,0 +1,33 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/Vehicle/VehicleTest.h>
+#include <Jolt/Physics/Vehicle/VehicleConstraint.h>
+
+// This test simulates a large amount of vehicles
+class VehicleStressTest : public Test
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, VehicleStressTest)
+
+	// Destructor
+	virtual							~VehicleStressTest() override;
+
+	// See: Test
+	virtual void					Initialize() override;
+	virtual void					ProcessInput(const ProcessInputParams &inParams) override;
+	virtual void					PrePhysicsUpdate(const PreUpdateParams &inParams) override;
+	virtual void					SaveInputState(StateRecorder &inStream) const override;
+	virtual void					RestoreInputState(StateRecorder &inStream) override;
+
+private:
+	Array<Ref<VehicleConstraint>>	mVehicles;							///< The vehicle constraints
+
+	// Player input
+	float							mForward = 0.0f;
+	float							mRight = 0.0f;
+	float							mHandBrake = 0.0f;
+};

Samples/Tests/Vehicle/VehicleTest.cpp

@@ -28,6 +28,7 @@ const char *VehicleTest::sScenes[] =
 	"Flat With Slope",
 	"Steep Slope",
 	"Step",
+	"Dynamic Step",
 	"Playground",
 	"Terrain1",
 };
@@ -85,6 +86,19 @@ void VehicleTest::Initialize()
 		step.SetFriction(1.0f);
 		mBodyInterface->AddBody(step.GetID(), EActivation::DontActivate);
 	}
+	else if (strcmp(sSceneName, "Dynamic Step") == 0)
+	{
+		// 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);
+
+		// A dynamic body that acts as a step to test sleeping behavior
+		constexpr float cStepHeight = 0.05f;
+		Body &step = *mBodyInterface->CreateBody(BodyCreationSettings(new BoxShape(Vec3(15.0f, 0.5f * cStepHeight, 15.0f), 0.0f), RVec3(-2.0f, 0.5f * cStepHeight, 30.0f), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING));
+		step.SetFriction(1.0f);
+		mBodyInterface->AddBody(step.GetID(), EActivation::Activate);
+	}
 	else if (strcmp(sSceneName, "Playground") == 0)
 	{
 		// Scene with hilly terrain and some objects to drive into