A* Implementation Problem

[Nexgr]

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 !

[gashtio]

You can do fine using the STL's priority queue with some modification of that pseudocode - namely always inserting the neighbours in the PQ, unless they are in the closed set. This will result in having duplicate nodes (with different f/g costs) in the PQ, so each time you pop a node, you have to check if it's already in the closed set.

i.e the pseudocode becomes (I'll be referring to the openset as PQ):

while PQ is not empty
	x := the top node of PQ
	remove x from PQ
	if x = goal
		return reconstruct_path(goal.came_from)

	if x in closedset
		continue

	add x to closedset

	foreach y in neighbor_nodes(x)
		if y in closedset
			continue

		y.came_from = x
		y.g_score = x.g_score + dist_between(x,y)
		y.h_score = heuristic_estimate_of_distance(y, goal)
		y.f_score = y.g_score + y.h_score // this is useless...

		add y to PQ

return failure

The author of the wikipedia article probably had in mind using a priority queue, which supports the DecreaseKey operation. That way the queue won't get bloated with duplicate items and you'll have a performance gain.

[Nexgr]

Thanks for your reply, but when a node is already in the openset, and I can approach it later from another node, the g score, as well as the came_from pointer must be changed (that is what the condition: "elseif tentative_g_score < g_score[y]" checking). But your code, adds again a node with the same coordinates and different g,h scores if I understand right... ?

[gashtio]

Yes, that's right - you add a node with the same coordinates but different f/g/h costs - that's the duplication I was talking about.

The "g_score[y]" solution uses an array which maps all nodes with the same coordinates to the same element of the array(and consequently each coordinate has only 1 set of f/g/h values associated with it).

In the solution I posted, no such array exist but rather each node has coordinates + f/g/h costs associated with it. Because the priority queue sorts the nodes by their f value, you're guaranteed to first get the one closest to the target, and when you encounter a node with the same coordinates you just skip it (it's inserted in the closed set the first time you see it).

[Nexgr]

Now I understand your thought, I will proceed with the implementation and I will inform.

Thanks a lot for your help !

[Nexgr]

Hello again,

I implement the algorithm, but i used three vectors, one for the openset, one for the closedset and one for the current element neighbour nodes. In order to find the element from the openset with the lower f score I create the findMin function which is described below. The problem is that the program returns the last three nodes and then stops ! Any Help ??

Thanks !

class node {
        public:

        int row;                    //Node's row number
        int col;                    //Node's column number
        int g;                      //distance of the current node from the start node
        int h;                      //estimation distance from current node to the finish node
        int f;                      //Sum of g and h
        node *came_from;      //Pointer to the previous Node

        node(int _row, int _col) {   //Constructor for dummy Node
        row = _row;
        col = _col;
        g = 0;
        h = 0;
        f = 0;
        came_from = 0;
        }
}

void reconstruct_path(node a) {
            if (a.came_from) {
                reconstruct_path(*a.came_from);
				cout << " -> (" << a.row <<"," << a.col << ")";
				               
            }
            else {
                cout <<"The path is: ";
                cout << "(" << a.row <<"," << a.col << ")";
            }
        }


int md (node a, node b){

    return abs(b.row - a.row) + abs(b.col - a.col);
}


int findMin(vector<node>& set) {

    int min = 99999;
    int pos = -1;

    for (int i = 0; i < set.size(); i++) {

        if(set[i].f < min) {
            min = set[i].f;
            pos = i;
        }
    }

    return pos;
}


int aStar (node start, node goal, int lb[rows][columns]) { //load the start, goal nodes and labyrinth

    int min_pos;
    start.g = 0;                //The distance from the start node is 0 !
    start.h = md(start, goal);  //Heuristic estimate of distance from start to goal node
    start.f = start.h;          //The total distance is equal to the heuristic estimate
    start.came_from = 0;        //Start node does not came from another node

    vector<node> openset;		// The set of tentative nodes to be evaluated

    vector<node> closedset;     //The set of nodes already evaluated

    vector<node> neighbors;     //The set of nodes that are near to current node

    openset.push_back(start);        //openset contains only the start node

    while (openset.size() > 0) {    

    min_pos = findMin(openset);	//Current evaluation node is the node with the lowest f score
    

    closedset.push_back(openset[min_pos]);      //Add the evaluated node to the closedset
	
	openset.erase(openset.begin()+min_pos);     //Remove the evaluated node from the openset

    
    if (exist(closedset,goal)>=0) {
		reconstruct_path(closedset[exist(closedset,goal)]);
        return 0;       
    }

    findNeighbors(closedset.back(), lb, neighbors);  //the function returns a vector with the neighbors of the current node

    while(!neighbors.empty()) {       //For each neighbor node

        //neighbors.back() = Node to check

        //neighbors.pop_back();               

        if (exist(closedset, neighbors.back()) >= 0) {
            neighbors.pop_back();	//Remove that node from neighbors vector
			continue;
		}

        int nb_pos = exist(openset, neighbors.back());

        if (nb_pos == -1) { //if the neighbor doesn't exist
            neighbors.back().g = closedset.back().g + 1;
            neighbors.back().h = md(neighbors.back(), goal);
            neighbors.back().f = neighbors.back().g + neighbors.back().h;
            neighbors.back().came_from = &closedset.back();				
            openset.push_back(neighbors.back());
            }


        if (nb_pos >=0 && (closedset.back().g + 1) < openset[nb_pos].g) { //if the neighbor exists and has bigger g score
            openset[nb_pos].g = closedset.back().g + 1;
            openset[nb_pos].h = md(openset[nb_pos], goal);
            openset[nb_pos].f = openset[nb_pos].g + openset[nb_pos].h;
            openset[nb_pos].came_from = &closedset.back();			
            }
			
		neighbors.pop_back();
			
        }   //end of inner while

    }       //end of outer while

    cout <<"Error: Path cannot be constructed !" << endl;

    return -1;  //else return -1
}