diff --git a/AstroLib/Src/AsAttitudeParam_2023.cpp b/AstroLib/Src/AsAttitudeParam_2023.cpp index ac0c2cfdc360ebb26320d5cef50e36195b6bd664..cde58dbbbc41a02c4ca39882049ff9139585d0a2 100644 --- a/AstroLib/Src/AsAttitudeParam_2023.cpp +++ b/AstroLib/Src/AsAttitudeParam_2023.cpp @@ -1,4 +1,4 @@ - + #include "AsAttitudeParam_2023.h" #include "AsCoordinate.h" #include "AsMath.h" @@ -759,4 +759,37 @@ void AsMtxToEuler212(const CMatrix& mtx, CEuler& euler) euler.m_Angle1 = AsCPI-atan2(mtx[1][0], -mtx[1][2]); euler.m_Angle3 = AsCPI-atan2(mtx[0][1], mtx[2][1]); } +} + + +void AsMtxToEuler121(const CMatrix &mtx, + CEuler &euler) + CEuler &euler) +{ + // euler.m_Angle1 = atan2(mtx[0][1], -mtx[0][2]); + // euler.m_Angle2 = acos(mtx[0][0]); + // euler.m_Angle3 = atan2(mtx[1][0], mtx[2][0]); + + if (fabs(mtx[0][0]) != 1) + { + euler.m_Angle1 = atan2(mtx[0][1], -mtx[0][2]); + } + + else + { + euler.m_Angle1 = 0; + double summ = atan2(mtx[1][1], mtx[1][2]); + if (mtx[2][0] == -1) + double summ = atan2(-mtx[2][1], mtx[1][1]); + if (mtx[0][0] == -1) + { + euler.m_Angle2 = AsCPI; + euler.m_Angle3 = summ; + } + else + { + euler.m_Angle2 = 0; + euler.m_Angle3 = summ; + } + } } \ No newline at end of file