OrbitalElements.hh

Go to the documentation of this file.

/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *\
 * Copyright (c) 2025, Davide Stocco and Enrico Bertolazzi.                  *
 *                                                                           *
 * The Astro 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_ORBITALELEMENTS_HH
#define ASTRO_ORBITALELEMENTS_HH
 
#include "Astro.hh"
#include "Astro/Utilities.hh"
 
namespace Astro
{
  using Factor = enum class Factor : Integer {
    POSIGRADE = 1, UNDEFINED = 0, RETROGRADE = -1
  };

  using Type = enum class Type : Integer {
    UNDEFINED = 0, HYPERBOLIC = 1, ELLIPTIC = 2, CIRCULAR = 3, PARABOLIC = 4
  };

  namespace OrbitalElements
  {
 
    /*\
     |     ____           _            _
     |    / ___|__ _ _ __| |_ ___  ___(_) __ _ _ __
     |   | |   / _` | '__| __/ _ \/ __| |/ _` | '_ \
     |   | |__| (_| | |  | ||  __/\__ \ | (_| | | | |
     |    \____\__,_|_|   \__\___||___/_|\__,_|_| |_|
     |
    \*/

    struct Cartesian
    {
      Vector3 r{NAN_VEC3}; 
      Vector3 v{NAN_VEC3}; 

      Cartesian() {}

      Cartesian(Vector3 const &t_r, Vector3 const &t_v) : r(t_r), v(t_v) {}

      Cartesian(Vector6 const &t_cart) : r(t_cart.head<3>()), v(t_cart.tail<3>()) {}

      Cartesian(Real r_x, Real r_y, Real r_z, Real v_x, Real v_y, Real v_z) :
        r(r_x, r_y, r_z), v(v_x, v_y, v_z) {}

      Cartesian(Cartesian const &) = default;

      Cartesian(Cartesian &&) = default;

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

      Cartesian & operator=(Cartesian &&) = default;

      Vector6 vector() const {
        Vector6 vec;
        vec << this->r, this->v;
        return vec;
      }

      std::string info() const {
        std::ostringstream os;
        os <<
          "r = " << this->r.transpose() << " (UA)" << std::endl <<
          "v = " << this->v.transpose() << " (UA/day)" << std::endl;
          return os.str();
      }

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

      void reset() {
        this->r = NAN_VEC3;
        this->v = NAN_VEC3;
      }

      bool sanity_check() const {
        #define CMD "Astro::OrbitalElements::Cartesian::sanity_check(...): "
 
        if (!(this->r.allFinite())) {
          ASTRO_WARNING(CMD "invalid position vector detected." << r.transpose());
          return false;
        }
        if (!(this->v.allFinite())) {
          ASTRO_WARNING(CMD "invalid velocity vector detected.");
          return false;
        }
        return true;
 
        #undef CMD
      }

      Vector3 h() const {return this->r.cross(this->v);}

      Real e(Real const mu) const
      {
        return ((this->v.cross(this->h()) / mu) - (this->r / this->r.norm())).norm();
      }

      Vector3 spherical_coordinates() const
      {
        Real r = this->r.norm();
        Real theta = std::acos(this->r.z() / r); // polar angle (colatitude)
        Real phi = std::atan2(this->r.y(), this->r.x()); // azimuthal angle
        return Vector3(r, theta, phi);
      }
 
 
    }; // struct Cartesian
 
    /*\
     |    _  __          _           _
     |   | |/ /___ _ __ | | ___ _ __(_) __ _ _ __
     |   | ' // _ \ '_ \| |/ _ \ '__| |/ _` | '_ \
     |   | . \  __/ |_) | |  __/ |  | | (_| | | | |
     |   |_|\_\___| .__/|_|\___|_|  |_|\__,_|_| |_|
     |            |_|
    \*/

    struct Keplerian
    {
      Real a{QUIET_NAN};     
      Real e{QUIET_NAN};     
      Real i{QUIET_NAN};     
      Real Omega{QUIET_NAN}; 
      Real omega{QUIET_NAN}; 

      Keplerian() {}

      Keplerian(Real const t_a, Real const t_e, Real const t_i, Real const t_Omega, Real const t_omega)
        : a(t_a), e(t_e), i(t_i), Omega(t_Omega), omega(t_omega)
      {}

      Keplerian(Vector5 const & t_kepl)
        : a(t_kepl(0)), e(t_kepl(1)), i(t_kepl(2)), Omega(t_kepl(3)), omega(t_kepl(4))
      {}

      Keplerian(Keplerian const &) = default;

      Keplerian(Keplerian &&) = default;

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

      Keplerian & operator=(Keplerian &&) = default;

      Vector5 vector () const
      {
        Vector5 vec;
        vec << this->a, this->e, this->i, this->Omega, this->omega;
        return vec;
      }

