From b89a3715a1251b3214b48488148e76156838bcaa Mon Sep 17 00:00:00 2001 From: glax Date: Sun, 23 Apr 2023 14:47:26 +0200 Subject: [PATCH] Created variable for maximum turn angle. --- Pathfinding/Pathfinder.cs | 25 +++++++++++++++---------- 1 file changed, 15 insertions(+), 10 deletions(-) diff --git a/Pathfinding/Pathfinder.cs b/Pathfinding/Pathfinder.cs index b0ad52e..65e94fc 100644 --- a/Pathfinding/Pathfinder.cs +++ b/Pathfinding/Pathfinder.cs @@ -13,9 +13,9 @@ public class Pathfinder public Dictionary? gScore; private Dictionary? _cameFromDict; private SpeedType _speedType; - private double roadPriorityFactor, junctionFactor, sameRoadFactor, nodeAngleFactor; + private double roadPriorityFactor, junctionFactor, sameRoadFactor, nodeAngleFactor, turnAngle; - public Pathfinder(string workingDirectory, double roadPriorityFactor, double junctionFactor, double sameRoadFactor, double nodeAngleFactor) + public Pathfinder(string workingDirectory, double roadPriorityFactor, double junctionFactor, double sameRoadFactor, double nodeAngleFactor, double turnAngle) { if (!Path.Exists(workingDirectory)) throw new DirectoryNotFoundException(workingDirectory); @@ -24,15 +24,17 @@ public class Pathfinder this.junctionFactor = junctionFactor; this.sameRoadFactor = sameRoadFactor; this.nodeAngleFactor = nodeAngleFactor; + this.turnAngle = turnAngle; } - public Pathfinder(RegionManager regionManager, double roadPriorityFactor, double junctionFactor, double sameRoadFactor, double nodeAngleFactor) + 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) @@ -91,18 +93,21 @@ public class Pathfinder double distance = Utils.DistanceBetween(currentNode, neighborNode); double speed = regionManager.GetSpeedForEdge(currentNode, edge.wayId, _speedType); - double nodeAngle = 1; + double angle = 1; if (_cameFromDict!.ContainsKey(currentNode)) { OsmNode previousNode = _cameFromDict[currentNode]; Vector v1 = new(currentNode, previousNode); Vector v2 = new(currentNode, neighborNode); - double angle = v1.Angle(v2); - nodeAngle = (angle + 1) / 181; + double nodeAngle = v1.Angle(v2); + if (nodeAngle < turnAngle) + angle = 0; + else + angle = nodeAngle / 180; } double prio = regionManager.GetPriorityForVehicle(_speedType,edge, currentNode); - return distance / (speed * nodeAngle + prio + 1); + return distance / (speed * angle + prio + 1); } private double Heuristic(OsmNode currentNode, OsmNode neighborNode, OsmNode goalNode, OsmEdge edge) @@ -121,10 +126,10 @@ public class Pathfinder Vector v1 = new(currentNode, previousNode); Vector v2 = new(currentNode, neighborNode); double nodeAngle = v1.Angle(v2); - if (nodeAngle < 60) - angle = 0.001; + if (nodeAngle < turnAngle) + angle = 0; else - angle = (nodeAngle + 1) / (181); + angle = nodeAngle / 180; } TagManager curTags = regionManager.GetRegion(currentNode.coordinates)!.tagManager;