@splatwalk/core 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 +21 -0
- package/README.md +73 -0
- package/floor.d.ts +258 -0
- package/floor.js +972 -0
- package/package.json +59 -0
- package/wasm_splatwalk.d.ts +481 -0
- package/wasm_splatwalk.js +947 -0
- package/wasm_splatwalk_bg.wasm +0 -0
package/floor.js
ADDED
|
@@ -0,0 +1,972 @@
|
|
|
1
|
+
/**
|
|
2
|
+
* Canonical FAST NAV floor-field preset.
|
|
3
|
+
*
|
|
4
|
+
* These are the conservative reconstruction + 2.5D floor-field settings the
|
|
5
|
+
* reference FAST NAV path uses to extract a room floor. Binary-only integrators
|
|
6
|
+
* can pass this straight to `build_walkable_ground_field` / `build_room_floor_mesh`
|
|
7
|
+
* (merged with their own per-scene `rotation`, `flip_y`, `collision_seed`, and
|
|
8
|
+
* optional `region_min`/`region_max`) instead of reverse-engineering the values.
|
|
9
|
+
*
|
|
10
|
+
* It intentionally omits per-scene fields (`rotation`, `flip_y`, `collision_seed`,
|
|
11
|
+
* `region_*`) so callers supply those for their own splat orientation and seed.
|
|
12
|
+
*
|
|
13
|
+
* Keep this in sync with the Rust `fast_nav_preset_json()` in
|
|
14
|
+
* `wasm-splatwalk/src/lib.rs`, which the WASM core exports via `fast_nav_preset()`
|
|
15
|
+
* and bakes into `build_room_floor_mesh`.
|
|
16
|
+
*/
|
|
17
|
+
export const FAST_NAV_PRESET = {
|
|
18
|
+
mode: 2,
|
|
19
|
+
voxel_target: 9000,
|
|
20
|
+
min_alpha: 0.08,
|
|
21
|
+
max_scale: 3.5,
|
|
22
|
+
sdf_cell_size: 0.14,
|
|
23
|
+
sdf_vertical_cell_size: 0.05,
|
|
24
|
+
sdf_density_threshold: 0.06,
|
|
25
|
+
sdf_max_layers: 2,
|
|
26
|
+
sdf_smoothing_radius: 2,
|
|
27
|
+
sdf_influence_radius_scale: 2.6,
|
|
28
|
+
prune_floaters: true,
|
|
29
|
+
prune_floaters_k: 16,
|
|
30
|
+
prune_floaters_std_ratio: 2.0,
|
|
31
|
+
normal_align: 0.3,
|
|
32
|
+
ransac_thresh: 0.16,
|
|
33
|
+
floor_projection_epsilon: 0.2,
|
|
34
|
+
height_projection_epsilon: 0.16,
|
|
35
|
+
obstacle_height_epsilon: 0.34,
|
|
36
|
+
obstacle_clearance_min: 0.18,
|
|
37
|
+
obstacle_clearance_max: 1.7,
|
|
38
|
+
max_local_height_variance: 0.14,
|
|
39
|
+
min_floor_confidence: 0.005,
|
|
40
|
+
hole_fill_radius: 2,
|
|
41
|
+
agent_radius_erode: 0,
|
|
42
|
+
component_mode: 'all',
|
|
43
|
+
collision_carve_height: 1.7,
|
|
44
|
+
collision_carve_radius: 0.35,
|
|
45
|
+
};
|
|
46
|
+
/**
|
|
47
|
+
* Default vertical tolerance (meters) within which a floor corner height is snapped
|
|
48
|
+
* to the dominant floor plane, so a flat floor triangulates as a single flat surface
|
|
49
|
+
* instead of a noisy set of stepped quads that Recast fragments into islands.
|
|
50
|
+
*/
|
|
51
|
+
export const DEFAULT_FLOOR_FLATTEN_TOLERANCE = 0.12;
|
|
52
|
+
/**
|
|
53
|
+
* Largest enclosed gap (in cells) that {@link buildFastFloorMesh} will bridge across
|
|
54
|
+
* when a void / low-confidence pocket is fully surrounded by accepted floor (seams,
|
|
55
|
+
* painted lines, reflective patches). Larger holes are treated as real openings.
|
|
56
|
+
*/
|
|
57
|
+
const MAX_BRIDGE_GAP_CELLS = 12;
|
|
58
|
+
/** Default ceiling on total navmesh voxel columns when auto-sizing the cell size. */
|
|
59
|
+
export const DEFAULT_MAX_NAV_CELLS = 1000000;
|
|
60
|
+
/**
|
|
61
|
+
* Auto-size the Recast cell size (`cs`) for a floor/collider mesh of the given
|
|
62
|
+
* horizontal extent.
|
|
63
|
+
*
|
|
64
|
+
* Follows the standard Recast guideline that `cs` belongs in
|
|
65
|
+
* `[agentRadius / 3, agentRadius / 2]`, and within that window picks the FINEST
|
|
66
|
+
* cell size whose grid (`width/cs * depth/cs`) still fits under `maxCells`. This
|
|
67
|
+
* keeps coverage of a large scene complete (the grid is bounded by a cell budget
|
|
68
|
+
* instead of a fixed small `cs` that either truncates the area or explodes the
|
|
69
|
+
* voxel count) while never going finer/coarser than the agent radius warrants.
|
|
70
|
+
*
|
|
71
|
+
* `agentRadiusM` is the agent radius in metres; with the gaming-standard 0.5 m
|
|
72
|
+
* agent this yields `cs` in `[0.167, 0.25]`.
|
|
73
|
+
*/
|
|
74
|
+
export function autoNavCellSize(widthM, depthM, agentRadiusM, maxCells = DEFAULT_MAX_NAV_CELLS) {
|
|
75
|
+
const r = agentRadiusM > 0 ? agentRadiusM : 0.5;
|
|
76
|
+
const finest = r / 3;
|
|
77
|
+
const coarsest = r / 2;
|
|
78
|
+
const area = Math.max(0, widthM) * Math.max(0, depthM);
|
|
79
|
+
// cs such that (width/cs) * (depth/cs) <= maxCells => cs >= sqrt(area / maxCells)
|
|
80
|
+
const budgetCs = maxCells > 0 && area > 0 ? Math.sqrt(area / maxCells) : finest;
|
|
81
|
+
return Math.min(coarsest, Math.max(finest, budgetCs));
|
|
82
|
+
}
|
|
83
|
+
/**
|
|
84
|
+
* Typed error thrown by {@link buildFastFloorMesh} when the floor field does not
|
|
85
|
+
* yield a usable room floor. The {@link reason} lets the recovery loop decide
|
|
86
|
+
* whether to escalate extraction parameters and retry.
|
|
87
|
+
*/
|
|
88
|
+
export class FastNavFloorError extends Error {
|
|
89
|
+
constructor(reason, message, diagnostics = {}) {
|
|
90
|
+
super(message);
|
|
91
|
+
this.name = 'FastNavFloorError';
|
|
92
|
+
this.reason = reason;
|
|
93
|
+
this.area = diagnostics.area;
|
|
94
|
+
this.components = diagnostics.components;
|
|
95
|
+
this.stateCounts = diagnostics.stateCounts;
|
|
96
|
+
// Restore the prototype chain so `instanceof` works after transpilation.
|
|
97
|
+
Object.setPrototypeOf(this, FastNavFloorError.prototype);
|
|
98
|
+
}
|
|
99
|
+
}
|
|
100
|
+
/**
|
|
101
|
+
* Default, built-in recovery ladder. It first escalates extraction parameters
|
|
102
|
+
* (coarser cells, lower density threshold, higher variance tolerance, higher
|
|
103
|
+
* voxel target, lower confidence) and only relaxes the room-area gate on later
|
|
104
|
+
* steps as a last resort. Integrators can override any/all of this.
|
|
105
|
+
*/
|
|
106
|
+
export const DEFAULT_FAST_NAV_RECOVERY = {
|
|
107
|
+
steps: [
|
|
108
|
+
{ label: 'default', settings: {}, minRoomFloorArea: 4.0 },
|
|
109
|
+
{
|
|
110
|
+
label: 'relaxed',
|
|
111
|
+
settings: {
|
|
112
|
+
sdf_density_threshold: 0.04,
|
|
113
|
+
max_local_height_variance: 0.2,
|
|
114
|
+
obstacle_height_epsilon: 0.42,
|
|
115
|
+
min_floor_confidence: 0.003,
|
|
116
|
+
hole_fill_radius: 3,
|
|
117
|
+
voxel_target: 12000,
|
|
118
|
+
},
|
|
119
|
+
minRoomFloorArea: 4.0,
|
|
120
|
+
},
|
|
121
|
+
{
|
|
122
|
+
label: 'coarse',
|
|
123
|
+
settings: {
|
|
124
|
+
sdf_cell_size: 0.2,
|
|
125
|
+
sdf_density_threshold: 0.03,
|
|
126
|
+
max_local_height_variance: 0.28,
|
|
127
|
+
min_floor_confidence: 0.002,
|
|
128
|
+
voxel_target: 14000,
|
|
129
|
+
hole_fill_radius: 3,
|
|
130
|
+
},
|
|
131
|
+
minRoomFloorArea: 2.5,
|
|
132
|
+
},
|
|
133
|
+
{
|
|
134
|
+
label: 'coarse-last-resort',
|
|
135
|
+
settings: {
|
|
136
|
+
sdf_cell_size: 0.26,
|
|
137
|
+
sdf_density_threshold: 0.022,
|
|
138
|
+
max_local_height_variance: 0.36,
|
|
139
|
+
min_floor_confidence: 0.0015,
|
|
140
|
+
voxel_target: 16000,
|
|
141
|
+
hole_fill_radius: 4,
|
|
142
|
+
},
|
|
143
|
+
minRoomFloorArea: 1.5,
|
|
144
|
+
},
|
|
145
|
+
],
|
|
146
|
+
};
|
|
147
|
+
/**
|
|
148
|
+
* Resolve a (possibly partial/omitted) recovery config to a concrete one,
|
|
149
|
+
* falling back to {@link DEFAULT_FAST_NAV_RECOVERY} when no steps are supplied.
|
|
150
|
+
* This is what makes adaptive recovery on-by-default for every caller.
|
|
151
|
+
*/
|
|
152
|
+
export function resolveRecovery(partial) {
|
|
153
|
+
if (!partial || !partial.steps || partial.steps.length === 0) {
|
|
154
|
+
return DEFAULT_FAST_NAV_RECOVERY;
|
|
155
|
+
}
|
|
156
|
+
return { steps: partial.steps };
|
|
157
|
+
}
|
|
158
|
+
/**
|
|
159
|
+
* Ignore a small number of stray peripheral splats/cells in a detected floor.
|
|
160
|
+
*
|
|
161
|
+
* Large, floater-heavy scans leave scattered cells at outlier heights inside an
|
|
162
|
+
* otherwise-flat floor component; triangulating them creates vertical cliffs that
|
|
163
|
+
* Recast splits into tiny fragments. This helper drops height outliers (relative
|
|
164
|
+
* to the median floor plane) and any spatially-isolated specks, keeping the
|
|
165
|
+
* largest contiguous core. It is deliberately conservative: if the would-be
|
|
166
|
+
* removals exceed `maxStrayFraction`, the spread is treated as structural and the
|
|
167
|
+
* input is returned unchanged, so clean and legitimately multi-level floors are
|
|
168
|
+
* untouched.
|
|
169
|
+
*/
|
|
170
|
+
export function trimStrayFloorCells(field, cells, options = {}) {
|
|
171
|
+
const enabled = options.enabled ?? true;
|
|
172
|
+
const heightTolerance = options.heightTolerance ?? 0.5;
|
|
173
|
+
const maxStrayFraction = options.maxStrayFraction ?? 0.3;
|
|
174
|
+
const minKeepCells = options.minKeepCells ?? 16;
|
|
175
|
+
const noop = {
|
|
176
|
+
cells,
|
|
177
|
+
droppedHeightOutliers: 0,
|
|
178
|
+
droppedPeripheral: 0,
|
|
179
|
+
medianHeight: Number.NaN,
|
|
180
|
+
changed: false,
|
|
181
|
+
};
|
|
182
|
+
if (!enabled || cells.length <= minKeepCells)
|
|
183
|
+
return noop;
|
|
184
|
+
const heights = cells
|
|
185
|
+
.map((idx) => field.cells[idx]?.height)
|
|
186
|
+
.filter((h) => Number.isFinite(h))
|
|
187
|
+
.sort((a, b) => a - b);
|
|
188
|
+
if (heights.length === 0)
|
|
189
|
+
return noop;
|
|
190
|
+
const medianHeight = heights[Math.floor(heights.length / 2)];
|
|
191
|
+
const withinBand = cells.filter((idx) => {
|
|
192
|
+
const h = field.cells[idx]?.height;
|
|
193
|
+
return Number.isFinite(h) && Math.abs(h - medianHeight) <= heightTolerance;
|
|
194
|
+
});
|
|
195
|
+
const droppedHeightOutliers = cells.length - withinBand.length;
|
|
196
|
+
if (droppedHeightOutliers > maxStrayFraction * cells.length || withinBand.length < minKeepCells) {
|
|
197
|
+
return noop;
|
|
198
|
+
}
|
|
199
|
+
const width = field.width;
|
|
200
|
+
const height = field.height;
|
|
201
|
+
const inBand = new Set(withinBand);
|
|
202
|
+
const visited = new Set();
|
|
203
|
+
let best = [];
|
|
204
|
+
for (const startIdx of withinBand) {
|
|
205
|
+
if (visited.has(startIdx))
|
|
206
|
+
continue;
|
|
207
|
+
const queue = [startIdx];
|
|
208
|
+
visited.add(startIdx);
|
|
209
|
+
const cluster = [];
|
|
210
|
+
while (queue.length > 0) {
|
|
211
|
+
const idx = queue.shift();
|
|
212
|
+
cluster.push(idx);
|
|
213
|
+
const row = Math.floor(idx / width);
|
|
214
|
+
const col = idx % width;
|
|
215
|
+
const neighbors = [
|
|
216
|
+
row > 0 ? idx - width : -1,
|
|
217
|
+
row + 1 < height ? idx + width : -1,
|
|
218
|
+
col > 0 ? idx - 1 : -1,
|
|
219
|
+
col + 1 < width ? idx + 1 : -1,
|
|
220
|
+
];
|
|
221
|
+
for (const next of neighbors) {
|
|
222
|
+
if (next >= 0 && inBand.has(next) && !visited.has(next)) {
|
|
223
|
+
visited.add(next);
|
|
224
|
+
queue.push(next);
|
|
225
|
+
}
|
|
226
|
+
}
|
|
227
|
+
}
|
|
228
|
+
if (cluster.length > best.length)
|
|
229
|
+
best = cluster;
|
|
230
|
+
}
|
|
231
|
+
const droppedPeripheral = withinBand.length - best.length;
|
|
232
|
+
const totalDropped = droppedHeightOutliers + droppedPeripheral;
|
|
233
|
+
if (best.length < minKeepCells || totalDropped > maxStrayFraction * cells.length) {
|
|
234
|
+
return noop;
|
|
235
|
+
}
|
|
236
|
+
return {
|
|
237
|
+
cells: best,
|
|
238
|
+
droppedHeightOutliers,
|
|
239
|
+
droppedPeripheral,
|
|
240
|
+
medianHeight,
|
|
241
|
+
changed: totalDropped > 0,
|
|
242
|
+
};
|
|
243
|
+
}
|
|
244
|
+
function cellCenterWorld(field, idx) {
|
|
245
|
+
const width = field.width;
|
|
246
|
+
const row = Math.floor(idx / width);
|
|
247
|
+
const col = idx % width;
|
|
248
|
+
const cell = field.cells[idx];
|
|
249
|
+
const h = Number.isFinite(cell.height) ? cell.height : 0;
|
|
250
|
+
const o = field.basis.origin;
|
|
251
|
+
const t = field.basis.tangent;
|
|
252
|
+
const b = field.basis.bitangent;
|
|
253
|
+
const u = field.basis.up;
|
|
254
|
+
const c = col + 0.5;
|
|
255
|
+
const r = row + 0.5;
|
|
256
|
+
return [
|
|
257
|
+
o[0] + t[0] * c * field.cell_size + b[0] * r * field.cell_size + u[0] * h,
|
|
258
|
+
o[1] + t[1] * c * field.cell_size + b[1] * r * field.cell_size + u[1] * h,
|
|
259
|
+
o[2] + t[2] * c * field.cell_size + b[2] * r * field.cell_size + u[2] * h,
|
|
260
|
+
];
|
|
261
|
+
}
|
|
262
|
+
/**
|
|
263
|
+
* Find the dense floor band: among cells carrying real surface density, keep the
|
|
264
|
+
* densest fraction, take the density-weighted modal height (where most splats
|
|
265
|
+
* actually sit), and return that band's cells + density-weighted centroid. Sparse
|
|
266
|
+
* peripheral/under-floor floaters contribute little weight and fall outside the
|
|
267
|
+
* modal band, so they are effectively ignored.
|
|
268
|
+
*/
|
|
269
|
+
function computeDenseFloorCore(field, heightBin, densityPercentile) {
|
|
270
|
+
const candidates = [];
|
|
271
|
+
for (let i = 0; i < field.cells.length; i++) {
|
|
272
|
+
const cell = field.cells[i];
|
|
273
|
+
if (!Number.isFinite(cell.height))
|
|
274
|
+
continue;
|
|
275
|
+
const density = Math.max(cell.peak_density ?? 0, cell.surface_confidence ?? 0);
|
|
276
|
+
if (density <= 0)
|
|
277
|
+
continue;
|
|
278
|
+
candidates.push({ idx: i, height: cell.height, density });
|
|
279
|
+
}
|
|
280
|
+
if (candidates.length < 8)
|
|
281
|
+
return null;
|
|
282
|
+
const sortedDensity = candidates.map((c) => c.density).sort((a, b) => a - b);
|
|
283
|
+
const threshold = sortedDensity[Math.min(sortedDensity.length - 1, Math.floor(sortedDensity.length * densityPercentile))];
|
|
284
|
+
const dense = candidates.filter((c) => c.density >= threshold);
|
|
285
|
+
if (dense.length === 0)
|
|
286
|
+
return null;
|
|
287
|
+
const bins = new Map();
|
|
288
|
+
for (const c of dense) {
|
|
289
|
+
const bin = Math.round(c.height / heightBin);
|
|
290
|
+
bins.set(bin, (bins.get(bin) ?? 0) + c.density);
|
|
291
|
+
}
|
|
292
|
+
let modalBin = 0;
|
|
293
|
+
let modalWeight = -1;
|
|
294
|
+
for (const [bin, weight] of bins) {
|
|
295
|
+
if (weight > modalWeight) {
|
|
296
|
+
modalWeight = weight;
|
|
297
|
+
modalBin = bin;
|
|
298
|
+
}
|
|
299
|
+
}
|
|
300
|
+
const modalHeight = modalBin * heightBin;
|
|
301
|
+
const core = dense.filter((c) => Math.abs(c.height - modalHeight) <= heightBin);
|
|
302
|
+
if (core.length === 0)
|
|
303
|
+
return null;
|
|
304
|
+
let wx = 0;
|
|
305
|
+
let wy = 0;
|
|
306
|
+
let wz = 0;
|
|
307
|
+
let ws = 0;
|
|
308
|
+
for (const c of core) {
|
|
309
|
+
const center = cellCenterWorld(field, c.idx);
|
|
310
|
+
wx += center[0] * c.density;
|
|
311
|
+
wy += center[1] * c.density;
|
|
312
|
+
wz += center[2] * c.density;
|
|
313
|
+
ws += c.density;
|
|
314
|
+
}
|
|
315
|
+
if (ws <= 0)
|
|
316
|
+
return null;
|
|
317
|
+
return {
|
|
318
|
+
cells: core.map((c) => c.idx),
|
|
319
|
+
modalHeight,
|
|
320
|
+
centroid: [wx / ws, wy / ws, wz / ws],
|
|
321
|
+
};
|
|
322
|
+
}
|
|
323
|
+
/**
|
|
324
|
+
* Estimate a collision seed located in the DENSE floor area of the scene, so the
|
|
325
|
+
* pipeline anchors on the real room floor instead of a sparse floater plane below
|
|
326
|
+
* it (the common failure on large, floater-heavy scans). Returns `fallbackSeed`
|
|
327
|
+
* when there isn't enough signal to be confident.
|
|
328
|
+
*/
|
|
329
|
+
export function estimateDenseFloorSeed(field, fallbackSeed, options = {}) {
|
|
330
|
+
if (!(options.enabled ?? true))
|
|
331
|
+
return fallbackSeed;
|
|
332
|
+
const core = computeDenseFloorCore(field, options.heightBin ?? 0.25, options.densityPercentile ?? 0.6);
|
|
333
|
+
return core ? core.centroid : fallbackSeed;
|
|
334
|
+
}
|
|
335
|
+
/**
|
|
336
|
+
* Estimate an adapted default region (oriented-space AABB) around the dense floor:
|
|
337
|
+
* generous in XZ (covers the whole floor band) but tightly clamped in Y to the
|
|
338
|
+
* floor plus walkable headroom. This excludes deep stray floaters below/above the
|
|
339
|
+
* real floor so floor detection is no longer dragged off the dense area. Returns
|
|
340
|
+
* `null` when there isn't enough signal.
|
|
341
|
+
*/
|
|
342
|
+
export function estimateDenseFloorRegion(field, options = {}) {
|
|
343
|
+
if (!(options.enabled ?? true))
|
|
344
|
+
return null;
|
|
345
|
+
const core = computeDenseFloorCore(field, options.heightBin ?? 0.25, options.densityPercentile ?? 0.6);
|
|
346
|
+
if (!core)
|
|
347
|
+
return null;
|
|
348
|
+
const yMin = core.modalHeight - 0.6;
|
|
349
|
+
const yMax = core.modalHeight + 3.0;
|
|
350
|
+
let minX = Infinity;
|
|
351
|
+
let minZ = Infinity;
|
|
352
|
+
let maxX = -Infinity;
|
|
353
|
+
let maxZ = -Infinity;
|
|
354
|
+
let any = false;
|
|
355
|
+
for (let i = 0; i < field.cells.length; i++) {
|
|
356
|
+
const cell = field.cells[i];
|
|
357
|
+
if (!Number.isFinite(cell.height))
|
|
358
|
+
continue;
|
|
359
|
+
if (cell.height < yMin || cell.height > yMax)
|
|
360
|
+
continue;
|
|
361
|
+
const center = cellCenterWorld(field, i);
|
|
362
|
+
if (center[0] < minX)
|
|
363
|
+
minX = center[0];
|
|
364
|
+
if (center[0] > maxX)
|
|
365
|
+
maxX = center[0];
|
|
366
|
+
if (center[2] < minZ)
|
|
367
|
+
minZ = center[2];
|
|
368
|
+
if (center[2] > maxZ)
|
|
369
|
+
maxZ = center[2];
|
|
370
|
+
any = true;
|
|
371
|
+
}
|
|
372
|
+
if (!any)
|
|
373
|
+
return null;
|
|
374
|
+
const margin = field.cell_size * 2;
|
|
375
|
+
return {
|
|
376
|
+
min: [minX - margin, yMin, minZ - margin],
|
|
377
|
+
max: [maxX + margin, yMax, maxZ + margin],
|
|
378
|
+
};
|
|
379
|
+
}
|
|
380
|
+
/**
|
|
381
|
+
* Build a planar floor mesh from a walkable ground field by selecting the best
|
|
382
|
+
* connected floor component near the seed. Throws a typed {@link FastNavFloorError}
|
|
383
|
+
* when no usable floor of at least `minRoomFloorArea` square meters is found, so
|
|
384
|
+
* callers can escalate extraction parameters and retry. A small number of stray
|
|
385
|
+
* peripheral cells are ignored via {@link trimStrayFloorCells} (configurable).
|
|
386
|
+
*/
|
|
387
|
+
export function buildFastFloorMesh(field, seed, minRoomFloorArea, log,
|
|
388
|
+
// Retained for signature/call-site stability. The merged same-level emission now
|
|
389
|
+
// bounds heights via the per-component height gate + local median leveling, so the
|
|
390
|
+
// old global stray-trim is intentionally not applied here.
|
|
391
|
+
_strayTrim, floorFlattenTolerance = DEFAULT_FLOOR_FLATTEN_TOLERANCE) {
|
|
392
|
+
const width = field.width;
|
|
393
|
+
const height = field.height;
|
|
394
|
+
const stateCounts = field.cells.reduce((counts, cell) => {
|
|
395
|
+
counts[cell.state] = (counts[cell.state] ?? 0) + 1;
|
|
396
|
+
return counts;
|
|
397
|
+
}, {});
|
|
398
|
+
const obstacleCellCount = (stateCounts.obstacle ?? 0) + (stateCounts.height_variance ?? 0);
|
|
399
|
+
const origin = field.basis.origin;
|
|
400
|
+
const tangent = field.basis.tangent;
|
|
401
|
+
const bitangent = field.basis.bitangent;
|
|
402
|
+
const up = field.basis.up;
|
|
403
|
+
const pointAt = (col, row, cellHeight) => [
|
|
404
|
+
origin[0] + tangent[0] * col * field.cell_size + bitangent[0] * row * field.cell_size + up[0] * cellHeight,
|
|
405
|
+
origin[1] + tangent[1] * col * field.cell_size + bitangent[1] * row * field.cell_size + up[1] * cellHeight,
|
|
406
|
+
origin[2] + tangent[2] * col * field.cell_size + bitangent[2] * row * field.cell_size + up[2] * cellHeight,
|
|
407
|
+
];
|
|
408
|
+
// Cells filled by the morphological close (bridging thin void seams between floor
|
|
409
|
+
// patches at the SAME level) carry no WASM height, so we track an interpolated
|
|
410
|
+
// height for them here.
|
|
411
|
+
const bridgeHeight = new Map();
|
|
412
|
+
const heightOf = (idx) => {
|
|
413
|
+
const h = field.cells[idx]?.height;
|
|
414
|
+
if (Number.isFinite(h))
|
|
415
|
+
return h;
|
|
416
|
+
const b = bridgeHeight.get(idx);
|
|
417
|
+
return b !== undefined ? b : Number.NaN;
|
|
418
|
+
};
|
|
419
|
+
const cellCenter = (idx) => {
|
|
420
|
+
const row = Math.floor(idx / width);
|
|
421
|
+
const col = idx % width;
|
|
422
|
+
const h = heightOf(idx);
|
|
423
|
+
return pointAt(col + 0.5, row + 0.5, Number.isFinite(h) ? h : 0);
|
|
424
|
+
};
|
|
425
|
+
const buildMask = (relaxed) => field.cells.map((cell) => {
|
|
426
|
+
if (cell.state === 'walkable' || cell.state === 'filled')
|
|
427
|
+
return true;
|
|
428
|
+
if (!relaxed)
|
|
429
|
+
return false;
|
|
430
|
+
if (!Number.isFinite(cell.height))
|
|
431
|
+
return false;
|
|
432
|
+
if (cell.state === 'discarded_component')
|
|
433
|
+
return true;
|
|
434
|
+
if (cell.state === 'low_confidence') {
|
|
435
|
+
return cell.variance <= 0.18 && cell.obstacle_score <= 0.42;
|
|
436
|
+
}
|
|
437
|
+
if (cell.state === 'height_variance') {
|
|
438
|
+
return cell.confidence >= 0.01 && cell.variance <= 0.08 && cell.obstacle_score <= 0.35;
|
|
439
|
+
}
|
|
440
|
+
if (cell.state === 'obstacle') {
|
|
441
|
+
return cell.confidence >= 0.02 && cell.variance <= 0.05 && cell.obstacle_score <= 0.52;
|
|
442
|
+
}
|
|
443
|
+
return false;
|
|
444
|
+
});
|
|
445
|
+
const collectComponents = (mask) => {
|
|
446
|
+
const visited = new Uint8Array(field.cells.length);
|
|
447
|
+
const components = [];
|
|
448
|
+
for (let start = 0; start < field.cells.length; start++) {
|
|
449
|
+
if (!mask[start] || visited[start])
|
|
450
|
+
continue;
|
|
451
|
+
const queue = [start];
|
|
452
|
+
const cells = [];
|
|
453
|
+
visited[start] = 1;
|
|
454
|
+
let sx = 0;
|
|
455
|
+
let sy = 0;
|
|
456
|
+
let sz = 0;
|
|
457
|
+
while (queue.length > 0) {
|
|
458
|
+
const idx = queue.shift();
|
|
459
|
+
cells.push(idx);
|
|
460
|
+
const center = cellCenter(idx);
|
|
461
|
+
sx += center[0];
|
|
462
|
+
sy += center[1];
|
|
463
|
+
sz += center[2];
|
|
464
|
+
const row = Math.floor(idx / width);
|
|
465
|
+
const col = idx % width;
|
|
466
|
+
const neighbors = [
|
|
467
|
+
row > 0 ? idx - width : -1,
|
|
468
|
+
row + 1 < height ? idx + width : -1,
|
|
469
|
+
col > 0 ? idx - 1 : -1,
|
|
470
|
+
col + 1 < width ? idx + 1 : -1,
|
|
471
|
+
];
|
|
472
|
+
for (const next of neighbors) {
|
|
473
|
+
if (next >= 0 && mask[next] && !visited[next]) {
|
|
474
|
+
visited[next] = 1;
|
|
475
|
+
queue.push(next);
|
|
476
|
+
}
|
|
477
|
+
}
|
|
478
|
+
}
|
|
479
|
+
const inv = 1 / cells.length;
|
|
480
|
+
const centroid = [sx * inv, sy * inv, sz * inv];
|
|
481
|
+
const distanceToSeed = seed
|
|
482
|
+
? Math.hypot(centroid[0] - seed[0], centroid[1] - seed[1], centroid[2] - seed[2])
|
|
483
|
+
: 0;
|
|
484
|
+
components.push({ cells, centroid, distanceToSeed });
|
|
485
|
+
}
|
|
486
|
+
return components;
|
|
487
|
+
};
|
|
488
|
+
const selectComponent = (components) => {
|
|
489
|
+
const minCells = 20;
|
|
490
|
+
const minArea = 1.2;
|
|
491
|
+
const maxSeedDistance = 3.25;
|
|
492
|
+
const viableComponents = components.filter((component) => {
|
|
493
|
+
const area = component.cells.length * field.cell_size * field.cell_size;
|
|
494
|
+
return component.cells.length >= minCells && area >= minArea;
|
|
495
|
+
});
|
|
496
|
+
if (viableComponents.length === 0)
|
|
497
|
+
return null;
|
|
498
|
+
const seedNear = seed
|
|
499
|
+
? viableComponents.filter((component) => component.distanceToSeed <= maxSeedDistance)
|
|
500
|
+
: viableComponents;
|
|
501
|
+
const candidates = seedNear.length > 0 ? seedNear : viableComponents;
|
|
502
|
+
candidates.sort((a, b) => {
|
|
503
|
+
if (!seed || seedNear.length === 0)
|
|
504
|
+
return b.cells.length - a.cells.length;
|
|
505
|
+
const score = (component) => {
|
|
506
|
+
const area = component.cells.length * field.cell_size * field.cell_size;
|
|
507
|
+
return component.distanceToSeed - Math.sqrt(area) * 0.45;
|
|
508
|
+
};
|
|
509
|
+
return score(a) - score(b) || b.cells.length - a.cells.length;
|
|
510
|
+
});
|
|
511
|
+
const selected = candidates[0];
|
|
512
|
+
return {
|
|
513
|
+
selected,
|
|
514
|
+
usedLargestFallback: seedNear.length === 0 && !!seed,
|
|
515
|
+
};
|
|
516
|
+
};
|
|
517
|
+
const areaOfSelection = (sel) => sel.selected.cells.length * field.cell_size * field.cell_size;
|
|
518
|
+
// Bridge small enclosed gaps (seams, painted lines, reflective patches that read
|
|
519
|
+
// as void / low_confidence) that are fully surrounded by accepted floor, so they
|
|
520
|
+
// do not split an otherwise-continuous flat floor into separate fragments. Gaps
|
|
521
|
+
// touching the grid border or adjacent to a real obstacle/discontinuity are left
|
|
522
|
+
// alone, as are holes larger than MAX_BRIDGE_GAP_CELLS (treated as real openings).
|
|
523
|
+
const bridgeEnclosedGaps = (mask) => {
|
|
524
|
+
const isBridgeable = (state) => state === 'void' || state === 'low_confidence';
|
|
525
|
+
const visited = new Uint8Array(field.cells.length);
|
|
526
|
+
let bridged = 0;
|
|
527
|
+
for (let start = 0; start < field.cells.length; start++) {
|
|
528
|
+
if (mask[start] || visited[start])
|
|
529
|
+
continue;
|
|
530
|
+
visited[start] = 1;
|
|
531
|
+
if (!isBridgeable(field.cells[start].state))
|
|
532
|
+
continue;
|
|
533
|
+
const queue = [start];
|
|
534
|
+
const run = [];
|
|
535
|
+
let enclosedByFloor = true;
|
|
536
|
+
let touchesBorder = false;
|
|
537
|
+
while (queue.length > 0) {
|
|
538
|
+
const idx = queue.shift();
|
|
539
|
+
run.push(idx);
|
|
540
|
+
const row = Math.floor(idx / width);
|
|
541
|
+
const col = idx % width;
|
|
542
|
+
if (row === 0 || col === 0 || row + 1 === height || col + 1 === width) {
|
|
543
|
+
touchesBorder = true;
|
|
544
|
+
}
|
|
545
|
+
const neighbors = [
|
|
546
|
+
row > 0 ? idx - width : -1,
|
|
547
|
+
row + 1 < height ? idx + width : -1,
|
|
548
|
+
col > 0 ? idx - 1 : -1,
|
|
549
|
+
col + 1 < width ? idx + 1 : -1,
|
|
550
|
+
];
|
|
551
|
+
for (const next of neighbors) {
|
|
552
|
+
if (next < 0)
|
|
553
|
+
continue;
|
|
554
|
+
if (mask[next])
|
|
555
|
+
continue; // accepted floor: a valid hole boundary
|
|
556
|
+
if (isBridgeable(field.cells[next].state)) {
|
|
557
|
+
if (!visited[next]) {
|
|
558
|
+
visited[next] = 1;
|
|
559
|
+
queue.push(next);
|
|
560
|
+
}
|
|
561
|
+
}
|
|
562
|
+
else {
|
|
563
|
+
// adjacent to a real obstacle/discontinuity: not a floor-enclosed hole
|
|
564
|
+
enclosedByFloor = false;
|
|
565
|
+
}
|
|
566
|
+
}
|
|
567
|
+
}
|
|
568
|
+
if (enclosedByFloor && !touchesBorder && run.length <= MAX_BRIDGE_GAP_CELLS) {
|
|
569
|
+
for (const idx of run) {
|
|
570
|
+
mask[idx] = true;
|
|
571
|
+
}
|
|
572
|
+
bridged += run.length;
|
|
573
|
+
}
|
|
574
|
+
}
|
|
575
|
+
return bridged;
|
|
576
|
+
};
|
|
577
|
+
// Bounded, height-aware morphological close: bridge THIN void/low-confidence seams
|
|
578
|
+
// that fragment an otherwise-continuous SAME-LEVEL floor (sparse outdoor ground
|
|
579
|
+
// capture reads as void, splitting a lawn/deck into many tiny components). A gap
|
|
580
|
+
// cell is filled only when accepted floor is found within `maxGap` on at least two
|
|
581
|
+
// of four cardinal sides AND those floor heights agree within `heightTol`. This can
|
|
582
|
+
// NEVER bridge across a pool (gap >> maxGap, and the pool bottom height disagrees)
|
|
583
|
+
// or onto a box top (separated by obstacle sides, height disagrees); obstacle and
|
|
584
|
+
// height_variance cells are skipped so real blockers/ledges stay rejected.
|
|
585
|
+
const closeFloorSeams = (mask, maxGap, heightTol) => {
|
|
586
|
+
if (maxGap < 1)
|
|
587
|
+
return 0;
|
|
588
|
+
const dirs = [
|
|
589
|
+
[-1, 0],
|
|
590
|
+
[1, 0],
|
|
591
|
+
[0, -1],
|
|
592
|
+
[0, 1],
|
|
593
|
+
];
|
|
594
|
+
const additions = [];
|
|
595
|
+
for (let idx = 0; idx < field.cells.length; idx++) {
|
|
596
|
+
if (mask[idx])
|
|
597
|
+
continue;
|
|
598
|
+
const state = field.cells[idx]?.state;
|
|
599
|
+
if (state === 'obstacle' || state === 'height_variance')
|
|
600
|
+
continue;
|
|
601
|
+
const row = Math.floor(idx / width);
|
|
602
|
+
const col = idx % width;
|
|
603
|
+
const hits = [];
|
|
604
|
+
for (const [dr, dc] of dirs) {
|
|
605
|
+
for (let step = 1; step <= maxGap; step++) {
|
|
606
|
+
const nr = row + dr * step;
|
|
607
|
+
const nc = col + dc * step;
|
|
608
|
+
if (nr < 0 || nc < 0 || nr >= height || nc >= width)
|
|
609
|
+
break;
|
|
610
|
+
const nidx = nr * width + nc;
|
|
611
|
+
if (mask[nidx]) {
|
|
612
|
+
const h = heightOf(nidx);
|
|
613
|
+
if (Number.isFinite(h))
|
|
614
|
+
hits.push(h);
|
|
615
|
+
break;
|
|
616
|
+
}
|
|
617
|
+
}
|
|
618
|
+
}
|
|
619
|
+
if (hits.length >= 2) {
|
|
620
|
+
const mn = Math.min(...hits);
|
|
621
|
+
const mx = Math.max(...hits);
|
|
622
|
+
if (mx - mn <= heightTol)
|
|
623
|
+
additions.push([idx, (mn + mx) / 2]);
|
|
624
|
+
}
|
|
625
|
+
}
|
|
626
|
+
for (const [idx, h] of additions) {
|
|
627
|
+
mask[idx] = true;
|
|
628
|
+
bridgeHeight.set(idx, h);
|
|
629
|
+
}
|
|
630
|
+
return additions.length;
|
|
631
|
+
};
|
|
632
|
+
const maxSeamGapCells = Math.max(1, Math.round(0.6 / field.cell_size));
|
|
633
|
+
const seamHeightTolerance = 0.35;
|
|
634
|
+
const strictMask = buildMask(false);
|
|
635
|
+
const bridgedCellCount = bridgeEnclosedGaps(strictMask);
|
|
636
|
+
const closedCellCount = closeFloorSeams(strictMask, maxSeamGapCells, seamHeightTolerance);
|
|
637
|
+
const acceptedCellCount = strictMask.filter(Boolean).length;
|
|
638
|
+
const rejectedCellCount = field.cells.length - acceptedCellCount;
|
|
639
|
+
const strictComponents = collectComponents(strictMask);
|
|
640
|
+
let selection = selectComponent(strictComponents);
|
|
641
|
+
let selectedMask = strictMask;
|
|
642
|
+
let components = strictComponents;
|
|
643
|
+
let fallbackUsed = false;
|
|
644
|
+
if (!selection || areaOfSelection(selection) < minRoomFloorArea) {
|
|
645
|
+
const relaxedMask = buildMask(true);
|
|
646
|
+
bridgeEnclosedGaps(relaxedMask);
|
|
647
|
+
closeFloorSeams(relaxedMask, maxSeamGapCells, seamHeightTolerance);
|
|
648
|
+
const relaxedComponents = collectComponents(relaxedMask);
|
|
649
|
+
const relaxedSelection = selectComponent(relaxedComponents);
|
|
650
|
+
if (relaxedSelection && (!selection || areaOfSelection(relaxedSelection) > areaOfSelection(selection))) {
|
|
651
|
+
selection = relaxedSelection;
|
|
652
|
+
selectedMask = relaxedMask;
|
|
653
|
+
components = relaxedComponents;
|
|
654
|
+
fallbackUsed = true;
|
|
655
|
+
log(`[WARN] Fast floor relaxed mask used: strictComponents=${strictComponents.length}, ` +
|
|
656
|
+
`relaxedComponents=${relaxedComponents.length}, states=${JSON.stringify(stateCounts)}`);
|
|
657
|
+
}
|
|
658
|
+
}
|
|
659
|
+
if (!selection) {
|
|
660
|
+
const largest = [...components].sort((a, b) => b.cells.length - a.cells.length)[0];
|
|
661
|
+
const largestArea = largest ? largest.cells.length * field.cell_size * field.cell_size : 0;
|
|
662
|
+
throw new FastNavFloorError('no_component', `Fast nav could not find a viable floor component. ` +
|
|
663
|
+
`Components=${components.length}, largest=${largest?.cells.length ?? 0} cells ` +
|
|
664
|
+
`(${largestArea.toFixed(2)} m^2), states=${JSON.stringify(stateCounts)}. ` +
|
|
665
|
+
`Try a different splat with a clearer room floor.`, { area: largestArea, components: components.length, stateCounts });
|
|
666
|
+
}
|
|
667
|
+
const selected = selection.selected;
|
|
668
|
+
const medianHeightOf = (cells) => {
|
|
669
|
+
const hs = cells
|
|
670
|
+
.map((i) => heightOf(i))
|
|
671
|
+
.filter((h) => Number.isFinite(h))
|
|
672
|
+
.sort((a, b) => a - b);
|
|
673
|
+
return hs.length ? hs[Math.floor(hs.length / 2)] : Number.NaN;
|
|
674
|
+
};
|
|
675
|
+
// Emit EVERY SAME-LEVEL viable floor component for full coverage of the walkable
|
|
676
|
+
// level - not just the seed component - so a floor split into separate patches by a
|
|
677
|
+
// wide seam (a central walkway, a pool, sparse capture) is covered rather than left
|
|
678
|
+
// as one disjoint area. The height gate keeps only components whose median height is
|
|
679
|
+
// in the floor band [ref-0.25, ref+0.30]; this EXCLUDES sunken surfaces (pool
|
|
680
|
+
// bottoms, below the floor) and elevated surfaces (box tops / shelves, above the
|
|
681
|
+
// floor), so it can neither fill pools nor climb boxes. Genuinely different
|
|
682
|
+
// elevation levels fall outside the band and remain their own regions, and because
|
|
683
|
+
// walkableClimb is unchanged (0.25) nothing is stitched across a real step.
|
|
684
|
+
const refHeight = medianHeightOf(selected.cells);
|
|
685
|
+
const sameLevelLow = Number.isFinite(refHeight) ? refHeight - 0.25 : -Infinity;
|
|
686
|
+
const sameLevelHigh = Number.isFinite(refHeight) ? refHeight + 0.3 : Infinity;
|
|
687
|
+
// Per-cell band: even within an accepted same-level component, individual cells far
|
|
688
|
+
// below the floor (pool-edge spill, capture noise) must not be emitted or they drag
|
|
689
|
+
// the surface underground. Slightly wider than the component-median gate to keep
|
|
690
|
+
// genuine gentle slope.
|
|
691
|
+
const cellBandLow = Number.isFinite(refHeight) ? refHeight - 0.5 : -Infinity;
|
|
692
|
+
const cellBandHigh = Number.isFinite(refHeight) ? refHeight + 0.45 : Infinity;
|
|
693
|
+
const minComponentCells = 20;
|
|
694
|
+
const minComponentArea = 1.2;
|
|
695
|
+
const cellArea = field.cell_size * field.cell_size;
|
|
696
|
+
const floorCells = [];
|
|
697
|
+
let emittedComponentCount = 0;
|
|
698
|
+
for (const component of components) {
|
|
699
|
+
if (component.cells.length < minComponentCells)
|
|
700
|
+
continue;
|
|
701
|
+
if (component.cells.length * cellArea < minComponentArea)
|
|
702
|
+
continue;
|
|
703
|
+
if (Number.isFinite(refHeight)) {
|
|
704
|
+
const med = medianHeightOf(component.cells);
|
|
705
|
+
if (!Number.isFinite(med) || med < sameLevelLow || med > sameLevelHigh)
|
|
706
|
+
continue;
|
|
707
|
+
}
|
|
708
|
+
let pushed = 0;
|
|
709
|
+
for (const idx of component.cells) {
|
|
710
|
+
const h = heightOf(idx);
|
|
711
|
+
if (Number.isFinite(h) && (h < cellBandLow || h > cellBandHigh))
|
|
712
|
+
continue;
|
|
713
|
+
floorCells.push(idx);
|
|
714
|
+
pushed += 1;
|
|
715
|
+
}
|
|
716
|
+
if (pushed > 0)
|
|
717
|
+
emittedComponentCount += 1;
|
|
718
|
+
}
|
|
719
|
+
if (floorCells.length === 0) {
|
|
720
|
+
for (const idx of selected.cells)
|
|
721
|
+
floorCells.push(idx);
|
|
722
|
+
emittedComponentCount = 1;
|
|
723
|
+
}
|
|
724
|
+
// Local median leveling, scoped STRICTLY to the emitted floor cells: replace each
|
|
725
|
+
// floor cell's height with the median of finite floor-cell heights in a small
|
|
726
|
+
// neighborhood. Merging same-level patches (and the interpolated close cells) leaves
|
|
727
|
+
// per-cell sensor noise that tilts otherwise-flat ground past Recast's walkable
|
|
728
|
+
// slope limit, so Recast culls it (the merged floor reads as only ~42% up-facing).
|
|
729
|
+
// Median leveling removes that noise while preserving genuine large-scale slope, and
|
|
730
|
+
// because it only samples cells already in the floor set it can never pull a pool
|
|
731
|
+
// bottom or box top into the surface.
|
|
732
|
+
const floorSetForLeveling = new Set(floorCells);
|
|
733
|
+
const levelRadius = 3;
|
|
734
|
+
const leveledHeight = new Map();
|
|
735
|
+
for (const idx of floorCells) {
|
|
736
|
+
const row = Math.floor(idx / width);
|
|
737
|
+
const col = idx % width;
|
|
738
|
+
const samples = [];
|
|
739
|
+
for (let dr = -levelRadius; dr <= levelRadius; dr++) {
|
|
740
|
+
for (let dc = -levelRadius; dc <= levelRadius; dc++) {
|
|
741
|
+
const nr = row + dr;
|
|
742
|
+
const nc = col + dc;
|
|
743
|
+
if (nr < 0 || nc < 0 || nr >= height || nc >= width)
|
|
744
|
+
continue;
|
|
745
|
+
const nidx = nr * width + nc;
|
|
746
|
+
if (!floorSetForLeveling.has(nidx))
|
|
747
|
+
continue;
|
|
748
|
+
const h = heightOf(nidx);
|
|
749
|
+
if (Number.isFinite(h))
|
|
750
|
+
samples.push(h);
|
|
751
|
+
}
|
|
752
|
+
}
|
|
753
|
+
if (samples.length > 0) {
|
|
754
|
+
samples.sort((a, b) => a - b);
|
|
755
|
+
leveledHeight.set(idx, samples[Math.floor(samples.length / 2)]);
|
|
756
|
+
}
|
|
757
|
+
}
|
|
758
|
+
const surfaceHeightOf = (idx) => {
|
|
759
|
+
const lv = leveledHeight.get(idx);
|
|
760
|
+
return lv !== undefined ? lv : heightOf(idx);
|
|
761
|
+
};
|
|
762
|
+
let cx = 0;
|
|
763
|
+
let cy = 0;
|
|
764
|
+
let cz = 0;
|
|
765
|
+
for (const idx of floorCells) {
|
|
766
|
+
const center = cellCenter(idx);
|
|
767
|
+
cx += center[0];
|
|
768
|
+
cy += center[1];
|
|
769
|
+
cz += center[2];
|
|
770
|
+
}
|
|
771
|
+
const invFloor = floorCells.length > 0 ? 1 / floorCells.length : 0;
|
|
772
|
+
const floorCentroid = [cx * invFloor, cy * invFloor, cz * invFloor];
|
|
773
|
+
const selectedArea = floorCells.length * field.cell_size * field.cell_size;
|
|
774
|
+
if (selectedArea < minRoomFloorArea) {
|
|
775
|
+
throw new FastNavFloorError('too_small', `Fast nav floor is too small to be a room (${selectedArea.toFixed(2)} m^2 < ` +
|
|
776
|
+
`${minRoomFloorArea.toFixed(1)} m^2). components=${components.length}, ` +
|
|
777
|
+
`accepted=${acceptedCellCount}, states=${JSON.stringify(stateCounts)}.`, { area: selectedArea, components: components.length, stateCounts });
|
|
778
|
+
}
|
|
779
|
+
if (selection.usedLargestFallback) {
|
|
780
|
+
fallbackUsed = true;
|
|
781
|
+
log(`[WARN] Fast floor used largest viable island because no viable component was close to the seed: ` +
|
|
782
|
+
`${floorCells.length} cells, ${selectedArea.toFixed(2)} m^2, ` +
|
|
783
|
+
`seedDistance=${selected.distanceToSeed.toFixed(2)}`);
|
|
784
|
+
}
|
|
785
|
+
else if (components[0] !== selected) {
|
|
786
|
+
log(`[WARN] Fast floor ignored tiny or weak seed-near fragments and selected a viable floor island: ` +
|
|
787
|
+
`${floorCells.length} cells, ${selectedArea.toFixed(2)} m^2, ` +
|
|
788
|
+
`seedDistance=${selected.distanceToSeed.toFixed(2)}`);
|
|
789
|
+
}
|
|
790
|
+
// Emit a single connected, shared-vertex surface instead of one independent quad
|
|
791
|
+
// per cell. Each grid corner gets ONE vertex whose height is the average of the
|
|
792
|
+
// accepted cells touching it, then snapped to the dominant floor plane when within
|
|
793
|
+
// `floorFlattenTolerance`. This keeps neighbouring cells C0-continuous so Recast
|
|
794
|
+
// does not shatter a flat floor on per-cell height noise / vertical cracks.
|
|
795
|
+
const floorSet = new Set(floorCells);
|
|
796
|
+
// Anchor flattening/fallback to a height that actually belongs to THIS floor level.
|
|
797
|
+
// `floor_plane_height` is a global estimate that, on scenes with a large pool/sunken
|
|
798
|
+
// area, lands well below the deck (e.g. -3.120 vs a real floor of -1.972). Snapping or
|
|
799
|
+
// filling corners to that out-of-band value injects deep spikes into the emitted sheet
|
|
800
|
+
// (cornerY reaching -3.120) and balloons the mesh's vertical extent. Only trust the
|
|
801
|
+
// WASM plane when it sits inside the gated per-cell band; otherwise use the in-band
|
|
802
|
+
// component median (refHeight). This preserves real terrain height (no flattening) and
|
|
803
|
+
// simply stops corners from being dragged to the pool plane.
|
|
804
|
+
const rawFloorPlaneHeight = field.diagnostics.floor_plane_height;
|
|
805
|
+
const floorPlaneHeight = Number.isFinite(rawFloorPlaneHeight) &&
|
|
806
|
+
rawFloorPlaneHeight >= cellBandLow &&
|
|
807
|
+
rawFloorPlaneHeight <= cellBandHigh
|
|
808
|
+
? rawFloorPlaneHeight
|
|
809
|
+
: Number.isFinite(refHeight)
|
|
810
|
+
? refHeight
|
|
811
|
+
: rawFloorPlaneHeight;
|
|
812
|
+
const planeUsable = Number.isFinite(floorPlaneHeight);
|
|
813
|
+
const cornerCols = width + 1;
|
|
814
|
+
const cornerKey = (cc, rr) => rr * cornerCols + cc;
|
|
815
|
+
const cornerHeights = new Map();
|
|
816
|
+
const cornerHeightAt = (cc, rr) => {
|
|
817
|
+
const key = cornerKey(cc, rr);
|
|
818
|
+
const cached = cornerHeights.get(key);
|
|
819
|
+
if (cached !== undefined)
|
|
820
|
+
return cached;
|
|
821
|
+
let sum = 0;
|
|
822
|
+
let count = 0;
|
|
823
|
+
for (let dr = -1; dr <= 0; dr++) {
|
|
824
|
+
for (let dc = -1; dc <= 0; dc++) {
|
|
825
|
+
const col = cc + dc;
|
|
826
|
+
const row = rr + dr;
|
|
827
|
+
if (col < 0 || row < 0 || col >= width || row >= height)
|
|
828
|
+
continue;
|
|
829
|
+
const cidx = row * width + col;
|
|
830
|
+
if (!floorSet.has(cidx))
|
|
831
|
+
continue;
|
|
832
|
+
const ch = surfaceHeightOf(cidx);
|
|
833
|
+
if (Number.isFinite(ch)) {
|
|
834
|
+
sum += ch;
|
|
835
|
+
count += 1;
|
|
836
|
+
}
|
|
837
|
+
}
|
|
838
|
+
}
|
|
839
|
+
let h = count > 0 ? sum / count : planeUsable ? floorPlaneHeight : 0;
|
|
840
|
+
if (planeUsable && Math.abs(h - floorPlaneHeight) <= floorFlattenTolerance) {
|
|
841
|
+
h = floorPlaneHeight;
|
|
842
|
+
}
|
|
843
|
+
cornerHeights.set(key, h);
|
|
844
|
+
return h;
|
|
845
|
+
};
|
|
846
|
+
const positions = [];
|
|
847
|
+
const indices = [];
|
|
848
|
+
const cornerVertices = new Map();
|
|
849
|
+
const cornerVertex = (cc, rr) => {
|
|
850
|
+
const key = cornerKey(cc, rr);
|
|
851
|
+
const existing = cornerVertices.get(key);
|
|
852
|
+
if (existing !== undefined)
|
|
853
|
+
return existing;
|
|
854
|
+
const p = pointAt(cc, rr, cornerHeightAt(cc, rr));
|
|
855
|
+
const vi = positions.length / 3;
|
|
856
|
+
positions.push(p[0], p[1], p[2]);
|
|
857
|
+
cornerVertices.set(key, vi);
|
|
858
|
+
return vi;
|
|
859
|
+
};
|
|
860
|
+
for (const idx of floorCells) {
|
|
861
|
+
const row = Math.floor(idx / width);
|
|
862
|
+
const col = idx % width;
|
|
863
|
+
const v00 = cornerVertex(col, row);
|
|
864
|
+
const v01 = cornerVertex(col, row + 1);
|
|
865
|
+
const v11 = cornerVertex(col + 1, row + 1);
|
|
866
|
+
const v10 = cornerVertex(col + 1, row);
|
|
867
|
+
indices.push(v00, v01, v11, v00, v11, v10);
|
|
868
|
+
}
|
|
869
|
+
log(`[INFO] Fast floor field: accepted=${acceptedCellCount}, obstacles=${obstacleCellCount}, ` +
|
|
870
|
+
`rejected=${rejectedCellCount}, bridged=${bridgedCellCount}, closed=${closedCellCount}, ` +
|
|
871
|
+
`components=${components.length}, emittedComponents=${emittedComponentCount}, ` +
|
|
872
|
+
`floorBand=[${Number.isFinite(refHeight) ? sameLevelLow.toFixed(2) : 'n/a'},${Number.isFinite(refHeight) ? sameLevelHigh.toFixed(2) : 'n/a'}], ` +
|
|
873
|
+
`selectedCells=${floorCells.length}, selectedArea=${selectedArea.toFixed(2)}, ` +
|
|
874
|
+
`vertices=${positions.length / 3}, maskCells=${selectedMask.filter(Boolean).length}`);
|
|
875
|
+
return {
|
|
876
|
+
positions: new Float32Array(positions),
|
|
877
|
+
indices: new Uint32Array(indices),
|
|
878
|
+
selectedCellCount: floorCells.length,
|
|
879
|
+
acceptedCellCount,
|
|
880
|
+
obstacleCellCount,
|
|
881
|
+
rejectedCellCount,
|
|
882
|
+
selectedArea,
|
|
883
|
+
centroid: floorCentroid,
|
|
884
|
+
componentCount: components.length,
|
|
885
|
+
fallbackUsed,
|
|
886
|
+
seedDistance: selected.distanceToSeed,
|
|
887
|
+
bridgedCellCount,
|
|
888
|
+
};
|
|
889
|
+
}
|
|
890
|
+
/**
|
|
891
|
+
* Run floor-field extraction with the built-in adaptive recovery ladder: for each
|
|
892
|
+
* step, merge its settings over `baseSettings`, build the walkable ground field,
|
|
893
|
+
* snap the seed to the detected floor plane, then build the floor mesh with that
|
|
894
|
+
* step's `minRoomFloorArea`. On a {@link FastNavFloorError} (including an empty
|
|
895
|
+
* mesh) it logs and escalates to the next step. The first success wins; if every
|
|
896
|
+
* step fails it throws an aggregated {@link FastNavFloorError}.
|
|
897
|
+
*/
|
|
898
|
+
export async function extractFloorFieldWithRecovery(args) {
|
|
899
|
+
const { bytes, buildField, baseSettings, seed, recovery, strayTrim, denseSeed, log } = args;
|
|
900
|
+
const steps = recovery.steps;
|
|
901
|
+
const reseedThreshold = denseSeed?.reseedThreshold ?? 2.0;
|
|
902
|
+
const denseSeedEnabled = denseSeed?.enabled ?? true;
|
|
903
|
+
let lastError = null;
|
|
904
|
+
const attempted = [];
|
|
905
|
+
for (let i = 0; i < steps.length; i++) {
|
|
906
|
+
const step = steps[i];
|
|
907
|
+
const hasMore = i < steps.length - 1;
|
|
908
|
+
const settings = { ...baseSettings, ...step.settings };
|
|
909
|
+
try {
|
|
910
|
+
let field = await buildField(bytes, settings);
|
|
911
|
+
let effectiveSeed = seed;
|
|
912
|
+
const floorPlaneY = field.diagnostics.floor_plane_height;
|
|
913
|
+
if (Number.isFinite(floorPlaneY)) {
|
|
914
|
+
effectiveSeed = [seed[0], floorPlaneY, seed[2]];
|
|
915
|
+
log(`[INFO] Snapped fast seed to floor plane y=${floorPlaneY.toFixed(3)} (step "${step.label}")`);
|
|
916
|
+
}
|
|
917
|
+
// Anchor on the dense floor area (where most splats actually sit) instead of
|
|
918
|
+
// a sparse floater plane below it; rebuild the field around the dense seed
|
|
919
|
+
// AND with a default region adapted to the dense floor band, so deep stray
|
|
920
|
+
// floaters below/above the real floor no longer drag floor detection off.
|
|
921
|
+
if (denseSeedEnabled) {
|
|
922
|
+
const dense = estimateDenseFloorSeed(field, effectiveSeed, denseSeed);
|
|
923
|
+
const movedXZ = Math.hypot(dense[0] - effectiveSeed[0], dense[2] - effectiveSeed[2]);
|
|
924
|
+
const movedY = Math.abs(dense[1] - effectiveSeed[1]);
|
|
925
|
+
if (movedXZ > reseedThreshold || movedY > reseedThreshold) {
|
|
926
|
+
const rebuild = { ...settings, collision_seed: dense };
|
|
927
|
+
// Only adapt the default region when the caller hasn't pinned one.
|
|
928
|
+
if (!settings.region_min || !settings.region_max) {
|
|
929
|
+
const region = estimateDenseFloorRegion(field, denseSeed);
|
|
930
|
+
if (region) {
|
|
931
|
+
rebuild.region_min = region.min;
|
|
932
|
+
rebuild.region_max = region.max;
|
|
933
|
+
log(`[INFO] FAST NAV adapting default region to dense floor band ` +
|
|
934
|
+
`(y ${region.min[1].toFixed(2)}..${region.max[1].toFixed(2)}) (step "${step.label}").`);
|
|
935
|
+
}
|
|
936
|
+
}
|
|
937
|
+
log(`[INFO] FAST NAV re-seeding to dense floor area ` +
|
|
938
|
+
`(${dense.map((v) => v.toFixed(2)).join(', ')}; moved ${movedXZ.toFixed(1)}m XZ, ${movedY.toFixed(1)}m Y) (step "${step.label}").`);
|
|
939
|
+
field = await buildField(bytes, rebuild);
|
|
940
|
+
const reFloorY = field.diagnostics.floor_plane_height;
|
|
941
|
+
effectiveSeed = Number.isFinite(reFloorY) ? [dense[0], reFloorY, dense[2]] : dense;
|
|
942
|
+
}
|
|
943
|
+
}
|
|
944
|
+
const floorMesh = buildFastFloorMesh(field, effectiveSeed, step.minRoomFloorArea, log, step.strayTrim ?? strayTrim);
|
|
945
|
+
if (floorMesh.positions.length === 0 || floorMesh.indices.length === 0) {
|
|
946
|
+
throw new FastNavFloorError('empty_mesh', 'Fast nav produced an empty floor mesh.', {
|
|
947
|
+
components: floorMesh.componentCount,
|
|
948
|
+
});
|
|
949
|
+
}
|
|
950
|
+
if (i > 0) {
|
|
951
|
+
log(`[SUCCESS] FAST NAV recovery succeeded on step "${step.label}" ` +
|
|
952
|
+
`after ${i} escalation(s): area=${floorMesh.selectedArea.toFixed(2)} m^2.`);
|
|
953
|
+
}
|
|
954
|
+
return { field, floorMesh, effectiveSeed, stepLabel: step.label };
|
|
955
|
+
}
|
|
956
|
+
catch (error) {
|
|
957
|
+
if (!(error instanceof FastNavFloorError)) {
|
|
958
|
+
throw error;
|
|
959
|
+
}
|
|
960
|
+
lastError = error;
|
|
961
|
+
const areaStr = error.area !== undefined ? `${error.area.toFixed(2)} m^2` : 'n/a';
|
|
962
|
+
attempted.push(`${step.label}(${error.reason})`);
|
|
963
|
+
log(`[WARN] FAST NAV recovery: step "${step.label}" failed (${error.reason}, ${areaStr})` +
|
|
964
|
+
(hasMore ? '; escalating extraction parameters...' : '.'));
|
|
965
|
+
}
|
|
966
|
+
}
|
|
967
|
+
const summary = attempted.join(' -> ');
|
|
968
|
+
if (lastError) {
|
|
969
|
+
throw new FastNavFloorError(lastError.reason, `FAST NAV floor extraction failed after ${steps.length} recovery step(s): ${summary}. ${lastError.message}`, { area: lastError.area, components: lastError.components, stateCounts: lastError.stateCounts });
|
|
970
|
+
}
|
|
971
|
+
throw new FastNavFloorError('no_component', 'FAST NAV recovery had no configured steps.');
|
|
972
|
+
}
|