// Iterative method to calculate new X and Z values for the specified time of flight
static inline void CalcXandZ(double &X, double &Z, const VECTOR3 Pos, const VECTOR3 Vel,
double a, const double Time, const double SqrtMu)
{
const double MAX_ITERS = 10;
double C, S, T, dTdX, DeltaTime, r = Mag(Pos), IterNum = 0;
// These don't change over the iterations
double RVMu = (Pos * Vel) / SqrtMu; // Dot product of position and velocity divided
// by the squareroot of Mu
double OneRA = (1 - (r / a)); // One minus Pos over the semi-major axis
C = CalcC(Z);
S = CalcS(Z);
T = ((RVMu * pow(X, 2) * C) + (OneRA * pow(X, 3) * S) + (r * X)) / SqrtMu;
DeltaTime = Time - T;
// Iterate while the result isn't within tolerances
while (fabs(DeltaTime) > EPSILON && IterNum++ < MAX_ITERS) {
dTdX = ((pow(X, 2) * C) + (RVMu * X * (1 - Z * S)) + (r * (1 - Z * C))) / SqrtMu;
X = X + (DeltaTime / dTdX);
Z = CalcZ(X, a);
C = CalcC(Z);
S = CalcS(Z);
T = ((RVMu * pow(X, 2) * C) + (OneRA * pow(X, 3) * S) + (r * X)) / SqrtMu;
DeltaTime = Time - T;
}
}
// Given the specified position and velocity vectors for a given orbit, retuns the position
// and velocity vectors after a specified time
void PredictPosVelVectors(const VECTOR3 &Pos, const VECTOR3 &Vel, double a, double Mu,
double Time, VECTOR3 &NewPos, VECTOR3 &NewVel, double &NewVelMag)
{
double SqrtMu = sqrt(Mu);
// Variables for computation
double X = (SqrtMu * Time) / a; // Initial guesses for X
double Z = CalcZ(X, a); // and Z
double C, S; // C(Z) and S(Z)
double F, FDot, G, GDot;
// Calculate the X and Z for the specified time of flight
CalcXandZ(X, Z, Pos, Vel, a, Time, SqrtMu);
// Calculate C(Z) and S(Z)
C = CalcC(Z);
S = CalcS(Z);
// Calculate the new position and velocity vectors
F = CalcF(X, C, Mag(Pos));
G = CalcG(Time, X, S, SqrtMu);
NewPos = (Pos * F) + (Vel * G);
FDot = CalcFDot(SqrtMu, Mag(Pos), Mag(NewPos), X, Z, S);
GDot = CalcGDot(X, C, Mag(NewPos));
NewVel = (Pos * FDot) + (Vel * GDot);
NewVelMag = Mag(NewVel);
}
void JacobiPolynomialAlpha :: Calc (int n, int alpha)
{
if (coefs.Size() < (n+1)*(alpha+1))
{
coefs.SetSize ( /* 3* */ (n+1)*(alpha+1));
for (int a = 0; a <= alpha; a++)
{
for (int i = 1; i <= n; i++)
{
coefs[a*(n+1)+i][0] = CalcA (i, a, 0);
coefs[a*(n+1)+i][1] = CalcB (i, a, 0);
coefs[a*(n+1)+i][2] = CalcC (i, a, 0);
}
// ALWAYS_INLINE S P1(S x) const { return 0.5 * (2*(al+1)+(al+be+2)*(x-1)); }
double al = a, be = 0;
coefs[a*(n+1)+1][0] = 0.5 * (al+be+2);
coefs[a*(n+1)+1][1] = 0.5 * (2*(al+1)-(al+be+2));
coefs[a*(n+1)+1][2] = 0.0;
}
maxnp = n+1;
maxalpha = alpha;
}
}
void IntLegNoBubble :: Calc (int n)
{
if (coefs.Size() > n) return;
#pragma omp critical (calcintlegnobub)
{
if (coefs.Size() <= n)
{
coefs.SetSize (n+1);
coefs[0][0] = coefs[0][1] = 1e10;
// coefs[0][0] = -0.5;
// coefs[1][1] = -0.5;
for (int i = 1; i <= n; i++)
{
coefs[i][0] = CalcA(i);
coefs[i][1] = CalcC(i);
}
}
}
}
void LegendrePolynomial :: Calc (int n)
{
if (coefs.Size() > n) return;
#pragma omp critical (calclegendre)
{
if (coefs.Size() <= n)
{
coefs.SetSize (n+1);
coefs[0][0] = 1;
coefs[1][1] = 1;
for (int i = 1; i <= n; i++)
{
coefs[i][0] = CalcA(i); // (2.0*i-1)/i;
coefs[i][1] = CalcC(i); // (1.0-i)/i;
}
}
}
}
