public static IEnumerable<Point> AStar(IEnumerable<Point> starts, Func<Point, bool> isGoal, BitArray passable, Func<Point, Point, int> c, Func<Point, int> h) { // closedset := the empty set // The set of nodes already evaluated. var closedset = new HashSet<Point>(); // openset := {start} // The set of tentative nodes to be evaluated, initially containing the start node var openset = new HashSet<Point>(starts); // came_from := the empty map // The map of navigated nodes. var came_from = new Dictionary<Point, Point>(); // g_score[start] := 0 // Cost from start along best known path. var g_score = new Dictionary<Point, int>(); starts.ForEach(s => g_score[s] = 0); // // Estimated total cost from start to goal through y. // f_score[start] := g_score[start] + heuristic_cost_estimate(start, goal) var f_score = new Dictionary<Point, int>(); starts.ForEach(s => f_score[s] = h(s)); // while openset is not empty while (openset.Count > 0) { // current := the node in openset having the lowest f_score[] value var current = openset.minByValue(p => f_score[p]); // if current = goal if (isGoal(current)) { // if current_node in came_from // p := reconstruct_path(came_from, came_from[current_node]) // return (p + current_node) // else // return current_node var path = new LinkedList<Point>(); path.AddFirst(current); while (came_from.ContainsKey(current)) { current = came_from[current]; path.AddFirst(current); } return path; } // remove current from openset openset.Remove(current); // add current to closedset closedset.Add(current); // for each neighbor in neighbor_nodes(current) foreach (Point neighbor in GetNeighbors(current, passable)) { // tentative_g_score := g_score[current] + dist_between(current,neighbor) var gs = g_score[current] + c(current, neighbor); // tentative_f_score := tentative_g_score + heuristic_cost_estimate(neighbor, goal) var fs = gs + h(neighbor); // if neighbor in closedset and tentative_f_score >= f_score[neighbor] if (closedset.Contains(neighbor) && fs >= f_score[neighbor]) // continue continue; // if neighbor not in openset or tentative_f_score < f_score[neighbor] // came_from[neighbor] := current came_from[neighbor] = current; // g_score[neighbor] := tentative_g_score g_score[neighbor] = gs; // f_score[neighbor] := tentative_f_score f_score[neighbor] = fs; // if neighbor not in openset // add neighbor to openset openset.Add(neighbor); } } // return failure return null; }