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

Datastructure rewrite

Browse Source
This commit is contained in:
C9Glax
2023-02-06 17:32:55 +01:00
parent 2c18162398
commit e265c56bce
19 changed files with 567 additions and 658 deletions
Download Patch File Download Diff File Expand all files Collapse all files

74
Pathfinding/Pathfinder.cs
View File

@ -6,48 +6,50 @@ namespace Pathfinding;
public class Pathfinder
{
public static List<PathNode> CustomAStar(string workingDir, Coordinates start, Coordinates goal)
public static List<OsmNode> CustomAStar(string workingDir, Coordinates start, Coordinates goal)
{
RegionManager regionManager = new RegionManager(workingDir);
Region startRegion, goalRegion;
PathNode startNode, goalNode;
OsmNode? startNode, goalNode;
try
{
startRegion = regionManager.GetRegion(start);
startNode = PathNode.FromNode(ClosestNodeToCoordinates(start, startRegion)!); //TODO null handling
}
catch (FileNotFoundException e)
{
throw new Exception(string.Format("No region at coordinates {0}", start), e);
}
try
{
goalRegion = regionManager.GetRegion(goal);
goalNode = PathNode.FromNode(ClosestNodeToCoordinates(goal, goalRegion)!); //TODO null handling
}
catch (FileNotFoundException e)
{
throw new Exception(string.Format("No region at coordinates {0}", start), e);
throw new Exception(string.Format("No region at coordinates {0}", e.FileName), e);
}
startNode = ClosestNodeToCoordinates(start, startRegion);
goalNode = ClosestNodeToCoordinates(goal, goalRegion);
if (startNode == null || goalNode == null)
return new List<OsmNode>();
List<PathNode> toVisit = new() { startNode };
startNode.DirectDistanceToGoal = Utils.DistanceBetween(startNode, goalNode);
List<OsmNode> toVisit = new() { startNode };
OsmNode closestNodeToGoal;
bool stop = false;
while (toVisit.Count > 0 && !stop)
{
PathNode closestNodeToGoal = toVisit.First();
foreach (PathNode node in toVisit)
Console.WriteLine("toVisit length: {0}", toVisit.Count.ToString());
closestNodeToGoal = toVisit.First();
foreach (OsmNode node in toVisit)
{
if (node.DirectDistanceToGoal.Equals(double.MaxValue))
{
node.DirectDistanceToGoal = Utils.DistanceBetween(node, goalNode);
}
if (node.DirectDistanceToGoal < closestNodeToGoal.DirectDistanceToGoal)
{
closestNodeToGoal = node;
}
}
foreach (Connection connection in closestNodeToGoal.GetConnections())
foreach (OsmEdge connection in closestNodeToGoal.edges)
{
PathNode? neighbor = (PathNode)regionManager.GetNode(connection.endNodeCoordinates);
OsmNode? neighbor = regionManager.GetNode(connection.neighborCoordinates);
Console.WriteLine(neighbor);
if (neighbor != null && neighbor.currentPathWeight < closestNodeToGoal.currentPathWeight + Utils.DistanceBetween(closestNodeToGoal, neighbor))
{
neighbor.previousPathNode = closestNodeToGoal;
@ -60,40 +62,36 @@ public class Pathfinder
toVisit.Add(neighbor);
}
}
toVisit.Remove(closestNodeToGoal);
}
List<PathNode> path = new();
PathNode currentNode = goalNode;
while (!currentNode.Equals(startNode))
List<OsmNode> path = new();
OsmNode? currentNode = goalNode;
while (currentNode != null && !currentNode.Equals(startNode))
{
path.Add(currentNode);
currentNode = currentNode.previousPathNode!;
currentNode = currentNode.previousPathNode;
}
path.Add(startNode);
return path;
}
private static Node? ClosestNodeToCoordinates(Coordinates coordinates, Region region)
private static OsmNode? ClosestNodeToCoordinates(Coordinates coordinates, Region region)
{
ulong? closestId = ClosestNodeIdToCoordinates(coordinates, region);
return closestId != null ? region.GetNode((ulong)closestId) : null;
}
private static ulong? ClosestNodeIdToCoordinates(Coordinates coordinates, Region region)
{
ulong? closestId = null;
double closestDistance = double.MaxValue, distance;
foreach (KeyValuePair<ulong, Node> kv in region.GetNodes())
OsmNode? closest = null;
double distance = double.MaxValue;
foreach (OsmNode node in region.nodes)
{
distance = Utils.DistanceBetween(kv.Value, coordinates);
if (distance < closestDistance)
double nodeDistance = Utils.DistanceBetween(node, coordinates);
if (nodeDistance < distance)
{
closestDistance = distance;
closestId = kv.Key;
closest = node;
distance = nodeDistance;
}
}
return closestId;
return closest;
}
}