1 // Copyright (C) 2007-2013 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.
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"
38 namespace INTERP_KERNEL
40 template<class ConnType, NumberingPolicy numPol>
41 class OTT//OffsetToolTrait
45 template<class ConnType>
46 class OTT<ConnType,ALL_C_MODE>
49 static ConnType indFC(ConnType i) { return i; }
50 static ConnType ind2C(ConnType i) { return i; }
51 static ConnType conn2C(ConnType i) { return i; }
52 static ConnType coo2C(ConnType i) { return i; }
55 template<class ConnType>
56 class OTT<ConnType,ALL_FORTRAN_MODE>
59 static ConnType indFC(ConnType i) { return i+1; }
60 static ConnType ind2C(ConnType i) { return i-1; }
61 static ConnType conn2C(ConnType i) { return i-1; }
62 static ConnType coo2C(ConnType i) { return i-1; }
65 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
66 /* calcul la surface d'un triangle */
67 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
69 inline double Surf_Tri(const double* P_1,const double* P_2,const double* P_3)
71 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]);
72 double Surface = 0.5*fabs(A);
76 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
77 /* fonction qui calcul le determinant */
78 /* de deux vecteur(cf doc CGAL). */
79 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
81 //fonction qui calcul le determinant des vecteurs: P3P1 et P3P2
84 inline double mon_determinant(const double* P_1,
88 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]);
92 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
93 //calcul la norme du vecteur P1P2
95 inline double norme_vecteur(const double* P_1,const double* P_2)
97 double X=P_1[0]-P_2[0];
98 double Y=P_1[1]-P_2[1];
99 double norme=sqrt(X*X+Y*Y);
103 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
104 /* calcul le cos et le sin de l'angle P1P2,P1P3 */
105 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
107 inline std::vector<double> calcul_cos_et_sin(const double* P_1,
112 std::vector<double> Vect;
113 double P1_P2=norme_vecteur(P_1,P_2);
114 double P2_P3=norme_vecteur(P_2,P_3);
115 double P3_P1=norme_vecteur(P_3,P_1);
117 double N=P1_P2*P1_P2+P3_P1*P3_P1-P2_P3*P2_P3;
118 double D=2.0*P1_P2*P3_P1;
120 if (COS>1.0) COS=1.0;
121 if (COS<-1.0) COS=-1.0;
123 double V=mon_determinant(P_2,P_3,P_1);
124 double D_1=P1_P2*P3_P1;
126 if (SIN>1.0) SIN=1.0;
127 if (SIN<-1.0) SIN=-1.0;
135 * This method builds a quadrangle built with the first point of 'triIn' the barycenter of two edges starting or ending with
136 * the first point of 'triIn' and the barycenter of 'triIn'.
138 * @param triIn is a 6 doubles array in full interlace mode, that represents a triangle.
139 * @param quadOut is a 8 doubles array filled after the following call.
141 template<int SPACEDIM>
142 inline void fillDualCellOfTri(const double *triIn, double *quadOut)
145 std::copy(triIn,triIn+SPACEDIM,quadOut);
146 double tmp[SPACEDIM];
147 std::transform(triIn,triIn+SPACEDIM,triIn+SPACEDIM,tmp,std::plus<double>());
149 std::transform(tmp,tmp+SPACEDIM,quadOut+SPACEDIM,std::bind2nd(std::multiplies<double>(),0.5));
150 std::transform(tmp,tmp+SPACEDIM,triIn+2*SPACEDIM,tmp,std::plus<double>());
152 std::transform(tmp,tmp+SPACEDIM,quadOut+2*SPACEDIM,std::bind2nd(std::multiplies<double>(),1/3.));
154 std::transform(triIn,triIn+SPACEDIM,triIn+2*SPACEDIM,tmp,std::plus<double>());
155 std::transform(tmp,tmp+SPACEDIM,quadOut+3*SPACEDIM,std::bind2nd(std::multiplies<double>(),0.5));
159 * 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
160 * the first point of 'triIn' and the barycenter of 'triIn'.
162 * @param triIn is a 6 doubles array in full interlace mode, that represents a triangle.
163 * @param quadOut is a 8 doubles array filled after the following call.
165 template<int SPACEDIM>
166 inline void fillDualCellOfPolyg(const double *polygIn, int nPtsPolygonIn, double *polygOut)
169 std::copy(polygIn,polygIn+SPACEDIM,polygOut);
170 std::transform(polygIn,polygIn+SPACEDIM,polygIn+SPACEDIM,polygOut+SPACEDIM,std::plus<double>());
172 std::transform(polygOut+SPACEDIM,polygOut+2*SPACEDIM,polygOut+SPACEDIM,std::bind2nd(std::multiplies<double>(),0.5));
173 double tmp[SPACEDIM];
175 for(int i=0;i<nPtsPolygonIn-2;i++)
177 std::transform(polygIn,polygIn+SPACEDIM,polygIn+(i+2)*SPACEDIM,tmp,std::plus<double>());
178 std::transform(tmp,tmp+SPACEDIM,polygOut+(2*i+3)*SPACEDIM,std::bind2nd(std::multiplies<double>(),0.5));
179 std::transform(polygIn+(i+1)*SPACEDIM,polygIn+(i+2)*SPACEDIM,tmp,tmp,std::plus<double>());
180 std::transform(tmp,tmp+SPACEDIM,polygOut+(2*i+2)*SPACEDIM,std::bind2nd(std::multiplies<double>(),1./3.));
184 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
185 /* calcul les coordonnees du barycentre d'un polygone */
186 /* le vecteur en entree est constitue des coordonnees */
187 /* des sommets du polygone */
188 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
190 inline std::vector<double> bary_poly(const std::vector<double>& V)
192 std::vector<double> Bary;
193 long taille=V.size();
197 for(long i=0;i<taille/2;i++)
202 double A=2*x/((double)taille);
203 double B=2*y/((double)taille);
204 Bary.push_back(A);//taille vecteur=2*nb de points.
212 * Given 6 coeffs of a Tria6 returns the corresponding value of a given pos
214 inline double computeTria6RefBase(const double *coeffs, const double *pos)
216 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];
220 * Given xsi,eta in refCoo (length==2) return 6 coeffs in weightedPos.
222 inline void computeWeightedCoeffsInTria6FromRefBase(const double *refCoo, double *weightedPos)
224 weightedPos[0]=(1.-refCoo[0]-refCoo[1])*(1.-2*refCoo[0]-2.*refCoo[1]);
225 weightedPos[1]=refCoo[0]*(2.*refCoo[0]-1.);
226 weightedPos[2]=refCoo[1]*(2.*refCoo[1]-1.);
227 weightedPos[3]=4.*refCoo[0]*(1.-refCoo[0]-refCoo[1]);
228 weightedPos[4]=4.*refCoo[0]*refCoo[1];
229 weightedPos[5]=4.*refCoo[1]*(1.-refCoo[0]-refCoo[1]);
233 * Given 10 coeffs of a Tetra10 returns the corresponding value of a given pos
235 inline double computeTetra10RefBase(const double *coeffs, const double *pos)
237 return coeffs[0]+coeffs[1]*pos[0]+coeffs[2]*pos[1]+coeffs[3]*pos[2]+
238 coeffs[4]*pos[0]*pos[0]+coeffs[5]*pos[0]*pos[1]+coeffs[6]*pos[0]*pos[2]+
239 coeffs[7]*pos[1]*pos[1]+coeffs[8]*pos[1]*pos[2]+coeffs[9]*pos[2]*pos[2];
243 * Given xsi,eta,z in refCoo (length==3) return 10 coeffs in weightedPos.
245 inline void computeWeightedCoeffsInTetra10FromRefBase(const double *refCoo, double *weightedPos)
247 //http://www.cadfamily.com/download/CAE/ABAQUS/The%20Finite%20Element%20Method%20-%20A%20practical%20course%20abaqus.pdf page 217
248 //L1=1-refCoo[0]-refCoo[1]-refCoo[2]
249 //L2=refCoo[0] L3=refCoo[1] L4=refCoo[2]
250 weightedPos[0]=(-2.*(refCoo[0]+refCoo[1]+refCoo[2])+1)*(1-refCoo[0]-refCoo[1]-refCoo[2]);//(2*L1-1)*L1
251 weightedPos[1]=(2.*refCoo[0]-1.)*refCoo[0];//(2*L2-1)*L2
252 weightedPos[2]=(2.*refCoo[1]-1.)*refCoo[1];//(2*L3-1)*L3
253 weightedPos[3]=(2.*refCoo[2]-1.)*refCoo[2];//(2*L4-1)*L4
254 weightedPos[4]=4.*(1-refCoo[0]-refCoo[1]-refCoo[2])*refCoo[0];//4*L1*L2
255 weightedPos[5]=4.*refCoo[0]*refCoo[1];//4*L2*L3
256 weightedPos[6]=4.*(1-refCoo[0]-refCoo[1]-refCoo[2])*refCoo[1];//4*L1*L3
257 weightedPos[7]=4.*(1-refCoo[0]-refCoo[1]-refCoo[2])*refCoo[2];//4*L1*L4
258 weightedPos[8]=4.*refCoo[0]*refCoo[2];//4*L2*L4
259 weightedPos[9]=4.*refCoo[1]*refCoo[2];//4*L3*L4
263 * \brief Solve system equation in matrix form using Gaussian elimination algorithm
264 * \param M - N x N+1 matrix
265 * \param sol - vector of N solutions
266 * \retval bool - true if succeeded
268 template<unsigned nbRow>
269 bool solveSystemOfEquations(double M[nbRow][nbRow+1], double* sol)
271 const int nbCol=nbRow+1;
273 // make upper triangular matrix (forward elimination)
275 int iR[nbRow];// = { 0, 1, 2 };
276 for ( int i = 0; i < (int) nbRow; ++i )
278 for ( int i = 0; i < (int)(nbRow-1); ++i ) // nullify nbRow-1 rows
280 // swap rows to have max value of i-th column in i-th row
281 double max = std::fabs( M[ iR[i] ][i] );
282 for ( int r = i+1; r < (int)nbRow; ++r )
284 double m = std::fabs( M[ iR[r] ][i] );
288 std::swap( iR[r], iR[i] );
291 if ( max < std::numeric_limits<double>::min() )
293 //sol[0]=1; sol[1]=sol[2]=sol[3]=0;
294 return false; // no solution
296 // make 0 below M[i][i] (actually we do not modify i-th column)
297 double* tUpRow = M[ iR[i] ];
298 for ( int r = i+1; r < (int)nbRow; ++r )
300 double* mRow = M[ iR[r] ];
301 double coef = mRow[ i ] / tUpRow[ i ];
302 for ( int c = i+1; c < nbCol; ++c )
303 mRow[ c ] -= tUpRow[ c ] * coef;
306 double* mRow = M[ iR[nbRow-1] ];
307 if ( std::fabs( mRow[ nbRow-1 ] ) < std::numeric_limits<double>::min() )
309 //sol[0]=1; sol[1]=sol[2]=sol[3]=0;
310 return false; // no solution
312 mRow[ nbRow ] /= mRow[ nbRow-1 ];
314 // calculate solution (back substitution)
316 sol[ nbRow-1 ] = mRow[ nbRow ];
318 for ( int i = nbRow-2; i+1; --i )
321 sol[ i ] = mRow[ nbRow ];
322 for ( int j = nbRow-1; j > i; --j )
323 sol[ i ] -= sol[j]*mRow[ j ];
324 sol[ i ] /= mRow[ i ];
332 * \brief Solve system equation in matrix form using Gaussian elimination algorithm
333 * \param M - N x N+NB_OF_VARS matrix
334 * \param sol - vector of N solutions
335 * \retval bool - true if succeeded
337 template<unsigned SZ, unsigned NB_OF_RES>
338 bool solveSystemOfEquations2(const double *matrix, double *solutions, double eps)
345 double B[SZ*(SZ+NB_OF_RES)];
346 std::copy(matrix,matrix+SZ*(SZ+NB_OF_RES),B);
358 if(fabs(B[nr*k+n])>eps)
359 {/* Rows permutation */
361 std::swap(B[nr*k+m],B[nr*n+m]);
366 s=B[np];//s is the Pivot
367 std::transform(B+k*nr,B+(k+1)*nr,B+k*nr,std::bind2nd(std::divides<double>(),s));
374 B[j*nr+mb]-=B[k*nr+mb]*g;
378 for(j=0;j<NB_OF_RES;j++)
380 solutions[j*SZ+k]=B[nr*k+SZ+j];
385 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
386 /* Calculate barycentric coordinates of a 2D point p */
387 /* with respect to the triangle verices. */
388 /* triaCoords are in full interlace */
389 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
391 template<int SPACEDIM>
392 inline void barycentric_coords(const double* triaCoords, const double* p, double* bc)
396 T11 = triaCoords[0]-triaCoords[2*SPACEDIM], T12 = triaCoords[SPACEDIM]-triaCoords[2*SPACEDIM],
397 T21 = triaCoords[1]-triaCoords[2*SPACEDIM+1], T22 = triaCoords[SPACEDIM+1]-triaCoords[2*SPACEDIM+1];
398 // matrix determinant
399 double Tdet = T11*T22 - T12*T21;
400 if ( fabs( Tdet ) < std::numeric_limits<double>::min() ) {
401 bc[0]=1; bc[1]=0; bc[2]=0;
405 double t11 = T22, t12 = -T12, t21 = -T21, t22 = T11;
407 double r11 = p[0]-triaCoords[2*SPACEDIM], r12 = p[1]-triaCoords[2*SPACEDIM+1];
408 // barycentric coordinates: mutiply matrix by vector
409 bc[0] = (t11 * r11 + t12 * r12)/Tdet;
410 bc[1] = (t21 * r11 + t22 * r12)/Tdet;
411 bc[2] = 1. - bc[0] - bc[1];
415 * Calculate barycentric coordinates of a point p with respect to triangle or tetra verices.
416 * This method makes 2 assumptions :
417 * - this is a simplex
418 * - 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.
419 * If not the case (3D surf for example) a previous projection should be done before.
421 inline void barycentric_coords(const std::vector<const double*>& n, const double *p, double *bc)
428 double delta=n[0][0]-n[1][0];
429 bc[0]=fabs((*p-n[1][0])/delta);
430 bc[1]=fabs((*p-n[0][0])/delta);
437 T11 = n[0][_X]-n[2][_X], T12 = n[1][_X]-n[2][_X],
438 T21 = n[0][_Y]-n[2][_Y], T22 = n[1][_Y]-n[2][_Y];
439 // matrix determinant
440 double Tdet = T11*T22 - T12*T21;
441 if ( (std::fabs( Tdet) ) < (std::numeric_limits<double>::min()) )
443 bc[0]=1; bc[1]=bc[2]=0; // no solution
447 double t11 = T22, t12 = -T12, t21 = -T21, t22 = T11;
449 double r11 = p[_X]-n[2][_X], r12 = p[_Y]-n[2][_Y];
450 // barycentric coordinates: mutiply matrix by vector
451 bc[0] = (t11 * r11 + t12 * r12)/Tdet;
452 bc[1] = (t21 * r11 + t22 * r12)/Tdet;
453 bc[2] = 1. - bc[0] - bc[1];
458 // Find bc by solving system of 3 equations using Gaussian elimination algorithm
459 // bc1*( x1 - x4 ) + bc2*( x2 - x4 ) + bc3*( x3 - x4 ) = px - x4
460 // bc1*( y1 - y4 ) + bc2*( y2 - y4 ) + bc3*( y3 - y4 ) = px - y4
461 // bc1*( z1 - z4 ) + bc2*( z2 - z4 ) + bc3*( z3 - z4 ) = px - z4
464 {{ n[0][_X]-n[3][_X], n[1][_X]-n[3][_X], n[2][_X]-n[3][_X], p[_X]-n[3][_X] },
465 { n[0][_Y]-n[3][_Y], n[1][_Y]-n[3][_Y], n[2][_Y]-n[3][_Y], p[_Y]-n[3][_Y] },
466 { n[0][_Z]-n[3][_Z], n[1][_Z]-n[3][_Z], n[2][_Z]-n[3][_Z], p[_Z]-n[3][_Z] }};
468 if ( !solveSystemOfEquations<3>( T, bc ) )
469 bc[0]=1., bc[1] = bc[2] = bc[3] = 0;
471 bc[ 3 ] = 1. - bc[0] - bc[1] - bc[2];
477 double matrix2[48]={1., 0., 0., 0., 0., 0., 0., 0.,
478 1., 0., 0., 0., 0., 0., 1., 0.,
479 1., 0., 0., 0., 0., 0., 0., 1.,
480 1., 0., 0., 0., 0., 0., 0.5, 0.,
481 1., 0., 0., 0., 0., 0., 0.5, 0.5,
482 1., 0., 0., 0., 0., 0., 0.,0.5};
485 matrix2[8*i+1]=n[i][0];
486 matrix2[8*i+2]=n[i][1];
487 matrix2[8*i+3]=n[i][0]*n[i][0];
488 matrix2[8*i+4]=n[i][0]*n[i][1];
489 matrix2[8*i+5]=n[i][1]*n[i][1];
492 solveSystemOfEquations2<6,2>(matrix2,res,std::numeric_limits<double>::min());
494 refCoo[0]=computeTria6RefBase(res,p);
495 refCoo[1]=computeTria6RefBase(res+6,p);
496 computeWeightedCoeffsInTria6FromRefBase(refCoo,bc);
501 double matrix2[130]={1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.,
502 1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 1., 0., 0.,
503 1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 1., 0.,
504 1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 1.,
505 1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.5, 0., 0.,
506 1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.5, 0.5, 0.,
507 1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.,0.5, 0.,
508 1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.5,
509 1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.5, 0., 0.5,
510 1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.5, 0.5};
511 for(int i=0;i<10;i++)
513 matrix2[13*i+1]=n[i][0];
514 matrix2[13*i+2]=n[i][1];
515 matrix2[13*i+3]=n[i][2];
516 matrix2[13*i+4]=n[i][0]*n[i][0];
517 matrix2[13*i+5]=n[i][0]*n[i][1];
518 matrix2[13*i+6]=n[i][0]*n[i][2];
519 matrix2[13*i+7]=n[i][1]*n[i][1];
520 matrix2[13*i+8]=n[i][1]*n[i][2];
521 matrix2[13*i+9]=n[i][2]*n[i][2];
524 solveSystemOfEquations2<10,3>(matrix2,res,std::numeric_limits<double>::min());
526 refCoo[0]=computeTetra10RefBase(res,p);
527 refCoo[1]=computeTetra10RefBase(res+10,p);
528 refCoo[2]=computeTetra10RefBase(res+20,p);
529 computeWeightedCoeffsInTetra10FromRefBase(refCoo,bc);
533 throw INTERP_KERNEL::Exception("INTERP_KERNEL::barycentric_coords : unrecognized simplex !");
537 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
538 /* calcul la surface d'un polygone. */
539 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
541 inline double Surf_Poly(const std::vector<double>& Poly)
545 for(unsigned long i=0; i<(Poly.size())/2-2; i++)
547 double Surf=Surf_Tri( &Poly[0],&Poly[2*(i+1)],&Poly[2*(i+2)] );
548 Surface=Surface + Surf ;
553 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
554 /* fonction qui teste si un point est dans une maille */
556 /* P_1, P_2, P_3 sommet des mailles */
557 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
559 inline bool point_dans_triangle(const double* P_0,const double* P_1,
560 const double* P_2,const double* P_3,
565 double det_1=mon_determinant(P_1,P_3,P_0);
566 double det_2=mon_determinant(P_3,P_2,P_0);
567 double det_3=mon_determinant(P_2,P_1,P_0);
568 if( (det_1>=-eps && det_2>=-eps && det_3>=-eps) || (det_1<=eps && det_2<=eps && det_3<=eps) )
576 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
577 /*fonction pour verifier qu'un point n'a pas deja ete considerer dans */
578 /* le vecteur et le rajouter au vecteur sinon. */
579 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
581 inline void verif_point_dans_vect(const double* P, std::vector<double>& V, double absolute_precision )
583 long taille=V.size();
584 bool isPresent=false;
585 for(long i=0;i<taille/2;i++)
587 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)
599 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
600 /* fonction qui rajoute les sommet du triangle P dans le vecteur V */
601 /* si ceux-ci sont compris dans le triangle S et ne sont pas deja dans */
603 /*sommets de P: P_1, P_2, P_3 */
604 /*sommets de S: P_4, P_5, P_6 */
605 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
607 inline void rajou_sommet_triangl(const double* P_1,const double* P_2,const double* P_3,
608 const double* P_4,const double* P_5,const double* P_6,
609 std::vector<double>& V, double dim_caracteristic, double precision)
612 double absolute_precision = precision*dim_caracteristic;
613 bool A_1=INTERP_KERNEL::point_dans_triangle(P_1,P_4,P_5,P_6,absolute_precision);
615 verif_point_dans_vect(P_1,V,absolute_precision);
616 bool A_2=INTERP_KERNEL::point_dans_triangle(P_2,P_4,P_5,P_6,absolute_precision);
618 verif_point_dans_vect(P_2,V,absolute_precision);
619 bool A_3=INTERP_KERNEL::point_dans_triangle(P_3,P_4,P_5,P_6,absolute_precision);
621 verif_point_dans_vect(P_3,V,absolute_precision);
625 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
626 /* calcul de l'intersection de deux segments: segments P1P2 avec P3P4 */
627 /* . Si l'intersection est non nulle et si celle-ci n'est */
628 /* n'est pas deja contenue dans Vect on la rajoute a Vect */
629 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
631 inline void inters_de_segment(const double * P_1,const double * P_2,
632 const double * P_3,const double * P_4,
633 std::vector<double>& Vect,
634 double dim_caracteristic, double precision)
636 // calcul du determinant de P_1P_2 et P_3P_4.
637 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]);
639 double absolute_precision = dim_caracteristic*precision;
640 if(fabs(det)>absolute_precision)
642 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;
644 if (k_1 >= -absolute_precision && k_1 <= 1+absolute_precision)
645 //if( k_1 >= -precision && k_1 <= 1+precision)
647 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;
649 if (k_2 >= -absolute_precision && k_2 <= 1+absolute_precision)
650 //if( k_2 >= -precision && k_2 <= 1+precision)
653 P_0[0]=P_1[0]+k_1*(P_2[0]-P_1[0]);
654 P_0[1]=P_1[1]+k_1*(P_2[1]-P_1[1]);
655 verif_point_dans_vect(P_0,Vect,absolute_precision);
663 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
664 /* calcul l'intersection de deux triangles */
665 /* P_1, P_2, P_3: sommets du premier triangle */
666 /* P_4, P_5, P_6: sommets du deuxi�me triangle */
667 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
669 inline void intersec_de_triangle(const double* P_1,const double* P_2, const double* P_3,
670 const double* P_4,const double* P_5,const double* P_6,
671 std::vector<double>& Vect, double dim_caracteristic, double precision)
673 inters_de_segment(P_1,P_2,P_4,P_5,Vect, dim_caracteristic, precision);
674 inters_de_segment(P_1,P_2,P_5,P_6,Vect, dim_caracteristic, precision);
675 inters_de_segment(P_1,P_2,P_6,P_4,Vect, dim_caracteristic, precision);
676 inters_de_segment(P_2,P_3,P_4,P_5,Vect, dim_caracteristic, precision);
677 inters_de_segment(P_2,P_3,P_5,P_6,Vect, dim_caracteristic, precision);
678 inters_de_segment(P_2,P_3,P_6,P_4,Vect, dim_caracteristic, precision);
679 inters_de_segment(P_3,P_1,P_4,P_5,Vect, dim_caracteristic, precision);
680 inters_de_segment(P_3,P_1,P_5,P_6,Vect, dim_caracteristic, precision);
681 inters_de_segment(P_3,P_1,P_6,P_4,Vect, dim_caracteristic, precision);
682 rajou_sommet_triangl(P_1,P_2,P_3,P_4,P_5,P_6,Vect, dim_caracteristic, precision);
683 rajou_sommet_triangl(P_4,P_5,P_6,P_1,P_2,P_3,Vect, dim_caracteristic, precision);
686 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
687 /* fonction pour verifier qu'un node maille n'a pas deja ete considerer */
688 /* dans le vecteur et le rajouter au vecteur sinon. */
689 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
691 inline void verif_maill_dans_vect(int Num, std::vector<int>& V)
693 long taille=V.size();
695 for(long i=0;i<taille;i++)
707 /*! Function that compares two angles from the values of the pairs (sin,cos)*/
708 /*! Angles are considered in [0, 2Pi] bt are not computed explicitely */
712 bool operator()(std::pair<double,double>theta1, std::pair<double,double> theta2)
714 double norm1 = sqrt(theta1.first*theta1.first +theta1.second*theta1.second);
715 double norm2 = sqrt(theta2.first*theta2.first +theta2.second*theta2.second);
717 double epsilon = 1.e-12;
719 if( norm1 < epsilon || norm2 < epsilon )
720 std::cout << "Warning InterpolationUtils.hxx: AngleLess : Vector with zero norm, cannot define the angle !!!! " << std::endl;
722 return theta1.second*(norm2 + theta2.first) < theta2.second*(norm1 + theta1.first);
728 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
729 /* fonction pour reconstituer un polygone convexe a partir */
730 /* d'un nuage de point. */
731 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
733 inline std::vector<double> reconstruct_polygon(const std::vector<double>& V)
736 std::size_t taille=V.size();
744 double *COS=new double[taille/2];
745 double *SIN=new double[taille/2];
746 //double *angle=new double[taille/2];
747 std::vector<double> Bary=bary_poly(V);
751 for(std::size_t i=0; i<taille/2-1;i++)
753 std::vector<double> Trigo=calcul_cos_et_sin(&Bary[0],&V[0],&V[2*(i+1)]);
757 // {angle[i+1]=atan2(SIN[i+1],COS[i+1]);}
759 // {angle[i+1]=-atan2(SIN[i+1],COS[i+1]);}
762 //ensuite on ordonne les angles.
763 std::vector<double> Pt_ordonne;
764 Pt_ordonne.reserve(taille);
765 // std::multimap<double,int> Ordre;
766 std::multimap<std::pair<double,double>,int, AngleLess> CosSin;
767 for(std::size_t i=0;i<taille/2;i++)
769 // Ordre.insert(std::make_pair(angle[i],i));
770 CosSin.insert(std::make_pair(std::make_pair(SIN[i],COS[i]),i));
772 // std::multimap <double,int>::iterator mi;
773 std::multimap<std::pair<double,double>,int, AngleLess>::iterator micossin;
774 // for(mi=Ordre.begin();mi!=Ordre.end();mi++)
776 // int j=(*mi).second;
777 // Pt_ordonne.push_back(V[2*j]);
778 // Pt_ordonne.push_back(V[2*j+1]);
780 for(micossin=CosSin.begin();micossin!=CosSin.end();micossin++)
782 int j=(*micossin).second;
783 Pt_ordonne.push_back(V[2*j]);
784 Pt_ordonne.push_back(V[2*j+1]);
793 template<int DIM, NumberingPolicy numPol, class MyMeshType>
794 inline void getElemBB(double* bb, const double *coordsOfMesh, int iP, int nb_nodes)
796 bb[0]=std::numeric_limits<double>::max();
797 bb[1]=-std::numeric_limits<double>::max();
798 bb[2]=std::numeric_limits<double>::max();
799 bb[3]=-std::numeric_limits<double>::max();
800 bb[4]=std::numeric_limits<double>::max();
801 bb[5]=-std::numeric_limits<double>::max();
803 for (int i=0; i<nb_nodes; i++)
805 double x = coordsOfMesh[3*(iP+i)];
806 double y = coordsOfMesh[3*(iP+i)+1];
807 double z = coordsOfMesh[3*(iP+i)+2];
808 bb[0]=(x<bb[0])?x:bb[0];
809 bb[1]=(x>bb[1])?x:bb[1];
810 bb[2]=(y<bb[2])?y:bb[2];
811 bb[3]=(y>bb[3])?y:bb[3];
812 bb[4]=(z<bb[4])?z:bb[4];
813 bb[5]=(z>bb[5])?z:bb[5];
817 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
818 /* Computes the dot product of a and b */
819 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
821 inline double dotprod( const double * a, const double * b)
824 for(int idim = 0; idim < dim ; idim++) result += a[idim]*b[idim];
827 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
828 /* Computes the norm of vector v */
829 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
831 inline double norm(const double * v)
834 for(int idim =0; idim<dim; idim++) result+=v[idim]*v[idim];
837 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
838 /* Computes the square norm of vector a-b */
839 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
841 inline double distance2( const double * a, const double * b)
844 for(int idim =0; idim<dim; idim++) result+=(a[idim]-b[idim])*(a[idim]-b[idim]);
847 template<class T, int dim>
848 inline double distance2( T * a, int inda, T * b, int indb)
851 for(int idim =0; idim<dim; idim++) result += ((*a)[inda+idim] - (*b)[indb+idim])* ((*a)[inda+idim] - (*b)[indb+idim]);
854 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
855 /* Computes the determinant of a and b */
856 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
857 inline double determinant ( double * a, double * b)
859 return a[0]*b[1]-a[1]*b[0];
861 inline double determinant ( double * a, double * b, double * c)
863 return a[0]*determinant(b+1,c+1)-b[0]*determinant(a+1,c+1)+c[0]*determinant(a+1,b+1);
866 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
867 /* Computes the cross product of AB and AC */
868 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
870 template<int dim> inline void crossprod( const double * A, const double * B, const double * C, double * V);
873 void crossprod<2>( const double * A, const double * B, const double * C, double * V)
877 for(int idim =0; idim<2; idim++) AB[idim] = B[idim]-A[idim];//B-A
878 for(int idim =0; idim<2; idim++) AC[idim] = C[idim]-A[idim];//C-A;
880 V[0]=determinant(AB,AC);
884 void crossprod<3>( const double * A, const double * B, const double * C, double * V)
888 for(int idim =0; idim<3; idim++) AB[idim] = B[idim]-A[idim];//B-A
889 for(int idim =0; idim<3; idim++) AC[idim] = C[idim]-A[idim];//C-A;
891 V[0]=AB[1]*AC[2]-AB[2]*AC[1];
892 V[1]=-AB[0]*AC[2]+AB[2]*AC[0];
893 V[2]=AB[0]*AC[1]-AB[1]*AC[0];
896 void crossprod<1>( const double * /*A*/, const double * /*B*/, const double * /*C*/, double * /*V*/)
898 // just to be able to compile
901 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
902 /* Checks wether point A is inside the quadrangle BCDE */
903 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
905 template<int dim> inline double check_inside(const double* A,const double* B,const double* C,const double* D,
906 const double* E,double* ABC, double* ADE)
908 crossprod<dim>(A,B,C,ABC);
909 crossprod<dim>(A,D,E,ADE);
910 return dotprod<dim>(ABC,ADE);
914 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
915 /* Computes the geometric angle (in [0,Pi]) between two non zero vectors AB and AC */
916 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
917 template<int dim> inline double angle(const double * A, const double * B, const double * C, double * n)
923 for(int idim =0; idim<dim; idim++) AB[idim] = B[idim]-A[idim];//B-A;
924 for(int idim =0; idim<dim; idim++) AC[idim] = C[idim]-A[idim];//C-A;
926 double normAB= norm<dim>(AB);
927 for(int idim =0; idim<dim; idim++) AB[idim]/=normAB;
929 double normAC= norm<dim>(AC);
930 double AB_dot_AC=dotprod<dim>(AB,AC);
931 for(int idim =0; idim<dim; idim++) orthAB[idim] = AC[idim]-AB_dot_AC*AB[idim];
933 double denom= normAC+AB_dot_AC;
934 double numer=norm<dim>(orthAB);
936 return 2*atan2(numer,denom);
939 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
940 /* Tells whether the frame constituted of vectors AB, AC and n is direct */
941 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
942 template<int dim> inline double direct_frame(const double * A, const double * B, const double * C, double * n);
944 double direct_frame<2>(const double * A, const double * B, const double * C, double * n)
948 for(int idim =0; idim<2; idim++) AB[idim] = B[idim]-A[idim];//B-A;
949 for(int idim =0; idim<2; idim++) AC[idim] = C[idim]-A[idim];//C-A;
951 return determinant(AB,AC)*n[0];
954 double direct_frame<3>(const double * A, const double * B, const double * C, double * n)
958 for(int idim =0; idim<3; idim++) AB[idim] = B[idim]-A[idim];//B-A;
959 for(int idim =0; idim<3; idim++) AC[idim] = C[idim]-A[idim];//C-A;
961 return determinant(AB,AC,n)>0;
964 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
965 /* calcul l'intersection de deux polygones COPLANAIRES */
966 /* en dimension DIM (2 ou 3). Si DIM=3 l'algorithme ne considere*/
967 /* que les deux premieres coordonnees de chaque point */
968 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
969 template<int DIM> inline void intersec_de_polygone(const double * Coords_A, const double * Coords_B,
970 int nb_NodesA, int nb_NodesB,
971 std::vector<double>& inter,
972 double dim_caracteristic, double precision)
974 for(int i_A = 1; i_A<nb_NodesA-1; i_A++)
976 for(int i_B = 1; i_B<nb_NodesB-1; i_B++)
978 INTERP_KERNEL::intersec_de_triangle(&Coords_A[0],&Coords_A[DIM*i_A],&Coords_A[DIM*(i_A+1)],
979 &Coords_B[0],&Coords_B[DIM*i_B],&Coords_B[DIM*(i_B+1)],
980 inter, dim_caracteristic, precision);
983 int nb_inter=((int)inter.size())/DIM;
984 if(nb_inter >3) inter=INTERP_KERNEL::reconstruct_polygon(inter);
988 *_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _
989 * fonctions qui calcule l'aire d'un polygone en dimension 2 ou 3
990 *_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
991 template<int DIM> inline double polygon_area(std::vector<double>& inter)
996 for(int i = 1; i<(int)inter.size()/DIM-1; i++)
998 INTERP_KERNEL::crossprod<DIM>(&inter[0],&inter[DIM*i],&inter[DIM*(i+1)],area);
999 result +=0.5*norm<DIM>(area);
1004 template<int DIM> inline double polygon_area(std::deque<double>& inter)
1009 for(int i = 1; i<(int)inter.size()/DIM-1; i++)
1011 INTERP_KERNEL::crossprod<DIM>(&inter[0],&inter[DIM*i],&inter[DIM*(i+1)],area);
1012 result +=0.5*norm<DIM>(area);
1017 /*! Computes the triple product (XA^XB).XC (in 3D)*/
1018 inline double triple_product(const double* A, const double*B, const double*C, const double*X)
1034 (XA[1]*XB[2]-XA[2]*XB[1])*XC[0]+
1035 (XA[2]*XB[0]-XA[0]*XB[2])*XC[1]+
1036 (XA[0]*XB[1]-XA[1]*XB[0])*XC[2];
1039 /*! Subroutine of checkEqualPolygins that tests if two list of nodes (not necessarily distincts) describe the same polygon, assuming they share a comon point.*/
1040 /*! Indexes istart1 and istart2 designate two points P1 in L1 and P2 in L2 that have identical coordinates. Generally called with istart1=0.*/
1041 /*! Integer sign ( 1 or -1) indicate the direction used in going all over L2. */
1042 template<class T, int dim>
1043 bool checkEqualPolygonsOneDirection(T * L1, T * L2, int size1, int size2, int istart1, int istart2, double epsilon, int sign)
1047 int i1next = ( i1 + 1 ) % size1;
1048 int i2next = ( i2 + sign +size2) % size2;
1052 while( i1next!=istart1 && distance2<T,dim>(L1,i1*dim, L1,i1next*dim) < epsilon ) i1next = ( i1next + 1 ) % size1;
1053 while( i2next!=istart2 && distance2<T,dim>(L2,i2*dim, L2,i2next*dim) < epsilon ) i2next = ( i2next + sign +size2 ) % size2;
1055 if(i1next == istart1)
1057 if(i2next == istart2)
1062 if(i2next == istart2)
1066 if(distance2<T,dim>(L1,i1next*dim, L2,i2next*dim) > epsilon )
1072 i1next = ( i1 + 1 ) % size1;
1073 i2next = ( i2 + sign + size2 ) % size2;
1079 /*! Tests if two list of nodes (not necessarily distincts) describe the same polygon.*/
1080 /*! Existence of multiple points in the list is considered.*/
1081 template<class T, int dim>
1082 bool checkEqualPolygons(T * L1, T * L2, double epsilon)
1084 if(L1==NULL || L2==NULL)
1086 std::cout << "Warning InterpolationUtils.hxx:checkEqualPolygonsPointer: Null pointer " << std::endl;
1087 throw(Exception("big error: not closed polygon..."));
1090 int size1 = (*L1).size()/dim;
1091 int size2 = (*L2).size()/dim;
1095 while( istart2 < size2 && distance2<T,dim>(L1,istart1*dim, L2,istart2*dim) > epsilon ) istart2++;
1097 if(istart2 == size2)
1099 return (size1 == 0) && (size2 == 0);
1102 return checkEqualPolygonsOneDirection<T,dim>( L1, L2, size1, size2, istart1, istart2, epsilon, 1)
1103 || checkEqualPolygonsOneDirection<T,dim>( L1, L2, size1, size2, istart1, istart2, epsilon, -1);