diff --git a/packages/common/src/AStar.ts b/packages/common/src/AStar.ts
index c2707d9cf..82ed6ea9d 100644
--- a/packages/common/src/AStar.ts
+++ b/packages/common/src/AStar.ts
@@ -33,17 +33,21 @@ class Node implements AStarNode {
     f: number;
     parent: Node | null;
 
+    id: number;
+
     constructor(point: number[], g: number, h: number, parent: Node | null = null) {
         this.point = point;
         this.g = g;
         this.h = h;
         this.f = g + h;
         this.parent = parent;
+        this.id = AStar.pointKey(this);
     }
 }
 
 export class AStar {
     static precision = 1e5;
+
     static calculateDistance(point1: number[], point2: number[]): number {
         // const dx = point2[0] - point1[0];
         // const dy = point2[1] - point1[1];
@@ -61,64 +65,73 @@ export class AStar {
         // return point1[0] === point2[0] && point1[1] === point2[1];
     }
 
-    private static pointKey(node: AStarNode): number {
+    static pointKey(node: AStarNode): number {
         const precision = AStar.precision;
         const point = node.point;
         return (point[0] * precision ^ 0) * 1e7 + (point[1] * precision ^ 0);
     }
 
+    private static reconstructPath(current: Node) {
+        const path: AStarNode[] = [];
+        while (current !== null) {
+            path.unshift(current);
+            current = current.parent;
+        }
+        return path;
+    }
+
     public static findPath(
         from: AStarNode,
         endNode: AStarNode,
         getNeighbors: (node: AStarNode) => AStarNode[],
         weight: (nodeA: AStarNode, nodeB: AStarNode) => number = AStar.weight
     ): AStarNode[] | null {
-        const start = from.point;
-        const endCoordinate = endNode.point;
         const openList = new BinaryHeap<Node>((a, b) => a.f - b.f);
-        const closedList = new Set<number>();
-        // const startNode = new NavNode(start, 0, AStar.calculateDistance(start, endCoordinate));
-        const startNode = new Node(start, 0, Infinity);
+        const closedList = new Map<string | number, boolean>();
+        // const startNode = new NavNode(start, 0, AStar.calculateDistance(start, endNode.point));
+        const startNode = new Node(from.point, 0, Infinity);
+
         startNode.data = from.data;
 
         openList.push(startNode);
 
+        const endNodeId = this.pointKey(endNode);
+
         while (openList.size() > 0) {
             const currentNode = openList.pop()!;
 
-            if (AStar.isPointEqual(currentNode.point, endCoordinate)) {
-                // reconstruct the path
-                const path: AStarNode[] = [];
-                let current: Node | null = currentNode;
-                while (current !== null) {
-                    path.unshift(current);
-                    current = current.parent;
-                }
-                return path;
+            if (currentNode.id === endNodeId) {
+                // if (AStar.isPointEqual(currentNode.point, endNode.point)) {
+                return this.reconstructPath(currentNode);
             }
 
-            const pointKey = AStar.pointKey(currentNode);
-            closedList.add(pointKey);
+            closedList.set(currentNode.id, true);
+
             for (const neighborNode of getNeighbors(currentNode)) {
-                const {point: neighbor, data} = neighborNode;
-                const neighborKey = AStar.pointKey(neighborNode);
+                const {point, data} = neighborNode;
+                const neighborKey = this.pointKey(neighborNode);
+
                 if (closedList.has(neighborKey)) {
                     continue;
                 }
+
                 const g = currentNode.g + weight(currentNode, neighborNode);
                 const h = weight(neighborNode, endNode);
-                // const h = AStar.calculateDistance(neighborNode.point, endNode.point);
-                const existingNode = openList.find((node) => AStar.isPointEqual(node.point, neighbor));
-                if (existingNode) {
+                // const existingNodeIndex = openList.findIndex((node) => AStar.isPointEqual(node.point, point));
+                const existingNodeIndex = openList.findIndex((node) => node.id == neighborKey);
+
+                if (existingNodeIndex != -1) {
+                    const existingNode = openList.get(existingNodeIndex);
                     if (g < existingNode.g) {
                         existingNode.g = g;
                         existingNode.h = h;
                         existingNode.f = g + h;
                         existingNode.parent = currentNode;
                         existingNode.data = data;
+                        openList.adjustElement(existingNodeIndex);
                     }
                 } else {
-                    const newNode = new Node(neighbor, g, h, currentNode);
+                    const newNode = new Node(point, g, h, currentNode);
                     newNode.data = data;
                     openList.push(newNode);
                 }
diff --git a/packages/common/src/BinaryHeap.ts b/packages/common/src/BinaryHeap.ts
index 713b2983b..444f6dd1e 100644
--- a/packages/common/src/BinaryHeap.ts
+++ b/packages/common/src/BinaryHeap.ts
@@ -25,10 +25,18 @@ export class BinaryHeap<T> {
         this.compare = compare;
     }
 
-    find(predicate: (this: void, value: T, index: number, obj: T[]) => boolean) {
+    get(index: number) {
+        return this.heap[index];
+    }
+
+    find(predicate: (value: T, index: number, obj: T[]) => boolean) {
         return this.heap.find(predicate);
     }
 
+    findIndex(predicate: (this: void, value: T, index: number, obj: T[]) => boolean): number {
+        return this.heap.findIndex(predicate);
+    }
+
     includes(value: T) {
         return this.heap.includes(value);
     }
@@ -52,6 +60,29 @@ export class BinaryHeap<T> {
         return result;
     }
 
+    adjustElement(index: number): void {
+        const element = this.heap[index];
+        const parentIndex = Math.floor((index - 1) / 2);
+        const parent = this.heap[parentIndex];
+
+        if (parentIndex > 0 && this.compare(element, parent) < 0) {
+            this.bubbleUp(index);
+        } else {
+            this.sinkDown(index);
+        }
+    }
+
+    remove(value: T): void {
+        const index = this.heap.findIndex((item) => item === value);
+        if (index !== -1) {
+            const end = this.heap.pop()!;
+            if (index !== this.heap.length) {
+                this.heap[index] = end;
+                this.adjustElement(index);
+            }
+        }
+    }
+
     size(): number {
         return this.heap.length;
     }
@@ -71,26 +102,28 @@ export class BinaryHeap<T> {
     private sinkDown(index: number): void {
         const length = this.heap.length;
         const element = this.heap[index];
+
         while (true) {
             let leftChildIndex = 2 * index + 1;
             let rightChildIndex = 2 * index + 2;
             let swap = null;
-            let leftChild;
-            let rightChild;
 
             if (leftChildIndex < length) {
-                leftChild = this.heap[leftChildIndex];
+                const leftChild = this.heap[leftChildIndex];
                 if (this.compare(leftChild, element) < 0) {
                     swap = leftChildIndex;
                 }
             }
             if (rightChildIndex < length) {
-                rightChild = this.heap[rightChildIndex];
-                if ((swap === null && this.compare(rightChild, element) < 0) ||
-                    (swap !== null && this.compare(rightChild, leftChild!) < 0)) {
+                const rightChild = this.heap[rightChildIndex];
+                if (
+                    (swap === null && this.compare(rightChild, element) < 0) ||
+                    (swap !== null && this.compare(rightChild, this.heap[swap]!) < 0)
+                ) {
                     swap = rightChildIndex;
                 }
             }
+
             if (swap === null) break;
             this.heap[index] = this.heap[swap];
             this.heap[swap] = element;
@@ -98,3 +131,4 @@ export class BinaryHeap<T> {
         }
     }
 }
+
diff --git a/packages/core/src/providers/FeatureProvider.ts b/packages/core/src/providers/FeatureProvider.ts
index 411e68678..761e33749 100644
--- a/packages/core/src/providers/FeatureProvider.ts
+++ b/packages/core/src/providers/FeatureProvider.ts
@@ -736,28 +736,28 @@ export class FeatureProvider extends Provider {
             /**
              * Object representing the source road segment of the turn.
              */
-            from: {
+            readonly from: {
                 /**
                  * GeoJSON Feature representing the source road segment.
                  */
-                link: Feature<'LineString'>,
+                readonly link: Feature<'LineString'>,
                 /**
                  * Index of the Coordinates array of the source road segment.
                  */
-                index: number
+                readonly index: number
             },
             /**
              * Object representing the destination road segment of the turn.
              */
-            to: {
+            readonly to: {
                 /**
                  * GeoJSON Feature representing the destination road segment.
                  */
-                link: Feature<'LineString'>,
+                readonly link: Feature<'LineString'>,
                 /**
                  * Index of the Coordinates array of the destination road segment.
                  */
-                index: number
+                readonly index: number
             }
         }) => boolean,
         /**
diff --git a/packages/core/src/route/Route.ts b/packages/core/src/route/Route.ts
index 5e075f79c..c0f01a845 100644
--- a/packages/core/src/route/Route.ts
+++ b/packages/core/src/route/Route.ts
@@ -17,7 +17,7 @@
  * License-Filename: LICENSE
  */
 import {AStar, AStarNode} from '@here/xyz-maps-common';
-import {Feature, FeatureProvider, GeoJSONCoordinate} from '@here/xyz-maps-core';
+import {Feature, FeatureProvider} from '@here/xyz-maps-core';
 
 type NodeData = { link: Feature, index: number };
 
@@ -29,12 +29,14 @@ type Weight = (options: { feature: Feature; distance: number; from: number[]; to
 //     return [0, feature.geometry.coordinates.length - 1];
 // };
 
+type Turn = { from: NodeData, to: NodeData };
+
 export class PathFinder {
     static async findPath(
         provider: FeatureProvider,
         fromNode: Node,
         toNode: Node,
-        isTurnAllowed: (turn: { from: NodeData, to: NodeData }) => boolean,
+        isTurnAllowed: (turn: Turn) => boolean,
         weight?: Weight
         // filterCoordinates: ((feature: Feature) => number[]) | undefined = defaultCoordinatesFilter
     ): Promise<Node[]> {
@@ -43,8 +45,8 @@ export class PathFinder {
             const getNeighbor = (node: Node): Node[] => {
                 const point = node.point;
                 const fromLink = node.data.link;
-                const turn = {from: node.data, to: {link: null, index: null}};
-                const neighbors: { point: number[], data: { link: Feature, index: number } }[] = [];
+                const turn = {from: node.data, to: null};
+                const neighbors: Node[] = [];
                 const geoJSONFeatures = <Feature[]>provider.search({point: {longitude: point[0], latitude: point[1]}, radius: .5});
                 for (const feature of geoJSONFeatures) {
                     if (feature.geometry.type != 'LineString' /* || feature.id == fromLink.id*/) continue;