有没有大神能帮个忙...死循环不知道卡哪了...谢谢

#include <cstdio>
#include <cmath>
#include <cstring>
#include <string.h>
#include <algorithm>
#include <iostream>
using namespace std;

long long h,r,n,T;

int flag[2005]={0};

struct point
{
    long long x,y,z;
};
point p[1010];

double dist(point a,point b)
{
    double dis=sqrt((a.x-b.x)*(a.x-b.x)+(a.y-b.y)*(a.y-b.y)+(a.z-b.z)*(a.z-b.z));
    return dis;
}

bool cmp(point a,point b)
{
    if(a.z<=b.z)
    {
        return true;
    }
    else
    {
        return false;
    }
}

bool dfs(point a,int step)
{
    if(a.z+r>=h)
    {
        return true;
    }
    flag[step]==1;
    for(int i=1;i<=n;i++)
    {
        if(!flag[i]&&dist(a,p[i])<=2*r)
        {
            return dfs(p[i],i);
        }
        return false;
    }
}

int main()
{
    //freopen("cheese.in","r",stdin);
    //freopen("cheese.out","w",stdout);
    cin>>T;
    for(int i=1;i<=T;i++)
    {
        memset(p,0,sizeof(p));
        memset(flag,0,sizeof(flag));
        cin>>n>>h>>r;
        for(int j=1;j<=n;j++)
        {
            cin>>p[j].x>>p[j].y>>p[j].z;
        }
        sort(p,p+n+1,cmp);
        long long d=2*r;

        if(p[1].z-r>0)
        {
            cout<<"No";
            printf("\n");
            continue;
        }

        if(p[n].z+r<h)
        {
            cout<<"No";
            printf("\n");
            continue;
        }

        if(dfs(p[1],1)==true)
        {
            cout<<"Yes";
            printf("\n");
        }
        else
        {
            cout<<"N0";
            printf("\n");
        }
    }
    return 0;
}