路径规划——Floyd算法

路径规划——Floyd算法

算法原理

Floyd算法是一个基于贪心、动态规划来求一个图中所有点到所有点最短路径的算法，适用于带权有向图或无向图，可以处理负权边，但不能处理负权回路，时间复杂度$O(n^3)$。
$$\begin{gathered} D[i][j]=min(D[i][j],D[i][k]+D[k][j]) \\ i,j分别指行、列; \\ k指起点到终点途中除终点外的所有节点数; \\ D[i][j]表示从i节点到j节点的最短路径; \\ D[i][k]表示从i节点到k节点的最短路径; \\ D[k][j]表示从k节点到j节点的最短路径。 \end{gathered}$$
通过上式规则，逐步引入中间节点k的信息来改进从i到j的最短路径估计。

算法流程：

1.首先初始化图中的邻接矩阵，D [i] [j]初始化为图中i到j的边权，如果i和j之间没有直接边，则设置为无穷大(∞)，自环的距离为0。
2.更新上述邻接矩阵为距离矩阵，具体是使用三重循环遍历所有顶点对和中间点来更新距离矩阵。在此过程中主要思想是对于每对顶点(i,j)，检查是否通过某个中间点k可以找到更短的路径。
3.检测负权回路，Floyd算法可以检测负权回路（即从某个顶点出发经过若干条边后回到该顶点的总权重为负），但无法处理负权回路。在完成所有的更新后，检查对角线元素D[i] [i] 是否小于0.如果小于0则表示存在负权回路。

以下图为例，逐步计算一下：

计算流程如下：

到此Floyd便结束了，得到了最终结果——每组点对间的最短路径距离。
如果还需要最短路径距离对应的最短路径则需要另外存储每次循环时的路径并更新。

算法实现

import numpy as np

def build_adjacency_matrix(vertices, edges):
    """
        创建邻接矩阵
    """
    V = len(vertices)
    # 初始化邻接矩阵，所有值设为无穷大，主对角线设为0
    adj_matrix = np.full((V, V), float('inf'))
    np.fill_diagonal(adj_matrix, 0)
    
    for (u, v, weight) in edges:
        adj_matrix[u][v] = weight
        adj_matrix[v][u] = weight
        
    return adj_matrix

def floyd_warshall(graph):
    """
        Floyd 算法
    """
    V = len(graph)
    dist = np.array(graph, dtype=float)
    
    # 算法核心：三重嵌套循环
    for k in range(V):
        for i in range(V):
            for j in range(V):
                if dist[i][k] + dist[k][j] < dist[i][j]:
                    dist[i][j] = dist[i][k] + dist[k][j]
    
    # 检测负权回路
    for i in range(V):
        if dist[i][i] < 0:
            print("Negative weight cycle detected")
            return None
    
    return dist

def res(dist):
    V = len(dist)
    print("Shortest distances between every pair of vertices:")
    for i in range(V):
        for j in range(V):
            if dist[i][j] == float('inf'):
                # print(f"dist[{i}][{j}] = INF", end='\t')
                print(f"INF", end='\t')
            else:
                # print(f"dist[{i}][{j}] = {dist[i][j]:.2f}", end='\t')
                print(f"{dist[i][j]:.2f}", end='\t')
        print()

if __name__ == "__main__":
    # 定义图的顶点和边
    vertices = [0, 1, 2, 3, 4, 5, 6, 7, 8]
    edges = [
        (0, 1, 10),
        (0, 5, 11),
        (1, 2, 18),
        (1, 6, 16),
        (1, 7, 12),
        (2, 3, 22),
        (2, 7, 8),
        (3, 4, 20),
        (3, 6, 24),
        (3, 7, 21),
        (3, 8, 16),
        (4, 5, 26),
        (4, 8, 7),
        (5, 6, 17),
        (6, 8, 19)
    ]
    
    # 从边列表构建邻接矩阵
    adj_matrix = build_adjacency_matrix(vertices, edges)
    
    # 运行Floyd-Warshall算法
    dist = floyd_warshall(adj_matrix)
    
    if dist is not None:
        # 打印最短路径矩阵
        res(dist)

输出结果：

如果在程序中需要输出每组点对间的最短路径，则需要使用列表来存储路径，在哪个节点处更新的距离值则在该节点处更新路径列表。