OGS
OctTree-impl.h
Go to the documentation of this file.
1
13namespace GeoLib
14{
15template <typename POINT, std::size_t MAX_POINTS>
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>
69{
70 for (auto c : _children)
71 {
72 delete c;
73 }
74}
75
76template <typename POINT, std::size_t MAX_POINTS>
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>
149 T const& min, T const& max, std::vector<POINT*>& pnts) const
150{
151 if (_ur[0] < min[0] || _ur[1] < min[1] || _ur[2] < min[2])
152 {
153 return;
154 }
155
156 if (max[0] < _ll[0] || max[1] < _ll[1] || max[2] < _ll[2])
157 {
158 return;
159 }
160
161 if (_is_leaf)
162 {
163 std::copy_if(_pnts.begin(), _pnts.end(), std::back_inserter(pnts),
164 [&min, &max](auto const* p)
165 {
166 return (min[0] <= (*p)[0] && (*p)[0] < max[0] &&
167 min[1] <= (*p)[1] && (*p)[1] < max[1] &&
168 min[2] <= (*p)[2] && (*p)[2] < max[2]);
169 });
170 }
171 else
172 {
173 for (std::size_t k(0); k < 8; k++)
174 {
175 _children[k]->getPointsInRange(min, max, pnts);
176 }
177 }
178}
179
180template <typename POINT, std::size_t MAX_POINTS>
181OctTree<POINT, MAX_POINTS>::OctTree(Eigen::Vector3d const& ll,
182 Eigen::Vector3d const& ur, double eps)
183 : _ll(ll), _ur(ur), _is_leaf(true), _eps(eps)
184{
185 _children.fill(nullptr);
186}
187
188template <typename POINT, std::size_t MAX_POINTS>
190{
191 if (isOutside(pnt))
192 {
193 ret_pnt = nullptr;
194 return false;
195 }
196
197 // at this place it holds true that the point is within [_ll, _ur]
198 if (!_is_leaf)
199 {
200 for (auto c : _children)
201 {
202 if (c->addPoint_(pnt, ret_pnt))
203 {
204 return true;
205 }
206 if (ret_pnt != nullptr)
207 {
208 return false;
209 }
210 }
211 }
212
213 ret_pnt = pnt;
214 if (_pnts.size() < MAX_POINTS)
215 {
216 _pnts.push_back(pnt);
217 }
218 else
219 { // i.e. _pnts.size () == MAX_POINTS
220 splitNode(pnt);
221 _pnts.clear();
222 }
223 return true;
224}
225
226template <typename POINT, std::size_t MAX_POINTS>
228{
229 if (isOutside(pnt))
230 {
231 return false;
232 }
233
234 if (_pnts.size() < MAX_POINTS)
235 {
236 _pnts.push_back(pnt);
237 }
238 else
239 { // i.e. _pnts.size () == MAX_POINTS
240 splitNode(pnt);
241 _pnts.clear();
242 }
243 return true;
244}
245
246template <typename POINT, std::size_t MAX_POINTS>
248{
249 const double x_mid((_ur[0] + _ll[0]) / 2.0);
250 const double y_mid((_ur[1] + _ll[1]) / 2.0);
251 const double z_mid((_ur[2] + _ll[2]) / 2.0);
252 Eigen::Vector3d p0{x_mid, y_mid, _ll[2]};
253 Eigen::Vector3d p1{_ur[0], _ur[1], z_mid};
254
255 // create child NEL
257 new OctTree<POINT, MAX_POINTS>(p0, p1, _eps);
258
259 // create child NWL
260 p0[0] = _ll[0];
261 p1[0] = x_mid;
263 new OctTree<POINT, MAX_POINTS>(p0, p1, _eps);
264
265 // create child SWL
266 p0[1] = _ll[1];
267 p1[1] = y_mid;
269 new OctTree<POINT, MAX_POINTS>(_ll, p1, _eps);
270
271 // create child NEU
273 new OctTree<POINT, MAX_POINTS>(p1, _ur, _eps);
274
275 // create child SEL
276 p0[0] = x_mid;
277 p1[0] = _ur[0];
279 new OctTree<POINT, MAX_POINTS>(p0, p1, _eps);
280
281 // create child NWU
282 p0[0] = _ll[0];
283 p0[1] = y_mid;
284 p0[2] = z_mid;
285 p1[0] = x_mid;
286 p1[1] = _ur[1];
287 p1[2] = _ur[2];
289 new OctTree<POINT, MAX_POINTS>(p0, p1, _eps);
290
291 // create child SWU
292 p0[1] = _ll[1];
293 p1[1] = y_mid;
295 new OctTree<POINT, MAX_POINTS>(p0, p1, _eps);
296
297 // create child SEU
298 p0[0] = x_mid;
299 p1[0] = _ur[0];
300 p1[1] = y_mid;
301 p1[2] = _ur[2];
303 new OctTree<POINT, MAX_POINTS>(p0, p1, _eps);
304
305 // add the passed point pnt to the children at first
306 for (std::size_t k(0); k < 8; k++)
307 {
309 {
310 break;
311 }
312 }
313
314 // distribute points to sub quadtrees
315 const std::size_t n_pnts(_pnts.size());
316 for (std::size_t j(0); j < n_pnts; j++)
317 {
318 if (std::any_of(begin(_children), end(_children),
319 [&](auto* child)
320 { return child->addPointToChild(_pnts[j]); }))
321 {
322 continue;
323 }
324 }
325 _is_leaf = false;
326}
327
328template <typename POINT, std::size_t MAX_POINTS>
330{
331 if ((*pnt)[0] < _ll[0] || (*pnt)[1] < _ll[1] || (*pnt)[2] < _ll[2])
332 {
333 return true;
334 }
335 if ((*pnt)[0] >= _ur[0] || (*pnt)[1] >= _ur[1] || (*pnt)[2] >= _ur[2])
336 {
337 return true;
338 }
339 return false;
340}
341} // end namespace GeoLib
bool addPoint(POINT *pnt, POINT *&ret_pnt)