feat(06-01): point-to-edge GPS-fix snapping with memoized grid index (GREEN)

- 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
2026-10-24 12:10:46 +08:00
parent 72703d26ec
commit 2e73068af5
2 changed files with 173 additions and 1 deletions
@@ -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<string, number[]>;
+
+const grids = new WeakMap<RoadGraph, Grid>();
+
+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,
+  };
+}