## Tuesday, March 26, 2013

### Djikstra's Algorithm: Finding the Shortest Path

For those that are unfamiliar, Djikstra's Algorithm is used to find the guaranteed shortest path between two points in a graph (as opposed to A* which will take an educated guess, but is more efficient). For the second time in as many years I've found myself needing a shortest path algorithm in a game project. The last one that I wrote was before my blog existed and I never published it, and since then I've lost access to the code base it was published in. This time I'm a bit more pressed for time and didn't want to write my own from scratch so I decided to grab one from online and fix it up a bit.

It has several issues with it that I needed to solve:
• The algorithm itself and several methods related to it were not encapsulated, which is messy. At the least they should be in their own file.
• Any nodes and edges that were created had to be deleted by the user otherwise the program would leak memory.
• The code assumes all edges were bi-directional, I need to handle the possibility of a directional edge.
• Querying the node graph would remove the nodes from the graph, making it so that if you wanted to query again you had to recreate the graph.
• The code didn't handle the case in which there wasn't a possible path between two nodes
• There were several inefficiencies.
The changes I made were:
• Encapsulated the algorithm, and the containers for the nodes and edges into a Graph class.
• The Graph class destructor made sure to clean up the nodes and edges to prevent memory leaks.
• Made edges directional while still allowing for bi-directional connections.
• Allowing the Graph class to not remove nodes while calculating the shortest path, so the user can query the graph as often as they'd like without recreating it.
• Made a user-friendly method to allow the user to get a path between any two nodes.
• Corrected inefficiencies.
For the code containing all of my changes, (click here). I go over the changes in more detail at the end of this post.

In the example code, here is the graph:

Notice that the edges between A and B, and E and F, are uni-directional, you cannot move from B to A, or F to E. In a shortest path algorithm, each node connection (edge) has a weight rather than a distance, this is because we're really not looking for the path of least distance, but the path that can be traversed the fastest, so we may want to factor in things along the path that may slow down the traveller. For example, the connection between B and E, and B and D are of different distances, but both have a weight of 4. Maybe the path between B and E is a smooth concrete path, but from B to D is a swampy path that is hard to traverse.

Given the weights that we've given to each edge, denoted by the numbers near each edge, the shortest path from A to F is: A->B->E->F. Because some edges are uni-directional, the shortest path from F to A is: F->D->B->E->C->A (in fact, that's the only path from F to A). We can also start on nodes besides A and F. For example, the shortest path from C to D is: C->E->F->D.

Below here you'll find the code, organized into main.cpp and three classes. Graph is the class that contains the algorithm and stores Nodes and Edges. And a class for Node, and a class for Edge.

First let's look at main.cpp so we can see how the graph is created and utilized:

```#include <iostream>

#include "graph.h"
#include "node.h"

// NOTE: Credit for the original code for this implementation of
// Djikstra's Algorithm goes to:
// http://www.reviewmylife.co.uk/blog/2008/07/15/dijkstras-algorithm-code-in-c/
// I've modified it to suit my needs.

void outputPath(std::vector<node> path)
{
for ( long i = (path.size() - 1); i >= 0; --i )
{
std::cout << path[i]->getID() << " ";
}

std::cout << std::endl;
}

int main(int argc, const char * argv[])
{
Graph graph;

// Create and name our nodes
Node* a = graph.createNode('a');
Node* b = graph.createNode('b');
Node* c = graph.createNode('c');
Node* d = graph.createNode('d');
Node* e = graph.createNode('e');
Node* f = graph.createNode('f');

// Create some edges
graph.createEdge(a, b, 5);
graph.createEdge(a, c, 4);
graph.createEdge(b, d, 4);
graph.createEdge(b, e, 4);
graph.createEdge(c, e, 6);
graph.createEdge(d, f, 3);
graph.createEdge(e, f, 2);

// Create some edges so we can get from 'f' to 'a'
graph.createEdge(f, d, 3);
graph.createEdge(d, b, 4);
graph.createEdge(e, c, 6);
graph.createEdge(c, a, 4);

std::vector<node> path;

// Go from 'c' to 'f'
graph.getPathFromStartNodeToFinishNode(c, f, path);
outputPath(path);

// Go from 'a' to 'f'
graph.getPathFromStartNodeToFinishNode(a, f, path);
outputPath(path);

// Go from 'f' to 'a'
graph.getPathFromStartNodeToFinishNode(f, a, path);
outputPath(path);

return 0;
}
```

And below you'll find the three class .h/.cpp files.

graph.h
```#pragma once

#include <vector>

class Node;
class Edge;

class Graph
{
public:
Graph() {}
~Graph();

Node* createNode(char id);
Edge* createEdge(Node* node1, Node* node2, const int weight);
void removeEdge(Edge* edge);

void getPathFromStartNodeToFinishNode(Node* node1, Node* node2, std::vector<node>& path);

private:
void processGraph();
Node* extractSmallest(std::vector<node>& nodes);

private:
std::vector<node> m_nodes;
std::vector<edge> m_edges;
};
```

