From ad59c19d16fa4dd6587c0225012e8e1ea36336fe Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?F=E5=90=9B?= <15498453+F-jun777@user.noreply.gitee.com> Date: Sun, 9 Mar 2025 07:26:18 +0000 Subject: [PATCH] =?UTF-8?q?=E5=9B=9B=E5=85=83=E6=95=B0=E8=BD=AC=E4=B8=BA31?= =?UTF-8?q?3=E8=BD=AC=E5=BA=8F=E6=AC=A7=E6=8B=89=E8=A7=92-=E8=8C=83?= =?UTF-8?q?=E8=95=B4=E5=AE=9D?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: F君 <15498453+F-jun777@user.noreply.gitee.com> --- AstroLib/Include/AsAttitudeParam.h | 3 ++- AstroLib/Src/AsAttitudeParam.cpp | 38 +++++++++++++++++++++++++++++- Examples/AstroLibExamples.cpp | 17 ++++++++++++- 3 files changed, 55 insertions(+), 3 deletions(-) diff --git a/AstroLib/Include/AsAttitudeParam.h b/AstroLib/Include/AsAttitudeParam.h index b199c9e..6c8f188 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 4167dfe..5d726d8 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 3ebdf82..985732b 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"); -- Gitee