diff --git a/scipy/spatial/transform/_rotation.pyx b/scipy/spatial/transform/_rotation.pyx index 9f30ef1ce769..9bba02902ad8 100644 --- a/scipy/spatial/transform/_rotation.pyx +++ b/scipy/spatial/transform/_rotation.pyx @@ -1243,7 +1243,7 @@ cdef class Rotation: for ind in range(num_rotations): angle = _norm3(crotvec[ind, :]) - if angle <= 1e-3: # small angle order 5 Taylor series expansion + if angle <= 1e-3: # small angle Taylor series expansion angle2 = angle * angle scale = 0.5 - angle2 / 48 + angle2 * angle2 / 3840 else: # large angle @@ -1916,7 +1916,7 @@ cdef class Rotation: angle = 2 * atan2(_norm3(quat), quat[3]) - if angle <= 1e-3: # small angle order 5 Taylor series expansion + if angle <= 1e-3: # small angle Taylor series expansion angle2 = angle * angle scale = 2 + angle2 / 12 + 7 * angle2 * angle2 / 2880 else: # large angle @@ -3484,26 +3484,26 @@ cdef class Rotation: # vectors. cross = np.cross(b_pri[0], a_pri[0]) cross_norm = _norm3(cross) - theta = atan2(cross_norm, np.dot(a_pri[0], b_pri[0])) + 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 abs(np.pi - theta) < tolerance: - # At 180 degrees, cross = [0, 0, 0] so we need to flip the - # vectors (along an arbitrary orthogonal axis of rotation) + # 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: - vec_non_parallel = np.roll(a_pri[0], 1) + np.array([1, 0, 0]) - vec_orthogonal = np.cross(a_pri[0], vec_non_parallel) - r_flip = vec_orthogonal / _norm3(vec_orthogonal) * np.pi - R_flip = cls.from_rotvec(r_flip) - - # Near 180 degrees, the small angle appoximation of x/sin(x) - # diverges, so for numerical stability we flip and then rotate - # back by pi - theta + # 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_flip = cls.from_rotvec(cross / _norm3(cross) * np.pi) - theta = np.pi - theta - cross = -cross - if theta < 1e-3: # small angle order 5 Taylor series approximation + 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: diff --git a/scipy/spatial/transform/tests/test_rotation.py b/scipy/spatial/transform/tests/test_rotation.py index bbeec15ba268..af3f4107ebb5 100644 --- a/scipy/spatial/transform/tests/test_rotation.py +++ b/scipy/spatial/transform/tests/test_rotation.py @@ -1470,19 +1470,25 @@ def test_align_vectors_parallel(): def test_align_vectors_antiparallel(): # Test exact 180 deg rotation atol = 1e-12 - a = [[0, 1, 0], [-1, 0, 0]] - b = [[0, -1, 0], [-1, 0, 0]] - R, _ = Rotation.align_vectors(a, b, weights=[np.inf, 1]) - assert_allclose(R.magnitude(), np.pi, atol=atol) + as_to_test = np.array([[[0, 1, 0], [1, 0, 0]], + [[1, 0, 0], [0, 1, 0]], + [[0, 0, 1], [1, 0, 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 - as_to_test = [a, - [[0, 1, 1e-4], [-1, 0, 0]], - [[0, 1, -1e-4], [-1, 0, 0]]] + Rs = Rotation.random(100, random_state=0) + dRs = Rotation.from_rotvec(Rs.as_rotvec()*1e-4) # scale down to small angle + 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 R.approx_equal(R2, atol=atol) + assert_allclose(R.as_matrix(), R2.as_matrix(), atol=atol) def test_align_vectors_primary_only():