using System.Text.Json; using OSMDatastructure; using OSMDatastructure.Graph; using static OSMDatastructure.Tag; using WayType = OSMDatastructure.Tag.WayType; namespace Pathfinding; public static class Pathfinder { public static PathResult AStar(string workingDir, Coordinates startCoordinates, Coordinates goalCoordinates, SpeedType vehicle, double heuristicRoadLevelPriority, double heuristicSameRoadPriority, double heuristicFewJunctionsPriority) { DateTime startCalc = DateTime.Now; RegionManager 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()); PriorityQueue openSetfScore = new(); openSetfScore.Enqueue(startNode, 0); Dictionary cameFromDict = new(); Dictionary gScore = new(); gScore.Add(startNode, 0); while (openSetfScore.Count > 0) { OsmNode currentNode = openSetfScore.Dequeue(); 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; } foreach (OsmEdge edge in currentNode.edges) { OsmNode? neighbor = regionManager.GetNode(edge.neighborId, edge.neighborRegion); if (neighbor is not null) { double tentativeGScore = gScore[currentNode] + Weight(currentNode, neighbor, edge, vehicle, regionManager); gScore.TryAdd(neighbor, double.MaxValue); if (tentativeGScore < gScore[neighbor]) { if (cameFromDict.ContainsKey(neighbor)) cameFromDict[neighbor] = currentNode; else cameFromDict.Add(neighbor, currentNode); if (gScore.ContainsKey(neighbor)) gScore[neighbor] = tentativeGScore; else gScore.Add(neighbor, tentativeGScore); double h = Heuristic(currentNode, neighbor, goalNode, edge, vehicle, regionManager, 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); } } } } return new PathResult(DateTime.Now - startCalc, new List()); } private static void SaveGraph(PathResult pathResult, string outputFilepath) { FileStream fs = new FileStream(outputFilepath, FileMode.CreateNew); JsonSerializer.Serialize(fs, pathResult, JsonSerializerOptions.Default); fs.Dispose(); Console.WriteLine($"Saved result to {outputFilepath}"); } private static PathResult GetPath(Dictionary cameFromDict, OsmNode goalNode, RegionManager regionManager, TimeSpan calcFinished) { List path = new List(); OsmNode currentNode = goalNode; while (cameFromDict.ContainsKey(cameFromDict[currentNode])) { OsmEdge? currentEdge = cameFromDict[currentNode].edges.First(edge => edge.neighborId == currentNode.nodeId); HashSet? tags = regionManager.GetRegion(currentNode.coordinates)!.tagManager.GetTagsForWayId(currentEdge.wayId); PathNode? newNode = PathNode.FromOsmNode(currentNode, tags); if(newNode is not null) path.Add(newNode); currentNode = cameFromDict[currentNode]; } path.Reverse(); return new PathResult(calcFinished, path); } private static double Weight(OsmNode fromNode, OsmNode neighborNode, OsmEdge edge, SpeedType vehicle, RegionManager regionManager) { double distance = Utils.DistanceBetween(fromNode, neighborNode); double speed = regionManager.GetSpeedForEdge(fromNode, edge.wayId, vehicle); 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) { double roadPriority = GetPriorityVehicleRoad(edge, vehicle, regionManager.GetRegion(fromNode.coordinates)!) * roadPriorityFactor; TagManager curTags = regionManager.GetRegion(fromNode.coordinates)!.tagManager; TagManager nextTags = regionManager.GetRegion(neighborNode.coordinates)!.tagManager; bool sameName = false; string? curName = (string?)curTags.GetTag(edge.wayId, TagType.name); bool sameRef = false; string? curRef = (string?)curTags.GetTag(edge.wayId, TagType.tagref); if(curName is not null) foreach (OsmEdge pEdge in neighborNode.edges) { if ((string?)nextTags.GetTag(pEdge.wayId, TagType.name) == curName) sameName = true; if ((string?)nextTags.GetTag(pEdge.wayId, TagType.tagref) == curRef) sameRef = true; } double sameRoadName = (sameRef || sameName ? 1 : 0) * sameRoadFactor; double junctionCount = (neighborNode.edges.Count > 2 ? 0 : 1) * junctionFactor; return Utils.DistanceBetween(neighborNode, goalNode) / (1 + roadPriority + sameRoadName + junctionCount); } private static double GetPriorityVehicleRoad(OsmEdge edge, SpeedType vehicle, Region region) { if (vehicle == SpeedType.any) return 1; WayType? wayType = (WayType?)region.tagManager.GetTag(edge.wayId, TagType.highway); if(wayType is null) return 0; if (vehicle == SpeedType.car) { switch (wayType) { case WayType.motorway: case WayType.motorway_link: case WayType.motorroad: case WayType.trunk: case WayType.trunk_link: case WayType.primary: case WayType.primary_link: return 10; case WayType.secondary: case WayType.secondary_link: return 7; case WayType.tertiary: case WayType.tertiary_link: return 5; case WayType.unclassified: case WayType.residential: case WayType.road: case WayType.living_street: return 2; case WayType.service: case WayType.track: return 0.0001; } } if (vehicle == SpeedType.pedestrian) { switch (wayType) { case WayType.pedestrian: case WayType.corridor: case WayType.footway: case WayType.path: case WayType.steps: case WayType.residential: case WayType.living_street: return 10; case WayType.service: case WayType.cycleway: case WayType.bridleway: case WayType.road: case WayType.track: case WayType.unclassified: return 5; case WayType.tertiary: case WayType.tertiary_link: case WayType.escape: return 2; } } return 0.01; } }