OSMServer/Pathfinding/Pathfinder.cs

using OSMDatastructure;
using OSMDatastructure.Graph;
using OSMImporter;

namespace Pathfinding;

public static class Pathfinder
{
    public static List<OsmNode> CustomAStar(string workingDir, Coordinates start, Coordinates goal, Tag.SpeedType vehicle)
    {
        RegionManager regionManager = new RegionManager(workingDir);
        Region? startRegion = regionManager.GetRegion(start);
        Region? goalRegion = regionManager.GetRegion(goal);
        if (startRegion is null || goalRegion is null)
            return new List<OsmNode>();

        OsmNode? startNode = ClosestNodeToCoordinates(start, startRegion, vehicle, ref regionManager);
        OsmNode? goalNode = ClosestNodeToCoordinates(goal, goalRegion, vehicle, ref regionManager);
        if (startNode == null || goalNode == null)
            return new List<OsmNode>();

        PriorityQueue<OsmNode, double> toVisit = new();
        toVisit.Enqueue(startNode, 0);
        startNode.currentPathWeight = 0;
        startNode.currentPathLength = 0;
        startNode.directDistanceToGoal =  Utils.DistanceBetween(startNode, goalNode);
        OsmNode closestNodeToGoal = startNode;
        bool stop = false;

        while (toVisit.Count > 0)
        {
            closestNodeToGoal = toVisit.Dequeue();
            Console.WriteLine($"{toVisit.Count:000} {closestNodeToGoal.directDistanceToGoal:#.00} current:{closestNodeToGoal}");

            foreach (OsmEdge edge in closestNodeToGoal.edges)
            {
                OsmNode? neighbor = regionManager.GetNode(edge.neighborId, edge.neighborRegion);
                if (neighbor is not null)
                {
                    double newPotentialWeight =
                        closestNodeToGoal.currentPathWeight + EdgeWeight(closestNodeToGoal, neighbor, edge.wayId, vehicle, ref regionManager);
                    if (neighbor.currentPathWeight > newPotentialWeight)
                    {
                        neighbor.previousPathNode = closestNodeToGoal;
                        neighbor.currentPathWeight = newPotentialWeight;
                        neighbor.currentPathLength = closestNodeToGoal.currentPathLength + Utils.DistanceBetween(closestNodeToGoal, neighbor);

                        if (neighbor.Equals(goalNode) || closestNodeToGoal.directDistanceToGoal < 10)
                        {
                            stop = true;
                            goalNode = neighbor;
                        }
                        else if(!stop)
                        {
                            neighbor.directDistanceToGoal = Utils.DistanceBetween(neighbor, goalNode);
                            toVisit.Enqueue(neighbor, neighbor.directDistanceToGoal);
                        }
                    }
                }
            }
        }

        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, Tag.SpeedType vehicle, ref RegionManager regionManager)
    {
        OsmNode? closest = null;
        double distance = double.MaxValue;
        foreach (OsmNode node in region.nodes)
        {
            bool hasConnection = false;
            foreach (OsmEdge edge in node.edges)
            {
                double speed = GetSpeed(node, edge.wayId, vehicle, ref regionManager);
                if (speed != 0)
                    hasConnection = true;
            }
            double nodeDistance = Utils.DistanceBetween(node, coordinates);
            if (nodeDistance < distance && hasConnection)
            {
                closest = node;
                distance = nodeDistance;
            }
        }

        return closest;
    }

    private static double GetSpeed(OsmNode node1, ulong wayId, Tag.SpeedType vehicle, ref RegionManager regionManager)
    {
        TagManager tags = regionManager.GetRegion(node1.coordinates)!.tagManager;
        Tag.WayType wayType = (Tag.WayType)tags.GetTag(wayId, Tag.TagType.highway)!;
        switch (vehicle)
        {
            case Tag.SpeedType.pedestrian:
                byte speed = Tag.defaultSpeedPedestrian[wayType];
                if (speed is not 0)
                    return speed;
                return 0;
            case Tag.SpeedType.car: 
            case Tag.SpeedType.road:
                byte? maxspeed = (byte?)tags.GetTag(wayId, Tag.TagType.maxspeed);
                if (maxspeed is not null)
                    return (double)maxspeed;
                maxspeed = Tag.defaultSpeedCar[wayType];
                if(maxspeed is not 0)
                    return (byte)maxspeed;
                return 0;
            default:
                return 0;
        }
    }

    private static double EdgeWeight(OsmNode node1, OsmNode node2, ulong wayId, Tag.SpeedType vehicle, ref RegionManager regionManager)
    {
        double distance = Utils.DistanceBetween(node1, node2);
        double speed = GetSpeed(node1, wayId, vehicle, ref regionManager);
        if (speed is not 0)
            return distance / speed;
        else
            return double.PositiveInfinity;
    }
}