diff --git a/src/disturbances/geopotential.cpp b/src/disturbances/geopotential.cpp index 8bacfccc3..c8d4d5195 100644 --- a/src/disturbances/geopotential.cpp +++ b/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(chrono::duration_cast(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> v(degree_vw + 1, std::vector(degree_vw + 1, 0.0)); - std::vector> w(degree_vw + 1, std::vector(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 = ""; diff --git a/src/disturbances/geopotential.hpp b/src/disturbances/geopotential.hpp index 2bdd06ee2..df6603815 100644 --- a/src/disturbances/geopotential.hpp +++ b/src/disturbances/geopotential.hpp @@ -8,12 +8,9 @@ #include -#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> c_; //!< Cosine coefficients std::vector> 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_ diff --git a/src/library/CMakeLists.txt b/src/library/CMakeLists.txt index 3501f87d0..e3298444b 100644 --- a/src/library/CMakeLists.txt +++ b/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 diff --git a/src/library/gravity/gravity_potential.cpp b/src/library/gravity/gravity_potential.cpp new file mode 100644 index 000000000..c9edee159 --- /dev/null +++ b/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 +#include +#include +#include + +#include "../library/logger/log_utility.hpp" +#include "../library/utilities/macros.hpp" + +GravityPotential::GravityPotential(const size_t degree, const std::vector> cosine_coefficients, + const std::vector> 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> v(degree_vw + 1, std::vector(degree_vw + 1, 0.0)); + std::vector> w(degree_vw + 1, std::vector(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; +} diff --git a/src/library/gravity/gravity_potential.hpp b/src/library/gravity/gravity_potential.hpp new file mode 100644 index 000000000..60b7ed9e9 --- /dev/null +++ b/src/library/gravity/gravity_potential.hpp @@ -0,0 +1,77 @@ +/** + * @file gravity_potential.hpp + * @brief Class to calculate gravity potential + */ + +#ifndef S2E_LIBRARY_GRAVITY_GRAVITY_POTENTIAL_HPP_ +#define S2E_LIBRARY_GRAVITY_GRAVITY_POTENTIAL_HPP_ + +#include +#include + +#include "../math/vector.hpp" + +/** + * @class GravityPotential + * @brief Class to calculate gravity potential + */ +class GravityPotential { + public: + /** + * @fn GravityPotential + * @brief Constructor + */ + GravityPotential(const double gravity_constants_m3_s2 = environment::earth_gravitational_constant_m3_s2, + const double center_body_radius_m = environment::earth_equatorial_radius_m) + : gravity_constants_m3_s2_(gravity_constants_m3_s2), center_body_radius_m_(center_body_radius_m) {} + /** + * @fn GravityPotential + * @brief Constructor + * @param [in] degree: Maximum degree setting to calculate the geo-potential + */ + GravityPotential(const size_t degree, const std::vector> cosine_coefficients, + const std::vector> sine_coefficients, + const double gravity_constants_m3_s2 = environment::earth_gravitational_constant_m3_s2, + const double center_body_radius_m = environment::earth_equatorial_radius_m); + /** + * @fn ~GravityPotential + * @brief Destructor + */ + ~GravityPotential() {} + + /** + * @fn CalcAcceleration_xcxf_m_s2 + * @brief Calculate the high-order earth gravity in the XCXF frame (Arbitrary celestial body centered and fixed frame) + * @param [in] position_xcxf_m: Position of the spacecraft in the XCXF fram [m] + * @return Acceleration in XCXF frame [m/s2] + */ + libra::Vector<3> CalcAcceleration_xcxf_m_s2(const libra::Vector<3> &position_xcxf_m); + + private: + size_t degree_ = 0; //!< Maximum degree + size_t n_ = 0, m_ = 0; //!< Degree and order (FIXME: follow naming rule) + std::vector> c_; //!< Cosine coefficients + std::vector> s_; //!< Sine coefficients + double gravity_constants_m3_s2_; //!< Gravity constant of the center body [m3/s2] + double center_body_radius_m_; //!< Radius of the center body [m] + + // calculation + double radius_m_ = 0.0; //!< Radius [m] + double xcxf_x_m_ = 0.0, xcxf_y_m_ = 0.0, xcxf_z_m_ = 0.0; //!< Spacecraft position in XCXF frame [m] + + /** + * @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_LIBRARY_GRAVITY_GRAVITY_POTENTIAL_HPP_