Orbit.hh

Go to the documentation of this file.

/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *\
 * Copyright (c) 2025, Davide Stocco and Enrico Bertolazzi.                  *
 *                                                                           *
 * The Orbit project is distributed under the GNU GPLv3.                     *
 *                                                                           *
 * Davide Stocco                                           Enrico Bertolazzi *
 * University of Trento                                 University of Trento *
 * e-mail: davide.stocco@unitn.it         e-mail: enrico.bertolazzi@unitn.it *
\* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
 
#pragma once
 
#ifndef ASTRO_ORBIT_HH
#define ASTRO_ORBIT_HH
 
#include "Astro/OrbitalElements.hh"
 
namespace Astro
{
 
  /*\
   |    ___       _     _ _
   |   / _ \ _ __| |__ (_) |_
   |  | | | | '__| '_ \| | __|
   |  | |_| | |  | |_) | | |_
   |   \___/|_|  |_.__/|_|\__|
   |
  \*/

  class Orbit
  {
    using Cartesian    = OrbitalElements::Cartesian;
    using Keplerian    = OrbitalElements::Keplerian;
    using Equinoctial  = OrbitalElements::Equinoctial;
    using Quaternionic = OrbitalElements::Quaternionic;
    using Anomaly      = OrbitalElements::Anomaly;
 
  protected:
    Cartesian    m_cart;                      
    Keplerian    m_kepl;                      
    Equinoctial  m_equi;                      
    Quaternionic m_quat;                      
    Type         m_type{Type::UNDEFINED};     
    Factor       m_factor{Factor::UNDEFINED}; 
    Real         m_mu{QUIET_NAN};             
 
  public:
    Orbit() {}

    Orbit(Orbit const &) = default;

    Orbit(Orbit &&) = default;

    Orbit & operator=(const Orbit &) = default;

    Orbit & operator=(Orbit &&) = default;

    Cartesian const & cartesian() const {return this->m_cart;}

    void set_cartesian(Real r_x, Real r_y, Real r_z, Real v_x, Real v_y, Real v_z)
    {
      this->sanity_check();
      this->m_cart.r << r_x, r_y, r_z;
      this->m_cart.v << v_x, v_y, v_z;
      OrbitalElements::cartesian_to_keplerian(this->m_cart, this->m_mu, this->m_kepl);
      OrbitalElements::cartesian_to_equinoctial(this->m_cart, this->m_mu, this->m_equi);
      // TODO: OrbitalElements::cartesian_to_quaternionic(this->m_cart, this->m_quat);
      this->set_type(this->m_kepl.e);
    }

    void set_cartesian(Vector3 const & t_r, Vector3 const & t_v)
    {
      this->set_cartesian(t_r(0), t_r(1), t_r(2), t_v(0), t_v(1), t_v(2));
    }

    void set_cartesian(Vector6 const & t_cart)
    {
      this->set_cartesian(t_cart(0), t_cart(1), t_cart(2), t_cart(3), t_cart(4), t_cart(5));
    }

    void set_cartesian(Cartesian const & t_cart)
    {
      this->set_cartesian(t_cart.r, t_cart.v);
    }

    Keplerian const & keplerian() const {return this->m_kepl;}

    void set_keplerian(Real const t_a, Real const t_e, Real const t_i, Real const t_Omega,
      Real const t_omega, Real const nu)
    {
      this->sanity_check();
      this->m_kepl.a     = t_a;
      this->m_kepl.e     = t_e;
      this->m_kepl.i     = t_i;
      this->m_kepl.Omega = t_Omega;
      this->m_kepl.omega = t_omega;
      OrbitalElements::keplerian_to_cartesian(this->m_kepl, nu, this->m_mu, this->m_cart);
      //OrbitalElements::keplerian_to_equinoctial(this->m_kepl, this->m_factor, this->m_equi);
      OrbitalElements::cartesian_to_equinoctial(this->m_cart, this->m_mu, this->m_equi);
      // TODO: OrbitalElements::keplerian_to_quaternionic(this->m_kepl, this->m_quat);
      this->set_type(this->m_kepl.e);
    }

    void set_keplerian(Vector5 const & t_kepl, Real const nu)
    {
      this->set_keplerian(t_kepl(0), t_kepl(1), t_kepl(2), t_kepl(3), t_kepl(4), nu);
    }

    void set_keplerian(Keplerian const & t_kepl, Real const nu)
    {
      this->set_keplerian(t_kepl.a, t_kepl.e, t_kepl.i, t_kepl.Omega, t_kepl.omega, nu);
    }

