Created Pathfinding

OSMServer.sln

@@ -6,6 +6,8 @@ Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Server", "Server\Server.csp
 EndProject
 Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "OSMDatastructure", "OSMDatastructure\OSMDatastructure.csproj", "{A2489EF1-64E9-41C9-A295-B3D551D810DA}"
 EndProject
+Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Pathfinding", "Pathfinding\Pathfinding.csproj", "{D4AA1C47-98D4-415C-B026-3BC25B6B6F9D}"
+EndProject
 Global
 	GlobalSection(SolutionConfigurationPlatforms) = preSolution
 		Debug|Any CPU = Debug|Any CPU
@@ -24,5 +26,9 @@ Global
 		{A2489EF1-64E9-41C9-A295-B3D551D810DA}.Debug|Any CPU.Build.0 = Debug|Any CPU
 		{A2489EF1-64E9-41C9-A295-B3D551D810DA}.Release|Any CPU.ActiveCfg = Release|Any CPU
 		{A2489EF1-64E9-41C9-A295-B3D551D810DA}.Release|Any CPU.Build.0 = Release|Any CPU
+		{D4AA1C47-98D4-415C-B026-3BC25B6B6F9D}.Debug|Any CPU.ActiveCfg = Debug|Any CPU
+		{D4AA1C47-98D4-415C-B026-3BC25B6B6F9D}.Debug|Any CPU.Build.0 = Debug|Any CPU
+		{D4AA1C47-98D4-415C-B026-3BC25B6B6F9D}.Release|Any CPU.ActiveCfg = Release|Any CPU
+		{D4AA1C47-98D4-415C-B026-3BC25B6B6F9D}.Release|Any CPU.Build.0 = Release|Any CPU
 	EndGlobalSection
 EndGlobal

Pathfinding/Graph.cs

@@ -0,0 +1,12 @@
+namespace Pathfinding;
+
+public class Graph
+{
+    public class Node : OSMDatastructure.Node
+    {
+        public Node(float lat, float lon) : base(lat, lon)
+        {
+            
+        }
+    }
+}

Pathfinding/Pathfinder.cs

@@ -0,0 +1,41 @@
+using OSMDatastructure;
+using OSMImporter;
+
+namespace Pathfinding;
+
+public class Pathfinder
+{
+
+    private static void CustomAStar(string workingDir, Coordinates start, Coordinates goal)
+    {
+        RegionManager regionManager = new RegionManager(workingDir);
+        Region startRegion = regionManager.GetRegion(start)!; //TODO null handling
+        Node startNode = ClosestNodeToCoordinates(start, startRegion)!; //TODO null handling
+        Region goalRegion = regionManager.GetRegion(goal)!; //TODO null handling
+        Node goalNode = ClosestNodeToCoordinates(goal, goalRegion)!; //TODO null handling
+        
+    }
+    
+    private static Node? ClosestNodeToCoordinates(Coordinates coordinates, Region region)
+    {
+        ulong? closestId = ClosestNodeIdToCoordinates(coordinates, region);
+        return closestId != null ? region.GetNode((ulong)closestId) : null;
+    }
+    
+    private static ulong? ClosestNodeIdToCoordinates(Coordinates coordinates, Region region)
+    {
+        ulong? closestId = null;
+        double closestDistance = double.MaxValue, distance;
+
+        foreach (KeyValuePair<ulong, Node> kv in region.GetNodes())
+        {
+            distance = Utils.DistanceBetween(kv.Value, coordinates);
+            if (distance < closestDistance)
+            {
+                closestDistance = distance;
+                closestId = kv.Key;
+            }
+        }
+        return closestId;
+    }
+}

Pathfinding/Pathfinding.csproj

@@ -0,0 +1,13 @@
+<Project Sdk="Microsoft.NET.Sdk">
+
+    <PropertyGroup>
+        <TargetFramework>net7.0</TargetFramework>
+        <ImplicitUsings>enable</ImplicitUsings>
+        <Nullable>enable</Nullable>
+    </PropertyGroup>
+
+    <ItemGroup>
+      <ProjectReference Include="..\OSMDatastructure\OSMDatastructure.csproj" />
+    </ItemGroup>
+
+</Project>

