// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2025 Rasmus Munk Larsen <rmlarsen@google.com>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
// SPDX-License-Identifier: MPL-2.0

// Tests for Geometry module classes that lack dedicated test coverage:
//   - AngleAxis edge cases
//   - Rotation2D standalone tests
//   - Translation standalone tests
//   - Scaling standalone tests
//   - Quaternion fromRotationMatrix with non-orthogonal input
//   - Quaternion slerp edge cases
//   - Umeyama degenerate input
//   - Transform shear ground truth

#include "main.h"
#include <Eigen/Geometry>
#include <Eigen/LU>
#include <Eigen/SVD>

template <typename Scalar>
void angleaxis_edge_cases() {
  typedef Matrix<Scalar, 3, 1> Vector3;
  typedef Matrix<Scalar, 3, 3> Matrix3;
  typedef AngleAxis<Scalar> AngleAxisx;
  typedef Quaternion<Scalar> Quaternionx;

  Scalar eps = test_precision<Scalar>();

  // Identity rotation (angle = 0)
  {
    AngleAxisx aa(Scalar(0), Vector3::UnitZ());
    Matrix3 m = aa.toRotationMatrix();
    VERIFY_IS_APPROX(m, Matrix3::Identity());
    // Verify orthogonality: R^T * R = I
    VERIFY_IS_APPROX(m.transpose() * m, Matrix3::Identity());
    VERIFY_IS_APPROX(m.determinant(), Scalar(1));
    Quaternionx q(aa);
    VERIFY_IS_APPROX(q.toRotationMatrix(), Matrix3::Identity());
    VERIFY_IS_APPROX(q.norm(), Scalar(1));
  }

  // 180-degree rotation around each axis
  {
    for (int axis = 0; axis < 3; ++axis) {
      Vector3 ax = Vector3::Zero();
      ax(axis) = Scalar(1);
      AngleAxisx aa(Scalar(EIGEN_PI), ax);
      Matrix3 m = aa.toRotationMatrix();
      // Should be its own inverse
      VERIFY_IS_APPROX(m * m, Matrix3::Identity());
      // Verify orthogonality
      VERIFY_IS_APPROX(m.transpose() * m, Matrix3::Identity());
      VERIFY_IS_APPROX(m.determinant(), Scalar(1));
      // Diagonal entry for the rotation axis should be +1, others -1
      VERIFY_IS_APPROX(m(axis, axis), Scalar(1));
      for (int j = 0; j < 3; ++j) {
        if (j != axis) VERIFY_IS_APPROX(m(j, j), Scalar(-1));
      }
    }
  }

  // Very small angle: verify Rodrigues formula accuracy
  {
    Scalar tiny = Scalar(10) * NumTraits<Scalar>::epsilon();
    AngleAxisx aa(tiny, Vector3::UnitX());
    Matrix3 m = aa.toRotationMatrix();
    VERIFY((m - Matrix3::Identity()).norm() < Scalar(4) * tiny);
    // Orthogonality should hold to machine precision even for tiny angles
    VERIFY_IS_APPROX(m.transpose() * m, Matrix3::Identity());
    VERIFY_IS_APPROX(m.determinant(), Scalar(1));
    // Round-trip through quaternion
    Quaternionx q(aa);
    AngleAxisx aa2(q);
    VERIFY(numext::abs(aa2.angle()) < Scalar(4) * tiny);
  }

  // Round-trip: AngleAxis -> Matrix -> AngleAxis
  {
    Scalar a = internal::random<Scalar>(Scalar(0.1), Scalar(EIGEN_PI) - Scalar(0.1));
    Vector3 axis = Vector3::Random().normalized();
    AngleAxisx aa(a, axis);
    Matrix3 m = aa.toRotationMatrix();
    VERIFY_IS_APPROX(m.transpose() * m, Matrix3::Identity());
    VERIFY_IS_APPROX(m.determinant(), Scalar(1));
    AngleAxisx aa2;
    aa2.fromRotationMatrix(m);
    VERIFY_IS_APPROX(aa.toRotationMatrix(), aa2.toRotationMatrix());
    // Verify angle is preserved
    VERIFY(numext::abs(aa2.angle() - a) < eps);
  }

  // Round-trip for near-180-degree rotation
  {
    Scalar a = Scalar(EIGEN_PI) - Scalar(10) * NumTraits<Scalar>::epsilon();
    Vector3 axis = Vector3::Random().normalized();
    AngleAxisx aa(a, axis);
    Matrix3 m = aa.toRotationMatrix();
    VERIFY_IS_APPROX(m.transpose() * m, Matrix3::Identity());
    VERIFY_IS_APPROX(m.determinant(), Scalar(1));
    AngleAxisx aa2(m);
    // The rotation should be equivalent even if axis flips sign
    VERIFY_IS_APPROX(aa.toRotationMatrix(), aa2.toRotationMatrix());
  }

  // Near-180-degree rotation with negative dominant axis component:
  // when sin(angle) is well above machine epsilon (e.g. angle = pi - 0.25 * sqrt(eps)),
  // the axis sign is well-defined and must not flip.
  {
    Scalar delta = Scalar(0.25) * numext::sqrt(NumTraits<Scalar>::epsilon());
    Scalar a = Scalar(EIGEN_PI) - delta;
    for (int d = 0; d < 3; ++d) {
      Vector3 axis(Scalar(0.1), Scalar(-0.2), Scalar(0.15));
      axis(d) = Scalar(-1);
      axis.normalize();
      AngleAxisx aa(a, axis);
      Matrix3 m = aa.toRotationMatrix();
      AngleAxisx aa2;
      aa2.fromRotationMatrix(m);
      VERIFY_IS_APPROX(aa2.axis(), axis);
      VERIFY_IS_APPROX(aa2.angle(), a);
    }
  }

  // Near-180-degree rotation extracted from a composed rotation matrix
  // R_parent^T * (R_parent * R_true): rounding errors in R must not be
  // amplified by 1 / sin(angle).
  {
    Vector3 r_true(Scalar(-3.134111), Scalar(-0.011655), Scalar(-0.003976));
    AngleAxisx aa_true(r_true.norm(), r_true.normalized());
    Matrix3 R_true = aa_true.toRotationMatrix();
    Vector3 r_parent(Scalar(1.946572), Scalar(-1.908926), Scalar(0.648041));
    Matrix3 R_parent = AngleAxisx(r_parent.norm(), r_parent.normalized()).toRotationMatrix();
    Matrix3 R_composed = R_parent.transpose() * (R_parent * R_true);
    AngleAxisx aa_extracted(R_composed);
    Vector3 r_extracted = aa_extracted.angle() * aa_extracted.axis();
    VERIFY((r_extracted - r_true).norm() < Scalar(16) * NumTraits<Scalar>::epsilon());
  }

  // Accessors
  {
    Vector3 axis = Vector3::Random().normalized();
    Scalar angle = internal::random<Scalar>(Scalar(-EIGEN_PI), Scalar(EIGEN_PI));
    AngleAxisx aa(angle, axis);
    VERIFY_IS_APPROX(aa.angle(), angle);
    VERIFY_IS_APPROX(aa.axis(), axis);

    // Mutable accessors
    aa.angle() = Scalar(1);
    aa.axis() = Vector3::UnitZ();
    VERIFY_IS_APPROX(aa.angle(), Scalar(1));
    VERIFY_IS_APPROX(aa.axis(), Vector3::UnitZ());
  }

  // inverse
  {
    Scalar a = internal::random<Scalar>(Scalar(0.1), Scalar(EIGEN_PI));
    Vector3 axis = Vector3::Random().normalized();
    AngleAxisx aa(a, axis);
    VERIFY_IS_APPROX((aa * aa.inverse()).toRotationMatrix(), Matrix3::Identity());
  }

  // isApprox
  {
    Scalar a = internal::random<Scalar>(Scalar(0.1), Scalar(EIGEN_PI));
    Vector3 axis = Vector3::Random().normalized();
    AngleAxisx aa1(a, axis);
    AngleAxisx aa2(a + eps / Scalar(10), axis);
    VERIFY(aa1.isApprox(aa2, eps));
  }

  // Quaternion round-trip: AngleAxis -> Quaternion -> AngleAxis
  {
    Scalar a = internal::random<Scalar>(Scalar(0.1), Scalar(EIGEN_PI) - Scalar(0.1));
    Vector3 axis = Vector3::Random().normalized();
    AngleAxisx aa(a, axis);
    Quaternionx q(aa);
    VERIFY_IS_APPROX(q.norm(), Scalar(1));
    AngleAxisx aa2(q);
    VERIFY(numext::abs(aa2.angle() - a) < eps);
    // Axis should agree (possibly with sign flip for angle > pi)
    VERIFY(aa2.axis().isApprox(axis, eps) || aa2.axis().isApprox(-axis, eps));
  }
}

