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; }