A* versus Bidirectional Dijkstra's algorithm

Question

I have added bidirectional Dijkstra's algorithm into my pathfinding "framework", and I would like to make good use of C++ programming idioms, eliminate all possible memory leaks, otherwise improve readability, but I need your help for that to happen.

That's what I have scrambled:

shortest_path.h:

#ifndef SHORTEST_PATH_H
#define SHORTEST_PATH_H

#include <queue>
#include <string>
#include <unordered_map>
#include <unordered_set>
#include <vector>

namespace coderodde {

    template<class NodeType>
    class AbstractGraphNode {
    protected:

        using Set = std::unordered_set<NodeType*>;

    public:

        AbstractGraphNode(std::string name) : m_name{name} {}
        virtual void connect_to(NodeType* other) = 0;
        virtual bool is_connected_to(NodeType* other) const = 0;
        virtual void disconnect_from(NodeType* other) = 0;
        virtual typename Set::iterator begin() const = 0;
        virtual typename Set::iterator end() const = 0;

        class ParentIterator {
        public:

            ParentIterator() : mp_set{nullptr} {}

            typename Set::iterator begin() 
            {
                return mp_set->begin();
            }

            typename Set::iterator end()  
            {
                return mp_set->end();
            }

            void set_list(Set* p_list)
            {
                this->mp_set = p_list;
            }

        private:

            std::unordered_set<NodeType*>* mp_set;
        };

        virtual ParentIterator* parents() = 0;

        bool operator==(const NodeType& other) const
        {
            return m_name == other.m_name;
        }

        std::string& get_name() {return m_name;}

    protected:

        std::string m_name;
    };

    template<class T, class FloatType = double>
    class AbstractWeightFunction {
    public:

        virtual FloatType& operator()(T* p_node1, T* p_node2) = 0;
    };

    template<class FloatType>
    class Point3D {
    private:
        const FloatType m_x;
        const FloatType m_y;
        const FloatType m_z;

    public:
        Point3D(const FloatType x = FloatType(),
                const FloatType y = FloatType(),
                const FloatType z = FloatType())
                                    :
                                    m_x{x},
                                    m_y{y},
                                    m_z{z} {}

        FloatType x() const {return m_x;}
        FloatType y() const {return m_y;}
        FloatType z() const {return m_z;}
    };

    template<class FloatType>
    class AbstractMetric {
    public:

        virtual FloatType operator()(coderodde::Point3D<FloatType>& p1,
                                     coderodde::Point3D<FloatType>& p2) = 0;
    };

    template<class FloatType>
    class EuclideanMetric : public coderodde::AbstractMetric<FloatType> {
    public:

        FloatType operator()(coderodde::Point3D<FloatType>& p1,
                             coderodde::Point3D<FloatType>& p2) {
            const FloatType dx = p1.x() - p2.x();
            const FloatType dy = p1.y() - p2.y();
            const FloatType dz = p1.z() - p2.z();

            return std::sqrt(dx * dx + dy * dy + dz * dz);
        }
    };

    template<class T, class FloatType = double>
    class LayoutMap {
    public:

        virtual coderodde::Point3D<FloatType>*& operator()(T* key)
        {
            return m_map[key];
        }

        ~LayoutMap() 
        {
            typedef typename std::unordered_map<T*, 
                             coderodde::Point3D<FloatType>*>::iterator it_type;
            for (it_type iterator = m_map.begin(); 
                    iterator != m_map.end(); iterator++)
            {
                delete iterator->second;
            }
        }

    private:

        std::unordered_map<T*, coderodde::Point3D<FloatType>*> m_map;
    };

    template<class NodeType, class DistanceType = double>
    class HeapNode {
    public:
        HeapNode(NodeType* p_node, DistanceType distance) :
            mp_node{p_node},
            m_distance{distance} {}

        NodeType* get_node()
        {
            return mp_node;
        }

        DistanceType get_distance()
        {
            return m_distance;
        }

    private:
        NodeType*    mp_node;
        DistanceType m_distance;
    };

    template<class NodeType, class DistanceType = double>
    class HeapNodeComparison {
    public:

        bool operator()(HeapNode<NodeType, DistanceType>* p_first,
                        HeapNode<NodeType, DistanceType>* p_second)
        {
            return p_first->get_distance() > p_second->get_distance();
        }
    };

