diff --git a/Pathfinding/Pathfinder.cs b/Pathfinding/Pathfinder.cs index fa7044a..45b3c71 100644 --- a/Pathfinding/Pathfinder.cs +++ b/Pathfinding/Pathfinder.cs @@ -6,25 +6,41 @@ using WayType = OSMDatastructure.Tag.WayType; namespace Pathfinding; -public static class Pathfinder +//TODO check parameters for all functions and determine global fields +public class Pathfinder { - public static PathResult AStar(string workingDir, Coordinates startCoordinates, Coordinates goalCoordinates, + public RegionManager regionManager; + public readonly string workingDir; + public PathResult? pathResult; + public Dictionary? gScore; + + public Pathfinder(string workingDirectory) + { + if (!Path.Exists(workingDirectory)) + throw new DirectoryNotFoundException(workingDirectory); + regionManager = new(workingDirectory); + workingDir = workingDirectory; + } + + public Pathfinder AStar(Coordinates startCoordinates, Coordinates goalCoordinates, SpeedType vehicle, double heuristicRoadLevelPriority, double heuristicSameRoadPriority, double heuristicFewJunctionsPriority) { DateTime startCalc = DateTime.Now; - RegionManager regionManager = new RegionManager(workingDir); + regionManager = new RegionManager(workingDir); OsmNode? startNode = regionManager.ClosestNodeToCoordinates(startCoordinates, vehicle); OsmNode? goalNode = regionManager.ClosestNodeToCoordinates(goalCoordinates, vehicle); if (startNode is null || goalNode is null) - return new PathResult(DateTime.Now - startCalc, new List()); + { + pathResult = new(DateTime.Now - startCalc, new List()); + return this; + } PriorityQueue openSetfScore = new(); openSetfScore.Enqueue(startNode, 0); Dictionary cameFromDict = new(); - Dictionary gScore = new(); - gScore.Add(startNode, 0); + gScore = new() { { startNode, 0 } }; while (openSetfScore.Count > 0) { @@ -32,14 +48,8 @@ public static class Pathfinder if (currentNode.Equals(goalNode)) { Console.WriteLine("Path found."); - PathResult path = GetPath(cameFromDict, goalNode, regionManager, DateTime.Now - startCalc); - string fileName = $"{new DirectoryInfo(workingDir).Name}-{DateTime.Now.ToFileTime()}.result"; - string outputFilepath = Path.Join(Directory.GetParent(workingDir)!.FullName, fileName); - path.name = outputFilepath; - path.AddGScores(gScore); - SaveGraph(path, outputFilepath); - path.regionManager = regionManager; - return path; + this.pathResult = GetPath(cameFromDict, goalNode, DateTime.Now - startCalc); + return this; } foreach (OsmEdge edge in currentNode.edges) @@ -48,7 +58,7 @@ public static class Pathfinder if (neighbor is not null) { double tentativeGScore = - gScore[currentNode] + Weight(currentNode, neighbor, edge, vehicle, regionManager); + gScore[currentNode] + Weight(currentNode, neighbor, edge, vehicle); gScore.TryAdd(neighbor, double.MaxValue); if (tentativeGScore < gScore[neighbor]) { @@ -60,7 +70,7 @@ public static class Pathfinder gScore[neighbor] = tentativeGScore; else gScore.Add(neighbor, tentativeGScore); - double h = Heuristic(currentNode, neighbor, goalNode, edge, vehicle, regionManager, + double h = Heuristic(currentNode, neighbor, goalNode, edge, vehicle, heuristicRoadLevelPriority, heuristicFewJunctionsPriority, heuristicSameRoadPriority); //Console.WriteLine($"Queue: {openSetfScore.Count:00000} Current Distance: {Utils.DistanceBetween(currentNode, goalNode):000000.00} Visited: {cameFromDict.Count:00000} Current heuristic: {h:00000.00}"); openSetfScore.Enqueue(neighbor, tentativeGScore + h); @@ -69,20 +79,21 @@ public static class Pathfinder } } - return new PathResult(DateTime.Now - startCalc, new List()); + pathResult = new(DateTime.Now - startCalc, new List()); + return this; } - private static void SaveGraph(PathResult pathResult, string outputFilepath) + public void SaveResult(string path) { - FileStream fs = new FileStream(outputFilepath, FileMode.CreateNew); + FileStream fs = new (path, FileMode.CreateNew); JsonSerializer.Serialize(fs, pathResult, JsonSerializerOptions.Default); fs.Dispose(); - Console.WriteLine($"Saved result to {outputFilepath}"); + Console.WriteLine($"Saved result to {path}"); } - private static PathResult GetPath(Dictionary cameFromDict, OsmNode goalNode, RegionManager regionManager, TimeSpan calcFinished) + private PathResult GetPath(Dictionary cameFromDict, OsmNode goalNode, TimeSpan calcFinished) { - List path = new List(); + List path = new(); OsmNode currentNode = goalNode; while (cameFromDict.ContainsKey(cameFromDict[currentNode])) { @@ -100,15 +111,15 @@ public static class Pathfinder return new PathResult(calcFinished, path); } - private static double Weight(OsmNode fromNode, OsmNode neighborNode, OsmEdge edge, SpeedType vehicle, RegionManager regionManager) + private double Weight(OsmNode fromNode, OsmNode neighborNode, OsmEdge edge, SpeedType vehicle) { double distance = Utils.DistanceBetween(fromNode, neighborNode); double speed = regionManager.GetSpeedForEdge(fromNode, edge.wayId, vehicle); - double prio = GetPriorityVehicleRoad(edge, vehicle, regionManager.GetRegion(fromNode.coordinates)!); + //double prio = GetPriorityVehicleRoad(edge, vehicle, regionManager.GetRegion(fromNode.coordinates)!); return distance / speed; } - private static double Heuristic(OsmNode fromNode, OsmNode neighborNode, OsmNode goalNode, OsmEdge edge, SpeedType vehicle, RegionManager regionManager, double roadPriorityFactor, double junctionFactor, double sameRoadFactor) + private double Heuristic(OsmNode fromNode, OsmNode neighborNode, OsmNode goalNode, OsmEdge edge, SpeedType vehicle, double roadPriorityFactor, double junctionFactor, double sameRoadFactor) { double roadPriority = GetPriorityVehicleRoad(edge, vehicle, regionManager.GetRegion(fromNode.coordinates)!) * roadPriorityFactor;