private void DoSearch(IEnumerable<Point> starts, Func<Point, bool> isPassable, Func<Point, bool> isGoal, Func<Point, Point, int> getCost, Func<Point, int> getH, int maxCost) { ClosedSet = new HashSet<Point>(); OpenSet = new HashSet<Point>(starts); CameFrom = new Dictionary<Point, Point>(); GScore = starts.ToDictionary(s => s, s => 0); FScore = starts.ToDictionary(s => s, s => GScore[s] + getH(s)); while (OpenSet.Any()) { var current = OpenSet.MinBy(p => FScore[p]); if (GScore[current] > maxCost) { return; } if (isGoal(current)) { Path = ConstructPath(CameFrom, current); return; } OpenSet.Remove(current); ClosedSet.Add(current); foreach (var n in GetNeighbors(current, isPassable)) { if (ClosedSet.Contains(n)) continue; var tentativeG = GScore[current] + getCost(current, n); if (!OpenSet.Contains(n) || tentativeG < GScore[n]) { CameFrom[n] = current; GScore[n] = tentativeG; FScore[n] = tentativeG + getH(n); OpenSet.Add(n); } } } }
/// <summary> /// Calculates the shortest path to every other node in the network. /// The implementation uses Dijkstra's algorithm, for use in a link-state routing protocol. /// </summary> /// <returns> /// The tree, in the form of a dictionary (node => predecessor of node in shortest path to node), or (this => null). /// </returns> private IDictionary<Node, Node> CalculateShortestPaths() { var nodes = Simulator.Nodes.Values; /// Best known distance (sum of costs) from this to each node IDictionary<Node, double> dist = nodes.ToDictionary(node => node, _ => Double.PositiveInfinity); /// Predecessor of each node in shortest-paths tree IDictionary<Node, Node> previous = nodes.ToDictionary(node => node, _ => (Node)null); dist[this] = 0; /// Nodes that have yet to be processed ISet<Node> queue = new HashSet<Node>(nodes); // TODO Priority queue would be faster; replace if needed while(queue.Count > 0) { Node u = queue.MinBy(node => dist[node]); queue.Remove(u); if(dist[u] == Double.PositiveInfinity) break; // remaining nodes are inaccessible foreach(Node v in queue .Where(v => Simulator.LinksBySrcDest.ContainsKey(Tuple.Create(u, v)))) { Link uv = Simulator.LinksBySrcDest[Tuple.Create(u, v)]; // Look up link cost from our own local table, instead of statically double new_dist = dist[u] + this.known_link_costs[uv]; if(new_dist < dist[v]) { dist[v] = new_dist; previous[v] = u; // TODO for priority queue, run queue.DecreaseKey(new_dist, v) } } } return previous; }