C - Push!!

Mr. Schwarz was a famous powerful pro wrestler. He starts a part time job as a warehouseman. His task is to move a cargo to a goal by repeatedly pushing the cargo in the warehouse, of course, without breaking the walls and the pillars of the warehouse.
There may be some pillars in the warehouse. Except for the locations of the pillars, the floor of the warehouse is paved with square tiles whose size fits with the cargo. Each pillar occupies the same area as a tile.

Initially, the cargo is on the center of a tile. With one push, he can move the cargo onto the center of an adjacent tile if he is in proper position. The tile onto which he will move the cargo must be one of (at most) four tiles (i.e., east, west, north or south) adjacent to the tile where the cargo is present.
To push, he must also be on the tile adjacent to the present tile. He can only push the cargo in the same direction as he faces to it and he cannot pull it. So, when the cargo is on the tile next to a wall (or a pillar), he can only move it along the wall (or the pillar). Furthermore, once he places it on a corner tile, he cannot move it anymore.
He can change his position, if there is a path to the position without obstacles (such as the cargo and pillars) in the way. The goal is not an obstacle. In addition, he can move only in the four directions (i.e., east, west, north or south) and change his direction only at the center of a tile.
As he is not so young, he wants to save his energy by keeping the number of required pushes as small as possible. But he does not mind the count of his pedometer, because walking is very light exercise for him.
Your job is to write a program that outputs the minimum number of pushes required to move the cargo to the goal, if ever possible.
Input
The input consists of multiple maps, each representing the size and the arrangement of the warehouse. A map is given in the following format.

w h
d11 d12 d13 … d1w
d21 d22 d23 … d2w
…
dh1 dh2 dh3 … dhw

The integers w and h are the lengths of the two sides of the floor of the warehouse in terms of widths of floor tiles. w and h are less than or equal to 7. The integer dij represents what is initially on the corresponding floor area in the following way.
0:
nothing (simply a floor tile)
1:
pillar
2:
the cargo
3:
the goal
4:
the warehouseman (Mr. Schwarz)
Each of the integers 2, 3 and 4 appears exactly once as dij in the map. Integer numbers in an input line are separated by at least one space character. The end of the input is indicated by a line containing two zeros.
Output
For each map, your program should output a line containing the minimum number of pushes. If the cargo cannot be moved to the goal, `-1’ should be output instead.
Sample Input
5 5
0 0 0 0 0
4 2 0 1 1
0 1 0 0 0
1 0 0 0 3
1 0 0 0 0
5 3
4 0 0 0 0
2 0 0 0 0
0 0 0 0 3
7 5
1 1 4 1 0 0 0
1 1 2 1 0 0 0
3 0 0 0 0 0 0
0 1 0 1 0 0 0
0 0 0 1 0 0 0
6 6
0 0 0 0 0 3
0 0 0 0 0 0
0 0 0 0 0 0
0 0 0 0 0 0
0 2 0 0 0 0
4 0 0 0 0 0
0 0
Sample Output
5
-1
11
8

題意：求箱子到達終點的最少步數。

bfs求解，枚舉人的狀態從而推出箱子的狀態。

代碼：

#include <cstdio>
#include <cstring>
#include <iostream>
#include <algorithm>
#include <queue>

using namespace std;

int dx[4]={1,-1,0,0};
int dy[4]={0,0,-1,1};
int vis[8][8][8][8];
int map[8][8];
int m,n;
struct node
{
    int xp,yp,xb,yb;
    int step;
}cur,tmp,gg;

int jud(int x1,int y1)
{
    if(x1>=n||y1>=m||x1<0||y1<0) return 0;

    return 1;
}
int bfs()
{
    queue <node> que;
    que.push(gg);

    int ans=0x3f3f3f3f;

    while(!que.empty())
    {
        tmp=que.front();
        que.pop();

        if(map[tmp.xb][tmp.yb]==3)
        {
            ans=min(ans,tmp.step);
            //箱子被移到終點
        }

        for(int i=0;i<4;i++)
        {
            //枚舉人
            cur.xp=tmp.xp+dx[i];
            cur.yp=tmp.yp+dy[i];
            //當前沒走過或者當前步數比過往步數少並且是可走的
            if( (vis[cur.xp][cur.yp][tmp.xb][tmp.yb]==-1 || tmp.step<vis[cur.xp][cur.yp][tmp.xb][tmp.yb]) && map[cur.xp][cur.yp]!=1 && jud(cur.xp,cur.yp) )
            {
                if(cur.xp==tmp.xb&&cur.yp==tmp.yb)
                {
                    //當人是箱子的上一狀態
                    cur.xb=cur.xp+dx[i];
                    cur.yb=cur.yp+dy[i];
                    if(jud(cur.xb,cur.yb))//箱子可移
                    {
                        cur.step=tmp.step+1;
                        vis[cur.xp][cur.yp][cur.xb][cur.yb]=cur.step;
                        que.push(cur);
                    }
                }

                if(!(cur.xp==tmp.xb&&cur.yp==tmp.yb))
                {
                    //當人不是箱子的上一狀態
                    //箱子保持原狀態
                    cur.xb=tmp.xb;
                    cur.yb=tmp.yb;
                    cur.step=tmp.step;
                    vis[cur.xp][cur.yp][cur.xb][cur.yb]=cur.step;
                    que.push(cur);
                }
            }
        }
    }

    if(ans==0x3f3f3f3f)
        return -1;
    else
        return ans;

}
int main (void)
{
    while(~scanf("%d %d",&m,&n))
    {
        memset(vis,-1,sizeof(vis));
        if(m==0&&n==0)
            break;

        for(int i=0;i<n;i++)
        {
            for(int j=0;j<m;j++)
            {
                scanf("%d",&map[i][j]);
                if(map[i][j]==4)
                {
                    gg.xp=i;
                    gg.yp=j;
                    map[i][j]=0;
                }
                if(map[i][j]==2)
                {
                    gg.xb=i;
                    gg.yb=j;
                    map[i][j]=0;
                }
            }
        }

        gg.step=0;
        vis[gg.xp][gg.yp][gg.xb][gg.yb]=0;

        int ans=bfs();
        printf("%d\n",ans);
    }
    return 0;
}