4
\$\begingroup\$

(See the next iteration.)

I have this pathfinding algorithm:

DirectedGraph.hpp

#ifndef COM_GITHUB_CODERODDE_DIRECTED_GRAPH_HPP
#define COM_GITHUB_CODERODDE_DIRECTED_GRAPH_HPP

#include <cstddef> // for std::size_t
#include <sstream>
#include <stdexcept>
#include <unordered_map>
#include <unordered_set>

namespace com::github::coderodde::directed_graph {

    template<typename Node = int>
    class DirectedGraph {
    private:
        std::unordered_map<Node, std::unordered_set<Node>> child_map_;
        std::unordered_map<Node, std::unordered_set<Node>> parent_map_;
        std::unordered_set<Node> nodes_;
        std::size_t number_of_arcs_;

    public:
        DirectedGraph() : number_of_arcs_{ 0 } {}

        bool addNode(Node const& node) {
            if (!hasNode(node)) {
                child_map_[node] = {};
                parent_map_[node] = {};
                nodes_.insert(node);
                return true;
            }

            return false;
        }

        bool hasNode(Node const& node) {
            return nodes_.contains(node);
        }

        bool removeNode(Node const& node) {
            if (!hasNode(node)) {
                return false;
            }

            number_of_arcs_ -=
                child_map_[node].size() +
                parent_map_[node].size();

            child_map_.erase(node);
            parent_map_.erase(node);
            nodes_.erase(node);
            return true;
        }

        bool addArc(Node const& tail, Node const& head) {
            bool state_changed = false;

            if (!hasNode(tail)) {
                addNode(tail);
                state_changed = true;
            }

            if (!hasNode(head)) {
                addNode(head);
                state_changed = true;
            }

            if (!child_map_[tail].contains(head)) {
                child_map_[tail].insert(head);
                state_changed = true;
            }

            if (!parent_map_[head].contains(tail)) {
                parent_map_[head].insert(tail);
                state_changed = true;
            }

            if (state_changed) {
                number_of_arcs_++;
            }

            return state_changed;
        }

        bool hasArc(Node const& tail, Node const& head) {
            if (!child_map_.contains(tail)) {
                return false;
            }

            return child_map_[tail].contains(head);
        }

        bool removeArc(Node const& tail, Node const& head) {
            if (!child_map_.contains(tail)) {
                return false;
            }

            if (!child_map_[tail].contains(head)) {
                return false;
            }

            child_map_[tail].erase(head);
            parent_map_[head].erase(tail);
            number_of_arcs_--;
            return true;
        }

        std::unordered_set<Node>* getParentNodesOf(Node const& node) {
            return &parent_map_[node];
        }

        std::unordered_set<Node>* getChildNodesOf(Node const& node) {
            return &child_map_[node];
        }

        std::unordered_set<Node> const& getNodes() const {
            return nodes_;
        }

        std::size_t getNumberOfNodes() const {
            return nodes_.size();
        }

        std::size_t getNumberOfArcs() const {
            return number_of_arcs_;
        }
    };

    template<typename Node = int>
    std::string buildNonExistingArcErrorMessage(
        Node const& tail,
        Node const& head) {

        std::stringstream ss;
        ss << "The arc (" << tail << ", " << head << ") does not exist.";
        return ss.str();
    }

    class NonExistingArcException : public std::logic_error {
    public:
        NonExistingArcException(std::string const& err_msg)
            :
            std::logic_error{ err_msg }
        {}
    };

    template<typename Node = int, typename Weight = double>
    class DirectedGraphWeightFunction {
    private:
        std::unordered_map<Node, std::unordered_map<Node, Weight>> weight_map_;

    public:
        void addWeight(Node const& tail, Node const& head, Weight weight) {
            weight_map_[tail][head] = weight;
        }

        void removeWeight(Node const& tail, Node const& head) {
            if (!weight_map_.contains(tail) 
                || !weight_map_[tail].contains(head)) {
                return;
            }

            weight_map_[tail].erase(head);
        }

        Weight getWeight(Node const& tail, Node const& head) {
            if (!weight_map_.contains(tail)
                || !weight_map_[tail].contains(head)) {
                throw NonExistingArcException{
                    buildNonExistingArcErrorMessage(tail, head)
                };
            }

            return weight_map_[tail][head];
        }
    };
} // End of namespace com::github::coderodde::directed_graph.

#endif // COM_GITHUB_CODERODDE_DIRECTED_GRAPH_HPP

Pathfinders.NBAstar.hpp

