From 2ac00cdaf30b978561f892ce58ea78ddb8dfd792 Mon Sep 17 00:00:00 2001 From: giulero Date: Wed, 29 Nov 2023 16:18:30 +0100 Subject: [PATCH] [SLERP] Return the first quaternion when the two quaternion are equal --- src/liecasadi/quaternion.py | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/liecasadi/quaternion.py b/src/liecasadi/quaternion.py index ce5c352..44594de 100644 --- 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), + ) )