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

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) -> 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<'a, 'b, T: Copy + Num + Sqrt>(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<'a, 'b, T: Copy + Num + Sqrt>(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<'a, 'b, T: Copy + Num>(a: &'b [T; 3], b: &'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, 'b, T: Copy + Num>(out: &'a mut [T; 3], a: &'b [T; 3], b: &'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<'a, T: Copy + Num + Sqrt>(out: &'a [T; 3]) -> T {
    length_values_sq(out[0], out[1], out[2])
}
#[test]
fn test_length_sq() {
    assert!(length_sq(&[1, 2, 2]) == 9);
}

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