1 // Copyright (C) 2007-2014 CEA/DEN, EDF R&D
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.
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.
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
17 // See http://www.salome-platform.org/ or email : webmaster.salome@opencascade.com
19 // Author : Anthony Geay (CEA/DEN)
21 #ifndef __INTERPOLATIONUTILS_HXX__
22 #define __INTERPOLATIONUTILS_HXX__
24 #include "INTERPKERNELDefines.hxx"
25 #include "InterpKernelException.hxx"
27 #include "NormalizedUnstructuredMesh.hxx"
39 namespace INTERP_KERNEL
41 template<class ConnType, NumberingPolicy numPol>
42 class OTT//OffsetToolTrait
46 template<class ConnType>
47 class OTT<ConnType,ALL_C_MODE>
50 static ConnType indFC(ConnType i) { return i; }
51 static ConnType ind2C(ConnType i) { return i; }
52 static ConnType conn2C(ConnType i) { return i; }
53 static ConnType coo2C(ConnType i) { return i; }
56 template<class ConnType>
57 class OTT<ConnType,ALL_FORTRAN_MODE>
60 static ConnType indFC(ConnType i) { return i+1; }
61 static ConnType ind2C(ConnType i) { return i-1; }
62 static ConnType conn2C(ConnType i) { return i-1; }
63 static ConnType coo2C(ConnType i) { return i-1; }
66 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
67 /* calcul la surface d'un triangle */
68 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
70 inline double Surf_Tri(const double* P_1,const double* P_2,const double* P_3)
72 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]);
73 double Surface = 0.5*fabs(A);
77 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
78 /* fonction qui calcul le determinant */
79 /* de deux vecteur(cf doc CGAL). */
80 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
82 //fonction qui calcul le determinant des vecteurs: P3P1 et P3P2
85 inline double mon_determinant(const double* P_1,
89 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]);
93 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
94 //calcul la norme du vecteur P1P2
96 inline double norme_vecteur(const double* P_1,const double* P_2)
98 double X=P_1[0]-P_2[0];
99 double Y=P_1[1]-P_2[1];
100 double norme=sqrt(X*X+Y*Y);
104 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
105 /* calcul le cos et le sin de l'angle P1P2,P1P3 */
106 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
108 inline std::vector<double> calcul_cos_et_sin(const double* P_1,
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);
118 double N=P1_P2*P1_P2+P3_P1*P3_P1-P2_P3*P2_P3;
119 double D=2.0*P1_P2*P3_P1;
121 if (COS>1.0) COS=1.0;
122 if (COS<-1.0) COS=-1.0;
124 double V=mon_determinant(P_2,P_3,P_1);
125 double D_1=P1_P2*P3_P1;
127 if (SIN>1.0) SIN=1.0;
128 if (SIN<-1.0) SIN=-1.0;
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'.
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.
142 template<int SPACEDIM>
143 inline void fillDualCellOfTri(const double *triIn, double *quadOut)
146 std::copy(triIn,triIn+SPACEDIM,quadOut);
147 double tmp[SPACEDIM];
148 std::transform(triIn,triIn+SPACEDIM,triIn+SPACEDIM,tmp,std::plus<double>());
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>());
153 std::transform(tmp,tmp+SPACEDIM,quadOut+2*SPACEDIM,std::bind2nd(std::multiplies<double>(),1/3.));
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));
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'.
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.
166 template<int SPACEDIM>
167 inline void fillDualCellOfPolyg(const double *polygIn, int nPtsPolygonIn, double *polygOut)
170 std::copy(polygIn,polygIn+SPACEDIM,polygOut);
171 std::transform(polygIn,polygIn+SPACEDIM,polygIn+SPACEDIM,polygOut+SPACEDIM,std::plus<double>());
173 std::transform(polygOut+SPACEDIM,polygOut+2*SPACEDIM,polygOut+SPACEDIM,std::bind2nd(std::multiplies<double>(),0.5));
174 double tmp[SPACEDIM];
176 for(int i=0;i<nPtsPolygonIn-2;i++)
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.));
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 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
191 inline std::vector<double> bary_poly(const std::vector<double>& V)
193 std::vector<double> Bary;
194 long taille=V.size();
198 for(long i=0;i<taille/2;i++)
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.
213 * Given 6 coeffs of a Tria6 returns the corresponding value of a given pos
215 inline double computeTria6RefBase(const double *coeffs, const double *pos)
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];
221 * Given xsi,eta in refCoo (length==2) return 6 coeffs in weightedPos.
223 inline void computeWeightedCoeffsInTria6FromRefBase(const double *refCoo, double *weightedPos)
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]);
234 * Given 10 coeffs of a Tetra10 returns the corresponding value of a given pos
236 inline double computeTetra10RefBase(const double *coeffs, const double *pos)
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];
244 * Given xsi,eta,z in refCoo (length==3) return 10 coeffs in weightedPos.
246 inline void computeWeightedCoeffsInTetra10FromRefBase(const double *refCoo, double *weightedPos)
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
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
269 template<unsigned nbRow>
270 bool solveSystemOfEquations(double M[nbRow][nbRow+1], double* sol)
272 const int nbCol=nbRow+1;
274 // make upper triangular matrix (forward elimination)
276 int iR[nbRow];// = { 0, 1, 2 };
277 for ( int i = 0; i < (int) nbRow; ++i )
279 for ( int i = 0; i < (int)(nbRow-1); ++i ) // nullify nbRow-1 rows
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 )
285 double m = std::fabs( M[ iR[r] ][i] );
289 std::swap( iR[r], iR[i] );
292 if ( max < std::numeric_limits<double>::min() )
294 //sol[0]=1; sol[1]=sol[2]=sol[3]=0;
295 return false; // no solution
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 )
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;
307 double* mRow = M[ iR[nbRow-1] ];
308 if ( std::fabs( mRow[ nbRow-1 ] ) < std::numeric_limits<double>::min() )
310 //sol[0]=1; sol[1]=sol[2]=sol[3]=0;
311 return false; // no solution
313 mRow[ nbRow ] /= mRow[ nbRow-1 ];
315 // calculate solution (back substitution)
317 sol[ nbRow-1 ] = mRow[ nbRow ];
319 for ( int i = nbRow-2; i+1; --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 ];
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
338 template<unsigned SZ, unsigned NB_OF_RES>
339 bool solveSystemOfEquations2(const double *matrix, double *solutions, double eps)
346 double B[SZ*(SZ+NB_OF_RES)];
347 std::copy(matrix,matrix+SZ*(SZ+NB_OF_RES),B);
359 if(fabs(B[nr*k+n])>eps)
360 {/* Rows permutation */
362 std::swap(B[nr*k+m],B[nr*n+m]);
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));
375 B[j*nr+mb]-=B[k*nr+mb]*g;
379 for(j=0;j<NB_OF_RES;j++)
381 solutions[j*SZ+k]=B[nr*k+SZ+j];
386 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
387 /* Calculate barycentric coordinates of a 2D point p */
388 /* with respect to the triangle verices. */
389 /* triaCoords are in full interlace */
390 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
392 template<int SPACEDIM>
393 inline void barycentric_coords(const double* triaCoords, const double* p, double* bc)
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;
406 double t11 = T22, t12 = -T12, t21 = -T21, t22 = T11;
408 double r11 = p[0]-triaCoords[2*SPACEDIM], r12 = p[1]-triaCoords[2*SPACEDIM+1];
409 // barycentric coordinates: mutiply 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];
416 * Calculate barycentric coordinates of a point p with respect to triangle or tetra verices.
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.
422 inline void barycentric_coords(const std::vector<const double*>& n, const double *p, double *bc)
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);
438 T11 = n[0][_X]-n[2][_X], T12 = n[1][_X]-n[2][_X],
439 T21 = n[0][_Y]-n[2][_Y], T22 = n[1][_Y]-n[2][_Y];
440 // matrix determinant
441 double Tdet = T11*T22 - T12*T21;
442 if ( (std::fabs( Tdet) ) < (std::numeric_limits<double>::min()) )
444 bc[0]=1; bc[1]=bc[2]=0; // no solution
448 double t11 = T22, t12 = -T12, t21 = -T21, t22 = T11;
450 double r11 = p[_X]-n[2][_X], r12 = p[_Y]-n[2][_Y];
451 // barycentric coordinates: mutiply 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];
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
465 {{ n[0][_X]-n[3][_X], n[1][_X]-n[3][_X], n[2][_X]-n[3][_X], p[_X]-n[3][_X] },
466 { n[0][_Y]-n[3][_Y], n[1][_Y]-n[3][_Y], n[2][_Y]-n[3][_Y], p[_Y]-n[3][_Y] },
467 { n[0][_Z]-n[3][_Z], n[1][_Z]-n[3][_Z], n[2][_Z]-n[3][_Z], p[_Z]-n[3][_Z] }};
469 if ( !solveSystemOfEquations<3>( T, bc ) )
470 bc[0]=1., bc[1] = bc[2] = bc[3] = 0;
472 bc[ 3 ] = 1. - bc[0] - bc[1] - bc[2];
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};
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];
493 solveSystemOfEquations2<6,2>(matrix2,res,std::numeric_limits<double>::min());
495 refCoo[0]=computeTria6RefBase(res,p);
496 refCoo[1]=computeTria6RefBase(res+6,p);
497 computeWeightedCoeffsInTria6FromRefBase(refCoo,bc);
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++)
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];
525 solveSystemOfEquations2<10,3>(matrix2,res,std::numeric_limits<double>::min());
527 refCoo[0]=computeTetra10RefBase(res,p);
528 refCoo[1]=computeTetra10RefBase(res+10,p);
529 refCoo[2]=computeTetra10RefBase(res+20,p);
530 computeWeightedCoeffsInTetra10FromRefBase(refCoo,bc);
534 throw INTERP_KERNEL::Exception("INTERP_KERNEL::barycentric_coords : unrecognized simplex !");
538 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
539 /* calcul la surface d'un polygone. */
540 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
542 inline double Surf_Poly(const std::vector<double>& Poly)
546 for(unsigned long i=0; i<(Poly.size())/2-2; i++)
548 double Surf=Surf_Tri( &Poly[0],&Poly[2*(i+1)],&Poly[2*(i+2)] );
549 Surface=Surface + Surf ;
554 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
555 /* fonction qui teste si un point est dans une maille */
557 /* P_1, P_2, P_3 sommet des mailles */
558 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
560 inline bool point_dans_triangle(const double* P_0,const double* P_1,
561 const double* P_2,const double* P_3,
566 double det_1=mon_determinant(P_1,P_3,P_0);
567 double det_2=mon_determinant(P_3,P_2,P_0);
568 double det_3=mon_determinant(P_2,P_1,P_0);
569 if( (det_1>=-eps && det_2>=-eps && det_3>=-eps) || (det_1<=eps && det_2<=eps && det_3<=eps) )
577 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
578 /*fonction pour verifier qu'un point n'a pas deja ete considerer dans */
579 /* le vecteur et le rajouter au vecteur sinon. */
580 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
582 inline void verif_point_dans_vect(const double* P, std::vector<double>& V, double absolute_precision )
584 long taille=V.size();
585 bool isPresent=false;
586 for(long i=0;i<taille/2;i++)
588 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)
600 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
601 /* fonction qui rajoute les sommet du triangle P dans le vecteur V */
602 /* si ceux-ci sont compris dans le triangle S et ne sont pas deja dans */
604 /*sommets de P: P_1, P_2, P_3 */
605 /*sommets de S: P_4, P_5, P_6 */
606 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
608 inline void rajou_sommet_triangl(const double* P_1,const double* P_2,const double* P_3,
609 const double* P_4,const double* P_5,const double* P_6,
610 std::vector<double>& V, double dim_caracteristic, double precision)
613 double absolute_precision = precision*dim_caracteristic;
614 bool A_1=INTERP_KERNEL::point_dans_triangle(P_1,P_4,P_5,P_6,absolute_precision);
616 verif_point_dans_vect(P_1,V,absolute_precision);
617 bool A_2=INTERP_KERNEL::point_dans_triangle(P_2,P_4,P_5,P_6,absolute_precision);
619 verif_point_dans_vect(P_2,V,absolute_precision);
620 bool A_3=INTERP_KERNEL::point_dans_triangle(P_3,P_4,P_5,P_6,absolute_precision);
622 verif_point_dans_vect(P_3,V,absolute_precision);
626 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
627 /* calcul de l'intersection de deux segments: segments P1P2 avec P3P4 */
628 /* . Si l'intersection est non nulle et si celle-ci n'est */
629 /* n'est pas deja contenue dans Vect on la rajoute a Vect */
630 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
632 inline void inters_de_segment(const double * P_1,const double * P_2,
633 const double * P_3,const double * P_4,
634 std::vector<double>& Vect,
635 double dim_caracteristic, double precision)
637 // calcul du determinant de P_1P_2 et P_3P_4.
638 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]);
640 double absolute_precision = dim_caracteristic*precision;
641 if(fabs(det)>absolute_precision)
643 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;
645 if (k_1 >= -absolute_precision && k_1 <= 1+absolute_precision)
646 //if( k_1 >= -precision && k_1 <= 1+precision)
648 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;
650 if (k_2 >= -absolute_precision && k_2 <= 1+absolute_precision)
651 //if( k_2 >= -precision && k_2 <= 1+precision)
654 P_0[0]=P_1[0]+k_1*(P_2[0]-P_1[0]);
655 P_0[1]=P_1[1]+k_1*(P_2[1]-P_1[1]);
656 verif_point_dans_vect(P_0,Vect,absolute_precision);
664 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
665 /* calcul l'intersection de deux triangles */
666 /* P_1, P_2, P_3: sommets du premier triangle */
667 /* P_4, P_5, P_6: sommets du deuxi�me triangle */
668 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
670 inline void intersec_de_triangle(const double* P_1,const double* P_2, const double* P_3,
671 const double* P_4,const double* P_5,const double* P_6,
672 std::vector<double>& Vect, double dim_caracteristic, double precision)
674 inters_de_segment(P_1,P_2,P_4,P_5,Vect, dim_caracteristic, precision);
675 inters_de_segment(P_1,P_2,P_5,P_6,Vect, dim_caracteristic, precision);
676 inters_de_segment(P_1,P_2,P_6,P_4,Vect, dim_caracteristic, precision);
677 inters_de_segment(P_2,P_3,P_4,P_5,Vect, dim_caracteristic, precision);
678 inters_de_segment(P_2,P_3,P_5,P_6,Vect, dim_caracteristic, precision);
679 inters_de_segment(P_2,P_3,P_6,P_4,Vect, dim_caracteristic, precision);
680 inters_de_segment(P_3,P_1,P_4,P_5,Vect, dim_caracteristic, precision);
681 inters_de_segment(P_3,P_1,P_5,P_6,Vect, dim_caracteristic, precision);
682 inters_de_segment(P_3,P_1,P_6,P_4,Vect, dim_caracteristic, precision);
683 rajou_sommet_triangl(P_1,P_2,P_3,P_4,P_5,P_6,Vect, dim_caracteristic, precision);
684 rajou_sommet_triangl(P_4,P_5,P_6,P_1,P_2,P_3,Vect, dim_caracteristic, precision);
687 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
688 /* fonction pour verifier qu'un node maille n'a pas deja ete considerer */
689 /* dans le vecteur et le rajouter au vecteur sinon. */
690 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
692 inline void verif_maill_dans_vect(int Num, std::vector<int>& V)
694 long taille=V.size();
696 for(long i=0;i<taille;i++)
708 /*! Function that compares two angles from the values of the pairs (sin,cos)*/
709 /*! Angles are considered in [0, 2Pi] bt are not computed explicitely */
713 bool operator()(std::pair<double,double>theta1, std::pair<double,double> theta2)
715 double norm1 = sqrt(theta1.first*theta1.first +theta1.second*theta1.second);
716 double norm2 = sqrt(theta2.first*theta2.first +theta2.second*theta2.second);
718 double epsilon = 1.e-12;
720 if( norm1 < epsilon || norm2 < epsilon )
721 std::cout << "Warning InterpolationUtils.hxx: AngleLess : Vector with zero norm, cannot define the angle !!!! " << std::endl;
723 return theta1.second*(norm2 + theta2.first) < theta2.second*(norm1 + theta1.first);
729 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
730 /* fonction pour reconstituer un polygone convexe a partir */
731 /* d'un nuage de point. */
732 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
734 inline std::vector<double> reconstruct_polygon(const std::vector<double>& V)
737 std::size_t taille=V.size();
745 double *COS=new double[taille/2];
746 double *SIN=new double[taille/2];
747 //double *angle=new double[taille/2];
748 std::vector<double> Bary=bary_poly(V);
752 for(std::size_t i=0; i<taille/2-1;i++)
754 std::vector<double> Trigo=calcul_cos_et_sin(&Bary[0],&V[0],&V[2*(i+1)]);
758 // {angle[i+1]=atan2(SIN[i+1],COS[i+1]);}
760 // {angle[i+1]=-atan2(SIN[i+1],COS[i+1]);}
763 //ensuite on ordonne les angles.
764 std::vector<double> Pt_ordonne;
765 Pt_ordonne.reserve(taille);
766 // std::multimap<double,int> Ordre;
767 std::multimap<std::pair<double,double>,int, AngleLess> CosSin;
768 for(std::size_t i=0;i<taille/2;i++)
770 // Ordre.insert(std::make_pair(angle[i],i));
771 CosSin.insert(std::make_pair(std::make_pair(SIN[i],COS[i]),i));
773 // std::multimap <double,int>::iterator mi;
774 std::multimap<std::pair<double,double>,int, AngleLess>::iterator micossin;
775 // for(mi=Ordre.begin();mi!=Ordre.end();mi++)
777 // int j=(*mi).second;
778 // Pt_ordonne.push_back(V[2*j]);
779 // Pt_ordonne.push_back(V[2*j+1]);
781 for(micossin=CosSin.begin();micossin!=CosSin.end();micossin++)
783 int j=(*micossin).second;
784 Pt_ordonne.push_back(V[2*j]);
785 Pt_ordonne.push_back(V[2*j+1]);
794 template<int DIM, NumberingPolicy numPol, class MyMeshType>
795 inline void getElemBB(double* bb, const double *coordsOfMesh, int iP, int nb_nodes)
797 bb[0]=std::numeric_limits<double>::max();
798 bb[1]=-std::numeric_limits<double>::max();
799 bb[2]=std::numeric_limits<double>::max();
800 bb[3]=-std::numeric_limits<double>::max();
801 bb[4]=std::numeric_limits<double>::max();
802 bb[5]=-std::numeric_limits<double>::max();
804 for (int i=0; i<nb_nodes; i++)
806 double x = coordsOfMesh[3*(iP+i)];
807 double y = coordsOfMesh[3*(iP+i)+1];
808 double z = coordsOfMesh[3*(iP+i)+2];
809 bb[0]=(x<bb[0])?x:bb[0];
810 bb[1]=(x>bb[1])?x:bb[1];
811 bb[2]=(y<bb[2])?y:bb[2];
812 bb[3]=(y>bb[3])?y:bb[3];
813 bb[4]=(z<bb[4])?z:bb[4];
814 bb[5]=(z>bb[5])?z:bb[5];
818 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
819 /* Computes the dot product of a and b */
820 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
822 inline double dotprod( const double * a, const double * b)
825 for(int idim = 0; idim < dim ; idim++) result += a[idim]*b[idim];
828 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
829 /* Computes the norm of vector v */
830 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
832 inline double norm(const double * v)
835 for(int idim =0; idim<dim; idim++) result+=v[idim]*v[idim];
838 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
839 /* Computes the square norm of vector a-b */
840 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
842 inline double distance2( const double * a, const double * b)
845 for(int idim =0; idim<dim; idim++) result+=(a[idim]-b[idim])*(a[idim]-b[idim]);
848 template<class T, int dim>
849 inline double distance2( T * a, int inda, T * b, int indb)
852 for(int idim =0; idim<dim; idim++) result += ((*a)[inda+idim] - (*b)[indb+idim])* ((*a)[inda+idim] - (*b)[indb+idim]);
855 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
856 /* Computes the determinant of a and b */
857 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
858 inline double determinant ( double * a, double * b)
860 return a[0]*b[1]-a[1]*b[0];
862 inline double determinant ( double * a, double * b, double * c)
864 return a[0]*determinant(b+1,c+1)-b[0]*determinant(a+1,c+1)+c[0]*determinant(a+1,b+1);
867 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
868 /* Computes the cross product of AB and AC */
869 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
871 template<int dim> inline void crossprod( const double * A, const double * B, const double * C, double * V);
874 void crossprod<2>( const double * A, const double * B, const double * C, double * V)
878 for(int idim =0; idim<2; idim++) AB[idim] = B[idim]-A[idim];//B-A
879 for(int idim =0; idim<2; idim++) AC[idim] = C[idim]-A[idim];//C-A;
881 V[0]=determinant(AB,AC);
885 void crossprod<3>( const double * A, const double * B, const double * C, double * V)
889 for(int idim =0; idim<3; idim++) AB[idim] = B[idim]-A[idim];//B-A
890 for(int idim =0; idim<3; idim++) AC[idim] = C[idim]-A[idim];//C-A;
892 V[0]=AB[1]*AC[2]-AB[2]*AC[1];
893 V[1]=-AB[0]*AC[2]+AB[2]*AC[0];
894 V[2]=AB[0]*AC[1]-AB[1]*AC[0];
897 void crossprod<1>( const double * /*A*/, const double * /*B*/, const double * /*C*/, double * /*V*/)
899 // just to be able to compile
902 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
903 /* Checks wether point A is inside the quadrangle BCDE */
904 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
906 template<int dim> inline double check_inside(const double* A,const double* B,const double* C,const double* D,
907 const double* E,double* ABC, double* ADE)
909 crossprod<dim>(A,B,C,ABC);
910 crossprod<dim>(A,D,E,ADE);
911 return dotprod<dim>(ABC,ADE);
915 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
916 /* Computes the geometric angle (in [0,Pi]) between two non zero vectors AB and AC */
917 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
918 template<int dim> inline double angle(const double * A, const double * B, const double * C, double * n)
924 for(int idim =0; idim<dim; idim++) AB[idim] = B[idim]-A[idim];//B-A;
925 for(int idim =0; idim<dim; idim++) AC[idim] = C[idim]-A[idim];//C-A;
927 double normAB= norm<dim>(AB);
928 for(int idim =0; idim<dim; idim++) AB[idim]/=normAB;
930 double normAC= norm<dim>(AC);
931 double AB_dot_AC=dotprod<dim>(AB,AC);
932 for(int idim =0; idim<dim; idim++) orthAB[idim] = AC[idim]-AB_dot_AC*AB[idim];
934 double denom= normAC+AB_dot_AC;
935 double numer=norm<dim>(orthAB);
937 return 2*atan2(numer,denom);
940 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
941 /* Tells whether the frame constituted of vectors AB, AC and n is direct */
942 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
943 template<int dim> inline double direct_frame(const double * A, const double * B, const double * C, double * n);
945 double direct_frame<2>(const double * A, const double * B, const double * C, double * n)
949 for(int idim =0; idim<2; idim++) AB[idim] = B[idim]-A[idim];//B-A;
950 for(int idim =0; idim<2; idim++) AC[idim] = C[idim]-A[idim];//C-A;
952 return determinant(AB,AC)*n[0];
955 double direct_frame<3>(const double * A, const double * B, const double * C, double * n)
959 for(int idim =0; idim<3; idim++) AB[idim] = B[idim]-A[idim];//B-A;
960 for(int idim =0; idim<3; idim++) AC[idim] = C[idim]-A[idim];//C-A;
962 return determinant(AB,AC,n)>0;
965 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
966 /* calcul l'intersection de deux polygones COPLANAIRES */
967 /* en dimension DIM (2 ou 3). Si DIM=3 l'algorithme ne considere*/
968 /* que les deux premieres coordonnees de chaque point */
969 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
970 template<int DIM> inline void intersec_de_polygone(const double * Coords_A, const double * Coords_B,
971 int nb_NodesA, int nb_NodesB,
972 std::vector<double>& inter,
973 double dim_caracteristic, double precision)
975 for(int i_A = 1; i_A<nb_NodesA-1; i_A++)
977 for(int i_B = 1; i_B<nb_NodesB-1; i_B++)
979 INTERP_KERNEL::intersec_de_triangle(&Coords_A[0],&Coords_A[DIM*i_A],&Coords_A[DIM*(i_A+1)],
980 &Coords_B[0],&Coords_B[DIM*i_B],&Coords_B[DIM*(i_B+1)],
981 inter, dim_caracteristic, precision);
984 int nb_inter=((int)inter.size())/DIM;
985 if(nb_inter >3) inter=INTERP_KERNEL::reconstruct_polygon(inter);
989 *_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _
990 * fonctions qui calcule l'aire d'un polygone en dimension 2 ou 3
991 *_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
992 template<int DIM> inline double polygon_area(std::vector<double>& inter)
997 for(int i = 1; i<(int)inter.size()/DIM-1; i++)
999 INTERP_KERNEL::crossprod<DIM>(&inter[0],&inter[DIM*i],&inter[DIM*(i+1)],area);
1000 result +=0.5*norm<DIM>(area);
1005 template<int DIM> inline double polygon_area(std::deque<double>& inter)
1010 for(int i = 1; i<(int)inter.size()/DIM-1; i++)
1012 INTERP_KERNEL::crossprod<DIM>(&inter[0],&inter[DIM*i],&inter[DIM*(i+1)],area);
1013 result +=0.5*norm<DIM>(area);
1018 /*! Computes the triple product (XA^XB).XC (in 3D)*/
1019 inline double triple_product(const double* A, const double*B, const double*C, const double*X)
1035 (XA[1]*XB[2]-XA[2]*XB[1])*XC[0]+
1036 (XA[2]*XB[0]-XA[0]*XB[2])*XC[1]+
1037 (XA[0]*XB[1]-XA[1]*XB[0])*XC[2];
1040 /*! Subroutine of checkEqualPolygins that tests if two list of nodes (not necessarily distincts) describe the same polygon, assuming they share a comon point.*/
1041 /*! Indexes istart1 and istart2 designate two points P1 in L1 and P2 in L2 that have identical coordinates. Generally called with istart1=0.*/
1042 /*! Integer sign ( 1 or -1) indicate the direction used in going all over L2. */
1043 template<class T, int dim>
1044 bool checkEqualPolygonsOneDirection(T * L1, T * L2, int size1, int size2, int istart1, int istart2, double epsilon, int sign)
1048 int i1next = ( i1 + 1 ) % size1;
1049 int i2next = ( i2 + sign +size2) % size2;
1053 while( i1next!=istart1 && distance2<T,dim>(L1,i1*dim, L1,i1next*dim) < epsilon ) i1next = ( i1next + 1 ) % size1;
1054 while( i2next!=istart2 && distance2<T,dim>(L2,i2*dim, L2,i2next*dim) < epsilon ) i2next = ( i2next + sign +size2 ) % size2;
1056 if(i1next == istart1)
1058 if(i2next == istart2)
1063 if(i2next == istart2)
1067 if(distance2<T,dim>(L1,i1next*dim, L2,i2next*dim) > epsilon )
1073 i1next = ( i1 + 1 ) % size1;
1074 i2next = ( i2 + sign + size2 ) % size2;
1080 /*! Tests if two list of nodes (not necessarily distincts) describe the same polygon.*/
1081 /*! Existence of multiple points in the list is considered.*/
1082 template<class T, int dim>
1083 bool checkEqualPolygons(T * L1, T * L2, double epsilon)
1085 if(L1==NULL || L2==NULL)
1087 std::cout << "Warning InterpolationUtils.hxx:checkEqualPolygonsPointer: Null pointer " << std::endl;
1088 throw(Exception("big error: not closed polygon..."));
1091 int size1 = (*L1).size()/dim;
1092 int size2 = (*L2).size()/dim;
1096 while( istart2 < size2 && distance2<T,dim>(L1,istart1*dim, L2,istart2*dim) > epsilon ) istart2++;
1098 if(istart2 == size2)
1100 return (size1 == 0) && (size2 == 0);
1103 return checkEqualPolygonsOneDirection<T,dim>( L1, L2, size1, size2, istart1, istart2, epsilon, 1)
1104 || checkEqualPolygonsOneDirection<T,dim>( L1, L2, size1, size2, istart1, istart2, epsilon, -1);