Hi Robert,
This morning i was about 1hour in train, and reviewed again the kdTree
implementation. Yesterday i wrote that the kdTree has some strange behaviour
when triangles have equal center points. yes this is still true, and we can
(as you told) ignore such bad geometries. but one thing i worked out is that
the kdTree isn't oriented to the geometry topology, assume we have a torus
like box, then the bounding boxes are far away from a optional kdTree-based
bounding box hierarchy. for line intersection checks this is not as bad as
it sounds, but for further more complex checks, like polytop intersection,
haptic rendering this will turn in a more expensive overhead. so i propose
to use the latest code, with the ADRIAN_DIVIDE_BB_FIX define switch on.
to compare the kdTree, performance, nodes, leafs, i added some statistics to
the code: #define VERBOSE_OUTPUT_TREE_INFO
the building time still fast enough, but of course no for free :-(, the tree
is smaller. he is not just smaller, it's more correct w.r.t kdTree spatial
datastructure / bounding hierarchy.
adrian
## ADRIAN ##
#define ADRIAN_DIVIDE_BB_FIX
KD-Tree Build Time = 0.0590638s
+ TriList = 69451
+ Nodes = 23776
+ depth sum = 316836
+ avg depth = 13.3259
+ Leaves = 23777
+ depth sum = 364388
+ avg depth = 15.3252
+ max depth = 19
+ min depth = 9
## SVN ##
KD-Tree Build Time = 0.0404889s
+ TriList = 69451
+ Nodes = 32084
+ depth sum = 520696
+ avg depth = 16.2291
+ Leaves = 23143
+ depth sum = 436283
+ avg depth = 18.8516
+ max depth = 22
+ min depth = 7
23776 / 32084 / -8303 / ~33% win
23777 / 23143 / +634 / ~ 0% lost
0.059 / 0.040 / +0.019 / ~33% lost
****** win ******
memory ....
intersection speed ...
:-)
****** pay with build time *****
:-(
--
********************************************
Adrian Egli
/* -*-c++-*- OpenSceneGraph - Copyright (C) 1998-2006 Robert Osfield
*
* This library is open source and may be redistributed and/or modified under
* the terms of the OpenSceneGraph Public License (OSGPL) version 0.0 or
* (at your option) any later version. The full license is in LICENSE file
* included with this distribution, and on the openscenegraph.org website.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* OpenSceneGraph Public License for more details.
*/
#include <osg/KdTree>
#include <osg/Geode>
#include <osg/TriangleIndexFunctor>
#include <osg/Timer>
#include <osg/io_utils>
using namespace osg;
// hold the bounding box close to the geometry
#define ADRIAN_DIVIDE_BB_FIX
//#define VERBOSE_OUTPUT
//#define VERBOSE_OUTPUT_TREE_INFO
#ifdef VERBOSE_OUTPUT_TREE_INFO
int MAX_LEVEL(0);
int MIN_LEVEL(INT_MAX);
int COUNT_LEAF(0);
int COUNT_NODE(0);
int COUNT_NODE_DEPTH_SUM(0);
int COUNT_LEAF_DEPTH_SUM(0);
#endif
////////////////////////////////////////////////////////////////////////////////
//
// BuildKdTree Declarartion - class used for building an single KdTree
struct BuildKdTree
{
BuildKdTree(KdTree& kdTree):
_kdTree(kdTree) {}
typedef std::vector< osg::Vec3 > CenterList;
typedef std::vector< unsigned int > Indices;
typedef std::vector< unsigned int > AxisStack;
bool build(KdTree::BuildOptions& options, osg::Geometry* geometry);
void computeDivisions(KdTree::BuildOptions& options);
int divide(KdTree::BuildOptions& options, osg::BoundingBox& bb, int nodeIndex, unsigned int level);
KdTree& _kdTree;
osg::BoundingBox _bb;
AxisStack _axisStack;
Indices _primitiveIndices;
CenterList _centers;
};
////////////////////////////////////////////////////////////////////////////////
//
// Functor for collecting triangle indices from Geometry
struct TriangleIndicesCollector
{
TriangleIndicesCollector():
_buildKdTree(0)
{
}
inline void operator () (unsigned int p0, unsigned int p1, unsigned int p2)
{
const osg::Vec3& v0 = (*(_buildKdTree->_kdTree.getVertices()))[p0];
const osg::Vec3& v1 = (*(_buildKdTree->_kdTree.getVertices()))[p1];
const osg::Vec3& v2 = (*(_buildKdTree->_kdTree.getVertices()))[p2];
// discard degenerate points
if (v0==v1 || v1==v2 || v1==v2)
{
//osg::notify(osg::NOTICE)<<"Disgarding degenerate triangle"<<std::endl;
return;
}
unsigned int i = _buildKdTree->_kdTree.addTriangle(KdTree::Triangle(p0,p1,p2));
osg::BoundingBox bb;
bb.expandBy(v0);
bb.expandBy(v1);
bb.expandBy(v2);
_buildKdTree->_centers.push_back(bb.center());
_buildKdTree->_primitiveIndices.push_back(i);
}
BuildKdTree* _buildKdTree;
};
////////////////////////////////////////////////////////////////////////////////
//
// BuildKdTree Implementation
bool BuildKdTree::build(KdTree::BuildOptions& options, osg::Geometry* geometry)
{
#ifdef VERBOSE_OUTPUT_TREE_INFO
MAX_LEVEL=(0);
MIN_LEVEL=(INT_MAX);
COUNT_LEAF=(0);
COUNT_NODE=(0);
COUNT_NODE_DEPTH_SUM=(0);
COUNT_LEAF_DEPTH_SUM=(0);
osg::Timer_t startBuildTick = osg::Timer::instance()->tick();
#endif
#ifdef VERBOSE_OUTPUT
osg::notify(osg::NOTICE)<<"osg::KDTreeBuilder::createKDTree()"<<std::endl;
#endif
osg::Vec3Array* vertices = dynamic_cast<osg::Vec3Array*>(geometry->getVertexArray());
if (!vertices) return false;
if (vertices->size() <= options._targetNumTrianglesPerLeaf) return false;
_bb = geometry->getBound();
_kdTree.setVertices(vertices);
unsigned int estimatedSize = (unsigned int)(2.0*float(vertices->size())/float(options._targetNumTrianglesPerLeaf));
#ifdef VERBOSE_OUTPUT
osg::notify(osg::NOTICE)<<"kdTree->_kdNodes.reserve()="<<estimatedSize<<std::endl<<std::endl;
#endif
_kdTree.getNodes().reserve(estimatedSize*5);
computeDivisions(options);
options._numVerticesProcessed += vertices->size();
unsigned int estimatedNumTriangles = vertices->size()*2;
_primitiveIndices.reserve(estimatedNumTriangles);
_centers.reserve(estimatedNumTriangles);
_kdTree.getTriangles().reserve(estimatedNumTriangles);
osg::TriangleIndexFunctor<TriangleIndicesCollector> collectTriangleIndices;
collectTriangleIndices._buildKdTree = this;
geometry->accept(collectTriangleIndices);
_primitiveIndices.reserve(vertices->size());
KdTree::KdNode node(-1, _primitiveIndices.size());
node.bb = _bb;
int nodeNum = _kdTree.addNode(node);
osg::BoundingBox bb = _bb;
nodeNum = divide(options, bb, nodeNum, 0);
// now reorder the triangle list so that it's in order as per the primitiveIndex list.
KdTree::TriangleList triangleList(_kdTree.getTriangles().size());
for(unsigned int i=0; i<_primitiveIndices.size(); ++i)
{
triangleList[i] = _kdTree.getTriangle(_primitiveIndices[i]);
}
_kdTree.getTriangles().swap(triangleList);
#ifdef VERBOSE_OUTPUT
osg::notify(osg::NOTICE)<<"Root nodeNum="<<nodeNum<<std::endl;
#endif
#ifdef VERBOSE_OUTPUT_TREE_INFO
osg::Timer_t endBuildTick = osg::Timer::instance()->tick();
osg::notify(osg::NOTICE) << "KD-Tree Build Time\t=\t" << osg::Timer::instance()->delta_m(startBuildTick,endBuildTick)/1000.0 <<"s" << std::endl;
osg::notify(osg::NOTICE) << " + TriList = " << _kdTree.getTriangles().size() << std::endl;
osg::notify(osg::NOTICE) << " + Nodes = " << COUNT_NODE << std::endl;
osg::notify(osg::NOTICE) << " + depth sum = " << COUNT_NODE_DEPTH_SUM << std::endl;
osg::notify(osg::NOTICE) << " + avg depth = " << (double) COUNT_NODE_DEPTH_SUM /(double) COUNT_NODE << std::endl;
osg::notify(osg::NOTICE) << " + Leaves = " << COUNT_LEAF << std::endl;
osg::notify(osg::NOTICE) << " + depth sum = " << COUNT_LEAF_DEPTH_SUM << std::endl;
osg::notify(osg::NOTICE) << " + avg depth = " << (double) COUNT_LEAF_DEPTH_SUM /(double)COUNT_LEAF << std::endl;
osg::notify(osg::NOTICE) << " + max depth = " << MAX_LEVEL << std::endl;
osg::notify(osg::NOTICE) << " + min depth = " << MIN_LEVEL << std::endl;
#endif
// osg::notify(osg::NOTICE)<<"_kdNodes.size()="<<k_kdNodes.size()<<" estimated size = "<<estimatedSize<<std::endl;
// osg::notify(osg::NOTICE)<<"_kdLeaves.size()="<<_kdLeaves.size()<<" estimated size = "<<estimatedSize<<std::endl<<std::endl;
return !_kdTree.getNodes().empty();
}
void BuildKdTree::computeDivisions(KdTree::BuildOptions& options)
{
osg::Vec3 dimensions(_bb.xMax()-_bb.xMin(),
_bb.yMax()-_bb.yMin(),
_bb.zMax()-_bb.zMin());
#ifdef VERBOSE_OUTPUT
osg::notify(osg::NOTICE)<<"computeDivisions("<<options._maxNumLevels<<") "<<dimensions<< " { "<<std::endl;
#endif
_axisStack.reserve(options._maxNumLevels);
for(int level=0; level<options._maxNumLevels; ++level)
{
int axis = 0;
if (dimensions[0]>=dimensions[1])
{
if (dimensions[0]>=dimensions[2]) axis = 0;
else axis = 2;
}
else if (dimensions[1]>=dimensions[2]) axis = 1;
else axis = 2;
_axisStack.push_back(axis);
dimensions[axis] /= 2.0f;
#ifdef VERBOSE_OUTPUT
osg::notify(osg::NOTICE)<<" "<<level<<", "<<dimensions<<", "<<axis << std::endl;
#endif
}
#ifdef VERBOSE_OUTPUT
osg::notify(osg::NOTICE)<<"}"<<std::endl;
#endif
}
int BuildKdTree::divide(KdTree::BuildOptions& options, osg::BoundingBox& bb, int nodeIndex, unsigned int level)
{
KdTree::KdNode& node = _kdTree.getNode(nodeIndex);
bool needToDivide = level < _axisStack.size() &&
(node.first<0 && node.second>options._targetNumTrianglesPerLeaf);
if (!needToDivide)
{
if (node.first<0)
{
int istart = -node.first-1;
int iend = istart+node.second-1;
// leaf is done, now compute bound on it.
node.bb.init();
for(int i=istart; i<=iend; ++i)
{
const KdTree::Triangle& tri = _kdTree.getTriangle(_primitiveIndices[i]);
const osg::Vec3& v0 = (*_kdTree.getVertices())[tri.p0];
const osg::Vec3& v1 = (*_kdTree.getVertices())[tri.p1];
const osg::Vec3& v2 = (*_kdTree.getVertices())[tri.p2];
node.bb.expandBy(v0);
node.bb.expandBy(v1);
node.bb.expandBy(v2);
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;
}
#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
#ifdef VERBOSE_OUTPUT_TREE_INFO
COUNT_LEAF++;
if ( MAX_LEVEL < level ) MAX_LEVEL = level;
COUNT_LEAF_DEPTH_SUM+=level;
if ( MIN_LEVEL > level ) MIN_LEVEL = level;
#endif
}
return nodeIndex;
}
int axis = _axisStack[level];
#ifdef VERBOSE_OUTPUT_TREE_INFO
COUNT_NODE++;
COUNT_NODE_DEPTH_SUM+=level;
#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;
#ifdef ADRIAN_DIVIDE_BB_FIX
osg::BoundingBox bbLeft;
osg::BoundingBox bbRight;
#endif
float original_min = bb._min[axis];
float original_max = bb._max[axis];
float mid = (original_min+original_max)*0.5f;
int originalLeftChildIndex = 0;
int originalRightChildIndex = 0;
bool insitueDivision = false;
{
int left = istart;
int right = iend;
while(left<right)
{
while(left<right && (_centers[_primitiveIndices[left]][axis]<=mid)) {
#ifdef ADRIAN_DIVIDE_BB_FIX
bbLeft.expandBy(_centers[_primitiveIndices[left]]);
#endif
++left;
}\
while(left<right && (_centers[_primitiveIndices[right]][axis]>mid)) {
#ifdef ADRIAN_DIVIDE_BB_FIX
bbRight.expandBy(_centers[_primitiveIndices[right]]);
#endif
--right;
}
while(left<right && (_centers[_primitiveIndices[right]][axis]>mid)) {
#ifdef ADRIAN_DIVIDE_BB_FIX
bbRight.expandBy(_centers[_primitiveIndices[right]]);
#endif
--right;
}
if (left<right)
{
std::swap(_primitiveIndices[left], _primitiveIndices[right]);
#ifdef ADRIAN_DIVIDE_BB_FIX
bbLeft.expandBy(_centers[_primitiveIndices[left]]);
bbRight.expandBy(_centers[_primitiveIndices[right]]);
#endif
++left;
--right;
}
}
if (left==right)
{
if (_centers[_primitiveIndices[left]][axis]<=mid) {
#ifdef ADRIAN_DIVIDE_BB_FIX
bbLeft.expandBy(_centers[_primitiveIndices[left]]);
#endif
++left;
}
else {
#ifdef ADRIAN_DIVIDE_BB_FIX
bbRight.expandBy(_centers[_primitiveIndices[right]]);
#endif
--right;
}
}
#if 0
int nbrPrimitives(iend-istart);
int nbrRight(iend-right);
int nbrLeft(left-istart);
if ( nbrLeft == 0.0 || nbrRight == 0.0 ) {
osg::notify(osg::NOTICE)<< "Geometry can not perfectly divided, some triangles have exactly same center point ! " << std::endl;
osg::notify(osg::NOTICE)<< " -> " << nbrPrimitives << "\t" << nbrLeft << "\t" << nbrRight << "\t" << mid << std::endl;
for (int i=istart;i<iend;i++)
{
osg::notify(osg::NOTICE) << " ref: "<< i << " : center " << _centers[_primitiveIndices[i]][axis] << std::endl;
}
}
#endif
KdTree::KdNode leftLeaf(-istart-1, (right-istart)+1);
KdTree::KdNode rightLeaf(-left-1, (iend-left)+1);
#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 (node.second != (leftLeaf.second +rightLeaf.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="<<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;
originalLeftChildIndex = 0;
//originalRightChildIndex = addNode(rightLeaf);
originalRightChildIndex = nodeIndex;
insitueDivision = true;
}
else if (rightLeaf.second<=0)
{
//osg::notify(osg::NOTICE)<<"RightLeaf empty"<<std::endl;
// originalLeftChildIndex = addNode(leftLeaf);
originalLeftChildIndex = nodeIndex;
originalRightChildIndex = 0;
insitueDivision = true;
}
else
{
originalLeftChildIndex = _kdTree.addNode(leftLeaf);
originalRightChildIndex = _kdTree.addNode(rightLeaf);
}
}
#ifndef ADRIAN_DIVIDE_BB_FIX
float restore = bb._max[axis];
bb._max[axis] = mid;
//osg::notify(osg::NOTICE)<<" divide leftLeaf "<<kdTree.getNode(nodeNum).first<<std::endl;
int leftChildIndex = originalLeftChildIndex!=0 ? divide(options, bb, originalLeftChildIndex, level+1) : 0;
bb._max[axis] = restore;
restore = bb._min[axis];
bb._min[axis] = mid;
//osg::notify(osg::NOTICE)<<" divide rightLeaf "<<kdTree.getNode(nodeNum).second<<std::endl;
int rightChildIndex = originalRightChildIndex!=0 ? divide(options, bb, originalRightChildIndex, level+1) : 0;
bb._min[axis] = restore;
#else
int leftChildIndex = originalLeftChildIndex!=0 ? divide(options, bbLeft, originalLeftChildIndex, level+1) : 0;
int rightChildIndex = originalRightChildIndex!=0 ? divide(options, bbRight, originalRightChildIndex, level+1) : 0;
#endif
if (!insitueDivision)
{
// take a second reference to node we are working on as the std::vector<> resize could
// have invalidate the previous node ref.
KdTree::KdNode& newNodeRef = _kdTree.getNode(nodeIndex);
newNodeRef.first = leftChildIndex;
newNodeRef.second = rightChildIndex;
insitueDivision = true;
newNodeRef.bb.init();
if (leftChildIndex!=0) newNodeRef.bb.expandBy(_kdTree.getNode(leftChildIndex).bb);
if (rightChildIndex!=0) newNodeRef.bb.expandBy(_kdTree.getNode(rightChildIndex).bb);
if (!newNodeRef.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 ("<<newNodeRef.bb._min<<")"<<std::endl;
osg::notify(osg::NOTICE)<<" bb._max ("<<newNodeRef.bb._max<<")"<<std::endl;
if (leftChildIndex!=0)
{
osg::notify(osg::NOTICE)<<" getNode(leftChildIndex).bb min = "<<_kdTree.getNode(leftChildIndex).bb._min<<std::endl;
osg::notify(osg::NOTICE)<<" max = "<<_kdTree.getNode(leftChildIndex).bb._max<<std::endl;
}
if (rightChildIndex!=0)
{
osg::notify(osg::NOTICE)<<" getNode(rightChildIndex).bb min = "<<_kdTree.getNode(rightChildIndex).bb._min<<std::endl;
osg::notify(osg::NOTICE)<<" max = "<<_kdTree.getNode(rightChildIndex).bb._max<<std::endl;
}
}
}
}
else
{
osg::notify(osg::NOTICE)<<"NOT expecting to get here"<<std::endl;
}
return nodeIndex;
}
////////////////////////////////////////////////////////////////////////////////
//
// IntersectKdTree
//
struct IntersectKdTree
{
IntersectKdTree(const osg::Vec3Array& vertices,
const KdTree::KdNodeList& nodes,
const KdTree::TriangleList& triangles,
KdTree::LineSegmentIntersections& intersections,
const osg::Vec3& s, const osg::Vec3& e):
_vertices(vertices),
_kdNodes(nodes),
_triangles(triangles),
_intersections(intersections),
_s(s),
_e(e)
{
_d = e - s;
_length = _d.length();
if ( _length == 0.0f ) _inverse_length=0.0f; else _inverse_length = 1.0f/_length;
_d *= _inverse_length;
}
void intersect(const KdTree::KdNode& root, const osg::Vec3& s, const osg::Vec3& e,const osg::Vec3& dir, const osg::Vec3& invDir) const;
bool intersectAndClip(osg::Vec3& s, osg::Vec3& e, const osg::BoundingBox& bb, const osg::Vec3& dir, const osg::Vec3& invDir) const;
const osg::Vec3Array& _vertices;
const KdTree::KdNodeList& _kdNodes;
const KdTree::TriangleList& _triangles;
KdTree::LineSegmentIntersections& _intersections;
osg::Vec3 _s;
osg::Vec3 _e;
osg::Vec3 _d;
float _length;
float _inverse_length;
};
void IntersectKdTree::intersect(const KdTree::KdNode& root, const osg::Vec3& ls, const osg::Vec3& le,const osg::Vec3& dir, const osg::Vec3& invDir) const
{
if (root.first<0)
{
// treat as a leaf
//osg::notify(osg::NOTICE)<<"KdTree::intersect("<<&leaf<<")"<<std::endl;
int istart = -root.first-1;
int iend = istart + root.second;
for(int i=istart; i<iend; ++i)
{
//const Triangle& tri = _triangles[_primitiveIndices[i]];
const KdTree::Triangle& tri = _triangles[i];
// osg::notify(osg::NOTICE)<<" tri("<<tri.p1<<","<<tri.p2<<","<<tri.p3<<")"<<std::endl;
const osg::Vec3& v0 = _vertices[tri.p0];
const osg::Vec3& v1 = _vertices[tri.p1];
const osg::Vec3& v2 = _vertices[tri.p2];
osg::Vec3 T = _s - v0;
osg::Vec3 E2 = v2 - v0;
osg::Vec3 E1 = v1 - v0;
osg::Vec3 P = _d ^ E2;
float det = P * E1;
float r,r0,r1,r2;
const float esplison = 1e-10;
if (det>esplison)
{
float u = (P*T);
if (u<0.0 || u>det) continue;
osg::Vec3 Q = T ^ E1;
float v = (Q*_d);
if (v<0.0 || v>det) continue;
if ((u+v)> det) continue;
float inv_det = 1.0f/det;
float t = (Q*E2)*inv_det;
if (t<0.0 || t>_length) continue;
u *= inv_det;
v *= inv_det;
r0 = 1.0f-u-v;
r1 = u;
r2 = v;
r = t * _inverse_length;
}
else if (det<-esplison)
{
float u = (P*T);
if (u>0.0 || u<det) continue;
osg::Vec3 Q = T ^ E1;
float v = (Q*_d);
if (v>0.0 || v<det) continue;
if ((u+v) < det) continue;
float inv_det = 1.0f/det;
float t = (Q*E2)*inv_det;
if (t<0.0 || t>_length) continue;
u *= inv_det;
v *= inv_det;
r0 = 1.0f-u-v;
r1 = u;
r2 = v;
r = t * _inverse_length;
}
else
{
continue;
}
osg::Vec3 in = v0*r0 + v1*r1 + v2*r2;
osg::Vec3 normal = E1^E2;
normal.normalize();
#if 1
_intersections.push_back(KdTree::LineSegmentIntersection());
KdTree::LineSegmentIntersection& intersection = _intersections.back();
intersection.ratio = r;
intersection.primitiveIndex = i;
intersection.intersectionPoint = in;
intersection.intersectionNormal = normal;
intersection.p0 = tri.p0;
intersection.p1 = tri.p1;
intersection.p2 = tri.p2;
intersection.r0 = r0;
intersection.r1 = r1;
intersection.r2 = r2;
#endif
// osg::notify(osg::NOTICE)<<" got intersection ("<<in<<") ratio="<<r<<std::endl;
}
}
else
{
if (root.first>0)
{
osg::Vec3 s(ls);
osg::Vec3 e(le);
if (intersectAndClip(s,e, _kdNodes[root.first].bb,dir,invDir))
{
intersect(_kdNodes[root.first], s, e,dir,invDir);
}
}
if (root.second>0)
{
osg::Vec3 s(ls);
osg::Vec3 e(le);
if (intersectAndClip(s,e, _kdNodes[root.second].bb,dir,invDir))
{
intersect(_kdNodes[root.second], s, e,dir,invDir);
}
}
}
}
bool IntersectKdTree::intersectAndClip(osg::Vec3& s, osg::Vec3& e, const osg::BoundingBox& bb, const osg::Vec3& dir, const osg::Vec3& invDir) 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())
{
// trivial reject of segment wholely outside.
if (e.x()<bb.xMin()) return false;
if (s.x()>bb.xMax()) return false;
if (s.x()<bb.xMin())
{
// clip s to xMin.
s += dir*(bb.xMin()-s.x())*invDir.x();
}
if (e.x()>bb.xMax())
{
// clip e to xMax.
e = s+dir*(bb.xMax()-s.x())*invDir.x();
}
}
else
{
if (s.x()<bb.xMin()) return false;
if (e.x()>bb.xMax()) return false;
if (e.x()<bb.xMin())
{
// clip s to xMin.
e = s+dir*(bb.xMin()-s.x())*invDir.x();
}
if (s.x()>bb.xMax())
{
// clip e to xMax.
s += dir*(bb.xMax()-s.x())*invDir.x();
}
}
// compate s and e against the yMin to yMax range of bb.
if (s.y()<=e.y())
{
// trivial reject of segment wholely outside.
if (e.y()<bb.yMin()) return false;
if (s.y()>bb.yMax()) return false;
if (s.y()<bb.yMin())
{
// clip s to yMin.
s += dir*(bb.yMin()-s.y())*invDir.y();
}
if (e.y()>bb.yMax())
{
// clip e to yMax.
e = s+dir*(bb.yMax()-s.y())*invDir.y();
}
}
else
{
if (s.y()<bb.yMin()) return false;
if (e.y()>bb.yMax()) return false;
if (e.y()<bb.yMin())
{
// clip s to yMin.
e = s+dir*(bb.yMin()-s.y())*invDir.y();
}
if (s.y()>bb.yMax())
{
// clip e to yMax.
s +=dir*(bb.yMax()-s.y())*invDir.y();
}
}
// compate s and e against the zMin to zMax range of bb.
if (s.z()<=e.z())
{
// trivial reject of segment wholely outside.
if (e.z()<bb.zMin()) return false;
if (s.z()>bb.zMax()) return false;
if (s.z()<bb.zMin())
{
// clip s to zMin.
s += dir*(bb.zMin()-s.z())*invDir.z();
}
if (e.z()>bb.zMax())
{
// clip e to zMax.
e = s+dir*(bb.zMax()-s.z())*invDir.z();
}
}
else
{
if (s.z()<bb.zMin()) return false;
if (e.z()>bb.zMax()) return false;
if (e.z()<bb.zMin())
{
// clip s to zMin.
e = s+dir*(bb.zMin()-s.z())*invDir.z();
}
if (s.z()>bb.zMax())
{
// clip e to zMax.
s = s+dir*(bb.zMax()-s.z())*invDir.z();
}
}
// osg::notify(osg::NOTICE)<<"clampped segment "<<s<<" "<<e<<std::endl;
// if (s==e) return false;
return true;
}
////////////////////////////////////////////////////////////////////////////////
//
// KdTree::BuildOptions
KdTree::BuildOptions::BuildOptions():
_numVerticesProcessed(0),
_targetNumTrianglesPerLeaf(4),
_maxNumLevels(32)
{
}
////////////////////////////////////////////////////////////////////////////////
//
// KdTree
KdTree::KdTree()
{
}
KdTree::KdTree(const KdTree& rhs, const osg::CopyOp& copyop):
Shape(rhs)
{
}
bool KdTree::build(BuildOptions& options, osg::Geometry* geometry)
{
BuildKdTree build(*this);
return build.build(options, geometry);
}
bool KdTree::intersect(const osg::Vec3& start, const osg::Vec3& end, LineSegmentIntersections& intersections) const
{
int numIntersectionsBefore = intersections.size();
IntersectKdTree intersector(*_vertices,
_kdNodes,
_triangles,
intersections,
start, end);
osg::Vec3 dir(end-start);
const osg::Vec3 invDir(
dir.x() != 0.0f ? 1.0f/dir.x() : 0.0f,
dir.y() != 0.0f ? 1.0f/dir.y() : 0.0f,
dir.z() != 0.0f ? 1.0f/dir.z() : 0.0f
);
intersector.intersect(getNode(0), start, end,dir,invDir);
return numIntersectionsBefore != intersections.size();
}
////////////////////////////////////////////////////////////////////////////////
//
// KdTreeBuilder
KdTreeBuilder::KdTreeBuilder():
osg::NodeVisitor(osg::NodeVisitor::TRAVERSE_ALL_CHILDREN)
{
_kdTreePrototype = new osg::KdTree;
}
KdTreeBuilder::KdTreeBuilder(const KdTreeBuilder& rhs):
osg::NodeVisitor(osg::NodeVisitor::TRAVERSE_ALL_CHILDREN),
_kdTreePrototype(rhs._kdTreePrototype),
_buildOptions(rhs._buildOptions)
{
}
void KdTreeBuilder::apply(osg::Geode& geode)
{
for(unsigned int i=0; i<geode.getNumDrawables(); ++i)
{
osg::Geometry* geom = geode.getDrawable(i)->asGeometry();
if (geom)
{
osg::KdTree* previous = dynamic_cast<osg::KdTree*>(geom->getShape());
if (previous) continue;
osg::ref_ptr<osg::KdTree> kdTree = dynamic_cast<osg::KdTree*>(_kdTreePrototype->cloneType());
if (kdTree->build(_buildOptions, geom))
{
geom->setShape(kdTree.get());
}
}
}
}
_______________________________________________
osg-submissions mailing list
[email protected]
http://lists.openscenegraph.org/listinfo.cgi/osg-submissions-openscenegraph.org
