helper_functions: simplify unwrap function

src/lib/matrix/matrix/helper_functions.hpp

@@ -133,15 +133,7 @@ Type wrap_2pi(Type x)
 template<typename Type>
 Type unwrap(const Type last_angle, const Type new_angle)
 {
-	// wrap the last angle in [-pi,pi]
+	return last_angle + wrap_pi(new_angle - last_angle);
-	const Type last_angle_wrapped = matrix::detail::wrap_floating(last_angle, -Type(M_PI), Type(M_PI));
-
-	// use the shortest distance
-	Type delta = new_angle - last_angle_wrapped;
-	delta += ((delta < -Type(M_PI)) - (delta > Type(M_PI))) * Type(2 *
-			M_PI); // adds or subtracts 2*pi if delta out of range
-
-	return delta + last_angle;
 }
 
 template<typename T>