#ifndef COM_GITHUB_CODERODDE_GRAPH_PATHFINDERS_NBA_STAR_HPP
#define COM_GITHUB_CODERODDE_GRAPH_PATHFINDERS_NBA_STAR_HPP

#include "DirectedGraph.hpp"
#include "Pathfinders.SharedUtils.hpp"
#include <algorithm>
#include <cstdlib>
#include <queue>
#include <sstream>
#include <stdexcept>
#include <unordered_map>
#include <unordered_set>
#include <vector>

namespace com::github::coderodde::pathfinders {

    using namespace com::github::coderodde::directed_graph;
    using namespace com::github::coderodde::pathfinders::util;

    template<typename Node = int, typename Weight = double> 
    void stabilizeForward(
        DirectedGraph<Node>& graph,
        DirectedGraphWeightFunction<Node, Weight>& weight_function,
        HeuristicFunction<Node, Weight>& heuristic_function,
        std::priority_queue<
            HeapNode<Node, Weight>*,
            std::vector<HeapNode<Node, Weight>*>,
            HeapNodeComparator<Node, Weight>>& OPEN_FORWARD,
        std::unordered_set<Node>& CLOSED,
        std::unordered_map<Node, Weight>& distance_map_forward,
        std::unordered_map<Node, Weight>& distance_map_backward,
        std::unordered_map<Node, Node*>& parent_map_forward,
        Node const& current_node,
        Node const& target_node,
        Weight& best_cost,
        const Node** touch_node_ptr) {

        std::unordered_set<Node>* children =
            graph.getChildNodesOf(current_node);

        for (Node const& child_node : *children) {
            if (CLOSED.contains(child_node)) {
                continue;
            }

            Weight tentative_distance =
                distance_map_forward[current_node] +
                weight_function.getWeight(current_node, child_node);

            if (!distance_map_forward.contains(child_node)
                || distance_map_forward[child_node] > tentative_distance) {
                OPEN_FORWARD.push(
                    new HeapNode<Node, Weight>(
                        child_node,
                        tentative_distance + 
                        heuristic_function.estimate(child_node, target_node)));

                distance_map_forward[child_node] = tentative_distance;
                Node* node_ptr = new Node{ current_node };
                parent_map_forward[child_node] = node_ptr;

                if (distance_map_backward.contains(child_node)) {
                    Weight path_length = tentative_distance +
                        distance_map_backward[child_node];

                    if (best_cost > path_length) {
                        best_cost = path_length;
                        *touch_node_ptr = &child_node;
                    }
                }
            }
        }
    }

    template<typename Node = int, typename Weight = double>
    void stabilizeBackward(
        DirectedGraph<Node>& graph,
        DirectedGraphWeightFunction<Node, Weight>& weight_function,
        HeuristicFunction<Node, Weight>& heuristic_function,
        std::priority_queue<
            HeapNode<Node, Weight>*,
            std::vector<HeapNode<Node, Weight>*>,
            HeapNodeComparator<Node, Weight>>& OPEN_BACKWARD,
        std::unordered_set<Node>& CLOSED,
        std::unordered_map<Node, Weight>& distance_map_forward,
        std::unordered_map<Node, Weight>& distance_map_backward,
        std::unordered_map<Node, Node*>& parent_map_backward,
        Node const& current_node,
        Node const& source_node,
        Weight& best_cost,
        const Node** touch_node_ptr) {

        std::unordered_set<Node>* parents =
            graph.getParentNodesOf(current_node);

        for (Node const& parent_node : *parents) {
            if (CLOSED.contains(parent_node)) {
                continue;
            }

            Weight tentative_distance =
                distance_map_backward[current_node] +
                weight_function.getWeight(parent_node, current_node);

            if (!distance_map_backward.contains(parent_node)
                || distance_map_backward[parent_node] > tentative_distance) {
                OPEN_BACKWARD.push(
                    new HeapNode<Node, Weight>(
                        parent_node,
                        tentative_distance +
                        heuristic_function.estimate(parent_node, source_node)));

                distance_map_backward[parent_node] = tentative_distance;
                Node* node_ptr = new Node{ current_node };
                parent_map_backward[parent_node] = node_ptr;

                if (distance_map_forward.contains(parent_node)) {
                    Weight path_length = tentative_distance +
                        distance_map_forward[parent_node];

                    if (best_cost > path_length) {
                        best_cost = path_length;
                        *touch_node_ptr = &parent_node;
                    }
                }
            }
        }
    }

