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

Made Pathfinder non-static to enable field-usage in other methods/classes.

Browse Source 
Added fields RegionManager, workingDirectory, PathResult (result of AStar), gScore (result of AStar). Fields later used in Renderer.

SaveGraph is now called SaveResult and serializes PathResult as JSON.
This commit is contained in:
glax 2023-04-20 19:39:18 +02:00
parent 5a1dce9883
commit 946fa0206b
1 changed files with 36 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

61
Pathfinding/Pathfinder.cs
View File

@ -6,25 +6,41 @@ using WayType = OSMDatastructure.Tag.WayType;
namespace Pathfinding;
public static class Pathfinder
//TODO check parameters for all functions and determine global fields
public class Pathfinder
{
public static PathResult AStar(string workingDir, Coordinates startCoordinates, Coordinates goalCoordinates,
public RegionManager regionManager;
public readonly string workingDir;
public PathResult? pathResult;
public Dictionary<OsmNode, double>? gScore;
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)
{
DateTime startCalc = DateTime.Now;
RegionManager regionManager = new RegionManager(workingDir);
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>());
{
pathResult = new(DateTime.Now - startCalc, new List<PathNode>());
return this;
}
PriorityQueue<OsmNode, double> openSetfScore = new();
openSetfScore.Enqueue(startNode, 0);
Dictionary<OsmNode, OsmNode> cameFromDict = new();
Dictionary<OsmNode, double> gScore = new();
gScore.Add(startNode, 0);
gScore = new() { { startNode, 0 } };
while (openSetfScore.Count > 0)
{
@ -32,14 +48,8 @@ public static class Pathfinder
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;
this.pathResult = GetPath(cameFromDict, goalNode, DateTime.Now - startCalc);
return this;
}
foreach (OsmEdge edge in currentNode.edges)
@ -48,7 +58,7 @@ public static class Pathfinder
if (neighbor is not null)
{
double tentativeGScore =
gScore[currentNode] + Weight(currentNode, neighbor, edge, vehicle, regionManager);
gScore[currentNode] + Weight(currentNode, neighbor, edge, vehicle);
gScore.TryAdd(neighbor, double.MaxValue);
if (tentativeGScore < gScore[neighbor])
{
@ -60,7 +70,7 @@ public static class Pathfinder
gScore[neighbor] = tentativeGScore;
else
gScore.Add(neighbor, tentativeGScore);
double h = Heuristic(currentNode, neighbor, goalNode, edge, vehicle, regionManager,
double h = Heuristic(currentNode, neighbor, goalNode, edge, vehicle,
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);
@ -69,20 +79,21 @@ public static class Pathfinder
}
}
return new PathResult(DateTime.Now - startCalc, new List<PathNode>());
pathResult = new(DateTime.Now - startCalc, new List<PathNode>());
return this;
}
private static void SaveGraph(PathResult pathResult, string outputFilepath)
public void SaveResult(string path)
{
FileStream fs = new FileStream(outputFilepath, FileMode.CreateNew);
FileStream fs = new (path, FileMode.CreateNew);
JsonSerializer.Serialize(fs, pathResult, JsonSerializerOptions.Default);
fs.Dispose();
Console.WriteLine($"Saved result to {outputFilepath}");
Console.WriteLine($"Saved result to {path}");
}
private static PathResult GetPath(Dictionary<OsmNode, OsmNode> cameFromDict, OsmNode goalNode, RegionManager regionManager, TimeSpan calcFinished)
private PathResult GetPath(Dictionary<OsmNode, OsmNode> cameFromDict, OsmNode goalNode, TimeSpan calcFinished)
{
List<PathNode> path = new List<PathNode>();
List<PathNode> path = new();
OsmNode currentNode = goalNode;
while (cameFromDict.ContainsKey(cameFromDict[currentNode]))
{
@ -100,15 +111,15 @@ public static class Pathfinder
return new PathResult(calcFinished, path);
}
private static double Weight(OsmNode fromNode, OsmNode neighborNode, OsmEdge edge, SpeedType vehicle, RegionManager regionManager)
private double Weight(OsmNode fromNode, OsmNode neighborNode, OsmEdge edge, SpeedType vehicle)
{
double distance = Utils.DistanceBetween(fromNode, neighborNode);
double speed = regionManager.GetSpeedForEdge(fromNode, edge.wayId, vehicle);
double prio = GetPriorityVehicleRoad(edge, vehicle, regionManager.GetRegion(fromNode.coordinates)!);
//double prio = GetPriorityVehicleRoad(edge, vehicle, regionManager.GetRegion(fromNode.coordinates)!);
return distance / speed;
}
private static double Heuristic(OsmNode fromNode, OsmNode neighborNode, OsmNode goalNode, OsmEdge edge, SpeedType vehicle, RegionManager regionManager, double roadPriorityFactor, double junctionFactor, double sameRoadFactor)
private double Heuristic(OsmNode fromNode, OsmNode neighborNode, OsmNode goalNode, OsmEdge edge, SpeedType vehicle, double roadPriorityFactor, double junctionFactor, double sameRoadFactor)
{
double roadPriority = GetPriorityVehicleRoad(edge, vehicle, regionManager.GetRegion(fromNode.coordinates)!) * roadPriorityFactor;