void AddDriveWheel(Vector3 suspensionOffset, Entity body, bool leftSide, out RevoluteMotor drivingMotor, out RevoluteMotor steeringMotor, out Box suspensionLeg)
{
suspensionLeg = new Box(body.Position + suspensionOffset, 0.25f, 0.8f, 0.25f, 10);
const float horizontalWheelOffset = 0.2f;
var wheel = new Cylinder(suspensionLeg.Position + new Vector3(leftSide ? -horizontalWheelOffset : horizontalWheelOffset, -suspensionLeg.HalfHeight, 0), .2f, .3f, 5f);
wheel.Material.KineticFriction = 2.5f;
wheel.Material.StaticFriction = 3.5f;
wheel.Orientation = Quaternion.CreateFromAxisAngle(Vector3.Forward, MathHelper.PiOver2);
//Preventing the occasional pointless collision pair can speed things up.
CollisionRules.AddRule(wheel, body, CollisionRule.NoBroadPhase);
CollisionRules.AddRule(wheel, suspensionLeg, CollisionRule.NoBroadPhase);
CollisionRules.AddRule(suspensionLeg, body, CollisionRule.NoBroadPhase);
//Connect the suspension to the body.
var bodyToSuspension = new LineSliderJoint(body, suspensionLeg, suspensionLeg.Position, Vector3.Down, suspensionLeg.Position);
bodyToSuspension.Limit.IsActive = true;
bodyToSuspension.Limit.Minimum = -0.5f;
bodyToSuspension.Limit.Maximum = 0;
//This linear axis motor will give the suspension its springiness by pushing the wheels outward.
bodyToSuspension.Motor.IsActive = true;
bodyToSuspension.Motor.Settings.Mode = MotorMode.Servomechanism;
bodyToSuspension.Motor.Settings.Servo.Goal = 0;
bodyToSuspension.Motor.Settings.Servo.SpringSettings.Stiffness = 300;
bodyToSuspension.Motor.Settings.Servo.SpringSettings.Damping = 70;
steeringMotor = new RevoluteMotor(body, suspensionLeg, Vector3.Up);
steeringMotor.Settings.Mode = MotorMode.Servomechanism;
//The constructor makes a guess about how to set up the constraint.
//It can't always be right since it doesn't have all the information;
//in this case, it chooses the basis and test axis incorrectly.
//This leads to a 'flipping' behavior when the wheel is rolling
//(the test axis is 'rolling' with the wheel, and passes over
//a singularity which causes a flip).
//To fix this, we configure the constraint directly.
//The basis is aligned with how the wheel is set up; we choose 'up' as
//the motorized axis, and right/forward to define the angle measurement plane.
//The test axis is set to be perpendicular to the wheel's rotation so that
//it only measures the steering angle.
//If you're curious, the angle measurement is just a Math.Atan2.
//The current world test axis is dotted against the two plane axes (Right and Forward here).
//This gives an x and y value. These can be plugged into Atan2 just like when
//you compute an angle on a normal 2d graph.
steeringMotor.Basis.SetWorldAxes(Vector3.Up, Vector3.Right);
steeringMotor.TestAxis = Vector3.Right;
//To make the steering a little more responsive, set a base speed at which error gets corrected.
//This works on top of the default error reduction implied by the constraint's spring constants.
steeringMotor.Settings.Servo.BaseCorrectiveSpeed = 1;
//The revolute motor is weaker than some other types of constraints and maintaining a goal in the presence of extremely fast rotation and integration issues.
//Laying a revolute limit on top of it can help mitigate the problem.
var steeringConstraint = new RevoluteLimit(body, suspensionLeg, Vector3.Up, Vector3.Right, -maximumTurnAngle, maximumTurnAngle);
//Connect the wheel to the suspension.
var suspensionToWheel = new RevoluteJoint(suspensionLeg, wheel, wheel.Position, Vector3.Right);
drivingMotor = suspensionToWheel.Motor;
//The driving motor's default, created by the RevoluteJoint constructor above, chose the axis of rotation such that negatives values made the car go forward and vice versa.
//Swap it around so that the positive values make the car roll forward instead!
drivingMotor.Basis.SetWorldAxes(Vector3.Left, Vector3.Forward);
drivingMotor.TestAxis = Vector3.Forward;
drivingMotor.Settings.VelocityMotor.Softness = .3f;
drivingMotor.Settings.MaximumForce = 100;
//Add the wheel and connection to the space.
Space.Add(wheel);
Space.Add(suspensionLeg);
Space.Add(bodyToSuspension);
Space.Add(drivingMotor);
Space.Add(steeringMotor);
Space.Add(steeringConstraint);
Space.Add(suspensionToWheel);
}
