public static Queue<string> Serialize(Route route)
{
var result = new Queue<string>();
if (route == null || route.Steps == null || route.Steps.Count < 1 ||
route.Start == null || route.Start.Offset == null || route.Start.Position == null)
return result;
var steps = route.Steps.OrderByDescending(x => x.Order).ToList();
Point current = route.Start.Position,
previous = route.Start.Offset,
next = steps[0].Point;
for (var i = 0; i < steps.Count; i++) {
if (i > 0) {
current = steps[i - 1].Point;
previous = i == 1
? route.Start.Position
: steps[i - 2].Point;
next = steps[i].Point;
}
var currentVector = new Vector(previous, current);
var nextVector = new Vector(current, next);
if (i == 0)
currentVector.InvertDirection();
var angle = currentVector.AngleBetween(nextVector);
if (Math.Abs(angle) > AngleEpsilon)
result.Enqueue(RotateCommand(angle, route.Id.ToString(), steps[i].Id.ToString()));
result.Enqueue(MoveCommand(nextVector.Absolute(), route.Scale, route.Id.ToString(), steps[i].Id.ToString()));
}
return result;
}
public double ComputeAngle(Vector vector)
{
return Math.Acos((X * vector.X + Y * vector.Y) / (Absolute() * vector.Absolute()));
}