      std::string info() const {
        std::ostringstream os;
        os <<
          "a : semi-major axis   = " << this->a << " (UA)" << std::endl <<
          "e : eccentricity      = " << this->e << " (-)" << std::endl <<
          "i : inclination       = " << this->i << " (rad) = " << Rad_To_Deg(this->i) << " (deg)" << std::endl <<
          "Ω : right ascension … = " << this->Omega << " (rad) = " << Rad_To_Deg(this->Omega) << " (deg)" << std::endl <<
          "ω : arg. of periapsis = " << this->omega << " (rad) = " << Rad_To_Deg(this->omega) << " (deg)" << std::endl;
          return os.str();
      }

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

      void reset() {
        this->a = QUIET_NAN;
        this->e = QUIET_NAN;
        this->i = QUIET_NAN;
        this->Omega = QUIET_NAN;
        this->omega = QUIET_NAN;
      }

      bool sanity_check() const
      {
        #define CMD "Astro::OrbitalElements::Keplerian::sanity_check(...): "
 
        if (!std::isfinite(this->a) && this->a > 0.0) {
          ASTRO_WARNING(CMD "invalid semi-major axis, a = " << this->a << ".");
          return false;
        }
        if (!std::isfinite(this->e) && this->e > 0.0 && this->e < 1.0) {
          ASTRO_WARNING(CMD "invalid eccentricity, e = " << this->e << ".");
          return false;
        }
        if (!std::isfinite(this->i)) {
          ASTRO_WARNING(CMD "invalid inclination, i = " << this->i << ".");
          return false;
        }
        if (!std::isfinite(this->Omega)) {
          ASTRO_WARNING(CMD "invalid right ascension of the ascending node, Ω = " << this->Omega << ".");
          return false;
        }
        if (!std::isfinite(this->omega)) {
          ASTRO_WARNING(CMD "invalid argument of periapsis, ω = " << this->omega << ".");
          return false;
        }
        ASTRO_ASSERT_WARNING(this->is_nonsingular(), CMD "singular orbit detected.");
        return true;
 
        #undef CMD
      }

      bool is_singular(Real tol_i = EPSILON_LOW, Real tol_e = EPSILON_LOW) const
      {
        #define CMD "Astro::OrbitalElements::Keplerian::is_singular(...): "
 
        Real i{AngleInRange(this->i)};
        if (std::abs(i) < tol_i && std::abs(i - PI) < tol_i) {
          ASTRO_WARNING(CMD "singular inclination detected.");
          return true;
        }
        if (std::abs(this->e) < tol_e || std::abs(this->e - 1.0) < tol_e) {
          ASTRO_WARNING(CMD "singular eccentricity detected.");
          return true;
        }
        return false;
 
        #undef CMD
      }

      bool is_nonsingular(Real tol_i = EPSILON_LOW, Real tol_e = EPSILON_LOW) const
      {
        return !this->is_singular(tol_i, tol_e);
      }

      Real u() const {return this->omega + this->Omega;}

      Real p() const {return this->a * (1.0 - this->e * this->e);}
 
    }; // struct Keplerian
 
    /*\
     |   _____            _                  _   _       _
     |  | ____|__ _ _   _(_)_ __   ___   ___| |_(_) __ _| |
     |  |  _| / _` | | | | | '_ \ / _ \ / __| __| |/ _` | |
     |  | |__| (_| | |_| | | | | | (_) | (__| |_| | (_| | |
     |  |_____\__, |\__,_|_|_| |_|\___/ \___|\__|_|\__,_|_|
     |           |_|
    \*/

    struct Equinoctial
    {
      Real p{QUIET_NAN}; 
      Real f{QUIET_NAN}; 
      Real g{QUIET_NAN}; 
      Real h{QUIET_NAN}; 
      Real k{QUIET_NAN}; 
 
    public:
      Equinoctial() {}

      Equinoctial(Real const t_p, Real const t_f, Real const t_g, Real const t_h, Real const t_k)
        : p(t_p), f(t_f), g(t_g), h(t_h), k(t_k) {}

      Equinoctial(Vector5 const & t_equi)
        : p(t_equi(0)), f(t_equi(1)), g(t_equi(2)), h(t_equi(3)), k(t_equi(4))
      {}

      Equinoctial(Equinoctial const &) = default;

      Equinoctial(Equinoctial &&) = default;

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

      Equinoctial & operator=(Equinoctial &&) = default;

      Vector5 vector() const {
        Vector5 vec;
        vec << this->p, this->f, this->g, this->h, this->k;
        return vec;
      }

      std::string info() const {
        std::ostringstream os;
        os <<
          "p : semi-latus rectum  = " << this->p << " (UA)" << std::endl <<
          "f : x-axis ecc. vector = " << this->f << " (-)" << std::endl <<
          "g : y-axis ecc. vector = " << this->g << " (-)" << std::endl <<
          "h : x-axis node vector = " << this->h << " (-)" << std::endl <<
          "k : y-axis node vector = " << this->k << " (-)" << std::endl;
          return os.str();
      }

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

