Skip to content

Commit f5573e2

Browse files
committed
feat(tier4_autoware_utils): faster sin and cos
Signed-off-by: veqcc <ryuta.kambe@tier4.jp>
1 parent d5e691e commit f5573e2

File tree

3 files changed

+45
-0
lines changed

3 files changed

+45
-0
lines changed

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

+4
Original file line numberDiff line numberDiff line change
@@ -15,13 +15,17 @@
1515
#ifndef TIER4_AUTOWARE_UTILS__MATH__TRIGONOMETRY_HPP_
1616
#define TIER4_AUTOWARE_UTILS__MATH__TRIGONOMETRY_HPP_
1717

18+
#include<utility>
19+
1820
namespace tier4_autoware_utils
1921
{
2022

2123
float sin(float radian);
2224

2325
float cos(float radian);
2426

27+
std::pair<float, float> sin_and_cos(float radian);
28+
2529
} // namespace tier4_autoware_utils
2630

2731
#endif // TIER4_AUTOWARE_UTILS__MATH__TRIGONOMETRY_HPP_

common/tier4_autoware_utils/src/math/trigonometry.cpp

+28
Original file line numberDiff line numberDiff line change
@@ -49,4 +49,32 @@ float cos(float radian)
4949
return sin(radian + static_cast<float>(tier4_autoware_utils::pi) / 2.f);
5050
}
5151

52+
std::pair<float, float> sin_and_cos(float radian) {
53+
constexpr float tmp = (180.f / static_cast<float>(tier4_autoware_utils::pi)) * (discrete_arcs_num_360 / 360.f);
54+
const float degree = radian * tmp;
55+
size_t idx =
56+
(static_cast<int>(std::round(degree)) % discrete_arcs_num_360 + discrete_arcs_num_360) %
57+
discrete_arcs_num_360;
58+
59+
float sin, cos;
60+
if (idx < discrete_arcs_num_90) {
61+
sin = g_sin_table[idx];
62+
cos = g_sin_table[discrete_arcs_num_90 - idx];
63+
} else if (discrete_arcs_num_90 <= idx && idx < 2 * discrete_arcs_num_90) {
64+
idx = 2 * discrete_arcs_num_90 - idx;
65+
sin = g_sin_table[idx];
66+
cos = -1.f * g_sin_table[discrete_arcs_num_90 - idx];
67+
} else if (2 * discrete_arcs_num_90 <= idx && idx < 3 * discrete_arcs_num_90) {
68+
idx = idx - 2 * discrete_arcs_num_90;
69+
sin = -1.f * g_sin_table[idx];
70+
cos = -1.f * g_sin_table[discrete_arcs_num_90 - idx];
71+
} else { // 3 * discrete_arcs_num_90 <= idx && idx < 4 * discrete_arcs_num_90
72+
idx = 4 * discrete_arcs_num_90 - idx;
73+
sin = -1.f * g_sin_table[idx];
74+
cos = g_sin_table[discrete_arcs_num_90 - idx];
75+
}
76+
77+
return std::make_pair(sin, cos);
78+
}
79+
5280
} // namespace tier4_autoware_utils

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

+13
Original file line numberDiff line numberDiff line change
@@ -40,3 +40,16 @@ TEST(trigonometry, cos)
4040
tier4_autoware_utils::cos(x * static_cast<float>(i))) < 10e-5);
4141
}
4242
}
43+
44+
TEST(trigonometry, sin_and_cos)
45+
{
46+
float x = 4.f * tier4_autoware_utils::pi / 128.f;
47+
for (int i = 0; i < 128; i++) {
48+
const auto sin_and_cos = tier4_autoware_utils::sin_and_cos(x * static_cast<float>(i));
49+
EXPECT_TRUE(
50+
std::abs(std::sin(x * static_cast<float>(i)) - sin_and_cos.first) < 10e-7);
51+
EXPECT_TRUE(
52+
std::abs(std::cos(x * static_cast<float>(i)) - sin_and_cos.second) < 10e-7);
53+
}
54+
}
55+

0 commit comments

Comments
 (0)