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