Merge pull request #85 from csherratt/fix_matrix

Fix inverse matrix4

src/cgmath/matrix.rs

@@ -913,44 +913,36 @@ impl<S: BaseFloat> Matrix<S, Vector4<S>> for Matrix4<S> {
     }
 
     fn invert(&self) -> Option<Matrix4<S>> {
-        if self.is_invertible() {
+        let det = self.determinant();
-            // Gauss Jordan Elimination with partial pivoting
+        if !det.approx_eq(&zero()) {
-            // So take this matrix ('mat') augmented with the identity ('ident'),
+            let one: S = one();
-            // and essentially reduce [mat|ident]
+            let inv_det = one / det;
+            let t = self.transpose();
+            let cf = |i, j| {
+                let mat = match i {
+                    0 => Matrix3::from_cols(t.y.truncate_n(j),
+                                            t.z.truncate_n(j),
+                                            t.w.truncate_n(j)),
+                    1 => Matrix3::from_cols(t.x.truncate_n(j),
+                                            t.z.truncate_n(j),
+                                            t.w.truncate_n(j)),
+                    2 => Matrix3::from_cols(t.x.truncate_n(j),
+                                            t.y.truncate_n(j),
+                                            t.w.truncate_n(j)),
+                    3 => Matrix3::from_cols(t.x.truncate_n(j),
+                                            t.y.truncate_n(j),
+                                            t.z.truncate_n(j)),
+                    _ => fail!("out of range")
+                };
+                let sign = if (i+j) & 1 == 1 {-one} else {one};
+                mat.determinant() * sign * inv_det
+            };
 
-            let mut mat = self.clone();
+            Some(Matrix4::new(cf(0, 0), cf(0, 1), cf(0, 2), cf(0, 3),
-            let mut ident = Matrix4::identity();
+                              cf(1, 0), cf(1, 1), cf(1, 2), cf(1, 3),
+                              cf(2, 0), cf(2, 1), cf(2, 2), cf(2, 3),
+                              cf(3, 0), cf(3, 1), cf(3, 2), cf(3, 3)))
 
-            for j in range(0u, 4u) {
-                // Find largest element in col j
-                let mut i1 = j;
-                for i in range(j + 1, 4) {
-                    if mat.cr(j, i).abs() > mat.cr(j, i1).abs() {
-                        i1 = i;
-                    }
-                }
-
-                // Swap columns i1 and j in mat and ident to
-                // put pivot on diagonal
-                mat.swap_c(i1, j);
-                ident.swap_c(i1, j);
-
-                // Scale col j to have a unit diagonal
-                *ident.mut_c(j) = ident.c(j).div_s(mat.cr(j, j).clone());
-                *mat.mut_c(j) = mat.c(j).div_s(mat.cr(j, j).clone());
-
-                // Eliminate off-diagonal elems in col j of 'mat',
-                // doing identical ops to 'ident'
-                for i in range(0u, 4u) {
-                    if i != j {
-                        let ij_mul_aij = ident.c(j).mul_s(mat.cr(i, j).clone());
-                        let aj_mul_aij = mat.c(j).mul_s(mat.cr(i, j).clone());
-                        *ident.mut_c(i) = ident.c(i).sub_v(&ij_mul_aij);
-                        *mat.mut_c(i) = mat.c(i).sub_v(&aj_mul_aij);
-                    }
-                }
-            }
-            Some(ident)
         } else {
             None
         }

src/cgmath/vector.rs

@@ -301,6 +301,18 @@ impl<S: BaseNum> Vector4<S> {
     pub fn truncate(&self)-> Vector3<S> {
         Vector3::new(self.x.clone(), self.y.clone(), self.z.clone())
     }
+
+    /// Create a `Vector3`, dropping the nth element
+    #[inline]
+    pub fn truncate_n(&self, n: int)-> Vector3<S> {
+        match n {
+            0 => Vector3::new(self.y.clone(), self.z.clone(), self.w.clone()),
+            1 => Vector3::new(self.x.clone(), self.z.clone(), self.w.clone()),
+            2 => Vector3::new(self.x.clone(), self.y.clone(), self.w.clone()),
+            3 => Vector3::new(self.x.clone(), self.y.clone(), self.z.clone()),
+            _ => fail!("{} is out of range", n)
+        }
+    }
 }
 
 /// Specifies geometric operations for vectors. This is only implemented for

src/test/matrix.rs

@@ -302,6 +302,30 @@ fn test_invert() {
     let mut mut_c = matrix4::C;
     mut_c.invert_self();
     assert_eq!(mut_c, matrix4::C.invert().unwrap());
+
+    let mat_c = Matrix4::new(-0.131917, -0.76871,   0.625846, 0.,
+                             -0.,        0.631364,  0.775487, 0.,
+                             -0.991261,  0.1023,   -0.083287, 0.,
+                              0.,       -1.262728, -1.550973, 1.);
+    assert!(mat_c.invert().unwrap().mul_m(&mat_c).is_identity());
+
+    let mat_d = Matrix4::new( 0.065455, -0.720002,  0.690879, 0.,
+                             -0.,        0.692364,  0.721549, 0.,
+                             -0.997856, -0.047229,  0.045318, 0.,
+                              0.,       -1.384727, -1.443098, 1.);
+    assert!(mat_d.invert().unwrap().mul_m(&mat_d).is_identity());
+
+    let mat_e = Matrix4::new( 0.409936,  0.683812, -0.603617, 0.,
+                              0.,        0.661778,  0.7497,   0.,
+                              0.912114, -0.307329,  0.271286, 0.,
+                             -0.,       -1.323555, -1.499401, 1.);
+    assert!(mat_e.invert().unwrap().mul_m(&mat_e).is_identity());
+
+    let mat_f = Matrix4::new(-0.160691, -0.772608,  0.614211, 0.,
+                             -0.,        0.622298,  0.78278,  0.,
+                             -0.987005,  0.125786, -0.099998, 0.,
+                              0.,       -1.244597, -1.565561, 1.);
+    assert!(mat_f.invert().unwrap().mul_m(&mat_f).is_identity());
 }
 
 #[test]