MBSim

Project Page
rotarymatrices.h
00001 /* Copyright (C) 2004-2012  Robert Huber
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, or (at your option) any later version. 
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., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301 USA
00016 
00017  *
00018  * Contact:
00019  *   rzander@users.berlios.de
00020  *
00021  */
00022 #ifndef ROTARYMATRICES_H
00023 #define ROTARYMATRICES_H
00024 #include "fmatvec/fmatvec.h"
00025 #include "mbsim/utils/utils.h"
00026 
00027 namespace MBSim {
00028 
00036   fmatvec::SqrMat BasicRotAKIx(double phi);
00037   fmatvec::SqrMat BasicRotAKIy(double phi);
00038   fmatvec::SqrMat BasicRotAKIz(double phi);
00039 
00040   /*
00041    *  \brief Rotate CoSy K (angle phi) to obtain CoSy I
00042    */
00043   fmatvec::SqrMat BasicRotAIKx(double phi);
00044   fmatvec::SqrMat BasicRotAIKy(double phi);
00045   fmatvec::SqrMat BasicRotAIKz(double phi);
00046 
00050   template<class Row>
00051   fmatvec::Vector<Row,double> AIK2Cardan(const fmatvec::SquareMatrix<Row,double> &AIK) { 
00052     fmatvec::Vector<Row,double> AlphaBetaGamma(AIK.size(),fmatvec::NONINIT);    
00053     AlphaBetaGamma(1)= asin(AIK(0,2));
00054     double nenner = cos(AlphaBetaGamma(1));
00055     if (fabs(nenner)>1e-10) {
00056       AlphaBetaGamma(0) = atan2(-AIK(1,2),AIK(2,2));
00057       AlphaBetaGamma(2) = atan2(-AIK(0,1),AIK(0,0));
00058     } else {
00059       AlphaBetaGamma(0)=0;
00060       AlphaBetaGamma(2)=atan2(AIK(1,0),AIK(1,1));
00061     }
00062     return AlphaBetaGamma;
00063   }
00064 
00065   template<class Row>
00066   fmatvec::Vector<Row,double> AKI2Cardan(const fmatvec::SquareMatrix<Row,double> &AKI) {
00067     return AIK2Cardan(trans(AKI));
00068   }
00069 
00073   template<class Row>
00074   fmatvec::Vector<Row,double> AIK2RevCardan(const fmatvec::SquareMatrix<Row,double> &AIK) {
00075     fmatvec::Vector<Row,double> AlphaBetaGamma(AIK.size(),fmatvec::NONINIT);
00076     AlphaBetaGamma(1)= asin(-AIK(2,0));
00077     double nenner = cos(AlphaBetaGamma(1));
00078     if (fabs(nenner)>1e-10) {
00079       AlphaBetaGamma(0) = atan2(AIK(2,1),AIK(2,2));
00080       AlphaBetaGamma(2) = ArcTan(AIK(0,0),AIK(1,0));
00081     } else {
00082       AlphaBetaGamma(0)=0;
00083       AlphaBetaGamma(2)=atan2(-AIK(0,1),AIK(1,1));
00084     }
00085     return AlphaBetaGamma;
00086   }
00087 
00088   template<class Row>
00089   fmatvec::Vector<Row,double> AKI2RevCardan(const fmatvec::SquareMatrix<Row,double> &AKI) {
00090     return AIK2RevCardan(trans(AKI));
00091   }
00092 
00093 
00099   fmatvec::Vec AIK2ParametersZXY(const fmatvec::SqrMat &AIK);
00100 
00104   fmatvec::Vec calcParametersDotZXY(const fmatvec::SqrMat &AIK, const fmatvec::Vec &KomegaK);
00105 
00109   fmatvec::SqrMat Cardan2AIK(double alpha,double beta,double gamma);
00110 
00114   fmatvec::SqrMat Euler2AIK(double psi, double theta, double phi);
00115 }
00116 
00117 #endif