Bugfix: Fixed incorrect calculation of local constraint position which caused bodies under the wheels to act weirdly

- Added documentation to the vehicle constraint to list solutions to the problem of driving over light objects.
- Added rubble to the VehicleTest level

Jolt/Physics/Vehicle/VehicleConstraint.cpp

@@ -279,7 +279,7 @@ void VehicleConstraint::CalculateWheelContactPoint(Mat44Arg inBodyTransform, con
 {
 	Vec3 contact_pos = inBodyTransform * (inWheel.mSettings->mPosition + inWheel.mSettings->mDirection * inWheel.mContactLength);
 	outR1PlusU = contact_pos - mBody->GetCenterOfMassPosition();
-	outR2 = contact_pos - mBody->GetCenterOfMassPosition();
+	outR2 = contact_pos - inWheel.mContactBody->GetCenterOfMassPosition();
 }
 
 void VehicleConstraint::CalculatePitchRollConstraintProperties(float inDeltaTime, Mat44Arg inBodyTransform)

Jolt/Physics/Vehicle/VehicleConstraint.h

@@ -42,6 +42,26 @@ protected:
 
 /// Constraint that simulates a vehicle
 /// Note: Don't forget to register the constraint as a StepListener with the PhysicsSystem!
+///
+/// When the vehicle drives over very light objects (rubble) you may see the car body dip down. This is a known issue and is an artifact of the iterative solver that Jolt is using.
+/// Basically if a light object is sandwiched between two heavy objects (the static floor and the car body), the light object is not able to transfer enough force from the ground to
+/// the car body to keep the car body up. You can see this effect in the HeavyOnLightTest sample, the boxes on the right have a lot of penetration because they're on top of light objects.
+/// 
+/// There are a couple of ways to improve this:
+/// 
+/// 1. You can increase the number of velocity steps (global settings PhysicsSettings::mNumVelocitySteps or if you only want to increase it on
+/// the vehicle you can use VehicleConstraintSettings::mNumVelocityStepsOverride). E.g. going from 10 to 30 steps in the HeavyOnLightTest sample makes the penetration a lot less.
+/// The number of position steps can also be increased (the first prevents the body from going down, the second corrects it if the problem did
+/// occur which inevitably happens due to numerical drift). This solution costs CPU cycles.
+/// 
+/// 2. You can reduce the mass difference between the vehicle body and the rubble on the floor (by making the rubble heavier or the car lighter).
+///
+/// 3. You could filter out collisions between the vehicle collision test and the rubble completely. This would make the wheels ignore the rubble but would cause the vehicle to drive
+/// through it as if nothing happened. You could create fake wheels (keyframed bodies) that move along with the vehicle and that only collide with rubble (and not the vehicle or the ground).
+/// This would cause the vehicle to push away the rubble without the rubble being able to affect the vehicle (unless it hits the main body of course).
+///
+/// Note that when driving over rubble, you may see the wheel jump up and down quite quickly because one frame a collision is found and the next frame not.
+/// To alleviate this, it may be needed to smooth the motion of the visual mesh for the wheel.
 class VehicleConstraint : public Constraint, public PhysicsStepListener
 {
 public:

Samples/Tests/Vehicle/VehicleTest.cpp

@@ -6,6 +6,7 @@
 #include <Tests/Vehicle/VehicleTest.h>
 #include <Jolt/Physics/Constraints/DistanceConstraint.h>
 #include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Collision/Shape/ConvexHullShape.h>
 #include <Jolt/Physics/Collision/GroupFilterTable.h>
 #include <Jolt/Physics/Body/BodyCreationSettings.h>
 #include <Jolt/Physics/PhysicsScene.h>
@@ -46,6 +47,8 @@ void VehicleTest::Initialize()
 		CreateBridge();
 
 		CreateWall();
+
+		CreateRubble();
 	}	
 	else
 	{
@@ -114,8 +117,34 @@ void VehicleTest::CreateWall()
 		for (int j = i / 2; j < 5 - (i + 1) / 2; ++j)
 		{
 			Vec3 position(2.0f + j * 1.0f + (i & 1? 0.5f : 0.0f), 2.0f + i * 1.0f, 10.0f);
-			Body &wall = *mBodyInterface->CreateBody(BodyCreationSettings(box_shape, position, Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING));
-			mBodyInterface->AddBody(wall.GetID(), EActivation::Activate);
+			mBodyInterface->CreateAndAddBody(BodyCreationSettings(box_shape, position, Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING), EActivation::Activate);
+		}
+}
+
+void VehicleTest::CreateRubble()
+{
+	// Flat and light objects
+	RefConst<Shape> box_shape = new BoxShape(Vec3(0.5f, 0.1f, 0.5f));
+	for (int i = 0; i < 5; ++i)
+		for (int j = 0; j < 5; ++j)
+		{
+			Vec3 position(-5.0f + j, 2.0f + i * 0.2f, 10.0f + 0.5f * i);
+			mBodyInterface->CreateAndAddBody(BodyCreationSettings(box_shape, position, Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING), EActivation::Activate);
+		}
+
+
+	// Light convex shapes
+	default_random_engine random;
+	uniform_real_distribution<float> hull_size(0.2f, 0.4f);
+	for (int i = 0; i < 10; ++i)
+		for (int j = 0; j < 10; ++j)
+		{
+			// Create random points
+			Array<Vec3> points;
+			for (int k = 0; k < 20; ++k)
+				points.push_back(hull_size(random) * Vec3::sRandom(random));
+
+			mBodyInterface->CreateAndAddBody(BodyCreationSettings(new ConvexHullShapeSettings(points), Vec3(-5.0f + 0.5f * j, 2.0f, 15.0f + 0.5f * i), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING), EActivation::Activate);
 		}
 }
 

Samples/Tests/Vehicle/VehicleTest.h

@@ -27,4 +27,5 @@ private:
 
 	void					CreateBridge();
 	void					CreateWall();
+	void					CreateRubble();
 };