Added integral component to motor cycle lean angle spring so that it becomes a PID controller (#564)

* Added step scene to vehicle test to test vehicle response of driving over a small step

Jolt/Physics/Vehicle/MotorcycleController.cpp

@@ -22,6 +22,8 @@ JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(MotorcycleControllerSettings)
 	JPH_ADD_ATTRIBUTE(MotorcycleControllerSettings, mMaxLeanAngle)
 	JPH_ADD_ATTRIBUTE(MotorcycleControllerSettings, mLeanSpringConstant)
 	JPH_ADD_ATTRIBUTE(MotorcycleControllerSettings, mLeanSpringDamping)
+	JPH_ADD_ATTRIBUTE(MotorcycleControllerSettings, mLeanSpringIntegrationCoefficient)
+	JPH_ADD_ATTRIBUTE(MotorcycleControllerSettings, mLeanSpringIntegrationCoefficientDecay)
 	JPH_ADD_ATTRIBUTE(MotorcycleControllerSettings, mLeanSmoothingFactor)
 }
 
@@ -37,6 +39,8 @@ void MotorcycleControllerSettings::SaveBinaryState(StreamOut &inStream) const
 	inStream.Write(mMaxLeanAngle);
 	inStream.Write(mLeanSpringConstant);
 	inStream.Write(mLeanSpringDamping);
+	inStream.Write(mLeanSpringIntegrationCoefficient);
+	inStream.Write(mLeanSpringIntegrationCoefficientDecay);
 	inStream.Write(mLeanSmoothingFactor);
 }
 
@@ -47,6 +51,8 @@ void MotorcycleControllerSettings::RestoreBinaryState(StreamIn &inStream)
 	inStream.Read(mMaxLeanAngle);
 	inStream.Read(mLeanSpringConstant);
 	inStream.Read(mLeanSpringDamping);
+	inStream.Read(mLeanSpringIntegrationCoefficient);
+	inStream.Read(mLeanSpringIntegrationCoefficientDecay);
 	inStream.Read(mLeanSmoothingFactor);
 }
 
