From f7fb7f71005d809f598f506f5729feed2c2525cd Mon Sep 17 00:00:00 2001 From: Luqman Aden Date: Wed, 7 Nov 2012 21:34:38 -0500 Subject: [PATCH] Added determinants and inversion. --- src/matrix.rs | 161 +++++++++++++++++++++++++++++++++++----- src/test/test_matrix.rs | 47 +++++++++++- 2 files changed, 190 insertions(+), 18 deletions(-) diff --git a/src/matrix.rs b/src/matrix.rs index 307c327..57cc78d 100644 --- a/src/matrix.rs +++ b/src/matrix.rs @@ -49,6 +49,8 @@ pub trait Matrix { pure fn col(i: uint) -> ColVec; pure fn row(i: uint) -> RowVec; + + pure fn det() -> T; } pub trait NumericMatrix { @@ -61,13 +63,14 @@ pub trait SquareMatrix { pure fn sub_m(other: &self) -> self; pure fn mul_m(other: &self) -> self; - // pure fn invert(other: &self) -> self; + pure fn invert() -> Option; 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 { @@ -131,7 +134,7 @@ pub mod Mat2 { } } -pub impl Mat2: Matrix, Vec2> { +pub impl Mat2: Matrix, Vec2> { #[inline(always)] pure fn rows() -> uint { 2 } @@ -152,9 +155,13 @@ pub impl Mat2: Matrix, Vec2> { Vec2::new(self[0][i], self[1][i]) } + + pure fn det() -> T { + self[0][0] * self[1][1] + } } -pub impl Mat2: NumericMatrix> { +pub impl Mat2: NumericMatrix> { #[inline(always)] pure fn mul_t(value: T) -> Mat2 { Mat2::from_cols(self[0].mul_t(value), @@ -187,9 +194,17 @@ pub impl Mat2: SquareMatrix { self.row(0).dot(&other.col(1)), self.row(1).dot(&other.col(1))) } - // TODO - inversion is harrrd D: - // #[inline(always)] - // pure fn invert(other: &Mat2) -> Mat2 {} + #[inline(always)] + pure fn invert() -> Option> { + 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 { @@ -219,6 +234,12 @@ pub impl Mat2: SquareMatrix { 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 Mat2: Matrix2 { @@ -342,7 +363,7 @@ pub mod Mat3 { } } -pub impl Mat3: Matrix, Vec3> { +pub impl Mat3: Matrix, Vec3> { #[inline(always)] pure fn rows() -> uint { 3 } @@ -364,9 +385,13 @@ pub impl Mat3: Matrix, Vec3> { self[1][i], self[2][i]) } + + pure fn det() -> T { + self.col(0).dot(&self.col(1).cross(&self.col(2))) + } } -pub impl Mat3: NumericMatrix> { +pub impl Mat3: NumericMatrix> { #[inline(always)] pure fn mul_t(value: T) -> Mat3 { Mat3::from_cols(self[0].mul_t(value), @@ -404,9 +429,19 @@ pub impl Mat3: SquareMatrix { 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> { + 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 { @@ -449,6 +484,12 @@ pub impl Mat3: SquareMatrix { 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 Mat3: Matrix3 { @@ -636,7 +677,7 @@ pub mod Mat4 { } } -pub impl Mat4: Matrix, Vec4> { +pub impl Mat4: Matrix, Vec4> { #[inline(always)] pure fn rows() -> uint { 4 } @@ -659,9 +700,24 @@ pub impl Mat4: Matrix, Vec4> { 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 Mat4: NumericMatrix> { +pub impl Mat4: NumericMatrix> { #[inline(always)] pure fn mul_t(value: T) -> Mat4 { Mat4::from_cols(self[0].mul_t(value), @@ -679,7 +735,7 @@ pub impl Mat4: NumericMatrix> { } } -pub impl Mat4: SquareMatrix { +pub impl Mat4: SquareMatrix { #[inline(always)] pure fn add_m(other: &Mat4) -> Mat4 { Mat4::from_cols(self[0].add_v(&other[0]), @@ -707,9 +763,74 @@ pub impl Mat4: SquareMatrix { 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: - // #[inline(always)] - // pure fn invert(other: &Mat4) -> Mat4 {} + pure fn invert() -> Option> { + let d = self.det(); + 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::(); + + // 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 { @@ -767,6 +888,12 @@ pub impl Mat4: SquareMatrix { 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 Mat4: Matrix4 { @@ -828,4 +955,4 @@ pub impl Mat4: ToPtr { pure fn to_ptr() -> *T { self[0].to_ptr() } -} \ No newline at end of file +} diff --git a/src/test/test_matrix.rs b/src/test/test_matrix.rs index cc7142e..770a21d 100644 --- a/src/test/test_matrix.rs +++ b/src/test/test_matrix.rs @@ -29,6 +29,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); @@ -47,6 +49,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 @@ -56,11 +64,13 @@ fn test_Mat2() { assert Mat2::identity::().is_symmetric(); assert Mat2::identity::().is_diagonal(); assert !Mat2::identity::().is_rotated(); + assert Mat2::identity::().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(); @@ -120,6 +131,8 @@ fn test_Mat3() { assert a.col(0) == Vec3::new(1f, 4f, 7f); 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, @@ -144,7 +157,18 @@ fn test_Mat3() { assert a.transpose() == Mat3::new(1f, 2f, 3f, 4f, 5f, 6f, 7f, 8f, 9f); + + assert a.invert().is_none(); + + assert option::unwrap(Mat3::identity::().invert()) + == Mat3::identity::(); + 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::().is_symmetric(); assert Mat3::identity::().is_diagonal(); assert !Mat3::identity::().is_rotated(); + assert Mat3::identity::().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; @@ -232,6 +263,8 @@ fn test_Mat4() { assert a.col(1) == Vec4::new(2f, 6f, 10f, 14f); 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, @@ -262,6 +295,15 @@ fn test_Mat4() { 5f, 6f, 7f, 8f, 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::().invert()) + == Mat4::identity::(); // exact_eq // fuzzy_eq @@ -271,11 +313,13 @@ fn test_Mat4() { assert Mat4::identity::().is_symmetric(); assert Mat4::identity::().is_diagonal(); assert !Mat4::identity::().is_rotated(); + assert Mat4::identity::().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(); -} \ No newline at end of file +}