From 750ba5c624935a1ba61669e0e90037c8146a3dfc Mon Sep 17 00:00:00 2001
From: glax <johanna@bernloehr.eu>
Date: Fri, 21 Apr 2023 11:01:31 +0200
Subject: [PATCH] Fixed angle calculation

---
 Pathfinding/Pathfinder.cs | 49 +++++++++++++++++++++++++--------------
 1 file changed, 32 insertions(+), 17 deletions(-)

diff --git a/Pathfinding/Pathfinder.cs b/Pathfinding/Pathfinder.cs
index 147d193..f469344 100644
--- a/Pathfinding/Pathfinder.cs
+++ b/Pathfinding/Pathfinder.cs
@@ -51,8 +51,8 @@ public class Pathfinder
             if (currentNode.Equals(goalNode))
             {
                 TimeSpan calcTime = DateTime.Now - startCalc;
-                Console.WriteLine($"Path found. {calcTime}");
                 pathResult = GetPath(goalNode, calcTime);
+                Console.WriteLine($"Path found. {calcTime} PathLength {pathResult.pathNodes.Count} VisitedNodes {gScore.Count}");
                 return this;
             }
 
@@ -70,7 +70,7 @@ public class Pathfinder
                             _cameFromDict[neighbor] = currentNode;
                         gScore[neighbor] = tentativeGScore;
                         double h = Heuristic(currentNode, neighbor, goalNode, edge,
-                            heuristicRoadLevelPriority, heuristicFewJunctionsPriority, heuristicSameRoadPriority);
+                            heuristicRoadLevelPriority, heuristicFewJunctionsPriority, heuristicSameRoadPriority, angleWeightFactor);
                         //Console.WriteLine($"Queue: {openSetfScore.Count:00000} Current Distance: {Utils.DistanceBetween(currentNode, goalNode):000000.00} Visited: {cameFromDict.Count:00000} Current heuristic: {h:00000.00}");
                         openSetfScore.Enqueue(neighbor, tentativeGScore + h);
                     }
@@ -119,20 +119,22 @@ public class Pathfinder
         if (_cameFromDict!.ContainsKey(currentNode))
         {
             OsmNode previousNode = _cameFromDict[currentNode];
-            Vector v1 = new(currentNode.coordinates.longitude - previousNode.coordinates.longitude,
-                currentNode.coordinates.latitude - previousNode.coordinates.latitude);
-            Vector v2 = new(currentNode.coordinates.longitude - neighborNode.coordinates.longitude,
-                currentNode.coordinates.latitude - neighborNode.coordinates.latitude);
+            Vector v1 = new(previousNode, currentNode);
+            Vector v2 = new(currentNode, neighborNode);
             double nodeAngle = v1.Angle(v2);
-            angle = (nodeAngle / 180) * angleWeightFactor;
+            angle = ((180 - nodeAngle) / 180) * angleWeightFactor;
         }
         double prio = GetPriorityVehicleRoad(edge, regionManager.GetRegion(currentNode.coordinates)!);
-        return distance / (speed + angle + prio * 0.5);
+        return distance / (1 + speed + angle + prio);
     }
 
-    private double Heuristic(OsmNode currentNode, OsmNode neighborNode, OsmNode goalNode, OsmEdge edge, double roadPriorityFactor, double junctionFactor, double sameRoadFactor)
+    private double Heuristic(OsmNode currentNode, OsmNode neighborNode, OsmNode goalNode, OsmEdge edge, double roadPriorityFactor, double junctionFactor, double sameRoadFactor, double nodeAngleFactor)
     {
         double roadPriority = GetPriorityVehicleRoad(edge, regionManager.GetRegion(currentNode.coordinates)!) * roadPriorityFactor;
+        if (roadPriority == 0)
+            return double.MaxValue;
+        
+        double speed = regionManager.GetSpeedForEdge(currentNode, edge.wayId, _speedType) * 0.0003;
         
         TagManager curTags = regionManager.GetRegion(currentNode.coordinates)!.tagManager;
         TagManager nextTags = regionManager.GetRegion(neighborNode.coordinates)!.tagManager;
@@ -152,8 +154,17 @@ public class Pathfinder
         double sameRoadName = (sameRef || sameName ? 1 : 0) * sameRoadFactor;
 
         double junctionCount = (neighborNode.edges.Count > 2 ? 0 : 1) * junctionFactor;
-
-        return Utils.DistanceBetween(neighborNode, goalNode) / (1 + roadPriority + sameRoadName + junctionCount);
+        
+        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);
     }
 
     private class Vector
@@ -166,6 +177,12 @@ public class Pathfinder
             this.y = y;
         }
 
+        public Vector(OsmNode n1, OsmNode n2)
+        {
+            this.x = n1.coordinates.longitude - n2.coordinates.longitude;
+            this.y = n1.coordinates.latitude - n2.coordinates.latitude;
+        }
+
         public double Angle(Vector v2)
         {
             return Angle(this, v2);
@@ -176,8 +193,9 @@ public class Pathfinder
             double dotProd = v1.x * v2.x + v1.y * v2.y;
             double v1L = Math.Sqrt(v1.x * v1.x + v1.y * v1.y);
             double v2L = Math.Sqrt(v2.x * v2.x + v2.y * v2.y);
-            double ang = 180 - Math.Acos(dotProd / (v1L * v2L));
-            return ang;
+            double ang = Math.Acos(dotProd / (v1L * v2L));
+            double angle = Utils.RadiansToDegrees(ang);
+            return angle;
         }
     }
     
@@ -212,9 +230,6 @@ public class Pathfinder
                 case WayType.road:
                 case WayType.living_street:
                     return 2;
-                case WayType.service:
-                case WayType.track:
-                    return 0.0001;
             }
         }
         if (_speedType == SpeedType.pedestrian)
@@ -243,6 +258,6 @@ public class Pathfinder
             }
         }
 
-        return 0.01;
+        return 0;
     }
 }
\ No newline at end of file