From bc57b926a0214340c67d91501f4672a014ecf8fe Mon Sep 17 00:00:00 2001
From: H S Helson Go <hellston20a@gmail.com>
Date: Tue, 6 Jun 2023 21:51:22 -0400
Subject: [PATCH] Add Quaternion constructor from real scalar and imaginary
 vector

---
 Eigen/src/Geometry/Quaternion.h | 9 +++++++++
 test/geo_quaternion.cpp         | 4 ++++
 2 files changed, 13 insertions(+)

diff --git a/Eigen/src/Geometry/Quaternion.h b/Eigen/src/Geometry/Quaternion.h
index 0aca4c471..3413a5197 100644
--- a/Eigen/src/Geometry/Quaternion.h
+++ b/Eigen/src/Geometry/Quaternion.h
@@ -298,6 +298,15 @@ public:
     */
   EIGEN_DEVICE_FUNC inline Quaternion(const Scalar& w, const Scalar& x, const Scalar& y, const Scalar& z) : m_coeffs(x, y, z, w){}
 
+  /** Constructs and initializes a quaternion from its real part as a scalar,
+   *  and its imaginary part as a 3-vector [\c x, \c y, \c z]
+   */
+  template <typename Derived>
+  EIGEN_DEVICE_FUNC inline Quaternion(const Scalar& w, const Eigen::MatrixBase<Derived>& vec)
+      : m_coeffs(vec.x(), vec.y(), vec.z(), w) {
+    EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 3);
+  }
+
   /** Constructs and initialize a quaternion from the array data */
   EIGEN_DEVICE_FUNC explicit inline Quaternion(const Scalar* data) : m_coeffs(data) {}
 
diff --git a/test/geo_quaternion.cpp b/test/geo_quaternion.cpp
index a821cf21d..2eef40bc0 100644
--- a/test/geo_quaternion.cpp
+++ b/test/geo_quaternion.cpp
@@ -98,6 +98,10 @@ template<typename Scalar, int Options> void quaternion(void)
     VERIFY_IS_MUCH_SMALLER_THAN(abs(q1.angularDistance(q2) - refangle), Scalar(1));
   }
 
+  // Action on vector by the q v q* formula
+  VERIFY_IS_APPROX(q1 * v2, (q1 * Quaternionx(Scalar(0), v2) * q1.inverse()).vec());
+  VERIFY_IS_APPROX(q1.inverse() * v2, (q1.inverse() * Quaternionx(Scalar(0), v2) * q1).vec());
+
   // rotation matrix conversion
   VERIFY_IS_APPROX(q1 * v2, q1.toRotationMatrix() * v2);
   VERIFY_IS_APPROX(q1 * q2 * v2,