.. _program_listing_file_conversions_frame_converter.cc:

Program Listing for File frame_converter.cc
===========================================

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

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

.. code-block:: cpp

   
   
   #include "lupnt/conversions/frame_converter.h"
   
   #include "lupnt/conversions/frame_conversions.h"
   #include "lupnt/conversions/frame_converter.h"
   #include "lupnt/conversions/frame_converter_spice.h"
   #include "lupnt/conversions/state_converter.h"
   #include "lupnt/core/constants.h"
   #include "lupnt/states/state.h"
   
   #define FRAME_CONVERSION(from, to, func) \
     {{Frame::from, Frame::to}, [](Real t, const Vec6& rv) -> Vec6 { return func(t, rv); }}
   
   namespace lupnt {
   
     const std::map<Frame, BodyId> frame_centers = {
         // Earth
         {Frame::ITRF, BodyId::EARTH},
         {Frame::ECEF, BodyId::EARTH},
         {Frame::GCRF, BodyId::EARTH},
         {Frame::EME, BodyId::EARTH},
         {Frame::ECI, BodyId::EARTH},
         {Frame::ICRF, BodyId::SOLAR_SYSTEM_BARYCENTER},
         // Moon
         {Frame::MOON_ME, BodyId::MOON},
         {Frame::MOON_CI, BodyId::MOON},
         {Frame::MOON_PA, BodyId::MOON},
         {Frame::MOON_OP, BodyId::MOON},
         // Solar System
         {Frame::MARS_FIXED, BodyId::MARS},
         {Frame::VENUS_FIXED, BodyId::VENUS},
     };
   
     BodyId GetFrameCenter(Frame frame) {
       auto it = frame_centers.find(frame);
       LUPNT_CHECK(it != frame_centers.end(),
                   fmt::format("Frame {} not found in frame_centers", enum_name(frame)),
                   "FrameConverter");
       return it->second;
     }
   
     std::ostream& operator<<(std::ostream& os, Frame frame) {
       switch (frame) {
         case Frame::ITRF: os << "ITRF"; break;
         case Frame::GCRF: os << "GCRF"; break;
         case Frame::EME: os << "EME"; break;
         case Frame::ICRF: os << "ICRF"; break;
         case Frame::SER: os << "SER"; break;
         case Frame::GSE: os << "GSE"; break;
         case Frame::MOD: os << "MOD"; break;
         case Frame::TOD: os << "TOD"; break;
         case Frame::EMR: os << "EMR"; break;
         case Frame::MOON_CI: os << "MOON_CI"; break;
         case Frame::MOON_PA: os << "MOON_PA"; break;
         case Frame::MOON_ME: os << "MOON_ME"; break;
         case Frame::MOON_OP: os << "MOON_OP"; break;
         case Frame::MARS_FIXED: os << "MARS_FIXED"; break;
         case Frame::VENUS_FIXED: os << "VENUS_FIXED"; break;
         default: throw std::runtime_error("Frame not implemented");
       }
       return os;
     }
   
     template <int N>
     Vec<N> ConvertFrameBase(Real t_tdb, const Vec<N>& rv_in, Frame frame_in, Frame frame_out) {
       if (frame_in == frame_out) return rv_in;
       switch (frame_in) {
         // Earth ***************
         case Frame::ICRF:
           return ConvertFrame(t_tdb, IcrfToGcrf(t_tdb, rv_in), Frame::GCRF, frame_out);
         case Frame::ITRF:  // Frame::ECEF:
           return ConvertFrame(t_tdb, ItrfToGcrf(t_tdb, rv_in), Frame::GCRF, frame_out);
         case Frame::GCRF:
           switch (frame_out) {
             case Frame::ICRF: return GcrfToIcrf(t_tdb, rv_in);
             case Frame::ITRF: return GcrfToItrf(t_tdb, rv_in);
             case Frame::SER:
             case Frame::GSE: throw std::runtime_error("Not implemented");
             case Frame::EMR: return GcrfToEmr(t_tdb, rv_in);
             case Frame::EME: return GcrfToEme(rv_in);
             case Frame::MOD:
             case Frame::TOD: throw std::runtime_error("Not implemented");
             case Frame::MOON_PA:
             case Frame::MOON_ME:
             case Frame::MOON_OP:
             case Frame::MOON_CI:
               return ConvertFrame(t_tdb, GcrfToMoonCi(t_tdb, rv_in), Frame::MOON_CI, frame_out);
             default: throw std::runtime_error("Not implemented");
           }
         case Frame::EME:  // Frame::ECI:
           return ConvertFrame(t_tdb, EmeToGcrf(rv_in), Frame::GCRF, frame_out);
         case Frame::EMR: return ConvertFrame(t_tdb, EmrToGcrf(t_tdb, rv_in), Frame::GCRF, frame_out);
         case Frame::SER:
         case Frame::GSE:
         case Frame::MOD:
         case Frame::TOD: throw std::runtime_error("Not implemented");
         // Moon ***************
         case Frame::MOON_CI:
           switch (frame_out) {
             case Frame::MOON_OP: return MoonCiToOp(t_tdb, rv_in);
             case Frame::MOON_PA: return MoonCiToPa(t_tdb, rv_in);
             case Frame::MOON_ME: return MoonPaToMe(MoonCiToPa(t_tdb, rv_in));
             default: return ConvertFrame(t_tdb, MoonCiToGcrf(t_tdb, rv_in), Frame::GCRF, frame_out);
           }
         case Frame::MOON_ME: return ConvertFrame(t_tdb, MoonMeToPa(rv_in), Frame::MOON_PA, frame_out);
         case Frame::MOON_PA: {
           if (frame_out == Frame::MOON_ME) return MoonPaToMe(rv_in);
           return ConvertFrame(t_tdb, MoonPaToCi(t_tdb, rv_in), Frame::MOON_CI, frame_out);
         }
         case Frame::MOON_OP:
           return ConvertFrame(t_tdb, MoonOpToCi(t_tdb, rv_in), Frame::MOON_CI, frame_out);
           // Solar System ***************
         case Frame::MERCURY_FIXED:
         case Frame::VENUS_FIXED:
         case Frame::MARS_FIXED:
         case Frame::JUPITER_FIXED:
         case Frame::SATURN_FIXED:
         case Frame::URANUS_FIXED:
         case Frame::NEPTUNE_FIXED:
         case Frame::MERCURY_CI:
         case Frame::VENUS_CI:
         case Frame::MARS_CI:
         case Frame::JUPITER_CI:
         case Frame::SATURN_CI:
         case Frame::URANUS_CI:
         case Frame::NEPTUNE_CI:
         case Frame::UNDEFINED: throw std::runtime_error("Not implemented");
       }
       throw std::runtime_error("Conversion not implemented");
       return Vec<N>::Zero();
     }
   
     template Vec6 ConvertFrameBase(Real t_tdb, const Vec6& rv_in, Frame frame_in, Frame frame_out);
     template Vec3 ConvertFrameBase(Real t_tdb, const Vec3& r_in, Frame frame_in, Frame frame_out);
   
     Vec3 ConvertFrame(Real t_tdb, const Vec3& r_in, Frame frame_in, Frame frame_out,
                       bool rotate_only) {
       if (frame_in == frame_out) return r_in;
       if (!rotate_only) return ConvertFrameBase(t_tdb, r_in, frame_in, frame_out);
       auto [R, r] = GetFrameRotationTranslation(t_tdb, frame_in, frame_out);
       return R * r_in + r;
     }
   
     Vec6 ConvertFrame(Real t_tdb, const Vec6& rv_in, Frame frame_in, Frame frame_out,
                       bool rotate_only) {
       if (frame_in == frame_out) return rv_in;
       if (!rotate_only) return ConvertFrameBase(t_tdb, rv_in, frame_in, frame_out);
       auto [R, r] = GetFrameRotationTranslation(t_tdb, frame_in, frame_out);
       Vec6 rv_out;
       rv_out.head(3) = R * rv_in.head(3) + r;
       rv_out.tail(3) = R * rv_in.tail(3);
       return rv_out;
     }
   
     MatX6 ConvertFrame(Real t_tdb, const MatX6& rv_in, Frame frame_in, Frame frame_out,
                        bool rotate_only) {
       MatX6 rv_out(rv_in.rows(), 6);
       if (rotate_only) {
         auto [R, r] = GetFrameRotationTranslation(t_tdb, frame_in, frame_out);
         for (int i = 0; i < rv_in.rows(); i++) {
           rv_out.row(i).head(3) = R * rv_in.row(i).head(3).transpose() + r;
           rv_out.row(i).tail(3) = R * rv_in.row(i).tail(3).transpose();
         }
       } else {
         for (int i = 0; i < rv_in.rows(); i++)
           rv_out.row(i)
               = ConvertFrameBase(t_tdb, rv_in.row(i).transpose().eval(), frame_in, frame_out);
       }
       return rv_out;
     }
   
     MatX3 ConvertFrame(Real t_tdb, const MatX3& r_in, Frame frame_in, Frame frame_out,
                        bool rotate_only) {
       MatX3 r_out(r_in.rows(), 3);
       if (rotate_only) {
         auto [R, r] = GetFrameRotationTranslation(t_tdb, frame_in, frame_out);
         for (int i = 0; i < r_in.rows(); i++) r_out.row(i) = R * r_in.row(i).transpose() + r;
       } else {
         for (int i = 0; i < r_in.rows(); i++)
           r_out.row(i) = ConvertFrameBase(t_tdb, r_in.row(i).transpose().eval(), frame_in, frame_out);
       }
       return r_out;
     }
   
     MatX6 ConvertFrame(const VecX& t_tdb, const Vec6& rv_in, Frame frame_in, Frame frame_out,
                        bool rotate_only) {
       MatX6 rv_out(t_tdb.size(), 6);
       for (int i = 0; i < t_tdb.size(); i++)
         rv_out.row(i) = ConvertFrame(t_tdb(i), rv_in, frame_in, frame_out, rotate_only);
       return rv_out;
     }
   
     MatX3 ConvertFrame(const VecX& t_tdb, const Vec3& r_in, Frame frame_in, Frame frame_out,
                        bool rotate_only) {
       MatX3 r_out(t_tdb.size(), 3);
       for (int i = 0; i < t_tdb.size(); i++)
         r_out.row(i) = ConvertFrame(t_tdb(i), r_in, frame_in, frame_out, rotate_only);
       return r_out;
     }
   
     MatX6 ConvertFrame(const VecX& t_tdb, const MatX6& rv_in, Frame frame_in, Frame frame_out,
                        bool rotate_only) {
       LUPNT_CHECK(t_tdb.size() == rv_in.rows(), "Epoch and rv_in must have same size",
                   "FrameConverter");
       MatX6 rv_out(t_tdb.size(), 6);
       for (int i = 0; i < t_tdb.size(); i++)
         rv_out.row(i) = ConvertFrame(t_tdb(i), rv_in.row(i).transpose().eval(), frame_in, frame_out,
                                      rotate_only);
       return rv_out;
     }
   
     MatX3 ConvertFrame(const VecX& t_tdb, const MatX3& r_in, Frame frame_in, Frame frame_out,
                        bool rotate_only) {
       if (t_tdb.size() != r_in.rows()) throw std::runtime_error("Epoch and r_in must have same size");
       MatX3 r_out(t_tdb.size(), 3);
       for (int i = 0; i < t_tdb.size(); i++) {
         Vec3 r_in_ = r_in.row(i).transpose();
         r_out.row(i) = ConvertFrame(t_tdb(i), r_in_, frame_in, frame_out, rotate_only);
       }
       return r_out;
     }
   
     State ConvertFrame(Real t_tdb, const State& x_in, Frame frame_out, bool rotate_only) {
       if (x_in.GetFrame() == frame_out) return x_in;
       Cart6 rv_in = ConvertState(x_in, Cart6::TYPE);
       Cart6 rv_out = ConvertFrame(t_tdb, Vec6(rv_in), x_in.GetFrame(), frame_out, rotate_only);
       rv_out.SetFrame(frame_out);
       return ConvertState(rv_out, x_in.GetType());
     }
   
     std::pair<Mat3, Vec3> GetFrameRotationTranslation(Real t_tdb, Frame from_frame, Frame to_frame) {
       MatX3 I3 = Mat3::Identity();
       Mat3 M = ConvertFrame(t_tdb, I3, from_frame, to_frame);
       MatX3 r0 = MatX3::Zero(1, 3);
       Vec3 r = ConvertFrame(t_tdb, r0, from_frame, to_frame).transpose();
       Mat3 R = (M.rowwise() - r.transpose()).transpose();
       return std::make_pair(R, r);
     }
     std::pair<Mat6, Vec6> GetFrameRotationTranslationRv(Real t_tdb, Frame from_frame,
                                                         Frame to_frame) {
       // Transform the zero state -> this is the affine offset t6.
       MatX6 zero_row = MatX6::Zero(1, 6);
       Vec6 t6 = ConvertFrame(t_tdb, zero_row, from_frame, to_frame, false).row(0).transpose();  // 6x1
   
       // Transform the six basis states (each row is one basis vector).
       MatX6 I6 = Mat6::Identity();
       MatX6 Y = ConvertFrame(t_tdb, I6, from_frame, to_frame, false);  // 6x6
   
       // Build R6: columns are (Y_row_i^T - t6), for i=0..5.
       Mat6 R6;
       for (int j = 0; j < 6; ++j) {
         R6.col(j) = Y.row(j).transpose() - t6;  // 6x1
       }
   
       return {R6, t6};
     }
   
   }  // namespace lupnt