hodasemi/cgmath
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity

Merge pull request #85 from csherratt/fix_matrix

Browse source 
Fix inverse matrix4
This commit is contained in:
Corey Richardson 2014-06-05 13:07:52 -07:00
commit b86166cfd4
3 changed files with 64 additions and 36 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

64
src/cgmath/matrix.rs
View file

@ -913,44 +913,36 @@ impl<S: BaseFloat> Matrix<S, Vector4<S>> for Matrix4<S> {
}
fn invert(&self) -> Option<Matrix4<S>> {
if self.is_invertible() {
// Gauss Jordan Elimination with partial pivoting
// So take this matrix ('mat') augmented with the identity ('ident'),
// and essentially reduce [mat|ident]
let det = self.determinant();
if !det.approx_eq(&zero()) {
let one: S = one();
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();
let mut ident = Matrix4::identity();
Some(Matrix4::new(cf(0, 0), cf(0, 1), cf(0, 2), cf(0, 3),
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
}

12
src/cgmath/vector.rs
View file

@ -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

24
src/test/matrix.rs
View file

@ -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]