// Computes an impulse that would stop motion in the given direction. private float ComputeStopImpulse(Wheel wheel, Vector3F direction) { // A = chassis, B = ground var bodyA = Vehicle.Chassis; var bodyB = wheel.TouchedBody; // This method computes a constraint impulse that makes the relative velocity of the touching // points 0 in the given direction. - A 1D no-movement constraint. // If you want to learn more about this, there are a few literature references in the // DigitalRune Physics Documentation (section "Best Practices and Recommended Literature"). // Radius vectors. var rA = wheel.GroundPosition - bodyA.PoseCenterOfMass.Position; var rB = wheel.GroundPosition - bodyB.PoseCenterOfMass.Position; // Jacobians. var jLinA = -direction; var jAngA = -Vector3F.Cross(rA, direction); var jLinB = direction; var jAngB = Vector3F.Cross(rB, direction); // M^-1 * J^T var WJTLinA = bodyA.MassInverse * jLinA; var WJTAngA = bodyA.InertiaInverseWorld * jAngA; var WJTLinB = bodyB.MassInverse * jLinB; var WJTAngB = bodyB.InertiaInverseWorld * jAngB; // J * M^-1 * J^T float JWJT = Vector3F.Dot(jLinA, WJTLinA) + Vector3F.Dot(jAngA, WJTAngA) + Vector3F.Dot(jLinB, WJTLinB) + Vector3F.Dot(jAngB, WJTAngB); var JWJTInverse = 1 / JWJT; // Relative velocity = J * v var relativeVelocity = Vector3F.Dot(jLinA, bodyA.LinearVelocity) + Vector3F.Dot(jAngA, bodyA.AngularVelocity) + Vector3F.Dot(jLinB, bodyB.LinearVelocity) + Vector3F.Dot(jAngB, bodyB.AngularVelocity); // The impulse (lambda) is (J * M^-1 * J^T)^-1 * (newRelativeVelocity - oldRelativeVelocity). return JWJTInverse * relativeVelocity; }
/// <summary> /// Sets the steering angles for a standard 4 wheel car. /// </summary> /// <param name="steeringAngle">The steering angle.</param> /// <param name="frontLeft">The front left wheel.</param> /// <param name="frontRight">The front right wheel.</param> /// <param name="backLeft">The back left wheel.</param> /// <param name="backRight">The back right wheel.</param> /// <remarks> /// In a real car, the steerable front wheels do not always have the same steering angle. Have a /// look at http://www.asawicki.info/Mirror/Car%20Physics%20for%20Games/Car%20Physics%20for%20Games.html /// (section "Curves") for an explanation. The steering angle defines the angle of the inner /// wheel. The outer wheel is adapted. This works only for 4 wheels in a normal car setup. /// </remarks> /// <exception cref="ArgumentNullException"> /// <paramref name="frontLeft"/>, <paramref name="frontRight"/>, <paramref name="backLeft"/>, or /// <paramref name="backRight"/> is <see langword="null"/>. /// </exception> public static void SetCarSteeringAngle(float steeringAngle, Wheel frontLeft, Wheel frontRight, Wheel backLeft, Wheel backRight) { if (frontLeft == null) throw new ArgumentNullException("frontLeft"); if (frontRight == null) throw new ArgumentNullException("frontRight"); if (backLeft == null) throw new ArgumentNullException("backLeft"); if (backRight == null) throw new ArgumentNullException("backRight"); backLeft.SteeringAngle = 0; backRight.SteeringAngle = 0; if (Numeric.IsZero(steeringAngle)) { frontLeft.SteeringAngle = 0; frontRight.SteeringAngle = 0; return; } Wheel inner, outer; if (steeringAngle > 0) { inner = frontLeft; outer = frontRight; } else { inner = frontRight; outer = frontLeft; } inner.SteeringAngle = steeringAngle; float backToFront = backLeft.Offset.Z - frontLeft.Offset.Z; float rightToLeft = frontRight.Offset.X - frontLeft.Offset.X; float innerAngle = Math.Abs(steeringAngle); float outerAngle = (float)Math.Atan2(backToFront, backToFront / Math.Tan(innerAngle) + rightToLeft); outer.SteeringAngle = Math.Sign(steeringAngle) * outerAngle; }