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

use div::sdiv;

use number_traits::{Num, Sqrt};


#[inline]
pub fn length_values_sq<'a, 'b, 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<'a, 'b, T: Copy + Num + Sqrt>(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<'a, 'b, T: Copy + Num + Sqrt>(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, 'b, T: Copy + Num>(a: &'b [T; 4], b: &'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<'a, T: Copy + Num + Sqrt>(out: &'a [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<'a, T: Copy + Num + Sqrt>(out: &'a [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, 'b, T: Copy + Num + Sqrt>(out: &'a mut [T; 4], a: &'b [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);
}