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