From d23cf87dd48ef9cd16838d33b42d34175d28c62a Mon Sep 17 00:00:00 2001 From: C9Glax <13404778+C9Glax@users.noreply.github.com> Date: Wed, 2 Nov 2022 18:07:22 +0100 Subject: [PATCH 01/16] Nullable Node --- Graph/Node.cs | 5 ++--- astar/Astar.cs | 15 +++++++++------ 2 files changed, 11 insertions(+), 9 deletions(-) diff --git a/Graph/Node.cs b/Graph/Node.cs index 433f884..4e076dc 100644 --- a/Graph/Node.cs +++ b/Graph/Node.cs @@ -6,7 +6,7 @@ public float lon { get; } public HashSet edges { get; } - public Node previousNode { get; set; } + public Node? previousNode { get; set; } public float goalDistance { get; set; } public float pathLength { get; set; } @@ -16,10 +16,9 @@ this.lat = lat; this.lon = lon; this.edges = new(); - this.previousNode = nullnode; + this.previousNode = null; this.goalDistance = float.MaxValue; this.pathLength = float.MaxValue; } - public static Node nullnode = new(float.NaN, float.NaN); } } diff --git a/astar/Astar.cs b/astar/Astar.cs index 7c42af1..5ab8499 100644 --- a/astar/Astar.cs +++ b/astar/Astar.cs @@ -13,7 +13,7 @@ namespace astar { foreach(Node n in nodes.Values) { - n.previousNode = Node.nullnode; + n.previousNode = null; n.goalDistance = float.MaxValue; n.pathLength = float.MaxValue; } @@ -55,7 +55,7 @@ namespace astar toVisit.Remove(currentNode); //"Mark" as visited toVisit.Sort(CompareDistance); } - if(goal.previousNode != Node.nullnode) + if(goal.previousNode != null) { logger?.Log(LogLevel.INFO, "Way found, shortest option."); } @@ -68,19 +68,22 @@ namespace astar path.Add(goal); while(currentNode != start) { - path.Add(currentNode.previousNode); - currentNode = currentNode.previousNode; + if(currentNode.previousNode != null) + { + path.Add(currentNode.previousNode); + currentNode = currentNode.previousNode; + } } path.Reverse(); logger?.Log(LogLevel.INFO, "Path found"); float distance = 0; - Node prev = Node.nullnode; + Node? prev = null; TimeSpan totalTime = TimeSpan.FromSeconds(path.ElementAt(path.Count - 1).pathLength); foreach (Node n in path) { - if(!prev.Equals(Node.nullnode)) + if(prev != null) { distance += Utils.DistanceBetweenNodes(prev, n); } From 12d7f9b78c97aab52d97927dd96a634470bb1b15 Mon Sep 17 00:00:00 2001 From: C9Glax <13404778+C9Glax@users.noreply.github.com> Date: Wed, 2 Nov 2022 18:09:30 +0100 Subject: [PATCH 02/16] pathLength => Timespent --- Graph/Node.cs | 4 ++-- astar/Astar.cs | 20 ++++++++++---------- 2 files changed, 12 insertions(+), 12 deletions(-) diff --git a/Graph/Node.cs b/Graph/Node.cs index 4e076dc..701c51d 100644 --- a/Graph/Node.cs +++ b/Graph/Node.cs @@ -9,7 +9,7 @@ public Node? previousNode { get; set; } public float goalDistance { get; set; } - public float pathLength { get; set; } + public float timeSpent { get; set; } public Node(float lat, float lon) { @@ -18,7 +18,7 @@ this.edges = new(); this.previousNode = null; this.goalDistance = float.MaxValue; - this.pathLength = float.MaxValue; + this.timeSpent = float.MaxValue; } } } diff --git a/astar/Astar.cs b/astar/Astar.cs index 5ab8499..9b6f791 100644 --- a/astar/Astar.cs +++ b/astar/Astar.cs @@ -15,7 +15,7 @@ namespace astar { n.previousNode = null; n.goalDistance = float.MaxValue; - n.pathLength = float.MaxValue; + n.timeSpent = float.MaxValue; } } @@ -30,23 +30,23 @@ namespace astar List toVisit = new(); toVisit.Add(start); Node currentNode = start; - start.pathLength = 0; + start.timeSpent = 0; start.goalDistance = Utils.DistanceBetweenNodes(start, goal); - while (toVisit.Count > 0 && toVisit[0].pathLength < goal.pathLength) + while (toVisit.Count > 0 && toVisit[0].timeSpent < goal.timeSpent) { 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.pathLength, currentNode.goalDistance); + logger?.Log(LogLevel.VERBOSE, "toVisit-length: {0} path-length: {1} goal-distance: {2}", toVisit.Count, currentNode.timeSpent, currentNode.goalDistance); //Check all neighbors of current node foreach (Edge e in currentNode.edges) { - if (e.neighbor.pathLength > currentNode.pathLength + e.weight) + if (e.neighbor.timeSpent > currentNode.timeSpent + e.weight) { e.neighbor.goalDistance = Utils.DistanceBetweenNodes(e.neighbor, goal); - e.neighbor.pathLength = currentNode.pathLength + e.weight; + e.neighbor.timeSpent = currentNode.timeSpent + e.weight; e.neighbor.previousNode = currentNode; toVisit.Add(e.neighbor); } @@ -79,7 +79,7 @@ namespace astar logger?.Log(LogLevel.INFO, "Path found"); float distance = 0; Node? prev = null; - TimeSpan totalTime = TimeSpan.FromSeconds(path.ElementAt(path.Count - 1).pathLength); + TimeSpan totalTime = TimeSpan.FromSeconds(path.ElementAt(path.Count - 1).timeSpent); foreach (Node n in path) { @@ -88,7 +88,7 @@ namespace astar distance += Utils.DistanceBetweenNodes(prev, n); } 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.pathLength), totalTime, n.goalDistance, distance); + 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); } @@ -127,9 +127,9 @@ namespace astar return 0; else { - if (n1.pathLength < n2.pathLength) + if (n1.timeSpent < n2.timeSpent) return -1; - else if (n1.pathLength > n2.pathLength) + else if (n1.timeSpent > n2.timeSpent) return 1; else return 0; } From 485f04e6a2c836ecef0d90bafb1ee24fd15fbdfa Mon Sep 17 00:00:00 2001 From: C9Glax <13404778+C9Glax@users.noreply.github.com> Date: Thu, 3 Nov 2022 18:16:53 +0100 Subject: [PATCH 03/16] More logging --- OpenStreetMap Importer/Importer.cs | 19 +++++++++++++------ 1 file changed, 13 insertions(+), 6 deletions(-) diff --git a/OpenStreetMap Importer/Importer.cs b/OpenStreetMap Importer/Importer.cs index ac27626..f2c94c7 100644 --- a/OpenStreetMap Importer/Importer.cs +++ b/OpenStreetMap Importer/Importer.cs @@ -21,22 +21,23 @@ namespace OpenStreetMap_Importer /* * Count Node occurances when tag is "highway" */ - logger?.Log(LogLevel.INFO, "Counting Node-Occurances..."); + logger?.Log(LogLevel.DEBUG, "Opening File..."); Stream mapData = File.Exists(filePath) ? new FileStream(filePath, FileMode.Open, FileAccess.Read) : new MemoryStream(OSM_Data.map); + logger?.Log(LogLevel.INFO, "Counting Node-Occurances..."); Dictionary occuranceCount = CountNodeOccurances(mapData, logger); - mapData.Close(); logger?.Log(LogLevel.DEBUG, "Way Nodes: {0}", occuranceCount.Count); /* * Import Nodes and Edges */ + mapData.Position = 0; logger?.Log(LogLevel.INFO, "Importing Graph..."); - mapData = File.Exists(filePath) ? new FileStream(filePath, FileMode.Open, FileAccess.Read) : new MemoryStream(OSM_Data.map); Dictionary graph = CreateGraph(mapData, occuranceCount, onlyJunctions, logger); + logger?.Log(LogLevel.DEBUG, "Loaded Nodes: {0}", graph.Count); + mapData.Close(); occuranceCount.Clear(); GC.Collect(); - logger?.Log(LogLevel.DEBUG, "Loaded Nodes: {0}", graph.Count); return graph; } @@ -57,6 +58,7 @@ namespace OpenStreetMap_Importer _isHighway = false; _currentIds.Clear(); _wayReader = _reader.ReadSubtree(); + logger?.Log(LogLevel.VERBOSE, "WAY: {0}", _reader.GetAttribute("id")); while (_wayReader.Read()) { if (_reader.Name == "tag" && _reader.GetAttribute("k").Equals("highway")) @@ -65,6 +67,7 @@ namespace OpenStreetMap_Importer { if (!Enum.Parse(typeof(Way.type), _reader.GetAttribute("v"), true).Equals(Way.type.NONE)) _isHighway = true; + logger?.Log(LogLevel.VERBOSE, "Highway: {0}", _reader.GetAttribute("v")); } catch (ArgumentException) { }; } @@ -73,6 +76,7 @@ namespace OpenStreetMap_Importer try { _currentIds.Add(Convert.ToUInt64(_reader.GetAttribute("ref"))); + logger?.Log(LogLevel.VERBOSE, "node-ref: {0}", _reader.GetAttribute("ref")); } catch (FormatException) { }; } @@ -122,6 +126,7 @@ namespace OpenStreetMap_Importer { _wayReader = _reader.ReadSubtree(); _currentWay = new(); + logger?.Log(LogLevel.VERBOSE, "WAY: {0}", _reader.GetAttribute("id")); while (_wayReader.Read()) { _wayReader = _reader.ReadSubtree(); @@ -139,7 +144,7 @@ namespace OpenStreetMap_Importer { ulong _id = Convert.ToUInt64(_reader.GetAttribute("ref")); _currentWay.nodeIds.Add(_id); - logger?.Log(LogLevel.VERBOSE, "nd: {0}", _id); + logger?.Log(LogLevel.VERBOSE, "node-ref: {0}", _id); } } } @@ -147,7 +152,7 @@ namespace OpenStreetMap_Importer if (!_currentWay.GetHighwayType().Equals(Way.type.NONE)) { - logger?.Log(LogLevel.VERBOSE, "WAY Nodes: {0} Type: {1}", _currentWay.nodeIds.Count, _currentWay.GetHighwayType()); + logger?.Log(LogLevel.VERBOSE, "WAY Nodes-count: {0} Type: {1}", _currentWay.nodeIds.Count, _currentWay.GetHighwayType()); if (!onlyJunctions) { for (int _nodeIdIndex = 0; _nodeIdIndex < _currentWay.nodeIds.Count - 1; _nodeIdIndex++) @@ -168,6 +173,7 @@ namespace OpenStreetMap_Importer { _n2.edges.Add(new Edge(_n1, _weight)); } + logger?.Log(LogLevel.VERBOSE, "Add Edge: {0} & {1} Weight: {2}", _currentWay.nodeIds[_nodeIdIndex], _currentWay.nodeIds[_nodeIdIndex + 1], _weight); } _graph = _tempAll; } @@ -206,6 +212,7 @@ namespace OpenStreetMap_Importer { _n2.edges.Add(new Edge(_n1, _weight, _currentWay.GetId())); } + logger?.Log(LogLevel.VERBOSE, "Add Edge: {0} & {1} Weight: {2}", _currentWay.nodeIds[_nodeIdIndex], _currentWay.nodeIds[_nodeIdIndex + 1], _weight); } else { From 4930423cb5c837be0d9809f67b603e75623a350c Mon Sep 17 00:00:00 2001 From: C9Glax <13404778+C9Glax@users.noreply.github.com> Date: Thu, 3 Nov 2022 18:21:13 +0100 Subject: [PATCH 04/16] Removed unessecary duplication of graph --- OpenStreetMap Importer/Importer.cs | 23 ++++++----------------- 1 file changed, 6 insertions(+), 17 deletions(-) diff --git a/OpenStreetMap Importer/Importer.cs b/OpenStreetMap Importer/Importer.cs index f2c94c7..4a28f38 100644 --- a/OpenStreetMap Importer/Importer.cs +++ b/OpenStreetMap Importer/Importer.cs @@ -99,7 +99,6 @@ namespace OpenStreetMap_Importer private static Dictionary CreateGraph(Stream mapData, Dictionary occuranceCount, bool onlyJunctions, Logger? logger = null) { - Dictionary _tempAll = new(); Dictionary _graph = new(); Way _currentWay; Node _n1, _n2, _currentNode; @@ -118,7 +117,7 @@ namespace OpenStreetMap_Importer { float lat = Convert.ToSingle(_reader.GetAttribute("lat").Replace('.', ',')); float lon = Convert.ToSingle(_reader.GetAttribute("lon").Replace('.', ',')); - _tempAll.Add(id, new Node(lat, lon)); + _graph.Add(id, new Node(lat, lon)); logger?.Log(LogLevel.VERBOSE, "NODE {0} {1} {2} {3}", id, lat, lon, occuranceCount[id]); } } @@ -157,8 +156,8 @@ namespace OpenStreetMap_Importer { for (int _nodeIdIndex = 0; _nodeIdIndex < _currentWay.nodeIds.Count - 1; _nodeIdIndex++) { - _n1 = _tempAll[_currentWay.nodeIds[_nodeIdIndex]]; - _n2 = _tempAll[_currentWay.nodeIds[_nodeIdIndex + 1]]; + _n1 = _graph[_currentWay.nodeIds[_nodeIdIndex]]; + _n2 = _graph[_currentWay.nodeIds[_nodeIdIndex + 1]]; _weight = Utils.DistanceBetweenNodes(_n1, _n2) * 1000 / _currentWay.GetMaxSpeed(); if (!_currentWay.IsOneWay()) { @@ -175,24 +174,14 @@ namespace OpenStreetMap_Importer } logger?.Log(LogLevel.VERBOSE, "Add Edge: {0} & {1} Weight: {2}", _currentWay.nodeIds[_nodeIdIndex], _currentWay.nodeIds[_nodeIdIndex + 1], _weight); } - _graph = _tempAll; } else { - if (!_tempAll.TryGetValue(_currentWay.nodeIds[0], out _n1)) - { - _n1 = _graph[_currentWay.nodeIds[0]]; - } - else - { - _graph.TryAdd(_currentWay.nodeIds[0], _n1); - _tempAll.Remove(_currentWay.nodeIds[0]); - } + _n1 = _graph[_currentWay.nodeIds[0]]; _currentNode = _n1; for(int _nodeIdIndex = 0; _nodeIdIndex < _currentWay.nodeIds.Count - 1; _nodeIdIndex++) { - if (!_tempAll.TryGetValue(_currentWay.nodeIds[_nodeIdIndex + 1], out _n2)) - _n2 = _graph[_currentWay.nodeIds[_nodeIdIndex + 1]]; + _n2 = _graph[_currentWay.nodeIds[_nodeIdIndex + 1]]; _distance += Utils.DistanceBetweenNodes(_currentNode, _n2); if (occuranceCount[_currentWay.nodeIds[_nodeIdIndex]] > 1 || _nodeIdIndex == _currentWay.nodeIds.Count - 2) //junction found { @@ -216,7 +205,7 @@ namespace OpenStreetMap_Importer } else { - _tempAll.Remove(_currentWay.nodeIds[_nodeIdIndex]); + _graph.Remove(_currentWay.nodeIds[_nodeIdIndex]); //Not a junction } _currentNode = _n2; } From b88e1a8dfc3bf6f6ea473b877e2a43a858ced3a7 Mon Sep 17 00:00:00 2001 From: C9Glax <13404778+C9Glax@users.noreply.github.com> Date: Thu, 3 Nov 2022 19:13:26 +0100 Subject: [PATCH 05/16] Added Distance --- Graph/Edge.cs | 18 +++++++----------- 1 file changed, 7 insertions(+), 11 deletions(-) diff --git a/Graph/Edge.cs b/Graph/Edge.cs index 26faf6f..b834a4e 100644 --- a/Graph/Edge.cs +++ b/Graph/Edge.cs @@ -1,22 +1,18 @@ namespace Graph { - public struct Edge + public class Edge { public ulong id { get; } public Node neighbor { get; } - public float weight { get; } - public Edge(Node neighbor, float weight) - { - this.neighbor = neighbor; - this.weight = weight; - this.id = 0; - } + public float time { get; } + public float distance { get; } - public Edge(Node neighbor, float weight, ulong id) + public Edge(Node neighbor, float time, float distance, ulong? id) { this.neighbor = neighbor; - this.weight = weight; - this.id = id; + this.time = time; + this.distance = distance; + this.id = (id != null) ? id.Value : 0; } } } From 88ed5472a83f9b7adc72136762bed7ab69cdab53 Mon Sep 17 00:00:00 2001 From: C9Glax <13404778+C9Glax@users.noreply.github.com> Date: Thu, 3 Nov 2022 19:13:38 +0100 Subject: [PATCH 06/16] timeSpent -> TimeRequired --- Graph/Node.cs | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/Graph/Node.cs b/Graph/Node.cs index 701c51d..449a15f 100644 --- a/Graph/Node.cs +++ b/Graph/Node.cs @@ -9,7 +9,7 @@ public Node? previousNode { get; set; } public float goalDistance { get; set; } - public float timeSpent { get; set; } + public float timeRequired { get; set; } public Node(float lat, float lon) { @@ -18,7 +18,15 @@ this.edges = new(); this.previousNode = null; this.goalDistance = float.MaxValue; - this.timeSpent = float.MaxValue; + this.timeRequired = float.MaxValue; + } + + public Edge GetEdgeToNode(Node n) + { + foreach (Edge e in this.edges) + if (e.neighbor == n) + return e; + return null; } } } From fcae3e5be2be156f2fa1415ace42579c928998e7 Mon Sep 17 00:00:00 2001 From: C9Glax <13404778+C9Glax@users.noreply.github.com> Date: Thu, 3 Nov 2022 19:13:53 +0100 Subject: [PATCH 07/16] Changed Distance to meters --- Graph/Utils.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Graph/Utils.cs b/Graph/Utils.cs index 550ed10..30f60ea 100644 --- a/Graph/Utils.cs +++ b/Graph/Utils.cs @@ -9,7 +9,7 @@ public static float DistanceBetweenCoordinates(float lat1, float lon1, float lat2, float lon2) { - const int earthRadius = 6371; + const int earthRadius = 6371000; double differenceLat = DegreesToRadians(lat2 - lat1); double differenceLon = DegreesToRadians(lon2 - lon1); From a1fb19a080a55b59f6d412d890dbcd284e480089 Mon Sep 17 00:00:00 2001 From: C9Glax <13404778+C9Glax@users.noreply.github.com> Date: Thu, 3 Nov 2022 19:14:07 +0100 Subject: [PATCH 08/16] distance and time import --- OpenStreetMap Importer/Importer.cs | 31 +++++++++++++++--------------- 1 file changed, 16 insertions(+), 15 deletions(-) diff --git a/OpenStreetMap Importer/Importer.cs b/OpenStreetMap Importer/Importer.cs index 4a28f38..b52a23a 100644 --- a/OpenStreetMap Importer/Importer.cs +++ b/OpenStreetMap Importer/Importer.cs @@ -102,7 +102,7 @@ namespace OpenStreetMap_Importer Dictionary _graph = new(); Way _currentWay; Node _n1, _n2, _currentNode; - float _weight, _distance = 0; + float _time, _distance = 0; XmlReader _reader = XmlReader.Create(mapData, readerSettings); XmlReader _wayReader; @@ -158,21 +158,23 @@ namespace OpenStreetMap_Importer { _n1 = _graph[_currentWay.nodeIds[_nodeIdIndex]]; _n2 = _graph[_currentWay.nodeIds[_nodeIdIndex + 1]]; - _weight = Utils.DistanceBetweenNodes(_n1, _n2) * 1000 / _currentWay.GetMaxSpeed(); + + _distance = Utils.DistanceBetweenNodes(_n1, _n2); + _time = _distance / _currentWay.GetMaxSpeed(); if (!_currentWay.IsOneWay()) { - _n1.edges.Add(new Edge(_n2, _weight)); - _n2.edges.Add(new Edge(_n1, _weight)); + _n1.edges.Add(new Edge(_n2, _time, _distance, _currentWay.GetId())); + _n2.edges.Add(new Edge(_n1, _time, _distance, _currentWay.GetId())); } else if (_currentWay.IsForward()) { - _n1.edges.Add(new Edge(_n2, _weight)); + _n1.edges.Add(new Edge(_n2, _time, _distance, _currentWay.GetId())); } else { - _n2.edges.Add(new Edge(_n1, _weight)); + _n2.edges.Add(new Edge(_n1, _time, _distance, _currentWay.GetId())); } - logger?.Log(LogLevel.VERBOSE, "Add Edge: {0} & {1} Weight: {2}", _currentWay.nodeIds[_nodeIdIndex], _currentWay.nodeIds[_nodeIdIndex + 1], _weight); + logger?.Log(LogLevel.VERBOSE, "Add Edge: {0} & {1} Weight: {2}", _currentWay.nodeIds[_nodeIdIndex], _currentWay.nodeIds[_nodeIdIndex + 1], _time); } } else @@ -185,23 +187,22 @@ namespace OpenStreetMap_Importer _distance += Utils.DistanceBetweenNodes(_currentNode, _n2); if (occuranceCount[_currentWay.nodeIds[_nodeIdIndex]] > 1 || _nodeIdIndex == _currentWay.nodeIds.Count - 2) //junction found { - _weight = _distance * 1000 / _currentWay.GetMaxSpeed(); - _distance = 0; - _graph.TryAdd(_currentWay.nodeIds[_nodeIdIndex + 1], _n2); + _time = _distance / _currentWay.GetMaxSpeed(); if (!_currentWay.IsOneWay()) { - _n1.edges.Add(new Edge(_n2, _weight, _currentWay.GetId())); - _n2.edges.Add(new Edge(_n1, _weight, _currentWay.GetId())); + _n1.edges.Add(new Edge(_n2, _time, _distance, _currentWay.GetId())); + _n2.edges.Add(new Edge(_n1, _time, _distance, _currentWay.GetId())); } else if (_currentWay.IsForward()) { - _n1.edges.Add(new Edge(_n2, _weight, _currentWay.GetId())); + _n1.edges.Add(new Edge(_n2, _time, _distance, _currentWay.GetId())); } else { - _n2.edges.Add(new Edge(_n1, _weight, _currentWay.GetId())); + _n2.edges.Add(new Edge(_n1, _time, _distance, _currentWay.GetId())); } - logger?.Log(LogLevel.VERBOSE, "Add Edge: {0} & {1} Weight: {2}", _currentWay.nodeIds[_nodeIdIndex], _currentWay.nodeIds[_nodeIdIndex + 1], _weight); + _distance = 0; + logger?.Log(LogLevel.VERBOSE, "Add Edge: {0} & {1} Weight: {2}", _currentWay.nodeIds[_nodeIdIndex], _currentWay.nodeIds[_nodeIdIndex + 1], _time); } else { From f2cf676c319411ae46d42099c4e9780a3db982f6 Mon Sep 17 00:00:00 2001 From: C9Glax <13404778+C9Glax@users.noreply.github.com> Date: Thu, 3 Nov 2022 19:14:14 +0100 Subject: [PATCH 09/16] Added "Route" --- Executable/Program.cs | 5 +-- astar/Astar.cs | 78 ++++++++++++++++++++++++------------------- astar/Route.cs | 33 ++++++++++++++++++ 3 files changed, 80 insertions(+), 36 deletions(-) create mode 100644 astar/Route.cs 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; + } + } +} From a971e4ed6cbefec674785262453043b4b69781f6 Mon Sep 17 00:00:00 2001 From: C9Glax <13404778+C9Glax@users.noreply.github.com> Date: Thu, 3 Nov 2022 19:35:04 +0100 Subject: [PATCH 10/16] GetEdgeToNode Nullable --- Graph/Node.cs | 2 +- astar/Astar.cs | 10 +++++++--- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/Graph/Node.cs b/Graph/Node.cs index 449a15f..1e56b42 100644 --- a/Graph/Node.cs +++ b/Graph/Node.cs @@ -21,7 +21,7 @@ this.timeRequired = float.MaxValue; } - public Edge GetEdgeToNode(Node n) + public Edge? GetEdgeToNode(Node n) { foreach (Edge e in this.edges) if (e.neighbor == n) diff --git a/astar/Astar.cs b/astar/Astar.cs index ba189a3..6a149fe 100644 --- a/astar/Astar.cs +++ b/astar/Astar.cs @@ -23,7 +23,7 @@ namespace astar 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}", start.lat, start.lon, goal.lat, goal.lon, Utils.DistanceBetweenNodes(start, goal)); + 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); @@ -71,14 +71,18 @@ namespace astar 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; } tempNodes.Reverse(); 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); +#pragma warning restore CS8600, CS8604 _route.distance += e.distance; } @@ -90,13 +94,13 @@ namespace astar float time = 0; float distance = 0; - logger?.Log(LogLevel.DEBUG, "Route Distance: {0} Time: {1}", _route.distance, TimeSpan.FromSeconds(_route.time)); + logger?.Log(LogLevel.DEBUG, "Route Distance: {0:00000.00km} Time: {1}", _route.distance/1000, TimeSpan.FromSeconds(_route.time)); for(int i = 0; i < _route.steps.Count; i++) { 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); + 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); } } From 23bab6a593e8363f0358e8cb1fd692bde54cf2a5 Mon Sep 17 00:00:00 2001 From: C9Glax <13404778+C9Glax@users.noreply.github.com> Date: Sun, 13 Nov 2022 13:21:24 +0100 Subject: [PATCH 11/16] Changed distance back to double --- Graph/Utils.cs | 9 +++++---- OpenStreetMap Importer/Importer.cs | 4 ++-- astar/Astar.cs | 4 ++-- 3 files changed, 9 insertions(+), 8 deletions(-) diff --git a/Graph/Utils.cs b/Graph/Utils.cs index 30f60ea..ab6f989 100644 --- a/Graph/Utils.cs +++ b/Graph/Utils.cs @@ -1,13 +1,14 @@ -namespace Graph{ +namespace Graph +{ public struct Utils { - public static float DistanceBetweenNodes(Node n1, Node n2) + public static double DistanceBetweenNodes(Node n1, Node n2) { return DistanceBetweenCoordinates(n1.lat, n1.lon, n2.lat, n2.lon); } - public static float DistanceBetweenCoordinates(float lat1, float lon1, float lat2, float lon2) + public static double DistanceBetweenCoordinates(float lat1, float lon1, float lat2, float lon2) { const int earthRadius = 6371000; double differenceLat = DegreesToRadians(lat2 - lat1); @@ -19,7 +20,7 @@ double a = Math.Sin(differenceLat / 2) * Math.Sin(differenceLat / 2) + Math.Sin(differenceLon / 2) * Math.Sin(differenceLon / 2) * Math.Cos(lat1Rads) * Math.Cos(lat2Rads); double c = 2 * Math.Atan2(Math.Sqrt(a), Math.Sqrt(1 - a)); - return Convert.ToSingle(earthRadius * c); + return earthRadius * c; } private static double DegreesToRadians(double deg) diff --git a/OpenStreetMap Importer/Importer.cs b/OpenStreetMap Importer/Importer.cs index b52a23a..656dca0 100644 --- a/OpenStreetMap Importer/Importer.cs +++ b/OpenStreetMap Importer/Importer.cs @@ -159,7 +159,7 @@ namespace OpenStreetMap_Importer _n1 = _graph[_currentWay.nodeIds[_nodeIdIndex]]; _n2 = _graph[_currentWay.nodeIds[_nodeIdIndex + 1]]; - _distance = Utils.DistanceBetweenNodes(_n1, _n2); + _distance = Convert.ToSingle(Utils.DistanceBetweenNodes(_n1, _n2)); _time = _distance / _currentWay.GetMaxSpeed(); if (!_currentWay.IsOneWay()) { @@ -184,7 +184,7 @@ namespace OpenStreetMap_Importer for(int _nodeIdIndex = 0; _nodeIdIndex < _currentWay.nodeIds.Count - 1; _nodeIdIndex++) { _n2 = _graph[_currentWay.nodeIds[_nodeIdIndex + 1]]; - _distance += Utils.DistanceBetweenNodes(_currentNode, _n2); + _distance += Convert.ToSingle(Utils.DistanceBetweenNodes(_currentNode, _n2)); if (occuranceCount[_currentWay.nodeIds[_nodeIdIndex]] > 1 || _nodeIdIndex == _currentWay.nodeIds.Count - 2) //junction found { _time = _distance / _currentWay.GetMaxSpeed(); diff --git a/astar/Astar.cs b/astar/Astar.cs index 6a149fe..8b06c43 100644 --- a/astar/Astar.cs +++ b/astar/Astar.cs @@ -29,7 +29,7 @@ namespace astar toVisit.Add(start); Node currentNode = start; start.timeRequired = 0; - start.goalDistance = Utils.DistanceBetweenNodes(start, goal); + start.goalDistance = Convert.ToSingle(Utils.DistanceBetweenNodes(start, goal)); while (toVisit.Count > 0 && toVisit[0].timeRequired < goal.timeRequired) { if(currentNode == goal) @@ -43,7 +43,7 @@ namespace astar { if (e.neighbor.timeRequired > currentNode.timeRequired + e.time) { - e.neighbor.goalDistance = Utils.DistanceBetweenNodes(e.neighbor, goal); + e.neighbor.goalDistance = Convert.ToSingle(Utils.DistanceBetweenNodes(e.neighbor, goal)); e.neighbor.timeRequired = currentNode.timeRequired + e.time; e.neighbor.previousNode = currentNode; toVisit.Add(e.neighbor); From a4ebf05d7afa452b62a046e894fcc813bb74188c Mon Sep 17 00:00:00 2001 From: C9Glax <13404778+C9Glax@users.noreply.github.com> Date: Sun, 13 Nov 2022 14:02:27 +0100 Subject: [PATCH 12/16] Initial change --- Executable/Program.cs | 8 ++-- Graph/Graph.cs | 73 +++++++++++++++++++++++++++++ Graph/Node.cs | 13 ++---- Graph/Utils.cs | 3 +- OpenStreetMap Importer/Importer.cs | 25 +++++----- astar/Astar.cs | 74 +++++++++++++----------------- astar/Route.cs | 25 +++++----- 7 files changed, 142 insertions(+), 79 deletions(-) create mode 100644 Graph/Graph.cs diff --git a/Executable/Program.cs b/Executable/Program.cs index 2adf137..9d94fe1 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(@"", true, logger); logger.level = LogLevel.DEBUG; Random r = new(); @@ -13,8 +13,8 @@ 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.nodes[r.Next(0, graph.nodes.Count - 1)]; + n2 = graph.nodes[r.Next(0, graph.nodes.Count - 1)]; + _route = new Astar().FindPath(graph, n1, n2, logger); } while (!_route.routeFound); } 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..74fa14f --- /dev/null +++ b/Graph/Graph.cs @@ -0,0 +1,73 @@ + +namespace Graph +{ + public class Graph + { + public List nodes { get; } + + public Graph() + { + this.nodes = new(); + } + + public bool AddNode(Node n) + { + if (!this.ContainsNode(n.id)) + { + this.nodes.Add(n); + return true; + } + else + { + return false; + } + } + + public Node? GetNode(ulong id) + { + foreach(Node n in this.nodes) + { + if (n.id == id) + return n; + } + return null; + } + + public bool ContainsNode(ulong id) + { + return this.GetNode(id) != null; + } + + public bool RemoveNode(ulong id) + { + Node? n = this.GetNode(id); + if(n != null) + { + this.nodes.Remove(n); + return true; + } + else + { + return false; + } + } + + public bool RemoveNode(Node n) + { + if (this.RemoveNode(n.id)) + { + this.nodes.Remove(n); + return true; + } + else + { + return false; + } + } + + public Node ClosestNodeToCoordinates(float lat, float lon) + { + throw new NotImplementedException(); + } + } +} diff --git a/Graph/Node.cs b/Graph/Node.cs index 1e56b42..b63bcd5 100644 --- a/Graph/Node.cs +++ b/Graph/Node.cs @@ -4,21 +4,16 @@ { public float lat { get; } public float lon { get; } + + public ulong id { 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) + public Node(ulong id, float lat, float lon) { + this.id = id; 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..f983c16 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.nodes.Count); 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(new Node(id, 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..80f4581 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 { + Dictionary timeRequired = new(); + Dictionary goalDistance = new(); + 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) + timeRequired.Add(start, 0); + goalDistance.Add(start, Convert.ToSingle(Utils.DistanceBetweenNodes(start, goal))); + while (toVisit.Count > 0 && timeRequired[toVisit[0]] < timeRequired[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 (timeRequired[e.neighbor] > timeRequired[currentNode] + e.time) { - e.neighbor.goalDistance = Convert.ToSingle(Utils.DistanceBetweenNodes(e.neighbor, goal)); - e.neighbor.timeRequired = currentNode.timeRequired + e.time; - e.neighbor.previousNode = currentNode; + goalDistance[e.neighbor] = Convert.ToSingle(Utils.DistanceBetweenNodes(e.neighbor, goal)); + timeRequired[e.neighbor] = timeRequired[currentNode] + e.time; + previousNode[e.neighbor] = currentNode; toVisit.Add(e.neighbor); } } @@ -53,18 +41,16 @@ namespace astar toVisit.Remove(currentNode); //"Mark" as visited toVisit.Sort(CompareDistance); } - if(goal.previousNode != null) + + if(previousNode[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(); @@ -72,20 +58,26 @@ namespace astar while(currentNode != start) { #pragma warning disable CS8604 // Route was found, so has to have a previous node - tempNodes.Add(currentNode.previousNode); + tempNodes.Add(previousNode[currentNode]); #pragma warning restore CS8604 - currentNode = currentNode.previousNode; + currentNode = previousNode[currentNode]; } 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, timeRequired[tempNodes[i]], goalDistance[tempNodes[i]])); #pragma warning restore CS8600, CS8604 - _route.distance += e.distance; + totalDistance += e.distance; } + Route _route = new Route(steps, true, totalDistance, timeRequired[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 (goalDistance[n1] < goalDistance[n2]) return -1; - else if (n1.goalDistance > n2.goalDistance) + else if (goalDistance[n1] > goalDistance[n2]) return 1; else return 0; } @@ -133,15 +125,15 @@ 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 (timeRequired[n1] < timeRequired[n2]) return -1; - else if (n1.timeRequired > n2.timeRequired) + else if (timeRequired[n1] > timeRequired[n2]) return 1; else return 0; } 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; } } } From 6d338018b577b0dfc619ef76a45c64d4feb98cdd Mon Sep 17 00:00:00 2001 From: C9Glax <13404778+C9Glax@users.noreply.github.com> Date: Sun, 13 Nov 2022 14:14:55 +0100 Subject: [PATCH 13/16] Null pointer fixes --- astar/Astar.cs | 94 +++++++++++++++++++++++++++++++++++++++----------- 1 file changed, 73 insertions(+), 21 deletions(-) diff --git a/astar/Astar.cs b/astar/Astar.cs index 80f4581..74954a7 100644 --- a/astar/Astar.cs +++ b/astar/Astar.cs @@ -6,9 +6,9 @@ namespace astar { public class Astar { - Dictionary timeRequired = new(); - Dictionary goalDistance = new(); - Dictionary previousNode = new(); + private Dictionary timeRequired = new(); + private Dictionary goalDistance = new(); + private Dictionary previousNode = new(); public Route FindPath(Graph.Graph graph, Node start, Node goal, Logger? logger) { @@ -16,9 +16,9 @@ namespace astar List toVisit = new(); toVisit.Add(start); Node currentNode = start; - timeRequired.Add(start, 0); - goalDistance.Add(start, Convert.ToSingle(Utils.DistanceBetweenNodes(start, goal))); - while (toVisit.Count > 0 && timeRequired[toVisit[0]] < timeRequired[goal]) + SetTimeRequiredToReach(start, 0); + SetDistanceToGoal(start, Convert.ToSingle(Utils.DistanceBetweenNodes(start, goal))); + while (toVisit.Count > 0 && GetTimeRequiredToReach(toVisit[0]) < GetTimeRequiredToReach(goal)) { if(currentNode == goal) { @@ -29,11 +29,11 @@ namespace astar //Check all neighbors of current node foreach (Edge e in currentNode.edges) { - if (timeRequired[e.neighbor] > timeRequired[currentNode] + e.time) + if (GetTimeRequiredToReach(e.neighbor) > GetTimeRequiredToReach(currentNode) + e.time) { - goalDistance[e.neighbor] = Convert.ToSingle(Utils.DistanceBetweenNodes(e.neighbor, goal)); - timeRequired[e.neighbor] = timeRequired[currentNode] + e.time; - previousNode[e.neighbor] = 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); } } @@ -42,7 +42,7 @@ namespace astar toVisit.Sort(CompareDistance); } - if(previousNode[goal] != null) + if(GetPreviousNodeOf(goal) != null) { logger?.Log(LogLevel.INFO, "Way found, shortest option."); currentNode = goal; @@ -57,10 +57,10 @@ namespace astar tempNodes.Add(goal); while(currentNode != start) { -#pragma warning disable CS8604 // Route was found, so has to have a previous node - tempNodes.Add(previousNode[currentNode]); -#pragma warning restore CS8604 - currentNode = previousNode[currentNode]; +#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(); @@ -71,12 +71,12 @@ namespace astar { #pragma warning disable CS8600, CS8604 // Route was found, so has to have an edge Edge e = tempNodes[i].GetEdgeToNode(tempNodes[i + 1]); - steps.Add(new Step(tempNodes[i], e, timeRequired[tempNodes[i]], goalDistance[tempNodes[i]])); + steps.Add(new Step(tempNodes[i], e, GetTimeRequiredToReach(tempNodes[i]), GetDistanceToGoal(tempNodes[i]))); #pragma warning restore CS8600, CS8604 totalDistance += e.distance; } - Route _route = new Route(steps, true, totalDistance, timeRequired[goal]); + Route _route = new Route(steps, true, totalDistance, GetTimeRequiredToReach(goal)); logger?.Log(LogLevel.INFO, "Path found"); @@ -111,9 +111,9 @@ namespace astar return 0; else { - if (goalDistance[n1] < goalDistance[n2]) + if (GetDistanceToGoal(n1) < GetDistanceToGoal(n2)) return -1; - else if (goalDistance[n1] > goalDistance[n2]) + else if (GetDistanceToGoal(n1) > GetDistanceToGoal(n2)) return 1; else return 0; } @@ -131,14 +131,66 @@ namespace astar return 0; else { - if (timeRequired[n1] < timeRequired[n2]) + if (GetTimeRequiredToReach(n1) < GetTimeRequiredToReach(n2)) return -1; - else if (timeRequired[n1] > timeRequired[n2]) + 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 From 3498fe5ae3b5343ff7e35a1ef0ef294d1921e929 Mon Sep 17 00:00:00 2001 From: C9Glax <13404778+C9Glax@users.noreply.github.com> Date: Sun, 13 Nov 2022 14:19:07 +0100 Subject: [PATCH 14/16] Changed protection of Graph.nodes to private --- Executable/Program.cs | 4 ++-- Graph/Graph.cs | 12 +++++++++++- OpenStreetMap Importer/Importer.cs | 2 +- 3 files changed, 14 insertions(+), 4 deletions(-) diff --git a/Executable/Program.cs b/Executable/Program.cs index 9d94fe1..03dfbcb 100644 --- a/Executable/Program.cs +++ b/Executable/Program.cs @@ -13,8 +13,8 @@ do { do { - n1 = graph.nodes[r.Next(0, graph.nodes.Count - 1)]; - n2 = graph.nodes[r.Next(0, graph.nodes.Count - 1)]; + n1 = graph.GetNodeAtIndex(r.Next(0, graph.GetNodeCount() - 1)); + n2 = graph.GetNodeAtIndex(r.Next(0, graph.GetNodeCount() - 1)); _route = new Astar().FindPath(graph, n1, n2, logger); } while (!_route.routeFound); } while (Console.ReadKey().Key.Equals(ConsoleKey.Enter)); \ No newline at end of file diff --git a/Graph/Graph.cs b/Graph/Graph.cs index 74fa14f..8ec209e 100644 --- a/Graph/Graph.cs +++ b/Graph/Graph.cs @@ -3,7 +3,7 @@ namespace Graph { public class Graph { - public List nodes { get; } + private List nodes { get; } public Graph() { @@ -23,6 +23,16 @@ namespace Graph } } + public Node GetNodeAtIndex(int i) + { + return this.nodes[i]; + } + + public int GetNodeCount() + { + return this.nodes.Count; + } + public Node? GetNode(ulong id) { foreach(Node n in this.nodes) diff --git a/OpenStreetMap Importer/Importer.cs b/OpenStreetMap Importer/Importer.cs index f983c16..4e9b9f6 100644 --- a/OpenStreetMap Importer/Importer.cs +++ b/OpenStreetMap Importer/Importer.cs @@ -34,7 +34,7 @@ namespace OpenStreetMap_Importer mapData.Position = 0; logger?.Log(LogLevel.INFO, "Importing Graph..."); Graph.Graph _graph = CreateGraph(mapData, occuranceCount, onlyJunctions, logger); - logger?.Log(LogLevel.DEBUG, "Loaded Nodes: {0}", _graph.nodes.Count); + logger?.Log(LogLevel.DEBUG, "Loaded Nodes: {0}", _graph.GetNodeCount()); mapData.Close(); occuranceCount.Clear(); From adda057b887b3c10a10b433141f10ff5171f72f8 Mon Sep 17 00:00:00 2001 From: C9Glax <13404778+C9Glax@users.noreply.github.com> Date: Sun, 13 Nov 2022 14:59:06 +0100 Subject: [PATCH 15/16] removed duplicate --- Executable/Program.cs | 6 ++-- Graph/Graph.cs | 56 +++++++++--------------------- Graph/Node.cs | 4 +-- OpenStreetMap Importer/Importer.cs | 2 +- 4 files changed, 22 insertions(+), 46 deletions(-) diff --git a/Executable/Program.cs b/Executable/Program.cs index 03dfbcb..a476d52 100644 --- a/Executable/Program.cs +++ b/Executable/Program.cs @@ -3,7 +3,7 @@ using Logging; using astar; Logger logger = new (LogType.CONSOLE, LogLevel.DEBUG); -Graph.Graph 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,8 +13,8 @@ do { do { - n1 = graph.GetNodeAtIndex(r.Next(0, graph.GetNodeCount() - 1)); - n2 = graph.GetNodeAtIndex(r.Next(0, graph.GetNodeCount() - 1)); + 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); } while (Console.ReadKey().Key.Equals(ConsoleKey.Enter)); \ No newline at end of file diff --git a/Graph/Graph.cs b/Graph/Graph.cs index 8ec209e..efeae28 100644 --- a/Graph/Graph.cs +++ b/Graph/Graph.cs @@ -3,29 +3,21 @@ namespace Graph { public class Graph { - private List nodes { get; } + private Dictionary nodes { get; } public Graph() { this.nodes = new(); } - public bool AddNode(Node n) + public bool AddNode(ulong id, Node n) { - if (!this.ContainsNode(n.id)) - { - this.nodes.Add(n); - return true; - } - else - { - return false; - } + return this.nodes.TryAdd(id, n); } - public Node GetNodeAtIndex(int i) + public Node NodeAtIndex(int i) { - return this.nodes[i]; + return this.nodes.Values.ToArray()[i]; } public int GetNodeCount() @@ -35,44 +27,30 @@ namespace Graph public Node? GetNode(ulong id) { - foreach(Node n in this.nodes) - { - if (n.id == id) - return n; - } - return null; + if (this.nodes.TryGetValue(id, out Node? n)) + return n; + else + return null; } public bool ContainsNode(ulong id) { - return this.GetNode(id) != null; + return this.nodes.ContainsKey(id); + } + + public bool ContainsNode(Node n) + { + return this.nodes.Values.Contains(n); } public bool RemoveNode(ulong id) { - Node? n = this.GetNode(id); - if(n != null) - { - this.nodes.Remove(n); - return true; - } - else - { - return false; - } + return this.nodes.Remove(id); } public bool RemoveNode(Node n) { - if (this.RemoveNode(n.id)) - { - this.nodes.Remove(n); - return true; - } - else - { - return false; - } + throw new NotImplementedException(); } public Node ClosestNodeToCoordinates(float lat, float lon) diff --git a/Graph/Node.cs b/Graph/Node.cs index b63bcd5..c450258 100644 --- a/Graph/Node.cs +++ b/Graph/Node.cs @@ -5,12 +5,10 @@ public float lat { get; } public float lon { get; } - public ulong id { get; } public HashSet edges { get; } - public Node(ulong id, float lat, float lon) + public Node(float lat, float lon) { - this.id = id; this.lat = lat; this.lon = lon; this.edges = new(); diff --git a/OpenStreetMap Importer/Importer.cs b/OpenStreetMap Importer/Importer.cs index 4e9b9f6..83c329d 100644 --- a/OpenStreetMap Importer/Importer.cs +++ b/OpenStreetMap Importer/Importer.cs @@ -118,7 +118,7 @@ namespace OpenStreetMap_Importer { float lat = Convert.ToSingle(_reader.GetAttribute("lat").Replace('.', ',')); float lon = Convert.ToSingle(_reader.GetAttribute("lon").Replace('.', ',')); - _graph.AddNode(new Node(id, lat, lon)); + _graph.AddNode(id, new Node(lat, lon)); logger?.Log(LogLevel.VERBOSE, "NODE {0} {1} {2} {3}", id, lat, lon, occuranceCount[id]); } } From 246ba9d1826ffca92328da7784740ec56121d8fe Mon Sep 17 00:00:00 2001 From: C9Glax <13404778+C9Glax@users.noreply.github.com> Date: Sun, 13 Nov 2022 15:03:35 +0100 Subject: [PATCH 16/16] Added log --- Executable/Program.cs | 1 + 1 file changed, 1 insertion(+) diff --git a/Executable/Program.cs b/Executable/Program.cs index 2adf137..15eae09 100644 --- a/Executable/Program.cs +++ b/Executable/Program.cs @@ -17,4 +17,5 @@ do n2 = graph[graph.Keys.ElementAt(r.Next(0, graph.Count - 1))]; _route = Astar.FindPath(ref 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