template <typename Scalar>
void rotation2d_standalone() {
  typedef Matrix<Scalar, 2, 1> Vector2;
  typedef Matrix<Scalar, 2, 2> Matrix2;
  typedef Rotation2D<Scalar> Rotation2Dx;

  Scalar eps = test_precision<Scalar>();

  // Construction and basic properties
  {
    Rotation2Dx r(Scalar(0));
    VERIFY_IS_APPROX(r.toRotationMatrix(), Matrix2::Identity());
  }

  // 90-degree rotation
  {
    Rotation2Dx r(Scalar(EIGEN_PI) / Scalar(2));
    Vector2 v(1, 0);
    Vector2 rv = r * v;
    VERIFY_IS_APPROX(rv, Vector2(0, 1));
  }

  // 180-degree rotation
  {
    Rotation2Dx r(Scalar(EIGEN_PI));
    Vector2 v(1, 0);
    Vector2 rv = r * v;
    VERIFY_IS_APPROX(rv, Vector2(-1, 0));
  }

  // inverse
  {
    Scalar a = internal::random<Scalar>(-Scalar(EIGEN_PI), Scalar(EIGEN_PI));
    Rotation2Dx r(a);
    VERIFY_IS_APPROX((r * r.inverse()).toRotationMatrix(), Matrix2::Identity());
  }

  // composition
  {
    Scalar a = internal::random<Scalar>(-Scalar(EIGEN_PI), Scalar(EIGEN_PI));
    Scalar b = internal::random<Scalar>(-Scalar(EIGEN_PI), Scalar(EIGEN_PI));
    Rotation2Dx ra(a), rb(b);
    VERIFY_IS_APPROX((ra * rb).toRotationMatrix(), ra.toRotationMatrix() * rb.toRotationMatrix());
  }

  // fromRotationMatrix round-trip: angle should agree to tight tolerance
  {
    Scalar a = internal::random<Scalar>(-Scalar(EIGEN_PI), Scalar(EIGEN_PI));
    Rotation2Dx r(a);
    Matrix2 m = r.toRotationMatrix();
    // Verify orthogonality
    VERIFY_IS_APPROX(m.transpose() * m, Matrix2::Identity());
    VERIFY_IS_APPROX(m.determinant(), Scalar(1));
    Rotation2Dx r2(m);
    // Angles should agree mod 2*pi. Use rotation matrices for exact comparison.
    VERIFY_IS_APPROX(r2.toRotationMatrix(), r.toRotationMatrix());
  }

  // Rotation2D slerp interpolation: verify linearity of angle
  {
    Scalar a0 = internal::random<Scalar>(Scalar(-EIGEN_PI), Scalar(EIGEN_PI));
    Scalar a1 = a0 + internal::random<Scalar>(Scalar(0.1), Scalar(1.0));
    Rotation2Dx r0(a0), r1(a1);
    Scalar t = Scalar(0.3);
    Rotation2Dx rt = r0.slerp(t, r1);
    // Slerp for 2D rotations is just linear interpolation of angle along shortest path.
    Scalar expected = a0 + t * Rotation2Dx(a1 - a0).smallestAngle();
    VERIFY(numext::abs(Rotation2Dx(rt.angle() - expected).smallestAngle()) < eps);
  }
}

