diff --git a/AstroLib/Src/AsAttitudeParam.cpp b/AstroLib/Src/AsAttitudeParam.cpp index 08aea9d1ebbdbaeac40012583b836f466ba892b8..d0f52e3bd10c91510b158db9625543c76f6265dd 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瑙掕绠梣uaternion /// @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; +}