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

wpimath/src/main/java/edu/wpi/first/math/geometry/CoordinateSystem.java

@@ -38,50 +38,9 @@ public class CoordinateSystem {
     R.assignBlock(0, 1, positiveY.m_axis);
     R.assignBlock(0, 2, positiveZ.m_axis);
 
-    // Require that the change of basis matrix is special orthogonal. This is true
-    // if the axes used are orthogonal and normalized. The CoordinateAxis class
-    // already normalizes itself, so we just need to check for orthogonality.
-    if (!R.times(R.transpose()).equals(Matrix.eye(Nat.N3()))) {
-      throw new IllegalArgumentException("Coordinate system isn't special orthogonal");
-    }
-
-    // Turn change of basis matrix into a quaternion since it's a pure rotation
-    // https://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
-    double trace = R.get(0, 0) + R.get(1, 1) + R.get(2, 2);
-    double w;
-    double x;
-    double y;
-    double z;
-
-    if (trace > 0.0) {
-      double s = 0.5 / Math.sqrt(trace + 1.0);
-      w = 0.25 / s;
-      x = (R.get(2, 1) - R.get(1, 2)) * s;
-      y = (R.get(0, 2) - R.get(2, 0)) * s;
-      z = (R.get(1, 0) - R.get(0, 1)) * s;
-    } else {
-      if (R.get(0, 0) > R.get(1, 1) && R.get(0, 0) > R.get(2, 2)) {
-        double s = 2.0 * Math.sqrt(1.0 + R.get(0, 0) - R.get(1, 1) - R.get(2, 2));
-        w = (R.get(2, 1) - R.get(1, 2)) / s;
-        x = 0.25 * s;
-        y = (R.get(0, 1) + R.get(1, 0)) / s;
-        z = (R.get(0, 2) + R.get(2, 0)) / s;
-      } else if (R.get(1, 1) > R.get(2, 2)) {
-        double s = 2.0 * Math.sqrt(1.0 + R.get(1, 1) - R.get(0, 0) - R.get(2, 2));
-        w = (R.get(0, 2) - R.get(2, 0)) / s;
-        x = (R.get(0, 1) + R.get(1, 0)) / s;
-        y = 0.25 * s;
-        z = (R.get(1, 2) + R.get(2, 1)) / s;
-      } else {
-        double s = 2.0 * Math.sqrt(1.0 + R.get(2, 2) - R.get(0, 0) - R.get(1, 1));
-        w = (R.get(1, 0) - R.get(0, 1)) / s;
-        x = (R.get(0, 2) + R.get(2, 0)) / s;
-        y = (R.get(1, 2) + R.get(2, 1)) / s;
-        z = 0.25 * s;
-      }
-    }
-
-    m_rotation = new Rotation3d(new Quaternion(w, x, y, z));
+    // The change of basis matrix should be a pure rotation. The Rotation3d
+    // constructor will verify this by checking for special orthogonality.
+    m_rotation = new Rotation3d(R);
   }
 
   /**

wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java

@@ -13,7 +13,9 @@ import edu.wpi.first.math.interpolation.Interpolatable;
 import edu.wpi.first.math.util.Units;
 import java.util.Objects;
 
-/** A rotation in a 2D coordinate frame represented a point on the unit circle (cosine and sine). */
+/**
+ * A rotation in a 2D coordinate frame represented by a point on the unit circle (cosine and sine).
+ */
 @JsonIgnoreProperties(ignoreUnknown = true)
 @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
 public class Rotation2d implements Interpolatable<Rotation2d> {

wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java

@@ -5,7 +5,9 @@
 package edu.wpi.first.math.geometry;
 
 import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.MathSharedStore;
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.Vector;
@@ -14,7 +16,7 @@ import edu.wpi.first.math.numbers.N3;
 import java.util.Objects;
 import org.ejml.dense.row.factory.DecompositionFactory_DDRM;
 
-/** A rotation in a 3D coordinate. */
+/** A rotation in a 3D coordinate frame represented by a quaternion. */
 public class Rotation3d implements Interpolatable<Rotation3d> {
   private Quaternion m_q = new Quaternion();
 
@@ -77,6 +79,74 @@ public class Rotation3d implements Interpolatable<Rotation3d> {
     m_q = new Quaternion(Math.cos(angleRadians / 2.0), v.get(0, 0), v.get(1, 0), v.get(2, 0));
   }
 
+  /**
+   * Constructs a Rotation3d from a rotation matrix.
+   *
+   * @param rotationMatrix The rotation matrix.
+   * @throws IllegalArgumentException if the rotation matrix isn't special orthogonal.
+   */
+  public Rotation3d(Matrix<N3, N3> rotationMatrix) {
+    final var R = rotationMatrix;
+
+    // Require that the rotation matrix is special orthogonal. This is true if
+    // the matrix is orthogonal (RRᵀ = I) and normalized (determinant is 1).
+    if (!R.times(R.transpose()).equals(Matrix.eye(Nat.N3()))) {
+      var builder = new StringBuilder("Rotation matrix isn't orthogonal\n\nR =\n");
+      builder.append(R.getStorage().toString()).append('\n');
+
+      var msg = builder.toString();
+      MathSharedStore.reportError(msg, Thread.currentThread().getStackTrace());
+      throw new IllegalArgumentException(msg);
+    }
+    if (R.det() != 1.0) {
+      var builder =
+          new StringBuilder("Rotation matrix is orthogonal but not special orthogonal\n\nR =\n");
+      builder.append(R.getStorage().toString()).append('\n');
+
+      var msg = builder.toString();
+      MathSharedStore.reportError(msg, Thread.currentThread().getStackTrace());
+      throw new IllegalArgumentException(msg);
+    }
+
+    // Turn rotation matrix into a quaternion
+    // https://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
+    double trace = R.get(0, 0) + R.get(1, 1) + R.get(2, 2);
+    double w;
+    double x;
+    double y;
+    double z;
+
+    if (trace > 0.0) {
+      double s = 0.5 / Math.sqrt(trace + 1.0);
+      w = 0.25 / s;
+      x = (R.get(2, 1) - R.get(1, 2)) * s;
+      y = (R.get(0, 2) - R.get(2, 0)) * s;
+      z = (R.get(1, 0) - R.get(0, 1)) * s;
+    } else {
+      if (R.get(0, 0) > R.get(1, 1) && R.get(0, 0) > R.get(2, 2)) {
+        double s = 2.0 * Math.sqrt(1.0 + R.get(0, 0) - R.get(1, 1) - R.get(2, 2));
+        w = (R.get(2, 1) - R.get(1, 2)) / s;
+        x = 0.25 * s;
+        y = (R.get(0, 1) + R.get(1, 0)) / s;
+        z = (R.get(0, 2) + R.get(2, 0)) / s;
+      } else if (R.get(1, 1) > R.get(2, 2)) {
+        double s = 2.0 * Math.sqrt(1.0 + R.get(1, 1) - R.get(0, 0) - R.get(2, 2));
+        w = (R.get(0, 2) - R.get(2, 0)) / s;
+        x = (R.get(0, 1) + R.get(1, 0)) / s;
+        y = 0.25 * s;
+        z = (R.get(1, 2) + R.get(2, 1)) / s;
+      } else {
+        double s = 2.0 * Math.sqrt(1.0 + R.get(2, 2) - R.get(0, 0) - R.get(1, 1));
+        w = (R.get(1, 0) - R.get(0, 1)) / s;
+        x = (R.get(0, 2) + R.get(2, 0)) / s;
+        y = (R.get(1, 2) + R.get(2, 1)) / s;
+        z = 0.25 * s;
+      }
+    }
+
+    m_q = new Quaternion(w, x, y, z);
+  }
+
   /**
    * Constructs a Rotation3d that rotates the initial vector onto the final vector.
    *

wpimath/src/main/native/cpp/geometry/CoordinateSystem.cpp

@@ -23,50 +23,9 @@ CoordinateSystem::CoordinateSystem(const CoordinateAxis& positiveX,
   R.block<3, 1>(0, 1) = positiveY.m_axis;
   R.block<3, 1>(0, 2) = positiveZ.m_axis;
 
-  // Require that the change of basis matrix is special orthogonal. This is true
-  // if the axes used are orthogonal and normalized. The CoordinateAxis class
-  // already normalizes itself, so we just need to check for orthogonality.
-  if (R * R.transpose() != Matrixd<3, 3>::Identity()) {
-    throw std::domain_error("Coordinate system isn't special orthogonal");
-  }
-
-  // Turn change of basis matrix into a quaternion since it's a pure rotation
-  // https://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
-  double trace = R(0, 0) + R(1, 1) + R(2, 2);
-  double w;
-  double x;
-  double y;
-  double z;
-
-  if (trace > 0.0) {
-    double s = 0.5 / std::sqrt(trace + 1.0);
-    w = 0.25 / s;
-    x = (R(2, 1) - R(1, 2)) * s;
-    y = (R(0, 2) - R(2, 0)) * s;
-    z = (R(1, 0) - R(0, 1)) * s;
-  } else {
-    if (R(0, 0) > R(1, 1) && R(0, 0) > R(2, 2)) {
-      double s = 2.0 * std::sqrt(1.0 + R(0, 0) - R(1, 1) - R(2, 2));
-      w = (R(2, 1) - R(1, 2)) / s;
-      x = 0.25 * s;
-      y = (R(0, 1) + R(1, 0)) / s;
-      z = (R(0, 2) + R(2, 0)) / s;
-    } else if (R(1, 1) > R(2, 2)) {
-      double s = 2.0 * std::sqrt(1.0 + R(1, 1) - R(0, 0) - R(2, 2));
-      w = (R(0, 2) - R(2, 0)) / s;
-      x = (R(0, 1) + R(1, 0)) / s;
-      y = 0.25 * s;
-      z = (R(1, 2) + R(2, 1)) / s;
-    } else {
-      double s = 2.0 * std::sqrt(1.0 + R(2, 2) - R(0, 0) - R(1, 1));
-      w = (R(1, 0) - R(0, 1)) / s;
-      x = (R(0, 2) + R(2, 0)) / s;
-      y = (R(1, 2) + R(2, 1)) / s;
-      z = 0.25 * s;
-    }
-  }
-
-  m_rotation = Rotation3d{Quaternion{w, x, y, z}};
+  // The change of basis matrix should be a pure rotation. The Rotation3d
+  // constructor will verify this by checking for special orthogonality.
+  m_rotation = Rotation3d{R};
 }
 
 const CoordinateSystem& CoordinateSystem::NWU() {

wpimath/src/main/native/cpp/geometry/Rotation3d.cpp

@@ -9,8 +9,11 @@
 #include <wpi/numbers>
 
 #include "Eigen/Core"
+#include "Eigen/LU"
 #include "Eigen/QR"
+#include "frc/fmt/Eigen.h"
 #include "units/math.h"
+#include "wpimath/MathShared.h"
 
 using namespace frc;
 
@@ -45,6 +48,66 @@ Rotation3d::Rotation3d(const Vectord<3>& axis, units::radian_t angle) {
   m_q = Quaternion{units::math::cos(angle / 2.0), v(0), v(1), v(2)};
 }
 
+Rotation3d::Rotation3d(const Matrixd<3, 3>& rotationMatrix) {
+  const auto& R = rotationMatrix;
+
+  // Require that the rotation matrix is special orthogonal. This is true if the
+  // matrix is orthogonal (RRᵀ = I) and normalized (determinant is 1).
+  if (R * R.transpose() != Matrixd<3, 3>::Identity()) {
+    std::string msg =
+        fmt::format("Rotation matrix isn't orthogonal\n\nR =\n{}\n", R);
+
+    wpi::math::MathSharedStore::ReportError(msg);
+    throw std::domain_error(msg);
+  }
+  if (R.determinant() != 1.0) {
+    std::string msg = fmt::format(
+        "Rotation matrix is orthogonal but not special orthogonal\n\nR =\n{}\n",
+        R);
+
+    wpi::math::MathSharedStore::ReportError(msg);
+    throw std::domain_error(msg);
+  }
+
+  // Turn rotation matrix into a quaternion
+  // https://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
+  double trace = R(0, 0) + R(1, 1) + R(2, 2);
+  double w;
+  double x;
+  double y;
+  double z;
+
+  if (trace > 0.0) {
+    double s = 0.5 / std::sqrt(trace + 1.0);
+    w = 0.25 / s;
+    x = (R(2, 1) - R(1, 2)) * s;
+    y = (R(0, 2) - R(2, 0)) * s;
+    z = (R(1, 0) - R(0, 1)) * s;
+  } else {
+    if (R(0, 0) > R(1, 1) && R(0, 0) > R(2, 2)) {
+      double s = 2.0 * std::sqrt(1.0 + R(0, 0) - R(1, 1) - R(2, 2));
+      w = (R(2, 1) - R(1, 2)) / s;
+      x = 0.25 * s;
+      y = (R(0, 1) + R(1, 0)) / s;
+      z = (R(0, 2) + R(2, 0)) / s;
+    } else if (R(1, 1) > R(2, 2)) {
+      double s = 2.0 * std::sqrt(1.0 + R(1, 1) - R(0, 0) - R(2, 2));
+      w = (R(0, 2) - R(2, 0)) / s;
+      x = (R(0, 1) + R(1, 0)) / s;
+      y = 0.25 * s;
+      z = (R(1, 2) + R(2, 1)) / s;
+    } else {
+      double s = 2.0 * std::sqrt(1.0 + R(2, 2) - R(0, 0) - R(1, 1));
+      w = (R(1, 0) - R(0, 1)) / s;
+      x = (R(0, 2) + R(2, 0)) / s;
+      y = (R(1, 2) + R(2, 1)) / s;
+      z = 0.25 * s;
+    }
+  }
+
+  m_q = Quaternion{w, x, y, z};
+}
+
 Rotation3d::Rotation3d(const Vectord<3>& initial, const Vectord<3>& final) {
   double dot = initial.dot(final);
   double normProduct = initial.norm() * final.norm();

wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.inc

@@ -4,8 +4,6 @@
 
 #pragma once
 
-#include <frc/fmt/Eigen.h>
-
 #include <stdexcept>
 #include <string>
 
@@ -14,6 +12,7 @@
 #include "drake/math/discrete_algebraic_riccati_equation.h"
 #include "frc/StateSpaceUtil.h"
 #include "frc/controller/LinearQuadraticRegulator.h"
+#include "frc/fmt/Eigen.h"
 #include "frc/system/Discretization.h"
 #include "unsupported/Eigen/MatrixFunctions"
 #include "wpimath/MathShared.h"

wpimath/src/main/native/include/frc/geometry/Rotation3d.h

@@ -14,7 +14,7 @@
 namespace frc {
 
 /**
- * A rotation in a 3D coordinate frame.
+ * A rotation in a 3D coordinate frame represented by a quaternion.
  */
 class WPILIB_DLLEXPORT Rotation3d {
  public:
@@ -51,6 +51,14 @@ class WPILIB_DLLEXPORT Rotation3d {
    */
   Rotation3d(const Vectord<3>& axis, units::radian_t angle);
 
+  /**
+   * Constructs a Rotation3d from a rotation matrix.
+   *
+   * @param rotationMatrix The rotation matrix.
+   * @throws std::domain_error if the rotation matrix isn't special orthogonal.
+   */
+  explicit Rotation3d(const Matrixd<3, 3>& rotationMatrix);
+
   /**
    * Constructs a Rotation3d that rotates the initial vector onto the final
    * vector.

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};