/
Kepler5Reduced.cs
79 lines (65 loc) · 2.96 KB
/
Kepler5Reduced.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
/*
* 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+
*/
using System;
using MechJebLib.Functions;
using MechJebLib.Primitives;
using static MechJebLib.Utils.Statics;
#nullable enable
namespace MechJebLib.PVG.Terminal
{
/// <summary>
/// 5 Constraint terminal conditions with free attachment for the minimum propellant / maximum mass problem.
/// Many references including:
/// Brown, K., and G. Johnson. “Real-Time Optimal Guidance.” IEEE Transactions on Automatic Control 12, no. 5 (October
/// 1967): 501–6. https://doi.org/10.1109/TAC.1967.1098718.
/// Pan, Binfeng, Zheng Chen, Ping Lu, and Bo Gao. “Reduced Transversality Conditions in Optimal Space Trajectories.”
/// Journal of Guidance, Control, and Dynamics 36, no. 5 (September 2013): 1289–1300. https://doi.org/10.2514/1.60181.
/// </summary>
public readonly struct Kepler5Reduced : IPVGTerminal
{
private readonly double _smaT;
private readonly double _eccT;
private readonly double _incT;
private readonly double _lanT;
private readonly double _argpT;
private readonly V3 _hT;
private readonly V3 _ehat1;
private readonly V3 _ehat2;
private readonly double _e1;
private readonly double _e2;
public Kepler5Reduced(double smaT, double eccT, double incT, double lanT, double argpT)
{
_smaT = smaT;
_eccT = eccT;
_incT = Math.Abs(ClampPi(incT));
_lanT = lanT;
_argpT = argpT;
_hT = Astro.HvecFromKeplerian(1.0, _smaT, _eccT, _incT, _lanT);
// r guaranteed not to be colinear with hT
V3 r = V3.zero;
r[_hT.min_magnitude_index] = 1.0;
// basis vectors orthonormal to hT
_ehat1 = V3.Cross(_hT, r).normalized;
_ehat2 = V3.Cross(_hT, _ehat1).normalized;
// projection of eT onto ehat1/ehat2
V3 eT = Astro.EvecFromKeplerian(_eccT, _incT, _lanT, _argpT);
_e1 = V3.Dot(eT, _ehat1);
_e2 = V3.Dot(eT, _ehat2);
}
public IPVGTerminal Rescale(Scale scale) => new Kepler5Reduced(_smaT / scale.LengthScale, _eccT, _incT, _lanT, _argpT);
public (double a, double b, double c, double d, double e, double f) TerminalConstraints(OutputLayout yf)
{
double rfm = yf.R.magnitude;
double rf3 = rfm * rfm * rfm;
var hf = V3.Cross(yf.R, yf.V);
V3 ef = V3.Cross(yf.V, hf) - yf.R.normalized;
V3 hmiss = hf - _hT;
double emiss1 = _e1 - V3.Dot(ef, _ehat1);
double emiss2 = _e2 - V3.Dot(ef, _ehat2);
double t1 = V3.Dot(yf.PR, yf.V) - V3.Dot(yf.PV, yf.R) / rf3;
return (hmiss[0], hmiss[1], hmiss[2], emiss1, emiss2, t1);
}
}
}