internal AstarNode(AstarNode <NODE, NUMERIC> previous, NODE value, NUMERIC priority, NUMERIC cost)
{
this.Previous = previous;
this.Value = value;
this.Priority = priority;
this.Cost = cost;
}
internal static Stepper <NODE> BuildPath <NODE, NUMERIC>(AstarNode <NODE, NUMERIC> node)
{
PathNode <NODE> start = BuildPath(node, out PathNode <NODE> end);
return((Step <NODE> step) =>
{
PathNode <NODE> current = start;
while (current != null)
{
step(current.Value);
current = current.Next;
}
});
}
internal static PathNode <NODE> BuildPath <NODE, NUMERIC>(AstarNode <NODE, NUMERIC> currentNode, out PathNode <NODE> currentPathNode)
{
if (currentNode.Previous == null)
{
PathNode <NODE> start = new PathNode <NODE>(currentNode.Value);
currentPathNode = start;
return(start);
}
else
{
PathNode <NODE> start = BuildPath(currentNode.Previous, out PathNode <NODE> previous);
currentPathNode = new PathNode <NODE>(currentNode.Value);
previous.Next = currentPathNode;
return(start);
}
}
/// <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 <NODE> Graph <NODE, NUMERIC>(NODE start, Neighbors <NODE> neighbors, Heuristic <NODE, NUMERIC> heuristic, Cost <NODE, NUMERIC> cost, Goal <NODE> goal)
{
// using a heap (aka priority queue) to store nodes based on their computed A* f(n) value
IHeap <AstarNode <NODE, NUMERIC> > fringe = new HeapArray <AstarNode <NODE, NUMERIC> >(
// NOTE: Typical A* implementations prioritize smaller values
(a, b) => Compute.Compare(b.Priority, a.Priority));
// push starting node
fringe.Enqueue(
new AstarNode <NODE, NUMERIC>(
null,
start,
default(NUMERIC),
Constant <NUMERIC> .Zero));
// run the algorithm
while (fringe.Count != 0)
{
AstarNode <NODE, NUMERIC> current = fringe.Dequeue();
if (goal(current.Value))
{
return(BuildPath(current));
}
else
{
neighbors(current.Value,
(NODE neighbor) =>
{
NUMERIC costValue = Compute.Add(current.Cost, cost(current.Value, neighbor));
fringe.Enqueue(
new AstarNode <NODE, NUMERIC>(
current,
neighbor,
Compute.Add(heuristic(neighbor), costValue),
costValue));
});
}
}
return(null); // goal node was not reached (no path exists)
}
