From 57eade83b60c8dbc7fb86c5d6e096c6ad66daa85 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E5=BC=A0=E8=AF=B4=E8=B0=8E?= <12711057+zhang-lies@user.noreply.gitee.com> Date: Mon, 10 Apr 2023 12:37:29 +0000 Subject: [PATCH] =?UTF-8?q?update=20AstroLib/Src/AsOrbitParam.cpp.=20?= =?UTF-8?q?=E4=BF=AE=E6=AD=A3=E8=BD=A8=E9=81=93=E6=A0=B9=E6=95=B0=E8=BD=AC?= =?UTF-8?q?=E4=BD=8D=E7=BD=AE=E9=80=9F=E5=BA=A6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: 张说谎 <12711057+zhang-lies@user.noreply.gitee.com> --- AstroLib/Src/AsOrbitParam.cpp | 102 +++++++++++++++++++++++++++------- 1 file changed, 81 insertions(+), 21 deletions(-) diff --git a/AstroLib/Src/AsOrbitParam.cpp b/AstroLib/Src/AsOrbitParam.cpp index a737c6b..70b4357 100644 --- a/AstroLib/Src/AsOrbitParam.cpp +++ b/AstroLib/Src/AsOrbitParam.cpp @@ -34,13 +34,13 @@ CCartState::CCartState(double xPos, double yPos, double zPos, double xVel, doubl { } -// +//重载求负运算 const CCartState CCartState::operator -() const { return CCartState(-m_Pos, -m_Vel); } -// +//重载相减运算 const CCartState& CCartState::operator -=(const CCartState& state) { m_Pos -= state.m_Pos; @@ -107,12 +107,12 @@ COrbElem::COrbElem(double sMajAx, double ecc, double i, //******************************************************************** -/// λúٶʸõĹٶ +/// 从位置和速度矢量得到飞行器的轨道角速度 /// @Author Wang Hua /// @Date 2005.3.4 /// @Input -/// @Param pos λ -/// @Param vel ٶ +/// @Param pos 飞行器位置 +/// @Param vel 飞行器速度 /// @Output /// @Param orbAngVel orbit angular velocity /// @Return @@ -128,18 +128,18 @@ bool AsCartStateToOrbAngVel( //******************************************************************** -/// ɹϵеľٶȺλüĿVVLHϵ(xָǰzָģ -/// yֱ)еλٶʸ +/// 由惯性系中的绝对速度和位置计算目标VVLH坐标系(x指向前，z指向地心， +/// y垂直轨道面)中的相对位置坐标和速度矢量 /// @author Wang Hua /// @Date 2005.12.22 /// @Input -/// @Param assocPos λã׷ٷλ -/// @Param assocVel ٶȣ׷ٷٶ -/// @Param basePos ολãĿλ -/// @Param baseVel οٶȣĿٶ +/// @Param assocPos 绝对位置，即追踪飞行器位置 +/// @Param assocVel 绝对速度，即追踪飞行器速度 +/// @Param basePos 参考飞行器位置，即目标飞行器位置 +/// @Param baseVel 参考飞行器速度，即目标飞行器速度 /// @Output -/// @Param relPos Ŀϵеλ -/// @Param relVel Ŀϵеٶ +/// @Param relPos 目标轨道坐标系中的相对位置 +/// @Param relVel 目标轨道坐标系中的相对速度 //******************************************************************** void AsStateToVVLHRelState( const CCoord& assocPos, const CCoord& assocVel, @@ -160,18 +160,18 @@ void AsStateToVVLHRelState( //******************************************************************** -/// ĿVVLHϵ(xָǰzָģyֱ)еλ -/// ٶʸϵеľٶȺλ +/// 由目标VVLH坐标系(x指向前，z指向地心，y垂直轨道面)中的相对位置坐标和 +/// 速度矢量计算惯性系中的绝对速度和位置 /// @author Wang Hua /// @Date 2005.12.22 /// @Input -/// @Param relPos Ŀϵеλ -/// @Param relVel Ŀϵеٶ -/// @Param basePos ολãĿλ -/// @Param baseVel οٶȣĿٶ +/// @Param relPos 目标轨道坐标系中的相对位置 +/// @Param relVel 目标轨道坐标系中的相对速度 +/// @Param basePos 参考飞行器位置，即目标飞行器位置 +/// @Param baseVel 参考飞行器速度，即目标飞行器速度 /// @Output -/// @Param assocPos λã׷ٷλ -/// @Param assocVel ٶȣ׷ٷٶ +/// @Param assocPos 绝对位置，即追踪飞行器位置 +/// @Param assocVel 绝对速度，即追踪飞行器速度 //******************************************************************** void AsVVLHRelStateToState( const CCoord& relPos, const CCoord& relVel, @@ -188,4 +188,64 @@ void AsVVLHRelStateToState( AsCartStateToOrbAngVel(basePos, baseVel, angVel); assocVel += baseVel+angVel*assocPos; assocPos += basePos; +} + +/// 修正轨道根数转换为位置速度/modified orbit element to cartesian state element. +/// @Author Zhang Yu +/// @Date 2023/4/2 +/// @Input +/// @Param modOrb 修正轨道根数 +/// @Param gm 中心体引力常数 +/// @Output +/// @Param pos 位置 +/// @Param vel 速度 +/// @Return true=成功; false=输入错误 +//******************************************************************** +bool AsModOrbElemToCart(const CModOrbElem& modOrb, double gm, CCoord3& pos, CCoord3& vel) +{ + double m_PeriRad = modOrb.m_PeriRad; ///<近拱点半径 periapsis radius in m (0,∞) + double m_Ecc = modOrb.m_Ecc; ///<偏心率 eccentricity [0,∞) + double m_I = modOrb.m_I; ///<轨道倾角 inclination in radians [0,pi] + double m_RAAN = modOrb.m_RAAN; ///<升交点赤经 right ascension of ascending node [-pi,2pi] + double m_ArgPeri = modOrb.m_ArgPeri; ///<近拱点角 arg of periapsis in rad [0,2pi] + double m_TrueA = modOrb.m_TrueA; ///<真近点角 true anomaly in radians [-pi,2pi] + if ( gm <= 0 || m_PeriRad <= 0 || m_Ecc < 0 || m_I<0 || m_I>AsCPI || m_RAAN < -AsCPI || m_RAAN>2 * AsCPI || m_ArgPeri < 0 || m_ArgPeri>2 * AsCPI || m_TrueA < -AsCPI || m_TrueA>2 * AsCPI||(m_Ecc>=1&& m_TrueA>=AsCPI-acos(1.0/m_Ecc)&& m_TrueA<= AsCPI + acos(1.0 / m_Ecc))) + { + return 0; + } + else { + double p = m_PeriRad * (1 + m_Ecc); ///<半通径 + double miudh = sqrt(gm / p); ///<常数 + + double sinf = sin(m_TrueA); + double cosf = cos(m_TrueA); + double cosi = cos(m_I); + double sini = sin(m_I); + double cosAr = cos(m_ArgPeri); + double sinAr = sin(m_ArgPeri); + double cosRA = cos(m_RAAN); + double sinRA = sin(m_RAAN); + double cosRAsinAr = cosRA * sinAr; + double sinRAsinAr = sinRA * sinAr; + double cosRAcosAr = cosRA * cosAr; + double sinRAcosAr = sinRA * cosAr; + + double r = p / (1 + m_Ecc * cosf); ///<地心距大小 + double rcosf = r * cosf; + double rsinf = r * sinf; + + double iex = cosRAcosAr - sinRAsinAr * cosi; + double iey = sinRAcosAr + cosRAsinAr * cosi; + double iez = sini * sinAr; + double ipx = -cosRAsinAr - sinRAcosAr * cosi; + double ipy = -sinRAsinAr + cosRAcosAr * cosi; + double ipz = sini * cosAr; + pos[0] = rcosf * iex + rsinf * ipx; + pos[1] = rcosf * iey + rsinf * ipy; + pos[2] = rcosf * iez + rsinf * ipz; + vel[0] = -miudh * sinf * iex + miudh * (m_Ecc + cosf) * ipx; + vel[1] = -miudh * sinf * iey + miudh * (m_Ecc + cosf) * ipy; + vel[2] = -miudh * sinf * iez + miudh * (m_Ecc + cosf) * ipz; + return 1; + } } \ No newline at end of file -- Gitee