From 0474f89ae27a4da52758f902d1cd19da5b94667f Mon Sep 17 00:00:00 2001
From: XiaoYao <yaoxiao0710@163.com>
Date: Thu, 6 Apr 2023 08:16:22 +0800
Subject: [PATCH] =?UTF-8?q?=E5=9D=90=E6=A0=87=E8=BD=AC=E6=8D=A2=E7=9F=A9?=
 =?UTF-8?q?=E9=98=B5=E8=BD=AC=E6=AC=A7=E6=8B=89=E8=A7=92/=E8=BD=B4-xiaoyao?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 AstroLib/AsAttitudeParam_2023.cpp | 91 +++++++++++++++++++++++++++++++
 AstroLib/AsAttitudeParam_2023.h   | 18 ++++++
 AstroLib/AstroLib.vcxproj         |  6 ++
 AstroLib/AstroLib.vcxproj.filters |  9 +++
 4 files changed, 124 insertions(+)
 create mode 100644 AstroLib/AsAttitudeParam_2023.cpp
 create mode 100644 AstroLib/AsAttitudeParam_2023.h

diff --git a/AstroLib/AsAttitudeParam_2023.cpp b/AstroLib/AsAttitudeParam_2023.cpp
new file mode 100644
index 0000000..f54421b
--- /dev/null
+++ b/AstroLib/AsAttitudeParam_2023.cpp
@@ -0,0 +1,91 @@
+#include "AsCoordinate.h"
+#include "AsMath.h"
+#include "AsAttitudeParam.h"
+
+///***********************************************************************
+/// The conversion from matrix to axis and angle.
+/// @Author	Xiao Yao
+/// @Date	2023.4.4
+/// @Input
+/// @Param	mtx	    坐标转换矩阵
+/// @Output
+/// @Param	axis	旋转轴矢量
+/// @Param	angle	旋转角[0, pi]
+///***********************************************************************
+void AsMtxToAxAng(const CMatrix<double>& mtx,
+	CCoord& axis, double& angle)
+{
+
+	CMatrix<double> mm = mtx;
+	/************** 判断是否为正交矩阵 ******************/
+	double p1 = mm[0][0] * mm[0][1] + mm[1][0] * mm[1][1] + mm[2][0] * mm[2][1];
+	double p2 = mm[0][0] * mm[0][2] + mm[1][0] * mm[1][2] + mm[2][0] * mm[2][2];
+	double p3 = mm[0][2] * mm[0][1] + mm[1][2] * mm[1][1] + mm[2][2] * mm[2][1];
+	double p4 = mm[0][0] * mm[0][0] + mm[1][0] * mm[1][0] + mm[2][0] * mm[2][0];
+	double p5 = mm[0][1] * mm[0][1] + mm[1][1] * mm[1][1] + mm[2][1] * mm[2][1];
+	double p6 = mm[0][2] * mm[0][2] + mm[1][2] * mm[1][2] + mm[2][2] * mm[2][2];
+	if (!(abs(p1)<1e-9 && abs(p2)<1e-9 && abs(p3)<1e-9 && 
+		  abs(p4-1)<1e-9 && abs(p5 - 1)<1e-9 && abs(p6 - 1)<1e-9) ){
+		printf("warning! not Orthogonal Matrix\n");
+		angle = 0;
+		axis[0] = 0;
+		axis[1] = 0;
+		axis[2] = 0;
+		return;
+	}
+
+	// 矩阵的迹
+	double T = mm[0][0] + mm[1][1] + mm[2][2];
+
+	if (abs(T - 3) > 1e-9 && abs(T + 1) > 1e-9) {
+		double cc = 0.5 * (T - 1);   // cos(angle)
+		double ss = sin(acos(cc));   // sin(angle)
+		angle = atan2(ss, cc);       // (0, pi)
+		axis[0] = 0.5 * (mm[1][2] - mm[2][1]) / ss;
+		axis[1] = 0.5 * (mm[2][0] - mm[0][2]) / ss;
+		axis[2] = 0.5 * (mm[0][1] - mm[1][0]) / ss;
+	}
+	else if (abs(T + 1) < 1e-9) {
+		// 转轴有两个解，指向相差180°,且角度不影响, 本程序仅输出一个
+		// sin(angle) = 0， angle=pi+2k*pi,k为整数
+		angle = AsCPI;
+
+		double a12 = mm[0][1] / 2.0;
+		double a23 = mm[1][2] / 2.0;
+		double a31 = mm[2][0] / 2.0;
+		double aa1 = sqrt((1.0 + mm[0][0]) / 2.0);
+		double aa2 = sqrt((1.0 + mm[1][1]) / 2.0);
+		double aa3 = sqrt((1.0 + mm[2][2]) / 2.0);
+		if (a12 > 0 && a23 > 0 && a31 > 0) {
+			// a1 a2 a3同号
+			axis[0] = aa1;
+			axis[1] = aa2;
+			axis[2] = aa3;
+		}
+		else if (a12 > 0) {
+			// a1 a2 同号
+			axis[0] = aa1;
+			axis[1] = aa2;
+			axis[2] = -aa3;
+		}
+		else if (a23 > 0) {
+			// a2 a3 同号
+			axis[0] = aa1;
+			axis[1] = -aa2;
+			axis[2] = -aa3;
+		}
+		else {
+			// a1 a3同号
+			axis[0] = aa1;
+			axis[1] = -aa2;
+			axis[2] = aa3;
+		}
+	}
+	else {
+		// 转轴不确定
+		angle = 0;
+		axis[0] = 1;
+		axis[1] = 0;
+		axis[2] = 0;
+	}
+}
\ No newline at end of file
diff --git a/AstroLib/AsAttitudeParam_2023.h b/AstroLib/AsAttitudeParam_2023.h
new file mode 100644
index 0000000..e58f793
--- /dev/null
+++ b/AstroLib/AsAttitudeParam_2023.h
@@ -0,0 +1,18 @@
+#pragma once
+
+#include "AsCoordinate.h"
+#include "AsMath.h"
+#include "AsAttitudeParam.h"
+
+///***********************************************************************
+/// The conversion from matrix to axis and angle.
+/// @Author	Xiao Yao
+/// @Date	2023.4.2
+/// @Input
+/// @Param	mtx	    坐标转换矩阵
+/// @Output
+/// @Param	axis	旋转轴矢量
+/// @Param	angle	旋转角[0, pi]
+///***********************************************************************
+void AsMtxToAxAng(const CMatrix<double>& mtx,
+	CCoord& axis, double& angle);
\ No newline at end of file
diff --git a/AstroLib/AstroLib.vcxproj b/AstroLib/AstroLib.vcxproj
index 9a412bb..6e85432 100644
--- a/AstroLib/AstroLib.vcxproj
+++ b/AstroLib/AstroLib.vcxproj
@@ -70,20 +70,24 @@
   <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
     <OutDir>$(ProjectDir)</OutDir>
     <IntDir>$(ProjectDir)</IntDir>
