From d3fd43cc6f572d3adc14687b7700b1c825043aea Mon Sep 17 00:00:00 2001
From: Robert Osfield <robert@openscenegraph.com>
Date: Thu, 10 Jul 2008 15:50:10 +0000
Subject: [PATCH] Streamlined KdTree implementation

---
 include/osg/KdTree |  95 +++------
 src/osg/KdTree.cpp | 478 +++++++++++++++++++++++++--------------------
 2 files changed, 286 insertions(+), 287 deletions(-)

diff --git a/include/osg/KdTree b/include/osg/KdTree
index 49ff58f3a..baf4c5b43 100644
--- a/include/osg/KdTree
+++ b/include/osg/KdTree
@@ -91,24 +91,7 @@ class OSG_EXPORT KdTree : public osg::Shape
             value_type first;    
             value_type second;
         };
-
-
-        struct KdLeaf
-        {
-            KdLeaf():
-                first(0),
-                second(0) {}
-
-            KdLeaf(value_type f, value_type s):
-                first(f),
-                second(s) {}
-
-            osg::BoundingBox bb;
-
-            value_type first;    
-            value_type second;
-        };
-
+    
         struct Triangle
         {
             Triangle(unsigned int p1, unsigned int p2, unsigned int p3):
@@ -130,46 +113,6 @@ class OSG_EXPORT KdTree : public osg::Shape
 
         typedef std::vector< unsigned int > AxisStack;
         typedef std::vector< KdNode >       KdNodeList;
-        typedef std::vector< KdLeaf >       KdLeafList;
-
-        /// note, leafNum is negative to distinguish from nodeNum
-        int addLeaf(const KdLeaf& leaf) { int num = _kdLeaves.size(); _kdLeaves.push_back(leaf); return -(num+1); }
-
-        int replaceLeaf(int leafNum, const KdLeaf& leaf)
-        {
-            int num = -leafNum-1; 
-            
-            if (num>static_cast<int>(_kdLeaves.size())-1)
-            {
-                osg::notify(osg::NOTICE)<<"Warning: replaceChild("<<leafNum<<", leaf), num = "<<num<<" _kdLeaves.size()="<<_kdLeaves.size()<<std::endl;
-                return leafNum;
-            }
-            
-            _kdLeaves[num] = leaf; return leafNum;
-        }
-
-        /// note, leafNum is negative to distinguish from nodeNum
-        KdLeaf& getLeaf(int leafNum)
-        {
-            int num = -leafNum-1;
-            if (num<0 || num>static_cast<int>(_kdLeaves.size())-1)
-            {
-                osg::notify(osg::NOTICE)<<"Warning: getLeaf("<<leafNum<<", num = "<<num<<") _kdLeaves.size()="<<_kdLeaves.size()<<std::endl;
-            }
-
-            return _kdLeaves[num];
-        }
-
-        const KdLeaf& getLeaf(int leafNum) const
-        {
-            int num = -leafNum-1;
-            if (num<0 || num>static_cast<int>(_kdLeaves.size())-1)
-            {
-                osg::notify(osg::NOTICE)<<"Warning: getLeaf("<<leafNum<<", num = "<<num<<") _kdLeaves.size()="<<_kdLeaves.size()<<std::endl;
-            }
-
-            return _kdLeaves[num];
-        }
 
         int addNode(const KdNode& node)
         {
@@ -200,15 +143,7 @@ class OSG_EXPORT KdTree : public osg::Shape
         
         osg::BoundingBox& getBoundingBox(int nodeNum)
         {
-            if (nodeNum<0)
-            {
-                int num = -nodeNum-1;
-                return _kdLeaves[num].bb;            
-            }
-            else
-            {
-                return _kdNodes[nodeNum].bb;
-            }
+            return _kdNodes[nodeNum].bb;
         }
 
 
@@ -216,8 +151,28 @@ class OSG_EXPORT KdTree : public osg::Shape
 
         int divide(BuildOptions& options, osg::BoundingBox& bb, int nodeIndex, unsigned int level);
 
-        bool intersect(const KdLeaf& leaf, const osg::Vec3& start, const osg::Vec3& end, LineSegmentIntersections& intersections) const;
-        bool intersect(const KdNode& node, const osg::Vec3& start, const osg::Vec3& end, const osg::Vec3& s, const osg::Vec3& e, LineSegmentIntersections& intersections) const;
+        struct RayData
+        {
+            RayData(const osg::Vec3& s, const osg::Vec3& e):
+                _s(s),
+                _e(e)
+            {
+                _d = e - s;
+                _length = _d.length();
+                _inverse_length = 1.0f/_length;
+                _d *= _inverse_length;
+            }
+            
+        
+            osg::Vec3 _s;
+            osg::Vec3 _e;
+            
+            osg::Vec3 _d;
+            float     _length;
+            float     _inverse_length;
+        };
+
+        bool intersect(const KdNode& node, const RayData& rayData, osg::Vec3 s, osg::Vec3 e, LineSegmentIntersections& intersections) const;
         bool intersectAndClip(osg::Vec3& s, osg::Vec3& e, const osg::BoundingBox& bb) const;
 
         typedef std::vector< osg::BoundingBox >                 BoundingBoxList;
@@ -230,13 +185,11 @@ class OSG_EXPORT KdTree : public osg::Shape
 
         AxisStack                           _axisStack;
         KdNodeList                          _kdNodes;
-        KdLeafList                          _kdLeaves;
 
         osg::ref_ptr<osg::Vec3Array>        _vertices;
         
         Indices                             _primitiveIndices;
         
-        BoundingBoxList                     _boundingBoxes;
         TriangleList                        _triangles;
         CenterList                          _centers;
 
diff --git a/src/osg/KdTree.cpp b/src/osg/KdTree.cpp
index 0c6e21348..efedf483f 100644
--- a/src/osg/KdTree.cpp
+++ b/src/osg/KdTree.cpp
@@ -20,7 +20,7 @@
 
 using namespace osg;
 
-// #define VERBOSE_OUTPUT
+//#define VERBOSE_OUTPUT
 
 ////////////////////////////////////////////////////////////////////////////////
 //
@@ -42,10 +42,8 @@ struct TriangleIndicesCollector
         bb.expandBy((*(_kdTree->_vertices))[p1]);
         bb.expandBy((*(_kdTree->_vertices))[p2]);
         bb.expandBy((*(_kdTree->_vertices))[p3]);
-        _kdTree->_boundingBoxes.push_back(bb);
-        
-        _kdTree->_centers.push_back(bb.center());
 
+        _kdTree->_centers.push_back(bb.center());
         _kdTree->_primitiveIndices.push_back(i);
         
     }
@@ -60,7 +58,7 @@ struct TriangleIndicesCollector
 
 KdTree::BuildOptions::BuildOptions():
         _numVerticesProcessed(0),
-        _targetNumTrianglesPerLeaf(4),
+        _targetNumTrianglesPerLeaf(2),
         _maxNumLevels(32)
 {
 }
@@ -99,8 +97,7 @@ bool KdTree::build(BuildOptions& options, osg::Geometry* geometry)
     osg::notify(osg::NOTICE)<<"kdTree->_kdNodes.reserve()="<<estimatedSize<<std::endl<<std::endl;
 #endif
 
-    _kdNodes.reserve(estimatedSize);
-    _kdLeaves.reserve(estimatedSize);
+    _kdNodes.reserve(estimatedSize*5);
     
     computeDivisions(options);
 
@@ -108,7 +105,6 @@ bool KdTree::build(BuildOptions& options, osg::Geometry* geometry)
 
     unsigned int estimatedNumTriangles = vertices->size()*2;
     _primitiveIndices.reserve(estimatedNumTriangles);
-    _boundingBoxes.reserve(estimatedNumTriangles);
     _triangles.reserve(estimatedNumTriangles);
     _centers.reserve(estimatedNumTriangles);
 
@@ -120,14 +116,13 @@ bool KdTree::build(BuildOptions& options, osg::Geometry* geometry)
 
     _primitiveIndices.reserve(vertices->size());
 
+    KdNode node(-1, _primitiveIndices.size());
+    node.bb = _bb;
 
-
-    KdLeaf leaf(0, _primitiveIndices.size());
-
-    int leafNum = addLeaf(leaf);
+    int nodeNum = addNode(node);
 
     osg::BoundingBox bb = _bb;
-    int nodeNum = divide(options, bb, leafNum, 0);
+    nodeNum = divide(options, bb, nodeNum, 0);
     
 #ifdef VERBOSE_OUTPUT    
     osg::notify(osg::NOTICE)<<"Root nodeNum="<<nodeNum<<std::endl;
@@ -179,61 +174,84 @@ void KdTree::computeDivisions(BuildOptions& options)
 
 int KdTree::divide(BuildOptions& options, osg::BoundingBox& bb, int nodeIndex, unsigned int level)
 {
-    if (_axisStack.size()<=level) return nodeIndex;
+    KdNode& node = getNode(nodeIndex);
 
-    int axis = _axisStack[level];
-
-#ifdef VERBOSE_OUTPUT    
-    //osg::notify(osg::NOTICE)<<"divide("<<nodeIndex<<", "<<level<< "), axis="<<axis<<std::endl;
-#endif
-
-    if (nodeIndex>=0)
+    bool needToDivide = level < _axisStack.size() &&
+                        (node.first<0 && node.second>options._targetNumTrianglesPerLeaf);
+                        
+    if (!needToDivide)
     {
-#ifdef VERBOSE_OUTPUT    
-        osg::notify(osg::NOTICE)<<"  divide node"<<std::endl;
-#endif
-        KdNode& node = getNode(nodeIndex);
-        return nodeIndex;
-    }
-    else
-    {    
-
-        if (getLeaf(nodeIndex).second<=options._targetNumTrianglesPerLeaf) 
+        if (node.first<0)
         {
+            int istart = -node.first-1;
+            int iend = istart+node.second-1;
+    
             // leaf is done, now compute bound on it.
-            KdLeaf& leaf = getLeaf(nodeIndex);
-            leaf.bb.init();
-            int iend = leaf.first+leaf.second;
-            for(int i=leaf.first; i<iend; ++i)
+            node.bb.init();
+            for(int i=istart; i<=iend; ++i)
             {
                 const Triangle& tri = _triangles[_primitiveIndices[i]];
                 const osg::Vec3& v1 = (*_vertices)[tri._p1];
                 const osg::Vec3& v2 = (*_vertices)[tri._p2];
                 const osg::Vec3& v3 = (*_vertices)[tri._p3];
-                leaf.bb.expandBy(v1);
-                leaf.bb.expandBy(v2);
-                leaf.bb.expandBy(v3);
+                node.bb.expandBy(v1);
+                node.bb.expandBy(v2);
+                node.bb.expandBy(v3);
+                
+                float epsilon = 1e-6;
+                node.bb._min.x() -= epsilon;
+                node.bb._min.y() -= epsilon;
+                node.bb._min.z() -= epsilon;
+                node.bb._max.x() += epsilon;
+                node.bb._max.y() += epsilon;
+                node.bb._max.z() += epsilon;
             }
 
-            return nodeIndex;
+#ifdef VERBOSE_OUTPUT    
+            if (!node.bb.valid())
+            {
+                osg::notify(osg::NOTICE)<<"After reset "<<node.first<<","<<node.second<<std::endl;
+                osg::notify(osg::NOTICE)<<"  bb._min ("<<node.bb._min<<")"<<std::endl;
+                osg::notify(osg::NOTICE)<<"  bb._max ("<<node.bb._max<<")"<<std::endl;
+            }
+            else
+            {
+                osg::notify(osg::NOTICE)<<"Set bb for nodeIndex = "<<nodeIndex<<std::endl;
+            }
+#endif
         }
 
+        return nodeIndex;
+
+    }
+
+    int axis = _axisStack[level];
+
+#ifdef VERBOSE_OUTPUT    
+    osg::notify(osg::NOTICE)<<"divide("<<nodeIndex<<", "<<level<< "), axis="<<axis<<std::endl;
+#endif
+
+    if (node.first<0)
+    {    
+        // leaf node as first <= 0, so look at dividing it.
+        
+        int istart = -node.first-1;
+        int iend = istart+node.second-1;
+
         //osg::notify(osg::NOTICE)<<"  divide leaf"<<std::endl;
         
-        int nodeNum = addNode(KdNode());
-
         float original_min = bb._min[axis];
         float original_max = bb._max[axis];
 
         float mid = (original_min+original_max)*0.5f;
 
-        {
-            KdLeaf& leaf = getLeaf(nodeIndex);
+        int originalLeftChildIndex = 0;
+        int originalRightChildIndex = 0;
 
+        {
             //osg::Vec3Array* vertices = kdTree._vertices.get();
-            int end = leaf.first+leaf.second-1;
-            int left = leaf.first;
-            int right = leaf.first+leaf.second-1;
+            int left = istart;
+            int right = iend;
             
             while(left<right)
             {
@@ -257,57 +275,54 @@ int KdTree::divide(BuildOptions& options, osg::BoundingBox& bb, int nodeIndex, u
                 else --right;
             }
             
-            KdLeaf leftLeaf(leaf.first, (right-leaf.first)+1);
-            KdLeaf rightLeaf(left, (end-left)+1);
+            KdNode leftLeaf(-istart-1, (right-istart)+1);
+            KdNode rightLeaf(-left-1, (iend-left)+1);
 
-#if 0            
-            osg::notify(osg::NOTICE)<<"In  leaf.first     ="<<leaf.first     <<" leaf.second     ="<<leaf.second<<std::endl;
+#if 0
+            osg::notify(osg::NOTICE)<<"In  node.first     ="<<node.first     <<" node.second     ="<<node.second<<std::endl;
             osg::notify(osg::NOTICE)<<"    leftLeaf.first ="<<leftLeaf.first <<" leftLeaf.second ="<<leftLeaf.second<<std::endl;
             osg::notify(osg::NOTICE)<<"    rightLeaf.first="<<rightLeaf.first<<" rightLeaf.second="<<rightLeaf.second<<std::endl;
             osg::notify(osg::NOTICE)<<"    left="<<left<<" right="<<right<<std::endl;
 
-            if (leaf.second != (leftLeaf.second +rightLeaf.second))
+            if (node.second != (leftLeaf.second +rightLeaf.second))
             {
-                osg::notify(osg::NOTICE)<<"*** Error in size, leaf.second="<<leaf.second
+                osg::notify(osg::NOTICE)<<"*** Error in size, leaf.second="<<node.second
                                         <<", leftLeaf.second="<<leftLeaf.second
                                         <<", rightLeaf.second="<<rightLeaf.second<<std::endl;
             }
             else
             {
-                osg::notify(osg::NOTICE)<<"Size OK, leaf.second="<<leaf.second
+                osg::notify(osg::NOTICE)<<"Size OK, leaf.second="<<node.second
                                         <<", leftLeaf.second="<<leftLeaf.second
                                         <<", rightLeaf.second="<<rightLeaf.second<<std::endl;
             }
 #endif
+
             if (leftLeaf.second<=0)
             {
                 //osg::notify(osg::NOTICE)<<"LeftLeaf empty"<<std::endl;
-                getNode(nodeNum).first = 0;
-                getNode(nodeNum).second = replaceLeaf(nodeIndex, rightLeaf);
+                originalLeftChildIndex = 0;
+                originalRightChildIndex = addNode(rightLeaf);
             }
             else if (rightLeaf.second<=0)
             {
                 //osg::notify(osg::NOTICE)<<"RightLeaf empty"<<std::endl;
-                getNode(nodeNum).first = replaceLeaf(nodeIndex, leftLeaf);
-                getNode(nodeNum).second = 0;
+                originalLeftChildIndex = addNode(leftLeaf);
+                originalRightChildIndex = 0;
             }
             else
             {
-                getNode(nodeNum).first = replaceLeaf(nodeIndex, leftLeaf);
-                getNode(nodeNum).second = addLeaf(rightLeaf);
+                originalLeftChildIndex = addNode(leftLeaf);
+                originalRightChildIndex = addNode(rightLeaf);
             }
         }
 
         
-        int originalLeftChildIndex = getNode(nodeNum).first;
-        int originalRightChildIndex = getNode(nodeNum).second;
-
-
         float restore = bb._max[axis];
         bb._max[axis] = mid;
 
         //osg::notify(osg::NOTICE)<<"  divide leftLeaf "<<kdTree.getNode(nodeNum).first<<std::endl;
-        int leftChildIndex = divide(options, bb, originalLeftChildIndex, level+1);
+        int leftChildIndex = originalLeftChildIndex!=0 ? divide(options, bb, originalLeftChildIndex, level+1) : 0;
 
         bb._max[axis] = restore;
         
@@ -315,175 +330,206 @@ int KdTree::divide(BuildOptions& options, osg::BoundingBox& bb, int nodeIndex, u
         bb._min[axis] = mid;
 
         //osg::notify(osg::NOTICE)<<"  divide rightLeaf "<<kdTree.getNode(nodeNum).second<<std::endl;
-        int rightChildIndex = divide(options, bb, originalRightChildIndex, level+1);
+        int rightChildIndex = originalRightChildIndex!=0 ? divide(options, bb, originalRightChildIndex, level+1) : 0;
         
         bb._min[axis] = restore;
         
-        getNode(nodeNum).first = leftChildIndex;
-        getNode(nodeNum).second = rightChildIndex; 
+        getNode(nodeIndex).first = leftChildIndex;
+        getNode(nodeIndex).second = rightChildIndex; 
         
-        getNode(nodeNum).bb.init();
-        if (leftChildIndex!=0) getNode(nodeNum).bb.expandBy(getBoundingBox(leftChildIndex));
-        if (rightChildIndex!=0) getNode(nodeNum).bb.expandBy(getBoundingBox(rightChildIndex));
-        
-        return nodeNum;
+        getNode(nodeIndex).bb.init();
+        if (leftChildIndex!=0) getNode(nodeIndex).bb.expandBy(getBoundingBox(leftChildIndex));
+        if (rightChildIndex!=0) getNode(nodeIndex).bb.expandBy(getBoundingBox(rightChildIndex));
+
+        if (!getNode(nodeIndex).bb.valid())
+        {
+            osg::notify(osg::NOTICE)<<"leftChildIndex="<<leftChildIndex<<" && originalLeftChildIndex="<<originalLeftChildIndex<<std::endl;
+            osg::notify(osg::NOTICE)<<"rightChildIndex="<<rightChildIndex<<" && originalRightChildIndex="<<originalRightChildIndex<<std::endl;
+
+            osg::notify(osg::NOTICE)<<"Invalid BB leftChildIndex="<<leftChildIndex<<", "<<rightChildIndex<<std::endl;
+            osg::notify(osg::NOTICE)<<"  bb._min ("<<getNode(nodeIndex).bb._min<<")"<<std::endl;
+            osg::notify(osg::NOTICE)<<"  bb._max ("<<getNode(nodeIndex).bb._max<<")"<<std::endl;
+            
+            if (leftChildIndex!=0)
+            {
+                osg::notify(osg::NOTICE)<<"  getBoundingBox(leftChildIndex) min = "<<getBoundingBox(leftChildIndex)._min<<std::endl;
+                osg::notify(osg::NOTICE)<<"                                 max = "<<getBoundingBox(leftChildIndex)._max<<std::endl;
+            }
+            if (rightChildIndex!=0)
+            {
+                osg::notify(osg::NOTICE)<<"  getBoundingBox(rightChildIndex) min = "<<getBoundingBox(rightChildIndex)._min<<std::endl;
+                osg::notify(osg::NOTICE)<<"                                 max = "<<getBoundingBox(rightChildIndex)._max<<std::endl;
+            }
+        }
+
     }
-}
-
-bool KdTree::intersect(const KdLeaf& leaf, const osg::Vec3& start, const osg::Vec3& end, LineSegmentIntersections& intersections) const
-{
-    osg::Vec3 _s = start;
-    osg::Vec3 _d = end - start;
-    float _length = _d.length();
-    _d /= _length;
-
-
-    //osg::notify(osg::NOTICE)<<"KdTree::intersect("<<&leaf<<")"<<std::endl;
-    bool intersects = false;
-    int iend = leaf.first + leaf.second;
-    for(int i=leaf.first; i<iend; ++i)
+    else
     {
-        const Triangle& tri = _triangles[_primitiveIndices[i]];
-        const osg::Vec3& v1 = (*_vertices)[tri._p1];
-        const osg::Vec3& v2 = (*_vertices)[tri._p2];
-        const osg::Vec3& v3 = (*_vertices)[tri._p3];
-        // osg::notify(osg::NOTICE)<<"   tri("<<tri._p1<<","<<tri._p2<<","<<tri._p3<<")"<<std::endl;
-
-        if (v1==v2 || v2==v3 || v1==v3) continue;
-
-        osg::Vec3 v12 = v2-v1;
-        osg::Vec3 n12 = v12^_d;
-        float ds12 = (_s-v1)*n12;
-        float d312 = (v3-v1)*n12;
-        if (d312>=0.0f)
-        {
-            if (ds12<0.0f) continue;
-            if (ds12>d312) continue;
-        }
-        else                     // d312 < 0
-        {
-            if (ds12>0.0f) continue;
-            if (ds12<d312) continue;
-        }
-
-        osg::Vec3 v23 = v3-v2;
-        osg::Vec3 n23 = v23^_d;
-        float ds23 = (_s-v2)*n23;
-        float d123 = (v1-v2)*n23;
-        if (d123>=0.0f)
-        {
-            if (ds23<0.0f) continue;
-            if (ds23>d123) continue;
-        }
-        else                     // d123 < 0
-        {
-            if (ds23>0.0f) continue;
-            if (ds23<d123) continue;
-        }
-
-        osg::Vec3 v31 = v1-v3;
-        osg::Vec3 n31 = v31^_d;
-        float ds31 = (_s-v3)*n31;
-        float d231 = (v2-v3)*n31;
-        if (d231>=0.0f)
-        {
-            if (ds31<0.0f) continue;
-            if (ds31>d231) continue;
-        }
-        else                     // d231 < 0
-        {
-            if (ds31>0.0f) continue;
-            if (ds31<d231) continue;
-        }
-
-
-        float r3;
-        if (ds12==0.0f) r3=0.0f;
-        else if (d312!=0.0f) r3 = ds12/d312;
-        else continue; // the triangle and the line must be parallel intersection.
-
-        float r1;
-        if (ds23==0.0f) r1=0.0f;
-        else if (d123!=0.0f) r1 = ds23/d123;
-        else continue; // the triangle and the line must be parallel intersection.
-
-        float r2;
-        if (ds31==0.0f) r2=0.0f;
-        else if (d231!=0.0f) r2 = ds31/d231;
-        else continue; // the triangle and the line must be parallel intersection.
-
-        float total_r = (r1+r2+r3);
-        if (total_r!=1.0f)
-        {
-            if (total_r==0.0f) continue; // the triangle and the line must be parallel intersection.
-            float inv_total_r = 1.0f/total_r;
-            r1 *= inv_total_r;
-            r2 *= inv_total_r;
-            r3 *= inv_total_r;
-        }
-
-        osg::Vec3 in = v1*r1+v2*r2+v3*r3;
-        if (!in.valid())
-        {
-            osg::notify(osg::WARN)<<"Warning:: Picked up error in TriangleIntersect"<<std::endl;
-            osg::notify(osg::WARN)<<"   ("<<v1<<",\t"<<v2<<",\t"<<v3<<")"<<std::endl;
-            osg::notify(osg::WARN)<<"   ("<<r1<<",\t"<<r2<<",\t"<<r3<<")"<<std::endl;
-            continue;
-        }
-
-        float d = (in-_s)*_d;
-
-        if (d<0.0f) continue;
-        if (d>_length) continue;
-
-        osg::Vec3 normal = v12^v23;
-        normal.normalize();
-
-        float r = d/_length;
-
-        LineSegmentIntersection intersection;
-        intersection.ratio = r;
-        intersection.primitiveIndex = _primitiveIndices[i];
-        intersection.intersectionPoint = in;
-        intersection.intersectionNormal = normal;
-
-        intersection.indexList.push_back(tri._p1);
-        intersection.indexList.push_back(tri._p2);
-        intersection.indexList.push_back(tri._p3);
-
-        intersection.ratioList.push_back(r1);
-        intersection.ratioList.push_back(r2);
-        intersection.ratioList.push_back(r3);
-        
-        intersections.insert(intersection);
-
-        // osg::notify(osg::NOTICE)<<"  got intersection ("<<in<<") ratio="<<r<<std::endl;
-
-        intersects = true;
+        osg::notify(osg::NOTICE)<<"NOT expecting to get here"<<std::endl;
     }
     
-    return intersects;
+    return nodeIndex;
+    
 }
 
-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
+bool KdTree::intersect(const KdNode& node, const RayData& rayData, osg::Vec3 ls, osg::Vec3 le, LineSegmentIntersections& intersections) const
 {
     //osg::notify(osg::NOTICE)<<"  Intersect "<<&node<<std::endl;
-    //osg::Vec3 ls(start), le(end);
-    osg::Vec3 ls(s), le(e);
-    int numIntersectionsBefore = intersections.size();
-    if (intersectAndClip(ls, le, node.bb))
-    {
-        if (node.first>0) intersect(getNode(node.first), start, end, ls, le, intersections);
-        else if (node.first<0) intersect(getLeaf(node.first), start, end, intersections);
+    if (!intersectAndClip(ls, le, node.bb)) return false;
 
-        if (node.second>0) intersect(getNode(node.second), start, end, ls, le, intersections);
-        else if (node.second<0) intersect(getLeaf(node.second), start, end, intersections);
+#if 0
+    {
+        osg::notify(osg::NOTICE)<<"Failed intersectAndClip("<<s<<","<<e<<")"<<std::endl;
+        osg::notify(osg::NOTICE)<<"  bb._min ("<<node.bb._min<<")"<<std::endl;
+        osg::notify(osg::NOTICE)<<"  bb._max ("<<node.bb._max<<")"<<std::endl;
+        return false;
     }
+#endif
+
+    int numIntersectionsBefore = intersections.size();
+
+    if (node.first<0)
+    {
+        // treat as a leaf
+
+        //osg::notify(osg::NOTICE)<<"KdTree::intersect("<<&leaf<<")"<<std::endl;
+        int istart = -node.first-1;
+        int iend = istart + node.second;
+        
+        for(int i=istart; i<iend; ++i)
+        {
+            const Triangle& tri = _triangles[_primitiveIndices[i]];
+            const osg::Vec3& v1 = (*_vertices)[tri._p1];
+            const osg::Vec3& v2 = (*_vertices)[tri._p2];
+            const osg::Vec3& v3 = (*_vertices)[tri._p3];
+            // osg::notify(osg::NOTICE)<<"   tri("<<tri._p1<<","<<tri._p2<<","<<tri._p3<<")"<<std::endl;
+
+            if (v1==v2 || v2==v3 || v1==v3) continue;
+
+            osg::Vec3 v12 = v2-v1;
+            osg::Vec3 n12 = v12^rayData._d;
+            float ds12 = (rayData._s-v1)*n12;
+            float d312 = (v3-v1)*n12;
+            if (d312>=0.0f)
+            {
+                if (ds12<0.0f) continue;
+                if (ds12>d312) continue;
+            }
+            else                     // d312 < 0
+            {
+                if (ds12>0.0f) continue;
+                if (ds12<d312) continue;
+            }
+
+            osg::Vec3 v23 = v3-v2;
+            osg::Vec3 n23 = v23^rayData._d;
+            float ds23 = (rayData._s-v2)*n23;
+            float d123 = (v1-v2)*n23;
+            if (d123>=0.0f)
+            {
+                if (ds23<0.0f) continue;
+                if (ds23>d123) continue;
+            }
+            else                     // d123 < 0
+            {
+                if (ds23>0.0f) continue;
+                if (ds23<d123) continue;
+            }
+
+            osg::Vec3 v31 = v1-v3;
+            osg::Vec3 n31 = v31^rayData._d;
+            float ds31 = (rayData._s-v3)*n31;
+            float d231 = (v2-v3)*n31;
+            if (d231>=0.0f)
+            {
+                if (ds31<0.0f) continue;
+                if (ds31>d231) continue;
+            }
+            else                     // d231 < 0
+            {
+                if (ds31>0.0f) continue;
+                if (ds31<d231) continue;
+            }
+
+
+            float r3;
+            if (ds12==0.0f) r3=0.0f;
+            else if (d312!=0.0f) r3 = ds12/d312;
+            else continue; // the triangle and the line must be parallel intersection.
+
+            float r1;
+            if (ds23==0.0f) r1=0.0f;
+            else if (d123!=0.0f) r1 = ds23/d123;
+            else continue; // the triangle and the line must be parallel intersection.
+
+            float r2;
+            if (ds31==0.0f) r2=0.0f;
+            else if (d231!=0.0f) r2 = ds31/d231;
+            else continue; // the triangle and the line must be parallel intersection.
+
+            float total_r = (r1+r2+r3);
+            if (total_r!=1.0f)
+            {
+                if (total_r==0.0f) continue; // the triangle and the line must be parallel intersection.
+                float inv_total_r = 1.0f/total_r;
+                r1 *= inv_total_r;
+                r2 *= inv_total_r;
+                r3 *= inv_total_r;
+            }
+
+            osg::Vec3 in = v1*r1+v2*r2+v3*r3;
+            if (!in.valid())
+            {
+                osg::notify(osg::WARN)<<"Warning:: Picked up error in TriangleIntersect"<<std::endl;
+                osg::notify(osg::WARN)<<"   ("<<v1<<",\t"<<v2<<",\t"<<v3<<")"<<std::endl;
+                osg::notify(osg::WARN)<<"   ("<<r1<<",\t"<<r2<<",\t"<<r3<<")"<<std::endl;
+                continue;
+            }
+
+            float d = (in-rayData._s)*rayData._d;
+
+            if (d<0.0f) continue;
+            if (d>rayData._length) continue;
+
+            osg::Vec3 normal = v12^v23;
+            normal.normalize();
+
+            float r = d* rayData._inverse_length;
+
+            LineSegmentIntersection intersection;
+            intersection.ratio = r;
+            intersection.primitiveIndex = _primitiveIndices[i];
+            intersection.intersectionPoint = in;
+            intersection.intersectionNormal = normal;
+
+            intersection.indexList.push_back(tri._p1);
+            intersection.indexList.push_back(tri._p2);
+            intersection.indexList.push_back(tri._p3);
+
+            intersection.ratioList.push_back(r1);
+            intersection.ratioList.push_back(r2);
+            intersection.ratioList.push_back(r3);
+
+            intersections.insert(intersection);
+
+            // osg::notify(osg::NOTICE)<<"  got intersection ("<<in<<") ratio="<<r<<std::endl;
+        }
+    }
+    else
+    {
+        if (node.first>0) intersect(getNode(node.first), rayData, ls, le, intersections);
+        if (node.second>0) intersect(getNode(node.second), rayData, ls, le, intersections);
+    }
+
     return numIntersectionsBefore != intersections.size();
 }
 
 bool KdTree::intersectAndClip(osg::Vec3& s, osg::Vec3& e, const osg::BoundingBox& bb) const
 {
+    //return true;
+
+    //if (!bb.valid()) return true;
+
     // compate s and e against the xMin to xMax range of bb.
     if (s.x()<=e.x())
     {
@@ -607,8 +653,8 @@ bool KdTree::intersectAndClip(osg::Vec3& s, osg::Vec3& e, const osg::BoundingBox
 
 bool KdTree::intersect(const osg::Vec3& start, const osg::Vec3& end, LineSegmentIntersections& intersections) const
 {
-    osg::Vec3 s(start), e(end);
-    return intersect(getNode(0), start, end, s,e, intersections);
+    RayData rayData(start,end);
+    return intersect(getNode(0), rayData, start, end, intersections);
 }
 
 ////////////////////////////////////////////////////////////////////////////////