template <typename Scalar>
void translation_standalone() {
  typedef Matrix<Scalar, 3, 1> Vector3;
  typedef Translation<Scalar, 3> Translation3;
  typedef Transform<Scalar, 3, Isometry> Isometry3;

  // Construction
  {
    Vector3 v(1, 2, 3);
    Translation3 t(v);
    VERIFY_IS_APPROX(t.vector(), v);

    Translation3 t2(Scalar(1), Scalar(2), Scalar(3));
    VERIFY_IS_APPROX(t.vector(), t2.vector());
  }

  // Identity
  {
    Translation3 t = Translation3::Identity();
    VERIFY_IS_APPROX(t.vector(), Vector3::Zero());
  }

  // Composition
  {
    Vector3 v1(1, 2, 3), v2(4, 5, 6);
    Translation3 t1(v1), t2(v2);
    Translation3 t3 = t1 * t2;
    VERIFY_IS_APPROX(t3.vector(), v1 + v2);
  }

  // Inverse
  {
    Vector3 v(1, 2, 3);
    Translation3 t(v);
    Translation3 tinv = t.inverse();
    VERIFY_IS_APPROX(tinv.vector(), -v);
    VERIFY_IS_APPROX((t * tinv).vector(), Vector3::Zero());
  }

  // Translation * vector (via transform)
  {
    Vector3 v(1, 2, 3), p(10, 20, 30);
    Translation3 t(v);
    VERIFY_IS_APPROX(t * p, v + p);
  }

  // Casting
  {
    Translation3 t(Vector3(1, 2, 3));
    Translation<float, 3> tf = t.template cast<float>();
    VERIFY_IS_APPROX(tf.template cast<Scalar>().vector(), t.vector());
  }

  // Convert to Isometry
  {
    Vector3 v(1, 2, 3);
    Translation3 t(v);
    Isometry3 iso(t);
    VERIFY_IS_APPROX(iso.translation(), v);
    typedef Matrix<Scalar, 3, 3> Matrix3;
    VERIFY_IS_APPROX(iso.linear(), Matrix3::Identity());
  }
}

