/// <summary>
/// Create a graph from the matrix
/// </summary>
/// <param name="mat"></param>
/// <returns></returns>
private GraphVertex[,] CreateGraph(int[,] mat)
{
// Create the graph
GraphVertex[,] AllVerticesMat = new GraphVertex[mat.GetLength(0), mat.GetLength(1)];
for (int r = 0; r < mat.GetLength(0); r++)
{
for (int c = 0; c < mat.GetLength(1); c++)
{
if (mat[r, c] == 1)
{
// Add new graph vertex
AllVerticesMat[r, c] = new GraphVertex(r, c);
}
}
}
for (int r = 0; r < mat.GetLength(0); r++)
{
for (int c = 0; c < mat.GetLength(1); c++)
{
// Add the edges
if (r + 1 < mat.GetLength(0) && AllVerticesMat[r + 1, c] != null && AllVerticesMat[r, c] != null)
{
AllVerticesMat[r, c].AddEdge(AllVerticesMat[r + 1, c]);
}
if (c + 1 < mat.GetLength(1) && AllVerticesMat[r, c + 1] != null && AllVerticesMat[r, c] != null)
{
AllVerticesMat[r, c].AddEdge(AllVerticesMat[r, c + 1]);
}
}
}
return(AllVerticesMat);
}
public List <int> GetShortestPath(List <GraphVertex> allVertices, GraphVertex source, GraphVertex destination)
{
List <GraphVertex> shortestPath = new List <GraphVertex>();
DistanceHeapMap = new MinHeapMap <GraphVertexWithDistance>(allVertices.Count);
ParentBacktrack = new Dictionary <int, int>();
FinalDistance = new Dictionary <int, int>();
foreach (GraphVertex gv in allVertices)
{
if (gv == source)
{
DistanceHeapMap.Insert(new GraphVertexWithDistance(gv, 0));
}
else
{
DistanceHeapMap.Insert(new GraphVertexWithDistance(gv));
}
}
while (DistanceHeapMap.Count != 0)
{
GraphVertexWithDistance currentVertex = DistanceHeapMap.ExtractMin();
FinalDistance[currentVertex.Id] = currentVertex.Distance;
if (currentVertex.Id == destination.Id)
{
// we have reached the destination
return(BacktrackToGetPath(currentVertex.Id, 0));
}
foreach (KeyValuePair <GraphVertex, int> edge in currentVertex.NeighbouringEdgesWithWeight)
{
if (DistanceHeapMap.AllEntities.ContainsKey(edge.Key.Id))
{
GraphVertexWithDistance endVertex = DistanceHeapMap.AllEntities[edge.Key.Id];
if (endVertex.Distance > currentVertex.Distance + edge.Value)
{
GraphVertexWithDistance gvClone = endVertex.ShallowClone();
gvClone.Distance = currentVertex.Distance + edge.Value;
ParentBacktrack[endVertex.Id] = currentVertex.Id;
DistanceHeapMap.ChangePriority(endVertex, gvClone);
}
}
}
}
// we do not have a path from source to destination
return(null);
}
public void AddEdge(int startVertexId, int endVertexId, int weight)
{
if (!AllVertices.ContainsKey(startVertexId))
{
// if the graphvertex is not present then we need to add it
AllVertices[startVertexId] = new GraphVertex(startVertexId);
}
if (!AllVertices.ContainsKey(endVertexId))
{
// if the graphvertex is not present then we need to add it
AllVertices[endVertexId] = new GraphVertex(endVertexId);
}
AllVertices[startVertexId].NeighbouringEdgesWithWeight.Add(new KeyValuePair <GraphVertex, int>(AllVertices[endVertexId], weight));
if (!IsDirected)
{
AllVertices[endVertexId].NeighbouringEdgesWithWeight.Add(new KeyValuePair <GraphVertex, int>(AllVertices[startVertexId], weight));
}
}
/// <summary>
/// Get the path using the Heuristic search
/// Here we will use the manhattan distance as the heuristic and
/// the algorithm would optimize in the path with less manhattan distance from the endpoint
///
/// This is different than the breadth first search which expands in all direction,
/// here we travel the path which points to the direction where the endpoint is located.
///
/// Heuristic search might not find the shortest path.
///
/// Heuristic search is faster than the BFS in cases where we dont have many obstracles
/// But as the number of obstracles increases this would become closer to BFS.
///
/// A* algorithm uses a combination of dijkstra and heuristic search
/// The running time here is O(V + E)
/// </summary>
/// <param name="mat"></param>
/// <returns></returns>
public List <GraphVertex> GetPath(int[,] mat, int stRow, int stCol, int endRow, int endCol)
{
// Create the graph
GraphVertex[,] AllVerticesMat = CreateGraph(mat);
GraphVertex endVertex = AllVerticesMat[endRow, endCol];
GraphVertex startVertex = AllVerticesMat[stRow, stCol];
MinHeap <GraphVertex> priorityQueue = new MinHeap <GraphVertex>(mat.GetLength(0) * mat.GetLength(1));
Dictionary <GraphVertex, GraphVertex> backtract = new Dictionary <GraphVertex, GraphVertex>();
backtract[startVertex] = null;
startVertex.ManhattanDistance = ManhattanDistance(startVertex, endVertex);
priorityQueue.Insert(startVertex);
while (priorityQueue.HeapSize > 0)
{
GraphVertex closestVertex = priorityQueue.ExtractMin();
if (closestVertex == endVertex)
{
// We have reached the endvertex
return(BackTractToGetThePath(backtract, startVertex, endVertex));
}
foreach (GraphVertex neighbour in closestVertex.Neighbours)
{
if (!backtract.ContainsKey(neighbour))
{
// this condition indicates that the node has not been visited yet, so add the node to backtrack dictionary
// to indicate that we have visited neighbour
backtract[neighbour] = closestVertex;
// calculate the manhattan distance and insert it into priority queue
neighbour.ManhattanDistance = ManhattanDistance(neighbour, endVertex);
priorityQueue.Insert(neighbour);
}
}
}
return(null);
}
/// <summary>
/// Returns the path from the parent/backtrack dictionary
/// </summary>
/// <param name="backtract">backtrack or parent dictionary</param>
/// <param name="start">start node</param>
/// <param name="end">end node</param>
/// <returns></returns>
private List <GraphVertex> BackTractToGetThePath(Dictionary <GraphVertex, GraphVertex> backtract, GraphVertex start, GraphVertex end)
{
List <GraphVertex> path = new List <GraphVertex>();
while (end != null)
{
path.Add(end);
end = backtract[end];
}
path.Reverse();
return(path);
}
/// <summary>
/// Add an undirected edge
/// </summary>
/// <param name="neighbour"></param>
public void AddEdge(GraphVertex neighbour)
{
Neighbours.Add(neighbour);
neighbour.Neighbours.Add(this);
}
/// <summary>
/// Calculate the manhattan distance between 2 points in the graph
/// Manhattan distance is the sum of difference of distance along x and y axis
/// </summary>
/// <param name="v1"></param>
/// <param name="v2"></param>
/// <returns></returns>
private int ManhattanDistance(GraphVertex v1, GraphVertex v2)
{
return(Math.Abs(v1.Row - v2.Row) + Math.Abs(v1.Column - v2.Column));
}
public GraphVertexWithDistance(GraphVertex gv, int distance = int.MaxValue) : base(gv.Id, gv.NeighbouringEdgesWithWeight)
{
Distance = distance;
}