1use std::{f64::consts::PI, fmt, fmt::{
2 Display,
3 Formatter
4 }, ops::{
5 Add,
6 AddAssign,
7 Index,
8 IndexMut,
9 Sub,
10 SubAssign,
11 Mul,
12 Div,
13 Neg
14 }};
15use crate::{Float, functions::utils::eq_eps_f64, matrix, vec3, vector, vector3::Vector3};
16use super::{matrix::Matrix, matrix4::Matrix4, vector::Vector, vector4::Vector4};
17
18
19
20#[macro_export]
21macro_rules! matrix3 {
22 (
23 $x1:expr, $y1:expr, $z1:expr,
24 $x2:expr, $y2:expr, $z2:expr,
25 $x3:expr, $y3:expr, $z3:expr
26 ) => {
27 {
28 Matrix3::new([
29 $x1, $y1, $z1,
30 $x2, $y2, $z2,
31 $x3, $y3, $z3
32 ])
33 }
34 };
35}
36
37
38
39#[macro_export]
40macro_rules! compose_basis3 {
41 (
42 $x:expr,
43 $y:expr,
44 $z:expr
45 ) => {
46 {
47 Matrix3::new([
48 $x[0], $y[0], $z[0],
49 $x[1], $y[1], $z[1],
50 $x[2], $y[2], $z[2]
51 ])
52 }
53 };
54}
55
56
57
58#[derive(Clone, Debug)]
59pub struct Matrix3 {
60 pub data: [Float; 9]
61}
62
63
64
65impl Display for Matrix3 {
66 fn fmt(&self, f: &mut Formatter<'_>) -> fmt::Result {
67 write!(f,
68 "\n [{}, {}, {}] \n [{}, {}, {}] \n [{}, {}, {}] \n",
69 self[0], self[1], self[2],
70 self[3], self[4], self[5],
71 self[6], self[7], self[8]
72 )
73 }
74}
75
76
77
78impl Index<usize> for Matrix3 {
79 type Output = Float;
80
81 fn index(&self, idx:usize) -> &Float {
82 &self.data[idx]
83 }
84}
85
86
87
88impl IndexMut<usize> for Matrix3 {
89 fn index_mut(&mut self, idx:usize) -> &mut Float {
90 &mut self.data[idx]
91 }
92}
93
94
95
96impl Matrix3 {
97
98 pub fn new(data: [Float; 9]) -> Matrix3 {
99 Matrix3 {
100 data
101 }
102 }
103
104
105
106 pub fn id() -> Matrix3 {
107 Matrix3::new([
108 1., 0., 0.,
109 0., 1., 0.,
110 0., 0., 1.
111 ])
112 }
113
114
115
116 pub fn transpose(&self) -> Matrix3 {
117 Matrix3::new([
118 self[0], self[3], self[6],
119 self[1], self[4], self[7],
120 self[2], self[5], self[8]
121 ])
122 }
123
124
125
126 pub fn det(&self) -> Float {
127 self[0]*(self[4] * self[8] - self[5] * self[7]) -
128 self[1]*(self[3] * self[8] - self[5] * self[6]) +
129 self[2]*(self[3] * self[7] - self[6] * self[4])
130 }
131
132
133
134 pub fn inv(&self) -> Option<Matrix3> {
135 let a = self;
136 let s = a.det();
137
138 let a11 = a[0];
139 let a12 = a[1];
140 let a13 = a[2];
141
142 let a21 = a[3];
143 let a22 = a[4];
144 let a23 = a[5];
145
146 let a31 = a[6];
147 let a32 = a[7];
148 let a33 = a[8];
149
150 if s==0. {
151 return None;
152 }
153
154 let xx = a22 * a33 - a23 * a32;
155 let xy = a12 * a33 - a13 * a32;
156 let xz = a12 * a23 - a13 * a22;
157
158 let yx = a21 * a33 - a23 * a31;
159 let yy = a11 * a33 - a13 * a31;
160 let yz = a11 * a23 - a13 * a21;
161
162 let zx = a21 * a32 - a22 * a31;
163 let zy = a11 * a32 - a12 * a31;
164 let zz = a11 * a22 - a12 * a21;
165
166 let mut m = matrix3![
167 xx, -yx, zx,
168 -xy, yy,-zy,
169 xz, -yz, zz
170 ];
171
172 m = m.transpose();
173
174 m = m * (1./s);
175
176 Some(m)
177 }
178
179
180
181 pub fn rotation(rad: f64) -> Matrix3 {
182
183 let z = matrix3![
184 rad.cos(), -rad.sin(), 0.,
185 rad.sin(), rad.cos(), 0.,
186 0., 0., 1.
187 ];
188
189 z
190 }
191
192
193
194 pub fn scale(s: f64) -> Matrix3 {
195 matrix3![
196 s, 0., 0.,
197 0., s, 0.,
198 0., 0., 1.
199 ]
200 }
201
202
203
204 pub fn translate(x: f64, y: f64) -> Matrix3 {
205 matrix3![
206 1., 0., x,
207 0., 1., y,
208 0., 0., 1.
209 ]
210 }
211
212
213
214 pub fn sheer(xy:f64, yx:f64) -> Matrix3 {
215 matrix3![
216 1., yx, 0.,
217 xy, 1., 0.,
218 0., 0., 1.
219 ]
220 }
221
222
223
224 pub fn cross(v:&Vector3) -> Matrix3 {
225 matrix3![
226 0., -v.z, v.y,
227 v.z, 0., -v.x,
228 -v.y, v.x, 0.
229 ]
230 }
231
232
233
234 pub fn from_basis(
235 x: Vector3,
236 y: Vector3,
237 z: Vector3
238 ) -> Matrix3 {
239 let r = compose_basis3![
240 &x,
241 &y,
242 &z
243 ];
244
245 r
246 }
247
248
249
250 pub fn into_basis(&self) -> [Vector3; 3] {
251
252 let x = vec3![self[0], self[3], self[6]];
253 let y = vec3![self[1], self[4], self[7]];
254 let z = vec3![self[2], self[5], self[8]];
255
256 [x,y,z]
257 }
258
259
260
261 pub fn orthonormal(v:&Vector3) -> Matrix3 {
262 let mut x = *v;
263
264 let mut y = Vector3::rand(10.);
265
266 while y * x == 0. {
267 y = Vector3::rand(10.);
268 }
269
270 let p = y.project_on(&x);
271
272 y -= p;
273
274 let mut z = x.cross(&y);
275
276 x.normalize();
277 y.normalize();
278 z.normalize();
279
280 Matrix3::from_basis(x, y, z)
281 }
282
283
284
285 pub fn apply(&mut self, f: fn(f64) -> f64) {
286 self[0] = f(self[0]);
287 self[1] = f(self[1]);
288 self[2] = f(self[2]);
289 self[3] = f(self[3]);
290 self[4] = f(self[4]);
291 self[5] = f(self[5]);
292 self[6] = f(self[6]);
293 self[7] = f(self[7]);
294 self[8] = f(self[8]);
295 }
296
297
298
299 pub fn project(&self, b:&Vector3) -> Vector3 {
300
301 let A = self;
302
303 let mut At = A.clone();
304
305 At = At.transpose();
306
307 let Atb: Vector3 = &At * b;
308
309 let AtA: Matrix3 = &At * A;
310
311 let x = AtA.solve(&Atb);
312
313 A * x.unwrap()
314 }
315
316
317
318 pub fn solve(&self, b: &Vector3) -> Option<Vector3> {
319
320 let A: Matrix<f64> = self.into();
321
322 let b: Vector<f64> = b.into();
323
324 let lu = A.lu();
325
326 let result = A.solve(&b, &lu);
327
328 if result.is_some() {
329 let v: Vector3 = result.unwrap().into();
330 Some(v)
331 } else {
332 None
333 }
334 }
335}
336
337
338
339fn add(a:&Matrix3, b:&Matrix3) -> Matrix3 {
340 Matrix3::new([
341 a[0] + b[0], a[1] + b[1], a[2] + b[2],
342 a[3] + b[3], a[4] + b[4], a[5] + b[5],
343 a[6] + b[6], a[7] + b[7], a[8] + b[8]
344 ])
345}
346
347
348
349fn sub(a:&Matrix3, b:&Matrix3) -> Matrix3 {
350 Matrix3::new([
351 a[0] - b[0], a[1] - b[1], a[2] - b[2],
352 a[3] - b[3], a[4] - b[4], a[5] - b[5],
353 a[6] - b[6], a[7] - b[7], a[8] - b[8]
354 ])
355}
356
357
358
359fn mul(a:&Matrix3, b:&Matrix3) -> Matrix3 {
360 Matrix3::new([
361 a[0] * b[0] + a[1] * b[3] + a[2] * b[6], a[0] * b[1] + a[1] * b[4] + a[2] * b[7], a[0] * b[2] + a[1] * b[5] + a[2] * b[8],
362 a[3] * b[0] + a[4] * b[3] + a[5] * b[6], a[3] * b[1] + a[4] * b[4] + a[5] * b[7], a[3] * b[2] + a[4] * b[5] + a[5] * b[8],
363 a[6] * b[0] + a[7] * b[3] + a[8] * b[6], a[6] * b[1] + a[7] * b[4] + a[8] * b[7], a[6] * b[2] + a[7] * b[5] + a[8] * b[8]
364 ])
365}
366
367
368
369fn mul_v(a:&Matrix3, v:&Vector3) -> Vector3 {
370 Vector3::new(
371 a[0] * v.x + a[1] * v.y + a[2] * v.z,
372 a[3] * v.x + a[4] * v.y + a[5] * v.z,
373 a[6] * v.x + a[7] * v.y + a[8] * v.z
374 )
375}
376
377
378
379impl Add for &Matrix3 {
380 type Output = Matrix3;
381
382 fn add(self, b:&Matrix3) -> Matrix3 {
383 add(self, b)
384 }
385}
386
387
388
389impl Add for Matrix3 {
390 type Output = Matrix3;
391
392 fn add(self, b:Matrix3) -> Matrix3 {
393 add(&self, &b)
394 }
395}
396
397
398
399impl Sub for &Matrix3 {
400 type Output = Matrix3;
401
402 fn sub(self, b:&Matrix3) -> Matrix3 {
403 sub(self, b)
404 }
405}
406
407
408
409impl Sub for Matrix3 {
410 type Output = Matrix3;
411
412 fn sub(self, b:Matrix3) -> Matrix3 {
413 sub(&self, &b)
414 }
415}
416
417
418
419impl Mul for &Matrix3 {
420 type Output = Matrix3;
421
422 fn mul(self, b: &Matrix3) -> Matrix3 {
423 mul(self, b)
424 }
425}
426
427
428
429impl Mul for Matrix3 {
430 type Output = Matrix3;
431
432 fn mul(self, b: Matrix3) -> Matrix3 {
433 mul(&self, &b)
434 }
435}
436
437
438
439impl Mul <Vector3> for &Matrix3 {
440 type Output = Vector3;
441
442 fn mul(self, v:Vector3) -> Vector3 {
443 mul_v(&self, &v)
444 }
445}
446
447
448
449impl Mul <Vector3> for Matrix3 {
450 type Output = Vector3;
451
452 fn mul(self, v:Vector3) -> Vector3 {
453 mul_v(&self, &v)
454 }
455}
456
457
458
459impl Mul <&Vector3> for Matrix3 {
460 type Output = Vector3;
461
462 fn mul(self, v:&Vector3) -> Vector3 {
463 mul_v(&self, v)
464 }
465}
466
467
468
469impl Mul <&Vector3> for &Matrix3 {
470 type Output = Vector3;
471
472 fn mul(self, v:&Vector3) -> Vector3 {
473 mul_v(self, v)
474 }
475}
476
477
478
479impl Mul <Float> for Matrix3 {
480 type Output = Matrix3;
481
482 fn mul(mut self, s:f64) -> Matrix3 {
483 self[0] *= s;
484 self[1] *= s;
485 self[2] *= s;
486 self[3] *= s;
487 self[4] *= s;
488 self[5] *= s;
489 self[6] *= s;
490 self[7] *= s;
491 self[8] *= s;
492 self
493 }
494}
495
496
497
498impl Div <Float> for Matrix3 {
499 type Output = Matrix3;
500
501 fn div(mut self, s:f64) -> Matrix3 {
502 self[0] /= s;
503 self[1] /= s;
504 self[2] /= s;
505 self[3] /= s;
506 self[4] /= s;
507 self[5] /= s;
508 self[6] /= s;
509 self[7] /= s;
510 self[8] /= s;
511 self
512 }
513}
514
515
516
517impl AddAssign for Matrix3 {
518 fn add_assign(&mut self, b:Matrix3) {
519 self[0] += b[0];
520 self[1] += b[1];
521 self[2] += b[2];
522 self[3] += b[3];
523 self[4] += b[4];
524 self[5] += b[5];
525 self[6] += b[6];
526 self[7] += b[7];
527 self[8] += b[8];
528 }
529}
530
531
532
533impl SubAssign for Matrix3 {
534 fn sub_assign(&mut self, b:Matrix3) {
535 self[0] -= b[0];
536 self[1] -= b[1];
537 self[2] -= b[2];
538 self[3] -= b[3];
539 self[4] -= b[4];
540 self[5] -= b[5];
541 self[6] -= b[6];
542 self[7] -= b[7];
543 self[8] -= b[8];
544 }
545}
546
547
548
549impl PartialEq for Matrix3 {
550 fn eq(&self, b: &Matrix3) -> bool {
551 self[0] == b[0] &&
552 self[1] == b[1] &&
553 self[2] == b[2] &&
554 self[3] == b[3] &&
555 self[4] == b[4] &&
556 self[5] == b[5] &&
557 self[6] == b[6] &&
558 self[7] == b[7] &&
559 self[8] == b[8]
560 }
561}
562
563
564
565impl Eq for Matrix3 {}
566
567
568
569impl Neg for Matrix3 {
570
571 type Output = Matrix3;
572
573 fn neg(mut self) -> Self {
574 self[0] *= -1.;
575 self[1] *= -1.;
576 self[2] *= -1.;
577 self[3] *= -1.;
578 self[4] *= -1.;
579 self[5] *= -1.;
580 self[6] *= -1.;
581 self[7] *= -1.;
582 self[8] *= -1.;
583 self
584 }
585}
586
587
588
589impl From<Matrix<f64>> for Matrix3 {
590 fn from(m: Matrix<f64>) -> Matrix3 {
591 matrix3![
592 m[[0,0]], m[[0,1]], m[[0,2]],
593 m[[1,0]], m[[1,1]], m[[1,2]],
594 m[[2,0]], m[[2,1]], m[[2,2]]
595 ]
596 }
597}
598
599
600
601impl From<&Matrix<f64>> for Matrix3 {
602 fn from(m: &Matrix<f64>) -> Matrix3 {
603 matrix3![
604 m[[0,0]], m[[0,1]], m[[0,2]],
605 m[[1,0]], m[[1,1]], m[[1,2]],
606 m[[2,0]], m[[2,1]], m[[2,2]]
607 ]
608 }
609}
610
611
612
613impl From<Matrix4> for Matrix3 {
614 fn from(m: Matrix4) -> Matrix3 {
615 matrix3![
616 m[0], m[1], m[2],
617 m[4], m[5], m[6],
618 m[8], m[9], m[10]
619 ]
620 }
621}
622
623
624
625impl From<&Matrix4> for Matrix3 {
626 fn from(m: &Matrix4) -> Matrix3 {
627 matrix3![
628 m[0], m[1], m[2],
629 m[4], m[5], m[6],
630 m[8], m[9], m[10]
631 ]
632 }
633}
634
635
636
637mod tests {
638 use std::{f32::EPSILON as EP, f64::EPSILON, f64::consts::PI};
639 use rand::Rng;
640 use crate::core::{matrix::Matrix, matrix4::Matrix4, vector4::Vector4};
641 use super::{Matrix3, vec3, Vector, matrix, vector, Vector3, eq_eps_f64};
642
643
644
645 #[test]
646 fn inv() {
647
648 let a = matrix3![
649 0.00000000046564561, 5., 44363463473474574.,
650 0.00000000046346561, 3., 44574574574573.,
651 0.0000000006461, 1., 7534534534534.
652 ];
653
654 let e = a.inv().unwrap();
655
656 println!("\n e is {} \n", e);
657
658 let id: Matrix3 = a * e;
659
660 println!("\n id is {} \n", id);
661 }
662
663
664
665 #[test]
666 fn projection() {
667
668 let test = 10; for i in 1..test {
671 let mut rng = rand::thread_rng();
672 let f = i as f64;
673 let c1 = rng.gen_range(-f, f);
674 let c2 = rng.gen_range(-f, f);
675
676 let rot: Matrix3 = Matrix4::rotation(PI / 4., 0., 0.).into();
677
678 let x = Vector3::rand(10.);
679 let y = Vector3::rand(10.);
680
681 let mut z = (&x * c1) + (&y * c2);
682
683 let h = x.cross(&y);
686
687 let a: f64 = &x * &h;
688 let b: f64 = &y * &h;
689 let c: f64 = &z * &h;
690
691 assert!(a < f32::EPSILON as f64, "a should be equal to zero");
692 assert!(b < f32::EPSILON as f64, "a should be equal to zero");
693 assert!(c < f32::EPSILON as f64, "a should be equal to zero");
694
695 let m: Matrix<f64> = Matrix3::from_basis(x,y,z).into();
696 assert_eq!(m.rank(), 2, "projection: m rank should be 2");
697
698 let ortho: Matrix3 = Matrix3::orthonormal(&x);
699 let v: Vector3 = &(ortho * rot) * y;
700 let result = m.solve(&v.into(), &(m.lu()));
701 assert!(result.is_none(), "projection: result should be none");
702
703 let mp: Matrix3 = m.clone().into();
704 let vp: Vector3 = mp.project(&v);
705
706 let dot: f64 = &vp * &h;
707 let angle = vp.angle(&h);
708
709 println!("\n dot is {} | angle is {} \n", dot, angle);
710
711 assert!(dot < f32::EPSILON as f64, "z * vp should be equal to zero");
712 assert!((angle - PI / 2.).abs() < f32::EPSILON as f64, "angle should be equal to pi / 2");
713
714 let e: Vector3 = &vp - &v;
715 let dot: f64 = &e * &x;
716 assert!(dot < f32::EPSILON as f64, "e * x should be equal to zero");
717
718 let mp: Matrix3 = m.clone().into();
719 let hp: Vector<f64> = mp.project(&h).into();
720
721 println!("\n hp is {} \n", hp);
722 assert!(hp.is_zero(), "hp should be zero");
723
724 let mp: Matrix3 = m.clone().into();
725 let vp2: Vector3 = mp.project(&vp);
726 let diff: Vector<f64> = (vp - vp2).into();
727 assert!(diff.is_zero(), "diff should be zero");
728
729 let id = Matrix3::id();
730 let r = id.project(&vp);
731
732 println!("\n r is {} | vp is {} \n", r, vp);
733 let diff2: Vector<f64> = (r - vp).into();
734 assert!(diff2.is_zero(), "diff2 should be zero");
735 }
736 }
737
738
739
740 #[test]
741 fn orthonormal() {
742 let v = Vector3::rand(1.);
743 let mut m = Matrix3::orthonormal(&v);
744 let basis = m.into_basis();
745
746 let xl = basis[0].length();
747 let yl = basis[1].length();
748 let zl = basis[2].length();
749
750 let a = v.angle(&basis[0]);
751 assert!(eq_eps_f64(a, 0.), "angle with first vector should be zero {}", a);
752
753 assert!(eq_eps_f64(xl, 1.), "xl length should be 1 {}", xl);
754 assert!(eq_eps_f64(yl, 1.), "yl length should be 1 {}", yl);
755 assert!(eq_eps_f64(zl, 1.), "zl length should be 1 {}", zl);
756
757 let a: f64 = &basis[0] * &basis[1];
758 let b: f64 = &basis[0] * &basis[2];
759 assert!(eq_eps_f64(a, 0.), "a should be 0 {}", a);
760 assert!(eq_eps_f64(b, 0.), "b should be 0 {}", b);
761
762 let c: f64 = &basis[1] * &basis[0];
763 let d: f64 = &basis[1] * &basis[2];
764 assert!(eq_eps_f64(c, 0.), "c should be 0 {}", c);
765 assert!(eq_eps_f64(d, 0.), "d should be 0 {}", d);
766
767 let e: f64 = &basis[2] * &basis[1];
768 let f: f64 = &basis[2] * &basis[0];
769 assert!(eq_eps_f64(e, 0.), "e should be 0 {}", e);
770 assert!(eq_eps_f64(f, 0.), "f should be 0 {}", f);
771
772 let y = m.clone();
773
774 m = m.transpose();
775
776 let id0 = Matrix3::id();
777 let mut id: Matrix3 = y * m;
778 fn round(n:f64) -> f64 {
781 let c = (2. as f64).powf(32.);
782 (n * c).round() / c
783 }
784
785 id.apply(round);
786
787 assert_eq!(id0, id, "product with transpose should be equal identity {}", id);
788 }
789
790
791
792 #[test]
793 fn operations() {
794
795 let id = Matrix3::id();
796
797 let inv = id.inv().unwrap();
800
801 let test = matrix3![
802 1., 2., 2.,
803 2., 1., 2.,
804 1., 2., 3.
805 ];
806
807 let test_inv = test.inv().unwrap();
810
811 let p = &test_inv * &test;
812
813 }
816}