glax/OSMServer
Archived
1
0
Fork 0
Code Pull Requests Releases Wiki Activity

Created variable for maximum turn angle.

Browse Source
This commit is contained in:
glax 2023-04-23 14:47:26 +02:00
parent 6bc1d3c7ce
commit b89a3715a1
1 changed files with 15 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
Pathfinding/Pathfinder.cs
View File

@ -13,9 +13,9 @@ public class Pathfinder
public Dictionary<OsmNode, double>? gScore; public Dictionary<OsmNode, double>? gScore;
private Dictionary<OsmNode, OsmNode>? _cameFromDict; private Dictionary<OsmNode, OsmNode>? _cameFromDict;
private SpeedType _speedType; 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)) if (!Path.Exists(workingDirectory))
throw new DirectoryNotFoundException(workingDirectory); throw new DirectoryNotFoundException(workingDirectory);
@ -24,15 +24,17 @@ public class Pathfinder
this.junctionFactor = junctionFactor; this.junctionFactor = junctionFactor;
this.sameRoadFactor = sameRoadFactor; this.sameRoadFactor = sameRoadFactor;
this.nodeAngleFactor = nodeAngleFactor; 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.regionManager = regionManager;
this.roadPriorityFactor = roadPriorityFactor; this.roadPriorityFactor = roadPriorityFactor;
this.junctionFactor = junctionFactor; this.junctionFactor = junctionFactor;
this.sameRoadFactor = sameRoadFactor; this.sameRoadFactor = sameRoadFactor;
this.nodeAngleFactor = nodeAngleFactor; this.nodeAngleFactor = nodeAngleFactor;
this.turnAngle = turnAngle;
} }
public Pathfinder AStar(Coordinates startCoordinates, Coordinates goalCoordinates, SpeedType vehicle) public Pathfinder AStar(Coordinates startCoordinates, Coordinates goalCoordinates, SpeedType vehicle)
@ -91,18 +93,21 @@ public class Pathfinder
double distance = Utils.DistanceBetween(currentNode, neighborNode); double distance = Utils.DistanceBetween(currentNode, neighborNode);
double speed = regionManager.GetSpeedForEdge(currentNode, edge.wayId, _speedType); double speed = regionManager.GetSpeedForEdge(currentNode, edge.wayId, _speedType);
double nodeAngle = 1; double angle = 1;
if (_cameFromDict!.ContainsKey(currentNode)) if (_cameFromDict!.ContainsKey(currentNode))
{ {
OsmNode previousNode = _cameFromDict[currentNode]; OsmNode previousNode = _cameFromDict[currentNode];
Vector v1 = new(currentNode, previousNode); Vector v1 = new(currentNode, previousNode);
Vector v2 = new(currentNode, neighborNode); Vector v2 = new(currentNode, neighborNode);
double angle = v1.Angle(v2); double nodeAngle = v1.Angle(v2);
nodeAngle = (angle + 1) / 181; if (nodeAngle < turnAngle)
angle = 0;
else
angle = nodeAngle / 180;
} }
double prio = regionManager.GetPriorityForVehicle(_speedType,edge, currentNode); 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) private double Heuristic(OsmNode currentNode, OsmNode neighborNode, OsmNode goalNode, OsmEdge edge)
@ -121,10 +126,10 @@ public class Pathfinder
Vector v1 = new(currentNode, previousNode); Vector v1 = new(currentNode, previousNode);
Vector v2 = new(currentNode, neighborNode); Vector v2 = new(currentNode, neighborNode);
double nodeAngle = v1.Angle(v2); double nodeAngle = v1.Angle(v2);
if (nodeAngle < 60) if (nodeAngle < turnAngle)
angle = 0.001; angle = 0;
else else
angle = (nodeAngle + 1) / (181); angle = nodeAngle / 180;
} }
TagManager curTags = regionManager.GetRegion(currentNode.coordinates)!.tagManager; TagManager curTags = regionManager.GetRegion(currentNode.coordinates)!.tagManager;