Fix matrix slerp function (#568)

* Fix matrix slerp function

* Adding slerp doc test
This commit is contained in:
Samuel Hurel 2019-03-24 21:44:49 +01:00 committed by Sébastien Crozet
parent d6b5c7f8f7
commit b4d800f3e2
1 changed files with 15 additions and 2 deletions

View File

@ -1480,6 +1480,19 @@ impl<N: Scalar + Zero + One + ClosedAdd + ClosedSub + ClosedMul, D: Dim, S: Stor
impl<N: Real, D: Dim, S: Storage<N, D>> Unit<Vector<N, D, S>> {
/// Computes the spherical linear interpolation between two unit vectors.
///
/// # Examples:
///
/// ```
/// # use nalgebra::geometry::UnitQuaternion;
///
/// let q1 = UnitQuaternion::from_euler_angles(std::f32::consts::FRAC_PI_4, 0.0, 0.0);
/// let q2 = UnitQuaternion::from_euler_angles(-std::f32::consts::PI, 0.0, 0.0);
///
/// let q = q1.slerp(&q2, 1.0 / 3.0);
///
/// assert_eq!(q.euler_angles(), (std::f32::consts::FRAC_PI_2, 0.0, 0.0));
/// ```
pub fn slerp<S2: Storage<N, D>>(
&self,
rhs: &Unit<Vector<N, D, S2>>,
@ -1513,7 +1526,7 @@ impl<N: Real, D: Dim, S: Storage<N, D>> Unit<Vector<N, D, S>> {
return Some(Unit::new_unchecked(self.clone_owned()));
}
let hang = c_hang.acos();
let hang = c_hang.abs().acos();
let s_hang = (N::one() - c_hang * c_hang).sqrt();
// FIXME: what if s_hang is 0.0 ? The result is not well-defined.
@ -1522,7 +1535,7 @@ impl<N: Real, D: Dim, S: Storage<N, D>> Unit<Vector<N, D, S>> {
} else {
let ta = ((N::one() - t) * hang).sin() / s_hang;
let tb = (t * hang).sin() / s_hang;
let res = &**self * ta + &**rhs * tb;
let res = &**self * ta + &**rhs * tb * c_hang.signum();
Some(Unit::new_unchecked(res))
}