Merge pull request #7 from C9Glax/temp

dev

Executable/Program.cs

@@ -1,4 +1,19 @@
-Logging.Logger logger = new (Logging.LogType.CONSOLE, Logging.LogLevel.DEBUG);
+using Graph;
-Dictionary<UInt64, Graph.Node> nodes = OpenStreetMap_Importer.Importer.Import(@"", logger);
+using Logging;
-logger.level = Logging.LogLevel.VERBOSE;
+using astar;
-astar.Astar astar = new(nodes, logger);
+
+Logger logger = new (LogType.CONSOLE, LogLevel.DEBUG);
+Dictionary<ulong, Node> graph = OpenStreetMap_Importer.Importer.Import(@"", true, logger);
+logger.level = LogLevel.DEBUG;
+
+Random r = new();
+List<Node> path;
+Node n1, n2;
+do
+{
+    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));
+} while (Console.ReadKey().Key.Equals(ConsoleKey.Enter));

Graph/Edge.cs

@@ -2,12 +2,21 @@
 {
     public struct Edge
     {
+        public ulong id { get; }
         public Node neighbor { get; }
-        public double weight { get; }
+        public float weight { get; }
-        public Edge(Node neighbor, double weight)
+        public Edge(Node neighbor, float weight)
         {
             this.neighbor = neighbor;
             this.weight = weight;
+            this.id = 0;
+        }
+
+        public Edge(Node neighbor, float weight, ulong id)
+        {
+            this.neighbor = neighbor;
+            this.weight = weight;
+            this.id = id;
         }
     }
 }

Graph/Node.cs

@@ -4,21 +4,21 @@
     {
         public float lat { get; }
         public float lon { get; }
-        public List<Edge> edges { get; }
+        public HashSet<Edge> edges { get; }
 
         public Node previousNode { get; set; }
-        public double goalDistance { get; set; }
+        public float goalDistance { get; set; }
 
-        public double pathLength { get; set; }
+        public float pathLength { get; set; }
 
         public Node(float lat, float lon)
         {
             this.lat = lat;
             this.lon = lon;
-            this.edges = new List<Edge>();
+            this.edges = new();
             this.previousNode = nullnode;
-            this.goalDistance = double.MaxValue;
+            this.goalDistance = float.MaxValue;
-            this.pathLength = double.MaxValue;
+            this.pathLength = float.MaxValue;
         }
         public static Node nullnode = new(float.NaN, float.NaN);
     }

Graph/Utils.cs

