From c7d90070ffea3710a887a164690118c9de290da7 Mon Sep 17 00:00:00 2001 From: Chadwick Maycumber <36460656+cmaycumber@users.noreply.github.com> Date: Thu, 24 Sep 2026 18:38:37 -0700 Subject: [PATCH] feat(nav): renderoni/nav with tiled grid A*, expedition router and spatial hash NavGrid is generic over a NavCostField the game supplies (size, cellCost, optional coarseWalkable/passable). Tiles are sampled lazily and LRU-evicted (evicted arrays are pooled), blockers are re-applied on rebuild, and A* runs in a bounded window with generation-stamped scratch and a typed-array heap. Cell size, tile size, tile cap, window, budget, easy-cost and blocker slack are options; defaults reproduce the Sunder March grid exactly. ExpeditionRouter joins complete local paths into long corridors against a small RouterNav interface. SpatialHash works over anything with a position {x, z}. Co-Authored-By: Claude Opus 5.5 (1M context) --- AGENTS.md | 2 + README.md | 35 ++ package.json | 4 + src/nav/expedition-router.ts | 364 +++++++++++++++++++ src/nav/index.ts | 3 + src/nav/nav-grid.ts | 658 +++++++++++++++++++++++++++++++++++ src/nav/spatial-hash.ts | 65 ++++ tests/nav.test.ts | 300 ++++++++++++++++ tsup.config.ts | 1 + 9 files changed, 1432 insertions(+) create mode 100644 src/nav/expedition-router.ts create mode 100644 src/nav/index.ts create mode 100644 src/nav/nav-grid.ts create mode 100644 src/nav/spatial-hash.ts create mode 100644 tests/nav.test.ts diff --git a/AGENTS.md b/AGENTS.md index 4176102..968d257 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -11,6 +11,7 @@ Renderoni is structured in 4 strict hierarchical layers: - **Rule 1**: NEVER call `Math.random()`, `Date.now()`, `performance.now()`, or `requestAnimationFrame()` inside simulation logic or entity updates. Always use `engine.prng` and `engine.clock.tick`. - **Rule 2**: NEVER bypass the dual-buffer transform pipeline. Write physics transforms into canonical buffer slots, never directly into render scene graphs. - **L1: Batteries & Subsystems (`src/presets/`, `src/animation/`, `src/audio/`, `src/vfx/`, `src/ui/`, `src/scene/`)**: High-level declarative presets (`body`, `sensor`, `light`, `kccPlayer`, `dynamicPlayer`, `proceduralModel`) and compact scene inventories for prompt → img2threejs factories. + - **Navigation (`src/nav/`, `renderoni/nav`)**: `NavGrid` (tiled, lazily sampled grid A* over a game-supplied `NavCostField`, with dynamic blockers, a per-step search `budget` and windowed partial paths), `ExpeditionRouter` (incremental long-distance corridors from verified local paths) and `SpatialHash`. Deterministic and allocation-light: keep hot paths free of per-call allocation and never read time or `Math.random()` in them. - **L2: Agent Tooling & MCP (`src/mcp/`, `src/testing/`)**: Stdio Model Context Protocol server, custom Vitest matchers, and headless CLI verification. - **L3: Web Application & Demos (`src/demo/`, `index.html`)**: Interactive playground and multi-archetype web showcases. @@ -29,6 +30,7 @@ import { animation } from 'renderoni/animation'; import { vfx } from 'renderoni/vfx'; import { ui } from 'renderoni/ui'; import { createMCPServer } from 'renderoni/mcp'; +import { NavGrid, ExpeditionRouter, SpatialHash } from 'renderoni/nav'; import 'renderoni/testing/matchers'; ``` diff --git a/README.md b/README.md index 65b4f80..07e43fc 100644 --- a/README.md +++ b/README.md @@ -192,6 +192,40 @@ test('player collects coin deterministically', async () => { --- +## 🧭 Navigation (`renderoni/nav`) + +Grid pathfinding for large, streamed worlds. The game supplies only its cost rules; the grid samples them lazily in tiles (LRU-evicted), layers rectangular blockers on top, and runs A* inside a bounded window with persistent scratch. + +```ts +import { NavGrid, ExpeditionRouter, SpatialHash } from 'renderoni/nav'; + +const nav = new NavGrid({ + size: 3000, // metres, centred on the origin + cellCost: (x, z) => (Math.abs(x) < 20 && z < 1000 ? 0 : 1), // 0 = blocked, >1 slower, <1 faster +}); +nav.addBlocker('hut', 40, 40, 12, 8, 0); + +nav.budget = 14; // searches allowed this step +const path = nav.findPath(-100, 0, 100, 0); // Vector2 waypoints (y = world z) +const arrived = path !== null && nav.lastPathComplete; // partial paths are never arrival + +// Long trips: corridors built from complete local paths, advanced a little each step. +const router = new ExpeditionRouter(nav); +const ticket = router.request({ x: -1400, z: -1400 }, { x: 1400, z: -1400 }); +router.update(1); // pass the sim tick; check ticket.state === 'ready' + +const hash = new SpatialHash<{ position: { x: number; z: number } }>(16); +hash.rebuild([{ position: { x: 0, z: 0 } }]); +const near = hash.within(0, 0, 5); +console.log(arrived, ticket.state, near.length); +``` + +- `walkable(x, z, coarse?)`, `walkableFast` (no tile builds), `terrainPassable`, `segmentWalkable`, `lineClear`, `nearestWalkable`. +- `revision` bumps on every blocker change; `refused` reports a search denied for lack of `budget`. +- Results are a pure function of costs, blockers and the query. + +--- + ## 🤖 MCP Agent Tools When connected to AI coding assistants (Antigravity, Claude Code, Cursor), use Renderoni's built-in MCP server: @@ -235,6 +269,7 @@ import { ui } from 'renderoni/ui'; import { animation } from 'renderoni/animation'; import { startEditorServer, generateAsset, scaffoldAsset } from 'renderoni/editor'; import { createMCPServer } from 'renderoni/mcp'; +import { NavGrid, ExpeditionRouter, SpatialHash } from 'renderoni/nav'; import 'renderoni/testing/matchers'; ``` diff --git a/package.json b/package.json index 02e2fca..3884c5b 100644 --- a/package.json +++ b/package.json @@ -60,6 +60,10 @@ "./editor": { "types": "./dist/editor/index.d.ts", "import": "./dist/editor/index.js" + }, + "./nav": { + "types": "./dist/nav/index.d.ts", + "import": "./dist/nav/index.js" } }, "files": [ diff --git a/src/nav/expedition-router.ts b/src/nav/expedition-router.ts new file mode 100644 index 0000000..a2f098c --- /dev/null +++ b/src/nav/expedition-router.ts @@ -0,0 +1,364 @@ +/** + * Renderoni Expedition Router + * + * Sparse hierarchical A* for trips longer than one NavGrid search window. A coarse lattice of + * waypoints (`spacing` metres apart, snapped to walkable cells) is searched with A*, and every + * lattice edge is verified by a complete local `findPath`, so a finished corridor consists only + * of real, walkable paths. Searches are incremental: `update(time)` advances all pending + * requests a little each step, sharing the nav grid's per-step search budget, with priority + * requests (e.g. the player's) receiving `priorityTurns` turns for each ordinary turn. + * + * Edge results are cached per nav `revision`; any blocker change cancels in-flight requests. + * Deterministic for a deterministic sequence of requests and `update` times. + */ +import type { NavPoint } from './nav-grid.js'; + +/** The subset of NavGrid the router needs. */ +export interface RouterNav { + readonly size: number; + readonly revision: number; + readonly budget: number; + readonly lastPathComplete: boolean; + inBounds(x: number, z: number): boolean; + walkable(x: number, z: number): boolean; + nearestWalkable(x: number, z: number): NavPoint; + /** Waypoints use `.y` for world z, as NavGrid's Vector2 results do. */ + findPath(sx: number, sz: number, tx: number, tz: number, maxNodes?: number): ReadonlyArray<{ x: number; y: number }> | null; +} + +export type RoutePoint = NavPoint; + +export interface RouteTicket { + state: 'planning' | 'ready' | 'blocked' | 'cancelled'; + points: RoutePoint[]; + reason: string | null; + revision: number; + expanded: number; + connections: number; +} + +export interface ExpeditionRouterOptions { + /** Initial lattice spacing in metres. Default 192. */ + spacing?: number; + /** Failed searches retry at half spacing down to (but not below) this. Default 48. */ + minSpacing?: number; + /** Pending requests beyond this are rejected as blocked. Default 128. */ + maxPending?: number; + /** Local A* node budget for edges touching the start or goal. Default 7000. */ + endpointNodes?: number; + /** Local A* node budget for lattice-to-lattice edges. Default 2200. */ + latticeNodes?: number; + /** Connection limit per search: [priority, ordinary]. Default [9000, 1800]. */ + connectionLimit?: readonly [number, number]; + /** Attempt limit per search: [priority, ordinary]. Default [90000, 18000]. */ + attemptLimit?: readonly [number, number]; + /** Stop advancing while nav.budget is at or below this, leaving searches for direct callers. Default 3. */ + budgetReserve?: number; + /** Maximum advances per update. Default 256. */ + workPerUpdate?: number; + /** Priority turns per ordinary turn. Default 3. */ + priorityTurns?: number; + /** Cached lattice points and edges (each). Default 16000. */ + cacheCap?: number; +} + +interface Node { + id: number; + point: RoutePoint; + g: number; + parent: number | null; + edge: RoutePoint[]; +} + +interface Search { + ticket: RouteTicket; + start: RoutePoint; + goal: RoutePoint; + spacing: number; + width: number; + nodes: Map; + closed: Set; + open: StableHeap; + current: Node | null; + neighbours: number[]; + edgeIndex: number; + attempts: number; + priority: boolean; + isCurrent: () => boolean; +} + +interface Edge { + points: RoutePoint[]; + length: number; +} + +const START = -1; +const GOAL = -2; + +export class ExpeditionRouter { + private requests: Search[] = []; + private priorityCursor = 0; + private ordinaryCursor = 0; + private lastTime = -Infinity; + private revision = -1; + private points = new Map(); + private edges = new Map(); + private readonly opts: Required; + + constructor(private nav: RouterNav, options: ExpeditionRouterOptions = {}) { + this.opts = { + spacing: 192, + minSpacing: 48, + maxPending: 128, + endpointNodes: 7000, + latticeNodes: 2200, + connectionLimit: [9000, 1800], + attemptLimit: [90000, 18000], + budgetReserve: 3, + workPerUpdate: 256, + priorityTurns: 3, + cacheCap: 16000, + ...options, + }; + } + + /** Requests still planning whose owner still wants them. */ + get pending(): number { + return this.requests.filter((s) => s.ticket.state === 'planning' && s.isCurrent()).length; + } + + /** Cached edge count (diagnostics). */ + get cacheSize(): number { + return this.edges.size; + } + + /** + * Start planning a route. The returned ticket updates in place as `update` runs. `isCurrent` + * lets the owner abandon the request implicitly (e.g. when its order changes). + */ + request(start: RoutePoint, goal: RoutePoint, priority = false, isCurrent: () => boolean = () => true): RouteTicket { + this.syncRevision(); + const ticket: RouteTicket = { state: 'planning', points: [], reason: 'Planning a terrain route', revision: this.nav.revision, expanded: 0, connections: 0 }; + if (this.pending >= this.opts.maxPending) { + ticket.state = 'blocked'; + ticket.reason = 'Route planner busy; waiting to retry'; + return ticket; + } + this.requests.push(this.search(ticket, start, goal, this.opts.spacing, priority, isCurrent)); + return ticket; + } + + cancel(ticket: RouteTicket | null): void { + if (ticket?.state === 'planning') ticket.state = 'cancelled'; + } + + /** Advance pending searches; runs at most once per distinct `time` (pass the sim tick or time). */ + update(time: number): void { + this.syncRevision(); + if (time === this.lastTime) return; + this.lastTime = time; + this.requests = this.requests.filter((s) => { + if (!s.isCurrent()) s.ticket.state = 'cancelled'; + return s.ticket.state === 'planning'; + }); + const priority = this.requests.filter((s) => s.priority), ordinary = this.requests.filter((s) => !s.priority); + const turns = this.opts.priorityTurns + 1; + let work = 0; + while ((priority.length || ordinary.length) && this.nav.budget > this.opts.budgetReserve && work < this.opts.workPerUpdate) { + const preferred = work++ % turns !== turns - 1; + const group = priority.length && (preferred || !ordinary.length) ? priority : ordinary; + const cursor = group === priority ? this.priorityCursor++ : this.ordinaryCursor++; + const index = cursor % group.length, search = group[index]; + this.advance(search); + if (search.ticket.state !== 'planning') group.splice(index, 1); + } + } + + private search(ticket: RouteTicket, start: RoutePoint, goal: RoutePoint, spacing: number, priority: boolean, isCurrent: () => boolean): Search { + const first: Node = { id: START, point: { ...start }, g: 0, parent: null, edge: [] }; + const open = new StableHeap(); + open.push(START, 0); + return { + ticket, start, goal, spacing, width: Math.ceil(this.nav.size / spacing), nodes: new Map([[START, first]]), closed: new Set(), open, + current: null, neighbours: [], edgeIndex: 0, attempts: 0, priority, isCurrent, + }; + } + + private syncRevision(): void { + if (this.revision === this.nav.revision) return; + this.revision = this.nav.revision; + this.points.clear(); + this.edges.clear(); + for (const s of this.requests) { + s.ticket.state = 'cancelled'; + s.ticket.reason = 'Terrain obstacles changed'; + } + this.requests = []; + this.priorityCursor = 0; + this.ordinaryCursor = 0; + } + + private point(s: Search, id: number): RoutePoint | null { + if (id === START) return s.start; + if (id === GOAL) return s.goal; + const key = `${s.spacing}:${id}`; + if (this.points.has(key)) return this.points.get(key)!; + const half = this.nav.size / 2; + const x = -half + ((id % s.width) + 0.5) * s.spacing, z = -half + (Math.floor(id / s.width) + 0.5) * s.spacing; + let result: RoutePoint | null = null; + if (this.nav.inBounds(x, z)) { + const p = this.nav.nearestWalkable(x, z); + if (this.nav.walkable(p.x, p.z)) result = p; + } + if (this.points.size >= this.opts.cacheCap) this.points.delete(this.points.keys().next().value!); + this.points.set(key, result); + return result; + } + + private adjacent(s: Search, node: Node): number[] { + const half = this.nav.size / 2; + const x = node.id >= 0 ? node.id % s.width : Math.floor((node.point.x + half) / s.spacing); + const z = node.id >= 0 ? Math.floor(node.id / s.width) : Math.floor((node.point.z + half) / s.spacing); + const out: number[] = []; + for (let dz = -1; dz <= 1; dz++) { + for (let dx = -1; dx <= 1; dx++) { + const nx = x + dx, nz = z + dz; + if (nx >= 0 && nz >= 0 && nx < s.width && nz < s.width && nz * s.width + nx !== node.id) out.push(nz * s.width + nx); + } + } + if (Math.hypot(node.point.x - s.goal.x, node.point.z - s.goal.z) <= s.spacing * 2) out.unshift(GOAL); + return out; + } + + private connection(s: Search, from: Node, id: number, to: RoutePoint): Edge | null { + const cacheable = from.id >= 0 && id >= 0; + const reverse = from.id > id, key = `${s.spacing}:${Math.min(from.id, id)}:${Math.max(from.id, id)}`; + if (cacheable && this.edges.has(key)) { + const edge = this.edges.get(key); + if (!edge) return null; + if (!reverse) return edge; + return { length: edge.length, points: [...edge.points.slice(0, -1).reverse(), { ...to }] }; + } + const path = this.nav.findPath(from.point.x, from.point.z, to.x, to.z, from.id < 0 || id < 0 ? this.opts.endpointNodes : this.opts.latticeNodes); + s.ticket.connections++; + if (!path || !this.nav.lastPathComplete) { + if (cacheable) this.cache(key, null); + return null; + } + const points = path.map((p) => ({ x: p.x, z: p.y })); + let length = 0, previous = from.point; + for (const p of points) { + length += Math.hypot(p.x - previous.x, p.z - previous.z); + previous = p; + } + const edge = { points, length }; + if (cacheable) this.cache(key, reverse ? { length, points: [...points.slice(0, -1).reverse(), { ...from.point }] } : edge); + return edge; + } + + private cache(key: string, edge: Edge | null): void { + if (this.edges.size >= this.opts.cacheCap) this.edges.delete(this.edges.keys().next().value!); + this.edges.set(key, edge); + } + + private advance(s: Search): void { + const which = s.priority ? 0 : 1; + const connectionLimit = this.opts.connectionLimit[which], attemptLimit = this.opts.attemptLimit[which]; + if (!s.current || s.edgeIndex >= s.neighbours.length) { + s.current = null; + while (s.open.size) { + const id = s.open.pop(); + if (s.closed.has(id)) continue; + const node = s.nodes.get(id)!; + if (id === GOAL) { + this.finish(s, node); + return; + } + s.closed.add(id); + s.ticket.expanded++; + s.current = node; + s.neighbours = this.adjacent(s, node); + s.edgeIndex = 0; + break; + } + if (!s.current || s.ticket.connections >= connectionLimit || s.attempts >= attemptLimit) { + if (!s.current && s.spacing > this.opts.minSpacing && s.ticket.connections < connectionLimit) { + Object.assign(s, this.search(s.ticket, s.start, s.goal, s.spacing / 2, s.priority, s.isCurrent)); + s.ticket.reason = 'Checking narrower terrain passages'; + return; + } + s.ticket.state = 'blocked'; + s.ticket.reason = s.current ? 'Route search limit reached; try a waypoint or retry' : 'No terrain route found; try another waypoint'; + return; + } + } + s.attempts++; + const from = s.current, id = s.neighbours[s.edgeIndex++]; + if (s.closed.has(id)) return; + const to = this.point(s, id); + if (!to) return; + const edge = this.connection(s, from, id, to); + if (!edge) return; + const g = from.g + edge.length, previous = s.nodes.get(id); + if (!previous || g < previous.g) { + s.nodes.set(id, { id, point: to, g, parent: from.id, edge: edge.points }); + s.open.push(id, g + Math.hypot(to.x - s.goal.x, to.z - s.goal.z)); + } + } + + private finish(s: Search, node: Node): void { + const edges: RoutePoint[][] = []; + let current: Node | undefined = node; + while (current?.parent != null) { + edges.push(current.edge); + current = s.nodes.get(current.parent); + } + s.ticket.points = edges.reverse().flat(); + s.ticket.state = 'ready'; + s.ticket.reason = null; + } +} + +/** Stable binary heap: equal priorities pop in insertion order, keeping choices deterministic. */ +class StableHeap { + private data: { id: number; priority: number; sequence: number }[] = []; + private sequence = 0; + + get size(): number { + return this.data.length; + } + + push(id: number, priority: number): void { + this.data.push({ id, priority, sequence: this.sequence++ }); + let i = this.data.length - 1; + while (i) { + const p = (i - 1) >> 1; + if (!this.less(i, p)) break; + [this.data[i], this.data[p]] = [this.data[p], this.data[i]]; + i = p; + } + } + + pop(): number { + const first = this.data[0], last = this.data.pop()!; + if (this.data.length) { + this.data[0] = last; + let i = 0; + for (;;) { + let m = i; + const a = i * 2 + 1, b = a + 1; + if (a < this.data.length && this.less(a, m)) m = a; + if (b < this.data.length && this.less(b, m)) m = b; + if (m === i) break; + [this.data[i], this.data[m]] = [this.data[m], this.data[i]]; + i = m; + } + } + return first.id; + } + + private less(a: number, b: number): boolean { + const x = this.data[a], y = this.data[b]; + return x.priority < y.priority || (x.priority === y.priority && x.sequence < y.sequence); + } +} diff --git a/src/nav/index.ts b/src/nav/index.ts new file mode 100644 index 0000000..e89f71a --- /dev/null +++ b/src/nav/index.ts @@ -0,0 +1,3 @@ +export * from './nav-grid.js'; +export * from './spatial-hash.js'; +export * from './expedition-router.js'; diff --git a/src/nav/nav-grid.ts b/src/nav/nav-grid.ts new file mode 100644 index 0000000..22e6770 --- /dev/null +++ b/src/nav/nav-grid.ts @@ -0,0 +1,658 @@ +/** + * Renderoni Nav Grid + * + * Tiled, lazily sampled grid navigation for large worlds. The world is a square of side + * `field.size` metres centred on the origin, divided into square cells. Cell costs live in + * tiles (tileSize × tileSize cells) that are built on first touch, so nothing world-sized is + * ever allocated: each cell asks the game's cost field once (NaN marks "not sampled yet"), + * respects any rectangular blockers that overlap it, and the least recently used tiles are + * evicted when too many are resident. Blockers are kept outside the tiles, so a rebuilt tile + * is identical to the evicted one. + * + * A* runs inside a bounded window around the start and goal with persistent, + * generation-stamped scratch (no per-search allocation beyond the returned waypoints). + * Requests longer than the window, or that exhaust `maxNodes`, return the best partial path + * with `lastPathComplete = false`. A per-step `budget` bounds how many searches run; callers + * refused for lack of budget (`refused = true`) retry on a later step. + * + * Deterministic: results are a pure function of the cost field, the blockers and the query. + */ +import { Vector2 } from 'three'; + +/** The game's cost rules. Sampled at cell centres, once per cell while its tile is resident. */ +export interface NavCostField { + /** World side length in metres; the world spans [-size/2, size/2) on x and z. */ + readonly size: number; + /** Traversal multiplier at a cell centre: 0 (or NaN / negative) = blocked, 1 = normal, >1 slow, <1 fast. */ + cellCost(x: number, z: number): number; + /** + * Cheap far-field check used by `walkable(x, z, true)` and `nearestWalkable(x, z, true)`; + * should not touch expensive data. Defaults to `walkableFast`. + */ + coarseWalkable?(x: number, z: number): boolean; + /** + * Terrain-only passability at a cell centre (no blockers). Used by `terrainPassable` and + * `walkableFast` for cells whose tile is not resident. Defaults to `cellCost(x, z) > 0`; + * supply it when it is cheaper than the full cost (e.g. skips road lookups). + */ + passable?(x: number, z: number): boolean; +} + +export interface NavGridOptions { + /** Metres per cell. Default 6. */ + cell?: number; + /** Cells per tile side; a power of two. Default 64. */ + tileSize?: number; + /** Maximum resident tiles before LRU eviction (at least 4). Default 512. */ + tileCap?: number; + /** A* search window side in cells. Default 256. */ + window?: number; + /** Initial per-step search budget. Default 8. */ + budget?: number; + /** `lineClear` rejects cells costlier than this. Default 2. */ + easyCost?: number; + /** Metres of slack around thick blockers (thin ones get half a cell). Default 1. */ + blockerSlack?: number; + /** `nearestWalkable` ring search radius in cells. Default 12. */ + nearestRadius?: number; +} + +export interface NavPoint { + x: number; + z: number; +} + +export interface NavCell { + i: number; + j: number; +} + +interface NavTile { + ti: number; + tj: number; + cost: Float32Array; + base: Float32Array; + use: number; +} + +interface Blocker { + cells: Int32Array; + tiles: number[]; +} + +/** Unit directions for the coarse nearestWalkable probe. */ +const PROBE_DIRS: ReadonlyArray = Array.from({ length: 8 }, (_, k) => { + const a = (k / 8) * Math.PI * 2; + return [Math.cos(a), Math.sin(a)] as const; +}); + +export class NavGrid { + /** Metres per cell. */ + readonly cell: number; + /** Cells per world side. */ + readonly n: number; + /** World side length in metres. */ + readonly size: number; + /** A* searches allowed in the current step; reset it each step. Refused callers retry later. */ + budget: number; + /** True when the last findPath was refused for lack of budget. */ + refused = false; + /** Blocker generation; bumps on every add/remove so cached routes can invalidate. */ + revision = 0; + /** True when the last findPath reached its goal (false for partial paths). */ + lastPathComplete = false; + /** Nodes expanded by the last findPath (diagnostics). */ + lastExpanded = 0; + /** Tile builds so far (diagnostics). */ + builds = 0; + + private readonly field: NavCostField; + private readonly half: number; + private readonly tn: number; + private readonly shift: number; + private readonly mask: number; + private readonly tileCap: number; + private readonly win: number; + private readonly margin: number; + private readonly easyCost: number; + private readonly slack: number; + private readonly nearestRadius: number; + + private tiles = new Map(); + private pool: NavTile[] = []; + private useTick = 0; + private last: NavTile | null = null; + private tileBlockers = new Map>(); + private blockers = new Map(); + + // Windowed A* scratch, indexed by window-local cell; `sstamp`/`sclosed` hold the generation + // that last wrote an entry, so nothing is cleared between searches. + private sg: Float32Array; + private sparent: Int32Array; + private sstamp: Uint32Array; + private sclosed: Uint32Array; + private gen = 0; + private heap = new NavHeap(); + private trail = new Int32Array(256); + + constructor(field: NavCostField, options: NavGridOptions = {}) { + const tn = options.tileSize ?? 64; + if (!Number.isInteger(tn) || tn < 1 || (tn & (tn - 1)) !== 0) { + throw new Error(`NavGrid tileSize must be a power of two, got ${tn}`); + } + this.field = field; + this.size = field.size; + this.half = field.size / 2; + this.cell = options.cell ?? 6; + this.n = Math.ceil(this.size / this.cell); + this.tn = tn; + this.shift = Math.log2(tn); + this.mask = tn - 1; + this.tileCap = Math.max(4, options.tileCap ?? 512); + this.win = options.window ?? 256; + this.margin = this.win >> 3; + this.budget = options.budget ?? 8; + this.easyCost = options.easyCost ?? 2; + this.slack = options.blockerSlack ?? 1; + this.nearestRadius = options.nearestRadius ?? 12; + const area = this.win * this.win; + this.sg = new Float32Array(area); + this.sparent = new Int32Array(area); + this.sstamp = new Uint32Array(area); + this.sclosed = new Uint32Array(area); + } + + // ---- tiles ---- + + private tileKey(ti: number, tj: number): number { + return ti * 65536 + tj; + } + + private tile(ti: number, tj: number): NavTile { + const last = this.last; + if (last && last.ti === ti && last.tj === tj) return last; + const key = this.tileKey(ti, tj); + let t = this.tiles.get(key); + if (!t) { + if (this.tiles.size >= this.tileCap) this.evict(); + t = this.buildTile(ti, tj); + this.tiles.set(key, t); + } + t.use = ++this.useTick; + this.last = t; + return t; + } + + private buildTile(ti: number, tj: number): NavTile { + const tn = this.tn, n = this.n, shift = this.shift; + const t = this.pool.pop() ?? { ti, tj, cost: new Float32Array(tn * tn), base: new Float32Array(tn * tn), use: 0 }; + t.ti = ti; + t.tj = tj; + t.use = 0; + // NaN means unsampled; zero is a real obstruction. Cells past the world edge are blocked. + const base = t.base; + base.fill(NaN); + const i0 = ti * tn, j0 = tj * tn; + if (i0 + tn > n || j0 + tn > n) { + for (let j = 0; j < tn; j++) for (let i = 0; i < tn; i++) if (i0 + i >= n || j0 + j >= n) base[j * tn + i] = 0; + } + const cost = t.cost; + cost.set(base); + const ids = this.tileBlockers.get(this.tileKey(ti, tj)); + if (ids) { + for (const id of ids) { + const b = this.blockers.get(id); + if (!b) continue; + for (const k of b.cells) { + const i = k % n, j = (k / n) | 0; + if (i >> shift === ti && j >> shift === tj) cost[this.local(i, j)] = 0; + } + } + } + this.builds++; + return t; + } + + private evict(): void { + const arr = [...this.tiles.entries()].sort((a, b) => a[1].use - b[1].use); + for (let i = 0, count = Math.max(1, arr.length >> 2); i < count; i++) { + this.tiles.delete(arr[i][0]); + this.pool.push(arr[i][1]); + } + if (this.last && this.tiles.get(this.tileKey(this.last.ti, this.last.tj)) !== this.last) this.last = null; + } + + /** Tile-local index of a global cell. */ + private local(i: number, j: number): number { + return (j & this.mask) * this.tn + (i & this.mask); + } + + private tileCost(t: NavTile, i: number, j: number): number { + const index = this.local(i, j), cached = t.cost[index]; + if (cached === cached) return cached; // not NaN + let cost = this.field.cellCost(-this.half + (i + 0.5) * this.cell, -this.half + (j + 0.5) * this.cell); + if (!(cost > 0)) cost = 0; + // A blocker zeroes `cost` before this is reached. Removing a blocker from an unsampled cell + // restores its NaN base, so the field is sampled on the next read. + t.base[index] = cost; + t.cost[index] = cost; + return t.cost[index]; + } + + private costIJ(i: number, j: number): number { + return this.tileCost(this.tile(i >> this.shift, j >> this.shift), i, j); + } + + private blockedByBlocker(tileKey: number, k: number): boolean { + const ids = this.tileBlockers.get(tileKey); + if (ids) for (const id of ids) if (this.blockers.get(id)?.cells.includes(k)) return true; + return false; + } + + private terrainAt(x: number, z: number): boolean { + return this.field.passable ? this.field.passable(x, z) : this.field.cellCost(x, z) > 0; + } + + /** Resident tile count (diagnostics). */ + get resident(): number { + return this.tiles.size; + } + + // ---- cells ---- + + private ci(x: number): number { + return Math.max(0, Math.min(this.n - 1, Math.floor((x + this.half) / this.cell))); + } + + private cj(z: number): number { + return Math.max(0, Math.min(this.n - 1, Math.floor((z + this.half) / this.cell))); + } + + /** Cell containing a world point, clamped to the grid. */ + toCell(x: number, z: number): NavCell { + return { i: this.ci(x), j: this.cj(z) }; + } + + /** World-space centre of a cell. */ + cellCenter(i: number, j: number): NavPoint { + return { x: -this.half + (i + 0.5) * this.cell, z: -this.half + (j + 0.5) * this.cell }; + } + + inBounds(x: number, z: number): boolean { + return Number.isFinite(x) && Number.isFinite(z) && Math.abs(x) < this.half && Math.abs(z) < this.half; + } + + /** Current cost of a cell (0 = blocked), building its tile if needed. */ + cost(i: number, j: number): number { + if (i < 0 || j < 0 || i >= this.n || j >= this.n) return 0; + return this.costIJ(i, j); + } + + // ---- queries ---- + + /** + * Is the cell open? `coarse` answers from `field.coarseWalkable` alone (no blockers, no tile + * builds): use it for places nobody can see, where exact answers would build tiles world-wide. + */ + walkable(x: number, z: number, coarse = false): boolean { + if (!this.inBounds(x, z)) return false; + if (coarse) return this.field.coarseWalkable ? this.field.coarseWalkable(x, z) : this.walkableFast(x, z); + return this.costIJ(this.ci(x), this.cj(z)) > 0; + } + + /** Terrain passability of the containing cell, ignoring blockers. */ + terrainPassable(x: number, z: number): boolean { + if (!this.inBounds(x, z)) return false; + const i = this.ci(x), j = this.cj(z); + return this.terrainAt(-this.half + (i + 0.5) * this.cell, -this.half + (j + 0.5) * this.cell); + } + + /** Exact single-cell test that never builds a tile; for offscreen sweeps. Agrees with `walkable`. */ + walkableFast(x: number, z: number): boolean { + if (!this.inBounds(x, z)) return false; + const i = this.ci(x), j = this.cj(z); + const key = this.tileKey(i >> this.shift, j >> this.shift), cached = this.tiles.get(key); + if (cached) return this.tileCost(cached, i, j) > 0; + if (this.blockedByBlocker(key, j * this.n + i)) return false; + return this.terrainAt(-this.half + (i + 0.5) * this.cell, -this.half + (j + 0.5) * this.cell); + } + + /** True when movement between the points crosses no blocked cell (and cuts no blocked corner). */ + segmentWalkable(ax: number, az: number, bx: number, bz: number, fast = false): boolean { + if (!this.inBounds(ax, az) || !this.inBounds(bx, bz)) return false; + const steps = Math.max(1, Math.ceil(Math.hypot(bx - ax, bz - az) / (this.cell * 0.25))); + let pi = this.ci(ax), pj = this.cj(az); + for (let s = 0; s <= steps; s++) { + const x = ax + ((bx - ax) * s) / steps, z = az + ((bz - az) * s) / steps; + if (!(fast ? this.walkableFast(x, z) : this.walkable(x, z))) return false; + const i = this.ci(x), j = this.cj(z); + if (i !== pi && j !== pj) { + const ax2 = -this.half + (i + 0.5) * this.cell, az2 = -this.half + (pj + 0.5) * this.cell; + const bx2 = -this.half + (pi + 0.5) * this.cell, bz2 = -this.half + (j + 0.5) * this.cell; + const open = fast + ? this.walkableFast(ax2, az2) && this.walkableFast(bx2, bz2) + : this.walkable(ax2, az2) && this.walkable(bx2, bz2); + if (!open) return false; + } + pi = i; + pj = j; + } + return true; + } + + /** Build every tile within `r` metres of a point up front. */ + prewarm(x: number, z: number, r: number): void { + const s = this.tn * this.cell; + for (let zz = z - r; zz <= z + r + s; zz += s) for (let xx = x - r; xx <= x + r + s; xx += s) this.walkable(xx, zz); + } + + /** Nearest walkable cell centre to a point (the point itself when already walkable). */ + nearestWalkable(x: number, z: number, coarse = false): NavPoint { + if (coarse) { + if (this.walkable(x, z, true)) return { x, z }; + for (let r = this.cell * 2; r <= this.cell * 16; r *= 2) { + for (const [dx, dz] of PROBE_DIRS) { + const px = x + dx * r, pz = z + dz * r; + if (this.walkable(px, pz, true)) return { x: px, z: pz }; + } + } + return { x, z }; + } + const i = this.ci(x), j = this.cj(z), n = this.n; + if (this.costIJ(i, j) > 0) return { x, z }; + for (let r = 1; r < this.nearestRadius; r++) { + for (let dj = -r; dj <= r; dj++) { + for (let di = -r; di <= r; di++) { + if (Math.max(Math.abs(di), Math.abs(dj)) !== r) continue; + const ii = i + di, jj = j + dj; + if (ii < 0 || jj < 0 || ii >= n || jj >= n) continue; + if (this.costIJ(ii, jj) > 0) return this.cellCenter(ii, jj); + } + } + } + return { x, z }; + } + + /** True when a straight walk between the points crosses only open cells no costlier than `easyCost`. */ + lineClear(x0: number, z0: number, x1: number, z1: number): boolean { + if (!this.inBounds(x0, z0) || !this.inBounds(x1, z1)) return false; + const steps = Math.max(1, Math.ceil(Math.hypot(x1 - x0, z1 - z0) / (this.cell * 0.5))); + for (let s = 0; s <= steps; s++) { + const t = s / steps; + const c = this.costIJ(this.ci(x0 + (x1 - x0) * t), this.cj(z0 + (z1 - z0) * t)); + if (c <= 0 || c > this.easyCost) return false; + } + return true; + } + + // ---- blockers ---- + + /** + * Register a rectangular blocker of footprint `w` × `d` metres centred on (cx, cz), rotated + * `rotY` radians. Thin blockers (walls) get half a cell of slack so a wall between cell centres + * still blocks; thick ones (houses) only `blockerSlack`, so their doors stay reachable. + * Re-adding an existing id replaces it. + */ + addBlocker(id: string, cx: number, cz: number, w: number, d: number, rotY: number): void { + if (this.blockers.has(id)) this.removeBlocker(id); + this.revision++; + const cells: number[] = []; + const hw = w / 2, hd = d / 2; + const cos = Math.cos(rotY), sin = Math.sin(rotY); + const sx = w < this.cell ? this.cell * 0.5 : this.slack, sz = d < this.cell ? this.cell * 0.5 : this.slack; + const r = Math.hypot(hw + sx, hd + sz); + const i0 = this.ci(cx - r), j0 = this.cj(cz - r), i1 = this.ci(cx + r), j1 = this.cj(cz + r); + for (let j = j0; j <= j1; j++) { + for (let i = i0; i <= i1; i++) { + const x = -this.half + (i + 0.5) * this.cell, z = -this.half + (j + 0.5) * this.cell; + const lx = (x - cx) * cos - (z - cz) * sin, lz = (x - cx) * sin + (z - cz) * cos; + if (Math.abs(lx) <= hw + sx && Math.abs(lz) <= hd + sz) cells.push(j * this.n + i); + } + } + const tiles: number[] = []; + for (let tj = j0 >> this.shift; tj <= j1 >> this.shift; tj++) { + for (let ti = i0 >> this.shift; ti <= i1 >> this.shift; ti++) { + const key = this.tileKey(ti, tj); + tiles.push(key); + let set = this.tileBlockers.get(key); + if (!set) this.tileBlockers.set(key, (set = new Set())); + set.add(id); + } + } + this.blockers.set(id, { cells: Int32Array.from(cells), tiles }); + for (const k of cells) { + const i = k % this.n, j = (k / this.n) | 0; + const t = this.tiles.get(this.tileKey(i >> this.shift, j >> this.shift)); + if (t) t.cost[this.local(i, j)] = 0; + } + } + + removeBlocker(id: string): void { + const b = this.blockers.get(id); + if (!b) return; + this.blockers.delete(id); + this.revision++; + for (const key of b.tiles) { + const set = this.tileBlockers.get(key); + if (set) { + set.delete(id); + if (!set.size) this.tileBlockers.delete(key); + } + } + for (const k of b.cells) { + const i = k % this.n, j = (k / this.n) | 0; + const key = this.tileKey(i >> this.shift, j >> this.shift), t = this.tiles.get(key); + if (!t) continue; + const l = this.local(i, j); + t.cost[l] = this.blockedByBlocker(key, k) ? 0 : t.base[l]; + } + } + + hasBlocker(id: string): boolean { + return this.blockers.has(id); + } + + // ---- A* ---- + + /** + * A* with 8-connectivity and no corner cutting. Returns string-pulled world-space waypoints + * (excluding the start; `Vector2.y` is world z), or null when out of bounds, refused, the goal is + * blocked, or no progress is possible. Check `lastPathComplete` before treating the last + * waypoint as the goal. `maxNodes` defaults to a budget that grows with distance. + */ + findPath(sx: number, sz: number, tx: number, tz: number, maxNodes?: number): Vector2[] | null { + this.refused = false; + this.lastPathComplete = false; + this.lastExpanded = 0; + if (!this.inBounds(sx, sz) || !this.inBounds(tx, tz)) return null; + if (this.budget <= 0) { + this.refused = true; + return null; + } + this.budget--; + // Search budget grows with distance so short hops fail fast when boxed in. + if (maxNodes === undefined) { + const dc = Math.hypot(tx - sx, tz - sz) / this.cell; + maxNodes = Math.max(2500, Math.min(24000, Math.round(dc * dc * 0.6 + dc * 60))); + } + const target = this.nearestWalkable(tx, tz); + tx = target.x; + tz = target.z; + if (this.lineClear(sx, sz, tx, tz) && this.segmentWalkable(sx, sz, tx, tz)) { + this.lastPathComplete = true; + return [new Vector2(tx, tz)]; + } + const n = this.n, W = this.win, M = this.margin, shift = this.shift; + const si = this.ci(sx), sj = this.cj(sz), gi = this.ci(tx), gj = this.cj(tz); + if (this.costIJ(gi, gj) <= 0) return null; + // Window: the start/goal bounding box padded for detours, clamped to W cells and the world. + // A request longer than the window keeps the start inside and reaches toward the goal. + const bi0 = Math.min(si, gi), bi1 = Math.max(si, gi), bj0 = Math.min(sj, gj), bj1 = Math.max(sj, gj); + let wi0 = bi1 - bi0 < W - 2 * M ? Math.round((bi0 + bi1) / 2 - W / 2) : gi > si ? si - M : si + M - W; + let wj0 = bj1 - bj0 < W - 2 * M ? Math.round((bj0 + bj1) / 2 - W / 2) : gj > sj ? sj - M : sj + M - W; + wi0 = Math.max(0, Math.min(n - W, wi0)); + wj0 = Math.max(0, Math.min(n - W, wj0)); + const wi1 = Math.min(n, wi0 + W), wj1 = Math.min(n, wj0 + W); + const sIdx = sj * n + si, gIdx = gj * n + gi; + + const gen = ++this.gen; + if (gen === 0xffffffff) { + this.sstamp.fill(0); + this.sclosed.fill(0); + this.gen = 1; + } + const stamp = this.gen; + const sg = this.sg, sparent = this.sparent, sstamp = this.sstamp, sclosed = this.sclosed; + const heap = this.heap; + heap.clear(); + const sL = (sj - wj0) * W + (si - wi0); + sg[sL] = 0; + sstamp[sL] = stamp; + sparent[sL] = -1; + heap.push(sIdx, this.h(sIdx, gIdx)); + let expanded = 0; + let best = sIdx, bestH = Infinity; + while (heap.size > 0) { + const cur = heap.pop(); + const ci = cur % n, cj = (cur / n) | 0; + const cL = (cj - wj0) * W + (ci - wi0); + if (sclosed[cL] === stamp) continue; + sclosed[cL] = stamp; + if (cur === gIdx) { + best = cur; + break; + } + if (++expanded > maxNodes) break; + const hh = this.h(cur, gIdx); + if (hh < bestH) { + bestH = hh; + best = cur; + } + const tile = this.tile(ci >> shift, cj >> shift); + for (let dj = -1; dj <= 1; dj++) { + for (let di = -1; di <= 1; di++) { + if (!di && !dj) continue; + const ni = ci + di, nj = cj + dj; + if (ni < wi0 || nj < wj0 || ni >= wi1 || nj >= wj1) continue; + const nL = (nj - wj0) * W + (ni - wi0); + if (sclosed[nL] === stamp) continue; + // Same tile in the common case; fall back to the lookup at tile borders. + const c = ni >> shift === tile.ti && nj >> shift === tile.tj ? this.tileCost(tile, ni, nj) : this.costIJ(ni, nj); + if (c <= 0) continue; + if (di && dj && (this.costIJ(ni, cj) <= 0 || this.costIJ(ci, nj) <= 0)) continue; // no corner cutting + const ng = sg[cL] + (di && dj ? 1.4142 : 1) * c; + if (sstamp[nL] !== stamp || ng < sg[nL]) { + sg[nL] = ng; + sstamp[nL] = stamp; + sparent[nL] = cur; + const nIdx = nj * n + ni; + heap.push(nIdx, ng + this.h(nIdx, gIdx)); + } + } + } + } + this.lastExpanded = expanded; + const complete = best === gIdx; + this.lastPathComplete = complete; + if (best === sIdx && !complete) return null; + + // Reconstruct into the persistent trail: trail[count - 1] is the start cell, trail[0] is `best`. + let count = 0; + for (let c = best; c !== -1; c = sparent[(((c / n) | 0) - wj0) * W + ((c % n) - wi0)]) { + if (count === this.trail.length) { + const grown = new Int32Array(this.trail.length * 2); + grown.set(this.trail); + this.trail = grown; + } + this.trail[count++] = c; + } + const trail = this.trail; + const px = (k: number) => (complete && k === 0 ? tx : -this.half + ((trail[k] % n) + 0.5) * this.cell); + const pz = (k: number) => (complete && k === 0 ? tz : -this.half + (((trail[k] / n) | 0) + 0.5) * this.cell); + + // String pulling: from each anchor, jump to the farthest waypoint reachable in a straight, easy line. + const out: Vector2[] = []; + let ax = sx, az = sz; + let k = count - 1; + while (k >= 0) { + let far = k; + for (let m = 0; m < k; m++) { + const mx = px(m), mz = pz(m); + if (this.lineClear(ax, az, mx, mz) && this.segmentWalkable(ax, az, mx, mz)) { + far = m; + break; + } + } + ax = px(far); + az = pz(far); + out.push(new Vector2(ax, az)); + k = far - 1; + } + return out; + } + + /** Octile distance heuristic between two global cell indices. */ + private h(a: number, b: number): number { + const n = this.n; + const dx = Math.abs((a % n) - (b % n)), dz = Math.abs(((a / n) | 0) - ((b / n) | 0)); + return Math.max(dx, dz) + 0.4142 * Math.min(dx, dz); + } +} + +/** Binary min-heap of cell indices keyed by f-score, on growable typed arrays. */ +class NavHeap { + private items = new Int32Array(1024); + private keys = new Float64Array(1024); + size = 0; + + clear(): void { + this.size = 0; + } + + push(v: number, k: number): void { + if (this.size === this.items.length) { + const items = new Int32Array(this.size * 2), keys = new Float64Array(this.size * 2); + items.set(this.items); + keys.set(this.keys); + this.items = items; + this.keys = keys; + } + const items = this.items, keys = this.keys; + let i = this.size++; + while (i > 0) { + const p = (i - 1) >> 1; + if (keys[p] <= k) break; + items[i] = items[p]; + keys[i] = keys[p]; + i = p; + } + items[i] = v; + keys[i] = k; + } + + pop(): number { + const items = this.items, keys = this.keys; + const top = items[0]; + const n = --this.size; + if (n > 0) { + const v = items[n], k = keys[n]; + let i = 0; + for (;;) { + const l = 2 * i + 1, r = l + 1; + let m = -1, mk = k; + if (l < n && keys[l] < mk) { + m = l; + mk = keys[l]; + } + if (r < n && keys[r] < mk) m = r; + if (m < 0) break; + items[i] = items[m]; + keys[i] = keys[m]; + i = m; + } + items[i] = v; + keys[i] = k; + } + return top; + } +} diff --git a/src/nav/spatial-hash.ts b/src/nav/spatial-hash.ts new file mode 100644 index 0000000..4f5c89a --- /dev/null +++ b/src/nav/spatial-hash.ts @@ -0,0 +1,65 @@ +/** + * Renderoni Spatial Hash + * + * Uniform grid over item positions on the x/z plane, rebuilt once per step; makes radius + * queries O(k). Items only need a `position` with `x` and `z` (a THREE.Vector3 works). + * Iteration order is the rebuild order within a cell and row-major (z, then x) across cells, + * so queries are deterministic for a deterministic rebuild order. + */ + +export interface Positioned { + position: { x: number; z: number }; +} + +export class SpatialHash { + private cells = new Map(); + private spare: T[][] = []; + + constructor(public readonly cell = 16) {} + + private key(cx: number, cz: number): number { + return (cx + 32768) * 65536 + (cz + 32768); + } + + /** Replace the contents with `items`. Bucket arrays are reused across rebuilds. */ + rebuild(items: Iterable): void { + for (const arr of this.cells.values()) { + arr.length = 0; + this.spare.push(arr); + } + this.cells.clear(); + for (const item of items) { + const k = this.key(Math.floor(item.position.x / this.cell), Math.floor(item.position.z / this.cell)); + let arr = this.cells.get(k); + if (!arr) { + arr = this.spare.pop() ?? []; + this.cells.set(k, arr); + } + arr.push(item); + } + } + + /** Visit every item in the cells overlapping the square of half-side `r` around (x, z); may include items slightly beyond `r`. */ + each(x: number, z: number, r: number, fn: (item: T) => void): void { + const c0x = Math.floor((x - r) / this.cell), c1x = Math.floor((x + r) / this.cell); + const c0z = Math.floor((z - r) / this.cell), c1z = Math.floor((z + r) / this.cell); + for (let cz = c0z; cz <= c1z; cz++) { + for (let cx = c0x; cx <= c1x; cx++) { + const arr = this.cells.get(this.key(cx, cz)); + if (!arr) continue; + for (const item of arr) fn(item); + } + } + } + + /** Collect the items within exactly `r` of (x, z) into `out` (cleared first) and return it. */ + within(x: number, z: number, r: number, out: T[] = []): T[] { + out.length = 0; + const r2 = r * r; + this.each(x, z, r, (item) => { + const dx = item.position.x - x, dz = item.position.z - z; + if (dx * dx + dz * dz <= r2) out.push(item); + }); + return out; + } +} diff --git a/tests/nav.test.ts b/tests/nav.test.ts new file mode 100644 index 0000000..9538037 --- /dev/null +++ b/tests/nav.test.ts @@ -0,0 +1,300 @@ +import { describe, expect, it } from 'vitest'; +import { readFileSync } from 'node:fs'; +import { fileURLToPath } from 'node:url'; +import { dirname, resolve } from 'node:path'; +import { ExpeditionRouter, NavGrid, SpatialHash, type NavCostField } from '../src/nav/index.js'; + +const here = dirname(fileURLToPath(import.meta.url)); + +function field(size: number, cellCost: (x: number, z: number) => number = () => 1): NavCostField { + return { size, cellCost }; +} + +/** Every leg from the start through the waypoints is walkable. */ +function legsWalkable(nav: NavGrid, sx: number, sz: number, path: { x: number; y: number }[]): boolean { + let ax = sx, az = sz; + for (const p of path) { + if (!nav.segmentWalkable(ax, az, p.x, p.y)) return false; + ax = p.x; + az = p.y; + } + return true; +} + +describe('NavGrid', () => { + it('is published as the renderoni/nav subpath', () => { + const pkg = JSON.parse(readFileSync(resolve(here, '../package.json'), 'utf-8')); + expect(pkg.exports['./nav']).toEqual({ types: './dist/nav/index.d.ts', import: './dist/nav/index.js' }); + }); + + it('maps points to cells and back', () => { + const nav = new NavGrid(field(600)); + expect(nav.n).toBe(100); + expect(nav.toCell(-300, -300)).toEqual({ i: 0, j: 0 }); + expect(nav.toCell(299.9, 1e9)).toEqual({ i: 99, j: 99 }); + expect(nav.cellCenter(50, 50)).toEqual({ x: 3, z: 3 }); + expect(nav.inBounds(300, 0)).toBe(false); + expect(nav.inBounds(Number.NaN, 0)).toBe(false); + }); + + it('paths around a blocker', () => { + const nav = new NavGrid(field(600)); + nav.addBlocker('wall', 0, 0, 4, 120, 0); // thin north-south wall + expect(nav.walkable(0, 0)).toBe(false); + const path = nav.findPath(-60, 0, 60, 0)!; + expect(path).not.toBeNull(); + expect(nav.lastPathComplete).toBe(true); + expect(path.length).toBeGreaterThan(1); + const end = path[path.length - 1]; + expect(end.x).toBeCloseTo(60); + expect(end.y).toBeCloseTo(0); + expect(legsWalkable(nav, -60, 0, path)).toBe(true); + expect(path.some((p) => Math.abs(p.y) >= 60)).toBe(true); + }); + + it('returns a straight single waypoint when the line is clear', () => { + const nav = new NavGrid(field(600)); + const path = nav.findPath(-60, 0, 60, 10)!; + expect(path.map((p) => [p.x, p.y])).toEqual([[60, 10]]); + expect(nav.lastPathComplete).toBe(true); + }); + + it('returns the best partial path with lastPathComplete=false for an unreachable goal', () => { + // A ring of blocked terrain around (150, 0) with an open interior. + const nav = new NavGrid(field(600, (x, z) => { + const d = Math.hypot(x - 150, z); + return d > 30 && d < 45 ? 0 : 1; + })); + const path = nav.findPath(-200, 0, 150, 0)!; + expect(path).not.toBeNull(); + expect(nav.lastPathComplete).toBe(false); + const end = path[path.length - 1]; + expect(Math.hypot(end.x - 150, end.y)).toBeLessThan(50); + expect(legsWalkable(nav, -200, 0, path)).toBe(true); + }); + + it('keeps long requests inside the search window and returns a partial path', () => { + // Cost 3 everywhere: never "easy", so lineClear fails and A* must run. + const nav = new NavGrid(field(6000, () => 3)); + const path = nav.findPath(-2800, 0, 2800, 0)!; + expect(path).not.toBeNull(); + expect(nav.lastPathComplete).toBe(false); + const end = path[path.length - 1]; + expect(end.x).toBeGreaterThan(-2800 + 500); // real progress toward the goal + expect(end.x).toBeLessThan(-3000 + (33 - 32 + 256) * 6); // window: start cell 33, margin 32 + expect(nav.resident).toBeLessThan(40); + // Unlimited nodes: the search fills the window and stops at its far edge. + const edge = nav.findPath(-2800, 0, 2800, 0, 1e7)!; + expect(nav.lastPathComplete).toBe(false); + expect(edge[edge.length - 1].x).toBeGreaterThan(-3000 + 250 * 6); + expect(edge[edge.length - 1].x).toBeLessThan(-3000 + 257 * 6); + }); + + it('refuses searches beyond the per-step budget', () => { + const nav = new NavGrid(field(600), { budget: 1 }); + expect(nav.findPath(-60, 0, 60, 0)).not.toBeNull(); + expect(nav.refused).toBe(false); + expect(nav.budget).toBe(0); + expect(nav.findPath(-60, 0, 60, 0)).toBeNull(); + expect(nav.refused).toBe(true); + nav.budget = 1; + expect(nav.findPath(-60, 0, 60, 0)).not.toBeNull(); + expect(nav.refused).toBe(false); + }); + + it('restores costs when blockers are removed, including overlaps', () => { + const nav = new NavGrid(field(600, (x) => (x > 100 ? 2 : 1))); + const r0 = nav.revision; + nav.addBlocker('a', 0, 0, 20, 20, 0); + nav.addBlocker('b', 10, 0, 20, 20, 0); + expect(nav.revision).toBe(r0 + 2); + expect(nav.walkable(5, 0)).toBe(false); + nav.removeBlocker('a'); + expect(nav.walkable(-8, 0)).toBe(true); + expect(nav.walkable(5, 0)).toBe(false); // still covered by b + nav.removeBlocker('b'); + expect(nav.walkable(5, 0)).toBe(true); + const { i, j } = nav.toCell(5, 0); + expect(nav.cost(i, j)).toBe(1); + expect(nav.revision).toBe(r0 + 4); + nav.removeBlocker('missing'); + expect(nav.revision).toBe(r0 + 4); + }); + + it('keeps blockers across tile eviction and rebuild', () => { + const nav = new NavGrid(field(6000), { tileCap: 4 }); + nav.addBlocker('house', 0, 0, 20, 20, 0); // tile not built yet + expect(nav.walkable(0, 0)).toBe(false); + // Touch many other tiles so the blocker's tile is evicted. + for (let k = 0; k < 12; k++) nav.walkable(-2900 + k * 400, 2900); + expect(nav.resident).toBeLessThanOrEqual(4); + const builds = nav.builds; + expect(nav.walkable(0, 0)).toBe(false); // rebuilt with the blocker applied + expect(nav.builds).toBe(builds + 1); + // Remove while evicted, then rebuild: open again. + for (let k = 0; k < 12; k++) nav.walkable(-2900 + k * 400, -2900); + nav.removeBlocker('house'); + expect(nav.walkable(0, 0)).toBe(true); + expect(nav.walkableFast(0, 0)).toBe(true); + }); + + it('finds the nearest walkable cell', () => { + const nav = new NavGrid(field(600)); + expect(nav.nearestWalkable(1, 2)).toEqual({ x: 1, z: 2 }); + nav.addBlocker('block', 0, 0, 30, 30, 0); + const p = nav.nearestWalkable(0, 0); + expect(nav.walkable(p.x, p.z)).toBe(true); + expect(Math.hypot(p.x, p.z)).toBeLessThan(40); + // coarse answers from coarseWalkable without building tiles + const coarse = new NavGrid({ size: 600, cellCost: () => 1, coarseWalkable: (x) => x > 20 }); + const q = coarse.nearestWalkable(0, 0, true); + expect(q.x).toBeGreaterThan(20); + expect(coarse.resident).toBe(0); + }); + + it('checks lines and segments', () => { + const nav = new NavGrid(field(600, (x, z) => (x > 50 && x < 60 && z > 0 ? 5 : 1))); + expect(nav.lineClear(-100, 0, 100, -10)).toBe(true); + expect(nav.lineClear(-100, 20, 100, 20)).toBe(false); // crosses the costly strip + expect(nav.segmentWalkable(-100, 20, 100, 20)).toBe(true); // costly is still walkable + nav.addBlocker('wall', 0, 0, 2, 400, 0); + expect(nav.segmentWalkable(-10, 0, 10, 0)).toBe(false); + expect(nav.segmentWalkable(-10, 0, 10, 0, true)).toBe(false); + expect(nav.lineClear(-10, 0, 10, 0)).toBe(false); + expect(nav.segmentWalkable(-10, 0, -10, 50)).toBe(true); + expect(nav.segmentWalkable(-10, 0, 400, 0)).toBe(false); // out of bounds + }); + + it('rejects diagonal moves that cut a blocked corner', () => { + const nav = new NavGrid(field(60, (x, z) => (x > 0 && z < 0) || (x < 0 && z > 0) ? 0 : 1), { cell: 6 }); + expect(nav.segmentWalkable(-3, -3, 3, 3)).toBe(false); + }); + + it('walkableFast agrees with walkable, before and after tiles are built', () => { + const cost = (x: number, z: number) => (Math.sin(x * 0.05) + Math.cos(z * 0.07) > 1.2 ? 0 : 1); + const nav = new NavGrid(field(1200, cost), { tileCap: 8 }); + nav.addBlocker('a', 100, 100, 40, 12, 0.6); + nav.addBlocker('b', -300, 50, 3, 80, 0); + const pts: [number, number][] = []; + for (let z = -590; z < 600; z += 37) for (let x = -590; x < 600; x += 29) pts.push([x, z]); + const cold = pts.map(([x, z]) => nav.walkableFast(x, z)); + expect(nav.resident).toBe(0); + const exact = pts.map(([x, z]) => nav.walkable(x, z)); + const warm = pts.map(([x, z]) => nav.walkableFast(x, z)); + expect(cold).toEqual(exact); + expect(warm).toEqual(exact); + expect(exact.some((w) => !w)).toBe(true); + for (const [x, z] of pts) expect(nav.terrainPassable(x, z)).toBe(cost(nav.cellCenter(nav.toCell(x, z).i, nav.toCell(x, z).j).x, nav.cellCenter(nav.toCell(x, z).i, nav.toCell(x, z).j).z) > 0); + }); + + it('is deterministic: identical queries give identical paths', () => { + const cost = (x: number, z: number) => 1 + Math.abs((Math.floor(x / 18) * 7 + Math.floor(z / 18) * 13) % 5) * 0.4; + const make = () => { + const nav = new NavGrid(field(1200, cost), { budget: 100 }); + nav.addBlocker('w1', 0, 0, 3, 300, 0); + nav.addBlocker('w2', 100, 50, 200, 3, 0.3); + return nav; + }; + const a = make(), b = make(); + const run = (nav: NavGrid) => nav.findPath(-250, -20, 260, 120, 60000)!.map((p) => [p.x, p.y]); + const first = run(a); + expect(first.length).toBeGreaterThan(1); + expect(a.lastPathComplete).toBe(true); + expect(run(a)).toEqual(first); + expect(run(b)).toEqual(first); + }); +}); + +describe('SpatialHash', () => { + const item = (x: number, z: number) => ({ position: { x, z } }); + + it('answers radius queries and rebuilds cleanly', () => { + const hash = new SpatialHash>(16); + const a = item(0, 0), b = item(10, 0), c = item(30, 30), d = item(-100, 5); + hash.rebuild([a, b, c, d]); + expect(hash.within(0, 0, 12)).toEqual([a, b]); + expect(hash.within(0, 0, 50).sort((p, q) => p.position.x - q.position.x)).toEqual([a, b, c]); + const seen: unknown[] = []; + hash.each(0, 0, 1, (o) => seen.push(o)); + expect(seen).toContain(a); + expect(seen).not.toContain(d); + b.position.x = -95; + hash.rebuild([a, b, c, d]); + expect(new Set(hash.within(-100, 5, 10))).toEqual(new Set([b, d])); + expect(hash.within(0, 0, 12)).toEqual([a]); + }); +}); + +describe('ExpeditionRouter', () => { + // A 3 km world split by a north-south river with a single ford near the south edge. + const river = (x: number, z: number) => (Math.abs(x) < 20 && z < 1100 ? 0 : 1); + + function drive(nav: NavGrid, router: ExpeditionRouter, done: () => boolean, maxTicks = 2000): number { + for (let tick = 1; tick <= maxTicks; tick++) { + nav.budget = 14; + router.update(tick); + if (done()) return tick; + } + return -1; + } + + it('joins complete local paths into a corridor around a long obstacle', () => { + const nav = new NavGrid(field(3000, river)); + const router = new ExpeditionRouter(nav); + const ticket = router.request({ x: -1200, z: -1200 }, { x: 1200, z: -1200 }, true); + expect(ticket.state).toBe('planning'); + expect(drive(nav, router, () => ticket.state !== 'planning')).toBeGreaterThan(0); + expect(ticket.state).toBe('ready'); + const end = ticket.points[ticket.points.length - 1]; + expect(Math.hypot(end.x - 1200, end.z + 1200)).toBeLessThan(1); + let ax = -1200, az = -1200; + for (const p of ticket.points) { + expect(nav.segmentWalkable(ax, az, p.x, p.z)).toBe(true); + ax = p.x; + az = p.z; + } + expect(ticket.points.some((p) => p.z > 1100)).toBe(true); + }); + + it('gives priority requests more turns without starving ordinary ones', () => { + const nav = new NavGrid(field(3000, river)); + const router = new ExpeditionRouter(nav); + const ordinary = router.request({ x: -1200, z: -1000 }, { x: 1200, z: -1000 }, false); + const priority = router.request({ x: -1200, z: -1000 }, { x: 1200, z: -1000 }, true); + const readyAt = { ordinary: -1, priority: -1 }; + let tick = 0; + drive(nav, router, () => { + tick++; + if (priority.state === 'ready' && readyAt.priority < 0) readyAt.priority = tick; + if (ordinary.state === 'ready' && readyAt.ordinary < 0) readyAt.ordinary = tick; + return ordinary.state !== 'planning' && priority.state !== 'planning'; + }); + expect(priority.state).toBe('ready'); + expect(ordinary.state).toBe('ready'); + expect(readyAt.priority).toBeLessThan(readyAt.ordinary); + expect(priority.points).toEqual(ordinary.points); + }); + + it('cancels planning requests when blockers change', () => { + const nav = new NavGrid(field(3000, river)); + const router = new ExpeditionRouter(nav); + const ticket = router.request({ x: -1200, z: -1200 }, { x: 1200, z: -1200 }); + nav.budget = 14; + router.update(1); + nav.addBlocker('gate', 0, 1200, 60, 10, 0); + router.update(2); + expect(ticket.state).toBe('cancelled'); + expect(router.pending).toBe(0); + }); + + it('abandons requests whose owner no longer wants them', () => { + const nav = new NavGrid(field(3000, river)); + const router = new ExpeditionRouter(nav); + let wanted = true; + const ticket = router.request({ x: -1200, z: -1200 }, { x: 1200, z: -1200 }, false, () => wanted); + expect(router.pending).toBe(1); + wanted = false; + router.update(1); + expect(ticket.state).toBe('cancelled'); + }); +}); diff --git a/tsup.config.ts b/tsup.config.ts index 4c756fb..8c9bf1e 100644 --- a/tsup.config.ts +++ b/tsup.config.ts @@ -15,6 +15,7 @@ export default defineConfig({ 'testing/matchers': 'src/testing/matchers.ts', 'input/index': 'src/input/index.ts', 'editor/index': 'src/editor/index.ts', + 'nav/index': 'src/nav/index.ts', }, format: ['esm'], dts: true,