|
| 1 | +import importlib.util |
| 2 | +import math |
| 3 | + |
| 4 | +import pytest |
| 5 | + |
| 6 | +from wpimath.geometry import Rotation2d |
| 7 | + |
| 8 | + |
| 9 | +@pytest.mark.parametrize( |
| 10 | + "radians,degrees", |
| 11 | + [ |
| 12 | + (math.pi / 3, 60.0), |
| 13 | + (math.pi / 4, 45.0), |
| 14 | + ], |
| 15 | +) |
| 16 | +def test_radians_to_degrees(radians: float, degrees: float): |
| 17 | + rot = Rotation2d(radians) |
| 18 | + assert math.isclose(degrees, rot.degrees()) |
| 19 | + |
| 20 | + |
| 21 | +@pytest.mark.parametrize( |
| 22 | + "degrees,radians", |
| 23 | + [ |
| 24 | + (45.0, math.pi / 4), |
| 25 | + (30.0, math.pi / 6), |
| 26 | + ], |
| 27 | +) |
| 28 | +def test_degrees_to_radians(degrees: float, radians: float): |
| 29 | + rot = Rotation2d.fromDegrees(degrees) |
| 30 | + assert math.isclose(radians, rot.radians()) |
| 31 | + |
| 32 | + |
| 33 | +def test_rotate_by_from_zero() -> None: |
| 34 | + zero = Rotation2d() |
| 35 | + rotated = zero + Rotation2d.fromDegrees(90) |
| 36 | + |
| 37 | + assert math.isclose(math.pi / 2, rotated.radians()) |
| 38 | + assert math.isclose(90.0, rotated.degrees()) |
| 39 | + |
| 40 | + |
| 41 | +def test_rotate_by_non_zero() -> None: |
| 42 | + rot = Rotation2d.fromDegrees(90.0) |
| 43 | + rot += Rotation2d.fromDegrees(30.0) |
| 44 | + |
| 45 | + assert math.isclose(120.0, rot.degrees()) |
| 46 | + |
| 47 | + |
| 48 | +def test_minus() -> None: |
| 49 | + rot1 = Rotation2d.fromDegrees(70) |
| 50 | + rot2 = Rotation2d.fromDegrees(30) |
| 51 | + |
| 52 | + assert math.isclose(40.0, (rot1 - rot2).degrees()) |
| 53 | + |
| 54 | + |
| 55 | +def test_unary_minus() -> None: |
| 56 | + rot = Rotation2d.fromDegrees(20) |
| 57 | + assert math.isclose(-20.0, (-rot).degrees()) |
| 58 | + |
| 59 | + |
| 60 | +def test_equality() -> None: |
| 61 | + rot1 = Rotation2d.fromDegrees(43.0) |
| 62 | + rot2 = Rotation2d.fromDegrees(43.0) |
| 63 | + assert rot1 == rot2 |
| 64 | + |
| 65 | + rot1 = Rotation2d.fromDegrees(-180.0) |
| 66 | + rot2 = Rotation2d.fromDegrees(180.0) |
| 67 | + assert rot1 == rot2 |
| 68 | + |
| 69 | + |
| 70 | +def test_inequality() -> None: |
| 71 | + rot1 = Rotation2d.fromDegrees(43.0) |
| 72 | + rot2 = Rotation2d.fromDegrees(43.5) |
| 73 | + assert rot1 != rot2 |
| 74 | + |
| 75 | + |
| 76 | +@pytest.mark.skipif( |
| 77 | + importlib.util.find_spec("numpy") is None, reason="numpy is not available" |
| 78 | +) |
| 79 | +def test_to_matrix() -> None: |
| 80 | + before = Rotation2d.fromDegrees(20.0) |
| 81 | + after = Rotation2d.fromMatrix(before.toMatrix()) |
| 82 | + assert before == after |
0 commit comments