From 308579279b292c4a227fdbf416d7dc0bb6db3509 Mon Sep 17 00:00:00 2001 From: glax Date: Tue, 11 Apr 2023 01:04:19 +0200 Subject: [PATCH] Working. Weight calculation is still wonky, as well as heuristic needing tuning. --- API/Program.cs | 33 +++-- OSMDatastructure/Graph/OsmNode.cs | 21 +-- Pathfinding/PathNode.cs | 18 +-- Pathfinding/Pathfinder.cs | 203 ++++++++++++++--------------- Pathfinding/Pathfinder_Distance.cs | 50 ------- Pathfinding/Pathfinder_Time.cs | 63 --------- 6 files changed, 124 insertions(+), 264 deletions(-) delete mode 100644 Pathfinding/Pathfinder_Distance.cs delete mode 100644 Pathfinding/Pathfinder_Time.cs diff --git a/API/Program.cs b/API/Program.cs index 0356d22..f1f4a37 100644 --- a/API/Program.cs +++ b/API/Program.cs @@ -1,4 +1,3 @@ -using System.Text; using System.Text.Json.Serialization; using OSMDatastructure; using OSMDatastructure.Graph; @@ -15,22 +14,37 @@ builder.Services.AddSwaggerGen(); var app = builder.Build(); -app.MapGet("/getRouteDistance", (float latStart, float lonStart, float latEnd, float lonEnd) => +app.MapGet("/getRouteBeta", (float latStart, float lonStart, float latEnd, float lonEnd, Tag.SpeedType vehicle, double stayOnSameRoadPriority, double useHigherLevelRoadsPriority, double useRoadsWithLessJunctionsPriority) => { DateTime startCalc = DateTime.Now; - List result = Pathfinder.AStarDistance("D:/stuttgart-regbez-latest", new Coordinates(latStart, lonStart), new Coordinates(latEnd, lonEnd)); + List result = Pathfinder.AStar("D:/stuttgart-regbez-latest", new Coordinates(latStart, lonStart), + new Coordinates(latEnd, lonEnd), vehicle, useHigherLevelRoadsPriority, stayOnSameRoadPriority, + useRoadsWithLessJunctionsPriority); + PathResult pathResult = new PathResult(DateTime.Now - startCalc, result); + return RenderPath.Renderer.DrawFromPath(result); + } +); + +app.MapGet("/getRoute", (float latStart, float lonStart, float latEnd, float lonEnd, Tag.SpeedType vehicle, double stayOnSameRoadPriority, double useHigherLevelRoadsPriority, double useRoadsWithLessJunctionsPriority) => + { + DateTime startCalc = DateTime.Now; + List result = Pathfinder.AStar("D:/stuttgart-regbez-latest", new Coordinates(latStart, lonStart), + new Coordinates(latEnd, lonEnd), vehicle, useHigherLevelRoadsPriority, stayOnSameRoadPriority, + useRoadsWithLessJunctionsPriority); PathResult pathResult = new PathResult(DateTime.Now - startCalc, result); return pathResult; } ); -app.MapGet("/getRouteTime", (float latStart, float lonStart, float latEnd, float lonEnd, Tag.SpeedType vehicle) => +app.MapGet("/getShortestRoute", (float latStart, float lonStart, float latEnd, float lonEnd) => { DateTime startCalc = DateTime.Now; - List result = Pathfinder.AStarTime("D:/stuttgart-regbez-latest", new Coordinates(latStart, lonStart), new Coordinates(latEnd, lonEnd), vehicle); + List result = Pathfinder.AStar("D:/stuttgart-regbez-latest", new Coordinates(latStart, lonStart), + new Coordinates(latEnd, lonEnd), Tag.SpeedType.any, 0, 0, + 0); PathResult pathResult = new PathResult(DateTime.Now - startCalc, result); return pathResult; - } + } ); app.MapGet("/getClosestNode", (float lat, float lon) => @@ -57,18 +71,11 @@ app.Run(); internal class PathResult { [JsonInclude]public TimeSpan calcTime; - [JsonInclude] public double pathWeight = double.MaxValue; - [JsonInclude] public double pathTravelDistance = double.MaxValue; [JsonInclude]public List pathNodes; public PathResult(TimeSpan calcTime, List pathNodes) { this.calcTime = calcTime; this.pathNodes = pathNodes; - if (pathNodes.Count > 0) - { - this.pathWeight = pathNodes.Last().currentPathWeight; - this.pathTravelDistance = pathNodes.First().currentPathLength; - } } } \ No newline at end of file diff --git a/OSMDatastructure/Graph/OsmNode.cs b/OSMDatastructure/Graph/OsmNode.cs index 0469330..f42a19f 100644 --- a/OSMDatastructure/Graph/OsmNode.cs +++ b/OSMDatastructure/Graph/OsmNode.cs @@ -1,4 +1,3 @@ -using System.Runtime.Serialization; using System.Text.Json.Serialization; namespace OSMDatastructure.Graph; @@ -8,21 +7,8 @@ public class OsmNode { public ulong nodeId { get; } public HashSet edges { get; set; } - public Coordinates coordinates { get; } - - [JsonIgnore][NonSerialized]public OsmNode? previousPathNode = null; - [JsonIgnore][NonSerialized]public double currentPathWeight = double.MaxValue; - [JsonIgnore][NonSerialized]public double currentPathLength = double.MaxValue; - [JsonIgnore][NonSerialized]public double directDistanceToGoal = double.MaxValue; + public Coordinates coordinates { get; } - [OnDeserialized] - internal void SetDefaultValues(StreamingContext context) - { - currentPathWeight = double.MaxValue; - currentPathLength = double.MaxValue; - directDistanceToGoal = double.MaxValue; - } - public OsmNode(ulong nodeId, float lat, float lon) { this.nodeId = nodeId; @@ -53,9 +39,6 @@ public class OsmNode public override string ToString() { - if(previousPathNode is not null) - return $"{nodeId} {coordinates} ec:{edges.Count} d:{directDistanceToGoal} w:{currentPathWeight} l:{currentPathLength} p:{previousPathNode.nodeId}"; - return - $"{nodeId} {coordinates} ec:{edges.Count} d:{directDistanceToGoal} w:{currentPathWeight} l:{currentPathLength} null"; + return $"{nodeId} {coordinates} ec:{edges.Count}"; } } \ No newline at end of file diff --git a/Pathfinding/PathNode.cs b/Pathfinding/PathNode.cs index a526339..6a50d99 100644 --- a/Pathfinding/PathNode.cs +++ b/Pathfinding/PathNode.cs @@ -6,12 +6,6 @@ namespace Pathfinding; public class PathNode : OsmNode { - [JsonInclude]public new double directDistanceToGoal = double.MaxValue; - [JsonInclude]public double directDistanceDelta = double.MaxValue; - [JsonInclude]public new double currentPathLength = double.MaxValue; - [JsonInclude]public double pathDistanceDelta = double.MaxValue; - [JsonInclude]public new double currentPathWeight = double.MaxValue; - [JsonInclude]public double pathWeightDelta = double.MaxValue; [JsonInclude]public Dictionary tags = new(); public PathNode(ulong nodeId, float lat, float lon) : base(nodeId, lat, lon) @@ -22,19 +16,11 @@ public class PathNode : OsmNode { } - public static PathNode? FromOsmNode(OsmNode? node, HashSet? tags, double pathDistanceDelta, double pathWeightDelta, double directDistanceDelta) + public static PathNode? FromOsmNode(OsmNode? node, HashSet? tags) { if (node is null) return null; - PathNode retNode = new(node.nodeId, node.coordinates) - { - currentPathLength = node.currentPathLength, - currentPathWeight = double.IsPositiveInfinity(node.currentPathWeight) ? double.MaxValue : node.currentPathWeight, - directDistanceToGoal = node.directDistanceToGoal, - directDistanceDelta = directDistanceDelta, - pathDistanceDelta = pathDistanceDelta, - pathWeightDelta = pathWeightDelta - }; + PathNode retNode = new(node.nodeId, node.coordinates); if (tags != null) foreach (Tag tag in tags) { diff --git a/Pathfinding/Pathfinder.cs b/Pathfinding/Pathfinder.cs index f919d8b..0c4eca8 100644 --- a/Pathfinding/Pathfinder.cs +++ b/Pathfinding/Pathfinder.cs @@ -5,93 +5,124 @@ using WayType = OSMDatastructure.Tag.WayType; namespace Pathfinding; -public static partial class Pathfinder +public static class Pathfinder { - - private static ValueTuple SetupNodes(Coordinates startCoordinates, Coordinates goalCoordinates, RegionManager regionManager, SpeedType vehicle) + + public static List AStar(string workingDir, Coordinates startCoordinates, Coordinates goalCoordinates, + SpeedType vehicle, double heuristicRoadLevelPriority, double heuristicSameRoadPriority, + double heuristicFewJunctionsPriority) { - ValueTuple retTuple = new(); - retTuple.Item1 = regionManager.ClosestNodeToCoordinates(startCoordinates, vehicle); - retTuple.Item2 = regionManager.ClosestNodeToCoordinates(goalCoordinates, vehicle); - 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; - } + 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 List(); - private static double EdgeWeight(OsmNode node1, OsmEdge edge, 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; - } + PriorityQueue openSetfScore = new(); + openSetfScore.Enqueue(startNode, 0); + Dictionary cameFromDict = new(); + Dictionary gScore = new(); + gScore.Add(startNode, 0); - private static double GetPriority(OsmNode currentNode, OsmNode? previousNode, OsmEdge currentEdge, SpeedType vehicle, RegionManager regionManager) - { - if (vehicle == SpeedType.any) - return 1; - 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)!; - - 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); - - double wayChange = 0; - if (previousNode is not null && previousNode.edges.Count > 0) + while (openSetfScore.Count > 0) { - OsmEdge? pEdge = previousNode.edges.FirstOrDefault(e => e.neighborId.Equals(currentNode.nodeId)); - if (pEdge is not null) + OsmNode currentNode = openSetfScore.Dequeue(); + if (currentNode.Equals(goalNode)) + return GetPath(cameFromDict, goalNode, regionManager); + + foreach (OsmEdge edge in currentNode.edges) { - TagManager? prevTags = regionManager.GetRegion(previousNode.coordinates)?.tagManager; - if (prevTags is not null) + OsmNode? neighbor = regionManager.GetNode(edge.neighborId, edge.neighborRegion); + if (neighbor 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; + 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); - 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; + return new List(); } + private static List GetPath(Dictionary cameFromDict, OsmNode goalNode, RegionManager regionManager) + { + 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 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 + (prio * 10)); + } + + 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; + + //Console.WriteLine($"{roadPriority:000.00} {sameRoadName:000.00} {junctionCount:000.00} {distanceImprovement:+000.00;-000.00;0000.00}"); + + return Utils.DistanceBetween(neighborNode, goalNode) - (roadPriority + sameRoadName + junctionCount) * 100; + } + 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 double.MaxValue; + return 0; if (vehicle == SpeedType.car) { switch (wayType) @@ -106,6 +137,7 @@ public static partial class Pathfinder return 8; case WayType.secondary: case WayType.secondary_link: + return 6; case WayType.tertiary: case WayType.tertiary_link: return 5; @@ -113,12 +145,10 @@ public static partial class Pathfinder case WayType.residential: case WayType.road: case WayType.living_street: - return 3; + return 2; case WayType.service: case WayType.track: return 0.01; - default: - return 1; } } if (vehicle == SpeedType.pedestrian) @@ -144,42 +174,9 @@ public static partial class Pathfinder case WayType.tertiary_link: case WayType.escape: return 2; - default: - return 1; } } return 0.01; - - } - - 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; } } \ No newline at end of file diff --git a/Pathfinding/Pathfinder_Distance.cs b/Pathfinding/Pathfinder_Distance.cs deleted file mode 100644 index efa082d..0000000 --- a/Pathfinding/Pathfinder_Distance.cs +++ /dev/null @@ -1,50 +0,0 @@ -using OSMDatastructure; -using OSMDatastructure.Graph; - -namespace Pathfinding; - -public static partial class Pathfinder -{ - public static List AStarDistance(string workingDir, Coordinates start, - Coordinates goal) - { - RegionManager regionManager = new (workingDir); - ValueTuple startAndEndNode = SetupNodes(start, goal, regionManager, Tag.SpeedType.any); - if (startAndEndNode.Item1 is null || startAndEndNode.Item2 is null) - return new List(); - OsmNode goalNode = startAndEndNode.Item2!; - - PriorityQueue toVisit = new(); - toVisit.Enqueue(startAndEndNode.Item1, 0); - bool stop = false; - - while (toVisit.Count > 0) - { - OsmNode currentNode = toVisit.Dequeue(); - - foreach (OsmEdge edge in currentNode.edges) - { - OsmNode? neighbor = regionManager.GetNode(edge.neighborId, edge.neighborRegion); - if (neighbor is not null) - { - if (Math.Abs(neighbor.directDistanceToGoal - double.MaxValue) < 1) - neighbor.directDistanceToGoal = Utils.DistanceBetween(neighbor, goalNode); - double newPotentialLength = currentNode.currentPathLength + Utils.DistanceBetween(currentNode, neighbor); - if (newPotentialLength < neighbor.currentPathLength) - { - neighbor.previousPathNode = currentNode; - neighbor.currentPathLength = newPotentialLength; - - if(neighbor.Equals(goalNode)) - return GetRouteFromCalc(goalNode, regionManager); - if (!toVisit.UnorderedItems.Any(item => item.Element.Equals(neighbor)) && !stop) - { - toVisit.Enqueue(neighbor, neighbor.directDistanceToGoal); - } - } - } - } - } - return GetRouteFromCalc(goalNode, regionManager); - } -} \ No newline at end of file diff --git a/Pathfinding/Pathfinder_Time.cs b/Pathfinding/Pathfinder_Time.cs deleted file mode 100644 index 833895e..0000000 --- a/Pathfinding/Pathfinder_Time.cs +++ /dev/null @@ -1,63 +0,0 @@ -using OSMDatastructure; -using OSMDatastructure.Graph; -using Utils = OSMDatastructure.Utils; - -namespace Pathfinding; - -public static partial class Pathfinder -{ - public static List AStarTime(string workingDir, Coordinates start, - Coordinates goal, Tag.SpeedType vehicle) - { - RegionManager regionManager = new (workingDir); - ValueTuple startAndEndNode = SetupNodes(start, goal, regionManager, vehicle); - if (startAndEndNode.Item1 is null || startAndEndNode.Item2 is null) - return new List(); - OsmNode goalNode = startAndEndNode.Item2!; - - PriorityQueue toVisit = new(); - toVisit.Enqueue(startAndEndNode.Item1, 0); - - while (toVisit.Count > 0) - { - OsmNode currentNode = toVisit.Dequeue(); - - foreach (OsmEdge edge in currentNode.edges.Where( - edge => regionManager.TestValidConnectionForType(currentNode, edge, vehicle))) - { - OsmNode? neighbor = regionManager.GetNode(edge.neighborId, edge.neighborRegion); - if (neighbor is not null) - { - if (Math.Abs(neighbor.directDistanceToGoal - double.MaxValue) < 1) - neighbor.directDistanceToGoal = Utils.DistanceBetween(neighbor, goalNode); - - double newPotentialWeight = currentNode.currentPathWeight + - EdgeWeight(currentNode, edge, vehicle, regionManager); - - if (newPotentialWeight < neighbor.currentPathWeight) - { - neighbor.previousPathNode = currentNode; - neighbor.currentPathWeight = newPotentialWeight; - - if (neighbor.Equals(goalNode)) - { - currentNode = neighbor; - currentNode.currentPathLength = 0; - while (!currentNode.Equals(startAndEndNode.Item1) && currentNode.previousPathNode is not null) - { - currentNode.previousPathNode.currentPathLength = currentNode.currentPathLength + - Utils.DistanceBetween(currentNode, currentNode.previousPathNode); - currentNode = currentNode.previousPathNode; - } - - return GetRouteFromCalc(goalNode, regionManager); - } - - toVisit.Enqueue(neighbor, GetPriority(currentNode, currentNode.previousPathNode, edge, vehicle, regionManager)); - } - } - } - } - return GetRouteFromCalc(goalNode, regionManager); - } -} \ No newline at end of file