Move Quaternion::{slerp, to_euler} out of separate impl block
This commit is contained in:
Brendan Zabarauskas
parent
7b04a30104
commit
e9671e6070
1 changed files with 81 additions and 83 deletions
|
|
@ -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> {
|
||||||
|
|
|
||||||
Loading…
Reference in a new issue