1#![allow(dead_code)]
2
3use std::{
4 fmt::Display,
5 ops::{Add, AddAssign, Div, DivAssign, Index, IndexMut, Mul, MulAssign, Sub, SubAssign},
6};
7
8use crate::algebra::{vec3f, Mat2f, Mat3d, Mat4f, Vec3f};
9
10#[derive(Clone, Copy, PartialEq, Debug)]
11pub struct Mat3f {
12 x_axis: Vec3f,
13 y_axis: Vec3f,
14 z_axis: Vec3f,
15}
16
17pub fn mat3f(
18 m00: f32,
19 m01: f32,
20 m02: f32,
21 m10: f32,
22 m11: f32,
23 m12: f32,
24 m20: f32,
25 m21: f32,
26 m22: f32,
27) -> Mat3f {
28 Mat3f::from_array([m00, m01, m02, m10, m11, m12, m20, m21, m22])
29}
30
31impl Display for Mat3f {
32 fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
33 let (m00, m01, m02) = self.x_axis.to_tuple();
34 let (m10, m11, m12) = self.y_axis.to_tuple();
35 let (m20, m21, m22) = self.z_axis.to_tuple();
36 write!(
37 f,
38 "Mat3f({}, {}, {} | {}, {}, {} | {}, {}, {})",
39 m00, m01, m02, m10, m11, m12, m20, m21, m22
40 )
41 }
42}
43
44impl Default for Mat3f {
45 fn default() -> Self {
46 Self {
47 x_axis: Vec3f::default(),
48 y_axis: Vec3f::default(),
49 z_axis: Vec3f::default(),
50 }
51 }
52}
53
54impl Add<Mat3f> for Mat3f {
55 type Output = Mat3f;
56
57 fn add(self, rhs: Mat3f) -> Self::Output {
58 Mat3f::new(
59 self.x_axis + rhs.x_axis,
60 self.y_axis + rhs.y_axis,
61 self.z_axis + rhs.z_axis,
62 )
63 }
64}
65
66impl Add<f32> for Mat3f {
67 type Output = Mat3f;
68
69 fn add(self, rhs: f32) -> Self::Output {
70 Mat3f::new(self.x_axis + rhs, self.y_axis + rhs, self.z_axis + rhs)
71 }
72}
73
74impl Add<Mat3f> for f32 {
75 type Output = Mat3f;
76
77 fn add(self, rhs: Mat3f) -> Self::Output {
78 Mat3f::new(self + rhs.x_axis, self + rhs.y_axis, self + rhs.z_axis)
79 }
80}
81
82impl AddAssign<Mat3f> for Mat3f {
83 fn add_assign(&mut self, rhs: Mat3f) {
84 *self = *self + rhs;
85 }
86}
87
88impl AddAssign<f32> for Mat3f {
89 fn add_assign(&mut self, rhs: f32) {
90 *self = *self + rhs;
91 }
92}
93
94impl Sub<Mat3f> for Mat3f {
95 type Output = Mat3f;
96
97 fn sub(self, rhs: Mat3f) -> Self::Output {
98 Mat3f::new(
99 self.x_axis - rhs.x_axis,
100 self.y_axis - rhs.y_axis,
101 self.z_axis - rhs.z_axis,
102 )
103 }
104}
105
106impl Sub<f32> for Mat3f {
107 type Output = Mat3f;
108
109 fn sub(self, rhs: f32) -> Self::Output {
110 Mat3f::new(self.x_axis - rhs, self.y_axis - rhs, self.z_axis - rhs)
111 }
112}
113
114impl Sub<Mat3f> for f32 {
115 type Output = Mat3f;
116
117 fn sub(self, rhs: Mat3f) -> Self::Output {
118 Mat3f::new(self - rhs.x_axis, self - rhs.y_axis, self - rhs.z_axis)
119 }
120}
121
122impl SubAssign<Mat3f> for Mat3f {
123 fn sub_assign(&mut self, rhs: Mat3f) {
124 *self = *self - rhs;
125 }
126}
127
128impl SubAssign<f32> for Mat3f {
129 fn sub_assign(&mut self, rhs: f32) {
130 *self = *self - rhs;
131 }
132}
133
134impl Mul<Mat3f> for Mat3f {
135 type Output = Mat3f;
136
137 fn mul(self, rhs: Mat3f) -> Self::Output {
138 let m00 = self.x_axis.dot(vec3f(rhs[0][0], rhs[1][0], rhs[2][0]));
139 let m01 = self.x_axis.dot(vec3f(rhs[0][1], rhs[1][1], rhs[2][1]));
140 let m02 = self.x_axis.dot(vec3f(rhs[0][2], rhs[1][2], rhs[2][2]));
141
142 let m10 = self.y_axis.dot(vec3f(rhs[0][0], rhs[1][0], rhs[2][0]));
143 let m11 = self.y_axis.dot(vec3f(rhs[0][1], rhs[1][1], rhs[2][1]));
144 let m12 = self.y_axis.dot(vec3f(rhs[0][2], rhs[1][2], rhs[2][2]));
145
146 let m20 = self.z_axis.dot(vec3f(rhs[0][0], rhs[1][0], rhs[2][0]));
147 let m21 = self.z_axis.dot(vec3f(rhs[0][1], rhs[1][1], rhs[2][1]));
148 let m22 = self.z_axis.dot(vec3f(rhs[0][2], rhs[1][2], rhs[2][2]));
149
150 Mat3f::new(
151 vec3f(m00, m01, m02),
152 vec3f(m10, m11, m12),
153 vec3f(m20, m21, m22),
154 )
155 }
156}
157
158impl Mul<Vec3f> for Mat3f {
159 type Output = Vec3f;
160
161 fn mul(self, rhs: Vec3f) -> Self::Output {
162 let v0 = self.x_axis.dot(rhs);
163 let v1 = self.y_axis.dot(rhs);
164 let v2 = self.z_axis.dot(rhs);
165 vec3f(v0, v1, v2)
166 }
167}
168
169impl Mul<f32> for Mat3f {
170 type Output = Mat3f;
171
172 fn mul(self, rhs: f32) -> Self::Output {
173 Mat3f::new(self.x_axis * rhs, self.y_axis * rhs, self.z_axis * rhs)
174 }
175}
176
177impl Mul<Mat3f> for f32 {
178 type Output = Mat3f;
179
180 fn mul(self, rhs: Mat3f) -> Self::Output {
181 Mat3f::new(self * rhs.x_axis, self * rhs.y_axis, self * rhs.z_axis)
182 }
183}
184
185impl MulAssign<Mat3f> for Mat3f {
186 fn mul_assign(&mut self, rhs: Mat3f) {
187 *self = *self * rhs;
188 }
189}
190
191impl MulAssign<f32> for Mat3f {
192 fn mul_assign(&mut self, rhs: f32) {
193 *self = *self * rhs;
194 }
195}
196
197impl Div<f32> for Mat3f {
198 type Output = Mat3f;
199
200 fn div(self, rhs: f32) -> Self::Output {
201 Mat3f::new(self.x_axis / rhs, self.y_axis / rhs, self.z_axis / rhs)
202 }
203}
204
205impl DivAssign<f32> for Mat3f {
206 fn div_assign(&mut self, rhs: f32) {
207 *self = *self / rhs;
208 }
209}
210
211impl Index<usize> for Mat3f {
212 type Output = Vec3f;
213
214 fn index(&self, index: usize) -> &Self::Output {
215 match index {
216 0 => &self.x_axis,
217 1 => &self.y_axis,
218 2 => &self.z_axis,
219 _ => panic!("`rmath::algebra::Mat3f::index`: index out of bounds."),
220 }
221 }
222}
223
224impl IndexMut<usize> for Mat3f {
225 fn index_mut(&mut self, index: usize) -> &mut Self::Output {
226 match index {
227 0 => &mut self.x_axis,
228 1 => &mut self.y_axis,
229 2 => &mut self.z_axis,
230 _ => panic!("`rmath::algebra::Mat3f::index_mut`: index out of bounds."),
231 }
232 }
233}
234
235impl Mat3f {
236 pub fn new(x_axis: Vec3f, y_axis: Vec3f, z_axis: Vec3f) -> Self {
237 Self {
238 x_axis,
239 y_axis,
240 z_axis,
241 }
242 }
243
244 pub fn zero() -> Self {
245 Self::new(
246 vec3f(0.0, 0.0, 0.0),
247 vec3f(0.0, 0.0, 0.0),
248 vec3f(0.0, 0.0, 0.0),
249 )
250 }
251
252 pub fn identity() -> Self {
253 Self::new(
254 vec3f(1.0, 0.0, 0.0),
255 vec3f(0.0, 1.0, 0.0),
256 vec3f(0.0, 0.0, 1.0),
257 )
258 }
259}
260
261impl Mat3f {
262 pub fn col(&self, index: usize) -> Vec3f {
263 match index {
264 0 => vec3f(self[0][0], self[1][0], self[2][0]),
265 1 => vec3f(self[0][1], self[1][1], self[2][1]),
266 2 => vec3f(self[0][2], self[1][2], self[2][2]),
267 _ => panic!("`rmath::algebra::Mat3f::col`: index out of bounds."),
268 }
269 }
270
271 pub fn row(&self, index: usize) -> Vec3f {
272 match index {
273 0 => self.x_axis,
274 1 => self.y_axis,
275 2 => self.z_axis,
276 _ => panic!("`rmath::algebra::Mat3f::row`: index out of bounds."),
277 }
278 }
279
280 pub fn is_nan(&self) -> bool {
281 self.x_axis.is_nan() || self.y_axis.is_nan() || self.z_axis.is_nan()
282 }
283
284 pub fn is_infinite(&self) -> bool {
285 self.x_axis.is_infinite() || self.y_axis.is_infinite() || self.z_axis.is_infinite()
286 }
287
288 pub fn is_finite(&self) -> bool {
289 self.x_axis.is_finite() && self.y_axis.is_finite() && self.z_axis.is_finite()
290 }
291
292 pub fn transpose(&self) -> Self {
293 let (m00, m01, m02) = self.x_axis.to_tuple();
294 let (m10, m11, m12) = self.y_axis.to_tuple();
295 let (m20, m21, m22) = self.z_axis.to_tuple();
296 Self::new(
297 vec3f(m00, m10, m20),
298 vec3f(m01, m11, m21),
299 vec3f(m02, m12, m22),
300 )
301 }
302
303 pub fn determinant(&self) -> f32 {
304 let (m00, m01, m02) = self.x_axis.to_tuple();
305 let (m10, m11, m12) = self.y_axis.to_tuple();
306 let (m20, m21, m22) = self.z_axis.to_tuple();
307
308 m00 * m11 * m22 + m01 * m12 * m20 + m02 * m10 * m21
309 - m00 * m12 * m21
310 - m01 * m10 * m22
311 - m02 * m11 * m20
312 }
313
314 pub fn inverse(&self) -> Self {
315 let det = self.determinant();
316 ruby_assert!(!(det.abs() < f32::EPSILON));
317
318 let inv_det = det.recip();
319 let (m00, m01, m02) = self.x_axis.to_tuple();
320 let (m10, m11, m12) = self.y_axis.to_tuple();
321 let (m20, m21, m22) = self.z_axis.to_tuple();
322
323 let a00 = m11 * m22 - m12 * m21;
324 let a01 = -(m10 * m22 - m12 * m20);
325 let a02 = m10 * m21 - m11 * m20;
326
327 let a10 = -(m01 * m22 - m02 * m21);
328 let a11 = m00 * m22 - m02 * m20;
329 let a12 = -(m00 * m21 - m01 * m20);
330
331 let a20 = m01 * m12 - m02 * m11;
332 let a21 = -(m00 * m12 - m02 * m10);
333 let a22 = m00 * m11 - m01 * m10;
334
335 Self::new(
336 vec3f(a00, a10, a20),
337 vec3f(a01, a11, a21),
338 vec3f(a02, a12, a22),
339 )
340 .mul(inv_det)
341 }
342
343 pub fn try_inverse(&self) -> Option<Self> {
344 let det = self.determinant();
345 if det.abs() < f32::EPSILON {
346 return None;
347 }
348
349 let inv_det = det.recip();
350 let (m00, m01, m02) = self.x_axis.to_tuple();
351 let (m10, m11, m12) = self.y_axis.to_tuple();
352 let (m20, m21, m22) = self.z_axis.to_tuple();
353
354 let a00 = m11 * m22 - m12 * m21;
355 let a01 = -(m10 * m22 - m12 * m20);
356 let a02 = m10 * m21 - m11 * m20;
357
358 let a10 = -(m01 * m22 - m02 * m21);
359 let a11 = m00 * m22 - m02 * m20;
360 let a12 = -(m00 * m21 - m01 * m20);
361
362 let a20 = m01 * m12 - m02 * m11;
363 let a21 = -(m00 * m12 - m02 * m10);
364 let a22 = m00 * m11 - m01 * m10;
365
366 Some(
367 Self::new(
368 vec3f(a00, a10, a20),
369 vec3f(a01, a11, a21),
370 vec3f(a02, a12, a22),
371 )
372 .mul(inv_det),
373 )
374 }
375}
376
377impl From<Mat2f> for Mat3f {
378 fn from(m: Mat2f) -> Self {
379 mat3f(m[0][0], m[0][1], 0.0, m[1][0], m[1][1], 0.0, 0.0, 0.0, 1.0)
380 }
381}
382
383impl From<Mat4f> for Mat3f {
384 fn from(m: Mat4f) -> Self {
385 mat3f(
386 m[0][0], m[0][1], m[0][2], m[1][0], m[1][1], m[1][2], m[2][0], m[2][1], m[2][2],
387 )
388 }
389}
390
391impl Mat3f {
392 pub fn from_array(m: [f32; 9]) -> Self {
393 Self::new(
394 vec3f(m[0], m[1], m[2]),
395 vec3f(m[3], m[4], m[5]),
396 vec3f(m[6], m[7], m[8]),
397 )
398 }
399
400 pub fn from_array_2d(m: [[f32; 3]; 3]) -> Self {
401 Self::new(
402 vec3f(m[0][0], m[0][1], m[0][2]),
403 vec3f(m[1][0], m[1][1], m[1][2]),
404 vec3f(m[2][0], m[2][1], m[2][2]),
405 )
406 }
407
408 pub fn from_diagonal(diagonal: Vec3f) -> Self {
409 Self::new(
410 vec3f(diagonal.x(), 0.0, 0.0),
411 vec3f(0.0, diagonal.y(), 0.0),
412 vec3f(0.0, 0.0, diagonal.z()),
413 )
414 }
415}
416
417impl Mat3f {
418 pub fn to_array(self) -> [f32; 9] {
419 [
420 self[0][0], self[0][1], self[0][2], self[1][0], self[1][1], self[1][2], self[2][0],
421 self[2][1], self[2][2],
422 ]
423 }
424
425 pub fn to_array_2d(self) -> [[f32; 3]; 3] {
426 [
427 [self[0][0], self[0][1], self[0][2]],
428 [self[1][0], self[1][1], self[1][2]],
429 [self[2][0], self[2][1], self[2][2]],
430 ]
431 }
432
433 pub fn to_mat2f(self) -> Mat2f {
434 Mat2f::from(self)
435 }
436
437 pub fn to_mat4f(self) -> Mat4f {
438 Mat4f::from(self)
439 }
440
441 pub fn to_mat3d(self) -> Mat3d {
442 Mat3d::new(
443 self.x_axis.to_vec3d(),
444 self.y_axis.to_vec3d(),
445 self.z_axis.to_vec3d(),
446 )
447 }
448}