StateVector LagrangianPropagator::PropagateStateVector(const StateVector& stateVector, double mu, const Time& timeDelta)
{
// Compute frequently used variables
const auto& R1 = stateVector.position;
const auto& V1 = stateVector.velocity;
const double seconds = timeDelta.Seconds();
const double sqrtMu = sqrt(mu);
const double r0 = R1.norm();
const double v0 = V1.norm();
const double rdotv = R1.dot(V1);
// Compute the universal variable results
auto results = CalculateUniversalVariable(r0, v0, rdotv, seconds, mu);
// Compute the Lagrange coefficients
auto coeff = CalculateLagrangeCoefficients(r0, seconds, sqrtMu, results);
// Compute the final cartesian vectors
Vector3d R2 = coeff.f * R1 + coeff.g * V1;
Vector3d V2 = coeff.fDot * R1 + coeff.gDot * V1;
return StateVector(R2, V2);
}
StateVector StateVector::makeXYZ(double x, double y, double z, double trk, double gs, double vs, double t){
Vect3 s = Vect3::makeXYZ(x,"nm",y,"nm",z,"ft");
Velocity v = Velocity::makeTrkGsVs(trk,gs,vs);
return StateVector(s,v,t);
}
