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 "TetraAffineTransform.hxx"
21 #include "VectorUtils.hxx"
29 namespace INTERP_KERNEL
31 /////////////////////////////////////////////////////////////////////////////////////////
32 /// PUBLIC INTERFACE METHODS //////////////
33 /////////////////////////////////////////////////////////////////////////////////////////
37 * Create the TetraAffineTransform object from the tetrahedron
38 * with corners specified in pts. If the tetrahedron is degenerate or almost degenerate,
39 * construction succeeds, but the determinant of the transform is set to 0.
41 * @param pts a 4x3 matrix containing 4 points (P0X,P0Y,P0Z,P1X,P1Y,P1Z,...) of 3 coordinates each
43 TetraAffineTransform::TetraAffineTransform(const double *pts)
46 LOG(2,"Creating transform from tetraeder : ");
48 // three last points -> linear transform
49 for(int i = 0; i < 3 ; ++i)
51 for(int j = 0 ; j < 3 ; ++j)
53 // NB we insert columns, not rows
54 _linear_transform[3*j + i] = pts[(i+1)*3+j] - pts[j];
58 // remember _linear_transform for the reverse transformation
59 memcpy( _back_linear_transform, _linear_transform, 9*sizeof(double));
60 memcpy( _back_translation, pts, 3*sizeof(double));
62 calculateDeterminant();
64 LOG(3, "determinant before inverse = " << _determinant);
66 double ni(1./INTERP_KERNEL::normInf(_linear_transform));
69 // check that tetra is non-planar -> determinant is not zero
70 // AGY : the check to 0. must integrate the infinite norm of _linear_transform matrix.
71 // otherwise set _determinant to zero to signal caller that transformation did not work
72 if(epsilonEqual(ni*_determinant, 0.0))
78 // we need the inverse transform
79 invertLinearTransform();
81 // first point -> translation
82 // calculate here because translation takes place in "transformed space",
83 // or in other words b = -A*O where A is the linear transform
84 // and O is the position vector of the point that is mapped onto the origin
85 for(int i = 0 ; i < 3 ; ++i)
87 _translation[i] = -(_linear_transform[3*i]*pts[0] + _linear_transform[3*i+1]*pts[1] + _linear_transform[3*i+2]*pts[2]) ;
90 // precalculate determinant (again after inversion of transform)
91 calculateDeterminant();
93 #ifdef INVERSION_SELF_CHECK
94 // debugging : check that applying the inversed transform to the original points
95 // gives us the unit tetrahedron
96 LOG(4, "transform determinant is " << _determinant);
97 LOG(4, "*Self-check : Applying transformation to original points ... ");
98 for(int i = 0; i < 4 ; ++i)
103 for(int j = 0; j < 3; ++j)
105 assert(epsilonEqual(v[j], (3*i+j == 3 || 3*i+j == 7 || 3*i+j == 11 ) ? 1.0 : 0.0));
114 * Calculates the transform of point srcPt and stores the result in destPt.
115 * If destPt == srcPt, then srcPt is overwritten safely.
118 * @param destPt double[3] in which to store the transformed point
119 * @param srcPt double[3] containing coordinates of points to be transformed
122 void TetraAffineTransform::apply(double* destPt, const double* srcPt) const
124 double* dest = destPt;
126 // are we self-allocating ?
127 const bool selfAllocation = (destPt == srcPt);
131 // alloc temporary memory
132 dest = new double[3];
134 LOG(6, "Info : Self-affectation in TetraAffineTransform::apply");
137 for(int i = 0 ; i < 3 ; ++i)
139 // matrix - vector multiplication
140 dest[i] = _linear_transform[3*i] * srcPt[0] + _linear_transform[3*i + 1] * srcPt[1] + _linear_transform[3*i + 2] * srcPt[2];
143 dest[i] += _translation[i];
148 // copy result back to destPt
149 for(int i = 0 ; i < 3 ; ++i)
158 * Calculates the reverse transform of point srcPt and stores the result in destPt.
159 * If destPt == srcPt, then srcPt is overwritten safely.
161 * @param destPt double[3] in which to store the transformed point
162 * @param srcPt double[3] containing coordinates of points to be transformed
164 void TetraAffineTransform::reverseApply(double* destPt, const double* srcPt) const
166 double* dest = destPt;
168 // are we self-allocating ?
169 const bool selfAllocation = (destPt == srcPt);
173 // alloc temporary memory
174 dest = new double[3];
176 LOG(6, "Info : Self-affectation in TetraAffineTransform::reverseApply");
179 for(int i = 0 ; i < 3 ; ++i)
181 // matrix - vector multiplication
182 dest[i] = _back_linear_transform[3*i] * srcPt[0] + _back_linear_transform[3*i + 1] * srcPt[1] + _back_linear_transform[3*i + 2] * srcPt[2];
185 dest[i] += _back_translation[i];
190 // copy result back to destPt
191 for(int i = 0 ; i < 3 ; ++i)
200 * Returns the determinant of the linear part A of the transform.
202 * @return determinant of the transform
205 double TetraAffineTransform::determinant() const
211 * Outputs to std::cout the matrix A and the vector b
212 * of the transform Ax + b
215 void TetraAffineTransform::dump() const
217 std::cout << "A = " << std::endl << "[";
218 for(int i = 0; i < 3; ++i)
220 std::cout << _linear_transform[3*i] << ", " << _linear_transform[3*i + 1] << ", " << _linear_transform[3*i + 2];
221 if(i != 2 ) std::cout << std::endl;
223 std::cout << "]" << std::endl;
225 std::cout << "b = " << "[" << _translation[0] << ", " << _translation[1] << ", " << _translation[2] << "]" << std::endl;
228 /////////////////////////////////////////////////////////////////////////////////////////
229 /// PRIVATE METHODS //////////////
230 /////////////////////////////////////////////////////////////////////////////////////////
233 * Calculates the inverse of the matrix A, stored in _linear_transform
234 * by LU-factorization and substitution
237 void TetraAffineTransform::invertLinearTransform()
239 //{ we copy the matrix for the lu-factorization
242 for(int i = 0 ; i < 9; ++i)
244 lu[i] = _linear_transform[i];
247 // calculate LU factorization
249 factorizeLU(lu, idx);
251 // calculate inverse by forward and backward substitution
252 // store in _linear_transform
253 // NB _linear_transform cannot be overwritten with lu in the loop
254 for(int i = 0 ; i < 3 ; ++i)
256 // form standard base vector i
259 double ( int(i == 0) ),
260 double ( int(i == 1) ),
261 double ( int(i == 2) ),
264 LOG(6, "b = [" << b[0] << ", " << b[1] << ", " << b[2] << "]");
267 forwardSubstitution(y, lu, b, idx);
270 backwardSubstitution(x, lu, y, idx);
272 // copy to _linear_transform matrix
273 // NB : this is a column operation, so we cannot
274 // do this directly when we calculate x
275 for(int j = 0 ; j < 3 ; j++)
277 _linear_transform[3*j + i] = x[idx[j]];
283 * Updates the member _determinant of the matrix A of the transformation.
286 void TetraAffineTransform::calculateDeterminant()
288 const double subDet[3] =
290 _linear_transform[4] * _linear_transform[8] - _linear_transform[5] * _linear_transform[7],
291 _linear_transform[3] * _linear_transform[8] - _linear_transform[5] * _linear_transform[6],
292 _linear_transform[3] * _linear_transform[7] - _linear_transform[4] * _linear_transform[6]
295 _determinant = _linear_transform[0] * subDet[0] - _linear_transform[1] * subDet[1] + _linear_transform[2] * subDet[2];
299 /////////////////////////////////////////////////
300 /// Auxiliary methods for inverse calculation ///
301 /////////////////////////////////////////////////
305 * Calculates the LU-factorization of the matrix A (_linear_transform)
306 * and stores it in lu. Since partial pivoting is used, there are
307 * row swaps. This is represented by the index permutation vector idx : to access element
308 * (i,j) of lu, use lu[3*idx[i] + j]
310 * @param lu double[9] in which to store LU-factorization
311 * @param idx int[3] in which to store row permutation vector
313 void TetraAffineTransform::factorizeLU(double* lu, int* idx) const
315 // 3 x 3 LU factorization
317 for(int i = 0 ; i < 3 ; ++i)
322 for(int k = 0; k < 2 ; ++k)
327 double max = std::fabs(lu[3*idx[k] + k]);
331 if(std::fabs(lu[3*idx[i] + k]) > max)
333 max = fabs(lu[3*idx[i] + k]);
339 // swap rows in index vector
345 for(int j = k + 1 ; j < 3 ; ++j)
347 // l_jk = u_jk / u_kk
348 lu[3*idx[j] + k] /= lu[3*idx[k] + k];
349 for(int s = k + 1; s < 3 ; ++s)
351 // case s = k will always become zero, and then be replaced by
353 // there's no need to calculate this explicitly
355 // u_js = u_js - l_jk * u_ks
356 lu[3*idx[j] + s] -= lu[3*idx[j] + k] * lu[3*idx[k] + s] ;
363 * Solves the system Lx = b, where L is lower unit-triangular (ones on the diagonal)
365 * @param x double[3] in which the solution is stored
366 * @param lu double[9] containing the LU-factorization
367 * @param b double[3] containing the right-hand side
368 * @param idx int[3] containing the permutation vector associated with lu
371 void TetraAffineTransform::forwardSubstitution(double* x, const double* lu, const double* b, const int* idx) const
373 // divisions are not carried out explicitly because lu does not store
374 // the diagonal values of L, which are all 1
375 // Compare with backwardSubstitution()
376 x[idx[0]] = ( b[idx[0]] ); // / lu[3*idx[0]]
377 x[idx[1]] = ( b[idx[1]] - lu[3*idx[1]] * x[idx[0]] ); // / lu[3*idx[1] + 1];
378 x[idx[2]] = ( b[idx[2]] - lu[3*idx[2]] * x[idx[0]] - lu[3*idx[2] + 1] * x[idx[1]] ); // / lu[3*idx[2] + 2];
382 * Solves the system Ux = b, where U is upper-triangular
384 * @param x double[3] in which the solution is stored
385 * @param lu double[9] containing the LU-factorization
386 * @param b double[3] containing the right-hand side
387 * @param idx int[3] containing the permutation vector associated with lu
390 void TetraAffineTransform::backwardSubstitution(double* x, const double* lu, const double* b, const int* idx) const
392 x[idx[2]] = ( b[idx[2]] ) / lu[3*idx[2] + 2];
393 x[idx[1]] = ( b[idx[1]] - lu[3*idx[1] + 2] * x[idx[2]] ) / lu[3*idx[1] + 1];
394 x[idx[0]] = ( b[idx[0]] - lu[3*idx[0] + 1] * x[idx[1]] - lu[3*idx[0] + 2] * x[idx[2]] ) / lu[3*idx[0]];