/cygdrive/d/src/svn/vrut/trunk/modules/collision_detection/CollisionDetection_BVH.cpp

Go to the documentation of this file.

#include "CollisionDetection_BVH.h"

using namespace VRUT;
using namespace CollisionDetectionNamespace;


CollisionDetection_BVH::~CollisionDetection_BVH(void)
{
       //SAFE_DELETE(root);
}


struct QNode
{
       BVHNode ** currentNode;
       std::deque<BVHNode *> * itemsPending;
       BVHNode * parent;

       QNode(BVHNode ** n,
              std::deque<BVHNode *> * m,
              BVHNode * p) : currentNode(n), itemsPending(m), parent(p)      {}
};


void CollisionDetection_BVH::UpdateBVH(CollisionDetection_BVHNode * node)
{
       if (!node)
              node = root;


       if (node->GetTargetOrigTriangles()->empty())
       {
              if (!node->GetLChild() && node->GetRChild())
              {
                     UpdateBVH(node->GetRChild());
                     node->aabb = node->GetRChild()->aabb;
              }
              else if (!node->GetRChild() && node->GetLChild())
              {
                     UpdateBVH(node->GetLChild());
                     node->aabb = node->GetLChild()->aabb;
              }
              else if (node->GetLChild() && node->GetRChild())
              {
                     UpdateBVH(node->GetLChild());
                     UpdateBVH(node->GetRChild());
                     node->aabb = node->GetLChild()->aabb + node->GetRChild()->aabb;
              }
       }
       else
       {

              node->aabb.MinBound = VECTOR3(MAX_FLOAT,MAX_FLOAT,MAX_FLOAT);
              node->aabb.MaxBound = VECTOR3(MIN_FLOAT,MIN_FLOAT,MIN_FLOAT);

              node->targetTriangles.clear();
              for (int i = 0; i < (int)node->targetTrianglesOrig.size(); i++)
              {
                     node->targetTriangles.push_back(Triangle());
                     node->targetTriangles[i].v1 = CurrMatrix.TransformCoord(node->targetTrianglesOrig[i].v1);
                     node->targetTriangles[i].v2 = CurrMatrix.TransformCoord(node->targetTrianglesOrig[i].v2);
                     node->targetTriangles[i].v3 = CurrMatrix.TransformCoord(node->targetTrianglesOrig[i].v3);
                     calculateNode(node,node->targetTriangles[i]);
              }
       }

       node->bSphere.Center = (node->aabb.MinBound + node->aabb.MaxBound) * 0.5f;
       node->bSphere.Radius = (node->aabb.MaxBound - node->aabb.MinBound).Length() * 0.5f;
       node->valid = true;
}

struct MyQNode
{
       CollisionDetection_BVHNode * currentNode;
       std::vector<Triangle>* itemsPending;
       //CollisionDetection_BVHNode * parent;

       MyQNode(CollisionDetection_BVHNode * n,
              std::vector<Triangle> * m) : currentNode(n), itemsPending(m)   {}
};

void CollisionDetection_BVH::Build(std::vector<Triangle> * items)
{

       //clock_t t,t1,t2,t3;

       std::vector<Triangle> * itemsPending = new std::vector<Triangle>();
       for (int i = 0; i < (int)items->size();i++)
       {
              itemsPending->push_back((*items)[i]);
       }


       std::deque<MyQNode> queue;
       root = new CollisionDetection_BVHNode();

       MyQNode node(root, itemsPending);
       queue.push_back(node);

       int index = 0;
       while (!queue.empty())
       {
              //t=clock();

              MyQNode qnode = queue.front();
              queue.pop_front();

              //if (qnode.itemsPending->empty())
              //{
              //     delete qnode.itemsPending;
              //     continue;
              //}

              if (qnode.itemsPending->size() <= TRIANGLES_IN_LEAF)
              {
                     
                     qnode.currentNode->aabb.MinBound = VECTOR3(MAX_FLOAT,MAX_FLOAT,MAX_FLOAT);
                     qnode.currentNode->aabb.MaxBound = VECTOR3(MIN_FLOAT,MIN_FLOAT,MIN_FLOAT);


                     int i = 0;
                     std::vector<Triangle>::iterator it = qnode.itemsPending->begin();
                     while (i < TRIANGLES_IN_LEAF && it != qnode.itemsPending->end())
                     {

                            Triangle triangle1 = (*it++);
                            //triangle1.v1 = transfMatrix->TransformCoord(triangle1.v1);
                            //triangle1.v2 = transfMatrix->TransformCoord(triangle1.v2);
                            //triangle1.v3 = transfMatrix->TransformCoord(triangle1.v3);

                            calculateNode(qnode.currentNode,triangle1);
                            
                            qnode.currentNode->GetTargetOrigTriangles()->push_back(triangle1);

                            i++;
                     }

                     //*(qnode.currentNode) = qnode.itemsPending->front();
                     //(*(qnode.currentNode))->parent = qnode.parent;
                     if (qnode.itemsPending)
                     {
                            qnode.itemsPending->clear();
                            delete qnode.itemsPending;
                     }

                     continue;
              }

                     

              std::vector<Triangle > * geometryL = new std::vector<Triangle>;
              std::vector<Triangle > * geometryR = new std::vector<Triangle>;
              

              //t1=clock();

              //double d1 = double(t1-t);



              splitGeometry(qnode.currentNode, qnode.itemsPending, geometryL, geometryR);
              
              //t2=clock();

              //double d2 = double(t2-t1);
              
              //This should be there!!!!!!
              if (qnode.itemsPending)
              {
                     qnode.itemsPending->clear();
                     delete qnode.itemsPending;

              }

              if (!geometryL->empty())
              {
                     qnode.currentNode->CreateLChild();
                     queue.push_back(MyQNode(qnode.currentNode->GetLChild(), geometryL));
              }

              if (!geometryR->empty())
              {
                     qnode.currentNode->CreateRChild();
                     queue.push_back(MyQNode(qnode.currentNode->GetRChild(), geometryR));
              }

              //t3=clock();

              //double d3 = double(t3-t1);

              //index ++;
              //int kk = 7;
       }

       //int ahhh = 9;


}


