Changeset 8557

Show
Ignore:
Timestamp:
07/11/08 18:48:39
Author:
robert
Message:

Moved the building and intersecting of the KdTree? into the .cpp, and cleaned up
the header to ready it for wider usage

Files:

Legend:

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

    r8556 r8557  
    1717#include <osg/Shape> 
    1818#include <osg/Geometry> 
     19 
     20#include <map> 
    1921 
    2022namespace osg 
     
    5153            LineSegmentIntersection(): 
    5254                ratio(-1.0), 
     55                p0(0), 
     56                p1(0), 
     57                p2(0), 
     58                r0(0.0f), 
     59                r1(0.0f), 
     60                r2(0.0f), 
    5361                primitiveIndex(0) {} 
    5462 
     
    6169            osg::Vec3d                      intersectionPoint; 
    6270            osg::Vec3                       intersectionNormal; 
    63             IndexList                       indexList; 
    64             RatioList                       ratioList; 
     71 
     72            unsigned int                    p0; 
     73            unsigned int                    p1; 
     74            unsigned int                    p2; 
     75            float                           r0; 
     76            float                           r1; 
     77            float                           r2; 
     78 
    6579            unsigned int                    primitiveIndex; 
    6680        };         
    6781         
    6882 
    69         typedef std::multiset<LineSegmentIntersection> LineSegmentIntersections; 
     83        typedef std::vector<LineSegmentIntersection> LineSegmentIntersections; 
    7084         
    7185        /** compute the intersection of a line segment and the kdtree, return true if an intersection has been found.*/ 
     
    7387 
    7488 
    75  
    7689        typedef int value_type; 
    77         typedef std::vector< value_type >   Indices; 
    7890 
    7991        struct KdNode 
     
    95107        struct Triangle 
    96108        { 
    97             Triangle(unsigned int p1, unsigned int p2, unsigned int p3): 
    98                 _p1(p1), _p2(p2), _p3(p3) {} 
     109            Triangle(): 
     110                p0(0),p1(0),p2(0) {} 
     111         
     112            Triangle(unsigned int ip0, unsigned int ip1, unsigned int ip2): 
     113                p0(ip0), p1(ip1), p2(ip2) {} 
    99114 
    100115            bool operator < (const Triangle& rhs) const 
    101116            { 
    102                 if (_p1<rhs._p1) return true; 
    103                 if (_p1>rhs._p1) return false; 
    104                 if (_p2<rhs._p2) return true; 
    105                 if (_p2>rhs._p2) return false; 
    106                 return _p3<rhs._p3
     117                if (p0<rhs.p0) return true; 
     118                if (p0>rhs.p0) return false; 
     119                if (p1<rhs.p1) return true; 
     120                if (p1>rhs.p1) return false; 
     121                return p2<rhs.p2
    107122            } 
    108123 
    109             unsigned int _p1
    110             unsigned int _p2
    111             unsigned int _p3;     
     124            unsigned int p0
     125            unsigned int p1
     126            unsigned int p2; 
    112127        }; 
    113128 
    114         typedef std::vector< unsigned int > AxisStack; 
    115129        typedef std::vector< KdNode >       KdNodeList; 
     130        typedef std::vector< Triangle >     TriangleList; 
    116131 
    117132        int addNode(const KdNode& node) 
     
    122137        } 
    123138 
    124         /// note, nodeNum is positive to distinguish from leftNum 
    125         KdNode& getNode(int nodeNum) 
     139        KdNode& getNode(int nodeNum) { return _kdNodes[nodeNum]; } 
     140        const KdNode& getNode(int nodeNum) const { return _kdNodes[nodeNum]; } 
     141         
     142        KdNodeList& getNodes() { return _kdNodes; } 
     143        const KdNodeList& getNodes() const { return _kdNodes; } 
     144 
     145        void setVertices(osg::Vec3Array* vertices) { _vertices = vertices; } 
     146        const osg::Vec3Array* getVertices() const { return _vertices.get(); } 
     147 
     148        unsigned int addTriangle(const Triangle& tri) 
    126149        { 
    127             if (nodeNum<0 || nodeNum>static_cast<int>(_kdNodes.size())-1) 
    128             { 
    129                 osg::notify(osg::NOTICE)<<"Warning: getNode("<<nodeNum<<") _kdNodes.size()="<<_kdNodes.size()<<std::endl; 
    130             } 
    131             return _kdNodes[nodeNum]; 
     150            unsigned int num = _triangles.size(); 
     151            _triangles.push_back(tri); 
     152            return num; 
    132153        } 
    133154 
    134         /// note, nodeNum is positive to distinguish from leftNum 
    135         const KdNode& getNode(int nodeNum) const 
    136         { 
    137             if (nodeNum<0 || nodeNum>static_cast<int>(_kdNodes.size())-1) 
    138             { 
    139                 osg::notify(osg::NOTICE)<<"Warning: getNode("<<nodeNum<<") _kdNodes.size()="<<_kdNodes.size()<<std::endl; 
    140             } 
    141             return _kdNodes[nodeNum]; 
    142         } 
    143          
    144         osg::BoundingBox& getBoundingBox(int nodeNum) 
    145         { 
    146             return _kdNodes[nodeNum].bb; 
    147         } 
     155        Triangle& getTriangle(unsigned int i) { return _triangles[i]; } 
     156        const Triangle& getTriangle(unsigned int i) const { return _triangles[i]; } 
     157 
     158        TriangleList& getTriangles() { return _triangles; } 
     159        const TriangleList& getTriangles() const { return _triangles; } 
    148160 
    149161 
    150         void computeDivisions(BuildOptions& options); 
    151  
    152         int divide(BuildOptions& options, osg::BoundingBox& bb, int nodeIndex, unsigned int level); 
    153  
    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; 
    176         bool intersectAndClip(osg::Vec3& s, osg::Vec3& e, const osg::BoundingBox& bb) const; 
    177  
    178         typedef std::vector< osg::BoundingBox >                 BoundingBoxList; 
    179         typedef std::vector< Triangle >                         TriangleList; 
    180         typedef std::vector< osg::Vec3 >                        CenterList; 
    181  
    182         osg::observer_ptr<osg::Geometry>    _geometry; 
    183  
    184         osg::BoundingBox                    _bb; 
    185  
    186         AxisStack                           _axisStack; 
    187         KdNodeList                          _kdNodes; 
     162    protected: 
    188163 
    189164        osg::ref_ptr<osg::Vec3Array>        _vertices; 
    190          
    191         Indices                             _primitiveIndices; 
    192          
     165        KdNodeList                          _kdNodes; 
    193166        TriangleList                        _triangles; 
    194         CenterList                          _centers; 
    195167 
    196168}; 
     
    211183 
    212184        osg::ref_ptr<osg::KdTree> _kdTreePrototype; 
     185 
     186 
    213187                 
    214188    protected: 
  • OpenSceneGraph/trunk/src/osg/KdTree.cpp

    r8556 r8557  
    2525//////////////////////////////////////////////////////////////////////////////// 
    2626// 
    27 // Functor for collecting triangle indices from Geometry 
    28  
    29 struct TriangleIndicesCollector 
    30 
    31     TriangleIndicesCollector(): 
    32         _kdTree(0) 
    33     { 
    34     } 
    35  
    36     inline void operator () (unsigned int p1, unsigned int p2, unsigned int p3) 
    37     { 
    38         unsigned int i = _kdTree->_triangles.size(); 
    39         _kdTree->_triangles.push_back(KdTree::Triangle(p1,p2,p3)); 
    40          
    41         osg::BoundingBox bb; 
    42         bb.expandBy((*(_kdTree->_vertices))[p1]); 
    43         bb.expandBy((*(_kdTree->_vertices))[p2]); 
    44         bb.expandBy((*(_kdTree->_vertices))[p3]); 
    45  
    46         _kdTree->_centers.push_back(bb.center()); 
    47         _kdTree->_primitiveIndices.push_back(i); 
    48          
    49     } 
    50      
    51     KdTree* _kdTree; 
     27// BuildKdTree Declarartion - class used for building an single KdTree 
     28 
     29struct BuildKdTree 
     30
     31    BuildKdTree(KdTree& kdTree): 
     32        _kdTree(kdTree) {} 
     33 
     34    typedef std::vector< osg::Vec3 >            CenterList; 
     35    typedef std::vector< unsigned int >           Indices; 
     36    typedef std::vector< unsigned int >         AxisStack; 
     37 
     38    bool build(KdTree::BuildOptions& options, osg::Geometry* geometry); 
     39 
     40    void computeDivisions(KdTree::BuildOptions& options); 
     41 
     42    int divide(KdTree::BuildOptions& options, osg::BoundingBox& bb, int nodeIndex, unsigned int level); 
     43 
     44    KdTree&             _kdTree; 
     45 
     46    osg::BoundingBox    _bb; 
     47    AxisStack           _axisStack; 
     48    Indices             _primitiveIndices; 
     49    CenterList          _centers; 
    5250 
    5351}; 
     
    5553//////////////////////////////////////////////////////////////////////////////// 
    5654// 
    57 // KdTree 
    58  
    59 KdTree::BuildOptions::BuildOptions(): 
    60         _numVerticesProcessed(0), 
    61         _targetNumTrianglesPerLeaf(2), 
    62         _maxNumLevels(32) 
    63 
    64 
     55// Functor for collecting triangle indices from Geometry 
     56 
     57struct TriangleIndicesCollector 
     58
     59    TriangleIndicesCollector(): 
     60        _buildKdTree(0) 
     61    { 
     62    } 
     63 
     64    inline void operator () (unsigned int p0, unsigned int p1, unsigned int p2) 
     65    { 
     66        const osg::Vec3& v0 = (*(_buildKdTree->_kdTree.getVertices()))[p0]; 
     67        const osg::Vec3& v1 = (*(_buildKdTree->_kdTree.getVertices()))[p1]; 
     68        const osg::Vec3& v2 = (*(_buildKdTree->_kdTree.getVertices()))[p2]; 
     69     
     70        // discard degenerate points 
     71        if (v0==v1 || v1==v2 || v1==v2) 
     72        { 
     73            //osg::notify(osg::NOTICE)<<"Disgarding degenerate triangle"<<std::endl; 
     74            return; 
     75        } 
     76 
     77        unsigned int i = _buildKdTree->_kdTree.addTriangle(KdTree::Triangle(p0,p1,p2)); 
     78         
     79        osg::BoundingBox bb; 
     80        bb.expandBy(v0); 
     81        bb.expandBy(v1); 
     82        bb.expandBy(v2); 
     83 
     84        _buildKdTree->_centers.push_back(bb.center()); 
     85        _buildKdTree->_primitiveIndices.push_back(i); 
     86         
     87    } 
     88     
     89    BuildKdTree* _buildKdTree; 
     90 
     91}; 
     92 
    6593 
    6694//////////////////////////////////////////////////////////////////////////////// 
    6795// 
    68 // KdTree 
    69  
    70 KdTree::KdTree() 
    71 
    72 
    73  
    74 KdTree::KdTree(const KdTree& rhs, const osg::CopyOp& copyop): 
    75     Shape(rhs) 
    76 
    77 
    78  
    79 bool KdTree::build(BuildOptions& options, osg::Geometry* geometry) 
    80 
     96// BuildKdTree Implementation 
     97 
     98bool BuildKdTree::build(KdTree::BuildOptions& options, osg::Geometry* geometry) 
     99
     100     
    81101#ifdef VERBOSE_OUTPUT     
    82     osg::notify(osg::NOTICE)<<"osg::KDTreeBuilder::createKDTree()"<<std::endl; 
     102    osg::notify(osg::NOTICE)<<"osg::KDTreeBuilder::createKDTree()"<<std::endl;146 
    83103#endif 
    84104 
     
    88108    if (vertices->size() <= options._targetNumTrianglesPerLeaf) return false; 
    89109 
    90     _geometry = geometry; 
    91     _bb = _geometry->getBound(); 
    92     _vertices = vertices; 
     110    _bb = geometry->getBound(); 
     111    _kdTree.setVertices(vertices); 
    93112     
    94113    unsigned int estimatedSize = (unsigned int)(2.0*float(vertices->size())/float(options._targetNumTrianglesPerLeaf)); 
     
    98117#endif 
    99118 
    100     _kdNodes.reserve(estimatedSize*5); 
     119    _kdTree.getNodes().reserve(estimatedSize*5); 
    101120     
    102121    computeDivisions(options); 
     
    106125    unsigned int estimatedNumTriangles = vertices->size()*2; 
    107126    _primitiveIndices.reserve(estimatedNumTriangles); 
    108     _triangles.reserve(estimatedNumTriangles); 
    109127    _centers.reserve(estimatedNumTriangles); 
    110128 
    111  
     129    _kdTree.getTriangles().reserve(estimatedNumTriangles); 
    112130 
    113131    osg::TriangleIndexFunctor<TriangleIndicesCollector> collectTriangleIndices; 
    114     collectTriangleIndices._kdTree = this; 
     132    collectTriangleIndices._buildKdTree = this; 
    115133    geometry->accept(collectTriangleIndices); 
    116134 
    117135    _primitiveIndices.reserve(vertices->size()); 
    118136 
    119     KdNode node(-1, _primitiveIndices.size()); 
     137    KdTree::KdNode node(-1, _primitiveIndices.size()); 
    120138    node.bb = _bb; 
    121139 
    122     int nodeNum = addNode(node); 
     140    int nodeNum = _kdTree.addNode(node); 
    123141 
    124142    osg::BoundingBox bb = _bb; 
    125143    nodeNum = divide(options, bb, nodeNum, 0); 
    126144     
     145    // now reorder the triangle list so that it's in order as per the primitiveIndex list. 
     146    KdTree::TriangleList triangleList(_kdTree.getTriangles().size()); 
     147    for(unsigned int i=0; i<_primitiveIndices.size(); ++i) 
     148    { 
     149        triangleList[i] = _kdTree.getTriangle(_primitiveIndices[i]); 
     150    } 
     151     
     152    _kdTree.getTriangles().swap(triangleList); 
     153     
     154     
    127155#ifdef VERBOSE_OUTPUT     
    128156    osg::notify(osg::NOTICE)<<"Root nodeNum="<<nodeNum<<std::endl; 
     
    134162 
    135163 
    136     return !_kdNodes.empty(); 
    137 } 
    138  
    139 void KdTree::computeDivisions(BuildOptions& options) 
     164    return !_kdTree.getNodes().empty(); 
     165} 
     166 
     167void BuildKdTree::computeDivisions(KdTree::BuildOptions& options) 
    140168{ 
    141169    osg::Vec3 dimensions(_bb.xMax()-_bb.xMin(), 
     
    173201} 
    174202 
    175 int KdTree::divide(BuildOptions& options, osg::BoundingBox& bb, int nodeIndex, unsigned int level) 
    176 { 
    177     KdNode& node = getNode(nodeIndex); 
     203int BuildKdTree::divide(KdTree::BuildOptions& options, osg::BoundingBox& bb, int nodeIndex, unsigned int level) 
     204{ 
     205    KdTree::KdNode& node = _kdTree.getNode(nodeIndex); 
    178206 
    179207    bool needToDivide = level < _axisStack.size() && 
     
    191219            for(int i=istart; i<=iend; ++i) 
    192220            { 
    193                 const Triangle& tri = _triangles[_primitiveIndices[i]]; 
    194                 const osg::Vec3& v1 = (*_vertices)[tri._p1]; 
    195                 const osg::Vec3& v2 = (*_vertices)[tri._p2]; 
    196                 const osg::Vec3& v3 = (*_vertices)[tri._p3]; 
     221                const KdTree::Triangle& tri = _kdTree.getTriangle(_primitiveIndices[i]); 
     222                const osg::Vec3& v0 = (*_kdTree.getVertices())[tri.p0]; 
     223                const osg::Vec3& v1 = (*_kdTree.getVertices())[tri.p1]; 
     224                const osg::Vec3& v2 = (*_kdTree.getVertices())[tri.p2]; 
     225                node.bb.expandBy(v0); 
    197226                node.bb.expandBy(v1); 
    198227                node.bb.expandBy(v2); 
    199                 node.bb.expandBy(v3); 
    200228                 
    201229                float epsilon = 1e-6; 
     
    248276        int originalLeftChildIndex = 0; 
    249277        int originalRightChildIndex = 0; 
     278        bool insitueDivision = false; 
    250279 
    251280        { 
     
    276305            } 
    277306             
    278             KdNode leftLeaf(-istart-1, (right-istart)+1); 
    279             KdNode rightLeaf(-left-1, (iend-left)+1); 
     307            KdTree::KdNode leftLeaf(-istart-1, (right-istart)+1); 
     308            KdTree::KdNode rightLeaf(-left-1, (iend-left)+1); 
    280309 
    281310#if 0 
     
    303332                //osg::notify(osg::NOTICE)<<"LeftLeaf empty"<<std::endl; 
    304333                originalLeftChildIndex = 0; 
    305                 originalRightChildIndex = addNode(rightLeaf); 
     334                //originalRightChildIndex = addNode(rightLeaf); 
     335                originalRightChildIndex = nodeIndex; 
     336                insitueDivision = true; 
    306337            } 
    307338            else if (rightLeaf.second<=0) 
    308339            { 
    309340                //osg::notify(osg::NOTICE)<<"RightLeaf empty"<<std::endl; 
    310                 originalLeftChildIndex = addNode(leftLeaf); 
     341                // originalLeftChildIndex = addNode(leftLeaf); 
     342                originalLeftChildIndex = nodeIndex; 
    311343                originalRightChildIndex = 0; 
     344                insitueDivision = true; 
    312345            } 
    313346            else 
    314347            { 
    315                 originalLeftChildIndex = addNode(leftLeaf); 
    316                 originalRightChildIndex = addNode(rightLeaf); 
     348                originalLeftChildIndex = _kdTree.addNode(leftLeaf); 
     349                originalRightChildIndex = _kdTree.addNode(rightLeaf); 
    317350            } 
    318351        } 
     
    335368        bb._min[axis] = restore; 
    336369         
    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  
     370 
     371        if (!insitueDivision) 
     372        { 
     373            // take a second reference to node we are working on as the std::vector<> resize could 
     374            // have invalidate the previous node ref. 
     375            KdTree::KdNode& newNodeRef = _kdTree.getNode(nodeIndex); 
     376         
     377            newNodeRef.first = leftChildIndex; 
     378            newNodeRef.second = rightChildIndex;  
     379 
     380            insitueDivision = true; 
     381 
     382            newNodeRef.bb.init(); 
     383            if (leftChildIndex!=0) newNodeRef.bb.expandBy(_kdTree.getNode(leftChildIndex).bb); 
     384            if (rightChildIndex!=0) newNodeRef.bb.expandBy(_kdTree.getNode(rightChildIndex).bb); 
     385 
     386            if (!newNodeRef.bb.valid()) 
     387            { 
     388                osg::notify(osg::NOTICE)<<"leftChildIndex="<<leftChildIndex<<" && originalLeftChildIndex="<<originalLeftChildIndex<<std::endl; 
     389                osg::notify(osg::NOTICE)<<"rightChildIndex="<<rightChildIndex<<" && originalRightChildIndex="<<originalRightChildIndex<<std::endl; 
     390 
     391                osg::notify(osg::NOTICE)<<"Invalid BB leftChildIndex="<<leftChildIndex<<", "<<rightChildIndex<<std::endl; 
     392                osg::notify(osg::NOTICE)<<"  bb._min ("<<newNodeRef.bb._min<<")"<<std::endl; 
     393                osg::notify(osg::NOTICE)<<"  bb._max ("<<newNodeRef.bb._max<<")"<<std::endl; 
     394 
     395                if (leftChildIndex!=0) 
     396                { 
     397                    osg::notify(osg::NOTICE)<<"  getNode(leftChildIndex).bb min = "<<_kdTree.getNode(leftChildIndex).bb._min<<std::endl; 
     398                    osg::notify(osg::NOTICE)<<"                                 max = "<<_kdTree.getNode(leftChildIndex).bb._max<<std::endl; 
     399                } 
     400                if (rightChildIndex!=0) 
     401                { 
     402                    osg::notify(osg::NOTICE)<<"  getNode(rightChildIndex).bb min = "<<_kdTree.getNode(rightChildIndex).bb._min<<std::endl; 
     403                    osg::notify(osg::NOTICE)<<"                              max = "<<_kdTree.getNode(rightChildIndex).bb._max<<std::endl; 
     404                } 
     405            } 
     406        } 
    365407    } 
    366408    else 
     
    373415} 
    374416 
    375 bool KdTree::intersect(const KdNode& node, const RayData& rayData, osg::Vec3 ls, osg::Vec3 le, LineSegmentIntersections& intersections) const 
    376 
    377     //osg::notify(osg::NOTICE)<<"  Intersect "<<&node<<std::endl; 
    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  
    389     int numIntersectionsBefore = intersections.size(); 
    390  
     417//////////////////////////////////////////////////////////////////////////////// 
     418// 
     419// IntersectKdTree 
     420// 
     421struct IntersectKdTree 
     422
     423    IntersectKdTree(const osg::Vec3Array& vertices, 
     424                    const KdTree::KdNodeList& nodes, 
     425                    const KdTree::TriangleList& triangles, 
     426                    KdTree::LineSegmentIntersections& intersections, 
     427                    const osg::Vec3& s, const osg::Vec3& e): 
     428                        _vertices(vertices), 
     429                        _kdNodes(nodes), 
     430                        _triangles(triangles), 
     431                        _intersections(intersections), 
     432                        _s(s), 
     433                        _e(e) 
     434    { 
     435        _d = e - s; 
     436        _length = _d.length(); 
     437        _inverse_length = 1.0f/_length; 
     438        _d *= _inverse_length; 
     439    } 
     440 
     441    void intersect(const KdTree::KdNode& node, const osg::Vec3& s, const osg::Vec3& e) const; 
     442    bool intersectAndClip(osg::Vec3& s, osg::Vec3& e, const osg::BoundingBox& bb) const; 
     443 
     444    const osg::Vec3Array&               _vertices; 
     445    const KdTree::KdNodeList&           _kdNodes; 
     446    const KdTree::TriangleList&         _triangles; 
     447    KdTree::LineSegmentIntersections&   _intersections; 
     448 
     449    osg::Vec3 _s; 
     450    osg::Vec3 _e; 
     451 
     452    osg::Vec3 _d; 
     453    float     _length; 
     454    float     _inverse_length; 
     455}; 
     456 
     457 
     458void IntersectKdTree::intersect(const KdTree::KdNode& node, const osg::Vec3& ls, const osg::Vec3& le) const 
     459
    391460    if (node.first<0) 
    392461    { 
    393462        // treat as a leaf 
    394  
     463         
    395464        //osg::notify(osg::NOTICE)<<"KdTree::intersect("<<&leaf<<")"<<std::endl; 
    396465        int istart = -node.first-1; 
     
    399468        for(int i=istart; i<iend; ++i) 
    400469        { 
    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; 
     470            //const Triangle& tri = _triangles[_primitiveIndices[i]]; 
     471            const KdTree::Triangle& tri = _triangles[i]; 
     472            // osg::notify(osg::NOTICE)<<"   tri("<<tri.p1<<","<<tri.p2<<","<<tri.p3<<")"<<std::endl; 
     473 
     474            const osg::Vec3& v0 = _vertices[tri.p0]; 
     475            const osg::Vec3& v1 = _vertices[tri.p1]; 
     476            const osg::Vec3& v2 = _vertices[tri.p2]; 
     477 
     478            osg::Vec3 T = _s - v0; 
     479            osg::Vec3 E2 = v2 - v0; 
     480            osg::Vec3 E1 = v1 - v0; 
     481             
     482            osg::Vec3 P =  _d ^ E2; 
     483             
     484            float det = P * E1; 
     485             
     486            float r,r0,r1,r2; 
     487             
     488            const float esplison = 1e-10; 
     489            if (det>esplison) 
     490            { 
     491                float u = (P*T); 
     492                if (u<0.0 || u>det) continue; 
     493                 
     494                osg::Vec3 Q = T ^ E1; 
     495                float v = (Q*_d); 
     496                if (v<0.0 || v>det) continue; 
     497                 
     498                if ((u+v)> det) continue; 
     499 
     500                float inv_det = 1.0f/det;                 
     501                float t = (Q*E2)*inv_det;             
     502                if (t<0.0 || t>_length) continue; 
     503 
     504                u *= inv_det; 
     505                v *= inv_det; 
     506 
     507                r0 = 1.0f-u-v; 
     508                r1 = u; 
     509                r2 = v;  
     510                r = t * _inverse_length; 
     511            } 
     512            else if (det<-esplison) 
     513            { 
     514 
     515                float u = (P*T); 
     516                if (u>0.0 || u<det) continue; 
     517                 
     518                osg::Vec3 Q = T ^ E1; 
     519                float v = (Q*_d); 
     520                if (v>0.0 || v<det) continue; 
     521                 
     522                if ((u+v) < det) continue; 
     523 
     524                float inv_det = 1.0f/det;                 
     525                float t = (Q*E2)*inv_det;             
     526                if (t<0.0 || t>_length) continue; 
     527 
     528                u *= inv_det; 
     529                v *= inv_det; 
     530 
     531                r0 = 1.0f-u-v; 
     532                r1 = u; 
     533                r2 = v; 
     534                r = t * _inverse_length; 
     535            } 
     536            else 
     537            { 
    486538                continue; 
    487539            } 
    488540 
    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; 
     541            osg::Vec3 in = v0*r0 + v1*r1 + v2*r2; 
     542            osg::Vec3 normal = E1^E2; 
    495543            normal.normalize(); 
    496  
    497             float r = d* rayData._inverse_length; 
    498  
    499             LineSegmentIntersection intersection; 
     544             
     545#if 1 
     546            _intersections.push_back(KdTree::LineSegmentIntersection()); 
     547            KdTree::LineSegmentIntersection& intersection = _intersections.back(); 
     548 
    500549            intersection.ratio = r; 
    501             intersection.primitiveIndex = _primitiveIndices[i]
     550            intersection.primitiveIndex = i
    502551            intersection.intersectionPoint = in; 
    503552            intersection.intersectionNormal = normal; 
    504553 
    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  
     554            intersection.p0 = tri.p0; 
     555            intersection.p1 = tri.p1; 
     556            intersection.p2 = tri.p2; 
     557            intersection.r0 = r0; 
     558            intersection.r1 = r1; 
     559            intersection.r2 = r1; 
     560 
     561#endif 
    515562            // osg::notify(osg::NOTICE)<<"  got intersection ("<<in<<") ratio="<<r<<std::endl; 
    516563        } 
     
    518565    else 
    519566    { 
    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  
    524     return numIntersectionsBefore != intersections.size(); 
    525 
    526  
    527 bool KdTree::intersectAndClip(osg::Vec3& s, osg::Vec3& e, const osg::BoundingBox& bb) const 
     567        if (node.first>0) 
     568        { 
     569            osg::Vec3 l(ls), e(le); 
     570            if (intersectAndClip(l,e, _kdNodes[node.first].bb)) 
     571            { 
     572                intersect(_kdNodes[node.first], l, e); 
     573            } 
     574        } 
     575        if (node.second>0) 
     576        { 
     577            osg::Vec3 l(ls), e(le); 
     578            if (intersectAndClip(l,e, _kdNodes[node.second].bb)) 
     579            { 
     580                intersect(_kdNodes[node.second], l, e); 
     581            } 
     582        } 
     583    } 
     584
     585 
     586bool IntersectKdTree::intersectAndClip(osg::Vec3& s, osg::Vec3& e, const osg::BoundingBox& bb) const 
    528587{ 
    529588    //return true; 
     
    652711} 
    653712 
     713 
     714//////////////////////////////////////////////////////////////////////////////// 
     715// 
     716// KdTree::BuildOptions 
     717 
     718KdTree::BuildOptions::BuildOptions(): 
     719        _numVerticesProcessed(0), 
     720        _targetNumTrianglesPerLeaf(4), 
     721        _maxNumLevels(32) 
     722{ 
     723} 
     724 
     725//////////////////////////////////////////////////////////////////////////////// 
     726// 
     727// KdTree 
     728 
     729KdTree::KdTree() 
     730{ 
     731} 
     732 
     733KdTree::KdTree(const KdTree& rhs, const osg::CopyOp& copyop): 
     734    Shape(rhs) 
     735{ 
     736} 
     737 
     738bool KdTree::build(BuildOptions& options, osg::Geometry* geometry) 
     739{ 
     740    BuildKdTree build(*this); 
     741    return build.build(options, geometry); 
     742} 
     743 
    654744bool KdTree::intersect(const osg::Vec3& start, const osg::Vec3& end, LineSegmentIntersections& intersections) const 
    655745{ 
    656     RayData rayData(start,end); 
    657     return intersect(getNode(0), rayData, start, end, intersections); 
     746    int numIntersectionsBefore = intersections.size(); 
     747 
     748    IntersectKdTree intersector(*_vertices, 
     749                                _kdNodes, 
     750                                _triangles, 
     751                                intersections, 
     752                                start, end); 
     753                     
     754    intersector.intersect(getNode(0), start, end); 
     755     
     756    return numIntersectionsBefore != intersections.size(); 
    658757} 
    659758