Added Pathfinding Time

Pathfinding/Pathfinder_Time.cs

@@ -0,0 +1,65 @@
+using OSMDatastructure.Graph;
+using Utils = OSMDatastructure.Utils;
+
+namespace Pathfinding;
+
+public static partial class Pathfinder
+{
+    public static List<PathNode> AStarTime(string workingDir, Coordinates start,
+        Coordinates goal, Tag.SpeedType vehicle)
+    {
+        RegionManager regionManager = new (workingDir);
+        ValueTuple<OsmNode?, OsmNode?> startAndEndNode = SetupNodes(start, goal, regionManager);
+        if (startAndEndNode.Item1 is null || startAndEndNode.Item2 is null)
+            return new List<PathNode>();
+        OsmNode goalNode = startAndEndNode.Item2!;
+
+        PriorityQueue<OsmNode, double> toVisit = new();
+        toVisit.Enqueue(startAndEndNode.Item1, 0);
+        bool stop = false;
+
+        while (toVisit.Count > 0)
+        {
+            OsmNode closestNodeToGoal = toVisit.Dequeue();
+
+            foreach (OsmEdge edge in closestNodeToGoal.edges.Where(
+                         edge => regionManager.TestValidConnectionForType(closestNodeToGoal, edge, vehicle)))
+            {
+                OsmNode? neighbor = regionManager.GetNode(edge.neighborId, edge.neighborRegion);
+                if (neighbor is not null)
+                {
+                    double newPotentialWeight = closestNodeToGoal.currentPathLength +
+                                                EdgeWeight(closestNodeToGoal, neighbor, edge.wayId, vehicle,
+                                                    ref regionManager);
+                    if (newPotentialWeight < neighbor.currentPathWeight)
+                    {
+                        neighbor.previousPathNode = closestNodeToGoal;
+                        neighbor.currentPathLength = closestNodeToGoal.currentPathLength + Utils.DistanceBetween(closestNodeToGoal, neighbor);
+                        neighbor.currentPathWeight = newPotentialWeight;
+                        neighbor.directDistanceToGoal = Utils.DistanceBetween(neighbor, goalNode);
+
+                        if (neighbor.Equals(goalNode))
+                        {
+                            stop = true;
+                        }
+                        else if(!stop)
+                        {
+                            toVisit.Enqueue(neighbor, neighbor.directDistanceToGoal);
+                        }
+                    }
+                }
+            }
+        }
+
+        List<PathNode> path = new();
+        OsmNode? currentNode = goalNode;
+        while (currentNode is not null)
+        {
+            path.Add(PathNode.FromOsmNode(currentNode)!);
+            currentNode = currentNode.previousPathNode;
+        }
+        path.Reverse();
+
+        return path;
+    }
+}