dualquat 0.1.3

Quaternion and Dual Quaternion math library for high-precision physics simulation
Documentation 
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
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132

use crate::{DualQuaternion, Quaternion, Vec3};

pub const FORWARD: Vec3 = Vec3::new(1.0, 0.0, 0.0);
pub const UP: Vec3 = Vec3::new(0.0, 0.0, 1.0);

use nanorand::{Rng, WyRand};

pub(crate) fn gen_rand(generator: Option<&mut WyRand>) -> f64 {
    let rng: &mut WyRand;
    let mut _holder: Option<WyRand> = None;
    match generator {
        Some(gen) => {
            rng = gen;
        }
        None => {
            _holder = Some(WyRand::new());
            rng = _holder.as_mut().unwrap();
        }
    }
    let sign = match rng.generate_range(1_u64..=2) {
        1 => -1.0,
        _ => 1.0,
    };
    let dividend: f64 = rng.generate_range(1_u64..=2) as f64;
    let divisor: f64 = rng.generate_range(1_u64..=2) as f64;
    return sign * (dividend / divisor);
}

pub(crate) fn gen_rand_mat() -> [f64; 16] {
    let mut rng = WyRand::new();
    let mut ret: [f64; 16] = [0.0; 16];
    for i in 0..16 {
        ret[i] = gen_rand(Some(&mut rng));
    }
    return ret;
}

pub(crate) fn gen_rand_vec() -> Vec3 {
    let mut rng = WyRand::new();
    Vec3::new(
        gen_rand(Some(&mut rng)),
        gen_rand(Some(&mut rng)),
        gen_rand(Some(&mut rng)),
    )
}

pub(crate) fn trans_mat(mat: &[f64; 16]) -> [f64; 16] {
    let mut ret: [f64; 16] = [0.0; 16];
    let mut k = 0;
    for i in 0..4 {
        for j in 0..4 {
            ret[k] = mat[j * 4 + i];
            k += 1
        }
    }
    return ret;
}

pub(crate) fn gen_rand_q() -> Quaternion {
    fn gen_rand(rng: &mut WyRand) -> f64 {
        let sign = match rng.generate_range(1_u64..=2) {
            1 => -1.0,
            _ => 1.0,
        };
        let dividend: f64 = rng.generate_range(1_u64..=2) as f64;
        let divisor: f64 = rng.generate_range(1_u64..=2) as f64;
        return sign * dividend / divisor;
    }
    let mut rng = WyRand::new();
    let ret = Quaternion {
        scalar: gen_rand(&mut rng),
        vector: Vec3::new(gen_rand(&mut rng), gen_rand(&mut rng), gen_rand(&mut rng)),
    };

    #[cfg(unused)]
    match class {
        Some(Class::Vector) => {
            ret.scalar = 0.0;
        }
        Some(Class::Unit) => ret = ret.normalized(),
        _ => (),
    };
    return ret;
}

pub(crate) enum Class {
    Unit,
}

pub(crate) fn gen_rand_dq(class: Option<Class>) -> DualQuaternion {
    fn gen_rand(rng: &mut WyRand) -> f64 {
        let sign = match rng.generate_range(1_u64..=2) {
            1 => -1.0,
            _ => 1.0,
        };
        let dividend: f64 = rng.generate_range(1_u64..=2) as f64;
        let divisor: f64 = rng.generate_range(1_u64..=2) as f64;
        return sign * dividend / divisor;
    }
    let mut rng = WyRand::new();
    let ret = DualQuaternion {
        real: Quaternion {
            scalar: gen_rand(&mut rng),
            vector: Vec3::new(gen_rand(&mut rng), gen_rand(&mut rng), gen_rand(&mut rng)),
        },
        dual: Quaternion {
            scalar: gen_rand(&mut rng),
            vector: Vec3::new(gen_rand(&mut rng), gen_rand(&mut rng), gen_rand(&mut rng)),
        },
    };
    match class {
        #[cfg(unused)]
        Some(Class::Vector) => {
            ret.real.scalar = 0.0;
            ret.dual.scalar = 0.0;
        }
        Some(Class::Unit) => ret.normalized(),
        _ => ret,
    }
}

pub(crate) fn eps_equal(
    first: impl Iterator<Item = f64>,
    second: impl Iterator<Item = f64>,
    eps: f64,
) -> bool {
    first
        .zip(second)
        .filter(|(val1, val2)| (val1 - val2).abs() > eps)
        .count()
        == 0
}