Merge pull request #8 from C9Glax/DatastructureAndAlgoChange

Structure Change

Executable/Program.cs

@@ -3,7 +3,7 @@ using Logging;
 using astar;
 
 Logger logger = new (LogType.CONSOLE, LogLevel.DEBUG);
-Dictionary<ulong, Node> 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))];
+        n1 = graph.NodeAtIndex(r.Next(0, graph.GetNodeCount() - 1));
-        n2 = graph[graph.Keys.ElementAt(r.Next(0, graph.Count - 1))];
+        n2 = graph.NodeAtIndex(r.Next(0, graph.GetNodeCount() - 1));
-        _route = Astar.FindPath(ref graph, n1, n2, logger);
+        _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));

Graph/Graph.cs

@@ -0,0 +1,61 @@
+
+namespace Graph
+{
+    public class Graph
+    {
+        private Dictionary<ulong, Node> 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();
+        }
+    }
+}

Graph/Node.cs

@@ -4,21 +4,14 @@
     {
         public float lat { get; }
         public float lon { get; }
+
         public HashSet<Edge> 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)

Graph/Utils.cs

@@ -1,6 +1,5 @@
-namespace Graph
+namespace Graph.Utils
 {
-
     public struct Utils
     {
         public static double DistanceBetweenNodes(Node n1, Node n2)

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<ulong, Node> 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<ulong, Node> graph = CreateGraph(mapData, occuranceCount, onlyJunctions, logger);
+            Graph.Graph _graph = CreateGraph(mapData, occuranceCount, onlyJunctions, logger);
-            logger?.Log(LogLevel.DEBUG, "Loaded Nodes: {0}", graph.Count);
+            logger?.Log(LogLevel.DEBUG, "Loaded Nodes: {0}", _graph.GetNodeCount());
 
             mapData.Close();
             occuranceCount.Clear();
             GC.Collect();
 
-            return graph;
+            return _graph;
         }
 
         private static Dictionary<ulong, ushort> CountNodeOccurances(Stream mapData, Logger? logger = null)
@@ -97,9 +98,9 @@ namespace OpenStreetMap_Importer
             return _occurances;
         }
 
-        private static Dictionary<ulong, Node> CreateGraph(Stream mapData, Dictionary<ulong, ushort> occuranceCount, bool onlyJunctions, Logger? logger = null)
+        private static Graph.Graph CreateGraph(Stream mapData, Dictionary<ulong, ushort> occuranceCount, bool onlyJunctions, Logger? logger = null)
         {
-            Dictionary<ulong, Node> _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]];
+                                _n1 = _graph.GetNode(_currentWay.nodeIds[_nodeIdIndex]);
-                                _n2 = _graph[_currentWay.nodeIds[_nodeIdIndex + 1]];
+                                _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;
                             }

astar/Astar.cs

@@ -1,51 +1,39 @@
 using Logging;
 using Graph;
+using Graph.Utils;
 
 namespace astar
 {
     public class Astar
     {
+        private Dictionary<Node, float> timeRequired = new();
+        private Dictionary<Node, float> goalDistance = new();
+        private Dictionary<Node, Node> previousNode = new();
 
-        /*
+        public Route FindPath(Graph.Graph graph, Node start, Node goal, Logger? logger)
-         * Resets the calculated previous nodes and distance to goal
-         */
-        private static void Reset(ref Dictionary<ulong, Node> nodes)
         {
-            foreach(Node n in nodes.Values)
-            {
-                n.previousNode = null;
-                n.goalDistance = float.MaxValue;
-                n.timeRequired = float.MaxValue;
-            }
-        }
-
-
-        public static Route FindPath(ref Dictionary<ulong, Node> 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<Node> toVisit = new();
             toVisit.Add(start);
             Node currentNode = start;
-            start.timeRequired = 0;
+            SetTimeRequiredToReach(start, 0);
-            start.goalDistance = Convert.ToSingle(Utils.DistanceBetweenNodes(start, goal));
+            SetDistanceToGoal(start, Convert.ToSingle(Utils.DistanceBetweenNodes(start, goal)));
-            while (toVisit.Count > 0 && toVisit[0].timeRequired < goal.timeRequired)
+            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));
+                        SetDistanceToGoal(e.neighbor, Convert.ToSingle(Utils.DistanceBetweenNodes(e.neighbor, goal)));
-                        e.neighbor.timeRequired = currentNode.timeRequired + e.time;
+                        SetTimeRequiredToReach(e.neighbor, GetTimeRequiredToReach(currentNode) + e.time);
-                        e.neighbor.previousNode = currentNode;
+                        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 new Route(new List<Step>(), false, float.MaxValue, float.MaxValue);
-                return _route;
             }
 
             List<Node> tempNodes = new();
             tempNodes.Add(goal);
             while(currentNode != start)
             {
-#pragma warning disable CS8604 // Route was found, so has to have a previous node
+#pragma warning disable CS8604, CS8600 // Route was found, so has to have a previous node
-                tempNodes.Add(currentNode.previousNode);
+                tempNodes.Add(GetPreviousNodeOf(currentNode));
-#pragma warning restore CS8604
+                currentNode = GetPreviousNodeOf(currentNode);
-                currentNode = currentNode.previousNode;
+#pragma warning restore CS8604, CS8600
             }
             tempNodes.Reverse();
+
+            List<Step> 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;
+        }
     }
 }

astar/Route.cs

@@ -4,19 +4,17 @@ namespace astar
     public class Route
     {
         public List<Step> steps { get; }
-        public bool routeFound { get; set; }
+        public bool routeFound { get; }
-        public float distance { get; set; }
+        public float distance { get; }
-        public float time { get; set; }
+        public float time { get; }
 
-        public Route()
-        {
-            this.steps = new();
-            this.distance = 0;
-        }
 
-        public void AddStep(Node start, Edge way)
+        public Route(List<Step> 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;
         }
     }
 }