helper_functions: generalize unwrapping function

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<typename Type>
+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<typename Type>
-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<typename T>

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]);
 	}
 }