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); }