@@ -55,6 +61,8 @@ MotorcycleController::MotorcycleController(const MotorcycleControllerSettings &i
 	mMaxLeanAngle(inSettings.mMaxLeanAngle),
 	mLeanSpringConstant(inSettings.mLeanSpringConstant),
 	mLeanSpringDamping(inSettings.mLeanSpringDamping),
+	mLeanSpringIntegrationCoefficient(inSettings.mLeanSpringIntegrationCoefficient),
+	mLeanSpringIntegrationCoefficientDecay(inSettings.mLeanSpringIntegrationCoefficientDecay),
 	mLeanSmoothingFactor(inSettings.mLeanSmoothingFactor)
 {
 }
@@ -104,11 +112,19 @@ void MotorcycleController::PreCollide(float inDeltaTime, PhysicsSystem &inPhysic
 		// Remove forward component, we can only lean sideways
 		mTargetLean -= mTargetLean * mTargetLean.Dot(forward);
 		mTargetLean = mTargetLean.NormalizedOr(world_up);
+
+		// Integrate the delta angle
+		Vec3 up = body->GetRotation() * mConstraint.GetLocalUp();
+		float d_angle = -Sign(mTargetLean.Cross(up).Dot(forward)) * ACos(mTargetLean.Dot(up));
+		mLeanSpringIntegratedDeltaAngle += d_angle * inDeltaTime;
 	}
 	else
 	{
 		// Controller not enabled, reset target lean
 		mTargetLean = world_up;
+
+		// Reset integrated delta angle
+		mLeanSpringIntegratedDeltaAngle = 0;
 	}
 
 	JPH_DET_LOG("WheeledVehicleController::PreCollide: mTargetLean: " << mTargetLean);
@@ -189,7 +205,7 @@ bool MotorcycleController::SolveLongitudinalAndLateralConstraints(float inDeltaT
 			float ddt_angle = body->GetAngularVelocity().Dot(forward);
 
 			// Calculate impulse to apply to get to target lean angle
-			float total_impulse = (mLeanSpringConstant * d_angle - mLeanSpringDamping * ddt_angle) * inDeltaTime;
+			float total_impulse = (mLeanSpringConstant * d_angle - mLeanSpringDamping * ddt_angle + mLeanSpringIntegrationCoefficient * mLeanSpringIntegratedDeltaAngle) * inDeltaTime;
 
 			// Remember angular velocity pre angular impulse
 			Vec3 old_w = mp->GetAngularVelocity();
@@ -223,6 +239,12 @@ bool MotorcycleController::SolveLongitudinalAndLateralConstraints(float inDeltaT
 			// Return true if we applied an impulse
 			impulse |= delta_impulse != 0.0f;
 		}
+		else
+		{
+			// Decay the integrated angle because we won't be applying a torque this frame
+			// Uses 1st order Taylor approximation of e^(-decay * dt) = 1 - decay * dt
+			mLeanSpringIntegratedDeltaAngle *= max(0.0f, 1.0f - mLeanSpringIntegrationCoefficientDecay * inDeltaTime);
+		}
 	}
 
 	return impulse;

Jolt/Physics/Vehicle/MotorcycleController.h

@@ -29,6 +29,12 @@ public:
 	/// Spring damping constant for the lean spring
 	float						mLeanSpringDamping = 1000.0f;
 
+	/// The lean spring applies an additional force equal to this coefficient * Integral(delta angle, 0, t), this effectively makes the lean spring a PID controller
+	float						mLeanSpringIntegrationCoefficient = 0.0f;
+
+	/// How much to decay the angle integral when the wheels are not touching the floor: new_value = e^(-decay * t) * initial_value
+	float						mLeanSpringIntegrationCoefficientDecay = 4.0f;
+
 	/// How much to smooth the lean angle (0 = no smoothing, 1 = lean angle never changes)
 	/// Note that this is frame rate dependent because the formula is: smoothing_factor * previous + (1 - smoothing_factor) * current
 	float						mLeanSmoothingFactor = 0.8f;
@@ -67,11 +73,16 @@ protected:
 	float						mMaxLeanAngle;
 	float						mLeanSpringConstant;
 	float						mLeanSpringDamping;
+	float						mLeanSpringIntegrationCoefficient;
+	float						mLeanSpringIntegrationCoefficientDecay;
 	float						mLeanSmoothingFactor;
 
 	// Run-time calculated target lean vector
 	Vec3						mTargetLean = Vec3::sZero();
 
+	// Integrated error for the lean spring
+	float						mLeanSpringIntegratedDeltaAngle = 0.0f;	
+
 	// Run-time total angular impulse applied to turn the cycle towards the target lean angle
 	float						mAppliedImpulse = 0.0f;
 };

Samples/Tests/Vehicle/VehicleTest.cpp

@@ -26,6 +26,7 @@ const char *VehicleTest::sScenes[] =
 {
 	"Flat",
 	"Steep Slope",
+	"Step",
 	"Playground",
 	"Terrain1",
 };
@@ -51,6 +52,19 @@ void VehicleTest::Initialize()
 		floor.SetFriction(1.0f);
 		mBodyInterface->AddBody(floor.GetID(), EActivation::DontActivate);
 	}
+	else if (strcmp(sSceneName, "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 5cm step rotated under an angle
+		constexpr float cStepHeight = 0.05f;
+		Body &step = *mBodyInterface->CreateBody(BodyCreationSettings(new BoxShape(Vec3(5.0f, 0.5f * cStepHeight, 5.0f), 0.0f), RVec3(-2.0f, 0.5f * cStepHeight, 60.0f), Quat::sRotation(Vec3::sAxisY(), -0.3f * JPH_PI), EMotionType::Static, Layers::NON_MOVING));
+		step.SetFriction(1.0f);
+		mBodyInterface->AddBody(step.GetID(), EActivation::DontActivate);
+	}
 	else if (strcmp(sSceneName, "Playground") == 0)
 	{
 		// Scene with hilly terrain and some objects to drive into