public void Correctly_Compute_Right_45_Angle()
{
var start = new Vector(new Point(1, 1), new Point(1, 2));
var finish = new Vector(new Point(1, 1), new Point(2, 2));
Assert.AreEqual(45 * Right, start.AngleBetween(finish));
}
public void Correctly_Compute_Left_90_Angle()
{
var start = new Vector(new Point(1, 1), new Point(1, 2));
var finish = new Vector(new Point(1, 1), new Point(0, 1));
Assert.AreEqual(90 * Left, start.AngleBetween(finish));
}
public void Correctly_Compute_180_Angle()
{
var start = new Vector(new Point(1, 1), new Point(1, 2));
var finish = new Vector(new Point(1, 1), new Point(1, 0));
Assert.AreEqual(180, start.AngleBetween(finish));
}
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;
}
private int AngleSign(Vector vector)
{
return (X * vector.Y > Y * vector.X) ? -1 : 1;
}
public double ComputeAngle(Vector vector)
{
return Math.Acos((X * vector.X + Y * vector.Y) / (Absolute() * vector.Absolute()));
}
public int AngleBetween(Vector vector)
{
return ToDegrees(ComputeAngle(vector)) * AngleSign(vector);
}
