apex_manifolds/

rn.rs

//! Rn - n-dimensional Euclidean Space
//!
//! This module implements the n-dimensional Euclidean space Rⁿ with vector addition
//! as the group operation.
//!
//! Rⁿ elements are represented using nalgebra's `DVector<f64>` for dynamic sizing.
//! Rⁿ tangent elements are also represented as `DVector<f64>` since the tangent space
//! is isomorphic to the manifold itself.
//!
//! # Dimension limits
//!
//! Most operations accept any `n`, but the two that leave Rⁿ do not:
//!
//! - [`Rn::act`] is the translation action on R³ and is only defined for
//!   `Rn(3)`: dimensions other than 3 hit at best a `debug_assert` (debug
//!   builds) and at worst an out-of-bounds panic or a silently truncated
//!   translation (release builds). Do not call it on any other `n`.
//! - The default [`LieGroup::between`] composes `inverse` + `compose` and
//!   requires both operands to share one dimension (asserted).
//!
//! The implementation follows the [manif](https://github.com/artivis/manif) C++ library
//! conventions and provides all operations required by the LieGroup and Tangent traits.

use crate::{Interpolatable, LieGroup, Tangent};
use nalgebra::{DMatrix, DVector, Matrix3, Vector3};
use std::{
    fmt,
    fmt::{Display, Formatter},
};

/// Rⁿ group element representing n-dimensional Euclidean vectors.
///
/// Internally represented using nalgebra's `DVector<f64>` for dynamic sizing.
#[derive(Clone, PartialEq)]
pub struct Rn {
    /// Internal representation as a dynamic vector
    data: DVector<f64>,
}

impl Display for Rn {
    fn fmt(&self, f: &mut Formatter<'_>) -> fmt::Result {
        write!(f, "Rn(dim: {}, data: [", self.data.len())?;
        for (i, val) in self.data.iter().enumerate() {
            if i > 0 {
                write!(f, ", ")?;
            }
            write!(f, "{:.4}", val)?;
        }
        write!(f, "])")
    }
}

// Conversion traits for integration with generic Problem
impl From<DVector<f64>> for Rn {
    fn from(data: DVector<f64>) -> Self {
        Rn::new(data)
    }
}

impl From<Rn> for DVector<f64> {
    fn from(rn: Rn) -> Self {
        rn.data
    }
}

/// Rⁿ tangent space element representing elements in the Lie algebra rⁿ.
///
/// For Euclidean space, the tangent space is isomorphic to the manifold itself,
/// so this is also represented as a `DVector<f64>`.
#[derive(Clone, PartialEq)]
pub struct RnTangent {
    /// Internal data: n-dimensional vector
    data: DVector<f64>,
}
impl From<DVector<f64>> for RnTangent {
    fn from(data: DVector<f64>) -> Self {
        RnTangent::new(data)
    }
}

impl From<RnTangent> for DVector<f64> {
    fn from(rn: RnTangent) -> Self {
        rn.data
    }
}

impl Display for RnTangent {
    fn fmt(&self, f: &mut Formatter<'_>) -> fmt::Result {
        write!(f, "rn(dim: {}, data: [", self.data.len())?;
        for (i, val) in self.data.iter().enumerate() {
            if i > 0 {
                write!(f, ", ")?;
            }
            write!(f, "{:.4}", val)?;
        }
        write!(f, "])")
    }
}

impl Rn {
    /// Sentinel: Rⁿ has no compile-time ambient dimension. The runtime value
    /// is the data length; gate any branch with [`Rn::is_dynamic`].
    #[deprecated(
        note = "Rn is dynamic: this const is the 0 sentinel, not a dimension. Use is_dynamic() and tangent_dim()."
    )]
    pub const DIM: usize = 0;

    /// Sentinel: Rⁿ has no compile-time DOF. The runtime value comes from
    /// `tangent_dim()`.
    #[deprecated(
        note = "Rn is dynamic: this const is the 0 sentinel, not a DOF. Use tangent_dim()."
    )]
    pub const DOF: usize = 0;

    /// Sentinel: Rⁿ has no compile-time representation size. The runtime
    /// value is the data length.
    #[deprecated(
        note = "Rn is dynamic: this const is the 0 sentinel, not a size. Use as_param_slice().len()."
    )]
    pub const REP_SIZE: usize = 0;

    /// Get the identity element of the group.
    ///
    /// Returns the neutral element e such that e ∘ g = g ∘ e = g for any group element g.
    /// Note: Default to 3D identity for compatibility, but this should be created with specific dimension
    pub fn identity() -> Self {
        Rn::new(DVector::zeros(3))
    }

    /// Get the identity matrix for Jacobians.
    ///
    /// Returns the identity matrix in the appropriate dimension for Jacobian computations.
    /// Note: Default to 3x3 identity, but this should be created with specific dimension
    pub fn jacobian_identity() -> DMatrix<f64> {
        DMatrix::identity(3, 3)
    }

    /// Create a new Rⁿ element from a vector.
    ///
    /// # Arguments
    /// * `data` - Vector data
    #[inline]
    pub fn new(data: DVector<f64>) -> Self {
        Rn { data }
    }

    /// Create Rⁿ from a slice.
    ///
    /// # Arguments
    /// * `slice` - Data slice
    pub fn from_slice(slice: &[f64]) -> Self {
        Rn::new(DVector::from_row_slice(slice))
    }

    /// Create Rⁿ from individual components (up to 6D for convenience).
    pub fn from_vec(components: Vec<f64>) -> Self {
        Rn::new(DVector::from_vec(components))
    }

    /// Get the underlying vector.
    #[inline]
    pub fn data(&self) -> &DVector<f64> {
        &self.data
    }

    /// Get the dimension of the space.
    #[inline]
    pub fn dim(&self) -> usize {
        self.data.len()
    }

    /// Get a specific component.
    #[inline]
    pub fn component(&self, index: usize) -> f64 {
        self.data[index]
    }

    /// Set a specific component.
    pub fn set_component(&mut self, index: usize, value: f64) {
        self.data[index] = value;
    }

    /// Get the norm (Euclidean length) of the vector.
    #[inline]
    pub fn norm(&self) -> f64 {
        self.data.norm()
    }

    /// Get the squared norm of the vector.
    #[inline]
    pub fn norm_squared(&self) -> f64 {
        self.data.norm_squared()
    }

    /// Convert Rn to a DVector
    #[inline]
    pub fn to_vector(&self) -> DVector<f64> {
        self.data.clone()
    }
}

