spatial_math/

matrix.rs

// Copyright (C) 2020-2025 spatial-math authors. All Rights Reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

use std::ops::{Add, Index, Mul, Sub};

use nalgebra::{Dim, MatrixView3};

use super::{Mat3, Vec3};
use crate::Real;

/// A memory-efficient 3×3 symmetric matrix for spatial algebra operations.
///
/// This structure stores a symmetric 3×3 matrix using only 6 elements instead of 9,
/// exploiting the mathematical property that `a_ij = a_ji` for symmetric matrices.
/// This provides significant memory savings and computational efficiency for
/// inertia tensors and other symmetric operations in spatial algebra.
///
/// # Storage Format
///
/// The matrix is stored in column-major order, containing the lower triangular
/// elements of the symmetric matrix:
/// ```text
/// Stored array: [a₀₀, a₁₀, a₂₀, a₁₁, a₂₁, a₂₂]
///
/// Full matrix:
/// | a₀₀  a₀₁  a₀₂ |
/// | a₁₀  a₁₁  a₁₂ |  where a_ij = a_ji
/// | a₂₀  a₂₁  a₂₂ |
/// ```
///
/// # Applications in Spatial Algebra
///
/// - **Rotational inertia tensors**: Always symmetric, stored efficiently
/// - **Covariance matrices**: Common in state estimation and filtering
/// - **Stiffness/damping matrices**: Used in compliant dynamics
/// - **Quadratic forms**: Energy calculations and optimization
///
/// # Performance Benefits
///
/// - **Memory**: 33% reduction compared to full 3×3 matrix storage
/// - **Cache efficiency**: Compact layout improves memory access patterns
/// - **Computation**: Avoids redundant calculations on symmetric elements
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
#[derive(Debug, Clone, Copy, Default)]
pub struct SymmetricMat3([Real; 6]);

impl SymmetricMat3 {
    /// Identity matrix (diagonal elements are 1, off-diagonal are 0).
    ///
    /// Represents the 3×3 identity matrix in symmetric storage format.
    pub const ONE: Self = Self([1.0, 0.0, 0.0, 1.0, 0.0, 1.0]);
    /// Zero matrix (all elements are 0).
    ///
    /// Useful for initialization and as the additive identity.
    pub const ZERO: Self = Self([0.0; 6]);

    /// Create a symmetric matrix from a 6-element array in column-major order.
    ///
    /// # Input Format
    ///
    /// The array should contain the lower triangular elements in column-major order:
    /// ```text
    /// array = [a₀₀, a₁₀, a₂₀, a₁₁, a₂₁, a₂₂]
    /// ```
    ///
    /// # Example
    ///
    /// ```rust
    /// use spatial_math::SymmetricMat3;
    ///
    /// // Create matrix |1 2 3|
    /// //               |2 4 5|
    /// //               |3 5 6|
    /// let matrix = SymmetricMat3::from_array([1.0, 2.0, 3.0, 4.0, 5.0, 6.0]);
    /// ```
    #[inline]
    pub fn from_array(array: [Real; 6]) -> Self {
        Self(array)
    }

    #[inline]
    pub fn from_iterator(iter: impl IntoIterator<Item = Real>) -> Self {
        let mut array: [Real; 6] = [0.0; 6];
        for (i, val) in iter.into_iter().enumerate() {
            array[i] = val;
        }
        Self(array)
    }

    #[rustfmt::skip]
    #[inline]
    pub fn mat3(&self) -> Mat3 {
        Mat3::from_iterator([
            self.0[0], self.0[1], self.0[2],
            self.0[1], self.0[3], self.0[4],
            self.0[2], self.0[4], self.0[5]
        ])
    }

    #[inline]
    pub fn mul_vec(&self, v: Vec3) -> Vec3 {
        self.mat3() * v
    }

    #[inline]
    #[must_use]
    pub fn scale(&self, scalar: Real) -> Self {
        Self([
            self.0[0] * scalar,
            self.0[1] * scalar,
            self.0[2] * scalar,
            self.0[3] * scalar,
            self.0[4] * scalar,
            self.0[5] * scalar,
        ])
    }

    #[inline]
    #[must_use]
    pub fn add(&self, rhs: Self) -> Self {
        Self([
            self.0[0] + rhs.0[0],
            self.0[1] + rhs.0[1],
            self.0[2] + rhs.0[2],
            self.0[3] + rhs.0[3],
            self.0[4] + rhs.0[4],
            self.0[5] + rhs.0[5],
        ])
    }

    #[inline]
    #[must_use]
    pub fn sub(&self, rhs: Self) -> Self {
        Self([
            self.0[0] - rhs.0[0],
            self.0[1] - rhs.0[1],
            self.0[2] - rhs.0[2],
            self.0[3] - rhs.0[3],
            self.0[4] - rhs.0[4],
            self.0[5] - rhs.0[5],
        ])
    }

    #[inline]
    pub fn into_array(self) -> [Real; 6] {
        self.0
    }
}

impl From<[Real; 6]> for SymmetricMat3 {
    #[inline]
    fn from(array: [Real; 6]) -> Self {
        Self(array)
    }
}

impl Index<(usize, usize)> for SymmetricMat3 {
    type Output = Real;

    #[inline]
    fn index(&self, (row, col): (usize, usize)) -> &Self::Output {
        let (row, col) = if col > row { (col, row) } else { (row, col) };
        if col == 0 {
            &self.0[row]
        } else if col == 1 {
            &self.0[row + col * 3 - 1]
        } else {
            &self.0[row + col * 3 - 3]
        }
    }
}

impl Mul<Vec3> for SymmetricMat3 {
    type Output = Vec3;

    #[inline]
    fn mul(self, rhs: Vec3) -> Self::Output {
        self.mul_vec(rhs)
    }
}

impl<RStride, CStride> From<MatrixView3<'_, Real, RStride, CStride>> for SymmetricMat3
where
    RStride: Dim,
    CStride: Dim,
{
    /// Takes the lower triangle of the matrix and converts it into a `SymmetricMat3`.
    #[inline]
    fn from(mat: MatrixView3<Real, RStride, CStride>) -> Self {
        Self([
            mat[(0, 0)],
            mat[(1, 0)],
            mat[(2, 0)],
            mat[(1, 1)],
            mat[(2, 1)],
            mat[(2, 2)],
        ])
    }
}

impl From<Mat3> for SymmetricMat3 {
    #[inline]
    fn from(mat: Mat3) -> Self {
        Self([
            mat[(0, 0)],
            mat[(1, 0)],
            mat[(2, 0)],
            mat[(1, 1)],
            mat[(2, 1)],
            mat[(2, 2)],
        ])
    }
}

impl Add<Self> for SymmetricMat3 {
    type Output = Self;

    #[inline]
    fn add(self, rhs: Self) -> Self::Output {
        SymmetricMat3::add(&self, rhs)
    }
}

impl Sub<Self> for SymmetricMat3 {
    type Output = Self;

    #[inline]
    fn sub(self, rhs: Self) -> Self::Output {
        SymmetricMat3::sub(&self, rhs)
    }
}

impl Mul<Real> for SymmetricMat3 {
    type Output = Self;

    #[inline]
    fn mul(self, rhs: Real) -> Self::Output {
        self.scale(rhs)
    }
}