@@ -2,12 +2,12 @@
 
     public struct Utils
     {
-        public static double DistanceBetweenNodes(Node n1, Node n2)
+        public static float DistanceBetweenNodes(Node n1, Node n2)
         {
             return DistanceBetweenCoordinates(n1.lat, n1.lon, n2.lat, n2.lon);
         }
 
-        public static double DistanceBetweenCoordinates(float lat1, float lon1, float lat2, float lon2)
+        public static float DistanceBetweenCoordinates(float lat1, float lon1, float lat2, float lon2)
         {
             const int earthRadius = 6371;
             double differenceLat = DegreesToRadians(lat2 - lat1);
@@ -19,7 +19,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 earthRadius * c;
+            return Convert.ToSingle(earthRadius * c);
         }
 
         private static double DegreesToRadians(double deg)

OpenStreetMap Importer/Importer.cs

@@ -1,4 +1,7 @@
-using Logging;
+#pragma warning disable CS8600
+#pragma warning disable CS8602
+#pragma warning disable CS8604
+using Logging;
 using System.Xml;
 using Graph;
 
@@ -7,282 +10,341 @@ namespace OpenStreetMap_Importer
     public class Importer
     {
 
-        public static Dictionary<ulong, Node> Import(string filePath = "", Logger ?logger = null)
+        private static XmlReaderSettings readerSettings = new()
-        {
-            List<Way> ways = new();
-            Dictionary<ulong, Node> nodes = new();
-            Stream mapData;
-            XmlReaderSettings readerSettings = new()
         {
             IgnoreWhitespace = true,
             IgnoreComments = true
         };
 
-            bool wayTag = false;
+        public static Dictionary<ulong, Node> Import(string filePath = "", bool onlyJunctions = true, Logger? logger = null)
-            Way currentWay = new();
+        {
-            Dictionary<ulong, ushort> count = new();
+            /*
+             * Count Node occurances when tag is "highway"
+             */
+            logger?.Log(LogLevel.INFO, "Counting Node-Occurances...");
+            Stream mapData = File.Exists(filePath) ? new FileStream(filePath, FileMode.Open, FileAccess.Read) : new MemoryStream(OSM_Data.map);
+            Dictionary<ulong, ushort> occuranceCount = CountNodeOccurances(mapData, logger);
+            mapData.Close();
+            logger?.Log(LogLevel.DEBUG, "Way Nodes: {0}", occuranceCount.Count);
 
             /*
-             * First iteration
+             * Import Nodes and Edges
-             * Import "ways" with a tag "highway"
-             * Count occurances of "nodes" to find junctions
              */
-            
+            logger?.Log(LogLevel.INFO, "Importing Graph...");
-            if (!File.Exists(filePath))
+            mapData = File.Exists(filePath) ? new FileStream(filePath, FileMode.Open, FileAccess.Read) : new MemoryStream(OSM_Data.map);
-            {
+            Dictionary<ulong, Node> graph = CreateGraph(mapData, occuranceCount, onlyJunctions, logger);
-                mapData = new MemoryStream(OSM_Data.map);
+            mapData.Close();
-                logger?.Log(LogLevel.INFO, "Filepath '{0}' does not exist. Using standard file.", filePath);
+            occuranceCount.Clear();
-            }
-            else
-            {
-                mapData = new FileStream(filePath, FileMode.Open, FileAccess.Read);
-                logger?.Log(LogLevel.INFO, "Using file '{0}'", filePath);
-            }
-            XmlReader reader = XmlReader.Create(mapData, readerSettings);
-            reader.MoveToContent();
-
-            logger?.Log(LogLevel.INFO, "Importing ways and counting nodes...");
-            while (reader.Read())
-            {
-                if (reader.Name == "way" && reader.IsStartElement())
-                {
-                    logger?.Log(LogLevel.VERBOSE, "WAY {0} nodes {1}", currentWay.highway.ToString(), currentWay.nodeIds.Count);
-                    if (currentWay.highway != Way.highwayType.NONE)
-                    {
-                        ways.Add(currentWay);
-                        foreach (ulong id in currentWay.nodeIds)
-                            if(count.ContainsKey(id))
-                                count[id]++;
-                            else
-                                count.TryAdd(id, 1);
-                    }
-                    wayTag = true;
-                    currentWay = new Way();
-                }
-                else if (reader.Name == "tag" && wayTag)
-                {
-#pragma warning disable CS8600 //tags will always have a value and key
-#pragma warning disable CS8604
-                    string value = reader.GetAttribute("v");
-                    string key = reader.GetAttribute("k");
-                    logger?.Log(LogLevel.VERBOSE, "TAG {0} {1}", key, value);
-#pragma warning restore CS8600
-                    switch (key)
-                    {
-                        case "highway":
-                            currentWay.SetHighwayType(value);
-                            break;
-                        case "oneway":
-                            switch (value)
-                            {
-                                case "yes":
-                                    currentWay.oneway = true;
-                                    break;
-                               /*case "no":
-                                    currentWay.oneway = false;
-                                    break;*/
-                                case "-1":
-                                    currentWay.oneway = true;
-                                    currentWay.direction = Way.wayDirection.backward;
-                                    break;
-                            }
-                            break;
-                            /*case "name":
-                            
-                            break;*/
-                    }
-#pragma warning restore CS8604
-                }
-                else if(reader.Name == "nd" && wayTag)
-                {
-                    ulong id = Convert.ToUInt64(reader.GetAttribute("ref"));
-                    currentWay.nodeIds.Add(id);
-                    logger?.Log(LogLevel.VERBOSE, "nd: {0}", id);
-                }
-                else if(reader.Name == "node")
-                {
-                    wayTag = false;
-                }
-            }
-
-            logger?.Log(LogLevel.DEBUG, "Loaded Ways: {0} Required Nodes: {1}", ways.Count, count.Count);
-
-            reader.Close();
             GC.Collect();
-            if (!File.Exists(filePath))
+            logger?.Log(LogLevel.DEBUG, "Loaded Nodes: {0}", graph.Count);
-            {
-                mapData = new MemoryStream(OSM_Data.map);
-                logger?.Log(LogLevel.INFO, "Filepath '{0}' does not exist. Using standard file.", filePath);
-            }
-            else
-            {
-                mapData = new FileStream(filePath, FileMode.Open, FileAccess.Read);
-                logger?.Log(LogLevel.INFO, "Using file '{0}'", filePath);
-            }
-            reader = XmlReader.Create(mapData, readerSettings);
-            reader.MoveToContent();
 
-            /*
+            return graph;
-             * Second iteration
-             * Import nodes that are needed by the "ways"
-             */
-            logger?.Log(LogLevel.INFO, "Importing nodes...");
-            while (reader.Read())
-            {
-                if (reader.Name == "node")
-                {
-                    ulong id = Convert.ToUInt64(reader.GetAttribute("id"));
-                    if (count.ContainsKey(id))
-                    {
-#pragma warning disable CS8602 //node will always have a lat and lon
-                        float lat = Convert.ToSingle(reader.GetAttribute("lat").Replace('.', ','));
-                        float lon = Convert.ToSingle(reader.GetAttribute("lon").Replace('.', ','));
-#pragma warning restore CS8602
-                        nodes.TryAdd(id, new Node(lat, lon));
-                        logger?.Log(LogLevel.VERBOSE, "NODE {0} {1} {2} {3}", id, lat, lon, count[id]);
-                    }
-                }
-            }
-            /*
-             * Add connections between nodes (only junctions, e.g. nodes are referenced more than once)
-             * Remove non-junction nodes
-             */
-            logger?.Log(LogLevel.INFO, "Calculating Edges and distances...");
-            ulong edges = 0;
-            foreach(Way way in ways)
-            {
-                Node junction1 = nodes[way.nodeIds[0]];
-                Node junction2;
-                double weight = 0;
-                //Iterate Node-ids in current way forwards or backwards (depending on way.direction)
-                if (way.direction == Way.wayDirection.forward)
-                {
-                    for (int index = 0; index < way.nodeIds.Count - 1; index++)
-                    {
-                        Node currentNode = nodes[way.nodeIds[index]];
-                        Node nextNode = nodes[way.nodeIds[index + 1]];
-                        weight += Utils.DistanceBetweenNodes(currentNode, nextNode);
-                        if (count[way.nodeIds[index + 1]] > 1 || index == way.nodeIds.Count - 2)
-                        {
-                            /*
-                             * If Node is referenced more than once => Junction
-                             * If Node is last node of way => Junction
-                             * Add an edge between two junctions
-                             */
-                            junction2 = nodes[way.nodeIds[index + 1]];
-                            junction1.edges.Add(new Edge(junction2, weight));
-                            logger?.Log(LogLevel.VERBOSE, "EDGE {0} -- {1} --> {2}", way.nodeIds[index], weight, way.nodeIds[index + 1]);
-                            edges++;
-
-                            if (!way.oneway)
-                            {
-                                junction2.edges.Add(new Edge(junction1, weight));
-                                logger?.Log(LogLevel.VERBOSE, "EDGE {0} -- {1} --> {2}", way.nodeIds[index + 1], weight, way.nodeIds[index]);
-                                edges++;
         }
 
-                            junction1 = junction2;
+        private static Dictionary<ulong, ushort> CountNodeOccurances(Stream mapData, Logger? logger = null)
-                            weight = 0;
-                        }
-                    }
-                }
-                else
         {
-                    for (int index = way.nodeIds.Count - 2; index > 0; index--)
+            Dictionary<ulong, ushort> _occurances = new();
-                    {
-                        Node currentNode = nodes[way.nodeIds[index]];
-                        Node nextNode = nodes[way.nodeIds[index - 1]];
-                        weight += Utils.DistanceBetweenNodes(currentNode, nextNode);
-                        if (count[way.nodeIds[index - 1]] > 1 || index == 1)
-                        {
-                            /*
-                             * If Node is referenced more than once => Junction
-                             * If Node is last node of way => Junction
-                             * Add an edge between two junctions
-                             */
-                            junction2 = nodes[way.nodeIds[index - 1]];
-                            junction1.edges.Add(new Edge(junction2, weight));
-                            logger?.Log(LogLevel.VERBOSE, "EDGE {0} -- {1} --> {2}", way.nodeIds[index], weight, way.nodeIds[index - 1]);
-                            edges++;
 
-                            if (!way.oneway)
+            XmlReader _reader = XmlReader.Create(mapData, readerSettings);
-                            {
+            XmlReader _wayReader;
-                                junction2.edges.Add(new Edge(junction1, weight));
+            _reader.MoveToContent();
-                                logger?.Log(LogLevel.VERBOSE, "EDGE {0} -- {1} --> {2}", way.nodeIds[index - 1], weight, way.nodeIds[index]);
-                                edges++;
-                            }
 
-                            junction1 = junction2;
+            bool _isHighway;
-                            weight = 0;
+            List<ulong> _currentIds = new();
+
+            while (_reader.ReadToFollowing("way"))
+            {
+                _isHighway = false;
+                _currentIds.Clear();
+                _wayReader = _reader.ReadSubtree();
+                while (_wayReader.Read())
+                {
+                    if (_reader.Name == "tag" && _reader.GetAttribute("k").Equals("highway"))
+                    {
+                        try
+                        {
+                            if (!Enum.Parse(typeof(Way.type), _reader.GetAttribute("v"), true).Equals(Way.type.NONE))
+                                _isHighway = true;
+                        }
+                        catch (ArgumentException) { };
+                    }
+                    else if(_reader.Name == "nd")
+                    {
+                        try
+                        {
+                            _currentIds.Add(Convert.ToUInt64(_reader.GetAttribute("ref")));
+                        }
+                        catch (FormatException) { };
                     }
                 }
+                if (_isHighway)
+                {
+                    foreach(ulong _id in _currentIds)
+                    {
+                        if (!_occurances.TryAdd(_id, 1))
+                            _occurances[_id]++;
                     }
                 }
-            reader.Close();
+                _wayReader.Close();
+            }
+            _reader.Close();
             GC.Collect();
 
-            logger?.Log(LogLevel.DEBUG, "Loaded Edges: {0}", edges);
+            return _occurances;
-            return nodes.Where(node => count[node.Key] > 1).ToDictionary(node => node.Key, node => node.Value);
+        }
+
+        private static Dictionary<ulong, Node> CreateGraph(Stream mapData, Dictionary<ulong, ushort> occuranceCount, bool onlyJunctions, Logger? logger = null)
+        {
+            Dictionary<ulong, Node> _tempAll = new();
+            Dictionary<ulong, Node> _graph = new();
+            Way _currentWay;
+            Node _n1, _n2, _currentNode;
+            float _weight, _distance = 0;
+
+            XmlReader _reader = XmlReader.Create(mapData, readerSettings);
+            XmlReader _wayReader;
+            _reader.MoveToContent();
+
+            while (_reader.Read())
+            {
+                if(_reader.Name == "node")
+                {
+                    ulong id = Convert.ToUInt64(_reader.GetAttribute("id"));
+                    if (occuranceCount.ContainsKey(id))
+                    {
+                        float lat = Convert.ToSingle(_reader.GetAttribute("lat").Replace('.', ','));
+                        float lon = Convert.ToSingle(_reader.GetAttribute("lon").Replace('.', ','));
+                        _tempAll.Add(id, new Node(lat, lon));
+                        logger?.Log(LogLevel.VERBOSE, "NODE {0} {1} {2} {3}", id, lat, lon, occuranceCount[id]);
+                    }
+                }
+                else if(_reader.Name == "way")
+                {
+                    _wayReader = _reader.ReadSubtree();
+                    _currentWay = new();
+                    while (_wayReader.Read())
+                    {
+                        _wayReader = _reader.ReadSubtree();
+                        _currentWay.AddTag("id", _reader.GetAttribute("id"));
+                        while (_wayReader.Read())
+                        {
+                            if (_reader.Name == "tag")
+                            {
+                                string _value = _reader.GetAttribute("v");
+                                string _key = _reader.GetAttribute("k");
+                                logger?.Log(LogLevel.VERBOSE, "TAG {0} {1}", _key, _value);
+                                _currentWay.AddTag(_key, _value);
+                            }
+                            else if (_reader.Name == "nd")
+                            {
+                                ulong _id = Convert.ToUInt64(_reader.GetAttribute("ref"));
+                                _currentWay.nodeIds.Add(_id);
+                                logger?.Log(LogLevel.VERBOSE, "nd: {0}", _id);
+                            }
+                        }
+                    }
+                    _wayReader.Close();
+
+                    if (!_currentWay.GetHighwayType().Equals(Way.type.NONE))
+                    {
+                        logger?.Log(LogLevel.VERBOSE, "WAY Nodes: {0} Type: {1}", _currentWay.nodeIds.Count, _currentWay.GetHighwayType());
+                        if (!onlyJunctions)
+                        {
+                            for (int _nodeIdIndex = 0; _nodeIdIndex < _currentWay.nodeIds.Count - 1; _nodeIdIndex++)
+                            {
+                                _n1 = _tempAll[_currentWay.nodeIds[_nodeIdIndex]];
+                                _n2 = _tempAll[_currentWay.nodeIds[_nodeIdIndex + 1]];
+                                _weight = Utils.DistanceBetweenNodes(_n1, _n2) * 1000 / _currentWay.GetMaxSpeed();
+                                if (!_currentWay.IsOneWay())
+                                {
+                                    _n1.edges.Add(new Edge(_n2, _weight));
+                                    _n2.edges.Add(new Edge(_n1, _weight));
+                                }
+                                else if (_currentWay.IsForward())
+                                {
+                                    _n1.edges.Add(new Edge(_n2, _weight));
+                                }
+                                else
+                                {
+                                    _n2.edges.Add(new Edge(_n1, _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]);
+                            }
+                            _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]];
+                                _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);
+                                    if (!_currentWay.IsOneWay())
+                                    {
+                                        _n1.edges.Add(new Edge(_n2, _weight, _currentWay.GetId()));
+                                        _n2.edges.Add(new Edge(_n1, _weight, _currentWay.GetId()));
+                                    }
+                                    else if (_currentWay.IsForward())
+                                    {
+                                        _n1.edges.Add(new Edge(_n2, _weight, _currentWay.GetId()));
+                                    }
+                                    else
+                                    {
+                                        _n2.edges.Add(new Edge(_n1, _weight, _currentWay.GetId()));
+                                    }
+                                }
+                                else
+                                {
+                                    _tempAll.Remove(_currentWay.nodeIds[_nodeIdIndex]);
+                                }
+                                _currentNode = _n2;
+                            }
+                        }
+                    }
+                }
+            }
+            _reader.Close();
+            GC.Collect();
+            return _graph;
         }
 
         internal struct Way
         {
             public List<ulong> nodeIds;
-            public bool oneway;
+            private Dictionary<string, object> tags;
-            public wayDirection direction;
+
-            public highwayType highway;
+
-            public enum wayDirection { forward, backward }
+            public Dictionary<type, int> speed = new() {
-            public enum highwayType : uint
+                { type.NONE, 1 },
-            {
+                { type.motorway, 130 },
-                NONE = 1,
+                { type.trunk, 125 },
-                motorway = 130,
+                { type.primary, 110 },
-                trunk = 125,
+                { type.secondary, 100 },
-                primary = 110,
+                { type.tertiary, 90 },
-                secondary = 100,
+                { type.unclassified, 40 },
-                tertiary = 80,
+                { type.residential, 20 },
-                unclassified = 40,
+                { type.motorway_link, 50 },
-                residential = 23,
+                { type.trunk_link, 50 },
-                motorway_link = 55,
+                { type.primary_link, 30 },
-                trunk_link = 50,
+                { type.secondary_link, 25 },
-                primary_link = 30,
+                { type.tertiary_link, 25 },
-                secondary_link = 25,
+                { type.living_street, 20 },
-                tertiary_link = 24,
+                { type.service, 10 },
-                living_street = 20,
+                { type.pedestrian, 10 },
-                service = 14,
+                { type.track, 1 },
-                pedestrian = 12,
+                { type.bus_guideway, 5 },
-                track = 6,
+                { type.escape, 1 },
-                bus_guideway = 15,
+                { type.raceway, 1 },
-                escape = 3,
+                { type.road, 25 },
-                raceway = 4,
+                { type.busway, 5 },
-                road = 28,
+                { type.footway, 1 },
-                busway = 13,
+                { type.bridleway, 1 },
-                footway = 8,
+                { type.steps, 1 },
-                bridleway = 7,
+                { type.corridor, 1 },
-                steps = 5,
+                { type.path, 10 },
-                corridor = 9,
+                { type.cycleway, 5 },
-                path = 10,
+                { type.construction, 1 }
-                cycleway = 11,
+            };
-                construction = 2
+            public enum type { NONE, motorway, trunk, primary, secondary, tertiary, unclassified, residential, motorway_link, trunk_link, primary_link, secondary_link, tertiary_link, living_street, service, pedestrian, track, bus_guideway, escape, raceway, road, busway, footway, bridleway, steps, corridor, path, cycleway, construction }
-            }
 
 
             public Way()
             {
                 this.nodeIds = new List<ulong>();
-                this.oneway = false;
+                this.tags = new();
-                this.direction = wayDirection.forward;
-                this.highway = highwayType.NONE;
             }
-
+            public void AddTag(string key, string value, Logger? logger = null)
-            public void SetHighwayType(string waytype)
             {
+                switch (key)
+                {
+                    case "highway":
                         try
                         {
-                    this.highway = (highwayType)Enum.Parse(typeof(highwayType), waytype, true);
+                            this.tags.Add(key, (type)Enum.Parse(typeof(type), value, true));
-                }catch(Exception)
+                            if (this.GetMaxSpeed().Equals((int)type.NONE))
                             {
-                    this.highway = highwayType.NONE;
+                                this.tags["maxspeed"] = (int)this.GetHighwayType();
-                }
+                            }
+                        }
+                        catch (ArgumentException)
+                        {
+                            this.tags.Add(key, type.NONE);
+                        }
+                        break;
+                    case "maxspeed":
+                        try
+                        {
+                            if (this.tags.ContainsKey("maxspeed"))
+                                this.tags["maxspeed"] = Convert.ToInt32(value);
+                            else
+                                this.tags.Add(key, Convert.ToInt32(value));
+                        }
+                        catch (FormatException)
+                        {
+                            this.tags.Add(key, (int)this.GetHighwayType());
+                        }
+                        break;
+                    case "oneway":
+                        switch (value)
+                        {
+                            case "yes":
+                                this.tags.Add(key, true);
+                                break;
+                            case "-1":
+                                this.tags.Add("forward", false);
+                                break;
+                            case "no":
+                                this.tags.Add(key, false);
+                                break;
+                        }
+                        break;
+                    case "id":
+                        this.tags.Add(key, Convert.ToUInt64(value));
+                        break;
+                    default:
+                        logger?.Log(LogLevel.VERBOSE, "Tag {0} - {1} was not added.", key, value);
+                        break;
+                }
+            }
+
+            public ulong GetId()
+            {
+                return this.tags.ContainsKey("id") ? (ulong)this.tags["id"] : 0;
+            }
+
+            public type GetHighwayType()
+            {
+                return this.tags.ContainsKey("highway") ? (type)this.tags["highway"] : type.NONE;
+            }
+
+            public bool IsOneWay()
+            {
+                return this.tags.ContainsKey("oneway") ? (bool)this.tags["oneway"] : false;
+            }
+
+            public int GetMaxSpeed()
+            {
+                return this.tags.ContainsKey("maxspeed") ? (int)this.tags["maxspeed"] : (int)this.GetHighwayType();
+
+            }
+
+            public bool IsForward()
+            {
+                return this.tags.ContainsKey("forward") ? (bool)this.tags["forward"] : true;
             }
         }
     }

