diff --git a/src/cgmath/traits/alg/matrix.rs b/src/cgmath/traits/alg/matrix.rs index 7f201e2..e782c22 100644 --- a/src/cgmath/traits/alg/matrix.rs +++ b/src/cgmath/traits/alg/matrix.rs @@ -21,7 +21,7 @@ use traits::alg::VectorSpace; pub trait Matrix < - S: Field + Clone, + S: Field + Clone + ApproxEq, RV: Clone + VectorSpace + ClonableArray, RVSlice, RSlice, CV: Clone + VectorSpace + ClonableArray, CVSlice, CSlice, MT//: Matrix diff --git a/src/cgmath/traits/alg/square_matrix.rs b/src/cgmath/traits/alg/square_matrix.rs index 2bdf594..16cfd99 100644 --- a/src/cgmath/traits/alg/square_matrix.rs +++ b/src/cgmath/traits/alg/square_matrix.rs @@ -13,13 +13,15 @@ // See the License for the specific language governing permissions and // limitations under the License. +use std::num; + use traits::alg::Field; use traits::alg::Matrix; use traits::alg::VectorSpace; pub trait SquareMatrix < - S: Field, + S: Field + ApproxEq, V: VectorSpace, VVSlice, VSlice > @@ -27,8 +29,16 @@ pub trait SquareMatrix { fn transpose_self(&mut self); fn trace(&self) -> S; - fn det(&self) -> S; + fn determinant(&self) -> S; fn invert(&self) -> Option; - fn invert_self(&mut self) -> Self; - fn is_invertable(&self) -> bool; + + #[inline] + fn invert_self(&mut self) { + *self = self.invert().expect("Attempted to invert a matrix with zero determinant."); + } + + #[inline] + fn is_invertible(&self) -> bool { + !self.determinant().approx_eq(&num::zero::()) + } } diff --git a/src/cgmath/types/matrix.rs b/src/cgmath/types/matrix.rs index dd6e1b4..77b8701 100644 --- a/src/cgmath/types/matrix.rs +++ b/src/cgmath/types/matrix.rs @@ -138,8 +138,7 @@ impl Ring for Mat2; impl Ring for Mat3; impl Ring for Mat4; -impl - +impl> Matrix < S, @@ -161,8 +160,7 @@ for Mat2 } } -impl - +impl> Matrix < S, @@ -186,8 +184,7 @@ for Mat3 } } -impl - +impl> Matrix < S, @@ -213,7 +210,7 @@ for Mat4 } } -impl +impl> SquareMatrix, [S, ..2], [Vec2, ..2]> for Mat2 { @@ -228,18 +225,22 @@ for Mat2 } #[inline] - fn det(&self) -> S { + fn determinant(&self) -> S { *self.cr(0, 0) * *self.cr(1, 1) - *self.cr(1, 0) * *self.cr(0, 1) } - fn invert(&self) -> Option> { fail!() } - - fn invert_self(&mut self) -> Mat2 { fail!() } - - fn is_invertable(&self) -> bool { fail!() } + fn invert(&self) -> Option> { + let det = self.determinant(); + if det.approx_eq(&zero()) { + None + } else { + Some(Mat2::new( self.cr(1, 1) / det, -self.cr(0, 1) / det, + -self.cr(1, 0) / det, self.cr(0, 0) / det)) + } + } } -impl +impl> SquareMatrix, [S, ..3], [Vec3, ..3]> for Mat3 { @@ -255,20 +256,25 @@ for Mat3 *self.cr(0, 0) + *self.cr(1, 1) + *self.cr(2, 2) } - fn det(&self) -> S { + fn determinant(&self) -> S { *self.cr(0, 0) * (*self.cr(1, 1) * *self.cr(2, 2) - *self.cr(2, 1) * *self.cr(1, 2)) - *self.cr(1, 0) * (*self.cr(0, 1) * *self.cr(2, 2) - *self.cr(2, 1) * *self.cr(0, 2)) + *self.cr(2, 0) * (*self.cr(0, 1) * *self.cr(1, 2) - *self.cr(1, 1) * *self.cr(0, 2)) } - fn invert(&self) -> Option> { fail!() } - - fn invert_self(&mut self) -> Mat3 { fail!() } - - fn is_invertable(&self) -> bool { fail!() } + fn invert(&self) -> Option> { + let det = self.determinant(); + if det.approx_eq(&zero()) { + None + } else { + Some(Mat3::from_cols(self.c(1).cross(self.c(2)) / det, + self.c(2).cross(self.c(0)) / det, + self.c(0).cross(self.c(1)) / det).transpose()) + } + } } -impl +impl> SquareMatrix, [S, ..4], [Vec4, ..4]> for Mat4 { @@ -287,7 +293,7 @@ for Mat4 *self.cr(0, 0) + *self.cr(1, 1) + *self.cr(2, 2) + *self.cr(3, 3) } - fn det(&self) -> S { + fn determinant(&self) -> S { let m0 = Mat3::new(self.cr(1, 1).clone(), self.cr(2, 1).clone(), self.cr(3, 1).clone(), self.cr(1, 2).clone(), self.cr(2, 2).clone(), self.cr(3, 2).clone(), self.cr(1, 3).clone(), self.cr(2, 3).clone(), self.cr(3, 3).clone()); @@ -301,16 +307,54 @@ for Mat4 self.cr(0, 2).clone(), self.cr(1, 2).clone(), self.cr(2, 2).clone(), self.cr(0, 3).clone(), self.cr(1, 3).clone(), self.cr(2, 3).clone()); - self.cr(0, 0) * m0.det() - - self.cr(1, 0) * m1.det() + - self.cr(2, 0) * m2.det() - - self.cr(3, 0) * m3.det() + self.cr(0, 0) * m0.determinant() - + self.cr(1, 0) * m1.determinant() + + self.cr(2, 0) * m2.determinant() - + self.cr(3, 0) * m3.determinant() } - fn invert(&self) -> Option> { fail!() } + fn invert(&self) -> Option> { + if self.is_invertible() { + // Gauss Jordan Elimination with partial pivoting + // So take this matrix, A, augmented with the identity + // and essentially reduce [A|I] - fn invert_self(&mut self) -> Mat4 { fail!() } + let mut A = self.clone(); + let mut I = one::>(); - fn is_invertable(&self) -> bool { fail!() } + for j in range(0u, 4u) { + // Find largest element in col j + let mut i1 = j; + for i in range(j + 1, 4) { + if A.cr(j, i).abs() > A.cr(j, i1).abs() { + i1 = i; + } + } + + // Swap columns i1 and j in A and I to + // put pivot on diagonal + A.swap_c(i1, j); + I.swap_c(i1, j); + + // Scale col j to have a unit diagonal + *I.mut_c(j) = I.c(j) / *A.cr(j, j); + *A.mut_c(j) = A.c(j) / *A.cr(j, j); + + // Eliminate off-diagonal elems in col j of A, + // doing identical ops to I + for i in range(0u, 4u) { + if i != j { + let ij_mul_aij = I.c(j) * *A.cr(i, j); + let aj_mul_aij = A.c(j) * *A.cr(i, j); + *I.mut_c(i) = I.c(i) - ij_mul_aij; + *A.mut_c(i) = A.c(i) - aj_mul_aij; + } + } + } + Some(I) + } else { + None + } + } }