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

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.