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::{vec3d, Mat2d, Mat3f, Mat4d, Vec3d};
9
10#[derive(Clone, Copy, PartialEq, Debug)]
11pub struct Mat3d {
12 x_axis: Vec3d,
13 y_axis: Vec3d,
14 z_axis: Vec3d,
15}
16
17pub fn mat3d(
18 m00: f64,
19 m01: f64,
20 m02: f64,
21 m10: f64,
22 m11: f64,
23 m12: f64,
24 m20: f64,
25 m21: f64,
26 m22: f64,
27) -> Mat3d {
28 Mat3d::from_array([m00, m01, m02, m10, m11, m12, m20, m21, m22])
29}
30
31impl Display for Mat3d {
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 "Mat3d({}, {}, {} | {}, {}, {} | {}, {}, {})",
39 m00, m01, m02, m10, m11, m12, m20, m21, m22
40 )
41 }
42}
43
44impl Default for Mat3d {
45 fn default() -> Self {
46 Self {
47 x_axis: Vec3d::default(),
48 y_axis: Vec3d::default(),
49 z_axis: Vec3d::default(),
50 }
51 }
52}
53
54impl Add<Mat3d> for Mat3d {
55 type Output = Mat3d;
56
57 fn add(self, rhs: Mat3d) -> Self::Output {
58 Mat3d::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<f64> for Mat3d {
67 type Output = Mat3d;
68
69 fn add(self, rhs: f64) -> Self::Output {
70 Mat3d::new(self.x_axis + rhs, self.y_axis + rhs, self.z_axis + rhs)
71 }
72}
73
74impl Add<Mat3d> for f64 {
75 type Output = Mat3d;
76
77 fn add(self, rhs: Mat3d) -> Self::Output {
78 Mat3d::new(self + rhs.x_axis, self + rhs.y_axis, self + rhs.z_axis)
79 }
80}
81
82impl AddAssign<Mat3d> for Mat3d {
83 fn add_assign(&mut self, rhs: Mat3d) {
84 *self = *self + rhs;
85 }
86}
87
88impl AddAssign<f64> for Mat3d {
89 fn add_assign(&mut self, rhs: f64) {
90 *self = *self + rhs;
91 }
92}
93
94impl Sub<Mat3d> for Mat3d {
95 type Output = Mat3d;
96
97 fn sub(self, rhs: Mat3d) -> Self::Output {
98 Mat3d::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<f64> for Mat3d {
107 type Output = Mat3d;
108
109 fn sub(self, rhs: f64) -> Self::Output {
110 Mat3d::new(self.x_axis - rhs, self.y_axis - rhs, self.z_axis - rhs)
111 }
112}
113
114impl Sub<Mat3d> for f64 {
115 type Output = Mat3d;
116
117 fn sub(self, rhs: Mat3d) -> Self::Output {
118 Mat3d::new(self - rhs.x_axis, self - rhs.y_axis, self - rhs.z_axis)
119 }
120}
121
122impl SubAssign<Mat3d> for Mat3d {
123 fn sub_assign(&mut self, rhs: Mat3d) {
124 *self = *self - rhs;
125 }
126}
127
128impl SubAssign<f64> for Mat3d {
129 fn sub_assign(&mut self, rhs: f64) {
130 *self = *self - rhs;
131 }
132}
133
134impl Mul<Mat3d> for Mat3d {
135 type Output = Mat3d;
136
137 fn mul(self, rhs: Mat3d) -> Self::Output {
138 let m00 = self.x_axis.dot(vec3d(rhs[0][0], rhs[1][0], rhs[2][0]));
139 let m01 = self.x_axis.dot(vec3d(rhs[0][1], rhs[1][1], rhs[2][1]));
140 let m02 = self.x_axis.dot(vec3d(rhs[0][2], rhs[1][2], rhs[2][2]));
141
142 let m10 = self.y_axis.dot(vec3d(rhs[0][0], rhs[1][0], rhs[2][0]));
143 let m11 = self.y_axis.dot(vec3d(rhs[0][1], rhs[1][1], rhs[2][1]));
144 let m12 = self.y_axis.dot(vec3d(rhs[0][2], rhs[1][2], rhs[2][2]));
145
146 let m20 = self.z_axis.dot(vec3d(rhs[0][0], rhs[1][0], rhs[2][0]));
147 let m21 = self.z_axis.dot(vec3d(rhs[0][1], rhs[1][1], rhs[2][1]));
148 let m22 = self.z_axis.dot(vec3d(rhs[0][2], rhs[1][2], rhs[2][2]));
149
150 Mat3d::new(
151 vec3d(m00, m01, m02),
152 vec3d(m10, m11, m12),
153 vec3d(m20, m21, m22),
154 )
155 }
156}
157
158impl Mul<Vec3d> for Mat3d {
159 type Output = Vec3d;
160
161 fn mul(self, rhs: Vec3d) -> 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 vec3d(v0, v1, v2)
166 }
167}
168
169impl Mul<f64> for Mat3d {
170 type Output = Mat3d;
171
172 fn mul(self, rhs: f64) -> Self::Output {
173 Mat3d::new(self.x_axis * rhs, self.y_axis * rhs, self.z_axis * rhs)
174 }
175}
176
177impl Mul<Mat3d> for f64 {
178 type Output = Mat3d;
179
180 fn mul(self, rhs: Mat3d) -> Self::Output {
181 Mat3d::new(self * rhs.x_axis, self * rhs.y_axis, self * rhs.z_axis)
182 }
183}
184
185impl MulAssign<Mat3d> for Mat3d {
186 fn mul_assign(&mut self, rhs: Mat3d) {
187 *self = *self * rhs;
188 }
189}
190
191impl MulAssign<f64> for Mat3d {
192 fn mul_assign(&mut self, rhs: f64) {
193 *self = *self * rhs;
194 }
195}
196
197impl Div<f64> for Mat3d {
198 type Output = Mat3d;
199
200 fn div(self, rhs: f64) -> Self::Output {
201 Mat3d::new(self.x_axis / rhs, self.y_axis / rhs, self.z_axis / rhs)
202 }
203}
204
205impl DivAssign<f64> for Mat3d {
206 fn div_assign(&mut self, rhs: f64) {
207 *self = *self / rhs;
208 }
209}
210
211impl Index<usize> for Mat3d {
212 type Output = Vec3d;
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::Mat3d::index`: index out of bounds."),
220 }
221 }
222}
223
224impl IndexMut<usize> for Mat3d {
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::Mat3d::index_mut`: index out of bounds."),
231 }
232 }
233}
234
235impl Mat3d {
236 pub fn new(x_axis: Vec3d, y_axis: Vec3d, z_axis: Vec3d) -> Self {
237 Self {
238 x_axis,
239 y_axis,
240 z_axis,
241 }
242 }
243
244 pub fn zero() -> Self {
245 Self::new(
246 vec3d(0.0, 0.0, 0.0),
247 vec3d(0.0, 0.0, 0.0),
248 vec3d(0.0, 0.0, 0.0),
249 )
250 }
251
252 pub fn identity() -> Self {
253 Self::new(
254 vec3d(1.0, 0.0, 0.0),
255 vec3d(0.0, 1.0, 0.0),
256 vec3d(0.0, 0.0, 1.0),
257 )
258 }
259}
260
261impl Mat3d {
262 pub fn col(&self, index: usize) -> Vec3d {
263 match index {
264 0 => vec3d(self[0][0], self[1][0], self[2][0]),
265 1 => vec3d(self[0][1], self[1][1], self[2][1]),
266 2 => vec3d(self[0][2], self[1][2], self[2][2]),
267 _ => panic!("`rmath::algebra::Mat3d::col`: index out of bounds."),
268 }
269 }
270
271 pub fn row(&self, index: usize) -> Vec3d {
272 match index {
273 0 => self.x_axis,
274 1 => self.y_axis,
275 2 => self.z_axis,
276 _ => panic!("`rmath::algebra::Mat3d::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 vec3d(m00, m10, m20),
298 vec3d(m01, m11, m21),
299 vec3d(m02, m12, m22),
300 )
301 }
302
303 pub fn determinant(&self) -> f64 {
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() < f64::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 vec3d(a00, a10, a20),
337 vec3d(a01, a11, a21),
338 vec3d(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() < f64::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 vec3d(a00, a10, a20),
369 vec3d(a01, a11, a21),
370 vec3d(a02, a12, a22),
371 )
372 .mul(inv_det),
373 )
374 }
375}
376
377impl From<Mat2d> for Mat3d {
378 fn from(m: Mat2d) -> Self {
379 mat3d(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<Mat4d> for Mat3d {
384 fn from(m: Mat4d) -> Self {
385 mat3d(
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 Mat3d {
392 pub fn from_array(m: [f64; 9]) -> Self {
393 Self::new(
394 vec3d(m[0], m[1], m[2]),
395 vec3d(m[3], m[4], m[5]),
396 vec3d(m[6], m[7], m[8]),
397 )
398 }
399
400 pub fn from_array_2d(m: [[f64; 3]; 3]) -> Self {
401 Self::new(
402 vec3d(m[0][0], m[0][1], m[0][2]),
403 vec3d(m[1][0], m[1][1], m[1][2]),
404 vec3d(m[2][0], m[2][1], m[2][2]),
405 )
406 }
407
408 pub fn from_diagonal(diagonal: Vec3d) -> Self {
409 Self::new(
410 vec3d(diagonal.x(), 0.0, 0.0),
411 vec3d(0.0, diagonal.y(), 0.0),
412 vec3d(0.0, 0.0, diagonal.z()),
413 )
414 }
415}
416
417impl Mat3d {
418 pub fn to_array(self) -> [f64; 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) -> [[f64; 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_mat2d(self) -> Mat2d {
434 Mat2d::from(self)
435 }
436
437 pub fn to_mat4d(self) -> Mat4d {
438 Mat4d::from(self)
439 }
440
441 pub fn to_mat3f(self) -> Mat3f {
442 Mat3f::new(
443 self.x_axis.to_vec3f(),
444 self.y_axis.to_vec3f(),
445 self.z_axis.to_vec3f(),
446 )
447 }
448}