template <typename Scalar>
void scaling_standalone() {
  typedef Matrix<Scalar, 3, 1> Vector3;
  typedef Translation<Scalar, 3> Translation3;
  typedef Transform<Scalar, 3, Affine> Affine3;
  typedef UniformScaling<Scalar> UniformScalingx;

  // UniformScaling construction
  {
    UniformScalingx s(Scalar(2));
    VERIFY_IS_APPROX(s.factor(), Scalar(2));
  }

  // UniformScaling * vector
  {
    UniformScalingx s(Scalar(3));
    Vector3 v(1, 2, 3);
    // UniformScaling * Translation
    Translation3 t(v);
    Affine3 result = s * t;
    VERIFY_IS_APPROX(result.translation(), v * Scalar(3));
  }

  // UniformScaling composition
  {
    UniformScalingx s1(Scalar(2)), s2(Scalar(3));
    UniformScalingx s3 = s1 * s2;
    VERIFY_IS_APPROX(s3.factor(), Scalar(6));
  }

  // UniformScaling inverse
  {
    UniformScalingx s(Scalar(4));
    UniformScalingx si = s.inverse();
    VERIFY_IS_APPROX(si.factor(), Scalar(1) / Scalar(4));
    VERIFY_IS_APPROX((s * si).factor(), Scalar(1));
  }

  // Scaling convenience functions
  {
    Scalar s = Scalar(3);
    auto su = Eigen::Scaling(s);
    VERIFY_IS_APPROX(su.factor(), s);

    Vector3 sv(1, 2, 3);
    auto sd = Eigen::Scaling(sv);
    VERIFY_IS_APPROX(Vector3(sd.diagonal()), sv);
  }
}

