Fix Optimization new Routes.

For optimization, include nodes withing x meters from route

Executable/Program.cs

@@ -61,7 +61,7 @@ if (arguments.TryGetValue(pathArg, out string[]? pathValue))
     converter.SplitOsmExportIntoRegionFiles(pathValue[0]);
 }
 
-Route route = Astar.FindPath(startLat, startLon, endLat, endLon, regionSize, true, importFolderPath: importPath, logger:  logger);
+Route route = new Astar().FindPath(startLat, startLon, endLat, endLon, regionSize, true, importFolderPath: importPath, logger:  logger);
 if(route.RouteFound)
     Console.WriteLine(route);
 else

astar/Astar.cs

@@ -6,12 +6,16 @@ using OSM_Regions;
 
 namespace astar
 {
-    public static class Astar
+    public class Astar(ValueTuple<float, float, float, float>? priorityWeights = null, int? explorationMultiplier = null, float? nonPriorityRoadSpeedPenalty = null, RegionLoader? regionLoader = null)
     {
-        public static 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)
+        private readonly ValueTuple<float, float, float, float> DefaultPriorityWeights = priorityWeights ?? new(0.6f, 1.05f, 0, 0);
+        private readonly int _explorationMultiplier = explorationMultiplier ?? 120;
+        private readonly float _nonPriorityRoadSpeedPenalty = nonPriorityRoadSpeedPenalty ?? 0.85f;
+        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)
         {
-            RegionLoader rl = new(regionSize, importFolderPath, logger: logger);
+            rl ??= new(regionSize, importFolderPath, logger: logger);
             Graph graph = Spiral(rl, startLat, startLon, regionSize);
             Graph endRegion = Spiral(rl, endLat, endLon, regionSize);
             graph.ConcatGraph(endRegion);
@@ -26,7 +30,7 @@ namespace astar
             endNode.Value.Metric = 0f;
 
             double totalDistance = NodeUtils.DistanceBetween(startNode.Value, endNode.Value);
-            PriorityHelper priorityHelper = new(totalDistance, SpeedHelper.GetMaxSpeed(car));
+            PriorityHelper priorityHelper = new(totalDistance, SpeedHelper.GetTheoreticalMaxSpeed(car));
 
             logger?.Log(LogLevel.Information,
                 "From {0:00.00000}#{1:000.00000} to {2:00.00000}#{3:000.00000} Great-Circle {4:00000.00}km",
@@ -37,61 +41,69 @@ namespace astar
             PriorityQueue<ulong, int> toVisitEnd = new();
             toVisitEnd.Enqueue(endNode.Key, 0);
 
+            ValueTuple<Node, Node>? meetingEnds = null;
             while (toVisitStart.Count > 0 && toVisitEnd.Count > 0)
             {
-                Route? route = null;
-                if (toVisitStart.Count >= toVisitEnd.Count && route is null)
+                for (int i = 0; i < Math.Min(toVisitStart.Count * 0.5, 50) && meetingEnds is null; i++)
                 {
-                    for(int i = 0; i < toVisitStart.Count / 10 && route is null; i++)
-                        route = ExploreSide(true, graph, toVisitStart, rl, priorityHelper, endNode.Value, car, pathing, logger);
+                    ulong closestEndNodeId = toVisitEnd.UnorderedItems.MinBy(node => graph.Nodes[node.Element].DistanceTo(graph.Nodes[toVisitStart.Peek()])).Element;
+                    Node closestEndNode = graph.Nodes[closestEndNodeId];
+                    meetingEnds = ExploreSide(true, graph, toVisitStart, priorityHelper, closestEndNode, car, DefaultPriorityWeights, pathing, logger);
                 }
-                if(route is null)
-                    route = ExploreSide(true, graph, toVisitStart, rl, priorityHelper, endNode.Value, car, pathing, logger);
                 
-                if (toVisitEnd.Count >= toVisitStart.Count && route is null)
+                for (int i = 0; i < Math.Min(toVisitEnd.Count * 0.5, 50) && meetingEnds is null; i++)
                 {
-                    for(int i = 0; i < toVisitEnd.Count / 10 && route is null; i++)
-                        route = ExploreSide(false, graph, toVisitEnd, rl, priorityHelper, startNode.Value, car, pathing, logger);
+                    ulong closestStartNodeId = toVisitStart.UnorderedItems.MinBy(node => graph.Nodes[node.Element].DistanceTo(graph.Nodes[toVisitEnd.Peek()])).Element;
+                    Node closestStartNode = graph.Nodes[closestStartNodeId];
+                    meetingEnds = ExploreSide(false, graph, toVisitEnd, priorityHelper, closestStartNode, car, DefaultPriorityWeights, pathing, logger);
                 }
-                if(route is null)
-                    route = ExploreSide(false, graph, toVisitEnd, rl, priorityHelper, startNode.Value, car, pathing, logger);
 
-                if (route is not null)
-                    return route;
+                if (meetingEnds is not null)
+                    break;
                 logger?.LogDebug($"toVisit-Queues: {toVisitStart.Count} {toVisitStart.UnorderedItems.MinBy(i => i.Priority).Priority} {toVisitEnd.Count} {toVisitEnd.UnorderedItems.MinBy(i => i.Priority).Priority}");
             }
-            return new Route(graph, Array.Empty<Step>().ToList(), false);
+            if(meetingEnds is null)
+                return new Route(graph, Array.Empty<Step>().ToList(), false);
+
+            Queue<ulong> routeQueue = new();
+            toVisitStart.EnqueueRange(toVisitEnd.UnorderedItems);
+            while(toVisitStart.Count > 0)
+            {
+                routeQueue.Enqueue(toVisitStart.Dequeue());
+            }
+            int optimizeAfterFound = graph.Nodes.Count(n => n.Value.PreviousNodeId is not null) * _explorationMultiplier; //Check another x% of unexplored Paths.
+            List<ValueTuple<Node, Node>> newMeetingEnds = Optimize(graph, routeQueue, optimizeAfterFound, car, rl, pathing, logger);
+            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 =>
+            {
+                if (pathing is PathMeasure.Distance)
+                    return route.Distance;
+                return route.Time.Ticks;
+            })!;
         }
 
