src/INTERP_KERNEL/TranslationRotationMatrix.hxx

   1 //  Copyright (C) 2007-2008  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 #ifndef __TRANSLATIONROTATIONMATRIX_HXX__
  20 #define __TRANSLATIONROTATIONMATRIX_HXX__
  21
  22 #include <cmath>
  23
  24 namespace INTERP_KERNEL
  25 {
  26   class TranslationRotationMatrix
  27   {
  28
  29   public:
  30
  31     TranslationRotationMatrix()
  32     {
  33       unsigned i;
  34       for(i=0;i<TRANSL_SIZE;i++)
  35         _translation_coeffs[i]=0.;
  36       for(i=0;i<ROT_SIZE;i++)
  37         _rotation_coeffs[i]=i%4?0.:1.;
  38     }
  39
  40     void multiply(const TranslationRotationMatrix& A)
  41     {
  42       TranslationRotationMatrix result;
  43       //setting output matrix to zero
  44       for (int i=0; i<3; i++)
  45         result._rotation_coeffs[i*4]=0.0;
  46       //multiplying
  47       for (int i=0; i<3;i++)
  48         for (int j=0; j<3; j++)
  49           for (int k=0; k<3; k++)
  50             result._rotation_coeffs[j+i*3]+=A._rotation_coeffs[3*i+k]*_rotation_coeffs[j+k*3];
  51
  52       for (int i=0;i<9; i++)
  53         _rotation_coeffs[i]=result._rotation_coeffs[i];
  54     }
  55
  56     void rotate_vector(double* P)
  57     {
  58       double temp[3]={0.0, 0.0, 0.0};
  59
  60       for (int i=0; i<3;i++)
  61         for (int j=0; j<3; j++)
  62           temp[i] +=_rotation_coeffs[3*i+j]*P[j];
  63
  64       P[0]=temp[0];P[1]=temp[1];P[2]=temp[2];
  65     }
  66
  67     void transform_vector(double*P)
  68     {
  69       P[0]+=_translation_coeffs[0];
  70       P[1]+=_translation_coeffs[1];
  71       P[2]+=_translation_coeffs[2];
  72       rotate_vector(P);
  73     }
  74
  75     void translate(const double* P)
  76     {
  77       _translation_coeffs[0]=P[0];
  78       _translation_coeffs[1]=P[1];
  79       _translation_coeffs[2]=P[2];
  80     }
  81
  82     void  rotate_x (double* P)
  83     {
  84       _rotation_coeffs[0]=1.0;
  85       double r_sqr = P[1]*P[1]+P[2]*P[2];
  86       if (r_sqr < EPS)
  87         {_rotation_coeffs[4]=1.0; _rotation_coeffs[8]=1.0; return;}
  88       double r = sqrt(r_sqr);
  89       double cos =P[1]/r;
  90       double sin =P[2]/r;
  91
  92       _rotation_coeffs[4]=cos;
  93       _rotation_coeffs[5]=sin;
  94       _rotation_coeffs[7]=-sin;
  95       _rotation_coeffs[8]=cos;
  96
  97
  98       rotate_vector(P);
  99     }
 100
 101     void  rotate_z (double* P)
 102     {
 103       _rotation_coeffs[8]=1.0;
 104       double r_sqr = P[0]*P[0]+P[1]*P[1];
 105       if (r_sqr < EPS)
 106         {_rotation_coeffs[4]=1.0; _rotation_coeffs[0]=1.0; return;}
 107       double r = sqrt(r_sqr);
 108       double cos =P[0]/r;
 109       double sin =P[1]/r;
 110
 111       _rotation_coeffs[0]=cos;
 112       _rotation_coeffs[1]=sin;
 113       _rotation_coeffs[3]=-sin;
 114       _rotation_coeffs[4]=cos;
 115
 116       rotate_vector(P);
 117     }
 118
 119
 120   private:
 121     static const double EPS;
 122     static const unsigned ROT_SIZE=9;
 123     static const unsigned TRANSL_SIZE=3;
 124     double _rotation_coeffs[ROT_SIZE];
 125     double _translation_coeffs[TRANSL_SIZE];
 126   };
 127   const double TranslationRotationMatrix::EPS=1e-12;
 128 }
 129
 130 #endif