[wpimath] Add two-vector Rotation3d constructor (#4398)

This is useful for turning a 3D vector into an orientation relative a
coordinate system vector.

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

@@ -8,6 +8,8 @@
 
 #include <wpi/numbers>
 
+#include "Eigen/Core"
+#include "Eigen/QR"
 #include "units/math.h"
 
 using namespace frc;
@@ -43,6 +45,40 @@ 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 Vectord<3>& initial, const Vectord<3>& final) {
+  double dot = initial.dot(final);
+  double normProduct = initial.norm() * final.norm();
+  double dotNorm = dot / normProduct;
+
+  if (dotNorm > 1.0 - 1E-9) {
+    // If the dot product is 1, the two vectors point in the same direction so
+    // there's no rotation. The default initialization of m_q will work.
+    return;
+  } else if (dotNorm < -1.0 + 1E-9) {
+    // If the dot product is -1, the two vectors point in opposite directions so
+    // a 180 degree rotation is required. Any orthogonal vector can be used for
+    // it. Q in the QR decomposition is an orthonormal basis, so it contains
+    // orthogonal unit vectors.
+    Eigen::Matrix<double, 3, 1> X = initial;
+    Eigen::HouseholderQR<decltype(X)> qr{X};
+    Eigen::Matrix<double, 3, 3> Q = qr.householderQ();
+
+    // w = std::cos(θ/2) = std::cos(90°) = 0
+    //
+    // For x, y, and z, we use the second column of Q because the first is
+    // parallel instead of orthogonal. The third column would also work.
+    m_q = Quaternion{0.0, Q(0, 1), Q(1, 1), Q(2, 1)};
+  } else {
+    // initial x final
+    Eigen::Vector3d axis{initial(1) * final(2) - final(1) * initial(2),
+                         final(0) * initial(2) - initial(0) * final(2),
+                         initial(0) * final(1) - final(0) * initial(1)};
+
+    // https://stackoverflow.com/a/11741520
+    m_q = Quaternion{normProduct + dot, axis(0), axis(1), axis(2)}.Normalize();
+  }
+}
+
 Rotation3d Rotation3d::operator+(const Rotation3d& other) const {
   return RotateBy(other);
 }