53 #include "nanoflann/nanoflann.hpp"
63 class CSG_KDTree_Adaptor
66 CSG_KDTree_Adaptor(
void) { m_pData = NULL; m_zScale = 1.; }
67 virtual ~CSG_KDTree_Adaptor(
void) {}
69 typedef nanoflann::KDTreeSingleIndexAdaptor<nanoflann::L2_Simple_Adaptor<double, CSG_KDTree_Adaptor>,
70 CSG_KDTree_Adaptor, 2> kd_tree_2d;
72 typedef nanoflann::KDTreeSingleIndexAdaptor<nanoflann::L2_Simple_Adaptor<double, CSG_KDTree_Adaptor>,
73 CSG_KDTree_Adaptor, 3> kd_tree_3d;
76 virtual size_t kdtree_get_point_count (
void)
const = 0;
77 virtual double kdtree_get_pt (
const size_t Index,
int Dimension)
const = 0;
79 template <
class BBOX>
bool kdtree_get_bbox (BBOX &bb)
const
83 if( !Get_Extent(Extent) )
88 bb[0].low = Extent[0][0]; bb[0].high = Extent[0][1];
89 bb[1].low = Extent[1][0]; bb[1].high = Extent[1][1];
93 bb[2].low = Extent[2][0]; bb[2].high = Extent[2][1];
110 virtual bool Get_Extent (
double Extent[3][2])
const {
return(
false ); }
115 class CSG_KDTree_Adaptor_Points :
public CSG_KDTree_Adaptor
118 CSG_KDTree_Adaptor_Points(
CSG_Shapes *pPoints,
int zField = -1,
double zScale = 1.)
120 m_pData = m_pPoints = pPoints;
121 m_zField = m_pPoints && zField < m_pPoints->
Get_Count() ? zField : -1;
125 virtual ~CSG_KDTree_Adaptor_Points(
void) {}
127 virtual size_t kdtree_get_point_count (
void)
const
129 return( m_pPoints->Get_Count() );
132 virtual double kdtree_get_pt (
const size_t Index,
int Dimension)
const
134 CSG_Shape *pPoint = m_pPoints->Get_Shape(Index);
136 if( Dimension == 0 ) {
return( pPoint->
Get_Point().
x ); }
137 if( Dimension == 1 ) {
return( pPoint->
Get_Point().
y ); }
138 if( Dimension == 2 ) {
return( (m_zField < 0 ? pPoint->Get_Z() : pPoint->
asDouble(m_zField)) * m_zScale); }
151 virtual bool Get_Extent (
double Extent[3][2])
const
166 class CSG_KDTree_Adaptor_PointCloud :
public CSG_KDTree_Adaptor
169 CSG_KDTree_Adaptor_PointCloud(
CSG_PointCloud *pPoints,
double zScale = 1.)
171 m_pData = m_pPoints = pPoints;
175 virtual ~CSG_KDTree_Adaptor_PointCloud(
void) {}
177 virtual size_t kdtree_get_point_count (
void)
const
182 virtual double kdtree_get_pt (
const size_t Index,
int Dimension)
const
184 if( Dimension == 0 ) {
return( m_pPoints->Get_X((
sLong)Index) ); }
185 if( Dimension == 1 ) {
return( m_pPoints->Get_Y((
sLong)Index) ); }
186 if( Dimension == 2 ) {
return( m_pPoints->Get_Z((
sLong)Index) * m_zScale ); }
197 virtual bool Get_Extent (
double Extent[3][2])
const
203 Extent[2][0] = m_pPoints-> Get_ZMin();
204 Extent[2][1] = m_pPoints-> Get_ZMax();
212 class CSG_KDTree_Adaptor_Coordinates :
public CSG_KDTree_Adaptor
215 CSG_KDTree_Adaptor_Coordinates(
const double **Points,
size_t nPoints)
221 virtual ~CSG_KDTree_Adaptor_Coordinates(
void) {}
223 virtual size_t kdtree_get_point_count (
void)
const
228 virtual double kdtree_get_pt (
const size_t Index,
int Dimension)
const
230 return( m_Points[Index][Dimension] );
238 const double **m_Points;
270 (NANOFLANN_VERSION&0xf00)/0x100,
271 (NANOFLANN_VERSION&0x0f0)/0x010,
272 (NANOFLANN_VERSION&0x00f)/0x001)
348 if( Field >= 0 && Field < pPoints->Get_Field_Count() )
354 for(
int i=0; i<pPoints->
Get_Count(); i++)
378 m_pKDTree =
new CSG_KDTree_Adaptor::kd_tree_2d(2, *
m_pAdaptor, nanoflann::KDTreeSingleIndexAdaptorParams(10));
380 ((CSG_KDTree_Adaptor::kd_tree_2d *)
m_pKDTree)->buildIndex();
391 m_pAdaptor =
new CSG_KDTree_Adaptor_Points(pPoints);
392 m_pKDTree =
new CSG_KDTree_Adaptor::kd_tree_2d(2, *
m_pAdaptor, nanoflann::KDTreeSingleIndexAdaptorParams(10));
394 ((CSG_KDTree_Adaptor::kd_tree_2d *)
m_pKDTree)->buildIndex();
420 m_pAdaptor =
new CSG_KDTree_Adaptor_PointCloud(pPoints);
421 m_pKDTree =
new CSG_KDTree_Adaptor::kd_tree_2d(2, *
m_pAdaptor, nanoflann::KDTreeSingleIndexAdaptorParams(10));
423 ((CSG_KDTree_Adaptor::kd_tree_2d *)
m_pKDTree)->buildIndex();
473 m_pAdaptor =
new CSG_KDTree_Adaptor_Coordinates(Points, nPoints);
474 m_pKDTree =
new CSG_KDTree_Adaptor::kd_tree_2d(2, *
m_pAdaptor, nanoflann::KDTreeSingleIndexAdaptorParams(10));
476 ((CSG_KDTree_Adaptor::kd_tree_2d *)
m_pKDTree)->buildIndex();
486 delete((CSG_KDTree_Adaptor::kd_tree_2d *)
m_pKDTree);
510 nanoflann::SearchParams SearchParams;
512 SearchParams.sorted = Count > 0;
514 std::vector<std::pair<size_t, double>> Matches;
516 ((CSG_KDTree_Adaptor::kd_tree_2d *)
m_pKDTree)->radiusSearch(Coordinate, Radius*Radius, Matches, SearchParams);
518 if( Count == 0 || Count > Matches.size() )
520 Count = Matches.size();
526 for(
size_t i=0; i<Count; i++)
528 Indices [i] = (int)Matches[i]. first ;
529 Distances[i] = sqrt(Matches[i].second);
534 size_t *_Indices =
new size_t[Count];
540 if( Count < (
size_t)Distances.
Get_N() )
547 for(
size_t i=0; i<Count; i++)
549 Indices[i] = (int)(_Indices[i]);
561 Count = ((CSG_KDTree_Adaptor::kd_tree_2d *)
m_pKDTree)->knnSearch(Coordinate, Count, Indices, Distances);
563 for(
size_t i=0; i<Count; i++)
565 Distances[i] = sqrt(Distances[i]);
588 size_t Index;
double Distance;
611 nanoflann::SearchParams SearchParams;
613 SearchParams.sorted =
false;
615 std::vector<std::pair<size_t, double>> Matches;
617 ((CSG_KDTree_Adaptor::kd_tree_2d *)
m_pKDTree)->radiusSearch(Coordinate, 0.0000001, Matches, SearchParams);
619 Indices .
Create(Matches.size());
620 Distances.
Create(Matches.size());
624 for(
size_t i=0; i<Matches.size(); i++)
626 if( Matches[i].second > 0. )
632 Indices[Count++] = (int)Matches[i].first;
653 double c[2]; c[0] = x; c[1] = y;
return(
Get_Nearest_Points(c, Count, Radius, Indices, Distances) );
658 double c[2]; c[0] = x; c[1] = y;
return(
Get_Nearest_Points(c, Count, Indices, Distances) );
663 double c[2]; c[0] = x; c[1] = y;
return(
Get_Nearest_Point(c, Index, Distance) );
683 double c[2]; c[0] = x; c[1] = y;
return(
Get_Duplicates(c, Indices, Distances) );
724 Create(pPoints, Field, zField, zScale);
732 if( Field >= 0 && Field < pPoints->Get_Field_Count() )
738 for(
int i=0; i<pPoints->
Get_Count(); i++)
763 m_pKDTree =
new CSG_KDTree_Adaptor::kd_tree_3d(3, *
m_pAdaptor, nanoflann::KDTreeSingleIndexAdaptorParams(10));
765 ((CSG_KDTree_Adaptor::kd_tree_3d *)
m_pKDTree)->buildIndex();
776 m_pAdaptor =
new CSG_KDTree_Adaptor_Points(pPoints, zField, zScale);
777 m_pKDTree =
new CSG_KDTree_Adaptor::kd_tree_3d(3, *
m_pAdaptor, nanoflann::KDTreeSingleIndexAdaptorParams(10));
779 ((CSG_KDTree_Adaptor::kd_tree_3d *)
m_pKDTree)->buildIndex();
801 m_pAdaptor =
new CSG_KDTree_Adaptor_PointCloud(pPoints);
802 m_pKDTree =
new CSG_KDTree_Adaptor::kd_tree_3d(3, *
m_pAdaptor, nanoflann::KDTreeSingleIndexAdaptorParams(10));
804 ((CSG_KDTree_Adaptor::kd_tree_3d *)
m_pKDTree)->buildIndex();
854 m_pAdaptor =
new CSG_KDTree_Adaptor_Coordinates(Points, nPoints);
855 m_pKDTree =
new CSG_KDTree_Adaptor::kd_tree_3d(3, *
m_pAdaptor, nanoflann::KDTreeSingleIndexAdaptorParams(10));
857 ((CSG_KDTree_Adaptor::kd_tree_3d *)
m_pKDTree)->buildIndex();
867 delete((CSG_KDTree_Adaptor::kd_tree_3d *)
m_pKDTree);
891 nanoflann::SearchParams SearchParams;
893 SearchParams.sorted = Count > 0;
895 std::vector<std::pair<size_t, double>> Matches;
897 ((CSG_KDTree_Adaptor::kd_tree_3d *)
m_pKDTree)->radiusSearch(Coordinate, Radius*Radius, Matches, SearchParams);
899 if( Count == 0 || Count > Matches.size() )
901 Count = Matches.size();
907 for(
size_t i=0; i<Count; i++)
909 Indices [i] = (int)Matches[i]. first ;
910 Distances[i] = sqrt(Matches[i].second);
915 size_t *_Indices =
new size_t[Count];
921 if( Count < (
size_t)Distances.
Get_N() )
928 for(
size_t i=0; i<Count; i++)
930 Indices[i] = (int)(_Indices[i]);
942 Count = ((CSG_KDTree_Adaptor::kd_tree_3d *)
m_pKDTree)->knnSearch(Coordinate, Count, Indices, Distances);
944 for(
size_t i=0; i<Count; i++)
946 Distances[i] = sqrt(Distances[i]);
969 size_t Index;
double Distance;
992 nanoflann::SearchParams SearchParams;
994 SearchParams.sorted =
false;
996 std::vector<std::pair<size_t, double>> Matches;
998 ((CSG_KDTree_Adaptor::kd_tree_3d *)
m_pKDTree)->radiusSearch(Coordinate, 0.0000001, Matches, SearchParams);
1000 Indices .
Create(Matches.size());
1001 Distances.
Create(Matches.size());
1005 for(
size_t i=0; i<Matches.size(); i++)
1007 if( Matches[i].second > 0. )
1013 Indices[Count++] = (int)Matches[i].first;
1029 double c[3]; c[0] = x; c[1] = y; c[2] = z;
return(
Get_Nearest_Points(c, Count, Radius) );
1034 double c[3]; c[0] = x; c[1] = y; c[2] = z;
return(
Get_Nearest_Points(c, Count, Radius, Indices, Distances) );
1039 double c[3]; c[0] = x; c[1] = y; c[2] = z;
return(
Get_Nearest_Points(c, Count, Indices, Distances) );
1044 double c[3]; c[0] = x; c[1] = y; c[2] = z;
return(
Get_Nearest_Point(c, Index, Distance) );
1049 double c[3]; c[0] = x; c[1] = y; c[2] = z;
return(
Get_Nearest_Point(c, Index) );
1054 double c[3]; c[0] = x; c[1] = y; c[2] = z;
return(
Get_Nearest_Value(c, Value) );
1064 double c[3]; c[0] = x; c[1] = y; c[2] = z;
return(
Get_Duplicates(c, Indices, Distances) );
1069 double c[3]; c[0] = x; c[1] = y; c[2] = z;
return(
Get_Duplicates(c) );
1109 "SEARCH_RANGE" ,
_TL(
"Search Range"),
1118 "SEARCH_RADIUS" ,
_TL(
"Maximum Search Distance"),
1119 _TL(
"local maximum search distance given in map units"),
1124 "SEARCH_POINTS_ALL" ,
_TL(
"Number of Points"),
1127 _TL(
"maximum number of nearest points"),
1128 _TL(
"all points within search distance")
1135 "SEARCH_POINTS_MIN" ,
_TL(
"Minimum"),
1136 _TL(
"minimum number of points to use"),
1137 (
int)minPoints, 1,
true
1142 "SEARCH_POINTS_MAX" ,
_TL(
"Maximum"),
1143 _TL(
"maximum number of nearest points"),
1203 m_minPoints = (*m_pParameters)(
"SEARCH_POINTS_MIN")
1205 m_maxPoints = (*m_pParameters)(
"SEARCH_POINTS_ALL")->asInt () == 0
1206 ? (*m_pParameters)(
"SEARCH_POINTS_MAX")->asInt () : 0;
1207 m_Radius = (*m_pParameters)(
"SEARCH_RANGE" )->asInt () == 0
1208 ? (*m_pParameters)(
"SEARCH_RADIUS" )->asDouble() : 0.;
1255 m_pPoints = pPoints;
1261 return( m_Search.
Create(pPoints, m_zField = zField) );
1311 if( pPoint && !pPoint->
is_NoData(m_zField) )
1316 z = m_zField < 0 ? (double)Index : pPoint->
asDouble(m_zField);
1327 double *p = m_Search.
Get_Point((
size_t)Index);
1329 x = p[0]; y = p[1]; z = p[2];
1369 double *p = m_Search.
Get_Point((
size_t)Index[i]);
1371 Points.
Add(p[0], p[1], p[2]);