    template<class NodeType, class FloatType = double>
    class DistanceMap {
    public:

        FloatType& operator()(const NodeType* p_node)
        {
            return m_map[p_node];
        }

    private:

        std::unordered_map<const NodeType*, FloatType> m_map;
    };

    template<class NodeType>
    class ParentMap {
    public:

        NodeType*& operator()(const NodeType* p_node)
        {
            return m_map[p_node];
        }

        bool has(NodeType* p_node)
        {
            return m_map.find(p_node) != m_map.end();
        }

    private:

        std::unordered_map<const NodeType*, NodeType*> m_map;
    };

    template<class NodeType>
    std::vector<NodeType*>* traceback_path(NodeType* p_touch,
                                           ParentMap<NodeType>* parent_map1,
                                           ParentMap<NodeType>* parent_map2 = nullptr)
    {
        std::vector<NodeType*>* p_path = new std::vector<NodeType*>();
        NodeType* p_current = p_touch;

        while (p_current != nullptr)
        {
            p_path->push_back(p_current);
            p_current = (*parent_map1)(p_current);
        }

        std::reverse(p_path->begin(), p_path->end());

        if (parent_map2 != nullptr)
        {
            p_current = (*parent_map2)(p_touch);

            while (p_current != nullptr)
            {
                p_path->push_back(p_current);
                p_current = (*parent_map2)(p_current);
            }
        }

        return p_path;
    }

    template<class T, class FloatType = double>
    class HeuristicFunction {

    public:
        HeuristicFunction(T* p_target_element,
                          LayoutMap<T, FloatType>& layout_map,
                          AbstractMetric<FloatType>& metric)
        :
        mp_layout_map{&layout_map},
        mp_metric{&metric},
        mp_target_point{layout_map(p_target_element)}
        {

        }

        FloatType operator()(T* element)
        {
            return (*mp_metric)(*(*mp_layout_map)(element), *mp_target_point);
        }

    private:
        coderodde::LayoutMap<T, FloatType>*   mp_layout_map;
        coderodde::AbstractMetric<FloatType>* mp_metric;
        coderodde::Point3D<FloatType>*        mp_target_point;
    };

    template<class NodeType, class WeightType = double>
    std::vector<NodeType*>* 
    astar(NodeType* p_source,
          NodeType* p_target,
          coderodde::AbstractWeightFunction<NodeType, WeightType>& w,
          coderodde::LayoutMap<NodeType, WeightType>& layout_map,
          coderodde::AbstractMetric<WeightType>& metric)
    {
        std::priority_queue<HeapNode<NodeType, WeightType>*,
                            std::vector<HeapNode<NodeType, WeightType>*>,
                            HeapNodeComparison<NodeType, WeightType>> OPEN;

        std::unordered_set<NodeType*> CLOSED;

        coderodde::HeuristicFunction<NodeType,
                                     WeightType> h(p_target,
                                                   layout_map,
                                                   metric);
        DistanceMap<NodeType, WeightType> d;
        ParentMap<NodeType> p;

        OPEN.push(new HeapNode<NodeType, WeightType>(p_source, WeightType(0)));
        p(p_source) = nullptr;
        d(p_source) = WeightType(0);

        while (!OPEN.empty())
        {
            HeapNode<NodeType, WeightType>* p_heap_node = OPEN.top();
            NodeType* p_current = p_heap_node->get_node();
            OPEN.pop();
            delete p_heap_node;

            if (*p_current == *p_target)
            {
                // Found the path.
                return traceback_path(p_target, &p);
            }

            CLOSED.insert(p_current);

            // For each child of 'p_current' do...
            for (NodeType* p_child : *p_current)
            {

                if (CLOSED.find(p_child) != CLOSED.end())
                {
                    // The optimal distance from source to p_child is known.
                    continue;
                }

                WeightType cost = d(p_current) + w(p_current, p_child);

                if (!p.has(p_child) || cost < d(p_child))
                {
                    WeightType f = cost + h(p_child);
                    OPEN.push(new HeapNode<NodeType, WeightType>(p_child, f));
                    d(p_child) = cost;
                    p(p_child) = p_current;
                }
            }
        }

        // p_target not reachable from p_source.
        return nullptr;
    }

