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))
        }
    }
}