diff --git a/src/osgSim/ElevationSlice.cpp b/src/osgSim/ElevationSlice.cpp index 2d90228ac..3051ed90b 100644 --- a/src/osgSim/ElevationSlice.cpp +++ b/src/osgSim/ElevationSlice.cpp @@ -77,7 +77,7 @@ struct DistanceHeightCalculator _radiusList.push_back(newRadius); _distanceList.push_back(distance); - // osg::notify(osg::INFO)<<" newVector = "<height)<<" p2 = "<<(_p2->distance)<<" "<<(_p2->height)<distance - rhs._p1->distance) < epsilon) + double delta_distance = _p2->distance - rhs._p1->distance; + if (fabs(delta_distance) < epsilon) { if (fabs(_p2->height - rhs._p1->height) < epsilon) return JOINED; - // else return SEPERATE; + } + + if (delta_distance==0.0) + { + return SEPERATE; } if (rhs._p2->distance < _p1->distance || _p2->distance < rhs._p1->distance) return SEPERATE; @@ -297,11 +306,44 @@ struct Segment _p1->position * one_minus_r + _p2->position * r); } - Point* createIntersectinPoint(const Segment& rhs) const + Point* createIntersectionPoint(const Segment& rhs) const { - // temporary implementation - osg::notify(osg::INFO)<<"Error: Segment::createIntersectinPoint() not implementated yet"<distance; + double B = _p2->distance - _p1->distance; + double C = _p1->height; + double D = _p2->height - _p1->height; + + double E = rhs._p1->distance; + double F = rhs._p2->distance - rhs._p1->distance; + double G = rhs._p1->height; + double H = rhs._p2->height - rhs._p1->height; + + double div = D*F - B*H; + if (div==0.0) + { + osg::notify(osg::NOTICE)<<"ElevationSlideUtils::Segment::createIntersectionPoint(): error Segments are parallel."<height<<" p2 = "<<_p2->distance<<" "<<_p2->height<height<<" p2 = "<distance<<" "<height<position + (_p2->position - _p1->position)*r); } @@ -348,47 +390,14 @@ struct LineConstructor void report() { - osg::notify(osg::INFO)<<"Number of segments = "<<_segments.size()<height)<<" p2 = "<<(seg._p2->distance)<<" "<<(seg._p2->height)<<"\t"; + osg::notify(osg::NOTICE)<<"p1 = "<<(seg._p1->distance)<<" "<<(seg._p1->height)<<" p2 = "<<(seg._p2->distance)<<" "<<(seg._p2->height)<<"\t"; SegmentSet::iterator nextItr = itr; ++nextItr; @@ -397,23 +406,22 @@ struct LineConstructor Segment::Classification classification = itr->compare(*nextItr); switch(classification) { - case(Segment::IDENTICAL): osg::notify(osg::INFO)<<"i"; break; - case(Segment::SEPERATE): osg::notify(osg::INFO)<<"s"<0.0 || delta2>0.0) { - osg::notify(osg::INFO)<<" "<<&s<<" computed height delta ="<=Segment::OVERLAPPING) osg::notify(osg::NOTICE)<=Segment::OVERLAPPING) { + switch(classification) { case(Segment::OVERLAPPING): @@ -511,21 +521,21 @@ struct LineConstructor osg::notify(osg::INFO)<<" rhs_p2 "<distance<<" "<height<distance) ); Segment seg2( rhs._p1.get(), cp ); Segment seg3( cp, lhs._p2.get() ); Segment seg4( rhs.createPoint(lhs._p2->distance), lhs._p2.get() ); + _segments.erase(nextItr); + _segments.erase(itr); + _segments.insert(seg1); _segments.insert(seg2); _segments.insert(seg3); _segments.insert(seg4); - _segments.erase(nextItr); - _segments.erase(itr); - itr = _segments.find(seg1); nextItr = itr; ++nextItr; @@ -541,8 +551,8 @@ struct LineConstructor Segment newSeg(rhs.createPoint(lhs._p2->distance), rhs._p2.get()); - _segments.insert(newSeg); _segments.erase(nextItr); + _segments.insert(newSeg); nextItr = itr; ++nextItr; @@ -560,8 +570,8 @@ struct LineConstructor Segment newSeg(lhs._p1.get(), lhs.createPoint(rhs._p1->distance)); - _segments.insert(newSeg); _segments.erase(itr); + _segments.insert(newSeg); itr = _segments.find(newSeg); nextItr = itr; ++nextItr; @@ -572,18 +582,18 @@ struct LineConstructor } else { - osg::notify(osg::INFO)<<"OVERLAPPING : unidentified "<distance<<" "<height<distance<<" "<height<distance<<" "<height<distance<<" "<height<distance<<" "<height<distance<<" "<height<distance<<" "<height<distance<<" "<height<distance), enclosing._p2.get()); -#if 1 nextItr = itr; ++nextItr; @@ -649,7 +658,6 @@ struct LineConstructor _segments.insert(newSeg); itr = nextItr; -#endif ++nextItr; osg::notify(osg::INFO)<<" newSeg_p1 "<distance<<" "<height<distance) ); + Segment newSeg(enclosing._p1.get(), enclosing.createPoint(enclosed._p1->distance) ); _segments.insert(newSeg); - _segments.erase(nextItr); + _segments.erase(itr); + itr = _segments.find(newSeg); nextItr = itr; ++nextItr; @@ -687,7 +696,24 @@ struct LineConstructor osg::notify(osg::INFO)<<" enclosed_p2 "<distance<<" "<height<distance) ); + Segment newSegRight(enclosing.createPoint(enclosed._p2->distance), enclosing._p2.get()); + + _segments.erase(itr); + _segments.insert(newSegLeft); + _segments.insert(newSegRight); + + itr = _segments.find(newSegLeft); + nextItr = itr; ++nextItr; + + osg::notify(osg::INFO)<<" newSegLeft_p1 "<distance<<" "<height<distance<<" "<height<distance<<" "<height<distance<<" "<height<distance<<" "<height<distance<<" "<height<distance - enclosing._p1->distance; + double d_right = enclosing._p2->distance - enclosed._p2->distance; + + if (d_left < epsilon && d_right < epsilon) + { + // treat ENCLOSED as ENCLOSING. + if (dh1 < 0.0) + { + osg::notify(osg::INFO)<<" >> enclosing left side is above enclosed left side"<> enclosing left side is above enclosed left side"<> Replace enclosing with right section"<> enclosing left side is above enclosed left side"<distance), enclosing._p2.get()); + + _segments.erase(itr); + _segments.erase(nextItr); + + _segments.insert(newSegLeft); + _segments.insert(newSegMid); + _segments.insert(newSegRight); + + itr = _segments.find(newSegLeft); + nextItr = itr; + ++nextItr; + } + else + { + osg::notify(osg::INFO)<<" >> enclosing left side is above enclosed left side"<> Replace enclosing with left section"<> enclosing left side is above enclosed left side"<> enclosing left side is above enclosed left side"<distance)); + Segment newSegMid(enclosed._p1.get(), cp); + Segment newSegRight(cp, enclosing._p2.get()); + + _segments.erase(itr); + _segments.erase(nextItr); + + _segments.insert(newSegLeft); + _segments.insert(newSegMid); + _segments.insert(newSegRight); + + itr = _segments.find(newSegLeft); + nextItr = itr; + ++nextItr; + } + } + else + { + osg::notify(osg::INFO)<<" >> Replace enclosing with left and right sections"<> enclosing left side is above enclosed left side"<distance), enclosing._p2.get()); + + _segments.erase(itr); + _segments.erase(nextItr); + + _segments.insert(newSegLeft); + _segments.insert(newSegMid); + _segments.insert(newSegRight); + + itr = _segments.find(newSegLeft); + nextItr = itr; + ++nextItr; + } + else + { + osg::notify(osg::INFO)<<" >> enclosing left side is above enclosed left side"<distance)); + Segment newSegMid(enclosed._p1.get(), cp); + Segment newSegRight(cp, enclosing._p2.get()); + + _segments.erase(itr); + _segments.erase(nextItr); + + _segments.insert(newSegLeft); + _segments.insert(newSegMid); + _segments.insert(newSegRight); + + itr = _segments.find(newSegLeft); + nextItr = itr; + ++nextItr; + } + + } } else { - osg::notify(osg::INFO)<<"ENCLOSING: ENCLOSING - not sure "<distance<<" "<height<distance<<" "<height<distance<<" "<height<distance<<" "<height<distance<<" "<height<distance<<" "<height<distance<<" "<height<distance<<" "<height<=0.0 && dh2>=0.0) { - osg::notify(osg::INFO)<<"ENCLOSED: ENCLOSING is below enclosed "<distance<<" "<height<distance<<" "<height<distance<<" "<height<distance<<" "<height<distance<<" "<height<distance<<" "<height<distance<<" "<height<distance<<" "<height<distance<<" "<height<distance<<" "<height<distance<<" "<height<distance<<" "<height<distance<<" "<height<distance<<" "<height<distance<<" "<height<distance<<" "<height<distance<<" "<height<distance<<" "<height<distance<<" "<height<distance<<" "<height<distance<<" "<height<distance<<" "<height<distance<<" "<height<distance<<" "<height<compare(*nextItr)>=Segment::OVERLAPPING) { - ++numOverlapping; + ++num; ++nextItr; } - return numOverlapping; + return num; } + unsigned int totalNumOverlapping() const + { + unsigned int total = 0; + for(SegmentSet::const_iterator itr = _segments.begin(); + itr != _segments.end(); + ++itr) + { + total += numOverlapping(itr); + } + return total; + } + void copyPoints(ElevationSlice::Vec3dList& intersections, ElevationSlice::DistanceHeightList& distanceHeightIntersections) { SegmentSet::iterator prevItr = _segments.begin(); @@ -817,11 +1032,27 @@ struct LineConstructor } case(Segment::JOINED): { +#if 1 + intersections.push_back( nextItr->_p2->position ); + distanceHeightIntersections.push_back( ElevationSlice::DistanceHeight(nextItr->_p2->distance, nextItr->_p2->height) ); +#else + intersections.push_back( nextItr->_p1->position ); + distanceHeightIntersections.push_back( ElevationSlice::DistanceHeight(nextItr->_p1->distance, nextItr->_p1->height) ); + + intersections.push_back( nextItr->_p2->position ); + distanceHeightIntersections.push_back( ElevationSlice::DistanceHeight(nextItr->_p2->distance, nextItr->_p2->height) ); +#endif + break; + } + default: + { + intersections.push_back( nextItr->_p1->position ); + distanceHeightIntersections.push_back( ElevationSlice::DistanceHeight(nextItr->_p1->distance, nextItr->_p1->height) ); + intersections.push_back( nextItr->_p2->position ); distanceHeightIntersections.push_back( ElevationSlice::DistanceHeight(nextItr->_p2->distance, nextItr->_p2->height) ); break; } - default : break; } } @@ -861,13 +1092,13 @@ void ElevationSlice::computeIntersections(osg::Node* scene) em->convertXYZToLatLongHeight(_startPoint.x(), _startPoint.y(), _startPoint.z(), start_latitude, start_longitude, start_height); - osg::notify(osg::INFO)<<"start_lat = "<