BVH.cc

Go to the documentation of this file.

#include <esg/spacesorting/BVH.h>
#include <esg/spacesorting/BVDistRot.h>
#include <esg/iterator/Iterator.h>
#include <esg/iterator/IteratorBVH.h>
#include <esg/InspectorBVH.h>

using namespace esg;

bool BVH::_append(SceneGraphObject*  obj,
                  Geometry*          bv,
                  Node*              node,
                  unsigned           depth)
{
    if (!node) return false;

    if (node->leaf) {
        node->list->append(obj);
        // Need/can we  increase tree depth?
        if (node->list->length() > _leafLimit && depth < _depthLimit) {
            // create new nodes:
            Node* newLeftLeaf    = new Node;
            Node* newRightLeaf   = new Node;
            // setup new right leaf:
            newRightLeaf->leaf        = true;

            newRightLeaf->list = _create_list();

            (void) node->list->split(*(newRightLeaf->list), _splitStrategy);
            
            //newRightLeaf->list        = _split_list(*(node->list));
            newRightLeaf->leftChild   = newLeftLeaf;
            newRightLeaf->rightChild  = node->rightChild;
            newRightLeaf->parent      = node;
            newRightLeaf->sibling     = newLeftLeaf;
            newRightLeaf->bv          = _create_bv(*(newRightLeaf->list));
            // setup new left leaf:
            newLeftLeaf->leaf        = true;
            newLeftLeaf->list        = node->list;
            newLeftLeaf->leftChild   = node->leftChild;
            newLeftLeaf->rightChild  = newRightLeaf;
            newLeftLeaf->parent      = node;
            newLeftLeaf->sibling     = newRightLeaf;
            newLeftLeaf->bv          = _create_bv(*(newLeftLeaf->list));
            // setup its original right sibling:
            if (node->rightChild)
                node->rightChild->leftChild = newRightLeaf;
            // setup its original left sibling:
            if (node->leftChild)
                node->leftChild->rightChild = newLeftLeaf;
            // setup itself:
            node->leaf        = false;
            node->list        = NULL;
            node->leftChild   = newLeftLeaf;
            node->rightChild  = newRightLeaf;
            delete node->bv;
            node->bv          = _create_bv(*(newLeftLeaf->bv),
                                           *(newRightLeaf->bv));
            if (_leftLeaf == node) _leftLeaf = node->leftChild;
            return true;
        }
        else 
            return _enlarge_bv(&(node->bv), *bv);
    }
    else { // inner node
        double dist1 = bv->distance(*(node->leftChild->bv),  NULL);
        double dist2 = bv->distance(*(node->rightChild->bv), NULL);
        bool   bvChanged;

        if (dist1 <= dist2) // visit the most closed branch
            bvChanged = _append(obj, bv, node->leftChild,  depth+1);
        else 
            bvChanged = _append(obj, bv, node->rightChild, depth+1);

        if (bvChanged) {  // recompute BV if necessarry
            Geometry* pNewBV = _create_bv(*(node->leftChild->bv),
                                          *(node->rightChild->bv));
            bvChanged = _enlarge_bv(&(node->bv), *pNewBV);
            delete pNewBV;
        }

        return bvChanged;
    }
}


void BVH::_remove_leaf(Node* leaf)
{
    if (!leaf->parent) { // leaf is also root
        _leftLeaf = (_root = NULL);
        delete leaf->list;
        delete leaf->bv;
        delete leaf;
        return;
    }

    Node* grandparent = leaf->parent->parent;
    Node* sibling;

    // find leaf's sibling
    if (leaf->parent->leftChild == leaf)
        sibling = leaf->parent->rightChild;
    else
        sibling = leaf->parent->leftChild;

    if (grandparent) {
        // connect sibling with grandparent
        if (grandparent->leftChild == leaf->parent)
            grandparent->leftChild = sibling;
        else
            grandparent->rightChild = sibling;
        sibling->parent = grandparent;

        // correct leaves concatenation
        if (sibling->leftChild == leaf)
            sibling->leftChild  = leaf->leftChild;
        else
            sibling->rightChild = leaf->rightChild;
    } else { // no grandparent
        _root               = sibling;
        sibling->parent     = NULL;
        sibling->leftChild  = NULL;
        sibling->rightChild = NULL;
    }

    // destroy leaf
    delete leaf->parent->bv;
    delete leaf->parent;
    delete leaf->bv;
    delete leaf->list;
    delete leaf;

    // recompute BVs
    for (Node* node = sibling->parent; node != NULL; node = node->parent) {
        delete node->bv;
        node->bv = _create_bv(*(node->leftChild->bv),
                              *(node->rightChild->bv));
    }

    // setup most-left leaf pointer
    if (_leftLeaf == leaf) _leftLeaf = sibling;
}

