rsm_lib/mat3.rs
1use num_traits::{
2 NumAssign, Float
3};
4
5use std::slice::{
6 Iter,
7 IterMut
8};
9
10use std::ops::{
11 Neg, Add, Sub, Mul, Div,
12 Index, IndexMut
13};
14
15use crate::vec2::Vec2;
16use crate::vec3::Vec3;
17
18use std::fmt;
19
20#[derive(Debug, Default, Copy, Clone, PartialEq)]
21pub struct Mat3<T> (
22 pub Vec3<T>,
23 pub Vec3<T>,
24 pub Vec3<T>
25);
26
27impl<T> Mat3<T>
28where
29 T: NumAssign + Copy,
30{
31 /// Creates a new 3x3 matrix from three columns.
32 ///
33 /// This constructor initializes a `Mat3` instance with the provided column vectors.
34 /// Each column vector is represented as a `Vec3<T>`.
35 ///
36 /// # Parameters
37 /// - `col0`: The first column of the matrix.
38 /// - `col1`: The second column of the matrix.
39 /// - `col2`: The third column of the matrix.
40 ///
41 /// # Returns
42 /// Returns a `Mat3` instance with the specified columns.
43 ///
44 /// # Examples
45 /// ```
46 /// let col0 = Vec3::new(1.0, 0.0, 0.0);
47 /// let col1 = Vec3::new(0.0, 1.0, 0.0);
48 /// let col2 = Vec3::new(0.0, 0.0, 1.0);
49 /// let matrix = Mat3::new(&col0, &col1, &col2);
50 /// ```
51 #[inline]
52 pub fn new(col0: &Vec3<T>, col1: &Vec3<T>, col2: &Vec3<T>) -> Self {
53 Self(col0.clone(), col1.clone(), col2.clone())
54 }
55
56 /// Creates a 3x3 zero matrix.
57 ///
58 /// This constructor initializes a `Mat3` instance where all elements are set to zero.
59 ///
60 /// # Returns
61 /// Returns a `Mat3` instance where all columns are zero vectors.
62 ///
63 /// # Examples
64 /// ```
65 /// let matrix = Mat3::zero();
66 /// ```
67 #[inline]
68 pub fn zero() -> Self {
69 Self(
70 Vec3::zero(),
71 Vec3::zero(),
72 Vec3::zero(),
73 )
74 }
75
76 /// Creates an identity matrix.
77 ///
78 /// This constructor initializes a `Mat3` instance as the identity matrix, which has `1`s on the diagonal
79 /// and `0`s elsewhere. This is useful for matrix transformations where the identity matrix represents no change.
80 ///
81 /// # Returns
82 /// Returns a `Mat3` instance representing the identity matrix.
83 ///
84 /// # Examples
85 /// ```
86 /// let matrix = Mat3::identity();
87 /// ```
88 #[inline]
89 pub fn identity() -> Self {
90 Self(
91 Vec3::new(T::one(), T::zero(), T::zero()),
92 Vec3::new(T::zero(), T::one(), T::zero()),
93 Vec3::new(T::zero(), T::zero(), T::one()),
94 )
95 }
96
97 /// Computes the transpose of the matrix.
98 ///
99 /// This function returns a new `Mat3` instance where the rows and columns of the original matrix are swapped.
100 ///
101 /// # Returns
102 /// Returns the transpose of the matrix.
103 ///
104 /// # Examples
105 /// ```
106 /// let matrix = Mat3::identity();
107 /// let transposed = matrix.transpose();
108 /// ```
109 #[inline]
110 pub fn transpose(&self) -> Self {
111 Self(
112 Vec3::new(self.0.x, self.1.x, self.2.x),
113 Vec3::new(self.0.y, self.1.y, self.2.y),
114 Vec3::new(self.0.z, self.1.z, self.2.z),
115 )
116 }
117
118 /// Calculates the determinant of the matrix.
119 ///
120 /// This function computes the determinant of the 3x3 matrix, which can be used to determine if the matrix
121 /// is invertible (a non-zero determinant indicates the matrix is invertible).
122 ///
123 /// # Returns
124 /// Returns the determinant of the matrix.
125 ///
126 /// # Examples
127 /// ```
128 /// let matrix = Mat3::identity();
129 /// let det = matrix.determinant();
130 /// assert_eq!(det, 1.0); // Determinant of the identity matrix
131 /// ```
132 pub fn determinant(&self) -> T {
133 let a00 = self.0.x;
134 let a01 = self.0.y;
135 let a02 = self.0.z;
136 let a10 = self.1.x;
137 let a11 = self.1.y;
138 let a12 = self.1.z;
139 let a20 = self.2.x;
140 let a21 = self.2.y;
141 let a22 = self.2.z;
142
143 a00 * (a11 * a22 - a12 * a21) -
144 a01 * (a10 * a22 - a12 * a20) +
145 a02 * (a10 * a21 - a11 * a20)
146 }
147
148 /// Computes the trace of the matrix.
149 ///
150 /// The trace of a matrix is the sum of its diagonal elements. For a 3x3 matrix, it is the sum of `self.0.x`,
151 /// `self.1.y`, and `self.2.z`.
152 ///
153 /// # Returns
154 /// Returns the trace of the matrix.
155 ///
156 /// # Examples
157 /// ```
158 /// let matrix = Mat3::identity();
159 /// let trace = matrix.trace();
160 /// assert_eq!(trace, 3.0); // Trace of the identity matrix
161 /// ```
162 pub fn trace(&self) -> T {
163 self.0.x + self.1.y + self.2.z
164 }
165
166 /// Multiplies the current matrix by another matrix.
167 ///
168 /// This function performs matrix multiplication with another `Mat3` instance. The resulting matrix is computed
169 /// as the dot products of rows from the first matrix and columns from the second matrix.
170 ///
171 /// # Parameters
172 /// - `other`: The matrix to multiply with.
173 ///
174 /// # Returns
175 /// Returns the product of the two matrices.
176 ///
177 /// # Examples
178 /// ```
179 /// let a = Mat3::identity();
180 /// let b = Mat3::identity();
181 /// let product = a.mul(&b);
182 /// assert_eq!(product, a); // Product of two identity matrices is an identity matrix
183 /// ```
184 pub fn mul(&self, other: &Self) -> Self {
185 let col0 = Vec3::new(
186 self.0.dot(&Vec3::new(other.0.x, other.1.x, other.2.x)),
187 self.1.dot(&Vec3::new(other.0.x, other.1.x, other.2.x)),
188 self.2.dot(&Vec3::new(other.0.x, other.1.x, other.2.x)),
189 );
190 let col1 = Vec3::new(
191 self.0.dot(&Vec3::new(other.0.y, other.1.y, other.2.y)),
192 self.1.dot(&Vec3::new(other.0.y, other.1.y, other.2.y)),
193 self.2.dot(&Vec3::new(other.0.y, other.1.y, other.2.y)),
194 );
195 let col2 = Vec3::new(
196 self.0.dot(&Vec3::new(other.0.z, other.1.z, other.2.z)),
197 self.1.dot(&Vec3::new(other.0.z, other.1.z, other.2.z)),
198 self.2.dot(&Vec3::new(other.0.z, other.1.z, other.2.z)),
199 );
200 Self::new(&col0, &col1, &col2)
201 }
202
203 /// Translates the matrix in 2D space.
204 ///
205 /// This function translates the matrix by adding the translation vector to the matrix's translation components.
206 ///
207 /// # Parameters
208 /// - `translate`: The 2D translation vector.
209 ///
210 /// # Examples
211 /// ```
212 /// let mut matrix = Mat3::identity();
213 /// let translate = Vec2::new(2.0, 3.0);
214 /// matrix.translate_2d(&translate);
215 /// ```
216 #[inline]
217 pub fn translate_2d(&mut self, translate: &Vec2<T>) {
218 self.2.x += translate.x;
219 self.2.y += translate.y;
220 }
221
222 /// Scales the matrix in 2D space.
223 ///
224 /// This function scales the matrix by multiplying the appropriate components of the matrix by the given scale vector.
225 ///
226 /// # Parameters
227 /// - `scale`: The 2D scale vector.
228 ///
229 /// # Examples
230 /// ```
231 /// let mut matrix = Mat3::identity();
232 /// let scale = Vec2::new(2.0, 3.0);
233 /// matrix.scale_2d(&scale);
234 /// ```
235 #[inline]
236 pub fn scale_2d(&mut self, scale: &Vec2<T>) {
237 self.0.x *= scale.x;
238 self.1.y *= scale.y;
239 }
240
241 /// Scales the matrix in 3D space.
242 ///
243 /// This function scales the matrix by multiplying each component of the matrix with the corresponding component
244 /// of the scale vector.
245 ///
246 /// # Parameters
247 /// - `scale`: The 3D scale vector.
248 ///
249 /// # Examples
250 /// ```
251 /// let mut matrix = Mat3::identity();
252 /// let scale = Vec3::new(2.0, 3.0, 4.0);
253 /// matrix.scale_3d(&scale);
254 /// ```
255 #[inline]
256 pub fn scale_3d(&mut self, scale: &Vec3<T>) {
257 self.0.x *= scale.x;
258 self.1.y *= scale.y;
259 self.2.z *= scale.z;
260 }
261}
262
263impl<T> Mat3<T>
264where
265 T: NumAssign + Float
266{
267 /// Computes the inverse of the matrix.
268 ///
269 /// This method calculates the inverse of the 3x3 matrix, if it exists. The inverse of a matrix is used to
270 /// reverse the effects of the matrix transformation. If the matrix is singular (i.e., its determinant is zero),
271 /// the function returns `None`.
272 ///
273 /// # Returns
274 /// Returns `Some(Self)` containing the inverse of the matrix if the determinant is non-zero; otherwise, returns `None`.
275 ///
276 /// # Examples
277 /// ```
278 /// let matrix = Mat3::identity();
279 /// let inverse = matrix.invert();
280 /// assert!(inverse.is_some()); // Identity matrix is invertible
281 /// ```
282 pub fn invert(&self) -> Option<Self> {
283 let a00 = self.0.x;
284 let a01 = self.0.y;
285 let a02 = self.0.z;
286 let a10 = self.1.x;
287 let a11 = self.1.y;
288 let a12 = self.1.z;
289 let a20 = self.2.x;
290 let a21 = self.2.y;
291 let a22 = self.2.z;
292
293 let b01 = a22 * a11 - a12 * a21;
294 let b11 = -a22 * a10 + a12 * a20;
295 let b21 = a21 * a10 - a11 * a20;
296
297 let det = a00 * b01 + a01 * b11 + a02 * b21;
298
299 if det.is_zero() {
300 return None;
301 }
302
303 let inv_det = T::one() / det;
304
305 Some(Self(
306 Vec3::new(b01 * inv_det, (-a22 * a01 + a02 * a21) * inv_det, (a12 * a01 - a02 * a11) * inv_det),
307 Vec3::new(b11 * inv_det, (a22 * a00 - a02 * a20) * inv_det, (-a12 * a00 + a02 * a10) * inv_det),
308 Vec3::new(b21 * inv_det, (-a21 * a00 + a01 * a20) * inv_det, (a11 * a00 - a01 * a10) * inv_det)
309 ))
310 }
311
312 /// Rotates the matrix by a given angle in 2D space.
313 ///
314 /// This method applies a 2D rotation transformation to the matrix. The rotation is counterclockwise by the
315 /// specified angle in radians. This transformation affects only the 2D components of the matrix.
316 ///
317 /// # Parameters
318 /// - `angle`: The angle of rotation in radians.
319 ///
320 /// # Examples
321 /// ```
322 /// let mut matrix = Mat3::identity();
323 /// matrix.rotate_2d(std::f64::consts::FRAC_PI_2); // Rotate by 90 degrees
324 /// ```
325 pub fn rotate_2d(&mut self, angle: T) {
326 let c = angle.cos();
327 let s = angle.sin();
328
329 let x0 = self[0][0] * c + self[1][0] * s;
330 let x1 = self[0][1] * c + self[1][1] * s;
331 let x2 = self[0][2] * c + self[1][2] * s;
332
333 let y0 = -self[0][0] * s + self[1][0] * c;
334 let y1 = -self[0][1] * s + self[1][1] * c;
335 let y2 = -self[0][2] * s + self[1][2] * c;
336
337 self[0][0] = x0;
338 self[0][1] = x1;
339 self[0][2] = x2;
340
341 self[1][0] = y0;
342 self[1][1] = y1;
343 self[1][2] = y2;
344 }
345
346 /// Rotates the matrix by a given angle around a specified 3D axis.
347 ///
348 /// This method applies a 3D rotation transformation to the matrix. The rotation is counterclockwise around
349 /// the specified axis by the given angle in radians. The axis is normalized before applying the rotation.
350 ///
351 /// # Parameters
352 /// - `axis`: The 3D axis around which to rotate.
353 /// - `angle`: The angle of rotation in radians.
354 ///
355 /// # Examples
356 /// ```
357 /// let mut matrix = Mat3::identity();
358 /// let axis = Vec3::new(0.0, 0.0, 1.0); // Rotation around the Z axis
359 /// matrix.rotate_3d(axis, std::f64::consts::FRAC_PI_2); // Rotate by 90 degrees
360 /// ```
361 pub fn rotate_3d(&mut self, mut axis: Vec3<T>, angle: T) {
362 let len_sq = axis.length_squared();
363 if !(len_sq.is_one() || len_sq.is_zero()) {
364 let inv_len = len_sq.sqrt();
365 axis *= inv_len;
366 }
367
368 let s = angle.sin();
369 let c = angle.cos();
370 let t = T::one() - c;
371
372 let rotation = Self::new(
373 &Vec3::new(
374 axis.x * axis.x * t + c,
375 axis.y * axis.x * t + axis.z * s,
376 axis.z * axis.x * t - axis.y * s,
377 ),
378 &Vec3::new(
379 axis.x * axis.y * t - axis.z * s,
380 axis.y * axis.y * t + c,
381 axis.z * axis.y * t + axis.x * s,
382 ),
383 &Vec3::new(
384 axis.x * axis.z * t + axis.y * s,
385 axis.y * axis.z * t - axis.x * s,
386 axis.z * axis.z * t + c,
387 )
388 );
389
390 *self = self.mul(rotation);
391 }
392
393 /// Rotates the matrix by a given angle around the X-axis in 3D space.
394 ///
395 /// This method applies a 3D rotation transformation to the matrix around the X-axis. The rotation is
396 /// counterclockwise by the specified angle in radians.
397 ///
398 /// # Parameters
399 /// - `angle`: The angle of rotation in radians.
400 ///
401 /// # Examples
402 /// ```
403 /// let mut matrix = Mat3::identity();
404 /// matrix.rotate_x_3d(std::f64::consts::FRAC_PI_2); // Rotate around X-axis by 90 degrees
405 /// ```
406 pub fn rotate_x_3d(&mut self, angle: T) {
407 let c = angle.cos();
408 let s = angle.sin();
409
410 let y0 = self[1][0] * c + self[2][0] * s;
411 let y1 = self[1][1] * c + self[2][1] * s;
412 let y2 = self[1][2] * c + self[2][2] * s;
413
414 let z0 = -self[1][0] * s + self[2][0] * c;
415 let z1 = -self[1][1] * s + self[2][1] * c;
416 let z2 = -self[1][2] * s + self[2][2] * c;
417
418 self[1][0] = y0;
419 self[1][1] = y1;
420 self[1][2] = y2;
421
422 self[2][0] = z0;
423 self[2][1] = z1;
424 self[2][2] = z2;
425 }
426
427 /// Rotates the matrix by a given angle around the Y-axis in 3D space.
428 ///
429 /// This method applies a 3D rotation transformation to the matrix around the Y-axis. The rotation is
430 /// counterclockwise by the specified angle in radians.
431 ///
432 /// # Parameters
433 /// - `angle`: The angle of rotation in radians.
434 ///
435 /// # Examples
436 /// ```
437 /// let mut matrix = Mat3::identity();
438 /// matrix.rotate_y_3d(std::f64::consts::FRAC_PI_2); // Rotate around Y-axis by 90 degrees
439 /// ```
440 pub fn rotate_y_3d(&mut self, angle: T) {
441 let c = angle.cos();
442 let s = angle.sin();
443
444 let x0 = self[0][0] * c - self[2][0] * s;
445 let x1 = self[0][1] * c - self[2][1] * s;
446 let x2 = self[0][2] * c - self[2][2] * s;
447
448 let z0 = self[0][0] * s + self[2][0] * c;
449 let z1 = self[0][1] * s + self[2][1] * c;
450 let z2 = self[0][2] * s + self[2][2] * c;
451
452 self[0][0] = x0;
453 self[0][1] = x1;
454 self[0][2] = x2;
455
456 self[2][0] = z0;
457 self[2][1] = z1;
458 self[2][2] = z2;
459 }
460
461 /// Rotates the matrix by a given angle around the Z-axis in 3D space.
462 ///
463 /// This method applies a 3D rotation transformation to the matrix around the Z-axis. The rotation is
464 /// counterclockwise by the specified angle in radians.
465 ///
466 /// # Parameters
467 /// - `angle`: The angle of rotation in radians.
468 ///
469 /// # Examples
470 /// ```
471 /// let mut matrix = Mat3::identity();
472 /// matrix.rotate_z_3d(std::f64::consts::FRAC_PI_2); // Rotate around Z-axis by 90 degrees
473 /// ```
474 pub fn rotate_z_3d(&mut self, angle: T) {
475 let c = angle.cos();
476 let s = angle.sin();
477
478 let x0 = self[0][0] * c + self[1][0] * s;
479 let x1 = self[0][1] * c + self[1][1] * s;
480 let x2 = self[0][2] * c + self[1][2] * s;
481
482 let y0 = -self[0][0] * s + self[1][0] * c;
483 let y1 = -self[0][1] * s + self[1][1] * c;
484 let y2 = -self[0][2] * s + self[1][2] * c;
485
486 self[0][0] = x0;
487 self[0][1] = x1;
488 self[0][2] = x2;
489
490 self[1][0] = y0;
491 self[1][1] = y1;
492 self[1][2] = y2;
493 }
494}
495
496impl<T> fmt::Display for Mat3<T>
497where
498 T: fmt::Display,
499{
500 #[inline]
501 fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
502 writeln!(f, "[{}, {}, {}]", self.0, self.1, self.2)
503 }
504}
505
506impl<T> Index<usize> for Mat3<T> {
507 type Output = Vec3<T>;
508
509 #[inline]
510 fn index(&self, index: usize) -> &Self::Output {
511 match index {
512 0 => &self.0,
513 1 => &self.1,
514 2 => &self.2,
515 _ => panic!("Index out of bounds for Mat3"),
516 }
517 }
518}
519
520impl<T> IndexMut<usize> for Mat3<T> {
521 #[inline]
522 fn index_mut(&mut self, index: usize) -> &mut Self::Output {
523 match index {
524 0 => &mut self.0,
525 1 => &mut self.1,
526 2 => &mut self.2,
527 _ => panic!("Index out of bounds for Mat3"),
528 }
529 }
530}
531
532impl<'a, T> IntoIterator for &'a Mat3<T>
533where
534 T: NumAssign + Copy
535{
536 type Item = &'a Vec3<T>;
537 type IntoIter = Iter<'a, Vec3<T>>;
538
539 #[inline]
540 fn into_iter(self) -> Self::IntoIter {
541 let slice: &[Vec3<T>; 3] = unsafe { std::mem::transmute(self) };
542 slice.iter()
543 }
544}
545
546impl<'a, T> IntoIterator for &'a mut Mat3<T>
547where
548 T: NumAssign + Copy
549{
550 type Item = &'a mut Vec3<T>;
551 type IntoIter = IterMut<'a, Vec3<T>>;
552
553 #[inline]
554 fn into_iter(self) -> Self::IntoIter {
555 let slice: &mut [Vec3<T>; 3] = unsafe { std::mem::transmute(self) };
556 slice.iter_mut()
557 }
558}
559
560impl<T> Neg for Mat3<T>
561where
562 T: NumAssign + Copy + Neg<Output = T>,
563{
564 type Output = Self;
565
566 fn neg(self) -> Self::Output {
567 Self (
568 -self.0,
569 -self.1,
570 -self.2
571 )
572 }
573}
574
575impl<T> Add<Mat3<T>> for Mat3<T>
576where
577 T: NumAssign + Copy,
578{
579 type Output = Self;
580
581 fn add(self, other: Self) -> Self {
582 Self (
583 self.0 + other.0,
584 self.1 + other.1,
585 self.2 + other.2
586 )
587 }
588}
589
590impl<T> Sub<Mat3<T>> for Mat3<T>
591where
592 T: NumAssign + Copy,
593{
594 type Output = Self;
595
596 fn sub(self, other: Self) -> Self {
597 Self (
598 self.0 - other.0,
599 self.1 - other.1,
600 self.2 - other.2
601 )
602 }
603}
604
605impl<T> Mul<Mat3<T>> for Mat3<T>
606where
607 T: NumAssign + Copy,
608{
609 type Output = Mat3<T>;
610
611 #[inline]
612 fn mul(self, other: Mat3<T>) -> Mat3<T> {
613 Self::mul(&self, &other)
614 }
615}
616
617impl<T> Mul<T> for Mat3<T>
618where
619 T: NumAssign + Copy,
620{
621 type Output = Self;
622
623 fn mul(self, scalar: T) -> Self {
624 Self (
625 self.0 * scalar,
626 self.1 * scalar,
627 self.2 * scalar
628 )
629 }
630}
631
632impl<T> Div<T> for Mat3<T>
633where
634 T: NumAssign + Copy,
635{
636 type Output = Self;
637
638 fn div(self, scalar: T) -> Self {
639 Self (
640 self.0 / scalar,
641 self.1 / scalar,
642 self.2 / scalar
643 )
644 }
645}