impl LieGroup for Rn {
    const NAME: &'static str = "Rn";

    type TangentVector = RnTangent;
    type JacobianMatrix = DMatrix<f64>;
    type LieAlgebra = DMatrix<f64>;

    /// Rⁿ inverse (negation for additive group).
    ///
    /// # Arguments
    /// * `jacobian` - Optional Jacobian matrix of the inverse wrt self.
    ///
    /// # Notes
    /// For Euclidean space with addition: -v
    /// Jacobian of inverse: d(-v)/dv = -I
    fn inverse(&self, jacobian: Option<&mut Self::JacobianMatrix>) -> Self {
        let dim = self.data.len();
        if let Some(jac) = jacobian {
            *jac = -DMatrix::identity(dim, dim);
        }
        Rn::new(-&self.data)
    }

    /// Rⁿ composition (vector addition).
    ///
    /// # Arguments
    /// * `other` - Another Rⁿ element.
    /// * `jacobian_self` - Optional Jacobian matrix of the composition wrt self.
    /// * `jacobian_other` - Optional Jacobian matrix of the composition wrt other.
    ///
    /// # Notes
    /// For Euclidean space: v₁ + v₂
    /// Jacobians: d(v₁ + v₂)/dv₁ = I, d(v₁ + v₂)/dv₂ = I
    fn compose(
        &self,
        other: &Self,
        jacobian_self: Option<&mut Self::JacobianMatrix>,
        jacobian_other: Option<&mut Self::JacobianMatrix>,
    ) -> Self {
        assert_eq!(
            self.data.len(),
            other.data.len(),
            "Rn elements must have the same dimension for composition"
        );

        let dim = self.data.len();
        if let Some(jac_self) = jacobian_self {
            *jac_self = DMatrix::identity(dim, dim);
        }
        if let Some(jac_other) = jacobian_other {
            *jac_other = DMatrix::identity(dim, dim);
        }

        Rn::new(&self.data + &other.data)
    }

    /// Logarithmic map from manifold to tangent space.
    ///
    /// # Arguments
    /// * `jacobian` - Optional Jacobian matrix of the tangent wrt to self.
    ///
    /// # Notes
    /// For Euclidean space, log is identity: log(v) = v
    /// Jacobian: dlog(v)/dv = I
    fn log(&self, jacobian: Option<&mut Self::JacobianMatrix>) -> Self::TangentVector {
        let dim = self.data.len();
        if let Some(jac) = jacobian {
            *jac = DMatrix::identity(dim, dim);
        }
        RnTangent::new(self.data.clone())
    }

    /// Vee operator: log(g)^∨.
    ///
    /// For Euclidean space, this is the same as log().
    fn vee(&self) -> Self::TangentVector {
        self.log(None)
    }

    /// Action on a 3-vector (for compatibility with the trait).
    ///
    /// # Arguments
    /// * `vector` - A 3-vector.
    /// * `jacobian_self` - Optional Jacobian of the new object wrt this.
    /// * `jacobian_vector` - Optional Jacobian of the new object wrt input object.
    ///
    /// # Returns
    /// The transformed 3-vector.
    ///
    /// # Notes
    /// For Euclidean space, the action is translation: v + x
    /// This only works if this Rⁿ element is 3-dimensional.
    fn act(
        &self,
        vector: &Vector3<f64>,
        jacobian_self: Option<&mut Self::JacobianMatrix>,
        jacobian_vector: Option<&mut Matrix3<f64>>,
    ) -> Vector3<f64> {
        debug_assert_eq!(
            self.data.len(),
            3,
            "Rn::act is the translation action on R³ and is only defined for Rn(3)"
        );
        if let Some(jac_self) = jacobian_self {
            *jac_self = DMatrix::identity(3, 3);
        }
        if let Some(jac_vector) = jacobian_vector {
            *jac_vector = Matrix3::identity();
        }

        Vector3::new(
            self.data[0] + vector.x,
            self.data[1] + vector.y,
            self.data[2] + vector.z,
        )
    }

    /// Get the adjoint matrix of Rⁿ.
    ///
    /// # Notes
    /// For Euclidean space (abelian group), adjoint is identity.
    fn adjoint(&self) -> Self::JacobianMatrix {
        let dim = self.data.len();
        DMatrix::identity(dim, dim)
    }

    /// Generate a random element.
    fn random() -> Self {
        // Default to 3D random vector
        let data = DVector::from_fn(3, |_, _| rand::random::<f64>() * 10.0 - 5.0);
        Rn::new(data)
    }

    fn jacobian_identity() -> Self::JacobianMatrix {
        // Default to 3D identity for compatibility
        DMatrix::identity(3, 3)
    }

    fn zero_jacobian() -> Self::JacobianMatrix {
        // Default to 3D zero matrix for compatibility
        DMatrix::zeros(3, 3)
    }

    /// Identity sized from this element's runtime dimension.
    ///
    /// `Rn` is dynamically sized, so the associated
    /// [`jacobian_identity`](LieGroup::jacobian_identity) cannot produce a
    /// conformant block — it returns 3×3 for every dimension. Chain-rule
    /// products built from it (e.g. `BetweenFactor<Rn>` via the default
    /// [`LieGroup::between`]) then panic in nalgebra for any `dim != 3`.
    fn jacobian_identity_for(&self) -> Self::JacobianMatrix {
        Rn::jacobian_identity_with_dim(self.dim())
    }

    fn normalize(&mut self) {
        let norm = self.data.norm();
        if norm > 1e-12 {
            self.data /= norm;
        }
    }

    fn is_valid(&self, _tolerance: f64) -> bool {
        self.data.iter().all(|x| x.is_finite())
    }

    fn as_param_slice(&self) -> &[f64] {
        self.data.as_slice()
    }

    fn as_param_slice_mut(&mut self) -> &mut [f64] {
        self.data.as_mut_slice()
    }

    fn from_param_slice(s: &[f64]) -> Self {
        Rn::from_slice(s)
    }

    /// Check if the element is approximately equal to another element.
    ///
    /// # Arguments
    /// * `other` - The other element to compare with
    /// * `tolerance` - The tolerance for the comparison
    fn is_approx(&self, other: &Self, tolerance: f64) -> bool {
        if self.data.len() != other.data.len() {
            return false;
        }
        let difference = self.right_minus(other, None, None);
        difference.is_zero(tolerance)
    }

