diff --git a/Aquaria/PathFinding.cpp b/Aquaria/PathFinding.cpp index fa43d38..5adc264 100644 --- a/Aquaria/PathFinding.cpp +++ b/Aquaria/PathFinding.cpp @@ -19,7 +19,7 @@ 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 #include "PathFinding.h" #include "DSQ.h" #include "Game.h" @@ -41,15 +41,15 @@ public: class PathFinding::State : public ScriptObject { public: - State(ObsType obs) : grid(obs), searcher(grid), result(JPS::NO_PATH) {} + State(ObsType obs) : grid(obs), searcher(grid), result(JPS_NO_PATH) {} SearchGridRaw grid; JPS::Searcher searcher; - JPS::Result result; + JPS_Result result; }; static void generateVectorPath(const JPS::PathVector& rawpath, VectorPath& vp, int offx, int offy) { - for(JPS::PathVector::const_iterator it = rawpath.begin(); it != rawpath.end(); ++it) + for(JPS::PathVector::const_iterator it = rawpath.cbegin(); it != rawpath.cend(); ++it) vp.addPathNode(Vector((it->x*TILE_SIZE)+TILE_SIZE/2+offx, (it->y*TILE_SIZE)+TILE_SIZE/2)+offy, 0); } @@ -221,18 +221,18 @@ void PathFinding::beginFindPath(PathFinding::State *state, const Vector& start, bool PathFinding::updateFindPath(PathFinding::State *state, int limit) { - if(state->result == JPS::NEED_MORE_STEPS) + if(state->result == JPS_NEED_MORE_STEPS) { state->result = state->searcher.findPathStep(limit); - return state->result != JPS::NEED_MORE_STEPS; + return state->result != JPS_NEED_MORE_STEPS; } return true; // done } bool PathFinding::finishFindPath(PathFinding::State *state, VectorPath& path, unsigned step /* = 0 */) { - if(state->result != JPS::FOUND_PATH) - return state->result == JPS::EMPTY_PATH; + if(state->result != JPS_FOUND_PATH) + return state->result == JPS_EMPTY_PATH; JPS::PathVector rawpath; state->searcher.findPathFinish(rawpath, step); diff --git a/ExternalLibs/JPS.h b/ExternalLibs/JPS.h deleted file mode 100644 index d307ff0..0000000 --- a/ExternalLibs/JPS.h +++ /dev/null @@ -1,932 +0,0 @@ -#ifndef JUMP_POINT_SEARCH_H -#define JUMP_POINT_SEARCH_H - -// Public domain Jump Point Search implementation by False.Genesis -// Very fast pathfinding for uniform cost grids. -// Supports incremental pathfinding. - -// Please keep the following source information intact when you use this file in your own projects: -// This file originates from: https://github.com/fgenesis/jps -// Based on the paper http://users.cecs.anu.edu.au/~dharabor/data/papers/harabor-grastien-aaai11.pdf -// by Daniel Harabor & Alban Grastien. -// Jumper (https://github.com/Yonaba/Jumper) and PathFinding.js (https://github.com/qiao/PathFinding.js) -// served as reference for this implementation. -// If you use this, attribution would be nice, but is not necessary. - -// ====== COMPILE CONFIG ====== - -// If this is defined, compare all jumps against recursive reference implementation (only if _DEBUG is defined) -//#define JPS_VERIFY - -// If this is defined, use standard A* instead of JPS (e.g. if you want to compare performance in your scenario) -//#define JPS_ASTAR_ONLY - -// If this is defined, disable the greedy direct-short-path check that avoids the large area scanning that JPS does. -// Does not change optimality of results when left enabled -//#define JPS_DISABLE_GREEDY - -// ============================ - -// Usage: -/* -// Define a class that overloads `operator()(x, y) const`, returning a value that can be treated as boolean. -// You are responsible for bounds checking! -// You want your operator() to be as fast as possible, as it will be called a LOT. - -struct MyGrid -{ - inline bool operator()(unsigned x, unsigned y) const - { - if(x < width && y < height) // Unsigned will wrap if < 0 - ... return true if terrain at (x, y) is walkable. - } - unsigned width, height; -}; - -// Then you can retrieve a path: - -MyGrid grid; -// ... set grid width, height, and whatever -unsigned step = 0; // set this to 1 if you want a detailed single-step path - // (e.g. if you plan to further mangle the path yourself), - // or any other higher value to output every Nth position. -JPS::PathVector path; // The resulting path will go here. - - -// Single-call interface: -bool found = JPS::findPath(path, grid, startx, starty, endx, endy, step); - - -// Alternatively, if you want more control: - -JPS::Searcher search(grid); -while(true) -{ - // ..stuff happening ... - - // build path incrementally from waypoints - JPS::Position a, b, c, d = <...>; // set some waypoints - search.findPath(path, a, b); - search.findPath(path, b, c); - search.findPath(path, c, d); - - // re-use existing pathfinder instance - if(!search.findPath(path2, JPS::Pos(startx, starty), JPS::Pos(endx, endy), step)) - { - // ...handle failure... - } - // ... more stuff happening ... - - // At convenient times, you can clean up accumulated nodes to reclaim memory. - // This is never necessary for correct function, but performance will drop if too many cached nodes exist. - if(mapWasReloaded) - search.freeMemory(); -} - -// Further remarks about the super lazy single-call function can be found at the bottom of this file. - -// ------------------------------- -// --- Incremental pathfinding --- -// ------------------------------- - -First, call findPathInit(Position start, Position end). -Don't forget to check the return value, as it may return: -- NO_PATH if one or both of the points are obstructed -- EMPTY_PATH if the points are equal and not obstructed -- FOUND_PATH if the initial greedy heuristic could find a path quickly. -If it returns NEED_MORE_STEPS then the next part can start. - -Repeatedly call findPathStep(int limit) until it returns NO_PATH or FOUND_PATH. -For consistency, you will want to ensure that the grid does not change between subsequent calls; -if the grid changes, parts of the path may go through a now obstructed area. -If limit is 0, it will perform the pathfinding in one go. Values > 0 abort the search -as soon as possible after the number of steps was exceeded, returning NEED_MORE_STEPS. -Use getStepsDone() after some test runs to find a good value for the limit. - -Lastly, generate the actual path points from a successful run via findPathFinish(PathVector& path, unsigned step = 0). -Like described above, path points are appended, and granularity can be adjusted with the step parameter. -Returns false if the pathfinding did not finish or generating the path failed. - -*/ - - -#include -#include -#include -#include - -#include - -#ifdef _DEBUG -#include -#define JPS_ASSERT(cond) assert(cond) -#else -#define JPS_ASSERT(cond) -#endif - - -namespace JPS { - -enum Result -{ - NO_PATH, - FOUND_PATH, - NEED_MORE_STEPS, - EMPTY_PATH -}; - -struct Position -{ - unsigned x, y; - - inline bool operator==(const Position& p) const - { - return x == p.x && y == p.y; - } - inline bool operator!=(const Position& p) const - { - return x != p.x || y != p.y; - } - - // for sorting - inline bool operator<(const Position& p) const - { - return y < p.y || (y == p.y && x < p.x); - } - - inline bool isValid() const { return x != unsigned(-1); } -}; - -typedef std::vector PathVector; - -// ctor function to keep Position a real POD struct. -inline static Position Pos(unsigned x, unsigned y) -{ - Position p; - p.x = x; - p.y = y; - return p; -} - -namespace Internal { - -static const Position npos = Pos(-1, -1); - -class Node -{ -public: - Node(const Position& p) : f(0), g(0), pos(p), parent(0), flags(0) {} - unsigned f, g; - const Position pos; - const Node *parent; - - inline void setOpen() { flags |= 1; } - inline void setClosed() { flags |= 2; } - inline unsigned isOpen() const { return flags & 1; } - inline unsigned isClosed() const { return flags & 2; } - inline void clearState() { f = 0; g = 0, parent = 0; flags = 0; } - -private: - unsigned flags; - - bool operator==(const Node& o); // not implemented, nodes should not be compared -}; -} // end namespace Internal - -namespace Heuristic -{ - inline unsigned Manhattan(const Internal::Node *a, const Internal::Node *b) - { - return abs(int(a->pos.x - b->pos.x)) + abs(int(a->pos.y - b->pos.y)); - } - - inline unsigned Euclidean(const Internal::Node *a, const Internal::Node *b) - { - float fx = float(int(a->pos.x - b->pos.x)); - float fy = float(int(a->pos.y - b->pos.y)); - return unsigned(int(sqrtf(fx*fx + fy*fy))); - } -} // end namespace heuristic - -namespace Internal { - -typedef std::vector NodeVector; - -class OpenList -{ -public: - inline void push(Node *node) - { - JPS_ASSERT(node); - nodes.push_back(node); - std::push_heap(nodes.begin(), nodes.end(), _compare); - } - inline Node *pop() - { - std::pop_heap(nodes.begin(), nodes.end(), _compare); - Node *node = nodes.back(); - nodes.pop_back(); - return node; - } - inline bool empty() const - { - return nodes.empty(); - } - inline void clear() - { - nodes.clear(); - } - inline void fixup() - { - std::make_heap(nodes.begin(), nodes.end(), _compare); - } - -protected: - static inline bool _compare(const Node *a, const Node *b) - { - return a->f > b->f; - } - NodeVector nodes; -}; - -template class Searcher -{ -public: - Searcher(const GRID& g) - : grid(g), endNode(NULL), skip(1), stepsRemain(0), stepsDone(0) - {} - - // single-call - bool findPath(PathVector& path, Position start, Position end, unsigned step); - - // incremental pathfinding - Result findPathInit(Position start, Position end); - Result findPathStep(int limit); - bool findPathFinish(PathVector& path, unsigned step); - - // misc - void freeMemory(); - inline void setSkip(int s) { skip = std::max(1, s); } - inline size_t getStepsDone() const { return stepsDone; } - inline size_t getNodesExpanded() const { return nodegrid.size(); } - -private: - - typedef std::map NodeGrid; - - const GRID& grid; - Node *endNode; - int skip; - int stepsRemain; - size_t stepsDone; - OpenList open; - - NodeGrid nodegrid; - - Node *getNode(const Position& pos); - void identifySuccessors(const Node *n); - bool generatePath(PathVector& path, unsigned step) const; -#ifndef JPS_DISABLE_GREEDY - bool findPathGreedy(Node *start); -#endif - -#ifdef JPS_ASTAR_ONLY - unsigned findNeighborsAStar(const Node *n, Position *wptr); -#else - unsigned findNeighbors(const Node *n, Position *wptr) const; - Position jumpP(const Position& p, const Position& src); - Position jumpD(Position p, int dx, int dy); - Position jumpX(Position p, int dx); - Position jumpY(Position p, int dy); -#endif -#ifdef JPS_VERIFY - Position jumpPRec(const Position& p, const Position& src) const; -#endif -}; - -template inline Node *Searcher::getNode(const Position& pos) -{ - JPS_ASSERT(grid(pos.x, pos.y)); - return &nodegrid.insert(std::make_pair(pos, Node(pos))).first->second; -} - -#ifndef JPS_ASTAR_ONLY -template Position Searcher::jumpP(const Position &p, const Position& src) -{ - JPS_ASSERT(grid(p.x, p.y)); - - int dx = int(p.x - src.x); - int dy = int(p.y - src.y); - JPS_ASSERT(dx || dy); - - if(dx && dy) - return jumpD(p, dx, dy); - else if(dx) - return jumpX(p, dx); - else if(dy) - return jumpY(p, dy); - - // not reached - JPS_ASSERT(false); - return npos; -} - -template Position Searcher::jumpD(Position p, int dx, int dy) -{ - JPS_ASSERT(grid(p.x, p.y)); - JPS_ASSERT(dx && dy); - - const Position& endpos = endNode->pos; - int steps = 0; - - while(true) - { - if(p == endpos) - break; - - ++steps; - const unsigned x = p.x; - const unsigned y = p.y; - - if( (grid(x-dx, y+dy) && !grid(x-dx, y)) || (grid(x+dx, y-dy) && !grid(x, y-dy)) ) - break; - - const bool gdx = grid(x+dx, y); - const bool gdy = grid(x, y+dy); - - if(gdx && jumpX(Pos(x+dx, y), dx).isValid()) - break; - - if(gdy && jumpY(Pos(x, y+dy), dy).isValid()) - break; - - if((gdx || gdy) && grid(x+dx, y+dy)) - { - p.x += dx; - p.y += dy; - } - else - { - p = npos; - break; - } - } - stepsDone += (unsigned)steps; - stepsRemain -= steps; - return p; -} - -template inline Position Searcher::jumpX(Position p, int dx) -{ - JPS_ASSERT(dx); - JPS_ASSERT(grid(p.x, p.y)); - - const unsigned y = p.y; - const Position& endpos = endNode->pos; - const int skip = this->skip; - int steps = 0; - - unsigned a = ~((!!grid(p.x, y+skip)) | ((!!grid(p.x, y-skip)) << 1)); - - while(true) - { - const unsigned xx = p.x + dx; - const unsigned b = (!!grid(xx, y+skip)) | ((!!grid(xx, y-skip)) << 1); - - if((b & a) || p == endpos) - break; - if(!grid(xx, y)) - { - p = npos; - break; - } - - p.x += dx; - a = ~b; - ++steps; - } - - stepsDone += (unsigned)steps; - stepsRemain -= steps; - return p; -} - -template inline Position Searcher::jumpY(Position p, int dy) -{ - JPS_ASSERT(dy); - JPS_ASSERT(grid(p.x, p.y)); - - const unsigned x = p.x; - const Position& endpos = endNode->pos; - const int skip = this->skip; - int steps = 0; - - unsigned a = ~((!!grid(x+skip, p.y)) | ((!!grid(x-skip, p.y)) << 1)); - - while(true) - { - const unsigned yy = p.y + dy; - const unsigned b = (!!grid(x+skip, yy)) | ((!!grid(x-skip, yy)) << 1); - - if((a & b) || p == endpos) - break; - if(!grid(x, yy)) - { - p = npos; - break; - } - - p.y += dy; - a = ~b; - } - - stepsDone += (unsigned)steps; - stepsRemain -= steps; - return p; -} -#endif // JPS_ASTAR_ONLY - -#ifdef JPS_VERIFY -// Recursive reference implementation -- for comparison only -template Position Searcher::jumpPRec(const Position& p, const Position& src) const -{ - unsigned x = p.x; - unsigned y = p.y; - if(!grid(x, y)) - return npos; - if(p == endNode->pos) - return p; - - int dx = int(x - src.x); - int dy = int(y - src.y); - JPS_ASSERT(dx || dy); - - if(dx && dy) - { - if( (grid(x-dx, y+dy) && !grid(x-dx, y)) || (grid(x+dx, y-dy) && !grid(x, y-dy)) ) - return p; - } - else if(dx) - { - if( (grid(x+dx, y+skip) && !grid(x, y+skip)) || (grid(x+dx, y-skip) && !grid(x, y-skip)) ) - return p; - } - else if(dy) - { - if( (grid(x+skip, y+dy) && !grid(x+skip, y)) || (grid(x-skip, y+dy) && !grid(x-skip, y)) ) - return p; - } - - if(dx && dy) - { - if(jumpPRec(Pos(x+dx, y), p).isValid()) - return p; - if(jumpPRec(Pos(x, y+dy), p).isValid()) - return p; - } - - if(grid(x+dx, y) || grid(x, y+dy)) - return jumpPRec(Pos(x+dx, y+dy), p); - - return npos; -} -#endif // JPS_VERIFY - -#define JPS_CHECKGRID(dx, dy) (grid(x+(dx), y+(dy))) -#define JPS_ADDPOS(dx, dy) do { *w++ = Pos(x+(dx), y+(dy)); } while(0) -#define JPS_ADDPOS_CHECK(dx, dy) do { if(JPS_CHECKGRID(dx, dy)) JPS_ADDPOS(dx, dy); } while(0) -#define JPS_ADDPOS_NO_TUNNEL(dx, dy) do { if(grid(x+(dx),y) || grid(x,y+(dy))) JPS_ADDPOS_CHECK(dx, dy); } while(0) - -#ifndef JPS_ASTAR_ONLY -template unsigned Searcher::findNeighbors(const Node *n, Position *wptr) const -{ - Position *w = wptr; - const unsigned x = n->pos.x; - const unsigned y = n->pos.y; - const int skip = this->skip; - - if(!n->parent) - { - // straight moves - JPS_ADDPOS_CHECK(-skip, 0); - JPS_ADDPOS_CHECK(0, -skip); - JPS_ADDPOS_CHECK(0, skip); - JPS_ADDPOS_CHECK(skip, 0); - - // diagonal moves + prevent tunneling - JPS_ADDPOS_NO_TUNNEL(-skip, -skip); - JPS_ADDPOS_NO_TUNNEL(-skip, skip); - JPS_ADDPOS_NO_TUNNEL(skip, -skip); - JPS_ADDPOS_NO_TUNNEL(skip, skip); - - return unsigned(w - wptr); - } - - // jump directions (both -1, 0, or 1) - int dx = int(x - n->parent->pos.x); - dx /= std::max(abs(dx), 1); - dx *= skip; - int dy = int(y - n->parent->pos.y); - dy /= std::max(abs(dy), 1); - dy *= skip; - - if(dx && dy) - { - // diagonal - // natural neighbors - bool walkX = false; - bool walkY = false; - if((walkX = grid(x+dx, y))) - *w++ = Pos(x+dx, y); - if((walkY = grid(x, y+dy))) - *w++ = Pos(x, y+dy); - - if(walkX || walkY) - JPS_ADDPOS_CHECK(dx, dy); - - // forced neighbors - if(walkY && !JPS_CHECKGRID(-dx,0)) - JPS_ADDPOS_CHECK(-dx, dy); - - if(walkX && !JPS_CHECKGRID(0,-dy)) - JPS_ADDPOS_CHECK(dx, -dy); - } - else if(dx) - { - // along X axis - if(JPS_CHECKGRID(dx, 0)) - { - JPS_ADDPOS(dx, 0); - - // Forced neighbors (+ prevent tunneling) - if(!JPS_CHECKGRID(0, skip)) - JPS_ADDPOS_CHECK(dx, skip); - if(!JPS_CHECKGRID(0,-skip)) - JPS_ADDPOS_CHECK(dx,-skip); - } - - - } - else if(dy) - { - // along Y axis - if(JPS_CHECKGRID(0, dy)) - { - JPS_ADDPOS(0, dy); - - // Forced neighbors (+ prevent tunneling) - if(!JPS_CHECKGRID(skip, 0)) - JPS_ADDPOS_CHECK(skip, dy); - if(!JPS_CHECKGRID(-skip, 0)) - JPS_ADDPOS_CHECK(-skip,dy); - } - } - - return unsigned(w - wptr); -} - -#else -//-------------- Plain old A* search ---------------- -template unsigned Searcher::findNeighborsAStar(const Node *n, Position *wptr) -{ - Position *w = wptr; - const int x = n->pos.x; - const int y = n->pos.y; - const int d = skip; - JPS_ADDPOS_NO_TUNNEL(-d, -d); - JPS_ADDPOS_CHECK ( 0, -d); - JPS_ADDPOS_NO_TUNNEL(+d, -d); - JPS_ADDPOS_CHECK (-d, 0); - JPS_ADDPOS_CHECK (+d, 0); - JPS_ADDPOS_NO_TUNNEL(-d, +d); - JPS_ADDPOS_CHECK ( 0, +d); - JPS_ADDPOS_NO_TUNNEL(+d, +d); - stepsDone += 8; - return unsigned(w - wptr); -} -#endif // JPS_ASTAR_ONLY -//------------------------------------------------- -#undef JPS_ADDPOS -#undef JPS_ADDPOS_CHECK -#undef JPS_ADDPOS_NO_TUNNEL -#undef JPS_CHECKGRID - - -template void Searcher::identifySuccessors(const Node *n) -{ - Position buf[8]; -#ifdef JPS_ASTAR_ONLY - const int num = findNeighborsAStar(n, &buf[0]); -#else - const int num = findNeighbors(n, &buf[0]); -#endif - for(int i = num-1; i >= 0; --i) - { - // Invariant: A node is only a valid neighbor if the corresponding grid position is walkable (asserted in jumpP) -#ifdef JPS_ASTAR_ONLY - Position jp = buf[i]; -#else - Position jp = jumpP(buf[i], n->pos); - #ifdef JPS_VERIFY - JPS_ASSERT(jp == jumpPRec(buf[i], n->pos)); - #endif - if(!jp.isValid()) - continue; -#endif - // Now that the grid position is definitely a valid jump point, we have to create the actual node. - Node *jn = getNode(jp); - JPS_ASSERT(jn && jn != n); - if(!jn->isClosed()) - { - unsigned extraG = Heuristic::Euclidean(jn, n); - unsigned newG = n->g + extraG; - if(!jn->isOpen() || newG < jn->g) - { - jn->g = newG; - jn->f = jn->g + Heuristic::Manhattan(jn, endNode); - jn->parent = n; - if(!jn->isOpen()) - { - open.push(jn); - jn->setOpen(); - } - else - open.fixup(); - } - } - } -} - -template bool Searcher::generatePath(PathVector& path, unsigned step) const -{ - if(!endNode) - return false; - size_t offset = path.size(); - if(step) - { - const Node *next = endNode; - const Node *prev = endNode->parent; - if(!prev) - return false; - do - { - const unsigned x = next->pos.x, y = next->pos.y; - int dx = int(prev->pos.x - x); - int dy = int(prev->pos.y - y); - JPS_ASSERT(!dx || !dy || abs(dx) == abs(dy)); // known to be straight, if diagonal - const int steps = std::max(abs(dx), abs(dy)); - dx /= std::max(abs(dx), 1); - dy /= std::max(abs(dy), 1); - dx *= int(step); - dy *= int(step); - int dxa = 0, dya = 0; - for(int i = 0; i < steps; i += step) - { - path.push_back(Pos(x+dxa, y+dya)); - dxa += dx; - dya += dy; - } - next = prev; - prev = prev->parent; - } - while (prev); - } - else - { - const Node *next = endNode; - if(!next->parent) - return false; - do - { - JPS_ASSERT(next != next->parent); - path.push_back(next->pos); - next = next->parent; - } - while (next->parent); - } - std::reverse(path.begin() + offset, path.end()); - return true; -} - -template bool Searcher::findPath(PathVector& path, Position start, Position end, unsigned step) -{ - Result res = findPathInit(start, end); - - // If this is true, the resulting path is empty (findPathFinish() would fail, so this needs to be checked before) - if(res == EMPTY_PATH) - return true; - - while(true) - { - switch(res) - { - case NEED_MORE_STEPS: - res = findPathStep(0); - break; // the switch - - case FOUND_PATH: - return findPathFinish(path, step); - - case NO_PATH: - default: - return false; - } - } -} - -template Result Searcher::findPathInit(Position start, Position end) -{ - for(NodeGrid::iterator it = nodegrid.begin(); it != nodegrid.end(); ++it) - it->second.clearState(); - open.clear(); - endNode = NULL; - stepsDone = 0; - - // If skip is > 1, make sure the points are aligned so that the search will always hit them - start.x = (start.x / skip) * skip; - start.y = (start.y / skip) * skip; - end.x = (end.x / skip) * skip; - end.y = (end.y / skip) * skip; - - if(start == end) - { - // There is only a path if this single position is walkable. - // But since the starting position is omitted, there is nothing to do here. - return grid(end.x, end.y) ? EMPTY_PATH : NO_PATH; - } - - // If start or end point are obstructed, don't even start - if(!grid(start.x, start.y) || !grid(end.x, end.y)) - return NO_PATH; - - endNode = getNode(end); - Node *startNode = getNode(start); - JPS_ASSERT(startNode && endNode); - -#ifndef JPS_DISABLE_GREEDY - // Try the quick way out first - if(findPathGreedy(startNode)) - return FOUND_PATH; -#endif - - open.push(startNode); - - return NEED_MORE_STEPS; -} - -template Result Searcher::findPathStep(int limit) -{ - stepsRemain = limit; - do - { - if(open.empty()) - return NO_PATH; - Node *n = open.pop(); - n->setClosed(); - if(n == endNode) - return FOUND_PATH; - identifySuccessors(n); - } - while(stepsRemain >= 0); - return NEED_MORE_STEPS; -} - -template bool Searcher::findPathFinish(PathVector& path, unsigned step) -{ - return generatePath(path, step); -} - -#ifndef JPS_DISABLE_GREEDY -template bool Searcher::findPathGreedy(Node *n) -{ - Position midpos = npos; - int x = n->pos.x; - int y = n->pos.y; - int ex = endNode->pos.x; - int ey = endNode->pos.y; - - JPS_ASSERT(x != ex || y != ey); // must not be called when start==end - JPS_ASSERT(n != endNode); - - const int skip = this->skip; - - int dx = int(ex - x); - int dy = int(ey - y); - const int adx = abs(dx); - const int ady = abs(dy); - dx /= std::max(adx, 1); - dy /= std::max(ady, 1); - dx *= skip; - dy *= skip; - - // go diagonally first - if(x != ex && y != ey) - { - JPS_ASSERT(dx && dy); - const int minlen = (int)std::min(adx, ady); - const int tx = x + dx * minlen; - for( ; x != tx; ) - { - if(grid(x, y) && (grid(x+dx, y) || grid(x, y+dy))) // prevent tunneling as well - { - x += dx; - y += dy; - } - else - return false; - } - - if(!grid(x, y)) - return false; - - midpos = Pos(x, y); - } - - // at this point, we're aligned to at least one axis - JPS_ASSERT(x == ex || y == ey); - - if(!(x == ex && y == ey)) - { - while(x != ex) - if(!grid(x += dx, y)) - return false; - - while(y != ey) - if(!grid(x, y += dy)) - return false; - - JPS_ASSERT(x == ex && y == ey); - } - - if(midpos.isValid()) - { - Node *mid = getNode(midpos); - JPS_ASSERT(mid && mid != n); - mid->parent = n; - if(mid != endNode) - endNode->parent = mid; - } - else - endNode->parent = n; - - return true; -} -#endif - -template void Searcher::freeMemory() -{ - NodeGrid v; - nodegrid.swap(v); - endNode = NULL; - open.clear(); - // other containers known to be empty. -} - -} // end namespace Internal - -using Internal::Searcher; - -// Single-call convenience function -// -// path: If the function returns true, the path is stored in this vector. -// The path does NOT contain the starting position, i.e. if start and end are the same, -// the resulting path has no elements. -// The vector does not have to be empty. The function does not clear it; -// instead, the new path positions are appended at the end. -// This allows building a path incrementally. -// -// grid: expected to overload operator()(x, y), return true if position is walkable, false if not. -// -// step: If 0, only return waypoints. -// If 1, create exhaustive step-by-step path. -// If N, put in one position for N blocks travelled, or when a waypoint is hit. -// All returned points are guaranteed to be on a straight line (vertically, horizontally, or diagonally), -// and there is no obstruction between any two consecutive points. -// Note that this parameter does NOT influence the pathfinding in any way; -// it only controls the coarseness of the output path. -// -// skip: If you know your map data well enough, this can be set to > 1 to speed up pathfinding even more. -// Warning: Start and end positions will be rounded down to the nearest -aligned position, -// so make sure to give appropriate positions so they do not end up in a wall. -// This will also skip through walls if they are less than blocks thick at any reachable position. -template bool findPath(PathVector& path, const GRID& grid, unsigned startx, unsigned starty, unsigned endx, unsigned endy, - unsigned step = 0, int skip = 0, // optional params - size_t *stepsDone = NULL, size_t *nodesExpanded = NULL // for information - ) -{ - Searcher search(grid); - search.setSkip(skip); - bool found = search.findPath(path, Pos(startx, starty), Pos(endx, endy), step); - if(stepsDone) - *stepsDone = search.getStepsDone(); - if(nodesExpanded) - *nodesExpanded = search.getNodesExpanded(); - return found; -} - - - -} // end namespace JPS - - -#endif diff --git a/ExternalLibs/jps.hh b/ExternalLibs/jps.hh new file mode 100644 index 0000000..fce8af7 --- /dev/null +++ b/ExternalLibs/jps.hh @@ -0,0 +1,1539 @@ +#pragma once + +/* +Public domain Jump Point Search implementation -- very fast pathfinding for uniform cost grids. +Scroll down for compile config, usage tips, example code. + +License: + Public domain, WTFPL, CC0 or your favorite permissive license; whatever is available in your country. + +Dependencies: + libc (stdlib.h, math.h) by default, change defines below to use your own functions. (realloc(), free(), sqrt()) + Compiles as C++98, does not require C++11 nor the STL. + Does not throw exceptions, works without RTTI, does not contain any virtual methods. + +Thread safety: + No global state. Searcher instances are not thread-safe. Grid template class is up to you. + If your grid access is read-only while pathfinding you may have many threads compute paths at the same time, + each with its own Searcher instance. + +Background: + If you want to generate paths on a map with the following properties: + - You have a 2D grid (exactly two dimensions!), where each tile has exactly 8 neighbors (up, down, left, right + diagonals) + - There is no "cost" -- a tile is either walkable, or not. + then you may want to avoid full fledged A* and go for Jump Point Search (this lib). + JPS is usually much faster than plain old A*, as long as your tile traversability check function is fast. + +Origin: + https://github.com/fgenesis/tinypile/blob/master/jps.hh + +Based on my older implementation: + https://github.com/fgenesis/jps/blob/master/JPS.h + (For changes compared to that version go to the end of this file) + +Inspired by: + http://users.cecs.anu.edu.au/~dharabor/data/papers/harabor-grastien-aaai11.pdf (The original paper) + https://github.com/Yonaba/Jumper + https://github.com/qiao/PathFinding.js + +Usage: + + Define a class that overloads `operator()(x, y) const`, returning a value that can be treated as boolean. + You are responsible for bounds checking! + You want your operator() to be as fast and small as possible, as it will be called a LOT. + Ask your compiler to force-inline it if possible. + +// --- Begin example code --- + +struct MyGrid +{ + inline bool operator()(unsigned x, unsigned y) const // coordinates must be unsigned; method must be const + { + if(x < width && y < height) // Unsigned will wrap if < 0 + ... return true if terrain at (x, y) is walkable. + // return false if terrain is not walkable or out-of-bounds. + } + unsigned width, height; +}; + +// Then you can retrieve a path: + +MyGrid grid(... set grid width, height, map data, whatever); + +const unsigned step = 0; // 0 compresses the path as much as possible and only records waypoints. + // Set this to 1 if you want a detailed single-step path + // (e.g. if you plan to further mangle the path yourself), + // or any other higher value to output every Nth position. + // (Waypoints are always output regardless of the step size.) + +JPS::PathVector path; // The resulting path will go here. + // You may also use std::vector or whatever, as long as your vector type + // has push_back(), begin(), end(), resize() methods (same semantics as std::vector). + // Note that the path will NOT include the starting position! + // --> If called with start == end it will report that a path has been found, + // but the resulting path vector will be empty! + +// Single-call interface: +// (Further remarks about this function can be found near the bottom of this file.) +// Note that the path vector is NOT cleared! New path points are appended at the end. +bool found = JPS::findPath(path, grid, startx, starty, endx, endy, step); + + +// --- Alternatively, if you want more control & efficiency for repeated pathfinding runs: --- + +// Use a Searcher instance (can be a class member, on the stack, ...) +// Make sure the passed grid reference stays valid throughout the searcher's lifetime. +// If you need control over memory allocation, you may pass an extra pointer that will be +// forwarded to your own JPS_realloc & JPS_free if you've set those. Otherwise it's ignored. +JPS::Searcher search(grid, userPtr = NULL); + +// build path incrementally from waypoints: +JPS::Position a, b, c, d = <...>; // set some waypoints +if (search.findPath(path, a, b) + && search.findPath(path, b, c) + && search.findPath(path, c, d)) +{ + // found path: a->b->c->d +} + +// keep re-using existing pathfinder instance +while(whatever) +{ + // Set startx, starty, endx, endy = <...> + if(!search.findPath(path, JPS::Pos(startx, starty), JPS::Pos(endx, endy), step)) + { + // ...handle failure... + } +} + +// If necessary, you may free internal memory -- this is never required; neither for performance, nor for correct function. +// If you do pathfinding after freeing memory, it'll allocate new memory. +// Note that freeing memory aborts any incremental search currently ongoing. +search.freeMemory(); + +// If you need to know how much memory is internally allocated by a searcher: +unsigned bytes = search.getTotalMemoryInUse(); + + +// ------------------------------- +// --- Incremental pathfinding --- +// ------------------------------- + +Calling JPS::findPath() or Searcher<>::findPath() always computes an entire path or returns failure. +If the path is long or costly and you have a tight CPU budget per frame you may want to perform pathfinding incrementally, +stretched over multiple frames. + +First, call + ### JPS_Result res = search.findPathInit(Position start, Position end) ### +Don't forget to check the return value, as it may return: +- JPS_NO_PATH if one or both of the points are obstructed +- JPS_EMPTY_PATH if the points are equal and not obstructed +- JPS_FOUND_PATH if the initial greedy heuristic could find a path quickly. +- JPS_OUT_OF_MEMORY if... well yeah. +If it returns JPS_NEED_MORE_STEPS then the next part can start. + +Repeatedly call + ### JPS_Result res = search.findPathStep(int limit) ### +until it returns JPS_NO_PATH or JPS_FOUND_PATH, or JPS_OUT_OF_MEMORY. +For consistency, you will want to ensure that the grid does not change between subsequent calls; +if the grid changes, parts of the path may go through a now obstructed area or may be no longer optimal. +If limit is 0, it will perform the pathfinding in one go. Values > 0 pause the search +as soon as possible after the number of steps was exceeded, returning NEED_MORE_STEPS. +Use search.getStepsDone() after some test runs to find a good value for the limit. + +After getting JPS_FOUND_PATH, generate the actual path points via + ### JPS_Result res = search.findPathFinish(PathVector& path, unsigned step = 0) ### +As described above, path points are appended, and granularity can be adjusted with the step parameter. +Returns JPS_FOUND_PATH if the path was successfully built and appended to the path vector. +Returns JPS_NO_PATH if the pathfinding did not finish or generating the path failed. +May return JPS_OUT_OF_MEMORY if the path vector must be resized but fails to allocate. + +If findPathInit() or findPathStep() return JPS_OUT_OF_MEMORY, the current searcher progress becomes undefined. +To recover, free some memory elsewhere and call findPathInit() to try again. + +If findPathFinish() returns out-of-memory but previous steps finished successfully, +then the found path is still valid for generating the path vector. +In that case you may call findPathFinish() again after making some memory available. + +If you do not worry about memory, treat JPS_OUT_OF_MEMORY as if JPS_NO_PATH was returned. + +You may pass JPS::PathVector, std::vector, or your own to findPathFinish(). +Note that if the path vector type you pass throws exceptions in case of allocation failures (std::vector does, for example), +you'll get that exception, and the path vector will be in whatever state it was in when the last element was successfully inserted. +If no exception is thrown (ie. you used JPS::PathVector) then the failure cases do not modify the path vector. + +You may abort a search anytime by starting a new one via findPathInit(), calling freeMemory(), or by destroying the searcher instance. +Aborting or starting a search resets the values returned by .getStepsDone() and .getNodesExpanded() to 0. + +*/ + +// ============================ +// ====== COMPILE CONFIG ====== +// ============================ + + +// If you want to avoid sqrt() or floats in general, define this. +// Turns out in some testing this was ~12% faster, so it's the default. +#define JPS_NO_FLOAT + + +// ------------------------------------------------ + +#include // for size_t (needed for operator new) + +// Assertions +#ifndef JPS_ASSERT +# ifdef _DEBUG +# include +# define JPS_ASSERT(cond) assert(cond) +# else +# define JPS_ASSERT(cond) +# endif +#endif + +// The default allocator uses realloc(), free(). Change if necessary. +// You will get the user pointer that you passed to findPath() or the Searcher ctor. +#if !defined(JPS_realloc) || !defined(JPS_free) +# include // for realloc, free +# ifndef JPS_realloc +# define JPS_realloc(p, newsize, oldsize, user) realloc(p, newsize) +# endif +# ifndef JPS_free +# define JPS_free(p, oldsize, user) free(p) +# endif +#endif + +#ifdef JPS_NO_FLOAT +#define JPS_HEURISTIC_ACCURATE(a, b) (Heuristic::Chebyshev(a, b)) +#else +# ifndef JPS_sqrt +// for Euclidean heuristic. +# include +# define JPS_sqrt(x) sqrtf(float(x)) // float cast here avoids a warning about implicit int->float cast +# endif +#endif + +// Which heuristics to use. +// Basic property: Distance estimate, returns values >= 0. Smaller is better. +// The accurate heuristic should always return guesses less or equal than the estimate heuristic, +// otherwise the resulting paths may not be optimal. +// (The rule of thumb is that the estimate is fast but can overestimate) +// For the implementation of heuristics, scroll down. +#ifndef JPS_HEURISTIC_ACCURATE +#define JPS_HEURISTIC_ACCURATE(a, b) (Heuristic::Euclidean(a, b)) +#endif + +#ifndef JPS_HEURISTIC_ESTIMATE +#define JPS_HEURISTIC_ESTIMATE(a, b) (Heuristic::Manhattan(a, b)) +#endif + + +// --- Data types --- +namespace JPS { + +// unsigned integer type wide enough to store a position on one grid axis. +// Note that on x86, u32 is actually faster than u16. +typedef unsigned PosType; + +// Result of heuristics. can also be (unsigned) int but using float by default since that's what sqrtf() returns +// and we don't need to cast float->int that way. Change if you use integer-only heuristics. +// (Euclidean heuristic using sqrt() works fine even if cast to int. Your choice really.) +#ifdef JPS_NO_FLOAT +typedef int ScoreType; +#else +typedef float ScoreType; +#endif + +// Size type; used internally for vectors and the like. You can set this to size_t if you want, but 32 bits is more than enough. +typedef unsigned SizeT; + +} // end namespace JPS + + +// ================================ +// ====== COMPILE CONFIG END ====== +// ================================ +// ---------------------------------------------------------------------------------------- + +typedef unsigned JPS_Flags; +enum JPS_Flags_ +{ + // No special behavior + JPS_Flag_Default = 0x00, + + // If this is defined, disable the greedy direct-short-path check that avoids the large area scanning that JPS does. + // This is just a performance tweak. May save a lot of CPU when constantly re-planning short paths without obstacles + // (e.g. an entity follows close behind another). + // Does not change optimality of results. If you perform your own line-of-sight checks + // before starting a pathfinding run you can disable greedy since checking twice isn't needed, + // but otherwise it's better to leave it enabled. + JPS_Flag_NoGreedy = 0x01, + + // If this is set, use standard A* instead of JPS (e.g. if you want to compare performance in your scenario). + // In most cases this will be MUCH slower, but might be beneficial if your grid lookup + // is slow (aka worse than O(1) or more than a few inlined instructions), + // as it avoids the large area scans that the JPS algorithm does. + // (Also increases memory usage as each checked position is expanded into a node.) + JPS_Flag_AStarOnly = 0x02, + + // Don't check whether start position is walkable. + // This makes the start position always walkable, even if the map data say otherwise. + JPS_Flag_NoStartCheck = 0x04, + + // Don't check whether end position is walkable. + JPS_Flag_NoEndCheck = 0x08, +}; + +enum JPS_Result +{ + JPS_NO_PATH, + JPS_FOUND_PATH, + JPS_NEED_MORE_STEPS, + JPS_EMPTY_PATH, + JPS_OUT_OF_MEMORY +}; + +// operator new() without #include +// Unfortunately the standard mandates the use of size_t, so we need stddef.h the very least. +// Trick via https://github.com/ocornut/imgui +// "Defining a custom placement new() with a dummy parameter allows us to bypass including +// which on some platforms complains when user has disabled exceptions." +struct JPS__NewDummy {}; +inline void* operator new(size_t, JPS__NewDummy, void* ptr) { return ptr; } +inline void operator delete(void*, JPS__NewDummy, void*) {} +#define JPS_PLACEMENT_NEW(p) new(JPS__NewDummy(), p) + + +namespace JPS { + +struct Position +{ + PosType x, y; + + inline bool operator==(const Position& p) const + { + return x == p.x && y == p.y; + } + inline bool operator!=(const Position& p) const + { + return x != p.x || y != p.y; + } + + inline bool isValid() const { return x != PosType(-1); } +}; + +// The invalid position. Used internally to mark non-walkable points. +static const Position npos = {PosType(-1), PosType(-1)}; +static const SizeT noidx = SizeT(-1); + +// ctor function to keep Position a real POD struct. +inline static Position Pos(PosType x, PosType y) +{ + Position p; + p.x = x; + p.y = y; + return p; +} + +template inline static T Max(T a, T b) { return a < b ? b : a; } +template inline static T Min(T a, T b) { return a < b ? a : b; } +template inline static T Abs(T a) { return a < T(0) ? -a : a; } +template inline static int Sgn(T val) { return (T(0) < val) - (val < T(0)); } + + +// Heuristics. Add new ones if you need them. +namespace Heuristic +{ + inline ScoreType Manhattan(const Position& a, const Position& b) + { + const int dx = Abs(int(a.x - b.x)); + const int dy = Abs(int(a.y - b.y)); + return static_cast(dx + dy); + } + + inline ScoreType Chebyshev(const Position& a, const Position& b) + { + const int dx = Abs(int(a.x - b.x)); + const int dy = Abs(int(a.y - b.y)); + return static_cast(Max(dx, dy)); + } +#ifdef JPS_sqrt + inline ScoreType Euclidean(const Position& a, const Position& b) + { + const int dx = (int(a.x - b.x)); + const int dy = (int(a.y - b.y)); + return static_cast(JPS_sqrt(dx*dx + dy*dy)); + } +#endif +} // end namespace heuristic + + + +// --- Begin infrastructure, data structures --- + +namespace Internal { + +// Never allocated outside of a PodVec --> All nodes are linearly adjacent in memory. +struct Node +{ + ScoreType f, g; // heuristic distances + Position pos; + int parentOffs; // no parent if 0 + unsigned _flags; + + inline int hasParent() const { return parentOffs; } + inline void setOpen() { _flags |= 1; } + inline void setClosed() { _flags |= 2; } + inline unsigned isOpen() const { return _flags & 1; } + inline unsigned isClosed() const { return _flags & 2; } + + // We know nodes are allocated sequentially in memory, so this is fine. + inline Node& getParent() { JPS_ASSERT(parentOffs); return this[parentOffs]; } + inline const Node& getParent() const { JPS_ASSERT(parentOffs); return this[parentOffs]; } + inline const Node *getParentOpt() const { return parentOffs ? this + parentOffs : 0; } + inline void setParent(const Node& p) { JPS_ASSERT(&p != this); parentOffs = static_cast(&p - this); } +}; + +template +class PodVec +{ +public: + PodVec(void *user = 0) + : _data(0), used(0), cap(0), _user(user) + {} + ~PodVec() { dealloc(); } + inline void clear() + { + used = 0; + } + void dealloc() + { + JPS_free(_data, cap * sizeof(T), _user); + _data = 0; + used = 0; + cap = 0; + } + T *alloc() + { + T *e = 0; + if(used < cap || _grow()) + { + e = _data + used; + ++used; + } + return e; + } + inline void push_back(const T& e) + { + if(T *dst = alloc()) // yes, this silently fails when OOM. this is handled internally. + *dst = e; + } + inline void pop_back() { JPS_ASSERT(used); --used; } + inline T& back() { JPS_ASSERT(used); return _data[used-1]; } + inline SizeT size() const { return used; } + inline bool empty() const { return !used; } + inline T *data() { return _data; } + inline const T *data() const { return _data; } + inline T& operator[](size_t idx) const { JPS_ASSERT(idx < used); return _data[idx]; } + inline SizeT getindex(const T *e) const + { + JPS_ASSERT(e && _data <= e && e < _data + used); + return static_cast(e - _data); + } + + void *_reserve(SizeT newcap) // for internal use + { + return cap < newcap ? _grow(newcap) : _data; + } + void resize(SizeT sz) + { + if(_reserve(sz)) + used = sz; + } + SizeT _getMemSize() const + { + return cap * sizeof(T); + } + + // minimal iterator interface + typedef T* iterator; + typedef const T* const_iterator; + typedef SizeT size_type; + typedef T value_type; + inline iterator begin() { return data(); } + inline iterator end() { return data() + size(); } + inline const_iterator cbegin() const { return data(); } + inline const_iterator cend() const { return data() + size(); } + +private: + void *_grow(SizeT newcap) + { + void *p = JPS_realloc(_data, newcap * sizeof(T), cap * sizeof(T), _user); + if(p) + { + _data = (T*)p; + cap = newcap; + } + return p; + } + void * _grow() + { + const SizeT newcap = cap + (cap / 2) + 32; + return _grow(newcap); + } + T *_data; + SizeT used, cap; + +public: + void * const _user; + +private: + // forbid ops + PodVec& operator=(const PodVec&); + PodVec(const PodVec&); +}; + +template +inline static void Swap(T& a, T& b) +{ + const T tmp = a; + a = b; + b = tmp; +} + +template +inline static void Reverse(IT first, IT last) +{ + while((first != last) && (first != --last)) + { + Swap(*first, *last); + ++first; + } +} + +typedef PodVec Storage; + + +class NodeMap +{ +private: + static const unsigned LOAD_FACTOR = 8; // estimate: {CPU cache line size (64)} / sizeof(HashLoc) + static const unsigned INITIAL_BUCKETS = 16; // must be > 1 and power of 2 + + struct HashLoc + { + unsigned hash2; // for early-out check only + SizeT idx; // index in central storage + }; + typedef PodVec Bucket; + + // hash function to determine bucket. only uses lower few bits. should jumble lower bits nicely. + static inline unsigned Hash(PosType x, PosType y) + { + return x ^ y; + } + + // hash function designed to lose as little data as possible. for early-out checks. all bits used. + static inline unsigned Hash2(PosType x, PosType y) + { + return (y << 16) ^ x; + } + +public: + + NodeMap(Storage& storage) + : _storageRef(storage), _buckets(storage._user) + {} + + ~NodeMap() + { + dealloc(); + } + + void dealloc() + { + for(SizeT i = 0; i < _buckets.size(); ++i) + _buckets[i].~Bucket(); + _buckets.dealloc(); + } + void clear() + { + // clear the buckets, but *not* the bucket vector + for(SizeT i = 0; i < _buckets.size(); ++i) + _buckets[i].clear(); + } + + Node *operator()(PosType x, PosType y) + { + const unsigned h = Hash(x, y); + const unsigned h2 = Hash2(x, y); + const SizeT ksz = _buckets.size(); // known to be power-of-2 + Bucket *b = 0; // MSVC /W4 complains that this was uninitialized and used, so we init it... + if (ksz) + { + b = &_buckets[h & (ksz - 1)]; + const SizeT bsz = b->size(); + const HashLoc * const bdata = b->data(); + for (SizeT i = 0; i < bsz; ++i) + { + // this is the only place where HashLoc::hash2 is used; it *could*be removed, which means: + // - twice as much space for indexes per cache line + // - but also higher chances for a cache miss because for each entry in the bucket we still need to check the node's X/Y coords, + // and we'll likely end up in a random location in RAM for each node. + // Quick benchmarking showed that *with* the hash2 check it's almost immeasurably (less than 1%) faster. + if (bdata[i].hash2 == h2) + { + Node &n = _storageRef[bdata[i].idx]; + if(n.pos.x == x && n.pos.y == y) + return &n; + } + } + } + + // enlarge hashmap if necessary; fix bucket if so + SizeT newbsz = _enlarge(); + if(newbsz > 1) + b = &_buckets[h & (newbsz - 1)]; + else if(newbsz == 1) // error case + return 0; + + HashLoc *loc = b->alloc(); // ... see above. b is always initialized here. when ksz==0, _enlarge() will do its initial allocation, so it can never return 0. + + if(!loc) + return 0; + + loc->hash2 = h2; + loc->idx = _storageRef.size(); + + // no node at (x, y), create new one + Node *n = _storageRef.alloc(); + if(n) + { + n->f = 0; + n->g = 0; + n->pos.x = x; + n->pos.y = y; + n->parentOffs = 0; + n->_flags = 0; + } + return n; + } + + SizeT _getMemSize() const + { + SizeT sum = _buckets._getMemSize(); + for(Buckets::const_iterator it = _buckets.cbegin(); it != _buckets.cend(); ++it) + sum += it->_getMemSize(); + return sum; + } + +private: + + // return values: 0 = nothing to do; 1 = error; >1: internal storage was enlarged to this many buckets + SizeT _enlarge() + { + const SizeT n = _storageRef.size(); + const SizeT oldsz = _buckets.size(); + if (n < oldsz * LOAD_FACTOR) + return 0; + + // pre-allocate bucket storage that we're going to use + const SizeT newsz = oldsz ? oldsz * 2 : INITIAL_BUCKETS; // stays power of 2 + + if(!_buckets._reserve(newsz)) + return 0; // early out if realloc fails; this not a problem and we can continue. + + // forget everything + for(SizeT i = 0; i < oldsz; ++i) + _buckets[i].clear(); + + // resize and init + for(SizeT i = oldsz; i < newsz; ++i) + { + void *p = _buckets.alloc(); // can't fail since the space was reserved + JPS_PLACEMENT_NEW(p) PodVec(_buckets._user); + } + + const SizeT mask = _buckets.size() - 1; + for(SizeT i = 0; i < n; ++i) + { + const Position p = _storageRef[i].pos; + HashLoc *loc = _buckets[Hash(p.x, p.y) & mask].alloc(); + if(!loc) + return 1; // error case + + loc->hash2 = Hash2(p.x, p.y); + loc->idx = i; + } + return newsz; + } + + Storage& _storageRef; + typedef PodVec Buckets; + Buckets _buckets; +}; + +class OpenList +{ +private: + const Storage& _storageRef; + PodVec idxHeap; + +public: + + OpenList(const Storage& storage) + : _storageRef(storage), idxHeap(storage._user) + {} + + + inline void pushNode(Node *n) + { + _heapPushIdx(_storageRef.getindex(n)); + } + + inline Node& popNode() + { + return _storageRef[_popIdx()]; + } + + // re-heapify after node changed its order + inline void fixNode(const Node& n) + { + const unsigned ni = _storageRef.getindex(&n); + const unsigned sz = idxHeap.size(); + unsigned *p = idxHeap.data(); + for(unsigned i = 0; i < sz; ++i) // TODO: if this ever becomes a perf bottleneck: make it so that each node knows its heap index + if(p[i] == ni) + { + _fixIdx(i); + return; + } + JPS_ASSERT(false); // expect node to be found + } + + inline void dealloc() { idxHeap.dealloc(); } + inline void clear() { idxHeap.clear(); } + inline bool empty() const { return idxHeap.empty(); } + + inline SizeT _getMemSize() const + { + return idxHeap._getMemSize(); + } + +private: + + inline bool _heapLess(SizeT a, SizeT b) + { + return _storageRef[idxHeap[a]].f > _storageRef[idxHeap[b]].f; + } + + inline bool _heapLessIdx(SizeT a, SizeT idx) + { + return _storageRef[idxHeap[a]].f > _storageRef[idx].f; + } + + void _percolateUp(SizeT i) + { + const SizeT idx = idxHeap[i]; + SizeT p; + goto start; + do + { + idxHeap[i] = idxHeap[p]; // parent is smaller, move it down + i = p; // continue with parent +start: + p = (i - 1) >> 1; + } + while(i && _heapLessIdx(p, idx)); + idxHeap[i] = idx; // found correct place for idx + } + + void _percolateDown(SizeT i) + { + const SizeT idx = idxHeap[i]; + const SizeT sz = idxHeap.size(); + SizeT child; + goto start; + do + { + // pick right sibling if exists and larger or equal + if(child + 1 < sz && !_heapLess(child+1, child)) + ++child; + idxHeap[i] = idxHeap[child]; + i = child; +start: + child = (i << 1) + 1; + } + while(child < sz); + idxHeap[i] = idx; + _percolateUp(i); + } + + void _heapPushIdx(SizeT idx) + { + SizeT i = idxHeap.size(); + idxHeap.push_back(idx); + _percolateUp(i); + } + + SizeT _popIdx() + { + SizeT sz = idxHeap.size(); + JPS_ASSERT(sz); + const SizeT root = idxHeap[0]; + idxHeap[0] = idxHeap[--sz]; + idxHeap.pop_back(); + if(sz > 1) + _percolateDown(0); + return root; + } + + // re-heapify node at index i + inline void _fixIdx(SizeT i) + { + _percolateDown(i); + _percolateUp(i); + } +}; + +#undef JPS_PLACEMENT_NEW + +// --- End infrastructure, data structures --- + +// All those things that don't depend on template parameters... +class SearcherBase +{ +protected: + Storage storage; + OpenList open; + NodeMap nodemap; + + Position endPos; + SizeT endNodeIdx; + JPS_Flags flags; + int stepsRemain; + SizeT stepsDone; + + + SearcherBase(void *user) + : storage(user) + , open(storage) + , nodemap(storage) + , endPos(npos), endNodeIdx(noidx) + , flags(0) + , stepsRemain(0), stepsDone(0) + {} + + void clear() + { + open.clear(); + nodemap.clear(); + storage.clear(); + endNodeIdx = noidx; + stepsDone = 0; + } + + void _expandNode(const Position jp, Node& jn, const Node& parent) + { + JPS_ASSERT(jn.pos == jp); + ScoreType extraG = JPS_HEURISTIC_ACCURATE(jp, parent.pos); + ScoreType newG = parent.g + extraG; + if(!jn.isOpen() || newG < jn.g) + { + jn.g = newG; + jn.f = jn.g + JPS_HEURISTIC_ESTIMATE(jp, endPos); + jn.setParent(parent); + if(!jn.isOpen()) + { + open.pushNode(&jn); + jn.setOpen(); + } + else + open.fixNode(jn); + } + } + +public: + + template + JPS_Result generatePath(PV& path, unsigned step) const; + + void freeMemory() + { + open.dealloc(); + nodemap.dealloc(); + storage.dealloc(); + endNodeIdx = noidx; + } + + // --- Statistics --- + + inline SizeT getStepsDone() const { return stepsDone; } + inline SizeT getNodesExpanded() const { return storage.size(); } + + SizeT getTotalMemoryInUse() const + { + return storage._getMemSize() + + nodemap._getMemSize() + + open._getMemSize(); + } +}; + +template class Searcher : public SearcherBase +{ +public: + Searcher(const GRID& g, void *user = 0) + : SearcherBase(user), grid(g) + {} + + // single-call + template + bool findPath(PV& path, Position start, Position end, unsigned step, JPS_Flags flags = JPS_Flag_Default); + + // incremental pathfinding + JPS_Result findPathInit(Position start, Position end, JPS_Flags flags = JPS_Flag_Default); + JPS_Result findPathStep(int limit); + // generate path after one was found + template + JPS_Result findPathFinish(PV& path, unsigned step) const; + +private: + + const GRID& grid; + + Node *getNode(const Position& pos); + bool identifySuccessors(const Node& n); + + bool findPathGreedy(Node *start, Node *end); + + unsigned findNeighborsAStar(const Node& n, Position *wptr); + + unsigned findNeighborsJPS(const Node& n, Position *wptr) const; + Position jumpP(const Position& p, const Position& src); + Position jumpD(Position p, int dx, int dy); + Position jumpX(Position p, int dx); + Position jumpY(Position p, int dy); + + // forbid any ops + Searcher& operator=(const Searcher&); + Searcher(const Searcher&); +}; + + +// ----------------------------------------------------------------------- + +template JPS_Result SearcherBase::generatePath(PV& path, unsigned step) const +{ + if(endNodeIdx == noidx) + return JPS_NO_PATH; + const SizeT offset = path.size(); + SizeT added = 0; + const Node& endNode = storage[endNodeIdx]; + const Node *next = &endNode; + if(!next->hasParent()) + return JPS_NO_PATH; + if(step) + { + const Node *prev = endNode.getParentOpt(); + if(!prev) + return JPS_NO_PATH; + do + { + const unsigned x = next->pos.x, y = next->pos.y; + int dx = int(prev->pos.x - x); + int dy = int(prev->pos.y - y); + const int adx = Abs(dx); + const int ady = Abs(dy); + JPS_ASSERT(!dx || !dy || adx == ady); // known to be straight, if diagonal + const int steps = Max(adx, ady); + dx = int(step) * Sgn(dx); + dy = int(step) * Sgn(dy); + int dxa = 0, dya = 0; + for(int i = 0; i < steps; i += step) + { + path.push_back(Pos(x+dxa, y+dya)); + ++added; + dxa += dx; + dya += dy; + } + next = prev; + prev = prev->getParentOpt(); + } + while (prev); + } + else + { + do + { + JPS_ASSERT(next != &next->getParent()); + path.push_back(next->pos); + ++added; + next = &next->getParent(); + } + while (next->hasParent()); + } + + // JPS::PathVector silently discards push_back() when memory allocation fails; + // detect that case and roll back. + if(path.size() != offset + added) + { + path.resize(offset); + return JPS_OUT_OF_MEMORY; + } + + // Nodes were traversed backwards, fix that + Reverse(path.begin() + offset, path.end()); + return JPS_FOUND_PATH; +} + +//----------------------------------------- + +template inline Node *Searcher::getNode(const Position& pos) +{ + JPS_ASSERT(grid(pos.x, pos.y)); + return nodemap(pos.x, pos.y); +} + +template Position Searcher::jumpP(const Position &p, const Position& src) +{ + JPS_ASSERT(grid(p.x, p.y)); + + int dx = int(p.x - src.x); + int dy = int(p.y - src.y); + JPS_ASSERT(dx || dy); + + if(dx && dy) + return jumpD(p, dx, dy); + else if(dx) + return jumpX(p, dx); + else if(dy) + return jumpY(p, dy); + + // not reached + JPS_ASSERT(false); + return npos; +} + +template Position Searcher::jumpD(Position p, int dx, int dy) +{ + JPS_ASSERT(grid(p.x, p.y)); + JPS_ASSERT(dx && dy); + + const Position endpos = endPos; + unsigned steps = 0; + + while(true) + { + if(p == endpos) + break; + + ++steps; + const PosType x = p.x; + const PosType y = p.y; + + if( (grid(x-dx, y+dy) && !grid(x-dx, y)) || (grid(x+dx, y-dy) && !grid(x, y-dy)) ) + break; + + const bool gdx = !!grid(x+dx, y); + const bool gdy = !!grid(x, y+dy); + + if(gdx && jumpX(Pos(x+dx, y), dx).isValid()) + break; + + if(gdy && jumpY(Pos(x, y+dy), dy).isValid()) + break; + + if((gdx || gdy) && grid(x+dx, y+dy)) + { + p.x += dx; + p.y += dy; + } + else + { + p = npos; + break; + } + } + stepsDone += steps; + stepsRemain -= steps; + return p; +} + +template inline Position Searcher::jumpX(Position p, int dx) +{ + JPS_ASSERT(dx); + JPS_ASSERT(grid(p.x, p.y)); + + const PosType y = p.y; + const Position endpos = endPos; + unsigned steps = 0; + + unsigned a = ~((!!grid(p.x, y+1)) | ((!!grid(p.x, y-1)) << 1)); + + while(true) + { + const unsigned xx = p.x + dx; + const unsigned b = (!!grid(xx, y+1)) | ((!!grid(xx, y-1)) << 1); + + if((b & a) || p == endpos) + break; + if(!grid(xx, y)) + { + p = npos; + break; + } + + p.x += dx; + a = ~b; + ++steps; + } + + stepsDone += steps; + stepsRemain -= steps; + return p; +} + +template inline Position Searcher::jumpY(Position p, int dy) +{ + JPS_ASSERT(dy); + JPS_ASSERT(grid(p.x, p.y)); + + const PosType x = p.x; + const Position endpos = endPos; + unsigned steps = 0; + + unsigned a = ~((!!grid(x+1, p.y)) | ((!!grid(x-1, p.y)) << 1)); + + while(true) + { + const unsigned yy = p.y + dy; + const unsigned b = (!!grid(x+1, yy)) | ((!!grid(x-1, yy)) << 1); + + if((a & b) || p == endpos) + break; + if(!grid(x, yy)) + { + p = npos; + break; + } + + p.y += dy; + a = ~b; + ++steps; + } + + stepsDone += steps; + stepsRemain -= steps; + return p; +} + +#define JPS_CHECKGRID(dx, dy) (grid(x+(dx), y+(dy))) +#define JPS_ADDPOS(dx, dy) do { *w++ = Pos(x+(dx), y+(dy)); } while(0) +#define JPS_ADDPOS_CHECK(dx, dy) do { if(JPS_CHECKGRID(dx, dy)) JPS_ADDPOS(dx, dy); } while(0) +#define JPS_ADDPOS_NO_TUNNEL(dx, dy) do { if(grid(x+(dx),y) || grid(x,y+(dy))) JPS_ADDPOS_CHECK(dx, dy); } while(0) + +template unsigned Searcher::findNeighborsJPS(const Node& n, Position *wptr) const +{ + Position *w = wptr; + const unsigned x = n.pos.x; + const unsigned y = n.pos.y; + + if(!n.hasParent()) + { + // straight moves + JPS_ADDPOS_CHECK(-1, 0); + JPS_ADDPOS_CHECK(0, -1); + JPS_ADDPOS_CHECK(0, 1); + JPS_ADDPOS_CHECK(1, 0); + + // diagonal moves + prevent tunneling + JPS_ADDPOS_NO_TUNNEL(-1, -1); + JPS_ADDPOS_NO_TUNNEL(-1, 1); + JPS_ADDPOS_NO_TUNNEL(1, -1); + JPS_ADDPOS_NO_TUNNEL(1, 1); + + return unsigned(w - wptr); + } + const Node& p = n.getParent(); + // jump directions (both -1, 0, or 1) + const int dx = Sgn(x - p.pos.x); + const int dy = Sgn(y - p.pos.y); + + if(dx && dy) + { + // diagonal + // natural neighbors + const bool walkX = !!grid(x+dx, y); + if(walkX) + *w++ = Pos(x+dx, y); + const bool walkY = !!grid(x, y+dy); + if(walkY) + *w++ = Pos(x, y+dy); + + if(walkX || walkY) + JPS_ADDPOS_CHECK(dx, dy); + + // forced neighbors + if(walkY && !JPS_CHECKGRID(-dx,0)) + JPS_ADDPOS_CHECK(-dx, dy); + + if(walkX && !JPS_CHECKGRID(0,-dy)) + JPS_ADDPOS_CHECK(dx, -dy); + } + else if(dx) + { + // along X axis + if(JPS_CHECKGRID(dx, 0)) + { + JPS_ADDPOS(dx, 0); + + // Forced neighbors (+ prevent tunneling) + if(!JPS_CHECKGRID(0, 1)) + JPS_ADDPOS_CHECK(dx, 1); + if(!JPS_CHECKGRID(0,-1)) + JPS_ADDPOS_CHECK(dx,-1); + } + } + else if(dy) + { + // along Y axis + if(JPS_CHECKGRID(0, dy)) + { + JPS_ADDPOS(0, dy); + + // Forced neighbors (+ prevent tunneling) + if(!JPS_CHECKGRID(1, 0)) + JPS_ADDPOS_CHECK(1, dy); + if(!JPS_CHECKGRID(-1, 0)) + JPS_ADDPOS_CHECK(-1,dy); + } + } + + return unsigned(w - wptr); +} + +//-------------- Plain old A* search ---------------- +template unsigned Searcher::findNeighborsAStar(const Node& n, Position *wptr) +{ + Position *w = wptr; + const int x = n.pos.x; + const int y = n.pos.y; + const int d = 1; + JPS_ADDPOS_NO_TUNNEL(-d, -d); + JPS_ADDPOS_CHECK ( 0, -d); + JPS_ADDPOS_NO_TUNNEL(+d, -d); + JPS_ADDPOS_CHECK (-d, 0); + JPS_ADDPOS_CHECK (+d, 0); + JPS_ADDPOS_NO_TUNNEL(-d, +d); + JPS_ADDPOS_CHECK ( 0, +d); + JPS_ADDPOS_NO_TUNNEL(+d, +d); + stepsDone += 8; + return unsigned(w - wptr); +} + +//------------------------------------------------- +#undef JPS_ADDPOS +#undef JPS_ADDPOS_CHECK +#undef JPS_ADDPOS_NO_TUNNEL +#undef JPS_CHECKGRID + + +template bool Searcher::identifySuccessors(const Node& n_) +{ + const SizeT nidx = storage.getindex(&n_); + const Position np = n_.pos; + Position buf[8]; + + const int num = (flags & JPS_Flag_AStarOnly) + ? findNeighborsAStar(n_, &buf[0]) + : findNeighborsJPS(n_, &buf[0]); + + for(int i = num-1; i >= 0; --i) + { + // Invariant: A node is only a valid neighbor if the corresponding grid position is walkable (asserted in jumpP) + Position jp; + if(flags & JPS_Flag_AStarOnly) + jp = buf[i]; + else + { + jp = jumpP(buf[i], np); + if(!jp.isValid()) + continue; + } + // Now that the grid position is definitely a valid jump point, we have to create the actual node. + Node *jn = getNode(jp); // this might realloc the storage + if(!jn) + return false; // out of memory + + Node& n = storage[nidx]; // get valid ref in case we realloc'd + JPS_ASSERT(jn != &n); + if(!jn->isClosed()) + _expandNode(jp, *jn, n); + } + return true; +} + +template template bool Searcher::findPath(PV& path, Position start, Position end, unsigned step, JPS_Flags flags) +{ + JPS_Result res = findPathInit(start, end, flags); + + // If this is true, the resulting path is empty (findPathFinish() would fail, so this needs to be checked before) + if(res == JPS_EMPTY_PATH) + return true; + + while(true) + { + switch(res) + { + case JPS_NEED_MORE_STEPS: + res = findPathStep(0); + break; // the switch + + case JPS_FOUND_PATH: + return findPathFinish(path, step) == JPS_FOUND_PATH; + + case JPS_EMPTY_PATH: + JPS_ASSERT(false); // can't happen + // fall through + case JPS_NO_PATH: + case JPS_OUT_OF_MEMORY: + return false; + } + } +} + +template JPS_Result Searcher::findPathInit(Position start, Position end, JPS_Flags flags) +{ + // This just resets a few counters; container memory isn't touched + this->clear(); + + this->flags = flags; + endPos = end; + + // FIXME: check this + if(start == end && !(flags & (JPS_Flag_NoStartCheck|JPS_Flag_NoEndCheck))) + { + // There is only a path if this single position is walkable. + // But since the starting position is omitted in the output, there is nothing to do here. + return grid(end.x, end.y) ? JPS_EMPTY_PATH : JPS_NO_PATH; + } + + if(!(flags & JPS_Flag_NoStartCheck)) + if(!grid(start.x, start.y)) + return JPS_NO_PATH; + + if(!(flags & JPS_Flag_NoEndCheck)) + if(!grid(end.x, end.y)) + return JPS_NO_PATH; + + Node *endNode = getNode(end); // this might realloc the internal storage... + if(!endNode) + return JPS_OUT_OF_MEMORY; + endNodeIdx = storage.getindex(endNode); // .. so we keep this for later + + Node *startNode = getNode(start); // this might also realloc + if(!startNode) + return JPS_OUT_OF_MEMORY; + endNode = &storage[endNodeIdx]; // startNode is valid, make sure that endNode is valid too in case we reallocated + + if(!(flags & JPS_Flag_NoGreedy)) + { + // Try the quick way out first + if(findPathGreedy(startNode, endNode)) + return JPS_FOUND_PATH; + } + + open.pushNode(startNode); + + return JPS_NEED_MORE_STEPS; +} + +template JPS_Result Searcher::findPathStep(int limit) +{ + stepsRemain = limit; + do + { + if(open.empty()) + return JPS_NO_PATH; + Node& n = open.popNode(); + n.setClosed(); + if(n.pos == endPos) + return JPS_FOUND_PATH; + if(!identifySuccessors(n)) + return JPS_OUT_OF_MEMORY; + } + while(stepsRemain >= 0); + return JPS_NEED_MORE_STEPS; +} + +template template JPS_Result Searcher::findPathFinish(PV& path, unsigned step) const +{ + return this->generatePath(path, step); +} + +template bool Searcher::findPathGreedy(Node *n, Node *endnode) +{ + Position midpos = npos; + PosType x = n->pos.x; + PosType y = n->pos.y; + const Position endpos = endnode->pos; + + JPS_ASSERT(x != endpos.x || y != endpos.y); // must not be called when start==end + JPS_ASSERT(n != endnode); + + int dx = int(endpos.x - x); + int dy = int(endpos.y - y); + const int adx = Abs(dx); + const int ady = Abs(dy); + dx = Sgn(dx); + dy = Sgn(dy); + + // go diagonally first + if(x != endpos.x && y != endpos.y) + { + JPS_ASSERT(dx && dy); + const int minlen = Min(adx, ady); + const PosType tx = x + dx * minlen; + while(x != tx) + { + if(grid(x, y) && (grid(x+dx, y) || grid(x, y+dy))) // prevent tunneling as well + { + x += dx; + y += dy; + } + else + return false; + } + + if(!grid(x, y)) + return false; + + midpos = Pos(x, y); + } + + // at this point, we're aligned to at least one axis + JPS_ASSERT(x == endpos.x || y == endpos.y); + + if(!(x == endpos.x && y == endpos.y)) + { + while(x != endpos.x) + if(!grid(x += dx, y)) + return false; + + while(y != endpos.y) + if(!grid(x, y += dy)) + return false; + + JPS_ASSERT(x == endpos.x && y == endpos.y); + } + + if(midpos.isValid()) + { + const unsigned nidx = storage.getindex(n); + Node *mid = getNode(midpos); // this might invalidate n, endnode + if(!mid) + return false; + n = &storage[nidx]; // reload pointers + endnode = &storage[endNodeIdx]; + JPS_ASSERT(mid && mid != n); + mid->setParent(*n); + if(mid != endnode) + endnode->setParent(*mid); + } + else + endnode->setParent(*n); + + return true; +} + +#undef JPS_ASSERT +#undef JPS_realloc +#undef JPS_free +#undef JPS_sqrt +#undef JPS_HEURISTIC_ACCURATE +#undef JPS_HEURISTIC_ESTIMATE + + +} // end namespace Internal + +using Internal::Searcher; + +typedef Internal::PodVec PathVector; + +// Single-call convenience function. For efficiency, do NOT use this if you need to compute paths repeatedly. +// +// Returns: 0 if failed or no path could be found, otherwise number of steps taken. +// +// path: If the function returns success, the path is appended to this vector. +// The path does NOT contain the starting position, i.e. if start and end are the same, +// the resulting path has no elements. +// The vector does not have to be empty. The function does not clear it; +// instead, the new path positions are appended at the end. +// This allows building a path incrementally. +// +// grid: Functor, expected to overload operator()(x, y), return true if position is walkable, false if not. +// +// step: If 0, only return waypoints. +// If 1, create exhaustive step-by-step path. +// If N, put in one position for N blocks travelled, or when a waypoint is hit. +// All returned points are guaranteed to be on a straight line (vertically, horizontally, or diagonally), +// and there is no obstruction between any two consecutive points. +// Note that this parameter does NOT influence the pathfinding in any way; +// it only controls the coarseness of the output path. +template +SizeT findPath(PV& path, const GRID& grid, PosType startx, PosType starty, PosType endx, PosType endy, + unsigned step = 0, // optional + JPS_Flags flags = JPS_Flag_Default, + void *user = 0) // memory allocation userdata +{ + Searcher search(grid, user); + if(!search.findPath(path, Pos(startx, starty), Pos(endx, endy), step, flags)) + return 0; + const SizeT done = search.getStepsDone(); + return done + !done; // report at least 1 step; as 0 would indicate failure +} + +} // end namespace JPS + + +/* +Changes compared to the older JPS.h at https://github.com/fgenesis/jps: + +- Explicitly freeing memory is no longer necessary. The freeMemory() method is still there + and does its thing (drop all internal storage), but you never have to call it explicitly. + Unlike the old version, there will be no performance degradation if you don't free memory every now and then. + Actually it'll be slightly slower if you free memory and pathfind again for the first time, + as it has to re-allocate internal data structures. + +- Searcher::getNodesExpanded() is now reset to 0 upon starting a search. + +- Added optional JPS_Flags parameter to pathfind (-init) functions to control search + behavior. Compile-time #defines are gone. + +- Removed skip parameter. Imho that one just added confusion and no real benefit. + If you want it back for some reason: poke me, open an issue, whatever. + +- Renamed JPS::Result to JPS_Result. Enum values gained JPS_ prefix, so JPS::NO_PATH is now JPS_NO_PATH, and so on. + +- Added one more JPS_Result value: JPS_OUT_OF_MEMORY. See info block at the top how to handle this. + +- Changed signature of Searcher<>::findPathFinish() to return JPS_Result (was bool). + This is more in line with the other 2 methods, as it can now return JPS_OUT_OF_MEMORY. + +- Changed signature of JPS::findPath(). Nonzero return is still success. Pointers to output stats are gone. + Use a Searcher instance if you need the details. + +- This version no longer depends on the C++ STL: , , , operator new(), all gone. + Makes things more memory- and cache-friendly, and quite a bit faster, too. + +- The canonical file name is now "jps.hh" instead of "JPS.h" +*/ + + +/* +TODO: +- make int -> DirType +- make possible to call findPathStep()/findPathFinish() even when JPS_EMPTY_PATH was returned on init (simplifies switch-case) +- make node know its heap index +- optional diagonals (make runtime param) +*/ diff --git a/win/vc90/external.vcproj b/win/vc90/external.vcproj index b304964..f1624ef 100644 --- a/win/vc90/external.vcproj +++ b/win/vc90/external.vcproj @@ -1400,7 +1400,7 @@ >