graph.cpp
```#include "graph.h"
#include "node.h"
#include "edge.h"

#include <iostream>

Graph::~Graph()
{
for ( int i = 0; i < m_nodes.size(); ++i )
{
delete m_nodes[i];
}

for ( int i = 0; i < m_edges.size(); ++i )
{
delete m_edges[i];
}
}

Node* Graph::createNode(char id)
{
Node* new_node = new Node(id);

m_nodes.push_back(new_node);

return new_node;
}

Edge* Graph::createEdge(Node* node1, Node* node2, const int weight)
{
// Yes, we intentionally reverse the node order here
Edge* new_edge = new Edge(node2, node1, weight);

m_edges.push_back(new_edge);

return new_edge;
}

void Graph::removeEdge(Edge* edge)
{
std::vector<edge>::iterator it = m_edges.begin();
for ( ; it < m_edges.end(); ++it)
{
if (*it == edge)
{
m_edges.erase(it);
return;
}
}
}

void Graph::getPathFromStartNodeToFinishNode(Node* start, Node* finish, std::vector<node>& path)
{
path.clear();

// Clear any existing node information
for ( int i = 0; i < m_nodes.size(); ++i )
{
m_nodes[i]->setDistanceFromStart(INT_MAX);
m_nodes[i]->setPreviousNode(NULL);
}

start->setDistanceFromStart(0);

processGraph();

Node* previous = finish;

while (previous)
{
path.push_back(previous);

previous = previous->getPreviousNode();
}

if ( path.size() == 1 )
{
path.clear();
}
}

void Graph::processGraph()
{
// Create a copy of the nodes.
std::vector<node> nodes = m_nodes;

while (nodes.size() > 0)
{
Node* smallest = extractSmallest(nodes);

std::vector<node> out_nodes;

const size_t size = out_nodes.size();
for (int i = 0; i < size; ++i)
{
Node* adjacent = out_nodes[i];

const int distance_to_node = adjacent->distanceToNode(m_edges, smallest);
if ( distance_to_node == -1 )
{
continue;
}

const int total_distance = distance_to_node + smallest->getDistanceFromStart();
if ( total_distance < adjacent->getDistanceFromStart() )
{
}
}
}
}

Node* Graph::extractSmallest(std::vector&ltnode&gt& nodes)
{
if ( nodes.empty() )
{
return NULL;
}

int smallest_position = 0;

Node* smallest = nodes;
for ( int i = 1; i < nodes.size(); ++i )
{
Node* current = nodes[i];

if ( current->getDistanceFromStart() < smallest->getDistanceFromStart() )
{
smallest = current;
smallest_position = i;
}
}

nodes.erase(nodes.begin() + smallest_position);

return smallest;
}
```

node.h
```#pragma once

#include <vector>

class Edge;

class Node
{
public:
Node(char id) :
id(id)
, m_previous(NULL)
, m_distance_from_start(INT_MAX)
{
}

char getID() const                          { return id; }

void setPreviousNode(Node* node)            { m_previous = node; }
Node* getPreviousNode() const               { return m_previous; }

void setDistanceFromStart(int distance)     { m_distance_from_start = distance; }
int getDistanceFromStart() const            { return m_distance_from_start; }

Node* extractSmallest(std::vector<node>& nodes);

void adjacentRemainingNodes(const std::vector<node>& nodes, const std::vector<edge>& edges,
std::vector<node>& out_nodes) const;

int distanceToNode(const std::vector<edge>& edges, const Node* other_node) const;

bool contains(const std::vector<node>& nodes, const Node* node) const;

private:
char    id;
Node*   m_previous;
int     m_distance_from_start;
};
```

node.cpp
```#include "node.h"
#include "edge.h"

void Node::adjacentRemainingNodes(const std::vector<node>& nodes, const std::vector<edge>& edges,
std::vector<node>& out_nodes) const
{
const size_t size = edges.size();

for(int i = 0; i < size; ++i)
{
const Edge* edge = edges[i];
Node* adjacent = NULL;
if (edge->first() == this)
{
}
else if (edge->second() == this)
{
}

{
}
}
}

int Node::distanceToNode(const std::vector<edge>& edges, const Node* other_node) const
{
const size_t size = edges.size();
for(int i = 0; i < size; ++i)
{
const Edge* edge = edges[i];
if ( edge->connects(this, other_node) )
{
return edge->getDistance();
}
}

// Happens if two nodes do not connect or the connection is directed in the opposite direction
return -1;
}

bool Node::contains(const std::vector<node>& nodes, const Node* node) const
{
for(int i = 0; i < nodes.size(); ++i)
{
if (node == nodes[i])
{
return true;
}
}

return false;
}
```

edge.h
```#pragma once

#include <vector>

class Node;

class Edge
{
public:
Edge(Node* node1, Node* node2, int distance) :
m_node1(node1)
, m_node2(node2)
, m_distance(distance)
{
// Assert that node1 and node2 aren't null and that distance is a positive value
}

Node* first() const           { return m_node1; }
Node* second() const          { return m_node2; }
int getDistance() const       { return m_distance; }

bool connects(const Node* node1, const Node* node2) const;

private:
Node* m_node1;
Node* m_node2;
int m_distance;
};
```

