1 // Copyright (C) 2007-2012 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
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 long 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 long 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 long double TransformedTriangle::THRESHOLD_F = 500.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 // after transformation to the U-space, coordinates are inaccurate
78 // small variations around zero should not be taken into account
79 void TransformedTriangle::resetNearZeroCoordinates()
81 for (int i=0; i<15; i++)
82 if (fabs(_coords[i])<TransformedTriangle::MACH_EPS*20.0) _coords[i]=0.0;
85 // ----------------------------------------------------------------------------------
86 // Double and triple product calculations
87 // ----------------------------------------------------------------------------------
90 * Pre-calculates all double products for this triangle, and stores
91 * them internally. This method makes compensation for precision errors,
92 * and it is thus the "stable" double products that are stored.
95 void TransformedTriangle::preCalculateDoubleProducts(void)
97 if(_is_double_products_calculated)
100 // -- calculate all unstable double products -- store in _doubleProducts
101 for(TriSegment seg = PQ ; seg <= RP ; seg = TriSegment(seg + 1))
103 for(DoubleProduct dp = C_YZ ; dp <= C_10 ; dp = DoubleProduct(dp + 1))
104 _doubleProducts[8*seg + dp] = calcUnstableC(seg, dp);
107 std::map<double, TetraCorner> distances;
109 // -- (1) for each segment : check that double products satisfy Grandy, [46]
110 // -- and make corrections if not
111 for(TriSegment seg = PQ ; seg <= RP ; seg = TriSegment(seg + 1))
113 if(!areDoubleProductsConsistent(seg))
115 LOG(4, "inconsistent! ");
116 for(TetraCorner corner = O ; corner <= Z ; corner = TetraCorner(corner + 1))
118 // calculate distance corner - segment axis
119 const double dist = calculateDistanceCornerSegment(corner, seg);
120 distances.insert( std::make_pair( dist, corner ) );
123 // first element -> minimum distance
124 const TetraCorner minCorner = distances.begin()->second;
125 resetDoubleProducts(seg, minCorner);
130 // -- (2) check that each double product statisfies Grandy, [47], else set to 0
131 for(TriSegment seg = PQ ; seg <= RP ; seg = TriSegment(seg + 1))
133 for(DoubleProduct dp = C_YZ ; dp <= C_10 ; dp = DoubleProduct(dp + 1))
135 // find the points of the triangle
136 // 0 -> P, 1 -> Q, 2 -> R
138 const int pt2 = (seg + 1) % 3;
141 const int off1 = DP_OFFSET_1[dp];
142 const int off2 = DP_OFFSET_2[dp];
144 const double term1 = _coords[5*pt1 + off1] * _coords[5*pt2 + off2];
145 const double term2 = _coords[5*pt1 + off2] * _coords[5*pt2 + off1];
147 const long double delta = MULT_PREC_F * ( std::fabs(term1) + std::fabs(term2) );
149 if( epsilonEqual(_doubleProducts[8*seg + dp], 0.0, THRESHOLD_F * delta))
153 if(_doubleProducts[8*seg + dp] != 0.0)
155 LOG(5, "Double product for (seg,dp) = (" << seg << ", " << dp << ") = " );
156 LOG(5, std::fabs(_doubleProducts[8*seg + dp]) << " is imprecise, reset to 0.0" );
161 _doubleProducts[8*seg + dp] = 0.0;
167 _is_double_products_calculated = true;
171 * Checks if the double products for a given segment are consistent, as defined by
174 * @param seg Segment for which to check consistency of double products
175 * @return true if the double products are consistent, false if not
177 bool TransformedTriangle::areDoubleProductsConsistent(const TriSegment seg) const
179 const double term1 = _doubleProducts[8*seg + C_YZ] * _doubleProducts[8*seg + C_XH];
180 const double term2 = _doubleProducts[8*seg + C_ZX] * _doubleProducts[8*seg + C_YH];
181 const double term3 = _doubleProducts[8*seg + C_XY] * _doubleProducts[8*seg + C_ZH];
183 LOG(2, "for seg " << seg << " consistency " << term1 + term2 + term3 );
184 LOG(2, "term1 :" << term1 << " term2 :" << term2 << " term3: " << term3 );
186 const int num_zero = (term1 == 0.0 ? 1 : 0) + (term2 == 0.0 ? 1 : 0) + (term3 == 0.0 ? 1 : 0);
187 const int num_neg = (term1 < 0.0 ? 1 : 0) + (term2 < 0.0 ? 1 : 0) + (term3 < 0.0 ? 1 : 0);
188 const int num_pos = (term1 > 0.0 ? 1 : 0) + (term2 > 0.0 ? 1 : 0) + (term3 > 0.0 ? 1 : 0);
190 assert( num_zero + num_neg + num_pos == 3 );
192 // calculated geometry is inconsistent if we have one of the following cases
193 // * one term zero and the other two of the same sign
195 // * all terms positive
196 // * all terms negative
197 if(((num_zero == 1 && num_neg != 1) || num_zero == 2 || (num_neg == 0 && num_zero != 3) || num_neg == 3 ))
199 LOG(4, "inconsistent dp found" );
201 return !((num_zero == 1 && num_neg != 1) || num_zero == 2 || (num_neg == 0 && num_zero != 3) || num_neg == 3 );
206 * Calculate the shortest distance between a tetrahedron corner and a triangle segment.
208 * @param corner corner of the tetrahedron
209 * @param seg segment of the triangle
210 * @return shortest distance from the corner to the segment
212 double TransformedTriangle::calculateDistanceCornerSegment(const TetraCorner corner, const TriSegment seg) const
214 // NB uses fact that TriSegment <=> TriCorner that is first point of segment (PQ <=> P)
215 const TriCorner ptP_idx = TriCorner(seg);
216 const TriCorner ptQ_idx = TriCorner( (seg + 1) % 3);
218 const double ptP[3] = { _coords[5*ptP_idx], _coords[5*ptP_idx + 1], _coords[5*ptP_idx + 2] };
219 const double ptQ[3] = { _coords[5*ptQ_idx], _coords[5*ptQ_idx + 1], _coords[5*ptQ_idx + 2] };
221 // coordinates of corner
222 const double ptTetCorner[3] =
224 COORDS_TET_CORNER[3*corner ],
225 COORDS_TET_CORNER[3*corner + 1],
226 COORDS_TET_CORNER[3*corner + 2]
229 // dist^2 = ( PQ x CP )^2 / |PQ|^2 where C is the corner point
231 // difference vectors
232 const double diffPQ[3] = { ptQ[0] - ptP[0], ptQ[1] - ptP[1], ptQ[2] - ptP[2] };
233 const double diffCornerP[3] = { ptP[0] - ptTetCorner[0], ptP[1] - ptTetCorner[1], ptP[2] - ptTetCorner[2] };
235 // cross product of difference vectors
237 cross(diffPQ, diffCornerP, crossProd);
239 const double cross_squared = dot(crossProd, crossProd);
240 const double norm_diffPQ_squared = dot(diffPQ, diffPQ);
242 assert(norm_diffPQ_squared != 0.0);
244 return cross_squared / norm_diffPQ_squared;
248 * Pre-calculates all triple products for the tetrahedron with respect to
249 * this triangle, and stores them internally. This method takes into account
250 * the problem of errors due to cancellation.
253 void TransformedTriangle::preCalculateTripleProducts(void)
255 if(_is_triple_products_calculated)
260 // find edge / row to use -> that whose edge makes the smallest angle to the triangle
261 // use a map to find the minimum
262 std::map<double, int> anglesForRows;
264 LOG(4, "Precalculating triple products" );
265 for(TetraCorner corner = O ; corner <= Z ; corner = TetraCorner(corner + 1))
267 LOG(6, "- Triple product for corner " << corner );
269 for(int row = 1 ; row < 4 ; ++row)
271 const DoubleProduct dp = DP_FOR_DETERMINANT_EXPANSION[3*corner + (row - 1)];
273 // get edge by using correspondance between Double Product and Edge
274 TetraEdge edge = TetraEdge(dp);
276 // use edge only if it is surrounded by the surface
277 if( _triangleSurroundsEdgeCache[edge] )
279 // -- calculate angle between edge and PQR
280 const double angle = calculateAngleEdgeTriangle(edge);
281 anglesForRows.insert(std::make_pair(angle, row));
285 if(anglesForRows.size() != 0) // we have found a good row
287 const double minAngle = anglesForRows.begin()->first;
288 const int minRow = anglesForRows.begin()->second;
290 if(minAngle < TRIPLE_PRODUCT_ANGLE_THRESHOLD)
292 _tripleProducts[corner] = calcTByDevelopingRow(corner, minRow, true);
296 _tripleProducts[corner] = calcTByDevelopingRow(corner, minRow, false);
298 _validTP[corner] = true;
302 // this value will not be used
303 // we set it to whatever
304 LOG(6, "Triple product not calculated for corner " << corner );
305 _tripleProducts[corner] = -3.14159265;
306 _validTP[corner] = false;
309 anglesForRows.clear();
313 _is_triple_products_calculated = true;
317 * Calculates the angle between an edge of the tetrahedron and the triangle
319 * @param edge edge of the tetrahedron
320 * @return angle between triangle and edge
322 double TransformedTriangle::calculateAngleEdgeTriangle(const TetraEdge edge) const
324 // find normal to PQR - cross PQ and PR
327 _coords[5*Q] - _coords[5*P],
328 _coords[5*Q + 1] - _coords[5*P + 1],
329 _coords[5*Q + 2] - _coords[5*P + 2]
334 _coords[5*R] - _coords[5*P],
335 _coords[5*R + 1] - _coords[5*P + 1],
336 _coords[5*R + 2] - _coords[5*P + 2]
341 cross(pq, pr, normal);
343 static const double EDGE_VECTORS[18] =
348 -1.0, 1.0, 0.0, // XY
353 const double edgeVec[3] = {
354 EDGE_VECTORS[3*edge],
355 EDGE_VECTORS[3*edge + 1],
356 EDGE_VECTORS[3*edge + 2],
359 //return angleBetweenVectors(normal, edgeVec);
361 const double lenNormal = norm(normal);
362 const double lenEdgeVec = norm(edgeVec);
363 const double dotProd = dot(normal, edgeVec);
365 //? is this more stable? -> no subtraction
366 // return asin( dotProd / ( lenNormal * lenEdgeVec ) ) + 3.141592625358979 / 2.0;
367 double tmp=dotProd / ( lenNormal * lenEdgeVec );
368 tmp=std::max(tmp,-1.);
369 tmp=std::min(tmp,1.);
370 return atan(1.0)*4.0 - acos(tmp);
375 * Calculates triple product associated with the given corner of tetrahedron, developing
376 * the determinant by the given row. The triple product gives the signed volume of
377 * the tetrahedron between this corner and the triangle PQR. If the flag project is true,
378 * one coordinate is projected out in order to eliminate errors in the intersection point
379 * calculation due to cancellation.
381 * @pre double products have already been calculated
382 * @param corner corner for which the triple product is calculated
383 * @param row row (1 <= row <= 3) used to calculate the determinant
384 * @param project indicates whether or not to perform projection as inidicated in Grandy, p.446
385 * @return triple product associated with corner (see Grandy, [50]-[52])
387 double TransformedTriangle::calcTByDevelopingRow(const TetraCorner corner, const int row, const bool project) const
390 // OVERVIEW OF CALCULATION
391 // --- sign before the determinant
392 // the sign used depends on the sign in front of the triple product (Grandy, [15]),
393 // and the convention used in the definition of the double products
395 // the sign in front of the determinant gives the following schema for the three terms (I):
402 // the 2x2 determinants are the following (C_AB <=> A_p*B_q - B_p*A_q, etc)
404 // O (sign:+) C_YZ C_XZ C_XY
405 // X (sign:-) C_YZ C_HZ C_HY
406 // Y (sign:-) C_HZ C_XZ C_XH
407 // Z (sign:-) C_YH C_XH C_XY
409 // these are represented in DP_FOR_DETERMINANT_EXPANSION,
410 // except for the fact that certain double products are inversed (C_AB <-> C_BA)
412 // comparing with the DOUBLE_PRODUCTS and using the fact that C_AB = -C_BA
413 // we deduce the following schema (II) :
420 // comparing the two schemas (I) and (II) gives us the following matrix of signs,
421 // putting 1 when the signs in (I) and (II) are equal and -1 when they are different :
423 static const int SIGNS[12] =
431 // find the offsets of the rows of the determinant
432 const int offset = COORDINATE_FOR_DETERMINANT_EXPANSION[3 * corner + (row - 1)];
434 const DoubleProduct dp = DP_FOR_DETERMINANT_EXPANSION[3 * corner + (row - 1)];
436 const int sign = SIGNS[3 * corner + (row - 1)];
438 const double cQR = calcStableC(QR, dp);
439 const double cRP = calcStableC(RP, dp);
440 const double cPQ = calcStableC(PQ, dp);
444 // coordinate to use for projection (Grandy, [57]) with edges
445 // OX, OY, OZ, XY, YZ, ZX in order :
446 // (y, z, x, h, h, h)
447 // for the first three we could also use {2, 0, 1}
448 static const int PROJECTION_COORDS[6] = { 1, 2, 0, 3, 3, 3 } ;
450 const int coord = PROJECTION_COORDS[ dp ];
452 // coordinate values for P, Q and R
453 const double coordValues[3] = { _coords[5*P + coord], _coords[5*Q + coord], _coords[5*R + coord] };
457 // products coordinate values with corresponding double product
458 const double coordDPProd[3] = { coordValues[0] * cQR, coordValues[1] * cRP, coordValues[2] * cPQ };
460 const double sumDPProd = coordDPProd[0] + coordDPProd[1] + coordDPProd[2];
461 const double sumDPProdSq = dot(coordDPProd, coordDPProd);
463 // alpha = sumDPProd / sumDPProdSq;
464 alpha = (sumDPProdSq != 0.0) ? sumDPProd / sumDPProdSq : 0.0;
467 const double cQRbar = cQR * (1.0 - alpha * coordValues[0] * cQR);
468 const double cRPbar = cRP * (1.0 - alpha * coordValues[1] * cRP);
469 const double cPQbar = cPQ * (1.0 - alpha * coordValues[2] * cPQ);
471 // check sign of barred products - should not change
472 // assert(cQRbar * cQR >= 0.0);
473 //assert(cRPbar * cRP >= 0.0);
474 //assert(cPQbar * cPQ >= 0.0);
476 const double p_term = _coords[5*P + offset] * cQRbar;
477 const double q_term = _coords[5*Q + offset] * cRPbar;
478 const double r_term = _coords[5*R + offset] * cPQbar;
480 // check that we are not so close to zero that numerical errors could take over,
481 // otherwise reset to zero (cf Grandy, p. 446)
483 const double delta = FIXED_DELTA;
485 const long double delta = MULT_PREC_F * (std::fabs(p_term) + std::fabs(q_term) + std::fabs(r_term));
488 if( epsilonEqual( p_term + q_term + r_term, 0.0, THRESHOLD_F * delta) )
490 LOG(4, "Reset imprecise triple product for corner " << corner << " to zero" );
495 // NB : using plus also for the middle term compensates for a double product
496 // which is inversely ordered
497 LOG(6, "Triple product for corner " << corner << ", row " << row << " = " << sign*( p_term + q_term + r_term ) );
498 return sign*( p_term + q_term + r_term );