From f42e45804876d351da6dfd0725dc74c5db18db8b Mon Sep 17 00:00:00 2001 From: glax Date: Thu, 6 Apr 2023 02:23:12 +0200 Subject: [PATCH] Created GetSpeed method for uniform implementation Changed Pathfinding class to static Changed AStar to continue after path found. --- Pathfinding/Pathfinder.cs | 65 +++++++++++++++++++-------------------- 1 file changed, 32 insertions(+), 33 deletions(-) diff --git a/Pathfinding/Pathfinder.cs b/Pathfinding/Pathfinder.cs index dec6866..b1568f8 100644 --- a/Pathfinding/Pathfinder.cs +++ b/Pathfinding/Pathfinder.cs @@ -4,7 +4,7 @@ using OSMImporter; namespace Pathfinding; -public class Pathfinder +public static class Pathfinder { public static List CustomAStar(string workingDir, Coordinates start, Coordinates goal, Tag.SpeedType vehicle) { @@ -14,8 +14,8 @@ public class Pathfinder if (startRegion is null || goalRegion is null) return new List(); - OsmNode? startNode = ClosestNodeToCoordinates(start, startRegion, vehicle); - OsmNode? goalNode = ClosestNodeToCoordinates(goal, goalRegion, vehicle); + OsmNode? startNode = ClosestNodeToCoordinates(start, startRegion, vehicle, ref regionManager); + OsmNode? goalNode = ClosestNodeToCoordinates(goal, goalRegion, vehicle, ref regionManager); if (startNode == null || goalNode == null) return new List(); @@ -27,7 +27,7 @@ public class Pathfinder OsmNode closestNodeToGoal = startNode; bool stop = false; - while (toVisit.Count > 0 && !stop) + while (toVisit.Count > 0) { closestNodeToGoal = toVisit.Dequeue(); Console.WriteLine($"{toVisit.Count:000} {closestNodeToGoal.directDistanceToGoal:#.00} current:{closestNodeToGoal}"); @@ -44,10 +44,13 @@ public class Pathfinder neighbor.previousPathNode = closestNodeToGoal; neighbor.currentPathWeight = newPotentialWeight; neighbor.currentPathLength = closestNodeToGoal.currentPathLength + Utils.DistanceBetween(closestNodeToGoal, neighbor); - - if (neighbor.Equals(goalNode) || closestNodeToGoal.directDistanceToGoal < 10) //change for faster + + if (neighbor.Equals(goalNode) || closestNodeToGoal.directDistanceToGoal < 10) + { stop = true; - else + goalNode = neighbor; + } + else if(!stop) { neighbor.directDistanceToGoal = Utils.DistanceBetween(neighbor, goalNode); toVisit.Enqueue(neighbor, neighbor.directDistanceToGoal); @@ -58,7 +61,7 @@ public class Pathfinder } List path = new(); - OsmNode? currentNode = closestNodeToGoal; + OsmNode? currentNode = goalNode; while (currentNode != null && !currentNode.Equals(startNode)) { path.Add(currentNode); @@ -70,7 +73,7 @@ public class Pathfinder return path; } - private static OsmNode? ClosestNodeToCoordinates(Coordinates coordinates, Region region, Tag.SpeedType vehicle) + private static OsmNode? ClosestNodeToCoordinates(Coordinates coordinates, Region region, Tag.SpeedType vehicle, ref RegionManager regionManager) { OsmNode? closest = null; double distance = double.MaxValue; @@ -79,19 +82,7 @@ public class Pathfinder 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; - } - + double speed = GetSpeed(node, edge.wayId, vehicle, ref regionManager); if (speed != 0) hasConnection = true; } @@ -106,30 +97,38 @@ public class Pathfinder return closest; } - private static double EdgeWeight(OsmNode node1, OsmNode node2, ulong wayId, Tag.SpeedType vehicle, ref RegionManager regionManager) + private static double GetSpeed(OsmNode node1, ulong wayId, Tag.SpeedType vehicle, ref RegionManager regionManager) { TagManager tags = regionManager.GetRegion(node1.coordinates)!.tagManager; - double distance = Utils.DistanceBetween(node1, node2); Tag.WayType wayType = (Tag.WayType)tags.GetTag(wayId, Tag.TagType.highway)!; switch (vehicle) { case Tag.SpeedType.pedestrian: byte speed = Tag.defaultSpeedPedestrian[wayType]; - if(speed is not 0) - return distance / speed; - else return Double.PositiveInfinity; + if (speed is not 0) + return speed; + return 0; case Tag.SpeedType.car: case Tag.SpeedType.road: byte? maxspeed = (byte?)tags.GetTag(wayId, Tag.TagType.maxspeed); if (maxspeed is not null) - return distance / Convert.ToDouble(maxspeed); - else - maxspeed = Tag.defaultSpeedCar[wayType]; + return (double)maxspeed; + maxspeed = Tag.defaultSpeedCar[wayType]; if(maxspeed is not 0) - return distance / Tag.defaultSpeedCar[wayType]; - else return Double.PositiveInfinity;; + return (byte)maxspeed; + return 0; default: - return double.PositiveInfinity; + return 0; } } + + private static double EdgeWeight(OsmNode node1, OsmNode node2, ulong wayId, Tag.SpeedType vehicle, ref RegionManager regionManager) + { + double distance = Utils.DistanceBetween(node1, node2); + double speed = GetSpeed(node1, wayId, vehicle, ref regionManager); + if (speed is not 0) + return distance / speed; + else + return double.PositiveInfinity; + } } \ No newline at end of file