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);
178 node->full_leaf_neighbors_.insert (
this);
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 )
273 if ( x >= c[0] )
id |= 4;
274 if ( y >= c[1] )
id |= 2;
275 if ( z >= c[2] )
id |= 1;
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 )
327 if ( x >= c[0] )
id |= 4;
328 if ( y >= c[1] )
id |= 2;
329 if ( z >= c[2] )
id |= 1;
341 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline void
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);
void setBounds(const Scalar *b)
void makeNeighbors(Node *node)
Make this and 'node' neighbors by inserting each node in the others node neighbor set.
void computeRadius()
Computes the "radius" of the node which is half the diagonal length.
void setData(const NodeData &src)
void setCenter(const Scalar *c)
const Scalar * getCenter() const
void insertNeighbors(Node *node)
Node * createLeaf(Scalar x, Scalar y, Scalar z)
Creates the leaf containing p = (x, y, z) and returns a pointer to it, however, only if p lies within...
void build(const Scalar *bounds, Scalar voxel_size, NodeDataCreator *node_data_creator)
Creates an empty octree with bounds at least as large as the ones provided as input and with leaf siz...
Node * getFullLeaf(int i, int j, int k)
Since the leaves are aligned in a rectilinear grid, each leaf has a unique id.