OSMDatastructure/Region.cs

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

Pathfinding/Pathfinder.cs

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

Pathfinding/RegionManager.cs

@@ -43,7 +43,6 @@ 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):###,###,###})]";
+        string dateTimeString = $"[{now.ToUniversalTime():u} ({Math.Floor((now - execStart).TotalMilliseconds):####00000})]";
         if(text is not null)
             OnWriteLine?.Invoke(this, new ConsoleWriterEventArgs($"{dateTimeString} {text}"));
         stdOut.WriteLine($"{dateTimeString} {text}");

Server/RegionConverter.cs

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

Server/Server.cs

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