From 5d89986d64c76274c9bbc47a7e1a5e0c9484163d Mon Sep 17 00:00:00 2001
From: KKK <2278651978@qq.com>
Date: Mon, 10 Mar 2025 06:34:00 +0000
Subject: [PATCH] =?UTF-8?q?update=20AstroLib/Src/AsAttitudeParam=5F2023.cp?=
 =?UTF-8?q?p.=20=E6=B7=BB=E5=8A=A0=E5=9B=9B=E5=85=83=E6=95=B0=E8=BD=AC213?=
 =?UTF-8?q?=E8=BD=AC=E5=BA=8F=E6=AC=A7=E6=8B=89=E8=A7=92=E4=BB=A3=E7=A0=81?=
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 | 61 +++++++++++++++++++++++++++
 1 file changed, 61 insertions(+)

diff --git a/AstroLib/Src/AsAttitudeParam_2023.cpp b/AstroLib/Src/AsAttitudeParam_2023.cpp
index 8486dbe..eb344c3 100644
--- a/AstroLib/Src/AsAttitudeParam_2023.cpp
+++ b/AstroLib/Src/AsAttitudeParam_2023.cpp
@@ -159,6 +159,67 @@ 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
-- 
Gitee