graph.h

Go to the documentation of this file.

#pragma once
 
#include <functional>
#include <iostream>
#include <memory>
#include <queue>
#include <vector>
 
namespace Routemaker
{
 
    /// \brief Represents a node in a Graph data structured made for
    /// path-finding
    ///
    /// \tparam T Type of data to store inside the node
    template<typename T>
    struct Node
    {
        /// \brief Data stored in the the node.
        ///
        /// Stores data not needed by the A\* path-finding algorithm. This is
        /// what the user actually wants to store in the \ref Graph.
        T Data;
 
        /// \brief A non-owner pointer to the parent of the node.
        ///
        /// Should not be set by user. The A\* path-finding algorithm sets the
        /// value for this member when traversing the \ref Graph. It used to
        /// find the way back to the start after the goal is found.
        std::weak_ptr<Node<T>> Parent;
 
        /// \brief Specifies if a given node has been visited during
        /// path-finding.
        ///
        /// Should not be set by user. Is generally only used internally by the
        /// A\* path-finding algorithm when traversing the \ref Graph. May be
        /// used in debug views to visualize which nodes are visited during
        /// path-finding.
        bool Visited;
 
        /// \brief Represents the assumed cost from the start to the goal node
        /// through this node.
        ///
        /// Should not be set by the user.
        /// The A\* path-finding algorithm uses cost to find the shortest path
        /// in a reasonable amount of time. This member contains the sum of the
        /// cost to get to this node from the start node, represented in \ref
        /// LocalGoal, plus the assumed cost to get from this node to the goal
        /// node. The A\* path-finding algorithm uses this value during \ref
        /// Graph traversal to sort a priority queue in order to explore the
        /// assumed shortest paths first.
        double GlobalGoal;
 
        /// \brief Represents the cost from the start node to this node.
        ///
        /// Should not be set by the user.
        /// The A\* path-finding algorithm uses cost to find the shortest path
        /// in a reasonable amount of time. This member contains the sum of the
        /// cost to get to this node from the start node. While traversing the
        /// \ref Graph, the A\* path-finding algorithm updates and uses this
        /// member to check for shorter paths.
        double LocalGoal;
    };
 
    /// \brief Abstract graph interface optimized for path-finding.
    ///
    /// \tparam T Type of user data to store in each node
    ///
    /// This interface is designed to be flexible and scalable. The sub-classes
    /// are required to implement a few methods, such as \ref Graph.GetNeighbors
    /// and \ref Graph.GetCost for the A\* path-finding algorithm to work.
    template<typename T>
    class Graph
    {
      public:
        using NodePtr = std::shared_ptr<Node<T>>; ///< Helper alias to make code
                                                  ///< more readable.
 
      public:
        /// \brief Collects all neighbor nodes of \p node.
        ///
        /// Implemented by sub-classes of Graph.
        /// The neighbor relationship between nodes define the edges of the
        /// graph. It is up to the subclass to define these relationships. For a
        /// 2D grid, the neighbors would simply be the nodes directly to the
        /// north, south, east and west, in addition to the corners between
        /// them. For a road network, the relationships may be more complex.
        ///
        /// \param node A pointer to the node from which to collect all
        /// neighbors \return A vector of pointers to all the neighbors of \p
        /// node
        virtual std::vector<NodePtr> GetNeighbors(NodePtr node) = 0;
 
        /// \brief Returns the cost between \p a and \p b.
        ///
        /// Implemented by sub-classes of Graph.
        /// The a\* path-finding algorithm uses cost to efficiently find the
        /// best path between two nodes. In order to do this, it requires some
        /// method of calculating the cost of moving between any two nodes. It
        /// is up to the sub-class to define how this is calulated. An example
        /// of this cost may be the euclidean distance between two nodes.
        ///
        /// \param a Pointer to the first \ref Node
        /// \param b Pointer to the second \ref Node
        /// \return Cost between node \p a and node \p b.
        virtual double GetCost(NodePtr a, NodePtr b) = 0;
 
        /// \brief Determines if there is a direct line of sight between node \p
        /// a and node \p b.
        ///
        /// Implemented by sub-classes of Graph.
        /// The \ref Graph.PostSmooth method traverses the already found path
        /// through the A\* path-finding algorithm and simplifies it by using
        /// this method. In a graph representing a 2D grid, a Bresenham
        /// implementation or ray-casting can be used to determine line of
        /// sight.
        ///
        /// \param a Pointer to the first \ref Node
        /// \param b Pointer to the second \ref Node
        /// \return bool specifying whether or not there is a direct line of
        /// sight
        virtual bool HasLineOfSight(NodePtr a, NodePtr b) = 0;
 
