I have this generic framework for finding (weighted) shortest paths. Also, the program demonstrates the performance of A* and Dijkstra's algorithms on a directed weighted graph. Both run in \$\mathcal{O}(E \log E) = \mathcal{O}(E \log V^2) = \mathcal{O}(2E \log V) = \mathcal{O}(E \log V)\$ time.
shortest_path.h:
#ifndef SHORTEST_PATH_H
#define SHORTEST_PATH_H
#include <queue>
#include <unordered_map>
#include <unordered_set>
/*******************************************************************************
* This namespace holds a small, generic graph search framework. *
*******************************************************************************/
namespace coderodde {
/***************************************************************************
* This abstract type defines the API for graph nodes. *
***************************************************************************/
template<class NodeType>
class AbstractGraphNode {
protected:
using Set = std::unordered_set<NodeType*>;
std::string m_name;
public:
/***********************************************************************
* Assigns a name to the node. The name is treated as the node ID. *
***********************************************************************/
AbstractGraphNode(std::string name) : m_name{name} {}
/***********************************************************************
* Connects this node to 'other'. *
***********************************************************************/
virtual void connect_to(NodeType* other) = 0;
/***********************************************************************
* Tests whether this node is connected to 'other'. *
***********************************************************************/
virtual bool is_connected_to(NodeType* other) const = 0;
/***********************************************************************
* Disconnects this node from 'other'. *
***********************************************************************/
virtual void disconnect_from(NodeType* other) = 0;
/***********************************************************************
* Returns the beginning iterator over this node's child nodes. *
***********************************************************************/
virtual typename Set::iterator begin() const = 0;
/***********************************************************************
* Returns the ending iterator over this node's child nodes. *
***********************************************************************/
virtual typename Set::iterator end() const = 0;
/***********************************************************************
* Tests whether this node and 'other' have same identity. *
***********************************************************************/
bool operator==(const NodeType& other) const
{
return m_name == other.m_name;
}
/***********************************************************************
* Returns the name (identity) of this node. *
***********************************************************************/
std::string& get_name() {return m_name;}
};
/***************************************************************************
* This abstract type defines the API for an edge weight function. *
***************************************************************************/
template<class T, class FloatType = double>
class AbstractWeightFunction {
public:
virtual FloatType& operator()(T* p_node1, T* p_node2) = 0;
};
/***************************************************************************
* This class defines a point in 3D-space for node locations. *
***************************************************************************/
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;}
};
/***************************************************************************
* This class defines the API for a metric. *
***************************************************************************/
template<class FloatType>
class AbstractMetric {
public:
virtual FloatType operator()(coderodde::Point3D<FloatType>& p1,
coderodde::Point3D<FloatType>& p2) = 0;
};
/***************************************************************************
* This class implements an Euclidean metric. *
***************************************************************************/
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);
}
};
/***************************************************************************
* This class implements a map from a node to its locations. *
***************************************************************************/
template<class T, class FloatType = double>
class LayoutMap {
public:
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;
};
/***************************************************************************
* This class holds a node and its priority. Used in the heap. *
***************************************************************************/
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;
};
/***************************************************************************
* This function object compares two heap nodes. *
***************************************************************************/
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();
}
};
/***************************************************************************
* This class implements a map from nodes to its best known distance from *
* source. *
***************************************************************************/
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;
};
/***************************************************************************
* This class maps a graph node to its parent node on a shortest path. *
***************************************************************************/
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;
};
/***************************************************************************
* Constructs a shortest path from the target node and parent map. *
***************************************************************************/
template<class NodeType>
std::vector<NodeType*>* traceback_path(NodeType* p_target,
ParentMap<NodeType>* parent_map)
{
std::vector<NodeType*>* p_path = new std::vector<NodeType*>();
NodeType* p_current = p_target;
while (p_current != nullptr)
{
p_path->push_back(p_current);
p_current = (*parent_map)(p_current);
}
std::reverse(p_path->begin(), p_path->end());
return p_path;
}
/***************************************************************************
* This class implements a heuristic function. *
***************************************************************************/
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;
};
/***************************************************************************
* This function template implements the A* path search algorithm. *
***************************************************************************/
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())
{
NodeType* p_current = OPEN.top()->get_node();
OPEN.pop();
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;
}
/***************************************************************************
* 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)
{
std::priority_queue<HeapNode<NodeType, WeightType>*,
std::vector<HeapNode<NodeType, WeightType>*>,
HeapNodeComparison<NodeType, WeightType>> OPEN;
std::unordered_set<NodeType*> CLOSED;
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())
{
NodeType* p_current = OPEN.top()->get_node();
OPEN.pop();
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))
{
OPEN.push(new HeapNode<NodeType, WeightType>(p_child, cost));
d(p_child) = cost;
p(p_child) = p_current;
}
}
}
// p_target not reachable from p_source.
return nullptr;
}
/***************************************************************************
* This class implements a directed graph node. *
***************************************************************************/
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);
}
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;
};
/***************************************************************************
* This class implements an (asymmetric) directed graph weight function. *
***************************************************************************/
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::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;
}
int main(int argc, const char * argv[]) {
random_device rd;
mt19937 random_gen(rd());
tuple<vector<DirectedGraphNode*>*,
DirectedGraphWeightFunction*,
LayoutMap<DirectedGraphNode, double>*> graph_data =
create_random_graph(20000,
1000.0,
700.0,
0.001f,
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;
cout << "Building graph..." << 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;
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;
}
As I am only a beginner whenever talking about C++, please feel free to point anything that comes to mind, as I am trying to learn the language by using it. For instance, I was fighting with const qualifiers, and once defeated, decided to exclude that keyword. Also, I understand that there is that "move semantics" - thingee, but don't know how and where to apply it.
