Dijkstra's shortest path in C++

HEADER FILE
==========================================

//////////////////////////////////////////////////////////////////////////
/// Dijkstra's Algorithm is an algorithm written by Professor
/// Edsger Wybe Dijkstra in 1959. It is used to find the shortest path
/// between two nodes in a node graph.
///
/// This implementation is a modified version of the original algorithm.
///
/// \author n.foster
//////////////////////////////////////////////////////////////////////////

#include <map>
#include <vector>
#include <set>

#include "PathNode.h"

//////////////////////////////////////////////////////////////////////////
// PathEdge
//
// \brief Represents a two-way connection between two PathNodes.
//        Connection weight is the same in both directions, which is not
//        suitable for something like open-world AI use.
//
// \author n.foster
//////////////////////////////////////////////////////////////////////////
struct PathEdge
{
    PathNode* side1;
    PathNode* side2;
    float weight;

    PathEdge( PathNode* side1, PathNode* side2, float weight )
    {
        this->side1 = side1;
        this->side2 = side2;
        this->weight = weight;

        side1->AddEdge(this);
        side2->AddEdge(this);
    }

    // Returns true if this edge connects to 'targetNode'
    bool ConnectsToNode( PathNode* targetNode )
    {
        return ( side1 == targetNode || side2 == targetNode );
    }

    // Pass in a node that is at one side of the edge, it will return the node
    // at the other side, or NULL if the side you passed in is not part of the edge.
    PathNode* GetOtherSide( PathNode* side )
    {
        if ( side1 == side )
        {
            return side2;
        }
        else if ( side2 == side )
        {
            return side1;
        }
        else
        {
            return NULL;
        }
    }
};

//////////////////////////////////////////////////////////////////////////
// ShortestPath
//
// \brief Stores a list of PathNodes and allows for calculation of the
//        shortest path, given all connected nodes have a weight for each
//        connection.
//
// \author n.foster
//////////////////////////////////////////////////////////////////////////
class ShortestPath
{
public:
    ShortestPath();
    ~ShortestPath();

    void AddNode( PathNode* newNode );

    PathNode* GetNodeByIndex( const unsigned int index ) const;

    void Initialize( PathNode* const startNode );

    float FindShortestPathToNode( PathNode* targetNode );

    void DeletePath( void );

private: // Variables

    // For each node, the total weight from the source node
    typedef std::pair<unsigned int, float> TIndexDistancePair;
    std::map<unsigned int, float> m_Weights;

    // For each node, the previous node in the shortest path
    std::vector<PathNode*> m_NodesInPath;

    // List of all nodes in the path
    std::vector<PathNode*> m_Nodes;

    // List of nodes that haven't been searched yet
    std::set<PathNode*> m_NodesLeftInSearch;

    // Node from which to start a search
    PathNode* m_sourceNode;

private: // Members

    PathNode* FindNodeClosestToSource( PathNode* sourceNode );

    PathNode* FindLowestWeightNodeLeftInSearch(void);
};


CPP FILE
==========================================================================
//////////////////////////////////////////////////////////////////////////
/// Dijkstra's Algorithm is an algorithm written by Professor
/// Edsger Wybe Dijkstra in 1959. It is used to find the shortest path
/// between two nodes in a node graph.
///
/// This implementation is a modified version of the original algorithm.
///
/// \author n.foster
//////////////////////////////////////////////////////////////////////////

#include "ShortestPath.h"
#include <set>
#include <vector>

namespace NS_ShortestPath
{
    static const float cfLargeFloat = 10000000.0f;
}

ShortestPath::ShortestPath() :
m_sourceNode(NULL)
{
}

ShortestPath::~ShortestPath()
{
    for ( int i = m_Nodes.size() - 1; i >= 0; --i )
    {
        delete m_Nodes[i];
        m_Nodes[i] = NULL;
    }
    m_Nodes.clear();

    m_sourceNode = NULL;

    m_Weights.clear();
    m_NodesInPath.clear();
    m_NodesLeftInSearch.clear();
}

void ShortestPath::Initialize( PathNode* const startNode )
{
    if ( startNode == NULL )
        return;

    m_sourceNode = startNode;

    // We know the distance to the source node is always zero
    m_Weights.clear();
    m_Weights.insert( TIndexDistancePair(m_sourceNode->GetIndex(), 0.0f) );

    // For each edge connected to the source node, get an index and distance
    std::set<PathEdge*>::iterator it = m_sourceNode->GetEdges().begin();

    // For each edge connected to the source node, we setup some initial distances
    for ( it; it != m_sourceNode->GetEdges().end(); ++it )
    {
        if ( (*it)->side1 == m_sourceNode )
        {
            m_Weights.insert( TIndexDistancePair( (*it)->side2->GetIndex(), (*it)->weight ));
        }
        else
        {
            m_Weights.insert( TIndexDistancePair( (*it)->side1->GetIndex(), (*it)->weight ));
        }
    }

    // We'll start out our list of nodes in the path by having them all point to the source node
    m_NodesInPath.resize( m_Nodes.size() );
    for ( unsigned int i = 0; i < m_NodesInPath.size(); ++i )
    {
        m_NodesInPath[i] = m_sourceNode;
    }

    // Deep copy all nodes into a set of nodes that we have left to search
    m_NodesLeftInSearch.clear();
    for ( unsigned int i = 0; i < m_Nodes.size(); ++i )
    {
        m_NodesLeftInSearch.insert(m_Nodes[i]);
    }

    // We don't need to check the source node
    m_NodesLeftInSearch.erase(m_sourceNode);
}

