Move Quaternion::{slerp, to_euler} out of separate impl block

This commit is contained in:
Brendan Zabarauskas 2016-04-16 14:32:28 +10:00
parent 7b04a30104
commit e9671e6070

View file

@ -72,6 +72,87 @@ impl<S: BaseFloat> Quaternion<S> {
pub fn nlerp(self, other: Quaternion<S>, amount: S) -> Quaternion<S> { pub fn nlerp(self, other: Quaternion<S>, amount: S) -> Quaternion<S> {
(self * (S::one() - amount) + other * amount).normalize() (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<S>, amount: S) -> Quaternion<S> {
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<S>, Rad<S>, Rad<S>) {
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<S: BaseFloat> VectorSpace for Quaternion<S> { impl<S: BaseFloat> VectorSpace for Quaternion<S> {
@ -194,89 +275,6 @@ impl<S: BaseFloat> ApproxEq for Quaternion<S> {
} }
} }
impl<S: BaseFloat> Quaternion<S> {
/// 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<S>, amount: S) -> Quaternion<S> {
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<S>, Rad<S>, Rad<S>) {
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<S: BaseFloat> From<Quaternion<S>> for Matrix3<S> { impl<S: BaseFloat> From<Quaternion<S>> for Matrix3<S> {
/// Convert the quaternion to a 3 x 3 rotation matrix /// Convert the quaternion to a 3 x 3 rotation matrix
fn from(quat: Quaternion<S>) -> Matrix3<S> { fn from(quat: Quaternion<S>) -> Matrix3<S> {