matrices: implement Matrix4x4::inverse.

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<T: Into<Self::Margin>>(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::*;