Skip to content

Commit

Permalink
PVG cleanup: replace ResidualWrapper with ContinuityLayout/ResidualLa…
Browse files Browse the repository at this point in the history
…yout

also cleans up the "Problem" a bit and removes that one last annoying
null warning i've left in for about a year.

Signed-off-by: Lamont Granquist <lamont@scriptkiddie.org>
  • Loading branch information
lamont-granquist committed Jun 17, 2023
1 parent 9d9f9c6 commit 118a797
Show file tree
Hide file tree
Showing 9 changed files with 446 additions and 477 deletions.
517 changes: 259 additions & 258 deletions MechJeb2/MechJeb2.csproj

Large diffs are not rendered by default.

56 changes: 56 additions & 0 deletions MechJeb2/MechJebLib/PVG/ContinuityLayout.cs
@@ -0,0 +1,56 @@
/*
* 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.Collections.Generic;
using MechJebLib.Primitives;

namespace MechJebLib.PVG
{
public class ContinuityLayout
{
public const int CONTINUITY_LAYOUT_LEN = ResidualLayout.RESIDUAL_LAYOUT_LEN;

public V3 R;
public V3 V;
public V3 Pv;
public V3 Pr;
public double M;
public double Pm;
public double Bt;

public void CopyTo(IList<double> other)
{
R.CopyTo(other);
V.CopyTo(other, 3);
Pv.CopyTo(other, 6);
Pr.CopyTo(other, 9);
other[12] = M;
other[13] = Pm;
other[14] = Bt;
}

public void CopyFrom(IList<double> other)
{
R.CopyFrom(other);
V.CopyFrom(other, 3);
Pv.CopyFrom(other, 6);
Pr.CopyFrom(other, 9);
M = other[12];
Pm = other[13];
Bt = other[14];
}

public static ContinuityLayout CreateFrom(IList<double> other)
{
var a = new ContinuityLayout();

a.CopyFrom(other);

return a;
}
}
}
9 changes: 8 additions & 1 deletion MechJeb2/MechJebLib/PVG/InputLayout.cs
@@ -1,4 +1,11 @@
using System;
/*
* 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.Primitives;

Expand Down
49 changes: 25 additions & 24 deletions MechJeb2/MechJebLib/PVG/Optimizer.cs
Expand Up @@ -52,7 +52,7 @@ private void ExpandArrays()
while (_terminal.Count < _phases.Count)
_terminal.Add(Vn.Rent(OutputLayout.OUTPUT_LAYOUT_LEN));
while (_residual.Count < _phases.Count)
_residual.Add(Vn.Rent(ResidualWrapper.RESIDUAL_WRAPPER_LEN));
_residual.Add(Vn.Rent(ResidualLayout.RESIDUAL_LAYOUT_LEN));
}

private void CopyToInitial(double[] yin)
Expand Down Expand Up @@ -112,14 +112,15 @@ private void BaseResiduals()
{
var y0 = InputLayout.CreateFrom(_initial[0]);
var yf = OutputLayout.CreateFrom(_terminal[lastPhase]);
using var z = ResidualWrapper.Rent(_residual[0]);
var z = ResidualLayout.CreateFrom(_residual[0]);

z.R = y0.R - _problem.R0Bar;
z.V = y0.V - _problem.V0Bar;
z.M = y0.M - _problem.M0Bar;
z.R = y0.R - _problem.R0;
z.V = y0.V - _problem.V0;
z.M = y0.M - _problem.M0;
z.Terminal = _problem.Terminal.TerminalConstraints(yf);
z.Bt = CalcBTConstraint(0);
//z.Pm_transversality = yf_scratch[phases.Count - 1].Pm - 1;
z.CopyTo(_residual[0]);
}

private void ContinuityConditions()
Expand All @@ -128,21 +129,21 @@ private void ContinuityConditions()
{
var y0 = InputLayout.CreateFrom(_initial[p]);
var yf = OutputLayout.CreateFrom(_terminal[p - 1]);
using var z = ResidualWrapper.Rent(_residual[p]);
var z = ContinuityLayout.CreateFrom(_residual[p]);

z.RContinuity = yf.R - y0.R;
z.VContinuity = yf.V - y0.V;
z.PvContinuity = yf.PV - y0.PV;
z.PrContinuity = yf.PR - y0.PR;
z.R = yf.R - y0.R;
z.V = yf.V - y0.V;
z.Pv = yf.PV - y0.PV;
z.Pr = yf.PR - y0.PR;

if (_phases[p].MassContinuity)
z.M_continuity = yf.M - (_phases[p - 1].DropMass + y0.M);
z.M = yf.M - (_phases[p - 1].DropMass + y0.M);
else
z.M_continuity = _phases[p].m0 - y0.M;
z.M = _phases[p].m0 - y0.M;

z.Bt = CalcBTConstraint(p);

//z.Pm_transversality =
z.CopyTo(_residual[p]);
}
}

Expand All @@ -156,7 +157,7 @@ private void CopyToZ(double[] z)
{
for (int i = 0; i < z.Length; i++)
{
z[i] = _residual[i / ResidualWrapper.RESIDUAL_WRAPPER_LEN][i % ResidualWrapper.RESIDUAL_WRAPPER_LEN];
z[i] = _residual[i / ResidualLayout.RESIDUAL_LAYOUT_LEN][i % ResidualLayout.RESIDUAL_LAYOUT_LEN];
}
}

Expand Down Expand Up @@ -220,7 +221,7 @@ private void UnSafeRun()

double[] yGuess = new double[_phases.Count * InputLayout.INPUT_LAYOUT_LEN];
double[] yNew = new double[_phases.Count * InputLayout.INPUT_LAYOUT_LEN];
double[] z = new double[_phases.Count * ResidualWrapper.RESIDUAL_WRAPPER_LEN];
double[] z = new double[_phases.Count * ResidualLayout.RESIDUAL_LAYOUT_LEN];
double[] bndu = new double[_phases.Count * InputLayout.INPUT_LAYOUT_LEN];
double[] bndl = new double[_phases.Count * InputLayout.INPUT_LAYOUT_LEN];

Expand All @@ -237,7 +238,7 @@ private void UnSafeRun()
if (!_phases[i].Coast && !_phases[i].Infinite)
bndu[i * InputLayout.INPUT_LAYOUT_LEN + InputLayout.BT_INDEX] = _phases[i].tau * 0.999;

alglib.minlmcreatev(ResidualWrapper.RESIDUAL_WRAPPER_LEN * _phases.Count, yGuess, LmDiffStep, out _state);
alglib.minlmcreatev(ResidualLayout.RESIDUAL_LAYOUT_LEN * _phases.Count, yGuess, LmDiffStep, out _state);
alglib.minlmsetbc(_state, bndl, bndu);
alglib.minlmsetcond(_state, LmEpsx, MaxIter);
alglib.minlmoptimize(_state, _residualHandle, null, null);
Expand Down Expand Up @@ -314,9 +315,9 @@ private void Shooting(Solution? solution = null)

if (p == 0)
{
y0.R = _problem.R0Bar;
y0.V = _problem.V0Bar;
y0.M = _problem.M0Bar;
y0.R = _problem.R0;
y0.V = _problem.V0;
y0.M = _problem.M0;
}

y0.CopyTo(_initial[p]);
Expand Down Expand Up @@ -361,8 +362,8 @@ public Optimizer Bootstrap(V3 pv0, V3 pr0)

if (p == 0)
{
y0.R = _problem.R0Bar;
y0.V = _problem.V0Bar;
y0.R = _problem.R0;
y0.V = _problem.V0;
y0.M = phase.m0;
y0.PV = pv0;
y0.PR = pr0;
Expand Down Expand Up @@ -446,9 +447,9 @@ public Optimizer Bootstrap(Solution solution)

if (p == 0)
{
y0.R = _problem.R0Bar;
y0.V = _problem.V0Bar;
y0.M = _problem.M0Bar;
y0.R = _problem.R0;
y0.V = _problem.V0;
y0.M = _problem.M0;
y0.Bt = phase.bt;
y0.PV = solution.Pv(_problem.T0);
y0.PR = solution.Pr(_problem.T0);
Expand Down
11 changes: 5 additions & 6 deletions MechJeb2/MechJebLib/PVG/OptimizerBuilder.cs
Expand Up @@ -31,7 +31,11 @@ public Optimizer Build(List<Phase> phases)
_phases = phases;

double m0 = _phases[0].m0;
var problem = new Problem(_r0, _v0, _u0, m0, _t0, _mu, _rbody);

if (_terminal == null)
throw new Exception("Optimizer.Build() called with no terminal conditions");

var problem = new Problem(_r0, _v0, _u0, m0, _t0, _mu, _rbody, _terminal);

var normalizedPhases = new List<Phase>();

Expand All @@ -40,11 +44,6 @@ public Optimizer Build(List<Phase> phases)
normalizedPhases.Add(phase.Rescale(problem.Scale));
}

if (_terminal == null)
throw new Exception("Optimizer.Build() called with no terminal conditions");

problem.Terminal = _terminal.Rescale(problem.Scale);

var solver = new Optimizer(problem, normalizedPhases);
return solver;
}
Expand Down
43 changes: 19 additions & 24 deletions MechJeb2/MechJebLib/PVG/Problem.cs
Expand Up @@ -10,33 +10,28 @@

namespace MechJebLib.PVG
{
public class Problem
public readonly struct Problem
{
public readonly Scale Scale;
public IPVGTerminal? Terminal;
public readonly V3 R0;
public readonly V3 R0Bar;
public readonly double M0;
public readonly double M0Bar;
public readonly V3 V0;
public readonly V3 V0Bar;
public readonly double T0;
public readonly V3 U0;
public readonly double Mu;
public readonly double Rbody;
public readonly Scale Scale;
public readonly IPVGTerminal Terminal;
public readonly V3 R0;
public readonly V3 V0;
public readonly double M0;
public readonly double T0;
public readonly V3 U0;
public readonly double Mu;
public readonly double Rbody;

public Problem(V3 r0, V3 v0, V3 u0, double m0, double t0, double mu, double rbody)
public Problem(V3 r0, V3 v0, V3 u0, double m0, double t0, double mu, double rbody, IPVGTerminal terminal)
{
Scale = Scale.Create(mu, r0.magnitude, m0);
Mu = mu;
R0 = r0;
R0Bar = r0 / Scale.LengthScale;
V0 = v0;
V0Bar = v0 / Scale.VelocityScale;
M0 = m0;
M0Bar = m0 / Scale.MassScale;
U0 = u0;
Rbody = rbody;
Scale = Scale.Create(mu, r0.magnitude, m0);
Mu = mu;
R0 = r0 / Scale.LengthScale;
V0 = v0 / Scale.VelocityScale;
M0 = m0 / Scale.MassScale;
U0 = u0;
Rbody = rbody;
Terminal = terminal.Rescale(Scale);

T0 = t0;
}
Expand Down
73 changes: 73 additions & 0 deletions MechJeb2/MechJebLib/PVG/ResidualLayout.cs
@@ -0,0 +1,73 @@
/*
* 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.Collections.Generic;
using MechJebLib.Primitives;

namespace MechJebLib.PVG
{
public class ResidualLayout
{
public const int RESIDUAL_LAYOUT_LEN = 15;

public V3 R;
public V3 V;
private V3 _terminal0;
private V3 _terminal1;
public double M;
public double Bt;
public double Pm;

public (double a, double b, double c, double d, double e, double f) Terminal
{
set
{
_terminal0[0] = value.a;
_terminal0[1] = value.b;
_terminal0[2] = value.c;
_terminal1[0] = value.d;
_terminal1[1] = value.e;
_terminal1[2] = value.f;
}
}

public void CopyTo(IList<double> other)
{
R.CopyTo(other);
V.CopyTo(other, 3);
other[6] = M;
other[7] = _terminal0[0];
other[8] = _terminal0[1];
other[9] = _terminal0[2];
other[10] = _terminal1[0];
other[11] = _terminal1[1];
other[12] = _terminal1[2];
other[13] = Pm;
other[14] = Bt;
}

public void CopyFrom(IList<double> other)
{
R.CopyFrom(other);
V.CopyFrom(other, 3);
M = other[6];
_terminal0 = new V3(other[7], other[8], other[9]);
_terminal1 = new V3(other[10], other[11], other[12]);
Pm = other[13];
Bt = other[14];
}

public static ResidualLayout CreateFrom(IList<double> other)
{
var a = new ResidualLayout();

a.CopyFrom(other);

return a;
}
}
}

0 comments on commit 118a797

Please sign in to comment.