public static IEnumerable <IGraphNode <T> > AStar <T>(this IGraphNode <T> from, IGraphNode <T> to)
{
//'let'
//Initial state of the algorithm
var closedSet = new HashSet <IGraphNode <T> >();
var openSet = new HashSet <IGraphNode <T> >()
{
from
};
var gScore = new Map <IGraphNode <T>, float>()
{
{ from, 0 }
};
var fScore = new Map <IGraphNode <T>, float>()
{
{ from, from.Heuristic(to) }
};
var previous = new Map <IGraphNode <T>, IGraphNode <T> >();
var initialState = AStarState.New(closedSet, openSet, gScore, fScore, previous);
//Compute final state from a sequence generated from the initial state
//(the functional replacement of the queue), that is the last state of the following
//generated sequence
var finalState = Generate(initialState, state =>
{
//If we found the last node, we're finished generating states
if (state.lastNode != null)
{
return(null);
}
//'let'
//Get the one with lowest fScore, lazyness prevents the full query from executing
var node = state.openSet.OrderBy(x => state.fScore[x]).First();
//Reached the target, finish the state generation
if (node.Satisfies(to) || openSet.Count == 0)
{
return(FinalAStarState(state, node));
}
//'in'
return(NextAStarState(CloseNode(state, node), node, to));
}).Last();
//'in'
//Reconstruct the path
return(Generate(finalState.lastNode, node => finalState.previous[node]).Reverse());
}