CG_elem.cpp

/*---------------------------------------------------------------------------*\
 *
 *  bitpit
 *
 *  Copyright (C) 2015-2021 OPTIMAD engineering Srl
 *
 *  -------------------------------------------------------------------------
 *  License
 *  This file is part of bitpit.
 *
 *  bitpit is free software: you can redistribute it and/or modify it
 *  under the terms of the GNU Lesser General Public License v3 (LGPL)
 *  as published by the Free Software Foundation.
 *
 *  bitpit 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 Lesser General Public
 *  License for more details.
 *
 *  You should have received a copy of the GNU Lesser General Public License
 *  along with bitpit. If not, see <http://www.gnu.org/licenses/>.
 *
\*---------------------------------------------------------------------------*/
 
# include <cmath>
# include <memory>
# include <string>
# include <iostream>
 
# include <assert.h>
 
# include "bitpit_operators.hpp"
 
# include "CG.hpp"
# include "CG_private.hpp"
 
 
namespace bitpit{
 
namespace CGElem{
 
 
void _projectPointsTriangle( int nPoints, array3D const *points, array3D const &Q0, array3D const &Q1, array3D const &Q2, array3D *proj, double *lambda )
{
    assert( validTriangle(Q0,Q1,Q2) );
 
    array3D v0 = Q1 - Q0;
    array3D v1 = Q2 - Q0;
 
    array3D n = crossProduct(v0, v1);
    double n2 = dotProduct(n, n);
 
    for( int i=0; i<nPoints; ++i){
        array3D v2 = points[i] - Q0;
 
        double *pointLambda = lambda + 3 * i;
        pointLambda[2] = dotProduct(crossProduct(v0, v2), n) / n2;
        pointLambda[1] = dotProduct(crossProduct(v2, v1), n) / n2;
        pointLambda[0] = 1. - pointLambda[1] - pointLambda[2];
 
        array3D *pointProjections = proj + i;
        *pointProjections = restrictPointTriangle( Q0, Q1, Q2, pointLambda);
    }
}
 
bool _intersectBoxTriangle(array3D const &A0, array3D const &A1, array3D const &V0, array3D const &V1, array3D const &V2, bool interiorTriangleVertices, bool triangleEdgeBoxHullIntersections, bool triangleBoxEdgeIntersections, std::vector<array3D> *intrPtr, std::vector<int> *flagPtr, int dim, const double distanceTolerance)
{
 
    bool intersect(false);
    bool addFlag( flagPtr!=nullptr);
    bool computeIntersection(interiorTriangleVertices||triangleBoxEdgeIntersections||triangleEdgeBoxHullIntersections);
 
    assert( ! (computeIntersection && (intrPtr==nullptr) ) );
 
    if(computeIntersection){
        intrPtr->clear();
    }
 
    if(addFlag){
        flagPtr->clear();
    }
 
    //check if Triangle Boundig Box and Box overlap -> necessary condition
    array3D B0, B1;
    computeAABBTriangle( V0, V1, V2, B0, B1);
 
    if( !intersectBoxBox( A0, A1, B0, B1, dim, distanceTolerance) ) {
        return false;
    }
 
    //check if triangle vertices are within the box
    for( int i=0; i<3; ++i){
        vertexOfTriangle(i, V0, V1, V2, B0);
        if( intersectPointBox(B0, A0, A1, dim, distanceTolerance) ){
            intersect = true;
            if(!interiorTriangleVertices) break;
 
            intrPtr->push_back(B0);
            if(addFlag) flagPtr->push_back(0);
        }
    }
 
    //check if triangle edges and box faces intersect
    if( !intersect || triangleEdgeBoxHullIntersections) {
 
        for( int edge=0; edge<3; ++edge){
 
            edgeOfTriangle(edge, V0, V1, V2, B0, B1);
 
            if(dim==2){
                array3D p;
                array3D faceVertex0, faceVertex1;
 
                for( int face=0; face<4; ++face){
                    edgeOfBox( face, A0, A1, faceVertex0, faceVertex1);
 
                    if( intersectSegmentSegment(B0, B1, faceVertex0, faceVertex1, p, distanceTolerance) ){
                        intersect=true;
                        if(!triangleEdgeBoxHullIntersections) break;
 
                        intrPtr->push_back(p);
                        if(addFlag) flagPtr->push_back(1);
                    }
 
                }
 
            } else if(dim==3){
                array3D p;
                std::array<array3D, 4> V;
 
                for( int face=0; face<6; ++face){
                    faceOfBox( face, A0, A1, V[0], V[1], V[2], V[3] );
 
                    if( intersectSegmentPolygon( B0, B1, V.size(), V.data(), p, distanceTolerance ) ) {
                        intersect=true;
                        if(!triangleEdgeBoxHullIntersections) break;
 
                        intrPtr->push_back(p);
                        if(addFlag) flagPtr->push_back(1);
                    }
                }
            }
        }
    }
 
    //check if triangle and box edges (dim=3) or box vertices (dim=2) intersect
    if( !intersect || triangleBoxEdgeIntersections ) {
 
        array3D p;
 
        if(dim==2){
            for( int i=0; i<4; ++i){
                vertexOfBox( i, A0, A1, B0);
                if( intersectPointTriangle(B0,V0,V1,V2,distanceTolerance)) {
                    intersect = true;
                    if(!triangleBoxEdgeIntersections) break;
 
                    intrPtr->push_back(B0);
                    if(addFlag) flagPtr->push_back(2);
                }
            }
 
        } else if(dim==3){
            for( int i=0; i<12; ++i){
                edgeOfBox( i, A0, A1, B0, B1);
                if( intersectSegmentTriangle(B0,B1,V0,V1,V2,p,distanceTolerance)) {
                    intersect = true;
                    if(!triangleBoxEdgeIntersections) break;
 
                    intrPtr->push_back(p);
                    if(addFlag) flagPtr->push_back(2);
                }
            }
        }
    }
 
    return intersect;
}
 
bool _intersectSegmentBox(array3D const &V0, array3D const &V1, array3D const &A0, array3D const &A1, bool interiorSegmentVertice, bool segmentBoxHullIntersection, std::vector<array3D> *intrPtr, std::vector<int> *flagPtr, int dim, const double distanceTolerance)
{
 
    bool intersect(false);
 
    bool addFlag(flagPtr!=nullptr);
    bool computeIntersection(interiorSegmentVertice||segmentBoxHullIntersection);
 
    assert( ! (computeIntersection && (intrPtr==nullptr) ) );
 
    if(computeIntersection){
        intrPtr->clear();
    }
 
    if(addFlag){
        flagPtr->clear();
    }
 
    array3D p, B0, B1;
 
    //check if segment boundig box and Box overlap
    computeAABBSegment( V0, V1, B0, B1);
    if( !intersectBoxBox( A0, A1, B0, B1, dim, distanceTolerance) ) {
        return false;
    }
 
    //check segment points
    for( int i=0; i<2; ++i){
        vertexOfSegment(i, V0, V1, B0);
 
        if( intersectPointBox(B0,A0,A1,dim,distanceTolerance) ){
            intersect = true;
            if(!interiorSegmentVertice) break;
 
            intrPtr->push_back(B0);
            if(addFlag) flagPtr->push_back(0);
        }
    }
 
    //check if segment intersects outer hull of box
    if( !intersect || segmentBoxHullIntersection){
        if( dim == 2){ //check if box edge and segment intersect
 
            for( int i=0; i<4; ++i){
                edgeOfBox( i, A0, A1, B0, B1);
 
                if( intersectSegmentSegment(B0,B1,V0,V1,p,distanceTolerance)) {
                    intersect = true;
                    if(!segmentBoxHullIntersection) break;
 
                    intrPtr->push_back(p);
                    if(addFlag) flagPtr->push_back(1);
                }
            }
 
 
        } else if( dim==3 ) { //3D check if box face and segment intersect
 
            std::array<array3D, 4> E;
 
            for( int i=0; i<6; ++i){
                faceOfBox( i, A0, A1, E[0], E[1], E[2], E[3]);
 
                if( intersectSegmentPolygon(V0,V1,E.size(),E.data(),p,distanceTolerance) ) {
                    intersect = true;
                    if(!segmentBoxHullIntersection) break;
 
                    intrPtr->push_back(p);
                    if(addFlag) flagPtr->push_back(1);
                }
            }
        }
    }
 
    return intersect;
 
}
 
bool _intersectPlaneBox(array3D const &P, array3D const &N, array3D const &A0, array3D const &A1, std::vector<array3D> *intrPtr, int dim, const double distanceTolerance)
{
 
    bool intersect(false);
    bool computeIntersection(intrPtr);
 
    if(computeIntersection){
        intrPtr->clear();
    } else if( intersectPointBox(P,A0,A1,dim,distanceTolerance)) {
        return true;
    }
 
    // check if plane intersects the edges of the box
    // and store eventually intersection points
    int edgeCount = (dim==2) ? 4 : 12;
    array3D E0, E1, V;
 
    for(int i=0; i<edgeCount; ++i){
        edgeOfBox(i, A0, A1, E0, E1);
 
        if( intersectSegmentPlane( E0, E1, P, N, V, distanceTolerance) ){
            intersect = true;
 
            if(intrPtr){
                intrPtr->push_back(V);
            } else {
                break;
            }
        }
    }
 
 
    // sort intersection points in couterclock-wise sense
    if( dim==3 && intrPtr && intersect){
 
        const array3D origin = intrPtr->at(0);
 
        std::sort(intrPtr->begin(), intrPtr->end(), [&](const array3D &lhs, const array3D &rhs) -> bool {
            array3D v = crossProduct(lhs-origin,rhs-origin);
            return dotProduct(v, N) < 0;
        } );
    }
 
    return intersect;
}
 
bool _intersectBoxPolygon(array3D const &A0, array3D const &A1, std::size_t nVS, array3D const *VS, bool innerPolygonPoints, bool polygonEdgeBoxHullIntersection, bool polygonBoxEdgeIntersections, std::vector<array3D> *intrPtr, std::vector<int> *flagPtr, int dim, const double distanceTolerance)
{
 
    bool intersect(false);
    bool addFlag(flagPtr!=nullptr);
    bool computeIntersection(innerPolygonPoints || polygonEdgeBoxHullIntersection || polygonBoxEdgeIntersections);
 
    assert( ! (computeIntersection && (intrPtr==nullptr) ) );
 
    if(computeIntersection){
        intrPtr->clear();
    }
 
    if(addFlag){
        flagPtr->clear();
    }
 
    array3D V0, V1, V2;
 
    //check if simplex boundig box and box overlap -> necessary condition
    computeAABBPolygon( nVS, VS, V0, V1);
    if( !intersectBoxBox( A0, A1, V0, V1, dim, distanceTolerance) ) {
        return false;
    }
 
    std::vector<array3D> partialIntr;
    std::vector<int> partialFlag;
 
    // check if triangle vertices lie within box
    // or if triangles intersect edges of box
    computeIntersection = innerPolygonPoints || polygonBoxEdgeIntersections;
    int trianglesCount = polygonSubtriangleCount(nVS, VS);
    for (int triangle=0; triangle<trianglesCount; ++triangle) {
        subtriangleOfPolygon(triangle, nVS, VS, V0, V1, V2);
 
        if( _intersectBoxTriangle( A0, A1, V0, V1, V2, innerPolygonPoints, false, polygonBoxEdgeIntersections, &partialIntr, &partialFlag, dim, distanceTolerance ) ){
 
            intersect = true;
            if(!computeIntersection) break;
 
            int intrCount = partialIntr.size();
            for( int i=0; i<intrCount; ++i){
                // Get the intersection point
                //
                // Polygon triangulation adds a dummy vertex at the centroid of the polygon (V0).
                // This vertex should not be taken into account for intersection identification.
                array3D &candidateCoord = partialIntr[i];
                if (utils::DoubleFloatingEqual()(norm2(V0 - candidateCoord), 0., distanceTolerance )) {
                    continue;
                }
 
                int candidateFlag = partialFlag[i];
 
                //prune duplicate points
                auto PItr = intrPtr->begin();
                bool iterate = (PItr!=intrPtr->end());
                while(iterate){
 
                    iterate = !utils::DoubleFloatingEqual()( norm2( *PItr -candidateCoord ), 0., distanceTolerance );
 
                    if(iterate){
                        ++PItr;
                    }
                    iterate &= PItr!=intrPtr->end();
                }
 
                if(PItr!=intrPtr->end()){
                    continue;
                }
 
                intrPtr->push_back(candidateCoord);
                if(addFlag){
                    flagPtr->push_back(candidateFlag);
                }
            }
        }
    }
 
    // check if edges of polygon intersect box face
    computeIntersection = polygonEdgeBoxHullIntersection;
    int edgesCount = polygonEdgesCount(nVS, VS);
    if(!intersect || polygonEdgeBoxHullIntersection){
        for (int edge=0; edge<edgesCount; ++edge) {
            edgeOfPolygon(edge, nVS, VS, V0, V1);
 
            if( _intersectSegmentBox( V0, V1, A0, A1, false, polygonEdgeBoxHullIntersection, &partialIntr, &partialFlag, dim, distanceTolerance) ){
 
                intersect = true;
                if(!computeIntersection) break;
 
                int intrCount = partialIntr.size();
                for( int i=0; i<intrCount; ++i){
 
                    array3D &candidateCoord = partialIntr[i];
                    int candidateFlag = partialFlag[i];
 
                    //prune duplicate points
                    auto PItr = intrPtr->begin();
                    bool iterate = (PItr!=intrPtr->end());
                    while(iterate){
 
                        iterate = !utils::DoubleFloatingEqual()( norm2( *PItr -candidateCoord ), 0., distanceTolerance );
 
                        if(iterate){
                            ++PItr;
                        }
                        iterate &= PItr!=intrPtr->end();
                    }
 
                    if(PItr!=intrPtr->end()){
                        continue;
                    }
 
                    intrPtr->push_back(candidateCoord);
                    if(addFlag){
                        flagPtr->push_back(candidateFlag);
                    }
                }
            }
        }
    }
 
    return intersect;
}
 
bool validSegment(const array3D &P0, const array3D &P1 )
{
    return !utils::DoubleFloatingEqual()( norm2(P1-P0), 0.);
}
 
bool validLine(const array3D &P, const array3D &n )
{
    BITPIT_UNUSED(P);
    return utils::DoubleFloatingEqual()( norm2(n), 1.);
}
 
bool validPlane(const array3D &P, const array3D &n )
{
    BITPIT_UNUSED(P);
    return utils::DoubleFloatingEqual()( norm2(n), 1.);
}
 
bool validTriangle(const array3D &P0, const array3D &P1, const array3D &P2 )
{
 
    array3D v0 = P1 -P0;
    if( utils::DoubleFloatingEqual()( norm2(v0), 0.) ){
        return false;
    }
 
    array3D v1 = P2 -P1;
    if( utils::DoubleFloatingEqual()( norm2(v1), 0.) ){
        return false;
    }
 
    array3D v2 = P0 -P2;
    if( utils::DoubleFloatingEqual()( norm2(v2), 0.) ){
        return false;
    }
 
    array3D n = crossProduct( v0, -1.*v2);
    if( utils::DoubleFloatingEqual()( norm2(n), 0. ) ){
        return false;
    }
 
    return true;
}
 
bool validBarycentric(double const *lambdaPtr, int n )
{
 
    double sum(-1.);
    double maxValue(0.);
 
    for(int i=0; i<n; ++i){
 
        double value = lambdaPtr[i];
        if( std::abs(value) > std::abs(maxValue) ){
            sum -= maxValue;
            maxValue = -value;
 
        } else {
            sum += value;
 
        }
 
    }
 
    double tolerance = std::max(1., std::abs(maxValue)) * std::numeric_limits<double>::epsilon();
    return utils::DoubleFloatingEqual()(sum, maxValue, tolerance);
}
 
int convertBarycentricToFlagSegment( std::array<double,2> const &lambda, double tolerance)
{
 
    return convertBarycentricToFlagSegment( lambda.data(), tolerance);
}
 
int convertBarycentricToFlagSegment( const double *lambda, double tolerance)
{
 
    assert( validBarycentric(&lambda[0],2) );
 
    if ( lambda[0] > 1. || utils::DoubleFloatingEqual()( lambda[0], 1., tolerance ) ) {
        return 1;
 
    } else if ( lambda[1] > 1. || utils::DoubleFloatingEqual()( lambda[1], 1., tolerance ) ) {
        return 2;
 
    }
 
    return 0;
}
 
int convertBarycentricToFlagTriangle( array3D const &lambda, double tolerance)
{
    return convertBarycentricToFlagPolygon( 3, lambda.data(), tolerance);
}
 
int convertBarycentricToFlagTriangle( const double *lambda, double tolerance)
{
    return convertBarycentricToFlagPolygon( 3, lambda, tolerance);
}
 
int convertBarycentricToFlagPolygon( std::vector<double> const &lambda, double tolerance)
{
    return convertBarycentricToFlagPolygon( lambda.size(), lambda.data(), tolerance);
}
 
int convertBarycentricToFlagPolygon( std::size_t nLambda, double const *lambda, double tolerance)
{
 
    assert( validBarycentric(&lambda[0],nLambda) );
 
    int count = 0;
    std::size_t lastPositive = std::numeric_limits<std::size_t>::max();
    for( std::size_t i=0; i<nLambda; ++i){
        if ( lambda[i] < 0. || utils::DoubleFloatingEqual()( lambda[i], 0., tolerance ) ) {
            continue;
        }
 
        ++count;
        if (count > 2) {
            return 0;
        }
 
        if (lastPositive != 0 || i == 1) {
            lastPositive = i;
        }
    }
 
    if( count == 1){
        count = lastPositive + 1;
 
    } else if (count==2) {
        if (lastPositive != 0) {
            count = - static_cast<int>(lastPositive);
        } else {
            count = - static_cast<int>(nLambda);
        }
 
    }
 
    return count;
}
 
void computeGeneralizedBarycentric( array3D const &p, std::vector<array3D> const &vertex, std::vector<double> &lambda)
{
    computeGeneralizedBarycentric( p, vertex.size(), vertex.data(), lambda);
}
 
void computeGeneralizedBarycentric( array3D const &p, std::size_t nVertices, array3D const *vertex, std::vector<double> &lambda)
{
    lambda.resize(nVertices);
 
    computeGeneralizedBarycentric( p, nVertices, vertex, lambda.data());
}
 
void computeGeneralizedBarycentric( array3D const &p, std::size_t nVertices, array3D const *vertex, double *lambda)
{
    const int MAX_ELEM_VERTICES = 8;
    BITPIT_CREATE_WORKSPACE(area, double, nVertices, MAX_ELEM_VERTICES);
 
    for( std::size_t i=0; i<nVertices; ++i){
        int next = (i +1) %nVertices;
        area[i] = areaTriangle( vertex[i], vertex[next], p);
    }
 
    double sumWeight(0);
 
    for( std::size_t i=0; i<nVertices; ++i){
        std::size_t prev = (i +nVertices -1) %nVertices;
        std::size_t next = (i +1) %nVertices;
        lambda[i]  = areaTriangle(vertex[prev], vertex[i], vertex[next]);
 
        for( std::size_t j=0; j<nVertices; ++j){
            if( j==prev || j==i){
                continue;
            }
 
            lambda[i] *= area[j];
        }
 
        sumWeight += lambda[i];
    }
 
    for( std::size_t i=0; i<nVertices; ++i){
        lambda[i] /= sumWeight;
    }
}
 
array3D reconstructPointFromBarycentricSegment(array3D const &Q0, array3D const &Q1, std::array<double,2> const &lambda)
{
    assert( validBarycentric(&lambda[0],2) );
 
    return lambda[0]*Q0 +lambda[1]*Q1;
}
 
array3D reconstructPointFromBarycentricSegment(array3D const &Q0, array3D const &Q1, double const *lambda)
{
    assert( validBarycentric(&lambda[0],2) );
 
    return lambda[0]*Q0 +lambda[1]*Q1;
}
 
array3D reconstructPointFromBarycentricTriangle(array3D const &Q0, array3D const &Q1, array3D const &Q2, array3D const &lambda)
{
    assert( validBarycentric(&lambda[0],3) );
 
    return lambda[0]*Q0 +lambda[1]*Q1 +lambda[2]*Q2;
}
 
array3D reconstructPointFromBarycentricTriangle(array3D const &Q0, array3D const &Q1, array3D const &Q2, double const *lambda)
{
    assert( validBarycentric(&lambda[0],3) );
 
    return lambda[0]*Q0 +lambda[1]*Q1 +lambda[2]*Q2;
}
 
array3D rotatePoint(const array3D &P, const array3D &n0, const array3D &n1, double angle)
{
    // Check if the axis is valid
    double axisNorm = norm2(n1 - n0);
    if (utils::DoubleFloatingEqual()(axisNorm, 0.)) {
        return P;
    }
 
    // Transform point to a coordinates system centered in n0
    array3D Q = P - n0;
 
    // Rotate the point
    std::array<double, 3> axis = (n1 - n0) / axisNorm;
    double axis_dot = dotProduct(axis, Q);
    std::array<double, 3> axis_cross = crossProduct(axis, Q);
 
    double angle_cos = std::cos(angle);
    double angle_sin = std::sin(angle);
 
    Q[0] = Q[0] * angle_cos + axis[0] * axis_dot * (1. - angle_cos) + axis_cross[0] * angle_sin;
    Q[1] = Q[1] * angle_cos + axis[1] * axis_dot * (1. - angle_cos) + axis_cross[1] * angle_sin;
    Q[2] = Q[2] * angle_cos + axis[2] * axis_dot * (1. - angle_cos) + axis_cross[2] * angle_sin;
 
    // Transform point back to the original coordinate system
    Q = Q + n0;
 
    return Q;
}
 
array3D reconstructPointFromBarycentricPolygon( std::vector<array3D> const &V, std::vector<double> const &lambda)
{
    return reconstructPointFromBarycentricPolygon( V.size(), V.data(), lambda);
}
 
array3D reconstructPointFromBarycentricPolygon( std::size_t nV, array3D const *V, std::vector<double> const &lambda)
{
    return reconstructPointFromBarycentricPolygon(nV, V, lambda.data());
}
 
array3D reconstructPointFromBarycentricPolygon( std::size_t nV, array3D const *V, double const *lambda)
{
    assert( validBarycentric(lambda, nV) );
 
    array3D xP = {{0.,0.,0.}};
    for(std::size_t i=0; i<nV; ++i){
        xP += lambda[i]*V[i];
    }
 
    return xP;
}
 
array3D projectPointLine( array3D const &P, array3D const &Q, array3D const &n )
{
    assert( validLine(Q,n) );
    return Q + dotProduct(P - Q, n) * n;
}
 
array3D projectPointPlane( array3D const &P, array3D const &Q, array3D const &n )
{
    assert( validPlane(Q,n) );
    return P - dotProduct(P - Q, n) * n;
}
 
array3D projectPointSegment( array3D const &P, array3D const &Q0, array3D const &Q1)
{
 
    std::array<double,2> lambda;
    return projectPointSegment( P, Q0, Q1, &lambda[0] );
}
 
array3D projectPointSegment( array3D const &P, array3D const &Q0, array3D const &Q1, std::array<double,2> &lambda )
{
    return projectPointSegment( P, Q0, Q1, &lambda[0]);
}
 
array3D projectPointSegment( array3D const &P, array3D const &Q0, array3D const &Q1, double *lambda )
{
 
    assert( validSegment(Q0,Q1) );
 
    array3D n = Q1 -Q0;
    double t =  -dotProduct(n,Q0-P) / dotProduct(n,n);
 
    // Restrict projection onto the segment
    t = std::max( std::min( t, 1.), 0. );
 
    lambda[0] = 1. - t;
    lambda[1] = t;
 
    return reconstructPointFromBarycentricSegment( Q0, Q1, lambda);
}
 
array3D projectPointTriangle( array3D const &P, array3D const &Q0, array3D const &Q1, array3D const &Q2)
{
    array3D xP;
    array3D lambda;
    _projectPointsTriangle( 1, &P, Q0, Q1, Q2, &xP, lambda.data() );
    return xP;
}
 
array3D projectPointTriangle( array3D const &P, array3D const &Q0, array3D const &Q1, array3D const &Q2, array3D &lambda)
{
    return projectPointTriangle( P, Q0, Q1, Q2, lambda.data() );
}
 
array3D projectPointTriangle( array3D const &P, array3D const &Q0, array3D const &Q1, array3D const &Q2, double *lambda)
{
    array3D xP;
    _projectPointsTriangle( 1, &P, Q0, Q1, Q2, &xP, lambda );
 
    return xP;
}
 
array3D restrictPointTriangle( array3D const &Q0, array3D const &Q1, array3D const &Q2, array3D &lambda)
{
    return restrictPointTriangle( Q0, Q1, Q2, &lambda[0] );
}
 
array3D restrictPointTriangle( array3D const &Q0, array3D const &Q1, array3D const &Q2, double *lambda)
{
 
    assert( validBarycentric(&lambda[0], 3) );
 
    int count = 0;
    std::array<int,2> negatives = {{ 0, 0 }};
 
    for( int i=0; i<3; ++i){
        if( lambda[i] < 0){
            negatives[count] = i;
            ++count;
        }
    }
 
    if( count == 0){
        return reconstructPointFromBarycentricTriangle( Q0, Q1, Q2, lambda );
    }
 
    std::array<const array3D*,3> r = {{&Q0, &Q1, &Q2}};
    if( count == 1){
        std::array<double,2>   lambdaLocal;
        int vertex0 = (negatives[0] +1) %3;
        int vertex1 = (vertex0     +1) %3;
        array3D P = reconstructPointFromBarycentricTriangle( Q0, Q1, Q2, lambda );
        array3D xP = projectPointSegment(P, *r[vertex0], *r[vertex1], lambdaLocal);
        lambda[negatives[0]] = 0.;
        lambda[vertex0] = lambdaLocal[0];
        lambda[vertex1] = lambdaLocal[1];
        return xP;
 
    } else {
        int vertex0 = 3 -negatives[0] -negatives[1];
        int vertex1 = (vertex0 +1) %3;
        int vertex2 = (vertex1 +1) %3;
 
        array3D s01 = *r[vertex1] - *r[vertex0];
        array3D s02 = *r[vertex2] - *r[vertex0];
 
        if(dotProduct(s01,s02) >= 0 ){
            lambda[0] = 0.;
            lambda[1] = 0.;
            lambda[2] = 0.;
            lambda[vertex0] = 1.;
            return *r[vertex0];
 
        } else {
            std::array<double,2> lambdaLocal01, lambdaLocal02;
            array3D P = reconstructPointFromBarycentricTriangle( Q0, Q1, Q2, lambda );
 
            array3D xP01 = projectPointSegment(P, *r[vertex0], *r[vertex1], lambdaLocal01);
            array3D xP02 = projectPointSegment(P, *r[vertex0], *r[vertex2], lambdaLocal02);
 
            if( norm2(P-xP01) <= norm2(P-xP02) ){
                lambda[vertex0] = lambdaLocal01[0];
                lambda[vertex1] = lambdaLocal01[1];
                lambda[vertex2] = 0.;
                return xP01;
 
            } else {
                lambda[vertex0] = lambdaLocal02[0];
                lambda[vertex1] = 0;
                lambda[vertex2] = lambdaLocal02[1];
                return xP02;
            }
 
        }
 
    }
 
    BITPIT_UNREACHABLE("CANNOT REACH");
 
}
 
std::vector<array3D> projectCloudTriangle( std::vector<array3D> const &cloud, array3D const &Q0, array3D const &Q1, array3D const &Q2, std::vector<array3D> &lambda )
{
 
    int cloudCount(cloud.size());
 
    std::vector<array3D> xP(cloudCount);
 
    lambda.resize(cloudCount);
 
    _projectPointsTriangle( cloudCount, cloud.data(), Q0, Q1, Q2, xP.data(), &lambda[0][0]);
 
    return xP;
 
}
 
array3D projectPointPolygon( array3D const &P, std::vector<array3D> const &V)
{
    return projectPointPolygon( P, V.size(), V.data());
}
 
array3D projectPointPolygon( array3D const &P, std::size_t nV, array3D const *V)
{
    std::vector<double> lambda(nV);
    return projectPointPolygon( P, nV, V, lambda);
}
 
array3D projectPointPolygon( array3D const &P, std::vector<array3D> const &V, std::vector<double> &lambda)
{
    return projectPointPolygon( P, V.size(), V.data(), lambda);
}
 
array3D projectPointPolygon( array3D const &P, std::size_t nV, array3D const *V, std::vector<double> &lambda)
{
    lambda.resize(nV);
 
    return projectPointPolygon(P, nV, V, lambda.data());
}
 
array3D projectPointPolygon( array3D const &P, std::size_t nV, array3D const *V, double *lambda)
{
    array3D xP;
 
    double distance, minDistance(std::numeric_limits<double>::max());
    int minTriangle = -1;
    array3D V0, V1, V2;
    array3D localLambda, minLambda;
 
    // Compute the distance from each triangle in the simplex
    int triangleCount = polygonSubtriangleCount(nV, V);
 
    for (int triangle=0; triangle < triangleCount; ++triangle) {
 
        subtriangleOfPolygon( triangle, nV, V, V0, V1, V2);
 
        distance = distancePointTriangle(P, V0, V1, V2, localLambda);
 
        if (distance <= minDistance) {
 
            minDistance = distance;
 
            minLambda = localLambda;
            minTriangle = triangle;
        }
 
    } //next triangle
 
    assert(minTriangle >= 0);
    subtriangleOfPolygon( minTriangle, nV, V, V0, V1, V2);
    xP = reconstructPointFromBarycentricTriangle( V0, V1, V2, minLambda);
 
    computeGeneralizedBarycentric( xP, nV, V, lambda);
 
    return xP;
 
}
 
array3D projectPointCone( array3D const &point, array3D const &apex, array3D const &axis, double alpha)
{
 
 
    if( alpha <= BITPIT_PI_2 ) { //accute cone angle
 
        array3D versor = point-apex;
        versor /= norm2(versor);
 
        double cosPointAxis = dotProduct(versor,axis);
        double cosCriticalAngle = cos(alpha+BITPIT_PI_2);
 
        if( cosPointAxis <= cosCriticalAngle ){ //point projects on cone apex
            return apex;
 
        } else { // point projects on cone surface
 
            array3D planeNormal = crossProduct(axis,versor);
            planeNormal /= norm2(planeNormal);
 
            array3D direction = rotateVector(axis,planeNormal,alpha);
 
            return projectPointLine(point,apex,direction);
 
        }
 
    } else { // abtuse cone angle -> project on complement
        return projectPointCone( point, apex, -1.*axis, BITPIT_PI-alpha);
 
    }
 
}
 
double distancePointLine( array3D const &P, array3D const &Q, array3D const &n, array3D &xP)
{
    xP = projectPointLine(P,Q,n);
    return norm2( P-xP);
}
 
double distancePointPlane( array3D const &P, array3D const &Q, array3D const &n, array3D &xP)
{
    xP = projectPointPlane(P,Q,n);
    return norm2(P-xP);
}
 
double distancePointSegment( array3D const &P, array3D const &Q0, array3D const &Q1)
{
    array3D xP = projectPointSegment( P, Q0, Q1);
    return norm2(P-xP);
}
 
double distancePointSegment( array3D const &P, array3D const &Q0, array3D const &Q1, std::array<double,2> &lambda)
{
    array3D xP = projectPointSegment( P, Q0, Q1, lambda);
    return norm2(P-xP);
}
 
double distancePointTriangle( array3D const &P, array3D const &Q0, array3D const &Q1, array3D const &Q2)
{
    array3D xP = projectPointTriangle(P, Q0, Q1, Q2);
    return norm2(P-xP);
}
 
double distancePointTriangle( array3D const &P, array3D const &Q0, array3D const &Q1, array3D const &Q2, array3D &lambda)
{
    array3D xP = projectPointTriangle(P, Q0, Q1, Q2, lambda);
    return norm2(P-xP);
}
 
double distancePointCone( array3D const &point, array3D const &apex, array3D const &axis, double alpha)
{
    array3D xP = projectPointCone( point, apex, axis, alpha);
    return norm2(point-xP);
}
 
std::vector<double> distanceCloudTriangle( std::vector<array3D> const &cloud, array3D const &Q0, array3D const &Q1, array3D const &Q2)
{
    std::vector<array3D> lambda(cloud.size());
    return distanceCloudTriangle( cloud, Q0, Q1, Q2, lambda);
}
 
std::vector<double> distanceCloudTriangle( std::vector<array3D> const &cloud, array3D const &Q0, array3D const &Q1, array3D const &Q2, std::vector<array3D> &lambda )
{
    int N(cloud.size());
 
    lambda.resize(N);
 
    std::vector<array3D> xP(N);
    _projectPointsTriangle( N, cloud.data(), Q0, Q1, Q2, xP.data(), &lambda[0][0]);
 
    std::vector<double> d(N);
    std::vector<double>::iterator distance = d.begin();
 
    std::vector<array3D>::iterator projection = xP.begin();
    for( const auto &point : cloud){
        *distance = norm2( point - *projection);
 
        ++projection;
        ++distance;
    }
 
    return d;
}
 
double distancePointPolygon( array3D const &P, std::vector<array3D> const &V, array3D &xP, int &flag)
{
    return distancePointPolygon( P, V.size(), V.data(), xP, flag);
}
 
double distancePointPolygon( array3D const &P, std::size_t nV, array3D const *V, array3D &xP, int &flag)
{
    const int MAX_ELEM_VERTICES = 8;
    BITPIT_CREATE_WORKSPACE(lambda, double, nV, MAX_ELEM_VERTICES);
 
    double distance = distancePointPolygon( P, nV, V, lambda);
    xP = reconstructPointFromBarycentricPolygon( nV, V, lambda );
    flag = convertBarycentricToFlagPolygon( nV, lambda );
 
    return distance;
}
 
double distancePointPolygon( array3D const &P, std::vector<array3D> const &V)
{
    return distancePointPolygon( P, V.size(), V.data());
}
 
double distancePointPolygon( array3D const &P, std::size_t nV, array3D const *V)
{
    std::vector<double> lambda(nV);
    return distancePointPolygon( P, nV, V, lambda);
}
 
double distancePointPolygon( array3D const &P, std::vector<array3D> const &V,std::vector<double> &lambda)
{
    return distancePointPolygon(P, V.size(), V.data(), lambda);
}
 
double distancePointPolygon( array3D const &P, std::size_t nV, array3D const *V,std::vector<double> &lambda)
{
    lambda.resize(nV);
 
    return distancePointPolygon(P, nV, V, lambda.data());
}
 
double distancePointPolygon( array3D const &P, std::size_t nV, array3D const *V, double *lambda)
{
    array3D xP = projectPointPolygon( P, nV, V, lambda);
    return norm2(P-xP);
}
 
std::vector<double> distanceCloudPolygon( std::vector<array3D> const &cloud, std::vector<array3D> const &V, std::vector<array3D> &xP, std::vector<int> &flag)
{
    return distanceCloudPolygon( cloud, V.size(), V.data(), xP, flag);
}
 
std::vector<double> distanceCloudPolygon( std::vector<array3D> const &cloud, std::size_t nV, array3D const *V, std::vector<array3D> &xP, std::vector<int> &flag)
{
    int cloudCount( cloud.size() );
 
    std::vector<std::vector<double>> lambda( cloudCount, std::vector<double> (nV));
    std::vector<double> d = distanceCloudPolygon( cloud, nV, V, lambda);
 
    xP.resize(cloudCount);
    std::vector<array3D>::iterator xPItr = xP.begin();
 
    flag.resize(cloudCount);
    std::vector<int>::iterator flagItr = flag.begin();
 
    for( const auto &l : lambda){
        *flagItr = convertBarycentricToFlagPolygon( l );
        *xPItr = reconstructPointFromBarycentricPolygon( nV, V, l );
 
        ++xPItr;
        ++flagItr;
    }
 
    return d;
}
 
std::vector<double> distanceCloudPolygon( std::vector<array3D> const &P, std::vector<array3D> const &V)
{
    return distanceCloudPolygon( P, V.size(), V.data());
}
 
std::vector<double> distanceCloudPolygon( std::vector<array3D> const &P, std::size_t nV, array3D const *V)
{
    int cloudCount(P.size());
 
    std::vector<double> d(cloudCount,std::numeric_limits<double>::max());
 
    int triangleCount = polygonSubtriangleCount(nV, V);
    array3D V0, V1, V2;
 
    for (int triangle=0; triangle < triangleCount; ++triangle) { // foreach triangle
        subtriangleOfPolygon( triangle, nV, V, V0, V1, V2);
        std::vector<double> dT = distanceCloudTriangle(P, V0, V1, V2);
 
        d = min(d,dT);
    }
 
    return d;
}
 
std::vector<double> distanceCloudPolygon( std::vector<array3D> const &cloud, std::vector<array3D> const &V, std::vector<std::vector<double>> &lambda)
{
    return distanceCloudPolygon( cloud, V.size(), V.data(), lambda);
}
 
std::vector<double> distanceCloudPolygon( std::vector<array3D> const &cloud, std::size_t nV, array3D const *V, std::vector<std::vector<double>> &lambda)
{
    int cloudCount(cloud.size()), vertexCount(nV);
 
    std::vector<double> d(cloudCount,std::numeric_limits<double>::max());
    lambda.resize(cloudCount, std::vector<double>(vertexCount,0) );
 
    std::vector<double> dTemp(cloudCount);
    std::vector<array3D> lambdaTemp(cloudCount);
    int triangleCount = polygonSubtriangleCount(nV, V);
    array3D V0, V1, V2;
 
    for (int triangle=0; triangle < triangleCount; ++triangle) { // foreach triangle
        subtriangleOfPolygon( triangle, nV, V, V0, V1, V2);
        dTemp = distanceCloudTriangle(cloud, V0, V1, V2, lambdaTemp);
 
        for(int i=0; i< cloudCount; ++i){
            if( dTemp[i] < d[i]){
                d[i] = dTemp[i];
                std::copy( lambdaTemp[i].begin(), lambdaTemp[i].end(), lambda[i].begin());
            }
        }
    }
 
    return d;
}
 
double distanceLineLine( array3D const &P0, array3D const &n0, array3D const &P1, array3D const &n1)
{
    array3D xP0, xP1;
    return distanceLineLine( P0, n0, P1, n1, xP0, xP1);
}
 
double distanceLineLine( array3D const &P0, array3D const &n0, array3D const &P1, array3D const &n1, array3D &xP0, array3D &xP1)
{
    assert( validLine(P0,n0) );
    assert( validLine(P1,n1) );
 
    double n01 = dotProduct(n0,n1);
    double det = 1. - n01*n01;
 
    // check if lines are parallel
    if( std::abs(det) < 1.e-12){
        double distance = distancePointLine(P0, P1, n1, xP1);
        xP0 = projectPointLine(xP1, P0, n0);
        return distance;
    }
 
 
    array3D dP = P1-P0;
    double rhs0 =  dotProduct(dP,n0);
    double rhs1 = -dotProduct(dP,n1);
 
    double det0 = rhs0 +rhs1*n01;
    double det1 = rhs1 +rhs0*n01;
 
    double s0 = det0/det;
    double s1 = det1/det;
 
    xP0 = P0 +s0*n0;
    xP1 = P1 +s1*n1;
 
    return norm2( xP0 - xP1);
}
 
bool intersectLineLine( array3D const &P1, array3D const &n1, array3D const &P2, array3D const &n2, array3D &P, double distanceTolerance)
{
    array3D xP1, xP2;
    if( distanceLineLine(P1,n1,P2,n2,xP1,xP2) < distanceTolerance){
        P = xP1;
        return true;
    }
 
    return false;
}
 
bool intersectSegmentSegment( array3D const &P1, array3D const &P2, array3D const &Q1, array3D const &Q2, array3D &x, double distanceTolerance)
{
    assert( validSegment(P1,P2) );
    assert( validSegment(Q1,Q2) );
 
    array3D nP = P2 - P1;
    nP /= norm2(nP);
 
    array3D nQ = Q2 - Q1;
    nQ /= norm2(nQ);
 
    array3D temp;
    if( intersectLineLine(P1, nP, Q1, nQ, temp, distanceTolerance) && intersectPointSegment( temp, P1, P2, distanceTolerance) && intersectPointSegment(temp, Q1, Q2, distanceTolerance) ){
        x = temp;
        return true;
    }
 
    return false;
}
 
bool intersectLinePlane( array3D const &P1, array3D const &n1, array3D const &P2, array3D const &n2, array3D &P, const double coplanarityTolerance)
{
 
    assert( validLine(P1,n1) );
    assert( validPlane(P2,n2) );
 
    // ========================================================================== //
    // CHECK DEGENERATE CASES                                                     //
    // ========================================================================== //
    double s = dotProduct(n1, n2);
    if (std::abs(s) < std::cos(0.5*BITPIT_PI-coplanarityTolerance)) {
        return false;
    }
 
    // ========================================================================== //
    // FIND INTERSECTION POINTS                                                   //
    // ========================================================================== //
    double xi = -dotProduct(P1 - P2, n2) /s;
    P = P1 + xi *n1;
 
    return true;
}
 
bool intersectSegmentPlane( array3D const &Q1, array3D const &Q2, array3D const &P2, array3D const &n2, array3D &P, const double distanceTolerance)
{
 
    assert( validSegment(Q1,Q2) );
    assert( validPlane(P2,n2) );
 
    array3D n = Q2 - Q1;
    n /= norm2(n);
 
    array3D xP;
    if ( intersectLinePlane(Q1, n, P2, n2, xP, distanceTolerance) && intersectPointSegment(xP, Q1, Q2, distanceTolerance) ) {
        P = xP;
        return true;
    }
 
    return false;
}
 
bool intersectPlanePlane( array3D const &P1, array3D const &n1, array3D const &P2, array3D const &n2,
        array3D &Pl, array3D &nl, const double coplanarityTolerance)
{
 
    assert( validPlane(P1,n1) );
    assert( validPlane(P2,n2) );
 
    double n12 = dotProduct(n1, n2);
    // check degenerate condition
    if( std::abs(n12) > std::cos(coplanarityTolerance)) {
        return false;
    }
 
 
    double detCB = 1.0-n12*n12;
 
 
    nl = crossProduct(n1,n2);
    nl /= norm2(nl);
 
    // if planes intersect, determine the closest point
    // to P1 and P2 as anchor point. The augmented functional
    // I = 0.5*[ (Pl-P1)^2 + (Pl-P2)^2] + lambda1[ n1.(Pl-P1) ] +lambda2[ n2.(Pl-P2) ]
    // where lambda1 and lambda2 are Lagrange multipliers.
    // The optimality conditions I,Pl I,lambda1 I,lambda2 are
    // solved using the Schur complment
 
    array3D  dP = P2-P1;
    std::array<double,2>  rhs = {{ dotProduct(n1,dP) , -dotProduct(n2,dP) }};
 
    double det1 = rhs[0] - n12*rhs[1];
    double det2 = rhs[1] - n12*rhs[0];
    double lambda1 = det1 /detCB;
    double lambda2 = det2 /detCB;
 
    Pl = P1 +P2 -lambda1*n1 -lambda2*n2;
    Pl *= 0.5;
 
    return true;
 
}
 
bool intersectPlaneBox( array3D const &P1, array3D const &n1, array3D const &A0, array3D const &A1, int dim, const double distanceTolerance)
{
    assert( validPlane(P1,n1) );
    return _intersectPlaneBox( P1, n1, A0, A1, nullptr, dim, distanceTolerance);
}
 
bool intersectPlaneBox( array3D const &P1, array3D const &n1, array3D const &A0, array3D const &A1, std::vector<array3D> &intersects, int dim, const double distanceTolerance)
{
    assert( validPlane(P1,n1) );
    return _intersectPlaneBox( P1, n1, A0, A1, &intersects, dim, distanceTolerance);
}
 
bool intersectLineTriangle( array3D const &P, array3D const &n, array3D const &A, array3D const &B, array3D const &C, array3D &Q, const double distanceTolerance)
{
    assert( validLine(P,n) );
    assert( validTriangle(A,B,C) );
 
    array3D nT  = crossProduct(B - A, C - A);
    nT /= norm2(nT);
 
    array3D xP;
    if ( intersectLinePlane(P, n, A, nT, xP, distanceTolerance) && intersectPointTriangle(xP, A, B, C, distanceTolerance) ) {
        Q = xP;
        return true;
    }
 
    return false;
}
 
bool intersectSegmentTriangle( array3D const &P0, array3D const &P1, array3D const &A, array3D const &B, array3D const &C, array3D &Q, const double distanceTolerance)
{
    assert( validSegment(P0,P1) );
    assert( validTriangle(A,B,C) );
 
    array3D n = P1 - P0;
    n /= norm2(n);
 
    array3D xP;
    if ( intersectLineTriangle(P0, n, A, B, C, xP, distanceTolerance) && intersectPointSegment(xP, P0, P1, distanceTolerance)  ) {
        Q = xP;
        return true;
    }
 
    return false;
}
 
bool intersectLinePolygon( array3D const &P, array3D const &n, std::vector<array3D > const &V, array3D &Q, const double distanceTolerance)
{
    return intersectLinePolygon( P, n, V.size(), V.data(), Q, distanceTolerance);
}
 
bool intersectLinePolygon( array3D const &P, array3D const &n, std::size_t nV, array3D const *V, array3D &Q, const double distanceTolerance)
{
    assert( validLine(P,n) );
 
    int nTriangles = polygonSubtriangleCount(nV, V);
    array3D V0, V1, V2;
 
    for( int i=0; i< nTriangles; ++i){
        subtriangleOfPolygon(i, nV, V, V0, V1, V2);
 
        if( intersectLineTriangle(P, n, V0, V1, V2, Q, distanceTolerance) ) {
            return true;
        }
    }
 
    return false;
}
 
bool intersectSegmentPolygon( array3D const &P0, array3D const &P1, std::vector<array3D > const &V, array3D &Q, const double distanceTolerance)
{
    return intersectSegmentPolygon( P0, P1, V.size(), V.data(), Q, distanceTolerance);
}
 
bool intersectSegmentPolygon( array3D const &P0, array3D const &P1, std::size_t nV, array3D const *V, array3D &Q, const double distanceTolerance)
{
    assert( validSegment(P0,P1) );
 
    int nTriangles = polygonSubtriangleCount(nV, V);
    array3D V0, V1, V2;
 
    for( int i=0; i< nTriangles; ++i){
        subtriangleOfPolygon(i, nV, V, V0, V1, V2);
 
        if( intersectSegmentTriangle(P0, P1, V0, V1, V2, Q, distanceTolerance) ) {
            return true;
        }
    }
 
    return false;
}
 
bool intersectBoxBox(array3D const &A1, array3D const &A2, array3D const &B1, array3D const &B2, int dim, const double distanceTolerance)
{
    for( int d=0; d<dim; ++d){
        if( B1[d] > A2[d] + distanceTolerance || B2[d] < A1[d] - distanceTolerance ){
            return false;
        }
    }
 
    return true;
}
 
bool intersectBoxBox(array3D const &A1, array3D const &A2, array3D const &B1, array3D const &B2, array3D &I1, array3D &I2, int dim, const double distanceTolerance )
{
    for( int d=0; d<dim; ++d){
 
        if( B1[d] > A2[d] + distanceTolerance || B2[d] < A1[d] - distanceTolerance ){
            return false;
        }
 
        else{
            I1[d] = std::max( A1[d], B1[d] );
            I2[d] = std::min( A2[d], B2[d] );
        }
 
    }
 
    return true;
}
 
bool intersectBoxTriangle(array3D const &A0, array3D const &A1, array3D const &V0, array3D const &V1, array3D const &V2, int dim, const double distanceTolerance)
{
    return _intersectBoxTriangle( A0, A1, V0, V1, V2, false, false, false, nullptr, nullptr, dim, distanceTolerance);
}
 
bool intersectBoxTriangle(array3D const &A0, array3D const &A1, array3D const &V0, array3D const &V1, array3D const &V2, bool interiorTriangleVertices, bool triangleEdgeBoxFaceIntersections, bool triangleBoxEdgeIntersections, std::vector<array3D> &P, int dim, const double distanceTolerance)
{
    return _intersectBoxTriangle( A0, A1, V0, V1, V2, interiorTriangleVertices, triangleEdgeBoxFaceIntersections, triangleBoxEdgeIntersections, &P, nullptr, dim, distanceTolerance);
}
 
bool intersectBoxTriangle(array3D const &A0, array3D const &A1, array3D const &V0, array3D const &V1, array3D const &V2, bool interiorTriangleVertices, bool triangleEdgeBoxHullIntersections, bool triangleBoxEdgeIntersections, std::vector<array3D> &P, std::vector<int> &flag, int dim, const double distanceTolerance)
{
    return _intersectBoxTriangle( A0, A1, V0, V1, V2, interiorTriangleVertices, triangleEdgeBoxHullIntersections, triangleBoxEdgeIntersections, &P, &flag, dim, distanceTolerance);
}
 
bool intersectSegmentBox( array3D const &V0, array3D const &V1, array3D const &A0, array3D const &A1, int dim, const double distanceTolerance)
{
    return _intersectSegmentBox( V0, V1, A0, A1, false, false, nullptr, nullptr, dim, distanceTolerance);
}
 
bool intersectSegmentBox( array3D const &V0, array3D const &V1, array3D const &A0, array3D const &A1, bool interiorSegmentVertice, bool segmentBoxHullIntersection, std::vector<array3D> &P, int dim, const double distanceTolerance)
{
    return _intersectSegmentBox( V0, V1, A0, A1, interiorSegmentVertice, segmentBoxHullIntersection, &P, nullptr, dim, distanceTolerance);
}
 
bool intersectSegmentBox( array3D const &V0, array3D const &V1, array3D const &A0, array3D const &A1, bool interiorSegmentVertice, bool segmentBoxHullIntersection, std::vector<array3D> &P, std::vector<int> &flag, int dim, const double distanceTolerance)
{
    return _intersectSegmentBox( V0, V1, A0, A1, interiorSegmentVertice, segmentBoxHullIntersection, &P, &flag, dim, distanceTolerance);
}
 
bool intersectBoxPolygon( array3D const &A0, array3D const &A1, std::vector<array3D> const &VS, int dim, const double distanceTolerance )
{
    return intersectBoxPolygon( A0, A1, VS.size(), VS.data(), dim, distanceTolerance );
}
bool intersectBoxPolygon( array3D const &A0, array3D const &A1, std::size_t nVS, array3D const *VS, int dim, const double distanceTolerance )
{
    return _intersectBoxPolygon(A0, A1, nVS, VS, false, false, false, nullptr, nullptr, dim, distanceTolerance);
}
 
bool intersectBoxPolygon( array3D const &A0, array3D const &A1, std::vector<array3D> const &VS, bool innerPolygonPoints, bool polygonEdgeBoxFaceIntersections, bool polygonBoxEdgeIntersections, std::vector<array3D> &P, int dim, const double distanceTolerance)
{
    return intersectBoxPolygon( A0, A1, VS.size(), VS.data(), innerPolygonPoints, polygonEdgeBoxFaceIntersections, polygonBoxEdgeIntersections, P, dim, distanceTolerance);
}
 
 
bool intersectBoxPolygon( array3D const &A0, array3D const &A1, std::size_t nVS, array3D const *VS, bool innerPolygonPoints, bool polygonEdgeBoxFaceIntersections, bool polygonBoxEdgeIntersections, std::vector<array3D> &P, int dim, const double distanceTolerance)
{
    return _intersectBoxPolygon(A0, A1, nVS, VS, innerPolygonPoints, polygonEdgeBoxFaceIntersections, polygonBoxEdgeIntersections, &P, nullptr, dim, distanceTolerance);
}
 
bool intersectBoxPolygon( array3D const &A0, array3D const &A1, std::vector<array3D> const &VS, bool innerPolygonPoints, bool polygonEdgeBoxFaceIntersections, bool polygonBoxEdgeIntersections, std::vector<array3D> &P, std::vector<int> &flag, int dim, const double distanceTolerance)
{
    return intersectBoxPolygon( A0, A1, VS.size(), VS.data(), innerPolygonPoints, polygonEdgeBoxFaceIntersections, polygonBoxEdgeIntersections, P, flag, dim, distanceTolerance);
}
 
bool intersectBoxPolygon( array3D const &A0, array3D const &A1, std::size_t nVS, array3D const *VS, bool innerPolygonPoints, bool polygonEdgeBoxFaceIntersections, bool polygonBoxEdgeIntersections, std::vector<array3D> &P, std::vector<int> &flag, int dim, const double distanceTolerance)
{
    return _intersectBoxPolygon(A0, A1, nVS, VS, innerPolygonPoints, polygonEdgeBoxFaceIntersections, polygonBoxEdgeIntersections, &P, &flag, dim, distanceTolerance);
}
 
bool intersectBoxCircle( array3D const &A0, array3D const &A1, array3D const &centre, double radius, const double distanceTolerance)
{
    double minimumDistance = 0.;
    for (int i = 0; i < 2; ++i) {
        if (centre[i] < A0[i]) {
            minimumDistance += (centre[i] - A0[i]) * (centre[i] - A0[i]);
        } else if (centre[i] > A1[i]) {
            minimumDistance += (centre[i] - A1[i]) * (centre[i] - A1[i]);
        }
    }
 
    double inflatedRadius = radius + distanceTolerance;
 
    return (minimumDistance <= (inflatedRadius * inflatedRadius));
}
 
bool intersectBoxSphere( array3D const &A0, array3D const &A1, array3D const &centre, double radius, const double distanceTolerance)
{
    double minimumDistance = 0.;
    for (int i = 0; i < 3; ++i) {
        if (centre[i] < A0[i]) {
            minimumDistance += (centre[i] - A0[i]) * (centre[i] - A0[i]);
        } else if (centre[i] > A1[i]) {
            minimumDistance += (centre[i] - A1[i]) * (centre[i] - A1[i]);
        }
    }
 
    double inflatedRadius = radius + distanceTolerance;
 
    return (minimumDistance <= (inflatedRadius * inflatedRadius));
}
 
//to levelset // -------------------------------------------------------------------------- //
//to levelset bool intersectLineSurface(
//to levelset         array3D  const  &x1,
//to levelset         array3D  const  &n1,
//to levelset         array3D  const  &x2,
//to levelset         array3D  const  &n2,
//to levelset         array3D  const  &xL,
//to levelset         array3D  const  &nL,
//to levelset         array3D         &xp,
//to levelset         array3D         &np
//to levelset         ) {
//to levelset
//to levelset     // ========================================================================== //
//to levelset     //                                                                            //
//to levelset     // Reconstruct Surface given by two points and their normals and computes     //
//to levelset     // intersection of reconstruction with a line given by point and versor     ' //
//to levelset     // ========================================================================== //
//to levelset     // INPUT                                                                      //
//to levelset     // ========================================================================== //
//to levelset     // ========================================================================== //
//to levelset     // OUTPUT                                                                     //
//to levelset     // ========================================================================== //
//to levelset     // - none                                                                     //
//to levelset     // ========================================================================== //
//to levelset
//to levelset     // ========================================================================== //
//to levelset     // VARIABLES DECLARATION                                                      //
//to levelset     // ========================================================================== //
//to levelset
//to levelset     double                  w1(0), w2(0), w(0);
//to levelset     array3D         c1, c2;
//to levelset     bool                    s1, s2;
//to levelset
//to levelset     s1      =   intersectLinePlane( xL, nL, x1, n1, c1);
//to levelset     s2      =   intersectLinePlane( xL, nL, x2, n2, c2);
//to levelset
//to levelset     if( s1 && s2) {
//to levelset         w1      =   norm2( c2-x2);
//to levelset         w2      =   norm2( c1-x1);
//to levelset         w       =   w1+w2;
//to levelset
//to levelset         w1      =   w1 /w;
//to levelset         w2      =   w2 /w;
//to levelset
//to levelset         xp      =   w1*c1 +w2*c2;
//to levelset         np      =   w1*n1 +w2*n2;
//to levelset
//to levelset         np      =   np /norm2(np);
//to levelset
//to levelset         return (true);
//to levelset     }
//to levelset
//to levelset     else if( s1 ){
//to levelset         xp      =   c1;
//to levelset         np      =   n1;
//to levelset
//to levelset         return(true);
//to levelset     }
//to levelset
//to levelset     else if( s2 ){
//to levelset         xp      =   c2;
//to levelset         np      =   n2;
//to levelset
//to levelset         return(true);
//to levelset     }
//to levelset
//to levelset     return (false);
//to levelset
//to levelset }
 
bool intersectPointLine( array3D const &P, array3D const &Q, array3D const &n, const double distanceTolerance)
{
    assert( validLine(Q,n) );
 
    array3D xP;
    if(distancePointLine(P, Q, n, xP) > distanceTolerance){
        return false;
    }
 
    return true;
}
 
bool intersectPointSegment( array3D const &P, array3D const &P1, array3D const &P2, const double distanceTolerance)
{
    assert( validSegment(P1,P2) );
 
    array3D xP     = projectPointSegment( P, P1, P2 );
    array3D offset = xP - P;
 
    return (dotProduct(offset, offset) <= distanceTolerance * distanceTolerance);
}
 
bool intersectPointTriangle( array3D const &P, array3D const &A, array3D const &B, array3D const &C, const double distanceTolerance)
{
 
    if(distancePointTriangle(P, A, B, C) > distanceTolerance){
        return false;
    }
 
    return true;
}
 
bool intersectPointBox( array3D const &P, array3D const &B1, array3D const &B2, int dim, const double distanceTolerance)
{
 
    for( int d=0; d<dim; ++d){
        if( P[d]< (B1[d] - distanceTolerance) || P[d] > (B2[d] + distanceTolerance)){
            return false;
        }
    }
 
    return true;
}
 
void computeAABBSegment(array3D const &A, array3D const &B, array3D &P0, array3D &P1)
{
 
    P0 = A;
    P1 = A;
 
    for(int i=0; i<3; ++i){
        P0[i] = std::min( P0[i], B[i] );
        P1[i] = std::max( P1[i], B[i] );
    }
 
    return;
}
 
void computeAABBTriangle(array3D const &A, array3D const &B, array3D const &C, array3D &P0, array3D &P1)
{
    P0 = A;
    P1 = A;
 
    for(int i=0; i<3; ++i){
        P0[i] = std::min( P0[i], B[i] );
        P0[i] = std::min( P0[i], C[i] );
 
        P1[i] = std::max( P1[i], B[i] );
        P1[i] = std::max( P1[i], C[i] );
    }
 
    return;
}
 
void computeAABBPolygon(std::vector<array3D> const &VS, array3D &P0, array3D &P1)
{
    computeAABBPolygon(VS.size(), VS.data(), P0, P1);
}
 
void computeAABBPolygon(std::size_t nVS, array3D const *VS, array3D &P0, array3D &P1)
{
    P0 = VS[0];
    P1 = VS[0];
 
    for( std::size_t j=1; j<nVS; ++j){
        for( int i=0; i<3; ++i){
            P0[i] = std::min( P0[i], VS[j][i] );
            P1[i] = std::max( P1[i], VS[j][i] );
        }
    }
 
    return;
}
 
void unionAABB( array3D const &A0, array3D const &A1, array3D const &B0, array3D const &B1, array3D &C0, array3D &C1)
{
    for( int i=0; i<3; ++i){
        C0[i] = std::min( A0[i], B0[i]);
        C1[i] = std::max( A1[i], B1[i]);
    }
 
    return;
}
 
void unionAABB(std::vector<array3D> const &A0, std::vector<array3D> const &A1, array3D &C0, array3D &C1)
{
 
    int n( std::min(A0.size(), A1.size() ) );
 
    if( n > 0 ){
        C0 =  A0[0];
        C1 =  A1[0];
 
        for( int i=1; i<n; ++i){
            unionAABB( A0[i], A1[i], C0, C1, C0, C1);
        }
    }
 
    return;
}
 
void intersectionAABB(array3D const &A0, array3D const &A1, array3D const &B0, array3D const &B1, array3D &C0, array3D  &C1)
{
    intersectBoxBox( A0, A1, B0, B1, C0, C1 );
    return;
}
 
void subtractionAABB(array3D const &A0, array3D const &A1, array3D const &B0, array3D const &B1, array3D &C0, array3D  &C1)
{
    // X direction
    if( B0[1]<=A0[1] && B0[2]<=A0[2] && B1[1]>=A1[1] && B1[2]>=A1[2] ){
        C0[0] = ( B0[0]<=A0[0] && B1[0]>=A0[0] ) ? B1[0] : A0[0];
        C1[0] = ( B0[0]<=A1[0] && B1[0]>=A1[0] ) ? B0[0] : A1[0];
    }
 
    // Y direction
    if( B0[0]<=A0[0] && B0[2]<=A0[2] && B1[0]>=A1[0] && B1[2]>=A1[2] ){
        C0[1] = ( B0[1]<=A0[1] && B1[1]>=A0[1] ) ? B1[1] : A0[2];
        C1[1] = ( B0[1]<=A1[1] && B1[1]>=A1[1] ) ? B0[1] : A1[2];
    }
 
    // Z direction
    if( B0[0]<=A0[0] && B0[1]<=A0[1] && B1[0]>=A1[0] && B1[1]>=A1[1] ){
        C0[2] = ( B0[2]<=A0[2] && B1[2]>=A0[2] ) ? B1[2] : A0[2];
        C1[2] = ( B0[2]<=A1[2] && B1[2]>=A1[2] ) ? B0[2] : A1[2];
    }
 
    return;
}
 
void vertexOfSegment(int i, array3D const &V0, array3D const &V1, array3D &P)
{
    switch(i){
 
        case 0:
            P = V0;
            break;
 
        case 1:
            P = V1;
            break;
 
        default:
            assert(false);
            break;
    }
 
    return;
}
 
void vertexOfTriangle(int i, array3D const &V0, array3D const &V1, array3D const &V2, array3D &P)
{
    switch(i){
 
        case 0:
            P = V0;
            break;
 
        case 1:
            P = V1;
            break;
 
        case 2:
            P = V2;
            break;
 
        default:
            assert(false);
            break;
    }
 
    return;
}
 
void edgeOfTriangle(int i, array3D const &V0, array3D const &V1, array3D const &V2, array3D &P0, array3D &P1)
{
    switch(i){
 
        case 0:
            P0 = V0;
            P1 = V1;
            break;
 
        case 1:
            P0 = V1;
            P1 = V2;
            break;
 
        case 2:
            P0 = V2;
            P1 = V0;
            break;
 
        default:
            assert(false);
            break;
    }
 
    return;
}
 
void faceOfBox(int i, array3D const &A0, array3D const &A1, array3D &P0, array3D &P1, array3D &P2, array3D &P3)
{
 
    assert(i<6);
 
    int v0 = boxFaceVertexConnectivity[i][0];
    int v1 = boxFaceVertexConnectivity[i][1];
    int v2 = boxFaceVertexConnectivity[i][2];
    int v3 = boxFaceVertexConnectivity[i][3];
 
    vertexOfBox(v0, A0, A1, P0 );
    vertexOfBox(v1, A0, A1, P1 );
    vertexOfBox(v2, A0, A1, P2 );
    vertexOfBox(v3, A0, A1, P3 );
 
    return;
}
 
void edgeOfBox(int i, array3D const &A0, array3D const &A1, array3D &P0, array3D &P1)
{
    assert(i<12);
 
    int v0 = boxEdgeVertexConnectivity[i][0];
    int v1 = boxEdgeVertexConnectivity[i][1];
 
    vertexOfBox(v0, A0, A1, P0 );
    vertexOfBox(v1, A0, A1, P1 );
 
    return;
}
 
void vertexOfBox(int i, array3D const &A0, array3D const &A1, array3D &P)
{
 
    switch(i){
 
        case 0:
            P[0] = A0[0];
            P[1] = A0[1];
            P[2] = A0[2];
            break;
 
        case 1:
            P[0] = A1[0];
            P[1] = A0[1];
            P[2] = A0[2];
            break;
 
        case 2:
            P[0] = A0[0];
            P[1] = A1[1];
            P[2] = A0[2];
            break;
 
        case 3:
            P[0] = A1[0];
            P[1] = A1[1];
            P[2] = A0[2];
            break;
 
        case 4:
            P[0] = A0[0];
            P[1] = A0[1];
            P[2] = A1[2];
            break;
 
        case 5:
            P[0] = A1[0];
            P[1] = A0[1];
            P[2] = A1[2];
            break;
 
        case 6:
            P[0] = A0[0];
            P[1] = A1[1];
            P[2] = A1[2];
            break;
 
        case 7:
            P[0] = A1[0];
            P[1] = A1[1];
            P[2] = A1[2];
            break;
 
        default:
            assert(false);
            break;
 
    }
 
    return;
}
 
array3D rotateVector( array3D const &vector, array3D const &axis, double theta)
{
 
    array3D rotated;
    double cosTheta = cos(theta);
 
    rotated  = cosTheta * vector;
    rotated += sin(theta) * crossProduct(axis,vector);
    rotated += (1.0 - cosTheta) * dotProduct(axis,vector) * axis;
 
    return rotated;
}
 
double areaTriangle( array3D const &a, array3D const &b, array3D const &c)
{
    return 0.5 *norm2(crossProduct(b-a,c-a));
}
 
int polygonEdgesCount( std::vector<array3D> const &V)
{
    return polygonEdgesCount( V.size(), V.data());
}
 
int polygonEdgesCount( std::size_t nV, array3D const *V)
{
    BITPIT_UNUSED(V);
 
    return nV;
}
 
int polygonSubtriangleCount( std::vector<array3D> const &V)
{
    return polygonSubtriangleCount( V.size(), V.data());
}
 
int polygonSubtriangleCount( std::size_t nV, array3D const *V)
{
    BITPIT_UNUSED(V);
 
    return nV;
}
 
void edgeOfPolygon( int edge, std::vector<array3D> const &V, array3D &V0, array3D &V1)
{
    edgeOfPolygon( edge, V.size(), V.data(), V0, V1);
}
 
void edgeOfPolygon( int edge, std::size_t nV, array3D const *V, array3D &V0, array3D &V1)
{
    assert(edge<polygonEdgesCount(nV, V));
 
    V0 = V[edge];
    V1 = V[(edge+1) % nV];
    return;
}
 
void subtriangleOfPolygon( int triangle, std::vector<array3D> const &V, array3D &V0, array3D &V1, array3D &V2)
{
    subtriangleOfPolygon( triangle, V.size(), V.data(), V0, V1, V2);
}
 
void subtriangleOfPolygon( int triangle, std::size_t nV, array3D const *V, array3D &V0, array3D &V1, array3D &V2)
{
    assert(triangle<polygonSubtriangleCount(nV, V));
 
    V0 = {0., 0., 0.};
    for (std::size_t i = 0; i < nV; i++) {
        for (int d = 0; d < 3; d++) {
            V0[d] += V[i][d];
        }
    }
    for (int d = 0; d < 3; d++) {
        V0[d] /= nV;
    }
    V1 = V[triangle%nV];
    V2 = V[(triangle+1)%nV];
    return;
}
 
}
 
}