From 3410dbbf96c2804d801078bd404ba862582ea913 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?=E6=AD=A5=E6=99=93=E7=AB=A5?= <1243783974@qq.com>
Date: Thu, 6 Mar 2025 07:08:52 +0000
Subject: [PATCH] update AstroLib/Src/AsAttitudeParam_2023.cpp.
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Signed-off-by: 步晓童 <1243783974@qq.com>
---
 AstroLib/Src/AsAttitudeParam_2023.cpp | 81 +++++++++++++++++++++++++++
 1 file changed, 81 insertions(+)

diff --git a/AstroLib/Src/AsAttitudeParam_2023.cpp b/AstroLib/Src/AsAttitudeParam_2023.cpp
index 1db41de..8486dbe 100644
--- a/AstroLib/Src/AsAttitudeParam_2023.cpp
+++ b/AstroLib/Src/AsAttitudeParam_2023.cpp
@@ -477,3 +477,84 @@ void AsQuatToEuler321(CQuaternion& quat, CEuler& euler)
 	//euler.m_Angle1 = euler.m_Angle1 * 180 / AsCPI;
 }
 
+///***********************************************************************
+/// 四元数323转换成Euler角
+/// @Author	BXT
+/// @Date	2025.3
+/// @Input
+/// @Param quat		四元数
+/// @Output
+/// @Param euler	欧拉角
+///***********************************************************************
+void CQuaternion::ToEuler323(CQuaternion& quat, CEuler& euler) const
+{
+	const double eps = 1e-8;
+
+	// 四元数标准化验证
+	const double norm_sq = 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 (fabs(norm_sq - 1.0) > eps)
+	{
+		printf("[ERR]AsQuatToEuler321: 四元数未标准化! 模长平方=%.4f\n", norm_sq);
+		return;
+	}
+
+	// 将四元数转换为方向余弦矩阵
+	CMatrix<double> m(3, 3);
+	ToMtx(m);
+
+	// 提取方向余弦矩阵中的元素
+	double m11 = m[0][0];
+	double m12 = m[0][1];
+	double m13 = m[0][2];
+	double m21 = m[1][0];
+	double m22 = m[1][1];
+	double m23 = m[1][2];
+	double m31 = m[2][0];
+	double m32 = m[2][1];
+	double m33 = m[2][2];
+
+	// 计算ZYZ顺序的欧拉角
+	// 参考公式：
+	// 旋转矩阵m(323转序)
+	// m = [cos(a)*cos(b)*cos(c)-sin(a)*sin(c), cos(a)*sin(c)+cos(b)*cos(c)*sin(a), -cos(c)*sin(b)
+	//	   -cos(c)*sin(a)-cos(a)*cos(b)*sin(c), cos(a)*cos(c)-cos(b)*sin(a)*sin(c), sin(b)*sin(c)
+	//	   cos(a)*sin(b), sin(a)*sin(b), cos(b)]
+	// θ = acos(m33)
+	// φ = atan2(m32, m31)
+	// ψ = atan2(m23, -m13)
+
+	// 计算theta并判断奇异情况,θ∈[0,π]
+	double cosTheta = m33;
+	euler.m_Angle2 = acos(cosTheta);
+
+	double sinTheta = sqrt(1.0 - cosTheta*cosTheta);
+	if (sinTheta > eps) 
+	{
+		// 非奇异情况，正常计算
+		euler.m_Angle1 = atan2(m32, m31);
+		euler.m_Angle3 = atan2(m23, -m13);
+	}
+	else 
+	{
+		// 奇异情况：theta接近0或π
+		// theta接近0
+		if (cosTheta > 0) 
+		{
+			euler.m_Angle1 = atan2(m12, m11);
+			euler.m_Angle3 = 0.0;
+		}
+		// theta接近π
+		else 
+		{
+			euler.m_Angle1 = atan2(-m21, -m11);
+			euler.m_Angle3 = 0.0;
+		}
+	}
+
+	// 确保m_Angle1和m_Angle3在[-π,π]
+	if (euler.m_Angle3 > AsCPI) euler.m_Angle3 -= 2 * AsCPI;
+	if (euler.m_Angle3 < -AsCPI) euler.m_Angle3 += 2 * AsCPI;
+	if (euler.m_Angle1 > AsCPI) euler.m_Angle1 -= 2 * AsCPI;
+	if (euler.m_Angle1 < -AsCPI) euler.m_Angle1 += 2 * AsCPI;
+
+}
\ No newline at end of file
-- 
Gitee