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
133
134
135
136
137
138
139

#[cfg(feature = "utils")]

use std::mem;
use std::sync::atomic::{AtomicUsize, ATOMIC_USIZE_INIT};
use std::sync::atomic::Ordering::SeqCst;
use time;

static DEVICE_ID_COUNTER: AtomicUsize  = ATOMIC_USIZE_INIT;

// Generates a unique identifier for any VRDisplay
#[allow(dead_code)]
pub fn new_id() -> u32 {
    DEVICE_ID_COUNTER.fetch_add(1, SeqCst) as u32
}

// Returns the current time in milliseconds
#[allow(dead_code)]
pub fn timestamp() -> f64 {
    let timespec = time::get_time();
    timespec.sec as f64 * 1000.0 + (timespec.nsec as f64 * 1e-6)
}

// Multiply 4x4 matrices
#[allow(dead_code)]
pub fn multiply_matrix(a: &[f32; 16], b: &[f32; 16], out: &mut [f32; 16]) {
    let mut tmp: [f32; 16] = unsafe { mem::uninitialized() };

    tmp[0] = b[0] * a[0] + b[1] * a[4] + b[2] * a[8] + b[3] * a[12];
    tmp[1] = b[0] * a[1] + b[1] * a[5] + b[2] * a[9] + b[3] * a[13];
    tmp[2] = b[0] * a[2] + b[1] * a[6] + b[2] * a[10] + b[3] * a[14];
    tmp[3] = b[0] * a[3] + b[1] * a[7] + b[2] * a[11] + b[3] * a[15];
    
    tmp[4] = b[4] * a[0] + b[5] * a[4] + b[6] * a[8] + b[7] * a[12];
    tmp[5] = b[4] * a[1] + b[5] * a[5] + b[6] * a[9] + b[7] * a[13];
    tmp[6] = b[4] * a[2] + b[5] * a[6] + b[6] * a[10] + b[7] * a[14];
    tmp[7] = b[4] * a[3] + b[5] * a[7] + b[6] * a[11] + b[7] * a[15];
    
    tmp[8] = b[8] * a[0] + b[9] * a[4] + b[10] * a[8] + b[11] * a[12];
    tmp[9] = b[8] * a[1] + b[9] * a[5] + b[10] * a[9] + b[11] * a[13];
    tmp[10] = b[8] * a[2] + b[9] * a[6] + b[10] * a[10] + b[11] * a[14];
    tmp[11] = b[8] * a[3] + b[9] * a[7] + b[10] * a[11] + b[11] * a[15];
    
    tmp[12] = b[12] * a[0] + b[13] * a[4] + b[14] * a[8] + b[15] * a[12];
    tmp[13] = b[12] * a[1] + b[13] * a[5] + b[14] * a[9] + b[15] * a[13];
    tmp[14] = b[12] * a[2] + b[13] * a[6] + b[14] * a[10] + b[15] * a[14];
    tmp[15] = b[12] * a[3] + b[13] * a[7] + b[14] * a[11] + b[15] * a[15];

    *out = tmp;
}

#[allow(dead_code)]
pub fn inverse_matrix(m: &[f32; 16], out: &mut [f32; 16]) -> bool {
    adjoint_matrix(&m, out);

    let det = determinant4x4(m);
    if det == 0f32 {
        return false;
    }

    for i in 0..16 {
        out[i] = out[i] / det;
    }
    true
}

