From cf5bd3358eb177cf282c33db3e3fcfd4c75675fa Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E5=AE=AB=E8=B4=BA?= <12742204+gonghe18@user.noreply.gitee.com> Date: Sun, 16 Apr 2023 11:06:42 +0000 Subject: [PATCH] update AstroLib/Src/AsAttitudeParam.cpp. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: 宫贺 <12742204+gonghe18@user.noreply.gitee.com> --- AstroLib/Src/AsAttitudeParam.cpp | 168 +++++++++++++++++++------------ 1 file changed, 105 insertions(+), 63 deletions(-) diff --git a/AstroLib/Src/AsAttitudeParam.cpp b/AstroLib/Src/AsAttitudeParam.cpp index 08aea9d..d0f52e3 100644 --- a/AstroLib/Src/AsAttitudeParam.cpp +++ b/AstroLib/Src/AsAttitudeParam.cpp @@ -37,13 +37,13 @@ CEuler::~CEuler() ///*********************************************************************** -/// תǶȺתתƾ +/// 根据旋转角度和旋转轴计算坐标转移矩阵 /// @Author Wang Hua /// @Date 2004.11.24 /// @Input -/// @Param seqFlag תsequence +/// @Param seqFlag 旋转sequence /// @Output -/// @Param mtx صתƾ +/// @Param mtx 返回的转移矩阵 ///*********************************************************************** void CEuler::ToMtx (int seqFlag, CMatrix& mtx) const { @@ -69,13 +69,13 @@ void CEuler::ToMtx (int seqFlag, CMatrix& mtx) const ///*********************************************************************** -/// תǶȺתquaternion +/// 根据旋转角度和旋转轴计算quaternion /// @Author Wang Hua /// @Date 2004.11.24 /// @Input -/// @Param seqFlag תsequence +/// @Param seqFlag 旋转sequence /// @Output -/// @Param quat Ԫ +/// @Param quat 四元数 ///*********************************************************************** void CEuler::ToQuat (int seqFlag, CQuaternion& quat) const { @@ -229,7 +229,7 @@ bool CQuaternion::operator!=(const CQuaternion& q) const } -//õԪĹԪ +//得到四元数的共轭四元数 const CQuaternion CQuaternion::Conjugate() const { CQuaternion q; @@ -240,14 +240,14 @@ const CQuaternion CQuaternion::Conjugate() const return q; } -//õԪԪ +//得到四元数的逆元数 const CQuaternion CQuaternion::Invert() const { return Conjugate()/Norm(); } -//õԪķ +//得到四元数的范数 double CQuaternion::Norm() const { return m_Qs*m_Qs+m_Qx*m_Qx+m_Qy*m_Qy+m_Qz*m_Qz; @@ -263,12 +263,12 @@ void CQuaternion::Unit() ///*********************************************************************** -/// תǶȺתԪ +/// 根据旋转角度和旋转坐标轴计算四元数 /// @Author Wang Hua /// @Date 2004.3 /// @Input -/// @Param angle תǶ -/// @Param axFlag ת(1,2,3-X,Y,Z) +/// @Param angle 旋转角度 +/// @Param axFlag 旋转轴(1,2,3-X,Y,Z) /// @Output ///*********************************************************************** void CQuaternion::FromEuler(double angle, int axFlag) @@ -299,12 +299,12 @@ void CQuaternion::FromEuler(double angle, int axFlag) ///*********************************************************************** -/// EulerǺתԪ +/// 根据Euler角和旋转坐标轴计算四元数 /// @Author Wang Hua /// @Date 2004.11.24 /// @Input -/// @Param euler תEuler -/// @Param seqFlag ת(1,2,3-X,Y,Z) +/// @Param euler 旋转Euler角 +/// @Param seqFlag 旋转轴(1,2,3-X,Y,Z) /// @Output ///*********************************************************************** void CQuaternion::FromEuler(const CEuler& angle, int seqFlag) @@ -333,12 +333,12 @@ void CQuaternion::FromEuler(const CEuler& angle, int seqFlag) ///*********************************************************************** -/// תǶȺתԪ +/// 根据旋转角度和旋转坐标轴计算四元数 /// @Author Wang Hua /// @Date 2004.10.22 /// @Input -/// @Param angle ת -/// @Param seqFlag ת(1,2,3-X,Y,Z) +/// @Param angle 旋转角 +/// @Param seqFlag 旋转轴(1,2,3-X,Y,Z) ///*********************************************************************** void CQuaternion::FromEuler(const CVector& angle, const CVector& seqFlag) { @@ -361,12 +361,12 @@ void CQuaternion::FromEuler(const CVector& angle, const CVector& se ///*********************************************************************** -/// תǶȺתԪ +/// 根据旋转角度和旋转轴计算四元数 /// @Author Wang Hua /// @Date 2004.10.12 /// @Input -/// @Param axis תʸ -/// @Param angle ת +/// @Param axis 旋转轴矢量 +/// @Param angle 旋转角 ///*********************************************************************** void CQuaternion::FromAxAng(const CCoord& axis, double angle) { @@ -382,12 +382,12 @@ void CQuaternion::FromAxAng(const CCoord& axis, double angle) ///*********************************************************************** -/// ԪתǶȺת +/// 根据四元数计算旋转角度和旋转轴 /// @Author Wang Hua /// @Date 2004.10.26 /// @Output -/// @Param angle תǶ -/// @Param axFlag ת +/// @Param angle 旋转角度 +/// @Param axFlag 旋转轴 ///*********************************************************************** void CQuaternion::ToAxAng(CCoord& axis, double& angle) const { @@ -409,11 +409,11 @@ void CQuaternion::ToAxAng(CCoord& axis, double& angle) const ///*********************************************************************** -/// ҾתԪ +/// 方向余弦矩阵转换成四元数 /// @Author Wang Hua /// @Date 2004.4 /// @Input -/// @Param mtx ת +/// @Param mtx 旋转矩阵 ///*********************************************************************** void CQuaternion::FromMtx(const CMatrix& m) { @@ -429,12 +429,12 @@ void CQuaternion::FromMtx(const CMatrix& m) ///*********************************************************************** -/// ԪתɷҾ +/// 四元数转换成方向余弦矩阵 /// @Author Wang Hua /// @Date 2004.4 /// @Output -/// @Param axis תʸ -/// @Param angle ת +/// @Param axis 旋转轴矢量 +/// @Param angle 旋转角 ///*********************************************************************** void CQuaternion::ToMtx(CMatrix& m) const { @@ -468,14 +468,14 @@ void CQuaternion::ToMtx(CMatrix& m) const ///*********************************************************************** -/// תǶȺתתƾ +/// 根据旋转角度和旋转轴计算坐标转移矩阵 /// @Author Wang Hua /// @Date 2004.3 /// @Input -/// @Param angle תǶ -/// @Param axFlag ת +/// @Param angle 旋转角度 +/// @Param axFlag 旋转轴 /// @Output -/// @Param mtx صתƾ +/// @Param mtx 返回的转移矩阵 ///*********************************************************************** void AsEulerToMtx(double angle, int axFlag, CMatrix& mtx) { @@ -505,14 +505,14 @@ void AsEulerToMtx(double angle, int axFlag, CMatrix& mtx) ///*********************************************************************** -/// תǶȺתתƾ +/// 根据旋转角度和旋转轴计算坐标转移矩阵 /// @Author Wang Hua /// @Date 2004.11.24 /// @Input -/// @Param euler Euler -/// @Param seqFlag תsequence +/// @Param euler Euler角 +/// @Param seqFlag 旋转sequence /// @Output -/// @Param mtx صתƾ +/// @Param mtx 返回的转移矩阵 ///*********************************************************************** void AsEulerToMtx (const CEuler& euler, int seqFlag, CMatrix& mtx) @@ -526,15 +526,15 @@ void AsEulerToMtx (const CEuler& euler, int seqFlag, ///*********************************************************************** -/// תǶȺתתƾ +/// 根据旋转角度和旋转轴计算坐标转移矩阵 /// @Author Wang Hua /// @Date 2004.9.7 /// @Input -/// @Param angle תǶ() -/// @Param seqFlag ת־:1=x,2=y,3=z -/// @Param n Ľǵĸ +/// @Param angle 旋转角度(弧度) +/// @Param seqFlag 旋转轴标志:1=x,2=y,3=z +/// @Param n 输入的角的个数 /// @Output -/// @Param mtx תƾ +/// @Param mtx 转移矩阵 ///*********************************************************************** void AsEulerToMtx(const double angle[], const int seqFlag[], int n, CMatrix& mtx) @@ -556,14 +556,14 @@ void AsEulerToMtx(const double angle[], ///*********************************************************************** -/// תǶȺתתƾ +/// 根据旋转角度和旋转轴计算坐标转移矩阵 /// @Author Wang Hua /// @Date 2004.9.7 /// @Input -/// @Param angle תǶ() -/// @Param seqFlag ת־:1=x,2=y,3=z +/// @Param angle 旋转角度(弧度) +/// @Param seqFlag 旋转轴标志:1=x,2=y,3=z /// @Output -/// @Param mtx תƾ +/// @Param mtx 转移矩阵 ///*********************************************************************** void AsEulerToMtx(const CVector& angle, const CVector& seqFlag, CMatrix& mtx) @@ -574,14 +574,14 @@ void AsEulerToMtx(const CVector& angle, ///*********************************************************************** -/// EulerǼquaternion +/// 根据Euler角计算quaternion /// @Author Wang Hua /// @Date 2004.11.24 /// @Input -/// @Param euler Euler -/// @Param seqFlag תsequence +/// @Param euler Euler角 +/// @Param seqFlag 旋转sequence /// @Output -/// @Param quat Ԫ +/// @Param quat 四元数 ///*********************************************************************** void AsEulerToQuat (const CEuler& euler, int seqFlag, CQuaternion& quat) @@ -595,10 +595,10 @@ void AsEulerToQuat (const CEuler& euler, int seqFlag, /// @Author Wang Hua /// @Date 2004.10.13 /// @Input -/// @Param axis תʸ -/// @Param angle ת +/// @Param axis 旋转轴矢量 +/// @Param angle 旋转角 /// @Output -/// @Param mtx תƾ +/// @Param mtx 转移矩阵 ///*********************************************************************** void AsAxAngToMtx(const CCoord& axis, double angle, CMatrix& mtx) @@ -625,10 +625,10 @@ void AsAxAngToMtx(const CCoord& axis, double angle, /// @Author Wang Hua /// @Date 2004.11.24 /// @Input -/// @Param axis תʸ -/// @Param angle ת +/// @Param axis 旋转轴矢量 +/// @Param angle 旋转角 /// @Output -/// @Param quat Ԫ +/// @Param quat 四元数 ///*********************************************************************** void AsAxAngToQuat(const CCoord& axis, double angle, CQuaternion& quat) @@ -642,9 +642,9 @@ void AsAxAngToQuat(const CCoord& axis, double angle, /// @Author Wang Hua /// @Date 2004.11.24 /// @Input -/// @Param mtx תƾ +/// @Param mtx 转移矩阵 /// @Output -/// @Param quat Ԫ +/// @Param quat 四元数 ///*********************************************************************** void AsMtxToQuat (const CMatrix& mtx, CQuaternion& quat) { @@ -657,10 +657,10 @@ void AsMtxToQuat (const CMatrix& mtx, CQuaternion& quat) /// @Author Wang Hua /// @Date 2004.11.24 /// @Input -/// @Param quat Ԫ +/// @Param quat 四元数 /// @Output -/// @Param axis תʸ -/// @Param angle ת[0, pi] +/// @Param axis 旋转轴矢量 +/// @Param angle 旋转角[0, pi] ///*********************************************************************** void AsQuatToAxAng(const CQuaternion& quat, CCoord& axis, double& angle) @@ -674,13 +674,55 @@ void AsQuatToAxAng(const CQuaternion& quat, CCoord& axis, /// @Author Wang Hua /// @Date 2004.11.24 /// @Input -/// @Param quat Ԫ +/// @Param quat 四元数 /// @Output -/// @Param mtx תƾ +/// @Param mtx 转移矩阵 ///*********************************************************************** void AsQuatToMtx(const CQuaternion& quat, CMatrix& mtx) { quat.ToMtx(mtx); } +///*********************************************************************** +/// The conversion from matrix to angle123. +/// @Author gong he +/// @Date 2023.4.14 +/// @Input +/// @Param mtx 坐标转换矩阵 +/// @Output +/// @Param angle 旋转角[0, pi] +///*********************************************************************** +void AsMtxtoEuler123(const CMatrix& mtx, + CEuler& angle) +{ + CMatrix mm = mtx; + float theta = 0, psi = 0, pfi = 0; + if (abs(mm[2][0]) != 1) { + if (cos(-asin(mm(2, 0)) >= 0)) { + theta= -asin(mm(2, 0)); + psi= atan2(mm(2, 1) / cos(theta), mm(2, 2) / cos(theta)); + pfi= atan2(mm(1, 0) / cos(theta), mm(0, 0) / cos(theta)); + } + else { + theta= AsCPI - theta; + psi= atan2(mm(2, 0) / cos(theta), mm(2, 2) / cos(theta)); + pfi= atan2(mm(1, 0) / cos(theta), mm(0, 0) / cos(theta)); + } + } + else { + float pfi = 0; + float delta = atan2(mm(0, 1), mm(0, 2)); + if (mm[2][0] == -1) { + theta = AsCPI / 2; + psi = pfi + delta; + } + else { + theta = -AsCPI / 2; + psi = -pfi + delta; + } + } + angle.m_Angle1 = pfi; + angle.m_Angle2 = theta; + angle.m_Angle3 = psi; +} -- Gitee