8#ifndef SIMPLE_OCTREE_HPP_
9#define SIMPLE_OCTREE_HPP_
20template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline
28template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline
31 this->deleteChildren ();
36template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline void
45template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline void
57template<
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]));
68template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline bool
75 children_ =
new Node[8];
86 children_[0].setBounds(
bounds);
87 children_[0].setCenter(
center);
94 children_[1].setBounds(
bounds);
95 children_[1].setCenter(
center);
102 children_[3].setBounds(
bounds);
103 children_[3].setCenter(
center);
110 children_[2].setBounds(
bounds);
111 children_[2].setCenter(
center);
118 children_[6].setBounds(
bounds);
119 children_[6].setCenter(
center);
127 children_[7].setCenter(
center);
134 children_[5].setBounds(
bounds);
135 children_[5].setCenter(
center);
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);
155template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline void
163template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline void
171template<
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);
182template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline
190template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline
197template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline void
203 full_leaves_.clear();
207template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline void
228 tree_levels_ =
static_cast<int> (std::ceil (std::log (
arg)/std::log (2.0)) + 0.5);
245 root_->setCenter (
center);
246 root_->setBounds (bounds_);
247 root_->setParent (
nullptr);
248 root_->computeRadius ();
252template<
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);
291template<
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]));
304template<
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;
341template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline void
345 Scalar s =
static_cast<Scalar
> (0.5)*voxel_size_;
Iterator class for point clouds with or without given indices.
void setBounds(const Scalar *b)
void makeNeighbors(Node *node)
Make this and 'node' neighbors by inserting each node in the others node neighbor set.
const Scalar * getCenter() const
void computeRadius()
Computes the "radius" of the node which is half the diagonal length.
void setData(const NodeData &src)
void setCenter(const Scalar *c)
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.