From 14acdb1745255a666b319374732eae72915f564d Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?=E5=BC=A0=E9=9B=AA=E9=A2=96?= <2674871752@qq.com>
Date: Tue, 11 Apr 2023 13:39:37 +0000
Subject: [PATCH] =?UTF-8?q?=E8=BE=93=E5=85=A5=E4=B8=89=E7=BB=B4=E6=96=B9?=
 =?UTF-8?q?=E5=90=91=E4=BD=99=E5=BC=A6=E7=9F=A9=E9=98=B5=EF=BC=8C=E8=A7=A3?=
 =?UTF-8?q?=E7=AE=97=E6=AC=A7=E6=8B=89=E8=A7=92=EF=BC=8C=E5=9C=A8=E5=B1=8F?=
 =?UTF-8?q?=E5=B9=95=E4=B8=8A=E6=89=93=E5=8D=B0=E8=A7=A3=E7=AE=97=E5=BE=97?=
 =?UTF-8?q?=E5=88=B0=E7=9A=84=E6=AC=A7=E6=8B=89=E8=A7=92=EF=BC=8C=E5=B9=B6?=
 =?UTF-8?q?=E8=BE=93=E5=87=BA=E6=AC=A7=E6=8B=89=E8=A7=92=E7=9A=84=E6=95=B0?=
 =?UTF-8?q?=E5=80=BC?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Signed-off-by: 张雪颖 <2674871752@qq.com>
---
 AsMtxToEuler.cpp | 65 ++++++++++++++++++++++++++++++++++++++++++++++++
 AsMtxToEuler.h   | 15 +++++++++++
 2 files changed, 80 insertions(+)
 create mode 100644 AsMtxToEuler.cpp
 create mode 100644 AsMtxToEuler.h

diff --git a/AsMtxToEuler.cpp b/AsMtxToEuler.cpp
new file mode 100644
index 0000000..3737f49
--- /dev/null
+++ b/AsMtxToEuler.cpp
@@ -0,0 +1,65 @@
+///***********************************************************************
+/// 方向余弦矩阵转为312转序Euler角
+/// @Author	Zhang Xuaying
+/// @Date	2023.04.01
+/// @Input  Mtx  	   方向余弦矩阵
+/// @Param	
+/// @Param	
+/// @Output	eulerangles	欧拉角
+///***********************************************************************
+#include "AsMtxToEuler.h"
+#include "SimDoF6.h"
+#include "DoF6.h"
+
+const double PI = 3.141592653;
+
+
+//定义欧拉角的结构体
+struct Angles
+{
+	double m_Angle2, m_Angle1, m_Angle3;
+};
+
+
+CVector<double> AsMtxToEuler(CMatrix<double>& Mtx)
+{
+	CMatrix<double> mtx(3, 3);//定义一个矩阵用于存放方向余弦矩阵的数据
+	Angles angles; //定义一个结构体，用于储存欧拉角
+	mtx = Mtx;
+
+	CVector<double> eulerangles(3);
+
+	bool singular = mtx(1, 2) > 1 - 1e-8;//判断是否出现欧拉角奇异问题	
+	if (!singular)
+	{
+		angles.m_Angle2 = asin(mtx(1, 2)) / PI * 180;
+		if (mtx(1, 0) == 0)
+		{
+			angles.m_Angle1 = 0;
+		}
+		else
+		{
+			angles.m_Angle1 = -atan2(mtx(1, 0), mtx(1, 1)) / PI * 180;//偏航
+		}
+
+		if (mtx(0, 2) == 0)
+		{
+			angles.m_Angle3 = 0;
+		}
+		else
+		{
+			angles.m_Angle3 = -atan2(mtx(0, 2), mtx(2, 2)) / PI * 180;//俯仰
+		}
+	}
+	else
+	{
+		cout << "出现欧拉角数值奇异问题" << endl;
+		angles.m_Angle1 = 0;
+		angles.m_Angle2 = 90;
+		angles.m_Angle3 = atan2(mtx(2, 0), mtx(0, 0));
+	}
+	eulerangles[0] = angles.m_Angle1;
+	eulerangles[1] = angles.m_Angle2;
+	eulerangles[2] = angles.m_Angle3;
+	return eulerangles;
+}
\ No newline at end of file
diff --git a/AsMtxToEuler.h b/AsMtxToEuler.h
new file mode 100644
index 0000000..8a95836
--- /dev/null
+++ b/AsMtxToEuler.h
@@ -0,0 +1,15 @@
+#pragma once
+///***********************************************************************
+/// 方向余弦矩阵转为312转序Euler角
+/// @Author	Zhang Xuaying
+/// @Date	2023.04.01
+/// @Input  Mtx  	   方向余弦矩阵
+/// @Param	
+/// @Param	
+/// @Output	eulerangles	欧拉角
+///***********************************************************************
+#include<iostream>
+#include "SimDoF6.h"
+#include "DoF6.h"
+using namespace std;
+CVector<double> AsMtxToEuler(CMatrix<double>& Mtx);
\ No newline at end of file
-- 
Gitee