Added determinants and inversion.

src/matrix.rs

@@ -49,6 +49,8 @@ pub trait Matrix<T, ColVec, RowVec> {
     
     pure fn col(i: uint) -> ColVec;
     pure fn row(i: uint) -> RowVec;
+
+    pure fn det() -> T;
 }
 
 pub trait NumericMatrix<T, ColVec> {
@@ -61,13 +63,14 @@ pub trait SquareMatrix<T> {
     pure fn sub_m(other: &self) -> self;
     pure fn mul_m(other: &self) -> self;
     
-    // pure fn invert(other: &self) -> self;
+    pure fn invert() -> Option<self>;
     pure fn transpose() -> self;
     
     pure fn is_identity() -> bool;
     pure fn is_symmetric() -> bool;
     pure fn is_diagonal() -> bool;
     pure fn is_rotated() -> bool;
+    pure fn is_invertible() -> bool;
 }
 
 pub trait Matrix2<T> {
@@ -131,7 +134,7 @@ pub mod Mat2 {
     }
 }
 
-pub impl<T:Copy> Mat2<T>: Matrix<T, Vec2<T>, Vec2<T>> {
+pub impl<T:Copy Num NumCast> Mat2<T>: Matrix<T, Vec2<T>, Vec2<T>> {
     #[inline(always)]
     pure fn rows() -> uint { 2 }
     
@@ -152,9 +155,13 @@ pub impl<T:Copy> Mat2<T>: Matrix<T, Vec2<T>, Vec2<T>> {
         Vec2::new(self[0][i],
                   self[1][i])
     }
+
+    pure fn det() -> T {
+       self[0][0] * self[1][1]
+    }
 }
 
-pub impl<T:Copy Num> Mat2<T>: NumericMatrix<T, Vec2<T>> {
+pub impl<T:Copy Num NumCast> Mat2<T>: NumericMatrix<T, Vec2<T>> {
     #[inline(always)]
     pure fn mul_t(value: T) -> Mat2<T> {
         Mat2::from_cols(self[0].mul_t(value),
@@ -187,9 +194,17 @@ pub impl<T:Copy Num NumCast FuzzyEq> Mat2<T>: SquareMatrix<T> {
                   self.row(0).dot(&other.col(1)), self.row(1).dot(&other.col(1)))
     }
     
-    // TODO - inversion is harrrd D:
+    #[inline(always)]
-    // #[inline(always)]
+    pure fn invert() -> Option<Mat2<T>> {
-    // pure fn invert(other: &Mat2<T>) -> Mat2<T> {}
+        let _0 = cast(0);
+        let d = self.det();
+        if d.fuzzy_eq(&_0) {
+            None
+        } else {
+            Some(Mat2::new(self[1][1]/d, -self[1][0]/d,
+                           -self[0][1]/d, self[0][0]/d))
+        }
+    }
     
     #[inline(always)]
     pure fn transpose() -> Mat2<T> {
@@ -219,6 +234,12 @@ pub impl<T:Copy Num NumCast FuzzyEq> Mat2<T>: SquareMatrix<T> {
     pure fn is_rotated() -> bool {
         !self.fuzzy_eq(&Mat2::identity())
     }
+
+    #[inline(always)]
+    pure fn is_invertible() -> bool {
+        let _0 = cast(0);
+        !self.det().fuzzy_eq(&_0)
+    }
 }
 
 pub impl<T:Copy Num NumCast FuzzyEq> Mat2<T>: Matrix2<T> {
@@ -342,7 +363,7 @@ pub mod Mat3 {
     }
 }
 
-pub impl<T:Copy> Mat3<T>: Matrix<T, Vec3<T>, Vec3<T>> {
+pub impl<T:Copy Num NumCast> Mat3<T>: Matrix<T, Vec3<T>, Vec3<T>> {
     #[inline(always)]
     pure fn rows() -> uint { 3 }
     
@@ -364,9 +385,13 @@ pub impl<T:Copy> Mat3<T>: Matrix<T, Vec3<T>, Vec3<T>> {
                   self[1][i],
                   self[2][i])
     }
+
+    pure fn det() -> T {
+        self.col(0).dot(&self.col(1).cross(&self.col(2)))
+    }
 }
 
-pub impl<T:Copy Num> Mat3<T>: NumericMatrix<T, Vec3<T>> {
+pub impl<T:Copy Num NumCast> Mat3<T>: NumericMatrix<T, Vec3<T>> {
     #[inline(always)]
     pure fn mul_t(value: T) -> Mat3<T> {
         Mat3::from_cols(self[0].mul_t(value),
@@ -404,9 +429,19 @@ pub impl<T:Copy Num NumCast FuzzyEq> Mat3<T>: SquareMatrix<T> {
                   self.row(0).dot(&other.col(2)), self.row(1).dot(&other.col(2)), self.row(2).dot(&other.col(2)))
     }
     
-    // TODO - inversion is harrrd D:
     // #[inline(always)]
-    // pure fn invert(other: &Mat3) -> Mat3 {}
+    pure fn invert() -> Option<Mat3<T>> {
+        let d = self.det();
+        let _0 = cast(0);
+        if d.fuzzy_eq(&_0) {
+            None
+        } else {
+            Some(Mat3::from_cols(self[1].cross(&self[2]).div_t(d),
+                                 self[2].cross(&self[0]).div_t(d),
+                                 self[0].cross(&self[1]).div_t(d))
+            .transpose())
+        }
+    }
     
     #[inline(always)]
     pure fn transpose() -> Mat3<T> {
@@ -449,6 +484,12 @@ pub impl<T:Copy Num NumCast FuzzyEq> Mat3<T>: SquareMatrix<T> {
     pure fn is_rotated() -> bool {
         !self.fuzzy_eq(&Mat3::identity())
     }
+
+    #[inline(always)]
+    pure fn is_invertible() -> bool {
+        let _0 = cast(0);
+        !self.det().fuzzy_eq(&_0)
+    }
 }
 
 pub impl<T:Copy Num NumCast FuzzyEq> Mat3<T>: Matrix3<T> {
@@ -636,7 +677,7 @@ pub mod Mat4 {
     }
 }
 
-pub impl<T:Copy> Mat4<T>: Matrix<T, Vec4<T>, Vec4<T>> {
+pub impl<T:Copy Num NumCast FuzzyEq> Mat4<T>: Matrix<T, Vec4<T>, Vec4<T>> {
     #[inline(always)]
     pure fn rows() -> uint { 4 }
     
@@ -659,9 +700,24 @@ pub impl<T:Copy> Mat4<T>: Matrix<T, Vec4<T>, Vec4<T>> {
                   self[2][i],
                   self[3][i])
     }
+
+    pure fn det() -> T {
+        self[0][0]*Mat3::new(self[1][1], self[2][1], self[3][1],
+                             self[1][2], self[2][2], self[3][2],
+                             self[1][3], self[2][3], self[3][3]).det() -
+        self[1][0]*Mat3::new(self[0][1], self[2][1], self[3][1],
+                             self[0][2], self[2][2], self[3][2],
+                             self[0][3], self[2][3], self[3][3]).det() +
+        self[2][0]*Mat3::new(self[0][1], self[1][1], self[3][1],
+                             self[0][2], self[1][2], self[3][2],
+                             self[0][3], self[1][3], self[3][3]).det() -
+        self[3][0]*Mat3::new(self[0][1], self[1][1], self[2][1],
+                             self[0][2], self[1][2], self[2][2],
+                             self[0][3], self[1][3], self[2][3]).det()
+    }
 }
 
-pub impl<T:Copy Num> Mat4<T>: NumericMatrix<T, Vec4<T>> {
+pub impl<T:Copy Num NumCast FuzzyEq> Mat4<T>: NumericMatrix<T, Vec4<T>> {
     #[inline(always)]
     pure fn mul_t(value: T) -> Mat4<T> {
         Mat4::from_cols(self[0].mul_t(value),
@@ -679,7 +735,7 @@ pub impl<T:Copy Num> Mat4<T>: NumericMatrix<T, Vec4<T>> {
     }
 }
 
-pub impl<T:Copy Num NumCast FuzzyEq> Mat4<T>: SquareMatrix<T> {
+pub impl<T:Copy Num NumCast FuzzyEq Ord> Mat4<T>: SquareMatrix<T> {
     #[inline(always)]
     pure fn add_m(other: &Mat4<T>) -> Mat4<T> {
         Mat4::from_cols(self[0].add_v(&other[0]),
@@ -707,9 +763,74 @@ pub impl<T:Copy Num NumCast FuzzyEq> Mat4<T>: SquareMatrix<T> {
                   self.row(0).dot(&other.col(3)), self.row(1).dot(&other.col(3)), self.row(2).dot(&other.col(3)), self.row(3).dot(&other.col(3)))
     }
     
-    // TODO - inversion is harrrd D:
+    pure fn invert() -> Option<Mat4<T>> {
-    // #[inline(always)]
+        let d = self.det();
-    // pure fn invert(other: &Mat4<T>) -> Mat4<T> {}
+        let _0 = cast(0);
+        if d.fuzzy_eq(&_0) {
+            None
+        } else {
+
+            // Gauss Jordan Elimination with partial pivoting
+
+            let mut a = self.transpose();
+            let mut inv = Mat4::identity::<T>();
+
+            // Find largest pivot column j among rows j..3
+            uint::range(0, 4, |j| {
+                let mut i1 = j;
+                uint::range(j + 1, 4, |i| {
+                    // There should really be a generic abs function
+                    let one = a[i][j];
+                    let two = a[i1][j];
+                    if one < _0 && two < _0 && -one > -two {
+                        i1 = i;
+                    } else if one > _0 && two > _0 && one > two {
+                        i1 = i;
+                    } else if one < _0 && two > _0 && -one > two {
+                        i1 = i;
+                    } else if one > _0 && two < _0 && one > -two {
+                        i1 = i;
+                    }
+                    true
+                });
+
+                // Swap rows i1 and j in a and inv to
+                // put pivot on diagonal
+                let c = [mut a.x, a.y, a.z, a.w];
+                c[i1] <-> c[j];
+                a = Mat4::from_cols(c[0], c[1], c[2], c[3]);
+                let c = [mut inv.x, inv.y, inv.z, inv.w];
+                c[i1] <-> c[j];
+                inv = Mat4::from_cols(c[0], c[1], c[2], c[3]);
+
+                // Scale row j to have a unit diagonal
+                let c = [mut inv.x, inv.y, inv.z, inv.w];
+                c[j] = c[j].div_t(a[j][j]);
+                inv = Mat4::from_cols(c[0], c[1], c[2], c[3]);
+                let c = [mut a.x, a.y, a.z, a.w];
+                c[j] = c[j].div_t(a[j][j]);
+                a = Mat4::from_cols(c[0], c[1], c[2], c[3]);
+
+                // Eliminate off-diagonal elems in col j of a,
+                // doing identical ops to inv
+                uint::range(0, 4, |i| {
+                    if i != j {
+                        let c = [mut inv.x, inv.y, inv.z, inv.w];
+                        c[i] = c[i].sub_v(&c[j].mul_t(a[i][j]));
+                        inv = Mat4::from_cols(c[0], c[1], c[2], c[3]);
+
+                        let c = [mut a.x, a.y, a.z, a.w];
+                        c[i] = c[i].sub_v(&c[j].mul_t(a[i][j]));
+                        a = Mat4::from_cols(c[0], c[1], c[2], c[3]); 
+                    }
+                    true
+                });
+
+                true
+            });
+            Some(inv.transpose())
+        }
+    }
     
     #[inline(always)]
     pure fn transpose() -> Mat4<T> {
@@ -767,6 +888,12 @@ pub impl<T:Copy Num NumCast FuzzyEq> Mat4<T>: SquareMatrix<T> {
     pure fn is_rotated() -> bool {
         !self.fuzzy_eq(&Mat4::identity())
     }
+
+    #[inline(always)]
+    pure fn is_invertible() -> bool {
+        let _0 = cast(0);
+        !self.det().fuzzy_eq(&_0)
+    }
 }
 
 pub impl<T> Mat4<T>: Matrix4<T> {

src/test/test_matrix.rs

@@ -30,6 +30,8 @@ fn test_Mat2() {
     assert a.col(0) == Vec2::new(1f, 3f);
     assert a.col(1) == Vec2::new(2f, 4f);
 
+    assert a.det() == 4f;
+    
     assert a.neg() == Mat2::new(-1f, -3f,
                                 -2f, -4f);
     assert -a == a.neg();
@@ -48,6 +50,12 @@ fn test_Mat2() {
     assert a.transpose() == Mat2::new(1f, 2f,
                                       3f, 4f);
 
+    assert option::unwrap(a.invert()) == Mat2::new(1f,     -0.5f,
+                                                   -0.75f, 0.25f);
+
+    assert Mat2::new(0f, 2f,
+                     3f, 5f).invert().is_none();
+    
     // exact_eq
     // fuzzy_eq
     // eq
@@ -56,11 +64,13 @@ fn test_Mat2() {
     assert Mat2::identity::<float>().is_symmetric();
     assert Mat2::identity::<float>().is_diagonal();
     assert !Mat2::identity::<float>().is_rotated();
+    assert Mat2::identity::<float>().is_invertible();
     
     assert !a.is_identity();
     assert !a.is_symmetric();
     assert !a.is_diagonal();
     assert a.is_rotated();
+    assert a.is_invertible();
     
     let c = Mat2::new(2f, 1f,
                       1f, 2f);
@@ -68,6 +78,7 @@ fn test_Mat2() {
     assert c.is_symmetric();
     assert !c.is_diagonal();
     assert c.is_rotated();
+    assert c.is_invertible();
     
     assert Mat2::from_value(6f).is_diagonal();
     
@@ -121,6 +132,8 @@ fn test_Mat3() {
     assert a.col(1) == Vec3::new(2f, 5f, 8f);
     assert a.col(2) == Vec3::new(3f, 6f, 9f);
 
+    assert a.det() == 0f;
+    
     assert a.neg() == Mat3::new(-1f, -4f, -7f,
                                 -2f, -5f, -8f,
                                 -3f, -6f, -9f);
@@ -145,6 +158,17 @@ fn test_Mat3() {
                                       4f, 5f, 6f,
                                       7f, 8f, 9f);
 
+    assert a.invert().is_none();
+
+    assert option::unwrap(Mat3::identity::<float>().invert())
+        == Mat3::identity::<float>();
+    
+    assert option::unwrap(Mat3::new(2f, 4f, 6f,
+                                    0f, 2f, 4f,
+                                    0f, 0f, 1f).invert())
+        == Mat3::new(0.5f,  -1f,  1f,
+                       0f, 0.5f, -2f,
+                       0f,   0f,  1f);
     // exact_eq
     // fuzzy_eq
     // eq
@@ -153,11 +177,13 @@ fn test_Mat3() {
     assert Mat3::identity::<float>().is_symmetric();
     assert Mat3::identity::<float>().is_diagonal();
     assert !Mat3::identity::<float>().is_rotated();
+    assert Mat3::identity::<float>().is_invertible();
     
     assert !a.is_identity();
     assert !a.is_symmetric();
     assert !a.is_diagonal();
     assert a.is_rotated();
+    assert !a.is_invertible();
     
     let c = Mat3::new(3f, 2f, 1f,
                       2f, 3f, 2f,
@@ -166,6 +192,7 @@ fn test_Mat3() {
     assert c.is_symmetric();
     assert !c.is_diagonal();
     assert c.is_rotated();
+    assert c.is_invertible();
     
     assert Mat3::from_value(6f).is_diagonal();
     
@@ -187,6 +214,10 @@ fn test_Mat4() {
                    y: Vec4 { x: 3f, y: 7f, z: 11f, w: 15f },
                    z: Vec4 { x: 4f, y: 8f, z: 12f, w: 16f },
                    w: Vec4 { x: 5f, y: 9f, z: 13f, w: 17f } };
+    let c = Mat4 { x: Vec4 { x: 3f, y: 2f, z:  1f, w:  1f },
+                   y: Vec4 { x: 2f, y: 3f, z:  2f, w:  2f },
+                   z: Vec4 { x: 1f, y: 2f, z:  3f, w:  3f },
+                   w: Vec4 { x: 0f, y: 1f, z:  1f, w:  0f } };
     let v1 = Vec4::new(1f, 2f, 3f, 4f);
     let f1 = 0.5f;
     
@@ -233,6 +264,8 @@ fn test_Mat4() {
     assert a.col(2) == Vec4::new(3f, 7f, 11f, 15f);
     assert a.col(3) == Vec4::new(4f, 8f, 12f, 16f);
 
+    assert a.det() == 0f;
+    
     assert a.neg() == Mat4::new(-1f, -5f,  -9f, -13f,
                                 -2f, -6f, -10f, -14f,
                                 -3f, -7f, -11f, -15f,
@@ -263,6 +296,15 @@ fn test_Mat4() {
                                        9f, 10f, 11f, 12f,
                                       13f, 14f, 15f, 16f);
 
+    assert option::unwrap(c.invert())
+        == Mat4::new( 5f, -4f,  1f,  0f,
+                     -4f,  8f, -4f,  0f,
+                      4f, -8f,  4f,  8f,
+                     -3f,  4f,  1f, -8f).mul_t(0.125f);
+
+    assert option::unwrap(Mat4::identity::<float>().invert())
+        == Mat4::identity::<float>();
+    
     // exact_eq
     // fuzzy_eq
     // eq
@@ -271,11 +313,13 @@ fn test_Mat4() {
     assert Mat4::identity::<float>().is_symmetric();
     assert Mat4::identity::<float>().is_diagonal();
     assert !Mat4::identity::<float>().is_rotated();
+    assert Mat4::identity::<float>().is_invertible();
     
     assert !a.is_identity();
     assert !a.is_symmetric();
     assert !a.is_diagonal();
     assert a.is_rotated();
+    assert !a.is_invertible();
     
     let c = Mat4::new(4f, 3f, 2f, 1f,
                       3f, 4f, 3f, 2f,
@@ -285,6 +329,7 @@ fn test_Mat4() {
     assert c.is_symmetric();
     assert !c.is_diagonal();
     assert c.is_rotated();
+    assert c.is_invertible();
     
     assert Mat4::from_value(6f).is_diagonal();
 }