Bugfix: Pitch/roll limit constraint didn't work when local vehicle up did not match world up (#510)

* Now calculating world up vector from gravity every frame and using that to calculate the pitch/roll angle. 
* Fixed a division by zero if the caster angle is 90 degrees
* Fixed the pitch/roll constraint turning off if the max angle is 180 degrees

Jolt/Physics/Vehicle/MotorcycleController.cpp

@@ -82,10 +82,6 @@ void MotorcycleController::PreCollide(float inDeltaTime, PhysicsSystem &inPhysic
 {
 	WheeledVehicleController::PreCollide(inDeltaTime, inPhysicsSystem);
 
-	Vec3 gravity = inPhysicsSystem.GetGravity();
-	float gravity_len = gravity.Length();
-	Vec3 world_up = gravity_len > 0.0f? -gravity / gravity_len : mConstraint.GetLocalUp();
-
 	const Body *body = mConstraint.GetVehicleBody();
 	Vec3 forward = body->GetRotation() * mConstraint.GetLocalForward();
 	float wheel_base = GetWheelBase();
@@ -99,6 +95,7 @@ void MotorcycleController::PreCollide(float inDeltaTime, PhysicsSystem &inPhysic
 			target_lean += w->GetContactNormal() * w->GetSuspensionLambda() + w->GetContactLateral() * w->GetLateralLambda();
 
 	// Normalize the impulse
+	Vec3 world_up = mConstraint.GetWorldUp();
 	target_lean = target_lean.NormalizedOr(world_up);
 
 	// Smooth the impulse to avoid jittery behavior
@@ -118,7 +115,7 @@ void MotorcycleController::PreCollide(float inDeltaTime, PhysicsSystem &inPhysic
 	// TurnRadius = WheelBase / (Sin(SteerAngle) * Cos(CasterAngle))
 	// => SteerAngle = ASin(WheelBase * Tan(LeanAngle) * Gravity / (Velocity^2 * Cos(CasterAngle))
 	// The caster angle is different for each wheel so we can only calculate part of the equation here
-	float max_steer_angle_factor = wheel_base * Tan(mMaxLeanAngle) * gravity_len;
+	float max_steer_angle_factor = wheel_base * Tan(mMaxLeanAngle) * inPhysicsSystem.GetGravity().Length();
 
 	// Decompose steering into sign and direction
 	float steer_strength = abs(mRightInput);
@@ -139,7 +136,7 @@ void MotorcycleController::PreCollide(float inDeltaTime, PhysicsSystem &inPhysic
 			float steer_angle = steer_strength * w->GetSettings()->mMaxSteerAngle;
 
 			// Clamp to max steering angle
-			if (velocity_sq > 1.0e-6f)
+			if (velocity_sq > 1.0e-6f && cos_caster_angle > 1.0e-6f)
 			{
 				float max_steer_angle = ASin(max_steer_angle_factor / (velocity_sq * cos_caster_angle));
 				steer_angle = min(steer_angle, max_steer_angle);

Jolt/Physics/Vehicle/VehicleConstraint.cpp

@@ -76,17 +76,18 @@ void VehicleConstraintSettings::RestoreBinaryState(StreamIn &inStream)
 }
 
 VehicleConstraint::VehicleConstraint(Body &inVehicleBody, const VehicleConstraintSettings &inSettings) :
-	Constraint(inSettings)
+	Constraint(inSettings),
+	mBody(&inVehicleBody),
+	mForward(inSettings.mForward),
+	mUp(inSettings.mUp),
+	mWorldUp(inSettings.mUp)
 {
 	// Check sanity of incoming settings
-	JPH_ASSERT(inSettings.mForward.IsNormalized());
 	JPH_ASSERT(inSettings.mUp.IsNormalized());
+	JPH_ASSERT(inSettings.mForward.IsNormalized());
 	JPH_ASSERT(!inSettings.mWheels.empty());
 
-	// Store general properties
-	mBody = &inVehicleBody;
-	mUp = inSettings.mUp;
-	mForward = inSettings.mForward;
+	// Store max pitch/roll angle
 	SetMaxPitchRollAngle(inSettings.mMaxPitchRollAngle);
 
 	// Copy anti-rollbar settings
@@ -157,6 +158,9 @@ void VehicleConstraint::OnStep(float inDeltaTime, PhysicsSystem &inPhysicsSystem
 {
 	JPH_PROFILE_FUNCTION();
 
+	// Calculate new world up vector by inverting gravity
+	mWorldUp = (-inPhysicsSystem.GetGravity()).NormalizedOr(mWorldUp);
+
 	// Callback on our controller
 	mController->PreCollide(inDeltaTime, inPhysicsSystem);
 
@@ -320,15 +324,15 @@ void VehicleConstraint::CalculateWheelContactPoint(const Wheel &inWheel, Vec3 &o
 void VehicleConstraint::CalculatePitchRollConstraintProperties(float inDeltaTime, RMat44Arg inBodyTransform)
 {
 	// Check if a limit was specified
-	if (mCosMaxPitchRollAngle < JPH_PI)
+	if (mCosMaxPitchRollAngle > -1.0f)
 	{
 		// Calculate cos of angle between world up vector and vehicle up vector
 		Vec3 vehicle_up = inBodyTransform.Multiply3x3(mUp);
-		mCosPitchRollAngle = mUp.Dot(vehicle_up);
+		mCosPitchRollAngle = mWorldUp.Dot(vehicle_up);
 		if (mCosPitchRollAngle < mCosMaxPitchRollAngle)
 		{
 			// Calculate rotation axis to rotate vehicle towards up
-			Vec3 rotation_axis = mUp.Cross(vehicle_up);
+			Vec3 rotation_axis = mWorldUp.Cross(vehicle_up);
 			float len = rotation_axis.Length();
 			if (len > 0.0f)
 				mPitchRollRotationAxis = rotation_axis / len;

Jolt/Physics/Vehicle/VehicleConstraint.h

@@ -85,6 +85,9 @@ public:
 	/// Get the local space up vector of the vehicle
 	Vec3						GetLocalUp() const							{ return mUp; }
 
+	/// Vector indicating the world space up direction (used to limit vehicle pitch/roll), calculated every frame by inverting gravity
+	Vec3						GetWorldUp() const							{ return mWorldUp; }
+
 	/// Access to the vehicle body
 	Body *						GetVehicleBody() const						{ return mBody; }
 
@@ -154,6 +157,7 @@ private:
 	Body *						mBody;										///< Body of the vehicle
 	Vec3						mForward;									///< Local space forward vector for the vehicle
 	Vec3						mUp;										///< Local space up vector for the vehicle
+	Vec3						mWorldUp;									///< Vector indicating the world space up direction (used to limit vehicle pitch/roll)
 	Wheels						mWheels;									///< Wheel states of the vehicle
 	Array<VehicleAntiRollBar>	mAntiRollBars;								///< Anti rollbars of the vehicle
 	VehicleController *			mController;								///< Controls the acceleration / declerration of the vehicle