mirror/Aquaria
1
0
Fork 0
mirror of https://github.com/AquariaOSE/Aquaria.git synced 2024-12-25 14:15:46 +00:00
Code Issues Releases Wiki Activity

Replace A* pathfinding with much faster jump point search.

Browse source 
This also fixes entities glithing through walls, as diagonal cracks
were assumed walkable.
This commit is contained in:
fgenesis 2013-12-10 03:47:58 +01:00
parent 890ca90bd4
commit 70286954a0
4 changed files with 53 additions and 1454 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

2
Aquaria/Game.cpp
View file

@ -2157,8 +2157,6 @@ void Game::reconstructGrid(bool force)
}
trimGrid();
dsq->pathFinding.generateZones();
}
void Game::trimGrid()

5
Aquaria/Game.h
View file

@ -1207,8 +1207,9 @@ int Game::getGridRaw(unsigned int x, unsigned int y) const
inline
int Game::getGrid(const TileVector &tile) const
{
if (tile.x < 0 || tile.x >= MAX_GRID || tile.y < 0 || tile.y >= MAX_GRID) return OT_INVISIBLE;
return grid[tile.x][tile.y];
//if (tile.x < 0 || tile.x >= MAX_GRID || tile.y < 0 || tile.y >= MAX_GRID) return OT_INVISIBLE;
//return grid[tile.x][tile.y];
return (unsigned(tile.x) < unsigned(MAX_GRID) && unsigned(tile.y) < unsigned(MAX_GRID)) ? grid[tile.x][tile.y] : OT_INVISIBLE;
}
inline

728
Aquaria/PathFinding.cpp
View file

