diff --git a/src/cgmath/matrix.rs b/src/cgmath/matrix.rs index 7ac8223..36d5ba3 100644 --- a/src/cgmath/matrix.rs +++ b/src/cgmath/matrix.rs @@ -15,7 +15,7 @@ //! Column major, square matrix types and traits. -use std::num::{Zero, zero, One, one}; +use std::num::{Zero, zero, One, one, sqrt}; use angle::{Rad, sin, cos}; use array::Array; @@ -576,46 +576,44 @@ impl ToQuat for Mat3 { fn to_quat(&self) -> Quat { // Implemented using a mix of ideas from jMonkeyEngine and Ken Shoemake's // paper on Quaternions: http://www.cs.ucr.edu/~vbz/resources/Quatut.pdf - - let mut s; - let w; let x; let y; let z; let trace = self.trace(); - cond! ( (trace >= zero::()) { - s = (one::() + trace).sqrt(); - w = half::() * s; - s = half::() / s; - x = (*self.cr(1, 2) - *self.cr(2, 1)) * s; - y = (*self.cr(2, 0) - *self.cr(0, 2)) * s; - z = (*self.cr(0, 1) - *self.cr(1, 0)) * s; + let s = sqrt(one::() + trace); + let w = half::() * s; + let s = half::() / s; + let x = (*self.cr(1, 2) - *self.cr(2, 1)) * s; + let y = (*self.cr(2, 0) - *self.cr(0, 2)) * s; + let z = (*self.cr(0, 1) - *self.cr(1, 0)) * s; + Quat::new(w, x, y, z) } - ((*self.cr(0, 0) > *self.cr(1, 1)) - && (*self.cr(0, 0) > *self.cr(2, 2))) { - s = (half::() + (*self.cr(0, 0) - *self.cr(1, 1) - *self.cr(2, 2))).sqrt(); - w = half::() * s; - s = half::() / s; - x = (*self.cr(0, 1) - *self.cr(1, 0)) * s; - y = (*self.cr(2, 0) - *self.cr(0, 2)) * s; - z = (*self.cr(1, 2) - *self.cr(2, 1)) * s; + ((*self.cr(0, 0) > *self.cr(1, 1)) && (*self.cr(0, 0) > *self.cr(2, 2))) { + let s = sqrt(half::() + (*self.cr(0, 0) - *self.cr(1, 1) - *self.cr(2, 2))); + let w = half::() * s; + let s = half::() / s; + let x = (*self.cr(0, 1) - *self.cr(1, 0)) * s; + let y = (*self.cr(2, 0) - *self.cr(0, 2)) * s; + let z = (*self.cr(1, 2) - *self.cr(2, 1)) * s; + Quat::new(w, x, y, z) } (*self.cr(1, 1) > *self.cr(2, 2)) { - s = (half::() + (*self.cr(1, 1) - *self.cr(0, 0) - *self.cr(2, 2))).sqrt(); - w = half::() * s; - s = half::() / s; - x = (*self.cr(0, 1) - *self.cr(1, 0)) * s; - y = (*self.cr(1, 2) - *self.cr(2, 1)) * s; - z = (*self.cr(2, 0) - *self.cr(0, 2)) * s; + let s = sqrt(half::() + (*self.cr(1, 1) - *self.cr(0, 0) - *self.cr(2, 2))); + let w = half::() * s; + let s = half::() / s; + let x = (*self.cr(0, 1) - *self.cr(1, 0)) * s; + let y = (*self.cr(1, 2) - *self.cr(2, 1)) * s; + let z = (*self.cr(2, 0) - *self.cr(0, 2)) * s; + Quat::new(w, x, y, z) } _ { - s = (half::() + (*self.cr(2, 2) - *self.cr(0, 0) - *self.cr(1, 1))).sqrt(); - w = half::() * s; - s = half::() / s; - x = (*self.cr(2, 0) - *self.cr(0, 2)) * s; - y = (*self.cr(1, 2) - *self.cr(2, 1)) * s; - z = (*self.cr(0, 1) - *self.cr(1, 0)) * s; + let s = sqrt(half::() + (*self.cr(2, 2) - *self.cr(0, 0) - *self.cr(1, 1))); + let w = half::() * s; + let s = half::() / s; + let x = (*self.cr(2, 0) - *self.cr(0, 2)) * s; + let y = (*self.cr(1, 2) - *self.cr(2, 1)) * s; + let z = (*self.cr(0, 1) - *self.cr(1, 0)) * s; + Quat::new(w, x, y, z) } ) - Quat::new(w, x, y, z) } }