poj Dungeon Master (bfs)

http://poj.org/problem?id=2251

#include<stdio.h>
#include<string.h>
#include<queue>
#include<iostream>
using namespace std;
#define max 999999
#define N 40
int l,r,c,ans,ex,ey,ez;
char map[N][N][N];
int vis[N][N][N];
int dx[6]={-1,0,1,0,0,0};
int dy[6]={0,1,0,-1,0,0};
int dz[6]={0,0,0,0,1,-1};
int inmap(int x,int y,int z)
{
    if(x>=0&&x<l&&y>=0&&y<r&&z>=0&&z<c)return 1;
    else  return 0;
}
struct node
{
    int x;
    int y;
    int z;
    int num;
};
queue<node>q;
void init()
{
    while(!q.empty())q.pop();

    ans=max;
    memset(vis,0,sizeof(vis));

}
void  bfs(int x,int y,int z)
{
    int i,x1,y1,z1;
    struct node p,k;
    p.x=x;
    p.y=y;
    p.z=z;
    p.num=0;
    q.push(p);
    while(!q.empty())
    {
        p=q.front();
        q.pop();
        for(i=0;i<6;i++)
        {
            x1=p.x+dx[i];
            y1=p.y+dy[i];
            z1=p.z+dz[i];
           if(!inmap(x1,y1,z1)) continue;
           if(x1==ex&&y1==ey&&z1==ez)
         {
             ans=p.num+1;
             return ;
          }
         else
         {
            if(map[x1][y1][z1]!='#'&&!vis[x1][y1][z1])
            {
                vis[x1][y1][z1]=1;
                k.x=x1;
                k.y=y1;
                k.z=z1;
                k.num=p.num+1;
                q.push(k);
            }
         }

        }
    }
}
int main()
{
    int i,j,d;
    int sx,sy,sz;
    while(scanf("%d%d%d",&l,&r,&c),l+r+c)
    {
        getchar();
        for(i=0;i<l;i++)
        {
            for(j=0;j<r;j++)
         {


            for(d=0;d<c;d++)
            {
                scanf("%c",&map[i][j][d]);
                if(map[i][j][d]=='S')
                {
                    sx=i;
                    sy=j;
                    sz=d;
                }

                if(map[i][j][d]=='E')
                {
                    ex=i;
                    ey=j;
                    ez=d;
                }

            }
            getchar();
         }
            getchar();
        }

       init();
       vis[sx][sy][sz]=1;
        bfs(sx,sy,sz);
        if(ans!=max)
        printf("Escaped in %d minute(s).\n",ans);
        else printf("Trapped!\n");

    }

}