jps 1.0.0

This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
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
+ */