    // Explicit implementations of plus/minus operations for optimal performance
    // These override the default LieGroup implementations to provide correct
    // Jacobians for Euclidean space (simple identity matrices)

    /// Right plus operation: v ⊞ δ = v + δ.
    ///
    /// For Euclidean space, this is simple vector addition.
    ///
    /// # Arguments
    /// * `tangent` - Tangent vector perturbation
    /// * `jacobian_self` - Optional Jacobian ∂(v ⊞ δ)/∂v = I
    /// * `jacobian_tangent` - Optional Jacobian ∂(v ⊞ δ)/∂δ = I
    ///
    /// # Notes
    /// For Euclidean space: v ⊞ δ = v + δ
    /// Jacobians: ∂(v + δ)/∂v = I, ∂(v + δ)/∂δ = I
    fn right_plus(
        &self,
        tangent: &Self::TangentVector,
        jacobian_self: Option<&mut Self::JacobianMatrix>,
        jacobian_tangent: Option<&mut Self::JacobianMatrix>,
    ) -> Self {
        assert_eq!(
            self.data.len(),
            tangent.data.len(),
            "Rn element and tangent must have the same dimension"
        );

        let dim = self.data.len();

        if let Some(jac_self) = jacobian_self {
            *jac_self = DMatrix::identity(dim, dim);
        }

        if let Some(jac_tangent) = jacobian_tangent {
            *jac_tangent = DMatrix::identity(dim, dim);
        }

        Rn::new(&self.data + &tangent.data)
    }

    /// Right minus operation: v₁ ⊟ v₂ = v₁ - v₂.
    ///
    /// For Euclidean space, this is simple vector subtraction.
    ///
    /// # Arguments
    /// * `other` - The reference element v₂
    /// * `jacobian_self` - Optional Jacobian ∂(v₁ ⊟ v₂)/∂v₁ = I
    /// * `jacobian_other` - Optional Jacobian ∂(v₁ ⊟ v₂)/∂v₂ = -I
    ///
    /// # Notes
    /// For Euclidean space: v₁ ⊟ v₂ = v₁ - v₂
    /// Jacobians: ∂(v₁ - v₂)/∂v₁ = I, ∂(v₁ - v₂)/∂v₂ = -I
    fn right_minus(
        &self,
        other: &Self,
        jacobian_self: Option<&mut Self::JacobianMatrix>,
        jacobian_other: Option<&mut Self::JacobianMatrix>,
    ) -> Self::TangentVector {
        assert_eq!(
            self.data.len(),
            other.data.len(),
            "Rn elements must have the same dimension"
        );

        let dim = self.data.len();

        if let Some(jac_self) = jacobian_self {
            *jac_self = DMatrix::identity(dim, dim);
        }

        if let Some(jac_other) = jacobian_other {
            *jac_other = -DMatrix::identity(dim, dim);
        }

        RnTangent::new(&self.data - &other.data)
    }

    /// Left plus operation: δ ⊞ v = δ + v.
    ///
    /// For Euclidean space (abelian group), left plus is the same as right plus.
    ///
    /// # Arguments
    /// * `tangent` - Tangent vector perturbation
    /// * `jacobian_tangent` - Optional Jacobian ∂(δ ⊞ v)/∂δ = I
    /// * `jacobian_self` - Optional Jacobian ∂(δ ⊞ v)/∂v = I
    ///
    /// # Notes
    /// For abelian groups: δ ⊞ v = v ⊞ δ = δ + v
    fn left_plus(
        &self,
        tangent: &Self::TangentVector,
        jacobian_tangent: Option<&mut Self::JacobianMatrix>,
        jacobian_self: Option<&mut Self::JacobianMatrix>,
    ) -> Self {
        assert_eq!(
            self.data.len(),
            tangent.data.len(),
            "Rn element and tangent must have the same dimension"
        );

        let dim = self.data.len();

        if let Some(jac_tangent) = jacobian_tangent {
            *jac_tangent = DMatrix::identity(dim, dim);
        }

        if let Some(jac_self) = jacobian_self {
            *jac_self = DMatrix::identity(dim, dim);
        }

        // For abelian groups, left plus is the same as right plus
        Rn::new(&tangent.data + &self.data)
    }

    /// Left minus operation: v₁ ⊟ v₂ = v₁ - v₂.
    ///
    /// For Euclidean space (abelian group), left minus is the same as right minus.
    ///
    /// # Arguments
    /// * `other` - The reference element v₂
    /// * `jacobian_self` - Optional Jacobian ∂(v₁ ⊟ v₂)/∂v₁ = I
    /// * `jacobian_other` - Optional Jacobian ∂(v₁ ⊟ v₂)/∂v₂ = -I
    ///
    /// # Notes
    /// For abelian groups: left minus = right minus
    fn left_minus(
        &self,
        other: &Self,
        jacobian_self: Option<&mut Self::JacobianMatrix>,
        jacobian_other: Option<&mut Self::JacobianMatrix>,
    ) -> Self::TangentVector {
        // For abelian groups, left minus is the same as right minus
        self.right_minus(other, jacobian_self, jacobian_other)
    }

    /// Get the dimension of the tangent space for this Rⁿ element.
    ///
    /// # Returns
    /// The actual runtime dimension of this Rⁿ element.
    ///
    /// # Notes
    /// Overrides the default implementation to return the dynamic size
    /// based on the actual data vector length, since Rⁿ has variable dimension.
    fn tangent_dim(&self) -> usize {
        self.data.len()
    }
}

impl RnTangent {
    /// Create a new RnTangent from a vector.
    ///
    /// # Arguments
    /// * `data` - Vector data
    #[inline]
    pub fn new(data: DVector<f64>) -> Self {
        RnTangent { data }
    }

    /// Create RnTangent from a slice.
    ///
    /// # Arguments
    /// * `slice` - Data slice
    pub fn from_slice(slice: &[f64]) -> Self {
        RnTangent::new(DVector::from_row_slice(slice))
    }

    /// Create RnTangent from individual components.
    pub fn from_vec(components: Vec<f64>) -> Self {
        RnTangent::new(DVector::from_vec(components))
    }

    /// Get the underlying vector.
    #[inline]
    pub fn data(&self) -> &DVector<f64> {
        &self.data
    }

