Giúp đỡ về thuật toán Floyd tìm đường đi ngắn nhất

[thaidz199x (LemonTree)]

Chào các bác, e cài đặt thuật toán Floyd với đồ thị như trong ảnh
Dữ Liệu đầu vào là ma trận trọng số giữa các cạnh (giá trị 999 hiểu ngầm là vô cực)
sau khi chạy tt Floyd thì kết quả cho ra mảng 2 chiều A[][] với A[ i ][ j ] là tổng đường đi ngắn nhất từ i đến j

• E gặp vấn đề ở phần TRUY VẾT mảng A, khi nhập đỉnh đầu = 5 và đỉnh cuối = 2 thì kết quả cho ra 5->2->1->6->4->3 thay vì KẾT QUẢ CHÍNH XÁC là 5->4->2 (tổng trọng số =33)
  Ai có ý tưởng nào về TRUY VẾT giúp em với ạ , hoặc tag link của 1 số tài liệu có thể giải quyết khó khăn của e đang vướng phải . E cảm ơn mọi người

[noname00 (HK boy)]

Gọi small[u][v] là đỉnh mà muốn đạt được đường đi ngắn nhất từ u -> v thì phải đi qua nó.

• Nếu không có đường đi ngắn nhất từ u -> v, small[u][v] = -1.
• Nếu u -> v có cạnh nối trực tiếp, small[u][v] = u.

Tại bước gán f[u][v], thêm bước tính small[u][v]:

for (k...)
    for (u...)
        for (v...)
            if (f[u][v] > f[u][k] + f[k][v]) {
                f[u][v] = f[u][k] + f[k][v];
                small[u][v] = k;
            }

Truy vết:

// pseudo-code
array trace(src, dest) {
    if (src == dest)
        return {src}

    mid = small[src][dest]

    if (mid != -1) {
        if (mid == src || mid == dest)
            return {src, dest}

        return (trace(src, mid) \ {mid}) + trace(mid, dest)
    }
    return {}
}

Mình truy vết một cách thủ công, không biết có cách nào chuẩn hơn không