Matlab 广度优先搜索求解迷宫问题

function BFS_Maze(maze)

% maze:是迷宫矩阵，其中0表示可以去走的路
%                     1表示障碍
%                     2表示入口
%                     3表示出径
%                     5表示路径
%             0 2 0 0 1
%             0 1 1 0 1
%             0 1 3 0 1
%             0 1 0 0 1

% copyright: qbb 2013-3-12
if nargin == 0
    maze = [0 2 0 0 1
        0 1 1 0 1
        0 1 3 0 1
        0 1 0 0 1];
end
mazecopy = maze;
% 定义四个方向
directions = kron(eye(2),[-1,1]);
% 数组存储解的情况，I, J ,Pre
remark = zeros(100,3);
[I,J] = find(maze == 2);% 找到起点
remark(1,:) = [I,J,-1];
count = 2;
search(1);
sol = remark(count-1,:);
while(sol(end,3) > 0)
    sol = [sol;remark(sol(end,3),:)];
end
mazecopy(sub2ind(size(maze), sol(2:end-1,1), sol(2:end-1,2))) = 5;
disp(mazecopy);
disp(sol);
    function search(front)
        x = remark(front,1);
        y = remark(front,2);        
        for i = 1 : 4
            if cango(x + directions(1,i),y + directions(2,i))
                remark(count,1) = x + directions(1,i);
                remark(count,2) = y + directions(2,i);
                remark(count,3) = front;
                count = count + 1;
                if maze(x + directions(1,i),y + directions(2,i)) ~= 3
                    maze(x + directions(1,i),y + directions(2,i)) = 5;% 记录下来
                else
                    return;
                end
                
            end
        end
        search(front + 1);
    end
% 该函数判断该点是否可以经过
    function z = cango(x,y)
        % 用try来判断边界
        z = true;
        try
            if ismember(maze(x,y),[1,2,5])% 路障或者已经走过
                z = false;
            end
        catch
            z = false; % 边界
        end
    end
end