OSMServer/Pathfinding/Pathfinder.cs

99 lines
3.4 KiB
C#
Raw Normal View History

2023-02-03 23:35:22 +01:00
using OSMDatastructure;
using OSMImporter;
namespace Pathfinding;
public class Pathfinder
{
2023-02-05 20:03:47 +01:00
public static List<PathNode> CustomAStar(string workingDir, Coordinates start, Coordinates goal)
2023-02-03 23:35:22 +01:00
{
RegionManager regionManager = new RegionManager(workingDir);
2023-02-05 20:03:47 +01:00
Region startRegion, goalRegion;
PathNode startNode, goalNode;
try
{
startRegion = regionManager.GetRegion(start);
2023-02-05 20:45:12 +01:00
startNode = PathNode.FromNode(ClosestNodeToCoordinates(start, startRegion)!); //TODO null handling
2023-02-05 20:03:47 +01:00
}
catch (FileNotFoundException e)
{
throw new Exception(string.Format("No region at coordinates {0}", start), e);
}
try
{
goalRegion = regionManager.GetRegion(goal);
2023-02-05 20:45:12 +01:00
goalNode = PathNode.FromNode(ClosestNodeToCoordinates(goal, goalRegion)!); //TODO null handling
2023-02-05 20:03:47 +01:00
}
catch (FileNotFoundException e)
{
throw new Exception(string.Format("No region at coordinates {0}", start), e);
}
2023-02-03 23:35:22 +01:00
2023-02-05 20:03:47 +01:00
List<PathNode> toVisit = new() { startNode };
startNode.DirectDistanceToGoal = Utils.DistanceBetween(startNode, goalNode);
bool stop = false;
while (toVisit.Count > 0 && !stop)
{
PathNode closestNodeToGoal = toVisit.First();
foreach (PathNode node in toVisit)
{
if (node.DirectDistanceToGoal < closestNodeToGoal.DirectDistanceToGoal)
{
closestNodeToGoal = node;
}
}
foreach (Connection connection in closestNodeToGoal.GetConnections())
{
2023-02-05 20:45:12 +01:00
PathNode? neighbor = (PathNode)regionManager.GetNode(connection.endNodeCoordinates);
2023-02-05 20:03:47 +01:00
if (neighbor != null && neighbor.currentPathWeight < closestNodeToGoal.currentPathWeight + Utils.DistanceBetween(closestNodeToGoal, neighbor))
{
neighbor.previousPathNode = closestNodeToGoal;
neighbor.currentPathWeight = closestNodeToGoal.currentPathWeight +
Utils.DistanceBetween(closestNodeToGoal, neighbor);
if (neighbor.Equals(goalNode))
stop = true;
else
toVisit.Add(neighbor);
}
}
}
List<PathNode> path = new();
PathNode currentNode = goalNode;
while (!currentNode.Equals(startNode))
{
path.Add(currentNode);
currentNode = currentNode.previousPathNode!;
}
path.Add(startNode);
return path;
2023-02-03 23:35:22 +01:00
}
private static Node? 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())
{
distance = Utils.DistanceBetween(kv.Value, coordinates);
if (distance < closestDistance)
{
closestDistance = distance;
closestId = kv.Key;
}
}
return closestId;
}
}