void BVH::_build_tree(Node* leaf, unsigned depth)
{
    if (!leaf || !leaf->leaf || !leaf->list) return;

    // "non-separable" leaf:
    if (leaf->list->length() <= _leafLimit || depth >= _depthLimit) {
        if (leaf->bv) delete leaf->bv;
        leaf->bv = _create_bv(*(leaf->list));
        return;
    }

    // "separable" leaf:
    // store original leaf siblings first
    Node* leftSibling            = leaf->leftChild;
    Node* rightSibling           = leaf->rightChild;
    // allocate new leaves
    leaf->leftChild              = new Node;
    leaf->rightChild             = new Node;
    leaf->leftChild->sibling     = leaf->rightChild;
    leaf->rightChild->sibling    = leaf->leftChild;
    // change leaf indicators properly
    leaf->leftChild->leaf        = true;
    leaf->rightChild->leaf       = true;
    leaf->leaf                   = false;
    // split original list into children
    leaf->leftChild->list = _create_list();
    (void) leaf->list->split(*(leaf->leftChild->list), _splitStrategy);
    leaf->rightChild->list       = leaf->list;
    leaf->list                   = NULL;
    // set up links of left child
    leaf->leftChild->parent      = leaf;
    leaf->leftChild->rightChild  = leaf->rightChild;
    leaf->leftChild->leftChild   = leftSibling;
    if (leftSibling) leftSibling->rightChild = leaf->leftChild;
    // set up links of right child
    leaf->rightChild->parent     = leaf;
    leaf->rightChild->leftChild  = leaf->leftChild;
    leaf->rightChild->rightChild = rightSibling;
    if (rightSibling) rightSibling->leftChild = leaf->rightChild;
    
    // set "the most left leaf" pointer
    if (_leftLeaf == leaf) _leftLeaf = leaf->leftChild;
    
    // go recursivelly into the new children
    _build_tree(leaf->leftChild,  depth+1);
    _build_tree(leaf->rightChild, depth+1);
    
    // recompute BV of (now) inner node
    if (leaf->bv) delete leaf->bv;
    leaf->bv = _create_bv(*(leaf->leftChild->bv), *(leaf->rightChild->bv));
}

void BVH::_destroy_tree(Node* node)
{
    if (node->leaf) {
        /*
        SceneGraphObject* pObj = node->list->firstItem();
        while (pObj) {
            node->list->remove(pObj);
            delete pObj;
            pObj = node->list->firstItem();
        }
        */
        delete node->list; // non-destructive
    } else {
        _destroy_tree(node->leftChild);
        _destroy_tree(node->rightChild);
    }

    delete node->bv;
    delete node;
}

void BVH::_rebuild_tree(Node* pNode)
{
    delete pNode->bv;
    if (pNode->leaf)
        pNode->bv = _create_bv(*(pNode->list));
    else {
        _rebuild_tree(pNode->leftChild);
        _rebuild_tree(pNode->rightChild);
        pNode->bv = _create_bv(*(pNode->leftChild->bv),
                               *(pNode->rightChild->bv));
    }
}

Geometry* BVH::_create_bv(BVList& list)
{
    List<SceneGraphObject> tmpList;
    SceneGraphObject*      pObject = list.firstItem();
    while (pObject) {
        tmpList.append(pObject);
        pObject = list.nextItem();
    }

    return _create_bv(tmpList);
}

void BVH::_collision(Node * pMyNode, Geometry& bv,
                     List<SceneGraphObject>* pList)
{
    if (pMyNode->bv->separation(bv, NULL)) return;
    
    if (pMyNode->leaf) {
        for (SceneGraphObject * pObj = pMyNode->list->firstItem();
             pObj;
             pObj = pMyNode->list->nextItem())
        {
            pList->append(pObj);
        }
    } else {
        _collision(pMyNode->leftChild, bv, pList);
        _collision(pMyNode->rightChild, bv, pList);
    }
}

