30tps求助

#include<iostream>
#include<cstdio>
#include<algorithm>
#include<string.h>
#include<cmath>
using std::ios;
using std::cout;
using std::cin;

typedef long long ll;//可以用ll防精度误差

const int maxn = 1030;

ll fa[maxn];
ll under[maxn], up[maxn];
ll under_zz, up_zz;

struct node{
    ll x, y, z;
}a[maxn];

ll quick_pow(ll a, ll b){
    ll ans = 1;
    while(b){
        if((b&1) == 1) ans *= a;
        a *= a;
        b>>=1;
    }
    return ans;
}

ll dist(ll p1, ll p2){
    return quick_pow((a[p1].x-a[p2].x),2) +
        quick_pow(a[p1].y-a[p2].y,2) +
        quick_pow(a[p1].z-a[p2].z,2);
}

ll find(ll x){
    if(fa[x] == x) return x;
    else return fa[x] = find(fa[x]);
}

void merge(ll x, ll y){
    ll fx = find(x);
    ll fy = find(y);
    fa[fx] = fy;
    return;
}

int main(){
    // freopen("test.out", "r", stdin);
    // freopen("error.out", "w", stdout);
    ios::sync_with_stdio(false);
    cin.tie(0), cout.tie(0);
    int T;
    cin >> T;
    while(T--){
        under_zz = 0; up_zz = 0;
       // memset(under, 0, sizeof(under));
       // memset(up, 0, sizeof(up));
       // memset(fa, 0, sizeof(fa));
        ll n, h, r;
        cin >> n >> h >> r;
        for(int i = 1; i <= n; i++){
            cin >> a[i].x >> a[i].y >> a[i].z;
        }
        for(int i = 1; i <= n; i++) fa[i] = i;
        for(int i = 1; i <= n; i++){
            if(a[i].z + r >= h) up[++up_zz] = i;//
            if(a[i].z - r <= 0) under[++under_zz] = i;
            //上面和下面相切或相交的园的洞的编号
        }
        for(int i = 2; i <= n; i++){
            if(dist(i, i-1) <= 4*r*r) merge(i, i-1);//距离判断错了,平方去了之后r忘改了
        }
        bool f = false;
        for(int i = 1; i <= under_zz; i++){
            for(int j = 1; j <= up_zz; j++){
                int fx = find(under[i]);
                int fy = find(up[j]);
                //cout << fx << " " << fy << " -- ";
                if(fx == fy) {
                    f = true;
                    break;
                }
            }
        }

        if(f) cout << "Yes\n";
        else cout << "No\n";
    }
    return 0;
}