    template<class T, class FloatType>
    class ConstantLayoutMap : public coderodde::LayoutMap<T, FloatType> {
    public:

        ConstantLayoutMap() : mp_point{new Point3D<FloatType>()} {}

        ~ConstantLayoutMap() 
        {
            delete mp_point;
        }

        Point3D<FloatType>*& operator()(T* key)
        {
            return mp_point;
        }

    private:

        Point3D<FloatType>* mp_point;
    };

    /***************************************************************************
    * This function template implements Dijkstra's shortest path algorithm.    *
    ***************************************************************************/
    template<class NodeType, class WeightType = double>
    std::vector<NodeType*>*
    dijkstra(NodeType* p_source,
             NodeType* p_target,
             coderodde::AbstractWeightFunction<NodeType, WeightType>& w)
    {
        ConstantLayoutMap<NodeType, WeightType> layout;
        EuclideanMetric<WeightType> metric;

        return astar(p_source,
                     p_target,
                     w,
                     layout,
                     metric);
    }

    template<class NodeType, class WeightType = double>
    std::vector<NodeType*>*
    bidirectional_dijkstra(
        NodeType* p_source,
        NodeType* p_target,
        coderodde::AbstractWeightFunction<NodeType, WeightType>& w) 
    {
        std::priority_queue<HeapNode<NodeType, WeightType>*,
                            std::vector<HeapNode<NodeType, WeightType>*>,
                            HeapNodeComparison<NodeType, WeightType>> OPENA;

        std::priority_queue<HeapNode<NodeType, WeightType>*,
                            std::vector<HeapNode<NodeType, WeightType>*>,
                            HeapNodeComparison<NodeType, WeightType>> OPENB;

        std::unordered_set<NodeType*> CLOSEDA;
        std::unordered_set<NodeType*> CLOSEDB;

        DistanceMap<NodeType, WeightType> DISTANCEA;
        DistanceMap<NodeType, WeightType> DISTANCEB;

        ParentMap<NodeType> PARENTA;
        ParentMap<NodeType> PARENTB;

        OPENA.push(new HeapNode<NodeType, WeightType>(p_source, 0.0));
        OPENB.push(new HeapNode<NodeType, WeightType>(p_target, 0.0));

        DISTANCEA(p_source) = WeightType(0);
        DISTANCEB(p_target) = WeightType(0);

        PARENTA(p_source) = nullptr;
        PARENTB(p_target) = nullptr;

        NodeType* p_touch = nullptr;
        WeightType best_cost = std::numeric_limits<WeightType>::max();

        while (!OPENA.empty() && !OPENB.empty())
        {
            if (OPENA.top()->get_distance() + 
                OPENB.top()->get_distance() >= best_cost)
            {
                return traceback_path(p_touch, &PARENTA, &PARENTB);
            }

            if (OPENA.top()->get_distance() < OPENB.top()->get_distance())
            {
                HeapNode<NodeType, WeightType>* p_heap_node = OPENA.top();
                NodeType* p_current = p_heap_node->get_node();
                OPENA.pop();
                delete p_heap_node;

                CLOSEDA.insert(p_current);

                for (NodeType* p_child : *p_current)
                {
                    if (CLOSEDA.find(p_child) != CLOSEDA.end()) 
                    {
                        continue;
                    }

                    WeightType g = DISTANCEA(p_current) + w(p_current, p_child);

                    if (!PARENTA.has(p_child) || g < DISTANCEA(p_current))
                    {
                        OPENA.push(new HeapNode<NodeType, 
                                                WeightType>(p_child, g));
                        DISTANCEA(p_child) = g;
                        PARENTA(p_child) = p_current;

                        if (CLOSEDB.find(p_child) != CLOSEDB.end())
                        {
                            WeightType path_len = g + DISTANCEB(p_child);

                            if (best_cost > path_len)
                            {
                                best_cost = path_len;
                                p_touch = p_child;
                            }
                        }
                    }
                }
            } 
            else
            {
                HeapNode<NodeType, WeightType>* p_heap_node = OPENB.top();
                NodeType* p_current = p_heap_node->get_node();
                OPENB.pop();
                delete p_heap_node;

                CLOSEDB.insert(p_current);

                typename coderodde::AbstractGraphNode<NodeType>::ParentIterator*
                        p_iterator = p_current->parents();

                for (NodeType* p_parent : *p_iterator)
                {
                    if (CLOSEDB.find(p_parent) != CLOSEDB.end()) 
                    {
                        continue;
                    }

                    WeightType g = DISTANCEB(p_current) + 
                                   w(p_parent, p_current);

                    if (!PARENTB.has(p_parent) || g < DISTANCEB(p_parent))
                    {
                        OPENB.push(new HeapNode<NodeType, 
                                                WeightType>(p_parent, g));
                        DISTANCEB(p_parent) = g;
                        PARENTB(p_parent) = p_current;

                        if (CLOSEDA.find(p_parent) != CLOSEDA.end())
                        {
                            WeightType path_len = g + DISTANCEA(p_parent);

                            if (best_cost > path_len)
                            {
                                best_cost = path_len;
                                p_touch = p_parent;
                            }
                        }
                    }
                }
            }
        }

        return nullptr;
    }

