Back to index

salome-med  6.5.0
TetraAffineTransform.cxx
Go to the documentation of this file.
00001 // Copyright (C) 2007-2012  CEA/DEN, EDF R&D
00002 //
00003 // This library is free software; you can redistribute it and/or
00004 // modify it under the terms of the GNU Lesser General Public
00005 // License as published by the Free Software Foundation; either
00006 // version 2.1 of the License.
00007 //
00008 // This library is distributed in the hope that it will be useful,
00009 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00010 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00011 // Lesser General Public License for more details.
00012 //
00013 // You should have received a copy of the GNU Lesser General Public
00014 // License along with this library; if not, write to the Free Software
00015 // Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA
00016 //
00017 // See http://www.salome-platform.org/ or email : webmaster.salome@opencascade.com
00018 //
00019 
00020 #include "TetraAffineTransform.hxx"
00021 #include "VectorUtils.hxx"
00022 
00023 #include <cmath>
00024 #include <cstring>
00025 #include <iostream>
00026 
00027 #include "Log.hxx"
00028 
00029 namespace INTERP_KERNEL
00030 {
00034 
00043   TetraAffineTransform::TetraAffineTransform(const double** pts)
00044   {
00045     
00046     LOG(2,"Creating transform from tetraeder : ");
00047     LOG(2, vToStr(pts[0]) << ", " << vToStr(pts[1]) << ", " << vToStr(pts[2]) << ", " << vToStr(pts[3]));
00048     
00049     // three last points -> linear transform
00050     for(int i = 0; i < 3 ; ++i)
00051       {
00052         for(int j = 0 ; j < 3 ; ++j)
00053           {
00054             // NB we insert columns, not rows
00055             _linear_transform[3*j + i] = (pts[i+1])[j] - (pts[0])[j];
00056           }
00057       }
00058 
00059     // remember _linear_transform for the reverse transformation
00060     memcpy( _back_linear_transform, _linear_transform, 9*sizeof(double));
00061     memcpy( _back_translation,      pts[0],          3*sizeof(double));
00062     
00063     calculateDeterminant();
00064     
00065     LOG(3, "determinant before inverse = " << _determinant);
00066     
00067     // check that tetra is non-planar -> determinant is not zero
00068     // otherwise set _determinant to zero to signal caller that transformation did not work
00069     if(epsilonEqual(_determinant, 0.0))
00070       {
00071         _determinant = 0.0;
00072         return;
00073       }
00074 
00075     // we need the inverse transform
00076     invertLinearTransform();
00077     
00078     // first point -> translation
00079     // calculate here because translation takes place in "transformed space",
00080     // or in other words b = -A*O where A is the linear transform
00081     // and O is the position vector of the point that is mapped onto the origin
00082     for(int i = 0 ; i < 3 ; ++i)
00083       {
00084         _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]) ;
00085       }
00086     
00087     // precalculate determinant (again after inversion of transform)
00088     calculateDeterminant();
00089 
00090 #ifdef INVERSION_SELF_CHECK
00091     // debugging : check that applying the inversed transform to the original points
00092     // gives us the unit tetrahedron
00093     LOG(4, "transform determinant is " << _determinant);
00094     LOG(4, "*Self-check : Applying transformation to original points ... ");
00095     for(int i = 0; i < 4 ; ++i)
00096       {
00097         double v[3];
00098         apply(v, pts[i]);
00099         LOG(4, vToStr(v))
00100           for(int j = 0; j < 3; ++j)
00101             {
00102               assert(epsilonEqual(v[j], (3*i+j == 3 || 3*i+j == 7 || 3*i+j == 11 ) ? 1.0 : 0.0));
00103             }
00104       }
00105     
00106     LOG(4, " ok");
00107 #endif
00108   }
00109 
00119   void TetraAffineTransform::apply(double* destPt, const double* srcPt) const
00120   {
00121     double* dest = destPt;
00122     
00123     // are we self-allocating ?
00124     const bool selfAllocation = (destPt == srcPt);
00125     
00126     if(selfAllocation)
00127       {
00128         // alloc temporary memory
00129         dest = new double[3];
00130        
00131         LOG(6, "Info : Self-affectation in TetraAffineTransform::apply");
00132       }
00133     
00134     for(int i = 0 ; i < 3 ; ++i)
00135       {
00136         // matrix - vector multiplication
00137         dest[i] = _linear_transform[3*i] * srcPt[0] + _linear_transform[3*i + 1] * srcPt[1] + _linear_transform[3*i + 2] * srcPt[2];
00138        
00139         // translation
00140         dest[i] += _translation[i];
00141       }
00142 
00143     if(selfAllocation)
00144       {
00145         // copy result back to destPt
00146         for(int i = 0 ; i < 3 ; ++i)
00147           {
00148             destPt[i] = dest[i];
00149           }
00150         delete[] dest;
00151       }
00152   }
00153 
00161   void TetraAffineTransform::reverseApply(double* destPt, const double* srcPt) const
00162   {
00163     double* dest = destPt;
00164     
00165     // are we self-allocating ?
00166     const bool selfAllocation = (destPt == srcPt);
00167     
00168     if(selfAllocation)
00169       {
00170         // alloc temporary memory
00171         dest = new double[3];
00172        
00173         LOG(6, "Info : Self-affectation in TetraAffineTransform::reverseApply");
00174       }
00175     
00176     for(int i = 0 ; i < 3 ; ++i)
00177       {
00178         // matrix - vector multiplication
00179         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];
00180 
00181         // translation
00182         dest[i] += _back_translation[i];
00183       }
00184 
00185     if(selfAllocation)
00186       {
00187         // copy result back to destPt
00188         for(int i = 0 ; i < 3 ; ++i)
00189           {
00190             destPt[i] = dest[i];
00191           }
00192         delete[] dest;
00193       }
00194   }
00195 
00202   double TetraAffineTransform::determinant() const
00203   {
00204     return _determinant;
00205   }
00206 
00212   void TetraAffineTransform::dump() const
00213   {
00214     std::cout << "A = " << std::endl << "[";
00215     for(int i = 0; i < 3; ++i)
00216       {
00217         std::cout << _linear_transform[3*i] << ", " << _linear_transform[3*i + 1] << ", " << _linear_transform[3*i + 2];
00218         if(i != 2 ) std::cout << std::endl;
00219       }
00220     std::cout << "]" << std::endl;
00221     
00222     std::cout << "b = " << "[" << _translation[0] << ", " << _translation[1] << ", " << _translation[2] << "]" << std::endl;
00223   }
00224 
00228 
00234   void TetraAffineTransform::invertLinearTransform()
00235   {
00236     //{ we copy the matrix for the lu-factorization
00237     // maybe inefficient    
00238     double lu[9];
00239     for(int i = 0 ; i < 9; ++i)
00240       {
00241         lu[i] = _linear_transform[i];
00242       }
00243     
00244     // calculate LU factorization
00245     int idx[3];
00246     factorizeLU(lu, idx);
00247     
00248     // calculate inverse by forward and backward substitution
00249     // store in _linear_transform
00250     // NB _linear_transform cannot be overwritten with lu in the loop
00251     for(int i = 0 ; i < 3 ; ++i)
00252       {
00253         // form standard base vector i
00254         const double b[3] = 
00255           {
00256             int(i == 0),
00257             int(i == 1),
00258             int(i == 2)
00259           };
00260 
00261         LOG(6,  "b = [" << b[0] << ", " << b[1] << ", " << b[2] << "]");
00262        
00263         double y[3];
00264         forwardSubstitution(y, lu, b, idx);
00265        
00266         double x[3];
00267         backwardSubstitution(x, lu, y, idx);
00268        
00269         // copy to _linear_transform matrix
00270         // NB : this is a column operation, so we cannot 
00271         // do this directly when we calculate x
00272         for(int j = 0 ; j < 3 ; j++)
00273           {
00274             _linear_transform[3*j + i] = x[idx[j]];
00275           }
00276       }
00277   }
00278   
00283   void TetraAffineTransform::calculateDeterminant()
00284   {
00285     const double subDet[3] = 
00286       {
00287         _linear_transform[4] * _linear_transform[8] - _linear_transform[5] * _linear_transform[7],
00288         _linear_transform[3] * _linear_transform[8] - _linear_transform[5] * _linear_transform[6],
00289         _linear_transform[3] * _linear_transform[7] - _linear_transform[4] * _linear_transform[6]
00290       };
00291 
00292     _determinant = _linear_transform[0] * subDet[0] - _linear_transform[1] * subDet[1] + _linear_transform[2] * subDet[2]; 
00293   }
00294 
00295   
00299 
00300 
00310   void TetraAffineTransform::factorizeLU(double* lu, int* idx) const
00311   {
00312     // 3 x 3 LU factorization
00313     // initialise idx
00314     for(int i = 0 ; i < 3 ; ++i)
00315       {
00316         idx[i] = i;
00317       }
00318             
00319     for(int k = 0; k < 2 ; ++k)
00320       {
00321          
00322         // find pivot
00323         int i = k;
00324         double max = std::fabs(lu[3*idx[k] + k]);
00325         int row = i;
00326         while(i < 3)
00327           {
00328             if(std::fabs(lu[3*idx[i] + k]) > max)
00329               {
00330                 max = fabs(lu[3*idx[i] + k]);
00331                 row = i;
00332               }
00333             ++i;
00334           }
00335              
00336         // swap rows in index vector
00337         int tmp = idx[k];
00338         idx[k] = idx[row];
00339         idx[row] = tmp;
00340       
00341         // calculate row
00342         for(int j = k + 1 ; j < 3 ; ++j)
00343           {
00344             // l_jk = u_jk / u_kk
00345             lu[3*idx[j] + k] /= lu[3*idx[k] + k];
00346             for(int s = k + 1; s < 3 ; ++s)
00347               {
00348                 // case s = k will always become zero, and then be replaced by
00349                 // the l-value
00350                 // there's no need to calculate this explicitly
00351 
00352                 // u_js = u_js - l_jk * u_ks
00353                 lu[3*idx[j] + s] -= lu[3*idx[j] + k] * lu[3*idx[k] + s] ;
00354               }
00355           }
00356       }
00357   }
00358 
00368   void TetraAffineTransform::forwardSubstitution(double* x, const double* lu, const double* b, const int* idx) const
00369   {
00370     // divisions are not carried out explicitly because lu does not store
00371     // the diagonal values of L, which are all 1
00372     // Compare with backwardSubstitution()
00373     x[idx[0]] = ( b[idx[0]] ); // / lu[3*idx[0]]
00374     x[idx[1]] = ( b[idx[1]] - lu[3*idx[1]] * x[idx[0]] ); // / lu[3*idx[1] + 1];
00375     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];
00376   }
00377 
00387   void TetraAffineTransform::backwardSubstitution(double* x, const double* lu, const double* b, const int* idx) const
00388   {
00389     x[idx[2]] = ( b[idx[2]] ) / lu[3*idx[2] + 2];
00390     x[idx[1]] = ( b[idx[1]] - lu[3*idx[1] + 2] * x[idx[2]] ) / lu[3*idx[1] + 1];
00391     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]];
00392   }
00393   
00394 }