1use crate::matrix::Matrix;
2use crate::utils::EPSILON;
3use std::f32;
4
5pub type Mat3 = [f32; 9];
6pub type Vec3 = [f32; 3];
7
8impl Matrix for Mat3 {
9 type MatrixType = Mat3;
10 type VectorType = Vec3;
11
12 fn zeros() -> Self {
13 [0., 0., 0., 0., 0., 0., 0., 0., 0.]
14 }
15 fn ones() -> Self {
16 [1., 1., 1., 1., 1., 1., 1., 1., 1.]
17 }
18 fn identity() -> Self {
19 [1., 0., 0., 0., 1., 0., 0., 0., 1.]
20 }
21
22 fn copy_to(&self, dst: &mut Self) {
23 dst[0] = self[0];
24 dst[1] = self[1];
25 dst[2] = self[2];
26 dst[3] = self[3];
27 dst[4] = self[4];
28 dst[5] = self[5];
29 dst[6] = self[6];
30 dst[7] = self[7];
31 dst[8] = self[8];
32 }
33
34 fn transpose(&mut self) -> &mut Self {
35 let v01 = self[1];
36 let v02 = self[2];
37 let v12 = self[5];
38 self[1] = self[3];
39 self[2] = self[6];
40 self[3] = v01;
41 self[5] = self[7];
42 self[6] = v02;
43 self[7] = v12;
44
45 self
46 }
47
48 fn mul(&mut self, rhs: &Self) -> &mut Self {
49 let lhs00 = self[0];
50 let lhs01 = self[1];
51 let lhs02 = self[2];
52 let lhs10 = self[3];
53 let lhs11 = self[4];
54 let lhs12 = self[5];
55 let lhs20 = self[6];
56 let lhs21 = self[7];
57 let lhs22 = self[8];
58
59 let rhs00 = rhs[0];
60 let rhs01 = rhs[1];
61 let rhs02 = rhs[2];
62 let rhs10 = rhs[3];
63 let rhs11 = rhs[4];
64 let rhs12 = rhs[5];
65 let rhs20 = rhs[6];
66 let rhs21 = rhs[7];
67 let rhs22 = rhs[8];
68
69 self[0] = lhs00 * rhs00 + lhs01 * rhs10 + lhs02 * rhs20;
70 self[1] = lhs00 * rhs01 + lhs01 * rhs11 + lhs02 * rhs21;
71 self[2] = lhs00 * rhs02 + lhs01 * rhs12 + lhs02 * rhs22;
72 self[3] = lhs10 * rhs00 + lhs11 * rhs10 + lhs12 * rhs20;
73 self[4] = lhs10 * rhs01 + lhs11 * rhs11 + lhs12 * rhs21;
74 self[5] = lhs10 * rhs02 + lhs11 * rhs12 + lhs12 * rhs22;
75 self[6] = lhs20 * rhs00 + lhs21 * rhs10 + lhs22 * rhs20;
76 self[7] = lhs20 * rhs01 + lhs21 * rhs11 + lhs22 * rhs21;
77 self[8] = lhs20 * rhs02 + lhs21 * rhs12 + lhs22 * rhs22;
78
79 self
80 }
81
82 fn mul_vector(&self, rhs: &[f32]) -> Vec3 {
83 debug_assert!(rhs.len() > 1);
84
85 let x = rhs[0];
86 let y = rhs[1];
87 let w = if rhs.len() > 2 { rhs[2] } else { 1. };
88 [
89 self[0] * x + self[1] * y + self[2] * w,
90 self[3] * x + self[4] * y + self[5] * w,
91 self[6] * x + self[7] * y + self[8] * w,
92 ]
93 }
94
95 fn mul_vector_left(&self, lhs: &[f32]) -> Vec3 {
96 debug_assert!(lhs.len() > 1);
97 let x = lhs[0];
98 let y = lhs[1];
99 let w = if lhs.len() > 2 { lhs[2] } else { 1. };
100 [
101 self[0] * x + self[3] * y + self[6] * w,
102 self[1] * x + self[4] * y + self[7] * w,
103 self[2] * x + self[5] * y + self[8] * w,
104 ]
105 }
106
107 fn add(&mut self, rhs: &Self) -> &mut Self {
108 self[0] += rhs[0];
109 self[1] += rhs[1];
110 self[2] += rhs[2];
111 self[3] += rhs[3];
112 self[4] += rhs[4];
113 self[5] += rhs[5];
114 self[6] += rhs[6];
115 self[7] += rhs[7];
116 self[8] += rhs[8];
117
118 self
119 }
120
121 fn sub(&mut self, rhs: &Self) -> &mut Self {
122 self[0] -= rhs[0];
123 self[1] -= rhs[1];
124 self[2] -= rhs[2];
125 self[3] -= rhs[3];
126 self[4] -= rhs[4];
127 self[5] -= rhs[5];
128 self[6] -= rhs[6];
129 self[7] -= rhs[7];
130 self[8] -= rhs[8];
131
132 self
133 }
134
135 fn scale(&mut self, factor: f32) -> &mut Self {
136 self[0] *= factor;
137 self[1] *= factor;
138 self[2] *= factor;
139 self[3] *= factor;
140 self[4] *= factor;
141 self[5] *= factor;
142 self[6] *= factor;
143 self[7] *= factor;
144 self[8] *= factor;
145
146 self
147 }
148
149 fn inverse(&mut self) -> Option<&mut Self> {
150 let v00 = self[0];
151 let v01 = self[1];
152 let v02 = self[2];
153 let v10 = self[3];
154 let v11 = self[4];
155 let v12 = self[5];
156 let v20 = self[6];
157 let v21 = self[7];
158 let v22 = self[8];
159
160 let tmp01 = v22 * v11 - v12 * v21;
161 let tmp11 = -v22 * v10 + v12 * v20;
162 let tmp21 = v21 * v10 - v11 * v20;
163
164 let det = v00 * tmp01 + v01 * tmp11 + v02 * tmp21;
165
166 if det.abs() <= EPSILON {
167 return None;
168 }
169
170 let det_inv = 1.0 / det;
171
172 self[0] = tmp01 * det_inv;
173 self[1] = (-v22 * v01 + v02 * v21) * det_inv;
174 self[2] = (v12 * v01 - v02 * v11) * det_inv;
175 self[3] = tmp11 * det_inv;
176 self[4] = (v22 * v00 - v02 * v20) * det_inv;
177 self[5] = (-v12 * v00 + v02 * v10) * det_inv;
178 self[6] = tmp21 * det_inv;
179 self[7] = (-v21 * v00 + v01 * v20) * det_inv;
180 self[8] = (v11 * v00 - v01 * v10) * det_inv;
181
182 Some(self)
183 }
184
185 fn det(&self) -> f32 {
186 let v00 = self[0];
187 let v01 = self[1];
188 let v02 = self[2];
189 let v10 = self[3];
190 let v11 = self[4];
191 let v12 = self[5];
192 let v20 = self[6];
193 let v21 = self[7];
194 let v22 = self[8];
195
196 v00 * (v22 * v11 - v12 * v21)
197 + v01 * (-v22 * v10 + v12 * v20)
198 + v02 * (v21 * v10 - v11 * v20)
199 }
200
201 fn adjugate(&mut self) -> &mut Self {
202 let v00 = self[0];
203 let v01 = self[1];
204 let v02 = self[2];
205 let v10 = self[3];
206 let v11 = self[4];
207 let v12 = self[5];
208 let v20 = self[6];
209 let v21 = self[7];
210 let v22 = self[8];
211
212 self[0] = v11 * v22 - v12 * v21;
213 self[1] = v02 * v21 - v01 * v22;
214 self[2] = v01 * v12 - v02 * v11;
215 self[3] = v12 * v20 - v10 * v22;
216 self[4] = v00 * v22 - v02 * v20;
217 self[5] = v02 * v10 - v00 * v12;
218 self[6] = v10 * v21 - v11 * v20;
219 self[7] = v01 * v20 - v00 * v21;
220 self[8] = v00 * v11 - v01 * v10;
221
222 self
223 }
224
225 fn translate(&mut self, direction: &[f32]) -> &mut Self {
226 debug_assert!(direction.len() > 1);
227
228 let mut x = direction[0];
229 let mut y = direction[1];
230
231 if direction.len() > 2 {
232 x /= direction[2];
233 y /= direction[2];
234 }
235
236 self[6] += x * self[0] + y * self[3];
237 self[7] += x * self[1] + y * self[4];
238 self[8] += x * self[2] + y * self[5];
239
240 self
241 }
242
243 fn rotate(&mut self, angle: f32, _: &[f32]) -> &mut Self {
246 let v00 = self[0];
247 let v01 = self[1];
248 let v02 = self[2];
249 let v10 = self[3];
250 let v11 = self[4];
251 let v12 = self[5];
252
253 let (s, c) = angle.sin_cos();
254
255 self[0] = c * v00 + s * v10;
256 self[1] = c * v01 + s * v11;
257 self[2] = c * v02 + s * v12;
258 self[3] = c * v10 - s * v00;
259 self[4] = c * v11 - s * v01;
260 self[5] = c * v12 - s * v02;
261
262 self
263 }
264}
265
266#[cfg(test)]
267mod tests {
268 use super::*;
269 use crate::utils::almost_eq;
270
271 #[test]
272 fn mat3_zeros() {
273 let zeros = Mat3::zeros();
274 assert!(zeros.iter().all(|&x| x == 0.0));
275 }
276
277 #[test]
278 fn mat3_ones() {
279 let ones = Mat3::ones();
280 assert!(ones.iter().all(|&x| x == 1.0));
281 }
282
283 #[test]
284 fn mat3_identity() {
285 let i = Mat3::identity();
286 assert_eq!(i[0], 1.0);
287 assert_eq!(i[1], 0.0);
288 assert_eq!(i[2], 0.0);
289
290 assert_eq!(i[3], 0.0);
291 assert_eq!(i[4], 1.0);
292 assert_eq!(i[5], 0.0);
293
294 assert_eq!(i[6], 0.0);
295 assert_eq!(i[7], 0.0);
296 assert_eq!(i[8], 1.0);
297 }
298
299 #[test]
300 fn mat3_copy_to() {
301 let mut a = Mat3::zeros();
302 let b = Mat3::ones();
303
304 b.copy_to(&mut a);
305 assert!(a.iter().all(|&x| x == 1.0));
306 }
307
308 #[test]
309 fn mat3_transpose() {
310 let mut a = [1., 2., 3., 4., 5., 6., 7., 8., 9.];
320 a.transpose();
321 let b: Mat3 = [1., 4., 7., 2., 5., 8., 3., 6., 9.];
322
323 assert_eq!(a, b);
324 }
325
326 #[test]
327 fn mat3_mul() {
328 let mut a: Mat3 = [1., 2., 3., 4., 5., 6., 7., 8., 9.];
329 let b: Mat3 = [11., 12., 13., 14., 15., 16., 17., 18., 19.];
330
331 let c: Mat3 = [90., 96., 102., 216., 231., 246., 342., 366., 390.];
332
333 assert_eq!(a.mul(&b), &c);
334 }
335
336 #[test]
337 fn mat3_mul_identity() {
338 let a = [1., 2., 3., 4., 5., 6., 7., 8., 9.];
339 let mut b = Mat3::identity();
340
341 assert_eq!(b.mul(&a), &a);
342 }
343
344 #[test]
345 fn mat3_add() {
346 let mut a = [1., 2., 3., 4., 5., 6., 7., 8., 9.];
347 let b = [11., 12., 13., 14., 15., 16., 17., 18., 19.];
348
349 let c = [12., 14., 16., 18., 20., 22., 24., 26., 28.];
350
351 assert_eq!(a.add(&b), &c);
352 }
353
354 #[test]
355 fn mat3_sub() {
356 let mut a = [9., 8., 7., 6., 5., 4., 3., 2., 1.];
357 let b = [11., 12., 13., 14., 15., 16., 17., 18., 19.];
358
359 let c = [-2., -4., -6., -8., -10., -12., -14., -16., -18.];
360
361 assert_eq!(a.sub(&b), &c);
362 }
363
364 #[test]
365 fn mat3_scale() {
366 let mut a = [9., 8., 7., 6., 5., 4., 3., 2., 1.];
367 let b = [18., 16., 14., 12., 10., 8., 6., 4., 2.];
368
369 assert_eq!(a.scale(2.0), &b);
370 }
371
372 #[test]
373 fn mat3_inverse_valid() {
374 let mut a = [1., 3., 2., 4., 2., 8., 9., 2., 7.];
375 let b = a.clone();
376
377 let a = a.inverse().expect("Inverse should exist");
378
379 let inv = [
380 -0.01818, -0.15455, 0.18182, 0.4, -0.1, 0.0, -0.09091, 0.22727, -0.09091,
381 ];
382 assert!(almost_eq(&inv, a));
383
384 assert!(almost_eq(a.mul(&b), &Mat3::identity()));
385 }
386
387 #[test]
388 fn mat3_inverse_invalid() {
389 let mut a = [9., 8., 7., 6., 5., 4., 3., 2., 1.];
390 assert_eq!(a.inverse(), None);
391 }
392
393 #[test]
394 fn mat3_det() {
395 let a = [1., 3., 2., 4., 2., 8., 9., 2., 7.];
396 assert_eq!(a.det(), 110.0);
397 }
398
399 #[test]
400 fn mat3_adjugate() {
401 let mut a = [1., 3., 2., 4., 2., 8., 9., 2., 7.];
402 let b = [-2., -17., 20., 44., -11., 0., -10., 25., -10.];
403 assert_eq!(a.adjugate(), &b);
404 }
405
406 #[test]
407 fn mat3_mul_vector() {
408 let a = [1., 2., 3., 4., 5., 6., 7., 8., 9.];
409 let b = [11., 12., 13.];
410
411 let c = a.mul_vector(&b);
412 assert_eq!(c, [74., 182., 290.]);
413 }
414
415 #[test]
416 fn mat3_mul_vector_left() {
417 let a = [1., 2., 3., 4., 5., 6., 7., 8., 9.];
418 let b = [11., 12., 13.];
419
420 let c = a.mul_vector_left(&b);
421 assert_eq!(c, [150., 186., 222.]);
422 }
423
424 #[test]
425 fn mat3_translate() {
426 let d = [3., -5., 1.];
427 let mut m = Mat3::identity();
428 m.translate(&d);
429
430 let a = [-3., 5., 1.];
431 assert_eq!(m.mul_vector_left(&a), [0., 0., 1.]);
432 }
433
434 #[test]
435 fn mat3_rotate() {
436 let mut m = Mat3::identity();
437 m.rotate(f32::consts::FRAC_PI_2, &[0.; 3]);
438
439 let v = [-1., 3., 1.];
440
441 let r = m.mul_vector_left(&v);
442 assert!(almost_eq(&r, &[-3., -1., 1.]));
443 }
444}