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

use number_traits::{Cast, Signed, Sqrt, Trig};
use create::clone;
use length::normalize;


pub use vec4::inverse;
pub use vec4::lerp;
pub use vec4::min;
pub use vec4::max;
pub use vec4::clamp;
pub use vec4::eq;
pub use vec4::ne;


#[inline]
pub fn conjugate<'a, 'b, T: Copy + Signed>(out: &'a mut [T; 4], a: &'b [T; 4]) -> &'a mut [T; 4] {
    out[0] = -a[0];
    out[1] = -a[1];
    out[2] = -a[2];
    out[3] = a[3];
    out
}
#[test]
fn test_conjugate() {
    let mut v = [0, 0, 0, 0];
    conjugate(&mut v, &[1, 1, 1, 1]);
    assert!(v[0] == -1);
    assert!(v[1] == -1);
    assert!(v[2] == -1);
    assert!(v[3] == 1);
}

#[inline]
pub fn calculate_w<'a, 'b, T: Copy + Signed + Sqrt>(out: &'a mut [T; 4], a: &'b [T; 4]) -> &'a mut [T; 4] {
    out[0] = a[0];
    out[1] = a[1];
    out[2] = a[2];
    out[3] = (T::one() - a[0] * a[0] - a[1] * a[1] - a[2] * a[2]).abs().sqrt();
    out
}

#[inline]
pub fn nlerp<'a, 'b, T, F>(out: &'a mut [T; 4], a: &'b [T; 4], b: &'b [T; 4], t: F) -> &'a mut [T; 4]
    where T: Copy + Signed + Cast<F> + Sqrt,
          F: Copy + Signed + Cast<T>,

{
    let tmp = clone(lerp(out, a, b, t));
    normalize(out, &tmp)
}

#[test]
fn test_nlerp() {
    use super::create::new;
    assert_eq!(slerp(&mut new(0, 0, 0, 1), &new(0, 0, 0, 1), &new(0, 0, 0, 1), 0.5), &new(0, 0, 0, 1));
}

#[inline]
pub fn slerp<'a, 'b, T, F>(out: &'a mut [T; 4], a: &'b [T; 4], b: &'b [T; 4], t: F) -> &'a mut [T; 4]
    where T: Copy + Signed + Cast<F>,
          F: Copy + Signed + Cast<T> + Trig,
{
    let ax = a[0].cast();
    let ay = a[1].cast();
    let az = a[2].cast();
    let aw = a[3].cast();
    let mut bx = b[0].cast();
    let mut by = b[1].cast();
    let mut bz = b[2].cast();
    let mut bw = b[3].cast();

    let mut cosom = ax * bx + ay * by + az * bz + aw * bw;
    let mut sinom;
    let omega;
    let scale0;
    let scale1;

    if cosom < F::zero() {
        cosom = -cosom;
        bx = -bx;
        by = -by;
        bz = -bz;
        bw = -bw;
    }

    if F::one() - cosom > F::zero() {
        omega = cosom.acos();

        sinom = omega.sin();
        sinom = if sinom != F::zero() {F::one() / sinom} else {sinom};

        scale0 = ((F::one() - t) * omega).sin() * sinom;
        scale1 = (t * omega).sin() * sinom;
    } else {
        scale0 = F::one() - t;
        scale1 = t;
    }

    out[0] = (scale0 * ax + scale1 * bx).cast();
    out[1] = (scale0 * ay + scale1 * by).cast();
    out[2] = (scale0 * az + scale1 * bz).cast();
    out[3] = (scale0 * aw + scale1 * bw).cast();

    out
}

#[test]
fn test_slerp() {
    use super::create::new;
    assert_eq!(slerp(&mut new(0, 0, 0, 1), &new(0, 0, 0, 1), &new(0, 0, 0, 1), 0.5), &new(0, 0, 0, 1));
}