From b6147bdb8f4d656bb028343a5e815dd1f1140c7c Mon Sep 17 00:00:00 2001
From: KKK <2278651978@qq.com>
Date: Mon, 17 Mar 2025 01:08:48 +0000
Subject: [PATCH] =?UTF-8?q?update=20AstroLib/Src/AsAttitudeParam=5F2023.cp?=
 =?UTF-8?q?p.=20=E4=BF=AE=E6=94=B9213=E8=BD=AC=E5=BA=8F=E5=9B=9B=E5=85=83?=
 =?UTF-8?q?=E6=95=B0=E8=BD=AC=E6=AC=A7=E6=8B=89=E8=A7=92=E5=87=BD=E6=95=B0?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Signed-off-by: KKK <2278651978@qq.com>
---
 AstroLib/Src/AsAttitudeParam_2023.cpp | 122 +++++++++++++-------------
 1 file changed, 61 insertions(+), 61 deletions(-)

diff --git a/AstroLib/Src/AsAttitudeParam_2023.cpp b/AstroLib/Src/AsAttitudeParam_2023.cpp
index 58f715b..be33261 100644
--- a/AstroLib/Src/AsAttitudeParam_2023.cpp
+++ b/AstroLib/Src/AsAttitudeParam_2023.cpp
@@ -159,67 +159,6 @@ bool AsQuatToEuler123(const CQuaternion& q, CEuler& euler)
 }
 
 
-///***********************************************************************
-///将四元数转化成欧拉角_213转序
-/// @Author	Kong Lingyue
-/// @Date	2025.03.10
-/// @Input
-/// @Param	q		标准化四元数
-/// @Output
-/// @Param	euler		欧拉角(rad)
-///***********************************************************************
-bool AsQuatToEuler213(const CQuaternion& q, CEuler& euler)
-{
-	//设置默认参数
-	double epsilon = 1e-6;		//精确度
-	euler = { 0, 0, 0 };
-	//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 (fabs(q_norm2 - 1.0) > epsilon)			//判断输入的四元数是否标准化
-	{
-		cout << "请输入标准化四元数！！！" << endl;
-		return false;
-	}
-	
-	else if (fabs(q0q0 - 1) < epsilon) {}	//没有旋转时，直接输出
-	else
-	{
-		double q0q1 = q.m_Qs*q.m_Qx;
-		double q0q2 = q.m_Qs*q.m_Qy;
-		double q0q3 = q.m_Qs*q.m_Qz;
-		double q1q2 = q.m_Qx*q.m_Qy;
-		double q1q3 = q.m_Qx*q.m_Qz;
-		double q2q3 = q.m_Qy*q.m_Qz;
-		cout << q0q1 << endl;
-		double s = -2 * (q2q3 - q0q1);   //正弦值
-		//判断是否出现奇异，当出现奇异时，令 Angle3或Angle1 为 0，确保程序可以继续运行，这里默认 Angle3为0
-		if (s > 1 - epsilon)		//判断为pi/2
-		{
-			euler.m_Angle2 = AsCHalfPI;
-			euler.m_Angle1 = 2 * atan(q.m_Qy / q.m_Qs);
-			euler.m_Angle3 = 0.0;
-		}
-		else if (s < - 1 + epsilon)   //判断为-pi/2
-		{
-			euler.m_Angle2 = -AsCHalfPI;
-			euler.m_Angle1 = 2 * atan(q.m_Qy / q.m_Qs) ;
-			euler.m_Angle3 = 0.0;
-		}
-		else
-		{
-			euler.m_Angle2 = asin( s );
-			euler.m_Angle1 = -atan2( -2 * (q1q3 + q0q2), 1 - 2 * (q1q1 + q2q2));
-			euler.m_Angle3 = -atan2(-2 * (q1q2 + q0q3), 1 - 2 * (q1q1 + q3q3));
-		}
-	}
-	return true;
-}
-
-
 ///***********************************************************************
 /// 方向余弦阵转321转序欧拉角
 /// @Author	Li Sibei
@@ -620,6 +559,67 @@ void AsQuatToEuler323(CQuaternion& quat, CEuler& euler)
 
 }
 
+
+///***********************************************************************
+///将四元数转化成欧拉角_213转序
+/// @Author	KLY
+/// @Date	2025.3
+/// @Input
+/// @Param	q		标准化四元数
+/// @Output
+/// @Param	euler		欧拉角(rad)
+///***********************************************************************
+bool AsQuatToEuler213(const CQuaternion& q, CEuler& euler)
+{
+	//设置默认参数
+	double epsilon = 1e-6;		//精确度
+	euler = { 0, 0, 0 };
+	//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 (fabs(q_norm2 - 1.0) > epsilon)
+	{
+		printf("[ERROR]AsQuatToEuler213: 四元数未标准化! 模长平方=%.4f\n", q_norm2);
+		q.Norm();  // 强制标准化四元数
+	}
+
+	else if (fabs(q0q0 - 1) < epsilon) {}	//没有旋转时，直接输出
+	else
+	{
+		double q0q1 = q.m_Qs*q.m_Qx;
+		double q0q2 = q.m_Qs*q.m_Qy;
+		double q0q3 = q.m_Qs*q.m_Qz;
+		double q1q2 = q.m_Qx*q.m_Qy;
+		double q1q3 = q.m_Qx*q.m_Qz;
+		double q2q3 = q.m_Qy*q.m_Qz;
+		double s = -2 * (q2q3 - q0q1);   //正弦值
+		//判断是否出现奇异，当出现奇异时，令 Angle3或Angle1 为 0，确保程序可以继续运行，这里默认 Angle3为0
+		if (s >= 1)		     //判断为pi/2
+		{
+			euler.m_Angle2 = AsCHalfPI;
+			euler.m_Angle1 = 2 * atan2(q.m_Qy, q.m_Qs);
+			euler.m_Angle3 = 0.0;
+		}
+		else if (s <= -1)    //判断为-pi/2
+		{
+			euler.m_Angle2 = -AsCHalfPI;
+			euler.m_Angle1 = 2 * atan2(q.m_Qy, q.m_Qs);
+			euler.m_Angle3 = 0.0;
+		}
+		else                 //一般情况求解
+		{
+			euler.m_Angle2 = asin(s);
+			euler.m_Angle1 = atan2(2 * (q1q3 + q0q2), 1 - 2 * (q1q1 + q2q2));
+			euler.m_Angle3 = atan2(2 * (q1q2 + q0q3), 1 - 2 * (q1q1 + q3q3));
+		}
+	}
+	return true;
+}
+
+
 ///***********************************************************************
 /// 根据坐标转移矩阵计算132转序欧拉角
 /// @Author	Wang Deyu
-- 
Gitee