Geometry module: Optimize AngleAxis, EulerAngles

libeigen/eigen!2392

Co-authored-by: Rasmus Munk Larsen <rmlarsen@gmail.com>

Eigen/src/Geometry/Quaternion.h

diff --git a/Eigen/src/Geometry/Quaternion.h b/Eigen/src/Geometry/Quaternion.h
index 3dff591..b409833 100644
--- a/Eigen/src/Geometry/Quaternion.h
+++ b/Eigen/src/Geometry/Quaternion.h
@@ -821,12 +821,8 @@
 EIGEN_DEVICE_FUNC inline typename internal::traits<Derived>::Scalar QuaternionBase<Derived>::angularDistance(
     const QuaternionBase<OtherDerived>& other) const {
   EIGEN_USING_STD(atan2)
-  // For unit quaternions: dot = cos(theta/2), so theta = 2*atan2(sin(theta/2), |cos(theta/2)|).
-  // Using atan2 instead of acos for numerical accuracy at small angles.
-  Scalar d = numext::abs(this->dot(other));
-  // Clamp to handle non-unit quaternions gracefully.
-  if (d >= Scalar(1)) return Scalar(0);
-  return Scalar(2) * atan2(numext::sqrt(Scalar(1) - d * d), d);
+  Quaternion<Scalar> d = (*this) * other.conjugate();
+  return Scalar(2) * atan2(d.vec().norm(), numext::abs(d.w()));
 }
 
 /** \returns the spherical linear interpolation between the two quaternions

test/geo_quaternion.cpp

diff --git a/test/geo_quaternion.cpp b/test/geo_quaternion.cpp
index 27c6881..897c5e9 100644
--- a/test/geo_quaternion.cpp
+++ b/test/geo_quaternion.cpp
@@ -104,6 +104,15 @@
     VERIFY_IS_MUCH_SMALLER_THAN(abs(q1.angularDistance(q2) - refangle), Scalar(1));
   }
 
+  // angular distance with non-unit quaternions (scale-invariance)
+  {
+    Quaternionx qunit(AngleAxisx(Scalar(EIGEN_PI / 2), Vector3::UnitX()));
+    Quaternionx qscaled;
+    qscaled.coeffs() = qunit.coeffs() * Scalar(2);
+    VERIFY_IS_APPROX(qscaled.angularDistance(Quaternionx::Identity()), Scalar(EIGEN_PI / 2));
+    VERIFY_IS_APPROX(Quaternionx::Identity().angularDistance(qscaled), Scalar(EIGEN_PI / 2));
+  }
+
   // 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());