jps 1.0.0

Sign up to get free protection for your applications and to get access to all the features.
checksums.yaml ADDED
@@ -0,0 +1,7 @@
1
+ ---
2
+ SHA1:
3
+ metadata.gz: 6b28b7b851c5c0a8c437e28c05b21d4a5bc808a7
4
+ data.tar.gz: d032f82ba7aa9b9c8759754a63ce7036f7a61fd0
5
+ SHA512:
6
+ metadata.gz: 73b594670ef6bb4bb5030287e12e98e9366a8e7c0c5cd5b253d7a4f8ec57a7cba76201f3ab24a2c29156cad09d7a12684797eb9519e143c0170c9b48ed696b7d
7
+ data.tar.gz: 6a33cdcdcb403f3efd60b7dabaa06efe6d59c557af1aa3effd21bec29c3a43aa3064dd48248ec93fa14dd18d921641adb9555f834af10ae5f3aeabd024055484
@@ -0,0 +1,3 @@
1
+ require 'mkmf'
2
+ $libs = '-lstdc++'
3
+ create_makefile 'rjps'
data/ext/jps/jps.hh ADDED
@@ -0,0 +1,1539 @@
1
+ #pragma once
2
+
3
+ /*
4
+ Public domain Jump Point Search implementation -- very fast pathfinding for uniform cost grids.
5
+ Scroll down for compile config, usage tips, example code.
6
+
7
+ License:
8
+ Public domain, WTFPL, CC0 or your favorite permissive license; whatever is available in your country.
9
+
10
+ Dependencies:
11
+ libc (stdlib.h, math.h) by default, change defines below to use your own functions. (realloc(), free(), sqrt())
12
+ Compiles as C++98, does not require C++11 nor the STL.
13
+ Does not throw exceptions, works without RTTI, does not contain any virtual methods.
14
+
15
+ Thread safety:
16
+ No global state. Searcher instances are not thread-safe. Grid template class is up to you.
17
+ If your grid access is read-only while pathfinding you may have many threads compute paths at the same time,
18
+ each with its own Searcher instance.
19
+
20
+ Background:
21
+ If you want to generate paths on a map with the following properties:
22
+ - You have a 2D grid (exactly two dimensions!), where each tile has exactly 8 neighbors (up, down, left, right + diagonals)
23
+ - There is no "cost" -- a tile is either walkable, or not.
24
+ then you may want to avoid full fledged A* and go for Jump Point Search (this lib).
25
+ JPS is usually much faster than plain old A*, as long as your tile traversability check function is fast.
26
+
27
+ Origin:
28
+ https://github.com/fgenesis/tinypile/blob/master/jps.hh
29
+
30
+ Based on my older implementation:
31
+ https://github.com/fgenesis/jps/blob/master/JPS.h
32
+ (For changes compared to that version go to the end of this file)
33
+
34
+ Inspired by:
35
+ http://users.cecs.anu.edu.au/~dharabor/data/papers/harabor-grastien-aaai11.pdf (The original paper)
36
+ https://github.com/Yonaba/Jumper
37
+ https://github.com/qiao/PathFinding.js
38
+
39
+ Usage:
40
+
41
+ Define a class that overloads `operator()(x, y) const`, returning a value that can be treated as boolean.
42
+ You are responsible for bounds checking!
43
+ You want your operator() to be as fast and small as possible, as it will be called a LOT.
44
+ Ask your compiler to force-inline it if possible.
45
+
46
+ // --- Begin example code ---
47
+
48
+ struct MyGrid
49
+ {
50
+ inline bool operator()(unsigned x, unsigned y) const // coordinates must be unsigned; method must be const
51
+ {
52
+ if(x < width && y < height) // Unsigned will wrap if < 0
53
+ ... return true if terrain at (x, y) is walkable.
54
+ // return false if terrain is not walkable or out-of-bounds.
55
+ }
56
+ unsigned width, height;
57
+ };
58
+
59
+ // Then you can retrieve a path:
60
+
61
+ MyGrid grid(... set grid width, height, map data, whatever);
62
+
63
+ const unsigned step = 0; // 0 compresses the path as much as possible and only records waypoints.
64
+ // Set this to 1 if you want a detailed single-step path
65
+ // (e.g. if you plan to further mangle the path yourself),
66
+ // or any other higher value to output every Nth position.
67
+ // (Waypoints are always output regardless of the step size.)
68
+
69
+ JPS::PathVector path; // The resulting path will go here.
70
+ // You may also use std::vector or whatever, as long as your vector type
71
+ // has push_back(), begin(), end(), resize() methods (same semantics as std::vector).
72
+ // Note that the path will NOT include the starting position!
73
+ // --> If called with start == end it will report that a path has been found,
74
+ // but the resulting path vector will be empty!
75
+
76
+ // Single-call interface:
77
+ // (Further remarks about this function can be found near the bottom of this file.)
78
+ // Note that the path vector is NOT cleared! New path points are appended at the end.
79
+ bool found = JPS::findPath(path, grid, startx, starty, endx, endy, step);
80
+
81
+
82
+ // --- Alternatively, if you want more control & efficiency for repeated pathfinding runs: ---
83
+
84
+ // Use a Searcher instance (can be a class member, on the stack, ...)
85
+ // Make sure the passed grid reference stays valid throughout the searcher's lifetime.
86
+ // If you need control over memory allocation, you may pass an extra pointer that will be
87
+ // forwarded to your own JPS_realloc & JPS_free if you've set those. Otherwise it's ignored.
88
+ JPS::Searcher<MyGrid> search(grid, userPtr = NULL);
89
+
90
+ // build path incrementally from waypoints:
91
+ JPS::Position a, b, c, d = <...>; // set some waypoints
92
+ if (search.findPath(path, a, b)
93
+ && search.findPath(path, b, c)
94
+ && search.findPath(path, c, d))
95
+ {
96
+ // found path: a->b->c->d
97
+ }
98
+
99
+ // keep re-using existing pathfinder instance
100
+ while(whatever)
101
+ {
102
+ // Set startx, starty, endx, endy = <...>
103
+ if(!search.findPath(path, JPS::Pos(startx, starty), JPS::Pos(endx, endy), step))
104
+ {
105
+ // ...handle failure...
106
+ }
107
+ }
108
+
109
+ // If necessary, you may free internal memory -- this is never required; neither for performance, nor for correct function.
110
+ // If you do pathfinding after freeing memory, it'll allocate new memory.
111
+ // Note that freeing memory aborts any incremental search currently ongoing.
112
+ search.freeMemory();
113
+
114
+ // If you need to know how much memory is internally allocated by a searcher:
115
+ unsigned bytes = search.getTotalMemoryInUse();
116
+
117
+
118
+ // -------------------------------
119
+ // --- Incremental pathfinding ---
120
+ // -------------------------------
121
+
122
+ Calling JPS::findPath() or Searcher<>::findPath() always computes an entire path or returns failure.
123
+ If the path is long or costly and you have a tight CPU budget per frame you may want to perform pathfinding incrementally,
124
+ stretched over multiple frames.
125
+
126
+ First, call
127
+ ### JPS_Result res = search.findPathInit(Position start, Position end) ###
128
+ Don't forget to check the return value, as it may return:
129
+ - JPS_NO_PATH if one or both of the points are obstructed
130
+ - JPS_EMPTY_PATH if the points are equal and not obstructed
131
+ - JPS_FOUND_PATH if the initial greedy heuristic could find a path quickly.
132
+ - JPS_OUT_OF_MEMORY if... well yeah.
133
+ If it returns JPS_NEED_MORE_STEPS then the next part can start.
134
+
135
+ Repeatedly call
136
+ ### JPS_Result res = search.findPathStep(int limit) ###
137
+ until it returns JPS_NO_PATH or JPS_FOUND_PATH, or JPS_OUT_OF_MEMORY.
138
+ For consistency, you will want to ensure that the grid does not change between subsequent calls;
139
+ if the grid changes, parts of the path may go through a now obstructed area or may be no longer optimal.
140
+ If limit is 0, it will perform the pathfinding in one go. Values > 0 pause the search
141
+ as soon as possible after the number of steps was exceeded, returning NEED_MORE_STEPS.
142
+ Use search.getStepsDone() after some test runs to find a good value for the limit.
143
+
144
+ After getting JPS_FOUND_PATH, generate the actual path points via
145
+ ### JPS_Result res = search.findPathFinish(PathVector& path, unsigned step = 0) ###
146
+ As described above, path points are appended, and granularity can be adjusted with the step parameter.
147
+ Returns JPS_FOUND_PATH if the path was successfully built and appended to the path vector.
148
+ Returns JPS_NO_PATH if the pathfinding did not finish or generating the path failed.
149
+ May return JPS_OUT_OF_MEMORY if the path vector must be resized but fails to allocate.
150
+
151
+ If findPathInit() or findPathStep() return JPS_OUT_OF_MEMORY, the current searcher progress becomes undefined.
152
+ To recover, free some memory elsewhere and call findPathInit() to try again.
153
+
154
+ If findPathFinish() returns out-of-memory but previous steps finished successfully,
155
+ then the found path is still valid for generating the path vector.
156
+ In that case you may call findPathFinish() again after making some memory available.
157
+
158
+ If you do not worry about memory, treat JPS_OUT_OF_MEMORY as if JPS_NO_PATH was returned.
159
+
160
+ You may pass JPS::PathVector, std::vector, or your own to findPathFinish().
161
+ Note that if the path vector type you pass throws exceptions in case of allocation failures (std::vector does, for example),
162
+ you'll get that exception, and the path vector will be in whatever state it was in when the last element was successfully inserted.
163
+ If no exception is thrown (ie. you used JPS::PathVector) then the failure cases do not modify the path vector.
164
+
165
+ You may abort a search anytime by starting a new one via findPathInit(), calling freeMemory(), or by destroying the searcher instance.
166
+ Aborting or starting a search resets the values returned by .getStepsDone() and .getNodesExpanded() to 0.
167
+
168
+ */
169
+
170
+ // ============================
171
+ // ====== COMPILE CONFIG ======
172
+ // ============================
173
+
174
+
175
+ // If you want to avoid sqrt() or floats in general, define this.
176
+ // Turns out in some testing this was ~12% faster, so it's the default.
177
+ #define JPS_NO_FLOAT
178
+
179
+
180
+ // ------------------------------------------------
181
+
182
+ #include <stddef.h> // for size_t (needed for operator new)
183
+
184
+ // Assertions
185
+ #ifndef JPS_ASSERT
186
+ # ifdef _DEBUG
187
+ # include <assert.h>
188
+ # define JPS_ASSERT(cond) assert(cond)
189
+ # else
190
+ # define JPS_ASSERT(cond)
191
+ # endif
192
+ #endif
193
+
194
+ // The default allocator uses realloc(), free(). Change if necessary.
195
+ // You will get the user pointer that you passed to findPath() or the Searcher ctor.
196
+ #if !defined(JPS_realloc) || !defined(JPS_free)
197
+ # include <stdlib.h> // for realloc, free
198
+ # ifndef JPS_realloc
199
+ # define JPS_realloc(p, newsize, oldsize, user) realloc(p, newsize)
200
+ # endif
201
+ # ifndef JPS_free
202
+ # define JPS_free(p, oldsize, user) free(p)
203
+ # endif
204
+ #endif
205
+
206
+ #ifdef JPS_NO_FLOAT
207
+ #define JPS_HEURISTIC_ACCURATE(a, b) (Heuristic::Chebyshev(a, b))
208
+ #else
209
+ # ifndef JPS_sqrt
210
+ // for Euclidean heuristic.
211
+ # include <math.h>
212
+ # define JPS_sqrt(x) sqrtf(float(x)) // float cast here avoids a warning about implicit int->float cast
213
+ # endif
214
+ #endif
215
+
216
+ // Which heuristics to use.
217
+ // Basic property: Distance estimate, returns values >= 0. Smaller is better.
218
+ // The accurate heuristic should always return guesses less or equal than the estimate heuristic,
219
+ // otherwise the resulting paths may not be optimal.
220
+ // (The rule of thumb is that the estimate is fast but can overestimate)
221
+ // For the implementation of heuristics, scroll down.
222
+ #ifndef JPS_HEURISTIC_ACCURATE
223
+ #define JPS_HEURISTIC_ACCURATE(a, b) (Heuristic::Euclidean(a, b))
224
+ #endif
225
+
226
+ #ifndef JPS_HEURISTIC_ESTIMATE
227
+ #define JPS_HEURISTIC_ESTIMATE(a, b) (Heuristic::Manhattan(a, b))
228
+ #endif
229
+
230
+
231
+ // --- Data types ---
232
+ namespace JPS {
233
+
234
+ // unsigned integer type wide enough to store a position on one grid axis.
235
+ // Note that on x86, u32 is actually faster than u16.
236
+ typedef unsigned PosType;
237
+
238
+ // Result of heuristics. can also be (unsigned) int but using float by default since that's what sqrtf() returns
239
+ // and we don't need to cast float->int that way. Change if you use integer-only heuristics.
240
+ // (Euclidean heuristic using sqrt() works fine even if cast to int. Your choice really.)
241
+ #ifdef JPS_NO_FLOAT
242
+ typedef int ScoreType;
243
+ #else
244
+ typedef float ScoreType;
245
+ #endif
246
+
247
+ // 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.
248
+ typedef unsigned SizeT;
249
+
250
+ } // end namespace JPS
251
+
252
+
253
+ // ================================
254
+ // ====== COMPILE CONFIG END ======
255
+ // ================================
256
+ // ----------------------------------------------------------------------------------------
257
+
258
+ typedef unsigned JPS_Flags;
259
+ enum JPS_Flags_
260
+ {
261
+ // No special behavior
262
+ JPS_Flag_Default = 0x00,
263
+
264
+ // If this is defined, disable the greedy direct-short-path check that avoids the large area scanning that JPS does.
265
+ // This is just a performance tweak. May save a lot of CPU when constantly re-planning short paths without obstacles
266
+ // (e.g. an entity follows close behind another).
267
+ // Does not change optimality of results. If you perform your own line-of-sight checks
268
+ // before starting a pathfinding run you can disable greedy since checking twice isn't needed,
269
+ // but otherwise it's better to leave it enabled.
270
+ JPS_Flag_NoGreedy = 0x01,
271
+
272
+ // If this is set, use standard A* instead of JPS (e.g. if you want to compare performance in your scenario).
273
+ // In most cases this will be MUCH slower, but might be beneficial if your grid lookup
274
+ // is slow (aka worse than O(1) or more than a few inlined instructions),
275
+ // as it avoids the large area scans that the JPS algorithm does.
276
+ // (Also increases memory usage as each checked position is expanded into a node.)
277
+ JPS_Flag_AStarOnly = 0x02,
278
+
279
+ // Don't check whether start position is walkable.
280
+ // This makes the start position always walkable, even if the map data say otherwise.
281
+ JPS_Flag_NoStartCheck = 0x04,
282
+
283
+ // Don't check whether end position is walkable.
284
+ JPS_Flag_NoEndCheck = 0x08,
285
+ };
286
+
287
+ enum JPS_Result
288
+ {
289
+ JPS_NO_PATH,
290
+ JPS_FOUND_PATH,
291
+ JPS_NEED_MORE_STEPS,
292
+ JPS_EMPTY_PATH,
293
+ JPS_OUT_OF_MEMORY
294
+ };
295
+
296
+ // operator new() without #include <new>
297
+ // Unfortunately the standard mandates the use of size_t, so we need stddef.h the very least.
298
+ // Trick via https://github.com/ocornut/imgui
299
+ // "Defining a custom placement new() with a dummy parameter allows us to bypass including <new>
300
+ // which on some platforms complains when user has disabled exceptions."
301
+ struct JPS__NewDummy {};
302
+ inline void* operator new(size_t, JPS__NewDummy, void* ptr) { return ptr; }
303
+ inline void operator delete(void*, JPS__NewDummy, void*) {}
304
+ #define JPS_PLACEMENT_NEW(p) new(JPS__NewDummy(), p)
305
+
306
+
307
+ namespace JPS {
308
+
309
+ struct Position
310
+ {
311
+ PosType x, y;
312
+
313
+ inline bool operator==(const Position& p) const
314
+ {
315
+ return x == p.x && y == p.y;
316
+ }
317
+ inline bool operator!=(const Position& p) const
318
+ {
319
+ return x != p.x || y != p.y;
320
+ }
321
+
322
+ inline bool isValid() const { return x != PosType(-1); }
323
+ };
324
+
325
+ // The invalid position. Used internally to mark non-walkable points.
326
+ static const Position npos = {PosType(-1), PosType(-1)};
327
+ static const SizeT noidx = SizeT(-1);
328
+
329
+ // ctor function to keep Position a real POD struct.
330
+ inline static Position Pos(PosType x, PosType y)
331
+ {
332
+ Position p;
333
+ p.x = x;
334
+ p.y = y;
335
+ return p;
336
+ }
337
+
338
+ template<typename T> inline static T Max(T a, T b) { return a < b ? b : a; }
339
+ template<typename T> inline static T Min(T a, T b) { return a < b ? a : b; }
340
+ template<typename T> inline static T Abs(T a) { return a < T(0) ? -a : a; }
341
+ template<typename T> inline static int Sgn(T val) { return (T(0) < val) - (val < T(0)); }
342
+
343
+
344
+ // Heuristics. Add new ones if you need them.
345
+ namespace Heuristic
346
+ {
347
+ inline ScoreType Manhattan(const Position& a, const Position& b)
348
+ {
349
+ const int dx = Abs(int(a.x - b.x));
350
+ const int dy = Abs(int(a.y - b.y));
351
+ return static_cast<ScoreType>(dx + dy);
352
+ }
353
+
354
+ inline ScoreType Chebyshev(const Position& a, const Position& b)
355
+ {
356
+ const int dx = Abs(int(a.x - b.x));
357
+ const int dy = Abs(int(a.y - b.y));
358
+ return static_cast<ScoreType>(Max(dx, dy));
359
+ }
360
+ #ifdef JPS_sqrt
361
+ inline ScoreType Euclidean(const Position& a, const Position& b)
362
+ {
363
+ const int dx = (int(a.x - b.x));
364
+ const int dy = (int(a.y - b.y));
365
+ return static_cast<ScoreType>(JPS_sqrt(dx*dx + dy*dy));
366
+ }
367
+ #endif
368
+ } // end namespace heuristic
369
+
370
+
371
+
372
+ // --- Begin infrastructure, data structures ---
373
+
374
+ namespace Internal {
375
+
376
+ // Never allocated outside of a PodVec<Node> --> All nodes are linearly adjacent in memory.
377
+ struct Node
378
+ {
379
+ ScoreType f, g; // heuristic distances
380
+ Position pos;
381
+ int parentOffs; // no parent if 0
382
+ unsigned _flags;
383
+
384
+ inline int hasParent() const { return parentOffs; }
385
+ inline void setOpen() { _flags |= 1; }
386
+ inline void setClosed() { _flags |= 2; }
387
+ inline unsigned isOpen() const { return _flags & 1; }
388
+ inline unsigned isClosed() const { return _flags & 2; }
389
+
390
+ // We know nodes are allocated sequentially in memory, so this is fine.
391
+ inline Node& getParent() { JPS_ASSERT(parentOffs); return this[parentOffs]; }
392
+ inline const Node& getParent() const { JPS_ASSERT(parentOffs); return this[parentOffs]; }
393
+ inline const Node *getParentOpt() const { return parentOffs ? this + parentOffs : 0; }
394
+ inline void setParent(const Node& p) { JPS_ASSERT(&p != this); parentOffs = static_cast<int>(&p - this); }
395
+ };
396
+
397
+ template<typename T>
398
+ class PodVec
399
+ {
400
+ public:
401
+ PodVec(void *user = 0)
402
+ : _data(0), used(0), cap(0), _user(user)
403
+ {}
404
+ ~PodVec() { dealloc(); }
405
+ inline void clear()
406
+ {
407
+ used = 0;
408
+ }
409
+ void dealloc()
410
+ {
411
+ JPS_free(_data, cap * sizeof(T), _user);
412
+ _data = 0;
413
+ used = 0;
414
+ cap = 0;
415
+ }
416
+ T *alloc()
417
+ {
418
+ T *e = 0;
419
+ if(used < cap || _grow())
420
+ {
421
+ e = _data + used;
422
+ ++used;
423
+ }
424
+ return e;
425
+ }
426
+ inline void push_back(const T& e)
427
+ {
428
+ if(T *dst = alloc()) // yes, this silently fails when OOM. this is handled internally.
429
+ *dst = e;
430
+ }
431
+ inline void pop_back() { JPS_ASSERT(used); --used; }
432
+ inline T& back() { JPS_ASSERT(used); return _data[used-1]; }
433
+ inline SizeT size() const { return used; }
434
+ inline bool empty() const { return !used; }
435
+ inline T *data() { return _data; }
436
+ inline const T *data() const { return _data; }
437
+ inline T& operator[](size_t idx) const { JPS_ASSERT(idx < used); return _data[idx]; }
438
+ inline SizeT getindex(const T *e) const
439
+ {
440
+ JPS_ASSERT(e && _data <= e && e < _data + used);
441
+ return static_cast<SizeT>(e - _data);
442
+ }
443
+
444
+ void *_reserve(SizeT newcap) // for internal use
445
+ {
446
+ return cap < newcap ? _grow(newcap) : _data;
447
+ }
448
+ void resize(SizeT sz)
449
+ {
450
+ if(_reserve(sz))
451
+ used = sz;
452
+ }
453
+ SizeT _getMemSize() const
454
+ {
455
+ return cap * sizeof(T);
456
+ }
457
+
458
+ // minimal iterator interface
459
+ typedef T* iterator;
460
+ typedef const T* const_iterator;
461
+ typedef SizeT size_type;
462
+ typedef T value_type;
463
+ inline iterator begin() { return data(); }
464
+ inline iterator end() { return data() + size(); }
465
+ inline const_iterator cbegin() const { return data(); }
466
+ inline const_iterator cend() const { return data() + size(); }
467
+
468
+ private:
469
+ void *_grow(SizeT newcap)
470
+ {
471
+ void *p = JPS_realloc(_data, newcap * sizeof(T), cap * sizeof(T), _user);
472
+ if(p)
473
+ {
474
+ _data = (T*)p;
475
+ cap = newcap;
476
+ }
477
+ return p;
478
+ }
479
+ void * _grow()
480
+ {
481
+ const SizeT newcap = cap + (cap / 2) + 32;
482
+ return _grow(newcap);
483
+ }
484
+ T *_data;
485
+ SizeT used, cap;
486
+
487
+ public:
488
+ void * const _user;
489
+
490
+ private:
491
+ // forbid ops
492
+ PodVec<T>& operator=(const PodVec<T>&);
493
+ PodVec(const PodVec<T>&);
494
+ };
495
+
496
+ template<typename T>
497
+ inline static void Swap(T& a, T& b)
498
+ {
499
+ const T tmp = a;
500
+ a = b;
501
+ b = tmp;
502
+ }
503
+
504
+ template<typename IT>
505
+ inline static void Reverse(IT first, IT last)
506
+ {
507
+ while((first != last) && (first != --last))
508
+ {
509
+ Swap(*first, *last);
510
+ ++first;
511
+ }
512
+ }
513
+
514
+ typedef PodVec<Node> Storage;
515
+
516
+
517
+ class NodeMap
518
+ {
519
+ private:
520
+ static const unsigned LOAD_FACTOR = 8; // estimate: {CPU cache line size (64)} / sizeof(HashLoc)
521
+ static const unsigned INITIAL_BUCKETS = 16; // must be > 1 and power of 2
522
+
523
+ struct HashLoc
524
+ {
525
+ unsigned hash2; // for early-out check only
526
+ SizeT idx; // index in central storage
527
+ };
528
+ typedef PodVec<HashLoc> Bucket;
529
+
530
+ // hash function to determine bucket. only uses lower few bits. should jumble lower bits nicely.
531
+ static inline unsigned Hash(PosType x, PosType y)
532
+ {
533
+ return x ^ y;
534
+ }
535
+
536
+ // hash function designed to lose as little data as possible. for early-out checks. all bits used.
537
+ static inline unsigned Hash2(PosType x, PosType y)
538
+ {
539
+ return (y << 16) ^ x;
540
+ }
541
+
542
+ public:
543
+
544
+ NodeMap(Storage& storage)
545
+ : _storageRef(storage), _buckets(storage._user)
546
+ {}
547
+
548
+ ~NodeMap()
549
+ {
550
+ dealloc();
551
+ }
552
+
553
+ void dealloc()
554
+ {
555
+ for(SizeT i = 0; i < _buckets.size(); ++i)
556
+ _buckets[i].~Bucket();
557
+ _buckets.dealloc();
558
+ }
559
+ void clear()
560
+ {
561
+ // clear the buckets, but *not* the bucket vector
562
+ for(SizeT i = 0; i < _buckets.size(); ++i)
563
+ _buckets[i].clear();
564
+ }
565
+
566
+ Node *operator()(PosType x, PosType y)
567
+ {
568
+ const unsigned h = Hash(x, y);
569
+ const unsigned h2 = Hash2(x, y);
570
+ const SizeT ksz = _buckets.size(); // known to be power-of-2
571
+ Bucket *b = 0; // MSVC /W4 complains that this was uninitialized and used, so we init it...
572
+ if (ksz)
573
+ {
574
+ b = &_buckets[h & (ksz - 1)];
575
+ const SizeT bsz = b->size();
576
+ const HashLoc * const bdata = b->data();
577
+ for (SizeT i = 0; i < bsz; ++i)
578
+ {
579
+ // this is the only place where HashLoc::hash2 is used; it *could*be removed, which means:
580
+ // - twice as much space for indexes per cache line
581
+ // - 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,
582
+ // and we'll likely end up in a random location in RAM for each node.
583
+ // Quick benchmarking showed that *with* the hash2 check it's almost immeasurably (less than 1%) faster.
584
+ if (bdata[i].hash2 == h2)
585
+ {
586
+ Node &n = _storageRef[bdata[i].idx];
587
+ if(n.pos.x == x && n.pos.y == y)
588
+ return &n;
589
+ }
590
+ }
591
+ }
592
+
593
+ // enlarge hashmap if necessary; fix bucket if so
594
+ SizeT newbsz = _enlarge();
595
+ if(newbsz > 1)
596
+ b = &_buckets[h & (newbsz - 1)];
597
+ else if(newbsz == 1) // error case
598
+ return 0;
599
+
600
+ 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.
601
+
602
+ if(!loc)
603
+ return 0;
604
+
605
+ loc->hash2 = h2;
606
+ loc->idx = _storageRef.size();
607
+
608
+ // no node at (x, y), create new one
609
+ Node *n = _storageRef.alloc();
610
+ if(n)
611
+ {
612
+ n->f = 0;
613
+ n->g = 0;
614
+ n->pos.x = x;
615
+ n->pos.y = y;
616
+ n->parentOffs = 0;
617
+ n->_flags = 0;
618
+ }
619
+ return n;
620
+ }
621
+
622
+ SizeT _getMemSize() const
623
+ {
624
+ SizeT sum = _buckets._getMemSize();
625
+ for(Buckets::const_iterator it = _buckets.cbegin(); it != _buckets.cend(); ++it)
626
+ sum += it->_getMemSize();
627
+ return sum;
628
+ }
629
+
630
+ private:
631
+
632
+ // return values: 0 = nothing to do; 1 = error; >1: internal storage was enlarged to this many buckets
633
+ SizeT _enlarge()
634
+ {
635
+ const SizeT n = _storageRef.size();
636
+ const SizeT oldsz = _buckets.size();
637
+ if (n < oldsz * LOAD_FACTOR)
638
+ return 0;
639
+
640
+ // pre-allocate bucket storage that we're going to use
641
+ const SizeT newsz = oldsz ? oldsz * 2 : INITIAL_BUCKETS; // stays power of 2
642
+
643
+ if(!_buckets._reserve(newsz))
644
+ return 0; // early out if realloc fails; this not a problem and we can continue.
645
+
646
+ // forget everything
647
+ for(SizeT i = 0; i < oldsz; ++i)
648
+ _buckets[i].clear();
649
+
650
+ // resize and init
651
+ for(SizeT i = oldsz; i < newsz; ++i)
652
+ {
653
+ void *p = _buckets.alloc(); // can't fail since the space was reserved
654
+ JPS_PLACEMENT_NEW(p) PodVec<HashLoc>(_buckets._user);
655
+ }
656
+
657
+ const SizeT mask = _buckets.size() - 1;
658
+ for(SizeT i = 0; i < n; ++i)
659
+ {
660
+ const Position p = _storageRef[i].pos;
661
+ HashLoc *loc = _buckets[Hash(p.x, p.y) & mask].alloc();
662
+ if(!loc)
663
+ return 1; // error case
664
+
665
+ loc->hash2 = Hash2(p.x, p.y);
666
+ loc->idx = i;
667
+ }
668
+ return newsz;
669
+ }
670
+
671
+ Storage& _storageRef;
672
+ typedef PodVec<Bucket> Buckets;
673
+ Buckets _buckets;
674
+ };
675
+
676
+ class OpenList
677
+ {
678
+ private:
679
+ const Storage& _storageRef;
680
+ PodVec<SizeT> idxHeap;
681
+
682
+ public:
683
+
684
+ OpenList(const Storage& storage)
685
+ : _storageRef(storage), idxHeap(storage._user)
686
+ {}
687
+
688
+
689
+ inline void pushNode(Node *n)
690
+ {
691
+ _heapPushIdx(_storageRef.getindex(n));
692
+ }
693
+
694
+ inline Node& popNode()
695
+ {
696
+ return _storageRef[_popIdx()];
697
+ }
698
+
699
+ // re-heapify after node changed its order
700
+ inline void fixNode(const Node& n)
701
+ {
702
+ const unsigned ni = _storageRef.getindex(&n);
703
+ const unsigned sz = idxHeap.size();
704
+ unsigned *p = idxHeap.data();
705
+ 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
706
+ if(p[i] == ni)
707
+ {
708
+ _fixIdx(i);
709
+ return;
710
+ }
711
+ JPS_ASSERT(false); // expect node to be found
712
+ }
713
+
714
+ inline void dealloc() { idxHeap.dealloc(); }
715
+ inline void clear() { idxHeap.clear(); }
716
+ inline bool empty() const { return idxHeap.empty(); }
717
+
718
+ inline SizeT _getMemSize() const
719
+ {
720
+ return idxHeap._getMemSize();
721
+ }
722
+
723
+ private:
724
+
725
+ inline bool _heapLess(SizeT a, SizeT b)
726
+ {
727
+ return _storageRef[idxHeap[a]].f > _storageRef[idxHeap[b]].f;
728
+ }
729
+
730
+ inline bool _heapLessIdx(SizeT a, SizeT idx)
731
+ {
732
+ return _storageRef[idxHeap[a]].f > _storageRef[idx].f;
733
+ }
734
+
735
+ void _percolateUp(SizeT i)
736
+ {
737
+ const SizeT idx = idxHeap[i];
738
+ SizeT p;
739
+ goto start;
740
+ do
741
+ {
742
+ idxHeap[i] = idxHeap[p]; // parent is smaller, move it down
743
+ i = p; // continue with parent
744
+ start:
745
+ p = (i - 1) >> 1;
746
+ }
747
+ while(i && _heapLessIdx(p, idx));
748
+ idxHeap[i] = idx; // found correct place for idx
749
+ }
750
+
751
+ void _percolateDown(SizeT i)
752
+ {
753
+ const SizeT idx = idxHeap[i];
754
+ const SizeT sz = idxHeap.size();
755
+ SizeT child;
756
+ goto start;
757
+ do
758
+ {
759
+ // pick right sibling if exists and larger or equal
760
+ if(child + 1 < sz && !_heapLess(child+1, child))
761
+ ++child;
762
+ idxHeap[i] = idxHeap[child];
763
+ i = child;
764
+ start:
765
+ child = (i << 1) + 1;
766
+ }
767
+ while(child < sz);
768
+ idxHeap[i] = idx;
769
+ _percolateUp(i);
770
+ }
771
+
772
+ void _heapPushIdx(SizeT idx)
773
+ {
774
+ SizeT i = idxHeap.size();
775
+ idxHeap.push_back(idx);
776
+ _percolateUp(i);
777
+ }
778
+
779
+ SizeT _popIdx()
780
+ {
781
+ SizeT sz = idxHeap.size();
782
+ JPS_ASSERT(sz);
783
+ const SizeT root = idxHeap[0];
784
+ idxHeap[0] = idxHeap[--sz];
785
+ idxHeap.pop_back();
786
+ if(sz > 1)
787
+ _percolateDown(0);
788
+ return root;
789
+ }
790
+
791
+ // re-heapify node at index i
792
+ inline void _fixIdx(SizeT i)
793
+ {
794
+ _percolateDown(i);
795
+ _percolateUp(i);
796
+ }
797
+ };
798
+
799
+ #undef JPS_PLACEMENT_NEW
800
+
801
+ // --- End infrastructure, data structures ---
802
+
803
+ // All those things that don't depend on template parameters...
804
+ class SearcherBase
805
+ {
806
+ protected:
807
+ Storage storage;
808
+ OpenList open;
809
+ NodeMap nodemap;
810
+
811
+ Position endPos;
812
+ SizeT endNodeIdx;
813
+ JPS_Flags flags;
814
+ int stepsRemain;
815
+ SizeT stepsDone;
816
+
817
+
818
+ SearcherBase(void *user)
819
+ : storage(user)
820
+ , open(storage)
821
+ , nodemap(storage)
822
+ , endPos(npos), endNodeIdx(noidx)
823
+ , flags(0)
824
+ , stepsRemain(0), stepsDone(0)
825
+ {}
826
+
827
+ void clear()
828
+ {
829
+ open.clear();
830
+ nodemap.clear();
831
+ storage.clear();
832
+ endNodeIdx = noidx;
833
+ stepsDone = 0;
834
+ }
835
+
836
+ void _expandNode(const Position jp, Node& jn, const Node& parent)
837
+ {
838
+ JPS_ASSERT(jn.pos == jp);
839
+ ScoreType extraG = JPS_HEURISTIC_ACCURATE(jp, parent.pos);
840
+ ScoreType newG = parent.g + extraG;
841
+ if(!jn.isOpen() || newG < jn.g)
842
+ {
843
+ jn.g = newG;
844
+ jn.f = jn.g + JPS_HEURISTIC_ESTIMATE(jp, endPos);
845
+ jn.setParent(parent);
846
+ if(!jn.isOpen())
847
+ {
848
+ open.pushNode(&jn);
849
+ jn.setOpen();
850
+ }
851
+ else
852
+ open.fixNode(jn);
853
+ }
854
+ }
855
+
856
+ public:
857
+
858
+ template <typename PV>
859
+ JPS_Result generatePath(PV& path, unsigned step) const;
860
+
861
+ void freeMemory()
862
+ {
863
+ open.dealloc();
864
+ nodemap.dealloc();
865
+ storage.dealloc();
866
+ endNodeIdx = noidx;
867
+ }
868
+
869
+ // --- Statistics ---
870
+
871
+ inline SizeT getStepsDone() const { return stepsDone; }
872
+ inline SizeT getNodesExpanded() const { return storage.size(); }
873
+
874
+ SizeT getTotalMemoryInUse() const
875
+ {
876
+ return storage._getMemSize()
877
+ + nodemap._getMemSize()
878
+ + open._getMemSize();
879
+ }
880
+ };
881
+
882
+ template <typename GRID> class Searcher : public SearcherBase
883
+ {
884
+ public:
885
+ Searcher(const GRID& g, void *user = 0)
886
+ : SearcherBase(user), grid(g)
887
+ {}
888
+
889
+ // single-call
890
+ template<typename PV>
891
+ bool findPath(PV& path, Position start, Position end, unsigned step, JPS_Flags flags = JPS_Flag_Default);
892
+
893
+ // incremental pathfinding
894
+ JPS_Result findPathInit(Position start, Position end, JPS_Flags flags = JPS_Flag_Default);
895
+ JPS_Result findPathStep(int limit);
896
+ // generate path after one was found
897
+ template<typename PV>
898
+ JPS_Result findPathFinish(PV& path, unsigned step) const;
899
+
900
+ private:
901
+
902
+ const GRID& grid;
903
+
904
+ Node *getNode(const Position& pos);
905
+ bool identifySuccessors(const Node& n);
906
+
907
+ bool findPathGreedy(Node *start, Node *end);
908
+
909
+ unsigned findNeighborsAStar(const Node& n, Position *wptr);
910
+
911
+ unsigned findNeighborsJPS(const Node& n, Position *wptr) const;
912
+ Position jumpP(const Position& p, const Position& src);
913
+ Position jumpD(Position p, int dx, int dy);
914
+ Position jumpX(Position p, int dx);
915
+ Position jumpY(Position p, int dy);
916
+
917
+ // forbid any ops
918
+ Searcher& operator=(const Searcher<GRID>&);
919
+ Searcher(const Searcher<GRID>&);
920
+ };
921
+
922
+
923
+ // -----------------------------------------------------------------------
924
+
925
+ template<typename PV> JPS_Result SearcherBase::generatePath(PV& path, unsigned step) const
926
+ {
927
+ if(endNodeIdx == noidx)
928
+ return JPS_NO_PATH;
929
+ const SizeT offset = path.size();
930
+ SizeT added = 0;
931
+ const Node& endNode = storage[endNodeIdx];
932
+ const Node *next = &endNode;
933
+ if(!next->hasParent())
934
+ return JPS_NO_PATH;
935
+ if(step)
936
+ {
937
+ const Node *prev = endNode.getParentOpt();
938
+ if(!prev)
939
+ return JPS_NO_PATH;
940
+ do
941
+ {
942
+ const unsigned x = next->pos.x, y = next->pos.y;
943
+ int dx = int(prev->pos.x - x);
944
+ int dy = int(prev->pos.y - y);
945
+ const int adx = Abs(dx);
946
+ const int ady = Abs(dy);
947
+ JPS_ASSERT(!dx || !dy || adx == ady); // known to be straight, if diagonal
948
+ const int steps = Max(adx, ady);
949
+ dx = int(step) * Sgn(dx);
950
+ dy = int(step) * Sgn(dy);
951
+ int dxa = 0, dya = 0;
952
+ for(int i = 0; i < steps; i += step)
953
+ {
954
+ path.push_back(Pos(x+dxa, y+dya));
955
+ ++added;
956
+ dxa += dx;
957
+ dya += dy;
958
+ }
959
+ next = prev;
960
+ prev = prev->getParentOpt();
961
+ }
962
+ while (prev);
963
+ }
964
+ else
965
+ {
966
+ do
967
+ {
968
+ JPS_ASSERT(next != &next->getParent());
969
+ path.push_back(next->pos);
970
+ ++added;
971
+ next = &next->getParent();
972
+ }
973
+ while (next->hasParent());
974
+ }
975
+
976
+ // JPS::PathVector silently discards push_back() when memory allocation fails;
977
+ // detect that case and roll back.
978
+ if(path.size() != offset + added)
979
+ {
980
+ path.resize(offset);
981
+ return JPS_OUT_OF_MEMORY;
982
+ }
983
+
984
+ // Nodes were traversed backwards, fix that
985
+ Reverse(path.begin() + offset, path.end());
986
+ return JPS_FOUND_PATH;
987
+ }
988
+
989
+ //-----------------------------------------
990
+
991
+ template <typename GRID> inline Node *Searcher<GRID>::getNode(const Position& pos)
992
+ {
993
+ JPS_ASSERT(grid(pos.x, pos.y));
994
+ return nodemap(pos.x, pos.y);
995
+ }
996
+
997
+ template <typename GRID> Position Searcher<GRID>::jumpP(const Position &p, const Position& src)
998
+ {
999
+ JPS_ASSERT(grid(p.x, p.y));
1000
+
1001
+ int dx = int(p.x - src.x);
1002
+ int dy = int(p.y - src.y);
1003
+ JPS_ASSERT(dx || dy);
1004
+
1005
+ if(dx && dy)
1006
+ return jumpD(p, dx, dy);
1007
+ else if(dx)
1008
+ return jumpX(p, dx);
1009
+ else if(dy)
1010
+ return jumpY(p, dy);
1011
+
1012
+ // not reached
1013
+ JPS_ASSERT(false);
1014
+ return npos;
1015
+ }
1016
+
1017
+ template <typename GRID> Position Searcher<GRID>::jumpD(Position p, int dx, int dy)
1018
+ {
1019
+ JPS_ASSERT(grid(p.x, p.y));
1020
+ JPS_ASSERT(dx && dy);
1021
+
1022
+ const Position endpos = endPos;
1023
+ unsigned steps = 0;
1024
+
1025
+ while(true)
1026
+ {
1027
+ if(p == endpos)
1028
+ break;
1029
+
1030
+ ++steps;
1031
+ const PosType x = p.x;
1032
+ const PosType y = p.y;
1033
+
1034
+ if( (grid(x-dx, y+dy) && !grid(x-dx, y)) || (grid(x+dx, y-dy) && !grid(x, y-dy)) )
1035
+ break;
1036
+
1037
+ const bool gdx = !!grid(x+dx, y);
1038
+ const bool gdy = !!grid(x, y+dy);
1039
+
1040
+ if(gdx && jumpX(Pos(x+dx, y), dx).isValid())
1041
+ break;
1042
+
1043
+ if(gdy && jumpY(Pos(x, y+dy), dy).isValid())
1044
+ break;
1045
+
1046
+ if((gdx || gdy) && grid(x+dx, y+dy))
1047
+ {
1048
+ p.x += dx;
1049
+ p.y += dy;
1050
+ }
1051
+ else
1052
+ {
1053
+ p = npos;
1054
+ break;
1055
+ }
1056
+ }
1057
+ stepsDone += steps;
1058
+ stepsRemain -= steps;
1059
+ return p;
1060
+ }
1061
+
1062
+ template <typename GRID> inline Position Searcher<GRID>::jumpX(Position p, int dx)
1063
+ {
1064
+ JPS_ASSERT(dx);
1065
+ JPS_ASSERT(grid(p.x, p.y));
1066
+
1067
+ const PosType y = p.y;
1068
+ const Position endpos = endPos;
1069
+ unsigned steps = 0;
1070
+
1071
+ unsigned a = ~((!!grid(p.x, y+1)) | ((!!grid(p.x, y-1)) << 1));
1072
+
1073
+ while(true)
1074
+ {
1075
+ const unsigned xx = p.x + dx;
1076
+ const unsigned b = (!!grid(xx, y+1)) | ((!!grid(xx, y-1)) << 1);
1077
+
1078
+ if((b & a) || p == endpos)
1079
+ break;
1080
+ if(!grid(xx, y))
1081
+ {
1082
+ p = npos;
1083
+ break;
1084
+ }
1085
+
1086
+ p.x += dx;
1087
+ a = ~b;
1088
+ ++steps;
1089
+ }
1090
+
1091
+ stepsDone += steps;
1092
+ stepsRemain -= steps;
1093
+ return p;
1094
+ }
1095
+
1096
+ template <typename GRID> inline Position Searcher<GRID>::jumpY(Position p, int dy)
1097
+ {
1098
+ JPS_ASSERT(dy);
1099
+ JPS_ASSERT(grid(p.x, p.y));
1100
+
1101
+ const PosType x = p.x;
1102
+ const Position endpos = endPos;
1103
+ unsigned steps = 0;
1104
+
1105
+ unsigned a = ~((!!grid(x+1, p.y)) | ((!!grid(x-1, p.y)) << 1));
1106
+
1107
+ while(true)
1108
+ {
1109
+ const unsigned yy = p.y + dy;
1110
+ const unsigned b = (!!grid(x+1, yy)) | ((!!grid(x-1, yy)) << 1);
1111
+
1112
+ if((a & b) || p == endpos)
1113
+ break;
1114
+ if(!grid(x, yy))
1115
+ {
1116
+ p = npos;
1117
+ break;
1118
+ }
1119
+
1120
+ p.y += dy;
1121
+ a = ~b;
1122
+ ++steps;
1123
+ }
1124
+
1125
+ stepsDone += steps;
1126
+ stepsRemain -= steps;
1127
+ return p;
1128
+ }
1129
+
1130
+ #define JPS_CHECKGRID(dx, dy) (grid(x+(dx), y+(dy)))
1131
+ #define JPS_ADDPOS(dx, dy) do { *w++ = Pos(x+(dx), y+(dy)); } while(0)
1132
+ #define JPS_ADDPOS_CHECK(dx, dy) do { if(JPS_CHECKGRID(dx, dy)) JPS_ADDPOS(dx, dy); } while(0)
1133
+ #define JPS_ADDPOS_NO_TUNNEL(dx, dy) do { if(grid(x+(dx),y) || grid(x,y+(dy))) JPS_ADDPOS_CHECK(dx, dy); } while(0)
1134
+
1135
+ template <typename GRID> unsigned Searcher<GRID>::findNeighborsJPS(const Node& n, Position *wptr) const
1136
+ {
1137
+ Position *w = wptr;
1138
+ const unsigned x = n.pos.x;
1139
+ const unsigned y = n.pos.y;
1140
+
1141
+ if(!n.hasParent())
1142
+ {
1143
+ // straight moves
1144
+ JPS_ADDPOS_CHECK(-1, 0);
1145
+ JPS_ADDPOS_CHECK(0, -1);
1146
+ JPS_ADDPOS_CHECK(0, 1);
1147
+ JPS_ADDPOS_CHECK(1, 0);
1148
+
1149
+ // diagonal moves + prevent tunneling
1150
+ JPS_ADDPOS_NO_TUNNEL(-1, -1);
1151
+ JPS_ADDPOS_NO_TUNNEL(-1, 1);
1152
+ JPS_ADDPOS_NO_TUNNEL(1, -1);
1153
+ JPS_ADDPOS_NO_TUNNEL(1, 1);
1154
+
1155
+ return unsigned(w - wptr);
1156
+ }
1157
+ const Node& p = n.getParent();
1158
+ // jump directions (both -1, 0, or 1)
1159
+ const int dx = Sgn<int>(x - p.pos.x);
1160
+ const int dy = Sgn<int>(y - p.pos.y);
1161
+
1162
+ if(dx && dy)
1163
+ {
1164
+ // diagonal
1165
+ // natural neighbors
1166
+ const bool walkX = !!grid(x+dx, y);
1167
+ if(walkX)
1168
+ *w++ = Pos(x+dx, y);
1169
+ const bool walkY = !!grid(x, y+dy);
1170
+ if(walkY)
1171
+ *w++ = Pos(x, y+dy);
1172
+
1173
+ if(walkX || walkY)
1174
+ JPS_ADDPOS_CHECK(dx, dy);
1175
+
1176
+ // forced neighbors
1177
+ if(walkY && !JPS_CHECKGRID(-dx,0))
1178
+ JPS_ADDPOS_CHECK(-dx, dy);
1179
+
1180
+ if(walkX && !JPS_CHECKGRID(0,-dy))
1181
+ JPS_ADDPOS_CHECK(dx, -dy);
1182
+ }
1183
+ else if(dx)
1184
+ {
1185
+ // along X axis
1186
+ if(JPS_CHECKGRID(dx, 0))
1187
+ {
1188
+ JPS_ADDPOS(dx, 0);
1189
+
1190
+ // Forced neighbors (+ prevent tunneling)
1191
+ if(!JPS_CHECKGRID(0, 1))
1192
+ JPS_ADDPOS_CHECK(dx, 1);
1193
+ if(!JPS_CHECKGRID(0,-1))
1194
+ JPS_ADDPOS_CHECK(dx,-1);
1195
+ }
1196
+ }
1197
+ else if(dy)
1198
+ {
1199
+ // along Y axis
1200
+ if(JPS_CHECKGRID(0, dy))
1201
+ {
1202
+ JPS_ADDPOS(0, dy);
1203
+
1204
+ // Forced neighbors (+ prevent tunneling)
1205
+ if(!JPS_CHECKGRID(1, 0))
1206
+ JPS_ADDPOS_CHECK(1, dy);
1207
+ if(!JPS_CHECKGRID(-1, 0))
1208
+ JPS_ADDPOS_CHECK(-1,dy);
1209
+ }
1210
+ }
1211
+
1212
+ return unsigned(w - wptr);
1213
+ }
1214
+
1215
+ //-------------- Plain old A* search ----------------
1216
+ template <typename GRID> unsigned Searcher<GRID>::findNeighborsAStar(const Node& n, Position *wptr)
1217
+ {
1218
+ Position *w = wptr;
1219
+ const int x = n.pos.x;
1220
+ const int y = n.pos.y;
1221
+ const int d = 1;
1222
+ JPS_ADDPOS_NO_TUNNEL(-d, -d);
1223
+ JPS_ADDPOS_CHECK ( 0, -d);
1224
+ JPS_ADDPOS_NO_TUNNEL(+d, -d);
1225
+ JPS_ADDPOS_CHECK (-d, 0);
1226
+ JPS_ADDPOS_CHECK (+d, 0);
1227
+ JPS_ADDPOS_NO_TUNNEL(-d, +d);
1228
+ JPS_ADDPOS_CHECK ( 0, +d);
1229
+ JPS_ADDPOS_NO_TUNNEL(+d, +d);
1230
+ stepsDone += 8;
1231
+ return unsigned(w - wptr);
1232
+ }
1233
+
1234
+ //-------------------------------------------------
1235
+ #undef JPS_ADDPOS
1236
+ #undef JPS_ADDPOS_CHECK
1237
+ #undef JPS_ADDPOS_NO_TUNNEL
1238
+ #undef JPS_CHECKGRID
1239
+
1240
+
1241
+ template <typename GRID> bool Searcher<GRID>::identifySuccessors(const Node& n_)
1242
+ {
1243
+ const SizeT nidx = storage.getindex(&n_);
1244
+ const Position np = n_.pos;
1245
+ Position buf[8];
1246
+
1247
+ const int num = (flags & JPS_Flag_AStarOnly)
1248
+ ? findNeighborsAStar(n_, &buf[0])
1249
+ : findNeighborsJPS(n_, &buf[0]);
1250
+
1251
+ for(int i = num-1; i >= 0; --i)
1252
+ {
1253
+ // Invariant: A node is only a valid neighbor if the corresponding grid position is walkable (asserted in jumpP)
1254
+ Position jp;
1255
+ if(flags & JPS_Flag_AStarOnly)
1256
+ jp = buf[i];
1257
+ else
1258
+ {
1259
+ jp = jumpP(buf[i], np);
1260
+ if(!jp.isValid())
1261
+ continue;
1262
+ }
1263
+ // Now that the grid position is definitely a valid jump point, we have to create the actual node.
1264
+ Node *jn = getNode(jp); // this might realloc the storage
1265
+ if(!jn)
1266
+ return false; // out of memory
1267
+
1268
+ Node& n = storage[nidx]; // get valid ref in case we realloc'd
1269
+ JPS_ASSERT(jn != &n);
1270
+ if(!jn->isClosed())
1271
+ _expandNode(jp, *jn, n);
1272
+ }
1273
+ return true;
1274
+ }
1275
+
1276
+ template <typename GRID> template<typename PV> bool Searcher<GRID>::findPath(PV& path, Position start, Position end, unsigned step, JPS_Flags flags)
1277
+ {
1278
+ JPS_Result res = findPathInit(start, end, flags);
1279
+
1280
+ // If this is true, the resulting path is empty (findPathFinish() would fail, so this needs to be checked before)
1281
+ if(res == JPS_EMPTY_PATH)
1282
+ return true;
1283
+
1284
+ while(true)
1285
+ {
1286
+ switch(res)
1287
+ {
1288
+ case JPS_NEED_MORE_STEPS:
1289
+ res = findPathStep(0);
1290
+ break; // the switch
1291
+
1292
+ case JPS_FOUND_PATH:
1293
+ return findPathFinish(path, step) == JPS_FOUND_PATH;
1294
+
1295
+ case JPS_EMPTY_PATH:
1296
+ JPS_ASSERT(false); // can't happen
1297
+ // fall through
1298
+ case JPS_NO_PATH:
1299
+ case JPS_OUT_OF_MEMORY:
1300
+ return false;
1301
+ }
1302
+ }
1303
+ }
1304
+
1305
+ template <typename GRID> JPS_Result Searcher<GRID>::findPathInit(Position start, Position end, JPS_Flags flags)
1306
+ {
1307
+ // This just resets a few counters; container memory isn't touched
1308
+ this->clear();
1309
+
1310
+ this->flags = flags;
1311
+ endPos = end;
1312
+
1313
+ // FIXME: check this
1314
+ if(start == end && !(flags & (JPS_Flag_NoStartCheck|JPS_Flag_NoEndCheck)))
1315
+ {
1316
+ // There is only a path if this single position is walkable.
1317
+ // But since the starting position is omitted in the output, there is nothing to do here.
1318
+ return grid(end.x, end.y) ? JPS_EMPTY_PATH : JPS_NO_PATH;
1319
+ }
1320
+
1321
+ if(!(flags & JPS_Flag_NoStartCheck))
1322
+ if(!grid(start.x, start.y))
1323
+ return JPS_NO_PATH;
1324
+
1325
+ if(!(flags & JPS_Flag_NoEndCheck))
1326
+ if(!grid(end.x, end.y))
1327
+ return JPS_NO_PATH;
1328
+
1329
+ Node *endNode = getNode(end); // this might realloc the internal storage...
1330
+ if(!endNode)
1331
+ return JPS_OUT_OF_MEMORY;
1332
+ endNodeIdx = storage.getindex(endNode); // .. so we keep this for later
1333
+
1334
+ Node *startNode = getNode(start); // this might also realloc
1335
+ if(!startNode)
1336
+ return JPS_OUT_OF_MEMORY;
1337
+ endNode = &storage[endNodeIdx]; // startNode is valid, make sure that endNode is valid too in case we reallocated
1338
+
1339
+ if(!(flags & JPS_Flag_NoGreedy))
1340
+ {
1341
+ // Try the quick way out first
1342
+ if(findPathGreedy(startNode, endNode))
1343
+ return JPS_FOUND_PATH;
1344
+ }
1345
+
1346
+ open.pushNode(startNode);
1347
+
1348
+ return JPS_NEED_MORE_STEPS;
1349
+ }
1350
+
1351
+ template <typename GRID> JPS_Result Searcher<GRID>::findPathStep(int limit)
1352
+ {
1353
+ stepsRemain = limit;
1354
+ do
1355
+ {
1356
+ if(open.empty())
1357
+ return JPS_NO_PATH;
1358
+ Node& n = open.popNode();
1359
+ n.setClosed();
1360
+ if(n.pos == endPos)
1361
+ return JPS_FOUND_PATH;
1362
+ if(!identifySuccessors(n))
1363
+ return JPS_OUT_OF_MEMORY;
1364
+ }
1365
+ while(stepsRemain >= 0);
1366
+ return JPS_NEED_MORE_STEPS;
1367
+ }
1368
+
1369
+ template<typename GRID> template<typename PV> JPS_Result Searcher<GRID>::findPathFinish(PV& path, unsigned step) const
1370
+ {
1371
+ return this->generatePath(path, step);
1372
+ }
1373
+
1374
+ template<typename GRID> bool Searcher<GRID>::findPathGreedy(Node *n, Node *endnode)
1375
+ {
1376
+ Position midpos = npos;
1377
+ PosType x = n->pos.x;
1378
+ PosType y = n->pos.y;
1379
+ const Position endpos = endnode->pos;
1380
+
1381
+ JPS_ASSERT(x != endpos.x || y != endpos.y); // must not be called when start==end
1382
+ JPS_ASSERT(n != endnode);
1383
+
1384
+ int dx = int(endpos.x - x);
1385
+ int dy = int(endpos.y - y);
1386
+ const int adx = Abs(dx);
1387
+ const int ady = Abs(dy);
1388
+ dx = Sgn(dx);
1389
+ dy = Sgn(dy);
1390
+
1391
+ // go diagonally first
1392
+ if(x != endpos.x && y != endpos.y)
1393
+ {
1394
+ JPS_ASSERT(dx && dy);
1395
+ const int minlen = Min(adx, ady);
1396
+ const PosType tx = x + dx * minlen;
1397
+ while(x != tx)
1398
+ {
1399
+ if(grid(x, y) && (grid(x+dx, y) || grid(x, y+dy))) // prevent tunneling as well
1400
+ {
1401
+ x += dx;
1402
+ y += dy;
1403
+ }
1404
+ else
1405
+ return false;
1406
+ }
1407
+
1408
+ if(!grid(x, y))
1409
+ return false;
1410
+
1411
+ midpos = Pos(x, y);
1412
+ }
1413
+
1414
+ // at this point, we're aligned to at least one axis
1415
+ JPS_ASSERT(x == endpos.x || y == endpos.y);
1416
+
1417
+ if(!(x == endpos.x && y == endpos.y))
1418
+ {
1419
+ while(x != endpos.x)
1420
+ if(!grid(x += dx, y))
1421
+ return false;
1422
+
1423
+ while(y != endpos.y)
1424
+ if(!grid(x, y += dy))
1425
+ return false;
1426
+
1427
+ JPS_ASSERT(x == endpos.x && y == endpos.y);
1428
+ }
1429
+
1430
+ if(midpos.isValid())
1431
+ {
1432
+ const unsigned nidx = storage.getindex(n);
1433
+ Node *mid = getNode(midpos); // this might invalidate n, endnode
1434
+ if(!mid)
1435
+ return false;
1436
+ n = &storage[nidx]; // reload pointers
1437
+ endnode = &storage[endNodeIdx];
1438
+ JPS_ASSERT(mid && mid != n);
1439
+ mid->setParent(*n);
1440
+ if(mid != endnode)
1441
+ endnode->setParent(*mid);
1442
+ }
1443
+ else
1444
+ endnode->setParent(*n);
1445
+
1446
+ return true;
1447
+ }
1448
+
1449
+ #undef JPS_ASSERT
1450
+ #undef JPS_realloc
1451
+ #undef JPS_free
1452
+ #undef JPS_sqrt
1453
+ #undef JPS_HEURISTIC_ACCURATE
1454
+ #undef JPS_HEURISTIC_ESTIMATE
1455
+
1456
+
1457
+ } // end namespace Internal
1458
+
1459
+ using Internal::Searcher;
1460
+
1461
+ typedef Internal::PodVec<Position> PathVector;
1462
+
1463
+ // Single-call convenience function. For efficiency, do NOT use this if you need to compute paths repeatedly.
1464
+ //
1465
+ // Returns: 0 if failed or no path could be found, otherwise number of steps taken.
1466
+ //
1467
+ // path: If the function returns success, the path is appended to this vector.
1468
+ // The path does NOT contain the starting position, i.e. if start and end are the same,
1469
+ // the resulting path has no elements.
1470
+ // The vector does not have to be empty. The function does not clear it;
1471
+ // instead, the new path positions are appended at the end.
1472
+ // This allows building a path incrementally.
1473
+ //
1474
+ // grid: Functor, expected to overload operator()(x, y), return true if position is walkable, false if not.
1475
+ //
1476
+ // step: If 0, only return waypoints.
1477
+ // If 1, create exhaustive step-by-step path.
1478
+ // If N, put in one position for N blocks travelled, or when a waypoint is hit.
1479
+ // All returned points are guaranteed to be on a straight line (vertically, horizontally, or diagonally),
1480
+ // and there is no obstruction between any two consecutive points.
1481
+ // Note that this parameter does NOT influence the pathfinding in any way;
1482
+ // it only controls the coarseness of the output path.
1483
+ template <typename GRID, typename PV>
1484
+ SizeT findPath(PV& path, const GRID& grid, PosType startx, PosType starty, PosType endx, PosType endy,
1485
+ unsigned step = 0, // optional
1486
+ JPS_Flags flags = JPS_Flag_Default,
1487
+ void *user = 0) // memory allocation userdata
1488
+ {
1489
+ Searcher<GRID> search(grid, user);
1490
+ if(!search.findPath(path, Pos(startx, starty), Pos(endx, endy), step, flags))
1491
+ return 0;
1492
+ const SizeT done = search.getStepsDone();
1493
+ return done + !done; // report at least 1 step; as 0 would indicate failure
1494
+ }
1495
+
1496
+ } // end namespace JPS
1497
+
1498
+
1499
+ /*
1500
+ Changes compared to the older JPS.h at https://github.com/fgenesis/jps:
1501
+
1502
+ - Explicitly freeing memory is no longer necessary. The freeMemory() method is still there
1503
+ and does its thing (drop all internal storage), but you never have to call it explicitly.
1504
+ Unlike the old version, there will be no performance degradation if you don't free memory every now and then.
1505
+ Actually it'll be slightly slower if you free memory and pathfind again for the first time,
1506
+ as it has to re-allocate internal data structures.
1507
+
1508
+ - Searcher::getNodesExpanded() is now reset to 0 upon starting a search.
1509
+
1510
+ - Added optional JPS_Flags parameter to pathfind (-init) functions to control search
1511
+ behavior. Compile-time #defines are gone.
1512
+
1513
+ - Removed skip parameter. Imho that one just added confusion and no real benefit.
1514
+ If you want it back for some reason: poke me, open an issue, whatever.
1515
+
1516
+ - Renamed JPS::Result to JPS_Result. Enum values gained JPS_ prefix, so JPS::NO_PATH is now JPS_NO_PATH, and so on.
1517
+
1518
+ - Added one more JPS_Result value: JPS_OUT_OF_MEMORY. See info block at the top how to handle this.
1519
+
1520
+ - Changed signature of Searcher<>::findPathFinish() to return JPS_Result (was bool).
1521
+ This is more in line with the other 2 methods, as it can now return JPS_OUT_OF_MEMORY.
1522
+
1523
+ - Changed signature of JPS::findPath(). Nonzero return is still success. Pointers to output stats are gone.
1524
+ Use a Searcher instance if you need the details.
1525
+
1526
+ - This version no longer depends on the C++ STL: <algorithm>, <vector>, <map>, operator new(), all gone.
1527
+ Makes things more memory- and cache-friendly, and quite a bit faster, too.
1528
+
1529
+ - The canonical file name is now "jps.hh" instead of "JPS.h"
1530
+ */
1531
+
1532
+
1533
+ /*
1534
+ TODO:
1535
+ - make int -> DirType
1536
+ - make possible to call findPathStep()/findPathFinish() even when JPS_EMPTY_PATH was returned on init (simplifies switch-case)
1537
+ - make node know its heap index
1538
+ - optional diagonals (make runtime param)
1539
+ */