/
ChangeOrbitalElement.cs
134 lines (112 loc) · 4.86 KB
/
ChangeOrbitalElement.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
/*
* 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.Primitives;
using static MechJebLib.Utils.Statics;
#nullable enable
namespace MechJebLib.Maneuvers
{
public static class ChangeOrbitalElement
{
public enum Type { PERIAPSIS, APOAPSIS, SMA, ECC }
private struct Args
{
public V3 P;
public V3 Q;
public double Value;
public Type Type;
}
private static void NLPFunction(double[] x, double[] fi, object obj)
{
var dv = new V3(x[0], x[1], 0);
var args = (Args)obj;
V3 p = args.P;
V3 q = args.Q;
double value = args.Value;
Type type = args.Type;
fi[0] = dv.sqrMagnitude;
switch (type)
{
case Type.PERIAPSIS:
fi[1] = Core.Maths.PeriapsisFromStateVectors(1.0, p, q + dv) - value;
break;
case Type.APOAPSIS:
fi[1] = 1.0 / Core.Maths.ApoapsisFromStateVectors(1.0, p, q + dv) - 1.0 / value;
break;
case Type.SMA:
fi[1] = 1.0 / Core.Maths.SmaFromStateVectors(1.0, p, q + dv) - 1.0 / value;
break;
case Type.ECC:
double ecc = Core.Maths.EccFromStateVectors(1.0, p, q + dv);
fi[1] = ecc - value;
break;
}
}
public static V3 DeltaV(double mu, V3 r, V3 v, double value, Type type)
{
if (!mu.IsFinite())
throw new ArgumentException("bad mu in ChangeOrbitalElement");
if (!r.IsFinite())
throw new ArgumentException("bad r in ChangeOrbitalElement");
if (!v.IsFinite())
throw new ArgumentException("bad v in ChangeOrbitalElement");
switch (type)
{
case Type.PERIAPSIS:
if (value < 0 || (value > r.magnitude) | !value.IsFinite())
throw new ArgumentException($"Bad periapsis in ChangeOrbitalElement = {value}");
break;
case Type.APOAPSIS:
if (!value.IsFinite() || (value > 0 && value < r.magnitude))
throw new ArgumentException($"Bad apoapsis in ChangeOrbitalElement = {value}");
break;
case Type.SMA:
if (!value.IsFinite())
throw new ArgumentException($"Bad SMA in ChangeOrbitalElement = {value}");
break;
case Type.ECC:
if (!value.IsFinite() || value < 0)
throw new ArgumentException($"Bad Ecc in ChangeOrbitalElement = {value}");
break;
}
const double DIFFSTEP = 1e-7;
const double EPSX = 1e-15;
const int MAXITS = 1000;
const int NVARIABLES = 2;
const int NEQUALITYCONSTRAINTS = 1;
const int NINEQUALITYCONSTRAINTS = 0;
double[] x = new double[NVARIABLES];
var scale = Scale.Create(mu, r.magnitude);
(V3 p, V3 q, Q3 rot) = Core.Maths.PerifocalFromStateVectors(mu, r, v);
p /= scale.LengthScale;
q /= scale.VelocityScale;
if (type == Type.ECC)
{
// changing the ECC is actually a global optimization problem due to basins around parabolic ecc == 1.0
double boost = Math.Sign(value - 1.0) * 0.1 + Math.Sqrt(2);
V3 dv = Core.Functions.Maneuvers.DeltaVRelativeToCircularVelocity(1.0, p, q, boost);
x[0] = dv.x;
x[1] = dv.y;
}
else
{
x[0] = 0; // maneuver vy
x[1] = 0; // maneuver vy
}
var args = new Args { P = p, Q = q, Value = type == Type.ECC ? value : value / scale.LengthScale, Type = type };
alglib.minnlccreatef(NVARIABLES, x, DIFFSTEP, out alglib.minnlcstate state);
alglib.minnlcsetalgosqp(state);
alglib.minnlcsetcond(state, EPSX, MAXITS);
alglib.minnlcsetnlc(state, NEQUALITYCONSTRAINTS, NINEQUALITYCONSTRAINTS);
alglib.minnlcoptimize(state, NLPFunction, null, args);
alglib.minnlcresults(state, out x, out alglib.minnlcreport rep);
if (rep.terminationtype < 0)
throw new Exception(
$"DeltaVToChangeApsis({mu}, {r}, {v}, {value}, {type}): SQP solver terminated abnormally: {rep.terminationtype}"
);
return rot * new V3(x[0], x[1], 0) * scale.VelocityScale;
}
}
}