libsfcgal-dev 1.2.2-1 / usr / include / SFCGAL / algorithm / differencePrimitives.h

/**
 *   SFCGAL
 *
 *   Copyright (C) 2012-2013 Oslandia <infos@oslandia.com>
 *   Copyright (C) 2012-2013 IGN (http://www.ign.fr)
 *
 *   This library is free software; you can redistribute it and/or
 *   modify it under the terms of the GNU Library General Public
 *   License as published by the Free Software Foundation; either
 *   version 2 of the License, or (at your option) any later version.
 *
 *   This library is distributed in the hope that it will be useful,
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 *   Library General Public License for more details.

 *   You should have received a copy of the GNU Library General Public
 *   License along with this library; if not, see <http://www.gnu.org/licenses/>.
 */

#ifndef _SFCGAL_ALGORITHM_DIFFERENCEPRIMITIVES_H_
#define _SFCGAL_ALGORITHM_DIFFERENCEPRIMITIVES_H_

#include <SFCGAL/Exception.h>
#include <SFCGAL/triangulate/triangulatePolygon.h>
#include <SFCGAL/Polygon.h>
#include <SFCGAL/TriangulatedSurface.h>
#include <SFCGAL/detail/GeometrySet.h>

#include <CGAL/Boolean_set_operations_2.h>
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Exact_predicates_exact_constructions_kernel.h>
#include <CGAL/box_intersection_d.h>
#include <CGAL/corefinement_operations.h>
#include <SFCGAL/detail/Point_inside_polyhedron.h>



