BFS(最短路径)—— 迷宫问题

#define _CRT_SECURE_NO_WARNINGS
#include<stdio.h>
#include<stdlib.h>
#include<queue>
using namespace std;
typedef struct Node
{
    int x, y, id, from;
    //  id 是给每一个如果队列的结构体标号
    //  from 是该点所连点 所代表的结构体编号
}st; 
int map[5][5], dx[4] = { -1,0,1,0 }, dy[4] = { 0,1,0,-1 }, c = 0;
queue<st>q, p;
st way[50]; 
void outs(st now)
{
    if (now.from == -1)
        printf("(0, 0)\n");
    else
    {
        outs(way[now.from]);;
        printf("(%d, %d)\n", now.x, now.y);
    }
}
void bfs()
{
    st a = { 0,0,0,-1 };
    way[c++] = a;
    q.push(a);
    p.push(a);
    while (q.size())
    {
        st vertex = q.front();
        int pre = vertex.id;
        q.pop();
        if (vertex.x == 4 && vertex.y == 4)
        {
            outs(way[c-1]);
            return;
        }
        map[vertex.x][vertex.y] = 1;
        for (int i = 0; i < 4; i++)
        {
            st next;
            next.x = vertex.x + dx[i], next.y = vertex.y + dy[i];
            next.from = pre, next.id = c;
            if (next.x >= 0 && next.x < 5 && next.y >= 0 && next.y < 5&&map[next.x][next.y]==0)
            {
                q.push(next);
                way[c++] = next;    
            }
        }
    }
    return;
}
int main(void)
{
    for (int i = 0; i < 5; i++)
    {
        for (int j = 0; j < 5; j++)
        {
            scanf("%d", &map[i][j]);
        }
    }

    bfs();

    system("pause");
    return 0;
}