glax/AStar
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Fix and Improve PriorityQueue, Distance Calculation for previous

Browse Source
This commit is contained in:
glax 2024-07-23 04:24:35 +02:00
parent 0bbda3dfd0
commit eabceed59d
4 changed files with 58 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
astar/Astar.cs
View File

@ -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<ulong, double> toVisitStart = new();
toVisitStart.Enqueue(startNode.Key, NodeUtils.DistanceBetween(startNode.Value, endNode.Value));
PriorityQueue<ulong, double> toVisitEnd = new();
toVisitEnd.Enqueue(endNode.Key, NodeUtils.DistanceBetween(endNode.Value, startNode.Value));
PriorityQueue<ulong, int> toVisitStart = new();
toVisitStart.Enqueue(startNode.Key, 0);
PriorityQueue<ulong, int> 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<Step>().ToList(), false);

2
astar/Node.cs
View File

@ -10,6 +10,6 @@ public class Node(float lat, float lon, Dictionary<ulong, ulong>? 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)}";
}
}

25
astar/PathingHelper/PriorityHelper.cs Normal file
View File

@ -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);
}
}

26
astar/PathingHelper/SpeedHelper.cs
View File

@ -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<HighwayType, byte> SpeedPedestrian = new() {
@ -48,25 +60,25 @@ internal static class SpeedHelper
private static Dictionary<HighwayType, byte> 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 },