diff --git a/Executable/Program.cs b/Executable/Program.cs index cbc5daa..2adf137 100644 --- a/Executable/Program.cs +++ b/Executable/Program.cs @@ -7,7 +7,7 @@ Dictionary graph = OpenStreetMap_Importer.Importer.Import(@"", true logger.level = LogLevel.DEBUG; Random r = new(); -List path; +Route _route; Node n1, n2; do { @@ -15,5 +15,6 @@ do { n1 = graph[graph.Keys.ElementAt(r.Next(0, graph.Count - 1))]; n2 = graph[graph.Keys.ElementAt(r.Next(0, graph.Count - 1))]; - } while (!Astar.FindPath(ref graph, n1, n2, out path, logger)); + _route = Astar.FindPath(ref graph, n1, n2, logger); + } while (!_route.routeFound); } while (Console.ReadKey().Key.Equals(ConsoleKey.Enter)); \ No newline at end of file diff --git a/astar/Astar.cs b/astar/Astar.cs index 9b6f791..ba189a3 100644 --- a/astar/Astar.cs +++ b/astar/Astar.cs @@ -15,38 +15,36 @@ namespace astar { n.previousNode = null; n.goalDistance = float.MaxValue; - n.timeSpent = float.MaxValue; + n.timeRequired = float.MaxValue; } } - /* - * - */ - public static bool FindPath(ref Dictionary nodes, Node start, Node goal, out List path, Logger? logger) + + public static Route FindPath(ref Dictionary nodes, Node start, Node goal, Logger? logger) { - path = new List(); - logger?.Log(LogLevel.INFO, "From {0} - {1} to {2} - {3} Distance {4}", start.lat, start.lon, goal.lat, goal.lon, Utils.DistanceBetweenNodes(start, goal)); + Route _route = new Route(); + logger?.Log(LogLevel.INFO, "From {0:000.00000}#{1:000.00000} to {2:000.00000}#{3:000.00000} Great-Circle {4}", start.lat, start.lon, goal.lat, goal.lon, Utils.DistanceBetweenNodes(start, goal)); Reset(ref nodes); List toVisit = new(); toVisit.Add(start); Node currentNode = start; - start.timeSpent = 0; + start.timeRequired = 0; start.goalDistance = Utils.DistanceBetweenNodes(start, goal); - while (toVisit.Count > 0 && toVisit[0].timeSpent < goal.timeSpent) + while (toVisit.Count > 0 && toVisit[0].timeRequired < goal.timeRequired) { if(currentNode == goal) { logger?.Log(LogLevel.INFO, "Way found, checking for shorter option."); } currentNode = toVisit.First(); - logger?.Log(LogLevel.VERBOSE, "toVisit-length: {0} path-length: {1} goal-distance: {2}", toVisit.Count, currentNode.timeSpent, currentNode.goalDistance); + logger?.Log(LogLevel.VERBOSE, "toVisit-length: {0} path-length: {1} goal-distance: {2}", toVisit.Count, currentNode.timeRequired, currentNode.goalDistance); //Check all neighbors of current node foreach (Edge e in currentNode.edges) { - if (e.neighbor.timeSpent > currentNode.timeSpent + e.weight) + if (e.neighbor.timeRequired > currentNode.timeRequired + e.time) { e.neighbor.goalDistance = Utils.DistanceBetweenNodes(e.neighbor, goal); - e.neighbor.timeSpent = currentNode.timeSpent + e.weight; + e.neighbor.timeRequired = currentNode.timeRequired + e.time; e.neighbor.previousNode = currentNode; toVisit.Add(e.neighbor); } @@ -58,41 +56,51 @@ namespace astar if(goal.previousNode != null) { logger?.Log(LogLevel.INFO, "Way found, shortest option."); + currentNode = goal; + _route.routeFound = true; + _route.time = goal.timeRequired; } else { - logger?.Log(LogLevel.INFO, "No path between {0} - {1} and {2} - {3}", start.lat, start.lon, goal.lat, goal.lon); - return false; + logger?.Log(LogLevel.INFO, "No path between {0:000.00000}#{1:000.00000} and {2:000.00000}#{3:000.00000}", start.lat, start.lon, goal.lat, goal.lon); + _route.routeFound = false; + return _route; } - path.Add(goal); + List tempNodes = new(); + tempNodes.Add(goal); while(currentNode != start) { - if(currentNode.previousNode != null) - { - path.Add(currentNode.previousNode); - currentNode = currentNode.previousNode; - } + tempNodes.Add(currentNode.previousNode); + currentNode = currentNode.previousNode; + } + tempNodes.Reverse(); + for(int i = 0; i < tempNodes.Count - 1; i++) + { + Edge e = tempNodes[i].GetEdgeToNode(tempNodes[i + 1]); + _route.AddStep(tempNodes[i], e); + _route.distance += e.distance; } - path.Reverse(); logger?.Log(LogLevel.INFO, "Path found"); - float distance = 0; - Node? prev = null; - TimeSpan totalTime = TimeSpan.FromSeconds(path.ElementAt(path.Count - 1).timeSpent); - - foreach (Node n in path) + + if(logger?.level > LogLevel.INFO) { - if(prev != null) + + float time = 0; + float distance = 0; + + logger?.Log(LogLevel.DEBUG, "Route Distance: {0} Time: {1}", _route.distance, TimeSpan.FromSeconds(_route.time)); + for(int i = 0; i < _route.steps.Count; i++) { - distance += Utils.DistanceBetweenNodes(prev, n); + Step s = _route.steps[i]; + time += s.edge.time; + distance += s.edge.distance; + logger?.Log(LogLevel.DEBUG, "Step {0:000} From {1:000.00000}#{2:000.00000} To {3:000.00000}#{4:000.00000} along {5}\tafter {6} and {7} m", i, s.start.lat, s.start.lon, s.edge.neighbor.lat, s.edge.neighbor.lon, s.edge.id, TimeSpan.FromSeconds(s.start.timeRequired), distance); } - prev = n; - logger?.Log(LogLevel.DEBUG, "lat {0:000.00000} lon {1:000.00000} traveled {5:0000.00}km in {2:G} / {3:G} Great-Circle to Goal {4:0000.00}", n.lat, n.lon, TimeSpan.FromSeconds(n.timeSpent), totalTime, n.goalDistance, distance); } - - return true; + return _route; } /* @@ -127,12 +135,14 @@ namespace astar return 0; else { - if (n1.timeSpent < n2.timeSpent) + if (n1.timeRequired < n2.timeRequired) return -1; - else if (n1.timeSpent > n2.timeSpent) + else if (n1.timeRequired > n2.timeRequired) return 1; else return 0; } } + + } } \ No newline at end of file diff --git a/astar/Route.cs b/astar/Route.cs new file mode 100644 index 0000000..af42f2a --- /dev/null +++ b/astar/Route.cs @@ -0,0 +1,33 @@ +using Graph; +namespace astar +{ + public class Route + { + public List steps { get; } + public bool routeFound { get; set; } + public float distance { get; set; } + public float time { get; set; } + + public Route() + { + this.steps = new(); + this.distance = 0; + } + + public void AddStep(Node start, Edge way) + { + this.steps.Add(new Step(start, way)); + } + } + + public struct Step + { + public Node start { get; } + public Edge edge { get; } + public Step(Node start, Edge route) + { + this.start = start; + this.edge = route; + } + } +}