terra-route 0.0.13 → 0.0.17

package/src/terra-route.spec.ts

@@ -836,6 +836,87 @@ describe("TerraRoute", () => {
             expect(result!.geometry.coordinates.at(-1)).toEqual([2, 0]);
         });
 
+
+        it("fails gracefully when a custom heap returns null unexpectedly", () => {
+            class NullingHeap {
+                private items: Array<{ key: number; value: number }> = [];
+
+                insert(key: number, value: number) {
+                    this.items.push({ key, value });
+                }
+
+                extractMin() {
+                    if (this.items.length === 0) {
+                        return null;
+                    }
+                    return null;
+                }
+
+                size() {
+                    return this.items.length;
+                }
+            }
+
+            const heapConstructor = NullingHeap as unknown as new () => {
+                insert: (key: number, value: number) => void;
+                extractMin: () => number | null;
+                size: () => number;
+            };
+
+            const network = createFeatureCollection([
+                createLineStringFeature([
+                    [0, 0],
+                    [1, 0],
+                    [2, 0],
+                ]),
+            ]);
+
+            const rf = new TerraRoute({ heap: heapConstructor });
+
+            expect(() => rf.buildRouteGraph(network)).not.toThrow();
+
+            const result = rf.getRoute(
+                createPointFeature([0, 0]),
+                createPointFeature([2, 0]),
+            );
+
+            expect(result).toBeNull();
+        });
+
+
+        it("routes correctly when ALT landmarks are in another disconnected component", () => {
+            // Component A is listed first so ALT landmark selection starts here.
+            // Query is in component B, where landmark distances become Infinity and
+            // the ALT lower bound should safely degrade to 0 (Dijkstra behavior).
+            const network = createFeatureCollection([
+                createLineStringFeature([
+                    [0, 0],
+                    [1, 0],
+                    [2, 0],
+                ]),
+                createLineStringFeature([
+                    [100, 0],
+                    [101, 0],
+                    [102, 0],
+                ]),
+            ]);
+
+            routeFinder.buildRouteGraph(network);
+
+            const start = createPointFeature([100, 0]);
+            const end = createPointFeature([102, 0]);
+
+            const result = routeFinder.getRoute(start, end);
+
+            expect(result).not.toBeNull();
+            expect(result!.geometry.coordinates).toEqual([
+                [100, 0],
+                [101, 0],
+                [102, 0],
+            ]);
+        });
+
+
         describe('returns null if', () => {
             it("start and end are the same", () => {
                 const network = createFeatureCollection([

package/src/terra-route.ts

@@ -26,19 +26,26 @@ class TerraRoute implements Router {
     private csrIndices: Int32Array | null = null;      // Column indices: neighbor node IDs, length = totalEdges
     private csrDistances: Float64Array | null = null;  // Edge weights aligned to csrIndices, length = totalEdges
     private csrNodeCount = 0; // Number of nodes captured in the CSR arrays
-
-    // Reusable typed scratch buffers for A*
+    // ALT-style landmark heuristic data (query-time lower bounds via triangle inequality)
+    private landmarkNodeCount = 0;
+    private landmarkCount = 0;
+    private landmarkDistancesFlat: Float64Array | null = null; // Layout: [landmark0 distances..., landmark1 distances...]
+    private readonly maxLandmarks = 4;
+    private landmarksDirty = true;
+
+    // Reusable typed scratch buffers for shortest-path search
     private gScoreScratch: Float64Array | null = null; // gScore per node (cost from start)
     private cameFromScratch: Int32Array | null = null; // Predecessor per node for path reconstruction
     private visitedScratch: Uint8Array | null = null; // Visited set to avoid reprocessing
-    private hScratch: Float64Array | null = null; // Per-node heuristic cache for the current query (lazy compute)
-    // Reverse-direction scratch for bidirectional search
-    private gScoreRevScratch: Float64Array | null = null; // gScore per node from the end
-    private cameFromRevScratch: Int32Array | null = null; // Successor per node (next step toward the end)
-    private visitedRevScratch: Uint8Array | null = null; // Visited set for reverse search
-    private hRevScratch: Float64Array | null = null; // Heuristic cache for reverse direction per query
+    private heuristicScratch: Float64Array | null = null; // Cached h(node) per query for A*
+    private heuristicStampScratch: Uint32Array | null = null; // Query-stamp per node for heuristic cache validity
+    private heuristicQueryStamp = 1; // Monotonic stamp to avoid clearing heuristic cache each query
     private scratchCapacity = 0; // Current capacity of scratch arrays
 
+    // Reused open set to avoid heap allocations during repeated getRoute calls.
+    // (If a custom heap doesn't implement `clear()`, we fall back to constructing anew.)
+    private openForward: InstanceType<HeapConstructor> | null = null;
+
     constructor(options?: {
         distanceMeasurement?: (a: Position, b: Position) => number; // Optional distance function override
         heap?: HeapConstructor; // Optional heap implementation override
@@ -63,21 +70,40 @@ class TerraRoute implements Router {
         this.csrIndices = null;
         this.csrDistances = null;
         this.csrNodeCount = 0;
+        this.landmarkNodeCount = 0;
+        this.landmarkCount = 0;
+        this.landmarkDistancesFlat = null;
+        this.landmarksDirty = true;
 
         // Hoist to locals for speed (avoid repeated property lookups in hot loops)
         const coordIndexMapLocal = this.coordinateIndexMap; // Local alias for coord map
         const coordsLocal = this.coordinates; // Local alias for coordinates array
         const measureDistance = this.distanceMeasurement; // Local alias for distance function
 
-        // Pass 1: assign indices and count degrees per node
+        // Assign indices, count degrees and capture edges in one pass
         const degree: number[] = []; // Dynamic degree array; grows as nodes are discovered
-        this.forEachSegment(network, (a, b) => {
-            const indexA = this.indexCoordinate(a, coordIndexMapLocal, coordsLocal);
-            const indexB = this.indexCoordinate(b, coordIndexMapLocal, coordsLocal);
+        const edgeFrom: number[] = [];
+        const edgeTo: number[] = [];
+        const edgeDistance: number[] = [];
 
-            degree[indexA] = (degree[indexA] ?? 0) + 1;
-            degree[indexB] = (degree[indexB] ?? 0) + 1;
-        });
+        const features = network.features;
+        for (let f = 0, featureLength = features.length; f < featureLength; f++) {
+            const lineCoords = features[f].geometry.coordinates;
+            for (let i = 0, segmentLength = lineCoords.length - 1; i < segmentLength; i++) {
+                const from = lineCoords[i] as Position;
+                const to = lineCoords[i + 1] as Position;
+
+                const indexA = this.indexCoordinate(from, coordIndexMapLocal, coordsLocal);
+                const indexB = this.indexCoordinate(to, coordIndexMapLocal, coordsLocal);
+
+                degree[indexA] = (degree[indexA] ?? 0) + 1;
+                degree[indexB] = (degree[indexB] ?? 0) + 1;
+
+                edgeFrom.push(indexA);
+                edgeTo.push(indexB);
+                edgeDistance.push(measureDistance(from, to));
+            }
+        }
 
         // Build CSR arrays from degree counts
         const nodeCount = this.coordinates.length; // Total nodes discovered
@@ -91,14 +117,12 @@ class TerraRoute implements Router {
         const indices = new Int32Array(totalEdges); // Neighbor indices array
         const distances = new Float64Array(totalEdges); // Distances array aligned to indices
 
-        // Pass 2: fill CSR arrays using a write cursor per node
+        // Fill CSR arrays using a write cursor per node
         const cursor = offsets.slice(); // Current write positions per node
-        this.forEachSegment(network, (a, b) => {
-            // Read back indices (guaranteed to exist from pass 1)
-            const indexA = this.coordinateIndexMap.get(a[0])!.get(a[1])!;
-            const indexB = this.coordinateIndexMap.get(b[0])!.get(b[1])!;
-
-            const segmentDistance = measureDistance(a, b); // Edge weight once
+        for (let i = 0, edgeLength = edgeFrom.length; i < edgeLength; i++) {
+            const indexA = edgeFrom[i];
+            const indexB = edgeTo[i];
+            const segmentDistance = edgeDistance[i];
 
             let pos = cursor[indexA]++;
             indices[pos] = indexB;
@@ -106,7 +130,7 @@ class TerraRoute implements Router {
             pos = cursor[indexB]++;
             indices[pos] = indexA;
             distances[pos] = segmentDistance;
-        });
+        }
 
         // Commit CSR to instance
         this.csrOffsets = offsets;
@@ -228,6 +252,10 @@ class TerraRoute implements Router {
         this.csrIndices = indices;
         this.csrDistances = distances;
         this.csrNodeCount = nodeCount;
+        this.landmarksDirty = true;
+        this.landmarkNodeCount = 0;
+        this.landmarkCount = 0;
+        this.landmarkDistancesFlat = null;
 
         // Keep adjacency list for *future* dynamic additions, but clear existing edges to avoid duplication.
         this.adjacencyList = new Array(nodeCount);
@@ -260,204 +288,154 @@ class TerraRoute implements Router {
         }
 
         // Local aliases
-        const coords = this.coordinates; // Alias to coordinates array
-        const adj = this.adjacencyList; // Alias to sparse adjacency list (for dynamic nodes)
+        const coordinates = this.coordinates; // Alias to coordinates array
+        const adjacency = this.adjacencyList; // Alias to sparse adjacency list (for dynamic nodes)
+        const csrOffsets = this.csrOffsets;
+        const csrIndices = this.csrIndices;
+        const csrDistances = this.csrDistances;
+        const csrNodeCount = this.csrNodeCount;
+        const hasCsr = !!csrOffsets; // indices/distances should exist whenever offsets exist
+        const PositiveInfinity = Number.POSITIVE_INFINITY;
+        const measureDistance = this.distanceMeasurement;
+        const endCoordinates = end.geometry.coordinates;
+
+        this.ensureLandmarkHeuristicData();
+        const landmarkDistancesFlat = this.landmarkDistancesFlat;
+        const landmarkNodeCount = this.landmarkNodeCount;
+        const landmarkCount = this.landmarkCount;
 
         // Ensure and init scratch buffers
-        const nodeCount = coords.length; // Current number of nodes (may be >= csrNodeCount if new nodes added)
+        const nodeCount = coordinates.length; // Current number of nodes (may be >= csrNodeCount if new nodes added)
         this.ensureScratch(nodeCount); // Allocate scratch arrays if needed
 
         // Non-null after ensure
-        const gF = this.gScoreScratch!; // forward gScore (from start)
-        const gR = this.gScoreRevScratch!; // reverse gScore (from end)
-        const prevF = this.cameFromScratch!; // predecessor in forward search
-        const nextR = this.cameFromRevScratch!; // successor in reverse search (toward end)
+        const gF = this.gScoreScratch!; // gScore from start
+        const prevF = this.cameFromScratch!; // predecessor for reconstruction
         const visF = this.visitedScratch!;
-        const visR = this.visitedRevScratch!;
-        const hF = this.hScratch!;
-        const hR = this.hRevScratch!;
+        const heuristic = this.heuristicScratch!;
+        const heuristicStamp = this.heuristicStampScratch!;
 
-        gF.fill(Number.POSITIVE_INFINITY, 0, nodeCount);
-        gR.fill(Number.POSITIVE_INFINITY, 0, nodeCount);
+        gF.fill(PositiveInfinity, 0, nodeCount);
         prevF.fill(-1, 0, nodeCount);
-        nextR.fill(-1, 0, nodeCount);
         visF.fill(0, 0, nodeCount);
-        visR.fill(0, 0, nodeCount);
-        hF.fill(-1, 0, nodeCount);
-        hR.fill(-1, 0, nodeCount);
 
-        const openF = new this.heapConstructor();
-        const openR = new this.heapConstructor();
+        // Increment query stamp for heuristic cache validity; handle wraparound.
+        let queryStamp = (this.heuristicQueryStamp + 1) >>> 0;
+        if (queryStamp === 0) {
+            heuristicStamp.fill(0, 0, nodeCount);
+            queryStamp = 1;
+        }
+        this.heuristicQueryStamp = queryStamp;
 
-        const startCoord = coords[startIndex];
-        const endCoord = coords[endIndex];
+        const getHeuristic = (node: number): number => {
+            if (heuristicStamp[node] !== queryStamp) {
+                heuristicStamp[node] = queryStamp;
 
-        gF[startIndex] = 0;
-        gR[endIndex] = 0;
-
-        // Bidirectional Dijkstra (A* with zero heuristic). This keeps correctness simple and matches the
-        // reference pathfinder while still saving work by meeting in the middle.
-        openF.insert(0, startIndex);
-        openR.insert(0, endIndex);
-
-        // Best meeting point found so far
-        let bestPathCost = Number.POSITIVE_INFINITY;
-        let meetingNode = -1;
-
-        // Main bidirectional loop: expand alternately.
-        // Without a heap peek, a safe and effective stopping rule is based on the last extracted keys:
-        // once min_g_forward + min_g_reverse >= bestPathCost, no shorter path can still be found.
-        let lastExtractedGForward = 0;
-        let lastExtractedGReverse = 0;
-        while (openF.size() > 0 && openR.size() > 0) {
-            if (meetingNode >= 0 && (lastExtractedGForward + lastExtractedGReverse) >= bestPathCost) {
-                break;
-            }
+                if (landmarkDistancesFlat && landmarkCount > 0 && node < landmarkNodeCount && endIndex < landmarkNodeCount) {
+                    let lowerBound = 0;
+                    for (let l = 0, offset = 0; l < landmarkCount; l++, offset += landmarkNodeCount) {
+                        const distanceToNode = landmarkDistancesFlat[offset + node];
+                        const distanceToEnd = landmarkDistancesFlat[offset + endIndex];
 
-            // Expand one step from each side, prioritizing the smaller frontier.
-            const expandForward = openF.size() <= openR.size();
-
-            if (expandForward) {
-                const current = openF.extractMin()!;
-                if (visF[current] !== 0) continue;
-                lastExtractedGForward = gF[current];
-                visF[current] = 1;
-
-                // If reverse has finalized this node, we have a candidate meeting.
-                if (visR[current] !== 0) {
-                    const total = gF[current] + gR[current];
-                    if (total < bestPathCost) {
-                        bestPathCost = total;
-                        meetingNode = current;
-                    }
-                }
+                        if (!Number.isFinite(distanceToNode) || !Number.isFinite(distanceToEnd)) {
+                            continue;
+                        }
 
-                // Relax neighbors and push newly improved ones
-                if (this.csrOffsets && current < this.csrNodeCount) {
-                    const csrOffsets = this.csrOffsets!;
-                    const csrIndices = this.csrIndices!;
-                    const csrDistances = this.csrDistances!;
-                    for (let i = csrOffsets[current], endOff = csrOffsets[current + 1]; i < endOff; i++) {
-                        const nbNode = csrIndices[i];
-                        const tentativeG = gF[current] + csrDistances[i];
-                        if (tentativeG < gF[nbNode]) {
-                            gF[nbNode] = tentativeG;
-                            prevF[nbNode] = current;
-                            const otherG = gR[nbNode];
-                            if (otherG !== Number.POSITIVE_INFINITY) {
-                                const total = tentativeG + otherG;
-                                if (total < bestPathCost) { bestPathCost = total; meetingNode = nbNode; }
-                            }
-                            openF.insert(tentativeG, nbNode);
+                        const landmarkLowerBound = distanceToEnd >= distanceToNode
+                            ? distanceToEnd - distanceToNode
+                            : distanceToNode - distanceToEnd;
+
+                        if (landmarkLowerBound > lowerBound) {
+                            lowerBound = landmarkLowerBound;
                         }
                     }
+                    heuristic[node] = lowerBound;
                 } else {
-                    const neighbors = adj[current];
-                    if (neighbors && neighbors.length) {
-                        for (let i = 0, n = neighbors.length; i < n; i++) {
-                            const nb = neighbors[i];
-                            const nbNode = nb.node;
-                            const tentativeG = gF[current] + nb.distance;
-                            if (tentativeG < gF[nbNode]) {
-                                gF[nbNode] = tentativeG;
-                                prevF[nbNode] = current;
-                                const otherG = gR[nbNode];
-                                if (otherG !== Number.POSITIVE_INFINITY) {
-                                    const total = tentativeG + otherG;
-                                    if (total < bestPathCost) { bestPathCost = total; meetingNode = nbNode; }
-                                }
-                                openF.insert(tentativeG, nbNode);
-                            }
-                        }
-                    }
+                    heuristic[node] = measureDistance(coordinates[node], endCoordinates);
                 }
-            } else {
-                const current = openR.extractMin()!;
-                if (visR[current] !== 0) continue;
-                lastExtractedGReverse = gR[current];
-                visR[current] = 1;
-
-                if (visF[current] !== 0) {
-                    const total = gF[current] + gR[current];
-                    if (total < bestPathCost) {
-                        bestPathCost = total;
-                        meetingNode = current;
-                    }
+            }
+            return heuristic[node];
+        };
+
+        // Prefer reusing heaps if supported.
+        const openFReuse = this.openForward ?? (this.openForward = new this.heapConstructor());
+        const openFAny = openFReuse as unknown as { clear?: () => void };
+        const canReuse = !!openFAny.clear;
+
+        const openF2 = canReuse ? openFReuse : new this.heapConstructor();
+
+        if (canReuse) {
+            openFAny.clear!();
+        }
+
+        gF[startIndex] = 0;
+        openF2.insert(getHeuristic(startIndex), startIndex);
+
+        while (openF2.size() > 0) {
+            const current = openF2.extractMin();
+            if (current === null) {
+                break;
+            }
+            if (visF[current] !== 0) {
+                continue;
+            }
+            if (current === endIndex) {
+                break;
+            }
+
+            visF[current] = 1;
+            const currentDistance = gF[current];
+
+            const isCsrNode = hasCsr && current < csrNodeCount;
+            if (!isCsrNode) {
+                const neighbors = adjacency[current];
+                if (!neighbors || neighbors.length === 0) {
+                    continue;
                 }
 
-                // Reverse direction: same neighbor iteration because graph is undirected.
-                // Store successor pointer (next step toward end) i.e. nextR[neighbor] = current.
-                if (this.csrOffsets && current < this.csrNodeCount) {
-                    const csrOffsets = this.csrOffsets!;
-                    const csrIndices = this.csrIndices!;
-                    const csrDistances = this.csrDistances!;
-                    for (let i = csrOffsets[current], endOff = csrOffsets[current + 1]; i < endOff; i++) {
-                        const nbNode = csrIndices[i];
-                        const tentativeG = gR[current] + csrDistances[i];
-                        if (tentativeG < gR[nbNode]) {
-                            gR[nbNode] = tentativeG;
-                            nextR[nbNode] = current;
-                            const otherG = gF[nbNode];
-                            if (otherG !== Number.POSITIVE_INFINITY) {
-                                const total = tentativeG + otherG;
-                                if (total < bestPathCost) { bestPathCost = total; meetingNode = nbNode; }
-                            }
-                            openR.insert(tentativeG, nbNode);
-                        }
-                    }
-                } else {
-                    const neighbors = adj[current];
-                    if (neighbors && neighbors.length) {
-                        for (let i = 0, n = neighbors.length; i < n; i++) {
-                            const nb = neighbors[i];
-                            const nbNode = nb.node;
-                            const tentativeG = gR[current] + nb.distance;
-                            if (tentativeG < gR[nbNode]) {
-                                gR[nbNode] = tentativeG;
-                                nextR[nbNode] = current;
-                                const otherG = gF[nbNode];
-                                if (otherG !== Number.POSITIVE_INFINITY) {
-                                    const total = tentativeG + otherG;
-                                    if (total < bestPathCost) { bestPathCost = total; meetingNode = nbNode; }
-                                }
-                                openR.insert(tentativeG, nbNode);
-                            }
-                        }
+                for (let i = 0, n = neighbors.length; i < n; i++) {
+                    const nb = neighbors[i];
+                    const nbNode = nb.node;
+                    const tentativeG = currentDistance + nb.distance;
+                    if (tentativeG >= gF[nbNode]) {
+                        continue;
                     }
+
+                    gF[nbNode] = tentativeG;
+                    prevF[nbNode] = current;
+                    openF2.insert(tentativeG + getHeuristic(nbNode), nbNode);
+                }
+                continue;
+            }
+
+            for (let i = csrOffsets![current], endOff = csrOffsets![current + 1]; i < endOff; i++) {
+                const nbNode = csrIndices![i];
+                const tentativeG = currentDistance + csrDistances![i];
+                if (tentativeG >= gF[nbNode]) {
+                    continue;
                 }
+
+                gF[nbNode] = tentativeG;
+                prevF[nbNode] = current;
+                openF2.insert(tentativeG + getHeuristic(nbNode), nbNode);
             }
         }
 
-        if (meetingNode < 0) {
+        if (gF[endIndex] === PositiveInfinity) {
             return null;
         }
 
-        // Reconstruct path: start -> meeting using prevF, then meeting -> end using nextR
+        // Reconstruct path from end back to start through predecessor links.
         const path: Position[] = [];
 
-        // Walk back from meeting to start, collecting nodes
-        let cur = meetingNode;
+        let cur = endIndex;
         while (cur !== startIndex && cur >= 0) {
-            path.push(coords[cur]);
+            path.push(coordinates[cur]);
             cur = prevF[cur];
         }
-        if (cur !== startIndex) {
-            // Forward tree doesn't connect start to meeting (shouldn't happen if meeting is valid)
-            return null;
-        }
-        path.push(coords[startIndex]);
+        path.push(coordinates[startIndex]);
         path.reverse();
 
-        // Walk from meeting to end (skip meeting node because it's already included)
-        cur = meetingNode;
-        while (cur !== endIndex) {
-            cur = nextR[cur];
-            if (cur < 0) {
-                return null;
-            }
-            path.push(coords[cur]);
-        }
-
         return {
             type: "Feature",
             geometry: { type: "LineString", coordinates: path },
@@ -465,6 +443,120 @@ class TerraRoute implements Router {
         };
     }
 
+    // Build ALT heuristic tables by running shortest-path trees from selected landmarks.
+    private buildLandmarkHeuristicData(): void {
+        const offsets = this.csrOffsets;
+        const indices = this.csrIndices;
+        const distances = this.csrDistances;
+        const nodeCount = this.csrNodeCount;
+
+        if (!offsets || !indices || !distances || nodeCount === 0) {
+            this.landmarkNodeCount = 0;
+            this.landmarkCount = 0;
+            this.landmarkDistancesFlat = null;
+            return;
+        }
+
+        const targetLandmarkCount = Math.min(this.maxLandmarks, nodeCount);
+        const selected = new Uint8Array(nodeCount);
+        const allDistances: Float64Array[] = [];
+
+        let source = 0;
+        for (let landmarkIndex = 0; landmarkIndex < targetLandmarkCount; landmarkIndex++) {
+            selected[source] = 1;
+
+            const computedDistances = this.computeShortestDistancesFrom(source, nodeCount, offsets, indices, distances);
+            allDistances.push(computedDistances);
+
+            let farthestDistance = -1;
+            let farthestIndex = -1;
+            for (let node = 0; node < nodeCount; node++) {
+                if (selected[node] !== 0) {
+                    continue;
+                }
+
+                const distanceAtNode = computedDistances[node];
+                if (!Number.isFinite(distanceAtNode)) {
+                    continue;
+                }
+
+                if (distanceAtNode > farthestDistance) {
+                    farthestDistance = distanceAtNode;
+                    farthestIndex = node;
+                }
+            }
+
+            if (farthestIndex < 0) {
+                break;
+            }
+            source = farthestIndex;
+        }
+
+        const landmarkCount = allDistances.length;
+
+        const flat = new Float64Array(landmarkCount * nodeCount);
+        for (let i = 0; i < landmarkCount; i++) {
+            flat.set(allDistances[i], i * nodeCount);
+        }
+
+        this.landmarkNodeCount = nodeCount;
+        this.landmarkCount = landmarkCount;
+        this.landmarkDistancesFlat = flat;
+        this.landmarksDirty = false;
+    }
+
+    // Build landmark tables only when needed by getRoute.
+    private ensureLandmarkHeuristicData(): void {
+        if (!this.landmarksDirty) {
+            return;
+        }
+        this.buildLandmarkHeuristicData();
+    }
+
+    // Dijkstra over CSR to compute all-pairs distances from one source node.
+    private computeShortestDistancesFrom(
+        source: number,
+        nodeCount: number,
+        offsets: Int32Array,
+        indices: Int32Array,
+        distances: Float64Array,
+    ): Float64Array {
+        const PositiveInfinity = Number.POSITIVE_INFINITY;
+        const bestDistance = new Float64Array(nodeCount);
+        const visited = new Uint8Array(nodeCount);
+        bestDistance.fill(PositiveInfinity);
+        bestDistance[source] = 0;
+
+        const openSet = new this.heapConstructor();
+        openSet.insert(0, source);
+
+        while (openSet.size() > 0) {
+            const current = openSet.extractMin();
+            if (current === null) {
+                break;
+            }
+            if (visited[current] !== 0) {
+                continue;
+            }
+
+            visited[current] = 1;
+            const currentDistance = bestDistance[current];
+
+            for (let i = offsets[current], endOffset = offsets[current + 1]; i < endOffset; i++) {
+                const neighbor = indices[i];
+                const tentativeDistance = currentDistance + distances[i];
+                if (tentativeDistance >= bestDistance[neighbor]) {
+                    continue;
+                }
+
+                bestDistance[neighbor] = tentativeDistance;
+                openSet.insert(tentativeDistance, neighbor);
+            }
+        }
+
+        return bestDistance;
+    }
+
     /**
      * Helper to index start/end in getRoute.
      */
@@ -516,11 +608,8 @@ class TerraRoute implements Router {
             && this.gScoreScratch
             && this.cameFromScratch
             && this.visitedScratch
-            && this.hScratch
-            && this.gScoreRevScratch
-            && this.cameFromRevScratch
-            && this.visitedRevScratch
-            && this.hRevScratch;
+            && this.heuristicScratch
+            && this.heuristicStampScratch;
 
         if (ifAlreadyBigEnough) {
             return; // Nothing to do
@@ -529,11 +618,8 @@ class TerraRoute implements Router {
         this.gScoreScratch = new Float64Array(capacity);
         this.cameFromScratch = new Int32Array(capacity);
         this.visitedScratch = new Uint8Array(capacity);
-        this.hScratch = new Float64Array(capacity);
-        this.gScoreRevScratch = new Float64Array(capacity);
-        this.cameFromRevScratch = new Int32Array(capacity);
-        this.visitedRevScratch = new Uint8Array(capacity);
-        this.hRevScratch = new Float64Array(capacity);
+        this.heuristicScratch = new Float64Array(capacity);
+        this.heuristicStampScratch = new Uint32Array(capacity);
         this.scratchCapacity = capacity;
     }