/// <summary>Runs the A* search algorithm algorithm on a graph.</summary>
/// <param name="start">The node to start at.</param>
/// <param name="neighbors">Step function for all neigbors of a given node.</param>
/// <param name="heuristic">Computes the heuristic value of a given node in a graph.</param>
/// <param name="cost">Computes the cost of moving from the current node to a specific neighbor.</param>
/// <param name="goal">Predicate for determining if we have reached the goal node.</param>
/// <returns>Stepper of the shortest path or null if no path exists.</returns>
public static Stepper <T> Astar(T start, Neighbors neighbors, Heuristic heuristic, Cost cost, Goal goal)
{
// using a heap (aka priority queue) to store nodes based on their computed A* f(n) value
Heap <Astar_Node> fringe = new HeapArray <Astar_Node>(
// NOTE: Typical A* implementations prioritize smaller values
(Astar_Node left, Astar_Node right) =>
{
Comparison comparison = Compute.Compare <Math>(right.Priority, left.Priority);
return(comparison);
});
// using a map (aka dictionary) to store costs from start to current nodes
Map <Math, Astar_Node> computed_costs = new MapHashArray <Math, Astar_Node>();
// construct the f(n) for this A* execution
Astar_function function = (T node, Astar_Node previous) =>
{
Math previousCost = computed_costs.Get(previous);
Math currentCost = cost(previous.Value, node);
Math costFromStart = Compute.Add <Math>(previousCost, currentCost);
Math hueristic = heuristic(node);
return(Compute.Add <Math>(costFromStart, hueristic));
};
// push starting node
Astar_Node start_node = new Astar_Node(null, start, default(Math));
fringe.Enqueue(start_node);
computed_costs.Add(start_node, default(Math));
// run the algorithm
while (fringe.Count != 0)
{
Astar_Node current = fringe.Dequeue();
if (goal(current.Value))
{
return(Astar_BuildPath(current));
}
else
{
neighbors(current.Value,
(T neighbor) =>
{
Astar_Node newNode = new Astar_Node(current, neighbor, function(neighbor, current));
Math costValue = Compute.Add <Math>(computed_costs.Get(current), cost(current.Value, neighbor));
computed_costs.Add(newNode, costValue);
fringe.Enqueue(newNode);
});
}
}
return(null); // goal node was not reached (no path exists)
}
// methods
#region private static Stepper<T> BuildPath(Node node)
/// <summary>Builds the path from resulting from the A* algorithm.</summary>
/// <param name="node">The resulting final node fromt he A* algorithm.</param>
/// <returns>A stepper function of the computed path frmo the A* algorithm.</returns>
private static Stepper <T> Astar_BuildPath(Astar_Node node)
{
Astar_PathNode end;
Astar_PathNode start = Astar_BuildPath(node, out end);
return((Step <T> step) =>
{
Astar_PathNode current = start;
while (current != null)
{
step(current.Value);
current = current.Next;
}
});
}
private static Astar_PathNode Astar_BuildPath(Astar_Node currentNode, out Astar_PathNode currentPathNode)
{
if (currentNode.Previous == null)
{
Astar_PathNode start = new Astar_PathNode(currentNode.Value);
currentPathNode = start;
return(start);
}
else
{
Astar_PathNode previous;
Astar_PathNode start = Astar_BuildPath(currentNode.Previous, out previous);
currentPathNode = new Astar_PathNode(currentNode.Value);
previous.Next = currentPathNode;
return(start);
}
}
public Astar_Node(Astar_Node previous, T value, Math priority)
{
this._previous = previous;
this._value = value;
this._priority = priority;
}