namespace SFCGAL {

namespace algorithm {

typedef CGAL::Vector_2<Kernel> Vector_2;
typedef CGAL::Point_2<Kernel> Point_2;
typedef CGAL::Segment_2<Kernel> Segment_2;
typedef CGAL::Triangle_2<Kernel> Triangle_2;
typedef CGAL::Polygon_2<Kernel> Polygon_2;
typedef CGAL::Polygon_with_holes_2<Kernel> PolygonWH_2;
typedef detail::NoVolume NoVolume;

typedef CGAL::Vector_3<Kernel> Vector_3;
typedef CGAL::Point_3<Kernel> Point_3;
typedef CGAL::Segment_3<Kernel> Segment_3;
typedef CGAL::Triangle_3<Kernel> Triangle_3;
typedef CGAL::Plane_3<Kernel> Plane_3;
typedef detail::MarkedPolyhedron MarkedPolyhedron;

CGAL::Object intersection( const CGAL::Triangle_3<Kernel>& a, const CGAL::Triangle_3<Kernel>& b );

inline
bool do_intersect( const Point_2& point, const PolygonWH_2& polygon )
{
    // point intersects if it's inside the ext ring and outside all holes

    if ( CGAL::bounded_side_2( polygon.outer_boundary().vertices_begin(),
                               polygon.outer_boundary().vertices_end(), point, Kernel() )
            == CGAL::ON_UNBOUNDED_SIDE ) {
        return false;
    }

    for ( PolygonWH_2::Hole_const_iterator hit = polygon.holes_begin();
            hit != polygon.holes_end();
            ++hit ) {
        if ( CGAL::bounded_side_2( hit->vertices_begin(),
                                   hit->vertices_end(), point, Kernel() )
                !=  CGAL::ON_UNBOUNDED_SIDE ) {
            return false;
        }
    }

    return true;
}



template < typename PointOutputIteratorType>
PointOutputIteratorType difference( const Point_2& a, const Point_2& b, PointOutputIteratorType out )
{
    if ( a != b ) {
        *out++ = a;
    }

    return out;
}

template < typename PointOutputIteratorType>
PointOutputIteratorType difference( const Point_2& a, const Segment_2& b, PointOutputIteratorType out )
{
    if ( ! CGAL::do_intersect( a, b ) ) {
        *out++ = a;
    }

    return out;
}

template < typename PointOutputIteratorType>
PointOutputIteratorType difference( const Point_2& a, const PolygonWH_2& b, PointOutputIteratorType out )
{
    if ( ! do_intersect( a, b ) ) {
        *out++ = a;
    }

    return out;
}

template < typename PointOutputIteratorType>
PointOutputIteratorType difference( const Point_2&, const NoVolume& , PointOutputIteratorType out )
{
    BOOST_ASSERT( false );
    return out;
}

template < typename SegmentOutputIteratorType>
SegmentOutputIteratorType difference( const Segment_2&, const NoVolume& , SegmentOutputIteratorType out )
{
    BOOST_ASSERT( false );
    return out;
}

template < typename SurfaceOutputIteratorType>
SurfaceOutputIteratorType difference( const PolygonWH_2&, const NoVolume& , SurfaceOutputIteratorType out )
{
    BOOST_ASSERT( false );
    return out;
}


template < typename PointOutputIteratorType>
PointOutputIteratorType difference( const Point_3& a, const Point_3& b, PointOutputIteratorType out )
{
    if ( a != b ) {
        *out++ = a;
    }

    return out;
}

template < typename PointOutputIteratorType>
PointOutputIteratorType difference( const Point_3& a, const Segment_3& b, PointOutputIteratorType out )
{
    if ( ! b.has_on( a ) ) {
        *out++ = a;
    }

    return out;
}

template < typename PointOutputIteratorType>
PointOutputIteratorType difference( const Point_3& a, const Triangle_3& b, PointOutputIteratorType out )
{
    if ( ! b.has_on( a ) ) {
        *out++ = a;
    }

    return out;
}

template < typename PointOutputIteratorType>
PointOutputIteratorType difference( const Point_3& a, const MarkedPolyhedron& b, PointOutputIteratorType out )
{
    Point_inside_polyhedron<MarkedPolyhedron, Kernel> is_in_poly( b );

    if ( CGAL::ON_UNBOUNDED_SIDE == is_in_poly( a ) ) {
        *out++ = a;
    }

    return out;
}


template < typename SegmentType , typename SegmentOrSurfaceType, typename SegmentOutputIteratorType>
SegmentOutputIteratorType difference( const SegmentType& a, const SegmentOrSurfaceType& b, SegmentOutputIteratorType out )
{
    CGAL::Object inter = CGAL::intersection( a, b );
    const SegmentType* s = CGAL::object_cast< SegmentType >( &inter );

    if ( s ) { // there maybe zero, one or two segments as a result
        if ( CGAL::squared_distance( a.source(), s->source() ) < CGAL::squared_distance( a.source(), s->target() ) ) {
            if ( a.source() != s->source() ) {
                *out++ = SegmentType( a.source(), s->source() );
            }

            if ( s->target() != a.target() ) {
                *out++ = SegmentType( s->target(), a.target() );
            }
        }
        else {
            if ( a.source() != s->target() ) {
                *out++ = SegmentType( a.source(), s->target() );
            }

            if ( s->source() != a.target() ) {
                *out++ = SegmentType( s->source(), a.target() );
            }
        }
    }
    else { // intersection is a point or nothing, leave a unchanged
        *out++ = a;
    }

    return out;
}

template<typename PointType>
struct Nearer {
    Nearer( const PointType& reference ) :_ref( reference ) {}
    bool operator()( const PointType& lhs, const PointType& rhs ) const {
        return CGAL::squared_distance( _ref, lhs ) < CGAL::squared_distance( _ref, rhs );
    }
private:
    const PointType _ref;
};

template < typename SegmentOutputIteratorType>
SegmentOutputIteratorType difference( const Segment_2& segment, const PolygonWH_2& polygon, SegmentOutputIteratorType out )
{
    // we could triangulate the polygon and substract each triangle
    //
    // we could also cut the line by polygon contours and test if the middle of the segment is inside
    // but if the segment lies on the contour it's a special case
    // we first substract the contours to take care of this
    // special case, we obtain a vector of segments,
    // for each segment of this vector, we subdivide it with the intersection
    // points with the rings
    // once done, we check, for each subdivision that has distinct end-points
    // if the middle is in or out.

    std::vector< Segment_2 > result( 1, segment );
    std::vector< Polygon_2 > rings( 1, polygon.outer_boundary() );
    rings.insert( rings.end(), polygon.holes_begin(), polygon.holes_end() );

    for ( std::vector< Polygon_2 >::iterator ring = rings.begin(); ring != rings.end(); ++ring ) {
        for ( Polygon_2::Vertex_const_iterator target = ring->vertices_begin();
                target != ring->vertices_end(); ++target ) {
            const Segment_2 sc( target == ring->vertices_begin()
                                ? *( ring->vertices_end() - 1 )
                                : *( target - 1 )
                                ,
                                *target );
            std::vector< Segment_2 > tmp;

            for ( std::vector< Segment_2 >::const_iterator s = result.begin(); s != result.end(); ++s ) {
                difference( *s, sc, std::back_inserter( tmp ) );
            }

            tmp.swap( result );
        }
    }

    for ( std::vector< Segment_2 >::const_iterator s = result.begin(); s != result.end(); ++s ) {
        std::vector< Point_2 > points( 1, s->source() );

        for ( std::vector< Polygon_2 >::iterator ring = rings.begin(); ring != rings.end(); ++ring ) {
            for ( Polygon_2::Vertex_const_iterator target = ring->vertices_begin();
                    target != ring->vertices_end(); ++target ) {
                Segment_2 sc( target == ring->vertices_begin()
                              ? *( ring->vertices_end() - 1 )
                              : *( target - 1 )
                              ,
                              *target );
                CGAL::Object inter = CGAL::intersection( *s, sc );
                const Point_2* p = CGAL::object_cast< Point_2 >( &inter );

                if ( p ) {
                    points.push_back( *p );
                }
            }
        }

        points.push_back( s->target() );
        // order point according to the distance from source
        const Nearer<Point_2> nearer( s->source() );
        std::sort( points.begin()+1, points.end()-1, nearer );

        // append segments that has length and wich midpoint is outside polygon to result
        for ( std::vector< Point_2 >::const_iterator p = points.begin(); p != points.end()-1; ++p ) {
            std::vector< Point_2 >::const_iterator q = p+1;

            if ( *p != *q && !do_intersect( CGAL::midpoint( *p,*q ), polygon ) ) {
                *out++ = Segment_2( *p, *q );
            }
        }
    }

    return out;
}

// assuming two disjoint (except at a point) polygon, test if the first a hole of the second
inline
bool isHoleOf( const Polygon_2& hole, const Polygon_2& poly )
{
    return CGAL::bounded_side_2( poly.vertices_begin(),
                                 poly.vertices_end(), *hole.vertices_begin(), Kernel() )
           == CGAL::ON_BOUNDED_SIDE ||
           CGAL::bounded_side_2( poly.vertices_begin(),
                                 poly.vertices_end(), *( hole.vertices_begin()+1 ), Kernel() )
           == CGAL::ON_BOUNDED_SIDE ;

}

template < typename PolygonOutputIteratorType>
PolygonOutputIteratorType fix_cgal_valid_polygon( const PolygonWH_2& p, PolygonOutputIteratorType out )
{
    const Polygon_2& outer = p.outer_boundary();

    //std::cerr << "in fix outer " << outer << "\n";
    if ( !outer.is_simple() ) {
        // the holes are simple, so we need to find the intersection points, then split
        // the outer ring into simple components and put holes in the right one
        // note that the hole may touch the outer boundary at a point,
        // so if the tested hole point falls on the boundary, we test the next

        std::vector< Polygon_2 > boundaries;
        std::vector< std::vector<Point_2> > stack( 1 );

        for ( Polygon_2::Vertex_const_iterator v = outer.vertices_begin();
                v != outer.vertices_end(); ++v ) {
            if ( stack.back().size() && stack.back()[0] == *v ) { // closing ring
                boundaries.push_back( Polygon_2( stack.back().begin(), stack.back().end() ) );
                stack.pop_back();
            }
            else if ( std::find( v+1, outer.vertices_end(), *v ) != outer.vertices_end() ) { //split point
                stack.back().push_back( *v );
                stack.resize( stack.size() + 1 );
                stack.back().push_back( *v );
            }
            else {
                stack.back().push_back( *v );
            }
        }

        if ( stack.size() ) {
            boundaries.push_back( Polygon_2( stack.back().begin(), stack.back().end() ) );
        }

        //std::cerr << "in fix boundaries " << boundaries.size() << "\n";

        std::vector<Polygon_2> holes( p.holes_begin(), p.holes_end() );

        // one of the boundaries may be a hole
        std::vector< Polygon_2 > cw;
        std::vector< Polygon_2 > ccw;

        for ( std::vector< Polygon_2 >::const_iterator b = boundaries.begin();
                b != boundaries.end(); ++b ) {
            if ( b->orientation() == CGAL::CLOCKWISE ) {
                cw.push_back( *b );
            }
            else {
                ccw.push_back( *b );
            }
        }

        //std::cerr << "in fix " << ccw.size() << " ccw and " << cw.size() << " cw\n";

        // if we have holes, check the orientation of the first hole to see
        // what is a hole orientation
        // if we don't have holes, we test if the first ccw is a hole of any
        // of the cw, if not, then the other are holes
        bool holesAreCCW = false;

        if ( !cw.size() ) {
            holesAreCCW = false;
        }
        else if ( !ccw.size() ) {
            holesAreCCW = true;
        }
        else if ( holes.size() ) {
            holesAreCCW = holes[0].orientation() != CGAL::CLOCKWISE;
        }
        else {
            for ( std::vector< Polygon_2 >::const_iterator b = cw.begin();
                    b != boundaries.end(); ++b ) {
                if ( isHoleOf( ccw[0], *b ) ) {
                    holesAreCCW = true;
                    break;
                }
            }
        }

        if ( holesAreCCW ) {
            holes.insert( holes.end(), ccw.begin(), ccw.end() );
            boundaries.swap( cw );
        }
        else {
            holes.insert( holes.end(), cw.begin(), cw.end() );
            boundaries.swap( ccw );
        }

        std::vector< std::vector< Polygon_2 > > sortedHoles( boundaries.size() ); // 1/1 with boudaries

        unsigned nbHoles = 0;

        for ( std::vector< Polygon_2 >::const_iterator h = holes.begin(); h != holes.end(); ++h ) {
            ++nbHoles;

            for ( std::vector< Polygon_2 >::const_iterator b = boundaries.begin();
                    b != boundaries.end(); ++b ) {
                if ( isHoleOf( *h, *b ) ) {
                    sortedHoles[ b - boundaries.begin() ].push_back( *h );
                }
            }
        }

        for ( unsigned i = 0; i < boundaries.size(); i++ ) {
            *out++ = PolygonWH_2( boundaries[i], sortedHoles[i].begin(), sortedHoles[i].end() );
        }

        //std::cerr << "extracted " << boundaries.size() << " boundaries, dispatched " << nbHoles << " holes \n";
    }
    else {
        *out++ = p;
    }

    return out;
}

inline
PolygonWH_2
fix_sfs_valid_polygon( const PolygonWH_2& p )
{
    CGAL::Gps_segment_traits_2<Kernel> traits;

    if ( are_holes_and_boundary_pairwise_disjoint( p, traits ) ) {
        return p;
    }

    // a polygon is valid for sfs and invalid for CGAL when two rings intersect
    // on a point that is not a ring vertex
    // we add this vertex to fix the polygon
    // for each ring segment
    //    for every other ring point
    //        add point to segment

    // put all rings in a vector to avoid distinction between outer and holes
    std::vector< Polygon_2 > rings( 1, p.outer_boundary() );
    rings.insert( rings.end(), p.holes_begin(), p.holes_end() );

    std::vector< Polygon_2 > out;

    for ( std::vector< Polygon_2 >::iterator ring = rings.begin(); ring != rings.end(); ++ring ) {
        out.push_back( Polygon_2() );

        for ( Polygon_2::Vertex_const_iterator target = ring->vertices_begin(); target != ring->vertices_end(); ++target ) {
            Segment_2 segment(
                target == ring->vertices_begin()
                ? *( ring->vertices_end() - 1 )
                : *( target - 1 )
                ,
                *target );

            // for every other ring
            for ( std::vector< Polygon_2 >::const_iterator other = rings.begin(); other != rings.end(); ++other ) {
                if ( ring == other ) {
                    continue;
                }

                for ( Polygon_2::Vertex_const_iterator vertex = other->vertices_begin();
                        vertex != other->vertices_end(); ++vertex ) {
                    if ( CGAL::do_intersect( *vertex, segment ) ) {
                        out.back().push_back( *vertex );
                    }
                }
            }

            out.back().push_back( *target );
        }
    }

    return PolygonWH_2( out[0], out.begin()+1, out.end() );
}

template < typename OutputIteratorType >
OutputIteratorType difference( const Triangle_3& p, const Triangle_3& q, OutputIteratorType out )
{

    const Plane_3 plane = p.supporting_plane();

    if ( plane != q.supporting_plane() || !CGAL::do_intersect( p,q ) ) {
        *out++ = p;
    }
    else {
        // project on plane
        // difference between polygons
        // triangulate the result

        PolygonWH_2 pProj, qProj;

        for ( unsigned i=0; i<3; i++ ) {
            pProj.outer_boundary().push_back( plane.to_2d( p.vertex( i ) ) );
            qProj.outer_boundary().push_back( plane.to_2d( q.vertex( i ) ) );
        }

        std::vector< PolygonWH_2 > res;
        difference( pProj, qProj, std::back_inserter( res ) );


        for ( std::vector< PolygonWH_2 >::const_iterator i = res.begin(); i != res.end(); ++i ) {
            const Polygon poly( *i );
            TriangulatedSurface ts;
            triangulate::triangulatePolygon3D( poly, ts );

            for ( TriangulatedSurface::iterator t = ts.begin(); t != ts.end(); ++t ) {
                *out++ = Triangle_3( plane.to_3d( t->vertex( 0 ).toPoint_2() ),
                                     plane.to_3d( t->vertex( 1 ).toPoint_2() ),
                                     plane.to_3d( t->vertex( 2 ).toPoint_2() ) ) ;
            }
        }
    }

    return out;
}

template < typename VolumeOutputIteratorType>
VolumeOutputIteratorType difference( const MarkedPolyhedron& a, const MarkedPolyhedron& b, VolumeOutputIteratorType out )
{
    MarkedPolyhedron& p = const_cast<MarkedPolyhedron&>( a );
    MarkedPolyhedron& q = const_cast<MarkedPolyhedron&>( b );
    typedef CGAL::Polyhedron_corefinement<MarkedPolyhedron> Corefinement;
    Corefinement coref;
    CGAL::Emptyset_iterator no_polylines;
    typedef std::vector<std::pair<MarkedPolyhedron*, int> >  ResultType;
    ResultType result;
    coref( p, q, no_polylines, std::back_inserter( result ), Corefinement::P_minus_Q_tag );

    for ( ResultType::iterator it = result.begin(); it != result.end(); it++ ) {
        *out++ = *it->first;
        delete it->first;
    }

    return out;
}

typedef  CGAL::Box_intersection_d::Box_with_handle_d<double,3,MarkedPolyhedron::Halfedge_around_facet_const_circulator> FaceBboxBase;


struct FaceBbox: FaceBboxBase {
    struct Bbox: CGAL::Bbox_3 {
        Bbox( MarkedPolyhedron::Halfedge_around_facet_const_circulator handle )
            : CGAL::Bbox_3( handle->vertex()->point().bbox() ) {
            const MarkedPolyhedron::Halfedge_around_facet_const_circulator end = handle;

            do {
                // @note with CGAL 4.5 you would write simply
                // *this += (++handle)->vertex()->point().bbox();
                this->CGAL::Bbox_3::operator=( *this + ( ++handle )->vertex()->point().bbox() );
            }
            while ( handle != end );
        }
    };

