diff --git a/examples/osgspheresegment/osgspheresegment.cpp b/examples/osgspheresegment/osgspheresegment.cpp index 99c79ab1c..76162b645 100644 --- a/examples/osgspheresegment/osgspheresegment.cpp +++ b/examples/osgspheresegment/osgspheresegment.cpp @@ -337,7 +337,24 @@ void build_world(osg::Group *root) osgSim::SphereSegment::LineList lines = ss->computeIntersection(osg::Matrixd::identity(), terrainGeode.get()); if (!lines.empty()) { - osg::notify(osg::NOTICE)<<"We've found intersections!!!!"<addChild(geode); + + geode->getOrCreateStateSet()->setMode(GL_LIGHTING,osg::StateAttribute::OFF); + + for(osgSim::SphereSegment::LineList::iterator itr=lines.begin(); + itr!=lines.end(); + ++itr) + { + osg::Geometry* geom = new osg::Geometry; + geode->addDrawable(geom); + + osg::Vec3Array* vertices = itr->get(); + geom->setVertexArray(vertices); + geom->addPrimitiveSet(new osg::DrawArrays(GL_LINE_STRIP, 0, vertices->getNumElements())); + } } else { diff --git a/src/osgSim/SphereSegment.cpp b/src/osgSim/SphereSegment.cpp index 1762a4b0b..2a43d23b8 100644 --- a/src/osgSim/SphereSegment.cpp +++ b/src/osgSim/SphereSegment.cpp @@ -22,6 +22,7 @@ #include #include +#include using namespace osgSim; @@ -1118,6 +1119,32 @@ SphereSegment::LineList SphereSegment::computeIntersection(const osg::Matrixd& t return all_lines; } + +struct dereference_less +{ + template + inline bool operator() (const T& lhs,const U& rhs) const + { + return *lhs < *rhs; + } +}; + +struct SortFunctor +{ + typedef std::vector< osg::Vec3 > VertexArray; + + SortFunctor(VertexArray& vertices): + _vertices(vertices) {} + + bool operator() (unsigned int p1, unsigned int p2) const + { + return _vertices[p1]<_vertices[p2]; + } + + VertexArray& _vertices; +}; + + struct TriangleIntersectOperator { @@ -1131,37 +1158,175 @@ struct TriangleIntersectOperator _numInside(0), _numIntersecting(0) {} - typedef std::vector PositionArray; - typedef std::vector RegionArray; + class Triangle; + + struct Edge : public osg::Referenced + { + typedef std::vector TriangleList; + + enum IntersectionType + { + NO_INTERSECTION, + POINT_1, + POINT_2, + MID_POINT, + BOTH_ENDS + }; + + Edge(unsigned int p1, unsigned int p2) + { + if (p1>p2) + { + _p1 = p2; + _p2 = p1; + } + else + { + _p1 = p1; + _p2 = p2; + } + } - PositionArray _positions; - RegionArray _regions; - osg::Vec3 _centre; - float _radius; - float _azMin, _azMax, _elevMin, _elevMax; + bool operator < (const Edge& edge) const + { + if (_p1edge._p1) return false; + else return _p2 rhs._p1) return false; + else if (_p2 < rhs._p2) return true; + else if (_p2 > rhs._p2) return false; + else return (_p3 < rhs._p3); + } + + bool operator == (const Triangle& rhs) const + { + return (_p1 == rhs._p1) && (_p2 != rhs._p2) && (_p3 != rhs._p3); + } + + bool operator != (const Triangle& rhs) const + { + return (_p1 != rhs._p1) || (_p2 != rhs._p2) || (_p3 != rhs._p3); + } + + void sort() + { + if (_p1>_p2) std::swap(_p1,_p2); + if (_p1>_p3) std::swap(_p1,_p3); + if (_p2>_p3) std::swap(_p2,_p3); + } + + Edge* oppositeActiveEdge(Edge* edge) + { + if (edge!=_e1 && edge!=_e2 && edge!=_e3) + { + osg::notify(osg::NOTICE)<<"Edge problem"<_intersectionType!=Edge::NO_INTERSECTION) return _e1; + if (edge!=_e2 && _e2 && _e2->_intersectionType!=Edge::NO_INTERSECTION) return _e2; + if (edge!=_e3 && _e3 && _e3->_intersectionType!=Edge::NO_INTERSECTION) return _e3; + return 0; + } + + + unsigned int _p1; + unsigned int _p2; + unsigned int _p3; + + Edge* _e1; + Edge* _e2; + Edge* _e3; + }; + + typedef std::vector< osg::Vec3 > VertexArray; + typedef std::vector< int > RegionArray; + typedef std::vector< bool > BoolArray; + typedef std::vector< unsigned int > IndexArray; + typedef std::vector< osg::ref_ptr > TriangleArray; + typedef std::set< osg::ref_ptr, dereference_less > EdgeSet; + typedef std::list< osg::ref_ptr > EdgeList; + + VertexArray _originalVertices; + RegionArray _regions; + BoolArray _vertexInIntersectionSet; + IndexArray _candidateVertexIndices; + IndexArray _remapIndices; + TriangleArray _triangles; + EdgeSet _edges; + + osg::Vec3 _centre; + float _radius; + float _azMin, _azMax, _elevMin, _elevMax; + + unsigned int _numOutside; + unsigned int _numInside; + unsigned int _numIntersecting; + + SphereSegment::LineList _generatedLines; void computePositionAndRegions(const osg::Matrixd& matrix, osg::Vec3Array& array) { - _positions.resize(array.size()); + _originalVertices.resize(array.size()); _regions.resize(array.size(), 1); + _vertexInIntersectionSet.resize(array.size(), false); + _candidateVertexIndices.clear(); + + float radius2 = _radius*_radius; for(unsigned int i=0; i_radius) + _originalVertices[i] = vertex; + float rad2 = vertex.length2(); + if (rad2 > radius2) { _regions[i] = 1; } else { - float inv_length_xy = sqrtf(vertex.x()*vertex.x() + vertex.y()*vertex.y()); - float elevation = atan2(vertex.z(),inv_length_xy); + _regions[i] = -1; +#if 0 + float length_xy = sqrtf(vertex.x()*vertex.x() + vertex.y()*vertex.y()); + float elevation = atan2(vertex.z(),length_xy); if (elevation<_elevMin || elevation>_elevMax) { _regions[i] = 1; @@ -1176,7 +1341,7 @@ struct TriangleIntersectOperator } else { - if (radius==_radius || elevation==_elevMin || elevation==_elevMax || azim==_azMin || azim==_azMax) + if (rad2==radius2 || elevation==_elevMin || elevation==_elevMax || azim==_azMin || azim==_azMax) { _regions[i] = 0; } @@ -1187,7 +1352,7 @@ struct TriangleIntersectOperator } } - +#endif } } @@ -1210,11 +1375,355 @@ struct TriangleIntersectOperator } ++_numIntersecting; + + _triangles.push_back(new Triangle(p1,p2,p3)); + + if (!_vertexInIntersectionSet[p1]) + { + _vertexInIntersectionSet[p1] = true; + _candidateVertexIndices.push_back(p1); + } + + if (!_vertexInIntersectionSet[p2]) + { + _vertexInIntersectionSet[p2] = true; + _candidateVertexIndices.push_back(p2); + } + + if (!_vertexInIntersectionSet[p3]) + { + _vertexInIntersectionSet[p3] = true; + _candidateVertexIndices.push_back(p3); + } } + + void removeDuplicateVertices() + { + osg::notify(osg::NOTICE)<<"Removing duplicates : num vertices in "<<_candidateVertexIndices.size()<_p1 = _remapIndices[(*titr)->_p1]; + (*titr)->_p2 = _remapIndices[(*titr)->_p2]; + (*titr)->_p3 = _remapIndices[(*titr)->_p3]; + (*titr)->sort(); + } + } + } + + void removeDuplicateTriangles() + { + osg::notify(osg::NOTICE)<<"Removing duplicate triangles : num triangles in "<<_triangles.size()<get(); + tri->_e1 = addEdge(tri->_p1, tri->_p2, tri); + tri->_e2 = addEdge(tri->_p2, tri->_p3, tri); + tri->_e3 = addEdge(tri->_p1, tri->_p3, tri); + } + osg::notify(osg::NOTICE)<<"Number of edges "<<_edges.size()<get(); + unsigned int numConnections = edge->_triangles.size(); + if (numConnections==0) ++numZeroConnections; + else if (numConnections==1) ++numSingleConnections; + else if (numConnections==2) ++numDoubleConnections; + else ++numMultiConnections; + } + + osg::notify(osg::NOTICE)<<"Number of numZeroConnections "< edge = new Edge(p1, p2); + EdgeSet::iterator itr = _edges.find(edge); + if (itr==_edges.end()) + { + edge->addTriangle(tri); + _edges.insert(edge); + return edge.get(); + } + else + { + Edge* edge = const_cast(itr->get()); + edge->addTriangle(tri); + return edge; + } + } + + template + void computeIntersections(I intersector) + { + // collect all the intersecting edges + EdgeList hitEdges; + for(EdgeSet::iterator itr = _edges.begin(); + itr != _edges.end(); + ++itr) + { + Edge* edge = const_cast(itr->get()); + if (intersector(edge)) + { + hitEdges.push_back(edge); + } + } + osg::notify(osg::NOTICE)<<"Number of edge intersections "<get(); + edge->_toTraverse.clear(); + osg::notify(osg::NOTICE)<<"edge= "<_triangles.begin(); + titr != edge->_triangles.end(); + ++titr) + { + Triangle* tri = *titr; + + // count how many active edges there are on this triangle + unsigned int numActiveEdges = 0; + unsigned int numEdges = 0; + if (tri->_e1 && tri->_e1->_intersectionType!=Edge::NO_INTERSECTION) ++numActiveEdges; + if (tri->_e2 && tri->_e2->_intersectionType!=Edge::NO_INTERSECTION) ++numActiveEdges; + if (tri->_e3 && tri->_e3->_intersectionType!=Edge::NO_INTERSECTION) ++numActiveEdges; + + if (tri->_e1) ++numEdges; + if (tri->_e2) ++numEdges; + if (tri->_e3) ++numEdges; + + // if we have one or more then add it into the edges to traverse list + if (numActiveEdges>1) + { + osg::notify(osg::NOTICE)<<" adding tri="<_toTraverse.push_back(tri); + } + + // osg::notify(osg::NOTICE)<<"Number active edges "<get(); + osg::notify(osg::NOTICE)<<"edge= "<_toTraverse.begin(); + titr != edge->_toTraverse.end(); + ++titr) + { + Triangle* tri = *titr; + osg::notify(osg::NOTICE)<<" "<get(); + if (edge->_toTraverse.size()==1) break; + } + + if (hitr == hitEdges.end()) + { + hitr = hitEdges.begin(); + } + + osg::notify(osg::NOTICE)<<"New line "<get(); + while (edge) + { + osg::notify(osg::NOTICE)<<" vertex "<_intersectionVertex<push_back(edge->_intersectionVertex+_centre/*+osg::Vec3(0.0f,0.0f,200.0f)*/); + + Edge* newEdge = 0; + + Triangle* tri = !(edge->_toTraverse.empty()) ? edge->_toTraverse.back() : 0; + if (tri) + { + + newEdge = tri->oppositeActiveEdge(edge); + + edge->removeFromToTraverseList(tri); + newEdge->removeFromToTraverseList(tri); + + osg::notify(osg::NOTICE)<<" tri="<_toTraverse.empty()) + { + edge->_intersectionType = Edge::NO_INTERSECTION; + + // remove edge for the hitEdges. + hitr = find(hitEdges.begin(), hitEdges.end(), edge); + if (hitr!=hitEdges.end()) hitEdges.erase(hitr); + } + + // move on to next edge in line. + edge = newEdge; + + } + + } + } }; +struct RadiusIntersector +{ + RadiusIntersector(TriangleIntersectOperator& tif): + _tif(tif) {} + + TriangleIntersectOperator& _tif; + + inline bool operator() (TriangleIntersectOperator::Edge* edge) + { + edge->_intersectionType = TriangleIntersectOperator::Edge::NO_INTERSECTION; + + osg::Vec3& v1 = _tif._originalVertices[edge->_p1]; + osg::Vec3& v2 = _tif._originalVertices[edge->_p2]; + float radius1 = v1.length(); + float radius2 = v2.length(); + + // if both points inside then disgard + if (radius1<_tif._radius && radius2<_tif._radius) return false; + + // if both points outside then disgard + if (radius1>_tif._radius && radius2>_tif._radius) return false; + + if (radius1==_tif._radius) + { + if (radius2==_tif._radius) + { + edge->_intersectionType = TriangleIntersectOperator::Edge::BOTH_ENDS; + } + else + { + edge->_intersectionType = TriangleIntersectOperator::Edge::POINT_1; + } + } + else if (radius2==_tif._radius) + { + edge->_intersectionType = TriangleIntersectOperator::Edge::POINT_2; + } + else + { + edge->_intersectionType = TriangleIntersectOperator::Edge::MID_POINT; + float r = (_tif._radius-radius1)/(radius2-radius1); + float one_minus_r = 1.0f-r; + edge->_intersectionVertex = v1*one_minus_r + v2*r; + } + + return true; + } +}; SphereSegment::LineList SphereSegment::computeIntersection(const osg::Matrixd& matrix, osg::Drawable* drawable) { @@ -1244,6 +1753,12 @@ SphereSegment::LineList SphereSegment::computeIntersection(const osg::Matrixd& m osg::notify(osg::NOTICE)<<"_numOutside = "<