using System.Text.Json; using OSMDatastructure; using OSMDatastructure.Graph; using static OSMDatastructure.Tag; namespace Pathfinding; public class Pathfinder { public RegionManager regionManager; public PathResult? pathResult; public Dictionary? gScore; private Dictionary? _cameFromDict; private SpeedType _speedType; private double roadPriorityFactor, junctionFactor, sameRoadFactor, nodeAngleFactor, turnAngle; public Pathfinder(string workingDirectory, double roadPriorityFactor, double junctionFactor, double sameRoadFactor, double nodeAngleFactor, double turnAngle) { if (!Path.Exists(workingDirectory)) throw new DirectoryNotFoundException(workingDirectory); regionManager = new(workingDirectory); this.roadPriorityFactor = roadPriorityFactor; this.junctionFactor = junctionFactor; this.sameRoadFactor = sameRoadFactor; this.nodeAngleFactor = nodeAngleFactor; this.turnAngle = turnAngle; } public Pathfinder(RegionManager regionManager, double roadPriorityFactor, double junctionFactor, double sameRoadFactor, double nodeAngleFactor, double turnAngle) { this.regionManager = regionManager; this.roadPriorityFactor = roadPriorityFactor; this.junctionFactor = junctionFactor; this.sameRoadFactor = sameRoadFactor; this.nodeAngleFactor = nodeAngleFactor; this.turnAngle = turnAngle; } public Pathfinder AStar(Coordinates startCoordinates, Coordinates goalCoordinates, SpeedType vehicle) { DateTime startCalc = DateTime.Now; _speedType = vehicle; OsmNode? startNode = regionManager.ClosestNodeToCoordinates(startCoordinates, _speedType); OsmNode? goalNode = regionManager.ClosestNodeToCoordinates(goalCoordinates, _speedType); if (startNode is null || goalNode is null) { pathResult = new(DateTime.Now - startCalc, new List(),0 ,0); return this; } PriorityQueue openSetfScore = new(); openSetfScore.Enqueue(startNode, 0); gScore = new() { { startNode, 0 } }; _cameFromDict = new(); while (openSetfScore.Count > 0) { OsmNode currentNode = openSetfScore.Dequeue(); if (currentNode.Equals(goalNode)) { TimeSpan calcTime = DateTime.Now - startCalc; pathResult = GetPath(goalNode, calcTime); Console.WriteLine($"Path found. {calcTime} PathLength {pathResult.pathNodes.Count} VisitedNodes {gScore.Count} Distance {pathResult.distance} Duration {pathResult.weight}"); return this; } foreach (OsmEdge edge in currentNode.edges) { OsmNode? neighbor = regionManager.GetNode(edge.neighborId, edge.neighborRegion); if (neighbor is not null) { double tentativeGScore = gScore[currentNode] + Weight(currentNode, neighbor, edge); gScore.TryAdd(neighbor, double.MaxValue); if (tentativeGScore < gScore[neighbor]) { if(!_cameFromDict.TryAdd(neighbor, currentNode)) _cameFromDict[neighbor] = currentNode; gScore[neighbor] = tentativeGScore; double h = Heuristic(currentNode, neighbor, goalNode, edge); openSetfScore.Enqueue(neighbor, tentativeGScore + h); //Currently set includes a lot of duplicates } } } } pathResult = new(DateTime.Now - startCalc, new List(),0 ,0); return this; } private double Weight(OsmNode currentNode, OsmNode neighborNode, OsmEdge edge) { double distance = Utils.DistanceBetween(currentNode, neighborNode); double speed = regionManager.GetSpeedForEdge(currentNode, edge.wayId, _speedType); double angle = 1; if (_cameFromDict!.ContainsKey(currentNode)) { OsmNode previousNode = _cameFromDict[currentNode]; Vector v1 = new(currentNode, previousNode); Vector v2 = new(currentNode, neighborNode); double nodeAngle = v1.Angle(v2); if (nodeAngle < turnAngle) angle = 0; else angle = nodeAngle / 180; } double prio = regionManager.GetPriorityForVehicle(_speedType,edge, currentNode); return distance / (speed * angle + prio + 1); } private double Heuristic(OsmNode currentNode, OsmNode neighborNode, OsmNode goalNode, OsmEdge edge) { if (neighborNode.Equals(goalNode)) return 0; double priority = regionManager.GetPriorityForVehicle(_speedType, edge, currentNode); if (priority == 0) return double.MaxValue; double speed = regionManager.GetSpeedForEdge(currentNode, edge.wayId, _speedType); double angle = 0; if (_cameFromDict!.ContainsKey(currentNode)) { OsmNode previousNode = _cameFromDict[currentNode]; Vector v1 = new(currentNode, previousNode); Vector v2 = new(currentNode, neighborNode); double nodeAngle = v1.Angle(v2); if (nodeAngle < turnAngle) angle = 0; else angle = nodeAngle / 180; } TagManager curTags = regionManager.GetRegion(currentNode.coordinates)!.tagManager; TagManager nextTags = regionManager.GetRegion(neighborNode.coordinates)!.tagManager; bool sameName = false; string? curName = (string?)curTags.GetTag(edge.wayId, TagType.name); bool sameRef = false; string? curRef = (string?)curTags.GetTag(edge.wayId, TagType.tagref); if(curName is not null) foreach (OsmEdge pEdge in neighborNode.edges) { if ((string?)nextTags.GetTag(pEdge.wayId, TagType.name) == curName) sameName = true; if ((string?)nextTags.GetTag(pEdge.wayId, TagType.tagref) == curRef) sameRef = true; } double junctionCount = (neighborNode.edges.Count > 2 ? 0 : 1) * junctionFactor; double roadPriority = priority * roadPriorityFactor; double sameRoadName = (sameRef || sameName ? 1 : 0) * sameRoadFactor; return Utils.DistanceBetween(neighborNode, goalNode) / (speed * angle + 1); } public void SaveResult(string path) { FileStream fs = new (path, FileMode.CreateNew); JsonSerializer.Serialize(fs, pathResult, JsonSerializerOptions.Default); fs.Dispose(); Console.WriteLine($"Saved result to {path}"); } private PathResult GetPath(OsmNode goalNode, TimeSpan calcFinished) { List path = new(); OsmNode currentNode = goalNode; double retDistance = 0; double weight = 0; while (_cameFromDict!.ContainsKey(_cameFromDict[currentNode])) { OsmEdge? currentEdge = _cameFromDict[currentNode].edges.FirstOrDefault(edge => edge.neighborId == currentNode.nodeId); HashSet? tags = regionManager.GetRegion(currentNode.coordinates)!.tagManager.GetTagsForWayId(currentEdge!.wayId); PathNode? newNode = PathNode.FromOsmNode(currentNode, tags); if(newNode is not null) path.Add(newNode); double distance = Utils.DistanceBetween(currentNode, _cameFromDict[currentNode]); retDistance += distance; weight += regionManager.GetSpeedForEdge(_cameFromDict[currentNode], currentEdge.wayId, _speedType); currentNode = _cameFromDict[currentNode]; } path.Reverse(); return new PathResult(calcFinished, path, retDistance, retDistance / (weight / path.Count)); } private class Vector { public readonly float x, y; public Vector(float x, float y) { this.x = x; this.y = y; } public Vector(OsmNode n1, OsmNode n2) { this.x = n1.coordinates.longitude - n2.coordinates.longitude; this.y = n1.coordinates.latitude - n2.coordinates.latitude; } public double Angle(Vector v2) { return Angle(this, v2); } public static double Angle(Vector v1, Vector v2) { double dotProd = v1.x * v2.x + v1.y * v2.y; double v1L = Math.Sqrt(v1.x * v1.x + v1.y * v1.y); double v2L = Math.Sqrt(v2.x * v2.x + v2.y * v2.y); double ang = Math.Acos(dotProd / (v1L * v2L)); if (ang.Equals(double.NaN)) return 0; double angle = Utils.RadiansToDegrees(ang); return angle; } } }