template <typename Scalar>
void quaternion_from_non_orthogonal_matrix() {
  typedef Matrix<Scalar, 3, 3> Matrix3;
  typedef Quaternion<Scalar> Quaternionx;

  // Slightly non-orthogonal matrix should not produce NaN
  {
    Matrix3 m = Matrix3::Identity();
    // Perturb to make slightly non-orthogonal
    m(0, 0) += Scalar(1e-6);
    m(1, 1) -= Scalar(1e-6);
    Quaternionx q(m);
    // Should not be NaN
    VERIFY(!(numext::isnan)(q.w()));
    VERIFY(!(numext::isnan)(q.x()));
    VERIFY(!(numext::isnan)(q.y()));
    VERIFY(!(numext::isnan)(q.z()));
    // Should still be approximately a rotation
    VERIFY(numext::abs(q.norm() - Scalar(1)) < Scalar(0.01));
  }

  // Negative trace case with non-orthogonal matrix
  {
    // 180-degree rotation around Z axis, slightly perturbed
    Matrix3 m;
    m << Scalar(-1), Scalar(0), Scalar(0), Scalar(0), Scalar(-1), Scalar(0), Scalar(0), Scalar(0), Scalar(1);
    m(0, 0) += Scalar(1e-6);
    Quaternionx q(m);
    VERIFY(!(numext::isnan)(q.w()));
    VERIFY(!(numext::isnan)(q.x()));
    VERIFY(!(numext::isnan)(q.y()));
    VERIFY(!(numext::isnan)(q.z()));
  }

  // Matrix with trace very close to -1 (worst case for positive trace branch)
  {
    Matrix3 m;
    m << Scalar(-1), Scalar(0), Scalar(0), Scalar(0), Scalar(-0.5), Scalar(0), Scalar(0), Scalar(0), Scalar(-0.5);
    // trace = -2, negative trace path
    Quaternionx q(m);
    VERIFY(!(numext::isnan)(q.w()));
    VERIFY(!(numext::isnan)(q.x()));
  }

  // Verify graceful handling of arbitrary non-rotation matrices.
  // Shoemake's argmax-diagonal selection makes the sqrt argument non-negative
  // for any real matrix, but the numext::maxi guard provides additional safety.
  {
    Matrix3 m;
    m << Scalar(-0.4), Scalar(0.5), Scalar(0), Scalar(-0.5), Scalar(-0.4), Scalar(0), Scalar(0), Scalar(0),
        Scalar(-0.4);
    Quaternionx q(m);
    VERIFY(!(numext::isnan)(q.w()));
    VERIFY(!(numext::isnan)(q.x()));
    VERIFY(!(numext::isnan)(q.y()));
    VERIFY(!(numext::isnan)(q.z()));
  }

  // Pivot component must be rounded as 0.5 * sqrt(t) (single rounding, <= 0.5 ULP)
  // rather than t * (0.5 / sqrt(t)) (which loses 1 ULP for t = 2 and 2 ULP for
  // t = 3 in double, degrading downstream extraction on 90-deg and 60-deg rotations).
  {
    Matrix3 r90;
    r90 << Scalar(0), Scalar(0), Scalar(1), Scalar(0), Scalar(1), Scalar(0), Scalar(-1), Scalar(0), Scalar(0);
    Quaternionx q90(r90);
    VERIFY_IS_EQUAL(q90.w(), Scalar(0.5) * numext::sqrt(Scalar(2)));

    Matrix3 r180_xy;
    r180_xy << Scalar(0), Scalar(1), Scalar(0), Scalar(1), Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(-1);
    Quaternionx q180(r180_xy);
    VERIFY_IS_EQUAL(q180.x(), Scalar(0.5) * numext::sqrt(Scalar(2)));

    const Scalar s60 = numext::sqrt(Scalar(3)) * Scalar(0.5);
    Matrix3 r60;
    r60 << Scalar(0.5), -s60, Scalar(0), s60, Scalar(0.5), Scalar(0), Scalar(0), Scalar(0), Scalar(1);
    Quaternionx q60(r60);
    VERIFY_IS_EQUAL(q60.w(), Scalar(0.5) * numext::sqrt(Scalar(3)));
  }
}

