matlab实现障碍物的环境建模

实现3个动态障碍物和若干个静态障碍物的环境建模，动态障碍物能显示其运动路径

function D=G2D（G） 
%Construct Adjacency Matrix by path graph 
% G is matrix of 0 and 1 1 represents obstacles  
a=1; 
N=size（G1）; 
D=inf*ones（N^2N^2）; 
for i=1:（N^2） 
    x=ceil（i/N-0.00001）; 
    y=mod（iN）; 
    if y==0 
        y=N; 
    end 
    x1=x-1;y1=y-1; 
    if x1>=1&&y1>=1 
        j=（x1-1）*N+y1; 
        D（ij）=1.414*a; 
        D（ji）=1.414*a; 
    end 
    x2=x-1;y2=y; 
    if x2>=1 
        j=（x2-1）*N+y2; 
        D（ij）=a; 
        D（ji）=a; 
    end 
    x3=x-1;y3=y+1; 
    if x3>=1&&x3<=N 
        j=（x3-1）*N+y3; 
        D（ij）=1.414*a; 
        D（ji）=1.414*a; 
    end 
    x4=x;y4=y-1; 
    if y4>=1 
        j=（x4-1）*N+y4; 
        D（ij）=a; 
        D（ji）=a; 
    end 
    x5=x;y5=y+1; 
    if y5<=N 
        j=（x5-1）*N+y5; 
  

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件       1502  2014-03-15 22:59  environment\G2D.m

     文件       2481  2014-03-15 23:22  environment\maincode.m

     目录          0  2014-03-16 09:45  environment

----------- ---------  ---------- -----  ----

                 3983                    3