@ -18,333 +18,37 @@ You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
*/
#include <JPS.h>
#include "PathFinding.h"
#include "DSQ.h"
#include "Game.h"
const int divs = 6;
const int MAX_ZONES=1000;
const int MAX_STEPS = 5000;
const int cutOff = int((divs*divs)*0.75f);
namespace PathFindingGlobals
class SearchGrid
{
// This isn't used by the current code, so I've commented it out to
// save 4MB of RAM. --achurch
//int zones[MAX_ZONES][MAX_ZONES];
MapSearchNode node_goal;
MapSearchNode node_start;
RenderObject *render_object;
bool hate_diagonals;
}
float MapSearchNode::GoalDistanceEstimate( MapSearchNode &nodeGoal )
{
float xd = float( ( (float)x - (float)nodeGoal.x ) );
float yd = float( ( (float)y - (float)nodeGoal.y) );
return ((xd*xd) + (yd*yd));
//return 0;
/*
int r = 10;
float c = 0;
for (int x = -r; x < r; x+=2)
public:
SearchGrid() : game(dsq->game) {}
inline bool operator()(unsigned x, unsigned y) const
{
for (int y = -r; y < r; y+=2)
{
if (dsq->game->getGrid(TileVector(this->x+x, this->y+y)))
{
//c+= r*TILE_SIZE
c++;
}
}
}
return ((xd*xd) + (yd*yd)) + c * (2*TILE_SIZE);
*/
return game->getGrid(TileVector(x, y)) == OT_EMPTY;
}
private:
const Game *game;
};
/*
float xd = float( ( (float)x - (float)nodeGoal.x ) );
float yd = float( ( (float)y - (float)nodeGoal.y) );
int dist = ((xd*xd) + (yd*yd));
return (int(dist/80)*80);
*/
//return ((xd*xd) + (yd*yd));
// + c; //+ c * (2*TILE_SIZE);
}
bool MapSearchNode::IsGoal( MapSearchNode &nodeGoal )
static void generateVectorPath(const JPS::PathVector& rawpath, VectorPath& vp, int offx, int offy)
{
Vector v(x, y);
Vector g(nodeGoal.x, nodeGoal.y);
if (divs > 1)
{
if ((v - g).getSquaredLength2D() <= sqr(divs+1))
{
// HACK: remember this
//debugLog ("really close to the goal!");
return true;
}
}
if( int(x/divs) == int(nodeGoal.x/divs) && int(y/divs) == int(nodeGoal.y/divs))
{
return true;
}
return false;
for(JPS::PathVector::const_iterator it = rawpath.begin(); it != rawpath.end(); ++it)
vp.addPathNode(Vector((it->x*TILE_SIZE)+TILE_SIZE/2+offx, (it->y*TILE_SIZE)+TILE_SIZE/2)+offy, 0);
}
// This generates the successors to the given Node. It uses a helper function called
// AddSuccessor to give the successors to the AStar class. The A* specific initialisation
// is done for each node internally, so here you just set the state information that
// is specific to the application
bool MapSearchNode::GetSuccessors( AStarSearch *astarsearch, MapSearchNode *parent_node )
{
int parent_x = -1;
int parent_y = -1;
if( parent_node )
{
parent_x = parent_node->x;
parent_y = parent_node->y;
}
MapSearchNode NewNode;
int i = divs;
// push each possible move except allowing the search to go backwards
if ((GetMap (x-i, y) <= 0) && !((parent_x == x-i) && (parent_y == y)))
{
NewNode = MapSearchNode( x-i, y );
astarsearch->AddSuccessor( NewNode );
}
if ((GetMap (x, y-i) <= 0) && !((parent_x == x) && (parent_y == y-i)))
{
NewNode = MapSearchNode( x, y-i );
astarsearch->AddSuccessor( NewNode );
}
if ((GetMap (x+i, y) <= 0) && !((parent_x == x+i) && (parent_y == y)))
{
NewNode = MapSearchNode( x+i, y );
astarsearch->AddSuccessor( NewNode );
}
if ((GetMap (x, y+i) <= 0) && !((parent_x == x) && (parent_y == y+i)))
{
NewNode = MapSearchNode( x, y+i );
astarsearch->AddSuccessor( NewNode );
}
if (!PathFindingGlobals::hate_diagonals)
{
if ((GetMap (x-i, y-i) < 1) && !((parent_x == x-i) && (parent_y == y-i)))
{
NewNode = MapSearchNode( x-i, y-i );
astarsearch->AddSuccessor( NewNode );
}
if ((GetMap (x-i, y+i) < 1) && !((parent_x == x-i) && (parent_y == y+i)))
{
NewNode = MapSearchNode( x-i, y+i );
astarsearch->AddSuccessor( NewNode );
}
if ((GetMap (x+i, y+i) <1) && !((parent_x == x+i) && (parent_y == y+i)))
{
NewNode = MapSearchNode( x+i, y+i );
astarsearch->AddSuccessor( NewNode );
}
if ((GetMap (x+i, y-i) < 1) && !((parent_x == x+i) && (parent_y == y-i)))
{
NewNode = MapSearchNode( x+i, y-i );
astarsearch->AddSuccessor( NewNode );
}
}
return true;
}
// given this node, what does it cost to move to successor. In the case
// of our map the answer is the map terrain value at this node since that is
// conceptually where we're moving
float MapSearchNode::GetCost( MapSearchNode &successor )
{
float cost = 1;
/*
if (PathFindingGlobals::hate_diagonals)
{
if (successor.x != x && successor.y != y)
{
cost = 0.1;
}
}
*/
//Vector p(x, y);
//penalize moving towards obstructions
/*
int r = 20;
float costy=0;
int c = 0;
float v = 0;
float dist = sqr(r*TILE_SIZE);
TileVector tme(this->x, this->y);
for (int x = -r; x < r; x++)
{
for (int y = -r; y < r; y++)
{
TileVector t(this->x + x, this->y + y);
if (dsq->game->isObstructed(t))
{
Vector diff = t.worldVector() - tme.worldVector();
int d = diff.getSquaredLength2D();
if (d < dist)
{
costy += 0.1;
}
//TileVector tme(this->x, this->y);
//Vector diff = t.worldVector() - tme.worldVector();
//int d = diff.getSquaredLength2D();
//if (d < dist)
//{
// v += dist-diff.getSquaredLength2D();
//}
}
c++;
}
}
cost += costy;
*/
/*
if (v > 0)
{
v /= float(c);
v /= float(dist);
cost += v*TILE_SIZE*10;
}
*/
//penalize changing direction to tempt computer into moving in "straighter" paths
/*
if (successor.y != y && (dir == LEFT || dir == RIGHT))
cost +=39;
if (successor.x != x && (dir == UP || dir == DOWN))
cost +=39;
*/
return cost;
}
int MapSearchNode::GetMap (int tx, int ty)
{
//return 0;
//return PathFindingGlobals::zones[int(tx/divs)][int(ty/divs)] > cutOff;
int v = dsq->game->getGrid(TileVector(tx,ty));
/*
if (v != 0 && v != 1)
{
std::ostringstream os;
os << "v: " << v;
debugLog(os.str());
}
*/
return v;
/*
if (dsq->game->getGrid(TileVector(x,y))
|| dsq->game->getGrid(TileVector(x+1,y))
|| dsq->game->getGrid(TileVector(x-1,y))
|| dsq->game->getGrid(TileVector(x+2,y))
|| dsq->game->getGrid(TileVector(x-2,y))
*/
/*
int r = 3;
for (int x = -r; x < r; x++)
{
for (int y = -r; y < r; y++)
{
if (dsq->game->getGrid(TileVector(tx+x, ty+y)))
return 1;
}
}
return 0;
*/
/*
// ignore the start node
if (x == KittyTown::instance->nodeStart->x && y == KittyTown::instance->nodeStart->y)
{
obs = -1;
}
return obs;
*/
}
// same state in a maze search is simply when (x,y) are the same
bool MapSearchNode::IsSameState( MapSearchNode &rhs )
{
if( (int(x/divs) == int(rhs.x/divs)) && (int(y/divs) == int(rhs.y/divs)) )
return true;
else
return false;
}
void PathFinding::generateZones()
{
return;
/*
for (int x = 0; x < MAX_ZONES; x++)
{
for (int y = 0; y < MAX_ZONES; y++)
{
PathFindingGlobals::zones[x][y] = 0;
}
}
for (int x = 0; x < MAX_GRID; x+=divs)
{
for (int y = 0; y < MAX_GRID; y+=divs)
{
for (int xx = x; xx < x + divs; xx++)
{
for (int yy = y; yy < y + divs; yy++)
{
if (dsq->game->getGrid(TileVector(xx,yy)) > 0)
{
PathFindingGlobals::zones[int(x/divs)][int(y/divs)]++;
}
}
}
}
}
*/
}
void PathFinding::forceMinimumPath(VectorPath &path, const Vector &start, const Vector &dest)
{
if (path.getNumPathNodes() <= 2)
{
debugLog(" Path is <= 2 nodes... setting up simple path");
//debugLog(" Path is <= 2 nodes... setting up simple path");
path.clear();
path.addPathNode(start, 0);
path.addPathNode(dest, 1);
@ -353,17 +57,8 @@ void PathFinding::forceMinimumPath(VectorPath &path, const Vector &start, const
void PathFinding::molestPath(VectorPath &path)
{
//path.cut(2);
int sz=path.getNumPathNodes();
/*
//normals.resize(sz);
*/
//float maxDist = 15155;
int i = 0;
// make normals
std::vector<Vector> normals;
@ -372,26 +67,12 @@ void PathFinding::molestPath(VectorPath &path)
{
Vector node = path.getPathNode(i)->value;
float dist;
/*
float coverage = dsq->game->getCoverage(node, 100);
int sample = 10;
if (coverage > 0.4f)
sample = 5;
*/
int sample = 20;
float maxDist = sample * TILE_SIZE;
//sqrtf(sqr(sample*TILE_SIZE)+sqr(sample*TILE_SIZE));
//if (coverage < 0.6f)
{
Vector n = dsq->game->getWallNormal(node, sample, &dist);
if (dist != -1 && (n.x != 0 || n.y != 0))
{
/*
if (dist > maxDist)
maxDist = dist;
n *= (maxDist-dist); // *(1.0f-coverage);
*/
n.setLength2D(200);
TileVector test(node + n);
if (dsq->game->isObstructed(test))
@ -408,55 +89,27 @@ void PathFinding::molestPath(VectorPath &path)
}
}
}
/*std::ostringstream os;
os << "pushing node [" << i << "] out by (" << n.x << ", " << n.y << ") - dist: " << dist << " maxDist: " << maxDist;
debugLog(os.str());*/
//path.getPathNode(i)->value += n;
normals[i] = n;
}
/*
std::ostringstream os;
os << "largest maxDist: " << maxDist;
debugLog(os.str());
*/
}
}
// use wall normal to push out node a bit
std::vector<Vector> newNormals;
newNormals.resize(normals.size());
for (i = 1; i < normals.size()-1; i++)
{
// not doing smoothing!
Vector thisOne = normals[i];
Vector lastOne = normals[i-1];
Vector nextOne = normals[i+1];
newNormals[i] = (thisOne + lastOne + nextOne)/3.0f;
//newNormals[i] = thisOne;
}
newNormals[i] = (normals[i] + normals[i-1] + normals[i+1])/3.0f;
for (i = 1; i < sz-1; i++)
{
path.getPathNode(i)->value += newNormals[i];
}
// kill bowls
int start = 0;
//int minDist = 150;
int runs=0;
bool hadSuccess = false;
int lastSuccessNode = 0;
//int adjust = int(minDist/float(TILE_SIZE*8));
int adjust = 2; // 1
//bowl_loop:
int adjust = 2;
sz=path.getNumPathNodes();
/*std::ostringstream os;
os << "kill bowls # " << runs;
debugLog(os.str());*/
for (i = start; i < sz-1; i++)
{
runs++;
@ -469,349 +122,58 @@ void PathFinding::molestPath(VectorPath &path)
hadSuccess = false;
Vector node = path.getPathNode(i)->value;
for (int j = sz-3; j >= i+adjust; j--)
//for (int j = i+adjust; j < sz-1; j++)
{
Vector target = path.getPathNode(j)->value;
//if ((target-node).getSquaredLength2D() >= sqr(minDist))
if (dsq->game->trace(node, target))
{
if (dsq->game->trace(node, target))
{
hadSuccess = true;
lastSuccessNode = j;
break;
}
/*
else if (hadSuccess)
{
//break;
}
*/
hadSuccess = true;
lastSuccessNode = j;
break;
}
}
if (hadSuccess)
{
// only do this if
//VectorPath copy = path.copySection(i,lastSuccessNode);
/*
// this code will only delete things that are bowl-ish
// (things that take you on detours)
float len = path.getSubSectionLength(i, lastSuccessNode);
float shortCut = (path.getPathNode(lastSuccessNode)->value - path.getPathNode(i)->value).getLength2D();
if (len > shortCut+TILE_SIZE*4)
*/
{
path.removeNodes(i+1, lastSuccessNode-1);
/*std::ostringstream os;
os << "killing bowl: " << i+1 << " - " << lastSuccessNode-1;
debugLog(os.str());*/
//start = lastSuccessNode - (lastSuccessNode-i);
//start = i+1;
//i = i+1;
i++;
}
++i;
path.removeNodes(i, lastSuccessNode-1);
hadSuccess = false;
//start += 2;
//goto bowl_loop;
}
sz = path.getNumPathNodes();
}
//debugLog("kill bowls done");
sz=path.getNumPathNodes();
// remove last node
path.removeNodes(path.getNumPathNodes()-2, path.getNumPathNodes()-2);
/*
loop:
for (int i = 0; i < sz-2; i++)
{
Vector node = path.getPathNode(i)->value;
Vector next = path.getPathNode(i+1)->value;
Vector next2 = path.getPathNode(i+2)->value;
int dist1 = (next - node).getSquaredLength2D() + (next2 - next).getSquaredLength2D();
int dist2 = (next2 - node).getSquaredLength2D();
if (dist2 <= dist1)
{
// remove next
path.removeNode(i+1);
goto loop;
}
}
*/
//path.removeNodes(path.getNumPathNodes()-2, path.getNumPathNodes()-2);
path.realPercentageCalc();
//path.calculatePercentages();
/*
int sz=path.getNumPathNodes();
std::vector<Vector> normals;
normals.resize(sz);
for (int i = 1; i < sz-1; i++)
{
Vector node = path.getPathNode(i)->value;
Vector normal = dsq->game->getWallNormal(node, 10);
if (normal.x != 0 && normal.y != 0)
{
normal = normal*TILE_SIZE*10;
}
normals[i] = normal;
//path.getPathNode(i)->value = node;
}
for (int i = 1; i < sz-1; i++)
{
Vector normal = normals[i];
Vector lastNormal = normals[i-1];
//Vector node = path.getPathNode(i)->value;
//// average with the
//Vector prev = path.getPathNode(i-1)->value;
//Vector next = path.getPathNode(i+1)->value;
//node = (node + prev)/2.0f;
normal = (normal + lastNormal)/2.0f;
path.getPathNode(i)->value += normal;
}
*/
/*
for (int i = 1; i < sz; i++)
{
Vector node = path.getPathNode(i)->value;
Vector p0 = path.getPathNode(i-1)->value;
Vector p1 = path.getPathNode(i)->value;
Vector p = p1 - p0;
if (i < sz-1)
{
p += path.getPathNode(i+1)->value - path.getPathNode(i)->value;
p /= 2.0f;
}
Vector pl = p.getPerpendicularLeft();
Vector pr = p.getPerpendicularRight();
pl.normalize2D();
pr.normalize2D();
TileVector tl(node), tr(node);
int left, right;
int maxCheck = 40;
for (left = 0; left < maxCheck; left++)
{
if (dsq->game->isObstructed(tl))
break;
tl.x += pl.x;
tl.y += pl.y;
}
for (right = 0; right < maxCheck; right++)
{
if (dsq->game->isObstructed(tr))
break;
tr.x += pr.x;
tr.y += pr.y;
}
if (left == maxCheck && right == maxCheck)
{
continue;
}
else if (left != 0 || right != 0)
{
//Vector normal = dsq->game->getWallNormal(node);
//if (normal.x != 0 && normal.y != 0)
//{
// if (left < right)
// path.getPathNode(i)->value += normal * (right-left)*TILE_SIZE;
// if (right > left)
// path.getPathNode(i)->value += normal * (left-right)*TILE_SIZE;
//}
//int leftSz = left * TILE_SIZE;
////if (leftSz <= 0) leftSz = 1;
//int rightSz = right * TILE_SIZE;
////if (rightSz <= 0) rightSz = 1;
//pl |= leftSz;
//pr |= rightSz;
//
path.getPathNode(i)->value = (tr.worldVector() + tl.worldVector())/2.0f;
//path.getPathNode(i)->value = tl.worldVector() + (tr.worldVector() - tl.worldVector())/2.0f;//(node + pl) + (pr-pl)/2.0f;
path.getPathNode(i)->value.z = 0;
}
}
*/
/*
for (int i = 1; i < sz; i++)
{
Vector node = path.getPathNode(i)->value;
Vector pl = p.getPerpendicularLeft();
Vector pr = p.getPerpendicularRight();
pl.normalize2D();
pr.normalize2D();
TileVector tl(node), tr(node);
int left, right;
int maxCheck = 40;
for (int i = 0; i < maxCheck; i++)
{
dsq->game->position
}
if (left == maxCheck && right == maxCheck)
{
continue;
}
else if (left != 0 || right != 0)
{
//Vector normal = dsq->game->getWallNormal(node);
//if (normal.x != 0 && normal.y != 0)
//{
// if (left < right)
// path.getPathNode(i)->value += normal * (right-left)*TILE_SIZE;
// if (right > left)
// path.getPathNode(i)->value += normal * (left-right)*TILE_SIZE;
//}
//int leftSz = left * TILE_SIZE;
////if (leftSz <= 0) leftSz = 1;
//int rightSz = right * TILE_SIZE;
////if (rightSz <= 0) rightSz = 1;
//pl |= leftSz;
//pr |= rightSz;
//
path.getPathNode(i)->value = (tr.worldVector() + tl.worldVector())/2.0f;
//path.getPathNode(i)->value = tl.worldVector() + (tr.worldVector() - tl.worldVector())/2.0f;//(node + pl) + (pr-pl)/2.0f;
path.getPathNode(i)->value.z = 0;
}
}
*/
}
void PathFinding::generatePath(RenderObject *ro, TileVector start, TileVector goal, int offx, int offy, bool hate_diagonals)
void PathFinding::generatePath(RenderObject *ro, TileVector start, TileVector goal, int offx, int offy)
{
//return;
int sx = start.x;
int sy = start.y;
int gx = goal.x;
int gy = goal.y;
PathFindingGlobals::hate_diagonals = hate_diagonals;
/*
if (offx >= TILE_SIZE/2-1)
offx--;
if (offy >= TILE_SIZE/2-1)
offy--;
if (offx <= TILE_SIZE/2+1)
offx++;
if (offy <= TILE_SIZE/2+1)
offy++;
*/
ro->position.ensureData();
ro->position.data->path.clear();
VectorPath& vp = ro->position.data->path;
vp.clear();
PathFindingGlobals::render_object = ro;
AStarSearch astarsearch;
// Create a start state
MapSearchNode nodeStart;
nodeStart.x = sx;
nodeStart.y = sy;
PathFindingGlobals::node_start = nodeStart;
if (nodeStart.GetMap(gx, gy) > 0)
SearchGrid grid;
JPS::PathVector path;
if(JPS::findPath(path, grid, start.x, start.y, goal.x, goal.y, 10))
{
std::ostringstream os;
os << "goal (" << gx << ", " << gy << ") blocked";
debugLog (os.str());
return;
vp.addPathNode(ro->position, 0);
generateVectorPath(path, vp, offx, offy);
}
// Define the goal state
MapSearchNode nodeEnd;
nodeEnd.x = gx;
nodeEnd.y = gy;
PathFindingGlobals::node_goal = nodeEnd;
// Set Start and goal states
astarsearch.SetStartAndGoalStates( nodeStart, nodeEnd );
unsigned int SearchState;
unsigned int SearchSteps = 0;
do
{
SearchState = astarsearch.SearchStep();
if (SearchState != AStarSearch::SEARCH_STATE_SEARCHING)
break;
SearchSteps++;
if (SearchSteps > MAX_STEPS) break;
}
while( SearchState == AStarSearch::SEARCH_STATE_SEARCHING );
if( SearchState == AStarSearch::SEARCH_STATE_SUCCEEDED )
{
//errorLog ("Search found goal state",0);
MapSearchNode *node = astarsearch.GetSolutionStart();
int steps = 0;
//node->PrintNodeInfo();
ro->position.data->path.addPathNode(Vector((node->x*TILE_SIZE)+TILE_SIZE/2+offx, (node->y*TILE_SIZE)+TILE_SIZE/2)+offy, 0);
for( ;; )
{
node = astarsearch.GetSolutionNext();
if( !node )
{
break;
}
//node->PrintNodeInfo();
ro->position.data->path.addPathNode(Vector((node->x*TILE_SIZE)+TILE_SIZE/2+offx, (node->y*TILE_SIZE)+TILE_SIZE/2)+offy, steps);
steps ++;
};
//ro->position.path.addPathNode(Vector(goal.x*TILE_SIZE, goal.y*TILE_SIZE), steps);
/*
std::ostringstream os;
os << "Solution steps " << steps;
msg(os.str());
*/
// Once you're done with the solution you can free the nodes up
astarsearch.FreeSolutionNodes();
}
else if( SearchState == AStarSearch::SEARCH_STATE_FAILED )
{
debugLog("Search terminated. Did not find goal state");
//astarsearch.FreeSolutionNodes();
astarsearch.FreeStartAndGoalNodes();
}
else
{
// exceeded count
debugLog("Path too long");
astarsearch.FreeAllNodes();
astarsearch.FreeStartAndGoalNodes();
//astarsearch.FreeSolutionNodes();
}
if (astarsearch.m_AllocateNodeCount != astarsearch.m_FreeNodeCount)
{
debugLog("astar memory leak");
}
//return path_vector;
}
bool PathFinding::generatePathSimple(VectorPath& path, const Vector& start, const Vector& end, unsigned int step /* = 0 */)
{
SearchGrid grid;
JPS::PathVector p;
TileVector tstart(start);
TileVector tend(end);
if(!JPS::findPath(p, grid, tstart.x, tstart.y, tend.x, tend.y, step))
return false;
generateVectorPath(p, path, 0, 0);
molestPath(path);
return true;
}