    class DirectedGraphNode : public coderodde::AbstractGraphNode<DirectedGraphNode> {
    public:

        DirectedGraphNode(std::string name) :
                coderodde::AbstractGraphNode<DirectedGraphNode>(name)
        {
            this->m_name = name;
        }

        void connect_to(coderodde::DirectedGraphNode* p_other)
        {
            m_out.insert(p_other);
            p_other->m_in.insert(this);
        }

        bool is_connected_to(coderodde::DirectedGraphNode* p_other) const
        {
            return m_out.find(p_other) != m_out.end();
        }

        void disconnect_from(coderodde::DirectedGraphNode* p_other)
        {
            m_out.erase(p_other);
            p_other->m_in.erase(this);
        }

        ParentIterator* parents() 
        {
            m_iterator.set_list(&m_in);
            return &m_iterator;
        }

        typename Set::iterator begin() const
        {
            return m_out.begin();
        }

        typename Set::iterator end() const
        {
            return m_out.end();
        }

        friend std::ostream& operator<<(std::ostream& out,
                                        DirectedGraphNode& node) 
        {
            return out << "[DirectedGraphNode " << node.get_name() << "]";
        }

    private:
        Set m_in;
        Set m_out;
        ParentIterator m_iterator;
    };

    class DirectedGraphWeightFunction :
    public AbstractWeightFunction<coderodde::DirectedGraphNode, double> {

    public:

        double& operator()(coderodde::DirectedGraphNode* node1,
                           coderodde::DirectedGraphNode* node2)
        {
            if (m_map.find(node1) == m_map.end())
            {
                m_map[node1] =
                new std::unordered_map<coderodde::DirectedGraphNode*,
                                       double>();
            }

            return (*m_map.at(node1))[node2];
        }

    private:

        std::unordered_map<coderodde::DirectedGraphNode*,
        std::unordered_map<coderodde::DirectedGraphNode*, double>*> m_map;
    };
}

#endif // SHORTEST_PATH_H

main.cpp:

#include <iostream>
#include <random>
#include <string>
#include <tuple>
#include <vector>

#include "shortest_path.h"

using std::cout;
using std::endl;
using std::get;
using std::make_tuple;
using std::mt19937;
using std::random_device;
using std::string;
using std::to_string;
using std::tuple;
using std::vector;
using std::uniform_int_distribution;
using std::uniform_real_distribution;

using std::chrono::duration_cast;
using std::chrono::milliseconds;
using std::chrono::system_clock;

using coderodde::astar;
using coderodde::bidirectional_dijkstra;
using coderodde::dijkstra;
using coderodde::DirectedGraphNode;
using coderodde::DirectedGraphWeightFunction;
using coderodde::EuclideanMetric;
using coderodde::HeuristicFunction;
using coderodde::LayoutMap;
using coderodde::Point3D;

/*******************************************************************************
* Randomly selects an element from a vector.                                   *
*******************************************************************************/
template<class T>
T& choose(vector<T>& vec, mt19937& rnd_gen)
{
    uniform_int_distribution<size_t> dist(0, vec.size() - 1);
    return vec[dist(rnd_gen)];
}

