diff --git a/AstroLib/Src/AsAttitudeParam.cpp b/AstroLib/Src/AsAttitudeParam.cpp index 08aea9d1ebbdbaeac40012583b836f466ba892b8..283a5f0e0cc522833c95c6738452020f6b989626 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,9 +674,9 @@ 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) { @@ -684,3 +684,158 @@ void AsQuatToMtx(const CQuaternion& quat, CMatrix& mtx) } +#include +#include +using namespace std; + +class Vector +{ +public: + double x, y, z; + + Vector(double x_, double y_, double z_) + { + x = x_; + y = y_; + z = z_; + } + + Vector operator-(const Vector& vec) const + { + return Vector(x - vec.x, y - vec.y, z - vec.z); + } + + Vector operator*(const Vector& vec) const + { + return Vector(y * vec.z - z * vec.y, z * vec.x - x * vec.z, x * vec.y - y * vec.x); + } + + double dot(const Vector& vec) const + { + return x * vec.x + y * vec.y + z * vec.z; + } + + double norm() const + { + return sqrt(x * x + y * y + z * z); + } + + Vector normalize() const + { + double length = norm(); + return Vector(x / length, y / length, z / length); + } +}; + +class Matrix +{ +public: + double data[3][3]; + + Matrix() + { + for (int i = 0; i < 3; i++) + { + for (int j = 0; j < 3; j++) + { + data[i][j] = 0.0; + } + } + } + + Vector operator*(const Vector& vec) const + { + return Vector(data[0][0] * vec.x + data[0][1] * vec.y + data[0][2] * vec.z, + data[1][0] * vec.x + data[1][1] * vec.y + data[1][2] * vec.z, + data[2][0] * vec.x + data[2][1] * vec.y + data[2][2] * vec.z); + } + + Matrix operator*(const Matrix& mat) const + { + Matrix result; + for (int i = 0; i < 3; i++) + { + for (int j = 0; j < 3; j++) + { + for (int k = 0; k < 3; k++) + { + result.data[i][j] += data[i][k] * mat.data[k][j]; + } + } + } + return result; + } + + void alg(double alg[3][3])//鐭╅樀姹傞 + { + double a1 = data[0][0] * (data[1][1] * data[2][2] - data[1][2] * data[2][1]); + double a2 = data[0][1] * (data[1][2] * data[2][0] - data[1][0] * data[2][2]); + double a3 = data[0][2] * (data[1][0] * data[2][1] - data[1][1] * data[2][0]); + double det = a1 + a2 + a3; + if (det == 0) + { + cout << "鐭╅樀涓嶅彲閫"; + } + else + { + alg[0][0] = (data[1][1] * data[2][2] - data[1][2] * data[2][1]) / det; + alg[1][0] = (data[1][2] * data[2][0] - data[1][0] * data[2][2]) / det; + alg[2][0] = (data[1][0] * data[2][1] - data[2][0] * data[1][1]) / det; + alg[0][1] = (data[0][2] * data[2][1] - data[0][1] * data[2][2]) / det; + alg[1][1] = (data[0][0] * data[2][2] - data[2][0] * data[0][2]) / det; + alg[2][1] = (data[0][1] * data[2][0] - data[0][0] * data[2][1]) / det; + alg[0][2] = (data[0][1] * data[1][2] - data[1][1] * data[0][2]) / det; + alg[1][2] = (data[1][0] * data[0][2] - data[0][0] * data[1][2]) / det; + alg[2][2] = (data[0][0] * data[1][1] - data[1][0] * data[0][1]) / det; + } + } +}; + +void LvlhToInertial(const Vector& pos, const Vector& vel, double s[3][3]) +{ + Vector r(pos.x, pos.y, pos.z); + Vector v(vel.x, vel.y, vel.z); + Vector h = r * v; + Vector e_z = h.normalize(); + Vector e_x = r.normalize(); + Vector e_y = e_z * e_x; + + Matrix lvlh_to_intertial; + lvlh_to_intertial.data[0][0] = e_x.x; lvlh_to_intertial.data[0][1] = e_y.x; lvlh_to_intertial.data[0][2] = e_z.x; + lvlh_to_intertial.data[1][0] = e_x.y; lvlh_to_intertial.data[1][1] = e_y.y; lvlh_to_intertial.data[1][2] = e_z.y; + lvlh_to_intertial.data[2][0] = e_x.z; lvlh_to_intertial.data[2][1] = e_y.z; lvlh_to_intertial.data[2][2] = e_z.z; + lvlh_to_intertial.alg(s); +} + +int main() +{ + // 杈撳叆鍗槦鐨勪綅缃拰閫熷害 + Vector pos(0.0, 1.0, 0.0); + Vector vel(1.0, 0.0, 0.0); + double s[3][3]; + if (pos.x == 0 ) + if (pos.y ==0) + if (pos.z == 0) + cout << "Input position error" << endl;//瀵归敊璇殑浣嶇疆杈撳叆杩涜鎶ュ憡 + if (vel.x==0) + if (vel.x==0) + if (vel.z==0) + cout << "Input velocity error" << endl;//瀵归敊璇殑閫熷害杈撳叆杩涜鎶ュ憡 + + + // 璁＄畻浠嶦CI鍧愭爣绯诲埌LVLH鍧愭爣绯荤殑鏃嬭浆鐭╅樀 + LvlhToInertial(pos, vel, s); + + // 杈撳嚭鏃嬭浆鐭╅樀 + cout << "The transformation matrix from ECI frame to LVLH frame is:" << endl; + for (int i = 0; i < 3; i++) + { + for (int j = 0; j < 3; j++) + { + cout << s[i][j] << "\t"; + } + cout << endl; + } + //鍏堜笂浼犳病鏈夋帴鍏ョ幇鏈夌▼搴忕殑鐗堟湰 + return 0; +} \ No newline at end of file