diff --git a/src/osgSim/SphereSegment.cpp b/src/osgSim/SphereSegment.cpp index e2e057c79..df009208c 100644 --- a/src/osgSim/SphereSegment.cpp +++ b/src/osgSim/SphereSegment.cpp @@ -1077,7 +1077,7 @@ class PolytopeVisitor : public osg::NodeVisitor SphereSegment::LineList SphereSegment::computeIntersection(const osg::Matrixd& transform, osg::Node* subgraph) { - osg::notify(osg::NOTICE)<<"Creating line intersection between sphere segment and subgraph."< - 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 +namespace SphereSegmentIntersector { - TriangleIntersectOperator(): - _radius(-1.0f), - _azMin(0.0f), - _azMax(0.0f), - _elevMin(0.0f), - _elevMax(0.0f), - _numOutside(0), - _numInside(0), - _numIntersecting(0) {} - - enum SurfaceType + struct dereference_less { - NO_SURFACE = 0, - RADIUS_SURFACE, - LEFT_SURFACE, - RIGHT_SURFACE, - BOTTOM_SURFACE, - TOP_SURFACE + template + inline bool operator() (const T& lhs,const U& rhs) const + { + return *lhs < *rhs; + } }; - struct Triangle; - - struct Edge : public osg::Referenced + struct SortFunctor { - typedef std::vector TriangleList; - - enum IntersectionType + typedef std::vector< osg::Vec3 > VertexArray; + + SortFunctor(VertexArray& vertices): + _vertices(vertices) {} + + bool operator() (unsigned int p1, unsigned int p2) const { - NO_INTERSECTION, - POINT_1, - POINT_2, - MID_POINT, - BOTH_ENDS + return _vertices[p1]<_vertices[p2]; + } + + VertexArray& _vertices; + }; + + + struct TriangleIntersectOperator + { + + TriangleIntersectOperator(): + _radius(-1.0f), + _azMin(0.0f), + _azMax(0.0f), + _elevMin(0.0f), + _elevMax(0.0f), + _numOutside(0), + _numInside(0), + _numIntersecting(0) {} + + enum SurfaceType + { + NO_SURFACE = 0, + RADIUS_SURFACE, + LEFT_SURFACE, + RIGHT_SURFACE, + BOTTOM_SURFACE, + TOP_SURFACE }; - Edge(unsigned int p1, unsigned int p2, SurfaceType intersectionEdge=NO_SURFACE): - _intersectionType(NO_INTERSECTION), - _intersectionVertexIndex(0), - _p1Outside(false), - _p2Outside(false), - _intersectionEdge(intersectionEdge) + struct Triangle; + + struct Edge : public osg::Referenced { - if (p1>p2) + typedef std::vector TriangleList; + + enum IntersectionType { - _p1 = p2; - _p2 = p1; + NO_INTERSECTION, + POINT_1, + POINT_2, + MID_POINT, + BOTH_ENDS + }; + + Edge(unsigned int p1, unsigned int p2, SurfaceType intersectionEdge=NO_SURFACE): + _intersectionType(NO_INTERSECTION), + _intersectionVertexIndex(0), + _p1Outside(false), + _p2Outside(false), + _intersectionEdge(intersectionEdge) + { + if (p1>p2) + { + _p1 = p2; + _p2 = p1; + } + else + { + _p1 = p1; + _p2 = p2; + } } - else + + bool operator < (const Edge& edge) const { - _p1 = p1; - _p2 = p2; + if (_p1edge._p1) return false; + else return _p2edge._p1) return false; - else return _p2 > EdgeList; - - struct Triangle : public osg::Referenced - { - - Triangle(unsigned int p1, unsigned int p2, unsigned int p3): - _p1(p1), _p2(p2), _p3(p3), - _e1(0), _e2(0), _e3(0) - { - sort(); - } - - bool operator < (const Triangle& rhs) const - { - if (_p1 < rhs._p1) return true; - else if (_p1 > 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) + inline void addTriangle(Triangle* tri) { - osg::notify(osg::NOTICE)<<"Edge problem"< > EdgeList; + + struct Triangle : public osg::Referenced + { + + Triangle(unsigned int p1, unsigned int p2, unsigned int p3): + _p1(p1), _p2(p2), _p3(p3), + _e1(0), _e2(0), _e3(0) + { + sort(); + } + + bool operator < (const Triangle& rhs) const + { + if (_p1 < rhs._p1) return true; + else if (_p1 > 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::INFO)<<"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; } - - if (edge!=_e1 && _e1 && _e1->_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; + unsigned int _p1; + unsigned int _p2; + unsigned int _p3; - Edge* _e1; - Edge* _e2; - Edge* _e3; - }; - - - struct Region - { - enum Classification - { - INSIDE = -1, - INTERSECTS = 0, - OUTSIDE = 1 + Edge* _e1; + Edge* _e2; + Edge* _e3; }; - - Region(): - _radiusSurface(OUTSIDE), - _leftSurface(OUTSIDE), - _rightSurface(OUTSIDE), - _bottomSurface(OUTSIDE), - _topSurface(OUTSIDE) {} - - - - void classify(const osg::Vec3& vertex, double radius2, double azimMin, double azimMax, double elevMin, double elevMax) + + + struct Region { - double azimCenter = (azimMax+azimMin)*0.5; - double azimRange = (azimMax-azimMin)*0.5; - - double rad2 = vertex.length2(); - double length_xy = sqrtf(vertex.x()*vertex.x() + vertex.y()*vertex.y()); - double elevation = atan2((double)vertex.z(),length_xy); - - // radius surface - if (rad2 > radius2) _radiusSurface = OUTSIDE; - else if (rad2 < radius2) _radiusSurface = INSIDE; - else _radiusSurface = INTERSECTS; - - // bottom surface - if (elevationelevMin) _bottomSurface = INSIDE; - else _bottomSurface = INTERSECTS; - - // top surface - if (elevation>elevMax) _topSurface = OUTSIDE; - else if (elevation0.0) _leftSurface = INSIDE; - else _leftSurface = INTERSECTS; - - double dot_alphaMax = cos(azimMax)*vertex.x() - sin(azimMax)*vertex.y(); - if (dot_alphaMax>0.0) _rightSurface = OUTSIDE; - else if (dot_alphaMax<0.0) _rightSurface = INSIDE; - else _rightSurface = INTERSECTS; - - double azim = atan2(vertex.x(),vertex.y()); - double azimDelta1 = azim-azimCenter; - double azimDelta2 = 2.0*osg::PI + azim-azimCenter; - double azimDelta = std::min(fabs(azimDelta1), fabs(azimDelta2)); - if (azimDelta>azimRange) + enum Classification { - _leftRightSurfaces = OUTSIDE; - } - else if (azimDelta radius2) _radiusSurface = OUTSIDE; + else if (rad2 < radius2) _radiusSurface = INSIDE; + else _radiusSurface = INTERSECTS; + + // bottom surface + if (elevationelevMin) _bottomSurface = INSIDE; + else _bottomSurface = INTERSECTS; + + // top surface + if (elevation>elevMax) _topSurface = OUTSIDE; + else if (elevation0.0) _leftSurface = INSIDE; + else _leftSurface = INTERSECTS; - if (region._leftRightSurfaces == Region::OUTSIDE) ++_outside_leftRightSurfaces; - if (region._leftRightSurfaces == Region::INSIDE) ++_inside_leftRightSurfaces; - if (region._leftRightSurfaces == Region::INTERSECTS) ++_intersects_leftRightSurfaces; + double dot_alphaMax = cos(azimMax)*vertex.x() - sin(azimMax)*vertex.y(); + if (dot_alphaMax>0.0) _rightSurface = OUTSIDE; + else if (dot_alphaMax<0.0) _rightSurface = INSIDE; + else _rightSurface = INTERSECTS; - if (region._leftSurface == Region::OUTSIDE) ++_outside_leftSurface; - if (region._leftSurface == Region::INSIDE) ++_inside_leftSurface; - if (region._leftSurface == Region::INTERSECTS) ++_intersects_leftSurface; - - if (region._rightSurface == Region::OUTSIDE) ++_outside_rightSurface; - if (region._rightSurface == Region::INSIDE) ++_inside_rightSurface; - if (region._rightSurface == Region::INTERSECTS) ++_intersects_rightSurface; - - if (region._bottomSurface == Region::OUTSIDE) ++_outside_bottomSurface; - if (region._bottomSurface == Region::INSIDE) ++_inside_bottomSurface; - if (region._bottomSurface == Region::INTERSECTS) ++_intersects_bottomSurface; - - if (region._topSurface == Region::OUTSIDE) ++_outside_topSurface; - if (region._topSurface == Region::INSIDE) ++_inside_topSurface; - if (region._topSurface == Region::INTERSECTS) ++_intersects_topSurface; - } - - Region::Classification overallClassification() const - { - // if all vertices are outside any of the surfaces then we are completely outside - if (_outside_radiusSurface==_numVertices || - _outside_leftRightSurfaces==_numVertices || - _outside_topSurface==_numVertices || - _outside_bottomSurface==_numVertices) return Region::OUTSIDE; - - // if all the vertices on all the sides and inside then we are completely inside - if (_inside_radiusSurface==_numVertices && - _inside_leftRightSurfaces==_numVertices && - _inside_topSurface==_numVertices && - _inside_bottomSurface==_numVertices) return Region::INSIDE; - - return Region::INTERSECTS; - } - - bool intersecting(SurfaceType surfaceType) const - { - // if all vertices are outside any of the surfaces then we are completely outside - if ((surfaceType!=RADIUS_SURFACE && _outside_radiusSurface!=0) || - (surfaceType!=LEFT_SURFACE && _outside_leftSurface!=0) || - (surfaceType!=RIGHT_SURFACE && _outside_rightSurface!=0) || - (surfaceType!=TOP_SURFACE && _outside_topSurface!=0) || - (surfaceType!=BOTTOM_SURFACE && _outside_bottomSurface!=0)) return false; - - // if all the vertices on all the sides and inside then we are completely inside - if ((surfaceType!=RADIUS_SURFACE && _inside_radiusSurface!=0) && - (surfaceType!=LEFT_SURFACE && _inside_leftSurface!=0) && - (surfaceType!=RIGHT_SURFACE && _inside_rightSurface!=0) && - (surfaceType!=TOP_SURFACE && _inside_topSurface!=0) && - (surfaceType!=BOTTOM_SURFACE && _inside_bottomSurface!=0)) return false; - - return true; - } - - int numberOfIntersectingSurfaces() const - { - int sidesThatIntersect = 0; - if (_outside_radiusSurface!=_numVertices && _inside_radiusSurface!=_numVertices) ++sidesThatIntersect; - if (_outside_leftSurface!=_numVertices && _inside_leftSurface!=_numVertices) ++sidesThatIntersect; - if (_outside_rightSurface!=_numVertices && _inside_rightSurface!=_numVertices) ++sidesThatIntersect; - if (_outside_topSurface!=_numVertices && _inside_topSurface!=_numVertices) ++sidesThatIntersect; - if (_outside_bottomSurface!=_numVertices && _inside_bottomSurface!=_numVertices) ++sidesThatIntersect; - return sidesThatIntersect; - } - - unsigned int _numVertices; - unsigned int _outside_radiusSurface; - unsigned int _inside_radiusSurface; - unsigned int _intersects_radiusSurface; - - unsigned int _outside_leftRightSurfaces; - unsigned int _inside_leftRightSurfaces; - unsigned int _intersects_leftRightSurfaces; - - unsigned int _outside_leftSurface; - unsigned int _inside_leftSurface; - unsigned int _intersects_leftSurface; - - unsigned int _outside_rightSurface; - unsigned int _inside_rightSurface; - unsigned int _intersects_rightSurface; - - unsigned int _outside_bottomSurface; - unsigned int _inside_bottomSurface; - unsigned int _intersects_bottomSurface; - - unsigned int _outside_topSurface; - unsigned int _inside_topSurface; - unsigned int _intersects_topSurface; - - }; - - - typedef std::vector< osg::Vec3 > VertexArray; - typedef std::vector< Region > 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; - - VertexArray _originalVertices; - RegionArray _regions; - BoolArray _vertexInIntersectionSet; - IndexArray _candidateVertexIndices; - IndexArray _remapIndices; - TriangleArray _triangles; - EdgeSet _edges; - - osg::Vec3 _centre; - double _radius; - double _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) - { - _originalVertices.resize(array.size()); - _regions.resize(array.size()); - _vertexInIntersectionSet.resize(array.size(), false); - _candidateVertexIndices.clear(); - - double radius2 = _radius*_radius; - - for(unsigned int i=0; i_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()<azimRange) { - _triangles[lastUniqueTriangle] = _triangles[i]; + _leftRightSurfaces = OUTSIDE; + } + else if (azimDelta_e1 = addEdge(tri->_p1, tri->_p2, tri); - tri->_e2 = addEdge(tri->_p2, tri->_p3, tri); - tri->_e3 = addEdge(tri->_p1, tri->_p3, tri); - } - void buildEdges() - { - _edges.clear(); - for(TriangleArray::iterator itr = _triangles.begin(); - itr != _triangles.end(); - ++itr) - { - Triangle* tri = itr->get(); + Region::Classification overallClassification() const + { + // if all vertices are outside any of the surfaces then we are completely outside + if (_outside_radiusSurface==_numVertices || + _outside_leftRightSurfaces==_numVertices || + _outside_topSurface==_numVertices || + _outside_bottomSurface==_numVertices) return Region::OUTSIDE; + // if all the vertices on all the sides and inside then we are completely inside + if (_inside_radiusSurface==_numVertices && + _inside_leftRightSurfaces==_numVertices && + _inside_topSurface==_numVertices && + _inside_bottomSurface==_numVertices) return Region::INSIDE; + + return Region::INTERSECTS; + } + + bool intersecting(SurfaceType surfaceType) const + { + // if all vertices are outside any of the surfaces then we are completely outside + if ((surfaceType!=RADIUS_SURFACE && _outside_radiusSurface!=0) || + (surfaceType!=LEFT_SURFACE && _outside_leftSurface!=0) || + (surfaceType!=RIGHT_SURFACE && _outside_rightSurface!=0) || + (surfaceType!=TOP_SURFACE && _outside_topSurface!=0) || + (surfaceType!=BOTTOM_SURFACE && _outside_bottomSurface!=0)) return false; + + // if all the vertices on all the sides and inside then we are completely inside + if ((surfaceType!=RADIUS_SURFACE && _inside_radiusSurface!=0) && + (surfaceType!=LEFT_SURFACE && _inside_leftSurface!=0) && + (surfaceType!=RIGHT_SURFACE && _inside_rightSurface!=0) && + (surfaceType!=TOP_SURFACE && _inside_topSurface!=0) && + (surfaceType!=BOTTOM_SURFACE && _inside_bottomSurface!=0)) return false; + + return true; + } + + int numberOfIntersectingSurfaces() const + { + int sidesThatIntersect = 0; + if (_outside_radiusSurface!=_numVertices && _inside_radiusSurface!=_numVertices) ++sidesThatIntersect; + if (_outside_leftSurface!=_numVertices && _inside_leftSurface!=_numVertices) ++sidesThatIntersect; + if (_outside_rightSurface!=_numVertices && _inside_rightSurface!=_numVertices) ++sidesThatIntersect; + if (_outside_topSurface!=_numVertices && _inside_topSurface!=_numVertices) ++sidesThatIntersect; + if (_outside_bottomSurface!=_numVertices && _inside_bottomSurface!=_numVertices) ++sidesThatIntersect; + return sidesThatIntersect; + } + + unsigned int _numVertices; + unsigned int _outside_radiusSurface; + unsigned int _inside_radiusSurface; + unsigned int _intersects_radiusSurface; + + unsigned int _outside_leftRightSurfaces; + unsigned int _inside_leftRightSurfaces; + unsigned int _intersects_leftRightSurfaces; + + unsigned int _outside_leftSurface; + unsigned int _inside_leftSurface; + unsigned int _intersects_leftSurface; + + unsigned int _outside_rightSurface; + unsigned int _inside_rightSurface; + unsigned int _intersects_rightSurface; + + unsigned int _outside_bottomSurface; + unsigned int _inside_bottomSurface; + unsigned int _intersects_bottomSurface; + + unsigned int _outside_topSurface; + unsigned int _inside_topSurface; + unsigned int _intersects_topSurface; + + }; + + + typedef std::vector< osg::Vec3 > VertexArray; + typedef std::vector< Region > 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; + + VertexArray _originalVertices; + RegionArray _regions; + BoolArray _vertexInIntersectionSet; + IndexArray _candidateVertexIndices; + IndexArray _remapIndices; + TriangleArray _triangles; + EdgeSet _edges; + + osg::Vec3 _centre; + double _radius; + double _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) + { + _originalVertices.resize(array.size()); + _regions.resize(array.size()); + _vertexInIntersectionSet.resize(array.size(), false); + _candidateVertexIndices.clear(); + + double radius2 = _radius*_radius; + + for(unsigned int i=0; i_p1]); - rc.add(_regions[tri->_p2]); - rc.add(_regions[tri->_p3]); - int numIntersections = rc.numberOfIntersectingSurfaces(); + rc.add(_regions[p1]); + rc.add(_regions[p2]); + rc.add(_regions[p3]); - if (numIntersections>=1) + Region::Classification classification = rc.overallClassification(); + + // reject if outside. + if (classification==Region::OUTSIDE) { - buildEdges(tri); + ++_numOutside; + return; } - - } - 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; - } - } - - SphereSegment::LineList connectIntersections(EdgeList& hitEdges) - { - SphereSegment::LineList lineList; - - osg::notify(osg::NOTICE)<<"Number of edge intersections "<get(); - edge->_toTraverse.clear(); - //osg::notify(osg::NOTICE)<<"edge= "<_triangles.begin(); - titr != edge->_triangles.end(); - ++titr) + if (rc.numberOfIntersectingSurfaces()==0) { - 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; + ++_numInside; + return; + } - 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) + ++_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::INFO)<<"Removing duplicates : num vertices in "<<_candidateVertexIndices.size()<get(); - osg::notify(osg::NOTICE)<<"edge= "<_toTraverse.begin(); - titr != edge->_toTraverse.end(); - ++titr) + + if (verticesRemapped) { - Triangle* tri = *titr; - osg::notify(osg::NOTICE)<<" "<_p1 = _remapIndices[(*titr)->_p1]; + (*titr)->_p2 = _remapIndices[(*titr)->_p2]; + (*titr)->_p3 = _remapIndices[(*titr)->_p3]; + (*titr)->sort(); + } } } -#endif - while(!hitEdges.empty()) + + void removeDuplicateTriangles() { - // find the an open edge + osg::notify(osg::INFO)<<"Removing duplicate triangles : num triangles in "<<_triangles.size()<_e1 = addEdge(tri->_p1, tri->_p2, tri); + tri->_e2 = addEdge(tri->_p2, tri->_p3, tri); + tri->_e3 = addEdge(tri->_p1, tri->_p3, tri); + } + + void buildEdges() + { + _edges.clear(); + for(TriangleArray::iterator itr = _triangles.begin(); + itr != _triangles.end(); + ++itr) + { + Triangle* tri = itr->get(); + + RegionCounter rc; + rc.add(_regions[tri->_p1]); + rc.add(_regions[tri->_p2]); + rc.add(_regions[tri->_p3]); + int numIntersections = rc.numberOfIntersectingSurfaces(); + + if (numIntersections>=1) + { + buildEdges(tri); + } + + } + osg::notify(osg::INFO)<<"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::INFO)<<"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; + } + } + + SphereSegment::LineList connectIntersections(EdgeList& hitEdges) + { + SphereSegment::LineList lineList; + + osg::notify(osg::INFO)<<"Number of edge intersections "<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) + edge->_toTraverse.clear(); + //osg::notify(osg::INFO)<<"edge= "<_triangles.begin(); + titr != edge->_triangles.end(); + ++titr) { + Triangle* tri = *titr; - newEdge = tri->oppositeActiveEdge(edge); - - edge->removeFromToTraverseList(tri); - newEdge->removeFromToTraverseList(tri); + // 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; - // osg::notify(osg::NOTICE)<<" tri="<get(); + if (edge->_toTraverse.size()==1) break; } - - if (edge->_toTraverse.empty()) + + if (hitr == hitEdges.end()) { - edge->_intersectionType = Edge::NO_INTERSECTION; - - // remove edge for the hitEdges. - hitr = find(hitEdges.begin(), hitEdges.end(), edge); - if (hitr!=hitEdges.end()) hitEdges.erase(hitr); + hitr = hitEdges.begin(); } - // move on to next edge in line. - edge = newEdge; - - } - } - return lineList; - } - - template - SphereSegment::LineList 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); - } - } - - return connectIntersections(hitEdges); - } - - template - void trim(SphereSegment::LineList& lineList, osg::Vec3Array* sourceLine, I intersector) - { - if (sourceLine->empty()) return; - - // osg::notify(osg::NOTICE)<<"Testing line of "<size()<size()) - { - // find first valid vertex. - for(; firstsize(); ++first) - { - if (intersector.distance((*sourceLine)[first]-_centre)>=0.0) break; - } - - if (first==sourceLine->size()) - { - // osg::notify(osg::NOTICE)<<"No valid points found"<size(); ++last) - { - if (intersector.distance((*sourceLine)[last]-_centre)<0.0) break; - } - - if (first==0 && last==sourceLine->size()) - { - // osg::notify(osg::NOTICE)<<"Copying complete line"<0) - { - newLine->push_back(intersector.intersectionPoint((*sourceLine)[first-1]-_centre, (*sourceLine)[first]-_centre)+_centre); - } - - for(unsigned int i=first; ipush_back((*sourceLine)[i]); - } - if (lastsize()) - { - newLine->push_back(intersector.intersectionPoint((*sourceLine)[last-1]-_centre, (*sourceLine)[last]-_centre)+_centre); - } - lineList.push_back(newLine); - } - - first = last; - } - } - - // handle a paird of surfaces that work to enclose a convex region, which means that - // points can be inside either surface to be valid, and be outside both surfaces to be invalid. - template - void trim(SphereSegment::LineList& lineList, osg::Vec3Array* sourceLine, I intersector1, I intersector2) - { - if (sourceLine->empty()) return; - - // osg::notify(osg::NOTICE)<<"Testing line of "<size()<size()) - { - // find first valid vertex. - for(; firstsize(); ++first) - { - osg::Vec3 v = (*sourceLine)[first]-_centre; - if (intersector1.distance(v)>=0.0 || intersector2.distance(v)>=0.0 ) break; - } - - if (first==sourceLine->size()) - { - // osg::notify(osg::NOTICE)<<"No valid points found"<size(); ++last) - { - osg::Vec3 v = (*sourceLine)[last]-_centre; - if (intersector1.distance(v)<0.0 && intersector2.distance(v)<0.0 ) break; - } - - if (first==0 && last==sourceLine->size()) - { - // osg::notify(osg::NOTICE)<<"Copying complete line"<0) + Edge* edge = hitr->get(); + while (edge) { - osg::Vec3 start = (*sourceLine)[first-1]-_centre; - osg::Vec3 end = (*sourceLine)[first]-_centre; - - float end1 = intersector1.distance(end); - float end2 = intersector2.distance(end); - + // osg::notify(osg::INFO)<<" vertex "<_intersectionVertex<push_back(edge->_intersectionVertex+_centre/*+osg::Vec3(0.0f,0.0f,200.0f)*/); - // work out which intersector to use by discounting the one that - // isn't a plausible candiate. - bool possible1 = end1>=0.0; - bool possible2 = end2>=0.0; - if (possible1 && possible2) + Edge* newEdge = 0; + + Triangle* tri = !(edge->_toTraverse.empty()) ? edge->_toTraverse.back() : 0; + if (tri) { - double start1 = intersector1.distance(start); - double start2 = intersector2.distance(start); - - // need to check which intersection is latest. - double end1 = intersector1.distance(end); - double delta1 = (end1-start1); + newEdge = tri->oppositeActiveEdge(edge); - double end2 = intersector2.distance(end); - double delta2 = (end2-start2); + edge->removeFromToTraverseList(tri); + newEdge->removeFromToTraverseList(tri); - double r1 = fabs(delta1)>0.0 ? start1/delta1 : 0.0; - double r2 = fabs(delta2)>0.0 ? start2/delta2 : 0.0; + // osg::notify(osg::INFO)<<" tri="<push_back(intersector1.intersectionPoint(start, end)+_centre); } else { - newLine->push_back(intersector2.intersectionPoint(start, end)+_centre); + newEdge = 0; } - } - - for(unsigned int i=first; ipush_back((*sourceLine)[i]); - } - - if (lastsize()) - { - osg::Vec3 start = (*sourceLine)[last-1]-_centre; - osg::Vec3 end = (*sourceLine)[last]-_centre; - - double start1 = intersector1.distance(start); - double start2 = intersector2.distance(start); - - // work out which intersector to use by discounting the one that - // isn't a plausible candiate. - bool possible1 = start1>=0.0; - bool possible2 = start2>=0.0; - if (possible1 && possible2) + if (edge->_toTraverse.empty()) { - double end1 = intersector1.distance(end); - double end2 = intersector2.distance(end); - - possible1 = end1<0.0; - possible2 = end2<0.0; - + 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; + + } + } + return lineList; + } + + template + SphereSegment::LineList 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); + } + } + + return connectIntersections(hitEdges); + } + + template + void trim(SphereSegment::LineList& lineList, osg::Vec3Array* sourceLine, I intersector) + { + if (sourceLine->empty()) return; + + // osg::notify(osg::INFO)<<"Testing line of "<size()<size()) + { + // find first valid vertex. + for(; firstsize(); ++first) + { + if (intersector.distance((*sourceLine)[first]-_centre)>=0.0) break; + } + + if (first==sourceLine->size()) + { + // osg::notify(osg::INFO)<<"No valid points found"<size(); ++last) + { + if (intersector.distance((*sourceLine)[last]-_centre)<0.0) break; + } + + if (first==0 && last==sourceLine->size()) + { + // osg::notify(osg::INFO)<<"Copying complete line"<0) + { + newLine->push_back(intersector.intersectionPoint((*sourceLine)[first-1]-_centre, (*sourceLine)[first]-_centre)+_centre); + } + + for(unsigned int i=first; ipush_back((*sourceLine)[i]); + } + if (lastsize()) + { + newLine->push_back(intersector.intersectionPoint((*sourceLine)[last-1]-_centre, (*sourceLine)[last]-_centre)+_centre); + } + + lineList.push_back(newLine); + } + + first = last; + } + } + + + // handle a paird of surfaces that work to enclose a convex region, which means that + // points can be inside either surface to be valid, and be outside both surfaces to be invalid. + template + void trim(SphereSegment::LineList& lineList, osg::Vec3Array* sourceLine, I intersector1, I intersector2) + { + if (sourceLine->empty()) return; + + // osg::notify(osg::INFO)<<"Testing line of "<size()<size()) + { + // find first valid vertex. + for(; firstsize(); ++first) + { + osg::Vec3 v = (*sourceLine)[first]-_centre; + if (intersector1.distance(v)>=0.0 || intersector2.distance(v)>=0.0 ) break; + } + + if (first==sourceLine->size()) + { + // osg::notify(osg::INFO)<<"No valid points found"<size(); ++last) + { + osg::Vec3 v = (*sourceLine)[last]-_centre; + if (intersector1.distance(v)<0.0 && intersector2.distance(v)<0.0 ) break; + } + + if (first==0 && last==sourceLine->size()) + { + // osg::notify(osg::INFO)<<"Copying complete line"<0) + { + osg::Vec3 start = (*sourceLine)[first-1]-_centre; + osg::Vec3 end = (*sourceLine)[first]-_centre; + + float end1 = intersector1.distance(end); + float end2 = intersector2.distance(end); + + + // work out which intersector to use by discounting the one that + // isn't a plausible candiate. + bool possible1 = end1>=0.0; + bool possible2 = end2>=0.0; if (possible1 && possible2) { + + double start1 = intersector1.distance(start); + double start2 = intersector2.distance(start); + // need to check which intersection is latest. double end1 = intersector1.distance(end); double delta1 = (end1-start1); @@ -2142,577 +2066,623 @@ struct TriangleIntersectOperator double r1 = fabs(delta1)>0.0 ? start1/delta1 : 0.0; double r2 = fabs(delta2)>0.0 ? start2/delta2 : 0.0; - + // choose intersection which is nearest the end point. - if (r1>r2) + if (r1push_back(intersector1.intersectionPoint(start, end)+_centre); + } + else + { + newLine->push_back(intersector2.intersectionPoint(start, end)+_centre); + } + } - if (possible1) + for(unsigned int i=first; ipush_back(intersector1.intersectionPoint(start, end)+_centre); + newLine->push_back((*sourceLine)[i]); } - else + + if (lastsize()) { - newLine->push_back(intersector2.intersectionPoint(start, end)+_centre); + osg::Vec3 start = (*sourceLine)[last-1]-_centre; + osg::Vec3 end = (*sourceLine)[last]-_centre; + + double start1 = intersector1.distance(start); + double start2 = intersector2.distance(start); + + // work out which intersector to use by discounting the one that + // isn't a plausible candiate. + bool possible1 = start1>=0.0; + bool possible2 = start2>=0.0; + if (possible1 && possible2) + { + double end1 = intersector1.distance(end); + double end2 = intersector2.distance(end); + + possible1 = end1<0.0; + possible2 = end2<0.0; + + if (possible1 && possible2) + { + // need to check which intersection is latest. + double end1 = intersector1.distance(end); + double delta1 = (end1-start1); + + double end2 = intersector2.distance(end); + double delta2 = (end2-start2); + + double r1 = fabs(delta1)>0.0 ? start1/delta1 : 0.0; + double r2 = fabs(delta2)>0.0 ? start2/delta2 : 0.0; + + // choose intersection which is nearest the end point. + if (r1>r2) + { + osg::notify(osg::INFO)<<"end point, 1 near to end than 2"<push_back(intersector1.intersectionPoint(start, end)+_centre); + } + else + { + newLine->push_back(intersector2.intersectionPoint(start, end)+_centre); + } + } + lineList.push_back(newLine); } - lineList.push_back(newLine); + first = last; } - - first = last; } - } - template - void trim(SphereSegment::LineList& lineList, I intersector) - { - SphereSegment::LineList newLines; - - // collect all the intersecting edges - for(SphereSegment::LineList::iterator itr = lineList.begin(); - itr != lineList.end(); - ++itr) + template + void trim(SphereSegment::LineList& lineList, I intersector) { - osg::Vec3Array* line = itr->get(); - trim(newLines, line, intersector); - } - lineList.swap(newLines); - } + SphereSegment::LineList newLines; - - template - void trim(SphereSegment::LineList& lineList, I intersector1, I intersector2) - { - SphereSegment::LineList newLines; - - // collect all the intersecting edges - for(SphereSegment::LineList::iterator itr = lineList.begin(); - itr != lineList.end(); - ++itr) - { - osg::Vec3Array* line = itr->get(); - trim(newLines, line, intersector1, intersector2); - } - lineList.swap(newLines); - } - - struct LinePair - { - LinePair(osg::Vec3Array* line): - _line(line), - _lineEnd(0), - _neighbourLine(0), - _neighbourLineEnd(0) {} - - bool operator < (const LinePair& linePair) const - { - return _distance < linePair._distance; - } - - void consider(osg::Vec3Array* testline) - { - if (_neighbourLine.valid()) + // collect all the intersecting edges + for(SphereSegment::LineList::iterator itr = lineList.begin(); + itr != lineList.end(); + ++itr) { - float distance = ((*_line)[0]-(*testline)[0]).length(); - if (distance<_distance) - { - _lineEnd = 0; - _neighbourLine = testline; - _neighbourLineEnd = 0; - _distance = distance; - } - - distance = ((*_line)[0]-(*testline)[testline->size()-1]).length(); - if (distance<_distance) - { - _lineEnd = 0; - _neighbourLine = testline; - _neighbourLineEnd = testline->size()-1; - _distance = distance; - } - - distance = ((*_line)[_line->size()-1]-(*testline)[0]).length(); - if (distance<_distance) - { - _lineEnd = _line->size()-1; - _neighbourLine = testline; - _neighbourLineEnd = 0; - _distance = distance; - } - - distance = ((*_line)[_line->size()-1]-(*testline)[testline->size()-1]).length(); - if (distance<_distance) - { - _lineEnd = _line->size()-1; - _neighbourLine = testline; - _neighbourLineEnd = testline->size()-1; - _distance = distance; - } - + osg::Vec3Array* line = itr->get(); + trim(newLines, line, intersector); } - else + lineList.swap(newLines); + } + + + template + void trim(SphereSegment::LineList& lineList, I intersector1, I intersector2) + { + SphereSegment::LineList newLines; + + // collect all the intersecting edges + for(SphereSegment::LineList::iterator itr = lineList.begin(); + itr != lineList.end(); + ++itr) { - _neighbourLine = testline; - if (_neighbourLine==_line) + osg::Vec3Array* line = itr->get(); + trim(newLines, line, intersector1, intersector2); + } + lineList.swap(newLines); + } + + struct LinePair + { + LinePair(osg::Vec3Array* line): + _line(line), + _lineEnd(0), + _neighbourLine(0), + _neighbourLineEnd(0) {} + + bool operator < (const LinePair& linePair) const + { + return _distance < linePair._distance; + } + + void consider(osg::Vec3Array* testline) + { + if (_neighbourLine.valid()) { - _lineEnd = 0; - _neighbourLineEnd = _neighbourLine->size()-1; - _distance = ((*_line)[_lineEnd]-(*_neighbourLine)[_neighbourLineEnd]).length(); - } - else - { - _distance = ((*_line)[0]-(*_neighbourLine)[0]).length(); - _lineEnd = 0; - _neighbourLineEnd = 0; - - float distance = ((*_line)[0]-(*_neighbourLine)[_neighbourLine->size()-1]).length(); + float distance = ((*_line)[0]-(*testline)[0]).length(); if (distance<_distance) { _lineEnd = 0; - _neighbourLineEnd = _neighbourLine->size()-1; - _distance = distance; - } - - distance = ((*_line)[_line->size()-1]-(*_neighbourLine)[0]).length(); - if (distance<_distance) - { - _lineEnd = _line->size()-1; + _neighbourLine = testline; _neighbourLineEnd = 0; _distance = distance; } - distance = ((*_line)[_line->size()-1]-(*_neighbourLine)[_neighbourLine->size()-1]).length(); + distance = ((*_line)[0]-(*testline)[testline->size()-1]).length(); + if (distance<_distance) + { + _lineEnd = 0; + _neighbourLine = testline; + _neighbourLineEnd = testline->size()-1; + _distance = distance; + } + + distance = ((*_line)[_line->size()-1]-(*testline)[0]).length(); if (distance<_distance) { _lineEnd = _line->size()-1; - _neighbourLineEnd = _neighbourLine->size()-1; + _neighbourLine = testline; + _neighbourLineEnd = 0; + _distance = distance; + } + + distance = ((*_line)[_line->size()-1]-(*testline)[testline->size()-1]).length(); + if (distance<_distance) + { + _lineEnd = _line->size()-1; + _neighbourLine = testline; + _neighbourLineEnd = testline->size()-1; _distance = distance; } - } - } - }; - - bool contains(osg::Vec3Array* line) const - { - return _line==line || _neighbourLine==line; - } - - - osg::ref_ptr _line; - unsigned int _lineEnd; - osg::ref_ptr _neighbourLine; - unsigned int _neighbourLineEnd; - float _distance; - }; - void fuseEnds(float fuseDistance) - { - SphereSegment::LineList fusedLines; - SphereSegment::LineList unfusedLines; - - // first seperat the already fused lines from the unfused ones. - for(SphereSegment::LineList::iterator itr = _generatedLines.begin(); - itr != _generatedLines.end(); - ++itr) - { - osg::Vec3Array* line = itr->get(); - if (line->size()>=2) - { - if ((*line)[0]==(*line)[line->size()-1]) - { - fusedLines.push_back(line); } else { - unfusedLines.push_back(line); + _neighbourLine = testline; + if (_neighbourLine==_line) + { + _lineEnd = 0; + _neighbourLineEnd = _neighbourLine->size()-1; + _distance = ((*_line)[_lineEnd]-(*_neighbourLine)[_neighbourLineEnd]).length(); + } + else + { + _distance = ((*_line)[0]-(*_neighbourLine)[0]).length(); + _lineEnd = 0; + _neighbourLineEnd = 0; + + float distance = ((*_line)[0]-(*_neighbourLine)[_neighbourLine->size()-1]).length(); + if (distance<_distance) + { + _lineEnd = 0; + _neighbourLineEnd = _neighbourLine->size()-1; + _distance = distance; + } + + distance = ((*_line)[_line->size()-1]-(*_neighbourLine)[0]).length(); + if (distance<_distance) + { + _lineEnd = _line->size()-1; + _neighbourLineEnd = 0; + _distance = distance; + } + + distance = ((*_line)[_line->size()-1]-(*_neighbourLine)[_neighbourLine->size()-1]).length(); + if (distance<_distance) + { + _lineEnd = _line->size()-1; + _neighbourLineEnd = _neighbourLine->size()-1; + _distance = distance; + } + } } - } - } - - while (unfusedLines.size()>=1) - { - // generate a set of line pairs to establish which - // line pair has the minimum distance. - typedef std::multiset LinePairSet; - LinePairSet linePairs; - for(unsigned int i=0; i _line; + unsigned int _lineEnd; + osg::ref_ptr _neighbourLine; + unsigned int _neighbourLineEnd; + float _distance; + }; + + void joinEnds(float fuseDistance, bool doFuse, bool allowJoinToSelf) + { + SphereSegment::LineList fusedLines; + SphereSegment::LineList unfusedLines; + + // first seperat the already fused lines from the unfused ones. + for(SphereSegment::LineList::iterator itr = _generatedLines.begin(); + itr != _generatedLines.end(); ++itr) { - osg::notify(osg::NOTICE)<<"Line "<_line.get()<<" "<_lineEnd<<" neighbour "<_neighbourLine.get()<<" "<_neighbourLineEnd<<" distance="<_distance<get(); + if (line->size()>=2) + { + if ((*line)[0]==(*line)[line->size()-1]) + { + fusedLines.push_back(line); + } + else + { + unfusedLines.push_back(line); + } + } } - LinePair linePair = *linePairs.begin(); - if (linePair._distance > fuseDistance) + while (unfusedLines.size()>=1) { - osg::notify(osg::NOTICE)<<"Completed work, shortest distance left is "<size()-1])*0.5f; - (*line)[0] = average; - (*line)[line->size()-1] = average; - fusedLines.push_back(line); - - SphereSegment::LineList::iterator fitr = std::find(unfusedLines.begin(), unfusedLines.end(), line); - if (fitr != unfusedLines.end()) + // generate a set of line pairs to establish which + // line pair has the minimum distance. + typedef std::multiset LinePairSet; + LinePairSet linePairs; + for(unsigned int i=0; i_line.get()<<" "<_lineEnd<<" neighbour "<_neighbourLine.get()<<" "<_neighbourLineEnd<<" distance="<_distance< fuseDistance) + { + osg::notify(osg::INFO)<<"Completed work, shortest distance left is "<size()-1])*0.5f; + + if (doFuse) + { + (*line)[0] = average; + (*line)[line->size()-1] = average; + } + else + { + // add start of line to end. + line->push_back((*line)[0]); + } + + fusedLines.push_back(line); + + SphereSegment::LineList::iterator fitr = std::find(unfusedLines.begin(), unfusedLines.end(), line); + if (fitr != unfusedLines.end()) + { + unfusedLines.erase(fitr); + } + else + { + osg::notify(osg::INFO)<<"Error couldn't find line in unfused list, exiting fusing loop."<size()-1 : 0; - int direction1 = openEnd1size()-1 : 0; + int direction1 = openEnd1size()-1 : 0; - int direction2 = fuseEnd2size()-1 : 0; + int direction2 = fuseEnd2push_back((*line1)[i]); - } - - // add the average of the two fused ends - osg::Vec3 average = ((*line1)[fuseEnd1] + (*line2)[fuseEnd2])*0.5f; - newline->push_back(average); + osg::Vec3Array* newline = new osg::Vec3Array; - // copy across from the next point in from fuseEnd2 to the openEnd2. - for(int j=fuseEnd2 + direction2; - j != openEnd2 + direction2; - j += direction2) - { - newline->push_back((*line2)[j]); - } - - // remove line1 from unfused list. - SphereSegment::LineList::iterator fitr = std::find(unfusedLines.begin(), unfusedLines.end(), line1); - if (fitr != unfusedLines.end()) - { - unfusedLines.erase(fitr); + // copy across all but fuse end of line1 + for(int i=openEnd1; + i != fuseEnd1; + i += direction1) + { + newline->push_back((*line1)[i]); + } + + // add the average of the two fused ends + + if (doFuse) + { + osg::Vec3 average = ((*line1)[fuseEnd1] + (*line2)[fuseEnd2])*0.5f; + newline->push_back(average); + } + else + { + newline->push_back((*line1)[fuseEnd1]); + newline->push_back((*line2)[fuseEnd2]); + } + + // copy across from the next point in from fuseEnd2 to the openEnd2. + for(int j=fuseEnd2 + direction2; + j != openEnd2 + direction2; + j += direction2) + { + newline->push_back((*line2)[j]); + } + + // remove line1 from unfused list. + SphereSegment::LineList::iterator fitr = std::find(unfusedLines.begin(), unfusedLines.end(), line1); + if (fitr != unfusedLines.end()) + { + unfusedLines.erase(fitr); + } + + // remove line2 from unfused list. + fitr = std::find(unfusedLines.begin(), unfusedLines.end(), line2); + if (fitr != unfusedLines.end()) + { + unfusedLines.erase(fitr); + } + + // add the newline into the unfused for further processing. + unfusedLines.push_back(newline); + + osg::notify(osg::INFO)<<"Fusing two seperate lines "<_intersectionType = TriangleIntersectOperator::Edge::NO_INTERSECTION; - - osg::Vec3& v1 = _tif._originalVertices[edge->_p1]; - osg::Vec3& v2 = _tif._originalVertices[edge->_p2]; - - double azim1 = atan2(v1.x(),v1.y()); - if (azim1<0.0) azim1 += 2.0*osg::PI; - - double azim2 = atan2(v2.x(),v2.y()); - if (azim2<0.0) azim2 += 2.0*osg::PI; - - edge->_p1Outside = _lowerOutside ? (azim1<_azim) : (azim1>_azim); - edge->_p2Outside = _lowerOutside ? (azim2<_azim) : (azim2>_azim); - - // if both points inside then disgard - if (azim1<_azim && azim2<_azim) return false; - - // if both points outside then disgard - if (azim1>_azim && azim2>_azim) return false; - - if (azim1==_azim) - { - if (azim2==_azim) - { - edge->_intersectionType = TriangleIntersectOperator::Edge::BOTH_ENDS; - } - else - { - edge->_intersectionType = TriangleIntersectOperator::Edge::POINT_1; - } - } - else if (azim2==_azim) - { - edge->_intersectionType = TriangleIntersectOperator::Edge::POINT_2; - } - else - { - - double dx = v2.x()-v1.x(); - double dy = v2.y()-v1.y(); - double t = tan(_azim); - double div = dx - t*dy; - if (div==0.0) - { - edge->_intersectionType = TriangleIntersectOperator::Edge::NO_INTERSECTION; - return false; - } - - double r = (t*v1.y()-v1.x()) / div; - if (r<0.0 || r>1.0) - { - edge->_intersectionType = TriangleIntersectOperator::Edge::NO_INTERSECTION; - return false; - } - - - double one_minus_r = 1.0-r; - - edge->_intersectionType = TriangleIntersectOperator::Edge::MID_POINT; - edge->_intersectionVertex = v1*one_minus_r + v2*r; - } - return true; } -}; -struct AzimPlaneIntersector -{ - AzimPlaneIntersector(TriangleIntersectOperator& tif, double azim, bool lowerOutside): - _tif(tif), - _lowerOutside(lowerOutside) + struct AzimPlaneIntersector { - _plane.set(cos(azim),-sin(azim),0.0,0.0); - _endPlane.set(sin(azim),cos(azim),0.0,0.0); - } - - TriangleIntersectOperator& _tif; - osg::Plane _plane; - osg::Plane _endPlane; - bool _lowerOutside; - - 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]; - - double d1 = _plane.distance(v1); - double d2 = _plane.distance(v2); - - edge->_p1Outside = _lowerOutside ? (d1<0.0) : (d1>0.0); - edge->_p2Outside = _lowerOutside ? (d2<0.0) : (d2>0.0); - - // if both points inside then disgard - if (d1<0.0 && d2<0.0) return false; - - // if both points outside then disgard - if (d1>0.0 && d2>0.0) return false; - -#if 0 - double e1 = _endPlane.distance(v1); - double e2 = _endPlane.distance(v2); - if (e1<0.0 && e2<0.0) return false; -#endif - - if (d1==0.0) + AzimPlaneIntersector(TriangleIntersectOperator& tif, double azim, bool lowerOutside): + _tif(tif), + _lowerOutside(lowerOutside) { - if (d2==0.0) + _plane.set(cos(azim),-sin(azim),0.0,0.0); + _endPlane.set(sin(azim),cos(azim),0.0,0.0); + } + + TriangleIntersectOperator& _tif; + osg::Plane _plane; + osg::Plane _endPlane; + bool _lowerOutside; + + 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]; + + double d1 = _plane.distance(v1); + double d2 = _plane.distance(v2); + + edge->_p1Outside = _lowerOutside ? (d1<0.0) : (d1>0.0); + edge->_p2Outside = _lowerOutside ? (d2<0.0) : (d2>0.0); + + // if both points inside then disgard + if (d1<0.0 && d2<0.0) return false; + + // if both points outside then disgard + if (d1>0.0 && d2>0.0) return false; + + if (d1==0.0) { - edge->_intersectionType = TriangleIntersectOperator::Edge::BOTH_ENDS; + if (d2==0.0) + { + edge->_intersectionType = TriangleIntersectOperator::Edge::BOTH_ENDS; + } + else + { + edge->_intersectionType = TriangleIntersectOperator::Edge::POINT_1; + } + } + else if (d2==0.0) + { + edge->_intersectionType = TriangleIntersectOperator::Edge::POINT_2; } else { - edge->_intersectionType = TriangleIntersectOperator::Edge::POINT_1; + + double div = d2-d1; + if (div==0.0) + { + edge->_intersectionType = TriangleIntersectOperator::Edge::NO_INTERSECTION; + return false; + } + + double r = -d1 / div; + if (r<0.0 || r>1.0) + { + edge->_intersectionType = TriangleIntersectOperator::Edge::NO_INTERSECTION; + return false; + } + + // osg::notify(osg::INFO)<<"r = "<_intersectionType = TriangleIntersectOperator::Edge::MID_POINT; + edge->_intersectionVertex = v1*one_minus_r + v2*r; } + + return true; } - else if (d2==0.0) - { - edge->_intersectionType = TriangleIntersectOperator::Edge::POINT_2; - } - else + + // compute the intersection between line segment and surface + osg::Vec3 intersectionPoint(const osg::Vec3& v1, const osg::Vec3& v2) { + double d1 = _plane.distance(v1); + double d2 = _plane.distance(v2); double div = d2-d1; if (div==0.0) { - edge->_intersectionType = TriangleIntersectOperator::Edge::NO_INTERSECTION; - return false; + return v1; } double r = -d1 / div; - if (r<0.0 || r>1.0) - { - edge->_intersectionType = TriangleIntersectOperator::Edge::NO_INTERSECTION; - return false; - } - - // osg::notify(osg::NOTICE)<<"r = "<_intersectionType = TriangleIntersectOperator::Edge::MID_POINT; - edge->_intersectionVertex = v1*one_minus_r + v2*r; - } - - return true; - } - - // compute the intersection between line segment and surface - osg::Vec3 intersectionPoint(const osg::Vec3& v1, const osg::Vec3& v2) - { - double d1 = _plane.distance(v1); - double d2 = _plane.distance(v2); - - double div = d2-d1; - if (div==0.0) - { - return v1; + return v1*one_minus_r + v2*r; } - double r = -d1 / div; - double one_minus_r = 1.0-r; - - return v1*one_minus_r + v2*r; - } - - // positive distance to the inside. - double distance(const osg::Vec3& v) - { - return _lowerOutside ? _plane.distance(v) : -_plane.distance(v) ; - } -}; - -struct ElevationIntersector -{ - ElevationIntersector(TriangleIntersectOperator& tif, double elev, bool lowerOutside): - _tif(tif), - _elev(elev), - _lowerOutside(lowerOutside) {} - - TriangleIntersectOperator& _tif; - double _elev; - bool _lowerOutside; - - 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]; - - double length_xy1 = sqrt(v1.x()*v1.x() + v1.y()*v1.y()); - double elev1 = atan2(v1.z(),length_xy1); - - double length_xy2 = sqrt(v2.x()*v2.x() + v2.y()*v2.y()); - double elev2 = atan2(v2.z(),length_xy2); - - edge->_p1Outside = _lowerOutside ? (elev1<_elev) : (elev1>_elev); - edge->_p2Outside = _lowerOutside ? (elev2<_elev) : (elev2>_elev); - - // if both points inside then disgard - if (elev1<_elev && elev2<_elev) return false; - - // if both points outside then disgard - if (elev1>_elev && elev2>_elev) return false; - - if (elev1==_elev) + // positive distance to the inside. + double distance(const osg::Vec3& v) { - if (elev2==_elev) + return _lowerOutside ? _plane.distance(v) : -_plane.distance(v) ; + } + }; + + struct ElevationIntersector + { + ElevationIntersector(TriangleIntersectOperator& tif, double elev, bool lowerOutside): + _tif(tif), + _elev(elev), + _lowerOutside(lowerOutside) {} + + TriangleIntersectOperator& _tif; + double _elev; + bool _lowerOutside; + + 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]; + + double length_xy1 = sqrt(v1.x()*v1.x() + v1.y()*v1.y()); + double elev1 = atan2(v1.z(),length_xy1); + + double length_xy2 = sqrt(v2.x()*v2.x() + v2.y()*v2.y()); + double elev2 = atan2(v2.z(),length_xy2); + + edge->_p1Outside = _lowerOutside ? (elev1<_elev) : (elev1>_elev); + edge->_p2Outside = _lowerOutside ? (elev2<_elev) : (elev2>_elev); + + // if both points inside then disgard + if (elev1<_elev && elev2<_elev) return false; + + // if both points outside then disgard + if (elev1>_elev && elev2>_elev) return false; + + if (elev1==_elev) { - edge->_intersectionType = TriangleIntersectOperator::Edge::BOTH_ENDS; + if (elev2==_elev) + { + edge->_intersectionType = TriangleIntersectOperator::Edge::BOTH_ENDS; + } + else + { + edge->_intersectionType = TriangleIntersectOperator::Edge::POINT_1; + } + } + else if (elev2==_elev) + { + edge->_intersectionType = TriangleIntersectOperator::Edge::POINT_2; } else { - edge->_intersectionType = TriangleIntersectOperator::Edge::POINT_1; + double dx = v2.x()-v1.x(); + double dy = v2.y()-v1.y(); + double dz = v2.z()-v1.z(); + double t = tan(_elev); + double tt = t*t; + double a = dz*dz-tt*(dx*dx + dy*dy); + double b = 2.0*(v1.z()*dz - tt*(v1.x()*dx + v1.y()*dy)); + double c = v1.z()*v1.z() - tt*(v1.x()*v1.x() + v1.y()*v1.y()); + + double s1, s2; + if (!computeQuadraticSolution(a,b,c,s1,s2)) + { + edge->_intersectionType = TriangleIntersectOperator::Edge::NO_INTERSECTION; + return false; + } + double r = 0.0; + if (s1>=0.0 && s1<=1.0) + { + r = s1; + } + else if (s2>=0.0 && s2<=1.0) + { + r = s2; + } + else + { + osg::notify(osg::INFO)<<"neither segment intersects s1="<