AStar/astar/Astar.cs

using Logging;
using Graph;

namespace astar
{
    public class Astar
    {

        /*
         * Resets the calculated previous nodes and distance to goal
         */
        private static void Reset(ref Dictionary<ulong, Node> nodes)
        {
            foreach(Node n in nodes.Values)
            {
                n.previousNode = null;
                n.goalDistance = float.MaxValue;
                n.timeRequired = float.MaxValue;
            }
        }


        public static Route FindPath(ref Dictionary<ulong, Node> nodes, Node start, Node goal, Logger? logger)
        {
            Route _route = new Route();
            logger?.Log(LogLevel.INFO, "From {0:000.00000}#{1:000.00000} to {2:000.00000}#{3:000.00000} Great-Circle {4}", start.lat, start.lon, goal.lat, goal.lon, Utils.DistanceBetweenNodes(start, goal));
            Reset(ref nodes);
            List<Node> toVisit = new();
            toVisit.Add(start);
            Node currentNode = start;
            start.timeRequired = 0;
            start.goalDistance = Utils.DistanceBetweenNodes(start, goal);
            while (toVisit.Count > 0 && toVisit[0].timeRequired < goal.timeRequired)
            {
                if(currentNode == goal)
                {
                    logger?.Log(LogLevel.INFO, "Way found, checking for shorter option.");
                }
                currentNode = toVisit.First();
                logger?.Log(LogLevel.VERBOSE, "toVisit-length: {0} path-length: {1} goal-distance: {2}", toVisit.Count, currentNode.timeRequired, currentNode.goalDistance);
                //Check all neighbors of current node
                foreach (Edge e in currentNode.edges)
                {
                    if (e.neighbor.timeRequired > currentNode.timeRequired + e.time)
                    {
                        e.neighbor.goalDistance = Utils.DistanceBetweenNodes(e.neighbor, goal);
                        e.neighbor.timeRequired = currentNode.timeRequired + e.time;
                        e.neighbor.previousNode = currentNode;
                        toVisit.Add(e.neighbor);
                    }
                }

                toVisit.Remove(currentNode); //"Mark" as visited
                toVisit.Sort(CompareDistance);
            }
            if(goal.previousNode != null)
            {
                logger?.Log(LogLevel.INFO, "Way found, shortest option.");
                currentNode = goal;
                _route.routeFound = true;
                _route.time = goal.timeRequired;
            }
            else
            {
                logger?.Log(LogLevel.INFO, "No path between {0:000.00000}#{1:000.00000} and {2:000.00000}#{3:000.00000}", start.lat, start.lon, goal.lat, goal.lon);
                _route.routeFound = false;
                return _route;
            }

            List<Node> tempNodes = new();
            tempNodes.Add(goal);
            while(currentNode != start)
            {
                tempNodes.Add(currentNode.previousNode);
                currentNode = currentNode.previousNode;
            }
            tempNodes.Reverse();
            for(int i = 0; i < tempNodes.Count - 1; i++)
            {
                Edge e = tempNodes[i].GetEdgeToNode(tempNodes[i + 1]);
                _route.AddStep(tempNodes[i], e);
                _route.distance += e.distance;
            }

            logger?.Log(LogLevel.INFO, "Path found");

            if(logger?.level > LogLevel.INFO)
            {

                float time = 0;
                float distance = 0;

                logger?.Log(LogLevel.DEBUG, "Route Distance: {0} Time: {1}", _route.distance, TimeSpan.FromSeconds(_route.time));
                for(int i = 0; i < _route.steps.Count; i++)
                {
                    Step s = _route.steps[i];
                    time += s.edge.time;
                    distance += s.edge.distance;
                    logger?.Log(LogLevel.DEBUG, "Step {0:000} From {1:000.00000}#{2:000.00000} To {3:000.00000}#{4:000.00000} along {5}\tafter {6} and {7} m", i, s.start.lat, s.start.lon, s.edge.neighbor.lat, s.edge.neighbor.lon, s.edge.id, TimeSpan.FromSeconds(s.start.timeRequired), distance);
                }
            }

            return _route;
        }
        
        /*
         * Compares two nodes and returns the node closer to the goal
         * -1 => n1 smaller n2
         *  0 => n1 equal n2
         *  1 => n1 larger n2
         */
        private static int CompareDistance(Node n1, Node n2)
        {
            if (n1 == null || n2 == null)
                return 0;
            else
            {
                if (n1.goalDistance < n2.goalDistance)
                    return -1;
                else if (n1.goalDistance > n2.goalDistance)
                    return 1;
                else return 0;
            }
        }

        /*
         * Compares two nodes and returns the node with the shorter path
         * -1 => n1 smaller n2
         *  0 => n1 equal n2
         *  1 => n1 larger n2
         */
        private static int ComparePathLength(Node n1, Node n2)
        {
            if (n1 == null || n2 == null)
                return 0;
            else
            {
                if (n1.timeRequired < n2.timeRequired)
                    return -1;
                else if (n1.timeRequired > n2.timeRequired)
                    return 1;
                else return 0;
            }
        }


    }
}