diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/math/trigonometry.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/math/trigonometry.hpp index 0c53a9e3941dd..4901e28472acb 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/math/trigonometry.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/math/trigonometry.hpp @@ -15,6 +15,8 @@ #ifndef TIER4_AUTOWARE_UTILS__MATH__TRIGONOMETRY_HPP_ #define TIER4_AUTOWARE_UTILS__MATH__TRIGONOMETRY_HPP_ +#include + namespace tier4_autoware_utils { @@ -22,6 +24,8 @@ float sin(float radian); float cos(float radian); +std::pair sin_and_cos(float radian); + } // namespace tier4_autoware_utils #endif // TIER4_AUTOWARE_UTILS__MATH__TRIGONOMETRY_HPP_ diff --git a/common/tier4_autoware_utils/src/math/trigonometry.cpp b/common/tier4_autoware_utils/src/math/trigonometry.cpp index 15f5c71012722..0ce65c7aa5bc8 100644 --- a/common/tier4_autoware_utils/src/math/trigonometry.cpp +++ b/common/tier4_autoware_utils/src/math/trigonometry.cpp @@ -49,4 +49,27 @@ float cos(float radian) return sin(radian + static_cast(tier4_autoware_utils::pi) / 2.f); } +std::pair sin_and_cos(float radian) +{ + constexpr float tmp = + (180.f / static_cast(tier4_autoware_utils::pi)) * (discrete_arcs_num_360 / 360.f); + const float degree = radian * tmp; + size_t idx = + (static_cast(std::round(degree)) % discrete_arcs_num_360 + discrete_arcs_num_360) % + discrete_arcs_num_360; + + if (idx < discrete_arcs_num_90) { + return {g_sin_table[idx], g_sin_table[discrete_arcs_num_90 - idx]}; + } else if (discrete_arcs_num_90 <= idx && idx < 2 * discrete_arcs_num_90) { + idx = 2 * discrete_arcs_num_90 - idx; + return {g_sin_table[idx], -g_sin_table[discrete_arcs_num_90 - idx]}; + } else if (2 * discrete_arcs_num_90 <= idx && idx < 3 * discrete_arcs_num_90) { + idx = idx - 2 * discrete_arcs_num_90; + return {-g_sin_table[idx], -g_sin_table[discrete_arcs_num_90 - idx]}; + } else { // 3 * discrete_arcs_num_90 <= idx && idx < 4 * discrete_arcs_num_90 + idx = 4 * discrete_arcs_num_90 - idx; + return {-g_sin_table[idx], g_sin_table[discrete_arcs_num_90 - idx]}; + } +} + } // namespace tier4_autoware_utils diff --git a/common/tier4_autoware_utils/test/src/math/test_trigonometry.cpp b/common/tier4_autoware_utils/test/src/math/test_trigonometry.cpp index 379418539841c..d7106fd823682 100644 --- a/common/tier4_autoware_utils/test/src/math/test_trigonometry.cpp +++ b/common/tier4_autoware_utils/test/src/math/test_trigonometry.cpp @@ -40,3 +40,13 @@ TEST(trigonometry, cos) tier4_autoware_utils::cos(x * static_cast(i))) < 10e-5); } } + +TEST(trigonometry, sin_and_cos) +{ + float x = 4.f * tier4_autoware_utils::pi / 128.f; + for (int i = 0; i < 128; i++) { + const auto sin_and_cos = tier4_autoware_utils::sin_and_cos(x * static_cast(i)); + EXPECT_TRUE(std::abs(std::sin(x * static_cast(i)) - sin_and_cos.first) < 10e-7); + EXPECT_TRUE(std::abs(std::cos(x * static_cast(i)) - sin_and_cos.second) < 10e-7); + } +}