private void Relax(DirectedEdge edge) { int v = edge.From; int w = edge.To; if (_distanceTo[w] > _distanceTo[v] + edge.Weight) { _distanceTo[w] = _distanceTo[v] + edge.Weight; _edgeTo[w] = edge; if (_priorityQueue.Contains(w)) { _priorityQueue.DecreaseKey(w, _distanceTo[w]); } else { _priorityQueue.Insert(w, _distanceTo[w]); } } }
/// <summary> /// Returns an List of Cells representing a shortest path from the specified source to the specified destination /// </summary> /// <param name="source">The source Cell to find a shortest path from</param> /// <param name="destination">The destination Cell to find a shortest path to</param> /// <param name="map">The Map on which to find the shortest path between Cells</param> /// <returns>List of Cells representing a shortest path from the specified source to the specified destination</returns> public List <TCell> FindPath(TCell source, TCell destination, IMap <TCell> map) { // OPEN = the set of nodes to be evaluated IndexMinPriorityQueue <PathNode> openNodes = new IndexMinPriorityQueue <PathNode>(map.Height * map.Width); // CLOSED = the set of nodes already evaluated bool[] isNodeClosed = new bool[map.Height * map.Width]; // add the start node to OPEN openNodes.Insert(map.IndexFor(source), new PathNode { DistanceFromStart = 0, HeuristicDistanceFromEnd = CalculateDistance(source, destination, _diagonalCost), X = source.X, Y = source.Y, Parent = null }); PathNode currentNode; // loop while (true) { // current = node in OPEN with the lowest f_cost if (openNodes.Size < 1) { return(null); } currentNode = openNodes.MinKey(); // remove current from OPEN int currentIndex = openNodes.DeleteMin(); // add current to CLOSED isNodeClosed[currentIndex] = true; ICell currentCell = map.CellFor(currentIndex); // if current is the target node the path has been found if (currentCell.Equals(destination)) { break; } // foreach neighbor of the current node bool includeDiagonals = _diagonalCost.HasValue; foreach (TCell neighbor in map.GetAdjacentCells(currentCell.X, currentCell.Y, includeDiagonals)) { int neighborIndex = map.IndexFor(neighbor); // if neighbor is not walkable or neighbor is in CLOSED if (neighbor.IsWalkable == false || isNodeClosed[neighborIndex]) { // skip to the next neighbor continue; } bool isNeighborInOpen = openNodes.Contains(neighborIndex); // if neighbor is in OPEN if (isNeighborInOpen) { // if new path to neighbor is shorter PathNode neighborNode = openNodes.KeyAt(neighborIndex); double newDistance = currentNode.DistanceFromStart + 1; if (newDistance < neighborNode.DistanceFromStart) { // update neighbor distance neighborNode.DistanceFromStart = newDistance; // set parent of neighbor to current neighborNode.Parent = currentNode; } } else // if neighbor is not in OPEN { // set f_cost of neighbor // set parent of neighbor to current PathNode neighborNode = new PathNode { DistanceFromStart = currentNode.DistanceFromStart + 1, HeuristicDistanceFromEnd = CalculateDistance(source, destination, _diagonalCost), X = neighbor.X, Y = neighbor.Y, Parent = currentNode }; // add neighbor to OPEN openNodes.Insert(neighborIndex, neighborNode); } } } List <TCell> path = new List <TCell>(); path.Add(map.GetCell(currentNode.X, currentNode.Y)); while (currentNode.Parent != null) { currentNode = currentNode.Parent; path.Add(map.GetCell(currentNode.X, currentNode.Y)); } path.Reverse(); return(path); }