Housekeeping

More extensive logging

OSMDatastructure/OSMEdge.cs

@@ -44,7 +44,7 @@ public class OsmEdge
         highway, oneway, footway, sidewalk, cycleway, busway, forward, maxspeed, name, surface, lanes, access, tracktype, id, EMPTY
     }
 
-    public static Dictionary<wayType, byte> speedcar = new() {
+    private static readonly Dictionary<wayType, byte> defaultSpeedCar = new() {
         { wayType.NONE, 0 },
         { wayType.motorway, 110 },
         { wayType.trunk, 100 },
@@ -76,7 +76,7 @@ public class OsmEdge
         { wayType.construction, 0 }
     };
 
-    public static Dictionary<wayType, byte> speedped = new() {
+    private static readonly Dictionary<wayType, byte> defaultSpeedPedestrian = new() {
         { wayType.NONE, 0 },
         { wayType.motorway, 0 },
         { wayType.trunk, 0 },
@@ -115,7 +115,7 @@ public class OsmEdge
     {
         KeyValuePair<tagType, object> tag = ConvertToTag(key, value);
         if(tag.Key != tagType.EMPTY)
-            this.tags.Add(tag.Key, tag.Value);
+            tags.Add(tag.Key, tag.Value);
     }
 
     public static KeyValuePair<tagType, object> ConvertToTag(string key, string value)
@@ -134,7 +134,11 @@ public class OsmEdge
             case "maxspeed":
                 try
                 {
-                    return new KeyValuePair<tagType, object>(tagType.maxspeed, Convert.ToByte(value));
+                    byte speed = Convert.ToByte(value);
+                    if(speed == 255)
+                        return new KeyValuePair<tagType, object>(tagType.EMPTY, 0);
+                    else
+                        return new KeyValuePair<tagType, object>(tagType.maxspeed, speed);
                 }
                 catch (Exception)
                 {
@@ -161,36 +165,64 @@ public class OsmEdge
 
     public ulong GetId()
     {
-        return this.tags.ContainsKey(tagType.id) ? (ulong)this.tags[tagType.id] : 0;
+        return tags.ContainsKey(tagType.id) ? (ulong)tags[tagType.id] : 0;
     }
 
     public wayType GetHighwayType()
     {
-        return this.tags.ContainsKey(tagType.highway) ? (wayType)this.tags[tagType.highway] : wayType.NONE;
+        if (!tags.ContainsKey(tagType.highway))
+            throw new Exception("Not a road?");
+        return (wayType)tags[tagType.highway];
     }
 
     public bool IsOneWay()
     {
-        return this.tags.ContainsKey(tagType.oneway) && (bool)this.tags[tagType.oneway];
+        return tags.ContainsKey(tagType.oneway) && (bool)tags[tagType.oneway];
     }
 
-    public byte? GetMaxSpeed(speedType type)
+    public byte GetMaxSpeed(speedType type)
     {
-        if(type == speedType.road)
+        switch (type)
         {
-            return this.tags.ContainsKey(tagType.maxspeed) ? (byte)this.tags[tagType.maxspeed] : null;
+            case speedType.road:
+                return tags.ContainsKey(tagType.maxspeed) ? (byte)tags[tagType.maxspeed] : (byte)0;
+            case speedType.car:
+                return tags.ContainsKey(tagType.maxspeed) ? (byte)tags[tagType.maxspeed] : defaultSpeedCar[GetHighwayType()];
+            case speedType.pedestrian:
+                return defaultSpeedPedestrian[GetHighwayType()];
+            default:
+                return 0;
         }
-        if(type == speedType.car)
-        {
-            return this.tags.ContainsKey(tagType.maxspeed)
-                ? (byte)this.tags[tagType.maxspeed]
-                : speedcar[this.GetHighwayType()];
-        }
-        return speedped[this.GetHighwayType()];
     }
 
     public bool IsForward()
     {
         return this.tags.ContainsKey(tagType.forward) && (bool)this.tags[tagType.forward];
     }
+
+    public double GetWeight(OsmNode parentNode, speedType vehicle)
+    {
+        double distance = Utils.DistanceBetween(parentNode, neighborCoordinates);
+        byte speedByte = GetMaxSpeed(vehicle);
+        if (speedByte > 0)
+        {
+            double speed = Convert.ToDouble(speedByte);
+            return distance / speed;
+        }
+        else
+        {
+            return double.MaxValue;
+        }
+    }
+
+    public override string ToString()
+    {
+        string tagsString = "";
+        foreach (KeyValuePair<tagType, object> tag in tags)
+        {
+            tagsString += string.Format("\n\t{0}: {1}", tag.Key, tag.Value);
+        }
+
+        return string.Format("EDGE neighborCoordinates: {0} {1}", neighborCoordinates.ToString(), tagsString);
+    }
 }

OSMDatastructure/OsmNode.cs

@@ -7,6 +7,7 @@ public class OsmNode
     
     public OsmNode? previousPathNode = null;
     public double currentPathWeight = double.MaxValue;
+    public double currentPathLength = double.MaxValue;
     public double directDistanceToGoal = double.MaxValue;
     
     public OsmNode(float lat, float lon)
@@ -41,9 +42,15 @@ public class OsmNode
 
     public override string ToString()
     {
+        if(this.previousPathNode != null)
             return string.Format(
-            "NODE {0} Edges-Count: {1} previousPathNode: {2} currentPathWeight: {3} directDistanceToGoal: {4}",
+                "NODE {0} Edges-Count: {1} previousPathNode: {2} currentPathWeight: {3} currentPathLength: {4} directDistanceToGoal: {5}",
                 this.coordinates.ToString(), this.edges.Count, this.previousPathNode.coordinates.ToString(),
-            this.currentPathWeight, this.directDistanceToGoal);
+                this.currentPathWeight, this.currentPathLength, this.directDistanceToGoal);
+        else
+            return string.Format(
+                "NODE {0} Edges-Count: {1} previousPathNode: NO PREVIOUS NODE currentPathWeight: {2} currentPathLength: {3} directDistanceToGoal: {4}",
+                this.coordinates.ToString(), this.edges.Count, 
+                this.currentPathWeight, this.currentPathLength, this.directDistanceToGoal);
     }
 }

OSMDatastructure/Utils.cs

@@ -1,6 +1,6 @@
 using OSMDatastructure;
 
-namespace Pathfinding
+namespace OSMDatastructure
 {
     public struct Utils
     {

Pathfinding/Pathfinder.cs

@@ -6,11 +6,10 @@ namespace Pathfinding;
 public class Pathfinder
 {
 
-    public static List<OsmNode> CustomAStar(string workingDir, Coordinates start, Coordinates goal)
+    public static List<OsmNode> CustomAStar(string workingDir, Coordinates start, Coordinates goal, OsmEdge.speedType vehicle)
     {
         RegionManager regionManager = new RegionManager(workingDir);
         Region startRegion, goalRegion;
-        OsmNode? startNode, goalNode;
         try
         {
             startRegion = regionManager.GetRegion(start);
@@ -21,18 +20,20 @@ public class Pathfinder
             throw new Exception(string.Format("No region at coordinates {0}", e.FileName), e);
         }
 
-        startNode = ClosestNodeToCoordinates(start, startRegion);
-        goalNode = ClosestNodeToCoordinates(goal, goalRegion);
+        OsmNode? startNode = ClosestNodeToCoordinates(start, startRegion);
+        OsmNode? goalNode = ClosestNodeToCoordinates(goal, goalRegion);
         if (startNode == null || goalNode == null)
             return new List<OsmNode>();
         
         List<OsmNode> toVisit = new() { startNode };
-        OsmNode closestNodeToGoal;
+        OsmNode closestNodeToGoal = toVisit.First();
+        closestNodeToGoal.currentPathWeight = 0;
+        closestNodeToGoal.currentPathLength = 0;
         bool stop = false;
 
         while (toVisit.Count > 0 && !stop)
         {
-            Console.WriteLine("toVisit length: {0}", toVisit.Count.ToString());
+            //Console.WriteLine("toVisit-length: {0}", toVisit.Count);
             closestNodeToGoal = toVisit.First();
             foreach (OsmNode node in toVisit)
             {
@@ -46,15 +47,18 @@ public class Pathfinder
                 }
             }
 
-            foreach (OsmEdge connection in closestNodeToGoal.edges)
+            foreach (OsmEdge edge in closestNodeToGoal.edges)
             {
-                OsmNode? neighbor = regionManager.GetNode(connection.neighborCoordinates);
-                Console.WriteLine(neighbor);
-                if (neighbor != null && neighbor.currentPathWeight < closestNodeToGoal.currentPathWeight + Utils.DistanceBetween(closestNodeToGoal, neighbor))
+                OsmNode? neighbor = regionManager.GetNode(edge.neighborCoordinates);
+                if (neighbor != null)
+                {
+                    double newPotentialWeight =
+                        closestNodeToGoal.currentPathWeight + edge.GetWeight(closestNodeToGoal, vehicle);
+                    if (neighbor.currentPathWeight > newPotentialWeight)
                     {
                         neighbor.previousPathNode = closestNodeToGoal;
-                    neighbor.currentPathWeight = closestNodeToGoal.currentPathWeight +
-                                                 Utils.DistanceBetween(closestNodeToGoal, neighbor);
+                        neighbor.currentPathWeight = newPotentialWeight;
+                        neighbor.currentPathLength = closestNodeToGoal.currentPathLength + Utils.DistanceBetween(closestNodeToGoal, neighbor);
                     
                         if (neighbor.Equals(goalNode))
                             stop = true;
@@ -62,6 +66,7 @@ public class Pathfinder
                             toVisit.Add(neighbor);
                     }
                 }
+            }
 
             toVisit.Remove(closestNodeToGoal);
         }
@@ -74,6 +79,7 @@ public class Pathfinder
             currentNode = currentNode.previousPathNode;
         }
         path.Add(startNode);
+        path.Reverse();
         
         return path;
     }

Pathfinding/RegionManager.cs

@@ -45,18 +45,21 @@ namespace OSMImporter
         private Region LoadRegion(Coordinates coordinates)
         {
             string fullPath = Path.Combine(workingDirectory, coordinates.GetRegionHash().ToString());
-            Console.WriteLine("Loading {0}", fullPath);
+            DateTime startTime = DateTime.Now;
             if (!File.Exists(fullPath))
             {
-                throw new FileNotFoundException(string.Format("Region does not exist: {0}", fullPath));
+                throw new FileNotFoundException(string.Format("[{0}] Region does not exist: {1}", startTime, fullPath));
             }
             
             FileStream fileStream = new FileStream(fullPath, FileMode.Open);
+            long fileStreamLength = fileStream.Length;
+            Console.WriteLine("[{0}] Loading [{1}]bytes from {2}", startTime.ToLocalTime(), fileStreamLength, fullPath);
 
             byte[] regionBytes = new byte[fileStream.Length];
-            int _ = fileStream.Read(regionBytes, 0, regionBytes.Length);
+            int loadedBytesLength = fileStream.Read(regionBytes, 0, regionBytes.Length);
             fileStream.Close();
             
+            Console.WriteLine("\tLoaded [{0}]bytes ({1:P1}) in [{2}]ms", loadedBytesLength, fileStreamLength / loadedBytesLength,DateTime.Now.Subtract(startTime).TotalMilliseconds);
             return ByteConverter.ToRegion(regionBytes);
         }
 

Server/Server.cs

@@ -5,10 +5,39 @@ namespace Server;
 
 public class Server
 {
+    private static void WriteRegionsToFile(HashSet<Region> regions, string outputFolderPath)
+    {
+        Console.WriteLine(string.Format("[{0}] Writing files...", DateTime.Now.ToLocalTime()));
+        Directory.CreateDirectory(outputFolderPath);
+        foreach (Region region in regions)
+        {
+            FileStream regionFileStream =
+                new FileStream(Path.Combine(outputFolderPath, region.regionHash.ToString()), FileMode.Create);
+            regionFileStream.Write(ByteConverter.GetBytes(region));
+            regionFileStream.Close();
+        }
+    }
+    
     public static void Main(string[] args)
     {
-        //XmlImporter.Split("/home/glax/Downloads/oberbayern-latest.osm", "/home/glax/Downloads/oberbayern-latest");
-        //Region r = LoadRegion("/home/glax/Downloads/bayern-latest", new Coordinates(47.890f,12.56f));
-        Pathfinder.CustomAStar("/home/glax/Downloads/oberbayern-latest", new Coordinates(48.243351f, 11.640417f), new Coordinates(48.25239f, 11.53272f));
+        //HashSet<OsmNode> nodes = XmlImporter.ImportXml("/home/glax/Downloads/oberbayern-latest.osm");
+        //HashSet<Region> regions = XmlImporter.SplitIntoRegions(nodes);
+        //WriteRegionsToFile(regions, "/home/glax/Downloads/oberbayern-latest");
+        Coordinates start = new Coordinates(48.243351f, 11.640417f);
+        Coordinates finish = new Coordinates(48.25239f, 11.53272f);
+
+        OsmNode[] path = Pathfinder.CustomAStar("/home/glax/Downloads/oberbayern-latest", start, finish, OsmEdge.speedType.car).ToArray();
+        Console.WriteLine("{0}\n", path[0].ToString());
+        for (int i = 0; i < path.Length - 1; i++)
+        {
+            OsmNode n1 = path[i];
+            OsmNode n2 = path[i + 1];
+            OsmEdge? e = n1.GetEdgeToNode(n2);
+            if(e != null)
+                Console.WriteLine("{0}\n{1}", e.ToString(), n2.ToString());
+            else
+                Console.WriteLine("NO EDGE\n{0}", n2.ToString());
+        }
+        Console.WriteLine();
     }
 }

Server/XmlImporter.cs

@@ -0,0 +1,171 @@
+using System.Globalization;
+using System.Xml;
+using OSMDatastructure;
+using OSMImporter;
+
+namespace Server;
+
+public static class XmlImporter
+{
+    private static readonly XmlReaderSettings readerSettings = new()
+    {
+        IgnoreWhitespace = true,
+        IgnoreComments = true
+    };
+
+    public static HashSet<OsmNode> ImportXml(string filePath)
+    {
+        if (!File.Exists(filePath))
+            throw new FileNotFoundException();
+        
+        Console.WriteLine(string.Format("[{0}] Getting highwayNodeIds...", DateTime.Now.ToLocalTime()));
+        FileStream xmlFileStream = new FileStream(filePath, FileMode.Open);
+        HashSet<ulong> requiredNodeIds = GetHighwayNodeIds(XmlReader.Create(xmlFileStream, readerSettings));
+        xmlFileStream.Position = 0;
+
+        Console.WriteLine(string.Format("[{0}] Importing Nodes...", DateTime.Now.ToLocalTime()));
+        Dictionary<ulong, OsmNode> nodes =
+            GetHighwayNodesFromIds(XmlReader.Create(xmlFileStream, readerSettings), requiredNodeIds);
+        requiredNodeIds.Clear();
+        xmlFileStream.Position = 0;
+        
+        Console.WriteLine(string.Format("[{0}] Importing Ways...", DateTime.Now.ToLocalTime()));
+        HashSet<OsmNode> retNodes = ConnectNodes(XmlReader.Create(xmlFileStream, readerSettings), nodes);
+        nodes.Clear();
+
+        return retNodes;
+    }
+
+    public static HashSet<Region> SplitIntoRegions(HashSet<OsmNode> nodes)
+    {
+        Console.WriteLine(string.Format("[{0}] Splitting into Regions...", DateTime.Now.ToLocalTime()));
+        Dictionary<ulong, Region> retRegions = new();
+        foreach (OsmNode node in nodes)
+        {
+            ulong regionHash = node.coordinates.GetRegionHash();
+            if(retRegions.ContainsKey(regionHash))
+            {
+                retRegions[regionHash].nodes.Add(node);
+            }
+            else
+            {
+                Region newRegion = new Region(regionHash);
+                newRegion.nodes.Add(node);
+                retRegions.Add(regionHash, newRegion);
+            }
+        }
+
+        return retRegions.Values.ToHashSet();
+    }
+
+    private static readonly NumberFormatInfo decimalInfo = new()
+    {
+        NumberDecimalSeparator = "."
+    };
+    private static HashSet<ulong> GetHighwayNodeIds(XmlReader xmlReader)
+    {
+        HashSet<ulong> retSet = new();
+        bool isHighway;
+        HashSet<ulong> currentIds = new();
+        while (xmlReader.ReadToFollowing("way"))
+        {
+            isHighway = false;
+            currentIds.Clear();
+            XmlReader wayReader = xmlReader.ReadSubtree();
+            while (wayReader.Read())
+            {
+                if (xmlReader.Name == "tag" && xmlReader.GetAttribute("k")!.Equals("highway"))
+                {
+                    isHighway = true;
+                }
+                else if (xmlReader.Name == "nd")
+                {
+                    try
+                    {
+                        currentIds.Add(Convert.ToUInt64(xmlReader.GetAttribute("ref")));
+                    }
+                    catch (FormatException) { };
+                }
+            }
+            if (isHighway)
+            {
+                retSet.UnionWith(currentIds);
+            }
+            wayReader.Close();
+        }
+        xmlReader.Close();
+
+        return retSet;
+    }
+
+    private static Dictionary<ulong, OsmNode> GetHighwayNodesFromIds(XmlReader xmlReader, HashSet<ulong> ids)
+    {
+        Dictionary<ulong, OsmNode> retDict = new();
+
+        while (xmlReader.ReadToFollowing("node"))
+        {
+            ulong id = Convert.ToUInt64(xmlReader.GetAttribute("id"));
+            if (ids.Contains(id))
+            {
+                float lat = Convert.ToSingle(xmlReader.GetAttribute("lat")!, decimalInfo);
+                float lon = Convert.ToSingle(xmlReader.GetAttribute("lon")!, decimalInfo);
+                retDict.Add(id, new OsmNode(lat, lon));
+            }
+        }
+        xmlReader.Close();
+        
+        return retDict;
+    }
+    
+    private static HashSet<OsmNode> ConnectNodes(XmlReader xmlReader, Dictionary<ulong, OsmNode> dict)
+    {
+        while (xmlReader.ReadToFollowing("way"))
+        {
+            Dictionary<OsmEdge.tagType, object> tags = new();
+            List<ulong> wayNodeIds = new();
+            XmlReader wayReader = xmlReader.ReadSubtree();
+            while (wayReader.Read())
+            {
+                if (xmlReader.Name == "tag")
+                {
+                    KeyValuePair<OsmEdge.tagType, object> tag =
+                        OsmEdge.ConvertToTag(wayReader.GetAttribute("k")!, wayReader.GetAttribute("v")!);
+                    tags.TryAdd(tag.Key, tag.Value);
+                }
+                else if (xmlReader.Name == "nd")
+                {
+                    wayNodeIds.Add(Convert.ToUInt64(xmlReader.GetAttribute("ref")));
+                }
+            }
+            if (tags.ContainsKey(OsmEdge.tagType.highway))
+            {
+                ulong[] ids = wayNodeIds.Where(dict.ContainsKey).ToArray();
+                for (int i = 0; i < ids.Length - 1; i++)
+                {
+                    OsmNode n1 = dict[ids[i]];
+                    OsmNode n2 = dict[ids[i + 1]];
+                    if (tags.ContainsKey(OsmEdge.tagType.oneway) && (bool)tags[OsmEdge.tagType.oneway])
+                    {
+                        if (tags.ContainsKey(OsmEdge.tagType.forward) && !(bool)tags[OsmEdge.tagType.forward])
+                        {
+                            n2.edges.Add(new OsmEdge(n1.coordinates, tags));
+                        }
+                        else
+                        {
+                            n1.edges.Add(new OsmEdge(n2.coordinates, tags));
+                        }
+                    }
+                    else
+                    {
+                        n1.edges.Add(new OsmEdge(n2.coordinates, tags));
+                        n2.edges.Add(new OsmEdge(n1.coordinates, tags));
+                    }
+                }
+            }
+            wayReader.Close();
+        }
+        xmlReader.Close();
+
+        return dict.Values.ToHashSet();
+    }
+}