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

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

<iterator>, <stdlib.h> and <ctype.h> includes required for QNX compiler

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