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

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.