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