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
// 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<double>::min() )
+ {
+ //sol[0]=1; sol[1]=sol[2]=sol[3]=0;
+ return false; // no solution
}
- }
- if ( max < std::numeric_limits<double>::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<double>::min() ) {
- //sol[0]=1; sol[1]=sol[2]=sol[3]=0;
- return false; // no solution
- }
+ if ( std::fabs( mRow[ nbRow-1 ] ) < std::numeric_limits<double>::min() )
+ {
+ //sol[0]=1; sol[1]=sol[2]=sol[3]=0;
+ return false; // no solution
+ }
mRow[ nbRow ] /= mRow[ nbRow-1 ];
// calculate solution (back substitution)
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<unsigned SZ, unsigned NB_OF_RES>
+ 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;k<SZ;k++)
+ {
+ np=nr*k+k;
+ if(fabs(B[np])<eps)
+ {
+ n=k;
+ do
+ {
+ n=n++;
+ if(fabs(B[nr*k+n])>eps)
+ {/* Rows permutation */
+ for(m=0;m<nr;m++)
+ std::swap(B[nr*k+m],B[nr*n+m]);
+ }
+ }
+ while (n<SZ);
+ }
+ s=B[np];//s is the Pivot
+ std::transform(B+k*nr,B+(k+1)*nr,B+k*nr,std::bind2nd(std::divides<double>(),s));
+ for(j=0;j<SZ;j++)
+ {
+ if(j!=k)
+ {
+ g=B[j*nr+k];
+ for(mb=k;mb<nr;mb++)
+ B[j*nr+mb]-=B[k*nr+mb]*g;
+ }
+ }
+ }
+ for(j=0;j<NB_OF_RES;j++)
+ for(k=0;k<SZ;k++)
+ solutions[j*SZ+k]=B[nr*k+SZ+j];
+ //
+ return true;
+ }
/*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
/* Calculate barycentric coordinates of a 2D point p */
bc[2] = 1. - bc[0] - bc[1];
}
- /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
- /* Calculate barycentric coordinates of a point p */
- /* with respect to triangle or tetra verices. */
- /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _*/
-
- inline void barycentric_coords(const std::vector<const double*>& 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<const double*>& 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<double>::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<double>::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<double>::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 !");
}
}
