24 lines
1.0 KiB
C++
24 lines
1.0 KiB
C++
// 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);}
|