glax/OSMServer
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

49
Pathfinding/Pathfinder.cs
View File

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