double cOrbit::TimeAtMeanAnomaly(double M) const
{
if (IsHyperbolic())
{
return M / MeanMotion() + TimeOfPeriapsisPassage();
}
else
{
M = fmod(M + 32*Period(), Period());
return M / MeanMotion() + TimeOfPeriapsisPassage();
}
}
double cOrbit::MeanAnomalyAt(double t) const
{
if (IsHyperbolic())
{
return (t - TimeOfPeriapsisPassage()) * MeanMotion();
}
else
{
double M = fmod(t - TimeOfPeriapsisPassage() + 32*Period(), Period()) * MeanMotion();
return M;
}
}
// ==============================================================================================================================================
//
void Orbit::CreateProjectionPlane(VECTOR3 normal, VECTOR3 zero)
{
Reset();
myy = 1.327e20;
ecc = 0;
sma = AU;
rad = AU;
inc = 0;
lan = 0;
agp = 0;
tra = 0;
par = sma;
mna = 0;
mnm = MeanMotion();
ang = sqrt(myy*par);
lpe = 0;
trl = 0;
majv = unit(crossp(crossp(normal,zero),normal));
norv = unit(normal);
minv = unit(crossp(norv,majv));
Xmajv = majv * sma;
Xminv = minv * sma;
Xcv = _V(0,0,0);
SetProjection(this);
}
// ==============================================================================================================================================
//
void Orbit::Ecliptic()
{
if (EclipticPlane) return;
EclipticPlane=true;
Reset();
myy = 1.327e20;
ecc = 0;
sma = AU;
rad = AU;
inc = 0;
lan = 0;
agp = 0;
tra = 0;
par = sma;
mna = 0;
mnm = MeanMotion();
ang = sqrt(myy*par);
lpe = 0;
trl = 0;
majv = _V(1,0,0);
minv = _V(0,0,1);
norv = _V(0,-1,0);
Xmajv = majv * sma;
Xminv = minv * sma;
Xcv = _V(0,0,0);
SetProjection(this);
}
OrbitalElements::OrbitalElements(const Tle& tle)
{
/*
* extract and format tle data
*/
mean_anomoly_ = tle.MeanAnomaly(false);
ascending_node_ = tle.RightAscendingNode(false);
argument_perigee_ = tle.ArgumentPerigee(false);
eccentricity_ = tle.Eccentricity();
inclination_ = tle.Inclination(false);
mean_motion_ = tle.MeanMotion() * kTWOPI / kMINUTES_PER_DAY;
bstar_ = tle.BStar();
epoch_ = tle.Epoch();
/*
* recover original mean motion (xnodp) and semimajor axis (aodp)
* from input elements
*/
const double a1 = pow(kXKE / MeanMotion(), kTWOTHIRD);
const double cosio = cos(Inclination());
const double theta2 = cosio * cosio;
const double x3thm1 = 3.0 * theta2 - 1.0;
const double eosq = Eccentricity() * Eccentricity();
const double betao2 = 1.0 - eosq;
const double betao = sqrt(betao2);
const double temp = (1.5 * kCK2) * x3thm1 / (betao * betao2);
const double del1 = temp / (a1 * a1);
const double a0 = a1 * (1.0 - del1 * (1.0 / 3.0 + del1 * (1.0 + del1 * 134.0 / 81.0)));
const double del0 = temp / (a0 * a0);
recovered_mean_motion_ = MeanMotion() / (1.0 + del0);
/*
* alternative way to calculate
* doesnt affect final results
* recovered_semi_major_axis_ = pow(XKE / RecoveredMeanMotion(), TWOTHIRD);
*/
recovered_semi_major_axis_ = a0 / (1.0 - del0);
/*
* find perigee and period
*/
perigee_ = (RecoveredSemiMajorAxis() * (1.0 - Eccentricity()) - kAE) * kXKMPER;
period_ = kTWOPI / RecoveredMeanMotion();
}
double cOrbit::Period() const
{
if (IsHyperbolic())
{
return std::numeric_limits<double>::infinity();
}
else
{
return 2 * M_PI / MeanMotion();
}
}
// ==============================================================================================================================================
// Hyberbolic use only
// ReCalculate orbit position (mna,tra,trl,rad,vel)
//
void Orbit::SetTimeToPeriapsis(double t)
{
if (Defined()) {
mna = -t * MeanMotion();
tra = mna2tra(mna,ecc);
trl = limit(tra+lpe);
vv = Tangent(trl);
rv = Position(trl);
rad = length(rv);
vel = length(vv);
}
}
// ==============================================================================================================================================
//
void Orbit::Elements(VECTOR3 r, VECTOR3 v,double m,bool reset_proj)
{
Reset();
if (fabs(r.x)<0.01) r.x=0; if (fabs(r.y)<0.01) r.y=0;
if (fabs(r.z)<0.01) r.z=0; if (fabs(v.x)<0.01) v.x=0;
if (fabs(v.y)<0.01) v.y=0; if (fabs(v.z)<0.01) v.z=0;
vv=v; rv=r;
double vel2;
VECTOR3 z; z.x=0; z.z=0; z.y=1.0;
vel2 = dotp(v,v);
vel = sqrt(vel2);
rad = length(r);
myy = m;
// Computer normal vector and vector pointing ascenting node
norv = crossp(r,v);
ang = length(norv);
norv = norv*(1.0/ang);
VECTOR3 ANv = unit(crossp(z,norv));
// Inclination
inc = acos(-norv.y);
par = ang*ang/myy;
majv =( r * (vel2-myy/rad) ) - (v * dotp(r,v));
double ml=length(majv);
ecc = ml/myy;
if (ecc<1e-6) ecc=0;
if (inc<1e-6) inc=0;
sma = par / (1.0-ecc*ecc);
r=r*(1.0/rad); // Make the radius vector to unit size, After computing ecc-vector
if (inc==0) ANv=_V(1,0,0); // Place ANv to vernal equinox
if (ecc!=0) majv=majv*(1.0/ml); // Make the major vector to unit size
else majv=ANv; // Place major vector to ascenting node
// Longitude of ascenting node
if (inc!=0) {
double x=ANv.x;
if (x>=1) {
lan=0;
} else if (x<=-1) {
lan=PI;
} else {
lan=acos(x);
if (ANv.z<0) lan=PI2-lan;
}
} else {
lan=0.0;
}
// Argument of Periapsis
if (inc!=0 && ecc!=0) {
double x=dotp(ANv,majv);
if (x>1) x=1; // Avoid some precision problems
else if (x<-1) x=-1;
agp=acos(x);
if (majv.y<0) agp=PI2-agp;
} else if (ecc!=0) {
agp=acos(majv.x);
if (majv.z<0) agp=PI2-agp;
}
else agp=0.0;
// True anomaly
if (ecc!=0) {
double x=dotp(majv,r);
if (x>=1) tra=0;
else if (x<=-1) tra=PI;
else {
tra=acos(x);
x=dotp(r,v);
if (fabs(x)<1e-6) x=0; // Avoid some precision problems
if (x<0) tra=PI2-tra;
}
} else if (inc!=0) {
tra=acos(dotp(ANv,r));
if (dotp(ANv,v)>0) tra=PI2-tra;
} else {
tra=acos(r.x);
if (v.x>0) tra=PI2-tra;
}
lpe=limit(agp+lan); // Longitude of Periapsis
trl=limit(lpe+tra); // True longitude
minv=unit(crossp(norv,majv)); // Minor axis vector
mna=tra2mna(tra,ecc); // Mean anomaly
mnm=MeanMotion();
Xmajv = majv * sma;
Xminv = minv * sqrt(fabs(sma*par));
Xcv = majv * (sma*ecc);
if (reset_proj) SetProjection(this);
}