void BVH::_dump(const Node* pNode, const char* intent, const char* tab)
{
    char* newIntent = new char [strlen(intent) + strlen(tab) + 2];
    newIntent = strcpy(newIntent, intent);
    newIntent = strcat(newIntent, " ");
    newIntent = strcat(newIntent, tab);
    
    printf("%s bvhNode {\n", intent);
    if (pNode->bv) pNode->bv->dump(newIntent, tab);
    if (pNode->leaf && pNode->list) {
        for (SceneGraphObject * pObj = pNode->list->firstItem();
             pObj;
             pObj = pNode->list->nextItem())
        {
            // to do: dumping of whole scene graph object
            if (pObj->geometry()) pObj->geometry()->dump(newIntent,tab);
        }
    } else {
        _dump(pNode->leftChild,  newIntent, tab);
        _dump(pNode->rightChild, newIntent, tab);
    }
    printf("%s }\n", intent);
    delete [] newIntent;
}

DistRot* BVH::_create_dist_rot(BVH::Node * pNode)
{
    return new BVDistRot(*(pNode->bv), _coordMat, _scaleRatio);
}

void BVH::_dr_init(const Matrix4* pMyTr, const Matrix4* pHisTr, BVH* pPartner)
{
    (pMyTr) ? _myTrMat.set(*pMyTr) : _myTrMat.setIdentity();
    
    Matrix4 * m = multiTrans(pHisTr, pMyTr, _scaleRatio);
    if (m) {
        _coordMat.set(*m);
        delete m;
    } else {
        _coordMat.setIdentity();
    }

    _pPartner = pPartner;
}

void BVH::_dr_primitives(DRInfo& dr, Node* pNode, Vector3* pDir)
{
    /*
     * Handle primitives - HACK!
     */
    
    for (SceneGraphObject * pA = pNode->list->firstItem();
         pA;
         pA = pNode->list->nextItem())
    {
        Geometry * pGA = pA->geometry();
        if (!pGA) continue;
        
        for (SceneGraphObject * pB = dr.pLeaf->list->firstItem();
             pB;
             pB = dr.pLeaf->list->nextItem())
        {
            Geometry * pGB = pB->geometry();
            if (!pGB) continue;
            
            switch (dr.strategy) {
            case DRInfo::SEPARATION:
                if (! pGA->separation(*pGB, pDir)) {
                    dr.minD = -MAXDOUBLE;
                    if (dr.leafInMe) {
                        dr.pLeafA = dr.pLeaf;
                        dr.pLeafB = pNode;
                    } else {
                        dr.pLeafA = pNode;
                        dr.pLeafB = dr.pLeaf;
                    }
                    return;
                }
                break;
            case DRInfo::DISTANCE:
                break;
            case DRInfo::POINT_COLLISION:
                break;
            }
        } 
    } // for
}

