8 #ifndef SIMPLE_OCTREE_HPP_
9 #define SIMPLE_OCTREE_HPP_
13 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline
22 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline
25 this->deleteChildren ();
31 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline void
41 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline void
54 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline void
61 radius_ =
static_cast<Scalar
> (std::sqrt (v[0]*v[0] + v[1]*v[1] + v[2]*v[2]));
66 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline bool
72 Scalar bounds[6], center[3], childside =
static_cast<Scalar
> (0.5)*(
bounds_[1]-
bounds_[0]);
73 children_ =
new Node[8];
76 bounds[0] =
bounds_[0]; bounds[1] = center_[0];
77 bounds[2] =
bounds_[2]; bounds[3] = center_[1];
78 bounds[4] =
bounds_[4]; bounds[5] = center_[2];
80 center[0] = 0.5f*(bounds[0] + bounds[1]);
81 center[1] = 0.5f*(bounds[2] + bounds[3]);
82 center[2] = 0.5f*(bounds[4] + bounds[5]);
84 children_[0].setBounds(bounds);
85 children_[0].setCenter(center);
88 bounds[4] = center_[2]; bounds[5] =
bounds_[5];
90 center[2] += childside;
92 children_[1].setBounds(bounds);
93 children_[1].setCenter(center);
96 bounds[2] = center_[1]; bounds[3] =
bounds_[3];
98 center[1] += childside;
100 children_[3].setBounds(bounds);
101 children_[3].setCenter(center);
104 bounds[4] =
bounds_[4]; bounds[5] = center_[2];
106 center[2] -= childside;
108 children_[2].setBounds(bounds);
109 children_[2].setCenter(center);
112 bounds[0] = center_[0]; bounds[1] =
bounds_[1];
114 center[0] += childside;
116 children_[6].setBounds(bounds);
117 children_[6].setCenter(center);
120 bounds[4] = center_[2]; bounds[5] =
bounds_[5];
122 center[2] += childside;
124 children_[7].setBounds(bounds);
125 children_[7].setCenter(center);
128 bounds[2] =
bounds_[2]; bounds[3] = center_[1];
130 center[1] -= childside;
132 children_[5].setBounds(bounds);
133 children_[5].setCenter(center);
136 bounds[4] =
bounds_[4]; bounds[5] = center_[2];
138 center[2] -= childside;
140 children_[4].setBounds(bounds);
141 children_[4].setCenter(center);
143 for (
int i = 0 ; i < 8 ; ++i )
145 children_[i].computeRadius();
146 children_[i].setParent(
this);
154 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline void
166 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline void
178 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline void
181 if ( !this->hasData () || !node->
hasData () )
184 this->full_leaf_neighbors_.insert (node);
190 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline
199 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline
207 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline void
216 full_leaves_.clear();
221 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline void
223 NodeDataCreator* node_data_creator)
225 if ( voxel_size <= 0 )
230 voxel_size_ = voxel_size;
231 node_data_creator_ = node_data_creator;
233 Scalar extent = std::max (std::max (bounds[1]-bounds[0], bounds[3]-bounds[2]), bounds[5]-bounds[4]);
234 Scalar center[3] = {
static_cast<Scalar
> (0.5)*(bounds[0]+bounds[1]),
235 static_cast<Scalar
> (0.5)*(bounds[2]+bounds[3]),
236 static_cast<Scalar
> (0.5)*(bounds[4]+bounds[5])};
238 Scalar arg = extent/voxel_size;
242 tree_levels_ =
static_cast<int> (std::ceil (std::log (arg)/std::log (2.0)) + 0.5);
247 Scalar half_root_side =
static_cast<Scalar
> (0.5f*pow (2.0, tree_levels_)*voxel_size);
250 bounds_[0] = center[0] - half_root_side;
251 bounds_[1] = center[0] + half_root_side;
252 bounds_[2] = center[1] - half_root_side;
253 bounds_[3] = center[1] + half_root_side;
254 bounds_[4] = center[2] - half_root_side;
255 bounds_[5] = center[2] + half_root_side;
259 root_->setCenter (center);
260 root_->setBounds (bounds_);
261 root_->setParent (
nullptr);
262 root_->computeRadius ();
267 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline
272 if ( x < bounds_[0] || x > bounds_[1] ||
273 y < bounds_[2] || y > bounds_[3] ||
274 z < bounds_[4] || z > bounds_[5] )
284 for (
int l = 0 ; l < tree_levels_ ; ++l )
287 c = node->getCenter ();
290 if ( x >= c[0] )
id |= 4;
291 if ( y >= c[1] )
id |= 2;
292 if ( z >= c[2] )
id |= 1;
294 node = node->getChild (
id);
297 if ( !node->hasData () )
299 node->setData (node_data_creator_->create (node));
300 this->insertNeighbors (node);
301 full_leaves_.push_back (node);
309 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline
313 Scalar offset = 0.5f*voxel_size_;
314 Scalar p[3] = {bounds_[0] + offset +
static_cast<Scalar
> (i)*voxel_size_,
315 bounds_[2] + offset +
static_cast<Scalar
> (j)*voxel_size_,
316 bounds_[4] + offset +
static_cast<Scalar
> (k)*voxel_size_};
318 return (this->getFullLeaf (p[0], p[1], p[2]));
323 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline
328 if ( x < bounds_[0] || x > bounds_[1] ||
329 y < bounds_[2] || y > bounds_[3] ||
330 z < bounds_[4] || z > bounds_[5] )
340 for (
int l = 0 ; l < tree_levels_ ; ++l )
342 if ( !node->hasChildren () )
348 if ( x >= c[0] )
id |= 4;
349 if ( y >= c[1] )
id |= 2;
350 if ( z >= c[2] )
id |= 1;
352 node = node->getChild (
id);
355 if ( !node->hasData () )
363 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline void
366 const Scalar* c = node->getCenter ();
367 Scalar s =
static_cast<Scalar
> (0.5)*voxel_size_;
370 neigh = this->getFullLeaf (c[0]+s, c[1]+s, c[2]+s);
if ( neigh ) node->makeNeighbors (neigh);
371 neigh = this->getFullLeaf (c[0]+s, c[1]+s, c[2] );
if ( neigh ) node->makeNeighbors (neigh);
372 neigh = this->getFullLeaf (c[0]+s, c[1]+s, c[2]-s);
if ( neigh ) node->makeNeighbors (neigh);
373 neigh = this->getFullLeaf (c[0]+s, c[1] , c[2]+s);
if ( neigh ) node->makeNeighbors (neigh);
374 neigh = this->getFullLeaf (c[0]+s, c[1] , c[2] );
if ( neigh ) node->makeNeighbors (neigh);
375 neigh = this->getFullLeaf (c[0]+s, c[1] , c[2]-s);
if ( neigh ) node->makeNeighbors (neigh);
376 neigh = this->getFullLeaf (c[0]+s, c[1]-s, c[2]+s);
if ( neigh ) node->makeNeighbors (neigh);
377 neigh = this->getFullLeaf (c[0]+s, c[1]-s, c[2] );
if ( neigh ) node->makeNeighbors (neigh);
378 neigh = this->getFullLeaf (c[0]+s, c[1]-s, c[2]-s);
if ( neigh ) node->makeNeighbors (neigh);
380 neigh = this->getFullLeaf (c[0] , c[1]+s, c[2]+s);
if ( neigh ) node->makeNeighbors (neigh);
381 neigh = this->getFullLeaf (c[0] , c[1]+s, c[2] );
if ( neigh ) node->makeNeighbors (neigh);
382 neigh = this->getFullLeaf (c[0] , c[1]+s, c[2]-s);
if ( neigh ) node->makeNeighbors (neigh);
383 neigh = this->getFullLeaf (c[0] , c[1] , c[2]+s);
if ( neigh ) node->makeNeighbors (neigh);
385 neigh = this->getFullLeaf (c[0] , c[1] , c[2]-s);
if ( neigh ) node->makeNeighbors (neigh);
386 neigh = this->getFullLeaf (c[0] , c[1]-s, c[2]+s);
if ( neigh ) node->makeNeighbors (neigh);
387 neigh = this->getFullLeaf (c[0] , c[1]-s, c[2] );
if ( neigh ) node->makeNeighbors (neigh);
388 neigh = this->getFullLeaf (c[0] , c[1]-s, c[2]-s);
if ( neigh ) node->makeNeighbors (neigh);
390 neigh = this->getFullLeaf (c[0]-s, c[1]+s, c[2]+s);
if ( neigh ) node->makeNeighbors (neigh);
391 neigh = this->getFullLeaf (c[0]-s, c[1]+s, c[2] );
if ( neigh ) node->makeNeighbors (neigh);
392 neigh = this->getFullLeaf (c[0]-s, c[1]+s, c[2]-s);
if ( neigh ) node->makeNeighbors (neigh);
393 neigh = this->getFullLeaf (c[0]-s, c[1] , c[2]+s);
if ( neigh ) node->makeNeighbors (neigh);
394 neigh = this->getFullLeaf (c[0]-s, c[1] , c[2] );
if ( neigh ) node->makeNeighbors (neigh);
395 neigh = this->getFullLeaf (c[0]-s, c[1] , c[2]-s);
if ( neigh ) node->makeNeighbors (neigh);
396 neigh = this->getFullLeaf (c[0]-s, c[1]-s, c[2]+s);
if ( neigh ) node->makeNeighbors (neigh);
397 neigh = this->getFullLeaf (c[0]-s, c[1]-s, c[2] );
if ( neigh ) node->makeNeighbors (neigh);
398 neigh = this->getFullLeaf (c[0]-s, c[1]-s, c[2]-s);
if ( neigh ) node->makeNeighbors (neigh);