//************************************************************************************************
// Private Methods
//************************************************************************************************
/// <summary>
///
/// </summary>
/// <returns></returns>
private Queue <MoveCommand> ConvertPath()
{
Queue <MoveCommand> newPath = new Queue <MoveCommand>();
if (Status.Path != null && Status.Path.Count > 0)
{
Point facing = facingToVector(Status.Position.facing); // We'll use this to figure out the current facing and use it as a vector
Point previous = new Point(Status.Path[0].Position.X, Status.Path[0].Position.Y);
previous.Offset(new Point(-facing.X, -facing.Y)); // Make a fake previous point for later calculations.
int forwardDist = 0; // If we have multiple forwards, it will combine them to one move
Point current = Status.Position.location; // Robot's current point
Point next;
for (int i = 0; i < Status.Path.Count; i++) // we go to the second to last point, the last move will take us from 2nd-to-last to the last point.
{
next = Status.Path[i].Position; // Robot's next point, necessary for deciding if we need to turn.
Point lastDisplacement = new Point(current.X - previous.X, current.Y - previous.Y); // what direction are we currently facing?
Point nextDisplacement = new Point(next.X - current.X, next.Y - current.Y); // What direction and how far are we going
if (lastDisplacement.X == nextDisplacement.X || lastDisplacement.Y == nextDisplacement.Y) // not turning
{
forwardDist += Math.Abs(nextDisplacement.X + nextDisplacement.Y); // One of these will be zero, and we're just looking for the magnitude.
if (forwardDist > 0)
{
newPath.Enqueue(new MoveCommand(MoveCommand.Direction.Forward, forwardDist * FORWARD_MS)); // Then enqueue your forward
}
forwardDist = 0;
}
else // if we are turning
{
MoveCommand.Direction turnDirection = DirChange(lastDisplacement, nextDisplacement);
newPath.Enqueue(new MoveCommand(turnDirection, turnDirection == MoveCommand.Direction.CW ? TURN_CW_MS : TURN_CCW_MS)); // Enqueue your turn first
forwardDist += Math.Abs(nextDisplacement.X + nextDisplacement.Y); // One of these will be zero, and we're just looking for the magnitude.
if (forwardDist > 0)
{
newPath.Enqueue(new MoveCommand(MoveCommand.Direction.Forward, forwardDist * FORWARD_MS)); // Then enqueue your forward
}
forwardDist = 0; // reset forward
}
previous = current;
current = next;// Increment the previous point
}
}
foreach (MoveCommand move in newPath)
{
String msg = "\n\r dir: " + move.direction + " duration" + move.duration;
m_UI.PostMessage(msg);
}
return(newPath);
}
public MoveCommandParameter(MoveCommand.Direction _direction, Player _player)
{
m_direction = _direction;
m_player = _player;
}
