Dijikstra's Algorithm for FlashKiller

#include <string>
#include <vector>
#include <algorithm>
#include <map>
#include <cmath>
#include <iostream>

struct Node;
struct Arc {
    Node *pN1, *pN2;
    double dbWeight;
};

struct Node {
    double x, y;
    std::vector<Arc*> vecArcs;
    std::string szName;
};

std::vector<Node*> g_vecNodes;

void CalculateWeightOfArc( Arc* pArc )
{
    double distance = std::sqrt( std::pow( pArc->pN1->x - pArc->pN2->x, 2 ) + std::pow( pArc->pN1->y - pArc->pN2->y, 2 ) );
    pArc->dbWeight = distance;
}

double Dijikstra( std::vector<Arc*>& vecArcs /* Our vector of arcs which we will traverse */, Node* pStart, Node* pEnd )
{
    // For Dijikstra's algorithm, we associate each node with a temporary distance:
    std::map<Node*, std::pair<double, Node*>> mapNodeDistance;
    std::vector<Node*> vecGraphCopy = g_vecNodes;
    Node* pCurrNode = pStart;
    for( auto it = vecGraphCopy.begin(); it != vecGraphCopy.end(); ++it )
    {
        if( (*it) == pStart )
        {
            mapNodeDistance.insert( std::make_pair( (*it), std::make_pair(0.0, nullptr) ) );
            continue;
        }

        mapNodeDistance.insert( std::make_pair( (*it), std::make_pair(DBL_MAX, nullptr) ) );
    }

    vecGraphCopy.erase( std::find( vecGraphCopy.begin(), vecGraphCopy.end(), pStart ) );

    while( pCurrNode != pEnd )
    {
        // Go through and update the temporary distances:
        for( auto it = pCurrNode->vecArcs.begin(); it != pCurrNode->vecArcs.end(); ++it )
        {
            Node* pNext = ((*it)->pN1 != pCurrNode) ? (*it)->pN1 : (*it)->pN2;
            if( pNext == nullptr )
                continue;

            if( mapNodeDistance.find( pNext )->second.first > mapNodeDistance.find( pCurrNode )->second.first + (*it)->dbWeight )
            {
                mapNodeDistance.find( pNext )->second.first = mapNodeDistance.find( pCurrNode )->second.first + (*it)->dbWeight;
                mapNodeDistance.find( pNext )->second.second = pCurrNode;
            }
        }

        // Find the node with the smallest temporary value:
        double currSmallest = DBL_MAX;
        Node* pOldNode = pCurrNode;
        for( auto it = vecGraphCopy.begin(); it != vecGraphCopy.end(); ++it )
        {
            if( mapNodeDistance.find( (*it) )->second.first < currSmallest )
            {
                currSmallest = mapNodeDistance.find( (*it) )->second.first;
                pCurrNode = (*it);
            }
        }

        // Remove this node from the vector:
        vecGraphCopy.erase( std::find( vecGraphCopy.begin(), vecGraphCopy.end(), pCurrNode ) );

    }

    // Go from the end node to the start node using the previous node we've associated
    // with each node:
    Node *pCurr = pCurrNode, *pNext = nullptr;
    while( pCurr != pStart )
    {
        pNext = mapNodeDistance.find( pCurr )->second.second;

        Arc* pArc = nullptr;
        double dbCurrShortest = DBL_MAX;
        for( auto it = pCurr->vecArcs.begin(); it != pCurr->vecArcs.end(); ++it )
        {
            if( ((*it)->pN1 == pCurr || (*it)->pN1 == pNext) && ((*it)->pN2 == pCurr || (*it)->pN2 == pNext) && (*it)->dbWeight < DBL_MAX )
            {
                pArc = (*it);
                dbCurrShortest = (*it)->dbWeight;
            }
        }

        // Add the arc to the vector:
        vecArcs.insert( vecArcs.begin(), pArc );

        pCurr = pNext;

    }

    return mapNodeDistance.find( pEnd )->second.first;
}

int main() {
    Node A, B, C, D, E, F;
    Arc AB, AC, CD, BD, BE, EF, DF;

    AB.pN1 = &A;
    AB.pN2 = &B;
    AC.pN1 = &A;
    AC.pN2 = &C;
    CD.pN1 = &C;
    CD.pN2 = &D;
    BD.pN1 = &B;
    BD.pN2 = &D;
    BE.pN1 = &B;
    BE.pN2 = &E;
    EF.pN1 = &E;
    EF.pN2 = &F;
    DF.pN1 = &D;
    DF.pN2 = &F;

    AB.dbWeight = 9.0;
    AC.dbWeight = 11.0;
    CD.dbWeight = 5.0;
    DF.dbWeight = 2.0;
    BE.dbWeight = 7.0;
    BD.dbWeight = 6.0;
    EF.dbWeight = 3.0;

    A.szName = "A";
    B.szName = "B";
    C.szName = "C";
    D.szName = "D";
    E.szName = "E";
    F.szName = "F";

    A.vecArcs.push_back( &AB );
    A.vecArcs.push_back( &AC );
    B.vecArcs.push_back( &AB );
    B.vecArcs.push_back( &BD );
    B.vecArcs.push_back( &BE );
    C.vecArcs.push_back( &AC );
    C.vecArcs.push_back( &CD );
    D.vecArcs.push_back( &BD );
    D.vecArcs.push_back( &CD );
    D.vecArcs.push_back( &DF );
    E.vecArcs.push_back( &BE );
    E.vecArcs.push_back( &EF );
    F.vecArcs.push_back( &EF );
    F.vecArcs.push_back( &DF );

    g_vecNodes.push_back( &A );
    g_vecNodes.push_back( &B );
    g_vecNodes.push_back( &C );
    g_vecNodes.push_back( &D );
    g_vecNodes.push_back( &E );
    g_vecNodes.push_back( &F );

    std::vector<Arc*> vecArcs;
    std::cout << "The shortest distace is: " << Dijikstra( vecArcs, &A, &F ) << " going along: " << std::endl;

    for( auto it = vecArcs.begin(); it != vecArcs.end(); ++it )
    {
        std::cout << (*it)->pN1->szName << (*it)->pN2->szName << std::endl;
    }

    std::cin.ignore( 256, '\n' );
    std::cin.get();

    return 0;

}