diff --git a/Executable/Program.cs b/Executable/Program.cs index 15eae09..e8796a2 100644 --- a/Executable/Program.cs +++ b/Executable/Program.cs @@ -3,7 +3,7 @@ using Logging; using astar; Logger logger = new (LogType.CONSOLE, LogLevel.DEBUG); -Dictionary graph = OpenStreetMap_Importer.Importer.Import(@"", true, logger); +Graph.Graph graph = OpenStreetMap_Importer.Importer.Import(@"C:\Users\glax\Downloads\oberbayern-latest.osm", true, logger); logger.level = LogLevel.DEBUG; Random r = new(); @@ -13,9 +13,9 @@ do { do { - n1 = graph[graph.Keys.ElementAt(r.Next(0, graph.Count - 1))]; - n2 = graph[graph.Keys.ElementAt(r.Next(0, graph.Count - 1))]; - _route = Astar.FindPath(ref graph, n1, n2, logger); + n1 = graph.NodeAtIndex(r.Next(0, graph.GetNodeCount() - 1)); + n2 = graph.NodeAtIndex(r.Next(0, graph.GetNodeCount() - 1)); + _route = new Astar().FindPath(graph, n1, n2, logger); } while (!_route.routeFound); logger.Log(LogLevel.INFO, "Press Enter to find new path."); } while (Console.ReadKey().Key.Equals(ConsoleKey.Enter)); \ No newline at end of file diff --git a/Graph/Graph.cs b/Graph/Graph.cs new file mode 100644 index 0000000..efeae28 --- /dev/null +++ b/Graph/Graph.cs @@ -0,0 +1,61 @@ + +namespace Graph +{ + public class Graph + { + private Dictionary nodes { get; } + + public Graph() + { + this.nodes = new(); + } + + public bool AddNode(ulong id, Node n) + { + return this.nodes.TryAdd(id, n); + } + + public Node NodeAtIndex(int i) + { + return this.nodes.Values.ToArray()[i]; + } + + public int GetNodeCount() + { + return this.nodes.Count; + } + + public Node? GetNode(ulong id) + { + if (this.nodes.TryGetValue(id, out Node? n)) + return n; + else + return null; + } + + public bool ContainsNode(ulong id) + { + return this.nodes.ContainsKey(id); + } + + public bool ContainsNode(Node n) + { + return this.nodes.Values.Contains(n); + } + + public bool RemoveNode(ulong id) + { + return this.nodes.Remove(id); + } + + public bool RemoveNode(Node n) + { + throw new NotImplementedException(); + } + + public Node ClosestNodeToCoordinates(float lat, float lon) + { + throw new NotImplementedException(); + } + } +} diff --git a/Graph/Node.cs b/Graph/Node.cs index 1e56b42..c450258 100644 --- a/Graph/Node.cs +++ b/Graph/Node.cs @@ -4,21 +4,14 @@ { public float lat { get; } public float lon { get; } + public HashSet edges { get; } - public Node? previousNode { get; set; } - public float goalDistance { get; set; } - - public float timeRequired { get; set; } - public Node(float lat, float lon) { this.lat = lat; this.lon = lon; this.edges = new(); - this.previousNode = null; - this.goalDistance = float.MaxValue; - this.timeRequired = float.MaxValue; } public Edge? GetEdgeToNode(Node n) diff --git a/Graph/Utils.cs b/Graph/Utils.cs index ab6f989..d237d79 100644 --- a/Graph/Utils.cs +++ b/Graph/Utils.cs @@ -1,6 +1,5 @@ -namespace Graph +namespace Graph.Utils { - public struct Utils { public static double DistanceBetweenNodes(Node n1, Node n2) diff --git a/OpenStreetMap Importer/Importer.cs b/OpenStreetMap Importer/Importer.cs index 656dca0..83c329d 100644 --- a/OpenStreetMap Importer/Importer.cs +++ b/OpenStreetMap Importer/Importer.cs @@ -4,6 +4,7 @@ using Logging; using System.Xml; using Graph; +using Graph.Utils; namespace OpenStreetMap_Importer { @@ -16,7 +17,7 @@ namespace OpenStreetMap_Importer IgnoreComments = true }; - public static Dictionary Import(string filePath = "", bool onlyJunctions = true, Logger? logger = null) + public static Graph.Graph Import(string filePath = "", bool onlyJunctions = true, Logger? logger = null) { /* * Count Node occurances when tag is "highway" @@ -32,14 +33,14 @@ namespace OpenStreetMap_Importer */ mapData.Position = 0; logger?.Log(LogLevel.INFO, "Importing Graph..."); - Dictionary graph = CreateGraph(mapData, occuranceCount, onlyJunctions, logger); - logger?.Log(LogLevel.DEBUG, "Loaded Nodes: {0}", graph.Count); + Graph.Graph _graph = CreateGraph(mapData, occuranceCount, onlyJunctions, logger); + logger?.Log(LogLevel.DEBUG, "Loaded Nodes: {0}", _graph.GetNodeCount()); mapData.Close(); occuranceCount.Clear(); GC.Collect(); - return graph; + return _graph; } private static Dictionary CountNodeOccurances(Stream mapData, Logger? logger = null) @@ -97,9 +98,9 @@ namespace OpenStreetMap_Importer return _occurances; } - private static Dictionary CreateGraph(Stream mapData, Dictionary occuranceCount, bool onlyJunctions, Logger? logger = null) + private static Graph.Graph CreateGraph(Stream mapData, Dictionary occuranceCount, bool onlyJunctions, Logger? logger = null) { - Dictionary _graph = new(); + Graph.Graph _graph = new(); Way _currentWay; Node _n1, _n2, _currentNode; float _time, _distance = 0; @@ -117,7 +118,7 @@ namespace OpenStreetMap_Importer { float lat = Convert.ToSingle(_reader.GetAttribute("lat").Replace('.', ',')); float lon = Convert.ToSingle(_reader.GetAttribute("lon").Replace('.', ',')); - _graph.Add(id, new Node(lat, lon)); + _graph.AddNode(id, new Node(lat, lon)); logger?.Log(LogLevel.VERBOSE, "NODE {0} {1} {2} {3}", id, lat, lon, occuranceCount[id]); } } @@ -156,8 +157,8 @@ namespace OpenStreetMap_Importer { for (int _nodeIdIndex = 0; _nodeIdIndex < _currentWay.nodeIds.Count - 1; _nodeIdIndex++) { - _n1 = _graph[_currentWay.nodeIds[_nodeIdIndex]]; - _n2 = _graph[_currentWay.nodeIds[_nodeIdIndex + 1]]; + _n1 = _graph.GetNode(_currentWay.nodeIds[_nodeIdIndex]); + _n2 = _graph.GetNode(_currentWay.nodeIds[_nodeIdIndex + 1]); _distance = Convert.ToSingle(Utils.DistanceBetweenNodes(_n1, _n2)); _time = _distance / _currentWay.GetMaxSpeed(); @@ -179,11 +180,11 @@ namespace OpenStreetMap_Importer } else { - _n1 = _graph[_currentWay.nodeIds[0]]; + _n1 = _graph.GetNode(_currentWay.nodeIds[0]); _currentNode = _n1; for(int _nodeIdIndex = 0; _nodeIdIndex < _currentWay.nodeIds.Count - 1; _nodeIdIndex++) { - _n2 = _graph[_currentWay.nodeIds[_nodeIdIndex + 1]]; + _n2 = _graph.GetNode(_currentWay.nodeIds[_nodeIdIndex + 1]); _distance += Convert.ToSingle(Utils.DistanceBetweenNodes(_currentNode, _n2)); if (occuranceCount[_currentWay.nodeIds[_nodeIdIndex]] > 1 || _nodeIdIndex == _currentWay.nodeIds.Count - 2) //junction found { @@ -206,7 +207,7 @@ namespace OpenStreetMap_Importer } else { - _graph.Remove(_currentWay.nodeIds[_nodeIdIndex]); //Not a junction + _graph.RemoveNode(_currentWay.nodeIds[_nodeIdIndex]); //Not a junction } _currentNode = _n2; } diff --git a/astar/Astar.cs b/astar/Astar.cs index 8b06c43..74954a7 100644 --- a/astar/Astar.cs +++ b/astar/Astar.cs @@ -1,51 +1,39 @@ using Logging; using Graph; +using Graph.Utils; namespace astar { public class Astar { + private Dictionary timeRequired = new(); + private Dictionary goalDistance = new(); + private Dictionary previousNode = new(); - /* - * Resets the calculated previous nodes and distance to goal - */ - private static void Reset(ref Dictionary nodes) + public Route FindPath(Graph.Graph graph, Node start, Node goal, Logger? logger) { - foreach(Node n in nodes.Values) - { - n.previousNode = null; - n.goalDistance = float.MaxValue; - n.timeRequired = float.MaxValue; - } - } - - - public static Route FindPath(ref Dictionary nodes, Node start, Node goal, Logger? logger) - { - 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:00000.00}km", start.lat, start.lon, goal.lat, goal.lon, Utils.DistanceBetweenNodes(start, goal)/1000); - Reset(ref nodes); List toVisit = new(); toVisit.Add(start); Node currentNode = start; - start.timeRequired = 0; - start.goalDistance = Convert.ToSingle(Utils.DistanceBetweenNodes(start, goal)); - while (toVisit.Count > 0 && toVisit[0].timeRequired < goal.timeRequired) + SetTimeRequiredToReach(start, 0); + SetDistanceToGoal(start, Convert.ToSingle(Utils.DistanceBetweenNodes(start, goal))); + while (toVisit.Count > 0 && GetTimeRequiredToReach(toVisit[0]) < GetTimeRequiredToReach(goal)) { 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.timeRequired, currentNode.goalDistance); + logger?.Log(LogLevel.VERBOSE, "toVisit-length: {0} path-length: {1} goal-distance: {2}", toVisit.Count, timeRequired[currentNode], goalDistance[currentNode]); //Check all neighbors of current node foreach (Edge e in currentNode.edges) { - if (e.neighbor.timeRequired > currentNode.timeRequired + e.time) + if (GetTimeRequiredToReach(e.neighbor) > GetTimeRequiredToReach(currentNode) + e.time) { - e.neighbor.goalDistance = Convert.ToSingle(Utils.DistanceBetweenNodes(e.neighbor, goal)); - e.neighbor.timeRequired = currentNode.timeRequired + e.time; - e.neighbor.previousNode = currentNode; + SetDistanceToGoal(e.neighbor, Convert.ToSingle(Utils.DistanceBetweenNodes(e.neighbor, goal))); + SetTimeRequiredToReach(e.neighbor, GetTimeRequiredToReach(currentNode) + e.time); + SetPreviousNodeOf(e.neighbor, currentNode); toVisit.Add(e.neighbor); } } @@ -53,39 +41,43 @@ namespace astar toVisit.Remove(currentNode); //"Mark" as visited toVisit.Sort(CompareDistance); } - if(goal.previousNode != null) + + if(GetPreviousNodeOf(goal) != 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: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; + return new Route(new List(), false, float.MaxValue, float.MaxValue); } List tempNodes = new(); tempNodes.Add(goal); while(currentNode != start) { -#pragma warning disable CS8604 // Route was found, so has to have a previous node - tempNodes.Add(currentNode.previousNode); -#pragma warning restore CS8604 - currentNode = currentNode.previousNode; +#pragma warning disable CS8604, CS8600 // Route was found, so has to have a previous node + tempNodes.Add(GetPreviousNodeOf(currentNode)); + currentNode = GetPreviousNodeOf(currentNode); +#pragma warning restore CS8604, CS8600 } tempNodes.Reverse(); + + List steps = new(); + float totalDistance = 0; + for(int i = 0; i < tempNodes.Count - 1; i++) { #pragma warning disable CS8600, CS8604 // Route was found, so has to have an edge Edge e = tempNodes[i].GetEdgeToNode(tempNodes[i + 1]); - _route.AddStep(tempNodes[i], e); + steps.Add(new Step(tempNodes[i], e, GetTimeRequiredToReach(tempNodes[i]), GetDistanceToGoal(tempNodes[i]))); #pragma warning restore CS8600, CS8604 - _route.distance += e.distance; + totalDistance += e.distance; } + Route _route = new Route(steps, true, totalDistance, GetTimeRequiredToReach(goal)); + logger?.Log(LogLevel.INFO, "Path found"); if(logger?.level > LogLevel.INFO) @@ -100,7 +92,7 @@ namespace astar 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:0000000000} after {6} and {7:0000.00}km", i, s.start.lat, s.start.lon, s.edge.neighbor.lat, s.edge.neighbor.lon, s.edge.id, TimeSpan.FromSeconds(s.start.timeRequired), distance/1000); + logger?.Log(LogLevel.DEBUG, "Step {0:000} From {1:000.00000}#{2:000.00000} To {3:000.00000}#{4:000.00000} along {5:0000000000} after {6} and {7:0000.00}km", i, s.start.lat, s.start.lon, s.edge.neighbor.lat, s.edge.neighbor.lon, s.edge.id, TimeSpan.FromSeconds(timeRequired[s.start]), distance/1000); } } @@ -113,15 +105,15 @@ namespace astar * 0 => n1 equal n2 * 1 => n1 larger n2 */ - private static int CompareDistance(Node n1, Node n2) + private int CompareDistance(Node n1, Node n2) { if (n1 == null || n2 == null) return 0; else { - if (n1.goalDistance < n2.goalDistance) + if (GetDistanceToGoal(n1) < GetDistanceToGoal(n2)) return -1; - else if (n1.goalDistance > n2.goalDistance) + else if (GetDistanceToGoal(n1) > GetDistanceToGoal(n2)) return 1; else return 0; } @@ -133,20 +125,72 @@ namespace astar * 0 => n1 equal n2 * 1 => n1 larger n2 */ - private static int ComparePathLength(Node n1, Node n2) + private int ComparePathLength(Node n1, Node n2) { if (n1 == null || n2 == null) return 0; else { - if (n1.timeRequired < n2.timeRequired) + if (GetTimeRequiredToReach(n1) < GetTimeRequiredToReach(n2)) return -1; - else if (n1.timeRequired > n2.timeRequired) + else if (GetTimeRequiredToReach(n1) > GetTimeRequiredToReach(n2)) return 1; else return 0; } } + private float GetTimeRequiredToReach(Node n) + { + if (timeRequired.TryGetValue(n, out float t)) + { + return t; + } + else + { + return float.MaxValue; + } + } + + private void SetTimeRequiredToReach(Node n, float t) + { + if (!timeRequired.TryAdd(n, t)) + timeRequired[n] = t; + } + + private float GetDistanceToGoal(Node n) + { + if (goalDistance.TryGetValue(n, out float t)) + { + return t; + } + else + { + return float.MaxValue; + } + } + + private void SetDistanceToGoal(Node n, float d) + { + if (!goalDistance.TryAdd(n, d)) + goalDistance[n] = d; + } + + private Node? GetPreviousNodeOf(Node n) + { + if(previousNode.TryGetValue(n, out Node? t)) + { + return t; + }else + { + return null; + } + } + + private void SetPreviousNodeOf(Node n, Node p) + { + if (!previousNode.TryAdd(n, p)) + previousNode[n] = p; + } } } \ No newline at end of file diff --git a/astar/Route.cs b/astar/Route.cs index af42f2a..e81986a 100644 --- a/astar/Route.cs +++ b/astar/Route.cs @@ -4,19 +4,17 @@ 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 bool routeFound { get; } + public float distance { get; } + public float time { get; } - public Route() - { - this.steps = new(); - this.distance = 0; - } - public void AddStep(Node start, Edge way) + public Route(List steps, bool routeFound, float distance, float timeRequired) { - this.steps.Add(new Step(start, way)); + this.steps = steps; + this.routeFound = routeFound; + this.distance = distance; + this.time = timeRequired; } } @@ -24,10 +22,15 @@ namespace astar { public Node start { get; } public Edge edge { get; } - public Step(Node start, Edge route) + + public float timeRequired { get; } + public float goalDistance { get; } + public Step(Node start, Edge route, float timeRequired, float goalDistance) { this.start = start; this.edge = route; + this.timeRequired = timeRequired; + this.goalDistance = goalDistance; } } }