8 #ifndef SIMPLE_OCTREE_HPP_
9 #define SIMPLE_OCTREE_HPP_
20 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline
28 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline
31 this->deleteChildren ();
36 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline void
45 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline void
57 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline void
64 radius_ =
static_cast<Scalar
> (std::sqrt (v[0]*v[0] + v[1]*v[1] + v[2]*v[2]));
68 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline bool
74 Scalar bounds[6], center[3], childside =
static_cast<Scalar
> (0.5)*(
bounds_[1]-
bounds_[0]);
75 children_ =
new Node[8];
78 bounds[0] =
bounds_[0]; bounds[1] = center_[0];
79 bounds[2] =
bounds_[2]; bounds[3] = center_[1];
80 bounds[4] =
bounds_[4]; bounds[5] = center_[2];
82 center[0] = 0.5f*(bounds[0] + bounds[1]);
83 center[1] = 0.5f*(bounds[2] + bounds[3]);
84 center[2] = 0.5f*(bounds[4] + bounds[5]);
86 children_[0].setBounds(bounds);
87 children_[0].setCenter(center);
90 bounds[4] = center_[2]; bounds[5] =
bounds_[5];
92 center[2] += childside;
94 children_[1].setBounds(bounds);
95 children_[1].setCenter(center);
98 bounds[2] = center_[1]; bounds[3] =
bounds_[3];
100 center[1] += childside;
102 children_[3].setBounds(bounds);
103 children_[3].setCenter(center);
106 bounds[4] =
bounds_[4]; bounds[5] = center_[2];
108 center[2] -= childside;
110 children_[2].setBounds(bounds);
111 children_[2].setCenter(center);
114 bounds[0] = center_[0]; bounds[1] =
bounds_[1];
116 center[0] += childside;
118 children_[6].setBounds(bounds);
119 children_[6].setCenter(center);
122 bounds[4] = center_[2]; bounds[5] =
bounds_[5];
124 center[2] += childside;
126 children_[7].setBounds(bounds);
127 children_[7].setCenter(center);
130 bounds[2] =
bounds_[2]; bounds[3] = center_[1];
132 center[1] -= childside;
134 children_[5].setBounds(bounds);
135 children_[5].setCenter(center);
138 bounds[4] =
bounds_[4]; bounds[5] = center_[2];
140 center[2] -= childside;
142 children_[4].setBounds(bounds);
143 children_[4].setCenter(center);
145 for (
int i = 0 ; i < 8 ; ++i )
147 children_[i].computeRadius();
148 children_[i].setParent(
this);
155 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline void
163 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline void
171 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline void
174 if ( !this->hasData () || !node->
hasData () )
177 this->full_leaf_neighbors_.insert (node);
182 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline
190 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline
197 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline void
203 full_leaves_.
clear();
207 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline void
209 NodeDataCreator* node_data_creator)
211 if ( voxel_size <= 0 )
216 voxel_size_ = voxel_size;
217 node_data_creator_ = node_data_creator;
219 Scalar extent = std::max (std::max (bounds[1]-bounds[0], bounds[3]-bounds[2]), bounds[5]-bounds[4]);
220 Scalar center[3] = {
static_cast<Scalar
> (0.5)*(bounds[0]+bounds[1]),
221 static_cast<Scalar
> (0.5)*(bounds[2]+bounds[3]),
222 static_cast<Scalar
> (0.5)*(bounds[4]+bounds[5])};
224 Scalar arg = extent/voxel_size;
228 tree_levels_ =
static_cast<int> (std::ceil (std::log (arg)/std::log (2.0)) + 0.5);
233 Scalar half_root_side =
static_cast<Scalar
> (0.5f*pow (2.0, tree_levels_)*voxel_size);
236 bounds_[0] = center[0] - half_root_side;
237 bounds_[1] = center[0] + half_root_side;
238 bounds_[2] = center[1] - half_root_side;
239 bounds_[3] = center[1] + half_root_side;
240 bounds_[4] = center[2] - half_root_side;
241 bounds_[5] = center[2] + half_root_side;
245 root_->setCenter (center);
246 root_->setBounds (bounds_);
247 root_->setParent (
nullptr);
248 root_->computeRadius ();
252 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline
257 if ( x < bounds_[0] || x > bounds_[1] ||
258 y < bounds_[2] || y > bounds_[3] ||
259 z < bounds_[4] || z > bounds_[5] )
267 for (
int l = 0 ; l < tree_levels_ ; ++l )
269 node->createChildren ();
270 const Scalar *c = node->getCenter ();
273 if ( x >= c[0] )
id |= 4;
274 if ( y >= c[1] )
id |= 2;
275 if ( z >= c[2] )
id |= 1;
277 node = node->getChild (
id);
280 if ( !node->hasData () )
282 node->setData (node_data_creator_->create (node));
283 this->insertNeighbors (node);
284 full_leaves_.push_back (node);
291 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline
295 Scalar offset = 0.5f*voxel_size_;
296 Scalar p[3] = {bounds_[0] + offset +
static_cast<Scalar
> (i)*voxel_size_,
297 bounds_[2] + offset +
static_cast<Scalar
> (j)*voxel_size_,
298 bounds_[4] + offset +
static_cast<Scalar
> (k)*voxel_size_};
300 return (this->getFullLeaf (p[0], p[1], p[2]));
304 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline
309 if ( x < bounds_[0] || x > bounds_[1] ||
310 y < bounds_[2] || y > bounds_[3] ||
311 z < bounds_[4] || z > bounds_[5] )
319 for (
int l = 0 ; l < tree_levels_ ; ++l )
321 if ( !node->hasChildren () )
324 const Scalar *c = node->getCenter ();
327 if ( x >= c[0] )
id |= 4;
328 if ( y >= c[1] )
id |= 2;
329 if ( z >= c[2] )
id |= 1;
331 node = node->getChild (
id);
334 if ( !node->hasData () )
341 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline void
344 const Scalar* c = node->getCenter ();
345 Scalar s =
static_cast<Scalar
> (0.5)*voxel_size_;
348 neigh = this->getFullLeaf (c[0]+s, c[1]+s, c[2]+s);
if ( neigh ) node->makeNeighbors (neigh);
349 neigh = this->getFullLeaf (c[0]+s, c[1]+s, c[2] );
if ( neigh ) node->makeNeighbors (neigh);
350 neigh = this->getFullLeaf (c[0]+s, c[1]+s, c[2]-s);
if ( neigh ) node->makeNeighbors (neigh);
351 neigh = this->getFullLeaf (c[0]+s, c[1] , c[2]+s);
if ( neigh ) node->makeNeighbors (neigh);
352 neigh = this->getFullLeaf (c[0]+s, c[1] , c[2] );
if ( neigh ) node->makeNeighbors (neigh);
353 neigh = this->getFullLeaf (c[0]+s, c[1] , c[2]-s);
if ( neigh ) node->makeNeighbors (neigh);
354 neigh = this->getFullLeaf (c[0]+s, c[1]-s, c[2]+s);
if ( neigh ) node->makeNeighbors (neigh);
355 neigh = this->getFullLeaf (c[0]+s, c[1]-s, c[2] );
if ( neigh ) node->makeNeighbors (neigh);
356 neigh = this->getFullLeaf (c[0]+s, c[1]-s, c[2]-s);
if ( neigh ) node->makeNeighbors (neigh);
358 neigh = this->getFullLeaf (c[0] , c[1]+s, c[2]+s);
if ( neigh ) node->makeNeighbors (neigh);
359 neigh = this->getFullLeaf (c[0] , c[1]+s, c[2] );
if ( neigh ) node->makeNeighbors (neigh);
360 neigh = this->getFullLeaf (c[0] , c[1]+s, c[2]-s);
if ( neigh ) node->makeNeighbors (neigh);
361 neigh = this->getFullLeaf (c[0] , c[1] , c[2]+s);
if ( neigh ) node->makeNeighbors (neigh);
363 neigh = this->getFullLeaf (c[0] , c[1] , c[2]-s);
if ( neigh ) node->makeNeighbors (neigh);
364 neigh = this->getFullLeaf (c[0] , c[1]-s, c[2]+s);
if ( neigh ) node->makeNeighbors (neigh);
365 neigh = this->getFullLeaf (c[0] , c[1]-s, c[2] );
if ( neigh ) node->makeNeighbors (neigh);
366 neigh = this->getFullLeaf (c[0] , c[1]-s, c[2]-s);
if ( neigh ) node->makeNeighbors (neigh);
368 neigh = this->getFullLeaf (c[0]-s, c[1]+s, c[2]+s);
if ( neigh ) node->makeNeighbors (neigh);
369 neigh = this->getFullLeaf (c[0]-s, c[1]+s, c[2] );
if ( neigh ) node->makeNeighbors (neigh);
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] , c[2]+s);
if ( neigh ) node->makeNeighbors (neigh);
372 neigh = this->getFullLeaf (c[0]-s, c[1] , c[2] );
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]-s, c[2]+s);
if ( neigh ) node->makeNeighbors (neigh);
375 neigh = this->getFullLeaf (c[0]-s, c[1]-s, c[2] );
if ( neigh ) node->makeNeighbors (neigh);
376 neigh = this->getFullLeaf (c[0]-s, c[1]-s, c[2]-s);
if ( neigh ) node->makeNeighbors (neigh);