From 2131ac4afe6856322da5d54ed0b9241fb3403db1 Mon Sep 17 00:00:00 2001 From: glax Date: Mon, 10 Apr 2023 01:33:18 +0200 Subject: [PATCH] Still requires tuning --- Pathfinding/Pathfinder.cs | 82 ++++++++++++++++++++++++---------- Pathfinding/Pathfinder_Time.cs | 7 ++- 2 files changed, 61 insertions(+), 28 deletions(-) diff --git a/Pathfinding/Pathfinder.cs b/Pathfinding/Pathfinder.cs index 80bdbc3..f919d8b 100644 --- a/Pathfinding/Pathfinder.cs +++ b/Pathfinding/Pathfinder.cs @@ -1,5 +1,6 @@ using OSMDatastructure; using OSMDatastructure.Graph; +using static OSMDatastructure.Tag; using WayType = OSMDatastructure.Tag.WayType; namespace Pathfinding; @@ -7,7 +8,7 @@ namespace Pathfinding; public static partial class Pathfinder { - private static ValueTuple SetupNodes(Coordinates startCoordinates, Coordinates goalCoordinates, RegionManager regionManager, Tag.SpeedType vehicle) + private static ValueTuple SetupNodes(Coordinates startCoordinates, Coordinates goalCoordinates, RegionManager regionManager, SpeedType vehicle) { ValueTuple retTuple = new(); retTuple.Item1 = regionManager.ClosestNodeToCoordinates(startCoordinates, vehicle); @@ -20,7 +21,7 @@ public static partial class Pathfinder return retTuple; } - private static double EdgeWeight(OsmNode node1, OsmEdge edge, Tag.SpeedType vehicle, RegionManager regionManager) + private static double EdgeWeight(OsmNode node1, OsmEdge edge, SpeedType vehicle, RegionManager regionManager) { OsmNode? node2 = regionManager.GetNode(edge.neighborId, edge.neighborRegion); if (node2 is null) @@ -30,36 +31,68 @@ public static partial class Pathfinder return speed is 0 ? double.MaxValue : distance / speed; } - //TODO when refs are imported use same road - private static double GetPriority(OsmNode current, OsmEdge edge, Tag.SpeedType vehicle, RegionManager regionManager) + private static double GetPriority(OsmNode currentNode, OsmNode? previousNode, OsmEdge currentEdge, SpeedType vehicle, RegionManager regionManager) { - if (vehicle == Tag.SpeedType.any) + if (vehicle == SpeedType.any) return 1; - const double roadPriorityFactor = 1; - const double roadSpeedFactor = 0.0001; - Region r = regionManager.GetRegion(current.coordinates)!; - - double roadPriority = GetPriorityVehicleRoad(edge, vehicle, r) * roadPriorityFactor; - double roadSpeed = regionManager.GetSpeedForEdge(current, edge.wayId, vehicle) * roadSpeedFactor; + const double roadPriorityFactor = 3.5; + const double roadSpeedFactor = 1.4; + const double roadNameChangeFactor = 1.4; + const double distanceDeltaFactor = 0.17; + Region r = regionManager.GetRegion(currentNode.coordinates)!; - if (vehicle == Tag.SpeedType.pedestrian) - return current.directDistanceToGoal / roadPriority; + double distanceDelta = 0; + if(previousNode is not null) + distanceDelta = (previousNode.directDistanceToGoal - currentNode.directDistanceToGoal) * distanceDeltaFactor; + double roadPriority = GetPriorityVehicleRoad(currentEdge, vehicle, r) * roadPriorityFactor; + double roadSpeed = regionManager.GetSpeedForEdge(currentNode, currentEdge.wayId, vehicle) * 0.1 * roadSpeedFactor; + if (vehicle == SpeedType.pedestrian) + return currentNode.directDistanceToGoal / (roadPriority + roadSpeed + distanceDelta); - if (vehicle == Tag.SpeedType.car) - return current.directDistanceToGoal / roadPriority; + double wayChange = 0; + if (previousNode is not null && previousNode.edges.Count > 0) + { + OsmEdge? pEdge = previousNode.edges.FirstOrDefault(e => e.neighborId.Equals(currentNode.nodeId)); + if (pEdge is not null) + { + TagManager? prevTags = regionManager.GetRegion(previousNode.coordinates)?.tagManager; + if (prevTags is not null) + { + TagManager curTags = r.tagManager; + + bool sameName = false; + string? curName = (string?)curTags.GetTag(currentEdge.wayId, TagType.name); + if (curName is not null && (string?)prevTags.GetTag(pEdge.wayId, TagType.name) == curName) + sameName = true; + + bool sameRef = false; + string? curRef = (string?)curTags.GetTag(currentEdge.wayId, TagType.tagref); + if (curRef is not null && (string?)prevTags.GetTag(pEdge.wayId, TagType.tagref) == curRef) + sameRef = true; + + wayChange = (sameRef || sameName ? 1 : 0) * roadNameChangeFactor; + } + } + } + + double div = (roadPriority + wayChange + roadSpeed + (distanceDelta > 0 ? distanceDelta : 0)) + 1; + double prio = currentNode.directDistanceToGoal / div; + Console.WriteLine($"{currentNode.directDistanceToGoal:000000.00}/{div:+00.00;-00.00;000.00}={prio:+00000.00;-00000.00;000000.00} Type{roadPriority:00.00} name{wayChange:00.00} speed{roadSpeed:00.00} distance{distanceDelta:+00.00;-00.00;0}"); + if (vehicle == SpeedType.car) + return prio; return double.MaxValue; } - private static double GetPriorityVehicleRoad(OsmEdge edge, Tag.SpeedType vehicle, Region region) + private static double GetPriorityVehicleRoad(OsmEdge edge, SpeedType vehicle, Region region) { - if (vehicle == Tag.SpeedType.any) + if (vehicle == SpeedType.any) return 1; - WayType? wayType = (WayType?)region.tagManager.GetTag(edge.wayId, Tag.TagType.highway); + WayType? wayType = (WayType?)region.tagManager.GetTag(edge.wayId, TagType.highway); if(wayType is null) return double.MaxValue; - if (vehicle == Tag.SpeedType.car) + if (vehicle == SpeedType.car) { switch (wayType) { @@ -70,24 +103,25 @@ public static partial class Pathfinder case WayType.trunk_link: case WayType.primary: case WayType.primary_link: - return 7; + return 8; case WayType.secondary: case WayType.secondary_link: case WayType.tertiary: case WayType.tertiary_link: - return 4; + return 5; case WayType.unclassified: case WayType.residential: case WayType.road: case WayType.living_street: + return 3; case WayType.service: case WayType.track: - return 2; + return 0.01; default: return 1; } } - if (vehicle == Tag.SpeedType.pedestrian) + if (vehicle == SpeedType.pedestrian) { switch (wayType) { @@ -115,7 +149,7 @@ public static partial class Pathfinder } } - return 100; + return 0.01; } diff --git a/Pathfinding/Pathfinder_Time.cs b/Pathfinding/Pathfinder_Time.cs index 23f2144..833895e 100644 --- a/Pathfinding/Pathfinder_Time.cs +++ b/Pathfinding/Pathfinder_Time.cs @@ -21,7 +21,6 @@ public static partial class Pathfinder while (toVisit.Count > 0) { OsmNode currentNode = toVisit.Dequeue(); - Console.WriteLine($"{toVisit.Count} {currentNode.directDistanceToGoal}"); foreach (OsmEdge edge in currentNode.edges.Where( edge => regionManager.TestValidConnectionForType(currentNode, edge, vehicle))) @@ -44,7 +43,7 @@ public static partial class Pathfinder { currentNode = neighbor; currentNode.currentPathLength = 0; - while (currentNode != startAndEndNode.Item1 && currentNode.previousPathNode is not null) + while (!currentNode.Equals(startAndEndNode.Item1) && currentNode.previousPathNode is not null) { currentNode.previousPathNode.currentPathLength = currentNode.currentPathLength + Utils.DistanceBetween(currentNode, currentNode.previousPathNode); @@ -53,8 +52,8 @@ public static partial class Pathfinder return GetRouteFromCalc(goalNode, regionManager); } - //if (!toVisit.UnorderedItems.Any(item => item.Element.Equals(neighbor))) - toVisit.Enqueue(neighbor, GetPriority(currentNode, edge, vehicle, regionManager)); + + toVisit.Enqueue(neighbor, GetPriority(currentNode, currentNode.previousPathNode, edge, vehicle, regionManager)); } } }