bb_geometry/linear_algebra/matrix3x3/

mod.rs

use crate::linear_algebra::EPSILON;
use std::ops::{Index, IndexMut, Mul, Sub};

/// Represents a 3x3 linear_algebra with f32 values in row-major order.
///
/// Internal representation is `[[f32; 3]; 3]`.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct Matrix3x3 {
    /// The linear_algebra data stored as an array of arrays (rows).
    data: [[f32; 3]; 3],
}

impl Matrix3x3 {
    /// Creates a new 3x3 linear_algebra from the provided 2D array data.
    ///
    /// The data should be in row-major order: `data[row][column]`.
    ///
    /// # Example
    /// ```
    /// use crate::bb_geometry::linear_algebra::matrix3x3::*;
    /// let linear_algebra = Matrix3x3::new([
    ///     [1.0, 2.0, 3.0],
    ///     [4.0, 5.0, 6.0],
    ///     [7.0, 8.0, 9.0],
    /// ]);
    /// assert_eq!(linear_algebra.get(0, 1), 2.0);
    /// ```
    pub fn new(data: [[f32; 3]; 3]) -> Self {
        Self { data }
    }

    /// Creates an identity linear_algebra (1.0 on the diagonal, 0.0 elsewhere).
    ///
    /// # Example
    /// ```
    /// use crate::bb_geometry::linear_algebra::matrix3x3::*;
    /// let identity = Matrix3x3::identity();
    /// assert_eq!(identity.get(0, 0), 1.0);
    /// assert_eq!(identity.get(1, 1), 1.0);
    /// assert_eq!(identity.get(0, 1), 0.0);
    /// ```
    pub fn identity() -> Self {
        Self {
            data: [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]],
        }
    }

    /// Creates a zero linear_algebra (all elements are 0.0).
    ///
    /// # Example
    /// ```
    /// use crate::bb_geometry::linear_algebra::matrix3x3::*;
    /// let zero = Matrix3x3::zero();
    /// assert_eq!(zero.get(1, 2), 0.0);
    /// ```
    pub fn zero() -> Self {
        Self {
            data: [[0.0; 3]; 3], // Use array initializer
        }
    }

    /// Gets the value at the specified row and column using a method call.
    ///
    /// Prefer using the index operator `linear_algebra[(row, col)]` for more idiomatic access.
    ///
    /// # Panics
    /// Panics if `row` or `col` are out of bounds (>= 3).
    ///
    /// # Example
    /// ```
    /// use crate::bb_geometry::linear_algebra::matrix3x3::*;
    /// let linear_algebra = Matrix3x3::identity();
    /// assert_eq!(linear_algebra.get(1, 1), 1.0);
    /// ```
    #[inline]
    pub fn get(&self, row: usize, col: usize) -> f32 {
        assert!(row < 3, "Row index out of bounds: {}", row);
        assert!(col < 3, "Column index out of bounds: {}", col);
        self.data[row][col]
    }

    /// Sets the value at the specified row and column using a method call.
    ///
    /// Prefer using the mutable index operator `linear_algebra[(row, col)] = value`
    /// for more idiomatic modification.
    ///
    /// # Panics
    /// Panics if `row` or `col` are out of bounds (>= 3).
    ///
    /// # Example
    /// ```
    /// use crate::bb_geometry::linear_algebra::matrix3x3::*;
    /// let mut linear_algebra = Matrix3x3::zero();
    /// linear_algebra.set(2, 1, 5.0);
    /// assert_eq!(linear_algebra.get(2, 1), 5.0);
    /// ```
    #[inline]
    pub fn set(&mut self, row: usize, col: usize, value: f32) {
        assert!(row < 3, "Row index out of bounds: {}", row);
        assert!(col < 3, "Column index out of bounds: {}", col);
        self.data[row][col] = value;
    }

    /// Returns the transpose of the linear_algebra.
    ///
    /// The element at `(row, col)` in the original linear_algebra becomes
    /// the element at `(col, row)` in the transposed linear_algebra.
    /// Does not modify the original linear_algebra.
    ///
    /// # Example
    /// ```
    /// use crate::bb_geometry::linear_algebra::matrix3x3::*;
    /// let linear_algebra = Matrix3x3::new([
    ///     [1.0, 2.0, 3.0],
    ///     [4.0, 5.0, 6.0],
    ///     [7.0, 8.0, 9.0],
    /// ]);
    /// let transposed = linear_algebra.transpose();
    /// assert_eq!(transposed.get(0, 1), 4.0); // linear_algebra.get(1, 0)
    /// assert_eq!(transposed.get(1, 0), 2.0); // linear_algebra.get(0, 1)
    /// assert_eq!(transposed.get(2, 1), 6.0); // linear_algebra.get(1, 2)
    /// ```
    pub fn transpose(&self) -> Self {
        let mut result_data = [[0.0f32; 3]; 3];
        for i in 0..3 {
            for j in 0..3 {
                result_data[j][i] = self.data[i][j]; // Swap indices
            }
        }
        Self { data: result_data }
    }

    /// Performs linear_algebra multiplication: `self * other`.
    ///
    /// This method delegates to the `*` operator implementation provided by `Mul`.
    /// Does not modify the original matrices.
    ///
    /// # Example
    /// ```
    /// use crate::bb_geometry::linear_algebra::matrix3x3::*;
    /// let a = Matrix3x3::new([
    ///     [1.0, 2.0, 0.0],
    ///     [3.0, 4.0, 0.0],
    ///     [0.0, 0.0, 1.0],
    /// ]);
    /// let b = Matrix3x3::new([
    ///     [5.0, 6.0, 0.0],
    ///     [7.0, 8.0, 0.0],
    ///     [0.0, 0.0, 1.0],
    /// ]);
    /// let result = a.multiply(&b);
    /// // 1*5 + 2*7 = 19
    /// assert_eq!(result.get(0, 0), 19.0);
    /// // 1*6 + 2*8 = 22
    /// assert_eq!(result.get(0, 1), 22.0);
    /// // 3*5 + 4*7 = 15 + 28 = 43
    /// assert_eq!(result.get(1, 0), 43.0);
    /// // 3*6 + 4*8 = 18 + 32 = 50
    /// assert_eq!(result.get(1, 1), 50.0);
    /// ```
    #[inline]
    pub fn multiply(&self, other: &Self) -> Self {
        // Delegate to the Mul trait implementation
        self * other
    }

    /// Checks if a linear_algebra has components not exceeding EPSILON.
    ///
    /// # Example
    /// ```
    /// use crate::bb_geometry::linear_algebra::matrix3x3::*;
    /// use crate::bb_geometry::linear_algebra::EPSILON;
    /// let zero_matrix = Matrix3x3::zero();
    /// let matrix_with_epsilon_entries = Matrix3x3::new([[EPSILON, EPSILON, 0.0], [0.0, 0.0, 0.0], [0.0, EPSILON, 0.0]]);
    /// let matrix_with_entry_exceeding_epsilon = Matrix3x3::new([[EPSILON, 1.1* EPSILON, 0.0], [0.0, 0.0, 0.0], [0.0, EPSILON, 0.0]]);
    ///
    /// let zero_matrix_is_almost_zero = zero_matrix.is_almost_zero();
    /// let matrix_with_epsilon_entries_almost_zero = matrix_with_epsilon_entries.is_almost_zero();
    /// let matrix_with_entry_exceeding_epsilon_is_not_almost_zero = !matrix_with_entry_exceeding_epsilon.is_almost_zero();
    ///
    /// assert!(zero_matrix_is_almost_zero);
    /// assert!(matrix_with_epsilon_entries_almost_zero);
    /// assert!(matrix_with_entry_exceeding_epsilon_is_not_almost_zero);
    /// ```
    pub fn is_almost_zero(&self) -> bool {
        self.data
            .iter()
            .all(|row| row.iter().all(|&value| value.abs() <= EPSILON))
    }
}

