From 4600105b0b979186db2f0d1b5ef9112439fc005b Mon Sep 17 00:00:00 2001
From: glax <johanna@bernloehr.eu>
Date: Sat, 1 Apr 2023 18:10:21 +0200
Subject: [PATCH] Changed ClosestNodeToCoordinates to include only nodes that
 have connections for appropriate SpeedType (e.g. roads for cars, footways for
 pedestrians)

Changed toVisit to be a priorityqueue.

Search is aborted, if within 250m of goal.
---
 Pathfinding/Pathfinder.cs | 60 +++++++++++++++++++++++++--------------
 1 file changed, 38 insertions(+), 22 deletions(-)

diff --git a/Pathfinding/Pathfinder.cs b/Pathfinding/Pathfinder.cs
index b2adcad..13f2354 100644
--- a/Pathfinding/Pathfinder.cs
+++ b/Pathfinding/Pathfinder.cs
@@ -14,27 +14,23 @@ public class Pathfinder
         if (startRegion is null || goalRegion is null)
             return new List<OsmNode>();
 
-        OsmNode? startNode = ClosestNodeToCoordinates(start, startRegion);
-        OsmNode? goalNode = ClosestNodeToCoordinates(goal, goalRegion);
+        OsmNode? startNode = ClosestNodeToCoordinates(start, startRegion, vehicle);
+        OsmNode? goalNode = ClosestNodeToCoordinates(goal, goalRegion, vehicle);
         if (startNode == null || goalNode == null)
             return new List<OsmNode>();
-        
-        List<OsmNode> toVisit = new() { startNode };
-        OsmNode closestNodeToGoal = toVisit.First();
-        closestNodeToGoal.currentPathWeight = 0;
-        closestNodeToGoal.currentPathLength = 0;
+
+        PriorityQueue<OsmNode, double> toVisit = new();
+        toVisit.Enqueue(startNode, 0);
+        startNode.currentPathWeight = 0;
+        startNode.currentPathLength = 0;
+        startNode.directDistanceToGoal =  Utils.DistanceBetween(startNode, goalNode);
+        OsmNode closestNodeToGoal = startNode;
         bool stop = false;
 
         while (toVisit.Count > 0 && !stop)
         {
-            //Console.WriteLine("toVisit-length: {0}", toVisit.Count);
-            closestNodeToGoal = toVisit.First();
-            foreach (OsmNode node in toVisit.Where(node => node.directDistanceToGoal.Equals(Double.MaxValue)))
-            {
-                node.directDistanceToGoal = Utils.DistanceBetween(node, goalNode);
-            }
-
-            closestNodeToGoal = toVisit.OrderBy(node => node.directDistanceToGoal).First();
+            closestNodeToGoal = toVisit.Dequeue();
+            Console.WriteLine($"{toVisit.Count:000} {closestNodeToGoal.directDistanceToGoal:#.00} current:{closestNodeToGoal}");
 
             foreach (OsmEdge edge in closestNodeToGoal.edges)
             {
@@ -49,19 +45,20 @@ public class Pathfinder
                         neighbor.currentPathWeight = newPotentialWeight;
                         neighbor.currentPathLength = closestNodeToGoal.currentPathLength + Utils.DistanceBetween(closestNodeToGoal, neighbor);
                     
-                        if (neighbor.Equals(goalNode))
+                        if (neighbor.Equals(goalNode) || closestNodeToGoal.directDistanceToGoal < 250)
                             stop = true;
                         else
-                            toVisit.Add(neighbor);
+                        {
+                            neighbor.directDistanceToGoal = Utils.DistanceBetween(neighbor, goalNode);
+                            toVisit.Enqueue(neighbor, neighbor.directDistanceToGoal);
+                        }
                     }
                 }
             }
-
-            toVisit.Remove(closestNodeToGoal);
         }
 
         List<OsmNode> path = new();
-        OsmNode? currentNode = goalNode;
+        OsmNode? currentNode = closestNodeToGoal;
         while (currentNode != null && !currentNode.Equals(startNode))
         {
             path.Add(currentNode);
@@ -73,14 +70,33 @@ public class Pathfinder
         return path;
     }
     
-    private static OsmNode? ClosestNodeToCoordinates(Coordinates coordinates, Region region)
+    private static OsmNode? ClosestNodeToCoordinates(Coordinates coordinates, Region region, Tag.SpeedType vehicle)
     {
         OsmNode? closest = null;
         double distance = double.MaxValue;
         foreach (OsmNode node in region.nodes)
         {
+            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;
+                }
+
+                if (speed != 0)
+                    hasConnection = true;
+            }
             double nodeDistance = Utils.DistanceBetween(node, coordinates);
-            if (nodeDistance < distance)
+            if (nodeDistance < distance && hasConnection)
             {
                 closest = node;
                 distance = nodeDistance;