edge.cpp
```#include "edge.h"
#include "node.h"

bool Edge::connects(const Node* node1, const Node* node2) const
{
return ( (node1 == m_node1) && (node2 == m_node2) );
}```

### The Changes

The Graph class destructor made sure to clean up the nodes and edges to prevent memory leaks.
The original required you to keep track of all of your own pointers to nodes and edges and delete them.

```Graph::~Graph()
{
for ( int i = 0; i < m_nodes.size(); ++i )
{
delete m_nodes[i];
}

for ( int i = 0; i < m_edges.size(); ++i )
{
delete m_edges[i];
}
}
```

Made edges directional while still allowing for bi-directional connections.
The original 'connects' method checked if the edge went in either direction, which made it so you could not have uni-directional edges.

```#include "edge.h"
#include "node.h"

bool Edge::connects(const Node* node1, const Node* node2) const
{
return ( (node1 == m_node1) && (node2 == m_node2) );
}
```

And here we handle the case where we don't get a valid distance from a distance check, which can happen if there is no possible path.
```void Graph::processGraph()
{
// Create a copy of the nodes.
std::vector<Node*> nodes = m_nodes;

while (nodes.size() > 0)
{
Node* smallest = extractSmallest(nodes);

std::vector out_nodes;

const size_t size = out_nodes.size();
for (int i = 0; i < size; ++i)
{
Node* adjacent = out_nodes[i];

// We get the distance, if it's -1 then we know there is no possible path between the two nodes
const int distance_to_node = adjacent->distanceToNode(m_edges, smallest);
if ( distance_to_node == -1 )
{
continue;
}

const int total_distance = distance_to_node + smallest->getDistanceFromStart();
if ( total_distance < adjacent->getDistanceFromStart() )
{
}
}
}
}
```

Allowing the Graph class to not remove nodes while calculating the shortest path, so the user can query the graph as often as they'd like without recreating it.
The key change from the original is that we make a copy of the list of nodes. The copies list is then whittled away by this function and then goes out of scope, the original would alter the 'm_nodes' list, thus requiring the user to recreate it after each time they queried for a new path.

```void Graph::processGraph()
{
// Create a copy of the nodes.
std::vector<node> nodes = m_nodes;

// ... Rest of function ...
}
```

Made a user-friendly method to allow the user to get a path between any two nodes.
The original required you to recreate the entire graph, and then manually set the distance on the start node to 0, and then run the algorithm. I don't have the graph nodes deleted after they process so here I clear out changes the nodes might be storing, then set the distance on the start node, then iterate through the nodes and save out the path. If the path only ends up with the last node in it then we know that there is no path to the start node.

```void Graph::getPathFromStartNodeToFinishNode(Node* start, Node* finish, std::vector<node>& path)
{
path.clear();

// Clear any existing node information
for ( int i = 0; i < m_nodes.size(); ++i )
{
m_nodes[i]->setDistanceFromStart(INT_MAX);
m_nodes[i]->setPreviousNode(NULL);
}

start->setDistanceFromStart(0);

processGraph();

Node* previous = finish;

while (previous)
{
path.push_back(previous);

previous = previous->getPreviousNode();
}

if ( path.size() == 1 )
{
path.clear();
}
}
```

Corrected inefficiencies
There were several places in the original code sample that included the use of std::vector's 'at' method. This method is around 5-10x slower than using the [] operator. 'at' performs a range check, which is unnecessary if your code is setup to prevent indices from going out of range. Additionally there were places where entire containers were being created on the heap (using 'new'), and then deleted within the same scope. These have been changed to use the stack instead, which is generally much more efficient.

As an example, here was the original method:
```void Dijkstras()
{
while (nodes.size() > 0)
{
Node* smallest = ExtractSmallest(nodes);

const int size = adjacentNodes->size();
for (int i=0; i<size; ++i)
{
int distance = Distance(smallest, adjacent) + smallest->distanceFromStart;

if (distance < adjacent->distanceFromStart)
{
}
}
}
}
```

And here you can see that we're creating a vector on the stack and passing it by reference to the 'adjacentRemainingNodes' method. Because it's not being created on the heap we also don't have to worry about deleting a pointer that was created by another method:

```void Graph::processGraph()
{
// Create a copy of the nodes.
std::vector<Node*> nodes = m_nodes;

while (nodes.size() > 0)
{
Node* smallest = extractSmallest(nodes);

std::vector<node> out_nodes;

const size_t size = out_nodes.size();
for (int i = 0; i < size; ++i)
{
Node* adjacent = out_nodes[i];

const int distance_to_node = adjacent->distanceToNode(m_edges, smallest);
if ( distance_to_node == -1 )
{
continue;
}

const int total_distance = distance_to_node + smallest->getDistanceFromStart();
if ( total_distance < adjacent->getDistanceFromStart() )
{