diff --git a/scipy/spatial/transform/_rotation.pyx b/scipy/spatial/transform/_rotation.pyx index f18a24270277..2c4a3b70df41 100644 --- a/scipy/spatial/transform/_rotation.pyx +++ b/scipy/spatial/transform/_rotation.pyx @@ -3483,13 +3483,32 @@ cdef class Rotation: # We first find the minimum angle rotation between the primary # vectors. cross = np.cross(b_pri[0], a_pri[0]) - theta = atan2(_norm3(cross), np.dot(a_pri[0], b_pri[0])) - if theta < 1e-3: # small angle Taylor series approximation + cross_norm = _norm3(cross) + theta = atan2(cross_norm, _dot3(a_pri[0], b_pri[0])) + tolerance = 1e-3 # tolerance for small angle approximation (rad) + R_flip = cls.identity() + if (np.pi - theta) < tolerance: + # Near pi radians, the Taylor series appoximation of x/sin(x) + # diverges, so for numerical stability we flip pi and then + # rotate back by the small angle pi - theta + if cross_norm == 0: + # For antiparallel vectors, cross = [0, 0, 0] so we need to + # manually set an arbitrary orthogonal axis of rotation + i = np.argmin(np.abs(a_pri[0])) + r = np.zeros(3) + r[i - 1], r[i - 2] = a_pri[0][i - 2], -a_pri[0][i - 1] + else: + r = cross # Shortest angle orthogonal axis of rotation + R_flip = Rotation.from_rotvec(r / np.linalg.norm(r) * np.pi) + theta = np.pi - theta + cross = -cross + if abs(theta) < tolerance: + # Small angle Taylor series approximation for numerical stability theta2 = theta * theta r = cross * (1 + theta2 / 6 + theta2 * theta2 * 7 / 360) else: r = cross * theta / np.sin(theta) - R_pri = cls.from_rotvec(r) + R_pri = cls.from_rotvec(r) * R_flip if N == 1: # No secondary vectors, so we are done diff --git a/scipy/spatial/transform/tests/test_rotation.py b/scipy/spatial/transform/tests/test_rotation.py index bf208f34437b..381ce7ca06f2 100644 --- a/scipy/spatial/transform/tests/test_rotation.py +++ b/scipy/spatial/transform/tests/test_rotation.py @@ -1467,6 +1467,32 @@ def test_align_vectors_parallel(): assert_allclose(R.apply(b[0]), a[0], atol=atol) +def test_align_vectors_antiparallel(): + # Test exact 180 deg rotation + atol = 1e-12 + as_to_test = np.array([[[1, 0, 0], [0, 1, 0]], + [[0, 1, 0], [1, 0, 0]], + [[0, 0, 1], [0, 1, 0]]]) + bs_to_test = [[-a[0], a[1]] for a in as_to_test] + for a, b in zip(as_to_test, bs_to_test): + R, _ = Rotation.align_vectors(a, b, weights=[np.inf, 1]) + assert_allclose(R.magnitude(), np.pi, atol=atol) + assert_allclose(R.apply(b[0]), a[0], atol=atol) + + # Test exact rotations near 180 deg + Rs = Rotation.random(100, random_state=0) + dRs = Rotation.from_rotvec(Rs.as_rotvec()*1e-4) # scale down to small angle + a = [[ 1, 0, 0], [0, 1, 0]] + b = [[-1, 0, 0], [0, 1, 0]] + as_to_test = [] + for dR in dRs: + as_to_test.append([dR.apply(a[0]), a[1]]) + for a in as_to_test: + R, _ = Rotation.align_vectors(a, b, weights=[np.inf, 1]) + R2, _ = Rotation.align_vectors(a, b, weights=[1e10, 1]) + assert_allclose(R.as_matrix(), R2.as_matrix(), atol=atol) + + def test_align_vectors_primary_only(): atol = 1e-12 mats_a = Rotation.random(100, random_state=0).as_matrix()