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

Created seperate PathNode class for Json Serialization

Browse Source
This commit is contained in:
glax
2023-04-06 14:27:25 +02:00
parent 72b5511c26
commit 8813023cd6
4 changed files with 44 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
Pathfinding/Pathfinder.cs
View File

@ -6,30 +6,29 @@ namespace Pathfinding;
public static class Pathfinder
{
public static List<OsmNode> CustomAStar(string workingDir, Coordinates start, Coordinates goal, Tag.SpeedType vehicle)
public static List<PathNode> CustomAStar(string workingDir, Coordinates start, Coordinates goal, Tag.SpeedType vehicle)
{
RegionManager regionManager = new RegionManager(workingDir);
Region? startRegion = regionManager.GetRegion(start);
Region? goalRegion = regionManager.GetRegion(goal);
if (startRegion is null || goalRegion is null)
return new List<OsmNode>();
return new List<PathNode>();
OsmNode? startNode = ClosestNodeToCoordinates(start, startRegion, vehicle, ref regionManager);
OsmNode? goalNode = ClosestNodeToCoordinates(goal, goalRegion, vehicle, ref regionManager);
if (startNode == null || goalNode == null)
return new List<OsmNode>();
return new List<PathNode>();
PriorityQueue<OsmNode, double> toVisit = new();
toVisit.Enqueue(startNode, 0);
startNode.currentPathWeight = 0;
startNode.currentPathLength = 0;
startNode.directDistanceToGoal = Utils.DistanceBetween(startNode, goalNode);
OsmNode closestNodeToGoal = startNode;
bool stop = false;
while (toVisit.Count > 0)
{
closestNodeToGoal = toVisit.Dequeue();
OsmNode closestNodeToGoal = toVisit.Dequeue();
Console.WriteLine($"{toVisit.Count:000} {closestNodeToGoal.directDistanceToGoal:#.00} current:{closestNodeToGoal}");
foreach (OsmEdge edge in closestNodeToGoal.edges)
@ -44,6 +43,7 @@ public static class Pathfinder
neighbor.previousPathNode = closestNodeToGoal;
neighbor.currentPathWeight = newPotentialWeight;
neighbor.currentPathLength = closestNodeToGoal.currentPathLength + Utils.DistanceBetween(closestNodeToGoal, neighbor);
neighbor.directDistanceToGoal = Utils.DistanceBetween(neighbor, goalNode);
if (neighbor.Equals(goalNode) || closestNodeToGoal.directDistanceToGoal < 10)
{
@ -52,7 +52,6 @@ public static class Pathfinder
}
else if(!stop)
{
neighbor.directDistanceToGoal = Utils.DistanceBetween(neighbor, goalNode);
toVisit.Enqueue(neighbor, neighbor.directDistanceToGoal);
}
}
@ -60,14 +59,13 @@ public static class Pathfinder
}
}
List<OsmNode> path = new();
List<PathNode> path = new();
OsmNode? currentNode = goalNode;
while (currentNode != null && !currentNode.Equals(startNode))
while (currentNode is not null)
{
path.Add(currentNode);
path.Add(PathNode.FromOsmNode(currentNode)!);
currentNode = currentNode.previousPathNode;
}
path.Add(startNode);
path.Reverse();
return path;