diff --git a/Pathfinding/Pathfinder.cs b/Pathfinding/Pathfinder.cs
index ec99238..b0ad52e 100644
--- a/Pathfinding/Pathfinder.cs
+++ b/Pathfinding/Pathfinder.cs
@@ -68,8 +68,7 @@ public class Pathfinder
                 OsmNode? neighbor = regionManager.GetNode(edge.neighborId, edge.neighborRegion);
                 if (neighbor is not null)
                 {
-                    double tentativeGScore =
-                        gScore[currentNode] + Weight(currentNode, neighbor, edge);
+                    double tentativeGScore = gScore[currentNode] + Weight(currentNode, neighbor, edge);
                     gScore.TryAdd(neighbor, double.MaxValue);
                     if (tentativeGScore < gScore[neighbor])
                     {
@@ -108,6 +107,7 @@ public class Pathfinder
 
     private double Heuristic(OsmNode currentNode, OsmNode neighborNode, OsmNode goalNode, OsmEdge edge)
     {
+        if (neighborNode.Equals(goalNode)) return 0;
         double priority = regionManager.GetPriorityForVehicle(_speedType, edge, currentNode);
         if (priority == 0)
             return double.MaxValue;