OSMServer/Pathfinding/Pathfinder.cs

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;
        closestNodeToGoal.currentPathLength = 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;
                        neighbor.currentPathLength = closestNodeToGoal.currentPathLength + Utils.DistanceBetween(closestNodeToGoal, neighbor);
                    
                        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;
    }
}