void RayModel::computeBoundingTree(int maxDepth)
{
CubeModel* cubeModel = createPrevious<CubeModel>();
if (!isMoving() && !cubeModel->empty()) return; // No need to recompute BBox if immobile
Vector3 minElem, maxElem;
cubeModel->resize(size);
if (!empty())
{
for (int i=0; i<size; i++)
{
Ray r(this, i);
const Vector3& o = r.origin();
const Vector3& d = r.direction();
const SReal l = r.l();
for (int c=0; c<3; c++)
{
if (d[c]<0)
{
minElem[c] = o[c] + d[c]*l;
maxElem[c] = o[c];
}
else
{
minElem[c] = o[c];
maxElem[c] = o[c] + d[c]*l;
}
}
cubeModel->setParentOf(i, minElem, maxElem);
}
cubeModel->computeBoundingTree(maxDepth);
}
}
void CubeModel::computeBoundingTree(int maxDepth)
{
// if(maxDepth <= 0)
// return;
//sout << ">CubeModel::computeBoundingTree("<<maxDepth<<")"<<sendl;
std::list<CubeModel*> levels;
levels.push_front(createPrevious<CubeModel>());
for (int i=0; i<maxDepth; i++)
levels.push_front(levels.front()->createPrevious<CubeModel>());
CubeModel* root = levels.front();
//if (isStatic() && root->getPrevious() == NULL && !root->empty()) return; // No need to recompute BBox if immobile
if (root->empty() || root->getPrevious() != NULL)
{
// Tree must be reconstructed
//sout << "Building Tree with depth "<<maxDepth<<" from "<<size<<" elements."<<sendl;
// First remove extra levels
while(root->getPrevious()!=NULL)
{
core::CollisionModel::SPtr m = root->getPrevious();
root->setPrevious(m->getPrevious());
if (m->getMaster()) m->getMaster()->removeSlave(m);
//delete m;
m.reset();
}
// Then clear all existing levels
{
for (std::list<CubeModel*>::iterator it = levels.begin(); it != levels.end(); ++it)
(*it)->resize(0);
}
// Then build root cell
//sout << "CubeModel: add root cube"<<sendl;
root->addCube(Cube(this,0),Cube(this,size));
// Construct tree by splitting cells along their biggest dimension
std::list<CubeModel*>::iterator it = levels.begin();
CubeModel* level = *it;
++it;
int lvl = 0;
while(it != levels.end())
{
//sout << "CubeModel: split level "<<lvl<<sendl;
CubeModel* clevel = *it;
clevel->elems.reserve(level->size*2);
for(Cube cell = Cube(level->begin()); level->end() != cell; ++cell)
{
const std::pair<Cube,Cube>& subcells = cell.subcells();
int ncells = subcells.second.getIndex() - subcells.first.getIndex();
//sout << "CubeModel: level "<<lvl<<" cell "<<cell.getIndex()<<": current subcells "<<subcells.first.getIndex() << " - "<<subcells.second.getIndex()<<sendl;
if (ncells > 4)
{
// Only split cells with more than 4 childs
// Find the biggest dimension
int splitAxis;
Vector3 l = cell.maxVect()-cell.minVect();
int middle = subcells.first.getIndex()+(ncells+1)/2;
if(l[0]>l[1])
if (l[0]>l[2])
splitAxis = 0;
else
splitAxis = 2;
else if (l[1]>l[2])
splitAxis = 1;
else
splitAxis = 2;
// Separate cells on each side of the median cell
#if defined(__GNUC__) && (__GNUC__ == 4)
// && (__GNUC_MINOR__ == 1) && (__GNUC_PATCHLEVEL__ == 1)
// there is apparently a bug in std::sort with GCC 4.x
if (splitAxis == 0)
qsort(&(elems[subcells.first.getIndex()]), subcells.second.getIndex()-subcells.first.getIndex(), sizeof(elems[0]), CubeSortPredicate::sortCube<0>);
else if (splitAxis == 1)
qsort(&(elems[subcells.first.getIndex()]), subcells.second.getIndex()-subcells.first.getIndex(), sizeof(elems[0]), CubeSortPredicate::sortCube<1>);
else
qsort(&(elems[subcells.first.getIndex()]), subcells.second.getIndex()-subcells.first.getIndex(), sizeof(elems[0]), CubeSortPredicate::sortCube<2>);
#else
CubeSortPredicate sortpred(splitAxis);
//std::nth_element(elems.begin()+subcells.first.getIndex(),elems.begin()+middle,elems.begin()+subcells.second.getIndex(), sortpred);
std::sort(elems.begin()+subcells.first.getIndex(),elems.begin()+subcells.second.getIndex(), sortpred);
#endif
// Create the two new subcells
Cube cmiddle(this, middle);
int c1 = clevel->addCube(subcells.first, cmiddle);
int c2 = clevel->addCube(cmiddle, subcells.second);
//sout << "L"<<lvl<<" cell "<<cell.getIndex()<<" split along "<<(splitAxis==0?'X':splitAxis==1?'Y':'Z')<<" in cell "<<c1<<" size "<<middle-subcells.first.getIndex()<<" and cell "<<c2<<" size "<<subcells.second.getIndex()-middle<<"."<<sendl;
//level->elems[cell.getIndex()].subcells = std::make_pair(Cube(clevel,c1),Cube(clevel,c2+1));
level->elems[cell.getIndex()].subcells.first = Cube(clevel,c1);
level->elems[cell.getIndex()].subcells.second = Cube(clevel,c2+1);
}
}
++it;
level = clevel;
++lvl;
}
if (!parentOf.empty())
{
// Finally update parentOf to reflect new cell order
for (int i=0; i<size; i++)
parentOf[elems[i].children.first.getIndex()] = i;
}
}
else
{
// Simply update the existing tree, starting from the bottom
int lvl = 0;
for (std::list<CubeModel*>::reverse_iterator it = levels.rbegin(); it != levels.rend(); ++it)
{
//sout << "CubeModel: update level "<<lvl<<sendl;
(*it)->updateCubes();
++lvl;
}
}
//sout << "<CubeModel::computeBoundingTree("<<maxDepth<<")"<<sendl;
}
void TriangleOctreeModel::computeBoundingTree(int maxDepth)
{
const helper::vector<topology::Triangle>& tri = *triangles;
if(octreeRoot)
{
delete octreeRoot;
octreeRoot=NULL;
}
CubeModel* cubeModel = createPrevious<CubeModel>();
updateFromTopology();
if (!isMoving() && !cubeModel->empty()) return; // No need to recompute BBox if immobile
int size2=mstate->getSize();
pNorms.resize(size2);
for(int i=0; i<size2; i++)
{
pNorms[i]=defaulttype::Vector3(0,0,0);
}
defaulttype::Vector3 minElem, maxElem;
maxElem[0]=minElem[0]=mstate->read(core::ConstVecCoordId::position())->getValue()[0][0];
maxElem[1]=minElem[1]=mstate->read(core::ConstVecCoordId::position())->getValue()[0][1];
maxElem[2]=minElem[2]=mstate->read(core::ConstVecCoordId::position())->getValue()[0][2];
cubeModel->resize(1); // size = number of triangles
for (int i=1; i<size; i++)
{
Triangle t(this,i);
pNorms[tri[i][0]]+=t.n();
pNorms[tri[i][1]]+=t.n();
pNorms[tri[i][2]]+=t.n();
const defaulttype::Vector3* pt[3];
pt[0] = &t.p1();
pt[1] = &t.p2();
pt[2] = &t.p3();
t.n() = cross(*pt[1]-*pt[0],*pt[2]-*pt[0]);
t.n().normalize();
for (int p=0; p<3; p++)
{
for(int c=0; c<3; c++)
{
if ((*pt[p])[c] > maxElem[c]) maxElem[c] = (*pt[p])[c];
if ((*pt[p])[c] < minElem[c]) minElem[c] = (*pt[p])[c];
}
}
}
cubeModel->setParentOf(0, minElem, maxElem); // define the bounding box of the current triangle
cubeModel->computeBoundingTree(maxDepth);
for(int i=0; i<size2; i++)
{
pNorms[i].normalize();
}
#if 0
if(!pTri.size())
{
/*creates the list of triangles that are associated to a point*/
pTri.resize(size2);
for(int i=0; i<size; i++)
{
pTri[tri[i][0]].push_back(i);
pTri[tri[i][1]].push_back(i);
pTri[tri[i][2]].push_back(i);
}
}
#endif
}
void TetrahedronModel::computeBoundingTree(int maxDepth)
{
CubeModel* cubeModel = createPrevious<CubeModel>();
if (!mstate || !_topology) return;
if (!isMoving() && !cubeModel->empty()) return; // No need to recompute BBox if immobile
Vector3 minElem, maxElem;
const VecCoord& x = this->mstate->read(core::ConstVecCoordId::position())->getValue();
for (int i=0; i<size; i++)
{
Tetrahedron t(this,i);
const Vector3& pt1 = x[t.p1Index()];
const Vector3& pt2 = x[t.p2Index()];
const Vector3& pt3 = x[t.p3Index()];
const Vector3& pt4 = x[t.p4Index()];
Matrix3 m, minv;
m[0] = pt2-pt1;
m[1] = pt3-pt1;
m[2] = pt4-pt1;
m.transpose();
minv.invert(m);
elems[i].coord0 = pt1;
elems[i].bary2coord = m;
elems[i].coord2bary = minv;
}
if (maxDepth == 0)
{
// no hierarchy
if (empty())
cubeModel->resize(0);
else
{
cubeModel->resize(1);
minElem = x[0];
maxElem = x[0];
for (unsigned i=1; i<x.size(); i++)
{
const Vector3& pt1 = x[i];
if (pt1[0] > maxElem[0]) maxElem[0] = pt1[0];
else if (pt1[0] < minElem[0]) minElem[0] = pt1[0];
if (pt1[1] > maxElem[1]) maxElem[1] = pt1[1];
else if (pt1[1] < minElem[1]) minElem[1] = pt1[1];
if (pt1[2] > maxElem[2]) maxElem[2] = pt1[2];
else if (pt1[2] < minElem[2]) minElem[2] = pt1[2];
}
cubeModel->setLeafCube(0, std::make_pair(this->begin(),this->end()), minElem, maxElem); // define the bounding box of the current Tetrahedron
}
}
else
{
cubeModel->resize(size); // size = number of Tetrahedrons
if (!empty())
{
for (int i=0; i<size; i++)
{
Tetrahedron t(this,i);
const Vector3& pt1 = x[t.p1Index()];
const Vector3& pt2 = x[t.p2Index()];
const Vector3& pt3 = x[t.p3Index()];
const Vector3& pt4 = x[t.p4Index()];
for (int c = 0; c < 3; c++)
{
minElem[c] = pt1[c];
maxElem[c] = pt1[c];
if (pt2[c] > maxElem[c]) maxElem[c] = pt2[c];
else if (pt2[c] < minElem[c]) minElem[c] = pt2[c];
if (pt3[c] > maxElem[c]) maxElem[c] = pt3[c];
else if (pt3[c] < minElem[c]) minElem[c] = pt3[c];
if (pt4[c] > maxElem[c]) maxElem[c] = pt4[c];
else if (pt4[c] < minElem[c]) minElem[c] = pt4[c];
}
cubeModel->setParentOf(i, minElem, maxElem); // define the bounding box of the current Tetrahedron
}
cubeModel->computeBoundingTree(maxDepth);
}
}
}