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; } } }