    template<typename Node = int, typename Weight = double>
    Path<Node, Weight>
        runBidirectionalAstarAlgorithm(
            DirectedGraph<Node>& graph,
            DirectedGraphWeightFunction<Node, Weight>& weight_function,
            HeuristicFunction<Node, Weight>* heuristic_function,
            Node& source_node,
            Node& target_node) {

        checkTerminalNodes(graph, source_node, target_node);

        std::priority_queue<
            HeapNode<Node, Weight>*,
            std::vector<HeapNode<Node, Weight>*>,
            HeapNodeComparator<Node, Weight>> OPEN_FORWARD;

        std::priority_queue<
            HeapNode<Node, Weight>*,
            std::vector<HeapNode<Node, Weight>*>,
            HeapNodeComparator<Node, Weight>> OPEN_BACKWARD;

        std::unordered_set<Node> CLOSED;

        std::unordered_map<Node, Weight> distance_map_forward;
        std::unordered_map<Node, Weight> distance_map_backward;

        std::unordered_map<Node, Node*> parent_map_forward;
        std::unordered_map<Node, Node*> parent_map_backward;

        OPEN_FORWARD .push(new HeapNode<Node, Weight>(source_node, Weight{}));
        OPEN_BACKWARD.push(new HeapNode<Node, Weight>(target_node, Weight{}));

        distance_map_forward[source_node] = Weight{};
        distance_map_backward[target_node] = Weight{};

        parent_map_forward[source_node] = nullptr;
        parent_map_backward[target_node] = nullptr;

        const Node* touch_node = nullptr;
        Weight best_cost = std::numeric_limits<Weight>::max();

        Weight total_distance =
            heuristic_function
            ->estimate(
                source_node,
                target_node);


        Weight f_cost_forward = total_distance;
        Weight f_cost_backward = total_distance;

        while (!OPEN_FORWARD.empty() && !OPEN_BACKWARD.empty()) {
            if (OPEN_FORWARD.size() < OPEN_BACKWARD.size()) {
                HeapNode<Node, Weight>* top_heap_node = OPEN_FORWARD.top();
                OPEN_FORWARD.pop();
                Node current_node = top_heap_node->getElement();
                delete top_heap_node;

                if (CLOSED.contains(current_node)) {
                    continue;
                }

                CLOSED.insert(current_node);

                if (distance_map_forward[current_node] +
                    heuristic_function->estimate(current_node, target_node)
                    >= best_cost
                    ||
                    distance_map_forward[current_node] + f_cost_backward
                    - heuristic_function->estimate(current_node, source_node)
                    >= best_cost) {
                    // Reject the 'current_node'!
                } else {
                    // Stabilize the 'current_node':
                    stabilizeForward<Node, Weight>(
                                     graph,
                                     weight_function,
                                     *heuristic_function,
                                     OPEN_FORWARD,
                                     CLOSED,
                                     distance_map_forward,
                                     distance_map_backward,
                                     parent_map_forward,
                                     current_node,
                                     target_node,
                                     best_cost,
                                     &touch_node);
                }

                if (!OPEN_FORWARD.empty()) {
                    f_cost_forward = OPEN_FORWARD.top()->getDistance();
                }
            } else {
                HeapNode<Node, Weight>* top_heap_node = OPEN_BACKWARD.top();
                OPEN_BACKWARD.pop();
                Node current_node = top_heap_node->getElement();
                delete top_heap_node;

                if (CLOSED.contains(current_node)) {
                    continue;
                }

                CLOSED.insert(current_node);

                if (distance_map_backward[current_node] +
                    heuristic_function->estimate(current_node, source_node)
                    >= best_cost
                    ||
                    distance_map_backward[current_node] + f_cost_forward
                    - heuristic_function->estimate(current_node, target_node)
                    >= best_cost) {
                    // Reject the 'current_node'!
                }
                else {
                    // Stabilize the 'current_node':
                    stabilizeBackward<Node, Weight>(
                                      graph,
                                      weight_function,
                                      *heuristic_function,
                                      OPEN_BACKWARD,
                                      CLOSED,
                                      distance_map_forward,
                                      distance_map_backward,
                                      parent_map_backward,
                                      current_node,
                                      source_node,
                                      best_cost,
                                      &touch_node);
                }

                if (!OPEN_BACKWARD.empty()) {
                    f_cost_backward = OPEN_BACKWARD.top()->getDistance();
                }
            }
        }

        cleanPriorityQueue(OPEN_FORWARD);
        cleanPriorityQueue(OPEN_BACKWARD);

        if (touch_node == nullptr) {
            cleanParentMap(parent_map_forward);
            cleanParentMap(parent_map_backward);

            throw PathDoesNotExistException{
                buildPathNotExistsErrorMessage(source_node, target_node)
            };
        }

        Path<Node, Weight> path =
            tracebackPath(
                *touch_node,
                parent_map_forward,
                parent_map_backward,
                weight_function);

        cleanParentMap(parent_map_forward);
        cleanParentMap(parent_map_backward);
        return path;
    }
} // End of namespace 'com::github::coderodde::pathfinders'.

