From 8d0433481fedb9ef8a32d5ccfda9f656ee7080ef Mon Sep 17 00:00:00 2001 From: ageay Date: Fri, 18 Mar 2011 16:45:07 +0000 Subject: [PATCH] Implementation of P2 TRI6 barycentric_coords. --- src/INTERP_KERNEL/InterpolationUtils.hxx | 241 +++++++++++++----- src/MEDCoupling_Swig/MEDCouplingBasicsTest.py | 26 ++ 2 files changed, 204 insertions(+), 63 deletions(-) diff --git a/src/INTERP_KERNEL/InterpolationUtils.hxx b/src/INTERP_KERNEL/InterpolationUtils.hxx index e96f3fc1b..39683eaf3 100644 --- a/src/INTERP_KERNEL/InterpolationUtils.hxx +++ b/src/INTERP_KERNEL/InterpolationUtils.hxx @@ -206,6 +206,27 @@ namespace INTERP_KERNEL return Bary; } + + /*! + * Given 6 coeffs of a Tria6 returns the corresponding value of a given pos + */ + inline double computeTria6RefBase(const double *coeffs, const double *pos) + { + 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]; + } + + /*! + * Given xsi,eta in refCoo (length==2) return 6 coeffs in weightedPos. + */ + inline void computeWeightedCoeffsInTria6FromRefBase(const double *refCoo, double *weightedPos) + { + weightedPos[0]=(1.-refCoo[0]-refCoo[1])*(1.-2*refCoo[0]-2.*refCoo[1]); + weightedPos[1]=refCoo[0]*(2.*refCoo[0]-1.); + weightedPos[2]=refCoo[1]*(2.*refCoo[1]-1.); + weightedPos[3]=4.*refCoo[0]*(1.-refCoo[0]-refCoo[1]); + weightedPos[4]=4.*refCoo[0]*refCoo[1]; + weightedPos[5]=4.*refCoo[1]*(1.-refCoo[0]-refCoo[1]); + } /*! * \brief Solve system equation in matrix form using Gaussian elimination algorithm @@ -221,37 +242,42 @@ namespace INTERP_KERNEL // make upper triangular matrix (forward elimination) int iR[nbRow];// = { 0, 1, 2 }; - for ( int i = 0; i < (int) nbRow; ++i ) iR[i] = i; - + for ( int i = 0; i < (int) nbRow; ++i ) + iR[i] = i; for ( int i = 0; i < (int)(nbRow-1); ++i ) // nullify nbRow-1 rows { // swap rows to have max value of i-th column in i-th row double max = std::fabs( M[ iR[i] ][i] ); - for ( int r = i+1; r < (int)nbRow; ++r ) { - double m = std::fabs( M[ iR[r] ][i] ); - if ( m > max ) { - max = m; - std::swap( iR[r], iR[i] ); + for ( int r = i+1; r < (int)nbRow; ++r ) + { + double m = std::fabs( M[ iR[r] ][i] ); + if ( m > max ) + { + max = m; + std::swap( iR[r], iR[i] ); + } + } + if ( max < std::numeric_limits::min() ) + { + //sol[0]=1; sol[1]=sol[2]=sol[3]=0; + return false; // no solution } - } - if ( max < std::numeric_limits::min() ) { - //sol[0]=1; sol[1]=sol[2]=sol[3]=0; - return false; // no solution - } // make 0 below M[i][i] (actually we do not modify i-th column) double* tUpRow = M[ iR[i] ]; - for ( int r = i+1; r < (int)nbRow; ++r ) { - double* mRow = M[ iR[r] ]; - double coef = mRow[ i ] / tUpRow[ i ]; - for ( int c = i+1; c < nbCol; ++c ) - mRow[ c ] -= tUpRow[ c ] * coef; - } + for ( int r = i+1; r < (int)nbRow; ++r ) + { + double* mRow = M[ iR[r] ]; + double coef = mRow[ i ] / tUpRow[ i ]; + for ( int c = i+1; c < nbCol; ++c ) + mRow[ c ] -= tUpRow[ c ] * coef; + } } double* mRow = M[ iR[nbRow-1] ]; - if ( std::fabs( mRow[ nbRow-1 ] ) < std::numeric_limits::min() ) { - //sol[0]=1; sol[1]=sol[2]=sol[3]=0; - return false; // no solution - } + if ( std::fabs( mRow[ nbRow-1 ] ) < std::numeric_limits::min() ) + { + //sol[0]=1; sol[1]=sol[2]=sol[3]=0; + return false; // no solution + } mRow[ nbRow ] /= mRow[ nbRow-1 ]; // calculate solution (back substitution) @@ -270,6 +296,60 @@ namespace INTERP_KERNEL return true; } + + /*! + * \brief Solve system equation in matrix form using Gaussian elimination algorithm + * \param M - N x N+NB_OF_VARS matrix + * \param sol - vector of N solutions + * \retval bool - true if succeeded + */ + template + bool solveSystemOfEquations2(const double *matrix, double *solutions, double eps) + { + int nr,n,ka,nx,k,np; + double s,g; + int m,mb,j; + // + double B[SZ*(SZ+NB_OF_RES)]; + std::copy(matrix,matrix+SZ*(SZ+NB_OF_RES),B); + // + nr=SZ+NB_OF_RES; + nx=nr*SZ; + for(k=0;keps) + {/* Rows permutation */ + for(m=0;m(),s)); + for(j=0;j& n, const double* p, double* bc) + /*! + * Calculate barycentric coordinates of a point p with respect to triangle or tetra verices. + * This method makes 2 assumptions : + * - this is a simplex + * - 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. + * If not the case (3D surf for example) a previous projection should be done before. + */ + inline void barycentric_coords(const std::vector& n, const double *p, double *bc) { enum { _X, _Y, _Z }; - if ( n.size() == 3 ) // TRIA3 + switch(n.size()) { - // matrix 2x2 - double - T11 = n[0][_X]-n[2][_X], T12 = n[1][_X]-n[2][_X], - T21 = n[0][_Y]-n[2][_Y], T22 = n[1][_Y]-n[2][_Y]; - // matrix determinant - double Tdet = T11*T22 - T12*T21; - if ( std::fabs( Tdet ) < std::numeric_limits::min() ) { - bc[0]=1; bc[1]=bc[2]=0; // no solution - return; + case 3: + { // TRIA3 + // matrix 2x2 + double + T11 = n[0][_X]-n[2][_X], T12 = n[1][_X]-n[2][_X], + T21 = n[0][_Y]-n[2][_Y], T22 = n[1][_Y]-n[2][_Y]; + // matrix determinant + double Tdet = T11*T22 - T12*T21; + if ( std::fabs( Tdet ) < std::numeric_limits::min() ) + { + bc[0]=1; bc[1]=bc[2]=0; // no solution + return; + } + // matrix inverse + double t11 = T22, t12 = -T12, t21 = -T21, t22 = T11; + // vector + double r11 = p[_X]-n[2][_X], r12 = p[_Y]-n[2][_Y]; + // barycentric coordinates: mutiply matrix by vector + bc[0] = (t11 * r11 + t12 * r12)/Tdet; + bc[1] = (t21 * r11 + t22 * r12)/Tdet; + bc[2] = 1. - bc[0] - bc[1]; + break; } - // matrix inverse - double t11 = T22, t12 = -T12, t21 = -T21, t22 = T11; - // vector - double r11 = p[_X]-n[2][_X], r12 = p[_Y]-n[2][_Y]; - // barycentric coordinates: mutiply matrix by vector - bc[0] = (t11 * r11 + t12 * r12)/Tdet; - bc[1] = (t21 * r11 + t22 * r12)/Tdet; - bc[2] = 1. - bc[0] - bc[1]; - } - else // TETRA4 - { - // Find bc by solving system of 3 equations using Gaussian elimination algorithm - // bc1*( x1 - x4 ) + bc2*( x2 - x4 ) + bc3*( x3 - x4 ) = px - x4 - // bc1*( y1 - y4 ) + bc2*( y2 - y4 ) + bc3*( y3 - y4 ) = px - y4 - // bc1*( z1 - z4 ) + bc2*( z2 - z4 ) + bc3*( z3 - z4 ) = px - z4 - - double T[3][4]= - {{ n[0][_X]-n[3][_X], n[1][_X]-n[3][_X], n[2][_X]-n[3][_X], p[_X]-n[3][_X] }, - { n[0][_Y]-n[3][_Y], n[1][_Y]-n[3][_Y], n[2][_Y]-n[3][_Y], p[_Y]-n[3][_Y] }, - { n[0][_Z]-n[3][_Z], n[1][_Z]-n[3][_Z], n[2][_Z]-n[3][_Z], p[_Z]-n[3][_Z] }}; - - if ( !solveSystemOfEquations<3>( T, bc )) - bc[0]=1., bc[1] = bc[2] = bc[3] = 0; - else - bc[ 3 ] = 1. - bc[0] - bc[1] - bc[2]; + case 4: + { // TETRA4 + // Find bc by solving system of 3 equations using Gaussian elimination algorithm + // bc1*( x1 - x4 ) + bc2*( x2 - x4 ) + bc3*( x3 - x4 ) = px - x4 + // bc1*( y1 - y4 ) + bc2*( y2 - y4 ) + bc3*( y3 - y4 ) = px - y4 + // bc1*( z1 - z4 ) + bc2*( z2 - z4 ) + bc3*( z3 - z4 ) = px - z4 + + double T[3][4]= + {{ n[0][_X]-n[3][_X], n[1][_X]-n[3][_X], n[2][_X]-n[3][_X], p[_X]-n[3][_X] }, + { n[0][_Y]-n[3][_Y], n[1][_Y]-n[3][_Y], n[2][_Y]-n[3][_Y], p[_Y]-n[3][_Y] }, + { n[0][_Z]-n[3][_Z], n[1][_Z]-n[3][_Z], n[2][_Z]-n[3][_Z], p[_Z]-n[3][_Z] }}; + + if ( !solveSystemOfEquations<3>( T, bc ) ) + bc[0]=1., bc[1] = bc[2] = bc[3] = 0; + else + bc[ 3 ] = 1. - bc[0] - bc[1] - bc[2]; + break; + } + case 6: + { + // TRIA6 + double matrix2[48]={1., 0., 0., 0., 0., 0., 0., 0., + 1., 0., 0., 0., 0., 0., 1., 0., + 1., 0., 0., 0., 0., 0., 0., 1., + 1., 0., 0., 0., 0., 0., 0.5, 0., + 1., 0., 0., 0., 0., 0., 0.5, 0.5, + 1., 0., 0., 0., 0., 0., 0.,0.5}; + for(int i=0;i<6;i++) + { + matrix2[8*i+1]=n[i][0]; + matrix2[8*i+2]=n[i][1]; + matrix2[8*i+3]=n[i][0]*n[i][0]; + matrix2[8*i+4]=n[i][0]*n[i][1]; + matrix2[8*i+5]=n[i][1]*n[i][1]; + } + double res[12]; + solveSystemOfEquations2<6,2>(matrix2,res,std::numeric_limits::min()); + double refCoo[2]; + refCoo[0]=computeTria6RefBase(res,p); + refCoo[1]=computeTria6RefBase(res+6,p); + computeWeightedCoeffsInTria6FromRefBase(refCoo,bc); + break; + } + default: + throw INTERP_KERNEL::Exception("INTERP_KERNEL::barycentric_coords : unrecognized simplex !"); } } diff --git a/src/MEDCoupling_Swig/MEDCouplingBasicsTest.py b/src/MEDCoupling_Swig/MEDCouplingBasicsTest.py index 03a561c8f..0ec5268ac 100644 --- a/src/MEDCoupling_Swig/MEDCouplingBasicsTest.py +++ b/src/MEDCoupling_Swig/MEDCouplingBasicsTest.py @@ -6481,6 +6481,32 @@ class MEDCouplingBasicsTest(unittest.TestCase): # pass + def testP2Localization1(self): + m=MEDCouplingUMesh.New("testP2",2); + coords=[0.,2.,3.5,0.,4.5,1.5,1.2,0.32,3.4,1.,2.1,2.4] + conn=[0,1,2,3,4,5] + coo=DataArrayDouble.New(); + coo.setValues(coords,6,2); + m.setCoords(coo); + m.allocateCells(1); + m.insertNextCell(NORM_TRI6,6,conn[0:6]) + m.finishInsertingCells(); + # + f=MEDCouplingFieldDouble.New(ON_NODES,ONE_TIME); + f.setMesh(m); + da=DataArrayDouble.New(); + vals1=[1.2,2.3,3.4, 2.2,3.3,4.4, 3.2,4.3,5.4, 4.2,5.3,6.4, 5.2,6.3,7.4, 6.2,7.3,8.4] + da.setValues(vals1,6,3); + f.setArray(da); + # + loc=[2.27,1.3] + locs=f.getValueOnMulti(loc); + expected1=[6.0921164547752236, 7.1921164547752232, 8.2921164547752255] + for i in xrange(3): + self.assertAlmostEqual(expected1[i],locs.getIJ(0,i),12); + pass + pass + def testGetValueOn2(self): m=MEDCouplingDataForTest.build2DTargetMesh_1(); f=MEDCouplingFieldDouble.New(ON_CELLS,NO_TIME); -- 2.39.2