diff --git a/AstroLib/Include/AsAttitudeParam_2023.h b/AstroLib/Include/AsAttitudeParam_2023.h index e58f7934e795f8ee45e68effdc84e137123c8294..771845069ca38337f09068e81ee65150f92611e7 100644 --- a/AstroLib/Include/AsAttitudeParam_2023.h +++ b/AstroLib/Include/AsAttitudeParam_2023.h @@ -3,6 +3,7 @@ #include "AsCoordinate.h" #include "AsMath.h" #include "AsAttitudeParam.h" +#include "AstroLib.h" ///*********************************************************************** /// The conversion from matrix to axis and angle. @@ -15,4 +16,18 @@ /// @Param angle 旋转角[0, pi] ///*********************************************************************** void AsMtxToAxAng(const CMatrix& mtx, - CCoord& axis, double& angle); \ No newline at end of file + CCoord& axis, double& angle); + + +///*********************************************************************** +/// 方向余弦阵转321转序欧拉角 +/// @Author Li Sibei +/// @Date 2023.04.06 +/// @Input +/// @Param mtx 方向余弦阵 +/// @Output +/// @Param euler 欧拉角 +///*********************************************************************** +void AsMtxToEuler321( + const CMatrix& mtx, + CEuler& euler); \ No newline at end of file diff --git a/AstroLib/Src/AsAttitudeParam_2023.cpp b/AstroLib/Src/AsAttitudeParam_2023.cpp index f54421b8475fb3470523b283c9576906185a8450..4e1341e4b4c3b3fe07a6e6578cf6fbd5c2d76636 100644 --- a/AstroLib/Src/AsAttitudeParam_2023.cpp +++ b/AstroLib/Src/AsAttitudeParam_2023.cpp @@ -1,6 +1,7 @@ #include "AsCoordinate.h" #include "AsMath.h" #include "AsAttitudeParam.h" +#include "AstroLib.h" ///*********************************************************************** /// The conversion from matrix to axis and angle. @@ -24,8 +25,8 @@ void AsMtxToAxAng(const CMatrix& mtx, 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) ){ + 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; @@ -88,4 +89,56 @@ void AsMtxToAxAng(const CMatrix& mtx, axis[1] = 0; axis[2] = 0; } +} + + +///*********************************************************************** +/// 方向余弦阵转为321转序欧拉角 +/// @Author Li Sibei +/// @Date 2023.04.06 +/// @Input +/// @Param mtx 坐标转换矩阵 +/// @Output +/// @Param angle 欧拉角 +///*********************************************************************** +void AsMtxToEuler321(const CMatrix& mtx, CEuler& euler) +{ + /*异常数据处理1:输入非正交矩阵*/ + CMatrix MTX1(mtx); + CMatrix mtx1 = MTX1.Transpose(); + CMatrix mtx2 = MTX1.Inv(); + CMatrix mtx3 = mtx1 - mtx2; + for (int i = 0; i <= 2; i++) + { + for (int j = 0; j <= 2; j++) + { + if (mtx3[i][j] > 1e-15 || mtx3[i][j] < -1e-15) + { + printf("Error: 方向余弦阵非正交矩阵!\n"); + return; + } + } + } + /*异常数据处理2:奇异*/ + if (mtx[0][2] == 1) + { + euler.m_Angle1 = -atan2(mtx[2][1], mtx[1][1]); + euler.m_Angle2 = -asin(mtx[0][2]); + euler.m_Angle3 = 0; + return; + } + else if (mtx[0][2] == -1) + { + euler.m_Angle1 = atan2(mtx[2][1], mtx[1][1]); + euler.m_Angle2 = -asin(mtx[0][2]); + euler.m_Angle3 = 0; + return; + } + /*一般情况求解*/ + else + { + euler.m_Angle1 = atan2(mtx[0][1], mtx[0][0]); + euler.m_Angle2 = -asin(mtx[0][2]); + euler.m_Angle3 = atan2(mtx[1][2], mtx[2][2]); + } } \ No newline at end of file