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