Program Listing for File frame_converter_spice.cc

↰ Return to documentation for file (conversions/frame_converter_spice.cc)

#include "lupnt/conversions/frame_converter_spice.h"

#include "lupnt/conversions/state_conversions.h"
#include "lupnt/core/constants.h"
#include "lupnt/interfaces/spice.h"
#include "lupnt/numerics/math_utils.h"
#include "lupnt/states/state.h"

namespace lupnt {

  namespace spice {

    Cart6 ConvertFrameSpice(Real t_tdb, const Cart6& state_in, Frame frame_out) {
      Vec6 rv_in = state_in;
      Vec6 rv_out = ConvertFrameSpice(t_tdb, rv_in, state_in.GetFrame(), frame_out);
      return Cart6(rv_out, frame_out);
    }

    Vec3 ConvertFrameSpice(Real t_tdb, const Vec3& r_in, Frame frame_in, Frame frame_out) {
      Vec6 rv_in;
      rv_in << r_in, Vec3::Zero();
      Vec6 rv_out_6 = ConvertFrameSpice(t_tdb, rv_in, frame_in, frame_out);
      return rv_out_6.head(3);
    }

    MatX6 ConvertFrameSpice(Real t_tdb, const MatX6& rv_in, Frame frame_in, Frame frame_out) {
      MatX6 rv_out(rv_in.rows(), 6);
      for (int i = 0; i < rv_in.rows(); i++) {
        rv_out.row(i)
            = ConvertFrameSpice(t_tdb, rv_in.row(i).transpose().eval(), frame_in, frame_out);
      }
      return rv_out;
    }

    MatX3 ConvertFrameSpice(Real t_tdb, const MatX3& r_in, Frame frame_in, Frame frame_out) {
      MatX6 rv_in(r_in.rows(), 6);
      rv_in << r_in, MatX3::Zero(r_in.rows(), 3);
      MatX6 rv_out = ConvertFrameSpice(t_tdb, rv_in, frame_in, frame_out);
      return rv_out.leftCols(3);
    }

    MatX6 ConvertFrameSpice(VecX t_tdb, const Vec6& rv_in, Frame frame_in, Frame frame_out) {
      MatX6 rv_out(t_tdb.size(), 6);
      for (int i = 0; i < t_tdb.size(); i++) {
        rv_out.row(i) = ConvertFrameSpice(t_tdb(i), rv_in, frame_in, frame_out);
      }
      return rv_out;
    }

    MatX3 ConvertFrameSpice(VecX t_tdb, const Vec3& r_in, Frame frame_in, Frame frame_out) {
      MatX6 rv_in(t_tdb.size(), 6);
      rv_in << r_in, MatX3::Zero(t_tdb.size(), 3);
      MatX6 rv_out = ConvertFrameSpice(t_tdb, rv_in, frame_in, frame_out);
      return rv_out.leftCols(3);
    }

    MatX6 ConvertFrameSpice(VecX t_tdb, const MatX6& rv_in, Frame frame_in, Frame frame_out) {
      if (t_tdb.size() != rv_in.rows())
        throw std::runtime_error("Epoch and rv_in must have same size");
      MatX6 rv_out(t_tdb.size(), 6);
      for (int i = 0; i < t_tdb.size(); i++) {
        rv_out.row(i)
            = ConvertFrameSpice(t_tdb(i), rv_in.row(i).transpose().eval(), frame_in, frame_out);
      }
      return rv_out;
    }

    MatX3 ConvertFrameSpice(VecX t_tdb, const MatX3& r_in, Frame frame_in, Frame frame_out) {
      if (t_tdb.size() != r_in.rows())
        throw std::runtime_error("Epoch and r_in must have same size");
      MatX6 rv_in(t_tdb.size(), 6);
      rv_in << r_in, MatX3::Zero(t_tdb.size(), 3);
      MatX6 rv_out = ConvertFrameSpice(t_tdb, rv_in, frame_in, frame_out);
      return rv_out.leftCols(3);
    }

