From f5573e2e923f275cfd6005556cdd96fb9b23142b Mon Sep 17 00:00:00 2001 From: veqcc Date: Fri, 24 May 2024 19:52:44 +0900 Subject: [PATCH 1/4] feat(tier4_autoware_utils): faster sin and cos Signed-off-by: veqcc --- .../math/trigonometry.hpp | 4 +++ .../src/math/trigonometry.cpp | 28 +++++++++++++++++++ .../test/src/math/test_trigonometry.cpp | 13 +++++++++ 3 files changed, 45 insertions(+) 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..e79fc8bc6d35b 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..7007e0487bf63 100644 --- a/common/tier4_autoware_utils/src/math/trigonometry.cpp +++ b/common/tier4_autoware_utils/src/math/trigonometry.cpp @@ -49,4 +49,32 @@ 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; + + float sin, cos; + if (idx < discrete_arcs_num_90) { + sin = g_sin_table[idx]; + cos = 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; + sin = g_sin_table[idx]; + cos = -1.f * 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; + sin = -1.f * g_sin_table[idx]; + cos = -1.f * 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; + sin = -1.f * g_sin_table[idx]; + cos = g_sin_table[discrete_arcs_num_90 - idx]; + } + + return std::make_pair(sin, cos); +} + } // 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..18ac751e4691d 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,16 @@ 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); + } +} + From 071cfacf4c273ed75ec9ac137bba1677f5020362 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 24 May 2024 10:59:07 +0000 Subject: [PATCH 2/4] style(pre-commit): autofix --- .../include/tier4_autoware_utils/math/trigonometry.hpp | 2 +- common/tier4_autoware_utils/src/math/trigonometry.cpp | 8 +++++--- .../test/src/math/test_trigonometry.cpp | 7 ++----- 3 files changed, 8 insertions(+), 9 deletions(-) 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 e79fc8bc6d35b..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,7 +15,7 @@ #ifndef TIER4_AUTOWARE_UTILS__MATH__TRIGONOMETRY_HPP_ #define TIER4_AUTOWARE_UTILS__MATH__TRIGONOMETRY_HPP_ -#include +#include namespace tier4_autoware_utils { diff --git a/common/tier4_autoware_utils/src/math/trigonometry.cpp b/common/tier4_autoware_utils/src/math/trigonometry.cpp index 7007e0487bf63..7ff4bf0577a23 100644 --- a/common/tier4_autoware_utils/src/math/trigonometry.cpp +++ b/common/tier4_autoware_utils/src/math/trigonometry.cpp @@ -49,8 +49,10 @@ 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); +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) % @@ -68,7 +70,7 @@ std::pair sin_and_cos(float radian) { idx = idx - 2 * discrete_arcs_num_90; sin = -1.f * g_sin_table[idx]; cos = -1.f * g_sin_table[discrete_arcs_num_90 - idx]; - } else { // 3 * discrete_arcs_num_90 <= idx && idx < 4 * discrete_arcs_num_90 + } else { // 3 * discrete_arcs_num_90 <= idx && idx < 4 * discrete_arcs_num_90 idx = 4 * discrete_arcs_num_90 - idx; sin = -1.f * g_sin_table[idx]; cos = g_sin_table[discrete_arcs_num_90 - idx]; 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 18ac751e4691d..d7106fd823682 100644 --- a/common/tier4_autoware_utils/test/src/math/test_trigonometry.cpp +++ b/common/tier4_autoware_utils/test/src/math/test_trigonometry.cpp @@ -46,10 +46,7 @@ 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); + 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); } } - From c0e00ad3dee53228023bb0a804dcd3c557610ced Mon Sep 17 00:00:00 2001 From: veqcc Date: Mon, 27 May 2024 09:39:04 +0900 Subject: [PATCH 3/4] refactor Signed-off-by: veqcc --- .../src/math/trigonometry.cpp | 17 +++++------------ 1 file changed, 5 insertions(+), 12 deletions(-) diff --git a/common/tier4_autoware_utils/src/math/trigonometry.cpp b/common/tier4_autoware_utils/src/math/trigonometry.cpp index 7ff4bf0577a23..7a995bd602fe7 100644 --- a/common/tier4_autoware_utils/src/math/trigonometry.cpp +++ b/common/tier4_autoware_utils/src/math/trigonometry.cpp @@ -58,25 +58,18 @@ std::pair sin_and_cos(float radian) (static_cast(std::round(degree)) % discrete_arcs_num_360 + discrete_arcs_num_360) % discrete_arcs_num_360; - float sin, cos; if (idx < discrete_arcs_num_90) { - sin = g_sin_table[idx]; - cos = g_sin_table[discrete_arcs_num_90 - idx]; + 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; - sin = g_sin_table[idx]; - cos = -1.f * g_sin_table[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; - sin = -1.f * g_sin_table[idx]; - cos = -1.f * g_sin_table[discrete_arcs_num_90 - idx]; - } else { // 3 * discrete_arcs_num_90 <= idx && idx < 4 * 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; - sin = -1.f * g_sin_table[idx]; - cos = g_sin_table[discrete_arcs_num_90 - idx]; + return {- g_sin_table[idx], g_sin_table[discrete_arcs_num_90 - idx]}; } - - return std::make_pair(sin, cos); } } // namespace tier4_autoware_utils From 7f0cebd31f1d3e78593cdc076b83e414beea0fcc Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 27 May 2024 00:41:13 +0000 Subject: [PATCH 4/4] style(pre-commit): autofix --- common/tier4_autoware_utils/src/math/trigonometry.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/common/tier4_autoware_utils/src/math/trigonometry.cpp b/common/tier4_autoware_utils/src/math/trigonometry.cpp index 7a995bd602fe7..0ce65c7aa5bc8 100644 --- a/common/tier4_autoware_utils/src/math/trigonometry.cpp +++ b/common/tier4_autoware_utils/src/math/trigonometry.cpp @@ -62,13 +62,13 @@ std::pair sin_and_cos(float radian) 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]}; + 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 + 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]}; + return {-g_sin_table[idx], g_sin_table[discrete_arcs_num_90 - idx]}; } }