.. _program_listing_file_conversions_frame_conversions.cc:

Program Listing for File frame_conversions.cc
=============================================

|exhale_lsh| :ref:`Return to documentation for file <file_conversions_frame_conversions.cc>` (``conversions/frame_conversions.cc``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

   
   
   #include "lupnt/conversions/frame_conversions.h"
   
   #include <cspice/SpiceUsr.h>
   
   #include <cmath>
   #include <functional>
   #include <optional>
   #include <vector>
   
   #include "lupnt/conversions/state_conversions.h"
   #include "lupnt/conversions/time_conversions.h"
   #include "lupnt/core/constants.h"
   #include "lupnt/core/logger.h"
   #include "lupnt/data/eop.h"
   #include "lupnt/data/iau_sofa.h"
   #include "lupnt/data/kernels.h"
   #include "lupnt/interfaces/spice.h"
   #include "lupnt/numerics/cheby_fit.h"
   #include "lupnt/numerics/math_utils.h"
   
   #define FRAME_CONVERSION(from, to, func) \
     {{Frame::from, Frame::to}, [](Real t, const Vec6& rv) -> Vec6 { return func(t, rv); }}
   
   namespace lupnt {
   
     Mat3 RotPrecessionNutation(Real t_tdb) {
       Real t_tt = ConvertTime(t_tdb, Time::TDB, Time::TT);
       Real jd_tt = TimeToJd(t_tt);
   
       IauSofaData iau_data = GetIauSofaData(jd_tt);
       Real X = iau_data.X * RAD_ARCSEC;
       Real Y = iau_data.Y * RAD_ARCSEC;
       Real s = iau_data.s * RAD_ARCSEC;
   
       Real a = 1. / (1. + sqrt(1. - X * X - Y * Y));
       Mat3 mat{{1. - a * X * X, -a * X * Y, -X},
                {-a * X * Y, 1. - a * Y * Y, -Y},
                {X, Y, 1. - a * (X * X + Y * Y)}};
       Mat3 R_pn = RotZ(-s) * mat;
       return R_pn;
     }
   
     namespace {
       // Forward declaration; defined below alongside the SPICE-fitted EOP
       // infrastructure. Lets RotSideralMotion/RotSideralMotionDot/
       // RotPolarMotion transparently consult a SPICE-derived EOP fit (see
       // InitFrameConversionFromSpice / ComputeEopFromSpice) before falling
       // back to the loaded IERS EOP table (GetEopData).
       bool TryGetFittedEop(Real t_tdb, Real* xp, Real* yp, Real* ut1_utc, Real* ut1_utc_dot);
     }  // namespace
   
     Mat3 RotSideralMotion(Real t_tdb) {
       Real t_ut1;
       Real ut1_utc;
       if (TryGetFittedEop(t_tdb, nullptr, nullptr, &ut1_utc, nullptr)) {
         Real t_utc = ConvertTime(t_tdb, Time::TDB, Time::UTC);
         t_ut1 = t_utc + ut1_utc;
       } else {
         t_ut1 = ConvertTime(t_tdb, Time::TDB, Time::UT1);
       }
       Real theta_era = EarthRotationAngle(t_ut1);
       Mat3 R_s = RotZ(theta_era);
       return R_s;
     }
   
     Mat3 RotSideralMotionDot(Real t_tdb) {
       Real t_ut1;
       Real w_E;
   
       Real ut1_utc, ut1_utc_dot;
       if (TryGetFittedEop(t_tdb, nullptr, nullptr, &ut1_utc, &ut1_utc_dot)) {
         Real t_utc = ConvertTime(t_tdb, Time::TDB, Time::UTC);
         t_ut1 = t_utc + ut1_utc;
         w_E = (TWO_PI * 1.00273781191135448 / SECS_DAY) * (1. + ut1_utc_dot);
       } else {
         t_ut1 = ConvertTime(t_tdb, Time::TDB, Time::UT1);
   
         Real t_utc = ConvertTime(t_tdb, Time::TDB, Time::UTC);
         Real mjd_utc = TimeToMjd(t_utc);
         EopData eop = GetEopData(mjd_utc);
         Real lod = eop.lod;
         w_E = 7.292115146706979e-5 * (1. - lod / SECS_DAY);
       }
   
       Real theta_era = EarthRotationAngle(t_ut1);
       return RotZdot(theta_era, w_E);
     }
   
     Mat3 RotPolarMotion(Real t_tdb) {
       Real t_tt = ConvertTime(t_tdb, Time::TDB, Time::TT);
   
       Real xp, yp;
       if (!TryGetFittedEop(t_tdb, &xp, &yp, nullptr, nullptr)) {
         Real t_utc = ConvertTime(t_tdb, Time::TDB, Time::UTC);
         Real mjd_utc = TimeToMjd(t_utc);
         EopData eop = GetEopData(mjd_utc);
         xp = eop.x_pole;
         yp = eop.y_pole;
       }
       Real sp = -47e-6 * RAD_ARCSEC * (t_tt / DAYS_CENTURY);
   
       Mat3 R_po = RotX(-yp) * RotY(-xp) * RotZ(sp);
       return R_po;
     }
   
     // =====================================================================
     // SPICE-derived Earth orientation parameters & SPICE-fitted lunar
     // orientation
     // =====================================================================
     //
     // GcrfToItrf/ItrfToGcrf and MoonCiToPa/MoonPaToCi normally compute the
     // Earth/lunar orientation rotations from purely-analytic models (IAU
     // 2006/2000A precession-nutation + EOP-based polar motion/sidereal
     // rotation for Earth -- see RotPrecessionNutation/RotPolarMotion/
     // RotSideralMotion above, with EOP (x_pole, y_pole, UT1-UTC, LOD) coming
     // from the loaded IERS EOP table, see lupnt/data/eop.h; static,
     // pre-extracted DE Chebyshev "lunar mantle libration" angles for the
     // Moon -- see GetLunarMantleData in kernels.cc). These can disagree with
     // SPICE's high-accuracy binary PCK-based orientation
     // (lupnt::spice::ConvertFrameSpice, see frame_converter_spice.cc -- now
     // backed by the auto-downloaded earth_latest_high_prec.bpc /
     // moon_pa_de*.bpc + moon_de*.tf kernels, see spice::LoadSpiceKernel) by a
     // non-negligible amount -- in particular when the loaded EOP table does
     // not cover the requested epoch.
     //
     // ComputeEopFromSpice(t_tdb) (declared in frame_conversions.h) derives the
     // Earth orientation parameters (x_pole, y_pole, UT1-UTC) at a single epoch
     // directly from SPICE's "J2000"<->"ITRF93" rotation, by combining it with
     // LuPNT's analytic precession-nutation matrix (RotPrecessionNutation) and
     // inverting the polar-motion/sidereal-rotation decomposition
     // R_spice = R_po(xp,yp) * R_s(theta_era) * R_pn (closed form, see the
     // implementation below).
     //
     // InitFrameConversionFromSpice() (declared in frame_conversions.h) uses
     // this to close the Earth-orientation gap for a known simulation time
     // window: it samples ComputeEopFromSpice at a dense set of Chebyshev-Gauss
     // epochs spanning the window and fits piecewise Chebyshev polynomial
     // coefficients to the 3 EOP parameters -- the very same Chebyshev
     // representation (and evaluator, cheby_eval_ad) used internally for the
     // planetary/lunar position ephemeris data, see spice_cheby.h / kernels.cc.
     // Once initialized, RotPolarMotion/RotSideralMotion/RotSideralMotionDot
     // (and hence GcrfToItrf/ItrfToGcrf) transparently use these fitted EOP
     // values -- together with the *exact* analytic time derivative of
     // UT1-UTC, which a Chebyshev polynomial yields for free -- whenever the
     // requested epoch falls inside the fitted window, reproducing SPICE
     // closely without requiring SPICE at evaluation time. Outside of the
     // fitted window, the loaded IERS EOP table is used unchanged, so this
     // feature is purely additive/opt-in (nothing changes unless
     // InitFrameConversionFromSpice is explicitly called).
     //
     // The Moon CI<->PA fit (ZXZ Euler angles fitted directly to the SPICE
     // "J2000"<->"IAU_MOON" rotation) is unrelated and unchanged -- see
     // MoonCiToPa/MoonPaToCi and FitEulerAngleModel below.
   
     namespace {
   
       // ChebyshevFitSegment, ChebyshevFitModel, and FitChebyshevModel are now
       // defined in lupnt/numerics/cheby_fit.h (shared with time_conversions.cc).
   
       // A Chebyshev-fitted 3x3 rotation matrix R(t) together with its exact
       // Extracts ZXZ Euler angles (phi, theta, psi) from a rotation matrix R
       // satisfying R = Rz(psi) * Rx(theta) * Rz(phi) (passive convention,
       // matching RotZ / RotX in math_utils.cc).  Returns phi, psi in (-pi,pi]
       // and theta in [0, pi].
       //
       // Derived from the ZXZ product (cp=cos(psi), sp=sin(psi), etc.):
       //   R(2,2) = cos(theta)
       //   R(2,0) =  sin(phi)*sin(theta)   R(2,1) = -cos(phi)*sin(theta)
       //   R(0,2) =  sin(psi)*sin(theta)   R(1,2) =  cos(psi)*sin(theta)
       // => theta = acos(R(2,2)), phi = atan2(R(2,0), -R(2,1)),
       //    psi = atan2(R(0,2), R(1,2)).
       // Near gimbal lock (theta~0 or ~pi, where Earth's GCRF<->ITRF can be
       // transiently for very short windows), phi and psi become individually
       // sensitive, but the reconstructed R = Rz(psi)*Rx(theta)*Rz(phi) stays
       // exactly orthonormal regardless; in practice theta is always nonzero.
       inline void ExtractZXZAngles(const Mat3d& R, double& phi, double& theta, double& psi) {
         theta = std::acos(std::clamp(R(2, 2), -1.0, 1.0));
         phi = std::atan2(R(2, 0), -R(2, 1));
         psi = std::atan2(R(0, 2), R(1, 2));
       }
   
       // Adjusts `raw` by the nearest integer multiple of 2*pi so the result
       // is within pi of `ref` (standard single-step phase unwrap).
       inline double UnwrapAngle(double raw, double ref) {
         const double TWO_PI = 2.0 * PI;
         double diff = raw - ref;
         return raw - std::round(diff / TWO_PI) * TWO_PI;
       }
   
       // A Chebyshev-fitted rotation model that stores R(t) as ZXZ Euler angles
       // (phi, theta, psi) and reconstructs R = Rz(psi)*Rx(theta)*Rz(phi) at
       // evaluation time.
       //
       // KEY GUARANTEE: the reconstructed R_fit is ALWAYS exactly orthonormal
       // (to floating-point precision) regardless of the Chebyshev fit residual
       // in the angles -- because RotZ/RotX each satisfy R^T*R = I exactly via
       // sin^2+cos^2=1, and products of orthonormal matrices are orthonormal.
       // This eliminates the ~4e-8 orthonormality violation that occurred when
       // fitting the 9 matrix elements independently.
       struct FittedEulerAngleModel {
         ChebyshevFitModel fit;  // dim 0=phi, 1=theta, 2=psi
   
         bool Covers(Real t_tdb) const {
           double t = t_tdb.val();
           return t >= fit.t_start && t <= fit.t_end;
         }
   
         // Evaluates R(t) = Rz(psi)*Rx(theta)*Rz(phi) -- always exactly
         // orthonormal -- and, if R_dot != nullptr, its exact analytic dR/dt
         // via the product rule (same pattern as RotMoonCiToPa in kernels.cc).
         bool Eval(Real t_tdb, Mat3* R, Mat3* R_dot) const {
           VecX angles, angles_dot;
           if (!fit.Eval(t_tdb, &angles, R_dot != nullptr ? &angles_dot : nullptr)) return false;
   
           Real phi = angles(0), theta = angles(1), psi = angles(2);
           Mat3 R_z_phi = RotZ(phi);
           Mat3 R_x_theta = RotX(theta);
           Mat3 R_z_psi = RotZ(psi);
           *R = R_z_psi * R_x_theta * R_z_phi;  // always exactly orthonormal
   
           if (R_dot != nullptr) {
             Real phi_dot = angles_dot(0), theta_dot = angles_dot(1), psi_dot = angles_dot(2);
             *R_dot = RotZdot(psi, psi_dot) * R_x_theta * R_z_phi
                      + R_z_psi * RotXdot(theta, theta_dot) * R_z_phi
                      + R_z_psi * R_x_theta * RotZdot(phi, phi_dot);
           }
           return true;
         }
       };
   
       // Samples rotation matrices from `rotation_sampler` at Chebyshev-Gauss
       // nodes across [t_start, t_end] (split into ~segment_length segments),
       // extracts ZXZ Euler angles (phi, theta, psi) for R = Rz(psi)*Rx(theta)*
       // Rz(phi), UNWRAPS phi and psi continuously across the entire window so
       // the fitted functions are smooth, and returns a 3-dim piecewise
       // Chebyshev model ([dim 0]=phi, [dim 1]=theta, [dim 2]=psi).
       //
       // Unwrapping is essential for phi and psi: Earth's dominant spin angle
       // (psi~ERA) increases by ~2*pi per sidereal day, so without continuous
       // unwrapping it would wrap at +/-pi and break the polynomial fit.  Since
       // RotZ/RotX are 2*pi-periodic, R_fit is identical for wrapped or
       // unwrapped values; unwrapping only makes the polynomial smooth.
       ChebyshevFitModel FitEulerAngleModel(const std::function<Mat3d(double)>& rotation_sampler,
                                            double t_start, double t_end, double segment_length,
                                            int num_coeffs) {
         if (!(t_end > t_start))
           throw std::runtime_error("FitEulerAngleModel: t_end must be greater than t_start");
         if (num_coeffs < 2)
           throw std::runtime_error("FitEulerAngleModel: num_coeffs must be at least 2");
         if (!(segment_length > 0.0))
           throw std::runtime_error("FitEulerAngleModel: segment_length must be positive");
   
         double span = t_end - t_start;
         int num_segments = std::max(1, static_cast<int>(std::ceil(span / segment_length)));
         double seg_len = span / num_segments;
   
         ChebyshevFitModel model;
         model.t_start = t_start;
         model.t_end = t_end;
         model.num_dims = 3;  // [phi, theta, psi]
         model.num_coeffs = num_coeffs;
         model.segments.reserve(num_segments);
   
         // Running reference for inter-segment continuity: unwrapped phi, psi
         // of the most-recently-processed sample in temporal order.
         double ref_phi = 0.0, ref_psi = 0.0;
         bool first_sample = true;
   
         for (int s = 0; s < num_segments; s++) {
           double seg_start = t_start + s * seg_len;
           double seg_end = (s == num_segments - 1) ? t_end : seg_start + seg_len;
           double mid = 0.5 * (seg_start + seg_end);
           double radius = 0.5 * (seg_end - seg_start);
   
           // Chebyshev-Gauss nodes: t_k = mid + radius*cos((2k+1)*pi/(2N)),
           // k=0..N-1.  cos is strictly decreasing in k, so k=0 is the LATEST
           // node and k=N-1 is the EARLIEST.  Process in temporal order
           // (time-sorted index i=0..N-1 -> DCT index k=N-1-i) so ref_phi/psi
           // advances monotonically in time for correct unwrapping.
           std::vector<double> phi_dct(num_coeffs), theta_dct(num_coeffs), psi_dct(num_coeffs);
   
           for (int i = 0; i < num_coeffs; i++) {
             int k = num_coeffs - 1 - i;  // DCT index for the i-th earliest node
             double cheby_arg = (2.0 * k + 1.0) * PI / (2.0 * num_coeffs);
             double t = mid + radius * std::cos(cheby_arg);
   
             Mat3d Rmat = rotation_sampler(t);
             double phi_raw, theta_val, psi_raw;
             ExtractZXZAngles(Rmat, phi_raw, theta_val, psi_raw);
   
             double phi_uw, psi_uw;
             if (first_sample) {
               phi_uw = phi_raw;
               psi_uw = psi_raw;
               first_sample = false;
             } else {
               phi_uw = UnwrapAngle(phi_raw, ref_phi);
               psi_uw = UnwrapAngle(psi_raw, ref_psi);
             }
             ref_phi = phi_uw;
             ref_psi = psi_uw;
   
             phi_dct[k] = phi_uw;
             theta_dct[k] = theta_val;  // theta in [0,pi], no wrap possible
             psi_dct[k] = psi_uw;
           }
   
           // Discrete Chebyshev transform (DCT-II) -- identical formula to
           // FitChebyshevModel above.
           ChebyshevFitSegment seg;
           seg.t_mid = mid;
           seg.t_radius = radius;
           seg.coeffs.assign(3, std::vector<double>(num_coeffs, 0.0));
           for (int j = 0; j < num_coeffs; j++) {
             double w = (j == 0) ? 1.0 : 2.0;
             for (int k = 0; k < num_coeffs; k++) {
               double c = std::cos((2.0 * k + 1.0) * PI * j / (2.0 * num_coeffs));
               seg.coeffs[0][j] += phi_dct[k] * c;
               seg.coeffs[1][j] += theta_dct[k] * c;
               seg.coeffs[2][j] += psi_dct[k] * c;
             }
             seg.coeffs[0][j] *= w / num_coeffs;
             seg.coeffs[1][j] *= w / num_coeffs;
             seg.coeffs[2][j] *= w / num_coeffs;
           }
           model.segments.push_back(std::move(seg));
         }
         return model;
       }
   
       // Returns the 3x3 rotation block of the SPICE state-transformation matrix
       // `sxform_c(from_frame, to_frame, et)` at epoch t_tdb (seconds TDB since
       // J2000).
       Mat3d SampleSpiceRotationMatrix(double t_tdb, const std::string& from_frame,
                                       const std::string& to_frame) {
         SpiceDouble et = t_tdb;
         double xform[6][6];
   #pragma omp critical
         {
           sxform_c(from_frame.c_str(), to_frame.c_str(), et, xform);
         }
         Mat3d R;
         for (int i = 0; i < 3; i++)
           for (int j = 0; j < 3; j++) R(i, j) = xform[i][j];
         return R;
       }
   
       std::optional<ChebyshevFitModel> spice_fit_eop;  // dims: 0=x_pole, 1=y_pole, 2=ut1_utc
       std::optional<FittedEulerAngleModel> spice_fit_moonci_to_moonpa;
   
       bool TryGetFittedEop(Real t_tdb, Real* xp, Real* yp, Real* ut1_utc, Real* ut1_utc_dot) {
         VecX f, df;
         bool ok = false;
   #pragma omp critical
         {
           ok = spice_fit_eop.has_value()
                && spice_fit_eop->Eval(t_tdb, &f, ut1_utc_dot != nullptr ? &df : nullptr);
         }
         if (!ok) return false;
         if (xp != nullptr) *xp = f(0);
         if (yp != nullptr) *yp = f(1);
         if (ut1_utc != nullptr) *ut1_utc = f(2);
         if (ut1_utc_dot != nullptr) *ut1_utc_dot = df(2);
         return true;
       }
   
       bool TryGetFittedMoonCiToPa(Real t_tdb, Mat3* R, Mat3* R_dot) {
         bool ok = false;
   #pragma omp critical
         {
           ok = spice_fit_moonci_to_moonpa.has_value()
                && spice_fit_moonci_to_moonpa->Eval(t_tdb, R, R_dot);
         }
         return ok;
       }
   
     }  // namespace
   
     SpiceEopParams ComputeEopFromSpice(Real t_tdb) {
       spice::LoadSpiceKernel();
   
       Mat3d R_spice = SampleSpiceRotationMatrix(t_tdb.val(), "J2000", "ITRF93");
   
       Mat3 R_pn = RotPrecessionNutation(t_tdb);
       Mat3d R_pn_d;
       for (int i = 0; i < 3; i++)
         for (int j = 0; j < 3; j++) R_pn_d(i, j) = R_pn(i, j).val();
   
       // R_spice ~= R_po(xp,yp) * R_s(theta_era) * R_pn
       //          = RotX(-yp) * RotY(-xp) * RotZ(sp + theta_era) * R_pn
       Mat3d M = R_spice * R_pn_d.transpose();
   
       double xp = std::asin(std::clamp(M(0, 2), -1.0, 1.0));
       double yp = -std::atan2(M(1, 2), M(2, 2));
       double psi = std::atan2(M(0, 1), M(0, 0));  // = sp + theta_era
   
       double t_tt = ConvertTime(t_tdb, Time::TDB, Time::TT).val();
       double sp = -47e-6 * RAD_ARCSEC * (t_tt / DAYS_CENTURY);
       double theta_era = std::atan2(std::sin(psi - sp), std::cos(psi - sp));
   
       // Invert EarthRotationAngle(t_ut1) for t_ut1 by linearizing about
       // t_ut1 ~= t_tdb (the true UT1-UTC offset is at most ~1 s, far smaller
       // than the ~1-day period of theta_era).
       constexpr double theta_0 = 0.7790572732640;
       constexpr double dtheta_dt = 1.00273781191135448;
       double era_at_tdb_arg = TWO_PI * (theta_0 + dtheta_dt * t_tdb.val() / SECS_DAY);
       double era_at_tdb = std::atan2(std::sin(era_at_tdb_arg), std::cos(era_at_tdb_arg));
       double dtheta = std::atan2(std::sin(theta_era - era_at_tdb), std::cos(theta_era - era_at_tdb));
       double dt_ut1 = dtheta / (TWO_PI * dtheta_dt / SECS_DAY);
   
       double t_utc = ConvertTime(t_tdb, Time::TDB, Time::UTC).val();
       double ut1_utc = (t_tdb.val() + dt_ut1) - t_utc;
   
       SpiceEopParams params;
       params.x_pole = xp;
       params.y_pole = yp;
       params.ut1_utc = ut1_utc;
       return params;
     }
   
     void InitFrameConversionFromSpice(Real t_start_tdb, Real t_end_tdb, Real segment_length_s,
                                       int num_coeffs) {
       double t0 = t_start_tdb.val();
       double t1 = t_end_tdb.val();
       if (!(t1 > t0))
         throw std::runtime_error(
             "InitFrameConversionFromSpice: t_end_tdb must be greater than t_start_tdb");
   
       spice::LoadSpiceKernel();
   
       // The native GcrfToItrf/MoonCiToPa functions ultimately need to match
       // lupnt::spice::ConvertFrameSpice -- the SPICE-based reference path
       // already wired into LuPNT (see frame_converter_spice.cc) -- so the
       // fits are computed against the *exact same* SPICE frame pairs that
       // ConvertFrameSpice itself uses for these conversions: "J2000" <->
       // "ITRF93" for GCRF<->ITRF (the high-accuracy Earth orientation frame
       // defined by the auto-downloaded earth_latest_high_prec.bpc), and
       // "J2000" <-> "IAU_MOON" for MOON_CI<->MOON_PA.
       double seg_len = segment_length_s.val();
       ChebyshevFitModel earth_fit = FitChebyshevModel(
           [](double t) -> VecXd {
             SpiceEopParams p = ComputeEopFromSpice(Real(t));
             VecXd f(3);
             f << p.x_pole.val(), p.y_pole.val(), p.ut1_utc.val();
             return f;
           },
           t0, t1, /*num_dims=*/3, seg_len, num_coeffs);
       ChebyshevFitModel moon_fit = FitEulerAngleModel(
           [](double t) { return SampleSpiceRotationMatrix(t, "J2000", "IAU_MOON"); }, t0, t1, seg_len,
           num_coeffs);
   
       Logger::Info(
           fmt::format("Fitted SPICE-derived Earth orientation parameters (x_pole, y_pole, "
                       "UT1-UTC) and MOON_CI<->MOON_PA orientation over epoch window [{:.3f}, "
                       "{:.3f}] s TDB ({} segment(s) of ~{:.0f} s, {} Chebyshev coefficients per "
                       "fitted quantity)",
                       t0, t1, earth_fit.segments.size(), seg_len, num_coeffs),
           "FrameConverter");
   
   #pragma omp critical
       {
         spice_fit_eop = std::move(earth_fit);
         spice_fit_moonci_to_moonpa = FittedEulerAngleModel{std::move(moon_fit)};
       }
     }
   
     void ClearFrameConversionFit() {
   #pragma omp critical
       {
         spice_fit_eop.reset();
         spice_fit_moonci_to_moonpa.reset();
       }
     }
   
     bool HasFittedEarthOrientation(Real t_tdb) {
       bool covers = false;
       double t = t_tdb.val();
   #pragma omp critical
       {
         covers
             = spice_fit_eop.has_value() && t >= spice_fit_eop->t_start && t <= spice_fit_eop->t_end;
       }
       return covers;
     }
   
     bool HasFittedLunarOrientation(Real t_tdb) {
       bool covers = false;
   #pragma omp critical
       {
         covers = spice_fit_moonci_to_moonpa.has_value() && spice_fit_moonci_to_moonpa->Covers(t_tdb);
       }
       return covers;
     }
   
     Vec6 GcrfToItrf(Real t_tdb, const Vec6& rv_gcrf) {
       Mat3 R_po = RotPolarMotion(t_tdb);
       Mat3 R_pn = RotPrecessionNutation(t_tdb);
       Mat3 R_s = RotSideralMotion(t_tdb);
       Mat3 R_s_dot = RotSideralMotionDot(t_tdb);
   
       Mat3 R_GcrfToItrf = R_po * R_s * R_pn;
       Mat3 R_GcrfToItrf_dot = R_po * R_s_dot * R_pn;
   
       Vec3 r_gcrf = rv_gcrf.head(3);
       Vec3 v_gcrf = rv_gcrf.tail(3);
   
       Vec3 r_itrf = R_GcrfToItrf * r_gcrf;
       Vec3 v_itrf = R_GcrfToItrf * v_gcrf + R_GcrfToItrf_dot * r_gcrf;
   
       Vec6 rv_itrf;
       rv_itrf << r_itrf, v_itrf;
       return rv_itrf;
     }
   
     Vec3 GcrfToItrf(Real t_tdb, const Vec3& r_gcrf) {
       Mat3 R_po = RotPolarMotion(t_tdb);
       Mat3 R_pn = RotPrecessionNutation(t_tdb);
       Mat3 R_s = RotSideralMotion(t_tdb);
       Mat3 R_GcrfToItrf = R_po * R_s * R_pn;
   
       Vec3 r_itrf = R_GcrfToItrf * r_gcrf;
       return r_itrf;
     }
   
     Vec6 ItrfToGcrf(Real t_tdb, const Vec6& rv_itrf) {
       Mat3 R_po = RotPolarMotion(t_tdb);
       Mat3 R_pn = RotPrecessionNutation(t_tdb);
       Mat3 R_s = RotSideralMotion(t_tdb);
       Mat3 R_s_dot = RotSideralMotionDot(t_tdb);
   
       Mat3 R_GcrfToItrf = R_po * R_s * R_pn;
       Mat3 R_GcrfToItrf_dot = R_po * R_s_dot * R_pn;
   
       Vec3 r_itrf = rv_itrf.head(3);
       Vec3 v_itrf = rv_itrf.tail(3);
   
       Vec3 r_gcrf = R_GcrfToItrf.transpose() * r_itrf;
       Vec3 v_gcrf = R_GcrfToItrf.transpose() * (v_itrf - R_GcrfToItrf_dot * r_gcrf);
   
       Vec6 rv_gcrf;
       rv_gcrf << r_gcrf, v_gcrf;
       return rv_gcrf;
     }
   
     Vec3 ItrfToGcrf(Real t_tdb, const Vec3& r_itrf) {
       Mat3 R_po = RotPolarMotion(t_tdb);
       Mat3 R_pn = RotPrecessionNutation(t_tdb);
       Mat3 R_s = RotSideralMotion(t_tdb);
       Mat3 R_GcrfToItrf = R_po * R_s * R_pn;
   
       Vec3 r_gcrf = R_GcrfToItrf.transpose() * r_itrf;
       return r_gcrf;
     }
   
     Mat3d RotGcrfToEme() {
       const double da = FRAME_BIAS_DALPHA0;
       const double xi = FRAME_BIAS_XI0;
       const double eta = FRAME_BIAS_ETA0;
       Mat3d B_e = RotX(-eta) * RotY(xi) * RotZ(da);
       return B_e;
     }
   
     Mat3d RotGcrfToEmeFirstOrder() {
       const double da = FRAME_BIAS_DALPHA0;
       const double xi = FRAME_BIAS_XI0;
       const double eta = FRAME_BIAS_ETA0;
       Mat3d B_e{{1, da, -xi}, {-da, 1, -eta}, {xi, eta, 1}};
       return B_e;
     }
   
     Mat3d RotGcrfToEmeSecondOrder() {
       const double da = FRAME_BIAS_DALPHA0;
       const double xi = FRAME_BIAS_XI0;
       const double eta = FRAME_BIAS_ETA0;
       Mat3d B_e{{1. - 0.5 * (da * da + xi * xi), da, -xi},
                 {-da - eta * xi, 1. - 0.5 * (da * da + eta * eta), -eta},
                 {xi - eta * da, eta + xi * da, 1. - 0.5 * (eta * eta + xi * xi)}};
       return B_e;
     }
   
     Vec6 GcrfToEme(const Vec6& rv_gcrf) {
       Mat3d B_e = RotGcrfToEme();
       Vec3 r_gcrf = rv_gcrf.head(3);
       Vec3 v_gcrf = rv_gcrf.tail(3);
   
       Vec3 r_eme = B_e * r_gcrf;
       Vec3 v_eme = B_e * v_gcrf;
   
       Vec6 rv_eme;
       rv_eme << r_eme, v_eme;
       return rv_eme;
     }
     Vec3 GcrfToEme(const Vec3& r_gcrf) {
       Mat3d B_e = RotGcrfToEme();
       Vec3 r_eme = B_e * r_gcrf;
       return r_eme;
     }
   
     Vec6 EmeToGcrf(const Vec6& rv_eme) {
       Mat3d B_e = RotGcrfToEme();
       Vec3 r_eme = rv_eme.head(3);
       Vec3 v_eme = rv_eme.tail(3);
   
       Mat3d B_e_inv = B_e.transpose();
       Vec3 r_gcrf = B_e_inv * r_eme;
       Vec3 v_gcrf = B_e_inv * v_eme;
   
       Vec6 rv_gcrf;
       rv_gcrf << r_gcrf, v_gcrf;
       return rv_gcrf;
     }
     Vec3 EmeToGcrf(const Vec3& r_eme) {
       Mat3d B_e = RotGcrfToEme();
       Mat3d B_e_inv = B_e.transpose();
       Vec3 r_gcrf = B_e_inv * r_eme;
       return r_gcrf;
     }
   
     Vec6 GcrfToIcrf(Real t_tdb, const Vec6& rv_gcrf) {
       Vec6 rv_ssb2earth = GetBodyPosVel(t_tdb, BodyId::SSB, BodyId::EARTH, Frame::GCRF);
       Vec6 rv_icrf = rv_gcrf + rv_ssb2earth;
       return rv_icrf;
     }
     Vec3 GcrfToIcrf(Real t_tdb, const Vec3& r_gcrf) {
       Vec3 r_ssb2earth = GetBodyPos(t_tdb, BodyId::SSB, BodyId::EARTH, Frame::GCRF);
       Vec3 r_icrf = r_gcrf + r_ssb2earth;
       return r_icrf;
     }
   
     Vec6 IcrfToGcrf(Real t_tdb, const Vec6& rv_icrf) {
       Vec6 rv_ssb2earth = GetBodyPosVel(t_tdb, BodyId::SSB, BodyId::EARTH, Frame::GCRF);
       Vec6 rv_gcrf = rv_icrf - rv_ssb2earth;
       return rv_gcrf;
     }
     Vec3 IcrfToGcrf(Real t_tdb, const Vec3& r_icrf) {
       Vec3 r_ssb2earth = GetBodyPos(t_tdb, BodyId::SSB, BodyId::EARTH, Frame::GCRF);
       Vec3 r_gcrf = r_icrf - r_ssb2earth;
       return r_gcrf;
     }
   
     Vec6 GcrfToMoonCi(Real t_tdb, const Vec6& rv_gcrf) {
       Vec6 rv_earth2moon = GetBodyPosVel(t_tdb, BodyId::EARTH, BodyId::MOON, Frame::GCRF);
       Vec6 rv_mi = rv_gcrf - rv_earth2moon;
       return rv_mi;
     }
     Vec3 GcrfToMoonCi(Real t_tdb, const Vec3& r_gcrf) {
       Vec3 r_earth2moon = GetBodyPos(t_tdb, BodyId::EARTH, BodyId::MOON, Frame::GCRF);
       Vec3 r_mi = r_gcrf - r_earth2moon;
       return r_mi;
     }
   
     Vec6 MoonCiToGcrf(Real t_tdb, const Vec6& rv_mi) {
       Vec6 rv_earth2moon = GetBodyPosVel(t_tdb, BodyId::EARTH, BodyId::MOON, Frame::GCRF);
       Vec6 rv_gcrf = rv_mi + rv_earth2moon;
       return rv_gcrf;
     }
     Vec3 MoonCiToGcrf(Real t_tdb, const Vec3& r_mi) {
       Vec3 r_earth2moon = GetBodyPos(t_tdb, BodyId::EARTH, BodyId::MOON, Frame::GCRF);
       Vec3 r_gcrf = r_mi + r_earth2moon;
       return r_gcrf;
     }
   
     struct CacheRotMoonCiToPa {
       Real t_tdb = NAN;
       Mat3 R_mi2pa;
       Mat3 R_mi2pa_dot;
       bool compute_vel;
     };
     static CacheRotMoonCiToPa cache_rot_moon_ci2pa;
   
     Mat3 RotMoonCiToPa(Real t_tdb, Mat3* R_mi2pa_dot) {
       bool compute_vel = (R_mi2pa_dot != nullptr);
   
       {
         Mat3 R_mi2pa_fit, R_mi2pa_dot_fit;
         if (TryGetFittedMoonCiToPa(t_tdb, &R_mi2pa_fit, compute_vel ? &R_mi2pa_dot_fit : nullptr)) {
           if (compute_vel) *R_mi2pa_dot = R_mi2pa_dot_fit;
           return R_mi2pa_fit;
         }
       }
   
       // Check cache
       if (abs(t_tdb - cache_rot_moon_ci2pa.t_tdb) < EPS
           && cache_rot_moon_ci2pa.compute_vel == compute_vel) {
         Mat3 R_mi2pa;
         bool cached = false;
   #pragma omp critical
         {
           if (abs(t_tdb - cache_rot_moon_ci2pa.t_tdb) < EPS
               && cache_rot_moon_ci2pa.compute_vel == compute_vel) {
             if (compute_vel) *R_mi2pa_dot = cache_rot_moon_ci2pa.R_mi2pa_dot;
             R_mi2pa = cache_rot_moon_ci2pa.R_mi2pa;
             cached = true;
           }
         }
         if (cached) return R_mi2pa;
       }
   
       Vec6 lunar_mantle = GetLunarMantleData(t_tdb, compute_vel);
       auto [phi, theta, psi, phi_dot, theta_dot, psi_dot] = Unpack(lunar_mantle);
   
       Mat3 R_z_phi = RotZ(phi), R_x_theta = RotX(theta), R_z_psi = RotZ(psi);
       Mat3 R_mi2pa = R_z_psi * R_x_theta * R_z_phi;
   
       if (compute_vel) {
         Real spsi = sin(psi);
         Real cpsi = cos(psi);
         Mat3 mat = RotZdot(psi, psi_dot) * R_x_theta * R_z_phi
                    + R_z_psi * RotXdot(theta, theta_dot) * R_z_phi
                    + R_z_psi * R_x_theta * RotZdot(phi, phi_dot);
         *R_mi2pa_dot = mat;
       }
   
       // Update cache
   #pragma omp critical
       {
         if (t_tdb > cache_rot_moon_ci2pa.t_tdb + EPS) {
           cache_rot_moon_ci2pa.t_tdb = t_tdb;
           cache_rot_moon_ci2pa.R_mi2pa = R_mi2pa;
           cache_rot_moon_ci2pa.compute_vel = compute_vel;
           if (compute_vel) cache_rot_moon_ci2pa.R_mi2pa_dot = *R_mi2pa_dot;
         }
       }
       return R_mi2pa;
     }
   
     Vec6 MoonCiToPa(Real t_tdb, const Vec6& rv_mi) {
       Mat3 R_mi2pa_dot;
       Mat3 R_mi2pa = RotMoonCiToPa(t_tdb, &R_mi2pa_dot);
   
       Vec3 r_mi = rv_mi.head(3);
       Vec3 v_mi = rv_mi.tail(3);
   
       Vec3 r_pa = R_mi2pa * r_mi;
       Vec3 v_pa = R_mi2pa * v_mi + R_mi2pa_dot * r_mi;
   
       Vec6 rv_pa;
       rv_pa << r_pa, v_pa;
       return rv_pa;
     }
     Vec3 MoonCiToPa(Real t_tdb, const Vec3& r_mi) {
       Mat3 R_mi2pa = RotMoonCiToPa(t_tdb);
       Vec3 r_pa = R_mi2pa * r_mi;
       return r_pa;
     }
   
     Vec6 MoonPaToCi(Real t_tdb, const Vec6& rv_pa) {
       Mat3 R_mi2pa_dot;
       Mat3 R_mi2pa = RotMoonCiToPa(t_tdb, &R_mi2pa_dot);
   
       Vec3 r_pa = rv_pa.head(3);
       Vec3 v_pa = rv_pa.tail(3);
   
       Vec3 r_mi = R_mi2pa.transpose() * r_pa;
       Vec3 v_mi = R_mi2pa.transpose() * (v_pa - R_mi2pa_dot * r_mi);
       Vec6 rv_mi;
       rv_mi << r_mi, v_mi;
       return rv_mi;
     }
     Vec3 MoonPaToCi(Real t_tdb, const Vec3& r_pa) {
       Mat3 R_mi2pa = RotMoonCiToPa(t_tdb);
       Vec3 r_mi = R_mi2pa.transpose() * r_pa;
       return r_mi;
     }
   
     Mat3d RotMoonPaToMe() {
       Mat3d B_moon
           = RotX(-0.2785 * RAD_ARCSEC) * RotY(-78.6944 * RAD_ARCSEC) * RotZ(-67.8526 * RAD_ARCSEC);
       return B_moon;
     }
   
     Vec6 MoonPaToMe(const Vec6& rv_pa) {
       Mat3d B_moon = RotMoonPaToMe();
       Vec3 r_pa = rv_pa.head(3);
       Vec3 v_pa = rv_pa.tail(3);
   
       Vec3 r_me = B_moon * r_pa;
       Vec3 v_me = B_moon * v_pa;
   
       Vec6 rv_me;
       rv_me << r_me, v_me;
       return rv_me;
     }
     Vec3 MoonPaToMe(const Vec3& r_pa) {
       Mat3d B_moon = RotMoonPaToMe();
       Vec3 r_me = B_moon * r_pa;
       return r_me;
     }
   
     Vec6 MoonMeToPa(const Vec6& rv_me) {
       Mat3d B_moon = RotMoonPaToMe();
       Vec3 r_me = rv_me.head(3);
       Vec3 v_me = rv_me.tail(3);
   
       Mat3d B_moon_inv = B_moon.transpose();
       Vec3 r_pa = B_moon_inv * r_me;
       Vec3 v_pa = B_moon_inv * v_me;
   
       Vec6 rv_pa;
       rv_pa << r_pa, v_pa;
       return rv_pa;
     }
     Vec3 MoonMeToPa(const Vec3& r_me) {
       Mat3d B_moon = RotMoonPaToMe();
       Mat3d B_moon_inv = B_moon.transpose();
       Vec3 r_pa = B_moon_inv * r_me;
       return r_pa;
     }
   
     Vec6 GcrfToEmr(Real t_tdb, const Vec6& rv_gcrf) {
       Vec6 rv_earth2emb = GetBodyPosVel(t_tdb, BodyId::EARTH, BodyId::EMB, Frame::GCRF);
       Vec6 rv_emr = InertialToSynodic(rv_earth2emb, rv_gcrf);
       return rv_emr;
     }
     Vec3 GcrfToEmr(Real t_tdb, const Vec3& r_gcrf) {
       Vec6 rv_earth2emb = GetBodyPosVel(t_tdb, BodyId::EARTH, BodyId::EMB, Frame::GCRF);
       Vec6 rv_gcrf;
       rv_gcrf << r_gcrf, Vec3::Zero();
       Vec6 rv_emr = InertialToSynodic(rv_earth2emb, rv_gcrf);
       return rv_emr.head(3);
     }
   
     Vec6 EmrToGcrf(Real t_tdb, const Vec6& rv_emr) {
       Vec6 rv_earth2emb = GetBodyPosVel(t_tdb, BodyId::EARTH, BodyId::EMB, Frame::GCRF);
       Vec6 rv_gcrf = SynodicToInertial(rv_earth2emb, rv_emr);
       return rv_gcrf;
     }
     Vec3 EmrToGcrf(Real t_tdb, const Vec3& r_emr) {
       Vec6 rv_earth2emb = GetBodyPosVel(t_tdb, BodyId::EARTH, BodyId::EMB, Frame::GCRF);
       Vec6 rv_emr;
       rv_emr << r_emr, Vec3::Zero();
       Vec6 rv_gcrf = SynodicToInertial(rv_earth2emb, rv_emr);
       return rv_gcrf.head(3);
     }
   
     Mat3 RotMoonOpToCi(Real t_tdb) {
       Vec6 rv_m2e = GetBodyPosVel(t_tdb, BodyId::MOON, BodyId::EARTH, Frame::GCRF);
       Vec3 r = rv_m2e.head(3);
       Vec3 v = rv_m2e.tail(3);
   
       Vec3 z_op = r.cross(v).normalized();
       Vec3 i_pole_me = Vec3::UnitZ();
       Vec3 i_pole = ConvertFrame(t_tdb, i_pole_me, Frame::MOON_ME, Frame::MOON_CI);
       Vec3 x_op = i_pole.cross(z_op).normalized();
       Vec3 y_op = z_op.cross(x_op).normalized();
   
       // R = [x_OP, y_OP, z_OP]_CI (expressed in CI)
       Mat3 R_op2ci;
       R_op2ci << x_op, y_op, z_op;
       return R_op2ci;
     }
   
     Vec6 MoonCiToOp(Real t_tdb, const Vec6& rv_ci) {
       Mat3 R_ci2op = RotMoonOpToCi(t_tdb).transpose();
       Vec3 r_op = R_ci2op * rv_ci.head(3);
       Vec3 v_op = R_ci2op * rv_ci.tail(3);
       Vec6 rv_op;
       rv_op << r_op, v_op;
       return rv_op;
     }
     Vec3 MoonCiToOp(Real t_tdb, const Vec3& r_ci) {
       Mat3 R_ci2op = RotMoonOpToCi(t_tdb).transpose();
       Vec3 r_op = R_ci2op * r_ci;
       return r_op;
     }
   
     Vec6 MoonOpToCi(Real t_tdb, const Vec6& rv_op) {
       Mat3 R_op2ci = RotMoonOpToCi(t_tdb);
       Vec3 r_ci = R_op2ci * rv_op.head(3);
       Vec3 v_ci = R_op2ci * rv_op.tail(3);
       Vec6 rv_ci;
       rv_ci << r_ci, v_ci;
       return rv_ci;
     }
     Vec3 MoonOpToCi(Real t_tdb, const Vec3& r_op) {
       Mat3 R_op2ci = RotMoonOpToCi(t_tdb);
       Vec3 r_ci = R_op2ci * r_op;
       return r_ci;
     }
   
   }  // namespace lupnt