Geometry module: Speed up setFromTwoVectors and AngleAxis * vector libeigen/eigen!2393 Co-authored-by: Rasmus Munk Larsen <rmlarsen@gmail.com>
diff --git a/Eigen/src/Geometry/AngleAxis.h b/Eigen/src/Geometry/AngleAxis.h index a00ed17..02f3fb2 100644 --- a/Eigen/src/Geometry/AngleAxis.h +++ b/Eigen/src/Geometry/AngleAxis.h
@@ -130,6 +130,18 @@ EIGEN_DEVICE_FUNC AngleAxis& fromRotationMatrix(const MatrixBase<Derived>& m); EIGEN_DEVICE_FUNC Matrix3 toRotationMatrix(void) const; + /** Applies the rotation to a 3D vector using Rodrigues' formula directly, + * without constructing the full rotation matrix. */ + template <typename OtherVectorType> + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Vector3 _transformVector(const OtherVectorType& v) const { + EIGEN_USING_STD(sin) + EIGEN_USING_STD(cos) + // Rodrigues' rotation formula: v' = v*cos(θ) + (k×v)*sin(θ) + k*(k·v)*(1-cos(θ)) + const Scalar c = cos(m_angle); + const Scalar s = sin(m_angle); + return v * c + m_axis.cross(v) * s + m_axis * (m_axis.dot(v) * (Scalar(1) - c)); + } + /** \returns \c *this with scalar type casted to \a NewScalarType * * Note that if \a NewScalarType is equal to the current scalar type of \c *this @@ -197,9 +209,7 @@ template <typename Scalar> template <typename Derived> EIGEN_DEVICE_FUNC AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const MatrixBase<Derived>& mat) { - // Since a direct conversion would not be really faster, - // let's use the robust Quaternion implementation: - return *this = QuaternionType(mat); + return fromRotationMatrix(mat); } /** @@ -208,7 +218,65 @@ template <typename Scalar> template <typename Derived> EIGEN_DEVICE_FUNC AngleAxis<Scalar>& AngleAxis<Scalar>::fromRotationMatrix(const MatrixBase<Derived>& mat) { - return *this = QuaternionType(mat); + EIGEN_USING_STD(atan2) + EIGEN_USING_STD(sqrt) + EIGEN_STATIC_ASSERT( + (internal::is_same<Scalar, typename Derived::Scalar>::value), + YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY) + eigen_assert(mat.cols() == 3 && mat.rows() == 3); + + const typename internal::nested_eval<Derived, 3>::type m(mat); + + // Skew-symmetric part gives sin(angle) * axis. + const Scalar sx = m.coeff(2, 1) - m.coeff(1, 2); + const Scalar sy = m.coeff(0, 2) - m.coeff(2, 0); + const Scalar sz = m.coeff(1, 0) - m.coeff(0, 1); + const Scalar s = sqrt(sx * sx + sy * sy + sz * sz); // = 2*sin(angle) + + // trace = 1 + 2*cos(angle) + const Scalar c = m.trace() - Scalar(1); // = 2*cos(angle) + + // Use atan2 for the angle: accurate at all angles including near 0 and pi. + m_angle = atan2(s, c); + + // Use the skew-symmetric part only when sin(angle) is large enough for + // accurate axis extraction. Near angle=0 or angle=pi, sin(angle) is small + // and the axis must be computed differently. + const Scalar sin_threshold = sqrt(NumTraits<Scalar>::epsilon()); + if (s > sin_threshold) { + // General case: axis from skew-symmetric part. + const Scalar inv_s = Scalar(1) / s; + m_axis << sx * inv_s, sy * inv_s, sz * inv_s; + } else if (c > Scalar(0)) { + // Near identity (angle ≈ 0): axis is arbitrary, use (1,0,0). + m_axis << Scalar(1), Scalar(0), Scalar(0); + } else { + // Near angle = pi: extract axis from the symmetric part (R + I) / 2. + // The axis is the eigenvector corresponding to eigenvalue 1. + // Use the column of (R + I) with the largest diagonal entry for robustness. + const Scalar d0 = m.coeff(0, 0); + const Scalar d1 = m.coeff(1, 1); + const Scalar d2 = m.coeff(2, 2); + if (d0 >= d1 && d0 >= d2) { + // x is the largest component + const Scalar x = sqrt(numext::maxi(d0 - d1 - d2 + Scalar(1), Scalar(0)) * Scalar(0.5)); + const Scalar inv_2x = Scalar(0.5) / (x + NumTraits<Scalar>::epsilon()); + m_axis << x, (m.coeff(0, 1) + m.coeff(1, 0)) * inv_2x, (m.coeff(0, 2) + m.coeff(2, 0)) * inv_2x; + } else if (d1 >= d2) { + // y is the largest component + const Scalar y = sqrt(numext::maxi(d1 - d0 - d2 + Scalar(1), Scalar(0)) * Scalar(0.5)); + const Scalar inv_2y = Scalar(0.5) / (y + NumTraits<Scalar>::epsilon()); + m_axis << (m.coeff(0, 1) + m.coeff(1, 0)) * inv_2y, y, (m.coeff(1, 2) + m.coeff(2, 1)) * inv_2y; + } else { + // z is the largest component + const Scalar z = sqrt(numext::maxi(d2 - d0 - d1 + Scalar(1), Scalar(0)) * Scalar(0.5)); + const Scalar inv_2z = Scalar(0.5) / (z + NumTraits<Scalar>::epsilon()); + m_axis << (m.coeff(0, 2) + m.coeff(2, 0)) * inv_2z, (m.coeff(1, 2) + m.coeff(2, 1)) * inv_2z, z; + } + m_axis.normalize(); + } + + return *this; } /** Constructs and \returns an equivalent 3x3 rotation matrix.
diff --git a/Eigen/src/Geometry/EulerAngles.h b/Eigen/src/Geometry/EulerAngles.h index 9c6a0c1..8e50528 100644 --- a/Eigen/src/Geometry/EulerAngles.h +++ b/Eigen/src/Geometry/EulerAngles.h
@@ -91,8 +91,26 @@ // // Thus: m11.c1 - m21.s1 = c3 & m12.c1 - m22.s1 = s3 - Scalar s1 = numext::sin(res[0]); - Scalar c1 = numext::cos(res[0]); + // Recover sin(res[0]) and cos(res[0]) from the atan2 arguments directly, + // avoiding a redundant sin+cos evaluation. s2 = hypot(coeff(j,i), coeff(k,i)) + // is the norm of the atan2 arguments (with sign adjustment for !odd). + Scalar s1, c1; + if (s2 > NumTraits<Scalar>::epsilon()) { + Scalar inv_s2 = Scalar(1) / s2; + if (odd) { + // res[0] = atan2(coeff(j,i), coeff(k,i)) + s1 = coeff(j, i) * inv_s2; + c1 = coeff(k, i) * inv_s2; + } else { + // res[0] = atan2(-coeff(j,i), -coeff(k,i)) + s1 = -coeff(j, i) * inv_s2; + c1 = -coeff(k, i) * inv_s2; + } + } else { + // Gimbal lock (s2 ≈ 0): recover sin/cos from the computed angle. + s1 = numext::sin(res[0]); + c1 = numext::cos(res[0]); + } res[2] = numext::atan2(c1 * coeff(j, k) - s1 * coeff(k, k), c1 * coeff(j, j) - s1 * coeff(k, j)); } else { // Tait-Bryan angles (all three axes are different; typically used for yaw-pitch-roll calculations). @@ -102,6 +120,11 @@ // c2s3 s2s1s3 + c1c3 s2c1s3 - s1c3 // -s2 c2s1 c2c1 + // Recover sin(res[0]) and cos(res[0]) from the atan2 arguments directly: + // res[0] = atan2(coeff(j,k), coeff(k,k)) + // sin(res[0]) = coeff(j,k) / hypot(coeff(j,k), coeff(k,k)) + // cos(res[0]) = coeff(k,k) / hypot(coeff(j,k), coeff(k,k)) + Scalar n1 = numext::hypot(coeff(j, k), coeff(k, k)); res[0] = numext::atan2(coeff(j, k), coeff(k, k)); Scalar c2 = numext::hypot(coeff(i, i), coeff(i, j)); @@ -109,8 +132,17 @@ // range [-pi/2, pi/2] res[1] = numext::atan2(-coeff(i, k), c2); - Scalar s1 = numext::sin(res[0]); - Scalar c1 = numext::cos(res[0]); + Scalar s1, c1; + if (n1 > NumTraits<Scalar>::epsilon()) { + Scalar inv_n1 = Scalar(1) / n1; + s1 = coeff(j, k) * inv_n1; + c1 = coeff(k, k) * inv_n1; + } else { + // Gimbal lock: coeff(j,k) and coeff(k,k) are both near zero. + // Fall back to sin/cos of the computed angle. + s1 = numext::sin(res[0]); + c1 = numext::cos(res[0]); + } res[2] = numext::atan2(s1 * coeff(k, i) - c1 * coeff(j, i), c1 * coeff(j, j) - s1 * coeff(k, j)); } if (!odd) {
diff --git a/Eigen/src/Geometry/Quaternion.h b/Eigen/src/Geometry/Quaternion.h index bb6e962..3dff591 100644 --- a/Eigen/src/Geometry/Quaternion.h +++ b/Eigen/src/Geometry/Quaternion.h
@@ -690,19 +690,10 @@ Scalar c = v1.dot(v0); // if dot == -1, vectors are nearly opposites - // => accurately compute the rotation axis by computing the - // intersection of the two planes. This is done by solving: - // x^T v0 = 0 - // x^T v1 = 0 - // under the constraint: - // ||x|| = 1 - // which yields a singular value problem + // => any axis perpendicular to v0 will do for a ~180 degree rotation. if (c < Scalar(-1) + NumTraits<Scalar>::dummy_precision()) { c = numext::maxi(c, Scalar(-1)); - Matrix<Scalar, 2, 3> m; - m << v0.transpose(), v1.transpose(); - JacobiSVD<Matrix<Scalar, 2, 3>, ComputeFullV> svd(m); - Vector3 axis = svd.matrixV().col(2); + Vector3 axis = v0.unitOrthogonal(); Scalar w2 = (Scalar(1) + c) * Scalar(0.5); this->w() = sqrt(w2); @@ -830,8 +821,12 @@ EIGEN_DEVICE_FUNC inline typename internal::traits<Derived>::Scalar QuaternionBase<Derived>::angularDistance( const QuaternionBase<OtherDerived>& other) const { EIGEN_USING_STD(atan2) - Quaternion<Scalar> d = (*this) * other.conjugate(); - return Scalar(2) * atan2(d.vec().norm(), numext::abs(d.w())); + // 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); } /** \returns the spherical linear interpolation between the two quaternions
diff --git a/benchmarks/Geometry/bench_geometry_ops.cpp b/benchmarks/Geometry/bench_geometry_ops.cpp index 4d93533..cb743c5 100644 --- a/benchmarks/Geometry/bench_geometry_ops.cpp +++ b/benchmarks/Geometry/bench_geometry_ops.cpp
@@ -92,6 +92,17 @@ } } +template <typename Scalar> +static void BM_QuatSetFromTwoVectorsAntiparallel(benchmark::State& state) { + Matrix<Scalar, 3, 1> v0 = Matrix<Scalar, 3, 1>::Random().normalized(); + Matrix<Scalar, 3, 1> v1 = -v0; // exactly antiparallel + Quaternion<Scalar> q; + for (auto _ : state) { + q.setFromTwoVectors(v0, v1); + benchmark::DoNotOptimize(q.coeffs().data()); + } +} + // ============================================================================ // AngleAxis operations // ============================================================================ @@ -325,6 +336,8 @@ BENCHMARK(BM_QuatAngularDistance<double>); BENCHMARK(BM_QuatSetFromTwoVectors<float>); BENCHMARK(BM_QuatSetFromTwoVectors<double>); +BENCHMARK(BM_QuatSetFromTwoVectorsAntiparallel<float>); +BENCHMARK(BM_QuatSetFromTwoVectorsAntiparallel<double>); // AngleAxis BENCHMARK(BM_AngleAxisToRotationMatrix<float>);