src/INTERP_KERNEL/TetraAffineTransform.cxx

   1 // Copyright (C) 2007-2015  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     // 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))
  69       {
  70         _determinant = 0.0;
  71         return;
  72       }
  73
  74     // we need the inverse transform
  75     invertLinearTransform();
  76
  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)
  82       {
  83         _translation[i] = -(_linear_transform[3*i]*pts[0] + _linear_transform[3*i+1]*pts[1] + _linear_transform[3*i+2]*pts[2]) ;
  84       }
  85
  86     // precalculate determinant (again after inversion of transform)
  87     calculateDeterminant();
  88
  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)
  95       {
  96         double v[3];
  97         apply(v, pts+3*i);
  98         LOG(4, vToStr(v))
  99           for(int j = 0; j < 3; ++j)
 100             {
 101               assert(epsilonEqual(v[j], (3*i+j == 3 || 3*i+j == 7 || 3*i+j == 11 ) ? 1.0 : 0.0));
 102             }
 103       }
 104
 105     LOG(4, " ok");
 106 #endif
 107   }
 108
 109   /**
 110    * Calculates the transform of point srcPt and stores the result in destPt.
 111    * If destPt == srcPt, then srcPt is overwritten safely.
 112    *
 113    *
 114    * @param destPt  double[3] in which to store the transformed point
 115    * @param srcPt   double[3] containing coordinates of points to be transformed
 116    *
 117    */
 118   void TetraAffineTransform::apply(double* destPt, const double* srcPt) const
 119   {
 120     double* dest = destPt;
 121
 122     // are we self-allocating ?
 123     const bool selfAllocation = (destPt == srcPt);
 124
 125     if(selfAllocation)
 126       {
 127         // alloc temporary memory
 128         dest = new double[3];
 129
 130         LOG(6, "Info : Self-affectation in TetraAffineTransform::apply");
 131       }
 132
 133     for(int i = 0 ; i < 3 ; ++i)
 134       {
 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];
 137
 138         // translation
 139         dest[i] += _translation[i];
 140       }
 141
 142     if(selfAllocation)
 143       {
 144         // copy result back to destPt
 145         for(int i = 0 ; i < 3 ; ++i)
 146           {
 147             destPt[i] = dest[i];
 148           }
 149         delete[] dest;
 150       }
 151   }
 152
 153   /**
 154    * Calculates the reverse transform of point srcPt and stores the result in destPt.
 155    * If destPt == srcPt, then srcPt is overwritten safely.
 156    *
 157    * @param destPt  double[3] in which to store the transformed point
 158    * @param srcPt   double[3] containing coordinates of points to be transformed
 159    */
 160   void TetraAffineTransform::reverseApply(double* destPt, const double* srcPt) const
 161   {
 162     double* dest = destPt;
 163
 164     // are we self-allocating ?
 165     const bool selfAllocation = (destPt == srcPt);
 166
 167     if(selfAllocation)
 168       {
 169         // alloc temporary memory
 170         dest = new double[3];
 171
 172         LOG(6, "Info : Self-affectation in TetraAffineTransform::reverseApply");
 173       }
 174
 175     for(int i = 0 ; i < 3 ; ++i)
 176       {
 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];
 179
 180         // translation
 181         dest[i] += _back_translation[i];
 182       }
 183
 184     if(selfAllocation)
 185       {
 186         // copy result back to destPt
 187         for(int i = 0 ; i < 3 ; ++i)
 188           {
 189             destPt[i] = dest[i];
 190           }
 191         delete[] dest;
 192       }
 193   }
 194
 195   /**
 196    * Returns the determinant of the linear part A of the transform.
 197    *
 198    * @return determinant of the transform
 199    *
 200    */
 201   double TetraAffineTransform::determinant() const
 202   {
 203     return _determinant;
 204   }
 205
 206   /**
 207    * Outputs  to std::cout the matrix A and the vector b
 208    * of the transform Ax + b
 209    *
 210    */
 211   void TetraAffineTransform::dump() const
 212   {
 213     std::cout << "A = " << std::endl << "[";
 214     for(int i = 0; i < 3; ++i)
 215       {
 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;
 218       }
 219     std::cout << "]" << std::endl;
 220
 221     std::cout << "b = " << "[" << _translation[0] << ", " << _translation[1] << ", " << _translation[2] << "]" << std::endl;
 222   }
 223
 224   /////////////////////////////////////////////////////////////////////////////////////////
 225   /// PRIVATE  METHODS                                                       //////////////
 226   /////////////////////////////////////////////////////////////////////////////////////////
 227
 228   /**
 229    * Calculates the inverse of the matrix A, stored in _linear_transform
 230    * by LU-factorization and substitution
 231    *
 232    */
 233   void TetraAffineTransform::invertLinearTransform()
 234   {
 235     //{ we copy the matrix for the lu-factorization
 236     // maybe inefficient
 237     double lu[9];
 238     for(int i = 0 ; i < 9; ++i)
 239       {
 240         lu[i] = _linear_transform[i];
 241       }
 242
 243     // calculate LU factorization
 244     int idx[3];
 245     factorizeLU(lu, idx);
 246
 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)
 251       {
 252         // form standard base vector i
 253         const double b[3] =
 254           {
 255             double ( int(i == 0) ),
 256             double ( int(i == 1) ),
 257             double ( int(i == 2) ),
 258           };
 259
 260         LOG(6,  "b = [" << b[0] << ", " << b[1] << ", " << b[2] << "]");
 261
 262         double y[3];
 263         forwardSubstitution(y, lu, b, idx);
 264
 265         double x[3];
 266         backwardSubstitution(x, lu, y, idx);
 267
 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++)
 272           {
 273             _linear_transform[3*j + i] = x[idx[j]];
 274           }
 275       }
 276   }
 277
 278   /**
 279    * Updates the member _determinant of the matrix A of the transformation.
 280    *
 281    */
 282   void TetraAffineTransform::calculateDeterminant()
 283   {
 284     const double subDet[3] =
 285       {
 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]
 289       };
 290
 291     _determinant = _linear_transform[0] * subDet[0] - _linear_transform[1] * subDet[1] + _linear_transform[2] * subDet[2];
 292   }
 293
 294
 295   /////////////////////////////////////////////////
 296   /// Auxiliary methods for inverse calculation ///
 297   /////////////////////////////////////////////////
 298
 299
 300   /**
 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]
 305    *
 306    * @param lu double[9] in which to store LU-factorization
 307    * @param idx int[3] in which to store row permutation vector
 308    */
 309   void TetraAffineTransform::factorizeLU(double* lu, int* idx) const
 310   {
 311     // 3 x 3 LU factorization
 312     // initialise idx
 313     for(int i = 0 ; i < 3 ; ++i)
 314       {
 315         idx[i] = i;
 316       }
 317
 318     for(int k = 0; k < 2 ; ++k)
 319       {
 320
 321         // find pivot
 322         int i = k;
 323         double max = std::fabs(lu[3*idx[k] + k]);
 324         int row = i;
 325         while(i < 3)
 326           {
 327             if(std::fabs(lu[3*idx[i] + k]) > max)
 328               {
 329                 max = fabs(lu[3*idx[i] + k]);
 330                 row = i;
 331               }
 332             ++i;
 333           }
 334
 335         // swap rows in index vector
 336         int tmp = idx[k];
 337         idx[k] = idx[row];
 338         idx[row] = tmp;
 339
 340         // calculate row
 341         for(int j = k + 1 ; j < 3 ; ++j)
 342           {
 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)
 346               {
 347                 // case s = k will always become zero, and then be replaced by
 348                 // the l-value
 349                 // there's no need to calculate this explicitly
 350
 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] ;
 353               }
 354           }
 355       }
 356   }
 357
 358   /**
 359    * Solves the system Lx = b, where L is lower unit-triangular (ones on the diagonal)
 360    *
 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
 365    *
 366    */
 367   void TetraAffineTransform::forwardSubstitution(double* x, const double* lu, const double* b, const int* idx) const
 368   {
 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];
 375   }
 376
 377   /**
 378    * Solves the system Ux = b, where U is upper-triangular
 379    *
 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
 384    *
 385    */
 386   void TetraAffineTransform::backwardSubstitution(double* x, const double* lu, const double* b, const int* idx) const
 387   {
 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]];
 391   }
 392
 393 }