@flare-engine/pathfinding 0.2.1 → 0.3.0
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- package/LICENSE +201 -21
- package/NOTICE +2 -0
- package/README.md +4 -5
- package/dist/index.d.mts +56 -72
- package/dist/index.d.mts.map +1 -0
- package/dist/index.d.ts +56 -72
- package/dist/index.d.ts.map +1 -0
- package/dist/index.js +655 -640
- package/dist/index.js.map +1 -1
- package/dist/index.mjs +640 -599
- package/dist/index.mjs.map +1 -1
- package/package.json +17 -9
package/dist/index.mjs
CHANGED
|
@@ -1,655 +1,696 @@
|
|
|
1
|
-
|
|
1
|
+
//#region src/grid.ts
|
|
2
2
|
function createGrid(config) {
|
|
3
|
-
|
|
4
|
-
|
|
5
|
-
|
|
6
|
-
|
|
7
|
-
|
|
8
|
-
|
|
9
|
-
|
|
10
|
-
|
|
11
|
-
|
|
12
|
-
|
|
13
|
-
|
|
14
|
-
|
|
15
|
-
|
|
16
|
-
|
|
17
|
-
|
|
18
|
-
|
|
19
|
-
|
|
20
|
-
}
|
|
21
|
-
return { width: w, height: h, data };
|
|
3
|
+
const { width, height, defaultCost = 1 } = config;
|
|
4
|
+
if (width <= 0 || height <= 0) throw new RangeError("Grid dimensions must be positive");
|
|
5
|
+
const w = width | 0;
|
|
6
|
+
const h = height | 0;
|
|
7
|
+
let data;
|
|
8
|
+
if (config.data) {
|
|
9
|
+
if (config.data.length !== w * h) throw new RangeError(`Grid data length ${config.data.length} does not match ${w}x${h}`);
|
|
10
|
+
data = config.data;
|
|
11
|
+
} else {
|
|
12
|
+
data = new Uint8Array(w * h);
|
|
13
|
+
if (defaultCost > 0) data.fill(defaultCost);
|
|
14
|
+
}
|
|
15
|
+
return {
|
|
16
|
+
width: w,
|
|
17
|
+
height: h,
|
|
18
|
+
data
|
|
19
|
+
};
|
|
22
20
|
}
|
|
23
21
|
function inBounds(grid, x, y) {
|
|
24
|
-
|
|
22
|
+
return x >= 0 && y >= 0 && x < grid.width && y < grid.height;
|
|
25
23
|
}
|
|
26
24
|
function isWalkable(grid, x, y) {
|
|
27
|
-
|
|
28
|
-
|
|
25
|
+
if (x < 0 || y < 0 || x >= grid.width || y >= grid.height) return false;
|
|
26
|
+
return grid.data[(y | 0) * grid.width + (x | 0)] > 0;
|
|
29
27
|
}
|
|
30
28
|
function getCost(grid, x, y) {
|
|
31
|
-
|
|
32
|
-
|
|
29
|
+
if (x < 0 || y < 0 || x >= grid.width || y >= grid.height) return 0;
|
|
30
|
+
return grid.data[(y | 0) * grid.width + (x | 0)];
|
|
33
31
|
}
|
|
34
32
|
function setWalkable(grid, x, y, walkable) {
|
|
35
|
-
|
|
36
|
-
|
|
33
|
+
if (x < 0 || y < 0 || x >= grid.width || y >= grid.height) return;
|
|
34
|
+
grid.data[(y | 0) * grid.width + (x | 0)] = walkable ? 1 : 0;
|
|
37
35
|
}
|
|
38
36
|
function setCost(grid, x, y, cost) {
|
|
39
|
-
|
|
40
|
-
|
|
37
|
+
if (x < 0 || y < 0 || x >= grid.width || y >= grid.height) return;
|
|
38
|
+
grid.data[(y | 0) * grid.width + (x | 0)] = cost > 255 ? 255 : cost < 0 ? 0 : cost | 0;
|
|
41
39
|
}
|
|
42
|
-
|
|
43
|
-
|
|
44
|
-
|
|
40
|
+
//#endregion
|
|
41
|
+
//#region src/astar.ts
|
|
42
|
+
const SQRT2$1 = Math.SQRT2;
|
|
45
43
|
function manhattanDistance(ax, ay, bx, by) {
|
|
46
|
-
|
|
44
|
+
return Math.abs(ax - bx) + Math.abs(ay - by);
|
|
47
45
|
}
|
|
48
46
|
function octileDistance(ax, ay, bx, by) {
|
|
49
|
-
|
|
50
|
-
|
|
51
|
-
|
|
47
|
+
const dx = Math.abs(ax - bx);
|
|
48
|
+
const dy = Math.abs(ay - by);
|
|
49
|
+
return dx > dy ? dx + (SQRT2$1 - 1) * dy : dy + (SQRT2$1 - 1) * dx;
|
|
52
50
|
}
|
|
53
51
|
function chebyshevDistance(ax, ay, bx, by) {
|
|
54
|
-
|
|
52
|
+
return Math.max(Math.abs(ax - bx), Math.abs(ay - by));
|
|
55
53
|
}
|
|
56
54
|
function euclideanDistance(ax, ay, bx, by) {
|
|
57
|
-
|
|
58
|
-
|
|
59
|
-
|
|
55
|
+
const dx = ax - bx;
|
|
56
|
+
const dy = ay - by;
|
|
57
|
+
return Math.sqrt(dx * dx + dy * dy);
|
|
58
|
+
}
|
|
59
|
+
function heapCreate$1(capacity) {
|
|
60
|
+
return {
|
|
61
|
+
f: new Float64Array(capacity),
|
|
62
|
+
idx: new Int32Array(capacity),
|
|
63
|
+
size: 0
|
|
64
|
+
};
|
|
65
|
+
}
|
|
66
|
+
function heapPush$1(h, f, nodeIdx) {
|
|
67
|
+
let i = h.size++;
|
|
68
|
+
h.f[i] = f;
|
|
69
|
+
h.idx[i] = nodeIdx;
|
|
70
|
+
while (i > 0) {
|
|
71
|
+
const parent = i - 1 >> 1;
|
|
72
|
+
if (h.f[parent] <= h.f[i]) break;
|
|
73
|
+
const tf = h.f[parent];
|
|
74
|
+
const ti = h.idx[parent];
|
|
75
|
+
h.f[parent] = h.f[i];
|
|
76
|
+
h.idx[parent] = h.idx[i];
|
|
77
|
+
h.f[i] = tf;
|
|
78
|
+
h.idx[i] = ti;
|
|
79
|
+
i = parent;
|
|
80
|
+
}
|
|
81
|
+
}
|
|
82
|
+
function heapPop$1(h) {
|
|
83
|
+
const result = h.idx[0];
|
|
84
|
+
h.size--;
|
|
85
|
+
if (h.size > 0) {
|
|
86
|
+
h.f[0] = h.f[h.size];
|
|
87
|
+
h.idx[0] = h.idx[h.size];
|
|
88
|
+
let i = 0;
|
|
89
|
+
for (;;) {
|
|
90
|
+
let smallest = i;
|
|
91
|
+
const left = 2 * i + 1;
|
|
92
|
+
const right = 2 * i + 2;
|
|
93
|
+
if (left < h.size && h.f[left] < h.f[smallest]) smallest = left;
|
|
94
|
+
if (right < h.size && h.f[right] < h.f[smallest]) smallest = right;
|
|
95
|
+
if (smallest === i) break;
|
|
96
|
+
const tf = h.f[i];
|
|
97
|
+
const ti = h.idx[i];
|
|
98
|
+
h.f[i] = h.f[smallest];
|
|
99
|
+
h.idx[i] = h.idx[smallest];
|
|
100
|
+
h.f[smallest] = tf;
|
|
101
|
+
h.idx[smallest] = ti;
|
|
102
|
+
i = smallest;
|
|
103
|
+
}
|
|
104
|
+
}
|
|
105
|
+
return result;
|
|
106
|
+
}
|
|
107
|
+
const DIR4_X = [
|
|
108
|
+
0,
|
|
109
|
+
1,
|
|
110
|
+
0,
|
|
111
|
+
-1
|
|
112
|
+
];
|
|
113
|
+
const DIR4_Y = [
|
|
114
|
+
-1,
|
|
115
|
+
0,
|
|
116
|
+
1,
|
|
117
|
+
0
|
|
118
|
+
];
|
|
119
|
+
const DIR8_X = [
|
|
120
|
+
0,
|
|
121
|
+
1,
|
|
122
|
+
1,
|
|
123
|
+
1,
|
|
124
|
+
0,
|
|
125
|
+
-1,
|
|
126
|
+
-1,
|
|
127
|
+
-1
|
|
128
|
+
];
|
|
129
|
+
const DIR8_Y = [
|
|
130
|
+
-1,
|
|
131
|
+
-1,
|
|
132
|
+
0,
|
|
133
|
+
1,
|
|
134
|
+
1,
|
|
135
|
+
1,
|
|
136
|
+
0,
|
|
137
|
+
-1
|
|
138
|
+
];
|
|
139
|
+
const DIR8_COST = [
|
|
140
|
+
1,
|
|
141
|
+
SQRT2$1,
|
|
142
|
+
1,
|
|
143
|
+
SQRT2$1,
|
|
144
|
+
1,
|
|
145
|
+
SQRT2$1,
|
|
146
|
+
1,
|
|
147
|
+
SQRT2$1
|
|
148
|
+
];
|
|
149
|
+
function findPath(grid, startX, startY, endX, endY, options) {
|
|
150
|
+
const sx = startX | 0;
|
|
151
|
+
const sy = startY | 0;
|
|
152
|
+
const ex = endX | 0;
|
|
153
|
+
const ey = endY | 0;
|
|
154
|
+
if (!isWalkable(grid, sx, sy) || !isWalkable(grid, ex, ey)) return null;
|
|
155
|
+
if (sx === ex && sy === ey) return {
|
|
156
|
+
path: [{
|
|
157
|
+
x: sx,
|
|
158
|
+
y: sy
|
|
159
|
+
}],
|
|
160
|
+
cost: 0,
|
|
161
|
+
explored: 0
|
|
162
|
+
};
|
|
163
|
+
const diagonal = options?.diagonal !== false;
|
|
164
|
+
const maxIter = options?.maxIterations ?? grid.width * grid.height;
|
|
165
|
+
const heuristic = options?.heuristic ?? (diagonal ? octileDistance : manhattanDistance);
|
|
166
|
+
const w = grid.width;
|
|
167
|
+
const total = w * grid.height;
|
|
168
|
+
const g = new Float64Array(total);
|
|
169
|
+
g.fill(Number.POSITIVE_INFINITY);
|
|
170
|
+
const closed = new Uint8Array(total);
|
|
171
|
+
const cameFrom = new Int32Array(total);
|
|
172
|
+
cameFrom.fill(-1);
|
|
173
|
+
const startIdx = sy * w + sx;
|
|
174
|
+
const endIdx = ey * w + ex;
|
|
175
|
+
g[startIdx] = 0;
|
|
176
|
+
const heap = heapCreate$1(Math.min(total, 4096));
|
|
177
|
+
heapPush$1(heap, heuristic(sx, sy, ex, ey), startIdx);
|
|
178
|
+
const dirX = diagonal ? DIR8_X : DIR4_X;
|
|
179
|
+
const dirY = diagonal ? DIR8_Y : DIR4_Y;
|
|
180
|
+
const dirCost = diagonal ? DIR8_COST : null;
|
|
181
|
+
const dirCount = dirX.length;
|
|
182
|
+
let explored = 0;
|
|
183
|
+
while (heap.size > 0 && explored < maxIter) {
|
|
184
|
+
const currentIdx = heapPop$1(heap);
|
|
185
|
+
if (currentIdx === endIdx) return reconstructPath(cameFrom, w, currentIdx, g[currentIdx], explored);
|
|
186
|
+
if (closed[currentIdx]) continue;
|
|
187
|
+
closed[currentIdx] = 1;
|
|
188
|
+
explored++;
|
|
189
|
+
const cx = currentIdx % w;
|
|
190
|
+
const cy = currentIdx / w | 0;
|
|
191
|
+
const currentG = g[currentIdx];
|
|
192
|
+
for (let d = 0; d < dirCount; d++) {
|
|
193
|
+
const nx = cx + dirX[d];
|
|
194
|
+
const ny = cy + dirY[d];
|
|
195
|
+
if (nx < 0 || ny < 0 || nx >= w || ny >= grid.height) continue;
|
|
196
|
+
const nIdx = ny * w + nx;
|
|
197
|
+
if (closed[nIdx]) continue;
|
|
198
|
+
const tileCost = grid.data[nIdx];
|
|
199
|
+
if (tileCost === 0) continue;
|
|
200
|
+
if (diagonal && d & 1) {
|
|
201
|
+
const dx = dirX[d];
|
|
202
|
+
const dy = dirY[d];
|
|
203
|
+
if (!isWalkable(grid, cx + dx, cy) || !isWalkable(grid, cx, cy + dy)) continue;
|
|
204
|
+
}
|
|
205
|
+
const tentativeG = currentG + (dirCost ? dirCost[d] * tileCost : tileCost);
|
|
206
|
+
if (tentativeG < g[nIdx]) {
|
|
207
|
+
g[nIdx] = tentativeG;
|
|
208
|
+
cameFrom[nIdx] = currentIdx;
|
|
209
|
+
heapPush$1(heap, tentativeG + heuristic(nx, ny, ex, ey), nIdx);
|
|
210
|
+
}
|
|
211
|
+
}
|
|
212
|
+
}
|
|
213
|
+
return null;
|
|
60
214
|
}
|
|
215
|
+
function reconstructPath(cameFrom, w, endIdx, cost, explored) {
|
|
216
|
+
const path = [];
|
|
217
|
+
let idx = endIdx;
|
|
218
|
+
while (idx !== -1) {
|
|
219
|
+
path.push({
|
|
220
|
+
x: idx % w,
|
|
221
|
+
y: idx / w | 0
|
|
222
|
+
});
|
|
223
|
+
idx = cameFrom[idx];
|
|
224
|
+
}
|
|
225
|
+
path.reverse();
|
|
226
|
+
return {
|
|
227
|
+
path,
|
|
228
|
+
cost,
|
|
229
|
+
explored
|
|
230
|
+
};
|
|
231
|
+
}
|
|
232
|
+
//#endregion
|
|
233
|
+
//#region src/jps.ts
|
|
61
234
|
function heapCreate(capacity) {
|
|
62
|
-
|
|
63
|
-
|
|
64
|
-
|
|
65
|
-
|
|
66
|
-
|
|
235
|
+
return {
|
|
236
|
+
f: new Float64Array(capacity),
|
|
237
|
+
idx: new Int32Array(capacity),
|
|
238
|
+
size: 0
|
|
239
|
+
};
|
|
67
240
|
}
|
|
68
241
|
function heapPush(h, f, nodeIdx) {
|
|
69
|
-
|
|
70
|
-
|
|
71
|
-
|
|
72
|
-
|
|
73
|
-
|
|
74
|
-
|
|
75
|
-
|
|
76
|
-
|
|
77
|
-
|
|
78
|
-
|
|
79
|
-
|
|
80
|
-
|
|
81
|
-
|
|
82
|
-
|
|
242
|
+
let i = h.size++;
|
|
243
|
+
h.f[i] = f;
|
|
244
|
+
h.idx[i] = nodeIdx;
|
|
245
|
+
while (i > 0) {
|
|
246
|
+
const parent = i - 1 >> 1;
|
|
247
|
+
if (h.f[parent] <= h.f[i]) break;
|
|
248
|
+
const tf = h.f[parent];
|
|
249
|
+
const ti = h.idx[parent];
|
|
250
|
+
h.f[parent] = h.f[i];
|
|
251
|
+
h.idx[parent] = h.idx[i];
|
|
252
|
+
h.f[i] = tf;
|
|
253
|
+
h.idx[i] = ti;
|
|
254
|
+
i = parent;
|
|
255
|
+
}
|
|
83
256
|
}
|
|
84
257
|
function heapPop(h) {
|
|
85
|
-
|
|
86
|
-
|
|
87
|
-
|
|
88
|
-
|
|
89
|
-
|
|
90
|
-
|
|
91
|
-
|
|
92
|
-
|
|
93
|
-
|
|
94
|
-
|
|
95
|
-
|
|
96
|
-
|
|
97
|
-
|
|
98
|
-
|
|
99
|
-
|
|
100
|
-
|
|
101
|
-
|
|
102
|
-
|
|
103
|
-
|
|
104
|
-
|
|
105
|
-
|
|
106
|
-
|
|
107
|
-
|
|
108
|
-
}
|
|
109
|
-
|
|
110
|
-
var DIR4_Y = [-1, 0, 1, 0];
|
|
111
|
-
var DIR8_X = [0, 1, 1, 1, 0, -1, -1, -1];
|
|
112
|
-
var DIR8_Y = [-1, -1, 0, 1, 1, 1, 0, -1];
|
|
113
|
-
var DIR8_COST = [1, SQRT2, 1, SQRT2, 1, SQRT2, 1, SQRT2];
|
|
114
|
-
function findPath(grid, startX, startY, endX, endY, options) {
|
|
115
|
-
const sx = startX | 0;
|
|
116
|
-
const sy = startY | 0;
|
|
117
|
-
const ex = endX | 0;
|
|
118
|
-
const ey = endY | 0;
|
|
119
|
-
if (!isWalkable(grid, sx, sy) || !isWalkable(grid, ex, ey)) return null;
|
|
120
|
-
if (sx === ex && sy === ey) return { path: [{ x: sx, y: sy }], cost: 0, explored: 0 };
|
|
121
|
-
const diagonal = options?.diagonal !== false;
|
|
122
|
-
const maxIter = options?.maxIterations ?? grid.width * grid.height;
|
|
123
|
-
const heuristic = options?.heuristic ?? (diagonal ? octileDistance : manhattanDistance);
|
|
124
|
-
const w = grid.width;
|
|
125
|
-
const total = w * grid.height;
|
|
126
|
-
const g = new Float64Array(total);
|
|
127
|
-
g.fill(Number.POSITIVE_INFINITY);
|
|
128
|
-
const closed = new Uint8Array(total);
|
|
129
|
-
const cameFrom = new Int32Array(total);
|
|
130
|
-
cameFrom.fill(-1);
|
|
131
|
-
const startIdx = sy * w + sx;
|
|
132
|
-
const endIdx = ey * w + ex;
|
|
133
|
-
g[startIdx] = 0;
|
|
134
|
-
const heap = heapCreate(Math.min(total, 4096));
|
|
135
|
-
heapPush(heap, heuristic(sx, sy, ex, ey), startIdx);
|
|
136
|
-
const dirX = diagonal ? DIR8_X : DIR4_X;
|
|
137
|
-
const dirY = diagonal ? DIR8_Y : DIR4_Y;
|
|
138
|
-
const dirCost = diagonal ? DIR8_COST : null;
|
|
139
|
-
const dirCount = dirX.length;
|
|
140
|
-
let explored = 0;
|
|
141
|
-
while (heap.size > 0 && explored < maxIter) {
|
|
142
|
-
const currentIdx = heapPop(heap);
|
|
143
|
-
if (currentIdx === endIdx) {
|
|
144
|
-
return reconstructPath(cameFrom, w, currentIdx, g[currentIdx], explored);
|
|
145
|
-
}
|
|
146
|
-
if (closed[currentIdx]) continue;
|
|
147
|
-
closed[currentIdx] = 1;
|
|
148
|
-
explored++;
|
|
149
|
-
const cx = currentIdx % w;
|
|
150
|
-
const cy = currentIdx / w | 0;
|
|
151
|
-
const currentG = g[currentIdx];
|
|
152
|
-
for (let d = 0; d < dirCount; d++) {
|
|
153
|
-
const nx = cx + dirX[d];
|
|
154
|
-
const ny = cy + dirY[d];
|
|
155
|
-
if (nx < 0 || ny < 0 || nx >= w || ny >= grid.height) continue;
|
|
156
|
-
const nIdx = ny * w + nx;
|
|
157
|
-
if (closed[nIdx]) continue;
|
|
158
|
-
const tileCost = grid.data[nIdx];
|
|
159
|
-
if (tileCost === 0) continue;
|
|
160
|
-
if (diagonal && d & 1) {
|
|
161
|
-
const dx = dirX[d];
|
|
162
|
-
const dy = dirY[d];
|
|
163
|
-
if (!isWalkable(grid, cx + dx, cy) || !isWalkable(grid, cx, cy + dy)) continue;
|
|
164
|
-
}
|
|
165
|
-
const moveCost = dirCost ? dirCost[d] * tileCost : tileCost;
|
|
166
|
-
const tentativeG = currentG + moveCost;
|
|
167
|
-
if (tentativeG < g[nIdx]) {
|
|
168
|
-
g[nIdx] = tentativeG;
|
|
169
|
-
cameFrom[nIdx] = currentIdx;
|
|
170
|
-
const f = tentativeG + heuristic(nx, ny, ex, ey);
|
|
171
|
-
heapPush(heap, f, nIdx);
|
|
172
|
-
}
|
|
173
|
-
}
|
|
174
|
-
}
|
|
175
|
-
return null;
|
|
176
|
-
}
|
|
177
|
-
function reconstructPath(cameFrom, w, endIdx, cost, explored) {
|
|
178
|
-
const path = [];
|
|
179
|
-
let idx = endIdx;
|
|
180
|
-
while (idx !== -1) {
|
|
181
|
-
path.push({ x: idx % w, y: idx / w | 0 });
|
|
182
|
-
idx = cameFrom[idx];
|
|
183
|
-
}
|
|
184
|
-
path.reverse();
|
|
185
|
-
return { path, cost, explored };
|
|
186
|
-
}
|
|
187
|
-
|
|
188
|
-
// src/jps.ts
|
|
189
|
-
function heapCreate2(capacity) {
|
|
190
|
-
return {
|
|
191
|
-
f: new Float64Array(capacity),
|
|
192
|
-
idx: new Int32Array(capacity),
|
|
193
|
-
size: 0
|
|
194
|
-
};
|
|
195
|
-
}
|
|
196
|
-
function heapPush2(h, f, nodeIdx) {
|
|
197
|
-
let i = h.size++;
|
|
198
|
-
h.f[i] = f;
|
|
199
|
-
h.idx[i] = nodeIdx;
|
|
200
|
-
while (i > 0) {
|
|
201
|
-
const parent = i - 1 >> 1;
|
|
202
|
-
if (h.f[parent] <= h.f[i]) break;
|
|
203
|
-
const tf = h.f[parent];
|
|
204
|
-
const ti = h.idx[parent];
|
|
205
|
-
h.f[parent] = h.f[i];
|
|
206
|
-
h.idx[parent] = h.idx[i];
|
|
207
|
-
h.f[i] = tf;
|
|
208
|
-
h.idx[i] = ti;
|
|
209
|
-
i = parent;
|
|
210
|
-
}
|
|
211
|
-
}
|
|
212
|
-
function heapPop2(h) {
|
|
213
|
-
const result = h.idx[0];
|
|
214
|
-
h.size--;
|
|
215
|
-
if (h.size > 0) {
|
|
216
|
-
h.f[0] = h.f[h.size];
|
|
217
|
-
h.idx[0] = h.idx[h.size];
|
|
218
|
-
let i = 0;
|
|
219
|
-
for (; ; ) {
|
|
220
|
-
let smallest = i;
|
|
221
|
-
const left = 2 * i + 1;
|
|
222
|
-
const right = 2 * i + 2;
|
|
223
|
-
if (left < h.size && h.f[left] < h.f[smallest]) smallest = left;
|
|
224
|
-
if (right < h.size && h.f[right] < h.f[smallest]) smallest = right;
|
|
225
|
-
if (smallest === i) break;
|
|
226
|
-
const tf = h.f[i];
|
|
227
|
-
const ti = h.idx[i];
|
|
228
|
-
h.f[i] = h.f[smallest];
|
|
229
|
-
h.idx[i] = h.idx[smallest];
|
|
230
|
-
h.f[smallest] = tf;
|
|
231
|
-
h.idx[smallest] = ti;
|
|
232
|
-
i = smallest;
|
|
233
|
-
}
|
|
234
|
-
}
|
|
235
|
-
return result;
|
|
236
|
-
}
|
|
237
|
-
var SQRT22 = Math.SQRT2;
|
|
258
|
+
const result = h.idx[0];
|
|
259
|
+
h.size--;
|
|
260
|
+
if (h.size > 0) {
|
|
261
|
+
h.f[0] = h.f[h.size];
|
|
262
|
+
h.idx[0] = h.idx[h.size];
|
|
263
|
+
let i = 0;
|
|
264
|
+
for (;;) {
|
|
265
|
+
let smallest = i;
|
|
266
|
+
const left = 2 * i + 1;
|
|
267
|
+
const right = 2 * i + 2;
|
|
268
|
+
if (left < h.size && h.f[left] < h.f[smallest]) smallest = left;
|
|
269
|
+
if (right < h.size && h.f[right] < h.f[smallest]) smallest = right;
|
|
270
|
+
if (smallest === i) break;
|
|
271
|
+
const tf = h.f[i];
|
|
272
|
+
const ti = h.idx[i];
|
|
273
|
+
h.f[i] = h.f[smallest];
|
|
274
|
+
h.idx[i] = h.idx[smallest];
|
|
275
|
+
h.f[smallest] = tf;
|
|
276
|
+
h.idx[smallest] = ti;
|
|
277
|
+
i = smallest;
|
|
278
|
+
}
|
|
279
|
+
}
|
|
280
|
+
return result;
|
|
281
|
+
}
|
|
282
|
+
const SQRT2 = Math.SQRT2;
|
|
238
283
|
function findPathJPS(grid, startX, startY, endX, endY, options) {
|
|
239
|
-
|
|
240
|
-
|
|
241
|
-
|
|
242
|
-
|
|
243
|
-
|
|
244
|
-
|
|
245
|
-
|
|
246
|
-
|
|
247
|
-
|
|
248
|
-
|
|
249
|
-
|
|
250
|
-
|
|
251
|
-
|
|
252
|
-
|
|
253
|
-
|
|
254
|
-
|
|
255
|
-
|
|
256
|
-
|
|
257
|
-
|
|
258
|
-
|
|
259
|
-
|
|
260
|
-
|
|
261
|
-
|
|
262
|
-
|
|
263
|
-
|
|
264
|
-
|
|
265
|
-
|
|
266
|
-
|
|
267
|
-
|
|
268
|
-
|
|
269
|
-
|
|
270
|
-
|
|
271
|
-
|
|
272
|
-
|
|
273
|
-
|
|
274
|
-
|
|
275
|
-
|
|
276
|
-
|
|
277
|
-
|
|
278
|
-
|
|
279
|
-
|
|
280
|
-
|
|
281
|
-
|
|
282
|
-
|
|
283
|
-
|
|
284
|
-
|
|
285
|
-
|
|
286
|
-
|
|
287
|
-
|
|
288
|
-
|
|
289
|
-
|
|
290
|
-
|
|
284
|
+
const sx = startX | 0;
|
|
285
|
+
const sy = startY | 0;
|
|
286
|
+
const ex = endX | 0;
|
|
287
|
+
const ey = endY | 0;
|
|
288
|
+
if (!isWalkable(grid, sx, sy) || !isWalkable(grid, ex, ey)) return null;
|
|
289
|
+
if (sx === ex && sy === ey) return {
|
|
290
|
+
path: [{
|
|
291
|
+
x: sx,
|
|
292
|
+
y: sy
|
|
293
|
+
}],
|
|
294
|
+
cost: 0,
|
|
295
|
+
explored: 0
|
|
296
|
+
};
|
|
297
|
+
const w = grid.width;
|
|
298
|
+
const h = grid.height;
|
|
299
|
+
const total = w * h;
|
|
300
|
+
const maxIter = options?.maxIterations ?? total;
|
|
301
|
+
const heuristic = options?.heuristic ?? octileDistance;
|
|
302
|
+
const g = new Float64Array(total);
|
|
303
|
+
g.fill(Number.POSITIVE_INFINITY);
|
|
304
|
+
const closed = new Uint8Array(total);
|
|
305
|
+
const cameFrom = new Int32Array(total);
|
|
306
|
+
cameFrom.fill(-1);
|
|
307
|
+
const startIdx = sy * w + sx;
|
|
308
|
+
const endIdx = ey * w + ex;
|
|
309
|
+
g[startIdx] = 0;
|
|
310
|
+
const heap = heapCreate(Math.min(total, 4096));
|
|
311
|
+
heapPush(heap, heuristic(sx, sy, ex, ey), startIdx);
|
|
312
|
+
let explored = 0;
|
|
313
|
+
while (heap.size > 0 && explored < maxIter) {
|
|
314
|
+
const currentIdx = heapPop(heap);
|
|
315
|
+
if (currentIdx === endIdx) return buildPath(cameFrom, w, currentIdx, g[currentIdx], explored);
|
|
316
|
+
if (closed[currentIdx]) continue;
|
|
317
|
+
closed[currentIdx] = 1;
|
|
318
|
+
explored++;
|
|
319
|
+
const cx = currentIdx % w;
|
|
320
|
+
const cy = currentIdx / w | 0;
|
|
321
|
+
const neighbors = identifySuccessors(grid, cx, cy, cameFrom, w, h, ex, ey);
|
|
322
|
+
for (let i = 0; i < neighbors.length; i += 2) {
|
|
323
|
+
const nx = neighbors[i];
|
|
324
|
+
const ny = neighbors[i + 1];
|
|
325
|
+
const nIdx = ny * w + nx;
|
|
326
|
+
if (closed[nIdx]) continue;
|
|
327
|
+
const dx = Math.abs(nx - cx);
|
|
328
|
+
const dy = Math.abs(ny - cy);
|
|
329
|
+
const diag = Math.min(dx, dy);
|
|
330
|
+
const straight = Math.abs(dx - dy);
|
|
331
|
+
const dist = diag * SQRT2 + straight;
|
|
332
|
+
const tentativeG = g[currentIdx] + dist;
|
|
333
|
+
if (tentativeG < g[nIdx]) {
|
|
334
|
+
g[nIdx] = tentativeG;
|
|
335
|
+
cameFrom[nIdx] = currentIdx;
|
|
336
|
+
heapPush(heap, tentativeG + heuristic(nx, ny, ex, ey), nIdx);
|
|
337
|
+
}
|
|
338
|
+
}
|
|
339
|
+
}
|
|
340
|
+
return null;
|
|
291
341
|
}
|
|
292
342
|
function identifySuccessors(grid, cx, cy, cameFrom, w, h, ex, ey) {
|
|
293
|
-
|
|
294
|
-
|
|
295
|
-
|
|
296
|
-
|
|
297
|
-
|
|
298
|
-
|
|
299
|
-
|
|
300
|
-
|
|
301
|
-
|
|
302
|
-
|
|
303
|
-
|
|
304
|
-
|
|
305
|
-
|
|
306
|
-
|
|
307
|
-
|
|
308
|
-
|
|
309
|
-
|
|
310
|
-
|
|
311
|
-
|
|
312
|
-
|
|
313
|
-
|
|
314
|
-
|
|
315
|
-
|
|
316
|
-
|
|
317
|
-
|
|
318
|
-
|
|
319
|
-
|
|
320
|
-
|
|
321
|
-
|
|
322
|
-
|
|
323
|
-
|
|
324
|
-
|
|
325
|
-
|
|
326
|
-
|
|
327
|
-
|
|
328
|
-
|
|
329
|
-
|
|
330
|
-
|
|
331
|
-
|
|
332
|
-
|
|
333
|
-
|
|
334
|
-
|
|
335
|
-
|
|
336
|
-
|
|
337
|
-
|
|
338
|
-
|
|
339
|
-
|
|
340
|
-
|
|
341
|
-
|
|
342
|
-
|
|
343
|
-
|
|
344
|
-
|
|
345
|
-
|
|
346
|
-
|
|
347
|
-
|
|
348
|
-
|
|
349
|
-
|
|
350
|
-
|
|
351
|
-
|
|
352
|
-
|
|
353
|
-
|
|
354
|
-
|
|
355
|
-
if (jp !== -1) results.push(jp % w, jp / w | 0);
|
|
356
|
-
}
|
|
357
|
-
}
|
|
358
|
-
}
|
|
359
|
-
return results;
|
|
343
|
+
const results = [];
|
|
344
|
+
const parentIdx = cameFrom[cy * w + cx];
|
|
345
|
+
if (parentIdx === -1) for (let dx = -1; dx <= 1; dx++) for (let dy = -1; dy <= 1; dy++) {
|
|
346
|
+
if (dx === 0 && dy === 0) continue;
|
|
347
|
+
const jp = jump(grid, cx, cy, dx, dy, w, h, ex, ey);
|
|
348
|
+
if (jp !== -1) results.push(jp % w, jp / w | 0);
|
|
349
|
+
}
|
|
350
|
+
else {
|
|
351
|
+
const px = parentIdx % w;
|
|
352
|
+
const py = parentIdx / w | 0;
|
|
353
|
+
const dx = clampDir(cx - px);
|
|
354
|
+
const dy = clampDir(cy - py);
|
|
355
|
+
if (dx !== 0 && dy !== 0) {
|
|
356
|
+
if (isWalkable(grid, cx, cy + dy)) {
|
|
357
|
+
const jp = jump(grid, cx, cy, 0, dy, w, h, ex, ey);
|
|
358
|
+
if (jp !== -1) results.push(jp % w, jp / w | 0);
|
|
359
|
+
}
|
|
360
|
+
if (isWalkable(grid, cx + dx, cy)) {
|
|
361
|
+
const jp = jump(grid, cx, cy, dx, 0, w, h, ex, ey);
|
|
362
|
+
if (jp !== -1) results.push(jp % w, jp / w | 0);
|
|
363
|
+
}
|
|
364
|
+
if (isWalkable(grid, cx + dx, cy + dy)) {
|
|
365
|
+
const jp = jump(grid, cx, cy, dx, dy, w, h, ex, ey);
|
|
366
|
+
if (jp !== -1) results.push(jp % w, jp / w | 0);
|
|
367
|
+
}
|
|
368
|
+
if (!isWalkable(grid, cx - dx, cy) && isWalkable(grid, cx - dx, cy + dy)) {
|
|
369
|
+
const jp = jump(grid, cx, cy, -dx, dy, w, h, ex, ey);
|
|
370
|
+
if (jp !== -1) results.push(jp % w, jp / w | 0);
|
|
371
|
+
}
|
|
372
|
+
if (!isWalkable(grid, cx, cy - dy) && isWalkable(grid, cx + dx, cy - dy)) {
|
|
373
|
+
const jp = jump(grid, cx, cy, dx, -dy, w, h, ex, ey);
|
|
374
|
+
if (jp !== -1) results.push(jp % w, jp / w | 0);
|
|
375
|
+
}
|
|
376
|
+
} else if (dx !== 0) {
|
|
377
|
+
if (isWalkable(grid, cx + dx, cy)) {
|
|
378
|
+
const jp = jump(grid, cx, cy, dx, 0, w, h, ex, ey);
|
|
379
|
+
if (jp !== -1) results.push(jp % w, jp / w | 0);
|
|
380
|
+
}
|
|
381
|
+
if (!isWalkable(grid, cx, cy + 1) && isWalkable(grid, cx + dx, cy + 1)) {
|
|
382
|
+
const jp = jump(grid, cx, cy, dx, 1, w, h, ex, ey);
|
|
383
|
+
if (jp !== -1) results.push(jp % w, jp / w | 0);
|
|
384
|
+
}
|
|
385
|
+
if (!isWalkable(grid, cx, cy - 1) && isWalkable(grid, cx + dx, cy - 1)) {
|
|
386
|
+
const jp = jump(grid, cx, cy, dx, -1, w, h, ex, ey);
|
|
387
|
+
if (jp !== -1) results.push(jp % w, jp / w | 0);
|
|
388
|
+
}
|
|
389
|
+
} else {
|
|
390
|
+
if (isWalkable(grid, cx, cy + dy)) {
|
|
391
|
+
const jp = jump(grid, cx, cy, 0, dy, w, h, ex, ey);
|
|
392
|
+
if (jp !== -1) results.push(jp % w, jp / w | 0);
|
|
393
|
+
}
|
|
394
|
+
if (!isWalkable(grid, cx + 1, cy) && isWalkable(grid, cx + 1, cy + dy)) {
|
|
395
|
+
const jp = jump(grid, cx, cy, 1, dy, w, h, ex, ey);
|
|
396
|
+
if (jp !== -1) results.push(jp % w, jp / w | 0);
|
|
397
|
+
}
|
|
398
|
+
if (!isWalkable(grid, cx - 1, cy) && isWalkable(grid, cx - 1, cy + dy)) {
|
|
399
|
+
const jp = jump(grid, cx, cy, -1, dy, w, h, ex, ey);
|
|
400
|
+
if (jp !== -1) results.push(jp % w, jp / w | 0);
|
|
401
|
+
}
|
|
402
|
+
}
|
|
403
|
+
}
|
|
404
|
+
return results;
|
|
360
405
|
}
|
|
361
406
|
function jump(grid, cx, cy, dx, dy, w, h, ex, ey) {
|
|
362
|
-
|
|
363
|
-
|
|
364
|
-
|
|
365
|
-
|
|
366
|
-
|
|
367
|
-
|
|
368
|
-
|
|
369
|
-
|
|
370
|
-
|
|
371
|
-
|
|
372
|
-
|
|
373
|
-
|
|
374
|
-
return ny * w + nx;
|
|
375
|
-
}
|
|
376
|
-
} else {
|
|
377
|
-
if (!isWalkable(grid, nx + 1, ny) && isWalkable(grid, nx + 1, ny + dy) || !isWalkable(grid, nx - 1, ny) && isWalkable(grid, nx - 1, ny + dy)) {
|
|
378
|
-
return ny * w + nx;
|
|
379
|
-
}
|
|
380
|
-
}
|
|
381
|
-
return jump(grid, nx, ny, dx, dy, w, h, ex, ey);
|
|
407
|
+
const nx = cx + dx;
|
|
408
|
+
const ny = cy + dy;
|
|
409
|
+
if (!isWalkable(grid, nx, ny)) return -1;
|
|
410
|
+
if (nx === ex && ny === ey) return ny * w + nx;
|
|
411
|
+
if (dx !== 0 && dy !== 0) {
|
|
412
|
+
if (!isWalkable(grid, nx - dx, ny) && isWalkable(grid, nx - dx, ny + dy) || !isWalkable(grid, nx, ny - dy) && isWalkable(grid, nx + dx, ny - dy)) return ny * w + nx;
|
|
413
|
+
if (jump(grid, nx, ny, dx, 0, w, h, ex, ey) !== -1) return ny * w + nx;
|
|
414
|
+
if (jump(grid, nx, ny, 0, dy, w, h, ex, ey) !== -1) return ny * w + nx;
|
|
415
|
+
} else if (dx !== 0) {
|
|
416
|
+
if (!isWalkable(grid, nx, ny + 1) && isWalkable(grid, nx + dx, ny + 1) || !isWalkable(grid, nx, ny - 1) && isWalkable(grid, nx + dx, ny - 1)) return ny * w + nx;
|
|
417
|
+
} else if (!isWalkable(grid, nx + 1, ny) && isWalkable(grid, nx + 1, ny + dy) || !isWalkable(grid, nx - 1, ny) && isWalkable(grid, nx - 1, ny + dy)) return ny * w + nx;
|
|
418
|
+
return jump(grid, nx, ny, dx, dy, w, h, ex, ey);
|
|
382
419
|
}
|
|
383
420
|
function clampDir(v) {
|
|
384
|
-
|
|
421
|
+
return v > 0 ? 1 : v < 0 ? -1 : 0;
|
|
385
422
|
}
|
|
386
423
|
function buildPath(cameFrom, w, endIdx, cost, explored) {
|
|
387
|
-
|
|
388
|
-
|
|
389
|
-
|
|
390
|
-
|
|
391
|
-
|
|
392
|
-
|
|
393
|
-
|
|
394
|
-
|
|
395
|
-
|
|
396
|
-
|
|
397
|
-
|
|
398
|
-
|
|
399
|
-
|
|
400
|
-
|
|
401
|
-
|
|
402
|
-
|
|
403
|
-
|
|
404
|
-
|
|
405
|
-
|
|
406
|
-
|
|
407
|
-
|
|
408
|
-
|
|
409
|
-
|
|
410
|
-
|
|
411
|
-
|
|
412
|
-
|
|
413
|
-
|
|
414
|
-
|
|
415
|
-
}
|
|
416
|
-
|
|
417
|
-
|
|
424
|
+
const jumpPoints = [];
|
|
425
|
+
let idx = endIdx;
|
|
426
|
+
while (idx !== -1) {
|
|
427
|
+
jumpPoints.push(idx);
|
|
428
|
+
idx = cameFrom[idx];
|
|
429
|
+
}
|
|
430
|
+
jumpPoints.reverse();
|
|
431
|
+
const path = [];
|
|
432
|
+
for (let i = 0; i < jumpPoints.length; i++) {
|
|
433
|
+
const jpIdx = jumpPoints[i];
|
|
434
|
+
const jx = jpIdx % w;
|
|
435
|
+
const jy = jpIdx / w | 0;
|
|
436
|
+
if (i === 0) path.push({
|
|
437
|
+
x: jx,
|
|
438
|
+
y: jy
|
|
439
|
+
});
|
|
440
|
+
else {
|
|
441
|
+
const prev = path[path.length - 1];
|
|
442
|
+
let cx = prev.x;
|
|
443
|
+
let cy = prev.y;
|
|
444
|
+
while (cx !== jx || cy !== jy) {
|
|
445
|
+
if (cx < jx) cx++;
|
|
446
|
+
else if (cx > jx) cx--;
|
|
447
|
+
if (cy < jy) cy++;
|
|
448
|
+
else if (cy > jy) cy--;
|
|
449
|
+
path.push({
|
|
450
|
+
x: cx,
|
|
451
|
+
y: cy
|
|
452
|
+
});
|
|
453
|
+
}
|
|
454
|
+
}
|
|
455
|
+
}
|
|
456
|
+
return {
|
|
457
|
+
path,
|
|
458
|
+
cost,
|
|
459
|
+
explored
|
|
460
|
+
};
|
|
461
|
+
}
|
|
462
|
+
//#endregion
|
|
463
|
+
//#region src/navmesh.ts
|
|
418
464
|
function createNavMesh(polygons) {
|
|
419
|
-
|
|
465
|
+
return { polygons };
|
|
420
466
|
}
|
|
421
467
|
function pointInConvexPolygon(verts, px, py) {
|
|
422
|
-
|
|
423
|
-
|
|
424
|
-
|
|
425
|
-
|
|
426
|
-
|
|
427
|
-
|
|
428
|
-
|
|
429
|
-
|
|
430
|
-
|
|
431
|
-
|
|
432
|
-
|
|
433
|
-
|
|
434
|
-
|
|
435
|
-
|
|
468
|
+
const n = verts.length;
|
|
469
|
+
if (n < 3) return false;
|
|
470
|
+
let sign = 0;
|
|
471
|
+
for (let i = 0; i < n; i++) {
|
|
472
|
+
const a = verts[i];
|
|
473
|
+
const b = verts[(i + 1) % n];
|
|
474
|
+
const cross = (b.x - a.x) * (py - a.y) - (b.y - a.y) * (px - a.x);
|
|
475
|
+
if (cross !== 0) {
|
|
476
|
+
const s = cross > 0 ? 1 : -1;
|
|
477
|
+
if (sign === 0) sign = s;
|
|
478
|
+
else if (sign !== s) return false;
|
|
479
|
+
}
|
|
480
|
+
}
|
|
481
|
+
return true;
|
|
436
482
|
}
|
|
437
483
|
function findPolygonAt(mesh, x, y) {
|
|
438
|
-
|
|
439
|
-
|
|
440
|
-
return mesh.polygons[i].id;
|
|
441
|
-
}
|
|
442
|
-
}
|
|
443
|
-
return -1;
|
|
484
|
+
for (let i = 0; i < mesh.polygons.length; i++) if (pointInConvexPolygon(mesh.polygons[i].vertices, x, y)) return mesh.polygons[i].id;
|
|
485
|
+
return -1;
|
|
444
486
|
}
|
|
445
487
|
function centroid(verts) {
|
|
446
|
-
|
|
447
|
-
|
|
448
|
-
|
|
449
|
-
|
|
450
|
-
|
|
451
|
-
|
|
452
|
-
|
|
453
|
-
|
|
488
|
+
let cx = 0;
|
|
489
|
+
let cy = 0;
|
|
490
|
+
for (let i = 0; i < verts.length; i++) {
|
|
491
|
+
cx += verts[i].x;
|
|
492
|
+
cy += verts[i].y;
|
|
493
|
+
}
|
|
494
|
+
const n = verts.length;
|
|
495
|
+
return {
|
|
496
|
+
x: cx / n,
|
|
497
|
+
y: cy / n
|
|
498
|
+
};
|
|
454
499
|
}
|
|
455
500
|
function distSq(ax, ay, bx, by) {
|
|
456
|
-
|
|
457
|
-
|
|
458
|
-
|
|
501
|
+
const dx = ax - bx;
|
|
502
|
+
const dy = ay - by;
|
|
503
|
+
return dx * dx + dy * dy;
|
|
459
504
|
}
|
|
460
505
|
function dist(ax, ay, bx, by) {
|
|
461
|
-
|
|
506
|
+
return Math.sqrt(distSq(ax, ay, bx, by));
|
|
462
507
|
}
|
|
463
508
|
function findNavMeshPath(mesh, start, end) {
|
|
464
|
-
|
|
465
|
-
|
|
466
|
-
|
|
467
|
-
|
|
468
|
-
|
|
469
|
-
|
|
470
|
-
|
|
471
|
-
|
|
472
|
-
|
|
473
|
-
|
|
474
|
-
|
|
475
|
-
|
|
476
|
-
|
|
477
|
-
|
|
478
|
-
|
|
479
|
-
|
|
480
|
-
|
|
481
|
-
|
|
482
|
-
|
|
483
|
-
|
|
484
|
-
|
|
485
|
-
|
|
486
|
-
|
|
487
|
-
|
|
488
|
-
|
|
489
|
-
|
|
490
|
-
|
|
491
|
-
|
|
492
|
-
|
|
493
|
-
|
|
494
|
-
|
|
495
|
-
|
|
496
|
-
|
|
497
|
-
|
|
498
|
-
|
|
499
|
-
|
|
500
|
-
|
|
501
|
-
|
|
502
|
-
|
|
503
|
-
|
|
504
|
-
|
|
505
|
-
|
|
506
|
-
|
|
507
|
-
|
|
508
|
-
|
|
509
|
-
|
|
510
|
-
|
|
511
|
-
|
|
512
|
-
|
|
513
|
-
|
|
514
|
-
|
|
515
|
-
|
|
516
|
-
|
|
517
|
-
|
|
518
|
-
|
|
519
|
-
|
|
520
|
-
|
|
521
|
-
|
|
522
|
-
|
|
523
|
-
|
|
524
|
-
|
|
525
|
-
|
|
526
|
-
|
|
527
|
-
|
|
528
|
-
|
|
529
|
-
|
|
530
|
-
|
|
531
|
-
|
|
532
|
-
|
|
533
|
-
|
|
534
|
-
|
|
535
|
-
|
|
536
|
-
|
|
537
|
-
|
|
538
|
-
|
|
539
|
-
|
|
540
|
-
|
|
541
|
-
|
|
542
|
-
|
|
543
|
-
|
|
544
|
-
|
|
545
|
-
}
|
|
546
|
-
return null;
|
|
509
|
+
const startPolyId = findPolygonAt(mesh, start.x, start.y);
|
|
510
|
+
const endPolyId = findPolygonAt(mesh, end.x, end.y);
|
|
511
|
+
if (startPolyId === -1 || endPolyId === -1) return null;
|
|
512
|
+
if (startPolyId === endPolyId) return {
|
|
513
|
+
path: [{
|
|
514
|
+
x: start.x,
|
|
515
|
+
y: start.y
|
|
516
|
+
}, {
|
|
517
|
+
x: end.x,
|
|
518
|
+
y: end.y
|
|
519
|
+
}],
|
|
520
|
+
corridor: [startPolyId]
|
|
521
|
+
};
|
|
522
|
+
const byId = /* @__PURE__ */ new Map();
|
|
523
|
+
for (let i = 0; i < mesh.polygons.length; i++) {
|
|
524
|
+
const p = mesh.polygons[i];
|
|
525
|
+
byId.set(p.id, p);
|
|
526
|
+
}
|
|
527
|
+
const gScore = /* @__PURE__ */ new Map();
|
|
528
|
+
const fScore = /* @__PURE__ */ new Map();
|
|
529
|
+
const cameFrom = /* @__PURE__ */ new Map();
|
|
530
|
+
gScore.set(startPolyId, 0);
|
|
531
|
+
const endCenter = centroid(byId.get(endPolyId).vertices);
|
|
532
|
+
const startCenter = centroid(byId.get(startPolyId).vertices);
|
|
533
|
+
fScore.set(startPolyId, dist(startCenter.x, startCenter.y, endCenter.x, endCenter.y));
|
|
534
|
+
const open = [startPolyId];
|
|
535
|
+
const inOpen = /* @__PURE__ */ new Set([startPolyId]);
|
|
536
|
+
const closed = /* @__PURE__ */ new Set();
|
|
537
|
+
while (open.length > 0) {
|
|
538
|
+
let bestI = 0;
|
|
539
|
+
let bestF = fScore.get(open[0]) ?? Number.POSITIVE_INFINITY;
|
|
540
|
+
for (let i = 1; i < open.length; i++) {
|
|
541
|
+
const f = fScore.get(open[i]) ?? Number.POSITIVE_INFINITY;
|
|
542
|
+
if (f < bestF) {
|
|
543
|
+
bestF = f;
|
|
544
|
+
bestI = i;
|
|
545
|
+
}
|
|
546
|
+
}
|
|
547
|
+
const currentId = open[bestI];
|
|
548
|
+
if (currentId === endPolyId) {
|
|
549
|
+
const corridor = [];
|
|
550
|
+
let id = endPolyId;
|
|
551
|
+
while (id !== void 0) {
|
|
552
|
+
corridor.push(id);
|
|
553
|
+
id = cameFrom.get(id);
|
|
554
|
+
}
|
|
555
|
+
corridor.reverse();
|
|
556
|
+
return {
|
|
557
|
+
path: funnelSmooth(mesh, byId, corridor, start, end),
|
|
558
|
+
corridor
|
|
559
|
+
};
|
|
560
|
+
}
|
|
561
|
+
open[bestI] = open[open.length - 1];
|
|
562
|
+
open.length--;
|
|
563
|
+
inOpen.delete(currentId);
|
|
564
|
+
closed.add(currentId);
|
|
565
|
+
const current = byId.get(currentId);
|
|
566
|
+
const currentCenter = centroid(current.vertices);
|
|
567
|
+
const currentG = gScore.get(currentId);
|
|
568
|
+
for (let i = 0; i < current.portals.length; i++) {
|
|
569
|
+
const portal = current.portals[i];
|
|
570
|
+
const neighborId = portal.neighborId;
|
|
571
|
+
if (closed.has(neighborId)) continue;
|
|
572
|
+
const neighbor = byId.get(neighborId);
|
|
573
|
+
if (!neighbor) continue;
|
|
574
|
+
const portalMidX = (portal.a.x + portal.b.x) * .5;
|
|
575
|
+
const portalMidY = (portal.a.y + portal.b.y) * .5;
|
|
576
|
+
const tentativeG = currentG + dist(currentCenter.x, currentCenter.y, portalMidX, portalMidY);
|
|
577
|
+
const prevG = gScore.get(neighborId);
|
|
578
|
+
if (prevG !== void 0 && tentativeG >= prevG) continue;
|
|
579
|
+
gScore.set(neighborId, tentativeG);
|
|
580
|
+
cameFrom.set(neighborId, currentId);
|
|
581
|
+
const neighborCenter = centroid(neighbor.vertices);
|
|
582
|
+
fScore.set(neighborId, tentativeG + dist(neighborCenter.x, neighborCenter.y, endCenter.x, endCenter.y));
|
|
583
|
+
if (!inOpen.has(neighborId)) {
|
|
584
|
+
open.push(neighborId);
|
|
585
|
+
inOpen.add(neighborId);
|
|
586
|
+
}
|
|
587
|
+
}
|
|
588
|
+
}
|
|
589
|
+
return null;
|
|
547
590
|
}
|
|
548
591
|
function funnelSmooth(_mesh, byId, corridor, start, end) {
|
|
549
|
-
|
|
550
|
-
|
|
551
|
-
|
|
552
|
-
|
|
553
|
-
|
|
554
|
-
|
|
555
|
-
|
|
556
|
-
|
|
557
|
-
|
|
558
|
-
|
|
559
|
-
|
|
560
|
-
|
|
561
|
-
|
|
562
|
-
|
|
563
|
-
|
|
564
|
-
|
|
565
|
-
|
|
566
|
-
|
|
567
|
-
|
|
568
|
-
|
|
569
|
-
|
|
570
|
-
|
|
571
|
-
|
|
572
|
-
|
|
573
|
-
|
|
574
|
-
|
|
575
|
-
|
|
576
|
-
|
|
577
|
-
|
|
578
|
-
|
|
579
|
-
|
|
580
|
-
|
|
581
|
-
|
|
582
|
-
|
|
583
|
-
|
|
584
|
-
|
|
585
|
-
|
|
586
|
-
|
|
587
|
-
|
|
588
|
-
|
|
589
|
-
|
|
590
|
-
|
|
591
|
-
|
|
592
|
-
|
|
593
|
-
|
|
594
|
-
|
|
595
|
-
|
|
596
|
-
|
|
597
|
-
|
|
598
|
-
|
|
599
|
-
|
|
600
|
-
|
|
601
|
-
|
|
602
|
-
|
|
603
|
-
|
|
604
|
-
|
|
605
|
-
|
|
606
|
-
|
|
607
|
-
|
|
608
|
-
|
|
609
|
-
|
|
610
|
-
|
|
611
|
-
|
|
612
|
-
|
|
613
|
-
|
|
614
|
-
|
|
615
|
-
|
|
616
|
-
|
|
617
|
-
|
|
618
|
-
|
|
619
|
-
|
|
620
|
-
|
|
621
|
-
|
|
622
|
-
|
|
623
|
-
|
|
624
|
-
|
|
592
|
+
if (corridor.length <= 1) return [{
|
|
593
|
+
x: start.x,
|
|
594
|
+
y: start.y
|
|
595
|
+
}, {
|
|
596
|
+
x: end.x,
|
|
597
|
+
y: end.y
|
|
598
|
+
}];
|
|
599
|
+
const portalsLeft = [];
|
|
600
|
+
const portalsRight = [];
|
|
601
|
+
portalsLeft.push({
|
|
602
|
+
x: start.x,
|
|
603
|
+
y: start.y
|
|
604
|
+
});
|
|
605
|
+
portalsRight.push({
|
|
606
|
+
x: start.x,
|
|
607
|
+
y: start.y
|
|
608
|
+
});
|
|
609
|
+
for (let i = 0; i < corridor.length - 1; i++) {
|
|
610
|
+
const poly = byId.get(corridor[i]);
|
|
611
|
+
const nextId = corridor[i + 1];
|
|
612
|
+
let found = false;
|
|
613
|
+
for (let j = 0; j < poly.portals.length; j++) if (poly.portals[j].neighborId === nextId) {
|
|
614
|
+
portalsLeft.push(poly.portals[j].a);
|
|
615
|
+
portalsRight.push(poly.portals[j].b);
|
|
616
|
+
found = true;
|
|
617
|
+
break;
|
|
618
|
+
}
|
|
619
|
+
if (!found) {
|
|
620
|
+
const c = centroid(poly.vertices);
|
|
621
|
+
portalsLeft.push(c);
|
|
622
|
+
portalsRight.push(c);
|
|
623
|
+
}
|
|
624
|
+
}
|
|
625
|
+
portalsLeft.push({
|
|
626
|
+
x: end.x,
|
|
627
|
+
y: end.y
|
|
628
|
+
});
|
|
629
|
+
portalsRight.push({
|
|
630
|
+
x: end.x,
|
|
631
|
+
y: end.y
|
|
632
|
+
});
|
|
633
|
+
const path = [];
|
|
634
|
+
let apexPt = portalsLeft[0];
|
|
635
|
+
let leftPt = portalsLeft[0];
|
|
636
|
+
let rightPt = portalsRight[0];
|
|
637
|
+
let apexIdx = 0;
|
|
638
|
+
let leftIdx = 0;
|
|
639
|
+
let rightIdx = 0;
|
|
640
|
+
path.push({
|
|
641
|
+
x: apexPt.x,
|
|
642
|
+
y: apexPt.y
|
|
643
|
+
});
|
|
644
|
+
const n = portalsLeft.length;
|
|
645
|
+
for (let i = 1; i < n; i++) {
|
|
646
|
+
const pL = portalsLeft[i];
|
|
647
|
+
const pR = portalsRight[i];
|
|
648
|
+
if (triArea2(apexPt, rightPt, pR) <= 0) if (vEqual(apexPt, rightPt) || triArea2(apexPt, leftPt, pR) > 0) {
|
|
649
|
+
rightPt = pR;
|
|
650
|
+
rightIdx = i;
|
|
651
|
+
} else {
|
|
652
|
+
addUnique(path, leftPt);
|
|
653
|
+
apexPt = leftPt;
|
|
654
|
+
apexIdx = leftIdx;
|
|
655
|
+
leftPt = apexPt;
|
|
656
|
+
rightPt = apexPt;
|
|
657
|
+
leftIdx = apexIdx;
|
|
658
|
+
rightIdx = apexIdx;
|
|
659
|
+
i = apexIdx;
|
|
660
|
+
continue;
|
|
661
|
+
}
|
|
662
|
+
if (triArea2(apexPt, leftPt, pL) >= 0) if (vEqual(apexPt, leftPt) || triArea2(apexPt, rightPt, pL) < 0) {
|
|
663
|
+
leftPt = pL;
|
|
664
|
+
leftIdx = i;
|
|
665
|
+
} else {
|
|
666
|
+
addUnique(path, rightPt);
|
|
667
|
+
apexPt = rightPt;
|
|
668
|
+
apexIdx = rightIdx;
|
|
669
|
+
leftPt = apexPt;
|
|
670
|
+
rightPt = apexPt;
|
|
671
|
+
leftIdx = apexIdx;
|
|
672
|
+
rightIdx = apexIdx;
|
|
673
|
+
i = apexIdx;
|
|
674
|
+
}
|
|
675
|
+
}
|
|
676
|
+
addUnique(path, end);
|
|
677
|
+
return path;
|
|
625
678
|
}
|
|
626
679
|
function addUnique(path, p) {
|
|
627
|
-
|
|
628
|
-
|
|
629
|
-
|
|
630
|
-
|
|
680
|
+
const last = path[path.length - 1];
|
|
681
|
+
if (!last || last.x !== p.x || last.y !== p.y) path.push({
|
|
682
|
+
x: p.x,
|
|
683
|
+
y: p.y
|
|
684
|
+
});
|
|
631
685
|
}
|
|
686
|
+
/** Twice the signed area of the triangle (a, b, c). Positive = CCW. */
|
|
632
687
|
function triArea2(a, b, c) {
|
|
633
|
-
|
|
688
|
+
return (b.x - a.x) * (c.y - a.y) - (c.x - a.x) * (b.y - a.y);
|
|
634
689
|
}
|
|
635
690
|
function vEqual(a, b) {
|
|
636
|
-
|
|
637
|
-
}
|
|
638
|
-
|
|
639
|
-
|
|
640
|
-
|
|
641
|
-
createNavMesh,
|
|
642
|
-
euclideanDistance,
|
|
643
|
-
findNavMeshPath,
|
|
644
|
-
findPath,
|
|
645
|
-
findPathJPS,
|
|
646
|
-
findPolygonAt,
|
|
647
|
-
getCost,
|
|
648
|
-
inBounds,
|
|
649
|
-
isWalkable,
|
|
650
|
-
manhattanDistance,
|
|
651
|
-
octileDistance,
|
|
652
|
-
setCost,
|
|
653
|
-
setWalkable
|
|
654
|
-
};
|
|
691
|
+
return a.x === b.x && a.y === b.y;
|
|
692
|
+
}
|
|
693
|
+
//#endregion
|
|
694
|
+
export { chebyshevDistance, createGrid, createNavMesh, euclideanDistance, findNavMeshPath, findPath, findPathJPS, findPolygonAt, getCost, inBounds, isWalkable, manhattanDistance, octileDistance, setCost, setWalkable };
|
|
695
|
+
|
|
655
696
|
//# sourceMappingURL=index.mjs.map
|