From 57a6838f8638639d53d24d262af18566499c5242 Mon Sep 17 00:00:00 2001 From: glax Date: Tue, 23 Jul 2024 19:10:29 +0200 Subject: [PATCH] Switchable between distance and speed metric Fix Priority Angle rating --- Executable/Program.cs | 2 +- astar/Astar.cs | 141 ++++++++++++-------------- astar/Node.cs | 4 +- astar/PathingHelper/PathMeasure.cs | 6 ++ astar/PathingHelper/PriorityHelper.cs | 21 ++-- astar/PathingHelper/SpeedHelper.cs | 3 +- 6 files changed, 86 insertions(+), 91 deletions(-) create mode 100644 astar/PathingHelper/PathMeasure.cs diff --git a/Executable/Program.cs b/Executable/Program.cs index b77f16e..84c50c0 100644 --- a/Executable/Program.cs +++ b/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 diff --git a/astar/Astar.cs b/astar/Astar.cs index f998ea4..a92659a 100644 --- a/astar/Astar.cs +++ b/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 startNode = graph.ClosestNodeToCoordinates(startLat, startLon, car); startNode.Value.PreviousIsFromStart = true; startNode.Value.PreviousNodeId = startNode.Key; - startNode.Value.Distance = 0f; + startNode.Value.Metric = 0f; KeyValuePair 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 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)) - { - 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.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}"); + 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); - ulong currentNodeEndId = toVisitEnd.Dequeue(); - Node currentNodeEnd = graph.Nodes[currentNodeEndId]; - foreach ((ulong neighborId, KeyValuePair wayId) in currentNodeEnd.Neighbors) + if (toVisitEnd.Count >= toVisitStart.Count && route is null) { - 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}"); + for(int i = 0; i < toVisitEnd.Count / 10 && route is null; i++) + route = ExploreSide(false, graph, toVisitEnd, rl, priorityHelper, startNode.Value, car, pathing, logger); } - 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 null) + route = ExploreSide(false, graph, toVisitEnd, rl, priorityHelper, startNode.Value, car, pathing, logger); + 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().ToList(), false); } + private static Route? ExploreSide(bool fromStart, Graph graph, PriorityQueue 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 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]; diff --git a/astar/Node.cs b/astar/Node.cs index 7eec361..3a14b1d 100644 --- a/astar/Node.cs +++ b/astar/Node.cs @@ -3,13 +3,13 @@ public class Node(float lat, float lon, Dictionary>? 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)}"; } } \ No newline at end of file diff --git a/astar/PathingHelper/PathMeasure.cs b/astar/PathingHelper/PathMeasure.cs new file mode 100644 index 0000000..6cb8da7 --- /dev/null +++ b/astar/PathingHelper/PathMeasure.cs @@ -0,0 +1,6 @@ +namespace astar.PathingHelper; + +public enum PathMeasure +{ + Distance, Time +} \ No newline at end of file diff --git a/astar/PathingHelper/PriorityHelper.cs b/astar/PathingHelper/PriorityHelper.cs index def1bf4..560e222 100644 --- a/astar/PathingHelper/PriorityHelper.cs +++ b/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); } } \ No newline at end of file diff --git a/astar/PathingHelper/SpeedHelper.cs b/astar/PathingHelper/SpeedHelper.cs index 5f9a3c9..761f0d5 100644 --- a/astar/PathingHelper/SpeedHelper.cs +++ b/astar/PathingHelper/SpeedHelper.cs @@ -1,5 +1,4 @@ -using Graph; -using OSM_Graph.Enums; +using OSM_Graph.Enums; namespace astar.PathingHelper;