diff --git a/AstroLib/Src/AsAttitudeParam_2023.cpp b/AstroLib/Src/AsAttitudeParam_2023.cpp index 4a05eb0518f8d332e83ea8f6c13ef6161dcb3b0c..78d413bcd373dd7c3deccf37c28fe1ff3fd90d5b 100644 --- a/AstroLib/Src/AsAttitudeParam_2023.cpp +++ b/AstroLib/Src/AsAttitudeParam_2023.cpp @@ -656,4 +656,41 @@ void AsMtxToEuler132(const CMatrix& mtx, CEuler& euler) euler.m_Angle2 = asin(-mtx[1][0]); euler.m_Angle3 = atan2(mtx[2][0] / cbeta, mtx[0][0] / cbeta); } +} + +///*********************************************************************** +/// 方向余弦矩阵转为212转序Euler角 +/// @Author Chang Xiaokuan +/// @Date 2025.2.28 +/// @Input +/// @Param mtx 坐标转移矩阵 +/// @Output +/// @Param euler 欧拉角(rad) +///*********************************************************************** +void AsMtxToEuler212(const CMatrix& mtx, CEuler& euler) +{ + double epsilon = 1e-8; + double theta = 0, psi = 0, phi = 0; + theta = acos(mtx[1][1]); + if (fabs(sin(theta)) < epsilon) + { + if (fabs(theta) < epsilon) + { + phi = 0; + psi = atan2(mtx[0][2], mtx[0][0]); + } + else //theta == pi + { + psi = 0; + phi = atan2(-mtx[0][2], mtx[0][0]); + } + } + else //sin(theta) != 0 + { + psi = atan2(mtx[1][0], -mtx[1][2]); + phi = atan2(mtx[0][1], mtx[2][1]); + } + euler.m_Angle1 = phi; + euler.m_Angle2 = theta; + euler.m_Angle3 = psi; } \ No newline at end of file