From eabceed59d49dae21c968df4e3ae0c750c11371f Mon Sep 17 00:00:00 2001 From: glax Date: Tue, 23 Jul 2024 04:24:35 +0200 Subject: [PATCH] Fix and Improve PriorityQueue, Distance Calculation for previous --- astar/Astar.cs | 24 +++++++++++++----------- astar/Node.cs | 2 +- astar/PathingHelper/PriorityHelper.cs | 25 +++++++++++++++++++++++++ astar/PathingHelper/SpeedHelper.cs | 26 +++++++++++++++++++------- 4 files changed, 58 insertions(+), 19 deletions(-) create mode 100644 astar/PathingHelper/PriorityHelper.cs diff --git a/astar/Astar.cs b/astar/Astar.cs index c55c271..f44424c 100644 --- a/astar/Astar.cs +++ b/astar/Astar.cs @@ -25,15 +25,17 @@ namespace astar endNode.Value.PreviousNodeId = endNode.Key; endNode.Value.Distance = 0f; + double totalDistance = NodeUtils.DistanceBetween(startNode.Value, endNode.Value); + PriorityHelper priorityHelper = new(totalDistance, SpeedHelper.GetMaxSpeed(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", - startNode.Value.Lat, startNode.Value.Lon, endNode.Value.Lat, endNode.Value.Lon, - NodeUtils.DistanceBetween(startNode.Value, endNode.Value) / 1000); + startNode.Value.Lat, startNode.Value.Lon, endNode.Value.Lat, endNode.Value.Lon, totalDistance / 1000); - PriorityQueue toVisitStart = new(); - toVisitStart.Enqueue(startNode.Key, NodeUtils.DistanceBetween(startNode.Value, endNode.Value)); - PriorityQueue toVisitEnd = new(); - toVisitEnd.Enqueue(endNode.Key, NodeUtils.DistanceBetween(endNode.Value, startNode.Value)); + PriorityQueue toVisitStart = new(); + toVisitStart.Enqueue(startNode.Key, 0); + PriorityQueue toVisitEnd = new(); + toVisitEnd.Enqueue(endNode.Key, 0); while (toVisitStart.Count > 0 && toVisitEnd.Count > 0) { @@ -59,12 +61,12 @@ namespace astar return PathFound(graph, currentNodeStart, neighborNode, logger); float distance = (currentNodeStart.Distance??float.MaxValue) + (float)currentNodeStart.DistanceTo(neighborNode); - if (neighborNode.PreviousNodeId is null || neighborNode.Distance > distance && currentNodeStart.PreviousNodeId != neighborId) + if (neighborNode.PreviousNodeId is null || neighborNode.Distance > distance) { neighborNode.PreviousNodeId = currentNodeStartId; neighborNode.Distance = distance; neighborNode.PreviousIsFromStart = true; - toVisitStart.Enqueue(neighborId, NodeUtils.DistanceBetween(neighborNode, endNode.Value) / speed); + toVisitStart.Enqueue(neighborId, priorityHelper.CalculatePriority(currentNodeStart, neighborNode, endNode.Value, speed)); } logger?.LogTrace($"Neighbor {neighborId} {neighborNode}"); } @@ -90,17 +92,17 @@ namespace astar if (neighborNode.PreviousIsFromStart is true)//Check if we found the opposite End return PathFound(graph, neighborNode, currentNodeEnd, logger); - float distance = (currentNodeStart.Distance??float.MaxValue) + (float)currentNodeStart.DistanceTo(neighborNode); + 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, NodeUtils.DistanceBetween(neighborNode, startNode.Value) / speed); + 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} {toVisitEnd.Count}"); + 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}"); } return new Route(graph, Array.Empty().ToList(), false); diff --git a/astar/Node.cs b/astar/Node.cs index d5bc17e..eee8ac0 100644 --- a/astar/Node.cs +++ b/astar/Node.cs @@ -10,6 +10,6 @@ public class Node(float lat, float lon, Dictionary? neighbors = nu public override string ToString() { - return $"{Lat:00.000000} {Lon:000.000000} Previous {PreviousNodeId} {(PreviousIsFromStart is not null ? PreviousIsFromStart.Value ?"Start":"End" : null)}"; + return $"{Lat:00.000000} {Lon:000.000000} Previous {PreviousNodeId} {Distance} {(PreviousIsFromStart is not null ? PreviousIsFromStart.Value ?"Start":"End" : null)}"; } } \ No newline at end of file diff --git a/astar/PathingHelper/PriorityHelper.cs b/astar/PathingHelper/PriorityHelper.cs new file mode 100644 index 0000000..5560183 --- /dev/null +++ b/astar/PathingHelper/PriorityHelper.cs @@ -0,0 +1,25 @@ +using Graph; + +namespace astar.PathingHelper; + +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) + { + double neighborDistanceToGoal = neighbor.DistanceTo(goal); //we want this to be small + /*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 distanceRating = 100 - neighborDistanceToGoal / _totalDistance * 100; + //double angleRating = Math.Abs((180 - angle) / 180) * 100; + double speedRating = speed * 1.0 / _maxSpeed * 100; + + return 350 - (int)(distanceRating * 2 + speedRating * 1.5); + } +} \ No newline at end of file diff --git a/astar/PathingHelper/SpeedHelper.cs b/astar/PathingHelper/SpeedHelper.cs index 33e24bc..7439d48 100644 --- a/astar/PathingHelper/SpeedHelper.cs +++ b/astar/PathingHelper/SpeedHelper.cs @@ -10,7 +10,19 @@ internal static class SpeedHelper return 0; if (!Enum.TryParse(highwayTypeStr, out HighwayType highwayType)) return 0; - return car ? SpeedCar[highwayType] : SpeedPedestrian[highwayType]; + byte speed = car ? SpeedCar[highwayType] : SpeedPedestrian[highwayType]; + if (speed < 1) + return speed; + if(!way.Tags.TryGetValue("maxspeed", out string? maxSpeedStr)) + return speed; + if (!byte.TryParse(maxSpeedStr, out speed)) + return speed; + return speed; + } + + public static byte GetMaxSpeed(bool car = true) + { + return car ? SpeedCar.MaxBy(s => s.Value).Value : SpeedPedestrian.MaxBy(s => s.Value).Value; } private static Dictionary SpeedPedestrian = new() { @@ -48,25 +60,25 @@ internal static class SpeedHelper private static Dictionary SpeedCar = new() { { HighwayType.NONE, 0 }, { HighwayType.motorway, 110 }, - { HighwayType.trunk, 100 }, + { HighwayType.trunk, 80 }, { HighwayType.primary, 80 }, { HighwayType.secondary, 80 }, { HighwayType.tertiary, 70 }, - { HighwayType.unclassified, 20 }, + { HighwayType.unclassified, 30 }, { HighwayType.residential, 10 }, { HighwayType.motorway_link, 50 }, { HighwayType.trunk_link, 50 }, - { HighwayType.primary_link, 30 }, - { HighwayType.secondary_link, 25 }, + { HighwayType.primary_link, 50 }, + { HighwayType.secondary_link, 30 }, { HighwayType.tertiary_link, 25 }, - { HighwayType.living_street, 10 }, + { HighwayType.living_street, 5 }, { HighwayType.service, 0 }, { HighwayType.pedestrian, 0 }, { HighwayType.track, 0 }, { HighwayType.bus_guideway, 0 }, { HighwayType.escape, 0 }, { HighwayType.raceway, 0 }, - { HighwayType.road, 25 }, + { HighwayType.road, 30 }, { HighwayType.busway, 0 }, { HighwayType.footway, 0 }, { HighwayType.bridleway, 0 },