Matlab的D算法

在看了很多D算法的代码后，还是有点看不懂，于是自己按照书上的步骤写了一个D算法的代码，对于D算法，F算法相对简单理解一点，能力有限，我感觉比别人的代码更加复杂，时间复杂度更大。。。

function [S,R,dis,bp2] = D_search(m,n,D,len)

R = zeros(len);
D0 = D;    %初始距离矩阵
S = D;     %最短距离矩阵
for i=1:len                       
    for j=1:len
        if(D(i,j) ~= Inf)    %对角线矩阵不进行计算
           R(i,j) = j;
        end
    end
end
R
for k=1:len  %经过n-1次循环的比较
    D1 = D;  %更新
    for i=1:len                       
        for j=1:len
            if(i ~= j)    %对角线矩阵不进行计算
                for t=1:len
                    D2(t) = D1(i,t)+D0(t,j);
                end
                [c w]= min(D2);
                 %i到j经过k步的距离最短者
                D(i,j) = c;   
                d = min([S(i,j) D(i,j)]);    %比较经过k步和不经过的更小者
                if(d ~= S(i,j))
                    R(i,j) = w;
                end
                S(i,j) = d;    %更新最短距离矩阵
            end
        end
    end
end
bp = zeros(1,len);
i = 1;
flag = 1;
bp(1,i) = n;
bp1 = bp(1,i);
R(m,bp(i));
while flag
    if(R(m,bp1)~=bp1) 
        bp(i) = R(m,bp1);
        bp1 = bp(1,i);
        i= i+1;
    else
        flag = 0;
    end
    if i>len
        flag = 0;
    end
end
for i=1:len
    if(bp(i))
        bp1(i) = bp(i);
    else
        break;
    end
end
num=length(bp1)
bp2(1) = m;
for j=1:num
    bp2(j+1)=bp1(num-j+1);
end
bp2(num+2) = n;
dis = S(m,n);