    Mat6 RotOp2Mi(Real t_tdb) {
      Vec6 rv_earth_icrf = GetBodyPosVel(t_tdb, BodyId::SUN, BodyId::EARTH);
      Vec6 rv_moon_icrf = GetBodyPosVel(t_tdb, BodyId::SUN, BodyId::MOON);

      // IAU pole
      Vec3 iau_pole = Vec3::UnitZ();
      Vec3 iau_pole_icrf = ConvertFrame(t_tdb, iau_pole, Frame::MOON_PA, Frame::GCRF);

      // MOON_OP unit vectors
      Vec3 dr = rv_earth_icrf.head(3) - rv_moon_icrf.head(3);
      Vec3 dv = rv_earth_icrf.tail(3) - rv_moon_icrf.tail(3);
      Vec3 z_op = dr.cross(dv).normalized();
      Vec3 x_op = iau_pole_icrf.cross(z_op).normalized();
      Vec3 y_op = z_op.cross(x_op).normalized();

      // create rotation matrix from MOON_OP to MOON_CI
      Mat3 R_op2mi;
      R_op2mi << x_op, y_op, z_op;

      Mat6 R_op2mi_tot = Mat6::Zero();
      R_op2mi_tot.topLeftCorner(3, 3) = R_op2mi;
      R_op2mi_tot.bottomRightCorner(3, 3) = R_op2mi;

      return R_op2mi_tot.cast<double>();
    }
    Vec6 ConvertFrameSpice(Real t_tdb, const Vec6& rv_in, Frame frame_in, Frame frame_out) {
      if (frame_in == frame_out) {
        return rv_in;
      }

      switch (frame_in) {
        case Frame::ITRF: {
          switch (frame_out) {
            case Frame::GCRF: {
              // Convert to GCRF
              Mat6 Rrv_Itrf2Gcrf = GetFrameConversionMat(t_tdb, "ITRF93", "J2000");
              Vec6 rv_gcrf = Rrv_Itrf2Gcrf * rv_in;
              return rv_gcrf;
            }
            default: {  // First convert to GCRF and then to the desired frame
              Vec6 rv_gcrf = ConvertFrameSpice(t_tdb, rv_in, Frame::ITRF, Frame::GCRF);
              Vec6 rv_out = ConvertFrameSpice(t_tdb, rv_gcrf, Frame::GCRF, frame_out);
              return rv_out;
            }
          }
        }

        case Frame::MOON_ME: {
          switch (frame_out) {
            case Frame::MOON_PA: {  // Convert to MOON_PA
              Vec3 r_ME = rv_in.head(3);
              Vec3 v_ME = rv_in.tail(3);

              // Rotation Mat MOON_ME (in DE421) -> MOON_PA (in DE440)
              // Reference:
              // https://iopscience.iop.org/article/10.3847/1538-3881/abd414/pdf
              Mat3 B_M = RotX(-0.2785 * DEG_ARCSEC) * RotY(-78.6944 * DEG_ARCSEC)
                         * RotZ(-67.8526 * DEG_ARCSEC);

              Vec3 r_PA = B_M * r_ME;
              Vec3 v_PA = B_M * v_ME;
              Vec6 rv_PA;
              rv_PA << r_PA, v_PA;
              return rv_PA;
            }
            default: {  // first convert to MOON_PA and then to the desired frame
              Vec6 rv_pa = ConvertFrameSpice(t_tdb, rv_in, Frame::MOON_ME, Frame::MOON_PA);
              Vec6 rv_out = ConvertFrameSpice(t_tdb, rv_pa, Frame::MOON_PA, frame_out);
              return rv_out;
            }
          }
        }

        case Frame::MOON_PA: {
          switch (frame_out) {
            case Frame::MOON_ME: {
              // Rotation Mat MOON_ME (in DE421) -> MOON_PA (in DE440)
              // Reference:
              // https://iopscience.iop.org/article/10.3847/1538-3881/abd414/pdf
              Vec3 r_PA = rv_in.head(3);
              Vec3 v_PA = rv_in.tail(3);
              Mat3 B_M = RotX(-0.2785 * DEG_ARCSEC) * RotY(-78.6944 * DEG_ARCSEC)
                         * RotZ(-67.8526 * DEG_ARCSEC);
              Vec3 r_ME = B_M * r_PA;
              Vec3 v_ME = B_M * v_PA;
              Vec6 rv_me;
              rv_me << r_ME, v_ME;
              return rv_me;
            }
            case Frame::MOON_CI: {  // Convert to Moon Inertial
              Mat6 Mrot = GetFrameConversionMat(t_tdb, "IAU_MOON", "J2000");
              Vec6 rv_mi = Mrot * rv_in;
              return rv_mi;
            }
            default: {  // first convert to MOON_CI and then to the desired frame
              Vec6 rv_mi = ConvertFrameSpice(t_tdb, rv_in, Frame::MOON_PA, Frame::MOON_CI);
              Vec6 rv_out = ConvertFrameSpice(t_tdb, rv_mi, Frame::MOON_CI, frame_out);
              return rv_out;
            }
          }
        }

        case Frame::GCRF: {
          switch (frame_out) {
            case Frame::ICRF: {
              Vec6 rv_icrf_ssb2e
                  = GetBodyPosVel(t_tdb, BodyId::SOLAR_SYSTEM_BARYCENTER, BodyId::EARTH);
              Vec6 rv_icrf = rv_in + rv_icrf_ssb2e;
              return rv_icrf;
            }
            case Frame::ITRF: {
              Mat6 Rrv_GcrfToItrf = GetFrameConversionMat(t_tdb, "J2000", "ITRF93");
              Vec6 rv_itrf = Rrv_GcrfToItrf * rv_in;
              return rv_itrf;
            }
            case Frame::MOON_CI: {
              Vec6 rv_icrf_m2e = GetBodyPosVel(t_tdb, BodyId::MOON, BodyId::EARTH);
              Vec6 rv_mi = rv_in + rv_icrf_m2e;
              return rv_mi;
            }
            case Frame::EMR: {
              Vec6 rv_icrf_emb2e
                  = GetBodyPosVel(t_tdb, BodyId::EARTH_MOON_BARYCENTER, BodyId::EARTH);
              Vec6 rv_emr = InertialToSynodic(rv_icrf_emb2e, rv_in);
              return rv_emr;
            }
            case Frame::MOON_PA:
            case Frame::MOON_ME: {
              Vec6 rv_mi = ConvertFrameSpice(t_tdb, rv_in, Frame::GCRF, Frame::MOON_CI);
              Vec6 rv_out = ConvertFrameSpice(t_tdb, rv_mi, Frame::MOON_CI, frame_out);
              return rv_out;
            }
            default: throw std::runtime_error("Conversion not found");
          }
        }

        case Frame::MOON_CI: {
          switch (frame_out) {
            case Frame::GCRF: {
              Vec6 rv_icrf_e2m = GetBodyPosVel(t_tdb, BodyId::EARTH, BodyId::MOON);
              Vec6 rv_gcrf = rv_in + rv_icrf_e2m;
              return rv_gcrf;
            }
            case Frame::MOON_PA: {
              Mat6 Mrot = GetFrameConversionMat(t_tdb, "J2000", "IAU_MOON");
              Vec6 rv_pa = Mrot * rv_in;
              return rv_pa;
            }
            case Frame::MOON_ME: {
              Vec6 rv_pa = ConvertFrameSpice(t_tdb, rv_in, Frame::MOON_CI, Frame::MOON_PA);
              Vec6 rv_out = ConvertFrameSpice(t_tdb, rv_pa, Frame::MOON_PA, frame_out);
              return rv_out;
            }
            case Frame::MOON_OP: {
              Mat6 R_op2mi = RotOp2Mi(t_tdb);
              Vec6 rv_op = R_op2mi.transpose() * rv_in;
              return rv_op;
            }
            default: {
              Vec6 rv_gcrf = ConvertFrameSpice(t_tdb, rv_in, Frame::MOON_CI, Frame::GCRF);
              Vec6 rv_out = ConvertFrameSpice(t_tdb, rv_gcrf, Frame::GCRF, frame_out);
              return rv_out;
            }
          }
        }

        case Frame::ICRF: {
          switch (frame_out) {
            case Frame::GCRF: {
              Vec6 rv_icrf_ssb2e
                  = GetBodyPosVel(t_tdb, BodyId::SOLAR_SYSTEM_BARYCENTER, BodyId::EARTH);
              Vec6 rv_gcrf = rv_in - rv_icrf_ssb2e;
              return rv_gcrf;
            }
            default: {
              Vec6 rv_gcrf = ConvertFrameSpice(t_tdb, rv_in, Frame::ICRF, Frame::GCRF);
              Vec6 rv_out = ConvertFrameSpice(t_tdb, rv_gcrf, Frame::GCRF, frame_out);
              return rv_out;
            }
          }
        }

        case Frame::EMR: {
          switch (frame_out) {
            case Frame::GCRF: {
              Vec6 rv_icrf_emb2e
                  = GetBodyPosVel(t_tdb, BodyId::EARTH, BodyId::EARTH_MOON_BARYCENTER);
              Vec6 rv_gcrf = SynodicToInertial(rv_icrf_emb2e, rv_in);
              return rv_gcrf;
            }
            default: {
              Vec6 rv_gcrf = ConvertFrameSpice(t_tdb, rv_in, Frame::EMR, Frame::GCRF);
              Vec6 rv_out = ConvertFrameSpice(t_tdb, rv_gcrf, Frame::GCRF, frame_out);
              return rv_out;
            }
          }
        }

        case Frame::MOON_OP: {
          switch (frame_out) {
            case Frame::MOON_CI: {
              Mat6 R_op2mi = RotOp2Mi(t_tdb);
              Vec6 rv_mi = R_op2mi * rv_in;
              return rv_mi;
            }
            default: {
              Vec6 rv_mi = ConvertFrameSpice(t_tdb, rv_in, Frame::MOON_OP, Frame::MOON_CI);
              Vec6 rv_out = ConvertFrameSpice(t_tdb, rv_mi, Frame::MOON_CI, frame_out);
              return rv_out;
            }
          }
        }
        default: throw std::runtime_error("Conversion not found");
      }
      throw std::runtime_error("Conversion not found");
      return Vec6::Zero();
    }

  }  // namespace spice

}  // namespace lupnt