// --- Trait Implementations ---

/// Allows accessing linear_algebra elements using tuple indexing `linear_algebra[(row, col)]`.
///
/// # Panics
/// Panics if `row` or `col` are out of bounds (>= 3).
///
/// # Example
/// ```
/// use crate::bb_geometry::linear_algebra::matrix3x3::*;
/// let linear_algebra = Matrix3x3::new([
///     [1.0, 2.0, 3.0],
///     [4.0, 5.0, 6.0],
///     [7.0, 8.0, 9.0],
/// ]);
/// assert_eq!(linear_algebra[(1, 2)], 6.0);
/// ```
impl Index<(usize, usize)> for Matrix3x3 {
    type Output = f32;

    #[inline]
    fn index(&self, index: (usize, usize)) -> &Self::Output {
        assert!(index.0 < 3, "Row index out of bounds: {}", index.0);
        assert!(index.1 < 3, "Column index out of bounds: {}", index.1);
        &self.data[index.0][index.1]
    }
}

/// Allows mutating linear_algebra elements using tuple indexing `linear_algebra[(row, col)] = value`.
///
/// # Panics
/// Panics if `row` or `col` are out of bounds (>= 3).
///
/// # Example
/// ```
/// use crate::bb_geometry::linear_algebra::matrix3x3::*;
/// let mut linear_algebra = Matrix3x3::zero();
/// linear_algebra[(2, 1)] = 99.0;
/// assert_eq!(linear_algebra[(2, 1)], 99.0);
/// ```
impl IndexMut<(usize, usize)> for Matrix3x3 {
    #[inline]
    fn index_mut(&mut self, index: (usize, usize)) -> &mut Self::Output {
        assert!(index.0 < 3, "Row index out of bounds: {}", index.0);
        assert!(index.1 < 3, "Column index out of bounds: {}", index.1);
        &mut self.data[index.0][index.1]
    }
}

