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 "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 (pts[0], ..., pts[3]) of 3 coordinates each
43 TetraAffineTransform::TetraAffineTransform(const double** pts)
46 LOG(2,"Creating transform from tetraeder : ");
47 LOG(2, vToStr(pts[0]) << ", " << vToStr(pts[1]) << ", " << vToStr(pts[2]) << ", " << vToStr(pts[3]));
49 // three last points -> linear transform
50 for(int i = 0; i < 3 ; ++i)
52 for(int j = 0 ; j < 3 ; ++j)
54 // NB we insert columns, not rows
55 _linear_transform[3*j + i] = (pts[i+1])[j] - (pts[0])[j];
59 // remember _linear_transform for the reverse transformation
60 memcpy( _back_linear_transform, _linear_transform, 9*sizeof(double));
61 memcpy( _back_translation, pts[0], 3*sizeof(double));
63 calculateDeterminant();
65 LOG(3, "determinant before inverse = " << _determinant);
67 // check that tetra is non-planar -> determinant is not zero
68 // otherwise set _determinant to zero to signal caller that transformation did not work
69 if(epsilonEqual(_determinant, 0.0))
75 // we need the inverse transform
76 invertLinearTransform();
78 // first point -> translation
79 // calculate here because translation takes place in "transformed space",
80 // or in other words b = -A*O where A is the linear transform
81 // and O is the position vector of the point that is mapped onto the origin
82 for(int i = 0 ; i < 3 ; ++i)
84 _translation[i] = -(_linear_transform[3*i]*(pts[0])[0] + _linear_transform[3*i+1]*(pts[0])[1] + _linear_transform[3*i+2]*(pts[0])[2]) ;
87 // precalculate determinant (again after inversion of transform)
88 calculateDeterminant();
90 #ifdef INVERSION_SELF_CHECK
91 // debugging : check that applying the inversed transform to the original points
92 // gives us the unit tetrahedron
93 LOG(4, "transform determinant is " << _determinant);
94 LOG(4, "*Self-check : Applying transformation to original points ... ");
95 for(int i = 0; i < 4 ; ++i)
100 for(int j = 0; j < 3; ++j)
102 assert(epsilonEqual(v[j], (3*i+j == 3 || 3*i+j == 7 || 3*i+j == 11 ) ? 1.0 : 0.0));
111 * Calculates the transform of point srcPt and stores the result in destPt.
112 * If destPt == srcPt, then srcPt is overwritten safely.
115 * @param destPt double[3] in which to store the transformed point
116 * @param srcPt double[3] containing coordinates of points to be transformed
119 void TetraAffineTransform::apply(double* destPt, const double* srcPt) const
121 double* dest = destPt;
123 // are we self-allocating ?
124 const bool selfAllocation = (destPt == srcPt);
128 // alloc temporary memory
129 dest = new double[3];
131 LOG(6, "Info : Self-affectation in TetraAffineTransform::apply");
134 for(int i = 0 ; i < 3 ; ++i)
136 // matrix - vector multiplication
137 dest[i] = _linear_transform[3*i] * srcPt[0] + _linear_transform[3*i + 1] * srcPt[1] + _linear_transform[3*i + 2] * srcPt[2];
140 dest[i] += _translation[i];
145 // copy result back to destPt
146 for(int i = 0 ; i < 3 ; ++i)
155 * Calculates the reverse transform of point srcPt and stores the result in destPt.
156 * If destPt == srcPt, then srcPt is overwritten safely.
158 * @param destPt double[3] in which to store the transformed point
159 * @param srcPt double[3] containing coordinates of points to be transformed
161 void TetraAffineTransform::reverseApply(double* destPt, const double* srcPt) const
163 double* dest = destPt;
165 // are we self-allocating ?
166 const bool selfAllocation = (destPt == srcPt);
170 // alloc temporary memory
171 dest = new double[3];
173 LOG(6, "Info : Self-affectation in TetraAffineTransform::reverseApply");
176 for(int i = 0 ; i < 3 ; ++i)
178 // matrix - vector multiplication
179 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];
182 dest[i] += _back_translation[i];
187 // copy result back to destPt
188 for(int i = 0 ; i < 3 ; ++i)
197 * Returns the determinant of the linear part A of the transform.
199 * @return determinant of the transform
202 double TetraAffineTransform::determinant() const
208 * Outputs to std::cout the matrix A and the vector b
209 * of the transform Ax + b
212 void TetraAffineTransform::dump() const
214 std::cout << "A = " << std::endl << "[";
215 for(int i = 0; i < 3; ++i)
217 std::cout << _linear_transform[3*i] << ", " << _linear_transform[3*i + 1] << ", " << _linear_transform[3*i + 2];
218 if(i != 2 ) std::cout << std::endl;
220 std::cout << "]" << std::endl;
222 std::cout << "b = " << "[" << _translation[0] << ", " << _translation[1] << ", " << _translation[2] << "]" << std::endl;
225 /////////////////////////////////////////////////////////////////////////////////////////
226 /// PRIVATE METHODS //////////////
227 /////////////////////////////////////////////////////////////////////////////////////////
230 * Calculates the inverse of the matrix A, stored in _linear_transform
231 * by LU-factorization and substitution
234 void TetraAffineTransform::invertLinearTransform()
236 //{ we copy the matrix for the lu-factorization
239 for(int i = 0 ; i < 9; ++i)
241 lu[i] = _linear_transform[i];
244 // calculate LU factorization
246 factorizeLU(lu, idx);
248 // calculate inverse by forward and backward substitution
249 // store in _linear_transform
250 // NB _linear_transform cannot be overwritten with lu in the loop
251 for(int i = 0 ; i < 3 ; ++i)
253 // form standard base vector i
256 double ( int(i == 0) ),
257 double ( int(i == 1) ),
258 double ( int(i == 2) ),
261 LOG(6, "b = [" << b[0] << ", " << b[1] << ", " << b[2] << "]");
264 forwardSubstitution(y, lu, b, idx);
267 backwardSubstitution(x, lu, y, idx);
269 // copy to _linear_transform matrix
270 // NB : this is a column operation, so we cannot
271 // do this directly when we calculate x
272 for(int j = 0 ; j < 3 ; j++)
274 _linear_transform[3*j + i] = x[idx[j]];
280 * Updates the member _determinant of the matrix A of the transformation.
283 void TetraAffineTransform::calculateDeterminant()
285 const double subDet[3] =
287 _linear_transform[4] * _linear_transform[8] - _linear_transform[5] * _linear_transform[7],
288 _linear_transform[3] * _linear_transform[8] - _linear_transform[5] * _linear_transform[6],
289 _linear_transform[3] * _linear_transform[7] - _linear_transform[4] * _linear_transform[6]
292 _determinant = _linear_transform[0] * subDet[0] - _linear_transform[1] * subDet[1] + _linear_transform[2] * subDet[2];
296 /////////////////////////////////////////////////
297 /// Auxiliary methods for inverse calculation ///
298 /////////////////////////////////////////////////
302 * Calculates the LU-factorization of the matrix A (_linear_transform)
303 * and stores it in lu. Since partial pivoting is used, there are
304 * row swaps. This is represented by the index permutation vector idx : to access element
305 * (i,j) of lu, use lu[3*idx[i] + j]
307 * @param lu double[9] in which to store LU-factorization
308 * @param idx int[3] in which to store row permutation vector
310 void TetraAffineTransform::factorizeLU(double* lu, int* idx) const
312 // 3 x 3 LU factorization
314 for(int i = 0 ; i < 3 ; ++i)
319 for(int k = 0; k < 2 ; ++k)
324 double max = std::fabs(lu[3*idx[k] + k]);
328 if(std::fabs(lu[3*idx[i] + k]) > max)
330 max = fabs(lu[3*idx[i] + k]);
336 // swap rows in index vector
342 for(int j = k + 1 ; j < 3 ; ++j)
344 // l_jk = u_jk / u_kk
345 lu[3*idx[j] + k] /= lu[3*idx[k] + k];
346 for(int s = k + 1; s < 3 ; ++s)
348 // case s = k will always become zero, and then be replaced by
350 // there's no need to calculate this explicitly
352 // u_js = u_js - l_jk * u_ks
353 lu[3*idx[j] + s] -= lu[3*idx[j] + k] * lu[3*idx[k] + s] ;
360 * Solves the system Lx = b, where L is lower unit-triangular (ones on the diagonal)
362 * @param x double[3] in which the solution is stored
363 * @param lu double[9] containing the LU-factorization
364 * @param b double[3] containing the right-hand side
365 * @param idx int[3] containing the permutation vector associated with lu
368 void TetraAffineTransform::forwardSubstitution(double* x, const double* lu, const double* b, const int* idx) const
370 // divisions are not carried out explicitly because lu does not store
371 // the diagonal values of L, which are all 1
372 // Compare with backwardSubstitution()
373 x[idx[0]] = ( b[idx[0]] ); // / lu[3*idx[0]]
374 x[idx[1]] = ( b[idx[1]] - lu[3*idx[1]] * x[idx[0]] ); // / lu[3*idx[1] + 1];
375 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];
379 * Solves the system Ux = b, where U is upper-triangular
381 * @param x double[3] in which the solution is stored
382 * @param lu double[9] containing the LU-factorization
383 * @param b double[3] containing the right-hand side
384 * @param idx int[3] containing the permutation vector associated with lu
387 void TetraAffineTransform::backwardSubstitution(double* x, const double* lu, const double* b, const int* idx) const
389 x[idx[2]] = ( b[idx[2]] ) / lu[3*idx[2] + 2];
390 x[idx[1]] = ( b[idx[1]] - lu[3*idx[1] + 2] * x[idx[2]] ) / lu[3*idx[1] + 1];
391 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]];