void BVH::_dr_leaf(DRInfo & dr, Node * pNode, unsigned depth)
{
    if (pNode->leaf || depth >= dr.maxDepth) {
        /*
         * Both me and partner are in "leaves"
         */
        
        Vector3* pDir = (dr.pDir) ? new Vector3() : NULL;

        // This should be real distance of the objects in the leaf:
        double dL = dr.pDistRot->distance(*(pNode->bv), dr.minD, pDir);
        
        if (dL < dr.minD) {  // Update smallest value, remember leaves

#ifdef ESG_COLLISION_HACK
            if (pNode->leaf) _dr_primitives(dr, pNode, pDir);
#endif //ESG_COLLISION_HACK
            
            dr.minD = dL;
            
            if (dr.pDir) {
                Matrix3 rMat;
                Vector3 tVec, sVec;
                
                dr.pDir->set(*pDir); 
                
                if (dr.leafInMe) {
                    _coordMat.get(rMat, tVec, sVec);
                    ESG_INVERSE_TR_DIR(rMat, *(dr.pDir), *(dr.pDir));
                    _myTrMat.transform(*(dr.pDir));
                } else {
                    _pPartner->_coordMat.get(rMat, tVec, sVec);
                    ESG_INVERSE_TR_DIR(rMat, *(dr.pDir), *(dr.pDir));
                    _pPartner->_myTrMat.transform(*(dr.pDir));

                }
            }
            
            // Is the leaf in the tree of A?
            if (pNode->leaf) { 
                if (dr.leafInMe) { dr.pLeafA= dr.pLeaf; dr.pLeafB=pNode;    }
                else             { dr.pLeafA=pNode;     dr.pLeafB=dr.pLeaf; }
            }
        }

        if (pDir) delete pDir;
        
    } else { // Either me or partner is "in the leaf"
        
        // Select the most promising branch
        double dL=dr.pDistRot->distance(*(pNode->leftChild->bv), dr.minD,NULL);
        double dR=dr.pDistRot->distance(*(pNode->rightChild->bv),dr.minD,NULL);
        
        if (dL < dR) {
            if (dL < dr.minD) {
                _dr_leaf(dr, pNode->leftChild, depth+1);
                if (dR < dr.minD) _dr_leaf(dr, pNode->rightChild, depth+1);
            }
        } else {
            if (dR < dr.minD) {
                _dr_leaf(dr, pNode->rightChild, depth+1);
                if (dL < dr.minD) _dr_leaf(dr, pNode->leftChild, depth+1);
            }
        }
    }
}

void BVH::_dr_inner(DRInfo& dr, Node* pMyNode, Node* pHisNode, unsigned depth)
{
    if (pHisNode->leaf || depth >= dr.hisMaxDepth) {
        // partner is in the leaf node;
        // dr.pDistRot is set to transformed partner, search only me
        dr.leafInMe = false;
        dr.pLeaf    = pHisNode;
        dr.maxDepth = dr.myMaxDepth;
        _dr_leaf(dr, pMyNode, depth);
        return;
    }

    if (pMyNode->leaf || depth >= dr.myMaxDepth) {
        // my node is leaf, partner has inner node 
        // Set dr.pDistRot to transformed me, search only the partner
        DistRot * pDistRot = this->_create_dist_rot(pMyNode);
        dr.pDistRot = pDistRot;
        dr.leafInMe = true;
        dr.pLeaf    = pMyNode;
        dr.maxDepth = dr.hisMaxDepth;
        _dr_leaf(dr, pHisNode, depth);
        delete pDistRot;
        return;
    }

    // My node even the partner's node are inner
    // Start computing transformed volume of partner
    double     d[4], dd;
    int        way;
    DistRot  * pDistRotL = _pPartner->_create_dist_rot(pHisNode->leftChild);
    DistRot  * pDistRotR = _pPartner->_create_dist_rot(pHisNode->rightChild);

    // Compute distances between pairs of volumes
    d[0] = pDistRotL->distance(*(pMyNode->leftChild->bv),  dr.minD, NULL);
    d[1] = pDistRotR->distance(*(pMyNode->leftChild->bv),  dr.minD, NULL);
    d[2] = pDistRotL->distance(*(pMyNode->rightChild->bv), dr.minD, NULL);
    d[3] = pDistRotR->distance(*(pMyNode->rightChild->bv), dr.minD, NULL);

    // Search every pair of subtrees that can decrease value dr.minD,
    // start with most promising pair
    do {
        way = -1;
        dd  = dr.minD;

        // Find the closest branch
        for (int i = 0; i < 4; i++) if (d[i] < dd) { dd = d[i]; way = i; }

        // Inspect it
        switch (way) {
        case 0:
            dr.pDistRot = pDistRotL;
            _dr_inner(dr, pMyNode->leftChild, pHisNode->leftChild, depth+1);
            d[0] = FLT_MAX;
            break;
        case 1:
            dr.pDistRot = pDistRotR;
            _dr_inner(dr, pMyNode->leftChild, pHisNode->rightChild, depth+1);
            d[1] = FLT_MAX;
            break;
        case 2:
            dr.pDistRot = pDistRotL;
            _dr_inner(dr, pMyNode->rightChild, pHisNode->leftChild, depth+1);
            d[2] = FLT_MAX;
            break;
        case 3:
            dr.pDistRot = pDistRotR;
            _dr_inner(dr, pMyNode->rightChild, pHisNode->rightChild, depth+1);
            d[3] = FLT_MAX;
            break;
        }
    } while (way >= 0 && dr.minD > -MAXDOUBLE);

    delete pDistRotL;
    delete pDistRotR;
}

