How to print the lead of A* algorithm - c++

Question

-I wrote a program to find the shortest path from a source node to a target node. Everything is fine, the program found the shortest path. But i have a problem, that is not able to print or get each node in the path. I tried many ways but no result. Hope anyone can help me, thanks everyone.
///////////////////////////////
#include <vector>
#include <queue>
#include <iostream>
#include <algorithm>
typedef struct Node
{
int vertex;
int g;
int h;
int f;
Node* parent;
Node(int vertex)
{
this->vertex = vertex;
this->g = 0;
this->h = 0;
this->f = 0;
this->parent=NULL;
}
Node(int vertex,int g, int h, int f,Node*parent)
{
this->vertex = vertex;
this->g = g;
this->h = h;
this->f = f;
this->parent = parent;
}
}Node;
struct Edge
{
int source;
int dest;
int g;
int h;
};
struct comp
{
bool operator()(const Node* lhs, const Node* rhs) const {
return lhs->f < rhs->f;
}
};
std::vector<Node*>openList;
std::vector<Node*>closeList;
Node* startPos;
Node* endPos;
static const int WeightW = 10;
class Graph
{
public:
std::vector<std::vector<Edge>>adjlist;
Graph(const std::vector<Edge>& edges, int N)
{
adjlist.resize(N);
for (auto &edge:edges)
{
adjlist[edge.source].push_back(edge);
}
}
};
int isContains(std::vector<Node*>* Nodelist, int vertex);
void printPath(Node*node);
void findShortestPath(const Graph& grap,Node* start,Node* end, int N)
{
Node* node;
openList.push_back(start);
while (openList.size()>0)
{
node = openList[0];
closeList.push_back(node);
openList.erase(openList.begin());
std::cout << "start" << std::endl;
int u = node->vertex;
std::cout << "V: " << u << " g :" << node->g << std::endl;
std::cout << "continous" << std::endl;
for (auto v : grap.adjlist[u])
{
if (v.dest == end->vertex)
{
std::cout << "FindNode " << v.dest << std::endl;
printPath(node);
return;
}
if (isContains(&closeList, v.dest) == -1)
{
if (isContains(&openList, v.dest) == -1)
{
int vertex = v.dest;
std::cout <<"V: "<< vertex << std::endl;
int h = v.h;
int currentg = node->g + v.g;
int f = currentg + h;
std::cout <<"vertext: "<<vertex<< " h: " << h << " g: " << currentg << " f: " << f << std::endl;
Node* newNode = new Node(vertex, currentg, h, f,node->parent);
openList.push_back(newNode);
}
}
}
std::cout<<"Close: ";
for (size_t i = 0; i < closeList.size(); i++)
{
std::cout << closeList[i]->vertex << " ";
}
std::cout << std::endl;
sort(openList.begin(), openList.end(),comp());
std::cout << "Open: ";
for (size_t i = 0; i < openList.size(); i++)
{
std::cout << openList[i]->vertex << " ";
}
std::cout << std::endl;
std::cout << "end" << std::endl;
std::cout << std::endl;
}
}
void printPath(Node* node)
{
std::cout << std::endl;
if (node->parent != NULL)
printPath(node->parent);
std::cout << node->vertex << " ";
}
int isContains(std::vector<Node*>* Nodelist,int vertex)
{
for (int i = 0; i < Nodelist->size(); i++)
{
if (Nodelist->at(i)->vertex== vertex)
{
return i;
}
}
return -1;
}
int main()
{
//{Node,Node,G,H}
//Firt Node
//second Node
//G is the movement cost to move from the starting point to a given square on the grid
// following the path generated to get there
//H is the estimated movement cost to move from that given square on the grid to the final destination
std::vector<Edge>edges =
{
{0,1,5,17},
{0,2,5,13},
{1,0,5,16},
{1,3,3,16},
{1,2,4,13},
{2,0,5,16},
{2,1,4,17},
{2,3,7,16},
{2,4,7,16},
{2,7,8,11},
{3,2,7,13},
{3,7,11,11},
{3,10,16,4},
{3,11,13,7},
{3,12,14,10},
{4,2,7,13},
{4,5,4,20},
{4,7,5,11},
{5,4,4,16},
{5,6,9,17},
{6,5,9,20},
{6,13,12,7},
{7,3,11,16},
{7,4,5,16},
{7,8,3,10},
{8,7,3,11},
{8,9,4,8},
{9,8,4,10},
{9,13,3,7},
{9,15,8,0},
{10,3,16,16},
{10,11,5,7},
{10,13,7,7},
{10,15,4,0},
{11,3,13,16},
{11,10,5,4},
{11,12,9,10},
{11,14,4,5},
{12,3,14,16},
{12,11,9,7},
{12,14,5,5},
{13,9,3,8},
{13,10,7,4},
{13,15,7,0},
{14,11,4,7},
{14,12,5,10},
{15,9,8,8},
{15,10,4,4},
{15,13,7,7},
};
int n = edges.size();
Graph grap(edges, n);
//std::cout << h << std::endl;
Node* start = new Node(0);
Node* end = new Node(15);
findShortestPath(grap, start, end, n);
//Astar astar;
//Node* startPos = new Node(5, 1);
//Node* endPos = new Node(1, 8);
//astar.printMap();
//astar.search(startPos, endPos);
//cout << endl;
//astar.printMap();
system("pause");
return 0;
}

Answer 1

Your program doesn't find the shortest path. It gives the wrong output. (you're on the right track though)
I will assume you are trying to find the shortest path by using BFS. Let's take a look at line 113:
sort(openList.begin(), openList.end(),comp());
Here you're sorting your BFS queue (vector in your case) and thus destroying the right order.
Delete that line.
Congrats, now your program finds the shortest path!
Next, as I understand, for each node you branch into, you remember which node you came from in order to backtrack the path once you reach the destination or final node.
In line 102:
Node* newNode = new Node(vertex, currentg, h, f,node->parent);
you are assigning the new node's grandparent instead of parent. Change that line to
Node* newNode = new Node(vertex, currentg, h, f,node);
Now your printPath function works properly and prints the right path. (just add the target node)
Anyways, your code has a lot of space for improvements. Check out other implementations online and try to see if you can code it as short and clean for practice. Good luck!