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

Save Pathfinding result to file.

Browse Source 
Load result for render.
This commit is contained in:
glax
2023-04-13 19:18:25 +02:00
parent 08ebc9a26b
commit 055a751c9d
8 changed files with 123 additions and 50 deletions
Download Patch File Download Diff File Expand all files Collapse all files

43
Pathfinding/Pathfinder.cs
View File

@ -1,6 +1,6 @@
using OSMDatastructure;
using System.Text.Json;
using OSMDatastructure;
using OSMDatastructure.Graph;
using RenderPath;
using static OSMDatastructure.Tag;
using WayType = OSMDatastructure.Tag.WayType;
@ -8,16 +8,17 @@ namespace Pathfinding;
public static class Pathfinder
{
public static List<PathNode> AStar(string workingDir, Coordinates startCoordinates, Coordinates goalCoordinates,
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 List<PathNode>();
return new PathResult(DateTime.Now - startCalc, new List<PathNode>());
PriorityQueue<OsmNode, double> openSetfScore = new();
openSetfScore.Enqueue(startNode, 0);
@ -31,13 +32,12 @@ public static class Pathfinder
if (currentNode.Equals(goalNode))
{
Console.WriteLine("Path found.");
List<PathNode> path = GetPath(cameFromDict, goalNode, regionManager);
Console.WriteLine("Rendering.");
List<Coordinates> coords = new();
foreach(PathNode pn in path)
coords.Add(pn.coordinates);
Renderer.DrawLoadedNodes(gScore, coords, workingDir);
Console.WriteLine("Done.");
PathResult path = GetPath(cameFromDict, goalNode, regionManager, DateTime.Now - startCalc);
path.AddGScores(gScore);
string fileName = $"{new DirectoryInfo(workingDir).Name}-{DateTime.Now.ToFileTime()}.result";
string outputFilepath = Path.Join(Directory.GetParent(workingDir)!.FullName, fileName);
path.name = outputFilepath;
SaveGraph(path, outputFilepath);
return path;
}
@ -46,7 +46,8 @@ public static class Pathfinder
OsmNode? neighbor = regionManager.GetNode(edge.neighborId, edge.neighborRegion);
if (neighbor is not null)
{
double tentativeGScore = gScore[currentNode] + Weight(currentNode, neighbor, edge, vehicle, regionManager);
double tentativeGScore =
gScore[currentNode] + Weight(currentNode, neighbor, edge, vehicle, regionManager);
gScore.TryAdd(neighbor, double.MaxValue);
if (tentativeGScore < gScore[neighbor])
{
@ -62,16 +63,24 @@ public static class Pathfinder
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 List<PathNode>();
return new PathResult(DateTime.Now - startCalc, new List<PathNode>());
}
private static List<PathNode> GetPath(Dictionary<OsmNode, OsmNode> cameFromDict, OsmNode goalNode, RegionManager regionManager)
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;
@ -88,7 +97,7 @@ public static class Pathfinder
path.Reverse();
return path;
return new PathResult(calcFinished, path);
}
private static double Weight(OsmNode fromNode, OsmNode neighborNode, OsmEdge edge, SpeedType vehicle, RegionManager regionManager)