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

use core::ops::{Mul, Sub};

use cast_trait::Cast;
use num_traits::{FromPrimitive, One, Zero};

use vec2;

#[inline]
pub fn quadratic<'out, T, F>(
    out: &'out mut [T; 2],
    p0: &[T; 2],
    p1: &[T; 2],
    p2: &[T; 2],
    p3: &[T; 2],
    t: &F,
) -> &'out mut [T; 2]
where
    T: Clone + Cast<F>,
    F: Clone
        + One
        + Zero
        + FromPrimitive
        + PartialOrd
        + Cast<T>
        + Sub<F, Output = F>
        + Mul<F, Output = F>,
    for<'a, 'b> &'a F: Sub<&'b F, Output = F> + Mul<&'b F, Output = F>,
{
    if t <= &F::zero() {
        *out = p0.clone();
        out
    } else if t >= &F::one() {
        *out = p3.clone();
        out
    } else {
        let three = F::from_usize(3).unwrap();
        let one_min_t = &F::one() - t;
        let one_min_t_sq = &one_min_t * &one_min_t;
        let one_min_t_cb = &one_min_t_sq * &one_min_t;
        let t_sq = t * t;
        let t_cb = &t_sq * t;

        let p0x = p0[0].clone().cast();
        let p0y = p0[1].clone().cast();
        let p1x = p1[0].clone().cast();
        let p1y = p1[1].clone().cast();
        let p2x = p2[0].clone().cast();
        let p2y = p2[1].clone().cast();
        let p3x = p3[0].clone().cast();
        let p3y = p3[1].clone().cast();

        vec2::set(
            out,
            (one_min_t_cb.clone() * p0x
                + three.clone() * one_min_t_sq.clone() * t.clone() * p1x
                + three.clone() * one_min_t.clone() * t_sq.clone() * p2x
                + t_cb.clone() * p3x)
                .cast(),
            (one_min_t_cb * p0y
                + three.clone() * one_min_t_sq * t.clone() * p1y
                + three * one_min_t * t_sq * p2y
                + t_cb * p3y)
                .cast(),
        )
    }
}
#[test]
fn test_quadratic() {
    assert_eq!(
        quadratic(
            &mut [0, 0],
            &[0, 0],
            &[0, 200],
            &[200, 200],
            &[200, 0],
            &0.25
        ),
        &[31, 112]
    );
    assert_eq!(
        quadratic(
            &mut [0, 0],
            &[0, 0],
            &[0, 200],
            &[200, 200],
            &[200, 0],
            &0.5
        ),
        &[100, 150]
    );
    assert_eq!(
        quadratic(
            &mut [0, 0],
            &[0, 0],
            &[0, 200],
            &[200, 200],
            &[200, 0],
            &0.75
        ),
        &[168, 112]
    );
}