寻路算法
1. A* 算法
1.1 算法介绍
寻路，即找到一条从某个起点到某个终点的可通过路径。而因为实际情况中，起点和终点之间的直线方向往往有障碍物，便需要一个搜索的算法来解决。
有一定算法基础的同学可能知道从某个起点到某个终点通常使用深度优先搜索（DFS），DFS搜索的搜索方向一般是8个方向（如果不允许搜索斜向，则有4个），但是并无优先之分。
为了让DFS搜索更加高效，结合贪心思想，我们给搜索方向赋予了优先级，直观上离终点最近的方向（直观上的意思是无视障碍物的情况下）为最优先搜索方向，这就是A*算法。
1.2 算法步骤解析
设绿色为起点，红色为重点，蓝色为不可通过的障碍，如下图所示，
从起点开始开始往周围各个方向进行搜索，
对于二位矩阵，搜索的方向有8个，如下图所示，
为了区分搜索方向的优先级，需要给每个要搜索的点赋予2个值：
- G值（耗费值）：指从起点走到该点要耗费的值
- H值（预测值）：指从该点走到终点的预测的值（从该点到终点无视障碍情况下预测要耗费的值，也可以理解成该点到终点的直线距离的值）
在简单的模型中，通常距离与这两个值成正比，在举例中可以将方格的长宽定义为10，则斜着走一个的距离为14，
但是在复杂的应用中，会有不同的权重，例如此次软挑赛中，可以考虑将货物的价值与距离这两个相关量来计算权重，
定义一个 F值（优先级值），使得 F = G + H
F 是从起点经过该店再到重点的预测总耗费值，通过计算优先值的大小，我们可以选择 F 值 最小的方向来进行搜索，计算出每个方向对应点的F，G，H值，如下图（左上角为F值，左下角为G值，右下角为H值），
还需要给这些点赋予当前节点的指针值用于回溯路径。因为一直搜下去搜到终点后，如果没有前一个点的指针，我们将无从得知要上次经过的是哪个点，只知道走到终点最终耗费的最小值是多少），
- 将这些点放入OpenList 开启链表，这个链表用于存放可以搜索的点，
- 再将当前点放入CloseList 关闭链表，这个链表用于存放已经搜索过的点，避免重复搜索同一个点，
- 再从 openList 取出一个F值最小的点，进行上述同样的搜索
在搜索过程中，如果搜索方向上的点是障碍物或者 CloseList 里的点，则跳过之。
通过递归式搜索，最终找到终点，
搜到终点后，通过前一个点的指针值，便能从终点回溯通过的路径点，（回溯点通过红色标点标记）
1.3 A* 算法优化思路
1.3.1 OpenList 使用优先队列（二叉堆）
OpenList 需要实时添加点，还要每次取出最小值的点，所以可以使用优先队列来作为 OpenList 的容器，
优先队列（二叉堆）：插入一个点的复杂度为O(logN)，取出一个最值点复杂度为O(logN)
1.3.2 障碍链表与 CloseList 使用二维表（二维数组）
障碍物与 CloseList 都是存储的二维坐标，所以可以使用二维数组的方式进行存储，
1
2
| bool BarrierList[Width][Height];
bool ClosetList[Width][Height];
|
1.3.3 深度限制
有时要搜的路径非常长，利用A*算法搜一次付出的代价很高，
那么为了保证每次搜索不会超过一定代价，可以设置深度限制，每搜一次则深度+1，搜到一定深度限制还没搜到终点，则返还失败值。
1.4 A*算法实现（C++代码）
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
| #include <iostream>
#include <list>
#include <vector>
#include <queue>
const int width = 100;
const int height = 100;
char mapBuffer[width][height];
int depth = 0;
const int depthLimit = 5000;
struct OpenPoint {
int x;
int y;
int cost;
int pred;
size_t father;
OpenPoint() = default;
OpenPoint(int pX, int pY, int endX, int endY, int c, size_t fatherIndex) : x(pX), y(pY), cost(c), father(fatherIndex) {
int relativeX = std::abs(endX - pX);
int relativeY = std::abs(endY - pY);
int n = relativeX - relativeY;
this->pred = std::max(relativeX, relativeY) * 14 - std::abs(n) * 4 + c;
}
};
std::vector<OpenPoint> pointPool;
size_t createOpenPoint(int pX, int pY, int endX, int endY, int c, size_t fatherIndex) {
pointPool.emplace_back(OpenPoint(pX, pY, endX, endY, c, fatherIndex));
return pointPool.size() - 1;
}
bool closeAndBarrierList[width + 1][height + 1];
bool inBarrierAndCloseList(int pX, int pY) {
if (pX < 0 || pY < 0 || pX >= width || pY >= height)
return true;
return closeAndBarrierList[pX][pY];
}
struct OpenPointCompare {
bool operator()(const size_t &a, const size_t &b) {
return pointPool[a].pred > pointPool[b].pred;
}
};
std::priority_queue<size_t, std::vector<size_t>, OpenPointCompare> openlist;
void open(size_t openPointIndex, int endX, int endY) {
int px = pointPool[openPointIndex].x;
int py = pointPool[openPointIndex].y;
if (inBarrierAndCloseList(px, py)) {
return;
}
closeAndBarrierList[px][py] = true;
int pcost = pointPool[openPointIndex].cost;
const int dir[8][2] = { {1,0},{0,1},{-1,0},{0,-1},{1,1},{ -1,1 },{ -1,-1 },{ 1,-1 } };
const int cost[8] = { 10,10,10,10,14,14,14,14 };
for (int i = 0; i < 8; ++i)
{
int toOpenX = px + dir[i][0];
int toOpenY = py + dir[i][1];
openlist.push(createOpenPoint(toOpenX, toOpenY, endX, endY, pcost + cost[i], openPointIndex));
}
}
std::list<size_t> findway(int startX, int startY, int endX, int endY) {
openlist.push(createOpenPoint(startX, startY, endX, endY, 0, -1));
size_t endIndex = -1;
while (!openlist.empty())
{
depth++;
if (depth >= depthLimit) {
return std::list<size_t>();
}
size_t openPointIndex = openlist.top();
openlist.pop();
if (pointPool[openPointIndex].x == endX && pointPool[openPointIndex].y == endY) {
endIndex = openPointIndex;
break;
}
open(openPointIndex, endX, endY);
}
std::list<size_t> road;
for (size_t index = endIndex; index != -1; index = pointPool[index].father) {
road.push_back(index);
}
return road;
}
void createMap() {
for (int i = 0; i < width; ++i)
for (int j = 0; j < height; ++j) {
if (rand() % 5 == 0) {
mapBuffer[i][j] = '*';
closeAndBarrierList[i][j] = true;
}
else {
mapBuffer[i][j] = ' ';
closeAndBarrierList[i][j] = false;
}
}
}
void printMap() {
for (int i = 0; i < width; ++i) {
for (int j = 0; j < height; ++j)
std::cout << mapBuffer[i][j];
std::cout << std::endl;
}
std::cout << std::endl << std::endl << std::endl;
}
int main() {
int beginX = 0;
int beginY = 0;
int endX = 99;
int endY = 99;
createMap();
mapBuffer[beginX][beginY] = mapBuffer[endX][endY] = ' ';
closeAndBarrierList[beginX][beginY] = closeAndBarrierList[endX][endY] = false;
std::list<size_t> road = findway(beginX, beginY, endX, endY);
for (size_t index : road) { mapBuffer[pointPool[index].x][pointPool[index].y] = 'O'; }
printMap();
system("pause");
return 0;
}
|
运行结果如下：
2. JPS 算法
2.1 算法介绍
JPS(Jump Point Search)译作跳点搜索算法，是于 2011 年提出的基于 Grid 格子的寻路算法， A* 算法在扩展节点时会把节点所有邻居都考虑进去，这样 OpenList 中点的数量会很多，搜索效率较慢。JPS 算法在保留 A* 算法的框架的同时，进一步优化了 A* 算法寻找后继节点的操作；
例如在无遮挡情况下（往往会有多条等价路径），我们希望起点到终点实际只取其中一条路径，而该路径外其它节点可以没必要放入 OpenList（不希望加入没必要的邻居）。
其次我们还希望直线方向上中途的点不用放入 OpenList，如果只放入每段直线子路径的起点和终点，那 OpenList 又可以少放很多没必要的节点：
可以看到 JPS 算法搜到的节点总是“跳跃性”的，这是因为这些关键性的节点都是需要改变行走方向的拐点，因此这也是 Jump Point 命名的来历。
2.2 强迫邻居（Forced Neighbour）
强迫邻居：节点 x 的8个邻居中有障碍，且 x 的父节点 p 经过 x 到达 n 的距离代价比不经过 x 到达的 n 的任意路径的距离代价小，则称 n 是 x 的强迫邻居。
直观来说，实际就是因为前进方向（父节点到 x 节点的方向为前进方向）的某一边的靠后位置有障碍物，因此想要到该边靠前的空位有最短的路径，就必须得经过过 x 节点。
可能的情况见图示，黑色为障碍，红圈即为强迫邻居，
（左图为直线方向情况下的强迫邻居，右图为斜方向情况下的强迫邻居）
2.3 跳点（Jump Point）
跳点：当前点 x 满足以下三个条件之一
- 节点 x 是起点/终点
- 节点 x 至少有一个强迫邻居
- 如果父节点在斜方向（意味着这是斜向搜索），节点 x 的水平或垂直方向上有满足前两个条件的点
节点 y 的水平或垂直方向是斜向向量的拆解，比如向量 d=(1,1) ，那么水平方向则是 (1,0) ，并不会往左搜索，只会看右边，如果向量 d=(-1,-1) ，那么水平方向是 (-1,0) ，只会搜索左边，不看右边，其他同理。
下图举例，由于黄色节点的父节点是在斜方向，其对应分解成向上和向右两个方向，因为在右方向发现一个蓝色跳点，因此黄色节点也应被判断为跳点：
2.4 实现原理
JPS 算法和A* 算法非常相似，步骤大概如下：
- OpenList 取一个权值最低的节点，然后开始搜索。（这些和A*是一样的）
- 搜索时，先进行 直线搜索（4/8个方向，跳跃搜索），然后再 斜向搜索（4个方向，只搜索一步）。如果期间某个方向搜索到跳点或者碰到障碍（或边界），则当前方向完成搜索，若有搜到跳点就添加进 OpenList 。
跳跃搜索是指沿直线方向一直搜下去（可能会搜到很多格），直到搜到跳点或者障碍（边界）。一开始从起点搜索，会有4个直线方向（上下左右），要是4个斜方向都前进了一步，此时直线方向会有8个。
- 若斜方向没完成搜索，则斜方向前进一步，重复上述过程。
因为直线方向是跳跃式搜索，所以总是能完成搜索。
- 若所有方向已完成搜索，则认为当前节点搜索完毕，将当前节点移除于 OpenList ，加入CloseList 。
- 重复取 OpenList 权值最低节点搜索，直到 OpenList 为空或者找到终点。
下面结合图片更好说明过程 2 和 3 ：首先我们从 OpenList 取出绿色的节点，作为搜索的开始，先进行直线搜索，再斜向搜索，没有找到任何跳点。
斜方向前进一步后，重复直线搜索和斜向搜索过程，仍没发现跳点。
斜方向前进两步后，重复直线搜索和斜向搜索过程，仍没发现跳点。
斜方向前进了三步后（假设当前位置为 x），在水平直线搜索上发现了一个跳点（紫色节点为强迫邻居）。
于是 x 也被判断为跳点，添加进 OpenList 。斜方向结束，绿色节点的搜索过程也就此结束，被移除于 OpenList ，放入 CloseList 。
2.5 示例过程
下面展示JPS算法更加完整的过程：
- 重复直线搜索和斜向搜索过程，斜方向前进了3步。在第3步判断出黄色节点为跳点（依据是水平方向有其它跳点），将黄色跳点放入OpenList ，然后斜方向搜索完成，绿色节点移除于OpenList ，放入CloseList 。
- 对 OpenList 下一个权值最低的节点（即黄色节点）开启搜索，在直线方向上发现了蓝色节点为跳点（依据是紫色节点为强迫邻居），类似地，放入 OpenList 。
- 由于斜方向还没结束，继续前进一步。最后一次直线搜索和斜向搜索都碰到了边界，因此黄色节点搜索完成，移除于 OpenList ，放入 CloseList 。
- 对 OpenList 下一个权值最低的节点（原为蓝色节点，下图变为黄色节点）开启搜索，直线搜索碰到边界，斜向搜索无果。斜方继续前进一步，仍然直线搜索碰到边界，斜向搜索无果。
- 最终在直线方向上发现了红色节点为跳点，因此蓝色节点先被判断为跳点，只添加蓝色节点进 OpenList 。斜方向完成，黄色节点搜索完成。
- 最后 OpenList 取出的蓝色节点开启搜索，在水平方向上发现红色节点，判断为终点，算法完成。
回忆起跳点的第三个判断条件（如果父节点在斜方向，节点 x 的水平或垂直方向上有满足条件a，b的点），会发现这个条件判断是最复杂的。在寻路过程中，它使寻路多次在水平节点上搜到跳点，也只能先添加它本身。其次，这也是算法中需要使用到递归的地方，是JPS算法性能瓶颈所在。
2.6 JPS算法实现（C++代码）
main.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
| #include <iostream>
#include<cstring>
#include<fstream>
#include<time.h>
#include"jps.h"
using namespace std;
int mapBuffer[WIDTH][HEIGHT];
void createMap() {
for (int i = 0; i < WIDTH; ++i)
for (int j = 0; j < HEIGHT; ++j) {
if (rand() % 20 == 0) {
mapBuffer[i][j] = 1;
}
else {
mapBuffer[i][j] = 0;
}
}
}
int main() {
int start_x = 0, start_y = 0;
int end_x = 199, end_y = 199;
time_t time_start_ms, time_end_ms;
createMap();
cout << "------------JPS---------------" << "\n";
Jps jps;
Jps::PathNode jStart = { start_y,start_x };
Jps::PathNode jEnd = { end_y, end_x };
time_start_ms = clock();
jps.Init(HEIGHT, WIDTH);
jps.FindPath(jStart, jEnd);
time_end_ms = clock();
cout << "Jps usage time:" << difftime(time_end_ms, time_start_ms) << "ms";
jps.PrintRoute();
jps.PrintRouteMap();
return 0;
}
|
jps.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
| #include <iostream>
#include<vector>
#include<cmath>
using namespace std;
using std::vector;
#define WIDTH 200
#define HEIGHT 200
extern int mapBuffer[WIDTH][HEIGHT];
class Jps
{
public:
enum Direct {
p_up, p_down, p_left, p_right, p_leftup, p_leftdown, p_rightup, p_rightdown
};
struct PathNode {
int row;
int col;
int g, h, f;
void GetF() {
f = g + h;
}
int value;
bool isroute;
bool isfind;
bool inopen;
bool isnull;
PathNode* parent;
vector<PathNode*> keyNeighbours;
};
int height, width;
int** map;
PathNode*** pathMap;
PathNode startNode;
PathNode endNode;
PathNode nullNode;
vector<PathNode> retPath;
int GetDis(const PathNode& startNode, const PathNode& endNode) {
int dis = sqrt(pow((endNode.col - startNode.col), 2) + pow((endNode.row - startNode.row), 2)) * 10;
return dis;
}
int GetH(const PathNode& startNode, const PathNode& endNode) {
int x = abs(startNode.col - endNode.col);
int y = abs(startNode.row - endNode.row);
return (x + y) * 10;
}
bool* Prune(short unitMap, char p, char n);
void Init(int _height, int _width);
PathNode JumpStraight(PathNode*** _pathMap, PathNode currenNode, Direct dir);
PathNode JumpOblique(PathNode*** _pathMap, PathNode currenNode, Direct dir);
vector<PathNode> FindPath(PathNode _startNode, PathNode _endNode);
void PrintRoute();
void PrintRouteMap();
};
|
jsp.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
| #include"jps.h"
bool* Jps::Prune(short unitMap,char p,char n){
static bool retType[2];
char obstaclePos = 0;
#if 0
char unitMapTmp =0;
if(p ==0){
unitMapTmp = unitMapTmp | (unitMap<<8 &(1<<8) );
unitMapTmp = unitMapTmp | (unitMap<<6 &(1<<7) );
unitMapTmp = unitMapTmp | (unitMap<<4 &(1<<6) );
unitMapTmp = unitMapTmp | (unitMap<<2 &(1<<5) );
unitMapTmp = unitMapTmp | (unitMap>>2 &(1<<3) );
unitMapTmp = unitMapTmp | (unitMap>>4 &(1<<2) );
unitMapTmp = unitMapTmp | (unitMap>>6 &(1<<1) );
unitMapTmp = unitMapTmp | (unitMap>>8 &(1<<0) );
p = 8;
}
#endif
if(p == 0){
if(n ==7 ||n ==5 || n ==8){
retType[0] = true;
retType[1] = false;
}
if(n ==2){
obstaclePos = unitMap>>1 & 0x01;
if( 1 == obstaclePos){
retType[0] = true;
retType[1] = true;
}
if( 0 == obstaclePos){
retType[0] = false;
retType[1] = false;
}
}
if(n ==6){
obstaclePos = unitMap>>3 & 0x01;
if( 1 == obstaclePos){
retType[0] = true;
retType[1] = true;
}
if( 0 == obstaclePos){
retType[0] = false;
retType[1] = false;
}
}
if(n == 1||n ==3){
retType[0] = false;
retType[1] = false;
}
}
if(p == 1){
if(n ==7){
retType[0] = true;
retType[1] = false;
}
if(n ==6){
obstaclePos = unitMap>>3 & 0x01;
if( 1 == obstaclePos){
retType[0] = true;
retType[1] = true;
}
if( 0 == obstaclePos){
retType[0] = false;
retType[1] = false;
}
}
if(n ==8){
obstaclePos = unitMap>>5 & 0x01;
if( 1 == obstaclePos){
retType[0] = true;
retType[1] = true;
}
if( 0 == obstaclePos){
retType[0] = false;
retType[1] = false;
}
}
if(n == 0||n ==2||n ==3||n ==5){
retType[0] = false;
retType[1] = false;
}
}
if(p == 2){
if(n ==7 ||n ==6 || n ==3){
retType[0] = true;
retType[1] = false;
}
if(n ==0){
obstaclePos = unitMap>>1 & 0x01;
if( 1 == obstaclePos){
retType[0] = true;
retType[1] = true;
}
if( 0 == obstaclePos){
retType[0] = false;
retType[1] = false;
}
}
if(n ==8){
obstaclePos = unitMap>>5 & 0x01;
if( 1 == obstaclePos){
retType[0] = true;
retType[1] = true;
}
if( 0 == obstaclePos){
retType[0] = false;
retType[1] = false;
}
}
if(n == 1||n ==5){
retType[0] = false;
retType[1] = false;
}
}
if(p == 3){
if(n ==5){
retType[0] = true;
retType[1] = false;
}
if(n ==2){
obstaclePos = unitMap>>1 & 0x01;
if( 1 == obstaclePos){
retType[0] = true;
retType[1] = true;
}
if( 0 == obstaclePos){
retType[0] = false;
retType[1] = false;
}
}
if(n ==8){
obstaclePos = unitMap>>7 & 0x01;
if( 1 == obstaclePos){
retType[0] = true;
retType[1] = true;
}
if( 0 == obstaclePos){
retType[0] = false;
retType[1] = false;
}
}
if(n == 0||n ==1||n ==6||n ==7){
retType[0] = false;
retType[1] = false;
}
}
if(p == 5){
if(n ==3){
retType[0] = true;
retType[1] = false;
}
if(n ==0){
obstaclePos = unitMap>>1 & 0x01;
if( 1 == obstaclePos){
retType[0] = true;
retType[1] = true;
}
if( 0 == obstaclePos){
retType[0] = false;
retType[1] = false;
}
}
if(n ==6){
obstaclePos = unitMap>>7 & 0x01;
if( 1 == obstaclePos){
retType[0] = true;
retType[1] = true;
}
if( 0 == obstaclePos){
retType[0] = false;
retType[1] = false;
}
}
if(n == 1||n ==2||n ==7||n ==8){
retType[0] = false;
retType[1] = false;
}
}
if(p == 6){
if(n ==1 ||n ==2 || n ==5){
retType[0] = true;
retType[1] = false;
}
if(n ==0){
obstaclePos = unitMap>>3 & 0x01;
if( 1 == obstaclePos){
retType[0] = true;
retType[1] = true;
}
if( 0 == obstaclePos){
retType[0] = false;
retType[1] = false;
}
}
if(n ==8){
obstaclePos = unitMap>>7 & 0x01;
if( 1 == obstaclePos){
retType[0] = true;
retType[1] = true;
}
if( 0 == obstaclePos){
retType[0] = false;
retType[1] = false;
}
}
if(n == 3||n ==7){
retType[0] = false;
retType[1] = false;
}
}
if(p == 7){
if(n ==1){
retType[0] = true;
retType[1] = false;
}
if(n ==0){
obstaclePos = unitMap>>3 & 0x01;
if( 1 == obstaclePos){
retType[0] = true;
retType[1] = true;
}
if( 0 == obstaclePos){
retType[0] = false;
retType[1] = false;
}
}
if(n ==2){
obstaclePos = unitMap>>5 & 0x01;
if( 1 == obstaclePos){
retType[0] = true;
retType[1] = true;
}
if( 0 == obstaclePos){
retType[0] = false;
retType[1] = false;
}
}
if(n == 3||n ==5||n ==6||n ==8){
retType[0] = false;
retType[1] = false;
}
}
if(p == 8){
if(n ==0 ||n ==1 || n ==3){
retType[0] = true;
retType[1] = false;
}
if(n ==2){
obstaclePos = unitMap>>5 & 0x01;
if( 1 == obstaclePos){
retType[0] = true;
retType[1] = true;
}
if( 0 == obstaclePos){
retType[0] = false;
retType[1] = false;
}
}
if(n ==6){
obstaclePos = unitMap>>7 & 0x01;
if( 1 == obstaclePos){
retType[0] = true;
retType[1] = true;
}
if( 0 == obstaclePos){
retType[0] = false;
retType[1] = false;
}
}
if(n == 5||n ==7){
retType[0] = false;
retType[1] = false;
}
}
return retType;
}
void Jps::Init(int _height, int _width) {
nullNode.isnull = true;
height = _height;
width = _width;
pathMap = new PathNode**[height];
for(int i=0;i<height;i++){
pathMap[i] = new PathNode*[width];
for(int j=0;j<width;j++){
pathMap[i][j] = new PathNode;
pathMap[i][j]->row = i;
pathMap[i][j]->col = j;
pathMap[i][j]->isfind = false;
pathMap[i][j]->isroute = false;
pathMap[i][j]->inopen = false;
pathMap[i][j]->value = mapBuffer[i][j];
pathMap[i][j]->parent = NULL;
}
}
}
Jps::PathNode Jps::JumpStraight(PathNode*** _pathMap,PathNode currenNode,Direct dir){
int delta_x = 0;
int delta_y = 0;
short unitMap = 0;
char valueT = 0;
bool* nodeTyp;
char parent;
char neighbGroup[9] = {9,9,9,9,9,9,9,9,9};
switch(dir)
{
case p_up:
delta_x = 0;
delta_y = -1;
parent = 7;
break;
case p_down:
delta_x = 0;
delta_y = 1;
parent = 1;
break;
case p_left:
delta_x = -1;
delta_y = 0;
parent = 5;
break;
case p_right:
delta_x = 1;
delta_y = 0;
parent = 3;
break;
default:
break;
}
PathNode nodeTmp = currenNode;
while(1){
nodeTmp.row += delta_y;
nodeTmp.col += delta_x;
if(nodeTmp.row == endNode.row &&
nodeTmp.col == endNode.col) return nodeTmp;
if (height <= nodeTmp.row || 0 > nodeTmp.row ||
width <= nodeTmp.col || 0 > nodeTmp.col ||
1 == _pathMap[nodeTmp.row][nodeTmp.col]->value
)
{
return nullNode;
}
unitMap = 0;
for(int i=0; i<9; i++){
neighbGroup[i] = 9;
}
for(int i = 0;i <3; i++){
for(int j= 0;j <3; j++){
int row_t = nodeTmp.row +i-1;
int col_t = nodeTmp.col +j-1;
if(height > row_t && 0 <= row_t &&
width > col_t && 0 <= col_t
){
valueT = _pathMap[row_t][col_t]->value;
unitMap = unitMap|valueT<<(i*3 +j);
if(1 != valueT){
neighbGroup[i*3+j] = (i*3 +j);
}
}
}
}
for(int i=0;i <9;i++){
if(9 != neighbGroup[i] &&
parent != neighbGroup[i] &&
4 != neighbGroup[i]
){
nodeTyp = Prune(unitMap, parent, neighbGroup[i]);
if(1 == nodeTyp[1]){
return nodeTmp;
}
}
}
}
}
Jps::PathNode Jps::JumpOblique(PathNode*** _pathMap,PathNode currenNode,Direct dir){
int delta_x = 0;
int delta_y = 0;
short unitMap = 0;
char valueT = 0;
bool* nodeTyp;
char parent;
char neighbGroup[9] = {9,9,9,9,9,9,9,9,9};
Direct straightDirs[2] = {p_up,p_up};
switch(dir)
{
case p_leftup:
delta_x = -1;
delta_y = -1;
parent = 8;
straightDirs[0] = p_left;
straightDirs[1] = p_up;
break;
case p_leftdown:
delta_x = -1;
delta_y = 1;
parent = 2;
straightDirs[0] = p_left;
straightDirs[1] = p_down;
break;
case p_rightup:
delta_x = 1;
delta_y = -1;
parent = 6;
straightDirs[0] = p_right;
straightDirs[1] = p_up;
break;
case p_rightdown:
delta_x = 1;
delta_y = 1;
parent = 0;
straightDirs[0] = p_right;
straightDirs[1] = p_down;
break;
default:
break;
}
PathNode nodeTmp = currenNode;
while(1){
nodeTmp.row += delta_y;
nodeTmp.col += delta_x;
if(nodeTmp.row == endNode.row &&
nodeTmp.col == endNode.col) return nodeTmp;
if(height <= nodeTmp.row || 0 > nodeTmp.row||
width <= nodeTmp.col || 0 > nodeTmp.col ||
1 == _pathMap[nodeTmp.row][nodeTmp.col]->value
) return nullNode;
unitMap = 0;
for(int i=0; i<9; i++){
neighbGroup[i] = 9;
}
for(int i = 0;i <3; i++){
for(int j= 0;j <3; j++){
int row_t = nodeTmp.row +i-1;
int col_t = nodeTmp.col +j-1;
if(height > row_t && 0 <= row_t &&
width > col_t && 0 <= col_t
){
valueT = _pathMap[row_t][col_t]->value;
unitMap = unitMap|valueT<<(i*3 +j);
if(1 != valueT){
neighbGroup[i*3+j] = (i*3 +j);
}
}
}
}
for(int i=0;i <9;i++){
if(9 != neighbGroup[i] &&
parent != neighbGroup[i] &&
4 != neighbGroup[i]
){
nodeTyp = Prune(unitMap, parent, neighbGroup[i]);
if(1 == nodeTyp[1]){
return nodeTmp;
}
}
}
PathNode jumpNode;
for(int i=0;i <2; i++){
jumpNode = JumpStraight(_pathMap, nodeTmp, straightDirs[i]);
if(false == jumpNode.isnull) return nodeTmp;
}
}
}
vector<Jps::PathNode> Jps::FindPath(PathNode _startNode,PathNode _endNode){
startNode = _startNode;
endNode = _endNode;
vector<Direct> jumpDirs;
vector<Direct>::iterator dirsIt;
PathNode jumpNode;
bool* nodeTyp;
PathNode currentNode;
vector<PathNode> openTree;
vector<PathNode>::iterator it;
vector<PathNode>::iterator minF_iter;
currentNode = startNode;
pathMap[currentNode.row][currentNode.col]->isfind = true;
for(int i=0;i <8;i++){
jumpDirs.push_back( (Direct)i);
}
while(1){
for(dirsIt = jumpDirs.begin();dirsIt != jumpDirs.end(); dirsIt++){
if( *(dirsIt) == p_up|| *(dirsIt) == p_down|| *(dirsIt) == p_left|| *(dirsIt) == p_right){
jumpNode = JumpStraight(pathMap, currentNode, (*dirsIt) );
}
if( *(dirsIt) == p_leftup|| *(dirsIt) == p_leftdown|| *(dirsIt) == p_rightup|| *(dirsIt) == p_rightdown){
jumpNode = JumpOblique(pathMap, currentNode, (*dirsIt) );
}
if(false == jumpNode.isnull && false == pathMap[jumpNode.row][jumpNode.col]->isfind){
jumpNode.g = pathMap[currentNode.row][currentNode.col]->g +GetDis( currentNode, jumpNode);
if(pathMap[jumpNode.row][jumpNode.col]->inopen){
if(pathMap[jumpNode.row][jumpNode.col]->g > jumpNode.g){
pathMap[jumpNode.row][jumpNode.col]->g = jumpNode.g;
pathMap[jumpNode.row][jumpNode.col]->GetF();
pathMap[jumpNode.row][jumpNode.col]->parent = pathMap[currentNode.row][currentNode.col];
}
}
if(false == pathMap[jumpNode.row][jumpNode.col]->inopen){
jumpNode.h = GetH(jumpNode, endNode);
jumpNode.GetF();
jumpNode.inopen = true;
openTree.push_back(jumpNode);
pathMap[jumpNode.row][jumpNode.col]->g = jumpNode.g;
pathMap[jumpNode.row][jumpNode.col]->h = jumpNode.h;
pathMap[jumpNode.row][jumpNode.col]->f = jumpNode.f;
pathMap[jumpNode.row][jumpNode.col]->parent = pathMap[currentNode.row][currentNode.col];
pathMap[jumpNode.row][jumpNode.col]->inopen = jumpNode.inopen;
}
}
}
if (openTree.size() == 0)
{
break;
}
minF_iter = openTree.begin();
for(it =openTree.begin();it !=openTree.end(); it++){
if(pathMap[(*it).row][(*it).col]->f < pathMap[(*minF_iter).row][(*minF_iter).col]->f){
minF_iter = it;
}
}
#if 0
cout<<endl<<"找到的下一点: ";
cout<<(*minF_iter).row<<","<<(*minF_iter).col<<endl;
cout<<"此节点父节点坐标：";
PathNode tmp = { (*minF_iter).row,(*minF_iter).col};
while(NULL != pathMap[tmp.row][tmp.col]->parent){
int t_row = tmp.row,t_col = tmp.col;
tmp.row = pathMap[t_row][t_col]->parent->row;
tmp.col = pathMap[t_row][t_col]->parent->col;
cout<<tmp.row<<","<<tmp.col<<" ";
}
cout<<endl;
#endif
currentNode = (*minF_iter);
pathMap[currentNode.row][currentNode.col]->isfind = true;
if(currentNode.row == endNode.row && currentNode.col == endNode.col) break;
openTree.erase(minF_iter);
jumpDirs.clear();
int delta_y = currentNode.row - pathMap[currentNode.row][currentNode.col]->parent->row;
int delta_x = currentNode.col - pathMap[currentNode.row][currentNode.col]->parent->col;
char p;
short unitMap = 0;
char neighbGroup[9] = {9,9,9,9,9,9,9,9,9};
if(0 > delta_y && 0 ==delta_x) p = 7;
if(0 < delta_y && 0 ==delta_x) p = 1;
if(0 == delta_y && 0 >delta_x) p = 5;
if(0 == delta_y && 0 <delta_x) p = 3;
if(0 > delta_y && 0 >delta_x) p = 8;
if(0 < delta_y && 0 >delta_x) p = 2;
if(0 > delta_y && 0 <delta_x) p = 6;
if(0 < delta_y && 0 <delta_x) p = 0;
for(int i = 0;i <3; i++){
for(int j= 0;j <3; j++){
int row_t = currentNode.row +i-1;
int col_t = currentNode.col +j-1;
if(height > row_t && 0 <= row_t &&
width > col_t && 0 <= col_t
){
int valueT = pathMap[row_t][col_t]->value;
unitMap = unitMap|valueT<<(i*3 +j);
if(1 != valueT){
neighbGroup[i*3+j] = (i*3 +j);
}
}
}
}
for(int i=0;i <9;i++){
if(9 != neighbGroup[i] &&
p != neighbGroup[i] &&
4 != neighbGroup[i]
){
nodeTyp = Prune(unitMap, p, neighbGroup[i]);
if(1 == nodeTyp[0]){
if(1==i) jumpDirs.push_back(p_up);
if(7==i) jumpDirs.push_back(p_down);
if(3==i) jumpDirs.push_back(p_left);
if(5==i) jumpDirs.push_back(p_right);
if(0==i) jumpDirs.push_back(p_leftup);
if(6==i) jumpDirs.push_back(p_leftdown);
if(2==i) jumpDirs.push_back(p_rightup);
if(8==i) jumpDirs.push_back(p_rightdown);
}
}
}
}
vector<PathNode> retPathTmp;
PathNode nodeTmp = endNode;
while(1){
int row_t = nodeTmp.row;
int col_t = nodeTmp.col;
retPathTmp.push_back(nodeTmp);
if(NULL == pathMap[nodeTmp.row][nodeTmp.col]->parent) break;
nodeTmp.row = pathMap[row_t][col_t]->parent->row;
nodeTmp.col = pathMap[row_t][col_t]->parent->col;
pathMap[nodeTmp.row][nodeTmp.col]->isroute = true;
}
for(it = retPathTmp.end()-1;it != retPathTmp.begin(); it--){
retPath.push_back(*it);
}
retPath.push_back(*it);
vector<PathNode>().swap(retPathTmp);
return retPath;
}
void Jps::PrintRoute(){
vector<PathNode>::iterator it;
float routLength = 0;
cout<<endl<<"route"<<"("<<retPath.size()<<"): ";
for(it =retPath.begin();it != retPath.end(); it++){
cout<<(*it).row<<","<<(*it).col<<" ";
if(it > retPath.begin()){
int row_t = (*it).row,col_t = (*it).col;
int row_t_l = (*(it -1) ).row,col_t_l = (*(it -1) ).col;
routLength += sqrt( pow( col_t - col_t_l,2) +pow( (row_t - row_t_l),2) );
}
}
cout<<endl;
cout<<"routLength:"<<routLength;
}
void Jps::PrintRouteMap(){
cout << endl;
for(int i=0;i < height;i++){
for(int j=0;j < width;j++){
if(pathMap[i][j]->isroute)
cout<<"O";
else
{
if(pathMap[i][j]->value)
cout << "*";
else
cout << " ";
}
}
cout<<endl;
}
}
|
运行结果如下：
3. JPS+（Jump Point Search Plus）
JPS+ 本质上也是 JPS寻路，只是加上了预处理来改进，从而使寻路更加快速。
3.1 预处理
- 我们首先对地图每个节点进行跳点判断，找出所有主要跳点：
- 然后对每个节点进行跳点的直线可达性判断，并记录好跳点直线可达性：
以上预处理过程需要有一个数据结构存储地图上每个格子8个方向距离碰撞或跳点的距离。
3.2 示例过程
做好了地图的预处理之后，我们就可以使用JPS+算法了。大致思路与JPS算法相同，不过这次有了预处理的数据，我们可以更快的进行直线搜索和斜向搜索。
在某个搜索方向上有：
- 对于正数距离 n（意味着距离跳点 n 格），我们可以直接将n步远的节点作为跳点添加进OpenList ；
- 对于 0 距离（意味着一步都不可移动），我们无需在该方向搜索；
- 对于负数距离 -n（意味着距离边界或障碍 n 格），我们直接将 n 步远的节点进行一次跳点判断（有可能满足跳点的第三条件，不过得益于预处理的数据，这步也可以很快完成）。
如下图示，起始节点通过已记录的向上距离，直接将3步远的跳点添加进 OpenList ，而不再像以前需要迭代三步（还每步都要判断是否跳点）：
其它过程也是类似的：
4. 总结
可以看到 JPS/JPS+ 算法里只有跳点才会被加入 OpenList 里，排除了大量不必要的点，最后找出来的最短路径也是由跳点组成。这也是 JPS/JPS+ 高效的主要原因。
JPS:
- 绝大部分地图，使用 JPS 算法都会比 A* 算法更快，内存占用也更小（OpenList 里节点少了很多）;
- JPS 在跳点判断上，要尽可能避免递归的深度过大（或者期待一下以后出现避免递归的算法），否则在超大型的地图里递归判断跳点可能会造成灾难;
- JPS 也可以用于动态变化的地图，只是每次地图改变都需要再进行一次 JPS 搜索;
- JPS 天生拥有合并节点（亦或者说是在一条直线里移除中间不必要节点）的功能，但是仍存在一些可以继续合并的地方;
- JPS 只适用于 网格（grid）节点类型，不支持 Navmesh 或者路径点(Way Point)。
JPS+
- JPS+ 相比 JPS 算法又是更快上一个档次（特别是避免了过多层递归判断跳点），内存占用则是每个格子需要额外记录8个方向的距离数据;
- JPS+ 算法由于包含预处理过程，这让它面对动态变化的地图有天生的劣势（几乎是不可以接受动态地图的），因此更适合用于静态地图;
- JPS+ 预处理的复杂度为，n 代表地图格子数。
| 算法 |
性能 |
内存占用 |
支持动态地图 |
预处理 |
支持节点类型 |
| A* |
中等 |
大 |
支持 |
无 |
网格、Navmesh、路径点 |
| JPS |
快 |
小 |
支持 |
无 |
网格 |
| JPS+ |
非常快 |
大 |
不支持 |
有， |
网格 |
对比 A* 与 JPS 算法的存储结果如下：
参考网页：
- 最快速的寻路算法 Jump Point Search - 知乎 (zhihu.com)
- JPS/JPS+ 寻路算法 - KillerAery - 博客园 (cnblogs.com)
- A* 寻路算法 - KillerAery - 博客园 (cnblogs.com)
- C++ 语言中 priority_queue 的常见用法详解 - 知乎 (zhihu.com)
- C++对bool operator < (const p &a)const的运算符重载详解 - __MEET - 博客园 (cnblogs.com)
- Jump Point Search(JPS)算法总结与实现（附Demo) - ThisIsRone - 博客园 (cnblogs.com)
- JPS 路径规划算法 - 星火-AI - 博客园 (cnblogs.com)
- SteveRabin/JPSPlusWithGoalBounding: JPS+ and Goal Bounding (github.com)
- cloud8352/Jps: JPS寻路算法 (github.com)
