terra-route 0.0.12 → 0.0.14

package/src/terra-route.ts

@@ -6,6 +6,7 @@ import { FourAryHeap } from "./heap/four-ary-heap";
 
 interface Router {
     buildRouteGraph(network: FeatureCollection<LineString>): void;
+    expandRouteGraph(network: FeatureCollection<LineString>): void;
     getRoute(start: Feature<Point>, end: Feature<Point>): Feature<LineString> | null;
 }
 
@@ -31,6 +32,11 @@ class TerraRoute implements Router {
     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 scratchCapacity = 0; // Current capacity of scratch arrays
 
     constructor(options?: {
@@ -63,52 +69,15 @@ class TerraRoute implements Router {
         const coordsLocal = this.coordinates; // Local alias for coordinates array
         const measureDistance = this.distanceMeasurement; // Local alias for distance function
 
-        const features = network.features; // All LineString features
-
         // Pass 1: assign indices and count degrees per node
         const degree: number[] = []; // Dynamic degree array; grows as nodes are discovered
-        for (let f = 0, fLen = features.length; f < fLen; f++) { // Iterate features
-            const feature = features[f]; // Current feature
-            const lineCoords = feature.geometry.coordinates; // Coordinates for this LineString
-
-            for (let i = 0, len = lineCoords.length - 1; i < len; i++) { // Iterate segment pairs
-                const a = lineCoords[i]; // Segment start coord
-                const b = lineCoords[i + 1]; // Segment end coord
-
-                const lngA = a[0], latA = a[1]; // Keys for A
-                const lngB = b[0], latB = b[1]; // Keys for B
-
-                // Index A
-                let latMapA = coordIndexMapLocal.get(lngA);
-                if (latMapA === undefined) {
-                    latMapA = new Map<number, number>();
-                    coordIndexMapLocal.set(lngA, latMapA);
-                }
-                let indexA = latMapA.get(latA);
-                if (indexA === undefined) {
-                    indexA = coordsLocal.length;
-                    coordsLocal.push(a);
-                    latMapA.set(latA, indexA);
-                }
-
-                // Index B
-                let latMapB = coordIndexMapLocal.get(lngB);
-                if (latMapB === undefined) {
-                    latMapB = new Map<number, number>();
-                    coordIndexMapLocal.set(lngB, latMapB);
-                }
-                let indexB = latMapB.get(latB);
-                if (indexB === undefined) {
-                    indexB = coordsLocal.length;
-                    coordsLocal.push(b);
-                    latMapB.set(latB, indexB);
-                }
+        this.forEachSegment(network, (a, b) => {
+            const indexA = this.indexCoordinate(a, coordIndexMapLocal, coordsLocal);
+            const indexB = this.indexCoordinate(b, coordIndexMapLocal, coordsLocal);
 
-                // Count degree for both directions
-                degree[indexA] = (degree[indexA] ?? 0) + 1;
-                degree[indexB] = (degree[indexB] ?? 0) + 1;
-            }
-        }
+            degree[indexA] = (degree[indexA] ?? 0) + 1;
+            degree[indexB] = (degree[indexB] ?? 0) + 1;
+        });
 
         // Build CSR arrays from degree counts
         const nodeCount = this.coordinates.length; // Total nodes discovered
@@ -124,32 +93,20 @@ class TerraRoute implements Router {
 
         // Pass 2: fill CSR arrays using a write cursor per node
         const cursor = offsets.slice(); // Current write positions per node
-        for (let f = 0, fLen = features.length; f < fLen; f++) {
-            const feature = features[f];
-            const lineCoords = feature.geometry.coordinates;
-            for (let i = 0, len = lineCoords.length - 1; i < len; i++) {
-                const a = lineCoords[i];
-                const b = lineCoords[i + 1];
-
-                const lngA = a[0], latA = a[1];
-                const lngB = b[0], latB = b[1];
-
-                // Read back indices (guaranteed to exist from pass 1)
-                const indexA = this.coordinateIndexMap.get(lngA)!.get(latA)!;
-                const indexB = this.coordinateIndexMap.get(lngB)!.get(latB)!;
-
-                const segmentDistance = measureDistance(a, b); // Edge weight once
-
-                // Write A → B
-                let pos = cursor[indexA]++;
-                indices[pos] = indexB;
-                distances[pos] = segmentDistance;
-                // Write B → A
-                pos = cursor[indexB]++;
-                indices[pos] = indexA;
-                distances[pos] = segmentDistance;
-            }
-        }
+        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
+
+            let pos = cursor[indexA]++;
+            indices[pos] = indexB;
+            distances[pos] = segmentDistance;
+            pos = cursor[indexB]++;
+            indices[pos] = indexA;
+            distances[pos] = segmentDistance;
+        });
 
         // Commit CSR to instance
         this.csrOffsets = offsets;
@@ -160,6 +117,122 @@ class TerraRoute implements Router {
         this.adjacencyList = new Array(nodeCount);
     }
 
+    /**
+     * Expands (merges) the existing graph with an additional LineString FeatureCollection.
+     */
+    public expandRouteGraph(network: FeatureCollection<LineString>): void {
+        if (this.network === null) {
+            throw new Error("Network not built. Please call buildRouteGraph(network) first.");
+        }
+
+        // Merge the feature arrays for reference/debugging. We avoid copying properties deeply.
+        this.network = {
+            type: "FeatureCollection",
+            features: [...this.network.features, ...network.features],
+        };
+
+        const coordIndexMapLocal = this.coordinateIndexMap;
+        const coordsLocal = this.coordinates;
+        const measureDistance = this.distanceMeasurement;
+        const adj = this.adjacencyList;
+
+        // Ensure we have adjacency arrays for any existing CSR-only nodes.
+        // (During buildRouteGraph, adjacencyList is sized but entries are undefined.)
+        for (let i = 0; i < adj.length; i++) {
+            if (adj[i] === undefined) adj[i] = [];
+        }
+
+        // Add new edges into the adjacency list (sparse), then rebuild CSR from adjacency.
+        this.forEachSegment(network, (a, b) => {
+            const indexA = this.indexCoordinate(a, coordIndexMapLocal, coordsLocal, (idx) => { adj[idx] = []; });
+            const indexB = this.indexCoordinate(b, coordIndexMapLocal, coordsLocal, (idx) => { adj[idx] = []; });
+
+            const segmentDistance = measureDistance(a, b);
+
+            adj[indexA].push({ node: indexB, distance: segmentDistance });
+            adj[indexB].push({ node: indexA, distance: segmentDistance });
+        });
+
+        this.rebuildCsrFromAdjacency();
+    }
+
+    /**
+     * Rebuild CSR arrays for the full node set, using:
+     * - Existing CSR edges (from the last build/expand)
+     * - Any additional edges stored in `adjacencyList`
+     */
+    private rebuildCsrFromAdjacency(): void {
+        const nodeCount = this.coordinates.length;
+        const adj = this.adjacencyList;
+
+        // Compute degree using CSR degree + adjacency degree
+        const degree = new Int32Array(nodeCount);
+
+        if (this.csrOffsets && this.csrIndices && this.csrDistances) {
+            const csrOffsets = this.csrOffsets;
+            const covered = Math.min(this.csrNodeCount, nodeCount);
+            for (let i = 0; i < covered; i++) {
+                degree[i] += (csrOffsets[i + 1] - csrOffsets[i]);
+            }
+        }
+
+        for (let i = 0; i < nodeCount; i++) {
+            const neighbors = adj[i];
+            if (neighbors && neighbors.length) degree[i] += neighbors.length;
+        }
+
+        const offsets = new Int32Array(nodeCount + 1);
+        for (let i = 0; i < nodeCount; i++) {
+            offsets[i + 1] = offsets[i] + degree[i];
+        }
+        const totalEdges = offsets[nodeCount];
+        const indices = new Int32Array(totalEdges);
+        const distances = new Float64Array(totalEdges);
+        const cursor = offsets.slice();
+
+        // Copy existing CSR edges first
+        if (this.csrOffsets && this.csrIndices && this.csrDistances) {
+            const csrOffsets = this.csrOffsets;
+            const csrIndices = this.csrIndices;
+            const csrDistances = this.csrDistances;
+            const covered = Math.min(this.csrNodeCount, nodeCount);
+            for (let n = 0; n < covered; n++) {
+                const startOff = csrOffsets[n];
+                const endOff = csrOffsets[n + 1];
+                let pos = cursor[n];
+                for (let i = startOff; i < endOff; i++) {
+                    indices[pos] = csrIndices[i];
+                    distances[pos] = csrDistances[i];
+                    pos++;
+                }
+                cursor[n] = pos;
+            }
+        }
+
+        // Append adjacency edges
+        for (let n = 0; n < nodeCount; n++) {
+            const neighbors = adj[n];
+            if (!neighbors || neighbors.length === 0) continue;
+            let pos = cursor[n];
+            for (let i = 0, len = neighbors.length; i < len; i++) {
+                const nb = neighbors[i];
+                indices[pos] = nb.node;
+                distances[pos] = nb.distance;
+                pos++;
+            }
+            cursor[n] = pos;
+        }
+
+        // Commit and reset adjacency (we've absorbed edges into CSR)
+        this.csrOffsets = offsets;
+        this.csrIndices = indices;
+        this.csrDistances = distances;
+        this.csrNodeCount = nodeCount;
+
+        // Keep adjacency list for *future* dynamic additions, but clear existing edges to avoid duplication.
+        this.adjacencyList = new Array(nodeCount);
+    }
+
     /**
       * Computes the shortest route between two points in the network using the A* algorithm.
       * 
@@ -189,112 +262,202 @@ 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 measureDistance = this.distanceMeasurement; // Alias to distance function
 
         // Ensure and init scratch buffers
         const nodeCount = coords.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 gScore = this.gScoreScratch!; // gScore pointer
-        const cameFrom = this.cameFromScratch!; // cameFrom pointer
-        const visited = this.visitedScratch!; // visited pointer
-        const hCache = this.hScratch!; // heuristic cache pointer
-
-        // Reset only the used range for speed
-        gScore.fill(Number.POSITIVE_INFINITY, 0, nodeCount); // Init gScore to +∞
-        cameFrom.fill(-1, 0, nodeCount); // Init predecessors to -1 (unknown)
-        visited.fill(0, 0, nodeCount); // Init visited flags to 0
-        hCache.fill(-1, 0, nodeCount); // Init heuristic cache with sentinel (-1 means unknown)
-
-        // Create min-heap (priority queue)
-        const openSet = new this.heapConstructor();
-
-        // Precompute heuristic for start to prime the queue cost
-        const endCoord = coords[endIndex]; // Cache end coordinate for heuristic
-        let hStart = hCache[startIndex];
-
-        if (hStart < 0) {
-            hStart = measureDistance(coords[startIndex], endCoord);
-            hCache[startIndex] = hStart;
-        }
-
-        openSet.insert(hStart, startIndex); // Insert start with f = g(0) + h(start)
-        gScore[startIndex] = 0; // g(start) = 0
-
-        while (openSet.size() > 0) { // Main A* loop until queue empty
-            const current = openSet.extractMin()!; // Pop node with smallest f
-            if (visited[current] !== 0) { // Skip if already finalized
-                continue;
-            }
-            if (current === endIndex) { // Early exit if reached goal
+        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 visF = this.visitedScratch!;
+        const visR = this.visitedRevScratch!;
+        const hF = this.hScratch!;
+        const hR = this.hRevScratch!;
+
+        gF.fill(Number.POSITIVE_INFINITY, 0, nodeCount);
+        gR.fill(Number.POSITIVE_INFINITY, 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();
+
+        const startCoord = coords[startIndex];
+        const endCoord = coords[endIndex];
+
+        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;
             }
-            visited[current] = 1; // Mark as visited
-
-            // Prefer CSR neighbors if available for this node, fall back to sparse list for dynamically added nodes
-
-            if (this.csrOffsets && current < this.csrNodeCount) { // Use CSR fast path
-                const csrOffsets = this.csrOffsets!; // Local CSR offsets (non-null here)
-                const csrIndices = this.csrIndices!; // Local CSR neighbors
-                const csrDistances = this.csrDistances!; // Local CSR weights
-                const startOff = csrOffsets[current]; // Row start for current
-                const endOff = csrOffsets[current + 1]; // Row end for current
-
-                for (let i = startOff; i < endOff; i++) { // Iterate neighbors in CSR slice
-                    const nbNode = csrIndices[i]; // Neighbor node id
-                    const tentativeG = gScore[current] + csrDistances[i]; // g' = g(current) + w(current, nb)
-                    if (tentativeG < gScore[nbNode]) { // Relaxation check
-                        gScore[nbNode] = tentativeG; // Update best g
-                        cameFrom[nbNode] = current; // Track predecessor
-                        // A* priority = g + h (cache h per node for this query)
-                        let hVal = hCache[nbNode];
-                        if (hVal < 0) { hVal = measureDistance(coords[nbNode], endCoord); hCache[nbNode] = hVal; }
-                        openSet.insert(tentativeG + hVal, nbNode); // Push/update neighbor into open set
+
+            // 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;
                     }
                 }
-            }
-            // Fallback: use sparse adjacency (only for nodes added after CSR build)
-            else {
-
-                const neighbors = adj[current]; // Neighbor list for current
-                if (!neighbors || neighbors.length === 0) continue; // No neighbors
-                for (let i = 0, n = neighbors.length; i < n; i++) { // Iterate neighbors
-                    const nb = neighbors[i]; // Neighbor entry
-                    const nbNode = nb.node; // Neighbor id
-
-                    const tentativeG = gScore[current] + nb.distance; // g' via current
-                    if (tentativeG < gScore[nbNode]) { // Relax if better
-                        gScore[nbNode] = tentativeG; // Update best g
-                        cameFrom[nbNode] = current; // Track predecessor
-
-                        // A* priority = g + h (cached)
-                        let hVal = hCache[nbNode];
-                        if (hVal < 0) { hVal = measureDistance(coords[nbNode], endCoord); hCache[nbNode] = hVal; }
-                        openSet.insert(tentativeG + hVal, nbNode); // Enqueue neighbor
+
+                // 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);
+                        }
+                    }
+                } 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);
+                            }
+                        }
+                    }
+                }
+            } 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;
+                    }
+                }
+
+                // 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);
+                            }
+                        }
                     }
                 }
             }
         }
 
-        // If goal was never reached/relaxed, return null
-        if (cameFrom[endIndex] < 0) {
+        if (meetingNode < 0) {
             return null;
         }
 
-        // Reconstruct path (push + reverse to avoid O(n^2) unshift)
+        // Reconstruct path: start -> meeting using prevF, then meeting -> end using nextR
         const path: Position[] = [];
-        let cur = endIndex;
-        while (cur !== startIndex) {
+
+        // Walk back from meeting to start, collecting nodes
+        let cur = meetingNode;
+        while (cur !== startIndex && cur >= 0) {
             path.push(coords[cur]);
-            cur = cameFrom[cur];
+            cur = prevF[cur];
+        }
+        if (cur !== startIndex) {
+            // Forward tree doesn't connect start to meeting (shouldn't happen if meeting is valid)
+            return null;
         }
-        // Include start coordinate
         path.push(coords[startIndex]);
-
-        // Reverse to get start→end order
         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 },
@@ -353,7 +516,11 @@ class TerraRoute implements Router {
             && this.gScoreScratch
             && this.cameFromScratch
             && this.visitedScratch
-            && this.hScratch;
+            && this.hScratch
+            && this.gScoreRevScratch
+            && this.cameFromRevScratch
+            && this.visitedRevScratch
+            && this.hRevScratch;
 
         if (ifAlreadyBigEnough) {
             return; // Nothing to do
@@ -363,8 +530,58 @@ class TerraRoute implements Router {
         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.scratchCapacity = capacity;
     }
+
+
+    // Iterate all consecutive segment pairs in a LineString FeatureCollection.
+    // Kept as a simple loop helper to avoid repeating nested iteration logic.
+    private forEachSegment(
+        network: FeatureCollection<LineString>,
+        fn: (a: Position, b: Position) => void,
+    ): void {
+        const features = network.features;
+        for (let f = 0, fLen = features.length; f < fLen; f++) {
+            const lineCoords = features[f].geometry.coordinates;
+            for (let i = 0, len = lineCoords.length - 1; i < len; i++) {
+                // GeoJSON coordinates are compatible with Position (number[]), and the project assumes [lng,lat]
+                fn(lineCoords[i] as Position, lineCoords[i + 1] as Position);
+            }
+        }
+    }
+
+    // Hot-path coordinate indexer used by both build/expand.
+    // Accepts explicit maps/arrays so callers can hoist them once.
+    private indexCoordinate(
+        coord: Position,
+        coordIndexMapLocal: Map<number, Map<number, number>>,
+        coordsLocal: Position[],
+        onNewIndex?: (index: number) => void,
+    ): number {
+        const lng = coord[0];
+        const lat = coord[1];
+
+        let latMap = coordIndexMapLocal.get(lng);
+        if (latMap === undefined) {
+            latMap = new Map<number, number>();
+            coordIndexMapLocal.set(lng, latMap);
+        }
+
+        let idx = latMap.get(lat);
+        if (idx === undefined) {
+            idx = coordsLocal.length;
+            coordsLocal.push(coord);
+            latMap.set(lat, idx);
+            if (onNewIndex) onNewIndex(idx);
+        }
+
+        return idx;
+    }
+
 }
 
 export { TerraRoute, createCheapRuler, haversineDistance }