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>
77bool OctTree<POINT, MAX_POINTS>::addPoint(POINT* pnt, POINT*& ret_pnt)
78{
79 // first do a range query using a epsilon 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 = pnt->asEigenVector3d().array() + _eps;
83 getPointsInRange(min, max, query_pnts);
84 auto const it =
85 std::find_if(query_pnts.begin(), query_pnts.end(),
86 [pnt, this](auto const* p)
87 {
88 return (p->asEigenVector3d() - pnt->asEigenVector3d())
89 .squaredNorm() < _eps * _eps;
90 });
91 if (it != query_pnts.end())
92 {
93 ret_pnt = *it;
94 return false;
95 }
96
97 // the point pnt is not yet in the OctTree
98 if (isOutside(pnt))
99 {
100 ret_pnt = nullptr;
101 return false;
102 }
103
104 // at this place it holds true that the point is within [_ll, _ur]
105 if (!_is_leaf)
106 {
107 for (auto c : _children)
108 {
109 if (c->addPoint_(pnt, ret_pnt))
110 {
111 return true;
112 }
113 if (ret_pnt != nullptr)
114 {
115 return false;
116 }
117 }
118 }
119
120 ret_pnt = pnt;
121
122 if (_pnts.size() < MAX_POINTS)
123 {
124 _pnts.push_back(pnt);
125 }
126 else
127 { // i.e. _pnts.size () == MAX_POINTS
128 splitNode(pnt);
129 _pnts.clear();
130 }
131 return true;
132}
133
134template <typename POINT, std::size_t MAX_POINTS>
135template <typename T>
137 T const& min, T const& max, std::vector<POINT*>& pnts) const
138{
139 if (_ur[0] < min[0] || _ur[1] < min[1] || _ur[2] < min[2])
140 {
141 return;
142 }
143
144 if (max[0] < _ll[0] || max[1] < _ll[1] || max[2] < _ll[2])
145 {
146 return;
147 }
148
149 if (_is_leaf)
150 {
151 std::copy_if(_pnts.begin(), _pnts.end(), std::back_inserter(pnts),
152 [&min, &max](auto const* p)
153 {
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]);
157 });
158 }
159 else
160 {
161 for (std::size_t k(0); k < 8; k++)
162 {
163 _children[k]->getPointsInRange(min, max, pnts);
164 }
165 }
166}
167
168template <typename POINT, std::size_t MAX_POINTS>
169OctTree<POINT, MAX_POINTS>::OctTree(Eigen::Vector3d const& ll,
170 Eigen::Vector3d const& ur, double eps)
171 : _ll(ll), _ur(ur), _is_leaf(true), _eps(eps)
172{
173 _children.fill(nullptr);
174}
175
176template <typename POINT, std::size_t MAX_POINTS>
177bool OctTree<POINT, MAX_POINTS>::addPoint_(POINT* pnt, POINT*& ret_pnt)
178{
179 if (isOutside(pnt))
180 {
181 ret_pnt = nullptr;
182 return false;
183 }
184
185 // at this place it holds true that the point is within [_ll, _ur]
186 if (!_is_leaf)
187 {
188 for (auto c : _children)
189 {
190 if (c->addPoint_(pnt, ret_pnt))
191 {
192 return true;
193 }
194 if (ret_pnt != nullptr)
195 {
196 return false;
197 }
198 }
199 }
200
201 ret_pnt = pnt;
202 if (_pnts.size() < MAX_POINTS)
203 {
204 _pnts.push_back(pnt);
205 }
206 else
207 { // i.e. _pnts.size () == MAX_POINTS
208 splitNode(pnt);
209 _pnts.clear();
210 }
211 return true;
212}
213
214template <typename POINT, std::size_t MAX_POINTS>
216{
217 if (isOutside(pnt))
218 {
219 return false;
220 }
221
222 if (_pnts.size() < MAX_POINTS)
223 {
224 _pnts.push_back(pnt);
225 }
226 else
227 { // i.e. _pnts.size () == MAX_POINTS
228 splitNode(pnt);
229 _pnts.clear();
230 }
231 return true;
232}
233
234template <typename POINT, std::size_t MAX_POINTS>
236{
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};
242
243 // create child NEL
244 _children[static_cast<std::int8_t>(Quadrant::NEL)] =
245 new OctTree<POINT, MAX_POINTS>(p0, p1, _eps);
246
247 // create child NWL
248 p0[0] = _ll[0];
249 p1[0] = x_mid;
250 _children[static_cast<std::int8_t>(Quadrant::NWL)] =
251 new OctTree<POINT, MAX_POINTS>(p0, p1, _eps);
252
253 // create child SWL
254 p0[1] = _ll[1];
255 p1[1] = y_mid;
256 _children[static_cast<std::int8_t>(Quadrant::SWL)] =
257 new OctTree<POINT, MAX_POINTS>(_ll, p1, _eps);
258
259 // create child NEU
260 _children[static_cast<std::int8_t>(Quadrant::NEU)] =
261 new OctTree<POINT, MAX_POINTS>(p1, _ur, _eps);
262
263 // create child SEL
264 p0[0] = x_mid;
265 p1[0] = _ur[0];
266 _children[static_cast<std::int8_t>(Quadrant::SEL)] =
267 new OctTree<POINT, MAX_POINTS>(p0, p1, _eps);
268
269 // create child NWU
270 p0[0] = _ll[0];
271 p0[1] = y_mid;
272 p0[2] = z_mid;
273 p1[0] = x_mid;
274 p1[1] = _ur[1];
275 p1[2] = _ur[2];
276 _children[static_cast<std::int8_t>(Quadrant::NWU)] =
277 new OctTree<POINT, MAX_POINTS>(p0, p1, _eps);
278
279 // create child SWU
280 p0[1] = _ll[1];
281 p1[1] = y_mid;
282 _children[static_cast<std::int8_t>(Quadrant::SWU)] =
283 new OctTree<POINT, MAX_POINTS>(p0, p1, _eps);
284
285 // create child SEU
286 p0[0] = x_mid;
287 p1[0] = _ur[0];
288 p1[1] = y_mid;
289 p1[2] = _ur[2];
290 _children[static_cast<std::int8_t>(Quadrant::SEU)] =
291 new OctTree<POINT, MAX_POINTS>(p0, p1, _eps);
292
293 // add the passed point pnt to the children at first
294 for (std::size_t k(0); k < 8; k++)
295 {
296 if (_children[k]->addPointToChild(pnt))
297 {
298 break;
299 }
300 }
301
302 // distribute points to sub quadtrees
303 const std::size_t n_pnts(_pnts.size());
304 for (std::size_t j(0); j < n_pnts; j++)
305 {
306 for (auto c : _children)
307 {
308 if (c->addPointToChild(_pnts[j]))
309 {
310 break;
311 }
312 }
313 }
314 _is_leaf = false;
315}
316
317template <typename POINT, std::size_t MAX_POINTS>
319{
320 if ((*pnt)[0] < _ll[0] || (*pnt)[1] < _ll[1] || (*pnt)[2] < _ll[2])
321 {
322 return true;
323 }
324 if ((*pnt)[0] >= _ur[0] || (*pnt)[1] >= _ur[1] || (*pnt)[2] >= _ur[2])
325 {
326 return true;
327 }
328 return false;
329}
330} // end namespace GeoLib
bool addPoint(POINT *pnt, POINT *&ret_pnt)
Definition: OctTree-impl.h:77
bool addPointToChild(POINT *pnt)
Definition: OctTree-impl.h:215
bool addPoint_(POINT *pnt, POINT *&ret_pnt)
Definition: OctTree-impl.h:177
void splitNode(POINT *pnt)
Definition: OctTree-impl.h:235
void getPointsInRange(T const &min, T const &max, std::vector< POINT * > &pnts) const
Definition: OctTree-impl.h:136
OctTree(Eigen::Vector3d const &ll, Eigen::Vector3d const &ur, double eps)
Definition: OctTree-impl.h:169
std::array< OctTree< POINT, MAX_POINTS > *, 8 > _children
Definition: OctTree.h:139
bool isOutside(POINT *pnt) const
Definition: OctTree-impl.h:318
static OctTree< POINT, MAX_POINTS > * createOctTree(Eigen::Vector3d ll, Eigen::Vector3d ur, double eps=std::numeric_limits< double >::epsilon())
Definition: OctTree-impl.h:16
virtual ~OctTree()
Definition: OctTree-impl.h:68