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