Split Node.Previous into two fields for readability

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

View File

@ -63,12 +63,11 @@ if (arguments.TryGetValue(pathArg, out string[]? pathValue))
Route route = Astar.FindPath(startLat, startLon, endLat, endLon, regionSize, importPath, logger);
if(route.RouteFound)
Console.WriteLine($"{string.Join("\n", route.Steps)}\n" +
$"Distance: {route.Distance:000000.00}m");
Console.WriteLine(route);
else
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()

View File

@ -13,13 +13,15 @@ namespace astar
Graph graph = Spiral(rl, startLat, startLon, regionSize);
KeyValuePair<ulong, Node> startNode = graph.ClosestNodeToCoordinates(startLat, startLon);
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.ConcatGraph(endRegion);
KeyValuePair<ulong, Node> endNode = graph.ClosestNodeToCoordinates(endLat, endLon);
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,
"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)
{
logger?.LogDebug($"Length toVisit-Start: {toVisitStart.Count} -End: {toVisitEnd.Count}");
/*
* FROM START
*/
ulong currentNodeStartId = toVisitStart.Dequeue();
Node? currentNodeStart;
if (!graph.ContainsNode(currentNodeStartId))
Node currentNodeStart = graph.Nodes[currentNodeStartId];
foreach ((ulong neighborId, ulong wayId) in currentNodeStart.Neighbors)
{
Graph? newRegion = Graph.FromGraph(rl.LoadRegionFromNodeId(currentNodeStartId));
if (newRegion is null)
if (!graph.ContainsNode(neighborId))
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}");
currentNodeStart = null;
}
else
{
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}");
}
neighborNode.PreviousNodeId = currentNodeStartId;
neighborNode.Distance = distance;
neighborNode.PreviousIsFromStart = true;
toVisitStart.Enqueue(neighborId, NodeUtils.DistanceBetween(neighborNode, endNode.Value));
}
logger?.LogTrace($"Neighbor {neighborId} {neighborNode}");
}
/*
* FROM END
*/
ulong currentNodeEndId = toVisitEnd.Dequeue();
Node? currentNodeEnd;
if (!graph.ContainsNode(currentNodeEndId))
Node currentNodeEnd = graph.Nodes[currentNodeEndId];
foreach ((ulong neighborId, ulong wayId) in currentNodeEnd.Neighbors)
{
Graph? newRegion = Graph.FromGraph(rl.LoadRegionFromNodeId(currentNodeEndId));
if (newRegion is null)
if (!graph.ContainsNode(neighborId))
graph.ConcatGraph(Graph.FromGraph(rl.LoadRegionFromNodeId(neighborId)));
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 = (float)currentNodeStart.Distance! + (float)currentNodeStart.DistanceTo(neighborNode);
if (neighborNode.PreviousNodeId is null || neighborNode.Distance > distance)
{
logger?.LogError($"Could not load Region for Node {currentNodeEndId}");
currentNodeEnd = null;
}
else
{
graph.ConcatGraph(newRegion);
currentNodeEnd = graph.Nodes[currentNodeEndId];
neighborNode.PreviousNodeId = currentNodeEndId;
neighborNode.Distance = distance;
neighborNode.PreviousIsFromStart = false;
toVisitEnd.Enqueue(neighborId, NodeUtils.DistanceBetween(neighborNode, startNode.Value));
}
logger?.LogTrace($"Neighbor {neighborId} {neighborNode}");
}
else
currentNodeEnd = graph.Nodes[currentNodeEndId];
logger?.LogTrace($"Current Node End: {currentNodeEndId} {currentNodeEnd}");
if (currentNodeEnd is not null)
{
foreach ((ulong nodeId, ulong wayId) in currentNodeEnd.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 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);
}
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();
path.Add(new Step((float)NodeUtils.DistanceBetween(fromStart, fromEnd), fromStart, fromEnd));
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);
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.Add(new Step((float)NodeUtils.DistanceBetween(fromStart, fromEnd), fromStart, 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);
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)

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 KeyValuePair<ulong, float>? Previous = null;
public ulong? PreviousNodeId = null;
public float? Distance = null;
public bool? PreviousIsFromStart = null;
public static Node FromGraphNode(global::Graph.Node node) => new (node.Lat, node.Lon, node.Neighbors);
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)}";
}
}

View File

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