private static void Relax(DijkstraNode v, DijkstraNode u) { int alt = u.Distance + PathfindingConstants.OrthogonalCost; if (alt < v.Distance) { v.Distance = alt; v.Parent = u; } }
private static DijkstraNode ExtractMin(ref List <DijkstraNode> Q) { int index = 0; DijkstraNode min = Q[0]; for (int i = 1; i < Q.Count; ++i) { var curr = Q[i]; int comparison = curr.CompareTo(min); if (comparison < 0) { min = curr; index = i; } } Q.RemoveAt(index); return(min); }
/// <summary> /// Performs the Dijkstra searching algorithm to find the best paths to the goal node from any node. /// </summary> /// <param name="grid">The grid of nodes to solve (or search).</param> /// <param name="goal"> /// The goal node. Note that there is no need for a starting node because this algorithm solves the /// shortest path from any node to the goal node. /// </param> public static void SolveGrid(DijkstraNode[,] grid, DijkstraNode goal) { List <DijkstraNode> Q = new List <DijkstraNode>(grid.Length); int cols = grid.GetLength(0); int rows = grid.GetLength(1); for (int c = 0; c < cols; ++c) { for (int r = 0; r < rows; ++r) { DijkstraNode v = grid[c, r]; if (!v.IsIllegal) { v.Reset(); Q.Add(v); } } } goal.Reset(); goal.Distance = 0; while (Q.Count > 0) { // extra the minimum value DijkstraNode u = ExtractMin(ref Q); // relax each of the neighbors foreach (DijkstraNode v in GetNeighbors(u.GetAdjacentPoints(), grid)) { if (v == null || v.IsIllegal) { continue; } Relax(v, u); } } }