Added first Pathfinding

Pathfinding/PathNode.cs

@@ -4,9 +4,9 @@ namespace Pathfinding;
 
 public class PathNode : OSMDatastructure.Node
 {
-    PathNode? previousNode = null;
+    public PathNode? previousPathNode = null;
-    private Dictionary<Connection, double> distanceDictionary = new();
+    public double currentPathWeight = double.MaxValue;
-    double currentWeight = double.MaxValue;
+    public double DirectDistanceToGoal = double.MaxValue;
     
     
     public PathNode(float lat, float lon) : base(lat, lon)
@@ -14,14 +14,4 @@ public class PathNode : OSMDatastructure.Node
             
     }
 
-    public double? DistanceTo(Connection connection)
-    {
-        if (!this.GetConnections().Contains(connection))
-            return null; //TODO exception is not actually connected
-        if (!this.distanceDictionary.ContainsKey(connection))
-            this.distanceDictionary.Add(connection, Utils.DistanceBetween(this, connection.endNodeCoordinates));
-        return this.distanceDictionary[connection];
-    }
-    
-    
 }

Pathfinding/Pathfinder.cs

@@ -6,15 +6,72 @@ namespace Pathfinding;
 public class Pathfinder
 {
 
-    private static void CustomAStar(string workingDir, Coordinates start, Coordinates goal)
+    public static List<PathNode> CustomAStar(string workingDir, Coordinates start, Coordinates goal)
     {
         RegionManager regionManager = new RegionManager(workingDir);
-        Region startRegion = regionManager.GetRegion(start)!; //TODO null handling
+        Region startRegion, goalRegion;
-        PathNode startNode = (PathNode)ClosestNodeToCoordinates(start, startRegion)!; //TODO null handling
+        PathNode startNode, goalNode;
-        Region goalRegion = regionManager.GetRegion(goal)!; //TODO null handling
+        try
-        PathNode goalNode = (PathNode)ClosestNodeToCoordinates(goal, goalRegion)!; //TODO null handling
+        {
+            startRegion = regionManager.GetRegion(start);
+            startNode = (PathNode)ClosestNodeToCoordinates(start, startRegion)!; //TODO null handling
+        }
+        catch (FileNotFoundException e)
+        {
+            throw new Exception(string.Format("No region at coordinates {0}", start), e);
+        }
+        try
+        {
+            goalRegion = regionManager.GetRegion(goal);
+            goalNode = (PathNode)ClosestNodeToCoordinates(goal, goalRegion)!; //TODO null handling
+        }
+        catch (FileNotFoundException e)
+        {
+            throw new Exception(string.Format("No region at coordinates {0}", start), e);
+        }
         
+        List<PathNode> toVisit = new() { startNode };
+        startNode.DirectDistanceToGoal = Utils.DistanceBetween(startNode, goalNode);
+        bool stop = false;
 
+        while (toVisit.Count > 0 && !stop)
+        {
+            PathNode closestNodeToGoal = toVisit.First();
+            foreach (PathNode node in toVisit)
+            {
+                if (node.DirectDistanceToGoal < closestNodeToGoal.DirectDistanceToGoal)
+                {
+                    closestNodeToGoal = node;
+                }
+            }
+
+            foreach (Connection connection in closestNodeToGoal.GetConnections())
+            {
+                PathNode? neighbor = (PathNode?)regionManager.GetNode(connection.endNodeCoordinates);
+                if (neighbor != null && neighbor.currentPathWeight < closestNodeToGoal.currentPathWeight + Utils.DistanceBetween(closestNodeToGoal, neighbor))
+                {
+                    neighbor.previousPathNode = closestNodeToGoal;
+                    neighbor.currentPathWeight = closestNodeToGoal.currentPathWeight +
+                                                 Utils.DistanceBetween(closestNodeToGoal, neighbor);
+                    
+                    if (neighbor.Equals(goalNode))
+                        stop = true;
+                    else
+                        toVisit.Add(neighbor);
+                }
+            }
+        }
+
+        List<PathNode> path = new();
+        PathNode currentNode = goalNode;
+        while (!currentNode.Equals(startNode))
+        {
+            path.Add(currentNode);
+            currentNode = currentNode.previousPathNode!;
+        }
+        path.Add(startNode);
+
+        return path;
     }
     
     private static Node? ClosestNodeToCoordinates(Coordinates coordinates, Region region)

Server/Server.csproj

@@ -9,6 +9,7 @@
 
     <ItemGroup>
       <ProjectReference Include="..\OSMSplitter\OSMSplitter.csproj" />
+      <ProjectReference Include="..\Pathfinding\Pathfinding.csproj" />
     </ItemGroup>
 
 </Project>