diff --git a/AstroLib/Src/AsAttitudeParam_2023.cpp b/AstroLib/Src/AsAttitudeParam_2023.cpp index 78d413bcd373dd7c3deccf37c28fe1ff3fd90d5b..58f715ba809dfab3523443a232cec2a4f17ddcb5 100644 --- a/AstroLib/Src/AsAttitudeParam_2023.cpp +++ b/AstroLib/Src/AsAttitudeParam_2023.cpp @@ -623,7 +623,7 @@ void AsQuatToEuler323(CQuaternion& quat, CEuler& euler) ///*********************************************************************** /// 根据坐标转移矩阵计算132转序欧拉角 /// @Author Wang Deyu -/// @Date 2025.03.11 +/// @Date 2025.03.15 /// @Input /// @Param mtx 输入的转移矩阵 /// @Output @@ -631,23 +631,19 @@ void AsQuatToEuler323(CQuaternion& quat, CEuler& euler) ///*********************************************************************** void AsMtxToEuler132(const CMatrix& mtx, CEuler& euler) { - const double pi = 4 * std::atan(1.0); double cbeta; - //处理万向锁 - if (sqrt(mtx[0][0] * mtx[0][0] + mtx[2][0] * mtx[2][0]) < 1e-6) + //处理万向锁 + if (fabs(-mtx[1][0] - 1) < 1e-12) { - if (-mtx[1][0] > 0) - { - euler.m_Angle1 = 0; - euler.m_Angle2 = pi / 2; - euler.m_Angle3 = atan2(-mtx[0][2], mtx[0][1]); - } - else - { - euler.m_Angle1 = 0; - euler.m_Angle2 = -pi / 2; - euler.m_Angle3 = atan2(-mtx[0][2], -mtx[0][1]); - } + euler.m_Angle1 = 0; + euler.m_Angle2 = AsCPI / 2; + euler.m_Angle3 = atan2(-mtx[0][2], mtx[0][1]); + } + else if(fabs(-mtx[1][0] + 1) < 1e-12) + { + euler.m_Angle1 = 0; + euler.m_Angle2 = -AsCPI / 2; + euler.m_Angle3 = atan2(-mtx[0][2], -mtx[0][1]); } else {