# My a* pathfinding is very slow... (c++)

## Recommended Posts

VERY slow (unusable) right now.

Profiling it I get:

20x20 (400 nodes) = less than sec
30x30 (900 nodes) = 2s
40x40 (1600 nodes) = 5s
50x50 (2500 nodes) = 20s
60x60 (3600 nodes) = 40s

Can someone see what I did wrong? I've tried to follow the pseudo-code of what I find on the internet. I use simple Manhattan distance for H. Maybe I use priority queue wrongly?

This is the path-find function as it looks now:

int SimplePather::FindPath(const int nStartX, const int nStartY, const int nTargetX, const int nTargetY,
const unsigned char* pMap, const int nMapWidth, const int nMapHeight, int* pOutBuffer, const int nOutBufferSize)
{
static std::priority_queue<simplePatherNode> pq[2]; // list of open nodes. Open nodes are not yet tried
static int pqi; // pq index (0 or 1)
simplePatherNode n2(0,0,0,0); // used when checking the neighbors of n
static int i, j, x, y, x2, y2;
pqi=0;
int pathLength=0;

int mapSize = nMapWidth * nMapHeight;
std::vector<int> closedMap(mapSize, 0); // closed nodes have been checked
std::vector<int> openMap(mapSize, 0); // open nodes have not been checked
std::vector<int> dirMap(mapSize);

// create the start node and push into map of open nodes
simplePatherNode n(nStartX, nStartY, 0, 0);
n.setF(nTargetX, nTargetY);
pq[pqi].push(n);
openMap[nStartX+nStartY*nMapWidth]=n.getF(); // mark it on the open nodes map

// Main path-find loop
while(!pq[pqi].empty())
{
// get the current node w/ the highest priority
// from the list of open nodes
n.setAll(pq[pqi].top().getxPos(), pq[pqi].top().getyPos(), pq[pqi].top().getG(), pq[pqi].top().getF());

x=n.getxPos();
y=n.getyPos();

pq[pqi].pop(); // remove the node from the open list
openMap[x+y*nMapWidth]=0;
// mark it on the closed nodes map
closedMap[x+y*nMapWidth]=1;

// the goal state is reached = finish searching
if(x==nTargetX && y==nTargetY)
{
//hold the temporary node indexes
std::vector<int> tempNodes(nOutBufferSize);

// generate the path from target to start
// by following the directions back to the start
while(!(x==nStartX&&y==nStartY))
{
// not enough buffer to write our path = fail path-finding
if(pathLength>=nOutBufferSize)
return -1;

j=dirMap[x+y*nMapWidth];
tempNodes[pathLength]=x+y*nMapWidth;
pathLength++;

x+=dx[j];
y+=dy[j];
}

//invert the buffer since we want to return a buffer with nodes from start to target
for(int i=0;i<pathLength;i++){
pOutBuffer[i]=tempNodes[pathLength-1-i];
}

// garbage collection
for(int i=0;i<2;i++)
while(!pq[i].empty())
pq[i].pop();

return pathLength;
}

// If we are not at the target location, check neighbors in all possible directions
for(i=0;i<directions;i++)
{
x2=x+dx[i]; y2=y+dy[i];

//check if node is outside map-scope, impassable "terrain type" or already checked
if(!(x2<0 || x2>nMapWidth-1 || y2<0 || y2>nMapHeight-1 || pMap[x2+y2*nMapWidth]==IMPASSABLE_NODE_ID    || closedMap[x2+y2*nMapWidth]==1))
{
// generate a neighbor node
n2.setAll( x2, y2, n.getG(), n.getF());
n2.increaseG(i, diagonalAllowed);
n2.setF(nTargetX, nTargetY);

// if it is not in the open map then add into that
if(openMap[x2+y2*nMapWidth]==0)
{
openMap[x2+y2*nMapWidth]=n2.getF();
pq[pqi].push(n2);
// mark its parent node direction
dirMap[x2+y2*nMapWidth]=(i+directions/2)%directions;
}
else if(openMap[x2+y2*nMapWidth]>n2.getF())
{
// update the priority info
openMap[x2+y2*nMapWidth]=n2.getF();
// update the parent direction info
dirMap[x2+y2*nMapWidth]=(i+directions/2)%directions;

// replace the node
// by emptying one pq to the other one
// except the node to be replaced will be ignored
// and the new node will be pushed in instead
while(!(pq[pqi].top().getxPos()==x2 &&
pq[pqi].top().getyPos()==y2))
{
pq[1-pqi].push(pq[pqi].top());
pq[pqi].pop();
}
pq[pqi].pop(); // remove the wanted node

// empty the larger size pq to the smaller one
if(pq[pqi].size()>pq[1-pqi].size())
pqi=1-pqi;
while(!pq[pqi].empty())
{
pq[1-pqi].push(pq[pqi].top());
pq[pqi].pop();
}
pqi=1-pqi;
}
}
}
}
// garbage collection
for(int i=0;i<2;i++)
while(!pq[i].empty())
pq[i].pop();

return -1; // no route was found (couldn't reach the target node), indicate this with -1
}

