Merge pull request #40 from ut-issl/feature/add-relative-orbital-elem…

…ents

Add library for relative orbital elements

s2e-ff/CMakeLists.txt

@@ -95,6 +95,9 @@ set(SOURCE_FILES
   src/Library/math/DualQuaternion.cpp
   src/Library/math/RotationFirstDualQuaternion.cpp
   src/Library/math/TranslationFirstDualQuaternion.cpp
+  src/Library/Orbit/QuasiNonsingularOrbitalElements.cpp
+  src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp
+  src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp
   src/Simulation/Case/FfCase.cpp
   src/Simulation/Spacecraft/FfSat.cpp
   src/Simulation/Spacecraft/FfComponents.cpp
@@ -199,12 +202,21 @@ if(GOOGLE_TEST)
   ## Unit test
   set(TEST_PROJECT_NAME ${PROJECT_NAME}_TEST)
   set(TEST_FILES
+    # Dual Quaternion
     src/Library/math/DualQuaternion.cpp
     test/TestDualQuaternion.cpp
     src/Library/math/RotationFirstDualQuaternion.cpp
     test/TestRotationFirstDualQuaternion.cpp
     src/Library/math/TranslationFirstDualQuaternion.cpp
     test/TestTranslationFirstDualQuaternion.cpp
+    # Orbital elements
+    src/Library/Orbit/QuasiNonsingularOrbitalElements.cpp
+    test/TestQuasiNonsingularOrbitalElements.cpp
+    # Relative orbital elements
+    src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp
+    test/TestQuasiNonsingularOrbitalElementDifferences.cpp
+    src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp
+    test/TestQuasiNonsingularRelativeOrbitalElements.cpp
   )
   add_executable(${TEST_PROJECT_NAME} ${TEST_FILES})
   target_link_libraries(${TEST_PROJECT_NAME} gtest gtest_main)

s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.cpp

@@ -0,0 +1,99 @@
+/**
+ * @file QuasiNonsingularOrbitalElements.cpp
+ * @brief Orbital elements avoid singularity when the eccentricity is near zero.
+ */
+
+#include "QuasiNonsingularOrbitalElements.hpp"
+
+#include <cfloat>
+
+QuasiNonsingularOrbitalElements::QuasiNonsingularOrbitalElements() {
+  semi_major_axis_m_ = 0.0;
+  true_latitude_angle_rad_ = 0.0;
+  eccentricity_x_ = 0.0;
+  eccentricity_y_ = 0.0;
+  inclination_rad_ = 0.0;
+  raan_rad_ = 0.0;
+  CalcOrbitParameters();
+}
+
+QuasiNonsingularOrbitalElements::QuasiNonsingularOrbitalElements(const double semi_major_axis_m, const double eccentricity_x,
+                                                                 const double eccentricity_y, const double inclination_rad, const double raan_rad,
+                                                                 const double mean_arg_latitude_rad)
+    : semi_major_axis_m_(semi_major_axis_m),
+      eccentricity_x_(eccentricity_x),
+      eccentricity_y_(eccentricity_y),
+      inclination_rad_(inclination_rad),
+      raan_rad_(raan_rad),
+      true_latitude_angle_rad_(mean_arg_latitude_rad) {
+  CalcOrbitParameters();
+}
+
+QuasiNonsingularOrbitalElements::QuasiNonsingularOrbitalElements(const double mu_m3_s2, const libra::Vector<3> position_i_m,
+                                                                 const libra::Vector<3> velocity_i_m_s) {
+  // common variables
+  double r_m = norm(position_i_m);
+  double v2_m2_s2 = inner_product(velocity_i_m_s, velocity_i_m_s);
+  libra::Vector<3> h;
+  h = outer_product(position_i_m, velocity_i_m_s);
+  double h_norm = norm(h);
+
+  // semi major axis
+  semi_major_axis_m_ = mu_m3_s2 / (2.0 * mu_m3_s2 / r_m - v2_m2_s2);
+
+  // inclination
+  libra::Vector<3> h_direction = h;
+  h_direction = normalize(h_direction);
+  inclination_rad_ = acos(h_direction[2]);
+  inclination_rad_ = libra::WrapTo2Pi(inclination_rad_);
+
+  // RAAN
+  double norm_h = sqrt(h[0] * h[0] + h[1] * h[1]);
+  if (norm_h < 0.0 + DBL_EPSILON) {
+    // We cannot define raan when i = 0
+    raan_rad_ = 0.0;
+  } else {
+    raan_rad_ = asin(h[0] / sqrt(h[0] * h[0] + h[1] * h[1]));
+  }
+  raan_rad_ = libra::WrapTo2Pi(raan_rad_);
+
+  // position in plane
+  double x_p_m = position_i_m[0] * cos(raan_rad_) + position_i_m[1] * sin(raan_rad_);
+  double tmp_m = -position_i_m[0] * sin(raan_rad_) + position_i_m[1] * cos(raan_rad_);
+  double y_p_m = tmp_m * cos(inclination_rad_) + position_i_m[2] * sin(inclination_rad_);
+
+  // velocity in plane
+  double dx_p_m_s = velocity_i_m_s[0] * cos(raan_rad_) + velocity_i_m_s[1] * sin(raan_rad_);
+  double dtmp_m_s = -velocity_i_m_s[0] * sin(raan_rad_) + velocity_i_m_s[1] * cos(raan_rad_);
+  double dy_p_m_s = dtmp_m_s * cos(inclination_rad_) + velocity_i_m_s[2] * sin(inclination_rad_);
+
+  // eccentricity
+  eccentricity_x_ = (h_norm / mu_m3_s2) * dy_p_m_s - x_p_m / r_m;
+  eccentricity_y_ = -(h_norm / mu_m3_s2) * dx_p_m_s - y_p_m / r_m;
+
+  // true anomaly f_rad and eccentric anomaly u_rad
+  true_latitude_angle_rad_ = atan2(y_p_m, x_p_m);
+  true_latitude_angle_rad_ = libra::WrapTo2Pi(true_latitude_angle_rad_);
+
+  CalcOrbitParameters();
+}
+
+QuasiNonsingularOrbitalElements ::~QuasiNonsingularOrbitalElements() {}
+
+QuasiNonsingularOrbitalElements operator-(const QuasiNonsingularOrbitalElements lhs, const QuasiNonsingularOrbitalElements rhs) {
+  double semi_major_axis_m = lhs.GetSemiMajor_m() - rhs.GetSemiMajor_m();
+  double eccentricity_x = lhs.GetEccentricityX() - rhs.GetEccentricityX();
+  double eccentricity_y = lhs.GetEccentricityY() - rhs.GetEccentricityY();
+  double inclination_rad = lhs.GetInclination_rad() - rhs.GetInclination_rad();
+  double raan_rad = lhs.GetRaan_rad() - rhs.GetRaan_rad();
+  double true_latitude_angle_rad = lhs.GetTrueLatAng_rad() - rhs.GetTrueLatAng_rad();
+
+  QuasiNonsingularOrbitalElements out(semi_major_axis_m, eccentricity_x, eccentricity_y, inclination_rad, raan_rad, true_latitude_angle_rad);
+
+  return out;
+}
+
+void QuasiNonsingularOrbitalElements::CalcOrbitParameters() {
+  semi_latus_rectum_m_ = semi_major_axis_m_ * (1.0 - (eccentricity_x_ * eccentricity_x_ + eccentricity_y_ * eccentricity_y_));
+  radius_m_ = semi_latus_rectum_m_ / (1.0 + eccentricity_x_ * cos(true_latitude_angle_rad_) + eccentricity_y_ * sin(true_latitude_angle_rad_));
+}

s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.hpp

@@ -0,0 +1,123 @@
+/**
+ * @file QuasiNonsingularOrbitalElements.hpp
+ * @brief Orbital elements avoid singularity when the eccentricity is near zero.
+ */
+
+#ifndef QUASI_NONSINGULAR_ORBITAL_ELEMENTS_H_
+#define QUASI_NONSINGULAR_ORBITAL_ELEMENTS_H_
+
+#include <Library/math/Vector.hpp>
+#include <Library/math/s2e_math.hpp>
+
+/**
+ * @class QuasiNonsingularOrbitalElements
+ * @brief Orbital elements avoid singularity when the eccentricity is near zero.
+ */
+class QuasiNonsingularOrbitalElements {
+ public:
+  /**
+   * @fn QuasiNonsingularOrbitalElements
+   * @brief Default Constructor
+   */
+  QuasiNonsingularOrbitalElements();
+  /**
+   * @fn QuasiNonsingularOrbitalElements
+   * @brief Constructor initialized with values
+   * @param[in] semi_major_axis_m: Semi major axis [m]
+   * @param[in] eccentricity_x: Eccentricity X component
+   * @param[in] eccentricity_y: Eccentricity Y component
+   * @param[in] inclination_rad: Inclination [rad]
+   * @param[in] raan_rad: Right Ascension of the Ascending Node [rad]
+   * @param[in] true_latitude_angle_rad: True latitude angle [rad]
+   */
+  QuasiNonsingularOrbitalElements(const double semi_major_axis_m, const double eccentricity_x, const double eccentricity_y,
+                                  const double inclination_rad, const double raan_rad, const double true_latitude_angle_rad);
+  /**
+   * @fn QuasiNonsingularOrbitalElements
+   * @brief Constructor initialized with position and velocity
+   * @param[in] mu_m3_s2: Gravity constant [m3/s2]
+   * @param[in] position_i_m: Position vector in the inertial frame [m]
+   * @param[in] velocity_i_m_s: Velocity vector in the inertial frame [m/s]
+   */
+  QuasiNonsingularOrbitalElements(const double mu_m3_s2, const libra::Vector<3> position_i_m, const libra::Vector<3> velocity_i_m_s);
+  /**
+   * @fn ~QuasiNonsingularOrbitalElements
+   * @brief Destructor
+   */
+  ~QuasiNonsingularOrbitalElements();
+
+  // Getter
+  /**
+   * @fn GetSemiMajor_m
+   * @brief Return semi-major axis [m]
+   */
+  inline double GetSemiMajor_m() const { return semi_major_axis_m_; }
+  /**
+   * @fn GetEccentricityX
+   * @brief Return X component of eccentricity vector
+   */
+  inline double GetEccentricityX() const { return eccentricity_x_; }
+  /**
+   * @fn GetEccentricityY
+   * @brief Return Y component of eccentricity vector
+   */
+  inline double GetEccentricityY() const { return eccentricity_y_; }
+  /**
+   * @fn GetInclination_rad
+   * @brief Return inclination [rad]
+   */
+  inline double GetInclination_rad() const { return inclination_rad_; }
+  /**
+   * @fn GetRaan_rad
+   * @brief Return Right Ascension of the Ascending Node [rad]
+   */
+  inline double GetRaan_rad() const { return raan_rad_; }
+  /**
+   * @fn GetTrueLatAng_rad
+   * @brief Return True latitude angle [rad]
+   */
+  inline double GetTrueLatAng_rad() const { return true_latitude_angle_rad_; }
+  /**
+   * @fn GetSemiLatusRectum_m
+   * @brief Return Semi-latus rectum [m]
+   */
+  inline double GetSemiLatusRectum_m() const { return semi_latus_rectum_m_; }
+  /**
+   * @fn GetRadius_m
+   * @brief Return Current radius [m]
+   */
+  inline double GetRadius_m() const { return radius_m_; }
+
+  // Setter
+  /**
+   * @fn SetTrueLAtAng_rad
+   * @brief Set True latitude angle [rad]
+   */
+  inline void GetTrueLatAng_rad(const double true_latitude_angle_rad) { true_latitude_angle_rad_ = libra::WrapTo2Pi(true_latitude_angle_rad); }
+
+ private:
+  double semi_major_axis_m_;        //!< Semi major axis [m]
+  double eccentricity_x_;           //!< e * cos(arg_peri)
+  double eccentricity_y_;           //!< e * sin(arg_peri)
+  double inclination_rad_;          //!< Inclination [rad]
+  double raan_rad_;                 //!< Right Ascension of the Ascending Node [rad]
+  double true_latitude_angle_rad_;  //!< True latitude angle (argment of periapsis + true anomaly) [rad]
+
+  // Orbit states
+  double semi_latus_rectum_m_;  //!< Semi-latus rectum [m]
+  double radius_m_;             //!< Current radius [m]
+
+  /**
+   * @fn CalcOrbitParameters
+   * @brief Calculate parameters for orbit
+   */
+  void CalcOrbitParameters();
+};
+
+/**
+ * @fn Operator -
+ * @brief Calculate subtract of two quasi-nonsingular orbital elements
+ */
+QuasiNonsingularOrbitalElements operator-(const QuasiNonsingularOrbitalElements lhs, const QuasiNonsingularOrbitalElements rhs);
+
+#endif