public CserResult(Vector3 r1, Vector3 v1, double x, CserResult previous)
{
this.R1 = r1;
this.V1 = v1;
this.X = x;
this.Previous = previous;
}
public static CserResult CSE(Vector3 r0, Vector3 v0, double dt, CserResult previous, double mu, double x0 = 0,
double tol = 5e-9)
{
var rScale = r0.Magnitude;
var vScale = Math.Sqrt(mu / rScale);
var r0s = r0 / rScale;
var v0s = v0 / vScale;
var dts = dt * vScale / rScale;
var v2s = v0.SqrMagnitude * rScale / mu;
var alpha = 2 - v2s;
var armd1 = v2s - 1;
var rvr0s = r0.DotProduct(v0) / Math.Sqrt(mu * rScale);
var x = x0;
if (x0 == 0)
{
x = dts * Math.Abs(alpha);
}
var ratio = 1d;
var x2 = x * x;
var z = alpha * x2;
var SCz = SnC(z);
var x2Cz = x2 * SCz["C"];
var f = 0d;
var df = 0d;
while (Math.Abs(ratio) > tol)
{
f = x + rvr0s * x2Cz + armd1 * x * x2 * SCz["S"] - dts;
df = x * rvr0s * (1 - z * SCz["S"]) + armd1 * x2Cz + 1;
ratio = f / df;
x = x - ratio;
x2 = x * x;
z = alpha * x2;
SCz = SnC(z);
x2Cz = x2 * SCz["C"];
}
var Lf = 1 - x2Cz;
var Lg = dts - x2 * x * SCz["S"];
var r1 = Lf * r0s + Lg * v0s;
var ir1 = 1 / r1.Magnitude;
var LfDot = ir1 * x * (z * SCz["S"] - 1);
var LgDot = 1 - x2Cz * ir1;
var v1 = LfDot * r0s + LgDot * v0s;
return(new CserResult(r1 * rScale, v1 * vScale, x, previous));
}
