diff --git a/AstroLib.sln b/AstroLib.sln index 72c5ad097db1bd401ec75a1abe04916597c32d3b..b39cf851d43c64ebe1edd058a6ab6fe5d8312f0f 100644 --- a/AstroLib.sln +++ b/AstroLib.sln @@ -1,7 +1,7 @@  Microsoft Visual Studio Solution File, Format Version 12.00 -# Visual Studio 14 -VisualStudioVersion = 14.0.23107.0 +# Visual Studio Version 17 +VisualStudioVersion = 17.6.33829.357 MinimumVisualStudioVersion = 10.0.40219.1 Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "AstroLib", "AstroLib\AstroLib.vcxproj", "{7EACEEBD-39A5-4708-8D6C-2AF7275FF08B}" EndProject @@ -22,6 +22,11 @@ Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "Examples", "Examples\Exampl EndProject Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "ParallelSim", "ParallelSim\ParallelSim.vcxproj", "{C5BD0DEB-8148-49C6-9000-5077C4A74E6D}" EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "MultiRendezvous", "MultiRendezvous\MultiRendezvous.vcxproj", "{BA8DF975-D3D0-4E76-B967-901368AFFE4D}" + ProjectSection(ProjectDependencies) = postProject + {7EACEEBD-39A5-4708-8D6C-2AF7275FF08B} = {7EACEEBD-39A5-4708-8D6C-2AF7275FF08B} + EndProjectSection +EndProject Global GlobalSection(SolutionConfigurationPlatforms) = preSolution Debug|Win32 = Debug|Win32 @@ -66,8 +71,19 @@ Global {C5BD0DEB-8148-49C6-9000-5077C4A74E6D}.Release|Win32.Build.0 = Release|Win32 {C5BD0DEB-8148-49C6-9000-5077C4A74E6D}.Release|x64.ActiveCfg = Release|x64 {C5BD0DEB-8148-49C6-9000-5077C4A74E6D}.Release|x64.Build.0 = Release|x64 + {BA8DF975-D3D0-4E76-B967-901368AFFE4D}.Debug|Win32.ActiveCfg = Debug|Win32 + {BA8DF975-D3D0-4E76-B967-901368AFFE4D}.Debug|Win32.Build.0 = Debug|Win32 + {BA8DF975-D3D0-4E76-B967-901368AFFE4D}.Debug|x64.ActiveCfg = Debug|x64 + {BA8DF975-D3D0-4E76-B967-901368AFFE4D}.Debug|x64.Build.0 = Debug|x64 + {BA8DF975-D3D0-4E76-B967-901368AFFE4D}.Release|Win32.ActiveCfg = Release|Win32 + {BA8DF975-D3D0-4E76-B967-901368AFFE4D}.Release|Win32.Build.0 = Release|Win32 + {BA8DF975-D3D0-4E76-B967-901368AFFE4D}.Release|x64.ActiveCfg = Release|x64 + {BA8DF975-D3D0-4E76-B967-901368AFFE4D}.Release|x64.Build.0 = Release|x64 EndGlobalSection GlobalSection(SolutionProperties) = preSolution HideSolutionNode = FALSE EndGlobalSection + GlobalSection(ExtensibilityGlobals) = postSolution + SolutionGuid = {C41E03FF-4D0A-44DF-AA4B-52A8CED3E8A7} + EndGlobalSection EndGlobal diff --git a/MultiRendezvous/MultiRendezvous.vcxproj b/MultiRendezvous/MultiRendezvous.vcxproj new file mode 100644 index 0000000000000000000000000000000000000000..410e7fcad3b9f23a001fd0e3b4ae9a8151f73468 --- /dev/null +++ b/MultiRendezvous/MultiRendezvous.vcxproj @@ -0,0 +1,165 @@ + + + + + Debug + Win32 + + + Release + Win32 + + + Debug + x64 + + + Release + x64 + + + + 16.0 + Win32Proj + {ba8df975-d3d0-4e76-b967-901368affe4d} + MultiRendezvous + 8.1 + + + + Application + true + v140 + Unicode + + + Application + false + v140 + true + Unicode + + + Application + true + v140 + Unicode + + + Application + false + v140 + true + Unicode + + + + + + + + + + + + + + + + + + + + + $(VC_IncludePath);$(WindowsSDK_IncludePath);../AstroLib + $(VC_LibraryPath_x86);$(WindowsSDK_LibraryPath_x86);$(NETFXKitsDir)Lib\um\x86../AstroLib + + + $(VC_IncludePath);$(WindowsSDK_IncludePath);../AstroLib + $(VC_LibraryPath_x86);$(WindowsSDK_LibraryPath_x86);$(NETFXKitsDir)Lib\um\x86../AstroLib + + + $(VC_IncludePath);$(WindowsSDK_IncludePath);../AstroLib + + + $(VC_IncludePath);$(WindowsSDK_IncludePath);../AstroLib + + + + Level3 + true + WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + .\;.\Src; + + + Console + true + + + + + Level3 + true + true + true + WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + .\;.\Src; + + + Console + true + true + true + + + + + Level3 + true + _DEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + .\;.\Src + + + Console + true + + + + + Level3 + true + true + true + NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + .\;.\Src + + + Console + true + true + true + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/MultiRendezvous/MultiRendezvous.vcxproj.filters b/MultiRendezvous/MultiRendezvous.vcxproj.filters new file mode 100644 index 0000000000000000000000000000000000000000..1b35f6bcc025bc9b997b8fceef8ceb5ea28d64c0 --- /dev/null +++ b/MultiRendezvous/MultiRendezvous.vcxproj.filters @@ -0,0 +1,54 @@ + + + + + {4FC737F1-C7A5-4376-A066-2A32D752A2FF} + cpp;c;cc;cxx;c++;cppm;ixx;def;odl;idl;hpj;bat;asm;asmx + + + {93995380-89BD-4b04-88EB-625FBE52EBFB} + h;hh;hpp;hxx;h++;hm;inl;inc;ipp;xsd + + + {67DA6AB6-F800-4c08-8B7A-83BB121AAD01} + rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms + + + + + 头文件 + + + 头文件 + + + 头文件 + + + 头文件 + + + 头文件 + + + 头文件 + + + 头文件 + + + + + 源文件 + + + 源文件 + + + 源文件 + + + 源文件 + + + \ No newline at end of file diff --git a/MultiRendezvous/MultiRendezvous.vcxproj.user b/MultiRendezvous/MultiRendezvous.vcxproj.user new file mode 100644 index 0000000000000000000000000000000000000000..88a550947edbc3c5003a41726f0749201fdb6822 --- /dev/null +++ b/MultiRendezvous/MultiRendezvous.vcxproj.user @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/MultiRendezvous/Src/ChCtrl.cpp b/MultiRendezvous/Src/ChCtrl.cpp new file mode 100644 index 0000000000000000000000000000000000000000..e32b2b79fa7aa250b468db99ca6811d3b7432d74 --- /dev/null +++ b/MultiRendezvous/Src/ChCtrl.cpp @@ -0,0 +1,262 @@ +// ChCtrl.cpp: implementation of the CChCtrl class. +// +////////////////////////////////////////////////////////////////////// + + +#include "ChCtrl.h" +#include "izzoLam.h" + + +////////////////////////////////////////////////////////////////////// +// Construction/Destruction +////////////////////////////////////////////////////////////////////// + +CChCtrl::CChCtrl() +{ + m_UseMeasureRandom = 0; + m_UseEngineRandom = 0; + m_PosStdDev = 0; + m_VelStdDev = 0; + for (int i = 0; i < 100; i++) + { + m_FlyTime[i] = 0; + } + m_PulseNum = 0; +} + +CChCtrl::~CChCtrl() +{ + +} + + +//********************************************************************* +/// 初始化 +/// @author Wang Hua +/// @Date 2005.11.16 +//********************************************************************* +void CChCtrl::Init() +{ + for (int i = 0; i < 100; i++) + { + m_FlyTime[i] = 0; + } + m_PulseNum = 0; + + //随机数初始化 + int seed = AsRandomSeed(); + for (int i=0; i<3; i++) + { + m_RandomPos[i].Init(seed-i*3-1); + m_RandomVel[i].Init(seed-i*3-2); + m_RandomEng[i].Init(seed-i*3-3); + } +} + + +//******************************************************************** +/// 测量计算，包含测量误差 +/// @author Wang Hua +/// @Date 2022-6-29 +/// @Input +/// @Param chPos 追踪位置 +/// @Param chVel 追踪速度 +/// @Param tgPos 目标位置 +/// @Param tgVel 目标速度 +/// @Output +/// @Param relPos 相对位置 +/// @Param relVel 相对速度 +//******************************************************************** +void CChCtrl::Measure(const CCoord& chPos, const CCoord& chVel, + const CCoord& tgPos, const CCoord& tgVel, + CCoord& relPos, CCoord& relVel) +{ + //计算相对位置速度 + AsStateToVVLHRelState(chPos, chVel, tgPos, tgVel, relPos, relVel); + + //增加测量偏差 + if (m_UseMeasureRandom==0) + { + return; + } + else if (m_UseMeasureRandom==1) + { + relPos[0] += m_RandomPos[0].Gauss()*m_PosStdDev; + relPos[1] += m_RandomPos[1].Gauss()*m_PosStdDev; + relPos[2] += m_RandomPos[2].Gauss()*m_PosStdDev; + relVel[0] += m_RandomVel[0].Gauss()*m_VelStdDev; + relVel[1] += m_RandomVel[1].Gauss()*m_VelStdDev; + relVel[2] += m_RandomVel[2].Gauss()*m_VelStdDev; + } + +} + + +//******************************************************************** +/// 计算执行结构执行后的脉冲 +/// @author Wang Hua +/// @Date 2022-6-29 +/// @In/Out +/// @Param impulse 相对位置 +//******************************************************************** +void CChCtrl::Engine(CCoord& impulse) +{ + //增加测量偏差 + if (m_UseEngineRandom==0) + { + return; + } + else if (m_UseEngineRandom==1) + { + impulse[0] += m_RandomEng[0].Gauss()*m_EngStdDev; + impulse[1] += m_RandomEng[1].Gauss()*m_EngStdDev; + impulse[2] += m_RandomEng[2].Gauss()*m_EngStdDev; + } + +} + + +//******************************************************************** +/// 根据CW方程/lambert方程外推计算下一步需要施加的冲量 +/// @Author Wang Hua/Wang Hanwei +/// @Date 2005.11.17 +/// @Input +/// @Param chPos 追踪位置(m) +/// @Param chVel 追踪速度(m/s) +/// @Param tgPos 目标位置(m) +/// @Param tgVel 目标速度(m/s) +/// @Param endRelPos 制导终点所需相对位置(m) +/// @Param flyTime 制导飞行时间 +/// @Output +/// @Param impulse 计算得到的冲量 +//******************************************************************** +void CChCtrl::GuidanceImpulse1( + const CCoord& chPos, const CCoord& chVel, + const CCoord& tgPos, const CCoord& tgVel, + const CCoord& endRelPos, + double flyTime, CCoord& impulse) +{ + if(1){ + double orbAngVel; + CCoord OrbAngVelVec; + CCoord relPos, relVel, m_tgPos, m_tgVel, m_tgPos1, m_tgVel1; + Measure(tgPos, tgVel, chPos, chVel, relPos, relVel); + AsVVLHRelStateToState(relPos, relVel, chPos, chVel, m_tgPos, m_tgVel); + COrbit m_Orbit; + for (int i = 0; i < flyTime; i++) + { + m_Orbit.OrbitStep(1, m_tgPos, m_tgVel); + } + AsVVLHRelStateToState(endRelPos, CCoord(0,0,0), m_tgPos, m_tgVel, m_tgPos1, m_tgVel1); + m_tgPos = m_tgPos1; + double r1[3]{ chPos[0], chPos[1], chPos[2] }, r2[3]{ m_tgPos[0],m_tgPos[1],m_tgPos[2] }, tof = flyTime, mu = 3.986e14, v1[1][3], v2[1][3]; + int way = 1, multi_revs = 0; + solve_lambert_izzo(r1, r2, tof, mu, way, multi_revs, v1, v2); + CCoord impulse2(v1[0][0], v1[0][1], v1[0][2]); + AsStateToVVLHRelState(chPos, impulse2, chPos, chVel, m_tgPos, impulse); + Engine(impulse); + }else{ + double orbAngVel; + CCoord impulse2; + CCoord OrbAngVelVec; + CCoord relPos, relVel; + Measure(chPos, chVel, tgPos, tgVel, relPos, relVel); + AsCartStateToOrbAngVel(tgPos, tgVel, OrbAngVelVec); + orbAngVel = OrbAngVelVec.Norm(); + AsCWImpulse(relPos, relVel, endRelPos, + CCoord(0, 0, 0), flyTime, orbAngVel, impulse, impulse2); + Engine(impulse); + } + +} + + +//******************************************************************** +//根据CW方程外推计算下一步需要施加的冲量，准备开始调姿 +/// @Author Wang Hua +/// @Date 2005.11.17 +/// @Input +/// @Param chPos 追踪位置(m) +/// @Param chVel 追踪速度(m/s) +/// @Param tgPos 目标位置(m) +/// @Param tgVel 目标速度(m/s) +/// @Param endRelPos 制导终点所需相对速度(m/s) +/// @Output +/// @Param impulse 计算得到的冲量 +//******************************************************************** +void CChCtrl::GuidanceImpulse2(const CCoord& chPos, const CCoord& chVel, + const CCoord& tgPos, const CCoord& tgVel, + const CCoord& endRelVel, CCoord& impulse) +{ + CCoord relPos, relVel; + + Measure(chPos, chVel, tgPos, tgVel, relPos, relVel); + impulse = -relVel; +} + + +//********************************************************************* +/// 计算控制,直接加冲量 +/// @author Wang Hua +/// @Date 2006.8.12 +/// @Param step 时间步长 +/// @Param chPos 惯性系中的追踪器位置(m) +/// @Param chVel 惯性系中的追踪器速度(m/s) +/// @Param tgPos 惯性系中的目标器位置(m) +/// @Param tgVel 惯性系中的目标器速度(m/s) +/// @Param flyTimeApproach 接近段飞行时间 +/// @Output +/// @Param isEnd 是否本阶段结束 +/// @Param impulse VVLH系中的脉冲 +/// @Return +//********************************************************************* +void CChCtrl::TimeAdvance(double step, + const CCoord& chPos, const CCoord& chVel, + const CCoord& tgPos, const CCoord& tgVel, + double flyTimeApproach, const CCoord& endPos, + bool& isEnd, CCoord& impulse, int aim) +{ + isEnd = false; + impulse.SetDataAll(0); + + //至终端 + if (m_PulseNum==0) + { + m_PulseNum = 1; + GuidanceImpulse1(chPos, chVel, tgPos, tgVel, endPos, flyTimeApproach, impulse); + double t = 0; + for (int i = 0; i <= 3; i++) + { + t = t + m_FlyTime[i]; + } + CCTime c_t(2024, 3, 11, 12, 0, 0.0+t); + AsTimeToJD(c_t, t); + printf("儒略日（JD）：%f 远距离脉冲分量：%f %f %f \n", t, impulse[0], impulse[1], impulse[2]); + } + //终端冲量 + else if (m_PulseNum==1 && m_FlyTime[aim] >=flyTimeApproach) + { + double t = 0; + for (int i = 0; i <= 3; i++) + { + t = t + m_FlyTime[i]; + } + CCTime c_t(2024, 3, 11, 12, 0, 0.0 + t); + AsTimeToJD(c_t, t); + GuidanceImpulse2(chPos, chVel, tgPos, tgVel, endPos, impulse); + printf("儒略日（JD）：%f 近距离脉冲分量：%f %f %f \n", t, impulse[0], impulse[1], impulse[2]); + printf("儒略日（JD）：%f 观测目标%d \n", t, aim + 1); + m_PulseNum = 2; + } + else if (m_PulseNum == 2 && m_FlyTime[aim] >= flyTimeApproach+1800) + { + isEnd = true; + m_PulseNum = 0; + } + + //脉冲施加后时间累积 + m_FlyTime[aim] += step; +} + + + diff --git a/MultiRendezvous/Src/ChCtrl.h b/MultiRendezvous/Src/ChCtrl.h new file mode 100644 index 0000000000000000000000000000000000000000..962982dbbfe34725f2faa108905e386383ad8f35 --- /dev/null +++ b/MultiRendezvous/Src/ChCtrl.h @@ -0,0 +1,63 @@ +// ChCtrl.h: interface for the CChCtrl class. +// +////////////////////////////////////////////////////////////////////// + +#if !defined(_CHCTRL_H__) +#define _CHCTRL_H__ + + +#include "AstroLib.h" + + +// +//追踪器制导控制类 +// +class CChCtrl +{ +public: + CChCtrl(); + virtual ~CChCtrl(); + + void Init(); + void TimeAdvance(double step, + const CCoord& chPos, const CCoord& chVel, const CCoord& tgPos, const CCoord& tgVel, + double flyTimeApproach, const CCoord& endPos, bool& isEnd, CCoord& impulse, int aim); + + +private: + void GuidanceImpulse1( + const CCoord& chPos, const CCoord& chVel, + const CCoord& tgPos, const CCoord& tgVel, const CCoord& endRelPos, + double flyTime, CCoord& impulse); + void GuidanceImpulse2(const CCoord& chPos, const CCoord& chVel, const CCoord& tgPos, const CCoord& tgVel, + const CCoord& endRelVel, CCoord& impulse); + + void Measure(const CCoord& chPos, const CCoord& chVel, + const CCoord& tgPos, const CCoord& tgVel, + CCoord& relPos, CCoord& relVel); + void Engine(CCoord& impulse); + +public: + //测量误差 + int m_UseMeasureRandom; // 是否考虑测量误差:0=无随机;1=随机 + int m_UseEngineRandom; // 是否考虑执行误差:0=无随机;1=随机 + double m_PosStdDev; // 位置测量标准差 + double m_VelStdDev; // 速度测量标准差 + double m_EngStdDev; // 执行误差标准差 + + // + //内部参数 + // +private: + double m_FlyTime[100]; // 从施加上一次冲量开始的飞行时间 + int m_PulseNum; // 冲量次数 + + CRandom m_RandomPos[3]; // 位置随机数 + CRandom m_RandomVel[3]; // 速度随机数 + CRandom m_RandomEng[3]; // 执行机构随机数 + + +}; + + +#endif // !defined(_CHCTRL_H__) diff --git a/MultiRendezvous/Src/IzzoLam.h b/MultiRendezvous/Src/IzzoLam.h new file mode 100644 index 0000000000000000000000000000000000000000..ba268706b55d78a817eff8dc10c57b882e311fc4 --- /dev/null +++ b/MultiRendezvous/Src/IzzoLam.h @@ -0,0 +1,23 @@ +#ifndef IZZOLAM_H +#define IZZOLAM_H + +/** + * Calculates the solution to the Lambert's problem using the Izzo algorithm. + * + * @param r1 The position vector of the departure point. + * @param r2 The position vector of the arrival point. + * @param tof The time of flight between the departure and arrival points. + * @param mu The gravitational parameter of the central body. + * @param way The direction of the transfer: 1 for short way, -1 for long way. + * @param multi_revs The maximum number of complete revolutions allowed. + * @param v1 The velocity vector at the departure point (output parameter). + * @param v2 The velocity vector at the arrival point (output parameter). + * + * @return 0 if the algorithm successfully converges, -1 otherwise. + * + * @throws ErrorType if there was an error in the calculation. + */ +int solve_lambert_izzo(double r1[3], double r2[3], double tof, double mu, int way, int multi_revs, + double v1[][3], double v2[][3]); + +#endif // IZZOLAM_H \ No newline at end of file diff --git a/MultiRendezvous/Src/ObserveGEO.cpp b/MultiRendezvous/Src/ObserveGEO.cpp new file mode 100644 index 0000000000000000000000000000000000000000..b006a38f9dfe7a6a8a57215c13d5092659e1cfa6 --- /dev/null +++ b/MultiRendezvous/Src/ObserveGEO.cpp @@ -0,0 +1,222 @@ +// ObserveGEO.cpp : Defines the entry point for the console application. +// + +#include +#include +#include "ObserveGEO.h" +using namespace std; + +//*********************************************************************** +/// 仿真主程序 +/// @Author Wang Hua +/// @Date 2010-4-15 +//*********************************************************************** +void CRendezvous::Main() +{ + + bool isEnd = false; + + //步长 + double step = 5; + + //初始化 + Initialization(); + + + //循环到仿真结束 + while (!isEnd) + { + isEnd = TimeAdvance(step); + m_Time += step; + } + + //结果报告生成 + ReportGeneration(); + +} + + + +//*********************************************************************** +/// 初始化 +/// @Author Wang Hua +/// @Date 2010-4-15 +//*********************************************************************** +void CRendezvous::Initialization() +{ + //初始化时间 + m_Time = 0; + + //初始化追踪器和目标器 + CCoord chPos(-1.219064685855942e+07, -4.036403817972275e+07, 4.757418138915581e+04), chVel(2.943490270326994e+03, -8.890749827986162e+02, -13.681119407780425); + CCoord tgPos, tgVel; + AsVVLHRelStateToState(CCoord(100000, 0, 0), CCoord(0, 0, 0), + chPos, chVel, tgPos, tgVel); + m_ChDyn.SetName("Chase"); + m_ChDyn.SetPos(chPos); + m_ChDyn.SetVel(chVel); + m_TgDyn.resize(3); + m_TgDyn.at(0).SetName("Target0"); + m_TgDyn.at(0).SetPos(tgPos); + m_TgDyn.at(0).SetVel(tgVel); + AsVVLHRelStateToState(CCoord(200000, 0, 0), CCoord(0, 0, 0), + chPos, chVel, tgPos, tgVel); + m_TgDyn.at(1).SetName("Target1"); + m_TgDyn.at(1).SetPos(tgPos); + m_TgDyn.at(1).SetVel(tgVel); + AsVVLHRelStateToState(CCoord(400000, 0, 0), CCoord(0, 0, 0), + chPos, chVel, tgPos, tgVel); + m_TgDyn.at(2).SetName("Target2"); + m_TgDyn.at(2).SetPos(tgPos); + m_TgDyn.at(2).SetVel(tgVel); + + //初始化控制误差 + m_ChCtrl.m_UseMeasureRandom = 0; + m_ChCtrl.m_UseEngineRandom = 0; + m_ChCtrl.m_PosStdDev = 0.1; + m_ChCtrl.m_VelStdDev = 0.002; + m_ChCtrl.m_EngStdDev = 0.002; + + //初始化动力学和控制 + m_TgDyn.at(0).Init(); + m_TgDyn.at(1).Init(); + m_TgDyn.at(2).Init(); + m_ChDyn.Init(); + m_ChCtrl.Init(); + + //清空缓存 + m_HistoryData.m_TimeList.resize(0); + m_HistoryData.m_chList.resize(0); + m_HistoryData.m_ch_vList.resize(0); + //m_HistoryData.m_tg0List.resize(0); + //m_HistoryData.m_tg0_vList.resize(0); + m_HistoryData.m_tg1List.resize(0); + m_HistoryData.m_tg1_vList.resize(0); + m_HistoryData.m_tg2List.resize(0); + m_HistoryData.m_tg2_vList.resize(0); +} + + +//*********************************************************************** +/// 时间推进 +/// @Author Wang Hua +/// @Date 2010-4-15 +/// @Return true=仿真结束 +//*********************************************************************** +bool CRendezvous::TimeAdvance(double step) +{ + bool isEnd = false; + CCoord impulse; + vector t_list(3); + t_list[0] = 20000; t_list[1] = 20000; t_list[2] = 40000;//访问时间 + + //追踪器和目标器推进 + m_ChCtrl.TimeAdvance(step, m_ChDyn.GetPos(), m_ChDyn.GetVel(), m_TgDyn.at(m_aim).GetPos(), m_TgDyn.at(m_aim).GetVel(), + t_list[m_aim], CCoord(0, 0, 1000), isEnd, impulse, m_aim); + m_ChDyn.TimeAdvance(step, 1, impulse); + m_TgDyn.at(0).TimeAdvance(step, 0, CCoord(0, 0, 0)); + m_TgDyn.at(1).TimeAdvance(step, 0, CCoord(0, 0, 0)); + m_TgDyn.at(2).TimeAdvance(step, 0, CCoord(0, 0, 0)); + + //保存中间数据 + + m_HistoryData.m_TimeList.push_back(m_Time); + + AsStateToVVLHRelState(m_ChDyn.GetPos(), m_ChDyn.GetVel(),m_TgDyn.at(0).GetPos(), m_TgDyn.at(0).GetVel(), + m_RelPos, m_RelVel); + m_HistoryData.m_chList.push_back(m_RelPos); + m_HistoryData.m_ch_vList.push_back(m_RelVel); + if (m_aim == 0) + { + if (m_RelPos.Norm() < 5000) + { + cout << m_Time << "秒，可以看见目标1"<< endl; + } + } + + + AsStateToVVLHRelState(m_TgDyn.at(1).GetPos(), m_TgDyn.at(1).GetVel(),m_TgDyn.at(0).GetPos(), m_TgDyn.at(0).GetVel(), + m_RelPos, m_RelVel); + m_HistoryData.m_tg1List.push_back(m_RelPos); + m_HistoryData.m_tg1_vList.push_back(m_RelVel); + if (m_aim == 1) + { + if ((m_TgDyn.at(1).GetPos() - m_ChDyn.GetPos()).Norm() < 5000) + { + cout << m_Time << "秒，可以看见目标2" << endl; + } + } + + AsStateToVVLHRelState(m_TgDyn.at(2).GetPos(), m_TgDyn.at(2).GetVel(),m_TgDyn.at(0).GetPos(), m_TgDyn.at(0).GetVel(), + m_RelPos, m_RelVel); + m_HistoryData.m_tg2List.push_back(m_RelPos); + m_HistoryData.m_tg2_vList.push_back(m_RelVel); + if (m_aim == 2) + { + if ((m_TgDyn.at(2).GetPos() - m_ChDyn.GetPos()).Norm() < 5000) + { + cout << m_Time << "秒，可以看见目标3" << endl; + } + } + + if (isEnd == true) + { + m_aim += 1; + isEnd = false; + } + if (m_aim > 2) + { + isEnd = true; + } + return isEnd; +} + + +//*********************************************************************** +/// 结果报告生成 +/// @Author Wang Hua +/// @Date 2010-4-15 +//*********************************************************************** +void CRendezvous::ReportGeneration() +{ + ofstream oss(".\\Output\\RelPosVel.txt"); + int n = m_HistoryData.m_TimeList.size(); + for (int i = 0; i < n; i++) + { + oss << m_HistoryData.m_TimeList[i] << " " + << m_HistoryData.m_chList[i][0] << " " + << m_HistoryData.m_chList[i][1] << " " + << m_HistoryData.m_chList[i][2] << " " + << m_HistoryData.m_tg1List[i][0] << " " + << m_HistoryData.m_tg1List[i][1] << " " + << m_HistoryData.m_tg1List[i][2] << " " + << m_HistoryData.m_tg2List[i][0] << " " + << m_HistoryData.m_tg2List[i][1] << " " + << m_HistoryData.m_tg2List[i][2] << endl; + } + oss.close(); + + m_ChDyn.ReportGeneration(); + m_TgDyn[0].ReportGeneration(); + +} + + +int main() +{ + printf("*****************************************************************\n"); + printf("* ObserveGEO Example\n"); + printf("*****************************************************************\n\n"); + + + //执行交会对接仿真 + CRendezvous sim; + sim.Main(); //标称仿真 + //sim.MainMonteCarlo(); //MonteCarlo仿真 + + return 0; +} + + + + diff --git a/MultiRendezvous/Src/ObserveGEO.h b/MultiRendezvous/Src/ObserveGEO.h new file mode 100644 index 0000000000000000000000000000000000000000..77af2a130ba7d8f83e17f70628c46c0da5160d4b --- /dev/null +++ b/MultiRendezvous/Src/ObserveGEO.h @@ -0,0 +1,60 @@ + +#if !defined(AFX_ASTROLIBEXAMPLES_H__A94E219F_5EC1_4DF2_973D_CFEB16259FBE__INCLUDED_) +#define AFX_ASTROLIBEXAMPLES_H__A94E219F_5EC1_4DF2_973D_CFEB16259FBE__INCLUDED_ + +#pragma once + +#include "AstroLib.h" +#include "Spacecraft.h" +#include "ChCtrl.h" + + +// +//航天器交会对接总控 +// +class CRendezvous +{ +public: + void Main(); // 仿真主程序 + +private: + void Initialization(); // 初始化 + bool TimeAdvance(double step); // 时间推进 + void ReportGeneration(); // 结果报告生成 + + +public: + double m_Time; // 仿真时间 + int m_aim = 0; + CCoord m_RelPos; // 相对速度 + CCoord m_RelVel; // 相对速度 + +public: + //保存仿真中间数据 + struct CHistoryData + { + std::vector m_TimeList; // 仿真总共消耗时间[s] + std::vector m_chList; // 相对位置状态[m] + std::vector m_tg1List; // 相对位置状态[m] + std::vector m_tg2List; // 相对位置状态[m] + + std::vector m_ch_vList; // 相对速度状态[m/s] + std::vector m_tg1_vList; // 相对速度状态[m/s] + std::vector m_tg2_vList; // 相对速度状态[m/s] + + std::vector m_sun0List; // 阳光角[弧度] + std::vector m_sun1List; // 阳光角[弧度] + std::vector m_sun2List; // 阳光角[弧度] + }; + CHistoryData m_HistoryData; // 历史数据 + +private: + CChCtrl m_ChCtrl; // 追踪器控制 + CSpacecraft m_ChDyn; // 追踪器动力学 + std::vector m_TgDyn; // 目标器动力学 + +}; + + + +#endif // !defined(AFX_ASTROLIBEXAMPLES_H__A94E219F_5EC1_4DF2_973D_CFEB16259FBE__INCLUDED_) diff --git a/MultiRendezvous/Src/Spacecraft.cpp b/MultiRendezvous/Src/Spacecraft.cpp new file mode 100644 index 0000000000000000000000000000000000000000..d35bcd2a8643250e02436fd57ff79896dac4aa34 --- /dev/null +++ b/MultiRendezvous/Src/Spacecraft.cpp @@ -0,0 +1,122 @@ +// Spacecraft.cpp: implementation of the CSpacecraft class. +// +////////////////////////////////////////////////////////////////////// + +#include "Spacecraft.h" + +using namespace std; + + + +////////////////////////////////////////////////////////////////////// +// Construction/Destruction +////////////////////////////////////////////////////////////////////// + +CSpacecraft::CSpacecraft() +{ + // ʼ + m_Name = "Vehicle"; + m_ElapsedSec = 0.0; + m_Pos[0] = 0.0; + m_Pos[1] = 0.0; + m_Pos[2] = 0.0; +} + + +CSpacecraft::~CSpacecraft() +{ + +} + + +//******************************************************************** +//ʼ,״̬ļ,ʼ +//Author: Wang Hua +//Date: 2005.10.22 +//Input: fileName Ҫĺļ +//******************************************************************** +void CSpacecraft::Init() +{ + //ֵ + m_ElapsedSec = 0.0; //Epoch second set to zero. + + //ʷ + m_HistoryData.m_ElapsedSec.resize(0); + m_HistoryData.m_Pos.resize(0); + m_HistoryData.m_Vel.resize(0); +} + + +//******************************************************************** +/// ʱstepλ,,̬ +/// @Author Wang Hua +/// @Date 2004.11.19 +/// @Input +/// @Param step ʱ䲽 +/// @Param burnCoordSys ϵ: 0=J2000;1=VVLH;2=VNC;3=ϵ +/// @Param burnValue ܵij +/// @Output +/// @Return +//******************************************************************** +void CSpacecraft::TimeAdvance(double step, int burnCoordSys, + const CCoord& burnValue) +{ + CCoord burnValueInertial, burnValueBody, momentVCF, sunPos; + CMatrix mtx; + + // ٶ + switch (burnCoordSys) + { + case 0: + m_Vel += burnValue; + break; + case 1: + AsVVLHToICSMtx(m_Pos, m_Vel, mtx); + m_Vel += burnValue.RotateTo(mtx); + break; + } + + // + m_Orbit.OrbitStep(step, m_Pos, m_Vel); + + // ʱ + m_ElapsedSec += step; + + UpdateBuffer(); +} + + +//******************************************************************** +//ݻ +//Author: Wang Hua +//Date: 2005.10.22 +//******************************************************************** +void CSpacecraft::UpdateBuffer() +{ + m_HistoryData.m_ElapsedSec.push_back(m_ElapsedSec); + m_HistoryData.m_Pos.push_back(m_Pos); + m_HistoryData.m_Vel.push_back(m_Vel); +} + + +//*********************************************************************** +/// +/// @Author Wang Hua +/// @Date 2022-6-30 +//*********************************************************************** +void CSpacecraft::ReportGeneration() +{ + ofstream oss(".\\Output\\" + m_Name + ".txt"); + int n = m_HistoryData.m_ElapsedSec.size(); + for (int i = 0; i < n; i++) + { + oss << m_HistoryData.m_ElapsedSec[i] << " " + << m_HistoryData.m_Pos[i][0] << " " + << m_HistoryData.m_Pos[i][1] << " " + << m_HistoryData.m_Pos[i][2] << " " + << m_HistoryData.m_Vel[i][0] << " " + << m_HistoryData.m_Vel[i][1] << " " + << m_HistoryData.m_Vel[i][2] << endl; + } + oss.close(); +} \ No newline at end of file diff --git a/MultiRendezvous/Src/Spacecraft.h b/MultiRendezvous/Src/Spacecraft.h new file mode 100644 index 0000000000000000000000000000000000000000..63b8b797c05402ebc252052f5219d5142c1aa590 --- /dev/null +++ b/MultiRendezvous/Src/Spacecraft.h @@ -0,0 +1,132 @@ +// Spacecraft.h: interface for the CSpacecraft class. +// +////////////////////////////////////////////////////////////////////// + +#if !defined(_Spacecraft_H__INCLUDED_) +#define _Spacecraft_H__INCLUDED_ + +#pragma once + + +#include "AstroLib.h" +#include + + +// +//ѧ,˺IJͲ +// +class CSpacecraft +{ + // + // Operation. + // +public: + CSpacecraft(); + virtual ~CSpacecraft(); + + + // + //Բ + // + inline void SetName(const std::string& name); // ú + inline void SetPos(const CCoord& pos); // úλ + inline void SetVel(const CCoord& vel); // úٶ + inline void SetElapsedSec(double sec); // ۼʱ + inline const std::string& GetName() const; // õ + inline const CCoord& GetPos() const; // õλ + inline const CCoord& GetVel() const; // õٶ + inline double GetElapsedSec() const; // õۼʱ + + void Init(); // ʼ + void TimeAdvance(double step, int burnCoordSys, + const CCoord& burnValue); // ʱƽ + void ReportGeneration(); // + +private: + // + //м + // + void UpdateBuffer(); + + // + // Attribute. + // +public: + std::string m_Name; // + CCoord m_Pos; // λ,Ĺϵʾ + CCoord m_Vel; // ٶ,Ĺϵʾ + double m_ElapsedSec; // ۼʱ(s) + + +public: + // + //ݻ + // + struct CHistoryData + { + std::vector m_ElapsedSec; // ܹʱ[s] + std::vector m_Pos; // λ״̬[m] + std::vector m_Vel; // ٶ״̬[m/s] + }; + CHistoryData m_HistoryData; // ʷ + +protected: + COrbit m_Orbit; // + +}; + + +// +//inline function. +// + +// ú +void CSpacecraft::SetName(const std::string& name) +{ + m_Name = name; +} + +// ú״̬ +inline void CSpacecraft::SetPos(const CCoord& pos) +{ + m_Pos = pos; +} + +inline void CSpacecraft::SetVel(const CCoord& vel) +{ + m_Vel = vel; +} + +// ۼʱ +inline void CSpacecraft::SetElapsedSec(double sec) +{ + m_ElapsedSec = sec; +} + +// õ +inline const std::string& CSpacecraft::GetName() const +{ + return m_Name; +} + +// õ״̬ +inline const CCoord& CSpacecraft::GetPos() const +{ + return m_Pos; +} + +inline const CCoord& CSpacecraft::GetVel() const +{ + return m_Vel; +} + +// õۼʱ +inline double CSpacecraft::GetElapsedSec() const +{ + return m_ElapsedSec; +} + + + + +#endif // !defined(_Spacecraft_H__INCLUDED_) diff --git a/MultiRendezvous/Src/izzoLam.cpp b/MultiRendezvous/Src/izzoLam.cpp new file mode 100644 index 0000000000000000000000000000000000000000..c645f7d87a7a5a74800fda86fa27c211b5f2a589 --- /dev/null +++ b/MultiRendezvous/Src/izzoLam.cpp @@ -0,0 +1,37 @@ +#include "izzoLam.h" +#include +#include "lambert_problem_t.hpp" + +int solve_lambert_izzo(double r1[3], double r2[3], double tof, double mu, int way, int multi_revs, + double v1[][3], double v2[][3]) { + + int dimV = multi_revs * 2 + 1; + memset(v1, 0, sizeof(double) * 3 * dimV); memset(v2, 0, sizeof(double) * 3 * dimV); + + const Vector3 r1vec(r1[0], r1[1], r1[2]); + const Vector3 r2vec(r2[0], r2[1], r2[2]); + + lambert_problem lp(r1vec, r2vec, tof, mu, way, multi_revs); + lp.Solve(); + std::vector> v1ls = lp.get_v1(); + std::vector> v2ls = lp.get_v2(); + + for (size_t i = 0; i < v1ls.size(); i++) { + v1[i][0] = v1ls[i][0]; + v1[i][1] = v1ls[i][1]; + v1[i][2] = v1ls[i][2]; + v2[i][0] = v2ls[i][0]; + v2[i][1] = v2ls[i][1]; + v2[i][2] = v2ls[i][2]; + } + + return 0; +} + + +//{ +// double r1[3]{ 41960184.323318355,3473991.642108842,-96064.78455976033 }, r2[3]{ -6441593.34, 41608179.66, 13233.21 }, tof = 43200, mu = 3.986e14, v1[1][3], v2[1][3]; +// int way = 1, multi_revs = 0; +// solve_lambert_izzo(r1, r2, tof, mu, way, multi_revs, v1, v2); +// cout << v1[0][0] << endl; +//} \ No newline at end of file diff --git a/MultiRendezvous/Src/lambert_problem.hpp b/MultiRendezvous/Src/lambert_problem.hpp new file mode 100644 index 0000000000000000000000000000000000000000..cbf4cc9f0b05108fdca9c7520d88eef89f0b97d1 --- /dev/null +++ b/MultiRendezvous/Src/lambert_problem.hpp @@ -0,0 +1,63 @@ +#ifndef LAMBERT_PROBLEM_H +#define LAMBERT_PROBLEM_H + +#include "myvector_t.hpp" + +#define _USE_MATH_DEFINES +#include + +#include +#include + +#define Vector3T Vector3 + +template class lambert_problem { +public: + /** + * @brief Construct a new lambert problem object + * + * @param r1 first cartesian position + * @param r2 second cartesian position + * @param tof time of flight + * @param mu gravity parameter + * @param sl 1: short way transfer; -1: long way transfer + * @param multi_revs maximum number of multirevolutions to compute + */ + lambert_problem(const Vector3T &r1 = {1, 0, 0}, const Vector3T &r2 = {0, 1, 0}, + const ST &tof = M_PI_2, const double &mu = 1., const int &sl = 1, + const int &multi_revs = 6); + void Solve(); + void SolveTimeEquation(); + void ConstructTerminalVecolityComponent(const ST &x, ST &vr1, ST &vr2, ST &vt1, ST &vt2) const; + void CalTerminalVelocities(const ST &x, Vector3T &v1, Vector3T &v2) const; + + const std::vector &get_v1() const; + const std::vector &get_v2() const; + const Vector3T &get_r1() const; + const Vector3T &get_r2() const; + const ST &get_tof() const; + const double &get_mu() const; + ST get_lambda() const { return m_lambda; } + ST get_T() const { return sqrt(2.0 * m_mu / m_s / m_s / m_s) * m_tof; } + const std::vector &get_x() const; + const std::vector &get_iters() const; + int get_Nmax() const; + +private: + const Vector3T m_r1, m_r2; + const ST m_tof; + const double m_mu; + std::vector m_v1; + std::vector m_v2; + std::vector m_iters; + std::vector m_x; + ST m_s, m_c, m_lambda; + int m_Nmax; + int m_multi_revs; + + Vector3T ir1_, ir2_, ih_, it1_, it2_; +}; + +//#undef _USE_MATH_DEFINES + +#endif diff --git a/MultiRendezvous/Src/lambert_problem_t.hpp b/MultiRendezvous/Src/lambert_problem_t.hpp new file mode 100644 index 0000000000000000000000000000000000000000..fe5f563a38ff5ef2a9f691173f2c5a6811fe13c4 --- /dev/null +++ b/MultiRendezvous/Src/lambert_problem_t.hpp @@ -0,0 +1,349 @@ + +#ifndef LAMBERT_PROBLEM_T_H +#define LAMBERT_PROBLEM_T_H + +#include + +#include "lambert_problem.hpp" +#include + +template +void LambdaT2Pos(const T &lambda, const T &capsT, const double tof, const double mu, T &rx, T &ry) { + double s = cbrt(2 * mu * pow(tof / capsT, 2)); + double c = (1 - lambda * lambda) * s; + + rx = 2 * s * s - 2 * s * c - 2 * s + c + 1; + double sctemp = s * (1 + c - s); + ry = 2 * sqrt(sctemp * (c - sctemp)); +} +template T hypergeometricF(T z, double tol) { + T Sj = 1.0; + T Cj = 1.0; + double err = 1.0; + T Cj1 = 0.0; + T Sj1 = 0.0; + int j = 0; + while (err > tol) { + Cj1 = Cj * (3.0 + j) * (1.0 + j) / (2.5 + j) * z / (j + 1); + err = fabs(Cj1); + Sj1 = Sj + Cj1; + Sj = Sj1; + Cj = Cj1; + j = j + 1; + } + return Sj; +} + +template T x2tofLagrange(const T &lambda, const T &x, const int N) { + T a = 1.0 / (1.0 - x * x); + if (a > 0) // ellipse + { + T alfa = 2.0 * acos(x); + T beta = 2.0 * asin(sqrt(lambda * lambda / a)); + if (lambda < 0.0) beta = -beta; + T tof = ((a * sqrt(a) * ((alfa - sin(alfa)) - (beta - sin(beta)) + 2.0 * M_PI * N)) / 2.0); + return tof; + } else { + T alfa = 2.0 * acosh(x); + T beta = 2.0 * asinh(sqrt(-lambda * lambda / a)); + if (lambda < 0.0) beta = -beta; + T tof = (-a * sqrt(-a) * ((beta - sinh(beta)) - (alfa - sinh(alfa))) / 2.0); + return tof; + } +} +template T x2tof(const T &lambda, const T &x, const int N) { + double battin = 0.01; + double lagrange = 0.2; + double dist = fabs(x - 1); + if (dist < lagrange && dist > battin) { // We use Lagrange tof expression + return x2tofLagrange(lambda, x, N); + } + + T K = lambda * lambda; + T E = x * x - 1.0; + T z = sqrt(1 + K * E); + if (dist < battin) { // We use Battin series tof expression + T eta = z - lambda * x; + T S1 = 0.5 * (1.0 - lambda - x * eta); + T Q = hypergeometricF(S1, 1e-11); + Q = 4.0 / 3.0 * Q; + T tof = (eta * eta * eta * Q + 4.0 * lambda * eta) / 2.0; + if (N > 0) tof += N * M_PI / sqrt(-E * E * E); + return tof; + } else { // We use Lancaster tof expresion + T rho = (E < 0.0) ? -E : E; + T y = sqrt(rho); + T g = x * z - lambda * E; + T d = 0.0; + if (E < 0) { + T l = acos(g); + d = N * M_PI + l; + } else { + T f = y * (z - lambda * x); + d = log(f + g); + } + T tof = (x - lambda * z - d / y) / E; + return tof; + } +} + +template +void dTdx(T &DT, T &DDT, T &DDDT, const T &x, const T &lambda, const T &capsT) { + T l2 = lambda * lambda; + T l3 = l2 * lambda; + T umx2 = 1.0 - x * x; + T y = sqrt(1.0 - l2 * umx2); + T y2 = y * y; + T y3 = y2 * y; + DT = 1.0 / umx2 * (3.0 * capsT * x - 2.0 + 2.0 * l3 * x / y); + DDT = 1.0 / umx2 * (3.0 * capsT + 5.0 * x * DT + 2.0 * (1.0 - l2) * l3 / y3); + DDDT = 1.0 / umx2 * (7.0 * x * DDT + 8.0 * DT - 6.0 * (1.0 - l2) * l2 * l3 * x / y3 / y2); +} + +template int householder(const T &capsT, const T &lambda, T &x0, const int N, + const double eps, const int iter_max) { + int it = 0; + double err = 1.0; + double xnew = 0.0; + double tof = 0.0, delta = 0.0, DT = 0.0, DDT = 0.0, DDDT = 0.0; + while ((err > eps) && (it < iter_max)) { + tof = x2tof(lambda, x0, N); + dTdx(DT, DDT, DDDT, x0, lambda, tof); + delta = tof - capsT; + double DT2 = DT * DT; + xnew = x0 + - delta * (DT2 - delta * DDT / 2.0) + / (DT * (DT2 - delta * DDT) + DDDT * delta * delta / 6.0); + err = fabs(x0 - xnew); + x0 = xnew; + it++; + } + return it; +} + +template +lambert_problem::lambert_problem(const Vector3T &r1, const Vector3T &r2, const ST &tof, + const double &mu, const int &sl, const int &multi_revs) + : m_r1(r1), m_r2(r2), m_tof(tof), m_mu(mu), m_multi_revs(multi_revs) { + // 0 - Sanity checks + if (tof <= 0) { + throw std::domain_error("Time of flight is negative!"); + } + if (mu <= 0) { + throw std::domain_error("Gravity parameter is zero or negative!"); + } + if (std::abs(sl) != 1) { + throw std::domain_error("Transfer direction is not 1(short) or -1(long)!"); + } + + // 1 - Getting lambda and T + m_c = sqrt((r2[0] - r1[0]) * (r2[0] - r1[0]) + (r2[1] - r1[1]) * (r2[1] - r1[1]) + + (r2[2] - r1[2]) * (r2[2] - r1[2])); + ST R1 = m_r1.norm(); + ST R2 = m_r2.norm(); + m_s = (m_c + R1 + R2) / 2.0; + ST T = sqrt(2.0 * m_mu / m_s / m_s / m_s) * m_tof; + ST lambda2 = 1.0 - m_c / m_s; + m_lambda = sl * sqrt(lambda2); + + // 2.1 - Let us first detect the maximum number of revolutions for which there + // exists a solution + m_Nmax = static_cast(floor(T / M_PI)); + ST T00 = acos(m_lambda) + m_lambda * sqrt(1.0 - lambda2); + ST T0 = (T00 + m_Nmax * M_PI); + ST DT = 0.0, DDT = 0.0, DDDT = 0.0; + if (m_Nmax > 0) { + if (T < T0) { // We use Halley iterations to find xM and TM + int it = 0; + double err = 1.0; + ST T_min = T0; + ST x_old = 0.0, x_new = 0.0; + while (1) { + dTdx(DT, DDT, DDDT, x_old, m_lambda, T_min); + if (!(DT == 0.0)) { + x_new = x_old - DT * DDT / (DDT * DDT - DT * DDDT / 2.0); + } + err = fabs(x_old - x_new); + if ((err < 1e-13) || (it > 12)) { + break; + } + T_min = x2tof(m_lambda, x_new, m_Nmax); + x_old = x_new; + it++; + } + if (T_min > T) { + m_Nmax -= 1; + } + } + } + // We exit this if clause with Nmax being the maximum number of revolutions + // for which there exists a solution. We crop it to m_multi_revs + m_Nmax = std::min(m_multi_revs, m_Nmax); + + // 2.2 We now allocate the memory for the output variables + m_v1.resize(m_Nmax * 2 + 1); + m_v2.resize(m_Nmax * 2 + 1); + m_iters.resize(m_Nmax * 2 + 1); + m_x.resize(m_Nmax * 2 + 1); + + // + ir1_ = m_r1 / sqrt(m_r1[0] * m_r1[0] + m_r1[1] * m_r1[1] + m_r1[2] * m_r1[2]); + ir2_ = m_r2 / sqrt(m_r2[0] * m_r2[0] + m_r2[1] * m_r2[1] + m_r2[2] * m_r2[2]); + ih_ = ir1_.cross(ir2_); + ih_ = ih_ / sqrt(ih_[0] * ih_[0] + ih_[1] * ih_[1] + ih_[2] * ih_[2]); + if (ih_[2] == 0) { + // std::cout + // << "Warn: Angular momentum vector has no z component, set ih = + // [0,0,1]!" + // << std::endl; + ih_ = {0, 0, 1}; + } + if (m_lambda < 0) ih_ = ih_ * -1; + it1_ = ih_.cross(ir1_); + // it1_ /= sqrt(it1_[0] * it1_[0] + it1_[1] * it1_[1] + it1_[2] * it1_[2]); + it2_ = ih_.cross(ir2_); + // it2_ /= sqrt(it2_[0] * it2_[0] + it2_[1] * it2_[1] + it2_[2] * it2_[2]); +} +template void lambert_problem::SolveTimeEquation() { + // 3 - We may now find all solutions in x,y + // 3.1 0 rev solution + // 3.1.1 initial guess + ST lambda2 = 1.0 - m_c / m_s; + ST lambda3 = lambda2 * m_lambda; + ST T = sqrt(2.0 * m_mu / m_s / m_s / m_s) * m_tof; + ST T00 = acos(m_lambda) + m_lambda * sqrt(1.0 - lambda2); + ST T1 = 2.0 / 3.0 * (1.0 - lambda3); + if (T >= T00) { + m_x[0] = -(T - T00) / (T - T00 + 4); + } else if (T <= T1) { + m_x[0] = T1 * (T1 - T) / (2.0 / 5.0 * (1 - lambda2 * lambda3) * T) + 1; + } else { + m_x[0] = pow((T / T00), 0.69314718055994529 / log(T1 / T00)) - 1.0; + } + // 3.1.2 Householder iterations + m_iters[0] = householder(T, m_lambda, m_x[0], 0, 1e-5, 15); + // 3.2 multi rev solutions + ST tmp; + for (int i = 1; i < m_Nmax + 1; ++i) { + // 3.2.1 left Householder iterations + tmp = pow((i * M_PI + M_PI) / (8.0 * T), 2.0 / 3.0); + m_x[2 * i - 1] = (tmp - 1) / (tmp + 1); + m_iters[2 * i - 1] = householder(T, m_lambda, m_x[2 * i - 1], i, 1e-8, 15); + // 3.2.1 right Householder iterations + tmp = pow((8.0 * T) / (i * M_PI), 2.0 / 3.0); + m_x[2 * i] = (tmp - 1) / (tmp + 1); + m_iters[2 * i] = householder(T, m_lambda, m_x[2 * i], i, 1e-8, 15); + } +} +template void lambert_problem::Solve() { + SolveTimeEquation(); + // 4 - For each found x value we reconstruct the terminal velocities + ST vr1, vt1, vr2, vt2; + for (size_t i = 0; i < m_x.size(); ++i) { + ConstructTerminalVecolityComponent(m_x[i], vr1, vr2, vt1, vt2); + for (int j = 0; j < 3; ++j) m_v1[i][j] = vr1 * ir1_[j] + vt1 * it1_[j]; + for (int j = 0; j < 3; ++j) m_v2[i][j] = vr2 * ir2_[j] + vt2 * it2_[j]; + } +} + +template +void lambert_problem::ConstructTerminalVecolityComponent(const ST &x, ST &vr1, ST &vr2, ST &vt1, + ST &vt2) const { + ST R1 = m_r1.norm(); + ST R2 = m_r2.norm(); + ST gamma = sqrt(m_mu * m_s / 2.0); + ST rho = (R1 - R2) / m_c; + ST sigma = sqrt(1 - rho * rho); + ST lambda2 = 1.0 - m_c / m_s; + ST y = sqrt(1.0 - lambda2 + lambda2 * x * x); + vr1 = gamma * ((m_lambda * y - x) - rho * (m_lambda * y + x)) / R1; + vr2 = -gamma * ((m_lambda * y - x) + rho * (m_lambda * y + x)) / R2; + ST vt = gamma * sigma * (y + m_lambda * x); + vt1 = vt / R1; + vt2 = vt / R2; +} +template +void lambert_problem::CalTerminalVelocities(const ST &x, Vector3T &v1, Vector3T &v2) const { + ST vr1, vt1, vr2, vt2; + ConstructTerminalVecolityComponent(x, vr1, vr2, vt1, vt2); + + // Vector3T v1, v2; + v1 = vr1 * ir1_ + vt1 * it1_; + v2 = vr2 * ir2_ + vt2 * it2_; +} + +/// Gets velocity at r1 +/** + * + * \return an std::vector containing 3-d arrays with the cartesian components of + * the velocities at r1 for all 2N_max+1 solutions + */ +template const std::vector &lambert_problem::get_v1() const { + return m_v1; +} + +/// Gets velocity at r2 +/** + * + * \return an std::vector containing 3-d arrays with the cartesian components of + * the velocities at r2 for all 2N_max+1 solutions + */ +template const std::vector &lambert_problem::get_v2() const { + return m_v2; +} + +/// Gets r1 +/** + * + * \return a 3-d array with the cartesian components of r1 + */ +template const Vector3T &lambert_problem::get_r1() const { return m_r1; } + +/// Gets r2 +/** + * + * \return a 3-d array with the cartesian components of r2 + */ +template const Vector3T &lambert_problem::get_r2() const { return m_r2; } + +/// Gets the time of flight between r1 and r2 +/** + * + * \return the time of flight + */ +template const ST &lambert_problem::get_tof() const { return m_tof; } + +/// Gets the x variable +/** + * Gets the x variable for each solution found (0 revs, 1,1,2,2,3,3 .... N,N) + * + * \return the x variables in an std::vector + */ +template const std::vector &lambert_problem::get_x() const { return m_x; } + +/// Gets gravitational parameter +/** + * + * \return the gravitational parameter + */ +template const double &lambert_problem::get_mu() const { return m_mu; } + +/// Gets number of iterations +/** + * + * \return an std::vector containing the iterations taken to compute each one of + * the solutions + */ +template const std::vector &lambert_problem::get_iters() const { + return m_iters; +} + +/// Gets N_max +/** + * + * \return the maximum number of revolutions. The number of solutions to the + * problem will be Nmax*2 +1 + */ +template int lambert_problem::get_Nmax() const { return m_Nmax; } + +#endif \ No newline at end of file diff --git a/MultiRendezvous/Src/myvector_t.hpp b/MultiRendezvous/Src/myvector_t.hpp new file mode 100644 index 0000000000000000000000000000000000000000..0bfca8a2c03aeb5247692df7949fccde7028fa77 --- /dev/null +++ b/MultiRendezvous/Src/myvector_t.hpp @@ -0,0 +1,73 @@ +#ifndef MYVECTOR_H +#define MYVECTOR_H +#include + +template class Vector3 { +public: + T x, y, z; + + Vector3(T x_val = 0, T y_val = 0, T z_val = 0) : x(x_val), y(y_val), z(z_val) {} + + Vector3 operator+(const Vector3& other) const { + return Vector3(x + other.x, y + other.y, z + other.z); + } + + Vector3 operator-(const Vector3& other) const { + return Vector3(x - other.x, y - other.y, z - other.z); + } + + Vector3 operator*(T scalar) const { return Vector3(x * scalar, y * scalar, z * scalar); } + + Vector3 operator/(T scalar) const { + if (scalar == 0) { + throw std::invalid_argument("Division by zero."); + } + return Vector3(x / scalar, y / scalar, z / scalar); + } + + T& operator[](int index) { + if (index < 0 || index >= 3) { + throw std::out_of_range("Index out of range."); + } + + if (index == 0) { + return x; + } else if (index == 1) { + return y; + } else { + return z; + } + } + + const T& operator[](int index) const { + if (index < 0 || index >= 3) { + throw std::out_of_range("Index out of range."); + } + + if (index == 0) { + return x; + } else if (index == 1) { + return y; + } else { + return z; + } + } + + T dot(const Vector3& other) const { return x * other.x + y * other.y + z * other.z; } + + Vector3 cross(const Vector3& other) const { + return Vector3(y * other.z - z * other.y, z * other.x - x * other.z, x * other.y - y * other.x); + } + + T norm() const { return std::sqrt(x * x + y * y + z * z); } + + Vector3 normalized() const { + T n = norm(); + if (n != 0) { + return Vector3(x / n, y / n, z / n); + } + return *this; + } +}; + +#endif \ No newline at end of file diff --git a/SimDoF6/SimDoF6.vcxproj.user b/SimDoF6/SimDoF6.vcxproj.user new file mode 100644 index 0000000000000000000000000000000000000000..88a550947edbc3c5003a41726f0749201fdb6822 --- /dev/null +++ b/SimDoF6/SimDoF6.vcxproj.user @@ -0,0 +1,4 @@ + + + + \ No newline at end of file