    /// Get the dimension of the tangent space.
    #[inline]
    pub fn dim(&self) -> usize {
        self.data.len()
    }

    /// Get a specific component.
    #[inline]
    pub fn component(&self, index: usize) -> f64 {
        self.data[index]
    }

    /// Set a specific component.
    pub fn set_component(&mut self, index: usize, value: f64) {
        self.data[index] = value;
    }

    /// Get the norm (Euclidean length) of the tangent vector.
    #[inline]
    pub fn norm(&self) -> f64 {
        self.data.norm()
    }

    /// Get the squared norm of the tangent vector.
    #[inline]
    pub fn norm_squared(&self) -> f64 {
        self.data.norm_squared()
    }

    /// Convert RnTangent to a DVector
    #[inline]
    pub fn to_vector(&self) -> DVector<f64> {
        self.data.clone()
    }

    /// Create RnTangent from a DVector
    pub fn from_vector(data: DVector<f64>) -> Self {
        RnTangent::new(data)
    }
}

impl Tangent<Rn> for RnTangent {
    /// Dimension of the tangent space
    /// Note: For Rⁿ this is dynamic and determined at runtime
    const DIM: usize = 0;

    /// Exponential map for Euclidean space (identity).
    ///
    /// # Arguments
    /// * `jacobian` - Optional Jacobian matrix of the Rn element wrt this.
    ///
    /// # Notes
    /// For Euclidean space: exp(v) = v
    /// Jacobian: dexp(v)/dv = I
    fn exp(&self, jacobian: Option<&mut <Rn as LieGroup>::JacobianMatrix>) -> Rn {
        let dim = self.data.len();
        if let Some(jac) = jacobian {
            *jac = DMatrix::identity(dim, dim);
        }
        Rn::new(self.data.clone())
    }

    fn right_jacobian(&self) -> <Rn as LieGroup>::JacobianMatrix {
        let dim = self.data.len();
        DMatrix::identity(dim, dim)
    }

    fn left_jacobian(&self) -> <Rn as LieGroup>::JacobianMatrix {
        let dim = self.data.len();
        DMatrix::identity(dim, dim)
    }

    fn right_jacobian_inv(&self) -> <Rn as LieGroup>::JacobianMatrix {
        let dim = self.data.len();
        DMatrix::identity(dim, dim)
    }

    fn left_jacobian_inv(&self) -> <Rn as LieGroup>::JacobianMatrix {
        let dim = self.data.len();
        DMatrix::identity(dim, dim)
    }

    /// Hat operator: v^∧ (vector to matrix).
    ///
    /// For Euclidean space, this could be interpreted as a diagonal matrix
    /// or simply return the vector as a column matrix.
    fn hat(&self) -> <Rn as LieGroup>::LieAlgebra {
        DMatrix::from_diagonal(&self.data)
    }

    /// Small adjoint (zero for abelian group).
    fn small_adj(&self) -> <Rn as LieGroup>::JacobianMatrix {
        let dim = self.data.len();
        DMatrix::zeros(dim, dim)
    }

    /// Lie bracket for Euclidean space.
    ///
    /// For abelian groups: [v, w] = 0
    fn lie_bracket(&self, _other: &Self) -> <Rn as LieGroup>::TangentVector {
        RnTangent::new(DVector::zeros(self.data.len()))
    }

    /// Check if the tangent vector is approximately equal to another tangent vector.
    ///
    /// # Arguments
    /// * `other` - The other tangent vector to compare with
    /// * `tolerance` - The tolerance for the comparison
    fn is_approx(&self, other: &Self, tolerance: f64) -> bool {
        if self.data.len() != other.data.len() {
            return false;
        }
        (&self.data - &other.data).norm() < tolerance
    }

    /// Get the i-th generator of the Lie algebra.
    ///
    /// For Euclidean space, generators are standard basis vectors.
    fn generator(&self, i: usize) -> <Rn as LieGroup>::LieAlgebra {
        let dim = self.data.len();
        let mut generator_matrix = DMatrix::zeros(dim, dim);
        generator_matrix[(i, i)] = 1.0;
        generator_matrix
    }

    /// Zero tangent vector.
    fn zero() -> <Rn as LieGroup>::TangentVector {
        // Default to 3D zero vector for compatibility
        RnTangent::new(DVector::zeros(3))
    }

    /// Random tangent vector.
    fn random() -> <Rn as LieGroup>::TangentVector {
        // Default to 3D random vector
        let data = DVector::from_fn(3, |_, _| rand::random::<f64>() * 10.0 - 5.0);
        RnTangent::new(data)
    }

    /// Check if the tangent vector is approximately zero.
    fn is_zero(&self, tolerance: f64) -> bool {
        self.data.norm() < tolerance
    }

    /// Normalize the tangent vector to unit norm.
    fn normalize(&mut self) {
        let norm = self.data.norm();
        if norm > 1e-12 {
            self.data /= norm;
        }
    }

    /// Return a unit tangent vector in the same direction.
    fn normalized(&self) -> <Rn as LieGroup>::TangentVector {
        let mut result = self.clone();
        result.normalize();
        result
    }

    fn as_slice(&self) -> &[f64] {
        self.data.as_slice()
    }

    fn from_slice(s: &[f64]) -> Self {
        RnTangent::from_slice(s)
    }
}

// Implement Interpolatable trait for Rn
impl Interpolatable for Rn {
    /// Linear interpolation in Euclidean space.
    ///
    /// For parameter t ∈ `[0,1]`: interp(v₁, v₂, 0) = v₁, interp(v₁, v₂, 1) = v₂.
    ///
    /// # Arguments
    /// * `other` - Target element for interpolation
    /// * `t` - Interpolation parameter in `[0,1]`
    fn interp(&self, other: &Self, t: f64) -> Self {
        assert_eq!(
            self.data.len(),
            other.data.len(),
            "Rn elements must have the same dimension for interpolation"
        );

        let interpolated = &self.data * (1.0 - t) + &other.data * t;
        Rn::new(interpolated)
    }

    /// Spherical linear interpolation (same as linear for Euclidean space).
    fn slerp(&self, other: &Self, t: f64) -> Self {
        self.interp(other, t)
    }
}

