OSMServer/Pathfinding/Pathfinder.cs

using OSMDatastructure;
using OSMDatastructure.Graph;
using OSMImporter;

namespace Pathfinding;

public 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);
        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.Where(node => node.directDistanceToGoal.Equals(Double.MaxValue)))
            {
                node.directDistanceToGoal = Utils.DistanceBetween(node, goalNode);
            }

            closestNodeToGoal = toVisit.OrderBy(node => node.directDistanceToGoal).First();

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

    private static double EdgeWeight(OsmNode node1, OsmNode node2, ulong wayId, Tag.SpeedType vehicle, ref RegionManager regionManager)
    {
        TagManager tags = regionManager.GetRegion(node1.coordinates)!.tagManager;
        double distance = Utils.DistanceBetween(node1, node2);
        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 distance / speed;
                else return Double.PositiveInfinity;
            case Tag.SpeedType.car: 
            case Tag.SpeedType.road:
                byte? maxspeed = (byte?)tags.GetTag(wayId, Tag.TagType.maxspeed);
                if (maxspeed is not null)
                    return distance / Convert.ToDouble(maxspeed);
                else
                    maxspeed = Tag.defaultSpeedCar[wayType];
                if(maxspeed is not 0)
                    return distance / Tag.defaultSpeedCar[wayType];
                else return Double.PositiveInfinity;;
            default:
                return double.PositiveInfinity;
        }
    }
}