public static void Drive(Pose start, ReedsSheppActionSet actions, float unit)
{
Pose current = new Pose(start);
foreach (ReedsSheppAction action in actions.Actions)
{
switch (action.Steer)
{
case Steer.Straight:
current = Straight(current, action.Gear, action.Length, unit);
break;
case Steer.Left:
current = TurnLeft(current, action.Gear, action.Length, unit);
break;
case Steer.Right:
current = TurnRight(current, action.Gear, action.Length, unit);
break;
}
if (Debug != null)
{
current.DrawPose(Debug, 2, Color.LightBlue);
}
}
}
private static ReedsSheppActionSet timeflipTransform(ReedsSheppActionSet actions)
{
foreach (ReedsSheppAction a in actions.Actions)
{
a.Gear = a.Gear == Gear.Backward ? Gear.Forward : Gear.Backward;
}
return(actions);
}
private static ReedsSheppActionSet LfRfLbpath(float t, float u, float v)
{
ReedsSheppActionSet actions = new ReedsSheppActionSet();
actions.AddAction(Steer.Left, Gear.Forward, t);
actions.AddAction(Steer.Right, Gear.Forward, u);
actions.AddAction(Steer.Left, Gear.Backward, v);
return(actions);
}
private static ReedsSheppActionSet LfSfLfpi2Rbpath(float t, float u, float v)
{
ReedsSheppActionSet actions = new ReedsSheppActionSet();
actions.AddAction(Steer.Left, Gear.Forward, t);
actions.AddAction(Steer.Straight, Gear.Forward, u);
actions.AddAction(Steer.Left, Gear.Forward, MathHelper.PiOver2);
actions.AddAction(Steer.Right, Gear.Backward, v);
return(actions);
}
private static ReedsSheppActionSet reflectTransform(ReedsSheppActionSet actions)
{
foreach (ReedsSheppAction a in actions.Actions)
{
if (a.Steer == Steer.Left)
{
a.Steer = Steer.Right;
}
else if (a.Steer == Steer.Right)
{
a.Steer = Steer.Left;
}
}
return(actions);
}
public static ArrayList <Pose> Discretize(Pose start, ReedsSheppActionSet actions, float unit, float maxLength)
{
Pose prev = new Pose(start);
ArrayList <Pose> poses = new ArrayList <Pose>();
poses.Add(prev);
foreach (ReedsSheppAction action in actions.Actions)
{
int n = (int)Math.Ceiling(action.Length * unit / maxLength);
if (action.Steer != Steer.Straight)
{
float pieceAngle = action.Length / n;
float phi = pieceAngle / 2;
float sinPhi = (float)Math.Sin(phi);
float L = 2 * sinPhi * unit;
float dx = L * (float)Math.Cos(phi);
float dy = L * sinPhi;
if (action.Steer == Steer.Right)
{
dy = -dy;
pieceAngle = -pieceAngle;
}
if (action.Gear == Gear.Backward)
{
dx = -dx;
pieceAngle = -pieceAngle;
}
for (int i = 0; i < n; i++)
{
Vector2 pos = new Vector2(dx, dy);
pos = Vector2.Transform(pos, Matrix.CreateRotationZ(prev.Orientation));
prev = new Pose(pos + prev.Position, prev.Orientation + pieceAngle, action.Gear);
poses.Add(prev);
}
}
else
{
float pieceLength = action.Length * unit / n;
float dx = pieceLength * (float)Math.Cos(prev.Orientation);
float dy = pieceLength * (float)Math.Sin(prev.Orientation);
if (action.Gear == Gear.Backward)
{
dx = -dx;
dy = -dy;
}
for (int i = 0; i < n; i++)
{
prev = new Pose(dx + prev.X, dy + prev.Y, prev.Orientation, action.Gear);
poses.Add(prev);
}
}
}
return(poses);
}
private static ReedsSheppActionSet timeflipTransform(ReedsSheppActionSet actions)
{
foreach (ReedsSheppAction a in actions.Actions)
a.Gear = a.Gear == Gear.Backward ? Gear.Forward : Gear.Backward;
return actions;
}
private static ReedsSheppActionSet reflectTransform(ReedsSheppActionSet actions)
{
foreach (ReedsSheppAction a in actions.Actions)
if (a.Steer == Steer.Left)
a.Steer = Steer.Right;
else if (a.Steer == Steer.Right)
a.Steer = Steer.Left;
return actions;
}
private static ReedsSheppActionSet LfSfRfpi2Lbpath(float t, float u, float v)
{
ReedsSheppActionSet actions = new ReedsSheppActionSet();
actions.AddAction(Steer.Left, Gear.Forward, t);
actions.AddAction(Steer.Straight, Gear.Forward, u);
actions.AddAction(Steer.Right, Gear.Forward, MathHelper.PiOver2);
actions.AddAction(Steer.Left, Gear.Backward, v);
return actions;
}
private static ReedsSheppActionSet LfSfLfpath(float t, float u, float v)
{
ReedsSheppActionSet actions = new ReedsSheppActionSet();
actions.AddAction(Steer.Left, Gear.Forward, t);
actions.AddAction(Steer.Straight, Gear.Forward, u);
actions.AddAction(Steer.Left, Gear.Forward, v);
return actions;
}
private static ReedsSheppActionSet LfRufLubRbpath(float t, float u, float v)
{
ReedsSheppActionSet actions = new ReedsSheppActionSet();
actions.AddAction(Steer.Left, Gear.Forward, t);
actions.AddAction(Steer.Right, Gear.Forward, u);
actions.AddAction(Steer.Left, Gear.Backward, u);
actions.AddAction(Steer.Right, Gear.Backward, v);
return actions;
}
public static ArrayList<Pose> Discretize(Pose start, ReedsSheppActionSet actions, float unit, float maxLength)
{
Pose prev = new Pose(start);
ArrayList<Pose> poses = new ArrayList<Pose>();
poses.Add(prev);
foreach (ReedsSheppAction action in actions.Actions)
{
int n = (int)Math.Ceiling(action.Length * unit / maxLength);
if (action.Steer != Steer.Straight)
{
float pieceAngle = action.Length / n;
float phi = pieceAngle / 2;
float sinPhi = (float)Math.Sin(phi);
float L = 2 * sinPhi * unit;
float dx = L * (float)Math.Cos(phi);
float dy = L * sinPhi;
if (action.Steer == Steer.Right)
{
dy = -dy;
pieceAngle = -pieceAngle;
}
if (action.Gear == Gear.Backward)
{
dx = -dx;
pieceAngle = -pieceAngle;
}
for (int i = 0; i < n; i++)
{
Vector2 pos = new Vector2(dx, dy);
pos = Vector2.Transform(pos, Matrix.CreateRotationZ(prev.Orientation));
prev = new Pose(pos + prev.Position, prev.Orientation + pieceAngle, action.Gear);
poses.Add(prev);
}
}
else
{
float pieceLength = action.Length * unit / n;
float dx = pieceLength * (float)Math.Cos(prev.Orientation);
float dy = pieceLength * (float)Math.Sin(prev.Orientation);
if (action.Gear == Gear.Backward)
{
dx = -dx;
dy = -dy;
}
for (int i = 0; i < n; i++)
{
prev = new Pose(dx + prev.X, dy + prev.Y, prev.Orientation, action.Gear);
poses.Add(prev);
}
}
}
return poses;
}
public static void Drive(Pose start, ReedsSheppActionSet actions, float unit)
{
Pose current = new Pose(start);
foreach (ReedsSheppAction action in actions.Actions)
{
switch (action.Steer)
{
case Steer.Straight:
current = Straight(current, action.Gear, action.Length, unit);
break;
case Steer.Left:
current = TurnLeft(current, action.Gear, action.Length, unit);
break;
case Steer.Right:
current = TurnRight(current, action.Gear, action.Length, unit);
break;
}
if (Debug != null)
current.DrawPose(Debug, 2, Color.LightBlue);
}
}