1use crate::{f32::math, swizzles::*, DMat2, Mat3, Mat3A, Vec2};
4use core::fmt;
5use core::iter::{Product, Sum};
6use core::ops::{Add, AddAssign, Div, DivAssign, Mul, MulAssign, Neg, Sub, SubAssign};
7
8use core::arch::aarch64::*;
9
10#[repr(C)]
11union UnionCast {
12 a: [f32; 4],
13 v: Mat2,
14}
15
16#[inline(always)]
18#[must_use]
19pub const fn mat2(x_axis: Vec2, y_axis: Vec2) -> Mat2 {
20 Mat2::from_cols(x_axis, y_axis)
21}
22
23#[derive(Clone, Copy)]
29#[cfg_attr(feature = "bytemuck", derive(bytemuck::Pod, bytemuck::Zeroable))]
30#[repr(transparent)]
31pub struct Mat2(pub(crate) float32x4_t);
32
33impl Mat2 {
34 pub const ZERO: Self = Self::from_cols(Vec2::ZERO, Vec2::ZERO);
36
37 pub const IDENTITY: Self = Self::from_cols(Vec2::X, Vec2::Y);
39
40 pub const NAN: Self = Self::from_cols(Vec2::NAN, Vec2::NAN);
42
43 #[allow(clippy::too_many_arguments)]
44 #[inline(always)]
45 #[must_use]
46 const fn new(m00: f32, m01: f32, m10: f32, m11: f32) -> Self {
47 unsafe {
48 UnionCast {
49 a: [m00, m01, m10, m11],
50 }
51 .v
52 }
53 }
54
55 #[inline(always)]
57 #[must_use]
58 pub const fn from_cols(x_axis: Vec2, y_axis: Vec2) -> Self {
59 unsafe {
60 UnionCast {
61 a: [x_axis.x, x_axis.y, y_axis.x, y_axis.y],
62 }
63 .v
64 }
65 }
66
67 #[inline]
71 #[must_use]
72 pub const fn from_cols_array(m: &[f32; 4]) -> Self {
73 Self::new(m[0], m[1], m[2], m[3])
74 }
75
76 #[inline]
79 #[must_use]
80 pub const fn to_cols_array(&self) -> [f32; 4] {
81 unsafe { *(self as *const Self as *const [f32; 4]) }
82 }
83
84 #[inline]
88 #[must_use]
89 pub const fn from_cols_array_2d(m: &[[f32; 2]; 2]) -> Self {
90 Self::from_cols(Vec2::from_array(m[0]), Vec2::from_array(m[1]))
91 }
92
93 #[inline]
96 #[must_use]
97 pub const fn to_cols_array_2d(&self) -> [[f32; 2]; 2] {
98 unsafe { *(self as *const Self as *const [[f32; 2]; 2]) }
99 }
100
101 #[doc(alias = "scale")]
103 #[inline]
104 #[must_use]
105 pub const fn from_diagonal(diagonal: Vec2) -> Self {
106 Self::new(diagonal.x, 0.0, 0.0, diagonal.y)
107 }
108
109 #[inline]
112 #[must_use]
113 pub fn from_scale_angle(scale: Vec2, angle: f32) -> Self {
114 let (sin, cos) = math::sin_cos(angle);
115 Self::new(cos * scale.x, sin * scale.x, -sin * scale.y, cos * scale.y)
116 }
117
118 #[inline]
120 #[must_use]
121 pub fn from_angle(angle: f32) -> Self {
122 let (sin, cos) = math::sin_cos(angle);
123 Self::new(cos, sin, -sin, cos)
124 }
125
126 #[inline]
128 #[must_use]
129 pub fn from_mat3(m: Mat3) -> Self {
130 Self::from_cols(m.x_axis.xy(), m.y_axis.xy())
131 }
132
133 #[inline]
140 #[must_use]
141 pub fn from_mat3_minor(m: Mat3, i: usize, j: usize) -> Self {
142 match (i, j) {
143 (0, 0) => Self::from_cols(m.y_axis.yz(), m.z_axis.yz()),
144 (0, 1) => Self::from_cols(m.y_axis.xz(), m.z_axis.xz()),
145 (0, 2) => Self::from_cols(m.y_axis.xy(), m.z_axis.xy()),
146 (1, 0) => Self::from_cols(m.x_axis.yz(), m.z_axis.yz()),
147 (1, 1) => Self::from_cols(m.x_axis.xz(), m.z_axis.xz()),
148 (1, 2) => Self::from_cols(m.x_axis.xy(), m.z_axis.xy()),
149 (2, 0) => Self::from_cols(m.x_axis.yz(), m.y_axis.yz()),
150 (2, 1) => Self::from_cols(m.x_axis.xz(), m.y_axis.xz()),
151 (2, 2) => Self::from_cols(m.x_axis.xy(), m.y_axis.xy()),
152 _ => panic!("index out of bounds"),
153 }
154 }
155
156 #[inline]
158 #[must_use]
159 pub fn from_mat3a(m: Mat3A) -> Self {
160 Self::from_cols(m.x_axis.xy(), m.y_axis.xy())
161 }
162
163 #[inline]
170 #[must_use]
171 pub fn from_mat3a_minor(m: Mat3A, i: usize, j: usize) -> Self {
172 match (i, j) {
173 (0, 0) => Self::from_cols(m.y_axis.yz(), m.z_axis.yz()),
174 (0, 1) => Self::from_cols(m.y_axis.xz(), m.z_axis.xz()),
175 (0, 2) => Self::from_cols(m.y_axis.xy(), m.z_axis.xy()),
176 (1, 0) => Self::from_cols(m.x_axis.yz(), m.z_axis.yz()),
177 (1, 1) => Self::from_cols(m.x_axis.xz(), m.z_axis.xz()),
178 (1, 2) => Self::from_cols(m.x_axis.xy(), m.z_axis.xy()),
179 (2, 0) => Self::from_cols(m.x_axis.yz(), m.y_axis.yz()),
180 (2, 1) => Self::from_cols(m.x_axis.xz(), m.y_axis.xz()),
181 (2, 2) => Self::from_cols(m.x_axis.xy(), m.y_axis.xy()),
182 _ => panic!("index out of bounds"),
183 }
184 }
185
186 #[inline]
192 #[must_use]
193 pub const fn from_cols_slice(slice: &[f32]) -> Self {
194 Self::new(slice[0], slice[1], slice[2], slice[3])
195 }
196
197 #[inline]
203 pub fn write_cols_to_slice(self, slice: &mut [f32]) {
204 slice[0] = self.x_axis.x;
205 slice[1] = self.x_axis.y;
206 slice[2] = self.y_axis.x;
207 slice[3] = self.y_axis.y;
208 }
209
210 #[inline]
216 #[must_use]
217 pub fn col(&self, index: usize) -> Vec2 {
218 match index {
219 0 => self.x_axis,
220 1 => self.y_axis,
221 _ => panic!("index out of bounds"),
222 }
223 }
224
225 #[inline]
231 pub fn col_mut(&mut self, index: usize) -> &mut Vec2 {
232 match index {
233 0 => &mut self.x_axis,
234 1 => &mut self.y_axis,
235 _ => panic!("index out of bounds"),
236 }
237 }
238
239 #[inline]
245 #[must_use]
246 pub fn row(&self, index: usize) -> Vec2 {
247 match index {
248 0 => Vec2::new(self.x_axis.x, self.y_axis.x),
249 1 => Vec2::new(self.x_axis.y, self.y_axis.y),
250 _ => panic!("index out of bounds"),
251 }
252 }
253
254 #[inline]
257 #[must_use]
258 pub fn is_finite(&self) -> bool {
259 self.x_axis.is_finite() && self.y_axis.is_finite()
260 }
261
262 #[inline]
264 #[must_use]
265 pub fn is_nan(&self) -> bool {
266 self.x_axis.is_nan() || self.y_axis.is_nan()
267 }
268
269 #[inline]
271 #[must_use]
272 pub fn transpose(&self) -> Self {
273 Self(unsafe {
274 vsetq_lane_f32(
275 vgetq_lane_f32(self.0, 2),
276 vsetq_lane_f32(vgetq_lane_f32(self.0, 1), self.0, 2),
277 1,
278 )
279 })
280 }
281
282 #[inline]
284 #[must_use]
285 pub fn determinant(&self) -> f32 {
286 unsafe {
287 let abcd = self.0;
288 let badc = vrev64q_f32(abcd);
289 let dcba = vextq_f32(badc, badc, 2);
290 let prod = vmulq_f32(abcd, dcba);
291 let det = vsubq_f32(prod, vdupq_laneq_f32(prod, 1));
292 vgetq_lane_f32(det, 0)
293 }
294 }
295
296 #[inline]
304 #[must_use]
305 pub fn inverse(&self) -> Self {
306 unsafe {
307 const SIGN: float32x4_t = crate::neon::f32x4_from_array([1.0, -1.0, -1.0, 1.0]);
308 let abcd = self.0;
309 let badc = vrev64q_f32(abcd);
310 let dcba = vextq_f32(badc, badc, 2);
311 let prod = vmulq_f32(abcd, dcba);
312 let sub = vsubq_f32(prod, vdupq_laneq_f32(prod, 1));
313 let det = vdupq_laneq_f32(sub, 0);
314 let tmp = vdivq_f32(SIGN, det);
315 glam_assert!(Mat2(tmp).is_finite());
316 let dbca = vsetq_lane_f32(
318 vgetq_lane_f32(abcd, 0),
319 vsetq_lane_f32(vgetq_lane_f32(abcd, 3), abcd, 0),
320 3,
321 );
322 Self(vmulq_f32(dbca, tmp))
323 }
324 }
325
326 #[inline]
328 #[must_use]
329 pub fn mul_vec2(&self, rhs: Vec2) -> Vec2 {
330 unsafe {
331 let abcd = self.0;
332 let xxyy = vld1q_f32([rhs.x, rhs.x, rhs.y, rhs.y].as_ptr());
333 let axbxcydy = vmulq_f32(abcd, xxyy);
334 let cydyaxbx = vextq_f32(axbxcydy, axbxcydy, 2);
336 let result = vaddq_f32(axbxcydy, cydyaxbx);
337 *(&result as *const float32x4_t as *const Vec2)
338 }
339 }
340
341 #[inline]
343 #[must_use]
344 pub fn mul_mat2(&self, rhs: &Self) -> Self {
345 self.mul(rhs)
346 }
347
348 #[inline]
350 #[must_use]
351 pub fn add_mat2(&self, rhs: &Self) -> Self {
352 self.add(rhs)
353 }
354
355 #[inline]
357 #[must_use]
358 pub fn sub_mat2(&self, rhs: &Self) -> Self {
359 self.sub(rhs)
360 }
361
362 #[inline]
364 #[must_use]
365 pub fn mul_scalar(&self, rhs: f32) -> Self {
366 Self(unsafe { vmulq_f32(self.0, vld1q_dup_f32(&rhs)) })
367 }
368
369 #[inline]
371 #[must_use]
372 pub fn div_scalar(&self, rhs: f32) -> Self {
373 Self(unsafe { vdivq_f32(self.0, vld1q_dup_f32(&rhs)) })
374 }
375
376 #[inline]
386 #[must_use]
387 pub fn abs_diff_eq(&self, rhs: Self, max_abs_diff: f32) -> bool {
388 self.x_axis.abs_diff_eq(rhs.x_axis, max_abs_diff)
389 && self.y_axis.abs_diff_eq(rhs.y_axis, max_abs_diff)
390 }
391
392 #[inline]
394 #[must_use]
395 pub fn abs(&self) -> Self {
396 Self::from_cols(self.x_axis.abs(), self.y_axis.abs())
397 }
398
399 #[inline]
400 pub fn as_dmat2(&self) -> DMat2 {
401 DMat2::from_cols(self.x_axis.as_dvec2(), self.y_axis.as_dvec2())
402 }
403}
404
405impl Default for Mat2 {
406 #[inline]
407 fn default() -> Self {
408 Self::IDENTITY
409 }
410}
411
412impl Add for Mat2 {
413 type Output = Self;
414 #[inline]
415 fn add(self, rhs: Self) -> Self {
416 Self(unsafe { vaddq_f32(self.0, rhs.0) })
417 }
418}
419
420impl Add<&Self> for Mat2 {
421 type Output = Self;
422 #[inline]
423 fn add(self, rhs: &Self) -> Self {
424 self.add(*rhs)
425 }
426}
427
428impl Add<&Mat2> for &Mat2 {
429 type Output = Mat2;
430 #[inline]
431 fn add(self, rhs: &Mat2) -> Mat2 {
432 (*self).add(*rhs)
433 }
434}
435
436impl Add<Mat2> for &Mat2 {
437 type Output = Mat2;
438 #[inline]
439 fn add(self, rhs: Mat2) -> Mat2 {
440 (*self).add(rhs)
441 }
442}
443
444impl AddAssign for Mat2 {
445 #[inline]
446 fn add_assign(&mut self, rhs: Self) {
447 *self = self.add(rhs);
448 }
449}
450
451impl AddAssign<&Self> for Mat2 {
452 #[inline]
453 fn add_assign(&mut self, rhs: &Self) {
454 self.add_assign(*rhs);
455 }
456}
457
458impl Sub for Mat2 {
459 type Output = Self;
460 #[inline]
461 fn sub(self, rhs: Self) -> Self {
462 Self(unsafe { vsubq_f32(self.0, rhs.0) })
463 }
464}
465
466impl Sub<&Self> for Mat2 {
467 type Output = Self;
468 #[inline]
469 fn sub(self, rhs: &Self) -> Self {
470 self.sub(*rhs)
471 }
472}
473
474impl Sub<&Mat2> for &Mat2 {
475 type Output = Mat2;
476 #[inline]
477 fn sub(self, rhs: &Mat2) -> Mat2 {
478 (*self).sub(*rhs)
479 }
480}
481
482impl Sub<Mat2> for &Mat2 {
483 type Output = Mat2;
484 #[inline]
485 fn sub(self, rhs: Mat2) -> Mat2 {
486 (*self).sub(rhs)
487 }
488}
489
490impl SubAssign for Mat2 {
491 #[inline]
492 fn sub_assign(&mut self, rhs: Self) {
493 *self = self.sub(rhs);
494 }
495}
496
497impl SubAssign<&Self> for Mat2 {
498 #[inline]
499 fn sub_assign(&mut self, rhs: &Self) {
500 self.sub_assign(*rhs);
501 }
502}
503
504impl Neg for Mat2 {
505 type Output = Self;
506 #[inline]
507 fn neg(self) -> Self::Output {
508 Self(unsafe { vnegq_f32(self.0) })
509 }
510}
511
512impl Neg for &Mat2 {
513 type Output = Mat2;
514 #[inline]
515 fn neg(self) -> Mat2 {
516 (*self).neg()
517 }
518}
519
520impl Mul for Mat2 {
521 type Output = Self;
522 #[inline]
523 fn mul(self, rhs: Self) -> Self {
524 unsafe {
525 let abcd = self.0;
526 let xxyy0 = vzip1q_f32(rhs.0, rhs.0);
527 let xxyy1 = vzip2q_f32(rhs.0, rhs.0);
528 let axbxcydy0 = vmulq_f32(abcd, xxyy0);
529 let axbxcydy1 = vmulq_f32(abcd, xxyy1);
530 let cydyaxbx0 = vextq_f32(axbxcydy0, axbxcydy0, 2);
531 let cydyaxbx1 = vextq_f32(axbxcydy1, axbxcydy1, 2);
532 let result0 = vaddq_f32(axbxcydy0, cydyaxbx0);
533 let result1 = vaddq_f32(axbxcydy1, cydyaxbx1);
534 Self(vreinterpretq_f32_u64(vsetq_lane_u64(
535 vgetq_lane_u64(vreinterpretq_u64_f32(result1), 0),
536 vreinterpretq_u64_f32(result0),
537 1,
538 )))
539 }
540 }
541}
542
543impl Mul<&Self> for Mat2 {
544 type Output = Self;
545 #[inline]
546 fn mul(self, rhs: &Self) -> Self {
547 self.mul(*rhs)
548 }
549}
550
551impl Mul<&Mat2> for &Mat2 {
552 type Output = Mat2;
553 #[inline]
554 fn mul(self, rhs: &Mat2) -> Mat2 {
555 (*self).mul(*rhs)
556 }
557}
558
559impl Mul<Mat2> for &Mat2 {
560 type Output = Mat2;
561 #[inline]
562 fn mul(self, rhs: Mat2) -> Mat2 {
563 (*self).mul(rhs)
564 }
565}
566
567impl MulAssign for Mat2 {
568 #[inline]
569 fn mul_assign(&mut self, rhs: Self) {
570 *self = self.mul(rhs);
571 }
572}
573
574impl MulAssign<&Self> for Mat2 {
575 #[inline]
576 fn mul_assign(&mut self, rhs: &Self) {
577 self.mul_assign(*rhs);
578 }
579}
580
581impl Mul<Vec2> for Mat2 {
582 type Output = Vec2;
583 #[inline]
584 fn mul(self, rhs: Vec2) -> Self::Output {
585 self.mul_vec2(rhs)
586 }
587}
588
589impl Mul<&Vec2> for Mat2 {
590 type Output = Vec2;
591 #[inline]
592 fn mul(self, rhs: &Vec2) -> Vec2 {
593 self.mul(*rhs)
594 }
595}
596
597impl Mul<&Vec2> for &Mat2 {
598 type Output = Vec2;
599 #[inline]
600 fn mul(self, rhs: &Vec2) -> Vec2 {
601 (*self).mul(*rhs)
602 }
603}
604
605impl Mul<Vec2> for &Mat2 {
606 type Output = Vec2;
607 #[inline]
608 fn mul(self, rhs: Vec2) -> Vec2 {
609 (*self).mul(rhs)
610 }
611}
612
613impl Mul<Mat2> for f32 {
614 type Output = Mat2;
615 #[inline]
616 fn mul(self, rhs: Mat2) -> Self::Output {
617 rhs.mul_scalar(self)
618 }
619}
620
621impl Mul<&Mat2> for f32 {
622 type Output = Mat2;
623 #[inline]
624 fn mul(self, rhs: &Mat2) -> Mat2 {
625 self.mul(*rhs)
626 }
627}
628
629impl Mul<&Mat2> for &f32 {
630 type Output = Mat2;
631 #[inline]
632 fn mul(self, rhs: &Mat2) -> Mat2 {
633 (*self).mul(*rhs)
634 }
635}
636
637impl Mul<Mat2> for &f32 {
638 type Output = Mat2;
639 #[inline]
640 fn mul(self, rhs: Mat2) -> Mat2 {
641 (*self).mul(rhs)
642 }
643}
644
645impl Mul<f32> for Mat2 {
646 type Output = Self;
647 #[inline]
648 fn mul(self, rhs: f32) -> Self {
649 self.mul_scalar(rhs)
650 }
651}
652
653impl Mul<&f32> for Mat2 {
654 type Output = Self;
655 #[inline]
656 fn mul(self, rhs: &f32) -> Self {
657 self.mul(*rhs)
658 }
659}
660
661impl Mul<&f32> for &Mat2 {
662 type Output = Mat2;
663 #[inline]
664 fn mul(self, rhs: &f32) -> Mat2 {
665 (*self).mul(*rhs)
666 }
667}
668
669impl Mul<f32> for &Mat2 {
670 type Output = Mat2;
671 #[inline]
672 fn mul(self, rhs: f32) -> Mat2 {
673 (*self).mul(rhs)
674 }
675}
676
677impl MulAssign<f32> for Mat2 {
678 #[inline]
679 fn mul_assign(&mut self, rhs: f32) {
680 *self = self.mul(rhs);
681 }
682}
683
684impl MulAssign<&f32> for Mat2 {
685 #[inline]
686 fn mul_assign(&mut self, rhs: &f32) {
687 self.mul_assign(*rhs);
688 }
689}
690
691impl Div<Mat2> for f32 {
692 type Output = Mat2;
693 #[inline]
694 fn div(self, rhs: Mat2) -> Self::Output {
695 rhs.div_scalar(self)
696 }
697}
698
699impl Div<&Mat2> for f32 {
700 type Output = Mat2;
701 #[inline]
702 fn div(self, rhs: &Mat2) -> Mat2 {
703 self.div(*rhs)
704 }
705}
706
707impl Div<&Mat2> for &f32 {
708 type Output = Mat2;
709 #[inline]
710 fn div(self, rhs: &Mat2) -> Mat2 {
711 (*self).div(*rhs)
712 }
713}
714
715impl Div<Mat2> for &f32 {
716 type Output = Mat2;
717 #[inline]
718 fn div(self, rhs: Mat2) -> Mat2 {
719 (*self).div(rhs)
720 }
721}
722
723impl Div<f32> for Mat2 {
724 type Output = Self;
725 #[inline]
726 fn div(self, rhs: f32) -> Self {
727 self.div_scalar(rhs)
728 }
729}
730
731impl Div<&f32> for Mat2 {
732 type Output = Self;
733 #[inline]
734 fn div(self, rhs: &f32) -> Self {
735 self.div(*rhs)
736 }
737}
738
739impl Div<&f32> for &Mat2 {
740 type Output = Mat2;
741 #[inline]
742 fn div(self, rhs: &f32) -> Mat2 {
743 (*self).div(*rhs)
744 }
745}
746
747impl Div<f32> for &Mat2 {
748 type Output = Mat2;
749 #[inline]
750 fn div(self, rhs: f32) -> Mat2 {
751 (*self).div(rhs)
752 }
753}
754
755impl DivAssign<f32> for Mat2 {
756 #[inline]
757 fn div_assign(&mut self, rhs: f32) {
758 *self = self.div(rhs);
759 }
760}
761
762impl DivAssign<&f32> for Mat2 {
763 #[inline]
764 fn div_assign(&mut self, rhs: &f32) {
765 self.div_assign(*rhs);
766 }
767}
768
769impl Sum<Self> for Mat2 {
770 fn sum<I>(iter: I) -> Self
771 where
772 I: Iterator<Item = Self>,
773 {
774 iter.fold(Self::ZERO, Self::add)
775 }
776}
777
778impl<'a> Sum<&'a Self> for Mat2 {
779 fn sum<I>(iter: I) -> Self
780 where
781 I: Iterator<Item = &'a Self>,
782 {
783 iter.fold(Self::ZERO, |a, &b| Self::add(a, b))
784 }
785}
786
787impl Product for Mat2 {
788 fn product<I>(iter: I) -> Self
789 where
790 I: Iterator<Item = Self>,
791 {
792 iter.fold(Self::IDENTITY, Self::mul)
793 }
794}
795
796impl<'a> Product<&'a Self> for Mat2 {
797 fn product<I>(iter: I) -> Self
798 where
799 I: Iterator<Item = &'a Self>,
800 {
801 iter.fold(Self::IDENTITY, |a, &b| Self::mul(a, b))
802 }
803}
804
805impl PartialEq for Mat2 {
806 #[inline]
807 fn eq(&self, rhs: &Self) -> bool {
808 self.x_axis.eq(&rhs.x_axis) && self.y_axis.eq(&rhs.y_axis)
809 }
810}
811
812impl AsRef<[f32; 4]> for Mat2 {
813 #[inline]
814 fn as_ref(&self) -> &[f32; 4] {
815 unsafe { &*(self as *const Self as *const [f32; 4]) }
816 }
817}
818
819impl AsMut<[f32; 4]> for Mat2 {
820 #[inline]
821 fn as_mut(&mut self) -> &mut [f32; 4] {
822 unsafe { &mut *(self as *mut Self as *mut [f32; 4]) }
823 }
824}
825
826impl core::ops::Deref for Mat2 {
827 type Target = crate::deref::Cols2<Vec2>;
828 #[inline]
829 fn deref(&self) -> &Self::Target {
830 unsafe { &*(self as *const Self as *const Self::Target) }
831 }
832}
833
834impl core::ops::DerefMut for Mat2 {
835 #[inline]
836 fn deref_mut(&mut self) -> &mut Self::Target {
837 unsafe { &mut *(self as *mut Self as *mut Self::Target) }
838 }
839}
840
841impl fmt::Debug for Mat2 {
842 fn fmt(&self, fmt: &mut fmt::Formatter<'_>) -> fmt::Result {
843 fmt.debug_struct(stringify!(Mat2))
844 .field("x_axis", &self.x_axis)
845 .field("y_axis", &self.y_axis)
846 .finish()
847 }
848}
849
850impl fmt::Display for Mat2 {
851 fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
852 if let Some(p) = f.precision() {
853 write!(f, "[{:.*}, {:.*}]", p, self.x_axis, p, self.y_axis)
854 } else {
855 write!(f, "[{}, {}]", self.x_axis, self.y_axis)
856 }
857 }
858}