        /// \brief Resets all local and global goals and parent relationships of
        /// all nodes.
        ///
        /// Implemented by sub-classes of Graph.
        /// In order to be able to re-use the same graph for several A\*
        /// searches, the \ref Graph.SolveAStar method needs to be able to reset
        /// all the nodes. As this interface does not contain the actual
        /// collection of nodes, this needs to be implemented in the
        /// sub-classes.
        virtual void ResetNodes(void) = 0;
 
        /// \brief Finds *cheapest* path from \p start to \p goal.
        ///
        /// \param start Pointer to the node to start the path from
        /// \param goal  Pointer to the node to find the path to
        ///
        /// Using the A\* algorithm, this method explores the graph's nodes and
        /// updates their local and global goals, their visited flags, as well
        /// as their parent relationships.
        ///
        /// When the algorithm finishes, given that a path exists between the
        /// nodes, the cheapest path between them is defined by the parent
        /// relationships. The path can be *extracted* by starting at the \p
        /// goal and following the \ref Node.Parent pointers until \p start is
        /// reached, saving each node in a list and reversing it at the end.
        void SolveAStar(NodePtr start, NodePtr goal);
 
        /// \brief Simplifies the path from \p start to \p goal.
        ///
        /// \param start Pointer to the start node of the path
        /// \param goal  Pointer to the end node of the path
        ///
        /// Should be run on the same nodes as \ref Graph.SolveAStar, and should
        /// only be called after \ref Graph.SolveAStar has finished.
        ///
        ///
        void PostSmooth(NodePtr start, NodePtr goal);
    };
 
    template<typename T>
    void
    Graph<T>::SolveAStar(NodePtr start, NodePtr goal)
    {
        ResetNodes(); // Make sure all relational values are reset
 
        NodePtr current = start;
        current->LocalGoal = 0.0;
        current->GlobalGoal = GetCost(current, goal);
 
        // Create a priority queue that compares nodes' global goal value
        std::priority_queue<NodePtr, std::vector<NodePtr>,
                            std::function<bool(NodePtr, NodePtr)>>
            notTested([](NodePtr a, NodePtr b) {
                return a->GlobalGoal > b->GlobalGoal;
            });
 
        notTested.push(start);
 
        // Let's go for all long as we have untested discovered nodes
        while (!notTested.empty()) {
            // But in case we have already discovered nodes in our list, let's
            // remove them
            while (!notTested.empty() && notTested.top()->Visited) {
                notTested.pop();
            }
 
            // If we ended up removing some nodes, and we are now out of
            // untested nodes, let's break from the loop
            if (notTested.empty()) {
                break;
            }
 
            current = notTested.top();
            current->Visited = true;
 
            for (auto neighbor : GetNeighbors(current)) {
                // We only want to explore unoccupied cells.
                if (!neighbor->Visited && !neighbor->Data.Occupied) {
                    notTested.push(neighbor);
                }
 
                // Let's calculate the cost of the travel to this node so far +
                // the cost to get from here to the neighbor, and update the
                // neighbors relational values if it is a new record for the
                // neighbor.
                double candidateGoal =
                    current->LocalGoal + GetCost(current, neighbor);
                if (candidateGoal < neighbor->LocalGoal) {
                    neighbor->Parent = current;
                    neighbor->LocalGoal = candidateGoal;
                    neighbor->GlobalGoal =
                        neighbor->LocalGoal + GetCost(neighbor, goal);
                }
            }
        }
    }
 
    // Quite a simple little algorithm to simplify and smooth out a path found
    // through A*: We just start at the goal, and check if we have a direct line
    // of sight to our grandparent. If we do, then we can remove the middle man,
    // our parent, from the equation and make our grandparent our parent
    // instead. Then check again for our new grandparent. If we do not have a
    // direct line of sight to our grandparent, we move on to our parent and
    // check its grandparent. We do this recursively until we reach the start
    // node.
    template<typename T>
    void
    Graph<T>::PostSmooth(NodePtr start, NodePtr goal)
    {
        NodePtr current = goal;
        NodePtr parent = current->Parent.lock();
        while (current && parent && (current != start)) {
            NodePtr grandParent = parent->Parent.lock();
            if (!grandParent) {
                break;
            }
            if (HasLineOfSight(current, grandParent)) {
                current->Parent = grandParent;
                parent = grandParent;
                continue;
            }
            current = parent;
            parent = current->Parent.lock();
        }
    }
 
} // namespace Routemaker