/// <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);
}
// Brent's 1-dimensional derivative-free local minimization method
//
// Uses golden section search and successive parabolic interpolation.
//
// f - 1-dimensional BrentFun function to find the minimum of
// a - lower endpoint of the interval
// b - upper endpoint of the interval
// rtol - tolerance to solve to (1e-4 or something like that)
// x - output of solved value
// y - output of the function at the solved value
// o - object to be passed to BrentFun for extra data (may be null)
// maxiter - cap on iterations (will throw TimeoutException if exceeded)
//
public static void Minimize(BrentFun f, double a, double b, double tol, out double x, out double y, object o, int maxiter = 50)
{
double c;
double d = 0;
double e;
double eps;
double fu;
double fv;
double fw;
double fx;
double m;
double p;
double q;
double r;
double sa;
double sb;
double t2;
double t;
double u;
double v;
double w;
int i = 0;
//
// C is the square of the inverse of the golden ratio.
//
c = 0.5 * (3.0 - Math.Sqrt(5.0));
eps = Math.Sqrt(MuUtils.DBL_EPSILON);
sa = a;
sb = b;
x = sa + c * (b - a);
w = x;
v = w;
e = 0.0;
fx = f(x, o);
fw = fx;
fv = fw;
while (true)
{
m = 0.5 * (sa + sb);
t = eps * Math.Abs(x) + tol;
t2 = 2.0 * t;
//
// Check the stopping criterion.
//
if (Math.Abs(x - m) <= t2 - 0.5 * (sb - sa))
{
break;
}
//
// Fit a parabola.
//
r = 0.0;
q = r;
p = q;
if (t < Math.Abs(e))
{
r = (x - w) * (fx - fv);
q = (x - v) * (fx - fw);
p = (x - v) * q - (x - w) * r;
q = 2.0 * (q - r);
if (0.0 < q)
{
p = -p;
}
q = Math.Abs(q);
r = e;
e = d;
}
if (Math.Abs(p) < Math.Abs(0.5 * q * r) &&
q * (sa - x) < p &&
p < q * (sb - x))
{
//
// Take the parabolic interpolation step.
//
d = p / q;
u = x + d;
//
// F must not be evaluated too close to A or B.
//
if ((u - sa) < t2 || (sb - u) < t2)
{
if (x < m)
{
d = t;
}
else
{
d = -t;
}
}
}
//
// A golden-section step.
//
else
{
if (x < m)
{
e = sb - x;
}
else
{
e = sa - x;
}
d = c * e;
}
//
// F must not be evaluated too close to X.
//
if (t <= Math.Abs(d))
{
u = x + d;
}
else if (0.0 < d)
{
u = x + t;
}
else
{
u = x - t;
}
fu = f(u, o);
//
// Update A, B, V, W, and X.
//
if (fu <= fx)
{
if (u < x)
{
sb = x;
}
else
{
sa = x;
}
v = w;
fv = fw;
w = x;
fw = fx;
x = u;
fx = fu;
}
else
{
if (u < x)
{
sa = u;
}
else
{
sb = u;
}
if (fu <= fw || w == x)
{
v = w;
fv = fw;
w = u;
fw = fu;
}
else if (fu <= fv || v == x || v == w)
{
v = u;
fv = fu;
}
}
if (i++ >= maxiter)
{
throw new TimeoutException("Brent's minimization method: maximum iterations exceeded");
}
}
y = fx;
}
// Brent's rootfinding method
//
// Uses secant or inverse quadratic interpolation as appropriate, with a fallback to bisection if necessary.
//
// f - 1-dimensional BrentFun function to find the root on
// a - first guess of x value
// b - second guess of x value
// rtol - tolerance to solve to (1e-4 or something like that)
// x - output of solved value
// y - output of the function at the solved value (should be zero to rtol)
// o - object to be passed to BrentFun for extra data (may be null)
// maxiter - cap on iterations (will throw TimeoutException if exceeded)
// sign - select the positive or negative side of the root for return value
//
// NOTE: please make any fixes to both versions of this routine
//
public static void Root(BrentFun f, double a, double b, double rtol, out double x, out double y, object o, int maxiter = 50, int sign = 0)
{
double c = 0;
double d = Double.MaxValue;
double fa = f(a, o);
double fb = f(b, o);
double fc = 0;
double s = 0;
double fs = 0;
if (fa * fb >= 0)
{
throw new ArgumentException("Brent's rootfinding method: guess does not bracket the root");
}
if (Math.Abs(fa) < Math.Abs(fb))
{
double tmp = a;
a = b;
b = tmp;
tmp = fa;
fa = fb;
fb = tmp;
}
c = a;
fc = fa;
bool mflag = true;
int i = 0;
while ((!(fb == 0) && (Math.Abs(a - b) > rtol)) || ((sign != 0) && (Math.Sign(fb) != sign)))
{
if ((fa != fc) && (fb != fc))
{
// inverse quadratic interpolation
s = a * fb * fc / (fa - fb) / (fa - fc) + b * fa * fc / (fb - fa) /
(fb - fc) + c * fa * fb / (fc - fa) / (fc - fb);
}
else
{
// secant
s = b - fb * (b - a) / (fb - fa);
}
double tmp2 = (3 * a + b) / 4;
if ((!(((s > tmp2) && (s < b)) || ((s < tmp2) && (s > b)))) ||
(mflag && (Math.Abs(s - b) >= (Math.Abs(b - c) / 2))) ||
(!mflag && (Math.Abs(s - b) >= (Math.Abs(c - d) / 2))))
{
// bisection
s = (a + b) / 2;
mflag = true;
}
else
{
if ((mflag && (Math.Abs(b - c) < rtol)) || (!mflag && (Math.Abs(c - d) < rtol)))
{
s = (a + b) / 2;
mflag = true;
}
else
{
mflag = false;
}
}
fs = f(s, o);
d = c;
c = b;
fc = fb;
if (fa * fs < 0)
{
b = s;
fb = fs;
}
else
{
a = s;
fa = fs;
}
if (Math.Abs(fa) < Math.Abs(fb))
{
double tmp = a;
a = b;
b = tmp;
tmp = fa;
fa = fb;
fb = tmp;
}
if (i++ >= maxiter)
{
x = b;
y = fb;
throw new TimeoutException("Brent's rootfinding method: maximum iterations exceeded");
}
}
x = b;
y = fb;
}