// Additional convenience implementations
impl Rn {
    /// Create identity element with specific dimension.
    ///
    /// For Euclidean space, the identity (additive neutral element) is the zero vector.
    /// The default `identity()` returns a 3D zero vector for compatibility.
    /// Use this method when you need a specific dimension.
    pub fn identity_with_dim(dim: usize) -> Self {
        Rn::new(DVector::zeros(dim))
    }

    /// Create Rn with specific dimension filled with zeros.
    pub fn zeros(dim: usize) -> Self {
        Rn::new(DVector::zeros(dim))
    }

    /// Create Rn with specific dimension filled with ones.
    pub fn ones(dim: usize) -> Self {
        Rn::new(DVector::from_element(dim, 1.0))
    }

    /// Create Rn with specific dimension and random values.
    pub fn random_with_dim(dim: usize) -> Self {
        let data = DVector::from_fn(dim, |_, _| rand::random::<f64>() * 10.0 - 5.0);
        Rn::new(data)
    }

    /// Create identity matrix for Jacobians with specific dimension.
    pub fn jacobian_identity_with_dim(dim: usize) -> DMatrix<f64> {
        DMatrix::identity(dim, dim)
    }
}

impl RnTangent {
    /// Create zero tangent vector with specific dimension.
    pub fn zero_with_dim(dim: usize) -> Self {
        RnTangent::new(DVector::zeros(dim))
    }

    /// Create RnTangent with specific dimension filled with zeros.
    pub fn zeros(dim: usize) -> Self {
        RnTangent::new(DVector::zeros(dim))
    }

    /// Create RnTangent with specific dimension filled with ones.
    pub fn ones(dim: usize) -> Self {
        RnTangent::new(DVector::from_element(dim, 1.0))
    }

    /// Create RnTangent with specific dimension and random values.
    pub fn random_with_dim(dim: usize) -> Self {
        let data = DVector::from_fn(dim, |_, _| rand::random::<f64>() * 2.0 - 1.0);
        RnTangent::new(data)
    }
}

#[cfg(test)]
mod tests {
    use super::*;
    use crate::{Interpolatable, LieGroup, Tangent};

    #[test]
    fn test_rn_basic_operations() {
        // Test 3D Euclidean space
        let v1 = Rn::from_vec(vec![1.0, 2.0, 3.0]);
        let v2 = Rn::from_vec(vec![4.0, 5.0, 6.0]);

        // Test composition (addition)
        let sum = v1.compose(&v2, None, None);
        assert_eq!(sum.component(0), 5.0);
        assert_eq!(sum.component(1), 7.0);
        assert_eq!(sum.component(2), 9.0);

        // Test identity
        let identity = Rn::zeros(3);
        let result = v1.compose(&identity, None, None);
        assert_eq!(result.component(0), v1.component(0));
        assert_eq!(result.component(1), v1.component(1));
        assert_eq!(result.component(2), v1.component(2));

        // Test inverse
        let v1_inv = v1.inverse(None);
        assert_eq!(v1_inv.component(0), -1.0);
        assert_eq!(v1_inv.component(1), -2.0);
        assert_eq!(v1_inv.component(2), -3.0);

        // Test log/exp (should be identity for Euclidean space)
        let tangent = v1.log(None);
        let recovered = tangent.exp(None);
        assert!((recovered.component(0) - v1.component(0)).abs() < 1e-10);
        assert!((recovered.component(1) - v1.component(1)).abs() < 1e-10);
        assert!((recovered.component(2) - v1.component(2)).abs() < 1e-10);
    }

    #[test]
    fn test_rn_tangent_operations() {
        let t1 = RnTangent::from_vec(vec![1.0, 2.0, 3.0]);
        let t2 = RnTangent::from_vec(vec![4.0, 5.0, 6.0]);

        // Test Lie bracket (should be zero for abelian group)
        let bracket = t1.lie_bracket(&t2);
        assert!(bracket.is_zero(1e-10));

        // Test zero tangent
        let zero = RnTangent::zeros(3);
        assert!(zero.is_zero(1e-10));

        // Test normalization
        let mut t = RnTangent::from_vec(vec![3.0, 4.0, 0.0]);
        t.normalize();
        assert!((t.norm() - 1.0).abs() < 1e-10);
    }

    #[test]
    fn test_rn_interpolation() {
        let v1 = Rn::from_vec(vec![0.0, 0.0, 0.0]);
        let v2 = Rn::from_vec(vec![10.0, 20.0, 30.0]);

        // Test interpolation at t=0.5
        let mid = v1.interp(&v2, 0.5);
        assert_eq!(mid.component(0), 5.0);
        assert_eq!(mid.component(1), 10.0);
        assert_eq!(mid.component(2), 15.0);

        // Test interpolation at endpoints
        let start = v1.interp(&v2, 0.0);
        let end = v1.interp(&v2, 1.0);
        assert!(v1.is_approx(&start, 1e-10));
        assert!(v2.is_approx(&end, 1e-10));
    }

    #[test]
    fn test_rn_different_dimensions() {
        // Test 2D
        let v2d = Rn::from_vec(vec![1.0, 2.0]);
        assert_eq!(v2d.dim(), 2);

        // Test 1D
        let v1d = Rn::from_vec(vec![5.0]);
        assert_eq!(v1d.dim(), 1);

        // Test higher dimensions
        let v5d = Rn::from_vec(vec![1.0, 2.0, 3.0, 4.0, 5.0]);
        assert_eq!(v5d.dim(), 5);
        assert_eq!(v5d.component(4), 5.0);
    }

    #[test]
    fn test_rn_action() {
        // Test action on 3D vector (translation)
        let translation = Rn::from_vec(vec![1.0, 2.0, 3.0]);
        let point = Vector3::new(4.0, 5.0, 6.0);
        let transformed = translation.act(&point, None, None);

        assert_eq!(transformed.x, 5.0);
        assert_eq!(transformed.y, 7.0);
        assert_eq!(transformed.z, 9.0);
    }

