Skip to content

Commit

Permalink
Code review updates
Browse files Browse the repository at this point in the history
  • Loading branch information
scottshambaugh committed Apr 25, 2024
1 parent d2a690b commit e6fd5c0
Show file tree
Hide file tree
Showing 2 changed files with 31 additions and 25 deletions.
34 changes: 17 additions & 17 deletions scipy/spatial/transform/_rotation.pyx
Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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:
Expand Down
22 changes: 14 additions & 8 deletions scipy/spatial/transform/tests/test_rotation.py
Expand Up @@ -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():
Expand Down

0 comments on commit e6fd5c0

Please sign in to comment.