    Equinoctial const & equinoctial() const {return this->m_equi;}

    void set_equinoctial(Real const t_p, Real const t_f, Real const t_g, Real const t_h,
      Real const t_k, Real const L)
    {
      this->sanity_check();
      this->m_equi.p = t_p;
      this->m_equi.f = t_f;
      this->m_equi.g = t_g;
      this->m_equi.h = t_h;
      this->m_equi.k = t_k;
      OrbitalElements::equinoctial_to_cartesian(this->m_equi, L, this->m_mu, this->m_cart);
      //OrbitalElements::equinoctial_to_keplerian(this->m_equi, this->m_kepl);
      OrbitalElements::cartesian_to_keplerian(this->m_cart, this->m_mu, this->m_kepl);
      // TODO: OrbitalElements::equinoctial_to_quaternionic(this->m_equi, this->m_quat);
      this->set_type(this->m_kepl.e);
    }

    void set_equinoctial(Vector5 const & t_equi, Real const L)
    {
      this->set_equinoctial(t_equi(0), t_equi(1), t_equi(2), t_equi(3), t_equi(4), L);
    }

    void set_equinoctial(Equinoctial const & t_equi, Real const L)
    {
      this->set_equinoctial(t_equi.p, t_equi.f, t_equi.g, t_equi.h, t_equi.k, L);
    }

    Quaternionic const & quaternionic() const {return this->m_quat;}

    void set_quaternionic(Real const t_q_1, Real const t_q_2, Real const t_q_3, Real const t_q_4)
    {
      this->sanity_check();
      this->m_quat.q.x() = t_q_1;
      this->m_quat.q.y() = t_q_2;
      this->m_quat.q.z() = t_q_3;
      this->m_quat.q.w() = t_q_4;
      // TODO: OrbitalElements::quaternionic_to_keplerian(this->m_quat, this->m_kepl);
      // TODO: OrbitalElements::quaternionic_to_equinoctial(this->m_quat, this->m_factor, this->m_equi);
      // TODO: OrbitalElements::quaternionic_to_cartesian(this->m_quat, anom, this->m_mu, this->m_cart);
      this->set_type(this->m_kepl.e);
    }

    void set_quaternionic(Quaternion const & t_quat)
    {
      this->set_quaternionic(t_quat.x(), t_quat.y(), t_quat.z(), t_quat.w());
    }

    Type type() const {return this->m_type;}

    void set_type(Real const e)
    {
      if (e > EPSILON_MEDIUM && e < 1.0 - EPSILON_MEDIUM) {
        this->m_type = Type::ELLIPTIC;
      } else if (std::abs(e - 1.0) < EPSILON_MEDIUM) {
        this->m_type = Type::PARABOLIC;
      } else if (e > 1.0 + EPSILON_MEDIUM) {
        this->m_type = Type::HYPERBOLIC;
      } else if (std::abs(e) < EPSILON_MEDIUM) {
        this->m_type = Type::CIRCULAR; // Optional: treat e ≈ 0 as circular
      } else {
        ASTRO_ERROR("Astro::Orbit::type(...): invalid eccentricity detected: e = " << e);
      }
    }

    Factor factor() const {return this->m_factor;}

    void set_factor(Factor t_factor) {this->m_factor = t_factor;}

    Real mu() const {return this->m_mu;}

    void set_mu(Real const t_mu) {this->m_mu = t_mu;}

    std::string info() const {
      std::ostringstream os;
      os <<
        "Cartesian orbit parameters:" << std::endl <<
        this->m_cart.info() << std::endl <<
        "Keplerian orbit parameters:" << std::endl <<
        this->m_kepl.info() << std::endl <<
        "Equinoctial orbit parameters:" << std::endl <<
        this->m_equi.info() << std::endl <<
        "Quaternionic orbit parameters:" << std::endl <<
        this->m_quat.info() << std::endl <<
        "Orbit:" << std::endl <<
        "µ: grav. const. = " << this->m_mu << " (UA³/day²)" << std::endl <<
        "type            = " << (this->m_type == Type::UNDEFINED ? "undefined" :
          (this->m_type == Type::ELLIPTIC ? "elliptic" :
          (this->m_type == Type::PARABOLIC ? "parabolic" :
          (this->m_type == Type::HYPERBOLIC ? "hyperbolic" : "undefined")))) << std::endl <<
        "factor          = " << (this->m_factor == Factor::UNDEFINED ? "undefined" :
          (this->m_factor == Factor::POSIGRADE ? "posigrade" :
          (this->m_factor == Factor::RETROGRADE ? "retrograde" : "undefined"))) << std::endl;
        return os.str();
    }

