OSMServer/Pathfinding/Pathfinder.cs

203 lines
8.8 KiB
C#

using System.Text.Json;
using OSMDatastructure;
using OSMDatastructure.Graph;
using static OSMDatastructure.Tag;
using WayType = OSMDatastructure.Tag.WayType;
namespace Pathfinding;
public static class Pathfinder
{
public static PathResult AStar(string workingDir, Coordinates startCoordinates, Coordinates goalCoordinates,
SpeedType vehicle, double heuristicRoadLevelPriority, double heuristicSameRoadPriority,
double heuristicFewJunctionsPriority)
{
DateTime startCalc = DateTime.Now;
RegionManager regionManager = new RegionManager(workingDir);
OsmNode? startNode = regionManager.ClosestNodeToCoordinates(startCoordinates, vehicle);
OsmNode? goalNode = regionManager.ClosestNodeToCoordinates(goalCoordinates, vehicle);
if (startNode is null || goalNode is null)
return new PathResult(DateTime.Now - startCalc, new List<PathNode>());
PriorityQueue<OsmNode, double> openSetfScore = new();
openSetfScore.Enqueue(startNode, 0);
Dictionary<OsmNode, OsmNode> cameFromDict = new();
Dictionary<OsmNode, double> gScore = new();
gScore.Add(startNode, 0);
while (openSetfScore.Count > 0)
{
OsmNode currentNode = openSetfScore.Dequeue();
if (currentNode.Equals(goalNode))
{
Console.WriteLine("Path found.");
PathResult path = GetPath(cameFromDict, goalNode, regionManager, DateTime.Now - startCalc);
string fileName = $"{new DirectoryInfo(workingDir).Name}-{DateTime.Now.ToFileTime()}.result";
string outputFilepath = Path.Join(Directory.GetParent(workingDir)!.FullName, fileName);
path.name = outputFilepath;
path.AddGScores(gScore);
SaveGraph(path, outputFilepath);
path.regionManager = regionManager;
return path;
}
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, vehicle, regionManager);
gScore.TryAdd(neighbor, double.MaxValue);
if (tentativeGScore < gScore[neighbor])
{
if (cameFromDict.ContainsKey(neighbor))
cameFromDict[neighbor] = currentNode;
else
cameFromDict.Add(neighbor, currentNode);
if (gScore.ContainsKey(neighbor))
gScore[neighbor] = tentativeGScore;
else
gScore.Add(neighbor, tentativeGScore);
double h = Heuristic(currentNode, neighbor, goalNode, edge, vehicle, regionManager,
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);
}
}
}
}
return new PathResult(DateTime.Now - startCalc, new List<PathNode>());
}
private static void SaveGraph(PathResult pathResult, string outputFilepath)
{
FileStream fs = new FileStream(outputFilepath, FileMode.CreateNew);
JsonSerializer.Serialize(fs, pathResult, JsonSerializerOptions.Default);
fs.Dispose();
Console.WriteLine($"Saved result to {outputFilepath}");
}
private static PathResult GetPath(Dictionary<OsmNode, OsmNode> cameFromDict, OsmNode goalNode, RegionManager regionManager, TimeSpan calcFinished)
{
List<PathNode> path = new List<PathNode>();
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 static double Weight(OsmNode fromNode, OsmNode neighborNode, OsmEdge edge, SpeedType vehicle, RegionManager regionManager)
{
double distance = Utils.DistanceBetween(fromNode, neighborNode);
double speed = regionManager.GetSpeedForEdge(fromNode, edge.wayId, vehicle);
double prio = GetPriorityVehicleRoad(edge, vehicle, regionManager.GetRegion(fromNode.coordinates)!);
return distance / (speed + prio * 50);
}
private static double Heuristic(OsmNode fromNode, OsmNode neighborNode, OsmNode goalNode, OsmEdge edge, SpeedType vehicle, RegionManager regionManager, double roadPriorityFactor, double junctionFactor, double sameRoadFactor)
{
double roadPriority = GetPriorityVehicleRoad(edge, vehicle, regionManager.GetRegion(fromNode.coordinates)!) * roadPriorityFactor;
TagManager curTags = regionManager.GetRegion(fromNode.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;
//Console.WriteLine($"{roadPriority:000.00} {sameRoadName:000.00} {junctionCount:000.00} {distanceImprovement:+000.00;-000.00;0000.00}");
return Utils.DistanceBetween(neighborNode, goalNode) - (roadPriority + sameRoadName + junctionCount) * 20;
}
private static double GetPriorityVehicleRoad(OsmEdge edge, SpeedType vehicle, Region region)
{
if (vehicle == SpeedType.any)
return 1;
WayType? wayType = (WayType?)region.tagManager.GetTag(edge.wayId, TagType.highway);
if(wayType is null)
return 0;
if (vehicle == SpeedType.car)
{
switch (wayType)
{
case WayType.motorway:
case WayType.motorway_link:
case WayType.motorroad:
case WayType.trunk:
case WayType.trunk_link:
case WayType.primary:
case WayType.primary_link:
return 8;
case WayType.secondary:
case WayType.secondary_link:
return 6;
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.01;
}
}
if (vehicle == 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;
}
}