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
140
141
142
143
144
145
146
#[repr(C)]
#[derive(Copy, Clone, Default)]
pub struct matrix {
pub m: [[f32; 3]; 3],
pub invalid: bool,
}
#[repr(C)]
#[derive(Copy, Clone)]
pub struct vector {
pub v: [f32; 3],
}
pub fn matrix_eval(mut mat: matrix, mut v: vector) -> vector {
let mut result: vector = vector { v: [0.; 3] };
result.v[0] = mat.m[0][0] * v.v[0] + mat.m[0][1] * v.v[1] + mat.m[0][2] * v.v[2];
result.v[1] = mat.m[1][0] * v.v[0] + mat.m[1][1] * v.v[1] + mat.m[1][2] * v.v[2];
result.v[2] = mat.m[2][0] * v.v[0] + mat.m[2][1] * v.v[1] + mat.m[2][2] * v.v[2];
return result;
}
pub fn matrix_det(mut mat: matrix) -> f32 {
let mut det: f32 = mat.m[0][0] * mat.m[1][1] * mat.m[2][2]
+ mat.m[0][1] * mat.m[1][2] * mat.m[2][0]
+ mat.m[0][2] * mat.m[1][0] * mat.m[2][1]
- mat.m[0][0] * mat.m[1][2] * mat.m[2][1]
- mat.m[0][1] * mat.m[1][0] * mat.m[2][2]
- mat.m[0][2] * mat.m[1][1] * mat.m[2][0];
return det;
}
pub fn matrix_invert(mut mat: matrix) -> matrix {
let mut dest_mat: matrix = matrix {
m: [[0.; 3]; 3],
invalid: false,
};
let mut i: i32;
const a: [i32; 3] = [2, 2, 1];
const b: [i32; 3] = [1, 0, 0];
let mut det: f32 = matrix_det(mat);
if det == 0. {
dest_mat.invalid = true;
return dest_mat;
}
dest_mat.invalid = false;
det = 1. / det;
let mut j: i32 = 0;
while j < 3 {
i = 0;
while i < 3 {
let mut ai: i32 = a[i as usize];
let mut aj: i32 = a[j as usize];
let mut bi: i32 = b[i as usize];
let mut bj: i32 = b[j as usize];
let mut p: f64 = (mat.m[ai as usize][aj as usize] * mat.m[bi as usize][bj as usize]
- mat.m[ai as usize][bj as usize] * mat.m[bi as usize][aj as usize])
as f64;
if ((i + j) & 1) != 0 {
p = -p
}
dest_mat.m[j as usize][i as usize] = (det as f64 * p) as f32;
i += 1
}
j += 1
}
return dest_mat;
}
pub fn matrix_identity() -> matrix {
let mut i: matrix = matrix {
m: [[0.; 3]; 3],
invalid: false,
};
i.m[0][0] = 1.;
i.m[0][1] = 0.;
i.m[0][2] = 0.;
i.m[1][0] = 0.;
i.m[1][1] = 1.;
i.m[1][2] = 0.;
i.m[2][0] = 0.;
i.m[2][1] = 0.;
i.m[2][2] = 1.;
i.invalid = false;
return i;
}
pub fn matrix_invalid() -> matrix {
let mut inv: matrix = matrix_identity();
inv.invalid = true;
return inv;
}
pub fn matrix_multiply(mut a: matrix, mut b: matrix) -> matrix {
let mut result: matrix = matrix {
m: [[0.; 3]; 3],
invalid: false,
};
let mut dx: i32;
let mut o: i32;
let mut dy: i32 = 0;
while dy < 3 {
dx = 0;
while dx < 3 {
let mut v: f64 = 0f64;
o = 0;
while o < 3 {
v += (a.m[dy as usize][o as usize] * b.m[o as usize][dx as usize]) as f64;
o += 1
}
result.m[dy as usize][dx as usize] = v as f32;
dx += 1
}
dy += 1
}
result.invalid = a.invalid as i32 != 0 || b.invalid as i32 != 0;
return result;
}