#endif // COM_GITHUB_CODERODDE_GRAPH_PATHFINDERS_NBA_STAR_HPP

Pathfinders.SharedUtils.hpp

#ifndef COM_GITHUB_CODERODDE_PATHFINDERS_UTIL_HPP
#define COM_GITHUB_CODERODDE_PATHFINDERS_UTIL_HPP

#include "DirectedGraph.hpp"
#include <queue>
#include <stdexcept>
#include <string>
#include <vector>

using namespace com::github::coderodde::directed_graph;

namespace com::github::coderodde::pathfinders::util {

    template<typename Node = int, typename Weight = double>
    class HeuristicFunction {
    public:
        virtual Weight estimate(Node const& tail, Node const& head) = 0;

        virtual ~HeuristicFunction() {

        }
    };

    template<typename Node = int, typename Weight = double>
    class Path {
    private:
        std::vector<Node> nodes_;
        DirectedGraphWeightFunction<Node, Weight> weight_function_;

    public:
        Path(std::vector<Node> const& nodes,
            DirectedGraphWeightFunction<Node, Weight> const& weight_function)
            :
            weight_function_{ weight_function }
        {
            for (Node e : nodes) {
                nodes_.push_back(e);
            }
        }

        Node operator[](std::size_t index) {
            return nodes_[index];
        }

        std::size_t length() {
            return nodes_.size();
        }

        Weight distance() {
            Weight total_distance = {};

            for (std::size_t i = 0; i < nodes_.size() - 1; ++i) {
                total_distance += 
                    weight_function_.getWeight(
                        nodes_[i], 
                        nodes_[i + 1]);
            }

            return total_distance;
        }
    };

    class PathDoesNotExistException : public std::logic_error {
    public:
        PathDoesNotExistException(std::string const& err_msg)
            :
            std::logic_error{ err_msg }
        {}
    };

    class NodeNotPresentInGraphException : public std::logic_error {
    public:
        NodeNotPresentInGraphException(std::string const& err_msg)
            :
            std::logic_error{ err_msg }
        {}
    };

    template<typename Node = int, typename Weight = double>
    struct HeapNode {
    private:
        Weight distance_;
        Node element_;

    public:
        HeapNode(Node const& element, Weight const& distance)
            :
            distance_{ distance },
            element_{ element }
        {

        }

        [[nodiscard]] Node const& getElement() noexcept {
            return element_;
        }

        [[nodiscard]] Weight const& getDistance() noexcept {
            return distance_;
        }
    };

    template<typename Node = int, typename Weight = double>
    class HeapNodeComparator {
    public:

        bool operator()(HeapNode<Node, Weight>* first,
            HeapNode<Node, Weight>* second) {
            return first->getDistance() > second->getDistance();
        }
    };

    template<typename Node = int>
    std::string buildSourceNodeNotInGraphErrorMessage(Node source_node) {
        std::stringstream ss;
        ss << "There is no source node " << source_node << " in the graph.";
        return ss.str();
    }

    template<typename Node = int>
    std::string buildTargetNodeNotInGraphErrorMessage(Node target_node) {
        std::stringstream ss;
        ss << "There is no target node " << target_node << " in the graph.";
        return ss.str();
    }

    template<typename Node = int>
    void checkTerminalNodes(DirectedGraph<Node> graph,
        Node source_node,
        Node target_node) {

        if (!graph.hasNode(source_node)) {
            throw NodeNotPresentInGraphException{
                buildSourceNodeNotInGraphErrorMessage(source_node)
            };
        }

        if (!graph.hasNode(target_node)) {
            throw NodeNotPresentInGraphException{
                buildTargetNodeNotInGraphErrorMessage(target_node)
            };
        }
    }

    template<typename Node = int>
    std::string buildPathNotExistsErrorMessage(Node source_node, Node target_node) {
        std::stringstream ss;
        ss << "There is no path from "
           << source_node 
           << " to " 
           << target_node 
           << ".";

        return ss.str();
    }

