// Exact close-pair hazard and connectedness test; same predicates as NumPy.
// C ABI keeps this numerical hot loop independent of ROS and Python versions.
#include <cmath>
#include <vector>
template<class T> int connected_grade(const T* p, int n) {
    if (n <= 0) return 0;
    std::vector<int> parent(n);
    for(int i=0;i<n;i++) parent[i]=i;
    auto root=[&](int i) { while(parent[i]!=i) {parent[i]=parent[parent[i]];i=parent[i];} return i; };
    for(int i=0;i<n;i++) for(int j=i+1;j<n;j++) {
        const T dx=p[3*i]-p[3*j], dy=p[3*i+1]-p[3*j+1];
        const T distance=std::sqrt(dx*dx+dy*dy);
        if(distance<=T(0.12)) {
            if(std::abs(p[3*i+2]-p[3*j+2])>T(0.1001)) return 0;
            parent[root(i)]=root(j);
        }
    }
    const int first=root(0);
    for(int i=1;i<n;i++) if(root(i)!=first) return 0;
    return 1;
}
extern "C" int terrain_connected_f32(const float* p,int n) {return connected_grade(p,n);}
extern "C" int terrain_connected_f64(const double* p,int n) {return connected_grade(p,n);}