From c238a9eed345f1f0551846425ba84ce6b850efad Mon Sep 17 00:00:00 2001 From: glax Date: Sun, 9 Apr 2023 20:41:33 +0200 Subject: [PATCH] Implemented "smart" routing through priority: Factors: speed, roadtype, junctions / waychanges. --- Pathfinding/Pathfinder.cs | 105 +++++++++++++++++++++++++++++++++ Pathfinding/Pathfinder_Time.cs | 24 +++++--- 2 files changed, 121 insertions(+), 8 deletions(-) diff --git a/Pathfinding/Pathfinder.cs b/Pathfinding/Pathfinder.cs index 26e3662..e6a3f3a 100644 --- a/Pathfinding/Pathfinder.cs +++ b/Pathfinding/Pathfinder.cs @@ -1,5 +1,6 @@ using OSMDatastructure; using OSMDatastructure.Graph; +using WayType = OSMDatastructure.Tag.WayType; namespace Pathfinding; @@ -29,6 +30,110 @@ public static partial class Pathfinder 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(); diff --git a/Pathfinding/Pathfinder_Time.cs b/Pathfinding/Pathfinder_Time.cs index 4ef5fff..8b50d4a 100644 --- a/Pathfinding/Pathfinder_Time.cs +++ b/Pathfinding/Pathfinder_Time.cs @@ -22,7 +22,7 @@ public static partial class Pathfinder while (toVisit.Count > 0) { OsmNode currentNode = toVisit.Dequeue(); - + foreach (OsmEdge edge in currentNode.edges.Where( edge => regionManager.TestValidConnectionForType(currentNode, edge, vehicle))) { @@ -38,16 +38,24 @@ public static partial class Pathfinder if (newPotentialWeight < neighbor.currentPathWeight) { neighbor.previousPathNode = currentNode; - neighbor.currentPathLength = currentNode.currentPathLength + Utils.DistanceBetween(currentNode, neighbor); neighbor.currentPathWeight = newPotentialWeight; - - if(neighbor.Equals(goalNode)) - return GetRouteFromCalc(goalNode, regionManager); - //stop = true; - if (!toVisit.UnorderedItems.Any(item => item.Element.Equals(neighbor)) && !stop) + + if (neighbor.Equals(goalNode)) { - toVisit.Enqueue(neighbor, neighbor.currentPathWeight + neighbor.directDistanceToGoal); + currentNode = neighbor; + currentNode.currentPathLength = 0; + while (currentNode != startAndEndNode.Item1 && currentNode.previousPathNode is not null) + { + currentNode.previousPathNode.currentPathLength = currentNode.currentPathLength + + Utils.DistanceBetween(currentNode, currentNode.previousPathNode); + currentNode = currentNode.previousPathNode; + } + + return GetRouteFromCalc(goalNode, regionManager); } + //stop = true; + if (!toVisit.UnorderedItems.Any(item => item.Element.Equals(neighbor))) + toVisit.Enqueue(neighbor, GetPriority(currentNode, currentNode.previousPathNode, edge, vehicle, regionManager)); } } }