OSMServer/Pathfinding/Pathfinder.cs

231 lines
8.4 KiB
C#
Raw Normal View History

2023-04-21 13:43:50 +02:00
using System.Text.Json;
using OSMDatastructure;
using OSMDatastructure.Graph;
2023-04-10 01:33:18 +02:00
using static OSMDatastructure.Tag;
2023-02-03 23:35:22 +01:00
namespace Pathfinding;
public class Pathfinder
2023-02-03 23:35:22 +01:00
{
public RegionManager regionManager;
public PathResult? pathResult;
public Dictionary<OsmNode, double>? gScore;
2023-04-20 22:58:50 +02:00
private Dictionary<OsmNode, OsmNode>? _cameFromDict;
2023-04-20 22:58:27 +02:00
private SpeedType _speedType;
2023-04-23 15:07:26 +02:00
private double roadPriorityFactor, turnAngle;
2023-04-23 15:07:26 +02:00
public Pathfinder(string workingDirectory, double roadPriorityFactor, double turnAngle)
{
if (!Path.Exists(workingDirectory))
throw new DirectoryNotFoundException(workingDirectory);
regionManager = new(workingDirectory);
this.roadPriorityFactor = roadPriorityFactor;
this.turnAngle = turnAngle;
2023-04-21 15:13:42 +02:00
}
2023-04-23 15:07:26 +02:00
public Pathfinder(RegionManager regionManager, double roadPriorityFactor, double turnAngle)
2023-04-21 15:13:42 +02:00
{
this.regionManager = regionManager;
this.roadPriorityFactor = roadPriorityFactor;
this.turnAngle = turnAngle;
}
public Pathfinder AStar(Coordinates startCoordinates, Coordinates goalCoordinates, SpeedType vehicle)
{
DateTime startCalc = DateTime.Now;
2023-04-20 22:58:27 +02:00
_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>(),0 ,0);
return this;
}
RPriorityQueue<OsmNode, double> openSetfScore = new();
openSetfScore.Enqueue(startNode, 0);
gScore = new() { { startNode, 0 } };
2023-04-20 22:58:50 +02:00
_cameFromDict = new();
bool found = false;
bool stop = false;
TimeSpan firstFound = TimeSpan.MaxValue;
double maxGscore = double.MaxValue;
while (openSetfScore.Count > 0 && !stop)
{
OsmNode currentNode = openSetfScore.Dequeue()!;
if (currentNode.Equals(goalNode))
{
if (!found)
{
firstFound = DateTime.Now - startCalc;
found = true;
}
maxGscore = gScore[goalNode];
/*
int removed = openSetfScore.Remove(gScore.Where(item => item.Value > maxGscore)
.ToDictionary(item => item.Key, item => item.Value).Keys);
Console.WriteLine($"Removed {removed}");*/
}
if (found && DateTime.Now - startCalc > firstFound.Multiply(2))
stop = true;
foreach (OsmEdge edge in currentNode.edges)
{
OsmNode? neighbor = regionManager.GetNode(edge.neighborId, edge.neighborRegion);
if (neighbor is not null)
{
2023-04-23 13:43:01 +02:00
double tentativeGScore = gScore[currentNode] + Weight(currentNode, neighbor, edge);
gScore.TryAdd(neighbor, double.MaxValue);
if ((!found || (found && tentativeGScore < maxGscore)) && tentativeGScore < gScore[neighbor])
{
2023-04-20 22:58:50 +02:00
if(!_cameFromDict.TryAdd(neighbor, currentNode))
_cameFromDict[neighbor] = currentNode;
gScore[neighbor] = tentativeGScore;
double h = Heuristic(currentNode, neighbor, goalNode, edge);
openSetfScore.Enqueue(neighbor, tentativeGScore + h);
}
}
}
}
TimeSpan calcTime = DateTime.Now - startCalc;
if (!found)
{
pathResult = new(DateTime.Now - startCalc, new List<PathNode>(),0 ,0);
Console.Write("No path found.");
return this;
}
else
{
pathResult = GetPath(goalNode, calcTime);
}
Console.WriteLine($"Path found. {calcTime} PathLength {pathResult.pathNodes.Count} VisitedNodes {gScore.Count} Distance {pathResult.distance} Duration {pathResult.weight}");
return this;
}
2023-04-21 14:28:02 +02:00
private double Weight(OsmNode currentNode, OsmNode neighborNode, OsmEdge edge)
{
double distance = Utils.DistanceBetween(currentNode, neighborNode);
double speed = regionManager.GetSpeedForEdge(currentNode, edge.wayId, _speedType);
2023-04-23 12:58:32 +02:00
double angle = 1;
if (_cameFromDict!.ContainsKey(currentNode))
{
OsmNode previousNode = _cameFromDict[currentNode];
2023-04-21 14:28:02 +02:00
Vector v1 = new(currentNode, previousNode);
2023-04-21 11:01:31 +02:00
Vector v2 = new(currentNode, neighborNode);
double nodeAngle = v1.Angle(v2);
if (nodeAngle < turnAngle)
angle = 0;
else
angle = nodeAngle / 180;
}
double prio = regionManager.GetPriorityForVehicle(_speedType,edge, currentNode) * roadPriorityFactor;
2023-04-23 12:58:32 +02:00
return distance / (speed * angle + prio + 1);
}
2023-04-10 01:33:18 +02:00
private double Heuristic(OsmNode currentNode, OsmNode neighborNode, OsmNode goalNode, OsmEdge edge)
{
2023-04-23 13:43:01 +02:00
if (neighborNode.Equals(goalNode)) return 0;
double priority = regionManager.GetPriorityForVehicle(_speedType, edge, currentNode);
if (priority == 0)
2023-04-21 11:01:31 +02:00
return double.MaxValue;
2023-04-23 15:09:22 +02:00
double distance = Utils.DistanceBetween(neighborNode, goalNode);
2023-04-21 11:01:31 +02:00
double speed = regionManager.GetSpeedForEdge(currentNode, edge.wayId, _speedType);
2023-04-23 15:27:23 +02:00
double roadPriority = priority * roadPriorityFactor;
double angle = 0;
if (_cameFromDict!.ContainsKey(currentNode))
{
OsmNode previousNode = _cameFromDict[currentNode];
Vector v1 = new(currentNode, previousNode);
Vector v2 = new(currentNode, neighborNode);
double nodeAngle = v1.Angle(v2);
if (nodeAngle < turnAngle)
angle = 0;
else
angle = nodeAngle / 180;
}
2023-04-10 01:33:18 +02:00
2023-04-23 15:09:22 +02:00
return distance / (speed * angle + roadPriority + 1);
}
public void SaveResult(string path)
{
2023-04-23 16:02:55 +02:00
if(File.Exists(path))
File.Delete(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;
double retDistance = 0;
double weight = 0;
while (_cameFromDict!.ContainsKey(_cameFromDict[currentNode]))
{
2023-04-21 13:43:50 +02:00
OsmEdge? currentEdge = _cameFromDict[currentNode].edges.FirstOrDefault(edge => edge.neighborId == currentNode.nodeId);
HashSet<Tag>? tags =
2023-04-21 13:43:50 +02:00
regionManager.GetRegion(currentNode.coordinates)!.tagManager.GetTagsForWayId(currentEdge!.wayId);
PathNode? newNode = PathNode.FromOsmNode(currentNode, tags);
if(newNode is not null)
path.Add(newNode);
double distance = Utils.DistanceBetween(currentNode, _cameFromDict[currentNode]);
retDistance += distance;
weight += regionManager.GetSpeedForEdge(_cameFromDict[currentNode], currentEdge.wayId, _speedType);
currentNode = _cameFromDict[currentNode];
}
path.Reverse();
return new PathResult(calcFinished, path, retDistance, retDistance / (weight / path.Count));
}
private class Vector
{
2023-04-21 13:43:50 +02:00
public readonly float x, y;
public Vector(float x, float y)
{
this.x = x;
this.y = y;
}
2023-04-21 11:01:31 +02:00
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);
}
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);
2023-04-21 11:01:31 +02:00
double ang = Math.Acos(dotProd / (v1L * v2L));
if (ang.Equals(double.NaN))
return 0;
2023-04-21 11:01:31 +02:00
double angle = Utils.RadiansToDegrees(ang);
return angle;
}
}
2023-02-03 23:35:22 +01:00
}