##### Share on other sites

Have you tried running your code with a profiler attached to find any glaring hotspots? Are you compiling your code in release mode?

##### Share on other sites
• Why do you use two priority queues, and do complex maintenance to them? A queue backed by a std::vector grows automatically as much as your graph requires.
• What is a simplePatherNode exactly, and how are they compared? Post code.
I would expect it to contain a "primary key" (in this case, x and y coordinates are ok) and one path length, but it has at least four fields. What are F and G?
• If you store, for each map cell, the cost of reaching it from the starting location (defaulting to infinitely high, and repeatedly lowered when search finds the first path to that node and then shorter paths) you don't need dirMap: when you reach the destination and finish the open queue, go from the far end of the path to the neighbour that is closest to the starting node, breaking ties as you please.

##### Share on other sites

That first 'n.setF(nTargetX, nTargetY);' line looks wrong. That first node can't have an F value until you've calculated the G value (which is zero) and the H value (which is the estimate to the target). But, if setF is actually setting the H value and then calculating F, fine, but it's a misleadingly-named function.

The other culprit might be that you're sorting the priority queue wrongly. But we don't have that code here.

The obvious debugging step for you should have been to log each node that gets pulled off the open queue, so you can see if it's doing what you expect or not. Run it on a trivial 4x4 grid with logging and you should be able to draw it on graph paper and see whether your expectations are being met or not.

##### Share on other sites
Posted (edited)

