@esengine/pathfinding 13.2.0 → 13.3.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.
- package/dist/{KDTree-2rs2EXvm.d.ts → CollisionResolver-CSgWsegP.d.ts} +122 -86
- package/dist/FlowController-Dc3nuLq5.d.ts +2751 -0
- package/dist/KDTree-BRpn7O8K.d.ts +216 -0
- package/dist/avoidance.d.ts +26 -4
- package/dist/avoidance.js +10 -2
- package/dist/{chunk-JTZP55BJ.js → chunk-3VEX32JO.js} +385 -9
- package/dist/chunk-3VEX32JO.js.map +1 -0
- package/dist/chunk-H5EFZBBT.js +1 -0
- package/dist/chunk-NIKT3PQC.js +3811 -0
- package/dist/chunk-NIKT3PQC.js.map +1 -0
- package/dist/ecs.d.ts +440 -647
- package/dist/ecs.js +1003 -1399
- package/dist/ecs.js.map +1 -1
- package/dist/index.d.ts +153 -711
- package/dist/index.js +1335 -1735
- package/dist/index.js.map +1 -1
- package/package.json +3 -3
- package/dist/IIncrementalPathfinding-3qs7e_pO.d.ts +0 -450
- package/dist/LinearProgram-DyD3pI6v.d.ts +0 -56
- package/dist/chunk-JTZP55BJ.js.map +0 -1
- package/dist/chunk-KEYTX37K.js +0 -1
- package/dist/chunk-VNC2YAAL.js +0 -1650
- package/dist/chunk-VNC2YAAL.js.map +0 -1
- /package/dist/{chunk-KEYTX37K.js.map → chunk-H5EFZBBT.js.map} +0 -0
package/dist/index.js
CHANGED
|
@@ -1,326 +1,71 @@
|
|
|
1
1
|
import {
|
|
2
|
-
|
|
3
|
-
|
|
4
|
-
|
|
2
|
+
AStarPathfinder,
|
|
3
|
+
BinaryHeap,
|
|
4
|
+
CollisionResolverAdapter,
|
|
5
|
+
DEFAULT_FLOW_CONTROLLER_CONFIG,
|
|
6
|
+
DEFAULT_HPA_CONFIG,
|
|
7
|
+
DEFAULT_ORCA_PARAMS,
|
|
5
8
|
DEFAULT_PATHFINDING_OPTIONS,
|
|
6
9
|
DEFAULT_PATH_CACHE_CONFIG,
|
|
7
|
-
|
|
8
|
-
DIRECTIONS_8,
|
|
10
|
+
EMPTY_COLLISION_RESULT,
|
|
9
11
|
EMPTY_PATH_RESULT,
|
|
10
|
-
|
|
11
|
-
|
|
12
|
+
EMPTY_PLAN_RESULT,
|
|
13
|
+
FlowController,
|
|
14
|
+
GridPathfinderAdapter,
|
|
15
|
+
HPAPathfinder,
|
|
12
16
|
IncrementalAStarPathfinder,
|
|
17
|
+
IncrementalGridPathPlannerAdapter,
|
|
13
18
|
IndexedBinaryHeap,
|
|
14
|
-
|
|
15
|
-
|
|
19
|
+
JPSPathfinder,
|
|
20
|
+
NavMeshPathPlannerAdapter,
|
|
21
|
+
ORCALocalAvoidanceAdapter,
|
|
22
|
+
PassPermission,
|
|
16
23
|
PathCache,
|
|
17
|
-
|
|
18
|
-
bresenhamLineOfSight,
|
|
24
|
+
PathPlanState,
|
|
19
25
|
chebyshevDistance,
|
|
20
|
-
|
|
21
|
-
|
|
22
|
-
|
|
26
|
+
createAStarPathfinder,
|
|
27
|
+
createAStarPlanner,
|
|
28
|
+
createDefaultCollisionResolver,
|
|
29
|
+
createFlowController,
|
|
30
|
+
createHPAPathfinder,
|
|
31
|
+
createHPAPlanner,
|
|
23
32
|
createIncrementalAStarPathfinder,
|
|
24
|
-
|
|
25
|
-
|
|
33
|
+
createIncrementalAStarPlanner,
|
|
34
|
+
createJPSPathfinder,
|
|
35
|
+
createJPSPlanner,
|
|
36
|
+
createNavMeshPathPlanner,
|
|
37
|
+
createORCAAvoidance,
|
|
26
38
|
createPathCache,
|
|
27
|
-
createPathValidator,
|
|
28
39
|
createPoint,
|
|
29
40
|
euclideanDistance,
|
|
41
|
+
isIncrementalPlanner,
|
|
30
42
|
manhattanDistance,
|
|
31
|
-
octileDistance
|
|
32
|
-
|
|
33
|
-
} from "./chunk-VNC2YAAL.js";
|
|
43
|
+
octileDistance
|
|
44
|
+
} from "./chunk-NIKT3PQC.js";
|
|
34
45
|
import {
|
|
35
46
|
DEFAULT_REPLANNING_CONFIG,
|
|
36
47
|
EMPTY_PROGRESS,
|
|
37
48
|
PathfindingState
|
|
38
49
|
} from "./chunk-YKA3PWU3.js";
|
|
39
|
-
import "./chunk-
|
|
50
|
+
import "./chunk-H5EFZBBT.js";
|
|
40
51
|
import {
|
|
52
|
+
CollisionResolver,
|
|
41
53
|
DEFAULT_AGENT_PARAMS,
|
|
54
|
+
DEFAULT_COLLISION_CONFIG,
|
|
42
55
|
DEFAULT_ORCA_CONFIG,
|
|
56
|
+
EMPTY_COLLISION,
|
|
43
57
|
KDTree,
|
|
44
58
|
ORCASolver,
|
|
59
|
+
createCollisionResolver,
|
|
45
60
|
createKDTree,
|
|
46
61
|
createORCASolver,
|
|
47
62
|
solveORCALinearProgram
|
|
48
|
-
} from "./chunk-
|
|
63
|
+
} from "./chunk-3VEX32JO.js";
|
|
49
64
|
import {
|
|
50
65
|
__name,
|
|
51
66
|
__publicField
|
|
52
67
|
} from "./chunk-T626JPC7.js";
|
|
53
68
|
|
|
54
|
-
// src/core/BinaryHeap.ts
|
|
55
|
-
var _BinaryHeap = class _BinaryHeap {
|
|
56
|
-
/**
|
|
57
|
-
* @zh 创建二叉堆
|
|
58
|
-
* @en Create binary heap
|
|
59
|
-
*
|
|
60
|
-
* @param compare - @zh 比较函数,返回负数表示 a < b @en Compare function, returns negative if a < b
|
|
61
|
-
*/
|
|
62
|
-
constructor(compare) {
|
|
63
|
-
__publicField(this, "heap", []);
|
|
64
|
-
__publicField(this, "compare");
|
|
65
|
-
this.compare = compare;
|
|
66
|
-
}
|
|
67
|
-
/**
|
|
68
|
-
* @zh 堆大小
|
|
69
|
-
* @en Heap size
|
|
70
|
-
*/
|
|
71
|
-
get size() {
|
|
72
|
-
return this.heap.length;
|
|
73
|
-
}
|
|
74
|
-
/**
|
|
75
|
-
* @zh 是否为空
|
|
76
|
-
* @en Is empty
|
|
77
|
-
*/
|
|
78
|
-
get isEmpty() {
|
|
79
|
-
return this.heap.length === 0;
|
|
80
|
-
}
|
|
81
|
-
/**
|
|
82
|
-
* @zh 插入元素
|
|
83
|
-
* @en Push element
|
|
84
|
-
*/
|
|
85
|
-
push(item) {
|
|
86
|
-
this.heap.push(item);
|
|
87
|
-
this.bubbleUp(this.heap.length - 1);
|
|
88
|
-
}
|
|
89
|
-
/**
|
|
90
|
-
* @zh 弹出最小元素
|
|
91
|
-
* @en Pop minimum element
|
|
92
|
-
*/
|
|
93
|
-
pop() {
|
|
94
|
-
if (this.heap.length === 0) {
|
|
95
|
-
return void 0;
|
|
96
|
-
}
|
|
97
|
-
const result = this.heap[0];
|
|
98
|
-
const last = this.heap.pop();
|
|
99
|
-
if (this.heap.length > 0) {
|
|
100
|
-
this.heap[0] = last;
|
|
101
|
-
this.sinkDown(0);
|
|
102
|
-
}
|
|
103
|
-
return result;
|
|
104
|
-
}
|
|
105
|
-
/**
|
|
106
|
-
* @zh 查看最小元素(不移除)
|
|
107
|
-
* @en Peek minimum element (without removing)
|
|
108
|
-
*/
|
|
109
|
-
peek() {
|
|
110
|
-
return this.heap[0];
|
|
111
|
-
}
|
|
112
|
-
/**
|
|
113
|
-
* @zh 更新元素(重新排序)
|
|
114
|
-
* @en Update element (re-sort)
|
|
115
|
-
*/
|
|
116
|
-
update(item) {
|
|
117
|
-
const index = this.heap.indexOf(item);
|
|
118
|
-
if (index !== -1) {
|
|
119
|
-
this.bubbleUp(index);
|
|
120
|
-
this.sinkDown(index);
|
|
121
|
-
}
|
|
122
|
-
}
|
|
123
|
-
/**
|
|
124
|
-
* @zh 检查是否包含元素
|
|
125
|
-
* @en Check if contains element
|
|
126
|
-
*/
|
|
127
|
-
contains(item) {
|
|
128
|
-
return this.heap.indexOf(item) !== -1;
|
|
129
|
-
}
|
|
130
|
-
/**
|
|
131
|
-
* @zh 清空堆
|
|
132
|
-
* @en Clear heap
|
|
133
|
-
*/
|
|
134
|
-
clear() {
|
|
135
|
-
this.heap.length = 0;
|
|
136
|
-
}
|
|
137
|
-
/**
|
|
138
|
-
* @zh 上浮操作
|
|
139
|
-
* @en Bubble up operation
|
|
140
|
-
*/
|
|
141
|
-
bubbleUp(index) {
|
|
142
|
-
const item = this.heap[index];
|
|
143
|
-
while (index > 0) {
|
|
144
|
-
const parentIndex = Math.floor((index - 1) / 2);
|
|
145
|
-
const parent = this.heap[parentIndex];
|
|
146
|
-
if (this.compare(item, parent) >= 0) {
|
|
147
|
-
break;
|
|
148
|
-
}
|
|
149
|
-
this.heap[index] = parent;
|
|
150
|
-
index = parentIndex;
|
|
151
|
-
}
|
|
152
|
-
this.heap[index] = item;
|
|
153
|
-
}
|
|
154
|
-
/**
|
|
155
|
-
* @zh 下沉操作
|
|
156
|
-
* @en Sink down operation
|
|
157
|
-
*/
|
|
158
|
-
sinkDown(index) {
|
|
159
|
-
const length = this.heap.length;
|
|
160
|
-
const item = this.heap[index];
|
|
161
|
-
while (true) {
|
|
162
|
-
const leftIndex = 2 * index + 1;
|
|
163
|
-
const rightIndex = 2 * index + 2;
|
|
164
|
-
let smallest = index;
|
|
165
|
-
if (leftIndex < length && this.compare(this.heap[leftIndex], this.heap[smallest]) < 0) {
|
|
166
|
-
smallest = leftIndex;
|
|
167
|
-
}
|
|
168
|
-
if (rightIndex < length && this.compare(this.heap[rightIndex], this.heap[smallest]) < 0) {
|
|
169
|
-
smallest = rightIndex;
|
|
170
|
-
}
|
|
171
|
-
if (smallest === index) {
|
|
172
|
-
break;
|
|
173
|
-
}
|
|
174
|
-
this.heap[index] = this.heap[smallest];
|
|
175
|
-
this.heap[smallest] = item;
|
|
176
|
-
index = smallest;
|
|
177
|
-
}
|
|
178
|
-
}
|
|
179
|
-
};
|
|
180
|
-
__name(_BinaryHeap, "BinaryHeap");
|
|
181
|
-
var BinaryHeap = _BinaryHeap;
|
|
182
|
-
|
|
183
|
-
// src/core/AStarPathfinder.ts
|
|
184
|
-
var _AStarPathfinder = class _AStarPathfinder {
|
|
185
|
-
constructor(map) {
|
|
186
|
-
__publicField(this, "map");
|
|
187
|
-
__publicField(this, "nodeCache", /* @__PURE__ */ new Map());
|
|
188
|
-
__publicField(this, "openList");
|
|
189
|
-
this.map = map;
|
|
190
|
-
this.openList = new IndexedBinaryHeap((a, b) => a.f - b.f);
|
|
191
|
-
}
|
|
192
|
-
/**
|
|
193
|
-
* @zh 查找路径
|
|
194
|
-
* @en Find path
|
|
195
|
-
*/
|
|
196
|
-
findPath(startX, startY, endX, endY, options) {
|
|
197
|
-
const opts = {
|
|
198
|
-
...DEFAULT_PATHFINDING_OPTIONS,
|
|
199
|
-
...options
|
|
200
|
-
};
|
|
201
|
-
this.clear();
|
|
202
|
-
const startNode = this.map.getNodeAt(startX, startY);
|
|
203
|
-
const endNode = this.map.getNodeAt(endX, endY);
|
|
204
|
-
if (!startNode || !endNode) {
|
|
205
|
-
return EMPTY_PATH_RESULT;
|
|
206
|
-
}
|
|
207
|
-
if (!startNode.walkable || !endNode.walkable) {
|
|
208
|
-
return EMPTY_PATH_RESULT;
|
|
209
|
-
}
|
|
210
|
-
if (startNode.id === endNode.id) {
|
|
211
|
-
return {
|
|
212
|
-
found: true,
|
|
213
|
-
path: [
|
|
214
|
-
startNode.position
|
|
215
|
-
],
|
|
216
|
-
cost: 0,
|
|
217
|
-
nodesSearched: 1
|
|
218
|
-
};
|
|
219
|
-
}
|
|
220
|
-
const start = this.getOrCreateAStarNode(startNode);
|
|
221
|
-
start.g = 0;
|
|
222
|
-
start.h = this.map.heuristic(startNode.position, endNode.position) * opts.heuristicWeight;
|
|
223
|
-
start.f = start.h;
|
|
224
|
-
start.opened = true;
|
|
225
|
-
this.openList.push(start);
|
|
226
|
-
let nodesSearched = 0;
|
|
227
|
-
const endPosition = endNode.position;
|
|
228
|
-
while (!this.openList.isEmpty && nodesSearched < opts.maxNodes) {
|
|
229
|
-
const current = this.openList.pop();
|
|
230
|
-
current.closed = true;
|
|
231
|
-
nodesSearched++;
|
|
232
|
-
if (current.node.id === endNode.id) {
|
|
233
|
-
return this.buildPath(current, nodesSearched);
|
|
234
|
-
}
|
|
235
|
-
const neighbors = this.map.getNeighbors(current.node);
|
|
236
|
-
for (const neighborNode of neighbors) {
|
|
237
|
-
if (!neighborNode.walkable) {
|
|
238
|
-
continue;
|
|
239
|
-
}
|
|
240
|
-
const neighbor = this.getOrCreateAStarNode(neighborNode);
|
|
241
|
-
if (neighbor.closed) {
|
|
242
|
-
continue;
|
|
243
|
-
}
|
|
244
|
-
const movementCost = this.map.getMovementCost(current.node, neighborNode);
|
|
245
|
-
const tentativeG = current.g + movementCost;
|
|
246
|
-
if (!neighbor.opened) {
|
|
247
|
-
neighbor.g = tentativeG;
|
|
248
|
-
neighbor.h = this.map.heuristic(neighborNode.position, endPosition) * opts.heuristicWeight;
|
|
249
|
-
neighbor.f = neighbor.g + neighbor.h;
|
|
250
|
-
neighbor.parent = current;
|
|
251
|
-
neighbor.opened = true;
|
|
252
|
-
this.openList.push(neighbor);
|
|
253
|
-
} else if (tentativeG < neighbor.g) {
|
|
254
|
-
neighbor.g = tentativeG;
|
|
255
|
-
neighbor.f = neighbor.g + neighbor.h;
|
|
256
|
-
neighbor.parent = current;
|
|
257
|
-
this.openList.update(neighbor);
|
|
258
|
-
}
|
|
259
|
-
}
|
|
260
|
-
}
|
|
261
|
-
return {
|
|
262
|
-
found: false,
|
|
263
|
-
path: [],
|
|
264
|
-
cost: 0,
|
|
265
|
-
nodesSearched
|
|
266
|
-
};
|
|
267
|
-
}
|
|
268
|
-
/**
|
|
269
|
-
* @zh 清理状态
|
|
270
|
-
* @en Clear state
|
|
271
|
-
*/
|
|
272
|
-
clear() {
|
|
273
|
-
this.nodeCache.clear();
|
|
274
|
-
this.openList.clear();
|
|
275
|
-
}
|
|
276
|
-
/**
|
|
277
|
-
* @zh 获取或创建 A* 节点
|
|
278
|
-
* @en Get or create A* node
|
|
279
|
-
*/
|
|
280
|
-
getOrCreateAStarNode(node) {
|
|
281
|
-
let astarNode = this.nodeCache.get(node.id);
|
|
282
|
-
if (!astarNode) {
|
|
283
|
-
astarNode = {
|
|
284
|
-
node,
|
|
285
|
-
g: Infinity,
|
|
286
|
-
h: 0,
|
|
287
|
-
f: Infinity,
|
|
288
|
-
parent: null,
|
|
289
|
-
closed: false,
|
|
290
|
-
opened: false,
|
|
291
|
-
heapIndex: -1
|
|
292
|
-
};
|
|
293
|
-
this.nodeCache.set(node.id, astarNode);
|
|
294
|
-
}
|
|
295
|
-
return astarNode;
|
|
296
|
-
}
|
|
297
|
-
/**
|
|
298
|
-
* @zh 构建路径结果
|
|
299
|
-
* @en Build path result
|
|
300
|
-
*/
|
|
301
|
-
buildPath(endNode, nodesSearched) {
|
|
302
|
-
const path = [];
|
|
303
|
-
let current = endNode;
|
|
304
|
-
while (current) {
|
|
305
|
-
path.push(current.node.position);
|
|
306
|
-
current = current.parent;
|
|
307
|
-
}
|
|
308
|
-
path.reverse();
|
|
309
|
-
return {
|
|
310
|
-
found: true,
|
|
311
|
-
path,
|
|
312
|
-
cost: endNode.g,
|
|
313
|
-
nodesSearched
|
|
314
|
-
};
|
|
315
|
-
}
|
|
316
|
-
};
|
|
317
|
-
__name(_AStarPathfinder, "AStarPathfinder");
|
|
318
|
-
var AStarPathfinder = _AStarPathfinder;
|
|
319
|
-
function createAStarPathfinder(map) {
|
|
320
|
-
return new AStarPathfinder(map);
|
|
321
|
-
}
|
|
322
|
-
__name(createAStarPathfinder, "createAStarPathfinder");
|
|
323
|
-
|
|
324
69
|
// src/core/GridPathfinder.ts
|
|
325
70
|
var CLOSED_FLAG = 1;
|
|
326
71
|
var OPENED_FLAG = 2;
|
|
@@ -967,1555 +712,510 @@ function createGridPathfinder(map, config) {
|
|
|
967
712
|
}
|
|
968
713
|
__name(createGridPathfinder, "createGridPathfinder");
|
|
969
714
|
|
|
970
|
-
// src/core/
|
|
971
|
-
var
|
|
972
|
-
constructor(map) {
|
|
973
|
-
__publicField(this, "map");
|
|
974
|
-
__publicField(this, "width");
|
|
975
|
-
__publicField(this, "height");
|
|
976
|
-
__publicField(this, "openList");
|
|
977
|
-
__publicField(this, "nodeGrid");
|
|
978
|
-
this.map = map;
|
|
979
|
-
const bounds = this.getMapBounds();
|
|
980
|
-
this.width = bounds.width;
|
|
981
|
-
this.height = bounds.height;
|
|
982
|
-
this.openList = new BinaryHeap((a, b) => a.f - b.f);
|
|
983
|
-
this.nodeGrid = [];
|
|
984
|
-
}
|
|
715
|
+
// src/core/PathValidator.ts
|
|
716
|
+
var _PathValidator = class _PathValidator {
|
|
985
717
|
/**
|
|
986
|
-
* @zh
|
|
987
|
-
* @en
|
|
988
|
-
|
|
989
|
-
|
|
990
|
-
|
|
991
|
-
|
|
992
|
-
|
|
993
|
-
|
|
994
|
-
|
|
995
|
-
|
|
996
|
-
|
|
997
|
-
|
|
998
|
-
|
|
999
|
-
|
|
1000
|
-
|
|
1001
|
-
|
|
1002
|
-
x: startX,
|
|
1003
|
-
y: startY
|
|
1004
|
-
}
|
|
1005
|
-
],
|
|
1006
|
-
cost: 0,
|
|
1007
|
-
nodesSearched: 1
|
|
1008
|
-
};
|
|
1009
|
-
}
|
|
1010
|
-
this.initGrid();
|
|
1011
|
-
this.openList.clear();
|
|
1012
|
-
const startNode = this.getOrCreateNode(startX, startY);
|
|
1013
|
-
startNode.g = 0;
|
|
1014
|
-
startNode.h = this.heuristic(startX, startY, endX, endY) * opts.heuristicWeight;
|
|
1015
|
-
startNode.f = startNode.h;
|
|
1016
|
-
this.openList.push(startNode);
|
|
1017
|
-
let nodesSearched = 0;
|
|
1018
|
-
while (!this.openList.isEmpty && nodesSearched < opts.maxNodes) {
|
|
1019
|
-
const current = this.openList.pop();
|
|
1020
|
-
current.closed = true;
|
|
1021
|
-
nodesSearched++;
|
|
1022
|
-
if (current.x === endX && current.y === endY) {
|
|
718
|
+
* @zh 验证路径段的有效性
|
|
719
|
+
* @en Validate path segment validity
|
|
720
|
+
*
|
|
721
|
+
* @param path - @zh 要验证的路径 @en Path to validate
|
|
722
|
+
* @param fromIndex - @zh 起始索引 @en Start index
|
|
723
|
+
* @param toIndex - @zh 结束索引 @en End index
|
|
724
|
+
* @param map - @zh 地图实例 @en Map instance
|
|
725
|
+
* @returns @zh 验证结果 @en Validation result
|
|
726
|
+
*/
|
|
727
|
+
validatePath(path, fromIndex, toIndex, map) {
|
|
728
|
+
const end = Math.min(toIndex, path.length);
|
|
729
|
+
for (let i = fromIndex; i < end; i++) {
|
|
730
|
+
const point = path[i];
|
|
731
|
+
const x = Math.floor(point.x);
|
|
732
|
+
const y = Math.floor(point.y);
|
|
733
|
+
if (!map.isWalkable(x, y)) {
|
|
1023
734
|
return {
|
|
1024
|
-
|
|
1025
|
-
|
|
1026
|
-
cost: current.g,
|
|
1027
|
-
nodesSearched
|
|
735
|
+
valid: false,
|
|
736
|
+
invalidIndex: i
|
|
1028
737
|
};
|
|
1029
738
|
}
|
|
1030
|
-
|
|
739
|
+
if (i > fromIndex) {
|
|
740
|
+
const prev = path[i - 1];
|
|
741
|
+
if (!this.checkLineOfSight(prev.x, prev.y, point.x, point.y, map)) {
|
|
742
|
+
return {
|
|
743
|
+
valid: false,
|
|
744
|
+
invalidIndex: i
|
|
745
|
+
};
|
|
746
|
+
}
|
|
747
|
+
}
|
|
1031
748
|
}
|
|
1032
749
|
return {
|
|
1033
|
-
|
|
1034
|
-
|
|
1035
|
-
cost: 0,
|
|
1036
|
-
nodesSearched
|
|
750
|
+
valid: true,
|
|
751
|
+
invalidIndex: -1
|
|
1037
752
|
};
|
|
1038
753
|
}
|
|
1039
754
|
/**
|
|
1040
|
-
* @zh
|
|
1041
|
-
* @en
|
|
1042
|
-
|
|
1043
|
-
|
|
1044
|
-
|
|
1045
|
-
|
|
755
|
+
* @zh 检查两点之间的视线(使用 Bresenham 算法)
|
|
756
|
+
* @en Check line of sight between two points (using Bresenham algorithm)
|
|
757
|
+
*
|
|
758
|
+
* @param x1 - @zh 起点 X @en Start X
|
|
759
|
+
* @param y1 - @zh 起点 Y @en Start Y
|
|
760
|
+
* @param x2 - @zh 终点 X @en End X
|
|
761
|
+
* @param y2 - @zh 终点 Y @en End Y
|
|
762
|
+
* @param map - @zh 地图实例 @en Map instance
|
|
763
|
+
* @returns @zh 是否有视线 @en Whether there is line of sight
|
|
764
|
+
*/
|
|
765
|
+
checkLineOfSight(x1, y1, x2, y2, map) {
|
|
766
|
+
const ix1 = Math.floor(x1);
|
|
767
|
+
const iy1 = Math.floor(y1);
|
|
768
|
+
const ix2 = Math.floor(x2);
|
|
769
|
+
const iy2 = Math.floor(y2);
|
|
770
|
+
let dx = Math.abs(ix2 - ix1);
|
|
771
|
+
let dy = Math.abs(iy2 - iy1);
|
|
772
|
+
let x = ix1;
|
|
773
|
+
let y = iy1;
|
|
774
|
+
const sx = ix1 < ix2 ? 1 : -1;
|
|
775
|
+
const sy = iy1 < iy2 ? 1 : -1;
|
|
776
|
+
if (dx > dy) {
|
|
777
|
+
let err = dx / 2;
|
|
778
|
+
while (x !== ix2) {
|
|
779
|
+
if (!map.isWalkable(x, y)) {
|
|
780
|
+
return false;
|
|
781
|
+
}
|
|
782
|
+
err -= dy;
|
|
783
|
+
if (err < 0) {
|
|
784
|
+
y += sy;
|
|
785
|
+
err += dx;
|
|
786
|
+
}
|
|
787
|
+
x += sx;
|
|
788
|
+
}
|
|
789
|
+
} else {
|
|
790
|
+
let err = dy / 2;
|
|
791
|
+
while (y !== iy2) {
|
|
792
|
+
if (!map.isWalkable(x, y)) {
|
|
793
|
+
return false;
|
|
794
|
+
}
|
|
795
|
+
err -= dx;
|
|
796
|
+
if (err < 0) {
|
|
797
|
+
x += sx;
|
|
798
|
+
err += dy;
|
|
799
|
+
}
|
|
800
|
+
y += sy;
|
|
801
|
+
}
|
|
802
|
+
}
|
|
803
|
+
return map.isWalkable(ix2, iy2);
|
|
804
|
+
}
|
|
805
|
+
};
|
|
806
|
+
__name(_PathValidator, "PathValidator");
|
|
807
|
+
var PathValidator = _PathValidator;
|
|
808
|
+
var _ObstacleChangeManager = class _ObstacleChangeManager {
|
|
809
|
+
constructor() {
|
|
810
|
+
__publicField(this, "changes", /* @__PURE__ */ new Map());
|
|
811
|
+
__publicField(this, "epoch", 0);
|
|
812
|
+
}
|
|
813
|
+
/**
|
|
814
|
+
* @zh 记录障碍物变化
|
|
815
|
+
* @en Record obstacle change
|
|
816
|
+
*
|
|
817
|
+
* @param x - @zh X 坐标 @en X coordinate
|
|
818
|
+
* @param y - @zh Y 坐标 @en Y coordinate
|
|
819
|
+
* @param wasWalkable - @zh 变化前是否可通行 @en Was walkable before change
|
|
820
|
+
*/
|
|
821
|
+
recordChange(x, y, wasWalkable) {
|
|
822
|
+
const key = `${x},${y}`;
|
|
823
|
+
this.changes.set(key, {
|
|
824
|
+
x,
|
|
825
|
+
y,
|
|
826
|
+
wasWalkable,
|
|
827
|
+
timestamp: Date.now()
|
|
828
|
+
});
|
|
1046
829
|
}
|
|
1047
|
-
// =========================================================================
|
|
1048
|
-
// 私有方法 | Private Methods
|
|
1049
|
-
// =========================================================================
|
|
1050
830
|
/**
|
|
1051
|
-
* @zh
|
|
1052
|
-
* @en Get
|
|
831
|
+
* @zh 获取影响区域
|
|
832
|
+
* @en Get affected region
|
|
833
|
+
*
|
|
834
|
+
* @returns @zh 影响区域或 null(如果没有变化)@en Affected region or null if no changes
|
|
1053
835
|
*/
|
|
1054
|
-
|
|
1055
|
-
|
|
1056
|
-
|
|
1057
|
-
|
|
1058
|
-
|
|
1059
|
-
|
|
1060
|
-
|
|
836
|
+
getAffectedRegion() {
|
|
837
|
+
if (this.changes.size === 0) {
|
|
838
|
+
return null;
|
|
839
|
+
}
|
|
840
|
+
let minX = Infinity;
|
|
841
|
+
let minY = Infinity;
|
|
842
|
+
let maxX = -Infinity;
|
|
843
|
+
let maxY = -Infinity;
|
|
844
|
+
for (const change of this.changes.values()) {
|
|
845
|
+
minX = Math.min(minX, change.x);
|
|
846
|
+
minY = Math.min(minY, change.y);
|
|
847
|
+
maxX = Math.max(maxX, change.x);
|
|
848
|
+
maxY = Math.max(maxY, change.y);
|
|
1061
849
|
}
|
|
1062
850
|
return {
|
|
1063
|
-
|
|
1064
|
-
|
|
851
|
+
minX,
|
|
852
|
+
minY,
|
|
853
|
+
maxX,
|
|
854
|
+
maxY
|
|
1065
855
|
};
|
|
1066
856
|
}
|
|
1067
857
|
/**
|
|
1068
|
-
* @zh
|
|
1069
|
-
* @en
|
|
858
|
+
* @zh 获取所有变化
|
|
859
|
+
* @en Get all changes
|
|
860
|
+
*
|
|
861
|
+
* @returns @zh 变化列表 @en List of changes
|
|
1070
862
|
*/
|
|
1071
|
-
|
|
1072
|
-
this.
|
|
1073
|
-
for (let i = 0; i < this.width; i++) {
|
|
1074
|
-
this.nodeGrid[i] = [];
|
|
1075
|
-
}
|
|
863
|
+
getChanges() {
|
|
864
|
+
return Array.from(this.changes.values());
|
|
1076
865
|
}
|
|
1077
866
|
/**
|
|
1078
|
-
* @zh
|
|
1079
|
-
* @en
|
|
867
|
+
* @zh 检查是否有变化
|
|
868
|
+
* @en Check if there are changes
|
|
869
|
+
*
|
|
870
|
+
* @returns @zh 是否有变化 @en Whether there are changes
|
|
1080
871
|
*/
|
|
1081
|
-
|
|
1082
|
-
|
|
1083
|
-
const yi = y | 0;
|
|
1084
|
-
if (xi < 0 || xi >= this.width || yi < 0 || yi >= this.height) {
|
|
1085
|
-
throw new Error("[JPSPathfinder] Invalid grid coordinates");
|
|
1086
|
-
}
|
|
1087
|
-
if (!this.nodeGrid[xi]) {
|
|
1088
|
-
this.nodeGrid[xi] = [];
|
|
1089
|
-
}
|
|
1090
|
-
if (!this.nodeGrid[xi][yi]) {
|
|
1091
|
-
this.nodeGrid[xi][yi] = {
|
|
1092
|
-
x: xi,
|
|
1093
|
-
y: yi,
|
|
1094
|
-
g: Infinity,
|
|
1095
|
-
h: 0,
|
|
1096
|
-
f: Infinity,
|
|
1097
|
-
parent: null,
|
|
1098
|
-
closed: false
|
|
1099
|
-
};
|
|
1100
|
-
}
|
|
1101
|
-
return this.nodeGrid[xi][yi];
|
|
1102
|
-
}
|
|
1103
|
-
/**
|
|
1104
|
-
* @zh 启发式函数(八方向距离)
|
|
1105
|
-
* @en Heuristic function (octile distance)
|
|
1106
|
-
*/
|
|
1107
|
-
heuristic(x1, y1, x2, y2) {
|
|
1108
|
-
const dx = Math.abs(x1 - x2);
|
|
1109
|
-
const dy = Math.abs(y1 - y2);
|
|
1110
|
-
return dx + dy + (Math.SQRT2 - 2) * Math.min(dx, dy);
|
|
1111
|
-
}
|
|
1112
|
-
/**
|
|
1113
|
-
* @zh 识别后继节点(跳跃点)
|
|
1114
|
-
* @en Identify successors (jump points)
|
|
1115
|
-
*/
|
|
1116
|
-
identifySuccessors(node, endX, endY, opts) {
|
|
1117
|
-
const neighbors = this.findNeighbors(node);
|
|
1118
|
-
for (const neighbor of neighbors) {
|
|
1119
|
-
const jumpPoint = this.jump(neighbor.x, neighbor.y, node.x, node.y, endX, endY);
|
|
1120
|
-
if (jumpPoint) {
|
|
1121
|
-
const jx = jumpPoint.x;
|
|
1122
|
-
const jy = jumpPoint.y;
|
|
1123
|
-
const jpNode = this.getOrCreateNode(jx, jy);
|
|
1124
|
-
if (jpNode.closed) continue;
|
|
1125
|
-
const dx = Math.abs(jx - node.x);
|
|
1126
|
-
const dy = Math.abs(jy - node.y);
|
|
1127
|
-
const distance = Math.sqrt(dx * dx + dy * dy);
|
|
1128
|
-
const tentativeG = node.g + distance;
|
|
1129
|
-
if (tentativeG < jpNode.g) {
|
|
1130
|
-
jpNode.g = tentativeG;
|
|
1131
|
-
jpNode.h = this.heuristic(jx, jy, endX, endY) * opts.heuristicWeight;
|
|
1132
|
-
jpNode.f = jpNode.g + jpNode.h;
|
|
1133
|
-
jpNode.parent = node;
|
|
1134
|
-
if (!this.openList.contains(jpNode)) {
|
|
1135
|
-
this.openList.push(jpNode);
|
|
1136
|
-
} else {
|
|
1137
|
-
this.openList.update(jpNode);
|
|
1138
|
-
}
|
|
1139
|
-
}
|
|
1140
|
-
}
|
|
1141
|
-
}
|
|
872
|
+
hasChanges() {
|
|
873
|
+
return this.changes.size > 0;
|
|
1142
874
|
}
|
|
1143
875
|
/**
|
|
1144
|
-
* @zh
|
|
1145
|
-
* @en
|
|
876
|
+
* @zh 获取当前 epoch
|
|
877
|
+
* @en Get current epoch
|
|
878
|
+
*
|
|
879
|
+
* @returns @zh 当前 epoch @en Current epoch
|
|
1146
880
|
*/
|
|
1147
|
-
|
|
1148
|
-
|
|
1149
|
-
const neighbors = [];
|
|
1150
|
-
if (!parent) {
|
|
1151
|
-
for (let dx2 = -1; dx2 <= 1; dx2++) {
|
|
1152
|
-
for (let dy2 = -1; dy2 <= 1; dy2++) {
|
|
1153
|
-
if (dx2 === 0 && dy2 === 0) continue;
|
|
1154
|
-
const nx = x + dx2;
|
|
1155
|
-
const ny = y + dy2;
|
|
1156
|
-
if (this.isWalkableAt(nx, ny)) {
|
|
1157
|
-
if (dx2 !== 0 && dy2 !== 0) {
|
|
1158
|
-
if (this.isWalkableAt(x + dx2, y) || this.isWalkableAt(x, y + dy2)) {
|
|
1159
|
-
neighbors.push({
|
|
1160
|
-
x: nx,
|
|
1161
|
-
y: ny
|
|
1162
|
-
});
|
|
1163
|
-
}
|
|
1164
|
-
} else {
|
|
1165
|
-
neighbors.push({
|
|
1166
|
-
x: nx,
|
|
1167
|
-
y: ny
|
|
1168
|
-
});
|
|
1169
|
-
}
|
|
1170
|
-
}
|
|
1171
|
-
}
|
|
1172
|
-
}
|
|
1173
|
-
return neighbors;
|
|
1174
|
-
}
|
|
1175
|
-
const dx = Math.sign(x - parent.x);
|
|
1176
|
-
const dy = Math.sign(y - parent.y);
|
|
1177
|
-
if (dx !== 0 && dy !== 0) {
|
|
1178
|
-
if (this.isWalkableAt(x, y + dy)) {
|
|
1179
|
-
neighbors.push({
|
|
1180
|
-
x,
|
|
1181
|
-
y: y + dy
|
|
1182
|
-
});
|
|
1183
|
-
}
|
|
1184
|
-
if (this.isWalkableAt(x + dx, y)) {
|
|
1185
|
-
neighbors.push({
|
|
1186
|
-
x: x + dx,
|
|
1187
|
-
y
|
|
1188
|
-
});
|
|
1189
|
-
}
|
|
1190
|
-
if (this.isWalkableAt(x, y + dy) || this.isWalkableAt(x + dx, y)) {
|
|
1191
|
-
if (this.isWalkableAt(x + dx, y + dy)) {
|
|
1192
|
-
neighbors.push({
|
|
1193
|
-
x: x + dx,
|
|
1194
|
-
y: y + dy
|
|
1195
|
-
});
|
|
1196
|
-
}
|
|
1197
|
-
}
|
|
1198
|
-
if (!this.isWalkableAt(x - dx, y) && this.isWalkableAt(x, y + dy)) {
|
|
1199
|
-
if (this.isWalkableAt(x - dx, y + dy)) {
|
|
1200
|
-
neighbors.push({
|
|
1201
|
-
x: x - dx,
|
|
1202
|
-
y: y + dy
|
|
1203
|
-
});
|
|
1204
|
-
}
|
|
1205
|
-
}
|
|
1206
|
-
if (!this.isWalkableAt(x, y - dy) && this.isWalkableAt(x + dx, y)) {
|
|
1207
|
-
if (this.isWalkableAt(x + dx, y - dy)) {
|
|
1208
|
-
neighbors.push({
|
|
1209
|
-
x: x + dx,
|
|
1210
|
-
y: y - dy
|
|
1211
|
-
});
|
|
1212
|
-
}
|
|
1213
|
-
}
|
|
1214
|
-
} else if (dx !== 0) {
|
|
1215
|
-
if (this.isWalkableAt(x + dx, y)) {
|
|
1216
|
-
neighbors.push({
|
|
1217
|
-
x: x + dx,
|
|
1218
|
-
y
|
|
1219
|
-
});
|
|
1220
|
-
if (!this.isWalkableAt(x, y + 1) && this.isWalkableAt(x + dx, y + 1)) {
|
|
1221
|
-
neighbors.push({
|
|
1222
|
-
x: x + dx,
|
|
1223
|
-
y: y + 1
|
|
1224
|
-
});
|
|
1225
|
-
}
|
|
1226
|
-
if (!this.isWalkableAt(x, y - 1) && this.isWalkableAt(x + dx, y - 1)) {
|
|
1227
|
-
neighbors.push({
|
|
1228
|
-
x: x + dx,
|
|
1229
|
-
y: y - 1
|
|
1230
|
-
});
|
|
1231
|
-
}
|
|
1232
|
-
}
|
|
1233
|
-
} else if (dy !== 0) {
|
|
1234
|
-
if (this.isWalkableAt(x, y + dy)) {
|
|
1235
|
-
neighbors.push({
|
|
1236
|
-
x,
|
|
1237
|
-
y: y + dy
|
|
1238
|
-
});
|
|
1239
|
-
if (!this.isWalkableAt(x + 1, y) && this.isWalkableAt(x + 1, y + dy)) {
|
|
1240
|
-
neighbors.push({
|
|
1241
|
-
x: x + 1,
|
|
1242
|
-
y: y + dy
|
|
1243
|
-
});
|
|
1244
|
-
}
|
|
1245
|
-
if (!this.isWalkableAt(x - 1, y) && this.isWalkableAt(x - 1, y + dy)) {
|
|
1246
|
-
neighbors.push({
|
|
1247
|
-
x: x - 1,
|
|
1248
|
-
y: y + dy
|
|
1249
|
-
});
|
|
1250
|
-
}
|
|
1251
|
-
}
|
|
1252
|
-
}
|
|
1253
|
-
return neighbors;
|
|
881
|
+
getEpoch() {
|
|
882
|
+
return this.epoch;
|
|
1254
883
|
}
|
|
1255
884
|
/**
|
|
1256
|
-
* @zh
|
|
1257
|
-
* @en
|
|
885
|
+
* @zh 清空变化记录并推进 epoch
|
|
886
|
+
* @en Clear changes and advance epoch
|
|
1258
887
|
*/
|
|
1259
|
-
|
|
1260
|
-
|
|
1261
|
-
|
|
1262
|
-
let x = startX;
|
|
1263
|
-
let y = startY;
|
|
1264
|
-
while (true) {
|
|
1265
|
-
if (!this.isWalkableAt(x, y)) {
|
|
1266
|
-
return null;
|
|
1267
|
-
}
|
|
1268
|
-
if (x === endX && y === endY) {
|
|
1269
|
-
return {
|
|
1270
|
-
x,
|
|
1271
|
-
y
|
|
1272
|
-
};
|
|
1273
|
-
}
|
|
1274
|
-
if (dx !== 0 && dy !== 0) {
|
|
1275
|
-
if (this.isWalkableAt(x - dx, y + dy) && !this.isWalkableAt(x - dx, y) || this.isWalkableAt(x + dx, y - dy) && !this.isWalkableAt(x, y - dy)) {
|
|
1276
|
-
return {
|
|
1277
|
-
x,
|
|
1278
|
-
y
|
|
1279
|
-
};
|
|
1280
|
-
}
|
|
1281
|
-
if (this.jumpStraight(x + dx, y, dx, 0, endX, endY) || this.jumpStraight(x, y + dy, 0, dy, endX, endY)) {
|
|
1282
|
-
return {
|
|
1283
|
-
x,
|
|
1284
|
-
y
|
|
1285
|
-
};
|
|
1286
|
-
}
|
|
1287
|
-
if (!this.isWalkableAt(x + dx, y) && !this.isWalkableAt(x, y + dy)) {
|
|
1288
|
-
return null;
|
|
1289
|
-
}
|
|
1290
|
-
} else if (dx !== 0) {
|
|
1291
|
-
if (this.isWalkableAt(x + dx, y + 1) && !this.isWalkableAt(x, y + 1) || this.isWalkableAt(x + dx, y - 1) && !this.isWalkableAt(x, y - 1)) {
|
|
1292
|
-
return {
|
|
1293
|
-
x,
|
|
1294
|
-
y
|
|
1295
|
-
};
|
|
1296
|
-
}
|
|
1297
|
-
} else if (dy !== 0) {
|
|
1298
|
-
if (this.isWalkableAt(x + 1, y + dy) && !this.isWalkableAt(x + 1, y) || this.isWalkableAt(x - 1, y + dy) && !this.isWalkableAt(x - 1, y)) {
|
|
1299
|
-
return {
|
|
1300
|
-
x,
|
|
1301
|
-
y
|
|
1302
|
-
};
|
|
1303
|
-
}
|
|
1304
|
-
}
|
|
1305
|
-
x += dx;
|
|
1306
|
-
y += dy;
|
|
1307
|
-
}
|
|
888
|
+
flush() {
|
|
889
|
+
this.changes.clear();
|
|
890
|
+
this.epoch++;
|
|
1308
891
|
}
|
|
1309
892
|
/**
|
|
1310
|
-
* @zh
|
|
1311
|
-
* @en
|
|
893
|
+
* @zh 清空所有状态
|
|
894
|
+
* @en Clear all state
|
|
1312
895
|
*/
|
|
1313
|
-
|
|
1314
|
-
|
|
1315
|
-
|
|
1316
|
-
while (true) {
|
|
1317
|
-
if (!this.isWalkableAt(x, y)) {
|
|
1318
|
-
return false;
|
|
1319
|
-
}
|
|
1320
|
-
if (x === endX && y === endY) {
|
|
1321
|
-
return true;
|
|
1322
|
-
}
|
|
1323
|
-
if (dx !== 0) {
|
|
1324
|
-
if (this.isWalkableAt(x + dx, y + 1) && !this.isWalkableAt(x, y + 1) || this.isWalkableAt(x + dx, y - 1) && !this.isWalkableAt(x, y - 1)) {
|
|
1325
|
-
return true;
|
|
1326
|
-
}
|
|
1327
|
-
} else if (dy !== 0) {
|
|
1328
|
-
if (this.isWalkableAt(x + 1, y + dy) && !this.isWalkableAt(x + 1, y) || this.isWalkableAt(x - 1, y + dy) && !this.isWalkableAt(x - 1, y)) {
|
|
1329
|
-
return true;
|
|
1330
|
-
}
|
|
1331
|
-
}
|
|
1332
|
-
x += dx;
|
|
1333
|
-
y += dy;
|
|
1334
|
-
}
|
|
1335
|
-
}
|
|
1336
|
-
/**
|
|
1337
|
-
* @zh 检查位置是否可通行
|
|
1338
|
-
* @en Check if position is walkable
|
|
1339
|
-
*/
|
|
1340
|
-
isWalkableAt(x, y) {
|
|
1341
|
-
if (x < 0 || x >= this.width || y < 0 || y >= this.height) {
|
|
1342
|
-
return false;
|
|
1343
|
-
}
|
|
1344
|
-
return this.map.isWalkable(x, y);
|
|
1345
|
-
}
|
|
1346
|
-
/**
|
|
1347
|
-
* @zh 构建路径
|
|
1348
|
-
* @en Build path
|
|
1349
|
-
*/
|
|
1350
|
-
buildPath(endNode) {
|
|
1351
|
-
const path = [];
|
|
1352
|
-
let current = endNode;
|
|
1353
|
-
while (current) {
|
|
1354
|
-
path.unshift({
|
|
1355
|
-
x: current.x,
|
|
1356
|
-
y: current.y
|
|
1357
|
-
});
|
|
1358
|
-
current = current.parent;
|
|
1359
|
-
}
|
|
1360
|
-
return this.interpolatePath(path);
|
|
1361
|
-
}
|
|
1362
|
-
/**
|
|
1363
|
-
* @zh 插值路径(在跳跃点之间填充中间点)
|
|
1364
|
-
* @en Interpolate path (fill intermediate points between jump points)
|
|
1365
|
-
*/
|
|
1366
|
-
interpolatePath(jumpPoints) {
|
|
1367
|
-
if (jumpPoints.length < 2) {
|
|
1368
|
-
return jumpPoints;
|
|
1369
|
-
}
|
|
1370
|
-
const path = [
|
|
1371
|
-
jumpPoints[0]
|
|
1372
|
-
];
|
|
1373
|
-
for (let i = 1; i < jumpPoints.length; i++) {
|
|
1374
|
-
const prev = jumpPoints[i - 1];
|
|
1375
|
-
const curr = jumpPoints[i];
|
|
1376
|
-
const dx = curr.x - prev.x;
|
|
1377
|
-
const dy = curr.y - prev.y;
|
|
1378
|
-
const steps = Math.max(Math.abs(dx), Math.abs(dy));
|
|
1379
|
-
const stepX = dx === 0 ? 0 : dx / Math.abs(dx);
|
|
1380
|
-
const stepY = dy === 0 ? 0 : dy / Math.abs(dy);
|
|
1381
|
-
let x = prev.x;
|
|
1382
|
-
let y = prev.y;
|
|
1383
|
-
for (let j = 0; j < steps; j++) {
|
|
1384
|
-
if (x !== curr.x && y !== curr.y) {
|
|
1385
|
-
x += stepX;
|
|
1386
|
-
y += stepY;
|
|
1387
|
-
} else if (x !== curr.x) {
|
|
1388
|
-
x += stepX;
|
|
1389
|
-
} else if (y !== curr.y) {
|
|
1390
|
-
y += stepY;
|
|
1391
|
-
}
|
|
1392
|
-
if (x !== prev.x || y !== prev.y) {
|
|
1393
|
-
path.push({
|
|
1394
|
-
x,
|
|
1395
|
-
y
|
|
1396
|
-
});
|
|
1397
|
-
}
|
|
1398
|
-
}
|
|
1399
|
-
}
|
|
1400
|
-
return path;
|
|
896
|
+
clear() {
|
|
897
|
+
this.changes.clear();
|
|
898
|
+
this.epoch = 0;
|
|
1401
899
|
}
|
|
1402
900
|
};
|
|
1403
|
-
__name(
|
|
1404
|
-
var
|
|
1405
|
-
function
|
|
1406
|
-
return new
|
|
901
|
+
__name(_ObstacleChangeManager, "ObstacleChangeManager");
|
|
902
|
+
var ObstacleChangeManager = _ObstacleChangeManager;
|
|
903
|
+
function createPathValidator() {
|
|
904
|
+
return new PathValidator();
|
|
905
|
+
}
|
|
906
|
+
__name(createPathValidator, "createPathValidator");
|
|
907
|
+
function createObstacleChangeManager() {
|
|
908
|
+
return new ObstacleChangeManager();
|
|
1407
909
|
}
|
|
1408
|
-
__name(
|
|
910
|
+
__name(createObstacleChangeManager, "createObstacleChangeManager");
|
|
1409
911
|
|
|
1410
|
-
// src/
|
|
1411
|
-
var
|
|
1412
|
-
|
|
1413
|
-
|
|
1414
|
-
|
|
1415
|
-
|
|
1416
|
-
|
|
912
|
+
// src/grid/GridMap.ts
|
|
913
|
+
var _GridNode = class _GridNode {
|
|
914
|
+
constructor(x, y, width, walkable = true, cost = 1) {
|
|
915
|
+
__publicField(this, "id");
|
|
916
|
+
__publicField(this, "position");
|
|
917
|
+
__publicField(this, "x");
|
|
918
|
+
__publicField(this, "y");
|
|
919
|
+
__publicField(this, "cost");
|
|
920
|
+
__publicField(this, "walkable");
|
|
921
|
+
this.x = x;
|
|
922
|
+
this.y = y;
|
|
923
|
+
this.id = y * width + x;
|
|
924
|
+
this.position = createPoint(x, y);
|
|
925
|
+
this.walkable = walkable;
|
|
926
|
+
this.cost = cost;
|
|
927
|
+
}
|
|
1417
928
|
};
|
|
1418
|
-
|
|
1419
|
-
var
|
|
1420
|
-
|
|
1421
|
-
|
|
1422
|
-
|
|
1423
|
-
|
|
929
|
+
__name(_GridNode, "GridNode");
|
|
930
|
+
var GridNode = _GridNode;
|
|
931
|
+
var DIRECTIONS_4 = [
|
|
932
|
+
{
|
|
933
|
+
dx: 0,
|
|
934
|
+
dy: -1
|
|
935
|
+
},
|
|
936
|
+
{
|
|
937
|
+
dx: 1,
|
|
938
|
+
dy: 0
|
|
939
|
+
},
|
|
940
|
+
{
|
|
941
|
+
dx: 0,
|
|
942
|
+
dy: 1
|
|
943
|
+
},
|
|
944
|
+
{
|
|
945
|
+
dx: -1,
|
|
946
|
+
dy: 0
|
|
947
|
+
}
|
|
948
|
+
// Left
|
|
949
|
+
];
|
|
950
|
+
var DIRECTIONS_8 = [
|
|
951
|
+
{
|
|
952
|
+
dx: 0,
|
|
953
|
+
dy: -1
|
|
954
|
+
},
|
|
955
|
+
{
|
|
956
|
+
dx: 1,
|
|
957
|
+
dy: -1
|
|
958
|
+
},
|
|
959
|
+
{
|
|
960
|
+
dx: 1,
|
|
961
|
+
dy: 0
|
|
962
|
+
},
|
|
963
|
+
{
|
|
964
|
+
dx: 1,
|
|
965
|
+
dy: 1
|
|
966
|
+
},
|
|
967
|
+
{
|
|
968
|
+
dx: 0,
|
|
969
|
+
dy: 1
|
|
970
|
+
},
|
|
971
|
+
{
|
|
972
|
+
dx: -1,
|
|
973
|
+
dy: 1
|
|
974
|
+
},
|
|
975
|
+
{
|
|
976
|
+
dx: -1,
|
|
977
|
+
dy: 0
|
|
978
|
+
},
|
|
979
|
+
{
|
|
980
|
+
dx: -1,
|
|
981
|
+
dy: -1
|
|
982
|
+
}
|
|
983
|
+
// Up-Left
|
|
984
|
+
];
|
|
985
|
+
var DEFAULT_GRID_OPTIONS = {
|
|
986
|
+
allowDiagonal: true,
|
|
987
|
+
diagonalCost: Math.SQRT2,
|
|
988
|
+
avoidCorners: true,
|
|
989
|
+
heuristic: octileDistance
|
|
990
|
+
};
|
|
991
|
+
var _GridMap = class _GridMap {
|
|
992
|
+
constructor(width, height, options) {
|
|
1424
993
|
__publicField(this, "width");
|
|
1425
994
|
__publicField(this, "height");
|
|
1426
|
-
this
|
|
1427
|
-
this
|
|
1428
|
-
this.originY = originY;
|
|
995
|
+
__publicField(this, "nodes");
|
|
996
|
+
__publicField(this, "options");
|
|
1429
997
|
this.width = width;
|
|
1430
998
|
this.height = height;
|
|
1431
|
-
|
|
1432
|
-
|
|
1433
|
-
|
|
1434
|
-
* @en Convert local to global coordinates
|
|
1435
|
-
*/
|
|
1436
|
-
localToGlobal(localX, localY) {
|
|
1437
|
-
return {
|
|
1438
|
-
x: this.originX + localX,
|
|
1439
|
-
y: this.originY + localY
|
|
999
|
+
this.options = {
|
|
1000
|
+
...DEFAULT_GRID_OPTIONS,
|
|
1001
|
+
...options
|
|
1440
1002
|
};
|
|
1003
|
+
this.nodes = this.createNodes();
|
|
1441
1004
|
}
|
|
1442
1005
|
/**
|
|
1443
|
-
* @zh
|
|
1444
|
-
* @en
|
|
1006
|
+
* @zh 创建网格节点
|
|
1007
|
+
* @en Create grid nodes
|
|
1445
1008
|
*/
|
|
1446
|
-
|
|
1447
|
-
|
|
1448
|
-
|
|
1449
|
-
y
|
|
1450
|
-
|
|
1451
|
-
|
|
1452
|
-
isWalkable(x, y) {
|
|
1453
|
-
if (x < 0 || x >= this.width || y < 0 || y >= this.height) {
|
|
1454
|
-
return false;
|
|
1455
|
-
}
|
|
1456
|
-
return this.parentMap.isWalkable(this.originX + x, this.originY + y);
|
|
1457
|
-
}
|
|
1458
|
-
getNodeAt(x, y) {
|
|
1459
|
-
if (x < 0 || x >= this.width || y < 0 || y >= this.height) {
|
|
1460
|
-
return null;
|
|
1461
|
-
}
|
|
1462
|
-
const globalNode = this.parentMap.getNodeAt(this.originX + x, this.originY + y);
|
|
1463
|
-
if (!globalNode) return null;
|
|
1464
|
-
return {
|
|
1465
|
-
id: y * this.width + x,
|
|
1466
|
-
position: {
|
|
1467
|
-
x,
|
|
1468
|
-
y
|
|
1469
|
-
},
|
|
1470
|
-
cost: globalNode.cost,
|
|
1471
|
-
walkable: globalNode.walkable
|
|
1472
|
-
};
|
|
1473
|
-
}
|
|
1474
|
-
getNeighbors(node) {
|
|
1475
|
-
const neighbors = [];
|
|
1476
|
-
const { x, y } = node.position;
|
|
1477
|
-
const directions = [
|
|
1478
|
-
{
|
|
1479
|
-
dx: 0,
|
|
1480
|
-
dy: -1
|
|
1481
|
-
},
|
|
1482
|
-
{
|
|
1483
|
-
dx: 1,
|
|
1484
|
-
dy: -1
|
|
1485
|
-
},
|
|
1486
|
-
{
|
|
1487
|
-
dx: 1,
|
|
1488
|
-
dy: 0
|
|
1489
|
-
},
|
|
1490
|
-
{
|
|
1491
|
-
dx: 1,
|
|
1492
|
-
dy: 1
|
|
1493
|
-
},
|
|
1494
|
-
{
|
|
1495
|
-
dx: 0,
|
|
1496
|
-
dy: 1
|
|
1497
|
-
},
|
|
1498
|
-
{
|
|
1499
|
-
dx: -1,
|
|
1500
|
-
dy: 1
|
|
1501
|
-
},
|
|
1502
|
-
{
|
|
1503
|
-
dx: -1,
|
|
1504
|
-
dy: 0
|
|
1505
|
-
},
|
|
1506
|
-
{
|
|
1507
|
-
dx: -1,
|
|
1508
|
-
dy: -1
|
|
1509
|
-
}
|
|
1510
|
-
// NW
|
|
1511
|
-
];
|
|
1512
|
-
for (const dir of directions) {
|
|
1513
|
-
const nx = x + dir.dx;
|
|
1514
|
-
const ny = y + dir.dy;
|
|
1515
|
-
if (nx < 0 || nx >= this.width || ny < 0 || ny >= this.height) {
|
|
1516
|
-
continue;
|
|
1517
|
-
}
|
|
1518
|
-
if (!this.isWalkable(nx, ny)) {
|
|
1519
|
-
continue;
|
|
1520
|
-
}
|
|
1521
|
-
if (dir.dx !== 0 && dir.dy !== 0) {
|
|
1522
|
-
if (!this.isWalkable(x + dir.dx, y) || !this.isWalkable(x, y + dir.dy)) {
|
|
1523
|
-
continue;
|
|
1524
|
-
}
|
|
1525
|
-
}
|
|
1526
|
-
const neighborNode = this.getNodeAt(nx, ny);
|
|
1527
|
-
if (neighborNode) {
|
|
1528
|
-
neighbors.push(neighborNode);
|
|
1009
|
+
createNodes() {
|
|
1010
|
+
const nodes = [];
|
|
1011
|
+
for (let y = 0; y < this.height; y++) {
|
|
1012
|
+
nodes[y] = [];
|
|
1013
|
+
for (let x = 0; x < this.width; x++) {
|
|
1014
|
+
nodes[y][x] = new GridNode(x, y, this.width, true, 1);
|
|
1529
1015
|
}
|
|
1530
1016
|
}
|
|
1531
|
-
return
|
|
1532
|
-
}
|
|
1533
|
-
heuristic(a, b) {
|
|
1534
|
-
const dx = Math.abs(a.x - b.x);
|
|
1535
|
-
const dy = Math.abs(a.y - b.y);
|
|
1536
|
-
return dx + dy + (Math.SQRT2 - 2) * Math.min(dx, dy);
|
|
1537
|
-
}
|
|
1538
|
-
getMovementCost(from, to) {
|
|
1539
|
-
const dx = Math.abs(to.position.x - from.position.x);
|
|
1540
|
-
const dy = Math.abs(to.position.y - from.position.y);
|
|
1541
|
-
const baseCost = dx !== 0 && dy !== 0 ? Math.SQRT2 : 1;
|
|
1542
|
-
return baseCost * to.cost;
|
|
1543
|
-
}
|
|
1544
|
-
}, __name(_a3, "SubMap"), _a3);
|
|
1545
|
-
var _a4;
|
|
1546
|
-
var Cluster = (_a4 = class {
|
|
1547
|
-
constructor(id, originX, originY, width, height, parentMap) {
|
|
1548
|
-
__publicField(this, "id");
|
|
1549
|
-
__publicField(this, "originX");
|
|
1550
|
-
__publicField(this, "originY");
|
|
1551
|
-
__publicField(this, "width");
|
|
1552
|
-
__publicField(this, "height");
|
|
1553
|
-
__publicField(this, "subMap");
|
|
1554
|
-
/** @zh 集群内的抽象节点 ID 列表 @en Abstract node IDs in this cluster */
|
|
1555
|
-
__publicField(this, "nodeIds", []);
|
|
1556
|
-
/** @zh 预计算的距离缓存 @en Precomputed distance cache */
|
|
1557
|
-
__publicField(this, "distanceCache", /* @__PURE__ */ new Map());
|
|
1558
|
-
/** @zh 预计算的路径缓存 @en Precomputed path cache */
|
|
1559
|
-
__publicField(this, "pathCache", /* @__PURE__ */ new Map());
|
|
1560
|
-
this.id = id;
|
|
1561
|
-
this.originX = originX;
|
|
1562
|
-
this.originY = originY;
|
|
1563
|
-
this.width = width;
|
|
1564
|
-
this.height = height;
|
|
1565
|
-
this.subMap = new SubMap(parentMap, originX, originY, width, height);
|
|
1566
|
-
}
|
|
1567
|
-
/**
|
|
1568
|
-
* @zh 检查点是否在集群内
|
|
1569
|
-
* @en Check if point is in cluster
|
|
1570
|
-
*/
|
|
1571
|
-
containsPoint(x, y) {
|
|
1572
|
-
return x >= this.originX && x < this.originX + this.width && y >= this.originY && y < this.originY + this.height;
|
|
1017
|
+
return nodes;
|
|
1573
1018
|
}
|
|
1574
1019
|
/**
|
|
1575
|
-
* @zh
|
|
1576
|
-
* @en
|
|
1020
|
+
* @zh 获取指定位置的节点
|
|
1021
|
+
* @en Get node at position
|
|
1577
1022
|
*/
|
|
1578
|
-
|
|
1579
|
-
if (!this.
|
|
1580
|
-
|
|
1581
|
-
}
|
|
1582
|
-
}
|
|
1583
|
-
/**
|
|
1584
|
-
* @zh 移除节点 ID
|
|
1585
|
-
* @en Remove node ID
|
|
1586
|
-
*/
|
|
1587
|
-
removeNodeId(nodeId) {
|
|
1588
|
-
const idx = this.nodeIds.indexOf(nodeId);
|
|
1589
|
-
if (idx !== -1) {
|
|
1590
|
-
this.nodeIds.splice(idx, 1);
|
|
1023
|
+
getNodeAt(x, y) {
|
|
1024
|
+
if (!this.isInBounds(x, y)) {
|
|
1025
|
+
return null;
|
|
1591
1026
|
}
|
|
1027
|
+
return this.nodes[y][x];
|
|
1592
1028
|
}
|
|
1593
1029
|
/**
|
|
1594
|
-
* @zh
|
|
1595
|
-
* @en
|
|
1596
|
-
*/
|
|
1597
|
-
getCacheKey(fromId, toId) {
|
|
1598
|
-
return `${fromId}->${toId}`;
|
|
1599
|
-
}
|
|
1600
|
-
/**
|
|
1601
|
-
* @zh 设置缓存
|
|
1602
|
-
* @en Set cache
|
|
1603
|
-
*/
|
|
1604
|
-
setCache(fromId, toId, cost, path) {
|
|
1605
|
-
const key = this.getCacheKey(fromId, toId);
|
|
1606
|
-
this.distanceCache.set(key, cost);
|
|
1607
|
-
this.pathCache.set(key, path);
|
|
1608
|
-
}
|
|
1609
|
-
/**
|
|
1610
|
-
* @zh 获取缓存的距离
|
|
1611
|
-
* @en Get cached distance
|
|
1612
|
-
*/
|
|
1613
|
-
getCachedDistance(fromId, toId) {
|
|
1614
|
-
return this.distanceCache.get(this.getCacheKey(fromId, toId));
|
|
1615
|
-
}
|
|
1616
|
-
/**
|
|
1617
|
-
* @zh 获取缓存的路径
|
|
1618
|
-
* @en Get cached path
|
|
1619
|
-
*/
|
|
1620
|
-
getCachedPath(fromId, toId) {
|
|
1621
|
-
return this.pathCache.get(this.getCacheKey(fromId, toId));
|
|
1622
|
-
}
|
|
1623
|
-
/**
|
|
1624
|
-
* @zh 清除缓存
|
|
1625
|
-
* @en Clear cache
|
|
1030
|
+
* @zh 检查坐标是否在边界内
|
|
1031
|
+
* @en Check if coordinates are within bounds
|
|
1626
1032
|
*/
|
|
1627
|
-
|
|
1628
|
-
this.
|
|
1629
|
-
this.pathCache.clear();
|
|
1033
|
+
isInBounds(x, y) {
|
|
1034
|
+
return x >= 0 && x < this.width && y >= 0 && y < this.height;
|
|
1630
1035
|
}
|
|
1631
1036
|
/**
|
|
1632
|
-
* @zh
|
|
1633
|
-
* @en
|
|
1634
|
-
*/
|
|
1635
|
-
getCacheSize() {
|
|
1636
|
-
return this.distanceCache.size;
|
|
1637
|
-
}
|
|
1638
|
-
}, __name(_a4, "Cluster"), _a4);
|
|
1639
|
-
var _HPAPathfinder = class _HPAPathfinder {
|
|
1640
|
-
constructor(map, config) {
|
|
1641
|
-
__publicField(this, "map");
|
|
1642
|
-
__publicField(this, "config");
|
|
1643
|
-
__publicField(this, "mapWidth");
|
|
1644
|
-
__publicField(this, "mapHeight");
|
|
1645
|
-
// 集群管理
|
|
1646
|
-
__publicField(this, "clusters", []);
|
|
1647
|
-
__publicField(this, "clusterGrid", []);
|
|
1648
|
-
__publicField(this, "clustersX", 0);
|
|
1649
|
-
__publicField(this, "clustersY", 0);
|
|
1650
|
-
// 抽象图
|
|
1651
|
-
__publicField(this, "abstractNodes", /* @__PURE__ */ new Map());
|
|
1652
|
-
__publicField(this, "nodesByCluster", /* @__PURE__ */ new Map());
|
|
1653
|
-
__publicField(this, "nextNodeId", 0);
|
|
1654
|
-
// 入口统计
|
|
1655
|
-
__publicField(this, "entranceCount", 0);
|
|
1656
|
-
// 内部寻路器
|
|
1657
|
-
__publicField(this, "localPathfinder");
|
|
1658
|
-
// 完整路径缓存
|
|
1659
|
-
__publicField(this, "pathCache");
|
|
1660
|
-
__publicField(this, "mapVersion", 0);
|
|
1661
|
-
__publicField(this, "preprocessed", false);
|
|
1662
|
-
this.map = map;
|
|
1663
|
-
this.config = {
|
|
1664
|
-
...DEFAULT_HPA_CONFIG,
|
|
1665
|
-
...config
|
|
1666
|
-
};
|
|
1667
|
-
const bounds = this.getMapBounds();
|
|
1668
|
-
this.mapWidth = bounds.width;
|
|
1669
|
-
this.mapHeight = bounds.height;
|
|
1670
|
-
this.localPathfinder = new AStarPathfinder(map);
|
|
1671
|
-
this.pathCache = new PathCache({
|
|
1672
|
-
maxEntries: 1e3,
|
|
1673
|
-
ttlMs: 0
|
|
1674
|
-
});
|
|
1675
|
-
}
|
|
1676
|
-
// =========================================================================
|
|
1677
|
-
// 公共 API | Public API
|
|
1678
|
-
// =========================================================================
|
|
1679
|
-
/**
|
|
1680
|
-
* @zh 预处理地图(构建抽象图)
|
|
1681
|
-
* @en Preprocess map (build abstract graph)
|
|
1037
|
+
* @zh 检查位置是否可通行
|
|
1038
|
+
* @en Check if position is walkable
|
|
1682
1039
|
*/
|
|
1683
|
-
|
|
1684
|
-
this.
|
|
1685
|
-
|
|
1686
|
-
this.buildEntrances();
|
|
1687
|
-
this.buildIntraEdges();
|
|
1688
|
-
this.preprocessed = true;
|
|
1040
|
+
isWalkable(x, y) {
|
|
1041
|
+
const node = this.getNodeAt(x, y);
|
|
1042
|
+
return node !== null && node.walkable;
|
|
1689
1043
|
}
|
|
1690
1044
|
/**
|
|
1691
|
-
* @zh
|
|
1692
|
-
* @en
|
|
1045
|
+
* @zh 设置位置是否可通行
|
|
1046
|
+
* @en Set position walkability
|
|
1693
1047
|
*/
|
|
1694
|
-
|
|
1695
|
-
|
|
1696
|
-
|
|
1697
|
-
|
|
1698
|
-
const opts = {
|
|
1699
|
-
...DEFAULT_PATHFINDING_OPTIONS,
|
|
1700
|
-
...options
|
|
1701
|
-
};
|
|
1702
|
-
if (!this.map.isWalkable(startX, startY) || !this.map.isWalkable(endX, endY)) {
|
|
1703
|
-
return EMPTY_PATH_RESULT;
|
|
1704
|
-
}
|
|
1705
|
-
if (startX === endX && startY === endY) {
|
|
1706
|
-
return {
|
|
1707
|
-
found: true,
|
|
1708
|
-
path: [
|
|
1709
|
-
{
|
|
1710
|
-
x: startX,
|
|
1711
|
-
y: startY
|
|
1712
|
-
}
|
|
1713
|
-
],
|
|
1714
|
-
cost: 0,
|
|
1715
|
-
nodesSearched: 1
|
|
1716
|
-
};
|
|
1717
|
-
}
|
|
1718
|
-
const cached = this.pathCache.get(startX, startY, endX, endY, this.mapVersion);
|
|
1719
|
-
if (cached) {
|
|
1720
|
-
return cached;
|
|
1721
|
-
}
|
|
1722
|
-
const startCluster = this.getClusterAt(startX, startY);
|
|
1723
|
-
const endCluster = this.getClusterAt(endX, endY);
|
|
1724
|
-
if (!startCluster || !endCluster) {
|
|
1725
|
-
return EMPTY_PATH_RESULT;
|
|
1726
|
-
}
|
|
1727
|
-
let result;
|
|
1728
|
-
if (startCluster.id === endCluster.id) {
|
|
1729
|
-
result = this.findLocalPath(startX, startY, endX, endY, opts);
|
|
1730
|
-
} else {
|
|
1731
|
-
const startTemp = this.insertTempNode(startX, startY, startCluster);
|
|
1732
|
-
const endTemp = this.insertTempNode(endX, endY, endCluster);
|
|
1733
|
-
const abstractPath = this.abstractSearch(startTemp, endTemp, opts);
|
|
1734
|
-
this.removeTempNode(startTemp, startCluster);
|
|
1735
|
-
this.removeTempNode(endTemp, endCluster);
|
|
1736
|
-
if (!abstractPath || abstractPath.length === 0) {
|
|
1737
|
-
return EMPTY_PATH_RESULT;
|
|
1738
|
-
}
|
|
1739
|
-
result = this.refinePath(abstractPath, startX, startY, endX, endY, opts);
|
|
1740
|
-
}
|
|
1741
|
-
if (result.found) {
|
|
1742
|
-
this.pathCache.set(startX, startY, endX, endY, result, this.mapVersion);
|
|
1048
|
+
setWalkable(x, y, walkable) {
|
|
1049
|
+
const node = this.getNodeAt(x, y);
|
|
1050
|
+
if (node) {
|
|
1051
|
+
node.walkable = walkable;
|
|
1743
1052
|
}
|
|
1744
|
-
return result;
|
|
1745
1053
|
}
|
|
1746
1054
|
/**
|
|
1747
|
-
* @zh
|
|
1748
|
-
* @en
|
|
1055
|
+
* @zh 设置位置的移动代价
|
|
1056
|
+
* @en Set movement cost at position
|
|
1749
1057
|
*/
|
|
1750
|
-
|
|
1751
|
-
this.
|
|
1752
|
-
|
|
1753
|
-
|
|
1754
|
-
this.nodesByCluster.clear();
|
|
1755
|
-
this.nextNodeId = 0;
|
|
1756
|
-
this.entranceCount = 0;
|
|
1757
|
-
this.pathCache.invalidateAll();
|
|
1758
|
-
this.mapVersion++;
|
|
1759
|
-
this.preprocessed = false;
|
|
1760
|
-
}
|
|
1761
|
-
/**
|
|
1762
|
-
* @zh 通知地图区域变化
|
|
1763
|
-
* @en Notify map region change
|
|
1764
|
-
*/
|
|
1765
|
-
notifyRegionChange(minX, minY, maxX, maxY) {
|
|
1766
|
-
const affectedClusters = this.getAffectedClusters(minX, minY, maxX, maxY);
|
|
1767
|
-
for (const cluster of affectedClusters) {
|
|
1768
|
-
cluster.clearCache();
|
|
1769
|
-
for (const nodeId of cluster.nodeIds) {
|
|
1770
|
-
const node = this.abstractNodes.get(nodeId);
|
|
1771
|
-
if (node) {
|
|
1772
|
-
node.edges = node.edges.filter((e) => e.isInterEdge);
|
|
1773
|
-
}
|
|
1774
|
-
}
|
|
1775
|
-
this.buildClusterIntraEdges(cluster);
|
|
1058
|
+
setCost(x, y, cost) {
|
|
1059
|
+
const node = this.getNodeAt(x, y);
|
|
1060
|
+
if (node) {
|
|
1061
|
+
node.cost = cost;
|
|
1776
1062
|
}
|
|
1777
|
-
this.pathCache.invalidateRegion(minX, minY, maxX, maxY);
|
|
1778
|
-
this.mapVersion++;
|
|
1779
1063
|
}
|
|
1780
1064
|
/**
|
|
1781
|
-
* @zh
|
|
1782
|
-
* @en Get
|
|
1065
|
+
* @zh 获取节点的邻居
|
|
1066
|
+
* @en Get neighbors of a node
|
|
1783
1067
|
*/
|
|
1784
|
-
|
|
1785
|
-
|
|
1786
|
-
|
|
1787
|
-
|
|
1788
|
-
|
|
1789
|
-
|
|
1790
|
-
|
|
1791
|
-
|
|
1792
|
-
|
|
1793
|
-
|
|
1794
|
-
};
|
|
1795
|
-
}
|
|
1796
|
-
// =========================================================================
|
|
1797
|
-
// 预处理方法 | Preprocessing Methods
|
|
1798
|
-
// =========================================================================
|
|
1799
|
-
getMapBounds() {
|
|
1800
|
-
const mapAny = this.map;
|
|
1801
|
-
if (typeof mapAny.width === "number" && typeof mapAny.height === "number") {
|
|
1802
|
-
return {
|
|
1803
|
-
width: mapAny.width,
|
|
1804
|
-
height: mapAny.height
|
|
1805
|
-
};
|
|
1806
|
-
}
|
|
1807
|
-
return {
|
|
1808
|
-
width: 1e3,
|
|
1809
|
-
height: 1e3
|
|
1810
|
-
};
|
|
1811
|
-
}
|
|
1812
|
-
/**
|
|
1813
|
-
* @zh 构建集群
|
|
1814
|
-
* @en Build clusters
|
|
1815
|
-
*/
|
|
1816
|
-
buildClusters() {
|
|
1817
|
-
const clusterSize = this.config.clusterSize;
|
|
1818
|
-
this.clustersX = Math.ceil(this.mapWidth / clusterSize);
|
|
1819
|
-
this.clustersY = Math.ceil(this.mapHeight / clusterSize);
|
|
1820
|
-
this.clusterGrid = [];
|
|
1821
|
-
for (let cx = 0; cx < this.clustersX; cx++) {
|
|
1822
|
-
this.clusterGrid[cx] = [];
|
|
1823
|
-
for (let cy = 0; cy < this.clustersY; cy++) {
|
|
1824
|
-
this.clusterGrid[cx][cy] = null;
|
|
1068
|
+
getNeighbors(node) {
|
|
1069
|
+
const neighbors = [];
|
|
1070
|
+
const { x, y } = node.position;
|
|
1071
|
+
const directions = this.options.allowDiagonal ? DIRECTIONS_8 : DIRECTIONS_4;
|
|
1072
|
+
for (let i = 0; i < directions.length; i++) {
|
|
1073
|
+
const dir = directions[i];
|
|
1074
|
+
const nx = x + dir.dx;
|
|
1075
|
+
const ny = y + dir.dy;
|
|
1076
|
+
if (nx < 0 || nx >= this.width || ny < 0 || ny >= this.height) {
|
|
1077
|
+
continue;
|
|
1825
1078
|
}
|
|
1826
|
-
|
|
1827
|
-
|
|
1828
|
-
|
|
1829
|
-
for (let cx = 0; cx < this.clustersX; cx++) {
|
|
1830
|
-
const originX = cx * clusterSize;
|
|
1831
|
-
const originY = cy * clusterSize;
|
|
1832
|
-
const width = Math.min(clusterSize, this.mapWidth - originX);
|
|
1833
|
-
const height = Math.min(clusterSize, this.mapHeight - originY);
|
|
1834
|
-
const cluster = new Cluster(clusterId, originX, originY, width, height, this.map);
|
|
1835
|
-
this.clusters.push(cluster);
|
|
1836
|
-
this.clusterGrid[cx][cy] = clusterId;
|
|
1837
|
-
this.nodesByCluster.set(clusterId, []);
|
|
1838
|
-
clusterId++;
|
|
1079
|
+
const neighbor = this.nodes[ny][nx];
|
|
1080
|
+
if (!neighbor.walkable) {
|
|
1081
|
+
continue;
|
|
1839
1082
|
}
|
|
1840
|
-
|
|
1841
|
-
|
|
1842
|
-
|
|
1843
|
-
|
|
1844
|
-
|
|
1845
|
-
*/
|
|
1846
|
-
buildEntrances() {
|
|
1847
|
-
const clusterSize = this.config.clusterSize;
|
|
1848
|
-
for (let cy = 0; cy < this.clustersY; cy++) {
|
|
1849
|
-
for (let cx = 0; cx < this.clustersX; cx++) {
|
|
1850
|
-
const clusterId = this.clusterGrid[cx][cy];
|
|
1851
|
-
if (clusterId === null) continue;
|
|
1852
|
-
const cluster1 = this.clusters[clusterId];
|
|
1853
|
-
if (cx < this.clustersX - 1) {
|
|
1854
|
-
const cluster2Id = this.clusterGrid[cx + 1][cy];
|
|
1855
|
-
if (cluster2Id !== null) {
|
|
1856
|
-
const cluster2 = this.clusters[cluster2Id];
|
|
1857
|
-
this.detectAndCreateEntrances(cluster1, cluster2, "vertical");
|
|
1858
|
-
}
|
|
1859
|
-
}
|
|
1860
|
-
if (cy < this.clustersY - 1) {
|
|
1861
|
-
const cluster2Id = this.clusterGrid[cx][cy + 1];
|
|
1862
|
-
if (cluster2Id !== null) {
|
|
1863
|
-
const cluster2 = this.clusters[cluster2Id];
|
|
1864
|
-
this.detectAndCreateEntrances(cluster1, cluster2, "horizontal");
|
|
1865
|
-
}
|
|
1083
|
+
if (this.options.avoidCorners && dir.dx !== 0 && dir.dy !== 0) {
|
|
1084
|
+
const hNode = this.nodes[y][x + dir.dx];
|
|
1085
|
+
const vNode = this.nodes[y + dir.dy][x];
|
|
1086
|
+
if (!hNode.walkable || !vNode.walkable) {
|
|
1087
|
+
continue;
|
|
1866
1088
|
}
|
|
1867
1089
|
}
|
|
1090
|
+
neighbors.push(neighbor);
|
|
1868
1091
|
}
|
|
1092
|
+
return neighbors;
|
|
1869
1093
|
}
|
|
1870
1094
|
/**
|
|
1871
|
-
* @zh
|
|
1872
|
-
* @en
|
|
1873
|
-
*/
|
|
1874
|
-
detectAndCreateEntrances(cluster1, cluster2, boundaryDirection) {
|
|
1875
|
-
const spans = this.detectEntranceSpans(cluster1, cluster2, boundaryDirection);
|
|
1876
|
-
for (const span of spans) {
|
|
1877
|
-
this.createEntranceNodes(cluster1, cluster2, span, boundaryDirection);
|
|
1878
|
-
}
|
|
1879
|
-
}
|
|
1880
|
-
/**
|
|
1881
|
-
* @zh 检测边界上的连续可通行区间
|
|
1882
|
-
* @en Detect continuous walkable spans on boundary
|
|
1095
|
+
* @zh 遍历节点的邻居(零分配)
|
|
1096
|
+
* @en Iterate over neighbors (zero allocation)
|
|
1883
1097
|
*/
|
|
1884
|
-
|
|
1885
|
-
const
|
|
1886
|
-
|
|
1887
|
-
|
|
1888
|
-
const
|
|
1889
|
-
const
|
|
1890
|
-
const
|
|
1891
|
-
|
|
1892
|
-
|
|
1893
|
-
const walkable1 = this.map.isWalkable(x1, y);
|
|
1894
|
-
const walkable2 = this.map.isWalkable(x2, y);
|
|
1895
|
-
if (walkable1 && walkable2) {
|
|
1896
|
-
if (spanStart === null) {
|
|
1897
|
-
spanStart = y;
|
|
1898
|
-
}
|
|
1899
|
-
} else {
|
|
1900
|
-
if (spanStart !== null) {
|
|
1901
|
-
spans.push({
|
|
1902
|
-
start: spanStart,
|
|
1903
|
-
end: y - 1
|
|
1904
|
-
});
|
|
1905
|
-
spanStart = null;
|
|
1906
|
-
}
|
|
1907
|
-
}
|
|
1908
|
-
}
|
|
1909
|
-
if (spanStart !== null) {
|
|
1910
|
-
spans.push({
|
|
1911
|
-
start: spanStart,
|
|
1912
|
-
end: endY - 1
|
|
1913
|
-
});
|
|
1914
|
-
}
|
|
1915
|
-
} else {
|
|
1916
|
-
const y1 = cluster1.originY + cluster1.height - 1;
|
|
1917
|
-
const y2 = cluster2.originY;
|
|
1918
|
-
const startX = Math.max(cluster1.originX, cluster2.originX);
|
|
1919
|
-
const endX = Math.min(cluster1.originX + cluster1.width, cluster2.originX + cluster2.width);
|
|
1920
|
-
let spanStart = null;
|
|
1921
|
-
for (let x = startX; x < endX; x++) {
|
|
1922
|
-
const walkable1 = this.map.isWalkable(x, y1);
|
|
1923
|
-
const walkable2 = this.map.isWalkable(x, y2);
|
|
1924
|
-
if (walkable1 && walkable2) {
|
|
1925
|
-
if (spanStart === null) {
|
|
1926
|
-
spanStart = x;
|
|
1927
|
-
}
|
|
1928
|
-
} else {
|
|
1929
|
-
if (spanStart !== null) {
|
|
1930
|
-
spans.push({
|
|
1931
|
-
start: spanStart,
|
|
1932
|
-
end: x - 1
|
|
1933
|
-
});
|
|
1934
|
-
spanStart = null;
|
|
1935
|
-
}
|
|
1936
|
-
}
|
|
1937
|
-
}
|
|
1938
|
-
if (spanStart !== null) {
|
|
1939
|
-
spans.push({
|
|
1940
|
-
start: spanStart,
|
|
1941
|
-
end: endX - 1
|
|
1942
|
-
});
|
|
1098
|
+
forEachNeighbor(node, callback) {
|
|
1099
|
+
const { x, y } = node.position;
|
|
1100
|
+
const directions = this.options.allowDiagonal ? DIRECTIONS_8 : DIRECTIONS_4;
|
|
1101
|
+
for (let i = 0; i < directions.length; i++) {
|
|
1102
|
+
const dir = directions[i];
|
|
1103
|
+
const nx = x + dir.dx;
|
|
1104
|
+
const ny = y + dir.dy;
|
|
1105
|
+
if (nx < 0 || nx >= this.width || ny < 0 || ny >= this.height) {
|
|
1106
|
+
continue;
|
|
1943
1107
|
}
|
|
1944
|
-
|
|
1945
|
-
|
|
1946
|
-
|
|
1947
|
-
/**
|
|
1948
|
-
* @zh 为入口区间创建抽象节点
|
|
1949
|
-
* @en Create abstract nodes for entrance span
|
|
1950
|
-
*/
|
|
1951
|
-
createEntranceNodes(cluster1, cluster2, span, boundaryDirection) {
|
|
1952
|
-
const spanLength = span.end - span.start + 1;
|
|
1953
|
-
const maxWidth = this.config.maxEntranceWidth;
|
|
1954
|
-
const strategy = this.config.entranceStrategy;
|
|
1955
|
-
const positions = [];
|
|
1956
|
-
if (spanLength <= maxWidth) {
|
|
1957
|
-
positions.push(Math.floor((span.start + span.end) / 2));
|
|
1958
|
-
} else {
|
|
1959
|
-
const numNodes = Math.ceil(spanLength / maxWidth);
|
|
1960
|
-
const spacing = spanLength / numNodes;
|
|
1961
|
-
for (let i = 0; i < numNodes; i++) {
|
|
1962
|
-
const pos = Math.floor(span.start + spacing * (i + 0.5));
|
|
1963
|
-
positions.push(Math.min(pos, span.end));
|
|
1108
|
+
const neighbor = this.nodes[ny][nx];
|
|
1109
|
+
if (!neighbor.walkable) {
|
|
1110
|
+
continue;
|
|
1964
1111
|
}
|
|
1965
|
-
if (
|
|
1966
|
-
|
|
1967
|
-
|
|
1968
|
-
|
|
1969
|
-
|
|
1970
|
-
positions.push(span.end);
|
|
1112
|
+
if (this.options.avoidCorners && dir.dx !== 0 && dir.dy !== 0) {
|
|
1113
|
+
const hNode = this.nodes[y][x + dir.dx];
|
|
1114
|
+
const vNode = this.nodes[y + dir.dy][x];
|
|
1115
|
+
if (!hNode.walkable || !vNode.walkable) {
|
|
1116
|
+
continue;
|
|
1971
1117
|
}
|
|
1972
1118
|
}
|
|
1973
|
-
|
|
1974
|
-
|
|
1975
|
-
let p1, p2;
|
|
1976
|
-
if (boundaryDirection === "vertical") {
|
|
1977
|
-
p1 = {
|
|
1978
|
-
x: cluster1.originX + cluster1.width - 1,
|
|
1979
|
-
y: pos
|
|
1980
|
-
};
|
|
1981
|
-
p2 = {
|
|
1982
|
-
x: cluster2.originX,
|
|
1983
|
-
y: pos
|
|
1984
|
-
};
|
|
1985
|
-
} else {
|
|
1986
|
-
p1 = {
|
|
1987
|
-
x: pos,
|
|
1988
|
-
y: cluster1.originY + cluster1.height - 1
|
|
1989
|
-
};
|
|
1990
|
-
p2 = {
|
|
1991
|
-
x: pos,
|
|
1992
|
-
y: cluster2.originY
|
|
1993
|
-
};
|
|
1994
|
-
}
|
|
1995
|
-
const node1 = this.createAbstractNode(p1, cluster1);
|
|
1996
|
-
const node2 = this.createAbstractNode(p2, cluster2);
|
|
1997
|
-
const interCost = 1;
|
|
1998
|
-
node1.edges.push({
|
|
1999
|
-
targetNodeId: node2.id,
|
|
2000
|
-
cost: interCost,
|
|
2001
|
-
isInterEdge: true,
|
|
2002
|
-
innerPath: null
|
|
2003
|
-
});
|
|
2004
|
-
node2.edges.push({
|
|
2005
|
-
targetNodeId: node1.id,
|
|
2006
|
-
cost: interCost,
|
|
2007
|
-
isInterEdge: true,
|
|
2008
|
-
innerPath: null
|
|
2009
|
-
});
|
|
2010
|
-
this.entranceCount++;
|
|
2011
|
-
}
|
|
2012
|
-
}
|
|
2013
|
-
/**
|
|
2014
|
-
* @zh 创建抽象节点
|
|
2015
|
-
* @en Create abstract node
|
|
2016
|
-
*/
|
|
2017
|
-
createAbstractNode(position, cluster) {
|
|
2018
|
-
const concreteId = position.y * this.mapWidth + position.x;
|
|
2019
|
-
for (const nodeId of cluster.nodeIds) {
|
|
2020
|
-
const existing = this.abstractNodes.get(nodeId);
|
|
2021
|
-
if (existing && existing.concreteNodeId === concreteId) {
|
|
2022
|
-
return existing;
|
|
1119
|
+
if (callback(neighbor) === false) {
|
|
1120
|
+
return;
|
|
2023
1121
|
}
|
|
2024
1122
|
}
|
|
2025
|
-
const node = {
|
|
2026
|
-
id: this.nextNodeId++,
|
|
2027
|
-
position: {
|
|
2028
|
-
x: position.x,
|
|
2029
|
-
y: position.y
|
|
2030
|
-
},
|
|
2031
|
-
clusterId: cluster.id,
|
|
2032
|
-
concreteNodeId: concreteId,
|
|
2033
|
-
edges: []
|
|
2034
|
-
};
|
|
2035
|
-
this.abstractNodes.set(node.id, node);
|
|
2036
|
-
cluster.addNodeId(node.id);
|
|
2037
|
-
const clusterNodes = this.nodesByCluster.get(cluster.id);
|
|
2038
|
-
if (clusterNodes) {
|
|
2039
|
-
clusterNodes.push(node.id);
|
|
2040
|
-
}
|
|
2041
|
-
return node;
|
|
2042
1123
|
}
|
|
2043
1124
|
/**
|
|
2044
|
-
* @zh
|
|
2045
|
-
* @en
|
|
1125
|
+
* @zh 计算启发式距离
|
|
1126
|
+
* @en Calculate heuristic distance
|
|
2046
1127
|
*/
|
|
2047
|
-
|
|
2048
|
-
|
|
2049
|
-
this.buildClusterIntraEdges(cluster);
|
|
2050
|
-
}
|
|
2051
|
-
}
|
|
2052
|
-
/**
|
|
2053
|
-
* @zh 构建单个集群的 intra-edges
|
|
2054
|
-
* @en Build intra-edges for single cluster
|
|
2055
|
-
*/
|
|
2056
|
-
buildClusterIntraEdges(cluster) {
|
|
2057
|
-
const nodeIds = cluster.nodeIds;
|
|
2058
|
-
if (nodeIds.length < 2) return;
|
|
2059
|
-
if (this.config.lazyIntraEdges) {
|
|
2060
|
-
this.buildLazyIntraEdges(cluster);
|
|
2061
|
-
} else {
|
|
2062
|
-
this.buildEagerIntraEdges(cluster);
|
|
2063
|
-
}
|
|
2064
|
-
}
|
|
2065
|
-
/**
|
|
2066
|
-
* @zh 延迟构建 intra-edges(只用启发式距离)
|
|
2067
|
-
* @en Build lazy intra-edges (using heuristic distance only)
|
|
2068
|
-
*/
|
|
2069
|
-
buildLazyIntraEdges(cluster) {
|
|
2070
|
-
const nodeIds = cluster.nodeIds;
|
|
2071
|
-
for (let i = 0; i < nodeIds.length; i++) {
|
|
2072
|
-
for (let j = i + 1; j < nodeIds.length; j++) {
|
|
2073
|
-
const node1 = this.abstractNodes.get(nodeIds[i]);
|
|
2074
|
-
const node2 = this.abstractNodes.get(nodeIds[j]);
|
|
2075
|
-
const heuristicCost = this.heuristic(node1.position, node2.position);
|
|
2076
|
-
node1.edges.push({
|
|
2077
|
-
targetNodeId: node2.id,
|
|
2078
|
-
cost: heuristicCost,
|
|
2079
|
-
isInterEdge: false,
|
|
2080
|
-
innerPath: null
|
|
2081
|
-
// 标记为未计算
|
|
2082
|
-
});
|
|
2083
|
-
node2.edges.push({
|
|
2084
|
-
targetNodeId: node1.id,
|
|
2085
|
-
cost: heuristicCost,
|
|
2086
|
-
isInterEdge: false,
|
|
2087
|
-
innerPath: null
|
|
2088
|
-
});
|
|
2089
|
-
}
|
|
2090
|
-
}
|
|
2091
|
-
}
|
|
2092
|
-
/**
|
|
2093
|
-
* @zh 立即构建 intra-edges(计算真实路径)
|
|
2094
|
-
* @en Build eager intra-edges (compute actual paths)
|
|
2095
|
-
*/
|
|
2096
|
-
buildEagerIntraEdges(cluster) {
|
|
2097
|
-
const nodeIds = cluster.nodeIds;
|
|
2098
|
-
const subPathfinder = new AStarPathfinder(cluster.subMap);
|
|
2099
|
-
for (let i = 0; i < nodeIds.length; i++) {
|
|
2100
|
-
for (let j = i + 1; j < nodeIds.length; j++) {
|
|
2101
|
-
const node1 = this.abstractNodes.get(nodeIds[i]);
|
|
2102
|
-
const node2 = this.abstractNodes.get(nodeIds[j]);
|
|
2103
|
-
const local1 = cluster.subMap.globalToLocal(node1.position.x, node1.position.y);
|
|
2104
|
-
const local2 = cluster.subMap.globalToLocal(node2.position.x, node2.position.y);
|
|
2105
|
-
const result = subPathfinder.findPath(local1.x, local1.y, local2.x, local2.y);
|
|
2106
|
-
if (result.found && result.path.length > 0) {
|
|
2107
|
-
const globalPath = result.path.map((p) => {
|
|
2108
|
-
const global = cluster.subMap.localToGlobal(p.x, p.y);
|
|
2109
|
-
return global.y * this.mapWidth + global.x;
|
|
2110
|
-
});
|
|
2111
|
-
if (this.config.cacheInternalPaths) {
|
|
2112
|
-
cluster.setCache(node1.id, node2.id, result.cost, globalPath);
|
|
2113
|
-
cluster.setCache(node2.id, node1.id, result.cost, [
|
|
2114
|
-
...globalPath
|
|
2115
|
-
].reverse());
|
|
2116
|
-
}
|
|
2117
|
-
node1.edges.push({
|
|
2118
|
-
targetNodeId: node2.id,
|
|
2119
|
-
cost: result.cost,
|
|
2120
|
-
isInterEdge: false,
|
|
2121
|
-
innerPath: this.config.cacheInternalPaths ? globalPath : null
|
|
2122
|
-
});
|
|
2123
|
-
node2.edges.push({
|
|
2124
|
-
targetNodeId: node1.id,
|
|
2125
|
-
cost: result.cost,
|
|
2126
|
-
isInterEdge: false,
|
|
2127
|
-
innerPath: this.config.cacheInternalPaths ? [
|
|
2128
|
-
...globalPath
|
|
2129
|
-
].reverse() : null
|
|
2130
|
-
});
|
|
2131
|
-
}
|
|
2132
|
-
}
|
|
2133
|
-
}
|
|
2134
|
-
}
|
|
2135
|
-
/**
|
|
2136
|
-
* @zh 按需计算 intra-edge 的真实路径
|
|
2137
|
-
* @en Compute actual path for intra-edge on demand
|
|
2138
|
-
*/
|
|
2139
|
-
computeIntraEdgePath(fromNode, toNode, edge) {
|
|
2140
|
-
const cluster = this.clusters[fromNode.clusterId];
|
|
2141
|
-
if (!cluster) return null;
|
|
2142
|
-
const cachedPath = cluster.getCachedPath(fromNode.id, toNode.id);
|
|
2143
|
-
const cachedCost = cluster.getCachedDistance(fromNode.id, toNode.id);
|
|
2144
|
-
if (cachedPath && cachedCost !== void 0) {
|
|
2145
|
-
edge.cost = cachedCost;
|
|
2146
|
-
edge.innerPath = cachedPath;
|
|
2147
|
-
return {
|
|
2148
|
-
cost: cachedCost,
|
|
2149
|
-
path: cachedPath
|
|
2150
|
-
};
|
|
2151
|
-
}
|
|
2152
|
-
const subPathfinder = new AStarPathfinder(cluster.subMap);
|
|
2153
|
-
const local1 = cluster.subMap.globalToLocal(fromNode.position.x, fromNode.position.y);
|
|
2154
|
-
const local2 = cluster.subMap.globalToLocal(toNode.position.x, toNode.position.y);
|
|
2155
|
-
const result = subPathfinder.findPath(local1.x, local1.y, local2.x, local2.y);
|
|
2156
|
-
if (result.found && result.path.length > 0) {
|
|
2157
|
-
const globalPath = result.path.map((p) => {
|
|
2158
|
-
const global = cluster.subMap.localToGlobal(p.x, p.y);
|
|
2159
|
-
return global.y * this.mapWidth + global.x;
|
|
2160
|
-
});
|
|
2161
|
-
if (this.config.cacheInternalPaths) {
|
|
2162
|
-
cluster.setCache(fromNode.id, toNode.id, result.cost, globalPath);
|
|
2163
|
-
cluster.setCache(toNode.id, fromNode.id, result.cost, [
|
|
2164
|
-
...globalPath
|
|
2165
|
-
].reverse());
|
|
2166
|
-
}
|
|
2167
|
-
edge.cost = result.cost;
|
|
2168
|
-
edge.innerPath = globalPath;
|
|
2169
|
-
const reverseEdge = toNode.edges.find((e) => e.targetNodeId === fromNode.id);
|
|
2170
|
-
if (reverseEdge) {
|
|
2171
|
-
reverseEdge.cost = result.cost;
|
|
2172
|
-
reverseEdge.innerPath = [
|
|
2173
|
-
...globalPath
|
|
2174
|
-
].reverse();
|
|
2175
|
-
}
|
|
2176
|
-
return {
|
|
2177
|
-
cost: result.cost,
|
|
2178
|
-
path: globalPath
|
|
2179
|
-
};
|
|
2180
|
-
}
|
|
2181
|
-
return null;
|
|
1128
|
+
heuristic(a, b) {
|
|
1129
|
+
return this.options.heuristic(a, b);
|
|
2182
1130
|
}
|
|
2183
|
-
// =========================================================================
|
|
2184
|
-
// 搜索方法 | Search Methods
|
|
2185
|
-
// =========================================================================
|
|
2186
1131
|
/**
|
|
2187
|
-
* @zh
|
|
2188
|
-
* @en
|
|
1132
|
+
* @zh 计算移动代价
|
|
1133
|
+
* @en Calculate movement cost
|
|
2189
1134
|
*/
|
|
2190
|
-
|
|
2191
|
-
const
|
|
2192
|
-
const
|
|
2193
|
-
if (
|
|
2194
|
-
return
|
|
2195
|
-
}
|
|
2196
|
-
const clusterId = this.clusterGrid[cx]?.[cy];
|
|
2197
|
-
if (clusterId === null || clusterId === void 0) {
|
|
2198
|
-
return null;
|
|
2199
|
-
}
|
|
2200
|
-
return this.clusters[clusterId] || null;
|
|
2201
|
-
}
|
|
2202
|
-
/**
|
|
2203
|
-
* @zh 获取受影响的集群
|
|
2204
|
-
* @en Get affected clusters
|
|
2205
|
-
*/
|
|
2206
|
-
getAffectedClusters(minX, minY, maxX, maxY) {
|
|
2207
|
-
const affected = [];
|
|
2208
|
-
const clusterSize = this.config.clusterSize;
|
|
2209
|
-
const minCX = Math.floor(minX / clusterSize);
|
|
2210
|
-
const maxCX = Math.floor(maxX / clusterSize);
|
|
2211
|
-
const minCY = Math.floor(minY / clusterSize);
|
|
2212
|
-
const maxCY = Math.floor(maxY / clusterSize);
|
|
2213
|
-
for (let cy = minCY; cy <= maxCY; cy++) {
|
|
2214
|
-
for (let cx = minCX; cx <= maxCX; cx++) {
|
|
2215
|
-
if (cx >= 0 && cx < this.clustersX && cy >= 0 && cy < this.clustersY) {
|
|
2216
|
-
const clusterId = this.clusterGrid[cx]?.[cy];
|
|
2217
|
-
if (clusterId !== null && clusterId !== void 0) {
|
|
2218
|
-
affected.push(this.clusters[clusterId]);
|
|
2219
|
-
}
|
|
2220
|
-
}
|
|
2221
|
-
}
|
|
1135
|
+
getMovementCost(from, to) {
|
|
1136
|
+
const dx = Math.abs(from.position.x - to.position.x);
|
|
1137
|
+
const dy = Math.abs(from.position.y - to.position.y);
|
|
1138
|
+
if (dx !== 0 && dy !== 0) {
|
|
1139
|
+
return to.cost * this.options.diagonalCost;
|
|
2222
1140
|
}
|
|
2223
|
-
return
|
|
1141
|
+
return to.cost;
|
|
2224
1142
|
}
|
|
2225
1143
|
/**
|
|
2226
|
-
* @zh
|
|
2227
|
-
* @en
|
|
1144
|
+
* @zh 从二维数组加载地图
|
|
1145
|
+
* @en Load map from 2D array
|
|
1146
|
+
*
|
|
1147
|
+
* @param data - @zh 0=可通行,非0=不可通行 @en 0=walkable, non-0=blocked
|
|
2228
1148
|
*/
|
|
2229
|
-
|
|
2230
|
-
|
|
2231
|
-
|
|
2232
|
-
|
|
2233
|
-
if (existing && existing.concreteNodeId === concreteId) {
|
|
2234
|
-
return existing;
|
|
2235
|
-
}
|
|
2236
|
-
}
|
|
2237
|
-
const tempNode = {
|
|
2238
|
-
id: this.nextNodeId++,
|
|
2239
|
-
position: {
|
|
2240
|
-
x,
|
|
2241
|
-
y
|
|
2242
|
-
},
|
|
2243
|
-
clusterId: cluster.id,
|
|
2244
|
-
concreteNodeId: concreteId,
|
|
2245
|
-
edges: []
|
|
2246
|
-
};
|
|
2247
|
-
this.abstractNodes.set(tempNode.id, tempNode);
|
|
2248
|
-
cluster.addNodeId(tempNode.id);
|
|
2249
|
-
const subPathfinder = new AStarPathfinder(cluster.subMap);
|
|
2250
|
-
const localPos = cluster.subMap.globalToLocal(x, y);
|
|
2251
|
-
for (const existingNodeId of cluster.nodeIds) {
|
|
2252
|
-
if (existingNodeId === tempNode.id) continue;
|
|
2253
|
-
const existingNode = this.abstractNodes.get(existingNodeId);
|
|
2254
|
-
if (!existingNode) continue;
|
|
2255
|
-
const targetLocalPos = cluster.subMap.globalToLocal(existingNode.position.x, existingNode.position.y);
|
|
2256
|
-
const result = subPathfinder.findPath(localPos.x, localPos.y, targetLocalPos.x, targetLocalPos.y);
|
|
2257
|
-
if (result.found && result.path.length > 0) {
|
|
2258
|
-
const globalPath = result.path.map((p) => {
|
|
2259
|
-
const global = cluster.subMap.localToGlobal(p.x, p.y);
|
|
2260
|
-
return global.y * this.mapWidth + global.x;
|
|
2261
|
-
});
|
|
2262
|
-
tempNode.edges.push({
|
|
2263
|
-
targetNodeId: existingNode.id,
|
|
2264
|
-
cost: result.cost,
|
|
2265
|
-
isInterEdge: false,
|
|
2266
|
-
innerPath: globalPath
|
|
2267
|
-
});
|
|
2268
|
-
existingNode.edges.push({
|
|
2269
|
-
targetNodeId: tempNode.id,
|
|
2270
|
-
cost: result.cost,
|
|
2271
|
-
isInterEdge: false,
|
|
2272
|
-
innerPath: [
|
|
2273
|
-
...globalPath
|
|
2274
|
-
].reverse()
|
|
2275
|
-
});
|
|
1149
|
+
loadFromArray(data) {
|
|
1150
|
+
for (let y = 0; y < Math.min(data.length, this.height); y++) {
|
|
1151
|
+
for (let x = 0; x < Math.min(data[y].length, this.width); x++) {
|
|
1152
|
+
this.nodes[y][x].walkable = data[y][x] === 0;
|
|
2276
1153
|
}
|
|
2277
1154
|
}
|
|
2278
|
-
return tempNode;
|
|
2279
1155
|
}
|
|
2280
1156
|
/**
|
|
2281
|
-
* @zh
|
|
2282
|
-
* @en
|
|
1157
|
+
* @zh 从字符串加载地图
|
|
1158
|
+
* @en Load map from string
|
|
1159
|
+
*
|
|
1160
|
+
* @param str - @zh 地图字符串,'.'=可通行,'#'=障碍 @en Map string, '.'=walkable, '#'=blocked
|
|
2283
1161
|
*/
|
|
2284
|
-
|
|
2285
|
-
|
|
2286
|
-
|
|
2287
|
-
const
|
|
2288
|
-
|
|
2289
|
-
|
|
1162
|
+
loadFromString(str) {
|
|
1163
|
+
const lines = str.trim().split("\n");
|
|
1164
|
+
for (let y = 0; y < Math.min(lines.length, this.height); y++) {
|
|
1165
|
+
const line = lines[y];
|
|
1166
|
+
for (let x = 0; x < Math.min(line.length, this.width); x++) {
|
|
1167
|
+
this.nodes[y][x].walkable = line[x] !== "#";
|
|
2290
1168
|
}
|
|
2291
1169
|
}
|
|
2292
|
-
cluster.removeNodeId(node.id);
|
|
2293
|
-
this.abstractNodes.delete(node.id);
|
|
2294
1170
|
}
|
|
2295
1171
|
/**
|
|
2296
|
-
* @zh
|
|
2297
|
-
* @en
|
|
1172
|
+
* @zh 导出为字符串
|
|
1173
|
+
* @en Export to string
|
|
2298
1174
|
*/
|
|
2299
|
-
|
|
2300
|
-
|
|
2301
|
-
|
|
2302
|
-
|
|
2303
|
-
|
|
2304
|
-
const startSearchNode = {
|
|
2305
|
-
node: startNode,
|
|
2306
|
-
g: 0,
|
|
2307
|
-
h,
|
|
2308
|
-
f: h,
|
|
2309
|
-
parent: null,
|
|
2310
|
-
closed: false,
|
|
2311
|
-
opened: true,
|
|
2312
|
-
heapIndex: -1
|
|
2313
|
-
};
|
|
2314
|
-
openList.push(startSearchNode);
|
|
2315
|
-
nodeMap.set(startNode.id, startSearchNode);
|
|
2316
|
-
let nodesSearched = 0;
|
|
2317
|
-
while (!openList.isEmpty && nodesSearched < opts.maxNodes) {
|
|
2318
|
-
const current = openList.pop();
|
|
2319
|
-
current.closed = true;
|
|
2320
|
-
nodesSearched++;
|
|
2321
|
-
if (current.node.id === endNode.id) {
|
|
2322
|
-
return this.reconstructPath(current);
|
|
2323
|
-
}
|
|
2324
|
-
for (const edge of current.node.edges) {
|
|
2325
|
-
let neighbor = nodeMap.get(edge.targetNodeId);
|
|
2326
|
-
if (!neighbor) {
|
|
2327
|
-
const neighborNode = this.abstractNodes.get(edge.targetNodeId);
|
|
2328
|
-
if (!neighborNode) continue;
|
|
2329
|
-
const nh = this.heuristic(neighborNode.position, endPosition) * opts.heuristicWeight;
|
|
2330
|
-
neighbor = {
|
|
2331
|
-
node: neighborNode,
|
|
2332
|
-
g: Infinity,
|
|
2333
|
-
h: nh,
|
|
2334
|
-
f: Infinity,
|
|
2335
|
-
parent: null,
|
|
2336
|
-
closed: false,
|
|
2337
|
-
opened: false,
|
|
2338
|
-
heapIndex: -1
|
|
2339
|
-
};
|
|
2340
|
-
nodeMap.set(edge.targetNodeId, neighbor);
|
|
2341
|
-
}
|
|
2342
|
-
if (neighbor.closed) continue;
|
|
2343
|
-
const tentativeG = current.g + edge.cost;
|
|
2344
|
-
if (!neighbor.opened) {
|
|
2345
|
-
neighbor.g = tentativeG;
|
|
2346
|
-
neighbor.f = tentativeG + neighbor.h;
|
|
2347
|
-
neighbor.parent = current;
|
|
2348
|
-
neighbor.opened = true;
|
|
2349
|
-
openList.push(neighbor);
|
|
2350
|
-
} else if (tentativeG < neighbor.g) {
|
|
2351
|
-
neighbor.g = tentativeG;
|
|
2352
|
-
neighbor.f = tentativeG + neighbor.h;
|
|
2353
|
-
neighbor.parent = current;
|
|
2354
|
-
openList.update(neighbor);
|
|
2355
|
-
}
|
|
1175
|
+
toString() {
|
|
1176
|
+
let result = "";
|
|
1177
|
+
for (let y = 0; y < this.height; y++) {
|
|
1178
|
+
for (let x = 0; x < this.width; x++) {
|
|
1179
|
+
result += this.nodes[y][x].walkable ? "." : "#";
|
|
2356
1180
|
}
|
|
1181
|
+
result += "\n";
|
|
2357
1182
|
}
|
|
2358
|
-
return
|
|
2359
|
-
}
|
|
2360
|
-
/**
|
|
2361
|
-
* @zh 重建抽象路径
|
|
2362
|
-
* @en Reconstruct abstract path
|
|
2363
|
-
*/
|
|
2364
|
-
reconstructPath(endNode) {
|
|
2365
|
-
const path = [];
|
|
2366
|
-
let current = endNode;
|
|
2367
|
-
while (current) {
|
|
2368
|
-
path.unshift(current.node);
|
|
2369
|
-
current = current.parent;
|
|
2370
|
-
}
|
|
2371
|
-
return path;
|
|
1183
|
+
return result;
|
|
2372
1184
|
}
|
|
2373
1185
|
/**
|
|
2374
|
-
* @zh
|
|
2375
|
-
* @en
|
|
1186
|
+
* @zh 重置所有节点为可通行
|
|
1187
|
+
* @en Reset all nodes to walkable
|
|
2376
1188
|
*/
|
|
2377
|
-
|
|
2378
|
-
|
|
2379
|
-
|
|
2380
|
-
|
|
2381
|
-
|
|
2382
|
-
let totalCost = 0;
|
|
2383
|
-
let nodesSearched = abstractPath.length;
|
|
2384
|
-
for (let i = 0; i < abstractPath.length - 1; i++) {
|
|
2385
|
-
const fromNode = abstractPath[i];
|
|
2386
|
-
const toNode = abstractPath[i + 1];
|
|
2387
|
-
const edge = fromNode.edges.find((e) => e.targetNodeId === toNode.id);
|
|
2388
|
-
if (!edge) {
|
|
2389
|
-
const segResult = this.findLocalPath(fromNode.position.x, fromNode.position.y, toNode.position.x, toNode.position.y, opts);
|
|
2390
|
-
if (segResult.found) {
|
|
2391
|
-
this.appendPath(fullPath, segResult.path);
|
|
2392
|
-
totalCost += segResult.cost;
|
|
2393
|
-
nodesSearched += segResult.nodesSearched;
|
|
2394
|
-
}
|
|
2395
|
-
} else if (edge.isInterEdge) {
|
|
2396
|
-
if (fullPath.length === 0 || fullPath[fullPath.length - 1].x !== fromNode.position.x || fullPath[fullPath.length - 1].y !== fromNode.position.y) {
|
|
2397
|
-
fullPath.push({
|
|
2398
|
-
x: fromNode.position.x,
|
|
2399
|
-
y: fromNode.position.y
|
|
2400
|
-
});
|
|
2401
|
-
}
|
|
2402
|
-
fullPath.push({
|
|
2403
|
-
x: toNode.position.x,
|
|
2404
|
-
y: toNode.position.y
|
|
2405
|
-
});
|
|
2406
|
-
totalCost += edge.cost;
|
|
2407
|
-
} else if (edge.innerPath && edge.innerPath.length > 0) {
|
|
2408
|
-
const concretePath = edge.innerPath.map((id) => ({
|
|
2409
|
-
x: id % this.mapWidth,
|
|
2410
|
-
y: Math.floor(id / this.mapWidth)
|
|
2411
|
-
}));
|
|
2412
|
-
this.appendPath(fullPath, concretePath);
|
|
2413
|
-
totalCost += edge.cost;
|
|
2414
|
-
} else {
|
|
2415
|
-
const computed = this.computeIntraEdgePath(fromNode, toNode, edge);
|
|
2416
|
-
if (computed && computed.path.length > 0) {
|
|
2417
|
-
const concretePath = computed.path.map((id) => ({
|
|
2418
|
-
x: id % this.mapWidth,
|
|
2419
|
-
y: Math.floor(id / this.mapWidth)
|
|
2420
|
-
}));
|
|
2421
|
-
this.appendPath(fullPath, concretePath);
|
|
2422
|
-
totalCost += computed.cost;
|
|
2423
|
-
} else {
|
|
2424
|
-
const segResult = this.findLocalPath(fromNode.position.x, fromNode.position.y, toNode.position.x, toNode.position.y, opts);
|
|
2425
|
-
if (segResult.found) {
|
|
2426
|
-
this.appendPath(fullPath, segResult.path);
|
|
2427
|
-
totalCost += segResult.cost;
|
|
2428
|
-
nodesSearched += segResult.nodesSearched;
|
|
2429
|
-
}
|
|
2430
|
-
}
|
|
2431
|
-
}
|
|
2432
|
-
}
|
|
2433
|
-
if (fullPath.length > 0 && (fullPath[0].x !== startX || fullPath[0].y !== startY)) {
|
|
2434
|
-
const firstPoint = fullPath[0];
|
|
2435
|
-
if (Math.abs(firstPoint.x - startX) <= 1 && Math.abs(firstPoint.y - startY) <= 1) {
|
|
2436
|
-
fullPath.unshift({
|
|
2437
|
-
x: startX,
|
|
2438
|
-
y: startY
|
|
2439
|
-
});
|
|
2440
|
-
} else {
|
|
2441
|
-
const segResult = this.findLocalPath(startX, startY, firstPoint.x, firstPoint.y, opts);
|
|
2442
|
-
if (segResult.found) {
|
|
2443
|
-
fullPath.splice(0, 0, ...segResult.path.slice(0, -1));
|
|
2444
|
-
totalCost += segResult.cost;
|
|
2445
|
-
}
|
|
2446
|
-
}
|
|
2447
|
-
}
|
|
2448
|
-
if (fullPath.length > 0) {
|
|
2449
|
-
const lastPoint = fullPath[fullPath.length - 1];
|
|
2450
|
-
if (lastPoint.x !== endX || lastPoint.y !== endY) {
|
|
2451
|
-
if (Math.abs(lastPoint.x - endX) <= 1 && Math.abs(lastPoint.y - endY) <= 1) {
|
|
2452
|
-
fullPath.push({
|
|
2453
|
-
x: endX,
|
|
2454
|
-
y: endY
|
|
2455
|
-
});
|
|
2456
|
-
} else {
|
|
2457
|
-
const segResult = this.findLocalPath(lastPoint.x, lastPoint.y, endX, endY, opts);
|
|
2458
|
-
if (segResult.found) {
|
|
2459
|
-
fullPath.push(...segResult.path.slice(1));
|
|
2460
|
-
totalCost += segResult.cost;
|
|
2461
|
-
}
|
|
2462
|
-
}
|
|
1189
|
+
reset() {
|
|
1190
|
+
for (let y = 0; y < this.height; y++) {
|
|
1191
|
+
for (let x = 0; x < this.width; x++) {
|
|
1192
|
+
this.nodes[y][x].walkable = true;
|
|
1193
|
+
this.nodes[y][x].cost = 1;
|
|
2463
1194
|
}
|
|
2464
1195
|
}
|
|
2465
|
-
return {
|
|
2466
|
-
found: fullPath.length > 0,
|
|
2467
|
-
path: fullPath,
|
|
2468
|
-
cost: totalCost,
|
|
2469
|
-
nodesSearched
|
|
2470
|
-
};
|
|
2471
1196
|
}
|
|
2472
1197
|
/**
|
|
2473
|
-
* @zh
|
|
2474
|
-
* @en
|
|
1198
|
+
* @zh 设置矩形区域的通行性
|
|
1199
|
+
* @en Set walkability for a rectangle region
|
|
2475
1200
|
*/
|
|
2476
|
-
|
|
2477
|
-
|
|
2478
|
-
|
|
2479
|
-
|
|
2480
|
-
const last = fullPath[fullPath.length - 1];
|
|
2481
|
-
if (last.x === segment[0].x && last.y === segment[0].y) {
|
|
2482
|
-
startIdx = 1;
|
|
1201
|
+
setRectWalkable(x, y, width, height, walkable) {
|
|
1202
|
+
for (let dy = 0; dy < height; dy++) {
|
|
1203
|
+
for (let dx = 0; dx < width; dx++) {
|
|
1204
|
+
this.setWalkable(x + dx, y + dy, walkable);
|
|
2483
1205
|
}
|
|
2484
1206
|
}
|
|
2485
|
-
for (let i = startIdx; i < segment.length; i++) {
|
|
2486
|
-
fullPath.push({
|
|
2487
|
-
x: segment[i].x,
|
|
2488
|
-
y: segment[i].y
|
|
2489
|
-
});
|
|
2490
|
-
}
|
|
2491
|
-
}
|
|
2492
|
-
/**
|
|
2493
|
-
* @zh 局部寻路
|
|
2494
|
-
* @en Local pathfinding
|
|
2495
|
-
*/
|
|
2496
|
-
findLocalPath(startX, startY, endX, endY, opts) {
|
|
2497
|
-
return this.localPathfinder.findPath(startX, startY, endX, endY, opts);
|
|
2498
|
-
}
|
|
2499
|
-
/**
|
|
2500
|
-
* @zh 启发式函数(Octile 距离)
|
|
2501
|
-
* @en Heuristic function (Octile distance)
|
|
2502
|
-
*/
|
|
2503
|
-
heuristic(a, b) {
|
|
2504
|
-
const dx = Math.abs(a.x - b.x);
|
|
2505
|
-
const dy = Math.abs(a.y - b.y);
|
|
2506
|
-
return dx + dy + (Math.SQRT2 - 2) * Math.min(dx, dy);
|
|
2507
1207
|
}
|
|
2508
1208
|
};
|
|
2509
|
-
__name(
|
|
2510
|
-
var
|
|
2511
|
-
function
|
|
2512
|
-
return new
|
|
1209
|
+
__name(_GridMap, "GridMap");
|
|
1210
|
+
var GridMap = _GridMap;
|
|
1211
|
+
function createGridMap(width, height, options) {
|
|
1212
|
+
return new GridMap(width, height, options);
|
|
2513
1213
|
}
|
|
2514
|
-
__name(
|
|
1214
|
+
__name(createGridMap, "createGridMap");
|
|
2515
1215
|
|
|
2516
1216
|
// src/navmesh/NavMesh.ts
|
|
2517
|
-
var
|
|
2518
|
-
var NavMeshNode = (
|
|
1217
|
+
var _a3;
|
|
1218
|
+
var NavMeshNode = (_a3 = class {
|
|
2519
1219
|
constructor(polygon) {
|
|
2520
1220
|
__publicField(this, "id");
|
|
2521
1221
|
__publicField(this, "position");
|
|
@@ -2528,12 +1228,17 @@ var NavMeshNode = (_a5 = class {
|
|
|
2528
1228
|
this.walkable = true;
|
|
2529
1229
|
this.polygon = polygon;
|
|
2530
1230
|
}
|
|
2531
|
-
}, __name(
|
|
1231
|
+
}, __name(_a3, "NavMeshNode"), _a3);
|
|
2532
1232
|
var _NavMesh = class _NavMesh {
|
|
2533
1233
|
constructor() {
|
|
2534
1234
|
__publicField(this, "polygons", /* @__PURE__ */ new Map());
|
|
2535
1235
|
__publicField(this, "nodes", /* @__PURE__ */ new Map());
|
|
2536
1236
|
__publicField(this, "nextId", 0);
|
|
1237
|
+
// @zh 动态障碍物支持
|
|
1238
|
+
// @en Dynamic obstacle support
|
|
1239
|
+
__publicField(this, "obstacles", /* @__PURE__ */ new Map());
|
|
1240
|
+
__publicField(this, "nextObstacleId", 0);
|
|
1241
|
+
__publicField(this, "disabledPolygons", /* @__PURE__ */ new Set());
|
|
2537
1242
|
}
|
|
2538
1243
|
/**
|
|
2539
1244
|
* @zh 添加导航多边形
|
|
@@ -2741,7 +1446,7 @@ var _NavMesh = class _NavMesh {
|
|
|
2741
1446
|
}
|
|
2742
1447
|
const start = createPoint(startX, startY);
|
|
2743
1448
|
const end = createPoint(endX, endY);
|
|
2744
|
-
const pointPath = this.funnelPath(start, end, polygonPath.polygons);
|
|
1449
|
+
const pointPath = this.funnelPath(start, end, polygonPath.polygons, opts.agentRadius);
|
|
2745
1450
|
return {
|
|
2746
1451
|
found: true,
|
|
2747
1452
|
path: pointPath,
|
|
@@ -2752,8 +1457,13 @@ var _NavMesh = class _NavMesh {
|
|
|
2752
1457
|
/**
|
|
2753
1458
|
* @zh 在多边形图上寻路
|
|
2754
1459
|
* @en Find path on polygon graph
|
|
1460
|
+
*
|
|
1461
|
+
* @param start - @zh 起始多边形 @en Start polygon
|
|
1462
|
+
* @param end - @zh 目标多边形 @en End polygon
|
|
1463
|
+
* @param opts - @zh 寻路选项 @en Pathfinding options
|
|
1464
|
+
* @param checkObstacles - @zh 是否检查障碍物 @en Whether to check obstacles
|
|
2755
1465
|
*/
|
|
2756
|
-
findPolygonPath(start, end, opts) {
|
|
1466
|
+
findPolygonPath(start, end, opts, checkObstacles = false) {
|
|
2757
1467
|
const openList = new BinaryHeap((a, b) => a.f - b.f);
|
|
2758
1468
|
const closed = /* @__PURE__ */ new Set();
|
|
2759
1469
|
const states = /* @__PURE__ */ new Map();
|
|
@@ -2787,6 +1497,9 @@ var _NavMesh = class _NavMesh {
|
|
|
2787
1497
|
if (closed.has(neighborId)) {
|
|
2788
1498
|
continue;
|
|
2789
1499
|
}
|
|
1500
|
+
if (checkObstacles && this.isPolygonBlocked(neighborId)) {
|
|
1501
|
+
continue;
|
|
1502
|
+
}
|
|
2790
1503
|
const neighborPolygon = this.polygons.get(neighborId);
|
|
2791
1504
|
if (!neighborPolygon) {
|
|
2792
1505
|
continue;
|
|
@@ -2817,10 +1530,15 @@ var _NavMesh = class _NavMesh {
|
|
|
2817
1530
|
};
|
|
2818
1531
|
}
|
|
2819
1532
|
/**
|
|
2820
|
-
* @zh
|
|
2821
|
-
* @en Optimize path using funnel algorithm
|
|
1533
|
+
* @zh 使用漏斗算法优化路径(支持代理半径)
|
|
1534
|
+
* @en Optimize path using funnel algorithm (supports agent radius)
|
|
1535
|
+
*
|
|
1536
|
+
* @param start - @zh 起点 @en Start point
|
|
1537
|
+
* @param end - @zh 终点 @en End point
|
|
1538
|
+
* @param polygons - @zh 多边形路径 @en Polygon path
|
|
1539
|
+
* @param agentRadius - @zh 代理半径 @en Agent radius
|
|
2822
1540
|
*/
|
|
2823
|
-
funnelPath(start, end, polygons) {
|
|
1541
|
+
funnelPath(start, end, polygons, agentRadius = 0) {
|
|
2824
1542
|
if (polygons.length <= 1) {
|
|
2825
1543
|
return [
|
|
2826
1544
|
start,
|
|
@@ -2831,7 +1549,22 @@ var _NavMesh = class _NavMesh {
|
|
|
2831
1549
|
for (let i = 0; i < polygons.length - 1; i++) {
|
|
2832
1550
|
const portal = polygons[i].portals.get(polygons[i + 1].id);
|
|
2833
1551
|
if (portal) {
|
|
2834
|
-
|
|
1552
|
+
if (agentRadius > 0) {
|
|
1553
|
+
const shrunk = this.shrinkPortal(portal.left, portal.right, agentRadius);
|
|
1554
|
+
portals.push({
|
|
1555
|
+
left: shrunk.left,
|
|
1556
|
+
right: shrunk.right,
|
|
1557
|
+
originalLeft: portal.left,
|
|
1558
|
+
originalRight: portal.right
|
|
1559
|
+
});
|
|
1560
|
+
} else {
|
|
1561
|
+
portals.push({
|
|
1562
|
+
left: portal.left,
|
|
1563
|
+
right: portal.right,
|
|
1564
|
+
originalLeft: portal.left,
|
|
1565
|
+
originalRight: portal.right
|
|
1566
|
+
});
|
|
1567
|
+
}
|
|
2835
1568
|
}
|
|
2836
1569
|
}
|
|
2837
1570
|
if (portals.length === 0) {
|
|
@@ -2844,35 +1577,50 @@ var _NavMesh = class _NavMesh {
|
|
|
2844
1577
|
start
|
|
2845
1578
|
];
|
|
2846
1579
|
let apex = start;
|
|
1580
|
+
let apexOriginal = start;
|
|
2847
1581
|
let leftIndex = 0;
|
|
2848
1582
|
let rightIndex = 0;
|
|
2849
1583
|
let left = portals[0].left;
|
|
2850
1584
|
let right = portals[0].right;
|
|
1585
|
+
let leftOriginal = portals[0].originalLeft;
|
|
1586
|
+
let rightOriginal = portals[0].originalRight;
|
|
2851
1587
|
for (let i = 1; i <= portals.length; i++) {
|
|
2852
1588
|
const nextLeft = i < portals.length ? portals[i].left : end;
|
|
2853
1589
|
const nextRight = i < portals.length ? portals[i].right : end;
|
|
2854
1590
|
if (this.triArea2(apex, right, nextRight) <= 0) {
|
|
2855
|
-
if (apex
|
|
1591
|
+
if (this.pointsEqual(apex, right) || this.triArea2(apex, left, nextRight) > 0) {
|
|
2856
1592
|
right = nextRight;
|
|
2857
1593
|
rightIndex = i;
|
|
1594
|
+
if (i < portals.length) {
|
|
1595
|
+
rightOriginal = portals[i].originalRight;
|
|
1596
|
+
}
|
|
2858
1597
|
} else {
|
|
2859
|
-
|
|
1598
|
+
const turnPoint = agentRadius > 0 ? this.offsetTurningPoint(apexOriginal, leftOriginal, left, agentRadius, "left") : left;
|
|
1599
|
+
path.push(turnPoint);
|
|
2860
1600
|
apex = left;
|
|
1601
|
+
apexOriginal = leftOriginal;
|
|
2861
1602
|
leftIndex = rightIndex = leftIndex;
|
|
2862
1603
|
left = right = apex;
|
|
1604
|
+
leftOriginal = rightOriginal = apexOriginal;
|
|
2863
1605
|
i = leftIndex;
|
|
2864
1606
|
continue;
|
|
2865
1607
|
}
|
|
2866
1608
|
}
|
|
2867
1609
|
if (this.triArea2(apex, left, nextLeft) >= 0) {
|
|
2868
|
-
if (apex
|
|
1610
|
+
if (this.pointsEqual(apex, left) || this.triArea2(apex, right, nextLeft) < 0) {
|
|
2869
1611
|
left = nextLeft;
|
|
2870
1612
|
leftIndex = i;
|
|
1613
|
+
if (i < portals.length) {
|
|
1614
|
+
leftOriginal = portals[i].originalLeft;
|
|
1615
|
+
}
|
|
2871
1616
|
} else {
|
|
2872
|
-
|
|
1617
|
+
const turnPoint = agentRadius > 0 ? this.offsetTurningPoint(apexOriginal, rightOriginal, right, agentRadius, "right") : right;
|
|
1618
|
+
path.push(turnPoint);
|
|
2873
1619
|
apex = right;
|
|
1620
|
+
apexOriginal = rightOriginal;
|
|
2874
1621
|
leftIndex = rightIndex = rightIndex;
|
|
2875
1622
|
left = right = apex;
|
|
1623
|
+
leftOriginal = rightOriginal = apexOriginal;
|
|
2876
1624
|
i = rightIndex;
|
|
2877
1625
|
continue;
|
|
2878
1626
|
}
|
|
@@ -2881,6 +1629,63 @@ var _NavMesh = class _NavMesh {
|
|
|
2881
1629
|
path.push(end);
|
|
2882
1630
|
return path;
|
|
2883
1631
|
}
|
|
1632
|
+
/**
|
|
1633
|
+
* @zh 收缩 portal(将两端点向内移动 agentRadius)
|
|
1634
|
+
* @en Shrink portal (move endpoints inward by agentRadius)
|
|
1635
|
+
*/
|
|
1636
|
+
shrinkPortal(left, right, radius) {
|
|
1637
|
+
const dx = right.x - left.x;
|
|
1638
|
+
const dy = right.y - left.y;
|
|
1639
|
+
const len = Math.sqrt(dx * dx + dy * dy);
|
|
1640
|
+
if (len <= radius * 2) {
|
|
1641
|
+
const cx = (left.x + right.x) / 2;
|
|
1642
|
+
const cy = (left.y + right.y) / 2;
|
|
1643
|
+
return {
|
|
1644
|
+
left: createPoint(cx, cy),
|
|
1645
|
+
right: createPoint(cx, cy)
|
|
1646
|
+
};
|
|
1647
|
+
}
|
|
1648
|
+
const nx = dx / len;
|
|
1649
|
+
const ny = dy / len;
|
|
1650
|
+
return {
|
|
1651
|
+
left: createPoint(left.x + nx * radius, left.y + ny * radius),
|
|
1652
|
+
right: createPoint(right.x - nx * radius, right.y - ny * radius)
|
|
1653
|
+
};
|
|
1654
|
+
}
|
|
1655
|
+
/**
|
|
1656
|
+
* @zh 偏移拐点以保持与角落的距离
|
|
1657
|
+
* @en Offset turning point to maintain distance from corner
|
|
1658
|
+
*
|
|
1659
|
+
* @param prevApex - @zh 上一个顶点 @en Previous apex
|
|
1660
|
+
* @param cornerOriginal - @zh 原始角落位置 @en Original corner position
|
|
1661
|
+
* @param cornerShrunk - @zh 收缩后的角落位置 @en Shrunk corner position
|
|
1662
|
+
* @param radius - @zh 代理半径 @en Agent radius
|
|
1663
|
+
* @param side - @zh 转向侧 ('left' 或 'right') @en Turn side ('left' or 'right')
|
|
1664
|
+
*/
|
|
1665
|
+
offsetTurningPoint(prevApex, cornerOriginal, cornerShrunk, radius, side) {
|
|
1666
|
+
const dx = cornerOriginal.x - prevApex.x;
|
|
1667
|
+
const dy = cornerOriginal.y - prevApex.y;
|
|
1668
|
+
const len = Math.sqrt(dx * dx + dy * dy);
|
|
1669
|
+
if (len < 1e-4) {
|
|
1670
|
+
return cornerShrunk;
|
|
1671
|
+
}
|
|
1672
|
+
let perpX, perpY;
|
|
1673
|
+
if (side === "left") {
|
|
1674
|
+
perpX = dy / len;
|
|
1675
|
+
perpY = -dx / len;
|
|
1676
|
+
} else {
|
|
1677
|
+
perpX = -dy / len;
|
|
1678
|
+
perpY = dx / len;
|
|
1679
|
+
}
|
|
1680
|
+
return createPoint(cornerShrunk.x + perpX * radius, cornerShrunk.y + perpY * radius);
|
|
1681
|
+
}
|
|
1682
|
+
/**
|
|
1683
|
+
* @zh 检查两点是否相等
|
|
1684
|
+
* @en Check if two points are equal
|
|
1685
|
+
*/
|
|
1686
|
+
pointsEqual(a, b) {
|
|
1687
|
+
return Math.abs(a.x - b.x) < 1e-4 && Math.abs(a.y - b.y) < 1e-4;
|
|
1688
|
+
}
|
|
2884
1689
|
/**
|
|
2885
1690
|
* @zh 计算三角形面积的两倍(用于判断点的相对位置)
|
|
2886
1691
|
* @en Calculate twice the triangle area (for point relative position)
|
|
@@ -2899,6 +1704,416 @@ var _NavMesh = class _NavMesh {
|
|
|
2899
1704
|
}
|
|
2900
1705
|
return length;
|
|
2901
1706
|
}
|
|
1707
|
+
// =========================================================================
|
|
1708
|
+
// 动态障碍物管理 | Dynamic Obstacle Management
|
|
1709
|
+
// =========================================================================
|
|
1710
|
+
/**
|
|
1711
|
+
* @zh 添加圆形障碍物
|
|
1712
|
+
* @en Add circular obstacle
|
|
1713
|
+
*
|
|
1714
|
+
* @param x - @zh 中心 X @en Center X
|
|
1715
|
+
* @param y - @zh 中心 Y @en Center Y
|
|
1716
|
+
* @param radius - @zh 半径 @en Radius
|
|
1717
|
+
* @returns @zh 障碍物 ID @en Obstacle ID
|
|
1718
|
+
*/
|
|
1719
|
+
addCircleObstacle(x, y, radius) {
|
|
1720
|
+
const id = this.nextObstacleId++;
|
|
1721
|
+
this.obstacles.set(id, {
|
|
1722
|
+
id,
|
|
1723
|
+
type: "circle",
|
|
1724
|
+
enabled: true,
|
|
1725
|
+
position: createPoint(x, y),
|
|
1726
|
+
radius
|
|
1727
|
+
});
|
|
1728
|
+
return id;
|
|
1729
|
+
}
|
|
1730
|
+
/**
|
|
1731
|
+
* @zh 添加矩形障碍物
|
|
1732
|
+
* @en Add rectangular obstacle
|
|
1733
|
+
*
|
|
1734
|
+
* @param x - @zh 中心 X @en Center X
|
|
1735
|
+
* @param y - @zh 中心 Y @en Center Y
|
|
1736
|
+
* @param halfWidth - @zh 半宽 @en Half width
|
|
1737
|
+
* @param halfHeight - @zh 半高 @en Half height
|
|
1738
|
+
* @returns @zh 障碍物 ID @en Obstacle ID
|
|
1739
|
+
*/
|
|
1740
|
+
addRectObstacle(x, y, halfWidth, halfHeight) {
|
|
1741
|
+
const id = this.nextObstacleId++;
|
|
1742
|
+
this.obstacles.set(id, {
|
|
1743
|
+
id,
|
|
1744
|
+
type: "rect",
|
|
1745
|
+
enabled: true,
|
|
1746
|
+
position: createPoint(x, y),
|
|
1747
|
+
halfWidth,
|
|
1748
|
+
halfHeight
|
|
1749
|
+
});
|
|
1750
|
+
return id;
|
|
1751
|
+
}
|
|
1752
|
+
/**
|
|
1753
|
+
* @zh 添加多边形障碍物
|
|
1754
|
+
* @en Add polygon obstacle
|
|
1755
|
+
*
|
|
1756
|
+
* @param vertices - @zh 顶点列表 @en Vertex list
|
|
1757
|
+
* @returns @zh 障碍物 ID @en Obstacle ID
|
|
1758
|
+
*/
|
|
1759
|
+
addPolygonObstacle(vertices) {
|
|
1760
|
+
const id = this.nextObstacleId++;
|
|
1761
|
+
const center = this.calculateCenter(vertices);
|
|
1762
|
+
this.obstacles.set(id, {
|
|
1763
|
+
id,
|
|
1764
|
+
type: "polygon",
|
|
1765
|
+
enabled: true,
|
|
1766
|
+
position: center,
|
|
1767
|
+
vertices
|
|
1768
|
+
});
|
|
1769
|
+
return id;
|
|
1770
|
+
}
|
|
1771
|
+
/**
|
|
1772
|
+
* @zh 移除障碍物
|
|
1773
|
+
* @en Remove obstacle
|
|
1774
|
+
*/
|
|
1775
|
+
removeObstacle(obstacleId) {
|
|
1776
|
+
return this.obstacles.delete(obstacleId);
|
|
1777
|
+
}
|
|
1778
|
+
/**
|
|
1779
|
+
* @zh 启用/禁用障碍物
|
|
1780
|
+
* @en Enable/disable obstacle
|
|
1781
|
+
*/
|
|
1782
|
+
setObstacleEnabled(obstacleId, enabled) {
|
|
1783
|
+
const obstacle = this.obstacles.get(obstacleId);
|
|
1784
|
+
if (obstacle) {
|
|
1785
|
+
obstacle.enabled = enabled;
|
|
1786
|
+
}
|
|
1787
|
+
}
|
|
1788
|
+
/**
|
|
1789
|
+
* @zh 更新障碍物位置
|
|
1790
|
+
* @en Update obstacle position
|
|
1791
|
+
*/
|
|
1792
|
+
updateObstaclePosition(obstacleId, x, y) {
|
|
1793
|
+
const obstacle = this.obstacles.get(obstacleId);
|
|
1794
|
+
if (obstacle) {
|
|
1795
|
+
obstacle.position = createPoint(x, y);
|
|
1796
|
+
}
|
|
1797
|
+
}
|
|
1798
|
+
/**
|
|
1799
|
+
* @zh 获取所有障碍物
|
|
1800
|
+
* @en Get all obstacles
|
|
1801
|
+
*/
|
|
1802
|
+
getObstacles() {
|
|
1803
|
+
return Array.from(this.obstacles.values());
|
|
1804
|
+
}
|
|
1805
|
+
/**
|
|
1806
|
+
* @zh 获取启用的障碍物
|
|
1807
|
+
* @en Get enabled obstacles
|
|
1808
|
+
*/
|
|
1809
|
+
getEnabledObstacles() {
|
|
1810
|
+
return Array.from(this.obstacles.values()).filter((o) => o.enabled);
|
|
1811
|
+
}
|
|
1812
|
+
/**
|
|
1813
|
+
* @zh 清除所有障碍物
|
|
1814
|
+
* @en Clear all obstacles
|
|
1815
|
+
*/
|
|
1816
|
+
clearObstacles() {
|
|
1817
|
+
this.obstacles.clear();
|
|
1818
|
+
this.nextObstacleId = 0;
|
|
1819
|
+
}
|
|
1820
|
+
// =========================================================================
|
|
1821
|
+
// 多边形禁用管理 | Polygon Disable Management
|
|
1822
|
+
// =========================================================================
|
|
1823
|
+
/**
|
|
1824
|
+
* @zh 禁用多边形
|
|
1825
|
+
* @en Disable polygon
|
|
1826
|
+
*/
|
|
1827
|
+
disablePolygon(polygonId) {
|
|
1828
|
+
this.disabledPolygons.add(polygonId);
|
|
1829
|
+
}
|
|
1830
|
+
/**
|
|
1831
|
+
* @zh 启用多边形
|
|
1832
|
+
* @en Enable polygon
|
|
1833
|
+
*/
|
|
1834
|
+
enablePolygon(polygonId) {
|
|
1835
|
+
this.disabledPolygons.delete(polygonId);
|
|
1836
|
+
}
|
|
1837
|
+
/**
|
|
1838
|
+
* @zh 检查多边形是否被禁用
|
|
1839
|
+
* @en Check if polygon is disabled
|
|
1840
|
+
*/
|
|
1841
|
+
isPolygonDisabled(polygonId) {
|
|
1842
|
+
return this.disabledPolygons.has(polygonId);
|
|
1843
|
+
}
|
|
1844
|
+
/**
|
|
1845
|
+
* @zh 禁用包含指定点的多边形
|
|
1846
|
+
* @en Disable polygon containing specified point
|
|
1847
|
+
*/
|
|
1848
|
+
disablePolygonAt(x, y) {
|
|
1849
|
+
const polygon = this.findPolygonAt(x, y);
|
|
1850
|
+
if (polygon) {
|
|
1851
|
+
this.disablePolygon(polygon.id);
|
|
1852
|
+
return polygon.id;
|
|
1853
|
+
}
|
|
1854
|
+
return null;
|
|
1855
|
+
}
|
|
1856
|
+
/**
|
|
1857
|
+
* @zh 清除所有禁用的多边形
|
|
1858
|
+
* @en Clear all disabled polygons
|
|
1859
|
+
*/
|
|
1860
|
+
clearDisabledPolygons() {
|
|
1861
|
+
this.disabledPolygons.clear();
|
|
1862
|
+
}
|
|
1863
|
+
/**
|
|
1864
|
+
* @zh 获取被禁用的多边形 ID 列表
|
|
1865
|
+
* @en Get list of disabled polygon IDs
|
|
1866
|
+
*/
|
|
1867
|
+
getDisabledPolygons() {
|
|
1868
|
+
return Array.from(this.disabledPolygons);
|
|
1869
|
+
}
|
|
1870
|
+
// =========================================================================
|
|
1871
|
+
// 障碍物碰撞检测 | Obstacle Collision Detection
|
|
1872
|
+
// =========================================================================
|
|
1873
|
+
/**
|
|
1874
|
+
* @zh 检查点是否在任何障碍物内
|
|
1875
|
+
* @en Check if point is inside any obstacle
|
|
1876
|
+
*/
|
|
1877
|
+
isPointInObstacle(x, y) {
|
|
1878
|
+
for (const obstacle of this.obstacles.values()) {
|
|
1879
|
+
if (!obstacle.enabled) continue;
|
|
1880
|
+
if (this.isPointInSingleObstacle(x, y, obstacle)) {
|
|
1881
|
+
return true;
|
|
1882
|
+
}
|
|
1883
|
+
}
|
|
1884
|
+
return false;
|
|
1885
|
+
}
|
|
1886
|
+
/**
|
|
1887
|
+
* @zh 检查点是否在单个障碍物内
|
|
1888
|
+
* @en Check if point is inside single obstacle
|
|
1889
|
+
*/
|
|
1890
|
+
isPointInSingleObstacle(x, y, obstacle) {
|
|
1891
|
+
switch (obstacle.type) {
|
|
1892
|
+
case "circle": {
|
|
1893
|
+
const dx = x - obstacle.position.x;
|
|
1894
|
+
const dy = y - obstacle.position.y;
|
|
1895
|
+
return dx * dx + dy * dy <= (obstacle.radius ?? 0) ** 2;
|
|
1896
|
+
}
|
|
1897
|
+
case "rect": {
|
|
1898
|
+
const hw = obstacle.halfWidth ?? 0;
|
|
1899
|
+
const hh = obstacle.halfHeight ?? 0;
|
|
1900
|
+
return Math.abs(x - obstacle.position.x) <= hw && Math.abs(y - obstacle.position.y) <= hh;
|
|
1901
|
+
}
|
|
1902
|
+
case "polygon": {
|
|
1903
|
+
if (!obstacle.vertices) return false;
|
|
1904
|
+
return this.isPointInPolygon(x, y, obstacle.vertices);
|
|
1905
|
+
}
|
|
1906
|
+
default:
|
|
1907
|
+
return false;
|
|
1908
|
+
}
|
|
1909
|
+
}
|
|
1910
|
+
/**
|
|
1911
|
+
* @zh 检查线段是否与任何障碍物相交
|
|
1912
|
+
* @en Check if line segment intersects any obstacle
|
|
1913
|
+
*/
|
|
1914
|
+
doesLineIntersectObstacle(x1, y1, x2, y2) {
|
|
1915
|
+
for (const obstacle of this.obstacles.values()) {
|
|
1916
|
+
if (!obstacle.enabled) continue;
|
|
1917
|
+
if (this.doesLineIntersectSingleObstacle(x1, y1, x2, y2, obstacle)) {
|
|
1918
|
+
return true;
|
|
1919
|
+
}
|
|
1920
|
+
}
|
|
1921
|
+
return false;
|
|
1922
|
+
}
|
|
1923
|
+
/**
|
|
1924
|
+
* @zh 检查线段是否与单个障碍物相交
|
|
1925
|
+
* @en Check if line segment intersects single obstacle
|
|
1926
|
+
*/
|
|
1927
|
+
doesLineIntersectSingleObstacle(x1, y1, x2, y2, obstacle) {
|
|
1928
|
+
switch (obstacle.type) {
|
|
1929
|
+
case "circle": {
|
|
1930
|
+
return this.lineIntersectsCircle(x1, y1, x2, y2, obstacle.position.x, obstacle.position.y, obstacle.radius ?? 0);
|
|
1931
|
+
}
|
|
1932
|
+
case "rect": {
|
|
1933
|
+
const hw = obstacle.halfWidth ?? 0;
|
|
1934
|
+
const hh = obstacle.halfHeight ?? 0;
|
|
1935
|
+
const minX = obstacle.position.x - hw;
|
|
1936
|
+
const maxX = obstacle.position.x + hw;
|
|
1937
|
+
const minY = obstacle.position.y - hh;
|
|
1938
|
+
const maxY = obstacle.position.y + hh;
|
|
1939
|
+
return this.lineIntersectsRect(x1, y1, x2, y2, minX, minY, maxX, maxY);
|
|
1940
|
+
}
|
|
1941
|
+
case "polygon": {
|
|
1942
|
+
if (!obstacle.vertices) return false;
|
|
1943
|
+
return this.lineIntersectsPolygon(x1, y1, x2, y2, obstacle.vertices);
|
|
1944
|
+
}
|
|
1945
|
+
default:
|
|
1946
|
+
return false;
|
|
1947
|
+
}
|
|
1948
|
+
}
|
|
1949
|
+
/**
|
|
1950
|
+
* @zh 线段与圆相交检测
|
|
1951
|
+
* @en Line segment circle intersection
|
|
1952
|
+
*/
|
|
1953
|
+
lineIntersectsCircle(x1, y1, x2, y2, cx, cy, r) {
|
|
1954
|
+
const dx = x2 - x1;
|
|
1955
|
+
const dy = y2 - y1;
|
|
1956
|
+
const fx = x1 - cx;
|
|
1957
|
+
const fy = y1 - cy;
|
|
1958
|
+
const a = dx * dx + dy * dy;
|
|
1959
|
+
const b = 2 * (fx * dx + fy * dy);
|
|
1960
|
+
const c = fx * fx + fy * fy - r * r;
|
|
1961
|
+
let discriminant = b * b - 4 * a * c;
|
|
1962
|
+
if (discriminant < 0) return false;
|
|
1963
|
+
discriminant = Math.sqrt(discriminant);
|
|
1964
|
+
const t1 = (-b - discriminant) / (2 * a);
|
|
1965
|
+
const t2 = (-b + discriminant) / (2 * a);
|
|
1966
|
+
return t1 >= 0 && t1 <= 1 || t2 >= 0 && t2 <= 1 || t1 < 0 && t2 > 1;
|
|
1967
|
+
}
|
|
1968
|
+
/**
|
|
1969
|
+
* @zh 线段与矩形相交检测
|
|
1970
|
+
* @en Line segment rectangle intersection
|
|
1971
|
+
*/
|
|
1972
|
+
lineIntersectsRect(x1, y1, x2, y2, minX, minY, maxX, maxY) {
|
|
1973
|
+
if (x1 >= minX && x1 <= maxX && y1 >= minY && y1 <= maxY || x2 >= minX && x2 <= maxX && y2 >= minY && y2 <= maxY) {
|
|
1974
|
+
return true;
|
|
1975
|
+
}
|
|
1976
|
+
return this.lineSegmentsIntersect(x1, y1, x2, y2, minX, minY, maxX, minY) || this.lineSegmentsIntersect(x1, y1, x2, y2, maxX, minY, maxX, maxY) || this.lineSegmentsIntersect(x1, y1, x2, y2, maxX, maxY, minX, maxY) || this.lineSegmentsIntersect(x1, y1, x2, y2, minX, maxY, minX, minY);
|
|
1977
|
+
}
|
|
1978
|
+
/**
|
|
1979
|
+
* @zh 线段与多边形相交检测
|
|
1980
|
+
* @en Line segment polygon intersection
|
|
1981
|
+
*/
|
|
1982
|
+
lineIntersectsPolygon(x1, y1, x2, y2, vertices) {
|
|
1983
|
+
if (this.isPointInPolygon(x1, y1, vertices) || this.isPointInPolygon(x2, y2, vertices)) {
|
|
1984
|
+
return true;
|
|
1985
|
+
}
|
|
1986
|
+
for (let i = 0; i < vertices.length; i++) {
|
|
1987
|
+
const j = (i + 1) % vertices.length;
|
|
1988
|
+
if (this.lineSegmentsIntersect(x1, y1, x2, y2, vertices[i].x, vertices[i].y, vertices[j].x, vertices[j].y)) {
|
|
1989
|
+
return true;
|
|
1990
|
+
}
|
|
1991
|
+
}
|
|
1992
|
+
return false;
|
|
1993
|
+
}
|
|
1994
|
+
/**
|
|
1995
|
+
* @zh 两线段相交检测
|
|
1996
|
+
* @en Two line segments intersection
|
|
1997
|
+
*/
|
|
1998
|
+
lineSegmentsIntersect(x1, y1, x2, y2, x3, y3, x4, y4) {
|
|
1999
|
+
const d1 = this.direction(x3, y3, x4, y4, x1, y1);
|
|
2000
|
+
const d2 = this.direction(x3, y3, x4, y4, x2, y2);
|
|
2001
|
+
const d3 = this.direction(x1, y1, x2, y2, x3, y3);
|
|
2002
|
+
const d4 = this.direction(x1, y1, x2, y2, x4, y4);
|
|
2003
|
+
if ((d1 > 0 && d2 < 0 || d1 < 0 && d2 > 0) && (d3 > 0 && d4 < 0 || d3 < 0 && d4 > 0)) {
|
|
2004
|
+
return true;
|
|
2005
|
+
}
|
|
2006
|
+
const epsilon = 1e-4;
|
|
2007
|
+
if (Math.abs(d1) < epsilon && this.onSegment(x3, y3, x4, y4, x1, y1)) return true;
|
|
2008
|
+
if (Math.abs(d2) < epsilon && this.onSegment(x3, y3, x4, y4, x2, y2)) return true;
|
|
2009
|
+
if (Math.abs(d3) < epsilon && this.onSegment(x1, y1, x2, y2, x3, y3)) return true;
|
|
2010
|
+
if (Math.abs(d4) < epsilon && this.onSegment(x1, y1, x2, y2, x4, y4)) return true;
|
|
2011
|
+
return false;
|
|
2012
|
+
}
|
|
2013
|
+
direction(x1, y1, x2, y2, x3, y3) {
|
|
2014
|
+
return (x3 - x1) * (y2 - y1) - (y3 - y1) * (x2 - x1);
|
|
2015
|
+
}
|
|
2016
|
+
onSegment(x1, y1, x2, y2, x3, y3) {
|
|
2017
|
+
return Math.min(x1, x2) <= x3 && x3 <= Math.max(x1, x2) && Math.min(y1, y2) <= y3 && y3 <= Math.max(y1, y2);
|
|
2018
|
+
}
|
|
2019
|
+
// =========================================================================
|
|
2020
|
+
// 障碍物感知寻路 | Obstacle-Aware Pathfinding
|
|
2021
|
+
// =========================================================================
|
|
2022
|
+
/**
|
|
2023
|
+
* @zh 检查多边形是否被障碍物阻挡
|
|
2024
|
+
* @en Check if polygon is blocked by obstacle
|
|
2025
|
+
*
|
|
2026
|
+
* @zh 检查以下条件:
|
|
2027
|
+
* @en Checks the following conditions:
|
|
2028
|
+
* - @zh 多边形是否被禁用 @en Whether polygon is disabled
|
|
2029
|
+
* - @zh 多边形中心是否在障碍物内 @en Whether polygon center is inside obstacle
|
|
2030
|
+
* - @zh 多边形任意顶点是否在障碍物内 @en Whether any polygon vertex is inside obstacle
|
|
2031
|
+
* - @zh 多边形任意边是否与障碍物相交 @en Whether any polygon edge intersects obstacle
|
|
2032
|
+
*/
|
|
2033
|
+
isPolygonBlocked(polygonId) {
|
|
2034
|
+
if (this.disabledPolygons.has(polygonId)) {
|
|
2035
|
+
return true;
|
|
2036
|
+
}
|
|
2037
|
+
const polygon = this.polygons.get(polygonId);
|
|
2038
|
+
if (!polygon) return false;
|
|
2039
|
+
if (this.isPointInObstacle(polygon.center.x, polygon.center.y)) {
|
|
2040
|
+
return true;
|
|
2041
|
+
}
|
|
2042
|
+
for (const vertex of polygon.vertices) {
|
|
2043
|
+
if (this.isPointInObstacle(vertex.x, vertex.y)) {
|
|
2044
|
+
return true;
|
|
2045
|
+
}
|
|
2046
|
+
}
|
|
2047
|
+
const vertices = polygon.vertices;
|
|
2048
|
+
for (let i = 0; i < vertices.length; i++) {
|
|
2049
|
+
const v1 = vertices[i];
|
|
2050
|
+
const v2 = vertices[(i + 1) % vertices.length];
|
|
2051
|
+
if (this.doesLineIntersectObstacle(v1.x, v1.y, v2.x, v2.y)) {
|
|
2052
|
+
return true;
|
|
2053
|
+
}
|
|
2054
|
+
}
|
|
2055
|
+
return false;
|
|
2056
|
+
}
|
|
2057
|
+
/**
|
|
2058
|
+
* @zh 在导航网格上寻路(考虑障碍物)
|
|
2059
|
+
* @en Find path on navigation mesh (considering obstacles)
|
|
2060
|
+
*
|
|
2061
|
+
* @zh 此方法在规划阶段就考虑障碍物,自动绕过被阻挡的多边形
|
|
2062
|
+
* @en This method considers obstacles during planning, automatically avoiding blocked polygons
|
|
2063
|
+
*
|
|
2064
|
+
* @zh 与 findPath 不同,此方法会:
|
|
2065
|
+
* @en Unlike findPath, this method will:
|
|
2066
|
+
* - @zh 在 A* 搜索中跳过被障碍物阻挡的多边形
|
|
2067
|
+
* - @en Skip obstacle-blocked polygons during A* search
|
|
2068
|
+
* - @zh 验证起点和终点不在障碍物内
|
|
2069
|
+
* - @en Verify start and end points are not inside obstacles
|
|
2070
|
+
*/
|
|
2071
|
+
findPathWithObstacles(startX, startY, endX, endY, options) {
|
|
2072
|
+
const opts = {
|
|
2073
|
+
...DEFAULT_PATHFINDING_OPTIONS,
|
|
2074
|
+
...options
|
|
2075
|
+
};
|
|
2076
|
+
if (this.isPointInObstacle(startX, startY) || this.isPointInObstacle(endX, endY)) {
|
|
2077
|
+
return EMPTY_PATH_RESULT;
|
|
2078
|
+
}
|
|
2079
|
+
const startPolygon = this.findPolygonAt(startX, startY);
|
|
2080
|
+
const endPolygon = this.findPolygonAt(endX, endY);
|
|
2081
|
+
if (!startPolygon || !endPolygon) {
|
|
2082
|
+
return EMPTY_PATH_RESULT;
|
|
2083
|
+
}
|
|
2084
|
+
if (this.isPolygonBlocked(startPolygon.id) || this.isPolygonBlocked(endPolygon.id)) {
|
|
2085
|
+
return EMPTY_PATH_RESULT;
|
|
2086
|
+
}
|
|
2087
|
+
if (startPolygon.id === endPolygon.id) {
|
|
2088
|
+
const start2 = createPoint(startX, startY);
|
|
2089
|
+
const end2 = createPoint(endX, endY);
|
|
2090
|
+
if (this.doesLineIntersectObstacle(startX, startY, endX, endY)) {
|
|
2091
|
+
return EMPTY_PATH_RESULT;
|
|
2092
|
+
}
|
|
2093
|
+
return {
|
|
2094
|
+
found: true,
|
|
2095
|
+
path: [
|
|
2096
|
+
start2,
|
|
2097
|
+
end2
|
|
2098
|
+
],
|
|
2099
|
+
cost: euclideanDistance(start2, end2),
|
|
2100
|
+
nodesSearched: 1
|
|
2101
|
+
};
|
|
2102
|
+
}
|
|
2103
|
+
const polygonPath = this.findPolygonPath(startPolygon, endPolygon, opts, true);
|
|
2104
|
+
if (!polygonPath.found) {
|
|
2105
|
+
return EMPTY_PATH_RESULT;
|
|
2106
|
+
}
|
|
2107
|
+
const start = createPoint(startX, startY);
|
|
2108
|
+
const end = createPoint(endX, endY);
|
|
2109
|
+
const pointPath = this.funnelPath(start, end, polygonPath.polygons, opts.agentRadius);
|
|
2110
|
+
return {
|
|
2111
|
+
found: true,
|
|
2112
|
+
path: pointPath,
|
|
2113
|
+
cost: this.calculatePathLength(pointPath),
|
|
2114
|
+
nodesSearched: polygonPath.nodesSearched
|
|
2115
|
+
};
|
|
2116
|
+
}
|
|
2902
2117
|
/**
|
|
2903
2118
|
* @zh 清空导航网格
|
|
2904
2119
|
* @en Clear navigation mesh
|
|
@@ -2906,7 +2121,10 @@ var _NavMesh = class _NavMesh {
|
|
|
2906
2121
|
clear() {
|
|
2907
2122
|
this.polygons.clear();
|
|
2908
2123
|
this.nodes.clear();
|
|
2124
|
+
this.obstacles.clear();
|
|
2125
|
+
this.disabledPolygons.clear();
|
|
2909
2126
|
this.nextId = 0;
|
|
2127
|
+
this.nextObstacleId = 0;
|
|
2910
2128
|
}
|
|
2911
2129
|
/**
|
|
2912
2130
|
* @zh 获取所有多边形
|
|
@@ -2922,6 +2140,13 @@ var _NavMesh = class _NavMesh {
|
|
|
2922
2140
|
get polygonCount() {
|
|
2923
2141
|
return this.polygons.size;
|
|
2924
2142
|
}
|
|
2143
|
+
/**
|
|
2144
|
+
* @zh 获取障碍物数量
|
|
2145
|
+
* @en Get obstacle count
|
|
2146
|
+
*/
|
|
2147
|
+
get obstacleCount() {
|
|
2148
|
+
return this.obstacles.size;
|
|
2149
|
+
}
|
|
2925
2150
|
};
|
|
2926
2151
|
__name(_NavMesh, "NavMesh");
|
|
2927
2152
|
var NavMesh = _NavMesh;
|
|
@@ -2929,56 +2154,431 @@ function createNavMesh() {
|
|
|
2929
2154
|
return new NavMesh();
|
|
2930
2155
|
}
|
|
2931
2156
|
__name(createNavMesh, "createNavMesh");
|
|
2157
|
+
|
|
2158
|
+
// src/smoothing/PathSmoother.ts
|
|
2159
|
+
function bresenhamLineOfSight(x1, y1, x2, y2, map) {
|
|
2160
|
+
let ix1 = Math.floor(x1);
|
|
2161
|
+
let iy1 = Math.floor(y1);
|
|
2162
|
+
const ix2 = Math.floor(x2);
|
|
2163
|
+
const iy2 = Math.floor(y2);
|
|
2164
|
+
const dx = Math.abs(ix2 - ix1);
|
|
2165
|
+
const dy = Math.abs(iy2 - iy1);
|
|
2166
|
+
const sx = ix1 < ix2 ? 1 : -1;
|
|
2167
|
+
const sy = iy1 < iy2 ? 1 : -1;
|
|
2168
|
+
let err = dx - dy;
|
|
2169
|
+
while (true) {
|
|
2170
|
+
if (!map.isWalkable(ix1, iy1)) {
|
|
2171
|
+
return false;
|
|
2172
|
+
}
|
|
2173
|
+
if (ix1 === ix2 && iy1 === iy2) {
|
|
2174
|
+
break;
|
|
2175
|
+
}
|
|
2176
|
+
const e2 = 2 * err;
|
|
2177
|
+
if (e2 > -dy) {
|
|
2178
|
+
err -= dy;
|
|
2179
|
+
ix1 += sx;
|
|
2180
|
+
}
|
|
2181
|
+
if (e2 < dx) {
|
|
2182
|
+
err += dx;
|
|
2183
|
+
iy1 += sy;
|
|
2184
|
+
}
|
|
2185
|
+
}
|
|
2186
|
+
return true;
|
|
2187
|
+
}
|
|
2188
|
+
__name(bresenhamLineOfSight, "bresenhamLineOfSight");
|
|
2189
|
+
function raycastLineOfSight(x1, y1, x2, y2, map, stepSize = 0.5) {
|
|
2190
|
+
const dx = x2 - x1;
|
|
2191
|
+
const dy = y2 - y1;
|
|
2192
|
+
const distance = Math.sqrt(dx * dx + dy * dy);
|
|
2193
|
+
if (distance === 0) {
|
|
2194
|
+
return map.isWalkable(Math.floor(x1), Math.floor(y1));
|
|
2195
|
+
}
|
|
2196
|
+
const steps = Math.ceil(distance / stepSize);
|
|
2197
|
+
const stepX = dx / steps;
|
|
2198
|
+
const stepY = dy / steps;
|
|
2199
|
+
let x = x1;
|
|
2200
|
+
let y = y1;
|
|
2201
|
+
for (let i = 0; i <= steps; i++) {
|
|
2202
|
+
if (!map.isWalkable(Math.floor(x), Math.floor(y))) {
|
|
2203
|
+
return false;
|
|
2204
|
+
}
|
|
2205
|
+
x += stepX;
|
|
2206
|
+
y += stepY;
|
|
2207
|
+
}
|
|
2208
|
+
return true;
|
|
2209
|
+
}
|
|
2210
|
+
__name(raycastLineOfSight, "raycastLineOfSight");
|
|
2211
|
+
var _LineOfSightSmoother = class _LineOfSightSmoother {
|
|
2212
|
+
constructor(lineOfSight = bresenhamLineOfSight) {
|
|
2213
|
+
__publicField(this, "lineOfSight");
|
|
2214
|
+
this.lineOfSight = lineOfSight;
|
|
2215
|
+
}
|
|
2216
|
+
smooth(path, map) {
|
|
2217
|
+
if (path.length <= 2) {
|
|
2218
|
+
return [
|
|
2219
|
+
...path
|
|
2220
|
+
];
|
|
2221
|
+
}
|
|
2222
|
+
const result = [
|
|
2223
|
+
path[0]
|
|
2224
|
+
];
|
|
2225
|
+
let current = 0;
|
|
2226
|
+
while (current < path.length - 1) {
|
|
2227
|
+
let furthest = current + 1;
|
|
2228
|
+
for (let i = path.length - 1; i > current + 1; i--) {
|
|
2229
|
+
if (this.lineOfSight(path[current].x, path[current].y, path[i].x, path[i].y, map)) {
|
|
2230
|
+
furthest = i;
|
|
2231
|
+
break;
|
|
2232
|
+
}
|
|
2233
|
+
}
|
|
2234
|
+
result.push(path[furthest]);
|
|
2235
|
+
current = furthest;
|
|
2236
|
+
}
|
|
2237
|
+
return result;
|
|
2238
|
+
}
|
|
2239
|
+
};
|
|
2240
|
+
__name(_LineOfSightSmoother, "LineOfSightSmoother");
|
|
2241
|
+
var LineOfSightSmoother = _LineOfSightSmoother;
|
|
2242
|
+
var _CatmullRomSmoother = class _CatmullRomSmoother {
|
|
2243
|
+
/**
|
|
2244
|
+
* @param segments - @zh 每段之间的插值点数 @en Number of interpolation points per segment
|
|
2245
|
+
* @param tension - @zh 张力 (0-1) @en Tension (0-1)
|
|
2246
|
+
*/
|
|
2247
|
+
constructor(segments = 5, tension = 0.5) {
|
|
2248
|
+
__publicField(this, "segments");
|
|
2249
|
+
__publicField(this, "tension");
|
|
2250
|
+
this.segments = segments;
|
|
2251
|
+
this.tension = tension;
|
|
2252
|
+
}
|
|
2253
|
+
smooth(path, _map) {
|
|
2254
|
+
if (path.length <= 2) {
|
|
2255
|
+
return [
|
|
2256
|
+
...path
|
|
2257
|
+
];
|
|
2258
|
+
}
|
|
2259
|
+
const result = [];
|
|
2260
|
+
const points = [
|
|
2261
|
+
path[0],
|
|
2262
|
+
...path,
|
|
2263
|
+
path[path.length - 1]
|
|
2264
|
+
];
|
|
2265
|
+
for (let i = 1; i < points.length - 2; i++) {
|
|
2266
|
+
const p0 = points[i - 1];
|
|
2267
|
+
const p1 = points[i];
|
|
2268
|
+
const p2 = points[i + 1];
|
|
2269
|
+
const p3 = points[i + 2];
|
|
2270
|
+
for (let j = 0; j < this.segments; j++) {
|
|
2271
|
+
const t = j / this.segments;
|
|
2272
|
+
const point = this.interpolate(p0, p1, p2, p3, t);
|
|
2273
|
+
result.push(point);
|
|
2274
|
+
}
|
|
2275
|
+
}
|
|
2276
|
+
result.push(path[path.length - 1]);
|
|
2277
|
+
return result;
|
|
2278
|
+
}
|
|
2279
|
+
/**
|
|
2280
|
+
* @zh Catmull-Rom 插值
|
|
2281
|
+
* @en Catmull-Rom interpolation
|
|
2282
|
+
*/
|
|
2283
|
+
interpolate(p0, p1, p2, p3, t) {
|
|
2284
|
+
const t2 = t * t;
|
|
2285
|
+
const t3 = t2 * t;
|
|
2286
|
+
const tension = this.tension;
|
|
2287
|
+
const x = 0.5 * (2 * p1.x + (-p0.x + p2.x) * t * tension + (2 * p0.x - 5 * p1.x + 4 * p2.x - p3.x) * t2 * tension + (-p0.x + 3 * p1.x - 3 * p2.x + p3.x) * t3 * tension);
|
|
2288
|
+
const y = 0.5 * (2 * p1.y + (-p0.y + p2.y) * t * tension + (2 * p0.y - 5 * p1.y + 4 * p2.y - p3.y) * t2 * tension + (-p0.y + 3 * p1.y - 3 * p2.y + p3.y) * t3 * tension);
|
|
2289
|
+
return createPoint(x, y);
|
|
2290
|
+
}
|
|
2291
|
+
};
|
|
2292
|
+
__name(_CatmullRomSmoother, "CatmullRomSmoother");
|
|
2293
|
+
var CatmullRomSmoother = _CatmullRomSmoother;
|
|
2294
|
+
var _CombinedSmoother = class _CombinedSmoother {
|
|
2295
|
+
constructor(curveSegments = 5, tension = 0.5) {
|
|
2296
|
+
__publicField(this, "simplifier");
|
|
2297
|
+
__publicField(this, "curveSmoother");
|
|
2298
|
+
this.simplifier = new LineOfSightSmoother();
|
|
2299
|
+
this.curveSmoother = new CatmullRomSmoother(curveSegments, tension);
|
|
2300
|
+
}
|
|
2301
|
+
smooth(path, map) {
|
|
2302
|
+
const simplified = this.simplifier.smooth(path, map);
|
|
2303
|
+
return this.curveSmoother.smooth(simplified, map);
|
|
2304
|
+
}
|
|
2305
|
+
};
|
|
2306
|
+
__name(_CombinedSmoother, "CombinedSmoother");
|
|
2307
|
+
var CombinedSmoother = _CombinedSmoother;
|
|
2308
|
+
function createLineOfSightSmoother(lineOfSight) {
|
|
2309
|
+
return new LineOfSightSmoother(lineOfSight);
|
|
2310
|
+
}
|
|
2311
|
+
__name(createLineOfSightSmoother, "createLineOfSightSmoother");
|
|
2312
|
+
function createCatmullRomSmoother(segments, tension) {
|
|
2313
|
+
return new CatmullRomSmoother(segments, tension);
|
|
2314
|
+
}
|
|
2315
|
+
__name(createCatmullRomSmoother, "createCatmullRomSmoother");
|
|
2316
|
+
function createCombinedSmoother(curveSegments, tension) {
|
|
2317
|
+
return new CombinedSmoother(curveSegments, tension);
|
|
2318
|
+
}
|
|
2319
|
+
__name(createCombinedSmoother, "createCombinedSmoother");
|
|
2320
|
+
|
|
2321
|
+
// src/smoothing/RadiusAwarePathSmoother.ts
|
|
2322
|
+
var DEFAULT_CONFIG = {
|
|
2323
|
+
safetyMargin: 0.1,
|
|
2324
|
+
sampleDirections: 8,
|
|
2325
|
+
maxOffsetAttempts: 8,
|
|
2326
|
+
processCorners: true
|
|
2327
|
+
};
|
|
2328
|
+
var _RadiusAwarePathSmoother = class _RadiusAwarePathSmoother {
|
|
2329
|
+
constructor(config) {
|
|
2330
|
+
__publicField(this, "config");
|
|
2331
|
+
__publicField(this, "sampleAngles");
|
|
2332
|
+
this.config = {
|
|
2333
|
+
...DEFAULT_CONFIG,
|
|
2334
|
+
...config
|
|
2335
|
+
};
|
|
2336
|
+
this.sampleAngles = [];
|
|
2337
|
+
const step = Math.PI * 2 / this.config.sampleDirections;
|
|
2338
|
+
for (let i = 0; i < this.config.sampleDirections; i++) {
|
|
2339
|
+
this.sampleAngles.push(i * step);
|
|
2340
|
+
}
|
|
2341
|
+
}
|
|
2342
|
+
/**
|
|
2343
|
+
* @zh 平滑路径,确保与障碍物保持安全距离
|
|
2344
|
+
* @en Smooth path, ensuring safe distance from obstacles
|
|
2345
|
+
*
|
|
2346
|
+
* @param path - @zh 原始路径 @en Original path
|
|
2347
|
+
* @param map - @zh 地图 @en Map
|
|
2348
|
+
* @returns @zh 处理后的安全路径 @en Processed safe path
|
|
2349
|
+
*/
|
|
2350
|
+
smooth(path, map) {
|
|
2351
|
+
if (path.length <= 1) {
|
|
2352
|
+
return [
|
|
2353
|
+
...path
|
|
2354
|
+
];
|
|
2355
|
+
}
|
|
2356
|
+
const result = [];
|
|
2357
|
+
const clearance = this.config.agentRadius + this.config.safetyMargin;
|
|
2358
|
+
for (let i = 0; i < path.length; i++) {
|
|
2359
|
+
const point = path[i];
|
|
2360
|
+
const isCorner = this.config.processCorners && i > 0 && i < path.length - 1;
|
|
2361
|
+
let safePoint;
|
|
2362
|
+
if (isCorner) {
|
|
2363
|
+
const prev = path[i - 1];
|
|
2364
|
+
const next = path[i + 1];
|
|
2365
|
+
safePoint = this.offsetCornerPoint(point, prev, next, clearance, map);
|
|
2366
|
+
} else {
|
|
2367
|
+
safePoint = this.offsetPointFromObstacles(point, clearance, map);
|
|
2368
|
+
}
|
|
2369
|
+
result.push(safePoint);
|
|
2370
|
+
}
|
|
2371
|
+
return result;
|
|
2372
|
+
}
|
|
2373
|
+
/**
|
|
2374
|
+
* @zh 将点从障碍物偏移
|
|
2375
|
+
* @en Offset point away from obstacles
|
|
2376
|
+
*/
|
|
2377
|
+
offsetPointFromObstacles(point, clearance, map) {
|
|
2378
|
+
const obstacleDirections = this.detectNearbyObstacles(point, clearance, map);
|
|
2379
|
+
if (obstacleDirections.length === 0) {
|
|
2380
|
+
return point;
|
|
2381
|
+
}
|
|
2382
|
+
let avgDirX = 0;
|
|
2383
|
+
let avgDirY = 0;
|
|
2384
|
+
for (const dir of obstacleDirections) {
|
|
2385
|
+
avgDirX += dir.x;
|
|
2386
|
+
avgDirY += dir.y;
|
|
2387
|
+
}
|
|
2388
|
+
const len = Math.sqrt(avgDirX * avgDirX + avgDirY * avgDirY);
|
|
2389
|
+
if (len < 1e-4) {
|
|
2390
|
+
return point;
|
|
2391
|
+
}
|
|
2392
|
+
const offsetDirX = -avgDirX / len;
|
|
2393
|
+
const offsetDirY = -avgDirY / len;
|
|
2394
|
+
for (let attempt = 1; attempt <= this.config.maxOffsetAttempts; attempt++) {
|
|
2395
|
+
const offsetDist = clearance * attempt / this.config.maxOffsetAttempts;
|
|
2396
|
+
const newX = point.x + offsetDirX * offsetDist;
|
|
2397
|
+
const newY = point.y + offsetDirY * offsetDist;
|
|
2398
|
+
if (map.isWalkable(Math.floor(newX), Math.floor(newY))) {
|
|
2399
|
+
const newObstacles = this.detectNearbyObstacles(createPoint(newX, newY), clearance, map);
|
|
2400
|
+
if (newObstacles.length === 0) {
|
|
2401
|
+
return createPoint(newX, newY);
|
|
2402
|
+
}
|
|
2403
|
+
}
|
|
2404
|
+
}
|
|
2405
|
+
return point;
|
|
2406
|
+
}
|
|
2407
|
+
/**
|
|
2408
|
+
* @zh 偏移拐点(角落)
|
|
2409
|
+
* @en Offset corner point
|
|
2410
|
+
*/
|
|
2411
|
+
offsetCornerPoint(corner, prev, next, clearance, map) {
|
|
2412
|
+
const inDirX = corner.x - prev.x;
|
|
2413
|
+
const inDirY = corner.y - prev.y;
|
|
2414
|
+
const inLen = Math.sqrt(inDirX * inDirX + inDirY * inDirY);
|
|
2415
|
+
const outDirX = next.x - corner.x;
|
|
2416
|
+
const outDirY = next.y - corner.y;
|
|
2417
|
+
const outLen = Math.sqrt(outDirX * outDirX + outDirY * outDirY);
|
|
2418
|
+
if (inLen < 1e-4 || outLen < 1e-4) {
|
|
2419
|
+
return this.offsetPointFromObstacles(corner, clearance, map);
|
|
2420
|
+
}
|
|
2421
|
+
const inNormX = inDirX / inLen;
|
|
2422
|
+
const inNormY = inDirY / inLen;
|
|
2423
|
+
const outNormX = outDirX / outLen;
|
|
2424
|
+
const outNormY = outDirY / outLen;
|
|
2425
|
+
const bisectX = inNormX - outNormX;
|
|
2426
|
+
const bisectY = inNormY - outNormY;
|
|
2427
|
+
const bisectLen = Math.sqrt(bisectX * bisectX + bisectY * bisectY);
|
|
2428
|
+
if (bisectLen < 1e-4) {
|
|
2429
|
+
return this.offsetPointFromObstacles(corner, clearance, map);
|
|
2430
|
+
}
|
|
2431
|
+
const bisectNormX = bisectX / bisectLen;
|
|
2432
|
+
const bisectNormY = bisectY / bisectLen;
|
|
2433
|
+
const dotProduct = inNormX * outNormX + inNormY * outNormY;
|
|
2434
|
+
const angle = Math.acos(Math.max(-1, Math.min(1, dotProduct)));
|
|
2435
|
+
const halfAngle = angle / 2;
|
|
2436
|
+
const sinHalfAngle = Math.sin(halfAngle);
|
|
2437
|
+
if (sinHalfAngle < 0.1) {
|
|
2438
|
+
return this.offsetPointFromObstacles(corner, clearance, map);
|
|
2439
|
+
}
|
|
2440
|
+
const offsetDist = clearance / sinHalfAngle;
|
|
2441
|
+
const maxOffset = clearance * 3;
|
|
2442
|
+
const actualOffset = Math.min(offsetDist, maxOffset);
|
|
2443
|
+
const newX = corner.x + bisectNormX * actualOffset;
|
|
2444
|
+
const newY = corner.y + bisectNormY * actualOffset;
|
|
2445
|
+
if (map.isWalkable(Math.floor(newX), Math.floor(newY))) {
|
|
2446
|
+
return createPoint(newX, newY);
|
|
2447
|
+
}
|
|
2448
|
+
const altX = corner.x - bisectNormX * actualOffset;
|
|
2449
|
+
const altY = corner.y - bisectNormY * actualOffset;
|
|
2450
|
+
if (map.isWalkable(Math.floor(altX), Math.floor(altY))) {
|
|
2451
|
+
return createPoint(altX, altY);
|
|
2452
|
+
}
|
|
2453
|
+
return this.offsetPointFromObstacles(corner, clearance, map);
|
|
2454
|
+
}
|
|
2455
|
+
/**
|
|
2456
|
+
* @zh 检测附近的障碍物方向
|
|
2457
|
+
* @en Detect nearby obstacle directions
|
|
2458
|
+
*/
|
|
2459
|
+
detectNearbyObstacles(point, clearance, map) {
|
|
2460
|
+
const obstacles = [];
|
|
2461
|
+
for (const angle of this.sampleAngles) {
|
|
2462
|
+
const dirX = Math.cos(angle);
|
|
2463
|
+
const dirY = Math.sin(angle);
|
|
2464
|
+
const sampleX = point.x + dirX * clearance;
|
|
2465
|
+
const sampleY = point.y + dirY * clearance;
|
|
2466
|
+
if (!map.isWalkable(Math.floor(sampleX), Math.floor(sampleY))) {
|
|
2467
|
+
obstacles.push(createPoint(dirX, dirY));
|
|
2468
|
+
}
|
|
2469
|
+
}
|
|
2470
|
+
return obstacles;
|
|
2471
|
+
}
|
|
2472
|
+
};
|
|
2473
|
+
__name(_RadiusAwarePathSmoother, "RadiusAwarePathSmoother");
|
|
2474
|
+
var RadiusAwarePathSmoother = _RadiusAwarePathSmoother;
|
|
2475
|
+
var _CombinedRadiusAwareSmoother = class _CombinedRadiusAwareSmoother {
|
|
2476
|
+
constructor(baseSmoother, config) {
|
|
2477
|
+
__publicField(this, "baseSmoother");
|
|
2478
|
+
__publicField(this, "radiusAwareSmoother");
|
|
2479
|
+
this.baseSmoother = baseSmoother;
|
|
2480
|
+
this.radiusAwareSmoother = new RadiusAwarePathSmoother(config);
|
|
2481
|
+
}
|
|
2482
|
+
smooth(path, map) {
|
|
2483
|
+
const smoothed = this.baseSmoother.smooth(path, map);
|
|
2484
|
+
return this.radiusAwareSmoother.smooth(smoothed, map);
|
|
2485
|
+
}
|
|
2486
|
+
};
|
|
2487
|
+
__name(_CombinedRadiusAwareSmoother, "CombinedRadiusAwareSmoother");
|
|
2488
|
+
var CombinedRadiusAwareSmoother = _CombinedRadiusAwareSmoother;
|
|
2489
|
+
function createRadiusAwareSmoother(agentRadius, options) {
|
|
2490
|
+
return new RadiusAwarePathSmoother({
|
|
2491
|
+
agentRadius,
|
|
2492
|
+
...options
|
|
2493
|
+
});
|
|
2494
|
+
}
|
|
2495
|
+
__name(createRadiusAwareSmoother, "createRadiusAwareSmoother");
|
|
2496
|
+
function createCombinedRadiusAwareSmoother(baseSmoother, agentRadius, options) {
|
|
2497
|
+
return new CombinedRadiusAwareSmoother(baseSmoother, {
|
|
2498
|
+
agentRadius,
|
|
2499
|
+
...options
|
|
2500
|
+
});
|
|
2501
|
+
}
|
|
2502
|
+
__name(createCombinedRadiusAwareSmoother, "createCombinedRadiusAwareSmoother");
|
|
2932
2503
|
export {
|
|
2933
2504
|
AStarPathfinder,
|
|
2934
2505
|
BinaryHeap,
|
|
2935
2506
|
CatmullRomSmoother,
|
|
2507
|
+
CollisionResolver,
|
|
2508
|
+
CollisionResolverAdapter,
|
|
2509
|
+
CombinedRadiusAwareSmoother,
|
|
2936
2510
|
CombinedSmoother,
|
|
2937
2511
|
DEFAULT_AGENT_PARAMS,
|
|
2512
|
+
DEFAULT_COLLISION_CONFIG,
|
|
2513
|
+
DEFAULT_FLOW_CONTROLLER_CONFIG,
|
|
2938
2514
|
DEFAULT_GRID_OPTIONS,
|
|
2939
2515
|
DEFAULT_HPA_CONFIG,
|
|
2940
2516
|
DEFAULT_ORCA_CONFIG,
|
|
2517
|
+
DEFAULT_ORCA_PARAMS,
|
|
2941
2518
|
DEFAULT_PATHFINDING_OPTIONS,
|
|
2942
2519
|
DEFAULT_PATH_CACHE_CONFIG,
|
|
2943
2520
|
DEFAULT_REPLANNING_CONFIG,
|
|
2944
2521
|
DIRECTIONS_4,
|
|
2945
2522
|
DIRECTIONS_8,
|
|
2523
|
+
EMPTY_COLLISION,
|
|
2524
|
+
EMPTY_COLLISION_RESULT,
|
|
2946
2525
|
EMPTY_PATH_RESULT,
|
|
2526
|
+
EMPTY_PLAN_RESULT,
|
|
2947
2527
|
EMPTY_PROGRESS,
|
|
2528
|
+
FlowController,
|
|
2948
2529
|
GridMap,
|
|
2949
2530
|
GridNode,
|
|
2950
2531
|
GridPathfinder,
|
|
2532
|
+
GridPathfinderAdapter,
|
|
2951
2533
|
HPAPathfinder,
|
|
2952
2534
|
IncrementalAStarPathfinder,
|
|
2535
|
+
IncrementalGridPathPlannerAdapter,
|
|
2953
2536
|
IndexedBinaryHeap,
|
|
2954
2537
|
JPSPathfinder,
|
|
2955
2538
|
KDTree,
|
|
2956
2539
|
LineOfSightSmoother,
|
|
2957
2540
|
NavMesh,
|
|
2541
|
+
NavMeshPathPlannerAdapter,
|
|
2542
|
+
ORCALocalAvoidanceAdapter,
|
|
2958
2543
|
ORCASolver,
|
|
2959
2544
|
ObstacleChangeManager,
|
|
2545
|
+
PassPermission,
|
|
2960
2546
|
PathCache,
|
|
2547
|
+
PathPlanState,
|
|
2961
2548
|
PathValidator,
|
|
2962
2549
|
PathfindingState,
|
|
2550
|
+
RadiusAwarePathSmoother,
|
|
2963
2551
|
bresenhamLineOfSight,
|
|
2964
2552
|
chebyshevDistance,
|
|
2965
2553
|
createAStarPathfinder,
|
|
2554
|
+
createAStarPlanner,
|
|
2966
2555
|
createCatmullRomSmoother,
|
|
2556
|
+
createCollisionResolver,
|
|
2557
|
+
createCombinedRadiusAwareSmoother,
|
|
2967
2558
|
createCombinedSmoother,
|
|
2559
|
+
createDefaultCollisionResolver,
|
|
2560
|
+
createFlowController,
|
|
2968
2561
|
createGridMap,
|
|
2969
2562
|
createGridPathfinder,
|
|
2970
2563
|
createHPAPathfinder,
|
|
2564
|
+
createHPAPlanner,
|
|
2971
2565
|
createIncrementalAStarPathfinder,
|
|
2566
|
+
createIncrementalAStarPlanner,
|
|
2972
2567
|
createJPSPathfinder,
|
|
2568
|
+
createJPSPlanner,
|
|
2973
2569
|
createKDTree,
|
|
2974
2570
|
createLineOfSightSmoother,
|
|
2975
2571
|
createNavMesh,
|
|
2572
|
+
createNavMeshPathPlanner,
|
|
2573
|
+
createORCAAvoidance,
|
|
2976
2574
|
createORCASolver,
|
|
2977
2575
|
createObstacleChangeManager,
|
|
2978
2576
|
createPathCache,
|
|
2979
2577
|
createPathValidator,
|
|
2980
2578
|
createPoint,
|
|
2579
|
+
createRadiusAwareSmoother,
|
|
2981
2580
|
euclideanDistance,
|
|
2581
|
+
isIncrementalPlanner,
|
|
2982
2582
|
manhattanDistance,
|
|
2983
2583
|
octileDistance,
|
|
2984
2584
|
raycastLineOfSight,
|