From 6e1866ac1857d3466a236a8ad63f91be39e19c71 Mon Sep 17 00:00:00 2001 From: Robert Osfield Date: Tue, 9 May 2017 11:33:22 +0100 Subject: [PATCH] Added --points and --lines command line options that do a very simplistic conversion of geometry primitives to points or lines respectively, used to aid testing of intersectors --- .../osgkeyboardmouse/osgkeyboardmouse.cpp | 34 + include/osg/KdTree | 149 ++- src/osg/KdTree.cpp | 480 ++------ src/osgUtil/LineSegmentIntersector.cpp | 658 +++++----- src/osgUtil/PolytopeIntersector.cpp | 1057 ++++++----------- 5 files changed, 937 insertions(+), 1441 deletions(-) diff --git a/examples/osgkeyboardmouse/osgkeyboardmouse.cpp b/examples/osgkeyboardmouse/osgkeyboardmouse.cpp index 4df7ea9e3..90651ad9a 100644 --- a/examples/osgkeyboardmouse/osgkeyboardmouse.cpp +++ b/examples/osgkeyboardmouse/osgkeyboardmouse.cpp @@ -408,6 +408,36 @@ protected: }; +class ConvertPrimitives : public osg::NodeVisitor +{ +public: + + osg::PrimitiveSet::Mode _mode; + + ConvertPrimitives(osg::PrimitiveSet::Mode mode): + osg::NodeVisitor(osg::NodeVisitor::TRAVERSE_ALL_CHILDREN), + _mode(mode) {} + + void apply(osg::Geometry& geometry) + { + if (!geometry.getVertexArray()) return; + + unsigned int numVertices = geometry.getVertexArray()->getNumElements(); + + if (_mode==osg::PrimitiveSet::POINTS) + { + // remove previous primitive sets. + geometry.removePrimitiveSet(0, geometry.getNumPrimitiveSets()); + geometry.addPrimitiveSet(new osg::DrawArrays(_mode, 0,numVertices)); + } + else if (_mode==osg::PrimitiveSet::LINES) + { + geometry.removePrimitiveSet(0, geometry.getNumPrimitiveSets()); + geometry.addPrimitiveSet(new osg::DrawArrays(_mode, 0,numVertices)); + } + } +}; + int main( int argc, char **argv ) { osg::ArgumentParser arguments(&argc, argv); @@ -433,6 +463,9 @@ int main( int argc, char **argv ) return 1; } + while(arguments.read("--points")) { ConvertPrimitives cp(osg::PrimitiveSet::POINTS); loadedModel->accept(cp); } + while(arguments.read("--lines")) { ConvertPrimitives cp(osg::PrimitiveSet::LINES); loadedModel->accept(cp); } + if (useKdTree) { OSG_NOTICE<<"Buildering KdTrees"<accept(*builder); } + // assign the scene graph to viewer viewer.setSceneData(loadedModel); diff --git a/include/osg/KdTree b/include/osg/KdTree index 24d1995e0..25eca0954 100644 --- a/include/osg/KdTree +++ b/include/osg/KdTree @@ -48,42 +48,65 @@ class OSG_EXPORT KdTree : public osg::Shape * retun true on success. */ virtual bool build(BuildOptions& buildOptions, osg::Geometry* geometry); - struct LineSegmentIntersection + + void setVertices(osg::Vec3Array* vertices) { _vertices = vertices; } + const osg::Vec3Array* getVertices() const { return _vertices.get(); } + + + typedef std::vector< unsigned int > Indices; + + // index in the VertexIndices vector + void setPrimitiveIndices(const Indices& indices) { _primitiveIndices = indices; } + Indices& getPrimitiveIndices() { return _primitiveIndices; } + const Indices& getPrimitiveIndices() const { return _primitiveIndices; } + + // vector containing the primitive vertex index data packed as no_vertice_indices then vertex indices ie. for points it's (1, p0), for lines (2, p0, p1) etc. + void setVertexIndices(const Indices& indices) { _vertexIndices = indices; } + Indices& getVertexIndices() { return _vertexIndices; } + const Indices& getVertexIndices() const { return _vertexIndices; } + + + inline unsigned int addPoint(unsigned int p0) { - LineSegmentIntersection(): - ratio(-1.0), - p0(0), - p1(0), - p2(0), - r0(0.0f), - r1(0.0f), - r2(0.0f), - primitiveIndex(0) {} + unsigned int i = _vertexIndices.size(); + _primitiveIndices.push_back(i); + _vertexIndices.push_back(1); + _vertexIndices.push_back(p0); + return i; + } + inline unsigned int addLine(unsigned int p0, unsigned int p1) + { + unsigned int i = _vertexIndices.size(); + _primitiveIndices.push_back(i); + _vertexIndices.push_back(2); + _vertexIndices.push_back(p0); + _vertexIndices.push_back(p1); + return i; + } - bool operator < (const LineSegmentIntersection& rhs) const { return ratio < rhs.ratio; } + inline unsigned int addTriangle(unsigned int p0, unsigned int p1, unsigned int p2) + { + unsigned int i = _vertexIndices.size(); + _primitiveIndices.push_back(i); + _vertexIndices.push_back(3); + _vertexIndices.push_back(p0); + _vertexIndices.push_back(p1); + _vertexIndices.push_back(p2); + return i; + } - typedef std::vector IndexList; - typedef std::vector RatioList; + inline unsigned int addQuad(unsigned int p0, unsigned int p1, unsigned int p2, unsigned int p3) + { + unsigned int i = _vertexIndices.size(); + _primitiveIndices.push_back(i); + _vertexIndices.push_back(4); + _vertexIndices.push_back(p0); + _vertexIndices.push_back(p1); + _vertexIndices.push_back(p2); + _vertexIndices.push_back(p3); + return i; + } - double ratio; - osg::Vec3d intersectionPoint; - osg::Vec3 intersectionNormal; - - unsigned int p0; - unsigned int p1; - unsigned int p2; - float r0; - float r1; - float r2; - - unsigned int primitiveIndex; - }; - - - typedef std::vector LineSegmentIntersections; - - /** compute the intersection of a line segment and the kdtree, return true if an intersection has been found.*/ - virtual bool intersect(const osg::Vec3d& start, const osg::Vec3d& end, LineSegmentIntersections& intersections) const; typedef int value_type; @@ -103,31 +126,7 @@ class OSG_EXPORT KdTree : public osg::Shape value_type first; value_type second; }; - - struct Triangle - { - Triangle(): - p0(0),p1(0),p2(0) {} - - Triangle(unsigned int ip0, unsigned int ip1, unsigned int ip2): - p0(ip0), p1(ip1), p2(ip2) {} - - bool operator < (const Triangle& rhs) const - { - if (p0rhs.p0) return false; - if (p1rhs.p1) return false; - return p2 KdNodeList; - typedef std::vector< Triangle > TriangleList; int addNode(const KdNode& node) { @@ -142,21 +141,6 @@ class OSG_EXPORT KdTree : public osg::Shape KdNodeList& getNodes() { return _kdNodes; } const KdNodeList& getNodes() const { return _kdNodes; } - void setVertices(osg::Vec3Array* vertices) { _vertices = vertices; } - const osg::Vec3Array* getVertices() const { return _vertices.get(); } - - unsigned int addTriangle(const Triangle& tri) - { - unsigned int num = static_cast(_triangles.size()); - _triangles.push_back(tri); - return num; - } - - Triangle& getTriangle(unsigned int i) { return _triangles[i]; } - const Triangle& getTriangle(unsigned int i) const { return _triangles[i]; } - - TriangleList& getTriangles() { return _triangles; } - const TriangleList& getTriangles() const { return _triangles; } template void intersect(IntersectFunctor& functor, const KdNode& node) const @@ -169,11 +153,16 @@ class OSG_EXPORT KdTree : public osg::Shape for(int i=istart; i _vertices; - KdNodeList _kdNodes; - TriangleList _triangles; - + osg::ref_ptr _vertices; + Indices _primitiveIndices; + Indices _vertexIndices; + KdNodeList _kdNodes; }; class OSG_EXPORT KdTreeBuilder : public osg::NodeVisitor diff --git a/src/osg/KdTree.cpp b/src/osg/KdTree.cpp index 329b91bc1..5b09c8118 100644 --- a/src/osg/KdTree.cpp +++ b/src/osg/KdTree.cpp @@ -14,6 +14,7 @@ #include #include #include +#include #include #include @@ -26,6 +27,7 @@ using namespace osg; // // BuildKdTree Declarartion - class used for building an single KdTree + struct BuildKdTree { BuildKdTree(KdTree& kdTree): @@ -53,19 +55,54 @@ protected: BuildKdTree& operator = (const BuildKdTree&) { return *this; } }; -//////////////////////////////////////////////////////////////////////////////// -// -// Functor for collecting triangle indices from Geometry - -struct TriangleIndicesCollector +struct PrimitiveIndicesCollector { - TriangleIndicesCollector(): + PrimitiveIndicesCollector(): _buildKdTree(0) { } + inline void operator () (unsigned int p0) + { + OSG_NOTICE<<" point ("<_kdTree.getVertices()))[p0]; + + _buildKdTree->_kdTree.addPoint(p0); + + osg::BoundingBox bb; + bb.expandBy(v0); + + _buildKdTree->_primitiveIndices.push_back(_buildKdTree->_centers.size()); + _buildKdTree->_centers.push_back(bb.center()); + } + + inline void operator () (unsigned int p0, unsigned int p1) + { + OSG_NOTICE<<" line ("<_kdTree.getVertices()))[p0]; + const osg::Vec3& v1 = (*(_buildKdTree->_kdTree.getVertices()))[p1]; + + // discard degenerate points + if (v0==v1) + { + //OSG_NOTICE<<"Disgarding degenerate triangle"<_kdTree.addLine(p0,p1); + + osg::BoundingBox bb; + bb.expandBy(v0); + bb.expandBy(v1); + + _buildKdTree->_primitiveIndices.push_back(_buildKdTree->_centers.size()); + _buildKdTree->_centers.push_back(bb.center()); + } + inline void operator () (unsigned int p0, unsigned int p1, unsigned int p2) { + // OSG_NOTICE<<" triangle ("<_kdTree.getVertices()))[p0]; const osg::Vec3& v1 = (*(_buildKdTree->_kdTree.getVertices()))[p1]; const osg::Vec3& v2 = (*(_buildKdTree->_kdTree.getVertices()))[p2]; @@ -77,16 +114,43 @@ struct TriangleIndicesCollector return; } - unsigned int i = _buildKdTree->_kdTree.addTriangle(KdTree::Triangle(p0,p1,p2)); + _buildKdTree->_kdTree.addTriangle(p0,p1,p2); osg::BoundingBox bb; bb.expandBy(v0); bb.expandBy(v1); bb.expandBy(v2); + _buildKdTree->_primitiveIndices.push_back(_buildKdTree->_centers.size()); _buildKdTree->_centers.push_back(bb.center()); - _buildKdTree->_primitiveIndices.push_back(i); + } + inline void operator () (unsigned int p0, unsigned int p1, unsigned int p2, unsigned int p3) + { + OSG_NOTICE<<" quad ("<_kdTree.getVertices()))[p0]; + const osg::Vec3& v1 = (*(_buildKdTree->_kdTree.getVertices()))[p1]; + const osg::Vec3& v2 = (*(_buildKdTree->_kdTree.getVertices()))[p2]; + const osg::Vec3& v3 = (*(_buildKdTree->_kdTree.getVertices()))[p3]; + + // discard degenerate points + if (v0==v1 || v1==v2 || v2==v0 || v3==v0 || v3==v1 || v3==v2) + { + //OSG_NOTICE<<"Disgarding degenerate quad"<_kdTree.addQuad(p0,p1,p2,p3); + + osg::BoundingBox bb; + bb.expandBy(v0); + bb.expandBy(v1); + bb.expandBy(v2); + bb.expandBy(v3); + + _buildKdTree->_primitiveIndices.push_back(_buildKdTree->_centers.size()); + _buildKdTree->_centers.push_back(bb.center()); } BuildKdTree* _buildKdTree; @@ -129,11 +193,9 @@ bool BuildKdTree::build(KdTree::BuildOptions& options, osg::Geometry* geometry) _primitiveIndices.reserve(estimatedNumTriangles); _centers.reserve(estimatedNumTriangles); - _kdTree.getTriangles().reserve(estimatedNumTriangles); - - osg::TriangleIndexFunctor collectTriangleIndices; - collectTriangleIndices._buildKdTree = this; - geometry->accept(collectTriangleIndices); + osg::TemplatePrimitiveIndexFunctor collectIndices; + collectIndices._buildKdTree = this; + geometry->accept(collectIndices); _primitiveIndices.reserve(vertices->size()); @@ -145,14 +207,18 @@ bool BuildKdTree::build(KdTree::BuildOptions& options, osg::Geometry* geometry) 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]); - } + OSG_NOTICE<<"After KdTree setup"<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 || u0.0 || v_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_NOTICE<<" got intersection ("< #include #include +#include using namespace osgUtil; namespace LineSegmentIntersectorUtils { - struct TriangleIntersection + +struct Settings : public osg::Referenced +{ + Settings() : + _lineSegIntersector(0), + _iv(0), + _drawable(0), + _limitOneIntersection(false) {} + + osgUtil::LineSegmentIntersector* _lineSegIntersector; + osgUtil::IntersectionVisitor* _iv; + osg::Drawable* _drawable; + osg::ref_ptr _vertices; + bool _limitOneIntersection; +}; + +template +struct IntersectFunctor +{ + osg::ref_ptr _settings; + + unsigned int _primitiveIndex; + Vec3 _start; + Vec3 _end; + + typedef std::pair< Vec3, Vec3> StartEnd; + typedef std::vector< StartEnd > StartEndStack; + + StartEndStack _startEndStack; + + Vec3 _d; + value_type _length; + value_type _inverse_length; + + Vec3 _d_invX; + Vec3 _d_invY; + Vec3 _d_invZ; + + bool _hit; + + IntersectFunctor(): + _primitiveIndex(0), + _hit(false) { - TriangleIntersection(unsigned int index, const osg::Vec3& normal, float r1, const osg::Vec3* v1, float r2, const osg::Vec3* v2, float r3, const osg::Vec3* v3): - _index(index), - _normal(normal), - _r1(r1), - _v1(v1), - _r2(r2), - _v2(v2), - _r3(r3), - _v3(v3) {} - - unsigned int _index; - const osg::Vec3 _normal; - float _r1; - const osg::Vec3* _v1; - float _r2; - const osg::Vec3* _v2; - float _r3; - const osg::Vec3* _v3; - - protected: - - TriangleIntersection& operator = (const TriangleIntersection&) { return *this; } - }; - - typedef std::multimap TriangleIntersections; + } - template - struct TriangleIntersector + void set(const osg::Vec3d& s, const osg::Vec3d& e, Settings* settings) { - Vec3 _s; - Vec3 _d; - value_type _length; + _settings = settings; - int _index; - value_type _ratio; - bool _hit; - bool _limitOneIntersection; - TriangleIntersections* _intersections; + _start = s; + _end = e; - TriangleIntersector() + _startEndStack.push_back(StartEnd(_start,_end)); + + _d = e - s; + _length = _d.length(); + _inverse_length = (_length!=0.0) ? 1.0/_length : 0.0; + _d *= _inverse_length; + + _d_invX = _d.x()!=0.0 ? _d/_d.x() : Vec3(0.0,0.0,0.0); + _d_invY = _d.y()!=0.0 ? _d/_d.y() : Vec3(0.0,0.0,0.0); + _d_invZ = _d.z()!=0.0 ? _d/_d.z() : Vec3(0.0,0.0,0.0); + } + + bool enter(const osg::BoundingBox& bb) + { + StartEnd startend = _startEndStack.back(); + Vec3& s = startend.first; + Vec3& e = startend.second; + + //return true; + + //if (!bb.valid()) return true; + + // compare s and e against the xMin to xMax range of bb. + if (s.x()<=e.x()) { - _intersections = 0; - _length = 0.0f; - _index = 0; - _ratio = 0.0f; - _hit = false; - _limitOneIntersection = false; + + // trivial reject of segment wholely outside. + if (e.x()bb.xMax()) return false; + + if (s.x()bb.xMax()) + { + // clip e to xMax. + e = s+_d_invX*(bb.xMax()-s.x()); + } + } + else + { + if (s.x()bb.xMax()) return false; + + if (e.x()bb.xMax()) + { + // clip e to xMax. + s = s+_d_invX*(bb.xMax()-s.x()); + } } - void set(TriangleIntersections* intersections) + // compate s and e against the yMin to yMax range of bb. + if (s.y()<=e.y()) { - _intersections = intersections; + + // trivial reject of segment wholely outside. + if (e.y()bb.yMax()) return false; + + if (s.y()bb.yMax()) + { + // clip e to yMax. + e = s+_d_invY*(bb.yMax()-s.y()); + } + } + else + { + if (s.y()bb.yMax()) return false; + + if (e.y()bb.yMax()) + { + // clip e to yMax. + s = s+_d_invY*(bb.yMax()-s.y()); + } } - void set(const osg::Vec3d& start, const osg::Vec3d& end, value_type ratio=FLT_MAX) + // compate s and e against the zMin to zMax range of bb. + if (s.z()<=e.z()) { - _hit=false; - _index = 0; - _ratio = ratio; - _s = start; - _d = end - start; - _length = _d.length(); - _d /= _length; + // trivial reject of segment wholely outside. + if (e.z()bb.zMax()) return false; + + if (s.z()bb.zMax()) + { + // clip e to zMax. + e = s+_d_invZ*(bb.zMax()-s.z()); + } + } + else + { + if (s.z()bb.zMax()) return false; + + if (e.z()bb.zMax()) + { + // clip e to zMax. + s = s+_d_invZ*(bb.zMax()-s.z()); + } } - inline void operator () (const osg::Vec3& v1,const osg::Vec3& v2,const osg::Vec3& v3) + // OSG_NOTICE<<"clampped segment "<_limitOneIntersection && _hit) return; + + // OSG_NOTICE<<" intersect(v0=("<esplison) { - ++_index; + value_type u = (P*T); + if (u<0.0 || u>det) return; - if (_limitOneIntersection && _hit) return; + osg::Vec3 Q = T ^ E1; + value_type v = (Q*_d); + if (v<0.0 || v>det) return; - if (v1==v2 || v2==v3 || v1==v3) return; + if ((u+v)> det) return; - Vec3 v12 = v2-v1; - Vec3 n12 = v12^_d; - value_type ds12 = (_s-v1)*n12; - value_type d312 = (v3-v1)*n12; - if (d312>=0.0f) - { - if (ds12<0.0f) return; - if (ds12>d312) return; - } - else // d312 < 0 - { - if (ds12>0.0f) return; - if (ds12_length) return; - Vec3 v23 = v3-v2; - Vec3 n23 = v23^_d; - value_type ds23 = (_s-v2)*n23; - value_type d123 = (v1-v2)*n23; - if (d123>=0.0f) - { - if (ds23<0.0f) return; - if (ds23>d123) return; - } - else // d123 < 0 - { - if (ds23>0.0f) return; - if (ds23=0.0f) - { - if (ds31<0.0f) return; - if (ds31>d231) return; - } - else // d231 < 0 - { - if (ds31>0.0f) return; - if (ds310.0 || u0.0 || v_length) return; - value_type r2; - if (ds31==0.0f) r2=0.0f; - else if (d231!=0.0f) r2 = ds31/d231; - else return; // the triangle and the line must be parallel intersection. - - value_type total_r = (r1+r2+r3); - if (total_r!=1.0f) - { - if (total_r==0.0f) return; // the triangle and the line must be parallel intersection. - value_type inv_total_r = 1.0f/total_r; - r1 *= inv_total_r; - r2 *= inv_total_r; - r3 *= inv_total_r; - } - - Vec3 in = v1*r1+v2*r2+v3*r3; - if (!in.valid()) - { - OSG_WARN<<"Warning:: Picked up error in TriangleIntersect"<_length) return; - - Vec3 normal = v12^v23; - normal.normalize(); - - value_type r = d/_length; - - - _intersections->insert(std::pair(r,TriangleIntersection(_index-1,normal,r1,&v1,r2,&v2,r3,&v3))); - _hit = true; + u *= inv_det; + v *= inv_det; + r0 = 1.0-u-v; + r1 = u; + r2 = v; + r = t * _inverse_length; + } + else + { + return; } - }; + Vec3 in = v0*r0 + v1*r1 + v2*r2; + Vec3 normal = E1^E2; + normal.normalize(); -} + + LineSegmentIntersector::Intersection hit; + hit.ratio = r; + hit.matrix = _settings->_iv->getModelMatrix(); + hit.nodePath = _settings->_iv->getNodePath(); + hit.drawable = _settings->_drawable; + hit.primitiveIndex = _primitiveIndex; + + hit.localIntersectionPoint = in; + + //OSG_NOTICE<<" intersection ("<_vertices.valid()) + { + const osg::Vec3* first = &(_settings->_vertices->front()); + hit.indexList.reserve(3); + hit.ratioList.reserve(3); + + if (r0!=0.0f) + { + hit.indexList.push_back(&v0-first); + hit.ratioList.push_back(r0); + } + + if (r1!=0.0f) + { + hit.indexList.push_back(&v1-first); + hit.ratioList.push_back(r1); + } + + if (r2!=0.0f) + { + hit.indexList.push_back(&v2-first); + hit.ratioList.push_back(r2); + } + } + + _settings->_lineSegIntersector->insertIntersection(hit); + _hit = true; + } + + // handle lines + void operator()(const osg::Vec3&, bool /*treatVertexDataAsTemporary*/) + { + ++_primitiveIndex; + } + + void operator()(const osg::Vec3&, const osg::Vec3&, bool /*treatVertexDataAsTemporary*/) + { + ++_primitiveIndex; + } + + // handle triangles + void operator()(const osg::Vec3& v0, const osg::Vec3& v1, const osg::Vec3& v2, bool /*treatVertexDataAsTemporary*/) + { + ++_primitiveIndex; + intersect(v0,v1,v2); + } + + void operator()(const osg::Vec3& v0, const osg::Vec3& v1, const osg::Vec3& v2, const osg::Vec3& v3, bool /*treatVertexDataAsTemporary*/) + { + ++_primitiveIndex; + intersect(v0,v1,v3); + intersect(v1,v2,v3); + } + + bool intersect(const osg::Vec3Array*, int , unsigned int) + { + return false; + } + + bool intersect(const osg::Vec3Array*, int, unsigned int, unsigned int) + { + return false; + } + + bool intersect(const osg::Vec3Array* vertices, int primitiveIndex, unsigned int p0, unsigned int p1, unsigned int p2) + { + if (_settings->_limitOneIntersection && _hit) return false; + + _primitiveIndex = primitiveIndex; + + intersect((*vertices)[p0], (*vertices)[p1], (*vertices)[p2]); + return false; + } + + bool intersect(const osg::Vec3Array* vertices, int primitiveIndex, unsigned int p0, unsigned int p1, unsigned int p2, unsigned int p3) + { + if (_settings->_limitOneIntersection && _hit) return false; + + _primitiveIndex = primitiveIndex; + + intersect((*vertices)[p0], (*vertices)[p1], (*vertices)[p3]); + intersect((*vertices)[p1], (*vertices)[p2], (*vertices)[p3]); + return false; + } +}; + +} // namespace LineSegmentIntersectorUtils /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // @@ -315,157 +521,37 @@ void LineSegmentIntersector::intersect(osgUtil::IntersectionVisitor& iv, osg::Dr void LineSegmentIntersector::intersect(osgUtil::IntersectionVisitor& iv, osg::Drawable* drawable, const osg::Vec3d& s, const osg::Vec3d& e) { - osg::KdTree* kdTree = iv.getUseKdTreeWhenAvailable() ? dynamic_cast(drawable->getShape()) : 0; - if (kdTree) + if (reachedLimit()) return; + + osg::ref_ptr settings = new LineSegmentIntersectorUtils::Settings; + settings->_lineSegIntersector = this; + settings->_iv = &iv; + settings->_drawable = drawable; + settings->_limitOneIntersection = (_intersectionLimit == LIMIT_ONE_PER_DRAWABLE || _intersectionLimit == LIMIT_ONE); + + osg::Geometry* geometry = drawable->asGeometry(); + if (geometry) { - osg::KdTree::LineSegmentIntersections intersections; - intersections.reserve(4); - if (kdTree->intersect(s,e,intersections)) - { - // OSG_NOTICE<<"Got KdTree intersections"<_vertices = dynamic_cast(geometry->getVertexArray()); } - LineSegmentIntersectorUtils::TriangleIntersections intersections; + osg::KdTree* kdTree = iv.getUseKdTreeWhenAvailable() ? dynamic_cast(drawable->getShape()) : 0; if (getPrecisionHint()==USE_DOUBLE_CALCULATIONS) { - OSG_INFO<<"Using double intersections"< TriangleIntersector; - osg::TriangleFunctor< TriangleIntersector > ti; + osg::TemplatePrimitiveFunctor > intersector; + intersector.set(s,e, settings.get()); - ti.set(&intersections); - ti.set(s,e); - ti._limitOneIntersection = (_intersectionLimit == LIMIT_ONE_PER_DRAWABLE || _intersectionLimit == LIMIT_ONE); - drawable->accept(ti); + if (kdTree) kdTree->intersect(intersector, kdTree->getNode(0)); + else drawable->accept(intersector); } else { - OSG_INFO<<"Using float intersections"< TriangleIntersector; - osg::TriangleFunctor< TriangleIntersector > ti; + osg::TemplatePrimitiveFunctor > intersector; + intersector.set(s,e, settings.get()); - ti.set(&intersections); - ti.set(s,e); - ti._limitOneIntersection = (_intersectionLimit == LIMIT_ONE_PER_DRAWABLE || _intersectionLimit == LIMIT_ONE); - drawable->accept(ti); - } - - if (!intersections.empty()) - { - osg::Geometry* geometry = drawable->asGeometry(); - - for(LineSegmentIntersectorUtils::TriangleIntersections::iterator thitr = intersections.begin(); - thitr != intersections.end(); - ++thitr) - { - - // get ratio in s,e range - double ratio = thitr->first; - - // remap ratio into _start, _end range - double remap_ratio = ((s-_start).length() + ratio * (e-s).length() )/(_end-_start).length(); - - if ( _intersectionLimit == LIMIT_NEAREST && !getIntersections().empty() ) - { - if (remap_ratio >= getIntersections().begin()->ratio ) - break; - else - getIntersections().clear(); - } - - LineSegmentIntersectorUtils::TriangleIntersection& triHit = thitr->second; - - Intersection hit; - hit.ratio = remap_ratio; - hit.matrix = iv.getModelMatrix(); - hit.nodePath = iv.getNodePath(); - hit.drawable = drawable; - hit.primitiveIndex = triHit._index; - - hit.localIntersectionPoint = _start*(1.0-remap_ratio) + _end*remap_ratio; - - // OSG_NOTICE<<"Conventional: ratio="<(geometry->getVertexArray()); - if (vertices) - { - osg::Vec3* first = &(vertices->front()); - if (triHit._v1) - { - hit.indexList.push_back(triHit._v1-first); - hit.ratioList.push_back(triHit._r1); - } - if (triHit._v2) - { - hit.indexList.push_back(triHit._v2-first); - hit.ratioList.push_back(triHit._r2); - } - if (triHit._v3) - { - hit.indexList.push_back(triHit._v3-first); - hit.ratioList.push_back(triHit._r3); - } - } - } - - insertIntersection(hit); - - } + if (kdTree) kdTree->intersect(intersector, kdTree->getNode(0)); + else drawable->accept(intersector); } } diff --git a/src/osgUtil/PolytopeIntersector.cpp b/src/osgUtil/PolytopeIntersector.cpp index 957fe56e5..a8d07fae0 100644 --- a/src/osgUtil/PolytopeIntersector.cpp +++ b/src/osgUtil/PolytopeIntersector.cpp @@ -25,430 +25,396 @@ using namespace osgUtil; namespace PolytopeIntersectorUtils { - typedef osg::Plane::Vec3_type Vec3_type; - typedef Vec3_type::value_type value_type; - typedef osg::Polytope::ClippingMask PlaneMask; - typedef std::vector > CandList_t; +struct Settings : public osg::Referenced +{ + Settings() : + _polytopeIntersector(0), + _iv(0), + _drawable(0), + _limitOneIntersection(false) {} - class PolytopeIntersection { - public: - enum { MaxNumIntesections = PolytopeIntersector::Intersection::MaxNumIntesectionPoints }; + osgUtil::PolytopeIntersector* _polytopeIntersector; + osgUtil::IntersectionVisitor* _iv; + osg::Drawable* _drawable; + osg::ref_ptr _vertices; + bool _limitOneIntersection; +}; - PolytopeIntersection(unsigned int index, const CandList_t& cands, const osg::Plane &referencePlane) : - _maxDistance(-1.0), _index(index-1), _numPoints(0) +template +struct IntersectFunctor +{ + typedef typename Vec3::value_type value_type; + typedef Vec3 vec_type; + + typedef std::vector Vertices; + Vertices src, dest; + + osg::ref_ptr _settings; + unsigned int _primitiveIndex; + bool _hit; + + IntersectFunctor(): + _primitiveIndex(0), + _hit(false) + { + src.reserve(10); + dest.reserve(10); + } + + bool enter(const osg::BoundingBox& bb) + { + if (_settings->_polytopeIntersector->getPolytope().contains(bb)) { - Vec3_type center; - for (CandList_t::const_iterator it=cands.begin(); it!=cands.end(); ++it) - { - PlaneMask mask = it->first; - if (mask==0) continue; + _settings->_polytopeIntersector->getPolytope().pushCurrentMask(); - _points[_numPoints++] = it->second; - center += it->second; - value_type distance = referencePlane.distance(it->second); - if (distance > _maxDistance) _maxDistance = distance; - if (_numPoints==MaxNumIntesections) break; - } - if (_numPoints>0) + return true; + } + else + { + return false; + } + } + + void leave() + { + _settings->_polytopeIntersector->getPolytope().popCurrentMask(); + } + + void addIntersection() + { + + vec_type center(0.0,0.0,0.0); + double maxDistance = -DBL_MAX; + const osg::Plane& referencePlane = _settings->_polytopeIntersector->getReferencePlane(); + for(typename Vertices::iterator itr = src.begin(); + itr != src.end(); + ++itr) + { + center += *itr; + double d = referencePlane.distance(*itr); + if (d>maxDistance) maxDistance = d; + + } + + center /= value_type(src.size()); + + PolytopeIntersector::Intersection intersection; + intersection.primitiveIndex = _primitiveIndex; + intersection.distance = referencePlane.distance(center); + intersection.maxDistance = maxDistance; + intersection.nodePath = _settings->_iv->getNodePath(); + intersection.drawable = _settings->_drawable; + intersection.matrix = _settings->_iv->getModelMatrix(); + intersection.localIntersectionPoint = center; + + if (src.size()=0.0) + { + dest.push_back(*v_previous); + } + + if (d_previous*d_current<0.0) + { + // edge crosses plane so insert the vertex between them. + value_type distance = d_previous-d_current; + value_type r_current = d_previous/distance; + vec_type v_new = (*v_previous)*(1.0-r_current) + (*v_current)*r_current; + dest.push_back(v_new); + } + + d_previous = d_current; + v_previous = v_current; + + } + + if (d_previous>=0.0) + { + dest.push_back(*v_previous); + } + + if (dest.size()<=1) + { + // OSG_NOTICE<<"Polytope::contains() All points on triangle culled, dest.size()="<1.0f) continue; - - const Vec3_type q=s^e1; - const value_type v=f*(line.dir*q); - if (v<0.0f || u+v>1.0f) continue; - - const value_type t=f*(e2*q); - - _candidates.push_back(CandList_t::value_type(line.mask, line.pos+line.dir*t)); - } - - numCands=checkCandidatePoints(inside_mask); - - if (numCands>0) - { - addIntersection(_index, _candidates); - return; - } - + addIntersection(); } + } - /// handle quads - void operator()(const Vec3_type v1, const Vec3_type v2, const Vec3_type v3, const Vec3_type v4, bool treatVertexDataAsTemporary) + void operator()(const osg::Vec3& v0, const osg::Vec3& v1, const osg::Vec3& v2, const osg::Vec3& v3, bool /*treatVertexDataAsTemporary*/) + { + if (_settings->_limitOneIntersection && _hit) return; + + ++_primitiveIndex; + src.clear(); + + src.push_back(v0); + src.push_back(v1); + src.push_back(v2); + src.push_back(v3); + src.push_back(v0); + + if (contains()) { - if ((_dimensionMask & PolytopeIntersector::DimTwo) == 0) - { - ++_index; - return; - } - - this->operator()(v1,v2,v3,treatVertexDataAsTemporary); - - --_index; - - this->operator()(v1,v3,v4,treatVertexDataAsTemporary); + addIntersection(); } + } - void setDimensionMask(unsigned int dimensionMask) { _dimensionMask = dimensionMask; } + bool intersect(const osg::Vec3Array* vertices, int primitiveIndex, unsigned int p0) + { + if (_settings->_limitOneIntersection && _hit) return false; - void setLimitOneIntersection(bool limit) { _limitOneIntersection = limit; } - - void setPolytope(osg::Polytope& polytope, osg::Plane& referencePlane) + if (contains((*vertices)[p0])) { - _referencePlane = referencePlane; + _primitiveIndex = primitiveIndex; - const PlaneMask currentMask = polytope.getCurrentMask(); - PlaneMask selector_mask = 0x1; + addIntersection(); - const PlaneList& planeList = polytope.getPlaneList(); - unsigned int numActivePlanes = 0; - - PlaneList::const_iterator itr; - for(itr=planeList.begin(); itr!=planeList.end(); ++itr) - { - if (currentMask&selector_mask) ++numActivePlanes; - selector_mask <<= 1; - } - - _plane_mask = 0x0; - _planes.clear(); - _planes.reserve(numActivePlanes); - _lines.clear(); - - selector_mask=0x1; - for(itr=planeList.begin(); itr!=planeList.end(); ++itr) - { - if (currentMask&selector_mask) - { - _planes.push_back(*itr); - _plane_mask <<= 1; - _plane_mask |= 0x1; - } - selector_mask <<= 1; - } + return true; } - - - /// get boundary lines of polytope - LinesList& getPolytopeLines() + else { - if (!_lines.empty()) return _lines; - - PlaneMask selector_mask = 0x1; - for (PlaneList::const_iterator it=_planes.begin(); it!=_planes.end(); - ++it, selector_mask <<= 1 ) { - const osg::Plane& plane1=*it; - const Vec3_type normal1=plane1.getNormal(); - const Vec3_type point1=normal1*(-plane1[3]); /// canonical point on plane1 - PlaneMask sub_selector_mask = (selector_mask<<1); - for (PlaneList::const_iterator jt=it+1; jt!=_planes.end(); ++jt, sub_selector_mask <<= 1 ) { - const osg::Plane& plane2=*jt; - const Vec3_type normal2=plane2.getNormal(); - if (osg::absolute(normal1*normal2) > (1.0-eps())) continue; - const Vec3_type lineDirection = normal1^normal2; - - const Vec3_type searchDirection = lineDirection^normal1; /// search dir in plane1 - const value_type seachDist = -plane2.distance(point1)/(searchDirection*normal2); - if (osg::isNaN(seachDist)) continue; - const Vec3_type linePoint=point1+searchDirection*seachDist; - _lines.push_back(PlanesLine(selector_mask|sub_selector_mask, linePoint, lineDirection)); - } - } - return _lines; + return false; } + } - unsigned int getNumPlanes() const { return _planes.size(); } + bool intersect(const osg::Vec3Array* vertices, int primitiveIndex, unsigned int p0, unsigned int p1) + { + if (_settings->_limitOneIntersection && _hit) return false; - Intersections intersections; - osg::Plane _referencePlane; + if (contains((*vertices)[p0], (*vertices)[p1])) + { + _primitiveIndex = primitiveIndex; - unsigned int _index; + addIntersection(); - private: - bool _limitOneIntersection; - unsigned int _dimensionMask; - PlaneList _planes; ///< active planes extracted from polytope - LinesList _lines; ///< all intersection lines of two polytope planes - PlaneMask _plane_mask; ///< mask for all planes of the polytope - CandList_t _candidates; - }; // class PolytopePrimitiveIntersector + return true; + } + else + { + return false; + } + } -} // namespace PolytopeIntersectorUtils + bool intersect(const osg::Vec3Array* vertices, int primitiveIndex, unsigned int p0, unsigned int p1, unsigned int p2) + { + if (_settings->_limitOneIntersection && _hit) return false; + + if (contains((*vertices)[p0], (*vertices)[p1], (*vertices)[p2])) + { + _primitiveIndex = primitiveIndex; + + addIntersection(); + + return true; + } + else + { + return false; + } + } + + bool intersect(const osg::Vec3Array* vertices, int primitiveIndex, unsigned int p0, unsigned int p1, unsigned int p2, unsigned int p3) + { + if (_settings->_limitOneIntersection && _hit) return false; + + if (contains((*vertices)[p0], (*vertices)[p1], (*vertices)[p2], (*vertices)[p3])) + { + _primitiveIndex = primitiveIndex; + + addIntersection(); + + return true; + } + else + { + return false; + } + } + + +}; + +} // namespace PolytopeIntersectorUtils /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -510,6 +476,7 @@ Intersector* PolytopeIntersector::clone(osgUtil::IntersectionVisitor& iv) pi->_intersectionLimit = this->_intersectionLimit; pi->_dimensionMask = this->_dimensionMask; pi->_referencePlane = this->_referencePlane; + pi->setPrecisionHint(getPrecisionHint()); return pi.release(); } @@ -547,6 +514,7 @@ Intersector* PolytopeIntersector::clone(osgUtil::IntersectionVisitor& iv) pi->_dimensionMask = this->_dimensionMask; pi->_referencePlane = this->_referencePlane; pi->_referencePlane.transformProvidingInverse(matrix); + pi->setPrecisionHint(getPrecisionHint()); return pi.release(); } @@ -562,272 +530,6 @@ void PolytopeIntersector::leave() // do nothing. } -struct IntersectFunctor -{ - - typedef std::vector Vertices; - - IntersectFunctor(): - _polytopeIntersector(0), - _iv(0), - _drawable(0), - _primitiveIndex(0) - { - src.reserve(10); - dest.reserve(10); - } - - bool enter(const osg::BoundingBox& bb) - { - if (_polytopeIntersector->getPolytope().contains(bb)) - { - _polytopeIntersector->getPolytope().pushCurrentMask(); - return true; - } - else - { - return false; - } - } - - void leave() - { - _polytopeIntersector->getPolytope().popCurrentMask(); - } - - void addIntersection() - { - osg::Vec3d center(0.0,0.0,0.0); - double maxDistance = -DBL_MAX; - const osg::Plane& referencePlane = _polytopeIntersector->getReferencePlane(); - for(Vertices::iterator itr = src.begin(); - itr != src.end(); - ++itr) - { - center += *itr; - double d = referencePlane.distance(*itr); - if (d>maxDistance) maxDistance = d; - - } - - center /= double(src.size()); - - PolytopeIntersector::Intersection intersection; - intersection.primitiveIndex = _primitiveIndex; - intersection.distance = referencePlane.distance(center); - intersection.maxDistance = maxDistance; - intersection.nodePath = _iv->getNodePath(); - intersection.drawable = _drawable; - intersection.matrix = _iv->getModelMatrix(); - intersection.localIntersectionPoint = center; - - if (src.size()=0.0) - { - dest.push_back(*v_previous); - - } - - if (d_previous*d_current<0.0) - { - // edge crosses plane so insert the vertex between them. - double distance = d_previous-d_current; - double r_current = d_previous/distance; - osg::Vec3d v_new = (*v_previous)*(1.0-r_current) + (*v_current)*r_current; - dest.push_back(v_new); - } - - d_previous = d_current; - v_previous = v_current; - - } - - if (d_previous>=0.0) - { - dest.push_back(*v_previous); - } - - if (dest.size()<=1) - { - // OSG_NOTICE<<"Polytope::contains() All points on triangle culled, dest.size()="<