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

Split Node.Previous into two fields for readability

Browse Source 
Add ToString to Route
Simplify algorithm
MinCoordinates and MaxCoordinates returns graph min/max always
This commit is contained in:
glax 2024-07-23 02:21:20 +02:00
parent 41b73081a5
commit f21845ced4
4 changed files with 67 additions and 167 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
Executable/Program.cs
View File

@ -63,12 +63,11 @@ if (arguments.TryGetValue(pathArg, out string[]? pathValue))
Route route = Astar.FindPath(startLat, startLon, endLat, endLon, regionSize, importPath, logger); Route route = Astar.FindPath(startLat, startLon, endLat, endLon, regionSize, importPath, logger);
if(route.RouteFound) if(route.RouteFound)
Console.WriteLine($"{string.Join("\n", route.Steps)}\n" + Console.WriteLine(route);
$"Distance: {route.Distance:000000.00}m");
else else
Console.WriteLine("No route found."); Console.WriteLine("No route found.");
Console.WriteLine($"Visited Nodes: {route.Graph.Nodes.Values.Count(node => node.Previous is not null)}"); Console.WriteLine($"Visited Nodes: {route.Graph.Nodes.Values.Count(node => node.PreviousNodeId is not null)}");
void PrintUsage() void PrintUsage()

178
astar/Astar.cs
View File

