1 // Copyright (C) 2007-2012 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
20 #ifndef __INTERPOLATIONUTILS_HXX__
21 #define __INTERPOLATIONUTILS_HXX__
23 #include "INTERPKERNELDefines.hxx"
24 #include "InterpKernelException.hxx"
26 #include "NormalizedUnstructuredMesh.hxx"
37 namespace INTERP_KERNEL
39 template<class ConnType, NumberingPolicy numPol>
40 class OTT//OffsetToolTrait
44 template<class ConnType>
45 class OTT<ConnType,ALL_C_MODE>
48 static ConnType indFC(ConnType i) { return i; }
49 static ConnType ind2C(ConnType i) { return i; }
50 static ConnType conn2C(ConnType i) { return i; }
51 static ConnType coo2C(ConnType i) { return i; }
54 template<class ConnType>
55 class OTT<ConnType,ALL_FORTRAN_MODE>
58 static ConnType indFC(ConnType i) { return i+1; }
59 static ConnType ind2C(ConnType i) { return i-1; }
60 static ConnType conn2C(ConnType i) { return i-1; }
61 static ConnType coo2C(ConnType i) { return i-1; }
64 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
65 /* calcul la surface d'un triangle */
66 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
68 inline double Surf_Tri(const double* P_1,const double* P_2,const double* P_3)
70 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]);
71 double Surface = 0.5*fabs(A);
75 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
76 /* fonction qui calcul le determinant */
77 /* de deux vecteur(cf doc CGAL). */
78 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
80 //fonction qui calcul le determinant des vecteurs: P3P1 et P3P2
83 inline double mon_determinant(const double* P_1,
87 double mon_det=(P_1[0]-P_3[0])*(P_2[1]-P_3[1])-(P_2[0]-P_3[0])*(P_1[1]-P_3[1]);
91 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
92 //calcul la norme du vecteur P1P2
94 inline double norme_vecteur(const double* P_1,const double* P_2)
96 double X=P_1[0]-P_2[0];
97 double Y=P_1[1]-P_2[1];
98 double norme=sqrt(X*X+Y*Y);
102 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
103 /* calcul le cos et le sin de l'angle P1P2,P1P3 */
104 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
106 inline std::vector<double> calcul_cos_et_sin(const double* P_1,
111 std::vector<double> Vect;
112 double P1_P2=norme_vecteur(P_1,P_2);
113 double P2_P3=norme_vecteur(P_2,P_3);
114 double P3_P1=norme_vecteur(P_3,P_1);
116 double N=P1_P2*P1_P2+P3_P1*P3_P1-P2_P3*P2_P3;
117 double D=2.0*P1_P2*P3_P1;
119 if (COS>1.0) COS=1.0;
120 if (COS<-1.0) COS=-1.0;
122 double V=mon_determinant(P_2,P_3,P_1);
123 double D_1=P1_P2*P3_P1;
125 if (SIN>1.0) SIN=1.0;
126 if (SIN<-1.0) SIN=-1.0;
134 * This method builds a quadrangle built with the first point of 'triIn' the barycenter of two edges starting or ending with
135 * the first point of 'triIn' and the barycenter of 'triIn'.
137 * @param triIn is a 6 doubles array in full interlace mode, that represents a triangle.
138 * @param quadOut is a 8 doubles array filled after the following call.
140 template<int SPACEDIM>
141 inline void fillDualCellOfTri(const double *triIn, double *quadOut)
144 std::copy(triIn,triIn+SPACEDIM,quadOut);
145 double tmp[SPACEDIM];
146 std::transform(triIn,triIn+SPACEDIM,triIn+SPACEDIM,tmp,std::plus<double>());
148 std::transform(tmp,tmp+SPACEDIM,quadOut+SPACEDIM,std::bind2nd(std::multiplies<double>(),0.5));
149 std::transform(tmp,tmp+SPACEDIM,triIn+2*SPACEDIM,tmp,std::plus<double>());
151 std::transform(tmp,tmp+SPACEDIM,quadOut+2*SPACEDIM,std::bind2nd(std::multiplies<double>(),1/3.));
153 std::transform(triIn,triIn+SPACEDIM,triIn+2*SPACEDIM,tmp,std::plus<double>());
154 std::transform(tmp,tmp+SPACEDIM,quadOut+3*SPACEDIM,std::bind2nd(std::multiplies<double>(),0.5));
158 * 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
159 * the first point of 'triIn' and the barycenter of 'triIn'.
161 * @param triIn is a 6 doubles array in full interlace mode, that represents a triangle.
162 * @param quadOut is a 8 doubles array filled after the following call.
164 template<int SPACEDIM>
165 inline void fillDualCellOfPolyg(const double *polygIn, int nPtsPolygonIn, double *polygOut)
168 std::copy(polygIn,polygIn+SPACEDIM,polygOut);
169 std::transform(polygIn,polygIn+SPACEDIM,polygIn+SPACEDIM,polygOut+SPACEDIM,std::plus<double>());
171 std::transform(polygOut+SPACEDIM,polygOut+2*SPACEDIM,polygOut+SPACEDIM,std::bind2nd(std::multiplies<double>(),0.5));
172 double tmp[SPACEDIM];
174 for(int i=0;i<nPtsPolygonIn-2;i++)
176 std::transform(polygIn,polygIn+SPACEDIM,polygIn+(i+2)*SPACEDIM,tmp,std::plus<double>());
177 std::transform(tmp,tmp+SPACEDIM,polygOut+(2*i+3)*SPACEDIM,std::bind2nd(std::multiplies<double>(),0.5));
178 std::transform(polygIn+(i+1)*SPACEDIM,polygIn+(i+2)*SPACEDIM,tmp,tmp,std::plus<double>());
179 std::transform(tmp,tmp+SPACEDIM,polygOut+(2*i+2)*SPACEDIM,std::bind2nd(std::multiplies<double>(),1./3.));
183 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
184 /* calcul les coordonnees du barycentre d'un polygone */
185 /* le vecteur en entree est constitue des coordonnees */
186 /* des sommets du polygone */
187 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
189 inline std::vector<double> bary_poly(const std::vector<double>& V)
191 std::vector<double> Bary;
192 long taille=V.size();
196 for(long i=0;i<taille/2;i++)
201 double A=2*x/((double)taille);
202 double B=2*y/((double)taille);
203 Bary.push_back(A);//taille vecteur=2*nb de points.
211 * Given 6 coeffs of a Tria6 returns the corresponding value of a given pos
213 inline double computeTria6RefBase(const double *coeffs, const double *pos)
215 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];
219 * Given xsi,eta in refCoo (length==2) return 6 coeffs in weightedPos.
221 inline void computeWeightedCoeffsInTria6FromRefBase(const double *refCoo, double *weightedPos)
223 weightedPos[0]=(1.-refCoo[0]-refCoo[1])*(1.-2*refCoo[0]-2.*refCoo[1]);
224 weightedPos[1]=refCoo[0]*(2.*refCoo[0]-1.);
225 weightedPos[2]=refCoo[1]*(2.*refCoo[1]-1.);
226 weightedPos[3]=4.*refCoo[0]*(1.-refCoo[0]-refCoo[1]);
227 weightedPos[4]=4.*refCoo[0]*refCoo[1];
228 weightedPos[5]=4.*refCoo[1]*(1.-refCoo[0]-refCoo[1]);
232 * Given 10 coeffs of a Tetra10 returns the corresponding value of a given pos
234 inline double computeTetra10RefBase(const double *coeffs, const double *pos)
236 return coeffs[0]+coeffs[1]*pos[0]+coeffs[2]*pos[1]+coeffs[3]*pos[2]+
237 coeffs[4]*pos[0]*pos[0]+coeffs[5]*pos[0]*pos[1]+coeffs[6]*pos[0]*pos[2]+
238 coeffs[7]*pos[1]*pos[1]+coeffs[8]*pos[1]*pos[2]+coeffs[9]*pos[2]*pos[2];
242 * Given xsi,eta,z in refCoo (length==3) return 10 coeffs in weightedPos.
244 inline void computeWeightedCoeffsInTetra10FromRefBase(const double *refCoo, double *weightedPos)
246 //http://www.cadfamily.com/download/CAE/ABAQUS/The%20Finite%20Element%20Method%20-%20A%20practical%20course%20abaqus.pdf page 217
247 //L1=1-refCoo[0]-refCoo[1]-refCoo[2]
248 //L2=refCoo[0] L3=refCoo[1] L4=refCoo[2]
249 weightedPos[0]=(-2.*(refCoo[0]+refCoo[1]+refCoo[2])+1)*(1-refCoo[0]-refCoo[1]-refCoo[2]);//(2*L1-1)*L1
250 weightedPos[1]=(2.*refCoo[0]-1.)*refCoo[0];//(2*L2-1)*L2
251 weightedPos[2]=(2.*refCoo[1]-1.)*refCoo[1];//(2*L3-1)*L3
252 weightedPos[3]=(2.*refCoo[2]-1.)*refCoo[2];//(2*L4-1)*L4
253 weightedPos[4]=4.*(1-refCoo[0]-refCoo[1]-refCoo[2])*refCoo[0];//4*L1*L2
254 weightedPos[5]=4.*refCoo[0]*refCoo[1];//4*L2*L3
255 weightedPos[6]=4.*(1-refCoo[0]-refCoo[1]-refCoo[2])*refCoo[1];//4*L1*L3
256 weightedPos[7]=4.*(1-refCoo[0]-refCoo[1]-refCoo[2])*refCoo[2];//4*L1*L4
257 weightedPos[8]=4.*refCoo[0]*refCoo[2];//4*L2*L4
258 weightedPos[9]=4.*refCoo[1]*refCoo[2];//4*L3*L4
262 * \brief Solve system equation in matrix form using Gaussian elimination algorithm
263 * \param M - N x N+1 matrix
264 * \param sol - vector of N solutions
265 * \retval bool - true if succeeded
267 template<unsigned nbRow>
268 bool solveSystemOfEquations(double M[nbRow][nbRow+1], double* sol)
270 const int nbCol=nbRow+1;
272 // make upper triangular matrix (forward elimination)
274 int iR[nbRow];// = { 0, 1, 2 };
275 for ( int i = 0; i < (int) nbRow; ++i )
277 for ( int i = 0; i < (int)(nbRow-1); ++i ) // nullify nbRow-1 rows
279 // swap rows to have max value of i-th column in i-th row
280 double max = std::fabs( M[ iR[i] ][i] );
281 for ( int r = i+1; r < (int)nbRow; ++r )
283 double m = std::fabs( M[ iR[r] ][i] );
287 std::swap( iR[r], iR[i] );
290 if ( max < std::numeric_limits<double>::min() )
292 //sol[0]=1; sol[1]=sol[2]=sol[3]=0;
293 return false; // no solution
295 // make 0 below M[i][i] (actually we do not modify i-th column)
296 double* tUpRow = M[ iR[i] ];
297 for ( int r = i+1; r < (int)nbRow; ++r )
299 double* mRow = M[ iR[r] ];
300 double coef = mRow[ i ] / tUpRow[ i ];
301 for ( int c = i+1; c < nbCol; ++c )
302 mRow[ c ] -= tUpRow[ c ] * coef;
305 double* mRow = M[ iR[nbRow-1] ];
306 if ( std::fabs( mRow[ nbRow-1 ] ) < std::numeric_limits<double>::min() )
308 //sol[0]=1; sol[1]=sol[2]=sol[3]=0;
309 return false; // no solution
311 mRow[ nbRow ] /= mRow[ nbRow-1 ];
313 // calculate solution (back substitution)
315 sol[ nbRow-1 ] = mRow[ nbRow ];
317 for ( int i = nbRow-2; i+1; --i )
320 sol[ i ] = mRow[ nbRow ];
321 for ( int j = nbRow-1; j > i; --j )
322 sol[ i ] -= sol[j]*mRow[ j ];
323 sol[ i ] /= mRow[ i ];
331 * \brief Solve system equation in matrix form using Gaussian elimination algorithm
332 * \param M - N x N+NB_OF_VARS matrix
333 * \param sol - vector of N solutions
334 * \retval bool - true if succeeded
336 template<unsigned SZ, unsigned NB_OF_RES>
337 bool solveSystemOfEquations2(const double *matrix, double *solutions, double eps)
344 double B[SZ*(SZ+NB_OF_RES)];
345 std::copy(matrix,matrix+SZ*(SZ+NB_OF_RES),B);
357 if(fabs(B[nr*k+n])>eps)
358 {/* Rows permutation */
360 std::swap(B[nr*k+m],B[nr*n+m]);
365 s=B[np];//s is the Pivot
366 std::transform(B+k*nr,B+(k+1)*nr,B+k*nr,std::bind2nd(std::divides<double>(),s));
373 B[j*nr+mb]-=B[k*nr+mb]*g;
377 for(j=0;j<NB_OF_RES;j++)
379 solutions[j*SZ+k]=B[nr*k+SZ+j];
384 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
385 /* Calculate barycentric coordinates of a 2D point p */
386 /* with respect to the triangle verices. */
387 /* triaCoords are in full interlace */
388 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
390 template<int SPACEDIM>
391 inline void barycentric_coords(const double* triaCoords, const double* p, double* bc)
395 T11 = triaCoords[0]-triaCoords[2*SPACEDIM], T12 = triaCoords[SPACEDIM]-triaCoords[2*SPACEDIM],
396 T21 = triaCoords[1]-triaCoords[2*SPACEDIM+1], T22 = triaCoords[SPACEDIM+1]-triaCoords[2*SPACEDIM+1];
397 // matrix determinant
398 double Tdet = T11*T22 - T12*T21;
399 if ( fabs( Tdet ) < std::numeric_limits<double>::min() ) {
400 bc[0]=1; bc[1]=0; bc[2]=0;
404 double t11 = T22, t12 = -T12, t21 = -T21, t22 = T11;
406 double r11 = p[0]-triaCoords[2*SPACEDIM], r12 = p[1]-triaCoords[2*SPACEDIM+1];
407 // barycentric coordinates: mutiply matrix by vector
408 bc[0] = (t11 * r11 + t12 * r12)/Tdet;
409 bc[1] = (t21 * r11 + t22 * r12)/Tdet;
410 bc[2] = 1. - bc[0] - bc[1];
414 * Calculate barycentric coordinates of a point p with respect to triangle or tetra verices.
415 * This method makes 2 assumptions :
416 * - this is a simplex
417 * - 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.
418 * If not the case (3D surf for example) a previous projection should be done before.
420 inline void barycentric_coords(const std::vector<const double*>& n, const double *p, double *bc)
427 double delta=n[0][0]-n[1][0];
428 bc[0]=fabs((*p-n[1][0])/delta);
429 bc[1]=fabs((*p-n[0][0])/delta);
436 T11 = n[0][_X]-n[2][_X], T12 = n[1][_X]-n[2][_X],
437 T21 = n[0][_Y]-n[2][_Y], T22 = n[1][_Y]-n[2][_Y];
438 // matrix determinant
439 double Tdet = T11*T22 - T12*T21;
440 if ( std::fabs( Tdet ) < std::numeric_limits<double>::min() )
442 bc[0]=1; bc[1]=bc[2]=0; // no solution
446 double t11 = T22, t12 = -T12, t21 = -T21, t22 = T11;
448 double r11 = p[_X]-n[2][_X], r12 = p[_Y]-n[2][_Y];
449 // barycentric coordinates: mutiply matrix by vector
450 bc[0] = (t11 * r11 + t12 * r12)/Tdet;
451 bc[1] = (t21 * r11 + t22 * r12)/Tdet;
452 bc[2] = 1. - bc[0] - bc[1];
457 // Find bc by solving system of 3 equations using Gaussian elimination algorithm
458 // bc1*( x1 - x4 ) + bc2*( x2 - x4 ) + bc3*( x3 - x4 ) = px - x4
459 // bc1*( y1 - y4 ) + bc2*( y2 - y4 ) + bc3*( y3 - y4 ) = px - y4
460 // bc1*( z1 - z4 ) + bc2*( z2 - z4 ) + bc3*( z3 - z4 ) = px - z4
463 {{ n[0][_X]-n[3][_X], n[1][_X]-n[3][_X], n[2][_X]-n[3][_X], p[_X]-n[3][_X] },
464 { n[0][_Y]-n[3][_Y], n[1][_Y]-n[3][_Y], n[2][_Y]-n[3][_Y], p[_Y]-n[3][_Y] },
465 { n[0][_Z]-n[3][_Z], n[1][_Z]-n[3][_Z], n[2][_Z]-n[3][_Z], p[_Z]-n[3][_Z] }};
467 if ( !solveSystemOfEquations<3>( T, bc ) )
468 bc[0]=1., bc[1] = bc[2] = bc[3] = 0;
470 bc[ 3 ] = 1. - bc[0] - bc[1] - bc[2];
476 double matrix2[48]={1., 0., 0., 0., 0., 0., 0., 0.,
477 1., 0., 0., 0., 0., 0., 1., 0.,
478 1., 0., 0., 0., 0., 0., 0., 1.,
479 1., 0., 0., 0., 0., 0., 0.5, 0.,
480 1., 0., 0., 0., 0., 0., 0.5, 0.5,
481 1., 0., 0., 0., 0., 0., 0.,0.5};
484 matrix2[8*i+1]=n[i][0];
485 matrix2[8*i+2]=n[i][1];
486 matrix2[8*i+3]=n[i][0]*n[i][0];
487 matrix2[8*i+4]=n[i][0]*n[i][1];
488 matrix2[8*i+5]=n[i][1]*n[i][1];
491 solveSystemOfEquations2<6,2>(matrix2,res,std::numeric_limits<double>::min());
493 refCoo[0]=computeTria6RefBase(res,p);
494 refCoo[1]=computeTria6RefBase(res+6,p);
495 computeWeightedCoeffsInTria6FromRefBase(refCoo,bc);
500 double matrix2[130]={1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.,
501 1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 1., 0., 0.,
502 1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 1., 0.,
503 1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 1.,
504 1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.5, 0., 0.,
505 1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.5, 0.5, 0.,
506 1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.,0.5, 0.,
507 1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.5,
508 1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.5, 0., 0.5,
509 1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.5, 0.5};
510 for(int i=0;i<10;i++)
512 matrix2[13*i+1]=n[i][0];
513 matrix2[13*i+2]=n[i][1];
514 matrix2[13*i+3]=n[i][2];
515 matrix2[13*i+4]=n[i][0]*n[i][0];
516 matrix2[13*i+5]=n[i][0]*n[i][1];
517 matrix2[13*i+6]=n[i][0]*n[i][2];
518 matrix2[13*i+7]=n[i][1]*n[i][1];
519 matrix2[13*i+8]=n[i][1]*n[i][2];
520 matrix2[13*i+9]=n[i][2]*n[i][2];
523 solveSystemOfEquations2<10,3>(matrix2,res,std::numeric_limits<double>::min());
525 refCoo[0]=computeTetra10RefBase(res,p);
526 refCoo[1]=computeTetra10RefBase(res+10,p);
527 refCoo[2]=computeTetra10RefBase(res+20,p);
528 computeWeightedCoeffsInTetra10FromRefBase(refCoo,bc);
532 throw INTERP_KERNEL::Exception("INTERP_KERNEL::barycentric_coords : unrecognized simplex !");
536 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
537 /* calcul la surface d'un polygone. */
538 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
540 inline double Surf_Poly(const std::vector<double>& Poly)
544 for(unsigned long i=0; i<(Poly.size())/2-2; i++)
546 double Surf=Surf_Tri( &Poly[0],&Poly[2*(i+1)],&Poly[2*(i+2)] );
547 Surface=Surface + Surf ;
552 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
553 /* fonction qui teste si un point est dans une maille */
555 /* P_1, P_2, P_3 sommet des mailles */
556 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
558 inline bool point_dans_triangle(const double* P_0,const double* P_1,
559 const double* P_2,const double* P_3,
564 double det_1=mon_determinant(P_1,P_3,P_0);
565 double det_2=mon_determinant(P_3,P_2,P_0);
566 double det_3=mon_determinant(P_2,P_1,P_0);
567 if( (det_1>=-eps && det_2>=-eps && det_3>=-eps) || (det_1<=eps && det_2<=eps && det_3<=eps) )
575 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
576 /*fonction pour verifier qu'un point n'a pas deja ete considerer dans */
577 /* le vecteur et le rajouter au vecteur sinon. */
578 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
580 inline void verif_point_dans_vect(const double* P, std::vector<double>& V, double absolute_precision )
582 long taille=V.size();
583 bool isPresent=false;
584 for(long i=0;i<taille/2;i++)
586 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)
598 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
599 /* fonction qui rajoute les sommet du triangle P dans le vecteur V */
600 /* si ceux-ci sont compris dans le triangle S et ne sont pas deja dans */
602 /*sommets de P: P_1, P_2, P_3 */
603 /*sommets de S: P_4, P_5, P_6 */
604 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
606 inline void rajou_sommet_triangl(const double* P_1,const double* P_2,const double* P_3,
607 const double* P_4,const double* P_5,const double* P_6,
608 std::vector<double>& V, double dim_caracteristic, double precision)
611 double absolute_precision = precision*dim_caracteristic;
612 bool A_1=INTERP_KERNEL::point_dans_triangle(P_1,P_4,P_5,P_6,absolute_precision);
614 verif_point_dans_vect(P_1,V,absolute_precision);
615 bool A_2=INTERP_KERNEL::point_dans_triangle(P_2,P_4,P_5,P_6,absolute_precision);
617 verif_point_dans_vect(P_2,V,absolute_precision);
618 bool A_3=INTERP_KERNEL::point_dans_triangle(P_3,P_4,P_5,P_6,absolute_precision);
620 verif_point_dans_vect(P_3,V,absolute_precision);
624 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
625 /* calcul de l'intersection de deux segments: segments P1P2 avec P3P4 */
626 /* . Si l'intersection est non nulle et si celle-ci n'est */
627 /* n'est pas deja contenue dans Vect on la rajoute a Vect */
628 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
630 inline void inters_de_segment(const double * P_1,const double * P_2,
631 const double * P_3,const double * P_4,
632 std::vector<double>& Vect,
633 double dim_caracteristic, double precision)
635 // calcul du determinant de P_1P_2 et P_3P_4.
636 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]);
638 double absolute_precision = dim_caracteristic*precision;
639 if(fabs(det)>absolute_precision)
641 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;
643 if (k_1 >= -absolute_precision && k_1 <= 1+absolute_precision)
644 //if( k_1 >= -precision && k_1 <= 1+precision)
646 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;
648 if (k_2 >= -absolute_precision && k_2 <= 1+absolute_precision)
649 //if( k_2 >= -precision && k_2 <= 1+precision)
652 P_0[0]=P_1[0]+k_1*(P_2[0]-P_1[0]);
653 P_0[1]=P_1[1]+k_1*(P_2[1]-P_1[1]);
654 verif_point_dans_vect(P_0,Vect,absolute_precision);
662 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
663 /* calcul l'intersection de deux triangles */
664 /* P_1, P_2, P_3: sommets du premier triangle */
665 /* P_4, P_5, P_6: sommets du deuxi�me triangle */
666 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
668 inline void intersec_de_triangle(const double* P_1,const double* P_2, const double* P_3,
669 const double* P_4,const double* P_5,const double* P_6,
670 std::vector<double>& Vect, double dim_caracteristic, double precision)
672 inters_de_segment(P_1,P_2,P_4,P_5,Vect, dim_caracteristic, precision);
673 inters_de_segment(P_1,P_2,P_5,P_6,Vect, dim_caracteristic, precision);
674 inters_de_segment(P_1,P_2,P_6,P_4,Vect, dim_caracteristic, precision);
675 inters_de_segment(P_2,P_3,P_4,P_5,Vect, dim_caracteristic, precision);
676 inters_de_segment(P_2,P_3,P_5,P_6,Vect, dim_caracteristic, precision);
677 inters_de_segment(P_2,P_3,P_6,P_4,Vect, dim_caracteristic, precision);
678 inters_de_segment(P_3,P_1,P_4,P_5,Vect, dim_caracteristic, precision);
679 inters_de_segment(P_3,P_1,P_5,P_6,Vect, dim_caracteristic, precision);
680 inters_de_segment(P_3,P_1,P_6,P_4,Vect, dim_caracteristic, precision);
681 rajou_sommet_triangl(P_1,P_2,P_3,P_4,P_5,P_6,Vect, dim_caracteristic, precision);
682 rajou_sommet_triangl(P_4,P_5,P_6,P_1,P_2,P_3,Vect, dim_caracteristic, precision);
685 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
686 /* fonction pour verifier qu'un node maille n'a pas deja ete considerer */
687 /* dans le vecteur et le rajouter au vecteur sinon. */
688 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
690 inline void verif_maill_dans_vect(int Num, std::vector<int>& V)
692 long taille=V.size();
694 for(long i=0;i<taille;i++)
706 /*! Function that compares two angles from the values of the pairs (sin,cos)*/
707 /*! Angles are considered in [0, 2Pi] bt are not computed explicitely */
711 bool operator()(std::pair<double,double>theta1, std::pair<double,double> theta2)
713 double norm1 = sqrt(theta1.first*theta1.first +theta1.second*theta1.second);
714 double norm2 = sqrt(theta2.first*theta2.first +theta2.second*theta2.second);
716 double epsilon = 1.e-12;
718 if( norm1 < epsilon || norm2 < epsilon )
719 std::cout << "Warning InterpolationUtils.hxx: AngleLess : Vector with zero norm, cannot define the angle !!!! " << std::endl;
721 return theta1.second*(norm2 + theta2.first) < theta2.second*(norm1 + theta1.first);
727 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
728 /* fonction pour reconstituer un polygone convexe a partir */
729 /* d'un nuage de point. */
730 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
732 inline std::vector<double> reconstruct_polygon(const std::vector<double>& V)
735 std::size_t taille=V.size();
743 double *COS=new double[taille/2];
744 double *SIN=new double[taille/2];
745 //double *angle=new double[taille/2];
746 std::vector<double> Bary=bary_poly(V);
750 for(std::size_t i=0; i<taille/2-1;i++)
752 std::vector<double> Trigo=calcul_cos_et_sin(&Bary[0],&V[0],&V[2*(i+1)]);
756 // {angle[i+1]=atan2(SIN[i+1],COS[i+1]);}
758 // {angle[i+1]=-atan2(SIN[i+1],COS[i+1]);}
761 //ensuite on ordonne les angles.
762 std::vector<double> Pt_ordonne;
763 Pt_ordonne.reserve(taille);
764 // std::multimap<double,int> Ordre;
765 std::multimap<std::pair<double,double>,int, AngleLess> CosSin;
766 for(std::size_t i=0;i<taille/2;i++)
768 // Ordre.insert(std::make_pair(angle[i],i));
769 CosSin.insert(std::make_pair(std::make_pair(SIN[i],COS[i]),i));
771 // std::multimap <double,int>::iterator mi;
772 std::multimap<std::pair<double,double>,int, AngleLess>::iterator micossin;
773 // for(mi=Ordre.begin();mi!=Ordre.end();mi++)
775 // int j=(*mi).second;
776 // Pt_ordonne.push_back(V[2*j]);
777 // Pt_ordonne.push_back(V[2*j+1]);
779 for(micossin=CosSin.begin();micossin!=CosSin.end();micossin++)
781 int j=(*micossin).second;
782 Pt_ordonne.push_back(V[2*j]);
783 Pt_ordonne.push_back(V[2*j+1]);
792 template<int DIM, NumberingPolicy numPol, class MyMeshType>
793 inline void getElemBB(double* bb, const double *coordsOfMesh, int iP, int nb_nodes)
795 bb[0]=std::numeric_limits<double>::max();
796 bb[1]=-std::numeric_limits<double>::max();
797 bb[2]=std::numeric_limits<double>::max();
798 bb[3]=-std::numeric_limits<double>::max();
799 bb[4]=std::numeric_limits<double>::max();
800 bb[5]=-std::numeric_limits<double>::max();
802 for (int i=0; i<nb_nodes; i++)
804 double x = coordsOfMesh[3*(iP+i)];
805 double y = coordsOfMesh[3*(iP+i)+1];
806 double z = coordsOfMesh[3*(iP+i)+2];
807 bb[0]=(x<bb[0])?x:bb[0];
808 bb[1]=(x>bb[1])?x:bb[1];
809 bb[2]=(y<bb[2])?y:bb[2];
810 bb[3]=(y>bb[3])?y:bb[3];
811 bb[4]=(z<bb[4])?z:bb[4];
812 bb[5]=(z>bb[5])?z:bb[5];
816 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
817 /* Computes the dot product of a and b */
818 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
820 inline double dotprod( const double * a, const double * b)
823 for(int idim = 0; idim < dim ; idim++) result += a[idim]*b[idim];
826 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
827 /* Computes the norm of vector v */
828 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
830 inline double norm(const double * v)
833 for(int idim =0; idim<dim; idim++) result+=v[idim]*v[idim];
836 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
837 /* Computes the square norm of vector a-b */
838 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
840 inline double distance2( const double * a, const double * b)
843 for(int idim =0; idim<dim; idim++) result+=(a[idim]-b[idim])*(a[idim]-b[idim]);
846 template<class T, int dim>
847 inline double distance2( T * a, int inda, T * b, int indb)
850 for(int idim =0; idim<dim; idim++) result += ((*a)[inda+idim] - (*b)[indb+idim])* ((*a)[inda+idim] - (*b)[indb+idim]);
853 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
854 /* Computes the determinant of a and b */
855 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
856 inline double determinant ( double * a, double * b)
858 return a[0]*b[1]-a[1]*b[0];
860 inline double determinant ( double * a, double * b, double * c)
862 return a[0]*determinant(b+1,c+1)-b[0]*determinant(a+1,c+1)+c[0]*determinant(a+1,b+1);
865 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
866 /* Computes the cross product of AB and AC */
867 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
869 template<int dim> inline void crossprod( const double * A, const double * B, const double * C, double * V);
872 void crossprod<2>( const double * A, const double * B, const double * C, double * V)
876 for(int idim =0; idim<2; idim++) AB[idim] = B[idim]-A[idim];//B-A
877 for(int idim =0; idim<2; idim++) AC[idim] = C[idim]-A[idim];//C-A;
879 V[0]=determinant(AB,AC);
883 void crossprod<3>( const double * A, const double * B, const double * C, double * V)
887 for(int idim =0; idim<3; idim++) AB[idim] = B[idim]-A[idim];//B-A
888 for(int idim =0; idim<3; idim++) AC[idim] = C[idim]-A[idim];//C-A;
890 V[0]=AB[1]*AC[2]-AB[2]*AC[1];
891 V[1]=-AB[0]*AC[2]+AB[2]*AC[0];
892 V[2]=AB[0]*AC[1]-AB[1]*AC[0];
895 void crossprod<1>( const double * /*A*/, const double * /*B*/, const double * /*C*/, double * /*V*/)
897 // just to be able to compile
900 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
901 /* Checks wether point A is inside the quadrangle BCDE */
902 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
904 template<int dim> inline double check_inside(const double* A,const double* B,const double* C,const double* D,
905 const double* E,double* ABC, double* ADE)
907 crossprod<dim>(A,B,C,ABC);
908 crossprod<dim>(A,D,E,ADE);
909 return dotprod<dim>(ABC,ADE);
913 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
914 /* Computes the geometric angle (in [0,Pi]) between two non zero vectors AB and AC */
915 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
916 template<int dim> inline double angle(const double * A, const double * B, const double * C, double * n)
922 for(int idim =0; idim<dim; idim++) AB[idim] = B[idim]-A[idim];//B-A;
923 for(int idim =0; idim<dim; idim++) AC[idim] = C[idim]-A[idim];//C-A;
925 double normAB= norm<dim>(AB);
926 for(int idim =0; idim<dim; idim++) AB[idim]/=normAB;
928 double normAC= norm<dim>(AC);
929 double AB_dot_AC=dotprod<dim>(AB,AC);
930 for(int idim =0; idim<dim; idim++) orthAB[idim] = AC[idim]-AB_dot_AC*AB[idim];
932 double denom= normAC+AB_dot_AC;
933 double numer=norm<dim>(orthAB);
935 return 2*atan2(numer,denom);
938 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
939 /* Tells whether the frame constituted of vectors AB, AC and n is direct */
940 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
941 template<int dim> inline double direct_frame(const double * A, const double * B, const double * C, double * n);
943 double direct_frame<2>(const double * A, const double * B, const double * C, double * n)
947 for(int idim =0; idim<2; idim++) AB[idim] = B[idim]-A[idim];//B-A;
948 for(int idim =0; idim<2; idim++) AC[idim] = C[idim]-A[idim];//C-A;
950 return determinant(AB,AC)*n[0];
953 double direct_frame<3>(const double * A, const double * B, const double * C, double * n)
957 for(int idim =0; idim<3; idim++) AB[idim] = B[idim]-A[idim];//B-A;
958 for(int idim =0; idim<3; idim++) AC[idim] = C[idim]-A[idim];//C-A;
960 return determinant(AB,AC,n)>0;
963 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
964 /* calcul l'intersection de deux polygones COPLANAIRES */
965 /* en dimension DIM (2 ou 3). Si DIM=3 l'algorithme ne considere*/
966 /* que les deux premieres coordonnees de chaque point */
967 /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
968 template<int DIM> inline void intersec_de_polygone(const double * Coords_A, const double * Coords_B,
969 int nb_NodesA, int nb_NodesB,
970 std::vector<double>& inter,
971 double dim_caracteristic, double precision)
973 for(int i_A = 1; i_A<nb_NodesA-1; i_A++)
975 for(int i_B = 1; i_B<nb_NodesB-1; i_B++)
977 INTERP_KERNEL::intersec_de_triangle(&Coords_A[0],&Coords_A[DIM*i_A],&Coords_A[DIM*(i_A+1)],
978 &Coords_B[0],&Coords_B[DIM*i_B],&Coords_B[DIM*(i_B+1)],
979 inter, dim_caracteristic, precision);
982 int nb_inter=((int)inter.size())/DIM;
983 if(nb_inter >3) inter=INTERP_KERNEL::reconstruct_polygon(inter);
987 *_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _
988 * fonctions qui calcule l'aire d'un polygone en dimension 2 ou 3
989 *_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */
990 template<int DIM> inline double polygon_area(std::vector<double>& inter)
995 for(int i = 1; i<(int)inter.size()/DIM-1; i++)
997 INTERP_KERNEL::crossprod<DIM>(&inter[0],&inter[DIM*i],&inter[DIM*(i+1)],area);
998 result +=0.5*norm<DIM>(area);
1003 template<int DIM> inline double polygon_area(std::deque<double>& inter)
1008 for(int i = 1; i<(int)inter.size()/DIM-1; i++)
1010 INTERP_KERNEL::crossprod<DIM>(&inter[0],&inter[DIM*i],&inter[DIM*(i+1)],area);
1011 result +=0.5*norm<DIM>(area);
1016 /*! Computes the triple product (XA^XB).XC (in 3D)*/
1017 inline double triple_product(const double* A, const double*B, const double*C, const double*X)
1033 (XA[1]*XB[2]-XA[2]*XB[1])*XC[0]+
1034 (XA[2]*XB[0]-XA[0]*XB[2])*XC[1]+
1035 (XA[0]*XB[1]-XA[1]*XB[0])*XC[2];
1038 /*! Subroutine of checkEqualPolygins that tests if two list of nodes (not necessarily distincts) describe the same polygon, assuming they share a comon point.*/
1039 /*! Indexes istart1 and istart2 designate two points P1 in L1 and P2 in L2 that have identical coordinates. Generally called with istart1=0.*/
1040 /*! Integer sign ( 1 or -1) indicate the direction used in going all over L2. */
1041 template<class T, int dim>
1042 bool checkEqualPolygonsOneDirection(T * L1, T * L2, int size1, int size2, int istart1, int istart2, double epsilon, int sign)
1046 int i1next = ( i1 + 1 ) % size1;
1047 int i2next = ( i2 + sign +size2) % size2;
1051 while( i1next!=istart1 && distance2<T,dim>(L1,i1*dim, L1,i1next*dim) < epsilon ) i1next = ( i1next + 1 ) % size1;
1052 while( i2next!=istart2 && distance2<T,dim>(L2,i2*dim, L2,i2next*dim) < epsilon ) i2next = ( i2next + sign +size2 ) % size2;
1054 if(i1next == istart1)
1056 if(i2next == istart2)
1061 if(i2next == istart2)
1065 if(distance2<T,dim>(L1,i1next*dim, L2,i2next*dim) > epsilon )
1071 i1next = ( i1 + 1 ) % size1;
1072 i2next = ( i2 + sign + size2 ) % size2;
1078 /*! Tests if two list of nodes (not necessarily distincts) describe the same polygon.*/
1079 /*! Existence of multiple points in the list is considered.*/
1080 template<class T, int dim>
1081 bool checkEqualPolygons(T * L1, T * L2, double epsilon)
1083 if(L1==NULL || L2==NULL)
1085 std::cout << "Warning InterpolationUtils.hxx:checkEqualPolygonsPointer: Null pointer " << std::endl;
1086 throw(Exception("big error: not closed polygon..."));
1089 int size1 = (*L1).size()/dim;
1090 int size2 = (*L2).size()/dim;
1094 while( istart2 < size2 && distance2<T,dim>(L1,istart1*dim, L2,istart2*dim) > epsilon ) istart2++;
1096 if(istart2 == size2)
1098 return (size1 == 0) && (size2 == 0);
1101 return checkEqualPolygonsOneDirection<T,dim>( L1, L2, size1, size2, istart1, istart2, epsilon, 1)
1102 || checkEqualPolygonsOneDirection<T,dim>( L1, L2, size1, size2, istart1, istart2, epsilon, -1);