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