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