public static Pose TurnLeft(Pose startPose, Gear gear, float turnAngle, float unit)
{
if (gear == Gear.Backward)
{
turnAngle = -turnAngle;
}
float phi = turnAngle / 2;
float sinPhi = (float)Math.Sin(phi);
float L = 2 * sinPhi * unit;
float x = L * (float)Math.Cos(phi);
float y = L * sinPhi;
Vector2 pos = new Vector2(x, y);
pos = Vector2.Transform(pos, Matrix.CreateRotationZ(startPose.Orientation));
if (Debug != null)
{
float rotatedTheta = startPose.Orientation + (float)Math.PI / 2;
float xCenter = startPose.X + unit * (float)Math.Cos(rotatedTheta);
float yCenter = startPose.Y + unit * (float)Math.Sin(rotatedTheta);
Debug.DrawArc(new Vector2(xCenter, yCenter), unit, startPose.Orientation - (float)Math.PI / 2, startPose.Orientation + turnAngle - (float)Math.PI / 2, gear == Gear.Forward ? forwardColor : backwardColor);
}
return(new Pose(pos + startPose.Position, startPose.Orientation + turnAngle));
}