    template<typename Node = int, typename Weight = double>
    Path<Node, Weight>
        tracebackPath(Node& target_node,
            std::unordered_map<Node, Node*>& parent_map,
            DirectedGraphWeightFunction<Node, Weight>& weight_function) {

        std::vector<Node> path_nodes;
        Node previous_node = target_node;
        path_nodes.push_back(target_node);

        while (true) {
            Node* next_node = parent_map[previous_node];

            if (next_node == nullptr) {
                std::reverse(path_nodes.begin(), path_nodes.end());
                return Path<Node, Weight>{path_nodes, weight_function};
            }

            path_nodes.push_back(*next_node);
            previous_node = *next_node;
        }
    }

    template<typename Node = int, typename Weight = double>
    Path<Node, Weight>
        tracebackPath(
            const Node& touch_node,
            std::unordered_map<Node, Node*>& forward_parent_map,
            std::unordered_map<Node, Node*>& backward_parent_map,
            DirectedGraphWeightFunction<Node, Weight>& weight_function) {

        std::vector<Node> path_nodes;
        Node previous_node = touch_node;
        path_nodes.push_back(touch_node);

        while (true) {
            Node* next_node = forward_parent_map[previous_node];

            if (next_node == nullptr) {
                std::reverse(path_nodes.begin(), path_nodes.end());
                break;
            }

            path_nodes.push_back(*next_node);
            previous_node = *next_node;
        }

        Node* next_node = backward_parent_map[touch_node];

        while (next_node != nullptr) {
            path_nodes.push_back(*next_node);
            next_node = backward_parent_map[*next_node];
        }

        return Path<Node, Weight>{path_nodes, weight_function};
    }

    template<typename Node = int, typename Weight = double>
    void cleanPriorityQueue(
        std::priority_queue<HeapNode<Node, Weight>*,
        std::vector<HeapNode<Node, Weight>*>,
        HeapNodeComparator<Node, Weight>>&queue) {
        while (!queue.empty()) {
            HeapNode<Node, Weight>* heap_node = queue.top();
            queue.pop();
            delete heap_node;
        }
    }

    template<typename Node = int>
    void cleanParentMap(std::unordered_map<Node, Node*> parent_map) {
        for (const auto p : parent_map) {
            // One 'p.second' will be 'nullptr', but we can "delete" it too:
            delete p.second;
        }

        parent_map.clear();
    }

}; // End of namespace 'com::github::coderodde::pathfinders::util'.

#endif // COM_GITHUB_CODERODDE_PATHFINDERS_UTIL_HPP

main.cpp

#include "DirectedGraph.hpp"
#include "Pathfinders.API.hpp"
#include "Pathfinders.SharedUtils.hpp"
#include <chrono>
#include <cstddef>
#include <iostream>
#include <random>

constexpr std::size_t NUMBER_OF_NODES = 100 * 1000;
constexpr std::size_t NUMBER_OF_ARCS = 500 * 1000;
constexpr double SPACE_WIDTH = 10000.0;
constexpr double SPACE_HEIGHT = 10000.0;
constexpr double DISTANCE_FACTOR = 1.1;

using namespace com::github::coderodde::directed_graph;
using namespace com::github::coderodde::pathfinders;
using namespace com::github::coderodde::pathfinders::api;

class EuclideanCoordinates {
private:
    double x_;
    double y_;

public:
    EuclideanCoordinates(double x = 0.0, double y = 0.0) :
        x_{ x },
        y_{ y }
    {}

    double distanceTo(EuclideanCoordinates const& other) const {
        const auto dx = x_ - other.x_;
        const auto dy = y_ - other.y_;
        return std::sqrt(dx * dx + dy * dy);
    }
};

class MyHeuristicFunction : public HeuristicFunction<int, double> {
private:
    std::unordered_map<int, EuclideanCoordinates> map_;

public:
    MyHeuristicFunction(
        std::unordered_map<int, EuclideanCoordinates> map)
        : map_{ map } {}

    MyHeuristicFunction(const MyHeuristicFunction& other)
        :
        map_{ other.map_ }
    {

    }

    MyHeuristicFunction(MyHeuristicFunction&& other) {
        map_ = std::move(other.map_);
    }

    MyHeuristicFunction& operator=(const MyHeuristicFunction& other) {
        map_ = other.map_;
        return *this;
    }

