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
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);
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
186template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline
193template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline void
203template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline void
205 NodeDataCreator* node_data_creator)
207 if ( voxel_size <= 0 )
215 Scalar extent = std::max (std::max (bounds[1]-bounds[0], bounds[3]-bounds[2]), bounds[5]-bounds[4]);
216 Scalar center[3] = {
static_cast<Scalar
> (0.5)*(bounds[0]+bounds[1]),
217 static_cast<Scalar
> (0.5)*(bounds[2]+bounds[3]),
218 static_cast<Scalar
> (0.5)*(bounds[4]+bounds[5])};
220 Scalar arg = extent/voxel_size;
224 tree_levels_ =
static_cast<int> (std::ceil (std::log (arg)/std::log (2.0)) + 0.5);
229 Scalar half_root_side =
static_cast<Scalar
> (0.5f*pow (2.0,
tree_levels_)*voxel_size);
232 bounds_[0] = center[0] - half_root_side;
233 bounds_[1] = center[0] + half_root_side;
234 bounds_[2] = center[1] - half_root_side;
235 bounds_[3] = center[1] + half_root_side;
236 bounds_[4] = center[2] - half_root_side;
237 bounds_[5] = center[2] + half_root_side;
248template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline
269 if ( x >= c[0] )
id |= 4;
270 if ( y >= c[1] )
id |= 2;
271 if ( z >= c[2] )
id |= 1;
287template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline
300template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline
323 if ( x >= c[0] )
id |= 4;
324 if ( y >= c[1] )
id |= 2;
325 if ( z >= c[2] )
id |= 1;
337template<
typename NodeData,
typename NodeDataCreator,
typename Scalar>
inline void
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 setParent(Node *parent)
void setData(const NodeData &src)
void setCenter(const Scalar *c)
void insertNeighbors(Node *node)
NodeDataCreator * node_data_creator_
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...
std::vector< Node * > full_leaves_
Node * getFullLeaf(int i, int j, int k)
Since the leaves are aligned in a rectilinear grid, each leaf has a unique id.