src/INTERP_KERNEL/TetraAffineTransform.cxx

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