#[allow(dead_code)]
pub fn adjoint_matrix(m: &[f32; 16], out: &mut [f32; 16]) {
    let mut tmp: [f32; 16] = unsafe { mem::uninitialized() };

    tmp[0]  =   determinant3x3(m[5], m[9], m[13], m[6], m[10], m[14], m[7], m[11], m[15]);
    tmp[4]  = - determinant3x3(m[4], m[8], m[12], m[6], m[10], m[14], m[7], m[11], m[15]);
    tmp[8]  =   determinant3x3(m[4], m[8], m[12], m[5], m[9], m[13], m[7], m[11], m[15]);
    tmp[12] = - determinant3x3(m[4], m[8], m[12], m[5], m[9], m[13], m[6], m[10], m[14]);

    tmp[1]  = - determinant3x3(m[1], m[9], m[13], m[2], m[10], m[14], m[3], m[11], m[15]);
    tmp[5]  =   determinant3x3(m[0], m[8], m[12], m[2], m[10], m[14], m[3], m[11], m[15]);
    tmp[9]  = - determinant3x3(m[0], m[8], m[12], m[1], m[9], m[13], m[3], m[11], m[15]);
    tmp[13] =   determinant3x3(m[0], m[8], m[12], m[1], m[9], m[13], m[2], m[10], m[14]);

    tmp[2]  =   determinant3x3(m[1], m[5], m[13], m[2], m[6], m[14], m[3], m[7], m[15]);
    tmp[6]  = - determinant3x3(m[0], m[4], m[12], m[2], m[6], m[14], m[3], m[7], m[15]);
    tmp[10] =   determinant3x3(m[0], m[4], m[12], m[1], m[5], m[13], m[3], m[7], m[15]);
    tmp[14] = - determinant3x3(m[0], m[4], m[12], m[1], m[5], m[13], m[2], m[6], m[14]);

    tmp[3]  = - determinant3x3(m[1], m[5], m[9], m[2], m[6], m[10], m[3], m[7], m[11]);
    tmp[7]  =   determinant3x3(m[0], m[4], m[8], m[2], m[6], m[10], m[3], m[7], m[11]);
    tmp[11] = - determinant3x3(m[0], m[4], m[8], m[1], m[5], m[9], m[3], m[7], m[11]);
    tmp[15] =   determinant3x3(m[0], m[4], m[8], m[1], m[5], m[9], m[2], m[6], m[10]);
    
    *out = tmp;
}

#[allow(dead_code)]
pub fn determinant4x4(m: &[f32; 16]) -> f32 {
    m[0] * determinant3x3(m[5], m[9], m[13], m[6], m[10], m[14], m[7], m[11], m[15])
    - m[1] * determinant3x3(m[4], m[8], m[12], m[6], m[10], m[14], m[7], m[11], m[15])
    + m[2] * determinant3x3(m[4], m[8], m[12], m[5], m[9], m[13], m[7], m[11], m[15])
    - m[3] * determinant3x3(m[4], m[8], m[12], m[5], m[9], m[13], m[6], m[10], m[14])
}

#[allow(dead_code)]
fn determinant3x3(a1: f32, a2: f32, a3: f32, b1: f32, b2: f32, b3: f32, c1: f32, c2: f32, c3: f32) -> f32 {
    a1 * determinant2x2(b2, b3, c2, c3)
    - b1 * determinant2x2(a2, a3, c2, c3)
    + c1 * determinant2x2(a2, a3, b2, b3)
}

#[allow(dead_code)]
#[inline]
fn determinant2x2(a: f32, b: f32, c: f32, d: f32) -> f32 {
    a * d - b * c
}

// Adapted from http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm
#[allow(dead_code)]
#[inline]
pub fn matrix_to_quat(matrix: &[f32; 16]) -> [f32; 4] {
    let m: &[[f32; 4]; 4] = unsafe { mem::transmute(matrix) };
    let w = f32::max(0.0, 1.0 + m[0][0] + m[1][1] + m[2][2]).sqrt() * 0.5;
    let mut x = f32::max(0.0, 1.0 + m[0][0] - m[1][1] - m[2][2]).sqrt() * 0.5;
    let mut y = f32::max(0.0, 1.0 - m[0][0] + m[1][1] - m[2][2]).sqrt() * 0.5;
    let mut z = f32::max(0.0, 1.0 - m[0][0] - m[1][1] + m[2][2]).sqrt() * 0.5;

    x = copysign(x, m[2][1] - m[1][2]);
    y = copysign(y, m[0][2] - m[2][0]);
    z = copysign(z, m[1][0] - m[0][1]);

    [x, y, z, w]
}

#[allow(dead_code)]
#[inline]
pub fn copysign(a: f32, b: f32) -> f32 {
    if b == 0.0 {
        0.0
    } else {
        a.abs() * b.signum()
    }
}