      void reset() {
        this->p = QUIET_NAN;
        this->f = QUIET_NAN;
        this->g = QUIET_NAN;
        this->h = QUIET_NAN;
        this->k = QUIET_NAN;
      }

      bool sanity_check() const
      {
        #define CMD "Astro::OrbitalElements::Keplerian::sanity_check(...): "
 
        if (!std::isfinite(this->p) && this->p > 0.0) {
          ASTRO_WARNING(CMD "invalid semi-latus rectum, p = " << this->p << ".");
          return false;
        }
        if (!std::isfinite(this->f)) {
          ASTRO_WARNING(CMD "invalid x-axis component of the eccentricity vector, f = " << this->f << ".");
          return false;
        }
        if (!std::isfinite(this->g)) {
          ASTRO_WARNING(CMD "invalid y-axis component of the eccentricity vector, g = " << this->g << ".");
          return false;
        }
        if (!std::isfinite(this->h)) {
          ASTRO_WARNING(CMD "invalid x-axis component of the node vector, h = " << this->h << ".");
          return false;
        }
        if (!std::isfinite(this->k)) {
          ASTRO_WARNING(CMD "invalid y-axis component of the node vector, k = " << this->k << ".");
          return false;
        }
 
        // Check if it is singular
        ASTRO_ASSERT_WARNING(this->is_nonsingular(), CMD "singular orbit detected.");
 
        return true;
 
        #undef CMD
      }

      bool is_singular(Real tol_i = EPSILON_LOW) const
      {
        #define CMD "Astro::OrbitalElements::Keplerian::is_singular(...): "
 
        Real i{AngleInRange(this->i())};
        if (i < PI + tol_i && i > PI - tol_i) {
          ASTRO_WARNING(CMD "singular inclination detected.");
          return true;
        }
        return false;
 
        #undef CMD
      }

      bool is_nonsingular(Real tol_i = EPSILON_LOW) const
      {
        return !this->is_singular(tol_i);
      }

      Real u(Real L) const
      {
        Real s_L{std::sin(L)}, c_L{std::cos(L)};
        return std::atan2(this->h*s_L - this->k*c_L, this->h*c_L + this->k*s_L);
      }

      Real a() const {return this->p / (1.0 - this->f*this->f - this->g*this->g);}

      Real e() const {return std::sqrt(this->f*this->f + this->g*this->g);}

      Real i() const
      {
        Real h2{this->h * this->h}, k2{this->k * this->k};
        return std::atan2(2.0*std::sqrt(h2 + k2), 1.0 - h2 - k2);
      }

      Real omega() const
      {
        return std::atan2(
          this->g*this->h - this->f*this->k,
          this->f*this->h + this->g*this->k
        );
      }

      Real Omega() const {return std::atan2(this->k, this->h);}
 
    }; // struct Equinoctial
 
 
    /*\
     |    ___              _                  _             _
     |   / _ \ _   _  __ _| |_ ___ _ __ _ __ (_) ___  _ __ (_) ___
     |  | | | | | | |/ _` | __/ _ \ '__| '_ \| |/ _ \| '_ \| |/ __|
     |  | |_| | |_| | (_| | ||  __/ |  | | | | | (_) | | | | | (__
     |   \__\_\\__,_|\__,_|\__\___|_|  |_| |_|_|\___/|_| |_|_|\___|
     |
    \*/

    struct Quaternionic
    {
      Quaternion q{NAN_VEC4}; 

      Quaternionic() {}

      Quaternionic(Quaternion const &t_q) : q(t_q) {}

      Quaternionic(Real const t_q_1, Real const t_q_2, Real const t_q_3, Real const t_q_4)
        : q(t_q_1, t_q_2, t_q_3, t_q_4) {}

      Quaternionic(Quaternionic const &) = default;

      Quaternionic(Quaternionic &&) = default;

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

      Quaternionic & operator=(Quaternionic &&) = default;

      Vector4 vector() const {
        Vector4 vec;
        vec << this->q.x(), this->q.y(), this->q.z(), this->q.w();
        return vec;
      }

      Rotation rotation() const {
        Rotation r;
        r << 1.0 - 2.0*(q.y()*q.y() + q.z()*q.z()),
             2.0*(q.x()*q.y() - q.z()*q.w()),
             2.0*(q.x()*q.z() + q.y()*q.w()),
             2.0*(q.x()*q.y() + q.z()*q.w()),
             1.0 - 2.0*(q.x()*q.x() + q.z()*q.z()),
             2.0*(q.y()*q.z() - q.x()*q.w()),
             2.0*(q.x()*q.z() - q.y()*q.w()),
             2.0*(q.y()*q.z() + q.x()*q.w()),
             1.0 - 2.0*(q.x()*q.x() + q.y()*q.y());
        return r;
      }

      std::string info() const {
        std::ostringstream os;
        os <<
          "q¹ : 1st parameter = " << this->q.x() << " (-)" << std::endl <<
          "q² : 2nd parameter = " << this->q.y() << " (-)" << std::endl <<
          "q³ : 3rd parameter = " << this->q.z() << " (-)" << std::endl <<
          "q⁴ : 4th parameter = " << this->q.w() << " (-)" << std::endl;
          return os.str();
      }

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