void BVH::_duplicate_attributes(const SDS& src)
{
    SDS::_duplicate_attributes(src);
    _leafLimit   = ((BVH&)src)._leafLimit;
    _depthLimit  = ((BVH&)src)._depthLimit;
    _pPartner    = NULL;
    _root        = NULL;
    _leftLeaf    = NULL;
    _scaleRatio  = ((BVH&)src)._scaleRatio;
    _splitStrategy = ((BVH&)src)._splitStrategy;
}


// --------- public --------------

BVH::BVH(unsigned ll, unsigned dl, bool delayBuild, BVList::SplitStrategy ss)
    : SDS(delayBuild)
{
    _leafLimit       = (ll > 0) ? ll : 1;
    _depthLimit      = dl;
    _leftLeaf        = NULL;
    _root            = NULL;
    _pPartner        = NULL;
    _scaleRatio      = 1.0;
    _splitStrategy   = ss;
}

BVH::~BVH()
{
    if (_root) _destroy_tree(_root);
}

Iterator* BVH::createIterator()
{
    return (Iterator*) new IteratorBVH(this);
}

InspectorSDS* BVH::createInspector(unsigned level)
{
    return new InspectorBVH(this, level);
}

int BVH::append(SceneGraphObject* obj)
{
    if (!obj) return 0;

    if (!obj->tangible()) {
        _intangibleChildren.append(obj);
        return 1;
    }

    if (_delayBuild) {
        if (!_root) {
            _root       = new Node;
            _root->leaf = true;
            _root->list = _create_list();
            _leftLeaf   = _root;
        }
        _root->list->append(obj);
    } else {
        if (_root) {
            Geometry* bv = _create_bv(*obj);
            (void) _append(obj, bv, _root, 0);
            delete bv;
        } else {
            _root             = new Node;
            _root->bv         = _create_bv(*obj);
            _root->leaf       = true;
            _root->list       = _create_list();
            _root->list->append(obj);
            _leftLeaf         = _root;
        }
    }

    return 1;
}

SceneGraphObject* BVH::detach(SceneGraphObject::OID oid)
{
  for (Node* node = _leftLeaf; node != NULL; node = node->rightChild) {
    SceneGraphObject* obj = node->list->firstItem();
    while (obj) {
      if (obj->getOID() == oid) {
        SceneGraphObject* retObj = node->list->remove(obj);
        if (node->list->length() == 0) _remove_leaf(node);
        else {
          delete node->bv;
          node->bv = _create_bv(*(node->list));
        }
        return retObj;
      }
      obj = node->list->nextItem();
    }
  }
  return NULL;
}

bool BVH::update()
{
    if (_delayBuild) return false;
    if (_root) _rebuild_tree(_root);
    return true;
}

bool BVH::build()
{
    if (!_delayBuild) return false;
    if (!_root)       return false;
    _build_tree(_root, 0);
    _delayBuild = false;
    return true;
}

double BVH::distance(Matrix4  * pTrMy,
                     BVH&       bvhB,
                     Matrix4  * pTrB,
                     Node    ** ppMinA,
                     Node    ** ppMinB,
                     Vector3  * pDir,
                     unsigned   d1,
                     unsigned   d2)
{
    if (_delayBuild || bvhB._delayBuild) return -MAXDOUBLE;
    if (!_root      || !bvhB._root)      return -MAXDOUBLE;
    if (!_root->bv  || !bvhB._root->bv)  return -MAXDOUBLE;

    DRInfo dr;
    dr.strategy    = DRInfo::DISTANCE;
    dr.pDir        = pDir;
    dr.pLeafA      = NULL;
    dr.pLeafB      = NULL;
    dr.minD        = FLT_MAX;
    dr.myMaxDepth  = d1;
    dr.hisMaxDepth = d2;

    this->_dr_init(pTrMy, pTrB, &bvhB);
    bvhB._dr_init(pTrB, pTrMy, this);

    // Get current distance between two nearest leaves from the previous pass
    if (ppMinA && ppMinB) {
        DistRot * pDRot = bvhB._create_dist_rot(*ppMinB);
        dr.minD = pDRot->distance(*((*ppMinA)->bv), FLT_MAX, pDir); //bylo -MAX
        delete pDRot;
    } 

    DistRot * pDistRot = bvhB._create_dist_rot(bvhB._root);
    dr.pDistRot = pDistRot;

    // Get distance of the roots of the trees.
    // In this version the condition is always true, but we need it for
    // pure collision detection
        _dr_inner(dr, _root, bvhB._root, 0);

    if (ppMinA && ppMinB) { *ppMinA = dr.pLeafA; *ppMinB = dr.pLeafB; }

    delete pDistRot;

    return dr.minD;
}

