bb_geometry/linear_algebra/matrix4x4/

mod.rs

use crate::linear_algebra::matrix3x3::Matrix3x3;
use crate::linear_algebra::vector3::Vector3;
use std::ops::{Index, IndexMut, Mul, Sub};
use crate::linear_algebra::EPSILON;

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

impl Matrix4x4 {
    /// Creates a new 4x4 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::matrix4x4::*;
    /// let linear_algebra = Matrix4x4::new([
    ///     [1.0, 2.0, 3.0, 4.0],
    ///     [5.0, 6.0, 7.0, 8.0],
    ///     [9.0, 10.0, 11.0, 12.0],
    ///     [13.0, 14.0, 15.0, 16.0],
    /// ]);
    /// assert_eq!(linear_algebra.get(0, 1), 2.0);
    /// ```
    pub fn new(data: [[f32; 4]; 4]) -> Self {
        Self { data }
    }

    /// Creates an identity linear_algebra (1.0 on the diagonal, 0.0 elsewhere).
    ///
    /// # Example
    /// ```
    /// use crate::bb_geometry::linear_algebra::matrix4x4::*;
    /// let identity = Matrix4x4::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],
                [0.0, 1.0, 0.0, 0.0],
                [0.0, 0.0, 1.0, 0.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::matrix4x4::*;
    /// let zero = Matrix4x4::zero();
    /// assert_eq!(zero.get(2, 3), 0.0);
    /// ```
    pub fn zero() -> Self {
        Self {
            data: [[0.0; 4]; 4], // More concise way to initialize with zeros
        }
    }

    /// 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 (>= 4).
    ///
    /// # Example
    /// ```
    /// use crate::bb_geometry::linear_algebra::matrix4x4::*;
    /// let linear_algebra = Matrix4x4::identity();
    /// assert_eq!(linear_algebra.get(1, 1), 1.0);
    /// ```
    #[inline]
    pub fn get(&self, row: usize, col: usize) -> f32 {
        assert!(row < 4, "Row index out of bounds: {}", row);
        assert!(col < 4, "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 (>= 4).
    ///
    /// # Example
    /// ```
    /// use crate::bb_geometry::linear_algebra::matrix4x4::*;
    /// let mut linear_algebra = Matrix4x4::zero();
    /// linear_algebra.set(2, 3, 5.0);
    /// assert_eq!(linear_algebra.get(2, 3), 5.0);
    /// ```
    #[inline]
    pub fn set(&mut self, row: usize, col: usize, value: f32) {
        assert!(row < 4, "Row index out of bounds: {}", row);
        assert!(col < 4, "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::matrix4x4::*;
    /// let linear_algebra = Matrix4x4::new([
    ///     [1.0, 2.0, 3.0, 4.0],
    ///     [5.0, 6.0, 7.0, 8.0],
    ///     [9.0, 10.0, 11.0, 12.0],
    ///     [13.0, 14.0, 15.0, 16.0],
    /// ]);
    /// let transposed = linear_algebra.transpose();
    /// assert_eq!(transposed.get(0, 1), 5.0); // linear_algebra.get(1, 0)
    /// assert_eq!(transposed.get(1, 0), 2.0); // linear_algebra.get(0, 1)
    /// assert_eq!(transposed.get(3, 2), 12.0); // linear_algebra.get(2, 3)
    /// ```
    pub fn transpose(&self) -> Self {
        let mut result_data = [[0.0f32; 4]; 4];
        for i in 0..4 {
            for j in 0..4 {
                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::matrix4x4::*;
    /// let a = Matrix4x4::new([
    ///     [1.0, 2.0, 0.0, 0.0],
    ///     [3.0, 4.0, 0.0, 0.0],
    ///     [0.0, 0.0, 1.0, 0.0],
    ///     [0.0, 0.0, 0.0, 1.0],
    /// ]);
    /// let b = Matrix4x4::new([
    ///     [5.0, 6.0, 0.0, 0.0],
    ///     [7.0, 8.0, 0.0, 0.0],
    ///     [0.0, 0.0, 1.0, 0.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
    }

    pub fn is_almost_zero(&self) -> bool {
        self.data
            .iter()
            .all(|row| row.iter().all(|&value| value.abs() <= EPSILON))
    }

    /// Creates an affine transformation (rotation and translation) in 3D as 4x4 linear_algebra
    ///
    /// # Example
    ///```
    /// use bb_geometry::linear_algebra::matrix4x4::*;
    /// use bb_geometry::linear_algebra::matrix3x3::*;
    /// use bb_geometry::linear_algebra::vector3::*;
    /// use bb_geometry::linear_algebra::vector4::*;
    ///
    /// let r = Matrix3x3::rotation_from_euler_angles(0.2, 0.4, -0.2);
    /// let a = Vector3::new([5.0, 4.0, 3.0]);
    /// let affine_transformation = Matrix4x4::affine_transformation(&r, &a);
    /// let inverse_affine_transformation = Matrix4x4::affine_transformation(&(r.transpose()), &-(r.transpose() * a));
    ///
    /// let should_be_zero_matrix = inverse_affine_transformation * affine_transformation - Matrix4x4::identity();
    ///
    /// assert!(should_be_zero_matrix.is_almost_zero());
    /// ```
    pub fn affine_transformation(rotation: &Matrix3x3, translation: &Vector3) -> Self {
        Matrix4x4::new([
            [
                rotation[(0, 0)],
                rotation[(0, 1)],
                rotation[(0, 2)],
                translation[0],
            ],
            [
                rotation[(1, 0)],
                rotation[(1, 1)],
                rotation[(1, 2)],
                translation[1],
            ],
            [
                rotation[(2, 0)],
                rotation[(2, 1)],
                rotation[(2, 2)],
                translation[2],
            ],
            [0.0, 0.0, 0.0, 1.0],
        ])
    }

    /// Swaps basis directions
    ///
    /// # Example
    /// ```
    /// use bb_geometry::linear_algebra::matrix4x4::Matrix4x4;
    /// use bb_geometry::linear_algebra::vector4::{E_W, E_X, E_Y, E_Z};
    /// let swap_matrix = Matrix4x4::axes_swap(2, 3);
    ///
    /// let transformed_e_x = swap_matrix * E_X;
    /// let transformed_e_y = swap_matrix * E_Y;
    /// let transformed_e_z = swap_matrix * E_Z;
    /// let transformed_e_w = swap_matrix * E_W;
    /// assert!((transformed_e_x - E_X).is_almost_zero());
    /// assert!((transformed_e_y - E_Y).is_almost_zero());
    /// assert!((transformed_e_z - E_W).is_almost_zero());
    /// assert!((transformed_e_w - E_Z).is_almost_zero());
    /// ```
    pub fn axes_swap(dir1: usize, dir2: usize) -> Self {
        assert!(dir1 < 4, "Direction index out of bounds: {}", dir1);
        assert!(dir2 < 4, "Direction index out of bounds: {}", dir2);
        let mut swap_matrix = Matrix4x4::identity();
        swap_matrix.set(dir1, dir1, 0.0);
        swap_matrix.set(dir2, dir2, 0.0);
        swap_matrix.set(dir1, dir2, 1.0);
        swap_matrix.set(dir2, dir1, 1.0);
        swap_matrix
    }
}

// --- Trait Implementations ---

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

    #[inline]
    fn index(&self, index: (usize, usize)) -> &Self::Output {
        assert!(index.0 < 4, "Row index out of bounds: {}", index.0);
        assert!(index.1 < 4, "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 (>= 4).
///
/// # Example
/// ```
/// use crate::bb_geometry::linear_algebra::matrix4x4::*;
/// let mut linear_algebra = Matrix4x4::zero();
/// linear_algebra[(2, 1)] = 99.0;
/// assert_eq!(linear_algebra[(2, 1)], 99.0);
/// ```
impl IndexMut<(usize, usize)> for Matrix4x4 {
    #[inline]
    fn index_mut(&mut self, index: (usize, usize)) -> &mut Self::Output {
        assert!(index.0 < 4, "Row index out of bounds: {}", index.0);
        assert!(index.1 < 4, "Column index out of bounds: {}", index.1);
        &mut self.data[index.0][index.1]
    }
}

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

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

// Optional: Implement multiplication for owned values as well, typically by delegating
// It's common to implement M * M, M * &M, &M * M by calling the &M * &M version.
impl Mul<Matrix4x4> for Matrix4x4 {
    type Output = Matrix4x4;

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

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

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

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

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


impl Sub<&Matrix4x4> for &Matrix4x4 {
    type Output = Matrix4x4;
    #[inline]
    fn sub(self, rhs: &Matrix4x4) -> Self::Output {
        Matrix4x4::new([
            [
                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[(1, 0)] - rhs[(1, 0)],
                self[(1, 1)] - rhs[(1, 1)],
                self[(1, 2)] - rhs[(1, 2)],
                self[(1, 3)] - rhs[(1, 3)],
            ],
            [
                self[(2, 0)] - rhs[(2, 0)],
                self[(2, 1)] - rhs[(2, 1)],
                self[(2, 2)] - rhs[(2, 2)],
                self[(2, 3)] - rhs[(2, 3)],
            ],
            [
                self[(3, 0)] - rhs[(3, 0)],
                self[(3, 1)] - rhs[(3, 1)],
                self[(3, 2)] - rhs[(3, 2)],
                self[(3, 3)] - rhs[(3, 3)],
            ],
        ])
    }
}

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

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

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