1 function [ optimal,path,maxnum ] = Floyd( distance,liantong,num,p,q )
 2 %Author:ljy 
 3 %Date:20170919
 4 %弗洛伊德算法求最优路径和记录下最优路径中的节点信息
 5 %distance为节点间的连通距离。通过已知的节点的X、Y坐标和连通矩阵计算而得。其中Inf为不连通节点间的距离。连通节点间的距离为正数（非Inf），连通情况通过连通（liantong）矩阵判断所得。  
 6 %num为所有节点个数
 7 %p和q为不考虑路径的两端节点编号。例如p=[129,129];q=[9,10];表示129到9、129到10的路不连通。
 8 %optimal为最优路径的距离信息
 9 %path为最优路径的路径信息
10 %maxnum：最长的最优路径的节点总数
11 %liantong：节点间的连通情况，0为不连通，1为连通。
　　%path：行和列都为节点编号。path(i,j)为i到j的节点编号。
12 maxnum = 2;
13 
14 
15 %将distance变为邻接矩阵
16 for i = 1:num
17     for j = 1:num
18         if distance(i,j) == 0 & i ~= j
19             distance(i,j) = Inf;
20         end
21     end
22 end
23 
24 %将p与q之间的路径的权重赋值为Inf，即不考虑p与q之间这条路径的最优路径。p、q为0表示没有不考虑的路径
25 for i = 1:size(p)
26     for j = 1:size(q)
27         if p(i) > 0 & q(j) > 0
28             distance(p(i),q(j)) = Inf;
29             distance(q(j),p(i)) = Inf;
30             liantong(p(i),q(j)) = 0;
31             liantong(q(j),p(i)) = 0;
32         end
33     end
34 end
35 
36 %核心算法
37  for k = 1:num
38     for i = 1:num
39         for j = 1:num
40             r = 2;%最优路径所包含的节点个数
41             if distance(i,j) > distance(i,k) + distance(k,j)
42                 distance(i,j) = distance(i,k) + distance(k,j);
43                 %存储最优路径中的节点
44                 p = i;
45                 if liantong(p,j) ~= 0
46                     while liantong(i,j) ~= liantong(p,k) & p ~= k
47                         liantong(p,j) = liantong(p,k);
48                         p = liantong(p,k);
49                         r = r + 1;
50                     end
51                 else
52                     liantong(p,j) = liantong(p,k);
53                     r = r + 1;
54                 end
55             end
56             if r > maxnum
57                 maxnum = r;
58             end
59         end
60     end
61  end
62  
63 %返回数据
64 optimal = distance;
65 path = liantong;
66 end