root/OpenSceneGraph/trunk/src/osgShadow/ConvexPolyhedron.cpp @ 9475

Revision 9475, 60.9 kB (checked in by robert, 5 years ago)

From Andy Skinner, fixes for Solaris build

RevLine 
[8984]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 * ViewDependentShadow codes Copyright (C) 2008 Wojciech Lewandowski
14 * Thanks to to my company http://www.ai.com.pl for allowing me free this work.
15*/
16
17#include<osgShadow/ConvexPolyhedron>
18#include<osg/Group>
19#include<osg/Geode>
20#include<osgDB/WriteFile>
21
[9009]22#include<cassert>
23#include<deque>
24#include<algorithm>
25
[9475]26#include <stdio.h>
[9343]27#include <string.h>
[9009]28
[8984]29using namespace osgShadow;
30
31
32#if defined( DEBUG ) || defined( _DEBUG ) || defined( _DEBUG_ )
33
34// #define MAKE_CHECKS 0
35
36#endif
37
38#if MAKE_CHECKS
39
40inline std::ostream & DEBUG_NOTIFY( void )
41{
42    return osg::notify( osg::WARN );
43}
44#define WARN  DEBUG_NOTIFY()
45
46#define CONVEX_POLYHEDRON_CHECK_COHERENCY             1
47#define CONVEX_POLYHEDRON_WARN_ON_INCOHERENT_DATA     2
48#define CONVEX_POLYHEDRON_DUMP_ON_INCOHERENT_DATA     2
49#define CONVEX_POLYHEDRON_WARN_ON_BAD_POLYGON         1
50#define CONVEX_POLYHEDRON_DUMP_ON_BAD_POLYGON         1
51#define CONVEX_POLYHEDRON_WARN_ON_CONCAVE_POLYGON     1
52#define CONVEX_POLYHEDRON_DUMP_ON_CONCAVE_POLYGON     1
53#define CONVEX_POLYHEDRON_WARN_ON_INCORRECT_FACE_CUT  1
54#define CONVEX_POLYHEDRON_DUMP_ON_INCORRECT_FACE_CUT  1
55
56
57#else
58
59#define WARN osg::notify( osg::WARN )
60
61#define CONVEX_POLYHEDRON_CHECK_COHERENCY             0
62#define CONVEX_POLYHEDRON_WARN_ON_INCOHERENT_DATA     0
63#define CONVEX_POLYHEDRON_DUMP_ON_INCOHERENT_DATA     0
64#define CONVEX_POLYHEDRON_WARN_ON_BAD_POLYGON         0
65#define CONVEX_POLYHEDRON_DUMP_ON_BAD_POLYGON         0
66#define CONVEX_POLYHEDRON_WARN_ON_CONCAVE_POLYGON     0
67#define CONVEX_POLYHEDRON_DUMP_ON_CONCAVE_POLYGON     0
68#define CONVEX_POLYHEDRON_WARN_ON_INCORRECT_FACE_CUT  0
69#define CONVEX_POLYHEDRON_DUMP_ON_INCORRECT_FACE_CUT  0
70
71#endif
72
73const osg::Matrix & ConvexPolyhedron::defaultMatrix = *(osg::Matrix*)NULL;
74static const double epsi = pow( 2.0, -20.0 ); //circa 8 times float prec(~2^-23)
75static const double plane_hull_tolerance    = 1.0e-5;
76static const double point_plane_tolerance   = 0;
77static const double point_point_tolerance   = 0;
78static const double point_point_equivalence = 0;
79
80// Tim Moore modifications for GCC 4.3 August 15, 2008
81// they correspond to Adrian Egli tweaks for VS 7.3 on August 19, 2008
82namespace
83{
84typedef std::vector< double > Distances;
85typedef std::vector< osg::Vec4d > Points;
86
87// Auxilliary params contined per face
88struct FaceDistances
89{
90    ConvexPolyhedron::Faces::iterator itr;
91    Points points;
92    Distances distances;
93    int below, above, on;
94};
95} // namespace
96
97ConvexPolyhedron::ConvexPolyhedron( const osg::Matrix& matrix,
98                        const osg::Matrix& inverse,
99                        const osg::BoundingBox& bb )
100{
101    setToBoundingBox( bb );
102
103    if( &matrix != &defaultMatrix && &inverse != &defaultMatrix )
104        transform( matrix, inverse );
105    else if( &matrix != &defaultMatrix && &inverse == &defaultMatrix )
106        transform( matrix, osg::Matrix::inverse( matrix ) );
107    else if( &matrix == &defaultMatrix && &inverse != &defaultMatrix )
108        transform( osg::Matrix::inverse( inverse ), matrix );
109}
110
111void ConvexPolyhedron::setToUnitFrustum(bool withNear, bool withFar)
112{
113    const osg::Vec3d v000(-1.0,-1.0,-1.0);
114    const osg::Vec3d v010(-1.0,1.0,-1.0);
115    const osg::Vec3d v001(-1.0,-1.0,1.0);
116    const osg::Vec3d v011(-1.0,1.0,1.0);
117    const osg::Vec3d v100(1.0,-1.0,-1.0);
118    const osg::Vec3d v110(1.0,1.0,-1.0);
119    const osg::Vec3d v101(1.0,-1.0,1.0);
120    const osg::Vec3d v111(1.0,1.0,1.0);
121
122    _faces.clear();
123
124    {   // left plane.
125        Face& face = createFace();
126        face.name = "left";
127        face.plane.set(1.0,0.0,0.0,1.0);
128        face.vertices.push_back(v000);
129        face.vertices.push_back(v001);
130        face.vertices.push_back(v011);
131        face.vertices.push_back(v010);
132    }
133
134    {   // right plane.
135        Face& face = createFace();
136        face.name = "right";
137        face.plane.set(-1.0,0.0,0.0,1.0);
138        face.vertices.push_back(v100);
139        face.vertices.push_back(v110);
140        face.vertices.push_back(v111);
141        face.vertices.push_back(v101);
142    }
143
144    {   // bottom plane.
145        Face& face = createFace();
146        face.name = "bottom";
147        face.plane.set(0.0,1.0,0.0,1.0);
148        face.vertices.push_back(v000);
149        face.vertices.push_back(v100);
150        face.vertices.push_back(v101);
151        face.vertices.push_back(v001);
152    }
153
154    {   // top plane.
155        Face& face = createFace();
156        face.name = "top";
157        face.plane.set(0.0,-1.0,0.0,1.0);
158        face.vertices.push_back(v010);
159        face.vertices.push_back(v011);
160        face.vertices.push_back(v111);
161        face.vertices.push_back(v110);
162    }
163
164    if (withNear)
165    {   // near plane
166        Face& face = createFace();
167        face.name = "near";
168        face.plane.set(0.0,0.0,1.0,1.0);
169        face.vertices.push_back(v000);
170        face.vertices.push_back(v010);
171        face.vertices.push_back(v110);
172        face.vertices.push_back(v100);
173    }
174
175    if (withFar)
176    {   // far plane
177        Face& face = createFace();
178        face.name = "far";
179        face.plane.set(0.0,0.0,-1.0,1.0);
180        face.vertices.push_back(v001);
181        face.vertices.push_back(v101);
182        face.vertices.push_back(v111);
183        face.vertices.push_back(v011);
184    }
185}
186
187void ConvexPolyhedron::setToBoundingBox(const osg::BoundingBox& bb)
188{
189    _faces.clear();
190
191    // Ignore invalid and one dimensional boxes
192    if( bb._min[0] >= bb._max[0] ||
193        bb._min[1] >= bb._max[1] ||
194        bb._min[2] >= bb._max[2] ) return;
195
196    const osg::Vec3d v000(bb.xMin(),bb.yMin(), bb.zMin());
197    const osg::Vec3d v010(bb.xMin(),bb.yMax(), bb.zMin());
198    const osg::Vec3d v001(bb.xMin(),bb.yMin(), bb.zMax());
199    const osg::Vec3d v011(bb.xMin(),bb.yMax(), bb.zMax());
200    const osg::Vec3d v100(bb.xMax(),bb.yMin(), bb.zMin());
201    const osg::Vec3d v110(bb.xMax(),bb.yMax(), bb.zMin());
202    const osg::Vec3d v101(bb.xMax(),bb.yMin(), bb.zMax());
203    const osg::Vec3d v111(bb.xMax(),bb.yMax(), bb.zMax());
204
205
206
207    {   // x min plane
208        Face& face = createFace();
209        face.name = "xMin";
210        face.plane.set(1.0,0.0,0.0,-bb.xMin());
211        face.vertices.push_back(v000);
212        face.vertices.push_back(v001);
213        face.vertices.push_back(v011);
214        face.vertices.push_back(v010);
215    }
216
217    {   // x max plane.
218        Face& face = createFace();
219        face.name = "xMax";
220        face.plane.set(-1.0,0.0,0.0,bb.xMax());
221        face.vertices.push_back(v100);
222        face.vertices.push_back(v110);
223        face.vertices.push_back(v111);
224        face.vertices.push_back(v101);
225    }
226
227    {   // y min plane.
228        Face& face = createFace();
229        face.name = "yMin";
230        face.plane.set(0.0,1.0,0.0,-bb.yMin());
231        face.vertices.push_back(v000);
232        face.vertices.push_back(v100);
233        face.vertices.push_back(v101);
234        face.vertices.push_back(v001);
235    }
236
237    {   // y max plane.
238        Face& face = createFace();
239        face.name = "yMax";
240        face.plane.set(0.0,-1.0,0.0,bb.yMax());
241        face.vertices.push_back(v010);
242        face.vertices.push_back(v011);
243        face.vertices.push_back(v111);
244        face.vertices.push_back(v110);
245    }
246    {   // z min plane
247        Face& face = createFace();
248        face.name = "zMin";
249        face.plane.set(0.0,0.0,1.0,-bb.zMin());
250        face.vertices.push_back(v000);
251        face.vertices.push_back(v010);
252        face.vertices.push_back(v110);
253        face.vertices.push_back(v100);
254    }
255
256    {   // z max plane
257        Face& face = createFace();
258        face.name = "zMax";
259        face.plane.set(0.0,0.0,-1.0,bb.zMax());
260        face.vertices.push_back(v001);
261        face.vertices.push_back(v101);
262        face.vertices.push_back(v111);
263        face.vertices.push_back(v011);
264    }
265}
266
267void ConvexPolyhedron::transform(const osg::Matrix& matrix, const osg::Matrix& inverse)
268{
269    bool requires_infinite_plane_clip = false;
270
271    ConvexPolyhedron cp = *this;
272    for(Faces::iterator itr = _faces.begin();
273        itr != _faces.end() && !requires_infinite_plane_clip;
274        ++itr)
275    {
276        Face& face = *itr;
277        face.plane.transformProvidingInverse(inverse);
278        for(Vertices::iterator vitr = face.vertices.begin();
279            vitr != face.vertices.end();
280            ++vitr)
281        {
282            osg::Vec4d v( *vitr, 1.0 );
283            v = v * matrix;
284
285            if( v[3] <= 0 ) {
286                requires_infinite_plane_clip = true;
287                break;
288            }
289
290            vitr->set( v[0]/v[3], v[1]/v[3], v[2]/v[3] );
291        }
292    }
293
294    if( requires_infinite_plane_clip ) {
295        *this = cp;
296        transformClip( matrix, inverse );
297//        cp.dumpGeometry(&cp._faces.back(),&cp._faces.back().plane,this);
298    }
299
300    // Perpective transforms and lack of precision
301    // occasionaly cause removal of some points
302
303    removeDuplicateVertices( );
304
305    checkCoherency( true, "ConvexPolyhedron::transform" );
306}
307
308void ConvexPolyhedron::transformClip(const osg::Matrix& matrix, const osg::Matrix& inverse)
309{
310    double tolerance = 0.00001;
311
312    if( _faces.empty() ) return;
313
314
315    ConvexPolyhedron cp( *this );
316
317    typedef std::vector< FaceDistances > FaceDistancesList;
318    FaceDistancesList faceDistances;
319    faceDistances.resize( _faces.size() );
320
321    double min = FLT_MAX, max = -FLT_MAX; //Hull max & min point distances
322
323    FaceDistancesList::iterator fd = faceDistances.begin();
324    // First step compute each face points distances to cutting plane
325    for( Faces::iterator itr = _faces.begin();
326        itr != _faces.end();
327        ++itr, ++fd )
328    {
329        fd->itr = itr;
330        fd->distances.reserve( itr->vertices.size() );
331        fd->on = 0;
332        fd->above = 0;
333        fd->below = 0;
334
335        itr->plane.transformProvidingInverse(inverse);
336
337        for( Vertices::iterator vitr = itr->vertices.begin();
338            vitr != itr->vertices.end();
339            ++vitr)
340        {
341            osg::Vec4d p( *vitr, 1.0 );
342            p = p * matrix;
343            vitr->set( p[0]/p[3], p[1]/p[3], p[2]/p[3] );
344
345            double d = p[3]-tolerance;
346
347            fd->points.push_back( p );
348            fd->distances.push_back( d );
349            if ( d>point_plane_tolerance )       ++fd->above;
350            else if ( d<-point_plane_tolerance ) ++fd->below;
351            else                                 ++fd->on;
352            min = osg::minimum( min, d );
353            max = osg::maximum( max, d );
354        }
355    }
356
357    if( max <= plane_hull_tolerance ) { // All points on or below cutting plane
358        _faces.clear();
359        return;
360    }
361
362    if( min >= -plane_hull_tolerance ) { // All points on or above cutting plane
363        return;
364    }
365
366    typedef std::pair<osg::Vec3d, osg::Vec3d> Edge;
367    typedef std::set< Edge > Edges;
368    Edges edges;
369
370    for( FaceDistancesList::iterator fd = faceDistances.begin();
371        fd != faceDistances.end();
372        ++fd )
373    {
374        if ( fd->below == 0 )
375        {  // skip face if all points on or above cutting plane ( below == 0 )
376            continue;
377        }
378
379        if ( /* fd->below > 0 && */ fd->above == 0 && fd->on == 0 )
380        {
381            _faces.erase( fd->itr ); // remove face if points below or on
382            continue;
383        }
384
385        // cut the face if some points above and below plane
386        // assert( fd->below > 0 && fd->above > 0 );
387
388        Face& face = *(fd->itr);
389        Points& vertices = fd->points;
390        Distances& distances = fd->distances;
391        Vertices newFaceVertices;
392        Vertices newVertices;
393
394        for(unsigned int i=0; i < vertices.size(); ++i)
395        {
396            osg::Vec4d &va = vertices[i];
397            osg::Vec4d &vb = vertices[(i+1)%vertices.size()];
398            double &distance_a = distances[i];
399            double &distance_b = distances[(i+1)%vertices.size()];
400
401            // Is first edge point above or on the plane?
402            if ( -point_plane_tolerance <= distance_a ) {
403
404                osg::Vec3d v( vertices[i][0], vertices[i][1], vertices[i][2] );
405                v /= vertices[i][3];
406
407                if( newVertices.empty() || v != newVertices.back() )
408                    newVertices.push_back( v );
409
410                if ( distance_a <= point_plane_tolerance ) {
411                    if( newFaceVertices.empty() || v != newFaceVertices.back() )
412                        newFaceVertices.push_back( v );
413                }
414            }
415
416            // Does edge intersect plane ?
417            if ( ( distance_a < -point_plane_tolerance && distance_b > point_plane_tolerance ) ||
418                 ( distance_b < -point_plane_tolerance && distance_a > point_plane_tolerance ) )
419            {
420                osg::Vec4d intersection; // Inserting vertex
421                double da = fabs( distance_a ), db = fabs( distance_b );
422
423                // tweaks to improve coherency of polytope after cut
424                if( da <= point_point_equivalence && da <= db ) {
425                    intersection = va;
426                } else if( db <= point_point_equivalence && db <= da ) {
427                    intersection = vb;
428                } else {
429                    double dab4 = 0.25 * ( da + db );
430                    if( dab4 < da && dab4 < db ) {
431                        intersection = (vb*distance_a - va*distance_b)/(distance_a-distance_b);
432                    } else {
433                        osg::Vec4d v = (vb - va)/(distance_a-distance_b);
434                        if( da < db )
435                            intersection = va + v * distance_a;
436                        else
437                            intersection = vb + v * distance_b;
438                    }
439                }
440
441
442                osg::Vec3d v( intersection[0], intersection[1], intersection[2] );
443                v /= intersection[3];
444
445                if( newVertices.empty() || v != newVertices.back() )
446                    newVertices.push_back( v );
447
448                if( newFaceVertices.empty() || v != newFaceVertices.back() )
449                    newFaceVertices.push_back( v );
450            }
451        }
452
453        if( newVertices.size() && newVertices.front() == newVertices.back() )
454            newVertices.pop_back();
455
456        if( newFaceVertices.size() && newFaceVertices.front() == newFaceVertices.back() )
457            newFaceVertices.pop_back();
458
459        if( newFaceVertices.size() == 1 ) {  // This is very rare but correct
460            WARN
461                << "ConvexPolyhedron::transformClip - Slicing face polygon returns "
462                << newFaceVertices.size()
463                << " points. Should be 2 (usually) or 1 (rarely)."
464                << std::endl;
465
466        } else if( newFaceVertices.size() == 2 ) {
467            if( newFaceVertices[0] < newFaceVertices[1] ) {
468                edges.insert( Edge( newFaceVertices[0], newFaceVertices[1] ) );
469            } else {
470                edges.insert( Edge( newFaceVertices[1], newFaceVertices[0] ) );
471            }
472        } else if( newFaceVertices.size() > 2 ) {
473
474#if CONVEX_POLYHEDRON_WARN_ON_INCORRECT_FACE_CUT
475
476            // This may happen if face polygon is not planar or convex.
477            // It may happen when face was distorted by projection transform
478            // or when some polygon vertices land in incorrect positions after
479            // some transformation by badly conditioned matrix (weak precison)
480
481            WARN
482                << "ConvexPolyhedron::transformClip - Slicing face polygon returns "
483                << newFaceVertices.size()
484                << " points. Should be 2 (usually) or 1 (rarely)."
485                << std::endl;
486#endif
487
488#if CONVEX_POLYHEDRON_DUMP_ON_INCORRECT_FACE_CUT
489            dumpGeometry( &face, NULL, &cp );
490#endif
491
492            // Lets try to recover from this uneasy situation
493            // by comparing current face polygon edges cut by new plane
494            // with edges selected for new plane
495
496            unsigned i0 = 0, i1 = newFaceVertices.size() - 1;
497            unsigned j0 = 0, j1 = newVertices.size() - 1;
498
499            for( ; i0 < newFaceVertices.size(); i1 = i0++, j1 = j0++ ) {
500                while( newFaceVertices[i0] != newVertices[j0] ) j1 = j0++;
501                if( newFaceVertices[i1] == newVertices[j1] )
502                {
503                    if( newFaceVertices[i0] < newFaceVertices[i1] ) {
504                        edges.insert( Edge( newFaceVertices[i0], newFaceVertices[i1] ) );
505                    } else {
506                        edges.insert( Edge( newFaceVertices[i1], newFaceVertices[i0] ) );
507                    }
508                }
509            }
510        }
511
512        if( newVertices.size() >= 3 ) { //Add faces with at least 3 points
513
514#if ( CONVEX_POLYHEDRON_WARN_ON_CONCAVE_POLYGON || CONVEX_POLYHEDRON_DUMP_ON_CONCAVE_POLYGON )
515            int convex = isFacePolygonConvex( face );
516            face.vertices.swap( newVertices );
517            if( convex && !isFacePolygonConvex( face ) ) {
518#if CONVEX_POLYHEDRON_WARN_ON_CONCAVE_POLYGON
519                WARN << "ConvexPolyhedron::transformClip - polygon output non convex."
520                << " This may lead to other issues in ConvexPolyhedron math" << std::endl;
521#endif
522#if CONVEX_POLYHEDRON_DUMP_ON_CONCAVE_POLYGON
523                dumpGeometry( &face, NULL, &cp );
524#endif
525            }
526#else
527            face.vertices.swap( newVertices );
528#endif
529        } else //Remove face reduced to 0, 1, 2 points
530            _faces.erase( fd->itr );
531    }
532
533    osg::Vec3d center( 0, 0, 0 );
534    unsigned count = 0;
535    for( Faces::iterator itr = _faces.begin();
536        itr != _faces.end();
537        ++itr )
538    {
539        for( Vertices::iterator vitr = itr->vertices.begin();
540            vitr != itr->vertices.end();
541            ++vitr )
542        {
543            center += *vitr;
544            count ++;
545        }
546    }
547
548    center /= count;
549
550
551    if ( edges.size() > 1 ) //Ignore faces reduced to 0, 1, 2 points
552    {
553        Face face;
554        face.name = "Almost infinite plane";
555
556        std::deque< osg::Vec3d > vertices;
557
558        Edges::iterator itr = edges.begin();
559        vertices.push_back( itr->first );
560        vertices.push_back( itr->second );
561        edges.erase( itr++ );
562
563        for( unsigned int vertices_size = 0;
564            vertices_size < vertices.size(); )
565        {
566            vertices_size = vertices.size();
567            for( itr = edges.begin(); itr != edges.end(); )
568            {
569                bool not_added = false;
570
571                if( itr->first == vertices.back() )
572                    vertices.push_back( itr->second );
573                else if ( itr->first == vertices.front() )
574                    vertices.push_front( itr->second );
575                else if ( itr->second == vertices.back() )
576                    vertices.push_back( itr->first );
577                else if ( itr->second == vertices.front() )
578                    vertices.push_front( itr->first );
579                else
580                    not_added = true;
581
582                if( not_added )
583                    ++itr;
584                else
585                    edges.erase( itr++ );
586            }
587        }
588
589#if CONVEX_POLYHEDRON_WARN_ON_BAD_POLYGON
590        if( !edges.empty() ) {
591            WARN
592                << "ConvexPolyhedron::transformClip - Building new face polygon - "
593                << "Found edges not matching former polygon ends"
594                << std::endl;
595        }
596#endif
597
[9009]598        std::copy(vertices.begin(), vertices.end(), std::back_inserter(face.vertices));
[8984]599
600        face.plane.set( vertices[0], vertices[1], vertices[2] );
601
602        if( face.plane.distance( center ) < 0.0 ) face.plane.flip();
603
604        _faces.push_back(face);
605
606        // Last vertex is duplicated - remove one instance
607        if( face.vertices.front() == face.vertices.back() )
608            face.vertices.pop_back();
609        else {// If not duplicated then it may mean we have open polygon ;-(
610#if CONVEX_POLYHEDRON_WARN_ON_BAD_POLYGON
611            WARN
612            << "ConvexPolyhedron::transformClip - Building new face polygon - "
613            << " Polygon not properly closed."
614            << std::endl;
615#endif
616#if CONVEX_POLYHEDRON_DUMP_ON_BAD_POLYGON
617            dumpGeometry( &_faces.back(), NULL, &cp );
618#endif
619        }
620
621#if ( CONVEX_POLYHEDRON_WARN_ON_CONCAVE_POLYGON || CONVEX_POLYHEDRON_DUMP_ON_CONCAVE_POLYGON )
622        if( !isFacePolygonConvex( face ) ) {
623#if CONVEX_POLYHEDRON_WARN_ON_CONCAVE_POLYGON
624            WARN << "ConvexPolyhedron::transformClip - new face polygon non convex."
625                << " This may lead to other issues in ConvexPolyhedron math" << std::endl;
626#endif
627#if CONVEX_POLYHEDRON_DUMP_ON_CONCAVE_POLYGON
628            ConvexPolyhedron cp;
629            cp.createFace() = face;
630            cp.dumpGeometry( );
631#endif
632        }
633#endif
634    }
635
636    // Perpective transforms and lack of precision
637    // occasionaly cause removal of some points
638
639    removeDuplicateVertices( );
640
641    checkCoherency( true, "ConvexPolyhedron::transformClip" );
642}
643
644bool ConvexPolyhedron::mergeFaces
645    ( const Face & face0, const Face & face1, Face & face )
646{
647    typedef std::pair< osg::Vec3d, osg::Vec3d >  Edge;
648    typedef std::set<Edge> Edges;
649    Edges edges;
650
651    for(unsigned int i=0; i<face0.vertices.size(); ++i)
652    {
653        const osg::Vec3d* va = &face0.vertices[i];
654        const osg::Vec3d* vb = &face0.vertices[(i+1)%face0.vertices.size()];
655        if( *vb == *va ) continue; // ignore duplicated point edges
656        if( *vb < *va ) std::swap( va, vb );
657        edges.insert( Edge( *va, *vb ) );
658    }
659
660    bool intersection = false;
661    for(unsigned int i=0; i<face1.vertices.size(); ++i)
662    {
663        const osg::Vec3d* va = &face1.vertices[i];
664        const osg::Vec3d* vb = &face1.vertices[(i+1)%face1.vertices.size()];
665        if( *vb == *va ) continue; // ignore duplicated point edges
666        if( *vb < *va ) std::swap( va, vb );
667        Edge edge( *va, *vb );
668        Edges::iterator itr = edges.find( edge );
669        if( itr == edges.end() )
670            edges.insert( edge );
671        else {
672            edges.erase( itr );
673            intersection = true;
674        }
675    }
676
677    if( intersection )
678        face.vertices.clear();
679
680    if( intersection && edges.size() > 1 ) //Ignore faces reduced to 0, 1, 2 points
681    {
682        std::deque< osg::Vec3d > vertices;
683
684        Edges::iterator itr = edges.begin();
685        vertices.push_back( itr->first );
686        vertices.push_back( itr->second );
687        edges.erase( itr++ );
688
689        for( unsigned int vertices_size = 0;
690            vertices_size < vertices.size(); )
691        {
692            vertices_size = vertices.size();
693            for( itr = edges.begin(); itr != edges.end(); )
694            {
695                bool not_added = false;
696
697                if( itr->first == vertices.back() )
698                    vertices.push_back( itr->second );
699                else if ( itr->first == vertices.front() )
700                    vertices.push_front( itr->second );
701                else if ( itr->second == vertices.back() )
702                    vertices.push_back( itr->first );
703                else if ( itr->second == vertices.front() )
704                    vertices.push_front( itr->first );
705                else
706                    not_added = true;
707
708                if( not_added )
709                    ++itr;
710                else
711                    edges.erase( itr++ );
712            }
713        }
714
715#if CONVEX_POLYHEDRON_WARN_ON_BAD_POLYGON
716        if( !edges.empty() ) {
717            WARN
718                << "ConvexPolyhedron::mergeFaces - Building new face polygon - "
719                << "Found edges not matching former polygon ends."
720                << std::endl;
721        }
722#endif
723
724        // Last vertex is duplicated - remove one instance
725        if( vertices.front() == vertices.back() )
726            vertices.pop_back();
727        else {// If not duplicated then it may mean we have open polygon ;-(
728#if CONVEX_POLYHEDRON_WARN_ON_BAD_POLYGON
729            WARN
730            << "ConvexPolyhedron::mergeFaces - Building new face polygon - "
731            << " Polygon not properly closed."
732            << std::endl;
733#endif
734#if CONVEX_POLYHEDRON_DUMP_ON_BAD_POLYGON
735#endif
736        }
737
738#if 1 // Resulting plane will be the same as face0
739        face.plane = face0.plane;
[9009]740
741        std::copy(vertices.begin(), vertices.end(), std::back_inserter(face.vertices));
742
[8984]743#else // Compute resulting plane as average of faces (not a good idea)
744        osg::Vec3d normal = face0.plane.getNormal() + face1.plane.getNormal();
745        normal.normalize();
746
747        osg::Vec3d center;
748        for( unsigned int i = 0; i < vertices.size(); ++i )
749        {
750            center += vertices[i];
751            face.vertices.push_back( vertices[i] );
752        }
753        center /= vertices.size();
754
755        face.plane.set( normal, center );
756#endif
757
758        face.name = face0.name + " + "  + face1.name;
759
760        // No testing for concave polys
761        // Its possible to build concave polygon from two merged faces
762        // But after all coplanar faces are merged final result should be convex.
763    }
764    return intersection;
765}
766
767void ConvexPolyhedron::mergeCoplanarFaces
768    ( const double & dot_tolerance, const double & delta_tolerance )
769{
770    for(Faces::iterator itr0 = _faces.begin();
771        itr0 != _faces.end();
772        ++itr0 )
773    {
774        double tolerance = delta_tolerance;
775        for( unsigned i = 0; i < itr0->vertices.size(); ++i ) {
776            tolerance = osg::maximum( tolerance,
777                fabs( itr0->plane.distance( itr0->vertices[i] ) ) );
778        }
779
780        for(Faces::iterator itr1 = _faces.begin();
781            itr1 != _faces.end();
782            )
783        {
784            if( itr1 == itr0 ) {
785                ++itr1;
786                continue;
787            }
788
789            bool attempt_merge = true;
790            for( unsigned i = 0; i < itr1->vertices.size(); ++i ) {
791                if( fabs( itr0->plane.distance( itr1->vertices[i] ) ) > tolerance )
792                {
793                    attempt_merge = false;
794                    break;
795                }
796            }
797
798            if( !attempt_merge &&
799                1.0 - itr0->plane.getNormal() * itr1->plane.getNormal() < dot_tolerance &&
800                fabs( itr0->plane.ptr()[3] - itr1->plane.ptr()[3] ) < delta_tolerance )
801                    attempt_merge = true;
802
803            if( attempt_merge && mergeFaces( *itr0, *itr1, *itr0 ) )
804                itr1 = _faces.erase( itr1 );
805            else
806                ++itr1;
807        }
808    }
809}
810
811void ConvexPolyhedron::removeDuplicateVertices( void )
812{
813#if 1
814    // Aggressive removal, find very close points and replace them
815    // with their average. Second step wil do the rest.
816
817    typedef std::map< osg::Vec3f, osg::Vec4d > Points;
818    typedef std::set< osg::Vec3d > VertexSet;
819
820    VertexSet  vertexSet;
821    Points     points;
822
823    for( Faces::iterator itr = _faces.begin();
824         itr != _faces.end();
825         ++itr )
826    {
827        for( Vertices::iterator vitr = itr->vertices.begin();
828             vitr != itr->vertices.end();
829             ++vitr )
830        {
831            vertexSet.insert( *vitr );
832        }
833    }
834
835    for( VertexSet::iterator vitr = vertexSet.begin();
836         vitr != vertexSet.end();
837         ++vitr )
838    {
839        points[ *vitr ] += osg::Vec4d( *vitr, 1.0 );
840    }
841
842    for( Points::iterator itr = points.begin();
843         itr != points.end();
844         ++itr )
845    {
846        if( itr->second[3] > 1.0 )
847            itr->second /= itr->second[3];
848    }
849
850    for( Faces::iterator itr = _faces.begin();
851         itr != _faces.end();
852         ++itr )
853    {
854        for( Vertices::iterator vitr = itr->vertices.begin();
855             vitr != itr->vertices.end();
856             ++vitr )
857        {
858            osg::Vec4d &v = points[ *vitr ];
859            *vitr = osg::Vec3d( v[0], v[1], v[2] );
860        }
861    }
862#endif
863
864    for( Faces::iterator itr = _faces.begin();
865         itr != _faces.end();
866        )
867    {
868        assert( itr->vertices.size() > 0 );
869
870        osg::Vec3d prev = itr->vertices.back();
871
872        for( Vertices::iterator vitr = itr->vertices.begin();
873            vitr != itr->vertices.end();
874            )
875        {
876            if( *vitr == prev ) {
877                vitr = itr->vertices.erase( vitr );
878            } else {
879                prev = *vitr;
880                ++vitr;
881            }
882        }
883
884        if( itr->vertices.size() < 3 )
885            itr = _faces.erase( itr );
886        else
887            ++itr;
888    }
889
890    mergeCoplanarFaces();
891
892#if 1
893    // Experimentally remove vertices on colinear edge chains
894    // effectivelyy replacing them with one edge
895    typedef std::map<osg::Vec3d,int> VertexCounter;
896    VertexCounter vertexCounter;
897
898    for( Faces::iterator itr = _faces.begin();
899        itr != _faces.end();
900        ++itr)
901    {
902        for( Vertices::iterator vitr = itr->vertices.begin();
903             vitr != itr->vertices.end();
904             ++vitr )
905        {
906            ++vertexCounter[ *vitr ];
907        }
908    }
909
910    for( Faces::iterator itr = _faces.begin();
911        itr != _faces.end();
912        )
913    {
914        for( Vertices::iterator vitr = itr->vertices.begin();
915             vitr != itr->vertices.end();
916             )
917        {
918            if( vertexCounter[ *vitr ] == 2 ) {
919#if 0 // Sanity check if we could remove this point without changing poly shape
920
921                Vertices::iterator next = ( vitr + 1 == itr->vertices.end() ?
922                                            itr->vertices.begin() :
923                                            vitr + 1 );
924
925                Vertices::iterator prev = ( vitr == itr->vertices.begin() ?
926                                            itr->vertices.end() - 1 :
927                                            vitr - 1 );
928
929
930                osg::Vec3d va = *vitr - *prev;
931                osg::Vec3d vb = *next - *vitr;
932
933                double da = va.normalize();
934                double db = vb.normalize();
935
936                if( 0.001 < da && 0.001 < db )
937                {
938                    double dot = va * vb, limit = cos( 0.01 );
939                    if( dot < limit ) {
940                        WARN << "ConvexPolyhedron::removeDuplicateVertices"
941                            << " - removed mid point connecting two non colinear edges."
942                            << " Angle(deg): " << osg::RadiansToDegrees( acos( dot ) )
943                            << " Length1: " << da
944                            << " Length2: " << db
945                            << std::endl;
946                    }
947                } else {
948                    if( da == 0.0 || db == 0.0 )
949                        WARN << "ConvexPolyhedron::removeDuplicateVertices"
950                             << " - removed degenerated mid point connecting two edges"
951                            << std::endl;
952
953                }
954#endif
955                vitr = itr->vertices.erase( vitr );
956            } else {
957                ++vitr;
958            }
959        }
960
961        if( itr->vertices.size() < 3 )
962            itr = _faces.erase( itr );
963        else
964            ++itr;
965    }
966#endif
967
968    checkCoherency( false, "Leave ConvexPolyhedron::removeDuplicateVertices" );
969}
970
971int ConvexPolyhedron::pointsColinear
972    ( const osg::Vec3d & a, const osg::Vec3d & b, const osg::Vec3d & c,
973      const double & dot_tolerance, const double & delta_tolerance )
974{
975    osg::Vec3d va = b - a;
976    osg::Vec3d vb = c - b;
977
978    double da = va.normalize();
979    double db = vb.normalize();
980
981    if( delta_tolerance >= da || delta_tolerance >= db )
982        return -1; // assume collinearity if one of the edges is zero length
983
984    if( 1.0 - fabs( va * vb ) <= dot_tolerance )
985        return 1; // edge normals match collinearity condition
986
987    return 0; // nope. not collinear
988}
989
990int ConvexPolyhedron::isFacePolygonConvex( Face & face, bool ignoreColinearVertices )
991{
992    int positive = 0, negative = 0, colinear = 0;
993    for( unsigned int i = 0; i < face.vertices.size(); ++i )
994    {
995        osg::Vec3d va = face.vertices[i];
996        osg::Vec3d vb = face.vertices[(i+1)%face.vertices.size()];
997        osg::Vec3d vc = face.vertices[(i+2)%face.vertices.size()];
998
999
1000#if ( CONVEX_POLYHEDRON_WARN_ON_INCOHERENT_DATA  > 1 || CONVEX_POLYHEDRON_WARN_ON_CONCAVE_POLYGON )
[9376]1001        double dist = fabs( face.plane.distance( va ) );
[8984]1002        if( dist > 0.0001 )
1003        {
1004            WARN << "ConvexPolyhedron::isFacePolygonConvex - plane point too far from plane (" << dist <<")" << std::endl;
1005        }
1006#endif
1007
1008        // cast points on a plane
1009        va -= face.plane.getNormal() * face.plane.distance( va );
1010        vb -= face.plane.getNormal() * face.plane.distance( vb );
1011        vc -= face.plane.getNormal() * face.plane.distance( vc );
1012
1013        if( pointsColinear( va, vb, vc ) ) {
1014            colinear++;
1015        } else {
1016            double side =( ( vc  - vb ) ^ ( vb - va ) ) * face.plane.getNormal();
1017
1018            if( side < 0 ) negative++;
1019            if( side > 0 ) positive++;
1020        }
1021    }
1022
1023    if( !ignoreColinearVertices && colinear > 0 )
1024        return 0;
1025
1026    if( !negative && !positive )
1027        return 0;
1028
[9376]1029    if( (negative + colinear) == static_cast<int>(face.vertices.size()) )
[8984]1030        return -( negative + colinear );
1031
[9376]1032    if( (positive + colinear) == static_cast<int>(face.vertices.size()) )
[8984]1033        return +( positive + colinear );
1034
1035    return 0;
1036}
1037
1038bool ConvexPolyhedron::checkCoherency
1039    ( bool checkForNonConvexPolys, const char * errorPrefix )
1040{
1041    bool result = true;
1042    bool convex = true;
1043
1044#if CONVEX_POLYHEDRON_CHECK_COHERENCY
1045
1046    if( !errorPrefix ) errorPrefix = "ConvexPolyhedron";
1047
1048    typedef std::pair<osg::Vec3d, osg::Vec3d> Edge;
1049    typedef std::map<Edge,int> EdgeCounter;
1050    typedef std::map<osg::Vec3d,int> VertexCounter;
1051
1052    EdgeCounter edgeCounter;
1053    VertexCounter vertexCounter;
1054
1055    for( Faces::iterator itr = _faces.begin();
1056        itr != _faces.end();
1057        ++itr)
1058    {
1059        Face& face = *itr;
1060
1061        if( checkForNonConvexPolys && !isFacePolygonConvex( face ) ) {
1062            convex = false;
1063#if ( 1 < CONVEX_POLYHEDRON_WARN_ON_INCOHERENT_DATA || CONVEX_POLYHEDRON_WARN_ON_CONCAVE_POLYGON )
1064            WARN << errorPrefix <<
1065                " - coherency fail - face polygon concave" << std::endl;
1066#endif
1067#if ( 1 < CONVEX_POLYHEDRON_DUMP_ON_INCOHERENT_DATA || CONVEX_POLYHEDRON_DUMP_ON_CONCAVE_POLYGON )
1068            dumpGeometry( &face );
1069#endif
1070        }
1071
1072        Vertices& vertices = face.vertices;
1073        for(unsigned int i=0; i<vertices.size(); ++i)
1074        {
1075            osg::Vec3d& a = vertices[ i ];
1076            osg::Vec3d& b = vertices[ (i+1) % vertices.size()];
1077            ++vertexCounter[ a ];
1078            if (a<b)
1079                ++edgeCounter[Edge(a,b)];
1080            else
1081                ++edgeCounter[Edge(b,a)];
1082        }
1083    }
1084
1085
1086    for( EdgeCounter::iterator itr = edgeCounter.begin();
1087        itr != edgeCounter.end();
1088        ++itr)
1089    {
1090        const Edge &e = itr->first;
1091
1092        if( e.first.isNaN() ) {
1093            result = false;
1094#if ( 1 < CONVEX_POLYHEDRON_WARN_ON_INCOHERENT_DATA )
1095            WARN << errorPrefix <<
1096                " - coherency fail - Vertex is NaN." << std::endl;
1097#endif
1098        }
1099
1100        if( e.second.isNaN() ) {
1101            result = false;
1102#if ( 1 < CONVEX_POLYHEDRON_WARN_ON_INCOHERENT_DATA )
1103            WARN << errorPrefix <<
1104                " - coherency fail - Vertex is NaN." << std::endl;
1105#endif
1106        }
1107
1108        if( e.first == e.second ) {
1109            result = false;
1110#if ( 1 < CONVEX_POLYHEDRON_WARN_ON_INCOHERENT_DATA )
1111            WARN << errorPrefix <<
1112                " - coherency fail - Edge with identical vertices." << std::endl;
1113#endif
1114        }
1115
1116        if( vertexCounter[ e.first ] < 3 ) {
1117            result = false;
1118#if ( 1 < CONVEX_POLYHEDRON_WARN_ON_INCOHERENT_DATA )
1119            WARN << errorPrefix <<
1120                " - coherency fail - Vertex present " << vertexCounter[ e.first ] << " times" << std::endl;
1121#endif
1122        }
1123
1124        if( vertexCounter[ e.second ] < 3 ) {
1125            result = false;
1126#if ( 1 < CONVEX_POLYHEDRON_WARN_ON_INCOHERENT_DATA )
1127            WARN << errorPrefix <<
1128                " - coherency fail - Vertex present " << vertexCounter[ e.second ] << " times" << std::endl;
1129#endif
1130        }
1131
1132        if( itr->second != 2 ) {
1133            result = false;
1134#if ( 1 < CONVEX_POLYHEDRON_WARN_ON_INCOHERENT_DATA )
1135            WARN << errorPrefix <<
1136                " - coherency fail - Edge present " << itr->second << " times" << std::endl;
1137#endif
1138        }
1139    }
1140
1141#if ( 1 == CONVEX_POLYHEDRON_WARN_ON_INCOHERENT_DATA )
1142    if( !convex )
1143        WARN << errorPrefix
1144            << " - coherency fail - non convex output" << std::endl;
1145
1146    if ( !result )
1147        WARN << errorPrefix
1148            << " - coherency fail - incoherent output" << std::endl;
1149#endif
1150
1151#if ( 1 == CONVEX_POLYHEDRON_DUMP_ON_INCOHERENT_DATA )
1152    if( !result || !convex )
1153        dumpGeometry( );
1154#endif
1155
1156#if ( 1 < CONVEX_POLYHEDRON_DUMP_ON_INCOHERENT_DATA )
1157    if( !result )
1158        dumpGeometry( );
1159#endif
1160
1161
1162#endif // CONVEX_POLYHEDRON_CHECK_COHERENCY
1163    return result && convex;
1164}
1165
1166osg::BoundingBox ConvexPolyhedron::computeBoundingBox( const osg::Matrix & m ) const
1167{
1168    osg::BoundingBox bb;
1169
1170    if( &m != &defaultMatrix ) {
1171        for( Faces::const_iterator itr = _faces.begin(); itr != _faces.end(); ++itr )
1172            for( Vertices::const_iterator vitr = itr->vertices.begin();
1173                 vitr != itr->vertices.end();
1174                 ++vitr )
1175                    bb.expandBy( *vitr * m );
1176    } else {
1177        for( Faces::const_iterator itr = _faces.begin(); itr != _faces.end(); ++itr )
1178            for( Vertices::const_iterator vitr = itr->vertices.begin();
1179                 vitr != itr->vertices.end();
1180                 ++vitr )
1181                    bb.expandBy( *vitr );
1182    }
1183
1184    return bb;
1185}
1186
1187void ConvexPolyhedron::cut(const osg::Polytope & polytope)
1188{
1189    const char * apc[6] = { "left", "right", "bottom", "top", "near", "far" };
1190    char ac[16];
1191    int i = 0;
1192
1193    for(osg::Polytope::PlaneList::const_iterator itr = polytope.getPlaneList().begin();
1194        itr != polytope.getPlaneList().end();
1195        ++itr)
1196    {
1197        const char* arg;
1198        if (i < 6) {
1199            arg = apc[i];
1200        } else {
1201            sprintf(ac, "%d", i);
1202            arg = ac;
1203        }
1204        cut(*itr, std::string( arg ) );
1205
1206        i++;
1207    }
1208
1209    removeDuplicateVertices();
1210}
1211
1212void ConvexPolyhedron::cut(const ConvexPolyhedron& polytope)
1213{
1214    for(Faces::const_iterator itr = polytope._faces.begin();
1215        itr != polytope._faces.end();
1216        ++itr)
1217    {
1218        cut(itr->plane, itr->name);
1219    }
1220
1221    removeDuplicateVertices();
1222}
1223
1224void ConvexPolyhedron::cut(const osg::Plane& plane, const std::string& name)
1225{
1226    if( _faces.empty() ) return;
1227
1228    ConvexPolyhedron cp( *this );
1229
1230    typedef std::vector< FaceDistances > FaceDistancesList;
1231    FaceDistancesList faceDistances;
1232    faceDistances.resize( _faces.size() );
1233
1234    double min = FLT_MAX, max = -FLT_MAX; //Hull max & min point distances
1235
1236    FaceDistancesList::iterator fd = faceDistances.begin();
1237    // First step compute each face points distances to cutting plane
1238    for( Faces::iterator itr = _faces.begin();
1239        itr != _faces.end();
1240        ++itr, ++fd )
1241    {
1242        fd->itr = itr;
1243        fd->distances.reserve( itr->vertices.size() );
1244        fd->on = 0;
1245        fd->above = 0;
1246        fd->below = 0;
1247
1248#if 0 //  Skip if cutting plane the same as one of faces
1249        if( plane.ptr()[0] ) == itr->plane.ptr()[0] &&
1250            plane.ptr()[1] ) == itr->plane.ptr()[1] &&
1251            plane.ptr()[2] ) == itr->plane.ptr()[2] &&
[8986]1252#else    // check plane using less precise float values
[8984]1253        if( float( plane.ptr()[0] ) == float( itr->plane.ptr()[0] ) &&
1254            float( plane.ptr()[1] ) == float( itr->plane.ptr()[1] ) &&
1255            float( plane.ptr()[2] ) == float( itr->plane.ptr()[2] ) &&
1256#endif
1257            plane_hull_tolerance >= fabs( float( plane.ptr()[3] )-  float( itr->plane.ptr()[3] ) ) )
1258            return;
1259
1260        for( Vertices::iterator vitr = itr->vertices.begin();
1261            vitr != itr->vertices.end();
1262            ++vitr)
1263        {
1264            double d = plane.distance( *vitr );
1265
1266            fd->distances.push_back( d );
1267            if ( d>point_plane_tolerance )       ++fd->above;
1268            else if ( d<-point_plane_tolerance ) ++fd->below;
1269            else                                 ++fd->on;
1270            min = osg::minimum( min, d );
1271            max = osg::maximum( max, d );
1272        }
1273    }
1274
1275    if( max <= plane_hull_tolerance ) { // All points on or below cutting plane
1276        _faces.clear();
1277        return;
1278    }
1279
1280    if( min >= -plane_hull_tolerance ) { // All points on or above cutting plane
1281        return;
1282    }
1283
1284    typedef std::pair<osg::Vec3d, osg::Vec3d> Edge;
1285    typedef std::set< Edge > Edges;
1286    Edges edges;
1287
1288    for( FaceDistancesList::iterator fd = faceDistances.begin();
1289        fd != faceDistances.end();
1290        ++fd )
1291    {
1292        if ( fd->below == 0 )
1293        {  // skip face if all points on or above cutting plane ( below == 0 )
1294            continue;
1295        }
1296
1297        if ( /* fd->below > 0 && */ fd->above == 0 && fd->on == 0 )
1298        {
1299            _faces.erase( fd->itr ); // remove face if points below or on
1300            continue;
1301        }
1302
1303        // cut the face if some points above and below plane
1304        // assert( fd->below > 0 && fd->above > 0 );
1305
1306        Face& face = *(fd->itr);
1307        Vertices& vertices = face.vertices;
1308        Distances& distances = fd->distances;
1309        Vertices newFaceVertices;
1310        Vertices newVertices;
1311
1312
1313        for(unsigned int i=0; i < vertices.size(); ++i)
1314        {
1315            osg::Vec3d &va = vertices[i];
1316            osg::Vec3d &vb = vertices[(i+1)%vertices.size()];
1317            double &distance_a = distances[i];
1318            double &distance_b = distances[(i+1)%vertices.size()];
1319
1320            // Is first edge point above or on the plane?
1321            if ( -point_plane_tolerance <= distance_a ) {
1322
1323                if( newVertices.empty() || vertices[i] != newVertices.back() )
1324                    newVertices.push_back( vertices[i] );
1325
1326                if ( distance_a <= point_plane_tolerance ) {
1327                    if( newFaceVertices.empty() || vertices[i] != newFaceVertices.back() )
1328                        newFaceVertices.push_back( vertices[i] );
1329                }
1330            }
1331
1332            // Does edge intersect plane ?
1333            if ( ( distance_a < -point_plane_tolerance && distance_b > point_plane_tolerance ) ||
1334                 ( distance_b < -point_plane_tolerance && distance_a > point_plane_tolerance ) )
1335            {
1336                osg::Vec3d intersection; // Inserting vertex
1337                double da = fabs( distance_a ), db = fabs( distance_b );
1338
1339                // tweaks to improve coherency of polytope after cut
1340                if( da <= point_point_equivalence && da <= db ) {
1341                    intersection = va;
1342                } else if( db <= point_point_equivalence && db <= da ) {
1343                    intersection = vb;
1344                } else {
1345                    double dab4 = 0.25 * ( da + db );
1346                    if( dab4 < da && dab4 < db ) {
1347                        intersection = (vb*distance_a - va*distance_b)/(distance_a-distance_b);
1348                    } else {
1349                        osg::Vec3d v = (vb - va)/(distance_a-distance_b);
1350                        if( da < db )
1351                            intersection = va + v * distance_a;
1352                        else
1353                            intersection = vb + v * distance_b;
1354                    }
1355                }
1356
1357                if( newVertices.empty() || intersection != newVertices.back() )
1358                    newVertices.push_back( intersection );
1359
1360                if( newFaceVertices.empty() || intersection != newFaceVertices.back() )
1361                    newFaceVertices.push_back( intersection );
1362            }
1363        }
1364
1365        if( newVertices.size() && newVertices.front() == newVertices.back() )
1366            newVertices.pop_back();
1367
1368        if( newFaceVertices.size() && newFaceVertices.front() == newFaceVertices.back() )
1369            newFaceVertices.pop_back();
1370
1371        if( newFaceVertices.size() == 1 ) {  // This is very rare but correct
1372            WARN
1373                << "ConvexPolyhedron::cut - Slicing face polygon returns "
1374                << newFaceVertices.size()
1375                << " points. Should be 2 (usually) or 1 (rarely)."
1376                << std::endl;
1377
1378        } else if( newFaceVertices.size() == 2 ) {
1379            if( newFaceVertices[0] < newFaceVertices[1] ) {
1380                edges.insert( Edge( newFaceVertices[0], newFaceVertices[1] ) );
1381            } else {
1382                edges.insert( Edge( newFaceVertices[1], newFaceVertices[0] ) );
1383            }
1384        } else if( newFaceVertices.size() > 2 ) {
1385
1386#if CONVEX_POLYHEDRON_WARN_ON_INCORRECT_FACE_CUT
1387
1388            // This may happen if face polygon is not planar or convex.
1389            // It may happen when face was distorted by projection transform
1390            // or when some polygon vertices land in incorrect positions after
1391            // some transformation by badly conditioned matrix (weak precison)
1392
1393            WARN
1394                << "ConvexPolyhedron::cut - Slicing face polygon returns "
1395                << newFaceVertices.size()
1396                << " points. Should be 2 (usually) or 1 (rarely)."
1397                << std::endl;
1398#endif
1399
1400#if CONVEX_POLYHEDRON_DUMP_ON_INCORRECT_FACE_CUT
1401            dumpGeometry( &face, &plane );
1402#endif
1403
1404            // Let try to recover from this uneasy situation
1405            // by comparing current face polygon edges cut by new plane
1406            // with edges selected for new plane
1407
1408            unsigned i0 = 0, i1 = newFaceVertices.size() - 1;
1409            unsigned j0 = 0, j1 = newVertices.size() - 1;
1410
1411            for( ; i0 < newFaceVertices.size(); i1 = i0++, j1 = j0++ ) {
1412                while( newFaceVertices[i0] != newVertices[j0] ) j1 = j0++;
1413                if( newFaceVertices[i1] == newVertices[j1] )
1414                {
1415                    if( newFaceVertices[i0] < newFaceVertices[i1] ) {
1416                        edges.insert( Edge( newFaceVertices[i0], newFaceVertices[i1] ) );
1417                    } else {
1418                        edges.insert( Edge( newFaceVertices[i1], newFaceVertices[i0] ) );
1419                    }
1420                }
1421            }
1422        }
1423
1424        if( newVertices.size() >= 3 ) { //Add faces with at least 3 points
1425#if ( CONVEX_POLYHEDRON_WARN_ON_CONCAVE_POLYGON || CONVEX_POLYHEDRON_DUMP_ON_CONCAVE_POLYGON )
1426            int convex = isFacePolygonConvex( face );
1427            vertices.swap( newVertices );
1428            if( convex && !isFacePolygonConvex( face ) ) {
1429#if CONVEX_POLYHEDRON_WARN_ON_CONCAVE_POLYGON
1430                WARN << "ConvexPolyhedron::cut - polygon output non convex."
1431                << " This may lead to other issues in ConvexPolyhedron math" << std::endl;
1432#endif
1433#if CONVEX_POLYHEDRON_DUMP_ON_CONCAVE_POLYGON
1434                dumpGeometry( &face, &plane, &cp );
1435#endif
1436            }
1437#else
1438            vertices.swap( newVertices );
1439#endif
1440        } else //Remove face reduced to 0, 1, 2 points
1441            _faces.erase( fd->itr );
1442    }
1443
1444    if ( edges.size() > 1 ) //Ignore faces reduced to 0, 1, 2 points
1445    {
1446        Face face;
1447        face.name = name;
1448        face.plane = plane;
1449
1450        std::deque< osg::Vec3d > vertices;
1451
1452        Edges::iterator itr = edges.begin();
1453        vertices.push_back( itr->first );
1454        vertices.push_back( itr->second );
1455        edges.erase( itr++ );
1456
1457        for( unsigned int vertices_size = 0;
1458            vertices_size < vertices.size(); )
1459        {
1460            vertices_size = vertices.size();
1461            for( itr = edges.begin(); itr != edges.end(); )
1462            {
1463                bool not_added = false;
1464
1465                if( itr->first == vertices.back() )
1466                    vertices.push_back( itr->second );
1467                else if ( itr->first == vertices.front() )
1468                    vertices.push_front( itr->second );
1469                else if ( itr->second == vertices.back() )
1470                    vertices.push_back( itr->first );
1471                else if ( itr->second == vertices.front() )
1472                    vertices.push_front( itr->first );
1473                else
1474                    not_added = true;
1475
1476                if( not_added )
1477                    ++itr;
1478                else
1479                    edges.erase( itr++ );
1480            }
1481        }
1482
1483#if CONVEX_POLYHEDRON_WARN_ON_BAD_POLYGON
1484        if( !edges.empty() ) {
1485            WARN
1486                << "ConvexPolyhedron::cut - Building new face polygon - "
1487                << "Found edges not matching former polygon ends"
1488                << std::endl;
1489        }
1490#endif
1491
[9009]1492        std::copy(vertices.begin(), vertices.end(), std::back_inserter(face.vertices));
1493
[8984]1494        _faces.push_back(face);
1495
1496        // Last vertex is duplicated - remove one instance
1497        if( face.vertices.front() == face.vertices.back() )
1498            face.vertices.pop_back();
1499        else {// If not duplicated then it may mean we have open polygon ;-(
1500#if CONVEX_POLYHEDRON_WARN_ON_BAD_POLYGON
1501            WARN
1502            << "ConvexPolyhedron::cut - Building new face polygon - "
1503            << " Polygon not properly closed."
1504            << std::endl;
1505#endif
1506#if CONVEX_POLYHEDRON_DUMP_ON_BAD_POLYGON
1507            dumpGeometry( &_faces.back(), &plane, &cp );
1508#endif
1509        }
1510
1511#if ( CONVEX_POLYHEDRON_WARN_ON_CONCAVE_POLYGON || CONVEX_POLYHEDRON_DUMP_ON_CONCAVE_POLYGON )
1512        if( !isFacePolygonConvex( face ) ) {
1513#if CONVEX_POLYHEDRON_DUMP_ON_CONCAVE_POLYGON
1514            ConvexPolyhedron cp;
1515            cp.createFace() = face;
1516            cp.dumpGeometry( );
1517#endif
1518#if CONVEX_POLYHEDRON_WARN_ON_CONCAVE_POLYGON
1519            WARN << "ConvexPolyhedron::cut - new face polygon non convex."
1520                << " This may lead to other issues in ConvexPolyhedron math" << std::endl;
1521#endif
1522        }
1523#endif
1524    }
1525
1526//    removeDuplicateVertices( );
1527}
1528////////////////////////////////////////////////////////////////////////////
1529void ConvexPolyhedron::extrude( const osg::Vec3d & offset )
1530{
1531    if( offset.length2() == 0 ) return;
1532
1533    typedef std::pair<osg::Vec3d, osg::Vec3d> Edge;
1534    typedef std::vector<Face*> EdgeFaces;
1535    typedef std::map<Edge, EdgeFaces> EdgeMap;
1536
1537    EdgeMap edgeMap;
1538
1539    // Build edge maps
1540    for(Faces::iterator itr = _faces.begin();
1541        itr != _faces.end();
1542        ++itr)
1543    {
1544        Face& face = *itr;
1545        for(unsigned int i=0; i<face.vertices.size(); ++i)
1546        {
1547            osg::Vec3d& va = face.vertices[i];
1548            osg::Vec3d& vb = face.vertices[(i+1)%face.vertices.size()];
1549            if (va < vb) edgeMap[Edge(va,vb)].push_back(&face);
1550            else edgeMap[Edge(vb,va)].push_back(&face);
1551        }
1552    }
1553
1554    // Offset faces
1555    for(Faces::iterator itr = _faces.begin();
1556        itr != _faces.end();
1557        ++itr)
1558    {
1559        Face& face = *itr;
1560
1561        double dotOffset = face.plane.dotProductNormal( offset );
1562
1563        if( dotOffset >= 0 ) continue;
1564
1565        face.plane.ptr()[3] -= dotOffset;
1566        for(unsigned int i=0; i<face.vertices.size(); ++i)
1567        {
1568            face.vertices[i] += offset;
1569        }
1570    }
1571
1572    typedef std::set< Edge > SilhouetteEdges;
1573    typedef std::map< Face*, SilhouetteEdges > SilhouetteFaces;
1574    SilhouetteFaces silhouetteFaces;
1575
1576    int new_face_counter = 0;
1577
1578    // Now add new faces from slhouette edges extrusion
1579    for(EdgeMap::iterator eitr = edgeMap.begin();
1580        eitr != edgeMap.end();
1581        ++eitr)
1582    {
1583        const Edge& edge = eitr->first;
1584        const EdgeFaces& edgeFaces = eitr->second;
1585
1586        if ( edgeFaces.size()==1 )
1587        {
1588            // WL: Execution should not reach this line.
1589            // If you got here let me know: lewandowski@ai.com.pl
1590            assert( 0 );
1591        }
1592        else if ( edgeFaces.size()==2 )
1593        {
1594#if 0 // Use float normal computations
1595            osg::Vec3f vf( offset );
1596            double dotOffset0 = osg::Vec3f( edgeFaces[0]->plane.getNormal() ) * vf;
1597            double dotOffset1 = osg::Vec3f( edgeFaces[1]->plane.getNormal() ) * vf;
1598#else
1599            double dotOffset0 = edgeFaces[0]->plane.getNormal() * offset;
1600            double dotOffset1 = edgeFaces[1]->plane.getNormal() * offset;
1601#endif
1602            //Select orthogonal faces and vertices appropriate for offseting
[9376]1603            if( (dotOffset0 == 0.0 && dotOffset1 < 0.0) ||
1604                (dotOffset1 == 0.0 && dotOffset0 < 0.0) )
[8984]1605            {
1606                Face * face = ( dotOffset0 == 0 ? edgeFaces[0] : edgeFaces[1] );
1607                silhouetteFaces[ face ].insert( edge );
1608            }
1609
[9376]1610            if( (dotOffset0 < 0.0 && dotOffset1 > 0.0) ||
1611                (dotOffset1 < 0.0 && dotOffset0 > 0.0) )
[8984]1612            {
1613                Face & face = createFace();
1614                char ac[40] = "Side plane from edge extrude ";               
1615                sprintf(ac + strlen(ac), "%d", new_face_counter++);
1616                face.name = ac;
1617
1618                // Compute face plane
1619                face.vertices.push_back( edge.first );
1620                face.vertices.push_back( edge.second );
1621
1622                osg::Vec3d n = face.vertices[0] - face.vertices[1];
1623                n.normalize();
1624                n = ( n ^ offset );
1625                n.normalize();
1626
1627                if( n * ( edgeFaces[1]->plane.getNormal() + edgeFaces[0]->plane.getNormal() ) < 0 )
1628                {
1629                    n = -n;
1630                    std::swap( face.vertices[1], face.vertices[0] );
1631                }
1632
1633                face.vertices.push_back( face.vertices[1] + offset );
1634                face.vertices.push_back( face.vertices[0] + offset );
1635
1636                face.plane.set( n,(face.vertices[0] + face.vertices[1] +
1637                    face.vertices[2] + face.vertices[3]) * .25 );
1638            }
1639        }
1640        else if( edgeFaces.size() > 2 )
1641        {
1642            assert( 0 );
1643            // WL: Execution should not reach this line.
1644            // If you got here let me know: lewandowski@ai.com.pl
1645        }
1646    }
1647
1648    // Finally update faces which are orthogonal to our normal
1649    for(SilhouetteFaces::iterator itr = silhouetteFaces.begin();
1650        itr != silhouetteFaces.end();
1651        ++itr)
1652    {
1653        SilhouetteEdges & edges = itr->second;
1654        Vertices & vertices =  itr->first->vertices;
1655        Vertices newVertices;
1656
1657        for( unsigned int i = 0; i < vertices.size(); i++ )
1658        {
1659            osg::Vec3d
1660                &va = vertices[ ( i + vertices.size() - 1 ) % vertices.size() ],
1661                &vb = vertices[  i  ],
1662                &vc = vertices[ ( i +  1 ) % vertices.size() ];
1663
1664            Edge eab =  va < vb ? Edge( va, vb ) : Edge( vb, va );
1665            Edge ebc =  vb < vc ? Edge( vb, vc ) : Edge( vc, vb );
1666
1667            bool abFound = edges.find( eab ) != edges.end();
1668            bool bcFound = edges.find( ebc ) != edges.end();
1669
1670            if( abFound && bcFound ) {
1671                newVertices.push_back( vb + offset );
1672            } else if( !abFound && !bcFound ) {
1673                newVertices.push_back( vb );
1674            } else if( !abFound && bcFound ) {
1675                newVertices.push_back( vb );
1676                newVertices.push_back( vb + offset );
1677            } else if( abFound && !bcFound ) {
1678                newVertices.push_back( vb + offset );
1679                newVertices.push_back( vb );
1680            }
1681        }
1682
1683        vertices.swap( newVertices );
1684    }
1685
1686    removeDuplicateVertices( );
1687    checkCoherency( true, "ConvexPolyhedron::extrude" );
1688}
1689////////////////////////////////////////////////////////////////////////////
1690void ConvexPolyhedron::translate( const osg::Vec3d & offset )
1691{
1692    for( Faces::iterator itr = _faces.begin(); itr != _faces.end(); ++itr )
1693    {
1694        itr->plane.ptr()[3] -= itr->plane.dotProductNormal( offset );
1695
1696        for( Vertices::iterator vitr = itr->vertices.begin();
1697             vitr != itr->vertices.end();
1698             ++vitr )
1699        {
1700            *vitr += offset;
1701        }
1702    }
1703}
1704////////////////////////////////////////////////////////////////////////////
1705void ConvexPolyhedron::getPolytope(osg::Polytope& polytope) const
1706{
1707    for(Faces::const_iterator itr = _faces.begin();
1708        itr != _faces.end();
1709        ++itr)
1710    {
1711        polytope.add(itr->plane);
1712    }
1713}
1714////////////////////////////////////////////////////////////////////////////
1715void ConvexPolyhedron::getPoints(Vertices& vertices) const
1716{
1717    typedef std::set<osg::Vec3d> VerticesSet;
1718    VerticesSet verticesSet;
1719    for(Faces::const_iterator itr = _faces.begin();
1720        itr != _faces.end();
1721        ++itr)
1722    {
1723        const Face& face = *itr;
1724        for(Vertices::const_iterator vitr = face.vertices.begin();
1725            vitr != face.vertices.end();
1726            ++vitr)
1727        {
1728            verticesSet.insert(*vitr);
1729        }
1730    }
1731
1732    for(VerticesSet::iterator sitr = verticesSet.begin();
1733        sitr != verticesSet.end();
1734        ++sitr)
1735    {
1736        vertices.push_back(*sitr);
1737    }
1738}
1739////////////////////////////////////////////////////////////////////////////
1740osg::Geometry* ConvexPolyhedron::buildGeometry( const osg::Vec4d& colorOutline,
1741                                         const osg::Vec4d& colorInside,
1742                                         osg::Geometry* geometry ) const
1743{
1744    if( !geometry ) {
1745        geometry = new osg::Geometry;
1746    } else {
1747        geometry->getPrimitiveSetList( ).clear();
1748    }
1749
1750    osg::Vec3dArray* vertices = new osg::Vec3dArray;
1751    geometry->setVertexArray(vertices);
1752
1753    osg::Vec4Array* colors = new osg::Vec4Array;
1754    geometry->setColorArray(colors);
1755    geometry->setColorBinding(osg::Geometry::BIND_PER_PRIMITIVE);
1756
1757    for(Faces::const_iterator itr = _faces.begin();
1758        itr != _faces.end();
1759        ++itr)
1760    {
1761        if( colorInside[3] > 0 ) {
1762            geometry->addPrimitiveSet( new osg::DrawArrays( GL_TRIANGLE_FAN,
1763                vertices->size(), itr->vertices.size() ) );
1764
1765            colors->push_back( colorInside );
1766        }
1767
1768        if( colorOutline[3] > 0 ) {
1769            geometry->addPrimitiveSet( new osg::DrawArrays( GL_LINE_LOOP,
1770                vertices->size(), itr->vertices.size() ) );
1771
1772            colors->push_back( colorOutline );
1773        }
1774
1775        vertices->insert
1776            ( vertices->end(), itr->vertices.begin(), itr->vertices.end() );
1777    }
1778
1779    osg::StateSet* stateset = geometry->getOrCreateStateSet();
1780    stateset->setMode(GL_LIGHTING, osg::StateAttribute::OFF);
1781    stateset->setTextureMode(0, GL_TEXTURE_2D, osg::StateAttribute::OFF);
1782    stateset->setTextureMode(1, GL_TEXTURE_2D, osg::StateAttribute::OFF);
1783
1784    return geometry;
1785}
1786////////////////////////////////////////////////////////////////////////////
1787bool ConvexPolyhedron::dumpGeometry
1788( const Face * face,
1789  const osg::Plane * plane,
1790  ConvexPolyhedron * base,
1791  const char * filename,
1792  const osg::Vec4d& colorOutline,
1793  const osg::Vec4d& colorInside,
1794  const osg::Vec4d& faceColorOutline,
1795  const osg::Vec4d& faceColorInside,
1796  const osg::Vec4d& planeColorOutline,
1797  const osg::Vec4d& planeColorInside,
1798  const osg::Vec4d& baseColorOutline,
1799  const osg::Vec4d& baseColorInside ) const
1800{
1801    osg::Group * group = new osg::Group();
1802    osg::Geode * geode = new osg::Geode();
1803    geode->getOrCreateStateSet()->setMode( GL_BLEND, osg::StateAttribute::ON );
1804    geode->getOrCreateStateSet()->setMode( GL_CULL_FACE, osg::StateAttribute::OFF );
1805    geode->getOrCreateStateSet()->setMode( GL_DEPTH_TEST, osg::StateAttribute::OFF );
1806
1807    group->addChild( geode );
1808
1809    Vertices vertices;
1810    getPoints( vertices );
1811
1812    osg::BoundingBox bb;
1813    for( unsigned int i = 0; i < vertices.size(); i++ )
1814        bb.expandBy( vertices[i] );
1815
1816    ConvexPolyhedron cp( *this ), cpFace;
1817
1818    for( Faces::iterator itr = cp._faces.begin(); itr != cp._faces.end(); )
1819    {
1820        bool found = ( face &&
1821                       itr->name == face->name &&
1822                       itr->plane == face->plane &&
1823                       itr->vertices == face->vertices );
1824#if 1
1825        if( cp.isFacePolygonConvex( *itr ) < 0 )
1826            std::reverse( itr->vertices.begin(), itr->vertices.end() );
1827#endif
1828
1829        if( found ) {
1830            cpFace.createFace() = *face;
1831            itr = cp._faces.erase( itr );
1832        } else {
1833            ++itr;
1834        }
1835    }
1836
1837    osg::Geometry * geometry = cp.buildGeometry( colorOutline, colorInside );
1838    geometry->getOrCreateStateSet()->setMode( GL_CULL_FACE, osg::StateAttribute::ON );
1839
1840    geode->addDrawable( geometry );
1841
1842    if( face )
1843        geode->addDrawable( cpFace.buildGeometry( faceColorOutline, faceColorInside ) );
1844
1845    if( plane )
1846    {
1847        ConvexPolyhedron cp;
1848        Face & face = cp.createFace();
1849        face.plane = *plane;
1850
1851        osg::Vec3d normal = face.plane.getNormal();
1852        osg::Vec3d side = fabs(normal.x()) < fabs(normal.y()) ?
1853                            osg::Vec3d(1.0, 0.0, 0.0) :
1854                            osg::Vec3d(0.0, 1.0, 0.0);
1855
1856        osg::Vec3d v = normal ^ side;
1857        v.normalize();
1858        v *= bb.radius();
1859
1860        osg::Vec3d u = v ^ normal;
1861        u.normalize();
1862        u *= bb.radius();
1863
1864        osg::Vec3d c = bb.center();
1865        c -= face.plane.getNormal() * face.plane.distance( c );
1866
1867        face.vertices.push_back( c - u - v );
1868        face.vertices.push_back( c - u + v );
1869        face.vertices.push_back( c + u + v );
1870        face.vertices.push_back( c + u - v );
1871
1872        geode->addDrawable( cp.buildGeometry( planeColorOutline, planeColorInside ) );
1873    }
1874
1875    if( base )
1876        geode->addDrawable( base->buildGeometry( baseColorOutline, baseColorInside ) );
1877
1878    return osgDB::writeNodeFile( *group, std::string( filename ) );
1879}
1880////////////////////////////////////////////////////////////////////////////
Note: See TracBrowser for help on using the browser.