public IList <INode> Dijkstra(MyAbstractGraph <INode, IEdge> i_Graph, INode i_StartNode, INode i_TargetNode) { bool isTargetFound = false; if (!i_Graph.GetAllNodes().Contains(i_StartNode)) { Console.WriteLine("starting node is not in the graph"); return(null); } PriorityQueueCopied <INode> priorityQueue = new PriorityQueueCopied <INode>(true); Dictionary <INode, int> distances = new Dictionary <INode, int>(); Dictionary <INode, INode> parents = new Dictionary <INode, INode>(); foreach (INode node in i_Graph.GetAllNodes()) { priorityQueue.Enqueue(int.MaxValue / 3, node); distances[node] = int.MaxValue / 3; parents[node] = null; } distances[i_StartNode] = 0; priorityQueue.UpdatePriority(i_StartNode, 0); while (priorityQueue.Count != 0) { INode minNode = priorityQueue.Dequeue(); foreach (IEdge neighbor in i_Graph.GetNeighbors(minNode)) { if (priorityQueue.IsInQueue(neighbor.V)) { int newDistance = distances[minNode] + neighbor.GetWeight(); if (newDistance < distances[neighbor.V]) { { distances[neighbor.V] = newDistance; priorityQueue.UpdatePriority(neighbor.V, newDistance); parents[neighbor.V] = minNode; } } } } } //UnityEngine.Debug.Log("distance: " + distances[i_TargetNode]); if (distances[i_TargetNode] == int.MaxValue) { return(null); } else { List <INode> resPath = new List <INode>(); INode currentNode = i_TargetNode; while (currentNode != null) { resPath.Insert(0, currentNode); currentNode = parents[currentNode]; } return(resPath); } }
private void visit(MyAbstractGraph <INode, IEdge> i_Graph, INode i_Root, Dictionary <INode, NodeStatus> i_VisitStatus, List <Tuple <INode, NodeStatus> > i_VisitedOrder) { i_VisitStatus[i_Root] = NodeStatus.Visited; i_VisitedOrder.Add(new Tuple <INode, NodeStatus>(i_Root, NodeStatus.Visited)); foreach (IEdge edge in i_Graph.GetNeighbors(i_Root)) { if (i_VisitStatus[edge.V] == NodeStatus.Unvisited && !edge.V.IsObstacle) { visit(i_Graph, edge.V, i_VisitStatus, i_VisitedOrder); } } i_VisitStatus[i_Root] = NodeStatus.Done; i_VisitedOrder.Add(new Tuple <INode, NodeStatus>(i_Root, NodeStatus.Done)); }
//returns //1. list of tuples - (node, distance) //2. list of the result path public Tuple <IList <Tuple <INode, int> >, IList <INode> > AstarSearch(MyAbstractGraph <INode, IEdge> i_Graph, INode i_StartNode, INode i_TargetNode) { if (!i_Graph.GetAllNodes().Contains(i_StartNode)) { Console.WriteLine("starting node is not in the graph"); return(null); } List <Tuple <INode, int> > distancesHistory = new List <Tuple <INode, int> >(); PriorityQueueCopied <INode> fScorePriorityQueue = new PriorityQueueCopied <INode>(true); // For node n, fScore[n] := gScore[n] + h(n). Dictionary <INode, int> gScore = new Dictionary <INode, int>(); // For node n, gScore[n] is the cost of the cheapest path from start to n currently known. Dictionary <INode, INode> parents = new Dictionary <INode, INode>(); foreach (INode node in i_Graph.GetAllNodes()) { if (node.Id != i_StartNode.Id) { fScorePriorityQueue.Enqueue(int.MaxValue / 3, node); gScore[node] = int.MaxValue / 3; } parents[node] = null; } gScore[i_StartNode] = 0; fScorePriorityQueue.Enqueue(manhattanDistance(i_Graph, (MyUnityNode)i_StartNode, (MyUnityNode)i_TargetNode), i_StartNode); distancesHistory.Add(new Tuple <INode, int>(i_StartNode, 0)); while (fScorePriorityQueue.Count != 0) { INode minNode = fScorePriorityQueue.Dequeue(); if (minNode.Id == i_TargetNode.Id) { break; } foreach (IEdge neighbor in i_Graph.GetNeighbors(minNode)) { if (fScorePriorityQueue.IsInQueue(neighbor.V))// not sure if i need this line! { int tentative_gScore = gScore[minNode] + neighbor.GetWeight(); if (tentative_gScore < gScore[neighbor.V]) { { parents[neighbor.V] = minNode; gScore[neighbor.V] = tentative_gScore; fScorePriorityQueue.UpdatePriority(neighbor.V, tentative_gScore + manhattanDistance(i_Graph, (MyUnityNode)neighbor.V, (MyUnityNode)i_TargetNode)); if (neighbor.V.Id != i_TargetNode.Id) { distancesHistory.Add(new Tuple <INode, int>(neighbor.V, tentative_gScore)); } } } } } } //UnityEngine.Debug.Log("distance: " + distances[i_TargetNode]); if (gScore[i_TargetNode] == int.MaxValue) { return(null); } else { List <INode> resPath = new List <INode>(); INode currentNode = i_TargetNode; while (currentNode != null) { resPath.Insert(0, currentNode); currentNode = parents[currentNode]; } return(Tuple.Create <IList <Tuple <INode, int> >, IList <INode> >(distancesHistory, resPath)); } }