/
Ascent.cs
339 lines (266 loc) · 11.7 KB
/
Ascent.cs
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
/*
* Copyright Lamont Granquist, Sebastien Gaggini and the MechJeb contributors
* SPDX-License-Identifier: LicenseRef-PD-hp OR Unlicense OR CC0-1.0 OR 0BSD OR MIT-0 OR MIT OR LGPL-2.1+
*/
#nullable enable
using System;
using System.Collections.Generic;
using MechJebLib.Functions;
using MechJebLib.Primitives;
using static MechJebLib.Utils.Statics;
using static System.Math;
namespace MechJebLib.PVG
{
/// <summary>
/// </summary>
public partial class Ascent
{
private readonly AscentBuilder _input;
private List<Phase> _phases => _input._phases;
private V3 _r0 => _input._r0;
private V3 _v0 => _input._v0;
private V3 _u0 => _input._u0;
private double _t0 => _input._t0;
private double _mu => _input._mu;
private double _rbody => _input._rbody;
private double _peR => _input._peR;
private double _apR => _input._apR;
private double _attR => _input._attR;
private double _incT => _input._incT;
private double _lanT => _input._lanT;
private double _fpaT => _input._fpaT;
private double _hT => _input._hT;
private bool _attachAltFlag => _input._attachAltFlag;
private bool _lanflag => _input._lanflag;
private bool _fixedBurnTime => _input._fixedBurnTime;
private Solution? _solution => _input._solution;
private int _optimizedPhase => _input._optimizedPhase;
private int _optimizedCoastPhase => _input._optimizedCoastPhase;
private double _vT;
private double _gammaT;
private double _smaT;
private double _eccT;
private Optimizer? _optimizer;
private Ascent(AscentBuilder builder)
{
_input = builder;
}
public void Run()
{
(_smaT, _eccT) = Astro.SmaEccFromApsides(_peR, _apR);
using Optimizer.OptimizerBuilder builder = Optimizer.Builder()
.Initial(_r0, _v0, _u0, _t0, _mu, _rbody)
.TerminalConditions(_hT);
if (_solution == null)
{
_optimizer = _fixedBurnTime ? InitialBootstrappingFixed(builder) : InitialBootstrappingOptimized(builder);
}
else
{
_optimizer = ConvergedOptimization(builder, _solution);
}
}
public Optimizer? GetOptimizer() => _optimizer;
private Optimizer ConvergedOptimization(Optimizer.OptimizerBuilder builder, Solution solution)
{
ForceNumericalIntegration();
if (_fixedBurnTime)
{
ApplyEnergy(builder);
}
else
{
if (_attachAltFlag || _eccT < 1e-4)
ApplyFPA(builder);
else
ApplyKepler(builder);
}
ApplyOldBurnTimesToPhases(solution);
Optimizer pvg = builder.Build(_phases);
pvg.Bootstrap(solution);
pvg.Run();
if (!pvg.Success())
throw new Exception("converged optimizer failed");
using Solution solution2 = pvg.GetSolution();
(V3 rf, V3 vf) = solution2.TerminalStateVectors();
(_, _, _, _, _, double tanof, _) =
Astro.KeplerianFromStateVectors(_mu, rf, vf);
if (_attachAltFlag || Abs(ClampPi(tanof)) < PI / 2.0)
return pvg;
ApplyFPA(builder);
ApplyOldBurnTimesToPhases(solution2);
using Optimizer pvg2 = builder.Build(_phases);
pvg2.Bootstrap(solution2);
pvg2.Run();
return pvg2.Success() ? pvg2 : pvg;
}
private void ApplyFPA(Optimizer.OptimizerBuilder builder)
{
// If _attachAltFlag is NOT set then we are bootstrapping with ApplyFPA prior to
// trying free attachment with Kepler and attR is invalid and we need to fix to
// the PeR. This should be fixed in the AscentBuilder by having more APIs than
// just "SetTarget" that fixes this correctly there.
double attR = _attachAltFlag ? _attR : _peR;
(_vT, _gammaT) = Astro.ConvertApsidesTargetToFPA(_peR, _apR, attR, _mu);
if (_lanflag)
builder.TerminalFPA5(attR, _vT, _gammaT, _incT, _lanT);
else
builder.TerminalFPA4(attR, _vT, _gammaT, _incT);
}
private void ApplyKepler(Optimizer.OptimizerBuilder builder)
{
(_smaT, _eccT) = Astro.SmaEccFromApsides(_peR, _apR);
if (_lanflag)
builder.TerminalKepler4(_smaT, _eccT, _incT, _lanT);
else
builder.TerminalKepler3(_smaT, _eccT, _incT);
}
private void ApplyEnergy(Optimizer.OptimizerBuilder builder)
{
if (_lanflag)
builder.TerminalEnergy4(_attR, _incT, _lanT);
else
builder.TerminalEnergy3(_attR, _fpaT, _incT);
}
private Optimizer InitialBootstrappingFixed(Optimizer.OptimizerBuilder builder)
{
ApplyEnergy(builder);
// guess the initial launch direction
V3 enu = Astro.ENUHeadingForInclination(_incT, _r0);
enu.z = 1.0; // add 45 degrees up
V3 pvGuess = Astro.ENUToECI(_r0, enu).normalized;
List<Phase> bootphases = DupPhases(_phases);
// FIXME: we may want to convert this to an optimized burntime circular orbit problem with an infinite upper stage for bootstrapping
for (int p = 0; p < bootphases.Count; p++)
{
bootphases[p].Unguided = false;
if (p == _optimizedCoastPhase)
{
bootphases[p].OptimizeTime = false;
// FIXME: a problem here is that if we require a coast to hit the target then we'll never converge
bootphases[p].bt = 0;
}
}
using Optimizer pvg = builder.Build(bootphases);
pvg.Bootstrap(pvGuess, _r0.normalized);
pvg.Run();
if (!pvg.Success())
throw new Exception("Target unreachable (fixed bootstrapping)");
using Solution solution = pvg.GetSolution();
ApplyOldBurnTimesToPhases(solution);
List<Phase> bootphases2 = DupPhases(_phases);
if (_optimizedCoastPhase > -1)
{
double total = 0.0;
for (int i = 0; i < bootphases2.Count; i++)
total += bootphases2[i].bt;
bootphases2[_optimizedCoastPhase].bt = total;
}
Optimizer pvg2 = builder.Build(bootphases2);
pvg2.Bootstrap(solution);
pvg2.Run();
if (!pvg2.Success())
throw new Exception("Target unreachable");
return pvg2;
}
private Optimizer InitialBootstrappingOptimized(Optimizer.OptimizerBuilder builder)
{
ApplyFPA(builder);
// guess the initial launch direction
V3 enu = Astro.ENUHeadingForInclination(_incT, _r0);
enu.z = 1.0; // add 45 degrees up
V3 pvGuess = Astro.ENUToECI(_r0, enu).normalized;
List<Phase> bootphases = DupPhases(_phases);
// set the coast phase to fixed time of zero
if (_optimizedCoastPhase > -1)
{
bootphases[_optimizedCoastPhase].OptimizeTime = false;
bootphases[_optimizedCoastPhase].bt = 0;
}
// switch the optimized phase to the top stage of the rocket
bootphases[_optimizedPhase].OptimizeTime = false;
// set the top stage to infinite + optimized + unguided
bootphases[bootphases.Count - 1].Infinite = true;
bootphases[bootphases.Count - 1].Unguided = false;
bootphases[bootphases.Count - 1].OptimizeTime = true;
using Optimizer pvg = builder.Build(bootphases);
pvg.Bootstrap(pvGuess, _r0.normalized);
pvg.Run();
if (!pvg.Success())
throw new Exception("Target unreachable (infinite ISP)");
using Solution solution = pvg.GetSolution();
ApplyOldBurnTimesToPhases(solution);
if (_optimizedCoastPhase > -1)
{
double total = 0.0;
for (int i = 0; i < _phases.Count; i++)
total += _phases[i].bt;
_phases[_optimizedCoastPhase].OptimizeTime = true;
_phases[_optimizedCoastPhase].bt = total;
}
Optimizer pvg2 = builder.Build(_phases);
pvg2.Bootstrap(solution);
pvg2.Run();
if (!pvg2.Success())
{
// when analytic thrust integrals fail, the numerical thrust integrals may succeed.
ForceNumericalIntegration();
pvg2 = builder.Build(_phases);
pvg2.Bootstrap(solution);
pvg2.Run();
if (!pvg2.Success())
throw new Exception("Target unreachable");
}
if (_attachAltFlag || _eccT < 1e-4)
return pvg2;
// we have a periapsis attachment solution, redo with free attachment
using Solution solution2 = pvg2.GetSolution();
ApplyKepler(builder);
ApplyOldBurnTimesToPhases(solution2);
Optimizer pvg3 = builder.Build(_phases);
pvg3.Bootstrap(solution2);
pvg3.Run();
if (!pvg3.Success())
return pvg2;
// sanity check to force back near-apoapsis attatchment back to periapsis attachment
using Solution solution3 = pvg3.GetSolution();
(V3 rf, V3 vf) = solution3.TerminalStateVectors();
(_, _, _, _, _, double tanof, _) =
Astro.KeplerianFromStateVectors(_mu, rf, vf);
return Abs(ClampPi(tanof)) > PI / 2.0 ? pvg2 : pvg3;
}
private void ForceNumericalIntegration()
{
for (int i = 0; i < _phases.Count; i++)
_phases[i].Analytic = false;
}
private void ApplyOldBurnTimesToPhases(Solution oldSolution)
{
for (int i = 0; i < _phases.Count; i++)
{
if (!_phases[i].OptimizeTime)
continue;
for (int j = 0; j < oldSolution.Segments; j++)
{
if (!oldSolution.OptimizeTime(j))
continue;
if (oldSolution.CoastPhase(j) != _phases[i].Coast)
continue;
if (!_phases[i].Coast)
_phases[i].bt = Min(oldSolution.Bt(j, _t0), _phases[i].bt);
else
_phases[i].bt = oldSolution.Bt(j, _t0);
}
}
}
// FIXME: this obviously creates garbage and needs to return an IDisposable wrapping List<Phases> that has a pool
private List<Phase> DupPhases(List<Phase> oldphases)
{
var newphases = new List<Phase>();
foreach (Phase phase in oldphases)
newphases.Add(phase.DeepCopy());
return newphases;
}
public static AscentBuilder Builder() => new AscentBuilder();
}
}