Created Pathfinding

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