Least-cost search
32-bit in partSearches the grid for the cheapest allowed path and returns its points and total cost.
grade = |z₂ − z₁| ÷ length ≤ limit; cost = length × unit cost × factor × (1 + w·(grade ÷ limit)²) + area × length × earth cost
Grid elevations (z), area factors (fac) and accumulated costs (g) are Float32Array. The grade limit is tested on the difference of two 32-bit elevations, each new total is compared with and stored in the 32-bit array, and the returned cost is read back from it, so that cost carries about 7 digits. Cell indexes are truncated with a bitwise OR; these are indexes, not measured values.
const N = nx * ny, z = new Float32Array(N), fac = new Float32Array(N).fill(1);
for (let j = 0; j < ny; j++) for (let i = 0; i < nx; i++) {
const x = b.minX + (i + 0.5) * c, y = b.minY + (j + 0.5) * c, v = ground.elevationAt(x, y);
z[j * nx + i] = v === null || v === undefined || !Number.isFinite(v) ? NaN : v;
}
const mark = (list, fn) => { for (const it of list || []) { const poly = it.poly || it; if (!poly || poly.length < 3) continue; for (let j = 0; j < ny; j++) for (let i = 0; i < nx; i++) if (inPoly(poly, b.minX + (i + 0.5) * c, b.minY + (j + 0.5) * c)) fn(j * nx + i, it); } };
mark(opts.avoid, (k, it) => { fac[k] *= it.factor || 2; });
mark(opts.prefer, (k, it) => { fac[k] *= it.factor || 0.5; });
mark(opts.forbid, (k) => { fac[k] = Infinity; });
const cellOf = (p) => { const i = Math.floor((p.x - b.minX) / c), j = Math.floor((p.y - b.minY) / c); return i >= 0 && j >= 0 && i < nx && j < ny ? j * nx + i : -1; };
const s0 = cellOf(from), t0 = cellOf(to);
if (s0 < 0 || t0 < 0 || Number.isNaN(z[s0]) || Number.isNaN(z[t0])) return { found: false, reason: 'off-ground' };
if (!Number.isFinite(fac[s0]) || !Number.isFinite(fac[t0])) return { found: false, reason: 'forbidden-end' };
// allowed turns per incoming heading
const maxTurn = (l1, l2) => (o.minRadius > 0 ? (l1 + l2) / (2 * o.minRadius) + 1e-9 : Math.PI / 2 + 1e-9);
const turns = dirs.map((d1) => {
const list = [];
dirs.forEach((d2, k) => {
let da = Math.abs(d2.a - d1.a); if (da > Math.PI) da = 2 * Math.PI - da;
if (da <= maxTurn(d1.n * c, d2.n * c)) list.push(k);
});
return list;
});
const minFac = Math.min(1, ...[...(opts.prefer || [])].map((p) => p.factor || 0.5));
const tx = b.minX + ((t0 % nx) + 0.5) * c, ty = b.minY + (Math.floor(t0 / nx) + 0.5) * c;
const hOf = (k) => o.unitCost * minFac * Math.hypot(b.minX + ((k % nx) + 0.5) * c - tx, b.minY + (Math.floor(k / nx) + 0.5) * c - ty);
const g = new Float32Array(N * H).fill(Infinity), prev = new Int32Array(N * H).fill(-1), heap = new Heap();
for (let h = 0; h < H; h++) { g[s0 * H + h] = 0; heap.push(hOf(s0), s0 * H + h); }
const zAt = (x, y) => { const v = ground.elevationAt(x, y); return v === null || v === undefined ? NaN : v; };
const endTol = K * c * 1.0001;
let goal = -1, expanded = 0;
while (heap.size) {
const s = heap.pop(), cell = (s / H) | 0, h = s - cell * H, gs = g[s];
const cx = b.minX + ((cell % nx) + 0.5) * c, cy = b.minY + (Math.floor(cell / nx) + 0.5) * c;
if (Math.hypot(cx - to.x, cy - to.y) <= Math.max(endTol * 0.5, c * 0.75) && Number.isFinite(gs)) { goal = s; break; }
expanded++;
const ci = cell % nx, cj = (cell - ci) / nx;
for (const k2 of turns[h]) {
const d = dirs[k2], ni = ci + d.dx, nj = cj + d.dy;
if (ni < 0 || nj < 0 || ni >= nx || nj >= ny) continue;
const nc = nj * nx + ni;
if (Number.isNaN(z[nc]) || !Number.isFinite(fac[nc])) continue;
const len = d.n * c, grade = Math.abs(z[nc] - z[cell]) / len;
if (grade > o.maxGrade) continue;
const f = (fac[cell] + fac[nc]) / 2;
let cost = len * o.unitCost * f * (1 + o.gradeWeight * (grade / o.maxGrade) ** 2);
if (o.width > 0 && o.earthCost > 0) { // cut and fill the roadway's width needs across the slope at the middle of the step
const mx = (cx + b.minX + (ni + 0.5) * c) / 2, my = (cy + b.minY + (nj + 0.5) * c) / 2, ux = -d.dy / d.n, uy = d.dx / d.n, w = o.width / 2;
const zl = zAt(mx + ux * w, my + uy * w), zr = zAt(mx - ux * w, my - uy * w);
if (Number.isFinite(zl) && Number.isFinite(zr)) {
const s = Math.abs(zl - zr) / o.width, den = 1 - o.sideSlope * s;
const area = den > 0.05 ? (o.width * o.width * s) / 4 / den : (o.width * o.width * 0.05) / 4 / 0.05; // a slope steeper than the sides can hold: cap, very costly
cost += area * len * o.earthCost;
}
}
const ns = nc * H + k2, ng = gs + cost;
if (ng < g[ns]) { g[ns] = ng; prev[ns] = s; heap.push(ng + hOf(nc), ns); }
}
}
if (goal < 0) return { found: false, reason: expanded >= N * H * 0.98 ? 'unreachable' : (o.minRadius > 0 ? 'unreachable-curve' : 'unreachable'), expanded, cell: c, K };