src/INTERP_KERNEL/InterpolationUtils.hxx

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