1 // Copyright (C) 2007-2008 CEA/DEN, EDF R&D
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.
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
19 #include "TransformedTriangle.hxx"
28 #include "VectorUtils.hxx"
30 namespace INTERP_KERNEL
33 // ----------------------------------------------------------------------------------
35 // ----------------------------------------------------------------------------------
37 /// 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)
38 const int TransformedTriangle::DP_OFFSET_1[8] = {1, 2, 0, 2, 0, 1, 4, 1};
40 /// 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)
41 const int TransformedTriangle::DP_OFFSET_2[8] = {2, 0, 1, 3, 3, 3, 0, 4};
43 /// Coordinates used to calculate triple products by the expanding one of the three rows of the determinant (index to be used : 3*Corner + row)
44 const int TransformedTriangle::COORDINATE_FOR_DETERMINANT_EXPANSION[12] =
53 /// Double products used to calculate triple products by expanding one of the three rows of the determinant (index to be used : 3*Corner + row)
54 const TransformedTriangle::DoubleProduct TransformedTriangle::DP_FOR_DETERMINANT_EXPANSION[12] =
57 C_YZ, C_ZX, C_XY, // O
58 C_YZ, C_ZH, C_YH, // X
59 C_ZH, C_ZX, C_XH, // Y
63 /// The machine epsilon, used in precision corrections
64 const long double TransformedTriangle::MACH_EPS = std::numeric_limits<double>::epsilon();
66 /// 4.0 * the machine epsilon, represents the precision of multiplication when performing corrections corrections ( f in Grandy )
67 const long double TransformedTriangle::MULT_PREC_F = 4.0 * TransformedTriangle::MACH_EPS;
69 /// Threshold for resetting double and triple products to zero; ( F / f in Grandy )
70 const long double TransformedTriangle::THRESHOLD_F = 500.0;
72 /// Threshold for what is considered a small enough angle to warrant correction of triple products by Grandy, [57]
73 const double TransformedTriangle::TRIPLE_PRODUCT_ANGLE_THRESHOLD = 0.1;
75 // ----------------------------------------------------------------------------------
76 // Double and triple product calculations
77 // ----------------------------------------------------------------------------------
80 * Pre-calculates all double products for this triangle, and stores
81 * them internally. This method makes compensation for precision errors,
82 * and it is thus the "stable" double products that are stored.
85 void TransformedTriangle::preCalculateDoubleProducts(void)
87 if(_is_double_products_calculated)
90 // -- calculate all unstable double products -- store in _doubleProducts
91 for(TriSegment seg = PQ ; seg <= RP ; seg = TriSegment(seg + 1))
93 for(DoubleProduct dp = C_YZ ; dp <= C_10 ; dp = DoubleProduct(dp + 1))
94 _doubleProducts[8*seg + dp] = calcUnstableC(seg, dp);
97 std::map<double, TetraCorner> distances;
99 // -- (1) for each segment : check that double products satisfy Grandy, [46]
100 // -- and make corrections if not
101 for(TriSegment seg = PQ ; seg <= RP ; seg = TriSegment(seg + 1))
103 if(!areDoubleProductsConsistent(seg))
105 LOG(4, "inconsistent! ");
106 for(TetraCorner corner = O ; corner <= Z ; corner = TetraCorner(corner + 1))
108 // calculate distance corner - segment axis
109 const double dist = calculateDistanceCornerSegment(corner, seg);
110 distances.insert( std::make_pair( dist, corner ) );
113 // first element -> minimum distance
114 const TetraCorner minCorner = distances.begin()->second;
115 resetDoubleProducts(seg, minCorner);
120 // -- (2) check that each double product statisfies Grandy, [47], else set to 0
121 for(TriSegment seg = PQ ; seg <= RP ; seg = TriSegment(seg + 1))
123 for(DoubleProduct dp = C_YZ ; dp <= C_10 ; dp = DoubleProduct(dp + 1))
125 // find the points of the triangle
126 // 0 -> P, 1 -> Q, 2 -> R
128 const int pt2 = (seg + 1) % 3;
131 const int off1 = DP_OFFSET_1[dp];
132 const int off2 = DP_OFFSET_2[dp];
134 const double term1 = _coords[5*pt1 + off1] * _coords[5*pt2 + off2];
135 const double term2 = _coords[5*pt1 + off2] * _coords[5*pt2 + off1];
137 const long double delta = MULT_PREC_F * ( std::fabs(term1) + std::fabs(term2) );
139 if( epsilonEqual(_doubleProducts[8*seg + dp], 0.0, THRESHOLD_F * delta))
143 if(_doubleProducts[8*seg + dp] != 0.0)
145 LOG(5, "Double product for (seg,dp) = (" << seg << ", " << dp << ") = " );
146 LOG(5, std::fabs(_doubleProducts[8*seg + dp]) << " is imprecise, reset to 0.0" );
151 _doubleProducts[8*seg + dp] = 0.0;
157 _is_double_products_calculated = true;
161 * Checks if the double products for a given segment are consistent, as defined by
164 * @param seg Segment for which to check consistency of double products
165 * @return true if the double products are consistent, false if not
167 bool TransformedTriangle::areDoubleProductsConsistent(const TriSegment seg) const
169 const double term1 = _doubleProducts[8*seg + C_YZ] * _doubleProducts[8*seg + C_XH];
170 const double term2 = _doubleProducts[8*seg + C_ZX] * _doubleProducts[8*seg + C_YH];
171 const double term3 = _doubleProducts[8*seg + C_XY] * _doubleProducts[8*seg + C_ZH];
173 LOG(2, "for seg " << seg << " consistency " << term1 + term2 + term3 );
174 LOG(2, "term1 :" << term1 << " term2 :" << term2 << " term3: " << term3 );
176 const int num_zero = (term1 == 0.0 ? 1 : 0) + (term2 == 0.0 ? 1 : 0) + (term3 == 0.0 ? 1 : 0);
177 const int num_neg = (term1 < 0.0 ? 1 : 0) + (term2 < 0.0 ? 1 : 0) + (term3 < 0.0 ? 1 : 0);
178 const int num_pos = (term1 > 0.0 ? 1 : 0) + (term2 > 0.0 ? 1 : 0) + (term3 > 0.0 ? 1 : 0);
180 assert( num_zero + num_neg + num_pos == 3 );
182 // calculated geometry is inconsistent if we have one of the following cases
183 // * one term zero and the other two of the same sign
185 // * all terms positive
186 // * all terms negative
187 if(((num_zero == 1 && num_neg != 1) || num_zero == 2 || (num_neg == 0 && num_zero != 3) || num_neg == 3 ))
189 LOG(4, "inconsistent dp found" );
191 return !((num_zero == 1 && num_neg != 1) || num_zero == 2 || (num_neg == 0 && num_zero != 3) || num_neg == 3 );
196 * Calculate the shortest distance between a tetrahedron corner and a triangle segment.
198 * @param corner corner of the tetrahedron
199 * @param seg segment of the triangle
200 * @return shortest distance from the corner to the segment
202 double TransformedTriangle::calculateDistanceCornerSegment(const TetraCorner corner, const TriSegment seg) const
204 // NB uses fact that TriSegment <=> TriCorner that is first point of segment (PQ <=> P)
205 const TriCorner ptP_idx = TriCorner(seg);
206 const TriCorner ptQ_idx = TriCorner( (seg + 1) % 3);
208 const double ptP[3] = { _coords[5*ptP_idx], _coords[5*ptP_idx + 1], _coords[5*ptP_idx + 2] };
209 const double ptQ[3] = { _coords[5*ptQ_idx], _coords[5*ptQ_idx + 1], _coords[5*ptQ_idx + 2] };
211 // coordinates of corner
212 const double ptTetCorner[3] =
214 COORDS_TET_CORNER[3*corner ],
215 COORDS_TET_CORNER[3*corner + 1],
216 COORDS_TET_CORNER[3*corner + 2]
219 // dist^2 = ( PQ x CP )^2 / |PQ|^2 where C is the corner point
221 // difference vectors
222 const double diffPQ[3] = { ptQ[0] - ptP[0], ptQ[1] - ptP[1], ptQ[2] - ptP[2] };
223 const double diffCornerP[3] = { ptP[0] - ptTetCorner[0], ptP[1] - ptTetCorner[1], ptP[2] - ptTetCorner[2] };
225 // cross product of difference vectors
227 cross(diffPQ, diffCornerP, crossProd);
229 const double cross_squared = dot(crossProd, crossProd);
230 const double norm_diffPQ_squared = dot(diffPQ, diffPQ);
232 assert(norm_diffPQ_squared != 0.0);
234 return cross_squared / norm_diffPQ_squared;
238 * Pre-calculates all triple products for the tetrahedron with respect to
239 * this triangle, and stores them internally. This method takes into account
240 * the problem of errors due to cancellation.
243 void TransformedTriangle::preCalculateTripleProducts(void)
245 if(_is_triple_products_calculated)
250 // find edge / row to use -> that whose edge makes the smallest angle to the triangle
251 // use a map to find the minimum
252 std::map<double, int> anglesForRows;
254 LOG(4, "Precalculating triple products" );
255 for(TetraCorner corner = O ; corner <= Z ; corner = TetraCorner(corner + 1))
257 LOG(6, "- Triple product for corner " << corner );
259 for(int row = 1 ; row < 4 ; ++row)
261 const DoubleProduct dp = DP_FOR_DETERMINANT_EXPANSION[3*corner + (row - 1)];
263 // get edge by using correspondance between Double Product and Edge
264 TetraEdge edge = TetraEdge(dp);
266 // use edge only if it is surrounded by the surface
267 if( _triangleSurroundsEdgeCache[edge] )
269 // -- calculate angle between edge and PQR
270 const double angle = calculateAngleEdgeTriangle(edge);
271 anglesForRows.insert(std::make_pair(angle, row));
275 if(anglesForRows.size() != 0) // we have found a good row
277 const double minAngle = anglesForRows.begin()->first;
278 const int minRow = anglesForRows.begin()->second;
280 if(minAngle < TRIPLE_PRODUCT_ANGLE_THRESHOLD)
282 _tripleProducts[corner] = calcTByDevelopingRow(corner, minRow, true);
286 _tripleProducts[corner] = calcTByDevelopingRow(corner, minRow, false);
288 _validTP[corner] = true;
292 // this value will not be used
293 // we set it to whatever
294 LOG(6, "Triple product not calculated for corner " << corner );
295 _tripleProducts[corner] = -3.14159265;
296 _validTP[corner] = false;
299 anglesForRows.clear();
303 _is_triple_products_calculated = true;
307 * Calculates the angle between an edge of the tetrahedron and the triangle
309 * @param edge edge of the tetrahedron
310 * @return angle between triangle and edge
312 double TransformedTriangle::calculateAngleEdgeTriangle(const TetraEdge edge) const
314 // find normal to PQR - cross PQ and PR
317 _coords[5*Q] - _coords[5*P],
318 _coords[5*Q + 1] - _coords[5*P + 1],
319 _coords[5*Q + 2] - _coords[5*P + 2]
324 _coords[5*R] - _coords[5*P],
325 _coords[5*R + 1] - _coords[5*P + 1],
326 _coords[5*R + 2] - _coords[5*P + 2]
331 cross(pq, pr, normal);
333 static const double EDGE_VECTORS[18] =
338 -1.0, 1.0, 0.0, // XY
343 const double edgeVec[3] = {
344 EDGE_VECTORS[3*edge],
345 EDGE_VECTORS[3*edge + 1],
346 EDGE_VECTORS[3*edge + 2],
349 //return angleBetweenVectors(normal, edgeVec);
351 const double lenNormal = norm(normal);
352 const double lenEdgeVec = norm(edgeVec);
353 const double dotProd = dot(normal, edgeVec);
355 //? is this more stable? -> no subtraction
356 // return asin( dotProd / ( lenNormal * lenEdgeVec ) ) + 3.141592625358979 / 2.0;
357 return atan(1.0)*4.0 - acos( dotProd / ( lenNormal * lenEdgeVec ) );
362 * Calculates triple product associated with the given corner of tetrahedron, developing
363 * the determinant by the given row. The triple product gives the signed volume of
364 * the tetrahedron between this corner and the triangle PQR. If the flag project is true,
365 * one coordinate is projected out in order to eliminate errors in the intersection point
366 * calculation due to cancellation.
368 * @pre double products have already been calculated
369 * @param corner corner for which the triple product is calculated
370 * @param row row (1 <= row <= 3) used to calculate the determinant
371 * @param project indicates whether or not to perform projection as inidicated in Grandy, p.446
372 * @return triple product associated with corner (see Grandy, [50]-[52])
374 double TransformedTriangle::calcTByDevelopingRow(const TetraCorner corner, const int row, const bool project) const
377 // OVERVIEW OF CALCULATION
378 // --- sign before the determinant
379 // the sign used depends on the sign in front of the triple product (Grandy, [15]),
380 // and the convention used in the definition of the double products
382 // the sign in front of the determinant gives the following schema for the three terms (I):
389 // the 2x2 determinants are the following (C_AB <=> A_p*B_q - B_p*A_q, etc)
391 // O (sign:+) C_YZ C_XZ C_XY
392 // X (sign:-) C_YZ C_HZ C_HY
393 // Y (sign:-) C_HZ C_XZ C_XH
394 // Z (sign:-) C_YH C_XH C_XY
396 // these are represented in DP_FOR_DETERMINANT_EXPANSION,
397 // except for the fact that certain double products are inversed (C_AB <-> C_BA)
399 // comparing with the DOUBLE_PRODUCTS and using the fact that C_AB = -C_BA
400 // we deduce the following schema (II) :
407 // comparing the two schemas (I) and (II) gives us the following matrix of signs,
408 // putting 1 when the signs in (I) and (II) are equal and -1 when they are different :
410 static const int SIGNS[12] =
418 // find the offsets of the rows of the determinant
419 const int offset = COORDINATE_FOR_DETERMINANT_EXPANSION[3 * corner + (row - 1)];
421 const DoubleProduct dp = DP_FOR_DETERMINANT_EXPANSION[3 * corner + (row - 1)];
423 const int sign = SIGNS[3 * corner + (row - 1)];
425 const double cQR = calcStableC(QR, dp);
426 const double cRP = calcStableC(RP, dp);
427 const double cPQ = calcStableC(PQ, dp);
431 // coordinate to use for projection (Grandy, [57]) with edges
432 // OX, OY, OZ, XY, YZ, ZX in order :
433 // (y, z, x, h, h, h)
434 // for the first three we could also use {2, 0, 1}
435 static const int PROJECTION_COORDS[6] = { 1, 2, 0, 3, 3, 3 } ;
437 const int coord = PROJECTION_COORDS[ dp ];
439 // coordinate values for P, Q and R
440 const double coordValues[3] = { _coords[5*P + coord], _coords[5*Q + coord], _coords[5*R + coord] };
444 // products coordinate values with corresponding double product
445 const double coordDPProd[3] = { coordValues[0] * cQR, coordValues[1] * cRP, coordValues[2] * cPQ };
447 const double sumDPProd = coordDPProd[0] + coordDPProd[1] + coordDPProd[2];
448 const double sumDPProdSq = dot(coordDPProd, coordDPProd);
450 // alpha = sumDPProd / sumDPProdSq;
451 alpha = (sumDPProdSq != 0.0) ? sumDPProd / sumDPProdSq : 0.0;
454 const double cQRbar = cQR * (1.0 - alpha * coordValues[0] * cQR);
455 const double cRPbar = cRP * (1.0 - alpha * coordValues[1] * cRP);
456 const double cPQbar = cPQ * (1.0 - alpha * coordValues[2] * cPQ);
458 // check sign of barred products - should not change
459 // assert(cQRbar * cQR >= 0.0);
460 //assert(cRPbar * cRP >= 0.0);
461 //assert(cPQbar * cPQ >= 0.0);
463 const double p_term = _coords[5*P + offset] * cQRbar;
464 const double q_term = _coords[5*Q + offset] * cRPbar;
465 const double r_term = _coords[5*R + offset] * cPQbar;
467 // check that we are not so close to zero that numerical errors could take over,
468 // otherwise reset to zero (cf Grandy, p. 446)
470 const double delta = FIXED_DELTA;
472 const long double delta = MULT_PREC_F * (std::fabs(p_term) + std::fabs(q_term) + std::fabs(r_term));
475 if( epsilonEqual( p_term + q_term + r_term, 0.0, THRESHOLD_F * delta) )
477 LOG(4, "Reset imprecise triple product for corner " << corner << " to zero" );
482 // NB : using plus also for the middle term compensates for a double product
483 // which is inversely ordered
484 LOG(6, "Triple product for corner " << corner << ", row " << row << " = " << sign*( p_term + q_term + r_term ) );
485 return sign*( p_term + q_term + r_term );