bool cOrbit::TrueAnomalyAtRadius(double r, double timeOfStart, double &tAOut) const
{
double e = Eccentricity();
double a = SemiMajorAxis();
bool status = false;
double arg = (a - a*e*e - r)/(e*r);
if(-1 <= arg && arg <= 1)
{
double tA = TrueAnomalyAt(timeOfStart, status);
tAOut = acos(arg);
tAOut = min(
fmod(tAOut - tA + 4*M_PI, 2*M_PI),
fmod(-tAOut - tA + 4*M_PI, 2*M_PI)
) + tA;
return status;
}
else
{
return false;
}
}
double cOrbit::EccentricAnomalyAtTrueAnomaly(double tA) const
{
double e = Eccentricity();
if (IsHyperbolic()) {
double cosTrueAnomaly = std::cos(tA);
double H = acosh((e + cosTrueAnomaly) / (1.0 + e * cosTrueAnomaly));
if (tA < 0)
{
return -H;
}
else
{
return H;
}
} else {
double E = 2 * std::atan(std::tan(tA / 2) / std::sqrt((1 + e) / (1 - e)));
if (E < 0)
{
return E + TWO_PI;
}
else
{
return E;
}
}
}
double cOrbit::TrueAnomalyAt(double t, bool &status) const
{
double e = Eccentricity();
if (IsHyperbolic())
{
double H = EccentricAnomalyAt(t, status);
double tA = std::acos((e - std::cosh(H)) / (std::cosh(H) * e - 1.0));
if (H < 0)
{
return -tA;
}
else
{
return tA;
}
}
else
{
double E = EccentricAnomalyAt(t, status);
double tA = 2 * std::atan2(std::sqrt(1.0 + e) * std::sin(E / 2.0), std::sqrt(1.0 - e) * std::cos(E / 2.0));
if (tA < 0)
{
return tA + TWO_PI;
}
else
{
return tA;
}
}
}
double cOrbit::EccentricAnomalyAt(double t, bool &status) const
{
ECC_ANOMALY_T params;
params.e = Eccentricity();
params.M = MeanAnomalyAt(t);
double EccentricAnomaly = params.M;
// to prevent sinh/cosh overflow
if(EccentricAnomaly < -10.0)
{
EccentricAnomaly = -10.0;
}
if(EccentricAnomaly > 10.0)
{
EccentricAnomaly = 10.0;
}
if (IsHyperbolic())
{
status = cTools::NewtonMethod<double, ECC_ANOMALY_T>(EccentricAnomaly, params, eccAnomalyFunction1, eccAnomalyDFunction1);
}
else
{
status = cTools::NewtonMethod<double, ECC_ANOMALY_T>(EccentricAnomaly, params, eccAnomalyFunction2, eccAnomalyDFunction2);
}
return EccentricAnomaly;
}
cOrbit::cOrbit(const cTle &tle) :
m_tle(tle),
m_pNoradModel(NULL)
{
InitializeCachingVars();
int epochYear = (int)m_tle.GetField(cTle::FLD_EPOCHYEAR);
double epochDay = m_tle.GetField(cTle::FLD_EPOCHDAY );
if (epochYear < 57)
{
epochYear += 2000;
}
else
{
epochYear += 1900;
}
m_jdEpoch = cJulian(epochYear, epochDay);
m_secPeriod = -1.0;
// Recover the original mean motion and semimajor axis from the
// input elements.
double mm = MeanMotionTle();
double rpmin = mm * TWOPI / MIN_PER_DAY; // rads per minute
double a1 = pow(XKE / rpmin, 2.0 / 3.0);
double e = Eccentricity();
double i = Inclination();
double temp = (1.5 * CK2 * (3.0 * sqr(cos(i)) - 1.0) /
pow(1.0 - e * e, 1.5));
double delta1 = temp / (a1 * a1);
double a0 = a1 *
(1.0 - delta1 *
((1.0 / 3.0) + delta1 *
(1.0 + 134.0 / 81.0 * delta1)));
double delta0 = temp / (a0 * a0);
m_rmMeanMotionRec = rpmin / (1.0 + delta0);
m_aeAxisSemiMajorRec = a0 / (1.0 - delta0);
m_aeAxisSemiMinorRec = m_aeAxisSemiMajorRec * sqrt(1.0 - (e * e));
m_kmPerigeeRec = XKMPER_WGS72 * (m_aeAxisSemiMajorRec * (1.0 - e) - AE);
m_kmApogeeRec = XKMPER_WGS72 * (m_aeAxisSemiMajorRec * (1.0 + e) - AE);
if (TWOPI / m_rmMeanMotionRec >= 225.0)
{
// SDP4 - period >= 225 minutes.
m_pNoradModel = new cNoradSDP4(*this);
}
else
{
// SGP4 - period < 225 minutes
m_pNoradModel = new cNoradSGP4(*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::MeanAnomalyAtTrueAnomaly(double tA) const
{
double e = Eccentricity();
if (IsHyperbolic()) {
double H = EccentricAnomalyAtTrueAnomaly(tA);
return e * sinh(H) - H;
} else {
double E = EccentricAnomalyAtTrueAnomaly(tA);
return E - e * std::sin(E);
}
}
Vector3d cOrbit::VelocityAtTrueAnomaly(double tA) const
{
double mu = ReferenceBody().GravitationalParameter();
double e = Eccentricity();
double h = std::sqrt(mu * SemiMajorAxis() * (1.0 - e * e));
double r = RadiusAtTrueAnomaly(tA);
double sin = std::sin(tA);
double cos = std::cos(tA);
double vr = mu * e * sin / h;
double vtA = h / r;
return RotationToReferenceFrame()._transformVector(Vector3d(vr * cos - vtA * sin, vr * sin + vtA * cos, 0));
}
/**
* Constructs a TransverseMercator object
*
* @param label This argument must be a Label containing the proper mapping
* information as indicated in the Projection class. Additionally,
* the transversemercator projection requires the center longitude
* to be defined in the keyword CenterLongitude, and the scale
* factor to be defined in the keyword ScaleFactor.
*
* @param allowDefaults If set to false the constructor expects that a keyword
* of CenterLongitude and ScaleFactor will be in the label.
* Otherwise it will attempt to compute the center
* longitude using the middle of the longitude range
* specified in the label, and the scale factor will
* default to 1.0. Defaults to false.
*
* @throws Isis::iException::Io
*/
TransverseMercator::TransverseMercator(Isis::Pvl &label, bool allowDefaults) :
Isis::Projection::Projection (label) {
try {
// Try to read the mapping group
Isis::PvlGroup &mapGroup = label.FindGroup ("Mapping",Isis::Pvl::Traverse);
// Compute and write the default center longitude if allowed and
// necessary
if ((allowDefaults) && (!mapGroup.HasKeyword("CenterLongitude"))) {
double lon = (p_minimumLongitude + p_maximumLongitude) / 2.0;
mapGroup += Isis::PvlKeyword("CenterLongitude",lon);
}
// Compute and write the default center latitude if allowed and
// necessary
if ((allowDefaults) && (!mapGroup.HasKeyword("CenterLatitude"))) {
double lat = (p_minimumLatitude + p_maximumLatitude) / 2.0;
mapGroup += Isis::PvlKeyword("CenterLatitude",lat);
}
// Get the center longitude & latitude
p_centerLongitude = mapGroup["CenterLongitude"];
p_centerLatitude = mapGroup["CenterLatitude"];
// make sure the center latitude value is valid
if (fabs(p_centerLatitude) >= 90.0) {
string msg = "Invalid Center Latitude Value. Must be between -90 and 90";
throw Isis::iException::Message(Isis::iException::Io,msg,_FILEINFO_);
}
// make sure the center longitude value is valid
if (fabs(p_centerLongitude) > 360.0) {
string msg = "Invalid Center Longitude Value. Must be between -360 and 360";
throw Isis::iException::Message(Isis::iException::Io,msg,_FILEINFO_);
}
// convert latitude to planetographic if it is planetocentric
if (this->IsPlanetocentric()) {
p_centerLatitude = this->ToPlanetographic(p_centerLatitude);
}
// convert to radians and adjust for longitude direction
if (p_longitudeDirection == PositiveWest) p_centerLongitude *= -1.0;
p_centerLatitude *= Isis::PI / 180.0;
p_centerLongitude *= Isis::PI / 180.0;
// Compute other necessary variables
p_eccsq = Eccentricity()*Eccentricity();
p_esp = p_eccsq;
p_e0 = 1.0 - 0.25 * p_eccsq * (1.0 + p_eccsq / 16.0 * (3.0 + 1.25 *p_eccsq));
p_e1 = 0.375 * p_eccsq * (1.0 + 0.25 * p_eccsq * ( 1.0 + 0.468750 * p_eccsq));
p_e2 = 0.058593750 * p_eccsq * p_eccsq * (1.0 + 0.750 * p_eccsq);
p_e3 = p_eccsq * p_eccsq * p_eccsq * (35.0 / 3072.0);
p_ml0 = p_equatorialRadius * (p_e0 * p_centerLatitude - p_e1 * sin(2.0 * p_centerLatitude) +
p_e2 * sin(4.0 * p_centerLatitude) - p_e3 * sin(6.0 * p_centerLatitude));
// Set flag for sphere or ellipsiod
p_sph = true; // Sphere
if (Eccentricity() >= .00001) {
p_sph = false; // Ellipsoid
p_esp = p_eccsq / (1.0 - p_eccsq);
}
// Get the scale factor
if ((allowDefaults) && (!mapGroup.HasKeyword("ScaleFactor"))) {
mapGroup += Isis::PvlKeyword("ScaleFactor",1.0);
}
p_scalefactor = mapGroup["ScaleFactor"];
}
catch (Isis::iException &e) {
string message = "Invalid label group [Mapping]";
throw Isis::iException::Message(Isis::iException::Io,message,_FILEINFO_);
}
}
/**
* Constructs a TransverseMercator object
*
* @param label This argument must be a Label containing the proper mapping
* information as indicated in the Projection class. Additionally,
* the transversemercator projection requires the center longitude
* to be defined in the keyword CenterLongitude, and the scale
* factor to be defined in the keyword ScaleFactor.
*
* @param allowDefaults If set to false the constructor expects that a keyword
* of CenterLongitude and ScaleFactor will be in the label.
* Otherwise it will attempt to compute the center
* longitude using the middle of the longitude range
* specified in the label, and the scale factor will
* default to 1.0. Defaults to false.
*
* @throws IException
*/
TransverseMercator::TransverseMercator(Pvl &label, bool allowDefaults) :
TProjection::TProjection(label) {
try {
// Try to read the mapping group
PvlGroup &mapGroup = label.findGroup("Mapping", Pvl::Traverse);
// Compute and write the default center longitude if allowed and
// necessary
if ((allowDefaults) && (!mapGroup.hasKeyword("CenterLongitude"))) {
double lon = (m_minimumLongitude + m_maximumLongitude) / 2.0;
mapGroup += PvlKeyword("CenterLongitude", toString(lon));
}
// Compute and write the default center latitude if allowed and
// necessary
if ((allowDefaults) && (!mapGroup.hasKeyword("CenterLatitude"))) {
double lat = (m_minimumLatitude + m_maximumLatitude) / 2.0;
mapGroup += PvlKeyword("CenterLatitude", toString(lat));
}
// Get the center longitude & latitude
m_centerLongitude = mapGroup["CenterLongitude"];
m_centerLatitude = mapGroup["CenterLatitude"];
// make sure the center latitude value is valid
if (fabs(m_centerLatitude) >= 90.0) {
string msg = "Invalid Center Latitude Value. Must be between -90 and 90";
throw IException(IException::Io, msg, _FILEINFO_);
}
// make sure the center longitude value is valid
if (fabs(m_centerLongitude) > 360.0) {
string msg = "Invalid Center Longitude Value. Must be between -360 and 360";
throw IException(IException::Io, msg, _FILEINFO_);
}
// convert latitude to planetographic if it is planetocentric
if (IsPlanetocentric()) {
m_centerLatitude = ToPlanetographic(m_centerLatitude);
}
// convert to radians and adjust for longitude direction
if (m_longitudeDirection == PositiveWest) m_centerLongitude *= -1.0;
m_centerLatitude *= PI / 180.0;
m_centerLongitude *= PI / 180.0;
// Compute other necessary variables. See Snyder, page 61
m_eccsq = Eccentricity() * Eccentricity();
m_esp = m_eccsq;
m_e0 = 1.0 - 0.25 * m_eccsq * (1.0 + m_eccsq / 16.0 * (3.0 + 1.25 * m_eccsq));
m_e1 = 0.375 * m_eccsq * (1.0 + 0.25 * m_eccsq * (1.0 + 0.468750 * m_eccsq));
m_e2 = 0.058593750 * m_eccsq * m_eccsq * (1.0 + 0.750 * m_eccsq);
m_e3 = m_eccsq * m_eccsq * m_eccsq * (35.0 / 3072.0);
m_ml0 = m_equatorialRadius * (m_e0 * m_centerLatitude - m_e1 * sin(2.0 * m_centerLatitude) +
m_e2 * sin(4.0 * m_centerLatitude) - m_e3 * sin(6.0 * m_centerLatitude));
// Set flag for sphere or ellipsiod
m_sph = true; // Sphere
if (Eccentricity() >= .00001) {
m_sph = false; // Ellipsoid
m_esp = m_eccsq / (1.0 - m_eccsq);
}
// Get the scale factor
if ((allowDefaults) && (!mapGroup.hasKeyword("ScaleFactor"))) {
mapGroup += PvlKeyword("ScaleFactor", toString(1.0));
}
m_scalefactor = mapGroup["ScaleFactor"];
}
catch(IException &e) {
string message = "Invalid label group [Mapping]";
throw IException(e, IException::Io, message, _FILEINFO_);
}
}
double cOrbit::RadiusAtTrueAnomaly(double tA) const
{
double e = Eccentricity();
return SemiMajorAxis() * (1 - e * e) / (1 + e * std::cos(tA));
}
