[utils] canonicalize rotation matrices before caching
This commit is contained in:
parent
93dd3503ca
commit
e397b3f96e
2 changed files with 41 additions and 10 deletions
|
|
@ -89,6 +89,23 @@ def test_rotation_matrix_non_manhattan() -> None:
|
|||
assert_allclose(m, [[s, -s], [s, s]], atol=1e-10)
|
||||
|
||||
|
||||
def test_rotation_matrix_canonicalizes_before_caching() -> None:
|
||||
expected = rotation_matrix_2d(pi / 2)
|
||||
|
||||
for angle in (pi / 2 + 1e-12, pi / 2 - 1e-12, -3 * pi / 2, 5 * pi / 2):
|
||||
assert rotation_matrix_2d(angle) is expected
|
||||
|
||||
assert not expected.flags.writeable
|
||||
|
||||
|
||||
def test_rotation_matrix_does_not_snap_non_manhattan_angle() -> None:
|
||||
cardinal = rotation_matrix_2d(pi / 2)
|
||||
nearby = rotation_matrix_2d(pi / 2 + 2e-3)
|
||||
|
||||
assert nearby is not cardinal
|
||||
assert not numpy.array_equal(nearby, cardinal)
|
||||
|
||||
|
||||
def test_apply_transforms() -> None:
|
||||
# cumulative [x_offset, y_offset, rotation (rad), mirror_x (0 or 1)]
|
||||
t1 = [10, 20, 0, 0]
|
||||
|
|
|
|||
|
|
@ -3,6 +3,7 @@ Geometric transforms
|
|||
"""
|
||||
from collections.abc import Sequence
|
||||
from functools import lru_cache
|
||||
from math import acos
|
||||
|
||||
import numpy
|
||||
from numpy.typing import NDArray, ArrayLike
|
||||
|
|
@ -13,8 +14,26 @@ from numpy import pi
|
|||
R90 = pi / 2
|
||||
R180 = pi
|
||||
|
||||
# Preserve the effective tolerance of the historical
|
||||
# ``isclose(cos(4 * theta), 1, atol=1e-12)`` Manhattan check. Expressing it as
|
||||
# an angular distance lets us canonicalize before consulting the matrix cache.
|
||||
_MANHATTAN_SNAP_ATOL = acos(1 - (1e-5 + 1e-12)) / 4
|
||||
_CARDINAL_ANGLES = (0.0, R90, R180, 3 * R90)
|
||||
|
||||
|
||||
@lru_cache
|
||||
def _rotation_matrix_2d(theta: float) -> NDArray[numpy.float64]:
|
||||
"""Build and cache an immutable matrix for a canonicalized angle."""
|
||||
arr = numpy.array([[numpy.cos(theta), -numpy.sin(theta)],
|
||||
[numpy.sin(theta), +numpy.cos(theta)]])
|
||||
|
||||
if theta in _CARDINAL_ANGLES:
|
||||
arr = numpy.round(arr)
|
||||
|
||||
arr.flags.writeable = False
|
||||
return arr
|
||||
|
||||
|
||||
def rotation_matrix_2d(theta: float) -> NDArray[numpy.float64]:
|
||||
"""
|
||||
2D rotation matrix for rotating counterclockwise around the origin.
|
||||
|
|
@ -25,16 +44,11 @@ def rotation_matrix_2d(theta: float) -> NDArray[numpy.float64]:
|
|||
Returns:
|
||||
rotation matrix
|
||||
"""
|
||||
arr = numpy.array([[numpy.cos(theta), -numpy.sin(theta)],
|
||||
[numpy.sin(theta), +numpy.cos(theta)]])
|
||||
|
||||
# If this was a manhattan rotation, round to remove some inaccuracies in sin & cos
|
||||
# cos(4*theta) is 1 for any multiple of pi/2.
|
||||
if numpy.isclose(numpy.cos(4 * theta), 1, atol=1e-12):
|
||||
arr = numpy.round(arr)
|
||||
|
||||
arr.flags.writeable = False
|
||||
return arr
|
||||
theta = float(theta)
|
||||
quarter_turn = round(theta / R90)
|
||||
if abs(theta - quarter_turn * R90) <= _MANHATTAN_SNAP_ATOL:
|
||||
theta = _CARDINAL_ANGLES[quarter_turn % 4]
|
||||
return _rotation_matrix_2d(theta)
|
||||
|
||||
|
||||
def normalize_mirror(mirrored: Sequence[bool]) -> tuple[bool, float]:
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue