public void SpringDampingTest()
{
float erp = 0.3f;
float cfm = 0.001f;
float spring = ConstraintHelper.ComputeSpringConstant(1 / 60f, erp, cfm);
float damping = ConstraintHelper.ComputeDampingConstant(1 / 60f, erp, cfm);
Assert.IsTrue(Numeric.AreEqual(erp, ConstraintHelper.ComputeErrorReduction(1 / 60f, spring, damping)));
Assert.IsTrue(Numeric.AreEqual(cfm, ConstraintHelper.ComputeSoftness(1 / 60f, spring, damping)));
}
/// <summary>
/// Called when constraint should be set up for a new time step.
/// </summary>
/// <remarks>
/// This method is called by <see cref="Constraint.Setup"/>, but only if the constraint is
/// enabled and all <see cref="Constraint"/> properties are properly initialized.
/// </remarks>
protected override void OnSetup()
{
Debug.Assert(_wheel.HasGroundContact, "WheelConstraint must be disabled if wheel does not touch ground.");
// The pose where the suspension is fixed on the car.
Pose anchorPoseA = BodyA.Pose * new Pose(_wheel.Offset);
// The ground contact position relative to the anchor pose.
Vector3 relativePosition = anchorPoseA.ToLocalPosition(_wheel.GroundPosition);
// Radius vectors from the centers of mass to the points where the
// suspension forces should be applied:
Vector3 rA = anchorPoseA.Position - BodyA.PoseCenterOfMass.Position;
Vector3 rB = _wheel.GroundPosition - BodyB.PoseCenterOfMass.Position;
// Get some simulation settings.
float deltaTime = Simulation.Settings.Timing.FixedTimeStep;
float allowedDeviation = Simulation.Settings.Constraints.AllowedPenetration;
float maxErrorCorrectionVelocity = Simulation.Settings.Constraints.MaxErrorCorrectionVelocity;
// The position of ground contact relative to the suspension anchor pose.
float relativePositionY = relativePosition.Y;
// The suspension axis (= the Y axis of the chassis body).
var suspensionAxis = BodyA.Pose.Orientation.GetColumn(1);
{
// ----- Set up the suspension spring constraint:
float deviation = -_wheel.SuspensionRestLength - _wheel.Radius - relativePositionY + allowedDeviation;
// Compute error reduction and softness from spring and damping values.
// Note: The wheel suspension stiffness and damping are scaled by the mass because they
// should be scaled with different chassis masses.
var mass = BodyA.MassFrame.Mass;
var suspensionSpring = _wheel.SuspensionStiffness * mass;
var suspensionDamping = _wheel.SuspensionDamping * mass;
float errorReduction = ConstraintHelper.ComputeErrorReduction(deltaTime, suspensionSpring, suspensionDamping);
float softness = ConstraintHelper.ComputeSoftness(deltaTime, suspensionSpring, suspensionDamping);
_suspensionSpringConstraint.TargetRelativeVelocity = MathHelper.Clamp(deviation * errorReduction / deltaTime, -maxErrorCorrectionVelocity, maxErrorCorrectionVelocity);
_suspensionSpringConstraint.Softness = softness / deltaTime;
_suspensionSpringConstraint.Prepare(BodyA, BodyB, -suspensionAxis, -Vector3.Cross(rA, suspensionAxis), suspensionAxis, Vector3.Cross(rB, suspensionAxis));
}
// ----- Set up the hard suspension limit:
_suspensionLimitIsActive = relativePositionY > -_wheel.MinSuspensionLength - _wheel.Radius;
if (_suspensionLimitIsActive)
{
float deviation = -_wheel.MinSuspensionLength - _wheel.Radius - relativePositionY + allowedDeviation;
// This constraint is as "hard" as a normal contact constraint.
float errorReduction = Simulation.Settings.Constraints.ContactErrorReduction;
_suspensionLimitConstraint.TargetRelativeVelocity = MathHelper.Clamp(deviation * errorReduction / deltaTime, -maxErrorCorrectionVelocity, maxErrorCorrectionVelocity);
_suspensionLimitConstraint.Prepare(BodyA, BodyB, -suspensionAxis, -Vector3.Cross(rA, suspensionAxis), suspensionAxis, Vector3.Cross(rB, suspensionAxis));
}
else
{
_suspensionLimitConstraint.ConstraintImpulse = 0;
}
// The forward and side constraints are applied in the ground plane. But to make the
// car more stable we can optionally apply the impulses at a higher position (to avoid rolling).
rA = rA - (1 - _wheel.RollReduction) * (_wheel.SuspensionLength + _wheel.Radius) * suspensionAxis;
// ---- Set up lateral friction constraint:
_sideConstraint.TargetRelativeVelocity = 0;
_sideConstraint.Prepare(BodyA, BodyB, -_wheel.GroundRight, -Vector3.Cross(rA, _wheel.GroundRight), _wheel.GroundRight, Vector3.Cross(rB, _wheel.GroundRight));
// ----- Set up forward constraint (motor, brake, friction):
_forwardConstraint.Prepare(BodyA, BodyB, -_wheel.GroundForward, -Vector3.Cross(rA, _wheel.GroundForward), _wheel.GroundForward, Vector3.Cross(rB, _wheel.GroundForward));
if (Math.Abs(_wheel.MotorForce) > Math.Abs(_wheel.BrakeForce))
{
// The wheel is driven by the motor.
// The constraint tries to accelerate the car as much as possible. The actual acceleration
// is limited by the motor and brake force.
_forwardConstraint.TargetRelativeVelocity = -Math.Sign(_wheel.MotorForce) * float.PositiveInfinity;
_forwardImpulseLimit = (Math.Abs(_wheel.MotorForce) - Math.Abs(_wheel.BrakeForce)) * deltaTime;
}
else
{
// The wheel is freely rolling or braking.
_forwardConstraint.TargetRelativeVelocity = 0;
_forwardImpulseLimit = Math.Max(_wheel.RollingFrictionForce, _wheel.BrakeForce - Math.Abs(_wheel.MotorForce)) * deltaTime;
}
// Warmstart the suspension constraints.
_suspensionSpringConstraint.ApplyImpulse(BodyA, BodyB, _suspensionSpringConstraint.ConstraintImpulse);
_suspensionLimitConstraint.ApplyImpulse(BodyA, BodyB, _suspensionLimitConstraint.ConstraintImpulse);
// Do not warmstart the tangential friction-based constraints.
_forwardConstraint.ConstraintImpulse = 0;
_sideConstraint.ConstraintImpulse = 0;
}