推箱子小游戏《格鲁的实验室》13关 - bfs最短路径

#include <queue>
#include <cstdio>
#include <cstring>
#include <iostream>
#include <algorithm>
using namespace std;
typedef pair<int,int> Coord;
struct Status{
    Coord y,g;
    int step,fid,id;
    Status(const Coord& y,const Coord& g,int step=0):y(y),g(g),step(step){fid = -1; id = 0;}
    const Status& setId(int id,int fid){
        this->id = id;
        this->fid = fid;
        return *this;
    }
    bool operator<(const Status& another) const { return step>another.step; }
    int hash(){ return y.first*1000+y.second*100+g.first*10+g.second; }

    friend ostream& operator<<(ostream& out,const Status& s){
        return (out<<"yellow("<<s.y.first<<","<<s.y.second<<") green("<<s.g.first<<","<<s.g.second<<") step:"<<s.step);
    }
};
Coord Y = {0,3};
Coord G = {5,2};
const int M = 6;
const int N = 8;
const int skip[4][2] = {{-1,0},{1,0},{0,1},{0,-1}};
bool vis[6868];
bool cango(int& x,int& y,int& dir,const Coord& another){
     int tx = x + skip[dir][0];
     int ty = y + skip[dir][1];
     if(tx<0||ty<0||tx>=M||ty>=N) return false;
     if(tx==another.first&&ty==another.second) return false;
     x = tx,y = ty;
     if(x==3&&y==2) dir=1;
     if(x==4&&y==3) dir=0;
     return true;
}
bool mov(Coord &a,const Coord& b,int dir){
     int len = 0;
     while(cango(a.first,a.second,dir,b)) len++;
     return len>0;
}
//for print path
vector<Status> trace;
void print(const Status& node){
     if(node.fid==-1){
        std::cout<<node<<endl; return ;
     }
     print(trace[node.fid]);
     std::cout<<node<<endl;
}

int main(){
    Coord y = {0,5};
    Coord g = {0,6};
    Status start(y,g);
    memset(vis,false,sizeof(vis));

    priority_queue<Status> q;
    q.push(start);
    vis[start.hash()] = true;
    trace.push_back(start);
    while(q.size()){
        Status cur = q.top(); q.pop();
        if(cur.g==G&&cur.y==Y){
           printf("step: %d\n",cur.step);
           print(cur);
           break;
        }
        for(int i=0;i<4;i++){
            Coord tmpy = cur.y;
            Coord tmpg = cur.g;
            if(mov(tmpy,cur.g,i)) {
              Status next(tmpy,cur.g,cur.step+1);
              if(!vis[next.hash()]){
                 trace.push_back(next.setId(trace.size(),cur.id));
                 q.push(next);
                 vis[next.hash()]=true;
              }
            }
            if(mov(tmpg,cur.y,i)){
              Status next(cur.y,tmpg,cur.step+1);
              if(!vis[next.hash()]){
                 trace.push_back(next.setId(trace.size(),cur.id));
                 q.push(next);
                 vis[next.hash()]=true;
              }
            }
        }
    }
    return 0;
}