poj2251 16ms BFS

#include<cstdio>
#include<queue>
#include<cstring>
using namespace std;
//bfs水题，只是换成了三维
int l,r,c;
char matrix[35][35][35];
bool vis[35][35][35];
int nowx,nowy,nowz;
int dis[6][3]= {{1,0,0},{-1,0,0},{0,-1,0},{0,1,0},{0,0,1},{0,0,-1}};
struct Node
{
    int x;
    int y;
    int z;
    int steps;
    Node(int a,int b,int c,int cnt):x(a),y(b),z(c),steps(cnt) {}
};
queue<Node>q;
int BFS(int sx,int sy,int sz,int ex,int ey,int ez)
{
    q.push(Node(sx,sy,sz,0));
    vis[sx][sy][sz] = 1;
    while(!q.empty())
    {
        Node now = q.front();
        if(now.x==ex&&now.y==ey&&now.z==ez)
            return now.steps;
        q.pop();
        for(int i = 0; i<6; ++i)
        {
            nowx = now.x + dis[i][0];
            nowy = now.y + dis[i][1];
            nowz = now.z + dis[i][2];
            if(nowx>=0&&nowx<l&&nowy>=0&&nowy<r&&nowz>=0&&nowz<c)
            {
                if(!vis[nowx][nowy][nowz]&&matrix[nowx][nowy][nowz]!='#')
                {
                    q.push(Node(nowx,nowy,nowz,now.steps+1));
                    vis[nowx][nowy][nowz] = 1;
                }
            }
        }
    }
    return -1;
}
int main()
{
    int sx,sy,sz;
    int ex,ey,ez;
    int ans = 0;
    while(scanf("%d%d%d",&l,&r,&c),l)
    {
        memset(vis,0,sizeof(vis));
        while(!q.empty())
            q.pop();
        for(int i = 0; i<l; ++i)
        {
            getchar();
            for(int j = 0; j<r; ++j)
            {

                for(int k = 0; k<c; ++k)
                {
                    matrix[i][j][k] = getchar();
                    if(matrix[i][j][k]=='S')
                    {
                        sx = i;
                        sy = j;
                        sz = k;
                    }
                    else if(matrix[i][j][k]=='E')
                    {
                        ex = i;
                        ey = j;
                        ez = k;
                    }
                }
                getchar();
            }
        }
        ans = BFS(sx,sy,sz,ex,ey,ez);
        if(ans==-1)
            printf("Trapped!\n");
        else
            printf("Escaped in %d minute(s).\n",ans);
    }
    return 0;
}