diff --git a/Executable/Program.cs b/Executable/Program.cs index 512e86e..b77f16e 100644 --- a/Executable/Program.cs +++ b/Executable/Program.cs @@ -61,7 +61,7 @@ if (arguments.TryGetValue(pathArg, out string[]? pathValue)) converter.SplitOsmExportIntoRegionFiles(pathValue[0]); } -Route route = Astar.FindPath(startLat, startLon, endLat, endLon, regionSize, importPath, logger); +Route route = Astar.FindPath(startLat, startLon, endLat, endLon, regionSize, true, importPath, logger); if(route.RouteFound) Console.WriteLine(route); else diff --git a/astar/.idea/.idea.astar/.idea/inspectionProfiles/Project_Default.xml b/astar/.idea/.idea.astar/.idea/inspectionProfiles/Project_Default.xml new file mode 100644 index 0000000..a76871f --- /dev/null +++ b/astar/.idea/.idea.astar/.idea/inspectionProfiles/Project_Default.xml @@ -0,0 +1,10 @@ + + + + \ No newline at end of file diff --git a/astar/Astar.cs b/astar/Astar.cs index ceb65a6..78a87f1 100644 --- a/astar/Astar.cs +++ b/astar/Astar.cs @@ -1,4 +1,6 @@ -using Microsoft.Extensions.Logging; +using astar.PathingHelper; +using Graph; +using Microsoft.Extensions.Logging; using Graph.Utils; using OSM_Regions; @@ -6,19 +8,19 @@ namespace astar { public static class Astar { - public static Route FindPath(float startLat, float startLon, float endLat, float endLon, float regionSize, string? importFolderPath = null, + public static Route FindPath(float startLat, float startLon, float endLat, float endLon, float regionSize, bool car = true, string? importFolderPath = null, ILogger? logger = null) { RegionLoader rl = new(regionSize, importFolderPath, logger: logger); Graph graph = Spiral(rl, startLat, startLon, regionSize); - KeyValuePair startNode = graph.ClosestNodeToCoordinates(startLat, startLon); + 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); + KeyValuePair endNode = graph.ClosestNodeToCoordinates(endLat, endLon, car); endNode.Value.PreviousIsFromStart = false; endNode.Value.PreviousNodeId = endNode.Key; endNode.Value.Distance = 0f; @@ -42,6 +44,16 @@ namespace astar { if (!graph.ContainsNode(neighborId)) graph.ConcatGraph(Graph.FromGraph(rl.LoadRegionFromNodeId(neighborId))); + if (!graph.ContainsWay(wayId)) + { + foreach (global::Graph.Graph? g in rl.LoadRegionsFromWayId(wayId)) + graph.ConcatGraph(Graph.FromGraph(g)); + } + + Way way = graph.Ways[wayId]; + byte speed = SpeedHelper.GetSpeed(way, car); + if(speed < 1) + continue; Node neighborNode = graph.Nodes[neighborId]; if (neighborNode.PreviousIsFromStart is false)//Check if we found the opposite End @@ -53,7 +65,7 @@ namespace astar neighborNode.PreviousNodeId = currentNodeStartId; neighborNode.Distance = distance; neighborNode.PreviousIsFromStart = true; - toVisitStart.Enqueue(neighborId, NodeUtils.DistanceBetween(neighborNode, endNode.Value)); + toVisitStart.Enqueue(neighborId, NodeUtils.DistanceBetween(neighborNode, endNode.Value) / speed); } logger?.LogTrace($"Neighbor {neighborId} {neighborNode}"); } @@ -64,6 +76,16 @@ namespace astar { if (!graph.ContainsNode(neighborId)) graph.ConcatGraph(Graph.FromGraph(rl.LoadRegionFromNodeId(neighborId))); + if (!graph.ContainsWay(wayId)) + { + foreach (global::Graph.Graph? g in rl.LoadRegionsFromWayId(wayId)) + graph.ConcatGraph(Graph.FromGraph(g)); + } + + Way way = graph.Ways[wayId]; + byte speed = SpeedHelper.GetSpeed(way, car); + if(speed < 1) + continue; Node neighborNode = graph.Nodes[neighborId]; if (neighborNode.PreviousIsFromStart is true)//Check if we found the opposite End @@ -75,7 +97,7 @@ namespace astar neighborNode.PreviousNodeId = currentNodeEndId; neighborNode.Distance = distance; neighborNode.PreviousIsFromStart = false; - toVisitEnd.Enqueue(neighborId, NodeUtils.DistanceBetween(neighborNode, startNode.Value)); + toVisitEnd.Enqueue(neighborId, NodeUtils.DistanceBetween(neighborNode, startNode.Value) / speed); } logger?.LogTrace($"Neighbor {neighborId} {neighborNode}"); } diff --git a/astar/Graph.cs b/astar/Graph.cs index b3d7c2c..db7f03d 100644 --- a/astar/Graph.cs +++ b/astar/Graph.cs @@ -1,4 +1,5 @@ -using Graph; +using astar.PathingHelper; +using Graph; namespace astar; @@ -49,9 +50,9 @@ public class Graph return Ways.ContainsKey(wayId); } - public KeyValuePair ClosestNodeToCoordinates(float lat, float lon) + public KeyValuePair ClosestNodeToCoordinates(float lat, float lon, bool car = true) { - return Nodes.MinBy(n => n.Value.DistanceTo(lat, lon)); + return Nodes.Where(n => n.Value.Neighbors.Values.Any(wayId => SpeedHelper.GetSpeed(Ways[wayId], car) > 0)).MinBy(n => n.Value.DistanceTo(lat, lon)); } public override string ToString() diff --git a/astar/PathingHelper/HighwayType.cs b/astar/PathingHelper/HighwayType.cs new file mode 100644 index 0000000..8bb77c3 --- /dev/null +++ b/astar/PathingHelper/HighwayType.cs @@ -0,0 +1,36 @@ +// ReSharper disable InconsistentNaming +// ReSharper disable UnusedMember.Global +namespace astar.PathingHelper; + +public enum HighwayType +{ + NONE, + motorway, + trunk, + primary, + secondary, + tertiary, + unclassified, + residential, + motorway_link, + trunk_link, + primary_link, + secondary_link, + tertiary_link, + living_street, + service, + pedestrian, + track, + bus_guideway, + escape, + raceway, + road, + busway, + footway, + bridleway, + steps, + corridor, + path, + cycleway, + construction +} \ No newline at end of file diff --git a/astar/PathingHelper/SpeedHelper.cs b/astar/PathingHelper/SpeedHelper.cs new file mode 100644 index 0000000..33e24bc --- /dev/null +++ b/astar/PathingHelper/SpeedHelper.cs @@ -0,0 +1,79 @@ +using Graph; + +namespace astar.PathingHelper; + +internal static class SpeedHelper +{ + public static byte GetSpeed(Way way, bool car = true) + { + if (!way.Tags.TryGetValue("highway", out string? highwayTypeStr)) + return 0; + if (!Enum.TryParse(highwayTypeStr, out HighwayType highwayType)) + return 0; + return car ? SpeedCar[highwayType] : SpeedPedestrian[highwayType]; + } + + private static Dictionary SpeedPedestrian = new() { + { HighwayType.NONE, 0 }, + { HighwayType.motorway, 0 }, + { HighwayType.trunk, 0 }, + { HighwayType.primary, 0 }, + { HighwayType.secondary, 0 }, + { HighwayType.tertiary, 0 }, + { HighwayType.unclassified, 1 }, + { HighwayType.residential, 3 }, + { HighwayType.motorway_link, 0 }, + { HighwayType.trunk_link, 0 }, + { HighwayType.primary_link, 0 }, + { HighwayType.secondary_link, 0 }, + { HighwayType.tertiary_link, 0 }, + { HighwayType.living_street, 5 }, + { HighwayType.service, 2 }, + { HighwayType.pedestrian, 5 }, + { HighwayType.track, 0 }, + { HighwayType.bus_guideway, 0 }, + { HighwayType.escape, 0 }, + { HighwayType.raceway, 0 }, + { HighwayType.road, 3 }, + { HighwayType.busway, 0 }, + { HighwayType.footway, 4 }, + { HighwayType.bridleway, 1 }, + { HighwayType.steps, 2 }, + { HighwayType.corridor, 3 }, + { HighwayType.path, 4 }, + { HighwayType.cycleway, 2 }, + { HighwayType.construction, 0 } + }; + + private static Dictionary SpeedCar = new() { + { HighwayType.NONE, 0 }, + { HighwayType.motorway, 110 }, + { HighwayType.trunk, 100 }, + { HighwayType.primary, 80 }, + { HighwayType.secondary, 80 }, + { HighwayType.tertiary, 70 }, + { HighwayType.unclassified, 20 }, + { HighwayType.residential, 10 }, + { HighwayType.motorway_link, 50 }, + { HighwayType.trunk_link, 50 }, + { HighwayType.primary_link, 30 }, + { HighwayType.secondary_link, 25 }, + { HighwayType.tertiary_link, 25 }, + { HighwayType.living_street, 10 }, + { HighwayType.service, 0 }, + { HighwayType.pedestrian, 0 }, + { HighwayType.track, 0 }, + { HighwayType.bus_guideway, 0 }, + { HighwayType.escape, 0 }, + { HighwayType.raceway, 0 }, + { HighwayType.road, 25 }, + { HighwayType.busway, 0 }, + { HighwayType.footway, 0 }, + { HighwayType.bridleway, 0 }, + { HighwayType.steps, 0 }, + { HighwayType.corridor, 0 }, + { HighwayType.path, 0 }, + { HighwayType.cycleway, 0 }, + { HighwayType.construction, 0 } + }; +} \ No newline at end of file