AStar/astar/Astar.cs

178 lines
8.9 KiB
C#
Raw Normal View History

using astar.PathingHelper;
using Graph;
using Microsoft.Extensions.Logging;
2024-07-22 04:56:22 +02:00
using Graph.Utils;
using OSM_Regions;
2022-05-05 02:00:55 +02:00
namespace astar
{
2024-07-22 04:56:22 +02:00
public static class Astar
2022-05-05 02:00:55 +02:00
{
public static Route FindPath(float startLat, float startLon, float endLat, float endLon, float regionSize, bool car = true, string? importFolderPath = null,
2024-07-22 04:56:22 +02:00
ILogger? logger = null)
2022-05-06 00:02:28 +02:00
{
2024-07-22 04:56:22 +02:00
RegionLoader rl = new(regionSize, importFolderPath, logger: logger);
Graph graph = Spiral(rl, startLat, startLon, regionSize);
KeyValuePair<ulong, Node> startNode = graph.ClosestNodeToCoordinates(startLat, startLon, car);
2024-07-22 04:56:22 +02:00
startNode.Value.PreviousIsFromStart = true;
startNode.Value.PreviousNodeId = startNode.Key;
startNode.Value.Distance = 0f;
2024-07-22 04:56:22 +02:00
Graph endRegion = Spiral(rl, endLat, endLon, regionSize);
graph.ConcatGraph(endRegion);
KeyValuePair<ulong, Node> endNode = graph.ClosestNodeToCoordinates(endLat, endLon, car);
2024-07-22 04:56:22 +02:00
endNode.Value.PreviousIsFromStart = false;
endNode.Value.PreviousNodeId = endNode.Key;
endNode.Value.Distance = 0f;
2024-07-22 04:56:22 +02:00
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);
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));
while (toVisitStart.Count > 0 && toVisitEnd.Count > 0)
2022-05-06 00:02:28 +02:00
{
2024-07-22 04:56:22 +02:00
logger?.LogDebug($"Length toVisit-Start: {toVisitStart.Count} -End: {toVisitEnd.Count}");
ulong currentNodeStartId = toVisitStart.Dequeue();
Node currentNodeStart = graph.Nodes[currentNodeStartId];
foreach ((ulong neighborId, ulong wayId) in currentNodeStart.Neighbors)
2022-05-06 00:02:28 +02:00
{
if (!graph.ContainsNode(neighborId))
graph.ConcatGraph(Graph.FromGraph(rl.LoadRegionFromNodeId(neighborId)));
if (!graph.ContainsWay(wayId))
{
foreach (global::Graph.Graph? g in rl.LoadRegionsFromWayId(wayId))
graph.ConcatGraph(Graph.FromGraph(g));
}
Way way = graph.Ways[wayId];
byte speed = SpeedHelper.GetSpeed(way, car);
if(speed < 1)
continue;
Node neighborNode = graph.Nodes[neighborId];
if (neighborNode.PreviousIsFromStart is false)//Check if we found the opposite End
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)
2024-07-22 04:56:22 +02:00
{
neighborNode.PreviousNodeId = currentNodeStartId;
neighborNode.Distance = distance;
neighborNode.PreviousIsFromStart = true;
toVisitStart.Enqueue(neighborId, NodeUtils.DistanceBetween(neighborNode, endNode.Value) / speed);
2024-07-22 04:56:22 +02:00
}
logger?.LogTrace($"Neighbor {neighborId} {neighborNode}");
2022-11-01 05:08:09 +01:00
}
2024-07-22 04:56:22 +02:00
ulong currentNodeEndId = toVisitEnd.Dequeue();
Node currentNodeEnd = graph.Nodes[currentNodeEndId];
foreach ((ulong neighborId, ulong wayId) in currentNodeEnd.Neighbors)
2024-07-22 04:56:22 +02:00
{
if (!graph.ContainsNode(neighborId))
graph.ConcatGraph(Graph.FromGraph(rl.LoadRegionFromNodeId(neighborId)));
if (!graph.ContainsWay(wayId))
{
foreach (global::Graph.Graph? g in rl.LoadRegionsFromWayId(wayId))
graph.ConcatGraph(Graph.FromGraph(g));
}
Way way = graph.Ways[wayId];
byte speed = SpeedHelper.GetSpeed(way, car);
if(speed < 1)
continue;
Node neighborNode = graph.Nodes[neighborId];
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);
if (neighborNode.PreviousIsFromStart is null || neighborNode.Distance > distance && currentNodeEnd.PreviousNodeId != neighborId)
2024-07-22 04:56:22 +02:00
{
neighborNode.PreviousNodeId = currentNodeEndId;
neighborNode.Distance = distance;
neighborNode.PreviousIsFromStart = false;
toVisitEnd.Enqueue(neighborId, NodeUtils.DistanceBetween(neighborNode, startNode.Value) / speed);
2024-07-22 04:56:22 +02:00
}
logger?.LogTrace($"Neighbor {neighborId} {neighborNode}");
2024-07-22 04:56:22 +02:00
}
2022-11-13 14:14:55 +01:00
}
2024-07-22 04:56:22 +02:00
return new Route(graph, Array.Empty<Step>().ToList(), false);
2022-11-13 14:14:55 +01:00
}
private static Route PathFound(Graph graph, Node fromStart, Node fromEnd, ILogger? logger = null)
2022-11-13 14:14:55 +01:00
{
logger?.LogInformation("Path found!");
2024-07-22 04:56:22 +02:00
List<Step> path = new();
path.Add(new Step((float)NodeUtils.DistanceBetween(fromStart, fromEnd), fromStart, fromEnd));
2024-07-22 04:56:22 +02:00
Node current = fromStart;
while (current.Distance != 0f)
2022-11-13 14:14:55 +01:00
{
Step step = new((float)NodeUtils.DistanceBetween(graph.Nodes[(ulong)current.PreviousNodeId!], current), graph.Nodes[(ulong)current.PreviousNodeId!], current);
2024-07-22 04:56:22 +02:00
path.Add(step);
current = graph.Nodes[(ulong)current.PreviousNodeId!];
2022-11-13 14:14:55 +01:00
}
2024-07-22 04:56:22 +02:00
path.Reverse();//Since we go from the middle backwards until here
current = fromEnd;
while (current.Distance != 0f)
2022-11-13 14:14:55 +01:00
{
Step step = new((float)NodeUtils.DistanceBetween(graph.Nodes[(ulong)current.PreviousNodeId!], current), current, graph.Nodes[(ulong)current.PreviousNodeId!]);
2024-07-22 04:56:22 +02:00
path.Add(step);
current = graph.Nodes[(ulong)current.PreviousNodeId!];
2022-11-13 14:14:55 +01:00
}
Route r = new (graph, path, true);
logger?.LogInformation(r.ToString());
return r;
2022-11-13 14:14:55 +01:00
}
2024-07-22 04:56:22 +02:00
private static Graph Spiral(RegionLoader loader, float lat, float lon, float regionSize)
2022-11-13 14:14:55 +01:00
{
2024-07-22 04:56:22 +02:00
Graph? ret = Graph.FromGraph(loader.LoadRegionFromCoordinates(lat, lon));
int iteration = 1;
while (ret is null)
2022-11-13 14:14:55 +01:00
{
2024-07-22 04:56:22 +02:00
for (int x = -iteration; x <= iteration; x++)
{
Graph? g1 = Graph.FromGraph(loader.LoadRegionFromCoordinates(lat + x * regionSize, lon - iteration * regionSize));
Graph? g2 = Graph.FromGraph(loader.LoadRegionFromCoordinates(lat + x * regionSize, lon + iteration * regionSize));
if (ret is not null)
{
ret.ConcatGraph(g1);
ret.ConcatGraph(g2);
}
else if (ret is null && g1 is not null)
{
ret = g1;
ret.ConcatGraph(g2);
}else if (ret is null && g2 is not null)
ret = g2;
}
for (int y = -iteration + 1; y < iteration; y++)
{
Graph? g1 = Graph.FromGraph(loader.LoadRegionFromCoordinates(lat - iteration * regionSize, lon + y * regionSize));
Graph? g2 = Graph.FromGraph(loader.LoadRegionFromCoordinates(lat + iteration * regionSize, lon + y * regionSize));
if (ret is not null)
{
ret.ConcatGraph(g1);
ret.ConcatGraph(g2);
}
else if (ret is null && g1 is not null)
{
ret = g1;
ret.ConcatGraph(g2);
}else if (ret is null && g2 is not null)
ret = g2;
}
iteration++;
2022-11-13 14:14:55 +01:00
}
2024-07-22 04:56:22 +02:00
return ret;
2022-11-13 14:14:55 +01:00
}
2022-05-05 02:00:55 +02:00
}
}