void CollisionDetection_BVH::calculateNode(CollisionDetection_BVHNode* node,Triangle triangle)
{
       node->GetAABB()->MaxBound.x = std::max(node->aabb.MaxBound.x, std::max(triangle.v1.x,triangle.v2.x));
       node->GetAABB()->MaxBound.x = std::max(node->aabb.MaxBound.x, std::max(triangle.v2.x,triangle.v3.x));

       node->GetAABB()->MaxBound.y = std::max(node->aabb.MaxBound.y, std::max(triangle.v1.y,triangle.v2.y));
       node->GetAABB()->MaxBound.y = std::max(node->aabb.MaxBound.y, std::max(triangle.v2.y,triangle.v3.y));

       node->GetAABB()->MaxBound.z = std::max(node->aabb.MaxBound.z, std::max(triangle.v1.z,triangle.v2.z));
       node->GetAABB()->MaxBound.z = std::max(node->aabb.MaxBound.z, std::max(triangle.v2.z,triangle.v3.z));

       node->GetAABB()->MinBound.x = std::min(node->aabb.MinBound.x, std::min(triangle.v1.x,triangle.v2.x));
       node->aabb.MinBound.x = std::min(node->aabb.MinBound.x, std::min(triangle.v2.x,triangle.v3.x));

       node->aabb.MinBound.y = std::min(node->aabb.MinBound.y, std::min(triangle.v1.y,triangle.v2.y));
       node->aabb.MinBound.y = std::min(node->aabb.MinBound.y, std::min(triangle.v2.y,triangle.v3.y));

       node->aabb.MinBound.z = std::min(node->aabb.MinBound.z, std::min(triangle.v1.z,triangle.v2.z));
       node->aabb.MinBound.z = std::min(node->aabb.MinBound.z, std::min(triangle.v2.z,triangle.v3.z));

}

void CollisionDetection_BVH::splitGeometry(CollisionDetection_BVHNode* pNode, const std::vector<Triangle > * geometry,
                                          std::vector<Triangle > * geometryL,
                                          std::vector<Triangle > * geometryR)
{
       //clock_t t,t1,t2,t3,t4;
       //double d1,d2,d3,d4;

       if (geometry->empty())
              return;

       //t=clock();

       double iMaxX, iMaxY, iMaxZ;
       iMaxX = iMaxY = iMaxZ = 0;
       pNode->aabb.MinBound = VECTOR3(MAX_FLOAT, MAX_FLOAT, MAX_FLOAT);
       pNode->aabb.MaxBound = VECTOR3(MIN_FLOAT,MIN_FLOAT,MIN_FLOAT);

       std::vector<Triangle>::const_iterator it = geometry->begin();
       for (++it; it != geometry->end(); it++)
       {
              calculateNode(pNode,(*it));
              iMaxX += (*it).v1.x;
              iMaxY += (*it).v1.y;
              iMaxZ += (*it).v1.z;
              
              iMaxX += (*it).v2.x;
              iMaxY += (*it).v2.y;
              iMaxZ += (*it).v2.z;
              
              iMaxX += (*it).v3.x;
              iMaxY += (*it).v3.y;
              iMaxZ += (*it).v3.z;
       }

       iMaxX = iMaxX / geometry->size();
       iMaxY = iMaxY / geometry->size();
       iMaxZ = iMaxZ / geometry->size();

       double dMax = __max(iMaxX,iMaxY);
       dMax = __max(dMax,iMaxZ);


       //t1=clock();

       //d1 = double(t1-t);

       VECTOR3 diag = pNode->aabb.MaxBound - pNode->aabb.MinBound;
       VECTOR3 center = pNode->aabb.MinBound + (diag * 0.5f);

       int axis;
       if (diag.x >= __max(diag.y, diag.z))
              axis = X_AXIS;
       else if (diag.y >= __max(diag.x, diag.z))
              axis = Y_AXIS;
       else
              axis = Z_AXIS;

       for (int a = 0;
              a < 3 && ( geometryL->empty() || geometryR->empty() );
              a++, axis = (axis + 1) % 3 )
       {
              geometryL->clear();
              geometryR->clear();

              float axisVal = iMaxX / 2;

              if (axis == 1)
                     axisVal = iMaxY / 2;  //center._v[axis];
              else if (axis == 2)
                     axisVal = iMaxZ / 2;
              //t2=clock();

              //d2 = double(t2-t);

              for (std::vector<Triangle>::const_iterator it = geometry->begin(); it != geometry->end(); it++)
              {
                     if ((*it).v1._v[axis] + (*it).v2._v[axis] + (*it).v2._v[axis] < center._v[axis]*3)
                            geometryL->push_back(*it);
                     else
                            geometryR->push_back(*it);
              }

              //t3=clock();

              //d3 = double(t3-t2);
       }

       //t4=clock();

       //d4 = double(t4-t);

       if (geometryL->empty() || geometryR->empty())
       {
              geometryL->clear();
              geometryR->clear();
              unsigned i;
              std::vector<Triangle>::const_iterator it = geometry->begin();
              for (i = 0; i < geometry->size()/2; i++, it++)
                     geometryL->push_back(*it);
              for (; i < geometry->size(); i++, it++)
                     geometryR->push_back(*it);
       }


}

---