[SLERP] Return the first quaternion when the two quaternion are equal

ami-iit · Nov 29, 2023 · 2ac00cd · 2ac00cd
1 parent ea55d1f
commit 2ac00cd
Showing 1 changed file with 7 additions and 1 deletion.
diff --git a/src/liecasadi/quaternion.py b/src/liecasadi/quaternion.py
@@ -122,6 +122,12 @@ def slerp_step(q1: Vector, q2: Vector, t: Scalar) -> Vector:
 
         dot = cs.dot(q1, q2)
         angle = cs.acos(dot)
+        # if the angle is small (meaning the quaternions are "equal") we return the first quaternion
         return Quaternion(
-            (cs.sin((1.0 - t) * angle) * q1 + cs.sin(t * angle) * q2) / cs.sin(angle)
+            cs.if_else(
+                angle < 1e-6,
+                q1,
+                (cs.sin((1.0 - t) * angle) * q1 + cs.sin(t * angle) * q2)
+                / cs.sin(angle),
+            )
         )