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

Clean up Mat3 to Quat conversion code

Browse source
This commit is contained in:
Brendan Zabarauskas 2013-09-04 15:42:12 +10:00
parent 6117fef58b
commit bb17d95abe
1 changed files with 30 additions and 32 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

62
src/cgmath/matrix.rs
View file

@ -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<S:Clone + Float> ToQuat<S> for Mat3<S> {
fn to_quat(&self) -> Quat<S> {
// 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>()) {
s = (one::<S>() + trace).sqrt();
w = half::<S>() * s;
s = half::<S>() / 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::<S>() + trace);
let w = half::<S>() * s;
let s = half::<S>() / 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::<S>() + (*self.cr(0, 0) - *self.cr(1, 1) - *self.cr(2, 2))).sqrt();
w = half::<S>() * s;
s = half::<S>() / 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::<S>() + (*self.cr(0, 0) - *self.cr(1, 1) - *self.cr(2, 2)));
let w = half::<S>() * s;
let s = half::<S>() / 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::<S>() + (*self.cr(1, 1) - *self.cr(0, 0) - *self.cr(2, 2))).sqrt();
w = half::<S>() * s;
s = half::<S>() / 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::<S>() + (*self.cr(1, 1) - *self.cr(0, 0) - *self.cr(2, 2)));
let w = half::<S>() * s;
let s = half::<S>() / 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::<S>() + (*self.cr(2, 2) - *self.cr(0, 0) - *self.cr(1, 1))).sqrt();
w = half::<S>() * s;
s = half::<S>() / 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::<S>() + (*self.cr(2, 2) - *self.cr(0, 0) - *self.cr(1, 1)));
let w = half::<S>() * s;
let s = half::<S>() / 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)
}
}