diff --git a/AstroLib/Src/AsAttitudeParam_2023.cpp b/AstroLib/Src/AsAttitudeParam_2023.cpp index a2fbff06af5945887cdee10c49baf97e6d994161..232fb6148ffbc9d727b0269d19f63f3fc7895c4b 100644 --- a/AstroLib/Src/AsAttitudeParam_2023.cpp +++ b/AstroLib/Src/AsAttitudeParam_2023.cpp @@ -739,28 +739,24 @@ void AsMtxToEuler132(const CMatrix& mtx, CEuler& euler) ///*********************************************************************** 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) + double epsilon = 1e-12; + if (mtx[1][1]<(1 + epsilon) & mtx[1][1]>(-1 - 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]); - } + euler.m_Angle2 = acos(mtx[1][1]); + } + else + { + cout << "error,输入方向余弦矩阵存在大于1或小于-1的元素"; + } + + if (fabs(sin(euler.m_Angle2)) < epsilon) + { + euler.m_Angle1 = 0; + euler.m_Angle3 = 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 = AsCPI-atan2(mtx[1][0], -mtx[1][2]); + euler.m_Angle3 = AsCPI-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