00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
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
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