Алгоритм Дейкстры

#include <iostream>
#include <string>
#include <fstream>
#include <vector>
#include <limits>
#include <stack>

const int INF = std::numeric_limits<int>::max();

struct Node
{
    int id;
};

struct NodeWrapper
{
    Node* node;
    bool visited = false;
    int weight = INF;
    NodeWrapper* prev_node = nullptr;
};

struct Edge
{
    Node* start;
    Node* end;
    int weight;
};

struct EdgeWrapper
{
    NodeWrapper* start;
    NodeWrapper* end;
    int weight;
};

struct Graph
{
    std::vector<Node> nodes;
    std::vector<Edge> edges;
};

struct GraphWrapper
{
    std::vector<NodeWrapper> nodes;
    std::vector<EdgeWrapper> edges;

    GraphWrapper(Graph& graph)
    {
        nodes.reserve(graph.nodes.size());
        for (auto& node : graph.nodes)
        {
            nodes.push_back(NodeWrapper{ &node });
        }
        edges.reserve(graph.edges.size());
        for (auto& edge : graph.edges)
        {
            auto start_iter = std::find_if(nodes.begin(), nodes.end(), [&edge](const auto& node_wrapper) { return edge.start == node_wrapper.node; });
            auto end_iter = std::find_if(nodes.begin(), nodes.end(), [&edge](const auto& node_wrapper) { return edge.end == node_wrapper.node; });
            edges.push_back(EdgeWrapper{ &(*start_iter), &(*end_iter), edge.weight });
        }
    }
};

bool read_nodes(std::istream& is, int node_count, std::vector<Node>& nodes_out)
{
    nodes_out.clear();
    for (int i = 0; i < node_count; i++)
    {
        Node node;
        is >> node.id;
        nodes_out.push_back(node);
    }
    return true;
}

bool read_edges(std::istream& is, int edges_count, const std::vector<Node>& nodes, std::vector<Edge>& edges_out)
{
    edges_out.clear();
    for (int i = 0; i < edges_count; i++)
    {
        decltype(Node::id) start_id, end_id;
        decltype(Edge::weight) weight;

        is >> start_id >> end_id >> weight;
        auto start_iter = std::find_if(nodes.begin(), nodes.end(), [start_id](const auto& node) {return node.id == start_id; });
        auto end_iter = std::find_if(nodes.begin(), nodes.end(), [end_id](const auto& node) {return node.id == end_id; });

        if (start_iter == nodes.end() || end_iter == nodes.end())
        {
            edges_out.clear();
            return false;
        }

        edges_out.push_back(Edge
            {
                const_cast<Node*>(&(*start_iter)),
                const_cast<Node*>(&(*end_iter)),
                weight
            });
    }
    return true;
}

Graph read_graph(const std::string& file_path)
{
    Graph result;
    std::ifstream fin(file_path);
    if (fin)
    {
        int nodes_count, edges_count;
        fin >> nodes_count >> edges_count;
        if (read_nodes(fin, nodes_count, result.nodes))
        {
            read_edges(fin, edges_count, result.nodes, result.edges);
        }
        fin.close();
    }
    return result;
}

std::vector<NodeWrapper>::iterator find_next_node(std::vector<NodeWrapper>& nodes)
{
    auto result = nodes.end();
    for (auto iter = nodes.begin(); iter != nodes.end(); ++iter)
    {
        if (result == nodes.end())
        {
            if (!(*iter).visited)
            {
                result = iter;
            }
        }
        else
        {
            if (!(*iter).visited && (*iter).weight < (*result).weight)
            {
                result = iter;
            }
        }
    }
    return result;
}

std::vector<Node*> find_path(Graph& graph, Node* start, Node* end)
{
    GraphWrapper wrapper(graph);
    std::vector<Node*> path;
    for (int i = 0; i < wrapper.nodes.size(); i++)
    {
        if (wrapper.nodes[i].node == start)
        {
            wrapper.nodes[i].weight = 0;
        }
    }

    auto min_iter = wrapper.nodes.end();

    do
    {
        min_iter = find_next_node(wrapper.nodes);
        if (min_iter != wrapper.nodes.end())
        {
            for (int i = 0; i < wrapper.edges.size(); i++)
            {
                if (wrapper.edges[i].start == &(*min_iter) && !wrapper.edges[i].end->visited)
                {
                    if (wrapper.edges[i].start->weight + wrapper.edges[i].weight < wrapper.edges[i].end->weight)
                    {
                        wrapper.edges[i].end->weight = wrapper.edges[i].start->weight + wrapper.edges[i].weight;
                        wrapper.edges[i].end->prev_node = wrapper.edges[i].start;
                    }
                }
            }
            (*min_iter).visited = true;
        }

    } while (min_iter != wrapper.nodes.end());

    auto end_iter = std::find_if(wrapper.nodes.begin(), wrapper.nodes.end(), [end](const auto& wrapper) {return wrapper.node == end; });
    if (end_iter != wrapper.nodes.end())
    {
        NodeWrapper* current_node = &(*end_iter);
        std::stack<NodeWrapper*> stack;
        do
        {
            stack.push(current_node);
            current_node = current_node->prev_node;
        } while (current_node && current_node->weight != INF);

        while (!stack.empty())
        {
            path.push_back(stack.top()->node);
            stack.pop();
        }
    }

    return path;
}

int main()
{
    Graph graph = read_graph("input.txt");
    auto path = find_path(graph, &graph.nodes[0], &graph.nodes[4]);

    for (Node* node : path)
    {
        std::cout << node->id << " ";
    }

    return 0;
}