/*******************************************************************************
* Creates a random point in a plane.                                           *
*******************************************************************************/
static Point3D<double>* create_random_point(const double xlen,
                                            const double ylen,
                                            mt19937& random_engine)
{
    uniform_real_distribution<double> xdist(0.0, xlen);
    uniform_real_distribution<double> ydist(0.0, ylen);

    return new Point3D<double>(xdist(random_engine),
                               ydist(random_engine),
                               0.0);
}

/*******************************************************************************
* Creates a random directed, weighted graph.                                   *
*******************************************************************************/
static tuple<vector<DirectedGraphNode*>*,
             DirectedGraphWeightFunction*,
             LayoutMap<DirectedGraphNode, double>*>
    create_random_graph(const size_t length,
                        const double area_width,
                        const double area_height,
                        const float arc_load_factor,
                        const float distance_weight,
                        mt19937 random_gen)
{
    vector<DirectedGraphNode*>* p_vector = new vector<DirectedGraphNode*>();
    LayoutMap<DirectedGraphNode, double>* p_layout =
    new LayoutMap<DirectedGraphNode, double>();

    for (size_t i = 0; i < length; ++i)
    {
        DirectedGraphNode* p_node = new DirectedGraphNode(to_string(i));
        p_vector->push_back(p_node);
    }

    for (DirectedGraphNode* p_node : *p_vector)
    {
        Point3D<double>* p_point = create_random_point(area_width,
                                                       area_height,
                                                       random_gen);
        (*p_layout)(p_node) = p_point;
    }

    DirectedGraphWeightFunction* p_wf = new DirectedGraphWeightFunction();
    EuclideanMetric<double> euclidean_metric;

    size_t arcs = arc_load_factor > 0.9 ?
    length * (length - 1) :
    (arc_load_factor < 0.0 ? 0 : size_t(arc_load_factor * length * length));

    while (arcs > 0)
    {
        DirectedGraphNode* p_head = choose(*p_vector, random_gen);
        DirectedGraphNode* p_tail = choose(*p_vector, random_gen);

        Point3D<double>* p_head_point = (*p_layout)(p_head);
        Point3D<double>* p_tail_point = (*p_layout)(p_tail);

        const double cost = euclidean_metric(*p_head_point,
                                             *p_tail_point);


        (*p_wf)(p_tail, p_head) = distance_weight * cost;
        p_tail->connect_to(p_head);

        --arcs;
    }

    return make_tuple(p_vector, p_wf, p_layout);
}

/*******************************************************************************
* Returns the amount of milliseconds since Unix epoch.                         *
*******************************************************************************/
static unsigned long long get_milliseconds()
{
    return duration_cast<milliseconds>(system_clock::now()
                                       .time_since_epoch()).count();
}

/*******************************************************************************
* Checks that a path has all needed arcs.                                      *
*******************************************************************************/
static bool is_valid_path(vector<DirectedGraphNode*>* p_path)
{
    for (size_t i = 0; i < p_path->size() - 1; ++i)
    {
        if (!(*p_path)[i]->is_connected_to((*p_path)[i + 1]))
        {
            return false;
        }
    }

    return true;
}

/*******************************************************************************
* Computes the length (cost) of a path.                                        *
*******************************************************************************/
static double compute_path_length(vector<DirectedGraphNode*>* p_path,
                                  DirectedGraphWeightFunction* p_wf)
{
    double cost = 0.0;

    for (size_t i = 0; i < p_path->size() - 1; ++i)
    {
        cost += (*p_wf)(p_path->at(i), p_path->at(i + 1));
    }

    return cost;
}

