void Octree::getFullLeafsWithinSphere(double* p, double radius, list<OctreeNode*>& out)
{
list<OctreeNode*> nodes;
nodes.push_back(mRoot);
int i;
double nodeCenterToSphereCenterDist;
while ( nodes.size() )
{
// Get the last element in the list
OctreeNode* node = nodes.back();
// Remove the last element from the list
nodes.pop_back();
// Get the distance between the node center and the sphere center
nodeCenterToSphereCenterDist = Vector::dist3(p, node->getCenter());
// Check if the current node is completely contained in the sphere.
// If yes -> add all its full children to 'out'.
if ( nodeCenterToSphereCenterDist + node->getVoxelRadius() <= radius )
this->pushBackFullLeafsFromNode(node, out);
// Check if the sphere intersects the current node
else if ( fabs(radius - nodeCenterToSphereCenterDist) <= node->getVoxelRadius() )
{
// We have an intersection -> push back the children of the current node
if ( node->hasChildren() )
for ( i = 0 ; i < 8 ; ++i )
nodes.push_back(node->getChild(i));
else if ( node->isFull() )
out.push_back(node);
}
}
}
void Octree::getFullLeafsNearPlane(const double* p, const double* n, double dist, list<OctreeNode*>& out)
{
list<OctreeNode*> nodes;
nodes.push_back(mRoot);
int i;
while ( nodes.size() )
{
// Get the last element in the list
OctreeNode* node = nodes.back();
// Remove the last element from the list
nodes.pop_back();
// Check if the sphere intersects the current node
if ( fabs(Vector::signedDistToPlane3(p, n, node->getCenter())) <= dist + node->getVoxelRadius() )
{
// We have an intersection -> push back the children of the current node
if ( node->hasChildren() )
{
for ( i = 0 ; i < 8 ; ++i )
nodes.push_back(node->getChild(i));
}
else if ( node->isFull() )
out.push_back(node);
}
}
}
void Octree::fillOctree()
{
double p[3];
const double *center;
int i, l, id, numOfPoints = mInputPoints->GetNumberOfPoints();
OctreeNode* node;
for ( i = 0 ; i < numOfPoints ; ++i )
{
// Some initialization
mInputPoints->GetPoint(i, p);
node = mRoot;
// Go down to the right leaf
for ( l = 0 ; l < mTreeLevels ; ++l )
{
node->createChildren(); // If this node already has children -> nothing will happen
center = node->getCenter();
id = 0;
if ( p[0] >= center[0] ) id |= 4;
if ( p[1] >= center[1] ) id |= 2;
if ( p[2] >= center[2] ) id |= 1;
node->fullDescendantsFlagsBitwiseOR((unsigned char)0x01 << id);
node = node->getChild(id);
}
this->fillOctreeLeaf(node, i, p);
}
}
void Octree::getFullLeafsInRange(double* p, double r1, double r2, list<OctreeNode*>& out)
{
list<OctreeNode*> nodes;
nodes.push_back(mRoot);
int i;
double d1, d2, dist;
while ( nodes.size() )
{
// Get the last element in the list
OctreeNode* node = nodes.back();
// Remove the last element in the list
nodes.pop_back();
#ifdef OCTREE_TEST_MODE
++mNodeRangeTestCounter;
#endif
// Get the distance between the sphere radius and the node center
dist = Vector::dist3(p, node->getCenter());
d1 = dist + node->getVoxelRadius();
d2 = dist - node->getVoxelRadius();
if ( d1 >= r1 && d2 <= r2 )
{
// Check if we have the special case that the voxel is completely between both spheres
if ( d1 <= r2 && d2 >= r1 )
this->pushBackFullLeafsFromNode(node, out);
else
{
if ( node->hasChildren() )
{
// Push back the children for further processing
for ( i = 0 ; i < 8 ; ++i )
nodes.push_back(node->getChild(i));
}
// We have reached a leaf -> save it in 'out' if it's full
else if ( node->isFull() )
out.push_back(node);
}
}
}
}
OctreeNode* Octree::getRandomFullLeafOnSphere(const double* p, double radius) const
{
list<OctreeNode*> nodes;
nodes.push_back(mRoot);
int i, rand_id;
vector<int> tmp_ids;
tmp_ids.reserve(8);
while ( nodes.size() )
{
// Get the last element in the list
OctreeNode* node = nodes.back();
// Remove the last element from the list
nodes.pop_back();
// Check if the sphere intersects the current node
if ( fabs(radius - Vector::dist3(p, node->getCenter())) <= node->getVoxelRadius() )
{
// We have an intersection -> push back the children of the current node
if ( node->hasChildren() )
{
// Prepare the tmp id vector
for ( i = 0 ; i < 8 ; ++i )
tmp_ids.push_back(i);
// Push back the children in random order
for ( i = 0 ; i < 8 ; ++i )
{
rand_id = mRandGen.getRandomInteger(0, tmp_ids.size()-1);
nodes.push_back(node->getChild(tmp_ids[rand_id]));
// Remove the randomly selected id
tmp_ids.erase(tmp_ids.begin() + rand_id);
}
}
else if ( node->isFull() )
return node;
}
}
return NULL;
}