@ -13,13 +13,15 @@ namespace astar
Graph graph = Spiral(rl, startLat, startLon, regionSize); Graph graph = Spiral(rl, startLat, startLon, regionSize);
KeyValuePair<ulong, Node> startNode = graph.ClosestNodeToCoordinates(startLat, startLon); KeyValuePair<ulong, Node> startNode = graph.ClosestNodeToCoordinates(startLat, startLon);
startNode.Value.PreviousIsFromStart = true; startNode.Value.PreviousIsFromStart = true;
startNode.Value.Previous = new KeyValuePair<ulong, float>(startNode.Key, 0f); startNode.Value.PreviousNodeId = startNode.Key;
startNode.Value.Distance = 0f;
Graph endRegion = Spiral(rl, endLat, endLon, regionSize); Graph endRegion = Spiral(rl, endLat, endLon, regionSize);
graph.ConcatGraph(endRegion); graph.ConcatGraph(endRegion);
KeyValuePair<ulong, Node> endNode = graph.ClosestNodeToCoordinates(endLat, endLon); KeyValuePair<ulong, Node> endNode = graph.ClosestNodeToCoordinates(endLat, endLon);
endNode.Value.PreviousIsFromStart = false; endNode.Value.PreviousIsFromStart = false;
endNode.Value.Previous = new KeyValuePair<ulong, float>(endNode.Key, 0f); endNode.Value.PreviousNodeId = endNode.Key;
endNode.Value.Distance = 0f;
logger?.Log(LogLevel.Information, logger?.Log(LogLevel.Information,
"From {0:00.00000}#{1:000.00000} to {2:00.00000}#{3:000.00000} Great-Circle {4:00000.00}km", "From {0:00.00000}#{1:000.00000} to {2:00.00000}#{3:000.00000} Great-Circle {4:00000.00}km",
@ -34,164 +36,78 @@ namespace astar
while (toVisitStart.Count > 0 && toVisitEnd.Count > 0) while (toVisitStart.Count > 0 && toVisitEnd.Count > 0)
{ {
logger?.LogDebug($"Length toVisit-Start: {toVisitStart.Count} -End: {toVisitEnd.Count}"); logger?.LogDebug($"Length toVisit-Start: {toVisitStart.Count} -End: {toVisitEnd.Count}");
/*
* FROM START
*/
ulong currentNodeStartId = toVisitStart.Dequeue(); ulong currentNodeStartId = toVisitStart.Dequeue();
Node? currentNodeStart; Node currentNodeStart = graph.Nodes[currentNodeStartId];
if (!graph.ContainsNode(currentNodeStartId)) foreach ((ulong neighborId, ulong wayId) in currentNodeStart.Neighbors)
{ {
Graph? newRegion = Graph.FromGraph(rl.LoadRegionFromNodeId(currentNodeStartId)); if (!graph.ContainsNode(neighborId))
if (newRegion is null) graph.ConcatGraph(Graph.FromGraph(rl.LoadRegionFromNodeId(neighborId)));
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 = (float)currentNodeStart.Distance! + (float)currentNodeStart.DistanceTo(neighborNode);
if (neighborNode.PreviousNodeId is null || neighborNode.Distance > distance)
{ {
logger?.LogError($"Could not load Region for Node {currentNodeStartId}"); neighborNode.PreviousNodeId = currentNodeStartId;
currentNodeStart = null; neighborNode.Distance = distance;
neighborNode.PreviousIsFromStart = true;
toVisitStart.Enqueue(neighborId, NodeUtils.DistanceBetween(neighborNode, endNode.Value));
} }
else logger?.LogTrace($"Neighbor {neighborId} {neighborNode}");
{
graph.ConcatGraph(newRegion);
currentNodeStart = graph.Nodes[currentNodeStartId];
}
}
else
currentNodeStart = graph.Nodes[currentNodeStartId];
logger?.LogTrace($"Current Node Start: {currentNodeStartId} {currentNodeStart}");
if (currentNodeStart is not null)
{
foreach ((ulong nodeId, ulong wayId) in currentNodeStart.Neighbors)
{
//TODO checks for way-stuff
Node? neighbor;
if (!graph.ContainsNode(nodeId))
{
Graph? newRegion = Graph.FromGraph(rl.LoadRegionFromNodeId(nodeId));
if (newRegion is null)
{
logger?.LogError($"Could not load Region for Node {nodeId}");
neighbor = null;
}
else
{
graph.ConcatGraph(newRegion);
neighbor = Node.FromGraphNode(graph.Nodes[nodeId]);
}
}
else
{
neighbor = graph.Nodes[nodeId];
} }
if (neighbor is not null)
{
/*
* IMPORTANT SHIT BELOW
*/
if (neighbor.PreviousIsFromStart is false)//Check if we found the opposite End
return PathFound(graph, currentNodeStart, neighbor);
float distance = currentNodeStart.Previous!.Value.Value + (float)neighbor.DistanceTo(currentNodeStart);
if (neighbor.Previous is null || neighbor.Previous.Value.Value > distance)
{
neighbor.Previous = new KeyValuePair<ulong, float>(currentNodeStartId, distance);
neighbor.PreviousIsFromStart = true;
toVisitStart.Enqueue(nodeId, NodeUtils.DistanceBetween(neighbor, endNode.Value));
}
logger?.LogTrace($"Neighbor {nodeId} {neighbor}");
}
}
}
/*
* FROM END
*/
ulong currentNodeEndId = toVisitEnd.Dequeue(); ulong currentNodeEndId = toVisitEnd.Dequeue();
Node? currentNodeEnd; Node currentNodeEnd = graph.Nodes[currentNodeEndId];
if (!graph.ContainsNode(currentNodeEndId)) foreach ((ulong neighborId, ulong wayId) in currentNodeEnd.Neighbors)
{ {
Graph? newRegion = Graph.FromGraph(rl.LoadRegionFromNodeId(currentNodeEndId)); if (!graph.ContainsNode(neighborId))
if (newRegion is null) graph.ConcatGraph(Graph.FromGraph(rl.LoadRegionFromNodeId(neighborId)));
{ Node neighborNode = graph.Nodes[neighborId];
logger?.LogError($"Could not load Region for Node {currentNodeEndId}");
currentNodeEnd = null;
}
else
{
graph.ConcatGraph(newRegion);
currentNodeEnd = graph.Nodes[currentNodeEndId];
}
}
else
currentNodeEnd = graph.Nodes[currentNodeEndId];
logger?.LogTrace($"Current Node End: {currentNodeEndId} {currentNodeEnd}");
if (currentNodeEnd is not null) if (neighborNode.PreviousIsFromStart is true)//Check if we found the opposite End
return PathFound(graph, neighborNode, currentNodeEnd, logger);
float distance = (float)currentNodeStart.Distance! + (float)currentNodeStart.DistanceTo(neighborNode);
if (neighborNode.PreviousNodeId is null || neighborNode.Distance > distance)
{ {
foreach ((ulong nodeId, ulong wayId) in currentNodeEnd.Neighbors) neighborNode.PreviousNodeId = currentNodeEndId;
{ neighborNode.Distance = distance;
//TODO checks for way-stuff neighborNode.PreviousIsFromStart = false;
Node? neighbor; toVisitEnd.Enqueue(neighborId, NodeUtils.DistanceBetween(neighborNode, startNode.Value));
if (!graph.ContainsNode(nodeId))
{
Graph? newRegion = Graph.FromGraph(rl.LoadRegionFromNodeId(nodeId));
if (newRegion is null)
{
logger?.LogError($"Could not load Region for Node {nodeId}");
neighbor = null;
} }
else logger?.LogTrace($"Neighbor {neighborId} {neighborNode}");
{
graph.ConcatGraph(newRegion);
neighbor = Node.FromGraphNode(graph.Nodes[nodeId]);
}
}
else
{
neighbor = graph.Nodes[nodeId];
} }
if (neighbor is not null)
{
/*
* IMPORTANT SHIT BELOW
*/
if (neighbor.PreviousIsFromStart is true)//Check if we found the opposite End
return PathFound(graph, neighbor, currentNodeEnd);
float distance = currentNodeEnd.Previous!.Value.Value + (float)neighbor.DistanceTo(currentNodeEnd);
if (neighbor.Previous is null || neighbor.Previous.Value.Value > distance)
{
neighbor.Previous = new KeyValuePair<ulong, float>(currentNodeEndId, distance);
neighbor.PreviousIsFromStart = false;
toVisitEnd.Enqueue(nodeId, NodeUtils.DistanceBetween(neighbor, startNode.Value));
}
logger?.LogTrace($"Neighbor {nodeId} {neighbor}");
}
}
}
} }
return new Route(graph, Array.Empty<Step>().ToList(), false); return new Route(graph, Array.Empty<Step>().ToList(), false);
} }
private static Route PathFound(Graph graph, Node fromStart, Node fromEnd) private static Route PathFound(Graph graph, Node fromStart, Node fromEnd, ILogger? logger = null)
{ {
logger?.LogInformation("Path found!");
List<Step> path = new(); List<Step> path = new();
path.Add(new Step((float)NodeUtils.DistanceBetween(fromStart, fromEnd), fromStart, fromEnd));
Node current = fromStart; Node current = fromStart;
while (current.Previous is not null && current.Previous.Value.Value == 0f) while (current.Distance != 0f)
{ {
Step step = new((float)NodeUtils.DistanceBetween(graph.Nodes[current.Previous.Value.Key], current), graph.Nodes[current.Previous.Value.Key], current); Step step = new((float)NodeUtils.DistanceBetween(graph.Nodes[(ulong)current.PreviousNodeId!], current), graph.Nodes[(ulong)current.PreviousNodeId!], current);
path.Add(step); path.Add(step);
current = graph.Nodes[current.Previous.Value.Key]; current = graph.Nodes[(ulong)current.PreviousNodeId!];
} }
path.Reverse();//Since we go from the middle backwards until here path.Reverse();//Since we go from the middle backwards until here
path.Add(new Step((float)NodeUtils.DistanceBetween(fromStart, fromEnd), fromStart, fromEnd));
current = fromEnd; current = fromEnd;
while (current.Previous is not null && current.Previous.Value.Value == 0f) while (current.Distance != 0f)
{ {
Step step = new((float)NodeUtils.DistanceBetween(graph.Nodes[current.Previous.Value.Key], current), current, graph.Nodes[current.Previous.Value.Key]); Step step = new((float)NodeUtils.DistanceBetween(graph.Nodes[(ulong)current.PreviousNodeId!], current), current, graph.Nodes[(ulong)current.PreviousNodeId!]);
path.Add(step); path.Add(step);
current = graph.Nodes[current.Previous.Value.Key]; current = graph.Nodes[(ulong)current.PreviousNodeId!];
} }
return new Route(graph, path, true); Route r = new (graph, path, true);
logger?.LogInformation(r.ToString());
return r;
} }
private static Graph Spiral(RegionLoader loader, float lat, float lon, float regionSize) private static Graph Spiral(RegionLoader loader, float lat, float lon, float regionSize)

