Hello, I am currently trying to implement A* search, and I am pushing all of the costs to travel from one node to the next via priority queue. The only problem I am running into is that when I use the pop() feature, I am getting back the highest node cost as opposed to the lowest. Can anyone please teach me where is a good place to learn how to build a constructor for my PQ so that the LOWEST value gets priority as opposed to the highest? Thank you.
I have tried building my own constructor (commented out in beginning), but I am not very good with C++ so I am really not sure how to get it working correctly.
Here is my code thus far:
#include <iostream>
#include <list>
#include <queue>
#include <cmath>
using namespace std;
//setting up priority queue
/*struct Cost {
int f;
};
class CompareCost {
public:
bool operator()(Cost) // Returns true if lowest ????
{
if(f <
return 1;
}
}
}
*/
//declaring variables/vectors/matricies
double parent[10], whichlist[10], connected[10][10], h[10], g[10][10], f[10], xval[10], yval[10];
double bestcost = 0;
int mapsize = 9; //# of nodes, where 0 is counted as an node.
int currentnode, goalnode, nextnode, counter = 0, result = 0, flag = 0, compare = 1;
list<double> path;
priority_queue<double> bestnode;
//====================================function to create matrixes
void initmatrix(int cn, int gn) // cn = currentnode gn = goalnode
{
//initialize everything to 0
for(int i = 0; i <= mapsize; i++)
{
for(int j = 0; j <= mapsize; j++)
{
g[i][j] = 0;
}
h[i] = 0;
f[i] = 0;
}
//now determine values
for(int i = 0; i <= mapsize; i++)
{
for(int j = 0; j <= mapsize; j++)
{
if(connected[i][j] == 1 || connected[j][i] == 1)
{
//calculate distance between two nodes if they are connected
//g(n)
g[i][j] = sqrt((xval[i]-xval[j])*(xval[i]-xval[j])+(yval[i]-yval[j])*(yval[i]-yval[j]));
g[j][i] = g[i][j];
//calculate heuristic distance
//h(n)
h[i] = abs(xval[i]-xval[gn]) + abs(yval[i] - yval[gn]);
//calculate f score
//f(n) = g(n) + h(n)
f[i] = g[i][j] + h[i];
}
}
}
}
//=======================pathfinder=================
int pathfind(int cn, int gn)
{ // 1 = found 0 = not found 2 = error
while(1)
{
//check to see of goal reached
if(cn == gn)
{
return 1;
}
//push current node to open list if not already on it.
if(whichlist[cn] == 0)
{
whichlist[cn] = 1;
}
//add nearby walkable nodes to open list
for(int i = 0; i <= mapsize; i++)
{
if(connected[cn][i] == 1 && whichlist[i] == 0) //nodes are connected and not on closed/open already
{
whichlist[i] = 1; //place node on open list
cout << "Pushing in node to open: " << i << endl;
cout << "F cost: " << f[i] << endl;
bestnode.push(f[i]); //push f cost onto priority queue
counter++;
}
}
//add currentnode to closed list
cout << "Pushing node to closed: " << cn << endl;
whichlist[cn] = 2;
path.push_back(cn);
//check for dead end
if(counter == 0)
{
cout << "Open list empty" << endl;
break;
flag = 0;
while(flag = 0)
{
bestcost = bestnode.top();
if(!bestnode.empty())
{
bestnode.pop();
}
else
{
cout << "Open list empty" << endl;
break;
}
//search for next takable route
for(int i = 0; i <= mapsize; i++)
{
if(f[i] == bestcost && whichlist[i] == 1)
{
bestcost = i;
cout << endl << "alternate node: " << i << endl;
flag = 1;
}
}
}
}
else // if not at a dead end, then decide which open node to take
{
//takes in the lowest f cost
bestcost = bestnode.top();
//pops the lowest f cost off the list
if(!bestnode.empty()) bestnode.pop();
//display lowest f cost
cout << "Lowest Cost: " << bestcost << endl;
//search for which node corresponds to the lowest f cost
for(int i = 0; i <= mapsize; i++)
{
cout << "F[" << i << "]: " << f[i] << " Bestcost: " << bestcost << endl;
compare = f[i] - bestcost;
cout << "difference: " << compare << " Connected: " << connected[cn][i] << endl
<< "Whichlist: " << whichlist[i] << endl;
if(compare == 0 && connected[cn][i] == 1 && whichlist[i] == 1)
{
cout << "Next Node Search Successful" << endl;
nextnode = i;
cout << endl << "Nextnode: " << i << endl;
flag = 1;
}
else if(i >= mapsize && flag == 0)
{
cout << "Next Node Search failed" << endl;
}
}
flag = 0;
cn = nextnode;
cout << " from node: " << cn << endl;
}
counter = 0;
//check to see if route cannot be found
for(int i = 0; i <= mapsize; i++)
{
if(whichlist[i] == 2)
counter++;
if(counter >= mapsize)
return 2;
}
counter = 0;
}//end of while(1)
return 0;
}// end of path finding function
//main function
int main()
{
//hardcode map
xval[0] = 0;
yval[0] = 0;
xval[1] = 0;
yval[1] = -3;
xval[2] = 4;
yval[2] = -3;
xval[3] = 4;
yval[3] = 0;
xval[4] = 4;
yval[4] = 3;
xval[5] = 8;
yval[5] = 3;
xval[6] = 8;
yval[6] = -3;
xval[7] = 8;
yval[7] = -6;
xval[8] = 4;
yval[8] = -6;
xval[9] = 2;
yval[9] = -6;
//initialize connected matrix
for(int i = 0; i <= mapsize;i++)
{
for(int j = 0; j <= mapsize; j++)
{
connected[i][j] = 0;
}
}
//hardcore connection matrix. determines if two nodes are connected/walkable.
//node 0 to 1
connected[0][1] = 1;
connected[1][0] = 1;
//node 1 to 2
connected[1][2] = 1;
connected[2][1] = 1;
//node 0 to 3
connected[0][3] = 1;
connected[3][0] = 1;
//node 2 to 3
connected[3][2] = 1;
connected[2][3] = 1;
//node 3 to 4
connected[3][4] = 1;
connected[4][3] = 1;
//node 4 to 5
connected[4][5] = 1;
connected[5][4] = 1;
//node 5 to 6
connected[5][6] = 1;
connected[6][5] = 1;
//node 6 to 7
connected[6][7] = 1;
connected[7][6] = 1;
//node 2 to 6
connected[2][6] = 1;
connected[6][2] = 1;
//node 7 to 8
connected[7][8] = 1;
connected[8][7] = 1;
//node 2 to 8
connected[2][8] = 1;
connected[8][2] = 1;
//node 8 to 9
connected[8][9] = 1;
connected[9][8] = 1;
//================Set Start/Finish===========
currentnode = 0;
goalnode = 7;
//==========================================
//initialize matrix
initmatrix(currentnode, goalnode);
result = pathfind(currentnode, goalnode);
cout << "finished, the result is: " << endl;
if(result == 1) cout << "success" << endl;
else if(result == 0) cout << "failure" << endl;
else if(result == 2) cout << "error" << endl;
else cout << "big error" << endl;
return 0;
}