1 /*! 2 * \file CalculiXRotationMatrix.cxx 3 * \brief 4 * \author Thomas Helfer 5 * \date 18 sept. 2017 6 * \copyright Copyright (C) 2006-2018 CEA/DEN, EDF R&D. All rights 7 * reserved. 8 * This project is publicly released under either the GNU GPL Licence 9 * or the CECILL-A licence. A copy of thoses licences are delivered 10 * with the sources of TFEL. CEA or EDF may also distribute this 11 * project under specific licensing conditions. 12 */ 13 14 #include<cmath> 15 #include"MFront/CalculiX/CalculiXRotationMatrix.hxx" 16 17 namespace calculix 18 { 19 20 tfel::math::tmatrix<3u,3u,CalculiXReal> getRotationMatrix(const CalculiXReal * const c,const CalculiXReal * const p)21 getRotationMatrix(const CalculiXReal* const c, 22 const CalculiXReal* const p) 23 { 24 using namespace tfel::math; 25 constexpr const CalculiXReal eps = CalculiXReal(1.e-10); 26 tmatrix<3u,3u,CalculiXReal> r; 27 tvector<3u,CalculiXReal> e1,e2,e3; 28 if(c[6]>=0){ 29 // carthesian transformation 30 e1 = {c[0],c[1],c[2]}; 31 e2 = {c[3],c[4],c[5]}; 32 // 33 e1 /= std::sqrt(e1[0]*e1[0]+e1[1]*e1[1]+e1[2]*e1[2]); 34 e2 /= std::sqrt(e2[0]*e2[0]+e2[1]*e2[1]+e2[2]*e2[2]); 35 e3 = {e1[1]*e2[2]-e2[1]*e1[2], 36 e1[2]*e2[0]-e1[0]*e2[2], 37 e1[0]*e2[1]-e2[0]*e1[1]}; 38 } else { 39 // 40 // cylindrical coordinate system in point p 41 // 42 e1[0]=p[0]-c[0]; 43 e1[1]=p[1]-c[1]; 44 e1[2]=p[2]-c[2]; 45 // 46 e3[0]=c[3]-c[0]; 47 e3[1]=c[4]-c[1]; 48 e3[2]=c[5]-c[2]; 49 e3 /= std::sqrt(e3[0]*e3[0]+e3[1]*e3[1]+e3[2]*e3[2]); 50 const auto dd = e1[0]*e3[0]+e1[1]*e3[1]+e1[2]*e3[2]; 51 e1 -= dd*e3; 52 auto dd2 = std::sqrt(e1[0]*e1[0]+e1[1]*e1[1]+e1[2]*e1[2]); 53 // check whether p belongs to the cylindrical coordinate axis 54 // if so, an arbitrary vector perpendicular to the axis can 55 // be taken 56 if(dd2<=1.e-10){ 57 if(std::abs(e3[0])>=eps){ 58 e1[1]=1; 59 e1[2]=0; 60 e1[0]=-e3[1]/e3[0]; 61 } else if(std::abs(e3[1])>=eps){ 62 e1[2]=1; 63 e1[0]=0; 64 e1[1]=-e3[2]/e3[1]; 65 } else { 66 e1[0]=1; 67 e1[1]=0; 68 e1[2]=-e3[0]/e3[2]; 69 } 70 dd2=std::sqrt(e1[0]*e1[0]+e1[1]*e1[1]+e1[2]*e1[2]); 71 } 72 e1 /= dd2; 73 e2[0]=e3[1]*e1[2]-e1[1]*e3[2]; 74 e2[1]=e3[2]*e1(1)-e1[2]*e3[0]; 75 e2[2]=e3[0]*e1[1]-e1(1)*e3[1]; 76 // 77 } 78 r.column_view<0>() = e1; 79 r.column_view<1>() = e2; 80 r.column_view<2>() = e3; 81 return r; 82 } // end of getRotationMatrix( 83 84 } // end of namespace calculix 85