进阶实验6-3.4 拯救007（升级版） (30分)-BFS

#include <stdio.h>
#include <string.h>
#include <math.h>
#define MaxV 100+5
#define INF 0x3f3f3f3f
typedef struct {
    int x,y;
    double firstjump;
} GNode;
GNode G[MaxV];
int Q[MaxV],path[MaxV],dist[MaxV];
int n,D;
int Jump(int i,int j) {//从一个鳄鱼跳到另一个鳄鱼
    if(pow(G[i].x-G[j].x,2)+pow(G[i].y-G[j].y,2)<=pow(D,2))
        return 1;
    return 0;
}
int IsSafe(int i) {//是否可以直接上岸
    if(G[i].x-D<=-50||G[i].x+D>=50)
        return 1;
    if(G[i].y-D<=-50||G[i].y+D>=50)
        return 1;
    return 0;
}
void BFS() {//广度优先
    int u,v,w;
    int front=0,rear=0;
    for(v=0; v<n; v++) {
        if(G[v].firstjump>pow(7.5,2)&&G[v].firstjump<=pow(D+7.5,2)) {//合法第一跳加入队列
            Q[rear++]=v;
            dist[v]=1;
            path[v]=-1;
        }
    }
    while(front<rear) {
        u=Q[front++];//依次访问队列中的结点
        if(IsSafe(u)&&dist[n]==INF) {//如果可以直接上岸，结束
            dist[n]=dist[u]+1;
            path[n]=u;
            break;
        }
        for(w=0; w<n; w++) {
            if(dist[w]==INF&&Jump(u,w)) {//u结点可到达w结点，则w入队
                Q[rear++]=w;
                dist[w]=dist[u]+1;
                path[w]=u;
            }
        }
    }
}
int cmp(GNode a,GNode b) {
    return a.firstjump-b.firstjump;
}
int main() {
    scanf("%d %d",&n,&D);
    int i;
    for(i=0; i<n; i++) {
        scanf("%d %d",&G[i].x,&G[i].y);
        G[i].firstjump=pow(G[i].x,2)+pow(G[i].y,2);
    }
    if(D>=42.5) {
        printf("1\n");
        return 0;
    }
    qsort(G,n,sizeof(G[0]),cmp);//第一跳距离升序
    memset(dist,INF,sizeof(dist));
    memset(path,-1,sizeof(path));
    BFS();
    if(dist[n]==INF) {
        printf("0\n");
        return 0;
    }
    printf("%d\n",dist[n]);
    int u=path[n];
    int tmp[n];
    memset(tmp,-1,sizeof(tmp));
    int k=0;
    while(u!=-1) {
        tmp[k++]=u;
        u=path[u];
    }
    for(i=k-1; i>=0; i--) {//倒序输出路径
        printf("%d %d\n",G[tmp[i]].x,G[tmp[i]].y);
    }
    return 0;
}