迷宫的最短路径

#include<stdio.h>
#include<stdlib.h>
#include<string.h>
#include<math.h>
#define max(a,b) (a)>(b)?(a):(b)
#define min(a,b) (a)<(b)?(a):(b)
#define ll long long
#define ul unsigned long long
#define INF 1000000
int N,M;
int dx[4]={1,0,-1,0},dy[4]={0,1,0,-1};
char map[105][105];
int d[105][105];
int sx,sy,ex,ey;
struct node
{
int x,y;
}q[10005],now,path[105][105];
int bfs()
{
int head=0,tail=1;
q[head].x=sx;
q[head].y=sy;
d[sx][sy]=0;
while(head<tail)
{
now=q[head++];
if(now.x==ex&&now.y==ey)return d[ex][ey];
for(int i=0;i<4;i++)
{
int nx=now.x+dx[i];
int ny=now.y+dy[i];
if(nx>=0&&nx<N&&ny>=0&&ny<M&&map[nx][ny]!='#'&&d[nx][ny]==INF)
{
d[nx][ny]=d[now.x][now.y]+1;
path[nx][ny]=now;
q[tail].x=nx;
q[tail].y=ny;
tail++;
//printf("%d %d %d\n",nx,ny,d[nx][ny]);
}
}
}
return -1;
}
void outpath(int i,int j)
{
if(path[i][j].x!=sx||path[i][j].y!=sy)
outpath(path[i][j].x,path[i][j].y);
printf("%d %d\n",i,j);
}
void work()
{
for(int i=0;i<105;i++)for(int j=0;j<105;j++)d[i][j]=INF;
scanf("%d%d",&N,&M);
for(int i=0;i<N;i++)
scanf("%s",map[i]);
for(int i=0;i<N;i++)
for(int j=0;j<M;j++)
{
if(map[i][j]=='S')
{
sx=i;
sy=j;
}
else if(map[i][j]=='G')
{
ex=i;
ey=j;
}
}
int flag=bfs();
if(flag==-1)
printf("No way!\n");
else
{
printf("%d\n",flag);
printf("%d %d\n",sx,sy);
outpath(ex,ey);
}
}
int main()
{
work();
return 0;
}