Simple Dijkstra's algorithm implementation

dijkstra.cpp

#include <vector>
#include <limits>
#include <iostream>

struct Edge
{
    Edge(size_t to, int cost)
        : to(to - 1)
        , cost(cost)
    {}

    size_t to;
    int cost;
};

struct Vertex
{
    std::vector<Edge> edges;
};

struct Graph
{
    std::vector<Vertex> vertices;
};

std::vector<int> dijkstra(Graph const &graph, size_t start_vertex)
{
    int const INFINITY = std::numeric_limits<int>::max();

    std::vector<int> distances(graph.vertices.size(), INFINITY);
    std::vector<size_t> previous(graph.vertices.size(), std::numeric_limits<size_t>::max());

    distances[start_vertex] = 0;

    std::vector<size_t> queue;
    for (size_t i = 0; i < graph.vertices.size(); ++i)
        queue.push_back(i);

    while (!queue.empty())
    {
        size_t closest_vertex_index = 0;
        for (size_t i = 1; i < queue.size(); ++i)
            if (distances[queue[i]] < distances[queue[closest_vertex_index]])
                closest_vertex_index = i;

        size_t closest_vertex = queue[closest_vertex_index];
        queue.erase(queue.begin() + closest_vertex_index);

        if (distances[closest_vertex] == INFINITY)
            break;

        std::vector<Edge> const &edges = graph.vertices[closest_vertex].edges;
        for (size_t i = 0; i < edges.size(); ++i)
        {
            int distance = distances[closest_vertex] + edges[i].cost;
            size_t to = edges[i].to;
            if (distance < distances[to])
            {
                distances[to] = distance;
                previous[to] = closest_vertex;
            }
        }
    }

    return distances;
}

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

    // Test example from http://en.wikipedia.org/wiki/Dijkstra's_algorithm
    g.vertices.resize(6);

    g.vertices[0].edges.push_back(Edge(2, 7));
    g.vertices[0].edges.push_back(Edge(3, 9));
    g.vertices[0].edges.push_back(Edge(6, 14));

    g.vertices[1].edges.push_back(Edge(1, 7));
    g.vertices[1].edges.push_back(Edge(3, 10));
    g.vertices[1].edges.push_back(Edge(4, 15));

    g.vertices[2].edges.push_back(Edge(1, 9));
    g.vertices[2].edges.push_back(Edge(2, 10));
    g.vertices[2].edges.push_back(Edge(4, 11));
    g.vertices[2].edges.push_back(Edge(6, 2));

    g.vertices[3].edges.push_back(Edge(2, 15));
    g.vertices[3].edges.push_back(Edge(3, 11));
    g.vertices[3].edges.push_back(Edge(5, 6));

    g.vertices[4].edges.push_back(Edge(4, 6));
    g.vertices[4].edges.push_back(Edge(6, 9));

    g.vertices[5].edges.push_back(Edge(1, 14));
    g.vertices[5].edges.push_back(Edge(3, 2));
    g.vertices[5].edges.push_back(Edge(5, 9));

    std::vector<int> distances = dijkstra(g, 0);
    for (size_t i = 0; i < distances.size(); ++i)
        std::cout << distances[i] << "\n";

    return 0;
}