using OSMDatastructure; using OSMDatastructure.Graph; using OSMImporter; namespace Pathfinding; public class Pathfinder { public static List CustomAStar(string workingDir, Coordinates start, Coordinates goal, Tag.SpeedType vehicle) { RegionManager regionManager = new RegionManager(workingDir); Region? startRegion = regionManager.GetRegion(start); Region? goalRegion = regionManager.GetRegion(goal); if (startRegion is null || goalRegion is null) return new List(); OsmNode? startNode = ClosestNodeToCoordinates(start, startRegion, vehicle); OsmNode? goalNode = ClosestNodeToCoordinates(goal, goalRegion, vehicle); if (startNode == null || goalNode == null) return new List(); PriorityQueue toVisit = new(); toVisit.Enqueue(startNode, 0); startNode.currentPathWeight = 0; startNode.currentPathLength = 0; startNode.directDistanceToGoal = Utils.DistanceBetween(startNode, goalNode); OsmNode closestNodeToGoal = startNode; bool stop = false; while (toVisit.Count > 0 && !stop) { closestNodeToGoal = toVisit.Dequeue(); Console.WriteLine($"{toVisit.Count:000} {closestNodeToGoal.directDistanceToGoal:#.00} current:{closestNodeToGoal}"); foreach (OsmEdge edge in closestNodeToGoal.edges) { OsmNode? neighbor = regionManager.GetNode(edge.neighborId, edge.neighborRegion); if (neighbor is not null) { double newPotentialWeight = closestNodeToGoal.currentPathWeight + EdgeWeight(closestNodeToGoal, neighbor, edge.wayId, vehicle, ref regionManager); if (neighbor.currentPathWeight > newPotentialWeight) { neighbor.previousPathNode = closestNodeToGoal; neighbor.currentPathWeight = newPotentialWeight; neighbor.currentPathLength = closestNodeToGoal.currentPathLength + Utils.DistanceBetween(closestNodeToGoal, neighbor); if (neighbor.Equals(goalNode) || closestNodeToGoal.directDistanceToGoal < 10) //change for faster stop = true; else { neighbor.directDistanceToGoal = Utils.DistanceBetween(neighbor, goalNode); toVisit.Enqueue(neighbor, neighbor.directDistanceToGoal); } } } } } List path = new(); OsmNode? currentNode = closestNodeToGoal; while (currentNode != null && !currentNode.Equals(startNode)) { path.Add(currentNode); currentNode = currentNode.previousPathNode; } path.Add(startNode); path.Reverse(); return path; } private static OsmNode? ClosestNodeToCoordinates(Coordinates coordinates, Region region, Tag.SpeedType vehicle) { OsmNode? closest = null; double distance = double.MaxValue; foreach (OsmNode node in region.nodes) { bool hasConnection = false; foreach (OsmEdge edge in node.edges) { byte speed = 0; switch (vehicle) { case Tag.SpeedType.road: case Tag.SpeedType.car: speed = Tag.defaultSpeedCar[(Tag.WayType)region.tagManager.GetTag(edge.wayId, Tag.TagType.highway)!]; break; case Tag.SpeedType.pedestrian: speed = Tag.defaultSpeedPedestrian[ (Tag.WayType)region.tagManager.GetTag(edge.wayId, Tag.TagType.highway)!]; break; } if (speed != 0) hasConnection = true; } double nodeDistance = Utils.DistanceBetween(node, coordinates); if (nodeDistance < distance && hasConnection) { closest = node; distance = nodeDistance; } } return closest; } private static double EdgeWeight(OsmNode node1, OsmNode node2, ulong wayId, Tag.SpeedType vehicle, ref RegionManager regionManager) { TagManager tags = regionManager.GetRegion(node1.coordinates)!.tagManager; double distance = Utils.DistanceBetween(node1, node2); Tag.WayType wayType = (Tag.WayType)tags.GetTag(wayId, Tag.TagType.highway)!; switch (vehicle) { case Tag.SpeedType.pedestrian: byte speed = Tag.defaultSpeedPedestrian[wayType]; if(speed is not 0) return distance / speed; else return Double.PositiveInfinity; case Tag.SpeedType.car: case Tag.SpeedType.road: byte? maxspeed = (byte?)tags.GetTag(wayId, Tag.TagType.maxspeed); if (maxspeed is not null) return distance / Convert.ToDouble(maxspeed); else maxspeed = Tag.defaultSpeedCar[wayType]; if(maxspeed is not 0) return distance / Tag.defaultSpeedCar[wayType]; else return Double.PositiveInfinity;; default: return double.PositiveInfinity; } } }