From b6ca87ea7277b060cae2d9402a35549ffcd1b235 Mon Sep 17 00:00:00 2001 From: C9Glax <13404778+C9Glax@users.noreply.github.com> Date: Sun, 5 Feb 2023 20:03:47 +0100 Subject: [PATCH] Added first Pathfinding --- Pathfinding/PathNode.cs | 16 ++------- Pathfinding/Pathfinder.cs | 69 +++++++++++++++++++++++++++++++++++---- Server/Server.csproj | 1 + 3 files changed, 67 insertions(+), 19 deletions(-) diff --git a/Pathfinding/PathNode.cs b/Pathfinding/PathNode.cs index 22a4791..8899125 100644 --- a/Pathfinding/PathNode.cs +++ b/Pathfinding/PathNode.cs @@ -4,9 +4,9 @@ namespace Pathfinding; public class PathNode : OSMDatastructure.Node { - PathNode? previousNode = null; - private Dictionary distanceDictionary = new(); - double currentWeight = double.MaxValue; + public PathNode? previousPathNode = null; + public double currentPathWeight = double.MaxValue; + public double DirectDistanceToGoal = double.MaxValue; public PathNode(float lat, float lon) : base(lat, lon) @@ -14,14 +14,4 @@ public class PathNode : OSMDatastructure.Node } - public double? DistanceTo(Connection connection) - { - if (!this.GetConnections().Contains(connection)) - return null; //TODO exception is not actually connected - if (!this.distanceDictionary.ContainsKey(connection)) - this.distanceDictionary.Add(connection, Utils.DistanceBetween(this, connection.endNodeCoordinates)); - return this.distanceDictionary[connection]; - } - - } \ No newline at end of file diff --git a/Pathfinding/Pathfinder.cs b/Pathfinding/Pathfinder.cs index 77d8ced..d074571 100644 --- a/Pathfinding/Pathfinder.cs +++ b/Pathfinding/Pathfinder.cs @@ -6,15 +6,72 @@ namespace Pathfinding; public class Pathfinder { - private static void CustomAStar(string workingDir, Coordinates start, Coordinates goal) + public static List CustomAStar(string workingDir, Coordinates start, Coordinates goal) { RegionManager regionManager = new RegionManager(workingDir); - Region startRegion = regionManager.GetRegion(start)!; //TODO null handling - PathNode startNode = (PathNode)ClosestNodeToCoordinates(start, startRegion)!; //TODO null handling - Region goalRegion = regionManager.GetRegion(goal)!; //TODO null handling - PathNode goalNode = (PathNode)ClosestNodeToCoordinates(goal, goalRegion)!; //TODO null handling - + Region startRegion, goalRegion; + PathNode startNode, goalNode; + try + { + startRegion = regionManager.GetRegion(start); + startNode = (PathNode)ClosestNodeToCoordinates(start, startRegion)!; //TODO null handling + } + catch (FileNotFoundException e) + { + throw new Exception(string.Format("No region at coordinates {0}", start), e); + } + try + { + goalRegion = regionManager.GetRegion(goal); + goalNode = (PathNode)ClosestNodeToCoordinates(goal, goalRegion)!; //TODO null handling + } + catch (FileNotFoundException e) + { + throw new Exception(string.Format("No region at coordinates {0}", start), e); + } + List toVisit = new() { startNode }; + startNode.DirectDistanceToGoal = Utils.DistanceBetween(startNode, goalNode); + bool stop = false; + + while (toVisit.Count > 0 && !stop) + { + PathNode closestNodeToGoal = toVisit.First(); + foreach (PathNode node in toVisit) + { + if (node.DirectDistanceToGoal < closestNodeToGoal.DirectDistanceToGoal) + { + closestNodeToGoal = node; + } + } + + foreach (Connection connection in closestNodeToGoal.GetConnections()) + { + PathNode? neighbor = (PathNode?)regionManager.GetNode(connection.endNodeCoordinates); + if (neighbor != null && neighbor.currentPathWeight < closestNodeToGoal.currentPathWeight + Utils.DistanceBetween(closestNodeToGoal, neighbor)) + { + neighbor.previousPathNode = closestNodeToGoal; + neighbor.currentPathWeight = closestNodeToGoal.currentPathWeight + + Utils.DistanceBetween(closestNodeToGoal, neighbor); + + if (neighbor.Equals(goalNode)) + stop = true; + else + toVisit.Add(neighbor); + } + } + } + + List path = new(); + PathNode currentNode = goalNode; + while (!currentNode.Equals(startNode)) + { + path.Add(currentNode); + currentNode = currentNode.previousPathNode!; + } + path.Add(startNode); + + return path; } private static Node? ClosestNodeToCoordinates(Coordinates coordinates, Region region) diff --git a/Server/Server.csproj b/Server/Server.csproj index 35cc033..5843f3a 100644 --- a/Server/Server.csproj +++ b/Server/Server.csproj @@ -9,6 +9,7 @@ +