feat(06-01): typed-array A* + turn classifier + step derivation + public exports (GREEN)
- routeAstar: allocation-free hot loop — binary heap of node indices over preallocated typed arrays (grown by doubling), typed-array g/closed/ came-from, on-demand cached haversine/maxSpeed heuristic (admissible + consistent); oneway-reverse edges never relaxed; unreachable -> null - A1 proves A* == brute-force Dijkstra on ALL 625 mini-graph pairs - classifyTurn: the one turn authority (compass convention — negative delta = left; plan's (0,40)->slight-left case contradicted its own (0,-90)/(0,90) pins, documented deviation); TURN_* constants exported for plan 07's cron projection - deriveSteps: splits at non-straight bearings or name changes; steps carry kind/name/distanceM + polyline6; depart-first, arrive-last - index.ts: engine-A named exports + ladder note updated — backend #2 is now 'engine-a' (D-01 supersedes the wasm-router note; Valhalla port infeasible + geojson-path-finder rejected, engine B = post-v1 evolution, D-02); createHttpRoutingSource deliberately absent (plan 04)
This commit is contained in:
araharah-executor
@@ -0,0 +1,317 @@
|
||||
/*
|
||||
* astar.ts — typed-array A* over the CSR graph + turn-step derivation (D-01).
|
||||
* ----------------------------------------------------------------------------
|
||||
* The engine-A router core: routeAstar finds the minimum-seconds path
|
||||
* between two graph nodes with an ALLOCATION-FREE hot loop — open set is a
|
||||
* binary heap of node indices over preallocated typed arrays, g-scores and
|
||||
* the closed set are per-node typed arrays. No Map, no JS objects per node
|
||||
* (RESEARCH Pattern 2 + Anti-Patterns; the 10 ms free-plan CPU budget is
|
||||
* the design constraint, A1's equivalence sweep is the correctness gate).
|
||||
*
|
||||
* Heuristic: haversine(node, goal) / (maxSpeedKmh / 3.6) — the aerial
|
||||
* distance divided by the graph's maximum speed can never overestimate the
|
||||
* remaining travel time (every edge's seconds = meters / speed with speed
|
||||
* <= maxSpeedKmh), so it is admissible AND consistent; settling the goal
|
||||
* off the heap is therefore provably optimal (A1 proves it exhaustively on
|
||||
* the fixture). The heuristic is computed on demand and cached per node in
|
||||
* a Float64Array — no object is constructed in the search loop.
|
||||
*
|
||||
* Traversal law (shared with the tests' brute-force Dijkstra): an edge with
|
||||
* edgeFlags bit0 set ("oneway-reverse" — see graph.ts) is NEVER relaxed.
|
||||
* Snapping (snap.ts) ignores the flag; only A* enforces direction.
|
||||
*
|
||||
* classifyTurn is the ONE authority for "what is a turn" — plan 07's cron
|
||||
* projection imports this exact classifier. Convention: compass bearings
|
||||
* increase clockwise (N=0, E=90), so a NEGATIVE bearing delta is a left
|
||||
* turn.
|
||||
*
|
||||
* deriveSteps walks a node path and splits it into maneuver steps at
|
||||
* bearings that leave "straight" or at street-name changes; steps carry
|
||||
* kind/name/distanceM and polyline6 geometry (RouteStep.kind — the field
|
||||
* this phase added to types.ts; fixture-world steps stay empty arrays,
|
||||
* D-10).
|
||||
*
|
||||
* Pure: no I/O, no logging, no storage (T-06-05).
|
||||
*/
|
||||
|
||||
import type { RoadGraph } from "./graph.ts";
|
||||
import { haversineM } from "./geo.ts";
|
||||
import { encodePolyline6 } from "./polyline.ts";
|
||||
import type { RoutePoint, RouteStep } from "./types.ts";
|
||||
|
||||
/** Maneuver kinds — structurally the RouteStep.kind union (one authority). */
|
||||
export type GuidanceTurnKind = NonNullable<RouteStep["kind"]>;
|
||||
|
||||
/** |delta| <= this is "straight" (degrees). */
|
||||
export const TURN_STRAIGHT_MAX_DEG = 25;
|
||||
/** |delta| <= this (and > straight) is a "slight" turn (degrees). */
|
||||
export const TURN_SLIGHT_MAX_DEG = 60;
|
||||
/** |delta| >= this is a "uturn" (degrees). */
|
||||
export const TURN_UTURN_MIN_DEG = 150;
|
||||
|
||||
const DEG_TO_RAD = Math.PI / 180;
|
||||
/** Same mean-earth sphere as geo.ts haversineM (6 371 000 m). */
|
||||
const EARTH_RADIUS_M = 6_371_000;
|
||||
|
||||
export interface AstarResult {
|
||||
/** Node path, origin first, goal last. */
|
||||
nodes: number[];
|
||||
/** Total travel seconds (sum of traversed edgeSeconds). */
|
||||
seconds: number;
|
||||
/** Meters over the node path (haversineM post-hoc — the plan's law). */
|
||||
distanceM: number;
|
||||
}
|
||||
|
||||
/* ————— binary min-heap of node indices over preallocated typed arrays ————— */
|
||||
|
||||
const HEAP_INITIAL_CAPACITY = 1024;
|
||||
|
||||
class NodeHeap {
|
||||
private keys: Float64Array;
|
||||
private nodes: Uint32Array;
|
||||
size = 0;
|
||||
|
||||
constructor() {
|
||||
this.keys = new Float64Array(HEAP_INITIAL_CAPACITY);
|
||||
this.nodes = new Uint32Array(HEAP_INITIAL_CAPACITY);
|
||||
}
|
||||
|
||||
push(key: number, node: number): void {
|
||||
if (this.size === this.keys.length) this.grow();
|
||||
let i = this.size;
|
||||
this.size += 1;
|
||||
while (i > 0) {
|
||||
const parent = (i - 1) >> 1;
|
||||
if (this.keys[parent]! <= key) break;
|
||||
this.keys[i] = this.keys[parent]!;
|
||||
this.nodes[i] = this.nodes[parent]!;
|
||||
i = parent;
|
||||
}
|
||||
this.keys[i] = key;
|
||||
this.nodes[i] = node;
|
||||
}
|
||||
|
||||
/** Returns the min-key node, or -1 when empty. Lazy deletion: stale
|
||||
* entries are skipped by the closed-set check in routeAstar. */
|
||||
pop(): number {
|
||||
if (this.size === 0) return -1;
|
||||
const top = this.nodes[0]!;
|
||||
this.size -= 1;
|
||||
const lastKey = this.keys[this.size]!;
|
||||
const lastNode = this.nodes[this.size]!;
|
||||
let i = 0;
|
||||
for (;;) {
|
||||
const left = 2 * i + 1;
|
||||
if (left >= this.size) break;
|
||||
const right = left + 1;
|
||||
const child = right < this.size && this.keys[right]! < this.keys[left]! ? right : left;
|
||||
if (this.keys[child]! >= lastKey) break;
|
||||
this.keys[i] = this.keys[child]!;
|
||||
this.nodes[i] = this.nodes[child]!;
|
||||
i = child;
|
||||
}
|
||||
if (this.size > 0) {
|
||||
this.keys[i] = lastKey;
|
||||
this.nodes[i] = lastNode;
|
||||
}
|
||||
return top;
|
||||
}
|
||||
|
||||
private grow(): void {
|
||||
const keys = new Float64Array(this.keys.length * 2);
|
||||
keys.set(this.keys);
|
||||
const nodes = new Uint32Array(this.nodes.length * 2);
|
||||
nodes.set(this.nodes);
|
||||
this.keys = keys;
|
||||
this.nodes = nodes;
|
||||
}
|
||||
}
|
||||
|
||||
/* ————— A* ————— */
|
||||
|
||||
/**
|
||||
* Minimum-seconds path origin -> goal over the graph, or null when the pair
|
||||
* is unreachable (the honest miss). Exhaustively proven equal to
|
||||
* brute-force Dijkstra on the committed mini-graph (astar.test.mjs A1).
|
||||
*/
|
||||
export function routeAstar(graph: RoadGraph, originNode: number, goalNode: number): AstarResult | null {
|
||||
const n = graph.nodeCount;
|
||||
if (!Number.isInteger(originNode) || originNode < 0 || originNode >= n) return null;
|
||||
if (!Number.isInteger(goalNode) || goalNode < 0 || goalNode >= n) return null;
|
||||
if (originNode === goalNode) return { nodes: [originNode], seconds: 0, distanceM: 0 };
|
||||
|
||||
const maxSpeedMps = graph.maxSpeedKmh / 3.6;
|
||||
const heuristicScale = maxSpeedMps > 0 ? 1 / maxSpeedMps : 1;
|
||||
const goalLat = graph.lat[goalNode]!;
|
||||
const goalLng = graph.lng[goalNode]!;
|
||||
|
||||
const gScore = new Float64Array(n).fill(Number.POSITIVE_INFINITY);
|
||||
const cameFrom = new Int32Array(n).fill(-1);
|
||||
const closed = new Uint8Array(n);
|
||||
const hCache = new Float64Array(n).fill(Number.NaN); // NaN = not computed yet
|
||||
const heap = new NodeHeap();
|
||||
|
||||
// Inline haversine vs the goal, cached per node — zero allocations, and
|
||||
// only explored nodes ever pay for it. Admissible: aerial distance at
|
||||
// max speed upper-bounds nothing — it LOWER-bounds remaining seconds.
|
||||
const heuristic = (node: number): number => {
|
||||
const cached = hCache[node]!;
|
||||
if (!Number.isNaN(cached)) return cached;
|
||||
const lat1 = graph.lat[node]! * DEG_TO_RAD;
|
||||
const lat2 = goalLat * DEG_TO_RAD;
|
||||
const dLat = lat2 - lat1;
|
||||
const dLng = (goalLng - graph.lng[node]!) * DEG_TO_RAD;
|
||||
const sinLat = Math.sin(dLat / 2);
|
||||
const sinLng = Math.sin(dLng / 2);
|
||||
const h = sinLat * sinLat + Math.cos(lat1) * Math.cos(lat2) * sinLng * sinLng;
|
||||
const meters = 2 * EARTH_RADIUS_M * Math.asin(Math.min(1, Math.sqrt(h)));
|
||||
const seconds = meters * heuristicScale;
|
||||
hCache[node] = seconds;
|
||||
return seconds;
|
||||
};
|
||||
|
||||
gScore[originNode] = 0;
|
||||
heap.push(heuristic(originNode), originNode);
|
||||
|
||||
while (heap.size > 0) {
|
||||
const u = heap.pop();
|
||||
if (u < 0) break;
|
||||
if (closed[u]! !== 0) continue; // lazy deletion
|
||||
closed[u] = 1;
|
||||
if (u === goalNode) {
|
||||
// Reconstruct (allocation here is fine — the hot loop is over).
|
||||
const path: number[] = [u];
|
||||
let prev = cameFrom[u]!;
|
||||
while (prev >= 0) {
|
||||
path.push(prev);
|
||||
prev = cameFrom[prev]!;
|
||||
}
|
||||
path.reverse();
|
||||
let distanceM = 0;
|
||||
for (let i = 1; i < path.length; i++) {
|
||||
distanceM += haversineM(
|
||||
{ lat: graph.lat[path[i - 1]!]!, lng: graph.lng[path[i - 1]!]! },
|
||||
{ lat: graph.lat[path[i]!]!, lng: graph.lng[path[i]!]! },
|
||||
);
|
||||
}
|
||||
return { nodes: path, seconds: gScore[goalNode]!, distanceM };
|
||||
}
|
||||
const base = gScore[u]!;
|
||||
for (let e = graph.edgeStart[u]!; e < graph.edgeStart[u + 1]!; e++) {
|
||||
if ((graph.edgeFlags[e]! & 1) !== 0) continue; // oneway-reverse: never traversed
|
||||
const v = graph.edgeTarget[e]!;
|
||||
if (closed[v]! !== 0) continue;
|
||||
const candidate = base + graph.edgeSeconds[e]!;
|
||||
if (candidate < gScore[v]!) {
|
||||
gScore[v] = candidate;
|
||||
cameFrom[v] = u;
|
||||
heap.push(candidate + heuristic(v), v);
|
||||
}
|
||||
}
|
||||
}
|
||||
return null; // unreachable — honest miss
|
||||
}
|
||||
|
||||
/* ————— turn classification (the one authority; plan 07 imports this) ————— */
|
||||
|
||||
/**
|
||||
* Classify the maneuver between two compass bearings (degrees, 0..360,
|
||||
* clockwise). The delta normalizes to (-180, 180]; NEGATIVE delta is a
|
||||
* LEFT turn (counter-clockwise). Thresholds are the exported TURN_*
|
||||
* constants.
|
||||
*/
|
||||
export function classifyTurn(prevBearingDeg: number, nextBearingDeg: number): GuidanceTurnKind {
|
||||
let delta = (nextBearingDeg - prevBearingDeg) % 360;
|
||||
if (delta > 180) delta -= 360;
|
||||
if (delta <= -180) delta += 360;
|
||||
const abs = Math.abs(delta);
|
||||
if (abs >= TURN_UTURN_MIN_DEG) return "uturn";
|
||||
if (abs <= TURN_STRAIGHT_MAX_DEG) return "straight";
|
||||
if (abs <= TURN_SLIGHT_MAX_DEG) return delta < 0 ? "slight-left" : "slight-right";
|
||||
return delta < 0 ? "left" : "right";
|
||||
}
|
||||
|
||||
/* ————— step derivation ————— */
|
||||
|
||||
function initialBearingDeg(a: RoutePoint, b: RoutePoint): number {
|
||||
const lat1 = a.lat * DEG_TO_RAD;
|
||||
const lat2 = b.lat * DEG_TO_RAD;
|
||||
const dLng = (b.lng - a.lng) * DEG_TO_RAD;
|
||||
const y = Math.sin(dLng) * Math.cos(lat2);
|
||||
const x = Math.cos(lat1) * Math.sin(lat2) - Math.sin(lat1) * Math.cos(lat2) * Math.cos(dLng);
|
||||
return (Math.atan2(y, x) / DEG_TO_RAD + 360) % 360;
|
||||
}
|
||||
|
||||
/** The traversable (bit0 clear) edge u -> v, or -1. */
|
||||
function findTraversableEdge(graph: RoadGraph, u: number, v: number): number {
|
||||
for (let e = graph.edgeStart[u]!; e < graph.edgeStart[u + 1]!; e++) {
|
||||
if (graph.edgeTarget[e]! === v && (graph.edgeFlags[e]! & 1) === 0) return e;
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
interface StepAccumulator {
|
||||
kind: GuidanceTurnKind;
|
||||
name: string | undefined;
|
||||
points: RoutePoint[];
|
||||
distanceM: number;
|
||||
}
|
||||
|
||||
/**
|
||||
* Derive maneuver steps from a node path: split where the bearing leaves
|
||||
* "straight" or the street name changes. Step 0 is always "depart"; a
|
||||
* terminal "arrive" step anchors the goal node. Each step carries its
|
||||
* street name (omitted when unnamed), haversine meters, and polyline6
|
||||
* geometry (the RouteStep.kind field this phase added to types.ts —
|
||||
* fixture-world steps stay empty arrays, D-10).
|
||||
*
|
||||
* Throws on a malformed path (a non-traversable consecutive pair) — a
|
||||
* caller bug is loud, unlike a route miss which stays null.
|
||||
*/
|
||||
export function deriveSteps(graph: RoadGraph, nodes: number[]): RouteStep[] {
|
||||
if (nodes.length === 0) return [];
|
||||
if (nodes.length === 1) return [{ kind: "arrive" }];
|
||||
const pointAt = (node: number): RoutePoint => ({ lat: graph.lat[node]!, lng: graph.lng[node]! });
|
||||
const steps: RouteStep[] = [];
|
||||
let current: StepAccumulator | null = null;
|
||||
let prevBearing = 0;
|
||||
for (let i = 0; i < nodes.length - 1; i++) {
|
||||
const u = nodes[i]!;
|
||||
const v = nodes[i + 1]!;
|
||||
const edge = findTraversableEdge(graph, u, v);
|
||||
if (edge < 0) {
|
||||
throw new Error(`deriveSteps: nodes[${i}]->nodes[${i + 1}] (${u}->${v}) is not a traversable edge`);
|
||||
}
|
||||
const from = pointAt(u);
|
||||
const to = pointAt(v);
|
||||
const bearing = initialBearingDeg(from, to);
|
||||
const kind: GuidanceTurnKind = i === 0 ? "depart" : classifyTurn(prevBearing, bearing);
|
||||
prevBearing = bearing;
|
||||
const name = graph.nameAt(graph.edgeNameId[edge]!);
|
||||
const segmentM = haversineM(from, to);
|
||||
const splits = current !== null && (kind !== "straight" || name !== current.name);
|
||||
if (splits || current === null) {
|
||||
if (current !== null) {
|
||||
steps.push(toStep(current));
|
||||
}
|
||||
current = { kind, name, points: [from, to], distanceM: segmentM };
|
||||
} else {
|
||||
current.points.push(to);
|
||||
current.distanceM += segmentM;
|
||||
}
|
||||
}
|
||||
if (current !== null) steps.push(toStep(current));
|
||||
const goal = pointAt(nodes[nodes.length - 1]!);
|
||||
steps.push({ kind: "arrive", geometry: encodePolyline6([goal]) });
|
||||
return steps;
|
||||
}
|
||||
|
||||
function toStep(acc: StepAccumulator): RouteStep {
|
||||
const step: RouteStep = {
|
||||
kind: acc.kind,
|
||||
distanceM: acc.distanceM,
|
||||
geometry: encodePolyline6(acc.points),
|
||||
};
|
||||
if (acc.name !== undefined) step.name = acc.name;
|
||||
return step;
|
||||
}
|
||||
Reference in New Issue
Block a user