[wpimath] Add rotation matrix constructor to Rotation3d (#4413)

wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation3dTest.java

@@ -7,7 +7,11 @@ package edu.wpi.first.math.geometry;
 import static org.junit.jupiter.api.Assertions.assertAll;
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertNotEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
 
+import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.util.Units;
 import org.junit.jupiter.api.Test;
@@ -36,6 +40,32 @@ class Rotation3dTest {
     assertEquals(rot5, rot6);
   }
 
+  @Test
+  void testInitRotationMatrix() {
+    // No rotation
+    final var R1 = Matrix.eye(Nat.N3());
+    final var rot1 = new Rotation3d(R1);
+    assertEquals(new Rotation3d(), rot1);
+
+    // 90 degree CCW rotation around z-axis
+    final var R2 = new Matrix<>(Nat.N3(), Nat.N3());
+    R2.assignBlock(0, 0, VecBuilder.fill(0.0, 1.0, 0.0));
+    R2.assignBlock(0, 1, VecBuilder.fill(-1.0, 0.0, 0.0));
+    R2.assignBlock(0, 2, VecBuilder.fill(0.0, 0.0, 1.0));
+    final var rot2 = new Rotation3d(R2);
+    final var expected2 = new Rotation3d(0.0, 0.0, Units.degreesToRadians(90.0));
+    assertEquals(expected2, rot2);
+
+    // Matrix that isn't orthogonal
+    final var R3 =
+        new MatBuilder<>(Nat.N3(), Nat.N3()).fill(1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0);
+    assertThrows(IllegalArgumentException.class, () -> new Rotation3d(R3));
+
+    // Matrix that's orthogonal but not special orthogonal
+    final var R4 = Matrix.eye(Nat.N3()).times(2.0);
+    assertThrows(IllegalArgumentException.class, () -> new Rotation3d(R4));
+  }
+
   @Test
   void testInitTwoVector() {
     @SuppressWarnings("LocalVariableName")

wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp

@@ -7,6 +7,7 @@
 #include <wpi/MathExtras.h>
 #include <wpi/numbers>
 
+#include "frc/EigenCore.h"
 #include "frc/geometry/Rotation3d.h"
 #include "gtest/gtest.h"
 
@@ -29,6 +30,30 @@ TEST(Rotation3dTest, InitAxisAngleAndRollPitchYaw) {
   EXPECT_EQ(rot5, rot6);
 }
 
+TEST(Rotation3dTest, InitRotationMatrix) {
+  // No rotation
+  const Matrixd<3, 3> R1 = Matrixd<3, 3>::Identity();
+  const Rotation3d rot1{R1};
+  EXPECT_EQ(Rotation3d{}, rot1);
+
+  // 90 degree CCW rotation around z-axis
+  Matrixd<3, 3> R2;
+  R2.block<3, 1>(0, 0) = Vectord<3>{0.0, 1.0, 0.0};
+  R2.block<3, 1>(0, 1) = Vectord<3>{-1.0, 0.0, 0.0};
+  R2.block<3, 1>(0, 2) = Vectord<3>{0.0, 0.0, 1.0};
+  const Rotation3d rot2{R2};
+  const Rotation3d expected2{0_deg, 0_deg, 90_deg};
+  EXPECT_EQ(expected2, rot2);
+
+  // Matrix that isn't orthogonal
+  const Matrixd<3, 3> R3{{1.0, 0.0, 0.0}, {1.0, 0.0, 0.0}, {1.0, 0.0, 0.0}};
+  EXPECT_THROW(Rotation3d{R3}, std::domain_error);
+
+  // Matrix that's orthogonal but not special orthogonal
+  const Matrixd<3, 3> R4 = Matrixd<3, 3>::Identity() * 2.0;
+  EXPECT_THROW(Rotation3d{R4}, std::domain_error);
+}
+
 TEST(Rotation3dTest, InitTwoVector) {
   const Eigen::Vector3d xAxis{1.0, 0.0, 0.0};
   const Eigen::Vector3d yAxis{0.0, 1.0, 0.0};