From f15171a9f195bfc2902e895f449a19bdff0d32c6 Mon Sep 17 00:00:00 2001 From: C9Glax <13404778+C9Glax@users.noreply.github.com> Date: Wed, 8 Feb 2023 19:05:13 +0100 Subject: [PATCH] Fixed AStar Weight-assignment OSMEdge.cs: renamed speedped, speedcar --- OSMDatastructure/OSMEdge.cs | 42 +++++++++++++++++++------------------ Pathfinding/Pathfinder.cs | 26 ++++++++++++++--------- 2 files changed, 38 insertions(+), 30 deletions(-) diff --git a/OSMDatastructure/OSMEdge.cs b/OSMDatastructure/OSMEdge.cs index 6a6377b..35ab170 100644 --- a/OSMDatastructure/OSMEdge.cs +++ b/OSMDatastructure/OSMEdge.cs @@ -44,7 +44,7 @@ public class OsmEdge highway, oneway, footway, sidewalk, cycleway, busway, forward, maxspeed, name, surface, lanes, access, tracktype, id, EMPTY } - public static Dictionary speedcar = new() { + private static readonly Dictionary defaultSpeedCar = new() { { wayType.NONE, 0 }, { wayType.motorway, 110 }, { wayType.trunk, 100 }, @@ -76,7 +76,7 @@ public class OsmEdge { wayType.construction, 0 } }; - public static Dictionary speedped = new() { + private static readonly Dictionary defaultSpeedPedestrian = new() { { wayType.NONE, 0 }, { wayType.motorway, 0 }, { wayType.trunk, 0 }, @@ -115,7 +115,7 @@ public class OsmEdge { KeyValuePair tag = ConvertToTag(key, value); if(tag.Key != tagType.EMPTY) - this.tags.Add(tag.Key, tag.Value); + tags.Add(tag.Key, tag.Value); } public static KeyValuePair ConvertToTag(string key, string value) @@ -165,32 +165,34 @@ public class OsmEdge public ulong GetId() { - return this.tags.ContainsKey(tagType.id) ? (ulong)this.tags[tagType.id] : 0; + return tags.ContainsKey(tagType.id) ? (ulong)tags[tagType.id] : 0; } public wayType GetHighwayType() - { - return this.tags.ContainsKey(tagType.highway) ? (wayType)this.tags[tagType.highway] : wayType.NONE; + { + if (!tags.ContainsKey(tagType.highway)) + throw new Exception("Not a road?"); + return (wayType)tags[tagType.highway]; } public bool IsOneWay() { - return this.tags.ContainsKey(tagType.oneway) && (bool)this.tags[tagType.oneway]; + return tags.ContainsKey(tagType.oneway) && (bool)tags[tagType.oneway]; } - public byte? GetMaxSpeed(speedType type) + public byte GetMaxSpeed(speedType type) { - if(type == speedType.road) + switch (type) { - return this.tags.ContainsKey(tagType.maxspeed) ? (byte)this.tags[tagType.maxspeed] : null; + case speedType.road: + return tags.ContainsKey(tagType.maxspeed) ? (byte)tags[tagType.maxspeed] : (byte)0; + case speedType.car: + return tags.ContainsKey(tagType.maxspeed) ? (byte)tags[tagType.maxspeed] : defaultSpeedCar[GetHighwayType()]; + case speedType.pedestrian: + return defaultSpeedPedestrian[GetHighwayType()]; + default: + return 0; } - if(type == speedType.car) - { - return this.tags.ContainsKey(tagType.maxspeed) - ? (byte)this.tags[tagType.maxspeed] - : speedcar[this.GetHighwayType()]; - } - return speedped[this.GetHighwayType()]; } public bool IsForward() @@ -201,11 +203,11 @@ public class OsmEdge public double GetWeight(OsmNode parentNode, speedType vehicle) { double distance = Utils.DistanceBetween(parentNode, neighborCoordinates); - byte? speedByte = GetMaxSpeed(vehicle); - if (speedByte != null && speedByte > 0) + byte speedByte = GetMaxSpeed(vehicle); + if (speedByte > 0) { double speed = Convert.ToDouble(speedByte); - return distance / (speed * 1000 / 360); + return distance / speed; } else { diff --git a/Pathfinding/Pathfinder.cs b/Pathfinding/Pathfinder.cs index 8703bfb..521c8f6 100644 --- a/Pathfinding/Pathfinder.cs +++ b/Pathfinding/Pathfinder.cs @@ -6,7 +6,7 @@ namespace Pathfinding; public class Pathfinder { - public static List CustomAStar(string workingDir, Coordinates start, Coordinates goal) + public static List CustomAStar(string workingDir, Coordinates start, Coordinates goal, OsmEdge.speedType vehicle) { RegionManager regionManager = new RegionManager(workingDir); Region startRegion, goalRegion; @@ -32,6 +32,7 @@ public class Pathfinder while (toVisit.Count > 0 && !stop) { + Console.WriteLine("toVisit-length: {0}", toVisit.Count); closestNodeToGoal = toVisit.First(); foreach (OsmNode node in toVisit) { @@ -48,16 +49,20 @@ public class Pathfinder foreach (OsmEdge edge in closestNodeToGoal.edges) { OsmNode? neighbor = regionManager.GetNode(edge.neighborCoordinates); - if (neighbor != null && neighbor.currentPathWeight > closestNodeToGoal.currentPathWeight + Utils.DistanceBetween(closestNodeToGoal, neighbor)) + if (neighbor != null) { - neighbor.previousPathNode = closestNodeToGoal; - neighbor.currentPathWeight = closestNodeToGoal.currentPathWeight + - Utils.DistanceBetween(closestNodeToGoal, neighbor); + double newPotentialWeight = + closestNodeToGoal.currentPathWeight + edge.GetWeight(closestNodeToGoal, vehicle); + if (neighbor.currentPathWeight > newPotentialWeight) + { + neighbor.previousPathNode = closestNodeToGoal; + neighbor.currentPathWeight = newPotentialWeight; - if (neighbor.Equals(goalNode)) - stop = true; - else - toVisit.Add(neighbor); + if (neighbor.Equals(goalNode)) + stop = true; + else + toVisit.Add(neighbor); + } } } @@ -72,7 +77,8 @@ public class Pathfinder currentNode = currentNode.previousPathNode; } path.Add(startNode); - + path.Reverse(); + return path; }