@esengine/pathfinding 13.2.0 → 13.3.1
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-BztOzQsW.d.ts +2781 -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-ZYGBA7VK.js +3831 -0
- package/dist/chunk-ZYGBA7VK.js.map +1 -0
- package/dist/ecs.d.ts +440 -647
- package/dist/ecs.js +1020 -1399
- package/dist/ecs.js.map +1 -1
- package/dist/index.d.ts +158 -711
- package/dist/index.js +1353 -1739
- 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
|
@@ -0,0 +1,3831 @@
|
|
|
1
|
+
import {
|
|
2
|
+
EMPTY_PROGRESS,
|
|
3
|
+
PathfindingState
|
|
4
|
+
} from "./chunk-YKA3PWU3.js";
|
|
5
|
+
import {
|
|
6
|
+
createCollisionResolver,
|
|
7
|
+
createKDTree,
|
|
8
|
+
createORCASolver
|
|
9
|
+
} from "./chunk-3VEX32JO.js";
|
|
10
|
+
import {
|
|
11
|
+
__name,
|
|
12
|
+
__publicField
|
|
13
|
+
} from "./chunk-T626JPC7.js";
|
|
14
|
+
|
|
15
|
+
// src/core/IPathfinding.ts
|
|
16
|
+
function createPoint(x, y) {
|
|
17
|
+
return {
|
|
18
|
+
x,
|
|
19
|
+
y
|
|
20
|
+
};
|
|
21
|
+
}
|
|
22
|
+
__name(createPoint, "createPoint");
|
|
23
|
+
var EMPTY_PATH_RESULT = {
|
|
24
|
+
found: false,
|
|
25
|
+
path: [],
|
|
26
|
+
cost: 0,
|
|
27
|
+
nodesSearched: 0
|
|
28
|
+
};
|
|
29
|
+
function manhattanDistance(a, b) {
|
|
30
|
+
return Math.abs(a.x - b.x) + Math.abs(a.y - b.y);
|
|
31
|
+
}
|
|
32
|
+
__name(manhattanDistance, "manhattanDistance");
|
|
33
|
+
function euclideanDistance(a, b) {
|
|
34
|
+
const dx = a.x - b.x;
|
|
35
|
+
const dy = a.y - b.y;
|
|
36
|
+
return Math.sqrt(dx * dx + dy * dy);
|
|
37
|
+
}
|
|
38
|
+
__name(euclideanDistance, "euclideanDistance");
|
|
39
|
+
function chebyshevDistance(a, b) {
|
|
40
|
+
return Math.max(Math.abs(a.x - b.x), Math.abs(a.y - b.y));
|
|
41
|
+
}
|
|
42
|
+
__name(chebyshevDistance, "chebyshevDistance");
|
|
43
|
+
function octileDistance(a, b) {
|
|
44
|
+
const dx = Math.abs(a.x - b.x);
|
|
45
|
+
const dy = Math.abs(a.y - b.y);
|
|
46
|
+
const D = 1;
|
|
47
|
+
const D2 = Math.SQRT2;
|
|
48
|
+
return D * (dx + dy) + (D2 - 2 * D) * Math.min(dx, dy);
|
|
49
|
+
}
|
|
50
|
+
__name(octileDistance, "octileDistance");
|
|
51
|
+
var DEFAULT_PATHFINDING_OPTIONS = {
|
|
52
|
+
maxNodes: 1e4,
|
|
53
|
+
heuristicWeight: 1,
|
|
54
|
+
allowDiagonal: true,
|
|
55
|
+
avoidCorners: true,
|
|
56
|
+
agentRadius: 0
|
|
57
|
+
};
|
|
58
|
+
|
|
59
|
+
// src/core/BinaryHeap.ts
|
|
60
|
+
var _BinaryHeap = class _BinaryHeap {
|
|
61
|
+
/**
|
|
62
|
+
* @zh 创建二叉堆
|
|
63
|
+
* @en Create binary heap
|
|
64
|
+
*
|
|
65
|
+
* @param compare - @zh 比较函数,返回负数表示 a < b @en Compare function, returns negative if a < b
|
|
66
|
+
*/
|
|
67
|
+
constructor(compare) {
|
|
68
|
+
__publicField(this, "heap", []);
|
|
69
|
+
__publicField(this, "compare");
|
|
70
|
+
this.compare = compare;
|
|
71
|
+
}
|
|
72
|
+
/**
|
|
73
|
+
* @zh 堆大小
|
|
74
|
+
* @en Heap size
|
|
75
|
+
*/
|
|
76
|
+
get size() {
|
|
77
|
+
return this.heap.length;
|
|
78
|
+
}
|
|
79
|
+
/**
|
|
80
|
+
* @zh 是否为空
|
|
81
|
+
* @en Is empty
|
|
82
|
+
*/
|
|
83
|
+
get isEmpty() {
|
|
84
|
+
return this.heap.length === 0;
|
|
85
|
+
}
|
|
86
|
+
/**
|
|
87
|
+
* @zh 插入元素
|
|
88
|
+
* @en Push element
|
|
89
|
+
*/
|
|
90
|
+
push(item) {
|
|
91
|
+
this.heap.push(item);
|
|
92
|
+
this.bubbleUp(this.heap.length - 1);
|
|
93
|
+
}
|
|
94
|
+
/**
|
|
95
|
+
* @zh 弹出最小元素
|
|
96
|
+
* @en Pop minimum element
|
|
97
|
+
*/
|
|
98
|
+
pop() {
|
|
99
|
+
if (this.heap.length === 0) {
|
|
100
|
+
return void 0;
|
|
101
|
+
}
|
|
102
|
+
const result = this.heap[0];
|
|
103
|
+
const last = this.heap.pop();
|
|
104
|
+
if (this.heap.length > 0) {
|
|
105
|
+
this.heap[0] = last;
|
|
106
|
+
this.sinkDown(0);
|
|
107
|
+
}
|
|
108
|
+
return result;
|
|
109
|
+
}
|
|
110
|
+
/**
|
|
111
|
+
* @zh 查看最小元素(不移除)
|
|
112
|
+
* @en Peek minimum element (without removing)
|
|
113
|
+
*/
|
|
114
|
+
peek() {
|
|
115
|
+
return this.heap[0];
|
|
116
|
+
}
|
|
117
|
+
/**
|
|
118
|
+
* @zh 更新元素(重新排序)
|
|
119
|
+
* @en Update element (re-sort)
|
|
120
|
+
*/
|
|
121
|
+
update(item) {
|
|
122
|
+
const index = this.heap.indexOf(item);
|
|
123
|
+
if (index !== -1) {
|
|
124
|
+
this.bubbleUp(index);
|
|
125
|
+
this.sinkDown(index);
|
|
126
|
+
}
|
|
127
|
+
}
|
|
128
|
+
/**
|
|
129
|
+
* @zh 检查是否包含元素
|
|
130
|
+
* @en Check if contains element
|
|
131
|
+
*/
|
|
132
|
+
contains(item) {
|
|
133
|
+
return this.heap.indexOf(item) !== -1;
|
|
134
|
+
}
|
|
135
|
+
/**
|
|
136
|
+
* @zh 清空堆
|
|
137
|
+
* @en Clear heap
|
|
138
|
+
*/
|
|
139
|
+
clear() {
|
|
140
|
+
this.heap.length = 0;
|
|
141
|
+
}
|
|
142
|
+
/**
|
|
143
|
+
* @zh 上浮操作
|
|
144
|
+
* @en Bubble up operation
|
|
145
|
+
*/
|
|
146
|
+
bubbleUp(index) {
|
|
147
|
+
const item = this.heap[index];
|
|
148
|
+
while (index > 0) {
|
|
149
|
+
const parentIndex = Math.floor((index - 1) / 2);
|
|
150
|
+
const parent = this.heap[parentIndex];
|
|
151
|
+
if (this.compare(item, parent) >= 0) {
|
|
152
|
+
break;
|
|
153
|
+
}
|
|
154
|
+
this.heap[index] = parent;
|
|
155
|
+
index = parentIndex;
|
|
156
|
+
}
|
|
157
|
+
this.heap[index] = item;
|
|
158
|
+
}
|
|
159
|
+
/**
|
|
160
|
+
* @zh 下沉操作
|
|
161
|
+
* @en Sink down operation
|
|
162
|
+
*/
|
|
163
|
+
sinkDown(index) {
|
|
164
|
+
const length = this.heap.length;
|
|
165
|
+
const item = this.heap[index];
|
|
166
|
+
while (true) {
|
|
167
|
+
const leftIndex = 2 * index + 1;
|
|
168
|
+
const rightIndex = 2 * index + 2;
|
|
169
|
+
let smallest = index;
|
|
170
|
+
if (leftIndex < length && this.compare(this.heap[leftIndex], this.heap[smallest]) < 0) {
|
|
171
|
+
smallest = leftIndex;
|
|
172
|
+
}
|
|
173
|
+
if (rightIndex < length && this.compare(this.heap[rightIndex], this.heap[smallest]) < 0) {
|
|
174
|
+
smallest = rightIndex;
|
|
175
|
+
}
|
|
176
|
+
if (smallest === index) {
|
|
177
|
+
break;
|
|
178
|
+
}
|
|
179
|
+
this.heap[index] = this.heap[smallest];
|
|
180
|
+
this.heap[smallest] = item;
|
|
181
|
+
index = smallest;
|
|
182
|
+
}
|
|
183
|
+
}
|
|
184
|
+
};
|
|
185
|
+
__name(_BinaryHeap, "BinaryHeap");
|
|
186
|
+
var BinaryHeap = _BinaryHeap;
|
|
187
|
+
|
|
188
|
+
// src/core/IndexedBinaryHeap.ts
|
|
189
|
+
var _IndexedBinaryHeap = class _IndexedBinaryHeap {
|
|
190
|
+
/**
|
|
191
|
+
* @zh 创建带索引追踪的二叉堆
|
|
192
|
+
* @en Create indexed binary heap
|
|
193
|
+
*
|
|
194
|
+
* @param compare - @zh 比较函数,返回负数表示 a < b @en Compare function, returns negative if a < b
|
|
195
|
+
*/
|
|
196
|
+
constructor(compare) {
|
|
197
|
+
__publicField(this, "heap", []);
|
|
198
|
+
__publicField(this, "compare");
|
|
199
|
+
this.compare = compare;
|
|
200
|
+
}
|
|
201
|
+
/**
|
|
202
|
+
* @zh 堆大小
|
|
203
|
+
* @en Heap size
|
|
204
|
+
*/
|
|
205
|
+
get size() {
|
|
206
|
+
return this.heap.length;
|
|
207
|
+
}
|
|
208
|
+
/**
|
|
209
|
+
* @zh 是否为空
|
|
210
|
+
* @en Is empty
|
|
211
|
+
*/
|
|
212
|
+
get isEmpty() {
|
|
213
|
+
return this.heap.length === 0;
|
|
214
|
+
}
|
|
215
|
+
/**
|
|
216
|
+
* @zh 插入元素
|
|
217
|
+
* @en Push element
|
|
218
|
+
*/
|
|
219
|
+
push(item) {
|
|
220
|
+
item.heapIndex = this.heap.length;
|
|
221
|
+
this.heap.push(item);
|
|
222
|
+
this.bubbleUp(this.heap.length - 1);
|
|
223
|
+
}
|
|
224
|
+
/**
|
|
225
|
+
* @zh 弹出最小元素
|
|
226
|
+
* @en Pop minimum element
|
|
227
|
+
*/
|
|
228
|
+
pop() {
|
|
229
|
+
if (this.heap.length === 0) {
|
|
230
|
+
return void 0;
|
|
231
|
+
}
|
|
232
|
+
const result = this.heap[0];
|
|
233
|
+
result.heapIndex = -1;
|
|
234
|
+
const last = this.heap.pop();
|
|
235
|
+
if (this.heap.length > 0) {
|
|
236
|
+
last.heapIndex = 0;
|
|
237
|
+
this.heap[0] = last;
|
|
238
|
+
this.sinkDown(0);
|
|
239
|
+
}
|
|
240
|
+
return result;
|
|
241
|
+
}
|
|
242
|
+
/**
|
|
243
|
+
* @zh 查看最小元素(不移除)
|
|
244
|
+
* @en Peek minimum element (without removing)
|
|
245
|
+
*/
|
|
246
|
+
peek() {
|
|
247
|
+
return this.heap[0];
|
|
248
|
+
}
|
|
249
|
+
/**
|
|
250
|
+
* @zh 更新元素
|
|
251
|
+
* @en Update element
|
|
252
|
+
*/
|
|
253
|
+
update(item) {
|
|
254
|
+
const index = item.heapIndex;
|
|
255
|
+
if (index >= 0 && index < this.heap.length && this.heap[index] === item) {
|
|
256
|
+
this.bubbleUp(index);
|
|
257
|
+
this.sinkDown(item.heapIndex);
|
|
258
|
+
}
|
|
259
|
+
}
|
|
260
|
+
/**
|
|
261
|
+
* @zh 检查是否包含元素
|
|
262
|
+
* @en Check if contains element
|
|
263
|
+
*/
|
|
264
|
+
contains(item) {
|
|
265
|
+
const index = item.heapIndex;
|
|
266
|
+
return index >= 0 && index < this.heap.length && this.heap[index] === item;
|
|
267
|
+
}
|
|
268
|
+
/**
|
|
269
|
+
* @zh 从堆中移除指定元素
|
|
270
|
+
* @en Remove specific element from heap
|
|
271
|
+
*/
|
|
272
|
+
remove(item) {
|
|
273
|
+
const index = item.heapIndex;
|
|
274
|
+
if (index < 0 || index >= this.heap.length || this.heap[index] !== item) {
|
|
275
|
+
return false;
|
|
276
|
+
}
|
|
277
|
+
item.heapIndex = -1;
|
|
278
|
+
if (index === this.heap.length - 1) {
|
|
279
|
+
this.heap.pop();
|
|
280
|
+
return true;
|
|
281
|
+
}
|
|
282
|
+
const last = this.heap.pop();
|
|
283
|
+
last.heapIndex = index;
|
|
284
|
+
this.heap[index] = last;
|
|
285
|
+
this.bubbleUp(index);
|
|
286
|
+
this.sinkDown(last.heapIndex);
|
|
287
|
+
return true;
|
|
288
|
+
}
|
|
289
|
+
/**
|
|
290
|
+
* @zh 清空堆
|
|
291
|
+
* @en Clear heap
|
|
292
|
+
*/
|
|
293
|
+
clear() {
|
|
294
|
+
for (const item of this.heap) {
|
|
295
|
+
item.heapIndex = -1;
|
|
296
|
+
}
|
|
297
|
+
this.heap.length = 0;
|
|
298
|
+
}
|
|
299
|
+
/**
|
|
300
|
+
* @zh 上浮操作
|
|
301
|
+
* @en Bubble up operation
|
|
302
|
+
*/
|
|
303
|
+
bubbleUp(index) {
|
|
304
|
+
const item = this.heap[index];
|
|
305
|
+
while (index > 0) {
|
|
306
|
+
const parentIndex = index - 1 >> 1;
|
|
307
|
+
const parent = this.heap[parentIndex];
|
|
308
|
+
if (this.compare(item, parent) >= 0) {
|
|
309
|
+
break;
|
|
310
|
+
}
|
|
311
|
+
parent.heapIndex = index;
|
|
312
|
+
this.heap[index] = parent;
|
|
313
|
+
index = parentIndex;
|
|
314
|
+
}
|
|
315
|
+
item.heapIndex = index;
|
|
316
|
+
this.heap[index] = item;
|
|
317
|
+
}
|
|
318
|
+
/**
|
|
319
|
+
* @zh 下沉操作
|
|
320
|
+
* @en Sink down operation
|
|
321
|
+
*/
|
|
322
|
+
sinkDown(index) {
|
|
323
|
+
const length = this.heap.length;
|
|
324
|
+
const item = this.heap[index];
|
|
325
|
+
const halfLength = length >> 1;
|
|
326
|
+
while (index < halfLength) {
|
|
327
|
+
const leftIndex = (index << 1) + 1;
|
|
328
|
+
const rightIndex = leftIndex + 1;
|
|
329
|
+
let smallest = index;
|
|
330
|
+
let smallestItem = item;
|
|
331
|
+
const left = this.heap[leftIndex];
|
|
332
|
+
if (this.compare(left, smallestItem) < 0) {
|
|
333
|
+
smallest = leftIndex;
|
|
334
|
+
smallestItem = left;
|
|
335
|
+
}
|
|
336
|
+
if (rightIndex < length) {
|
|
337
|
+
const right = this.heap[rightIndex];
|
|
338
|
+
if (this.compare(right, smallestItem) < 0) {
|
|
339
|
+
smallest = rightIndex;
|
|
340
|
+
smallestItem = right;
|
|
341
|
+
}
|
|
342
|
+
}
|
|
343
|
+
if (smallest === index) {
|
|
344
|
+
break;
|
|
345
|
+
}
|
|
346
|
+
smallestItem.heapIndex = index;
|
|
347
|
+
this.heap[index] = smallestItem;
|
|
348
|
+
index = smallest;
|
|
349
|
+
}
|
|
350
|
+
item.heapIndex = index;
|
|
351
|
+
this.heap[index] = item;
|
|
352
|
+
}
|
|
353
|
+
};
|
|
354
|
+
__name(_IndexedBinaryHeap, "IndexedBinaryHeap");
|
|
355
|
+
var IndexedBinaryHeap = _IndexedBinaryHeap;
|
|
356
|
+
|
|
357
|
+
// src/core/AStarPathfinder.ts
|
|
358
|
+
var _AStarPathfinder = class _AStarPathfinder {
|
|
359
|
+
constructor(map) {
|
|
360
|
+
__publicField(this, "map");
|
|
361
|
+
__publicField(this, "nodeCache", /* @__PURE__ */ new Map());
|
|
362
|
+
__publicField(this, "openList");
|
|
363
|
+
this.map = map;
|
|
364
|
+
this.openList = new IndexedBinaryHeap((a, b) => a.f - b.f);
|
|
365
|
+
}
|
|
366
|
+
/**
|
|
367
|
+
* @zh 查找路径
|
|
368
|
+
* @en Find path
|
|
369
|
+
*/
|
|
370
|
+
findPath(startX, startY, endX, endY, options) {
|
|
371
|
+
const opts = {
|
|
372
|
+
...DEFAULT_PATHFINDING_OPTIONS,
|
|
373
|
+
...options
|
|
374
|
+
};
|
|
375
|
+
this.clear();
|
|
376
|
+
const startNode = this.map.getNodeAt(startX, startY);
|
|
377
|
+
const endNode = this.map.getNodeAt(endX, endY);
|
|
378
|
+
if (!startNode || !endNode) {
|
|
379
|
+
return EMPTY_PATH_RESULT;
|
|
380
|
+
}
|
|
381
|
+
if (!startNode.walkable || !endNode.walkable) {
|
|
382
|
+
return EMPTY_PATH_RESULT;
|
|
383
|
+
}
|
|
384
|
+
if (startNode.id === endNode.id) {
|
|
385
|
+
return {
|
|
386
|
+
found: true,
|
|
387
|
+
path: [
|
|
388
|
+
startNode.position
|
|
389
|
+
],
|
|
390
|
+
cost: 0,
|
|
391
|
+
nodesSearched: 1
|
|
392
|
+
};
|
|
393
|
+
}
|
|
394
|
+
const start = this.getOrCreateAStarNode(startNode);
|
|
395
|
+
start.g = 0;
|
|
396
|
+
start.h = this.map.heuristic(startNode.position, endNode.position) * opts.heuristicWeight;
|
|
397
|
+
start.f = start.h;
|
|
398
|
+
start.opened = true;
|
|
399
|
+
this.openList.push(start);
|
|
400
|
+
let nodesSearched = 0;
|
|
401
|
+
const endPosition = endNode.position;
|
|
402
|
+
while (!this.openList.isEmpty && nodesSearched < opts.maxNodes) {
|
|
403
|
+
const current = this.openList.pop();
|
|
404
|
+
current.closed = true;
|
|
405
|
+
nodesSearched++;
|
|
406
|
+
if (current.node.id === endNode.id) {
|
|
407
|
+
return this.buildPath(current, nodesSearched);
|
|
408
|
+
}
|
|
409
|
+
const neighbors = this.map.getNeighbors(current.node);
|
|
410
|
+
for (const neighborNode of neighbors) {
|
|
411
|
+
if (!neighborNode.walkable) {
|
|
412
|
+
continue;
|
|
413
|
+
}
|
|
414
|
+
const neighbor = this.getOrCreateAStarNode(neighborNode);
|
|
415
|
+
if (neighbor.closed) {
|
|
416
|
+
continue;
|
|
417
|
+
}
|
|
418
|
+
const movementCost = this.map.getMovementCost(current.node, neighborNode);
|
|
419
|
+
const tentativeG = current.g + movementCost;
|
|
420
|
+
if (!neighbor.opened) {
|
|
421
|
+
neighbor.g = tentativeG;
|
|
422
|
+
neighbor.h = this.map.heuristic(neighborNode.position, endPosition) * opts.heuristicWeight;
|
|
423
|
+
neighbor.f = neighbor.g + neighbor.h;
|
|
424
|
+
neighbor.parent = current;
|
|
425
|
+
neighbor.opened = true;
|
|
426
|
+
this.openList.push(neighbor);
|
|
427
|
+
} else if (tentativeG < neighbor.g) {
|
|
428
|
+
neighbor.g = tentativeG;
|
|
429
|
+
neighbor.f = neighbor.g + neighbor.h;
|
|
430
|
+
neighbor.parent = current;
|
|
431
|
+
this.openList.update(neighbor);
|
|
432
|
+
}
|
|
433
|
+
}
|
|
434
|
+
}
|
|
435
|
+
return {
|
|
436
|
+
found: false,
|
|
437
|
+
path: [],
|
|
438
|
+
cost: 0,
|
|
439
|
+
nodesSearched
|
|
440
|
+
};
|
|
441
|
+
}
|
|
442
|
+
/**
|
|
443
|
+
* @zh 清理状态
|
|
444
|
+
* @en Clear state
|
|
445
|
+
*/
|
|
446
|
+
clear() {
|
|
447
|
+
this.nodeCache.clear();
|
|
448
|
+
this.openList.clear();
|
|
449
|
+
}
|
|
450
|
+
/**
|
|
451
|
+
* @zh 获取或创建 A* 节点
|
|
452
|
+
* @en Get or create A* node
|
|
453
|
+
*/
|
|
454
|
+
getOrCreateAStarNode(node) {
|
|
455
|
+
let astarNode = this.nodeCache.get(node.id);
|
|
456
|
+
if (!astarNode) {
|
|
457
|
+
astarNode = {
|
|
458
|
+
node,
|
|
459
|
+
g: Infinity,
|
|
460
|
+
h: 0,
|
|
461
|
+
f: Infinity,
|
|
462
|
+
parent: null,
|
|
463
|
+
closed: false,
|
|
464
|
+
opened: false,
|
|
465
|
+
heapIndex: -1
|
|
466
|
+
};
|
|
467
|
+
this.nodeCache.set(node.id, astarNode);
|
|
468
|
+
}
|
|
469
|
+
return astarNode;
|
|
470
|
+
}
|
|
471
|
+
/**
|
|
472
|
+
* @zh 构建路径结果
|
|
473
|
+
* @en Build path result
|
|
474
|
+
*/
|
|
475
|
+
buildPath(endNode, nodesSearched) {
|
|
476
|
+
const path = [];
|
|
477
|
+
let current = endNode;
|
|
478
|
+
while (current) {
|
|
479
|
+
path.push(current.node.position);
|
|
480
|
+
current = current.parent;
|
|
481
|
+
}
|
|
482
|
+
path.reverse();
|
|
483
|
+
return {
|
|
484
|
+
found: true,
|
|
485
|
+
path,
|
|
486
|
+
cost: endNode.g,
|
|
487
|
+
nodesSearched
|
|
488
|
+
};
|
|
489
|
+
}
|
|
490
|
+
};
|
|
491
|
+
__name(_AStarPathfinder, "AStarPathfinder");
|
|
492
|
+
var AStarPathfinder = _AStarPathfinder;
|
|
493
|
+
function createAStarPathfinder(map) {
|
|
494
|
+
return new AStarPathfinder(map);
|
|
495
|
+
}
|
|
496
|
+
__name(createAStarPathfinder, "createAStarPathfinder");
|
|
497
|
+
|
|
498
|
+
// src/core/PathCache.ts
|
|
499
|
+
var DEFAULT_PATH_CACHE_CONFIG = {
|
|
500
|
+
maxEntries: 1e3,
|
|
501
|
+
ttlMs: 5e3,
|
|
502
|
+
enableApproximateMatch: false,
|
|
503
|
+
approximateRange: 2
|
|
504
|
+
};
|
|
505
|
+
var _PathCache = class _PathCache {
|
|
506
|
+
constructor(config = {}) {
|
|
507
|
+
__publicField(this, "config");
|
|
508
|
+
__publicField(this, "cache");
|
|
509
|
+
__publicField(this, "accessOrder");
|
|
510
|
+
this.config = {
|
|
511
|
+
...DEFAULT_PATH_CACHE_CONFIG,
|
|
512
|
+
...config
|
|
513
|
+
};
|
|
514
|
+
this.cache = /* @__PURE__ */ new Map();
|
|
515
|
+
this.accessOrder = [];
|
|
516
|
+
}
|
|
517
|
+
/**
|
|
518
|
+
* @zh 获取缓存的路径
|
|
519
|
+
* @en Get cached path
|
|
520
|
+
*
|
|
521
|
+
* @param startX - @zh 起点 X 坐标 @en Start X coordinate
|
|
522
|
+
* @param startY - @zh 起点 Y 坐标 @en Start Y coordinate
|
|
523
|
+
* @param endX - @zh 终点 X 坐标 @en End X coordinate
|
|
524
|
+
* @param endY - @zh 终点 Y 坐标 @en End Y coordinate
|
|
525
|
+
* @param mapVersion - @zh 地图版本号 @en Map version number
|
|
526
|
+
* @returns @zh 缓存的路径结果或 null @en Cached path result or null
|
|
527
|
+
*/
|
|
528
|
+
get(startX, startY, endX, endY, mapVersion) {
|
|
529
|
+
const key = this.generateKey(startX, startY, endX, endY);
|
|
530
|
+
const entry = this.cache.get(key);
|
|
531
|
+
if (!entry) {
|
|
532
|
+
if (this.config.enableApproximateMatch) {
|
|
533
|
+
return this.getApproximate(startX, startY, endX, endY, mapVersion);
|
|
534
|
+
}
|
|
535
|
+
return null;
|
|
536
|
+
}
|
|
537
|
+
if (!this.isValid(entry, mapVersion)) {
|
|
538
|
+
this.cache.delete(key);
|
|
539
|
+
this.removeFromAccessOrder(key);
|
|
540
|
+
return null;
|
|
541
|
+
}
|
|
542
|
+
this.updateAccessOrder(key);
|
|
543
|
+
return entry.result;
|
|
544
|
+
}
|
|
545
|
+
/**
|
|
546
|
+
* @zh 设置缓存路径
|
|
547
|
+
* @en Set cached path
|
|
548
|
+
*
|
|
549
|
+
* @param startX - @zh 起点 X 坐标 @en Start X coordinate
|
|
550
|
+
* @param startY - @zh 起点 Y 坐标 @en Start Y coordinate
|
|
551
|
+
* @param endX - @zh 终点 X 坐标 @en End X coordinate
|
|
552
|
+
* @param endY - @zh 终点 Y 坐标 @en End Y coordinate
|
|
553
|
+
* @param result - @zh 路径结果 @en Path result
|
|
554
|
+
* @param mapVersion - @zh 地图版本号 @en Map version number
|
|
555
|
+
*/
|
|
556
|
+
set(startX, startY, endX, endY, result, mapVersion) {
|
|
557
|
+
if (this.cache.size >= this.config.maxEntries) {
|
|
558
|
+
this.evictLRU();
|
|
559
|
+
}
|
|
560
|
+
const key = this.generateKey(startX, startY, endX, endY);
|
|
561
|
+
const entry = {
|
|
562
|
+
result,
|
|
563
|
+
timestamp: Date.now(),
|
|
564
|
+
mapVersion
|
|
565
|
+
};
|
|
566
|
+
this.cache.set(key, entry);
|
|
567
|
+
this.updateAccessOrder(key);
|
|
568
|
+
}
|
|
569
|
+
/**
|
|
570
|
+
* @zh 使所有缓存失效
|
|
571
|
+
* @en Invalidate all cache
|
|
572
|
+
*/
|
|
573
|
+
invalidateAll() {
|
|
574
|
+
this.cache.clear();
|
|
575
|
+
this.accessOrder.length = 0;
|
|
576
|
+
}
|
|
577
|
+
/**
|
|
578
|
+
* @zh 使指定区域的缓存失效
|
|
579
|
+
* @en Invalidate cache for specified region
|
|
580
|
+
*
|
|
581
|
+
* @param minX - @zh 最小 X 坐标 @en Minimum X coordinate
|
|
582
|
+
* @param minY - @zh 最小 Y 坐标 @en Minimum Y coordinate
|
|
583
|
+
* @param maxX - @zh 最大 X 坐标 @en Maximum X coordinate
|
|
584
|
+
* @param maxY - @zh 最大 Y 坐标 @en Maximum Y coordinate
|
|
585
|
+
*/
|
|
586
|
+
invalidateRegion(minX, minY, maxX, maxY) {
|
|
587
|
+
const keysToDelete = [];
|
|
588
|
+
for (const [key, entry] of this.cache) {
|
|
589
|
+
const path = entry.result.path;
|
|
590
|
+
if (path.length === 0) continue;
|
|
591
|
+
for (const point of path) {
|
|
592
|
+
if (point.x >= minX && point.x <= maxX && point.y >= minY && point.y <= maxY) {
|
|
593
|
+
keysToDelete.push(key);
|
|
594
|
+
break;
|
|
595
|
+
}
|
|
596
|
+
}
|
|
597
|
+
}
|
|
598
|
+
for (const key of keysToDelete) {
|
|
599
|
+
this.cache.delete(key);
|
|
600
|
+
this.removeFromAccessOrder(key);
|
|
601
|
+
}
|
|
602
|
+
}
|
|
603
|
+
/**
|
|
604
|
+
* @zh 获取缓存统计信息
|
|
605
|
+
* @en Get cache statistics
|
|
606
|
+
*/
|
|
607
|
+
getStats() {
|
|
608
|
+
return {
|
|
609
|
+
size: this.cache.size,
|
|
610
|
+
maxSize: this.config.maxEntries
|
|
611
|
+
};
|
|
612
|
+
}
|
|
613
|
+
/**
|
|
614
|
+
* @zh 清理过期条目
|
|
615
|
+
* @en Clean up expired entries
|
|
616
|
+
*/
|
|
617
|
+
cleanup() {
|
|
618
|
+
if (this.config.ttlMs === 0) return;
|
|
619
|
+
const now = Date.now();
|
|
620
|
+
const keysToDelete = [];
|
|
621
|
+
for (const [key, entry] of this.cache) {
|
|
622
|
+
if (now - entry.timestamp > this.config.ttlMs) {
|
|
623
|
+
keysToDelete.push(key);
|
|
624
|
+
}
|
|
625
|
+
}
|
|
626
|
+
for (const key of keysToDelete) {
|
|
627
|
+
this.cache.delete(key);
|
|
628
|
+
this.removeFromAccessOrder(key);
|
|
629
|
+
}
|
|
630
|
+
}
|
|
631
|
+
// =========================================================================
|
|
632
|
+
// 私有方法 | Private Methods
|
|
633
|
+
// =========================================================================
|
|
634
|
+
generateKey(startX, startY, endX, endY) {
|
|
635
|
+
return `${startX},${startY}->${endX},${endY}`;
|
|
636
|
+
}
|
|
637
|
+
isValid(entry, mapVersion) {
|
|
638
|
+
if (entry.mapVersion !== mapVersion) {
|
|
639
|
+
return false;
|
|
640
|
+
}
|
|
641
|
+
if (this.config.ttlMs > 0) {
|
|
642
|
+
const age = Date.now() - entry.timestamp;
|
|
643
|
+
if (age > this.config.ttlMs) {
|
|
644
|
+
return false;
|
|
645
|
+
}
|
|
646
|
+
}
|
|
647
|
+
return true;
|
|
648
|
+
}
|
|
649
|
+
getApproximate(startX, startY, endX, endY, mapVersion) {
|
|
650
|
+
const range = this.config.approximateRange;
|
|
651
|
+
for (let sx = startX - range; sx <= startX + range; sx++) {
|
|
652
|
+
for (let sy = startY - range; sy <= startY + range; sy++) {
|
|
653
|
+
for (let ex = endX - range; ex <= endX + range; ex++) {
|
|
654
|
+
for (let ey = endY - range; ey <= endY + range; ey++) {
|
|
655
|
+
const key = this.generateKey(sx, sy, ex, ey);
|
|
656
|
+
const entry = this.cache.get(key);
|
|
657
|
+
if (entry && this.isValid(entry, mapVersion)) {
|
|
658
|
+
this.updateAccessOrder(key);
|
|
659
|
+
return this.adjustPathForApproximate(entry.result, startX, startY, endX, endY);
|
|
660
|
+
}
|
|
661
|
+
}
|
|
662
|
+
}
|
|
663
|
+
}
|
|
664
|
+
}
|
|
665
|
+
return null;
|
|
666
|
+
}
|
|
667
|
+
adjustPathForApproximate(result, newStartX, newStartY, newEndX, newEndY) {
|
|
668
|
+
if (result.path.length === 0) {
|
|
669
|
+
return result;
|
|
670
|
+
}
|
|
671
|
+
const newPath = [];
|
|
672
|
+
const oldStart = result.path[0];
|
|
673
|
+
const oldEnd = result.path[result.path.length - 1];
|
|
674
|
+
if (newStartX !== oldStart.x || newStartY !== oldStart.y) {
|
|
675
|
+
newPath.push({
|
|
676
|
+
x: newStartX,
|
|
677
|
+
y: newStartY
|
|
678
|
+
});
|
|
679
|
+
}
|
|
680
|
+
newPath.push(...result.path);
|
|
681
|
+
if (newEndX !== oldEnd.x || newEndY !== oldEnd.y) {
|
|
682
|
+
newPath.push({
|
|
683
|
+
x: newEndX,
|
|
684
|
+
y: newEndY
|
|
685
|
+
});
|
|
686
|
+
}
|
|
687
|
+
return {
|
|
688
|
+
...result,
|
|
689
|
+
path: newPath
|
|
690
|
+
};
|
|
691
|
+
}
|
|
692
|
+
updateAccessOrder(key) {
|
|
693
|
+
this.removeFromAccessOrder(key);
|
|
694
|
+
this.accessOrder.push(key);
|
|
695
|
+
}
|
|
696
|
+
removeFromAccessOrder(key) {
|
|
697
|
+
const index = this.accessOrder.indexOf(key);
|
|
698
|
+
if (index !== -1) {
|
|
699
|
+
this.accessOrder.splice(index, 1);
|
|
700
|
+
}
|
|
701
|
+
}
|
|
702
|
+
evictLRU() {
|
|
703
|
+
const lruKey = this.accessOrder.shift();
|
|
704
|
+
if (lruKey) {
|
|
705
|
+
this.cache.delete(lruKey);
|
|
706
|
+
}
|
|
707
|
+
}
|
|
708
|
+
};
|
|
709
|
+
__name(_PathCache, "PathCache");
|
|
710
|
+
var PathCache = _PathCache;
|
|
711
|
+
function createPathCache(config) {
|
|
712
|
+
return new PathCache(config);
|
|
713
|
+
}
|
|
714
|
+
__name(createPathCache, "createPathCache");
|
|
715
|
+
|
|
716
|
+
// src/core/IncrementalAStarPathfinder.ts
|
|
717
|
+
var _IncrementalAStarPathfinder = class _IncrementalAStarPathfinder {
|
|
718
|
+
/**
|
|
719
|
+
* @zh 创建增量 A* 寻路器
|
|
720
|
+
* @en Create incremental A* pathfinder
|
|
721
|
+
*
|
|
722
|
+
* @param map - @zh 寻路地图实例 @en Pathfinding map instance
|
|
723
|
+
* @param config - @zh 配置选项 @en Configuration options
|
|
724
|
+
*/
|
|
725
|
+
constructor(map, config) {
|
|
726
|
+
__publicField(this, "map");
|
|
727
|
+
__publicField(this, "sessions", /* @__PURE__ */ new Map());
|
|
728
|
+
__publicField(this, "nextRequestId", 0);
|
|
729
|
+
__publicField(this, "affectedRegions", []);
|
|
730
|
+
__publicField(this, "maxRegionAge", 5e3);
|
|
731
|
+
__publicField(this, "cache");
|
|
732
|
+
__publicField(this, "enableCache");
|
|
733
|
+
__publicField(this, "mapVersion", 0);
|
|
734
|
+
__publicField(this, "cacheHits", 0);
|
|
735
|
+
__publicField(this, "cacheMisses", 0);
|
|
736
|
+
this.map = map;
|
|
737
|
+
this.enableCache = config?.enableCache ?? false;
|
|
738
|
+
this.cache = this.enableCache ? new PathCache(config?.cacheConfig) : null;
|
|
739
|
+
}
|
|
740
|
+
/**
|
|
741
|
+
* @zh 请求寻路(非阻塞)
|
|
742
|
+
* @en Request pathfinding (non-blocking)
|
|
743
|
+
*/
|
|
744
|
+
requestPath(startX, startY, endX, endY, options) {
|
|
745
|
+
const id = this.nextRequestId++;
|
|
746
|
+
const priority = options?.priority ?? 50;
|
|
747
|
+
const opts = {
|
|
748
|
+
...DEFAULT_PATHFINDING_OPTIONS,
|
|
749
|
+
...options
|
|
750
|
+
};
|
|
751
|
+
const request = {
|
|
752
|
+
id,
|
|
753
|
+
startX,
|
|
754
|
+
startY,
|
|
755
|
+
endX,
|
|
756
|
+
endY,
|
|
757
|
+
options: opts,
|
|
758
|
+
priority,
|
|
759
|
+
createdAt: Date.now()
|
|
760
|
+
};
|
|
761
|
+
if (this.cache) {
|
|
762
|
+
const cached = this.cache.get(startX, startY, endX, endY, this.mapVersion);
|
|
763
|
+
if (cached) {
|
|
764
|
+
this.cacheHits++;
|
|
765
|
+
const session2 = {
|
|
766
|
+
request,
|
|
767
|
+
state: cached.found ? PathfindingState.Completed : PathfindingState.Failed,
|
|
768
|
+
options: opts,
|
|
769
|
+
openList: new IndexedBinaryHeap((a, b) => a.f - b.f),
|
|
770
|
+
nodeCache: /* @__PURE__ */ new Map(),
|
|
771
|
+
startNode: this.map.getNodeAt(startX, startY),
|
|
772
|
+
endNode: this.map.getNodeAt(endX, endY),
|
|
773
|
+
endPosition: {
|
|
774
|
+
x: endX,
|
|
775
|
+
y: endY
|
|
776
|
+
},
|
|
777
|
+
nodesSearched: cached.nodesSearched,
|
|
778
|
+
framesUsed: 0,
|
|
779
|
+
initialDistance: 0,
|
|
780
|
+
result: {
|
|
781
|
+
requestId: id,
|
|
782
|
+
found: cached.found,
|
|
783
|
+
path: [
|
|
784
|
+
...cached.path
|
|
785
|
+
],
|
|
786
|
+
cost: cached.cost,
|
|
787
|
+
nodesSearched: cached.nodesSearched,
|
|
788
|
+
framesUsed: 0,
|
|
789
|
+
isPartial: false
|
|
790
|
+
},
|
|
791
|
+
affectedByChange: false
|
|
792
|
+
};
|
|
793
|
+
this.sessions.set(id, session2);
|
|
794
|
+
return request;
|
|
795
|
+
}
|
|
796
|
+
this.cacheMisses++;
|
|
797
|
+
}
|
|
798
|
+
const startNode = this.map.getNodeAt(startX, startY);
|
|
799
|
+
const endNode = this.map.getNodeAt(endX, endY);
|
|
800
|
+
if (!startNode || !endNode || !startNode.walkable || !endNode.walkable) {
|
|
801
|
+
const session2 = {
|
|
802
|
+
request,
|
|
803
|
+
state: PathfindingState.Failed,
|
|
804
|
+
options: opts,
|
|
805
|
+
openList: new IndexedBinaryHeap((a, b) => a.f - b.f),
|
|
806
|
+
nodeCache: /* @__PURE__ */ new Map(),
|
|
807
|
+
startNode,
|
|
808
|
+
endNode,
|
|
809
|
+
endPosition: endNode?.position ?? {
|
|
810
|
+
x: endX,
|
|
811
|
+
y: endY
|
|
812
|
+
},
|
|
813
|
+
nodesSearched: 0,
|
|
814
|
+
framesUsed: 0,
|
|
815
|
+
initialDistance: 0,
|
|
816
|
+
result: this.createEmptyResult(id),
|
|
817
|
+
affectedByChange: false
|
|
818
|
+
};
|
|
819
|
+
this.sessions.set(id, session2);
|
|
820
|
+
return request;
|
|
821
|
+
}
|
|
822
|
+
if (startNode.id === endNode.id) {
|
|
823
|
+
const session2 = {
|
|
824
|
+
request,
|
|
825
|
+
state: PathfindingState.Completed,
|
|
826
|
+
options: opts,
|
|
827
|
+
openList: new IndexedBinaryHeap((a, b) => a.f - b.f),
|
|
828
|
+
nodeCache: /* @__PURE__ */ new Map(),
|
|
829
|
+
startNode,
|
|
830
|
+
endNode,
|
|
831
|
+
endPosition: endNode.position,
|
|
832
|
+
nodesSearched: 1,
|
|
833
|
+
framesUsed: 0,
|
|
834
|
+
initialDistance: 0,
|
|
835
|
+
result: {
|
|
836
|
+
requestId: id,
|
|
837
|
+
found: true,
|
|
838
|
+
path: [
|
|
839
|
+
startNode.position
|
|
840
|
+
],
|
|
841
|
+
cost: 0,
|
|
842
|
+
nodesSearched: 1,
|
|
843
|
+
framesUsed: 0,
|
|
844
|
+
isPartial: false
|
|
845
|
+
},
|
|
846
|
+
affectedByChange: false
|
|
847
|
+
};
|
|
848
|
+
this.sessions.set(id, session2);
|
|
849
|
+
return request;
|
|
850
|
+
}
|
|
851
|
+
const initialDistance = this.map.heuristic(startNode.position, endNode.position);
|
|
852
|
+
const openList = new IndexedBinaryHeap((a, b) => a.f - b.f);
|
|
853
|
+
const nodeCache = /* @__PURE__ */ new Map();
|
|
854
|
+
const startAStarNode = {
|
|
855
|
+
node: startNode,
|
|
856
|
+
g: 0,
|
|
857
|
+
h: initialDistance * opts.heuristicWeight,
|
|
858
|
+
f: initialDistance * opts.heuristicWeight,
|
|
859
|
+
parent: null,
|
|
860
|
+
closed: false,
|
|
861
|
+
opened: true,
|
|
862
|
+
heapIndex: -1
|
|
863
|
+
};
|
|
864
|
+
nodeCache.set(startNode.id, startAStarNode);
|
|
865
|
+
openList.push(startAStarNode);
|
|
866
|
+
const session = {
|
|
867
|
+
request,
|
|
868
|
+
state: PathfindingState.InProgress,
|
|
869
|
+
options: opts,
|
|
870
|
+
openList,
|
|
871
|
+
nodeCache,
|
|
872
|
+
startNode,
|
|
873
|
+
endNode,
|
|
874
|
+
endPosition: endNode.position,
|
|
875
|
+
nodesSearched: 0,
|
|
876
|
+
framesUsed: 0,
|
|
877
|
+
initialDistance,
|
|
878
|
+
result: null,
|
|
879
|
+
affectedByChange: false
|
|
880
|
+
};
|
|
881
|
+
this.sessions.set(id, session);
|
|
882
|
+
return request;
|
|
883
|
+
}
|
|
884
|
+
/**
|
|
885
|
+
* @zh 执行一步搜索
|
|
886
|
+
* @en Execute one step of search
|
|
887
|
+
*/
|
|
888
|
+
step(requestId, maxIterations) {
|
|
889
|
+
const session = this.sessions.get(requestId);
|
|
890
|
+
if (!session) {
|
|
891
|
+
return EMPTY_PROGRESS;
|
|
892
|
+
}
|
|
893
|
+
if (session.state !== PathfindingState.InProgress) {
|
|
894
|
+
return this.createProgress(session);
|
|
895
|
+
}
|
|
896
|
+
session.framesUsed++;
|
|
897
|
+
let iterations = 0;
|
|
898
|
+
while (!session.openList.isEmpty && iterations < maxIterations) {
|
|
899
|
+
const current = session.openList.pop();
|
|
900
|
+
current.closed = true;
|
|
901
|
+
session.nodesSearched++;
|
|
902
|
+
iterations++;
|
|
903
|
+
if (current.node.id === session.endNode.id) {
|
|
904
|
+
session.state = PathfindingState.Completed;
|
|
905
|
+
session.result = this.buildResult(session, current);
|
|
906
|
+
if (this.cache && session.result.found) {
|
|
907
|
+
const req = session.request;
|
|
908
|
+
this.cache.set(req.startX, req.startY, req.endX, req.endY, {
|
|
909
|
+
found: true,
|
|
910
|
+
path: session.result.path,
|
|
911
|
+
cost: session.result.cost,
|
|
912
|
+
nodesSearched: session.result.nodesSearched
|
|
913
|
+
}, this.mapVersion);
|
|
914
|
+
}
|
|
915
|
+
return this.createProgress(session);
|
|
916
|
+
}
|
|
917
|
+
this.expandNeighbors(session, current);
|
|
918
|
+
if (session.nodesSearched >= session.options.maxNodes) {
|
|
919
|
+
session.state = PathfindingState.Failed;
|
|
920
|
+
session.result = this.createEmptyResult(requestId);
|
|
921
|
+
return this.createProgress(session);
|
|
922
|
+
}
|
|
923
|
+
}
|
|
924
|
+
if (session.openList.isEmpty && session.state === PathfindingState.InProgress) {
|
|
925
|
+
session.state = PathfindingState.Failed;
|
|
926
|
+
session.result = this.createEmptyResult(requestId);
|
|
927
|
+
}
|
|
928
|
+
return this.createProgress(session);
|
|
929
|
+
}
|
|
930
|
+
/**
|
|
931
|
+
* @zh 暂停寻路
|
|
932
|
+
* @en Pause pathfinding
|
|
933
|
+
*/
|
|
934
|
+
pause(requestId) {
|
|
935
|
+
const session = this.sessions.get(requestId);
|
|
936
|
+
if (session && session.state === PathfindingState.InProgress) {
|
|
937
|
+
session.state = PathfindingState.Paused;
|
|
938
|
+
}
|
|
939
|
+
}
|
|
940
|
+
/**
|
|
941
|
+
* @zh 恢复寻路
|
|
942
|
+
* @en Resume pathfinding
|
|
943
|
+
*/
|
|
944
|
+
resume(requestId) {
|
|
945
|
+
const session = this.sessions.get(requestId);
|
|
946
|
+
if (session && session.state === PathfindingState.Paused) {
|
|
947
|
+
session.state = PathfindingState.InProgress;
|
|
948
|
+
}
|
|
949
|
+
}
|
|
950
|
+
/**
|
|
951
|
+
* @zh 取消寻路
|
|
952
|
+
* @en Cancel pathfinding
|
|
953
|
+
*/
|
|
954
|
+
cancel(requestId) {
|
|
955
|
+
const session = this.sessions.get(requestId);
|
|
956
|
+
if (session && (session.state === PathfindingState.InProgress || session.state === PathfindingState.Paused)) {
|
|
957
|
+
session.state = PathfindingState.Cancelled;
|
|
958
|
+
session.result = this.createEmptyResult(requestId);
|
|
959
|
+
}
|
|
960
|
+
}
|
|
961
|
+
/**
|
|
962
|
+
* @zh 获取寻路结果
|
|
963
|
+
* @en Get pathfinding result
|
|
964
|
+
*/
|
|
965
|
+
getResult(requestId) {
|
|
966
|
+
const session = this.sessions.get(requestId);
|
|
967
|
+
return session?.result ?? null;
|
|
968
|
+
}
|
|
969
|
+
/**
|
|
970
|
+
* @zh 获取当前进度
|
|
971
|
+
* @en Get current progress
|
|
972
|
+
*/
|
|
973
|
+
getProgress(requestId) {
|
|
974
|
+
const session = this.sessions.get(requestId);
|
|
975
|
+
return session ? this.createProgress(session) : null;
|
|
976
|
+
}
|
|
977
|
+
/**
|
|
978
|
+
* @zh 清理已完成的请求
|
|
979
|
+
* @en Clean up completed request
|
|
980
|
+
*/
|
|
981
|
+
cleanup(requestId) {
|
|
982
|
+
const session = this.sessions.get(requestId);
|
|
983
|
+
if (session) {
|
|
984
|
+
session.openList.clear();
|
|
985
|
+
session.nodeCache.clear();
|
|
986
|
+
this.sessions.delete(requestId);
|
|
987
|
+
}
|
|
988
|
+
}
|
|
989
|
+
/**
|
|
990
|
+
* @zh 通知障碍物变化
|
|
991
|
+
* @en Notify obstacle change
|
|
992
|
+
*/
|
|
993
|
+
notifyObstacleChange(minX, minY, maxX, maxY) {
|
|
994
|
+
this.mapVersion++;
|
|
995
|
+
if (this.cache) {
|
|
996
|
+
this.cache.invalidateRegion(minX, minY, maxX, maxY);
|
|
997
|
+
}
|
|
998
|
+
const region = {
|
|
999
|
+
minX,
|
|
1000
|
+
minY,
|
|
1001
|
+
maxX,
|
|
1002
|
+
maxY,
|
|
1003
|
+
timestamp: Date.now()
|
|
1004
|
+
};
|
|
1005
|
+
this.affectedRegions.push(region);
|
|
1006
|
+
for (const session of this.sessions.values()) {
|
|
1007
|
+
if (session.state === PathfindingState.InProgress || session.state === PathfindingState.Paused) {
|
|
1008
|
+
if (this.sessionAffectedByRegion(session, region)) {
|
|
1009
|
+
session.affectedByChange = true;
|
|
1010
|
+
}
|
|
1011
|
+
}
|
|
1012
|
+
}
|
|
1013
|
+
this.cleanupOldRegions();
|
|
1014
|
+
}
|
|
1015
|
+
/**
|
|
1016
|
+
* @zh 清理所有请求
|
|
1017
|
+
* @en Clear all requests
|
|
1018
|
+
*/
|
|
1019
|
+
clear() {
|
|
1020
|
+
for (const session of this.sessions.values()) {
|
|
1021
|
+
session.openList.clear();
|
|
1022
|
+
session.nodeCache.clear();
|
|
1023
|
+
}
|
|
1024
|
+
this.sessions.clear();
|
|
1025
|
+
this.affectedRegions.length = 0;
|
|
1026
|
+
}
|
|
1027
|
+
/**
|
|
1028
|
+
* @zh 清空路径缓存
|
|
1029
|
+
* @en Clear path cache
|
|
1030
|
+
*/
|
|
1031
|
+
clearCache() {
|
|
1032
|
+
if (this.cache) {
|
|
1033
|
+
this.cache.invalidateAll();
|
|
1034
|
+
this.cacheHits = 0;
|
|
1035
|
+
this.cacheMisses = 0;
|
|
1036
|
+
}
|
|
1037
|
+
}
|
|
1038
|
+
/**
|
|
1039
|
+
* @zh 获取缓存统计信息
|
|
1040
|
+
* @en Get cache statistics
|
|
1041
|
+
*/
|
|
1042
|
+
getCacheStats() {
|
|
1043
|
+
if (!this.cache) {
|
|
1044
|
+
return {
|
|
1045
|
+
enabled: false,
|
|
1046
|
+
hits: 0,
|
|
1047
|
+
misses: 0,
|
|
1048
|
+
hitRate: 0,
|
|
1049
|
+
size: 0
|
|
1050
|
+
};
|
|
1051
|
+
}
|
|
1052
|
+
const total = this.cacheHits + this.cacheMisses;
|
|
1053
|
+
const hitRate = total > 0 ? this.cacheHits / total : 0;
|
|
1054
|
+
return {
|
|
1055
|
+
enabled: true,
|
|
1056
|
+
hits: this.cacheHits,
|
|
1057
|
+
misses: this.cacheMisses,
|
|
1058
|
+
hitRate,
|
|
1059
|
+
size: this.cache.getStats().size
|
|
1060
|
+
};
|
|
1061
|
+
}
|
|
1062
|
+
/**
|
|
1063
|
+
* @zh 检查会话是否被障碍物变化影响
|
|
1064
|
+
* @en Check if session is affected by obstacle change
|
|
1065
|
+
*/
|
|
1066
|
+
isAffectedByChange(requestId) {
|
|
1067
|
+
const session = this.sessions.get(requestId);
|
|
1068
|
+
return session?.affectedByChange ?? false;
|
|
1069
|
+
}
|
|
1070
|
+
/**
|
|
1071
|
+
* @zh 清除会话的变化标记
|
|
1072
|
+
* @en Clear session's change flag
|
|
1073
|
+
*/
|
|
1074
|
+
clearChangeFlag(requestId) {
|
|
1075
|
+
const session = this.sessions.get(requestId);
|
|
1076
|
+
if (session) {
|
|
1077
|
+
session.affectedByChange = false;
|
|
1078
|
+
}
|
|
1079
|
+
}
|
|
1080
|
+
// =========================================================================
|
|
1081
|
+
// 私有方法 | Private Methods
|
|
1082
|
+
// =========================================================================
|
|
1083
|
+
/**
|
|
1084
|
+
* @zh 展开邻居节点
|
|
1085
|
+
* @en Expand neighbor nodes
|
|
1086
|
+
*/
|
|
1087
|
+
expandNeighbors(session, current) {
|
|
1088
|
+
const neighbors = this.map.getNeighbors(current.node);
|
|
1089
|
+
for (const neighborNode of neighbors) {
|
|
1090
|
+
if (!neighborNode.walkable) {
|
|
1091
|
+
continue;
|
|
1092
|
+
}
|
|
1093
|
+
let neighbor = session.nodeCache.get(neighborNode.id);
|
|
1094
|
+
if (!neighbor) {
|
|
1095
|
+
neighbor = {
|
|
1096
|
+
node: neighborNode,
|
|
1097
|
+
g: Infinity,
|
|
1098
|
+
h: 0,
|
|
1099
|
+
f: Infinity,
|
|
1100
|
+
parent: null,
|
|
1101
|
+
closed: false,
|
|
1102
|
+
opened: false,
|
|
1103
|
+
heapIndex: -1
|
|
1104
|
+
};
|
|
1105
|
+
session.nodeCache.set(neighborNode.id, neighbor);
|
|
1106
|
+
}
|
|
1107
|
+
if (neighbor.closed) {
|
|
1108
|
+
continue;
|
|
1109
|
+
}
|
|
1110
|
+
const movementCost = this.map.getMovementCost(current.node, neighborNode);
|
|
1111
|
+
const tentativeG = current.g + movementCost;
|
|
1112
|
+
if (!neighbor.opened) {
|
|
1113
|
+
neighbor.g = tentativeG;
|
|
1114
|
+
neighbor.h = this.map.heuristic(neighborNode.position, session.endPosition) * session.options.heuristicWeight;
|
|
1115
|
+
neighbor.f = neighbor.g + neighbor.h;
|
|
1116
|
+
neighbor.parent = current;
|
|
1117
|
+
neighbor.opened = true;
|
|
1118
|
+
session.openList.push(neighbor);
|
|
1119
|
+
} else if (tentativeG < neighbor.g) {
|
|
1120
|
+
neighbor.g = tentativeG;
|
|
1121
|
+
neighbor.f = neighbor.g + neighbor.h;
|
|
1122
|
+
neighbor.parent = current;
|
|
1123
|
+
session.openList.update(neighbor);
|
|
1124
|
+
}
|
|
1125
|
+
}
|
|
1126
|
+
}
|
|
1127
|
+
/**
|
|
1128
|
+
* @zh 创建进度对象
|
|
1129
|
+
* @en Create progress object
|
|
1130
|
+
*/
|
|
1131
|
+
createProgress(session) {
|
|
1132
|
+
let estimatedProgress = 0;
|
|
1133
|
+
if (session.state === PathfindingState.Completed) {
|
|
1134
|
+
estimatedProgress = 1;
|
|
1135
|
+
} else if (session.state === PathfindingState.InProgress && session.initialDistance > 0) {
|
|
1136
|
+
const bestNode = session.openList.peek();
|
|
1137
|
+
if (bestNode) {
|
|
1138
|
+
const currentDistance = bestNode.h / session.options.heuristicWeight;
|
|
1139
|
+
estimatedProgress = Math.max(0, Math.min(1, 1 - currentDistance / session.initialDistance));
|
|
1140
|
+
}
|
|
1141
|
+
}
|
|
1142
|
+
return {
|
|
1143
|
+
state: session.state,
|
|
1144
|
+
nodesSearched: session.nodesSearched,
|
|
1145
|
+
openListSize: session.openList.size,
|
|
1146
|
+
estimatedProgress
|
|
1147
|
+
};
|
|
1148
|
+
}
|
|
1149
|
+
/**
|
|
1150
|
+
* @zh 构建路径结果
|
|
1151
|
+
* @en Build path result
|
|
1152
|
+
*/
|
|
1153
|
+
buildResult(session, endNode) {
|
|
1154
|
+
const path = [];
|
|
1155
|
+
let current = endNode;
|
|
1156
|
+
while (current) {
|
|
1157
|
+
path.push(current.node.position);
|
|
1158
|
+
current = current.parent;
|
|
1159
|
+
}
|
|
1160
|
+
path.reverse();
|
|
1161
|
+
return {
|
|
1162
|
+
requestId: session.request.id,
|
|
1163
|
+
found: true,
|
|
1164
|
+
path,
|
|
1165
|
+
cost: endNode.g,
|
|
1166
|
+
nodesSearched: session.nodesSearched,
|
|
1167
|
+
framesUsed: session.framesUsed,
|
|
1168
|
+
isPartial: false
|
|
1169
|
+
};
|
|
1170
|
+
}
|
|
1171
|
+
/**
|
|
1172
|
+
* @zh 创建空结果
|
|
1173
|
+
* @en Create empty result
|
|
1174
|
+
*/
|
|
1175
|
+
createEmptyResult(requestId) {
|
|
1176
|
+
return {
|
|
1177
|
+
requestId,
|
|
1178
|
+
found: false,
|
|
1179
|
+
path: [],
|
|
1180
|
+
cost: 0,
|
|
1181
|
+
nodesSearched: 0,
|
|
1182
|
+
framesUsed: 0,
|
|
1183
|
+
isPartial: false
|
|
1184
|
+
};
|
|
1185
|
+
}
|
|
1186
|
+
/**
|
|
1187
|
+
* @zh 检查会话是否被区域影响
|
|
1188
|
+
* @en Check if session is affected by region
|
|
1189
|
+
*/
|
|
1190
|
+
sessionAffectedByRegion(session, region) {
|
|
1191
|
+
for (const astarNode of session.nodeCache.values()) {
|
|
1192
|
+
if (astarNode.opened || astarNode.closed) {
|
|
1193
|
+
const pos = astarNode.node.position;
|
|
1194
|
+
if (pos.x >= region.minX && pos.x <= region.maxX && pos.y >= region.minY && pos.y <= region.maxY) {
|
|
1195
|
+
return true;
|
|
1196
|
+
}
|
|
1197
|
+
}
|
|
1198
|
+
}
|
|
1199
|
+
const start = session.request;
|
|
1200
|
+
const end = session.endPosition;
|
|
1201
|
+
if (start.startX >= region.minX && start.startX <= region.maxX && start.startY >= region.minY && start.startY <= region.maxY || end.x >= region.minX && end.x <= region.maxX && end.y >= region.minY && end.y <= region.maxY) {
|
|
1202
|
+
return true;
|
|
1203
|
+
}
|
|
1204
|
+
return false;
|
|
1205
|
+
}
|
|
1206
|
+
/**
|
|
1207
|
+
* @zh 清理过期的变化区域
|
|
1208
|
+
* @en Clean up expired change regions
|
|
1209
|
+
*/
|
|
1210
|
+
cleanupOldRegions() {
|
|
1211
|
+
const now = Date.now();
|
|
1212
|
+
let i = 0;
|
|
1213
|
+
while (i < this.affectedRegions.length) {
|
|
1214
|
+
if (now - this.affectedRegions[i].timestamp > this.maxRegionAge) {
|
|
1215
|
+
this.affectedRegions.splice(i, 1);
|
|
1216
|
+
} else {
|
|
1217
|
+
i++;
|
|
1218
|
+
}
|
|
1219
|
+
}
|
|
1220
|
+
}
|
|
1221
|
+
};
|
|
1222
|
+
__name(_IncrementalAStarPathfinder, "IncrementalAStarPathfinder");
|
|
1223
|
+
var IncrementalAStarPathfinder = _IncrementalAStarPathfinder;
|
|
1224
|
+
function createIncrementalAStarPathfinder(map) {
|
|
1225
|
+
return new IncrementalAStarPathfinder(map);
|
|
1226
|
+
}
|
|
1227
|
+
__name(createIncrementalAStarPathfinder, "createIncrementalAStarPathfinder");
|
|
1228
|
+
|
|
1229
|
+
// src/core/JPSPathfinder.ts
|
|
1230
|
+
var _JPSPathfinder = class _JPSPathfinder {
|
|
1231
|
+
constructor(map) {
|
|
1232
|
+
__publicField(this, "map");
|
|
1233
|
+
__publicField(this, "width");
|
|
1234
|
+
__publicField(this, "height");
|
|
1235
|
+
__publicField(this, "openList");
|
|
1236
|
+
__publicField(this, "nodeGrid");
|
|
1237
|
+
this.map = map;
|
|
1238
|
+
const bounds = this.getMapBounds();
|
|
1239
|
+
this.width = bounds.width;
|
|
1240
|
+
this.height = bounds.height;
|
|
1241
|
+
this.openList = new BinaryHeap((a, b) => a.f - b.f);
|
|
1242
|
+
this.nodeGrid = [];
|
|
1243
|
+
}
|
|
1244
|
+
/**
|
|
1245
|
+
* @zh 寻找路径
|
|
1246
|
+
* @en Find path
|
|
1247
|
+
*/
|
|
1248
|
+
findPath(startX, startY, endX, endY, options) {
|
|
1249
|
+
const opts = {
|
|
1250
|
+
...DEFAULT_PATHFINDING_OPTIONS,
|
|
1251
|
+
...options
|
|
1252
|
+
};
|
|
1253
|
+
if (!this.map.isWalkable(startX, startY) || !this.map.isWalkable(endX, endY)) {
|
|
1254
|
+
return EMPTY_PATH_RESULT;
|
|
1255
|
+
}
|
|
1256
|
+
if (startX === endX && startY === endY) {
|
|
1257
|
+
return {
|
|
1258
|
+
found: true,
|
|
1259
|
+
path: [
|
|
1260
|
+
{
|
|
1261
|
+
x: startX,
|
|
1262
|
+
y: startY
|
|
1263
|
+
}
|
|
1264
|
+
],
|
|
1265
|
+
cost: 0,
|
|
1266
|
+
nodesSearched: 1
|
|
1267
|
+
};
|
|
1268
|
+
}
|
|
1269
|
+
this.initGrid();
|
|
1270
|
+
this.openList.clear();
|
|
1271
|
+
const startNode = this.getOrCreateNode(startX, startY);
|
|
1272
|
+
startNode.g = 0;
|
|
1273
|
+
startNode.h = this.heuristic(startX, startY, endX, endY) * opts.heuristicWeight;
|
|
1274
|
+
startNode.f = startNode.h;
|
|
1275
|
+
this.openList.push(startNode);
|
|
1276
|
+
let nodesSearched = 0;
|
|
1277
|
+
while (!this.openList.isEmpty && nodesSearched < opts.maxNodes) {
|
|
1278
|
+
const current = this.openList.pop();
|
|
1279
|
+
current.closed = true;
|
|
1280
|
+
nodesSearched++;
|
|
1281
|
+
if (current.x === endX && current.y === endY) {
|
|
1282
|
+
return {
|
|
1283
|
+
found: true,
|
|
1284
|
+
path: this.buildPath(current),
|
|
1285
|
+
cost: current.g,
|
|
1286
|
+
nodesSearched
|
|
1287
|
+
};
|
|
1288
|
+
}
|
|
1289
|
+
this.identifySuccessors(current, endX, endY, opts);
|
|
1290
|
+
}
|
|
1291
|
+
return {
|
|
1292
|
+
found: false,
|
|
1293
|
+
path: [],
|
|
1294
|
+
cost: 0,
|
|
1295
|
+
nodesSearched
|
|
1296
|
+
};
|
|
1297
|
+
}
|
|
1298
|
+
/**
|
|
1299
|
+
* @zh 清理状态
|
|
1300
|
+
* @en Clear state
|
|
1301
|
+
*/
|
|
1302
|
+
clear() {
|
|
1303
|
+
this.openList.clear();
|
|
1304
|
+
this.nodeGrid = [];
|
|
1305
|
+
}
|
|
1306
|
+
// =========================================================================
|
|
1307
|
+
// 私有方法 | Private Methods
|
|
1308
|
+
// =========================================================================
|
|
1309
|
+
/**
|
|
1310
|
+
* @zh 获取地图边界
|
|
1311
|
+
* @en Get map bounds
|
|
1312
|
+
*/
|
|
1313
|
+
getMapBounds() {
|
|
1314
|
+
const mapAny = this.map;
|
|
1315
|
+
if (typeof mapAny.width === "number" && typeof mapAny.height === "number") {
|
|
1316
|
+
return {
|
|
1317
|
+
width: mapAny.width,
|
|
1318
|
+
height: mapAny.height
|
|
1319
|
+
};
|
|
1320
|
+
}
|
|
1321
|
+
return {
|
|
1322
|
+
width: 1e3,
|
|
1323
|
+
height: 1e3
|
|
1324
|
+
};
|
|
1325
|
+
}
|
|
1326
|
+
/**
|
|
1327
|
+
* @zh 初始化节点网格
|
|
1328
|
+
* @en Initialize node grid
|
|
1329
|
+
*/
|
|
1330
|
+
initGrid() {
|
|
1331
|
+
this.nodeGrid = [];
|
|
1332
|
+
for (let i = 0; i < this.width; i++) {
|
|
1333
|
+
this.nodeGrid[i] = [];
|
|
1334
|
+
}
|
|
1335
|
+
}
|
|
1336
|
+
/**
|
|
1337
|
+
* @zh 获取或创建节点
|
|
1338
|
+
* @en Get or create node
|
|
1339
|
+
*/
|
|
1340
|
+
getOrCreateNode(x, y) {
|
|
1341
|
+
const xi = x | 0;
|
|
1342
|
+
const yi = y | 0;
|
|
1343
|
+
if (xi < 0 || xi >= this.width || yi < 0 || yi >= this.height) {
|
|
1344
|
+
throw new Error("[JPSPathfinder] Invalid grid coordinates");
|
|
1345
|
+
}
|
|
1346
|
+
if (!this.nodeGrid[xi]) {
|
|
1347
|
+
this.nodeGrid[xi] = [];
|
|
1348
|
+
}
|
|
1349
|
+
if (!this.nodeGrid[xi][yi]) {
|
|
1350
|
+
this.nodeGrid[xi][yi] = {
|
|
1351
|
+
x: xi,
|
|
1352
|
+
y: yi,
|
|
1353
|
+
g: Infinity,
|
|
1354
|
+
h: 0,
|
|
1355
|
+
f: Infinity,
|
|
1356
|
+
parent: null,
|
|
1357
|
+
closed: false
|
|
1358
|
+
};
|
|
1359
|
+
}
|
|
1360
|
+
return this.nodeGrid[xi][yi];
|
|
1361
|
+
}
|
|
1362
|
+
/**
|
|
1363
|
+
* @zh 启发式函数(八方向距离)
|
|
1364
|
+
* @en Heuristic function (octile distance)
|
|
1365
|
+
*/
|
|
1366
|
+
heuristic(x1, y1, x2, y2) {
|
|
1367
|
+
const dx = Math.abs(x1 - x2);
|
|
1368
|
+
const dy = Math.abs(y1 - y2);
|
|
1369
|
+
return dx + dy + (Math.SQRT2 - 2) * Math.min(dx, dy);
|
|
1370
|
+
}
|
|
1371
|
+
/**
|
|
1372
|
+
* @zh 识别后继节点(跳跃点)
|
|
1373
|
+
* @en Identify successors (jump points)
|
|
1374
|
+
*/
|
|
1375
|
+
identifySuccessors(node, endX, endY, opts) {
|
|
1376
|
+
const neighbors = this.findNeighbors(node);
|
|
1377
|
+
for (const neighbor of neighbors) {
|
|
1378
|
+
const jumpPoint = this.jump(neighbor.x, neighbor.y, node.x, node.y, endX, endY);
|
|
1379
|
+
if (jumpPoint) {
|
|
1380
|
+
const jx = jumpPoint.x;
|
|
1381
|
+
const jy = jumpPoint.y;
|
|
1382
|
+
const jpNode = this.getOrCreateNode(jx, jy);
|
|
1383
|
+
if (jpNode.closed) continue;
|
|
1384
|
+
const dx = Math.abs(jx - node.x);
|
|
1385
|
+
const dy = Math.abs(jy - node.y);
|
|
1386
|
+
const distance = Math.sqrt(dx * dx + dy * dy);
|
|
1387
|
+
const tentativeG = node.g + distance;
|
|
1388
|
+
if (tentativeG < jpNode.g) {
|
|
1389
|
+
jpNode.g = tentativeG;
|
|
1390
|
+
jpNode.h = this.heuristic(jx, jy, endX, endY) * opts.heuristicWeight;
|
|
1391
|
+
jpNode.f = jpNode.g + jpNode.h;
|
|
1392
|
+
jpNode.parent = node;
|
|
1393
|
+
if (!this.openList.contains(jpNode)) {
|
|
1394
|
+
this.openList.push(jpNode);
|
|
1395
|
+
} else {
|
|
1396
|
+
this.openList.update(jpNode);
|
|
1397
|
+
}
|
|
1398
|
+
}
|
|
1399
|
+
}
|
|
1400
|
+
}
|
|
1401
|
+
}
|
|
1402
|
+
/**
|
|
1403
|
+
* @zh 查找邻居(根据父节点方向剪枝)
|
|
1404
|
+
* @en Find neighbors (pruned based on parent direction)
|
|
1405
|
+
*/
|
|
1406
|
+
findNeighbors(node) {
|
|
1407
|
+
const { x, y, parent } = node;
|
|
1408
|
+
const neighbors = [];
|
|
1409
|
+
if (!parent) {
|
|
1410
|
+
for (let dx2 = -1; dx2 <= 1; dx2++) {
|
|
1411
|
+
for (let dy2 = -1; dy2 <= 1; dy2++) {
|
|
1412
|
+
if (dx2 === 0 && dy2 === 0) continue;
|
|
1413
|
+
const nx = x + dx2;
|
|
1414
|
+
const ny = y + dy2;
|
|
1415
|
+
if (this.isWalkableAt(nx, ny)) {
|
|
1416
|
+
if (dx2 !== 0 && dy2 !== 0) {
|
|
1417
|
+
if (this.isWalkableAt(x + dx2, y) || this.isWalkableAt(x, y + dy2)) {
|
|
1418
|
+
neighbors.push({
|
|
1419
|
+
x: nx,
|
|
1420
|
+
y: ny
|
|
1421
|
+
});
|
|
1422
|
+
}
|
|
1423
|
+
} else {
|
|
1424
|
+
neighbors.push({
|
|
1425
|
+
x: nx,
|
|
1426
|
+
y: ny
|
|
1427
|
+
});
|
|
1428
|
+
}
|
|
1429
|
+
}
|
|
1430
|
+
}
|
|
1431
|
+
}
|
|
1432
|
+
return neighbors;
|
|
1433
|
+
}
|
|
1434
|
+
const dx = Math.sign(x - parent.x);
|
|
1435
|
+
const dy = Math.sign(y - parent.y);
|
|
1436
|
+
if (dx !== 0 && dy !== 0) {
|
|
1437
|
+
if (this.isWalkableAt(x, y + dy)) {
|
|
1438
|
+
neighbors.push({
|
|
1439
|
+
x,
|
|
1440
|
+
y: y + dy
|
|
1441
|
+
});
|
|
1442
|
+
}
|
|
1443
|
+
if (this.isWalkableAt(x + dx, y)) {
|
|
1444
|
+
neighbors.push({
|
|
1445
|
+
x: x + dx,
|
|
1446
|
+
y
|
|
1447
|
+
});
|
|
1448
|
+
}
|
|
1449
|
+
if (this.isWalkableAt(x, y + dy) || this.isWalkableAt(x + dx, y)) {
|
|
1450
|
+
if (this.isWalkableAt(x + dx, y + dy)) {
|
|
1451
|
+
neighbors.push({
|
|
1452
|
+
x: x + dx,
|
|
1453
|
+
y: y + dy
|
|
1454
|
+
});
|
|
1455
|
+
}
|
|
1456
|
+
}
|
|
1457
|
+
if (!this.isWalkableAt(x - dx, y) && this.isWalkableAt(x, y + dy)) {
|
|
1458
|
+
if (this.isWalkableAt(x - dx, y + dy)) {
|
|
1459
|
+
neighbors.push({
|
|
1460
|
+
x: x - dx,
|
|
1461
|
+
y: y + dy
|
|
1462
|
+
});
|
|
1463
|
+
}
|
|
1464
|
+
}
|
|
1465
|
+
if (!this.isWalkableAt(x, y - dy) && this.isWalkableAt(x + dx, y)) {
|
|
1466
|
+
if (this.isWalkableAt(x + dx, y - dy)) {
|
|
1467
|
+
neighbors.push({
|
|
1468
|
+
x: x + dx,
|
|
1469
|
+
y: y - dy
|
|
1470
|
+
});
|
|
1471
|
+
}
|
|
1472
|
+
}
|
|
1473
|
+
} else if (dx !== 0) {
|
|
1474
|
+
if (this.isWalkableAt(x + dx, y)) {
|
|
1475
|
+
neighbors.push({
|
|
1476
|
+
x: x + dx,
|
|
1477
|
+
y
|
|
1478
|
+
});
|
|
1479
|
+
if (!this.isWalkableAt(x, y + 1) && this.isWalkableAt(x + dx, y + 1)) {
|
|
1480
|
+
neighbors.push({
|
|
1481
|
+
x: x + dx,
|
|
1482
|
+
y: y + 1
|
|
1483
|
+
});
|
|
1484
|
+
}
|
|
1485
|
+
if (!this.isWalkableAt(x, y - 1) && this.isWalkableAt(x + dx, y - 1)) {
|
|
1486
|
+
neighbors.push({
|
|
1487
|
+
x: x + dx,
|
|
1488
|
+
y: y - 1
|
|
1489
|
+
});
|
|
1490
|
+
}
|
|
1491
|
+
}
|
|
1492
|
+
} else if (dy !== 0) {
|
|
1493
|
+
if (this.isWalkableAt(x, y + dy)) {
|
|
1494
|
+
neighbors.push({
|
|
1495
|
+
x,
|
|
1496
|
+
y: y + dy
|
|
1497
|
+
});
|
|
1498
|
+
if (!this.isWalkableAt(x + 1, y) && this.isWalkableAt(x + 1, y + dy)) {
|
|
1499
|
+
neighbors.push({
|
|
1500
|
+
x: x + 1,
|
|
1501
|
+
y: y + dy
|
|
1502
|
+
});
|
|
1503
|
+
}
|
|
1504
|
+
if (!this.isWalkableAt(x - 1, y) && this.isWalkableAt(x - 1, y + dy)) {
|
|
1505
|
+
neighbors.push({
|
|
1506
|
+
x: x - 1,
|
|
1507
|
+
y: y + dy
|
|
1508
|
+
});
|
|
1509
|
+
}
|
|
1510
|
+
}
|
|
1511
|
+
}
|
|
1512
|
+
return neighbors;
|
|
1513
|
+
}
|
|
1514
|
+
/**
|
|
1515
|
+
* @zh 跳跃函数(迭代版本,避免递归开销)
|
|
1516
|
+
* @en Jump function (iterative version to avoid recursion overhead)
|
|
1517
|
+
*/
|
|
1518
|
+
jump(startX, startY, px, py, endX, endY) {
|
|
1519
|
+
const dx = startX - px;
|
|
1520
|
+
const dy = startY - py;
|
|
1521
|
+
let x = startX;
|
|
1522
|
+
let y = startY;
|
|
1523
|
+
while (true) {
|
|
1524
|
+
if (!this.isWalkableAt(x, y)) {
|
|
1525
|
+
return null;
|
|
1526
|
+
}
|
|
1527
|
+
if (x === endX && y === endY) {
|
|
1528
|
+
return {
|
|
1529
|
+
x,
|
|
1530
|
+
y
|
|
1531
|
+
};
|
|
1532
|
+
}
|
|
1533
|
+
if (dx !== 0 && dy !== 0) {
|
|
1534
|
+
if (this.isWalkableAt(x - dx, y + dy) && !this.isWalkableAt(x - dx, y) || this.isWalkableAt(x + dx, y - dy) && !this.isWalkableAt(x, y - dy)) {
|
|
1535
|
+
return {
|
|
1536
|
+
x,
|
|
1537
|
+
y
|
|
1538
|
+
};
|
|
1539
|
+
}
|
|
1540
|
+
if (this.jumpStraight(x + dx, y, dx, 0, endX, endY) || this.jumpStraight(x, y + dy, 0, dy, endX, endY)) {
|
|
1541
|
+
return {
|
|
1542
|
+
x,
|
|
1543
|
+
y
|
|
1544
|
+
};
|
|
1545
|
+
}
|
|
1546
|
+
if (!this.isWalkableAt(x + dx, y) && !this.isWalkableAt(x, y + dy)) {
|
|
1547
|
+
return null;
|
|
1548
|
+
}
|
|
1549
|
+
} else if (dx !== 0) {
|
|
1550
|
+
if (this.isWalkableAt(x + dx, y + 1) && !this.isWalkableAt(x, y + 1) || this.isWalkableAt(x + dx, y - 1) && !this.isWalkableAt(x, y - 1)) {
|
|
1551
|
+
return {
|
|
1552
|
+
x,
|
|
1553
|
+
y
|
|
1554
|
+
};
|
|
1555
|
+
}
|
|
1556
|
+
} else if (dy !== 0) {
|
|
1557
|
+
if (this.isWalkableAt(x + 1, y + dy) && !this.isWalkableAt(x + 1, y) || this.isWalkableAt(x - 1, y + dy) && !this.isWalkableAt(x - 1, y)) {
|
|
1558
|
+
return {
|
|
1559
|
+
x,
|
|
1560
|
+
y
|
|
1561
|
+
};
|
|
1562
|
+
}
|
|
1563
|
+
}
|
|
1564
|
+
x += dx;
|
|
1565
|
+
y += dy;
|
|
1566
|
+
}
|
|
1567
|
+
}
|
|
1568
|
+
/**
|
|
1569
|
+
* @zh 直线跳跃(水平或垂直方向)
|
|
1570
|
+
* @en Straight jump (horizontal or vertical direction)
|
|
1571
|
+
*/
|
|
1572
|
+
jumpStraight(startX, startY, dx, dy, endX, endY) {
|
|
1573
|
+
let x = startX;
|
|
1574
|
+
let y = startY;
|
|
1575
|
+
while (true) {
|
|
1576
|
+
if (!this.isWalkableAt(x, y)) {
|
|
1577
|
+
return false;
|
|
1578
|
+
}
|
|
1579
|
+
if (x === endX && y === endY) {
|
|
1580
|
+
return true;
|
|
1581
|
+
}
|
|
1582
|
+
if (dx !== 0) {
|
|
1583
|
+
if (this.isWalkableAt(x + dx, y + 1) && !this.isWalkableAt(x, y + 1) || this.isWalkableAt(x + dx, y - 1) && !this.isWalkableAt(x, y - 1)) {
|
|
1584
|
+
return true;
|
|
1585
|
+
}
|
|
1586
|
+
} else if (dy !== 0) {
|
|
1587
|
+
if (this.isWalkableAt(x + 1, y + dy) && !this.isWalkableAt(x + 1, y) || this.isWalkableAt(x - 1, y + dy) && !this.isWalkableAt(x - 1, y)) {
|
|
1588
|
+
return true;
|
|
1589
|
+
}
|
|
1590
|
+
}
|
|
1591
|
+
x += dx;
|
|
1592
|
+
y += dy;
|
|
1593
|
+
}
|
|
1594
|
+
}
|
|
1595
|
+
/**
|
|
1596
|
+
* @zh 检查位置是否可通行
|
|
1597
|
+
* @en Check if position is walkable
|
|
1598
|
+
*/
|
|
1599
|
+
isWalkableAt(x, y) {
|
|
1600
|
+
if (x < 0 || x >= this.width || y < 0 || y >= this.height) {
|
|
1601
|
+
return false;
|
|
1602
|
+
}
|
|
1603
|
+
return this.map.isWalkable(x, y);
|
|
1604
|
+
}
|
|
1605
|
+
/**
|
|
1606
|
+
* @zh 构建路径
|
|
1607
|
+
* @en Build path
|
|
1608
|
+
*/
|
|
1609
|
+
buildPath(endNode) {
|
|
1610
|
+
const path = [];
|
|
1611
|
+
let current = endNode;
|
|
1612
|
+
while (current) {
|
|
1613
|
+
path.unshift({
|
|
1614
|
+
x: current.x,
|
|
1615
|
+
y: current.y
|
|
1616
|
+
});
|
|
1617
|
+
current = current.parent;
|
|
1618
|
+
}
|
|
1619
|
+
return this.interpolatePath(path);
|
|
1620
|
+
}
|
|
1621
|
+
/**
|
|
1622
|
+
* @zh 插值路径(在跳跃点之间填充中间点)
|
|
1623
|
+
* @en Interpolate path (fill intermediate points between jump points)
|
|
1624
|
+
*/
|
|
1625
|
+
interpolatePath(jumpPoints) {
|
|
1626
|
+
if (jumpPoints.length < 2) {
|
|
1627
|
+
return jumpPoints;
|
|
1628
|
+
}
|
|
1629
|
+
const path = [
|
|
1630
|
+
jumpPoints[0]
|
|
1631
|
+
];
|
|
1632
|
+
for (let i = 1; i < jumpPoints.length; i++) {
|
|
1633
|
+
const prev = jumpPoints[i - 1];
|
|
1634
|
+
const curr = jumpPoints[i];
|
|
1635
|
+
const dx = curr.x - prev.x;
|
|
1636
|
+
const dy = curr.y - prev.y;
|
|
1637
|
+
const steps = Math.max(Math.abs(dx), Math.abs(dy));
|
|
1638
|
+
const stepX = dx === 0 ? 0 : dx / Math.abs(dx);
|
|
1639
|
+
const stepY = dy === 0 ? 0 : dy / Math.abs(dy);
|
|
1640
|
+
let x = prev.x;
|
|
1641
|
+
let y = prev.y;
|
|
1642
|
+
for (let j = 0; j < steps; j++) {
|
|
1643
|
+
if (x !== curr.x && y !== curr.y) {
|
|
1644
|
+
x += stepX;
|
|
1645
|
+
y += stepY;
|
|
1646
|
+
} else if (x !== curr.x) {
|
|
1647
|
+
x += stepX;
|
|
1648
|
+
} else if (y !== curr.y) {
|
|
1649
|
+
y += stepY;
|
|
1650
|
+
}
|
|
1651
|
+
if (x !== prev.x || y !== prev.y) {
|
|
1652
|
+
path.push({
|
|
1653
|
+
x,
|
|
1654
|
+
y
|
|
1655
|
+
});
|
|
1656
|
+
}
|
|
1657
|
+
}
|
|
1658
|
+
}
|
|
1659
|
+
return path;
|
|
1660
|
+
}
|
|
1661
|
+
};
|
|
1662
|
+
__name(_JPSPathfinder, "JPSPathfinder");
|
|
1663
|
+
var JPSPathfinder = _JPSPathfinder;
|
|
1664
|
+
function createJPSPathfinder(map) {
|
|
1665
|
+
return new JPSPathfinder(map);
|
|
1666
|
+
}
|
|
1667
|
+
__name(createJPSPathfinder, "createJPSPathfinder");
|
|
1668
|
+
|
|
1669
|
+
// src/core/HPAPathfinder.ts
|
|
1670
|
+
var DEFAULT_HPA_CONFIG = {
|
|
1671
|
+
clusterSize: 64,
|
|
1672
|
+
maxEntranceWidth: 16,
|
|
1673
|
+
cacheInternalPaths: true,
|
|
1674
|
+
entranceStrategy: "end",
|
|
1675
|
+
lazyIntraEdges: true
|
|
1676
|
+
};
|
|
1677
|
+
var _a;
|
|
1678
|
+
var SubMap = (_a = class {
|
|
1679
|
+
constructor(parentMap, originX, originY, width, height) {
|
|
1680
|
+
__publicField(this, "parentMap");
|
|
1681
|
+
__publicField(this, "originX");
|
|
1682
|
+
__publicField(this, "originY");
|
|
1683
|
+
__publicField(this, "width");
|
|
1684
|
+
__publicField(this, "height");
|
|
1685
|
+
this.parentMap = parentMap;
|
|
1686
|
+
this.originX = originX;
|
|
1687
|
+
this.originY = originY;
|
|
1688
|
+
this.width = width;
|
|
1689
|
+
this.height = height;
|
|
1690
|
+
}
|
|
1691
|
+
/**
|
|
1692
|
+
* @zh 局部坐标转全局坐标
|
|
1693
|
+
* @en Convert local to global coordinates
|
|
1694
|
+
*/
|
|
1695
|
+
localToGlobal(localX, localY) {
|
|
1696
|
+
return {
|
|
1697
|
+
x: this.originX + localX,
|
|
1698
|
+
y: this.originY + localY
|
|
1699
|
+
};
|
|
1700
|
+
}
|
|
1701
|
+
/**
|
|
1702
|
+
* @zh 全局坐标转局部坐标
|
|
1703
|
+
* @en Convert global to local coordinates
|
|
1704
|
+
*/
|
|
1705
|
+
globalToLocal(globalX, globalY) {
|
|
1706
|
+
return {
|
|
1707
|
+
x: globalX - this.originX,
|
|
1708
|
+
y: globalY - this.originY
|
|
1709
|
+
};
|
|
1710
|
+
}
|
|
1711
|
+
isWalkable(x, y) {
|
|
1712
|
+
if (x < 0 || x >= this.width || y < 0 || y >= this.height) {
|
|
1713
|
+
return false;
|
|
1714
|
+
}
|
|
1715
|
+
return this.parentMap.isWalkable(this.originX + x, this.originY + y);
|
|
1716
|
+
}
|
|
1717
|
+
getNodeAt(x, y) {
|
|
1718
|
+
if (x < 0 || x >= this.width || y < 0 || y >= this.height) {
|
|
1719
|
+
return null;
|
|
1720
|
+
}
|
|
1721
|
+
const globalNode = this.parentMap.getNodeAt(this.originX + x, this.originY + y);
|
|
1722
|
+
if (!globalNode) return null;
|
|
1723
|
+
return {
|
|
1724
|
+
id: y * this.width + x,
|
|
1725
|
+
position: {
|
|
1726
|
+
x,
|
|
1727
|
+
y
|
|
1728
|
+
},
|
|
1729
|
+
cost: globalNode.cost,
|
|
1730
|
+
walkable: globalNode.walkable
|
|
1731
|
+
};
|
|
1732
|
+
}
|
|
1733
|
+
getNeighbors(node) {
|
|
1734
|
+
const neighbors = [];
|
|
1735
|
+
const { x, y } = node.position;
|
|
1736
|
+
const directions = [
|
|
1737
|
+
{
|
|
1738
|
+
dx: 0,
|
|
1739
|
+
dy: -1
|
|
1740
|
+
},
|
|
1741
|
+
{
|
|
1742
|
+
dx: 1,
|
|
1743
|
+
dy: -1
|
|
1744
|
+
},
|
|
1745
|
+
{
|
|
1746
|
+
dx: 1,
|
|
1747
|
+
dy: 0
|
|
1748
|
+
},
|
|
1749
|
+
{
|
|
1750
|
+
dx: 1,
|
|
1751
|
+
dy: 1
|
|
1752
|
+
},
|
|
1753
|
+
{
|
|
1754
|
+
dx: 0,
|
|
1755
|
+
dy: 1
|
|
1756
|
+
},
|
|
1757
|
+
{
|
|
1758
|
+
dx: -1,
|
|
1759
|
+
dy: 1
|
|
1760
|
+
},
|
|
1761
|
+
{
|
|
1762
|
+
dx: -1,
|
|
1763
|
+
dy: 0
|
|
1764
|
+
},
|
|
1765
|
+
{
|
|
1766
|
+
dx: -1,
|
|
1767
|
+
dy: -1
|
|
1768
|
+
}
|
|
1769
|
+
// NW
|
|
1770
|
+
];
|
|
1771
|
+
for (const dir of directions) {
|
|
1772
|
+
const nx = x + dir.dx;
|
|
1773
|
+
const ny = y + dir.dy;
|
|
1774
|
+
if (nx < 0 || nx >= this.width || ny < 0 || ny >= this.height) {
|
|
1775
|
+
continue;
|
|
1776
|
+
}
|
|
1777
|
+
if (!this.isWalkable(nx, ny)) {
|
|
1778
|
+
continue;
|
|
1779
|
+
}
|
|
1780
|
+
if (dir.dx !== 0 && dir.dy !== 0) {
|
|
1781
|
+
if (!this.isWalkable(x + dir.dx, y) || !this.isWalkable(x, y + dir.dy)) {
|
|
1782
|
+
continue;
|
|
1783
|
+
}
|
|
1784
|
+
}
|
|
1785
|
+
const neighborNode = this.getNodeAt(nx, ny);
|
|
1786
|
+
if (neighborNode) {
|
|
1787
|
+
neighbors.push(neighborNode);
|
|
1788
|
+
}
|
|
1789
|
+
}
|
|
1790
|
+
return neighbors;
|
|
1791
|
+
}
|
|
1792
|
+
heuristic(a, b) {
|
|
1793
|
+
const dx = Math.abs(a.x - b.x);
|
|
1794
|
+
const dy = Math.abs(a.y - b.y);
|
|
1795
|
+
return dx + dy + (Math.SQRT2 - 2) * Math.min(dx, dy);
|
|
1796
|
+
}
|
|
1797
|
+
getMovementCost(from, to) {
|
|
1798
|
+
const dx = Math.abs(to.position.x - from.position.x);
|
|
1799
|
+
const dy = Math.abs(to.position.y - from.position.y);
|
|
1800
|
+
const baseCost = dx !== 0 && dy !== 0 ? Math.SQRT2 : 1;
|
|
1801
|
+
return baseCost * to.cost;
|
|
1802
|
+
}
|
|
1803
|
+
}, __name(_a, "SubMap"), _a);
|
|
1804
|
+
var _a2;
|
|
1805
|
+
var Cluster = (_a2 = class {
|
|
1806
|
+
constructor(id, originX, originY, width, height, parentMap) {
|
|
1807
|
+
__publicField(this, "id");
|
|
1808
|
+
__publicField(this, "originX");
|
|
1809
|
+
__publicField(this, "originY");
|
|
1810
|
+
__publicField(this, "width");
|
|
1811
|
+
__publicField(this, "height");
|
|
1812
|
+
__publicField(this, "subMap");
|
|
1813
|
+
/** @zh 集群内的抽象节点 ID 列表 @en Abstract node IDs in this cluster */
|
|
1814
|
+
__publicField(this, "nodeIds", []);
|
|
1815
|
+
/** @zh 预计算的距离缓存 @en Precomputed distance cache */
|
|
1816
|
+
__publicField(this, "distanceCache", /* @__PURE__ */ new Map());
|
|
1817
|
+
/** @zh 预计算的路径缓存 @en Precomputed path cache */
|
|
1818
|
+
__publicField(this, "pathCache", /* @__PURE__ */ new Map());
|
|
1819
|
+
this.id = id;
|
|
1820
|
+
this.originX = originX;
|
|
1821
|
+
this.originY = originY;
|
|
1822
|
+
this.width = width;
|
|
1823
|
+
this.height = height;
|
|
1824
|
+
this.subMap = new SubMap(parentMap, originX, originY, width, height);
|
|
1825
|
+
}
|
|
1826
|
+
/**
|
|
1827
|
+
* @zh 检查点是否在集群内
|
|
1828
|
+
* @en Check if point is in cluster
|
|
1829
|
+
*/
|
|
1830
|
+
containsPoint(x, y) {
|
|
1831
|
+
return x >= this.originX && x < this.originX + this.width && y >= this.originY && y < this.originY + this.height;
|
|
1832
|
+
}
|
|
1833
|
+
/**
|
|
1834
|
+
* @zh 添加节点 ID
|
|
1835
|
+
* @en Add node ID
|
|
1836
|
+
*/
|
|
1837
|
+
addNodeId(nodeId) {
|
|
1838
|
+
if (!this.nodeIds.includes(nodeId)) {
|
|
1839
|
+
this.nodeIds.push(nodeId);
|
|
1840
|
+
}
|
|
1841
|
+
}
|
|
1842
|
+
/**
|
|
1843
|
+
* @zh 移除节点 ID
|
|
1844
|
+
* @en Remove node ID
|
|
1845
|
+
*/
|
|
1846
|
+
removeNodeId(nodeId) {
|
|
1847
|
+
const idx = this.nodeIds.indexOf(nodeId);
|
|
1848
|
+
if (idx !== -1) {
|
|
1849
|
+
this.nodeIds.splice(idx, 1);
|
|
1850
|
+
}
|
|
1851
|
+
}
|
|
1852
|
+
/**
|
|
1853
|
+
* @zh 生成缓存键
|
|
1854
|
+
* @en Generate cache key
|
|
1855
|
+
*/
|
|
1856
|
+
getCacheKey(fromId, toId) {
|
|
1857
|
+
return `${fromId}->${toId}`;
|
|
1858
|
+
}
|
|
1859
|
+
/**
|
|
1860
|
+
* @zh 设置缓存
|
|
1861
|
+
* @en Set cache
|
|
1862
|
+
*/
|
|
1863
|
+
setCache(fromId, toId, cost, path) {
|
|
1864
|
+
const key = this.getCacheKey(fromId, toId);
|
|
1865
|
+
this.distanceCache.set(key, cost);
|
|
1866
|
+
this.pathCache.set(key, path);
|
|
1867
|
+
}
|
|
1868
|
+
/**
|
|
1869
|
+
* @zh 获取缓存的距离
|
|
1870
|
+
* @en Get cached distance
|
|
1871
|
+
*/
|
|
1872
|
+
getCachedDistance(fromId, toId) {
|
|
1873
|
+
return this.distanceCache.get(this.getCacheKey(fromId, toId));
|
|
1874
|
+
}
|
|
1875
|
+
/**
|
|
1876
|
+
* @zh 获取缓存的路径
|
|
1877
|
+
* @en Get cached path
|
|
1878
|
+
*/
|
|
1879
|
+
getCachedPath(fromId, toId) {
|
|
1880
|
+
return this.pathCache.get(this.getCacheKey(fromId, toId));
|
|
1881
|
+
}
|
|
1882
|
+
/**
|
|
1883
|
+
* @zh 清除缓存
|
|
1884
|
+
* @en Clear cache
|
|
1885
|
+
*/
|
|
1886
|
+
clearCache() {
|
|
1887
|
+
this.distanceCache.clear();
|
|
1888
|
+
this.pathCache.clear();
|
|
1889
|
+
}
|
|
1890
|
+
/**
|
|
1891
|
+
* @zh 获取缓存大小
|
|
1892
|
+
* @en Get cache size
|
|
1893
|
+
*/
|
|
1894
|
+
getCacheSize() {
|
|
1895
|
+
return this.distanceCache.size;
|
|
1896
|
+
}
|
|
1897
|
+
}, __name(_a2, "Cluster"), _a2);
|
|
1898
|
+
var _HPAPathfinder = class _HPAPathfinder {
|
|
1899
|
+
constructor(map, config) {
|
|
1900
|
+
__publicField(this, "map");
|
|
1901
|
+
__publicField(this, "config");
|
|
1902
|
+
__publicField(this, "mapWidth");
|
|
1903
|
+
__publicField(this, "mapHeight");
|
|
1904
|
+
// 集群管理
|
|
1905
|
+
__publicField(this, "clusters", []);
|
|
1906
|
+
__publicField(this, "clusterGrid", []);
|
|
1907
|
+
__publicField(this, "clustersX", 0);
|
|
1908
|
+
__publicField(this, "clustersY", 0);
|
|
1909
|
+
// 抽象图
|
|
1910
|
+
__publicField(this, "abstractNodes", /* @__PURE__ */ new Map());
|
|
1911
|
+
__publicField(this, "nodesByCluster", /* @__PURE__ */ new Map());
|
|
1912
|
+
__publicField(this, "nextNodeId", 0);
|
|
1913
|
+
// 入口统计
|
|
1914
|
+
__publicField(this, "entranceCount", 0);
|
|
1915
|
+
// 内部寻路器
|
|
1916
|
+
__publicField(this, "localPathfinder");
|
|
1917
|
+
// 完整路径缓存
|
|
1918
|
+
__publicField(this, "pathCache");
|
|
1919
|
+
__publicField(this, "mapVersion", 0);
|
|
1920
|
+
__publicField(this, "preprocessed", false);
|
|
1921
|
+
this.map = map;
|
|
1922
|
+
this.config = {
|
|
1923
|
+
...DEFAULT_HPA_CONFIG,
|
|
1924
|
+
...config
|
|
1925
|
+
};
|
|
1926
|
+
const bounds = this.getMapBounds();
|
|
1927
|
+
this.mapWidth = bounds.width;
|
|
1928
|
+
this.mapHeight = bounds.height;
|
|
1929
|
+
this.localPathfinder = new AStarPathfinder(map);
|
|
1930
|
+
this.pathCache = new PathCache({
|
|
1931
|
+
maxEntries: 1e3,
|
|
1932
|
+
ttlMs: 0
|
|
1933
|
+
});
|
|
1934
|
+
}
|
|
1935
|
+
// =========================================================================
|
|
1936
|
+
// 公共 API | Public API
|
|
1937
|
+
// =========================================================================
|
|
1938
|
+
/**
|
|
1939
|
+
* @zh 预处理地图(构建抽象图)
|
|
1940
|
+
* @en Preprocess map (build abstract graph)
|
|
1941
|
+
*/
|
|
1942
|
+
preprocess() {
|
|
1943
|
+
this.clear();
|
|
1944
|
+
this.buildClusters();
|
|
1945
|
+
this.buildEntrances();
|
|
1946
|
+
this.buildIntraEdges();
|
|
1947
|
+
this.preprocessed = true;
|
|
1948
|
+
}
|
|
1949
|
+
/**
|
|
1950
|
+
* @zh 寻找路径
|
|
1951
|
+
* @en Find path
|
|
1952
|
+
*/
|
|
1953
|
+
findPath(startX, startY, endX, endY, options) {
|
|
1954
|
+
if (!this.preprocessed) {
|
|
1955
|
+
this.preprocess();
|
|
1956
|
+
}
|
|
1957
|
+
const opts = {
|
|
1958
|
+
...DEFAULT_PATHFINDING_OPTIONS,
|
|
1959
|
+
...options
|
|
1960
|
+
};
|
|
1961
|
+
if (!this.map.isWalkable(startX, startY) || !this.map.isWalkable(endX, endY)) {
|
|
1962
|
+
return EMPTY_PATH_RESULT;
|
|
1963
|
+
}
|
|
1964
|
+
if (startX === endX && startY === endY) {
|
|
1965
|
+
return {
|
|
1966
|
+
found: true,
|
|
1967
|
+
path: [
|
|
1968
|
+
{
|
|
1969
|
+
x: startX,
|
|
1970
|
+
y: startY
|
|
1971
|
+
}
|
|
1972
|
+
],
|
|
1973
|
+
cost: 0,
|
|
1974
|
+
nodesSearched: 1
|
|
1975
|
+
};
|
|
1976
|
+
}
|
|
1977
|
+
const cached = this.pathCache.get(startX, startY, endX, endY, this.mapVersion);
|
|
1978
|
+
if (cached) {
|
|
1979
|
+
return cached;
|
|
1980
|
+
}
|
|
1981
|
+
const startCluster = this.getClusterAt(startX, startY);
|
|
1982
|
+
const endCluster = this.getClusterAt(endX, endY);
|
|
1983
|
+
if (!startCluster || !endCluster) {
|
|
1984
|
+
return EMPTY_PATH_RESULT;
|
|
1985
|
+
}
|
|
1986
|
+
let result;
|
|
1987
|
+
if (startCluster.id === endCluster.id) {
|
|
1988
|
+
result = this.findLocalPath(startX, startY, endX, endY, opts);
|
|
1989
|
+
} else {
|
|
1990
|
+
const startTemp = this.insertTempNode(startX, startY, startCluster);
|
|
1991
|
+
const endTemp = this.insertTempNode(endX, endY, endCluster);
|
|
1992
|
+
const abstractPath = this.abstractSearch(startTemp, endTemp, opts);
|
|
1993
|
+
this.removeTempNode(startTemp, startCluster);
|
|
1994
|
+
this.removeTempNode(endTemp, endCluster);
|
|
1995
|
+
if (!abstractPath || abstractPath.length === 0) {
|
|
1996
|
+
return EMPTY_PATH_RESULT;
|
|
1997
|
+
}
|
|
1998
|
+
result = this.refinePath(abstractPath, startX, startY, endX, endY, opts);
|
|
1999
|
+
}
|
|
2000
|
+
if (result.found) {
|
|
2001
|
+
this.pathCache.set(startX, startY, endX, endY, result, this.mapVersion);
|
|
2002
|
+
}
|
|
2003
|
+
return result;
|
|
2004
|
+
}
|
|
2005
|
+
/**
|
|
2006
|
+
* @zh 清理状态
|
|
2007
|
+
* @en Clear state
|
|
2008
|
+
*/
|
|
2009
|
+
clear() {
|
|
2010
|
+
this.clusters = [];
|
|
2011
|
+
this.clusterGrid = [];
|
|
2012
|
+
this.abstractNodes.clear();
|
|
2013
|
+
this.nodesByCluster.clear();
|
|
2014
|
+
this.nextNodeId = 0;
|
|
2015
|
+
this.entranceCount = 0;
|
|
2016
|
+
this.pathCache.invalidateAll();
|
|
2017
|
+
this.mapVersion++;
|
|
2018
|
+
this.preprocessed = false;
|
|
2019
|
+
}
|
|
2020
|
+
/**
|
|
2021
|
+
* @zh 通知地图区域变化
|
|
2022
|
+
* @en Notify map region change
|
|
2023
|
+
*/
|
|
2024
|
+
notifyRegionChange(minX, minY, maxX, maxY) {
|
|
2025
|
+
const affectedClusters = this.getAffectedClusters(minX, minY, maxX, maxY);
|
|
2026
|
+
for (const cluster of affectedClusters) {
|
|
2027
|
+
cluster.clearCache();
|
|
2028
|
+
for (const nodeId of cluster.nodeIds) {
|
|
2029
|
+
const node = this.abstractNodes.get(nodeId);
|
|
2030
|
+
if (node) {
|
|
2031
|
+
node.edges = node.edges.filter((e) => e.isInterEdge);
|
|
2032
|
+
}
|
|
2033
|
+
}
|
|
2034
|
+
this.buildClusterIntraEdges(cluster);
|
|
2035
|
+
}
|
|
2036
|
+
this.pathCache.invalidateRegion(minX, minY, maxX, maxY);
|
|
2037
|
+
this.mapVersion++;
|
|
2038
|
+
}
|
|
2039
|
+
/**
|
|
2040
|
+
* @zh 获取预处理统计信息
|
|
2041
|
+
* @en Get preprocessing statistics
|
|
2042
|
+
*/
|
|
2043
|
+
getStats() {
|
|
2044
|
+
let cacheSize = 0;
|
|
2045
|
+
for (const cluster of this.clusters) {
|
|
2046
|
+
cacheSize += cluster.getCacheSize();
|
|
2047
|
+
}
|
|
2048
|
+
return {
|
|
2049
|
+
clusters: this.clusters.length,
|
|
2050
|
+
entrances: this.entranceCount,
|
|
2051
|
+
abstractNodes: this.abstractNodes.size,
|
|
2052
|
+
cacheSize
|
|
2053
|
+
};
|
|
2054
|
+
}
|
|
2055
|
+
// =========================================================================
|
|
2056
|
+
// 预处理方法 | Preprocessing Methods
|
|
2057
|
+
// =========================================================================
|
|
2058
|
+
getMapBounds() {
|
|
2059
|
+
const mapAny = this.map;
|
|
2060
|
+
if (typeof mapAny.width === "number" && typeof mapAny.height === "number") {
|
|
2061
|
+
return {
|
|
2062
|
+
width: mapAny.width,
|
|
2063
|
+
height: mapAny.height
|
|
2064
|
+
};
|
|
2065
|
+
}
|
|
2066
|
+
return {
|
|
2067
|
+
width: 1e3,
|
|
2068
|
+
height: 1e3
|
|
2069
|
+
};
|
|
2070
|
+
}
|
|
2071
|
+
/**
|
|
2072
|
+
* @zh 构建集群
|
|
2073
|
+
* @en Build clusters
|
|
2074
|
+
*/
|
|
2075
|
+
buildClusters() {
|
|
2076
|
+
const clusterSize = this.config.clusterSize;
|
|
2077
|
+
this.clustersX = Math.ceil(this.mapWidth / clusterSize);
|
|
2078
|
+
this.clustersY = Math.ceil(this.mapHeight / clusterSize);
|
|
2079
|
+
this.clusterGrid = [];
|
|
2080
|
+
for (let cx = 0; cx < this.clustersX; cx++) {
|
|
2081
|
+
this.clusterGrid[cx] = [];
|
|
2082
|
+
for (let cy = 0; cy < this.clustersY; cy++) {
|
|
2083
|
+
this.clusterGrid[cx][cy] = null;
|
|
2084
|
+
}
|
|
2085
|
+
}
|
|
2086
|
+
let clusterId = 0;
|
|
2087
|
+
for (let cy = 0; cy < this.clustersY; cy++) {
|
|
2088
|
+
for (let cx = 0; cx < this.clustersX; cx++) {
|
|
2089
|
+
const originX = cx * clusterSize;
|
|
2090
|
+
const originY = cy * clusterSize;
|
|
2091
|
+
const width = Math.min(clusterSize, this.mapWidth - originX);
|
|
2092
|
+
const height = Math.min(clusterSize, this.mapHeight - originY);
|
|
2093
|
+
const cluster = new Cluster(clusterId, originX, originY, width, height, this.map);
|
|
2094
|
+
this.clusters.push(cluster);
|
|
2095
|
+
this.clusterGrid[cx][cy] = clusterId;
|
|
2096
|
+
this.nodesByCluster.set(clusterId, []);
|
|
2097
|
+
clusterId++;
|
|
2098
|
+
}
|
|
2099
|
+
}
|
|
2100
|
+
}
|
|
2101
|
+
/**
|
|
2102
|
+
* @zh 检测入口并创建抽象节点
|
|
2103
|
+
* @en Detect entrances and create abstract nodes
|
|
2104
|
+
*/
|
|
2105
|
+
buildEntrances() {
|
|
2106
|
+
const clusterSize = this.config.clusterSize;
|
|
2107
|
+
for (let cy = 0; cy < this.clustersY; cy++) {
|
|
2108
|
+
for (let cx = 0; cx < this.clustersX; cx++) {
|
|
2109
|
+
const clusterId = this.clusterGrid[cx][cy];
|
|
2110
|
+
if (clusterId === null) continue;
|
|
2111
|
+
const cluster1 = this.clusters[clusterId];
|
|
2112
|
+
if (cx < this.clustersX - 1) {
|
|
2113
|
+
const cluster2Id = this.clusterGrid[cx + 1][cy];
|
|
2114
|
+
if (cluster2Id !== null) {
|
|
2115
|
+
const cluster2 = this.clusters[cluster2Id];
|
|
2116
|
+
this.detectAndCreateEntrances(cluster1, cluster2, "vertical");
|
|
2117
|
+
}
|
|
2118
|
+
}
|
|
2119
|
+
if (cy < this.clustersY - 1) {
|
|
2120
|
+
const cluster2Id = this.clusterGrid[cx][cy + 1];
|
|
2121
|
+
if (cluster2Id !== null) {
|
|
2122
|
+
const cluster2 = this.clusters[cluster2Id];
|
|
2123
|
+
this.detectAndCreateEntrances(cluster1, cluster2, "horizontal");
|
|
2124
|
+
}
|
|
2125
|
+
}
|
|
2126
|
+
}
|
|
2127
|
+
}
|
|
2128
|
+
}
|
|
2129
|
+
/**
|
|
2130
|
+
* @zh 检测并创建两个相邻集群之间的入口
|
|
2131
|
+
* @en Detect and create entrances between two adjacent clusters
|
|
2132
|
+
*/
|
|
2133
|
+
detectAndCreateEntrances(cluster1, cluster2, boundaryDirection) {
|
|
2134
|
+
const spans = this.detectEntranceSpans(cluster1, cluster2, boundaryDirection);
|
|
2135
|
+
for (const span of spans) {
|
|
2136
|
+
this.createEntranceNodes(cluster1, cluster2, span, boundaryDirection);
|
|
2137
|
+
}
|
|
2138
|
+
}
|
|
2139
|
+
/**
|
|
2140
|
+
* @zh 检测边界上的连续可通行区间
|
|
2141
|
+
* @en Detect continuous walkable spans on boundary
|
|
2142
|
+
*/
|
|
2143
|
+
detectEntranceSpans(cluster1, cluster2, boundaryDirection) {
|
|
2144
|
+
const spans = [];
|
|
2145
|
+
if (boundaryDirection === "vertical") {
|
|
2146
|
+
const x1 = cluster1.originX + cluster1.width - 1;
|
|
2147
|
+
const x2 = cluster2.originX;
|
|
2148
|
+
const startY = Math.max(cluster1.originY, cluster2.originY);
|
|
2149
|
+
const endY = Math.min(cluster1.originY + cluster1.height, cluster2.originY + cluster2.height);
|
|
2150
|
+
let spanStart = null;
|
|
2151
|
+
for (let y = startY; y < endY; y++) {
|
|
2152
|
+
const walkable1 = this.map.isWalkable(x1, y);
|
|
2153
|
+
const walkable2 = this.map.isWalkable(x2, y);
|
|
2154
|
+
if (walkable1 && walkable2) {
|
|
2155
|
+
if (spanStart === null) {
|
|
2156
|
+
spanStart = y;
|
|
2157
|
+
}
|
|
2158
|
+
} else {
|
|
2159
|
+
if (spanStart !== null) {
|
|
2160
|
+
spans.push({
|
|
2161
|
+
start: spanStart,
|
|
2162
|
+
end: y - 1
|
|
2163
|
+
});
|
|
2164
|
+
spanStart = null;
|
|
2165
|
+
}
|
|
2166
|
+
}
|
|
2167
|
+
}
|
|
2168
|
+
if (spanStart !== null) {
|
|
2169
|
+
spans.push({
|
|
2170
|
+
start: spanStart,
|
|
2171
|
+
end: endY - 1
|
|
2172
|
+
});
|
|
2173
|
+
}
|
|
2174
|
+
} else {
|
|
2175
|
+
const y1 = cluster1.originY + cluster1.height - 1;
|
|
2176
|
+
const y2 = cluster2.originY;
|
|
2177
|
+
const startX = Math.max(cluster1.originX, cluster2.originX);
|
|
2178
|
+
const endX = Math.min(cluster1.originX + cluster1.width, cluster2.originX + cluster2.width);
|
|
2179
|
+
let spanStart = null;
|
|
2180
|
+
for (let x = startX; x < endX; x++) {
|
|
2181
|
+
const walkable1 = this.map.isWalkable(x, y1);
|
|
2182
|
+
const walkable2 = this.map.isWalkable(x, y2);
|
|
2183
|
+
if (walkable1 && walkable2) {
|
|
2184
|
+
if (spanStart === null) {
|
|
2185
|
+
spanStart = x;
|
|
2186
|
+
}
|
|
2187
|
+
} else {
|
|
2188
|
+
if (spanStart !== null) {
|
|
2189
|
+
spans.push({
|
|
2190
|
+
start: spanStart,
|
|
2191
|
+
end: x - 1
|
|
2192
|
+
});
|
|
2193
|
+
spanStart = null;
|
|
2194
|
+
}
|
|
2195
|
+
}
|
|
2196
|
+
}
|
|
2197
|
+
if (spanStart !== null) {
|
|
2198
|
+
spans.push({
|
|
2199
|
+
start: spanStart,
|
|
2200
|
+
end: endX - 1
|
|
2201
|
+
});
|
|
2202
|
+
}
|
|
2203
|
+
}
|
|
2204
|
+
return spans;
|
|
2205
|
+
}
|
|
2206
|
+
/**
|
|
2207
|
+
* @zh 为入口区间创建抽象节点
|
|
2208
|
+
* @en Create abstract nodes for entrance span
|
|
2209
|
+
*/
|
|
2210
|
+
createEntranceNodes(cluster1, cluster2, span, boundaryDirection) {
|
|
2211
|
+
const spanLength = span.end - span.start + 1;
|
|
2212
|
+
const maxWidth = this.config.maxEntranceWidth;
|
|
2213
|
+
const strategy = this.config.entranceStrategy;
|
|
2214
|
+
const positions = [];
|
|
2215
|
+
if (spanLength <= maxWidth) {
|
|
2216
|
+
positions.push(Math.floor((span.start + span.end) / 2));
|
|
2217
|
+
} else {
|
|
2218
|
+
const numNodes = Math.ceil(spanLength / maxWidth);
|
|
2219
|
+
const spacing = spanLength / numNodes;
|
|
2220
|
+
for (let i = 0; i < numNodes; i++) {
|
|
2221
|
+
const pos = Math.floor(span.start + spacing * (i + 0.5));
|
|
2222
|
+
positions.push(Math.min(pos, span.end));
|
|
2223
|
+
}
|
|
2224
|
+
if (strategy === "end") {
|
|
2225
|
+
if (!positions.includes(span.start)) {
|
|
2226
|
+
positions.unshift(span.start);
|
|
2227
|
+
}
|
|
2228
|
+
if (!positions.includes(span.end)) {
|
|
2229
|
+
positions.push(span.end);
|
|
2230
|
+
}
|
|
2231
|
+
}
|
|
2232
|
+
}
|
|
2233
|
+
for (const pos of positions) {
|
|
2234
|
+
let p1, p2;
|
|
2235
|
+
if (boundaryDirection === "vertical") {
|
|
2236
|
+
p1 = {
|
|
2237
|
+
x: cluster1.originX + cluster1.width - 1,
|
|
2238
|
+
y: pos
|
|
2239
|
+
};
|
|
2240
|
+
p2 = {
|
|
2241
|
+
x: cluster2.originX,
|
|
2242
|
+
y: pos
|
|
2243
|
+
};
|
|
2244
|
+
} else {
|
|
2245
|
+
p1 = {
|
|
2246
|
+
x: pos,
|
|
2247
|
+
y: cluster1.originY + cluster1.height - 1
|
|
2248
|
+
};
|
|
2249
|
+
p2 = {
|
|
2250
|
+
x: pos,
|
|
2251
|
+
y: cluster2.originY
|
|
2252
|
+
};
|
|
2253
|
+
}
|
|
2254
|
+
const node1 = this.createAbstractNode(p1, cluster1);
|
|
2255
|
+
const node2 = this.createAbstractNode(p2, cluster2);
|
|
2256
|
+
const interCost = 1;
|
|
2257
|
+
node1.edges.push({
|
|
2258
|
+
targetNodeId: node2.id,
|
|
2259
|
+
cost: interCost,
|
|
2260
|
+
isInterEdge: true,
|
|
2261
|
+
innerPath: null
|
|
2262
|
+
});
|
|
2263
|
+
node2.edges.push({
|
|
2264
|
+
targetNodeId: node1.id,
|
|
2265
|
+
cost: interCost,
|
|
2266
|
+
isInterEdge: true,
|
|
2267
|
+
innerPath: null
|
|
2268
|
+
});
|
|
2269
|
+
this.entranceCount++;
|
|
2270
|
+
}
|
|
2271
|
+
}
|
|
2272
|
+
/**
|
|
2273
|
+
* @zh 创建抽象节点
|
|
2274
|
+
* @en Create abstract node
|
|
2275
|
+
*/
|
|
2276
|
+
createAbstractNode(position, cluster) {
|
|
2277
|
+
const concreteId = position.y * this.mapWidth + position.x;
|
|
2278
|
+
for (const nodeId of cluster.nodeIds) {
|
|
2279
|
+
const existing = this.abstractNodes.get(nodeId);
|
|
2280
|
+
if (existing && existing.concreteNodeId === concreteId) {
|
|
2281
|
+
return existing;
|
|
2282
|
+
}
|
|
2283
|
+
}
|
|
2284
|
+
const node = {
|
|
2285
|
+
id: this.nextNodeId++,
|
|
2286
|
+
position: {
|
|
2287
|
+
x: position.x,
|
|
2288
|
+
y: position.y
|
|
2289
|
+
},
|
|
2290
|
+
clusterId: cluster.id,
|
|
2291
|
+
concreteNodeId: concreteId,
|
|
2292
|
+
edges: []
|
|
2293
|
+
};
|
|
2294
|
+
this.abstractNodes.set(node.id, node);
|
|
2295
|
+
cluster.addNodeId(node.id);
|
|
2296
|
+
const clusterNodes = this.nodesByCluster.get(cluster.id);
|
|
2297
|
+
if (clusterNodes) {
|
|
2298
|
+
clusterNodes.push(node.id);
|
|
2299
|
+
}
|
|
2300
|
+
return node;
|
|
2301
|
+
}
|
|
2302
|
+
/**
|
|
2303
|
+
* @zh 构建所有集群的 intra-edges
|
|
2304
|
+
* @en Build intra-edges for all clusters
|
|
2305
|
+
*/
|
|
2306
|
+
buildIntraEdges() {
|
|
2307
|
+
for (const cluster of this.clusters) {
|
|
2308
|
+
this.buildClusterIntraEdges(cluster);
|
|
2309
|
+
}
|
|
2310
|
+
}
|
|
2311
|
+
/**
|
|
2312
|
+
* @zh 构建单个集群的 intra-edges
|
|
2313
|
+
* @en Build intra-edges for single cluster
|
|
2314
|
+
*/
|
|
2315
|
+
buildClusterIntraEdges(cluster) {
|
|
2316
|
+
const nodeIds = cluster.nodeIds;
|
|
2317
|
+
if (nodeIds.length < 2) return;
|
|
2318
|
+
if (this.config.lazyIntraEdges) {
|
|
2319
|
+
this.buildLazyIntraEdges(cluster);
|
|
2320
|
+
} else {
|
|
2321
|
+
this.buildEagerIntraEdges(cluster);
|
|
2322
|
+
}
|
|
2323
|
+
}
|
|
2324
|
+
/**
|
|
2325
|
+
* @zh 延迟构建 intra-edges(只用启发式距离)
|
|
2326
|
+
* @en Build lazy intra-edges (using heuristic distance only)
|
|
2327
|
+
*/
|
|
2328
|
+
buildLazyIntraEdges(cluster) {
|
|
2329
|
+
const nodeIds = cluster.nodeIds;
|
|
2330
|
+
for (let i = 0; i < nodeIds.length; i++) {
|
|
2331
|
+
for (let j = i + 1; j < nodeIds.length; j++) {
|
|
2332
|
+
const node1 = this.abstractNodes.get(nodeIds[i]);
|
|
2333
|
+
const node2 = this.abstractNodes.get(nodeIds[j]);
|
|
2334
|
+
const heuristicCost = this.heuristic(node1.position, node2.position);
|
|
2335
|
+
node1.edges.push({
|
|
2336
|
+
targetNodeId: node2.id,
|
|
2337
|
+
cost: heuristicCost,
|
|
2338
|
+
isInterEdge: false,
|
|
2339
|
+
innerPath: null
|
|
2340
|
+
// 标记为未计算
|
|
2341
|
+
});
|
|
2342
|
+
node2.edges.push({
|
|
2343
|
+
targetNodeId: node1.id,
|
|
2344
|
+
cost: heuristicCost,
|
|
2345
|
+
isInterEdge: false,
|
|
2346
|
+
innerPath: null
|
|
2347
|
+
});
|
|
2348
|
+
}
|
|
2349
|
+
}
|
|
2350
|
+
}
|
|
2351
|
+
/**
|
|
2352
|
+
* @zh 立即构建 intra-edges(计算真实路径)
|
|
2353
|
+
* @en Build eager intra-edges (compute actual paths)
|
|
2354
|
+
*/
|
|
2355
|
+
buildEagerIntraEdges(cluster) {
|
|
2356
|
+
const nodeIds = cluster.nodeIds;
|
|
2357
|
+
const subPathfinder = new AStarPathfinder(cluster.subMap);
|
|
2358
|
+
for (let i = 0; i < nodeIds.length; i++) {
|
|
2359
|
+
for (let j = i + 1; j < nodeIds.length; j++) {
|
|
2360
|
+
const node1 = this.abstractNodes.get(nodeIds[i]);
|
|
2361
|
+
const node2 = this.abstractNodes.get(nodeIds[j]);
|
|
2362
|
+
const local1 = cluster.subMap.globalToLocal(node1.position.x, node1.position.y);
|
|
2363
|
+
const local2 = cluster.subMap.globalToLocal(node2.position.x, node2.position.y);
|
|
2364
|
+
const result = subPathfinder.findPath(local1.x, local1.y, local2.x, local2.y);
|
|
2365
|
+
if (result.found && result.path.length > 0) {
|
|
2366
|
+
const globalPath = result.path.map((p) => {
|
|
2367
|
+
const global = cluster.subMap.localToGlobal(p.x, p.y);
|
|
2368
|
+
return global.y * this.mapWidth + global.x;
|
|
2369
|
+
});
|
|
2370
|
+
if (this.config.cacheInternalPaths) {
|
|
2371
|
+
cluster.setCache(node1.id, node2.id, result.cost, globalPath);
|
|
2372
|
+
cluster.setCache(node2.id, node1.id, result.cost, [
|
|
2373
|
+
...globalPath
|
|
2374
|
+
].reverse());
|
|
2375
|
+
}
|
|
2376
|
+
node1.edges.push({
|
|
2377
|
+
targetNodeId: node2.id,
|
|
2378
|
+
cost: result.cost,
|
|
2379
|
+
isInterEdge: false,
|
|
2380
|
+
innerPath: this.config.cacheInternalPaths ? globalPath : null
|
|
2381
|
+
});
|
|
2382
|
+
node2.edges.push({
|
|
2383
|
+
targetNodeId: node1.id,
|
|
2384
|
+
cost: result.cost,
|
|
2385
|
+
isInterEdge: false,
|
|
2386
|
+
innerPath: this.config.cacheInternalPaths ? [
|
|
2387
|
+
...globalPath
|
|
2388
|
+
].reverse() : null
|
|
2389
|
+
});
|
|
2390
|
+
}
|
|
2391
|
+
}
|
|
2392
|
+
}
|
|
2393
|
+
}
|
|
2394
|
+
/**
|
|
2395
|
+
* @zh 按需计算 intra-edge 的真实路径
|
|
2396
|
+
* @en Compute actual path for intra-edge on demand
|
|
2397
|
+
*/
|
|
2398
|
+
computeIntraEdgePath(fromNode, toNode, edge) {
|
|
2399
|
+
const cluster = this.clusters[fromNode.clusterId];
|
|
2400
|
+
if (!cluster) return null;
|
|
2401
|
+
const cachedPath = cluster.getCachedPath(fromNode.id, toNode.id);
|
|
2402
|
+
const cachedCost = cluster.getCachedDistance(fromNode.id, toNode.id);
|
|
2403
|
+
if (cachedPath && cachedCost !== void 0) {
|
|
2404
|
+
edge.cost = cachedCost;
|
|
2405
|
+
edge.innerPath = cachedPath;
|
|
2406
|
+
return {
|
|
2407
|
+
cost: cachedCost,
|
|
2408
|
+
path: cachedPath
|
|
2409
|
+
};
|
|
2410
|
+
}
|
|
2411
|
+
const subPathfinder = new AStarPathfinder(cluster.subMap);
|
|
2412
|
+
const local1 = cluster.subMap.globalToLocal(fromNode.position.x, fromNode.position.y);
|
|
2413
|
+
const local2 = cluster.subMap.globalToLocal(toNode.position.x, toNode.position.y);
|
|
2414
|
+
const result = subPathfinder.findPath(local1.x, local1.y, local2.x, local2.y);
|
|
2415
|
+
if (result.found && result.path.length > 0) {
|
|
2416
|
+
const globalPath = result.path.map((p) => {
|
|
2417
|
+
const global = cluster.subMap.localToGlobal(p.x, p.y);
|
|
2418
|
+
return global.y * this.mapWidth + global.x;
|
|
2419
|
+
});
|
|
2420
|
+
if (this.config.cacheInternalPaths) {
|
|
2421
|
+
cluster.setCache(fromNode.id, toNode.id, result.cost, globalPath);
|
|
2422
|
+
cluster.setCache(toNode.id, fromNode.id, result.cost, [
|
|
2423
|
+
...globalPath
|
|
2424
|
+
].reverse());
|
|
2425
|
+
}
|
|
2426
|
+
edge.cost = result.cost;
|
|
2427
|
+
edge.innerPath = globalPath;
|
|
2428
|
+
const reverseEdge = toNode.edges.find((e) => e.targetNodeId === fromNode.id);
|
|
2429
|
+
if (reverseEdge) {
|
|
2430
|
+
reverseEdge.cost = result.cost;
|
|
2431
|
+
reverseEdge.innerPath = [
|
|
2432
|
+
...globalPath
|
|
2433
|
+
].reverse();
|
|
2434
|
+
}
|
|
2435
|
+
return {
|
|
2436
|
+
cost: result.cost,
|
|
2437
|
+
path: globalPath
|
|
2438
|
+
};
|
|
2439
|
+
}
|
|
2440
|
+
return null;
|
|
2441
|
+
}
|
|
2442
|
+
// =========================================================================
|
|
2443
|
+
// 搜索方法 | Search Methods
|
|
2444
|
+
// =========================================================================
|
|
2445
|
+
/**
|
|
2446
|
+
* @zh 获取指定位置的集群
|
|
2447
|
+
* @en Get cluster at position
|
|
2448
|
+
*/
|
|
2449
|
+
getClusterAt(x, y) {
|
|
2450
|
+
const cx = Math.floor(x / this.config.clusterSize);
|
|
2451
|
+
const cy = Math.floor(y / this.config.clusterSize);
|
|
2452
|
+
if (cx < 0 || cx >= this.clustersX || cy < 0 || cy >= this.clustersY) {
|
|
2453
|
+
return null;
|
|
2454
|
+
}
|
|
2455
|
+
const clusterId = this.clusterGrid[cx]?.[cy];
|
|
2456
|
+
if (clusterId === null || clusterId === void 0) {
|
|
2457
|
+
return null;
|
|
2458
|
+
}
|
|
2459
|
+
return this.clusters[clusterId] || null;
|
|
2460
|
+
}
|
|
2461
|
+
/**
|
|
2462
|
+
* @zh 获取受影响的集群
|
|
2463
|
+
* @en Get affected clusters
|
|
2464
|
+
*/
|
|
2465
|
+
getAffectedClusters(minX, minY, maxX, maxY) {
|
|
2466
|
+
const affected = [];
|
|
2467
|
+
const clusterSize = this.config.clusterSize;
|
|
2468
|
+
const minCX = Math.floor(minX / clusterSize);
|
|
2469
|
+
const maxCX = Math.floor(maxX / clusterSize);
|
|
2470
|
+
const minCY = Math.floor(minY / clusterSize);
|
|
2471
|
+
const maxCY = Math.floor(maxY / clusterSize);
|
|
2472
|
+
for (let cy = minCY; cy <= maxCY; cy++) {
|
|
2473
|
+
for (let cx = minCX; cx <= maxCX; cx++) {
|
|
2474
|
+
if (cx >= 0 && cx < this.clustersX && cy >= 0 && cy < this.clustersY) {
|
|
2475
|
+
const clusterId = this.clusterGrid[cx]?.[cy];
|
|
2476
|
+
if (clusterId !== null && clusterId !== void 0) {
|
|
2477
|
+
affected.push(this.clusters[clusterId]);
|
|
2478
|
+
}
|
|
2479
|
+
}
|
|
2480
|
+
}
|
|
2481
|
+
}
|
|
2482
|
+
return affected;
|
|
2483
|
+
}
|
|
2484
|
+
/**
|
|
2485
|
+
* @zh 插入临时节点
|
|
2486
|
+
* @en Insert temporary node
|
|
2487
|
+
*/
|
|
2488
|
+
insertTempNode(x, y, cluster) {
|
|
2489
|
+
const concreteId = y * this.mapWidth + x;
|
|
2490
|
+
for (const nodeId of cluster.nodeIds) {
|
|
2491
|
+
const existing = this.abstractNodes.get(nodeId);
|
|
2492
|
+
if (existing && existing.concreteNodeId === concreteId) {
|
|
2493
|
+
return existing;
|
|
2494
|
+
}
|
|
2495
|
+
}
|
|
2496
|
+
const tempNode = {
|
|
2497
|
+
id: this.nextNodeId++,
|
|
2498
|
+
position: {
|
|
2499
|
+
x,
|
|
2500
|
+
y
|
|
2501
|
+
},
|
|
2502
|
+
clusterId: cluster.id,
|
|
2503
|
+
concreteNodeId: concreteId,
|
|
2504
|
+
edges: []
|
|
2505
|
+
};
|
|
2506
|
+
this.abstractNodes.set(tempNode.id, tempNode);
|
|
2507
|
+
cluster.addNodeId(tempNode.id);
|
|
2508
|
+
const subPathfinder = new AStarPathfinder(cluster.subMap);
|
|
2509
|
+
const localPos = cluster.subMap.globalToLocal(x, y);
|
|
2510
|
+
for (const existingNodeId of cluster.nodeIds) {
|
|
2511
|
+
if (existingNodeId === tempNode.id) continue;
|
|
2512
|
+
const existingNode = this.abstractNodes.get(existingNodeId);
|
|
2513
|
+
if (!existingNode) continue;
|
|
2514
|
+
const targetLocalPos = cluster.subMap.globalToLocal(existingNode.position.x, existingNode.position.y);
|
|
2515
|
+
const result = subPathfinder.findPath(localPos.x, localPos.y, targetLocalPos.x, targetLocalPos.y);
|
|
2516
|
+
if (result.found && result.path.length > 0) {
|
|
2517
|
+
const globalPath = result.path.map((p) => {
|
|
2518
|
+
const global = cluster.subMap.localToGlobal(p.x, p.y);
|
|
2519
|
+
return global.y * this.mapWidth + global.x;
|
|
2520
|
+
});
|
|
2521
|
+
tempNode.edges.push({
|
|
2522
|
+
targetNodeId: existingNode.id,
|
|
2523
|
+
cost: result.cost,
|
|
2524
|
+
isInterEdge: false,
|
|
2525
|
+
innerPath: globalPath
|
|
2526
|
+
});
|
|
2527
|
+
existingNode.edges.push({
|
|
2528
|
+
targetNodeId: tempNode.id,
|
|
2529
|
+
cost: result.cost,
|
|
2530
|
+
isInterEdge: false,
|
|
2531
|
+
innerPath: [
|
|
2532
|
+
...globalPath
|
|
2533
|
+
].reverse()
|
|
2534
|
+
});
|
|
2535
|
+
}
|
|
2536
|
+
}
|
|
2537
|
+
return tempNode;
|
|
2538
|
+
}
|
|
2539
|
+
/**
|
|
2540
|
+
* @zh 移除临时节点
|
|
2541
|
+
* @en Remove temporary node
|
|
2542
|
+
*/
|
|
2543
|
+
removeTempNode(node, cluster) {
|
|
2544
|
+
for (const existingNodeId of cluster.nodeIds) {
|
|
2545
|
+
if (existingNodeId === node.id) continue;
|
|
2546
|
+
const existingNode = this.abstractNodes.get(existingNodeId);
|
|
2547
|
+
if (existingNode) {
|
|
2548
|
+
existingNode.edges = existingNode.edges.filter((e) => e.targetNodeId !== node.id);
|
|
2549
|
+
}
|
|
2550
|
+
}
|
|
2551
|
+
cluster.removeNodeId(node.id);
|
|
2552
|
+
this.abstractNodes.delete(node.id);
|
|
2553
|
+
}
|
|
2554
|
+
/**
|
|
2555
|
+
* @zh 在抽象图上进行 A* 搜索
|
|
2556
|
+
* @en Perform A* search on abstract graph
|
|
2557
|
+
*/
|
|
2558
|
+
abstractSearch(startNode, endNode, opts) {
|
|
2559
|
+
const openList = new IndexedBinaryHeap((a, b) => a.f - b.f);
|
|
2560
|
+
const nodeMap = /* @__PURE__ */ new Map();
|
|
2561
|
+
const endPosition = endNode.position;
|
|
2562
|
+
const h = this.heuristic(startNode.position, endPosition) * opts.heuristicWeight;
|
|
2563
|
+
const startSearchNode = {
|
|
2564
|
+
node: startNode,
|
|
2565
|
+
g: 0,
|
|
2566
|
+
h,
|
|
2567
|
+
f: h,
|
|
2568
|
+
parent: null,
|
|
2569
|
+
closed: false,
|
|
2570
|
+
opened: true,
|
|
2571
|
+
heapIndex: -1
|
|
2572
|
+
};
|
|
2573
|
+
openList.push(startSearchNode);
|
|
2574
|
+
nodeMap.set(startNode.id, startSearchNode);
|
|
2575
|
+
let nodesSearched = 0;
|
|
2576
|
+
while (!openList.isEmpty && nodesSearched < opts.maxNodes) {
|
|
2577
|
+
const current = openList.pop();
|
|
2578
|
+
current.closed = true;
|
|
2579
|
+
nodesSearched++;
|
|
2580
|
+
if (current.node.id === endNode.id) {
|
|
2581
|
+
return this.reconstructPath(current);
|
|
2582
|
+
}
|
|
2583
|
+
for (const edge of current.node.edges) {
|
|
2584
|
+
let neighbor = nodeMap.get(edge.targetNodeId);
|
|
2585
|
+
if (!neighbor) {
|
|
2586
|
+
const neighborNode = this.abstractNodes.get(edge.targetNodeId);
|
|
2587
|
+
if (!neighborNode) continue;
|
|
2588
|
+
const nh = this.heuristic(neighborNode.position, endPosition) * opts.heuristicWeight;
|
|
2589
|
+
neighbor = {
|
|
2590
|
+
node: neighborNode,
|
|
2591
|
+
g: Infinity,
|
|
2592
|
+
h: nh,
|
|
2593
|
+
f: Infinity,
|
|
2594
|
+
parent: null,
|
|
2595
|
+
closed: false,
|
|
2596
|
+
opened: false,
|
|
2597
|
+
heapIndex: -1
|
|
2598
|
+
};
|
|
2599
|
+
nodeMap.set(edge.targetNodeId, neighbor);
|
|
2600
|
+
}
|
|
2601
|
+
if (neighbor.closed) continue;
|
|
2602
|
+
const tentativeG = current.g + edge.cost;
|
|
2603
|
+
if (!neighbor.opened) {
|
|
2604
|
+
neighbor.g = tentativeG;
|
|
2605
|
+
neighbor.f = tentativeG + neighbor.h;
|
|
2606
|
+
neighbor.parent = current;
|
|
2607
|
+
neighbor.opened = true;
|
|
2608
|
+
openList.push(neighbor);
|
|
2609
|
+
} else if (tentativeG < neighbor.g) {
|
|
2610
|
+
neighbor.g = tentativeG;
|
|
2611
|
+
neighbor.f = tentativeG + neighbor.h;
|
|
2612
|
+
neighbor.parent = current;
|
|
2613
|
+
openList.update(neighbor);
|
|
2614
|
+
}
|
|
2615
|
+
}
|
|
2616
|
+
}
|
|
2617
|
+
return null;
|
|
2618
|
+
}
|
|
2619
|
+
/**
|
|
2620
|
+
* @zh 重建抽象路径
|
|
2621
|
+
* @en Reconstruct abstract path
|
|
2622
|
+
*/
|
|
2623
|
+
reconstructPath(endNode) {
|
|
2624
|
+
const path = [];
|
|
2625
|
+
let current = endNode;
|
|
2626
|
+
while (current) {
|
|
2627
|
+
path.unshift(current.node);
|
|
2628
|
+
current = current.parent;
|
|
2629
|
+
}
|
|
2630
|
+
return path;
|
|
2631
|
+
}
|
|
2632
|
+
/**
|
|
2633
|
+
* @zh 细化抽象路径为具体路径
|
|
2634
|
+
* @en Refine abstract path to concrete path
|
|
2635
|
+
*/
|
|
2636
|
+
refinePath(abstractPath, startX, startY, endX, endY, opts) {
|
|
2637
|
+
if (abstractPath.length === 0) {
|
|
2638
|
+
return EMPTY_PATH_RESULT;
|
|
2639
|
+
}
|
|
2640
|
+
const fullPath = [];
|
|
2641
|
+
let totalCost = 0;
|
|
2642
|
+
let nodesSearched = abstractPath.length;
|
|
2643
|
+
for (let i = 0; i < abstractPath.length - 1; i++) {
|
|
2644
|
+
const fromNode = abstractPath[i];
|
|
2645
|
+
const toNode = abstractPath[i + 1];
|
|
2646
|
+
const edge = fromNode.edges.find((e) => e.targetNodeId === toNode.id);
|
|
2647
|
+
if (!edge) {
|
|
2648
|
+
const segResult = this.findLocalPath(fromNode.position.x, fromNode.position.y, toNode.position.x, toNode.position.y, opts);
|
|
2649
|
+
if (segResult.found) {
|
|
2650
|
+
this.appendPath(fullPath, segResult.path);
|
|
2651
|
+
totalCost += segResult.cost;
|
|
2652
|
+
nodesSearched += segResult.nodesSearched;
|
|
2653
|
+
}
|
|
2654
|
+
} else if (edge.isInterEdge) {
|
|
2655
|
+
if (fullPath.length === 0 || fullPath[fullPath.length - 1].x !== fromNode.position.x || fullPath[fullPath.length - 1].y !== fromNode.position.y) {
|
|
2656
|
+
fullPath.push({
|
|
2657
|
+
x: fromNode.position.x,
|
|
2658
|
+
y: fromNode.position.y
|
|
2659
|
+
});
|
|
2660
|
+
}
|
|
2661
|
+
fullPath.push({
|
|
2662
|
+
x: toNode.position.x,
|
|
2663
|
+
y: toNode.position.y
|
|
2664
|
+
});
|
|
2665
|
+
totalCost += edge.cost;
|
|
2666
|
+
} else if (edge.innerPath && edge.innerPath.length > 0) {
|
|
2667
|
+
const concretePath = edge.innerPath.map((id) => ({
|
|
2668
|
+
x: id % this.mapWidth,
|
|
2669
|
+
y: Math.floor(id / this.mapWidth)
|
|
2670
|
+
}));
|
|
2671
|
+
this.appendPath(fullPath, concretePath);
|
|
2672
|
+
totalCost += edge.cost;
|
|
2673
|
+
} else {
|
|
2674
|
+
const computed = this.computeIntraEdgePath(fromNode, toNode, edge);
|
|
2675
|
+
if (computed && computed.path.length > 0) {
|
|
2676
|
+
const concretePath = computed.path.map((id) => ({
|
|
2677
|
+
x: id % this.mapWidth,
|
|
2678
|
+
y: Math.floor(id / this.mapWidth)
|
|
2679
|
+
}));
|
|
2680
|
+
this.appendPath(fullPath, concretePath);
|
|
2681
|
+
totalCost += computed.cost;
|
|
2682
|
+
} else {
|
|
2683
|
+
const segResult = this.findLocalPath(fromNode.position.x, fromNode.position.y, toNode.position.x, toNode.position.y, opts);
|
|
2684
|
+
if (segResult.found) {
|
|
2685
|
+
this.appendPath(fullPath, segResult.path);
|
|
2686
|
+
totalCost += segResult.cost;
|
|
2687
|
+
nodesSearched += segResult.nodesSearched;
|
|
2688
|
+
}
|
|
2689
|
+
}
|
|
2690
|
+
}
|
|
2691
|
+
}
|
|
2692
|
+
if (fullPath.length > 0 && (fullPath[0].x !== startX || fullPath[0].y !== startY)) {
|
|
2693
|
+
const firstPoint = fullPath[0];
|
|
2694
|
+
if (Math.abs(firstPoint.x - startX) <= 1 && Math.abs(firstPoint.y - startY) <= 1) {
|
|
2695
|
+
fullPath.unshift({
|
|
2696
|
+
x: startX,
|
|
2697
|
+
y: startY
|
|
2698
|
+
});
|
|
2699
|
+
} else {
|
|
2700
|
+
const segResult = this.findLocalPath(startX, startY, firstPoint.x, firstPoint.y, opts);
|
|
2701
|
+
if (segResult.found) {
|
|
2702
|
+
fullPath.splice(0, 0, ...segResult.path.slice(0, -1));
|
|
2703
|
+
totalCost += segResult.cost;
|
|
2704
|
+
}
|
|
2705
|
+
}
|
|
2706
|
+
}
|
|
2707
|
+
if (fullPath.length > 0) {
|
|
2708
|
+
const lastPoint = fullPath[fullPath.length - 1];
|
|
2709
|
+
if (lastPoint.x !== endX || lastPoint.y !== endY) {
|
|
2710
|
+
if (Math.abs(lastPoint.x - endX) <= 1 && Math.abs(lastPoint.y - endY) <= 1) {
|
|
2711
|
+
fullPath.push({
|
|
2712
|
+
x: endX,
|
|
2713
|
+
y: endY
|
|
2714
|
+
});
|
|
2715
|
+
} else {
|
|
2716
|
+
const segResult = this.findLocalPath(lastPoint.x, lastPoint.y, endX, endY, opts);
|
|
2717
|
+
if (segResult.found) {
|
|
2718
|
+
fullPath.push(...segResult.path.slice(1));
|
|
2719
|
+
totalCost += segResult.cost;
|
|
2720
|
+
}
|
|
2721
|
+
}
|
|
2722
|
+
}
|
|
2723
|
+
}
|
|
2724
|
+
return {
|
|
2725
|
+
found: fullPath.length > 0,
|
|
2726
|
+
path: fullPath,
|
|
2727
|
+
cost: totalCost,
|
|
2728
|
+
nodesSearched
|
|
2729
|
+
};
|
|
2730
|
+
}
|
|
2731
|
+
/**
|
|
2732
|
+
* @zh 追加路径(避免重复点)
|
|
2733
|
+
* @en Append path (avoid duplicate points)
|
|
2734
|
+
*/
|
|
2735
|
+
appendPath(fullPath, segment) {
|
|
2736
|
+
if (segment.length === 0) return;
|
|
2737
|
+
let startIdx = 0;
|
|
2738
|
+
if (fullPath.length > 0) {
|
|
2739
|
+
const last = fullPath[fullPath.length - 1];
|
|
2740
|
+
if (last.x === segment[0].x && last.y === segment[0].y) {
|
|
2741
|
+
startIdx = 1;
|
|
2742
|
+
}
|
|
2743
|
+
}
|
|
2744
|
+
for (let i = startIdx; i < segment.length; i++) {
|
|
2745
|
+
fullPath.push({
|
|
2746
|
+
x: segment[i].x,
|
|
2747
|
+
y: segment[i].y
|
|
2748
|
+
});
|
|
2749
|
+
}
|
|
2750
|
+
}
|
|
2751
|
+
/**
|
|
2752
|
+
* @zh 局部寻路
|
|
2753
|
+
* @en Local pathfinding
|
|
2754
|
+
*/
|
|
2755
|
+
findLocalPath(startX, startY, endX, endY, opts) {
|
|
2756
|
+
return this.localPathfinder.findPath(startX, startY, endX, endY, opts);
|
|
2757
|
+
}
|
|
2758
|
+
/**
|
|
2759
|
+
* @zh 启发式函数(Octile 距离)
|
|
2760
|
+
* @en Heuristic function (Octile distance)
|
|
2761
|
+
*/
|
|
2762
|
+
heuristic(a, b) {
|
|
2763
|
+
const dx = Math.abs(a.x - b.x);
|
|
2764
|
+
const dy = Math.abs(a.y - b.y);
|
|
2765
|
+
return dx + dy + (Math.SQRT2 - 2) * Math.min(dx, dy);
|
|
2766
|
+
}
|
|
2767
|
+
};
|
|
2768
|
+
__name(_HPAPathfinder, "HPAPathfinder");
|
|
2769
|
+
var HPAPathfinder = _HPAPathfinder;
|
|
2770
|
+
function createHPAPathfinder(map, config) {
|
|
2771
|
+
return new HPAPathfinder(map, config);
|
|
2772
|
+
}
|
|
2773
|
+
__name(createHPAPathfinder, "createHPAPathfinder");
|
|
2774
|
+
|
|
2775
|
+
// src/interfaces/IPathPlanner.ts
|
|
2776
|
+
var EMPTY_PLAN_RESULT = {
|
|
2777
|
+
found: false,
|
|
2778
|
+
path: [],
|
|
2779
|
+
cost: 0,
|
|
2780
|
+
nodesSearched: 0
|
|
2781
|
+
};
|
|
2782
|
+
var PathPlanState = /* @__PURE__ */ (function(PathPlanState2) {
|
|
2783
|
+
PathPlanState2["Idle"] = "idle";
|
|
2784
|
+
PathPlanState2["InProgress"] = "in_progress";
|
|
2785
|
+
PathPlanState2["Completed"] = "completed";
|
|
2786
|
+
PathPlanState2["Failed"] = "failed";
|
|
2787
|
+
PathPlanState2["Cancelled"] = "cancelled";
|
|
2788
|
+
return PathPlanState2;
|
|
2789
|
+
})({});
|
|
2790
|
+
function isIncrementalPlanner(planner) {
|
|
2791
|
+
return "supportsIncremental" in planner && planner.supportsIncremental === true;
|
|
2792
|
+
}
|
|
2793
|
+
__name(isIncrementalPlanner, "isIncrementalPlanner");
|
|
2794
|
+
|
|
2795
|
+
// src/interfaces/ICollisionResolver.ts
|
|
2796
|
+
var EMPTY_COLLISION_RESULT = {
|
|
2797
|
+
collided: false,
|
|
2798
|
+
penetration: 0,
|
|
2799
|
+
normal: {
|
|
2800
|
+
x: 0,
|
|
2801
|
+
y: 0
|
|
2802
|
+
},
|
|
2803
|
+
closestPoint: {
|
|
2804
|
+
x: 0,
|
|
2805
|
+
y: 0
|
|
2806
|
+
}
|
|
2807
|
+
};
|
|
2808
|
+
|
|
2809
|
+
// src/interfaces/IFlowController.ts
|
|
2810
|
+
var PassPermission = /* @__PURE__ */ (function(PassPermission2) {
|
|
2811
|
+
PassPermission2["Proceed"] = "proceed";
|
|
2812
|
+
PassPermission2["Wait"] = "wait";
|
|
2813
|
+
PassPermission2["Yield"] = "yield";
|
|
2814
|
+
return PassPermission2;
|
|
2815
|
+
})({});
|
|
2816
|
+
var DEFAULT_FLOW_CONTROLLER_CONFIG = {
|
|
2817
|
+
detectionRadius: 3,
|
|
2818
|
+
minAgentsForCongestion: 3,
|
|
2819
|
+
defaultCapacity: 2,
|
|
2820
|
+
waitPointDistance: 1.5,
|
|
2821
|
+
yieldSpeedMultiplier: 0.3
|
|
2822
|
+
};
|
|
2823
|
+
|
|
2824
|
+
// src/adapters/NavMeshPathPlannerAdapter.ts
|
|
2825
|
+
var _NavMeshPathPlannerAdapter = class _NavMeshPathPlannerAdapter {
|
|
2826
|
+
constructor(navMesh) {
|
|
2827
|
+
__publicField(this, "navMesh");
|
|
2828
|
+
__publicField(this, "type", "navmesh");
|
|
2829
|
+
this.navMesh = navMesh;
|
|
2830
|
+
}
|
|
2831
|
+
findPath(start, end, options) {
|
|
2832
|
+
const result = this.navMesh.findPathWithObstacles(start.x, start.y, end.x, end.y, options ? {
|
|
2833
|
+
agentRadius: options.agentRadius
|
|
2834
|
+
} : void 0);
|
|
2835
|
+
if (!result.found) {
|
|
2836
|
+
return EMPTY_PLAN_RESULT;
|
|
2837
|
+
}
|
|
2838
|
+
return {
|
|
2839
|
+
found: true,
|
|
2840
|
+
path: result.path.map((p) => ({
|
|
2841
|
+
x: p.x,
|
|
2842
|
+
y: p.y
|
|
2843
|
+
})),
|
|
2844
|
+
cost: result.cost,
|
|
2845
|
+
nodesSearched: result.nodesSearched
|
|
2846
|
+
};
|
|
2847
|
+
}
|
|
2848
|
+
isWalkable(position) {
|
|
2849
|
+
return this.navMesh.isWalkable(position.x, position.y);
|
|
2850
|
+
}
|
|
2851
|
+
getNearestWalkable(position) {
|
|
2852
|
+
const polygon = this.navMesh.findPolygonAt(position.x, position.y);
|
|
2853
|
+
if (polygon) {
|
|
2854
|
+
return {
|
|
2855
|
+
x: position.x,
|
|
2856
|
+
y: position.y
|
|
2857
|
+
};
|
|
2858
|
+
}
|
|
2859
|
+
const polygons = this.navMesh.getPolygons();
|
|
2860
|
+
if (polygons.length === 0) {
|
|
2861
|
+
return null;
|
|
2862
|
+
}
|
|
2863
|
+
let nearestDist = Infinity;
|
|
2864
|
+
let nearestPoint = null;
|
|
2865
|
+
for (const poly of polygons) {
|
|
2866
|
+
const dx = poly.center.x - position.x;
|
|
2867
|
+
const dy = poly.center.y - position.y;
|
|
2868
|
+
const dist = dx * dx + dy * dy;
|
|
2869
|
+
if (dist < nearestDist) {
|
|
2870
|
+
nearestDist = dist;
|
|
2871
|
+
nearestPoint = {
|
|
2872
|
+
x: poly.center.x,
|
|
2873
|
+
y: poly.center.y
|
|
2874
|
+
};
|
|
2875
|
+
}
|
|
2876
|
+
}
|
|
2877
|
+
return nearestPoint;
|
|
2878
|
+
}
|
|
2879
|
+
clear() {
|
|
2880
|
+
}
|
|
2881
|
+
dispose() {
|
|
2882
|
+
}
|
|
2883
|
+
};
|
|
2884
|
+
__name(_NavMeshPathPlannerAdapter, "NavMeshPathPlannerAdapter");
|
|
2885
|
+
var NavMeshPathPlannerAdapter = _NavMeshPathPlannerAdapter;
|
|
2886
|
+
function createNavMeshPathPlanner(navMesh) {
|
|
2887
|
+
return new NavMeshPathPlannerAdapter(navMesh);
|
|
2888
|
+
}
|
|
2889
|
+
__name(createNavMeshPathPlanner, "createNavMeshPathPlanner");
|
|
2890
|
+
|
|
2891
|
+
// src/adapters/GridPathfinderAdapter.ts
|
|
2892
|
+
var _GridPathfinderAdapter = class _GridPathfinderAdapter {
|
|
2893
|
+
constructor(pathfinder, map, options, type = "grid", config) {
|
|
2894
|
+
__publicField(this, "pathfinder");
|
|
2895
|
+
__publicField(this, "map");
|
|
2896
|
+
__publicField(this, "options");
|
|
2897
|
+
__publicField(this, "type");
|
|
2898
|
+
__publicField(this, "cellSize");
|
|
2899
|
+
__publicField(this, "alignToCenter");
|
|
2900
|
+
this.pathfinder = pathfinder;
|
|
2901
|
+
this.map = map;
|
|
2902
|
+
this.options = options;
|
|
2903
|
+
this.type = type;
|
|
2904
|
+
const cellSize = config?.cellSize ?? 1;
|
|
2905
|
+
if (cellSize <= 0 || !Number.isFinite(cellSize)) {
|
|
2906
|
+
throw new Error(`cellSize must be a positive finite number, got: ${cellSize}`);
|
|
2907
|
+
}
|
|
2908
|
+
this.cellSize = cellSize;
|
|
2909
|
+
this.alignToCenter = config?.alignToCenter ?? cellSize > 1;
|
|
2910
|
+
}
|
|
2911
|
+
/**
|
|
2912
|
+
* @zh 像素坐标转网格坐标
|
|
2913
|
+
* @en Convert pixel coordinate to grid coordinate
|
|
2914
|
+
*/
|
|
2915
|
+
toGridCoord(pixel) {
|
|
2916
|
+
return Math.floor(pixel / this.cellSize);
|
|
2917
|
+
}
|
|
2918
|
+
/**
|
|
2919
|
+
* @zh 网格坐标转像素坐标
|
|
2920
|
+
* @en Convert grid coordinate to pixel coordinate
|
|
2921
|
+
*
|
|
2922
|
+
* @zh 根据 alignToCenter 配置决定是否偏移到单元格中心
|
|
2923
|
+
* @en Offsets to cell center based on alignToCenter configuration
|
|
2924
|
+
*/
|
|
2925
|
+
toPixelCoord(grid) {
|
|
2926
|
+
const base = grid * this.cellSize;
|
|
2927
|
+
return this.alignToCenter ? base + this.cellSize * 0.5 : base;
|
|
2928
|
+
}
|
|
2929
|
+
findPath(start, end) {
|
|
2930
|
+
const startGridX = this.toGridCoord(start.x);
|
|
2931
|
+
const startGridY = this.toGridCoord(start.y);
|
|
2932
|
+
const endGridX = this.toGridCoord(end.x);
|
|
2933
|
+
const endGridY = this.toGridCoord(end.y);
|
|
2934
|
+
const result = this.pathfinder.findPath(startGridX, startGridY, endGridX, endGridY, this.options);
|
|
2935
|
+
if (!result.found) {
|
|
2936
|
+
return EMPTY_PLAN_RESULT;
|
|
2937
|
+
}
|
|
2938
|
+
return {
|
|
2939
|
+
found: true,
|
|
2940
|
+
path: result.path.map((p) => ({
|
|
2941
|
+
x: this.toPixelCoord(p.x),
|
|
2942
|
+
y: this.toPixelCoord(p.y)
|
|
2943
|
+
})),
|
|
2944
|
+
cost: result.cost,
|
|
2945
|
+
nodesSearched: result.nodesSearched
|
|
2946
|
+
};
|
|
2947
|
+
}
|
|
2948
|
+
isWalkable(position) {
|
|
2949
|
+
return this.map.isWalkable(this.toGridCoord(position.x), this.toGridCoord(position.y));
|
|
2950
|
+
}
|
|
2951
|
+
getNearestWalkable(position) {
|
|
2952
|
+
const x = this.toGridCoord(position.x);
|
|
2953
|
+
const y = this.toGridCoord(position.y);
|
|
2954
|
+
if (this.map.isWalkable(x, y)) {
|
|
2955
|
+
return {
|
|
2956
|
+
x: this.toPixelCoord(x),
|
|
2957
|
+
y: this.toPixelCoord(y)
|
|
2958
|
+
};
|
|
2959
|
+
}
|
|
2960
|
+
for (let radius = 1; radius <= 10; radius++) {
|
|
2961
|
+
for (let dx = -radius; dx <= radius; dx++) {
|
|
2962
|
+
for (let dy = -radius; dy <= radius; dy++) {
|
|
2963
|
+
if (Math.abs(dx) === radius || Math.abs(dy) === radius) {
|
|
2964
|
+
if (this.map.isWalkable(x + dx, y + dy)) {
|
|
2965
|
+
return {
|
|
2966
|
+
x: this.toPixelCoord(x + dx),
|
|
2967
|
+
y: this.toPixelCoord(y + dy)
|
|
2968
|
+
};
|
|
2969
|
+
}
|
|
2970
|
+
}
|
|
2971
|
+
}
|
|
2972
|
+
}
|
|
2973
|
+
}
|
|
2974
|
+
return null;
|
|
2975
|
+
}
|
|
2976
|
+
clear() {
|
|
2977
|
+
this.pathfinder.clear();
|
|
2978
|
+
}
|
|
2979
|
+
dispose() {
|
|
2980
|
+
this.pathfinder.clear();
|
|
2981
|
+
}
|
|
2982
|
+
};
|
|
2983
|
+
__name(_GridPathfinderAdapter, "GridPathfinderAdapter");
|
|
2984
|
+
var GridPathfinderAdapter = _GridPathfinderAdapter;
|
|
2985
|
+
function createAStarPlanner(map, options, config) {
|
|
2986
|
+
return new GridPathfinderAdapter(new AStarPathfinder(map), map, options, "astar", config);
|
|
2987
|
+
}
|
|
2988
|
+
__name(createAStarPlanner, "createAStarPlanner");
|
|
2989
|
+
function createJPSPlanner(map, options, config) {
|
|
2990
|
+
return new GridPathfinderAdapter(new JPSPathfinder(map), map, options, "jps", config);
|
|
2991
|
+
}
|
|
2992
|
+
__name(createJPSPlanner, "createJPSPlanner");
|
|
2993
|
+
function createHPAPlanner(map, hpaConfig, options, adapterConfig) {
|
|
2994
|
+
return new GridPathfinderAdapter(new HPAPathfinder(map, hpaConfig), map, options, "hpa", adapterConfig);
|
|
2995
|
+
}
|
|
2996
|
+
__name(createHPAPlanner, "createHPAPlanner");
|
|
2997
|
+
|
|
2998
|
+
// src/adapters/IncrementalGridPathPlannerAdapter.ts
|
|
2999
|
+
function toPathPlanState(state) {
|
|
3000
|
+
switch (state) {
|
|
3001
|
+
case PathfindingState.Idle:
|
|
3002
|
+
return PathPlanState.Idle;
|
|
3003
|
+
case PathfindingState.InProgress:
|
|
3004
|
+
return PathPlanState.InProgress;
|
|
3005
|
+
case PathfindingState.Completed:
|
|
3006
|
+
return PathPlanState.Completed;
|
|
3007
|
+
case PathfindingState.Failed:
|
|
3008
|
+
return PathPlanState.Failed;
|
|
3009
|
+
case PathfindingState.Cancelled:
|
|
3010
|
+
return PathPlanState.Cancelled;
|
|
3011
|
+
default:
|
|
3012
|
+
return PathPlanState.Idle;
|
|
3013
|
+
}
|
|
3014
|
+
}
|
|
3015
|
+
__name(toPathPlanState, "toPathPlanState");
|
|
3016
|
+
var _IncrementalGridPathPlannerAdapter = class _IncrementalGridPathPlannerAdapter {
|
|
3017
|
+
constructor(map, options, config) {
|
|
3018
|
+
__publicField(this, "type", "incremental-astar");
|
|
3019
|
+
__publicField(this, "supportsIncremental", true);
|
|
3020
|
+
__publicField(this, "pathfinder");
|
|
3021
|
+
__publicField(this, "map");
|
|
3022
|
+
__publicField(this, "options");
|
|
3023
|
+
__publicField(this, "cellSize");
|
|
3024
|
+
__publicField(this, "alignToCenter");
|
|
3025
|
+
/**
|
|
3026
|
+
* @zh 活跃请求 ID 集合(用于跟踪)
|
|
3027
|
+
* @en Active request IDs set (for tracking)
|
|
3028
|
+
*/
|
|
3029
|
+
__publicField(this, "activeRequests", /* @__PURE__ */ new Set());
|
|
3030
|
+
/**
|
|
3031
|
+
* @zh 每个请求的累计搜索节点数
|
|
3032
|
+
* @en Accumulated searched nodes per request
|
|
3033
|
+
*/
|
|
3034
|
+
__publicField(this, "requestTotalNodes", /* @__PURE__ */ new Map());
|
|
3035
|
+
this.map = map;
|
|
3036
|
+
this.options = options;
|
|
3037
|
+
const cellSize = config?.cellSize ?? 1;
|
|
3038
|
+
if (cellSize <= 0 || !Number.isFinite(cellSize)) {
|
|
3039
|
+
throw new Error(`cellSize must be a positive finite number, got: ${cellSize}`);
|
|
3040
|
+
}
|
|
3041
|
+
this.cellSize = cellSize;
|
|
3042
|
+
this.alignToCenter = config?.alignToCenter ?? cellSize > 1;
|
|
3043
|
+
this.pathfinder = new IncrementalAStarPathfinder(map, config);
|
|
3044
|
+
}
|
|
3045
|
+
/**
|
|
3046
|
+
* @zh 像素坐标转网格坐标
|
|
3047
|
+
* @en Convert pixel coordinate to grid coordinate
|
|
3048
|
+
*/
|
|
3049
|
+
toGridCoord(pixel) {
|
|
3050
|
+
return Math.floor(pixel / this.cellSize);
|
|
3051
|
+
}
|
|
3052
|
+
/**
|
|
3053
|
+
* @zh 网格坐标转像素坐标
|
|
3054
|
+
* @en Convert grid coordinate to pixel coordinate
|
|
3055
|
+
*
|
|
3056
|
+
* @zh 根据 alignToCenter 配置决定是否偏移到单元格中心
|
|
3057
|
+
* @en Offsets to cell center based on alignToCenter configuration
|
|
3058
|
+
*/
|
|
3059
|
+
toPixelCoord(grid) {
|
|
3060
|
+
const base = grid * this.cellSize;
|
|
3061
|
+
return this.alignToCenter ? base + this.cellSize * 0.5 : base;
|
|
3062
|
+
}
|
|
3063
|
+
// =========================================================================
|
|
3064
|
+
// IPathPlanner 基础接口 | IPathPlanner Base Interface
|
|
3065
|
+
// =========================================================================
|
|
3066
|
+
findPath(start, end, options) {
|
|
3067
|
+
const startGridX = this.toGridCoord(start.x);
|
|
3068
|
+
const startGridY = this.toGridCoord(start.y);
|
|
3069
|
+
const endGridX = this.toGridCoord(end.x);
|
|
3070
|
+
const endGridY = this.toGridCoord(end.y);
|
|
3071
|
+
const request = this.pathfinder.requestPath(startGridX, startGridY, endGridX, endGridY, this.options);
|
|
3072
|
+
let progress = this.pathfinder.step(request.id, 1e5);
|
|
3073
|
+
while (progress.state === PathfindingState.InProgress) {
|
|
3074
|
+
progress = this.pathfinder.step(request.id, 1e5);
|
|
3075
|
+
}
|
|
3076
|
+
const result = this.pathfinder.getResult(request.id);
|
|
3077
|
+
this.pathfinder.cleanup(request.id);
|
|
3078
|
+
if (!result || !result.found) {
|
|
3079
|
+
return EMPTY_PLAN_RESULT;
|
|
3080
|
+
}
|
|
3081
|
+
return {
|
|
3082
|
+
found: true,
|
|
3083
|
+
path: result.path.map((p) => ({
|
|
3084
|
+
x: this.toPixelCoord(p.x),
|
|
3085
|
+
y: this.toPixelCoord(p.y)
|
|
3086
|
+
})),
|
|
3087
|
+
cost: result.cost,
|
|
3088
|
+
nodesSearched: result.nodesSearched
|
|
3089
|
+
};
|
|
3090
|
+
}
|
|
3091
|
+
isWalkable(position) {
|
|
3092
|
+
return this.map.isWalkable(this.toGridCoord(position.x), this.toGridCoord(position.y));
|
|
3093
|
+
}
|
|
3094
|
+
getNearestWalkable(position) {
|
|
3095
|
+
const x = this.toGridCoord(position.x);
|
|
3096
|
+
const y = this.toGridCoord(position.y);
|
|
3097
|
+
if (this.map.isWalkable(x, y)) {
|
|
3098
|
+
return {
|
|
3099
|
+
x: this.toPixelCoord(x),
|
|
3100
|
+
y: this.toPixelCoord(y)
|
|
3101
|
+
};
|
|
3102
|
+
}
|
|
3103
|
+
for (let radius = 1; radius <= 10; radius++) {
|
|
3104
|
+
for (let dx = -radius; dx <= radius; dx++) {
|
|
3105
|
+
for (let dy = -radius; dy <= radius; dy++) {
|
|
3106
|
+
if (Math.abs(dx) === radius || Math.abs(dy) === radius) {
|
|
3107
|
+
if (this.map.isWalkable(x + dx, y + dy)) {
|
|
3108
|
+
return {
|
|
3109
|
+
x: this.toPixelCoord(x + dx),
|
|
3110
|
+
y: this.toPixelCoord(y + dy)
|
|
3111
|
+
};
|
|
3112
|
+
}
|
|
3113
|
+
}
|
|
3114
|
+
}
|
|
3115
|
+
}
|
|
3116
|
+
}
|
|
3117
|
+
return null;
|
|
3118
|
+
}
|
|
3119
|
+
clear() {
|
|
3120
|
+
this.pathfinder.clear();
|
|
3121
|
+
this.activeRequests.clear();
|
|
3122
|
+
this.requestTotalNodes.clear();
|
|
3123
|
+
}
|
|
3124
|
+
dispose() {
|
|
3125
|
+
this.pathfinder.clear();
|
|
3126
|
+
this.activeRequests.clear();
|
|
3127
|
+
this.requestTotalNodes.clear();
|
|
3128
|
+
}
|
|
3129
|
+
// =========================================================================
|
|
3130
|
+
// IIncrementalPathPlanner 增量接口 | IIncrementalPathPlanner Incremental Interface
|
|
3131
|
+
// =========================================================================
|
|
3132
|
+
requestPath(start, end, options) {
|
|
3133
|
+
const startGridX = this.toGridCoord(start.x);
|
|
3134
|
+
const startGridY = this.toGridCoord(start.y);
|
|
3135
|
+
const endGridX = this.toGridCoord(end.x);
|
|
3136
|
+
const endGridY = this.toGridCoord(end.y);
|
|
3137
|
+
const request = this.pathfinder.requestPath(startGridX, startGridY, endGridX, endGridY, this.options);
|
|
3138
|
+
this.activeRequests.add(request.id);
|
|
3139
|
+
this.requestTotalNodes.set(request.id, 0);
|
|
3140
|
+
return {
|
|
3141
|
+
id: request.id,
|
|
3142
|
+
state: PathPlanState.InProgress
|
|
3143
|
+
};
|
|
3144
|
+
}
|
|
3145
|
+
step(requestId, iterations) {
|
|
3146
|
+
const progress = this.pathfinder.step(requestId, iterations);
|
|
3147
|
+
const prevTotal = this.requestTotalNodes.get(requestId) ?? 0;
|
|
3148
|
+
const newTotal = prevTotal + progress.nodesSearched;
|
|
3149
|
+
this.requestTotalNodes.set(requestId, newTotal);
|
|
3150
|
+
return {
|
|
3151
|
+
state: toPathPlanState(progress.state),
|
|
3152
|
+
estimatedProgress: progress.estimatedProgress,
|
|
3153
|
+
nodesSearched: progress.nodesSearched,
|
|
3154
|
+
totalNodesSearched: newTotal
|
|
3155
|
+
};
|
|
3156
|
+
}
|
|
3157
|
+
getResult(requestId) {
|
|
3158
|
+
const result = this.pathfinder.getResult(requestId);
|
|
3159
|
+
if (!result) {
|
|
3160
|
+
return null;
|
|
3161
|
+
}
|
|
3162
|
+
if (!result.found) {
|
|
3163
|
+
return EMPTY_PLAN_RESULT;
|
|
3164
|
+
}
|
|
3165
|
+
return {
|
|
3166
|
+
found: true,
|
|
3167
|
+
path: result.path.map((p) => ({
|
|
3168
|
+
x: this.toPixelCoord(p.x),
|
|
3169
|
+
y: this.toPixelCoord(p.y)
|
|
3170
|
+
})),
|
|
3171
|
+
cost: result.cost,
|
|
3172
|
+
nodesSearched: result.nodesSearched
|
|
3173
|
+
};
|
|
3174
|
+
}
|
|
3175
|
+
cancel(requestId) {
|
|
3176
|
+
this.pathfinder.cancel(requestId);
|
|
3177
|
+
}
|
|
3178
|
+
cleanup(requestId) {
|
|
3179
|
+
this.pathfinder.cleanup(requestId);
|
|
3180
|
+
this.activeRequests.delete(requestId);
|
|
3181
|
+
this.requestTotalNodes.delete(requestId);
|
|
3182
|
+
}
|
|
3183
|
+
getActiveRequestCount() {
|
|
3184
|
+
return this.activeRequests.size;
|
|
3185
|
+
}
|
|
3186
|
+
};
|
|
3187
|
+
__name(_IncrementalGridPathPlannerAdapter, "IncrementalGridPathPlannerAdapter");
|
|
3188
|
+
var IncrementalGridPathPlannerAdapter = _IncrementalGridPathPlannerAdapter;
|
|
3189
|
+
function createIncrementalAStarPlanner(map, options, config) {
|
|
3190
|
+
return new IncrementalGridPathPlannerAdapter(map, options, config);
|
|
3191
|
+
}
|
|
3192
|
+
__name(createIncrementalAStarPlanner, "createIncrementalAStarPlanner");
|
|
3193
|
+
|
|
3194
|
+
// src/adapters/ORCALocalAvoidanceAdapter.ts
|
|
3195
|
+
var DEFAULT_ORCA_PARAMS = {
|
|
3196
|
+
neighborDist: 15,
|
|
3197
|
+
maxNeighbors: 10,
|
|
3198
|
+
timeHorizon: 2,
|
|
3199
|
+
timeHorizonObst: 1
|
|
3200
|
+
};
|
|
3201
|
+
var _ORCALocalAvoidanceAdapter = class _ORCALocalAvoidanceAdapter {
|
|
3202
|
+
constructor(config) {
|
|
3203
|
+
__publicField(this, "type", "orca");
|
|
3204
|
+
__publicField(this, "solver");
|
|
3205
|
+
__publicField(this, "kdTree");
|
|
3206
|
+
__publicField(this, "defaultParams");
|
|
3207
|
+
this.solver = createORCASolver(config);
|
|
3208
|
+
this.kdTree = createKDTree();
|
|
3209
|
+
this.defaultParams = {
|
|
3210
|
+
...DEFAULT_ORCA_PARAMS
|
|
3211
|
+
};
|
|
3212
|
+
}
|
|
3213
|
+
/**
|
|
3214
|
+
* @zh 设置默认 ORCA 参数
|
|
3215
|
+
* @en Set default ORCA parameters
|
|
3216
|
+
*
|
|
3217
|
+
* @param params - @zh 参数 @en Parameters
|
|
3218
|
+
*/
|
|
3219
|
+
setDefaultParams(params) {
|
|
3220
|
+
Object.assign(this.defaultParams, params);
|
|
3221
|
+
}
|
|
3222
|
+
/**
|
|
3223
|
+
* @zh 获取默认 ORCA 参数
|
|
3224
|
+
* @en Get default ORCA parameters
|
|
3225
|
+
*/
|
|
3226
|
+
getDefaultParams() {
|
|
3227
|
+
return this.defaultParams;
|
|
3228
|
+
}
|
|
3229
|
+
computeAvoidanceVelocity(agent, neighbors, obstacles, deltaTime) {
|
|
3230
|
+
const orcaAgent = this.toORCAAgent(agent);
|
|
3231
|
+
const orcaNeighbors = neighbors.map((n) => this.toORCAAgent(n));
|
|
3232
|
+
const orcaObstacles = obstacles.map((o) => this.toORCAObstacle(o));
|
|
3233
|
+
const result = this.solver.computeNewVelocityWithResult(orcaAgent, orcaNeighbors, orcaObstacles, deltaTime);
|
|
3234
|
+
return {
|
|
3235
|
+
velocity: result.velocity,
|
|
3236
|
+
feasible: result.feasible
|
|
3237
|
+
};
|
|
3238
|
+
}
|
|
3239
|
+
computeBatchAvoidance(agents, obstacles, deltaTime) {
|
|
3240
|
+
const results = /* @__PURE__ */ new Map();
|
|
3241
|
+
const orcaAgents = agents.map((a) => this.toORCAAgent(a));
|
|
3242
|
+
const orcaObstacles = obstacles.map((o) => this.toORCAObstacle(o));
|
|
3243
|
+
this.kdTree.build(orcaAgents);
|
|
3244
|
+
for (let i = 0; i < agents.length; i++) {
|
|
3245
|
+
const agent = orcaAgents[i];
|
|
3246
|
+
const neighborResults = this.kdTree.queryNeighbors(agent.position, agent.neighborDist, agent.maxNeighbors, agent.id);
|
|
3247
|
+
const result = this.solver.computeNewVelocityWithResult(agent, neighborResults.map((r) => r.agent), orcaObstacles, deltaTime);
|
|
3248
|
+
results.set(agents[i].id, {
|
|
3249
|
+
velocity: result.velocity,
|
|
3250
|
+
feasible: result.feasible
|
|
3251
|
+
});
|
|
3252
|
+
}
|
|
3253
|
+
return results;
|
|
3254
|
+
}
|
|
3255
|
+
dispose() {
|
|
3256
|
+
this.kdTree.clear();
|
|
3257
|
+
}
|
|
3258
|
+
toORCAAgent(agent) {
|
|
3259
|
+
return {
|
|
3260
|
+
id: agent.id,
|
|
3261
|
+
position: {
|
|
3262
|
+
x: agent.position.x,
|
|
3263
|
+
y: agent.position.y
|
|
3264
|
+
},
|
|
3265
|
+
velocity: {
|
|
3266
|
+
x: agent.velocity.x,
|
|
3267
|
+
y: agent.velocity.y
|
|
3268
|
+
},
|
|
3269
|
+
preferredVelocity: {
|
|
3270
|
+
x: agent.preferredVelocity.x,
|
|
3271
|
+
y: agent.preferredVelocity.y
|
|
3272
|
+
},
|
|
3273
|
+
radius: agent.radius,
|
|
3274
|
+
maxSpeed: agent.maxSpeed,
|
|
3275
|
+
neighborDist: this.defaultParams.neighborDist,
|
|
3276
|
+
maxNeighbors: this.defaultParams.maxNeighbors,
|
|
3277
|
+
timeHorizon: this.defaultParams.timeHorizon,
|
|
3278
|
+
timeHorizonObst: this.defaultParams.timeHorizonObst
|
|
3279
|
+
};
|
|
3280
|
+
}
|
|
3281
|
+
toORCAObstacle(obstacle) {
|
|
3282
|
+
return {
|
|
3283
|
+
vertices: obstacle.vertices.map((v) => ({
|
|
3284
|
+
x: v.x,
|
|
3285
|
+
y: v.y
|
|
3286
|
+
}))
|
|
3287
|
+
};
|
|
3288
|
+
}
|
|
3289
|
+
};
|
|
3290
|
+
__name(_ORCALocalAvoidanceAdapter, "ORCALocalAvoidanceAdapter");
|
|
3291
|
+
var ORCALocalAvoidanceAdapter = _ORCALocalAvoidanceAdapter;
|
|
3292
|
+
function createORCAAvoidance(config) {
|
|
3293
|
+
return new ORCALocalAvoidanceAdapter(config);
|
|
3294
|
+
}
|
|
3295
|
+
__name(createORCAAvoidance, "createORCAAvoidance");
|
|
3296
|
+
|
|
3297
|
+
// src/adapters/CollisionResolverAdapter.ts
|
|
3298
|
+
var _CollisionResolverAdapter = class _CollisionResolverAdapter {
|
|
3299
|
+
constructor(config) {
|
|
3300
|
+
__publicField(this, "type", "default");
|
|
3301
|
+
__publicField(this, "resolver");
|
|
3302
|
+
this.resolver = createCollisionResolver(config);
|
|
3303
|
+
}
|
|
3304
|
+
detectCollision(position, radius, obstacles) {
|
|
3305
|
+
if (obstacles.length === 0) {
|
|
3306
|
+
return EMPTY_COLLISION_RESULT;
|
|
3307
|
+
}
|
|
3308
|
+
const result = this.resolver.detectCollisions(position, radius, obstacles.map((o) => ({
|
|
3309
|
+
vertices: o.vertices.map((v) => ({
|
|
3310
|
+
x: v.x,
|
|
3311
|
+
y: v.y
|
|
3312
|
+
}))
|
|
3313
|
+
})));
|
|
3314
|
+
return {
|
|
3315
|
+
collided: result.collided,
|
|
3316
|
+
penetration: result.penetration,
|
|
3317
|
+
normal: {
|
|
3318
|
+
x: result.normal.x,
|
|
3319
|
+
y: result.normal.y
|
|
3320
|
+
},
|
|
3321
|
+
closestPoint: {
|
|
3322
|
+
x: result.closestPoint.x,
|
|
3323
|
+
y: result.closestPoint.y
|
|
3324
|
+
}
|
|
3325
|
+
};
|
|
3326
|
+
}
|
|
3327
|
+
resolveCollision(position, radius, obstacles) {
|
|
3328
|
+
if (obstacles.length === 0) {
|
|
3329
|
+
return {
|
|
3330
|
+
x: position.x,
|
|
3331
|
+
y: position.y
|
|
3332
|
+
};
|
|
3333
|
+
}
|
|
3334
|
+
const resolved = this.resolver.resolveCollision(position, radius, obstacles.map((o) => ({
|
|
3335
|
+
vertices: o.vertices.map((v) => ({
|
|
3336
|
+
x: v.x,
|
|
3337
|
+
y: v.y
|
|
3338
|
+
}))
|
|
3339
|
+
})));
|
|
3340
|
+
return {
|
|
3341
|
+
x: resolved.x,
|
|
3342
|
+
y: resolved.y
|
|
3343
|
+
};
|
|
3344
|
+
}
|
|
3345
|
+
validateVelocity(position, velocity, radius, obstacles, deltaTime) {
|
|
3346
|
+
if (obstacles.length === 0) {
|
|
3347
|
+
return {
|
|
3348
|
+
x: velocity.x,
|
|
3349
|
+
y: velocity.y
|
|
3350
|
+
};
|
|
3351
|
+
}
|
|
3352
|
+
const result = this.resolver.validateVelocity(position, velocity, radius, obstacles.map((o) => ({
|
|
3353
|
+
vertices: o.vertices.map((v) => ({
|
|
3354
|
+
x: v.x,
|
|
3355
|
+
y: v.y
|
|
3356
|
+
}))
|
|
3357
|
+
})), deltaTime);
|
|
3358
|
+
return {
|
|
3359
|
+
x: result.x,
|
|
3360
|
+
y: result.y
|
|
3361
|
+
};
|
|
3362
|
+
}
|
|
3363
|
+
detectAgentCollision(posA, radiusA, posB, radiusB) {
|
|
3364
|
+
const result = this.resolver.detectAgentCollision(posA, radiusA, posB, radiusB);
|
|
3365
|
+
return {
|
|
3366
|
+
collided: result.collided,
|
|
3367
|
+
penetration: result.penetration,
|
|
3368
|
+
normal: {
|
|
3369
|
+
x: result.normal.x,
|
|
3370
|
+
y: result.normal.y
|
|
3371
|
+
},
|
|
3372
|
+
closestPoint: {
|
|
3373
|
+
x: result.closestPoint.x,
|
|
3374
|
+
y: result.closestPoint.y
|
|
3375
|
+
}
|
|
3376
|
+
};
|
|
3377
|
+
}
|
|
3378
|
+
dispose() {
|
|
3379
|
+
}
|
|
3380
|
+
};
|
|
3381
|
+
__name(_CollisionResolverAdapter, "CollisionResolverAdapter");
|
|
3382
|
+
var CollisionResolverAdapter = _CollisionResolverAdapter;
|
|
3383
|
+
function createDefaultCollisionResolver(config) {
|
|
3384
|
+
return new CollisionResolverAdapter(config);
|
|
3385
|
+
}
|
|
3386
|
+
__name(createDefaultCollisionResolver, "createDefaultCollisionResolver");
|
|
3387
|
+
|
|
3388
|
+
// src/adapters/FlowController.ts
|
|
3389
|
+
var _FlowController = class _FlowController {
|
|
3390
|
+
constructor(config = {}) {
|
|
3391
|
+
__publicField(this, "type", "fifo-priority");
|
|
3392
|
+
__publicField(this, "config");
|
|
3393
|
+
__publicField(this, "zoneStates", /* @__PURE__ */ new Map());
|
|
3394
|
+
__publicField(this, "agentZoneMap", /* @__PURE__ */ new Map());
|
|
3395
|
+
__publicField(this, "agentResults", /* @__PURE__ */ new Map());
|
|
3396
|
+
__publicField(this, "nextZoneId", 1);
|
|
3397
|
+
__publicField(this, "currentTime", 0);
|
|
3398
|
+
this.config = {
|
|
3399
|
+
...DEFAULT_FLOW_CONTROLLER_CONFIG,
|
|
3400
|
+
...config
|
|
3401
|
+
};
|
|
3402
|
+
}
|
|
3403
|
+
/**
|
|
3404
|
+
* @zh 更新流量控制状态
|
|
3405
|
+
* @en Update flow control state
|
|
3406
|
+
*/
|
|
3407
|
+
update(agents, deltaTime) {
|
|
3408
|
+
this.currentTime += deltaTime;
|
|
3409
|
+
this.agentResults.clear();
|
|
3410
|
+
this.detectDynamicCongestion(agents);
|
|
3411
|
+
this.updateZoneQueues(agents);
|
|
3412
|
+
this.computeFlowControlResults(agents);
|
|
3413
|
+
this.cleanupEmptyZones();
|
|
3414
|
+
}
|
|
3415
|
+
/**
|
|
3416
|
+
* @zh 获取代理的流量控制结果
|
|
3417
|
+
* @en Get flow control result for an agent
|
|
3418
|
+
*/
|
|
3419
|
+
getFlowControl(agentId) {
|
|
3420
|
+
return this.agentResults.get(agentId) ?? {
|
|
3421
|
+
permission: PassPermission.Proceed,
|
|
3422
|
+
waitPosition: null,
|
|
3423
|
+
speedMultiplier: 1,
|
|
3424
|
+
zone: null,
|
|
3425
|
+
queuePosition: 0
|
|
3426
|
+
};
|
|
3427
|
+
}
|
|
3428
|
+
/**
|
|
3429
|
+
* @zh 获取所有拥堵区域
|
|
3430
|
+
* @en Get all congestion zones
|
|
3431
|
+
*/
|
|
3432
|
+
getCongestionZones() {
|
|
3433
|
+
return Array.from(this.zoneStates.values()).map((s) => s.zone);
|
|
3434
|
+
}
|
|
3435
|
+
/**
|
|
3436
|
+
* @zh 添加静态拥堵区域
|
|
3437
|
+
* @en Add static congestion zone
|
|
3438
|
+
*/
|
|
3439
|
+
addStaticZone(center, radius, capacity) {
|
|
3440
|
+
const zoneId = this.nextZoneId++;
|
|
3441
|
+
const zone = {
|
|
3442
|
+
id: zoneId,
|
|
3443
|
+
center: {
|
|
3444
|
+
x: center.x,
|
|
3445
|
+
y: center.y
|
|
3446
|
+
},
|
|
3447
|
+
radius,
|
|
3448
|
+
agentIds: [],
|
|
3449
|
+
capacity,
|
|
3450
|
+
congestionLevel: 0
|
|
3451
|
+
};
|
|
3452
|
+
this.zoneStates.set(zoneId, {
|
|
3453
|
+
zone,
|
|
3454
|
+
queue: [],
|
|
3455
|
+
passingAgents: /* @__PURE__ */ new Set(),
|
|
3456
|
+
isStatic: true
|
|
3457
|
+
});
|
|
3458
|
+
return zoneId;
|
|
3459
|
+
}
|
|
3460
|
+
/**
|
|
3461
|
+
* @zh 移除静态拥堵区域
|
|
3462
|
+
* @en Remove static congestion zone
|
|
3463
|
+
*/
|
|
3464
|
+
removeStaticZone(zoneId) {
|
|
3465
|
+
const state = this.zoneStates.get(zoneId);
|
|
3466
|
+
if (state?.isStatic) {
|
|
3467
|
+
for (const agentId of state.zone.agentIds) {
|
|
3468
|
+
this.agentZoneMap.delete(agentId);
|
|
3469
|
+
}
|
|
3470
|
+
this.zoneStates.delete(zoneId);
|
|
3471
|
+
}
|
|
3472
|
+
}
|
|
3473
|
+
/**
|
|
3474
|
+
* @zh 清除所有状态
|
|
3475
|
+
* @en Clear all state
|
|
3476
|
+
*/
|
|
3477
|
+
clear() {
|
|
3478
|
+
this.zoneStates.clear();
|
|
3479
|
+
this.agentZoneMap.clear();
|
|
3480
|
+
this.agentResults.clear();
|
|
3481
|
+
this.currentTime = 0;
|
|
3482
|
+
}
|
|
3483
|
+
/**
|
|
3484
|
+
* @zh 释放资源
|
|
3485
|
+
* @en Dispose resources
|
|
3486
|
+
*/
|
|
3487
|
+
dispose() {
|
|
3488
|
+
this.clear();
|
|
3489
|
+
}
|
|
3490
|
+
// =========================================================================
|
|
3491
|
+
// 私有方法 | Private Methods
|
|
3492
|
+
// =========================================================================
|
|
3493
|
+
/**
|
|
3494
|
+
* @zh 检测动态拥堵区域
|
|
3495
|
+
* @en Detect dynamic congestion zones
|
|
3496
|
+
*/
|
|
3497
|
+
detectDynamicCongestion(agents) {
|
|
3498
|
+
const clusters = this.clusterAgents(agents);
|
|
3499
|
+
for (const cluster of clusters) {
|
|
3500
|
+
if (cluster.length < this.config.minAgentsForCongestion) {
|
|
3501
|
+
continue;
|
|
3502
|
+
}
|
|
3503
|
+
const center = this.computeClusterCenter(cluster);
|
|
3504
|
+
const radius = this.computeClusterRadius(cluster, center);
|
|
3505
|
+
const existingZone = this.findZoneContaining(center);
|
|
3506
|
+
if (existingZone && !existingZone.isStatic) {
|
|
3507
|
+
this.updateDynamicZone(existingZone, cluster, center, radius);
|
|
3508
|
+
} else if (!existingZone) {
|
|
3509
|
+
this.createDynamicZone(cluster, center, radius);
|
|
3510
|
+
}
|
|
3511
|
+
}
|
|
3512
|
+
}
|
|
3513
|
+
/**
|
|
3514
|
+
* @zh 聚类代理
|
|
3515
|
+
* @en Cluster agents
|
|
3516
|
+
*/
|
|
3517
|
+
clusterAgents(agents) {
|
|
3518
|
+
const clusters = [];
|
|
3519
|
+
const visited = /* @__PURE__ */ new Set();
|
|
3520
|
+
const detectionRadiusSq = this.config.detectionRadius * this.config.detectionRadius;
|
|
3521
|
+
for (const agent of agents) {
|
|
3522
|
+
if (visited.has(agent.id) || !agent.destination) {
|
|
3523
|
+
continue;
|
|
3524
|
+
}
|
|
3525
|
+
const cluster = [
|
|
3526
|
+
agent
|
|
3527
|
+
];
|
|
3528
|
+
visited.add(agent.id);
|
|
3529
|
+
const queue = [
|
|
3530
|
+
agent
|
|
3531
|
+
];
|
|
3532
|
+
while (queue.length > 0) {
|
|
3533
|
+
const current = queue.shift();
|
|
3534
|
+
for (const other of agents) {
|
|
3535
|
+
if (visited.has(other.id) || !other.destination) {
|
|
3536
|
+
continue;
|
|
3537
|
+
}
|
|
3538
|
+
const dx = other.position.x - current.position.x;
|
|
3539
|
+
const dy = other.position.y - current.position.y;
|
|
3540
|
+
const distSq = dx * dx + dy * dy;
|
|
3541
|
+
if (distSq <= detectionRadiusSq) {
|
|
3542
|
+
visited.add(other.id);
|
|
3543
|
+
cluster.push(other);
|
|
3544
|
+
queue.push(other);
|
|
3545
|
+
}
|
|
3546
|
+
}
|
|
3547
|
+
}
|
|
3548
|
+
if (cluster.length >= this.config.minAgentsForCongestion) {
|
|
3549
|
+
clusters.push(cluster);
|
|
3550
|
+
}
|
|
3551
|
+
}
|
|
3552
|
+
return clusters;
|
|
3553
|
+
}
|
|
3554
|
+
/**
|
|
3555
|
+
* @zh 计算聚类中心
|
|
3556
|
+
* @en Compute cluster center
|
|
3557
|
+
*/
|
|
3558
|
+
computeClusterCenter(cluster) {
|
|
3559
|
+
let sumX = 0, sumY = 0;
|
|
3560
|
+
for (const agent of cluster) {
|
|
3561
|
+
sumX += agent.position.x;
|
|
3562
|
+
sumY += agent.position.y;
|
|
3563
|
+
}
|
|
3564
|
+
return {
|
|
3565
|
+
x: sumX / cluster.length,
|
|
3566
|
+
y: sumY / cluster.length
|
|
3567
|
+
};
|
|
3568
|
+
}
|
|
3569
|
+
/**
|
|
3570
|
+
* @zh 计算聚类半径
|
|
3571
|
+
* @en Compute cluster radius
|
|
3572
|
+
*/
|
|
3573
|
+
computeClusterRadius(cluster, center) {
|
|
3574
|
+
let maxDistSq = 0;
|
|
3575
|
+
for (const agent of cluster) {
|
|
3576
|
+
const dx = agent.position.x - center.x;
|
|
3577
|
+
const dy = agent.position.y - center.y;
|
|
3578
|
+
const distSq = dx * dx + dy * dy;
|
|
3579
|
+
maxDistSq = Math.max(maxDistSq, distSq);
|
|
3580
|
+
}
|
|
3581
|
+
return Math.sqrt(maxDistSq) + this.config.detectionRadius * 0.5;
|
|
3582
|
+
}
|
|
3583
|
+
/**
|
|
3584
|
+
* @zh 查找包含点的区域
|
|
3585
|
+
* @en Find zone containing point
|
|
3586
|
+
*/
|
|
3587
|
+
findZoneContaining(point) {
|
|
3588
|
+
for (const state of this.zoneStates.values()) {
|
|
3589
|
+
const dx = point.x - state.zone.center.x;
|
|
3590
|
+
const dy = point.y - state.zone.center.y;
|
|
3591
|
+
const distSq = dx * dx + dy * dy;
|
|
3592
|
+
if (distSq <= state.zone.radius * state.zone.radius) {
|
|
3593
|
+
return state;
|
|
3594
|
+
}
|
|
3595
|
+
}
|
|
3596
|
+
return null;
|
|
3597
|
+
}
|
|
3598
|
+
/**
|
|
3599
|
+
* @zh 更新动态区域
|
|
3600
|
+
* @en Update dynamic zone
|
|
3601
|
+
*/
|
|
3602
|
+
updateDynamicZone(state, cluster, center, radius) {
|
|
3603
|
+
state.zone.center = center;
|
|
3604
|
+
state.zone.radius = Math.max(state.zone.radius, radius);
|
|
3605
|
+
state.zone.agentIds = cluster.map((a) => a.id);
|
|
3606
|
+
state.zone.congestionLevel = Math.min(1, cluster.length / (state.zone.capacity * 2));
|
|
3607
|
+
}
|
|
3608
|
+
/**
|
|
3609
|
+
* @zh 创建动态区域
|
|
3610
|
+
* @en Create dynamic zone
|
|
3611
|
+
*/
|
|
3612
|
+
createDynamicZone(cluster, center, radius) {
|
|
3613
|
+
const zoneId = this.nextZoneId++;
|
|
3614
|
+
const capacityEstimate = Math.max(this.config.defaultCapacity, Math.floor(Math.PI * radius * radius / (Math.PI * 0.5 * 0.5 * 4)));
|
|
3615
|
+
const zone = {
|
|
3616
|
+
id: zoneId,
|
|
3617
|
+
center,
|
|
3618
|
+
radius,
|
|
3619
|
+
agentIds: cluster.map((a) => a.id),
|
|
3620
|
+
capacity: capacityEstimate,
|
|
3621
|
+
congestionLevel: Math.min(1, cluster.length / (capacityEstimate * 2))
|
|
3622
|
+
};
|
|
3623
|
+
this.zoneStates.set(zoneId, {
|
|
3624
|
+
zone,
|
|
3625
|
+
queue: [],
|
|
3626
|
+
passingAgents: /* @__PURE__ */ new Set(),
|
|
3627
|
+
isStatic: false
|
|
3628
|
+
});
|
|
3629
|
+
}
|
|
3630
|
+
/**
|
|
3631
|
+
* @zh 更新区域队列
|
|
3632
|
+
* @en Update zone queues
|
|
3633
|
+
*/
|
|
3634
|
+
updateZoneQueues(agents) {
|
|
3635
|
+
const agentMap = new Map(agents.map((a) => [
|
|
3636
|
+
a.id,
|
|
3637
|
+
a
|
|
3638
|
+
]));
|
|
3639
|
+
for (const state of this.zoneStates.values()) {
|
|
3640
|
+
const zone = state.zone;
|
|
3641
|
+
const newAgentIds = [];
|
|
3642
|
+
for (const agent of agents) {
|
|
3643
|
+
if (!agent.destination) continue;
|
|
3644
|
+
const dx = agent.position.x - zone.center.x;
|
|
3645
|
+
const dy = agent.position.y - zone.center.y;
|
|
3646
|
+
const distSq = dx * dx + dy * dy;
|
|
3647
|
+
const expandedRadius = zone.radius + this.config.waitPointDistance;
|
|
3648
|
+
if (distSq <= expandedRadius * expandedRadius) {
|
|
3649
|
+
newAgentIds.push(agent.id);
|
|
3650
|
+
const existingEntry = state.queue.find((e) => e.agentId === agent.id);
|
|
3651
|
+
if (!existingEntry) {
|
|
3652
|
+
state.queue.push({
|
|
3653
|
+
agentId: agent.id,
|
|
3654
|
+
enterTime: agent.enterTime ?? this.currentTime,
|
|
3655
|
+
priority: agent.priority
|
|
3656
|
+
});
|
|
3657
|
+
this.agentZoneMap.set(agent.id, zone.id);
|
|
3658
|
+
}
|
|
3659
|
+
}
|
|
3660
|
+
}
|
|
3661
|
+
state.queue = state.queue.filter((entry) => {
|
|
3662
|
+
const agent = agentMap.get(entry.agentId);
|
|
3663
|
+
if (!agent || !agent.destination) {
|
|
3664
|
+
state.passingAgents.delete(entry.agentId);
|
|
3665
|
+
this.agentZoneMap.delete(entry.agentId);
|
|
3666
|
+
return false;
|
|
3667
|
+
}
|
|
3668
|
+
const dx = agent.position.x - zone.center.x;
|
|
3669
|
+
const dy = agent.position.y - zone.center.y;
|
|
3670
|
+
const distSq = dx * dx + dy * dy;
|
|
3671
|
+
const expandedRadius = zone.radius + this.config.waitPointDistance * 2;
|
|
3672
|
+
if (distSq > expandedRadius * expandedRadius) {
|
|
3673
|
+
state.passingAgents.delete(entry.agentId);
|
|
3674
|
+
this.agentZoneMap.delete(entry.agentId);
|
|
3675
|
+
return false;
|
|
3676
|
+
}
|
|
3677
|
+
return true;
|
|
3678
|
+
});
|
|
3679
|
+
state.queue.sort((a, b) => {
|
|
3680
|
+
if (a.priority !== b.priority) {
|
|
3681
|
+
return a.priority - b.priority;
|
|
3682
|
+
}
|
|
3683
|
+
return a.enterTime - b.enterTime;
|
|
3684
|
+
});
|
|
3685
|
+
zone.agentIds = state.queue.map((e) => e.agentId);
|
|
3686
|
+
zone.congestionLevel = Math.min(1, zone.agentIds.length / (zone.capacity * 2));
|
|
3687
|
+
}
|
|
3688
|
+
}
|
|
3689
|
+
/**
|
|
3690
|
+
* @zh 计算流量控制结果
|
|
3691
|
+
* @en Compute flow control results
|
|
3692
|
+
*/
|
|
3693
|
+
computeFlowControlResults(agents) {
|
|
3694
|
+
const agentMap = new Map(agents.map((a) => [
|
|
3695
|
+
a.id,
|
|
3696
|
+
a
|
|
3697
|
+
]));
|
|
3698
|
+
for (const state of this.zoneStates.values()) {
|
|
3699
|
+
const zone = state.zone;
|
|
3700
|
+
const capacity = zone.capacity;
|
|
3701
|
+
let passingCount = 0;
|
|
3702
|
+
for (const entry of state.queue) {
|
|
3703
|
+
const agent = agentMap.get(entry.agentId);
|
|
3704
|
+
if (!agent) continue;
|
|
3705
|
+
const dx = agent.position.x - zone.center.x;
|
|
3706
|
+
const dy = agent.position.y - zone.center.y;
|
|
3707
|
+
const distSq = dx * dx + dy * dy;
|
|
3708
|
+
const isInsideZone = distSq <= zone.radius * zone.radius;
|
|
3709
|
+
const queuePosition = state.queue.findIndex((e) => e.agentId === entry.agentId);
|
|
3710
|
+
if (passingCount < capacity) {
|
|
3711
|
+
state.passingAgents.add(entry.agentId);
|
|
3712
|
+
passingCount++;
|
|
3713
|
+
const speedMult = isInsideZone && zone.congestionLevel > 0.5 ? 1 - (zone.congestionLevel - 0.5) : 1;
|
|
3714
|
+
this.agentResults.set(entry.agentId, {
|
|
3715
|
+
permission: PassPermission.Proceed,
|
|
3716
|
+
waitPosition: null,
|
|
3717
|
+
speedMultiplier: speedMult,
|
|
3718
|
+
zone,
|
|
3719
|
+
queuePosition
|
|
3720
|
+
});
|
|
3721
|
+
} else if (state.passingAgents.has(entry.agentId) && isInsideZone) {
|
|
3722
|
+
this.agentResults.set(entry.agentId, {
|
|
3723
|
+
permission: PassPermission.Yield,
|
|
3724
|
+
waitPosition: null,
|
|
3725
|
+
speedMultiplier: this.config.yieldSpeedMultiplier,
|
|
3726
|
+
zone,
|
|
3727
|
+
queuePosition
|
|
3728
|
+
});
|
|
3729
|
+
} else {
|
|
3730
|
+
const waitPos = this.computeWaitPosition(agent, zone);
|
|
3731
|
+
this.agentResults.set(entry.agentId, {
|
|
3732
|
+
permission: PassPermission.Wait,
|
|
3733
|
+
waitPosition: waitPos,
|
|
3734
|
+
speedMultiplier: 0,
|
|
3735
|
+
zone,
|
|
3736
|
+
queuePosition
|
|
3737
|
+
});
|
|
3738
|
+
}
|
|
3739
|
+
}
|
|
3740
|
+
}
|
|
3741
|
+
}
|
|
3742
|
+
/**
|
|
3743
|
+
* @zh 计算等待位置
|
|
3744
|
+
* @en Compute wait position
|
|
3745
|
+
*/
|
|
3746
|
+
computeWaitPosition(agent, zone) {
|
|
3747
|
+
const dx = agent.position.x - zone.center.x;
|
|
3748
|
+
const dy = agent.position.y - zone.center.y;
|
|
3749
|
+
const dist = Math.sqrt(dx * dx + dy * dy);
|
|
3750
|
+
if (dist < 1e-3) {
|
|
3751
|
+
return {
|
|
3752
|
+
x: zone.center.x + zone.radius + this.config.waitPointDistance,
|
|
3753
|
+
y: zone.center.y
|
|
3754
|
+
};
|
|
3755
|
+
}
|
|
3756
|
+
const dirX = dx / dist;
|
|
3757
|
+
const dirY = dy / dist;
|
|
3758
|
+
const waitDist = zone.radius + this.config.waitPointDistance;
|
|
3759
|
+
return {
|
|
3760
|
+
x: zone.center.x + dirX * waitDist,
|
|
3761
|
+
y: zone.center.y + dirY * waitDist
|
|
3762
|
+
};
|
|
3763
|
+
}
|
|
3764
|
+
/**
|
|
3765
|
+
* @zh 清理空的动态区域
|
|
3766
|
+
* @en Cleanup empty dynamic zones
|
|
3767
|
+
*/
|
|
3768
|
+
cleanupEmptyZones() {
|
|
3769
|
+
const toRemove = [];
|
|
3770
|
+
for (const [zoneId, state] of this.zoneStates) {
|
|
3771
|
+
if (!state.isStatic && state.queue.length === 0) {
|
|
3772
|
+
toRemove.push(zoneId);
|
|
3773
|
+
}
|
|
3774
|
+
}
|
|
3775
|
+
for (const zoneId of toRemove) {
|
|
3776
|
+
this.zoneStates.delete(zoneId);
|
|
3777
|
+
}
|
|
3778
|
+
}
|
|
3779
|
+
};
|
|
3780
|
+
__name(_FlowController, "FlowController");
|
|
3781
|
+
var FlowController = _FlowController;
|
|
3782
|
+
function createFlowController(config) {
|
|
3783
|
+
return new FlowController(config);
|
|
3784
|
+
}
|
|
3785
|
+
__name(createFlowController, "createFlowController");
|
|
3786
|
+
|
|
3787
|
+
export {
|
|
3788
|
+
createPoint,
|
|
3789
|
+
EMPTY_PATH_RESULT,
|
|
3790
|
+
manhattanDistance,
|
|
3791
|
+
euclideanDistance,
|
|
3792
|
+
chebyshevDistance,
|
|
3793
|
+
octileDistance,
|
|
3794
|
+
DEFAULT_PATHFINDING_OPTIONS,
|
|
3795
|
+
BinaryHeap,
|
|
3796
|
+
IndexedBinaryHeap,
|
|
3797
|
+
AStarPathfinder,
|
|
3798
|
+
createAStarPathfinder,
|
|
3799
|
+
DEFAULT_PATH_CACHE_CONFIG,
|
|
3800
|
+
PathCache,
|
|
3801
|
+
createPathCache,
|
|
3802
|
+
IncrementalAStarPathfinder,
|
|
3803
|
+
createIncrementalAStarPathfinder,
|
|
3804
|
+
JPSPathfinder,
|
|
3805
|
+
createJPSPathfinder,
|
|
3806
|
+
DEFAULT_HPA_CONFIG,
|
|
3807
|
+
HPAPathfinder,
|
|
3808
|
+
createHPAPathfinder,
|
|
3809
|
+
EMPTY_PLAN_RESULT,
|
|
3810
|
+
PathPlanState,
|
|
3811
|
+
isIncrementalPlanner,
|
|
3812
|
+
EMPTY_COLLISION_RESULT,
|
|
3813
|
+
PassPermission,
|
|
3814
|
+
DEFAULT_FLOW_CONTROLLER_CONFIG,
|
|
3815
|
+
NavMeshPathPlannerAdapter,
|
|
3816
|
+
createNavMeshPathPlanner,
|
|
3817
|
+
GridPathfinderAdapter,
|
|
3818
|
+
createAStarPlanner,
|
|
3819
|
+
createJPSPlanner,
|
|
3820
|
+
createHPAPlanner,
|
|
3821
|
+
IncrementalGridPathPlannerAdapter,
|
|
3822
|
+
createIncrementalAStarPlanner,
|
|
3823
|
+
DEFAULT_ORCA_PARAMS,
|
|
3824
|
+
ORCALocalAvoidanceAdapter,
|
|
3825
|
+
createORCAAvoidance,
|
|
3826
|
+
CollisionResolverAdapter,
|
|
3827
|
+
createDefaultCollisionResolver,
|
|
3828
|
+
FlowController,
|
|
3829
|
+
createFlowController
|
|
3830
|
+
};
|
|
3831
|
+
//# sourceMappingURL=chunk-ZYGBA7VK.js.map
|