diff --git a/Pathfinding/Pathfinder.cs b/Pathfinding/Pathfinder.cs index be66723..2a9d1c4 100644 --- a/Pathfinding/Pathfinder.cs +++ b/Pathfinding/Pathfinder.cs @@ -28,7 +28,6 @@ public class Pathfinder double heuristicFewJunctionsPriority, double angleWeightFactor) { DateTime startCalc = DateTime.Now; - regionManager = new RegionManager(workingDir); _speedType = vehicle; OsmNode? startNode = regionManager.ClosestNodeToCoordinates(startCoordinates, _speedType); OsmNode? goalNode = regionManager.ClosestNodeToCoordinates(goalCoordinates, _speedType); @@ -60,7 +59,7 @@ public class Pathfinder if (neighbor is not null) { double tentativeGScore = - gScore[currentNode] + Weight(currentNode, neighbor, edge, angleWeightFactor); + gScore[currentNode] + Weight(currentNode, neighbor, edge); gScore.TryAdd(neighbor, double.MaxValue); if (tentativeGScore < gScore[neighbor]) { @@ -80,7 +79,7 @@ public class Pathfinder return this; } - private double Weight(OsmNode currentNode, OsmNode neighborNode, OsmEdge edge, double angleWeightFactor) + private double Weight(OsmNode currentNode, OsmNode neighborNode, OsmEdge edge) { double distance = Utils.DistanceBetween(currentNode, neighborNode); double speed = regionManager.GetSpeedForEdge(currentNode, edge.wayId, _speedType); @@ -89,13 +88,13 @@ public class Pathfinder if (_cameFromDict!.ContainsKey(currentNode)) { OsmNode previousNode = _cameFromDict[currentNode]; - Vector v1 = new(previousNode, currentNode); + Vector v1 = new(currentNode, previousNode); Vector v2 = new(currentNode, neighborNode); double nodeAngle = v1.Angle(v2); - angle = ((180 - nodeAngle) / 180) * angleWeightFactor; + angle = (nodeAngle / 180); } double prio = regionManager.GetPriorityForVehicle(_speedType,edge, currentNode); - return distance / (1 + speed + angle + prio); + return distance / (1 + speed * angle); } private double Heuristic(OsmNode currentNode, OsmNode neighborNode, OsmNode goalNode, OsmEdge edge, double roadPriorityFactor, double junctionFactor, double sameRoadFactor, double nodeAngleFactor)