From aa36e5ed7af0bd9d82f539a3aed97f890ec243f1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=80=9D=E8=B4=9D=20=E6=9D=8E?= Date: Mon, 10 Apr 2023 20:46:10 +0800 Subject: [PATCH 1/2] =?UTF-8?q?=E6=96=B9=E5=90=91=E4=BD=99=E5=BC=A6?= =?UTF-8?q?=E9=98=B5=E8=BD=AC321=E8=BD=AC=E5=BA=8F=E6=AC=A7=E6=8B=89?= =?UTF-8?q?=E8=A7=92?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- AstroLib/Include/AsAttitudeParam_2023.h | 34 +++--- AstroLib/Src/AsAttitudeParam_2023.cpp | 142 +++++++++--------------- 2 files changed, 68 insertions(+), 108 deletions(-) diff --git a/AstroLib/Include/AsAttitudeParam_2023.h b/AstroLib/Include/AsAttitudeParam_2023.h index e58f793..f405de2 100644 --- a/AstroLib/Include/AsAttitudeParam_2023.h +++ b/AstroLib/Include/AsAttitudeParam_2023.h @@ -1,18 +1,16 @@ -#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& mtx, - CCoord& axis, double& angle); \ No newline at end of file +#pragma once +#include "AstroLib.h" + + +///*********************************************************************** +/// 方向余弦阵转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 f54421b..6e8aeeb 100644 --- a/AstroLib/Src/AsAttitudeParam_2023.cpp +++ b/AstroLib/Src/AsAttitudeParam_2023.cpp @@ -1,91 +1,53 @@ -#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& mtx, - CCoord& axis, double& angle) -{ - - CMatrix 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; - } +#include "AstroLib.h" + + +///*********************************************************************** +/// 方向余弦阵转为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 -- Gitee From eee8d53b82d91f1b92e5fb305fb456de64c24214 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=80=9D=E8=B4=9D=20=E6=9D=8E?= Date: Mon, 10 Apr 2023 21:30:22 +0800 Subject: [PATCH 2/2] =?UTF-8?q?=E4=BF=AE=E6=94=B9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- AstroLib/Include/AsAttitudeParam_2023.h | 21 +++++- AstroLib/Src/AsAttitudeParam_2023.cpp | 93 ++++++++++++++++++++++++- 2 files changed, 111 insertions(+), 3 deletions(-) diff --git a/AstroLib/Include/AsAttitudeParam_2023.h b/AstroLib/Include/AsAttitudeParam_2023.h index f405de2..7718450 100644 --- a/AstroLib/Include/AsAttitudeParam_2023.h +++ b/AstroLib/Include/AsAttitudeParam_2023.h @@ -1,5 +1,22 @@ -#pragma once -#include "AstroLib.h" +#pragma once + +#include "AsCoordinate.h" +#include "AsMath.h" +#include "AsAttitudeParam.h" +#include "AstroLib.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& mtx, + CCoord& axis, double& angle); ///*********************************************************************** diff --git a/AstroLib/Src/AsAttitudeParam_2023.cpp b/AstroLib/Src/AsAttitudeParam_2023.cpp index 6e8aeeb..4e1341e 100644 --- a/AstroLib/Src/AsAttitudeParam_2023.cpp +++ b/AstroLib/Src/AsAttitudeParam_2023.cpp @@ -1,4 +1,95 @@ -#include "AstroLib.h" +#include "AsCoordinate.h" +#include "AsMath.h" +#include "AsAttitudeParam.h" +#include "AstroLib.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& mtx, + CCoord& axis, double& angle) +{ + + CMatrix 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; + } +} ///*********************************************************************** -- Gitee