Merge branch 'refs/heads/ExploreBothSidesEqually'

Fix Priority Angle rating

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, importPath, logger);
+Route route = Astar.FindPath(startLat, startLon, endLat, endLon, regionSize, true, importFolderPath: importPath, logger:  logger);
 if(route.RouteFound)
     Console.WriteLine(route);
 else

astar/Astar.cs

@@ -1,5 +1,4 @@
 using astar.PathingHelper;
-using Graph;
 using Microsoft.Extensions.Logging;
 using Graph.Utils;
 using OSM_Graph.Enums;
@@ -9,7 +8,7 @@ namespace astar
 {
     public static class Astar
     {
-        public static Route FindPath(float startLat, float startLon, float endLat, float endLon, float regionSize, bool car = true, string? importFolderPath = 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)
         {
             RegionLoader rl = new(regionSize, importFolderPath, logger: logger);
@@ -19,12 +18,12 @@ namespace astar
             KeyValuePair<ulong, Node> startNode = graph.ClosestNodeToCoordinates(startLat, startLon, car);
             startNode.Value.PreviousIsFromStart = true;
             startNode.Value.PreviousNodeId = startNode.Key;
-            startNode.Value.Distance = 0f;
+            startNode.Value.Metric = 0f;
             
             KeyValuePair<ulong, Node> endNode = graph.ClosestNodeToCoordinates(endLat, endLon, car);
             endNode.Value.PreviousIsFromStart = false;
             endNode.Value.PreviousNodeId = endNode.Key;
-            endNode.Value.Distance = 0f;
+            endNode.Value.Metric = 0f;
 
             double totalDistance = NodeUtils.DistanceBetween(startNode.Value, endNode.Value);
             PriorityHelper priorityHelper = new(totalDistance, SpeedHelper.GetMaxSpeed(car));
@@ -40,86 +39,76 @@ namespace astar
 
             while (toVisitStart.Count > 0 && toVisitEnd.Count > 0)
             {
-                ulong currentNodeStartId = toVisitStart.Dequeue();
-                Node currentNodeStart = graph.Nodes[currentNodeStartId];
-                foreach ((ulong neighborId, KeyValuePair<ulong, bool> wayId) in currentNodeStart.Neighbors)
+                Route? route = null;
+                if (toVisitStart.Count >= toVisitEnd.Count && route is null)
                 {
-                    if (!graph.ContainsNode(neighborId))
-                        graph.ConcatGraph(Graph.FromGraph(rl.LoadRegionFromNodeId(neighborId)));
-                    if (!graph.ContainsWay(wayId.Key))
+                    for(int i = 0; i < toVisitStart.Count / 10 && route is null; i++)
+                        route = ExploreSide(true, graph, toVisitStart, rl, priorityHelper, endNode.Value, car, 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)
                 {
-                        foreach (global::Graph.Graph? g in rl.LoadRegionsFromWayId(wayId.Key))
-                            graph.ConcatGraph(Graph.FromGraph(g));
+                    for(int i = 0; i < toVisitEnd.Count / 10 && route is null; i++)
+                        route = ExploreSide(false, graph, toVisitEnd, rl, priorityHelper, startNode.Value, car, pathing, logger);
                 }
+                if(route is null)
+                    route = ExploreSide(false, graph, toVisitEnd, rl, priorityHelper, startNode.Value, car, pathing, logger);
 
-                    OSM_Graph.Way way = graph.Ways[wayId.Key];
-                    byte speed = SpeedHelper.GetSpeed(way, car);
-                    if(speed < 1)
-                        continue;
-                    if(wayId.Value && way.GetDirection() == WayDirection.Forwards && car)
-                        continue;
-                    if(!wayId.Value && way.GetDirection() == WayDirection.Backwards && car)
-                        continue;
-                    
-                    Node neighborNode = graph.Nodes[neighborId];
-                    
-                    if (neighborNode.PreviousIsFromStart is false)//Check if we found the opposite End
-                        return PathFound(graph, currentNodeStart, neighborNode, car, logger);
-                    
-                    float distance = (currentNodeStart.Distance??float.MaxValue) + (float)currentNodeStart.DistanceTo(neighborNode);
-                    if (neighborNode.PreviousNodeId is null || neighborNode.Distance > distance)
-                    {
-                        neighborNode.PreviousNodeId = currentNodeStartId;
-                        neighborNode.Distance = distance;
-                        neighborNode.PreviousIsFromStart = true;
-                        toVisitStart.Enqueue(neighborId, priorityHelper.CalculatePriority(currentNodeStart, neighborNode, endNode.Value, speed));
-                    }
-                    logger?.LogTrace($"Neighbor {neighborId} {neighborNode}");
-                }
-                
-                ulong currentNodeEndId = toVisitEnd.Dequeue();
-                Node currentNodeEnd = graph.Nodes[currentNodeEndId];
-                foreach ((ulong neighborId, KeyValuePair<ulong, bool> wayId) in currentNodeEnd.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));
-                    }
-                    
-                    OSM_Graph.Way way = graph.Ways[wayId.Key];
-                    byte speed = SpeedHelper.GetSpeed(way, car);
-                    if(speed < 1)
-                        continue;
-                    
-                    if(wayId.Value && way.GetDirection() == WayDirection.Backwards && car)
-                        continue;
-                    if(!wayId.Value && way.GetDirection() == WayDirection.Forwards && car)
-                        continue;
-                    
-                    Node neighborNode = graph.Nodes[neighborId];
-                    
-                    if (neighborNode.PreviousIsFromStart is true)//Check if we found the opposite End
-                        return PathFound(graph, neighborNode, currentNodeEnd, car, logger);
-                    
-                    float distance = (currentNodeEnd.Distance??float.MaxValue) + (float)currentNodeEnd.DistanceTo(neighborNode);
-                    if (neighborNode.PreviousNodeId is null || neighborNode.Distance > distance)
-                    {
-                        neighborNode.PreviousNodeId = currentNodeEndId;
-                        neighborNode.Distance = distance;
-                        neighborNode.PreviousIsFromStart = false;
-                        toVisitEnd.Enqueue(neighborId, priorityHelper.CalculatePriority(currentNodeEnd, neighborNode, startNode.Value, speed));
-                    }
-                    logger?.LogTrace($"Neighbor {neighborId} {neighborNode}");
-                }
-                logger?.LogDebug($"Distance {currentNodeStart.DistanceTo(currentNodeEnd):000000.00}m toVisit-Queues: {toVisitStart.Count} {toVisitStart.UnorderedItems.MinBy(i => i.Priority).Priority} {toVisitEnd.Count} {toVisitEnd.UnorderedItems.MinBy(i => i.Priority).Priority}");
-
+                if (route is not null)
+                    return route;
+                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);
         }
 