-        private static Route? ExploreSide(bool fromStart, Graph graph, PriorityQueue<ulong, int> toVisit, RegionLoader rl, PriorityHelper priorityHelper, Node goalNode, bool car, PathMeasure pathing = PathMeasure.Distance,  ILogger? logger = null)
+        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)
         {
             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)
             {
-                if (!graph.ContainsNode(neighborId))
-                    graph.ConcatGraph(Graph.FromGraph(rl.LoadRegionFromNodeId(neighborId)));
-                if (!graph.ContainsWay(wayId.Key))
-                {
-                    foreach (global::Graph.Graph? g in rl.LoadRegionsFromWayId(wayId.Key))
-                        graph.ConcatGraph(Graph.FromGraph(g));
-                }
+                LoadNeighbor(graph, neighborId, wayId.Key, rl!, logger);
                     
                 OSM_Graph.Way way = graph.Ways[wayId.Key];
                 byte speed = SpeedHelper.GetSpeed(way, car);
-                if(speed < 1)
+                if(!IsNeighborReachable(speed, wayId.Value, fromStart, way, car))
                     continue;
+                if (car && !way.IsPriorityRoad())
+                    speed = (byte)(speed * _nonPriorityRoadSpeedPenalty);
                 
-                if(wayId.Value && way.GetDirection() == (fromStart ? WayDirection.Forwards : WayDirection.Backwards) && car)
-                    continue;
-                if(!wayId.Value && way.GetDirection() == (fromStart ? WayDirection.Backwards : WayDirection.Forwards) && car)
-                    continue;
-                    
                 Node neighborNode = graph.Nodes[neighborId];
-                    
-                if (neighborNode.PreviousIsFromStart is not null && neighborNode.PreviousIsFromStart != fromStart)//Check if we found the opposite End
-                    return fromStart ? PathFound(graph, currentNode, neighborNode, car, logger) : PathFound(graph, neighborNode, currentNode, car, logger);
+
+                if (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
                     ? (float)currentNode.DistanceTo(neighborNode)
@@ -101,17 +113,67 @@ namespace astar
                     neighborNode.PreviousNodeId = currentNodeId;
                     neighborNode.Metric = metric;
                     neighborNode.PreviousIsFromStart = fromStart;
-                    toVisit.Enqueue(neighborId, priorityHelper.CalculatePriority(currentNode, neighborNode, goalNode, speed));
+                    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)
+        {
+            int currentPathLength = graph.Nodes.Values.Count(node => node.PreviousNodeId is not null);
+            logger?.LogInformation($"Path found (explored {currentPathLength} Nodes). Optimizing route. (exploring {optimizeAfterFound} additional Nodes)");
+            List<ValueTuple<Node, Node>> newMeetingEnds = new();
+            while (optimizeAfterFound-- > 0 && combinedQueue.Count > 0)
+            {
+                ulong currentNodeId = combinedQueue.Dequeue();
+                Node currentNode = graph.Nodes[currentNodeId];
+                bool fromStart = (bool)currentNode.PreviousIsFromStart!;
+                foreach ((ulong neighborId, KeyValuePair<ulong, bool> wayId) in currentNode.Neighbors)
+                {
+                    LoadNeighbor(graph, neighborId, wayId.Key, rl, logger);
+                    
+                    OSM_Graph.Way way = graph.Ways[wayId.Key];
+                    byte speed = SpeedHelper.GetSpeed(way, car);
+                    if(!IsNeighborReachable(speed, wayId.Value, fromStart, way, car))
+                        continue;
+                    if (car && !way.IsPriorityRoad())
+                        speed = (byte)(speed * _nonPriorityRoadSpeedPenalty);
+                    
+                    Node neighborNode = graph.Nodes[neighborId];
+
+                    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));
+                    }
+
+                    float metric = (currentNode.Metric ?? float.MaxValue) + (pathing is PathMeasure.Distance
+                        ? (float)currentNode.DistanceTo(neighborNode)
+                        : (float)currentNode.DistanceTo(neighborNode) / speed);
+                    if (neighborNode.PreviousNodeId is null || (neighborNode.PreviousIsFromStart == fromStart && neighborNode.Metric > metric))
+                    {
+                        neighborNode.PreviousNodeId = currentNodeId;
+                        neighborNode.Metric = metric;
+                        neighborNode.PreviousIsFromStart = fromStart;
+                        combinedQueue.Enqueue(neighborId);
+                    }
+                    logger?.LogTrace($"Neighbor {neighborId} {neighborNode}");
+                    logger?.LogDebug($"Optimization Contingent: {optimizeAfterFound}/{combinedQueue.Count}");
+                }
+            }
+            
+            logger?.LogDebug($"Nodes in Queue after Optimization: {combinedQueue.Count}");
+
+            return newMeetingEnds;
+        }
         
         private static Route PathFound(Graph graph, Node fromStart, Node fromEnd, bool car = true, ILogger? logger = null)
         {
             logger?.LogInformation("Path found!");
+            
             List<Step> path = new();
             OSM_Graph.Way toNeighbor = graph.Ways[fromStart.Neighbors.First(n => graph.Nodes[n.Key] == fromEnd).Value.Key];
             path.Add(new Step(fromStart, fromEnd, (float)fromStart.DistanceTo(fromEnd), SpeedHelper.GetSpeed(toNeighbor, car)));
@@ -140,6 +202,32 @@ namespace astar
             return r;
         }
 