template <typename Scalar>
void quaternion_slerp_edge_cases() {
  typedef Quaternion<Scalar> Quaternionx;
  typedef AngleAxis<Scalar> AngleAxisx;
  typedef Matrix<Scalar, 3, 1> Vector3;
  typedef Matrix<Scalar, 3, 3> Matrix3;

  // The slerp formula preserves unit norm to O(epsilon^2) for unit inputs.
  Scalar tight = Scalar(32) * NumTraits<Scalar>::epsilon();

  // slerp with identical quaternions
  {
    Quaternionx q = Quaternionx::UnitRandom();
    Quaternionx r = q.slerp(Scalar(0.5), q);
    VERIFY_IS_APPROX(r.coeffs(), q.coeffs());
    VERIFY(numext::abs(r.norm() - Scalar(1)) < tight);
  }

  // slerp at t=0 and t=1
  {
    Quaternionx q0 = Quaternionx::UnitRandom();
    Quaternionx q1 = Quaternionx::UnitRandom();
    Quaternionx r0 = q0.slerp(Scalar(0), q1);
    Quaternionx r1 = q0.slerp(Scalar(1), q1);
    VERIFY(numext::abs(r0.norm() - Scalar(1)) < tight);
    VERIFY(numext::abs(r1.norm() - Scalar(1)) < tight);
    VERIFY_IS_APPROX(r0.toRotationMatrix(), q0.toRotationMatrix());
    VERIFY_IS_APPROX(r1.toRotationMatrix(), q1.toRotationMatrix());
  }

  // slerp with antipodal quaternions (represent same rotation)
  {
    Quaternionx q = Quaternionx::UnitRandom();
    Quaternionx qn;
    qn.coeffs() = -q.coeffs();
    for (Scalar t = 0; t <= Scalar(1.001); t += Scalar(0.25)) {
      Quaternionx r = q.slerp(t, qn);
      VERIFY(numext::abs(r.norm() - Scalar(1)) < tight);
      // Should stay at the same rotation
      VERIFY_IS_APPROX(r.toRotationMatrix(), q.toRotationMatrix());
    }
  }

  // slerp with 180-degree apart quaternions
  {
    Vector3 axis1 = Vector3::UnitX();
    Quaternionx q0(AngleAxisx(Scalar(0), axis1));
    Quaternionx q1(AngleAxisx(Scalar(EIGEN_PI), axis1));
    Quaternionx mid = q0.slerp(Scalar(0.5), q1);
    VERIFY(numext::abs(mid.norm() - Scalar(1)) < tight);
    // Midpoint should be a 90-degree rotation
    AngleAxisx aa(mid);
    VERIFY(numext::abs(aa.angle() - Scalar(EIGEN_PI) / Scalar(2)) < test_precision<Scalar>());
  }

  // slerp unit-norm preservation and rotation matrix orthogonality
  {
    Quaternionx q0 = Quaternionx::UnitRandom();
    Quaternionx q1 = Quaternionx::UnitRandom();
    for (Scalar t = 0; t <= Scalar(1.001); t += Scalar(0.05)) {
      Quaternionx r = q0.slerp(t, q1);
      VERIFY(numext::abs(r.norm() - Scalar(1)) < tight);
      Matrix3 m = r.toRotationMatrix();
      VERIFY_IS_APPROX(m.transpose() * m, Matrix3::Identity());
      VERIFY_IS_APPROX(m.determinant(), Scalar(1));
    }
  }

  // slerp monotonicity: angle from q0 should increase monotonically
  {
    Quaternionx q0 = Quaternionx::UnitRandom();
    Quaternionx q1 = Quaternionx::UnitRandom();
    Scalar prev_angle = Scalar(0);
    for (Scalar t = Scalar(0); t <= Scalar(1.001); t += Scalar(0.05)) {
      Quaternionx r = q0.slerp(t, q1);
      Scalar angle = AngleAxisx(r * q0.inverse()).angle();
      if (angle > Scalar(EIGEN_PI)) angle = Scalar(2) * Scalar(EIGEN_PI) - angle;
      VERIFY(angle >= prev_angle - tight);
      prev_angle = angle;
    }
  }
}

template <typename Scalar>
void umeyama_degenerate() {
  typedef Matrix<Scalar, 3, Dynamic> Points3;
  typedef Matrix<Scalar, 3, 1> Vector3;
  typedef Matrix<Scalar, 3, 3> Matrix3;
  typedef Matrix<Scalar, 4, 4> Matrix4;

  // All source points identical
  {
    int n = 10;
    Points3 src(3, n);
    src.colwise() = Vector3(1, 2, 3);
    Points3 dst = Points3::Random(3, n);

    Matrix4 T = umeyama(src, dst);
    // Result should NOT be NaN
    VERIFY(!(numext::isnan)(T(0, 0)));
    VERIFY(!(numext::isnan)(T(0, 3)));
  }

  // All destination points identical
  {
    int n = 10;
    Points3 src = Points3::Random(3, n);
    Points3 dst(3, n);
    dst.colwise() = Vector3(4, 5, 6);

    Matrix4 T = umeyama(src, dst);
    VERIFY(!(numext::isnan)(T(0, 0)));
  }

  // Pure translation (no rotation or scaling)
  {
    int n = 20;
    Points3 src = Points3::Random(3, n);
    Vector3 offset(10, 20, 30);
    Points3 dst = src.colwise() + offset;

    Matrix4 T = umeyama(src, dst);
    // Translation should be recovered
    Vector3 t_recovered = T.template block<3, 1>(0, 3);
    VERIFY_IS_APPROX(t_recovered, offset);
    // Rotation should be identity
    Matrix3 R = T.template block<3, 3>(0, 0);
    VERIFY(R.isApprox(Matrix3::Identity(), test_precision<Scalar>()));
  }
}