bool BVH::separation(Matrix4* pTrMy,
                     BVH&     bvhB,
                     Matrix4* pTrB,
                     Vector3* pDir,
                     unsigned d1,
                     unsigned d2)
{
    if (_delayBuild || bvhB._delayBuild) return true;
    if (!_root      || !bvhB._root)      return true;
    if (!_root->bv  || !bvhB._root->bv)  return true;

    DRInfo dr;
    dr.strategy = DRInfo::SEPARATION;
    dr.pDir = pDir;

    this->_dr_init(pTrMy, pTrB, &bvhB);
    bvhB._dr_init(pTrB, pTrMy, this);

    dr.minD        = 0;  // we check only separation
    dr.pLeafA      = NULL;
    dr.pLeafB      = NULL;
    dr.myMaxDepth  = d1;
    dr.hisMaxDepth = d2;

    DistRot * pDistRot = bvhB._create_dist_rot(bvhB._root);
    dr.pDistRot = pDistRot;

    // Check collision of top volumes
    if (!dr.pDistRot->separation(*(_root->bv), NULL))
        _dr_inner(dr, _root, bvhB._root, 0);

    delete pDistRot;

    return (dr.minD >= 0.);
}

Geometry** BVH::collision(Matrix4 * pTrMy,
                         float      x,
                         float      y,
                         float      z,
                         float      radius,
                         int      * numLeaves)
{
    
    Geometry *             pBV;
    List<SceneGraphObject> list;
    
    *numLeaves = 0;

    if (_delayBuild) return NULL;
    if (!_root)      return NULL;
    if (!_root->bv)  return NULL;

    if (pTrMy) {
        Vector3   point(x,y,z);
        Matrix4 * pTrMat = multiTrans(pTrMy, NULL, _scaleRatio);
        pTrMat->transform(point);
        Sphere    sphere(point.x, point.y, point.z, radius);
        // scaling factor affects measured distance but never collision
        // itself. Therefore we can ignore it here.
        pBV = _create_bv(sphere, sphere);
    } else {
        Sphere sphere(x, y, z, radius);
        pBV = _create_bv(sphere, sphere);
    }
    
    if (!pBV) return NULL;

    _collision(_root, *pBV, &list);

    *numLeaves = list.length();
    if (*numLeaves == 0) return NULL;

    Geometry** array = new Geometry* [*numLeaves]; 
    unsigned   i     = 0;
    for (SceneGraphObject * pObj = list.firstItem();
         pObj;
         pObj = list.nextItem())
    {
        array[i] = pObj->geometry();
        if (!array[i]) (*numLeaves)--;
        else i++;
    }

    delete pBV;
    return array;
}

void BVH::dump(const char* intent, const char* tab) 
{
    char* newIntent = new char [strlen(intent) + strlen(tab) + 2];
    newIntent = strcpy(newIntent, intent);
    newIntent = strcat(newIntent, " ");
    newIntent = strcat(newIntent, tab);
    
    printf("%s spacialDataStructure BVH {\n", intent);
    printf("%s %s leafLimit  %d\n", intent, tab, _leafLimit);
    printf("%s %s depthLimit %d\n", intent, tab, _depthLimit);
    printf("%s %s treeDepth  %d\n", intent, tab, _treeDepth);
    printf("%s %s splitMode  ", intent, tab);
    switch (_splitStrategy) {
    case BVList::SPLIT_BY_MAX_VARIATION: printf("MAX_VARIATION\n"); break;
    case BVList::SPLIT_BY_MAX_EXTENT:    printf("MAX_EXTENT\n");    break;
    default:                             printf("UNKNOWN\n");       break;
    }
    _dump(_root, newIntent, tab);
    printf("%s }\n", intent);
    delete [] newIntent;
}


