diff --git a/AstroLib/Src/AsAttitudeParam_2023.cpp b/AstroLib/Src/AsAttitudeParam_2023.cpp index 77156d2406464835f73e7dcf64bca1273197b662..a82832488dbae1a28fcaaff8515c00e766bcd5e7 100644 --- a/AstroLib/Src/AsAttitudeParam_2023.cpp +++ b/AstroLib/Src/AsAttitudeParam_2023.cpp @@ -14,17 +14,17 @@ using namespace std; /// @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]; @@ -41,7 +41,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) { @@ -53,8 +53,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; @@ -64,32 +64,32 @@ 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; @@ -98,33 +98,34 @@ void AsMtxToAxAng(const CMatrix& mtx, } -//************************************************ -//*将四元数转化成欧拉角_123转序(欧拉角单位为度)* -//************************************************ -//输入: -// 标准化四元数:q -// 代求欧拉角: Ang -// 卫星类型选择:cho (cho=1:自旋卫星;cho=3:非自旋卫星) -//************************************************ -bool AsQuatToEuler_123(const CQuaternion& q, CEuler& Ang, const int cho) +///************************************************ +///鍔熻兘锛氬皢鍥涘厓鏁拌浆鍖栨垚娆ф媺瑙抇123杞簭锛堟鎷夎鍗曚綅涓哄害锛 +///浣滆咃細鍒樼敯闈 +///鏃ユ湡锛2023/4/10 +///杈撳叆锛 +/// 鏍囧噯鍖栧洓鍏冩暟锛歲 +/// 浠f眰娆ф媺瑙掞細 Ang +/// 鍗槦绫诲瀷閫夋嫨锛歝ho (cho=1锛氳嚜鏃嬪崼鏄燂紱cho=3锛氶潪鑷棆鍗槦) +///************************************************ +bool AsQuatToEuler_123(const CQuaternion& q, CEuler& Ang, const int cho = 3) { - //设置默认参数 - double pi = acos(-1); //计算pi值 - double deg = 180 / pi; //弧度->度的转换 - double epsilon = 1e-6; //精确度 + //璁剧疆榛樿鍙傛暟 + double pi = acos(-1); //璁$畻pi鍊 + double deg = 180 / pi; //寮у害->搴︾殑杞崲 + double epsilon = 1e-6; //绮剧‘搴 Ang = { 0, 0, 0 }; - //q0, q1, q2, q3分别对应q.m_Qs, q.m_Qx, q.m_Qy, q.m_Qz + //q0, q1, q2, q3鍒嗗埆瀵瑰簲q.m_Qs, q.m_Qx, q.m_Qy, q.m_Qz double q0q0 = q.m_Qs*q.m_Qs; double q1q1 = q.m_Qx*q.m_Qx; double q2q2 = q.m_Qy*q.m_Qy; double q3q3 = q.m_Qz*q.m_Qz; double q_norm2 = q0q0 + q1q1 + q2q2 + q3q3; - if (abs(q_norm2 - 1) > epsilon) //判断输入的四元数是否标准化 + if (abs(q_norm2 - 1) > epsilon) //鍒ゆ柇杈撳叆鐨勫洓鍏冩暟鏄惁鏍囧噯鍖 { - cout << "请输入标准化四元数!!!" << endl; + cout << "璇疯緭鍏ユ爣鍑嗗寲鍥涘厓鏁帮紒锛侊紒" << endl; return false; } - else if (abs(q0q0 - 1) < epsilon) {} //没有旋转时,直接输出 + else if (abs(q0q0 - 1) < epsilon) {} //娌℃湁鏃嬭浆鏃讹紝鐩存帴杈撳嚭 else { double q0q1 = q.m_Qs*q.m_Qx; @@ -136,16 +137,16 @@ bool AsQuatToEuler_123(const CQuaternion& q, CEuler& Ang, const int cho) double a02 = q0q0 - q2q2; double a13 = q1q1 - q3q3; - double sy = 2 * (q1q3 + q0q2); //俯仰角 angle.y 的正弦值 + double sy = 2 * (q1q3 + q0q2); //淇话瑙 angle.y 鐨勬寮﹀ - //判断是否出现奇异,当出现奇异时,令 Angle3或Angle1 为 0,确保程序可以继续运行 - if (sy - 1 >= 0) //判断俯仰角为pi/2 + //鍒ゆ柇鏄惁鍑虹幇濂囧紓锛屽綋鍑虹幇濂囧紓鏃讹紝浠 Angle3鎴朅ngle1 涓 0锛岀‘淇濈▼搴忓彲浠ョ户缁繍琛 + if (sy - 1 >= 0) //鍒ゆ柇淇话瑙掍负pi/2 { Ang.m_Angle2 = 90; Ang.m_Angle1 = (3 - cho)*atan(q.m_Qx / q.m_Qy)*deg; Ang.m_Angle3 = (cho - 1)*atan(q.m_Qx / q.m_Qy)*deg; } - else if (sy + 1 <= 0) //判断俯仰角为-pi/2 + else if (sy + 1 <= 0) //鍒ゆ柇淇话瑙掍负-pi/2 { Ang.m_Angle2 = -90; Ang.m_Angle1 = -(3 - cho)*atan(q.m_Qx / q.m_Qy)*deg;