diff --git a/AstroLib/Src/AsOrbitParam.cpp b/AstroLib/Src/AsOrbitParam.cpp index a737c6bf956a5acf481ac51d56a7d4d12dec158b..70b4357ad3b17d39c4369dcac03614a802e17daa 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垂直轨道面)中的相对位置坐标和速度矢量 +/// 鐢辨儻鎬х郴涓殑缁濆閫熷害鍜屼綅缃绠楃洰鏍嘨VLH鍧愭爣绯(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垂直轨道面)中的相对位置坐标和 -/// 速度矢量计算惯性系中的绝对速度和位置 +/// 鐢辩洰鏍嘨VLH鍧愭爣绯(x鎸囧悜鍓嶏紝z鎸囧悜鍦板績锛寉鍨傜洿杞ㄩ亾闈)涓殑鐩稿浣嶇疆鍧愭爣鍜 +/// 閫熷害鐭㈤噺璁$畻鎯х郴涓殑缁濆閫熷害鍜屼綅缃 /// @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