README.md

@@ -1,4 +1,4 @@
-# astar
+### astar
 Test of A* Algorithm
 
 # Project

astar/Astar.cs

@@ -6,26 +6,6 @@ namespace astar
     public class Astar
     {
 
-        /*
-         * Loads the graph, chooses two nodes at random and calls a*
-         */
-        public Astar(Dictionary<UInt64, Node> nodes, Logger ?logger = null)
-        {
-            Random r = new();
-            List<Node> path = new();
-            while(path.Count < 1)
-            {
-                Node n1 = nodes[nodes.Keys.ElementAt(r.Next(0, nodes.Count - 1))];
-                Node n2 = nodes[nodes.Keys.ElementAt(r.Next(0, nodes.Count - 1))];
-                logger?.Log(LogLevel.INFO, "From {0} - {1} to {2} - {3} Distance {4}", n1.lat, n1.lon, n2.lat, n2.lon, Utils.DistanceBetweenNodes(n1,n2));
-                path = FindPath(ref nodes, n1, n2, ref logger);
-            }
-
-            logger?.Log(LogLevel.INFO, "Path found");
-            foreach (Node n in path)
-                logger?.Log(LogLevel.INFO, "lat {0:000.00000} lon {1:000.00000} traveled {2:0000.00} / {3:0000.00} beeline {4:0000.00}", n.lat, n.lon, n.pathLength, path.ElementAt(path.Count-1).pathLength, n.goalDistance);
-        }
-
         /*
          * Resets the calculated previous nodes and distance to goal
          */
@@ -34,77 +14,55 @@ namespace astar
             foreach(Node n in nodes.Values)
             {
                 n.previousNode = Node.nullnode;
-                n.goalDistance = double.MaxValue;
+                n.goalDistance = float.MaxValue;
+                n.pathLength = float.MaxValue;
             }
         }
 
         /*
          * 
          */
