有大佬帮忙看下代码吗：

题目：P3958 [NOIP2017 提高组] 奶酪

错误返回：process exited with return value 3221225725

下面是代码：

#include<iostream>
#include<cstdio>
#include<math.h>
#define INT long long int
using namespace std;
INT pos;
//Process exited after 26.59 seconds with return value 3221225725
//---------------------------------------------------------------------------
INT T,n,h,r;
struct point{
	INT x,y,z;
	INT father = -1;//1001 low    1002 up
	INT rank = 1;
}p[1005];
bool distance(point a,point b){
	if(((a.x-b.x)*(a.x-b.x)+(a.y-b.y)*(a.y-b.y)+(a.z-b.z)*(a.z-b.z)) > 4*r*r){
		return 0;
	}
	return 1;
}
inline void init(){
	for(int i = 0;i < T;i++){
		p[i].father = -1;
		p[i].rank = 1;
	}
	return;
}//--------------------------------------------------------------------------
INT find(INT i){
	if(p[i].father==i){
		return i;
	}return p[i].father=find(p[i].father);
}
void merge(point &a,point &b,int i,int j){////////可能是这个错    id return code:3221225477
	INT x = find(i),y = find(j);
	if(p[x].rank <= p[y].rank){
		p[x].father = p[y].father;
	}else if(p[x].rank > p[y].rank){
		p[y].father = p[x].father;
	}
	if(p[x].rank == p[y].rank&&x != y){
		p[y].rank++;
	}
}
void insert(point &a){
	if(h-a.y<=r)//up
		merge(a,p[1002],pos,1002);
	else if(a.y<=r)//down
		merge(a,p[1001],pos,1001);
	else{
		for(int i = 0;i < pos;i++)
			if(distance(a,p[i])) merge(a,p[i],pos,i);
	}
}
void solve(){
	printf("%s",find(1001) == find(1002)?"Yes":"No");
}
int main(){
	scanf("%lld",&T);
	p[1001].father = 1001;
	p[1002].father = 1002;//1001 low    1002 up
	while(T--){
		scanf("%lld%lld%lld",&n,&h,&r);
		for(int i = 0;i < n;i++){
			scanf("%lld%lld%lld",&p[i].x,&p[i].y,&p[i].z);
			pos = i;
			insert(p[i]);
		}
		solve();
		init();
	}
	return 0;
}

Thanks.