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