#include <algorithm>
#include <iostream>
#include <cstring>
#include <cstdio>
#include <math.h>
#include <float.h>
#include <queue>
using namespace std;

int n,m,p,q,startx,starty;
int a[50][50],book[50][50];
int tail,head;

struct node
{
    int x;
    int y;
    int step;
}que[2500];

void bfs()
{
    tail=head=0;
    int next[4][2]={{0,1},{0,-1},{-1,0},{1,0}};
    que[tail].x=startx;
    que[tail].y=starty;
    que[tail].step=0;
    tail++;
    book[startx][starty]=1;
    int flag=0,tx,ty;
    while(head<tail)
    {
        for(int i=0;i<4;i++)
        {
            tx=que[head].x+next[i][0];
            ty=que[head].y+next[i][1];

            if(tx>n||tx<0||ty>m||ty<0)
                continue;
            if(a[tx][ty]==0&&book[tx][ty]==0)
            {
                book[tx][ty]=1;
                que[tail].x=tx;
                que[tail].y=ty;
                que[tail].step=que[head].step+1;
                tail++;
            }
            if(tx==p&&ty==q)
            {
                flag=1;
                break;
            }
        }
        if(flag==1)
            break;
        head++;
    }
    return ;
}

int main()
{
    scanf("%d%d",&n,&m);
    for(int i=0;i<n;i++)
        for(int j=0;j<m;j++)
            scanf("%d",&a[i][j]);
    scanf("%d%d%d%d",&startx,&starty,&p,&q);
    bfs();
    printf("%d\n",que[tail-1].step);
    return 0;
}

（不打印路径）。 参考啊哈算法P88. 增加一版STL队列的做法：

#include <algorithm>
#include <iostream>
#include <cstring>
#include <cstdio>
#include <math.h>
#include <float.h>
#include <queue>
using namespace std;

int n,m,p,q,startx,starty;
int a[50][50],book[50][50];
int tail,head;

struct node
{
    int x;
    int y;
    int step;
}qu;

queue<struct node> que;

void bfs()
{
    int next[4][2]={{0,1},{0,-1},{-1,0},{1,0}};
    qu.x=startx;
    qu.y=starty;
    qu.step=0;
    que.push(qu);
    book[startx][starty]=1;
    int flag=0,tx,ty;
    while(!que.empty())
    {
        for(int i=0;i<4;i++)
        {
            tx=que.front().x+next[i][0];
            ty=que.front().y+next[i][1];
            if(tx>n||tx<0||ty>m||ty<0)
                continue;
            if(a[tx][ty]==0&&book[tx][ty]==0)
            {
                book[tx][ty]=1;
                qu.x=tx;
                qu.y=ty;
                qu.step=que.front().step+1;
                que.push(qu);
            }
            if(tx==p&&ty==q)
            {
                flag=1;
                break;
            }
        }
        if(flag==1)
            break;
        que.pop();
    }
    return ;
}

int main()
{
    scanf("%d%d",&n,&m);
    for(int i=0;i<n;i++)
        for(int j=0;j<m;j++)
            scanf("%d",&a[i][j]);
    scanf("%d%d%d%d",&startx,&starty,&p,&q);
    bfs();
    int z;
    while(!que.empty())
    {
        z=que.front().step;
        que.pop();
    }
    printf("%d\n",z);
    return 0;
}

打印路径：

#include<stdio.h>
#include<string.h>
#include<iostream>
#include<vector>
#include<set>
#include<map>
#include<queue>
#include<ctype.h>
#include<stack>
#include<math.h>
#include <string>
#include<algorithm>
using namespace std;
typedef unsigned long long ULL;

int maze[6][6];

struct node
{
    int x;
    int y;
}first,father[6][6];

queue<node> q;

int next[4][2]={0,1,
                0,-1,
                1,0,
                -1,0};

void bfs()
{
    first.x=0;
    first.y=0;
    q.push(first);
    while(!q.empty())
    {
        node fro=q.front();
        q.pop();
        if(fro.x==4&&fro.y==4)
            break;
        for(int i=0;i<4;i++)
        {
            int tx,ty;
            tx=fro.x+next[i][0];
            ty=fro.y+next[i][1];
            if(tx<0||ty<0||tx>4||ty>4)
                continue;
            if(maze[tx][ty]==0)
            {
                node v;
                v.x=tx;
                v.y=ty;
                father[tx][ty]=fro;
                maze[tx][ty]=1;
                q.push(v);
            }
        }
    }
}

int main()
{
    ios::sync_with_stdio(false);cin.tie(0);

    for(int i=0;i<5;i++)
        for(int j=0;j<5;j++)
            cin>>maze[i][j];
    bfs();

    node path[25];
    int cnt=0;
    path[cnt].x=4;
    path[cnt].y=4;
    for(;;)
    {
        if(path[cnt].x==0&&path[cnt].y==0)
            break;
        path[cnt+1]=father[path[cnt].x][path[cnt].y];
        cnt++;
    }
    while(cnt>=0)
    {
        printf("(%d, %d)\n", path[cnt].x, path[cnt].y);
        cnt--;
    }
    return 0;
}