src/INTERP_KERNEL/TetraAffineTransform.cxx

   1 // Copyright (C) 2007-2020  CEA/DEN, EDF R&D
   2 //
   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.
   7 //
   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.
  12 //
  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
  16 //
  17 // See http://www.salome-platform.org/ or email : webmaster.salome@opencascade.com
  18 //
  19
  20 #include "TetraAffineTransform.hxx"
  21 #include "VectorUtils.hxx"
  22
  23 #include <cmath>
  24 #include <cstring>
  25 #include <iostream>
  26
  27 #include "Log.hxx"
  28
  29 namespace INTERP_KERNEL
  30 {
  31   /////////////////////////////////////////////////////////////////////////////////////////
  32   /// PUBLIC INTERFACE METHODS                                               //////////////
  33   /////////////////////////////////////////////////////////////////////////////////////////
  34
  35   /**
  36    * Constructor
  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.
  40    *
  41    * @param pts  a 4x3 matrix containing 4 points (P0X,P0Y,P0Z,P1X,P1Y,P1Z,...) of 3 coordinates each
  42    */
  43   TetraAffineTransform::TetraAffineTransform(const double *pts)
  44   {
  45
  46     LOG(2,"Creating transform from tetraeder : ");
  47
  48     // three last points -> linear transform
  49     for(int i = 0; i < 3 ; ++i)
  50       {
  51         for(int j = 0 ; j < 3 ; ++j)
  52           {
  53             // NB we insert columns, not rows
  54             _linear_transform[3*j + i] = pts[(i+1)*3+j] - pts[j];
  55           }
  56       }
  57
  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));
  61
  62     calculateDeterminant();
  63
  64     LOG(3, "determinant before inverse = " << _determinant);
  65
  66     double ni(1./INTERP_KERNEL::normInf(_linear_transform));
  67     ni = ni*ni*ni;
  68
  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))
  73       {
  74         _determinant = 0.0;
  75         return;
  76       }
  77
  78     // we need the inverse transform
  79     invertLinearTransform();
  80
  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)
  86       {
  87         _translation[i] = -(_linear_transform[3*i]*pts[0] + _linear_transform[3*i+1]*pts[1] + _linear_transform[3*i+2]*pts[2]) ;
  88       }
  89
  90     // precalculate determinant (again after inversion of transform)
  91     calculateDeterminant();
  92
  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)
  99       {
 100         double v[3];
 101         apply(v, pts+3*i);
 102         LOG(4, vToStr(v))
 103           for(int j = 0; j < 3; ++j)
 104             {
 105               assert(epsilonEqual(v[j], (3*i+j == 3 || 3*i+j == 7 || 3*i+j == 11 ) ? 1.0 : 0.0));
 106             }
 107       }
 108
 109     LOG(4, " ok");
 110 #endif
 111   }
 112
 113   /**
 114    * Calculates the transform of point srcPt and stores the result in destPt.
 115    * If destPt == srcPt, then srcPt is overwritten safely.
 116    *
 117    *
 118    * @param destPt  double[3] in which to store the transformed point
 119    * @param srcPt   double[3] containing coordinates of points to be transformed
 120    *
 121    */
 122   void TetraAffineTransform::apply(double* destPt, const double* srcPt) const
 123   {
 124     double* dest = destPt;
 125
 126     // are we self-allocating ?
 127     const bool selfAllocation = (destPt == srcPt);
 128
 129     if(selfAllocation)
 130       {
 131         // alloc temporary memory
 132         dest = new double[3];
 133
 134         LOG(6, "Info : Self-affectation in TetraAffineTransform::apply");
 135       }
 136
 137     for(int i = 0 ; i < 3 ; ++i)
 138       {
 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];
 141
 142         // translation
 143         dest[i] += _translation[i];
 144       }
 145
 146     if(selfAllocation)
 147       {
 148         // copy result back to destPt
 149         for(int i = 0 ; i < 3 ; ++i)
 150           {
 151             destPt[i] = dest[i];
 152           }
 153         delete[] dest;
 154       }
 155   }
 156
 157   /**
 158    * Calculates the reverse transform of point srcPt and stores the result in destPt.
 159    * If destPt == srcPt, then srcPt is overwritten safely.
 160    *
 161    * @param destPt  double[3] in which to store the transformed point
 162    * @param srcPt   double[3] containing coordinates of points to be transformed
 163    */
 164   void TetraAffineTransform::reverseApply(double* destPt, const double* srcPt) const
 165   {
 166     double* dest = destPt;
 167
 168     // are we self-allocating ?
 169     const bool selfAllocation = (destPt == srcPt);
 170
 171     if(selfAllocation)
 172       {
 173         // alloc temporary memory
 174         dest = new double[3];
 175
 176         LOG(6, "Info : Self-affectation in TetraAffineTransform::reverseApply");
 177       }
 178
 179     for(int i = 0 ; i < 3 ; ++i)
 180       {
 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];
 183
 184         // translation
 185         dest[i] += _back_translation[i];
 186       }
 187
 188     if(selfAllocation)
 189       {
 190         // copy result back to destPt
 191         for(int i = 0 ; i < 3 ; ++i)
 192           {
 193             destPt[i] = dest[i];
 194           }
 195         delete[] dest;
 196       }
 197   }
 198
 199   /**
 200    * Returns the determinant of the linear part A of the transform.
 201    *
 202    * @return determinant of the transform
 203    *
 204    */
 205   double TetraAffineTransform::determinant() const
 206   {
 207     return _determinant;
 208   }
 209
 210   /**
 211    * Outputs  to std::cout the matrix A and the vector b
 212    * of the transform Ax + b
 213    *
 214    */
 215   void TetraAffineTransform::dump() const
 216   {
 217     std::cout << "A = " << std::endl << "[";
 218     for(int i = 0; i < 3; ++i)
 219       {
 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;
 222       }
 223     std::cout << "]" << std::endl;
 224
 225     std::cout << "b = " << "[" << _translation[0] << ", " << _translation[1] << ", " << _translation[2] << "]" << std::endl;
 226   }
 227
 228   /////////////////////////////////////////////////////////////////////////////////////////
 229   /// PRIVATE  METHODS                                                       //////////////
 230   /////////////////////////////////////////////////////////////////////////////////////////
 231
 232   /**
 233    * Calculates the inverse of the matrix A, stored in _linear_transform
 234    * by LU-factorization and substitution
 235    *
 236    */
 237   void TetraAffineTransform::invertLinearTransform()
 238   {
 239     //{ we copy the matrix for the lu-factorization
 240     // maybe inefficient
 241     double lu[9];
 242     for(int i = 0 ; i < 9; ++i)
 243       {
 244         lu[i] = _linear_transform[i];
 245       }
 246
 247     // calculate LU factorization
 248     int idx[3];
 249     factorizeLU(lu, idx);
 250
 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)
 255       {
 256         // form standard base vector i
 257         const double b[3] =
 258           {
 259             double ( int(i == 0) ),
 260             double ( int(i == 1) ),
 261             double ( int(i == 2) ),
 262           };
 263
 264         LOG(6,  "b = [" << b[0] << ", " << b[1] << ", " << b[2] << "]");
 265
 266         double y[3];
 267         forwardSubstitution(y, lu, b, idx);
 268
 269         double x[3];
 270         backwardSubstitution(x, lu, y, idx);
 271
 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++)
 276           {
 277             _linear_transform[3*j + i] = x[idx[j]];
 278           }
 279       }
 280   }
 281
 282   /**
 283    * Updates the member _determinant of the matrix A of the transformation.
 284    *
 285    */
 286   void TetraAffineTransform::calculateDeterminant()
 287   {
 288     const double subDet[3] =
 289       {
 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]
 293       };
 294
 295     _determinant = _linear_transform[0] * subDet[0] - _linear_transform[1] * subDet[1] + _linear_transform[2] * subDet[2];
 296   }
 297
 298
 299   /////////////////////////////////////////////////
 300   /// Auxiliary methods for inverse calculation ///
 301   /////////////////////////////////////////////////
 302
 303
 304   /**
 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]
 309    *
 310    * @param lu double[9] in which to store LU-factorization
 311    * @param idx int[3] in which to store row permutation vector
 312    */
 313   void TetraAffineTransform::factorizeLU(double* lu, int* idx) const
 314   {
 315     // 3 x 3 LU factorization
 316     // initialise idx
 317     for(int i = 0 ; i < 3 ; ++i)
 318       {
 319         idx[i] = i;
 320       }
 321
 322     for(int k = 0; k < 2 ; ++k)
 323       {
 324
 325         // find pivot
 326         int i = k;
 327         double max = std::fabs(lu[3*idx[k] + k]);
 328         int row = i;
 329         while(i < 3)
 330           {
 331             if(std::fabs(lu[3*idx[i] + k]) > max)
 332               {
 333                 max = fabs(lu[3*idx[i] + k]);
 334                 row = i;
 335               }
 336             ++i;
 337           }
 338
 339         // swap rows in index vector
 340         int tmp = idx[k];
 341         idx[k] = idx[row];
 342         idx[row] = tmp;
 343
 344         // calculate row
 345         for(int j = k + 1 ; j < 3 ; ++j)
 346           {
 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)
 350               {
 351                 // case s = k will always become zero, and then be replaced by
 352                 // the l-value
 353                 // there's no need to calculate this explicitly
 354
 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] ;
 357               }
 358           }
 359       }
 360   }
 361
 362   /**
 363    * Solves the system Lx = b, where L is lower unit-triangular (ones on the diagonal)
 364    *
 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
 369    *
 370    */
 371   void TetraAffineTransform::forwardSubstitution(double* x, const double* lu, const double* b, const int* idx) const
 372   {
 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];
 379   }
 380
 381   /**
 382    * Solves the system Ux = b, where U is upper-triangular
 383    *
 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
 388    *
 389    */
 390   void TetraAffineTransform::backwardSubstitution(double* x, const double* lu, const double* b, const int* idx) const
 391   {
 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]];
 395   }
 396
 397 }