M-Labs/nalgebra
3
0
Fork 1
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Add slerp for unit vectors.

Browse Source
This commit is contained in:
sebcrozet 2018-09-22 15:38:51 +02:00 committed by Sébastien Crozet
parent a03fd6bff7
commit 832bf42b56
2 changed files with 43 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
src/base/matrix.rs
View File

@ -1247,6 +1247,45 @@ impl<N: Real, R: Dim, C: Dim, S: Storage<N, R, C>> Matrix<N, R, C, S> {
}
}
impl<N: Real, D: Dim, S: Storage<N, D>> Unit<Vector<N, D, S>> {
/// Computes the spherical linear interpolation between two unit vectors.
pub fn slerp<S2: Storage<N, D>>(&self, rhs: &Unit<Vector<N, D, S2>>, t: N) -> Unit<VectorN<N, D>>
where
DefaultAllocator: Allocator<N, D> {
// FIXME: the result is wrong when self and rhs are collinear with opposite direction.
self.try_slerp(rhs, t, N::default_epsilon()).unwrap_or(Unit::new_unchecked(self.clone_owned()))
}
/// Computes the spherical linear interpolation between two unit vectors.
///
/// Returns `None` if the two vectors are almost collinear and with opposite direction
/// (in this case, there is an infinity of possible results).
pub fn try_slerp<S2: Storage<N, D>>(&self, rhs: &Unit<Vector<N, D, S2>>, t: N, epsilon: N) -> Option<Unit<VectorN<N, D>>>
where
DefaultAllocator: Allocator<N, D> {
let c_hang = self.dot(rhs);
// self == other
if c_hang.abs() >= N::one() {
return Some(Unit::new_unchecked(self.clone_owned()));
}
let hang = c_hang.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.
if relative_eq!(s_hang, N::zero(), epsilon = epsilon) {
None
} else {
let ta = ((N::one() - t) * hang).sin() / s_hang;
let tb = (t * hang).sin() / s_hang;
let res = &**self * ta + &**rhs * tb;
Some(Unit::new_unchecked(res))
}
}
}
impl<N: Real, R: Dim, C: Dim, S: StorageMut<N, R, C>> Matrix<N, R, C, S> {
/// Normalizes this matrix in-place and returns its norm.
#[inline]

27
src/geometry/quaternion.rs
View File

@ -451,9 +451,8 @@ impl<N: Real> UnitQuaternion<N> {
/// is not well-defined).
#[inline]
pub fn slerp(&self, other: &UnitQuaternion<N>, t: N) -> UnitQuaternion<N> {
self.try_slerp(other, t, N::zero()).expect(
"Unable to perform a spherical quaternion interpolation when they \
are 180 degree apart (the result is not unique).",
Unit::new_unchecked(
Quaternion::from_vector(Unit::new_unchecked(self.coords).slerp(&Unit::new_unchecked(other.coords), t).unwrap())
)
}
@ -474,26 +473,8 @@ impl<N: Real> UnitQuaternion<N> {
t: N,
epsilon: N,
) -> Option<UnitQuaternion<N>> {
let c_hang = self.coords.dot(&other.coords);
// self == other
if c_hang.abs() >= N::one() {
return Some(*self);
}
let hang = c_hang.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.
if relative_eq!(s_hang, N::zero(), epsilon = epsilon) {
None
} else {
let ta = ((N::one() - t) * hang).sin() / s_hang;
let tb = (t * hang).sin() / s_hang;
let res = self.as_ref() * ta + other.as_ref() * tb;
Some(UnitQuaternion::new_unchecked(res))
}
Unit::new_unchecked(self.coords).try_slerp(&Unit::new_unchecked(other.coords), t, epsilon)
.map(|q| Unit::new_unchecked(Quaternion::from_vector(q.unwrap())))
}
/// Compute the conjugate of this unit quaternion in-place.