From e16c6cce3ca94f13e63b3336805f46dff840d4c9 Mon Sep 17 00:00:00 2001 From: Scott Shambaugh Date: Wed, 24 Apr 2024 08:56:50 -0600 Subject: [PATCH 1/4] Fix exact rotations at 180 deg, improve near 180 deg Comments --- scipy/spatial/transform/_rotation.pyx | 29 ++++++++++++++++++++++----- 1 file changed, 24 insertions(+), 5 deletions(-) diff --git a/scipy/spatial/transform/_rotation.pyx b/scipy/spatial/transform/_rotation.pyx index f18a24270277..a1e29747ab1c 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 Taylor series expansion + if angle <= 1e-3: # small angle order 5 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 Taylor series expansion + if angle <= 1e-3: # small angle order 5 Taylor series expansion angle2 = angle * angle scale = 2 + angle2 / 12 + 7 * angle2 * angle2 / 2880 else: # large angle @@ -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, np.dot(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) + if cross_norm == 0: + vec_non_parallel = np.roll(a_pri, 1) + np.array([1, 0, 0]) + vec_orthogonal = np.cross(a_pri, 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 + 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 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 From ed9f2ce89b0330879c3ca837b8ff3d934585fc74 Mon Sep 17 00:00:00 2001 From: Scott Shambaugh Date: Wed, 24 Apr 2024 09:03:44 -0600 Subject: [PATCH 2/4] Tests for exact near 180 deg rotations --- scipy/spatial/transform/tests/test_rotation.py | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/scipy/spatial/transform/tests/test_rotation.py b/scipy/spatial/transform/tests/test_rotation.py index bf208f34437b..bc9af0363ae0 100644 --- a/scipy/spatial/transform/tests/test_rotation.py +++ b/scipy/spatial/transform/tests/test_rotation.py @@ -1467,6 +1467,24 @@ 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 + 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 np.isclose(R.magnitude(), np.pi, 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]]] + 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) + + def test_align_vectors_primary_only(): atol = 1e-12 mats_a = Rotation.random(100, random_state=0).as_matrix() From d2a690ba8b6aed61182d9f0ca735f61cd79d8fc9 Mon Sep 17 00:00:00 2001 From: Scott Shambaugh <14363975+scottshambaugh@users.noreply.github.com> Date: Wed, 24 Apr 2024 12:44:56 -0600 Subject: [PATCH 3/4] Fix tests --- scipy/spatial/transform/_rotation.pyx | 4 ++-- scipy/spatial/transform/tests/test_rotation.py | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/scipy/spatial/transform/_rotation.pyx b/scipy/spatial/transform/_rotation.pyx index a1e29747ab1c..9f30ef1ce769 100644 --- a/scipy/spatial/transform/_rotation.pyx +++ b/scipy/spatial/transform/_rotation.pyx @@ -3491,8 +3491,8 @@ cdef class Rotation: # At 180 degrees, cross = [0, 0, 0] so we need to flip the # vectors (along an arbitrary orthogonal axis of rotation) if cross_norm == 0: - vec_non_parallel = np.roll(a_pri, 1) + np.array([1, 0, 0]) - vec_orthogonal = np.cross(a_pri, vec_non_parallel) + 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) diff --git a/scipy/spatial/transform/tests/test_rotation.py b/scipy/spatial/transform/tests/test_rotation.py index bc9af0363ae0..bbeec15ba268 100644 --- a/scipy/spatial/transform/tests/test_rotation.py +++ b/scipy/spatial/transform/tests/test_rotation.py @@ -1473,7 +1473,7 @@ def test_align_vectors_antiparallel(): 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 np.isclose(R.magnitude(), np.pi, atol=atol) + assert_allclose(R.magnitude(), np.pi, atol=atol) # Test exact rotations near 180 deg as_to_test = [a, From 6cb522da81fae9f3cc64374c182c349d5cfb6d07 Mon Sep 17 00:00:00 2001 From: Scott Shambaugh <14363975+scottshambaugh@users.noreply.github.com> Date: Wed, 24 Apr 2024 13:38:48 -0600 Subject: [PATCH 4/4] Code review updates --- scipy/spatial/transform/_rotation.pyx | 36 +++++++++---------- .../spatial/transform/tests/test_rotation.py | 24 ++++++++----- 2 files changed, 34 insertions(+), 26 deletions(-) diff --git a/scipy/spatial/transform/_rotation.pyx b/scipy/spatial/transform/_rotation.pyx index 9f30ef1ce769..2c4a3b70df41 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) + 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: - 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..381ce7ca06f2 100644 --- a/scipy/spatial/transform/tests/test_rotation.py +++ b/scipy/spatial/transform/tests/test_rotation.py @@ -1470,19 +1470,27 @@ 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([[[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 - 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 + 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 R.approx_equal(R2, atol=atol) + assert_allclose(R.as_matrix(), R2.as_matrix(), atol=atol) def test_align_vectors_primary_only():