-        private static List<Node> FindPath(ref Dictionary<ulong, Node> nodes, Node start, Node goal, ref Logger? logger)
+        public static bool FindPath(ref Dictionary<ulong, Node> nodes, Node start, Node goal, out List<Node> path, Logger? logger)
         {
+            path = new List<Node>();
+            logger?.Log(LogLevel.INFO, "From {0} - {1} to {2} - {3} Distance {4}", start.lat, start.lon, goal.lat, goal.lon, Utils.DistanceBetweenNodes(start, goal));
             Reset(ref nodes);
             List<Node> toVisit = new();
             toVisit.Add(start);
             Node currentNode = start;
             start.pathLength = 0;
             start.goalDistance = Utils.DistanceBetweenNodes(start, goal);
-            while (currentNode != goal && toVisit.Count > 0)
+            while (toVisit.Count > 0 && toVisit[0].pathLength < goal.pathLength)
             {
-                currentNode = toVisit.First();
-                logger?.Log(LogLevel.VERBOSE, "toVisit-length: {0} path: {1} goal: {2}", toVisit.Count, currentNode.pathLength, currentNode.goalDistance);
-                //Check all neighbors of current node
-                foreach (Edge e in currentNode.edges)
-                {
-                    if (e.neighbor.goalDistance == double.MaxValue)
-                        e.neighbor.goalDistance = Utils.DistanceBetweenNodes(e.neighbor, goal);
-                    if (e.neighbor.pathLength > currentNode.pathLength + e.weight)
-                    {
-                        e.neighbor.pathLength = currentNode.pathLength + e.weight;
-                        e.neighbor.previousNode = currentNode;
-                        toVisit.Add(e.neighbor);
-                    }
-                }
-                toVisit.Remove(currentNode); //"Mark" as visited
-                toVisit.Sort(CompareDistanceToGoal);
-            }
-
-            List<Node> path = new();
-
                 if(currentNode == goal)
-            {
-                if(toVisit[0].pathLength < goal.pathLength)
                 {
                     logger?.Log(LogLevel.INFO, "Way found, checking for shorter option.");
-                    while (toVisit[0].pathLength < goal.pathLength)
+                }
-                    {
                 currentNode = toVisit.First();
-                        logger?.Log(LogLevel.VERBOSE, "toVisit-length: {0} path: {1} goal: {2}", toVisit.Count, currentNode.pathLength, currentNode.goalDistance);
+                logger?.Log(LogLevel.VERBOSE, "toVisit-length: {0} path-length: {1} goal-distance: {2}", toVisit.Count, currentNode.pathLength, currentNode.goalDistance);
                 //Check all neighbors of current node
                 foreach (Edge e in currentNode.edges)
                 {
-                            if (e.neighbor.goalDistance == double.MaxValue)
-                                e.neighbor.goalDistance = Utils.DistanceBetweenNodes(e.neighbor, goal);
                     if (e.neighbor.pathLength > currentNode.pathLength + e.weight)
                     {
+                        e.neighbor.goalDistance = Utils.DistanceBetweenNodes(e.neighbor, goal);
                         e.neighbor.pathLength = currentNode.pathLength + e.weight;
                         e.neighbor.previousNode = currentNode;
                         toVisit.Add(e.neighbor);
                     }
                 }
+
                 toVisit.Remove(currentNode); //"Mark" as visited
-                        toVisit.Sort(CompareDistanceToGoal);
+                toVisit.Sort(CompareDistance);
             }
-                }
+            if(goal.previousNode != Node.nullnode)
-                else
             {
                 logger?.Log(LogLevel.INFO, "Way found, shortest option.");
             }
-            }
             else
             {
                 logger?.Log(LogLevel.INFO, "No path between {0} - {1} and {2} - {3}", start.lat, start.lon, goal.lat, goal.lon);
-                return path;
+                return false;
             }
 
             path.Add(goal);