    #[test]
    fn test_rn_right_plus_operations() {
        let v = Rn::from_vec(vec![1.0, 2.0, 3.0]);
        let delta = RnTangent::from_vec(vec![0.1, 0.2, 0.3]);

        // Test right plus without Jacobians
        let result = v.right_plus(&delta, None, None);
        assert!((result.component(0) - 1.1).abs() < 1e-10);
        assert!((result.component(1) - 2.2).abs() < 1e-10);
        assert!((result.component(2) - 3.3).abs() < 1e-10);

        // Test right plus with Jacobians
        let mut jac_self = DMatrix::zeros(3, 3);
        let mut jac_tangent = DMatrix::zeros(3, 3);
        let result_jac = v.right_plus(&delta, Some(&mut jac_self), Some(&mut jac_tangent));

        // Verify result is the same
        assert!((result_jac.component(0) - result.component(0)).abs() < 1e-10);
        assert!((result_jac.component(1) - result.component(1)).abs() < 1e-10);
        assert!((result_jac.component(2) - result.component(2)).abs() < 1e-10);

        // Verify Jacobians are identity matrices
        let identity = DMatrix::identity(3, 3);
        assert!((jac_self - &identity).norm() < 1e-10);
        assert!((jac_tangent - &identity).norm() < 1e-10);
    }

    #[test]
    fn test_rn_right_minus_operations() {
        let v1 = Rn::from_vec(vec![5.0, 7.0, 9.0]);
        let v2 = Rn::from_vec(vec![1.0, 2.0, 3.0]);

        // Test right minus without Jacobians
        let result = v1.right_minus(&v2, None, None);
        assert!((result.component(0) - 4.0).abs() < 1e-10);
        assert!((result.component(1) - 5.0).abs() < 1e-10);
        assert!((result.component(2) - 6.0).abs() < 1e-10);

        // Test right minus with Jacobians
        let mut jac_self = DMatrix::zeros(3, 3);
        let mut jac_other = DMatrix::zeros(3, 3);
        let result_jac = v1.right_minus(&v2, Some(&mut jac_self), Some(&mut jac_other));

        // Verify result is the same
        assert!((result_jac.component(0) - result.component(0)).abs() < 1e-10);
        assert!((result_jac.component(1) - result.component(1)).abs() < 1e-10);
        assert!((result_jac.component(2) - result.component(2)).abs() < 1e-10);

        // Verify Jacobians
        let identity = DMatrix::identity(3, 3);
        let neg_identity = -&identity;
        assert!((jac_self - &identity).norm() < 1e-10);
        assert!((jac_other - &neg_identity).norm() < 1e-10);
    }

    #[test]
    fn test_rn_left_plus_operations() {
        let v = Rn::from_vec(vec![1.0, 2.0, 3.0]);
        let delta = RnTangent::from_vec(vec![0.1, 0.2, 0.3]);

        // Test left plus without Jacobians
        let result = v.left_plus(&delta, None, None);
        assert!((result.component(0) - 1.1).abs() < 1e-10);
        assert!((result.component(1) - 2.2).abs() < 1e-10);
        assert!((result.component(2) - 3.3).abs() < 1e-10);

        // Test left plus with Jacobians
        let mut jac_tangent = DMatrix::zeros(3, 3);
        let mut jac_self = DMatrix::zeros(3, 3);
        let result_jac = v.left_plus(&delta, Some(&mut jac_tangent), Some(&mut jac_self));

        // Verify result is the same
        assert!((result_jac.component(0) - result.component(0)).abs() < 1e-10);
        assert!((result_jac.component(1) - result.component(1)).abs() < 1e-10);
        assert!((result_jac.component(2) - result.component(2)).abs() < 1e-10);

        // Verify Jacobians are identity matrices
        let identity = DMatrix::identity(3, 3);
        assert!((jac_tangent - &identity).norm() < 1e-10);
        assert!((jac_self - &identity).norm() < 1e-10);
    }

    #[test]
    fn test_rn_left_minus_operations() {
        let v1 = Rn::from_vec(vec![5.0, 7.0, 9.0]);
        let v2 = Rn::from_vec(vec![1.0, 2.0, 3.0]);

        // Test left minus without Jacobians
        let result = v1.left_minus(&v2, None, None);
        assert!((result.component(0) - 4.0).abs() < 1e-10);
        assert!((result.component(1) - 5.0).abs() < 1e-10);
        assert!((result.component(2) - 6.0).abs() < 1e-10);

        // Test left minus with Jacobians
        let mut jac_self = DMatrix::zeros(3, 3);
        let mut jac_other = DMatrix::zeros(3, 3);
        let result_jac = v1.left_minus(&v2, Some(&mut jac_self), Some(&mut jac_other));

        // Verify result is the same
        assert!((result_jac.component(0) - result.component(0)).abs() < 1e-10);
        assert!((result_jac.component(1) - result.component(1)).abs() < 1e-10);
        assert!((result_jac.component(2) - result.component(2)).abs() < 1e-10);

        // Verify Jacobians
        let identity = DMatrix::identity(3, 3);
        let neg_identity = -&identity;
        assert!((jac_self - &identity).norm() < 1e-10);
        assert!((jac_other - &neg_identity).norm() < 1e-10);
    }

    #[test]
    fn test_rn_left_right_equivalence() {
        // For abelian groups, left and right operations should be equivalent
        let v1 = Rn::from_vec(vec![1.0, 2.0, 3.0]);
        let v2 = Rn::from_vec(vec![4.0, 5.0, 6.0]);
        let delta = RnTangent::from_vec(vec![0.1, 0.2, 0.3]);

        // Test plus operations equivalence
        let right_plus = v1.right_plus(&delta, None, None);
        let left_plus = v1.left_plus(&delta, None, None);
        assert!(right_plus.is_approx(&left_plus, 1e-10));

        // Test minus operations equivalence
        let right_minus = v1.right_minus(&v2, None, None);
        let left_minus = v1.left_minus(&v2, None, None);
        assert!(right_minus.is_approx(&left_minus, 1e-10));
    }

    #[test]
    fn test_rn_plus_minus_different_dimensions() {
        // Test 2D operations
        let v2d = Rn::from_vec(vec![1.0, 2.0]);
        let delta2d = RnTangent::from_vec(vec![0.5, 1.0]);
        let result2d = v2d.right_plus(&delta2d, None, None);
        assert_eq!(result2d.dim(), 2);
        assert!((result2d.component(0) - 1.5).abs() < 1e-10);
        assert!((result2d.component(1) - 3.0).abs() < 1e-10);

        // Test 5D operations
        let v5d = Rn::from_vec(vec![1.0, 2.0, 3.0, 4.0, 5.0]);
        let delta5d = RnTangent::from_vec(vec![0.1, 0.2, 0.3, 0.4, 0.5]);
        let result5d = v5d.right_plus(&delta5d, None, None);
        assert_eq!(result5d.dim(), 5);
        for i in 0..5 {
            assert!(
                (result5d.component(i) - (i as f64 + 1.0 + (i as f64 + 1.0) * 0.1)).abs() < 1e-10
            );
        }
    }

