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

Compare commits

base: glax:master
Branches Tags
glax:master
glax:different-optimization-strategy
glax:Jump-from-Junction-to-Junction
glax:Let-ends-find-each-other
glax:KeepOtimizing
glax:ExploreBothSidesEqually
..
compare: glax:Let-ends-find-each-other
Branches Tags
glax:master
glax:different-optimization-strategy
glax:Jump-from-Junction-to-Junction
glax:Let-ends-find-each-other
glax:KeepOtimizing
glax:ExploreBothSidesEqually

No commits in common. "master" and "Let-ends-find-each-other" have entirely different histories.
master ... Let-ends-f

3 changed files with 67 additions and 106 deletions
Show Stats Expand all files Collapse all files

161
astar/Astar.cs
View File

@ -6,16 +6,16 @@ using OSM_Regions;
namespace astar
{
public class Astar(ValueTuple<float, float, float, float>? priorityWeights = null, int? explorationMultiplier = null, float? nonPriorityRoadSpeedPenalty = null, RegionLoader? regionLoader = null)
public class Astar(ValueTuple<float, float, float, float>? priorityWeights = null, ValueTuple<float, float, float, float>? optimizingWeights = null, int? explorationDistance = null, int? explorationMultiplier = 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;
private readonly ValueTuple<float, float, float, float> DefaultPriorityWeights = priorityWeights ?? new(0.75f, 1f, 0.1f, 0);
private readonly ValueTuple<float, float, float, float> OptimizingWeights = optimizingWeights ?? new(0, 0.07f, 0, 0);
private int ExplorationDistanceFromRoute = explorationDistance ?? 1200;
private int ExplorationMultiplier = explorationMultiplier ?? 65;
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)
public Route FindPath(float startLat, float startLon, float endLat, float endLon, float regionSize, bool car = true, PathMeasure pathing = PathMeasure.Distance, float additionalExploration = 3, string? importFolderPath = null, ILogger? logger = null)
{
rl ??= new(regionSize, importFolderPath, logger: logger);
RegionLoader rl = new(regionSize, importFolderPath, logger: logger);
Graph graph = Spiral(rl, startLat, startLon, regionSize);
Graph endRegion = Spiral(rl, endLat, endLon, regionSize);
graph.ConcatGraph(endRegion);
@ -44,19 +44,27 @@ namespace astar
ValueTuple<Node, Node>? meetingEnds = null;
while (toVisitStart.Count > 0 && toVisitEnd.Count > 0)
{
for (int i = 0; i < Math.Min(toVisitStart.Count * 0.5, 50) && meetingEnds is null; i++)
ulong closestEndNodeId = toVisitEnd.UnorderedItems
.MinBy(n => graph.Nodes[n.Element].DistanceTo(startNode.Value)).Element;
Node closestEndNode = graph.Nodes[closestEndNodeId];
if (toVisitStart.Count >= toVisitEnd.Count && meetingEnds is null)
{
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);
for(int i = 0; i < Math.Min(toVisitStart.Count * 0.5, 50) && meetingEnds is null; i++)
meetingEnds = ExploreSide(true, graph, toVisitStart, rl, priorityHelper, closestEndNode, car, DefaultPriorityWeights, pathing, logger);
}
if(meetingEnds is null)
meetingEnds = ExploreSide(true, graph, toVisitStart, rl, priorityHelper, closestEndNode, car, DefaultPriorityWeights, pathing, logger);
for (int i = 0; i < Math.Min(toVisitEnd.Count * 0.5, 50) && meetingEnds is null; i++)
ulong closestStartNodeId = toVisitStart.UnorderedItems
.MinBy(n => graph.Nodes[n.Element].DistanceTo(endNode.Value)).Element;
Node closestStartNode = graph.Nodes[closestStartNodeId];
if (toVisitEnd.Count >= toVisitStart.Count && meetingEnds is null)
{
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);
for(int i = 0; i < Math.Min(toVisitEnd.Count * 0.5, 50) && meetingEnds is null; i++)
meetingEnds = ExploreSide(false, graph, toVisitEnd, rl, priorityHelper, closestStartNode, car, DefaultPriorityWeights, pathing, logger);
}
if(meetingEnds is null)
meetingEnds = ExploreSide(false, graph, toVisitEnd, rl, priorityHelper, closestStartNode, car, DefaultPriorityWeights, pathing, logger);
if (meetingEnds is not null)
break;
@ -65,44 +73,52 @@ namespace astar
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)
List<Node> routeNodes = PathFound(graph, meetingEnds.Value.Item1, meetingEnds.Value.Item2, car).Steps.Select(s => s.Node1).ToList();
Dictionary<ulong, int> routeQueue = toVisitStart.UnorderedItems.Select(l => l.Element).Union(toVisitEnd.UnorderedItems.Select(l => l.Element)).Where(id =>
{
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));
Node p = graph.Nodes[id];
return routeNodes.Any(route => route.DistanceTo(p) < ExplorationDistanceFromRoute);
}).ToDictionary(id => id, _ => int.MinValue);
PriorityQueue<ulong, int> combinedQueue = new();
foreach ((ulong key, int value) in routeQueue)
combinedQueue.Enqueue(key, value);
ValueTuple<Node, Node>? newMeetingEnds = Optimize(additionalExploration, graph, combinedQueue, car, rl, priorityHelper, pathing, startNode.Value, endNode.Value, logger);
meetingEnds = newMeetingEnds ?? meetingEnds;
return routes.MinBy(route =>
{
if (pathing is PathMeasure.Distance)
return route.Distance;
return route.Time.Ticks;
})!;
return PathFound(graph, meetingEnds!.Value.Item1, meetingEnds.Value.Item2, car, logger);
}
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)
private static ValueTuple<Node, Node>? ExploreSide(bool fromStart, Graph graph, PriorityQueue<ulong, int> toVisit, RegionLoader rl, PriorityHelper priorityHelper, Node goalNode, bool car, ValueTuple<float,float,float,float> ratingWeights, 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)
{
LoadNeighbor(graph, neighborId, wayId.Key, rl!, logger);
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(!IsNeighborReachable(speed, wayId.Value, fromStart, way, car))
if(speed < 1)
continue;
if(!way.AccessPermitted())
continue;
if(wayId.Value && way.GetDirection() == (fromStart ? WayDirection.Backwards : WayDirection.Forwards) && car)
continue;
if(!wayId.Value && way.GetDirection() == (fromStart ? WayDirection.Forwards : WayDirection.Backwards) && car)
continue;
if (car && !way.IsPriorityRoad())
speed = (byte)(speed * _nonPriorityRoadSpeedPenalty);
Node neighborNode = graph.Nodes[neighborId];
if (neighborNode.PreviousIsFromStart == !fromStart) //Check if we found the opposite End
if (neighborNode.PreviousIsFromStart is not null &&
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
@ -116,57 +132,26 @@ namespace astar
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)
private ValueTuple<Node, Node>? Optimize(float additionalExploration, Graph graph, PriorityQueue<ulong, int> combinedQueue, bool car, RegionLoader rl, PriorityHelper priorityHelper, PathMeasure pathing, Node startNode, Node goalNode, ILogger? logger = null)
{
int currentPathLength = graph.Nodes.Values.Count(node => node.PreviousNodeId is not null);
int optimizeAfterFound = (int)(combinedQueue.Count * additionalExploration); //Check another x% of unexplored Paths.
logger?.LogInformation($"Path found (explored {currentPathLength} Nodes). Optimizing route. (exploring {optimizeAfterFound} additional Nodes)");
List<ValueTuple<Node, Node>> newMeetingEnds = new();
ValueTuple<Node, Node>? newMeetingEnds = null;
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}");
}
ulong nodeId = combinedQueue.Peek();
Node node = graph.Nodes[nodeId];
bool fromStart = (bool)node.PreviousIsFromStart!;
newMeetingEnds = ExploreSide(fromStart, graph, combinedQueue, rl, priorityHelper, fromStart ? goalNode : startNode, car, OptimizingWeights, pathing, logger);
}
logger?.LogDebug($"Nodes in Queue after Optimization: {combinedQueue.Count}");
return newMeetingEnds;
}
@ -202,32 +187,6 @@ 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));

