feat: ssa template overload for containers (#18)

Author: Andeshog

cpp_test/test_math.cpp

@@ -25,6 +25,43 @@ TEST(ssa, test_ssa_minus_3_5) {
     EXPECT_NEAR(2.78, ssa(-3.5), 0.01);
 }
 
+TEST(ssa, test_ssa_minus_pi) {
+    EXPECT_EQ(M_PI, ssa(-M_PI));
+}
+
+TEST(ssa, test_ssa_pi) {
+    EXPECT_EQ(M_PI, ssa(M_PI));
+}
+
+TEST(ssa, test_ssa_vector) {
+    Eigen::VectorXd angles(6);
+    angles << 0.0, M_PI / 2, M_PI, 3 * M_PI, -4 * M_PI, -3 * M_PI / 2.0;
+
+    Eigen::VectorXd expected(6);
+    expected << 0.0, M_PI / 2, M_PI, M_PI, 0.0, M_PI / 2.0;
+
+    Eigen::VectorXd result = ssa(angles);
+    EXPECT_TRUE(result.isApprox(expected, 1e-12));
+
+    std::vector<double> angle_vec{0.0,      M_PI / 2,  M_PI,
+                                  3 * M_PI, -4 * M_PI, -3 * M_PI / 2.0};
+    std::vector<double> expected_vec{0.0,  M_PI / 2, M_PI,
+                                     M_PI, 0.0,      M_PI / 2.0};
+    std::vector<double> result_vec = ssa(angle_vec);
+    for (size_t i = 0; i < angle_vec.size(); ++i) {
+        EXPECT_NEAR(expected_vec[i], result_vec[i], 1e-12);
+    }
+
+    std::array<double, 6> angle_array{0.0,      M_PI / 2,  M_PI,
+                                      3 * M_PI, -4 * M_PI, -3 * M_PI / 2.0};
+    std::array<double, 6> expected_array{0.0,  M_PI / 2, M_PI,
+                                         M_PI, 0.0,      M_PI / 2.0};
+    std::array<double, 6> result_array = ssa(angle_array);
+    for (size_t i = 0; i < angle_array.size(); ++i) {
+        EXPECT_NEAR(expected_array[i], result_array[i], 1e-12);
+    }
+}
+
 // Test that the skew-symmetric matrix is correctly calculated
 TEST(get_skew_symmetric_matrix, test_skew_symmetric) {
     Eigen::Vector3d vector(1, 2, 3);

include/vortex/utils/math.hpp

@@ -2,19 +2,48 @@
 #define VORTEX_UTILS_MATH_HPP
 
 #include <cmath>
+#include <concepts>
 #include <eigen3/Eigen/Dense>
 #include <eigen3/Eigen/Geometry>
 
 namespace vortex::utils::math {
 
 /**
- * @brief Function to calculate the smallest signed angle between two angles.
- * Maps the angle to the interval [-pi, pi].
+ * @brief Function to calculate the smallest signed angle.
+ * Maps the angle to the interval (-pi, pi].
  * @param angle The input angle in radians.
  * @return The smallest signed angle in radians.
  */
 double ssa(const double angle);
 
+/**
+ * @brief Function to calculate the smallest signed angles in a container.
+ * Maps the angles to the interval [-pi, pi]. Works with containers like
+ * std::vector, std::array, Eigen::VectorXd etc.
+ * @param angles The input angles in radians.
+ * @return The smallest signed angles in radians.
+ */
+template <typename T>
+concept VectorLike = !std::is_arithmetic_v<std::decay_t<T>> && requires(T t) {
+    { *std::begin(t) } -> std::convertible_to<double>;
+};
+template <VectorLike Container>
+auto ssa(Container&& angles_in) {
+    using ContainerT = std::decay_t<Container>;
+    using std::begin;
+
+    ContainerT angles = std::forward<Container>(angles_in);
+
+    using value_type = std::decay_t<decltype(*begin(angles))>;
+    static_assert(std::is_floating_point_v<value_type>);
+
+    for (auto& a : angles) {
+        a = static_cast<value_type>(ssa(static_cast<double>(a)));
+    }
+
+    return angles;
+}
+
 /**
  * @brief Calculates the skew-symmetric matrix from a 3D vector.
  * @param vector Eigen::Vector3d

include/vortex/utils/types.hpp

@@ -60,6 +60,15 @@ struct Eta {
         return Eigen::Vector<double, 6>{x, y, z, roll, pitch, yaw};
     }
 
+    /**
+     * @brief Apply smallest signed angle to roll, pitch, and yaw.
+     */
+    void apply_ssa() {
+        roll = vortex::utils::math::ssa(roll);
+        pitch = vortex::utils::math::ssa(pitch);
+        yaw = vortex::utils::math::ssa(yaw);
+    }
+
     /**
      * @brief Make the rotation matrix corresponding to eq. 2.31 in Fossen, 2021
      * @return Eigen::Matrix3d rotation matrix

src/math.cpp

@@ -4,7 +4,7 @@ namespace vortex::utils::math {
 
 double ssa(const double angle) {
     double angle_ssa{fmod(angle + M_PI, 2 * M_PI)};
-    return angle_ssa < 0 ? angle_ssa + M_PI : angle_ssa - M_PI;
+    return angle_ssa <= 0 ? angle_ssa + M_PI : angle_ssa - M_PI;
 }
 
 Eigen::Matrix3d get_skew_symmetric_matrix(const Eigen::Vector3d& vector) {