Show
Ignore:
Timestamp:
06/05/08 19:28:06 (7 years ago)
Author:
robert
Message:

Basic implementation of kdtree generation based on vertices

Files:
1 modified

Legend:

Unmodified
Added
Removed
  • OpenSceneGraph/trunk/examples/osgkdtree/osgkdtree.cpp

    r8408 r8410  
    1717*/ 
    1818 
     19   
     20#include <osgDB/ReadFile> 
     21 
     22#define _GLIBCXX_DEBUG 
     23 
    1924#include <osg/ArgumentParser> 
    2025#include <osg/ApplicationUsage> 
     
    2530#include <osg/Geometry> 
    2631 
    27 #include <osgDB/ReadFile> 
    2832 
    2933#include <osgUtil/IntersectionVisitor> 
     
    3640#include <iostream> 
    3741 
     42 
    3843namespace osg 
    3944{ 
     
    4348    public: 
    4449     
    45         typedef short value_type; 
     50        KDNode(): 
     51            _leftChild(0), 
     52            _rightChild(0) {} 
     53     
     54        KDNode(const KDNode& rhs): 
     55            _leftChild(rhs._leftChild), 
     56            _rightChild(rhs._rightChild) {} 
     57 
     58        KDNode& operator = (const KDNode& rhs) 
     59        { 
     60            _leftChild = rhs._leftChild; 
     61            _rightChild = rhs._rightChild; 
     62            return *this; 
     63        } 
     64 
     65        typedef int value_type; 
    4666 
    4767        value_type _leftChild; 
     
    5373    public: 
    5474     
    55         KDLeaf(); 
     75        KDLeaf() {} 
    5676     
    5777        typedef unsigned int index_type; 
     
    80100        typedef std::vector< KDNode > KDNodeList; 
    81101        typedef std::vector< osg::ref_ptr<KDLeaf> > KDLeafList; 
     102         
     103         
     104        /// note, leafNum is negative to distinguish from nodeNum 
     105        int addLeaf(KDLeaf* leaf) { int num = _kdLeaves.size(); _kdLeaves.push_back(leaf); return -(num+1); } 
     106 
     107        int replaceLeaf(int leafNum, KDLeaf* leaf) 
     108        { 
     109            int num = -leafNum-1;  
     110             
     111            if (num>_kdLeaves.size()-1) 
     112            { 
     113                osg::notify(osg::NOTICE)<<"Warning: replaceChild("<<leafNum<<", leaf), num = "<<num<<" _kdLeaves.size()="<<_kdLeaves.size()<<std::endl; 
     114                return leafNum; 
     115            } 
     116             
     117            _kdLeaves[num] = leaf; return leafNum; 
     118        } 
     119 
     120        /// note, leafNum is negative to distinguish from nodeNum 
     121        KDLeaf* getLeaf(int leafNum) 
     122        { 
     123            int num = -leafNum-1; 
     124            if (num<0 || num>_kdLeaves.size()-1) 
     125            { 
     126                osg::notify(osg::NOTICE)<<"Warning: getLeaf("<<leafNum<<", num = "<<num<<") _kdLeaves.size()="<<_kdLeaves.size()<<std::endl; 
     127                return 0; 
     128            } 
     129 
     130            return _kdLeaves[num].get(); 
     131        } 
     132         
     133        int addNode(const KDNode& node) 
     134        { 
     135            int num = _kdNodes.size();  
     136            _kdNodes.push_back(node);  
     137            return num; 
     138        } 
     139 
     140        /// note, nodeNum is positive to distinguish from leftNum 
     141        KDNode& getNode(int nodeNum) 
     142        { 
     143            if (nodeNum<0 || nodeNum>_kdNodes.size()-1) 
     144            { 
     145                osg::notify(osg::NOTICE)<<"Warning: getNode("<<nodeNum<<") _kdNodes.size()="<<_kdNodes.size()<<std::endl; 
     146            } 
     147            return _kdNodes[nodeNum]; 
     148        } 
     149 
    82150 
    83151        osg::observer_ptr<osg::Geometry> _geometry; 
     
    94162    public: 
    95163     
     164     
     165        std::ostream& output(unsigned int level) 
     166        { 
     167            for(unsigned int i=0; i<level; ++i) 
     168            { 
     169                osg::notify(osg::NOTICE)<<"  "; 
     170            } 
     171            return osg::notify(osg::NOTICE); 
     172        } 
     173     
     174 
     175        void traverse(KDLeaf& leaf, unsigned int level) 
     176        { 
     177            output(level)<<"leaf("<<level<<") { "; 
     178 
     179            for(unsigned int i=0; i<leaf._vertexIndices.size(); ++i) 
     180            { 
     181                if (i==0) osg::notify(osg::NOTICE)<<leaf._vertexIndices[i]; 
     182                else osg::notify(osg::NOTICE)<<", "<<leaf._vertexIndices[i];                 
     183            } 
     184 
     185            osg::notify(osg::NOTICE)<<"}"<<std::endl;; 
     186             
     187        } 
     188 
    96189        void traverse(KDTree& tree, KDNode::value_type nodeIndex, unsigned int level) 
    97190        { 
    98             for(unsigned int i=0; i<level; ++i) 
    99             { 
    100                 osg::notify(osg::NOTICE)<<"  "; 
    101             } 
    102             osg::notify(osg::NOTICE)<<"traverse("<<nodeIndex<<", "<< level<<") { "<<std::endl; 
     191            output(level)<<"traverse("<<nodeIndex<<", "<< level<<") { "<<std::endl; 
    103192             
    104193            if (nodeIndex>=0) 
    105194            {         
    106195                KDNode& node = tree._kdNodes[nodeIndex]; 
    107                 traverse(tree,node._leftChild,level+1); 
    108                 traverse(tree,node._rightChild,level+1); 
     196                if (node._leftChild) traverse(tree,node._leftChild,level+1); 
     197                else output(level+1)<<"empty left child()"<<std::endl; 
     198                 
     199                if (node._rightChild) traverse(tree,node._rightChild,level+1); 
     200                else output(level+1)<<"empty right child()"<<std::endl; 
    109201            } 
    110202            else  
     
    112204                KDNode::value_type leafIndex = -nodeIndex-1; 
    113205                KDLeaf& leaf = *(tree._kdLeaves[leafIndex]); 
    114             } 
    115  
    116             for(unsigned int i=0; i<level; ++i) 
    117             { 
    118                 osg::notify(osg::NOTICE)<<"  "; 
    119             } 
    120             osg::notify(osg::NOTICE)<<"}"<<std::endl;; 
     206                traverse(leaf, level); 
     207            } 
     208 
     209            output(level)<<"}"<<std::endl;; 
    121210        } 
    122211 
     
    125214        { 
    126215            osg::notify(osg::NOTICE)<<"traverse(tree)"<<std::endl; 
    127             if (!tree._kdNodes.empty()) traverse(tree,0,0); 
     216            if (!tree._kdNodes.empty())  
     217            { 
     218                traverse(tree,0,0); 
     219            } 
     220            else if (!tree._kdLeaves.empty()) 
     221            { 
     222                traverse(*tree._kdLeaves.front(), 0); 
     223            } 
    128224        } 
    129225     
     
    136232     
    137233        KDTreeBuilder(): 
    138             osg::NodeVisitor(osg::NodeVisitor::TRAVERSE_ALL_CHILDREN) {} 
     234            osg::NodeVisitor(osg::NodeVisitor::TRAVERSE_ALL_CHILDREN), 
     235            _maxNumLevels(24), 
     236            _targetNumVerticesPerLeaf(8)             
     237        {             
     238        } 
    139239     
    140240     
     
    152252     
    153253        KDTree* createKDTree(osg::Geometry* geometry); 
     254         
     255        void computeDivisions(KDTree& kdTree); 
     256         
     257        int divide(KDTree& kdTree, osg::BoundingBox& bb, int nodeIndex, unsigned int level); 
     258 
     259        unsigned int _maxNumLevels; 
     260        unsigned int _targetNumVerticesPerLeaf;         
     261 
    154262}; 
    155263 
     
    157265KDTree* KDTreeBuilder::createKDTree(osg::Geometry* geometry) 
    158266{ 
    159     KDTree* kdTree = new KDTree; 
     267    osg::notify(osg::NOTICE)<<"osg::KDTreeBuilder::createKDTree()"<<std::endl; 
     268 
     269    osg::Vec3Array* vertices = dynamic_cast<osg::Vec3Array*>(geometry->getVertexArray()); 
     270    if (!vertices) return 0; 
     271 
     272    osg::ref_ptr<KDTree> kdTree = new KDTree; 
    160273    kdTree->_geometry = geometry; 
    161274    kdTree->_bb = kdTree->_geometry->getBound(); 
    162275     
    163     osg::notify(osg::NOTICE)<<"osg::KDTreeBuilder::createKDTree()"<<std::endl; 
    164      
    165     return kdTree; 
     276     
     277    unsigned int estimatedSize = (unsigned int)(float(vertices->size())/float(_targetNumVerticesPerLeaf)*1.5); 
     278 
     279    osg::notify(osg::NOTICE)<<"kdTree->_kdNodes.reserve()="<<estimatedSize<<std::endl<<std::endl; 
     280 
     281    kdTree->_kdNodes.reserve(estimatedSize); 
     282    kdTree->_kdLeaves.reserve(estimatedSize); 
     283     
     284    computeDivisions(*kdTree); 
     285 
     286    // create initial leaf list     
     287    osg::ref_ptr<KDLeaf> leaf = new KDLeaf; 
     288    leaf->_vertexIndices.reserve(vertices->size()); 
     289    for(unsigned int i=0; i<vertices->size(); ++i) 
     290    { 
     291        leaf->_vertexIndices.push_back(i); 
     292    } 
     293 
     294    osg::BoundingBox bb = kdTree->_bb; 
     295     
     296    int leafNum = kdTree->addLeaf(leaf.get()); 
     297    int nodeNum = divide(*kdTree, bb, leafNum, 0); 
     298     
     299    osg::notify(osg::NOTICE)<<"Root nodeNum="<<nodeNum<<std::endl; 
     300     
     301     
     302    KDTreeTraverser traverser; 
     303    traverser.traverse(*kdTree); 
     304     
     305    osg::notify(osg::NOTICE)<<"Final kdTree->_kdNodes.size()="<<kdTree->_kdNodes.size()<<std::endl; 
     306    osg::notify(osg::NOTICE)<<"Final kdTree->_kdLeaves.size()="<<kdTree->_kdLeaves.size()<<std::endl; 
     307 
     308    osg::notify(osg::NOTICE)<<"osg::KDTreeBuilder::createKDTree() completed"<<std::endl<<std::endl; 
     309 
     310    return kdTree.release(); 
    166311}     
     312 
     313void KDTreeBuilder::computeDivisions(KDTree& kdTree) 
     314{ 
     315 
     316 
     317    osg::Vec3 dimensions(kdTree._bb.xMax()-kdTree._bb.xMin(), 
     318                         kdTree._bb.yMax()-kdTree._bb.yMin(), 
     319                         kdTree._bb.zMax()-kdTree._bb.zMin()); 
     320 
     321    osg::notify(osg::NOTICE)<<"computeDivisions("<<_maxNumLevels<<") "<<dimensions<< " { "<<std::endl; 
     322 
     323    kdTree._axisStack.reserve(_maxNumLevels); 
     324  
     325    int level = 0; 
     326    for(unsigned int level=0; level<_maxNumLevels; ++level) 
     327    { 
     328        int axis = 0; 
     329        if (dimensions[0]>=dimensions[1]) 
     330        { 
     331            if (dimensions[0]>=dimensions[2]) axis = 0; 
     332            else axis = 2; 
     333        } 
     334        else if (dimensions[1]>=dimensions[2]) axis = 1; 
     335        else axis = 2; 
     336 
     337        kdTree._axisStack.push_back(axis); 
     338        dimensions[axis] /= 2.0f; 
     339 
     340        osg::notify(osg::NOTICE)<<"  "<<level<<", "<<dimensions<<", "<<axis<<std::endl; 
     341    } 
     342 
     343    osg::notify(osg::NOTICE)<<"}"<<std::endl; 
     344} 
     345 
     346int KDTreeBuilder::divide(KDTree& kdTree, osg::BoundingBox& bb, int nodeIndex, unsigned int level) 
     347{ 
     348    if (kdTree._axisStack.size()<=level) return nodeIndex; 
     349 
     350 
     351    int axis = kdTree._axisStack[level]; 
     352 
     353    osg::notify(osg::NOTICE)<<"divide("<<nodeIndex<<", "<<level<< "), axis="<<axis<<std::endl; 
     354 
     355    if (nodeIndex>=0) 
     356    { 
     357        osg::notify(osg::NOTICE)<<"  divide node"<<std::endl; 
     358        KDNode& node = kdTree.getNode(nodeIndex); 
     359        return nodeIndex; 
     360    } 
     361    else 
     362    {     
     363        if (kdTree.getLeaf(nodeIndex)->_vertexIndices.size()<=_targetNumVerticesPerLeaf) return nodeIndex; 
     364     
     365        osg::notify(osg::NOTICE)<<"  divide leaf"<<std::endl; 
     366         
     367        int nodeNum = kdTree.addNode(KDNode()); 
     368 
     369        float original_min = bb._min[axis]; 
     370        float original_max = bb._max[axis]; 
     371 
     372        float mid = (original_min+original_max)*0.5f; 
     373 
     374        { 
     375            osg::ref_ptr<KDLeaf> leaf = kdTree.getLeaf(nodeIndex); 
     376 
     377            // create new node, and add two leaves to it. 
     378            osg::ref_ptr<KDLeaf> leftLeaf = new KDLeaf; 
     379            osg::ref_ptr<KDLeaf> rightLeaf = new KDLeaf; 
     380 
     381 
     382            osg::Vec3Array* vertices = dynamic_cast<osg::Vec3Array*>(kdTree._geometry->getVertexArray()); 
     383 
     384            osg::notify(osg::NOTICE)<<"  divide leaf->_vertexIndices.size()="<<leaf->_vertexIndices.size()<<std::endl; 
     385 
     386            unsigned int estimatedSize = leaf->_vertexIndices.size(); 
     387            leftLeaf->_vertexIndices.reserve(estimatedSize); 
     388            rightLeaf->_vertexIndices.reserve(estimatedSize); 
     389 
     390            for(unsigned int i=0; i<leaf->_vertexIndices.size(); ++i) 
     391            { 
     392                unsigned int vi = leaf->_vertexIndices[i]; 
     393                osg::Vec3& v = (*vertices)[vi]; 
     394                if (v[axis] <= mid) leftLeaf->_vertexIndices.push_back(vi); 
     395                else rightLeaf->_vertexIndices.push_back(vi); 
     396            } 
     397 
     398            if (leftLeaf->_vertexIndices.empty()) 
     399            { 
     400                osg::notify(osg::NOTICE)<<"LeftLeaf empty"<<std::endl; 
     401                kdTree.getNode(nodeNum)._leftChild = 0; 
     402                kdTree.getNode(nodeNum)._rightChild = kdTree.replaceLeaf(nodeIndex, rightLeaf.get()); 
     403            } 
     404            else if (rightLeaf->_vertexIndices.empty()) 
     405            { 
     406                osg::notify(osg::NOTICE)<<"RightLeaf empty"<<std::endl; 
     407                kdTree.getNode(nodeNum)._leftChild = kdTree.replaceLeaf(nodeIndex, leftLeaf.get()); 
     408                kdTree.getNode(nodeNum)._rightChild = 0; 
     409            } 
     410            else 
     411            { 
     412                kdTree.getNode(nodeNum)._leftChild = kdTree.replaceLeaf(nodeIndex, leftLeaf.get()); 
     413                kdTree.getNode(nodeNum)._rightChild = kdTree.addLeaf(rightLeaf.get()); 
     414            } 
     415 
     416 
     417        } 
     418         
     419        int originalLeftChildIndex = kdTree.getNode(nodeNum)._leftChild; 
     420        int originalRightChildIndex = kdTree.getNode(nodeNum)._rightChild; 
     421 
     422 
     423        float restore = bb._max[axis]; 
     424        bb._max[axis] = mid; 
     425 
     426        osg::notify(osg::NOTICE)<<"  divide leftLeaf "<<kdTree.getNode(nodeNum)._leftChild<<std::endl; 
     427        int leftChildIndex = divide(kdTree, bb, originalLeftChildIndex, level+1); 
     428 
     429        bb._max[axis] = restore; 
     430         
     431        restore = bb._min[axis]; 
     432        bb._min[axis] = mid; 
     433 
     434        osg::notify(osg::NOTICE)<<"  divide rightLeaf "<<kdTree.getNode(nodeNum)._rightChild<<std::endl; 
     435        int rightChildIndex = divide(kdTree, bb, originalRightChildIndex, level+1); 
     436         
     437        bb._min[axis] = restore; 
     438         
     439        kdTree.getNode(nodeNum)._leftChild = leftChildIndex; 
     440        kdTree.getNode(nodeNum)._rightChild = rightChildIndex;  
     441         
     442        return nodeNum;         
     443    } 
     444 
     445 
     446} 
    167447 
    168448}