@@ -114,13 +72,33 @@ namespace astar
                 currentNode = currentNode.previousNode;
             }
             path.Reverse();
-            return path;
+
+            logger?.Log(LogLevel.INFO, "Path found");
+            float distance = 0;
+            Node prev = Node.nullnode;
+            TimeSpan totalTime = TimeSpan.FromSeconds(path.ElementAt(path.Count - 1).pathLength);
+            
+            foreach (Node n in path)
+            {
+                if(!prev.Equals(Node.nullnode))
+                {
+                    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);
+            }
+
+
+            return true;
         }
         
         /*
          * Compares two nodes and returns the node closer to the goal
+         * -1 => n1 smaller n2
+         *  0 => n1 equal n2
+         *  1 => n1 larger n2
          */
-        private static int CompareDistanceToGoal(Node n1, Node n2)
+        private static int CompareDistance(Node n1, Node n2)
         {
             if (n1 == null || n2 == null)
                 return 0;
@@ -133,5 +111,25 @@ namespace astar
                 else return 0;
             }
         }
+
+        /*
+         * Compares two nodes and returns the node with the shorter path
+         * -1 => n1 smaller n2
+         *  0 => n1 equal n2
+         *  1 => n1 larger n2
+         */
+        private static int ComparePathLength(Node n1, Node n2)
+        {
+            if (n1 == null || n2 == null)
+                return 0;
+            else
+            {
+                if (n1.pathLength < n2.pathLength)
+                    return -1;
+                else if (n1.pathLength > n2.pathLength)
+                    return 1;
+                else return 0;
+            }
+        }
     }
 }