Back to index

salome-med  6.5.0
TransformedTriangleIntersect.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 "VectorUtils.hxx"
00026 
00027 namespace INTERP_KERNEL
00028 {
00029 
00030   // ----------------------------------------------------------------------------------
00031   //  Correspondance tables describing all the variations of formulas. 
00032   // ----------------------------------------------------------------------------------
00033 
00037   const TransformedTriangle::DoubleProduct TransformedTriangle::DP_FOR_SEG_FACET_INTERSECTION[12] = 
00038     {
00039       C_XH, C_XY, C_ZX, // OYZ
00040       C_YH, C_YZ, C_XY, // OZX
00041       C_ZH, C_ZX, C_YZ, // OXY
00042       C_XH, C_YH, C_ZH  // XYZ
00043     };
00044 
00048   const double TransformedTriangle::SIGN_FOR_SEG_FACET_INTERSECTION[12] = 
00049     {
00050       1.0, 1.0, -1.0,
00051       1.0, 1.0, -1.0,
00052       1.0, 1.0, -1.0,
00053       1.0, 1.0,  1.0
00054     };
00055 
00059   const double TransformedTriangle::COORDS_TET_CORNER[12] = 
00060     {
00061       0.0, 0.0, 0.0,
00062       1.0, 0.0, 0.0,
00063       0.0, 1.0, 0.0,
00064       0.0, 0.0, 1.0
00065     };
00066 
00072   const int TransformedTriangle::DP_INDEX[12] =
00073     {
00074       // x, y, z
00075       -1, 1, 2,  // OYZ
00076       5, -1, 4,  // OZX
00077       7, 8, -1,  // OXY
00078       9, 10, 11  // XYZ
00079     };
00080 
00085   const TransformedTriangle::TetraCorner TransformedTriangle::CORNERS_FOR_EDGE[12] = 
00086     {
00087       O, X, // OX
00088       O, Y, // OY
00089       O, Z, // OZ
00090       X, Y, // XY
00091       Y, Z, // YZ
00092       Z, X  // ZX
00093     };
00094 
00098   const TransformedTriangle::TetraFacet TransformedTriangle::FACET_FOR_EDGE[12] =
00099     {
00100       OXY, OZX, // OX
00101       OXY, OYZ, // OY
00102       OZX, OYZ, // OZ
00103       OXY, XYZ, // XY
00104       OYZ, XYZ, // YZ
00105       OZX, XYZ  // ZX
00106     };
00107 
00111   const TransformedTriangle::TetraEdge TransformedTriangle::EDGES_FOR_CORNER[12] =
00112     {
00113       OX, OY, OZ, // O
00114       OX, XY, ZX, // X
00115       OY, XY, YZ, // Y
00116       OZ, ZX, YZ  // Z
00117     };
00118 
00124   const TransformedTriangle::DoubleProduct TransformedTriangle::DP_FOR_HALFSTRIP_INTERSECTION[12] =
00125     {
00126       C_10, C_01, C_ZH, C_10, // XY
00127       C_01, C_XY, C_XH, C_01, // YZ
00128       C_XY, C_10, C_YH, C_XY  // ZX
00129     };
00130   
00136   const TransformedTriangle::DoubleProduct TransformedTriangle::DP_SEGMENT_RAY_INTERSECTION[21] = 
00137     {
00138       C_10, C_YH, C_ZH, C_01, C_XY, C_YH, C_XY, // X
00139       C_01, C_XH, C_ZH, C_XY, C_10, C_ZH, C_10, // Y
00140       C_XY, C_YH, C_XH, C_10, C_01, C_XH, C_01  // Z
00141     };
00142 
00151   void TransformedTriangle::calcIntersectionPtSurfaceEdge(const TetraEdge edge, double* pt) const
00152   {
00153     assert(edge < H01);
00154 
00155     // barycentric interpolation between points A and B 
00156     // : (x,y,z)* = (1-alpha)*A + alpha*B where
00157     // alpha = t_A / (t_A - t_B)
00158     
00159     const TetraCorner corners[2] = 
00160       {
00161         CORNERS_FOR_EDGE[2*edge],
00162         CORNERS_FOR_EDGE[2*edge + 1]
00163       };
00164     
00165     // calculate alpha
00166     const double tA = calcStableT(corners[0]);
00167     const double tB = calcStableT(corners[1]);
00168     const double alpha = tA / (tA - tB);
00169 
00170     // calculate point
00171     LOG(4, "corner A = " << corners[0] << " corner B = " << corners[1] );
00172     LOG(4, "tA = " << tA << " tB = " << tB << " alpha= " << alpha );
00173     for(int i = 0; i < 3; ++i)
00174       {
00175 
00176         pt[i] = (1 - alpha) * COORDS_TET_CORNER[3*corners[0] + i] + 
00177           alpha * COORDS_TET_CORNER[3*corners[1] + i];
00178 #if 0
00179         pt[i] = (1 - alpha) * getCoordinateForTetCorner<corners[0], i>() + 
00180           alpha * getCoordinateForTetCorner<corners[0], i>();
00181 #endif
00182         LOG(6, pt[i] );
00183         assert(pt[i] >= 0.0);
00184         assert(pt[i] <= 1.0);
00185       }
00186   }
00187 
00198   void TransformedTriangle::calcIntersectionPtSegmentFacet(const TriSegment seg, const TetraFacet facet, double* pt) const
00199   {
00200     // calculate s
00201     double s = 0.0;
00202     for(int i = 0; i < 3; ++i)
00203       {
00204         const DoubleProduct dp = DP_FOR_SEG_FACET_INTERSECTION[3*facet + i];
00205         const double sign = SIGN_FOR_SEG_FACET_INTERSECTION[3*facet + i];
00206         s -= sign * calcStableC(seg, dp);
00207       }
00208 
00209     assert(s != 0.0);
00210 
00211     // calculate coordinates of intersection point
00212     for(int i = 0 ; i < 3; ++i)
00213       {
00214         const int dpIdx = DP_INDEX[3*facet + i];
00215        
00216         if(dpIdx < 0)
00217           {
00218             pt[i] = 0.0;
00219           }
00220         else
00221           {
00222             const DoubleProduct dp = DP_FOR_SEG_FACET_INTERSECTION[dpIdx];
00223             const double sign = SIGN_FOR_SEG_FACET_INTERSECTION[dpIdx];
00224             pt[i] = -( sign * calcStableC(seg, dp) ) / s;
00225 
00226             LOG(4, "SegmentFacetIntPtCalc : pt[" << i << "] = " << pt[i]  );
00227             LOG(4, "c(" << seg << ", " << dp << ") = " <<  sign * calcStableC(seg, dp) );
00228             assert(pt[i] >= 0.0); 
00229             assert(pt[i] <= 1.0);
00230           }
00231       }
00232   
00233   }
00234 
00242   bool TransformedTriangle::testSegmentEdgeIntersection(const TriSegment seg, const TetraEdge edge) const
00243   {
00244       {
00245         // check condition that the double products for one of the two
00246         // facets adjacent to the edge has a positive product
00247         bool isFacetCondVerified = false;
00248         TetraFacet facet[2];
00249         for(int i = 0 ; i < 2 ; ++i) 
00250           {
00251             facet[i] = FACET_FOR_EDGE[2*edge + i];
00252            
00253             // find the two c-values -> the two for the other edges of the facet
00254             int idx1 = 0 ; 
00255             int idx2 = 1;
00256             DoubleProduct dp1 = DP_FOR_SEG_FACET_INTERSECTION[3*facet[i] + idx1];
00257             DoubleProduct dp2 = DP_FOR_SEG_FACET_INTERSECTION[3*facet[i] + idx2];
00258            
00259             if(dp1 == DoubleProduct( edge ))
00260               {
00261                 idx1 = 2;
00262                 dp1 = DP_FOR_SEG_FACET_INTERSECTION[3*facet[i] + idx1];
00263               }
00264             else if(dp2 == DoubleProduct( edge ))
00265               {
00266                 idx2 = 2;
00267                 dp2 = DP_FOR_SEG_FACET_INTERSECTION[3*facet[i] + idx2];
00268               }
00269            
00270             const double c1 = SIGN_FOR_SEG_FACET_INTERSECTION[3*facet[i] + idx1]*calcStableC(seg, dp1);
00271             const double c2 = SIGN_FOR_SEG_FACET_INTERSECTION[3*facet[i] + idx2]*calcStableC(seg, dp2);
00272 
00273             //isFacetCondVerified = isFacetCondVerified || c1*c2 > 0.0;
00274             if(c1*c2 > 0.0)
00275               {
00276                 isFacetCondVerified = true;
00277               }
00278           }
00279 
00280         if(!isFacetCondVerified)
00281           {
00282             return false;
00283           }
00284         else
00285           {
00286             return testSegmentIntersectsFacet(seg, facet[0]) || testSegmentIntersectsFacet(seg, facet[1]);
00287           }
00288       }
00289   }
00290     
00301   void TransformedTriangle::calcIntersectionPtSegmentEdge(const TriSegment seg, const TetraEdge edge, double* pt) const 
00302   {
00303     assert(edge < H01);
00304 
00305     // get the two facets associated with the edge
00306     static const TetraFacet FACETS_FOR_EDGE[12] =
00307       {
00308         OXY, OZX, // OX
00309         OXY, OYZ, // OY
00310         OZX, OYZ, // OZ
00311         OXY, XYZ, // XY
00312         OYZ, XYZ, // YZ
00313         OZX, XYZ  // ZX
00314       };
00315 
00316     const TetraFacet facets[2] = 
00317       {
00318         FACETS_FOR_EDGE[2*edge],
00319         FACETS_FOR_EDGE[2*edge + 1]
00320       };
00321     
00322     // calculate s for the two edges
00323     double s[2];
00324     for(int i = 0; i < 2; ++i)
00325       {
00326         s[i] = 0.0;
00327         for(int j = 0; j < 3; ++j)
00328           {
00329             const DoubleProduct dp = DP_FOR_SEG_FACET_INTERSECTION[3*facets[i] + j];
00330             const double sign = SIGN_FOR_SEG_FACET_INTERSECTION[3*facets[i] + j];
00331             s[i] += sign * calcStableC(seg, dp);
00332           }
00333       }
00334 
00335     // calculate denominator
00336     const double denominator = s[0]*s[0] + s[1]*s[1];
00337 
00338     // calculate intersection point
00339     for(int i = 0; i < 3; ++i)
00340       {
00341         // calculate double product values for the two faces
00342         double c[2];
00343         for(int j = 0 ; j < 2; ++j)
00344           {
00345             const int dpIdx = DP_INDEX[3*facets[j] + i];
00346             const DoubleProduct dp = DP_FOR_SEG_FACET_INTERSECTION[dpIdx];
00347             const double sign = SIGN_FOR_SEG_FACET_INTERSECTION[dpIdx];
00348             c[j] = dpIdx < 0.0 ? 0.0 : sign * calcStableC(seg, dp);
00349           }
00350        
00351         // pt[i] = (c1*s1 + c2*s2) / (s1^2 + s2^2)
00352 
00353         pt[i] = (c[0] * s[0] + c[1] * s[1]) / denominator;
00354        
00355         // strange bug with -O2 enabled : assertion fails when we don't have the following
00356         // trace - line
00357         //std::cout << "pt[i] = " << pt[i] << std::endl;
00358         //assert(pt[i] >= 0.0); // check we are in tetraeder
00359         //assert(pt[i] <= 1.0);
00360        
00361       }
00362   }
00363 
00364     
00373   bool TransformedTriangle::testSegmentCornerIntersection(const TriSegment seg, const TetraCorner corner) const 
00374   {
00375     
00376 
00377     // facets meeting at a given corner
00378     static const TetraFacet FACETS_FOR_CORNER[12] =
00379       {
00380         OXY, OYZ, OZX, // O
00381         OZX, OXY, XYZ, // X
00382         OYZ, XYZ, OXY, // Y
00383         OZX, XYZ, OYZ  // Z
00384       };
00385     
00386     // check segment intersect a facet
00387     for(int i = 0 ; i < 3 ; ++i)
00388       {
00389         const TetraFacet facet = FACETS_FOR_CORNER[3*corner + i];
00390         if(testSegmentIntersectsFacet(seg, facet))
00391           {
00392             return true;
00393           }
00394       }
00395     
00396     return false;
00397   }
00398 
00407   bool TransformedTriangle::testSegmentHalfstripIntersection(const TriSegment seg, const TetraEdge edge)
00408   {
00409     // get right index here to avoid "filling out" array
00410     const int edgeIndex = static_cast<int>(edge) - 3;
00411     
00412     // double products used in test
00413     // products 1 and 2 for each edge -> first condition in Grandy [30]
00414     // products 3 and 4 for each edge -> third condition
00415     // NB : some uncertainty whether these last are correct
00416     // DP_FOR_HALFSTRIP_INTERSECTION
00417     
00418     // facets to use in second condition (S_m)
00419     static const TetraFacet FACET_FOR_HALFSTRIP_INTERSECTION[3] = 
00420       {
00421         NO_TET_FACET, // XY -> special case : test with plane H = 0
00422         OYZ, // YZ
00423         OZX  // ZX
00424       };
00425 
00426     const double cVals[4] = 
00427       {
00428         calcStableC(seg, DP_FOR_HALFSTRIP_INTERSECTION[4*edgeIndex]),
00429         calcStableC(seg, DP_FOR_HALFSTRIP_INTERSECTION[4*edgeIndex + 1]),
00430         calcStableC(seg, DP_FOR_HALFSTRIP_INTERSECTION[4*edgeIndex + 2]),
00431         calcStableC(seg, DP_FOR_HALFSTRIP_INTERSECTION[4*edgeIndex + 3])
00432       };
00433     
00434     const TetraFacet facet =  FACET_FOR_HALFSTRIP_INTERSECTION[edgeIndex];
00435 
00436     
00437     // special case : facet H = 0
00438     const bool cond2 = (facet == NO_TET_FACET) ? testSegmentIntersectsHPlane(seg) : testSegmentIntersectsFacet(seg, facet);
00439     LOG(4, "Halfstrip tests (" << seg << ", " << edge << ") : " << (cVals[0]*cVals[1] < 0.0) << ", " << cond2 << ", " << (cVals[2]*cVals[3] > 0.0) );
00440     LOG(4, "c2 = " << cVals[2] << ", c3 = " << cVals[3] ); 
00441   
00442     return (cVals[0]*cVals[1] < 0.0) && cond2 && (cVals[2]*cVals[3] > 0.0);
00443   }
00444 
00455   void TransformedTriangle::calcIntersectionPtSegmentHalfstrip(const TriSegment seg, const TetraEdge edge, double* pt) const
00456   {
00457     assert(edge > OZ);
00458     assert(edge < H01);
00459 
00460     // get right index here to avoid "filling out" array
00461     const int edgeIndex = static_cast<int>(edge) - 3;
00462     assert(edgeIndex >= 0);
00463     assert(edgeIndex < 3);
00464     
00465     // Barycentric interpolation on the edge
00466     // for edge AB : (x,y,z)* = (1-alpha) * A + alpha * B
00467     // where alpha = cB / (cB - cA)
00468 
00469     const double cA = calcStableC(seg, DP_FOR_HALFSTRIP_INTERSECTION[4*edgeIndex]);
00470     const double cB = calcStableC(seg, DP_FOR_HALFSTRIP_INTERSECTION[4*edgeIndex + 1]);
00471     assert(cA != cB);
00472     
00473     const double alpha = cA / (cA - cB);
00474     
00475     for(int i = 0; i < 3; ++i)
00476       {
00477         const TetraCorner corners[2] = 
00478           {
00479             CORNERS_FOR_EDGE[2*edge],
00480             CORNERS_FOR_EDGE[2*edge + 1]
00481           };
00482 
00483         const double cornerCoords[2] = 
00484           {
00485             COORDS_TET_CORNER[3*corners[0] + i],
00486             COORDS_TET_CORNER[3*corners[1] + i]
00487           };
00488 
00489         pt[i] = (1 - alpha) * cornerCoords[0] + alpha * cornerCoords[1];
00490         LOG(6, pt[i] );
00491         assert(pt[i] >= 0.0);
00492         assert(pt[i] <= 1.0);
00493       }
00494     assert(epsilonEqualRelative(pt[0] + pt[1] + pt[2], 1.0));
00495   }
00496     
00506   bool TransformedTriangle::testSegmentRayIntersection(const TriSegment seg, const TetraCorner corner) const
00507   {
00508     assert(corner == X || corner == Y || corner == Z);
00509     LOG(4, "Testing seg - ray intersection for seg = " << seg << ", corner = " << corner );
00510 
00511     // readjust index since O is not used
00512     const int cornerIdx = static_cast<int>(corner) - 1;
00513 
00514     // facets to use
00515     //? not sure this is correct
00516     static const TetraFacet FIRST_FACET_SEGMENT_RAY_INTERSECTION[3] = 
00517       {
00518         OZX, // X
00519         OYZ, // Y
00520         OZX, // Z
00521       };
00522 
00523     // cond 2
00524     const bool cond21 = testSegmentIntersectsFacet(seg, FIRST_FACET_SEGMENT_RAY_INTERSECTION[cornerIdx]);
00525     const bool cond22  = (corner == Z) ? testSegmentIntersectsFacet(seg, OYZ) : testSegmentIntersectsHPlane(seg);
00526     
00527     if(!(cond21 || cond22))
00528       {
00529         LOG(4, "SR fails at cond 2 : cond21 = " << cond21 << ", cond22 = " << cond22 );
00530         return false;
00531       }
00532     
00533     // cond 3 
00534     const double cVals[6] = 
00535       {
00536         calcStableC(seg, DP_SEGMENT_RAY_INTERSECTION[7*cornerIdx + 1]),
00537         calcStableC(seg, DP_SEGMENT_RAY_INTERSECTION[7*cornerIdx + 2]),
00538         calcStableC(seg, DP_SEGMENT_RAY_INTERSECTION[7*cornerIdx + 3]),
00539         calcStableC(seg, DP_SEGMENT_RAY_INTERSECTION[7*cornerIdx + 4]),
00540         calcStableC(seg, DP_SEGMENT_RAY_INTERSECTION[7*cornerIdx + 5]),
00541         calcStableC(seg, DP_SEGMENT_RAY_INTERSECTION[7*cornerIdx + 6]),
00542       };
00543     
00544     // cond. 3
00545     if(( (cVals[0] + cVals[1])*(cVals[2] - cVals[3]) - cVals[4]*cVals[5] ) >= 0.0)
00546       {
00547         LOG(4, "SR fails at cond 3 : " << (cVals[0] + cVals[1])*(cVals[2] - cVals[3]) - cVals[4]*cVals[5]  );
00548       }
00549     return ( (cVals[0] + cVals[1])*(cVals[2] - cVals[3]) - cVals[4]*cVals[5] ) < 0.0;
00550     
00551   } 
00552 
00553   // /////////////////////////////////////////////////////////////////////////////////
00554   //  Utility methods used in intersection tests                       ///////////////
00555   // /////////////////////////////////////////////////////////////////////////////////
00563   bool TransformedTriangle::testTriangleSurroundsEdge(const TetraEdge edge) const
00564   {
00565     // NB DoubleProduct enum corresponds to TetraEdge enum according to Grandy, table III
00566     // so we can use the edge directly
00567     
00568     const double cPQ = calcStableC(PQ, DoubleProduct(edge));
00569     const double cQR = calcStableC(QR, DoubleProduct(edge));
00570     const double cRP = calcStableC(RP, DoubleProduct(edge));
00571 
00572     LOG(5, "TriangleSurroundsEdge : edge = " << edge << " c = [" << cPQ << ", " << cQR << ", " << cRP << "]" );
00573 
00574     // if two or more c-values are zero we disallow x-edge intersection
00575     // Grandy, p.446
00576     const int numZeros = (cPQ == 0.0 ? 1 : 0) + (cQR == 0.0 ? 1 : 0) + (cRP == 0.0 ? 1 : 0);
00577     
00578     if(numZeros >= 2 ) 
00579       {
00580         LOG(5, "TriangleSurroundsEdge test fails due to too many 0 dp" ); 
00581       }
00582 
00583     return (cPQ*cQR >= 0.0) && (cQR*cRP >= 0.0) && (cRP*cPQ >= 0.0) && numZeros < 2;
00584   }
00585 
00586 } // NAMESPACE