From 2e73068af5ffed07122d3ab78711801de9fa31e8 Mon Sep 17 00:00:00 2001 From: araharah-executor Date: Sat, 24 Oct 2026 12:10:46 +0800 Subject: [PATCH] feat(06-01): point-to-edge GPS-fix snapping with memoized grid index (GREEN) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - snapToGraph: bbox+margin guard first (honest null = router-pending, D-03), then 3x3 coarse-grid neighborhood (0.002 deg cells, WeakMap memo per RoadGraph — LAW 1 lazy memoize, O(local) not O(n)) - projection in cos(lat)-scaled degree space (Pitfall 8); returned distance is haversineM; result carries the best segment's nameId (null for the unnamed sentinel) for plan 07's cron street-hop - oneway-reverse edges stay snap-visible (direction is A*'s law); gridBuildCount() instrumentation kept internal to the workspace --- routing/src/snap.ts | 172 +++++++++++++++++++++++++++++++++++++ routing/test/snap.test.mjs | 2 +- 2 files changed, 173 insertions(+), 1 deletion(-) create mode 100644 routing/src/snap.ts diff --git a/routing/src/snap.ts b/routing/src/snap.ts new file mode 100644 index 0000000..65a54b6 --- /dev/null +++ b/routing/src/snap.ts @@ -0,0 +1,172 @@ +/* + * snap.ts — server-side GPS-fix snapping onto the CSR road graph (D-05). + * ------------------------------------------------------------------------ + * Engine A owns snapping — ONE authority for road geometry, so the client + * and the router can never drift. This is the SERVER authority (a reroute + * origin lands here); the CLIENT progress-projection (phase 6) walks the + * selected route polyline instead and never calls this module. + * + * An honest miss resolves NULL: a fix outside the graph bbox (plus + * SNAP_BBOX_MARGIN_DEG) or outside any local grid neighborhood is the + * designed "router-pending" signal (D-03) — never a fabricated node, + * never a throw. + * + * Direction plays no part here: a oneway segment's flagged reverse edge + * (edgeFlags bit0 — see graph.ts) still carries segment geometry and its + * street name, so fixes near a oneway road snap to its endpoints exactly + * like two-way traffic. A* enforces the direction law; snapping ignores it. + * + * Cost: the coarse grid index (~0.002 deg cells) is built ONCE per + * RoadGraph and memoized in a module-scope WeakMap (LAW 1 lazy memoize — + * the fixture-source precedent; per-isolate in a Worker, per-process in + * Node). Each fix then touches only the 3x3 cell neighborhood — O(local), + * never O(nodes) (RESEARCH anti-pattern: global nearest-node scans). + * + * Pitfall 8 law: projection math scales lng by cos(lat) — never raw + * degree deltas; the RETURNED distance is haversineM (geo.ts, the only + * distance authority that leaves this module). + * + * Pure: no I/O, no logging, no storage (T-06-05). Coordinates exist only + * in the caller's frame. + * + * Known limitation (documented, honest): candidate segments come from the + * 3x3 cell neighborhood of the FIX's cell — a segment both of whose + * endpoints sit more than one cell (~0.002 deg) away from the fix's cell + * is not a candidate. KL-metro build data (plan 04) densifies at + * intersections, well inside this window; a miss degrades to null + * (router-pending), never a wrong snap. + */ + +import { UNNAMED_NAME_ID, type RoadGraph } from "./graph.ts"; +import { haversineM } from "./geo.ts"; +import type { RoutePoint } from "./types.ts"; + +/** Fixes beyond bbox+margin are honest misses (router-pending), in degrees. */ +export const SNAP_BBOX_MARGIN_DEG = 0.01; + +/** Coarse grid cell edge, degrees — tuned to urban block spacing. */ +const GRID_CELL_DEG = 0.002; + +const DEG_TO_RAD = Math.PI / 180; +/** Same mean-earth sphere as geo.ts haversineM (6 371 000 m). */ +const M_PER_DEG = (6_371_000 * Math.PI) / 180; + +/** A snap outcome: the graph node a fix resolves to. */ +export interface SnapResult { + /** Node index into the graph's coordinate arrays. */ + node: number; + /** haversineM(fix, snapped node) — meters. */ + distanceM: number; + /** + * The best segment's edgeNameId (edges carry names; nodes don't — plan + * 07's cron street-hop consumes this). null when the edge is unnamed + * (UNNAMED_NAME_ID sentinel). + */ + nameId: number | null; +} + +/* ————— per-graph coarse grid (built once, memoized by WeakMap) ————— */ + +type Grid = Map; + +const grids = new WeakMap(); + +let gridBuilds = 0; + +/** Instrumentation (tests/instruments): how many grids have been built. */ +export function gridBuildCount(): number { + return gridBuilds; +} + +function cellKey(cx: number, cy: number): string { + return `${cx}:${cy}`; +} + +function gridFor(graph: RoadGraph): Grid { + const memoized = grids.get(graph); + if (memoized) return memoized; + const grid: Grid = new Map(); + const inv = 1 / GRID_CELL_DEG; + for (let n = 0; n < graph.nodeCount; n++) { + const cx = Math.floor((graph.lng[n]! - graph.bbox.minLng) * inv); + const cy = Math.floor((graph.lat[n]! - graph.bbox.minLat) * inv); + const key = cellKey(cx, cy); + const bucket = grid.get(key); + if (bucket) bucket.push(n); + else grid.set(key, [n]); + } + gridBuilds += 1; + grids.set(graph, grid); + return grid; +} + +/** + * Snap a raw GPS fix to the nearest graph node by point-to-SEGMENT + * projection over the 3x3 cell neighborhood. Returns the closer endpoint + * of the best segment together with that segment's nameId, or null when + * the fix is an honest miss. + */ +export function snapToGraph(graph: RoadGraph, point: RoutePoint): SnapResult | null { + const margin = SNAP_BBOX_MARGIN_DEG; + const { minLat, minLng, maxLat, maxLng } = graph.bbox; + if ( + point.lat < minLat - margin || + point.lat > maxLat + margin || + point.lng < minLng - margin || + point.lng > maxLng + margin + ) { + return null; // honest miss — router-pending upstream (D-03) + } + const grid = gridFor(graph); + const inv = 1 / GRID_CELL_DEG; + const pcx = Math.floor((point.lng - minLng) * inv); + const pcy = Math.floor((point.lat - minLat) * inv); + // Pitfall 8: scale ALL lng deltas by cos(lat) before any comparison. + const cosLat = Math.cos(point.lat * DEG_TO_RAD); + const px = point.lng * cosLat; + const py = point.lat; + + let bestDist2 = Infinity; + let bestNode = -1; + let bestNameId: number | null = null; + for (let dy = -1; dy <= 1; dy++) { + for (let dx = -1; dx <= 1; dx++) { + const bucket = grid.get(cellKey(pcx + dx, pcy + dy)); + if (bucket === undefined) continue; + for (const u of bucket) { + for (let e = graph.edgeStart[u]!; e < graph.edgeStart[u + 1]!; e++) { + const v = graph.edgeTarget[e]!; + // Point-to-segment projection in cos-scaled degree space. + const ax = graph.lng[u]! * cosLat; + const ay = graph.lat[u]!; + const bx = graph.lng[v]! * cosLat; + const by = graph.lat[v]!; + const abx = bx - ax; + const aby = by - ay; + const len2 = abx * abx + aby * aby; + let t = 0; + if (len2 > 0) { + t = ((px - ax) * abx + (py - ay) * aby) / len2; + t = t < 0 ? 0 : t > 1 ? 1 : t; + } + const qx = ax + t * abx - px; + const qy = ay + t * aby - py; + // Squared meters — ranking only; the reported distance is haversine. + const d2 = (qx * qx + qy * qy) * M_PER_DEG * M_PER_DEG; + if (d2 < bestDist2) { + bestDist2 = d2; + bestNode = t <= 0.5 ? u : v; + const nameId = graph.edgeNameId[e]!; + bestNameId = nameId === UNNAMED_NAME_ID ? null : nameId; + } + } + } + } + } + if (bestNode < 0) return null; // in margin band but no local roads — honest miss + return { + node: bestNode, + distanceM: haversineM(point, { lat: graph.lat[bestNode]!, lng: graph.lng[bestNode]! }), + nameId: bestNameId, + }; +} diff --git a/routing/test/snap.test.mjs b/routing/test/snap.test.mjs index e5e81d5..75634bc 100644 --- a/routing/test/snap.test.mjs +++ b/routing/test/snap.test.mjs @@ -133,7 +133,7 @@ test("S4 bbox margin — outside bbox + SNAP_BBOX_MARGIN_DEG -> null; just outsi // bbox maxLat = 3.152; +0.008 above it is inside the margin AND near row-4 roads. const justOutside = snapToGraph(g, { lat: 3.1528, lng: nodeLng(1) }); assert.ok(justOutside, "a fix just outside the bbox but near roads still snaps"); - assert.ok(justOutside.node >= 10 && justOutside.node <= 14, "snaps to a row-4 node"); + assert.ok(justOutside.node >= 20 && justOutside.node <= 24, "snaps to a row-4 node (ids 20..24)"); // 0.011 deg above the bbox top -> outside bbox + margin -> honest null. assert.equal( snapToGraph(g, { lat: 3.163, lng: nodeLng(1) }),