Πρόβλημα με αλγόριθμο Α*

[Nexus]

Προσπαθώ να υλοποιήσω τον αλγόριθμο A* (http://en.wikipedia.org/wiki/A*_search_algorithm) σε C++.

Ο ψευδοκώδικας του αλγορίθμου που υλοποιώ είναι:

>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

 

Για την υλοποίηση του openset έχω χρησιμοποιήσει μία ουρά προτεραιότητας έτσι ώστε να μπορώ να βγάζω το στοιχείο με το μικρότερο f σκόρ. Το πρόβλημα που αντιμετωπίζω είναι στα σημεία που είναι υπογραμμισμένα, δηλαδή αρχικά πως θα μπορέσω να ελέγξω εάν υπάρχει κάποιο στοιχείο στο openset (μιας και η ουρά προτεραιότητας μου δίνει πρόσβαση μόνο στο πρώτο στοιχείο) καθώς επίσης και πως θα το τροποποιώ όταν απαιτείται ? Μήπως θα πρέπει να χρησιμοποιήσω κάποια άλλη δομή για το openset ?

 

Ευχαριστώ !