/// <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; }