15 template <
typename POINT, std::
size_t MAX_POINTS>
17 Eigen::Vector3d ll, Eigen::Vector3d ur,
double eps)
20 const double dx(ur[0] - ll[0]);
21 const double dy(ur[1] - ll[1]);
22 const double dz(ur[2] - ll[2]);
23 if (dx >= dy && dx >= dz)
25 ll[1] -= (dx - dy) / 2.0;
26 ur[1] += (dx - dy) / 2.0;
27 ll[2] -= (dx - dz) / 2.0;
28 ur[2] += (dx - dz) / 2.0;
32 if (dy >= dx && dy >= dz)
34 ll[0] -= (dy - dx) / 2.0;
35 ur[0] += (dy - dx) / 2.0;
36 ll[2] -= (dy - dz) / 2.0;
37 ur[2] += (dy - dz) / 2.0;
41 ll[0] -= (dz - dx) / 2.0;
42 ur[0] += (dz - dx) / 2.0;
43 ll[1] -= (dz - dy) / 2.0;
44 ur[1] += (dz - dy) / 2.0;
49 eps = std::numeric_limits<double>::epsilon();
51 for (std::size_t k(0); k < 3; ++k)
53 if (ur[k] - ll[k] > 0.0)
55 ur[k] += (ur[k] - ll[k]) * 1e-6;
62 Eigen::Vector3d lower_left{ll[0], ll[1], ll[2]};
63 Eigen::Vector3d upper_right{ur[0], ur[1], ur[2]};
67 template <
typename POINT, std::
size_t MAX_POINTS>
70 for (
auto c : _children)
76 template <
typename POINT, std::
size_t MAX_POINTS>
80 std::vector<POINT*> query_pnts;
81 Eigen::Vector3d
const min =
82 Eigen::Map<Eigen::Vector3d>(pnt->getCoords()).array() - _eps;
83 Eigen::Vector3d
const max =
84 Eigen::Map<Eigen::Vector3d>(pnt->getCoords()).array() + _eps;
85 getPointsInRange(min, max, query_pnts);
86 auto const it = std::find_if(
87 query_pnts.begin(), query_pnts.end(),
88 [pnt,
this](
auto const* p)
90 return (Eigen::Map<Eigen::Vector3d const>(p->getCoords()) -
91 Eigen::Map<Eigen::Vector3d const>(pnt->getCoords()))
92 .squaredNorm() < _eps * _eps;
94 if (it != query_pnts.end())
110 for (
auto c : _children)
112 if (
c->addPoint_(pnt, ret_pnt))
116 if (ret_pnt !=
nullptr)
125 if (_pnts.size() < MAX_POINTS)
127 _pnts.push_back(pnt);
137 template <
typename POINT, std::
size_t MAX_POINTS>
138 template <
typename T>
140 T
const& min, T
const& max, std::vector<POINT*>& pnts)
const
142 if (_ur[0] < min[0] || _ur[1] < min[1] || _ur[2] < min[2])
147 if (max[0] < _ll[0] || max[1] < _ll[1] || max[2] < _ll[2])
154 std::copy_if(_pnts.begin(), _pnts.end(), std::back_inserter(pnts),
155 [&min, &max](
auto const* p)
157 return (min[0] <= (*p)[0] && (*p)[0] < max[0] &&
158 min[1] <= (*p)[1] && (*p)[1] < max[1] &&
159 min[2] <= (*p)[2] && (*p)[2] < max[2]);
164 for (std::size_t k(0); k < 8; k++)
166 _children[k]->getPointsInRange(min, max, pnts);
171 template <
typename POINT, std::
size_t MAX_POINTS>
173 Eigen::Vector3d
const& ur,
double eps)
174 : _ll(ll), _ur(ur), _is_leaf(true), _eps(eps)
179 template <
typename POINT, std::
size_t MAX_POINTS>
191 for (
auto c : _children)
193 if (
c->addPoint_(pnt, ret_pnt))
197 if (ret_pnt !=
nullptr)
205 if (_pnts.size() < MAX_POINTS)
207 _pnts.push_back(pnt);
217 template <
typename POINT, std::
size_t MAX_POINTS>
225 if (_pnts.size() < MAX_POINTS)
227 _pnts.push_back(pnt);
237 template <
typename POINT, std::
size_t MAX_POINTS>
240 const double x_mid((_ur[0] + _ll[0]) / 2.0);
241 const double y_mid((_ur[1] + _ll[1]) / 2.0);
242 const double z_mid((_ur[2] + _ll[2]) / 2.0);
243 Eigen::Vector3d p0{x_mid, y_mid, _ll[2]};
244 Eigen::Vector3d p1{_ur[0], _ur[1], z_mid};
247 _children[
static_cast<std::int8_t
>(Quadrant::NEL)] =
253 _children[
static_cast<std::int8_t
>(Quadrant::NWL)] =
259 _children[
static_cast<std::int8_t
>(Quadrant::SWL)] =
263 _children[
static_cast<std::int8_t
>(Quadrant::NEU)] =
269 _children[
static_cast<std::int8_t
>(Quadrant::SEL)] =
279 _children[
static_cast<std::int8_t
>(Quadrant::NWU)] =
285 _children[
static_cast<std::int8_t
>(Quadrant::SWU)] =
293 _children[
static_cast<std::int8_t
>(Quadrant::SEU)] =
297 for (std::size_t k(0); k < 8; k++)
299 if (_children[k]->addPointToChild(pnt))
306 const std::size_t n_pnts(_pnts.size());
307 for (std::size_t j(0); j < n_pnts; j++)
309 for (
auto c : _children)
311 if (
c->addPointToChild(_pnts[j]))
320 template <
typename POINT, std::
size_t MAX_POINTS>
323 if ((*pnt)[0] < _ll[0] || (*pnt)[1] < _ll[1] || (*pnt)[2] < _ll[2])
327 if ((*pnt)[0] >= _ur[0] || (*pnt)[1] >= _ur[1] || (*pnt)[2] >= _ur[2])
bool addPoint(POINT *pnt, POINT *&ret_pnt)
bool addPointToChild(POINT *pnt)
bool addPoint_(POINT *pnt, POINT *&ret_pnt)
void splitNode(POINT *pnt)
void getPointsInRange(T const &min, T const &max, std::vector< POINT * > &pnts) const
OctTree(Eigen::Vector3d const &ll, Eigen::Vector3d const &ur, double eps)
std::array< OctTree< POINT, MAX_POINTS > *, 8 > _children
bool isOutside(POINT *pnt) const
static OctTree< POINT, MAX_POINTS > * createOctTree(Eigen::Vector3d ll, Eigen::Vector3d ur, double eps=std::numeric_limits< double >::epsilon())