From 465d40a4754327b407edd6cefd035bc7c282b62d Mon Sep 17 00:00:00 2001 From: glax Date: Sun, 23 Apr 2023 12:58:32 +0200 Subject: [PATCH] Cleanup variable rename --- Pathfinding/Pathfinder.cs | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/Pathfinding/Pathfinder.cs b/Pathfinding/Pathfinder.cs index 937c18b..d47b960 100644 --- a/Pathfinding/Pathfinder.cs +++ b/Pathfinding/Pathfinder.cs @@ -71,8 +71,7 @@ public class Pathfinder gScore[neighbor] = tentativeGScore; double h = Heuristic(currentNode, neighbor, goalNode, edge, heuristicRoadLevelPriority, heuristicFewJunctionsPriority, heuristicSameRoadPriority, angleWeightFactor); - //Console.WriteLine($"Queue: {openSetfScore.Count:00000} Current Distance: {Utils.DistanceBetween(currentNode, goalNode):000000.00} Visited: {cameFromDict.Count:00000} Current heuristic: {h:00000.00}"); - openSetfScore.Enqueue(neighbor, tentativeGScore + h); + openSetfScore.Enqueue(neighbor, tentativeGScore + h); //Currently set includes a lot of duplicates } } } @@ -86,18 +85,19 @@ public class Pathfinder { double distance = Utils.DistanceBetween(currentNode, neighborNode); double speed = regionManager.GetSpeedForEdge(currentNode, edge.wayId, _speedType); - - double angle = 1; + + double nodeAngle = 1; if (_cameFromDict!.ContainsKey(currentNode)) { OsmNode previousNode = _cameFromDict[currentNode]; Vector v1 = new(currentNode, previousNode); Vector v2 = new(currentNode, neighborNode); - double nodeAngle = v1.Angle(v2); - angle = (nodeAngle / 180); + double angle = v1.Angle(v2); + nodeAngle = (angle + 1) / 181; } double prio = regionManager.GetPriorityForVehicle(_speedType,edge, currentNode); - return distance / (1 + speed * angle); + + return distance / (speed * nodeAngle + prio + 1); } private double Heuristic(OsmNode currentNode, OsmNode neighborNode, OsmNode goalNode, OsmEdge edge, double roadPriorityFactor, double junctionFactor, double sameRoadFactor, double nodeAngleFactor)