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

Revision 13041, 61.5 kB (checked in by robert, 3 years ago)

Ran script to remove trailing spaces and tabs

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