    MyHeuristicFunction& operator=(MyHeuristicFunction&& other) {
        map_ = std::move(other.map_);
        return *this;
    }

    ~MyHeuristicFunction() {

    }

    double estimate(int const& tail, int const& head) override {
        const auto point1 = map_[tail];
        const auto point2 = map_[head];
        return point1.distanceTo(point2);
    }
};

class GraphData {
private:
    DirectedGraph<int> graph_;
    DirectedGraphWeightFunction<int, double> weight_function_;
    MyHeuristicFunction heuristic_function_;

public:
    GraphData(
        DirectedGraph<int> graph,
        DirectedGraphWeightFunction<int, double> weight_function,
        MyHeuristicFunction heuristic_function)
        :
        graph_{ graph },
        weight_function_{ weight_function },
        heuristic_function_{ heuristic_function }
    {}

    DirectedGraph<int>& getGraph() {
        return graph_;
    }

    DirectedGraphWeightFunction<int, double>& getWeightFunction() {
        return weight_function_;
    }

    HeuristicFunction<int, double>& getHeuristicFunction() {
        return heuristic_function_;
    }
};

EuclideanCoordinates getRandomEuclideanCoordinates(
    std::mt19937& mt,
    std::uniform_real_distribution<double> x_coord_distribution,
    std::uniform_real_distribution<double> y_coord_distribution) {
    double x = x_coord_distribution(mt);
    double y = y_coord_distribution(mt);
    EuclideanCoordinates coords{ x, y };
    return coords;
}

GraphData createRandomGraphData(std::size_t number_of_nodes,
    std::size_t number_of_arcs) {
    DirectedGraph<int> graph;
    DirectedGraphWeightFunction<int, double> weight_function;
    std::vector<int> node_vector;
    node_vector.reserve(number_of_nodes);
    std::random_device rd;
    std::mt19937 mt(rd());
    std::uniform_int_distribution<std::size_t>
        uniform_distribution(0, number_of_nodes - 1);

    std::uniform_real_distribution<double> x_coord_distribution(0, SPACE_WIDTH);
    std::uniform_real_distribution<double> y_coord_distribution(0, SPACE_HEIGHT);

    std::unordered_map<int, EuclideanCoordinates> coordinate_map;

    for (size_t node_id = 0; node_id < number_of_nodes; ++node_id) {
        graph.addNode((int)node_id);
        node_vector.push_back((int)node_id);
        EuclideanCoordinates coords =
            getRandomEuclideanCoordinates(
                mt,
                x_coord_distribution,
                y_coord_distribution);

        coordinate_map[(int)node_id] = coords;
    }

    for (size_t i = 0; i < number_of_arcs; ++i) {
        std::size_t tail_index = uniform_distribution(mt);
        std::size_t head_index = uniform_distribution(mt);
        int tail = node_vector[tail_index];
        int head = node_vector[head_index];
        EuclideanCoordinates tail_coords = coordinate_map[tail];
        EuclideanCoordinates head_coords = coordinate_map[head];
        graph.addArc(tail, head);
        weight_function.addWeight(tail,
            head,
            tail_coords.distanceTo(head_coords)
            * DISTANCE_FACTOR);
    }

    MyHeuristicFunction heuristic_function{ coordinate_map };
    GraphData graph_data(
        graph,
        weight_function,
        heuristic_function);

    return graph_data;
}

class Milliseconds {
private:
    std::chrono::high_resolution_clock m_clock;

public:
    auto milliseconds() {
        return std::chrono::duration_cast<std::chrono::milliseconds>
            (m_clock.now().time_since_epoch()).count();
    }
};

