8 #ifndef SIMPLE_OCTREE_HPP_
9 #define SIMPLE_OCTREE_HPP_
21 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline
29 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline
32 this->deleteChildren ();
37 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline void
46 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline void
58 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline void
65 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);
156 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline void
164 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline void
172 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline void
175 if ( !this->hasData () || !node->
hasData () )
178 this->full_leaf_neighbors_.insert (node);
183 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline
191 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline
198 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline void
204 full_leaves_.clear();
208 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline void
210 NodeDataCreator* node_data_creator)
212 if ( voxel_size <= 0 )
217 voxel_size_ = voxel_size;
218 node_data_creator_ = node_data_creator;
220 Scalar extent = std::max (std::max (bounds[1]-bounds[0], bounds[3]-bounds[2]), bounds[5]-bounds[4]);
221 Scalar center[3] = {
static_cast<Scalar
> (0.5)*(bounds[0]+bounds[1]),
222 static_cast<Scalar
> (0.5)*(bounds[2]+bounds[3]),
223 static_cast<Scalar
> (0.5)*(bounds[4]+bounds[5])};
225 Scalar arg = extent/voxel_size;
229 tree_levels_ =
static_cast<int> (std::ceil (std::log (arg)/std::log (2.0)) + 0.5);
234 Scalar half_root_side =
static_cast<Scalar
> (0.5f*pow (2.0, tree_levels_)*voxel_size);
237 bounds_[0] = center[0] - half_root_side;
238 bounds_[1] = center[0] + half_root_side;
239 bounds_[2] = center[1] - half_root_side;
240 bounds_[3] = center[1] + half_root_side;
241 bounds_[4] = center[2] - half_root_side;
242 bounds_[5] = center[2] + half_root_side;
246 root_->setCenter (center);
247 root_->setBounds (bounds_);
248 root_->setParent (
nullptr);
249 root_->computeRadius ();
253 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline
258 if ( x < bounds_[0] || x > bounds_[1] ||
259 y < bounds_[2] || y > bounds_[3] ||
260 z < bounds_[4] || z > bounds_[5] )
268 for (
int l = 0 ; l < tree_levels_ ; ++l )
274 if ( x >= c[0] )
id |= 4;
275 if ( y >= c[1] )
id |= 2;
276 if ( z >= c[2] )
id |= 1;
283 node->
setData (node_data_creator_->create (node));
284 this->insertNeighbors (node);
285 full_leaves_.push_back (node);
292 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline
296 Scalar offset = 0.5f*voxel_size_;
297 Scalar p[3] = {bounds_[0] + offset +
static_cast<Scalar
> (i)*voxel_size_,
298 bounds_[2] + offset +
static_cast<Scalar
> (j)*voxel_size_,
299 bounds_[4] + offset +
static_cast<Scalar
> (k)*voxel_size_};
301 return (this->getFullLeaf (p[0], p[1], p[2]));
305 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline
310 if ( x < bounds_[0] || x > bounds_[1] ||
311 y < bounds_[2] || y > bounds_[3] ||
312 z < bounds_[4] || z > bounds_[5] )
320 for (
int l = 0 ; l < tree_levels_ ; ++l )
328 if ( x >= c[0] )
id |= 4;
329 if ( y >= c[1] )
id |= 2;
330 if ( z >= c[2] )
id |= 1;
342 template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline void
346 Scalar s =
static_cast<Scalar
> (0.5)*voxel_size_;
349 neigh = this->getFullLeaf (c[0]+s, c[1]+s, c[2]+s);
if ( neigh ) node->
makeNeighbors (neigh);
350 neigh = this->getFullLeaf (c[0]+s, c[1]+s, c[2] );
if ( neigh ) node->
makeNeighbors (neigh);
351 neigh = this->getFullLeaf (c[0]+s, c[1]+s, c[2]-s);
if ( neigh ) node->
makeNeighbors (neigh);
352 neigh = this->getFullLeaf (c[0]+s, c[1] , c[2]+s);
if ( neigh ) node->
makeNeighbors (neigh);
353 neigh = this->getFullLeaf (c[0]+s, c[1] , c[2] );
if ( neigh ) node->
makeNeighbors (neigh);
354 neigh = this->getFullLeaf (c[0]+s, c[1] , c[2]-s);
if ( neigh ) node->
makeNeighbors (neigh);
355 neigh = this->getFullLeaf (c[0]+s, c[1]-s, c[2]+s);
if ( neigh ) node->
makeNeighbors (neigh);
356 neigh = this->getFullLeaf (c[0]+s, c[1]-s, c[2] );
if ( neigh ) node->
makeNeighbors (neigh);
357 neigh = this->getFullLeaf (c[0]+s, c[1]-s, c[2]-s);
if ( neigh ) node->
makeNeighbors (neigh);
359 neigh = this->getFullLeaf (c[0] , c[1]+s, c[2]+s);
if ( neigh ) node->
makeNeighbors (neigh);
360 neigh = this->getFullLeaf (c[0] , c[1]+s, c[2] );
if ( neigh ) node->
makeNeighbors (neigh);
361 neigh = this->getFullLeaf (c[0] , c[1]+s, c[2]-s);
if ( neigh ) node->
makeNeighbors (neigh);
362 neigh = this->getFullLeaf (c[0] , c[1] , c[2]+s);
if ( neigh ) node->
makeNeighbors (neigh);
364 neigh = this->getFullLeaf (c[0] , c[1] , c[2]-s);
if ( neigh ) node->
makeNeighbors (neigh);
365 neigh = this->getFullLeaf (c[0] , c[1]-s, c[2]+s);
if ( neigh ) node->
makeNeighbors (neigh);
366 neigh = this->getFullLeaf (c[0] , c[1]-s, c[2] );
if ( neigh ) node->
makeNeighbors (neigh);
367 neigh = this->getFullLeaf (c[0] , c[1]-s, c[2]-s);
if ( neigh ) node->
makeNeighbors (neigh);
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);
std::set< Node * > full_leaf_neighbors_
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.