      void reset() {this->q = NAN_VEC4;}

      bool sanity_check() const
      {
        #define CMD "Astro::OrbitalElements::Quaternionic::sanity_check(...): "
 
        if (!(std::isfinite(this->q.x()) && std::isfinite(this->q.y()) &&
              std::isfinite(this->q.z()) && std::isfinite(this->q.w()))) {
          ASTRO_WARNING(CMD "invalid quaternionic orbital elements.");
          return false;
        }
        return true;
 
        #undef CMD
      }
 
    }; // struct Quaternionic

    Real nu_to_M(Real const nu, Real const e)
    {
      Real beta{(1.0 + std::sqrt(1.0 - e*e)) / e};
      return AngleInRange(nu - 2.0 * std::atan(std::sin(nu) / (beta + std::cos(nu))) -
        (e * std::sqrt(1.0 - e*e) * std::sin(nu)) / (1.0 + e * std::cos(nu)));
    }

    Real nu_to_M(Real const nu, Keplerian const & kepl) {return nu_to_M(nu, kepl.e);}

    Real nu_to_E(Real const nu, Keplerian const & kepl)
    {
      Real beta{(1.0 + std::sqrt(1.0 - kepl.e*kepl.e)) / kepl.e};
      return AngleInRange(nu - 2.0 * std::atan(std::sin(nu) / (beta + std::cos(nu))));
    }

    Real nu_to_L(Real const nu, Keplerian const & kepl, Factor const I)
    {
      return AngleInRange(nu + kepl.omega + kepl.Omega*static_cast<Real>(I));
    }

    Real M_to_E(Real M, Real const e)
    {
      #define CMD "Astro::OrbitalElements::Anomaly::M_to_E(...): "
 
      // Clamp the mean anomaly in the range [0, 2\pi]
      M = AngleInRange(M);
 
      // Solve Kepler equation through a basic Newton method
      Real dE{0.0}, E{M};
      for (Integer k{0}; k < 100; ++k)
      {
        dE = (E - e * std::sin(E) - M) / (1.0 - e * std::cos(E));
        // Saturate if steps are too largex
        E -= std::max(std::min(dE, 0.1), -0.1);
        // Break if the error is small enough
        if (std::abs(dE) < EPSILON_HIGH || std::abs(E) < EPSILON_HIGH) {break;}
      }
 
      ASTRO_ASSERT(std::abs(dE) < EPSILON_HIGH, CMD "convergence not reached: E = " << E <<
        ", dE = " << dE << ", M = " << M << ", e = " << e << ".");
 
      return AngleInRange(E);
 
      #undef CMD
    }

    Real M_to_E(Real M, Keplerian const & kepl)
    {
      return M_to_E(M, kepl.e);
    }

    Real M_to_H(Real const M, Keplerian const & kepl)
    {
 
      #define CMD "Astro::OrbitalElements::Anomaly::M_to_H(...): "
 
      // Solve Kepler equation through a basic Newton method
      Real e{kepl.e}, abs_M{M > 0 ? M : -M};
      Real dH{0.0}, H{5.0*e-2.5 > abs_M ? std::pow(6.0*abs_M/e, 1.0/3.0) : std::log(2.0*abs_M/e)};
      for (Integer k{0}; k < 100; ++k)
      {
        dH = (e * std::sinh(H) - H - abs_M) / (e * std::cosh(H) - 1.0);
        H -= dH;
        // Break if the error is small enough
        if (std::abs(dH) < EPSILON_MEDIUM || std::abs(H) < EPSILON_MEDIUM) {break;}
      }
 
      ASTRO_ASSERT(std::abs(dH) < EPSILON_MEDIUM, CMD "convergence not reached: H = " << H <<
        ", dH = " << dH << ", M = " << M << ", e = " << e << ".");
 
      return AngleInRange(H > 0 ? H : -H); // Return the positive value
 
      #undef CMD
    }

    Real M_to_lambda(Real const M, Keplerian const & kepl, Factor const I)
    {
      return AngleInRange(M + kepl.omega + kepl.Omega*static_cast<Real>(I));
    }

    Real E_to_nu(Real const E, Real const e)
    {
      Real beta{(1.0 + std::sqrt(1.0 - e*e)) / e};
      return AngleInRange(E + 2.0 * std::atan(std::sin(E) / (beta - std::cos(E))));
    }

    Real E_to_nu(Real const E, Keplerian const & kepl)
    {
      return E_to_nu(E, kepl.e);
    }

    Real E_to_M(Real const E, Real const e)
    {
      return AngleInRange(E - e * std::sin(E));
    }

    Real E_to_M(Real const E, Keplerian const & kepl)
    {
      return E_to_M(E, kepl.e);
    }

