From f7d9e511d7842dec16a77f594d5d32a33be1d7a4 Mon Sep 17 00:00:00 2001 From: C9Glax <13404778+C9Glax@users.noreply.github.com> Date: Fri, 3 Feb 2023 23:35:22 +0100 Subject: [PATCH] Created Pathfinding --- OSMServer.sln | 6 ++++ Pathfinding/Graph.cs | 12 +++++++ Pathfinding/Pathfinder.cs | 41 +++++++++++++++++++++ Pathfinding/Pathfinding.csproj | 13 +++++++ Pathfinding/RegionManager.cs | 49 +++++++++++++++++++++++++ Pathfinding/Utils.cs | 65 ++++++++++++++++++++++++++++++++++ 6 files changed, 186 insertions(+) create mode 100644 Pathfinding/Graph.cs create mode 100644 Pathfinding/Pathfinder.cs create mode 100644 Pathfinding/Pathfinding.csproj create mode 100644 Pathfinding/RegionManager.cs create mode 100644 Pathfinding/Utils.cs diff --git a/OSMServer.sln b/OSMServer.sln index e87fc72..dd57ab1 100644 --- a/OSMServer.sln +++ b/OSMServer.sln @@ -6,6 +6,8 @@ Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Server", "Server\Server.csp EndProject Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "OSMDatastructure", "OSMDatastructure\OSMDatastructure.csproj", "{A2489EF1-64E9-41C9-A295-B3D551D810DA}" EndProject +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Pathfinding", "Pathfinding\Pathfinding.csproj", "{D4AA1C47-98D4-415C-B026-3BC25B6B6F9D}" +EndProject Global GlobalSection(SolutionConfigurationPlatforms) = preSolution Debug|Any CPU = Debug|Any CPU @@ -24,5 +26,9 @@ Global {A2489EF1-64E9-41C9-A295-B3D551D810DA}.Debug|Any CPU.Build.0 = Debug|Any CPU {A2489EF1-64E9-41C9-A295-B3D551D810DA}.Release|Any CPU.ActiveCfg = Release|Any CPU {A2489EF1-64E9-41C9-A295-B3D551D810DA}.Release|Any CPU.Build.0 = Release|Any CPU + {D4AA1C47-98D4-415C-B026-3BC25B6B6F9D}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {D4AA1C47-98D4-415C-B026-3BC25B6B6F9D}.Debug|Any CPU.Build.0 = Debug|Any CPU + {D4AA1C47-98D4-415C-B026-3BC25B6B6F9D}.Release|Any CPU.ActiveCfg = Release|Any CPU + {D4AA1C47-98D4-415C-B026-3BC25B6B6F9D}.Release|Any CPU.Build.0 = Release|Any CPU EndGlobalSection EndGlobal diff --git a/Pathfinding/Graph.cs b/Pathfinding/Graph.cs new file mode 100644 index 0000000..f8a3901 --- /dev/null +++ b/Pathfinding/Graph.cs @@ -0,0 +1,12 @@ +namespace Pathfinding; + +public class Graph +{ + public class Node : OSMDatastructure.Node + { + public Node(float lat, float lon) : base(lat, lon) + { + + } + } +} \ No newline at end of file diff --git a/Pathfinding/Pathfinder.cs b/Pathfinding/Pathfinder.cs new file mode 100644 index 0000000..f46a594 --- /dev/null +++ b/Pathfinding/Pathfinder.cs @@ -0,0 +1,41 @@ +using OSMDatastructure; +using OSMImporter; + +namespace Pathfinding; + +public class Pathfinder +{ + + private static void CustomAStar(string workingDir, Coordinates start, Coordinates goal) + { + RegionManager regionManager = new RegionManager(workingDir); + Region startRegion = regionManager.GetRegion(start)!; //TODO null handling + Node startNode = ClosestNodeToCoordinates(start, startRegion)!; //TODO null handling + Region goalRegion = regionManager.GetRegion(goal)!; //TODO null handling + Node goalNode = ClosestNodeToCoordinates(goal, goalRegion)!; //TODO null handling + + } + + private static Node? ClosestNodeToCoordinates(Coordinates coordinates, Region region) + { + ulong? closestId = ClosestNodeIdToCoordinates(coordinates, region); + return closestId != null ? region.GetNode((ulong)closestId) : null; + } + + private static ulong? ClosestNodeIdToCoordinates(Coordinates coordinates, Region region) + { + ulong? closestId = null; + double closestDistance = double.MaxValue, distance; + + foreach (KeyValuePair kv in region.GetNodes()) + { + distance = Utils.DistanceBetween(kv.Value, coordinates); + if (distance < closestDistance) + { + closestDistance = distance; + closestId = kv.Key; + } + } + return closestId; + } +} \ No newline at end of file diff --git a/Pathfinding/Pathfinding.csproj b/Pathfinding/Pathfinding.csproj new file mode 100644 index 0000000..32b5e92 --- /dev/null +++ b/Pathfinding/Pathfinding.csproj @@ -0,0 +1,13 @@ + + + + net7.0 + enable + enable + + + + + + + diff --git a/Pathfinding/RegionManager.cs b/Pathfinding/RegionManager.cs new file mode 100644 index 0000000..ba76f80 --- /dev/null +++ b/Pathfinding/RegionManager.cs @@ -0,0 +1,49 @@ +using OSMDatastructure; + +namespace OSMImporter +{ + public class RegionManager + { + private string workingDirectory { get; } + private readonly Dictionary _regions = new(); + + public RegionManager(string workingDirectory) + { + this.workingDirectory = workingDirectory; + } + + public Region? GetRegion(Coordinates coordinates) + { + if(this._regions.ContainsKey(coordinates.GetRegionHash())) + return this._regions[coordinates.GetRegionHash()]; + else + { + Region? loadedRegion = LoadRegion(coordinates); + if(loadedRegion != null) + this._regions.Add(loadedRegion.regionHash, value: loadedRegion); + return loadedRegion; + } + } + + public Region[] GetAllRegions() + { + return this._regions.Values.ToArray(); + } + + private Region? LoadRegion(Coordinates coordinates) + { + string fullPath = Path.Combine(workingDirectory, coordinates.GetRegionHash().ToString()); + Console.WriteLine(fullPath); + if (!File.Exists(fullPath)) + return null; + + FileStream fileStream = new FileStream(fullPath, FileMode.Open); + + byte[] regionBytes = new byte[fileStream.Length]; + fileStream.Read(regionBytes, 0, regionBytes.Length); + fileStream.Close(); + + return ByteConverter.ToRegion(regionBytes); + } + } +} \ No newline at end of file diff --git a/Pathfinding/Utils.cs b/Pathfinding/Utils.cs new file mode 100644 index 0000000..226e871 --- /dev/null +++ b/Pathfinding/Utils.cs @@ -0,0 +1,65 @@ +using OSMDatastructure; + +namespace Pathfinding +{ + public struct Utils + { + public static double DistanceBetween(Coordinates c1, Coordinates n2) + { + return DistanceBetween(c1.lat, c1.lon, n2.lat, n2.lon); + } + + public static double DistanceBetween(Coordinates c1, float lat2, float lon2) + { + return DistanceBetween(c1.lat, c1.lon, lat2, lon2); + } + + public static double DistanceBetween(float lat1, float lon1, Coordinates c2) + { + return DistanceBetween(c2, lat1, lon1); + } + + public static double DistanceBetween(float lat1, float lon1, float lat2, float lon2) //Law of Cosines + { + /* + * From https://www.movable-type.co.uk/scripts/latlong.html + */ + double rlat1 = DegreesToRadians(lat1); + double rlat2 = DegreesToRadians(lat2); + double drlon = DegreesToRadians(lon2 - lon1); + const int R = 6371000; + + double d = Math.Acos(Math.Sin(rlat1) * Math.Sin(rlat2) + Math.Cos(rlat1) * Math.Cos(rlat2) * Math.Cos(drlon)) * R; + + return d; + } + + public static double DistanceBetweenHaversine(float lat1, float lon1, float lat2, float lon2) + { + /* + * From https://www.movable-type.co.uk/scripts/latlong.html + */ + const int R = 6371000; + double rlat1 = DegreesToRadians(lat1); + double rlat2 = DegreesToRadians(lat2); + double rdlat = DegreesToRadians(lat2 - lat1); + double drlon = DegreesToRadians(lon2 - lon1); + double a = Math.Sin(rdlat / 2) * Math.Sin(rdlat / 2) + Math.Cos(rlat1) * Math.Cos(rlat2) * Math.Sin(drlon / 2) * Math.Sin(drlon / 2); + double c = 2 * Math.Atan2(Math.Sqrt(a), Math.Sqrt(1 - a)); + double d = R * c; + return d; + } + + private static double DegreesToRadians(double deg) + { + return deg * Math.PI / 180.0; + } + + private static double RadiansToDegrees(double rad) + { + return rad * 180.0 / Math.PI; + } + + + } +} \ No newline at end of file