/// <summary>
/// Calculates the shortest path through the submitted graph
/// from the submitted start and end points. Throws exception
/// if a path cannot be computed. The implementation uses the
/// breadth-first search algorithm to traverse the graph.
/// </summary>
/// <param name="graph">Graph to traverse.</param>
/// <param name="startPoint">Start point of path.</param>
/// <param name="endPoint">End point of path.</param>
/// <returns></returns>
public Point[] CalculateLine(Node[][] graph, Point startPoint, Point endPoint)
{
Stopwatch timer = new Stopwatch(); // DEBUG code
// Prepare the graph for searching.
GraphTools<Node>.SearchReset(graph);
// Queue that will hold the next nodes to be processed.
Queue<Node> queue = new Queue<Node>();
Node startNode = graph[startPoint.X][startPoint.Y];
// Set the start node to be parent of itself to signify it as start point.
startNode.Parent = graph[startPoint.X][startPoint.Y];
// Set distance of start node to 0
startNode.Distance = 0;
// Enqueue the start point unless it is allready occupied.
if (!startNode.Occupied)
queue.Enqueue(graph[startPoint.X][startPoint.Y]);
int count = 0; // DEBUG code
timer.Start(); // DEBUG code
while (queue.Count > 0)
{
count++; // DEBUG code
// Next node in the queue.
Node current = queue.Dequeue();
// If we found the end point end loop
if (current.X == endPoint.X && current.Y == endPoint.Y)
{
break;
}
// Get nodes adjacent to the node being processed.
Node[] adjacent = GraphTools<Node>.GetAdjacentElements(graph, current);
// For each adjacent node check that it's not occupied by another line
// and it has not been visisted by a shorter path.
foreach (Node node in adjacent)
{
if (!node.Occupied && (current.Distance + GraphTools<Node>.Distance(current, node)) < node.Distance)
{
// Update the adjacent node and enqueue it.
node.Parent = current;
node.Distance = current.Distance + GraphTools<Node>.Distance(current, node);
queue.Enqueue(node);
}
}
}
timer.Stop(); // DEBUG code
Debug.Print(string.Format("Number of iterations for BFS: {0}, time: {1}ms", count, timer.ElapsedMilliseconds)); // DEBUG code
// Use the helper function to find the computed path and return it.
return GetPath(graph[endPoint.X][endPoint.Y]);
}
public void WhenComparingSmaller_ReturnsPostive()
{
//Prepare
Node a = new Node { Distance = 2 };
Node b = new Node { Distance = 1 };
DistanceComparer<Node> target = new DistanceComparer<Node>();
//Act
int result = target.Compare(a, b);
//Verify
Assert.IsTrue(result > 0);
}
public void WhenComparingEqual_ReturnsZero()
{
//Prepare
Node a = new Node { Distance = 1 };
Node b = new Node { Distance = 1 };
DistanceComparer<Node> target = new DistanceComparer<Node>();
//Act
int result = target.Compare(a, b);
//Verify
Assert.AreEqual(0, result);
}
public void WhenComparingSmaller_ReturnsPostive()
{
//Prepare
Node a = new Node { Distance = 1, X = 1, Y = 1 };
Node b = new Node { Distance = 1, X = 2, Y = 2 };
Node c = new Node { Distance = 1, X = 1, Y = 1 };
Node d = new Node { Distance = 2, X = 1, Y = 1 };
Node goal = new Node { X = 5, Y = 5 };
DirectedDistanceComparer<Node> target = new DirectedDistanceComparer<Node>(goal);
//Act
int result1 = target.Compare(a, b);
int result2 = target.Compare(d, c);
//Verify
Assert.IsTrue(result1 > 0);
Assert.IsTrue(result2 > 0);
}
public void WhenDistanceCalled_ReturnsDouble()
{
//Prepare
Node a = new Node { X = 2, Y = 2 };
Node b = new Node { X = 3, Y = 3 };
Node c = new Node { X = 0, Y = 0 };
Node d = new Node { X = 1, Y = 1 };
Node e = new Node { X = 1, Y = 0 };
//Act
double result1 = GraphTools<Node>.Distance(a, b);
double result2 = GraphTools<Node>.Distance(b, a);
double result3 = GraphTools<Node>.Distance(c, d);
double result4 = GraphTools<Node>.Distance(d, d);
double result5 = GraphTools<Node>.Distance(d, e);
//Verify
Assert.AreEqual(Math.Sqrt(2), result1);
Assert.AreEqual(Math.Sqrt(2), result2);
Assert.AreEqual(Math.Sqrt(2), result3);
Assert.AreEqual(0, result4);
Assert.AreEqual(1, result5);
}
/// <summary>
/// Calculates the shortest path through the submitted graph
/// from the submitted start and end points. Throws exception
/// if a path cannot be computed. The implementation uses
/// the A* algorithm to find the shortest path.
/// </summary>
/// <param name="graph">Graph to traverse.</param>
/// <param name="startPoint">Start point of path.</param>
/// <param name="endPoint">End point of path.</param>
/// <returns></returns>
public Point[] CalculateLine(Node[][] graph, Point startPoint, Point endPoint)
{
bool found = false;
Stopwatch timer = new Stopwatch(); // DEBUG code
// This implementation requires the nodes to be PriorityQueueNodes.
PriorityQueueNode[][] prioGraph = graph as PriorityQueueNode[][];
// Prepare the nodes for searching
GraphTools<Node>.SearchReset(graph);
// Set distance to start point to 0
PriorityQueueNode source = prioGraph[startPoint.X][startPoint.Y];
source.Distance = 0;
source.Handle = null;
// Create a new priority queue and add the start node to it
IPriorityQueue<PriorityQueueNode> queue = this.CreateQueue(graph, graph[endPoint.X][endPoint.Y]);
queue.Add(ref source.Handle, source);
int count = 0; //DEBUG code
timer.Start(); // DEBUG code
while(!queue.IsEmpty)
{
count++; // DEBUG code
// Fetch node with least distance
PriorityQueueNode u = queue.DeleteMin();
// If we arrived at the end point break the loop
if (u.X == endPoint.X && u.Y == endPoint.Y)
{
found = true;
break;
}
// For each adjacent node
PriorityQueueNode[] adjacent = GraphTools<PriorityQueueNode>.GetAdjacentElements(prioGraph, u);
foreach (PriorityQueueNode v in adjacent)
{
// That is not occupied
if(!v.Occupied)
{
// If dist[u] + dist[u][v] < dist[v] we found a shorter path to v.
double distance = u.Distance + GraphTools<Node>.Distance(u, v);
if(distance < v.Distance)
{
v.Distance = distance;
v.Parent = u;
// Add or replace the found node to the priority queue
PriorityQueueNode x;
if(v.Handle == null)
queue.Add(ref v.Handle, v);
else if (queue.Find(v.Handle, out x))
queue.Replace(v.Handle, v);
else
queue.Add(ref v.Handle, v);
}
}
}
}
timer.Stop(); // DEBUG code
Debug.Print(string.Format("Number of iterations for A*: {0}, time: {1}ms", count, timer.ElapsedMilliseconds)); // DEBUG code
if (!found)
throw new ApplicationException("Unable to find a path between points.");
// Use the helper function to find the computed path and return it.
return GetPath(graph[endPoint.X][endPoint.Y]);
}
/// <summary>
/// Get the path from a traversed graph.
/// </summary>
/// <param name="endPoint">Point where path ends.</param>
/// <returns>An array of <see cref="Point"/>.</returns>
private Point[] GetPath(Node endPoint)
{
List<Point> path = new List<Point>();
Node current = endPoint;
// Add nodes to the path until we reach the nodes which has itself as parent
// which signifies it is the start point.
while (current.Parent != null)
{
path.Add(new Point { X = current.X, Y = current.Y });
current = current.Parent;
}
// Add the last point
path.Add(new Point { X = current.X, Y = current.Y });
// Reverse the order since we began at the end point.
path.Reverse();
return path.ToArray();
}
/// <summary>
/// Creates a new IPriorityQueue containing submitted
/// Node elements.
/// </summary>
/// <param name="nodes">Elements to create queue with.</param>
/// <param name="goal">The goal node</param>
/// <returns>A IPriorityQueue containing sumbitted nodes.</returns>
private IPriorityQueue<PriorityQueueNode> CreateQueue(Node[][] nodes, Node goal)
{
IntervalHeap<PriorityQueueNode> queue = new IntervalHeap<PriorityQueueNode>(PriorityQueueNode.SortByDirectedDistance(goal));
foreach (Node[] subnodes in nodes)
foreach (PriorityQueueNode node in subnodes)
{
node.Handle = null;
}
return queue;
}
/// <summary>
/// Get IComparer for sorting by the Distance property
/// with a predefined goal node.
/// </summary>
/// <param name="goal">The goal node</param>
/// <returns>A new instance of IComparer</returns>
public static IComparer<Node> SortByDirectedDistance(Node goal)
{
return new DirectedDistanceComparer<Node>(goal);
}
public void WhenSearchResetCalled_ResetsNodes()
{
//Prepare
Node parent = new Node();
int height = 100;
int width = 200;
Node[][] result = GraphTools<Node>.CreateGraph(height, width);
for (int i = 0; i < result.Length; i++)
for (int j = 0; j < result[i].Length; j++)
{
result[i][j].Parent = parent;
result[i][j].Distance = i+j;
}
//Before the Act phase check that values have indeed been set.
for (int i = 0; i < result.Length; i++)
for (int j = 0; j < result[i].Length; j++)
{
Assert.IsInstanceOfType(result[i][j], typeof(Node));
Assert.IsFalse(result[i][j].Occupied);
Assert.IsNotNull(result[i][j].Parent);
Assert.AreEqual(i, result[i][j].X);
Assert.AreEqual(j, result[i][j].Y);
Assert.AreEqual(i + j, result[i][j].Distance);
}
//Act
GraphTools<Node>.SearchReset(result);
//Verify
Assert.AreEqual(width, result.Length);
Assert.AreEqual(height, result[0].Length);
for (int i = 0; i < result.Length; i++)
for (int j = 0; j < result[i].Length; j++)
{
Assert.IsInstanceOfType(result[i][j], typeof(Node));
Assert.IsFalse(result[i][j].Occupied);
Assert.IsNull(result[i][j].Parent);
Assert.AreEqual(i, result[i][j].X);
Assert.AreEqual(j, result[i][j].Y);
Assert.AreEqual(int.MaxValue, result[i][j].Distance);
}
}
