From 20a761347bfbcde8675bac166716ec53a91ea2e4 Mon Sep 17 00:00:00 2001
From: tianlei12 <2771190571@qq.com>
Date: Mon, 10 Mar 2025 00:06:24 +0000
Subject: [PATCH] update AstroLib/Src/AsAttitudeParam.cpp.

Signed-off-by: tianlei12 <2771190571@qq.com>
---
 AstroLib/Src/AsAttitudeParam.cpp | 49 +++++++++++++++++++++++++++++++-
 1 file changed, 48 insertions(+), 1 deletion(-)

diff --git a/AstroLib/Src/AsAttitudeParam.cpp b/AstroLib/Src/AsAttitudeParam.cpp
index 4167dfe..1486f3e 100644
--- a/AstroLib/Src/AsAttitudeParam.cpp
+++ b/AstroLib/Src/AsAttitudeParam.cpp
@@ -1,4 +1,4 @@
-////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////
 //                                                                        //
 //                       Copyright (c) 2003-2006                          //
 //                              Wang Hua                                  //
@@ -683,4 +683,51 @@ void  AsQuatToMtx(const CQuaternion& quat, CMatrix<double>& mtx)
 	quat.ToMtx(mtx);
 }
 
+void  AsQuatToMtx(const CQuaternion& quat, CMatrix<double>& mtx)
+{
+	quat.ToMtx(mtx);
+}
+///***********************************************************************
+/// 将四元数转为3-1-2转序的欧拉角
+/// @Author	田磊
+/// @Date	2025.3.05
+/// @Input
+/// @Param	quat	四元数
+/// @Output
+/// @Param	euler	3-1-2转序欧拉角
+///***********************************************************************
+void AsQuatToEuler312(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<double>::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 分量
+	double sin_fi = 2.0 * (q2 * q3 + q0 * q1);
+	// 处理 asin 输入超出 [-1, 1] 范围的情况
+	if (sin_fi > 1.0) {
+		sin_fi = 1.0;
+	}
+	else if (sin_fi < -1.0) {
+		sin_fi = -1.0;
+	}
+	// 计算 312 转序的欧拉角
+	euler.m_Angle2 = asin(sin_fi);
+	// 处理绕x轴旋转+-pi/2的情形，此时直接将theta赋值为零，以防止多解。
+	if (fabs(2.0 * (q2 * q3 + q0 * q1)) >= 1.0 - std::numeric_limits<double>::epsilon()) {
+		euler.m_Angle3 = 0.0;  // theta (绕 Y 轴)
+		euler.m_Angle1 = 2.0 * atan2(q3, q0);  // psi (绕 Z 轴)
+	}
+	else {
+		euler.m_Angle1 = atan2(2.0 * (-q1 * q2 + q0 * q3), q0 * q0 - q1 * q1 + q2 * q2 - q3 * q3);  // psi (绕 Z 轴)
+		euler.m_Angle3 = atan2(2.0 * (-q1 * q3 + q0 * q2), q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3);  // theta (绕 Y 轴)
+	}
+}
+
 
-- 
Gitee