public WheelConstraint(ConstraintWheel wheel)
{
_wheel = wheel;
_suspensionSpringConstraint = new Constraint1D();
_suspensionLimitConstraint = new Constraint1D();
_sideConstraint = new Constraint1D();
_forwardConstraint = new Constraint1D();
}
/// <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, ConstraintWheel frontLeft, ConstraintWheel frontRight, ConstraintWheel backLeft, ConstraintWheel 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;
}
ConstraintWheel 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;
}
private static void UpdateWheelContactInfo(ConstraintWheel wheel, ContactSet contactSet)
{
if (contactSet != null && contactSet.HaveContact && contactSet.Count > 0)
{
// ----- Ray has contact.
var contact = contactSet[0];
wheel.HasGroundContact = true;
wheel.GroundPosition = contact.Position;
if (wheel.CollisionObject == contactSet.ObjectA)
{
wheel.GroundNormal = -contact.Normal;
wheel.TouchedBody = contactSet.ObjectB.GeometricObject as RigidBody;
}
else
{
wheel.GroundNormal = contact.Normal;
wheel.TouchedBody = contactSet.ObjectA.GeometricObject as RigidBody;
}
// If the ray is nearly parallel to the ground, then the contact is not
// useful and we ignore it.
Vector3 up = wheel.Vehicle.Chassis.Pose.Orientation.GetColumn(1);
float normalDotUp = Vector3.Dot(wheel.GroundNormal, up);
if (Numeric.IsGreater(normalDotUp, 0))
{
wheel.Tag = 1; // Tag = 1 means that this wheel has a useful ground contact.
float hitDistance = contact.PenetrationDepth;
wheel.SuspensionLength = Math.Max(hitDistance - wheel.Radius, wheel.MinSuspensionLength);
wheel.Constraint.BodyB = wheel.TouchedBody ?? wheel.Vehicle.Simulation.World;
wheel.Constraint.Enabled = true;
wheel.GroundRight = wheel.Vehicle.Chassis.Pose.ToWorldDirection(Matrix.CreateRotationY(wheel.SteeringAngle) * Vector3.UnitX);
wheel.GroundForward = Vector3.Cross(wheel.GroundNormal, wheel.GroundRight).Normalized;
}
}
else
{
// ----- The wheel is in the air.
wheel.Constraint.Enabled = false;
wheel.Constraint.BodyB = wheel.Vehicle.Simulation.World;
wheel.HasGroundContact = false;
wheel.TouchedBody = null;
wheel.SuspensionLength = wheel.SuspensionRestLength;
}
}
private static void UpdateWheelVelocity(float deltaTime, ConstraintVehicle vehicle, ConstraintWheel wheel)
{
if (wheel.Tag == 1)
{
wheel.Tag = 0;
if (Numeric.IsZero(wheel.BrakeForce))
{
// We set the angular velocity, so that the wheel matches the moving underground.
Vector3 relativeContactVelocity = vehicle.Chassis.GetVelocityOfWorldPoint(wheel.GroundPosition)
- wheel.TouchedBody.GetVelocityOfWorldPoint(wheel.GroundPosition);
float forwardVelocity = Vector3.Dot(relativeContactVelocity, wheel.GroundForward);
wheel.AngularVelocity = forwardVelocity / wheel.Radius;
wheel.RotationAngle += wheel.AngularVelocity * deltaTime;
}
else
{
// Braking wheels do not rotate.
wheel.AngularVelocity = 0;
}
}
else
{
// To keep it simple: The wheel continues spinning in the same direction.
// Damp angular velocity and update rotation angle.
wheel.AngularVelocity *= 0.99f;
wheel.RotationAngle += wheel.AngularVelocity * deltaTime;
}
}
