From c4bc062714a5dbf2610938d8b1243e04da46bc57 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Fri, 25 Feb 2022 17:22:31 +0100 Subject: [PATCH] helper_functions: generalize unwrapping function --- src/lib/matrix/matrix/helper_functions.hpp | 22 +++++++++++++++++++--- src/lib/matrix/test/unwrap.cpp | 8 ++++---- 2 files changed, 23 insertions(+), 7 deletions(-) diff --git a/src/lib/matrix/matrix/helper_functions.hpp b/src/lib/matrix/matrix/helper_functions.hpp index 780f6a8f13..fa4e5c84ca 100644 --- a/src/lib/matrix/matrix/helper_functions.hpp +++ b/src/lib/matrix/matrix/helper_functions.hpp @@ -124,16 +124,32 @@ Type wrap_2pi(Type x) } /** - * Unwrap angles + * Unwrap value that was wrapped with range [low, high) + * + * @param[in] last_x Last unwrapped value + * @param[in] new_x New value in range + * @param low lower limit of the wrapping range + * @param high upper limit of the wrapping range + * @return New unwrapped value + */ +template +Type unwrap(const Type last_x, const Type new_x, const Type low, const Type high) +{ + return last_x + wrap(new_x - last_x, low, high); +} + +/** + * Unwrap value with range [-π, π) * * @param[in] last_angle Last unwrapped angle [rad] * @param[in] new_angle New angle in [-pi, pi] [rad] + * @param * @return New unwrapped angle [rad] */ template -Type unwrap(const Type last_angle, const Type new_angle) +Type unwrap_pi(const Type last_angle, const Type new_angle) { - return last_angle + wrap_pi(new_angle - last_angle); + return unwrap(last_angle, new_angle, Type(-M_PI), Type(M_PI)); } template diff --git a/src/lib/matrix/test/unwrap.cpp b/src/lib/matrix/test/unwrap.cpp index 7495a0976b..09c24d00ed 100644 --- a/src/lib/matrix/test/unwrap.cpp +++ b/src/lib/matrix/test/unwrap.cpp @@ -19,7 +19,7 @@ TEST(Unwrap, UnwrapFloats) float last_angle = wrapped_angles[0]; for (int i = 1; i < 6; i++) { - last_angle = unwrap(last_angle, wrapped_angles[i]); + last_angle = unwrap_pi(last_angle, wrapped_angles[i]); EXPECT_FLOAT_EQ(last_angle, unwrapped_angles[i]); } @@ -27,7 +27,7 @@ TEST(Unwrap, UnwrapFloats) last_angle = -wrapped_angles[0]; for (int i = 1; i < 6; i++) { - last_angle = unwrap(last_angle, -wrapped_angles[i]); + last_angle = unwrap_pi(last_angle, -wrapped_angles[i]); EXPECT_FLOAT_EQ(last_angle, -unwrapped_angles[i]); } } @@ -48,7 +48,7 @@ TEST(Unwrap, UnwrapDoubles) double last_angle = wrapped_angles[0]; for (int i = 1; i < 6; i++) { - last_angle = unwrap(last_angle, wrapped_angles[i]); + last_angle = unwrap_pi(last_angle, wrapped_angles[i]); EXPECT_DOUBLE_EQ(last_angle, unwrapped_angles[i]); } @@ -56,7 +56,7 @@ TEST(Unwrap, UnwrapDoubles) last_angle = -wrapped_angles[0]; for (int i = 1; i < 6; i++) { - last_angle = unwrap(last_angle, -wrapped_angles[i]); + last_angle = unwrap_pi(last_angle, -wrapped_angles[i]); EXPECT_DOUBLE_EQ(last_angle, -unwrapped_angles[i]); } }