772
Aquaria/PathFinding.h
View file

@ -26,780 +26,18 @@ Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
#include "TileVector.h"
#include <assert.h>
/*
class AStarNode
{
public:
const bool operator==(const AStarNode &a) const
{
return x==a.x && y==a.y;
}
const bool operator<(const AStarNode &a) const;
AStarNode(){x=0;y=0;parent=0;}
AStarNode (int x, int y) : x(x),y(y){}
int x, y;
int f, g, h;
AStarNode *parent;
//int id;
//int pid;
};
*/
class AStarSearch;
class MapSearchNode
{
public:
unsigned int x; // the (x,y) positions of the node
unsigned int y;
MapSearchNode() { x = y = 0;}
MapSearchNode( unsigned int px, unsigned int py ) { x=px; y=py;}
float GoalDistanceEstimate( MapSearchNode &nodeGoal );
bool IsGoal( MapSearchNode &nodeGoal );
bool GetSuccessors( AStarSearch *astarsearch, MapSearchNode *parent_node );
float GetCost( MapSearchNode &successor );
bool IsSameState( MapSearchNode &rhs );
int GetMap (int x, int y);
//void PrintNodeInfo();
};
class RenderObject;
class SearchGrid;
class Game;
class PathFinding
{
public:
void forceMinimumPath(VectorPath &path, const Vector &start, const Vector &dest);
void molestPath(VectorPath &path);
void generateZones();
void generatePath(RenderObject *go, TileVector g1, TileVector g2, int offx=0, int offy=0, bool hate_diagonals=false);
};
// stl includes
#include <algorithm>
#include <set>
#include <vector>
//#define USE_FSA_MEMORY 1
#ifdef _MSC_VER
// disable warning that debugging information has lines that are truncated
// occurs in stl headers
#pragma warning( disable : 4786 )
#endif
#define UserState MapSearchNode
// The AStar search class. UserState is the users state space type
class AStarSearch
{
public: // data
enum
{
SEARCH_STATE_NOT_INITIALISED,
SEARCH_STATE_SEARCHING,
SEARCH_STATE_SUCCEEDED,
SEARCH_STATE_FAILED,
SEARCH_STATE_OUT_OF_MEMORY,
SEARCH_STATE_INVALID
};
// A node represents a possible state in the search
// The user provided state type is included inside this type
public:
class Node
{
public:
Node *parent; // used during the search to record the parent of successor nodes
Node *child; // used after the search for the application to view the search in reverse
float g; // cost of this node + it's predecessors
float h; // heuristic estimate of distance to goal
float f; // sum of cumulative cost of predecessors and self and heuristic
Node() :
parent( 0 ),
child( 0 ),
g( 0.0f ),
h( 0.0f ),
f( 0.0f )
{
}
UserState m_UserState;
};
typedef std::vector<Node*> NodeContainer;
class Test
{
};
// For sorting the heap the STL needs compare function that lets us compare
// the f value of two nodes
class HeapCompare_f
{
public:
bool operator() ( const Node *x, const Node *y ) const
{
return x->f > y->f;
}
};
public: // methods
// constructor just initialises private data
AStarSearch( int MaxNodes = 1000 ) :
m_AllocateNodeCount(0),
m_FreeNodeCount(0),
/*m_FixedSizeAllocator( MaxNodes ),*/
m_State( SEARCH_STATE_NOT_INITIALISED ),
m_CurrentSolutionNode( NULL ),
m_CancelRequest( false )
{
}
// call at any time to cancel the search and free up all the memory
void CancelSearch()
{
m_CancelRequest = true;
}
// Set Start and goal states
void SetStartAndGoalStates( UserState &Start, UserState &Goal )
{
m_CancelRequest = false;
m_Start = AllocateNode();
m_Goal = AllocateNode();
m_Start->m_UserState = Start;
m_Goal->m_UserState = Goal;
m_State = SEARCH_STATE_SEARCHING;
// Initialise the AStar specific parts of the Start Node
// The user only needs fill out the state information
m_Start->g = 0;
m_Start->h = m_Start->m_UserState.GoalDistanceEstimate( m_Goal->m_UserState );
m_Start->f = m_Start->g + m_Start->h;
m_Start->parent = 0;
// Push the start node on the Open list
m_OpenList.push_back( m_Start ); // heap now unsorted
// Sort back element into heap
push_heap( m_OpenList.begin(), m_OpenList.end(), HeapCompare_f() );
// Initialise counter for search steps
m_Steps = 0;
}
// Advances search one step
unsigned int SearchStep()
{
// Firstly break if the user has not initialised the search
assert( (m_State > SEARCH_STATE_NOT_INITIALISED) &&
(m_State < SEARCH_STATE_INVALID) );
// Next I want it to be safe to do a searchstep once the search has succeeded...
if( (m_State == SEARCH_STATE_SUCCEEDED) ||
(m_State == SEARCH_STATE_FAILED)
)
{
return m_State;
}
// Failure is defined as emptying the open list as there is nothing left to
// search...
// New: Allow user abort
if( m_OpenList.empty() || m_CancelRequest )
{
FreeAllNodes();
m_State = SEARCH_STATE_FAILED;
return m_State;
}
// Incremement step count
m_Steps ++;
// Pop the best node (the one with the lowest f)
Node *n = m_OpenList.front(); // get pointer to the node
pop_heap( m_OpenList.begin(), m_OpenList.end(), HeapCompare_f() );
m_OpenList.pop_back();
// Check for the goal, once we pop that we're done
if( n->m_UserState.IsGoal( m_Goal->m_UserState ) )
{
// The user is going to use the Goal Node he passed in
// so copy the parent pointer of n
m_Goal->parent = n->parent;
// A special case is that the goal was passed in as the start state
// so handle that here
if( n != m_Start )
{
//delete n;
FreeNode( n );
// set the child pointers in each node (except Goal which has no child)
Node *nodeChild = m_Goal;
Node *nodeParent = m_Goal->parent;
do
{
nodeParent->child = nodeChild;
nodeChild = nodeParent;
nodeParent = nodeParent->parent;
}
while( nodeChild != m_Start ); // Start is always the first node by definition
}
// delete nodes that aren't needed for the solution
FreeUnusedNodes();
m_State = SEARCH_STATE_SUCCEEDED;
return m_State;
}
else // not goal
{
// We now need to generate the successors of this node
// The user helps us to do this, and we keep the new nodes in
// m_Successors ...
m_Successors.clear(); // empty vector of successor nodes to n
// User provides this functions and uses AddSuccessor to add each successor of
// node 'n' to m_Successors
bool ret = n->m_UserState.GetSuccessors( this, n->parent ? &n->parent->m_UserState : NULL );
if( !ret )
{
// free the nodes that may previously have been added
NodeContainer::iterator successor;
for( successor = m_Successors.begin(); successor != m_Successors.end(); successor ++ )
{
FreeNode( (*successor) );
}
m_Successors.clear(); // empty vector of successor nodes to n
// free up everything else we allocated
FreeAllNodes();
m_State = SEARCH_STATE_OUT_OF_MEMORY;
return m_State;
}
// Now handle each successor to the current node ...
for( NodeContainer::iterator successor = m_Successors.begin(); successor != m_Successors.end(); successor ++ )
{
// The g value for this successor ...
float newg = n->g + n->m_UserState.GetCost( (*successor)->m_UserState );
// Now we need to find whether the node is on the open or closed lists
// If it is but the node that is already on them is better (lower g)
// then we can forget about this successor
// First linear search of open list to find node
NodeContainer::iterator openlist_result;
for( openlist_result = m_OpenList.begin(); openlist_result != m_OpenList.end(); openlist_result ++ )
{
if( (*openlist_result)->m_UserState.IsSameState( (*successor)->m_UserState ) )
{
break;
}
}
if( openlist_result != m_OpenList.end() )
{
// we found this state on open
if( (*openlist_result)->g <= newg )
{
FreeNode( (*successor) );
// the one on Open is cheaper than this one
continue;
}
}
NodeContainer::iterator closedlist_result;
for( closedlist_result = m_ClosedList.begin(); closedlist_result != m_ClosedList.end(); closedlist_result ++ )
{
if( (*closedlist_result)->m_UserState.IsSameState( (*successor)->m_UserState ) )
{
break;
}
}
if( closedlist_result != m_ClosedList.end() )
{
// we found this state on closed
if( (*closedlist_result)->g <= newg )
{
// the one on Closed is cheaper than this one
FreeNode( (*successor) );
continue;
}
}
// This node is the best node so far with this particular state
// so lets keep it and set up its AStar specific data ...
(*successor)->parent = n;
(*successor)->g = newg;
(*successor)->h = (*successor)->m_UserState.GoalDistanceEstimate( m_Goal->m_UserState );
(*successor)->f = (*successor)->g + (*successor)->h;
// Remove successor from closed if it was on it
if( closedlist_result != m_ClosedList.end() )
{
// remove it from Closed
FreeNode( (*closedlist_result) );
m_ClosedList.erase( closedlist_result );
}
// Update old version of this node
if( openlist_result != m_OpenList.end() )
{
FreeNode( (*openlist_result) );
m_OpenList.erase( openlist_result );
// re-make the heap
make_heap( m_OpenList.begin(), m_OpenList.end(), HeapCompare_f() );
// make_heap rather than sort_heap is an essential bug fix
// thanks to Mike Ryynanen for pointing this out and then explaining
// it in detail. sort_heap called on an invalid heap does not work
// sort_heap( m_OpenList.begin(), m_OpenList.end(), HeapCompare_f() );
// assert( is_heap( m_OpenList.begin(), m_OpenList.end(), HeapCompare_f() ) );
}
// heap now unsorted
m_OpenList.push_back( (*successor) );
// sort back element into heap
push_heap( m_OpenList.begin(), m_OpenList.end(), HeapCompare_f() );
}
// push n onto Closed, as we have expanded it now
m_ClosedList.push_back( n );
} // end else (not goal so expand)
return m_State; // Succeeded bool is false at this point.
}
// User calls this to add a successor to a list of successors
// when expanding the search frontier
bool AddSuccessor( UserState &State )
{
Node *node = AllocateNode();
if( node )
{
node->m_UserState = State;
m_Successors.push_back( node );
return true;
}
return false;
}
// Free the solution nodes
// This is done to clean up all used Node memory when you are done with the
// search
void FreeSolutionNodes()
{
Node *n = m_Start;
if( m_Start->child )
{
do
{
Node *del = n;
n = n->child;
FreeNode( del );
del = NULL;
} while( n != m_Goal );
FreeNode( n ); // Delete the goal
}
else
{
// if the start node is the solution we need to just delete the start and goal
// nodes
FreeNode( m_Start );
FreeNode( m_Goal );
}
}
void FreeStartAndGoalNodes()
{
//FreeNode( m_Start );
FreeNode( m_Goal );
}
// Functions for traversing the solution
// Get start node
UserState *GetSolutionStart()
{
m_CurrentSolutionNode = m_Start;
if( m_Start )
{
return &m_Start->m_UserState;
}
else
{
return NULL;
}
}
// Get next node
UserState *GetSolutionNext()
{
if( m_CurrentSolutionNode )
{
if( m_CurrentSolutionNode->child )
{
Node *child = m_CurrentSolutionNode->child;
m_CurrentSolutionNode = m_CurrentSolutionNode->child;
return &child->m_UserState;
}
}
return NULL;
}
// Get end node
UserState *GetSolutionEnd()
{
m_CurrentSolutionNode = m_Goal;
if( m_Goal )
{
return &m_Goal->m_UserState;
}
else
{
return NULL;
}
}
// Step solution iterator backwards
UserState *GetSolutionPrev()
{
if( m_CurrentSolutionNode )
{
if( m_CurrentSolutionNode->parent )
{
Node *parent = m_CurrentSolutionNode->parent;
m_CurrentSolutionNode = m_CurrentSolutionNode->parent;
return &parent->m_UserState;
}
}
return NULL;
}
// For educational use and debugging it is useful to be able to view
// the open and closed list at each step, here are two functions to allow that.
UserState *GetOpenListStart()
{
float f,g,h;
return GetOpenListStart( f,g,h );
}
UserState *GetOpenListStart( float &f, float &g, float &h )
{
/*
iterDbgOpen = m_OpenList.begin();
if( iterDbgOpen != m_OpenList.end() )
{
f = (*iterDbgOpen)->f;
g = (*iterDbgOpen)->g;
h = (*iterDbgOpen)->h;
return &(*iterDbgOpen)->m_UserState;
}
*/
return NULL;
}
UserState *GetOpenListNext()
{
float f,g,h;
return GetOpenListNext( f,g,h );
}
UserState *GetOpenListNext( float &f, float &g, float &h )
{
/*
iterDbgOpen++;
if( iterDbgOpen != m_OpenList.end() )
{
f = (*iterDbgOpen)->f;
g = (*iterDbgOpen)->g;
h = (*iterDbgOpen)->h;
return &(*iterDbgOpen)->m_UserState;
}
*/
return NULL;
}
UserState *GetClosedListStart()
{
float f,g,h;
return GetClosedListStart( f,g,h );
}
UserState *GetClosedListStart( float &f, float &g, float &h )
{
/*
iterDbgClosed = m_ClosedList.begin();
if( iterDbgClosed != m_ClosedList.end() )
{
f = (*iterDbgClosed)->f;
g = (*iterDbgClosed)->g;
h = (*iterDbgClosed)->h;
return &(*iterDbgClosed)->m_UserState;
}
*/
return NULL;
}
UserState *GetClosedListNext()
{
float f,g,h;
return GetClosedListNext( f,g,h );
}
UserState *GetClosedListNext( float &f, float &g, float &h )
{
/*
iterDbgClosed++;
if( iterDbgClosed != m_ClosedList.end() )
{
f = (*iterDbgClosed)->f;
g = (*iterDbgClosed)->g;
h = (*iterDbgClosed)->h;
return &(*iterDbgClosed)->m_UserState;
}
*/
return NULL;
}
// Get the number of steps
int GetStepCount() { return m_Steps; }
// debugging : count memory allocation and free's
int m_AllocateNodeCount;
int m_FreeNodeCount;
// This is called when a search fails or is cancelled to free all used
// memory
void FreeAllNodes()
{
// iterate open list and delete all nodes
NodeContainer::iterator iterOpen = m_OpenList.begin();
while( iterOpen != m_OpenList.end() )
{
Node *n = (*iterOpen);
FreeNode( n );
iterOpen ++;
}
m_OpenList.clear();
// iterate closed list and delete unused nodes
NodeContainer::iterator iterClosed;
for( iterClosed = m_ClosedList.begin(); iterClosed != m_ClosedList.end(); iterClosed ++ )
{
Node *n = (*iterClosed);
FreeNode( n );
}
m_ClosedList.clear();
}
private: // methods
// This call is made by the search class when the search ends. A lot of nodes may be
// created that are still present when the search ends. They will be deleted by this
// routine once the search ends
void FreeUnusedNodes()
{
// iterate open list and delete unused nodes
NodeContainer::iterator iterOpen = m_OpenList.begin();
while( iterOpen != m_OpenList.end() )
{
Node *n = (*iterOpen);
if( !n->child )
{
FreeNode( n );
n = NULL;
}
iterOpen ++;
}
m_OpenList.clear();
// iterate closed list and delete unused nodes
NodeContainer::iterator iterClosed;
for( iterClosed = m_ClosedList.begin(); iterClosed != m_ClosedList.end(); iterClosed ++ )
{
Node *n = (*iterClosed);
if( !n->child )
{
FreeNode( n );
n = NULL;
}
}
m_ClosedList.clear();
}
// Node memory management
Node *AllocateNode()
{
m_AllocateNodeCount ++;
#if !USE_FSA_MEMORY
Node *p = new Node;
return p;
#else
Node *address = m_FixedSizeAllocator.alloc();
if( !address )
{
return NULL;
}
Node *p = new (address) Node;
return p;
#endif
}
void FreeNode( Node *node )
{
m_FreeNodeCount ++;
#if !USE_FSA_MEMORY
delete node;
#else
m_FixedSizeAllocator.free( node );
#endif
}
private: // data
// Heap (simple vector but used as a heap, cf. Steve Rabin's game gems article)
NodeContainer m_OpenList;
// Closed list is a vector.
NodeContainer m_ClosedList;
// Successors is a vector filled out by the user each type successors to a node
// are generated
NodeContainer m_Successors;
// State
unsigned int m_State;
// Counts steps
int m_Steps;
// Start and goal state pointers
Node *m_Start;
Node *m_Goal;
Node *m_CurrentSolutionNode;
// Memory
// FixedSizeAllocator<Node> m_FixedSizeAllocator;
//Debug : need to keep these two iterators around
// for the user Dbg functions
/*
vector< Node* > ::iterator iterDbgOpen;
vector< Node* > ::iterator iterDbgClosed;
*/
bool m_CancelRequest;
void generatePath(RenderObject *go, TileVector g1, TileVector g2, int offx=0, int offy=0);
bool generatePathSimple(VectorPath& path, const Vector& start, const Vector& end, unsigned int step = 0);
};
#endif