洛谷-3958 奶酪

题目描
现有一块大奶酪，它的高度为 h，它的长度和宽度我们可以认为是无限大的，奶酪 中间有许多 半径相同 的球形空洞。我们可以在这块奶酪中建立空间坐标系，在坐标系中， 奶酪的下表面为z=0，奶酪的上表面为z=h。
现在，奶酪的下表面有一只小老鼠 Jerry，它知道奶酪中所有空洞的球心所在的坐 标。如果两个空洞相切或是相交，则 Jerry 可以从其中一个空洞跑到另一个空洞，特别 地，如果一个空洞与下表面相切或是相交，Jerry 则可以从奶酪下表面跑进空洞；如果 一个空洞与上表面相切或是相交，Jerry 则可以从空洞跑到奶酪上表面。
位于奶酪下表面的 Jerry 想知道，在 不破坏奶酪 的情况下，能否利用已有的空洞跑 到奶酪的上表面去?
空间内两点P1(x1,y1,z1)、P2(x2,y2,z2)的距离公式如下：
dist(P1,P2)=(x1−x2)²+(y1−y2)²+(z1−z2)²
输入输出格式
输入格式：
每个输入文件包含多组数据。
的第一行，包含一个正整数 T，代表该输入文件中所含的数据组数。
接下来是 T 组数据，每组数据的格式如下： 第一行包含三个正整数 n,h 和 r，两个数之间以一个空格分开，分别代表奶酪中空 洞的数量，奶酪的高度和空洞的半径。
接下来的 n 行，每行包含三个整数 x,y,z，两个数之间以一个空格分开，表示空 洞球心坐标为(x,y,z)。
输出格式：
T 行，分别对应 T 组数据的答案，如果在第 iii 组数据中，Jerry 能从下 表面跑到上表面，则输出Yes，如果不能，则输出No （均不包含引号）。

输入输出样例
输入样例#1：
3
2 4 1
0 0 1
0 0 3
2 5 1
0 0 1
0 0 4
2 5 2
0 0 2
2 0 4

输出样例#1：
Yes
No
Yes

说明
【数据规模与约定】
对于 100%的数据,1≤n≤1,000,1≤h,r≤1,000,000,000,T≤20,坐标的 绝对值不超过 1,000,000,000

解释：把球当成节点，然后地下和上面也看成两个节点，然后如果联通就连边，跑一遍最短路就好了

#include<stdio.h>
#include<queue>
#include<iostream>
#include<cstdlib>
#include<cstring>
#define N 1005
#define M 2000004
#define INF 2147483647
using namespace std;
int head[N]={0};
int next1[M]={0};
int V[M]={0};
bool vis[N]={0};
int W[M]={0};
int tot=0;
int dist[N]={0};
int n=0;
long long h=0,r=0;
void add(int x,int y,int c){
    tot++;
    next1[tot]=head[x];
    V[tot]=y;W[tot]=c;
    head[x]=tot;
}
struct node{
    long long a,b,c;
};
node s[N];
priority_queue<pair<int,int> > que;
void dij(int x){
    fill(dist+1,dist+n+5,INF);
    dist[x]=0;
    que.push(make_pair(0,x));
    while(que.size()){
        pair<int,int> temp=que.top();que.pop();
        int from=temp.second;if(vis[from]) continue;
        vis[from]=true;
        for(int i=head[from];i;i=next1[i]){
            int to=V[i],w=W[i];
            if(dist[to]>dist[from]+w){
                dist[to]=dist[from]+w;
                que.push(make_pair(-dist[to],to));
            }
        }
    }
}
int T=0;
bool ok(int x,int y){
    long long d1=(s[x].a-s[y].a)*(s[x].a-s[y].a)+(s[x].b-s[y].b)*(s[x].b-s[y].b)+(s[x].c-s[y].c)*(s[x].c-s[y].c);
    long long d2=4*r*r;
    return d1<=d2;
}
int main(){
    scanf("%d",&T);
    while(T--){
        scanf("%d%lld%lld",&n,&h,&r);
        memset(vis,0,sizeof(vis));
        memset(head,0,sizeof(head));tot=0;
        for(int i=1;i<=n;i++) scanf("%lld%lld%lld",&s[i].a,&s[i].b,&s[i].c);
        for(int i=1;i<=n;i++){
            if(s[i].c<=r) add(0,i,1);
            if(h-s[i].c<=r) add(i,n+1,1);
            for(int j=1;j<=n;j++){
                if(i==j) continue;
                if(ok(i,j)){
                    add(i,j,1);add(j,i,1);
                }
            }
        }
        dij(0);
        if(dist[n+1]!=INF) printf("Yes\n");
        else printf("No\n");
    }
    return 0;
}