    void info(std::ostream & os) {os << this->info();}

    void reset() {
      this->m_cart.reset();
      this->m_kepl.reset();
      this->m_equi.reset();
      this->m_quat.reset();
      this->m_type   = Type::UNDEFINED;
      this->m_factor = Factor::UNDEFINED;
    }

    bool sanity_check() const
    {
      #define CMD "Astro::Orbit::sanity_check(...): "
 
      if (this->m_factor == Factor::UNDEFINED) {ASTRO_ERROR(CMD "orbit factor is undefined."); return false;}
      if (std::isnan(this->m_mu)) {ASTRO_ERROR(CMD "gravitational constant is not set."); return false;}
      return true;
 
      #undef CMD
    }

    Rotation keplerian_to_reference() const
    {
      Real s_Omega{std::sin(this->m_kepl.Omega)};
      Real c_Omega{std::cos(this->m_kepl.Omega)};
      Real s_omega{std::sin(this->m_kepl.omega)};
      Real c_omega{std::cos(this->m_kepl.omega)};
      Real s_i{std::sin(this->m_kepl.i)};
      Real c_i{std::cos(this->m_kepl.i)};
 
      Rotation R;
      R <<
        c_Omega*c_omega - s_Omega*s_omega*c_i, -c_Omega*s_omega - s_Omega*c_omega*c_i, s_Omega*s_i,
        s_Omega*c_omega + c_Omega*s_omega*c_i, -s_Omega*s_omega + c_Omega*c_omega*c_i, -c_Omega*s_i,
        s_i*s_omega,                           s_i*c_omega,                            c_i;
      return R;
    }

    Rotation equinoctial_to_reference() const
    {
      Real h{this->m_equi.h};
      Real k{this->m_equi.k};
      Real h2{h*h};
      Real k2{k*k};
      Real hk{h*k};
      Real I{static_cast<Real>(this->m_factor)};
 
      Rotation R;
      R <<
        1.0-h2+k2, 2.0*I*hk,      2.0*h,
        2.0*hk,    I*(1.0+h2-k2), -2.0*k,
        -2.0*I*h,  2.0*k,         I*(1.0-h2-k2);
      return R / (1.0+h2+k2);
    }

    Rotation cartesian_to_frenet_rtn(Vector3 const & r, Vector3 const & v) const
    {
      // Check if the input vectors are valid
      ASTRO_ASSERT(r.allFinite() && v.allFinite(),
        "Astro::Orbit::cartesian_to_frenet_rtn(...): invalid input vectors detected.");
 
      // Initialize the Frenet-Serret frame
      Rotation rtn;
 
      // Compute the radial vector
      rtn.col(0) = r;
      rtn.col(0).normalize();
 
      // Compute the binormal vector
      rtn.col(2) = r.cross(v);
      rtn.col(2).normalize();
 
      // Compute the tangential vector
      rtn.col(1) = rtn.col(2).cross(r);
      rtn.col(1).normalize();
 
      // Return the Frenet-Serret frame
      return rtn;
    }

    Rotation equinoctial_to_frenet_rtn(Real const L) const
    {
      Real h{this->m_equi.h};
      Real k{this->m_equi.k};
      Real c_L{std::cos(L)};
      Real s_L{std::sin(L)};
      Real h2{h*h};
      Real k2{k*k};
      Real hk{h*k};
      Real bf{1.0+h2+k2};
      Real I{static_cast<Real>(this->m_factor)};
 
      Rotation R;
      R <<
        ((h2-k2+1)*c_L+2.0*I*hk*s_L)/bf,   ((k2-h2-1.0)*s_L+2.0*hk*I*c_L)/bf, 2.0*k/bf,
        (2.0*hk*c_L-I*(h2-k2-1.0)*s_L)/bf, (I*(1.0+k2-h2)*c_L-2.0*hk*s_L)/bf, -2.0*h/bf,
        (2.0*(h*s_L-c_L*k*I))/bf,          (2.0*k*I*s_L+2.0*h*c_L)/bf,        I*(1-h2-k2)/bf;
      return R;
    }

    Vector3 cartesian_rtn_to_xyz(Vector3 const & r, Vector3 const & v, Vector3 const & vec) const
    {
      return this->cartesian_to_frenet_rtn(r, v) * vec;
    }

    Vector3 equinoctial_rtn_to_xyz(Vector3 const & vec) const
    {
      Real L{0.0}; // FIXME: true longitude is needed here
      return this->equinoctial_to_frenet_rtn(L) * vec;
    }

