Clean up Mat3 to Quat conversion code

This commit is contained in:
Brendan Zabarauskas 2013-09-04 15:42:12 +10:00
parent 6117fef58b
commit bb17d95abe

View file

@ -15,7 +15,7 @@
//! Column major, square matrix types and traits. //! 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 angle::{Rad, sin, cos};
use array::Array; use array::Array;
@ -576,46 +576,44 @@ impl<S:Clone + Float> ToQuat<S> for Mat3<S> {
fn to_quat(&self) -> Quat<S> { fn to_quat(&self) -> Quat<S> {
// Implemented using a mix of ideas from jMonkeyEngine and Ken Shoemake'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 // 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(); let trace = self.trace();
cond! ( cond! (
(trace >= zero::<S>()) { (trace >= zero::<S>()) {
s = (one::<S>() + trace).sqrt(); let s = sqrt(one::<S>() + trace);
w = half::<S>() * s; let w = half::<S>() * s;
s = half::<S>() / s; let s = half::<S>() / s;
x = (*self.cr(1, 2) - *self.cr(2, 1)) * s; let x = (*self.cr(1, 2) - *self.cr(2, 1)) * s;
y = (*self.cr(2, 0) - *self.cr(0, 2)) * s; let y = (*self.cr(2, 0) - *self.cr(0, 2)) * s;
z = (*self.cr(0, 1) - *self.cr(1, 0)) * s; let z = (*self.cr(0, 1) - *self.cr(1, 0)) * s;
}
((*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(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;
}
_ {
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;
}
)
Quat::new(w, x, y, z) Quat::new(w, x, y, z)
} }
((*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)) {
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)
}
_ {
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)
}
)
}
} }