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> (ceil (log (arg)/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 (NULL);
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 )
290 if ( x >= c[0] )
id |= 4;
291 if ( y >= c[1] )
id |= 2;
292 if ( z >= c[2] )
id |= 1;
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 )
348 if ( x >= c[0] )
id |= 4;
349 if ( y >= c[1] )
id |= 2;
350 if ( z >= c[2] )
id |= 1;
363 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline void
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);
Node * getFullLeaf(int i, int j, int k)
Since the leaves are aligned in a rectilinear grid, each leaf has a unique id.
void setData(const NodeData &src)
const Scalar * getCenter() const
void makeNeighbors(Node *node)
Make this and 'node' neighbors by inserting each node in the others node neighbor set...
void setBounds(const Scalar *b)
void insertNeighbors(Node *node)
std::set< Node * > full_leaf_neighbors_
void setCenter(const Scalar *c)
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...
void computeRadius()
Computes the "radius" of the node which is half the diagonal length.