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
131
132
133
134
135
136
137
138
139
140
141
142
143
144

use get_input_index;
use num::cast;
use primitive::InterpolationPrimitive;

/// Do spherical linear interpolation.
///
/// `f(t) = sin((1 - d) * a) / sin (a) * p0 + sin(d * a) / sin (a) * p1`
/// `d = (t - t0) / (t1 - t0)`
/// `a = acos(p0 . p1)`
/// `p0 = output at left keyframe`
/// `p1 = output at right keyframe`
/// `t0 = input at left keyframe`
/// `t1 = input at right keyframe`
///
/// ## Parameters:
///
/// - `input`: the input value to the function
/// - `inputs`: list of discrete input values for each keyframe
/// - `outputs`: list of output values to interpolate between, for spherical
///              linear interpolation this should be the same size as `inputs`
/// - `normalize`: if true, normalize the interpolated value before returning it
pub fn spherical_linear_interpolate<T>(
    input: f32,
    inputs: &[f32],
    outputs: &[T],
    normalize: bool,
) -> T
where
    T: InterpolationPrimitive + Clone,
{
    let input_index = match get_input_index(input, inputs) {
        Some(index) => index,
        None => return outputs[0].clone(),
    };
    if input_index >= (inputs.len() - 1) {
        outputs[outputs.len() - 1].clone()
    } else {
        let d = (input - inputs[input_index]) / (inputs[input_index + 1] - inputs[input_index]);
        let left = &outputs[input_index];
        let mut right = outputs[input_index + 1].clone();

        let mut dot = left.dot(&right);
        if dot < 0. {
            dot = -dot;
            right = right.mul(-1.);
        }
        let dot_threshold = cast(0.9995f32).unwrap();
        let v = if dot > dot_threshold {
            left.add(&right.sub(&left).mul(d))
        } else {
            let r_dot = if dot > 1. {
                1.
            } else if dot < -1. {
                -1.
            } else {
                dot
            };

            let theta = r_dot.acos();

            let scale1 = (theta * (1. - d)).sin();
            let scale2 = (theta * d).sin();
            left.mul(scale1)
                .add(&right.mul(scale2))
                .mul(theta.sin().recip())
        };
        if normalize {
            v.normalize()
        } else {
            v
        }
    }
}

#[cfg(test)]
mod tests {
    use super::*;
    use mint::{Quaternion, Vector3};
    use std;

    #[test]
    fn test_linear_arr3() {
        let input = vec![0., 1., 2., 3., 4.];
        let output = vec![
            [0., 0., 0.],
            [1., 0., 0.],
            [0., 0., 0.],
            [-1., 0., 0.],
            [0., 0., 0.],
        ];
        assert_eq!(
            [std::f32::consts::FRAC_1_SQRT_2, 0., 0.],
            spherical_linear_interpolate(0.5, &input, &output, false)
        );
    }

    #[test]
    fn test_linear_arr4() {
        let input = vec![0., 1., 2., 3., 4.];
        let output = vec![
            [0., 0., 0., 0.],
            [1., 0., 0., 0.],
            [0., 0., 0., 0.],
            [-1., 0., 0., 0.],
            [0., 0., 0., 0.],
        ];
        assert_eq!(
            [0.99999994, 0., 0., 0.],
            spherical_linear_interpolate(0.5, &input, &output, true)
        );
    }

    #[test]
    fn test_linear_vec3() {
        let input = vec![0., 1., 2., 3., 4.];
        let output = vec![
            Vector3::from([0., 0., 0.]),
            Vector3::from([1., 0., 0.]),
            Vector3::from([0., 0., 0.]),
            Vector3::from([-1., 0., 0.]),
            Vector3::from([0., 0., 0.]),
        ];
        assert_eq!(
            Vector3::from([std::f32::consts::FRAC_1_SQRT_2, 0., 0.]),
            spherical_linear_interpolate(0.5, &input, &output, false)
        );
    }

    #[test]
    fn test_linear_quat() {
        let input = vec![0., 1., 2., 3., 4.];
        let output = vec![
            Quaternion::from([0., 0., 0., 0.]),
            Quaternion::from([1., 0., 0., 0.]),
            Quaternion::from([0., 0., 0., 0.]),
            Quaternion::from([-1., 0., 0., 0.]),
            Quaternion::from([0., 0., 0., 0.]),
        ];
        assert_eq!(
            Quaternion::from([0.99999994, 0., 0., 0.]),
            spherical_linear_interpolate(0.5, &input, &output, true)
        );
    }
}