1 // Copyright (C) 2007-2013 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 (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 // check that tetra is non-planar -> determinant is not zero
67 // otherwise set _determinant to zero to signal caller that transformation did not work
68 if(epsilonEqual(_determinant, 0.0))
74 // we need the inverse transform
75 invertLinearTransform();
77 // first point -> translation
78 // calculate here because translation takes place in "transformed space",
79 // or in other words b = -A*O where A is the linear transform
80 // and O is the position vector of the point that is mapped onto the origin
81 for(int i = 0 ; i < 3 ; ++i)
83 _translation[i] = -(_linear_transform[3*i]*pts[0] + _linear_transform[3*i+1]*pts[1] + _linear_transform[3*i+2]*pts[2]) ;
86 // precalculate determinant (again after inversion of transform)
87 calculateDeterminant();
89 #ifdef INVERSION_SELF_CHECK
90 // debugging : check that applying the inversed transform to the original points
91 // gives us the unit tetrahedron
92 LOG(4, "transform determinant is " << _determinant);
93 LOG(4, "*Self-check : Applying transformation to original points ... ");
94 for(int i = 0; i < 4 ; ++i)
99 for(int j = 0; j < 3; ++j)
101 assert(epsilonEqual(v[j], (3*i+j == 3 || 3*i+j == 7 || 3*i+j == 11 ) ? 1.0 : 0.0));
110 * Calculates the transform of point srcPt and stores the result in destPt.
111 * If destPt == srcPt, then srcPt is overwritten safely.
114 * @param destPt double[3] in which to store the transformed point
115 * @param srcPt double[3] containing coordinates of points to be transformed
118 void TetraAffineTransform::apply(double* destPt, const double* srcPt) const
120 double* dest = destPt;
122 // are we self-allocating ?
123 const bool selfAllocation = (destPt == srcPt);
127 // alloc temporary memory
128 dest = new double[3];
130 LOG(6, "Info : Self-affectation in TetraAffineTransform::apply");
133 for(int i = 0 ; i < 3 ; ++i)
135 // matrix - vector multiplication
136 dest[i] = _linear_transform[3*i] * srcPt[0] + _linear_transform[3*i + 1] * srcPt[1] + _linear_transform[3*i + 2] * srcPt[2];
139 dest[i] += _translation[i];
144 // copy result back to destPt
145 for(int i = 0 ; i < 3 ; ++i)
154 * Calculates the reverse transform of point srcPt and stores the result in destPt.
155 * If destPt == srcPt, then srcPt is overwritten safely.
157 * @param destPt double[3] in which to store the transformed point
158 * @param srcPt double[3] containing coordinates of points to be transformed
160 void TetraAffineTransform::reverseApply(double* destPt, const double* srcPt) const
162 double* dest = destPt;
164 // are we self-allocating ?
165 const bool selfAllocation = (destPt == srcPt);
169 // alloc temporary memory
170 dest = new double[3];
172 LOG(6, "Info : Self-affectation in TetraAffineTransform::reverseApply");
175 for(int i = 0 ; i < 3 ; ++i)
177 // matrix - vector multiplication
178 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];
181 dest[i] += _back_translation[i];
186 // copy result back to destPt
187 for(int i = 0 ; i < 3 ; ++i)
196 * Returns the determinant of the linear part A of the transform.
198 * @return determinant of the transform
201 double TetraAffineTransform::determinant() const
207 * Outputs to std::cout the matrix A and the vector b
208 * of the transform Ax + b
211 void TetraAffineTransform::dump() const
213 std::cout << "A = " << std::endl << "[";
214 for(int i = 0; i < 3; ++i)
216 std::cout << _linear_transform[3*i] << ", " << _linear_transform[3*i + 1] << ", " << _linear_transform[3*i + 2];
217 if(i != 2 ) std::cout << std::endl;
219 std::cout << "]" << std::endl;
221 std::cout << "b = " << "[" << _translation[0] << ", " << _translation[1] << ", " << _translation[2] << "]" << std::endl;
224 /////////////////////////////////////////////////////////////////////////////////////////
225 /// PRIVATE METHODS //////////////
226 /////////////////////////////////////////////////////////////////////////////////////////
229 * Calculates the inverse of the matrix A, stored in _linear_transform
230 * by LU-factorization and substitution
233 void TetraAffineTransform::invertLinearTransform()
235 //{ we copy the matrix for the lu-factorization
238 for(int i = 0 ; i < 9; ++i)
240 lu[i] = _linear_transform[i];
243 // calculate LU factorization
245 factorizeLU(lu, idx);
247 // calculate inverse by forward and backward substitution
248 // store in _linear_transform
249 // NB _linear_transform cannot be overwritten with lu in the loop
250 for(int i = 0 ; i < 3 ; ++i)
252 // form standard base vector i
255 double ( int(i == 0) ),
256 double ( int(i == 1) ),
257 double ( int(i == 2) ),
260 LOG(6, "b = [" << b[0] << ", " << b[1] << ", " << b[2] << "]");
263 forwardSubstitution(y, lu, b, idx);
266 backwardSubstitution(x, lu, y, idx);
268 // copy to _linear_transform matrix
269 // NB : this is a column operation, so we cannot
270 // do this directly when we calculate x
271 for(int j = 0 ; j < 3 ; j++)
273 _linear_transform[3*j + i] = x[idx[j]];
279 * Updates the member _determinant of the matrix A of the transformation.
282 void TetraAffineTransform::calculateDeterminant()
284 const double subDet[3] =
286 _linear_transform[4] * _linear_transform[8] - _linear_transform[5] * _linear_transform[7],
287 _linear_transform[3] * _linear_transform[8] - _linear_transform[5] * _linear_transform[6],
288 _linear_transform[3] * _linear_transform[7] - _linear_transform[4] * _linear_transform[6]
291 _determinant = _linear_transform[0] * subDet[0] - _linear_transform[1] * subDet[1] + _linear_transform[2] * subDet[2];
295 /////////////////////////////////////////////////
296 /// Auxiliary methods for inverse calculation ///
297 /////////////////////////////////////////////////
301 * Calculates the LU-factorization of the matrix A (_linear_transform)
302 * and stores it in lu. Since partial pivoting is used, there are
303 * row swaps. This is represented by the index permutation vector idx : to access element
304 * (i,j) of lu, use lu[3*idx[i] + j]
306 * @param lu double[9] in which to store LU-factorization
307 * @param idx int[3] in which to store row permutation vector
309 void TetraAffineTransform::factorizeLU(double* lu, int* idx) const
311 // 3 x 3 LU factorization
313 for(int i = 0 ; i < 3 ; ++i)
318 for(int k = 0; k < 2 ; ++k)
323 double max = std::fabs(lu[3*idx[k] + k]);
327 if(std::fabs(lu[3*idx[i] + k]) > max)
329 max = fabs(lu[3*idx[i] + k]);
335 // swap rows in index vector
341 for(int j = k + 1 ; j < 3 ; ++j)
343 // l_jk = u_jk / u_kk
344 lu[3*idx[j] + k] /= lu[3*idx[k] + k];
345 for(int s = k + 1; s < 3 ; ++s)
347 // case s = k will always become zero, and then be replaced by
349 // there's no need to calculate this explicitly
351 // u_js = u_js - l_jk * u_ks
352 lu[3*idx[j] + s] -= lu[3*idx[j] + k] * lu[3*idx[k] + s] ;
359 * Solves the system Lx = b, where L is lower unit-triangular (ones on the diagonal)
361 * @param x double[3] in which the solution is stored
362 * @param lu double[9] containing the LU-factorization
363 * @param b double[3] containing the right-hand side
364 * @param idx int[3] containing the permutation vector associated with lu
367 void TetraAffineTransform::forwardSubstitution(double* x, const double* lu, const double* b, const int* idx) const
369 // divisions are not carried out explicitly because lu does not store
370 // the diagonal values of L, which are all 1
371 // Compare with backwardSubstitution()
372 x[idx[0]] = ( b[idx[0]] ); // / lu[3*idx[0]]
373 x[idx[1]] = ( b[idx[1]] - lu[3*idx[1]] * x[idx[0]] ); // / lu[3*idx[1] + 1];
374 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];
378 * Solves the system Ux = b, where U is upper-triangular
380 * @param x double[3] in which the solution is stored
381 * @param lu double[9] containing the LU-factorization
382 * @param b double[3] containing the right-hand side
383 * @param idx int[3] containing the permutation vector associated with lu
386 void TetraAffineTransform::backwardSubstitution(double* x, const double* lu, const double* b, const int* idx) const
388 x[idx[2]] = ( b[idx[2]] ) / lu[3*idx[2] + 2];
389 x[idx[1]] = ( b[idx[1]] - lu[3*idx[1] + 2] * x[idx[2]] ) / lu[3*idx[1] + 1];
390 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]];