void BVH::__debug() {
    fprintf(stderr,"BVH Debugging:\n");
    //if (_leftLeaf) _leftLeaf->list->__debug();
    if (_root) __debug(_root,0);
    else fprintf(stderr,"BVH: No structure\n");
}
    
void BVH::__debug(Node* n, int d) {
    if (!n) return;
    for (int i=0;i<d;i++) fprintf(stderr,"   ");
    n->bv->__debug();
    if (n->leaf) {
        for (int i=0;i<d;i++) fprintf(stderr,"   ");
        n->list->__debug();
    } else {
        __debug(n->leftChild,d+1);
        __debug(n->rightChild,d+1);
    }
}

void BVH::__get_edges(Matrix4 * pTrMat,
                      Node*     pNode,
                      unsigned  level,
                      unsigned  maxLevel,
                      int       rot,
                      float*    array,
                      unsigned& size)
{
    if (pNode->leaf || level >= maxLevel) {
        Vector3 v;
        Geometry * pGeom;

        if      (rot == 1) pGeom = (Geometry*) pNode->bv->clone(pTrMat);
        else if (rot == 2) pGeom = (Geometry*) pNode->bv->clone(&_coordMat);
        else               pGeom = (Geometry*) pNode->bv->clone();

        if (!pGeom) {
            fprintf(stderr,"BVH::__get_edges(): Clonning of BV failed\n");
            return;
        }

        Mesh * pMesh = pGeom->mesh(0);
        if (!pMesh) { delete pGeom; return; }

        pMesh->resetActSolid();
        do {
            pMesh->resetEdgeWalkInSolid();
            do {
                v.set(pMesh->getActVert1(false));
                array[size++] = v.x;
                array[size++] = v.y;
                array[size++] = v.z;
                v.set(pMesh->getActVert2(false));
                array[size++] = v.x;
                array[size++] = v.y;
                array[size++] = v.z;
            } while (pMesh->stepInEdgeWalkInSolid());
        } while (pMesh->goToNextSolid());

        delete pMesh;
        delete pGeom;
        return;
    }

    if (pNode->leaf) return;
    
    __get_edges(pTrMat, pNode->leftChild, level+1, maxLevel, rot, array, size);
    __get_edges(pTrMat, pNode->rightChild,level+1, maxLevel, rot, array, size);
}

float* BVH::__getEdges(Matrix4 * pMyTr,
                       unsigned  level,
                       int       rot,
                       unsigned& size)
{
    size = 0;
    if (!_root) return NULL;
    
    float * array = new float[100000];
    
    __get_edges(pMyTr, _root, 0, level, rot, array, size);
    return array;
}

void BVH::__get_meshes(Matrix4 * pTrMat,
                       Node*     pNode,
                       unsigned  level,
                       unsigned  maxLevel,
                       int       rot,
                       Mesh**    array,
                       unsigned& size)
{
    if (pNode->leaf || level >= maxLevel) {
        Vector3 v;
        Geometry * pGeom;
        if (rot == 1)      pGeom = (Geometry*) pNode->bv->clone(pTrMat);
        else if (rot == 2) pGeom = (Geometry*) pNode->bv->clone(&_coordMat);
        else               pGeom = (Geometry*) pNode->bv->clone();
        array[size++] = pGeom->mesh(0);
        delete pGeom;
        return;
    }

    if (pNode->leaf) return;
    
    __get_meshes(pTrMat, pNode->leftChild, level+1, maxLevel, rot, array,size);
    __get_meshes(pTrMat, pNode->rightChild,level+1, maxLevel, rot, array,size);
}

Mesh** BVH::__getMeshes(Matrix4 * pMyTr,
                        unsigned  level,
                        int       rot,
                        unsigned& size)
{
    Mesh ** array = new Mesh* [10000];
    size = 0;
    __get_meshes(pMyTr, _root, 0, level, rot, array, size);
    return array;
}








                 

---