diff --git a/rtchallenge/src/matrices.rs b/rtchallenge/src/matrices.rs index 4bf79e3..e301e2a 100644 --- a/rtchallenge/src/matrices.rs +++ b/rtchallenge/src/matrices.rs @@ -1,5 +1,7 @@ use std::fmt; -use std::ops::{Index, Mul}; +use std::ops::{Index, IndexMut, Mul}; + +use float_cmp::{ApproxEq, F32Margin}; use crate::tuples::Tuple; @@ -150,6 +152,44 @@ impl From<[f32; 16]> for Matrix4x4 { } } +impl<'a> ApproxEq for &'a Matrix4x4 { + type Margin = F32Margin; + + /// Implement float_cmp::ApproxEq for Matrix4x4 + /// + /// # Examples + /// ``` + /// use float_cmp::approx_eq; + /// use rtchallenge::matrices::Matrix4x4; + /// + /// assert!(!approx_eq!( + /// &Matrix4x4, + /// &Matrix4x4::default(), + /// &Matrix4x4::identity(), + /// ulps = 1 + /// )); + /// + /// assert!(approx_eq!( + /// &Matrix4x4, + /// &Matrix4x4::identity(), + /// &Matrix4x4::identity(), + /// ulps = 1 + /// )); + /// ``` + fn approx_eq>(self, m2: Self, margin: T) -> bool { + let m = self; + let margin = margin.into(); + for row in 0..4 { + for col in 0..4 { + if !m[(row, col)].approx_eq(m2[(row, col)], margin) { + return false; + } + } + } + true + } +} + impl Matrix4x4 { /// Create a `Matrix4x4` containing the identity, all zeros with ones along the diagonal. /// # Examples @@ -380,6 +420,140 @@ impl Matrix4x4 { pub fn determinant(&self) -> f32 { (0..4).map(|i| self.cofactor(0, i) * self[(0, i)]).sum() } + + /// Compute invertibility of matrix (i.e. non-zero determinant. + /// + /// # Examples + /// ``` + /// use rtchallenge::matrices::Matrix4x4; + /// + /// let a = Matrix4x4::new( + /// [6., 4., 4., 4.], + /// [5., 5., 7., 6.], + /// [4., -9., 3., -7.], + /// [9., 1., 7., -6.], + /// ); + /// assert_eq!(a.determinant(), -2120.); + /// assert_eq!(a.invertable(), true); + /// + /// let a = Matrix4x4::new( + /// [-4., 2., -2., -3.], + /// [9., 6., 2., 6.], + /// [0., -5., 1., -5.], + /// [0., 0., 0., 0.], + /// ); + /// assert_eq!(a.determinant(), 0.); + /// assert_eq!(a.invertable(), false); + /// ``` + pub fn invertable(&self) -> bool { + self.determinant() != 0. + } + + /// Compute the inverse of a 4x4 matrix. + /// + /// # Examples + /// ``` + /// use float_cmp::approx_eq; + /// use rtchallenge::matrices::Matrix4x4; + /// + /// let a = Matrix4x4::new( + /// [-5., 2., 6., -8.], + /// [1., -5., 1., 8.], + /// [7., 7., -6., -7.], + /// [1., -3., 7., 4.], + /// ); + /// let b = a.inverse(); + /// + /// assert_eq!(a.determinant(), 532.); + /// assert_eq!(a.cofactor(2, 3), -160.); + /// assert_eq!(b[(3, 2)], -160. / 532.); + /// assert_eq!(a.cofactor(3, 2), 105.); + /// assert_eq!(b[(2, 3)], 105. / 532.); + /// assert!(approx_eq!( + /// &Matrix4x4, + /// &b, + /// &Matrix4x4::new( + /// [0.21805, 0.45113, 0.24060, -0.04511], + /// [-0.80827, -1.45677, -0.44361, 0.52068], + /// [-0.07895, -0.22368, -0.05263, 0.19737], + /// [-0.52256, -0.81391, -0.30075, 0.30639], + /// ), + /// epsilon = 0.0001 + /// )); + /// + /// // Second test case + /// assert!(approx_eq!( + /// &Matrix4x4, + /// &Matrix4x4::new( + /// [8., -5., 9., 2.], + /// [7., 5., 6., 1.], + /// [-6., 0., 9., 6.], + /// [-3., 0., -9., -4.], + /// ) + /// .inverse(), + /// &Matrix4x4::new( + /// [-0.15385, -0.15385, -0.28205, -0.53846], + /// [-0.07692, 0.12308, 0.02564, 0.03077], + /// [0.35897, 0.35897, 0.43590, 0.92308], + /// [-0.69231, -0.69241, -0.76923, -1.92308], + /// ), + /// epsilon = 0.0005 + /// )); + /// + /// // Third test case + /// assert!(approx_eq!( + /// &Matrix4x4, + /// &Matrix4x4::new( + /// [9., 3., 0., 9.], + /// [-5., -2., -6., -3.], + /// [-4., 9., 6., 4.], + /// [-7., 6., 6., 2.], + /// ) + /// .inverse(), + /// &Matrix4x4::new( + /// [-0.04074, -0.07778, 0.14444, -0.22222], + /// [-0.07778, 0.03333, 0.36667, -0.33333], + /// [-0.02901, -0.14630, -0.10926, 0.12963], + /// [0.17778, 0.06667, -0.26667, 0.33333], + /// ), + /// epsilon = 0.0001 + /// )); + /// + /// let a = Matrix4x4::new( + /// [3., -9., 7., 3.], + /// [3., -8., 2., -9.], + /// [-4., 4., 4., 1.], + /// [-6., 5., -1., 1.], + /// ); + /// let b = Matrix4x4::new( + /// [8., 2., 2., 2.], + /// [3., -1., 7., 0.], + /// [7., 0., 5., 4.], + /// [6., -2., 0., 5.], + /// ); + /// let c = a * b; + /// assert!(approx_eq!( + /// &Matrix4x4, + /// &(c * b.inverse()), + /// &a, + /// epsilon = 0.0001 + /// )); + /// ``` + pub fn inverse(&self) -> Matrix4x4 { + let m = self; + if !m.invertable() { + panic!("Matrix4x4::inverse called on matrix with determinant() == 0"); + } + let mut m2 = Matrix4x4::identity(); + let det = m.determinant(); + for row in 0..4 { + for col in 0..4 { + let c = m.cofactor(row, col); + m2[(col, row)] = c / det; + } + } + m2 + } } impl fmt::Debug for Matrix4x4 { @@ -484,6 +658,12 @@ impl Index<(usize, usize)> for Matrix4x4 { } } +impl IndexMut<(usize, usize)> for Matrix4x4 { + fn index_mut(&mut self, (row, col): (usize, usize)) -> &mut Self::Output { + &mut self.m[row][col] + } +} + #[cfg(test)] mod tests { use super::*;