nalgebra/base/
interpolation.rs

1use crate::storage::Storage;
2use crate::{
3    Allocator, DefaultAllocator, Dim, OVector, One, RealField, Scalar, Unit, Vector, Zero,
4};
5use simba::scalar::{ClosedAddAssign, ClosedMulAssign, ClosedSubAssign};
6
7/// # Interpolation
8impl<
9        T: Scalar + Zero + One + ClosedAddAssign + ClosedSubAssign + ClosedMulAssign,
10        D: Dim,
11        S: Storage<T, D>,
12    > Vector<T, D, S>
13{
14    /// Returns `self * (1.0 - t) + rhs * t`, i.e., the linear blend of the vectors x and y using the scalar value a.
15    ///
16    /// The value for a is not restricted to the range `[0, 1]`.
17    ///
18    /// # Examples:
19    ///
20    /// ```
21    /// # use nalgebra::Vector3;
22    /// let x = Vector3::new(1.0, 2.0, 3.0);
23    /// let y = Vector3::new(10.0, 20.0, 30.0);
24    /// assert_eq!(x.lerp(&y, 0.1), Vector3::new(1.9, 3.8, 5.7));
25    /// ```
26    #[must_use]
27    pub fn lerp<S2: Storage<T, D>>(&self, rhs: &Vector<T, D, S2>, t: T) -> OVector<T, D>
28    where
29        DefaultAllocator: Allocator<D>,
30    {
31        let mut res = self.clone_owned();
32        res.axpy(t.clone(), rhs, T::one() - t);
33        res
34    }
35
36    /// Computes the spherical linear interpolation between two non-zero vectors.
37    ///
38    /// The result is a unit vector.
39    ///
40    /// # Examples:
41    ///
42    /// ```
43    /// # use nalgebra::{Unit, Vector2};
44    ///
45    /// let v1 =Vector2::new(1.0, 2.0);
46    /// let v2 = Vector2::new(2.0, -3.0);
47    ///
48    /// let v = v1.slerp(&v2, 1.0);
49    ///
50    /// assert_eq!(v, v2.normalize());
51    /// ```
52    #[must_use]
53    pub fn slerp<S2: Storage<T, D>>(&self, rhs: &Vector<T, D, S2>, t: T) -> OVector<T, D>
54    where
55        T: RealField,
56        DefaultAllocator: Allocator<D>,
57    {
58        let me = Unit::new_normalize(self.clone_owned());
59        let rhs = Unit::new_normalize(rhs.clone_owned());
60        me.slerp(&rhs, t).into_inner()
61    }
62}
63
64/// # Interpolation between two unit vectors
65impl<T: RealField, D: Dim, S: Storage<T, D>> Unit<Vector<T, D, S>> {
66    /// Computes the spherical linear interpolation between two unit vectors.
67    ///
68    /// # Examples:
69    ///
70    /// ```
71    /// # use nalgebra::{Unit, Vector2};
72    ///
73    /// let v1 = Unit::new_normalize(Vector2::new(1.0, 2.0));
74    /// let v2 = Unit::new_normalize(Vector2::new(2.0, -3.0));
75    ///
76    /// let v = v1.slerp(&v2, 1.0);
77    ///
78    /// assert_eq!(v, v2);
79    /// ```
80    #[must_use]
81    pub fn slerp<S2: Storage<T, D>>(
82        &self,
83        rhs: &Unit<Vector<T, D, S2>>,
84        t: T,
85    ) -> Unit<OVector<T, D>>
86    where
87        DefaultAllocator: Allocator<D>,
88    {
89        // TODO: the result is wrong when self and rhs are collinear with opposite direction.
90        self.try_slerp(rhs, t, T::default_epsilon())
91            .unwrap_or_else(|| Unit::new_unchecked(self.clone_owned()))
92    }
93
94    /// Computes the spherical linear interpolation between two unit vectors.
95    ///
96    /// Returns `None` if the two vectors are almost collinear and with opposite direction
97    /// (in this case, there is an infinity of possible results).
98    #[must_use]
99    pub fn try_slerp<S2: Storage<T, D>>(
100        &self,
101        rhs: &Unit<Vector<T, D, S2>>,
102        t: T,
103        epsilon: T,
104    ) -> Option<Unit<OVector<T, D>>>
105    where
106        DefaultAllocator: Allocator<D>,
107    {
108        let c_hang = self.dot(rhs);
109
110        // self == other
111        if c_hang >= T::one() {
112            return Some(Unit::new_unchecked(self.clone_owned()));
113        }
114
115        let hang = c_hang.clone().acos();
116        let s_hang = (T::one() - c_hang.clone() * c_hang).sqrt();
117
118        // TODO: what if s_hang is 0.0 ? The result is not well-defined.
119        if relative_eq!(s_hang, T::zero(), epsilon = epsilon) {
120            None
121        } else {
122            let ta = ((T::one() - t.clone()) * hang.clone()).sin() / s_hang.clone();
123            let tb = (t * hang).sin() / s_hang;
124            let mut res = self.scale(ta);
125            res.axpy(tb, &**rhs, T::one());
126
127            Some(Unit::new_unchecked(res))
128        }
129    }
130}
nalgebra/base/interpolation.rs

nalgebra/base/
interpolation.rs
