mirror of
https://github.com/AquariaOSE/Aquaria.git
synced 2024-12-25 06:05:45 +00:00
792 lines
19 KiB
C++
792 lines
19 KiB
C++
#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
|
|
|
|
// ============================
|
|
|
|
// 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<MyGrid> search(grid);
|
|
while(true)
|
|
{
|
|
// ..stuff happening ...
|
|
|
|
// build path incrementally from waypoints
|
|
JPS::Position a, b, c, d; // some waypoints
|
|
search.findPath(path, a, b);
|
|
search.findPath(path, b, c);
|
|
search.findPath(path, c, d);
|
|
|
|
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, 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,
|
|
or FOUND_PATH if the points are equal and not obstructed.
|
|
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 <stdlib.h>
|
|
#include <algorithm>
|
|
#include <vector>
|
|
#include <map>
|
|
#include <cmath>
|
|
|
|
#ifdef _DEBUG
|
|
#include <cassert>
|
|
#define JPS_ASSERT(cond) assert(cond)
|
|
#else
|
|
#define JPS_ASSERT(cond)
|
|
#endif
|
|
|
|
|
|
namespace JPS {
|
|
|
|
enum Result
|
|
{
|
|
NO_PATH = 0,
|
|
FOUND_PATH = 1,
|
|
NEED_MORE_STEPS = 2
|
|
};
|
|
|
|
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<Position> 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 char isOpen() const { return flags & 1; }
|
|
inline unsigned char isClosed() const { return flags & 2; }
|
|
inline void clearState() { f = 0; g = 0, parent = 0; flags = 0; }
|
|
|
|
private:
|
|
unsigned char 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<Node*> 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(const Node *item)
|
|
{
|
|
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 <typename GRID> 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 = 0);
|
|
|
|
// incremental pathfinding
|
|
Result findPathInit(Position start, Position end);
|
|
Result findPathStep(int limit);
|
|
bool findPathFinish(PathVector& path, unsigned step = 0);
|
|
|
|
// 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<Position, Node> 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);
|
|
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);
|
|
bool generatePath(PathVector& path, unsigned step) const;
|
|
#ifdef JPS_VERIFY
|
|
Position jumpPRec(const Position& p, const Position& src) const;
|
|
#endif
|
|
};
|
|
|
|
template <typename GRID> inline Node *Searcher<GRID>::getNode(const Position& pos)
|
|
{
|
|
JPS_ASSERT(grid(pos.x, pos.y));
|
|
return &nodegrid.insert(std::make_pair(pos, Node(pos))).first->second;
|
|
}
|
|
|
|
template <typename GRID> Position Searcher<GRID>::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 <typename GRID> Position Searcher<GRID>::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 <typename GRID> inline Position Searcher<GRID>::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 <typename GRID> inline Position Searcher<GRID>::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;
|
|
}
|
|
|
|
#ifdef JPS_VERIFY
|
|
// Recursive reference implementation -- for comparison only
|
|
template <typename GRID> Position Searcher<GRID>::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
|
|
|
|
template <typename GRID> unsigned Searcher<GRID>::findNeighbors(const Node *n, Position *wptr) const
|
|
{
|
|
Position *w = wptr;
|
|
const unsigned x = n->pos.x;
|
|
const unsigned y = n->pos.y;
|
|
|
|
#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)
|
|
|
|
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);
|
|
}
|
|
}
|
|
#undef JPS_ADDPOS
|
|
#undef JPS_ADDPOS_CHECK
|
|
#undef JPS_ADDPOS_NO_TUNNEL
|
|
#undef JPS_CHECKGRID
|
|
|
|
return unsigned(w - wptr);
|
|
}
|
|
|
|
template <typename GRID> void Searcher<GRID>::identifySuccessors(const Node *n)
|
|
{
|
|
Position buf[8];
|
|
const int num = findNeighbors(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 = jumpP(buf[i], n->pos);
|
|
#ifdef JPS_VERIFY
|
|
JPS_ASSERT(jp == jumpPRec(buf[i], n->pos));
|
|
#endif
|
|
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);
|
|
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(jn);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
template <typename GRID> bool Searcher<GRID>::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
|
|
{
|
|
path.push_back(next->pos);
|
|
next = next->parent;
|
|
}
|
|
while (next->parent);
|
|
}
|
|
std::reverse(path.begin() + offset, path.end());
|
|
return true;
|
|
}
|
|
|
|
template <typename GRID> bool Searcher<GRID>::findPath(PathVector& path, Position start, Position end, unsigned step /* = 0 */)
|
|
{
|
|
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 == FOUND_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 <typename GRID> Result Searcher<GRID>::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) ? FOUND_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;
|
|
|
|
open.push(getNode(start));
|
|
endNode = getNode(end);
|
|
JPS_ASSERT(endNode);
|
|
|
|
return NEED_MORE_STEPS;
|
|
}
|
|
|
|
template <typename GRID> Result Searcher<GRID>::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<typename GRID> bool Searcher<GRID>::findPathFinish(PathVector& path, unsigned step /* = 0 */)
|
|
{
|
|
return generatePath(path, step);
|
|
}
|
|
|
|
template<typename GRID> void Searcher<GRID>::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 <skip>-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 <skip> blocks thick at any reachable position.
|
|
template <typename GRID> 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<GRID> 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
|