Back to index

salome-med  6.5.0
TranslationRotationMatrix.hxx
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 #ifndef __TRANSLATIONROTATIONMATRIX_HXX__
00021 #define __TRANSLATIONROTATIONMATRIX_HXX__
00022 
00023 #include "INTERPKERNELDefines.hxx"
00024 
00025 #include <cmath>
00026 
00027 namespace INTERP_KERNEL
00028 {
00029   class INTERPKERNEL_EXPORT TranslationRotationMatrix
00030   {
00031 
00032   public:
00033 
00034     TranslationRotationMatrix()
00035     { 
00036       unsigned i;
00037       for(i=0;i<TRANSL_SIZE;i++)
00038         _translation_coeffs[i]=0.;
00039       for(i=0;i<ROT_SIZE;i++)
00040         _rotation_coeffs[i]=i%4?0.:1.;
00041     }
00042 
00043     void multiply(const TranslationRotationMatrix& A)
00044     {
00045       TranslationRotationMatrix result;
00046       //setting output matrix to zero
00047       for (int i=0; i<3; i++)
00048         result._rotation_coeffs[i*4]=0.0;
00049       //multiplying
00050       for (int i=0; i<3;i++)
00051         for (int j=0; j<3; j++)
00052           for (int k=0; k<3; k++)
00053             result._rotation_coeffs[j+i*3]+=A._rotation_coeffs[3*i+k]*_rotation_coeffs[j+k*3];
00054     
00055       for (int i=0;i<9; i++)
00056         _rotation_coeffs[i]=result._rotation_coeffs[i];
00057     }
00058   
00059     void rotate_vector(double* P)
00060     {
00061       double temp[3]={0.0, 0.0, 0.0};
00062     
00063       for (int i=0; i<3;i++)
00064         for (int j=0; j<3; j++)
00065           temp[i] +=_rotation_coeffs[3*i+j]*P[j];
00066        
00067       P[0]=temp[0];P[1]=temp[1];P[2]=temp[2];
00068     }
00069  
00070     void transform_vector(double*P)
00071     {
00072       P[0]+=_translation_coeffs[0];
00073       P[1]+=_translation_coeffs[1];
00074       P[2]+=_translation_coeffs[2];
00075       rotate_vector(P);
00076     }
00077 
00078     void translate(const double* P)
00079     {
00080       _translation_coeffs[0]=P[0];
00081       _translation_coeffs[1]=P[1];
00082       _translation_coeffs[2]=P[2];
00083     }
00084   
00085     void  rotate_x (double* P)
00086     {
00087       _rotation_coeffs[0]=1.0;
00088       double r_sqr = P[1]*P[1]+P[2]*P[2];
00089       if (r_sqr < EPS)
00090         {_rotation_coeffs[4]=1.0; _rotation_coeffs[8]=1.0; return;}
00091       double r = sqrt(r_sqr);
00092       double cos =P[1]/r;
00093       double sin =P[2]/r;
00094 
00095       _rotation_coeffs[4]=cos;
00096       _rotation_coeffs[5]=sin;
00097       _rotation_coeffs[7]=-sin;
00098       _rotation_coeffs[8]=cos;
00099 
00100 
00101       rotate_vector(P);
00102     }
00103 
00104     void  rotate_z (double* P)
00105     {
00106       _rotation_coeffs[8]=1.0;
00107       double r_sqr = P[0]*P[0]+P[1]*P[1];
00108       if (r_sqr < EPS)
00109         {_rotation_coeffs[4]=1.0; _rotation_coeffs[0]=1.0; return;}
00110       double r = sqrt(r_sqr);
00111       double cos =P[0]/r;
00112       double sin =P[1]/r;
00113     
00114       _rotation_coeffs[0]=cos;
00115       _rotation_coeffs[1]=sin; 
00116       _rotation_coeffs[3]=-sin;
00117       _rotation_coeffs[4]=cos;
00118     
00119       rotate_vector(P);
00120     }
00121                      
00122        
00123   private:
00124     static const double EPS;
00125     static const unsigned ROT_SIZE=9;
00126     static const unsigned TRANSL_SIZE=3;
00127     double _rotation_coeffs[ROT_SIZE];
00128     double _translation_coeffs[TRANSL_SIZE];
00129   };
00130 }
00131 
00132 #endif