+        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)
+        {
+            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));
+                }
+                    
+                OSM_Graph.Way way = graph.Ways[wayId.Key];
+                byte speed = SpeedHelper.GetSpeed(way, car);
+                if(speed < 1)
+                    continue;
+                
+                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);
+
+                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.Metric > metric)
+                {
+                    neighborNode.PreviousNodeId = currentNodeId;
+                    neighborNode.Metric = metric;
+                    neighborNode.PreviousIsFromStart = fromStart;
+                    toVisit.Enqueue(neighborId, priorityHelper.CalculatePriority(currentNode, neighborNode, goalNode, speed));
+                }
+                logger?.LogTrace($"Neighbor {neighborId} {neighborNode}");
+            }
+
+            return null;
+        }
+        
         private static Route PathFound(Graph graph, Node fromStart, Node fromEnd, bool car = true, ILogger? logger = null)
         {
             logger?.LogInformation("Path found!");
@@ -127,7 +116,7 @@ namespace astar
             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)));
             Node current = fromStart;
-            while (current.Distance != 0f)
+            while (current.Metric != 0f)
             {
                 Node previous = graph.Nodes[(ulong)current.PreviousNodeId!];
                 OSM_Graph.Way previousToCurrent = graph.Ways[previous.Neighbors.First(n => graph.Nodes[n.Key] == current).Value.Key];
@@ -137,7 +126,7 @@ namespace astar
             }
             path.Reverse();//Since we go from the middle backwards until here
             current = fromEnd;
