g2d.m

粒子群算法的matlab程序在有障碍的路径图中寻找最短路径的程序。

function D=G2D(G)
%Construct Adjacency Matrix by path graph
% G is matrix of 0 and 1, 1 represents obstacles 
a=1;
N=size(G,1);
D=inf*ones(N^2,N^2);
for i=1:(N^2)
    x=ceil(i/N-0.00001);
    y=mod(i,N);
    if y==0
        y=N;
    end
    x1=x-1;y1=y-1;
    if x1>=1&&y1>=1
        j=(x1-1)*N+y1;
        D(i,j)=1.414*a;
        D(j,i)=1.414*a;
    end
    x2=x-1;y2=y;
    if x2>=1
        j=(x2-1)*N+y2;
        D(i,j)=a;
        D(j,i)=a;
    end
    x3=x-1;y3=y+1;
    if x3>=1&&x3<=N
        j=(x3-1)*N+y3;
        D(i,j)=1.414*a;
        D(j,i)=1.414*a;
    end
    x4=x;y4=y-1;
    if y4>=1
        j=(x4-1)*N+y4;
        D(i,j)=a;
        D(j,i)=a;
    end
    x5=x;y5=y+1;
    if y5<=N
        j=(x5-1)*N+y5;
        D(i,j)=a;
        D(j,i)=a;
    end        
    x6=x+1;y6=y-1;
    if x6<=N&&y6>=1
        j=(x6-1)*N+y6;
        D(i,j)=1.414*a;
        D(j,i)=1.414*a;
    end
    x7=x+1;y7=y;
    if x7<=N
        j=(x7-1)*N+y7;
        D(i,j)=a;
        D(j,i)=a;
    end
    x8=x+1;y8=y+1;
    if x8<=N&&y8<=N
        j=(x8-1)*N+y8;
        D(i,j)=1.414*a;
        D(j,i)=1.414*a;
    end
end
for x=1:N
    for y=1:N
        if G(x,y)==1
            J=(x-1)*N+y;
            D(:,J)=inf*ones(N^2,1);
            D(J,:)=inf*ones(1,N^2);
        end
    end
end
for i=1:(N-1)
    x=i*N+1;
    y=(i+1)*N;
    D(x,y)=inf;
    D(y,x)=inf;
end