FGFS-4.1/OpenSceneGraph
4
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Streamlined KdTree implementation

Browse Source
This commit is contained in:
Robert Osfield
2008-07-10 15:50:10 +00:00
parent 29d66125e2
commit d3fd43cc6f
2 changed files with 286 additions and 287 deletions
Download Patch File Download Diff File Expand all files Collapse all files

95
include/osg/KdTree
View File

@@ -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;

478
src/osg/KdTree.cpp
View File

@@ -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);
}
////////////////////////////////////////////////////////////////////////////////