Help please, I am attempting to create an A* algorithm to do some pathfinding in C++.

The function I use is:

pathfinder.aStar(400, 300, 25, 25);

Which is a fairly simple diagonal movement.

My map is divided in 64x64 nodes. And for some reason, the pathfinding follows the following path of nodes:

5,5

6,3

6,5

7,3

7,4

7,5

38,28

38,29

And crashes as only 39x29 nodes were created and it wants to go onto (39,30)

My code is as follows:

std::vector<Position*> Pathfinder::aStar(int x1, int y1, int x2, int y2) { start = getNodeFromCoord(x1, y1); endNode = getNodeFromCoord(x2, y2); n = 0; openList.push_back(start); start->opened = true; while (n == 0 || (current != endNode && n < 50)) { //Look for the smallest F value in the openList and make it the current point for (i = openList.begin(); i != openList.end(); ++ i) { if (i == openList.begin() || (*i)->getFScore() <= current->getFScore()) { current = (*i); } } if ( current == endNode ) { break; } //Change from open to closed list openList.remove(current); current->opened = false; closedList.push_back(current); current->closed = true; // Get all current's adjacent walkable points for (int x = -1; x < 2; x ++) { for (int y = -1; y < 2; y ++) { // If it's current point then pass if (x == 0 && y == 0) { continue; } // Get this point child = getPoint(current->getX() + x, current->getY() + y); std::cout << current->getX() + x << " --- " << current->getY() + y << std::endl; std::cout<<child->getX() << " " << child->getY() << std::endl << std::endl; // If it's closed or not walkable then pass if (child->closed || !child->walkable) { continue; } // If we are at a corner if (x != 0 && y != 0) { // if the next horizontal point is not walkable or in the closed list then pass if (!nodeIsWalkable(current->getX(), current->getY() + y) || getPoint(current->getX(), current->getY() + y)->closed) { continue; } // if the next vertical point is not walkable or in the closed list then pass if (!nodeIsWalkable(current->getX() + x, current->getY()) || getPoint(current->getX() + x, current->getY())->closed) { continue; } } // If it's already in the openList if (child->opened) { // If it has a worse g score than the one that pass through the current point // then its path is improved when it's parent is the current point if (child->getGScore() > child->getGScore(current)) { // Change its parent and g score child->setParentNode(current); child->computeScores(endNode); } } else { // Add it to the openList with current point as parent openList.push_back(child); child->opened = true; // Compute it's g, h and f score child->setParentNode(current); child->computeScores(endNode); } } } n++; } //Reset for (i = openList.begin(); i != openList.end(); ++ i) { (*i)->opened = false; } for (i = closedList.begin(); i != closedList.end(); ++ i) { (*i)->closed = false; } while (current->hasParent() && current != start) { path.push_back(current->getPosition()); current = current->getParentNode(); n ++; } return path; }

Any ideas? Thank you