working

Changed toVisit to be a priorityqueue.

Search is aborted, if within 250m of goal.

OSMDatastructure/Region.cs

@@ -6,7 +6,7 @@ namespace OSMDatastructure;
 [Serializable]
 public class Region
 {
-    [NonSerialized]public const float RegionSize = 0.1f;
+    [NonSerialized]public const float RegionSize = 0.025f;
     public readonly HashSet<OsmNode> nodes = new();
     public ulong regionHash { get; }
     public TagManager tagManager { get; }

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;

Pathfinding/RegionManager.cs

@@ -43,6 +43,7 @@ namespace OSMImporter
 
         private Region? LoadRegion(ulong id)
         {
+            Console.WriteLine($"Load Region {id}");
             return Region.FromId(workingDirectory, id);
         }
 

Server/ConsoleWriter.cs

@@ -28,7 +28,7 @@ public class ConsoleWriter : TextWriter
     public override void WriteLine(string? text)
     {
         DateTime now = DateTime.Now;
-        string dateTimeString = $"[{now.ToUniversalTime():u} ({Math.Floor((now - execStart).TotalMilliseconds):####00000})]";
+        string dateTimeString = $"[{now.ToUniversalTime():u} ({Math.Floor((now - execStart).TotalMilliseconds):###,###,###})]";
         if(text is not null)
             OnWriteLine?.Invoke(this, new ConsoleWriterEventArgs($"{dateTimeString} {text}"));
         stdOut.WriteLine($"{dateTimeString} {text}");

Server/RegionConverter.cs

@@ -251,7 +251,9 @@ public class RegionConverter
             throw new FileNotFoundException("Region does not exist");
         
 #pragma warning disable SYSLIB0011
-        using (FileStream wayFileStream = new FileStream(Path.Join(folderPath, regionHash.ToString(), RegionConverter.WaysFileName), FileMode.Open))
+        string waysPath = Path.Join(folderPath, regionHash.ToString(), RegionConverter.WaysFileName);
+        if(File.Exists(waysPath))
+            using (FileStream wayFileStream = new FileStream(waysPath, FileMode.Open))
             {
                 while (wayFileStream.Position < wayFileStream.Length)
                 {
@@ -269,7 +271,9 @@ public class RegionConverter
             }
         }
 
-        using (FileStream tagsFileStream = new FileStream(Path.Join(folderPath, regionHash.ToString(), RegionConverter.TagsFileName), FileMode.Open))
+        string tagsPath = Path.Join(folderPath, regionHash.ToString(), RegionConverter.TagsFileName);
+        if(File.Exists(tagsPath))
+            using (FileStream tagsFileStream = new FileStream(tagsPath, FileMode.Open))
             {
                 while (tagsFileStream.Position < tagsFileStream.Length)
                 {

Server/Server.cs

@@ -18,19 +18,15 @@ public class Server
         Console.WriteLine("Loaded");
 
         
-        Coordinates start = new Coordinates(48.7933989f, 9.8301467f);
-        Coordinates finish = new Coordinates(48.7906258f, 9.8355983f);
-        OsmNode[] path = Pathfinder.CustomAStar("D:/map", start, finish, Tag.SpeedType.pedestrian).ToArray();
-        Console.WriteLine("{0}\n", path[0].ToString());
+        Coordinates start = new Coordinates(48.793319f, 9.832102f);
+        Coordinates finish = new Coordinates(48.840728f, 10.067603f);
+        DateTime startTime = DateTime.Now;
+        OsmNode[] path = Pathfinder.CustomAStar("D:/stuttgart-regbez-latest", start, finish, Tag.SpeedType.car).ToArray();
+        TimeSpan duration = DateTime.Now - startTime;
+        Console.WriteLine($"Took {duration.TotalMilliseconds}ms ({duration:g})");
         for (int i = 0; i < path.Length - 1; i++)
         {
-            OsmNode n1 = path[i];
-            OsmNode n2 = path[i + 1];
-            OsmEdge? e = n1.GetEdgeToNode(n2);
-            if(e != null)
-                Console.WriteLine("{0}\n{1}", e.ToString(), n2.ToString());
-            else
-                Console.WriteLine("NO EDGE\n{0}", n2.ToString());
+            Console.WriteLine(path[i]);
         }
         Console.WriteLine();
     }