Add periodic_mean()

SimoxUtility/CMakeLists.txt

@@ -73,6 +73,7 @@ SET(SOURCES
     math/pose/orthogonalize.cpp
 
     math/periodic/periodic_diff.cpp
+    math/periodic/periodic_mean.cpp
 
     math/SoftMinMax.cpp
 
@@ -153,6 +154,7 @@ SET(INCLUDES
     math/periodic_clamp.h  # Legacy header.
     math/periodic/periodic_clamp.h
     math/periodic/periodic_diff.h
+    math/periodic/periodic_mean.h
 
     math/pose/align_box_orientation.h
     math/pose/check_rotation_matrix.h

SimoxUtility/math/periodic.h

@@ -4,3 +4,4 @@
 
 #include "periodic/periodic_clamp.h"
 #include "periodic/periodic_diff.h"
+#include "periodic/periodic_mean.h"

SimoxUtility/math/periodic/periodic_mean.cpp

+#include "periodic_mean.h"
+
+
+namespace simox
+{
+
+    float math::periodic_mean(const std::vector<float>& samples, float periodLo, float periodHi)
+    {
+        return periodic_mean<float>(samples, periodLo, periodHi);
+    }
+    double math::periodic_mean(const std::vector<double>& samples, double periodLo, double periodHi)
+    {
+        return periodic_mean<double>(samples, periodLo, periodHi);
+    }
+
+}
+

SimoxUtility/math/periodic/periodic_mean.h

+#pragma once
+
+#include <cmath>
+#include <type_traits>
+
+#include <Eigen/Core>
+
+#include <SimoxUtility/math/rescale.h>
+
+#include "periodic_clamp.h"
+
+
+namespace simox::math
+{
+
+    /**
+     * @brief Computes the periodic (cyclic, wrapped) difference `a - b`
+     * over the cyclic interval `[periodLo, periodHi]`.
+     */
+    template <class Float,
+              std::enable_if_t<std::is_floating_point_v<Float>, int> = 0>
+    inline
+    Float periodic_mean(const std::vector<Float>& samples, Float periodLo, Float periodHi)
+    {
+        /* Python: (when samples in [0, 2*pi]
+        nominator = np.sum(np.sin(samples))
+        denominator = np.sum(np.cos(samples))
+        return np.arctan2(nominator, denominator)
+        */
+
+        Float nominator = 0;
+        Float denominator = 0;
+        for (auto sample : samples)
+        {
+            auto scaled = rescale(sample, periodLo, periodHi, Float(0), Float(2*M_PI));
+            nominator += std::sin(scaled);
+            denominator += std::cos(scaled);
+        }
+        Float result = std::atan2(nominator, denominator);
+        return rescale(result, Float(0), Float(2*M_PI), periodLo, periodHi);
+    }
+
+
+    // Pre-compiled versions.
+
+    float periodic_mean(const std::vector<float>& samples, float periodLo, float periodHi);
+    double periodic_mean(const std::vector<double>& samples, double periodLo, double periodHi);
+
+}
+