Changeset 8556

Show
Ignore:
Timestamp:
07/10/08 17:50:10
Author:
robert
Message:

Streamlined KdTree? implementation

Files:

Legend:

Unmodified
Added
Removed
Modified
Copied
Moved
  • OpenSceneGraph/trunk/include/osg/KdTree

    r8553 r8556  
    9292            value_type second; 
    9393        }; 
    94  
    95  
    96         struct KdLeaf 
    97         { 
    98             KdLeaf(): 
    99                 first(0), 
    100                 second(0) {} 
    101  
    102             KdLeaf(value_type f, value_type s): 
    103                 first(f), 
    104                 second(s) {} 
    105  
    106             osg::BoundingBox bb; 
    107  
    108             value_type first;     
    109             value_type second; 
    110         }; 
    111  
     94     
    11295        struct Triangle 
    11396        { 
     
    131114        typedef std::vector< unsigned int > AxisStack; 
    132115        typedef std::vector< KdNode >       KdNodeList; 
    133         typedef std::vector< KdLeaf >       KdLeafList; 
    134  
    135         /// note, leafNum is negative to distinguish from nodeNum 
    136         int addLeaf(const KdLeaf& leaf) { int num = _kdLeaves.size(); _kdLeaves.push_back(leaf); return -(num+1); } 
    137  
    138         int replaceLeaf(int leafNum, const KdLeaf& leaf) 
    139         { 
    140             int num = -leafNum-1;  
    141              
    142             if (num>static_cast<int>(_kdLeaves.size())-1) 
    143             { 
    144                 osg::notify(osg::NOTICE)<<"Warning: replaceChild("<<leafNum<<", leaf), num = "<<num<<" _kdLeaves.size()="<<_kdLeaves.size()<<std::endl; 
    145                 return leafNum; 
    146             } 
    147              
    148             _kdLeaves[num] = leaf; return leafNum; 
    149         } 
    150  
    151         /// note, leafNum is negative to distinguish from nodeNum 
    152         KdLeaf& getLeaf(int leafNum) 
    153         { 
    154             int num = -leafNum-1; 
    155             if (num<0 || num>static_cast<int>(_kdLeaves.size())-1) 
    156             { 
    157                 osg::notify(osg::NOTICE)<<"Warning: getLeaf("<<leafNum<<", num = "<<num<<") _kdLeaves.size()="<<_kdLeaves.size()<<std::endl; 
    158             } 
    159  
    160             return _kdLeaves[num]; 
    161         } 
    162  
    163         const KdLeaf& getLeaf(int leafNum) const 
    164         { 
    165             int num = -leafNum-1; 
    166             if (num<0 || num>static_cast<int>(_kdLeaves.size())-1) 
    167             { 
    168                 osg::notify(osg::NOTICE)<<"Warning: getLeaf("<<leafNum<<", num = "<<num<<") _kdLeaves.size()="<<_kdLeaves.size()<<std::endl; 
    169             } 
    170  
    171             return _kdLeaves[num]; 
    172         } 
    173116 
    174117        int addNode(const KdNode& node) 
     
    201144        osg::BoundingBox& getBoundingBox(int nodeNum) 
    202145        { 
    203             if (nodeNum<0) 
    204             { 
    205                 int num = -nodeNum-1; 
    206                 return _kdLeaves[num].bb;             
    207             } 
    208             else 
    209             { 
    210                 return _kdNodes[nodeNum].bb; 
    211             } 
     146            return _kdNodes[nodeNum].bb; 
    212147        } 
    213148 
     
    217152        int divide(BuildOptions& options, osg::BoundingBox& bb, int nodeIndex, unsigned int level); 
    218153 
    219         bool intersect(const KdLeaf& leaf, const osg::Vec3& start, const osg::Vec3& end, LineSegmentIntersections& intersections) const; 
    220         bool intersect(const KdNode& node, const osg::Vec3& start, const osg::Vec3& end, const osg::Vec3& s, const osg::Vec3& e, LineSegmentIntersections& intersections) const; 
     154        struct RayData 
     155        { 
     156            RayData(const osg::Vec3& s, const osg::Vec3& e): 
     157                _s(s), 
     158                _e(e) 
     159            { 
     160                _d = e - s; 
     161                _length = _d.length(); 
     162                _inverse_length = 1.0f/_length; 
     163                _d *= _inverse_length; 
     164            } 
     165             
     166         
     167            osg::Vec3 _s; 
     168            osg::Vec3 _e; 
     169             
     170            osg::Vec3 _d; 
     171            float     _length; 
     172            float     _inverse_length; 
     173        }; 
     174 
     175        bool intersect(const KdNode& node, const RayData& rayData, osg::Vec3 s, osg::Vec3 e, LineSegmentIntersections& intersections) const; 
    221176        bool intersectAndClip(osg::Vec3& s, osg::Vec3& e, const osg::BoundingBox& bb) const; 
    222177 
     
    231186        AxisStack                           _axisStack; 
    232187        KdNodeList                          _kdNodes; 
    233         KdLeafList                          _kdLeaves; 
    234188 
    235189        osg::ref_ptr<osg::Vec3Array>        _vertices; 
     
    237191        Indices                             _primitiveIndices; 
    238192         
    239         BoundingBoxList                     _boundingBoxes; 
    240193        TriangleList                        _triangles; 
    241194        CenterList                          _centers; 
  • OpenSceneGraph/trunk/src/osg/KdTree.cpp

    r8554 r8556  
    2121using namespace osg; 
    2222 
    23 // #define VERBOSE_OUTPUT 
     23//#define VERBOSE_OUTPUT 
    2424 
    2525//////////////////////////////////////////////////////////////////////////////// 
     
    4343        bb.expandBy((*(_kdTree->_vertices))[p2]); 
    4444        bb.expandBy((*(_kdTree->_vertices))[p3]); 
    45         _kdTree->_boundingBoxes.push_back(bb); 
    46          
     45 
    4746        _kdTree->_centers.push_back(bb.center()); 
    48  
    4947        _kdTree->_primitiveIndices.push_back(i); 
    5048         
     
    6159KdTree::BuildOptions::BuildOptions(): 
    6260        _numVerticesProcessed(0), 
    63         _targetNumTrianglesPerLeaf(4), 
     61        _targetNumTrianglesPerLeaf(2), 
    6462        _maxNumLevels(32) 
    6563{ 
     
    10098#endif 
    10199 
    102     _kdNodes.reserve(estimatedSize); 
    103     _kdLeaves.reserve(estimatedSize); 
     100    _kdNodes.reserve(estimatedSize*5); 
    104101     
    105102    computeDivisions(options); 
     
    109106    unsigned int estimatedNumTriangles = vertices->size()*2; 
    110107    _primitiveIndices.reserve(estimatedNumTriangles); 
    111     _boundingBoxes.reserve(estimatedNumTriangles); 
    112108    _triangles.reserve(estimatedNumTriangles); 
    113109    _centers.reserve(estimatedNumTriangles); 
     
    121117    _primitiveIndices.reserve(vertices->size()); 
    122118 
    123  
    124  
    125     KdLeaf leaf(0, _primitiveIndices.size()); 
    126  
    127     int leafNum = addLeaf(leaf); 
     119    KdNode node(-1, _primitiveIndices.size()); 
     120    node.bb = _bb; 
     121 
     122    int nodeNum = addNode(node); 
    128123 
    129124    osg::BoundingBox bb = _bb; 
    130     int nodeNum = divide(options, bb, leafNum, 0); 
     125    nodeNum = divide(options, bb, nodeNum, 0); 
    131126     
    132127#ifdef VERBOSE_OUTPUT     
     
    180175int KdTree::divide(BuildOptions& options, osg::BoundingBox& bb, int nodeIndex, unsigned int level) 
    181176{ 
    182     if (_axisStack.size()<=level) return nodeIndex; 
    183  
    184     int axis = _axisStack[level]; 
    185  
    186 #ifdef VERBOSE_OUTPUT     
    187     //osg::notify(osg::NOTICE)<<"divide("<<nodeIndex<<", "<<level<< "), axis="<<axis<<std::endl; 
    188 #endif 
    189  
    190     if (nodeIndex>=0) 
    191     { 
    192 #ifdef VERBOSE_OUTPUT     
    193         osg::notify(osg::NOTICE)<<"  divide node"<<std::endl; 
    194 #endif 
    195         KdNode& node = getNode(nodeIndex); 
    196         return nodeIndex; 
    197     } 
    198     else 
    199     {     
    200  
    201         if (getLeaf(nodeIndex).second<=options._targetNumTrianglesPerLeaf)  
    202         { 
     177    KdNode& node = getNode(nodeIndex); 
     178 
     179    bool needToDivide = level < _axisStack.size() && 
     180                        (node.first<0 && node.second>options._targetNumTrianglesPerLeaf); 
     181                         
     182    if (!needToDivide) 
     183    { 
     184        if (node.first<0) 
     185        { 
     186            int istart = -node.first-1; 
     187            int iend = istart+node.second-1; 
     188     
    203189            // leaf is done, now compute bound on it. 
    204             KdLeaf& leaf = getLeaf(nodeIndex); 
    205             leaf.bb.init(); 
    206             int iend = leaf.first+leaf.second; 
    207             for(int i=leaf.first; i<iend; ++i) 
     190            node.bb.init(); 
     191            for(int i=istart; i<=iend; ++i) 
    208192            { 
    209193                const Triangle& tri = _triangles[_primitiveIndices[i]]; 
     
    211195                const osg::Vec3& v2 = (*_vertices)[tri._p2]; 
    212196                const osg::Vec3& v3 = (*_vertices)[tri._p3]; 
    213                 leaf.bb.expandBy(v1); 
    214                 leaf.bb.expandBy(v2); 
    215                 leaf.bb.expandBy(v3); 
    216             } 
    217  
    218             return nodeIndex; 
    219         } 
     197                node.bb.expandBy(v1); 
     198                node.bb.expandBy(v2); 
     199                node.bb.expandBy(v3); 
     200                 
     201                float epsilon = 1e-6; 
     202                node.bb._min.x() -= epsilon; 
     203                node.bb._min.y() -= epsilon; 
     204                node.bb._min.z() -= epsilon; 
     205                node.bb._max.x() += epsilon; 
     206                node.bb._max.y() += epsilon; 
     207                node.bb._max.z() += epsilon; 
     208            } 
     209 
     210#ifdef VERBOSE_OUTPUT     
     211            if (!node.bb.valid()) 
     212            { 
     213                osg::notify(osg::NOTICE)<<"After reset "<<node.first<<","<<node.second<<std::endl; 
     214                osg::notify(osg::NOTICE)<<"  bb._min ("<<node.bb._min<<")"<<std::endl; 
     215                osg::notify(osg::NOTICE)<<"  bb._max ("<<node.bb._max<<")"<<std::endl; 
     216            } 
     217            else 
     218            { 
     219                osg::notify(osg::NOTICE)<<"Set bb for nodeIndex = "<<nodeIndex<<std::endl; 
     220            } 
     221#endif 
     222        } 
     223 
     224        return nodeIndex; 
     225 
     226    } 
     227 
     228    int axis = _axisStack[level]; 
     229 
     230#ifdef VERBOSE_OUTPUT     
     231    osg::notify(osg::NOTICE)<<"divide("<<nodeIndex<<", "<<level<< "), axis="<<axis<<std::endl; 
     232#endif 
     233 
     234    if (node.first<0) 
     235    {     
     236        // leaf node as first <= 0, so look at dividing it. 
     237         
     238        int istart = -node.first-1; 
     239        int iend = istart+node.second-1; 
    220240 
    221241        //osg::notify(osg::NOTICE)<<"  divide leaf"<<std::endl; 
    222242         
    223         int nodeNum = addNode(KdNode()); 
    224  
    225243        float original_min = bb._min[axis]; 
    226244        float original_max = bb._max[axis]; 
     
    228246        float mid = (original_min+original_max)*0.5f; 
    229247 
    230         { 
    231             KdLeaf& leaf = getLeaf(nodeIndex); 
    232  
     248        int originalLeftChildIndex = 0; 
     249        int originalRightChildIndex = 0; 
     250 
     251        { 
    233252            //osg::Vec3Array* vertices = kdTree._vertices.get(); 
    234             int end = leaf.first+leaf.second-1; 
    235             int left = leaf.first; 
    236             int right = leaf.first+leaf.second-1; 
     253            int left = istart; 
     254            int right = iend; 
    237255             
    238256            while(left<right) 
     
    258276            } 
    259277             
    260             KdLeaf leftLeaf(leaf.first, (right-leaf.first)+1); 
    261             KdLeaf rightLeaf(left, (end-left)+1); 
    262  
    263 #if 0             
    264             osg::notify(osg::NOTICE)<<"In  leaf.first     ="<<leaf.first     <<" leaf.second     ="<<leaf.second<<std::endl; 
     278            KdNode leftLeaf(-istart-1, (right-istart)+1); 
     279            KdNode rightLeaf(-left-1, (iend-left)+1); 
     280 
     281#if 0 
     282            osg::notify(osg::NOTICE)<<"In  node.first     ="<<node.first     <<" node.second     ="<<node.second<<std::endl; 
    265283            osg::notify(osg::NOTICE)<<"    leftLeaf.first ="<<leftLeaf.first <<" leftLeaf.second ="<<leftLeaf.second<<std::endl; 
    266284            osg::notify(osg::NOTICE)<<"    rightLeaf.first="<<rightLeaf.first<<" rightLeaf.second="<<rightLeaf.second<<std::endl; 
    267285            osg::notify(osg::NOTICE)<<"    left="<<left<<" right="<<right<<std::endl; 
    268286 
    269             if (leaf.second != (leftLeaf.second +rightLeaf.second)) 
    270             { 
    271                 osg::notify(osg::NOTICE)<<"*** Error in size, leaf.second="<<leaf.second 
     287            if (node.second != (leftLeaf.second +rightLeaf.second)) 
     288            { 
     289                osg::notify(osg::NOTICE)<<"*** Error in size, leaf.second="<<node.second 
    272290                                        <<", leftLeaf.second="<<leftLeaf.second 
    273291                                        <<", rightLeaf.second="<<rightLeaf.second<<std::endl; 
     
    275293            else 
    276294            { 
    277                 osg::notify(osg::NOTICE)<<"Size OK, leaf.second="<<leaf.second 
     295                osg::notify(osg::NOTICE)<<"Size OK, leaf.second="<<node.second 
    278296                                        <<", leftLeaf.second="<<leftLeaf.second 
    279297                                        <<", rightLeaf.second="<<rightLeaf.second<<std::endl; 
    280298            } 
    281299#endif 
     300 
    282301            if (leftLeaf.second<=0) 
    283302            { 
    284303                //osg::notify(osg::NOTICE)<<"LeftLeaf empty"<<std::endl; 
    285                 getNode(nodeNum).first = 0; 
    286                 getNode(nodeNum).second = replaceLeaf(nodeIndex, rightLeaf); 
     304                originalLeftChildIndex = 0; 
     305                originalRightChildIndex = addNode(rightLeaf); 
    287306            } 
    288307            else if (rightLeaf.second<=0) 
    289308            { 
    290309                //osg::notify(osg::NOTICE)<<"RightLeaf empty"<<std::endl; 
    291                 getNode(nodeNum).first = replaceLeaf(nodeIndex, leftLeaf); 
    292                 getNode(nodeNum).second = 0; 
     310                originalLeftChildIndex = addNode(leftLeaf); 
     311                originalRightChildIndex = 0; 
    293312            } 
    294313            else 
    295314            { 
    296                 getNode(nodeNum).first = replaceLeaf(nodeIndex, leftLeaf); 
    297                 getNode(nodeNum).second = addLeaf(rightLeaf); 
    298             } 
    299         } 
    300  
    301          
    302         int originalLeftChildIndex = getNode(nodeNum).first; 
    303         int originalRightChildIndex = getNode(nodeNum).second; 
    304  
    305  
     315                originalLeftChildIndex = addNode(leftLeaf); 
     316                originalRightChildIndex = addNode(rightLeaf); 
     317            } 
     318        } 
     319 
     320         
    306321        float restore = bb._max[axis]; 
    307322        bb._max[axis] = mid; 
    308323 
    309324        //osg::notify(osg::NOTICE)<<"  divide leftLeaf "<<kdTree.getNode(nodeNum).first<<std::endl; 
    310         int leftChildIndex = divide(options, bb, originalLeftChildIndex, level+1)
     325        int leftChildIndex = originalLeftChildIndex!=0 ? divide(options, bb, originalLeftChildIndex, level+1) : 0
    311326 
    312327        bb._max[axis] = restore; 
     
    316331 
    317332        //osg::notify(osg::NOTICE)<<"  divide rightLeaf "<<kdTree.getNode(nodeNum).second<<std::endl; 
    318         int rightChildIndex = divide(options, bb, originalRightChildIndex, level+1)
     333        int rightChildIndex = originalRightChildIndex!=0 ? divide(options, bb, originalRightChildIndex, level+1) : 0
    319334         
    320335        bb._min[axis] = restore; 
    321336         
    322         getNode(nodeNum).first = leftChildIndex; 
    323         getNode(nodeNum).second = rightChildIndex;  
    324          
    325         getNode(nodeNum).bb.init(); 
    326         if (leftChildIndex!=0) getNode(nodeNum).bb.expandBy(getBoundingBox(leftChildIndex)); 
    327         if (rightChildIndex!=0) getNode(nodeNum).bb.expandBy(getBoundingBox(rightChildIndex)); 
    328          
    329         return nodeNum; 
    330     } 
    331 
    332  
    333 bool KdTree::intersect(const KdLeaf& leaf, const osg::Vec3& start, const osg::Vec3& end, LineSegmentIntersections& intersections) const 
    334 
    335     osg::Vec3 _s = start; 
    336     osg::Vec3 _d = end - start; 
    337     float _length = _d.length(); 
    338     _d /= _length; 
    339  
    340  
    341     //osg::notify(osg::NOTICE)<<"KdTree::intersect("<<&leaf<<")"<<std::endl; 
    342     bool intersects = false; 
    343     int iend = leaf.first + leaf.second; 
    344     for(int i=leaf.first; i<iend; ++i) 
    345     { 
    346         const Triangle& tri = _triangles[_primitiveIndices[i]]; 
    347         const osg::Vec3& v1 = (*_vertices)[tri._p1]; 
    348         const osg::Vec3& v2 = (*_vertices)[tri._p2]; 
    349         const osg::Vec3& v3 = (*_vertices)[tri._p3]; 
    350         // osg::notify(osg::NOTICE)<<"   tri("<<tri._p1<<","<<tri._p2<<","<<tri._p3<<")"<<std::endl; 
    351  
    352         if (v1==v2 || v2==v3 || v1==v3) continue; 
    353  
    354         osg::Vec3 v12 = v2-v1; 
    355         osg::Vec3 n12 = v12^_d; 
    356         float ds12 = (_s-v1)*n12; 
    357         float d312 = (v3-v1)*n12; 
    358         if (d312>=0.0f) 
    359         { 
    360             if (ds12<0.0f) continue; 
    361             if (ds12>d312) continue; 
    362         } 
    363         else                     // d312 < 0 
    364         { 
    365             if (ds12>0.0f) continue; 
    366             if (ds12<d312) continue; 
    367         } 
    368  
    369         osg::Vec3 v23 = v3-v2; 
    370         osg::Vec3 n23 = v23^_d; 
    371         float ds23 = (_s-v2)*n23; 
    372         float d123 = (v1-v2)*n23; 
    373         if (d123>=0.0f) 
    374         { 
    375             if (ds23<0.0f) continue; 
    376             if (ds23>d123) continue; 
    377         } 
    378         else                     // d123 < 0 
    379         { 
    380             if (ds23>0.0f) continue; 
    381             if (ds23<d123) continue; 
    382         } 
    383  
    384         osg::Vec3 v31 = v1-v3; 
    385         osg::Vec3 n31 = v31^_d; 
    386         float ds31 = (_s-v3)*n31; 
    387         float d231 = (v2-v3)*n31; 
    388         if (d231>=0.0f) 
    389         { 
    390             if (ds31<0.0f) continue; 
    391             if (ds31>d231) continue; 
    392         } 
    393         else                     // d231 < 0 
    394         { 
    395             if (ds31>0.0f) continue; 
    396             if (ds31<d231) continue; 
    397         } 
    398  
    399  
    400         float r3; 
    401         if (ds12==0.0f) r3=0.0f; 
    402         else if (d312!=0.0f) r3 = ds12/d312; 
    403         else continue; // the triangle and the line must be parallel intersection. 
    404  
    405         float r1; 
    406         if (ds23==0.0f) r1=0.0f; 
    407         else if (d123!=0.0f) r1 = ds23/d123; 
    408         else continue; // the triangle and the line must be parallel intersection. 
    409  
    410         float r2; 
    411         if (ds31==0.0f) r2=0.0f; 
    412         else if (d231!=0.0f) r2 = ds31/d231; 
    413         else continue; // the triangle and the line must be parallel intersection. 
    414  
    415         float total_r = (r1+r2+r3); 
    416         if (total_r!=1.0f) 
    417         { 
    418             if (total_r==0.0f) continue; // the triangle and the line must be parallel intersection. 
    419             float inv_total_r = 1.0f/total_r; 
    420             r1 *= inv_total_r; 
    421             r2 *= inv_total_r; 
    422             r3 *= inv_total_r; 
    423         } 
    424  
    425         osg::Vec3 in = v1*r1+v2*r2+v3*r3; 
    426         if (!in.valid()) 
    427         { 
    428             osg::notify(osg::WARN)<<"Warning:: Picked up error in TriangleIntersect"<<std::endl; 
    429             osg::notify(osg::WARN)<<"   ("<<v1<<",\t"<<v2<<",\t"<<v3<<")"<<std::endl; 
    430             osg::notify(osg::WARN)<<"   ("<<r1<<",\t"<<r2<<",\t"<<r3<<")"<<std::endl; 
    431             continue; 
    432         } 
    433  
    434         float d = (in-_s)*_d; 
    435  
    436         if (d<0.0f) continue; 
    437         if (d>_length) continue; 
    438  
    439         osg::Vec3 normal = v12^v23; 
    440         normal.normalize(); 
    441  
    442         float r = d/_length; 
    443  
    444         LineSegmentIntersection intersection; 
    445         intersection.ratio = r; 
    446         intersection.primitiveIndex = _primitiveIndices[i]; 
    447         intersection.intersectionPoint = in; 
    448         intersection.intersectionNormal = normal; 
    449  
    450         intersection.indexList.push_back(tri._p1); 
    451         intersection.indexList.push_back(tri._p2); 
    452         intersection.indexList.push_back(tri._p3); 
    453  
    454         intersection.ratioList.push_back(r1); 
    455         intersection.ratioList.push_back(r2); 
    456         intersection.ratioList.push_back(r3); 
    457          
    458         intersections.insert(intersection); 
    459  
    460         // osg::notify(osg::NOTICE)<<"  got intersection ("<<in<<") ratio="<<r<<std::endl; 
    461  
    462         intersects = true; 
    463     } 
    464      
    465     return intersects; 
    466 
    467  
    468 bool KdTree::intersect(const KdNode& node, const osg::Vec3& start, const osg::Vec3& end, const osg::Vec3& s, const osg::Vec3& e, LineSegmentIntersections& intersections) const 
     337        getNode(nodeIndex).first = leftChildIndex; 
     338        getNode(nodeIndex).second = rightChildIndex;  
     339         
     340        getNode(nodeIndex).bb.init(); 
     341        if (leftChildIndex!=0) getNode(nodeIndex).bb.expandBy(getBoundingBox(leftChildIndex)); 
     342        if (rightChildIndex!=0) getNode(nodeIndex).bb.expandBy(getBoundingBox(rightChildIndex)); 
     343 
     344        if (!getNode(nodeIndex).bb.valid()) 
     345        { 
     346            osg::notify(osg::NOTICE)<<"leftChildIndex="<<leftChildIndex<<" && originalLeftChildIndex="<<originalLeftChildIndex<<std::endl; 
     347            osg::notify(osg::NOTICE)<<"rightChildIndex="<<rightChildIndex<<" && originalRightChildIndex="<<originalRightChildIndex<<std::endl; 
     348 
     349            osg::notify(osg::NOTICE)<<"Invalid BB leftChildIndex="<<leftChildIndex<<", "<<rightChildIndex<<std::endl; 
     350            osg::notify(osg::NOTICE)<<"  bb._min ("<<getNode(nodeIndex).bb._min<<")"<<std::endl; 
     351            osg::notify(osg::NOTICE)<<"  bb._max ("<<getNode(nodeIndex).bb._max<<")"<<std::endl; 
     352             
     353            if (leftChildIndex!=0) 
     354            { 
     355                osg::notify(osg::NOTICE)<<"  getBoundingBox(leftChildIndex) min = "<<getBoundingBox(leftChildIndex)._min<<std::endl; 
     356                osg::notify(osg::NOTICE)<<"                                 max = "<<getBoundingBox(leftChildIndex)._max<<std::endl; 
     357            } 
     358            if (rightChildIndex!=0) 
     359            { 
     360                osg::notify(osg::NOTICE)<<"  getBoundingBox(rightChildIndex) min = "<<getBoundingBox(rightChildIndex)._min<<std::endl; 
     361                osg::notify(osg::NOTICE)<<"                                 max = "<<getBoundingBox(rightChildIndex)._max<<std::endl; 
     362            } 
     363        } 
     364 
     365    } 
     366    else 
     367    { 
     368        osg::notify(osg::NOTICE)<<"NOT expecting to get here"<<std::endl; 
     369    } 
     370     
     371    return nodeIndex; 
     372     
     373
     374 
     375bool KdTree::intersect(const KdNode& node, const RayData& rayData, osg::Vec3 ls, osg::Vec3 le, LineSegmentIntersections& intersections) const 
    469376{ 
    470377    //osg::notify(osg::NOTICE)<<"  Intersect "<<&node<<std::endl; 
    471     //osg::Vec3 ls(start), le(end); 
    472     osg::Vec3 ls(s), le(e); 
     378    if (!intersectAndClip(ls, le, node.bb)) return false; 
     379 
     380#if 0 
     381    { 
     382        osg::notify(osg::NOTICE)<<"Failed intersectAndClip("<<s<<","<<e<<")"<<std::endl; 
     383        osg::notify(osg::NOTICE)<<"  bb._min ("<<node.bb._min<<")"<<std::endl; 
     384        osg::notify(osg::NOTICE)<<"  bb._max ("<<node.bb._max<<")"<<std::endl; 
     385        return false; 
     386    } 
     387#endif 
     388 
    473389    int numIntersectionsBefore = intersections.size(); 
    474     if (intersectAndClip(ls, le, node.bb)) 
    475     { 
    476         if (node.first>0) intersect(getNode(node.first), start, end, ls, le, intersections); 
    477         else if (node.first<0) intersect(getLeaf(node.first), start, end, intersections); 
    478  
    479         if (node.second>0) intersect(getNode(node.second), start, end, ls, le, intersections); 
    480         else if (node.second<0) intersect(getLeaf(node.second), start, end, intersections); 
    481     } 
     390 
     391    if (node.first<0) 
     392    { 
     393        // treat as a leaf 
     394 
     395        //osg::notify(osg::NOTICE)<<"KdTree::intersect("<<&leaf<<")"<<std::endl; 
     396        int istart = -node.first-1; 
     397        int iend = istart + node.second; 
     398         
     399        for(int i=istart; i<iend; ++i) 
     400        { 
     401            const Triangle& tri = _triangles[_primitiveIndices[i]]; 
     402            const osg::Vec3& v1 = (*_vertices)[tri._p1]; 
     403            const osg::Vec3& v2 = (*_vertices)[tri._p2]; 
     404            const osg::Vec3& v3 = (*_vertices)[tri._p3]; 
     405            // osg::notify(osg::NOTICE)<<"   tri("<<tri._p1<<","<<tri._p2<<","<<tri._p3<<")"<<std::endl; 
     406 
     407            if (v1==v2 || v2==v3 || v1==v3) continue; 
     408 
     409            osg::Vec3 v12 = v2-v1; 
     410            osg::Vec3 n12 = v12^rayData._d; 
     411            float ds12 = (rayData._s-v1)*n12; 
     412            float d312 = (v3-v1)*n12; 
     413            if (d312>=0.0f) 
     414            { 
     415                if (ds12<0.0f) continue; 
     416                if (ds12>d312) continue; 
     417            } 
     418            else                     // d312 < 0 
     419            { 
     420                if (ds12>0.0f) continue; 
     421                if (ds12<d312) continue; 
     422            } 
     423 
     424            osg::Vec3 v23 = v3-v2; 
     425            osg::Vec3 n23 = v23^rayData._d; 
     426            float ds23 = (rayData._s-v2)*n23; 
     427            float d123 = (v1-v2)*n23; 
     428            if (d123>=0.0f) 
     429            { 
     430                if (ds23<0.0f) continue; 
     431                if (ds23>d123) continue; 
     432            } 
     433            else                     // d123 < 0 
     434            { 
     435                if (ds23>0.0f) continue; 
     436                if (ds23<d123) continue; 
     437            } 
     438 
     439            osg::Vec3 v31 = v1-v3; 
     440            osg::Vec3 n31 = v31^rayData._d; 
     441            float ds31 = (rayData._s-v3)*n31; 
     442            float d231 = (v2-v3)*n31; 
     443            if (d231>=0.0f) 
     444            { 
     445                if (ds31<0.0f) continue; 
     446                if (ds31>d231) continue; 
     447            } 
     448            else                     // d231 < 0 
     449            { 
     450                if (ds31>0.0f) continue; 
     451                if (ds31<d231) continue; 
     452            } 
     453 
     454 
     455            float r3; 
     456            if (ds12==0.0f) r3=0.0f; 
     457            else if (d312!=0.0f) r3 = ds12/d312; 
     458            else continue; // the triangle and the line must be parallel intersection. 
     459 
     460            float r1; 
     461            if (ds23==0.0f) r1=0.0f; 
     462            else if (d123!=0.0f) r1 = ds23/d123; 
     463            else continue; // the triangle and the line must be parallel intersection. 
     464 
     465            float r2; 
     466            if (ds31==0.0f) r2=0.0f; 
     467            else if (d231!=0.0f) r2 = ds31/d231; 
     468            else continue; // the triangle and the line must be parallel intersection. 
     469 
     470            float total_r = (r1+r2+r3); 
     471            if (total_r!=1.0f) 
     472            { 
     473                if (total_r==0.0f) continue; // the triangle and the line must be parallel intersection. 
     474                float inv_total_r = 1.0f/total_r; 
     475                r1 *= inv_total_r; 
     476                r2 *= inv_total_r; 
     477                r3 *= inv_total_r; 
     478            } 
     479 
     480            osg::Vec3 in = v1*r1+v2*r2+v3*r3; 
     481            if (!in.valid()) 
     482            { 
     483                osg::notify(osg::WARN)<<"Warning:: Picked up error in TriangleIntersect"<<std::endl; 
     484                osg::notify(osg::WARN)<<"   ("<<v1<<",\t"<<v2<<",\t"<<v3<<")"<<std::endl; 
     485                osg::notify(osg::WARN)<<"   ("<<r1<<",\t"<<r2<<",\t"<<r3<<")"<<std::endl; 
     486                continue; 
     487            } 
     488 
     489            float d = (in-rayData._s)*rayData._d; 
     490 
     491            if (d<0.0f) continue; 
     492            if (d>rayData._length) continue; 
     493 
     494            osg::Vec3 normal = v12^v23; 
     495            normal.normalize(); 
     496 
     497            float r = d* rayData._inverse_length; 
     498 
     499            LineSegmentIntersection intersection; 
     500            intersection.ratio = r; 
     501            intersection.primitiveIndex = _primitiveIndices[i]; 
     502            intersection.intersectionPoint = in; 
     503            intersection.intersectionNormal = normal; 
     504 
     505            intersection.indexList.push_back(tri._p1); 
     506            intersection.indexList.push_back(tri._p2); 
     507            intersection.indexList.push_back(tri._p3); 
     508 
     509            intersection.ratioList.push_back(r1); 
     510            intersection.ratioList.push_back(r2); 
     511            intersection.ratioList.push_back(r3); 
     512 
     513            intersections.insert(intersection); 
     514 
     515            // osg::notify(osg::NOTICE)<<"  got intersection ("<<in<<") ratio="<<r<<std::endl; 
     516        } 
     517    } 
     518    else 
     519    { 
     520        if (node.first>0) intersect(getNode(node.first), rayData, ls, le, intersections); 
     521        if (node.second>0) intersect(getNode(node.second), rayData, ls, le, intersections); 
     522    } 
     523 
    482524    return numIntersectionsBefore != intersections.size(); 
    483525} 
     
    485527bool KdTree::intersectAndClip(osg::Vec3& s, osg::Vec3& e, const osg::BoundingBox& bb) const 
    486528{ 
     529    //return true; 
     530 
     531    //if (!bb.valid()) return true; 
     532 
    487533    // compate s and e against the xMin to xMax range of bb. 
    488534    if (s.x()<=e.x()) 
     
    608654bool KdTree::intersect(const osg::Vec3& start, const osg::Vec3& end, LineSegmentIntersections& intersections) const 
    609655{ 
    610     osg::Vec3 s(start), e(end); 
    611     return intersect(getNode(0), start, end, s,e, intersections); 
     656    RayData rayData(start,end); 
     657    return intersect(getNode(0), rayData, start, end, intersections); 
    612658} 
    613659