Add simox::math::periodic_diff

SimoxUtility/CMakeLists.txt

@@ -72,6 +72,8 @@ SET(SOURCES
     math/pose/invert.cpp
     math/pose/orthogonalize.cpp
 
+    math/periodic/periodic_diff.cpp
+
     math/SoftMinMax.cpp
 
     shapes/AxisAlignedBoundingBox.cpp
@@ -149,6 +151,7 @@ SET(INCLUDES
     math/distance/angle_between.h
 
     math/periodic_clamp.h
+    math/periodic/periodic_diff.h
 
     math/pose/align_box_orientation.h
     math/pose/check_rotation_matrix.h

SimoxUtility/math/periodic/periodic_diff.cpp

+#include "periodic_diff.h"
+
+
+namespace simox
+{
+
+    float math::periodic_diff(float a, float b, float periodLo, float periodHi)
+    {
+        return periodic_diff<float>(a, b, periodLo, periodHi);
+    }
+
+    double math::periodic_diff(double a, double b, double periodLo, double periodHi)
+    {
+        return periodic_diff<double>(a, b, periodLo, periodHi);
+    }
+
+}
+

SimoxUtility/math/periodic/periodic_diff.h

+#pragma once
+
+#include <cmath>
+#include <type_traits>
+
+#include <Eigen/Core>
+
+#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_diff(Float a, Float b, Float periodLo, Float periodHi)
+    {
+        /* Python:
+        a, b = wrap_angles([a, b])
+        diffs = [a - b, (a + 2 * np.pi) - b, (a - 2 * np.pi) - b]
+        i_min = np.argmin(np.abs(diffs))
+        return diffs[int(i_min)]
+        */
+        a = periodic_clamp(a, periodLo, periodHi);
+        b = periodic_clamp(b, periodLo, periodHi);
+
+        const Float period = periodHi - periodLo;
+
+        Eigen::Array<Float, 3, 1> diffs = {
+            a - b,
+            (a + period) - b,
+            (a - period) - b
+        };
+
+        int i = 0;
+        diffs.abs().minCoeff(&i);
+        return diffs(i);
+    }
+
+
+    // Pre-compiled versions.
+
+    float periodic_diff(float a, float b, float periodLo, float periodHi);
+    double periodic_diff(double a, double b, double periodLo, double periodHi);
+
+}
+