8 #ifndef SIMPLE_OCTREE_HPP_
9 #define SIMPLE_OCTREE_HPP_
16 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline
25 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline
28 this->deleteChildren ();
34 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline void
44 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]));
69 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline bool
75 Scalar bounds[6], center[3], childside = static_cast<Scalar> (0.5)*(
bounds_[1]-
bounds_[0]);
76 children_ =
new Node[8];
79 bounds[0] =
bounds_[0]; bounds[1] = center_[0];
80 bounds[2] =
bounds_[2]; bounds[3] = center_[1];
81 bounds[4] =
bounds_[4]; bounds[5] = center_[2];
83 center[0] = 0.5f*(bounds[0] + bounds[1]);
84 center[1] = 0.5f*(bounds[2] + bounds[3]);
85 center[2] = 0.5f*(bounds[4] + bounds[5]);
87 children_[0].setBounds(bounds);
88 children_[0].setCenter(center);
91 bounds[4] = center_[2]; bounds[5] =
bounds_[5];
93 center[2] += childside;
95 children_[1].setBounds(bounds);
96 children_[1].setCenter(center);
99 bounds[2] = center_[1]; bounds[3] =
bounds_[3];
101 center[1] += childside;
103 children_[3].setBounds(bounds);
104 children_[3].setCenter(center);
107 bounds[4] =
bounds_[4]; bounds[5] = center_[2];
109 center[2] -= childside;
111 children_[2].setBounds(bounds);
112 children_[2].setCenter(center);
115 bounds[0] = center_[0]; bounds[1] =
bounds_[1];
117 center[0] += childside;
119 children_[6].setBounds(bounds);
120 children_[6].setCenter(center);
123 bounds[4] = center_[2]; bounds[5] =
bounds_[5];
125 center[2] += childside;
127 children_[7].setBounds(bounds);
128 children_[7].setCenter(center);
131 bounds[2] =
bounds_[2]; bounds[3] = center_[1];
133 center[1] -= childside;
135 children_[5].setBounds(bounds);
136 children_[5].setCenter(center);
139 bounds[4] =
bounds_[4]; bounds[5] = center_[2];
141 center[2] -= childside;
143 children_[4].setBounds(bounds);
144 children_[4].setCenter(center);
146 for (
int i = 0 ; i < 8 ; ++i )
148 children_[i].computeRadius();
149 children_[i].setParent(
this);
157 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline void
169 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline void
181 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline void
184 if ( !this->hasData () || !node->
hasData () )
187 this->full_leaf_neighbors_.insert (node);
193 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline
202 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline
210 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline void
219 full_leaves_.clear();
224 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline void
226 NodeDataCreator* node_data_creator)
228 if ( voxel_size <= 0 )
233 voxel_size_ = voxel_size;
234 node_data_creator_ = node_data_creator;
236 Scalar extent = std::max (std::max (bounds[1]-bounds[0], bounds[3]-bounds[2]), bounds[5]-bounds[4]);
237 Scalar center[3] = {static_cast<Scalar> (0.5)*(bounds[0]+bounds[1]),
238 static_cast<Scalar> (0.5)*(bounds[2]+bounds[3]),
239 static_cast<Scalar> (0.5)*(bounds[4]+bounds[5])};
241 Scalar arg = extent/voxel_size;
245 tree_levels_ = static_cast<int> (std::ceil (std::log (arg)/std::log (2.0)) + 0.5);
250 Scalar half_root_side = static_cast<Scalar> (0.5f*pow (2.0, tree_levels_)*voxel_size);
253 bounds_[0] = center[0] - half_root_side;
254 bounds_[1] = center[0] + half_root_side;
255 bounds_[2] = center[1] - half_root_side;
256 bounds_[3] = center[1] + half_root_side;
257 bounds_[4] = center[2] - half_root_side;
258 bounds_[5] = center[2] + half_root_side;
262 root_->setCenter (center);
263 root_->setBounds (bounds_);
264 root_->setParent (
nullptr);
265 root_->computeRadius ();
270 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline
275 if ( x < bounds_[0] || x > bounds_[1] ||
276 y < bounds_[2] || y > bounds_[3] ||
277 z < bounds_[4] || z > bounds_[5] )
285 for (
int l = 0 ; l < tree_levels_ ; ++l )
288 const Scalar *c = node->getCenter ();
291 if ( x >= c[0] )
id |= 4;
292 if ( y >= c[1] )
id |= 2;
293 if ( z >= c[2] )
id |= 1;
295 node = node->getChild (
id);
298 if ( !node->hasData () )
300 node->setData (node_data_creator_->create (node));
301 this->insertNeighbors (node);
302 full_leaves_.push_back (node);
310 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline
314 Scalar offset = 0.5f*voxel_size_;
315 Scalar p[3] = {bounds_[0] + offset + static_cast<Scalar> (i)*voxel_size_,
316 bounds_[2] + offset + static_cast<Scalar> (j)*voxel_size_,
317 bounds_[4] + offset + static_cast<Scalar> (k)*voxel_size_};
319 return (this->getFullLeaf (p[0], p[1], p[2]));
324 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline
329 if ( x < bounds_[0] || x > bounds_[1] ||
330 y < bounds_[2] || y > bounds_[3] ||
331 z < bounds_[4] || z > bounds_[5] )
339 for (
int l = 0 ; l < tree_levels_ ; ++l )
341 if ( !node->hasChildren () )
347 if ( x >= c[0] )
id |= 4;
348 if ( y >= c[1] )
id |= 2;
349 if ( z >= c[2] )
id |= 1;
351 node = node->getChild (
id);
354 if ( !node->hasData () )
362 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline void
365 const Scalar* c = node->getCenter ();
366 Scalar s = static_cast<Scalar> (0.5)*voxel_size_;
369 neigh = this->getFullLeaf (c[0]+s, c[1]+s, c[2]+s);
if ( neigh ) node->makeNeighbors (neigh);
370 neigh = this->getFullLeaf (c[0]+s, c[1]+s, c[2] );
if ( neigh ) node->makeNeighbors (neigh);
371 neigh = this->getFullLeaf (c[0]+s, c[1]+s, c[2]-s);
if ( neigh ) node->makeNeighbors (neigh);
372 neigh = this->getFullLeaf (c[0]+s, c[1] , c[2]+s);
if ( neigh ) node->makeNeighbors (neigh);
373 neigh = this->getFullLeaf (c[0]+s, c[1] , c[2] );
if ( neigh ) node->makeNeighbors (neigh);
374 neigh = this->getFullLeaf (c[0]+s, c[1] , c[2]-s);
if ( neigh ) node->makeNeighbors (neigh);
375 neigh = this->getFullLeaf (c[0]+s, c[1]-s, c[2]+s);
if ( neigh ) node->makeNeighbors (neigh);
376 neigh = this->getFullLeaf (c[0]+s, c[1]-s, c[2] );
if ( neigh ) node->makeNeighbors (neigh);
377 neigh = this->getFullLeaf (c[0]+s, c[1]-s, c[2]-s);
if ( neigh ) node->makeNeighbors (neigh);
379 neigh = this->getFullLeaf (c[0] , c[1]+s, c[2]+s);
if ( neigh ) node->makeNeighbors (neigh);
380 neigh = this->getFullLeaf (c[0] , c[1]+s, c[2] );
if ( neigh ) node->makeNeighbors (neigh);
381 neigh = this->getFullLeaf (c[0] , c[1]+s, c[2]-s);
if ( neigh ) node->makeNeighbors (neigh);
382 neigh = this->getFullLeaf (c[0] , c[1] , c[2]+s);
if ( neigh ) node->makeNeighbors (neigh);
384 neigh = this->getFullLeaf (c[0] , c[1] , c[2]-s);
if ( neigh ) node->makeNeighbors (neigh);
385 neigh = this->getFullLeaf (c[0] , c[1]-s, c[2]+s);
if ( neigh ) node->makeNeighbors (neigh);
386 neigh = this->getFullLeaf (c[0] , c[1]-s, c[2] );
if ( neigh ) node->makeNeighbors (neigh);
387 neigh = this->getFullLeaf (c[0] , c[1]-s, c[2]-s);
if ( neigh ) node->makeNeighbors (neigh);
389 neigh = this->getFullLeaf (c[0]-s, c[1]+s, c[2]+s);
if ( neigh ) node->makeNeighbors (neigh);
390 neigh = this->getFullLeaf (c[0]-s, c[1]+s, c[2] );
if ( neigh ) node->makeNeighbors (neigh);
391 neigh = this->getFullLeaf (c[0]-s, c[1]+s, c[2]-s);
if ( neigh ) node->makeNeighbors (neigh);
392 neigh = this->getFullLeaf (c[0]-s, c[1] , c[2]+s);
if ( neigh ) node->makeNeighbors (neigh);
393 neigh = this->getFullLeaf (c[0]-s, c[1] , c[2] );
if ( neigh ) node->makeNeighbors (neigh);
394 neigh = this->getFullLeaf (c[0]-s, c[1] , c[2]-s);
if ( neigh ) node->makeNeighbors (neigh);
395 neigh = this->getFullLeaf (c[0]-s, c[1]-s, c[2]+s);
if ( neigh ) node->makeNeighbors (neigh);
396 neigh = this->getFullLeaf (c[0]-s, c[1]-s, c[2] );
if ( neigh ) node->makeNeighbors (neigh);
397 neigh = this->getFullLeaf (c[0]-s, c[1]-s, c[2]-s);
if ( neigh ) node->makeNeighbors (neigh);