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

Add turn-angle to Weight calculation.

Browse Source 
For that added Vector-class.
Highways now have much higher priority.
This commit is contained in:
glax 2023-04-21 00:42:21 +02:00
parent d1f311a76b
commit 7201b9c993
1 changed files with 54 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

68
Pathfinding/Pathfinder.cs
View File

@ -1,4 +1,5 @@
using System.Text.Json;
using System.Diagnostics;
using System.Text.Json;
using OSMDatastructure;
using OSMDatastructure.Graph;
using static OSMDatastructure.Tag;
@ -26,7 +27,7 @@ public class Pathfinder
public Pathfinder AStar(Coordinates startCoordinates, Coordinates goalCoordinates,
SpeedType vehicle, double heuristicRoadLevelPriority, double heuristicSameRoadPriority,
double heuristicFewJunctionsPriority)
double heuristicFewJunctionsPriority, double angleWeightFactor)
{
DateTime startCalc = DateTime.Now;
regionManager = new RegionManager(workingDir);
@ -49,8 +50,9 @@ public class Pathfinder
OsmNode currentNode = openSetfScore.Dequeue();
if (currentNode.Equals(goalNode))
{
Console.WriteLine("Path found.");
pathResult = GetPath(goalNode, DateTime.Now - startCalc);
TimeSpan calcTime = DateTime.Now - startCalc;
Console.WriteLine($"Path found. {calcTime}");
pathResult = GetPath(goalNode, calcTime);
return this;
}
@ -60,7 +62,7 @@ public class Pathfinder
if (neighbor is not null)
{
double tentativeGScore =
gScore[currentNode] + Weight(currentNode, neighbor, edge);
gScore[currentNode] + Weight(currentNode, neighbor, edge, angleWeightFactor);
gScore.TryAdd(neighbor, double.MaxValue);
if (tentativeGScore < gScore[neighbor])
{
@ -108,19 +110,31 @@ public class Pathfinder
return new PathResult(calcFinished, path);
}
private double Weight(OsmNode fromNode, OsmNode neighborNode, OsmEdge edge)
private double Weight(OsmNode currentNode, OsmNode neighborNode, OsmEdge edge, double angleWeightFactor)
{
double distance = Utils.DistanceBetween(fromNode, neighborNode);
double speed = regionManager.GetSpeedForEdge(fromNode, edge.wayId, _speedType);
//double prio = GetPriorityVehicleRoad(edge, vehicle, regionManager.GetRegion(fromNode.coordinates)!);
return distance / speed;
double distance = Utils.DistanceBetween(currentNode, neighborNode);
double speed = regionManager.GetSpeedForEdge(currentNode, edge.wayId, _speedType);
double angle = 1;
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);
double nodeAngle = v1.Angle(v2);
angle = (nodeAngle / 180) * angleWeightFactor;
}
double prio = GetPriorityVehicleRoad(edge, regionManager.GetRegion(currentNode.coordinates)!);
return distance / (speed + angle + prio * 0.5);
}
private double Heuristic(OsmNode fromNode, 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 roadPriority = GetPriorityVehicleRoad(edge, regionManager.GetRegion(fromNode.coordinates)!) * roadPriorityFactor;
double roadPriority = GetPriorityVehicleRoad(edge, regionManager.GetRegion(currentNode.coordinates)!) * roadPriorityFactor;
TagManager curTags = regionManager.GetRegion(fromNode.coordinates)!.tagManager;
TagManager curTags = regionManager.GetRegion(currentNode.coordinates)!.tagManager;
TagManager nextTags = regionManager.GetRegion(neighborNode.coordinates)!.tagManager;
bool sameName = false;
@ -138,9 +152,34 @@ 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);
}
private class Vector
{
public float x, y;
public Vector(float x, float y)
{
this.x = x;
this.y = y;
}
public double Angle(Vector v2)
{
return Angle(this, v2);
}
public static double Angle(Vector v1, Vector v2)
{
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;
}
}
private double GetPriorityVehicleRoad(OsmEdge edge, Region region)
{
@ -156,6 +195,7 @@ public class Pathfinder
case WayType.motorway:
case WayType.motorway_link:
case WayType.motorroad:
return 17;
case WayType.trunk:
case WayType.trunk_link:
case WayType.primary: