Skip to content

Commit

Permalink
Merge pull request #466 from ut-issl/feature/add-partial-geopotential…
Browse files Browse the repository at this point in the history
…-partial-derivative

Add geopotential partial derivative
  • Loading branch information
200km committed Aug 15, 2023
2 parents ca49352 + 22e7e4a commit d704d49
Show file tree
Hide file tree
Showing 4 changed files with 305 additions and 6 deletions.
1 change: 1 addition & 0 deletions CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -233,6 +233,7 @@ if(GOOGLE_TEST)
src/library/math/test_matrix_vector.cpp
src/library/math/test_s2e_math.cpp
src/library/numerical_integration/test_runge_kutta.cpp
src/library/gravity/test_gravity_potential.cpp
)
add_executable(${TEST_PROJECT_NAME} ${TEST_FILES})
target_link_libraries(${TEST_PROJECT_NAME} gtest gtest_main)
Expand Down
144 changes: 139 additions & 5 deletions src/library/gravity/gravity_potential.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -10,9 +10,6 @@
#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)
Expand All @@ -22,7 +19,7 @@ GravityPotential::GravityPotential(const size_t degree, const std::vector<std::v
gravity_constants_m3_s2_(gravity_constants_m3_s2),
center_body_radius_m_(center_body_radius_m) {
// degree
if (degree_ <= 1) {
if (degree_ <= 1) { // TODO: Consider this assertion is needed
degree_ = 0;
}
// coefficients
Expand All @@ -31,7 +28,7 @@ GravityPotential::GravityPotential(const size_t degree, const std::vector<std::v

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;
if (degree_ <= 0) return acceleration_xcxf_m_s2; // TODO: Consider this assertion is needed

xcxf_x_m_ = position_xcxf_m[0];
xcxf_y_m_ = position_xcxf_m[1];
Expand Down Expand Up @@ -96,6 +93,143 @@ libra::Vector<3> GravityPotential::CalcAcceleration_xcxf_m_s2(const libra::Vecto
return acceleration_xcxf_m_s2;
}

libra::Matrix<3, 3> GravityPotential::CalcPartialDerivative_xcxf_s2(const libra::Vector<3> &position_xcxf_m) {
libra::Matrix<3, 3> partial_derivative(0.0);
if (degree_ <= 0) return partial_derivative;

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_ + 2;
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 partial derivatives
for (n_ = 0; n_ <= degree_; n_++) // this loop can integrate with previous loop
{
const double n_d = (double)n_;

// C_n_0 * V_n+2_m
const double normalize_cn0_v20 = sqrt((2.0 * n_d + 1.0) / (2.0 * n_d + 5.0));
const double normalize_cn0_v21 = normalize_cn0_v20 * sqrt((n_d + 2.0) * (n_d + 3.0) / 2.0);
const double normalize_cn0_v22 = normalize_cn0_v20 * sqrt((n_d + 1.0) * (n_d + 2.0) * (n_d + 3.0) * (n_d + 4.0) / 2.0);

for (m_ = 0; m_ <= n_; m_++) {
const double m_d = (double)m_;

// dx/dx, dx/dy, dy/dy
if (m_ == 0) {
partial_derivative[0][0] +=
0.5 * (c_[n_][0] * v[n_ + 2][2] * normalize_cn0_v22 - c_[n_][0] * v[n_ + 2][0] * (n_d + 1.0) * (n_d + 2.0) * normalize_cn0_v20);
partial_derivative[1][1] +=
0.5 * (-c_[n_][0] * v[n_ + 2][2] * normalize_cn0_v22 - c_[n_][0] * v[n_ + 2][0] * (n_d + 1.0) * (n_d + 2.0) * normalize_cn0_v20);

partial_derivative[0][1] += 0.5 * (c_[n_][0] * w[n_ + 2][2] * normalize_cn0_v22);
} else if (m_ == 1) {
const double normalize_cn1_v21 = normalize_cn0_v20 * sqrt((n_d + 2.0) * (n_d + 3.0) / (n_d * (n_d + 1.0)));
const double normalize_cn1_v21_with_coeff = n_d * (n_d + 1.0) * normalize_cn1_v21;
const double normalize_cn1_v23 = normalize_cn0_v20 * sqrt((n_d + 2.0) * (n_d + 3.0) * (n_d + 4.0) * (n_d + 5.0));

partial_derivative[0][0] += 0.25 * ((c_[n_][1] * v[n_ + 2][3] + s_[n_][1] * w[n_ + 2][3]) * normalize_cn1_v23 -
(3.0 * c_[n_][1] * v[n_ + 2][1] + s_[n_][1] * w[n_ + 2][1]) * normalize_cn1_v21_with_coeff);
partial_derivative[1][1] += 0.25 * ((-c_[n_][1] * v[n_ + 2][3] - s_[n_][1] * w[n_ + 2][3]) * normalize_cn1_v23 -
(c_[n_][1] * v[n_ + 2][1] + 3.0 * s_[n_][1] * w[n_ + 2][1]) * normalize_cn1_v21_with_coeff);

partial_derivative[0][1] += 0.25 * ((c_[n_][1] * w[n_ + 2][3] - s_[n_][1] * v[n_ + 2][3]) * normalize_cn1_v23 -
(c_[n_][1] * w[n_ + 2][1] + s_[n_][1] * v[n_ + 2][1]) * normalize_cn1_v21_with_coeff);
} else if (m_ == 2) {
double normalize_cnm_v2p2 = normalize_cn0_v20 * sqrt((n_d + m_d + 1.0) * (n_d + m_d + 2.0) * (n_d + m_d + 3.0) * (n_d + m_d + 4.0));
double normalize_cnm_v2m2 = normalize_cn0_v20 * sqrt(2.0 / ((n_d - m_d + 1.0) * (n_d - m_d + 2.0) * (n_d - m_d + 3.0) * (n_d - m_d + 4.0)));
double normalize_cnm_v2m2_with_coeff = (n_d - m_d + 1.0) * (n_d - m_d + 2.0) * (n_d - m_d + 3.0) * (n_d - m_d + 4.0) * normalize_cnm_v2m2;
double normalize_cnm_v20 = normalize_cn0_v20 * sqrt((n_d + m_d + 1.0) * (n_d + m_d + 2.0) / ((n_d - m_d + 1.0) * (n_d - m_d + 2.0)));
double normalize_cnm_v20_with_coeff = 2.0 * (n_d - m_d + 1.0) * (n_d - m_d + 2.0) * normalize_cnm_v20;

partial_derivative[0][0] += 0.25 * ((c_[n_][m_] * v[n_ + 2][m_ + 2] + s_[n_][m_] * w[n_ + 2][m_ + 2]) * normalize_cnm_v2p2 -
(c_[n_][m_] * v[n_ + 2][m_] + s_[n_][m_] * w[n_ + 2][m_]) * normalize_cnm_v20_with_coeff +
(c_[n_][m_] * v[n_ + 2][m_ - 2] + s_[n_][m_] * w[n_ + 2][m_ - 2]) * normalize_cnm_v2m2_with_coeff);
partial_derivative[1][1] += 0.25 * ((-c_[n_][m_] * v[n_ + 2][m_ + 2] - s_[n_][m_] * w[n_ + 2][m_ + 2]) * normalize_cnm_v2p2 -
(c_[n_][m_] * v[n_ + 2][m_] + s_[n_][m_] * w[n_ + 2][m_]) * normalize_cnm_v20_with_coeff -
(c_[n_][m_] * v[n_ + 2][m_ - 2] + s_[n_][m_] * w[n_ + 2][m_ - 2]) * normalize_cnm_v2m2_with_coeff);
partial_derivative[0][1] += 0.25 * ((c_[n_][m_] * w[n_ + 2][m_ + 2] - s_[n_][m_] * v[n_ + 2][m_ + 2]) * normalize_cnm_v2p2 +
(-c_[n_][m_] * w[n_ + 2][m_ - 2] + s_[n_][m_] * v[n_ + 2][m_ - 2]) * normalize_cnm_v2m2_with_coeff);
} else {
double normalize_cnm_v2p2 = normalize_cn0_v20 * sqrt((n_d + m_d + 1.0) * (n_d + m_d + 2.0) * (n_d + m_d + 3.0) * (n_d + m_d + 4.0));
double normalize_cnm_v2m2 = normalize_cn0_v20 * sqrt(1.0 / ((n_d - m_d + 1.0) * (n_d - m_d + 2.0) * (n_d - m_d + 3.0) * (n_d - m_d + 4.0)));
double normalize_cnm_v2m2_with_coeff = (n_d - m_d + 1.0) * (n_d - m_d + 2.0) * (n_d - m_d + 3.0) * (n_d - m_d + 4.0) * normalize_cnm_v2m2;
double normalize_cnm_v20 = normalize_cn0_v20 * sqrt((n_d + m_d + 1.0) * (n_d + m_d + 2.0) / ((n_d - m_d + 1.0) * (n_d - m_d + 2.0)));
double normalize_cnm_v20_with_coeff = 2.0 * (n_d - m_d + 1.0) * (n_d - m_d + 2.0) * normalize_cnm_v20;

partial_derivative[0][0] += 0.25 * ((c_[n_][m_] * v[n_ + 2][m_ + 2] + s_[n_][m_] * w[n_ + 2][m_ + 2]) * normalize_cnm_v2p2 -
(c_[n_][m_] * v[n_ + 2][m_] + s_[n_][m_] * w[n_ + 2][m_]) * normalize_cnm_v20_with_coeff +
(c_[n_][m_] * v[n_ + 2][m_ - 2] + s_[n_][m_] * w[n_ + 2][m_ - 2]) * normalize_cnm_v2m2_with_coeff);
partial_derivative[1][1] += 0.25 * ((-c_[n_][m_] * v[n_ + 2][m_ + 2] - s_[n_][m_] * w[n_ + 2][m_ + 2]) * normalize_cnm_v2p2 -
(c_[n_][m_] * v[n_ + 2][m_] + s_[n_][m_] * w[n_ + 2][m_]) * normalize_cnm_v20_with_coeff -
(c_[n_][m_] * v[n_ + 2][m_ - 2] + s_[n_][m_] * w[n_ + 2][m_ - 2]) * normalize_cnm_v2m2_with_coeff);
partial_derivative[0][1] += 0.25 * ((c_[n_][m_] * w[n_ + 2][m_ + 2] - s_[n_][m_] * v[n_ + 2][m_ + 2]) * normalize_cnm_v2p2 +
(-c_[n_][m_] * w[n_ + 2][m_ - 2] + s_[n_][m_] * v[n_ + 2][m_ - 2]) * normalize_cnm_v2m2_with_coeff);
}
// dx/dz, dy/dz
if (m_ == 0) {
partial_derivative[0][2] += (n_d + 1.0) * (c_[n_][0] * v[n_ + 2][1] * normalize_cn0_v21);
partial_derivative[1][2] += (n_d + 1.0) * (c_[n_][0] * w[n_ + 2][1] * normalize_cn0_v21);
} else if (m_ == 1) {
double normalize_cnm_v2p1 = normalize_cn0_v20 * sqrt((n_d + m_d + 1.0) * (n_d + m_d + 2.0) * (n_d + m_d + 3.0) / (n_d - m_d + 1.0));
double normalize_cnm_v2p1_with_coeff = (n_d - m_d + 1.0) * normalize_cnm_v2p1;
double normalize_cnm_v2m1 = normalize_cn0_v20 * sqrt(2.0 * (n_d + m_d + 1.0) / ((n_d - m_d + 1.0) * (n_d - m_d + 2.0) * (n_d - m_d + 3.0)));
double normalize_cnm_v2m1_with_coeff = (n_d - m_d + 1.0) * (n_d - m_d + 2.0) * (n_d - m_d + 3.0) * normalize_cnm_v2m1;

partial_derivative[0][2] += 0.5 * ((+c_[n_][m_] * v[n_ + 2][m_ + 1] + s_[n_][m_] * w[n_ + 2][m_ + 1]) * normalize_cnm_v2p1_with_coeff +
(-c_[n_][m_] * v[n_ + 2][m_ - 1] - s_[n_][m_] * w[n_ + 2][m_ - 1]) * normalize_cnm_v2m1_with_coeff);
partial_derivative[1][2] += 0.5 * ((+c_[n_][m_] * w[n_ + 2][m_ + 1] - s_[n_][m_] * v[n_ + 2][m_ + 1]) * normalize_cnm_v2p1_with_coeff +
(+c_[n_][m_] * w[n_ + 2][m_ - 1] - s_[n_][m_] * v[n_ + 2][m_ - 1]) * normalize_cnm_v2m1_with_coeff);
} else {
double normalize_cnm_v2p1 = normalize_cn0_v20 * sqrt((n_d + m_d + 1.0) * (n_d + m_d + 2.0) * (n_d + m_d + 3.0) / (n_d - m_d + 1.0));
double normalize_cnm_v2p1_with_coeff = (n_d - m_d + 1.0) * normalize_cnm_v2p1;
double normalize_cnm_v2m1 = normalize_cn0_v20 * sqrt((n_d + m_d + 1.0) / ((n_d - m_d + 1.0) * (n_d - m_d + 2.0) * (n_d - m_d + 3.0)));
double normalize_cnm_v2m1_with_coeff = (n_d - m_d + 1.0) * (n_d - m_d + 2.0) * (n_d - m_d + 3.0) * normalize_cnm_v2m1;

partial_derivative[0][2] += 0.5 * ((+c_[n_][m_] * v[n_ + 2][m_ + 1] + s_[n_][m_] * w[n_ + 2][m_ + 1]) * normalize_cnm_v2p1_with_coeff +
(-c_[n_][m_] * v[n_ + 2][m_ - 1] - s_[n_][m_] * w[n_ + 2][m_ - 1]) * normalize_cnm_v2m1_with_coeff);
partial_derivative[1][2] += 0.5 * ((+c_[n_][m_] * w[n_ + 2][m_ + 1] - s_[n_][m_] * v[n_ + 2][m_ + 1]) * normalize_cnm_v2p1_with_coeff +
(+c_[n_][m_] * w[n_ + 2][m_ - 1] - s_[n_][m_] * v[n_ + 2][m_ - 1]) * normalize_cnm_v2m1_with_coeff);
}
// dz/dz
double normalize_cnm_v20 = normalize_cn0_v20 * sqrt((n_d + m_d + 1.0) * (n_d + m_d + 2.0) / ((n_d - m_d + 1.0) * (n_d - m_d + 2.0)));
double normalize_cnm_v20_with_coeff = (n_d - m_d + 1.0) * (n_d - m_d + 2.0) * normalize_cnm_v20;
partial_derivative[2][2] += (c_[n_][m_] * v[n_ + 2][m_] + s_[n_][m_] * w[n_ + 2][m_]) * normalize_cnm_v20_with_coeff;
}
}
// Symmetry property
partial_derivative[1][0] = partial_derivative[0][1];
partial_derivative[2][0] = partial_derivative[0][2];
partial_derivative[2][1] = partial_derivative[1][2];

// Multiply common coefficients
partial_derivative *= gravity_constants_m3_s2_ / pow(center_body_radius_m_, 3.0);

return partial_derivative;
}

void GravityPotential::v_w_nn_update(double *v_nn, double *w_nn, const double v_prev, const double w_prev) {
if (n_ != m_) return;

Expand Down
11 changes: 10 additions & 1 deletion src/library/gravity/gravity_potential.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,7 @@
#include <environment/global/physical_constants.hpp>
#include <vector>

#include "../math/matrix.hpp"
#include "../math/vector.hpp"

/**
Expand Down Expand Up @@ -42,11 +43,19 @@ class 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]
* @param [in] position_xcxf_m: Position of the spacecraft in the XCXF frame [m]
* @return Acceleration in XCXF frame [m/s2]
*/
libra::Vector<3> CalcAcceleration_xcxf_m_s2(const libra::Vector<3> &position_xcxf_m);

/**
* @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 frame [m]
* @return Partial derivative of acceleration in XCXF frame [-/s2]
*/
libra::Matrix<3, 3> CalcPartialDerivative_xcxf_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)
Expand Down
155 changes: 155 additions & 0 deletions src/library/gravity/test_gravity_potential.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,155 @@
/**
* @file test_gravity_potential.cpp
* @brief Test codes for Gravity Potential class with GoogleTest
*/
#include <gtest/gtest.h>

#include "gravity_potential.hpp"

/**
* @brief Test for Acceleration calculation
*/
TEST(GravityPotential, Acceleration) {
const size_t degree = 10;

std::vector<std::vector<double>> c_; //!< Cosine coefficients
std::vector<std::vector<double>> s_; //!< Sine coefficients

// Unit coefficients
c_.assign(degree + 1, std::vector<double>(degree + 1, 1.0));
s_.assign(degree + 1, std::vector<double>(degree + 1, 1.0));

// Initialize GravityPotential
GravityPotential gravity_potential_(degree, c_, s_, 1.0, 1.0);

// Acceleration Calculation check
libra::Vector<3> position_xcxf_m;
libra::Vector<3> acceleration_xcxf_m_s2;
const double accuracy = 1.0e-3;

// Calc Acceleration
position_xcxf_m[0] = 1.0;
position_xcxf_m[1] = 0.0;
position_xcxf_m[2] = 0.0;
acceleration_xcxf_m_s2 = gravity_potential_.CalcAcceleration_xcxf_m_s2(position_xcxf_m);
// Check
EXPECT_NEAR(-100.0252, acceleration_xcxf_m_s2[0], accuracy);
EXPECT_NEAR(93.3516, acceleration_xcxf_m_s2[1], accuracy);
EXPECT_NEAR(41.0375, acceleration_xcxf_m_s2[2], accuracy);

// Calc Acceleration
position_xcxf_m[0] = 1.0;
position_xcxf_m[1] = 1.0;
position_xcxf_m[2] = 1.0;
acceleration_xcxf_m_s2 = gravity_potential_.CalcAcceleration_xcxf_m_s2(position_xcxf_m);
// Check
EXPECT_NEAR(-0.19228, acceleration_xcxf_m_s2[0], accuracy);
EXPECT_NEAR(-2.46144, acceleration_xcxf_m_s2[1], accuracy);
EXPECT_NEAR(0.242614, acceleration_xcxf_m_s2[2], accuracy);
}

/**
* @brief Test for PartialDerivative calculation case 1
*/
TEST(GravityPotential, PartialDerivative1) {
const size_t degree = 10;

std::vector<std::vector<double>> c_; //!< Cosine coefficients
std::vector<std::vector<double>> s_; //!< Sine coefficients

// Unit coefficients
c_.assign(degree + 1, std::vector<double>(degree + 1, 1.0));
s_.assign(degree + 1, std::vector<double>(degree + 1, 1.0));

// Initialize GravityPotential
GravityPotential gravity_potential_(degree, c_, s_, 1.0, 1.0);

// Calculation check
libra::Vector<3> position_xcxf_m;
libra::Matrix<3, 3> partial_derivative_xcxf_s2;
const double accuracy = 1.0e-3;

// Calc Partial Derivative
position_xcxf_m[0] = 1.0;
position_xcxf_m[1] = 1.0;
position_xcxf_m[2] = 1.0;
partial_derivative_xcxf_s2 = gravity_potential_.CalcPartialDerivative_xcxf_s2(position_xcxf_m);

// Calc Acceleration and numerical partial derivatives
double d_r = 1e-9;
libra::Matrix<3, 3> numerical_partial_derivative_xcxf_s2;
for (size_t i = 0; i < 3; i++) {
libra::Vector<3> position_1_xcxf_m = position_xcxf_m;
libra::Vector<3> position_2_xcxf_m = position_xcxf_m;
position_1_xcxf_m[i] = position_xcxf_m[i] - d_r / 2.0;
position_2_xcxf_m[i] = position_xcxf_m[i] + d_r / 2.0;
libra::Vector<3> acceleration_1_xcxf_m_s2 = gravity_potential_.CalcAcceleration_xcxf_m_s2(position_1_xcxf_m);
libra::Vector<3> acceleration_2_xcxf_m_s2 = gravity_potential_.CalcAcceleration_xcxf_m_s2(position_2_xcxf_m);
libra::Vector<3> diff_acceleration_xcxf_m_s2 = acceleration_2_xcxf_m_s2 - acceleration_1_xcxf_m_s2;
for (size_t j = 0; j < 3; j++) {
numerical_partial_derivative_xcxf_s2[i][j] = diff_acceleration_xcxf_m_s2[j] / d_r;
}
}

// Compare numerical and analytical calculation
libra::Matrix<3, 3> diff;
for (size_t i = 0; i < 3; i++) {
for (size_t j = 0; j < 3; j++) {
EXPECT_NEAR(numerical_partial_derivative_xcxf_s2[i][j], partial_derivative_xcxf_s2[i][j], accuracy);
diff[i][j] = numerical_partial_derivative_xcxf_s2[i][j] - partial_derivative_xcxf_s2[i][j];
}
}
}

/**
* @brief Test for PartialDerivative calculation case2
*/
TEST(GravityPotential, PartialDerivative2) {
const size_t degree = 10;

std::vector<std::vector<double>> c_; //!< Cosine coefficients
std::vector<std::vector<double>> s_; //!< Sine coefficients

// Unit coefficients
c_.assign(degree + 1, std::vector<double>(degree + 1, 1.0));
s_.assign(degree + 1, std::vector<double>(degree + 1, 1.0));

// Initialize GravityPotential
GravityPotential gravity_potential_(degree, c_, s_, 1.0, 1.0);

// Calculation check
libra::Vector<3> position_xcxf_m;
libra::Matrix<3, 3> partial_derivative_xcxf_s2;
const double accuracy = 1.0e-3;

// Calc Partial Derivative
position_xcxf_m[0] = 1.0;
position_xcxf_m[1] = 0.0;
position_xcxf_m[2] = 1.0;
partial_derivative_xcxf_s2 = gravity_potential_.CalcPartialDerivative_xcxf_s2(position_xcxf_m);

// Calc Acceleration and numerical partial derivatives
double d_r = 1e-9;
libra::Matrix<3, 3> numerical_partial_derivative_xcxf_s2;
for (size_t i = 0; i < 3; i++) {
libra::Vector<3> position_1_xcxf_m = position_xcxf_m;
libra::Vector<3> position_2_xcxf_m = position_xcxf_m;
position_1_xcxf_m[i] = position_xcxf_m[i] - d_r / 2.0;
position_2_xcxf_m[i] = position_xcxf_m[i] + d_r / 2.0;
libra::Vector<3> acceleration_1_xcxf_m_s2 = gravity_potential_.CalcAcceleration_xcxf_m_s2(position_1_xcxf_m);
libra::Vector<3> acceleration_2_xcxf_m_s2 = gravity_potential_.CalcAcceleration_xcxf_m_s2(position_2_xcxf_m);
libra::Vector<3> diff_acceleration_xcxf_m_s2 = acceleration_2_xcxf_m_s2 - acceleration_1_xcxf_m_s2;
for (size_t j = 0; j < 3; j++) {
numerical_partial_derivative_xcxf_s2[i][j] = diff_acceleration_xcxf_m_s2[j] / d_r;
}
}

// Compare numerical and analytical calculation
libra::Matrix<3, 3> diff;
for (size_t i = 0; i < 3; i++) {
for (size_t j = 0; j < 3; j++) {
EXPECT_NEAR(numerical_partial_derivative_xcxf_s2[i][j], partial_derivative_xcxf_s2[i][j], accuracy);
diff[i][j] = numerical_partial_derivative_xcxf_s2[i][j] - partial_derivative_xcxf_s2[i][j];
}
}
}

0 comments on commit d704d49

Please sign in to comment.