1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81
use crate::{RealField, Rotation2, Rotation3, SimdRealField, UnitComplex, UnitQuaternion};
/// # Interpolation
impl<T: SimdRealField> Rotation2<T> {
/// Spherical linear interpolation between two rotation matrices.
///
/// # Examples:
///
/// ```
/// # #[macro_use] extern crate approx;
/// # use nalgebra::geometry::Rotation2;
///
/// let rot1 = Rotation2::new(std::f32::consts::FRAC_PI_4);
/// let rot2 = Rotation2::new(-std::f32::consts::PI);
///
/// let rot = rot1.slerp(&rot2, 1.0 / 3.0);
///
/// assert_relative_eq!(rot.angle(), std::f32::consts::FRAC_PI_2);
/// ```
#[inline]
#[must_use]
pub fn slerp(&self, other: &Self, t: T) -> Self
where
T::Element: SimdRealField,
{
let c1 = UnitComplex::from(self.clone());
let c2 = UnitComplex::from(other.clone());
c1.slerp(&c2, t).into()
}
}
impl<T: SimdRealField> Rotation3<T> {
/// Spherical linear interpolation between two rotation matrices.
///
/// Panics if the angle between both rotations is 180 degrees (in which case the interpolation
/// is not well-defined). Use `.try_slerp` instead to avoid the panic.
///
/// # Examples:
///
/// ```
/// # use nalgebra::geometry::Rotation3;
///
/// let q1 = Rotation3::from_euler_angles(std::f32::consts::FRAC_PI_4, 0.0, 0.0);
/// let q2 = Rotation3::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));
/// ```
#[inline]
#[must_use]
pub fn slerp(&self, other: &Self, t: T) -> Self
where
T: RealField,
{
let q1 = UnitQuaternion::from(self.clone());
let q2 = UnitQuaternion::from(other.clone());
q1.slerp(&q2, t).into()
}
/// Computes the spherical linear interpolation between two rotation matrices or returns `None`
/// if both rotations are approximately 180 degrees apart (in which case the interpolation is
/// not well-defined).
///
/// # Arguments
/// * `self`: the first rotation to interpolate from.
/// * `other`: the second rotation to interpolate toward.
/// * `t`: the interpolation parameter. Should be between 0 and 1.
/// * `epsilon`: the value below which the sinus of the angle separating both rotations
/// must be to return `None`.
#[inline]
#[must_use]
pub fn try_slerp(&self, other: &Self, t: T, epsilon: T) -> Option<Self>
where
T: RealField,
{
let q1 = UnitQuaternion::from(self.clone());
let q2 = UnitQuaternion::from(other.clone());
q1.try_slerp(&q2, t, epsilon).map(|q| q.into())
}
}