public static void CSE(double mu, Vector3d r0, Vector3d v0, double dt, ref CSER last, out Vector3d r, out Vector3d v)
{
if (!r0.magnitude.IsFinite() || !v0.magnitude.IsFinite() || !dt.IsFinite() || !mu.IsFinite())
{
throw new ConicStateBadArguments();
}
double dtcp = last.dtcp;
if (last.dtcp == 0.0D)
{
dtcp = dt;
}
double xcp = last.xcp;
double x = xcp;
double A, D, E;
int kmax = 10;
int imax = 10;
double f0;
if (dt >= 0)
{
f0 = 1;
}
else
{
f0 = -1;
}
int n = 0;
double r0m = r0.magnitude;
double f1 = f0 * Math.Sqrt(r0m / mu);
double f2 = 1 / f1;
double f3 = f2 / r0m;
double f4 = f1 * r0m;
double f5 = f0 / Math.Sqrt(r0m);
double f6 = f0 * Math.Sqrt(r0m);
Vector3d ir0 = r0 / r0m;
Vector3d v0s = f1 * v0;
double sigma0s = Vector3d.Dot(ir0, v0s);
double b0 = Vector3d.Dot(v0s, v0s) - 1;
double alphas = 1 - b0; // reciprocal of the noramlized sma = r0m / sma = 2 - r0m * v0m * v0m / mu
double xguess = f5 * x;
double xlast = f5 * xcp;
double xmin = 0;
double dts = f3 * dt;
double dtlast = f3 * dtcp;
double dtmin = 0;
/* Math.Sqrt(Math.Abs(alphas)) < (small number) check is missing for parabolic orbits */
double xmax = 2 * Math.PI / Math.Sqrt(Math.Abs(alphas));
double dtmax;
double xP = 0.0;
double Ps = 0.0;
if (alphas > 0)
{
// circular/elliptical orbit
dtmax = xmax / alphas;
xP = xmax;
Ps = dtmax;
while (dts >= Ps)
{
// 1 or more orbits into the future
n = n + 1;
dts = dts - Ps;
dtlast = dtlast - Ps;
xguess = xguess - xP;
xlast = xlast - xP;
}
}
else
{
// hyperbolic orbit
KTTI(xmax, sigma0s, alphas, kmax, out dtmax, out A, out D, out E);
while (dtmax < dts)
{
dtmin = dtmax;
xmin = xmax;
xmax = 2 * xmax;
KTTI(xmax, sigma0s, alphas, kmax, out dtmax, out A, out D, out E);
}
}
if ((xguess <= xmin) || (xguess >= xmax))
{
xguess = (xmin + xmax) / 2.0;
}
double dtguess;
KTTI(xguess, sigma0s, alphas, kmax, out dtguess, out A, out D, out E);
if (dts < dtguess)
{
if ((xlast > xguess) && (xlast < xmax) && (dtlast > dtguess) && (dtlast < dtmax))
{
xmax = xlast;
dtmax = dtlast;
}
}
else
{
if ((xlast > xmin) && (xlast < xguess) && (dtlast > dtmin) && (dtlast < dtguess))
{
xmin = xlast;
dtmin = dtlast;
}
}
Kepler(imax, dts, ref xguess, ref dtguess, xmin, dtmin, xmax, dtmax, sigma0s, alphas, kmax, out A, out D, out E);
double rs = 1 + 2 * (b0 * A + sigma0s * D * E);
double b4 = 1 / rs;
double xc = f6 * (xguess + n * xP);
double dtc = f4 * (dtguess + n * Ps);
last.dtcp = dtc;
last.xcp = xc;
/* last.A = A;
* last.D = D;
* last.E = E; */
double F = 1 - 2 * A;
double Gs = 2 * (D * E + sigma0s * A);
double Fts = -2 * b4 * D * E;
double Gt = 1 - 2 * b4 * A;
r = r0m * (F * ir0 + Gs * v0s);
v = f2 * (Fts * ir0 + Gt * v0s);
}
/*
* mu - gravitational parameter
* r0 - starting radius vector
* v0 - starting velocity vector
* dt - time of extrapolation
* last - prior state for sequential calls, zero'd to initialize
* r - predicted radius vector
* v - predicted velocity vector
*/
public static void CSE(double mu, Vector3d r0, Vector3d v0, double dt, out Vector3d r, out Vector3d v)
{
CSER last = new CSER();
CSE(mu, r0, v0, dt, ref last, out r, out v);
}
