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

use div::sdiv;

use number_traits::Num;

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

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

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

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

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

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

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

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