1 // Copyright (C) 2007-2024 CEA, EDF
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, or (at your option) any later version.
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 #include "TransformedTriangle.hxx"
29 #include "VectorUtils.hxx"
31 namespace INTERP_KERNEL
34 // ----------------------------------------------------------------------------------
36 // ----------------------------------------------------------------------------------
38 /// Table with first coordinate (a) used to calculate double product c^pq_ab = p_a * q_b - p_b * q_a (index to be used : DoubleProduct)
39 const int TransformedTriangle::DP_OFFSET_1[8] = {1, 2, 0, 2, 0, 1, 4, 1};
41 /// Table with second coordinate (b) used to calculate double product c^pq_ab = p_a * q_b - p_b * q_a (index to be used : DoubleProduct)
42 const int TransformedTriangle::DP_OFFSET_2[8] = {2, 0, 1, 3, 3, 3, 0, 4};
44 /// Coordinates used to calculate triple products by the expanding one of the three rows of the determinant (index to be used : 3*Corner + row)
45 const int TransformedTriangle::COORDINATE_FOR_DETERMINANT_EXPANSION[12] =
54 /// Double products used to calculate triple products by expanding one of the three rows of the determinant (index to be used : 3*Corner + row)
55 const TransformedTriangle::DoubleProduct TransformedTriangle::DP_FOR_DETERMINANT_EXPANSION[12] =
58 C_YZ, C_ZX, C_XY, // O
59 C_YZ, C_ZH, C_YH, // X
60 C_ZH, C_ZX, C_XH, // Y
64 /// The machine epsilon, used in precision corrections
65 const double TransformedTriangle::MACH_EPS = std::numeric_limits<double>::epsilon();
67 /// 4.0 * the machine epsilon, represents the precision of multiplication when performing corrections corrections ( f in Grandy )
68 const double TransformedTriangle::MULT_PREC_F = 4.0 * TransformedTriangle::MACH_EPS;
70 /// Threshold for resetting double and triple products to zero; ( F / f in Grandy )
71 const double TransformedTriangle::THRESHOLD_F = 100.0;
73 /// Threshold for what is considered a small enough angle to warrant correction of triple products by Grandy, [57]
74 const double TransformedTriangle::TRIPLE_PRODUCT_ANGLE_THRESHOLD = 0.1;
77 // Handle cases where one of the segment (or all) is (almost) in XYZ plane.
78 // We follow Grandy's suggestion and perturb slightly to have exactly h=0 for the segment (Grandy p.447)
79 // Note that if PQR is == to the upper facet of the unit tetra (XYZ), the tetra-corner-inclusion test should take it in,
80 // thanks to Grandy [21] and the fact that S_x test is "<=0" (not <0)
81 // After that, we also snap P,Q,R to the corners if they're very close.
82 void TransformedTriangle::handleDegenerateCases()
84 static const TriCorner PT_SEG_MAP[] = {
90 const double eps = THRESHOLD_F*TransformedTriangle::MULT_PREC_F;
91 for (TriSegment seg = PQ; seg <= RP; seg = TriSegment(seg+1))
93 // Is h coordinate for both end of segment small enough?
94 int pt1 = PT_SEG_MAP[2*seg], pt2 = PT_SEG_MAP[2*seg+1];
95 if (fabs(_coords[5*pt1+3]) < eps && fabs(_coords[5*pt2+3]) < eps)
97 // If so, perturb x,y and z to reset h to exactly zero.
98 for (auto pt: {pt1, pt2}) // thx C++17
100 const double correc = _coords[pt*5+3]/3.; // this should be really small!
101 _coords[pt*5+0] += correc;
102 _coords[pt*5+1] += correc;
103 _coords[pt*5+2] += correc;
104 // And then, if x,y or z very close to 0 or 1, snap exactly to tetra corner:
105 for(int d=0; d < 3; d++)
107 if (fabs(_coords[5*pt+d]) < eps) _coords[5*pt+d] = 0.0;
108 if (fabs(_coords[5*pt+d]-1) < eps) _coords[5*pt+d] = 1.0;
110 _coords[pt*5+3] = 0.0;
116 // ----------------------------------------------------------------------------------
117 // Double and triple product calculations
118 // ----------------------------------------------------------------------------------
121 * Pre-calculates all double products for this triangle, and stores
122 * them internally. This method makes compensation for precision errors,
123 * and it is thus the "stable" double products that are stored.
126 void TransformedTriangle::preCalculateDoubleProducts()
128 if(_is_double_products_calculated)
131 // -- calculate all unstable double products -- store in _doubleProducts
132 for(TriSegment seg = PQ ; seg <= RP ; seg = TriSegment(seg + 1))
134 for(DoubleProduct dp = C_YZ ; dp <= C_10 ; dp = DoubleProduct(dp + 1))
136 const int idx = 8*seg + dp;
137 _doubleProducts[idx] = calcUnstableC(seg, dp, _deltas[idx]);
141 std::map<double, TetraCorner> distances;
143 // -- (1) for each segment : check that double products satisfy Grandy, [46]
144 // -- and make corrections if not
145 for(TriSegment seg = PQ ; seg <= RP ; seg = TriSegment(seg + 1))
147 if(!areDoubleProductsConsistent(seg))
149 LOG(4, "inconsistent! ");
150 for(TetraCorner corner = O ; corner <= Z ; corner = TetraCorner(corner + 1))
152 // calculate distance corner - segment axis
153 const double dist = calculateDistanceCornerSegment(corner, seg);
154 distances.insert( std::make_pair( dist, corner ) );
157 // first element -> minimum distance (because map is sorted)
158 const TetraCorner minCorner = distances.begin()->second;
159 resetDoubleProducts(seg, minCorner);
164 // -- (2) check that each double product satisfies Grandy, [47], else set to 0
165 for(TriSegment seg = PQ ; seg <= RP ; seg = TriSegment(seg + 1))
167 for(DoubleProduct dp = C_YZ ; dp <= C_10 ; dp = DoubleProduct(dp + 1))
169 const int idx = 8*seg+dp;
171 if( epsilonEqual(_doubleProducts[idx], 0.0, THRESHOLD_F * MULT_PREC_F * _deltas[idx]))
175 if(_doubleProducts[8*seg + dp] != 0.0)
177 LOG(5, "Double product for (seg,dp) = (" << seg << ", " << dp << ") = " );
178 LOG(5, std::fabs(_doubleProducts[8*seg + dp]) << " is imprecise, reset to 0.0" );
182 _doubleProducts[idx] = 0.0;
187 _is_double_products_calculated = true;
191 * Checks if the double products for a given segment are consistent, as defined by
194 * @param seg Segment for which to check consistency of double products
195 * @return true if the double products are consistent, false if not
197 bool TransformedTriangle::areDoubleProductsConsistent(const TriSegment seg) const
199 // Careful! Here doubleProducts have not yet been corrected for roundoff errors!
200 // So we need to epsilon-adjust to correctly identify zeros:
201 static const DoubleProduct DP_LST[6] = {C_YZ, C_XH,
205 for (int i = 0; i < 6; i++)
207 const double dp = _doubleProducts[8*seg + DP_LST[i]];
211 const double term1 = dps[0] * dps[1];
212 const double term2 = dps[2] * dps[3];
213 const double term3 = dps[4] * dps[5];
215 LOG(2, "for seg " << seg << " consistency " << term1 + term2 + term3 );
216 LOG(2, "term1 :" << term1 << " term2 :" << term2 << " term3: " << term3 );
218 // Test for "== 0.0" here is OK since doubleProduct has been fixed for rounding to zero already.
219 const int num_zero = (term1 == 0.0 ? 1 : 0) + (term2 == 0.0 ? 1 : 0) + (term3 == 0.0 ? 1 : 0);
220 const int num_neg = (term1 < 0.0 ? 1 : 0) + (term2 < 0.0 ? 1 : 0) + (term3 < 0.0 ? 1 : 0);
221 const int num_pos = (term1 > 0.0 ? 1 : 0) + (term2 > 0.0 ? 1 : 0) + (term3 > 0.0 ? 1 : 0);
223 assert( num_zero + num_neg + num_pos == 3 );
225 // Calculated geometry is inconsistent if we have one of the following cases
226 // * one term zero and the other two of the same sign
228 // * all terms positive
229 // * all terms negative
230 const bool inconsist = (num_zero == 1 && num_neg != 1) ||
232 (num_neg == 0 && num_zero != 3) ||
235 LOG(4, "inconsistent dp found" );
241 * Calculate the shortest distance between a tetrahedron corner and a triangle segment.
243 * @param corner corner of the tetrahedron
244 * @param seg segment of the triangle
245 * @return shortest distance from the corner to the segment
247 double TransformedTriangle::calculateDistanceCornerSegment(const TetraCorner corner, const TriSegment seg) const
249 // NB uses fact that TriSegment <=> TriCorner that is first point of segment (PQ <=> P)
250 const TriCorner ptP_idx = TriCorner(seg);
251 const TriCorner ptQ_idx = TriCorner( (seg + 1) % 3);
253 const double ptP[3] = { _coords[5*ptP_idx], _coords[5*ptP_idx + 1], _coords[5*ptP_idx + 2] };
254 const double ptQ[3] = { _coords[5*ptQ_idx], _coords[5*ptQ_idx + 1], _coords[5*ptQ_idx + 2] };
256 // coordinates of corner
257 const double ptTetCorner[3] =
259 COORDS_TET_CORNER[3*corner ],
260 COORDS_TET_CORNER[3*corner + 1],
261 COORDS_TET_CORNER[3*corner + 2]
264 // dist^2 = ( PQ x CP )^2 / |PQ|^2 where C is the corner point
266 // difference vectors
267 const double diffPQ[3] = { ptQ[0] - ptP[0], ptQ[1] - ptP[1], ptQ[2] - ptP[2] };
268 const double diffCornerP[3] = { ptP[0] - ptTetCorner[0], ptP[1] - ptTetCorner[1], ptP[2] - ptTetCorner[2] };
270 // cross product of difference vectors
272 cross(diffPQ, diffCornerP, crossProd);
274 const double cross_squared = dot(crossProd, crossProd);
275 const double norm_diffPQ_squared = dot(diffPQ, diffPQ);
277 assert(norm_diffPQ_squared != 0.0);
279 return cross_squared / norm_diffPQ_squared;
283 * Pre-calculates all triple products for the tetrahedron with respect to
284 * this triangle, and stores them internally. This method takes into account
285 * the problem of errors due to cancellation.
288 void TransformedTriangle::preCalculateTripleProducts()
290 if(_is_triple_products_calculated)
293 // find edge / row to use -> that whose edge makes the smallest angle to the triangle
294 // use a map to find the minimum
295 std::map<double, int> anglesForRows;
297 LOG(4, "Precalculating triple products" );
298 for(TetraCorner corner = O ; corner <= Z ; corner = TetraCorner(corner + 1))
300 LOG(6, "- Triple product for corner " << corner );
302 for(int row = 1 ; row < 4 ; ++row)
304 const DoubleProduct dp = DP_FOR_DETERMINANT_EXPANSION[3*corner + (row - 1)];
306 // get edge by using correspondence between Double Product and Edge
307 TetraEdge edge = TetraEdge(dp);
309 // use edge only if it is surrounded by the surface
310 if( _triangleSurroundsEdgeCache[edge] )
312 // -- calculate angle between edge and PQR
313 const double angle = calculateAngleEdgeTriangle(edge);
314 anglesForRows.insert(std::make_pair(angle, row));
318 if(anglesForRows.size() != 0) // we have found a good row
320 const double minAngle = anglesForRows.begin()->first;
321 const int minRow = anglesForRows.begin()->second;
323 if(minAngle < TRIPLE_PRODUCT_ANGLE_THRESHOLD)
324 _tripleProducts[corner] = calcTByDevelopingRow(corner, minRow, true);
326 _tripleProducts[corner] = calcTByDevelopingRow(corner, minRow, false);
328 _validTP[corner] = true;
332 // this value will not be used - we set it to whatever
333 LOG(6, "Triple product not calculated for corner " << corner );
334 _tripleProducts[corner] = -3.14159265;
335 _validTP[corner] = false;
337 anglesForRows.clear();
339 _is_triple_products_calculated = true;
343 * Calculates the angle between an edge of the tetrahedron and the triangle
345 * @param edge edge of the tetrahedron
346 * @return angle between triangle and edge
348 double TransformedTriangle::calculateAngleEdgeTriangle(const TetraEdge edge) const
350 // find normal to PQR - cross PQ and PR
353 _coords[5*Q] - _coords[5*P],
354 _coords[5*Q + 1] - _coords[5*P + 1],
355 _coords[5*Q + 2] - _coords[5*P + 2]
360 _coords[5*R] - _coords[5*P],
361 _coords[5*R + 1] - _coords[5*P + 1],
362 _coords[5*R + 2] - _coords[5*P + 2]
367 cross(pq, pr, normal);
369 static const double EDGE_VECTORS[18] =
374 -1.0, 1.0, 0.0, // XY
379 const double edgeVec[3] = {
380 EDGE_VECTORS[3*edge],
381 EDGE_VECTORS[3*edge + 1],
382 EDGE_VECTORS[3*edge + 2],
385 //return angleBetweenVectors(normal, edgeVec);
387 const double lenNormal = norm(normal);
388 const double lenEdgeVec = norm(edgeVec);
389 const double dotProd = dot(normal, edgeVec);
391 //? is this more stable? -> no subtraction
392 // return asin( dotProd / ( lenNormal * lenEdgeVec ) ) + 3.141592625358979 / 2.0;
393 double tmp=dotProd / ( lenNormal * lenEdgeVec );
394 tmp=std::max(tmp,-1.);
395 tmp=std::min(tmp,1.);
396 return atan(1.0)*4.0 - acos(tmp);
401 * Calculates triple product associated with the given corner of tetrahedron, developing
402 * the determinant by the given row. The triple product gives the signed volume of
403 * the tetrahedron between this corner and the triangle PQR. If the flag project is true,
404 * one coordinate is projected out in order to eliminate errors in the intersection point
405 * calculation due to cancellation.
407 * Consistency with the double product computation and potential cancellation is also done here.
410 * @pre double products have already been calculated
411 * @param corner corner for which the triple product is calculated
412 * @param row row (1 <= row <= 3) used to calculate the determinant
413 * @param project indicates whether or not to perform projection as inidicated in Grandy, p.446
414 * @return triple product associated with corner (see Grandy, [50]-[52])
416 double TransformedTriangle::calcTByDevelopingRow(const TetraCorner corner, const int row, const bool project) const
419 // OVERVIEW OF CALCULATION
420 // --- sign before the determinant
421 // the sign used depends on the sign in front of the triple product (Grandy, [15]),
422 // and the convention used in the definition of the double products
424 // the sign in front of the determinant gives the following schema for the three terms (I):
431 // the 2x2 determinants are the following (C_AB <=> A_p*B_q - B_p*A_q, etc)
433 // O (sign:+) C_YZ C_XZ C_XY
434 // X (sign:-) C_YZ C_HZ C_HY
435 // Y (sign:-) C_HZ C_XZ C_XH
436 // Z (sign:-) C_YH C_XH C_XY
438 // these are represented in DP_FOR_DETERMINANT_EXPANSION,
439 // except for the fact that certain double products are inversed (C_AB <-> C_BA)
441 // comparing with the DOUBLE_PRODUCTS and using the fact that C_AB = -C_BA
442 // we deduce the following schema (II) :
449 // comparing the two schemas (I) and (II) gives us the following matrix of signs,
450 // putting 1 when the signs in (I) and (II) are equal and -1 when they are different :
452 static const int SIGNS[12] =
460 // find the offsets of the rows of the determinant
461 const int offset = COORDINATE_FOR_DETERMINANT_EXPANSION[3 * corner + (row - 1)];
463 const DoubleProduct dp = DP_FOR_DETERMINANT_EXPANSION[3 * corner + (row - 1)];
465 const int sign = SIGNS[3 * corner + (row - 1)];
467 const double cQR = calcStableC(QR, dp);
468 const double cRP = calcStableC(RP, dp);
469 const double cPQ = calcStableC(PQ, dp);
473 // coordinate to use for projection (Grandy, [57]) with edges
474 // OX, OY, OZ, XY, YZ, ZX in order :
475 // (y, z, x, h, h, h)
476 // for the first three we could also use {2, 0, 1}
477 static const int PROJECTION_COORDS[6] = { 1, 2, 0, 3, 3, 3 } ;
479 const int coord = PROJECTION_COORDS[ dp ];
481 // coordinate values for P, Q and R
482 const double coordValues[3] = { _coords[5*P + coord], _coords[5*Q + coord], _coords[5*R + coord] };
486 // products coordinate values with corresponding double product
487 const double coordDPProd[3] = { coordValues[0] * cQR, coordValues[1] * cRP, coordValues[2] * cPQ };
489 const double sumDPProd = coordDPProd[0] + coordDPProd[1] + coordDPProd[2];
490 const double sumDPProdSq = dot(coordDPProd, coordDPProd);
492 // alpha = sumDPProd / sumDPProdSq;
493 alpha = (sumDPProdSq != 0.0) ? sumDPProd / sumDPProdSq : 0.0;
496 const double cQRbar = cQR * (1.0 - alpha * coordValues[0] * cQR);
497 const double cRPbar = cRP * (1.0 - alpha * coordValues[1] * cRP);
498 const double cPQbar = cPQ * (1.0 - alpha * coordValues[2] * cPQ);
500 // [ABN] Triple product cancellation logic:
501 // This part is not well described in Grandy (end of p.446) :
502 // "We use a method analogous to (47) to remove imprecise triple products,..."
504 // Our algo for cancelling a triple product:
505 // - retrieve the deltas associated with each DP involved (because a DP itself is a sum of two terms - see [42]
506 // - multiply them by the coordinate coming from the determinant expansion
507 // - and finally sum the 3 corresponding terms of the developement
509 // Using directly the DP (as was done before here) leads to issues, since this DP might have been cancelled
510 // already earlier on, and we lost the delta information -> doing this, we risk not cancelling the triple prod
511 // when we should have.
512 const double cQRbar_delta = _deltas[8*QR + dp],
513 cRPbar_delta = _deltas[8*RP + dp],
514 cPQbar_delta = _deltas[8*PQ + dp];
516 // check sign of barred products - should not change
517 // assert(cQRbar * cQR >= 0.0);
518 //assert(cRPbar * cRP >= 0.0);
519 //assert(cPQbar * cPQ >= 0.0);
521 const double p_term = _coords[5*P + offset] * cQRbar;
522 const double q_term = _coords[5*Q + offset] * cRPbar;
523 const double r_term = _coords[5*R + offset] * cPQbar;
525 const double p_delta = std::fabs(_coords[5*P + offset] * cQRbar_delta),
526 q_delta = std::fabs(_coords[5*Q + offset] * cRPbar_delta),
527 r_delta = std::fabs(_coords[5*R + offset] * cPQbar_delta);
529 const double delta = p_delta + q_delta + r_delta;
531 if( epsilonEqual( p_term + q_term + r_term, 0.0, THRESHOLD_F * MULT_PREC_F * delta) )
533 LOG(4, "Reset imprecise triple product for corner " << corner << " to zero" );
538 // NB : using plus also for the middle term compensates for a double product
539 // which is inversely ordered
540 LOG(6, "Triple product for corner " << corner << ", row " << row << " = " << sign*( p_term + q_term + r_term ) );
541 return sign*( p_term + q_term + r_term );