    Real H_to_nu(Real const H, Keplerian const & kepl)
    {
      return AngleInRange(2.0*std::atan(std::sqrt((kepl.e+1.0)/(kepl.e-1.0)) * std::tanh(H/2.0)));
    }

    Real H_to_M(Real const H, Keplerian const & kepl)
    {
      return AngleInRange(kepl.e * std::sinh(H) - H);
    }

    Real L_to_nu(Real const L, Keplerian const & kepl, Factor const I)
    {
      return AngleInRange(L - kepl.omega - kepl.Omega*static_cast<Real>(I));
    }

    Real L_to_lambda(Real const L, Real const nu, Real const M)
    {
      return AngleInRange(L - nu + M);
    }

    Real lambda_to_M(Real const lambda, Keplerian const & kepl, Factor const I)
    {
      return AngleInRange(lambda - kepl.omega - static_cast<Real>(I)*kepl.Omega);
    }

    Real lambda_to_L(Real const lambda, Real const nu, Real const M)
    {
      return AngleInRange(lambda + nu - M);
    }
 
    /*\
     |      _                                _
     |     / \   _ __   ___  _ __ ___   __ _| |_   _
     |    / _ \ | '_ \ / _ \| '_ ` _ \ / _` | | | | |
     |   / ___ \| | | | (_) | | | | | | (_| | | |_| |
     |  /_/   \_\_| |_|\___/|_| |_| |_|\__,_|_|\__, |
     |                                         |___/
    \*/

    struct Anomaly
    {
      Real nu{0.0};     
      Real M{0.0};      
      Real E{0.0};      
      Real H{0.0};      
      Real L{0.0};      
      Real lambda{0.0}; 

      Anomaly() {}

      Anomaly(Anomaly const &) = default;

      Anomaly(Anomaly &&) = default;

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

      Anomaly & operator=(Anomaly &&) = default;

      template<bool Hyperbolic = false>
      void set_nu(Real t_nu, Keplerian const & kepl, Factor const I)
      {
        t_nu = AngleInRange(t_nu);
        kepl.sanity_check();
        this->nu     = t_nu;
        this->M      = nu_to_M(this->nu, kepl);
        this->E      = nu_to_E(this->nu, kepl);
        this->L      = nu_to_L(this->nu, kepl, I);
        this->lambda = M_to_lambda(this->M, kepl, I);
        this->H      = QUIET_NAN;
        if constexpr (Hyperbolic) {this->H = M_to_H(this->M, kepl);}
      }
 

      template<bool Hyperbolic = false>
      void set_M(Real t_M, Keplerian const & kepl, Factor const I)
      {
        t_M = AngleInRange(t_M);
        kepl.sanity_check();
        this->E      = M_to_E(t_M, kepl);
        this->nu     = E_to_nu(this->E, kepl);
        this->M      = t_M;
        this->L      = nu_to_L(this->nu, kepl, I);
        this->lambda = M_to_lambda(this->M, kepl, I);
        this->H      = QUIET_NAN;
        if constexpr (Hyperbolic) {this->H = M_to_H(this->M, kepl);}
      }

      template<bool Hyperbolic = false>
      void set_E(Real t_E, Keplerian const & kepl, Factor const I)
      {
        t_E = AngleInRange(t_E);
        kepl.sanity_check();
        this->nu     = E_to_nu(t_E, kepl);
        this->M      = E_to_M(t_E, kepl);
        this->E      = t_E;
        this->L      = nu_to_L(this->nu, kepl, I);
        this->lambda = M_to_lambda(this->M, kepl, I);
        this->H      = QUIET_NAN;
        if constexpr (Hyperbolic) {this->H = M_to_H(this->M, kepl);}
      }

      template<bool Hyperbolic = false>
      void set_L(Real t_L, Keplerian const & kepl, Factor const I)
      {
        #define CMD "Astro::OrbitalElements::Anomaly::L(...): "
 
        t_L = AngleInRange(t_L);
        this->set_nu<Hyperbolic>(L_to_nu(t_L, kepl, I) , kepl, I);
 
        ASTRO_ASSERT(std::abs(this->L - t_L) < EPSILON_MEDIUM || std::abs(this->L - t_L - PIMUL2) < EPSILON_MEDIUM,
          CMD "conversion error, L = " << t_L << " ≠ " << this->L << ".");
 
        #undef CMD
      }

      template<bool Hyperbolic = false>
      void set_lambda(Real t_lambda, Keplerian const & kepl, Factor const I)
      {
        #define CMD "Astro::OrbitalElements::Anomaly::lambda(...): "
 
        t_lambda = AngleInRange(t_lambda);
        this->set_M<Hyperbolic>(lambda_to_M(t_lambda, kepl, I), kepl, I);
 
        ASTRO_ASSERT(std::abs(this->lambda - t_lambda) < EPSILON_HIGH,
          CMD "conversion error, lambda = " << t_lambda << " ≠ " << this->lambda << ".");
 
        #undef CMD
      }