    #[test]
    fn test_rn_plus_minus_edge_cases() {
        let v = Rn::from_vec(vec![1.0, 2.0, 3.0]);

        // Test with zero tangent vector
        let zero_tangent = RnTangent::zeros(3);
        let result_zero = v.right_plus(&zero_tangent, None, None);
        assert!(v.is_approx(&result_zero, 1e-10));

        // Test minus with itself (should give zero)
        let self_minus = v.right_minus(&v, None, None);
        assert!(self_minus.is_zero(1e-10));

        // Test plus then minus (should recover original)
        let delta = RnTangent::from_vec(vec![0.5, 1.0, 1.5]);
        let plus_result = v.right_plus(&delta, None, None);
        let recovered_delta = plus_result.right_minus(&v, None, None);
        assert!(delta.is_approx(&recovered_delta, 1e-10));
    }

    #[test]
    fn test_rn_jacobian_dimensions() {
        // Test that Jacobians have correct dimensions for different vector sizes
        let v1d = Rn::from_vec(vec![1.0]);
        let delta1d = RnTangent::from_vec(vec![0.1]);
        let mut jac1d = DMatrix::zeros(1, 1);
        v1d.right_plus(&delta1d, Some(&mut jac1d), None);
        assert_eq!(jac1d.nrows(), 1);
        assert_eq!(jac1d.ncols(), 1);

        let v4d = Rn::from_vec(vec![1.0, 2.0, 3.0, 4.0]);
        let delta4d = RnTangent::from_vec(vec![0.1, 0.2, 0.3, 0.4]);
        let mut jac4d = DMatrix::zeros(4, 4);
        v4d.right_plus(&delta4d, Some(&mut jac4d), None);
        assert_eq!(jac4d.nrows(), 4);
        assert_eq!(jac4d.ncols(), 4);

        // Verify they are identity matrices
        assert!((jac1d - DMatrix::identity(1, 1)).norm() < 1e-10);
        assert!((jac4d - DMatrix::identity(4, 4)).norm() < 1e-10);
    }

    #[test]
    fn test_rn_addition_chain() {
        // Add same vector 1000 times
        let increment = Rn::new(DVector::from_vec(vec![0.001, -0.002, 0.003]));
        let mut accumulated = Rn::new(DVector::zeros(3));

        for _ in 0..1000 {
            accumulated = accumulated.compose(&increment, None, None);
        }

        let expected = Rn::new(DVector::from_vec(vec![1.0, -2.0, 3.0]));
        assert!((accumulated.to_vector() - expected.to_vector()).norm() < 1e-10);
    }

    #[test]
    #[should_panic(expected = "Rn elements must have the same dimension")]
    fn test_rn_dimension_mismatch_compose() {
        let rn_2d = Rn::new(DVector::from_vec(vec![1.0, 2.0]));
        let rn_3d = Rn::new(DVector::from_vec(vec![3.0, 4.0, 5.0]));

        let _ = rn_2d.compose(&rn_3d, None, None); // Should panic
    }

    #[test]
    fn test_rn_zero_dimension() {
        let rn_0d = Rn::new(DVector::zeros(0));
        assert_eq!(rn_0d.dim(), 0);

        let identity = Rn::new(DVector::zeros(0));
        let composed = rn_0d.compose(&identity, None, None);
        assert_eq!(composed.dim(), 0);
    }

    #[test]
    fn test_rn_identity_with_dim() {
        for dim in [1, 2, 3, 5, 10] {
            let id = Rn::identity_with_dim(dim);
            assert_eq!(id.dim(), dim);
            assert!(id.norm() < 1e-15);

            // Composing with identity should be no-op
            let v = Rn::random_with_dim(dim);
            let result = v.compose(&id, None, None);
            assert!(v.is_approx(&result, 1e-10));
        }
    }

    #[test]
    fn test_rn_tangent_zero_with_dim() {
        for dim in [1, 2, 3, 5, 10] {
            let z = RnTangent::zero_with_dim(dim);
            assert_eq!(z.dim(), dim);
            assert!(z.is_zero(1e-15));
        }
    }

    #[test]
    fn test_rn_tangent_is_dynamic() {
        assert!(RnTangent::is_dynamic());
    }

    #[test]
    fn test_rn_display_format() {
        let rn = Rn::new(DVector::from_vec(vec![1.0, 2.0, 3.0]));
        let s = format!("{rn}");
        assert!(
            s.contains("1") && s.contains("2") && s.contains("3"),
            "got: {s}"
        );

        let t = RnTangent::new(DVector::from_vec(vec![4.0, 5.0]));
        let st = format!("{t}");
        assert!(st.contains("4") && st.contains("5"), "got: {st}");
    }

    #[test]
    fn test_rn_from_dvector_and_back() {
        let v = DVector::from_vec(vec![1.0, 2.0, 3.0]);
        let rn: Rn = Rn::from(v.clone());
        let v2: DVector<f64> = DVector::from(rn);
        assert_eq!(v, v2);
    }

    #[test]
    fn test_rn_tangent_from_vec_and_back() {
        let v = vec![4.0f64, 5.0];
        let t = RnTangent::from_vec(v.clone());
        let v2 = t.to_vector();
        assert!((v2[0] - v[0]).abs() < 1e-10);
        assert!((v2[1] - v[1]).abs() < 1e-10);
    }

    #[test]
    fn test_rn_from_slice_from_vec() {
        let rn = Rn::from_slice(&[1.0, 2.0, 3.0]);
        assert_eq!(rn.dim(), 3);
        assert!((rn.component(0) - 1.0).abs() < 1e-10);

        let rn2 = Rn::from_vec(vec![4.0, 5.0]);
        assert_eq!(rn2.dim(), 2);

        let t = RnTangent::from_slice(&[1.0, 2.0]);
        assert_eq!(t.dim(), 2);
        let t2 = RnTangent::from_vec(vec![3.0, 4.0]);
        assert_eq!(t2.dim(), 2);
    }