+        private static bool IsNeighborReachable(byte speed, bool wayDirection, bool fromStart, OSM_Graph.Way way, bool car)
+        {
+            if(speed < 1)
+                return false;
+            if(!way.AccessPermitted())
+                return false;
+            if(wayDirection && way.GetDirection() == (fromStart ? WayDirection.Backwards : WayDirection.Forwards) && car)
+                return false;
+            if(!wayDirection && way.GetDirection() == (fromStart ? WayDirection.Forwards : WayDirection.Backwards) && car)
+                return false;
+            return true;
+        }
+
+        private static void LoadNeighbor(Graph graph, ulong neighborId, ulong wayId, RegionLoader rl, ILogger? logger = null)
+        {
+            
+            if (!graph.ContainsNode(neighborId))
+                graph.ConcatGraph(Graph.FromGraph(rl.LoadRegionFromNodeId(neighborId)));
+            if (!graph.ContainsWay(wayId))
+            {
+                logger?.LogDebug("Loading way... This will be slow.");
+                foreach (global::Graph.Graph? g in rl.LoadRegionsFromWayId(wayId))
+                    graph.ConcatGraph(Graph.FromGraph(g));
+            }
+        }
+
         private static Graph Spiral(RegionLoader loader, float lat, float lon, float regionSize)
         {
             Graph? ret = Graph.FromGraph(loader.LoadRegionFromCoordinates(lat, lon));

astar/PathingHelper/PriorityHelper.cs

@@ -2,9 +2,7 @@
 
 public class PriorityHelper(double totalDistance, byte maxSpeed)
 {
-    private readonly double _totalDistance = totalDistance;
-    private readonly byte _maxSpeed = maxSpeed;
-    public int CalculatePriority(Node current, Node neighbor, Node goal, byte speed)
+    public int CalculatePriority(Node current, Node neighbor, Node goal, byte speed, ValueTuple<float, float, float, float> ratingWeights)
     {
         double neighborDistanceToGoal = neighbor.DistanceTo(goal); //we want this to be small
         double currentDistanceToGoal = current.DistanceTo(goal);
@@ -16,11 +14,11 @@ public class PriorityHelper(double totalDistance, byte maxSpeed)
                     neighborDistanceToGoal * neighborDistanceToGoal) /
                           (2 * currentDistanceToGoal * currentDistanceToNeighbor)));
 
