root/OpenSceneGraph/trunk/src/osgUtil/LineSegmentIntersector.cpp @ 12237

Revision 12237, 17.9 kB (checked in by robert, 4 years ago)

From Farshid Lashkari, "Another update. I added a LIMIT_NEAREST enum which implements your previous suggestion of rejecting bounding volumes further from the nearest existing intersection. I only implemented this for LineSegmentIntersector?. I'd appreciate it if you could double check the math I added to LineSegmentIntersector::intersects() for checking if the bounding sphere is further away. The results of this are promising. I'm getting noticeable performance increase for line intersections with scenes containing many drawables.
"

  • 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
15#include <osgUtil/LineSegmentIntersector>
16
17#include <osg/Geometry>
18#include <osg/Notify>
19#include <osg/io_utils>
20#include <osg/TriangleFunctor>
21#include <osg/KdTree>
22#include <osg/Timer>
23
24using namespace osgUtil;
25
26namespace LineSegmentIntersectorUtils
27{
28
29    struct TriangleIntersection
30    {
31        TriangleIntersection(unsigned int index, const osg::Vec3& normal, float r1, const osg::Vec3* v1, float r2, const osg::Vec3* v2, float r3, const osg::Vec3* v3):
32            _index(index),
33            _normal(normal),
34            _r1(r1),
35            _v1(v1),
36            _r2(r2),
37            _v2(v2),
38            _r3(r3),
39            _v3(v3) {}
40
41        unsigned int        _index;
42        const osg::Vec3     _normal;
43        float               _r1;
44        const osg::Vec3*    _v1;
45        float               _r2;
46        const osg::Vec3*    _v2;
47        float               _r3;
48        const osg::Vec3*    _v3;
49
50    protected:
51
52        TriangleIntersection& operator = (const TriangleIntersection&) { return *this; }
53    };
54
55    typedef std::multimap<float,TriangleIntersection> TriangleIntersections;
56
57    struct TriangleIntersector
58    {
59        osg::Vec3   _s;
60        osg::Vec3   _d;
61        float       _length;
62
63        int         _index;
64        float       _ratio;
65        bool        _hit;
66        bool        _limitOneIntersection;
67
68        TriangleIntersections _intersections;
69
70        TriangleIntersector()
71        {
72            _length = 0.0f;
73            _index = 0;
74            _ratio = 0.0f;
75            _hit = false;
76            _limitOneIntersection = false;
77        }
78
79        void set(const osg::Vec3d& start, osg::Vec3d& end, float ratio=FLT_MAX)
80        {
81            _hit=false;
82            _index = 0;
83            _ratio = ratio;
84
85            _s = start;
86            _d = end - start;
87            _length = _d.length();
88            _d /= _length;
89        }
90
91        inline void operator () (const osg::Vec3& v1,const osg::Vec3& v2,const osg::Vec3& v3, bool treatVertexDataAsTemporary)
92        {
93            ++_index;
94
95            if (_limitOneIntersection && _hit) return;
96
97            if (v1==v2 || v2==v3 || v1==v3) return;
98
99            osg::Vec3 v12 = v2-v1;
100            osg::Vec3 n12 = v12^_d;
101            float ds12 = (_s-v1)*n12;
102            float d312 = (v3-v1)*n12;
103            if (d312>=0.0f)
104            {
105                if (ds12<0.0f) return;
106                if (ds12>d312) return;
107            }
108            else                     // d312 < 0
109            {
110                if (ds12>0.0f) return;
111                if (ds12<d312) return;
112            }
113
114            osg::Vec3 v23 = v3-v2;
115            osg::Vec3 n23 = v23^_d;
116            float ds23 = (_s-v2)*n23;
117            float d123 = (v1-v2)*n23;
118            if (d123>=0.0f)
119            {
120                if (ds23<0.0f) return;
121                if (ds23>d123) return;
122            }
123            else                     // d123 < 0
124            {
125                if (ds23>0.0f) return;
126                if (ds23<d123) return;
127            }
128
129            osg::Vec3 v31 = v1-v3;
130            osg::Vec3 n31 = v31^_d;
131            float ds31 = (_s-v3)*n31;
132            float d231 = (v2-v3)*n31;
133            if (d231>=0.0f)
134            {
135                if (ds31<0.0f) return;
136                if (ds31>d231) return;
137            }
138            else                     // d231 < 0
139            {
140                if (ds31>0.0f) return;
141                if (ds31<d231) return;
142            }
143
144
145            float r3;
146            if (ds12==0.0f) r3=0.0f;
147            else if (d312!=0.0f) r3 = ds12/d312;
148            else return; // the triangle and the line must be parallel intersection.
149
150            float r1;
151            if (ds23==0.0f) r1=0.0f;
152            else if (d123!=0.0f) r1 = ds23/d123;
153            else return; // the triangle and the line must be parallel intersection.
154
155            float r2;
156            if (ds31==0.0f) r2=0.0f;
157            else if (d231!=0.0f) r2 = ds31/d231;
158            else return; // the triangle and the line must be parallel intersection.
159
160            float total_r = (r1+r2+r3);
161            if (total_r!=1.0f)
162            {
163                if (total_r==0.0f) return; // the triangle and the line must be parallel intersection.
164                float inv_total_r = 1.0f/total_r;
165                r1 *= inv_total_r;
166                r2 *= inv_total_r;
167                r3 *= inv_total_r;
168            }
169
170            osg::Vec3 in = v1*r1+v2*r2+v3*r3;
171            if (!in.valid())
172            {
173                OSG_WARN<<"Warning:: Picked up error in TriangleIntersect"<<std::endl;
174                OSG_WARN<<"   ("<<v1<<",\t"<<v2<<",\t"<<v3<<")"<<std::endl;
175                OSG_WARN<<"   ("<<r1<<",\t"<<r2<<",\t"<<r3<<")"<<std::endl;
176                return;
177            }
178
179            float d = (in-_s)*_d;
180
181            if (d<0.0f) return;
182            if (d>_length) return;
183
184            osg::Vec3 normal = v12^v23;
185            normal.normalize();
186
187            float r = d/_length;
188
189
190            if (treatVertexDataAsTemporary)
191            {
192                _intersections.insert(std::pair<const float,TriangleIntersection>(r,TriangleIntersection(_index-1,normal,r1,0,r2,0,r3,0)));
193            }
194            else
195            {
196                _intersections.insert(std::pair<const float,TriangleIntersection>(r,TriangleIntersection(_index-1,normal,r1,&v1,r2,&v2,r3,&v3)));
197            }
198            _hit = true;
199
200        }
201
202    };
203
204}
205
206///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
207//
208//  LineSegmentIntersector
209//
210
211LineSegmentIntersector::LineSegmentIntersector(const osg::Vec3d& start, const osg::Vec3d& end):
212    _parent(0),
213    _start(start),
214    _end(end)
215{
216}
217
218LineSegmentIntersector::LineSegmentIntersector(CoordinateFrame cf, const osg::Vec3d& start, const osg::Vec3d& end):
219    Intersector(cf),
220    _parent(0),
221    _start(start),
222    _end(end)
223{
224}
225
226LineSegmentIntersector::LineSegmentIntersector(CoordinateFrame cf, double x, double y):
227    Intersector(cf),
228    _parent(0)
229{
230    switch(cf)
231    {
232        case WINDOW : _start.set(x,y,0.0); _end.set(x,y,1.0); break;
233        case PROJECTION : _start.set(x,y,-1.0); _end.set(x,y,1.0); break;
234        case VIEW : _start.set(x,y,0.0); _end.set(x,y,1.0); break;
235        case MODEL : _start.set(x,y,0.0); _end.set(x,y,1.0); break;
236    }
237}
238
239Intersector* LineSegmentIntersector::clone(osgUtil::IntersectionVisitor& iv)
240{
241    if (_coordinateFrame==MODEL && iv.getModelMatrix()==0)
242    {
243        osg::ref_ptr<LineSegmentIntersector> lsi = new LineSegmentIntersector(_start, _end);
244        lsi->_parent = this;
245        lsi->_intersectionLimit = this->_intersectionLimit;
246        return lsi.release();
247    }
248
249    // compute the matrix that takes this Intersector from its CoordinateFrame into the local MODEL coordinate frame
250    // that geometry in the scene graph will always be in.
251    osg::Matrix matrix;
252    switch (_coordinateFrame)
253    {
254        case(WINDOW):
255            if (iv.getWindowMatrix()) matrix.preMult( *iv.getWindowMatrix() );
256            if (iv.getProjectionMatrix()) matrix.preMult( *iv.getProjectionMatrix() );
257            if (iv.getViewMatrix()) matrix.preMult( *iv.getViewMatrix() );
258            if (iv.getModelMatrix()) matrix.preMult( *iv.getModelMatrix() );
259            break;
260        case(PROJECTION):
261            if (iv.getProjectionMatrix()) matrix.preMult( *iv.getProjectionMatrix() );
262            if (iv.getViewMatrix()) matrix.preMult( *iv.getViewMatrix() );
263            if (iv.getModelMatrix()) matrix.preMult( *iv.getModelMatrix() );
264            break;
265        case(VIEW):
266            if (iv.getViewMatrix()) matrix.preMult( *iv.getViewMatrix() );
267            if (iv.getModelMatrix()) matrix.preMult( *iv.getModelMatrix() );
268            break;
269        case(MODEL):
270            if (iv.getModelMatrix()) matrix = *iv.getModelMatrix();
271            break;
272    }
273
274    osg::Matrix inverse;
275    inverse.invert(matrix);
276
277    osg::ref_ptr<LineSegmentIntersector> lsi = new LineSegmentIntersector(_start * inverse, _end * inverse);
278    lsi->_parent = this;
279    lsi->_intersectionLimit = this->_intersectionLimit;
280    return lsi.release();
281}
282
283bool LineSegmentIntersector::enter(const osg::Node& node)
284{
285    if (reachedLimit()) return false;
286    return !node.isCullingActive() || intersects( node.getBound() );
287}
288
289void LineSegmentIntersector::leave()
290{
291    // do nothing
292}
293
294void LineSegmentIntersector::intersect(osgUtil::IntersectionVisitor& iv, osg::Drawable* drawable)
295{
296    if (reachedLimit()) return;
297
298    osg::Vec3d s(_start), e(_end);
299    if ( !intersectAndClip( s, e, drawable->getBound() ) ) return;
300
301    if (iv.getDoDummyTraversal()) return;
302
303    osg::KdTree* kdTree = iv.getUseKdTreeWhenAvailable() ? dynamic_cast<osg::KdTree*>(drawable->getShape()) : 0;
304    if (kdTree)
305    {
306        osg::KdTree::LineSegmentIntersections intersections;
307        intersections.reserve(4);
308        if (kdTree->intersect(s,e,intersections))
309        {
310            // OSG_NOTICE<<"Got KdTree intersections"<<std::endl;
311            for(osg::KdTree::LineSegmentIntersections::iterator itr = intersections.begin();
312                itr != intersections.end();
313                ++itr)
314            {
315                osg::KdTree::LineSegmentIntersection& lsi = *(itr);
316
317                // get ratio in s,e range
318                double ratio = lsi.ratio;
319
320                // remap ratio into _start, _end range
321                double remap_ratio = ((s-_start).length() + ratio * (e-s).length() )/(_end-_start).length();
322
323
324                Intersection hit;
325                hit.ratio = remap_ratio;
326                hit.matrix = iv.getModelMatrix();
327                hit.nodePath = iv.getNodePath();
328                hit.drawable = drawable;
329                hit.primitiveIndex = lsi.primitiveIndex;
330
331                hit.localIntersectionPoint = _start*(1.0-remap_ratio) + _end*remap_ratio;
332
333                // OSG_NOTICE<<"KdTree: ratio="<<hit.ratio<<" ("<<hit.localIntersectionPoint<<")"<<std::endl;
334
335                hit.localIntersectionNormal = lsi.intersectionNormal;
336
337                hit.indexList.reserve(3);
338                hit.ratioList.reserve(3);
339                if (lsi.r0!=0.0f)
340                {
341                    hit.indexList.push_back(lsi.p0);
342                    hit.ratioList.push_back(lsi.r0);
343                }
344
345                if (lsi.r1!=0.0f)
346                {
347                    hit.indexList.push_back(lsi.p1);
348                    hit.ratioList.push_back(lsi.r1);
349                }
350
351                if (lsi.r2!=0.0f)
352                {
353                    hit.indexList.push_back(lsi.p2);
354                    hit.ratioList.push_back(lsi.r2);
355                }
356
357                insertIntersection(hit);
358            }
359        }
360
361        return;
362    }
363
364    osg::TriangleFunctor<LineSegmentIntersectorUtils::TriangleIntersector> ti;
365    ti.set(s,e);
366    ti._limitOneIntersection = (_intersectionLimit == LIMIT_ONE_PER_DRAWABLE || _intersectionLimit == LIMIT_ONE);
367    drawable->accept(ti);
368
369    if (ti._hit)
370    {
371        osg::Geometry* geometry = drawable->asGeometry();
372
373        for(LineSegmentIntersectorUtils::TriangleIntersections::iterator thitr = ti._intersections.begin();
374            thitr != ti._intersections.end();
375            ++thitr)
376        {
377
378            // get ratio in s,e range
379            double ratio = thitr->first;
380
381            // remap ratio into _start, _end range
382            double remap_ratio = ((s-_start).length() + ratio * (e-s).length() )/(_end-_start).length();
383
384            if ( _intersectionLimit == LIMIT_NEAREST && !getIntersections().empty() )
385            {
386                if (remap_ratio >= getIntersections().begin()->ratio )
387                    break;
388                else
389                    getIntersections().clear();
390            }
391
392            LineSegmentIntersectorUtils::TriangleIntersection& triHit = thitr->second;
393
394            Intersection hit;
395            hit.ratio = remap_ratio;
396            hit.matrix = iv.getModelMatrix();
397            hit.nodePath = iv.getNodePath();
398            hit.drawable = drawable;
399            hit.primitiveIndex = triHit._index;
400
401            hit.localIntersectionPoint = _start*(1.0-remap_ratio) + _end*remap_ratio;
402
403            // OSG_NOTICE<<"Conventional: ratio="<<hit.ratio<<" ("<<hit.localIntersectionPoint<<")"<<std::endl;
404
405            hit.localIntersectionNormal = triHit._normal;
406
407            if (geometry)
408            {
409                osg::Vec3Array* vertices = dynamic_cast<osg::Vec3Array*>(geometry->getVertexArray());
410                if (vertices)
411                {
412                    osg::Vec3* first = &(vertices->front());
413                    if (triHit._v1)
414                    {
415                        hit.indexList.push_back(triHit._v1-first);
416                        hit.ratioList.push_back(triHit._r1);
417                    }
418                    if (triHit._v2)
419                    {
420                        hit.indexList.push_back(triHit._v2-first);
421                        hit.ratioList.push_back(triHit._r2);
422                    }
423                    if (triHit._v3)
424                    {
425                        hit.indexList.push_back(triHit._v3-first);
426                        hit.ratioList.push_back(triHit._r3);
427                    }
428                }
429            }
430
431            insertIntersection(hit);
432
433        }
434    }
435}
436
437void LineSegmentIntersector::reset()
438{
439    Intersector::reset();
440
441    _intersections.clear();
442}
443
444bool LineSegmentIntersector::intersects(const osg::BoundingSphere& bs)
445{
446    // if bs not valid then return true based on the assumption that an invalid sphere is yet to be defined.
447    if (!bs.valid()) return true;
448
449    osg::Vec3d sm = _start - bs._center;
450    double c = sm.length2()-bs._radius*bs._radius;
451    if (c<0.0) return true;
452
453    osg::Vec3d se = _end-_start;
454    double a = se.length2();
455    double b = (sm*se)*2.0;
456    double d = b*b-4.0*a*c;
457
458    if (d<0.0) return false;
459
460    d = sqrt(d);
461
462    double div = 1.0/(2.0*a);
463
464    double r1 = (-b-d)*div;
465    double r2 = (-b+d)*div;
466
467    if (r1<=0.0 && r2<=0.0) return false;
468
469    if (r1>=1.0 && r2>=1.0) return false;
470
471    if (_intersectionLimit == LIMIT_NEAREST && !getIntersections().empty())
472    {
473        double ratio = (sm.length() - bs._radius) / sqrt(a);
474        if (ratio >= getIntersections().begin()->ratio) return false;
475    }
476
477    // passed all the rejection tests so line must intersect bounding sphere, return true.
478    return true;
479}
480
481bool LineSegmentIntersector::intersectAndClip(osg::Vec3d& s, osg::Vec3d& e,const osg::BoundingBox& bbInput)
482{
483    osg::Vec3d bb_min(bbInput._min);
484    osg::Vec3d bb_max(bbInput._max);
485
486#if 1
487    double epsilon = 1e-4;
488    bb_min.x() -= epsilon;
489    bb_min.y() -= epsilon;
490    bb_min.z() -= epsilon;
491    bb_max.x() += epsilon;
492    bb_max.y() += epsilon;
493    bb_max.z() += epsilon;
494#endif
495
496    // compate s and e against the xMin to xMax range of bb.
497    if (s.x()<=e.x())
498    {
499
500        // trivial reject of segment wholely outside.
501        if (e.x()<bb_min.x()) return false;
502        if (s.x()>bb_max.x()) return false;
503
504        if (s.x()<bb_min.x())
505        {
506            // clip s to xMin.
507            s = s+(e-s)*(bb_min.x()-s.x())/(e.x()-s.x());
508        }
509
510        if (e.x()>bb_max.x())
511        {
512            // clip e to xMax.
513            e = s+(e-s)*(bb_max.x()-s.x())/(e.x()-s.x());
514        }
515    }
516    else
517    {
518        if (s.x()<bb_min.x()) return false;
519        if (e.x()>bb_max.x()) return false;
520
521        if (e.x()<bb_min.x())
522        {
523            // clip s to xMin.
524            e = s+(e-s)*(bb_min.x()-s.x())/(e.x()-s.x());
525        }
526
527        if (s.x()>bb_max.x())
528        {
529            // clip e to xMax.
530            s = s+(e-s)*(bb_max.x()-s.x())/(e.x()-s.x());
531        }
532    }
533
534    // compate s and e against the yMin to yMax range of bb.
535    if (s.y()<=e.y())
536    {
537
538        // trivial reject of segment wholely outside.
539        if (e.y()<bb_min.y()) return false;
540        if (s.y()>bb_max.y()) return false;
541
542        if (s.y()<bb_min.y())
543        {
544            // clip s to yMin.
545            s = s+(e-s)*(bb_min.y()-s.y())/(e.y()-s.y());
546        }
547
548        if (e.y()>bb_max.y())
549        {
550            // clip e to yMax.
551            e = s+(e-s)*(bb_max.y()-s.y())/(e.y()-s.y());
552        }
553    }
554    else
555    {
556        if (s.y()<bb_min.y()) return false;
557        if (e.y()>bb_max.y()) return false;
558
559        if (e.y()<bb_min.y())
560        {
561            // clip s to yMin.
562            e = s+(e-s)*(bb_min.y()-s.y())/(e.y()-s.y());
563        }
564
565        if (s.y()>bb_max.y())
566        {
567            // clip e to yMax.
568            s = s+(e-s)*(bb_max.y()-s.y())/(e.y()-s.y());
569        }
570    }
571
572    // compate s and e against the zMin to zMax range of bb.
573    if (s.z()<=e.z())
574    {
575
576        // trivial reject of segment wholely outside.
577        if (e.z()<bb_min.z()) return false;
578        if (s.z()>bb_max.z()) return false;
579
580        if (s.z()<bb_min.z())
581        {
582            // clip s to zMin.
583            s = s+(e-s)*(bb_min.z()-s.z())/(e.z()-s.z());
584        }
585
586        if (e.z()>bb_max.z())
587        {
588            // clip e to zMax.
589            e = s+(e-s)*(bb_max.z()-s.z())/(e.z()-s.z());
590        }
591    }
592    else
593    {
594        if (s.z()<bb_min.z()) return false;
595        if (e.z()>bb_max.z()) return false;
596
597        if (e.z()<bb_min.z())
598        {
599            // clip s to zMin.
600            e = s+(e-s)*(bb_min.z()-s.z())/(e.z()-s.z());
601        }
602
603        if (s.z()>bb_max.z())
604        {
605            // clip e to zMax.
606            s = s+(e-s)*(bb_max.z()-s.z())/(e.z()-s.z());
607        }
608    }
609
610    // OSG_NOTICE<<"clampped segment "<<s<<" "<<e<<std::endl;
611
612    // if (s==e) return false;
613
614    return true;
615}
Note: See TracBrowser for help on using the browser.