public void WheelBaseTurn()
{
var transformer = new Program.CoordinatesTransformer(this.controller);
MockMotorSuspension frontLeftWheel = GetSuspension(new Vector3D(-1.5, 0, -2), true);
var frontLeft = new Program.PowerWheel(frontLeftWheel, this.wheelBase, transformer);
MockMotorSuspension frontRightWheel = GetSuspension(new Vector3D(1.5, 0, -2), false);
var frontRight = new Program.PowerWheel(frontRightWheel, this.wheelBase, transformer);
MockMotorSuspension middleLeftWheel = GetSuspension(new Vector3D(-1.5, 0, 0), true);
var middleLeft = new Program.PowerWheel(middleLeftWheel, this.wheelBase, transformer);
MockMotorSuspension middleRightWheel = GetSuspension(new Vector3D(1.5, 0, 0), false);
var middleRight = new Program.PowerWheel(middleRightWheel, this.wheelBase, transformer);
MockMotorSuspension rearLeftWheel = GetSuspension(new Vector3D(-1.5, 0, 2), true);
var rearLeft = new Program.PowerWheel(rearLeftWheel, this.wheelBase, transformer);
MockMotorSuspension rearRightWheel = GetSuspension(new Vector3D(1.5, 0, 2), false);
var rearRight = new Program.PowerWheel(rearRightWheel, this.wheelBase, transformer);
Assert.AreEqual(6, this.wheelBase.TurnRadius);
Assert.AreEqual(new Vector3D(-6, 0, 0), this.wheelBase.LeftCenterOfTurn);
Assert.AreEqual(new Vector3D(6, 0, 0), this.wheelBase.RightCenterOfTurn);
this.wheelBase.TurnRadiusOverride = 4;
Assert.AreEqual(4, this.wheelBase.TurnRadius);
Assert.AreEqual(new Vector3D(-4, 0, 0), this.wheelBase.LeftCenterOfTurn);
Assert.AreEqual(new Vector3D(4, 0, 0), this.wheelBase.RightCenterOfTurn);
Asserts.AreClose(18.43f, 0.01f, this.wheelBase.GetAngle(frontLeft, false));
Assert.AreEqual(0, this.wheelBase.GetAngle(middleLeft, false));
Asserts.AreClose(18.43f, 0.01f, this.wheelBase.GetAngle(rearLeft, false));
Assert.AreEqual(45, this.wheelBase.GetAngle(frontRight, false));
Assert.AreEqual(0, this.wheelBase.GetAngle(middleRight, false));
Assert.AreEqual(45, this.wheelBase.GetAngle(rearRight, false));
Assert.AreEqual(45, this.wheelBase.GetAngle(frontLeft, true));
Assert.AreEqual(0, this.wheelBase.GetAngle(middleLeft, true));
Assert.AreEqual(45, this.wheelBase.GetAngle(rearLeft, true));
Asserts.AreClose(18.43f, 0.01f, this.wheelBase.GetAngle(frontRight, true));
Assert.AreEqual(0, this.wheelBase.GetAngle(middleRight, true));
Asserts.AreClose(18.43f, 0.01f, this.wheelBase.GetAngle(rearRight, true));
}
static void assertAnglesClose(float expected, float actual) => Asserts.AreClose(expected, 0.0001f, MathHelper.ToDegrees(actual));