      void set_H(Real const t_H, Keplerian const & kepl, Factor const I)
      {
        #define CMD "Astro::OrbitalElements::Anomaly::H(...): "
 
        this->set_nu<true>(H_to_nu(t_H, kepl), kepl, I);
 
        ASTRO_ASSERT(std::abs(this->H - t_H) < EPSILON_HIGH,
          CMD "conversion error, H = " << t_H << " ≠ " << this->H << ".");
 
        #undef CMD
      }

      std::string info() const {
        std::ostringstream os;
        os <<
          "v : true anomaly       = " << this->nu << " (rad) = " << Rad_To_Deg(this->nu) << " (deg)" << std::endl <<
          "M : mean anomaly       = " << this->M << " (rad) = " << Rad_To_Deg(this->M) << " (deg)" << std::endl <<
          "E : eccentric anomaly  = " << this->E << " (rad) = " << Rad_To_Deg(this->E) << " (deg)" << std::endl <<
          "L : true longitude     = " << this->L << " (rad) = " << Rad_To_Deg(this->L) << " (deg)" << std::endl <<
          "λ : mean longitude     = " << this->lambda << " (rad) = " << Rad_To_Deg(this->lambda) << " (deg)" << std::endl <<
          "H : hyperbolic anomaly = " << this->H << " (rad) = " << Rad_To_Deg(this->H) << " (deg)" << std::endl;
          return os.str();
      }

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

      void reset()
      {
        this->nu     = QUIET_NAN;
        this->M      = QUIET_NAN;
        this->E      = QUIET_NAN;
        this->L      = QUIET_NAN;
        this->lambda = QUIET_NAN;
        this->H      = QUIET_NAN;
      }

      template<bool Hyperbolic = false>
      bool sanity_check() const
      {
        #define CMD "Astro::OrbitalElements::Anomaly::sanity_check(...): "
 
        if (!(std::isfinite(this->nu))) {
          ASTRO_ERROR(CMD "invalid true anomaly.");
          return false;
        }
        if (!(std::isfinite(this->M))) {
          ASTRO_ERROR(CMD "invalid mean anomaly.");
          return false;
        }
        if (!(std::isfinite(this->E))) {
          ASTRO_ERROR(CMD "invalid eccentric anomaly.");
          return false;
        }
        if (!(std::isfinite(this->L))) {
          ASTRO_ERROR(CMD "invalid true longitude.");
          return false;
        }
        if (!(std::isfinite(this->lambda))) {
          ASTRO_ERROR(CMD "invalid mean longitude.");
          return false;
        }
        if constexpr (Hyperbolic) {
          if (!(std::isfinite(this->H))) {
            ASTRO_ERROR(CMD "invalid hyperbolic anomaly.");
            return false;
          }
        }
        return true;
 
        #undef CMD
      }
 
    }; // struct Anomaly

    Real cartesian_to_keplerian(Cartesian const & cart, Real const mu, Keplerian & kepl)
    {
      #define CMD "Astro::OrbitalElements::cartesian_to_keplerian(...): "
 
      // Reset the keplerian orbital elements
      kepl.reset();
 
      // Retrieve the position and velocity vectors
      Vector3 const & r{cart.r};
      Vector3 const & v{cart.v};
      Real r_norm{r.norm()};
 
      // Compute the orbital momentum vector
      Vector3 h_vec{cart.h()};
      Real r_dot_v{r.dot(v)};
 
      // Compute the eccentricity
      Vector3 e_vec{1.0/mu*((v.squaredNorm() - mu/r_norm)*r - r_dot_v*v)};
      Real e{e_vec.norm()};
 
      // Determine the vector pointing towards the ascending node
      Vector3 n_vec{(Vector3::UnitZ().cross(h_vec)).normalized()};
 
      // Compute the inclination
      Real i{std::acos(h_vec.z() / h_vec.norm())};
 
      // Set the node vector to the x-axis if the orbit is circular
      if (std::abs(i) < EPSILON_LOW || std::abs(i - PI) < EPSILON_LOW) {n_vec << 1.0, 0.0, 0.0;}
 
      // Compute the longitude of the ascending node
      Real Omega{std::acos(n_vec.x())};
      if (n_vec.y() < 0.0) {Omega = 2.0*PI - Omega;}
 
      // Compute the argument of the periapsis
      Real omega{std::acos(n_vec.dot(e_vec)/e)};
      if (e_vec.z() < 0.0) {omega = 2.0*PI - omega;}
 
      // Compute the true anomaly
      Real nu{std::acos(e_vec.dot(r) / (e * r_norm))};
      if (r_dot_v < 0.0) {nu = 2.0*PI - nu;}
 
      if (std::abs(e) < EPSILON_LOW) {omega = nu = 0.0;}
 
      // Compute the semi-major axis
      Real a{1.0 / (2.0/r_norm - v.squaredNorm()/mu)};
 
      // Assign the keplerian orbital elements
      kepl.a     = a;
      kepl.e     = e;
      kepl.i     = i;
      kepl.Omega = Omega;
      kepl.omega = omega;
 
      ASTRO_ASSERT(kepl.sanity_check(), CMD "conversion error.");
 
      return nu;
 
      #undef CMD
    }

