From 4600105b0b979186db2f0d1b5ef9112439fc005b Mon Sep 17 00:00:00 2001 From: glax Date: Sat, 1 Apr 2023 18:10:21 +0200 Subject: [PATCH] Changed ClosestNodeToCoordinates to include only nodes that have connections for appropriate SpeedType (e.g. roads for cars, footways for pedestrians) Changed toVisit to be a priorityqueue. Search is aborted, if within 250m of goal. --- Pathfinding/Pathfinder.cs | 60 +++++++++++++++++++++++++-------------- 1 file changed, 38 insertions(+), 22 deletions(-) diff --git a/Pathfinding/Pathfinder.cs b/Pathfinding/Pathfinder.cs index b2adcad..13f2354 100644 --- a/Pathfinding/Pathfinder.cs +++ b/Pathfinding/Pathfinder.cs @@ -14,27 +14,23 @@ public class Pathfinder if (startRegion is null || goalRegion is null) return new List(); - OsmNode? startNode = ClosestNodeToCoordinates(start, startRegion); - OsmNode? goalNode = ClosestNodeToCoordinates(goal, goalRegion); + OsmNode? startNode = ClosestNodeToCoordinates(start, startRegion, vehicle); + OsmNode? goalNode = ClosestNodeToCoordinates(goal, goalRegion, vehicle); if (startNode == null || goalNode == null) return new List(); - - List toVisit = new() { startNode }; - OsmNode closestNodeToGoal = toVisit.First(); - closestNodeToGoal.currentPathWeight = 0; - closestNodeToGoal.currentPathLength = 0; + + 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) { - //Console.WriteLine("toVisit-length: {0}", toVisit.Count); - closestNodeToGoal = toVisit.First(); - foreach (OsmNode node in toVisit.Where(node => node.directDistanceToGoal.Equals(Double.MaxValue))) - { - node.directDistanceToGoal = Utils.DistanceBetween(node, goalNode); - } - - closestNodeToGoal = toVisit.OrderBy(node => node.directDistanceToGoal).First(); + closestNodeToGoal = toVisit.Dequeue(); + Console.WriteLine($"{toVisit.Count:000} {closestNodeToGoal.directDistanceToGoal:#.00} current:{closestNodeToGoal}"); foreach (OsmEdge edge in closestNodeToGoal.edges) { @@ -49,19 +45,20 @@ public class Pathfinder neighbor.currentPathWeight = newPotentialWeight; neighbor.currentPathLength = closestNodeToGoal.currentPathLength + Utils.DistanceBetween(closestNodeToGoal, neighbor); - if (neighbor.Equals(goalNode)) + if (neighbor.Equals(goalNode) || closestNodeToGoal.directDistanceToGoal < 250) stop = true; else - toVisit.Add(neighbor); + { + neighbor.directDistanceToGoal = Utils.DistanceBetween(neighbor, goalNode); + toVisit.Enqueue(neighbor, neighbor.directDistanceToGoal); + } } } } - - toVisit.Remove(closestNodeToGoal); } List path = new(); - OsmNode? currentNode = goalNode; + OsmNode? currentNode = closestNodeToGoal; while (currentNode != null && !currentNode.Equals(startNode)) { path.Add(currentNode); @@ -73,14 +70,33 @@ public class Pathfinder return path; } - private static OsmNode? ClosestNodeToCoordinates(Coordinates coordinates, Region region) + 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) + if (nodeDistance < distance && hasConnection) { closest = node; distance = nodeDistance;