poj2251 三维搜索 利用优先队列

从S到E问需要多长时间，每走一格浪费 1 minute

#include<iostream>
#include<cstdio>
#include<cstring>
#include<algorithm>
#include<queue>
using namespace std;
struct node
{
    int x;
    int y;
    int z;
    int step;
    bool friend operator<(node a,node b)
    {
        return a.step>b.step;
    }
};
char map[30][30][30];
int vis[30][30][30];
int L,R,C;
int sx,sy,sz,ex,ey,ez;
int code[6][3]= {{0,-1,0},{0,0,1},{0,1,0},{0,0,-1},{1,0,0},{-1,0,0}};
int ans=0;
void BFS(int x,int y,int z)
{
    int flag=0;
    priority_queue<node>q;
    node p,tem;
    p.x=x;
    p.y=y;
    p.z=z;
    p.step=0;
    q.push(p);
    while(!q.empty())
    {
        tem=q.top();
        q.pop();
        for(int i=0; i<6; i++)
        {
            int xx=tem.x+code[i][0];
            int yy=tem.y+code[i][1];
            int zz=tem.z+code[i][2];
            if(xx>=0&&xx<L&&yy>=0&&yy<R&&zz>=0&&zz<C&&map[xx][yy][zz]!='#'&&vis[xx][yy][zz]==0)
            {

                vis[xx][yy][zz]=1;
                p.x=xx;
                p.y=yy;
                p.z=zz;
                p.step=tem.step+1;
                if(p.x==ex&&p.y==ey&&p.z==ez)
                {
                    flag=1;
                    ans=p.step;
                    break;
                }
                q.push(p);
            }
        }
        if(flag==1)
            break;
    }
}
int main()
{
    while(scanf("%d%d%d",&L,&R,&C)!=EOF)
    {
        ans=0;
        memset(vis,0,sizeof(vis));
        if(L==R&&R==C&&R==0)
            break;
        for(int i=0; i<L; i++)
            for(int j=0; j<R; j++)
                for(int k=0; k<C; k++)
                {
                    cin>>map[i][j][k];
                    if(map[i][j][k]=='S')
                    {
                        sx=i;
                        sy=j;
                        sz=k;
                    }
                    if(map[i][j][k]=='E')
                    {
                        ex=i;
                        ey=j;
                        ez=k;
                    }
                }
       // printf("a=%d %d %d\n",ex,ey,ez);
        vis[sx][sy][sz]=1;
        BFS(sx,sy,sz);
        if(ans==0)
            printf("Trapped!\n");
        else
            printf("Escaped in %d minute(s).\n",ans);
    }
    return 0;
}