洛谷 1979 华容道 [NOIP2013] bfs 50分

题目：
https://www.luogu.org/problem/show?pid=1979

暴力50，（听说可以到70）；
看不懂题解，以后再改吧，我还是太弱……唉……；

#include<iostream>
#include<cstdio>
#include<cstring>
#include<algorithm>
#include<queue>
#include<map>
using namespace std;
int n,m,Q,sx,sy,zx,zy,kx,ky,fx,fy,t;
bool vis[32][32][32][32];
struct hh
{
    int ma[35][35],x,y,step,x1,y1;
}s_sta,x_sta,now,n_p;
int X[]={0,1,0,-1,0};
int Y[]={0,0,-1,0,1};
void init()
{
    memset(vis,0,sizeof(vis));
    return;
}
int bfs() 
{
    queue<hh>q;
    x_sta=s_sta,x_sta.x=kx,x_sta.y=ky,x_sta.x1=sx;
    x_sta.y1=sy,x_sta.step=0,vis[kx][ky][sx][sy]=1,q.push(x_sta);
    while(!q.empty())
    {
        now=q.front(),n_p;
        q.pop();
        if(now.x1==zx && now.y1==zy) return now.step;
        for(int i=1;i<=4;i++)
        {
            fx=X[i]+now.x,fy=Y[i]+now.y,n_p=now;
            if(now.ma[fx][fy]!=0 && (fx<=n && fx>=1 && fy<=m && fy>=1))
            {
                if(fx==n_p.x1 && fy==n_p.y1) n_p.x1=n_p.x,n_p.y1=n_p.y;
                t=n_p.ma[fx][fy],n_p.ma[fx][fy]=n_p.ma[n_p.x][n_p.y];
                n_p.ma[n_p.x][n_p.y]=t;
                n_p.x=fx,n_p.y=fy;
                if(!vis[n_p.x][n_p.y][n_p.x1][n_p.y1])
                {
                    n_p.step=now.step+1;
                    vis[n_p.x][n_p.y][n_p.x1][n_p.y1]=1;
                    q.push(n_p);
                }
            }
        }
    }
    return -1;
}
void solve()
{
    scanf("%d%d%d",&n,&m,&Q);
    for(int i=1;i<=n;i++)
        for(int j=1;j<=m;j++) 
            scanf("%d",&s_sta.ma[i][j]);
    while(Q--)
    {
        init();
        scanf("%d%d%d%d%d%d",&kx,&ky,&sx,&sy,&zx,&zy);
        if(sx==zx && sy==zy) {cout<<"0"<<endl;continue;}
        cout<<bfs()<<endl;
    }
}
int main()
{
    solve();
    return 0;
}