I am trying to implement the A* algorithm in C++ (http://en.wikipedia.org/wiki/A*_search_algorithm).
The pseudocode i am trying to implement is:
function A*(start,goal) closedset := the empty set // The set of nodes already evaluated. openset := set containing the initial node // The set of tentative nodes to be evaluated. g_score[start] := 0 // Distance from start along optimal path. h_score[start] := heuristic_estimate_of_distance(start, goal) f_score[start] := h_score[start] // Estimated total distance from start to goal through y. while openset is not empty x := the node in openset having the lowest f_score value if x = goal return reconstruct_path(came_from[goal]) remove x from openset add x to closedset foreach y in neighbor_nodes(x) if y in closedset continue tentative_g_score := g_score[x] + dist_between(x,y) [U] if y not in openset[/U] add y to openset tentative_is_better := true elseif tentative_g_score < g_score[y] tentative_is_better := true else tentative_is_better := false if tentative_is_better = true [U]came_from[y] := x[/U] [U]g_score[y] := tentative_g_score[/U] [U]h_score[y] := heuristic_estimate_of_distance(y, goal)[/U] [U] f_score[y] := g_score[y] + h_score[y][/U] return failure function reconstruct_path(current_node) if came_from[current_node] is set p = reconstruct_path(came_from[current_node]) return (p + current_node) else return current_node
I am using for the openset a priority queue (of a custom data type, which contains x,y coordinates as well as g,h,f scores) in order to pop the element with the lower f score. The problem i am facing is in the underlined points, firstly how can I check if an element is included into the openset (in priority queue you can access only first element) and how can I modify that element when it is necessary ? Do I have to use a different structure for the openset ?
Thanks in advance !