Compare commits
7 Commits
1604b150d4
...
793dd68cfd
Author | SHA1 | Date | |
---|---|---|---|
793dd68cfd | |||
a4dd0e1abd | |||
36c326b71b | |||
032cce4552 | |||
62d527737a | |||
25069630a7 | |||
cb2510a2a1 |
118
astar/Astar.cs
118
astar/Astar.cs
@ -6,12 +6,11 @@ using OSM_Regions;
|
||||
|
||||
namespace astar
|
||||
{
|
||||
public class Astar(ValueTuple<float, float, float, float>? priorityWeights = null, ValueTuple<float, float, float, float>? optimizingWeights = null, int? explorationDistance = null, int? explorationMultiplier = null)
|
||||
public class Astar(ValueTuple<float, float, float, float>? priorityWeights = null, int? explorationMultiplier = null, float? nonPriorityRoadSpeedPenalty = null)
|
||||
{
|
||||
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 readonly int _explorationDistanceFromRoute = explorationDistance ?? 1200;
|
||||
private readonly int _explorationMultiplier = explorationMultiplier ?? 65;
|
||||
private readonly ValueTuple<float, float, float, float> DefaultPriorityWeights = priorityWeights ?? new(0.7f, 1.08f, 0, 0);
|
||||
private readonly int _explorationMultiplier = explorationMultiplier ?? 150;
|
||||
private readonly float _nonPriorityRoadSpeedPenalty = nonPriorityRoadSpeedPenalty ?? 0.9f;
|
||||
|
||||
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)
|
||||
{
|
||||
@ -52,8 +51,6 @@ namespace astar
|
||||
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);
|
||||
|
||||
ulong closestStartNodeId = toVisitStart.UnorderedItems
|
||||
.MinBy(n => graph.Nodes[n.Element].DistanceTo(endNode.Value)).Element;
|
||||
@ -63,8 +60,6 @@ namespace astar
|
||||
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;
|
||||
@ -73,50 +68,34 @@ namespace astar
|
||||
if(meetingEnds is null)
|
||||
return new Route(graph, Array.Empty<Step>().ToList(), false);
|
||||
|
||||
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 =>
|
||||
Queue<ulong> routeQueue = new();
|
||||
foreach (ulong id in toVisitStart.UnorderedItems.Select(l => l.Element)
|
||||
.Union(toVisitEnd.UnorderedItems.Select(l => l.Element)))
|
||||
{
|
||||
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(graph, combinedQueue, car, rl, priorityHelper, pathing, startNode.Value, endNode.Value, logger);
|
||||
routeQueue.Enqueue(id);
|
||||
}
|
||||
ValueTuple<Node, Node>? newMeetingEnds = Optimize(graph, routeQueue, car, rl, pathing, logger);
|
||||
meetingEnds = newMeetingEnds ?? meetingEnds;
|
||||
|
||||
return PathFound(graph, meetingEnds!.Value.Item1, meetingEnds.Value.Item2, car, logger);
|
||||
}
|
||||
|
||||
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)
|
||||
private 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, 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));
|
||||
}
|
||||
LoadNeighbor(graph, neighborId, wayId.Key, rl, logger);
|
||||
|
||||
OSM_Graph.Way way = graph.Ways[wayId.Key];
|
||||
byte speed = SpeedHelper.GetSpeed(way, car);
|
||||
if(speed < 1)
|
||||
continue;
|
||||
if(!way.AccessPermitted())
|
||||
if(!IsNeighborReachable(speed, wayId.Value, fromStart, way, car))
|
||||
continue;
|
||||
if (car && !way.IsPriorityRoad())
|
||||
speed = (byte)(speed * 0.75f);
|
||||
speed = (byte)(speed * _nonPriorityRoadSpeedPenalty);
|
||||
|
||||
if(wayId.Value && way.GetDirection() == (fromStart ? WayDirection.Backwards : WayDirection.Forwards) && car)
|
||||
continue;
|
||||
if(!wayId.Value && way.GetDirection() == (fromStart ? WayDirection.Forwards : WayDirection.Backwards) && car)
|
||||
continue;
|
||||
|
||||
Node neighborNode = graph.Nodes[neighborId];
|
||||
|
||||
if (neighborNode.PreviousIsFromStart is not null &&
|
||||
@ -140,7 +119,7 @@ namespace astar
|
||||
return null;
|
||||
}
|
||||
|
||||
private ValueTuple<Node, Node>? Optimize(Graph graph, PriorityQueue<ulong, int> combinedQueue, bool car, RegionLoader rl, PriorityHelper priorityHelper, PathMeasure pathing, Node startNode, Node goalNode, ILogger? logger = null)
|
||||
private ValueTuple<Node, Node>? Optimize(Graph graph, Queue<ulong> combinedQueue, bool car, RegionLoader rl, PathMeasure pathing, ILogger? logger = null)
|
||||
{
|
||||
int currentPathLength = graph.Nodes.Values.Count(node => node.PreviousNodeId is not null);
|
||||
int optimizeAfterFound = (int)(combinedQueue.Count * _explorationMultiplier); //Check another x% of unexplored Paths.
|
||||
@ -148,11 +127,44 @@ namespace astar
|
||||
ValueTuple<Node, Node>? newMeetingEnds = null;
|
||||
while (optimizeAfterFound-- > 0 && combinedQueue.Count > 0)
|
||||
{
|
||||
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);
|
||||
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 = 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}");
|
||||
}
|
||||
}
|
||||
|
||||
logger?.LogDebug($"Nodes in Queue after Optimization: {combinedQueue.Count}");
|
||||
|
||||
return newMeetingEnds;
|
||||
}
|
||||
@ -189,6 +201,32 @@ 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));
|
||||
|
Loading…
Reference in New Issue
Block a user