/*******************************************************************************
* The demo.                                                                    *
*******************************************************************************/
int main(int argc, const char * argv[]) {
    random_device rd;
    mt19937 random_gen(rd());

    cout << "Building a graph..." << endl;

    tuple<vector<DirectedGraphNode*>*,
          DirectedGraphWeightFunction*,
          LayoutMap<DirectedGraphNode, double>*> graph_data =
    create_random_graph(50000,
                        1000.0,
                        700.0,
                        0.0001f,
                        1.2f,
                        random_gen);

    DirectedGraphNode *const p_source = choose(*std::get<0>(graph_data),
                                               random_gen);

    DirectedGraphNode *const p_target = choose(*std::get<0>(graph_data),
                                               random_gen);

    cout << "Source: " << *p_source << endl;
    cout << "Target: " << *p_target << endl;

    EuclideanMetric<double> em;

    unsigned long long ta = get_milliseconds();

    vector<DirectedGraphNode*>* p_path1 = 
            astar(p_source,
                  p_target,
                  *get<1>(graph_data),
                  *get<2>(graph_data),
                  em);

    unsigned long long tb = get_milliseconds();

    cout << endl;
    cout << "A* path:" << endl;

    if (!p_path1)
    {
        cout << "No path for A*!" << endl;
        return 0;
    }

    for (DirectedGraphNode* p_node : *p_path1)
    {
        cout << *p_node << endl;
    }

    cout << "Time elapsed: " << tb - ta << " ms." << endl;
    cout << std::boolalpha;
    cout << "Is valid path: " << is_valid_path(p_path1) << endl;
    cout << "Cost: " << compute_path_length(p_path1, get<1>(graph_data)) << endl;

    cout << endl;
    cout << "Dijkstra path:" << endl;

    ta = get_milliseconds();

    vector<DirectedGraphNode*>* p_path2 =
            dijkstra(p_source,
                     p_target,
                     *get<1>(graph_data));

    tb = get_milliseconds();

    if (!p_path2)
    {
        cout << "No path for Dijkstra's algorithm!" << endl;
        return 0;
    }

    for (DirectedGraphNode* p_node : *p_path2)
    {
        cout << *p_node << endl;
    }

    cout << "Time elapsed: " << tb - ta << " ms." << endl;
    cout << "Is valid path: " << is_valid_path(p_path2) << endl;
    cout << "Cost: " << compute_path_length(p_path2, get<1>(graph_data)) << endl;

    cout << endl;
    cout << "Bidirectional Dijkstra path:" << endl;

    ta = get_milliseconds();

    vector<DirectedGraphNode*>* p_path3 = 
            bidirectional_dijkstra(p_source,
                                   p_target,
                                   *get<1>(graph_data));
    tb = get_milliseconds();

    if (!p_path3)
    {
        cout << "No path for bidirectional Dijkstra's algorithm!" << endl;
        return 0;
    }

    for (DirectedGraphNode* p_node : *p_path3)
    {
        cout << *p_node << endl;
    }

    cout << "Time elapsed: " << tb - ta << " ms." << endl;
    cout << "Is valid path: " << is_valid_path(p_path3) << endl;
    cout << "Cost: " << compute_path_length(p_path3, get<1>(graph_data)) << endl;

    vector<coderodde::DirectedGraphNode*>* p_vec = get<0>(graph_data);

    while (!p_vec->empty())
    {
        delete p_vec->back();
        p_vec->pop_back();
    }

    delete get<0>(graph_data);
    delete get<1>(graph_data);
    delete get<2>(graph_data);

    return 0;
}

Answer 1

You have posted a lot of code, which makes it hard (for me) to find structural issues. However a few style items directly caught my attention:

1. All your code seems to be in a single header file. When you write libraries (or a framework as you indicate) you want to expose your end user to as little details as possible. Consider splitting logic to a C++ implementation and header file.
2. I see a lot of functions accepting pointers. Consider using references, e.g., with is_valid_path - by using references you can reduce the ASCII art in this line: !(*p_path)[i]->is_connected_to((*p_path)[i + 1]
3. The function create_random_graph takes floats and doubles, perhaps a simplified interface with only doubles makes usage simpler.
4. There are a lot of new statements. Especially with trivial functions such as create_random_point consider returning by value or using one of the smart pointers from the <memory> header.
5. There are no code comments. For example: When you compare arc_load_factor I can only guess why you used 0.9. Similarly vague issues arise when you cast floats to size_t.
6. I cannot vouch for your implementation of std::heap, but the standard does not have a very strict performance upper-bound. For example: a fibonacci heap has O(1) on operators in which the C++ standard requires only O(log n)
7. Highly personal, but I'd prefer std::function over functors. Especially when operator() is not declared const. Theoretically, your operator could maintain an internal state.
8. The <chrono> header has all kinds of type-safe features and operators. With your get_milliseconds functions you reduce all the glory to a unsigned long long. Just use std::chrono::time_point, it has 'difference' operators defined.
9. I find it confusing to see some variable names completely in uppercase.

Answer 2

I think that you could pass the variable "name" in your constructors as const reference(or just reference) and not a copy!