Восстановление пути по матрице, возвращаемой алгоритмом Флойда - Уоршелла

void FindPath(size_t first, size_t second)
    {
        if(first>=MatrPath.size() || second>=MatrPath.size())
            throw std::invalid_argument("One of nodes for searching is more than Matr size");
        ST Goals;
        Path.push(first);
        Goals.push(second);
        while(!Goals.empty())
        {
            int u=Path.top();
            int v=Goals.top();
            int s=MatrPath[u][v];
            if(v==s)
            {
                Path.push(v);
                Goals.pop();
            }
            else
                Goals.push(s);
        }
    }

#include <cstdio>
using namespace std;
 
const int N = 100; // max количество вершин в графе
const int INF = 1000000; // бесконечное расстояние, указывает на то, что между вершинами нет пути
 
int n; // кол-во вершин в графе
int adj[ N ][ N ] = { { 0 } }; // матрица смежности
int path[ N ][ N ]; // матрица путей
int dist[ N ][ N ]; // матрица расстояний
 
void read_graph()
{
// граф считывается следующим образом: сначала считывается кол-во вершин и кол-во ребер,
// а затем ребра в следующем формате: <начальная вершина> <конечная вершина> <расстояние между ними>
// замечание: данная функция считывает орграф
    int m;
 
    scanf( "%d%d", &n, &m );
    for ( int u, v, w; m--; adj[ u ][ v ] = w )
        scanf( "%d%d%d", &u, &v, &w );
}
 
void floid()
{
// инициализация матриц путей и расстояний
    for ( int u = 0; u < n; ++u )
        for ( int v = 0; v < n; ++v )
            if ( adj[ u ][ v ] || u == v )
            {
                path[ u ][ v ] = v;
                dist[ u ][ v ] = adj[ u ][ v ];
            }
            else
            {
                path[ u ][ v ] = N;
                dist[ u ][ v ] = INF;
            }
 
// сам алгоритм флойда
    for ( int k = 0; k < n; ++k )
        for ( int u = 0; u < n; ++u )
            if ( dist[ u ][ k ] != INF )
                for ( int v = 0; v < n; ++v )
                    if ( dist[ u ][ v ] > dist[ u ][ k ] + dist[ k ][ v ] )
                    {
                        dist[ u ][ v ] = dist[ u ][ k ] + dist[ k ][ v ];
                        path[ u ][ v ] = path[ u ][ k ];
                    }
}
 
void show_path( int u, int v )
{
// вывод пути из вершины u в v и расстояния между ними 
    if ( path[ u ][ v ] == N )
    {
        printf( "No path\n" );
        return;
    }
 
    int x = u;
 
    printf( "%d", x );
    while ( x != v )
        printf( " %d", x = path[ x ][ v ] );
    printf( " (%d)\n", dist[ u ][ v ] );
}
 
int main()
{
    int u, v;
 
    read_graph();
    floid();
 
    while ( true ) // обрабатываем запросы вывода путей и расстояний между вершинами пока не нажато ctrl-Z (ну или ctrl-D)
    {
        printf( "Enter source and destination vertices: " );
        if ( EOF == scanf( "%d%d", &u, &v ) )
            break;
        show_path( u, v );
    }
 
    return 0;
}

void show_path( int u, int v )
{
// вывод пути из вершины u в v и расстояния между ними 
    if ( path[ u ][ v ] == N )
    {
        printf( "No path\n" );
        return;
    }
 
    int x = u;
 
    printf( "%d", x );
    while ( x != v )
        printf( " %d", x = path[ x ][ v ] );
    printf( " (%d)\n", dist[ u ][ v ] );
}