Merge pull request #526 from ut-issl/feature/add-interpolation-library

Add interpolation library

CMakeLists.txt

@@ -232,6 +232,7 @@ if(GOOGLE_TEST)
  src/library/math/test_matrix.cpp
  src/library/math/test_matrix_vector.cpp
  src/library/math/test_s2e_math.cpp
+ src/library/math/test_interpolation.cpp
  src/library/numerical_integration/test_runge_kutta.cpp
  src/library/gravity/test_gravity_potential.cpp
  src/library/gnss/test_sp3_file_reader.cpp

src/library/CMakeLists.txt

@@ -26,6 +26,7 @@ add_library(${PROJECT_NAME} STATIC
  math/quaternion.cpp
  math/vector.cpp
  math/s2e_math.cpp
+ math/interpolation.cpp
 
  optics/gaussian_beam_base.cpp
 

src/library/gnss/sp3_file_reader.hpp

@@ -8,6 +8,8 @@
 #ifndef S2E_LIBRARY_GNSS_SP3_FILE_READER_HPP_
 #define S2E_LIBRARY_GNSS_SP3_FILE_READER_HPP_
 
+#include <stdint.h>
+
 #include <library/math/vector.hpp>
 #include <library/time_system/date_time_format.hpp>
 #include <library/time_system/gps_time.hpp>

src/library/initialize/c2a_command_database.hpp

@@ -6,6 +6,8 @@
 #ifndef S2E_LIBRARY_INITIALIZE_C2A_COMMAND_DATABASE_HPP_
 #define S2E_LIBRARY_INITIALIZE_C2A_COMMAND_DATABASE_HPP_
 
+#include <stdint.h>
+
 #include <map>
 #include <string>
 #include <vector>

src/library/math/interpolation.cpp

@@ -0,0 +1,86 @@
+/**
+ * @file interpolation.cpp
+ * @brief Implementation of mathematical interpolation method
+ */
+
+#include "interpolation.hpp"
+
+#include <cmath>
+
+namespace libra {
+
+double Interpolation::CalcPolynomial(const double x) {
+ // Search nearest point
+ size_t nearest_x_id = FindNearestPoint(x);
+
+ // Neville's algorithm
+ double y_output = dependent_variables_[nearest_x_id];
+ std::vector<double> down_diff = dependent_variables_;
+ std::vector<double> up_diff = dependent_variables_;
+ size_t d_idx = 1;
+ for (size_t m = 1; m < degree_; m++) {
+ // Calculate C and D
+ for (size_t i = 0; i < degree_ - m; i++) {
+ double denominator = independent_variables_[i] - independent_variables_[i + m];
+ double down_minus_up = down_diff[i + 1] - up_diff[i];
+ up_diff[i] = (independent_variables_[i + m] - x) * down_minus_up / denominator;
+ down_diff[i] = (independent_variables_[i] - x) * down_minus_up / denominator;
+ }
+
+ // Upstream first calculation
+ double dy;
+ if (nearest_x_id >= m) {
+ dy = up_diff[nearest_x_id - d_idx];
+ d_idx++;
+ } else {
+ dy = down_diff[0];
+ }
+ y_output += dy;
+ }
+
+ return y_output;
+}
+
+double Interpolation::CalcTrigonometric(const double x, const double period) {
+ double y_output = 0.0;
+ size_t end_id = degree_;
+ size_t start_id = 0;
+
+ // Modify to odd number degrees
+ // TODO: implement more efficient method
+ if (degree_ % 2 == 0) {
+ size_t nearest_point = FindNearestPoint(x);
+ // Remove the farthest point
+ if (nearest_point * 2 < degree_) {
+ end_id--;
+ } else {
+ start_id++;
+ }
+ }
+
+ for (size_t i = start_id; i < end_id; ++i) {
+ double t_k = 1.0;
+ for (size_t j = start_id; j < end_id; ++j) {
+ if (i == j) continue;
+ t_k *= sin(period * (x - independent_variables_[j]) / 2.0) / sin(period * (independent_variables_[i] - independent_variables_[j]) / 2.0);
+ }
+ y_output += t_k * dependent_variables_[i];
+ }
+
+ return y_output;
+}
+
+size_t Interpolation::FindNearestPoint(const double x) {
+ size_t output = 0;
+ double difference1 = fabs(x - independent_variables_[0]);
+ for (size_t i = 0; i < degree_; i++) {
+ double difference2 = fabs(x - independent_variables_[i]);
+ if (difference2 < difference1) {
+ difference1 = difference2;
+ output = i;
+ }
+ }
+ return output;
+}
+
+} // namespace libra

src/library/math/interpolation.hpp

@@ -0,0 +1,69 @@
+/**
+ * @file interpolation.hpp
+ * @brief Mathematical interpolation method
+ */
+
+#ifndef S2E_LIBRARY_MATH_INTERPOLATION_HPP_
+#define S2E_LIBRARY_MATH_INTERPOLATION_HPP_
+
+#include <iostream>
+#include <vector>
+
+namespace libra {
+
+/**
+ * @class Interpolation
+ * @brief Class for interpolation calculation
+ */
+class Interpolation {
+ public:
+ /**
+ * @fn Interpolation
+ * @brief Constructor without any initialization
+ * @note The size of independent variables automatically set as degree of interpolation
+ * @param[in] independent_variables: Set of independent variables
+ * @param[in] dependent_variables: Set of independent variables
+ */
+ inline Interpolation(std::vector<double> independent_variables, std::vector<double> dependent_variables)
+ : independent_variables_(independent_variables), dependent_variables_(dependent_variables) {
+ degree_ = independent_variables_.size();
+ if (degree_ < 2) {
+ std::cout << "[WARNINGS] Interpolation degree is smaller than 2" << std::endl;
+ }
+ }
+
+ /**
+ * @fn CalcPolynomial
+ * @brief Calculate polynomial interpolation with Neville's algorithm
+ * @note Ref: Numerical Recipes in C, Section. 3.1
+ * @param [in] x: Target independent variable
+ * @return Interpolated value at x
+ */
+ double CalcPolynomial(const double x);
+
+ /**
+ * @fn CalcTrigonometric
+ * @brief Calculate trigonometric interpolation
+ * @param [in] x: Target independent variable
+ * @param [in] period: Characteristic period
+ * @return Interpolated value at x
+ */
+ double CalcTrigonometric(const double x, const double period = 1.0);
+
+ private:
+ std::vector<double> independent_variables_{0.0};
+ std::vector<double> dependent_variables_{0.0};
+ size_t degree_;
+
+ /**
+ * @fn FindNearestPoint
+ * @brief Find nearest independent variables index
+ * @param [in] x: Target independent variable
+ * @return Index of the nearest independent variables
+ */
+ size_t FindNearestPoint(const double x);
+};
+
+} // namespace libra
+
+#endif // S2E_LIBRARY_MATH_INTERPOLATION_HPP_

src/library/math/test_interpolation.cpp

@@ -0,0 +1,92 @@
+/**
+ * @file test_interpolation.cpp
+ * @brief Test codes for Interpolation class with GoogleTest
+ */
+#include <gtest/gtest.h>
+
+#include <cmath>
+
+#include "constants.hpp"
+#include "interpolation.hpp"
+
+/**
+ * @brief Test for linear function with polynomial interpolation
+ */
+TEST(Interpolation, PolynomialLinearFunction) {
+ std::vector<double> x{0.0, 1.0, 2.0, 3.0, 4.0};
+ std::vector<double> y{0.0, 2.0, 4.0, 6.0, 8.0};
+ libra::Interpolation interpolation(x, y);
+
+ double xx = 0.4;
+ EXPECT_DOUBLE_EQ(2.0 * xx, interpolation.CalcPolynomial(xx));
+ xx = 2.6;
+ EXPECT_DOUBLE_EQ(2.0 * xx, interpolation.CalcPolynomial(xx));
+ xx = 3.6;
+ EXPECT_DOUBLE_EQ(2.0 * xx, interpolation.CalcPolynomial(xx));
+}
+
+/**
+ * @brief Test for quadratic function with polynomial interpolation
+ */
+TEST(Interpolation, PolynomialQuadraticFunction) {
+ std::vector<double> x{0.0, 1.0, 2.0, 3.0, 4.0};
+ std::vector<double> y{0.0, 1.0, 4.0, 9.0, 16.0};
+ libra::Interpolation interpolation(x, y);
+
+ double xx = 0.4;
+ EXPECT_DOUBLE_EQ(pow(xx, 2.0), interpolation.CalcPolynomial(xx));
+ xx = 2.4;
+ EXPECT_DOUBLE_EQ(pow(xx, 2.0), interpolation.CalcPolynomial(xx));
+ xx = 3.8;
+ EXPECT_DOUBLE_EQ(pow(xx, 2.0), interpolation.CalcPolynomial(xx));
+}
+
+/**
+ * @brief Test for sin function with trigonometric interpolation
+ */
+TEST(Interpolation, TrigonometricSinFunction) {
+ std::vector<double> x{0.0, libra::pi_2, libra::pi, libra::pi * 3.0 / 2.0, libra::tau};
+ std::vector<double> y;
+ for (size_t i = 0; i < x.size(); i++) {
+ y.push_back(sin(x[i]));
+ }
+ libra::Interpolation interpolation(x, y);
+
+ double xx = 0.4 * libra::pi;
+ EXPECT_DOUBLE_EQ(sin(xx), interpolation.CalcTrigonometric(xx));
+ xx = 1.4 * libra::pi;
+ EXPECT_DOUBLE_EQ(sin(xx), interpolation.CalcTrigonometric(xx));
+}
+
+/**
+ * @brief Test for cos function with trigonometric interpolation
+ */
+TEST(Interpolation, TrigonometricCosFunction) {
+ std::vector<double> x{0.0, 0.3 * libra::pi_2, 0.6 * libra::pi_2, 0.9 * libra::pi_2, 1.2 * libra::pi_2};
+ std::vector<double> y;
+ for (size_t i = 0; i < x.size(); i++) {
+ y.push_back(cos(x[i]));
+ }
+ libra::Interpolation interpolation(x, y);
+
+ double xx = 0.1 * libra::pi_2;
+ EXPECT_NEAR(cos(xx), interpolation.CalcTrigonometric(xx), 1e-6);
+ xx = 0.8 * libra::pi_2;
+ EXPECT_NEAR(cos(xx), interpolation.CalcTrigonometric(xx), 1e-6);
+}
+/**
+ * @brief Test for cos function with trigonometric interpolation
+ */
+TEST(Interpolation, TrigonometricCosSinFunctionOddDegree) {
+ std::vector<double> x{0.0, 0.3 * libra::pi_2, 0.6 * libra::pi_2, 0.9 * libra::pi_2, 1.2 * libra::pi_2, 1.5 * libra::pi_2};
+ std::vector<double> y;
+ for (size_t i = 0; i < x.size(); i++) {
+ y.push_back(cos(x[i]) + sin(x[i]));
+ }
+ libra::Interpolation interpolation(x, y);
+
+ double xx = 0.1 * libra::pi_2;
+ EXPECT_NEAR(cos(xx) + sin(xx), interpolation.CalcTrigonometric(xx), 1e-6);
+ xx = 0.8 * libra::pi_2;
+ EXPECT_NEAR(cos(xx) + sin(xx), interpolation.CalcTrigonometric(xx), 1e-6);
+}