feat(tier4_autoware_utils): faster sin and cos (#7120)

* feat(tier4_autoware_utils): faster sin and cos

Signed-off-by: veqcc <ryuta.kambe@tier4.jp>

Author: veqcc

common/tier4_autoware_utils/include/tier4_autoware_utils/math/trigonometry.hpp

@@ -15,13 +15,17 @@
 #ifndef TIER4_AUTOWARE_UTILS__MATH__TRIGONOMETRY_HPP_
 #define TIER4_AUTOWARE_UTILS__MATH__TRIGONOMETRY_HPP_
 
+#include <utility>
+
 namespace tier4_autoware_utils
 {
 
 float sin(float radian);
 
 float cos(float radian);
 
+std::pair<float, float> sin_and_cos(float radian);
+
 }  // namespace tier4_autoware_utils
 
 #endif  // TIER4_AUTOWARE_UTILS__MATH__TRIGONOMETRY_HPP_

common/tier4_autoware_utils/src/math/trigonometry.cpp

@@ -49,4 +49,27 @@ float cos(float radian)
   return sin(radian + static_cast<float>(tier4_autoware_utils::pi) / 2.f);
 }
 
+std::pair<float, float> sin_and_cos(float radian)
+{
+  constexpr float tmp =
+    (180.f / static_cast<float>(tier4_autoware_utils::pi)) * (discrete_arcs_num_360 / 360.f);
+  const float degree = radian * tmp;
+  size_t idx =
+    (static_cast<int>(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

common/tier4_autoware_utils/test/src/math/test_trigonometry.cpp

@@ -40,3 +40,13 @@ TEST(trigonometry, cos)
         tier4_autoware_utils::cos(x * static_cast<float>(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<float>(i));
+    EXPECT_TRUE(std::abs(std::sin(x * static_cast<float>(i)) - sin_and_cos.first) < 10e-7);
+    EXPECT_TRUE(std::abs(std::cos(x * static_cast<float>(i)) - sin_and_cos.second) < 10e-7);
+  }
+}