src/INTERP_KERNEL/InterpolationUtils.hxx

   1 //  Copyright (C) 2007-2008  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.
   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 #ifndef __INTERPOLATIONUTILS_HXX__
  20 #define __INTERPOLATIONUTILS_HXX__
  21
  22 #include "INTERPKERNELDefines.hxx"
  23 #include "InterpKernelException.hxx"
  24
  25 #include "NormalizedUnstructuredMesh.hxx"
  26
  27 #include <deque>
  28 #include <map>
  29 #include <cmath>
  30 #include <string>
  31 #include <vector>
  32 #include <algorithm>
  33 #include <iostream>
  34 #include <limits>
  35
  36 namespace INTERP_KERNEL
  37 {
  38   template<class ConnType, NumberingPolicy numPol>
  39   class OTT//OffsetToolTrait
  40   {
  41   };
  42
  43   template<class ConnType>
  44   class OTT<ConnType,ALL_C_MODE>
  45   {
  46   public:
  47     static ConnType indFC(ConnType i) { return i; }
  48     static ConnType ind2C(ConnType i) { return i; }
  49     static ConnType conn2C(ConnType i) { return i; }
  50     static ConnType coo2C(ConnType i) { return i; }
  51   };
  52
  53   template<class ConnType>
  54   class OTT<ConnType,ALL_FORTRAN_MODE>
  55   {
  56   public:
  57     static ConnType indFC(ConnType i) { return i+1; }
  58     static ConnType ind2C(ConnType i) { return i-1; }
  59     static ConnType conn2C(ConnType i) { return i-1; }
  60     static ConnType coo2C(ConnType i) { return i-1; }
  61   };
  62
  63   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
  64   /*   calcul la surface d'un triangle                  */
  65   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
  66
  67   inline double Surf_Tri(const double* P_1,const double* P_2,const double* P_3)
  68   {
  69     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]);
  70     double Surface = 0.5*fabs(A);
  71     return Surface;
  72   }
  73
  74   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
  75   /*     fonction qui calcul le déterminant            */
  76   /*      de deux vecteur(cf doc CGAL).                */
  77   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
  78
  79   //fonction qui calcul le déterminant des vecteurs: P3P1 et P3P2
  80   //(cf doc CGAL).
  81
  82   inline double mon_determinant(const double* P_1,
  83                                 const double*  P_2,
  84                                 const double* P_3)
  85   {
  86     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]);
  87     return mon_det;
  88   }
  89
  90   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
  91   //calcul la norme du vecteur P1P2
  92
  93   inline double norme_vecteur(const double* P_1,const double* P_2)
  94   {
  95     double X=P_1[0]-P_2[0];
  96     double Y=P_1[1]-P_2[1];
  97     double norme=sqrt(X*X+Y*Y);
  98     return norme;
  99   }
 100
 101   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
 102   /*         calcul le cos et le sin de l'angle P1P2,P1P3     */
 103   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
 104
 105   inline std::vector<double> calcul_cos_et_sin(const double* P_1,
 106                                                const double* P_2,
 107                                                const double* P_3)
 108   {
 109
 110     std::vector<double> Vect;
 111     double P1_P2=norme_vecteur(P_1,P_2);
 112     double P2_P3=norme_vecteur(P_2,P_3);
 113     double P3_P1=norme_vecteur(P_3,P_1);
 114
 115     double N=P1_P2*P1_P2+P3_P1*P3_P1-P2_P3*P2_P3;
 116     double D=2.0*P1_P2*P3_P1;
 117     double COS=N/D;
 118     if (COS>1.0) COS=1.0;
 119     if (COS<-1.0) COS=-1.0;
 120     Vect.push_back(COS);
 121     double V=mon_determinant(P_2,P_3,P_1);
 122     double D_1=P1_P2*P3_P1;
 123     double SIN=V/D_1;
 124     if (SIN>1.0) SIN=1.0;
 125     if (SIN<-1.0) SIN=-1.0;
 126     Vect.push_back(SIN);
 127
 128     return Vect;
 129
 130   }
 131
 132   /*!
 133    * This method builds a quadrangle built with the first point of 'triIn' the barycenter of two edges starting or ending with
 134    * the first point of 'triIn' and the barycenter of 'triIn'.
 135    *
 136    * @param triIn is a 6 doubles array in full interlace mode, that represents a triangle.
 137    * @param quadOut is a 8 doubles array filled after the following call.
 138    */
 139   template<int SPACEDIM>
 140   inline void fillDualCellOfTri(const double *triIn, double *quadOut)
 141   {
 142     //1st point
 143     std::copy(triIn,triIn+SPACEDIM,quadOut);
 144     double tmp[SPACEDIM];
 145     std::transform(triIn,triIn+SPACEDIM,triIn+SPACEDIM,tmp,std::plus<double>());
 146     //2nd point
 147     std::transform(tmp,tmp+SPACEDIM,quadOut+SPACEDIM,std::bind2nd(std::multiplies<double>(),0.5));
 148     std::transform(tmp,tmp+SPACEDIM,triIn+2*SPACEDIM,tmp,std::plus<double>());
 149     //3rd point
 150     std::transform(tmp,tmp+SPACEDIM,quadOut+2*SPACEDIM,std::bind2nd(std::multiplies<double>(),1/3.));
 151     //4th point
 152     std::transform(triIn,triIn+SPACEDIM,triIn+2*SPACEDIM,tmp,std::plus<double>());
 153     std::transform(tmp,tmp+SPACEDIM,quadOut+3*SPACEDIM,std::bind2nd(std::multiplies<double>(),0.5));
 154   }
 155
 156   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
 157   /*     calcul les coordonnées du barycentre d'un polygone   */
 158   /*     le vecteur en entrée est constitué des coordonnées   */
 159   /*     des sommets du polygone                              */
 160   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
 161
 162   inline std::vector<double> bary_poly(const std::vector<double>& V)
 163   {
 164     std::vector<double> Bary;
 165     long taille=V.size();
 166     double x=0;
 167     double y=0;
 168
 169     for(long i=0;i<taille/2;i++)
 170       {
 171         x=x+V[2*i];
 172         y=y+V[2*i+1];
 173       }
 174     double A=2*x/taille;
 175     double B=2*y/taille;
 176     Bary.push_back(A);//taille vecteur=2*nb de points.
 177     Bary.push_back(B);
 178
 179
 180     return Bary;
 181   }
 182
 183
 184   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
 185   /*     Calculate barycentric coordinates of a 2D point p */
 186   /*     with respect to the triangle verices.             */
 187   /*     triaCoords are in full interlace                  */
 188   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
 189
 190   inline void barycentric_coords(const double* triaCoords, const double* p, double* bc)
 191   {
 192     // matrix 2x2
 193     double
 194       T11 = triaCoords[0]-triaCoords[4], T12 = triaCoords[2]-triaCoords[4],
 195       T21 = triaCoords[1]-triaCoords[5], T22 = triaCoords[3]-triaCoords[5];
 196     // matrix determinant
 197     double Tdet = T11*T22 - T12*T21;
 198     if ( fabs( Tdet ) < std::numeric_limits<double>::min() ) {
 199       bc[0]=1; bc[1]=0; bc[2]=0;
 200       return;
 201     }
 202     // matrix inverse
 203     double t11 = T22, t12 = -T12, t21 = -T21, t22 = T11;
 204     // vector
 205     double r11 = p[0]-triaCoords[4], r12 = p[1]-triaCoords[5];
 206     // barycentric coordinates: mutiply matrix by vector
 207     bc[0] = (t11 * r11 + t12 * r12)/Tdet;
 208     bc[1] = (t21 * r11 + t22 * r12)/Tdet;
 209     bc[2] = 1. - bc[0] - bc[1];
 210   }
 211
 212   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
 213   /*     Calculate barycentric coordinates of a point p    */
 214   /*     with respect to triangle or tetra verices.        */
 215   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
 216
 217   inline void barycentric_coords(const std::vector<const double*>& n, const double* p, double* bc)
 218   {
 219     enum { _X, _Y, _Z };
 220     if ( n.size() == 3 ) // TRIA3
 221       {
 222         // matrix 2x2
 223         double
 224           T11 = n[0][_X]-n[2][_X], T12 = n[1][_X]-n[2][_X],
 225           T21 = n[0][_Y]-n[2][_Y], T22 = n[1][_Y]-n[2][_Y];
 226         // matrix determinant
 227         double Tdet = T11*T22 - T12*T21;
 228         if ( std::fabs( Tdet ) < std::numeric_limits<double>::min() ) {
 229           bc[0]=1; bc[1]=bc[2]=0; // no solution
 230           return;
 231         }
 232         // matrix inverse
 233         double t11 = T22, t12 = -T12, t21 = -T21, t22 = T11;
 234         // vector
 235         double r11 = p[_X]-n[2][_X], r12 = p[_Y]-n[2][_Y];
 236         // barycentric coordinates: mutiply matrix by vector
 237         bc[0] = (t11 * r11 + t12 * r12)/Tdet;
 238         bc[1] = (t21 * r11 + t22 * r12)/Tdet;
 239         bc[2] = 1. - bc[0] - bc[1];
 240       }
 241     else // TETRA4
 242       {
 243         bc[3]=0; // for no solution
 244
 245         // Find bc by solving system of 3 equations using Gaussian elimination algorithm
 246         // bc1*( x1 - x4 ) + bc2*( x2 - x4 ) + bc3*( x3 - x4 ) = px - x4
 247         // bc1*( y1 - y4 ) + bc2*( y2 - y4 ) + bc3*( y3 - y4 ) = px - y4
 248         // bc1*( z1 - z4 ) + bc2*( z2 - z4 ) + bc3*( z3 - z4 ) = px - z4
 249         const int nbCol=4, nbRow=3;
 250
 251         double T[nbRow][nbCol]=
 252           {{ n[0][_X]-n[3][_X], n[1][_X]-n[3][_X], n[2][_X]-n[3][_X], p[_X]-n[3][_X] },
 253            { n[0][_Y]-n[3][_Y], n[1][_Y]-n[3][_Y], n[2][_Y]-n[3][_Y], p[_Y]-n[3][_Y] },
 254            { n[0][_Z]-n[3][_Z], n[1][_Z]-n[3][_Z], n[2][_Z]-n[3][_Z], p[_Z]-n[3][_Z] }};
 255
 256         // make upper triangular matrix (forward elimination)
 257
 258         int iR[nbRow] = { 0, 1, 2 };
 259
 260         for ( int i = 0; i < 2; ++i ) // nullify 2 rows
 261           {
 262             // swap rows to have max value of i-th column in i-th row
 263             double max = std::fabs( T[ iR[i] ][i] );
 264             for ( int r = i+1; r < nbRow; ++r ) {
 265               double t = std::fabs( T[ iR[r] ][i] );
 266               if ( t > max ) {
 267                 max = t;
 268                 std::swap( iR[r], iR[i] );
 269               }
 270             }
 271             if ( max < std::numeric_limits<double>::min() ) {
 272               bc[0]=1; bc[1]=bc[2]=bc[3]=0;
 273               return; // no solution
 274             }
 275             // make 0 below T[i][i] (actually we do not modify i-th column)
 276             double* tUpRow = T[ iR[i] ];
 277             for ( int r = i+1; r < nbRow; ++r ) {
 278               double* tRow = T[ iR[r] ];
 279               double coef = tRow[ i ] / tUpRow[ i ];
 280               for ( int c = i+1; c < nbCol; ++c )
 281                 tRow[ c ] -= tUpRow[ c ] * coef;
 282             }
 283           }
 284         double* tRow = T[ iR[2] ];
 285         if ( std::fabs( tRow[ 2 ] ) < std::numeric_limits<double>::min() ) {
 286           bc[0]=1; bc[1]=bc[2]=bc[3]=0;
 287           return; // no solution
 288         }
 289         tRow[ 3 ] /= tRow[ 2 ];
 290
 291         // calculate solution (back substitution)
 292
 293         bc[ 2 ] = tRow[ 3 ];
 294
 295         tRow = T[ iR[1] ];
 296         bc[ 1 ] = (tRow[ 3 ] - bc[2]*tRow[ 2 ]) / tRow[ 1 ];
 297
 298         tRow = T[ iR[0] ];
 299         bc[ 0 ] = (tRow[ 3 ] - bc[2]*tRow[ 2 ] - bc[1]*tRow[ 1 ]) / tRow[ 0 ];
 300
 301         bc[ 3 ] = 1. - bc[0] - bc[1] - bc[2];
 302       }
 303   }
 304
 305   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
 306   /*         calcul la surface d'un polygone.                 */
 307   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
 308
 309   inline double  Surf_Poly(const std::vector<double>& Poly)
 310   {
 311
 312     double Surface=0;
 313     for(unsigned long i=0; i<(Poly.size())/2-2; i++)
 314       {
 315         double Surf=Surf_Tri( &Poly[0],&Poly[2*(i+1)],&Poly[2*(i+2)] );
 316         Surface=Surface + Surf ;
 317       }
 318     return Surface ;
 319   }
 320
 321   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
 322   /*   fonction qui teste si un point est dans une maille     */
 323   /*   point: P_0                                             */
 324   /*   P_1, P_2, P_3 sommet des mailles                       */
 325   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
 326
 327   inline bool point_dans_triangle(const double* P_0,const double* P_1,
 328                                   const double* P_2,const double* P_3,
 329                                   double eps)
 330   {
 331
 332     bool A=false;
 333     double det_1=mon_determinant(P_1,P_3,P_0);
 334     double det_2=mon_determinant(P_3,P_2,P_0);
 335     double det_3=mon_determinant(P_2,P_1,P_0);
 336     if( (det_1>=-eps && det_2>=-eps && det_3>=-eps) || (det_1<=eps && det_2<=eps && det_3<=eps) )
 337       {
 338         A=true;
 339       }
 340
 341     return A;
 342   }
 343
 344   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
 345   /*fonction pour vérifier qu'un point n'a pas déja été considérer dans   */
 346   /*      le vecteur et le rajouter au vecteur sinon.                     */
 347   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
 348
 349   inline void verif_point_dans_vect(const double* P, std::vector<double>& V, double absolute_precision )
 350   {
 351     long taille=V.size();
 352     bool isPresent=false;
 353     for(long i=0;i<taille/2;i++)
 354       {
 355         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)
 356           isPresent=true;
 357
 358       }
 359     if(!isPresent)
 360       {
 361
 362         V.push_back(P[0]);
 363         V.push_back(P[1]);
 364       }
 365   }
 366
 367   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
 368   /* fonction qui rajoute les sommet du triangle P dans le vecteur V        */
 369   /* si ceux-ci sont compris dans le triangle S et ne sont pas déjà dans    */
 370   /* V.                                                                     */
 371   /*sommets de P: P_1, P_2, P_3                                             */
 372   /*sommets de S: P_4, P_5, P_6                                             */
 373   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
 374
 375   inline void rajou_sommet_triangl(const double* P_1,const double* P_2,const double* P_3,
 376                                    const double* P_4,const double* P_5,const double* P_6,
 377                                    std::vector<double>& V, double dim_caracteristic, double precision)
 378   {
 379
 380     double absolute_precision = precision*dim_caracteristic;
 381     bool A_1=INTERP_KERNEL::point_dans_triangle(P_1,P_4,P_5,P_6,absolute_precision);
 382     if(A_1)
 383       verif_point_dans_vect(P_1,V,absolute_precision);
 384     bool A_2=INTERP_KERNEL::point_dans_triangle(P_2,P_4,P_5,P_6,absolute_precision);
 385     if(A_2)
 386       verif_point_dans_vect(P_2,V,absolute_precision);
 387     bool A_3=INTERP_KERNEL::point_dans_triangle(P_3,P_4,P_5,P_6,absolute_precision);
 388     if(A_3)
 389       verif_point_dans_vect(P_3,V,absolute_precision);
 390   }
 391
 392
 393   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _  _ _ _ _ _ _ _ _*/
 394   /*  calcul de l'intersection de deux segments: segments P1P2 avec P3P4      */
 395   /*  . Si l'intersection est non nulle et si celle-ci n'est                  */
 396   /*  n'est pas déjà contenue dans Vect on la rajoute à Vect                  */
 397   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _  _ _ _ _ _ _ _ _*/
 398
 399   inline void inters_de_segment(const double * P_1,const double * P_2,
 400                                 const double * P_3,const double * P_4,
 401                                 std::vector<double>& Vect,
 402                                 double dim_caracteristic, double precision)
 403   {
 404     // calcul du déterminant de P_1P_2 et P_3P_4.
 405     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]);
 406
 407     double absolute_precision = dim_caracteristic*precision;
 408     if(fabs(det)>absolute_precision)
 409       {
 410         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;
 411
 412         if (k_1 >= -absolute_precision && k_1 <= 1+absolute_precision)
 413           //if( k_1 >= -precision && k_1 <= 1+precision)
 414           {
 415             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;
 416
 417             if (k_2 >= -absolute_precision && k_2 <= 1+absolute_precision)
 418               //if( k_2 >= -precision && k_2 <= 1+precision)
 419               {
 420                 double P_0[2];
 421                 P_0[0]=P_1[0]+k_1*(P_2[0]-P_1[0]);
 422                 P_0[1]=P_1[1]+k_1*(P_2[1]-P_1[1]);
 423                 verif_point_dans_vect(P_0,Vect,absolute_precision);
 424               }
 425           }
 426       }
 427   }
 428
 429
 430
 431   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
 432   /*      calcul l'intersection de deux triangles            */
 433   /* P_1, P_2, P_3: sommets du premier triangle              */
 434   /* P_4, P_5, P_6: sommets du deuxième triangle             */
 435   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
 436
 437   inline void intersec_de_triangle(const double* P_1,const double* P_2, const double* P_3,
 438                                    const double* P_4,const double* P_5,const double* P_6,
 439                                    std::vector<double>& Vect, double dim_caracteristic, double precision)
 440   {
 441     inters_de_segment(P_1,P_2,P_4,P_5,Vect, dim_caracteristic, precision);
 442     inters_de_segment(P_1,P_2,P_5,P_6,Vect, dim_caracteristic, precision);
 443     inters_de_segment(P_1,P_2,P_6,P_4,Vect, dim_caracteristic, precision);
 444     inters_de_segment(P_2,P_3,P_4,P_5,Vect, dim_caracteristic, precision);
 445     inters_de_segment(P_2,P_3,P_5,P_6,Vect, dim_caracteristic, precision);
 446     inters_de_segment(P_2,P_3,P_6,P_4,Vect, dim_caracteristic, precision);
 447     inters_de_segment(P_3,P_1,P_4,P_5,Vect, dim_caracteristic, precision);
 448     inters_de_segment(P_3,P_1,P_5,P_6,Vect, dim_caracteristic, precision);
 449     inters_de_segment(P_3,P_1,P_6,P_4,Vect, dim_caracteristic, precision);
 450     rajou_sommet_triangl(P_1,P_2,P_3,P_4,P_5,P_6,Vect, dim_caracteristic, precision);
 451     rajou_sommet_triangl(P_4,P_5,P_6,P_1,P_2,P_3,Vect, dim_caracteristic, precision);
 452   }
 453
 454   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
 455   /* fonction pour vérifier qu'un n°de maille n'a pas déja été considérer  */
 456   /*  dans le vecteur et le rajouter au vecteur sinon.                     */
 457   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
 458
 459   inline void verif_maill_dans_vect(int Num, std::vector<int>& V)
 460   {
 461     long taille=V.size();
 462     int A=0;
 463     for(long i=0;i<taille;i++)
 464       {
 465         if(Num==V[i])
 466           {
 467             A=1;
 468             break;
 469           }
 470       }
 471     if(A==0)
 472       {V.push_back(Num); }
 473   }
 474
 475   /*! Function that compares two angles from the values of the pairs (sin,cos)*/
 476   /*! Angles are considered in [0, 2Pi] bt are not computed explicitely */
 477   class AngleLess
 478   {
 479   public:
 480     bool operator()(std::pair<double,double>theta1, std::pair<double,double> theta2)
 481     {
 482       double norm1 = sqrt(theta1.first*theta1.first +theta1.second*theta1.second);
 483       double norm2 = sqrt(theta2.first*theta2.first +theta2.second*theta2.second);
 484
 485       double epsilon = 1.e-12;
 486
 487       if( norm1 < epsilon || norm2 < epsilon  )
 488         std::cout << "Warning InterpolationUtils.hxx: AngleLess : Vector with zero norm, cannot define the angle !!!! " << std::endl;
 489
 490       return theta1.second*(norm2 + theta2.first) < theta2.second*(norm1 + theta1.first);
 491
 492     }
 493   };
 494
 495
 496   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
 497   /* fonction pour reconstituer un polygone convexe à partir  */
 498   /*              d'un nuage de point.                        */
 499   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
 500
 501   inline std::vector<double> reconstruct_polygon(const std::vector<double>& V)
 502   {
 503
 504     int taille=V.size();
 505
 506     //VB : why 6 ?
 507
 508     if(taille<=6)
 509       {return V;}
 510     else
 511       {
 512         double *COS=new double[taille/2];
 513         double *SIN=new double[taille/2];
 514         //double *angle=new double[taille/2];
 515         std::vector<double> Bary=bary_poly(V);
 516         COS[0]=1.0;
 517         SIN[0]=0.0;
 518         //angle[0]=0.0;
 519         for(int i=0; i<taille/2-1;i++)
 520           {
 521             std::vector<double> Trigo=calcul_cos_et_sin(&Bary[0],&V[0],&V[2*(i+1)]);
 522             COS[i+1]=Trigo[0];
 523             SIN[i+1]=Trigo[1];
 524             //if(SIN[i+1]>=0)
 525             //    {angle[i+1]=atan2(SIN[i+1],COS[i+1]);}
 526             //             else
 527             //               {angle[i+1]=-atan2(SIN[i+1],COS[i+1]);}
 528           }
 529
 530         //ensuite on ordonne les angles.
 531         std::vector<double> Pt_ordonne;
 532         Pt_ordonne.reserve(taille);
 533         //        std::multimap<double,int> Ordre;
 534         std::multimap<std::pair<double,double>,int, AngleLess> CosSin;
 535         for(int i=0;i<taille/2;i++)
 536           {
 537             //  Ordre.insert(std::make_pair(angle[i],i));
 538             CosSin.insert(std::make_pair(std::make_pair(SIN[i],COS[i]),i));
 539           }
 540         //        std::multimap <double,int>::iterator mi;
 541         std::multimap<std::pair<double,double>,int, AngleLess>::iterator   micossin;
 542         //         for(mi=Ordre.begin();mi!=Ordre.end();mi++)
 543         //           {
 544         //             int j=(*mi).second;
 545         //             Pt_ordonne.push_back(V[2*j]);
 546         //             Pt_ordonne.push_back(V[2*j+1]);
 547         //           }
 548         for(micossin=CosSin.begin();micossin!=CosSin.end();micossin++)
 549           {
 550             int j=(*micossin).second;
 551             Pt_ordonne.push_back(V[2*j]);
 552             Pt_ordonne.push_back(V[2*j+1]);
 553           }
 554         delete [] COS;
 555         delete [] SIN;
 556         //        delete [] angle;
 557         return Pt_ordonne;
 558       }
 559   }
 560
 561   template<int DIM, NumberingPolicy numPol, class MyMeshType>
 562   inline void getElemBB(double* bb, const double *coordsOfMesh, int iP, int nb_nodes)
 563   {
 564     bb[0]=std::numeric_limits<double>::max();
 565     bb[1]=-std::numeric_limits<double>::max();
 566     bb[2]=std::numeric_limits<double>::max();
 567     bb[3]=-std::numeric_limits<double>::max();
 568     bb[4]=std::numeric_limits<double>::max();
 569     bb[5]=-std::numeric_limits<double>::max();
 570
 571     for (int i=0; i<nb_nodes; i++)
 572       {
 573         double x = coordsOfMesh[3*(iP+i)];
 574         double y = coordsOfMesh[3*(iP+i)+1];
 575         double z = coordsOfMesh[3*(iP+i)+2];
 576         bb[0]=(x<bb[0])?x:bb[0];
 577         bb[1]=(x>bb[1])?x:bb[1];
 578         bb[2]=(y<bb[2])?y:bb[2];
 579         bb[3]=(y>bb[3])?y:bb[3];
 580         bb[4]=(z<bb[4])?z:bb[4];
 581         bb[5]=(z>bb[5])?z:bb[5];
 582       }
 583   }
 584
 585   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
 586   /* Computes the dot product of a and b */
 587   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
 588   template<int dim>
 589   inline double dotprod( double * a, double * b)
 590   {
 591     double result=0;
 592     for(int idim = 0; idim < dim ; idim++) result += a[idim]*b[idim];
 593     return result;
 594   }
 595   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
 596   /* Computes the norm of vector v */
 597   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
 598   template<int dim>
 599   inline double norm( double * v)
 600   {
 601     double result =0;
 602     for(int idim =0; idim<dim; idim++) result+=v[idim]*v[idim];
 603     return sqrt(result);
 604   }
 605   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
 606   /* Computes the square norm of vector a-b */
 607   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
 608   template<int dim>
 609   inline double distance2( const double * a, const double * b)
 610   {
 611     double result =0;
 612     for(int idim =0; idim<dim; idim++) result+=(a[idim]-b[idim])*(a[idim]-b[idim]);
 613     return result;
 614   }
 615   template<class T, int dim>
 616   inline double distance2(  T * a, int inda, T * b, int indb)
 617   {
 618     double result =0;
 619     for(int idim =0; idim<dim; idim++) result += ((*a)[inda+idim] - (*b)[indb+idim])* ((*a)[inda+idim] - (*b)[indb+idim]);
 620     return result;
 621   }
 622   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
 623   /* Computes the determinant of a and b */
 624   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
 625   inline double determinant ( double *  a, double * b)
 626   {
 627     return a[0]*b[1]-a[1]*b[0];
 628   }
 629   inline double determinant ( double *  a, double * b, double * c)
 630   {
 631     return a[0]*determinant(b+1,c+1)-b[0]*determinant(a+1,c+1)+c[0]*determinant(a+1,b+1);
 632   }
 633
 634   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
 635   /* Computes the cross product of AB and AC */
 636   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
 637
 638   template<int dim> inline void crossprod( const double * A, const double * B, const double * C, double * V);
 639
 640   template<> inline
 641   void crossprod<2>( const double * A, const double * B, const double * C, double * V)
 642   {
 643     double AB[2];
 644     double AC[2];
 645     for(int idim =0; idim<2; idim++) AB[idim] = B[idim]-A[idim];//B-A
 646     for(int idim =0; idim<2; idim++) AC[idim] = C[idim]-A[idim];//C-A;
 647
 648     V[0]=determinant(AB,AC);
 649     V[1]=0;
 650   }
 651   template<> inline
 652   void crossprod<3>( const double * A, const double * B, const double * C, double * V)
 653   {
 654     double AB[3];
 655     double AC[3];
 656     for(int idim =0; idim<3; idim++) AB[idim] = B[idim]-A[idim];//B-A
 657     for(int idim =0; idim<3; idim++) AC[idim] = C[idim]-A[idim];//C-A;
 658
 659     V[0]=AB[1]*AC[2]-AB[2]*AC[1];
 660     V[1]=-AB[0]*AC[2]+AB[2]*AC[0];
 661     V[2]=AB[0]*AC[1]-AB[1]*AC[0];
 662   }
 663
 664   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
 665   /* Checks wether point A is inside the quadrangle BCDE */
 666   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
 667
 668   template<int dim> inline double check_inside(const double* A,const double* B,const double* C,const double* D,
 669                                                const double* E,double* ABC, double* ADE)
 670   {
 671     crossprod<dim>(A,B,C,ABC);
 672     crossprod<dim>(A,D,E,ADE);
 673     return dotprod<dim>(ABC,ADE);
 674   }
 675
 676
 677   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
 678   /* Computes the geometric angle (in [0,Pi]) between two non zero vectors AB and AC */
 679   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
 680   template<int dim> inline double angle(const double * A, const double * B, const double * C, double * n)
 681   {
 682     double AB[dim];
 683     double AC[dim];
 684     double orthAB[dim];
 685
 686     for(int idim =0; idim<dim; idim++) AB[idim] = B[idim]-A[idim];//B-A;
 687     for(int idim =0; idim<dim; idim++) AC[idim] = C[idim]-A[idim];//C-A;
 688
 689     double normAB= norm<dim>(AB);
 690     for(int idim =0; idim<dim; idim++) AB[idim]/=normAB;
 691
 692     double normAC= norm<dim>(AC);
 693     double AB_dot_AC=dotprod<dim>(AB,AC);
 694     for(int idim =0; idim<dim; idim++) orthAB[idim] = AC[idim]-AB_dot_AC*AB[idim];
 695
 696     double denom= normAC+AB_dot_AC;
 697     double numer=norm<dim>(orthAB);
 698
 699     return 2*atan2(numer,denom);
 700   }
 701
 702   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
 703   /* Tells whether the frame constituted of vectors AB, AC and n is direct */
 704   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
 705   template<int dim> inline double direct_frame(const double * A, const double * B, const double * C, double * n);
 706   template<> inline
 707   double direct_frame<2>(const double * A, const double * B, const double * C, double * n)
 708   {
 709     double AB[2];
 710     double AC[2];
 711     for(int idim =0; idim<2; idim++) AB[idim] = B[idim]-A[idim];//B-A;
 712     for(int idim =0; idim<2; idim++) AC[idim] = C[idim]-A[idim];//C-A;
 713
 714     return  determinant(AB,AC)*n[0];
 715   }
 716   template<> inline
 717   double direct_frame<3>(const double * A, const double * B, const double * C, double * n)
 718   {
 719     double AB[3];
 720     double AC[3];
 721     for(int idim =0; idim<3; idim++) AB[idim] = B[idim]-A[idim];//B-A;
 722     for(int idim =0; idim<3; idim++) AC[idim] = C[idim]-A[idim];//C-A;
 723
 724     return determinant(AB,AC,n)>0;
 725   }
 726
 727   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
 728   /*      calcul l'intersection de deux polygones COPLANAIRES */
 729   /* en dimension DIM (2 ou 3). Si DIM=3 l'algorithme ne considère*/
 730   /* que les deux premières coordonnées de chaque point */
 731   /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
 732   template<int DIM> inline void intersec_de_polygone(const double * Coords_A, const double * Coords_B,
 733                                                      int nb_NodesA, int nb_NodesB,
 734                                                      std::vector<double>& inter,
 735                                                      double dim_caracteristic, double precision)
 736   {
 737     for(int i_A = 1; i_A<nb_NodesA-1; i_A++)
 738       {
 739         for(int i_B = 1; i_B<nb_NodesB-1; i_B++)
 740           {
 741             INTERP_KERNEL::intersec_de_triangle(&Coords_A[0],&Coords_A[DIM*i_A],&Coords_A[DIM*(i_A+1)],
 742                                                 &Coords_B[0],&Coords_B[DIM*i_B],&Coords_B[DIM*(i_B+1)],
 743                                                 inter, dim_caracteristic, precision);
 744           }
 745       }
 746     int nb_inter=((int)inter.size())/DIM;
 747     if(nb_inter >3) inter=INTERP_KERNEL::reconstruct_polygon(inter);
 748   }
 749
 750   /*_ _ _ _ _ _ _ _ _
 751    *_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _
 752    *  fonctions qui calcule l'aire d'un polygone en dimension 2 ou 3
 753    *_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
 754   template<int DIM> inline double polygon_area(std::vector<double>& inter)
 755   {
 756     double result=0.;
 757     double area[DIM];
 758
 759     for(int i = 1; i<(int)inter.size()/DIM-1; i++)
 760       {
 761         INTERP_KERNEL::crossprod<DIM>(&inter[0],&inter[DIM*i],&inter[DIM*(i+1)],area);
 762         result +=0.5*norm<DIM>(area);
 763       }
 764     return result;
 765   }
 766
 767   template<int DIM> inline double polygon_area(std::deque<double>& inter)
 768   {
 769     double result=0.;
 770     double area[DIM];
 771
 772     for(int i = 1; i<(int)inter.size()/DIM-1; i++)
 773       {
 774         INTERP_KERNEL::crossprod<DIM>(&inter[0],&inter[DIM*i],&inter[DIM*(i+1)],area);
 775         result +=0.5*norm<DIM>(area);
 776       }
 777     return result;
 778   }
 779
 780   /*! Computes the triple product (XA^XB).XC (in 3D)*/
 781   inline double triple_product(const double* A, const double*B, const double*C, const double*X)
 782   {
 783     double XA[3];
 784     XA[0]=A[0]-X[0];
 785     XA[1]=A[1]-X[1];
 786     XA[2]=A[2]-X[2];
 787     double XB[3];
 788     XB[0]=B[0]-X[0];
 789     XB[1]=B[1]-X[1];
 790     XB[2]=B[2]-X[2];
 791     double XC[3];
 792     XC[0]=C[0]-X[0];
 793     XC[1]=C[1]-X[1];
 794     XC[2]=C[2]-X[2];
 795
 796     return
 797       (XA[1]*XB[2]-XA[2]*XB[1])*XC[0]+
 798       (XA[2]*XB[0]-XA[0]*XB[2])*XC[1]+
 799       (XA[0]*XB[1]-XA[1]*XB[0])*XC[2];
 800   }
 801
 802   /*! Subroutine of checkEqualPolygins that tests if two list of nodes (not necessarily distincts) describe the same polygon, assuming they share a comon point.*/
 803   /*! Indexes istart1 and istart2 designate two points P1 in L1 and P2 in L2 that have identical coordinates. Generally called with istart1=0.*/
 804   /*! Integer sign ( 1 or -1) indicate the direction used in going all over L2. */
 805   template<class T, int dim>
 806   bool checkEqualPolygonsOneDirection(T * L1, T * L2, int size1, int size2, int istart1, int istart2, double epsilon, int sign)
 807   {
 808     int i1 = istart1;
 809     int i2 = istart2;
 810     int i1next = ( i1 + 1 ) % size1;
 811     int i2next = ( i2 + sign +size2) % size2;
 812
 813     while(true)
 814       {
 815         while( i1next!=istart1 && distance2<T,dim>(L1,i1*dim, L1,i1next*dim) < epsilon ) i1next = (  i1next + 1 ) % size1;
 816         while( i2next!=istart2 && distance2<T,dim>(L2,i2*dim, L2,i2next*dim) < epsilon ) i2next = (  i2next + sign +size2 ) % size2;
 817
 818         if(i1next == istart1)
 819           {
 820             if(i2next == istart2)
 821               return true;
 822             else return false;
 823           }
 824         else
 825           if(i2next == istart2)
 826             return false;
 827           else
 828             {
 829               if(distance2<T,dim>(L1,i1next*dim, L2,i2next*dim) > epsilon )
 830                 return false;
 831               else
 832                 {
 833                   i1 = i1next;
 834                   i2 = i2next;
 835                   i1next = ( i1 + 1 ) % size1;
 836                   i2next = ( i2 + sign + size2 ) % size2;
 837                 }
 838             }
 839       }
 840   }
 841
 842   /*! Tests if two list of nodes (not necessarily distincts) describe the same polygon.*/
 843   /*! Existence of multiple points in the list is considered.*/
 844   template<class T, int dim>
 845   bool checkEqualPolygons(T * L1, T * L2, double epsilon)
 846   {
 847     if(L1==NULL || L2==NULL)
 848       {
 849         std::cout << "Warning InterpolationUtils.hxx:checkEqualPolygonsPointer: Null pointer " << std::endl;
 850         throw(Exception("big error: not closed polygon..."));
 851       }
 852
 853     int size1 = (*L1).size()/dim;
 854     int size2 = (*L2).size()/dim;
 855     int istart1 = 0;
 856     int istart2 = 0;
 857
 858     while( istart2 < size2  && distance2<T,dim>(L1,istart1*dim, L2,istart2*dim) > epsilon ) istart2++;
 859
 860     if(istart2 == size2)
 861       {
 862         return (size1 == 0) && (size2 == 0);
 863       }
 864     else
 865       return   checkEqualPolygonsOneDirection<T,dim>( L1, L2, size1, size2, istart1, istart2, epsilon,  1)
 866         || checkEqualPolygonsOneDirection<T,dim>( L1, L2, size1, size2, istart1, istart2, epsilon, -1);
 867
 868   }
 869 }
 870
 871
 872 #endif