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

use div::sdiv;

use number_traits::Num;

#[inline]
pub fn length_values_sq<T: Copy + Num>(x: T, y: T, z: T, w: T) -> T {
    x * x + y * y + z * z + w * w
}
#[test]
fn test_length_values_sq() {
    assert!(length_values_sq(1, 1, 1, 1) == 4);
}

#[inline]
pub fn length_values<T: Copy + Num>(x: T, y: T, z: T, w: T) -> T {
    let lsq = length_values_sq(x, y, z, w);
    if lsq == T::zero() {
        lsq
    } else {
        lsq.sqrt()
    }
}
#[test]
fn test_length_values() {
    assert!(length_values(1, 1, 1, 1) == 2);
}

#[inline]
pub fn inv_length_values<T: Copy + Num>(x: T, y: T, z: T, w: T) -> T {
    let lsq = length_values_sq(x, y, z, w);
    if lsq == T::zero() {
        lsq
    } else {
        T::one() / lsq.sqrt()
    }
}
#[test]
fn test_inv_length_values() {
    assert!(inv_length_values(1.0, 1.0, 1.0, 1.0) == 1.0 / 2.0);
}

#[inline]
pub fn dot<'a, T: Copy + Num>(a: &[T; 4], b: &[T; 4]) -> T {
    a[0] * b[0] + a[1] * b[1] + a[2] * b[2] + a[3] * b[3]
}
#[test]
fn test_dot() {
    assert!(dot(&[1, 1, 1, 1], &[1, 1, 1, 1]) == 4);
}

#[inline]
pub fn length_sq<T: Copy + Num>(out: &[T; 4]) -> T {
    length_values_sq(out[0], out[1], out[2], out[3])
}
#[test]
fn test_length_sq() {
    assert!(length_sq(&[1, 1, 1, 1]) == 4);
}

#[inline]
pub fn length<T: Copy + Num>(out: &[T; 4]) -> T {
    length_values(out[0], out[1], out[2], out[3])
}
#[test]
fn test_length() {
    assert!(length(&[1, 1, 1, 1]) == 2);
}

#[inline]
pub fn normalize<'a, T: Copy + Num>(out: &'a mut [T; 4], a: &[T; 4]) -> &'a mut [T; 4] {
    sdiv(out, a, length(a))
}
#[test]
fn test_normalize() {
    let mut v = [0.0, 0.0, 0.0, 0.0];
    normalize(&mut v, &[0.0, 0.0, 0.0, 1.0]);
    assert!(v[0] == 0.0);
    assert!(v[1] == 0.0);
    assert!(v[2] == 0.0);
    assert!(v[3] == 1.0);
}