root/OpenSceneGraph/trunk/src/osgSim/ElevationSlice.cpp @ 13041

Revision 13041, 59.2 kB (checked in by robert, 2 years ago)

Ran script to remove trailing spaces and tabs

  • Property svn:eol-style set to native
  • Property svn:keywords set to Author Date Id Revision
Line 
1/* -*-c++-*- OpenSceneGraph - Copyright (C) 1998-2006 Robert Osfield
2 *
3 * This library is open source and may be redistributed and/or modified under
4 * the terms of the OpenSceneGraph Public License (OSGPL) version 0.0 or
5 * (at your option) any later version.  The full license is in LICENSE file
6 * included with this distribution, and on the openscenegraph.org website.
7 *
8 * This library is distributed in the hope that it will be useful,
9 * but WITHOUT ANY WARRANTY; without even the implied warranty of
10 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
11 * OpenSceneGraph Public License for more details.
12*/
13
14#include <osg/CoordinateSystemNode>
15#include <osg/io_utils>
16#include <osg/Geode>
17#include <osg/Geometry>
18
19#include <osgSim/ElevationSlice>
20
21#include <osg/Notify>
22#include <osgUtil/PlaneIntersector>
23
24#include <osgDB/WriteFile>
25
26using namespace osgSim;
27
28namespace ElevationSliceUtils
29{
30
31struct DistanceHeightCalculator
32{
33    DistanceHeightCalculator(osg::EllipsoidModel* em, const osg::Vec3d& startPoint, osg::Vec3d& endPoint):
34        _em(em),
35        _startPoint(startPoint),
36        _startNormal(startPoint),
37        _endPoint(endPoint),
38        _endNormal(endPoint)
39    {
40        double latitude, longitude, height;
41
42        // set up start point variables
43        _em->convertXYZToLatLongHeight(_startPoint.x(), _startPoint.y(), _startPoint.z(), latitude, longitude, height);
44        _startRadius = _startPoint.length() - height;
45        _startNormal.normalize();
46
47        // set up end point variables
48        _em->convertXYZToLatLongHeight(_endPoint.x(), _endPoint.y(), _endPoint.z(), latitude, longitude, height);
49        _endRadius = _endPoint.length() - height;
50        _endNormal.normalize();
51
52        osg::Vec3d normal = _startNormal ^ _endNormal;
53        normal.normalize();
54
55        _angleIncrement = 0.005;
56
57        _radiusList.push_back(_startRadius);
58        _distanceList.push_back(0.0);
59
60        osg::Matrixd rotationMatrix;
61        double angleBetweenStartEnd = acos( _startNormal * _endNormal );
62        double prevRadius = _startRadius;
63        double distance = 0.0;
64        for(double angle = _angleIncrement;
65            angle < angleBetweenStartEnd;
66            angle += _angleIncrement)
67        {
68            rotationMatrix.makeRotate(angle, normal);
69            osg::Vec3d newVector = osg::Matrixd::transform3x3(_startPoint, rotationMatrix);
70
71            _em->convertXYZToLatLongHeight(newVector.x(), newVector.y(), newVector.z(), latitude, longitude, height);
72            double newRadius = newVector.length() - height;
73
74            double distanceIncrement = _angleIncrement * (newRadius + prevRadius) *0.5;
75            distance  += distanceIncrement;
76
77            _radiusList.push_back(newRadius);
78            _distanceList.push_back(distance);
79
80            // OSG_NOTICE<<"  newVector = "<<newVector<<" newRadius = "<<newRadius<<" distanceIncrement="<<distanceIncrement<<std::endl;
81
82            prevRadius = newRadius;
83        }
84
85    }
86
87
88    void computeDistanceHeight(const osg::Vec3d& v, double& distance, double& height) const
89    {
90        osg::Vec3d vNormal = v;
91        vNormal.normalize();
92
93        // compute the height at position
94        double latitude, longitude;
95        _em->convertXYZToLatLongHeight(v.x(), v.y(), v.z(),
96                                       latitude, longitude, height);
97
98        // compute the radius at the point
99        double Rv = v.length() - height;
100
101        // compute the angle from the _startPoint
102        double alpha = acos( vNormal * _startNormal);
103
104        unsigned int int_alpha = static_cast<unsigned int>(floor(alpha / _angleIncrement));
105        if (int_alpha >= _distanceList.size())
106        {
107            int_alpha = _distanceList.size() - 1;
108        }
109
110        double prevAlpha = ((double)int_alpha) * _angleIncrement;
111        double deltaAlpha = alpha - prevAlpha;
112        double prevDistance = _distanceList[int_alpha];
113        double prevRadius = _radiusList[int_alpha];
114
115        double averageRadius = (prevRadius + Rv)*0.5;
116        double distanceIncrement = deltaAlpha * averageRadius;
117
118        distance = prevDistance + distanceIncrement;
119
120#if 0
121        double oldDistance = alpha * (_startRadius + Rv) *0.5;
122
123        distance = oldDistance;
124
125        OSG_NOTICE<<" new distance = "<<distance<<" old = "<<oldDistance<<" delta = "<<oldDistance-distance<<std::endl;
126#endif
127
128    }
129
130    typedef std::vector<double> DoubleList;
131
132    osg::ref_ptr<osg::EllipsoidModel>   _em;
133
134    osg::Vec3d                          _startPoint;
135    osg::Vec3d                          _startNormal;
136    double                              _startRadius;
137
138    osg::Vec3d                          _endPoint;
139    osg::Vec3d                          _endNormal;
140    double                              _endRadius;
141
142    double                              _angleIncrement;
143
144    DoubleList                          _radiusList;
145    DoubleList                          _distanceList;
146
147};
148
149struct DistanceHeightXYZ
150{
151
152    DistanceHeightXYZ():
153        distance(0.0),
154        height(0.0) {}
155
156    DistanceHeightXYZ(const DistanceHeightXYZ& dh):
157        distance(dh.distance),
158        height(dh.height),
159        position(dh.position) {}
160
161    DistanceHeightXYZ(double d, double h, const osg::Vec3d& pos):
162        distance(d),
163        height(h),
164        position(pos) {}
165
166    bool operator < (const DistanceHeightXYZ& rhs) const
167    {
168        // small distance values first
169        if (distance < rhs.distance) return true;
170        if (distance > rhs.distance) return false;
171
172        // smallest heights first
173        return (height < rhs.height);
174    }
175
176    bool operator == (const DistanceHeightXYZ& rhs) const
177    {
178        return distance==rhs.distance && height==rhs.height;
179    }
180
181    bool operator != (const DistanceHeightXYZ& rhs) const
182    {
183        return distance!=rhs.distance || height!=rhs.height;
184    }
185
186    bool equal_distance(const DistanceHeightXYZ& rhs, double epsilon=1e-6) const
187    {
188        return osg::absolute(rhs.distance - distance) <= epsilon;
189    }
190
191    double      distance;
192    double      height;
193    osg::Vec3d  position;
194};
195
196struct Point : public osg::Referenced, public DistanceHeightXYZ
197{
198    Point() {}
199    Point(double d, double h, const osg::Vec3d& pos):
200         DistanceHeightXYZ(d,h,pos)
201    {
202        //OSG_NOTICE<<"Point::Point distance="<<distance<<" height="<<height<<" position="<<position<<std::endl;
203    }
204
205    Point(const Point& point):
206        osg::Referenced(),
207        DistanceHeightXYZ(point) {}
208
209
210};
211
212struct Segment
213{
214    Segment(Point* p1, Point* p2)
215    {
216        if (*p1 < *p2)
217        {
218            _p1 = p1;
219            _p2 = p2;
220        }
221        else
222        {
223            _p1 = p2;
224            _p2 = p1;
225        }
226
227        //OSG_NOTICE.precision(12);
228        //OSG_NOTICE<<"Segment::Segment p1 = "<<(_p1->distance)<<" "<<(_p1->height)<<"  p2 = "<<(_p2->distance)<<" "<<(_p2->height)<<std::endl;
229    }
230
231
232    bool operator < ( const Segment& rhs) const
233    {
234        if (*_p1 < *rhs._p1) return true;
235        if (*rhs._p1 < *_p1) return false;
236
237        return (*_p2 < *rhs._p2);
238    }
239
240    enum Classification
241    {
242        UNCLASSIFIED,
243        IDENTICAL,
244        SEPERATE,
245        JOINED,
246        OVERLAPPING,
247        ENCLOSING,
248        ENCLOSED
249    };
250
251    Classification compare(const Segment& rhs) const
252    {
253        if (*_p1 == *rhs._p1 && *_p2==*rhs._p2) return IDENTICAL;
254
255        const double epsilon = 1e-3; // 1mm
256
257        double delta_distance = _p2->distance - rhs._p1->distance;
258        if (fabs(delta_distance) < epsilon)
259        {
260            if (fabs(_p2->height - rhs._p1->height) < epsilon) return JOINED;
261        }
262
263        if (delta_distance==0.0)
264        {
265            return SEPERATE;
266        }
267
268        if (rhs._p2->distance < _p1->distance || _p2->distance < rhs._p1->distance) return SEPERATE;
269
270        bool rhs_p1_inside = (_p1->distance <= rhs._p1->distance) && (rhs._p1->distance <= _p2->distance);
271        bool rhs_p2_inside = (_p1->distance <= rhs._p2->distance) && (rhs._p2->distance <= _p2->distance);
272
273        if (rhs_p1_inside && rhs_p2_inside) return ENCLOSING;
274
275        bool p1_inside = (rhs._p1->distance <= _p1->distance) && (_p1->distance <= rhs._p2->distance);
276        bool p2_inside = (rhs._p1->distance <= _p2->distance) && (_p2->distance <= rhs._p2->distance);
277
278        if (p1_inside && p2_inside) return ENCLOSED;
279
280        if (rhs_p1_inside || rhs_p2_inside || p1_inside || p2_inside) return OVERLAPPING;
281
282        return UNCLASSIFIED;
283    }
284
285    double height(double d) const
286    {
287        double delta = (_p2->distance - _p1->distance);
288        return _p1->height + ((_p2->height - _p1->height) * (d - _p1->distance) / delta);
289    }
290
291    double deltaHeight(Point& point) const
292    {
293        return point.height - height(point.distance);
294    }
295
296    Point* createPoint(double d) const
297    {
298        if (d == _p1->distance) return _p1.get();
299        if (d == _p2->distance) return _p2.get();
300
301        double delta = (_p2->distance - _p1->distance);
302        double r = (d - _p1->distance)/delta;
303        double one_minus_r = 1.0 - r;
304        return new Point(d,
305                         _p1->height * one_minus_r + _p2->height * r,
306                         _p1->position * one_minus_r + _p2->position * r);
307    }
308
309    Point* createIntersectionPoint(const Segment& rhs) const
310    {
311        double A = _p1->distance;
312        double B = _p2->distance - _p1->distance;
313        double C = _p1->height;
314        double D = _p2->height - _p1->height;
315
316        double E = rhs._p1->distance;
317        double F = rhs._p2->distance - rhs._p1->distance;
318        double G = rhs._p1->height;
319        double H = rhs._p2->height - rhs._p1->height;
320
321        double div = D*F - B*H;
322        if (div==0.0)
323        {
324            OSG_NOTICE<<"ElevationSlideUtils::Segment::createIntersectionPoint(): error Segments are parallel."<<std::endl;
325            return _p2.get();
326        }
327
328        double r = (G*F - E*H + A*H - C*F) / div;
329
330        if (r<0.0)
331        {
332            OSG_NOTICE<<"ElevationSlideUtils::Segment::createIntersectionPoint(): error intersection point outwith segment, r ="<<r<<std::endl;
333            return _p1.get();
334        }
335
336        if (r>1.0)
337        {
338            OSG_NOTICE<<"ElevationSlideUtils::Segment::createIntersectionPoint(): error intersection point outwith segment, r ="<<r<<std::endl;
339            return _p2.get();
340        }
341
342//         OSG_NOTICE<<"ElevationSlideUtils::Segment::createIntersectionPoint(): r="<<r<<std::endl;
343//         OSG_NOTICE<<"\tp1 = "<<_p1->distance<<" "<<_p1->height<<"  p2 = "<<_p2->distance<<" "<<_p2->height<<std::endl;
344//         OSG_NOTICE<<"\trrhs.p1 = "<<rhs._p1->distance<<" "<<rhs._p1->height<<"  p2 = "<<rhs._p2->distance<<" "<<rhs._p2->height<<std::endl;
345
346        return new Point(A + B*r, C + D*r, _p1->position + (_p2->position - _p1->position)*r);
347    }
348
349
350    osg::ref_ptr<Point> _p1;
351    osg::ref_ptr<Point> _p2;
352
353};
354
355
356struct LineConstructor
357{
358
359    typedef std::set<Segment> SegmentSet;
360
361    LineConstructor() {}
362
363
364
365    void add(double d, double h, const osg::Vec3d& pos)
366    {
367        osg::ref_ptr<Point> newPoint = new Point(d,h,pos);
368
369
370        if (_previousPoint.valid() && newPoint->distance != _previousPoint->distance)
371        {
372            const double maxGradient = 100.0;
373            double gradient = fabs( (newPoint->height - _previousPoint->height) / (newPoint->distance - _previousPoint->distance) );
374
375            if (gradient < maxGradient)
376            {
377                _segments.insert( Segment(_previousPoint.get(), newPoint.get()) );
378            }
379        }
380
381        _previousPoint = newPoint;
382    }
383
384    void endline()
385    {
386        _previousPoint = 0;
387    }
388
389    void report()
390    {
391
392        OSG_NOTICE<<"Number of segments = "<<_segments.size()<<std::endl;
393
394        for(SegmentSet::iterator itr = _segments.begin();
395             itr != _segments.end();
396             ++itr)
397        {
398            const Segment& seg = *itr;
399            OSG_NOTICE<<"p1 = "<<(seg._p1->distance)<<" "<<(seg._p1->height)<<"  p2 = "<<(seg._p2->distance)<<" "<<(seg._p2->height)<<"\t";
400
401            SegmentSet::iterator nextItr = itr;
402            ++nextItr;
403            if (nextItr != _segments.end())
404            {
405                Segment::Classification classification = itr->compare(*nextItr);
406                switch(classification)
407                {
408                    case(Segment::IDENTICAL): OSG_NOTICE<<"i"; break;
409                    case(Segment::SEPERATE): OSG_NOTICE<<"s"<<std::endl; break;
410                    case(Segment::JOINED): OSG_NOTICE<<"j"; break;
411                    case(Segment::OVERLAPPING): OSG_NOTICE<<"o"; break;
412                    case(Segment::ENCLOSING): OSG_NOTICE<<"E"; break;
413                    case(Segment::ENCLOSED): OSG_NOTICE<<"e"; break;
414                    case(Segment::UNCLASSIFIED): OSG_NOTICE<<"U"; break;
415                }
416            }
417
418            OSG_NOTICE<<std::endl;
419
420        }
421
422        OSG_NOTICE<<std::endl;
423
424        if (_em.valid())
425        {
426            for(SegmentSet::iterator itr = _segments.begin();
427                itr != _segments.end();
428                ++itr)
429            {
430                const Segment& s = *itr;
431                osg::Vec3d p;
432
433
434                double latitude, longitude, height;
435
436                p = s._p1->position;
437                _em->convertXYZToLatLongHeight(p.x(), p.y(), p.z(), latitude, longitude, height);
438                double delta1 = height - s._p1->height;
439
440                p = s._p1->position;
441                _em->convertXYZToLatLongHeight(p.x(), p.y(), p.z(), latitude, longitude, height);
442                double delta2 = height - s._p2->height;
443
444                if (delta1>0.0 || delta2>0.0)
445                {
446                    OSG_NOTICE<<"   "<<&s<<" computed height delta  ="<<delta1<<"  delta2= "<<delta2<<std::endl;
447                }
448            }
449        }
450
451    }
452
453    void pruneOverlappingSegments()
454    {
455        SegmentSet::iterator prevItr = _segments.begin();
456        SegmentSet::iterator nextItr = prevItr;
457        ++nextItr;
458
459        double epsilon = 0.001;
460
461        for(SegmentSet::iterator itr = _segments.begin();
462            itr != _segments.end();
463            ++itr)
464        {
465            SegmentSet::iterator nextItr = itr;
466            ++nextItr;
467            Segment::Classification classification = nextItr != _segments.end() ?  itr->compare(*nextItr) : Segment::UNCLASSIFIED;
468
469            // if (classification>=Segment::OVERLAPPING) OSG_NOTICE<<std::endl;
470            // else OSG_NOTICE<<".";
471            // OSG_NOTICE.precision(12);
472
473            while (classification>=Segment::OVERLAPPING)
474            {
475
476                switch(classification)
477                {
478                    case(Segment::OVERLAPPING):
479                    {
480                        // cases....
481                        // compute new end points for both segments
482                        // need to work out which points are overlapping - lhs_p2 && rhs_p1  or  lhs_p1 and rhs_p2
483                        // also need to check for cross cases.
484
485                        const Segment& lhs = *itr;
486                        const Segment& rhs = *nextItr;
487
488                        bool rhs_p1_inside = (lhs._p1->distance <= rhs._p1->distance) && (rhs._p1->distance <= lhs._p2->distance);
489                        bool lhs_p2_inside = (rhs._p1->distance <= lhs._p2->distance) && (lhs._p2->distance <= rhs._p2->distance);
490
491                        if (rhs_p1_inside && lhs_p2_inside)
492                        {
493                            double distance_between = osg::Vec2d(lhs._p2->distance - rhs._p1->distance,
494                                                                 lhs._p2->height - rhs._p1->height).length2();
495
496                            if (distance_between < epsilon)
497                            {
498                                // OSG_NOTICE<<"OVERLAPPING : distance_between acceptable "<<distance_between<<std::endl;
499
500                                Segment newSeg(lhs._p2.get(), rhs._p2.get());
501                                _segments.insert(newSeg);
502
503                                _segments.erase(nextItr);
504
505                                nextItr = _segments.find(newSeg);
506                            }
507                            else
508                            {
509//                                OSG_NOTICE<<"OVERLAPPING : distance_between unacceptable "<<distance_between<<std::endl;
510
511                                double dh1 = lhs.deltaHeight(*rhs._p1);
512                                double dh2 = -rhs.deltaHeight(*lhs._p2);
513
514                                if (dh1 * dh2 < 0.0)
515                                {
516//                                     OSG_NOTICE<<"OVERLAPPING : crossing "<<dh1<<" "<<dh2<<std::endl;
517//                                     OSG_NOTICE<<"    lhs_p1 "<<lhs._p1->distance<<" "<<lhs._p1->height<<std::endl;
518//                                     OSG_NOTICE<<"    lhs_p2 "<<lhs._p2->distance<<" "<<lhs._p2->height<<std::endl;
519//                                     OSG_NOTICE<<"    rhs_p1 "<<rhs._p1->distance<<" "<<rhs._p1->height<<std::endl;
520//                                     OSG_NOTICE<<"    rhs_p2 "<<rhs._p2->distance<<" "<<rhs._p2->height<<std::endl;
521
522                                    Point* cp = lhs.createIntersectionPoint(rhs);
523
524                                    Segment seg1( lhs._p1.get(), lhs.createPoint(rhs._p2->distance) );
525                                    Segment seg2( rhs._p1.get(), cp );
526                                    Segment seg3( cp, lhs._p2.get() );
527                                    Segment seg4( rhs.createPoint(lhs._p2->distance), lhs._p2.get() );
528
529                                    _segments.erase(nextItr);
530                                    _segments.erase(itr);
531
532                                    _segments.insert(seg1);
533                                    _segments.insert(seg2);
534                                    _segments.insert(seg3);
535                                    _segments.insert(seg4);
536
537                                    itr = _segments.find(seg1);
538                                    nextItr = itr;
539                                    ++nextItr;
540
541                                }
542                                else if (dh1 <= 0.0 && dh2 <= 0.0)
543                                {
544//                                     OSG_NOTICE<<"++ OVERLAPPING : rhs below lhs "<<dh1<<" "<<dh2<<std::endl;
545//                                     OSG_NOTICE<<"    lhs_p1 "<<lhs._p1->distance<<" "<<lhs._p1->height<<std::endl;
546//                                     OSG_NOTICE<<"    lhs_p2 "<<lhs._p2->distance<<" "<<lhs._p2->height<<std::endl;
547//                                     OSG_NOTICE<<"    rhs_p1 "<<rhs._p1->distance<<" "<<rhs._p1->height<<std::endl;
548//                                     OSG_NOTICE<<"    rhs_p2 "<<rhs._p2->distance<<" "<<rhs._p2->height<<std::endl;
549
550                                    Segment newSeg(rhs.createPoint(lhs._p2->distance), rhs._p2.get());
551
552                                    _segments.erase(nextItr);
553                                    _segments.insert(newSeg);
554                                    nextItr = itr;
555                                    ++nextItr;
556
557//                                     OSG_NOTICE<<"    newSeg_p1 "<<newSeg._p1->distance<<" "<<newSeg._p1->height<<std::endl;
558//                                     OSG_NOTICE<<"    newSeg_p2 "<<newSeg._p2->distance<<" "<<newSeg._p2->height<<std::endl;
559                                 }
560                                else if (dh1 >= 0.0 && dh2 >= 0.0)
561                                {
562//                                     OSG_NOTICE<<"++ OVERLAPPING : rhs above lhs "<<dh1<<" "<<dh2<<std::endl;
563//                                     OSG_NOTICE<<"    lhs_p1 "<<lhs._p1->distance<<" "<<lhs._p1->height<<std::endl;
564//                                     OSG_NOTICE<<"    lhs_p2 "<<lhs._p2->distance<<" "<<lhs._p2->height<<std::endl;
565//                                     OSG_NOTICE<<"    rhs_p1 "<<rhs._p1->distance<<" "<<rhs._p1->height<<std::endl;
566//                                     OSG_NOTICE<<"    rhs_p2 "<<rhs._p2->distance<<" "<<rhs._p2->height<<std::endl;
567
568
569                                    Segment newSeg(lhs._p1.get(), lhs.createPoint(rhs._p1->distance));
570
571                                    _segments.erase(itr);
572                                    _segments.insert(newSeg);
573                                    itr = _segments.find(newSeg);
574                                    nextItr = itr;
575                                    ++nextItr;
576
577//                                     OSG_NOTICE<<"    newSeg_p1 "<<newSeg._p1->distance<<" "<<newSeg._p1->height<<std::endl;
578//                                     OSG_NOTICE<<"    newSeg_p2 "<<newSeg._p2->distance<<" "<<newSeg._p2->height<<std::endl;
579
580                                }
581                                else
582                                {
583                                    OSG_NOTICE<<"OVERLAPPING : unidentified "<<dh1 <<" "<<dh2<<std::endl;
584                                    OSG_NOTICE<<"    lhs_p1 "<<lhs._p1->distance<<" "<<lhs._p1->height<<std::endl;
585                                    OSG_NOTICE<<"    lhs_p2 "<<lhs._p2->distance<<" "<<lhs._p2->height<<std::endl;
586                                    OSG_NOTICE<<"    rhs_p1 "<<rhs._p1->distance<<" "<<rhs._p1->height<<std::endl;
587                                    OSG_NOTICE<<"    rhs_p2 "<<rhs._p2->distance<<" "<<rhs._p2->height<<std::endl;
588                                    ++nextItr;
589                                }
590                            }
591                        }
592                        else
593                        {
594                            OSG_NOTICE<<" OVERLAPPING problem, !rhs_p1_inside || !lhs_p2_inside - unhandled case" <<std::endl;
595                            ++nextItr;
596                        }
597
598
599                        break;
600                    }
601                    case(Segment::ENCLOSING):
602                    {
603                        // need to work out if rhs is below lhs or rhs is above lhs or crossing
604
605                        const Segment& enclosing = *itr;
606                        const Segment& enclosed = *nextItr;
607                        double dh1 = enclosing.deltaHeight(*enclosed._p1);
608                        double dh2 = enclosing.deltaHeight(*enclosed._p2);
609
610                        if (dh1<=epsilon && dh2<=epsilon)
611                        {
612                            // OSG_NOTICE<<"+++ ENCLOSING: ENCLOSING is above enclosed "<<dh1<<" "<<dh2<<std::endl;
613
614                            _segments.erase(nextItr);
615
616                            nextItr = itr;
617                            ++nextItr;
618
619                        }
620                        else if (dh1>=0.0 && dh2>=0.0)
621                        {
622
623                            double d_left = enclosed._p1->distance - enclosing._p1->distance;
624                            double d_right = enclosing._p2->distance - enclosed._p2->distance;
625
626                            if (d_left < epsilon && d_right < epsilon)
627                            {
628                                // treat ENCLOSED as ENCLOSING.
629                                // OSG_NOTICE<<"   Treat enclosed above as enclosing "<<std::endl;
630
631                                nextItr = itr;
632                                ++nextItr;
633
634                                _segments.erase(itr);
635
636                                itr = nextItr;
637                                ++nextItr;
638
639                            }
640                            else if (d_left < epsilon)
641                            {
642//                                 OSG_NOTICE<<"ENCLOSING: ENCLOSING is below enclosed "<<dh1<<" "<<dh2<<std::endl;
643//
644//                                 OSG_NOTICE<<"    enclosing_p1 "<<enclosing._p1->distance<<" "<<enclosing._p1->height<<std::endl;
645//                                 OSG_NOTICE<<"    enclosing_p2 "<<enclosing._p2->distance<<" "<<enclosing._p2->height<<std::endl;
646//                                 OSG_NOTICE<<"    enclosed_p1 "<<enclosed._p1->distance<<" "<<enclosed._p1->height<<std::endl;
647//                                 OSG_NOTICE<<"    enclosed_p2 "<<enclosed._p2->distance<<" "<<enclosed._p2->height<<std::endl;
648//
649//                                 OSG_NOTICE<<"   Replace enclosing with right section"<<std::endl;
650
651                                Segment newSeg(enclosing.createPoint(enclosed._p2->distance), enclosing._p2.get());
652                                nextItr = itr;
653                                ++nextItr;
654
655                                _segments.erase(itr);
656                                _segments.insert(newSeg);
657
658                                itr = nextItr;
659                                ++nextItr;
660
661//                                 OSG_NOTICE<<"    newSeg_p1 "<<newSeg._p1->distance<<" "<<newSeg._p1->height<<std::endl;
662//                                 OSG_NOTICE<<"    newSeg_p2 "<<newSeg._p2->distance<<" "<<newSeg._p2->height<<std::endl;
663                            }
664                            else if (d_right < epsilon)
665                            {
666//                                 OSG_NOTICE<<"ENCLOSING: ENCLOSING is below enclosed "<<dh1<<" "<<dh2<<std::endl;
667//
668//                                 OSG_NOTICE<<"    enclosing_p1 "<<enclosing._p1->distance<<" "<<enclosing._p1->height<<std::endl;
669//                                 OSG_NOTICE<<"    enclosing_p2 "<<enclosing._p2->distance<<" "<<enclosing._p2->height<<std::endl;
670//                                 OSG_NOTICE<<"    enclosed_p1 "<<enclosed._p1->distance<<" "<<enclosed._p1->height<<std::endl;
671//                                 OSG_NOTICE<<"    enclosed_p2 "<<enclosed._p2->distance<<" "<<enclosed._p2->height<<std::endl;
672//
673//                                 OSG_NOTICE<<"   Replace enclosing with left section"<<std::endl;
674
675                                Segment newSeg(enclosing._p1.get(), enclosing.createPoint(enclosed._p1->distance) );
676
677                                _segments.insert(newSeg);
678                                _segments.erase(itr);
679
680                                itr = _segments.find(newSeg);
681                                nextItr = itr;
682                                ++nextItr;
683
684//                                 OSG_NOTICE<<"    newSeg_p1 "<<newSeg._p1->distance<<" "<<newSeg._p1->height<<std::endl;
685//                                 OSG_NOTICE<<"    newSeg_p2 "<<newSeg._p2->distance<<" "<<newSeg._p2->height<<std::endl;
686                            }
687                            else
688                            {
689//                                 OSG_NOTICE<<"ENCLOSING: ENCLOSING is below enclosed "<<dh1<<" "<<dh2<<std::endl;
690//
691//                                 OSG_NOTICE<<"    enclosing_p1 "<<enclosing._p1->distance<<" "<<enclosing._p1->height<<std::endl;
692//                                 OSG_NOTICE<<"    enclosing_p2 "<<enclosing._p2->distance<<" "<<enclosing._p2->height<<std::endl;
693//                                 OSG_NOTICE<<"    enclosed_p1 "<<enclosed._p1->distance<<" "<<enclosed._p1->height<<std::endl;
694//                                 OSG_NOTICE<<"    enclosed_p2 "<<enclosed._p2->distance<<" "<<enclosed._p2->height<<std::endl;
695//
696//                                 OSG_NOTICE<<"   Replace enclosing with left and right sections"<<std::endl;
697
698
699                                Segment newSegLeft(enclosing._p1.get(), enclosing.createPoint(enclosed._p1->distance) );
700                                Segment newSegRight(enclosing.createPoint(enclosed._p2->distance), enclosing._p2.get());
701
702                                _segments.erase(itr);
703                                _segments.insert(newSegLeft);
704                                _segments.insert(newSegRight);
705
706                                itr = _segments.find(newSegLeft);
707                                nextItr = itr;
708                                ++nextItr;
709
710//                                 OSG_NOTICE<<"    newSegLeft_p1 "<<newSegLeft._p1->distance<<" "<<newSegLeft._p1->height<<std::endl;
711//                                 OSG_NOTICE<<"    newSegLeft_p2 "<<newSegLeft._p2->distance<<" "<<newSegLeft._p2->height<<std::endl;
712//                                 OSG_NOTICE<<"    newSegRight_p1 "<<newSegRight._p1->distance<<" "<<newSegRight._p1->height<<std::endl;
713//                                 OSG_NOTICE<<"    newSegRight_p2 "<<newSegRight._p2->distance<<" "<<newSegRight._p2->height<<std::endl;
714
715                            }
716
717                        }
718                        else if (dh1 * dh2 < 0.0)
719                        {
720//                             OSG_NOTICE<<"ENCLOSING: ENCLOSING is crossing enclosed "<<dh1<<" "<<dh2<<std::endl;
721//
722//                             OSG_NOTICE<<"    enclosing_p1 "<<enclosing._p1->distance<<" "<<enclosing._p1->height<<std::endl;
723//                             OSG_NOTICE<<"    enclosing_p2 "<<enclosing._p2->distance<<" "<<enclosing._p2->height<<std::endl;
724//                             OSG_NOTICE<<"    enclosed_p1 "<<enclosed._p1->distance<<" "<<enclosed._p1->height<<std::endl;
725//                             OSG_NOTICE<<"    enclosed_p2 "<<enclosed._p2->distance<<" "<<enclosed._p2->height<<std::endl;
726
727                            double d_left = enclosed._p1->distance - enclosing._p1->distance;
728                            double d_right = enclosing._p2->distance - enclosed._p2->distance;
729
730                            if (d_left < epsilon && d_right < epsilon)
731                            {
732                                // treat ENCLOSED as ENCLOSING.
733                                if (dh1 < 0.0)
734                                {
735                                    // OSG_NOTICE<<"   >> enclosing left side is above enclosed left side"<<std::endl;
736
737                                    Point* cp = enclosing.createIntersectionPoint(enclosed);
738                                    Segment newSegLeft(enclosing._p1.get(), cp);
739                                    Segment newSegRight(cp, enclosed._p2.get());
740
741                                    _segments.erase(itr);
742                                    _segments.erase(nextItr);
743
744                                    _segments.insert(newSegLeft);
745                                    _segments.insert(newSegRight);
746
747                                    itr = _segments.find(newSegLeft);
748                                    nextItr = itr;
749                                    ++nextItr;
750                                }
751                                else
752                                {
753                                    // OSG_NOTICE<<"   >> enclosing left side is above enclosed left side"<<std::endl;
754
755                                    Point* cp = enclosing.createIntersectionPoint(enclosed);
756                                    Segment newSegLeft(enclosed._p1.get(), cp);
757                                    Segment newSegRight(cp, enclosing._p2.get());
758
759                                    _segments.erase(itr);
760                                    _segments.erase(nextItr);
761
762                                    _segments.insert(newSegLeft);
763                                    _segments.insert(newSegRight);
764
765                                    itr = _segments.find(newSegLeft);
766                                    nextItr = itr;
767                                    ++nextItr;
768                                }
769
770                            }
771                            else if (d_left < epsilon)
772                            {
773                                // OSG_NOTICE<<"   >> Replace enclosing with right section"<<std::endl;
774
775                                if (dh1 < 0.0)
776                                {
777                                    // OSG_NOTICE<<"   >> enclosing left side is above enclosed left side"<<std::endl;
778
779                                    Point* cp = enclosing.createIntersectionPoint(enclosed);
780                                    Segment newSegLeft(enclosing._p1.get(), cp);
781                                    Segment newSegMid(cp, enclosed._p2.get());
782                                    Segment newSegRight(enclosing.createPoint(enclosed._p2->distance), enclosing._p2.get());
783
784                                    _segments.erase(itr);
785                                    _segments.erase(nextItr);
786
787                                    _segments.insert(newSegLeft);
788                                    _segments.insert(newSegMid);
789                                    _segments.insert(newSegRight);
790
791                                    itr = _segments.find(newSegLeft);
792                                    nextItr = itr;
793                                    ++nextItr;
794                                }
795                                else
796                                {
797                                    // OSG_NOTICE<<"   >> enclosing left side is above enclosed left side"<<std::endl;
798
799                                    Point* cp = enclosing.createIntersectionPoint(enclosed);
800                                    Segment newSegLeft(enclosed._p1.get(), cp);
801                                    Segment newSegRight(cp, enclosing._p2.get());
802
803                                    _segments.erase(itr);
804                                    _segments.erase(nextItr);
805
806                                    _segments.insert(newSegLeft);
807                                    _segments.insert(newSegRight);
808
809                                    itr = _segments.find(newSegLeft);
810                                    nextItr = itr;
811                                    ++nextItr;
812                                }
813                            }
814                            else if (d_right < epsilon)
815                            {
816//                                OSG_NOTICE<<"   >> Replace enclosing with left section"<<std::endl;
817
818                                if (dh1 < 0.0)
819                                {
820//                                    OSG_NOTICE<<"   >> enclosing left side is above enclosed left side"<<std::endl;
821
822                                    Point* cp = enclosing.createIntersectionPoint(enclosed);
823                                    Segment newSegLeft(enclosing._p1.get(), cp);
824                                    Segment newSegRight(cp, enclosed._p2.get());
825
826                                    _segments.erase(itr);
827                                    _segments.erase(nextItr);
828
829                                    _segments.insert(newSegLeft);
830                                    _segments.insert(newSegRight);
831
832                                    itr = _segments.find(newSegLeft);
833                                    nextItr = itr;
834                                    ++nextItr;
835                                }
836                                else
837                                {
838//                                    OSG_NOTICE<<"   >> enclosing left side is above enclosed left side"<<std::endl;
839
840                                    Point* cp = enclosing.createIntersectionPoint(enclosed);
841                                    Segment newSegLeft(enclosing._p1.get(), enclosing.createPoint(enclosed._p1->distance));
842                                    Segment newSegMid(enclosed._p1.get(), cp);
843                                    Segment newSegRight(cp, enclosing._p2.get());
844
845                                    _segments.erase(itr);
846                                    _segments.erase(nextItr);
847
848                                    _segments.insert(newSegLeft);
849                                    _segments.insert(newSegMid);
850                                    _segments.insert(newSegRight);
851
852                                    itr = _segments.find(newSegLeft);
853                                    nextItr = itr;
854                                    ++nextItr;
855                                }
856                            }
857                            else
858                            {
859//                                OSG_NOTICE<<"   >> Replace enclosing with left and right sections"<<std::endl;
860
861
862                                if (dh1 < 0.0)
863                                {
864//                                    OSG_NOTICE<<"   >> enclosing left side is above enclosed left side"<<std::endl;
865
866                                    Point* cp = enclosing.createIntersectionPoint(enclosed);
867                                    Segment newSegLeft(enclosing._p1.get(), cp);
868                                    Segment newSegMid(cp, enclosed._p2.get());
869                                    Segment newSegRight(enclosing.createPoint(enclosed._p2->distance), enclosing._p2.get());
870
871                                    _segments.erase(itr);
872                                    _segments.erase(nextItr);
873
874                                    _segments.insert(newSegLeft);
875                                    _segments.insert(newSegMid);
876                                    _segments.insert(newSegRight);
877
878                                    itr = _segments.find(newSegLeft);
879                                    nextItr = itr;
880                                    ++nextItr;
881                                }
882                                else
883                                {
884//                                    OSG_NOTICE<<"   >> enclosing left side is above enclosed left side"<<std::endl;
885
886                                    Point* cp = enclosing.createIntersectionPoint(enclosed);
887                                    Segment newSegLeft(enclosing._p1.get(), enclosing.createPoint(enclosed._p1->distance));
888                                    Segment newSegMid(enclosed._p1.get(), cp);
889                                    Segment newSegRight(cp, enclosing._p2.get());
890
891                                    _segments.erase(itr);
892                                    _segments.erase(nextItr);
893
894                                    _segments.insert(newSegLeft);
895                                    _segments.insert(newSegMid);
896                                    _segments.insert(newSegRight);
897
898                                    itr = _segments.find(newSegLeft);
899                                    nextItr = itr;
900                                    ++nextItr;
901                                }
902
903                            }
904                        }
905                        else
906                        {
907                            OSG_NOTICE<<"ENCLOSING: ENCLOSING - error case "<<dh1<<" "<<dh2<<std::endl;
908
909                            OSG_NOTICE<<"    enclosing_p1 "<<enclosing._p1->distance<<" "<<enclosing._p1->height<<std::endl;
910                            OSG_NOTICE<<"    enclosing_p2 "<<enclosing._p2->distance<<" "<<enclosing._p2->height<<std::endl;
911                            OSG_NOTICE<<"    enclosed_p1 "<<enclosed._p1->distance<<" "<<enclosed._p1->height<<std::endl;
912                            OSG_NOTICE<<"    enclosed_p2 "<<enclosed._p2->distance<<" "<<enclosed._p2->height<<std::endl;
913                            ++nextItr;
914                        }
915
916                        break;
917                    }
918                    case(Segment::ENCLOSED):
919                    {
920                        // need to work out if lhs is below rhs or lhs is above rhs or crossing
921                        const Segment& enclosing = *nextItr;
922                        const Segment& enclosed = *itr;
923                        double dh1 = enclosing.deltaHeight(*enclosed._p1);
924                        double dh2 = enclosing.deltaHeight(*enclosed._p2);
925
926                        double d_left = enclosed._p1->distance - enclosing._p1->distance;
927                        double d_right = enclosing._p2->distance - enclosed._p2->distance;
928
929                        if (d_left<=epsilon)
930                        {
931
932                            if (dh1<=epsilon && dh2<=epsilon)
933                            {
934                                // OSG_NOTICE<<"+++ ENCLOSED: ENCLOSING is above enclosed "<<dh1<<" "<<dh2<<std::endl;
935                                _segments.erase(itr);
936
937                                itr = nextItr;
938                                ++nextItr;
939                            }
940                            else if (dh1>=0.0 && dh2>=0.0)
941                            {
942//                                 OSG_NOTICE<<"ENCLOSED: ENCLOSING is below enclosed "<<dh1<<" "<<dh2<<std::endl;
943//                                 OSG_NOTICE<<"    d_left "<<d_left<<" d_right="<<d_right<<std::endl;
944//                                 OSG_NOTICE<<"    enclosing_p1 "<<enclosing._p1->distance<<" "<<enclosing._p1->height<<std::endl;
945//                                 OSG_NOTICE<<"    enclosing_p2 "<<enclosing._p2->distance<<" "<<enclosing._p2->height<<std::endl;
946//                                 OSG_NOTICE<<"    enclosed_p1 "<<enclosed._p1->distance<<" "<<enclosed._p1->height<<std::endl;
947//                                 OSG_NOTICE<<"    enclosed_p2 "<<enclosed._p2->distance<<" "<<enclosed._p2->height<<std::endl;
948
949                                _segments.insert(Segment(enclosing.createPoint(enclosed._p2->distance), enclosed._p2.get()));
950                                _segments.erase(nextItr);
951
952                                nextItr = itr;
953                                ++nextItr;
954                            }
955                            else if (dh1 * dh2 < 0.0)
956                            {
957//                                 OSG_NOTICE<<"ENCLOSED: ENCLOSING is crossing enclosed "<<dh1<<" "<<dh2<<std::endl;
958//                                 OSG_NOTICE<<"    enclosing_p1 "<<enclosing._p1->distance<<" "<<enclosing._p1->height<<std::endl;
959//                                 OSG_NOTICE<<"    enclosing_p2 "<<enclosing._p2->distance<<" "<<enclosing._p2->height<<std::endl;
960//                                 OSG_NOTICE<<"    enclosed_p1 "<<enclosed._p1->distance<<" "<<enclosed._p1->height<<std::endl;
961//                                 OSG_NOTICE<<"    enclosed_p2 "<<enclosed._p2->distance<<" "<<enclosed._p2->height<<std::endl;
962
963                                if (d_right<=epsilon)
964                                {
965                                    // enclosed and enclosing effectively overlap
966                                    if (dh1 < 0.0)
967                                    {
968                                        Point* cp = enclosed.createIntersectionPoint(enclosing);
969                                        Segment segLeft(enclosed._p1.get(), cp);
970                                        Segment segRight(cp, enclosing._p2.get());
971
972                                        _segments.erase(itr);
973                                        _segments.erase(nextItr);
974
975                                        _segments.insert(segLeft);
976                                        _segments.insert(segRight);
977
978                                        itr = _segments.find(segLeft);
979                                        nextItr = itr;
980                                        ++nextItr;
981                                    }
982                                    else
983                                    {
984                                        Point* cp = enclosed.createIntersectionPoint(enclosing);
985                                        Segment segLeft(enclosing._p1.get(), cp);
986                                        Segment segRight(cp, enclosed._p2.get());
987
988                                        _segments.erase(itr);
989                                        _segments.erase(nextItr);
990
991                                        _segments.insert(segLeft);
992                                        _segments.insert(segRight);
993
994                                        itr = _segments.find(segLeft);
995                                        nextItr = itr;
996                                        ++nextItr;
997                                    }
998                                }
999                                else
1000                                {
1001                                    // right hand side needs to be created
1002                                    if (dh1 < 0.0)
1003                                    {
1004                                        Point* cp = enclosed.createIntersectionPoint(enclosing);
1005                                        Segment segLeft(enclosed._p1.get(), cp);
1006                                        Segment segRight(cp, enclosing._p2.get());
1007
1008                                        _segments.erase(itr);
1009                                        _segments.erase(nextItr);
1010
1011                                        _segments.insert(segLeft);
1012                                        _segments.insert(segRight);
1013
1014                                        itr = _segments.find(segLeft);
1015                                        nextItr = itr;
1016                                        ++nextItr;
1017                                    }
1018                                    else
1019                                    {
1020                                        Point* cp = enclosed.createIntersectionPoint(enclosing);
1021                                        Segment segLeft(enclosing._p1.get(), cp);
1022                                        Segment segMid(cp, enclosed._p2.get());
1023                                        Segment segRight(enclosing.createPoint(enclosed._p2->distance), enclosing._p2.get());
1024
1025                                        _segments.erase(itr);
1026                                        _segments.erase(nextItr);
1027
1028                                        _segments.insert(segLeft);
1029                                        _segments.insert(segMid);
1030                                        _segments.insert(segRight);
1031
1032                                        itr = _segments.find(segLeft);
1033                                        nextItr = itr;
1034                                        ++nextItr;
1035                                    }
1036                                }
1037                            }
1038                            else
1039                            {
1040                                OSG_NOTICE<<"ENCLOSED: ENCLOSING - error case "<<dh1<<" "<<dh2<<std::endl;
1041                                OSG_NOTICE<<"    enclosing_p1 "<<enclosing._p1->distance<<" "<<enclosing._p1->height<<std::endl;
1042                                OSG_NOTICE<<"    enclosing_p2 "<<enclosing._p2->distance<<" "<<enclosing._p2->height<<std::endl;
1043                                OSG_NOTICE<<"    enclosed_p1 "<<enclosed._p1->distance<<" "<<enclosed._p1->height<<std::endl;
1044                                OSG_NOTICE<<"    enclosed_p2 "<<enclosed._p2->distance<<" "<<enclosed._p2->height<<std::endl;
1045                                ++nextItr;
1046                            }
1047                        }
1048                        else
1049                        {
1050                            OSG_NOTICE<<"*** ENCLOSED: is not coincendet with left handside of ENCLOSING, case not handled, advancing."<<std::endl;
1051                        }
1052
1053                        break;
1054                    }
1055                    default:
1056                        OSG_NOTICE<<"** Not handled, advancing"<<std::endl;
1057                        ++nextItr;
1058                        break;
1059                }
1060
1061                classification = ((itr != _segments.end()) && (nextItr != _segments.end())) ?  itr->compare(*nextItr) : Segment::UNCLASSIFIED;
1062            }
1063        }
1064    }
1065
1066    unsigned int numOverlapping(SegmentSet::const_iterator startItr) const
1067    {
1068        if (startItr==_segments.end()) return 0;
1069
1070        SegmentSet::const_iterator nextItr = startItr;
1071        ++nextItr;
1072
1073        unsigned int num = 0;
1074        while (nextItr!=_segments.end() && startItr->compare(*nextItr)>=Segment::OVERLAPPING)
1075        {
1076            ++num;
1077            ++nextItr;
1078        }
1079        return num;
1080    }
1081
1082    unsigned int totalNumOverlapping() const
1083    {
1084        unsigned int total = 0;
1085        for(SegmentSet::const_iterator itr = _segments.begin();
1086            itr != _segments.end();
1087            ++itr)
1088        {
1089            total += numOverlapping(itr);
1090        }
1091        return total;
1092    }
1093
1094    void copyPoints(ElevationSlice::Vec3dList& intersections, ElevationSlice::DistanceHeightList& distanceHeightIntersections)
1095    {
1096        SegmentSet::iterator prevItr = _segments.begin();
1097        SegmentSet::iterator nextItr = prevItr;
1098        ++nextItr;
1099
1100        intersections.push_back( prevItr->_p1->position );
1101        distanceHeightIntersections.push_back( ElevationSlice::DistanceHeight(prevItr->_p1->distance, prevItr->_p1->height) );
1102
1103        intersections.push_back( prevItr->_p2->position );
1104        distanceHeightIntersections.push_back( ElevationSlice::DistanceHeight(prevItr->_p2->distance, prevItr->_p2->height) );
1105
1106        for(;
1107            nextItr != _segments.end();
1108            ++nextItr,++prevItr)
1109        {
1110            Segment::Classification classification = prevItr->compare(*nextItr);
1111            switch(classification)
1112            {
1113                case(Segment::SEPERATE):
1114                {
1115                    intersections.push_back( nextItr->_p1->position );
1116                    distanceHeightIntersections.push_back( ElevationSlice::DistanceHeight(nextItr->_p1->distance, nextItr->_p1->height) );
1117
1118                    intersections.push_back( nextItr->_p2->position );
1119                    distanceHeightIntersections.push_back( ElevationSlice::DistanceHeight(nextItr->_p2->distance, nextItr->_p2->height) );
1120                    break;
1121                }
1122                case(Segment::JOINED):
1123                {
1124#if 1
1125                    intersections.push_back( nextItr->_p2->position );
1126                    distanceHeightIntersections.push_back( ElevationSlice::DistanceHeight(nextItr->_p2->distance, nextItr->_p2->height) );
1127#else
1128                    intersections.push_back( nextItr->_p1->position );
1129                    distanceHeightIntersections.push_back( ElevationSlice::DistanceHeight(nextItr->_p1->distance, nextItr->_p1->height) );
1130
1131                    intersections.push_back( nextItr->_p2->position );
1132                    distanceHeightIntersections.push_back( ElevationSlice::DistanceHeight(nextItr->_p2->distance, nextItr->_p2->height) );
1133#endif
1134                    break;
1135                }
1136                default:
1137                {
1138                    intersections.push_back( nextItr->_p1->position );
1139                    distanceHeightIntersections.push_back( ElevationSlice::DistanceHeight(nextItr->_p1->distance, nextItr->_p1->height) );
1140
1141                    intersections.push_back( nextItr->_p2->position );
1142                    distanceHeightIntersections.push_back( ElevationSlice::DistanceHeight(nextItr->_p2->distance, nextItr->_p2->height) );
1143                    break;
1144                }
1145            }
1146
1147        }
1148
1149    }
1150
1151    SegmentSet _segments;
1152    osg::ref_ptr<Point> _previousPoint;
1153    osg::Plane  _plane;
1154    osg::ref_ptr<osg::EllipsoidModel>   _em;
1155
1156};
1157
1158}
1159
1160ElevationSlice::ElevationSlice()
1161{
1162    setDatabaseCacheReadCallback(new DatabaseCacheReadCallback);
1163}
1164
1165void ElevationSlice::computeIntersections(osg::Node* scene, osg::Node::NodeMask traversalMask)
1166{
1167    osg::CoordinateSystemNode* csn = dynamic_cast<osg::CoordinateSystemNode*>(scene);
1168    osg::EllipsoidModel* em = csn ? csn->getEllipsoidModel() : 0;
1169
1170
1171    osg::Plane plane;
1172    osg::Polytope boundingPolytope;
1173
1174    if (em)
1175    {
1176
1177        osg::Vec3d start_upVector = em->computeLocalUpVector(_startPoint.x(), _startPoint.y(), _startPoint.z());
1178        osg::Vec3d end_upVector = em->computeLocalUpVector(_endPoint.x(), _endPoint.y(), _endPoint.z());
1179
1180        double start_latitude, start_longitude, start_height;
1181        em->convertXYZToLatLongHeight(_startPoint.x(), _startPoint.y(), _startPoint.z(),
1182                                      start_latitude, start_longitude, start_height);
1183
1184        OSG_NOTICE<<"start_lat = "<<start_latitude<<" start_longitude = "<<start_longitude<<" start_height = "<<start_height<<std::endl;
1185
1186        double end_latitude, end_longitude, end_height;
1187        em->convertXYZToLatLongHeight(_endPoint.x(), _endPoint.y(), _endPoint.z(),
1188                                      end_latitude, end_longitude, end_height);
1189
1190        OSG_NOTICE<<"end_lat = "<<end_latitude<<" end_longitude = "<<end_longitude<<" end_height = "<<end_height<<std::endl;
1191
1192        // set up the main intersection plane
1193        osg::Vec3d planeNormal = (_endPoint - _startPoint) ^ start_upVector;
1194        planeNormal.normalize();
1195        plane.set( planeNormal, _startPoint );
1196
1197        // set up the start cut off plane
1198        osg::Vec3d startPlaneNormal = start_upVector ^ planeNormal;
1199        startPlaneNormal.normalize();
1200        boundingPolytope.add( osg::Plane(startPlaneNormal, _startPoint) );
1201
1202        // set up the end cut off plane
1203        osg::Vec3d endPlaneNormal = planeNormal ^ end_upVector;
1204        endPlaneNormal.normalize();
1205        boundingPolytope.add( osg::Plane(endPlaneNormal, _endPoint) );
1206    }
1207    else
1208    {
1209        osg::Vec3d upVector (0.0, 0.0, 1.0);
1210
1211        // set up the main intersection plane
1212        osg::Vec3d planeNormal = (_endPoint - _startPoint) ^ upVector;
1213        planeNormal.normalize();
1214        plane.set( planeNormal, _startPoint );
1215
1216        // set up the start cut off plane
1217        osg::Vec3d startPlaneNormal = upVector ^ planeNormal;
1218        startPlaneNormal.normalize();
1219        boundingPolytope.add( osg::Plane(startPlaneNormal, _startPoint) );
1220
1221        // set up the end cut off plane
1222        osg::Vec3d endPlaneNormal = planeNormal ^ upVector;
1223        endPlaneNormal.normalize();
1224        boundingPolytope.add( osg::Plane(endPlaneNormal, _endPoint) );
1225    }
1226
1227    osg::ref_ptr<osgUtil::PlaneIntersector> intersector = new osgUtil::PlaneIntersector(plane, boundingPolytope);
1228
1229
1230    intersector->setRecordHeightsAsAttributes(true);
1231    intersector->setEllipsoidModel(em);
1232
1233    _intersectionVisitor.reset();
1234    _intersectionVisitor.setTraversalMask(traversalMask);
1235    _intersectionVisitor.setIntersector( intersector.get() );
1236
1237    scene->accept(_intersectionVisitor);
1238
1239    osgUtil::PlaneIntersector::Intersections& intersections = intersector->getIntersections();
1240
1241    typedef osgUtil::PlaneIntersector::Intersection::Polyline Polyline;
1242    typedef osgUtil::PlaneIntersector::Intersection::Attributes Attributes;
1243
1244    if (!intersections.empty())
1245    {
1246
1247        osgUtil::PlaneIntersector::Intersections::iterator itr;
1248        for(itr = intersections.begin();
1249            itr != intersections.end();
1250            ++itr)
1251        {
1252            osgUtil::PlaneIntersector::Intersection& intersection = *itr;
1253
1254            if (intersection.matrix.valid())
1255            {
1256                // OSG_NOTICE<<"  transforming "<<std::endl;
1257                // transform points on polyline
1258                for(Polyline::iterator pitr = intersection.polyline.begin();
1259                    pitr != intersection.polyline.end();
1260                    ++pitr)
1261                {
1262                    *pitr = (*pitr) * (*intersection.matrix);
1263                }
1264
1265                // matrix no longer needed.
1266                intersection.matrix = 0;
1267            }
1268        }
1269
1270#if 0
1271        osg::ref_ptr<osg::Geode> geode = new osg::Geode;
1272
1273        for(itr = intersections.begin();
1274            itr != intersections.end();
1275            ++itr)
1276        {
1277
1278            osgUtil::PlaneIntersector::Intersection& intersection = *itr;
1279            osg::Geometry* geometry = new osg::Geometry;
1280
1281            osg::Vec3Array* vertices = new osg::Vec3Array;
1282            vertices->reserve(intersection.polyline.size());
1283            for(Polyline::iterator pitr = intersection.polyline.begin();
1284                pitr != intersection.polyline.end();
1285                ++pitr)
1286            {
1287                vertices->push_back(*pitr);
1288            }
1289
1290            geometry->setVertexArray( vertices );
1291            geometry->addPrimitiveSet( new osg::DrawArrays(GL_LINE_STRIP, 0, vertices->size()) );
1292
1293            osg::Vec4Array* colours = new osg::Vec4Array;
1294            colours->push_back( osg::Vec4(1.0f,1.0f,1.0f,1.0f) );
1295
1296            geometry->setColorArray( colours );
1297
1298            geode->addDrawable( geometry );
1299            geode->getOrCreateStateSet()->setMode( GL_LIGHTING, osg::StateAttribute::OFF );
1300        }
1301
1302        osgDB::writeNodeFile(*geode, "raw.osg");
1303#endif
1304
1305        ElevationSliceUtils::LineConstructor constructor;
1306        constructor._plane = plane;
1307        constructor._em = em;
1308
1309        if (em)
1310        {
1311            ElevationSliceUtils::DistanceHeightCalculator dhc(em, _startPoint, _endPoint);
1312
1313            // convert into distance/height
1314            for(itr = intersections.begin();
1315                itr != intersections.end();
1316                ++itr)
1317            {
1318                osgUtil::PlaneIntersector::Intersection& intersection = *itr;
1319
1320                if (intersection.attributes.size()!=intersection.polyline.size()) continue;
1321
1322                Attributes::iterator aitr = intersection.attributes.begin();
1323                for(Polyline::iterator pitr = intersection.polyline.begin();
1324                    pitr != intersection.polyline.end();
1325                    ++pitr, ++aitr)
1326                {
1327                    const osg::Vec3d& v = *pitr;
1328                    double distance, height;
1329                    dhc.computeDistanceHeight(v, distance, height);
1330
1331                    double pi_height = *aitr;
1332
1333                    // OSG_NOTICE<<"computed height = "<<height<<" PI height = "<<pi_height<<std::endl;
1334
1335                    constructor.add( distance, pi_height, v);
1336
1337                }
1338                constructor.endline();
1339            }
1340
1341
1342        }
1343        else
1344        {
1345            // convert into distance/height
1346            for(itr = intersections.begin();
1347                itr != intersections.end();
1348                ++itr)
1349            {
1350                osgUtil::PlaneIntersector::Intersection& intersection = *itr;
1351                for(Polyline::iterator pitr = intersection.polyline.begin();
1352                    pitr != intersection.polyline.end();
1353                    ++pitr)
1354                {
1355                    const osg::Vec3d& v = *pitr;
1356                    osg::Vec2d delta_xy( v.x() - _startPoint.x(), v.y() - _startPoint.y());
1357                    double distance = delta_xy.length();
1358
1359                    constructor.add( distance, v.z(), v);
1360                }
1361                constructor.endline();
1362            }
1363        }
1364
1365        // copy final results
1366        _intersections.clear();
1367        _distanceHeightIntersections.clear();
1368
1369        // constructor.report();
1370
1371        unsigned int numOverlapping = constructor.totalNumOverlapping();
1372
1373        while(numOverlapping>0)
1374        {
1375            unsigned int previousNumOverlapping = numOverlapping;
1376
1377            constructor.pruneOverlappingSegments();
1378            // constructor.report();
1379
1380            numOverlapping = constructor.totalNumOverlapping();
1381            if (previousNumOverlapping == numOverlapping) break;
1382        }
1383
1384        constructor.copyPoints(_intersections, _distanceHeightIntersections);
1385
1386#if 0
1387        {
1388            osg::ref_ptr<osg::Geode> geode = new osg::Geode;
1389
1390            osg::Geometry* geometry = new osg::Geometry;
1391
1392            osg::Vec3Array* vertices = new osg::Vec3Array;
1393            vertices->reserve(_intersections.size());
1394            for(Vec3dList::iterator pitr = _intersections.begin();
1395                pitr != _intersections.end();
1396                ++pitr)
1397            {
1398                vertices->push_back(*pitr);
1399            }
1400
1401            geometry->setVertexArray( vertices );
1402            geometry->addPrimitiveSet( new osg::DrawArrays(GL_LINE_STRIP, 0, _intersections.size()) );
1403
1404            osg::Vec4Array* colours = new osg::Vec4Array;
1405            colours->push_back( osg::Vec4(1.0f,0.5f,0.5f,1.0f) );
1406
1407            geometry->setColorArray( colours );
1408
1409            geode->addDrawable( geometry );
1410            geode->getOrCreateStateSet()->setMode( GL_LIGHTING, osg::StateAttribute::OFF );
1411
1412            osgDB::writeNodeFile(*geode, "processed.osg");
1413        }
1414#endif
1415
1416    }
1417    else
1418    {
1419        OSG_NOTICE<<"No intersections found."<<std::endl;
1420    }
1421
1422}
1423
1424ElevationSlice::Vec3dList ElevationSlice::computeElevationSlice(osg::Node* scene, const osg::Vec3d& startPoint, const osg::Vec3d& endPoint, osg::Node::NodeMask traversalMask)
1425{
1426    ElevationSlice es;
1427    es.setStartPoint(startPoint);
1428    es.setEndPoint(endPoint);
1429    es.computeIntersections(scene, traversalMask);
1430    return es.getIntersections();
1431}
1432
1433void ElevationSlice::setDatabaseCacheReadCallback(DatabaseCacheReadCallback* dcrc)
1434{
1435    _dcrc = dcrc;
1436    _intersectionVisitor.setReadCallback(dcrc);
1437}
Note: See TracBrowser for help on using the browser.