src/INTERP_KERNEL/InterpolationUtils.hxx

   1 // Copyright (C) 2007-2023  CEA, EDF
   2 //
   3 // This library is free software; you can redistribute it and/or
   4 // modify it under the terms of the GNU Lesser General Public
   5 // License as published by the Free Software Foundation; either
   6 // version 2.1 of the License, or (at your option) any later version.
   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 GNU
  11 // Lesser General Public License for more details.
  12 //
  13 // You should have received a copy of the GNU Lesser General Public
  14 // License along with this library; if not, write to the Free Software
  15 // Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA
  16 //
  17 // See http://www.salome-platform.org/ or email : webmaster.salome@opencascade.com
  18 //
  19 // Author : Anthony Geay (CEA/DEN)
  20
  21 #ifndef __INTERPOLATIONUTILS_HXX__
  22 #define __INTERPOLATIONUTILS_HXX__
  23
  24 #include "INTERPKERNELDefines.hxx"
  25 #include "InterpKernelException.hxx"
  26 #include "VolSurfUser.hxx"
  27
  28 #include "NormalizedUnstructuredMesh.hxx"
  29
  30 #include <deque>
  31 #include <map>
  32 #include <cmath>
  33 #include <string>
  34 #include <vector>
  35 #include <algorithm>
  36 #include <iostream>
  37 #include <limits>
  38 #include <functional>
  39
  40 namespace INTERP_KERNEL
  41 {
  42   template<class ConnType, NumberingPolicy numPol>
  43   class OTT//OffsetToolTrait
  44   {
  45   };
  46
  47   template<class ConnType>
  48   class OTT<ConnType,ALL_C_MODE>
  49   {
  50   public:
  51     static ConnType indFC(ConnType i) { return i; }
  52     static ConnType ind2C(ConnType i) { return i; }
  53     static ConnType conn2C(ConnType i) { return i; }
  54     static ConnType coo2C(ConnType i) { return i; }
  55   };
  56
  57   template<class ConnType>
  58   class OTT<ConnType,ALL_FORTRAN_MODE>
  59   {
  60   public:
  61     static ConnType indFC(ConnType i) { return i+1; }
  62     static ConnType ind2C(ConnType i) { return i-1; }
  63     static ConnType conn2C(ConnType i) { return i-1; }
  64     static ConnType coo2C(ConnType i) { return i-1; }
  65   };
  66
  67   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
  68   /*   calcul la surface d'un triangle                  */
  69   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
  70
  71   inline double Surf_Tri(const double *P_1,const double *P_2,const double *P_3)
  72   {
  73     double A=(P_3[1]-P_1[1])*(P_2[0]-P_1[0])-(P_2[1]-P_1[1])*(P_3[0]-P_1[0]);
  74     double Surface = 0.5*fabs(A);
  75     return Surface;
  76   }
  77
  78   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
  79   /*     fonction qui calcul le determinant            */
  80   /*      de deux vecteur(cf doc CGAL).                */
  81   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
  82
  83   //fonction qui calcul le determinant des vecteurs: P3P1 et P3P2
  84   //(cf doc CGAL).
  85
  86   inline double mon_determinant(const double *P_1,
  87                                 const double *P_2,
  88                                 const double *P_3)
  89   {
  90     double mon_det=(P_1[0]-P_3[0])*(P_2[1]-P_3[1])-(P_2[0]-P_3[0])*(P_1[1]-P_3[1]);
  91     return mon_det;
  92   }
  93
  94   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
  95   //calcul la norme du vecteur P1P2
  96
  97   inline double norme_vecteur(const double* P_1,const double* P_2)
  98   {
  99     double X=P_1[0]-P_2[0];
 100     double Y=P_1[1]-P_2[1];
 101     return sqrt(X*X+Y*Y);
 102   }
 103
 104   inline void mid_of_seg2(const double P1[2], const double P2[2], double MID[2])
 105   {
 106     MID[0]=(P1[0]+P2[0])/2.;
 107     MID[1]=(P1[1]+P1[1])/2.;
 108   }
 109
 110   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
 111   /*         calcul le cos et le sin de l'angle P1P2,P1P3     */
 112   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
 113
 114   inline std::vector<double> calcul_cos_et_sin(const double* P_1,
 115                                                const double* P_2,
 116                                                const double* P_3)
 117   {
 118
 119     std::vector<double> Vect;
 120     double P1_P2=norme_vecteur(P_1,P_2);
 121     double P2_P3=norme_vecteur(P_2,P_3);
 122     double P3_P1=norme_vecteur(P_3,P_1);
 123
 124     double N=P1_P2*P1_P2+P3_P1*P3_P1-P2_P3*P2_P3;
 125     double D=2.0*P1_P2*P3_P1;
 126     double COS=N/D;
 127     if (COS>1.0) COS=1.0;
 128     if (COS<-1.0) COS=-1.0;
 129     Vect.push_back(COS);
 130     double V=mon_determinant(P_2,P_3,P_1);
 131     double D_1=P1_P2*P3_P1;
 132     double SIN=V/D_1;
 133     if (SIN>1.0) SIN=1.0;
 134     if (SIN<-1.0) SIN=-1.0;
 135     Vect.push_back(SIN);
 136
 137     return Vect;
 138
 139   }
 140
 141   /*!
 142    * This method builds a quadrangle built with the first point of 'triIn' the barycenter of two edges starting or ending with
 143    * the first point of 'triIn' and the barycenter of 'triIn'.
 144    *
 145    * @param triIn is a 6 doubles array in full interlace mode, that represents a triangle.
 146    * @param quadOut is a 8 doubles array filled after the following call.
 147    */
 148   template<int SPACEDIM>
 149   inline void fillDualCellOfTri(const double *triIn, double *quadOut)
 150   {
 151     //1st point
 152     std::copy(triIn,triIn+SPACEDIM,quadOut);
 153     double tmp[SPACEDIM];
 154     std::transform(triIn,triIn+SPACEDIM,triIn+SPACEDIM,tmp,std::plus<double>());
 155     //2nd point
 156     std::transform(tmp,tmp+SPACEDIM,quadOut+SPACEDIM,std::bind(std::multiplies<double>(),std::placeholders::_1,0.5));
 157     std::transform(tmp,tmp+SPACEDIM,triIn+2*SPACEDIM,tmp,std::plus<double>());
 158     //3rd point
 159     std::transform(tmp,tmp+SPACEDIM,quadOut+2*SPACEDIM,std::bind(std::multiplies<double>(),std::placeholders::_1,1/3.));
 160     //4th point
 161     std::transform(triIn,triIn+SPACEDIM,triIn+2*SPACEDIM,tmp,std::plus<double>());
 162     std::transform(tmp,tmp+SPACEDIM,quadOut+3*SPACEDIM,std::bind(std::multiplies<double>(),std::placeholders::_1,0.5));
 163   }
 164
 165   /*!
 166    * This method builds a potentially non-convex polygon cell built with the first point of 'triIn' the barycenter of two edges starting or ending with
 167    * the first point of 'triIn' and the barycenter of 'triIn'.
 168    *
 169    * @param polygIn is a 6 doubles array in full interlace mode, that represents a triangle.
 170    * @param polygOut is a 8 doubles array filled after the following call.
 171    */
 172   template<int SPACEDIM>
 173   inline void fillDualCellOfPolyg(const double *polygIn, mcIdType nPtsPolygonIn, double *polygOut)
 174   {
 175     //1st point
 176     std::copy(polygIn,polygIn+SPACEDIM,polygOut);
 177     std::transform(polygIn,polygIn+SPACEDIM,polygIn+SPACEDIM,polygOut+SPACEDIM,std::plus<double>());
 178     //2nd point
 179     std::transform(polygOut+SPACEDIM,polygOut+2*SPACEDIM,polygOut+SPACEDIM,std::bind(std::multiplies<double>(),std::placeholders::_1,0.5));
 180     double tmp[SPACEDIM];
 181     //
 182     for(mcIdType i=0;i<nPtsPolygonIn-2;i++)
 183       {
 184         std::transform(polygIn,polygIn+SPACEDIM,polygIn+(i+2)*SPACEDIM,tmp,std::plus<double>());
 185         std::transform(tmp,tmp+SPACEDIM,polygOut+(2*i+3)*SPACEDIM,std::bind(std::multiplies<double>(),std::placeholders::_1,0.5));
 186         std::transform(polygIn+(i+1)*SPACEDIM,polygIn+(i+2)*SPACEDIM,tmp,tmp,std::plus<double>());
 187         std::transform(tmp,tmp+SPACEDIM,polygOut+(2*i+2)*SPACEDIM,std::bind(std::multiplies<double>(),std::placeholders::_1,1./3.));
 188       }
 189   }
 190
 191   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
 192   /*     calcul les coordonnees du barycentre d'un polygone   */
 193   /*     le vecteur en entree est constitue des coordonnees   */
 194   /*     des sommets du polygone                              */
 195   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
 196
 197   inline std::vector<double> bary_poly(const std::vector<double>& V)
 198   {
 199     std::vector<double> Bary;
 200     std::size_t taille=V.size();
 201     double x=0;
 202     double y=0;
 203
 204     for(std::size_t i=0;i<taille/2;i++)
 205       {
 206         x=x+V[2*i];
 207         y=y+V[2*i+1];
 208       }
 209     double A=2*x/(static_cast<double>(taille));
 210     double B=2*y/(static_cast<double>(taille));
 211     Bary.push_back(A);//taille vecteur=2*nb de points.
 212     Bary.push_back(B);
 213
 214
 215     return Bary;
 216   }
 217
 218   /*!
 219    * Given 6 coeffs of a Tria6 returns the corresponding value of a given pos
 220    */
 221   inline double computeTria6RefBase(const double *coeffs, const double *pos)
 222   {
 223     return coeffs[0]+coeffs[1]*pos[0]+coeffs[2]*pos[1]+coeffs[3]*pos[0]*pos[0]+coeffs[4]*pos[0]*pos[1]+coeffs[5]*pos[1]*pos[1];
 224   }
 225
 226   /*!
 227    * Given xsi,eta in refCoo (length==2) return 6 coeffs in weightedPos.
 228    */
 229   inline void computeWeightedCoeffsInTria6FromRefBase(const double *refCoo, double *weightedPos)
 230   {
 231     weightedPos[0]=(1.-refCoo[0]-refCoo[1])*(1.-2*refCoo[0]-2.*refCoo[1]);
 232     weightedPos[1]=refCoo[0]*(2.*refCoo[0]-1.);
 233     weightedPos[2]=refCoo[1]*(2.*refCoo[1]-1.);
 234     weightedPos[3]=4.*refCoo[0]*(1.-refCoo[0]-refCoo[1]);
 235     weightedPos[4]=4.*refCoo[0]*refCoo[1];
 236     weightedPos[5]=4.*refCoo[1]*(1.-refCoo[0]-refCoo[1]);
 237   }
 238
 239   /*!
 240    * Given 10 coeffs of a Tetra10 returns the corresponding value of a given pos
 241    */
 242   inline double computeTetra10RefBase(const double *coeffs, const double *pos)
 243   {
 244     return coeffs[0]+coeffs[1]*pos[0]+coeffs[2]*pos[1]+coeffs[3]*pos[2]+
 245       coeffs[4]*pos[0]*pos[0]+coeffs[5]*pos[0]*pos[1]+coeffs[6]*pos[0]*pos[2]+
 246       coeffs[7]*pos[1]*pos[1]+coeffs[8]*pos[1]*pos[2]+coeffs[9]*pos[2]*pos[2];
 247   }
 248
 249   /*!
 250    * Given xsi,eta,z in refCoo (length==3) return 10 coeffs in weightedPos.
 251    */
 252   inline void computeWeightedCoeffsInTetra10FromRefBase(const double *refCoo, double *weightedPos)
 253   {
 254     //http://www.cadfamily.com/download/CAE/ABAQUS/The%20Finite%20Element%20Method%20-%20A%20practical%20course%20abaqus.pdf page 217
 255     //L1=1-refCoo[0]-refCoo[1]-refCoo[2]
 256     //L2=refCoo[0] L3=refCoo[1] L4=refCoo[2]
 257     weightedPos[0]=(-2.*(refCoo[0]+refCoo[1]+refCoo[2])+1)*(1-refCoo[0]-refCoo[1]-refCoo[2]);//(2*L1-1)*L1
 258     weightedPos[1]=(2.*refCoo[0]-1.)*refCoo[0];//(2*L2-1)*L2
 259     weightedPos[2]=(2.*refCoo[1]-1.)*refCoo[1];//(2*L3-1)*L3
 260     weightedPos[3]=(2.*refCoo[2]-1.)*refCoo[2];//(2*L4-1)*L4
 261     weightedPos[4]=4.*(1-refCoo[0]-refCoo[1]-refCoo[2])*refCoo[0];//4*L1*L2
 262     weightedPos[5]=4.*refCoo[0]*refCoo[1];//4*L2*L3
 263     weightedPos[6]=4.*(1-refCoo[0]-refCoo[1]-refCoo[2])*refCoo[1];//4*L1*L3
 264     weightedPos[7]=4.*(1-refCoo[0]-refCoo[1]-refCoo[2])*refCoo[2];//4*L1*L4
 265     weightedPos[8]=4.*refCoo[0]*refCoo[2];//4*L2*L4
 266     weightedPos[9]=4.*refCoo[1]*refCoo[2];//4*L3*L4
 267   }
 268
 269   /*!
 270    * \brief Solve system equation in matrix form using Gaussian elimination algorithm
 271    *  \param M - N x N+1 matrix
 272    *  \param sol - vector of N solutions
 273    *  \retval bool - true if succeeded
 274    */
 275   template<unsigned nbRow>
 276   bool solveSystemOfEquations(double M[nbRow][nbRow+1], double* sol)
 277   {
 278     const int nbCol=nbRow+1;
 279
 280     // make upper triangular matrix (forward elimination)
 281
 282     int iR[nbRow];// = { 0, 1, 2 };
 283     for ( int i = 0; i < (int) nbRow; ++i )
 284       iR[i] = i;
 285     for ( int i = 0; i < (int)(nbRow-1); ++i ) // nullify nbRow-1 rows
 286       {
 287         // swap rows to have max value of i-th column in i-th row
 288         double max = std::fabs( M[ iR[i] ][i] );
 289         for ( int r = i+1; r < (int)nbRow; ++r )
 290           {
 291             double m = std::fabs( M[ iR[r] ][i] );
 292             if ( m > max )
 293               {
 294                 max = m;
 295                 std::swap( iR[r], iR[i] );
 296               }
 297           }
 298         if ( max < std::numeric_limits<double>::min() )
 299           {
 300             //sol[0]=1; sol[1]=sol[2]=sol[3]=0;
 301             return false; // no solution
 302           }
 303         // make 0 below M[i][i] (actually we do not modify i-th column)
 304         double* tUpRow = M[ iR[i] ];
 305         for ( int r = i+1; r < (int)nbRow; ++r )
 306           {
 307             double* mRow = M[ iR[r] ];
 308             double coef = mRow[ i ] / tUpRow[ i ];
 309             for ( int c = i+1; c < nbCol; ++c )
 310               mRow[ c ] -= tUpRow[ c ] * coef;
 311           }
 312       }
 313     double* mRow = M[ iR[nbRow-1] ];
 314     if ( std::fabs( mRow[ nbRow-1 ] ) < std::numeric_limits<double>::min() )
 315       {
 316         //sol[0]=1; sol[1]=sol[2]=sol[3]=0;
 317         return false; // no solution
 318       }
 319     mRow[ nbRow ] /= mRow[ nbRow-1 ];
 320
 321     // calculate solution (back substitution)
 322
 323     sol[ nbRow-1 ] = mRow[ nbRow ];
 324
 325     for ( int i = nbRow-2; i+1; --i )
 326       {
 327         mRow = M[ iR[i] ];
 328         sol[ i ] = mRow[ nbRow ];
 329         for ( int j = nbRow-1; j > i; --j )
 330           sol[ i ] -= sol[j]*mRow[ j ];
 331         sol[ i ] /= mRow[ i ];
 332       }
 333
 334     return true;
 335   }
 336
 337
 338   /*!
 339    * \brief Solve system equation in matrix form using Gaussian elimination algorithm
 340    *  \param matrix - N x N+NB_OF_VARS matrix
 341    *  \param solutions - vector of N solutions
 342    *  \param eps - precision
 343    *  \retval bool - true if succeeded
 344    */
 345   template<unsigned SZ, unsigned NB_OF_RES>
 346   bool solveSystemOfEquations2(const double *matrix, double *solutions, double eps)
 347   {
 348     unsigned k,j;
 349     int nr,n,m,np;
 350     double s,g;
 351     int mb;
 352     //
 353     double B[SZ*(SZ+NB_OF_RES)];
 354     std::copy(matrix,matrix+SZ*(SZ+NB_OF_RES),B);
 355     //
 356     nr=SZ+NB_OF_RES;
 357     for(k=0;k<SZ;k++)
 358       {
 359         np=nr*k+k;
 360         if(fabs(B[np])<eps)
 361           {
 362             n=k;
 363             do
 364               {
 365                 n++;
 366                 if(fabs(B[nr*k+n])>eps)
 367                   {/* Rows permutation */
 368                     for(m=0;m<nr;m++)
 369                       std::swap(B[nr*k+m],B[nr*n+m]);
 370                   }
 371               }
 372             while (n<(int)SZ);
 373           }
 374         s=B[np];//s is the Pivot
 375         std::transform(B+k*nr,B+(k+1)*nr,B+k*nr,std::bind(std::divides<double>(),std::placeholders::_1,s));
 376         for(j=0;j<SZ;j++)
 377           {
 378             if(j!=k)
 379               {
 380                 g=B[j*nr+k];
 381                 for(mb=k;mb<nr;mb++)
 382                   B[j*nr+mb]-=B[k*nr+mb]*g;
 383               }
 384           }
 385       }
 386     for(j=0;j<NB_OF_RES;j++)
 387       for(k=0;k<SZ;k++)
 388         solutions[j*SZ+k]=B[nr*k+SZ+j];
 389     //
 390     return true;
 391   }
 392
 393   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
 394   /*     Calculate barycentric coordinates of a 2D point p */
 395   /*     with respect to the triangle verices.             */
 396   /*     triaCoords are in full interlace                  */
 397   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
 398
 399   template<int SPACEDIM>
 400   inline void barycentric_coords(const double* triaCoords, const double* p, double* bc)
 401   {
 402     // matrix 2x2
 403     double
 404       T11 = triaCoords[0]-triaCoords[2*SPACEDIM], T12 = triaCoords[SPACEDIM]-triaCoords[2*SPACEDIM],
 405       T21 = triaCoords[1]-triaCoords[2*SPACEDIM+1], T22 = triaCoords[SPACEDIM+1]-triaCoords[2*SPACEDIM+1];
 406     // matrix determinant
 407     double Tdet = T11*T22 - T12*T21;
 408     if ( fabs( Tdet ) < std::numeric_limits<double>::min() ) {
 409       bc[0]=1; bc[1]=0; bc[2]=0;
 410       return;
 411     }
 412     // matrix inverse
 413     double t11 = T22, t12 = -T12, t21 = -T21, t22 = T11;
 414     // vector
 415     double r11 = p[0]-triaCoords[2*SPACEDIM], r12 = p[1]-triaCoords[2*SPACEDIM+1];
 416     // barycentric coordinates: multiply matrix by vector
 417     bc[0] = (t11 * r11 + t12 * r12)/Tdet;
 418     bc[1] = (t21 * r11 + t22 * r12)/Tdet;
 419     bc[2] = 1. - bc[0] - bc[1];
 420   }
 421
 422   inline void barycentric_coords_seg2(const std::vector<const double*>& n, const double *p, double *bc)
 423   {
 424     double delta=n[0][0]-n[1][0];
 425     bc[0]=fabs((*p-n[1][0])/delta);
 426     bc[1]=fabs((*p-n[0][0])/delta);
 427   }
 428
 429   inline void barycentric_coords_tri3(const std::vector<const double*>& n, const double *p, double *bc)
 430   {
 431     enum { _XX=0, _YY, _ZZ };
 432     // matrix 2x2
 433     double
 434       T11 = n[0][_XX]-n[2][_XX], T12 = n[1][_XX]-n[2][_XX],
 435       T21 = n[0][_YY]-n[2][_YY], T22 = n[1][_YY]-n[2][_YY];
 436     // matrix determinant
 437     double Tdet = T11*T22 - T12*T21;
 438     if ( (std::fabs( Tdet) ) < (std::numeric_limits<double>::min()) )
 439       {
 440         bc[0]=1; bc[1]=bc[2]=0; // no solution
 441         return;
 442       }
 443     // matrix inverse
 444     double t11 = T22, t12 = -T12, t21 = -T21, t22 = T11;
 445     // vector
 446     double r11 = p[_XX]-n[2][_XX], r12 = p[_YY]-n[2][_YY];
 447     // barycentric coordinates: multiply matrix by vector
 448     bc[0] = (t11 * r11 + t12 * r12)/Tdet;
 449     bc[1] = (t21 * r11 + t22 * r12)/Tdet;
 450     bc[2] = 1. - bc[0] - bc[1];
 451   }
 452
 453   inline void barycentric_coords_quad4(const std::vector<const double*>& n, const double *p, double *bc)
 454   {
 455     enum { _XX=0, _YY, _ZZ };
 456     // Find bc by solving system of 3 equations using Gaussian elimination algorithm
 457     // bc1*( x1 - x4 ) + bc2*( x2 - x4 ) + bc3*( x3 - x4 ) = px - x4
 458     // bc1*( y1 - y4 ) + bc2*( y2 - y4 ) + bc3*( y3 - y4 ) = px - y4
 459     // bc1*( z1 - z4 ) + bc2*( z2 - z4 ) + bc3*( z3 - z4 ) = px - z4
 460
 461     double T[3][4]=
 462       {{ n[0][_XX]-n[3][_XX], n[1][_XX]-n[3][_XX], n[2][_XX]-n[3][_XX], p[_XX]-n[3][_XX] },
 463        { n[0][_YY]-n[3][_YY], n[1][_YY]-n[3][_YY], n[2][_YY]-n[3][_YY], p[_YY]-n[3][_YY] },
 464        { n[0][_ZZ]-n[3][_ZZ], n[1][_ZZ]-n[3][_ZZ], n[2][_ZZ]-n[3][_ZZ], p[_ZZ]-n[3][_ZZ] }};
 465
 466     if ( !solveSystemOfEquations<3>( T, bc ) )
 467       bc[0]=1., bc[1] = bc[2] = bc[3] = 0;
 468     else
 469       bc[ 3 ] = 1. - bc[0] - bc[1] - bc[2];
 470   }
 471
 472   inline void barycentric_coords_tri6(const std::vector<const double*>& n, const double *p, double *bc)
 473   {
 474     double matrix2[48]={1., 0., 0., 0., 0., 0., 0., 0.,
 475                         1., 0., 0., 0., 0., 0., 1., 0.,
 476                         1., 0., 0., 0., 0., 0., 0., 1.,
 477                         1., 0., 0., 0., 0., 0., 0.5, 0.,
 478                         1., 0., 0., 0., 0., 0., 0.5, 0.5,
 479                         1., 0., 0., 0., 0., 0., 0.,0.5};
 480     for(int i=0;i<6;i++)
 481       {
 482         matrix2[8*i+1]=n[i][0];
 483         matrix2[8*i+2]=n[i][1];
 484         matrix2[8*i+3]=n[i][0]*n[i][0];
 485         matrix2[8*i+4]=n[i][0]*n[i][1];
 486         matrix2[8*i+5]=n[i][1]*n[i][1];
 487       }
 488     double res[12];
 489     solveSystemOfEquations2<6,2>(matrix2,res,std::numeric_limits<double>::min());
 490     double refCoo[2];
 491     refCoo[0]=computeTria6RefBase(res,p);
 492     refCoo[1]=computeTria6RefBase(res+6,p);
 493     computeWeightedCoeffsInTria6FromRefBase(refCoo,bc);
 494   }
 495
 496   inline void barycentric_coords_tetra10(const std::vector<const double*>& n, const double *p, double *bc)
 497   {
 498     double matrix2[130]={1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.,
 499                          1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 1., 0., 0.,
 500                          1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 1., 0.,
 501                          1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 1.,
 502                          1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.5, 0., 0.,
 503                          1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.5, 0.5, 0.,
 504                          1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.,0.5, 0.,
 505                          1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.5,
 506                          1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.5, 0., 0.5,
 507                          1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.5, 0.5};
 508     for(int i=0;i<10;i++)
 509       {
 510         matrix2[13*i+1]=n[i][0];
 511         matrix2[13*i+2]=n[i][1];
 512         matrix2[13*i+3]=n[i][2];
 513         matrix2[13*i+4]=n[i][0]*n[i][0];
 514         matrix2[13*i+5]=n[i][0]*n[i][1];
 515         matrix2[13*i+6]=n[i][0]*n[i][2];
 516         matrix2[13*i+7]=n[i][1]*n[i][1];
 517         matrix2[13*i+8]=n[i][1]*n[i][2];
 518         matrix2[13*i+9]=n[i][2]*n[i][2];
 519       }
 520     double res[30];
 521     solveSystemOfEquations2<10,3>(matrix2,res,std::numeric_limits<double>::min());
 522     double refCoo[3];
 523     refCoo[0]=computeTetra10RefBase(res,p);
 524     refCoo[1]=computeTetra10RefBase(res+10,p);
 525     refCoo[2]=computeTetra10RefBase(res+20,p);
 526     computeWeightedCoeffsInTetra10FromRefBase(refCoo,bc);
 527   }
 528
 529   /*!
 530    * Calculate barycentric coordinates of a point p with respect to triangle or tetra vertices.
 531    * This method makes 2 assumptions :
 532    *    - this is a simplex
 533    *    - spacedim == meshdim. For TRI3 and TRI6 spaceDim is expected to be equal to 2 and for TETRA4 spaceDim is expected to be equal to 3.
 534    *      If not the case (3D surf for example) a previous projection should be done before.
 535    */
 536   inline void barycentric_coords(const std::vector<const double*>& n, const double *p, double *bc)
 537   {
 538     switch(n.size())
 539       {
 540       case 2:
 541         {// SEG 2
 542           barycentric_coords_seg2(n,p,bc);
 543           break;
 544         }
 545       case 3:
 546         { // TRIA3
 547           barycentric_coords_tri3(n,p,bc);
 548           break;
 549         }
 550       case 4:
 551         { // TETRA4
 552           barycentric_coords_quad4(n,p,bc);
 553           break;
 554         }
 555       case 6:
 556         {
 557           // TRIA6
 558           barycentric_coords_tri6(n,p,bc);
 559           break;
 560         }
 561       case 10:
 562         {//TETRA10
 563           barycentric_coords_tetra10(n,p,bc);
 564           break;
 565         }
 566       default:
 567         throw INTERP_KERNEL::Exception("INTERP_KERNEL::barycentric_coords : unrecognized simplex !");
 568       }
 569   }
 570
 571   inline void barycentric_coords(INTERP_KERNEL::NormalizedCellType ct, const std::vector<const double*>& n, const double *p, double *bc)
 572   {
 573     switch(ct)
 574       {
 575       case NORM_SEG2:
 576         {// SEG 2
 577           barycentric_coords_seg2(n,p,bc);
 578           break;
 579         }
 580       case NORM_TRI3:
 581         { // TRIA3
 582           barycentric_coords_tri3(n,p,bc);
 583           break;
 584         }
 585       case NORM_TETRA4:
 586         { // TETRA4
 587           barycentric_coords_quad4(n,p,bc);
 588           break;
 589         }
 590       case NORM_TRI6:
 591         {
 592           // TRIA6
 593           barycentric_coords_tri6(n,p,bc);
 594           break;
 595         }
 596       case NORM_TETRA10:
 597         {//TETRA10
 598           barycentric_coords_tetra10(n,p,bc);
 599           break;
 600         }
 601       default:
 602         throw INTERP_KERNEL::Exception("INTERP_KERNEL::barycentric_coords : unrecognized simplex !");
 603       }
 604   }
 605
 606   /*!
 607    * Compute barycentric coords of point \a point relative to segment S [segStart,segStop] in 3D space.
 608    * If point \a point is further from S than eps false is returned.
 609    * If point \a point projection on S is outside S false is also returned.
 610    * If point \a point is closer from S than eps and its projection inside S true is returned and \a bc0 and \a bc1 output parameter set.
 611    */
 612   inline bool IsPointOn3DSeg(const double segStart[3], const double segStop[3], const double point[3], double eps, double& bc0, double& bc1)
 613   {
 614     double AB[3]={segStop[0]-segStart[0],segStop[1]-segStart[1],segStop[2]-segStart[2]},AP[3]={point[0]-segStart[0],point[1]-segStart[1],point[2]-segStart[2]};
 615     double l_AB(sqrt(AB[0]*AB[0]+AB[1]*AB[1]+AB[2]*AB[2]));
 616     double AP_dot_AB((AP[0]*AB[0]+AP[1]*AB[1]+AP[2]*AB[2])/(l_AB*l_AB));
 617     double projOfPOnAB[3]={segStart[0]+AP_dot_AB*AB[0],segStart[1]+AP_dot_AB*AB[1],segStart[2]+AP_dot_AB*AB[2]};
 618     double V_dist_P_AB[3]={point[0]-projOfPOnAB[0],point[1]-projOfPOnAB[1],point[2]-projOfPOnAB[2]};
 619     double dist_P_AB(sqrt(V_dist_P_AB[0]*V_dist_P_AB[0]+V_dist_P_AB[1]*V_dist_P_AB[1]+V_dist_P_AB[2]*V_dist_P_AB[2]));
 620     if(dist_P_AB>=eps)
 621       return false;//to far from segment [segStart,segStop]
 622     if(AP_dot_AB<-eps || AP_dot_AB>1.+eps)
 623       return false;
 624     AP_dot_AB=std::max(AP_dot_AB,0.); AP_dot_AB=std::min(AP_dot_AB,1.);
 625     bc0=1.-AP_dot_AB; bc1=AP_dot_AB;
 626     return true;
 627   }
 628
 629   /*!
 630    * Calculate pseudo barycentric coordinates of a point p with respect to the quadrangle vertices.
 631    * This method makes the assumption that:
 632    *  - spacedim == meshdim (2 here).
 633    *  - the point is within the quad
 634    *  Quadratic elements are not supported yet.
 635    *
 636    *  A quadrangle can be described as 3 vectors, one point being taken as the origin.
 637    *  Denoting A, B, C the three other points, any point P within the quad is written as
 638    *    P = xA+ yC + xy(B-A-C)
 639    *  This method solve those 2 equations (one per component) for x and y.
 640    *
 641
 642           A------B
 643           |      |
 644           |      |
 645           0------C
 646    */
 647   inline void quad_mapped_coords(const std::vector<const double*>& n, const double *p, double *bc)
 648   {
 649     double prec = 1.0e-14;
 650     enum { _XX=0, _YY, _ZZ };
 651
 652     if(n.size() != 4)
 653       throw INTERP_KERNEL::Exception("INTERP_KERNEL::quad_mapped_coords : unrecognized geometric type! Only QUAD4 supported.");
 654
 655     double A[2] = {n[1][_XX] - n[0][_XX],  n[1][_YY] - n[0][_YY]};
 656     double B[2] = {n[2][_XX] - n[0][_XX],  n[2][_YY] - n[0][_YY]};
 657     double C[2] = {n[3][_XX] - n[0][_XX],  n[3][_YY] - n[0][_YY]};
 658     double N[2] = {B[_XX] - A[_XX] - C[_XX], B[_YY] - A[_YY] - C[_YY]};
 659     double P[2] = {p[_XX] - n[0][_XX], p[_YY] - n[0][_YY]};
 660
 661     // degenerated case: a rectangle:
 662     if (fabs(N[0]) < prec && fabs(N[1]) < prec)
 663       {
 664         double det = C[0]*A[1] -C[1]*A[0];
 665         if (fabs(det) < prec)
 666           throw INTERP_KERNEL::Exception("MappedBarycentric intersection type: quad_mapped_coords() has a degenerated 2x2 system!");
 667         bc[0] = (P[0]*A[1]-P[1]*A[0])/det;
 668         bc[1] = (P[1]*C[0]-P[0]*C[1])/det;
 669         return;
 670       }
 671     double b,c ,a = A[1]*N[0]-A[0]*N[1];
 672     bool cas1;
 673     if (fabs(a) > 1.0e-14)
 674       {
 675         b = A[1]*C[0]+N[1]*P[0]-N[0]*P[1]-A[0]*C[1];
 676         c = P[0]*C[1] - P[1]*C[0];
 677         cas1 = true;
 678       }
 679     else
 680       {
 681         a = -C[1]*N[0]+C[0]*N[1];
 682         b = A[1]*C[0]-N[1]*P[0]+N[0]*P[1]-A[0]*C[1];
 683         c = -P[0]*A[1] + P[1]*A[0];
 684         cas1 = false;
 685       }
 686     double delta = b*b - 4.0*a*c;
 687     if (delta < 0.0)
 688       throw INTERP_KERNEL::Exception("MappedBarycentric intersection type: quad_mapped_coords(): imaginary solutions!");
 689     bc[1] = 0.5*(-b+sqrt(delta))/a;
 690     if (bc[1] < -prec || bc[1] > (1.0+prec))
 691       bc[1] = 0.5*(-b-sqrt(delta))/a;
 692     if (bc[1] < -prec || bc[1] > (1.0+prec))
 693       throw INTERP_KERNEL::Exception("MappedBarycentric intersection type: quad_mapped_coords(): point doesn't seem to be in quad4!");
 694     if (cas1)
 695       {
 696         double denom = C[0]+bc[1]*N[0];
 697         if (fabs(denom) < prec)
 698           throw INTERP_KERNEL::Exception("MappedBarycentric intersection type: quad_mapped_coords(): point doesn't seem to be in quad4!");
 699         bc[0] = (P[0]-bc[1]*A[0])/denom;
 700         if (bc[0] < -prec || bc[0] > (1.0+prec))
 701           throw INTERP_KERNEL::Exception("MappedBarycentric intersection type: quad_mapped_coords(): point doesn't seem to be in quad4!");
 702       }
 703     else
 704       {
 705         bc[0] = bc[1];
 706         double denom = A[1]+bc[0]*N[1];
 707         if (fabs(denom) < prec)
 708           throw INTERP_KERNEL::Exception("MappedBarycentric intersection type: cuboid_mapped_coord(): point doesn't seem to be in quad4!");
 709         bc[1] = (P[1]-bc[0]*C[1])/denom;
 710         if (bc[1] < -prec || bc[1] > (1.0+prec))
 711           throw INTERP_KERNEL::Exception("MappedBarycentric intersection type: cuboid_mapped_coord(): point doesn't seem to be in quad4!");
 712       }
 713   }
 714
 715   /*!
 716    * Doing as in quad_mapped_coords() would lead to a 4th order equation ... So go simpler here:
 717    * orthogonal distance to each pair of parallel faces is computed. The ratio gives a number in [0,1]
 718    *
 719    * Conventions:
 720    *   - for HEXA8, point F (5) is taken to be the origin (see med file ref connec):
 721    *          0 ------ 3
 722              /|       /|
 723             / |      / |
 724            1 ------ 2  |
 725            |  |     |  |
 726            |  |     |  |
 727            |  4-----|- 7
 728            | /      | /
 729            5 ------ 6
 730
 731    *
 732    */
 733
 734   inline void cuboid_mapped_coords(const std::vector<const double*>& n, const double *p, double *bc)
 735   {
 736     double prec = 1.0e-14;
 737     enum { _XX=0, _YY };
 738     if (n.size() != 8)
 739       throw INTERP_KERNEL::Exception("INTERP_KERNEL::cuboid_mapped_coords: unrecognized geometric type! Only HEXA8 supported.");
 740
 741     double dx1, dx2, dy1, dy2, dz1, dz2;
 742     dx1 = OrthoDistanceFromPtToPlaneInSpaceDim3(p, n[4],n[5],n[1]);
 743     dx2 = OrthoDistanceFromPtToPlaneInSpaceDim3(p, n[7],n[3],n[2]);
 744
 745     dy1 = OrthoDistanceFromPtToPlaneInSpaceDim3(p, n[5],n[6],n[2]);
 746     dy2 = OrthoDistanceFromPtToPlaneInSpaceDim3(p, n[4],n[0],n[3]);
 747
 748     dz1 = OrthoDistanceFromPtToPlaneInSpaceDim3(p, n[5],n[4],n[7]);
 749     dz2 = OrthoDistanceFromPtToPlaneInSpaceDim3(p, n[1],n[2],n[3]);
 750
 751     if (dx1 < -prec || dx2 < -prec || dy1 < -prec || dy2 < -prec || dz1 < -prec || dz2 < -prec)
 752       throw INTERP_KERNEL::Exception("INTERP_KERNEL::cuboid_mapped_coords: point outside HEXA8");
 753
 754     bc[0] = dx1+dx2 < prec ? 0.5 : dx1/(dx1+dx2);
 755     bc[1] = dy1+dy2 < prec ? 0.5 : dy1/(dy1+dy2);
 756     bc[2] = dz1+dz2 < prec ? 0.5 : dz1/(dz1+dz2);
 757   }
 758
 759   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
 760   /*         calcul la surface d'un polygone.                 */
 761   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
 762
 763   inline double  Surf_Poly(const std::vector<double>& Poly)
 764   {
 765
 766     double Surface=0;
 767     for(unsigned long i=0; i<(Poly.size())/2-2; i++)
 768       {
 769         double Surf=Surf_Tri( &Poly[0],&Poly[2*(i+1)],&Poly[2*(i+2)] );
 770         Surface=Surface + Surf ;
 771       }
 772     return Surface ;
 773   }
 774
 775   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
 776   /*   fonction qui teste si un point est dans une maille     */
 777   /*   point: P_0                                             */
 778   /*   P_1, P_2, P_3 sommet des mailles                       */
 779   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
 780
 781   inline bool point_dans_triangle(const double* P_0,const double* P_1,
 782                                   const double* P_2,const double* P_3,
 783                                   double eps)
 784   {
 785
 786     bool A=false;
 787     double det_1=mon_determinant(P_1,P_3,P_0);
 788     double det_2=mon_determinant(P_3,P_2,P_0);
 789     double det_3=mon_determinant(P_2,P_1,P_0);
 790     if( (det_1>=-eps && det_2>=-eps && det_3>=-eps) || (det_1<=eps && det_2<=eps && det_3<=eps) )
 791       {
 792         A=true;
 793       }
 794
 795     return A;
 796   }
 797
 798   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
 799   /*fonction pour verifier qu'un point n'a pas deja ete considerer dans   */
 800   /*      le vecteur et le rajouter au vecteur sinon.                     */
 801   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
 802
 803   inline void verif_point_dans_vect(const double* P, std::vector<double>& V, double absolute_precision )
 804   {
 805     std::size_t taille=V.size();
 806     bool isPresent=false;
 807     for(std::size_t i=0;i<taille/2;i++)
 808       {
 809         if (sqrt(((P[0]-V[2*i])*(P[0]-V[2*i])+(P[1]-V[2*i+1])*(P[1]-V[2*i+1])))<absolute_precision)
 810           isPresent=true;
 811
 812       }
 813     if(!isPresent)
 814       {
 815
 816         V.push_back(P[0]);
 817         V.push_back(P[1]);
 818       }
 819   }
 820
 821   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
 822   /* fonction qui rajoute les sommet du triangle P dans le vecteur V        */
 823   /* si ceux-ci sont compris dans le triangle S et ne sont pas deja dans    */
 824   /* V.                                                                     */
 825   /*sommets de P: P_1, P_2, P_3                                             */
 826   /*sommets de S: P_4, P_5, P_6                                             */
 827   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
 828
 829   inline void rajou_sommet_triangl(const double* P_1,const double* P_2,const double* P_3,
 830                                    const double* P_4,const double* P_5,const double* P_6,
 831                                    std::vector<double>& V, double dim_caracteristic, double precision)
 832   {
 833
 834     double absolute_precision = precision*dim_caracteristic;
 835     bool A_1=INTERP_KERNEL::point_dans_triangle(P_1,P_4,P_5,P_6,absolute_precision);
 836     if(A_1)
 837       verif_point_dans_vect(P_1,V,absolute_precision);
 838     bool A_2=INTERP_KERNEL::point_dans_triangle(P_2,P_4,P_5,P_6,absolute_precision);
 839     if(A_2)
 840       verif_point_dans_vect(P_2,V,absolute_precision);
 841     bool A_3=INTERP_KERNEL::point_dans_triangle(P_3,P_4,P_5,P_6,absolute_precision);
 842     if(A_3)
 843       verif_point_dans_vect(P_3,V,absolute_precision);
 844   }
 845
 846
 847   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _  _ _ _ _ _ _ _ _*/
 848   /*  calcul de l'intersection de deux segments: segments P1P2 avec P3P4      */
 849   /*  . Si l'intersection est non nulle et si celle-ci n'est                  */
 850   /*  n'est pas deja contenue dans Vect on la rajoute a Vect                  */
 851   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _  _ _ _ _ _ _ _ _*/
 852
 853   inline void inters_de_segment(const double * P_1,const double * P_2,
 854                                 const double * P_3,const double * P_4,
 855                                 std::vector<double>& Vect,
 856                                 double dim_caracteristic, double precision)
 857   {
 858     // calcul du determinant de P_1P_2 et P_3P_4.
 859     double det=(P_2[0]-P_1[0])*(P_4[1]-P_3[1])-(P_4[0]-P_3[0])*(P_2[1]-P_1[1]);
 860
 861     double absolute_precision = dim_caracteristic*precision;
 862     if(fabs(det)>absolute_precision)
 863       {
 864         double k_1=-((P_3[1]-P_4[1])*(P_3[0]-P_1[0])+(P_4[0]-P_3[0])*(P_3[1]-P_1[1]))/det;
 865
 866         if (k_1 >= -absolute_precision && k_1 <= 1+absolute_precision)
 867           //if( k_1 >= -precision && k_1 <= 1+precision)
 868           {
 869             double k_2= ((P_1[1]-P_2[1])*(P_1[0]-P_3[0])+(P_2[0]-P_1[0])*(P_1[1]-P_3[1]))/det;
 870
 871             if (k_2 >= -absolute_precision && k_2 <= 1+absolute_precision)
 872               //if( k_2 >= -precision && k_2 <= 1+precision)
 873               {
 874                 double P_0[2];
 875                 P_0[0]=P_1[0]+k_1*(P_2[0]-P_1[0]);
 876                 P_0[1]=P_1[1]+k_1*(P_2[1]-P_1[1]);
 877                 verif_point_dans_vect(P_0,Vect,absolute_precision);
 878               }
 879           }
 880       }
 881   }
 882
 883
 884
 885   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
 886   /*      calcul l'intersection de deux triangles            */
 887   /* P_1, P_2, P_3: sommets du premier triangle              */
 888   /* P_4, P_5, P_6: sommets du deuxi�me triangle             */
 889   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
 890
 891   inline void intersec_de_triangle(const double* P_1,const double* P_2, const double* P_3,
 892                                    const double* P_4,const double* P_5,const double* P_6,
 893                                    std::vector<double>& Vect, double dim_caracteristic, double precision)
 894   {
 895     inters_de_segment(P_1,P_2,P_4,P_5,Vect, dim_caracteristic, precision);
 896     inters_de_segment(P_1,P_2,P_5,P_6,Vect, dim_caracteristic, precision);
 897     inters_de_segment(P_1,P_2,P_6,P_4,Vect, dim_caracteristic, precision);
 898     inters_de_segment(P_2,P_3,P_4,P_5,Vect, dim_caracteristic, precision);
 899     inters_de_segment(P_2,P_3,P_5,P_6,Vect, dim_caracteristic, precision);
 900     inters_de_segment(P_2,P_3,P_6,P_4,Vect, dim_caracteristic, precision);
 901     inters_de_segment(P_3,P_1,P_4,P_5,Vect, dim_caracteristic, precision);
 902     inters_de_segment(P_3,P_1,P_5,P_6,Vect, dim_caracteristic, precision);
 903     inters_de_segment(P_3,P_1,P_6,P_4,Vect, dim_caracteristic, precision);
 904     rajou_sommet_triangl(P_1,P_2,P_3,P_4,P_5,P_6,Vect, dim_caracteristic, precision);
 905     rajou_sommet_triangl(P_4,P_5,P_6,P_1,P_2,P_3,Vect, dim_caracteristic, precision);
 906   }
 907
 908   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
 909   /* fonction pour verifier qu'un node maille n'a pas deja ete considerer  */
 910   /*  dans le vecteur et le rajouter au vecteur sinon.                     */
 911   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
 912
 913   inline void verif_maill_dans_vect(int Num, std::vector<int>& V)
 914   {
 915     std::size_t taille=V.size();
 916     int A=0;
 917     for(std::size_t i=0;i<taille;i++)
 918       {
 919         if(Num==V[i])
 920           {
 921             A=1;
 922             break;
 923           }
 924       }
 925     if(A==0)
 926       {V.push_back(Num); }
 927   }
 928
 929   /*! Function that compares two angles from the values of the pairs (sin,cos)*/
 930   /*! Angles are considered in [0, 2Pi] bt are not computed explicitly */
 931   class AngleLess
 932   {
 933   public:
 934     bool operator()(std::pair<double,double>theta1, std::pair<double,double> theta2) const
 935     {
 936       double norm1 = sqrt(theta1.first*theta1.first +theta1.second*theta1.second);
 937       double norm2 = sqrt(theta2.first*theta2.first +theta2.second*theta2.second);
 938
 939       double epsilon = 1.e-12;
 940
 941       if( norm1 < epsilon || norm2 < epsilon  )
 942         std::cout << "Warning InterpolationUtils.hxx: AngleLess : Vector with zero norm, cannot define the angle !!!! " << std::endl;
 943
 944       return theta1.second*(norm2 + theta2.first) < theta2.second*(norm1 + theta1.first);
 945
 946     }
 947   };
 948
 949
 950   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
 951   /* fonction pour reconstituer un polygone convexe a partir  */
 952   /*              d'un nuage de point.                        */
 953   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
 954
 955   inline std::vector<double> reconstruct_polygon(const std::vector<double>& V)
 956   {
 957
 958     int taille((int)V.size());
 959
 960     //VB : why 6 ?
 961
 962     if(taille<=6)
 963       {return V;}
 964     else
 965       {
 966         double *COS=new double[taille/2];
 967         double *SIN=new double[taille/2];
 968         //double *angle=new double[taille/2];
 969         std::vector<double> Bary=bary_poly(V);
 970         COS[0]=1.0;
 971         SIN[0]=0.0;
 972         //angle[0]=0.0;
 973         for(int i=0; i<taille/2-1;i++)
 974           {
 975             std::vector<double> Trigo=calcul_cos_et_sin(&Bary[0],&V[0],&V[2*(i+1)]);
 976             COS[i+1]=Trigo[0];
 977             SIN[i+1]=Trigo[1];
 978             //if(SIN[i+1]>=0)
 979             //    {angle[i+1]=atan2(SIN[i+1],COS[i+1]);}
 980             //             else
 981             //               {angle[i+1]=-atan2(SIN[i+1],COS[i+1]);}
 982           }
 983
 984         //ensuite on ordonne les angles.
 985         std::vector<double> Pt_ordonne;
 986         Pt_ordonne.reserve(taille);
 987         //        std::multimap<double,int> Ordre;
 988         std::multimap<std::pair<double,double>,int, AngleLess> CosSin;
 989         for(int i=0;i<taille/2;i++)
 990           {
 991             //  Ordre.insert(std::make_pair(angle[i],i));
 992             CosSin.insert(std::make_pair(std::make_pair(SIN[i],COS[i]),i));
 993           }
 994         //        std::multimap <double,int>::iterator mi;
 995         std::multimap<std::pair<double,double>,int, AngleLess>::iterator   micossin;
 996         //         for(mi=Ordre.begin();mi!=Ordre.end();mi++)
 997         //           {
 998         //             int j=(*mi).second;
 999         //             Pt_ordonne.push_back(V[2*j]);
1000         //             Pt_ordonne.push_back(V[2*j+1]);
1001         //           }
1002         for(micossin=CosSin.begin();micossin!=CosSin.end();micossin++)
1003           {
1004             int j=(*micossin).second;
1005             Pt_ordonne.push_back(V[2*j]);
1006             Pt_ordonne.push_back(V[2*j+1]);
1007           }
1008         delete [] COS;
1009         delete [] SIN;
1010         //        delete [] angle;
1011         return Pt_ordonne;
1012       }
1013   }
1014
1015   template<int DIM, NumberingPolicy numPol, class MyMeshType>
1016   inline void getElemBB(double* bb, const double *coordsOfMesh, mcIdType iP, int nb_nodes)
1017   {
1018     bb[0]=std::numeric_limits<double>::max();
1019     bb[1]=-std::numeric_limits<double>::max();
1020     bb[2]=std::numeric_limits<double>::max();
1021     bb[3]=-std::numeric_limits<double>::max();
1022     bb[4]=std::numeric_limits<double>::max();
1023     bb[5]=-std::numeric_limits<double>::max();
1024
1025     for (int i=0; i<nb_nodes; i++)
1026       {
1027         double x = coordsOfMesh[3*(iP+i)];
1028         double y = coordsOfMesh[3*(iP+i)+1];
1029         double z = coordsOfMesh[3*(iP+i)+2];
1030         bb[0]=(x<bb[0])?x:bb[0];
1031         bb[1]=(x>bb[1])?x:bb[1];
1032         bb[2]=(y<bb[2])?y:bb[2];
1033         bb[3]=(y>bb[3])?y:bb[3];
1034         bb[4]=(z<bb[4])?z:bb[4];
1035         bb[5]=(z>bb[5])?z:bb[5];
1036       }
1037   }
1038
1039   /*!
1040    * Find a vector orthogonal to the input vector
1041    */
1042   inline void orthogonalVect3(const double inpVect[3], double outVect[3])
1043   {
1044     std::vector<bool> sw(3,false);
1045     double inpVect2[3];
1046         std::transform(inpVect,inpVect + 3,inpVect2,[](double c){return fabs(c);});
1047     std::size_t posMin(std::distance(inpVect2,std::min_element(inpVect2,inpVect2+3)));
1048     sw[posMin]=true;
1049     std::size_t posMax(std::distance(inpVect2,std::max_element(inpVect2,inpVect2+3)));
1050     if(posMax==posMin)
1051       { posMax=(posMin+1)%3; }
1052     sw[posMax]=true;
1053     std::size_t posMid(std::distance(sw.begin(),std::find(sw.begin(),sw.end(),false)));
1054     outVect[posMin]=0.; outVect[posMid]=1.; outVect[posMax]=-inpVect[posMid]/inpVect[posMax];
1055   }
1056
1057   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
1058   /* Computes the dot product of a and b */
1059   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
1060   template<int dim>
1061   inline double dotprod( const double * a, const double * b)
1062   {
1063     double result=0;
1064     for(int idim = 0; idim < dim ; idim++) result += a[idim]*b[idim];
1065     return result;
1066   }
1067   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
1068   /* Computes the norm of vector v */
1069   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
1070   template<int dim>
1071   inline double norm(const double * v)
1072   {
1073     double result =0;
1074     for(int idim =0; idim<dim; idim++) result+=v[idim]*v[idim];
1075     return sqrt(result);
1076   }
1077   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
1078   /* Computes the square norm of vector a-b */
1079   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
1080   template<int dim>
1081   inline double distance2( const double * a, const double * b)
1082   {
1083     double result =0;
1084     for(int idim =0; idim<dim; idim++) result+=(a[idim]-b[idim])*(a[idim]-b[idim]);
1085     return result;
1086   }
1087   template<class T, int dim>
1088   inline double distance2(  T * a, std::size_t inda, T * b, std::size_t indb)
1089   {
1090     double result =0;
1091     for(int idim =0; idim<dim; idim++) result += ((*a)[inda+idim] - (*b)[indb+idim])* ((*a)[inda+idim] - (*b)[indb+idim]);
1092     return result;
1093   }
1094   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
1095   /* Computes the determinant of a and b */
1096   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
1097   inline double determinant ( double *  a, double * b)
1098   {
1099     return a[0]*b[1]-a[1]*b[0];
1100   }
1101   inline double determinant ( double *  a, double * b, double * c)
1102   {
1103     return a[0]*determinant(b+1,c+1)-b[0]*determinant(a+1,c+1)+c[0]*determinant(a+1,b+1);
1104   }
1105
1106   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
1107   /* Computes the cross product of AB and AC */
1108   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
1109
1110   template<int dim> inline void crossprod( const double * A, const double * B, const double * C, double * V);
1111
1112   template<> inline
1113   void crossprod<2>( const double * A, const double * B, const double * C, double * V)
1114   {
1115     double AB[2];
1116     double AC[2];
1117     for(int idim =0; idim<2; idim++) AB[idim] = B[idim]-A[idim];//B-A
1118     for(int idim =0; idim<2; idim++) AC[idim] = C[idim]-A[idim];//C-A;
1119
1120     V[0]=determinant(AB,AC);
1121     V[1]=0;
1122   }
1123   template<> inline
1124   void crossprod<3>( const double * A, const double * B, const double * C, double * V)
1125   {
1126     double AB[3];
1127     double AC[3];
1128     for(int idim =0; idim<3; idim++) AB[idim] = B[idim]-A[idim];//B-A
1129     for(int idim =0; idim<3; idim++) AC[idim] = C[idim]-A[idim];//C-A;
1130
1131     V[0]=AB[1]*AC[2]-AB[2]*AC[1];
1132     V[1]=-AB[0]*AC[2]+AB[2]*AC[0];
1133     V[2]=AB[0]*AC[1]-AB[1]*AC[0];
1134   }
1135   template<> inline
1136   void crossprod<1>( const double * /*A*/, const double * /*B*/, const double * /*C*/, double * /*V*/)
1137   {
1138     // just to be able to compile
1139   }
1140
1141   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
1142   /* Checks whether point A is inside the quadrangle BCDE */
1143   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
1144
1145   template<int dim> inline double check_inside(const double* A,const double* B,const double* C,const double* D,
1146                                                const double* E,double* ABC, double* ADE)
1147   {
1148     crossprod<dim>(A,B,C,ABC);
1149     crossprod<dim>(A,D,E,ADE);
1150     return dotprod<dim>(ABC,ADE);
1151   }
1152
1153
1154   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
1155   /* Computes the geometric angle (in [0,Pi]) between two non zero vectors AB and AC */
1156   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
1157   template<int dim> inline double angle(const double * A, const double * B, const double * C, double * n)
1158   {
1159     double AB[dim];
1160     double AC[dim];
1161     double orthAB[dim];
1162
1163     for(int idim =0; idim<dim; idim++) AB[idim] = B[idim]-A[idim];//B-A;
1164     for(int idim =0; idim<dim; idim++) AC[idim] = C[idim]-A[idim];//C-A;
1165
1166     double normAB= norm<dim>(AB);
1167     for(int idim =0; idim<dim; idim++) AB[idim]/=normAB;
1168
1169     double normAC= norm<dim>(AC);
1170     double AB_dot_AC=dotprod<dim>(AB,AC);
1171     for(int idim =0; idim<dim; idim++) orthAB[idim] = AC[idim]-AB_dot_AC*AB[idim];
1172
1173     double denom= normAC+AB_dot_AC;
1174     double numer=norm<dim>(orthAB);
1175
1176     return 2*atan2(numer,denom);
1177   }
1178
1179   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
1180   /* Tells whether the frame constituted of vectors AB, AC and n is direct */
1181   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
1182   template<int dim> inline double direct_frame(const double * A, const double * B, const double * C, double * n);
1183   template<> inline
1184   double direct_frame<2>(const double * A, const double * B, const double * C, double * n)
1185   {
1186     double AB[2];
1187     double AC[2];
1188     for(int idim =0; idim<2; idim++) AB[idim] = B[idim]-A[idim];//B-A;
1189     for(int idim =0; idim<2; idim++) AC[idim] = C[idim]-A[idim];//C-A;
1190
1191     return  determinant(AB,AC)*n[0];
1192   }
1193   template<> inline
1194   double direct_frame<3>(const double * A, const double * B, const double * C, double * n)
1195   {
1196     double AB[3];
1197     double AC[3];
1198     for(int idim =0; idim<3; idim++) AB[idim] = B[idim]-A[idim];//B-A;
1199     for(int idim =0; idim<3; idim++) AC[idim] = C[idim]-A[idim];//C-A;
1200
1201     return determinant(AB,AC,n)>0;
1202   }
1203
1204   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
1205   /*      calcul l'intersection de deux polygones COPLANAIRES */
1206   /* en dimension DIM (2 ou 3). Si DIM=3 l'algorithme ne considere*/
1207   /* que les deux premieres coordonnees de chaque point */
1208   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
1209   template<int DIM> inline void intersec_de_polygone(const double * Coords_A, const double * Coords_B,
1210                                                      int nb_NodesA, int nb_NodesB,
1211                                                      std::vector<double>& inter,
1212                                                      double dim_caracteristic, double precision)
1213   {
1214     for(int i_A = 1; i_A<nb_NodesA-1; i_A++)
1215       {
1216         for(int i_B = 1; i_B<nb_NodesB-1; i_B++)
1217           {
1218             INTERP_KERNEL::intersec_de_triangle(&Coords_A[0],&Coords_A[DIM*i_A],&Coords_A[DIM*(i_A+1)],
1219                                                 &Coords_B[0],&Coords_B[DIM*i_B],&Coords_B[DIM*(i_B+1)],
1220                                                 inter, dim_caracteristic, precision);
1221           }
1222       }
1223     int nb_inter=((int)inter.size())/DIM;
1224     if(nb_inter >3) inter=INTERP_KERNEL::reconstruct_polygon(inter);
1225   }
1226
1227   /*_ _ _ _ _ _ _ _ _
1228    *_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _
1229    *  fonctions qui calcule l'aire d'un polygone en dimension 2 ou 3
1230    *_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
1231   template<int DIM> inline double polygon_area(std::vector<double>& inter)
1232   {
1233     double result=0.;
1234     double area[DIM];
1235
1236     for(int i = 1; i<(int)inter.size()/DIM-1; i++)
1237       {
1238         INTERP_KERNEL::crossprod<DIM>(&inter[0],&inter[DIM*i],&inter[DIM*(i+1)],area);
1239         result +=0.5*norm<DIM>(area);
1240       }
1241     return result;
1242   }
1243
1244   template<int DIM> inline double polygon_area(std::deque<double>& inter)
1245   {
1246     double result=0.;
1247     double area[DIM];
1248
1249     for(int i = 1; i<(int)inter.size()/DIM-1; i++)
1250       {
1251         INTERP_KERNEL::crossprod<DIM>(&inter[0],&inter[DIM*i],&inter[DIM*(i+1)],area);
1252         result +=0.5*norm<DIM>(area);
1253       }
1254     return result;
1255   }
1256
1257   /*!
1258    * This method normalize input "tetrahedrized polyhedron" to put it around 0,0,0 point and with the normalization factor.
1259    *
1260    * \param [in,out] ptsOfTetrahedrizedPolyhedron nbfaces * 3 * 3 vector that store in full interlace all the 3 first points of each face of the input polyhedron
1261    * \param [in] nbfaces number of faces in the tetrahedrized polyhedron to be normalized
1262    * \param [out] centerPt the center of input tetrahedrized polyhedron
1263    * \return the normalization factor corresponding to the max amplitude among all nbfaces*3 input points and among X, Y and Z axis.
1264    */
1265   inline double NormalizeTetrahedrizedPolyhedron(double *ptsOfTetrahedrizedPolyhedron, int nbfaces, double centerPt[3])
1266   {
1267     centerPt[0] = 0.0; centerPt[1] = 0.0; centerPt[2] = 0.0;
1268     double minMax[6]={ std::numeric_limits<double>::max(), -std::numeric_limits<double>::max(),
1269     std::numeric_limits<double>::max(), -std::numeric_limits<double>::max(),
1270     std::numeric_limits<double>::max(), -std::numeric_limits<double>::max() };
1271     for(int iPt = 0 ; iPt < 3 * nbfaces ; ++iPt)
1272     {
1273       for(int k = 0 ; k < 3 ; ++k)
1274       {
1275         minMax[2*k]   = std::min(minMax[2*k],ptsOfTetrahedrizedPolyhedron[3*iPt+k]);
1276         minMax[2*k+1] = std::max(minMax[2*k+1],ptsOfTetrahedrizedPolyhedron[3*iPt+k]);
1277       }
1278     }
1279     double normFact = 0.0;
1280     for(int k = 0 ; k < 3 ; ++k)
1281     {
1282       centerPt[k] = (minMax[2*k] + minMax[2*k+1]) / 2.0 ;
1283       normFact = std::max(minMax[2*k+1] - minMax[2*k], normFact);
1284     }
1285     // renormalize into ptsOfTetrahedrizedPolyhedron
1286     for(int iPt = 0 ; iPt < 3 * nbfaces ; ++iPt)
1287     {
1288       for(int k = 0 ; k < 3 ; ++k)
1289       {
1290         ptsOfTetrahedrizedPolyhedron[3*iPt+k] = ( ptsOfTetrahedrizedPolyhedron[3*iPt+k] - centerPt[k] ) / normFact;
1291       }
1292     }
1293     return normFact;
1294   }
1295
1296   /*!
1297    * Computes the triple product (XA^XB).XC/(norm(XA^XB)) (in 3D)
1298    * Returned value is equal to the distance (positive or negative depending of the position of C relative to XAB plane) between XAB plane and C point.
1299    */
1300   inline double TripleProduct(const double *A, const double *B, const double *C, const double *X)
1301   {
1302     double XA[3]={ A[0]-X[0], A[1]-X[1], A[2]-X[2] };
1303     double XB[3]={ B[0]-X[0], B[1]-X[1], B[2]-X[2] };
1304     double XC[3]={ C[0]-X[0], C[1]-X[1], C[2]-X[2] };
1305
1306     double XA_cross_XB[3] = {XA[1]*XB[2]-XA[2]*XB[1], XA[2]*XB[0]-XA[0]*XB[2], XA[0]*XB[1]-XA[1]*XB[0]};
1307     // norm is equal to double the area of the triangle
1308     double norm = std::sqrt(XA_cross_XB[0]*XA_cross_XB[0]+XA_cross_XB[1]*XA_cross_XB[1]+XA_cross_XB[2]*XA_cross_XB[2]);
1309
1310     return ( XA_cross_XB[0]*XC[0]+ XA_cross_XB[1]*XC[1] + XA_cross_XB[2]*XC[2] ) / norm;
1311   }
1312
1313   /*! Subroutine of checkEqualPolygins that tests if two list of nodes (not necessarily distincts) describe the same polygon, assuming they share a common point.*/
1314   /*! Indexes istart1 and istart2 designate two points P1 in L1 and P2 in L2 that have identical coordinates. Generally called with istart1=0.*/
1315   /*! Integer sign ( 1 or -1) indicate the direction used in going all over L2. */
1316   template<class T, int dim>
1317   bool checkEqualPolygonsOneDirection(T * L1, T * L2, std::size_t size1, std::size_t size2, std::size_t istart1, std::size_t istart2, double epsilon, int sign)
1318   {
1319     std::size_t i1 = istart1;
1320     std::size_t i2 = istart2;
1321     std::size_t i1next = ( i1 + 1 ) % size1;
1322     std::size_t i2next = ( i2 + sign +size2) % size2;
1323
1324     while(true)
1325       {
1326         while( i1next!=istart1 && distance2<T,dim>(L1,i1*dim, L1,i1next*dim) < epsilon ) i1next = (  i1next + 1 ) % size1;
1327         while( i2next!=istart2 && distance2<T,dim>(L2,i2*dim, L2,i2next*dim) < epsilon ) i2next = (  i2next + sign +size2 ) % size2;
1328
1329         if(i1next == istart1)
1330           {
1331             if(i2next == istart2)
1332               return true;
1333             else return false;
1334           }
1335         else
1336           if(i2next == istart2)
1337             return false;
1338           else
1339             {
1340               if(distance2<T,dim>(L1,i1next*dim, L2,i2next*dim) > epsilon )
1341                 return false;
1342               else
1343                 {
1344                   i1 = i1next;
1345                   i2 = i2next;
1346                   i1next = ( i1 + 1 ) % size1;
1347                   i2next = ( i2 + sign + size2 ) % size2;
1348                 }
1349             }
1350       }
1351   }
1352
1353   /*! Tests if two list of nodes (not necessarily distincts) describe the same polygon.*/
1354   /*! Existence of multiple points in the list is considered.*/
1355   template<class T, int dim>
1356   bool checkEqualPolygons(T * L1, T * L2, double epsilon)
1357   {
1358     if(L1==NULL || L2==NULL)
1359       {
1360         std::cout << "Warning InterpolationUtils.hxx:checkEqualPolygonsPointer: Null pointer " << std::endl;
1361         throw(Exception("big error: not closed polygon..."));
1362       }
1363
1364     std::size_t size1 = (*L1).size()/dim;
1365     std::size_t size2 = (*L2).size()/dim;
1366     std::size_t istart1 = 0;
1367     std::size_t istart2 = 0;
1368
1369     while( istart2 < size2  && distance2<T,dim>(L1,istart1*dim, L2,istart2*dim) > epsilon ) istart2++;
1370
1371     if(istart2 == size2)
1372       {
1373         return (size1 == 0) && (size2 == 0);
1374       }
1375     else
1376       return   checkEqualPolygonsOneDirection<T,dim>( L1, L2, size1, size2, istart1, istart2, epsilon,  1)
1377         || checkEqualPolygonsOneDirection<T,dim>( L1, L2, size1, size2, istart1, istart2, epsilon, -1);
1378
1379   }
1380 }
1381
1382
1383 #endif