diff --git a/Pathfinding/Pathfinder.cs b/Pathfinding/Pathfinder.cs
index d47b960..3736c08 100644
--- a/Pathfinding/Pathfinder.cs
+++ b/Pathfinding/Pathfinder.cs
@@ -102,11 +102,24 @@ public class Pathfinder
 
     private double Heuristic(OsmNode currentNode, OsmNode neighborNode, OsmNode goalNode, OsmEdge edge, double roadPriorityFactor, double junctionFactor, double sameRoadFactor, double nodeAngleFactor)
     {
-        double roadPriority = regionManager.GetPriorityForVehicle(_speedType, edge, currentNode) * roadPriorityFactor;
-        if (roadPriority == 0)
+        double priority = regionManager.GetPriorityForVehicle(_speedType, edge, currentNode);
+        if (priority == 0)
             return double.MaxValue;
         
-        double speed = regionManager.GetSpeedForEdge(currentNode, edge.wayId, _speedType) * 0.0003;
+        double speed = regionManager.GetSpeedForEdge(currentNode, edge.wayId, _speedType);
+        
+        double angle = 0;
+        if (_cameFromDict!.ContainsKey(currentNode))
+        {
+            OsmNode previousNode = _cameFromDict[currentNode];
+            Vector v1 = new(currentNode, previousNode);
+            Vector v2 = new(currentNode, neighborNode);
+            double nodeAngle = v1.Angle(v2);
+            if (nodeAngle < 60)
+                angle = 0.001;
+            else
+                angle = (nodeAngle + 1) / (181);
+        }
         
         TagManager curTags = regionManager.GetRegion(currentNode.coordinates)!.tagManager;
         TagManager nextTags = regionManager.GetRegion(neighborNode.coordinates)!.tagManager;
@@ -123,20 +136,11 @@ public class Pathfinder
                 if ((string?)nextTags.GetTag(pEdge.wayId, TagType.tagref) == curRef)
                     sameRef = true;
             }
-        double sameRoadName = (sameRef || sameName ? 1 : 0) * sameRoadFactor;
 
         double junctionCount = (neighborNode.edges.Count > 2 ? 0 : 1) * junctionFactor;
-        
-        double angle = 0;
-        if (_cameFromDict!.ContainsKey(currentNode))
-        {
-            OsmNode previousNode = _cameFromDict[currentNode];
-            Vector v1 = new(previousNode, currentNode);
-            Vector v2 = new(currentNode, neighborNode);
-            double nodeAngle = v1.Angle(v2);
-            angle = ((180 - nodeAngle) / 180) * nodeAngleFactor;
-        }
-        return Utils.DistanceBetween(neighborNode, goalNode) / (roadPriority + sameRoadName + junctionCount + angle);
+        double roadPriority = priority * roadPriorityFactor;
+        double sameRoadName = (sameRef || sameName ? 1 : 0) * sameRoadFactor;
+        return Utils.DistanceBetween(neighborNode, goalNode) / (speed * angle + 1);
     }
 
     public void SaveResult(string path)