acme
Foundational cross-platform layer of the ca2 framework
Loading...
Searching...
No Matches
matrix.h
1// From sequence.h by camilo on 2025-07-30 20:59 <3ThomasBorregaardSørensen!!
2#pragma once
3
4
5#include "acme/prototype/geometry/quaternion.h"
6#include "acme/prototype/geometry/sequence.h"
7#include "acme/prototype/geometry2d/angle.h"
8
9//template<prototype_floating FLOATING_TYPE, ::i32 t_iDimension, enum_matrix_major t_emajor>
10//struct matrix_type;
11
12
13
14#if defined(__SSE__) || defined(_M_X64) || defined(_M_IX86)
15#include <immintrin.h>
16#include <xmmintrin.h>
17#include <emmintrin.h>
18#endif
19
20#include "acme/operating_system/cpu_features.h"
21
22
23//CLASS_DECL_ACME extern ::i32 g_iCpuFeatures; // 0=scalar,1=SSE,2=AVX,3=AVX2
24
25
26
27namespace geometry
28{
29
30
31 template<prototype_floating FLOATING, ::i32 t_iDimension>
32 inline void _transpose(FLOATING(& A_target)[t_iDimension][t_iDimension],
33 const FLOATING(& A_source)[t_iDimension][t_iDimension])
34 {
35 for (::i32 i = 0; i < t_iDimension; i++)
36 for (::i32 j = 0; j < t_iDimension; j++)
37 A_target[i][j] = A_source[j][i];
38 }
39
40
41} // namespace geometry
42
43
44
45
47template<prototype_floating FLOATING_TYPE, ::i32 t_iDimension>
48struct row_major_type
49{
50
51 using FLOATING = FLOATING_TYPE;
52 static constexpr ::i32 DIMENSION = t_iDimension;
53
54
55 union
56 {
57 FLOATING m[DIMENSION][DIMENSION];
58 sequence_type<FLOATING, DIMENSION> m_sequencea[DIMENSION];
59 };
60
61 matrix_type<FLOATING, t_iDimension> transpose() const;
62
63};
64
65struct translation_t
66{
67};
68
69
71template<prototype_floating FLOATING_TYPE, ::i32 t_iDimension, enum_matrix_major t_emajor>
72struct matrix_type
73{
74
75
76 struct column
77 {
78
79 using FLOATING = FLOATING_TYPE;
80 static constexpr ::i32 DIMENSION = t_iDimension;
81
82 FLOATING m_rowa[t_iDimension];
83
84 constexpr const FLOATING &operator[](::i32 i) const
85 {
86
87 return m_rowa[i];
88 }
89
90 FLOATING& operator[](::i32 i)
91 {
92 return m_rowa[i];
93 }
94
95 };
96
97 static constexpr bool IsNanoGUI = true;
98 static constexpr bool IsMatrix = true;
99
100
101 using FLOATING = FLOATING_TYPE;
102 static constexpr ::i32 DIMENSION = t_iDimension;
103 static constexpr ::i32 SQUARED_DIMENSION = DIMENSION * DIMENSION;
104
105 union
106 {
107
108
109 column columna[DIMENSION];
110 FLOATING m[DIMENSION][DIMENSION];
111 FLOATING fa[SQUARED_DIMENSION]; // floating number array
112
113 };
114
115
116 constexpr matrix_type() {}
117
118
119 constexpr matrix_type(const matrix_type<FLOATING, 4> &m)
120 requires(DIMENSION==3)
121 {
122 // Column 0
123 this->m[0][0] = m[0][0];
124 this->m[0][1] = m[0][1];
125 this->m[0][2] = m[0][2];
126
127 // Column 1
128 this->m[1][0] = m[1][0];
129 this->m[1][1] = m[1][1];
130 this->m[1][2] = m[1][2];
131
132 // Column 2
133 this->m[2][0] = m[2][0];
134 this->m[2][1] = m[2][1];
135 this->m[2][2] = m[2][2];
136 }
137
138
139 constexpr matrix_type(const matrix_type<FLOATING, 4> &m)
140 requires(DIMENSION == 4)
141 {
142
143 for (::i32 i = 0; i < SQUARED_DIMENSION; i++) this->fa[i] = m.fa[i];
144
145 }
146
147
150 template<prototype_floating FLOATING1>
151 matrix_type(const FLOATING1 *p)
152 requires(DIMENSION == 4)
153 {
154
155 auto c = SQUARED_DIMENSION;
156
157 for (::i32 i = 0; i < c; ++i) this->fa[i] = (FLOATING)p[i];
158
159 }
160
161
162 constexpr matrix_type(const matrix_type<FLOATING, 3> & m)
163 requires(DIMENSION == 4)
164 : matrix_type((FLOATING)1)
165 {
167 //*this = floating_matrix4(1.0f);
168
169 // Copy the 3×3 into the upper-left of the 4×4
170 // (column-major order)
171 for (::i32 c = 0; c < 3; ++c)
172 for (::i32 r = 0; r < 3; ++r)
173 this->m[c][r] = m.m[c][r];
174 }
175
176
177 template < prototype_number NUMBER >
178 constexpr matrix_type(NUMBER diagonal)
179 {
180
181 for (::i32 i = 0; i < DIMENSION; i++)
182 for (::i32 j = 0; j < DIMENSION; j++)
183 m[i][j] = i == j ? (FLOATING) diagonal : (FLOATING) 0;
184
185 }
186
187
188 matrix_type(const quaternion_type<FLOATING> & q)
189 requires(DIMENSION == 4)
190 {
191
192 this->operator=(q);
193
194 }
195
197
198 //inline matrix_type operator*(const matrix_type &B) const
199 // requires(DIMENSION == 4)
200 //{
201 // // Columns of result are A * columns_of(B)
202 // matrix_type R;
203 // for (::i32 col = 0; col < 4; ++col)
204 // {
205 // for (::i32 row = 0; row < 4; ++row)
206 // {
207 // R.m[col][row] =
208 // this->m[0][row] * B.m[col][0] +
209 // this->m[1][row] * B.m[col][1] +
210 // this->m[2][row] * B.m[col][2] +
211 // this->m[3][row] * B.m[col][3];
212 // }
213 // }
214 // return R;
215 //}
216
217 //matrix_type operator*(const matrix_type& b) const {
218
219 // auto &a = *this;
220 // matrix_type c;
221 // for (::i32 i = 0; i < DIMENSION; ++i) {
222 // for (::i32 j = 0; j < DIMENSION; ++j) {
223 // FLOATING accum = 0;
224 // for (::i32 k = 0; k < DIMENSION; ++k)
225 // accum += a.m[k][i] * b.m[j][k];
226 // c.m[j][i] = accum;
227 // }
228 // }
229 // return c;
230 //}
231
232
233
234 // -------------------- Scalar fallback --------------------
235
236 inline sequence_type<FLOATING, 4> mul_scalar(const sequence_type<FLOATING, 4> &s) const
237 requires(DIMENSION == 4)
238 {
239 const auto &M = *this;
240 sequence_type<FLOATING, 4> r;
241 r.x = M.fa[0] * s.x + M.fa[4] * s.y + M.fa[8] * s.z + M.fa[12] * s.w;
242 r.y = M.fa[1] * s.x + M.fa[5] * s.y + M.fa[9] * s.z + M.fa[13] * s.w;
243 r.z = M.fa[2] * s.x + M.fa[6] * s.y + M.fa[10] * s.z + M.fa[14] * s.w;
244 r.w = M.fa[3] * s.x + M.fa[7] * s.y + M.fa[11] * s.z + M.fa[15] * s.w;
245 return r;
246 }
247
248#if defined(__SSE__)
249 // -------------------- SSE 128-bit version --------------------
250 inline f32_sequence4 mul_sse(const f32_sequence4 &s) const
251 requires(DIMENSION == 4 && std::is_same_v<FLOATING, ::f32>)
252 {
253 const auto &M = *this;
254 __m128 vx = _mm_set_ps(s.w, s.z, s.y, s.x); // [w z y x]
255 __m128 col0 = _mm_loadu_ps(&M.fa[0]);
256 __m128 col1 = _mm_loadu_ps(&M.fa[4]);
257 __m128 col2 = _mm_loadu_ps(&M.fa[8]);
258 __m128 col3 = _mm_loadu_ps(&M.fa[12]);
259
260 __m128 res = _mm_add_ps(
261 _mm_add_ps(_mm_mul_ps(_mm_shuffle_ps(vx, vx, 0x00), col0), _mm_mul_ps(_mm_shuffle_ps(vx, vx, 0x55), col1)),
262 _mm_add_ps(_mm_mul_ps(_mm_shuffle_ps(vx, vx, 0xAA), col2), _mm_mul_ps(_mm_shuffle_ps(vx, vx, 0xFF), col3)));
263
264 f32_sequence4 out;
265 _mm_storeu_ps(&out.x, res);
266 return out;
267 }
268#endif
269
270
271#if defined(__AVX__)
272 // -------------------- AVX 256-bit version --------------------
273 inline f32_sequence4 mul_avx(const f32_sequence4 &s) const
274 requires(DIMENSION == 4 && std::is_same_v<FLOATING, ::f32>)
275 {
276 const auto &M = *this;
277 __m256 vx = _mm256_setr_m128(_mm_set1_ps(s.x), _mm_set1_ps(s.y));
278 __m256 vy = _mm256_setr_m128(_mm_set1_ps(s.z), _mm_set1_ps(s.w));
279
280 __m256 col01 = _mm256_loadu_ps(&M.fa[0]);
281 __m256 col23 = _mm256_loadu_ps(&M.fa[8]);
282
283 __m256 mul01 = _mm256_mul_ps(col01, vx);
284 __m256 mul23 = _mm256_mul_ps(col23, vy);
285
286 __m128 sum = _mm_add_ps(_mm256_castps256_ps128(_mm256_add_ps(mul01, mul23)),
287 _mm256_extractf128_ps(_mm256_add_ps(mul01, mul23), 1));
288
289 f32_sequence4 out;
290 _mm_storeu_ps(&out.x, sum);
291 return out;
292 }
293#endif
294
295
296#if defined(__AVX2__)
297 // -------------------- AVX2 fully unrolled --------------------
298 inline f32_sequence4 mul_avx2(const f32_sequence4 &s) const
299 requires(DIMENSION == 4 && std::is_same_v<FLOATING, ::f32>)
300 {
301 const auto &M = *this;
302 __m256 vx = _mm256_set1_ps(s.x);
303 __m256 vy = _mm256_set1_ps(s.y);
304 __m256 vz = _mm256_set1_ps(s.z);
305 __m256 vw = _mm256_set1_ps(s.w);
306
307 __m256 col01 = _mm256_loadu_ps(&M.fa[0]);
308 __m256 col23 = _mm256_loadu_ps(&M.fa[8]);
309
310 __m256 mul0 = _mm256_mul_ps(_mm256_permute_ps(col01, 0x00), vx);
311 __m256 mul1 = _mm256_mul_ps(_mm256_permute_ps(col01, 0xFF), vy);
312 __m256 mul2 = _mm256_mul_ps(_mm256_permute_ps(col23, 0x00), vz);
313 __m256 mul3 = _mm256_mul_ps(_mm256_permute_ps(col23, 0xFF), vw);
314
315 __m256 total = _mm256_add_ps(_mm256_add_ps(mul0, mul1), _mm256_add_ps(mul2, mul3));
316 __m128 sum = _mm_add_ps(_mm256_castps256_ps128(total), _mm256_extractf128_ps(total, 1));
317
318 f32_sequence4 out;
319 _mm_storeu_ps(&out.x, sum);
320 return out;
321 }
322#endif
323 // // AVX2 version of matrix × vector
324 //inline f32_sequence4 mul_avx2( const f32_sequence4 &s)
325 // requires(DIMENSION == 4 && std::is_same_v<FLOATING, ::f32>)
326 //{
327 // const auto &M = *this;
328 // // Load vector s into low 128 bits: [x y z w]
329 // __m128 xv = _mm_set_ps(s.w, s.z, s.y, s.x);
330
331 // // Broadcast vector components into AVX registers
332 // __m256 bx = _mm256_broadcast_ss(&s.x);
333 // __m256 by = _mm256_broadcast_ss(&s.y);
334 // __m256 bz = _mm256_broadcast_ss(&s.z);
335 // __m256 bw = _mm256_broadcast_ss(&s.w);
336
337 // // Load two columns at a time (2×8 f32s)
338 // __m256 col01 = _mm256_loadu_ps(&M.fa[0]); // columns 0 & 1
339 // __m256 col23 = _mm256_loadu_ps(&M.fa[8]); // columns 2 & 3
340
341 // // Multiply columns by their vector components
342 // __m256 mul01 = _mm256_add_ps(
343 // _mm256_mul_ps(col01, _mm256_permute2f128_ps(_mm256_castps256_ps128(bx), _mm256_castps256_ps128(by), 0x20)),
344 // _mm256_mul_ps(col23, _mm256_permute2f128_ps(_mm256_castps256_ps128(bz), _mm256_castps256_ps128(bw), 0x20)));
345
346 // // Now we have results in 8 f32s; we only need the lower 4 (the multiplication pattern fits that)
347
348 // __m128 low = _mm256_castps256_ps128(mul01);
349 // __m128 high = _mm256_extractf128_ps(mul01, 1);
350
351 // __m128 sum = _mm_add_ps(low, high); // combine contributions
352
353 // f32_sequence4 out;
354 // _mm_storeu_ps(&out.x, sum);
355
356 // return out;
357
358 //}
359
360 template < prototype_number NUMBER >
361 inline matrix_type &operator*=(NUMBER n)
362 {
363
364 auto c = SQUARED_DIMENSION;
365
366 for (::i32 i = 0; i < c; ++i)
367 {
368
369 this->fa[i] = (FLOATING)(this->fa[i] * n);
370
371 }
372
373 return *this;
374
375 }
376
377
378 template<prototype_number NUMBER>
379 inline matrix_type operator*(NUMBER n) const
380 {
381
382 auto m = *this;
383
384 m *= n;
385
386 return m;
387
388 }
389
390
391 inline f32_sequence4 operator*(const f32_sequence4 &s) const
392 requires(DIMENSION == 4 && std::is_same_v<FLOATING, ::f32>)
393 {
394 //if (g_cpufeatures.m_bAVX2)
395 // return mul_avx2(s);
396 //else if (g_cpufeatures.m_bAVX)
397 // return mul_avx(s);
398 //else if (g_cpufeatures.m_bSSE)
399 // return mul_sse(s);
400 //else
401 return mul_scalar(s);
402 }
403
404
405 inline sequence_type<FLOATING, 3> operator*(const sequence_type<FLOATING, 3> &v) const
406 requires(DIMENSION == 3)
407 {
408
409 const auto &M = *this;
410 sequence_type<FLOATING, 3> result;
411
412 // Column-major OpenGL multiplication: result = M * v
413 result.x = M[0][0] * v.x + M[1][0] * v.y + M[2][0] * v.z;
414 result.y = M[0][1] * v.x + M[1][1] * v.y + M[2][1] * v.z;
415 result.z = M[0][2] * v.x + M[1][2] * v.y + M[2][2] * v.z;
416
417 return result;
418 }
419 inline matrix_type mul_scalar(const matrix_type &B) const
420 requires(DIMENSION == 4)
421 {
422 auto &A = *this;
423 matrix_type R;
424
425
426 for (::i32 col = 0; col < 4; ++col)
427 {
428 for (::i32 row = 0; row < 4; ++row)
429 {
430 R.fa[col * 4 + row] =
431 A.fa[0 * 4 + row] * B.fa[col * 4 + 0] +
432 A.fa[1 * 4 + row] * B.fa[col * 4 + 1] +
433 A.fa[2 * 4 + row] * B.fa[col * 4 + 2] +
434 A.fa[3 * 4 + row] * B.fa[col * 4 + 3];
435 }
436 }
437
438 return R;
439 }
440
441#if defined(__SSE__)
442 inline matrix_type mul_sse(const matrix_type &B) const
443 requires(DIMENSION == 4 && std::is_same_v<FLOATING, ::f32>)
444 {
445 auto &A = *this;
446 matrix_type R;
447
448
449 for (::i32 col = 0; col < 4; ++col)
450 {
451 const ::f32 *bCol = &B.fa[col * 4];
452
453 // Load columns of A
454 __m128 a0 = _mm_loadu_ps(&A.fa[0]); // column 0
455 __m128 a1 = _mm_loadu_ps(&A.fa[4]); // column 1
456 __m128 a2 = _mm_loadu_ps(&A.fa[8]); // column 2
457 __m128 a3 = _mm_loadu_ps(&A.fa[12]); // column 3
458
459 // Broadcast B column elements
460 __m128 b0 = _mm_set1_ps(bCol[0]);
461 __m128 b1 = _mm_set1_ps(bCol[1]);
462 __m128 b2 = _mm_set1_ps(bCol[2]);
463 __m128 b3 = _mm_set1_ps(bCol[3]);
464
465 // Multiply and sum
466 __m128 res = _mm_add_ps(_mm_add_ps(_mm_mul_ps(a0, b0), _mm_mul_ps(a1, b1)),
467 _mm_add_ps(_mm_mul_ps(a2, b2), _mm_mul_ps(a3, b3)));
468
469 // Store result column
470 _mm_storeu_ps(&R.fa[col * 4], res);
471 }
472
473 return R;
474 }
475#endif
476
477
478#if defined(__AVX__)
479 inline matrix_type mul_avx(const matrix_type &B) const
480 requires(DIMENSION == 4 && std::is_same_v<FLOATING, ::f32>)
481 {
482 auto &A = *this;
483 matrix_type R;
484
485
486 for (::i32 col = 0; col < 4; ++col)
487 {
488 const ::f32 *bCol = &B.fa[col * 4];
489
490 // Broadcast elements of B column
491 __m256 b01 = _mm256_set_ps(bCol[1], bCol[1], bCol[0], bCol[0], bCol[1], bCol[1], bCol[0], bCol[0]);
492 __m256 b23 = _mm256_set_ps(bCol[3], bCol[3], bCol[2], bCol[2], bCol[3], bCol[3], bCol[2], bCol[2]);
493
494 // Load two columns of A
495 __m256 a01 = _mm256_loadu_ps(&A.fa[0]); // columns 0 & 1
496 __m256 a23 = _mm256_loadu_ps(&A.fa[8]); // columns 2 & 3
497
498 // Multiply element-wise
499 __m256 mul01 = _mm256_mul_ps(a01, b01);
500 __m256 mul23 = _mm256_mul_ps(a23, b23);
501
502 // Sum contributions
503 __m256 total = _mm256_add_ps(mul01, mul23);
504
505 // Add low/high 128-bit lanes to get final 4 f32s
506 __m128 result = _mm_add_ps(_mm256_castps256_ps128(total), _mm256_extractf128_ps(total, 1));
507
508 // Store result column
509 _mm_storeu_ps(&R.fa[col * 4], result);
510 }
511
512 return R;
513 }
514#endif
515
516
517#if defined(__AVX2__)
518
519inline matrix_type mul_avx2(const matrix_type &B) const
520 requires(DIMENSION == 4 && std::is_same_v<FLOATING, ::f32>)
521 {
522 auto &A = *this;
523 matrix_type R;
524
525 // Broadcast each column of B and multiply by columns of A
526 for (::i32 col = 0; col < 4; ++col)
527 {
528 // Load column of B
529 const ::f32 *bCol = &B.fa[col * 4];
530
531 // Broadcast each element of the column
532 __m256 b0 = _mm256_set1_ps(bCol[0]);
533 __m256 b1 = _mm256_set1_ps(bCol[1]);
534 __m256 b2 = _mm256_set1_ps(bCol[2]);
535 __m256 b3 = _mm256_set1_ps(bCol[3]);
536
537 // Load columns of A as 256-bit registers
538 __m256 a01 = _mm256_loadu_ps(&A.fa[0]); // columns 0 & 1
539 __m256 a23 = _mm256_loadu_ps(&A.fa[8]); // columns 2 & 3
540
541 // Multiply each column by broadcasted scalar
542 __m256 mul0 = _mm256_mul_ps(_mm256_permute_ps(a01, 0x00), b0); // column 0 * b0
543 __m256 mul1 = _mm256_mul_ps(_mm256_permute_ps(a01, 0xFF), b1); // column 1 * b1
544 __m256 mul2 = _mm256_mul_ps(_mm256_permute_ps(a23, 0x00), b2); // column 2 * b2
545 __m256 mul3 = _mm256_mul_ps(_mm256_permute_ps(a23, 0xFF), b3); // column 3 * b3
546
547 // Sum contributions
548 __m256 total = _mm256_add_ps(_mm256_add_ps(mul0, mul1), _mm256_add_ps(mul2, mul3));
549
550 // Combine low/high 128-bit lanes
551 __m128 res = _mm_add_ps(_mm256_castps256_ps128(total), _mm256_extractf128_ps(total, 1));
552
553 // Store column into result
554 _mm_storeu_ps(&R.fa[col * 4], res);
555 }
556
557 return R;
558 }
559
560#endif
561
562 inline matrix_type operator*(const matrix_type &m) const
563 requires(DIMENSION == 4 && std::is_same_v<FLOATING, ::f32>)
564 {
565 //if (g_cpufeatures.m_bAVX2)
566 // return mul_avx2(m);
567 //else if (g_cpufeatures.m_bAVX)
568 // return mul_avx(m);
569 //else if (g_cpufeatures.m_bSSE)
570 // return mul_sse(m);
571 //else
572 return mul_scalar(m);
573 }
574
575
576
577 // ------------------------------
578 // Translation
579 // ------------------------------
580 static matrix_type translation(const sequence_type<FLOATING, DIMENSION - 1> &t)
581 {
582
583 matrix_type m{(FLOATING)1};
584
585 auto c = DIMENSION - 1;
586
587 for (::i32 i = 0; i < c; ++i)
588 m[c][i] = t[i];
589
590 return m;
591
592 }
593
594
595 // ------------------------------
596 // Scaling
597 // ------------------------------
598 static matrix_type scaling(const sequence_type<FLOATING, DIMENSION - 1>& s)
599 {
600
601 matrix_type result{1.0f};
602
603 auto c = DIMENSION - 1;
604
605 for (::i32 i = 0; i < c; ++i)
606 result.m[i][i] = s[i];
607
608 return result;
609
610 }
611
612
613 // ------------------------------
614 // Translation
615 // ------------------------------
616 static matrix_type rotation(const quaternion_type<FLOATING> &q)
617 {
618
619 matrix_type m;
620
621 m = q;
622
623 return m;
624
625 }
626
627
628 //static matrix_type scale(const sequence_type<FLOATING, DIMENSION>& m_coordinatea) {
629 // matrix_type result;
630 // ::memory_set(result.m, 0, sizeof(FLOATING) * SQUARED_DIMENSION);
631 // for (::i32 i = 0; i < DIMENSION; ++i)
632 // result.m[i][i] = m_coordinatea[i];
633 // return result;
634 //}
635
636 //static matrix_type translate(const sequence_type<FLOATING, DIMENSION - 1>& m_coordinatea) {
637 // matrix_type result;
638 // ::memory_set(result.m, 0, sizeof(FLOATING) * SQUARED_DIMENSION);
639 // for (::i32 i = 0; i < DIMENSION; ++i) {
640 // result.m[i][i] = 1.f;
641 // if (i < DIMENSION - 1)
642 // result.m[DIMENSION - 1][i] = m_coordinatea[i];
643 // }
644 // return result;
645 //}
646
647 const column & operator[](::i32 i) const
648 {
649
650 return columna[i];
651 }
652
653 column & operator[](::i32 i) { return columna[i]; }
654
655
656 matrix_type & rotate(const sequence_type<FLOATING, 3>& axis, FLOATING angle)
657 requires(DIMENSION == 4)
658 {
659
660 FLOATING s = ::sin(angle);
661 FLOATING c = ::cos(angle);
662 FLOATING t = 1 - c;
663
664 auto & result = *this;
665 result.m[3][3] = 1;
666 result.m[0][0] = c + axis.x * axis.x * t;
667 result.m[1][1] = c + axis.y * axis.y * t;
668 result.m[2][2] = c + axis.z * axis.z * t;
669
670 FLOATING tmp1 = axis.x * axis.y * t;
671 FLOATING tmp2 = axis.z * s;
672
673 result.m[0][1] = tmp1 + tmp2;
674 result.m[1][0] = tmp1 - tmp2;
675
676 tmp1 = axis.x * axis.z * t;
677 tmp2 = axis.y * s;
678 result.m[0][2] = tmp1 - tmp2;
679 result.m[2][0] = tmp1 + tmp2;
680
681 tmp1 = axis.y * axis.z * t;
682 tmp2 = axis.x * s;
683 result.m[1][2] = tmp1 + tmp2;
684 result.m[2][1] = tmp1 - tmp2;
685
686 return result;
687
688 }
689
690 static matrix_type rotation(const sequence_type<FLOATING, 3> &axis, FLOATING angle)
691 requires(DIMENSION == 4)
692 {
693
694 matrix_type m(1);
695
696 m.rotate(axis, angle);
697
698 return m;
699
700 }
701
702 template <::i32 S = DIMENSION, std::enable_if_t<S == 4, ::i32> = 0>
703 static matrix_type perspective(FLOATING fov, FLOATING near_, FLOATING far_, FLOATING aspect = 1.f)
704 {
705
706 FLOATING recip = 1 / (near_ - far_);
707 FLOATING c = 1 / ::tan(.5f * fov);
708
709 matrix_type trafo = matrix_type::scaling(sequence_type<FLOATING, DIMENSION>(c / aspect, c, (near_ + far_) * recip, 0.f));
710
711 trafo.m[3][2] = 2.f * near_ * far_ * recip;
712 trafo.m[2][3] = -1.f;
713
714 return trafo;
715 }
716
717 template <::i32 S = DIMENSION, std::enable_if_t<S == 4, ::i32> = 0>
718 static matrix_type ortho(FLOATING left, FLOATING right,
719 FLOATING bottom, FLOATING top,
720 FLOATING near_, FLOATING far_)
721 {
722
723 FLOATING rl = 1 / (right - left);
724 FLOATING tb = 1 / (top - bottom);
725 FLOATING fn = 1 / (far_ - near_);
726
727 matrix_type result(0);
728
729 result.m[0][0] = 2 * rl;
730 result.m[1][1] = 2 * tb;
731 result.m[2][2] = -2 * fn;
732 result.m[3][3] = 1;
733 result.m[3][0] = -(right + left) * rl;
734 result.m[3][1] = -(top + bottom) * tb;
735 result.m[3][2] = -(far_ + near_) * fn;
736
737 return result;
738 }
739
740
741 static matrix_type look_at(const sequence_type<FLOATING, 3>& origin,
742 const sequence_type<FLOATING, 3>& target,
743 const sequence_type<FLOATING, 3>& up)
744 requires(DIMENSION == 4)
745 {
746
747 auto dir = normalize(target - origin);
748 auto left = normalize(cross(dir, up));
749 auto new_up = cross(left, dir);
750 dir = -dir;
751
752 matrix_type result(0);
753 result.m[0][0] = left.x;
754 result.m[0][1] = left.y;
755 result.m[0][2] = left.c();
756 result.m[1][0] = new_up.x;
757 result.m[1][1] = new_up.y;
758 result.m[1][2] = new_up.c();
759 result.m[2][0] = dir.x;
760 result.m[2][1] = dir.y;
761 result.m[2][2] = dir.c();
762 result.m[3][0] = -dot(left, origin);
763 result.m[3][1] = -dot(new_up, origin);
764 result.m[3][2] = -dot(dir, origin);
765 result.m[3][3] = 1.f;
766 return result;
767 }
768
769
770 //inline sequence_type<FLOATING, 4> mul_scalar(const sequence_type<FLOATING, 4> &s) const
771 // requires(DIMENSION == 4)
772 //{
773 // sequence_type<FLOATING, 4> r;
774
775 // r.x = this->m[0][0] * s.x + this->m[0][1] * s.y + this->m[0][2] * s.z + this->m[0][3] * s.w;
776 // r.y = this->m[1][0] * s.x + this->m[1][1] * s.y + this->m[1][2] * s.z + this->m[1][3] * s.w;
777 // r.z = this->m[2][0] * s.x + this->m[2][1] * s.y + this->m[2][2] * s.z + this->m[2][3] * s.w;
778 // r.w = this->m[3][0] * s.x + this->m[3][1] * s.y + this->m[3][2] * s.z + this->m[3][3] * s.w;
779
780 // return r;
781 //}
782
783 inline sequence_type<FLOATING, 3> operator*(const sequence_type<FLOATING, 3> &s) const
784 requires(DIMENSION == 4)
785 {
786 auto t = this->operator *( sequence_type<FLOATING, 4>(s, (FLOATING) 1));
787 return t;
788 }
789
790
791 matrix_type rotated(angle_type<FLOATING> angleRadians,
792 const sequence_type<FLOATING, 3> &axis) const
793 requires(DIMENSION == 4)
794 {
795
796 const auto &m = *this;
797 auto a = axis.normalized();
798 auto c = angleRadians.cos();
799 auto s = angleRadians.sin();
800 auto t = 1.0f - c;
801
802 // Build the rotation matrix (Rodrigues' rotation formula)
803 matrix_type rot(1.0f);
804 rot[0][0] = c + a.x * a.x * t;
805 rot[1][0] = a.x * a.y * t + a.z * s;
806 rot[2][0] = a.x * a.z * t - a.y * s;
807
808 rot[0][1] = a.y * a.x * t - a.z * s;
809 rot[1][1] = c + a.y * a.y * t;
810 rot[2][1] = a.y * a.z * t + a.x * s;
811
812 rot[0][2] = a.z * a.x * t + a.y * s;
813 rot[1][2] = a.z * a.y * t - a.x * s;
814 rot[2][2] = c + a.z * a.z * t;
815
816 // Apply rotation to existing matrix
817 return m * rot;
818
819 }
820
821
822 matrix_type & rotate(angle_type<FLOATING> angleRadians, const sequence_type<FLOATING, 3> & axis)
823 {
824
825 return *this = this->rotated(angleRadians, axis);
826
827 }
828
829
830 //matrix_type transposed() const
831 // requires(DIMENSION == 3)
832 //{
833 // auto &m = *this;
834 // matrix_type r;
835 // r[0][0] = m[0][0];
836 // r[0][1] = m[1][0];
837 // r[0][2] = m[2][0];
838
839 // r[1][0] = m[0][1];
840 // r[1][1] = m[1][1];
841 // r[1][2] = m[2][1];
842
843 // r[2][0] = m[0][2];
844 // r[2][1] = m[1][2];
845 // r[2][2] = m[2][2];
846 // return r;
847 //}
848
849
850 matrix_type & transpose()
851 //requires(DIMENSION == 4)
852 {
853 auto source = *this;
854 ::geometry::_transpose(this->m, source.m);
855 return * this;
856 }
857
858
859 //matrix_type & transpose()
860 // requires(DIMENSION == 3)
861 //{
862 // return *this = this->transposed();
863 //}
864
865
866 matrix_type & translate(const sequence_type<FLOATING, 3> &s)
867 requires(DIMENSION == 4)
868 {
869
870 this->m[3][0] += s.x;
871 this->m[3][1] += s.y;
872 this->m[3][2] += s.z;
873
874 return *this;
875 }
876
877 matrix_type translated(const sequence_type<FLOATING, 3> &s) const
878 requires(DIMENSION == 4)
879 {
880
881 auto m = *this;
882
883 return m.translate(s);
884 }
885
886
887 matrix_type & scale( const sequence_type<FLOATING, 3> &s)
888 requires(DIMENSION == 4)
889 {
890 this->m[0][0] *= s.x;
891 this->m[1][0] *= s.x;
892 this->m[2][0] *= s.x;
893 this->m[3][0] *= s.x;
894
895 this->m[0][1] *= s.y;
896 this->m[1][1] *= s.y;
897 this->m[2][1] *= s.y;
898 this->m[3][1] *= s.y;
899
900 this->m[0][2] *= s.z;
901 this->m[1][2] *= s.z;
902 this->m[2][2] *= s.z;
903 this->m[3][2] *= s.z;
904
905 return *this;
906 }
907
908
909 matrix_type scaled(const sequence_type<FLOATING, 3> &s) const
910 requires(DIMENSION == 4)
911 {
912
913 auto m = *this;
914
915 return m.scale(s);
916 }
917
918
919 FLOATING determinant() const
920 requires(DIMENSION == 3)
921 {
922 auto m = this->fa;
923
924 if constexpr (t_emajor == e_matrix_major_column)
925 {
926 // Column-major indexing: m[col*3 + row]
927 return m[0] * (m[4] * m[8] - m[5] * m[7]) - // m00*(m11*m22 - m12*m21)
928 m[3] * (m[1] * m[8] - m[2] * m[7]) + // m01*(m10*m22 - m12*m20)
929 m[6] * (m[1] * m[5] - m[2] * m[4]); // m02*(m10*m21 - m11*m20)
930 }
931 else
932 {
933 // Row-major indexing: m[row*3 + col]
934 return m[0] * (m[4] * m[8] - m[5] * m[7]) - // m00*(m11*m22 - m12*m21)
935 m[1] * (m[3] * m[8] - m[5] * m[6]) + // m01*(m10*m22 - m12*m20)
936 m[2] * (m[3] * m[7] - m[4] * m[6]); // m02*(m10*m21 - m11*m20)
937 }
938
939 }
940
941
942
943 FLOATING determinant() const
944 requires(DIMENSION == 4)
945 {
946
947 auto m = this->fa;
948
949 if constexpr (t_emajor == e_matrix_major_column)
950 {
951 // Column-major: m[col*4 + row]
952 return m[0] * (m[5] * (m[10] * m[15] - m[11] * m[14]) - m[9] * (m[6] * m[15] - m[7] * m[14]) +
953 m[13] * (m[6] * m[11] - m[7] * m[10])) -
954 m[4] * (m[1] * (m[10] * m[15] - m[11] * m[14]) - m[9] * (m[2] * m[15] - m[3] * m[14]) +
955 m[13] * (m[2] * m[11] - m[3] * m[10])) +
956 m[8] * (m[1] * (m[6] * m[15] - m[7] * m[14]) - m[5] * (m[2] * m[15] - m[3] * m[14]) +
957 m[13] * (m[2] * m[7] - m[3] * m[6])) -
958 m[12] * (m[1] * (m[6] * m[11] - m[7] * m[10]) - m[5] * (m[2] * m[11] - m[3] * m[10]) +
959 m[9] * (m[2] * m[7] - m[3] * m[6]));
960 }
961 else
962 {
963 // Row-major: m[row*4 + col]
964 return m[0] * (m[5] * (m[10] * m[15] - m[11] * m[14]) - m[6] * (m[9] * m[15] - m[11] * m[13]) +
965 m[7] * (m[9] * m[14] - m[10] * m[13])) -
966 m[1] * (m[4] * (m[10] * m[15] - m[11] * m[14]) - m[6] * (m[8] * m[15] - m[11] * m[12]) +
967 m[7] * (m[8] * m[14] - m[10] * m[12])) +
968 m[2] * (m[4] * (m[9] * m[15] - m[11] * m[13]) - m[5] * (m[8] * m[15] - m[11] * m[12]) +
969 m[7] * (m[8] * m[13] - m[9] * m[12])) -
970 m[3] * (m[4] * (m[9] * m[14] - m[10] * m[13]) - m[5] * (m[8] * m[14] - m[10] * m[12]) +
971 m[6] * (m[8] * m[13] - m[9] * m[12]));
972 }
973
974
975 }
976
977 inline matrix_type & inverse_scalar()
978 requires(DIMENSION == 3)
979 {
980
981 const auto & A = *this;
982 // Column-major indexing:
983 // A.m[0] A.m[3] A.m[6]
984 // A.m[1] A.m[4] A.m[7]
985 // A.m[2] A.m[5] A.m[8]
986
987 //::f32 a00 = A.fa[0];
988 //::f32 a01 = A.fa[3];
989 //::f32 a02 = A.fa[6];
990 //::f32 a10 = A.fa[1];
991 //::f32 a11 = A.fa[4];
992 //::f32 a12 = A.fa[7];
993 //::f32 a20 = A.fa[2];
994 //::f32 a21 = A.fa[5];
995 //::f32 a22 = A.fa[8];
996
997 // Cofactors
998 auto c00 = (A.fa[4] * A.fa[8] - A.fa[7] * A.fa[5]);
999 auto c01 = -(A.fa[1] * A.fa[8] - A.fa[7] * A.fa[2]);
1000 auto c02 = (A.fa[1] * A.fa[5] - A.fa[4] * A.fa[2]);
1001
1002 auto c10 = -(A.fa[3] * A.fa[8] - A.fa[6] * A.fa[5]);
1003 auto c11 = (A.fa[0] * A.fa[8] - A.fa[6] * A.fa[2]);
1004 auto c12 = -(A.fa[0] * A.fa[5] - A.fa[3] * A.fa[2]);
1005
1006 auto c20 = (A.fa[3] * A.fa[7] - A.fa[6] * A.fa[4]);
1007 auto c21 = -(A.fa[0] * A.fa[7] - A.fa[6] * A.fa[1]);
1008 auto c22 = (A.fa[0] * A.fa[4] - A.fa[3] * A.fa[1]);
1009
1010 // Determinant
1011 auto det = A.fa[0] * c00 + A.fa[3] * c01 + A.fa[6] * c02;
1012
1013 auto & out = *this;
1014
1015 if (fabs(det) < 1e-12f)
1016 {
1017 // Return identity or zero? Your choice
1018 // Here: return identity
1019 out.fa[0] = 1;
1020 out.fa[3] = 0;
1021 out.fa[6] = 0;
1022 out.fa[1] = 0;
1023 out.fa[4] = 1;
1024 out.fa[7] = 0;
1025 out.fa[2] = 0;
1026 out.fa[5] = 0;
1027 out.fa[8] = 1;
1028 return *this;
1029 }
1030
1031 auto invDet = (FLOATING) 1 / det;
1032
1033 // The adjugate is the transpose of the cofactor matrix
1034 // Write it directly in column-major:
1035
1036 out.fa[0] = c00 * invDet;
1037 out.fa[1] = c10 * invDet;
1038 out.fa[2] = c20 * invDet;
1039
1040 out.fa[3] = c01 * invDet;
1041 out.fa[4] = c11 * invDet;
1042 out.fa[5] = c21 * invDet;
1043
1044 out.fa[6] = c02 * invDet;
1045 out.fa[7] = c12 * invDet;
1046 out.fa[8] = c22 * invDet;
1047
1048 return *this;
1049
1050 }
1051
1052
1053#if defined(__SSE__)
1054 inline matrix_type inversed_sse() const
1055 requires(DIMENSION == 3 && std::is_same_v<FLOATING, ::f32>)
1056 {
1057
1058 const auto &A = *this;
1059 // Load columns (pad with 0 in 4th ::f32)
1060 __m128 c0 = _mm_set_ps(0.0f, A.fa[2], A.fa[1], A.fa[0]);
1061 __m128 c1 = _mm_set_ps(0.0f, A.fa[5], A.fa[4], A.fa[3]);
1062 __m128 c2 = _mm_set_ps(0.0f, A.fa[8], A.fa[7], A.fa[6]);
1063
1064 // Unpack to rows
1065 __m128 r0 = _mm_unpacklo_ps(c0, c1); // [a00 a01 a10 a11]
1066 //__m128 r1 = _mm_unpackhi_ps(c0, c1); // [a20 a21 0 0]
1067 __m128 r2 = c2; // [a02 a12 a22 0]
1068
1069 // Cofactor calculations:
1070 // c00 = a11*a22 - a12*a21
1071 __m128 t0 = _mm_mul_ps(_mm_shuffle_ps(r0, r0, _MM_SHUFFLE(3, 3, 1, 1)),
1072 _mm_shuffle_ps(r2, r2, _MM_SHUFFLE(3, 3, 3, 0))); // a11 * a22
1073
1074 __m128 t1 = _mm_mul_ps(_mm_shuffle_ps(r0, r0, _MM_SHUFFLE(3, 3, 1, 1)),
1075 _mm_shuffle_ps(r2, r2, _MM_SHUFFLE(3, 3, 0, 0))); // a12 * a21
1076
1077 __m128 cof0 = _mm_sub_ps(t0, t1);
1078
1079 // We will extract elements later.
1080 ::f32 c00 = _mm_cvtss_f32(cof0);
1081
1082 // Scalar fallback for the remaining cofactors (SSE only helps partially for 3×3)
1083 // (This keeps correctness while still accelerating the heavy parts.)
1084
1085 ::f32 a00 = A.fa[0], a01 = A.fa[3], a02 = A.fa[6];
1086 ::f32 a10 = A.fa[1], a11 = A.fa[4], a12 = A.fa[7];
1087 ::f32 a20 = A.fa[2], a21 = A.fa[5], a22 = A.fa[8];
1088
1089 ::f32 c01 = -(a10 * a22 - a12 * a20);
1090 ::f32 c02 = (a10 * a21 - a11 * a20);
1091 ::f32 c10 = -(a01 * a22 - a02 * a21);
1092 ::f32 c11 = (a00 * a22 - a02 * a20);
1093 ::f32 c12 = -(a00 * a21 - a01 * a20);
1094 ::f32 c20 = (a01 * a12 - a02 * a11);
1095 ::f32 c21 = -(a00 * a12 - a02 * a10);
1096 ::f32 c22 = (a00 * a11 - a01 * a10);
1097
1098 ::f32 det = a00 * c00 + a01 * c01 + a02 * c02;
1099
1100 matrix_type out;
1101
1102 if (fabs(det) < 1e-12f)
1103 {
1104 // return identity
1105 //out = {1, 0, 0, 0, 1, 0, 0, 0, 1};
1106 return {1.0f};
1107 }
1108
1109 ::f32 invDet = 1.0f / det;
1110
1111 // Store adjugate * invDet (column-major)
1112 out.fa[0] = c00 * invDet;
1113 out.fa[1] = c10 * invDet;
1114 out.fa[2] = c20 * invDet;
1115
1116 out.fa[3] = c01 * invDet;
1117 out.fa[4] = c11 * invDet;
1118 out.fa[5] = c21 * invDet;
1119
1120 out.fa[6] = c02 * invDet;
1121 out.fa[7] = c12 * invDet;
1122 out.fa[8] = c22 * invDet;
1123
1124 return out;
1125 }
1126#endif
1127
1128
1129#if defined(__AVX__)
1130
1131 inline matrix_type inversed_avx() const
1132 requires(DIMENSION == 3 && std::is_same_v<FLOATING, ::f32>)
1133 {
1134
1135 const ::f32 *in = this->fa;
1136 matrix_type result;
1137 ::f32 *out = result.fa;
1138 // Load 3×3 matrix (column-major)
1139 ::f32 m00 = in[0], m01 = in[3], m02 = in[6];
1140 ::f32 m10 = in[1], m11 = in[4], m12 = in[7];
1141 ::f32 m20 = in[2], m21 = in[5], m22 = in[8];
1142
1143 //
1144 // ---- Cofactor matrix (scalar – fastest for 3×3) ----
1145 //
1146 ::f32 cf00 = m11 * m22 - m12 * m21;
1147 ::f32 cf01 = -(m10 * m22 - m12 * m20);
1148 ::f32 cf02 = m10 * m21 - m11 * m20;
1149
1150 ::f32 cf10 = -(m01 * m22 - m02 * m21);
1151 ::f32 cf11 = m00 * m22 - m02 * m20;
1152 ::f32 cf12 = -(m00 * m21 - m01 * m20);
1153
1154 ::f32 cf20 = m01 * m12 - m02 * m11;
1155 ::f32 cf21 = -(m00 * m12 - m02 * m10);
1156 ::f32 cf22 = m00 * m11 - m01 * m10;
1157
1158 // Determinant
1159 ::f32 det = m00 * cf00 + m01 * cf01 + m02 * cf02;
1160 ::f32 invdet = 1.0f / det;
1161
1162 //
1163 // ---- Pack cofactors into column-major adjoint matrix ----
1164 //
1165 // adj = transpose(cofactor matrix)
1166 alignas(32) ::f32 adj[9] = {
1167 cf00, cf10, cf20, // column 0
1168 cf01, cf11, cf21, // column 1
1169 cf02, cf12, cf22 // column 2
1170 };
1171
1172 //
1173 // ---- AVX multiply first 8 f32s ----
1174 //
1175 __m256 scale = _mm256_set1_ps(invdet);
1176 __m256 adj_lo = _mm256_loadu_ps(&adj[0]); // adj[0..7]
1177 __m256 out_lo = _mm256_mul_ps(adj_lo, scale);
1178
1179 //
1180 // ---- Last element adj[8] ----
1181 //
1182 __m128 last = _mm_mul_ss(_mm_set_ss(adj[8]), _mm256_castps256_ps128(scale));
1183
1184 //
1185 // ---- Store result ----
1186 //
1187 _mm256_storeu_ps(&out[0], out_lo);
1188 _mm_store_ss(&out[8], last);
1189 return result;
1190 }
1191
1192
1193#endif
1194
1195
1196
1197 // ---------------- Scalar inverse ----------------
1198 inline matrix_type & inverse_scalar()
1199 requires(DIMENSION == 4)
1200 {
1201
1202 auto source = *this;
1203
1204 auto m = source.fa;
1205
1206 auto inv = this->fa;
1207
1208 inv[0] = m[5] * m[10] * m[15] - m[5] * m[11] * m[14] - m[9] * m[6] * m[15] + m[9] * m[7] * m[14] +
1209 m[13] * m[6] * m[11] - m[13] * m[7] * m[10];
1210 inv[4] = -m[4] * m[10] * m[15] + m[4] * m[11] * m[14] + m[8] * m[6] * m[15] - m[8] * m[7] * m[14] -
1211 m[12] * m[6] * m[11] + m[12] * m[7] * m[10];
1212 inv[8] = m[4] * m[9] * m[15] - m[4] * m[11] * m[13] - m[8] * m[5] * m[15] + m[8] * m[7] * m[13] +
1213 m[12] * m[5] * m[11] - m[12] * m[7] * m[9];
1214 inv[12] = -m[4] * m[9] * m[14] + m[4] * m[10] * m[13] + m[8] * m[5] * m[14] - m[8] * m[6] * m[13] -
1215 m[12] * m[5] * m[10] + m[12] * m[6] * m[9];
1216
1217 inv[1] = -m[1] * m[10] * m[15] + m[1] * m[11] * m[14] + m[9] * m[2] * m[15] - m[9] * m[3] * m[14] -
1218 m[13] * m[2] * m[11] + m[13] * m[3] * m[10];
1219 inv[5] = m[0] * m[10] * m[15] - m[0] * m[11] * m[14] - m[8] * m[2] * m[15] + m[8] * m[3] * m[14] +
1220 m[12] * m[2] * m[11] - m[12] * m[3] * m[10];
1221 inv[9] = -m[0] * m[9] * m[15] + m[0] * m[11] * m[13] + m[8] * m[1] * m[15] - m[8] * m[3] * m[13] -
1222 m[12] * m[1] * m[11] + m[12] * m[3] * m[9];
1223 inv[13] = m[0] * m[9] * m[14] - m[0] * m[10] * m[13] - m[8] * m[1] * m[14] + m[8] * m[2] * m[13] +
1224 m[12] * m[1] * m[10] - m[12] * m[2] * m[9];
1225
1226 inv[2] = m[1] * m[6] * m[15] - m[1] * m[7] * m[14] - m[5] * m[2] * m[15] + m[5] * m[3] * m[14] +
1227 m[13] * m[2] * m[7] - m[13] * m[3] * m[6];
1228 inv[6] = -m[0] * m[6] * m[15] + m[0] * m[7] * m[14] + m[4] * m[2] * m[15] - m[4] * m[3] * m[14] -
1229 m[12] * m[2] * m[7] + m[12] * m[3] * m[6];
1230 inv[10] = m[0] * m[5] * m[15] - m[0] * m[7] * m[13] - m[4] * m[1] * m[15] + m[4] * m[3] * m[13] +
1231 m[12] * m[1] * m[7] - m[12] * m[3] * m[5];
1232 inv[14] = -m[0] * m[5] * m[14] + m[0] * m[6] * m[13] + m[4] * m[1] * m[14] - m[4] * m[2] * m[13] -
1233 m[12] * m[1] * m[6] + m[12] * m[2] * m[5];
1234
1235 inv[3] = -m[1] * m[6] * m[11] + m[1] * m[7] * m[10] + m[5] * m[2] * m[11] - m[5] * m[3] * m[10] -
1236 m[9] * m[2] * m[7] + m[9] * m[3] * m[6];
1237 inv[7] = m[0] * m[6] * m[11] - m[0] * m[7] * m[10] - m[4] * m[2] * m[11] + m[4] * m[3] * m[10] +
1238 m[8] * m[2] * m[7] - m[8] * m[3] * m[6];
1239 inv[11] = -m[0] * m[5] * m[11] + m[0] * m[7] * m[9] + m[4] * m[1] * m[11] - m[4] * m[3] * m[9] -
1240 m[8] * m[1] * m[7] + m[8] * m[3] * m[5];
1241 inv[15] = m[0] * m[5] * m[10] - m[0] * m[6] * m[9] - m[4] * m[1] * m[10] + m[4] * m[2] * m[9] +
1242 m[8] * m[1] * m[6] - m[8] * m[2] * m[5];
1243
1244 ::f32 det = m[0] * inv[0] + m[1] * inv[4] + m[2] * inv[8] + m[3] * inv[12];
1245
1246 ::f32 invDet = 1.0f / det;
1247
1248 for (::i32 i = 0; i < 16; i++)
1249 inv[i] *= invDet;
1250
1251 return *this;
1252
1253 }
1254
1255
1256#if defined(__SSE__)
1257 inline matrix_type inversed_sse() const
1258 requires(DIMENSION == 4 && std::is_same_v<FLOATING, ::f32>)
1259 {
1260
1261 const auto &a = *this;
1262 // Load two columns at a time
1263 __m128 col0 = _mm_loadu_ps(&a.fa[0]);
1264 __m128 col1 = _mm_loadu_ps(&a.fa[4]);
1265 __m128 col2 = _mm_loadu_ps(&a.fa[8]);
1266 __m128 col3 = _mm_loadu_ps(&a.fa[12]);
1267
1268 // Unpack into scalar f32s for fully unrolled computation
1269 ::f32 m[16];
1270 _mm_storeu_ps(&m[0], col0);
1271 _mm_storeu_ps(&m[4], col1);
1272 _mm_storeu_ps(&m[8], col2);
1273 _mm_storeu_ps(&m[12], col3);
1274
1275 matrix_type inv;
1276
1277 // Fully unrolled cofactor computation (column-major)
1278 inv.fa[0] = m[5] * m[10] * m[15] - m[5] * m[11] * m[14] - m[9] * m[6] * m[15] + m[9] * m[7] * m[14] +
1279 m[13] * m[6] * m[11] - m[13] * m[7] * m[10];
1280 inv.fa[1] = -m[1] * m[10] * m[15] + m[1] * m[11] * m[14] + m[9] * m[2] * m[15] - m[9] * m[3] * m[14] -
1281 m[13] * m[2] * m[11] + m[13] * m[3] * m[10];
1282 inv.fa[2] = m[1] * m[6] * m[15] - m[1] * m[7] * m[14] - m[5] * m[2] * m[15] + m[5] * m[3] * m[14] +
1283 m[13] * m[2] * m[7] - m[13] * m[3] * m[6];
1284 inv.fa[3] = -m[1] * m[6] * m[11] + m[1] * m[7] * m[10] + m[5] * m[2] * m[11] - m[5] * m[3] * m[10] -
1285 m[9] * m[2] * m[7] + m[9] * m[3] * m[6];
1286
1287 inv.fa[4] = -m[4] * m[10] * m[15] + m[4] * m[11] * m[14] + m[8] * m[6] * m[15] - m[8] * m[7] * m[14] -
1288 m[12] * m[6] * m[11] + m[12] * m[7] * m[10];
1289 inv.fa[5] = m[0] * m[10] * m[15] - m[0] * m[11] * m[14] - m[8] * m[2] * m[15] + m[8] * m[3] * m[14] +
1290 m[12] * m[2] * m[11] - m[12] * m[3] * m[10];
1291 inv.fa[6] = -m[0] * m[6] * m[15] + m[0] * m[7] * m[14] + m[4] * m[2] * m[15] - m[4] * m[3] * m[14] -
1292 m[12] * m[2] * m[7] + m[12] * m[3] * m[6];
1293 inv.fa[7] = m[0] * m[6] * m[11] - m[0] * m[7] * m[10] - m[4] * m[2] * m[11] + m[4] * m[3] * m[10] +
1294 m[8] * m[2] * m[7] - m[8] * m[3] * m[6];
1295
1296 inv.fa[8] = m[4] * m[9] * m[15] - m[4] * m[11] * m[13] - m[8] * m[5] * m[15] + m[8] * m[7] * m[13] +
1297 m[12] * m[5] * m[11] - m[12] * m[7] * m[9];
1298 inv.fa[9] = -m[0] * m[9] * m[15] + m[0] * m[11] * m[13] + m[8] * m[1] * m[15] - m[8] * m[3] * m[13] -
1299 m[12] * m[1] * m[11] + m[12] * m[3] * m[9];
1300 inv.fa[10] = m[0] * m[5] * m[15] - m[0] * m[7] * m[13] - m[4] * m[1] * m[15] + m[4] * m[3] * m[13] +
1301 m[12] * m[1] * m[7] - m[12] * m[3] * m[5];
1302 inv.fa[11] = -m[0] * m[5] * m[11] + m[0] * m[7] * m[9] + m[4] * m[1] * m[11] - m[4] * m[3] * m[9] -
1303 m[8] * m[1] * m[7] + m[8] * m[3] * m[5];
1304
1305 inv.fa[12] = -m[4] * m[9] * m[14] + m[4] * m[10] * m[13] + m[8] * m[5] * m[14] - m[8] * m[6] * m[13] -
1306 m[12] * m[5] * m[10] + m[12] * m[6] * m[9];
1307 inv.fa[13] = m[0] * m[9] * m[14] - m[0] * m[10] * m[13] - m[8] * m[1] * m[14] + m[8] * m[2] * m[13] +
1308 m[12] * m[1] * m[10] - m[12] * m[2] * m[9];
1309 inv.fa[14] = -m[0] * m[5] * m[14] + m[0] * m[6] * m[13] + m[4] * m[1] * m[14] - m[4] * m[2] * m[13] -
1310 m[12] * m[1] * m[6] + m[12] * m[2] * m[5];
1311 inv.fa[15] = m[0] * m[5] * m[10] - m[0] * m[6] * m[9] - m[4] * m[1] * m[10] + m[4] * m[2] * m[9] +
1312 m[8] * m[1] * m[6] - m[8] * m[2] * m[5];
1313
1314 // Compute determinant
1315 ::f32 det = m[0] * inv.fa[0] + m[1] * inv.fa[4] + m[2] * inv.fa[8] + m[3] * inv.fa[12];
1316 ::f32 invDet = 1.0f / det;
1317
1318 // Multiply all elements by 1/det using SSE
1319 __m128 v0 = _mm_loadu_ps(&inv.fa[0]);
1320 __m128 v1 = _mm_loadu_ps(&inv.fa[4]);
1321 __m128 v2 = _mm_loadu_ps(&inv.fa[8]);
1322 __m128 v3 = _mm_loadu_ps(&inv.fa[12]);
1323 __m128 det128 = _mm_set1_ps(invDet);
1324
1325 _mm_storeu_ps(&inv.fa[0], _mm_mul_ps(v0, det128));
1326 _mm_storeu_ps(&inv.fa[4], _mm_mul_ps(v1, det128));
1327 _mm_storeu_ps(&inv.fa[8], _mm_mul_ps(v2, det128));
1328 _mm_storeu_ps(&inv.fa[12], _mm_mul_ps(v3, det128));
1329
1330 return inv;
1331 }
1332
1333#endif
1334
1335
1336#if defined(__AVX__)
1337
1338 inline matrix_type inversed_avx() const
1339 requires(DIMENSION == 4 && std::is_same_v<FLOATING, ::f32>)
1340 {
1341
1342 const auto &a = *this;
1343 // Load columns into AVX registers (2 columns per 256-bit register)
1344 __m256 col01 = _mm256_loadu_ps(&a.fa[0]); // columns 0 & 1
1345 __m256 col23 = _mm256_loadu_ps(&a.fa[8]); // columns 2 & 3
1346
1347 // Unpack into scalar f32s for full unrolled computation
1348 ::f32 m[16];
1349 _mm256_storeu_ps(&m[0], col01);
1350 _mm256_storeu_ps(&m[8], col23);
1351
1352 matrix_type inv;
1353
1354 // Fully unrolled cofactor computation (column-major)
1355 inv.fa[0] = m[5] * m[10] * m[15] - m[5] * m[11] * m[14] - m[9] * m[6] * m[15] + m[9] * m[7] * m[14] +
1356 m[13] * m[6] * m[11] - m[13] * m[7] * m[10];
1357 inv.fa[1] = -m[1] * m[10] * m[15] + m[1] * m[11] * m[14] + m[9] * m[2] * m[15] - m[9] * m[3] * m[14] -
1358 m[13] * m[2] * m[11] + m[13] * m[3] * m[10];
1359 inv.fa[2] = m[1] * m[6] * m[15] - m[1] * m[7] * m[14] - m[5] * m[2] * m[15] + m[5] * m[3] * m[14] +
1360 m[13] * m[2] * m[7] - m[13] * m[3] * m[6];
1361 inv.fa[3] = -m[1] * m[6] * m[11] + m[1] * m[7] * m[10] + m[5] * m[2] * m[11] - m[5] * m[3] * m[10] -
1362 m[9] * m[2] * m[7] + m[9] * m[3] * m[6];
1363
1364 inv.fa[4] = -m[4] * m[10] * m[15] + m[4] * m[11] * m[14] + m[8] * m[6] * m[15] - m[8] * m[7] * m[14] -
1365 m[12] * m[6] * m[11] + m[12] * m[7] * m[10];
1366 inv.fa[5] = m[0] * m[10] * m[15] - m[0] * m[11] * m[14] - m[8] * m[2] * m[15] + m[8] * m[3] * m[14] +
1367 m[12] * m[2] * m[11] - m[12] * m[3] * m[10];
1368 inv.fa[6] = -m[0] * m[6] * m[15] + m[0] * m[7] * m[14] + m[4] * m[2] * m[15] - m[4] * m[3] * m[14] -
1369 m[12] * m[2] * m[7] + m[12] * m[3] * m[6];
1370 inv.fa[7] = m[0] * m[6] * m[11] - m[0] * m[7] * m[10] - m[4] * m[2] * m[11] + m[4] * m[3] * m[10] +
1371 m[8] * m[2] * m[7] - m[8] * m[3] * m[6];
1372
1373 inv.fa[8] = m[4] * m[9] * m[15] - m[4] * m[11] * m[13] - m[8] * m[5] * m[15] + m[8] * m[7] * m[13] +
1374 m[12] * m[5] * m[11] - m[12] * m[7] * m[9];
1375 inv.fa[9] = -m[0] * m[9] * m[15] + m[0] * m[11] * m[13] + m[8] * m[1] * m[15] - m[8] * m[3] * m[13] -
1376 m[12] * m[1] * m[11] + m[12] * m[3] * m[9];
1377 inv.fa[10] = m[0] * m[5] * m[15] - m[0] * m[7] * m[13] - m[4] * m[1] * m[15] + m[4] * m[3] * m[13] +
1378 m[12] * m[1] * m[7] - m[12] * m[3] * m[5];
1379 inv.fa[11] = -m[0] * m[5] * m[11] + m[0] * m[7] * m[9] + m[4] * m[1] * m[11] - m[4] * m[3] * m[9] -
1380 m[8] * m[1] * m[7] + m[8] * m[3] * m[5];
1381
1382 inv.fa[12] = -m[4] * m[9] * m[14] + m[4] * m[10] * m[13] + m[8] * m[5] * m[14] - m[8] * m[6] * m[13] -
1383 m[12] * m[5] * m[10] + m[12] * m[6] * m[9];
1384 inv.fa[13] = m[0] * m[9] * m[14] - m[0] * m[10] * m[13] - m[8] * m[1] * m[14] + m[8] * m[2] * m[13] +
1385 m[12] * m[1] * m[10] - m[12] * m[2] * m[9];
1386 inv.fa[14] = -m[0] * m[5] * m[14] + m[0] * m[6] * m[13] + m[4] * m[1] * m[14] - m[4] * m[2] * m[13] -
1387 m[12] * m[1] * m[6] + m[12] * m[2] * m[5];
1388 inv.fa[15] = m[0] * m[5] * m[10] - m[0] * m[6] * m[9] - m[4] * m[1] * m[10] + m[4] * m[2] * m[9] +
1389 m[8] * m[1] * m[6] - m[8] * m[2] * m[5];
1390
1391 // Determinant
1392 ::f32 det = m[0] * inv.fa[0] + m[1] * inv.fa[4] + m[2] * inv.fa[8] + m[3] * inv.fa[12];
1393 ::f32 invDet = 1.0f / det;
1394
1395 // Multiply all 16 elements by 1/det using AVX
1396 __m256 v0 = _mm256_loadu_ps(&inv.fa[0]);
1397 __m256 v1 = _mm256_loadu_ps(&inv.fa[8]);
1398 __m256 invDet256 = _mm256_set1_ps(invDet);
1399 _mm256_storeu_ps(&inv.fa[0], _mm256_mul_ps(v0, invDet256));
1400 _mm256_storeu_ps(&inv.fa[8], _mm256_mul_ps(v1, invDet256));
1401
1402 return inv;
1403 }
1404
1405#endif
1406
1407
1408#if defined(__AVX2__)
1409
1410 inline matrix_type inversed_avx2() const
1411 requires(DIMENSION == 4 && std::is_same_v<FLOATING, ::f32>)
1412 {
1413
1414 const auto &a = *this;
1415 // Load all 16 elements
1416 __m256 col01 = _mm256_loadu_ps(&a.fa[0]); // columns 0 & 1
1417 __m256 col23 = _mm256_loadu_ps(&a.fa[8]); // columns 2 & 3
1418
1419 // Unpack columns into rows for computation
1420 __m128 r0 = _mm256_castps256_ps128(col01); // col0 low
1421 __m128 r1 = _mm256_extractf128_ps(col01, 1); // col1 low
1422 __m128 r2 = _mm256_castps256_ps128(col23); // col2 low
1423 __m128 r3 = _mm256_extractf128_ps(col23, 1); // col3 low
1424
1425 // Compute pairs for cofactors (fully unrolled)
1426 ::f32 m[16];
1427 _mm_storeu_ps(&m[0], r0);
1428 _mm_storeu_ps(&m[4], r1);
1429 _mm_storeu_ps(&m[8], r2);
1430 _mm_storeu_ps(&m[12], r3);
1431
1432 matrix_type inv;
1433
1434 // Fully unrolled cofactor computation (column-major)
1435 inv.fa[0] = m[5] * m[10] * m[15] - m[5] * m[11] * m[14] - m[9] * m[6] * m[15] + m[9] * m[7] * m[14] +
1436 m[13] * m[6] * m[11] - m[13] * m[7] * m[10];
1437 inv.fa[1] = -m[1] * m[10] * m[15] + m[1] * m[11] * m[14] + m[9] * m[2] * m[15] - m[9] * m[3] * m[14] -
1438 m[13] * m[2] * m[11] + m[13] * m[3] * m[10];
1439 inv.fa[2] = m[1] * m[6] * m[15] - m[1] * m[7] * m[14] - m[5] * m[2] * m[15] + m[5] * m[3] * m[14] +
1440 m[13] * m[2] * m[7] - m[13] * m[3] * m[6];
1441 inv.fa[3] = -m[1] * m[6] * m[11] + m[1] * m[7] * m[10] + m[5] * m[2] * m[11] - m[5] * m[3] * m[10] -
1442 m[9] * m[2] * m[7] + m[9] * m[3] * m[6];
1443
1444 inv.fa[4] = -m[4] * m[10] * m[15] + m[4] * m[11] * m[14] + m[8] * m[6] * m[15] - m[8] * m[7] * m[14] -
1445 m[12] * m[6] * m[11] + m[12] * m[7] * m[10];
1446 inv.fa[5] = m[0] * m[10] * m[15] - m[0] * m[11] * m[14] - m[8] * m[2] * m[15] + m[8] * m[3] * m[14] +
1447 m[12] * m[2] * m[11] - m[12] * m[3] * m[10];
1448 inv.fa[6] = -m[0] * m[6] * m[15] + m[0] * m[7] * m[14] + m[4] * m[2] * m[15] - m[4] * m[3] * m[14] -
1449 m[12] * m[2] * m[7] + m[12] * m[3] * m[6];
1450 inv.fa[7] = m[0] * m[6] * m[11] - m[0] * m[7] * m[10] - m[4] * m[2] * m[11] + m[4] * m[3] * m[10] +
1451 m[8] * m[2] * m[7] - m[8] * m[3] * m[6];
1452
1453 inv.fa[8] = m[4] * m[9] * m[15] - m[4] * m[11] * m[13] - m[8] * m[5] * m[15] + m[8] * m[7] * m[13] +
1454 m[12] * m[5] * m[11] - m[12] * m[7] * m[9];
1455 inv.fa[9] = -m[0] * m[9] * m[15] + m[0] * m[11] * m[13] + m[8] * m[1] * m[15] - m[8] * m[3] * m[13] -
1456 m[12] * m[1] * m[11] + m[12] * m[3] * m[9];
1457 inv.fa[10] = m[0] * m[5] * m[15] - m[0] * m[7] * m[13] - m[4] * m[1] * m[15] + m[4] * m[3] * m[13] +
1458 m[12] * m[1] * m[7] - m[12] * m[3] * m[5];
1459 inv.fa[11] = -m[0] * m[5] * m[11] + m[0] * m[7] * m[9] + m[4] * m[1] * m[11] - m[4] * m[3] * m[9] -
1460 m[8] * m[1] * m[7] + m[8] * m[3] * m[5];
1461
1462 inv.fa[12] = -m[4] * m[9] * m[14] + m[4] * m[10] * m[13] + m[8] * m[5] * m[14] - m[8] * m[6] * m[13] -
1463 m[12] * m[5] * m[10] + m[12] * m[6] * m[9];
1464 inv.fa[13] = m[0] * m[9] * m[14] - m[0] * m[10] * m[13] - m[8] * m[1] * m[14] + m[8] * m[2] * m[13] +
1465 m[12] * m[1] * m[10] - m[12] * m[2] * m[9];
1466 inv.fa[14] = -m[0] * m[5] * m[14] + m[0] * m[6] * m[13] + m[4] * m[1] * m[14] - m[4] * m[2] * m[13] -
1467 m[12] * m[1] * m[6] + m[12] * m[2] * m[5];
1468 inv.fa[15] = m[0] * m[5] * m[10] - m[0] * m[6] * m[9] - m[4] * m[1] * m[10] + m[4] * m[2] * m[9] +
1469 m[8] * m[1] * m[6] - m[8] * m[2] * m[5];
1470
1471 // Compute determinant
1472 ::f32 det = m[0] * inv.fa[0] + m[1] * inv.fa[4] + m[2] * inv.fa[8] + m[3] * inv.fa[12];
1473 ::f32 invDet = 1.0f / det;
1474
1475 // Scale all elements by 1/det using AVX2
1476 __m256 v0 = _mm256_loadu_ps(&inv.fa[0]);
1477 __m256 v1 = _mm256_loadu_ps(&inv.fa[8]);
1478 __m256 invDet256 = _mm256_set1_ps(invDet);
1479 _mm256_storeu_ps(&inv.fa[0], _mm256_mul_ps(v0, invDet256));
1480 _mm256_storeu_ps(&inv.fa[8], _mm256_mul_ps(v1, invDet256));
1481
1482 return inv;
1483 }
1484
1485#endif
1486
1487 inline matrix_type & inverse()
1488 {
1489
1490 if constexpr ( std::is_same_v<FLOATING, ::f32>)
1491 {
1492 if constexpr (DIMENSION == 4)
1493 {
1494 //if (g_cpufeatures.m_bAVX2)
1495 // return inversed_avx2();
1496 //else if (g_cpufeatures.m_bAVX)
1497 // return inversed_avx();
1498 //else if (g_cpufeatures.m_bSSE)
1499 // return inversed_sse();
1500 //else
1501 return inverse_scalar();
1502 }
1503 else if constexpr (DIMENSION == 3)
1504 {
1505 //if (g_cpufeatures.m_bAVX)
1506 // return inversed_avx();
1507 //if (g_cpufeatures.m_bSSE)
1508 // return inversed_sse();
1509 //else
1510 return inverse_scalar();
1511 }
1512 else
1513 {
1514 return inverse_scalar();
1515 }
1516
1517 }
1518 else
1519 {
1520 return inverse_scalar();
1521 }
1522
1523 }
1524
1525
1526 inline matrix_type inversed() const
1527 {
1528
1529 auto m = *this;
1530
1531 m.inverse();
1532
1533 return m;
1534
1535 }
1536
1537
1538 inline matrix_type transposed() const
1539 {
1540
1541 auto m = *this;
1542
1543 m.transpose();
1544
1545 return m;
1546
1547 }
1548
1549
1550 inline matrix_type &operator=(const quaternion_type<FLOATING> &q)
1551 requires (DIMENSION == 4)
1552 {
1553 auto & M = *this;
1554
1555 auto xx = q.x * q.x;
1556 auto yy = q.y * q.y;
1557 auto zz = q.z * q.z;
1558
1559 auto xy = q.x * q.y;
1560 auto xz = q.x * q.z;
1561 auto yz = q.y * q.z;
1562
1563 auto wx = q.w * q.x;
1564 auto wy = q.w * q.y;
1565 auto wz = q.w * q.z;
1566
1567 // Column-major (GLM style)
1568 M.m[0][0] = (FLOATING)1 - (FLOATING)2 * (yy + zz);
1569 M.m[0][1] = (FLOATING)2 * (xy + wz);
1570 M.m[0][2] = (FLOATING)2 * (xz - wy);
1571 M.m[0][3] = (FLOATING)0;
1572
1573 M.m[1][0] = (FLOATING)2 * (xy - wz);
1574 M.m[1][1] = (FLOATING)1 - (FLOATING)2 * (xx + zz);
1575 M.m[1][2] = (FLOATING)2 * (yz + wx);
1576 M.m[1][3] = (FLOATING)0;
1577
1578 M.m[2][0] = (FLOATING)2 * (xz + wy);
1579 M.m[2][1] = (FLOATING)2 * (yz - wx);
1580 M.m[2][2] = (FLOATING)1 - (FLOATING)2 * (xx + yy);
1581 M.m[2][3] = (FLOATING)0;
1582
1583 M.m[3][0] = (FLOATING)0;
1584 M.m[3][1] = (FLOATING)0;
1585 M.m[3][2] = (FLOATING)0;
1586 M.m[3][3] = (FLOATING)1;
1587
1588 return *this;
1589
1590 }
1591
1592
1593 static inline matrix_type from(const quaternion_type<FLOATING> &q)
1594 requires(DIMENSION == 4)
1595 {
1596
1597 return q;
1598
1599 }
1600
1601
1602
1603 inline row_major_type<FLOATING, t_iDimension> transpose() const
1604 {
1605 row_major_type<FLOATING, 4> r;
1606 _transpose(4, this->m);
1607 return r;
1608 }
1609};
1610
1611
1612using f32_matrix2 = matrix_type<::f32, 2>;
1613using f32_matrix3 = matrix_type<::f32, 3>;
1614using f32_matrix4 = matrix_type<::f32, 4>;
1615
1616
1617using f64_matrix2 = matrix_type<::f64, 2>;
1618using f64_matrix3 = matrix_type<::f64, 3>;
1619using f64_matrix4 = matrix_type<::f64, 4>;
1620
1621//
1622//namespace geometry
1623//{
1624//
1625//
1626// // static inline void _transpose(FLOATING A_target[t_iDimension][t_iDimension],
1627// // const FLOATING A_source[t_iDimension][t_iDimension])
1628// //{
1629// // for (::i32 i = 0; i < t_iDimension; i++)
1630// // for (::i32 j = 0; j < t_iDimension; j++)
1631// // A_target[i][j] = A_source[j][i];
1632// //}
1633// //
1634//
1635// ////template<prototype_floating FLOATING>
1636// ////row_major_type<FLOATING, 4> transpose(const matrix_type<FLOATING, 4> &m)
1637// ////{
1638// //// row_major_type<FLOATING, 4> r;
1639// //// _transpose(4, m.m);
1640// //// return r;
1641// ////}
1642//
1643//
1644// ////template<prototype_floating FLOATING>
1645// //////matrix_type<FLOATING, 3> inverse(const matrix_type<FLOATING, 3> &m)
1646// //////{
1647//
1648// ////// ::f32 det = m[0][0] * (m[1][1] * m[2][2] - m[2][1] * m[1][2]) -
1649// ////// m[1][0] * (m[0][1] * m[2][2] - m[2][1] * m[0][2]) + m[2][0] * (m[0][1] * m[1][2] - m[1][1] * m[0][2]);
1650//
1651// ////// if (std::fabs(det) < 1e-8f)
1652// ////// return matrix_type<FLOATING, 3>(1.0f); // fallback to identity (non-invertible)
1653//
1654// ////// ::f32 invDet = 1.0f / det;
1655//
1656// ////// matrix_type<FLOATING, 3> r;
1657// ////// r[0][0] = (m[1][1] * m[2][2] - m[2][1] * m[1][2]) * invDet;
1658// ////// r[1][0] = -(m[1][0] * m[2][2] - m[2][0] * m[1][2]) * invDet;
1659// ////// r[2][0] = (m[1][0] * m[2][1] - m[2][0] * m[1][1]) * invDet;
1660//
1661// ////// r[0][1] = -(m[0][1] * m[2][2] - m[2][1] * m[0][2]) * invDet;
1662// ////// r[1][1] = (m[0][0] * m[2][2] - m[2][0] * m[0][2]) * invDet;
1663// ////// r[2][1] = -(m[0][0] * m[2][1] - m[2][0] * m[0][1]) * invDet;
1664//
1665// ////// r[0][2] = (m[0][1] * m[1][2] - m[1][1] * m[0][2]) * invDet;
1666// ////// r[1][2] = -(m[0][0] * m[1][2] - m[1][0] * m[0][2]) * invDet;
1667// ////// r[2][2] = (m[0][0] * m[1][1] - m[1][0] * m[0][1]) * invDet;
1668//
1669// ////// return r;
1670// //////}
1671//
1672// //
1673//
1674//
1675// //template<prototype_floating FLOATING>
1676// //quaternion_type < FLOATING > quaternion(const ::sequence_type<FLOATING, 3> & euler)
1677// //{
1678//
1679// // return euler;
1680//
1681// //}
1682//
1683// //template<prototype_floating FLOATING>
1684// //inline matrix_type<FLOATING, 4> inverse(const matrix_type<FLOATING, 4> &A)
1685// //{
1686// // matrix_type<FLOATING, 4> inv{};
1687// // const FLOATING *m = &A.fa[0][0]; // treat as flat array
1688//
1689// // FLOATING invOut[16];
1690//
1691// // invOut[0] = m[5] * m[10] * m[15] - m[5] * m[11] * m[14] - m[9] * m[6] * m[15] + m[9] * m[7] * m[14] +
1692// // m[13] * m[6] * m[11] - m[13] * m[7] * m[10];
1693//
1694// // invOut[4] = -m[4] * m[10] * m[15] + m[4] * m[11] * m[14] + m[8] * m[6] * m[15] - m[8] * m[7] * m[14] -
1695// // m[12] * m[6] * m[11] + m[12] * m[7] * m[10];
1696//
1697// // invOut[8] = m[4] * m[9] * m[15] - m[4] * m[11] * m[13] - m[8] * m[5] * m[15] + m[8] * m[7] * m[13] +
1698// // m[12] * m[5] * m[11] - m[12] * m[7] * m[9];
1699//
1700// // invOut[12] = -m[4] * m[9] * m[14] + m[4] * m[10] * m[13] + m[8] * m[5] * m[14] - m[8] * m[6] * m[13] -
1701// // m[12] * m[5] * m[10] + m[12] * m[6] * m[9];
1702//
1703// // invOut[1] = -m[1] * m[10] * m[15] + m[1] * m[11] * m[14] + m[9] * m[2] * m[15] - m[9] * m[3] * m[14] -
1704// // m[13] * m[2] * m[11] + m[13] * m[3] * m[10];
1705//
1706// // invOut[5] = m[0] * m[10] * m[15] - m[0] * m[11] * m[14] - m[8] * m[2] * m[15] + m[8] * m[3] * m[14] +
1707// // m[12] * m[2] * m[11] - m[12] * m[3] * m[10];
1708//
1709// // invOut[9] = -m[0] * m[9] * m[15] + m[0] * m[11] * m[13] + m[8] * m[1] * m[15] - m[8] * m[3] * m[13] -
1710// // m[12] * m[1] * m[11] + m[12] * m[3] * m[9];
1711//
1712// // invOut[13] = m[0] * m[9] * m[14] - m[0] * m[10] * m[13] - m[8] * m[1] * m[14] + m[8] * m[2] * m[13] +
1713// // m[12] * m[1] * m[10] - m[12] * m[2] * m[9];
1714//
1715// // invOut[2] = m[1] * m[6] * m[15] - m[1] * m[7] * m[14] - m[5] * m[2] * m[15] + m[5] * m[3] * m[14] +
1716// // m[13] * m[2] * m[7] - m[13] * m[3] * m[6];
1717//
1718// // invOut[6] = -m[0] * m[6] * m[15] + m[0] * m[7] * m[14] + m[4] * m[2] * m[15] - m[4] * m[3] * m[14] -
1719// // m[12] * m[2] * m[7] + m[12] * m[3] * m[6];
1720//
1721// // invOut[10] = m[0] * m[5] * m[15] - m[0] * m[7] * m[13] - m[4] * m[1] * m[15] + m[4] * m[3] * m[13] +
1722// // m[12] * m[1] * m[7] - m[12] * m[3] * m[5];
1723//
1724// // invOut[14] = -m[0] * m[5] * m[14] + m[0] * m[6] * m[13] + m[4] * m[1] * m[14] - m[4] * m[2] * m[13] -
1725// // m[12] * m[1] * m[6] + m[12] * m[2] * m[5];
1726//
1727// // invOut[3] = -m[1] * m[6] * m[11] + m[1] * m[7] * m[10] + m[5] * m[2] * m[11] - m[5] * m[3] * m[10] -
1728// // m[9] * m[2] * m[7] + m[9] * m[3] * m[6];
1729//
1730// // invOut[7] = m[0] * m[6] * m[11] - m[0] * m[7] * m[10] - m[4] * m[2] * m[11] + m[4] * m[3] * m[10] +
1731// // m[8] * m[2] * m[7] - m[8] * m[3] * m[6];
1732//
1733// // invOut[11] = -m[0] * m[5] * m[11] + m[0] * m[7] * m[9] + m[4] * m[1] * m[11] - m[4] * m[3] * m[9] -
1734// // m[8] * m[1] * m[7] + m[8] * m[3] * m[5];
1735//
1736// // invOut[15] = m[0] * m[5] * m[10] - m[0] * m[6] * m[9] - m[4] * m[1] * m[10] + m[4] * m[2] * m[9] +
1737// // m[8] * m[1] * m[6] - m[8] * m[2] * m[5];
1738//
1739// // ::f32 det = m[0] * invOut[0] + m[1] * invOut[4] + m[2] * invOut[8] + m[3] * invOut[12];
1740//
1741// // // Determinant zero → not invertible
1742// // if (det == 0.0f)
1743// // return {}; // or assert / return identity
1744//
1745// // ::f32 invDet = 1.0f / det;
1746//
1747// // matrix_type<FLOATING, 4> R{};
1748// // for (::i32 i = 0; i < 16; i++)
1749// // ((::f32 *)R.m)[i] = invOut[i] * invDet;
1750//
1751// // return R;
1752// //}
1753//
1754//
1755// //template<prototype_floating FLOATING_TYPE>
1756// //inline row_major_type<FLOATING_TYPE, 4> inverse(const row_major_type<FLOATING_TYPE, 4> &M)
1757// //{
1758// // auto T = transpose(M);
1759// // auto C = inverse(T);
1760// // return transpose(C);
1761// //}
1762// //// --- Fast affine inverse (rotation + translation only) --------------------
1763// //template<prototype_floating FLOATING>
1764// //inline matrix_type<FLOATING, 4> inverse_affine(const matrix_type<FLOATING, 4> &A)
1765// //{
1766// // // assumes bottom row is [0 0 0 1] (i.e. affine matrix)
1767// // // and upper-left 3x3 is orthonormal (rotation + uniform scale = 1)
1768// // matrix_type<FLOATING, 4> R;
1769// // // transpose the 3x3 rotation
1770// // for (::i32 c = 0; c < 3; ++c)
1771// // for (::i32 r = 0; r < 3; ++r)
1772// // R.m[c][r] = A.m[r][c];
1773//
1774// // // compute -R^T * translation
1775// // ::f32 tx = A.m[3][0], ty = A.m[3][1], tz = A.m[3][2];
1776// // R.m[3][0] = -(R.m[0][0] * tx + R.m[1][0] * ty + R.m[2][0] * tz);
1777// // R.m[3][1] = -(R.m[0][1] * tx + R.m[1][1] * ty + R.m[2][1] * tz);
1778// // R.m[3][2] = -(R.m[0][2] * tx + R.m[1][2] * ty + R.m[2][2] * tz);
1779//
1780// // // last column/row
1781// // R.m[0][3] = R.m[1][3] = R.m[2][3] = 0.0f;
1782// // R.m[3][3] = 1.0f;
1783//
1784// // return R;
1785// //}
1786//
1787//
1788// //inline f32_matrix4 inverse_col_major_simd(const f32_matrix4 &A)
1789// //{
1790// // const __m128 row0 = _mm_loadu_ps(&A.m[0][0]);
1791// // const __m128 row1 = _mm_loadu_ps(&A.m[1][0]);
1792// // const __m128 row2 = _mm_loadu_ps(&A.m[2][0]);
1793// // const __m128 row3 = _mm_loadu_ps(&A.m[3][0]);
1794//
1795// // // Compute cofactors
1796// // __m128 tmp1, tmp2, tmp3, minor0, minor1, minor2, minor3;
1797//
1798// // tmp1 = _mm_mul_ps(row2, row3);
1799// // tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(1, 0, 3, 2));
1800//
1801// // tmp2 = _mm_mul_ps(row1, tmp1);
1802// // tmp3 = _mm_mul_ps(row0, tmp1);
1803//
1804// // tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(2, 3, 0, 1));
1805//
1806// // tmp2 = _mm_sub_ps(tmp2, _mm_mul_ps(row1, tmp1));
1807// // tmp3 = _mm_sub_ps(tmp3, _mm_mul_ps(row0, tmp1));
1808//
1809// // minor0 = tmp2;
1810// // minor1 = tmp3;
1811//
1812// // // Continue building cofactor matrix
1813// // tmp1 = _mm_mul_ps(row1, row2);
1814// // tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(1, 0, 3, 2));
1815//
1816// // tmp2 = _mm_mul_ps(row3, tmp1);
1817// // tmp3 = _mm_mul_ps(row0, tmp1);
1818//
1819// // tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(2, 3, 0, 1));
1820//
1821// // tmp2 = _mm_sub_ps(tmp2, _mm_mul_ps(row3, tmp1));
1822// // tmp3 = _mm_sub_ps(tmp3, _mm_mul_ps(row0, tmp1));
1823//
1824// // minor2 = tmp2;
1825// // minor3 = tmp3;
1826//
1827// // // Compute determinant
1828// // __m128 det = _mm_mul_ps(row0, minor0);
1829// // det = _mm_add_ps(det, _mm_shuffle_ps(det, det, _MM_SHUFFLE(2, 3, 0, 1)));
1830// // det = _mm_add_ps(det, _mm_shuffle_ps(det, det, _MM_SHUFFLE(1, 0, 3, 2)));
1831//
1832// // __m128 invDet = _mm_div_ps(_mm_set1_ps(1.0f), det);
1833//
1834// // // Multiply cofactors by reciprocal determinant
1835// // minor0 = _mm_mul_ps(minor0, invDet);
1836// // minor1 = _mm_mul_ps(minor1, invDet);
1837// // minor2 = _mm_mul_ps(minor2, invDet);
1838// // minor3 = _mm_mul_ps(minor3, invDet);
1839//
1840// // f32_matrix4 R;
1841//
1842// // _mm_storeu_ps(&R.m[0][0], minor0);
1843// // _mm_storeu_ps(&R.m[1][0], minor1);
1844// // _mm_storeu_ps(&R.m[2][0], minor2);
1845// // _mm_storeu_ps(&R.m[3][0], minor3);
1846//
1847// // return R;
1848// //}
1900//
2005//
2015// // // Multiply 4x4 matrix `a` by 4x4 matrix `b` (a*b) using AVX2 fully unrolled
2016// //inline f32_matrix4 mul_avx2(const f32_matrix4 &a, const f32_matrix4 &b)
2017// //{
2018// // f32_matrix4 result;
2019//
2020// // // Load columns of B
2021// // __m128 b0 = _mm_loadu_ps(&b.fa[0]);
2022// // __m128 b1 = _mm_loadu_ps(&b.fa[4]);
2023// // __m128 b2 = _mm_loadu_ps(&b.fa[8]);
2024// // __m128 b3 = _mm_loadu_ps(&b.fa[12]);
2025//
2026// // // For each column of B, broadcast each element and multiply-add with columns of A
2027// // for (::i32 col = 0; col < 4; ++col)
2028// // {
2029// // __m128 bc; // broadcast column element
2030// // __m256 sum01, sum23;
2031//
2032// // switch (col)
2033// // {
2034// // case 0:
2035// // bc = b0;
2036// // break;
2037// // case 1:
2038// // bc = b1;
2039// // break;
2040// // case 2:
2041// // bc = b2;
2042// // break;
2043// // case 3:
2044// // bc = b3;
2045// // break;
2046// // }
2047//
2048// // // Broadcast each element of the column
2049// // __m256 b0_256 = _mm256_broadcast_ps((__m128 *)&bc); // replicate column
2050// // // Load A columns
2051// // __m256 a01 = _mm256_loadu_ps(&a.fa[0]); // columns 0 & 1
2052// // __m256 a23 = _mm256_loadu_ps(&a.fa[8]); // columns 2 & 3
2053//
2054// // // Multiply and sum
2055// // sum01 = _mm256_mul_ps(a01, b0_256); // element-wise multiply
2056// // sum23 = _mm256_mul_ps(a23, b0_256);
2057//
2058// // // Add results
2059// // __m256 result256_0 = _mm256_add_ps(sum01, sum23);
2060//
2061// // // Store back
2062// // _mm256_storeu_ps(&result.fa[0], result256_0); // fully unrolled: can expand by column
2063// // }
2064//
2065// // return result;
2066// //}
2067//
2068//
2069//} // namespace geometry
2070//
2071//
2072
2073
2074
2075
2076
2077
2078//
2079//
2080// // Load all elements into scalar array for cofactor calculation
2081//::f32 m[16];
2082//_mm256_storeu_ps(&m[0], _mm256_loadu_ps(&a.fa[0])); // first 8 elements
2083//_mm256_storeu_ps(&m[8], _mm256_loadu_ps(&a.fa[8])); // last 8 elements
2084//
2086//inv.fa[0] = m[5] * m[10] * m[15] - m[5] * m[11] * m[14] - m[9] * m[6] * m[15] + m[9] * m[7] * m[14] +
2087// m[13] * m[6] * m[11] - m[13] * m[7] * m[10];
2088//inv.fa[1] = -m[1] * m[10] * m[15] + m[1] * m[11] * m[14] + m[9] * m[2] * m[15] - m[9] * m[3] * m[14] -
2089// m[13] * m[2] * m[11] + m[13] * m[3] * m[10];
2090//inv.fa[2] = m[1] * m[6] * m[15] - m[1] * m[7] * m[14] - m[5] * m[2] * m[15] + m[5] * m[3] * m[14] + m[13] * m[2] * m[7] -
2091// m[13] * m[3] * m[6];
2092//inv.fa[3] = -m[1] * m[6] * m[11] + m[1] * m[7] * m[10] + m[5] * m[2] * m[11] - m[5] * m[3] * m[10] - m[9] * m[2] * m[7] +
2093// m[9] * m[3] * m[6];
2094//
2095//inv.fa[4] = -m[4] * m[10] * m[15] + m[4] * m[11] * m[14] + m[8] * m[6] * m[15] - m[8] * m[7] * m[14] -
2096// m[12] * m[6] * m[11] + m[12] * m[7] * m[10];
2097//inv.fa[5] = m[0] * m[10] * m[15] - m[0] * m[11] * m[14] - m[8] * m[2] * m[15] + m[8] * m[3] * m[14] +
2098// m[12] * m[2] * m[11] - m[12] * m[3] * m[10];
2099//inv.fa[6] = -m[0] * m[6] * m[15] + m[0] * m[7] * m[14] + m[4] * m[2] * m[15] - m[4] * m[3] * m[14] -
2100// m[12] * m[2] * m[7] + m[12] * m[3] * m[6];
2101//inv.fa[7] = m[0] * m[6] * m[11] - m[0] * m[7] * m[10] - m[4] * m[2] * m[11] + m[4] * m[3] * m[10] + m[8] * m[2] * m[7] -
2102// m[8] * m[3] * m[6];
2103//
2104//inv.fa[8] = m[4] * m[9] * m[15] - m[4] * m[11] * m[13] - m[8] * m[5] * m[15] + m[8] * m[7] * m[13] +
2105// m[12] * m[5] * m[11] - m[12] * m[7] * m[9];
2106//inv.fa[9] = -m[0] * m[9] * m[15] + m[0] * m[11] * m[13] + m[8] * m[1] * m[15] - m[8] * m[3] * m[13] -
2107// m[12] * m[1] * m[11] + m[12] * m[3] * m[9];
2108//inv.fa[10] = m[0] * m[5] * m[15] - m[0] * m[7] * m[13] - m[4] * m[1] * m[15] + m[4] * m[3] * m[13] +
2109// m[12] * m[1] * m[7] - m[12] * m[3] * m[5];
2110//inv.fa[11] = -m[0] * m[5] * m[11] + m[0] * m[7] * m[9] + m[4] * m[1] * m[11] - m[4] * m[3] * m[9] - m[8] * m[1] * m[7] +
2111// m[8] * m[3] * m[5];
2112//
2113//inv.fa[12] = -m[4] * m[9] * m[14] + m[4] * m[10] * m[13] + m[8] * m[5] * m[14] - m[8] * m[6] * m[13] -
2114// m[12] * m[5] * m[10] + m[12] * m[6] * m[9];
2115//inv.fa[13] = m[0] * m[9] * m[14] - m[0] * m[10] * m[13] - m[8] * m[1] * m[14] + m[8] * m[2] * m[13] +
2116// m[12] * m[1] * m[10] - m[12] * m[2] * m[9];
2117//inv.fa[14] = -m[0] * m[5] * m[14] + m[0] * m[6] * m[13] + m[4] * m[1] * m[14] - m[4] * m[2] * m[13] -
2118// m[12] * m[1] * m[6] + m[12] * m[2] * m[5];
2119//inv.fa[15] = m[0] * m[5] * m[10] - m[0] * m[6] * m[9] - m[4] * m[1] * m[10] + m[4] * m[2] * m[9] + m[8] * m[1] * m[6] -
2120// m[8] * m[2] * m[5];
2121//
2123//::f32 det = m[0] * inv.fa[0] + m[1] * inv.fa[4] + m[2] * inv.fa[8] + m[3] * inv.fa[12];
2124//::f32 invDet = 1.0f / det;
2125//
2127//__m256 v0 = _mm256_loadu_ps(&inv.fa[0]);
2128//__m256 v1 = _mm256_loadu_ps(&inv.fa[8]);
2129//__m256 det256 = _mm256_set1_ps(invDet);
2130//
2131//_mm256_storeu_ps(&inv.fa[0], _mm256_mul_ps(v0, det256));
2132//_mm256_storeu_ps(&inv.fa[8], _mm256_mul_ps(v1, det256));
2133
2134
2135template<prototype_floating FLOATING, ::i32 t_iDimension>
2136inline matrix_type<FLOATING, t_iDimension> row_major_type< FLOATING, t_iDimension >::transpose() const
2137{
2138 matrix_type<FLOATING, 4> r;
2139 _transpose(4, this->m);
2140 return r;
2141}
2142
2143
2144template<prototype_number NUMBER, ::i32 t_iSize>
2145template<prototype_floating FLOATING1>
2146inline sequence_type<NUMBER, t_iSize>
2147sequence_type<NUMBER, t_iSize>::operator*(const matrix_type<FLOATING1, 3> &M) const
2148 requires(t_iSize == 3 && std::is_same_v < FLOATING1, NUMBER >)
2149{
2150 const auto &v = *this;
2151 sequence_type result;
2152 result.x = v.x * M[0][0] + v.y * M[0][1] + v.z * M[0][2];
2153 result.y = v.x * M[1][0] + v.y * M[1][1] + v.z * M[1][2];
2154 result.z = v.x * M[2][0] + v.y * M[2][1] + v.z * M[2][2];
2155 return result;
2156}
2157
2158
2159template<prototype_number NUMBER, ::i32 t_iSize>
2160 template<prototype_floating FLOATING1>
2161inline sequence_type<NUMBER, t_iSize>
2162sequence_type<NUMBER, t_iSize>::operator*(const matrix_type<FLOATING1, 4> &M) const
2163 requires(t_iSize == 4 && ::std::is_same_v<FLOATING1, NUMBER>)
2164{
2165
2166 const auto &v = *this;
2167 sequence_type result;
2168
2169 result.x = v.x * M[0][0] + v.y * M[0][1] + v.z * M[0][2] + v.w * M[0][3];
2170 result.y = v.x * M[1][0] + v.y * M[1][1] + v.z * M[1][2] + v.w * M[1][3];
2171 result.z = v.x * M[2][0] + v.y * M[2][1] + v.z * M[2][2] + v.w * M[2][3];
2172 result.w = v.x * M[3][0] + v.y * M[3][1] + v.z * M[3][2] + v.w * M[3][3];
2173
2174 return result;
2175}
2176
2177
2178
2179template<prototype_number UNIT_TYPE, ::i32 t_iSize>
2180auto sequence_type<UNIT_TYPE, t_iSize>::as_translation_matrix() const
2181 requires(t_iSize == 3 && prototype_floating<UNIT_TYPE>)
2182{
2183
2184 return matrix_type<UNIT_TYPE, 4>{(UNIT_TYPE)1}.translated(*this);
2185
2186}
2187
2188
2189template<prototype_number UNIT_TYPE, ::i32 t_iSize>
2190auto sequence_type<UNIT_TYPE, t_iSize>::as_scaling_matrix() const
2191 requires(t_iSize == 3 && prototype_floating<UNIT_TYPE>)
2192{
2193
2194 return matrix_type<UNIT_TYPE, 4>{(UNIT_TYPE)1}.scaled(*this);
2195
2196}
2197
2198template<prototype_number NUMBER, prototype_floating FLOATING_TYPE, ::i32 t_iDimension, enum_matrix_major t_emajor>
2199inline matrix_type<FLOATING_TYPE, t_iDimension, t_emajor>
2200operator*(NUMBER scalar, const matrix_type<FLOATING_TYPE, t_iDimension, t_emajor> & matrix)
2201{
2202
2203 return matrix * scalar;
2204
2205}
prototype_floating
Definition _initial_concepts.h:106
matrix_type::column
Definition matrix.h:77
matrix_type
Matrix class.
Definition matrix.h:73
matrix_type::matrix_type
matrix_type(const FLOATING1 *p)
Definition matrix.h:151
row_major_type
Simple matrix class with row-major storage.
Definition matrix.h:49
sequence_type
Definition sequence.h:236
translation_t
Definition matrix.h:66