利用广度优先算法实现路径搜索

问题描述：给定一个32*32的地图，地图中有若干障碍，使用蓝色方块画出，设计算法找到一条路径能够不碰障碍的从图的左上角转移到右下角，并用绿色线画出。

思路：首先使用BFS对全地图进行搜索，找出除去障碍的点之外的到达每一个点的最短步数。然后再依次的绘出当前点和其周围最小步数点的路径。

不用多说你也能看出来这是作业。。。代码很粗糙，很多该用变量表示的地方用了特定值，仅仅为了展示效果而实现。

%BFS路径搜索
%Date-2015-12-29
%-----初始化-----
clear all;
clc
z=zeros(32,32);
grd=zeros(32,32);
grd(1,:)=1;
grd(32,:)=1;
grd(:,1)=1;
grd(:,32)=1;
grd(6:12,7:12)=1;
grd(20:27,4:8)=1;
grd(16,22:27)=1;
grd(10:27,18)=1;
grd(7:16,27)=1;
grd(24,12:19)=1;
gimag=grd;
grd1=grd;
Tempx=2;
Tempy=2;
grd(Tempx,Tempy)=2;
grd(31,31)=3131; 
distancemin=64;
route=zeros(32,32);%32,32
route(2,2)=1;
length=5;
 flag=0;
 dirc=0;
 
 xqueue = zeros(3131);
 yqueue = zeros(3131);
 xqueue(1) = 2;
 yqueue(1) = 2;
 startflag = 1;
 endflag = 1;
 
 while 1
     for i=-1:1
         for j= -1:1;
             if grd(xqueue(startflag)+i,yqueue(startflag)+j)==0||grd(xqueue(startflag)+i,yqueue(startflag)+j)>grd(xqueue(startflag),yqueue(startflag))+1
                grd(xqueue(startflag)+i,yqueue(startflag)+j)=grd(xqueue(startflag),yqueue(startflag))+1;
                xqueue(endflag+1) = xqueue(startflag)+i;
                yqueue(endflag+1) = yqueue(startflag)+j;
                endflag = endflag+1;
                
             end
         end
     end
     if grd(31,31) ~= 3131
         break;
     end
     startflag = startflag + 1;
 end
 
 xnow = 2;
 ynow = 2;
 z(2,2) = 1;
 for i = 1:100
    max = 0;
    for p = 1:-1:-1
        for q = 1:-1:-1
            if grd(xnow+p,ynow+q)>max&&z(xnow+p,ynow+q)==0
                max = grd(xnow+p,ynow+q);
                
                xnowmax = xnow+p;
                ynowmax = ynow+q;
            end
        end
    end
    xnow = xnowmax;
    ynow = ynowmax;
    route(xnowmax,ynowmax) = 1;
    z(xnowmax,ynowmax)=1;
    imh = image(cat(3,z,route,gimag));
     set(imh, 'cdata', cat(3,z,route,gimag));
     drawnow; 
     pause(0.1);
     if xnow==31&&ynow==31
         break;
     end
 end
 
% 

效果：

来自为知笔记(Wiz)