src/INTERP_KERNEL/TranslationRotationMatrix.hxx

   1 // Copyright (C) 2007-2021  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 #ifndef __TRANSLATIONROTATIONMATRIX_HXX__
  21 #define __TRANSLATIONROTATIONMATRIX_HXX__
  22
  23 #include "INTERPKERNELDefines.hxx"
  24
  25 #include <cmath>
  26
  27 namespace INTERP_KERNEL
  28 {
  29   class INTERPKERNEL_EXPORT TranslationRotationMatrix
  30   {
  31
  32   public:
  33
  34     TranslationRotationMatrix()
  35     {
  36       unsigned i;
  37       for(i=0;i<TRANSL_SIZE;i++)
  38         _translation_coeffs[i]=0.;
  39       for(i=0;i<ROT_SIZE;i++)
  40         _rotation_coeffs[i]=i%4?0.:1.;
  41     }
  42
  43     void multiply(const TranslationRotationMatrix& A)
  44     {
  45       TranslationRotationMatrix result;
  46       //setting output matrix to zero
  47       for (int i=0; i<3; i++)
  48         result._rotation_coeffs[i*4]=0.0;
  49       //multiplying
  50       for (int i=0; i<3;i++)
  51         for (int j=0; j<3; j++)
  52           for (int k=0; k<3; k++)
  53             result._rotation_coeffs[j+i*3]+=A._rotation_coeffs[3*i+k]*_rotation_coeffs[j+k*3];
  54
  55       for (int i=0;i<9; i++)
  56         _rotation_coeffs[i]=result._rotation_coeffs[i];
  57     }
  58
  59     void rotate_vector(double* P)
  60     {
  61       double temp[3]={0.0, 0.0, 0.0};
  62
  63       for (int i=0; i<3;i++)
  64         for (int j=0; j<3; j++)
  65           temp[i] +=_rotation_coeffs[3*i+j]*P[j];
  66
  67       P[0]=temp[0];P[1]=temp[1];P[2]=temp[2];
  68     }
  69
  70     void transform_vector(double*P)
  71     {
  72       P[0]+=_translation_coeffs[0];
  73       P[1]+=_translation_coeffs[1];
  74       P[2]+=_translation_coeffs[2];
  75       rotate_vector(P);
  76     }
  77
  78     void translate(const double* P)
  79     {
  80       _translation_coeffs[0]=P[0];
  81       _translation_coeffs[1]=P[1];
  82       _translation_coeffs[2]=P[2];
  83     }
  84
  85     void  rotate_x (double* P)
  86     {
  87       _rotation_coeffs[0]=1.0;
  88       double r_sqr = P[1]*P[1]+P[2]*P[2];
  89       if (r_sqr < EPS)
  90         {_rotation_coeffs[4]=1.0; _rotation_coeffs[8]=1.0; return;}
  91       double r = sqrt(r_sqr);
  92       double cos =P[1]/r;
  93       double sin =P[2]/r;
  94
  95       _rotation_coeffs[4]=cos;
  96       _rotation_coeffs[5]=sin;
  97       _rotation_coeffs[7]=-sin;
  98       _rotation_coeffs[8]=cos;
  99
 100
 101       rotate_vector(P);
 102     }
 103
 104     void  rotate_z (double* P)
 105     {
 106       _rotation_coeffs[8]=1.0;
 107       double r_sqr = P[0]*P[0]+P[1]*P[1];
 108       if (r_sqr < EPS)
 109         {_rotation_coeffs[4]=1.0; _rotation_coeffs[0]=1.0; return;}
 110       double r = sqrt(r_sqr);
 111       double cos =P[0]/r;
 112       double sin =P[1]/r;
 113
 114       _rotation_coeffs[0]=cos;
 115       _rotation_coeffs[1]=sin;
 116       _rotation_coeffs[3]=-sin;
 117       _rotation_coeffs[4]=cos;
 118
 119       rotate_vector(P);
 120     }
 121
 122     /**
 123      * Fills in rotation_matrix so that the triangle PP1, PP2, PP3 is in the plane Oxy, with PP1 at the origin and PP2 on Ox.
 124      */
 125     static void Rotate3DTriangle(const double* PP1,const double*PP2,const double*PP3, TranslationRotationMatrix& rotation_matrix)
 126     {
 127       double P1w[3];
 128       double P2w[3];
 129       double P3w[3];
 130       P2w[0]=PP2[0]; P2w[1]=PP2[1];P2w[2]=PP2[2];
 131       P3w[0]=PP3[0]; P3w[1]=PP3[1];P3w[2]=PP3[2];
 132
 133       // translating to set P1 at the origin
 134       for (int i=0; i<3; i++)
 135         {
 136           P1w[i] = -PP1[i];
 137           P2w[i] -= PP1[i];
 138           P3w[i] -= PP1[i];
 139         }
 140
 141       //initializes matrix
 142       rotation_matrix.translate(P1w);
 143
 144       // rotating to set P2 on the Oxy plane
 145       TranslationRotationMatrix A;
 146       A.rotate_x(P2w);
 147       A.rotate_vector(P3w);
 148       rotation_matrix.multiply(A);
 149
 150       //rotating to set P2 on the Ox axis
 151       TranslationRotationMatrix B;
 152       B.rotate_z(P2w);
 153       B.rotate_vector(P3w);
 154       rotation_matrix.multiply(B);
 155
 156       //rotating to set P3 on the Oxy plane
 157       TranslationRotationMatrix C;
 158       C.rotate_x(P3w);
 159       rotation_matrix.multiply(C);
 160     }
 161
 162     /**
 163      * Fills in rotation_matrix so that the bipoint PP1, PP2 is on the Ox axis.
 164      */
 165     static void Rotate3DBipoint(const double* PP1,const double*PP2, TranslationRotationMatrix& rotation_matrix)
 166     {
 167       double P1w[3];
 168       double P2w[3];
 169       P2w[0]=PP2[0]; P2w[1]=PP2[1];P2w[2]=PP2[2];
 170
 171       // translating to set P1 at the origin
 172       for (int i=0; i<3; i++)
 173         {
 174           P1w[i] = -PP1[i];
 175           P2w[i] -= PP1[i];
 176         }
 177
 178       //initializes
 179       rotation_matrix.translate(P1w);
 180
 181       // rotating to set P2 on the Oxy plane
 182       TranslationRotationMatrix A;
 183       A.rotate_x(P2w);
 184       rotation_matrix.multiply(A);
 185
 186       //rotating to set P2 on the Ox axis
 187       TranslationRotationMatrix B;
 188       B.rotate_z(P2w);
 189       rotation_matrix.multiply(B);
 190     }
 191
 192
 193
 194   private:
 195     static const double EPS;
 196     static const unsigned ROT_SIZE=9;
 197     static const unsigned TRANSL_SIZE=3;
 198     double _rotation_coeffs[ROT_SIZE];
 199     double _translation_coeffs[TRANSL_SIZE];
 200   };
 201 }
 202
 203 #endif