    FaceBbox( const MarkedPolyhedron::Facet& facet )
        : FaceBboxBase( Bbox( facet.facet_begin() ), facet.facet_begin() ) {
    }
};

struct FaceSegmentCollide {
    typedef std::vector< MarkedPolyhedron::Halfedge_around_facet_const_circulator > CollisionVector;
    FaceSegmentCollide( CollisionVector& list ): _list( list ) {}
    void operator()( const FaceBboxBase&, const FaceBboxBase& face ) {
        _list.push_back( face.handle() );
    }
private:
    CollisionVector& _list;
};

template < typename TriangleOutputIteratorType>
TriangleOutputIteratorType collidingTriangles( const FaceSegmentCollide::CollisionVector& collisions, TriangleOutputIteratorType out )
{
    for ( FaceSegmentCollide::CollisionVector::const_iterator cit = collisions.begin();
            cit != collisions.end(); ++cit ) {
        MarkedPolyhedron::Halfedge_around_facet_const_circulator it = *cit;
        std::vector< Point > points( 1, it->vertex()->point() );

        do {
            points.push_back( ( ++it )->vertex()->point() );
        }
        while ( it != *cit );

        if ( points.size() == 3 ) {
            *out++ = Triangle_3( points[0].toPoint_3(), points[1].toPoint_3(), points[2].toPoint_3() ) ;
        }
        else {
            const Polygon poly( points );
            TriangulatedSurface ts;
            triangulate::triangulatePolygon3D( poly, ts );

            for ( TriangulatedSurface::iterator t = ts.begin(); t != ts.end(); ++t ) {
                *out++ = Triangle_3( t->vertex( 0 ).toPoint_3(),
                                     t->vertex( 1 ).toPoint_3(),
                                     t->vertex( 2 ).toPoint_3() );
            }
        }
    }

    return out;
}


template < typename SegmentOutputIteratorType>
SegmentOutputIteratorType difference( const Segment_3& segment, const MarkedPolyhedron& polyhedron, SegmentOutputIteratorType out )
{
    // this is a bit of a pain
    // the algo should follow the same lines as the Segment_2 - PolygonWH_2
    // namely, remove the pieces of the segment were it touches facets,
    // then compute the intersections with facets to cut the segments and
    // create segments for output were the middle point is inside
    //
    // to speed thing up we put facets in AABB-Tree

    std::vector< FaceBbox > bboxes( polyhedron.facets_begin(), polyhedron.facets_end() );
    std::vector< FaceBboxBase > bbox( 1, FaceBboxBase( segment.bbox(),polyhedron.facets_begin()->facet_begin() ) ); // nevermind the facet handle, it's not used anyway
    FaceSegmentCollide::CollisionVector collisions;
    FaceSegmentCollide cb( collisions );
    CGAL::box_intersection_d( bbox.begin(), bbox.end(),
                              bboxes.begin(), bboxes.end(),
                              cb );

    if ( !collisions.size() ) {
        // completely in or out, we just test one point
        Point_inside_polyhedron<MarkedPolyhedron, Kernel> is_in_poly( polyhedron );

        if ( CGAL::ON_UNBOUNDED_SIDE == is_in_poly( segment.source() ) ) {
            *out++ = segment;
        }
    }
    else {
        std::vector< Triangle_3 > triangles;
        collidingTriangles( collisions, std::back_inserter( triangles ) );

        // first step, substract faces
        std::vector< Segment_3 > res1( 1, segment );

        for ( std::vector< Triangle_3 >::const_iterator tri=triangles.begin();
                tri != triangles.end(); ++tri ) {
            std::vector< Segment_3 > tmp;

            for ( std::vector< Segment_3 >::const_iterator seg = res1.begin();
                    seg != res1.end(); ++seg ) {
                difference( *seg, *tri, std::back_inserter( tmp ) );
            }

            res1.swap( tmp );
        }

        // second step, for each segment, add intersection points and test each middle point
        // to know if it's in or out
        for ( std::vector< Segment_3 >::const_iterator seg = res1.begin();
                seg != res1.end(); ++seg ) {
            std::vector< Point_3 > points( 1, seg->source() );

            for ( std::vector< Triangle_3 >::const_iterator tri=triangles.begin();
                    tri != triangles.end(); ++tri ) {
                CGAL::Object inter = CGAL::intersection( *seg, *tri );
                const Point_3* p = CGAL::object_cast< Point_3 >( &inter );

                if ( p ) {
                    points.push_back( *p );
                }
            }

            points.push_back( seg->target() );
            // order point according to the distance from source

            const Nearer<Point_3> nearer( seg->source() );
            std::sort( points.begin()+1, points.end()-1, nearer );

            Point_inside_polyhedron<MarkedPolyhedron, Kernel> is_in_poly( polyhedron );

            // append segments that has length and wich midpoint is outside polyhedron to result
            for ( std::vector< Point_3 >::const_iterator p = points.begin(); p != points.end()-1; ++p ) {
                std::vector< Point_3 >::const_iterator q = p+1;

                if ( *p != *q && CGAL::ON_UNBOUNDED_SIDE == is_in_poly( CGAL::midpoint( *p,*q ) ) ) {
                    *out++ = Segment_3( *p, *q );
                }
            }
        }
    }

    return out;
}

// @TODO put that in a proper header
void _intersection_solid_triangle( const MarkedPolyhedron& pa, const Triangle_3& tri, detail::GeometrySet<3>& output );


template < typename TriangleOutputIteratorType>
TriangleOutputIteratorType difference( const Triangle_3& triangle, const MarkedPolyhedron& polyhedron, TriangleOutputIteratorType out )
{
    std::vector< Triangle_3 > inter;
    // call _intersection_solid_triangle
    detail::GeometrySet<3> interSet;
    _intersection_solid_triangle( polyhedron, triangle, interSet );

    for ( detail::GeometrySet<3>::SurfaceCollection::const_iterator it = interSet.surfaces().begin();
            it != interSet.surfaces().end(); ++it ) {
        inter.push_back( it->primitive() );
    }



    std::vector< Triangle_3 > res( 1, triangle );

    // GOTCHA for intersection points (volume touching triangle) , need to retriangulate
    for ( detail::GeometrySet<3>::PointCollection::const_iterator it = interSet.points().begin();
            it != interSet.points().end(); ++it ) {
        std::vector< Triangle_3 > tmp;

        for ( std::vector< Triangle_3 >::const_iterator tri = res.begin(); tri != res.end(); ++tri ) {
            const Point_3 p( it->primitive() );

            for ( int s = 0; s<3; s++ ) {
                if ( p != tri->vertex( s ) &&  p != tri->vertex( ( s+1 )%3 )
                        && Segment_3( tri->vertex( s ), tri->vertex( ( s+1 )%3 ) ).has_on( p ) ) {
                    tmp.push_back( Triangle_3( tri->vertex( s ), p, tri->vertex( ( s+2 )%3 ) ) );
                    tmp.push_back( Triangle_3( p, tri->vertex( ( s+1 )%3 ), tri->vertex( ( s+2 )%3 ) ) );
                }
            }
        }

        tmp.swap( res );
    }


    for ( std::vector< Triangle_3 >::const_iterator it = inter.begin(); it != inter.end(); ++it ) {
        std::vector< Triangle_3 > tmp;

        for ( std::vector< Triangle_3 >::const_iterator tri = res.begin(); tri != res.end(); ++tri ) {
            difference( *tri, *it, std::back_inserter( tmp ) );
        }

        tmp.swap( res );
    }

    for ( std::vector< Triangle_3 >::const_iterator tri = res.begin(); tri != res.end(); ++tri ) {
        *out++ = *tri;
    }

    return out;

    /*
        std::vector< FaceBbox > bboxes(polyhedron.facets_begin(), polyhedron.facets_end() );
        std::vector< FaceBboxBase > bbox( 1, FaceBboxBase(triangle.bbox(),polyhedron.facets_begin()->facet_begin()) ); // nevermind the facet handle, it's not used anyway
        FaceSegmentCollide::CollisionVector collisions;
        FaceSegmentCollide cb(collisions);
        CGAL::box_intersection_d( bbox.begin(), bbox.end(),
                                  bboxes.begin(), bboxes.end(),
                                  cb );

        if ( !collisions.size() ){
            // completely in or out, we just test one point
            CGAL::Point_inside_polyhedron_3<MarkedPolyhedron, Kernel> is_in_poly( polyhedron );
            if ( CGAL::ON_UNBOUNDED_SIDE == is_in_poly( triangle.vertex(0) ) ) *out++ = triangle;
        }
        else {
            // now we first transform bboxes colliding faces into triangles
            // then we test for intersection and store resulting segments in a vector
            // we also store resulting polygons as segments
            //
            // we need to convert the resulting segments to a multipolygon of sort
            //
            // finally we triangulate the result and substract those triangles
            //
            std::vector< Triangle_3 > interTriangles;
            collidingTriangles( collisions, std::back_inserter( interTriangles ) );

            std::vector< Segment_3 > intersectionCountours;

            BOOST_THROW_EXCEPTION(NotImplementedException("Triangle_3 - Volume is not implemented") );
        }
        return out;
    */
}

template < typename PolygonOutputIteratorType>
PolygonOutputIteratorType difference( const PolygonWH_2& a, const PolygonWH_2& b, PolygonOutputIteratorType out )
{
    CGAL::Gps_segment_traits_2<Kernel> traits;

    std::vector< PolygonWH_2 > temp;
    CGAL::difference(
        are_holes_and_boundary_pairwise_disjoint( a, traits ) ? a : fix_sfs_valid_polygon( a ),
        are_holes_and_boundary_pairwise_disjoint( b, traits ) ? b : fix_sfs_valid_polygon( b ),
        std::back_inserter( temp ) );

    // polygon outer rings from difference can self intersect at points
    // therefore we need to split the generated polygons so that they are valid for SFS
    for ( std::vector< PolygonWH_2 >::const_iterator poly=temp.begin(); poly!=temp.end(); ++poly  ) {
        out = fix_cgal_valid_polygon( *poly, out );
    }

    return out;
}


}
}
#endif