Fixed bug that caused suspension to be weaker when driving over dynamic bodies (#565)

Suspension stiffness and damping are now calculated based on the vehicle configuration instead of varying at runtime and becoming lower when driving over light objects.

Jolt/Physics/Vehicle/VehicleConstraint.cpp

@@ -371,69 +371,63 @@ void VehicleConstraint::SetupVelocityConstraint(float inDeltaTime)
 			// Suspension spring
 			if (settings->mSuspensionMaxLength > settings->mSuspensionMinLength)
 			{
-				// Calculate cos(alpha) where alpha is the angle between suspension direction and contact normal
-				// Note that we clamp 1 / cos(alpha) to the range [0.1, 1] in order not to increase the stiffness / damping by too much.
-				Vec3 ws_direction = body_transform.Multiply3x3(settings->mSuspensionDirection);
-				float cos_angle = max(0.1f, ws_direction.Dot(neg_contact_normal));
-
-				SpringSettings spring_settings = settings->mSuspensionSpring;
-				if (spring_settings.mMode == ESpringMode::FrequencyAndDamping)
+				float stiffness, damping;
+				if (settings->mSuspensionSpring.mMode == ESpringMode::FrequencyAndDamping)
 				{
-					// Calculate the damping and frequency of the suspension spring given the angle between the suspension direction and the contact normal
-					// If the angle between the suspension direction and the inverse of the contact normal is alpha then the force on the spring relates to the force along the contact normal as:
-					// 
-					// Fspring = Fnormal * cos(alpha)
-					//
-					// The spring force is:
-					//
-					// Fspring = -k * x
-					//
-					// where k is the spring constant and x is the displacement of the spring. So we have:
-					//
-					// Fnormal * cos(alpha) = -k * x <=> Fnormal = -k / cos(alpha) * x
-					//
-					// So we can see this as a spring with spring constant:
-					//
-					// k' = k / cos(alpha)
-					// 
-					// In the same way the velocity relates like:
-					// 
-					// Vspring = Vnormal * cos(alpha)
-					//
-					// Which results in the modified damping constant c:
-					//
-					// c' = c / cos(alpha)
-					//
-					// Since we're not supplying k and c directly but rather the frequency and damping we can calculate the spring constant and damping constant as:
-					//
-					// w = 2 * pi * f
-					// k = m * w^2
-					// c = 2 * m * w * d
-					//
-					// where m is the mass of the spring, f is the frequency and d is the damping factor (see SpringPart::CalculateSpringProperties). So we have:
-					//
-					// w' = w * pi * f'
-					// k' = m * w'^2
-					// c' = 2 * m * w' * d'
-					//
-					// where f' = f / sqrt(cos(alpha)) and d' = d / sqrt(cos(alpha))
-					//
-					// We ensure that the frequency doesn't go over half the simulation frequency to prevent the spring from getting unstable.
-					float sqrt_cos_angle = sqrt(cos_angle);
-					spring_settings.mDamping /= sqrt_cos_angle;
-					spring_settings.mFrequency = min(0.5f / inDeltaTime, spring_settings.mFrequency / sqrt_cos_angle);
+					// Calculate effective mass based on vehicle configuration (the stiffness of the spring should not be affected by the dynamics of the vehicle): K = 1 / (J M^-1 J^T)
+					// Note that if no suspension force point is supplied we don't know where the force is applied so we assume it is applied at average suspension length
+					Vec3 force_point = settings->mEnableSuspensionForcePoint? settings->mSuspensionForcePoint : settings->mPosition + 0.5f * (settings->mSuspensionMinLength + settings->mSuspensionMaxLength) * settings->mSuspensionDirection;
+					Vec3 force_point_x_neg_up = force_point.Cross(-mUp);
+					const MotionProperties *mp = mBody->GetMotionProperties();
+					float effective_mass = 1.0f / (mp->GetInverseMass() + force_point_x_neg_up.Dot(mp->GetLocalSpaceInverseInertia().Multiply3x3(force_point_x_neg_up)));
+
+					// Convert frequency and damping to stiffness and damping
+					float omega = 2.0f * JPH_PI * settings->mSuspensionSpring.mFrequency;
+					stiffness = effective_mass * Square(omega);
+					damping = 2.0f * effective_mass * settings->mSuspensionSpring.mDamping * omega;
 				}
 				else
 				{
-					// This case is similar to the one above but we're not supplying frequency and damping but rather the spring constant and damping constant directly.
-					spring_settings.mStiffness /= cos_angle;
-					spring_settings.mDamping /= cos_angle;
+					// In this case we can simply copy the properties
+					stiffness = settings->mSuspensionSpring.mStiffness;
+					damping = settings->mSuspensionSpring.mDamping;
 				}
 
+				// Calculate the damping and frequency of the suspension spring given the angle between the suspension direction and the contact normal
+				// If the angle between the suspension direction and the inverse of the contact normal is alpha then the force on the spring relates to the force along the contact normal as:
+				// 
+				// Fspring = Fnormal * cos(alpha)
+				//
+				// The spring force is:
+				//
+				// Fspring = -k * x
+				//
+				// where k is the spring constant and x is the displacement of the spring. So we have:
+				//
+				// Fnormal * cos(alpha) = -k * x <=> Fnormal = -k / cos(alpha) * x
+				//
+				// So we can see this as a spring with spring constant:
+				//
+				// k' = k / cos(alpha)
+				// 
+				// In the same way the velocity relates like:
+				// 
+				// Vspring = Vnormal * cos(alpha)
+				//
+				// Which results in the modified damping constant c:
+				//
+				// c' = c / cos(alpha)
+				// 
+				// Note that we clamp 1 / cos(alpha) to the range [0.1, 1] in order not to increase the stiffness / damping by too much.
+				Vec3 ws_direction = body_transform.Multiply3x3(settings->mSuspensionDirection);
+				float cos_angle = max(0.1f, ws_direction.Dot(neg_contact_normal));
+				stiffness /= cos_angle;
+				damping /= cos_angle;
+
 				// Get the value of the constraint equation
 				float c = w->mSuspensionLength - settings->mSuspensionMaxLength - settings->mSuspensionPreloadLength;
 
-				w->mSuspensionPart.CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody, r1_plus_u, *w->mContactBody, r2, neg_contact_normal, w->mAntiRollBarImpulse, c, spring_settings);
+				w->mSuspensionPart.CalculateConstraintPropertiesWithStiffnessAndDamping(inDeltaTime, *mBody, r1_plus_u, *w->mContactBody, r2, neg_contact_normal, w->mAntiRollBarImpulse, c, stiffness, damping);
 			}
 			else
 				w->mSuspensionPart.Deactivate();