    void keplerian_to_cartesian(Keplerian const & kepl, Real const nu, Real const mu, Cartesian & cart)
    {
      #define CMD "Astro::OrbitalElements::keplerian_to_cartesian(...): "
 
      // Reset the cartesian state vectors
      cart.reset();
 
      // Retrieve some keplerian orbital elements
      Real const & e{kepl.e};
      Real const & i{kepl.i};
      Real const & Omega{kepl.Omega};
      Real const & omega{kepl.omega};
 
      // Compute the semi-latus rectum
      Real p{kepl.p()};
 
      // Compute the distance to the central body
      Real r{p/(1.0 + e*std::cos(nu))};
 
      // Position and velocity vectors in the perifocal coordinate system
      Vector3 r_vec(r*std::cos(nu), r*std::sin(nu), 0.0);
      Vector3 v_vec(-std::sin(nu), e + std::cos(nu), 0.0);
      v_vec *= std::sqrt(mu/p);
 
      // Rotation matrices for the transformation from the perifocal to the inertial frame
      Matrix3 R_z, R_i, R_h, R;
      R_z <<  std::cos(Omega), std::sin(Omega), 0.0,
             -std::sin(Omega), std::cos(Omega), 0.0,
             0.0, 0.0, 1.0;
      R_i << 1.0, 0.0, 0.0,
             0.0, std::cos(i), std::sin(i),
             0.0, -std::sin(i), std::cos(i);
      R_h <<  std::cos(omega), std::sin(omega), 0.0,
             -std::sin(omega), std::cos(omega), 0.0,
             0.0, 0.0, 1.0;
      R = (R_h*R_i*R_z).transpose(); // Transpose to convert from column-major to row-major
 
      // Obtain the position and velocity vector r_of and v_of in the orbital frame
      cart.r = R*r_vec;
      cart.v = R*v_vec;
 
      ASTRO_ASSERT(cart.sanity_check(), CMD "conversion error.");
 
      #undef CMD
    }

    void equinoctial_to_cartesian(Equinoctial const & equi, Real const L, Real const mu,
      Cartesian & cart)
    {
      #define CMD "Astro::OrbitalElements::equinoctial_to_cartesian(...): "
 
      // Reset the cartesian state vectors
      cart.reset();
 
      // Retrieve the equinoctial orbital elements
      Real const & f{equi.f};
      Real const & g{equi.g};
      Real const & h{equi.h};
      Real const & k{equi.k};
      Real const & p{equi.p};
 
      // Compute the distance to the central body
      Real c_L{std::cos(L)}, s_L{std::sin(L)};
      Real alpha_2{h*h - k*k};
      Real s_2{1.0 + h*h + k*k};
      Real w{1.0 + f*c_L + g*s_L};
      Real r_c{p / w};
 
      // Compute the position and velocity vectors in the inertial frame
      Real tmp{r_c/s_2};
      cart.r <<
        tmp * (c_L + alpha_2*c_L + 2.0*h*k*s_L),
        tmp * (s_L - alpha_2*s_L + 2.0*h*k*c_L),
        2.0*tmp * (h*s_L - k*c_L);
 
      tmp = -1.0/s_2 * std::sqrt(mu/p);
      cart.v <<
        tmp * ( s_L + alpha_2*s_L - 2.0*h*k*c_L + g - 2.0*f*h*k + alpha_2*g),
        tmp * (-c_L + alpha_2*c_L + 2.0*h*k*s_L - f + 2.0*g*h*k - alpha_2*f),
        2.0*tmp * (-h*c_L - k*s_L + f*h - g*k);
 
      ASTRO_ASSERT(cart.sanity_check(), CMD "conversion error.");
 
      #undef CMD
    }

    void keplerian_to_equinoctial(Keplerian const & kepl, Factor const I, Equinoctial & equi)
    {
      #define CMD "Astro::OrbitalElements::keplerian_to_equinoctial(...): "
 
      // Reset the equinoctial orbital elements
      equi.reset();
 
      // Retrieve the keplerian orbital elements
      Real const & e{kepl.e};
      Real const & i{kepl.i};
      Real const & Omega{kepl.Omega};
      Real const & omega{kepl.omega};
 
      // Compute the semi-latus rectum
      equi.p = kepl.p();
 
      // Compute the x-axis component of the eccentricity vector in the orbital frame f
      Real omega_plus_Omega{omega + static_cast<Real>(I)*Omega};
      equi.f = e*std::cos(omega_plus_Omega);
 
      // Compute the y-axis component of the eccentricity vector in the orbital frame g
      equi.g = e*std::sin(omega_plus_Omega);
 
      // Compute the x- and y-axis components of the node vector in the orbital frame h
      Real tmp{std::tan(i/2.0)};
      if (I == Factor::RETROGRADE) {tmp = 1.0/tmp;} // 1/tan(x) = cot(x)
      equi.h = tmp*std::cos(Omega);
      equi.k = tmp*std::sin(Omega);
 
      ASTRO_ASSERT(equi.sanity_check(), CMD "conversion error.");
 
      #undef CMD
    }

