glax/OSMServer
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;
private Dictionary<OsmNode, OsmNode>? _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;