Fix slerp for regular vectors.

This commit is contained in:
sebcrozet 2019-12-23 23:27:20 +01:00
parent 1d746a02b7
commit e976ed675f
2 changed files with 40 additions and 24 deletions

View File

@ -1616,31 +1616,31 @@ impl<N: Scalar + Copy + Zero + One + ClosedAdd + ClosedSub + ClosedMul, D: Dim,
} }
} }
impl<N: ComplexField, D: Dim, S: Storage<N, D>> Unit<Vector<N, D, S>> { impl<N: RealField, D: Dim, S: Storage<N, D>> Unit<Vector<N, D, S>> {
/// Computes the spherical linear interpolation between two unit vectors. /// Computes the spherical linear interpolation between two unit vectors.
/// ///
/// # Examples: /// # Examples:
/// ///
/// ``` /// ```
/// # use nalgebra::geometry::UnitQuaternion; /// # use nalgebra::Vector2;
/// ///
/// let q1 = UnitQuaternion::from_euler_angles(std::f32::consts::FRAC_PI_4, 0.0, 0.0); /// let v1 = Vector2::new(1.0, 2.0);
/// let q2 = UnitQuaternion::from_euler_angles(-std::f32::consts::PI, 0.0, 0.0); /// let v2 = Vector2::new(2.0, -3.0);
/// ///
/// let q = q1.slerp(&q2, 1.0 / 3.0); /// let v = v1.slerp(&v2, 1.0);
/// ///
/// assert_eq!(q.euler_angles(), (std::f32::consts::FRAC_PI_2, 0.0, 0.0)); /// assert_eq!(v, v2);
/// ``` /// ```
pub fn slerp<S2: Storage<N, D>>( pub fn slerp<S2: Storage<N, D>>(
&self, &self,
rhs: &Unit<Vector<N, D, S2>>, rhs: &Unit<Vector<N, D, S2>>,
t: N::RealField, t: N,
) -> Unit<VectorN<N, D>> ) -> Unit<VectorN<N, D>>
where where
DefaultAllocator: Allocator<N, D>, DefaultAllocator: Allocator<N, D>,
{ {
// FIXME: the result is wrong when self and rhs are collinear with opposite direction. // FIXME: the result is wrong when self and rhs are collinear with opposite direction.
self.try_slerp(rhs, t, N::RealField::default_epsilon()) self.try_slerp(rhs, t, N::default_epsilon())
.unwrap_or(Unit::new_unchecked(self.clone_owned())) .unwrap_or(Unit::new_unchecked(self.clone_owned()))
} }
@ -1651,30 +1651,30 @@ impl<N: ComplexField, D: Dim, S: Storage<N, D>> Unit<Vector<N, D, S>> {
pub fn try_slerp<S2: Storage<N, D>>( pub fn try_slerp<S2: Storage<N, D>>(
&self, &self,
rhs: &Unit<Vector<N, D, S2>>, rhs: &Unit<Vector<N, D, S2>>,
t: N::RealField, t: N,
epsilon: N::RealField, epsilon: N,
) -> Option<Unit<VectorN<N, D>>> ) -> Option<Unit<VectorN<N, D>>>
where where
DefaultAllocator: Allocator<N, D>, DefaultAllocator: Allocator<N, D>,
{ {
let (c_hang, c_hang_sign) = self.dotc(rhs).to_exp(); let c_hang = self.dot(rhs);
// self == other // self == other
if c_hang >= N::RealField::one() { if c_hang >= N::one() {
return Some(Unit::new_unchecked(self.clone_owned())); return Some(Unit::new_unchecked(self.clone_owned()));
} }
let hang = c_hang.acos(); let hang = c_hang.acos();
let s_hang = (N::RealField::one() - c_hang * c_hang).sqrt(); let s_hang = (N::one() - c_hang * c_hang).sqrt();
// FIXME: what if s_hang is 0.0 ? The result is not well-defined. // FIXME: what if s_hang is 0.0 ? The result is not well-defined.
if relative_eq!(s_hang, N::RealField::zero(), epsilon = epsilon) { if relative_eq!(s_hang, N::zero(), epsilon = epsilon) {
None None
} else { } else {
let ta = ((N::RealField::one() - t) * hang).sin() / s_hang; let ta = ((N::one() - t) * hang).sin() / s_hang;
let tb = (t * hang).sin() / s_hang; let tb = (t * hang).sin() / s_hang;
let mut res = self.scale(ta); let mut res = self.scale(ta);
res.axpy(c_hang_sign.scale(tb), &**rhs, N::one()); res.axpy(tb, &**rhs, N::one());
Some(Unit::new_unchecked(res)) Some(Unit::new_unchecked(res))
} }

View File

@ -1067,13 +1067,22 @@ impl<N: RealField> UnitQuaternion<N> {
/// ///
/// Panics if the angle between both quaternion is 180 degrees (in which case the interpolation /// Panics if the angle between both quaternion is 180 degrees (in which case the interpolation
/// is not well-defined). Use `.try_slerp` instead to avoid the panic. /// is not well-defined). Use `.try_slerp` instead to avoid the panic.
///
/// # 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));
/// ```
#[inline] #[inline]
pub fn slerp(&self, other: &Self, t: N) -> Self { pub fn slerp(&self, other: &Self, t: N) -> Self {
Unit::new_unchecked(Quaternion::from( self.try_slerp(other, t, N::default_epsilon()).expect("Quaternion slerp: ambiguous configuration.")
Unit::new_unchecked(self.coords)
.slerp(&Unit::new_unchecked(other.coords), t)
.into_inner(),
))
} }
/// Computes the spherical linear interpolation between two unit quaternions or returns `None` /// Computes the spherical linear interpolation between two unit quaternions or returns `None`
@ -1094,9 +1103,16 @@ impl<N: RealField> UnitQuaternion<N> {
epsilon: N, epsilon: N,
) -> Option<Self> ) -> Option<Self>
{ {
let coords = if self.coords.dot(&other.coords) < N::zero() {
Unit::new_unchecked(self.coords)
.try_slerp(&Unit::new_unchecked(-other.coords), t, epsilon)
} else {
Unit::new_unchecked(self.coords) Unit::new_unchecked(self.coords)
.try_slerp(&Unit::new_unchecked(other.coords), t, epsilon) .try_slerp(&Unit::new_unchecked(other.coords), t, epsilon)
.map(|q| Unit::new_unchecked(Quaternion::from(q.into_inner()))) };
coords.map(|q| Unit::new_unchecked(Quaternion::from(q.into_inner())))
} }
/// Compute the conjugate of this unit quaternion in-place. /// Compute the conjugate of this unit quaternion in-place.