15template <
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]};
67template <
typename POINT, std::
size_t MAX_POINTS>
70 for (
auto c : _children)
76template <
typename POINT, std::
size_t MAX_POINTS>
80 std::vector<POINT*> query_pnts;
81 Eigen::Vector3d
const min = pnt->asEigenVector3d().array() - _eps;
82 Eigen::Vector3d
const max = pnt->asEigenVector3d().array() + _eps;
83 getPointsInRange(min, max, query_pnts);
85 std::find_if(query_pnts.begin(), query_pnts.end(),
86 [pnt,
this](
auto const* p)
88 return (p->asEigenVector3d() - pnt->asEigenVector3d())
89 .squaredNorm() < _eps * _eps;
91 if (it != query_pnts.end())
107 for (
auto c : _children)
109 if (
c->addPoint_(pnt, ret_pnt))
113 if (ret_pnt !=
nullptr)
122 if (_pnts.size() < MAX_POINTS)
124 _pnts.push_back(pnt);
134template <
typename POINT, std::
size_t MAX_POINTS>
137 T
const& min, T
const& max, std::vector<POINT*>& pnts)
const
139 if (_ur[0] < min[0] || _ur[1] < min[1] || _ur[2] < min[2])
144 if (max[0] < _ll[0] || max[1] < _ll[1] || max[2] < _ll[2])
151 std::copy_if(_pnts.begin(), _pnts.end(), std::back_inserter(pnts),
152 [&min, &max](
auto const* p)
154 return (min[0] <= (*p)[0] && (*p)[0] < max[0] &&
155 min[1] <= (*p)[1] && (*p)[1] < max[1] &&
156 min[2] <= (*p)[2] && (*p)[2] < max[2]);
161 for (std::size_t k(0); k < 8; k++)
163 _children[k]->getPointsInRange(min, max, pnts);
168template <
typename POINT, std::
size_t MAX_POINTS>
170 Eigen::Vector3d
const& ur,
double eps)
171 : _ll(ll), _ur(ur), _is_leaf(true), _eps(eps)
176template <
typename POINT, std::
size_t MAX_POINTS>
188 for (
auto c : _children)
190 if (
c->addPoint_(pnt, ret_pnt))
194 if (ret_pnt !=
nullptr)
202 if (_pnts.size() < MAX_POINTS)
204 _pnts.push_back(pnt);
214template <
typename POINT, std::
size_t MAX_POINTS>
222 if (_pnts.size() < MAX_POINTS)
224 _pnts.push_back(pnt);
234template <
typename POINT, std::
size_t MAX_POINTS>
237 const double x_mid((_ur[0] + _ll[0]) / 2.0);
238 const double y_mid((_ur[1] + _ll[1]) / 2.0);
239 const double z_mid((_ur[2] + _ll[2]) / 2.0);
240 Eigen::Vector3d p0{x_mid, y_mid, _ll[2]};
241 Eigen::Vector3d p1{_ur[0], _ur[1], z_mid};
244 _children[
static_cast<std::int8_t
>(Quadrant::NEL)] =
250 _children[
static_cast<std::int8_t
>(Quadrant::NWL)] =
256 _children[
static_cast<std::int8_t
>(Quadrant::SWL)] =
260 _children[
static_cast<std::int8_t
>(Quadrant::NEU)] =
266 _children[
static_cast<std::int8_t
>(Quadrant::SEL)] =
276 _children[
static_cast<std::int8_t
>(Quadrant::NWU)] =
282 _children[
static_cast<std::int8_t
>(Quadrant::SWU)] =
290 _children[
static_cast<std::int8_t
>(Quadrant::SEU)] =
294 for (std::size_t k(0); k < 8; k++)
296 if (_children[k]->addPointToChild(pnt))
303 const std::size_t n_pnts(_pnts.size());
304 for (std::size_t j(0); j < n_pnts; j++)
306 for (
auto c : _children)
308 if (
c->addPointToChild(_pnts[j]))
317template <
typename POINT, std::
size_t MAX_POINTS>
320 if ((*pnt)[0] < _ll[0] || (*pnt)[1] < _ll[1] || (*pnt)[2] < _ll[2])
324 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())