void ShortestPath::AddNode( PathNode* newNode )
{
    m_Nodes.push_back(newNode);
}

PathNode* ShortestPath::GetNodeByIndex( const unsigned int index ) const
{
    if ( index > (m_Nodes.size() - 1) )
        return NULL;

    return m_Nodes[index];
}

// Returns weight of shortest path. This is a modified implementation of Dijkstra's algorithm.
float ShortestPath::FindShortestPathToNode( PathNode* targetNode )
{
    if ( FindNodeClosestToSource( m_sourceNode ) == NULL )
        return NS_ShortestPath::cfLargeFloat;

    PathNode* tempNode = NULL;
    float oldDistance = 0.0f;

    // See if we've made it to the target node yet
    while ( m_NodesLeftInSearch.find(targetNode) != m_NodesLeftInSearch.end() )
    {
        tempNode = FindLowestWeightNodeLeftInSearch();
        m_NodesLeftInSearch.erase(tempNode);

        std::set<PathNode*>::iterator it = m_NodesLeftInSearch.begin();
        for ( it; it != m_NodesLeftInSearch.end(); ++it )
        {
            float weight = NS_ShortestPath::cfLargeFloat;

            std::map<unsigned int, float>::iterator distIt = m_Weights.find( (*it)->GetIndex() );

            if ( distIt != m_Weights.end() )
            {
                weight = distIt->second;
            }

            oldDistance = weight;

            if ( tempNode->WeightToNode(*it) == -1.0f )
                continue;

            weight = std::min(oldDistance, m_Weights.find( tempNode->GetIndex() )->second + tempNode->WeightToNode(*it) );

            // If weight has changed, then a shorter path has been found
            if ( weight != oldDistance )
            {
                if ( distIt == m_Weights.end() )
                {
                    m_Weights.insert( TIndexDistancePair((*it)->GetIndex(), weight) );
                }
                else
                {
                    distIt->second = weight;
                }

                m_NodesInPath[ (*it)->GetIndex() ] = tempNode;
            }
        }
    }

    // Total up weight of path
    float totalWeight = 0;

    // Create a node for iteration
    PathNode* currentNode = targetNode;

    // Iterate until the current node is pointing back to the source node
    while ( currentNode != m_sourceNode )
    {
        PathNode* prevNode = m_NodesInPath[ currentNode->GetIndex() ];

        float weightToNode = currentNode->WeightToNode(prevNode);

        currentNode->StoreCalculatedWeightToNode(prevNode, weightToNode);
        prevNode->StoreCalculatedWeightToNode(currentNode, weightToNode);

        totalWeight += weightToNode;

        currentNode = prevNode;
    }

    return totalWeight;
}

PathNode* ShortestPath::FindNodeClosestToSource( PathNode* sourceNode )
{
    std::set<PathEdge*> edges = sourceNode->GetEdges();

    if ( edges.empty() )
        return NULL;

    float leastWeight = 10000000;
    PathNode* nodeLeastWeight = NULL;

    // Go through all the edges connected to this node and find the one with the least weight
    std::set<PathEdge*>::iterator it = edges.begin();
    for ( it; it != edges.end(); ++it )
    {
        PathEdge* thisEdge = (*it);

        if ( thisEdge == NULL )
            continue;

        if ( (*it)->weight < leastWeight )
        {
            leastWeight = thisEdge->weight;
            nodeLeastWeight = (thisEdge->side1 == sourceNode) ? thisEdge->side2 : thisEdge->side1;
        }
    }

    return nodeLeastWeight;
}

// Finds the node with the least weight at a given time during a search.
PathNode* ShortestPath::FindLowestWeightNodeLeftInSearch(void)
{
    float lowestWeight = 10000000.0f;
    PathNode* lowestWeightNode = NULL;

    std::set<PathNode*>::iterator nodeIt = m_NodesLeftInSearch.begin();
    for (nodeIt; nodeIt != m_NodesLeftInSearch.end(); ++nodeIt)
    {
        std::map<unsigned int, float>::iterator distIt = m_Weights.find( (*nodeIt)->GetIndex() );
        if ( distIt == m_Weights.end() )
            continue;

        if ( distIt->second < lowestWeight )
        {
            lowestWeight = distIt->second;
            lowestWeightNode = GetNodeByIndex(distIt->first);
        }
    }

    return lowestWeightNode;
}