implement submatrix for matrix4x4

rtchallenge/src/matrices.rs

@@ -64,8 +64,8 @@ impl Matrix3x3 {
     /// );
     /// ```
     pub fn submatrix(&self, row: usize, col: usize) -> Matrix2x2 {
-        assert!(0 <= row && row < 3);
-        assert!(0 <= col && col < 3);
+        assert!(row < 3);
+        assert!(col < 3);
         let mut rows = vec![];
         for r in 0..3 {
             if r != row {
@@ -262,6 +262,45 @@ impl Matrix4x4 {
         }
         Matrix4x4 { m: minv }
     }
+    /// submatrix extracts a 3x3 matrix ignoring the 0-based `row` and `col` given.
+    ///
+    /// # Examples
+    /// ```
+    /// use rtchallenge::matrices::{Matrix3x3, Matrix4x4};
+    ///
+    /// assert_eq!(
+    ///     Matrix4x4::new(
+    ///         [-6., 1., 1., 6.],
+    ///         [-8., 5., 8., 6.],
+    ///         [-1., 0., 8., 2.],
+    ///         [-7., 1., -1., 1.],
+    ///     )
+    ///     .submatrix(2, 1),
+    ///     Matrix3x3::new([-6., 1., 6.], [-8., 8., 6.], [-7., -1., 1.],)
+    /// );
+    /// ```
+    pub fn submatrix(&self, row: usize, col: usize) -> Matrix3x3 {
+        assert!(row < 4);
+        assert!(col < 4);
+        let mut rows = vec![];
+        for r in 0..4 {
+            if r != row {
+                let mut v = vec![];
+                for c in 0..4 {
+                    if c != col {
+                        v.push(self[(r, c)]);
+                    }
+                }
+                rows.push(v);
+            }
+        }
+        let m = [
+            [rows[0][0], rows[0][1], rows[0][2]],
+            [rows[1][0], rows[1][1], rows[1][2]],
+            [rows[2][0], rows[2][1], rows[2][2]],
+        ];
+        Matrix3x3 { m }
+    }
 }
 
 impl fmt::Debug for Matrix4x4 {