int main() {
    GraphData graph_data = createRandomGraphData(NUMBER_OF_NODES,
        NUMBER_OF_ARCS);

    try {
        Milliseconds ms;
        std::random_device rd;
        std::mt19937 mt(rd());
        std::uniform_int_distribution<int> dist(0, NUMBER_OF_NODES - 1);

        int source_node = dist(mt);
        int target_node = dist(mt);

        std::cout << "Source node: " << source_node << "\n";
        std::cout << "Target node: " << target_node << "\n";
        std::cout << "--- Dijkstra's algorithm: ---\n";

        auto start_time = ms.milliseconds();

        Path<int, double> path =
            findShortestPath()
            .in(graph_data.getGraph())
            .withWeights(graph_data.getWeightFunction())
            .from(source_node)
            .to(target_node)
            .usingDijkstra();

        auto end_time = ms.milliseconds();

        std::cout << "Path:\n";

        for (size_t i = 0; i < path.length(); ++i) {
            std::cout << path[i] << "\n";
        }

        std::cout << "Path distance: " << path.distance() << "\n";
        std::cout << "Duration: " << (end_time - start_time) << " ms.\n\n";
        std::cout << "--- Bidirectional Dijkstra's algorithm: ---\n";

        start_time = ms.milliseconds();

        path =
            findShortestPath()
            .in(graph_data.getGraph())
            .withWeights(graph_data.getWeightFunction())
            .from(source_node)
            .to(target_node)
            .usingBidirectionalDijkstra();

        end_time = ms.milliseconds();

        std::cout << "Path:\n";

        for (size_t i = 0; i < path.length(); ++i) {
            std::cout << path[i] << "\n";
        }

        std::cout << "Path distance: " << path.distance() << "\n";
        std::cout << "Duration: " << (end_time - start_time) << " ms.\n\n";
        std::cout << "--- A* algorithm: ---\n";

        start_time = ms.milliseconds();

        path =
            findShortestPath()
            .in(graph_data.getGraph())
            .withWeights(graph_data.getWeightFunction())
            .from(source_node)
            .to(target_node)
            .withHeuristicFunction(graph_data.getHeuristicFunction())
            .usingAstar();

        end_time = ms.milliseconds();

        std::cout << "Path:\n";

        for (size_t i = 0; i < path.length(); ++i) {
            std::cout << path[i] << "\n";
        }

        std::cout << "Path distance: " << path.distance() << "\n";
        std::cout << "Duration: " << (end_time - start_time) << " ms.\n\n";

        //// NBA* ///////////////////////////////////////////////////////////// 
        std::cout << "--- Bidirectional A* (NBA*) algorithm: ---\n";
        start_time = ms.milliseconds();

        path =
            findShortestPath()
            .in(graph_data.getGraph())
            .withWeights(graph_data.getWeightFunction())
            .from(source_node)
            .to(target_node)
            .withHeuristicFunction(graph_data.getHeuristicFunction())
            .usingBidirectionalAstar();

        end_time = ms.milliseconds();

        std::cout << "Path:\n";

        for (size_t i = 0; i < path.length(); ++i) {
            std::cout << path[i] << "\n";
        }

        std::cout << "Path distance: " << path.distance() << "\n";
        std::cout << "Duration: " << (end_time - start_time) << " ms.\n\n";
    }
    catch (NodeNotPresentInGraphException const& err) {
        std::cout << err.what() << "\n";
    }
    catch (PathDoesNotExistException const& err) {
        std::cout << err.what() << "\n";
    }

    return 0;
}

Now, what bother me is that NBA* runs with -O3 optimization in around 200 milliseconds, whereas this demo of the same algorithm in Java in a graph with the same topological properties runs only in 29 milliseconds.

I suspect that I do implicitly copy assignments/constructors, but I am not sure about that. Please, help.

(The entire (Visual Studio 2022) project lives here.)

\$\endgroup\$
7
  • \$\begingroup\$ Should just have minimal effects, but you are not using the initialiser list with your move constructors. \$\endgroup\$ Commented Feb 22, 2022 at 13:57
  • \$\begingroup\$ Some std::move are missing. \$\endgroup\$ Commented Feb 22, 2022 at 16:40
  • \$\begingroup\$ @Jarod42 Could you tell me where? \$\endgroup\$ Commented Feb 22, 2022 at 16:50
  • \$\begingroup\$ : map_{ map } {}, graph_{ graph }, weight_function_{ weight_function },heuristic_function_{ heuristic_function }. \$\endgroup\$ Commented Feb 22, 2022 at 16:54
  • 2
    \$\begingroup\$ @coderodde, have you tried profiler inside VS? If yes, could you please post the output? It should have sort of a flame graph where it shows the call stack where most of the time is spent. It is not good benchmarking methodology, but will suffice if the only problem is a bottleneck. My expectation is that there is either redundant copying or some severe cache misses going on. \$\endgroup\$ Commented Feb 22, 2022 at 17:41

1 Answer 1

Reset to default
5
\$\begingroup\$

Avoid manual new and delete

I see a lot of new and delete statements. Those are rarely needed in modern C++, and usually point to a problem. In particular, you declare a lot of priority queues like so:

std::priority_queue<HeapNode<Node, Weight>*,
                    std::vector<HeapNode<Node, Weight>*>,
                    HeapNodeComparator<Node, Weight>> OPEN_FORWARD;