/// Implements linear_algebra multiplication using the `*` operator (`&Matrix3x3 * &Matrix3x3`).
///
/// This is often the most convenient way to perform linear_algebra multiplication.
///
/// # Example
/// ```
/// use crate::bb_geometry::linear_algebra::matrix3x3::*;
/// let a = Matrix3x3::identity();
/// let b = Matrix3x3::new([
///     [1.0, 2.0, 3.0],
///     [4.0, 5.0, 6.0],
///     [7.0, 8.0, 9.0],
/// ]);
/// let result = &a * &b; // Multiplying by identity returns the original
/// assert_eq!(result, b);
///
/// let c = Matrix3x3::new([
///     [2.0, 0.0, 0.0],
///     [0.0, 3.0, 0.0],
///     [0.0, 0.0, 1.0],
/// ]);
/// let scaled_b = &b * &c; // Scale columns of b
/// assert_eq!(scaled_b[(0, 0)], 2.0); // 1*2
/// assert_eq!(scaled_b[(1, 1)], 15.0); // 5*3
/// assert_eq!(scaled_b[(2, 2)], 9.0); // 9*1
/// ```
impl Mul<&Matrix3x3> for &Matrix3x3 {
    type Output = Matrix3x3;

    fn mul(self, rhs: &Matrix3x3) -> Self::Output {
        let mut result_data = [[0.0f32; 3]; 3];
        for i in 0..3 {
            // Result row
            for j in 0..3 {
                // Result column
                let mut sum = 0.0;
                for k in 0..3 {
                    // Inner dimension K
                    // Accessing via internal data:
                    sum += self.data[i][k] * rhs.data[k][j];
                }
                result_data[i][j] = sum;
            }
        }
        Matrix3x3 { data: result_data }
    }
}

// Optional: Implement multiplication for owned values as well, typically by delegating
impl Mul<Matrix3x3> for Matrix3x3 {
    type Output = Matrix3x3;

    #[inline]
    fn mul(self, rhs: Matrix3x3) -> Self::Output {
        // Delegate to the reference implementation
        &self * &rhs
    }
}

impl Mul<&Matrix3x3> for Matrix3x3 {
    type Output = Matrix3x3;

    #[inline]
    fn mul(self, rhs: &Matrix3x3) -> Self::Output {
        &self * rhs
    }
}

impl Mul<Matrix3x3> for &Matrix3x3 {
    type Output = Matrix3x3;

    #[inline]
    fn mul(self, rhs: Matrix3x3) -> Self::Output {
        self * &rhs
    }
}

impl Sub<&Matrix3x3> for &Matrix3x3 {
    type Output = Matrix3x3;
    #[inline]
    fn sub(self, rhs: &Matrix3x3) -> Self::Output {
        Matrix3x3::new([
            [
                self[(0, 0)] - rhs[(0, 0)],
                self[(0, 1)] - rhs[(0, 1)],
                self[(0, 2)] - rhs[(0, 2)],
            ],
            [
                self[(1, 0)] - rhs[(1, 0)],
                self[(1, 1)] - rhs[(1, 1)],
                self[(1, 2)] - rhs[(1, 2)],
            ],
            [
                self[(2, 0)] - rhs[(2, 0)],
                self[(2, 1)] - rhs[(2, 1)],
                self[(2, 2)] - rhs[(2, 2)],
            ],
        ])
    }
}

impl Sub<Matrix3x3> for Matrix3x3 {
    type Output = Matrix3x3;
    #[inline]
    fn sub(self, rhs: Matrix3x3) -> Self::Output {
        &self - &rhs
    }
}

impl Sub<&Matrix3x3> for Matrix3x3 {
    type Output = Matrix3x3;
    #[inline]
    fn sub(self, rhs: &Matrix3x3) -> Self::Output {
        &self - rhs
    }
}

impl Sub<Matrix3x3> for &Matrix3x3 {
    type Output = Matrix3x3;
    #[inline]
    fn sub(self, rhs: Matrix3x3) -> Self::Output {
        self - &rhs
    }
}