From 0758ae1d8627f8b24b701dc3726fbea6b8327665 Mon Sep 17 00:00:00 2001 From: glax Date: Tue, 23 Jul 2024 16:42:03 +0200 Subject: [PATCH] Update Dependencies, Introduced directional search --- astar/Astar.cs | 32 ++++++++++++++++++++---------- astar/Graph.cs | 10 +++++----- astar/Node.cs | 2 +- astar/PathingHelper/SpeedHelper.cs | 20 +++++++------------ 4 files changed, 35 insertions(+), 29 deletions(-) diff --git a/astar/Astar.cs b/astar/Astar.cs index f44424c..fc55dbb 100644 --- a/astar/Astar.cs +++ b/astar/Astar.cs @@ -2,6 +2,7 @@ using Graph; using Microsoft.Extensions.Logging; using Graph.Utils; +using OSM_Graph.Enums; using OSM_Regions; namespace astar @@ -13,13 +14,13 @@ namespace astar { RegionLoader rl = new(regionSize, importFolderPath, logger: logger); Graph graph = Spiral(rl, startLat, startLon, regionSize); + Graph endRegion = Spiral(rl, endLat, endLon, regionSize); + graph.ConcatGraph(endRegion); KeyValuePair startNode = graph.ClosestNodeToCoordinates(startLat, startLon, car); startNode.Value.PreviousIsFromStart = true; startNode.Value.PreviousNodeId = startNode.Key; startNode.Value.Distance = 0f; - Graph endRegion = Spiral(rl, endLat, endLon, regionSize); - graph.ConcatGraph(endRegion); KeyValuePair endNode = graph.ClosestNodeToCoordinates(endLat, endLon, car); endNode.Value.PreviousIsFromStart = false; endNode.Value.PreviousNodeId = endNode.Key; @@ -41,20 +42,25 @@ namespace astar { ulong currentNodeStartId = toVisitStart.Dequeue(); Node currentNodeStart = graph.Nodes[currentNodeStartId]; - foreach ((ulong neighborId, ulong wayId) in currentNodeStart.Neighbors) + foreach ((ulong neighborId, KeyValuePair wayId) in currentNodeStart.Neighbors) { if (!graph.ContainsNode(neighborId)) graph.ConcatGraph(Graph.FromGraph(rl.LoadRegionFromNodeId(neighborId))); - if (!graph.ContainsWay(wayId)) + if (!graph.ContainsWay(wayId.Key)) { - foreach (global::Graph.Graph? g in rl.LoadRegionsFromWayId(wayId)) + foreach (global::Graph.Graph? g in rl.LoadRegionsFromWayId(wayId.Key)) graph.ConcatGraph(Graph.FromGraph(g)); } - Way way = graph.Ways[wayId]; + OSM_Graph.Way way = graph.Ways[wayId.Key]; byte speed = SpeedHelper.GetSpeed(way, car); if(speed < 1) continue; + if(wayId.Value && way.GetDirection() == WayDirection.Forwards && car) + continue; + if(!wayId.Value && way.GetDirection() == WayDirection.Backwards && car) + continue; + Node neighborNode = graph.Nodes[neighborId]; if (neighborNode.PreviousIsFromStart is false)//Check if we found the opposite End @@ -73,20 +79,26 @@ namespace astar ulong currentNodeEndId = toVisitEnd.Dequeue(); Node currentNodeEnd = graph.Nodes[currentNodeEndId]; - foreach ((ulong neighborId, ulong wayId) in currentNodeEnd.Neighbors) + foreach ((ulong neighborId, KeyValuePair wayId) in currentNodeEnd.Neighbors) { if (!graph.ContainsNode(neighborId)) graph.ConcatGraph(Graph.FromGraph(rl.LoadRegionFromNodeId(neighborId))); - if (!graph.ContainsWay(wayId)) + if (!graph.ContainsWay(wayId.Key)) { - foreach (global::Graph.Graph? g in rl.LoadRegionsFromWayId(wayId)) + foreach (global::Graph.Graph? g in rl.LoadRegionsFromWayId(wayId.Key)) graph.ConcatGraph(Graph.FromGraph(g)); } - Way way = graph.Ways[wayId]; + OSM_Graph.Way way = graph.Ways[wayId.Key]; byte speed = SpeedHelper.GetSpeed(way, car); if(speed < 1) continue; + + if(wayId.Value && way.GetDirection() == WayDirection.Backwards && car) + continue; + if(!wayId.Value && way.GetDirection() == WayDirection.Forwards && car) + continue; + Node neighborNode = graph.Nodes[neighborId]; if (neighborNode.PreviousIsFromStart is true)//Check if we found the opposite End diff --git a/astar/Graph.cs b/astar/Graph.cs index db7f03d..cf6f250 100644 --- a/astar/Graph.cs +++ b/astar/Graph.cs @@ -6,7 +6,7 @@ namespace astar; public class Graph { public readonly Dictionary Nodes = new(); - public readonly Dictionary Ways = new (); + public readonly Dictionary Ways = new (); public static Graph? FromGraph(global::Graph.Graph? graph) { @@ -16,7 +16,7 @@ public class Graph foreach ((ulong id, global::Graph.Node? node) in graph.Nodes) ret.Nodes.Add(id, Node.FromGraphNode(node)); foreach ((ulong id, Way? way) in graph.Ways) - ret.Ways.Add(id, way); + ret.Ways.Add(id, new OSM_Graph.Way(id, way.Tags, new())); return ret; } @@ -26,7 +26,7 @@ public class Graph return; foreach ((ulong id, Node n) in graph.Nodes) this.Nodes.TryAdd(id, n); - foreach ((ulong id, Way w) in graph.Ways) + foreach ((ulong id, OSM_Graph.Way w) in graph.Ways) this.Ways.TryAdd(id, w); } @@ -40,7 +40,7 @@ public class Graph return Nodes.ContainsKey(nodeId); } - public bool ContainsWay(Way way) + public bool ContainsWay(OSM_Graph.Way way) { return Ways.ContainsValue(way); } @@ -52,7 +52,7 @@ public class Graph public KeyValuePair ClosestNodeToCoordinates(float lat, float lon, bool car = true) { - return Nodes.Where(n => n.Value.Neighbors.Values.Any(wayId => SpeedHelper.GetSpeed(Ways[wayId], car) > 0)).MinBy(n => n.Value.DistanceTo(lat, lon)); + return Nodes.Where(n => n.Value.Neighbors.Values.Any(way => SpeedHelper.GetSpeed(Ways[way.Key], car) > 0)).MinBy(n => n.Value.DistanceTo(lat, lon)); } public override string ToString() diff --git a/astar/Node.cs b/astar/Node.cs index eee8ac0..7eec361 100644 --- a/astar/Node.cs +++ b/astar/Node.cs @@ -1,6 +1,6 @@ namespace astar; -public class Node(float lat, float lon, Dictionary? neighbors = null) : global::Graph.Node(lat, lon, neighbors) +public class Node(float lat, float lon, Dictionary>? neighbors = null) : global::Graph.Node(lat, lon, neighbors) { public ulong? PreviousNodeId = null; public float? Distance = null; diff --git a/astar/PathingHelper/SpeedHelper.cs b/astar/PathingHelper/SpeedHelper.cs index 7439d48..5f9a3c9 100644 --- a/astar/PathingHelper/SpeedHelper.cs +++ b/astar/PathingHelper/SpeedHelper.cs @@ -1,23 +1,17 @@ using Graph; +using OSM_Graph.Enums; namespace astar.PathingHelper; internal static class SpeedHelper { - public static byte GetSpeed(Way way, bool car = true) + public static byte GetSpeed(OSM_Graph.Way way, bool car = true) { - if (!way.Tags.TryGetValue("highway", out string? highwayTypeStr)) - return 0; - if (!Enum.TryParse(highwayTypeStr, out HighwayType highwayType)) - return 0; - byte speed = car ? SpeedCar[highwayType] : SpeedPedestrian[highwayType]; - if (speed < 1) - return speed; - if(!way.Tags.TryGetValue("maxspeed", out string? maxSpeedStr)) - return speed; - if (!byte.TryParse(maxSpeedStr, out speed)) - return speed; - return speed; + byte maxspeed = way.GetMaxSpeed(); + if (maxspeed != 0) + return maxspeed; + HighwayType highwayType = way.GetHighwayType(); + return car ? SpeedCar[highwayType] : SpeedPedestrian[highwayType]; } public static byte GetMaxSpeed(bool car = true)