Pathfinding/RegionManager.cs

@@ -0,0 +1,49 @@
+using OSMDatastructure;
+
+namespace OSMImporter
+{
+    public class RegionManager
+    {
+        private string workingDirectory { get; }
+        private readonly Dictionary<ulong, Region> _regions = new();
+
+        public RegionManager(string workingDirectory)
+        {
+            this.workingDirectory = workingDirectory;
+        }
+
+        public Region? GetRegion(Coordinates coordinates)
+        {
+            if(this._regions.ContainsKey(coordinates.GetRegionHash()))
+                return this._regions[coordinates.GetRegionHash()];
+            else
+            {
+                Region? loadedRegion = LoadRegion(coordinates);
+                if(loadedRegion != null)
+                    this._regions.Add(loadedRegion.regionHash, value: loadedRegion);
+                return loadedRegion;
+            }
+        }
+
+        public Region[] GetAllRegions()
+        {
+            return this._regions.Values.ToArray();
+        }
+        
+        private Region? LoadRegion(Coordinates coordinates)
+        {
+            string fullPath = Path.Combine(workingDirectory, coordinates.GetRegionHash().ToString());
+            Console.WriteLine(fullPath);
+            if (!File.Exists(fullPath))
+                return null;
+            
+            FileStream fileStream = new FileStream(fullPath, FileMode.Open);
+
+            byte[] regionBytes = new byte[fileStream.Length];
+            fileStream.Read(regionBytes, 0, regionBytes.Length);
+            fileStream.Close();
+
+            return ByteConverter.ToRegion(regionBytes);
+        }
+    }
+}

Pathfinding/Utils.cs

@@ -0,0 +1,65 @@
+using OSMDatastructure;
+
+namespace Pathfinding
+{
+    public struct Utils
+    {
+        public static double DistanceBetween(Coordinates c1, Coordinates n2)
+        {
+            return DistanceBetween(c1.lat, c1.lon, n2.lat, n2.lon);
+        }
+
+        public static double DistanceBetween(Coordinates c1, float lat2, float lon2)
+        {
+            return DistanceBetween(c1.lat, c1.lon, lat2, lon2);
+        }
+
+        public static double DistanceBetween(float lat1, float lon1, Coordinates c2)
+        {
+            return DistanceBetween(c2, lat1, lon1);
+        }
+
+        public static double DistanceBetween(float lat1, float lon1, float lat2, float lon2) //Law of Cosines
+        {
+            /*
+             * From https://www.movable-type.co.uk/scripts/latlong.html
+             */
+            double rlat1 = DegreesToRadians(lat1);
+            double rlat2 = DegreesToRadians(lat2);
+            double drlon = DegreesToRadians(lon2 - lon1);
+            const int R = 6371000;
+
+            double d = Math.Acos(Math.Sin(rlat1) * Math.Sin(rlat2) + Math.Cos(rlat1) * Math.Cos(rlat2) * Math.Cos(drlon)) * R;
+
+            return d;
+        }
+
+        public static double DistanceBetweenHaversine(float lat1, float lon1, float lat2, float lon2)
+        {
+            /*
+             * From https://www.movable-type.co.uk/scripts/latlong.html
+             */
+            const int R = 6371000;
+            double rlat1 = DegreesToRadians(lat1);
+            double rlat2 = DegreesToRadians(lat2);
+            double rdlat = DegreesToRadians(lat2 - lat1);
+            double drlon = DegreesToRadians(lon2 - lon1);
+            double a = Math.Sin(rdlat / 2) * Math.Sin(rdlat / 2) + Math.Cos(rlat1) * Math.Cos(rlat2) * Math.Sin(drlon / 2) * Math.Sin(drlon / 2);
+            double c = 2 * Math.Atan2(Math.Sqrt(a), Math.Sqrt(1 - a));
+            double d = R * c;
+            return d;
+        }
+
+        private static double DegreesToRadians(double deg)
+        {
+            return deg * Math.PI / 180.0;
+        }
+
+        private static double RadiansToDegrees(double rad)
+        {
+            return rad * 180.0 / Math.PI;
+        }
+
+
+    }
+}