Dijkstra pathfinder. OOP C++11

#include <iostream>
#include <vector>
#include <map>

using namespace std;

const float Infinite = FLT_MAX;

class GraphVertex;

struct Edge {
    GraphVertex * destVertex;
    float distToDestVertex;

    Edge() {
        destVertex = nullptr;
        distToDestVertex = Infinite;
    }

    Edge( GraphVertex * destinationVertex, float distanceToDestinationVertex ) {
        destVertex = destinationVertex;
        distToDestVertex = distanceToDestinationVertex;
    }
};

class GraphVertex {
private:
    bool used;
    float distanceFromBegin;
    GraphVertex * ancestor;
    vector< Edge > edges;

    float DistanceToVertex( GraphVertex * vertex ) {
        return sqrtf( x * vertex->x + y * vertex->y + z * vertex->z );
    }
public:
    friend class Route;

    float x;
    float y;
    float z;

    GraphVertex( float vX, float vY, float vZ ) {
        x = vX;
        y = vY;
        z = vZ;
        ClearState();
    }

    void ClearState( ) {
        ancestor = nullptr;
        used = false;
        distanceFromBegin = Infinite;
    }

    void AddEdge( GraphVertex * vertex ) {
        edges.push_back( Edge( vertex, DistanceToVertex( vertex )));
        vertex->edges.push_back( Edge( this, DistanceToVertex( vertex )));
    }
};

class Route {
private:
    vector< GraphVertex* > graph;
public:
    Route() {

    }

    ~Route() {
        for( auto vertex : graph ) {
            delete vertex;
        }
    }

    void AddPoint( GraphVertex * newVertex ) {
        graph.push_back( newVertex );
    }

    GraphVertex * GetPoint( int i ) {
        if( i < 0 || i >= graph.size() ) {
            return nullptr;
        } else {
            return graph[i];
        }
    }

    int GetPointCount( ) {
        return graph.size();
    }

    void Build( GraphVertex * begin, GraphVertex * end, vector< GraphVertex* > & outPoints ) {
        outPoints.clear();
        // clear state of all vertices
        for( auto vertex : graph ) {
            vertex->ClearState();
        }
        // set begin graph vertex
        begin->distanceFromBegin = 0;

        for( int i = 0; i < graph.size(); i++ ) {
            // get nearest vertex
            GraphVertex * nearest = nullptr;
            for( auto vertex : graph ) {
                if( vertex->used ) {
                    continue;
                } else if( !nearest ) {
                    nearest = vertex;
                } else if( vertex->distanceFromBegin < nearest->distanceFromBegin ) {
                    nearest = vertex;
                }
            }

            if( nearest->distanceFromBegin >= Infinite )
                break;

            nearest->used = true;

            // relaxation
            for( auto & edge : nearest->edges ) {
                if( nearest->distanceFromBegin + edge.distToDestVertex < edge.destVertex->distanceFromBegin ) {
                    edge.destVertex->distanceFromBegin = nearest->distanceFromBegin + edge.distToDestVertex;
                    edge.destVertex->ancestor = nearest;
                }
            }
        }

        // restore path to dest vertex
        for( GraphVertex * v = end; v != begin; v = v->ancestor ) {
            outPoints.push_back (v);
        }

        outPoints.push_back( begin );

        reverse( outPoints.begin(), outPoints.end() );
    }
};

int main() {
    // route test
    vector< GraphVertex* > vertex;

    vertex.push_back( new GraphVertex( 0, 0, 0 ));
    vertex.push_back( new GraphVertex( 1, 2, 0 ));
    vertex.push_back( new GraphVertex( 1, 5, 0 ));
    vertex.push_back( new GraphVertex( 4, 4, 0 ));
    vertex.push_back( new GraphVertex( 4, 2, 0 ));
    vertex.push_back( new GraphVertex( 7, 2, 0 ));

    vertex[0]->AddEdge( vertex[1] );

    vertex[1]->AddEdge( vertex[2] );
    vertex[1]->AddEdge( vertex[4] );

    vertex[2]->AddEdge( vertex[3] );
    vertex[3]->AddEdge( vertex[4] );

    vertex[4]->AddEdge( vertex[5] );

    Route testRoute;

    for( auto v : vertex )
        testRoute.AddPoint( v );

    GraphVertex * begin = vertex[1];
    GraphVertex * end = vertex[3];

    vector< GraphVertex* > out;
    testRoute.Build( begin, end, out );


}