    #[test]
    fn test_rn_accessors() {
        let mut rn = Rn::from_slice(&[1.0, 2.0, 3.0]);
        assert_eq!(rn.data().len(), 3);
        assert_eq!(rn.dim(), 3);
        assert!((rn.component(1) - 2.0).abs() < 1e-10);
        rn.set_component(1, 99.0);
        assert!((rn.component(1) - 99.0).abs() < 1e-10);
        let rn2 = Rn::from_slice(&[3.0, 4.0]);
        assert!((rn2.norm() - 5.0).abs() < 1e-10);
        assert!((rn2.norm_squared() - 25.0).abs() < 1e-10);
        let v = rn2.to_vector();
        assert_eq!(v.len(), 2);
    }

    #[test]
    fn test_rn_tangent_accessors() {
        let mut t = RnTangent::from_slice(&[3.0, 4.0]);
        assert_eq!(t.data().len(), 2);
        assert_eq!(t.dim(), 2);
        assert!((t.component(0) - 3.0).abs() < 1e-10);
        t.set_component(0, 10.0);
        assert!((t.component(0) - 10.0).abs() < 1e-10);

        let t2 = RnTangent::from_slice(&[3.0, 4.0]);
        assert!((t2.norm() - 5.0).abs() < 1e-10);
        assert!((t2.norm_squared() - 25.0).abs() < 1e-10);

        let v = t2.to_vector();
        assert_eq!(v.len(), 2);

        let t3 = RnTangent::from_vector(DVector::from_vec(vec![1.0, 2.0]));
        assert_eq!(t3.dim(), 2);
    }

    #[test]
    fn test_rn_vee_adjoint() {
        let rn = Rn::from_slice(&[1.0, 2.0, 3.0]);
        let t = rn.vee();
        assert_eq!(t.dim(), 3);

        let adj = rn.adjoint();
        assert_eq!(adj.nrows(), 3);
        assert_eq!(adj.ncols(), 3);
    }

    #[test]
    fn test_rn_tangent_hat_small_adj_lie_bracket() {
        let t = RnTangent::from_slice(&[1.0, 2.0, 3.0]);
        let hat = t.hat();
        assert!(hat.nrows() > 0);

        let sadj = t.small_adj();
        assert!(sadj.nrows() > 0);

        let t2 = RnTangent::from_slice(&[0.5, 1.0, 1.5]);
        let bracket = t.lie_bracket(&t2);
        assert_eq!(bracket.dim(), 3);
        for i in 0..3 {
            assert!(bracket.component(i).abs() < 1e-10);
        }
    }

    #[test]
    fn test_rn_tangent_generator_normalize_normalized() {
        // normalize in-place
        let mut t2 = RnTangent::from_slice(&[3.0, 4.0]);
        t2.normalize();
        assert!((t2.norm() - 1.0).abs() < 1e-9);

        // normalized (returns new)
        let t3 = RnTangent::from_slice(&[0.0, 5.0]);
        let t3n = t3.normalized();
        assert!((t3n.norm() - 1.0).abs() < 1e-9);

        // generator via instance method
        let t_gen = RnTangent::from_slice(&[0.0, 0.0]);
        let gen0 = t_gen.generator(0);
        // generator returns a DMatrix with 1.0 at (0,0)
        assert!((gen0[(0, 0)] - 1.0).abs() < 1e-10);

        let t4 = RnTangent::from_slice(&[1.0, 2.0]);
        assert!(t4.is_approx(&t4, 1e-9));
    }

    #[test]
    fn test_rn_tangent_is_zero() {
        let zero = RnTangent::from_slice(&[0.0, 0.0, 0.0]);
        assert!(zero.is_zero(1e-9));

        let nonzero = RnTangent::from_slice(&[0.1, 0.0, 0.0]);
        assert!(!nonzero.is_zero(1e-9));
    }

    #[test]
    fn test_rn_factory_methods() {
        let id = Rn::identity_with_dim(3);
        assert_eq!(id.dim(), 3);

        let z = Rn::zeros(4);
        assert_eq!(z.dim(), 4);
        for i in 0..4 {
            assert!(z.component(i).abs() < 1e-10);
        }

        let o = Rn::ones(3);
        for i in 0..3 {
            assert!((o.component(i) - 1.0).abs() < 1e-10);
        }

        let r = Rn::random_with_dim(5);
        assert_eq!(r.dim(), 5);

        let ji = Rn::jacobian_identity_with_dim(3);
        assert_eq!(ji.nrows(), 3);
        assert_eq!(ji.ncols(), 3);
    }

    #[test]
    fn test_rn_tangent_factory_methods() {
        let zwd = RnTangent::zero_with_dim(4);
        assert_eq!(zwd.dim(), 4);

        let z = RnTangent::zeros(3);
        for i in 0..3 {
            assert!(z.component(i).abs() < 1e-10);
        }

        let o = RnTangent::ones(3);
        for i in 0..3 {
            assert!((o.component(i) - 1.0).abs() < 1e-10);
        }

        let r = RnTangent::random_with_dim(5);
        assert_eq!(r.dim(), 5);
    }

    #[test]
    fn test_rn_slerp() {
        let a = Rn::from_slice(&[0.0, 0.0]);
        let b = Rn::from_slice(&[4.0, 0.0]);
        let mid = a.slerp(&b, 0.5);
        assert!((mid.component(0) - 2.0).abs() < 1e-9);
    }

    #[test]
    fn test_rn_normalize_is_valid() {
        let mut rn = Rn::from_slice(&[1.0, 2.0]);
        rn.normalize();
        assert!(rn.is_valid(1e-9));
        // After normalize, it should have unit norm
        assert!((rn.norm() - 1.0).abs() < 1e-9);
    }

    #[test]
    fn rn_param_slice_round_trip() {
        let g = Rn::random_with_dim(4);
        let recovered = Rn::from_param_slice(g.as_param_slice());
        assert!(g.is_approx(&recovered, 1e-14));
    }

    #[test]
    fn rn_tangent_slice_round_trip() {
        let t = RnTangent::from_slice(&[1.0, 2.0, 3.0]);
        let recovered = RnTangent::from_slice(t.as_slice());
        assert!(t.is_approx(&recovered, 1e-14));
    }

    #[test]
    fn test_bijective_rn_tangent() {
        let tangent_expected = RnTangent::from_slice(&[1.0, 2.0, 3.0]);
        let slice_expected = tangent_expected.as_slice();
        let tangent_actual = RnTangent::from_slice(slice_expected);
        assert!(tangent_expected.is_approx(&tangent_actual, 1e-14));
    }
}