/// <summary>
/// Single impulse transfer from an ellipitical, non-coplanar parking orbit to an arbitrary hyperbolic v-infinity target.
///
/// Ocampo, C., & Saudemont, R. R. (2010). Initial Trajectory Model for a Multi-Maneuver Moon-to-Earth Abort Sequence.
/// Journal of Guidance, Control, and Dynamics, 33(4), 1184–1194.
/// </summary>
///
/// <param name="mu">Gravitational parameter of central body.</param>
/// <param name="r0">Reference position on the parking orbit.</param>
/// <param name="v0">Reference velocity on the parking orbit.</param>
/// <param name="v_inf">Target hyperbolic v-infinity vector.</param>
/// <param name="vneg">Velocity on the parking orbit before the burn.</param>
/// <param name="vpos">Velocity on the hyperboliic ejection orbit after the burn.</param>
/// <param name="r">Position of the burn.</param>
/// <param name="dt">Coasting time on the parking orbit from the reference to the burn.</param>
///
public static void singleImpulseHyperbolicBurn(double mu, Vector3d r0, Vector3d v0, Vector3d v_inf,
ref Vector3d vneg, ref Vector3d vpos, ref Vector3d r, ref double dt, bool debug = false)
{
double rot, dv;
BrentFun f = delegate(double testrot, object ign) {
double dt = 0;
Vector3d vneg = new Vector3d();
Vector3d vpos = new Vector3d();
Vector3d r = new Vector3d();
singleImpulseHyperbolicBurn(mu, r0, v0, v_inf, ref vneg, ref vpos, ref r, ref dt, (float)testrot, debug);
return((vpos - vneg).magnitude);
};
Brent.Minimize(f, -30, 30, 1e-6, out rot, out dv, null);
singleImpulseHyperbolicBurn(mu, r0, v0, v_inf, ref vneg, ref vpos, ref r, ref dt, (float)rot, debug);
}
// Dormand Prince 5(4)7FM ODE integrator (aka DOPRI5)
//
// We prefer this over Bogacki–Shampine in order to obtain the free 4th order interpolant.
//
// f - the ODE function to integrate
// o - object paramter which is passed through to the ODE function
// y0 - array of starting y0 values
// n - dimensionality of the problem
// xtbl - array of x values to evaluate at (2 minimum for start + end)
// ytbl - output jagged array of y values at the x evaluation points (n x 2 minimum)
// eps - accuracy
// h - starting step-size (can be zero for automatic guess)
// xlist - output list of all intermediate x values (user allocates, can be null or omitted)
// ylist - output list of all intermediate y values (user allocates, can be null or omitted)
// hmin - minimum h step (may be violated on the last step)
// hmax - maximum h step
// maxiter - maximum number of steps
// EvtFuns - array of event functions
//
// ref: https://github.com/scipy/scipy/blob/master/scipy/integrate/dop/dopri5.f
//
public static void RKDP547FM(ODEFun f, object o, double[] y0, int n, Span <double> xtbl, Span <double[]> ytbl, double eps, double hstart, List <double> xlist = null, List <double []> ylist = null, double hmin = 0, double hmax = 0, int maxiter = 0, EvtFun[] EvtFuns = null)
{
Span <double> k1 = stackalloc double[n];
Span <double> k2 = stackalloc double[n];
Span <double> k3 = stackalloc double[n];
Span <double> k4 = stackalloc double[n];
Span <double> k5 = stackalloc double[n];
Span <double> k6 = stackalloc double[n];
Span <double> k7 = stackalloc double[n];
// accumulator
Span <double> a = stackalloc double[n];
// error
Span <double> err = stackalloc double[n];
double h = hstart;
double x = xtbl[0];
Span <double> y = stackalloc double[n];
if (ylist != null && xlist != null)
{
ylist.Clear();
xlist.Clear();
}
y0.CopyTo(y);
// auto-guess starting value of h based on smallest dx in xtbl
if (h == 0)
{
double v = Math.Abs(xtbl[1] - xtbl[0]);
for (int i = 1; i < xtbl.Length - 1; i++)
{
v = Math.Min(v, Math.Abs(xtbl[i + 1] - xtbl[i]));
}
h = 0.001 * v;
}
// xtbl controls the direcction of integration, the sign of h is not relevant
h = Math.Sign(xtbl[1] - xtbl[0]) * Math.Abs(h);
// copy initial conditions to ybtl output
int j = 0;
for (int i = 0; i < n; i++)
{
ytbl[i][j] = y[i];
}
j++;
// copy initial conditions to full xlist/ylist output
if (xlist != null && ylist != null)
{
double[] ydup2 = new double[n];
y.CopyTo(ydup2);
ylist.Add(ydup2);
xlist.Add(x);
}
bool fsal = false;
bool at_hmin;
double h_restart = 0;
double niter = 0;
while (j < xtbl.Length)
{
double xf = xtbl[j];
while ((h > 0) ? x <xf : x> xf)
{
at_hmin = false;
if (hmax > 0 && Math.Abs(h) > hmax)
{
h = hmax * Math.Sign(h);
}
if (hmin > 0 && Math.Abs(h) < hmin)
{
h = hmin * Math.Sign(h);
at_hmin = true;
}
// we may violate hmin in order to exactly hit the boundary conditions
if (Math.Abs(h) > Math.Abs(xf - x))
{
h = xf - x;
}
if (fsal)
{
// FIXME: tricks to make this copy go away?
for (int i = 0; i < n; i++)
{
k1[i] = k7[i];
}
}
else
{
f(y, x, k1, n, o);
}
for (int i = 0; i < n; i++)
{
a[i] = y[i] + h * (a21 * k1[i]);
}
f(a, x + c2 * h, k2, n, o);
for (int i = 0; i < n; i++)
{
a[i] = y[i] + h * (a31 * k1[i] + a32 * k2[i]);
}
f(a, x + c3 * h, k3, n, o);
for (int i = 0; i < n; i++)
{
a[i] = y[i] + h * (a41 * k1[i] + a42 * k2[i] + a43 * k3[i]);
}
f(a, x + c4 * h, k4, n, o);
for (int i = 0; i < n; i++)
{
a[i] = y[i] + h * (a51 * k1[i] + a52 * k2[i] + a53 * k3[i] + a54 * k4[i]);
}
f(a, x + c5 * h, k5, n, o);
for (int i = 0; i < n; i++)
{
a[i] = y[i] + h * (a61 * k1[i] + a62 * k2[i] + a63 * k3[i] + a64 * k4[i] + a65 * k5[i]);
}
f(a, x + c6 * h, k6, n, o);
for (int i = 0; i < n; i++)
{
a[i] = y[i] + h * (a71 * k1[i] + a73 * k3[i] + a74 * k4[i] + a75 * k5[i] + a76 * k6[i]);
}
f(a, x + c7 * h, k7, n, o);
for (int i = 0; i < n; i++)
{
err[i] = k1[i] * (b1 - b1p) + k3[i] * (b3 - b3p) + k4[i] * (b4 - b4p) + k5[i] * (b5 - b5p) + k6[i] * (b6 - b6p) + k7[i] * (b7 - b7p);
}
double error = 0;
for (int i = 0; i < n; i++)
{
// FIXME: look at dopri fortran code to see how they generate this
error = Math.Max(error, Math.Abs(err[i]));
}
double s = 0.84 * Math.Pow(eps / error, 1.0 / 5.0);
int evt = -1;
if (error < eps || at_hmin || h_restart != 0)
{
int sign = 0;
if (EvtFuns != null)
{
for (int i = 0; i < EvtFuns.Length; i++)
{
EvtFun e = EvtFuns[i];
double e1 = e(y, x, n, o);
double e2 = e(a, x + h, n, o);
if (e1 * e2 < 0)
{
evt = i;
// sign is passed to Brent to ensure we select a value on the other side of the event
sign = Math.Sign(e2);
break;
}
}
}
if (evt < 0 || h_restart != 0)
{
fsal = true; // advancing so use k7 for k1 next time
for (int i = 0; i < n; i++)
{
y[i] = a[i]; // FSAL
}
x = x + h;
if (xlist != null && ylist != null)
{
double[] ydup = new double[n];
for (int i = 0; i < n; i++)
{
ydup[i] = y[i];
}
ylist.Add(ydup);
xlist.Add(x);
}
if (h_restart != 0)
{
h = h_restart;
s = 1;
h_restart = 0;
}
}
else
{
fsal = false;
s = 1;
if (h_restart == 0)
{
h_restart = h;
}
Brent.Root((newh, a1, a2, a3, a4, o2) => EvtWrapper(EvtFuns[evt], newh, x, a1, a2, x + h, a3, a4, n, o2), 0, h, 1e-15, out h, out _, o, y, k1, a, k7, sign: sign);
}
}
else
{
fsal = false; // rewinding so k7 is no longer valid
}
if (s < 0.1)
{
s = 0.1;
}
if (s > 4)
{
s = 4;
}
h = h * s;
if (maxiter > 0 && niter++ >= maxiter)
{
throw new ArgumentException("maximum iterations exceeded");
}
}
for (int i = 0; i < n; i++)
{
ytbl[i][j] = y[i];
}
j++;
}
}