OSMServer/Pathfinding/Pathfinder.cs

using OSMDatastructure;
using OSMImporter;

namespace Pathfinding;

public class Pathfinder
{

    public static List<OsmNode> CustomAStar(string workingDir, Coordinates start, Coordinates goal)
    {
        RegionManager regionManager = new RegionManager(workingDir);
        Region startRegion, goalRegion;
        OsmNode? startNode, goalNode;
        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);
        }

        startNode = ClosestNodeToCoordinates(start, startRegion);
        goalNode = ClosestNodeToCoordinates(goal, goalRegion);
        if (startNode == null || goalNode == null)
            return new List<OsmNode>();
        
        List<OsmNode> toVisit = new() { startNode };
        OsmNode closestNodeToGoal;
        bool stop = false;

        while (toVisit.Count > 0 && !stop)
        {
            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 (OsmEdge connection in closestNodeToGoal.edges)
            {
                OsmNode? neighbor = regionManager.GetNode(connection.neighborCoordinates);
                Console.WriteLine(neighbor);
                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);
                }
            }

            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);

        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;
    }
}