5
astar/Node.cs
View File

@ -2,13 +2,14 @@
public class Node(float lat, float lon, Dictionary<ulong, ulong>? neighbors = null) : global::Graph.Node(lat, lon, neighbors) public class Node(float lat, float lon, Dictionary<ulong, ulong>? neighbors = null) : global::Graph.Node(lat, lon, neighbors)
{ {
public KeyValuePair<ulong, float>? Previous = null; public ulong? PreviousNodeId = null;
public float? Distance = null;
public bool? PreviousIsFromStart = null; public bool? PreviousIsFromStart = null;
public static Node FromGraphNode(global::Graph.Node node) => new (node.Lat, node.Lon, node.Neighbors); public static Node FromGraphNode(global::Graph.Node node) => new (node.Lat, node.Lon, node.Neighbors);
public override string ToString() public override string ToString()
{ {
return $"{Lat:00.000000} {Lon:000.000000} Previous {Previous?.Key} {(PreviousIsFromStart is not null ? PreviousIsFromStart.Value ?"Start":"End" : null)}"; return $"{Lat:00.000000} {Lon:000.000000} Previous {PreviousNodeId} {(PreviousIsFromStart is not null ? PreviousIsFromStart.Value ?"Start":"End" : null)}";
} }
} }

40
astar/Route.cs
View File

