248 lines
9.6 KiB
C#
248 lines
9.6 KiB
C#
using System.Diagnostics;
|
|
using System.Text.Json;
|
|
using OSMDatastructure;
|
|
using OSMDatastructure.Graph;
|
|
using static OSMDatastructure.Tag;
|
|
using WayType = OSMDatastructure.Tag.WayType;
|
|
|
|
namespace Pathfinding;
|
|
|
|
public class Pathfinder
|
|
{
|
|
|
|
public RegionManager regionManager;
|
|
public readonly string workingDir;
|
|
public PathResult? pathResult;
|
|
public Dictionary<OsmNode, double>? gScore;
|
|
private Dictionary<OsmNode, OsmNode>? _cameFromDict;
|
|
private SpeedType _speedType;
|
|
|
|
public Pathfinder(string workingDirectory)
|
|
{
|
|
if (!Path.Exists(workingDirectory))
|
|
throw new DirectoryNotFoundException(workingDirectory);
|
|
regionManager = new(workingDirectory);
|
|
workingDir = workingDirectory;
|
|
}
|
|
|
|
public Pathfinder AStar(Coordinates startCoordinates, Coordinates goalCoordinates,
|
|
SpeedType vehicle, double heuristicRoadLevelPriority, double heuristicSameRoadPriority,
|
|
double heuristicFewJunctionsPriority, double angleWeightFactor)
|
|
{
|
|
DateTime startCalc = DateTime.Now;
|
|
regionManager = new RegionManager(workingDir);
|
|
_speedType = vehicle;
|
|
OsmNode? startNode = regionManager.ClosestNodeToCoordinates(startCoordinates, _speedType);
|
|
OsmNode? goalNode = regionManager.ClosestNodeToCoordinates(goalCoordinates, _speedType);
|
|
if (startNode is null || goalNode is null)
|
|
{
|
|
pathResult = new(DateTime.Now - startCalc, new List<PathNode>());
|
|
return this;
|
|
}
|
|
|
|
PriorityQueue<OsmNode, double> openSetfScore = new();
|
|
openSetfScore.Enqueue(startNode, 0);
|
|
gScore = new() { { startNode, 0 } };
|
|
_cameFromDict = new();
|
|
|
|
while (openSetfScore.Count > 0)
|
|
{
|
|
OsmNode currentNode = openSetfScore.Dequeue();
|
|
if (currentNode.Equals(goalNode))
|
|
{
|
|
TimeSpan calcTime = DateTime.Now - startCalc;
|
|
Console.WriteLine($"Path found. {calcTime}");
|
|
pathResult = GetPath(goalNode, calcTime);
|
|
return this;
|
|
}
|
|
|
|
foreach (OsmEdge edge in currentNode.edges)
|
|
{
|
|
OsmNode? neighbor = regionManager.GetNode(edge.neighborId, edge.neighborRegion);
|
|
if (neighbor is not null)
|
|
{
|
|
double tentativeGScore =
|
|
gScore[currentNode] + Weight(currentNode, neighbor, edge, angleWeightFactor);
|
|
gScore.TryAdd(neighbor, double.MaxValue);
|
|
if (tentativeGScore < gScore[neighbor])
|
|
{
|
|
if(!_cameFromDict.TryAdd(neighbor, currentNode))
|
|
_cameFromDict[neighbor] = currentNode;
|
|
gScore[neighbor] = tentativeGScore;
|
|
double h = Heuristic(currentNode, neighbor, goalNode, edge,
|
|
heuristicRoadLevelPriority, heuristicFewJunctionsPriority, heuristicSameRoadPriority);
|
|
//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);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
pathResult = new(DateTime.Now - startCalc, new List<PathNode>());
|
|
return this;
|
|
}
|
|
|
|
public void SaveResult(string path)
|
|
{
|
|
FileStream fs = new (path, FileMode.CreateNew);
|
|
JsonSerializer.Serialize(fs, pathResult, JsonSerializerOptions.Default);
|
|
fs.Dispose();
|
|
Console.WriteLine($"Saved result to {path}");
|
|
}
|
|
|
|
private PathResult GetPath(OsmNode goalNode, TimeSpan calcFinished)
|
|
{
|
|
List<PathNode> path = new();
|
|
OsmNode currentNode = goalNode;
|
|
while (_cameFromDict!.ContainsKey(_cameFromDict[currentNode]))
|
|
{
|
|
OsmEdge? currentEdge = _cameFromDict[currentNode].edges.First(edge => edge.neighborId == currentNode.nodeId);
|
|
HashSet<Tag>? tags =
|
|
regionManager.GetRegion(currentNode.coordinates)!.tagManager.GetTagsForWayId(currentEdge.wayId);
|
|
PathNode? newNode = PathNode.FromOsmNode(currentNode, tags);
|
|
if(newNode is not null)
|
|
path.Add(newNode);
|
|
currentNode = _cameFromDict[currentNode];
|
|
}
|
|
|
|
path.Reverse();
|
|
|
|
return new PathResult(calcFinished, path);
|
|
}
|
|
|
|
private double Weight(OsmNode currentNode, OsmNode neighborNode, OsmEdge edge, double angleWeightFactor)
|
|
{
|
|
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 currentNode, OsmNode neighborNode, OsmNode goalNode, OsmEdge edge, double roadPriorityFactor, double junctionFactor, double sameRoadFactor)
|
|
{
|
|
double roadPriority = GetPriorityVehicleRoad(edge, regionManager.GetRegion(currentNode.coordinates)!) * roadPriorityFactor;
|
|
|
|
TagManager curTags = regionManager.GetRegion(currentNode.coordinates)!.tagManager;
|
|
TagManager nextTags = regionManager.GetRegion(neighborNode.coordinates)!.tagManager;
|
|
|
|
bool sameName = false;
|
|
string? curName = (string?)curTags.GetTag(edge.wayId, TagType.name);
|
|
bool sameRef = false;
|
|
string? curRef = (string?)curTags.GetTag(edge.wayId, TagType.tagref);
|
|
if(curName is not null)
|
|
foreach (OsmEdge pEdge in neighborNode.edges)
|
|
{
|
|
if ((string?)nextTags.GetTag(pEdge.wayId, TagType.name) == curName)
|
|
sameName = true;
|
|
if ((string?)nextTags.GetTag(pEdge.wayId, TagType.tagref) == curRef)
|
|
sameRef = true;
|
|
}
|
|
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)
|
|
{
|
|
if (_speedType == SpeedType.any)
|
|
return 1;
|
|
WayType? wayType = (WayType?)region.tagManager.GetTag(edge.wayId, TagType.highway);
|
|
if(wayType is null)
|
|
return 0;
|
|
if (_speedType == SpeedType.car)
|
|
{
|
|
switch (wayType)
|
|
{
|
|
case WayType.motorway:
|
|
case WayType.motorway_link:
|
|
case WayType.motorroad:
|
|
return 17;
|
|
case WayType.trunk:
|
|
case WayType.trunk_link:
|
|
case WayType.primary:
|
|
case WayType.primary_link:
|
|
return 10;
|
|
case WayType.secondary:
|
|
case WayType.secondary_link:
|
|
return 7;
|
|
case WayType.tertiary:
|
|
case WayType.tertiary_link:
|
|
return 5;
|
|
case WayType.unclassified:
|
|
case WayType.residential:
|
|
case WayType.road:
|
|
case WayType.living_street:
|
|
return 2;
|
|
case WayType.service:
|
|
case WayType.track:
|
|
return 0.0001;
|
|
}
|
|
}
|
|
if (_speedType == SpeedType.pedestrian)
|
|
{
|
|
switch (wayType)
|
|
{
|
|
case WayType.pedestrian:
|
|
case WayType.corridor:
|
|
case WayType.footway:
|
|
case WayType.path:
|
|
case WayType.steps:
|
|
case WayType.residential:
|
|
case WayType.living_street:
|
|
return 10;
|
|
case WayType.service:
|
|
case WayType.cycleway:
|
|
case WayType.bridleway:
|
|
case WayType.road:
|
|
case WayType.track:
|
|
case WayType.unclassified:
|
|
return 5;
|
|
case WayType.tertiary:
|
|
case WayType.tertiary_link:
|
|
case WayType.escape:
|
|
return 2;
|
|
}
|
|
}
|
|
|
|
return 0.01;
|
|
}
|
|
} |