void
Octree::_place (nau::scene::SceneObject *aSceneObject)
{
static std::list<nau::scene::OctreeNode*> tmpVector;
static int octant = 0;
OctreeNode* aNode = reinterpret_cast<OctreeNode*> (aSceneObject);
if (0 == m_pOctreeRootNode) {
m_pOctreeRootNode = aNode;
tmpVector.push_back (m_pOctreeRootNode);
return;
}
if (aNode->m_NodeId < octant) {
tmpVector.pop_front();
}
OctreeNode* currentNode = *(tmpVector.begin());
octant = aNode->m_NodeId;
currentNode->_setChild(octant, aNode);
if (aNode->_getChildCount() > 0) {
tmpVector.push_back (aNode);
}
}
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);
}
}
//virtual
void LLViewerOctreeGroup::unbound()
{
if (isDirty())
{
return;
}
setState(DIRTY);
//all the parent nodes need to rebound this child
if (mOctreeNode)
{
OctreeNode* parent = (OctreeNode*) mOctreeNode->getParent();
while (parent != NULL)
{
LLViewerOctreeGroup* group = (LLViewerOctreeGroup*) parent->getListener(0);
if (!group || group->isDirty())
{
return;
}
group->setState(DIRTY);
parent = (OctreeNode*) parent->getParent();
}
}
}
// Returns the number of collision tests with the octree.
unsigned int OctreeNode::CollisionTest(const SceneObject& sceneObject)
{
if(!ObjectIsInside(sceneObject))
return 0;
bool leaf= true;
unsigned int collisionTestsCounter = 0;
// Find the leaf wich contained the sceneObject.
OctreeNode* currentNode = this->FindLeaf(sceneObject);
if(currentNode == NULL)
return collisionTestsCounter;
// For each contained object : increase the collisionTestsCounter and set the green color (collision test are execute on green spheres).
for(unsigned int i = 0; i < currentNode->GetContainedObjectsSize(); ++i)
{
collisionTestsCounter++;
currentNode->GetContainedObject(i)->SetColor(ColorRGB::PureGreen);
}
currentNode->SetColor(ColorRGB::PureRed);
return collisionTestsCounter;
}
void Octree::CollectNodes(Vector<Pair<OctreeNode*, float> >& result, const Octant* octant, const Ray& ray, unsigned short nodeFlags,
float maxDistance, unsigned layerMask) const
{
float octantDist = ray.HitDistance(octant->cullingBox);
if (octantDist >= maxDistance)
return;
const Vector<OctreeNode*>& octantNodes = octant->nodes;
for (auto it = octantNodes.Begin(); it != octantNodes.End(); ++it)
{
OctreeNode* node = *it;
if ((node->Flags() & nodeFlags) == nodeFlags && (node->LayerMask() & layerMask))
{
float distance = ray.HitDistance(node->WorldBoundingBox());
if (distance < maxDistance)
result.Push(MakePair(node, distance));
}
}
for (size_t i = 0; i < NUM_OCTANTS; ++i)
{
if (octant->children[i])
CollectNodes(result, octant->children[i], ray, nodeFlags, maxDistance, layerMask);
}
}
void Octree::DeleteChildOctants(Octant* octant, bool deletingOctree)
{
for (auto it = octant->nodes.Begin(); it != octant->nodes.End(); ++it)
{
OctreeNode* node = *it;
node->octant = nullptr;
node->SetFlag(NF_OCTREE_UPDATE_QUEUED, false);
if (deletingOctree)
node->octree = nullptr;
}
octant->nodes.Clear();
octant->numNodes = 0;
for (size_t i = 0; i < NUM_OCTANTS; ++i)
{
if (octant->children[i])
{
DeleteChildOctants(octant->children[i], deletingOctree);
octant->children[i] = nullptr;
}
}
if (octant != &root)
allocator.Free(octant);
}
void LightManager::CreateOctreeNode(Light* pLight)
{
ASSERT_PREDICATE_RETURN(pLight->GetUserObjectBindings().HasKey(m_StrOctreeNodeKey) == false);
OctreeNode<float64>* pOctreeNode = new OctreeNode_64();
pLight->GetUserObjectBindings().SetUserAny(m_StrOctreeNodeKey, pOctreeNode);
// Store a back pointer
pOctreeNode->GetUserObjectBindings().SetUserAny(m_StrLightKey, pLight);
}
template <typename PointT, typename NormalT> bool
cpu_tsdf::TSDFVolumeOctree::integrateCloud (
const pcl::PointCloud<PointT> &cloud,
const pcl::PointCloud<NormalT> &normals,
const Eigen::Affine3d &trans)
{
Eigen::Affine3f trans_inv = trans.inverse ().cast<float> ();
// First, sample a few points and force their containing voxels to split
int px_step = 1;
int nsplit = 0;
for (size_t u = 0; u < cloud.width; u += px_step)
{
for (size_t v = 0; v < cloud.height; v += px_step)
{
const PointT &pt_surface_orig = cloud (u, v);
if (pcl_isnan (pt_surface_orig.z))
continue;
// Look at surroundings
int nstep = 0;
Eigen::Vector3f ray = pt_surface_orig.getVector3fMap ().normalized ();
for (int perm = 0; perm < num_random_splits_; perm++)
{
// Get containing voxels
PointT pt_trans;
float scale = (float)rand () / (float)RAND_MAX * 0.03;
Eigen::Vector3f noise = Eigen::Vector3f::Random ().normalized () * scale;;
if (perm == 0) noise *= 0;
pt_trans.getVector3fMap () = trans.cast<float> () * (pt_surface_orig.getVector3fMap ()+ noise);
OctreeNode* voxel = octree_->getContainingVoxel (pt_trans.x, pt_trans.y, pt_trans.z);
if (voxel != NULL)
{
while (voxel->getMinSize () > xsize_ / xres_)
{
nsplit++;
voxel->split ();
voxel = voxel->getContainingVoxel (pt_trans.x, pt_trans.y, pt_trans.z);
}
}
}
}
}
// Do Frustum Culling to get rid of unseen voxels
std::vector<cpu_tsdf::OctreeNode::Ptr> voxels_culled;
getFrustumCulledVoxels(trans, voxels_culled);
#pragma omp parallel for
for (size_t i = 0; i < voxels_culled.size (); i++)
{
updateVoxel (voxels_culled[i], cloud, normals, trans_inv);
}
// Cloud is no longer empty
is_empty_ = false;
return (true);
}
//==============================================================================
void Octree::placeSceneNode(SceneNode* sn)
{
SpatialComponent* sp = sn->getSpatial();
ANKI_ASSERT(sp != nullptr);
OctreeNode* toBePlacedNode = place(sp->getAabb());
if(toBePlacedNode != nullptr)
{
toBePlacedNode->addSceneNode(sn);
}
}
OctreeNode * OctreeScene::_InsideChild(OctreeNode * node, const Aabb & bound)
{
const Aabb & ndbound = node->GetBound();
Vec3 _half = ndbound.GetHalfSize();
Vec3 _min = bound.minimum - ndbound.minimum;
Vec3 _max = bound.maximum - ndbound.minimum;
int ix0 = 0, iy0 = 0, iz0 = 0;
int ix1 = 0, iy1 = 0, iz1 = 0;
Aabb clbound(ndbound.minimum, ndbound.minimum);
if (_min.x >= _half.x)
{
ix0 = 1;
clbound.minimum.x += _half.x;
}
if (_min.y >= _half.y)
{
iy0 = 1;
clbound.minimum.y += _half.y;
}
if (_min.z >= _half.z)
{
iz0 = 1;
clbound.minimum.z += _half.z;
}
if (_max.x >= _half.x)
ix1 = 1;
if (_max.y >= _half.y)
iy1 = 1;
if (_max.z >= _half.z)
iz1 = 1;
clbound.maximum = clbound.minimum + _half;
if (ix0 != ix1 || iy0 != iy1 || iz0 != iz1)
return NULL;
d_assert (Math::AABBInside(clbound, bound));
int index = _MAKE_POSITION(ix0, iy1, iz1);
OctreeNode * child = node->_GetChild(index);
if (child == NULL)
{
child = AllocOctNode();
child->SetBound(clbound);
node->_SetChild(index, child);
}
return child;
}
void Octree::Update()
{
PROFILE(UpdateOctree);
for (auto it = updateQueue.Begin(); it != updateQueue.End(); ++it)
{
OctreeNode* node = *it;
// If node was removed before update could happen, a null pointer will be in its place
if (node)
{
node->SetFlag(NF_OCTREE_UPDATE_QUEUED, false);
// Do nothing if still fits the current octant
const BoundingBox& box = node->WorldBoundingBox();
Vector3 boxSize = box.Size();
Octant* oldOctant = node->octant;
if (oldOctant && oldOctant->cullingBox.IsInside(box) == INSIDE && oldOctant->FitBoundingBox(box, boxSize))
continue;
// Begin reinsert process. Start from root and check what level child needs to be used
Octant* newOctant = &root;
Vector3 boxCenter = box.Center();
for (;;)
{
bool insertHere;
// If node does not fit fully inside root octant, must remain in it
if (newOctant == &root)
insertHere = newOctant->cullingBox.IsInside(box) != INSIDE || newOctant->FitBoundingBox(box, boxSize);
else
insertHere = newOctant->FitBoundingBox(box, boxSize);
if (insertHere)
{
if (newOctant != oldOctant)
{
// Add first, then remove, because node count going to zero deletes the octree branch in question
AddNode(node, newOctant);
if (oldOctant)
RemoveNode(node, oldOctant);
}
break;
}
else
newOctant = CreateChildOctant(newOctant, newOctant->ChildIndex(boxCenter));
}
}
}
updateQueue.Clear();
}
SceneNode * OctreeSceneManager::createSceneNode( const String &name )
{
// Check name not used
if (mSceneNodes.find(name) != mSceneNodes.end())
{
OGRE_EXCEPT(
Exception::ERR_DUPLICATE_ITEM,
"A scene node with the name " + name + " already exists",
"OctreeSceneManager::createSceneNode" );
}
OctreeNode * on = new OctreeNode( this, name );
mSceneNodes[ on->getName() ] = on;
return on;
}
void OctreeScene::RemoveSceneNode(SceneNode * node)
{
OctreeNode::_dirt dirt = *(OctreeNode::_dirt*)node->_GetSceneDirtData();
dirt.node->RemoveNode(node);
OctreeNode * eraseNode = dirt.node;
if (eraseNode->_IsNil())
{
OctreeNode * parent = dirt.node->_GetParent();
parent->_NotifyErase(eraseNode);
FreeOctNode(eraseNode);
eraseNode = parent;
}
}
void OctreeNode::removeAllChildren()
{
ChildNodeMap::iterator i, iend;
iend = mChildren.end();
for (i = mChildren.begin(); i != iend; ++i)
{
OctreeNode* on = static_cast<OctreeNode*>(i->second);
on->setParent(0);
on->_removeNodeAndChildren();
}
mChildren.clear();
mChildrenToUpdate.clear();
}
// Returns the leaf wich contained the sceneObject.
// Returns NULL if the sceneObject parameter is not contained in the node.
OctreeNode* OctreeNode::FindLeaf(const SceneObject& sceneObject)
{
if(!this->ObjectIsInside(sceneObject))
return NULL;
OctreeNode* currentNode = this;
while(currentNode != NULL && !currentNode->IsALeaf())
{
currentNode = currentNode->FindNextNode(sceneObject);
}
return currentNode;
}
void Octree::CollectNodes(Vector<OctreeNode*>& result, const Octant* octant, unsigned short nodeFlags, unsigned layerMask) const
{
const Vector<OctreeNode*>& octantNodes = octant->nodes;
for (auto it = octantNodes.Begin(); it != octantNodes.End(); ++it)
{
OctreeNode* node = *it;
if ((node->Flags() & nodeFlags) == nodeFlags && (node->LayerMask() & layerMask))
result.Push(node);
}
for (size_t i = 0; i < NUM_OCTANTS; ++i)
{
if (octant->children[i])
CollectNodes(result, octant->children[i], nodeFlags, layerMask);
}
}
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);
}
}
}
// Generates the octree.
// Divides the node into 8 children while the contained object is superior to the maxElementPerNode parameter.
void OctreeNode::GenerateOctree(vector<SceneObject*>& sceneObjectsList, unsigned int maxElementPerNode)
{
stack<OctreeNode*> recursivityStack;
recursivityStack.push(this);
while(!recursivityStack.empty())
{
OctreeNode* currentNode = recursivityStack.top();
recursivityStack.pop();
// Add the sceneObjects which are inside the node to the contained list.
for(vector<SceneObject*>::iterator it = sceneObjectsList.begin(); it != sceneObjectsList.end(); ++it)
{
if(currentNode->ObjectIsInside((**it)))
{
currentNode->m_containedObjects.push_back(&(**it));
}
}
if(currentNode->m_containedObjects.size() > maxElementPerNode)
{
//Sons creation.
float newSize = abs((currentNode->GetAABBMax().X() - currentNode->GetAABBMin().X()) / 2);
currentNode->m_children[0] = new OctreeNode(Vector3(currentNode->GetAABBMin().X(), currentNode->GetAABBMin().Y(), currentNode->GetAABBMin().Z()), Vector3(currentNode->GetAABBMax().X() - newSize, currentNode->GetAABBMax().Y() - newSize, currentNode->GetAABBMax().Z() - newSize), ColorRGB::PureBlue);
currentNode->m_children[1] = new OctreeNode(Vector3(currentNode->GetAABBMin().X() + newSize, currentNode->GetAABBMin().Y(), currentNode->GetAABBMin().Z()), Vector3(currentNode->GetAABBMax().X(), currentNode->GetAABBMax().Y() - newSize, currentNode->GetAABBMax().Z() - newSize), ColorRGB::PureBlue);
currentNode->m_children[2] = new OctreeNode(Vector3(currentNode->GetAABBMin().X(), currentNode->GetAABBMin().Y() + newSize, currentNode->GetAABBMin().Z()), Vector3(currentNode->GetAABBMax().X() - newSize, currentNode->GetAABBMax().Y(), currentNode->GetAABBMax().Z() - newSize), ColorRGB::PureBlue);
currentNode->m_children[3] = new OctreeNode(Vector3(currentNode->GetAABBMin().X() + newSize, currentNode->GetAABBMin().Y() + newSize, currentNode->GetAABBMin().Z()), Vector3(currentNode->GetAABBMax().X(), currentNode->GetAABBMax().Y(), currentNode->GetAABBMax().Z() - newSize), ColorRGB::PureBlue);
currentNode->m_children[4] = new OctreeNode(Vector3(currentNode->GetAABBMin().X(), currentNode->GetAABBMin().Y(), currentNode->GetAABBMin().Z() + newSize), Vector3(currentNode->GetAABBMax().X() - newSize, currentNode->GetAABBMax().Y() - newSize, currentNode->GetAABBMax().Z()), ColorRGB::PureBlue);
currentNode->m_children[5] = new OctreeNode(Vector3(currentNode->GetAABBMin().X() + newSize, currentNode->GetAABBMin().Y(), currentNode->GetAABBMin().Z() + newSize), Vector3(currentNode->GetAABBMax().X(), currentNode->GetAABBMax().Y() - newSize, currentNode->GetAABBMax().Z()), ColorRGB::PureBlue);
currentNode->m_children[6] = new OctreeNode(Vector3(currentNode->GetAABBMin().X(), currentNode->GetAABBMin().Y() + newSize, currentNode->GetAABBMin().Z() + newSize), Vector3(currentNode->GetAABBMax().X() - newSize, currentNode->GetAABBMax().Y(), currentNode->GetAABBMax().Z()), ColorRGB::PureBlue);
currentNode->m_children[7] = new OctreeNode(Vector3(currentNode->GetAABBMin().X() + newSize, currentNode->GetAABBMin().Y() + newSize, currentNode->GetAABBMin().Z() + newSize), currentNode->GetAABBMax(), ColorRGB::PureBlue);
}
for(unsigned int i = 0; i < 8; i++)
{
if(currentNode->GetChild(i) != NULL)
recursivityStack.push(currentNode->GetChild(i));
}
}
}
OctreeNode*
sps_real(CBface_list& input_faces,
CBBOX& box,
int height,
double regularity,
double min_dist,
Bface_list& flist,
ARRAY<Wvec>& blist,
double& spacing
)
{
clock_t start, end;
double duration;
flist.clear();
blist.clear();
OctreeNode* root = new OctreeNode(box.min(), box.max(), 1, NULL);
if (height == 1) {
root->set_leaf(true);
root->set_disp(true);
}
root->intersects() = input_faces;
start = clock();
root->build_octree(height);
end = clock();
duration = (double)(end - start) / CLOCKS_PER_SEC;
err_adv(debug, "step 1 time: %f", duration);
start = clock();
visit(root, regularity, flist, blist);
end = clock();
duration = (double)(end - start) / CLOCKS_PER_SEC;
err_adv(debug, "step 2 time: %f", duration);
// remove bad samples
start = clock();
double dist = min_dist * box.dim().length() / (1<<(height-1));
spacing = dist;
root->set_neibors();
root->set_terms();
remove_nodes(flist, blist, dist, root->terms());
end = clock();
duration = (double)(end - start) / CLOCKS_PER_SEC;
err_adv(debug, "step 3 time: %f", duration);
err_adv(debug, "no of points: %d", flist.num());
return root;
}
void Octree::CollectNodes(Vector<RaycastResult>& result, const Octant* octant, const Ray& ray, unsigned short nodeFlags,
float maxDistance, unsigned layerMask) const
{
float octantDist = ray.HitDistance(octant->cullingBox);
if (octantDist >= maxDistance)
return;
const Vector<OctreeNode*>& octantNodes = octant->nodes;
for (auto it = octantNodes.Begin(); it != octantNodes.End(); ++it)
{
OctreeNode* node = *it;
if ((node->Flags() & nodeFlags) == nodeFlags && (node->LayerMask() & layerMask))
node->OnRaycast(result, ray, maxDistance);
}
for (size_t i = 0; i < NUM_OCTANTS; ++i)
{
if (octant->children[i])
CollectNodes(result, octant->children[i], ray, nodeFlags, maxDistance, layerMask);
}
}
LLViewerOctreeGroup* LLViewerOctreeGroup::getParent()
{
if (isDead())
{
return NULL;
}
if(!mOctreeNode)
{
return NULL;
}
OctreeNode* parent = mOctreeNode->getOctParent();
if (parent)
{
return (LLViewerOctreeGroup*) parent->getListener(0);
}
return NULL;
}
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::buildNeighbourhoodStructure()
{
if ( !mRoot )
return;
int i;
OctreeNode* node;
list<OctreeNode*> nodes;
nodes.push_back(mRoot);
while ( nodes.size() )
{
node = nodes.back();
nodes.pop_back();
if ( node->hasChildren() )
for ( i = 0 ; i < 8 ; ++i )
nodes.push_back(node->getChild(i));
else if ( node->hasData() )
this->collectFullNeighbours(node);
}
}
void OctreeScene::AddSceneNode(SceneNode * node)
{
const Aabb & bound = node->GetWorldAabb();
// check in this scene
Scene * scene = node->_GetScene();
if (scene == this)
return;
else if (scene)
scene->RemoveSceneNode(node);
// if not in root, force in root.
if (!Math::AABBInside(m_root->GetBound(), bound))
{
m_root->AddNode(node);
}
else
{
OctreeNode * ocnode = _InsideNode(m_root, 0, bound);
ocnode->AddNode(node);
}
}
OctreeNode* Octree::getRandomFullLeaf()
{
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();
// 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;
}
void
OctreeNode::build_octree(int height)
{
if (_leaf || _height == height)
return;
int i, j;
Wpt_list pts;
points(pts);
for (i = 0; i < 8; i++) {
_children[i] = new OctreeNode((pts[0]+pts[i])/2, (pts[i]+pts[7])/2,
_height+1, this);
}
for (j = 0; j < _intersects.num(); j++) {
Bface* f = _intersects[j];
for (i = 0; i < 8; i++) {
OctreeNode* n = _children[i];
if (n->contains(f->v1()->loc()) && n->contains(f->v2()->loc())
&& n->contains(f->v3()->loc())) {
n->intersects() += f;
break;
} else if (n->overlaps(bface_bbox(f))) {
n->intersects() += f;
}
}
}
for (i = 0; i < 8; i++) {
if (_height+1 == height) {
_children[i]->set_leaf(true);
_children[i]->set_disp(true);
}
if (_children[i]->intersects().empty()) {
_children[i]->set_leaf(true);
_children[i]->set_disp(false);
}
_children[i]->build_octree(height);
}
}
void
OctreeNode::set_neibors()
{
OctreeNode* n;
BBOX test_box = BBOX(min()-0.5*dim(), max()+0.5*dim());
if ((!_leaf || _display) && _height != 1) {
for (int i = 0; i < 8; i++) {
n = _parent->get_children()[i];
if (n != this && (!n->get_leaf() || n->get_disp())) {
_neibors += n;
}
}
_parent->neibors().num();
for (int i = 0; i < _parent->neibors().num(); i++) {
n = _parent->neibors()[i];
if (!n->get_leaf()) {
for (int j = 0; j < 8; j++) {
if (n->get_children()[j]->overlaps(test_box) &&
(!n->get_children()[j]->get_leaf() ||
n->get_children()[j]->get_disp())) {
//XXX - crashing here...
_neibors += n->get_children()[j];
}
}
}
}
}
if (!_leaf) {
for (int i = 0; i < 8; i++) {
_children[i]->set_neibors();
}
}
}
void OctreeNode::updateObjectPosition(Object3D* object){
OctreeNode* actualNode = object->getOctreeNode();
actualNode->objects.remove(object);
if(actualNode->objectsInBranch()==0){
actualNode->clearChildren();
}
if (actualNode->objectFits(object)){
actualNode->addObject(object);
return;
}
OctreeNode* parent =actualNode->getParent();
if(parent == NULL){
actualNode->objects.push_back(object);
return;
}
object->setOctreeNode(parent);
updateObjectPosition(object);
}
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;
}