And then proceed to create HeapNodes with new and add them to the queue. However, STL containers already allocate memory for you, so instead of having them allocate memory for pointers, and you allocating more memory for the actual HeapNode objects, and everything becoming slower because of the added pointer indirection, ideally you should just be able to write:

std::priority_queue<HeapNode<Node, Weight>> OPEN_FORWARD;

You can if you make the member functions of HeapNode const, and add an operator<() so the container can directly compare elements without needing a HeapNodeComparator. To add a new element, instead of:

OPEN_FORWARD.push(new HeapNode<Node, Weight>(source_node, Weight{}));

You can write:

OPEN_FORWARD.emplace(source_node, Weight{});

And accessing the top element:

HeapNode<Node, Weight>* top_heap_node = OPEN_FORWARD.top();
OPEN_FORWARD.pop();
Node current_node = top_heap_node->getElement();
delete top_heap_node;

Now can be simplified to:

Node current_node = OPEN_FORWARD.top().getElement();
OPEN_FORWARD.pop();

You also don't need cleanPriorityQueue anymore.

You can do something similar for the parent maps. Currently though, you use nullptrs to indicate that a node doesn't have a parent. Instead, either use find() to check if an element is in the std::unordered_map, or store the parent as std::optional<Node> (although I would prefer the former).

Consider using different data structures

runBidirectionalAstarAlgorithm() uses a large amount of containers to store information: two priority queues, an unordered set and four unordered maps. Most of these, possibly even all of them, will at one point contain as many Node objecs as there are in the input graph. That is a lot of duplication. Consider that even if Node is just a small type like an int (and not, say, a std::string or something even more complicated), a std::unordered_map<Node, ...> will still have to allocate memory for each entry it stores, and has to do bookkeeping for the allocated memory, which greatly increases the amount of memory used per Node.

You can already reduce that by combining the auxiliary information you want to store for each Node while the algorithm is running in a single struct:

struct Info {
    bool closed;
    Weight distance_forward;
    Weight distance_backward;
    std::optional<Node> parent_forward;
    std::optional<Node> parent_backward;
};

std::unordered_map<Node, Info> info;

The above map info replaces CLOSED, distance_map_forward, distance_map_backward, parent_map_forward and parent_map_backward. Apart from reducing the memory used by these data structures, having only one map also reduces the number of parameters you have to pass to the stabilize*() functions.

Something similar can be done in class DirectedGraph. Instead of nodes_, child_map_ and parent_map_ being different containers and having to add new nodes to all three of them, just do something like:

struct Adjacency {
    std::set<Node> children;
    std::set<Node> parents;
};

std::unordered_map<Node, Adjacency> nodes_;

Even better would be to add weights in there as well, so you don't need a separate DirectedGraphWeightFunction.

Add more const

You are already using const in a lot of places, but there is more that can be made const. I already mentioned the member functions of HeapNode, but also the parameters graph and weight_function of runBidirectionalAstarAlgorithm() should be const, and when you do that you will find out you need to make a bunch more member functions const.

Use std::function to pass heuristic_function

Instead of creating an abstract base class HeuristicFunction<Node, Weight>, which then must be inherited from, consider passing a std::function<Weight(const Node&, const Node&)> instead. This will still allow you to use a class to store the heuristic function (if you rename estimate() to operator()()), but now it will also allow you to use free functions or lambda expressions. For example, in createRandomGraphData() you could then write:

GraphData graph_data(graph, weight_function,
    [map = std::move(coordinate_map)](const Node& tail, const Node& head) {
        return map[tail].distanceTo(map[head]);
    }
);

Naming things

I would avoid using ALL_CAPS names for anything but macros and enum constants. Just write open, closed and so on for the priority queues and sets.

Some terms in graph theory overlap some terms in computer science that might cause some confusion. Consider arc, while this is a valid name for a connection between two nodes, some might think this is a part of a circle, and think this might cover more than just two nodes. The DirectedGraphWeightFunction doesn't look like a regular function, it's just a container that contains the weights for each arc in the graph. STL containers like std::unordered_map also have a concept call node handles. If you want to avoid confusion, I suggest renaming things like so:

  • node -> vertex
  • arc -> edge
  • DirectedGraphWeightFunction -> WeightMap (or even better, merge it somehow into DirectedGraph if you never need to supported multiple different weight functions for a given graph).
\$\endgroup\$

You must log in to answer this question.

Start asking to get answers

Find the answer to your question by asking.

Ask question

Explore related questions

See similar questions with these tags.