From e9671e6070b7d4c5350e6f840e1ddfadbd1e16dd Mon Sep 17 00:00:00 2001 From: Brendan Zabarauskas Date: Sat, 16 Apr 2016 14:32:28 +1000 Subject: [PATCH] Move Quaternion::{slerp, to_euler} out of separate impl block --- src/quaternion.rs | 164 +++++++++++++++++++++++----------------------- 1 file changed, 81 insertions(+), 83 deletions(-) diff --git a/src/quaternion.rs b/src/quaternion.rs index 5bf47d8..16dd56a 100644 --- a/src/quaternion.rs +++ b/src/quaternion.rs @@ -72,6 +72,87 @@ impl Quaternion { pub fn nlerp(self, other: Quaternion, amount: S) -> Quaternion { (self * (S::one() - amount) + other * amount).normalize() } + + /// Spherical Linear Intoperlation + /// + /// Return the spherical linear interpolation between the quaternion and + /// `other`. Both quaternions should be normalized first. + /// + /// # Performance notes + /// + /// The `acos` operation used in `slerp` is an expensive operation, so + /// unless your quarternions are far away from each other it's generally + /// more advisable to use `nlerp` when you know your rotations are going + /// to be small. + /// + /// - [Understanding Slerp, Then Not Using It] + /// (http://number-none.com/product/Understanding%20Slerp,%20Then%20Not%20Using%20It/) + /// - [Arcsynthesis OpenGL tutorial] + /// (http://www.arcsynthesis.org/gltut/Positioning/Tut08%20Interpolation.html) + pub fn slerp(self, other: Quaternion, amount: S) -> Quaternion { + let dot = self.dot(other); + let dot_threshold = cast(0.9995f64).unwrap(); + + // if quaternions are close together use `nlerp` + if dot > dot_threshold { + self.nlerp(other, amount) + } else { + // stay within the domain of acos() + // TODO REMOVE WHEN https://github.com/mozilla/rust/issues/12068 IS RESOLVED + let robust_dot = if dot > S::one() { + S::one() + } else if dot < -S::one() { + -S::one() + } else { + dot + }; + + let theta = Rad::acos(robust_dot.clone()); + + let scale1 = Rad::sin(theta * (S::one() - amount)); + let scale2 = Rad::sin(theta * amount); + + (self * scale1 + other * scale2) * Rad::sin(theta).recip() + } + } + + /// Convert a Quaternion to Eular angles + /// This is a polar singularity aware conversion + /// + /// Based on: + /// - [Maths - Conversion Quaternion to Euler] + /// (http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/) + pub fn to_euler(self) -> (Rad, Rad, Rad) { + let sig: S = cast(0.499f64).unwrap(); + let two: S = cast(2f64).unwrap(); + let one: S = cast(1f64).unwrap(); + + let (qw, qx, qy, qz) = (self.s, self.v.x, self.v.y, self.v.z); + let (sqw, sqx, sqy, sqz) = (qw * qw, qx * qx, qy * qy, qz * qz); + + let unit = sqx + sqy + sqz + sqw; + let test = qx * qy + qz * qw; + + if test > sig * unit { + ( + Rad::zero(), + Rad::turn_div_4(), + Rad::atan2(qx, qw) * two, + ) + } else if test < -sig * unit { + ( + Rad::zero(), + -Rad::turn_div_4(), + Rad::atan2(qx, qw) * two, + ) + } else { + ( + Rad::atan2(two * (qy * qw - qx * qz), one - two * (sqy + sqz)), + Rad::asin(two * (qx * qy + qz * qw)), + Rad::atan2(two * (qx * qw - qy * qz), one - two * (sqx + sqz)), + ) + } + } } impl VectorSpace for Quaternion { @@ -194,89 +275,6 @@ impl ApproxEq for Quaternion { } } -impl Quaternion { - /// Spherical Linear Intoperlation - /// - /// Return the spherical linear interpolation between the quaternion and - /// `other`. Both quaternions should be normalized first. - /// - /// # Performance notes - /// - /// The `acos` operation used in `slerp` is an expensive operation, so - /// unless your quarternions are far away from each other it's generally - /// more advisable to use `nlerp` when you know your rotations are going - /// to be small. - /// - /// - [Understanding Slerp, Then Not Using It] - /// (http://number-none.com/product/Understanding%20Slerp,%20Then%20Not%20Using%20It/) - /// - [Arcsynthesis OpenGL tutorial] - /// (http://www.arcsynthesis.org/gltut/Positioning/Tut08%20Interpolation.html) - pub fn slerp(self, other: Quaternion, amount: S) -> Quaternion { - let dot = self.dot(other); - let dot_threshold = cast(0.9995f64).unwrap(); - - // if quaternions are close together use `nlerp` - if dot > dot_threshold { - self.nlerp(other, amount) - } else { - // stay within the domain of acos() - // TODO REMOVE WHEN https://github.com/mozilla/rust/issues/12068 IS RESOLVED - let robust_dot = if dot > S::one() { - S::one() - } else if dot < -S::one() { - -S::one() - } else { - dot - }; - - let theta = Rad::acos(robust_dot.clone()); - - let scale1 = Rad::sin(theta * (S::one() - amount)); - let scale2 = Rad::sin(theta * amount); - - (self * scale1 + other * scale2) * Rad::sin(theta).recip() - } - } - - /// Convert a Quaternion to Eular angles - /// This is a polar singularity aware conversion - /// - /// Based on: - /// - [Maths - Conversion Quaternion to Euler] - /// (http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/) - pub fn to_euler(self) -> (Rad, Rad, Rad) { - let sig: S = cast(0.499f64).unwrap(); - let two: S = cast(2f64).unwrap(); - let one: S = cast(1f64).unwrap(); - - let (qw, qx, qy, qz) = (self.s, self.v.x, self.v.y, self.v.z); - let (sqw, sqx, sqy, sqz) = (qw * qw, qx * qx, qy * qy, qz * qz); - - let unit = sqx + sqy + sqz + sqw; - let test = qx * qy + qz * qw; - - if test > sig * unit { - ( - Rad::zero(), - Rad::turn_div_4(), - Rad::atan2(qx, qw) * two, - ) - } else if test < -sig * unit { - ( - Rad::zero(), - -Rad::turn_div_4(), - Rad::atan2(qx, qw) * two, - ) - } else { - ( - Rad::atan2(two * (qy * qw - qx * qz), one - two * (sqy + sqz)), - Rad::asin(two * (qx * qy + qz * qw)), - Rad::atan2(two * (qx * qw - qy * qz), one - two * (sqx + sqz)), - ) - } - } -} - impl From> for Matrix3 { /// Convert the quaternion to a 3 x 3 rotation matrix fn from(quat: Quaternion) -> Matrix3 {