学步园

这题刚开始时一直TLE，后来改了之后，一直wa,,重写一遍就过了。。

至今不知道原来错在哪里。。

郁闷啊。。

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <queue>

using namespace std;

int N, M, T, x1, x2, y1, y2;
int map[1010][1010];
int mark[1010][1010];
const int inf = 0x7f7f7f7f;

void debug( )
{
    #ifdef P
    freopen("in.txt","r",stdin);
    freopen("out.txt","w",stdout);
#endif
}

void init(  )
{
int i, j;
for  (i = 0; i < 1005; i++)
for (j = 0; j < 1005; j++)
            mark[i][j] = inf;
}



int xx[ ]= {0, 1, -1, 0};
int yy[ ]= {1, 0, 0, -1};


int judge( int x, int y)
{
if ( x > 0 && x <= N && y > 0 && y <= M && ( !map[x][y] || (x == x2 && y == y2)) )
return 1;
return  0;
}



struct node
{
int x, y, num, di;
}pp;


int BFS(  )
{
int i;
struct node pp, kk;
    init( );
    queue<node>q;
    pp.x = x1, pp.y = y1, pp.di = -1, pp.num = 0;
    q.push(pp);
while ( !q.empty())
    {
        kk = q.front( );
        q.pop( );
if (kk.x == x2 && kk.y == y2)
return 1;
for (i = 0; i < 4; i++)
        {
            pp = kk;
            pp.x += xx[i];
            pp.y += yy[i];
if(pp.di == -1)
                pp.di = i, pp.num = 0;
else if ( pp.di != i)
                pp.di = i, pp.num++;
if (pp.num  > 2) continue;
if (judge(pp.x, pp.y))
            {   


if ( mark[pp.x][pp.y] > pp.num)
                {
                    q.push(pp);
                    mark[pp.x][pp.y] = pp.num;
                }
            }
        }
    }
return 0;
}



int main( )
{  
int i, j;
    debug( );
while ( scanf("%d%d", &N, &M), N || M ) {
for(i = 1; i <= N; i++)
for(j = 1; j <= M; j++)
                scanf("%d",&map[i][j]);

        scanf("%d", &T);
for( i = 1; i <= T; i++) {
         scanf("%d%d%d%d", &x1, &y1, &x2, &y2);
if ( ( map[x1][y1] != map[x2][y2] ) || !map[x1][y1] || (x1==x2 && y1==y2) ) {
            puts("NO");
continue;
        }
if ( BFS( ) )    
            puts("YES");
else
            puts("NO");
        }
    }
return 0;
}