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;
pq[pqi].push(n2); // add the better node instead
}
}
}
}
// 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
}
