(POJ 3984)迷宫问题

どこでもドア: poj.org/problem?id=3984
BFS入门，记录最短路径

#include<iostream>
#include<cstdio>
#include<algorithm>
#include<cmath>
#include<cstring>
#include<string>
#include<cstdlib>
#include<sstream>
#include<cctype>
#include<set>
#include<queue>
#include<stack>
#include<vector>
#include <iterator>
using namespace std;
int ma[5][5],vis[5][5];
int dis[4][2]={0,1,1,0,-1,0,0,-1};
struct pos{
    int x,y,step;
    friend bool operator<(pos x1,pos x2){
        return x1.step>x2.step;
    }
};
bool check(int x,int y){
    if(x>=0&&x<5&&y>=0&&y<5&&!vis[x][y]&&!ma[x][y])
        return 1;
    return 0;
}
stack <pos> ANS;
int BFS(){
    pos next,p;
    priority_queue<pos> Q;
    p.x=0;p.y=0;p.step=0;
    Q.push(p);
    while(!Q.empty()){
        p=Q.top();
        ANS.push(p);
        Q.pop();
        if(p.x==4&&p.y==4)
            return p.step;
        vis[p.x][p.y]=1;
        for(int i=0;i<4;i++){
            next.x=p.x+dis[i][0];
            next.y=p.y+dis[i][1];
            if(check(next.x,next.y)){
                next.step=p.step+1;
                Q.push(next);
            }
        }
    }
    return -1;
}
int main()
{
    for(int i=0;i<5;i++)
        for(int j=0;j<5;j++)
        cin>>ma[i][j];
    int fla=BFS();
    if(fla!=-1)
    {
        stack<pos> ans;
        ans.push(ANS.top());
        ANS.pop();
        int si=4,sj=4;
        fla--;
        while(!ANS.empty()){
            if(fla==ANS.top().step&&(abs(si-ANS.top().x)+abs(sj-ANS.top().y))==1)
            {
                ans.push(ANS.top());
                fla--;
                si=ans.top().x;
                sj=ans.top().y;
            }
            ANS.pop();
        }
        while(!ans.empty()){
            cout<<"("<<ans.top().x<<", "<<ans.top().y<<")"<<endl;
            ans.pop();
        }
    }
    else cout<<"-1"<<endl;
    return 0;
}