From 70286954a0d9ad6c40d5fe1a77503e95e97ab6bc Mon Sep 17 00:00:00 2001 From: fgenesis Date: Tue, 10 Dec 2013 03:47:58 +0100 Subject: [PATCH] Replace A* pathfinding with much faster jump point search. This also fixes entities glithing through walls, as diagonal cracks were assumed walkable. --- Aquaria/Game.cpp | 2 - Aquaria/Game.h | 5 +- Aquaria/PathFinding.cpp | 728 +++---------------------------------- Aquaria/PathFinding.h | 772 +--------------------------------------- 4 files changed, 53 insertions(+), 1454 deletions(-) diff --git a/Aquaria/Game.cpp b/Aquaria/Game.cpp index 277589a..2fd1b28 100644 --- a/Aquaria/Game.cpp +++ b/Aquaria/Game.cpp @@ -2157,8 +2157,6 @@ void Game::reconstructGrid(bool force) } trimGrid(); - - dsq->pathFinding.generateZones(); } void Game::trimGrid() diff --git a/Aquaria/Game.h b/Aquaria/Game.h index dc2f7cf..3622df2 100644 --- a/Aquaria/Game.h +++ b/Aquaria/Game.h @@ -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 diff --git a/Aquaria/PathFinding.cpp b/Aquaria/PathFinding.cpp index 9080e09..df0bcae 100644 --- a/Aquaria/PathFinding.cpp +++ b/Aquaria/PathFinding.cpp @@ -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 #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 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 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 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; +} diff --git a/Aquaria/PathFinding.h b/Aquaria/PathFinding.h index 70456e3..c0dc9c2 100644 --- a/Aquaria/PathFinding.h +++ b/Aquaria/PathFinding.h @@ -26,780 +26,18 @@ Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. #include "TileVector.h" #include -/* -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 -#include -#include - -//#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 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 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