From f42e45804876d351da6dfd0725dc74c5db18db8b Mon Sep 17 00:00:00 2001
From: glax <johanna@bernloehr.eu>
Date: Thu, 6 Apr 2023 02:23:12 +0200
Subject: [PATCH] Created GetSpeed method for uniform implementation Changed
 Pathfinding class to static Changed AStar to continue after path found.

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

diff --git a/Pathfinding/Pathfinder.cs b/Pathfinding/Pathfinder.cs
index dec6866..b1568f8 100644
--- a/Pathfinding/Pathfinder.cs
+++ b/Pathfinding/Pathfinder.cs
@@ -4,7 +4,7 @@ using OSMImporter;
 
 namespace Pathfinding;
 
-public class Pathfinder
+public static class Pathfinder
 {
     public static List<OsmNode> CustomAStar(string workingDir, Coordinates start, Coordinates goal, Tag.SpeedType vehicle)
     {
@@ -14,8 +14,8 @@ public class Pathfinder
         if (startRegion is null || goalRegion is null)
             return new List<OsmNode>();
 
-        OsmNode? startNode = ClosestNodeToCoordinates(start, startRegion, vehicle);
-        OsmNode? goalNode = ClosestNodeToCoordinates(goal, goalRegion, vehicle);
+        OsmNode? startNode = ClosestNodeToCoordinates(start, startRegion, vehicle, ref regionManager);
+        OsmNode? goalNode = ClosestNodeToCoordinates(goal, goalRegion, vehicle, ref regionManager);
         if (startNode == null || goalNode == null)
             return new List<OsmNode>();
 
@@ -27,7 +27,7 @@ public class Pathfinder
         OsmNode closestNodeToGoal = startNode;
         bool stop = false;
 
-        while (toVisit.Count > 0 && !stop)
+        while (toVisit.Count > 0)
         {
             closestNodeToGoal = toVisit.Dequeue();
             Console.WriteLine($"{toVisit.Count:000} {closestNodeToGoal.directDistanceToGoal:#.00} current:{closestNodeToGoal}");
@@ -44,10 +44,13 @@ public class Pathfinder
                         neighbor.previousPathNode = closestNodeToGoal;
                         neighbor.currentPathWeight = newPotentialWeight;
                         neighbor.currentPathLength = closestNodeToGoal.currentPathLength + Utils.DistanceBetween(closestNodeToGoal, neighbor);
-                    
-                        if (neighbor.Equals(goalNode) || closestNodeToGoal.directDistanceToGoal < 10) //change for faster
+
+                        if (neighbor.Equals(goalNode) || closestNodeToGoal.directDistanceToGoal < 10)
+                        {
                             stop = true;
-                        else
+                            goalNode = neighbor;
+                        }
+                        else if(!stop)
                         {
                             neighbor.directDistanceToGoal = Utils.DistanceBetween(neighbor, goalNode);
                             toVisit.Enqueue(neighbor, neighbor.directDistanceToGoal);
@@ -58,7 +61,7 @@ public class Pathfinder
         }
 
         List<OsmNode> path = new();
-        OsmNode? currentNode = closestNodeToGoal;
+        OsmNode? currentNode = goalNode;
         while (currentNode != null && !currentNode.Equals(startNode))
         {
             path.Add(currentNode);
@@ -70,7 +73,7 @@ public class Pathfinder
         return path;
     }
     
-    private static OsmNode? ClosestNodeToCoordinates(Coordinates coordinates, Region region, Tag.SpeedType vehicle)
+    private static OsmNode? ClosestNodeToCoordinates(Coordinates coordinates, Region region, Tag.SpeedType vehicle, ref RegionManager regionManager)
     {
         OsmNode? closest = null;
         double distance = double.MaxValue;
@@ -79,19 +82,7 @@ public class Pathfinder
             bool hasConnection = false;
             foreach (OsmEdge edge in node.edges)
             {
-                byte speed = 0;
-                switch (vehicle)
-                {
-                    case Tag.SpeedType.road:
-                    case Tag.SpeedType.car:
-                        speed = Tag.defaultSpeedCar[(Tag.WayType)region.tagManager.GetTag(edge.wayId, Tag.TagType.highway)!];
-                        break;
-                    case Tag.SpeedType.pedestrian:
-                        speed = Tag.defaultSpeedPedestrian[
-                            (Tag.WayType)region.tagManager.GetTag(edge.wayId, Tag.TagType.highway)!];
-                        break;
-                }
-
+                double speed = GetSpeed(node, edge.wayId, vehicle, ref regionManager);
                 if (speed != 0)
                     hasConnection = true;
             }
@@ -106,30 +97,38 @@ public class Pathfinder
         return closest;
     }
 
-    private static double EdgeWeight(OsmNode node1, OsmNode node2, ulong wayId, Tag.SpeedType vehicle, ref RegionManager regionManager)
+    private static double GetSpeed(OsmNode node1, ulong wayId, Tag.SpeedType vehicle, ref RegionManager regionManager)
     {
         TagManager tags = regionManager.GetRegion(node1.coordinates)!.tagManager;
-        double distance = Utils.DistanceBetween(node1, node2);
         Tag.WayType wayType = (Tag.WayType)tags.GetTag(wayId, Tag.TagType.highway)!;
         switch (vehicle)
         {
             case Tag.SpeedType.pedestrian:
                 byte speed = Tag.defaultSpeedPedestrian[wayType];
-                if(speed is not 0)
-                    return distance / speed;
-                else return Double.PositiveInfinity;
+                if (speed is not 0)
+                    return speed;
+                return 0;
             case Tag.SpeedType.car: 
             case Tag.SpeedType.road:
                 byte? maxspeed = (byte?)tags.GetTag(wayId, Tag.TagType.maxspeed);
                 if (maxspeed is not null)
-                    return distance / Convert.ToDouble(maxspeed);
-                else
-                    maxspeed = Tag.defaultSpeedCar[wayType];
+                    return (double)maxspeed;
+                maxspeed = Tag.defaultSpeedCar[wayType];
                 if(maxspeed is not 0)
-                    return distance / Tag.defaultSpeedCar[wayType];
-                else return Double.PositiveInfinity;;
+                    return (byte)maxspeed;
+                return 0;
             default:
-                return double.PositiveInfinity;
+                return 0;
         }
     }
+
+    private static double EdgeWeight(OsmNode node1, OsmNode node2, ulong wayId, Tag.SpeedType vehicle, ref RegionManager regionManager)
+    {
+        double distance = Utils.DistanceBetween(node1, node2);
+        double speed = GetSpeed(node1, wayId, vehicle, ref regionManager);
+        if (speed is not 0)
+            return distance / speed;
+        else
+            return double.PositiveInfinity;
+    }
 }
\ No newline at end of file