From 01deb02666c7abe42ad5b7a1c18cdbcf25cef50c Mon Sep 17 00:00:00 2001 From: glax Date: Sat, 1 Apr 2023 14:43:05 +0200 Subject: [PATCH] Adjusted to new Region Format --- Pathfinding/Pathfinder.cs | 67 ++++++++++++++++++++++-------------- Pathfinding/RegionManager.cs | 53 +++++++++++++++------------- 2 files changed, 71 insertions(+), 49 deletions(-) diff --git a/Pathfinding/Pathfinder.cs b/Pathfinding/Pathfinder.cs index 76cb637..b2adcad 100644 --- a/Pathfinding/Pathfinder.cs +++ b/Pathfinding/Pathfinder.cs @@ -6,20 +6,13 @@ namespace Pathfinding; public class Pathfinder { - /* - public static List CustomAStar(string workingDir, Coordinates start, Coordinates goal, OsmWay.speedType vehicle) + public static List CustomAStar(string workingDir, Coordinates start, Coordinates goal, Tag.SpeedType vehicle) { RegionManager regionManager = new RegionManager(workingDir); - Region startRegion, goalRegion; - try - { - startRegion = regionManager.GetRegion(start); - goalRegion = regionManager.GetRegion(goal); - } - catch (FileNotFoundException e) - { - throw new Exception(string.Format("No region at coordinates {0}", e.FileName), e); - } + Region? startRegion = regionManager.GetRegion(start); + Region? goalRegion = regionManager.GetRegion(goal); + if (startRegion is null || goalRegion is null) + return new List(); OsmNode? startNode = ClosestNodeToCoordinates(start, startRegion); OsmNode? goalNode = ClosestNodeToCoordinates(goal, goalRegion); @@ -36,25 +29,20 @@ public class Pathfinder { //Console.WriteLine("toVisit-length: {0}", toVisit.Count); closestNodeToGoal = toVisit.First(); - foreach (OsmNode node in toVisit) + foreach (OsmNode node in toVisit.Where(node => node.directDistanceToGoal.Equals(Double.MaxValue))) { - if (node.directDistanceToGoal.Equals(double.MaxValue)) - { - node.directDistanceToGoal = Utils.DistanceBetween(node, goalNode); - } - if (node.directDistanceToGoal < closestNodeToGoal.directDistanceToGoal) - { - closestNodeToGoal = node; - } + node.directDistanceToGoal = Utils.DistanceBetween(node, goalNode); } - foreach (OsmWay edge in closestNodeToGoal.edges) + closestNodeToGoal = toVisit.OrderBy(node => node.directDistanceToGoal).First(); + + foreach (OsmEdge edge in closestNodeToGoal.edges) { - OsmNode? neighbor = regionManager.GetNode(edge.neighborCoordinates); - if (neighbor != null) + OsmNode? neighbor = regionManager.GetNode(edge.neighborId, edge.neighborRegion); + if (neighbor is not null) { double newPotentialWeight = - closestNodeToGoal.currentPathWeight + edge.GetWeight(closestNodeToGoal, vehicle); + closestNodeToGoal.currentPathWeight + EdgeWeight(closestNodeToGoal, neighbor, edge.wayId, vehicle, ref regionManager); if (neighbor.currentPathWeight > newPotentialWeight) { neighbor.previousPathNode = closestNodeToGoal; @@ -100,5 +88,32 @@ public class Pathfinder } return closest; - }*/ + } + + private static double EdgeWeight(OsmNode node1, OsmNode node2, 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; + 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]; + if(maxspeed is not 0) + return distance / Tag.defaultSpeedCar[wayType]; + else return Double.PositiveInfinity;; + default: + return double.PositiveInfinity; + } + } } \ No newline at end of file diff --git a/Pathfinding/RegionManager.cs b/Pathfinding/RegionManager.cs index 23f3aee..7af698b 100644 --- a/Pathfinding/RegionManager.cs +++ b/Pathfinding/RegionManager.cs @@ -12,21 +12,21 @@ namespace OSMImporter { this.workingDirectory = workingDirectory; } - - /// - /// Checks wether the Region is already loaded and returns the Region, or tries to load the Region from file in workingDirectory - /// - /// Coordinates of the Region (or within the Region) to load - /// The Region at the specified Coordinates containing Nodes and Connections - /// If the Regionfile can not be found. - public Region GetRegion(Coordinates coordinates) + + public Region? GetRegion(Coordinates coordinates) { - if(_regions.ContainsKey(Coordinates.GetRegionHashCode(coordinates))) - return _regions[Coordinates.GetRegionHashCode(coordinates)]; + return GetRegion(Coordinates.GetRegionHashCode(coordinates)); + } + + public Region? GetRegion(ulong id) + { + if(_regions.TryGetValue(id, out Region? value)) + return value; else { - Region loadedRegion = LoadRegion(coordinates); - _regions.Add(loadedRegion.regionHash, value: loadedRegion); + Region? loadedRegion = LoadRegion(id); + if(loadedRegion is not null) + _regions.Add(loadedRegion!.regionHash, value: loadedRegion); return loadedRegion; } } @@ -35,22 +35,29 @@ namespace OSMImporter { return this._regions.Values.ToArray(); } - - /// - /// - /// - /// Coordinates of the Region (or within the Region) to load - /// The Region at the specified Coordinates containing Nodes and Connections - /// If the Regionfile can not be found. - private Region LoadRegion(Coordinates coordinates) + + private Region? LoadRegion(Coordinates coordinates) { - throw new NotImplementedException(); + return LoadRegion(Coordinates.GetRegionHashCode(coordinates)); + } + + private Region? LoadRegion(ulong id) + { + return Region.FromId(workingDirectory, id); } public OsmNode? GetNode(Coordinates coordinates) { - Region regionWithNode = GetRegion(coordinates); - return regionWithNode.GetNode(coordinates); + Region? regionWithNode = GetRegion(coordinates); + if (regionWithNode is not null) + return regionWithNode.GetNode(coordinates); + else return null; + } + + public OsmNode? GetNode(ulong nodeId, ulong regionId) + { + Region? r = GetRegion(regionId); + return r?.GetNode(nodeId); } } } \ No newline at end of file