OSMServer/Pathfinding/Pathfinder.cs

using OSMDatastructure;
using OSMDatastructure.Graph;

namespace Pathfinding;

public static partial class Pathfinder
{

    private static ValueTuple<OsmNode?, OsmNode?> SetupNodes(Coordinates startCoordinates, Coordinates goalCoordinates, RegionManager regionManager )
    {
        ValueTuple<OsmNode?, OsmNode?> retTuple = new();
        retTuple.Item1 = regionManager.ClosestNodeToCoordinates(startCoordinates, Tag.SpeedType.any);
        retTuple.Item2 = regionManager.ClosestNodeToCoordinates(goalCoordinates, Tag.SpeedType.any);
        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, Tag.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);
        double speed = regionManager.GetSpeedForEdge(node1, edge.wayId, vehicle);
        if (speed is 0)
            return double.MaxValue;
        return distance / speed;
    }

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