template <typename Scalar>
void shear_ground_truth() {
  typedef Transform<Scalar, 2, Affine> Transform2;
  typedef Matrix<Scalar, 2, 1> Vector2;

  // Verify the actual matrix produced by shear()
  // Note: Eigen's shear(sx, sy) applies the matrix [[1, sy], [sx, 1]] to columns,
  // meaning sx controls the y-component shear and sy controls the x-component shear.
  {
    Transform2 t;
    t.setIdentity();
    t.shear(Scalar(2), Scalar(0));
    // After shear(2, 0), linear part should be [[1, 0], [2, 1]]
    VERIFY_IS_APPROX(t.linear()(0, 0), Scalar(1));
    VERIFY_IS_APPROX(t.linear()(0, 1), Scalar(0));
    VERIFY_IS_APPROX(t.linear()(1, 0), Scalar(2));
    VERIFY_IS_APPROX(t.linear()(1, 1), Scalar(1));
  }

  {
    Transform2 t;
    t.setIdentity();
    t.shear(Scalar(0), Scalar(3));
    // After shear(0, 3), linear part should be [[1, 3], [0, 1]]
    VERIFY_IS_APPROX(t.linear()(0, 0), Scalar(1));
    VERIFY_IS_APPROX(t.linear()(0, 1), Scalar(3));
    VERIFY_IS_APPROX(t.linear()(1, 0), Scalar(0));
    VERIFY_IS_APPROX(t.linear()(1, 1), Scalar(1));
  }

  // Verify shear on a point
  {
    Transform2 t;
    t.setIdentity();
    t.shear(Scalar(2), Scalar(3));
    Vector2 v(1, 0);
    Vector2 result = t.linear() * v;
    // [[1, 3], [2, 1]] * [1, 0]^T = [1, 2]
    VERIFY_IS_APPROX(result, Vector2(1, 2));

    v = Vector2(0, 1);
    result = t.linear() * v;
    // [[1, 3], [2, 1]] * [0, 1]^T = [3, 1]
    VERIFY_IS_APPROX(result, Vector2(3, 1));
  }
}

EIGEN_DECLARE_TEST(geo_misc) {
  for (int i = 0; i < g_repeat; i++) {
    CALL_SUBTEST_1(angleaxis_edge_cases<float>());
    CALL_SUBTEST_2(angleaxis_edge_cases<double>());

    CALL_SUBTEST_3(rotation2d_standalone<float>());
    CALL_SUBTEST_4(rotation2d_standalone<double>());

    CALL_SUBTEST_5(translation_standalone<float>());
    CALL_SUBTEST_5(translation_standalone<double>());

    CALL_SUBTEST_6(scaling_standalone<float>());
    CALL_SUBTEST_6(scaling_standalone<double>());

    CALL_SUBTEST_7(quaternion_from_non_orthogonal_matrix<float>());
    CALL_SUBTEST_7(quaternion_from_non_orthogonal_matrix<double>());

    CALL_SUBTEST_8(quaternion_slerp_edge_cases<float>());
    CALL_SUBTEST_8(quaternion_slerp_edge_cases<double>());

    CALL_SUBTEST_9(umeyama_degenerate<float>());
    CALL_SUBTEST_9(umeyama_degenerate<double>());

    CALL_SUBTEST_10(shear_ground_truth<float>());
    CALL_SUBTEST_10(shear_ground_truth<double>());
  }
}