From 819b7cc0a0bd4a4407a47807d5d2b6a230af6c93 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E5=91=A8=E7=9D=BF=E9=80=B8?= <12707050+zhouruiyi@user.noreply.gitee.com> Date: Fri, 14 Apr 2023 06:25:59 +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: 周睿逸 <12707050+zhouruiyi@user.noreply.gitee.com> --- AstroLib/Src/AsAttitudeParam.cpp | 281 ++++++++++++++++++++++++------- 1 file changed, 218 insertions(+), 63 deletions(-) diff --git a/AstroLib/Src/AsAttitudeParam.cpp b/AstroLib/Src/AsAttitudeParam.cpp index 08aea9d..283a5f0 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,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;//对错误的速度输入进行报告 + + + // 计算从ECI坐标系到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 -- Gitee