src/INTERP_KERNEL/InterpolationUtils.hxx

   1 // Copyright (C) 2007-2020  CEA/DEN, EDF R&D
   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::bind2nd(std::multiplies<double>(),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::bind2nd(std::multiplies<double>(),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::bind2nd(std::multiplies<double>(),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 triIn is a 6 doubles array in full interlace mode, that represents a triangle.
 170    * @param quadOut 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::bind2nd(std::multiplies<double>(),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::bind2nd(std::multiplies<double>(),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::bind2nd(std::multiplies<double>(),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 M - N x N+NB_OF_VARS matrix
 341    *  \param sol - vector of N solutions
 342    *  \retval bool - true if succeeded
 343    */
 344   template<unsigned SZ, unsigned NB_OF_RES>
 345   bool solveSystemOfEquations2(const double *matrix, double *solutions, double eps)
 346   {
 347     unsigned k,j;
 348     int nr,n,m,np;
 349     double s,g;
 350     int mb;
 351     //
 352     double B[SZ*(SZ+NB_OF_RES)];
 353     std::copy(matrix,matrix+SZ*(SZ+NB_OF_RES),B);
 354     //
 355     nr=SZ+NB_OF_RES;
 356     for(k=0;k<SZ;k++)
 357       {
 358         np=nr*k+k;
 359         if(fabs(B[np])<eps)
 360           {
 361             n=k;
 362             do
 363               {
 364                 n++;
 365                 if(fabs(B[nr*k+n])>eps)
 366                   {/* Rows permutation */
 367                     for(m=0;m<nr;m++)
 368                       std::swap(B[nr*k+m],B[nr*n+m]);
 369                   }
 370               }
 371             while (n<(int)SZ);
 372           }
 373         s=B[np];//s is the Pivot
 374         std::transform(B+k*nr,B+(k+1)*nr,B+k*nr,std::bind2nd(std::divides<double>(),s));
 375         for(j=0;j<SZ;j++)
 376           {
 377             if(j!=k)
 378               {
 379                 g=B[j*nr+k];
 380                 for(mb=k;mb<nr;mb++)
 381                   B[j*nr+mb]-=B[k*nr+mb]*g;
 382               }
 383           }
 384       }
 385     for(j=0;j<NB_OF_RES;j++)
 386       for(k=0;k<SZ;k++)
 387         solutions[j*SZ+k]=B[nr*k+SZ+j];
 388     //
 389     return true;
 390   }
 391
 392   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
 393   /*     Calculate barycentric coordinates of a 2D point p */
 394   /*     with respect to the triangle verices.             */
 395   /*     triaCoords are in full interlace                  */
 396   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
 397
 398   template<int SPACEDIM>
 399   inline void barycentric_coords(const double* triaCoords, const double* p, double* bc)
 400   {
 401     // matrix 2x2
 402     double
 403       T11 = triaCoords[0]-triaCoords[2*SPACEDIM], T12 = triaCoords[SPACEDIM]-triaCoords[2*SPACEDIM],
 404       T21 = triaCoords[1]-triaCoords[2*SPACEDIM+1], T22 = triaCoords[SPACEDIM+1]-triaCoords[2*SPACEDIM+1];
 405     // matrix determinant
 406     double Tdet = T11*T22 - T12*T21;
 407     if ( fabs( Tdet ) < std::numeric_limits<double>::min() ) {
 408       bc[0]=1; bc[1]=0; bc[2]=0;
 409       return;
 410     }
 411     // matrix inverse
 412     double t11 = T22, t12 = -T12, t21 = -T21, t22 = T11;
 413     // vector
 414     double r11 = p[0]-triaCoords[2*SPACEDIM], r12 = p[1]-triaCoords[2*SPACEDIM+1];
 415     // barycentric coordinates: multiply matrix by vector
 416     bc[0] = (t11 * r11 + t12 * r12)/Tdet;
 417     bc[1] = (t21 * r11 + t22 * r12)/Tdet;
 418     bc[2] = 1. - bc[0] - bc[1];
 419   }
 420
 421   inline void barycentric_coords_seg2(const std::vector<const double*>& n, const double *p, double *bc)
 422   {
 423     double delta=n[0][0]-n[1][0];
 424     bc[0]=fabs((*p-n[1][0])/delta);
 425     bc[1]=fabs((*p-n[0][0])/delta);
 426   }
 427
 428   inline void barycentric_coords_tri3(const std::vector<const double*>& n, const double *p, double *bc)
 429   {
 430     enum { _XX=0, _YY, _ZZ };
 431     // matrix 2x2
 432     double
 433       T11 = n[0][_XX]-n[2][_XX], T12 = n[1][_XX]-n[2][_XX],
 434       T21 = n[0][_YY]-n[2][_YY], T22 = n[1][_YY]-n[2][_YY];
 435     // matrix determinant
 436     double Tdet = T11*T22 - T12*T21;
 437     if ( (std::fabs( Tdet) ) < (std::numeric_limits<double>::min()) )
 438       {
 439         bc[0]=1; bc[1]=bc[2]=0; // no solution
 440         return;
 441       }
 442     // matrix inverse
 443     double t11 = T22, t12 = -T12, t21 = -T21, t22 = T11;
 444     // vector
 445     double r11 = p[_XX]-n[2][_XX], r12 = p[_YY]-n[2][_YY];
 446     // barycentric coordinates: multiply matrix by vector
 447     bc[0] = (t11 * r11 + t12 * r12)/Tdet;
 448     bc[1] = (t21 * r11 + t22 * r12)/Tdet;
 449     bc[2] = 1. - bc[0] - bc[1];
 450   }
 451
 452   inline void barycentric_coords_quad4(const std::vector<const double*>& n, const double *p, double *bc)
 453   {
 454     enum { _XX=0, _YY, _ZZ };
 455     // Find bc by solving system of 3 equations using Gaussian elimination algorithm
 456     // bc1*( x1 - x4 ) + bc2*( x2 - x4 ) + bc3*( x3 - x4 ) = px - x4
 457     // bc1*( y1 - y4 ) + bc2*( y2 - y4 ) + bc3*( y3 - y4 ) = px - y4
 458     // bc1*( z1 - z4 ) + bc2*( z2 - z4 ) + bc3*( z3 - z4 ) = px - z4
 459
 460     double T[3][4]=
 461       {{ n[0][_XX]-n[3][_XX], n[1][_XX]-n[3][_XX], n[2][_XX]-n[3][_XX], p[_XX]-n[3][_XX] },
 462        { n[0][_YY]-n[3][_YY], n[1][_YY]-n[3][_YY], n[2][_YY]-n[3][_YY], p[_YY]-n[3][_YY] },
 463        { n[0][_ZZ]-n[3][_ZZ], n[1][_ZZ]-n[3][_ZZ], n[2][_ZZ]-n[3][_ZZ], p[_ZZ]-n[3][_ZZ] }};
 464
 465     if ( !solveSystemOfEquations<3>( T, bc ) )
 466       bc[0]=1., bc[1] = bc[2] = bc[3] = 0;
 467     else
 468       bc[ 3 ] = 1. - bc[0] - bc[1] - bc[2];
 469   }
 470
 471   inline void barycentric_coords_tri6(const std::vector<const double*>& n, const double *p, double *bc)
 472   {
 473     double matrix2[48]={1., 0., 0., 0., 0., 0., 0., 0.,
 474                         1., 0., 0., 0., 0., 0., 1., 0.,
 475                         1., 0., 0., 0., 0., 0., 0., 1.,
 476                         1., 0., 0., 0., 0., 0., 0.5, 0.,
 477                         1., 0., 0., 0., 0., 0., 0.5, 0.5,
 478                         1., 0., 0., 0., 0., 0., 0.,0.5};
 479     for(int i=0;i<6;i++)
 480       {
 481         matrix2[8*i+1]=n[i][0];
 482         matrix2[8*i+2]=n[i][1];
 483         matrix2[8*i+3]=n[i][0]*n[i][0];
 484         matrix2[8*i+4]=n[i][0]*n[i][1];
 485         matrix2[8*i+5]=n[i][1]*n[i][1];
 486       }
 487     double res[12];
 488     solveSystemOfEquations2<6,2>(matrix2,res,std::numeric_limits<double>::min());
 489     double refCoo[2];
 490     refCoo[0]=computeTria6RefBase(res,p);
 491     refCoo[1]=computeTria6RefBase(res+6,p);
 492     computeWeightedCoeffsInTria6FromRefBase(refCoo,bc);
 493   }
 494
 495   inline void barycentric_coords_tetra10(const std::vector<const double*>& n, const double *p, double *bc)
 496   {
 497     double matrix2[130]={1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.,
 498                          1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 1., 0., 0.,
 499                          1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 1., 0.,
 500                          1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 1.,
 501                          1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.5, 0., 0.,
 502                          1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.5, 0.5, 0.,
 503                          1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.,0.5, 0.,
 504                          1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.5,
 505                          1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.5, 0., 0.5,
 506                          1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.5, 0.5};
 507     for(int i=0;i<10;i++)
 508       {
 509         matrix2[13*i+1]=n[i][0];
 510         matrix2[13*i+2]=n[i][1];
 511         matrix2[13*i+3]=n[i][2];
 512         matrix2[13*i+4]=n[i][0]*n[i][0];
 513         matrix2[13*i+5]=n[i][0]*n[i][1];
 514         matrix2[13*i+6]=n[i][0]*n[i][2];
 515         matrix2[13*i+7]=n[i][1]*n[i][1];
 516         matrix2[13*i+8]=n[i][1]*n[i][2];
 517         matrix2[13*i+9]=n[i][2]*n[i][2];
 518       }
 519     double res[30];
 520     solveSystemOfEquations2<10,3>(matrix2,res,std::numeric_limits<double>::min());
 521     double refCoo[3];
 522     refCoo[0]=computeTetra10RefBase(res,p);
 523     refCoo[1]=computeTetra10RefBase(res+10,p);
 524     refCoo[2]=computeTetra10RefBase(res+20,p);
 525     computeWeightedCoeffsInTetra10FromRefBase(refCoo,bc);
 526   }
 527
 528   /*!
 529    * Calculate barycentric coordinates of a point p with respect to triangle or tetra vertices.
 530    * This method makes 2 assumptions :
 531    *    - this is a simplex
 532    *    - 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.
 533    *      If not the case (3D surf for example) a previous projection should be done before.
 534    */
 535   inline void barycentric_coords(const std::vector<const double*>& n, const double *p, double *bc)
 536   {
 537     switch(n.size())
 538       {
 539       case 2:
 540         {// SEG 2
 541           barycentric_coords_seg2(n,p,bc);
 542           break;
 543         }
 544       case 3:
 545         { // TRIA3
 546           barycentric_coords_tri3(n,p,bc);
 547           break;
 548         }
 549       case 4:
 550         { // TETRA4
 551           barycentric_coords_quad4(n,p,bc);
 552           break;
 553         }
 554       case 6:
 555         {
 556           // TRIA6
 557           barycentric_coords_tri6(n,p,bc);
 558           break;
 559         }
 560       case 10:
 561         {//TETRA10
 562           barycentric_coords_tetra10(n,p,bc);
 563           break;
 564         }
 565       default:
 566         throw INTERP_KERNEL::Exception("INTERP_KERNEL::barycentric_coords : unrecognized simplex !");
 567       }
 568   }
 569
 570   inline void barycentric_coords(INTERP_KERNEL::NormalizedCellType ct, const std::vector<const double*>& n, const double *p, double *bc)
 571   {
 572     switch(ct)
 573       {
 574       case NORM_SEG2:
 575         {// SEG 2
 576           barycentric_coords_seg2(n,p,bc);
 577           break;
 578         }
 579       case NORM_TRI3:
 580         { // TRIA3
 581           barycentric_coords_tri3(n,p,bc);
 582           break;
 583         }
 584       case NORM_TETRA4:
 585         { // TETRA4
 586           barycentric_coords_quad4(n,p,bc);
 587           break;
 588         }
 589       case NORM_TRI6:
 590         {
 591           // TRIA6
 592           barycentric_coords_tri6(n,p,bc);
 593           break;
 594         }
 595       case NORM_TETRA10:
 596         {//TETRA10
 597           barycentric_coords_tetra10(n,p,bc);
 598           break;
 599         }
 600       default:
 601         throw INTERP_KERNEL::Exception("INTERP_KERNEL::barycentric_coords : unrecognized simplex !");
 602       }
 603   }
 604
 605   /*!
 606    * Compute barycentric coords of point \a point relative to segment S [segStart,segStop] in 3D space.
 607    * If point \a point is further from S than eps false is returned.
 608    * If point \a point projection on S is outside S false is also returned.
 609    * 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.
 610    */
 611   inline bool IsPointOn3DSeg(const double segStart[3], const double segStop[3], const double point[3], double eps, double& bc0, double& bc1)
 612   {
 613     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]};
 614     double l_AB(sqrt(AB[0]*AB[0]+AB[1]*AB[1]+AB[2]*AB[2]));
 615     double AP_dot_AB((AP[0]*AB[0]+AP[1]*AB[1]+AP[2]*AB[2])/(l_AB*l_AB));
 616     double projOfPOnAB[3]={segStart[0]+AP_dot_AB*AB[0],segStart[1]+AP_dot_AB*AB[1],segStart[2]+AP_dot_AB*AB[2]};
 617     double V_dist_P_AB[3]={point[0]-projOfPOnAB[0],point[1]-projOfPOnAB[1],point[2]-projOfPOnAB[2]};
 618     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]));
 619     if(dist_P_AB>=eps)
 620       return false;//to far from segment [segStart,segStop]
 621     if(AP_dot_AB<-eps || AP_dot_AB>1.+eps)
 622       return false;
 623     AP_dot_AB=std::max(AP_dot_AB,0.); AP_dot_AB=std::min(AP_dot_AB,1.);
 624     bc0=1.-AP_dot_AB; bc1=AP_dot_AB;
 625     return true;
 626   }
 627
 628   /*!
 629    * Calculate pseudo barycentric coordinates of a point p with respect to the quadrangle vertices.
 630    * This method makes the assumption that:
 631    *  - spacedim == meshdim (2 here).
 632    *  - the point is within the quad
 633    *  Quadratic elements are not supported yet.
 634    *
 635    *  A quadrangle can be described as 3 vectors, one point being taken as the origin.
 636    *  Denoting A, B, C the three other points, any point P within the quad is written as
 637    *    P = xA+ yC + xy(B-A-C)
 638    *  This method solve those 2 equations (one per component) for x and y.
 639    *
 640
 641           A------B
 642           |      |
 643           |      |
 644           0------C
 645    */
 646   inline void quad_mapped_coords(const std::vector<const double*>& n, const double *p, double *bc)
 647   {
 648     double prec = 1.0e-14;
 649     enum { _XX=0, _YY, _ZZ };
 650
 651     if(n.size() != 4)
 652       throw INTERP_KERNEL::Exception("INTERP_KERNEL::quad_mapped_coords : unrecognized geometric type! Only QUAD4 supported.");
 653
 654     double A[2] = {n[1][_XX] - n[0][_XX],  n[1][_YY] - n[0][_YY]};
 655     double B[2] = {n[2][_XX] - n[0][_XX],  n[2][_YY] - n[0][_YY]};
 656     double C[2] = {n[3][_XX] - n[0][_XX],  n[3][_YY] - n[0][_YY]};
 657     double N[2] = {B[_XX] - A[_XX] - C[_XX], B[_YY] - A[_YY] - C[_YY]};
 658     double P[2] = {p[_XX] - n[0][_XX], p[_YY] - n[0][_YY]};
 659
 660     // degenerated case: a rectangle:
 661     if (fabs(N[0]) < prec && fabs(N[1]) < prec)
 662       {
 663         double det = C[0]*A[1] -C[1]*A[0];
 664         if (fabs(det) < prec)
 665           throw INTERP_KERNEL::Exception("MappedBarycentric intersection type: quad_mapped_coords() has a degenerated 2x2 system!");
 666         bc[0] = (P[0]*A[1]-P[1]*A[0])/det;
 667         bc[1] = (P[1]*C[0]-P[0]*C[1])/det;
 668         return;
 669       }
 670     double b,c ,a = A[1]*N[0]-A[0]*N[1];
 671     bool cas1;
 672     if (fabs(a) > 1.0e-14)
 673       {
 674         b = A[1]*C[0]+N[1]*P[0]-N[0]*P[1]-A[0]*C[1];
 675         c = P[0]*C[1] - P[1]*C[0];
 676         cas1 = true;
 677       }
 678     else
 679       {
 680         a = -C[1]*N[0]+C[0]*N[1];
 681         b = A[1]*C[0]-N[1]*P[0]+N[0]*P[1]-A[0]*C[1];
 682         c = -P[0]*A[1] + P[1]*A[0];
 683         cas1 = false;
 684       }
 685     double delta = b*b - 4.0*a*c;
 686     if (delta < 0.0)
 687       throw INTERP_KERNEL::Exception("MappedBarycentric intersection type: quad_mapped_coords(): imaginary solutions!");
 688     bc[1] = 0.5*(-b+sqrt(delta))/a;
 689     if (bc[1] < -prec || bc[1] > (1.0+prec))
 690       bc[1] = 0.5*(-b-sqrt(delta))/a;
 691     if (bc[1] < -prec || bc[1] > (1.0+prec))
 692       throw INTERP_KERNEL::Exception("MappedBarycentric intersection type: quad_mapped_coords(): point doesn't seem to be in quad4!");
 693     if (cas1)
 694       {
 695         double denom = C[0]+bc[1]*N[0];
 696         if (fabs(denom) < prec)
 697           throw INTERP_KERNEL::Exception("MappedBarycentric intersection type: quad_mapped_coords(): point doesn't seem to be in quad4!");
 698         bc[0] = (P[0]-bc[1]*A[0])/denom;
 699         if (bc[0] < -prec || bc[0] > (1.0+prec))
 700           throw INTERP_KERNEL::Exception("MappedBarycentric intersection type: quad_mapped_coords(): point doesn't seem to be in quad4!");
 701       }
 702     else
 703       {
 704         bc[0] = bc[1];
 705         double denom = A[1]+bc[0]*N[1];
 706         if (fabs(denom) < prec)
 707           throw INTERP_KERNEL::Exception("MappedBarycentric intersection type: cuboid_mapped_coord(): point doesn't seem to be in quad4!");
 708         bc[1] = (P[1]-bc[0]*C[1])/denom;
 709         if (bc[1] < -prec || bc[1] > (1.0+prec))
 710           throw INTERP_KERNEL::Exception("MappedBarycentric intersection type: cuboid_mapped_coord(): point doesn't seem to be in quad4!");
 711       }
 712   }
 713
 714   /*!
 715    * Doing as in quad_mapped_coords() would lead to a 4th order equation ... So go simpler here:
 716    * orthogonal distance to each pair of parallel faces is computed. The ratio gives a number in [0,1]
 717    *
 718    * Conventions:
 719    *   - for HEXA8, point F (5) is taken to be the origin (see med file ref connec):
 720    *          0 ------ 3
 721              /|       /|
 722             / |      / |
 723            1 ------ 2  |
 724            |  |     |  |
 725            |  |     |  |
 726            |  4-----|- 7
 727            | /      | /
 728            5 ------ 6
 729
 730    *
 731    */
 732
 733   inline void cuboid_mapped_coords(const std::vector<const double*>& n, const double *p, double *bc)
 734   {
 735     double prec = 1.0e-14;
 736     enum { _XX=0, _YY };
 737     if (n.size() != 8)
 738       throw INTERP_KERNEL::Exception("INTERP_KERNEL::cuboid_mapped_coords: unrecognized geometric type! Only HEXA8 supported.");
 739
 740     double dx1, dx2, dy1, dy2, dz1, dz2;
 741     dx1 = OrthoDistanceFromPtToPlaneInSpaceDim3(p, n[4],n[5],n[1]);
 742     dx2 = OrthoDistanceFromPtToPlaneInSpaceDim3(p, n[7],n[3],n[2]);
 743
 744     dy1 = OrthoDistanceFromPtToPlaneInSpaceDim3(p, n[5],n[6],n[2]);
 745     dy2 = OrthoDistanceFromPtToPlaneInSpaceDim3(p, n[4],n[0],n[3]);
 746
 747     dz1 = OrthoDistanceFromPtToPlaneInSpaceDim3(p, n[5],n[4],n[7]);
 748     dz2 = OrthoDistanceFromPtToPlaneInSpaceDim3(p, n[1],n[2],n[3]);
 749
 750     if (dx1 < -prec || dx2 < -prec || dy1 < -prec || dy2 < -prec || dz1 < -prec || dz2 < -prec)
 751       throw INTERP_KERNEL::Exception("INTERP_KERNEL::cuboid_mapped_coords: point outside HEXA8");
 752
 753     bc[0] = dx1+dx2 < prec ? 0.5 : dx1/(dx1+dx2);
 754     bc[1] = dy1+dy2 < prec ? 0.5 : dy1/(dy1+dy2);
 755     bc[2] = dz1+dz2 < prec ? 0.5 : dz1/(dz1+dz2);
 756   }
 757
 758   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
 759   /*         calcul la surface d'un polygone.                 */
 760   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
 761
 762   inline double  Surf_Poly(const std::vector<double>& Poly)
 763   {
 764
 765     double Surface=0;
 766     for(unsigned long i=0; i<(Poly.size())/2-2; i++)
 767       {
 768         double Surf=Surf_Tri( &Poly[0],&Poly[2*(i+1)],&Poly[2*(i+2)] );
 769         Surface=Surface + Surf ;
 770       }
 771     return Surface ;
 772   }
 773
 774   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
 775   /*   fonction qui teste si un point est dans une maille     */
 776   /*   point: P_0                                             */
 777   /*   P_1, P_2, P_3 sommet des mailles                       */
 778   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
 779
 780   inline bool point_dans_triangle(const double* P_0,const double* P_1,
 781                                   const double* P_2,const double* P_3,
 782                                   double eps)
 783   {
 784
 785     bool A=false;
 786     double det_1=mon_determinant(P_1,P_3,P_0);
 787     double det_2=mon_determinant(P_3,P_2,P_0);
 788     double det_3=mon_determinant(P_2,P_1,P_0);
 789     if( (det_1>=-eps && det_2>=-eps && det_3>=-eps) || (det_1<=eps && det_2<=eps && det_3<=eps) )
 790       {
 791         A=true;
 792       }
 793
 794     return A;
 795   }
 796
 797   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
 798   /*fonction pour verifier qu'un point n'a pas deja ete considerer dans   */
 799   /*      le vecteur et le rajouter au vecteur sinon.                     */
 800   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
 801
 802   inline void verif_point_dans_vect(const double* P, std::vector<double>& V, double absolute_precision )
 803   {
 804     std::size_t taille=V.size();
 805     bool isPresent=false;
 806     for(std::size_t i=0;i<taille/2;i++)
 807       {
 808         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)
 809           isPresent=true;
 810
 811       }
 812     if(!isPresent)
 813       {
 814
 815         V.push_back(P[0]);
 816         V.push_back(P[1]);
 817       }
 818   }
 819
 820   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
 821   /* fonction qui rajoute les sommet du triangle P dans le vecteur V        */
 822   /* si ceux-ci sont compris dans le triangle S et ne sont pas deja dans    */
 823   /* V.                                                                     */
 824   /*sommets de P: P_1, P_2, P_3                                             */
 825   /*sommets de S: P_4, P_5, P_6                                             */
 826   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
 827
 828   inline void rajou_sommet_triangl(const double* P_1,const double* P_2,const double* P_3,
 829                                    const double* P_4,const double* P_5,const double* P_6,
 830                                    std::vector<double>& V, double dim_caracteristic, double precision)
 831   {
 832
 833     double absolute_precision = precision*dim_caracteristic;
 834     bool A_1=INTERP_KERNEL::point_dans_triangle(P_1,P_4,P_5,P_6,absolute_precision);
 835     if(A_1)
 836       verif_point_dans_vect(P_1,V,absolute_precision);
 837     bool A_2=INTERP_KERNEL::point_dans_triangle(P_2,P_4,P_5,P_6,absolute_precision);
 838     if(A_2)
 839       verif_point_dans_vect(P_2,V,absolute_precision);
 840     bool A_3=INTERP_KERNEL::point_dans_triangle(P_3,P_4,P_5,P_6,absolute_precision);
 841     if(A_3)
 842       verif_point_dans_vect(P_3,V,absolute_precision);
 843   }
 844
 845
 846   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _  _ _ _ _ _ _ _ _*/
 847   /*  calcul de l'intersection de deux segments: segments P1P2 avec P3P4      */
 848   /*  . Si l'intersection est non nulle et si celle-ci n'est                  */
 849   /*  n'est pas deja contenue dans Vect on la rajoute a Vect                  */
 850   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _  _ _ _ _ _ _ _ _*/
 851
 852   inline void inters_de_segment(const double * P_1,const double * P_2,
 853                                 const double * P_3,const double * P_4,
 854                                 std::vector<double>& Vect,
 855                                 double dim_caracteristic, double precision)
 856   {
 857     // calcul du determinant de P_1P_2 et P_3P_4.
 858     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]);
 859
 860     double absolute_precision = dim_caracteristic*precision;
 861     if(fabs(det)>absolute_precision)
 862       {
 863         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;
 864
 865         if (k_1 >= -absolute_precision && k_1 <= 1+absolute_precision)
 866           //if( k_1 >= -precision && k_1 <= 1+precision)
 867           {
 868             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;
 869
 870             if (k_2 >= -absolute_precision && k_2 <= 1+absolute_precision)
 871               //if( k_2 >= -precision && k_2 <= 1+precision)
 872               {
 873                 double P_0[2];
 874                 P_0[0]=P_1[0]+k_1*(P_2[0]-P_1[0]);
 875                 P_0[1]=P_1[1]+k_1*(P_2[1]-P_1[1]);
 876                 verif_point_dans_vect(P_0,Vect,absolute_precision);
 877               }
 878           }
 879       }
 880   }
 881
 882
 883
 884   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
 885   /*      calcul l'intersection de deux triangles            */
 886   /* P_1, P_2, P_3: sommets du premier triangle              */
 887   /* P_4, P_5, P_6: sommets du deuxi�me triangle             */
 888   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
 889
 890   inline void intersec_de_triangle(const double* P_1,const double* P_2, const double* P_3,
 891                                    const double* P_4,const double* P_5,const double* P_6,
 892                                    std::vector<double>& Vect, double dim_caracteristic, double precision)
 893   {
 894     inters_de_segment(P_1,P_2,P_4,P_5,Vect, dim_caracteristic, precision);
 895     inters_de_segment(P_1,P_2,P_5,P_6,Vect, dim_caracteristic, precision);
 896     inters_de_segment(P_1,P_2,P_6,P_4,Vect, dim_caracteristic, precision);
 897     inters_de_segment(P_2,P_3,P_4,P_5,Vect, dim_caracteristic, precision);
 898     inters_de_segment(P_2,P_3,P_5,P_6,Vect, dim_caracteristic, precision);
 899     inters_de_segment(P_2,P_3,P_6,P_4,Vect, dim_caracteristic, precision);
 900     inters_de_segment(P_3,P_1,P_4,P_5,Vect, dim_caracteristic, precision);
 901     inters_de_segment(P_3,P_1,P_5,P_6,Vect, dim_caracteristic, precision);
 902     inters_de_segment(P_3,P_1,P_6,P_4,Vect, dim_caracteristic, precision);
 903     rajou_sommet_triangl(P_1,P_2,P_3,P_4,P_5,P_6,Vect, dim_caracteristic, precision);
 904     rajou_sommet_triangl(P_4,P_5,P_6,P_1,P_2,P_3,Vect, dim_caracteristic, precision);
 905   }
 906
 907   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
 908   /* fonction pour verifier qu'un node maille n'a pas deja ete considerer  */
 909   /*  dans le vecteur et le rajouter au vecteur sinon.                     */
 910   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
 911
 912   inline void verif_maill_dans_vect(int Num, std::vector<int>& V)
 913   {
 914     std::size_t taille=V.size();
 915     int A=0;
 916     for(std::size_t i=0;i<taille;i++)
 917       {
 918         if(Num==V[i])
 919           {
 920             A=1;
 921             break;
 922           }
 923       }
 924     if(A==0)
 925       {V.push_back(Num); }
 926   }
 927
 928   /*! Function that compares two angles from the values of the pairs (sin,cos)*/
 929   /*! Angles are considered in [0, 2Pi] bt are not computed explicitly */
 930   class AngleLess
 931   {
 932   public:
 933     bool operator()(std::pair<double,double>theta1, std::pair<double,double> theta2) const
 934     {
 935       double norm1 = sqrt(theta1.first*theta1.first +theta1.second*theta1.second);
 936       double norm2 = sqrt(theta2.first*theta2.first +theta2.second*theta2.second);
 937
 938       double epsilon = 1.e-12;
 939
 940       if( norm1 < epsilon || norm2 < epsilon  )
 941         std::cout << "Warning InterpolationUtils.hxx: AngleLess : Vector with zero norm, cannot define the angle !!!! " << std::endl;
 942
 943       return theta1.second*(norm2 + theta2.first) < theta2.second*(norm1 + theta1.first);
 944
 945     }
 946   };
 947
 948
 949   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
 950   /* fonction pour reconstituer un polygone convexe a partir  */
 951   /*              d'un nuage de point.                        */
 952   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
 953
 954   inline std::vector<double> reconstruct_polygon(const std::vector<double>& V)
 955   {
 956
 957     int taille((int)V.size());
 958
 959     //VB : why 6 ?
 960
 961     if(taille<=6)
 962       {return V;}
 963     else
 964       {
 965         double *COS=new double[taille/2];
 966         double *SIN=new double[taille/2];
 967         //double *angle=new double[taille/2];
 968         std::vector<double> Bary=bary_poly(V);
 969         COS[0]=1.0;
 970         SIN[0]=0.0;
 971         //angle[0]=0.0;
 972         for(int i=0; i<taille/2-1;i++)
 973           {
 974             std::vector<double> Trigo=calcul_cos_et_sin(&Bary[0],&V[0],&V[2*(i+1)]);
 975             COS[i+1]=Trigo[0];
 976             SIN[i+1]=Trigo[1];
 977             //if(SIN[i+1]>=0)
 978             //    {angle[i+1]=atan2(SIN[i+1],COS[i+1]);}
 979             //             else
 980             //               {angle[i+1]=-atan2(SIN[i+1],COS[i+1]);}
 981           }
 982
 983         //ensuite on ordonne les angles.
 984         std::vector<double> Pt_ordonne;
 985         Pt_ordonne.reserve(taille);
 986         //        std::multimap<double,int> Ordre;
 987         std::multimap<std::pair<double,double>,int, AngleLess> CosSin;
 988         for(int i=0;i<taille/2;i++)
 989           {
 990             //  Ordre.insert(std::make_pair(angle[i],i));
 991             CosSin.insert(std::make_pair(std::make_pair(SIN[i],COS[i]),i));
 992           }
 993         //        std::multimap <double,int>::iterator mi;
 994         std::multimap<std::pair<double,double>,int, AngleLess>::iterator   micossin;
 995         //         for(mi=Ordre.begin();mi!=Ordre.end();mi++)
 996         //           {
 997         //             int j=(*mi).second;
 998         //             Pt_ordonne.push_back(V[2*j]);
 999         //             Pt_ordonne.push_back(V[2*j+1]);
1000         //           }
1001         for(micossin=CosSin.begin();micossin!=CosSin.end();micossin++)
1002           {
1003             int j=(*micossin).second;
1004             Pt_ordonne.push_back(V[2*j]);
1005             Pt_ordonne.push_back(V[2*j+1]);
1006           }
1007         delete [] COS;
1008         delete [] SIN;
1009         //        delete [] angle;
1010         return Pt_ordonne;
1011       }
1012   }
1013
1014   template<int DIM, NumberingPolicy numPol, class MyMeshType>
1015   inline void getElemBB(double* bb, const double *coordsOfMesh, mcIdType iP, int nb_nodes)
1016   {
1017     bb[0]=std::numeric_limits<double>::max();
1018     bb[1]=-std::numeric_limits<double>::max();
1019     bb[2]=std::numeric_limits<double>::max();
1020     bb[3]=-std::numeric_limits<double>::max();
1021     bb[4]=std::numeric_limits<double>::max();
1022     bb[5]=-std::numeric_limits<double>::max();
1023
1024     for (int i=0; i<nb_nodes; i++)
1025       {
1026         double x = coordsOfMesh[3*(iP+i)];
1027         double y = coordsOfMesh[3*(iP+i)+1];
1028         double z = coordsOfMesh[3*(iP+i)+2];
1029         bb[0]=(x<bb[0])?x:bb[0];
1030         bb[1]=(x>bb[1])?x:bb[1];
1031         bb[2]=(y<bb[2])?y:bb[2];
1032         bb[3]=(y>bb[3])?y:bb[3];
1033         bb[4]=(z<bb[4])?z:bb[4];
1034         bb[5]=(z>bb[5])?z:bb[5];
1035       }
1036   }
1037
1038   /*!
1039    * Find a vector orthogonal to the input vector
1040    */
1041   inline void orthogonalVect3(const double inpVect[3], double outVect[3])
1042   {
1043     std::vector<bool> sw(3,false);
1044     double inpVect2[3];
1045     std::transform(inpVect,inpVect+3,inpVect2,std::ptr_fun<double,double>(fabs));
1046     std::size_t posMin(std::distance(inpVect2,std::min_element(inpVect2,inpVect2+3)));
1047     sw[posMin]=true;
1048     std::size_t posMax(std::distance(inpVect2,std::max_element(inpVect2,inpVect2+3)));
1049     if(posMax==posMin)
1050       { posMax=(posMin+1)%3; }
1051     sw[posMax]=true;
1052     std::size_t posMid(std::distance(sw.begin(),std::find(sw.begin(),sw.end(),false)));
1053     outVect[posMin]=0.; outVect[posMid]=1.; outVect[posMax]=-inpVect[posMid]/inpVect[posMax];
1054   }
1055
1056   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
1057   /* Computes the dot product of a and b */
1058   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
1059   template<int dim>
1060   inline double dotprod( const double * a, const double * b)
1061   {
1062     double result=0;
1063     for(int idim = 0; idim < dim ; idim++) result += a[idim]*b[idim];
1064     return result;
1065   }
1066   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
1067   /* Computes the norm of vector v */
1068   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
1069   template<int dim>
1070   inline double norm(const double * v)
1071   {
1072     double result =0;
1073     for(int idim =0; idim<dim; idim++) result+=v[idim]*v[idim];
1074     return sqrt(result);
1075   }
1076   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
1077   /* Computes the square norm of vector a-b */
1078   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
1079   template<int dim>
1080   inline double distance2( const double * a, const double * b)
1081   {
1082     double result =0;
1083     for(int idim =0; idim<dim; idim++) result+=(a[idim]-b[idim])*(a[idim]-b[idim]);
1084     return result;
1085   }
1086   template<class T, int dim>
1087   inline double distance2(  T * a, int inda, T * b, int indb)
1088   {
1089     double result =0;
1090     for(int idim =0; idim<dim; idim++) result += ((*a)[inda+idim] - (*b)[indb+idim])* ((*a)[inda+idim] - (*b)[indb+idim]);
1091     return result;
1092   }
1093   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
1094   /* Computes the determinant of a and b */
1095   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
1096   inline double determinant ( double *  a, double * b)
1097   {
1098     return a[0]*b[1]-a[1]*b[0];
1099   }
1100   inline double determinant ( double *  a, double * b, double * c)
1101   {
1102     return a[0]*determinant(b+1,c+1)-b[0]*determinant(a+1,c+1)+c[0]*determinant(a+1,b+1);
1103   }
1104
1105   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
1106   /* Computes the cross product of AB and AC */
1107   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
1108
1109   template<int dim> inline void crossprod( const double * A, const double * B, const double * C, double * V);
1110
1111   template<> inline
1112   void crossprod<2>( const double * A, const double * B, const double * C, double * V)
1113   {
1114     double AB[2];
1115     double AC[2];
1116     for(int idim =0; idim<2; idim++) AB[idim] = B[idim]-A[idim];//B-A
1117     for(int idim =0; idim<2; idim++) AC[idim] = C[idim]-A[idim];//C-A;
1118
1119     V[0]=determinant(AB,AC);
1120     V[1]=0;
1121   }
1122   template<> inline
1123   void crossprod<3>( const double * A, const double * B, const double * C, double * V)
1124   {
1125     double AB[3];
1126     double AC[3];
1127     for(int idim =0; idim<3; idim++) AB[idim] = B[idim]-A[idim];//B-A
1128     for(int idim =0; idim<3; idim++) AC[idim] = C[idim]-A[idim];//C-A;
1129
1130     V[0]=AB[1]*AC[2]-AB[2]*AC[1];
1131     V[1]=-AB[0]*AC[2]+AB[2]*AC[0];
1132     V[2]=AB[0]*AC[1]-AB[1]*AC[0];
1133   }
1134   template<> inline
1135   void crossprod<1>( const double * /*A*/, const double * /*B*/, const double * /*C*/, double * /*V*/)
1136   {
1137     // just to be able to compile
1138   }
1139
1140   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
1141   /* Checks whether point A is inside the quadrangle BCDE */
1142   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
1143
1144   template<int dim> inline double check_inside(const double* A,const double* B,const double* C,const double* D,
1145                                                const double* E,double* ABC, double* ADE)
1146   {
1147     crossprod<dim>(A,B,C,ABC);
1148     crossprod<dim>(A,D,E,ADE);
1149     return dotprod<dim>(ABC,ADE);
1150   }
1151
1152
1153   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
1154   /* Computes the geometric angle (in [0,Pi]) between two non zero vectors AB and AC */
1155   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
1156   template<int dim> inline double angle(const double * A, const double * B, const double * C, double * n)
1157   {
1158     double AB[dim];
1159     double AC[dim];
1160     double orthAB[dim];
1161
1162     for(int idim =0; idim<dim; idim++) AB[idim] = B[idim]-A[idim];//B-A;
1163     for(int idim =0; idim<dim; idim++) AC[idim] = C[idim]-A[idim];//C-A;
1164
1165     double normAB= norm<dim>(AB);
1166     for(int idim =0; idim<dim; idim++) AB[idim]/=normAB;
1167
1168     double normAC= norm<dim>(AC);
1169     double AB_dot_AC=dotprod<dim>(AB,AC);
1170     for(int idim =0; idim<dim; idim++) orthAB[idim] = AC[idim]-AB_dot_AC*AB[idim];
1171
1172     double denom= normAC+AB_dot_AC;
1173     double numer=norm<dim>(orthAB);
1174
1175     return 2*atan2(numer,denom);
1176   }
1177
1178   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
1179   /* Tells whether the frame constituted of vectors AB, AC and n is direct */
1180   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
1181   template<int dim> inline double direct_frame(const double * A, const double * B, const double * C, double * n);
1182   template<> inline
1183   double direct_frame<2>(const double * A, const double * B, const double * C, double * n)
1184   {
1185     double AB[2];
1186     double AC[2];
1187     for(int idim =0; idim<2; idim++) AB[idim] = B[idim]-A[idim];//B-A;
1188     for(int idim =0; idim<2; idim++) AC[idim] = C[idim]-A[idim];//C-A;
1189
1190     return  determinant(AB,AC)*n[0];
1191   }
1192   template<> inline
1193   double direct_frame<3>(const double * A, const double * B, const double * C, double * n)
1194   {
1195     double AB[3];
1196     double AC[3];
1197     for(int idim =0; idim<3; idim++) AB[idim] = B[idim]-A[idim];//B-A;
1198     for(int idim =0; idim<3; idim++) AC[idim] = C[idim]-A[idim];//C-A;
1199
1200     return determinant(AB,AC,n)>0;
1201   }
1202
1203   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
1204   /*      calcul l'intersection de deux polygones COPLANAIRES */
1205   /* en dimension DIM (2 ou 3). Si DIM=3 l'algorithme ne considere*/
1206   /* que les deux premieres coordonnees de chaque point */
1207   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
1208   template<int DIM> inline void intersec_de_polygone(const double * Coords_A, const double * Coords_B,
1209                                                      int nb_NodesA, int nb_NodesB,
1210                                                      std::vector<double>& inter,
1211                                                      double dim_caracteristic, double precision)
1212   {
1213     for(int i_A = 1; i_A<nb_NodesA-1; i_A++)
1214       {
1215         for(int i_B = 1; i_B<nb_NodesB-1; i_B++)
1216           {
1217             INTERP_KERNEL::intersec_de_triangle(&Coords_A[0],&Coords_A[DIM*i_A],&Coords_A[DIM*(i_A+1)],
1218                                                 &Coords_B[0],&Coords_B[DIM*i_B],&Coords_B[DIM*(i_B+1)],
1219                                                 inter, dim_caracteristic, precision);
1220           }
1221       }
1222     int nb_inter=((int)inter.size())/DIM;
1223     if(nb_inter >3) inter=INTERP_KERNEL::reconstruct_polygon(inter);
1224   }
1225
1226   /*_ _ _ _ _ _ _ _ _
1227    *_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _
1228    *  fonctions qui calcule l'aire d'un polygone en dimension 2 ou 3
1229    *_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
1230   template<int DIM> inline double polygon_area(std::vector<double>& inter)
1231   {
1232     double result=0.;
1233     double area[DIM];
1234
1235     for(int i = 1; i<(int)inter.size()/DIM-1; i++)
1236       {
1237         INTERP_KERNEL::crossprod<DIM>(&inter[0],&inter[DIM*i],&inter[DIM*(i+1)],area);
1238         result +=0.5*norm<DIM>(area);
1239       }
1240     return result;
1241   }
1242
1243   template<int DIM> inline double polygon_area(std::deque<double>& inter)
1244   {
1245     double result=0.;
1246     double area[DIM];
1247
1248     for(int i = 1; i<(int)inter.size()/DIM-1; i++)
1249       {
1250         INTERP_KERNEL::crossprod<DIM>(&inter[0],&inter[DIM*i],&inter[DIM*(i+1)],area);
1251         result +=0.5*norm<DIM>(area);
1252       }
1253     return result;
1254   }
1255
1256   /*! Computes the triple product (XA^XB).XC (in 3D)*/
1257   inline double triple_product(const double* A, const double*B, const double*C, const double*X)
1258   {
1259     double XA[3];
1260     XA[0]=A[0]-X[0];
1261     XA[1]=A[1]-X[1];
1262     XA[2]=A[2]-X[2];
1263     double XB[3];
1264     XB[0]=B[0]-X[0];
1265     XB[1]=B[1]-X[1];
1266     XB[2]=B[2]-X[2];
1267     double XC[3];
1268     XC[0]=C[0]-X[0];
1269     XC[1]=C[1]-X[1];
1270     XC[2]=C[2]-X[2];
1271
1272     return
1273       (XA[1]*XB[2]-XA[2]*XB[1])*XC[0]+
1274       (XA[2]*XB[0]-XA[0]*XB[2])*XC[1]+
1275       (XA[0]*XB[1]-XA[1]*XB[0])*XC[2];
1276   }
1277
1278   /*! Subroutine of checkEqualPolygins that tests if two list of nodes (not necessarily distincts) describe the same polygon, assuming they share a common point.*/
1279   /*! Indexes istart1 and istart2 designate two points P1 in L1 and P2 in L2 that have identical coordinates. Generally called with istart1=0.*/
1280   /*! Integer sign ( 1 or -1) indicate the direction used in going all over L2. */
1281   template<class T, int dim>
1282   bool checkEqualPolygonsOneDirection(T * L1, T * L2, int size1, int size2, int istart1, int istart2, double epsilon, int sign)
1283   {
1284     int i1 = istart1;
1285     int i2 = istart2;
1286     int i1next = ( i1 + 1 ) % size1;
1287     int i2next = ( i2 + sign +size2) % size2;
1288
1289     while(true)
1290       {
1291         while( i1next!=istart1 && distance2<T,dim>(L1,i1*dim, L1,i1next*dim) < epsilon ) i1next = (  i1next + 1 ) % size1;
1292         while( i2next!=istart2 && distance2<T,dim>(L2,i2*dim, L2,i2next*dim) < epsilon ) i2next = (  i2next + sign +size2 ) % size2;
1293
1294         if(i1next == istart1)
1295           {
1296             if(i2next == istart2)
1297               return true;
1298             else return false;
1299           }
1300         else
1301           if(i2next == istart2)
1302             return false;
1303           else
1304             {
1305               if(distance2<T,dim>(L1,i1next*dim, L2,i2next*dim) > epsilon )
1306                 return false;
1307               else
1308                 {
1309                   i1 = i1next;
1310                   i2 = i2next;
1311                   i1next = ( i1 + 1 ) % size1;
1312                   i2next = ( i2 + sign + size2 ) % size2;
1313                 }
1314             }
1315       }
1316   }
1317
1318   /*! Tests if two list of nodes (not necessarily distincts) describe the same polygon.*/
1319   /*! Existence of multiple points in the list is considered.*/
1320   template<class T, int dim>
1321   bool checkEqualPolygons(T * L1, T * L2, double epsilon)
1322   {
1323     if(L1==NULL || L2==NULL)
1324       {
1325         std::cout << "Warning InterpolationUtils.hxx:checkEqualPolygonsPointer: Null pointer " << std::endl;
1326         throw(Exception("big error: not closed polygon..."));
1327       }
1328
1329     int size1 = (*L1).size()/dim;
1330     int size2 = (*L2).size()/dim;
1331     int istart1 = 0;
1332     int istart2 = 0;
1333
1334     while( istart2 < size2  && distance2<T,dim>(L1,istart1*dim, L2,istart2*dim) > epsilon ) istart2++;
1335
1336     if(istart2 == size2)
1337       {
1338         return (size1 == 0) && (size2 == 0);
1339       }
1340     else
1341       return   checkEqualPolygonsOneDirection<T,dim>( L1, L2, size1, size2, istart1, istart2, epsilon,  1)
1342         || checkEqualPolygonsOneDirection<T,dim>( L1, L2, size1, size2, istart1, istart2, epsilon, -1);
1343
1344   }
1345 }
1346
1347
1348 #endif