using OSMDatastructure; using OSMDatastructure.Graph; using WayType = OSMDatastructure.Tag.WayType; namespace Pathfinding; public static partial class Pathfinder { private static ValueTuple SetupNodes(Coordinates startCoordinates, Coordinates goalCoordinates, RegionManager regionManager ) { ValueTuple retTuple = new(); retTuple.Item1 = regionManager.ClosestNodeToCoordinates(startCoordinates, Tag.SpeedType.any); retTuple.Item2 = regionManager.ClosestNodeToCoordinates(goalCoordinates, Tag.SpeedType.any); if (retTuple.Item1 is null || retTuple.Item2 is null) return retTuple; retTuple.Item1.currentPathWeight = 0; retTuple.Item1.currentPathLength = 0; retTuple.Item1.directDistanceToGoal = Utils.DistanceBetween(retTuple.Item1, retTuple.Item2); return retTuple; } private static double EdgeWeight(OsmNode node1, OsmEdge edge, Tag.SpeedType vehicle, RegionManager regionManager) { OsmNode? node2 = regionManager.GetNode(edge.neighborId, edge.neighborRegion); if (node2 is null) return double.MaxValue; double distance = Utils.DistanceBetween(node1, node2); byte speed = regionManager.GetSpeedForEdge(node1, edge.wayId, vehicle); return speed is 0 ? double.MaxValue : distance / speed; } private static double GetPriority(OsmNode current, OsmNode? previous, OsmEdge edge, Tag.SpeedType vehicle, RegionManager regionManager) { if (vehicle == Tag.SpeedType.any) return 1; const double roadPriorityFactor = 1; const double junctionFactor = 2; const double wayChangeFactor = 2; Region r = regionManager.GetRegion(current.coordinates)!; double roadPriority = GetPriorityVehicleRoad(edge, vehicle, r) * 0.1 * roadPriorityFactor; double roadSpeed = regionManager.GetSpeedForEdge(current, edge.wayId, vehicle); if(vehicle == Tag.SpeedType.pedestrian) return (current.directDistanceToGoal / roadSpeed) * roadPriority; ulong previousWayId = UInt64.MaxValue; if (previous?.edges is not null) { foreach (OsmEdge e in previous.edges) if (e.neighborId.Equals(current.nodeId)) { previousWayId = e.wayId; break; } } double junctionPriority = (current.edges.Count > 2 ? 1 : 0) * junctionFactor; double wayChange = (previousWayId != edge.wayId ? 1 : 0) * wayChangeFactor; if(vehicle == Tag.SpeedType.car) return (current.directDistanceToGoal / roadSpeed) * roadPriority * ((junctionPriority + wayChange) / 2); return double.MaxValue; } private static double GetPriorityVehicleRoad(OsmEdge edge, Tag.SpeedType vehicle, Region region) { if (vehicle == Tag.SpeedType.any) return 1; WayType? wayType = (WayType?)region.tagManager.GetTag(edge.wayId, Tag.TagType.highway); if(wayType is null) return double.MaxValue; if (vehicle == Tag.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 1; case WayType.secondary: case WayType.secondary_link: return 2; case WayType.tertiary: case WayType.tertiary_link: return 3; case WayType.unclassified: case WayType.residential: case WayType.road: return 4; case WayType.living_street: case WayType.service: case WayType.track: return 5; default: return 100; } } if (vehicle == Tag.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 1; case WayType.service: case WayType.cycleway: case WayType.bridleway: case WayType.road: case WayType.track: case WayType.unclassified: return 2; case WayType.tertiary: case WayType.tertiary_link: case WayType.escape: return 5; default: return 100; } } return 100; } private static List GetRouteFromCalc(OsmNode goalNode, RegionManager regionManager) { List path = new(); OsmNode? currentNode = goalNode; while (currentNode is not null) { HashSet? tags = null; double pathDistanceDelta = 0; double pathWeightDelta = 0; double directDistanceDelta = 0; if (currentNode.previousPathNode is not null) { OsmEdge edge = currentNode.previousPathNode!.edges.First(e => e.neighborId.Equals(currentNode.nodeId)); tags = regionManager.GetRegion(currentNode.coordinates)!.tagManager.GetTagsForWayId(edge.wayId); pathDistanceDelta = currentNode.currentPathLength - currentNode.previousPathNode.currentPathLength; pathWeightDelta = currentNode.currentPathWeight - currentNode.previousPathNode.currentPathWeight; directDistanceDelta = currentNode.directDistanceToGoal - currentNode.previousPathNode.directDistanceToGoal; } PathNode? pn = PathNode.FromOsmNode(currentNode, tags, pathDistanceDelta, pathWeightDelta, directDistanceDelta); if(pn is not null) path.Add(pn!); currentNode = currentNode.previousPathNode; } path.Reverse(); return path; } }