I have managed to produce an A* search using c++ (all though it can probably be done much more efficiently) it does it fact go from the start [x][y] to the goal[x][y] and terminates.
Th problem i am having is when i want to print this path in terms of which coordiantes were following i try to do this by following pointers to each Nodes parent and keep doing thing untill the pointer == NULL.
The problem is when i execute my print method it constantly prints the coordinates of the final state, rather than following the pointer to it's parent untill there is no parent (back at the root Node)
Node.h
class Node{
private:
int xCoord;
int yCoord;
int value;
double fCost;
double gCost;
double hCost;
Node* parent;
public:
Node();
Node(int x, int y, int value, int cost, Node* parent);
void setParent(Node* parent);
int getX();
int getY();
int getValue();
double getHCost();
double getFCost();
double getGCost();
Node* getParent();
void setHCost(double hCost);
};
struct NodeComparator {
bool operator()(Node& first, Node& second) {
return (first.getFCost() < second.getFCost());
}
};
Node.cpp
Node::Node(){
this->xCoord = 0;
this->yCoord = 0;
this->value = 0;
this->parent = NULL;
this->fCost = 0;
this->gCost = 0;
this->hCost = 0.0;
}
Node::Node(int _x, int _y, int _value, int cost, Node* parent){
this->xCoord = _x;
this->yCoord = _y;
this->value = _value;
this->gCost = cost;
this->parent = parent;
this->hCost = 0.0;
this->fCost = 0;
}
void Node::setParent(Node* par){
this->parent = par;
}
int Node::getX(){
return xCoord;
}
int Node::getY(){
return yCoord;
}
int Node::getValue(){
return value;
}
double Node::getGCost(){
return gCost;
}
double Node::getFCost(){
return gCost + hCost;
}
double Node::getHCost(){
return hCost;
}
Node* Node::getParent(){
return parent;
}
void Node::setHCost(double cost){
this->hCost = cost;
}
The main.cpp file where the actual search takes place:
int main(){
int map[20][20] = {{0,0,0,0,0,0,0,0,0,0,0,1,1,1,0,0,0,0,0,2},
{0,0,0,0,0,0,0,0,0,0,0,1,1,1,0,0,0,0,0,0},
{0,0,0,0,0,0,0,0,0,0,0,1,1,1,0,0,0,0,0,0},
{0,0,0,0,0,1,1,1,0,0,0,0,1,0,0,0,0,0,0,0},
{0,0,0,0,0,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0},
{0,0,0,0,0,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0},
{0,0,0,0,0,1,1,1,1,1,0,0,0,0,0,0,1,0,0,0},
{0,0,0,0,0,1,1,1,1,1,0,0,0,0,0,0,1,0,0,0},
{0,0,0,0,0,1,1,1,1,1,0,0,0,0,0,0,1,0,0,0},
{0,0,3,0,0,1,1,1,1,1,1,1,0,0,0,0,1,0,0,0},
{0,0,0,0,0,1,1,1,1,1,1,0,0,0,0,1,1,0,0,0},
{0,0,0,0,0,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0},
{0,0,0,0,0,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0},
{0,0,0,1,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0},
{0,0,0,1,1,1,1,1,1,1,0,0,0,1,0,0,0,0,0,0},
{0,0,0,1,1,1,1,1,1,1,0,0,0,1,0,0,0,0,0,0},
{0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0},
{0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0},
{0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0},
{0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0}};
using namespace std;
list<Node> openList;
vector<Node> closedList;
Node end;
Node start = initiateStart(map);
openList.push_front(start);
cout <<"Start index: x " << start.getX() << " y " <<start.getY() << endl;
while (!openList.empty()) {
Node best = openList.front();
openList.pop_front();
if(!checkInClosedList(closedList, best.getX(), best.getY())){
calcManhattanDistance(best, map);
if(best.getValue() == 3){
end = best;
cout <<"end index: x " << end.getX() << " y " <<end.getY() << endl;
checkPath(&end, map);
exit(1);
}
if(map[best.getX()][best.getY()-1] != 1 && best.getY() - 1 > -1){
if(placeInOpen(openList,best.getX(), best.getY() - 1)){
openList.push_front(generateLeftChild(best, map));
}
}
//to the right
if(map[best.getX()][best.getY()+1] != 1 && best.getY() + 1 < 20){
if(placeInOpen(openList,best.getX(), best.getY() + 1)){
openList.push_front(generateRightChild(best, map));
}
}
//above
if(map[best.getX()-1][best.getY()] != 1 && best.getX() - 1 > -1){
if(placeInOpen(openList,best.getX()-1, best.getY())){
openList.push_front(generateAboveChild(best, map));
}
}
//below
if(map[best.getX()+1][best.getY()] != 1 && best.getX() + 1 < 20){
if(placeInOpen(openList,best.getX()+1, best.getY())){
openList.push_front(generateBelowChild(best, map));
}
}
closedList.push_back(best);
}
openList.sort(NodeComparator());
}
return 0;
}
Node initiateStart(int m[20][20]){
Node start;
for(int i = 0; i < 20; i++){
for(int j = 0; j < 20; j++){
if(m[i][j] == 2){
start = Node(i, j, m[i][j], 0, NULL);
}
}
}
return start;
}
Node generateLeftChild(Node parent, int m[20][20]){
Node child;
child = Node(parent.getX(), parent.getY() - 1, m[parent.getX()][parent.getY() - 1],
parent.getGCost() + 1, &parent);
calcManhattanDistance(child, m);
return child;
}
Node generateRightChild(Node parent, int m[20][20]){
Node child;
child = Node(parent.getX() , parent.getY() + 1, m[parent.getX()][parent.getY() + 1],parent.getGCost() + 1, &parent);
calcManhattanDistance(child, m);
return child;
}
Node generateAboveChild(Node parent, int m[20][20]){
Node child;
child = Node(parent.getX() - 1, parent.getY(), m[parent.getX() - 1][parent.getY()],
parent.getGCost() + 1, &parent);
std::cout << "parent for above child X: " << child.getParent()->getX() <<" Y: "
<< child.getParent()->getY() << std::endl;
calcManhattanDistance(child, m);
return child;
}
Node generateBelowChild(Node parent, int m[20][20]){
Node child;
child = Node(parent.getX() + 1, parent.getY(), m[parent.getX() + 1][parent.getY()],
parent.getGCost() + 1, &parent);
calcManhattanDistance(child, m);
return child;
}
void calcManhattanDistance(Node node, int m[20][20]){
int tempX;
int tempY;
double manhattanDistance;
int differenceX;
int differenceY;
//std::cout << "node x: " << node->getX() << " node y: " << node->getY() << std::endl;
for(int i = 0; i < 20; i++){
for(int j = 0; j < 20; j++){
if(m[i][j] == 3){
tempX = i;
tempY = j;
}
}
}
//sum of term difference, none of these can be negative hense the std::abs
differenceX = tempX - node.getX();
differenceY = tempY - node.getY();
manhattanDistance = std::abs(differenceX) + std::abs(differenceY);
//std::cout << "Manhattan distance: " << manhattanDistance << std::endl;
node.setHCost(manhattanDistance);
}
bool checkInClosedList(std::vector<Node>& v,int x, int y){
for (std::vector<Node>::iterator iter = v.begin(); iter != v.end(); ++iter) {
if(iter->getX() == x && iter->getY() == y){
return true;
}
}
return false;
}
bool placeInOpen(std::list<Node>& v,int x, int y){
for (std::list<Node>::iterator iter = v.begin(); iter != v.end(); ++iter) {
if(iter->getX() == x && iter->getY() == y){
return false;
}
}
return true;
}
void checkPath(Node *end, int m[20][20]){
int tempX, tempY;
Node *temp = end;
while(temp != NULL){
tempX = temp->getX();
tempY = temp->getY();
std:: cout << tempX << " " << tempY << std::endl;
temp = temp->getParent();
}
printMap(m);
}
void printMap(int m[20][20]){
std::cout << "printy mcprint" << std::endl;
for(int i = 0; i< 20; i++){
for(int j = 0; j< 20; j++){
std::cout << m[i][j];
}
std::cout<<std::endl;
}
}
Sorry for the large code dump, but i assume you will want to see everything just incase i am setting a parent wrong somewhere!
I added a print statement to test how i add the parents, you can see this in the generateAboveChild function, the program produce this output:
Start index: x 0 y 19
parent for above child X: 1 Y: 19
parent for above child X: 2 Y: 19
parent for above child X: 3 Y: 19
parent for above child X: 2 Y: 18
parent for above child X: 1 Y: 17
parent for above child X: 4 Y: 19
parent for above child X: 5 Y: 19
parent for above child X: 4 Y: 18
parent for above child X: 3 Y: 17
parent for above child X: 2 Y: 16
parent for above child X: 1 Y: 15
parent for above child X: 1 Y: 14
parent for above child X: 6 Y: 19
parent for above child X: 7 Y: 19
parent for above child X: 6 Y: 18
parent for above child X: 5 Y: 17
parent for above child X: 4 Y: 16
parent for above child X: 3 Y: 15
parent for above child X: 2 Y: 14
parent for above child X: 3 Y: 14
parent for above child X: 8 Y: 19
parent for above child X: 9 Y: 19
parent for above child X: 8 Y: 18
parent for above child X: 7 Y: 17
parent for above child X: 5 Y: 15
parent for above child X: 4 Y: 14
parent for above child X: 4 Y: 13
parent for above child X: 6 Y: 15
parent for above child X: 8 Y: 17
parent for above child X: 10 Y: 19
parent for above child X: 11 Y: 19
parent for above child X: 10 Y: 18
parent for above child X: 9 Y: 17
parent for above child X: 7 Y: 15
parent for above child X: 6 Y: 14
parent for above child X: 5 Y: 13
parent for above child X: 4 Y: 11
parent for above child X: 8 Y: 15
parent for above child X: 10 Y: 17
parent for above child X: 12 Y: 19
parent for above child X: 13 Y: 19
parent for above child X: 12 Y: 18
parent for above child X: 11 Y: 17
parent for above child X: 9 Y: 15
parent for above child X: 8 Y: 14
parent for above child X: 7 Y: 13
parent for above child X: 6 Y: 12
parent for above child X: 5 Y: 11
parent for above child X: 4 Y: 9
parent for above child X: 3 Y: 10
parent for above child X: 14 Y: 19
parent for above child X: 15 Y: 19
parent for above child X: 14 Y: 18
parent for above child X: 13 Y: 17
parent for above child X: 12 Y: 16
parent for above child X: 10 Y: 14
parent for above child X: 9 Y: 13
parent for above child X: 8 Y: 12
parent for above child X: 7 Y: 11
parent for above child X: 6 Y: 10
parent for above child X: 2 Y: 10
parent for above child X: 5 Y: 9
parent for above child X: 3 Y: 8
parent for above child X: 1 Y: 10
parent for above child X: 7 Y: 10
parent for above child X: 16 Y: 19
parent for above child X: 17 Y: 19
parent for above child X: 16 Y: 18
parent for above child X: 15 Y: 17
parent for above child X: 14 Y: 16
parent for above child X: 13 Y: 15
parent for above child X: 12 Y: 14
parent for above child X: 11 Y: 13
parent for above child X: 10 Y: 12
parent for above child X: 8 Y: 10
parent for above child X: 2 Y: 7
parent for above child X: 1 Y: 8
parent for above child X: 18 Y: 19
parent for above child X: 19 Y: 19
parent for above child X: 18 Y: 18
parent for above child X: 17 Y: 17
parent for above child X: 16 Y: 16
parent for above child X: 15 Y: 15
parent for above child X: 14 Y: 14
parent for above child X: 13 Y: 13
parent for above child X: 12 Y: 12
parent for above child X: 11 Y: 11
parent for above child X: 2 Y: 5
parent for above child X: 1 Y: 6
parent for above child X: 15 Y: 14
parent for above child X: 19 Y: 17
parent for above child X: 18 Y: 16
parent for above child X: 17 Y: 15
parent for above child X: 16 Y: 14
parent for above child X: 14 Y: 12
parent for above child X: 13 Y: 11
parent for above child X: 12 Y: 10
parent for above child X: 3 Y: 4
parent for above child X: 2 Y: 3
parent for above child X: 1 Y: 4
parent for above child X: 13 Y: 10
parent for above child X: 15 Y: 12
parent for above child X: 17 Y: 14
parent for above child X: 19 Y: 15
parent for above child X: 18 Y: 14
parent for above child X: 16 Y: 12
parent for above child X: 15 Y: 11
parent for above child X: 14 Y: 10
parent for above child X: 4 Y: 4
parent for above child X: 5 Y: 4
parent for above child X: 4 Y: 3
parent for above child X: 3 Y: 2
parent for above child X: 2 Y: 1
parent for above child X: 1 Y: 2
parent for above child X: 15 Y: 10
parent for above child X: 17 Y: 12
parent for above child X: 19 Y: 14
parent for above child X: 18 Y: 12
parent for above child X: 17 Y: 11
parent for above child X: 16 Y: 10
parent for above child X: 6 Y: 4
parent for above child X: 7 Y: 4
parent for above child X: 6 Y: 3
parent for above child X: 5 Y: 2
parent for above child X: 4 Y: 1
parent for above child X: 3 Y: 0
parent for above child X: 1 Y: 0
parent for above child X: 19 Y: 12
parent for above child X: 19 Y: 11
parent for above child X: 18 Y: 10
parent for above child X: 17 Y: 9
parent for above child X: 4 Y: 0
parent for above child X: 8 Y: 4
parent for above child X: 9 Y: 4
parent for above child X: 8 Y: 3
parent for above child X: 7 Y: 2
parent for above child X: 6 Y: 1
parent for above child X: 5 Y: 0
parent for above child X: 19 Y: 9
parent for above child X: 18 Y: 8
parent for above child X: 17 Y: 7
parent for above child X: 6 Y: 0
parent for above child X: 10 Y: 4
parent for above child X: 11 Y: 4
parent for above child X: 10 Y: 3
end index: x 9 y 2
But, when i uncomment the line which calls checkPath(&end, map);(this is near the top of main.cpp under the if conditon if(best.getValue() == 3) the i get this output: ( i have removed the cout print for the "parent for above child")
Start index: x 0 y 19
9 2
9 2
9 2
9 2
9 2
9 2
9 2
and these 9 2 prints continue forever, i assume the problem is in the printPath function ive tried debugging it but i cant see what i am doing wrong.
Cheers,
Chris.
I changed my implementation of how Node are handled and stored Node* rather than Node Objects in my list and vector as well as using the new keyword on creation. This resolved my problem my A* now works.