# A* versus Bidirectional Dijkstra's algorithm

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 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) :

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

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

*p_tail_point);

(*p_wf)(p_tail, p_head) = distance_weight * cost;

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


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.

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