-            while (current.Distance != 0f)
+            while (current.Metric != 0f)
             {
                 Node next = graph.Nodes[(ulong)current.PreviousNodeId!];
                 OSM_Graph.Way currentToNext = graph.Ways[current.Neighbors.First(n => graph.Nodes[n.Key] == next).Value.Key];

astar/Node.cs

@@ -3,13 +3,13 @@
 public class Node(float lat, float lon, Dictionary<ulong, KeyValuePair<ulong, bool>>? neighbors = null) : global::Graph.Node(lat, lon, neighbors)
 {
     public ulong? PreviousNodeId = null;
-    public float? Distance = null;
+    public float? Metric = null;
     public bool? PreviousIsFromStart = null;
 
     public static Node FromGraphNode(global::Graph.Node node) => new (node.Lat, node.Lon, node.Neighbors);
 
     public override string ToString()
     {
-        return $"{Lat:00.000000} {Lon:000.000000} Previous {PreviousNodeId} {Distance} {(PreviousIsFromStart is not null ? PreviousIsFromStart.Value ?"Start":"End" : null)}";
+        return $"{Lat:00.000000} {Lon:000.000000} Previous {PreviousNodeId} {Metric} {(PreviousIsFromStart is not null ? PreviousIsFromStart.Value ?"Start":"End" : null)}";
     }
 }

astar/PathingHelper/PathMeasure.cs

@@ -0,0 +1,6 @@
+namespace astar.PathingHelper;
+
+public enum PathMeasure
+{
+    Distance, Time
+}

astar/PathingHelper/PriorityHelper.cs

@@ -1,6 +1,4 @@
-using Graph;
-
-namespace astar.PathingHelper;
+namespace astar.PathingHelper;
 
 public class PriorityHelper(double totalDistance, byte maxSpeed)
 {
@@ -9,17 +7,20 @@ public class PriorityHelper(double totalDistance, byte maxSpeed)
     public int CalculatePriority(Node current, Node neighbor, Node goal, byte speed)
     {
         double neighborDistanceToGoal = neighbor.DistanceTo(goal); //we want this to be small
-        /*double currentDistanceToGoal = current.DistanceTo(goal);
+        double currentDistanceToGoal = current.DistanceTo(goal);
         double currentDistanceToNeighbor = current.DistanceTo(neighbor);
         double angle = //we want this to be small
-            Math.Acos((currentDistanceToGoal * currentDistanceToGoal + currentDistanceToNeighbor +
-                          currentDistanceToNeighbor - neighborDistanceToGoal * neighborDistanceToGoal) / (2 *
-                      currentDistanceToGoal * currentDistanceToNeighbor));*/
+            double.RadiansToDegrees(
+                Math.Acos((currentDistanceToGoal * currentDistanceToGoal +
+                    currentDistanceToNeighbor * currentDistanceToNeighbor -
+                    neighborDistanceToGoal * neighborDistanceToGoal) /
+                          (2 * currentDistanceToGoal * currentDistanceToNeighbor)));
 
-        double distanceRating = 100 - neighborDistanceToGoal / _totalDistance * 100;
-        //double angleRating = Math.Abs((180 - angle) / 180) * 100;
+        //double distanceRating = 100 - neighborDistanceToGoal / _totalDistance * 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;
 
-        return 300 - (int)(distanceRating * 2 + speedRating * 1);
+        return (int)-(speedRating + angleRating * 1.4);
     }
 }

astar/PathingHelper/SpeedHelper.cs

@@ -1,5 +1,4 @@
-using Graph;
-using OSM_Graph.Enums;
+using OSM_Graph.Enums;
 
 namespace astar.PathingHelper;