+    <ExecutablePath>.\Lib;$(ExecutablePath)</ExecutablePath>
   </PropertyGroup>
   <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
     <OutDir>$(ProjectDir)</OutDir>
     <IntDir>$(ProjectDir)</IntDir>
     <TargetName>$(ProjectName)</TargetName>
+    <ExecutablePath>.\Lib;$(ExecutablePath)</ExecutablePath>
   </PropertyGroup>
   <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
     <OutDir>$(ProjectDir)</OutDir>
     <IntDir>$(ProjectDir)</IntDir>
+    <ExecutablePath>.\Lib;$(ExecutablePath)</ExecutablePath>
   </PropertyGroup>
   <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
     <OutDir>$(ProjectDir)</OutDir>
     <IntDir>$(ProjectDir)</IntDir>
     <TargetName>$(ProjectName)</TargetName>
+    <ExecutablePath>.\Lib;$(ExecutablePath)</ExecutablePath>
   </PropertyGroup>
   <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
     <ClCompile>
@@ -203,6 +207,7 @@
     </Bscmake>
   </ItemDefinitionGroup>
   <ItemGroup>
+    <ClInclude Include="AsAttitudeParam_2023.h" />
     <ClInclude Include="AstroLib.h" />
     <ClInclude Include="Include\AsAtmosphere.h" />
     <ClInclude Include="Include\AsAttitude.h" />
@@ -226,6 +231,7 @@
     <Text Include="AstroLib_ReadMe.txt" />
   </ItemGroup>
   <ItemGroup>
+    <ClCompile Include="AsAttitudeParam_2023.cpp" />
     <ClCompile Include="Src\AsAtmosphere.cpp" />
     <ClCompile Include="Src\AsAttitude.cpp" />
     <ClCompile Include="Src\AsAttitudeParam.cpp" />
diff --git a/AstroLib/AstroLib.vcxproj.filters b/AstroLib/AstroLib.vcxproj.filters
index 05782ac..86f44cf 100644
--- a/AstroLib/AstroLib.vcxproj.filters
+++ b/AstroLib/AstroLib.vcxproj.filters
@@ -9,6 +9,9 @@
       <UniqueIdentifier>{af1c9278-c83b-4b59-a9a1-1735fbc3c6aa}</UniqueIdentifier>
       <Extensions>h;hpp;hxx;hm;inl</Extensions>
     </Filter>
+    <Filter Include="婧愭枃浠�">
+      <UniqueIdentifier>{d67e5173-e6b6-4afe-9c15-f55de9aad894}</UniqueIdentifier>
+    </Filter>
   </ItemGroup>
   <ItemGroup>
     <ClInclude Include="Include\AsAttitudeParam.h">
@@ -65,6 +68,9 @@
     <ClInclude Include="Include\AsAttitude.h">
       <Filter>Header Files</Filter>
     </ClInclude>
+    <ClInclude Include="AsAttitudeParam_2023.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
   </ItemGroup>
   <ItemGroup>
     <ClCompile Include="Src\AsAtmosphere.cpp">
@@ -115,6 +121,9 @@
     <ClCompile Include="Src\AsAttitude.cpp">
       <Filter>Source Files</Filter>
     </ClCompile>
+    <ClCompile Include="AsAttitudeParam_2023.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
   </ItemGroup>
   <ItemGroup>
     <Text Include="AstroLib_ReadMe.txt" />
-- 
Gitee