8
astar/PathingHelper/PriorityHelper.cs
View File

@ -2,6 +2,8 @@
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, ValueTuple<float, float, float, float> ratingWeights)
{
double neighborDistanceToGoal = neighbor.DistanceTo(goal); //we want this to be small
@ -14,10 +16,10 @@ public class PriorityHelper(double totalDistance, byte maxSpeed)
neighborDistanceToGoal * neighborDistanceToGoal) /
(2 * currentDistanceToGoal * currentDistanceToNeighbor)));
double speedRating = speed * 1.0 / maxSpeed * 100;
double speedRating = speed * 1.0 / _maxSpeed * 100;
double angleRating = 100 - (angle < 180 ? angle / 180 : (360 - angle) / 180) * 100;
double distanceImprovedRating = 100 - (neighborDistanceToGoal - currentDistanceToGoal ) / totalDistance * 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 * ratingWeights.Item1 + angleRating * ratingWeights.Item2 + distanceImprovedRating * ratingWeights.Item3 + distanceSpeedRating * ratingWeights.Item4);
}

4
astar/PathingHelper/SpeedHelper.cs
View File

@ -18,7 +18,7 @@ internal static class SpeedHelper
return car ? SpeedCar.MaxBy(s => s.Value).Value : SpeedPedestrian.MaxBy(s => s.Value).Value;
}
private static readonly Dictionary<HighwayType, byte> SpeedPedestrian = new() {
private static Dictionary<HighwayType, byte> SpeedPedestrian = new() {
{ HighwayType.NONE, 0 },
{ HighwayType.motorway, 0 },
{ HighwayType.trunk, 0 },
@ -50,7 +50,7 @@ internal static class SpeedHelper
{ HighwayType.construction, 0 }
};
private static readonly Dictionary<HighwayType, byte> SpeedCar = new() {
private static Dictionary<HighwayType, byte> SpeedCar = new() {
{ HighwayType.NONE, 0 },
{ HighwayType.motorway, 120 },
{ HighwayType.trunk, 80 },