Merge pull request #448 from ut-issl/feature/refactor-geopotential

Refactor geopotential

src/disturbances/geopotential.cpp

@@ -24,8 +24,7 @@ Geopotential::Geopotential(const int degree, const std::string file_path, const
  // degree
  if (degree_ > 360) {
  degree_ = 360;
- std::cout << "Inputted degree of Geopotential is too large for EGM96 "
- "model(limit is 360)\n";
+ std::cout << "Inputted degree of Geopotential is too large for EGM96 model(limit is 360)\n";
  std::cout << "degree of Geopotential set as " << degree_ << "\n";
  } else if (degree_ <= 1) {
  degree_ = 0;
@@ -42,17 +41,19 @@ Geopotential::Geopotential(const int degree, const std::string file_path, const
  std::cout << "degree of Geopotential set as " << degree_ << "\n";
  }
  }
+ // Initialize GravityPotential
+ geopotential_ = GravityPotential(degree, c_, s_);
 }
 
 bool Geopotential::ReadCoefficientsEgm96(std::string file_name) {
  std::ifstream coeff_file(file_name);
  if (!coeff_file.is_open()) {
- std::cerr << "file open error:Geopotential\n";
+ std::cerr << "File open error: Geopotential\n";
  return false;
  }
 
- int num_coeff = ((degree_ + 1) * (degree_ + 2) / 2) - 3; //-3 for C00,C10,C11
- for (int i = 0; i < num_coeff; i++) {
+ size_t num_coeff = ((degree_ + 1) * (degree_ + 2) / 2) - 3; //-3 for C00,C10,C11
+ for (size_t i = 0; i < num_coeff; i++) {
  int n, m;
  double c_nm_norm, s_nm_norm;
  std::string line;
@@ -73,7 +74,7 @@ void Geopotential::Update(const LocalEnvironment &local_environment, const Dynam
  debug_pos_ecef_m_ = spacecraft.dynamics_->orbit_->GetPosition_ecef_m();
 #endif
 
- CalcAccelerationEcef(dynamics.GetOrbit().GetPosition_ecef_m());
+ acceleration_ecef_m_s2_ = geopotential_.CalcAcceleration_xcxf_m_s2(dynamics.GetOrbit().GetPosition_ecef_m());
 #ifdef DEBUG_GEOPOTENTIAL
  end = chrono::system_clock::now();
  time_ms_ = static_cast<double>(chrono::duration_cast<chrono::microseconds>(end - start).count() / 1000.0);
@@ -86,113 +87,6 @@ void Geopotential::Update(const LocalEnvironment &local_environment, const Dynam
  acceleration_i_m_s2_ = trans_ecef2eci * acceleration_ecef_m_s2_;
 }
 
-void Geopotential::CalcAccelerationEcef(const libra::Vector<3> &position_ecef_m) {
- ecef_x_m_ = position_ecef_m[0];
- ecef_y_m_ = position_ecef_m[1];
- ecef_z_m_ = position_ecef_m[2];
- radius_m_ = sqrt(ecef_x_m_ * ecef_x_m_ + ecef_y_m_ * ecef_y_m_ + ecef_z_m_ * ecef_z_m_);
-
- // Calc V and W
- int degree_vw = degree_ + 1;
- std::vector<std::vector<double>> v(degree_vw + 1, std::vector<double>(degree_vw + 1, 0.0));
- std::vector<std::vector<double>> w(degree_vw + 1, std::vector<double>(degree_vw + 1, 0.0));
- // n=m=0
- v[0][0] = environment::earth_equatorial_radius_m / radius_m_;
- w[0][0] = 0.0;
- m_ = 0;
-
- while (m_ < degree_vw) {
- for (n_ = m_ + 1; n_ <= degree_vw; n_++) {
- if (n_ <= m_ + 1)
- v_w_nm_update(&v[n_][m_], &w[n_][m_], v[n_ - 1][m_], w[n_ - 1][m_], 0.0, 0.0);
- else
- v_w_nm_update(&v[n_][m_], &w[n_][m_], v[n_ - 1][m_], w[n_ - 1][m_], v[n_ - 2][m_], w[n_ - 2][m_]);
- }
- // next step
- m_++;
- n_ = m_;
- v_w_nn_update(&v[n_][m_], &w[n_][m_], v[n_ - 1][m_ - 1], w[n_ - 1][m_ - 1]);
- }
-
- // Calc Acceleration
- acceleration_ecef_m_s2_ *= 0.0;
- for (n_ = 0; n_ <= degree_; n_++) // this loop can integrate with previous loop
- {
- m_ = 0;
- double n_d = (double)n_;
- double normalize = sqrt((2.0 * n_d + 1.0) / (2.0 * n_d + 3.0));
- double normalize_xy = normalize * sqrt((n_d + 2.0) * (n_d + 1.0) / 2.0);
- // m_==0
- acceleration_ecef_m_s2_[0] += -c_[n_][0] * v[n_ + 1][1] * normalize_xy;
- acceleration_ecef_m_s2_[1] += -c_[n_][0] * w[n_ + 1][1] * normalize_xy;
- acceleration_ecef_m_s2_[2] += (n_ + 1.0) * (-c_[n_][0] * v[n_ + 1][0] - s_[n_][0] * w[n_ + 1][0]) * normalize;
- for (m_ = 1; m_ <= n_; m_++) {
- double m_d = (double)m_;
- double factorial = (n_d - m_d + 1.0) * (n_d - m_d + 2.0);
- double normalize_xy1 = normalize * sqrt((n_d + m_d + 1.0) * (n_d + m_d + 2.0));
- double normalize_xy2;
- if (m_ == 1)
- normalize_xy2 = normalize * sqrt(factorial) * sqrt(2.0);
- else
- normalize_xy2 = normalize * sqrt(factorial);
- double normalize_z = normalize * sqrt((n_d + m_d + 1.0) / (n_d - m_d + 1.0));
-
- acceleration_ecef_m_s2_[0] += 0.5 * (normalize_xy1 * (-c_[n_][m_] * v[n_ + 1][m_ + 1] - s_[n_][m_] * w[n_ + 1][m_ + 1]) +
- normalize_xy2 * (c_[n_][m_] * v[n_ + 1][m_ - 1] + s_[n_][m_] * w[n_ + 1][m_ - 1]));
- acceleration_ecef_m_s2_[1] += 0.5 * (normalize_xy1 * (-c_[n_][m_] * w[n_ + 1][m_ + 1] + s_[n_][m_] * v[n_ + 1][m_ + 1]) +
- normalize_xy2 * (-c_[n_][m_] * w[n_ + 1][m_ - 1] + s_[n_][m_] * v[n_ + 1][m_ - 1]));
- acceleration_ecef_m_s2_[2] += (n_d - m_d + 1.0) * (-c_[n_][m_] * v[n_ + 1][m_] - s_[n_][m_] * w[n_ + 1][m_]) * normalize_z;
- }
- }
- acceleration_ecef_m_s2_ *=
- environment::earth_gravitational_constant_m3_s2 / (environment::earth_equatorial_radius_m * environment::earth_equatorial_radius_m);
-
- return;
-}
-
-void Geopotential::v_w_nn_update(double *v_nn, double *w_nn, const double v_prev, const double w_prev) {
- if (n_ != m_) return;
-
- double n_d = (double)n_;
-
- double tmp = environment::earth_equatorial_radius_m / (radius_m_ * radius_m_);
- double x_tmp = ecef_x_m_ * tmp;
- double y_tmp = ecef_y_m_ * tmp;
- double c_normalize;
- if (n_ == 1)
- c_normalize = (2.0 * n_d - 1.0) * sqrt(2.0 * n_d + 1.0);
- else
- c_normalize = sqrt((2.0 * n_d + 1.0) / (2.0 * n_d));
-
- *v_nn = c_normalize * (x_tmp * v_prev - y_tmp * w_prev);
- *w_nn = c_normalize * (x_tmp * w_prev + y_tmp * v_prev);
- return;
-}
-
-void Geopotential::v_w_nm_update(double *v_nm, double *w_nm, const double v_prev, const double w_prev, const double v_prev2, const double w_prev2) {
- if (n_ == m_) return;
-
- double m_d = (double)m_;
- double n_d = (double)n_;
-
- double tmp = environment::earth_equatorial_radius_m / (radius_m_ * radius_m_);
- double z_tmp = ecef_z_m_ * tmp;
- double re_tmp = environment::earth_equatorial_radius_m * tmp;
- double c1 = (2.0 * n_d - 1.0) / (n_d - m_d);
- double c2 = (n_d + m_d - 1.0) / (n_d - m_d);
- double c_normalize, c2_normalize;
-
- c_normalize = sqrt(((2.0 * n_d + 1.0) * (n_d - m_d)) / ((2.0 * n_d - 1.0) * (n_d + m_d)));
- if (n_ <= 1)
- c2_normalize = 1.0;
- else
- c2_normalize = sqrt(((2.0 * n_d - 1.0) * (n_d - m_d - 1.0)) / ((2.0 * n_d - 3.0) * (n_d + m_d - 1.0)));
-
- *v_nm = c_normalize * (c1 * z_tmp * v_prev - c2 * c2_normalize * re_tmp * v_prev2);
- *w_nm = c_normalize * (c1 * z_tmp * w_prev - c2 * c2_normalize * re_tmp * w_prev2);
- return;
-}
-
 std::string Geopotential::GetLogHeader() const {
  std::string str_tmp = "";
 

src/disturbances/geopotential.hpp

@@ -8,12 +8,9 @@
 
 #include <string>
 
-#include "../library/logger/loggable.hpp"
-#include "../library/math/matrix.hpp"
-#include "../library/math/matrix_vector.hpp"
+#include "../library/gravity/gravity_potential.hpp"
 #include "../library/math/vector.hpp"
 #include "disturbance.hpp"
-
 /**
  * @class Geopotential
  * @brief Class to calculate the high-order earth gravity acceleration
@@ -29,6 +26,19 @@ class Geopotential : public Disturbance {
  */
  Geopotential(const int degree, const std::string file_path, const bool is_calculation_enabled = true);
 
+ /**
+ * @fn Geopotential
+ * @brief Copy Constructor
+ */
+ Geopotential(const Geopotential &obj) {
+ geopotential_ = obj.geopotential_;
+ degree_ = obj.degree_;
+ c_ = obj.c_;
+ s_ = obj.s_;
+ }
+
+ ~Geopotential() {}
+
  /**
  * @fn Update
  * @brief Override Updates function of SimpleDisturbance
@@ -50,47 +60,22 @@ class Geopotential : public Disturbance {
  virtual std::string GetLogValue() const;
 
  private:
- int degree_; //!< Maximum degree setting to calculate the geo-potential
- int n_ = 0, m_ = 0;  //!< Degree and order (FIXME: follow naming rule)
+ GravityPotential geopotential_;
+ size_t degree_;  //!< Maximum degree setting to calculate the geo-potential
  std::vector<std::vector<double>> c_; //!< Cosine coefficients
  std::vector<std::vector<double>> s_; //!< Sine coefficients
  Vector<3> acceleration_ecef_m_s2_; //!< Calculated acceleration in the ECEF frame [m/s2]
 
- // calculation
- double radius_m_ = 0.0; //!< Radius [m]
- double ecef_x_m_ = 0.0, ecef_y_m_ = 0.0, ecef_z_m_ = 0.0; //!< Spacecraft position in ECEF frame [m]
-
  // debug
  libra::Vector<3> debug_pos_ecef_m_; //!< Spacecraft position in ECEF frame [m]
  double time_ms_ = 0.0; //!< Calculation time [ms]
 
- /**
- * @fn CalcAccelerationEcef
- * @brief Calculate the high-order earth gravity in the ECEF frame
- * @param [in] position_ecef_m: Position of the spacecraft in the ECEF fram [m]
- */
- void CalcAccelerationEcef(const Vector<3> &position_ecef_m);
-
  /**
  * @fn ReadCoefficientsEgm96
  * @brief Read the geo-potential coefficients for the EGM96 model
  * @param [in] file_name: Coefficient file name
  */
  bool ReadCoefficientsEgm96(std::string file_name);
-
- /**
- * @fn v_w_nn_update
- * @brief Calculate V and W function for n = m
- * @note FIXME: fix function name
- */
- void v_w_nn_update(double *v_nn, double *w_nn, const double v_prev, const double w_prev);
-
- /**
- * @fn v_w_nm_update
- * @brief Calculate V and W function for n not equal m
- * @note FIXME: fix function name
- */
- void v_w_nm_update(double *v_nm, double *w_nm, const double v_prev, const double w_prev, const double v_prev2, const double w_prev2);
 };
 
 #endif // S2E_DISTURBANCES_GEOPOTENTIAL_HPP_

src/library/CMakeLists.txt

@@ -12,6 +12,8 @@ add_library(${PROJECT_NAME} STATIC
  logger/logger.cpp
  logger/initialize_log.cpp
 
+ gravity/gravity_potential.cpp
+
  randomization/global_randomization.cpp
  randomization/normal_randomization.cpp
  randomization/minimal_standard_linear_congruential_generator.cpp

src/library/gravity/gravity_potential.cpp

@@ -0,0 +1,143 @@
+/**
+ * @file gravity_potential.cpp
+ * @brief Class to calculate gravity potential
+ */
+
+#include "gravity_potential.hpp"
+
+#include <chrono>
+#include <cmath>
+#include <fstream>
+#include <iostream>
+
+#include "../library/logger/log_utility.hpp"
+#include "../library/utilities/macros.hpp"
+
+GravityPotential::GravityPotential(const size_t degree, const std::vector<std::vector<double>> cosine_coefficients,
+ const std::vector<std::vector<double>> sine_coefficients, const double gravity_constants_m3_s2,
+ const double center_body_radius_m)
+ : degree_(degree),
+ c_(cosine_coefficients),
+ s_(sine_coefficients),
+ gravity_constants_m3_s2_(gravity_constants_m3_s2),
+ center_body_radius_m_(center_body_radius_m) {
+ // degree
+ if (degree_ <= 1) {
+ degree_ = 0;
+ }
+ // coefficients
+ // TODO Check size
+}
+
+libra::Vector<3> GravityPotential::CalcAcceleration_xcxf_m_s2(const libra::Vector<3> &position_xcxf_m) {
+ libra::Vector<3> acceleration_xcxf_m_s2(0.0);
+ if (degree_ <= 0) return acceleration_xcxf_m_s2;
+
+ xcxf_x_m_ = position_xcxf_m[0];
+ xcxf_y_m_ = position_xcxf_m[1];
+ xcxf_z_m_ = position_xcxf_m[2];
+ radius_m_ = position_xcxf_m.CalcNorm();
+
+ // Calc V and W
+ const size_t degree_vw = degree_ + 1;
+ std::vector<std::vector<double>> v(degree_vw + 1, std::vector<double>(degree_vw + 1, 0.0));
+ std::vector<std::vector<double>> w(degree_vw + 1, std::vector<double>(degree_vw + 1, 0.0));
+ // n = m = 0
+ v[0][0] = center_body_radius_m_ / radius_m_;
+ w[0][0] = 0.0;
+ m_ = 0;
+
+ while (m_ < degree_vw) {
+ for (n_ = m_ + 1; n_ <= degree_vw; n_++) {
+ if (n_ <= m_ + 1) {
+ v_w_nm_update(&v[n_][m_], &w[n_][m_], v[n_ - 1][m_], w[n_ - 1][m_], 0.0, 0.0);
+ } else {
+ v_w_nm_update(&v[n_][m_], &w[n_][m_], v[n_ - 1][m_], w[n_ - 1][m_], v[n_ - 2][m_], w[n_ - 2][m_]);
+ }
+ }
+ // next step
+ m_++;
+ n_ = m_;
+ v_w_nn_update(&v[n_][m_], &w[n_][m_], v[n_ - 1][m_ - 1], w[n_ - 1][m_ - 1]);
+ }
+
+ // Calc Acceleration
+ for (n_ = 0; n_ <= degree_; n_++) // this loop can integrate with previous loop
+ {
+ m_ = 0;
+ const double n_d = (double)n_;
+ const double normalize = sqrt((2.0 * n_d + 1.0) / (2.0 * n_d + 3.0));
+ const double normalize_xy = normalize * sqrt((n_d + 2.0) * (n_d + 1.0) / 2.0);
+ // m_==0
+ acceleration_xcxf_m_s2[0] += -c_[n_][0] * v[n_ + 1][1] * normalize_xy;
+ acceleration_xcxf_m_s2[1] += -c_[n_][0] * w[n_ + 1][1] * normalize_xy;
+ acceleration_xcxf_m_s2[2] += (n_ + 1.0) * (-c_[n_][0] * v[n_ + 1][0] - s_[n_][0] * w[n_ + 1][0]) * normalize;
+ for (m_ = 1; m_ <= n_; m_++) {
+ const double m_d = (double)m_;
+ const double factorial = (n_d - m_d + 1.0) * (n_d - m_d + 2.0);
+ const double normalize_xy1 = normalize * sqrt((n_d + m_d + 1.0) * (n_d + m_d + 2.0));
+ double normalize_xy2;
+ if (m_ == 1) {
+ normalize_xy2 = normalize * sqrt(factorial) * sqrt(2.0);
+ } else {
+ normalize_xy2 = normalize * sqrt(factorial);
+ }
+ const double normalize_z = normalize * sqrt((n_d + m_d + 1.0) / (n_d - m_d + 1.0));
+
+ acceleration_xcxf_m_s2[0] += 0.5 * (normalize_xy1 * (-c_[n_][m_] * v[n_ + 1][m_ + 1] - s_[n_][m_] * w[n_ + 1][m_ + 1]) +
+ normalize_xy2 * (c_[n_][m_] * v[n_ + 1][m_ - 1] + s_[n_][m_] * w[n_ + 1][m_ - 1]));
+ acceleration_xcxf_m_s2[1] += 0.5 * (normalize_xy1 * (-c_[n_][m_] * w[n_ + 1][m_ + 1] + s_[n_][m_] * v[n_ + 1][m_ + 1]) +
+ normalize_xy2 * (-c_[n_][m_] * w[n_ + 1][m_ - 1] + s_[n_][m_] * v[n_ + 1][m_ - 1]));
+ acceleration_xcxf_m_s2[2] += (n_d - m_d + 1.0) * (-c_[n_][m_] * v[n_ + 1][m_] - s_[n_][m_] * w[n_ + 1][m_]) * normalize_z;
+ }
+ }
+ acceleration_xcxf_m_s2 *= gravity_constants_m3_s2_ / pow(center_body_radius_m_, 2.0);
+
+ return acceleration_xcxf_m_s2;
+}
+
+void GravityPotential::v_w_nn_update(double *v_nn, double *w_nn, const double v_prev, const double w_prev) {
+ if (n_ != m_) return;
+
+ const double n_d = (double)n_;
+
+ const double tmp = center_body_radius_m_ / pow(radius_m_, 2.0);
+ const double x_tmp = xcxf_x_m_ * tmp;
+ const double y_tmp = xcxf_y_m_ * tmp;
+ double c_normalize;
+ if (n_ == 1) {
+ c_normalize = (2.0 * n_d - 1.0) * sqrt(2.0 * n_d + 1.0);
+ } else {
+ c_normalize = sqrt((2.0 * n_d + 1.0) / (2.0 * n_d));
+ }
+
+ *v_nn = c_normalize * (x_tmp * v_prev - y_tmp * w_prev);
+ *w_nn = c_normalize * (x_tmp * w_prev + y_tmp * v_prev);
+ return;
+}
+
+void GravityPotential::v_w_nm_update(double *v_nm, double *w_nm, const double v_prev, const double w_prev, const double v_prev2,
+ const double w_prev2) {
+ if (n_ == m_) return;
+
+ const double m_d = (double)m_;
+ const double n_d = (double)n_;
+
+ const double tmp = center_body_radius_m_ / pow(radius_m_, 2.0);
+ const double z_tmp = xcxf_z_m_ * tmp;
+ const double re_tmp = center_body_radius_m_ * tmp;
+ const double c1 = (2.0 * n_d - 1.0) / (n_d - m_d);
+ const double c2 = (n_d + m_d - 1.0) / (n_d - m_d);
+ double c_normalize, c2_normalize;
+
+ c_normalize = sqrt(((2.0 * n_d + 1.0) * (n_d - m_d)) / ((2.0 * n_d - 1.0) * (n_d + m_d)));
+ if (n_ <= 1) {
+ c2_normalize = 1.0;
+ } else {
+ c2_normalize = sqrt(((2.0 * n_d - 1.0) * (n_d - m_d - 1.0)) / ((2.0 * n_d - 3.0) * (n_d + m_d - 1.0)));
+ }
+
+ *v_nm = c_normalize * (c1 * z_tmp * v_prev - c2 * c2_normalize * re_tmp * v_prev2);
+ *w_nm = c_normalize * (c1 * z_tmp * w_prev - c2 * c2_normalize * re_tmp * w_prev2);
+ return;
+}