搜索 bfs 路径

#include <stdio.h>
#include <iostream>
#include <cstring>
#include <stdlib.h>
#include <algorithm>
#include <queue>
#include <math.h>
#define MAX 30
#define INF 1e8

using namespace std;
char maze[5][5];
int d[5][5];
int move1[4] = {1,0,-1,0},move2[4] = {0,1,0,-1};
typedef pair<int,int> P;
void bfs()
{
    for (int i = 0;i <5;i++)
    {
        for (int j = 0;j <5;j++)
        {
            d[i][j]=INF;
        }
    }
    d[0][0]=0;
    queue<P> que;
    P q;
    que.push(P(0,0));
    while(!que.empty())
    {
        q = que.front();
        que.pop();
        for (int i = 0;i < 4;i++)
        {
            int nx = q.first+move1[i],ny = q.second+move2[i];
            if((nx>=0&&nx<5)&&(ny>=0&&ny<5)&&maze[nx][ny]!='1'&&d[nx][ny]==INF)
            {
                que.push(P(nx,ny));
                d[nx][ny]=d[q.first][q.second]+1;
            }
        }
    }
    /*for (int i = 0;i < 5;i++)
    {
        for (int j = 0;j <5;j++)
        {
            cout<<d[i][j]<<" ";
        }
        cout<<endl;
    }*/
}
int main(void)
{
    while(scanf("%c %c %c %c %c",&maze[0][0],&maze[0][1],&maze[0][2],&maze[0][3],&maze[0][4])!= EOF)
    {
        for (int i = 1;i < 5;i++)
        {
            getchar();
            scanf("%c %c %c %c %c",&maze[i][0],&maze[i][1],&maze[i][2],&maze[i][3],&maze[i][4]);
        }
        bfs();
        P ans[30];
        int tx = 4,ty = 4;
        ans[0].first = 4,ans[0].second = 4;
        int k = 1;
        while(d[tx][ty]!=0)
        {
            for (int i = 0;i < 4;i++)
            {
                if((tx+move1[i]>=0&&tx+move1[i]<5)&&(ty+move2[i]>=0&&ty+move2[i]<5)&&d[tx+move1[i]][ty+move2[i]]==d[tx][ty]-1)
                {
                    ans[k].first = tx+move1[i];
                    ans[k].second = ty+move2[i];
                    k++;
                    tx+=move1[i],ty+=move2[i];
                    break;
                }
            }
        }
        for (int i = k-1;i>=0;i--)
        {
            printf("(%d, %d)\n",ans[i].first,ans[i].second);
        }
    }
    return 0;
}