diff --git a/API/Program.cs b/API/Program.cs index 487db63..902ab0a 100644 --- a/API/Program.cs +++ b/API/Program.cs @@ -18,9 +18,9 @@ var app = builder.Build(); app.MapGet("/getRoute", (float latStart, float lonStart, float latEnd, float lonEnd) => { - ValueTuple> result = Pathfinder.CustomAStar("D:/stuttgart-regbez-latest", new Coordinates(latStart, lonStart), new Coordinates(latEnd, lonEnd), - Tag.SpeedType.car); - PathResult pathResult = new PathResult(result.Item1, result.Item2); + DateTime startCalc = DateTime.Now; + List result = Pathfinder.AStarDistance("D:/stuttgart-regbez-latest", new Coordinates(latStart, lonStart), new Coordinates(latEnd, lonEnd)); + PathResult pathResult = new PathResult(startCalc - DateTime.Now, result); return pathResult; } ); @@ -28,7 +28,7 @@ app.MapGet("/getRoute", (float latStart, float lonStart, float latEnd, float lon app.MapGet("/getClosestNode", (float lat, float lon) => { RegionManager regionManager = new RegionManager("D:/stuttgart-regbez-latest"); - return Pathfinder.ClosestNodeToCoordinates(new Coordinates(lat, lon), Tag.SpeedType.car, ref regionManager); + return Pathfinder.ClosestNodeToCoordinates(new Coordinates(lat, lon), Tag.SpeedType.car, regionManager); }); // Configure the HTTP request pipeline. diff --git a/Pathfinding/Pathfinder.cs b/Pathfinding/Pathfinder.cs index ff1c545..d2a0138 100644 --- a/Pathfinding/Pathfinder.cs +++ b/Pathfinding/Pathfinder.cs @@ -4,75 +4,23 @@ using OSMImporter; namespace Pathfinding; -public static class Pathfinder +public static partial class Pathfinder { - public static ValueTuple> CustomAStar(string workingDir, Coordinates start, Coordinates goal, Tag.SpeedType vehicle) + + private static ValueTuple SetupNodes(Coordinates startCoordinates, Coordinates goalCoordinates, RegionManager regionManager ) { - DateTime startTime = DateTime.Now; - TimeSpan calcTime; - RegionManager regionManager = new (workingDir); - OsmNode? startNode = ClosestNodeToCoordinates(start, vehicle, ref regionManager); - OsmNode? goalNode = ClosestNodeToCoordinates(goal, vehicle, ref regionManager); - if (startNode == null || goalNode == null) - { - calcTime = DateTime.Now - startTime; - return new ValueTuple>(calcTime, new List()); - } - - PriorityQueue toVisit = new(); - toVisit.Enqueue(startNode, 0); - startNode.currentPathWeight = 0; - startNode.currentPathLength = 0; - startNode.directDistanceToGoal = Utils.DistanceBetween(startNode, goalNode); - bool stop = false; - - while (toVisit.Count > 0) - { - OsmNode closestNodeToGoal = toVisit.Dequeue(); - //Console.WriteLine($"{toVisit.Count:000} {closestNodeToGoal.directDistanceToGoal:#.00} current:{closestNodeToGoal}"); - - foreach (OsmEdge edge in closestNodeToGoal.edges) - { - OsmNode? neighbor = regionManager.GetNode(edge.neighborId, edge.neighborRegion); - if (neighbor is not null) - { - double newPotentialWeight = - closestNodeToGoal.currentPathWeight + EdgeWeight(closestNodeToGoal, neighbor, edge.wayId, vehicle, ref regionManager); - if (newPotentialWeight < neighbor.currentPathWeight) - { - neighbor.previousPathNode = closestNodeToGoal; - neighbor.currentPathWeight = newPotentialWeight; - neighbor.currentPathLength = closestNodeToGoal.currentPathLength + Utils.DistanceBetween(closestNodeToGoal, neighbor); - neighbor.directDistanceToGoal = Utils.DistanceBetween(neighbor, goalNode); - - if (neighbor.Equals(goalNode) || closestNodeToGoal.directDistanceToGoal < 10) - { - stop = true; - goalNode = neighbor; - } - else if(!stop) - { - toVisit.Enqueue(neighbor, neighbor.directDistanceToGoal); - } - } - } - } - } - - List path = new(); - OsmNode? currentNode = goalNode; - while (currentNode is not null) - { - path.Add(PathNode.FromOsmNode(currentNode)!); - currentNode = currentNode.previousPathNode; - } - path.Reverse(); - - calcTime = DateTime.Now - startTime; - return new ValueTuple>(calcTime, path); + ValueTuple retTuple = new(); + retTuple.Item1 = ClosestNodeToCoordinates(startCoordinates, Tag.SpeedType.road, regionManager); + retTuple.Item2 = ClosestNodeToCoordinates(goalCoordinates, Tag.SpeedType.road, regionManager); + 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; } - - public static OsmNode? ClosestNodeToCoordinates(Coordinates coordinates, Tag.SpeedType vehicle, ref RegionManager regionManager) + + public static OsmNode? ClosestNodeToCoordinates(Coordinates coordinates, Tag.SpeedType vehicle, RegionManager regionManager) { OsmNode? closest = null; double distance = double.MaxValue; @@ -84,7 +32,7 @@ public static class Pathfinder bool hasConnectionUsingVehicle = false; foreach (OsmEdge edge in node.edges) { - double speed = GetSpeed(node, edge.wayId, vehicle, ref regionManager); + double speed = GetSpeedForEdge(node, edge.wayId, vehicle, regionManager); if (speed != 0) hasConnectionUsingVehicle = true; } @@ -99,7 +47,7 @@ public static class Pathfinder return closest; } - private static double GetSpeed(OsmNode node1, ulong wayId, Tag.SpeedType vehicle, ref RegionManager regionManager) + private static double GetSpeedForEdge(OsmNode node1, ulong wayId, Tag.SpeedType vehicle, RegionManager regionManager) { TagManager tags = regionManager.GetRegion(node1.coordinates)!.tagManager; Tag.WayType wayType = (Tag.WayType)tags.GetTag(wayId, Tag.TagType.highway)!; @@ -127,7 +75,7 @@ public static class Pathfinder 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); + double speed = GetSpeedForEdge(node1, wayId, vehicle, regionManager); if (speed is not 0) return distance / speed; return double.PositiveInfinity; diff --git a/Pathfinding/Pathfinder_Distance.cs b/Pathfinding/Pathfinder_Distance.cs new file mode 100644 index 0000000..462da26 --- /dev/null +++ b/Pathfinding/Pathfinder_Distance.cs @@ -0,0 +1,64 @@ +using OSMDatastructure; +using OSMDatastructure.Graph; +using OSMImporter; + +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); + 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 closestNodeToGoal = toVisit.Dequeue(); + + foreach (OsmEdge edge in closestNodeToGoal.edges) + { + OsmNode? neighbor = regionManager.GetNode(edge.neighborId, edge.neighborRegion); + if (neighbor is not null) + { + double newPotentialWeight = + closestNodeToGoal.currentPathWeight + Utils.DistanceBetween(closestNodeToGoal, neighbor); + if (newPotentialWeight < neighbor.currentPathWeight) + { + neighbor.previousPathNode = closestNodeToGoal; + neighbor.currentPathWeight = newPotentialWeight; //for other types of routing + neighbor.currentPathLength = newPotentialWeight; + neighbor.directDistanceToGoal = Utils.DistanceBetween(neighbor, goalNode); + + if (neighbor.Equals(goalNode)) + { + stop = true; + } + else if(!stop) + { + toVisit.Enqueue(neighbor, neighbor.directDistanceToGoal); + } + } + } + } + } + + List path = new(); + OsmNode? currentNode = goalNode; + while (currentNode is not null) + { + path.Add(PathNode.FromOsmNode(currentNode)!); + currentNode = currentNode.previousPathNode; + } + path.Reverse(); + + return path; + } +} \ No newline at end of file