Rename geometry from matrix constructor overloads

Author: auscompgeek

subprojects/robotpy-wpimath/gen/geometry/Pose2d.yml

@@ -21,13 +21,17 @@ classes:
           '':
           Translation2d, Rotation2d:
           units::meter_t, units::meter_t, Rotation2d:
+          const Eigen::Matrix3d&:
+            rename: fromMatrix
+            keepalive: []
       Translation:
       Rotation:
       RotateBy:
       TransformBy:
       RelativeTo:
       Exp:
       Log:
+      ToMatrix:
       Nearest:
         overloads:
           std::span<const Pose2d> [const]:

subprojects/robotpy-wpimath/gen/geometry/Pose3d.yml

@@ -23,6 +23,10 @@ classes:
           Translation3d, Rotation3d:
           units::meter_t, units::meter_t, units::meter_t, Rotation3d:
           const Pose2d&:
+            keepalive: []
+          const Eigen::Matrix4d&:
+            rename: fromMatrix
+            keepalive: []
       operator+:
       operator-:
       operator==:
@@ -39,6 +43,7 @@ classes:
       RelativeTo:
       Exp:
       Log:
+      ToMatrix:
       ToPose2d:
 
 inline_code: |

subprojects/robotpy-wpimath/gen/geometry/Rotation2d.yml

@@ -25,7 +25,11 @@ classes:
             template_impls:
               - ['units::radian_t']
           double, double:
+          const Eigen::Matrix2d&:
+            rename: fromMatrix
+            keepalive: []
       RotateBy:
+      ToMatrix:
       Radians:
       Degrees:
       Cos:

subprojects/robotpy-wpimath/gen/geometry/Rotation3d.yml

@@ -16,12 +16,18 @@ classes:
         overloads:
           '':
           const Quaternion&:
+            keepalive: []
           units::radian_t, units::radian_t, units::radian_t:
           const Eigen::Vector3d&, units::radian_t:
+            keepalive: []
           const Eigen::Vector3d&:
+            keepalive: []
           const Eigen::Matrix3d&:
+            keepalive: []
           const Eigen::Vector3d&, const Eigen::Vector3d&:
+            keepalive: []
           const Rotation2d&:
+            keepalive: []
       operator+:
       operator-:
         overloads:
@@ -38,6 +44,7 @@ classes:
       Z:
       Axis:
       Angle:
+      ToMatrix:
       ToRotation2d:
 
 inline_code: |

subprojects/robotpy-wpimath/gen/geometry/Transform2d.yml

@@ -16,13 +16,18 @@ classes:
       Transform2d:
         overloads:
           const Pose2d&, const Pose2d&:
+            keepalive: []
           Translation2d, Rotation2d:
           units::meter_t, units::meter_t, Rotation2d:
+          const Eigen::Matrix3d&:
+            rename: fromMatrix
+            keepalive: []
           '':
       Translation:
-      Rotation:
       X:
       Y:
+      ToMatrix:
+      Rotation:
       Inverse:
       operator*:
       operator/:

subprojects/robotpy-wpimath/gen/geometry/Transform3d.yml

@@ -15,14 +15,20 @@ classes:
       Transform3d:
         overloads:
           const Pose3d&, const Pose3d&:
+            keepalive: []
           Translation3d, Rotation3d:
           units::meter_t, units::meter_t, units::meter_t, Rotation3d:
+          const Eigen::Matrix4d&:
+            rename: fromMatrix
+            keepalive: []
           '':
           const frc::Transform2d&:
+            keepalive: []
       Translation:
       X:
       Y:
       Z:
+      ToMatrix:
       Rotation:
       Inverse:
       operator*:

subprojects/robotpy-wpimath/tests/geometry/test_rotation2d.py

@@ -0,0 +1,80 @@
+import importlib.util
+import math
+
+import pytest
+
+from wpimath.geometry import Rotation2d
+
+
+@pytest.mark.parametrize(
+    "radians,degrees",
+    [
+        (math.pi / 3, 60.0),
+        (math.pi / 4, 45.0),
+    ],
+)
+def test_radians_to_degrees(radians: float, degrees: float):
+    rot = Rotation2d(radians)
+    assert math.isclose(degrees, rot.degrees())
+
+
+@pytest.mark.parametrize(
+    "degrees,radians",
+    [
+        (45.0, math.pi / 4),
+        (30.0, math.pi / 6),
+    ],
+)
+def test_degrees_to_radians(degrees: float, radians: float):
+    rot = Rotation2d.fromDegrees(degrees)
+    assert math.isclose(radians, rot.radians())
+
+
+def test_rotate_by_from_zero() -> None:
+    zero = Rotation2d()
+    rotated = zero + Rotation2d.fromDegrees(90)
+
+    assert math.isclose(math.pi / 2, rotated.radians())
+    assert math.isclose(90.0, rotated.degrees())
+
+
+def test_rotate_by_non_zero() -> None:
+    rot = Rotation2d.fromDegrees(90.0)
+    rot += Rotation2d.fromDegrees(30.0)
+
+    assert math.isclose(120.0, rot.degrees())
+
+
+def test_minus() -> None:
+    rot1 = Rotation2d.fromDegrees(70)
+    rot2 = Rotation2d.fromDegrees(30)
+
+    assert math.isclose(40.0, (rot1 - rot2).degrees())
+
+
+def test_unary_minus() -> None:
+    rot = Rotation2d.fromDegrees(20)
+    assert math.isclose(-20.0, (-rot).degrees())
+
+
+def test_equality() -> None:
+    rot1 = Rotation2d.fromDegrees(43.0)
+    rot2 = Rotation2d.fromDegrees(43.0)
+    assert rot1 == rot2
+
+    rot1 = Rotation2d.fromDegrees(-180.0)
+    rot2 = Rotation2d.fromDegrees(180.0)
+    assert rot1 == rot2
+
+
+def test_inequality() -> None:
+    rot1 = Rotation2d.fromDegrees(43.0)
+    rot2 = Rotation2d.fromDegrees(43.5)
+    assert rot1 != rot2
+
+
+@pytest.mark.skipif(importlib.util.find_spec("numpy") is None)
+def test_to_matrix() -> None:
+    before = Rotation2d.fromDegrees(20.0)
+    after = Rotation2d.fromMatrix(before.toMatrix())
+    assert before == after