OSMServer/Pathfinding/Pathfinder.cs
2023-02-08 19:06:04 +01:00

101 lines
3.4 KiB
C#

using OSMDatastructure;
using OSMImporter;
namespace Pathfinding;
public class Pathfinder
{
public static List<OsmNode> CustomAStar(string workingDir, Coordinates start, Coordinates goal, OsmEdge.speedType vehicle)
{
RegionManager regionManager = new RegionManager(workingDir);
Region startRegion, goalRegion;
try
{
startRegion = regionManager.GetRegion(start);
goalRegion = regionManager.GetRegion(goal);
}
catch (FileNotFoundException e)
{
throw new Exception(string.Format("No region at coordinates {0}", e.FileName), e);
}
OsmNode? startNode = ClosestNodeToCoordinates(start, startRegion);
OsmNode? goalNode = ClosestNodeToCoordinates(goal, goalRegion);
if (startNode == null || goalNode == null)
return new List<OsmNode>();
List<OsmNode> toVisit = new() { startNode };
OsmNode closestNodeToGoal = toVisit.First();
closestNodeToGoal.currentPathWeight = 0;
bool stop = false;
while (toVisit.Count > 0 && !stop)
{
//Console.WriteLine("toVisit-length: {0}", toVisit.Count);
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 (OsmEdge edge in closestNodeToGoal.edges)
{
OsmNode? neighbor = regionManager.GetNode(edge.neighborCoordinates);
if (neighbor != null)
{
double newPotentialWeight =
closestNodeToGoal.currentPathWeight + edge.GetWeight(closestNodeToGoal, vehicle);
if (neighbor.currentPathWeight > newPotentialWeight)
{
neighbor.previousPathNode = closestNodeToGoal;
neighbor.currentPathWeight = newPotentialWeight;
if (neighbor.Equals(goalNode))
stop = true;
else
toVisit.Add(neighbor);
}
}
}
toVisit.Remove(closestNodeToGoal);
}
List<OsmNode> path = new();
OsmNode? currentNode = goalNode;
while (currentNode != null && !currentNode.Equals(startNode))
{
path.Add(currentNode);
currentNode = currentNode.previousPathNode;
}
path.Add(startNode);
path.Reverse();
return path;
}
private static OsmNode? ClosestNodeToCoordinates(Coordinates coordinates, Region region)
{
OsmNode? closest = null;
double distance = double.MaxValue;
foreach (OsmNode node in region.nodes)
{
double nodeDistance = Utils.DistanceBetween(node, coordinates);
if (nodeDistance < distance)
{
closest = node;
distance = nodeDistance;
}
}
return closest;
}
}