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

Fixed angle calculation

Browse Source
This commit is contained in:
glax 2023-04-21 11:01:31 +02:00
parent 1facca84ba
commit 750ba5c624
1 changed files with 32 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
Pathfinding/Pathfinder.cs
View File

@ -51,8 +51,8 @@ public class Pathfinder
if (currentNode.Equals(goalNode)) if (currentNode.Equals(goalNode))
{ {
TimeSpan calcTime = DateTime.Now - startCalc; TimeSpan calcTime = DateTime.Now - startCalc;
Console.WriteLine($"Path found. {calcTime}");
pathResult = GetPath(goalNode, calcTime); pathResult = GetPath(goalNode, calcTime);
Console.WriteLine($"Path found. {calcTime} PathLength {pathResult.pathNodes.Count} VisitedNodes {gScore.Count}");
return this; return this;
} }
@ -70,7 +70,7 @@ public class Pathfinder
_cameFromDict[neighbor] = currentNode; _cameFromDict[neighbor] = currentNode;
gScore[neighbor] = tentativeGScore; gScore[neighbor] = tentativeGScore;
double h = Heuristic(currentNode, neighbor, goalNode, edge, double h = Heuristic(currentNode, neighbor, goalNode, edge,
heuristicRoadLevelPriority, heuristicFewJunctionsPriority, heuristicSameRoadPriority); 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}"); //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);
} }
@ -119,20 +119,22 @@ public class Pathfinder
if (_cameFromDict!.ContainsKey(currentNode)) if (_cameFromDict!.ContainsKey(currentNode))
{ {
OsmNode previousNode = _cameFromDict[currentNode]; OsmNode previousNode = _cameFromDict[currentNode];
Vector v1 = new(currentNode.coordinates.longitude - previousNode.coordinates.longitude, Vector v1 = new(previousNode, currentNode);
currentNode.coordinates.latitude - previousNode.coordinates.latitude); Vector v2 = new(currentNode, neighborNode);
Vector v2 = new(currentNode.coordinates.longitude - neighborNode.coordinates.longitude,
currentNode.coordinates.latitude - neighborNode.coordinates.latitude);
double nodeAngle = v1.Angle(v2); double nodeAngle = v1.Angle(v2);
angle = (nodeAngle / 180) * angleWeightFactor; angle = ((180 - nodeAngle) / 180) * angleWeightFactor;
} }
double prio = GetPriorityVehicleRoad(edge, regionManager.GetRegion(currentNode.coordinates)!); double prio = GetPriorityVehicleRoad(edge, regionManager.GetRegion(currentNode.coordinates)!);
return distance / (speed + angle + prio * 0.5); return distance / (1 + speed + angle + prio);
} }
private double Heuristic(OsmNode currentNode, OsmNode neighborNode, OsmNode goalNode, OsmEdge edge, double roadPriorityFactor, double junctionFactor, double sameRoadFactor) private double Heuristic(OsmNode currentNode, OsmNode neighborNode, OsmNode goalNode, OsmEdge edge, double roadPriorityFactor, double junctionFactor, double sameRoadFactor, double nodeAngleFactor)
{ {
double roadPriority = GetPriorityVehicleRoad(edge, regionManager.GetRegion(currentNode.coordinates)!) * roadPriorityFactor; double roadPriority = GetPriorityVehicleRoad(edge, regionManager.GetRegion(currentNode.coordinates)!) * roadPriorityFactor;
if (roadPriority == 0)
return double.MaxValue;
double speed = regionManager.GetSpeedForEdge(currentNode, edge.wayId, _speedType) * 0.0003;
TagManager curTags = regionManager.GetRegion(currentNode.coordinates)!.tagManager; TagManager curTags = regionManager.GetRegion(currentNode.coordinates)!.tagManager;
TagManager nextTags = regionManager.GetRegion(neighborNode.coordinates)!.tagManager; TagManager nextTags = regionManager.GetRegion(neighborNode.coordinates)!.tagManager;
@ -153,7 +155,16 @@ public class Pathfinder
double junctionCount = (neighborNode.edges.Count > 2 ? 0 : 1) * junctionFactor; double junctionCount = (neighborNode.edges.Count > 2 ? 0 : 1) * junctionFactor;
return Utils.DistanceBetween(neighborNode, goalNode) / (1 + roadPriority + sameRoadName + junctionCount); double angle = 0;
if (_cameFromDict!.ContainsKey(currentNode))
{
OsmNode previousNode = _cameFromDict[currentNode];
Vector v1 = new(previousNode, currentNode);
Vector v2 = new(currentNode, neighborNode);
double nodeAngle = v1.Angle(v2);
angle = ((180 - nodeAngle) / 180) * nodeAngleFactor;
}
return Utils.DistanceBetween(neighborNode, goalNode) / (roadPriority + sameRoadName + junctionCount + angle);
} }
private class Vector private class Vector
@ -166,6 +177,12 @@ public class Pathfinder
this.y = y; this.y = y;
} }
public Vector(OsmNode n1, OsmNode n2)
{
this.x = n1.coordinates.longitude - n2.coordinates.longitude;
this.y = n1.coordinates.latitude - n2.coordinates.latitude;
}
public double Angle(Vector v2) public double Angle(Vector v2)
{ {
return Angle(this, v2); return Angle(this, v2);
@ -176,8 +193,9 @@ public class Pathfinder
double dotProd = v1.x * v2.x + v1.y * v2.y; double dotProd = v1.x * v2.x + v1.y * v2.y;
double v1L = Math.Sqrt(v1.x * v1.x + v1.y * v1.y); double v1L = Math.Sqrt(v1.x * v1.x + v1.y * v1.y);
double v2L = Math.Sqrt(v2.x * v2.x + v2.y * v2.y); double v2L = Math.Sqrt(v2.x * v2.x + v2.y * v2.y);
double ang = 180 - Math.Acos(dotProd / (v1L * v2L)); double ang = Math.Acos(dotProd / (v1L * v2L));
return ang; double angle = Utils.RadiansToDegrees(ang);
return angle;
} }
} }
@ -212,9 +230,6 @@ public class Pathfinder
case WayType.road: case WayType.road:
case WayType.living_street: case WayType.living_street:
return 2; return 2;
case WayType.service:
case WayType.track:
return 0.0001;
} }
} }
if (_speedType == SpeedType.pedestrian) if (_speedType == SpeedType.pedestrian)
@ -243,6 +258,6 @@ public class Pathfinder
} }
} }
return 0.01; return 0;
} }
} }