diff --git a/AstroLib/Src/AsAttitudeParam_2023.cpp b/AstroLib/Src/AsAttitudeParam_2023.cpp index 23c7fcab688f52bcdacb1e49839fe9043a1af433..d301e54ef685be918a9dd0b0bcfe90c86d037ea8 100644 --- a/AstroLib/Src/AsAttitudeParam_2023.cpp +++ b/AstroLib/Src/AsAttitudeParam_2023.cpp @@ -10,17 +10,17 @@ /// @Author Xiao Yao /// @Date 2023.4.4 /// @Input -/// @Param mtx 坐标转换矩阵 +/// @Param mtx 鍧愭爣杞崲鐭╅樀 /// @Output -/// @Param axis 旋转轴矢量 -/// @Param angle 旋转角[0, pi] +/// @Param axis 鏃嬭浆杞寸煝閲 +/// @Param angle 鏃嬭浆瑙抂0, pi] ///*********************************************************************** void AsMtxToAxAng(const CMatrix& mtx, CCoord& axis, double& angle) { CMatrix mm = mtx; - /************** 判断是否为正交矩阵 ******************/ + /************** 鍒ゆ柇鏄惁涓烘浜ょ煩闃 ******************/ double p1 = mm[0][0] * mm[0][1] + mm[1][0] * mm[1][1] + mm[2][0] * mm[2][1]; double p2 = mm[0][0] * mm[0][2] + mm[1][0] * mm[1][2] + mm[2][0] * mm[2][2]; double p3 = mm[0][2] * mm[0][1] + mm[1][2] * mm[1][1] + mm[2][2] * mm[2][1]; @@ -37,7 +37,7 @@ void AsMtxToAxAng(const CMatrix& mtx, return; } - // 矩阵的迹 + // 鐭╅樀鐨勮抗 double T = mm[0][0] + mm[1][1] + mm[2][2]; if (abs(T - 3) > 1e-9 && abs(T + 1) > 1e-9) { @@ -49,8 +49,8 @@ void AsMtxToAxAng(const CMatrix& mtx, axis[2] = 0.5 * (mm[0][1] - mm[1][0]) / ss; } else if (abs(T + 1) < 1e-9) { - // 转轴有两个解,指向相差180°,且角度不影响, 本程序仅输出一个 - // sin(angle) = 0, angle=pi+2k*pi,k为整数 + // 杞酱鏈変袱涓В锛屾寚鍚戠浉宸180掳,涓旇搴︿笉褰卞搷, 鏈▼搴忎粎杈撳嚭涓涓 + // sin(angle) = 0锛 angle=pi+2k*pi,k涓烘暣鏁 angle = AsCPI; double a12 = mm[0][1] / 2.0; @@ -60,35 +60,68 @@ void AsMtxToAxAng(const CMatrix& mtx, double aa2 = sqrt((1.0 + mm[1][1]) / 2.0); double aa3 = sqrt((1.0 + mm[2][2]) / 2.0); if (a12 > 0 && a23 > 0 && a31 > 0) { - // a1 a2 a3同号 + // a1 a2 a3鍚屽彿 axis[0] = aa1; axis[1] = aa2; axis[2] = aa3; } else if (a12 > 0) { - // a1 a2 同号 + // a1 a2 鍚屽彿 axis[0] = aa1; axis[1] = aa2; axis[2] = -aa3; } else if (a23 > 0) { - // a2 a3 同号 + // a2 a3 鍚屽彿 axis[0] = aa1; axis[1] = -aa2; axis[2] = -aa3; } else { - // a1 a3同号 + // a1 a3鍚屽彿 axis[0] = aa1; axis[1] = -aa2; axis[2] = aa3; } } else { - // 转轴不确定 + // 杞酱涓嶇‘瀹 angle = 0; axis[0] = 1; axis[1] = 0; axis[2] = 0; } -} \ No newline at end of file +} + + +//****************************************************** +/// 鏍规嵁杞簭鍜屽潗鏍囪浆绉荤煩闃佃绠桬uler瑙掞紝杞簭涓2-3-1 +/// @author Sun Fengyan +/// @data 2023.4 +/// @Version 1.0 +/// @Input +/// @Param mtx 鍧愭爣杞Щ鐭╅樀 +/// @Output +/// @Param euler 杩斿洖鐨勬鎷夎(鍗曚綅锛歳ad锛 +//****************************************************** +void AsMtxToEuler231(const CMatrix& mtx, CEuler& euler) +{ + euler.m_Angle2 = asin(mtx[0][1]); + + if (abs(mtx[0][1] - 1) < 1e-8 ) //euler.m_Angle2 = pi/2 + { + euler.m_Angle1 = atan2(mtx[1][2], mtx[2][2]); + euler.m_Angle3 = 0; + } + else if (abs(mtx[0][1] + 1) < 1e-8 ) //euler.m_Angle2 = -pi/2 + { + euler.m_Angle1 = -atan2(mtx[1][2], mtx[2][2]); + euler.m_Angle3 = 0; + } + else + { + euler.m_Angle1 = atan2(-mtx[0][2], mtx[0][0]); + euler.m_Angle3 = atan2(-mtx[2][1], mtx[1][1]); + + } +}