astar/Astar.cs

@@ -6,16 +6,15 @@ using OSM_Regions;
 
 namespace astar
 {
-    public class Astar(ValueTuple<float, float, float, float>? priorityWeights = null, int? explorationMultiplier = null, float? nonPriorityRoadSpeedPenalty = null, RegionLoader? regionLoader = null)
+    public class Astar(ValueTuple<float, float, float, float>? priorityWeights = null, int? explorationMultiplier = null, float? nonPriorityRoadSpeedPenalty = null)
     {
-        private readonly ValueTuple<float, float, float, float> DefaultPriorityWeights = priorityWeights ?? new(0.6f, 1.05f, 0, 0);
+        private readonly ValueTuple<float, float, float, float> DefaultPriorityWeights = priorityWeights ?? new(0.7f, 1.08f, 0, 0);
-        private readonly int _explorationMultiplier = explorationMultiplier ?? 120;
+        private readonly int _explorationMultiplier = explorationMultiplier ?? 150;
-        private readonly float _nonPriorityRoadSpeedPenalty = nonPriorityRoadSpeedPenalty ?? 0.85f;
+        private readonly float _nonPriorityRoadSpeedPenalty = nonPriorityRoadSpeedPenalty ?? 0.9f;
-        private RegionLoader? rl = regionLoader;
         
         public Route FindPath(float startLat, float startLon, float endLat, float endLon, float regionSize, bool car = true, PathMeasure pathing = PathMeasure.Distance, string? importFolderPath = null, ILogger? logger = null)
         {
-            rl ??= new(regionSize, importFolderPath, logger: logger);
+            RegionLoader rl = new(regionSize, importFolderPath, logger: logger);
             Graph graph = Spiral(rl, startLat, startLon, regionSize);
             Graph endRegion = Spiral(rl, endLat, endLon, regionSize);
             graph.ConcatGraph(endRegion);
@@ -44,18 +43,22 @@ namespace astar
             ValueTuple<Node, Node>? meetingEnds = null;
             while (toVisitStart.Count > 0 && toVisitEnd.Count > 0)
             {
-                for (int i = 0; i < Math.Min(toVisitStart.Count * 0.5, 50) && meetingEnds is null; i++)
+                ulong closestEndNodeId = toVisitEnd.UnorderedItems
+                    .MinBy(n => graph.Nodes[n.Element].DistanceTo(startNode.Value)).Element;
+                Node closestEndNode = graph.Nodes[closestEndNodeId];
+                if (toVisitStart.Count >= toVisitEnd.Count && meetingEnds is null)
                 {
-                    ulong closestEndNodeId = toVisitEnd.UnorderedItems.MinBy(node => graph.Nodes[node.Element].DistanceTo(graph.Nodes[toVisitStart.Peek()])).Element;
+                    for(int i = 0; i < Math.Min(toVisitStart.Count * 0.5, 50) && meetingEnds is null; i++)
-                    Node closestEndNode = graph.Nodes[closestEndNodeId];
+                        meetingEnds = ExploreSide(true, graph, toVisitStart, rl, priorityHelper, closestEndNode, car, DefaultPriorityWeights, pathing, logger);
-                    meetingEnds = ExploreSide(true, graph, toVisitStart, priorityHelper, closestEndNode, car, DefaultPriorityWeights, pathing, logger);
                 }
                 
-                for (int i = 0; i < Math.Min(toVisitEnd.Count * 0.5, 50) && meetingEnds is null; i++)
+                ulong closestStartNodeId = toVisitStart.UnorderedItems
+                    .MinBy(n => graph.Nodes[n.Element].DistanceTo(endNode.Value)).Element;
+                Node closestStartNode = graph.Nodes[closestStartNodeId];
+                if (toVisitEnd.Count >= toVisitStart.Count && meetingEnds is null)
                 {
-                    ulong closestStartNodeId = toVisitStart.UnorderedItems.MinBy(node => graph.Nodes[node.Element].DistanceTo(graph.Nodes[toVisitEnd.Peek()])).Element;
+                    for(int i = 0; i < Math.Min(toVisitEnd.Count * 0.5, 50) && meetingEnds is null; i++)
-                    Node closestStartNode = graph.Nodes[closestStartNodeId];
+                        meetingEnds = ExploreSide(false, graph, toVisitEnd, rl, priorityHelper, closestStartNode, car, DefaultPriorityWeights, pathing, logger);
-                    meetingEnds = ExploreSide(false, graph, toVisitEnd, priorityHelper, closestStartNode, car, DefaultPriorityWeights, pathing, logger);
                 }
 
                 if (meetingEnds is not null)
@@ -66,32 +69,25 @@ namespace astar
                 return new Route(graph, Array.Empty<Step>().ToList(), false);
 
             Queue<ulong> routeQueue = new();
-            toVisitStart.EnqueueRange(toVisitEnd.UnorderedItems);
+            foreach (ulong id in toVisitStart.UnorderedItems.Select(l => l.Element)
-            while(toVisitStart.Count > 0)
+                         .Union(toVisitEnd.UnorderedItems.Select(l => l.Element)))
             {
-                routeQueue.Enqueue(toVisitStart.Dequeue());
+                routeQueue.Enqueue(id);
             }
-            int optimizeAfterFound = graph.Nodes.Count(n => n.Value.PreviousNodeId is not null) * _explorationMultiplier; //Check another x% of unexplored Paths.
+            ValueTuple<Node, Node>? newMeetingEnds = Optimize(graph, routeQueue, car, rl, pathing, logger);
-            List<ValueTuple<Node, Node>> newMeetingEnds = Optimize(graph, routeQueue, optimizeAfterFound, car, rl, pathing, logger);
+            meetingEnds = newMeetingEnds ?? meetingEnds;
-            List<Route> routes = newMeetingEnds.Select(end => PathFound(graph, end.Item1, end.Item2, car)).ToList();
-            routes.Add(PathFound(graph, meetingEnds.Value.Item1, meetingEnds.Value.Item2, car));
 
-            return routes.MinBy(route =>
+            return PathFound(graph, meetingEnds!.Value.Item1, meetingEnds.Value.Item2, car, logger);
-            {
-                if (pathing is PathMeasure.Distance)
-                    return route.Distance;
-                return route.Time.Ticks;
-            })!;
         }
 
-        private ValueTuple<Node, Node>? ExploreSide(bool fromStart, Graph graph, PriorityQueue<ulong, int> toVisit, PriorityHelper priorityHelper, Node goalNode, bool car, ValueTuple<float,float,float,float> ratingWeights, PathMeasure pathing, ILogger? logger = null)
+        private ValueTuple<Node, Node>? ExploreSide(bool fromStart, Graph graph, PriorityQueue<ulong, int> toVisit, RegionLoader rl, PriorityHelper priorityHelper, Node goalNode, bool car, ValueTuple<float,float,float,float> ratingWeights, PathMeasure pathing, ILogger? logger = null)
         {
             ulong currentNodeId = toVisit.Dequeue();
             Node currentNode = graph.Nodes[currentNodeId];
             logger?.LogDebug($"Distance to goal {currentNode.DistanceTo(goalNode):00000.00}m");
             foreach ((ulong neighborId, KeyValuePair<ulong, bool> wayId) in currentNode.Neighbors)
             {
-                LoadNeighbor(graph, neighborId, wayId.Key, rl!, logger);
+                LoadNeighbor(graph, neighborId, wayId.Key, rl, logger);
                     
                 OSM_Graph.Way way = graph.Ways[wayId.Key];
                 byte speed = SpeedHelper.GetSpeed(way, car);
@@ -102,7 +98,8 @@ namespace astar
                 
                 Node neighborNode = graph.Nodes[neighborId];
 
-                if (neighborNode.PreviousIsFromStart == !fromStart) //Check if we found the opposite End
+                if (neighborNode.PreviousIsFromStart is not null &&
+                    neighborNode.PreviousIsFromStart != fromStart) //Check if we found the opposite End
                     return fromStart ? new(currentNode, neighborNode) : new(neighborNode, currentNode);
 
                 float metric = (currentNode.Metric ?? float.MaxValue) + (pathing is PathMeasure.Distance
@@ -116,16 +113,18 @@ namespace astar
                     toVisit.Enqueue(neighborId,
                         priorityHelper.CalculatePriority(currentNode, neighborNode, goalNode, speed, ratingWeights));
                 }
+                logger?.LogTrace($"Neighbor {neighborId} {neighborNode}");
             }
 
             return null;
         }
 
-        private List<ValueTuple<Node, Node>> Optimize(Graph graph, Queue<ulong> combinedQueue, int optimizeAfterFound, bool car, RegionLoader rl, PathMeasure pathing, ILogger? logger = null)
+        private ValueTuple<Node, Node>? Optimize(Graph graph, Queue<ulong> combinedQueue, bool car, RegionLoader rl, PathMeasure pathing, ILogger? logger = null)
         {
             int currentPathLength = graph.Nodes.Values.Count(node => node.PreviousNodeId is not null);
+            int optimizeAfterFound = (int)(combinedQueue.Count * _explorationMultiplier); //Check another x% of unexplored Paths.
             logger?.LogInformation($"Path found (explored {currentPathLength} Nodes). Optimizing route. (exploring {optimizeAfterFound} additional Nodes)");
-            List<ValueTuple<Node, Node>> newMeetingEnds = new();
+            ValueTuple<Node, Node>? newMeetingEnds = null;
             while (optimizeAfterFound-- > 0 && combinedQueue.Count > 0)
             {
                 ulong currentNodeId = combinedQueue.Dequeue();
@@ -147,7 +146,7 @@ namespace astar
                     if (neighborNode.PreviousIsFromStart is not null &&
                         neighborNode.PreviousIsFromStart != fromStart) //Check if we found the opposite End
                     {
-                        newMeetingEnds.Add(fromStart ? new(currentNode, neighborNode) : new(neighborNode, currentNode));
+                        newMeetingEnds = fromStart ? new(currentNode, neighborNode) : new(neighborNode, currentNode);
                     }
 
                     float metric = (currentNode.Metric ?? float.MaxValue) + (pathing is PathMeasure.Distance