Loading [MathJax]/extensions/tex2jax.js
OGS
OctTree-impl.h
Go to the documentation of this file.
1
13namespace GeoLib
14{
15template <typename POINT, std::size_t MAX_POINTS>
16OctTree<POINT, MAX_POINTS>* OctTree<POINT, MAX_POINTS>::createOctTree(
17 Eigen::Vector3d ll, Eigen::Vector3d ur, double eps)
18{
19 // compute an axis aligned cube around the points ll and ur
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)
24 {
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;
29 }
30 else
31 {
32 if (dy >= dx && dy >= dz)
33 {
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;
38 }
39 else
40 {
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;
45 }
46 }
47 if (eps == 0.0)
48 {
49 eps = std::numeric_limits<double>::epsilon();
50 }
51 for (std::size_t k(0); k < 3; ++k)
52 {
53 if (ur[k] - ll[k] > 0.0)
54 {
55 ur[k] += (ur[k] - ll[k]) * 1e-6;
56 }
57 else
58 {
59 ur[k] += eps;
60 }
61 }
62 Eigen::Vector3d lower_left{ll[0], ll[1], ll[2]};
63 Eigen::Vector3d upper_right{ur[0], ur[1], ur[2]};
64 return new OctTree<POINT, MAX_POINTS>(lower_left, upper_right, eps);
65}
66
67template <typename POINT, std::size_t MAX_POINTS>
68OctTree<POINT, MAX_POINTS>::~OctTree()
69{
70 for (auto c : _children)
71 {
72 delete c;
73 }
74}
75
76template <typename POINT, std::size_t MAX_POINTS>
77bool OctTree<POINT, MAX_POINTS>::addPoint(POINT* pnt, POINT*& ret_pnt)
78{
79 // first do a range query using a small box around the point pnt
80 std::vector<POINT*> query_pnts;
81 Eigen::Vector3d const min = pnt->asEigenVector3d().array() - _eps;
82 Eigen::Vector3d const max = {
83 std::abs(((*pnt)[0] + _eps) - (*pnt)[0]) > 0.0
84 ? (*pnt)[0] + _eps
85 : std::nextafter((*pnt)[0],
86 std::numeric_limits<double>::infinity()),
87 std::abs(((*pnt)[1] + _eps) - (*pnt)[1]) > 0.0
88 ? (*pnt)[1] + _eps
89 : std::nextafter((*pnt)[1],
90 std::numeric_limits<double>::infinity()),
91 std::abs(((*pnt)[2] + _eps) - (*pnt)[2]) > 0.0
92 ? (*pnt)[2] + _eps
93 : std::nextafter((*pnt)[2],
94 std::numeric_limits<double>::infinity())};
95 getPointsInRange(min, max, query_pnts);
96 auto const it =
97 std::find_if(query_pnts.begin(), query_pnts.end(),
98 [pnt, this](auto const* p)
99 {
100 return (p->asEigenVector3d() - pnt->asEigenVector3d())
101 .squaredNorm() < _eps * _eps;
102 });
103 if (it != query_pnts.end())
104 {
105 ret_pnt = *it;
106 return false;
107 }
108
109 // the point pnt is not yet in the OctTree
110 if (isOutside(pnt))
111 {
112 ret_pnt = nullptr;
113 return false;
114 }
115
116 // at this place it holds true that the point is within [_ll, _ur]
117 if (!_is_leaf)
118 {
119 for (auto c : _children)
120 {
121 if (c->addPoint_(pnt, ret_pnt))
122 {
123 return true;
124 }
125 if (ret_pnt != nullptr)
126 {
127 return false;
128 }
129 }
130 }
131
132 ret_pnt = pnt;
133
134 if (_pnts.size() < MAX_POINTS)
135 {
136 _pnts.push_back(pnt);
137 }
138 else
139 { // i.e. _pnts.size () == MAX_POINTS
140 splitNode(pnt);
141 _pnts.clear();
142 }
143 return true;
144}
145
146template <typename POINT, std::size_t MAX_POINTS>
147template <typename T>
148void OctTree<POINT, MAX_POINTS>::getPointsInRange(
149 T const& min, T const& max, std::vector<POINT*>& pnts) const
150{
151 if (max[0] == min[0] || max[1] == min[1] || max[2] == min[2])
152 {
153 ERR("The search range [{}, {}) x [{}, {}) x [{}, {}) is empty.", min[0],
154 max[0], min[1], max[1], min[2], max[2]);
155 return;
156 }
157
158 return getPointsInRange_(min, max, pnts);
159}
160
161template <typename POINT, std::size_t MAX_POINTS>
162template <typename T>
163void OctTree<POINT, MAX_POINTS>::getPointsInRange_(
164 T const& min, T const& max, std::vector<POINT*>& pnts) const
165{
166 if (_ur[0] < min[0] || _ur[1] < min[1] || _ur[2] < min[2])
167 {
168 return;
169 }
170
171 if (max[0] < _ll[0] || max[1] < _ll[1] || max[2] < _ll[2])
172 {
173 return;
174 }
175
176 if (_is_leaf)
177 {
178 std::copy_if(_pnts.begin(), _pnts.end(), std::back_inserter(pnts),
179 [&min, &max](auto const* p)
180 {
181 return (min[0] <= (*p)[0] && (*p)[0] < max[0] &&
182 min[1] <= (*p)[1] && (*p)[1] < max[1] &&
183 min[2] <= (*p)[2] && (*p)[2] < max[2]);
184 });
185 }
186 else
187 {
188 for (std::size_t k(0); k < 8; k++)
189 {
190 _children[k]->getPointsInRange_(min, max, pnts);
191 }
192 }
193}
194
195template <typename POINT, std::size_t MAX_POINTS>
196OctTree<POINT, MAX_POINTS>::OctTree(Eigen::Vector3d const& ll,
197 Eigen::Vector3d const& ur, double eps)
198 : _ll(ll), _ur(ur), _is_leaf(true), _eps(eps)
199{
200 _children.fill(nullptr);
201}
202
203template <typename POINT, std::size_t MAX_POINTS>
204bool OctTree<POINT, MAX_POINTS>::addPoint_(POINT* pnt, POINT*& ret_pnt)
205{
206 if (isOutside(pnt))
207 {
208 ret_pnt = nullptr;
209 return false;
210 }
211
212 // at this place it holds true that the point is within [_ll, _ur]
213 if (!_is_leaf)
214 {
215 for (auto c : _children)
216 {
217 if (c->addPoint_(pnt, ret_pnt))
218 {
219 return true;
220 }
221 if (ret_pnt != nullptr)
222 {
223 return false;
224 }
225 }
226 }
227
228 ret_pnt = pnt;
229 if (_pnts.size() < MAX_POINTS)
230 {
231 _pnts.push_back(pnt);
232 }
233 else
234 { // i.e. _pnts.size () == MAX_POINTS
235 splitNode(pnt);
236 _pnts.clear();
237 }
238 return true;
239}
240
241template <typename POINT, std::size_t MAX_POINTS>
242bool OctTree<POINT, MAX_POINTS>::addPointToChild(POINT* pnt)
243{
244 if (isOutside(pnt))
245 {
246 return false;
247 }
248
249 if (_pnts.size() < MAX_POINTS)
250 {
251 _pnts.push_back(pnt);
252 }
253 else
254 { // i.e. _pnts.size () == MAX_POINTS
255 splitNode(pnt);
256 _pnts.clear();
257 }
258 return true;
259}
260
261template <typename POINT, std::size_t MAX_POINTS>
262void OctTree<POINT, MAX_POINTS>::splitNode(POINT* pnt)
263{
264 const double x_mid((_ur[0] + _ll[0]) / 2.0);
265 const double y_mid((_ur[1] + _ll[1]) / 2.0);
266 const double z_mid((_ur[2] + _ll[2]) / 2.0);
267 Eigen::Vector3d p0{x_mid, y_mid, _ll[2]};
268 Eigen::Vector3d p1{_ur[0], _ur[1], z_mid};
269
270 // create child NEL
271 _children[static_cast<std::int8_t>(Quadrant::NEL)] =
272 new OctTree<POINT, MAX_POINTS>(p0, p1, _eps);
273
274 // create child NWL
275 p0[0] = _ll[0];
276 p1[0] = x_mid;
277 _children[static_cast<std::int8_t>(Quadrant::NWL)] =
278 new OctTree<POINT, MAX_POINTS>(p0, p1, _eps);
279
280 // create child SWL
281 p0[1] = _ll[1];
282 p1[1] = y_mid;
283 _children[static_cast<std::int8_t>(Quadrant::SWL)] =
284 new OctTree<POINT, MAX_POINTS>(_ll, p1, _eps);
285
286 // create child NEU
287 _children[static_cast<std::int8_t>(Quadrant::NEU)] =
288 new OctTree<POINT, MAX_POINTS>(p1, _ur, _eps);
289
290 // create child SEL
291 p0[0] = x_mid;
292 p1[0] = _ur[0];
293 _children[static_cast<std::int8_t>(Quadrant::SEL)] =
294 new OctTree<POINT, MAX_POINTS>(p0, p1, _eps);
295
296 // create child NWU
297 p0[0] = _ll[0];
298 p0[1] = y_mid;
299 p0[2] = z_mid;
300 p1[0] = x_mid;
301 p1[1] = _ur[1];
302 p1[2] = _ur[2];
303 _children[static_cast<std::int8_t>(Quadrant::NWU)] =
304 new OctTree<POINT, MAX_POINTS>(p0, p1, _eps);
305
306 // create child SWU
307 p0[1] = _ll[1];
308 p1[1] = y_mid;
309 _children[static_cast<std::int8_t>(Quadrant::SWU)] =
310 new OctTree<POINT, MAX_POINTS>(p0, p1, _eps);
311
312 // create child SEU
313 p0[0] = x_mid;
314 p1[0] = _ur[0];
315 p1[1] = y_mid;
316 p1[2] = _ur[2];
317 _children[static_cast<std::int8_t>(Quadrant::SEU)] =
318 new OctTree<POINT, MAX_POINTS>(p0, p1, _eps);
319
320 // add the passed point pnt to the children at first
321 for (std::size_t k(0); k < 8; k++)
322 {
323 if (_children[k]->addPointToChild(pnt))
324 {
325 break;
326 }
327 }
328
329 // distribute points to sub quadtrees
330 const std::size_t n_pnts(_pnts.size());
331 for (std::size_t j(0); j < n_pnts; j++)
332 {
333 if (std::any_of(begin(_children), end(_children),
334 [&](auto* child)
335 { return child->addPointToChild(_pnts[j]); }))
336 {
337 continue;
338 }
339 }
340 _is_leaf = false;
341}
342
343template <typename POINT, std::size_t MAX_POINTS>
344bool OctTree<POINT, MAX_POINTS>::isOutside(POINT* pnt) const
345{
346 if ((*pnt)[0] < _ll[0] || (*pnt)[1] < _ll[1] || (*pnt)[2] < _ll[2])
347 {
348 return true;
349 }
350 if ((*pnt)[0] >= _ur[0] || (*pnt)[1] >= _ur[1] || (*pnt)[2] >= _ur[2])
351 {
352 return true;
353 }
354 return false;
355}
356} // end namespace GeoLib
ERR
void ERR(fmt::format_string< Args... > fmt, Args &&... args)
Definition Logging.h:45
GeoLib::OctTree::addPoint
bool addPoint(POINT *pnt, POINT *&ret_pnt)
Definition OctTree-impl.h:77
GeoLib::OctTree::addPointToChild
bool addPointToChild(POINT *pnt)
Definition OctTree-impl.h:242
GeoLib::OctTree::addPoint_
bool addPoint_(POINT *pnt, POINT *&ret_pnt)
Definition OctTree-impl.h:204
GeoLib::OctTree::splitNode
void splitNode(POINT *pnt)
Definition OctTree-impl.h:262
GeoLib::OctTree::getPointsInRange
void getPointsInRange(T const &min, T const &max, std::vector< POINT * > &pnts) const
Definition OctTree-impl.h:148
GeoLib::OctTree::OctTree
OctTree(Eigen::Vector3d const &ll, Eigen::Vector3d const &ur, double eps)
Definition OctTree-impl.h:196
GeoLib::OctTree::getPointsInRange_
void getPointsInRange_(T const &min, T const &max, std::vector< POINT * > &pnts) const
Definition OctTree-impl.h:163
GeoLib::OctTree::_children
std::array< OctTree< POINT, MAX_POINTS > *, 8 > _children
Definition OctTree.h:145
GeoLib::OctTree::isOutside
bool isOutside(POINT *pnt) const
Definition OctTree-impl.h:344
GeoLib::OctTree::createOctTree
static OctTree< POINT, MAX_POINTS > * createOctTree(Eigen::Vector3d ll, Eigen::Vector3d ur, double eps=std::numeric_limits< double >::epsilon())
Definition OctTree-impl.h:16
GeoLib::OctTree::~OctTree
virtual ~OctTree()
Definition OctTree-impl.h:68