diff --git a/AstroLib/Include/AsAttitudeParam.h b/AstroLib/Include/AsAttitudeParam.h index b199c9e81e45f97b8a2856a129eb354577830797..6c8f188a5d2605ca126bd0e4cf211eff25b43f7c 100644 --- a/AstroLib/Include/AsAttitudeParam.h +++ b/AstroLib/Include/AsAttitudeParam.h @@ -1,4 +1,4 @@ -//////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////// // // // Copyright (c) 2003-2008 // // Wang Hua // @@ -406,3 +406,4 @@ inline const CEuler AsRadToDeg(CEuler rad) #endif // !defined(_ASATTITUDEPARAM_H_) +void AsQuatToEuler313(const CQuaternion& quat, CEuler& euler); \ No newline at end of file diff --git a/AstroLib/Src/AsAttitudeParam.cpp b/AstroLib/Src/AsAttitudeParam.cpp index 4167dfe6ed2f795a279222e74ba78cdb1aefa652..5d726d855f6bfa7a2cf0f79a9359d5af980912e0 100644 --- a/AstroLib/Src/AsAttitudeParam.cpp +++ b/AstroLib/Src/AsAttitudeParam.cpp @@ -1,4 +1,4 @@ -//////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////// // // // Copyright (c) 2003-2006 // // Wang Hua // @@ -683,4 +683,40 @@ void AsQuatToMtx(const CQuaternion& quat, CMatrix& mtx) quat.ToMtx(mtx); } +void AsQuatToEuler313(const CQuaternion& quat, CEuler& euler) +{ + // 检查四元数是否归一化 + double norm = quat.m_Qs * quat.m_Qs + quat.m_Qx * quat.m_Qx + quat.m_Qy * quat.m_Qy + quat.m_Qz * quat.m_Qz; + if (std::abs(norm - 1.0) > std::numeric_limits::epsilon()) { + std::cerr << "Warning: The quaternion is not normalized!" << std::endl; + return; + } + + // 提取单位化后的四元数分量 + double q0 = quat.m_Qs; // 四元数的实部 + double q1 = quat.m_Qx; // 四元数的 x 分量 + double q2 = quat.m_Qy; // 四元数的 y 分量 + double q3 = quat.m_Qz; // 四元数的 z 分量 + + // 计算 313 转序的欧拉角 + double sin_theta = 2.0 * (q1 * q3 - q0 * q2); + // 处理 asin 输入超出 [-1, 1] 范围的情况 + if (sin_theta > 1.0) { + sin_theta = 1.0; + } + else if (sin_theta < -1.0) { + sin_theta = -1.0; + } + + euler.m_Angle2 = asin(sin_theta); // theta (绕 X 轴) + // 处理绕X轴旋转+-pi/2的情形,此时直接将phi赋值为零,以防止多解。 + if (fabs(sin_theta) >= 1.0 - std::numeric_limits::epsilon()) { + euler.m_Angle1 = 0.0; // phi (绕 Z 轴) + euler.m_Angle3 = 2.0 * atan2(q1, q0); // psi (绕 Z 轴) + } + else { + euler.m_Angle1 = atan2(2.0 * (q2 * q3 + q0 * q1), q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3); // phi (绕 Z 轴) + euler.m_Angle3 = atan2(2.0 * (q1 * q2 + q0 * q3), q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3); // psi (绕 Z 轴) + } +} diff --git a/Examples/AstroLibExamples.cpp b/Examples/AstroLibExamples.cpp index 3ebdf824e2cf296748018e36b9e5b231a9dc9e83..985732be3ae4f910dae11e88cbe7b958aee23d23 100644 --- a/Examples/AstroLibExamples.cpp +++ b/Examples/AstroLibExamples.cpp @@ -1,4 +1,4 @@ -// AstroLibExamples.cpp : Defines the entry point for the console application. +// AstroLibExamples.cpp : Defines the entry point for the console application. // #include @@ -21,9 +21,24 @@ void main() printf("*****************************************************************\n\n"); int sel; + // 创建一个四元数对象 + CQuaternion quat(4, 1, 0, 0); // 示例四元数 + quat.Unit(); + // 创建一个欧拉角对象 + CEuler euler; + // 调用函数将四元数转换为欧拉角 + AsQuatToEuler313(quat, euler); + + // 输出结果 + std::cout << "Euler Angles (313 sequence):" << std::endl; + std::cout << "Fi (绕 X 轴): " << euler.m_Angle2 << std::endl; + std::cout << "Theta (绕 Y 轴): " << euler.m_Angle3 << std::endl; + std::cout << "Psi (绕 Z 轴): " << euler.m_Angle1 << std::endl; /////////////////////////////// + + while (true) { printf("\n*****************************************************************\n");