-        //double distanceRating = 100 - neighborDistanceToGoal / _totalDistance * 100;
+        double speedRating = speed * 1.0 / maxSpeed * 100;
         double angleRating = 100 - (angle < 180 ? angle / 180 : (360 - angle) / 180) * 100;
-        double speedRating = speed * 1.0 / _maxSpeed * 100;
-        //double distanceSpeedRating =  ((totalDistance / _maxSpeed) / (neighborDistanceToGoal / speed)) * 100;
+        double distanceImprovedRating = 100 - (neighborDistanceToGoal - currentDistanceToGoal ) / totalDistance * 100;
+        double distanceSpeedRating =  ((totalDistance / maxSpeed) / (neighborDistanceToGoal / speed)) * 100;
 
-        return (int)-(speedRating + angleRating * 1.4);
+        return (int)-(speedRating * ratingWeights.Item1 + angleRating * ratingWeights.Item2 + distanceImprovedRating * ratingWeights.Item3 + distanceSpeedRating * ratingWeights.Item4);
     }
 }

astar/PathingHelper/SpeedHelper.cs

@@ -8,17 +8,17 @@ internal static class SpeedHelper
     {
         byte maxspeed = way.GetMaxSpeed();
         if (maxspeed != 0)
-            return maxspeed;
+            return (byte)(maxspeed * 0.85);
         HighwayType highwayType = way.GetHighwayType();
         return car ? SpeedCar[highwayType] : SpeedPedestrian[highwayType];
     }
 
-    public static byte GetMaxSpeed(bool car = true)
+    public static byte GetTheoreticalMaxSpeed(bool car = true)
     {
         return car ? SpeedCar.MaxBy(s => s.Value).Value : SpeedPedestrian.MaxBy(s => s.Value).Value;
     }
 
-    private static Dictionary<HighwayType, byte> SpeedPedestrian = new() {
+    private static readonly Dictionary<HighwayType, byte> SpeedPedestrian = new() {
         { HighwayType.NONE, 0 },
         { HighwayType.motorway, 0 },
         { HighwayType.trunk, 0 },
@@ -50,20 +50,20 @@ internal static class SpeedHelper
         { HighwayType.construction, 0 }
     };
 
-    private static Dictionary<HighwayType, byte> SpeedCar = new() {
+    private static readonly Dictionary<HighwayType, byte> SpeedCar = new() {
         { HighwayType.NONE, 0 },
-        { HighwayType.motorway, 110 },
+        { HighwayType.motorway, 120 },
         { HighwayType.trunk, 80 },
-        { HighwayType.primary, 80 },
-        { HighwayType.secondary, 80 },
+        { HighwayType.primary, 70 },
+        { HighwayType.secondary, 70 },
         { HighwayType.tertiary, 70 },
         { HighwayType.unclassified, 30 },
         { HighwayType.residential, 10 },
-        { HighwayType.motorway_link, 50 },
+        { HighwayType.motorway_link, 70 },
         { HighwayType.trunk_link, 50 },
         { HighwayType.primary_link, 50 },
-        { HighwayType.secondary_link, 30 },
-        { HighwayType.tertiary_link, 25 },
+        { HighwayType.secondary_link, 50 },
+        { HighwayType.tertiary_link, 40 },
         { HighwayType.living_street, 5 },
         { HighwayType.service, 0 },
         { HighwayType.pedestrian, 0 },

astar/Route.cs

@@ -27,7 +27,7 @@
         {
             return $"{string.Join("\n", Steps)}\n" +
                    $"Distance: {Distance:000000.00}m\n" +
-                   $"Time: {Time:hh\\:m\\:ss}";
+                   $"Time: {Time:hh\\:mm\\:ss}";
         }
     }