    Real cartesian_to_equinoctial(Cartesian const & cart, Real const mu, Equinoctial & equi)
    {
      #define CMD "Astro::OrbitalElements::cartesian_to_equinoctial(...): "
 
      // Reset the equinoctial orbital elements
      equi.reset();
 
      // Retrieve the cartesian state vectors
      Vector3 const & r{cart.r};
      Vector3 const & v{cart.v};
 
      // Compute the equinoctial orbital elements
      Real r_dot_v{r.dot(v)};
      Real r_norm{r.norm()};
      Vector3 r_unit(r.normalized());
 
      Vector3 hvec(r.cross(v));
      Real h_norm{hvec.norm()};
      Vector3 h_unit(hvec.normalized());
 
      Vector3 v_unit((r_norm*v - r_dot_v*r_unit)/h_norm);
 
      Real p{h_norm*h_norm/mu};
 
      Real k{h_unit.x()/(1.0 + h_unit.z())};
      Real h{-h_unit.y()/(1.0 + h_unit.z())};
 
      Real kk{k*k};
      Real hh{h*h};
      Real s2{1.0 + hh + kk};
      Real tkh{2.0*k*h};
 
      Vector3 ecc(v.cross(hvec)/mu - r_unit);
 
      Vector3 f_unit(1.0 - kk + hh, tkh, -2.0*k);
      Vector3 g_unit(tkh, 1.0 + kk - hh, 2.0*h);
 
      // Normalize the f and g vectors
      f_unit /= s2;
      g_unit /= s2;
 
      Real f{ecc.dot(f_unit)};
      Real g{ecc.dot(g_unit)};
 
      Real L{std::atan2(r_unit.y() - v_unit.x(), r_unit.x() + v_unit.y())};
 
      // Assign the equinoctial orbital elements
      equi.p = p;
      equi.f = f;
      equi.g = g;
      equi.h = h;
      equi.k = k;
 
      ASTRO_ASSERT(equi.sanity_check(), CMD "conversion error.");
 
      // Return the true longitude
      return L;
 
      #undef CMD
    }

    void equinoctial_to_cartesian(Equinoctial const & equi, Real const L, Factor const I, Real const mu,
      Cartesian & cart)
    {
      #define CMD "Astro::OrbitalElements::equinoctial_to_cartesian(...): "
 
      // Reset the cartesian state vectors
      cart.reset();
 
      // Retrieve the equinoctial orbital elements
      Real p{equi.p};
      Real f{equi.f};
      Real g{equi.g};
      Real h{equi.h};
      Real k{equi.k};
 
      // Compute the distance to the central body
      Real c_L{std::cos(L)}, s_L{std::sin(L)};
      Real h2{h*h}, k2{k*k}, hk{h*k};
      Real bf{p / ((1.0+f*c_L+g*s_L) * (1.0+h2+k2))};
      Real x{bf*c_L}, y{bf*s_L};
      Real bf1{std::sqrt(mu/p) / (1.0+h2+k2)};
      Real c_Lf{bf1 * (c_L+f)};
      Real s_Lg{bf1 * (s_L+g)};
 
      // Compute the position and velocity vectors in the inertial frame
      cart.r <<
        (1.0+h2-k2)*x + 2.0*static_cast<Real>(I)*hk*y,
        static_cast<Real>(I)*(1.0-h2+k2)*y + 2.0*hk*x,
        2.0*(h*y-static_cast<Real>(I)*k*x);
      cart.v <<
        static_cast<Real>(I)*2.0*hk*c_Lf - (1.0+h2-k2)*s_Lg,
        static_cast<Real>(I)*(1.0-h2+k2)*c_Lf - 2.0*hk*s_Lg,
        2.0*(h*c_Lf + static_cast<Real>(I)*k*s_Lg);
 
      ASTRO_ASSERT(cart.sanity_check(), CMD "conversion error.");
 
      #undef CMD
    }

    void equinoctial_to_keplerian(Equinoctial const & equi, Keplerian & kepl)
    {
      #define CMD "Astro::OrbitalElements::equinoctial_to_keplerian(...): "
 
      // Reset the keplerian orbital elements
      kepl.reset();
 
      // Assign the keplerian orbital elements
      kepl.a     = equi.a();
      kepl.e     = equi.e();
      kepl.i     = equi.i();
      kepl.Omega = equi.Omega();
      kepl.omega = equi.omega();
 
      ASTRO_ASSERT(kepl.sanity_check(), CMD "conversion error.");
 
      #undef CMD
    }
 
  } // namespace OrbitalElements
 
} // namespace Astro
 
#endif // ASTRO_ORBITALELEMENTS_HH