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 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130
use crate::storage::Storage;
use crate::{
Allocator, DefaultAllocator, Dim, OVector, One, RealField, Scalar, Unit, Vector, Zero,
};
use simba::scalar::{ClosedAddAssign, ClosedMulAssign, ClosedSubAssign};
/// # Interpolation
impl<
T: Scalar + Zero + One + ClosedAddAssign + ClosedSubAssign + ClosedMulAssign,
D: Dim,
S: Storage<T, D>,
> Vector<T, D, S>
{
/// Returns `self * (1.0 - t) + rhs * t`, i.e., the linear blend of the vectors x and y using the scalar value a.
///
/// The value for a is not restricted to the range `[0, 1]`.
///
/// # Examples:
///
/// ```
/// # use nalgebra::Vector3;
/// let x = Vector3::new(1.0, 2.0, 3.0);
/// let y = Vector3::new(10.0, 20.0, 30.0);
/// assert_eq!(x.lerp(&y, 0.1), Vector3::new(1.9, 3.8, 5.7));
/// ```
#[must_use]
pub fn lerp<S2: Storage<T, D>>(&self, rhs: &Vector<T, D, S2>, t: T) -> OVector<T, D>
where
DefaultAllocator: Allocator<D>,
{
let mut res = self.clone_owned();
res.axpy(t.clone(), rhs, T::one() - t);
res
}
/// Computes the spherical linear interpolation between two non-zero vectors.
///
/// The result is a unit vector.
///
/// # Examples:
///
/// ```
/// # use nalgebra::{Unit, Vector2};
///
/// let v1 =Vector2::new(1.0, 2.0);
/// let v2 = Vector2::new(2.0, -3.0);
///
/// let v = v1.slerp(&v2, 1.0);
///
/// assert_eq!(v, v2.normalize());
/// ```
#[must_use]
pub fn slerp<S2: Storage<T, D>>(&self, rhs: &Vector<T, D, S2>, t: T) -> OVector<T, D>
where
T: RealField,
DefaultAllocator: Allocator<D>,
{
let me = Unit::new_normalize(self.clone_owned());
let rhs = Unit::new_normalize(rhs.clone_owned());
me.slerp(&rhs, t).into_inner()
}
}
/// # Interpolation between two unit vectors
impl<T: RealField, D: Dim, S: Storage<T, D>> Unit<Vector<T, D, S>> {
/// Computes the spherical linear interpolation between two unit vectors.
///
/// # Examples:
///
/// ```
/// # use nalgebra::{Unit, Vector2};
///
/// let v1 = Unit::new_normalize(Vector2::new(1.0, 2.0));
/// let v2 = Unit::new_normalize(Vector2::new(2.0, -3.0));
///
/// let v = v1.slerp(&v2, 1.0);
///
/// assert_eq!(v, v2);
/// ```
#[must_use]
pub fn slerp<S2: Storage<T, D>>(
&self,
rhs: &Unit<Vector<T, D, S2>>,
t: T,
) -> Unit<OVector<T, D>>
where
DefaultAllocator: Allocator<D>,
{
// TODO: the result is wrong when self and rhs are collinear with opposite direction.
self.try_slerp(rhs, t, T::default_epsilon())
.unwrap_or_else(|| 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).
#[must_use]
pub fn try_slerp<S2: Storage<T, D>>(
&self,
rhs: &Unit<Vector<T, D, S2>>,
t: T,
epsilon: T,
) -> Option<Unit<OVector<T, D>>>
where
DefaultAllocator: Allocator<D>,
{
let c_hang = self.dot(rhs);
// self == other
if c_hang >= T::one() {
return Some(Unit::new_unchecked(self.clone_owned()));
}
let hang = c_hang.clone().acos();
let s_hang = (T::one() - c_hang.clone() * c_hang).sqrt();
// TODO: what if s_hang is 0.0 ? The result is not well-defined.
if relative_eq!(s_hang, T::zero(), epsilon = epsilon) {
None
} else {
let ta = ((T::one() - t.clone()) * hang.clone()).sin() / s_hang.clone();
let tb = (t * hang).sin() / s_hang;
let mut res = self.scale(ta);
res.axpy(tb, &**rhs, T::one());
Some(Unit::new_unchecked(res))
}
}
}