Clamp arccos to (-1, 1) across codebase

dfki-ric · Aug 7, 2023 · b776a63 · b776a63
1 parent bc76646
commit b776a63
Show file tree

Hide file tree

Showing 2 changed files with 3 additions and 2 deletions.
diff --git a/pytransform3d/batch_rotations.py b/pytransform3d/batch_rotations.py
@@ -302,7 +302,7 @@ def axis_angles_from_matrices(Rs, traces=None, out=None):
             # out[False, n] = value will not assign value to out[n]
             traces = traces[0]
 
-    angles = np.arccos((traces - 1.0) / 2.0)
+    angles = np.arccos(np.clip((traces - 1.0) / 2.0, -1, 1))
 
     if out is None:
         out = np.empty(instances_shape + (4,))

diff --git a/pytransform3d/rotations/_conversions.py b/pytransform3d/rotations/_conversions.py
@@ -1813,7 +1813,8 @@ def axis_angle_from_quaternion(q):
         return np.array([1.0, 0.0, 0.0, 0.0])
 
     axis = p / p_norm
-    angle = (2.0 * np.arccos(q[0]),)
+    w_clamped = max(min(q[0], 1.0), -1.0)
+    angle = (2.0 * np.arccos(w_clamped),)
     return norm_axis_angle(np.hstack((axis, angle)))