@ -7,40 +7,24 @@
public bool RouteFound { get; } = routeFound; public bool RouteFound { get; } = routeFound;
public float Distance => Steps.Sum(step => step.Distance); public float Distance => Steps.Sum(step => step.Distance);
public KeyValuePair<float, float> MinCoordinates() public ValueTuple<float, float> MinCoordinates()
{ {
float minLat, minLon; float minLat = Graph.Nodes.MinBy(node => node.Value.Lat).Value.Lat;
if (RouteFound) float minLon = Graph.Nodes.MinBy(node => node.Value.Lon).Value.Lon;
{ return new ValueTuple<float, float>(minLat, minLon);
Step minLatStep = Steps.MinBy(step => step.Node1.Lat < step.Node2.Lat ? step.Node1.Lat : step.Node2.Lat);
Step minLonStep = Steps.MinBy(step => step.Node1.Lon < step.Node2.Lon ? step.Node1.Lon : step.Node2.Lon);
minLat = minLatStep.Node1.Lat < minLatStep.Node2.Lat ? minLatStep.Node1.Lat : minLatStep.Node2.Lat;
minLon = minLonStep.Node1.Lon < minLonStep.Node2.Lon ? minLonStep.Node1.Lon : minLonStep.Node2.Lon;
}
else
{
minLat = Graph.Nodes.MinBy(node => node.Value.Lat).Value.Lat;
minLon = Graph.Nodes.MinBy(node => node.Value.Lon).Value.Lon;
}
return new KeyValuePair<float, float>(minLat, minLon);
} }
public KeyValuePair<float, float> MaxCoordinates() public ValueTuple<float, float> MaxCoordinates()
{ {
float maxLat, maxLon; float maxLat = Graph.Nodes.MaxBy(node => node.Value.Lat).Value.Lat;
if (RouteFound) float maxLon = Graph.Nodes.MaxBy(node => node.Value.Lon).Value.Lon;
{ return new ValueTuple<float, float>(maxLat, maxLon);
Step maxLatStep = Steps.MaxBy(step => step.Node1.Lat > step.Node2.Lat ? step.Node1.Lat : step.Node2.Lat);
Step maxLonStep = Steps.MaxBy(step => step.Node1.Lon > step.Node2.Lon ? step.Node1.Lon : step.Node2.Lon);
maxLat = maxLatStep.Node1.Lat > maxLatStep.Node2.Lat ? maxLatStep.Node1.Lat : maxLatStep.Node2.Lat;
maxLon = maxLonStep.Node1.Lon > maxLonStep.Node2.Lon ? maxLonStep.Node1.Lon : maxLonStep.Node2.Lon;
} }
else
public override string ToString()
{ {
maxLat = Graph.Nodes.MaxBy(node => node.Value.Lat).Value.Lat; return $"{string.Join("\n", Steps)}\n" +
maxLon = Graph.Nodes.MaxBy(node => node.Value.Lon).Value.Lon; $"Distance: {Distance:000000.00}m";
}
return new KeyValuePair<float, float>(maxLat, maxLon);
} }
} }