Back to index

salome-med  6.5.0
TransformedTriangleMath.cxx
Go to the documentation of this file.
00001 // Copyright (C) 2007-2012  CEA/DEN, EDF R&D
00002 //
00003 // This library is free software; you can redistribute it and/or
00004 // modify it under the terms of the GNU Lesser General Public
00005 // License as published by the Free Software Foundation; either
00006 // version 2.1 of the License.
00007 //
00008 // This library is distributed in the hope that it will be useful,
00009 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00010 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00011 // Lesser General Public License for more details.
00012 //
00013 // You should have received a copy of the GNU Lesser General Public
00014 // License along with this library; if not, write to the Free Software
00015 // Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA
00016 //
00017 // See http://www.salome-platform.org/ or email : webmaster.salome@opencascade.com
00018 //
00019 
00020 #include "TransformedTriangle.hxx"
00021 #include <iostream>
00022 #include <fstream>
00023 #include <cassert>
00024 #include <cmath>
00025 #include <limits>
00026 #include <map>
00027 #include <utility>
00028 
00029 #include "VectorUtils.hxx"
00030 
00031 namespace INTERP_KERNEL
00032 {
00033   
00034   // ----------------------------------------------------------------------------------
00035   //  Tables                                                       
00036   // ----------------------------------------------------------------------------------
00037 
00039   const int TransformedTriangle::DP_OFFSET_1[8] = {1, 2, 0, 2, 0, 1, 4, 1};
00040 
00042   const int TransformedTriangle::DP_OFFSET_2[8] = {2, 0, 1, 3, 3, 3, 0, 4};
00043 
00045   const int TransformedTriangle::COORDINATE_FOR_DETERMINANT_EXPANSION[12] =
00046     {
00047       // row 1, 2, 3
00048       0, 1, 2, // O
00049       3, 1, 2, // X
00050       0, 3, 2, // Y
00051       0, 1, 3  // Z
00052     };
00053   
00055   const TransformedTriangle::DoubleProduct TransformedTriangle::DP_FOR_DETERMINANT_EXPANSION[12] = 
00056     {
00057       // row 1, 2, 3
00058       C_YZ, C_ZX, C_XY, // O
00059       C_YZ, C_ZH, C_YH, // X
00060       C_ZH, C_ZX, C_XH, // Y
00061       C_YH, C_XH, C_XY  // Z
00062     };
00063   
00065   const long double TransformedTriangle::MACH_EPS = std::numeric_limits<double>::epsilon();
00066   
00068   const long double TransformedTriangle::MULT_PREC_F = 4.0 * TransformedTriangle::MACH_EPS;
00069 
00071   const long double TransformedTriangle::THRESHOLD_F = 500.0;
00072 
00074   const double TransformedTriangle::TRIPLE_PRODUCT_ANGLE_THRESHOLD = 0.1;
00075 
00076 
00077   // after transformation to the U-space, coordinates are inaccurate
00078   // small variations around zero should not be taken into account
00079   void TransformedTriangle::resetNearZeroCoordinates()
00080   {
00081     for (int i=0; i<15; i++)
00082       if (fabs(_coords[i])<TransformedTriangle::MACH_EPS*20.0) _coords[i]=0.0;
00083   }
00084   
00085   // ----------------------------------------------------------------------------------
00086   //  Double and triple product calculations                           
00087   // ----------------------------------------------------------------------------------
00088   
00095   void TransformedTriangle::preCalculateDoubleProducts(void)
00096   {
00097     if(_is_double_products_calculated)
00098       return;
00099 
00100     // -- calculate all unstable double products -- store in _doubleProducts
00101     for(TriSegment seg = PQ ; seg <= RP ; seg = TriSegment(seg + 1))
00102       {
00103         for(DoubleProduct dp = C_YZ ; dp <= C_10 ; dp = DoubleProduct(dp + 1))
00104           _doubleProducts[8*seg + dp] = calcUnstableC(seg, dp);
00105       }
00106 
00107     std::map<double, TetraCorner> distances;
00108 
00109     // -- (1) for each segment : check that double products satisfy Grandy, [46]
00110     // -- and make corrections if not
00111     for(TriSegment seg = PQ ; seg <= RP ; seg = TriSegment(seg + 1))
00112       {
00113         if(!areDoubleProductsConsistent(seg))
00114           {
00115             LOG(4, "inconsistent! ");
00116             for(TetraCorner corner = O ; corner <= Z ; corner = TetraCorner(corner + 1))
00117               {
00118                 // calculate distance corner - segment axis
00119                 const double dist = calculateDistanceCornerSegment(corner, seg);
00120                 distances.insert( std::make_pair( dist, corner ) );
00121               }
00122 
00123             // first element -> minimum distance
00124             const TetraCorner minCorner = distances.begin()->second;
00125             resetDoubleProducts(seg, minCorner);
00126             distances.clear();
00127           }
00128       }
00129   
00130     // -- (2) check that each double product statisfies Grandy, [47], else set to 0
00131     for(TriSegment seg = PQ ; seg <= RP ; seg = TriSegment(seg + 1))
00132       {
00133         for(DoubleProduct dp = C_YZ ; dp <=  C_10 ; dp = DoubleProduct(dp + 1))
00134           {
00135             // find the points of the triangle
00136             // 0 -> P, 1 -> Q, 2 -> R 
00137             const int pt1 = seg;
00138             const int pt2 = (seg + 1) % 3;
00139 
00140             // find offsets
00141             const int off1 = DP_OFFSET_1[dp];
00142             const int off2 = DP_OFFSET_2[dp];
00143 
00144             const double term1 = _coords[5*pt1 + off1] * _coords[5*pt2 + off2]; 
00145             const double term2 = _coords[5*pt1 + off2] * _coords[5*pt2 + off1];
00146 
00147             const long double delta = MULT_PREC_F * ( std::fabs(term1) + std::fabs(term2) );
00148          
00149             if( epsilonEqual(_doubleProducts[8*seg + dp], 0.0, THRESHOLD_F * delta))
00150               {
00151                 // debug output
00152 #if LOG_LEVEL >= 5
00153                 if(_doubleProducts[8*seg + dp] != 0.0)
00154                   {
00155                     LOG(5, "Double product for (seg,dp) = (" << seg << ", " << dp << ") = " );
00156                     LOG(5, std::fabs(_doubleProducts[8*seg + dp]) << " is imprecise, reset to 0.0" );
00157                   }
00158 #endif 
00159 
00160 
00161                 _doubleProducts[8*seg + dp] = 0.0;
00162                 
00163               }
00164           }
00165       }
00166     
00167     _is_double_products_calculated = true;
00168   }
00169 
00177   bool TransformedTriangle::areDoubleProductsConsistent(const TriSegment seg) const
00178   {
00179     const double term1 = _doubleProducts[8*seg + C_YZ] * _doubleProducts[8*seg + C_XH];
00180     const double term2 = _doubleProducts[8*seg + C_ZX] * _doubleProducts[8*seg + C_YH];
00181     const double term3 = _doubleProducts[8*seg + C_XY] * _doubleProducts[8*seg + C_ZH];
00182 
00183     LOG(2, "for seg " << seg << " consistency " << term1 + term2 + term3 );
00184     LOG(2, "term1 :" << term1 << " term2 :" << term2 << " term3: " << term3 );
00185     
00186     const int num_zero = (term1 == 0.0 ? 1 : 0) + (term2 == 0.0 ? 1 : 0) + (term3 == 0.0 ? 1 : 0);
00187     const int num_neg = (term1 < 0.0 ? 1 : 0) + (term2 < 0.0 ? 1 : 0) + (term3 < 0.0 ? 1 : 0);
00188     const int num_pos = (term1 > 0.0 ? 1 : 0) + (term2 > 0.0 ? 1 : 0) + (term3 > 0.0 ? 1 : 0);
00189     
00190     assert( num_zero + num_neg + num_pos == 3 );
00191     
00192     // calculated geometry is inconsistent if we have one of the following cases
00193     // * one term zero and the other two of the same sign
00194     // * two terms zero
00195     // * all terms positive
00196     // * all terms negative
00197     if(((num_zero == 1 && num_neg != 1) || num_zero == 2 || (num_neg == 0 && num_zero != 3) || num_neg == 3 ))
00198       {
00199         LOG(4, "inconsistent dp found" );
00200       }
00201     return !((num_zero == 1 && num_neg != 1) || num_zero == 2 || (num_neg == 0 && num_zero != 3) || num_neg == 3 );
00202 
00203   }
00204 
00212   double TransformedTriangle::calculateDistanceCornerSegment(const TetraCorner corner, const TriSegment seg) const
00213   {
00214     // NB uses fact that TriSegment <=> TriCorner that is first point of segment (PQ <=> P)
00215     const TriCorner ptP_idx = TriCorner(seg);
00216     const TriCorner ptQ_idx = TriCorner( (seg + 1) % 3);
00217     
00218     const double ptP[3] = { _coords[5*ptP_idx], _coords[5*ptP_idx + 1], _coords[5*ptP_idx + 2]  };
00219     const double ptQ[3] = { _coords[5*ptQ_idx], _coords[5*ptQ_idx + 1], _coords[5*ptQ_idx + 2]  };
00220     
00221     // coordinates of corner
00222     const double ptTetCorner[3] = 
00223       { 
00224         COORDS_TET_CORNER[3*corner    ],
00225         COORDS_TET_CORNER[3*corner + 1],
00226         COORDS_TET_CORNER[3*corner + 2]
00227       };
00228     
00229     // dist^2 = ( PQ x CP )^2 / |PQ|^2 where C is the corner point
00230     
00231     // difference vectors
00232     const double diffPQ[3] = { ptQ[0] - ptP[0], ptQ[1] - ptP[1], ptQ[2] - ptP[2] };
00233     const double diffCornerP[3] = { ptP[0] - ptTetCorner[0], ptP[1] - ptTetCorner[1], ptP[2] - ptTetCorner[2] };
00234     
00235     // cross product of difference vectors
00236     double crossProd[3];
00237     cross(diffPQ, diffCornerP, crossProd);
00238     
00239     const double cross_squared = dot(crossProd, crossProd);
00240     const double norm_diffPQ_squared = dot(diffPQ, diffPQ);
00241     
00242     assert(norm_diffPQ_squared != 0.0);
00243     
00244     return cross_squared / norm_diffPQ_squared;
00245   }
00246 
00253   void TransformedTriangle::preCalculateTripleProducts(void)
00254   {
00255     if(_is_triple_products_calculated)
00256       {
00257         return;
00258       }
00259 
00260     // find edge / row to use -> that whose edge makes the smallest angle to the triangle
00261     // use a map to find the minimum
00262     std::map<double, int> anglesForRows;
00263 
00264     LOG(4, "Precalculating triple products" );
00265     for(TetraCorner corner = O ; corner <= Z ; corner = TetraCorner(corner + 1))
00266       {
00267         LOG(6, "- Triple product for corner " << corner );
00268 
00269         for(int row = 1 ; row < 4 ; ++row) 
00270           {
00271             const DoubleProduct dp = DP_FOR_DETERMINANT_EXPANSION[3*corner + (row - 1)];
00272 
00273             // get edge by using correspondance between Double Product and Edge
00274             TetraEdge edge = TetraEdge(dp);
00275            
00276             // use edge only if it is surrounded by the surface
00277             if( _triangleSurroundsEdgeCache[edge] )
00278                 {
00279                   // -- calculate angle between edge and PQR
00280                   const double angle = calculateAngleEdgeTriangle(edge);
00281                   anglesForRows.insert(std::make_pair(angle, row));              
00282                 }
00283           }
00284        
00285         if(anglesForRows.size() != 0) // we have found a good row
00286           {
00287             const double minAngle = anglesForRows.begin()->first;
00288             const int minRow = anglesForRows.begin()->second;
00289 
00290             if(minAngle < TRIPLE_PRODUCT_ANGLE_THRESHOLD)
00291               {
00292                 _tripleProducts[corner] = calcTByDevelopingRow(corner, minRow, true);
00293               } 
00294             else 
00295               {
00296                 _tripleProducts[corner] = calcTByDevelopingRow(corner, minRow, false);
00297               }
00298             _validTP[corner] = true;
00299           }
00300         else
00301           {
00302             // this value will not be used
00303             // we set it to whatever
00304             LOG(6, "Triple product not calculated for corner " << corner );
00305             _tripleProducts[corner] = -3.14159265;
00306             _validTP[corner] = false;
00307 
00308           }
00309         anglesForRows.clear();
00310 
00311       }
00312 
00313     _is_triple_products_calculated = true;
00314   }
00315 
00322   double TransformedTriangle::calculateAngleEdgeTriangle(const TetraEdge edge) const
00323   {
00324     // find normal to PQR - cross PQ and PR
00325     const double pq[3] = 
00326       { 
00327         _coords[5*Q]     - _coords[5*P], 
00328         _coords[5*Q + 1] - _coords[5*P + 1],
00329         _coords[5*Q + 2] - _coords[5*P + 2]
00330       };
00331     
00332     const double pr[3] = 
00333       { 
00334         _coords[5*R]     - _coords[5*P], 
00335         _coords[5*R + 1] - _coords[5*P + 1],
00336         _coords[5*R + 2] - _coords[5*P + 2]
00337       };
00338     
00339     double normal[3];
00340 
00341     cross(pq, pr, normal);
00342     
00343     static const double EDGE_VECTORS[18] =
00344       {
00345         1.0, 0.0, 0.0, // OX
00346         0.0, 1.0, 0.0, // OY
00347         0.0, 0.0, 1.0, // OZ
00348         -1.0, 1.0, 0.0, // XY
00349         0.0,-1.0, 1.0, // YZ
00350         1.0, 0.0,-1.0 // ZX
00351       };
00352     
00353     const double edgeVec[3] = { 
00354       EDGE_VECTORS[3*edge],
00355       EDGE_VECTORS[3*edge + 1],
00356       EDGE_VECTORS[3*edge + 2],
00357     };
00358 
00359     //return angleBetweenVectors(normal, edgeVec);
00360 
00361     const double lenNormal = norm(normal);
00362     const double lenEdgeVec = norm(edgeVec);
00363     const double dotProd = dot(normal, edgeVec);
00364     
00365     //? is this more stable? -> no subtraction
00366     //    return asin( dotProd / ( lenNormal * lenEdgeVec ) ) + 3.141592625358979 / 2.0;
00367     double tmp=dotProd / ( lenNormal * lenEdgeVec );
00368     tmp=std::max(tmp,-1.);
00369     tmp=std::min(tmp,1.);
00370     return atan(1.0)*4.0 - acos(tmp);
00371 
00372   }
00373 
00387   double TransformedTriangle::calcTByDevelopingRow(const TetraCorner corner, const int row, const bool project) const
00388   {
00389     
00390     // OVERVIEW OF CALCULATION
00391     // --- sign before the determinant
00392     // the sign used depends on the sign in front of the triple product (Grandy, [15]),
00393     // and the convention used in the definition of the double products
00394   
00395     // the sign in front of the determinant gives the following schema for the three terms (I): 
00396     // corner/row    1    2   3
00397     // O (sign:+)    +    -   +
00398     // X (sign:-)    -    +   -
00399     // Y (sign:-)    -    +   -
00400     // Z (sign:-)    -    +   -
00401 
00402     // the 2x2 determinants are the following (C_AB <=> A_p*B_q - B_p*A_q, etc)
00403     // corner/row    1       2     3
00404     // O (sign:+)   C_YZ   C_XZ  C_XY
00405     // X (sign:-)   C_YZ   C_HZ  C_HY
00406     // Y (sign:-)   C_HZ   C_XZ  C_XH
00407     // Z (sign:-)   C_YH   C_XH  C_XY
00408 
00409     // these are represented in DP_FOR_DETERMINANT_EXPANSION,
00410     // except for the fact that certain double products are inversed (C_AB <-> C_BA)
00411 
00412     // comparing with the DOUBLE_PRODUCTS and using the fact that C_AB = -C_BA
00413     // we deduce the following schema (II) :
00414     // corner/row    1    2   3
00415     // O (sign:+)    +    -   +
00416     // X (sign:-)    +    -   -
00417     // Y (sign:-)    -    -   +
00418     // Z (sign:-)    +    +   +
00419 
00420     // comparing the two schemas (I) and (II) gives us the following matrix of signs,
00421     // putting 1 when the signs in (I) and (II) are equal and -1 when they are different :
00422 
00423     static const int SIGNS[12] = 
00424       {
00425         1, 1, 1,
00426         -1,-1, 1,
00427         1,-1,-1,
00428         -1, 1,-1
00429       };
00430 
00431     // find the offsets of the rows of the determinant
00432     const int offset = COORDINATE_FOR_DETERMINANT_EXPANSION[3 * corner + (row - 1)];
00433   
00434     const DoubleProduct dp = DP_FOR_DETERMINANT_EXPANSION[3 * corner + (row - 1)];
00435 
00436     const int sign = SIGNS[3 * corner + (row - 1)];
00437 
00438     const double cQR = calcStableC(QR, dp);
00439     const double cRP = calcStableC(RP, dp);
00440     const double cPQ = calcStableC(PQ, dp);
00441 
00442     double alpha = 0.0;
00443     
00444     // coordinate to use for projection (Grandy, [57]) with edges
00445     // OX, OY, OZ, XY, YZ, ZX in order : 
00446     // (y, z, x, h, h, h)
00447     // for the first three we could also use {2, 0, 1}
00448     static const int PROJECTION_COORDS[6] = { 1, 2, 0, 3, 3, 3 } ;
00449     
00450     const int coord = PROJECTION_COORDS[ dp ];
00451     
00452     // coordinate values for P, Q and R
00453     const double coordValues[3] = { _coords[5*P + coord], _coords[5*Q + coord], _coords[5*R + coord] };
00454     
00455     if(project)
00456       {
00457         // products coordinate values with corresponding double product
00458         const double coordDPProd[3] = { coordValues[0] * cQR, coordValues[1] * cRP, coordValues[2] * cPQ };
00459        
00460         const double sumDPProd = coordDPProd[0] + coordDPProd[1] + coordDPProd[2];
00461         const double sumDPProdSq = dot(coordDPProd, coordDPProd);
00462 
00463         //       alpha = sumDPProd / sumDPProdSq;
00464         alpha = (sumDPProdSq != 0.0) ? sumDPProd / sumDPProdSq : 0.0;
00465       }
00466 
00467     const double cQRbar = cQR * (1.0 - alpha * coordValues[0] * cQR);
00468     const double cRPbar = cRP * (1.0 - alpha * coordValues[1] * cRP);
00469     const double cPQbar = cPQ * (1.0 - alpha * coordValues[2] * cPQ);
00470 
00471     // check sign of barred products - should not change
00472     //    assert(cQRbar * cQR >= 0.0);
00473     //assert(cRPbar * cRP >= 0.0);
00474     //assert(cPQbar * cPQ >= 0.0);
00475 
00476     const double p_term = _coords[5*P + offset] * cQRbar;
00477     const double q_term = _coords[5*Q + offset] * cRPbar;
00478     const double r_term = _coords[5*R + offset] * cPQbar;
00479 
00480     // check that we are not so close to zero that numerical errors could take over, 
00481     // otherwise reset to zero (cf Grandy, p. 446)
00482 #ifdef FIXED_DELTA
00483     const double delta = FIXED_DELTA;
00484 #else
00485     const long double delta = MULT_PREC_F * (std::fabs(p_term) + std::fabs(q_term) + std::fabs(r_term));
00486 #endif
00487 
00488     if( epsilonEqual( p_term + q_term + r_term, 0.0, THRESHOLD_F * delta) )
00489       {
00490         LOG(4, "Reset imprecise triple product for corner " << corner << " to zero" ); 
00491         return 0.0;
00492       }
00493     else
00494       {
00495         // NB : using plus also for the middle term compensates for a double product
00496         // which is inversely ordered
00497         LOG(6, "Triple product for corner " << corner << ", row " << row << " = " << sign*( p_term + q_term + r_term ) );
00498         return sign*( p_term + q_term + r_term );
00499       }
00500 
00501   }
00502 
00503 }