Not sure if this is related to the performance, but one thing I noticed is creation of nodes as local stack variables and adding them to the priority queue. The PQ stores references to objects, not copies of the object. So, if you insert multiple copies of n2 (containing different values at each insert) into a PQ, the PQ would just contain multiple references to the same n2 object (or multiple identical objects, from the PQ's view), which I suspect interferes with the behaviour of the PQ. Can you test this by creating node objects on the heap?

Edited by lisphacker

##### Share on other sites

It should be fine for the priority queue to contain actual nodes (if they're small), and that should make for reasonably fast comparisons, but this code is very confusing in having an 'openMap' that is actually a vector that in turn actually wants to be a bitset, and in having other properties of the node stored outside the node (e.g. in dirmap). There's also no reason for 2 priority queues. Or for variables to be static.

##### Share on other sites
Posted (edited)
Usually slow A* is due to poor data structure choice, but your data structures look good to me.

The only thing that seems like it might be problematic is your update-cost-and-fix-the-priority-queue code. How often is that being hit? In the A* pathfinder I wrote at work, I found that reprioritization after updating a node cost only occurs 0.2% of the time, so I didn't bother optimizing the operation to rebalance the binary heap. If it's happening all the time for you, then that could lead to a lot of performance degradation.

I would definitely run a profiler on it, though.

Some discussion about fixing the order in a priority_queue to change costs here: http://stackoverflow.com/questions/5810190/how-to-tell-a-stdpriority-queue-to-refresh-its-ordering

I also agree with lisphacker that the priority queue elements themselves need to be as small as possible to reduce memory access when the queue is modified. Ideally I try to keep just two values - the sort key and an index/pointer to the larger node struct/class. It looks like your priority queue is keeping a copy of X,Y,G, and F. All you should need are the F and a pointer/index to the X,Y,G (which can then remain stationary in memory).

I leave the actual cost in the priority queue itself so that modifying the queue is slightly more cache-friendly.

The last thing to note is that if you are running in debug mode, I've noticed that std's binary heap code does a LOT of extra checks which slow things down. I haven't used priority_queue, but it could be doing it as well. Definitely make sure you're testing with a fully optimized build. Edited by Nypyren

##### Share on other sites

The last thing to note is that if you are running in debug mode, I've noticed that std's binary heap code does a LOT of extra checks which slow things down. I haven't used priority_queue, but it could be doing it as well. Definitely make sure you're testing with a fully optimized build.

If you're not on Windows using the Microsoft compiler, this isn't a thing.  If you are in that environment, it's a Very Serious thing (ie. gives you lots of wonderful protection at the cost of really slow and inefficient implementation in a totally non-standards-conforming way that might, for example, turn a guaranteed O(log n) operation into an O(n²) operation with a large constant).

##### Share on other sites

Seriously, stop using static. Especially around pathfinding. Even if it doesn't cause a bug it's already costing you performance and it means you can't safely thread the function.

The PQ stores references to objects, not copies of the object.

This is not correct. STL containers in general cannot contain references. std::priority_queue is not an exception to this.

##### Share on other sites

Even if it doesn't cause a bug it's already costing you performance

Why does static incur a performance hit?

##### Share on other sites

Usually slow A* is due to poor data structure choice

I'd actually say that - on these forums, at least - slow A* is usually because the algorithm is implemented wrongly.

It doesn't help that few game developers understand state space search, and so they do things like re-opening nodes from the closed list, which makes no sense.

##### Share on other sites
Posted (edited)

I cleaned it up and removed the double PQ, and it's quick now- However in some cases it doesn't path find completely straight, it adds a little loop even when not needed. See below (first is fine, but when I path find with the target one step further away it adds the loop close to the start. What in my code can cause this?

Belwo is my full code (but the error should be in simplePather.cpp i think). I am very appreciative for any more improvement suggestions (both when it comes to cleanliness and optimization).

simplePather.ccp

#include "simplePather.h"
#include <vector>

SimplePather::SimplePather(){
setDiagonalMode( false); // by default, diagonal moves should not be allowed
}

void SimplePather::setDiagonalMode(bool allowed){
diagonalAllowed = allowed;

if(diagonalAllowed){
dx[0]=1; dy[0]=0;
dx[1]=1; dy[1]=1;
dx[2]=0; dy[2]=1;
dx[3]=-1; dy[3]=1;
dx[4]=-1; dy[4]=0;
dx[5]=-1; dy[5]=-1;
dx[6]=0; dy[6]=-1;
dx[7]=1; dy[7]=-1;
directions=8;
}
else{
dx[0]=1; dy[0]=0;
dx[1]=0; dy[1]=1;
dx[2]=-1; dy[2]=0;
dx[3]=0; dy[3]=-1;
directions=4;
}
}

int SimplePather::FindPath(const int nStartX, const int nStartY, const int nTargetX, const int nTargetY,
const unsigned char* pMap, const int nMapWidth, const int nMapHeight, int* pOutBuffer, const int nOutBufferSize)
{
std::priority_queue<simplePatherNode> pq; // list of open nodes. Open nodes are not yet tried
simplePatherNode n2(0,0,0,0); // used when checking the neighbors of n
int i, j, x, y, x2, y2;

int mapSize = nMapWidth * nMapHeight;
std::vector<int> closedMap(mapSize, 0); // closed nodes have been checked
std::vector<int> openMap(mapSize, 0); // open nodes have not been checked
std::vector<int> dirMap(mapSize);

// create the start node and push into map of open nodes
simplePatherNode n(nStartX, nStartY, 0, 0);
n.setF(nTargetX, nTargetY);
pq.push(n);
openMap[nStartX+nStartY*nMapWidth]=n.getF(); // mark it on the open nodes map

// Main path-find loop
while(!pq.empty())
{
// get the current node w/ the highest priority
// from the list of open nodes
n.setAll(pq.top().getxPos(), pq.top().getyPos(), pq.top().getG(), pq.top().getF());

x=n.getxPos();
y=n.getyPos();

// the goal state is reached = finish searching
if(x==nTargetX && y==nTargetY)
{
//hold the temporary node indexes
std::vector<int> tempNodes(nOutBufferSize);
int pathLength=0;

// generate the path from target to start
// by following the directions back to the start
while(!(x==nStartX&&y==nStartY))
{
// not enough buffer to write our path = fail path-finding
if(pathLength>=nOutBufferSize)
return -1;

j=dirMap[x+y*nMapWidth];
tempNodes[pathLength]=x+y*nMapWidth;
pathLength++;

x+=dx[j];
y+=dy[j];
}

//invert the buffer since we want to return a buffer with nodes from start to target
for(int i=0;i<pathLength;i++){
pOutBuffer[i]=tempNodes[pathLength-1-i];
}

// garbage collection
while(!pq.empty())
pq.pop();

return pathLength;
}

// If we are not at the target location, check neighbors in all possible directions
pq.pop(); // remove the node from the open list
openMap[x+y*nMapWidth]=0;
closedMap[x+y*nMapWidth]=1; // mark it on the closed nodes map

for(i=0;i<directions;i++)
{
x2=x+dx[i]; y2=y+dy[i]; // set position of next node to check

//check if already checked, is outside map-scope, impassable "terrain type"/obstacle
if(!(x2<0 || x2>nMapWidth-1 || y2<0 || y2>nMapHeight-1 || pMap[x2+y2*nMapWidth]==IMPASSABLE_NODE_TYPE || closedMap[x2+y2*nMapWidth]==1 ))
{
// set a neighbor node
n2.setAll( x2, y2, n.getG(), n.getF());
n2.increaseG(i, diagonalAllowed);
n2.setF(nTargetX, nTargetY);

// if it is not in the open map then add into that
if(openMap[x2+y2*nMapWidth]==0)
{
openMap[x2+y2*nMapWidth]=n2.getF();
pq.push(n2);
dirMap[x2+y2*nMapWidth]=(i+directions/2)%directions; // mark its parent node direction
}
else if(openMap[x2+y2*nMapWidth]<n2.getF())
{
openMap[x2+y2*nMapWidth]=n2.getF(); // update the priority info
dirMap[x2+y2*nMapWidth]=(i+directions/2)%directions; // update the parent direction info
}
}
}
}
// garbage collection
while(!pq.empty())
pq.pop();

return -1; // no route was found (couldn't reach the target node), indicate this with -1
}

// wrapper
int FindPath(const int nStartX, const int nStartY, const int nTargetX, const int nTargetY,
const unsigned char* pMap, const int nMapWidth, const int nMapHeight, int* pOutBuffer, const int nOutBufferSize)
{

SimplePather pather;
return pather.FindPath(nStartX, nStartY, nTargetX, nTargetY, pMap, nMapWidth, nMapHeight, pOutBuffer, nOutBufferSize);
};



-

simplePather.h

#pragma once

#include <queue>
#include "simplePatherNode.h"

/**
A class using a simple A-star implementation
that does not allow diagonal movement (by default)
Note that in this version all nodes (unless impassable) are considered equally easy to pass through (no variable "terrain cost")
*/
class SimplePather{

static const int IMPASSABLE_NODE_TYPE = 0;	// value of map-tile if impassable

int directions; // number of possible directions to move (from a node). Will be 4 or 8 depending on diagonalMode
int dx[8];
int dy[8];
bool diagonalAllowed;

public:

SimplePather();

/**
Sets if diagonal moves should be allowed or not. Also sets the concerned variables (dx, dy)

@param allowed If diagonal moves should be allowed
@return void
*/
void setDiagonalMode(bool allowed);

/**
Finds a path from start to target or returns -1. Populates a buffer to hold indexes of the nodes from start-node to target-node excluding the start-node itself.

@param nStartX The starting x-pos
@param nStartY The starting y-pos
@param nTargetX The target x-pos
@param nTargetY The target y-pos
@param pMap The array to hold the map data (all nodes). The grid should be in row-major order.
@param nMapWidth The width (x-size) of the map to search
@param nMapHeight The height (y-size) of the map to search
@param pOutBuffer The buffer to hold the indexes to the nodes in the found path
@param nOutBufferSize The buffer size
@return The number of nodes in the found path, or -1 if no found path was found or the buffer size was to small for the found path
*/
int FindPath(const int nStartX, const int nStartY, const int nTargetX, const int nTargetY,
const unsigned char* pMap, const int nMapWidth, const int nMapHeight, int* pOutBuffer, const int nOutBufferSize);

};

//simple wrapper for the simplePather::pathFind
int FindPath(const int nStartX, const int nStartY, const int nTargetX, const int nTargetY,
const unsigned char* pMap, const int nMapWidth, const int nMapHeight, int* pOutBuffer, const int nOutBufferSize);



-

simplePatherNode.h

#pragma once

#include <stdlib.h>

// A node-class to use with simplePather
class simplePatherNode
{
int xPos;
int yPos;
int gValue; // G = total "movement cost" to reach the node from the start
int fValue;  // F = G + H. Smaller F means higher priority

public:
void setAll(int xp, int yp, int gVal, int fVal)
{
xPos=xp;
yPos=yp;
gValue=gVal;
fValue=fVal;
}

simplePatherNode(int xp, int yp, int d, int p)
{
setAll(xp, yp, d, p);
}

int getxPos() const {return xPos;}
int getyPos() const {return yPos;}
int getG() const {return gValue;}
int getF() const {return fValue;}

void setF(const int & xDest, const int & yDest)
{
fValue=gValue+getH(xDest, yDest)*10;
}

// If diagonal movement is allowed, such moves cost 40% extra
// (1.4 is close enough to sqrt(2) which is the length of the diagonal side of a square
// Note! All ground is considered equally easy to travel.
void increaseG(const int & dir, bool diagonalAllowed)
{
if(diagonalAllowed)
gValue+=(dir%2==0?10:14);
else
gValue+=10;
}

// Get the remaining distance (not bothering with obstacles) to the goal
// (known as H-value or heuristic)
const int getH(const int & xDest, const int & yDest) const
{
// Use simple Manhattan distance for h-value
return abs(xDest-xPos)+abs(yDest-yPos);
}

};

// Used to determine priority (in the priority queue)
inline
bool operator<(const simplePatherNode & a, const simplePatherNode & b)
{
return a.getF() > b.getF();
}


It can be called like this:

unsigned char pMap[] = {1, 1, 1, 1, 0, 1, 0, 1, 0, 1, 1, 1};
int pOutBuffer[12];
int len=FindPath(0, 0, 1, 2, pMap, 4, 3, pOutBuffer, 12);

Greatly appreciated guys!
Edited by suliman

##### Share on other sites
Posted (edited)

Try rendering the direction of the path, to confirm how the loop is working. Also consider rendering the contents of your open/closed lists, the f/g/h values, etc. Ideally you could render step by step to see what is happening each time, but that might be tricky. Still, you have all the information that you need already.

In terms of code cleanliness you have a long way to go.

• Don't use a vector of int when you only store 2 values, 0 and 1.
• Don't reuse objects via weird functions like setAll to copy data bit by bit when really you should just be popping the node and using it directly.
• Don't manually push F values into a node - put the G and H values in, and calculate F from those
• move the final path generation to a separate function; there's no need for it to be inline in the main pathfinding loop.
• Wrap the hacky 'x2+y2*nMapWidth' stuff so that you have a functions that take an X and a Y coordinate and do whatever you need to do to the 'maps'
• I've no idea what dirMap[x2+y2*nMapWidth]=(i+directions/2)%directions is doing but it looks like a mess. Make it explicit why you're doing these divisions and modulos on this value.
• don't pass ints by const reference; it does nothing useful.
• drop the 'garbage collection'.
Edited by Kylotan

##### Share on other sites

If I may I'd also recommend you replace your deeply nested indecipherable walls of commented text with appropriately named functions.  This has the advantages of [a] your function name documents what the code it contains does, [2] when you profiler shows which function gobbles the most CPU time, you know which part of your algorithm needs tweaking, and [iii] you don't have to nest code so deeply that a horizontal scrollbar is required to see it all.

Most places in your code that are blocks starting with a comment could probably be refactored into meaningfully-named functions. In the process of doing that you are often presented with opportunities for improving your implementation.

I understand you can hold the code in your head and you understand it.  You wouldn't have gotten where you are today if you weren't clever.  It pays to assume the reader (the person marking the assignment, for example, or you yourself in a few weeks when you're studying for the exam) is a little slow and needs it explained to them like they've never seen your code before.

##### Share on other sites

I cleaned it up and removed the double PQ, and it's quick now- However in some cases it doesn't path find completely straight, it adds a little loop even when not needed. See below (first is fine, but when I path find with the target one step further away it adds the loop close to the start. What in my code can cause this?

Cool bug! I can't pick it out through code inspection, but I can offer a suggestion. Do you know how to use your debugger? Are you familiar with conditional breakpoints? In a case like this, where you have a clearly reproducible problem, you should be able to get right to the problem. Set a breakpoint when you are processing the problematic node. Probably right after:

 x=n.getxPos();
y=n.getyPos(); 

Set the condition such that x and y are the problem node. (23,6), I think. Then if you step through the code it should become apparent what's going wrong. Particularly, I don't think node (23,5) should even get evaluated in A*, but there it is on the path.

Hope that helps,

Geoff

##### Share on other sites

Not related to your current problem (since it doesn't look like you're using diagonals), but your heuristic won't work (i.e. won't be guaranteed to find the shortest path) if diagonalAllowed is true, since it would be overestimating the cost of getting to the goal.

##### Share on other sites

Actually, it might be relevant. An over-estimating heuristic will result in nodes being examined in the wrong order, which can mean the eventual path is not the optimal one.

##### Share on other sites
Posted (edited)

The bug was from comparing the F values with "<" instead of ">"... Now the loops are gone!

Im still not being accepted by the server compiler, it says I get the wrong answer to one of the (unknown) tests, but all maps I try it on works fine, so I might be stuck :(

But I really appreciate the help! My code is much neater now and I learned alot.

Edited by suliman

##### Share on other sites

but all maps I try it on works fine, so I might be stuck

Some suggestions for variations to test... What happens (and what should happen) in the following scenarios:

...if start and end are the same?

...if you cannot reach end from start (i.e. there is no valid path)?

...if end and/or start is inside collision (i.e. invalid map and/or invalid positions)?

...if start and/or end is not inside the map (i.e. invalid positions given for start and/or end)?

...if there is no valid map provided?

##### Share on other sites

Yeah I check for all those cases and still it complains. Rather frustrating.

When start = target i fail the path, but I also tried it with including the goal node (the instructions aren't clear on how that case should be handled). None passes through all the secret tests.

##### Share on other sites
Posted (edited)

I would expect start == goal to return just an empty path (length 0).

Could it be that you're still too slow, and that you're exceeding some time-limit they're enforcing?

What about trying to just generate a bunch of maps, and for those maps generating every single path possible (from all starts go to all ends), seeing if something fails.

EDIT: Something else to consider -- if you reverse the path, do you get the same path-length in all scenarios? If there's a mis-match, then you have a bug.

Edited by Lactose

##### Share on other sites
Posted (edited)

Nope timeout is actually something it tells you. I got that before but not anymore. It's really fast now:)

How would I generate random maps and then see if it fails (when it shouldn't)? Often there is no valid path in a randomly filled map.

All path-length seem to be fine. Here are some tests I made. Still cannot figure out why it tells me "wrong answer"...

Edited by suliman

##### Share on other sites

When I solved this problem I used maps and benchmarks from http://www.movingai.com/benchmarks/ to find any bugs in my implementation. These are nice because you get problems and the correct minimal path length with them.

If there's nothing wrong with the pathfinding, it might be something about the output format, IIRC that was somewhat tricky to get right for all cases. Make sure the output is still correct if you search for multiple paths without any additional cleanup in main().

##### Share on other sites

Can this be related to the "problem":

Changing the wall (obstacle) where it doesnt pass with the end path anyway (but still nodes it considers) slightly changes the end path. It changes between two different path of equal length though so both paths should be fine.

##### Share on other sites
Posted (edited)

I tried to get well into your code but I didn't.

Just a small tip: Neighbourhood operations are frequent. Do not check on the boundaries, add +1 (or more) size to your map. This will save you code and you can render it nicely.

Edited by krogoth.g

## Create an account

Register a new account

• ### Forum Statistics

• Total Topics
628359
• Total Posts
2982259

• 10
• 9
• 13
• 24
• 11