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
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]
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 ?

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

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...

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.

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... ?

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

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

Thanks a lot for your help !

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
}
Be a part of the DaniWeb community

We're a friendly, industry-focused community of developers, IT pros, digital marketers, and technology enthusiasts meeting, networking, learning, and sharing knowledge.