Nightmare Ⅱ（双向BFS）

Nightmare Ⅱ

简直就是晕，都怪我太菜了

#include <iostream>
#include <cstring>
#include <cstdio>
#include <string>
#include <cmath>
#include <queue>
using namespace std;
const int dis[4][2]={{1,0},{-1,0},{0,1},{0,-1}};
char str[810][810];
//标记是否有鬼 
int used[2][810][810];
int g_x,g_y,m_x,m_y,n,m,step;
struct node{
	int x,y;
}start,z[2];
queue<node> q[2];
void init()
{
	int i,j,cnt;
	scanf("%d%d",&n,&m);
	for(int i=0;i<n;i++)
	{
		scanf("%s",str[i]);
	}
	cnt=0;
	for(int i=0;i<n;i++)
	{
		for(int j=0;j<m;j++)
		{
			if(str[i][j]=='G')
			{
				g_x=i;g_y=j;
			}
			if(str[i][j]=='M')
			{
				m_x=i;m_y=j;
			}
			if(str[i][j]=='Z')
			{
				z[cnt].x=i;
				z[cnt].y=j;
				cnt++;
			}
		}
	}
}
int judge(node b)
{
	if(b.x<0 || b.y<0 || b.x>=n || b.y>=m) return 0;
	if(str[b.x][b.y]=='X') return 0;
	//曼哈顿距离判断问题
	if((abs(b.x-z[0].x)+abs(b.y-z[0].y))<=2*step) return 0;
	if((abs(b.x-z[1].x)+abs(b.y-z[1].y))<=2*step) return 0;
	return 1;
}
int bfs(int w)
{
	node next,now;
	int sum=q[w].size();
	while(sum--)
	{
		now=q[w].front();
		q[w].pop();
		if(judge(now)==0) continue;
		for(int i=0;i<4;i++)
		{
			next.x=now.x+dis[i][0];
			next.y=now.y+dis[i][1];
			if(judge(next)==0) continue;
			if(used[w][next.x][next.y]==0)
			{
				if(used[1-w][next.x][next.y]==1) return 1;
				used[w][next.x][next.y]=1;
				q[w].push(next);
			}
		}
	}
	return 0;
}
int solve()
{
	while(!q[0].empty()) q[0].pop();
	while(!q[1].empty()) q[1].pop();
	start.x=m_x;
	start.y=m_y;
	q[0].push(start);
	start.x=g_x;
	start.y=g_y;
	q[1].push(start);
	memset(used,0,sizeof(used));
	used[0][m_x][m_y]=used[1][g_x][g_y]=1;
	step=0;
	while((!q[0].empty()) || (!q[1].empty()))
	{
		step++;
		if(bfs(0)==1) return step;
		if(bfs(0)==1) return step;
		if(bfs(0)==1) return step;
		if(bfs(1)==1) return step;
	}
	return -1;
}
int main()
{
	int t;
	scanf("%d",&t);
	while(t--)
	{
		init();
		printf("%d\n",solve());
	}
	return 0;
}