    Vector3 cartesian_xyz_to_rtn(Vector3 const & r, Vector3 const & v, Vector3 const & vec) const
    {
      return this->cartesian_to_frenet_rtn(r, v).transpose() * vec;
    }

    Vector3 equinoctial_xyz_to_rtn(Vector3 const & vec) const
    {
      Real L{0.0}; // FIXME: true longitude is needed here
      return this->equinoctial_to_frenet_rtn(L).transpose() * vec;
    }

    Vector3 invariant_L() const {
      Real h2{this->m_equi.h * this->m_equi.h};
      Real k2{this->m_equi.k * this->m_equi.k};
      Real bf{std::sqrt(this->m_equi.p * this->m_mu) / (1.0 + h2 + k2)};
      Vector3 L;
      L << 2.0 * this->m_equi.k * bf, -2.0 * this->m_equi.h * bf, (1.0 - (h2 + k2)) * bf;
      if (this->m_factor == Factor::RETROGRADE) {L.z() = -L.z();}
      return L;
    }

    Vector3 invariant_A() const
    {
      Real hk{this->m_equi.h * this->m_equi.k};
      Real h2{this->m_equi.h * this->m_equi.h};
      Real k2{this->m_equi.k * this->m_equi.k};
      Real bf{this->m_mu / (1.0 + h2 + k2)};
      return Vector3(
        bf * ((1.0 + h2 - k2) * this->m_equi.f + 2.0 * hk * this->m_equi.g),
        bf * ((1.0 - h2 + k2) * this->m_equi.g + 2.0 * hk * this->m_equi.f),
        bf * (2.0 * (this->m_equi.h * this->m_equi.g - this->m_equi.k * this->m_equi.f))
      );
    }

    Real energy() const {
      return this->m_mu * (this->m_equi.g * this->m_equi.g + this->m_equi.f * this->m_equi.f - 1.0) /
        (2.0 * this->m_equi.p);
    }

    Real period() const {
 
      #define CMD "Astro::Orbit::period(...): "
 
      ASTRO_ASSERT(this->m_type == Type::ELLIPTIC || this->m_type == Type::CIRCULAR,
        CMD "the orbit period is defined only for elliptic and circular orbits.");
 
        return 2.0 * M_PI * std::sqrt(Power3(this->m_kepl.a) / this->m_mu);
 
      #undef CMD
    }

    Real compute_lambda(Real t, Anomaly const & epoch_anom) const
    {
      Real n{std::sqrt(this->m_mu / Power3(this->m_kepl.a))};
      Real M{epoch_anom.M + n * t}; // Assuming t is time since epoch
 
      Real nu{0.0};
      if (this->m_type == Type::ELLIPTIC || this->m_type == Type::CIRCULAR) {
        // Elliptic case: solve Kepler's equation for E
        Real E{OrbitalElements::M_to_E(M, this->m_kepl)};
        nu = OrbitalElements::E_to_nu(E, this->m_kepl);
      } else {
        // Hyperbolic case: solve Kepler's equation for H
        Real H{OrbitalElements::H_to_M(M, this->m_kepl)};
        nu = OrbitalElements::H_to_nu(H, this->m_kepl);
      }
 
      // Compute mean longitude lambda = nu + omega + Omega (retrograde sign handled by factor)
      Real lambda{nu + this->m_kepl.omega};
      if (this->m_factor == Factor::RETROGRADE) {lambda -= this->m_kepl.Omega;}
      else {lambda += this->m_kepl.Omega;}
 
      return lambda;
    }

    Real compute_lambda(Real t, Real dt, Anomaly const & epoch_anom) const
    {
      if (this->m_kepl.e < 1.0) {
        Real L0{this->compute_lambda(t, epoch_anom)};
        Real L1{this->compute_lambda(t + dt, epoch_anom)};
        Real dL{L1 - L0};
        Real pf{dt / this->period()}; // Fraction of period
 
        // Wrap angle for positive/negative dt
        if (dt > 0) {
          while (dL < 0.0) {dL += 2.0 * M_PI;}
          while (pf > 1.0) {dL += 2.0 * M_PI; pf -= 1.0;}
        } else {
          while (dL > 0.0) {dL -= 2.0 * M_PI;}
          while (pf < -1.0) {dL -= 2.0 * M_PI; pf += 1.0;}
        }
        return L0 + dL;
        } else {
          // For hyperbolic orbits, no wrapping needed
          return this->compute_lambda(t + dt, epoch_anom);
      }
    }
 
  }; // class Orbit
 
} // namespace Astro
 
#endif // ASTRO_ORBIT_HH