[py] Add wpimath/geometry tests (#8460)

wpimath/src/test/python/geometry/test_translation2d.py

@@ -0,0 +1,135 @@
+import pytest
+import math
+
+from wpimath import Translation2d, Rotation2d
+
+
+def test_sum():
+    one = Translation2d(1.0, 3.0)
+    two = Translation2d(2.0, 5.0)
+
+    sum_ = one + two
+
+    assert sum_.x == pytest.approx(3.0)
+    assert sum_.y == pytest.approx(8.0)
+
+
+def test_difference():
+    one = Translation2d(1.0, 3.0)
+    two = Translation2d(2.0, 5.0)
+
+    difference = one - two
+
+    assert difference.x == pytest.approx(-1.0)
+    assert difference.y == pytest.approx(-2.0)
+
+
+def test_rotate_by():
+    another = Translation2d(3.0, 0.0)
+    rotated = another.rotateBy(Rotation2d.fromDegrees(90))
+
+    assert rotated.x == pytest.approx(0.0, abs=1e-9)
+    assert rotated.y == pytest.approx(3.0, abs=1e-9)
+
+
+def test_rotate_around():
+    translation = Translation2d(2.0, 1.0)
+    other = Translation2d(3.0, 2.0)
+    rotated = translation.rotateAround(other, Rotation2d.fromDegrees(180))
+
+    assert rotated.x == pytest.approx(4.0, abs=1e-9)
+    assert rotated.y == pytest.approx(3.0, abs=1e-9)
+
+
+def test_multiplication():
+    original = Translation2d(3.0, 5.0)
+    mult = original * 3
+
+    assert mult.x == pytest.approx(9.0)
+    assert mult.y == pytest.approx(15.0)
+
+
+def test_division():
+    original = Translation2d(3.0, 5.0)
+    div = original / 2
+
+    assert div.x == pytest.approx(1.5)
+    assert div.y == pytest.approx(2.5)
+
+
+def test_norm():
+    one = Translation2d(3.0, 5.0)
+    assert one.norm() == pytest.approx(math.hypot(3.0, 5.0))
+
+
+def test_squared_norm():
+    one = Translation2d(3.0, 5.0)
+    assert one.squaredNorm() == pytest.approx(34.0)
+
+
+def test_distance():
+    one = Translation2d(1.0, 1.0)
+    two = Translation2d(6.0, 6.0)
+    assert one.distance(two) == pytest.approx(5.0 * math.sqrt(2.0))
+
+
+def test_squared_distance():
+    one = Translation2d(1.0, 1.0)
+    two = Translation2d(6.0, 6.0)
+    assert one.distance(two) ** 2 == pytest.approx(50.0)
+
+
+def test_unary_minus():
+    original = Translation2d(-4.5, 7.0)
+    inverted = -original
+
+    assert inverted.x == pytest.approx(4.5)
+    assert inverted.y == pytest.approx(-7.0)
+
+
+def test_equality():
+    one = Translation2d(9.0, 5.5)
+    two = Translation2d(9.0, 5.5)
+    assert one == two
+
+
+def test_inequality():
+    one = Translation2d(9.0, 5.5)
+    two = Translation2d(9.0, 5.7)
+    assert one != two
+
+
+def test_polar_constructor():
+    one = Translation2d(math.sqrt(2), Rotation2d.fromDegrees(45))
+    assert one.x == pytest.approx(1.0)
+    assert one.y == pytest.approx(1.0)
+
+    two = Translation2d(2.0, Rotation2d.fromDegrees(60))
+    assert two.x == pytest.approx(1.0)
+    assert two.y == pytest.approx(math.sqrt(3.0))
+
+
+def test_nearest():
+    origin = Translation2d(0.0, 0.0)
+
+    translation1 = Translation2d(1.0, Rotation2d.fromDegrees(45))
+    translation2 = Translation2d(2.0, Rotation2d.fromDegrees(90))
+    translation3 = Translation2d(3.0, Rotation2d.fromDegrees(135))
+    translation4 = Translation2d(4.0, Rotation2d.fromDegrees(180))
+    translation5 = Translation2d(5.0, Rotation2d.fromDegrees(270))
+
+    assert origin.nearest([translation5, translation3, translation4]) == translation3
+    assert origin.nearest([translation1, translation2, translation3]) == translation1
+    assert origin.nearest([translation4, translation2, translation3]) == translation2
+
+
+def test_dot():
+    one = Translation2d(2.0, 3.0)
+    two = Translation2d(3.0, 4.0)
+    assert one.dot(two) == pytest.approx(18.0)
+
+
+def test_cross():
+    one = Translation2d(2.0, 3.0)
+    two = Translation2d(3.0, 4.0)
+    assert one.cross(two) == pytest.approx(-1.0)