fix: avoid perpetual movement when the target ang motor position is overshot

1 files changed, 10 insertions, 0 deletions

diff --git a/src/utils.rs b/src/utils.rs
index 4cbc398..90401a4 100644
--- a/src/utils.rs
+++ b/src/utils.rs
@@ -804,3 +804,13 @@ impl<T> IndexMut2<usize> for [T] {
         }
     }
 }
+
+/// Calculate the difference with smallest absolute value between the two given values.
+pub fn smallest_abs_diff_between_sin_angles<N: WReal>(a: N, b: N) -> N {
+    // Select the smallest path among the two angles to reach the target.
+    let s_err = a - b;
+    let sgn = s_err.simd_signum();
+    let s_err_complement = s_err - sgn * N::splat(2.0);
+    let s_err_is_smallest = s_err.simd_abs().simd_lt(s_err_complement.simd_abs());
+    s_err.select(s_err_is_smallest, s_err_complement)
+}