// 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);
}
//
// Assembling algorithm for equation solving
//
void OdeProb::Assemble()
{
size_t i, j, psiI, psiJ;
int ni; // row position in matrix S
int nj; // column position in matrix S
// const size_t M = Dim();
const size_t N = EltNo(); // Number of elements
// const double bndr[3] = {0, m_left.m_val, m_right.m_val};
// Element loop
for(size_t n = 0; n < N; n++)
{
const Element& e = Elt(n);
const size_t DofNo = e.DofNo();
// Loop over basis functions
for(i = 0; i < DofNo; i++)
{
ni = e.m_dof[i];
if(ni < 0)
continue;
psiI = e.PsiId(i);
// Loop over basis functions
for(j = i; j < DofNo; j++)
{
psiJ = e.PsiId(j);
nj = e.m_dof[j];
if(nj > -1)
m_s->Set(ni, nj) += CalcS(e, psiI, psiJ);
//else // Dirichlet boundary conditions are ZERO, hence it can be skiped
// m_b->Set(ni) -= bndr[-nj] * CalcS(e, psiI, psiJ);
}
// Contribution of the vertex basis function $v_{m_1}$ to the right hand side $b$
m_b->Set(ni) += CalcB(e, psiI);
}
}
}
