diff --git a/src/base/matrix.rs b/src/base/matrix.rs index f3b9c044..95dda818 100644 --- a/src/base/matrix.rs +++ b/src/base/matrix.rs @@ -1616,31 +1616,31 @@ impl> Unit> { +impl> Unit> { /// Computes the spherical linear interpolation between two unit vectors. /// /// # Examples: /// /// ``` - /// # use nalgebra::geometry::UnitQuaternion; + /// # use nalgebra::{Unit, Vector2}; /// - /// 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 v1 = Unit::new_normalize(Vector2::new(1.0, 2.0)); + /// let v2 = Unit::new_normalize(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>( &self, rhs: &Unit>, - t: N::RealField, + t: N, ) -> Unit> where DefaultAllocator: Allocator, { // 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())) } @@ -1651,30 +1651,30 @@ impl> Unit> { pub fn try_slerp>( &self, rhs: &Unit>, - t: N::RealField, - epsilon: N::RealField, + t: N, + epsilon: N, ) -> Option>> where DefaultAllocator: Allocator, { - let (c_hang, c_hang_sign) = self.dotc(rhs).to_exp(); + let c_hang = self.dot(rhs); // self == other - if c_hang >= N::RealField::one() { + if c_hang >= N::one() { return Some(Unit::new_unchecked(self.clone_owned())); } 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. - if relative_eq!(s_hang, N::RealField::zero(), epsilon = epsilon) { + if relative_eq!(s_hang, N::zero(), epsilon = epsilon) { None } 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 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)) } diff --git a/src/geometry/quaternion.rs b/src/geometry/quaternion.rs index 40c085f2..a18873e5 100755 --- a/src/geometry/quaternion.rs +++ b/src/geometry/quaternion.rs @@ -1067,13 +1067,22 @@ impl UnitQuaternion { /// /// 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. + /// + /// # 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] pub fn slerp(&self, other: &Self, t: N) -> Self { - Unit::new_unchecked(Quaternion::from( - Unit::new_unchecked(self.coords) - .slerp(&Unit::new_unchecked(other.coords), t) - .into_inner(), - )) + self.try_slerp(other, t, N::default_epsilon()).expect("Quaternion slerp: ambiguous configuration.") } /// Computes the spherical linear interpolation between two unit quaternions or returns `None` @@ -1094,9 +1103,16 @@ impl UnitQuaternion { epsilon: N, ) -> Option { - Unit::new_unchecked(self.coords) - .try_slerp(&Unit::new_unchecked(other.coords), t, epsilon) - .map(|q| Unit::new_unchecked(Quaternion::from(q.into_inner()))) + 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) + .try_slerp(&Unit::new_unchecked(other.coords), t, epsilon) + }; + + + coords.map(|q| Unit::new_unchecked(Quaternion::from(q.into_inner()))) } /// Compute the conjugate of this unit quaternion in-place.