void updateViewer (ORROctree& octree, PCLVisualizer& viz, std::vector<ORROctree::Node*>::iterator leaf)
{
viz.removeAllShapes();
const float *b = (*leaf)->getBounds (), *center = (*leaf)->getData ()->getPoint ();
float radius = 0.1f*octree.getRoot ()->getRadius ();
// Add the main leaf as a cube
viz.addCube (b[0], b[1], b[2], b[3], b[4], b[5], 0.0, 0.0, 1.0, "main cube");
// Get all full leaves intersecting a sphere with certain radius
std::list<ORROctree::Node*> intersected_leaves;
octree.getFullLeavesIntersectedBySphere(center, radius, intersected_leaves);
char cube_id[128];
int i = 0;
// Show the cubes
for ( std::list<ORROctree::Node*>::iterator it = intersected_leaves.begin () ; it != intersected_leaves.end () ; ++it )
{
sprintf(cube_id, "cube %i", ++i);
b = (*it)->getBounds ();
viz.addCube (b[0], b[1], b[2], b[3], b[4], b[5], 1.0, 1.0, 0.0, cube_id);
}
// Get a random full leaf on the sphere defined by 'center' and 'radius'
ORROctree::Node *rand_leaf = octree.getRandomFullLeafOnSphere (center, radius);
if ( rand_leaf )
{
pcl::ModelCoefficients sphere_coeffs;
sphere_coeffs.values.resize (4);
sphere_coeffs.values[0] = rand_leaf->getCenter ()[0];
sphere_coeffs.values[1] = rand_leaf->getCenter ()[1];
sphere_coeffs.values[2] = rand_leaf->getCenter ()[2];
sphere_coeffs.values[3] = 0.5f*(b[1] - b[0]);
viz.addSphere (sphere_coeffs, "random_full_leaf");
}
}
