OSMServer/Pathfinding/Pathfinder.cs

using OSMDatastructure;
using OSMDatastructure.Graph;
using static OSMDatastructure.Tag;
using WayType = OSMDatastructure.Tag.WayType;

namespace Pathfinding;

public static partial class Pathfinder
{

    private static ValueTuple<OsmNode?, OsmNode?> SetupNodes(Coordinates startCoordinates, Coordinates goalCoordinates, RegionManager regionManager, SpeedType vehicle)
    {
        ValueTuple<OsmNode?, OsmNode?> retTuple = new();
        retTuple.Item1 = regionManager.ClosestNodeToCoordinates(startCoordinates, vehicle);
        retTuple.Item2 = regionManager.ClosestNodeToCoordinates(goalCoordinates, vehicle);
        if (retTuple.Item1 is null || retTuple.Item2 is null)
            return retTuple;
        retTuple.Item1.currentPathWeight = 0;
        retTuple.Item1.currentPathLength = 0;
        retTuple.Item1.directDistanceToGoal =  Utils.DistanceBetween(retTuple.Item1, retTuple.Item2);
        return retTuple;
    }

    private static double EdgeWeight(OsmNode node1, OsmEdge edge, SpeedType vehicle, RegionManager regionManager)
    {
        OsmNode? node2 = regionManager.GetNode(edge.neighborId, edge.neighborRegion);
        if (node2 is null)
            return double.MaxValue;
        double distance = Utils.DistanceBetween(node1, node2);
        byte speed = regionManager.GetSpeedForEdge(node1, edge.wayId, vehicle);
        return speed is 0 ? double.MaxValue : distance / speed;
    }

    private static double GetPriority(OsmNode currentNode, OsmNode? previousNode, OsmEdge currentEdge, SpeedType vehicle, RegionManager regionManager)
    {
        if (vehicle == SpeedType.any)
            return 1;
        const double roadPriorityFactor = 3.5;
        const double roadSpeedFactor = 1.4;
        const double roadNameChangeFactor = 1.4;
        const double distanceDeltaFactor = 0.17;
        Region r = regionManager.GetRegion(currentNode.coordinates)!;

        double distanceDelta = 0;
        if(previousNode is not null)
            distanceDelta = (previousNode.directDistanceToGoal - currentNode.directDistanceToGoal) * distanceDeltaFactor;
        double roadPriority = GetPriorityVehicleRoad(currentEdge, vehicle, r) * roadPriorityFactor;
        double roadSpeed = regionManager.GetSpeedForEdge(currentNode, currentEdge.wayId, vehicle) * 0.1 * roadSpeedFactor;

        if (vehicle == SpeedType.pedestrian)
            return currentNode.directDistanceToGoal / (roadPriority + roadSpeed + distanceDelta);

        double wayChange = 0;
        if (previousNode is not null && previousNode.edges.Count > 0)
        {
            OsmEdge? pEdge = previousNode.edges.FirstOrDefault(e => e.neighborId.Equals(currentNode.nodeId));
            if (pEdge is not null)
            {
                TagManager? prevTags = regionManager.GetRegion(previousNode.coordinates)?.tagManager;
                if (prevTags is not null)
                {
                    TagManager curTags = r.tagManager;

                    bool sameName = false;
                    string? curName = (string?)curTags.GetTag(currentEdge.wayId, TagType.name);
                    if (curName is not null && (string?)prevTags.GetTag(pEdge.wayId, TagType.name) == curName)
                        sameName = true;
                        
                    bool sameRef = false;
                    string? curRef = (string?)curTags.GetTag(currentEdge.wayId, TagType.tagref);
                    if (curRef is not null && (string?)prevTags.GetTag(pEdge.wayId, TagType.tagref) == curRef)
                        sameRef = true;

                    wayChange = (sameRef || sameName ? 1 : 0) * roadNameChangeFactor;
                }
            }
        }

        double div = (roadPriority + wayChange + roadSpeed + (distanceDelta > 0 ? distanceDelta : 0)) + 1;
        double prio = currentNode.directDistanceToGoal / div;
        Console.WriteLine($"{currentNode.directDistanceToGoal:000000.00}/{div:+00.00;-00.00;000.00}={prio:+00000.00;-00000.00;000000.00} Type{roadPriority:00.00} name{wayChange:00.00} speed{roadSpeed:00.00} distance{distanceDelta:+00.00;-00.00;0}");
        if (vehicle == SpeedType.car)
            return prio;

        return double.MaxValue;
    }

    private static double GetPriorityVehicleRoad(OsmEdge edge, SpeedType vehicle, Region region)
    {
        if (vehicle == SpeedType.any)
            return 1;
        WayType? wayType = (WayType?)region.tagManager.GetTag(edge.wayId, TagType.highway);
        if(wayType is null)
            return double.MaxValue;
        if (vehicle == SpeedType.car)
        {
            switch (wayType)
            {
                case WayType.motorway:
                case WayType.motorway_link:
                case WayType.motorroad:
                case WayType.trunk:
                case WayType.trunk_link:
                case WayType.primary:
                case WayType.primary_link:
                    return 8;
                case WayType.secondary:
                case WayType.secondary_link:
                case WayType.tertiary:
                case WayType.tertiary_link:
                    return 5;
                case WayType.unclassified:
                case WayType.residential:
                case WayType.road:
                case WayType.living_street:
                    return 3;
                case WayType.service:
                case WayType.track:
                    return 0.01;
                default:
                    return 1;
            }
        }
        if (vehicle == SpeedType.pedestrian)
        {
            switch (wayType)
            {
                case WayType.pedestrian:
                case WayType.corridor:
                case WayType.footway:
                case WayType.path:
                case WayType.steps:
                case WayType.residential: 
                case WayType.living_street:
                    return 10;
                case WayType.service:
                case WayType.cycleway:
                case WayType.bridleway:
                case WayType.road:
                case WayType.track:
                case WayType.unclassified:
                    return 5;
                case WayType.tertiary:
                case WayType.tertiary_link:
                case WayType.escape:
                    return 2;
                default:
                    return 1;
            }
        }

        return 0.01;

    }

    private static List<PathNode> GetRouteFromCalc(OsmNode goalNode, RegionManager regionManager)
    {
        List<PathNode> path = new();
        OsmNode? currentNode = goalNode;
        while (currentNode is not null)
        {
            HashSet<Tag>? tags = null;
            double pathDistanceDelta = 0;
            double pathWeightDelta = 0;
            double directDistanceDelta = 0;
            if (currentNode.previousPathNode is not null)
            {
                OsmEdge edge = currentNode.previousPathNode!.edges.First(e => e.neighborId.Equals(currentNode.nodeId));
                tags = regionManager.GetRegion(currentNode.coordinates)!.tagManager.GetTagsForWayId(edge.wayId);
                pathDistanceDelta = currentNode.currentPathLength - currentNode.previousPathNode.currentPathLength;
                pathWeightDelta = currentNode.currentPathWeight - currentNode.previousPathNode.currentPathWeight;
                directDistanceDelta =
                    currentNode.directDistanceToGoal - currentNode.previousPathNode.directDistanceToGoal;
            }

            PathNode? pn = PathNode.FromOsmNode(currentNode, tags, pathDistanceDelta, pathWeightDelta, directDistanceDelta);
            if(pn is not null)
                path.Add(pn!);
            currentNode = currentNode.previousPathNode;
        }

        path.Reverse();
        return path;
    }
}