matvec.h
Go to the documentation of this file.
1#pragma once
2
3#ifdef __cplusplus
4extern "C" {
5#endif
6
7// Developer helpers to visualize matrices
8#if 0
9void mat_fshow(float const *A, int n, char const *name)
10{
11 printf("%s = [\n",name);
12 for(int i = 0;i<n;i++)
13 {
14 for(int j = 0;j<n;j++) printf(" %+.4e",A[i+n*j]);
15 printf("\n");
16 }
17 printf("]\n\n");
18}
19
20void mat_show(double const *A, int n, char const *name)
21{
22 printf("%s = [\n",name);
23 for(int i = 0;i<n;i++)
24 {
25 for(int j = 0;j<n;j++) printf(" %+.4e",A[i+n*j]);
26 printf("\n");
27 }
28 printf("]\n\n");
29}
30#endif
31
38void mat2_rmul(double *A, double const *B)
39{
40 double M[4];
41 M[0] = A[0]*B[0] + A[2]*B[1];
42 M[1] = A[1]*B[0] + A[3]*B[1];
43 M[2] = A[0]*B[2] + A[2]*B[3];
44 M[3] = A[1]*B[2] + A[3]*B[3];
45 for(int k=0;k<4;k++) A[k]=M[k];
46}
47
54void mat2_lmul(double const *A, double *B)
55{
56 double M[4];
57 M[0] = A[0]*B[0] + A[2]*B[1];
58 M[1] = A[1]*B[0] + A[3]*B[1];
59 M[2] = A[0]*B[2] + A[2]*B[3];
60 M[3] = A[1]*B[2] + A[3]*B[3];
61 for(int k=0;k<4;k++) B[k]=M[k];
62}
63
71void mat2_vfms(double *C, double const *A, double const *B)
72{
73 double M[4];
74 M[0] = A[0]*B[0] + A[2]*B[1];
75 M[1] = A[1]*B[0] + A[3]*B[1];
76 M[2] = A[0]*B[2] + A[2]*B[3];
77 M[3] = A[1]*B[2] + A[3]*B[3];
78 for(int k=0;k<4;k++) C[k]-=M[k];
79}
80
87void mat2_inv(double *invA, double const *A)
88{
89 double M[4];
90 M[0] = A[3];
91 M[1] = -A[1];
92 M[2] = -A[2];
93 M[3] = A[0];
94
95 double inv_det = 1.0/(M[0]*M[3]-M[1]*M[2]);
96 for(int k=0;k<4;k++) invA[k]=inv_det*M[k];
97}
98
107void mat4_rmul(double *A, double const *B)
108{
109 // load left hand matrix
110 __m256d vA[4];
111 vA[0] = _mm256_loadu_pd(A+ 0);
112 vA[1] = _mm256_loadu_pd(A+ 4);
113 vA[2] = _mm256_loadu_pd(A+ 8);
114 vA[3] = _mm256_loadu_pd(A+12);
115
116 for(int j=0;j<4;j++)
117 {
118 // load column j of B matrix
119 __m256d vbj = _mm256_loadu_pd(B+4*j);
120
121 // multiply matrix A with column j of matrix B
122 __m256d vAB[4];
123 vAB[0] = vA[0]*_mm256_permute4x64_pd(vbj,0x00);
124 vAB[1] = vA[1]*_mm256_permute4x64_pd(vbj,0x55); // 0b01010101
125 vAB[2] = vA[2]*_mm256_permute4x64_pd(vbj,0xAA); // 0b10101010
126 vAB[3] = vA[3]*_mm256_permute4x64_pd(vbj,0xFF); // 0b10101010
127
128 __m256d vz = vAB[0] + vAB[1] + vAB[2] + vAB[3];
129
130 // Store result in column j of matrix A
131 _mm256_storeu_pd(A+4*j,vz);
132 }
133}
134
143void mat4_lmul(double const *A, double *B)
144{
145 // load left hand matrix
146 __m256d vA[4];
147 vA[0] = _mm256_loadu_pd(A+ 0);
148 vA[1] = _mm256_loadu_pd(A+ 4);
149 vA[2] = _mm256_loadu_pd(A+ 8);
150 vA[3] = _mm256_loadu_pd(A+12);
151
152 for(int j=0;j<4;j++)
153 {
154 // load column j of B matrix
155 __m256d vbj = _mm256_loadu_pd(B+4*j);
156
157 // multiply matrix A with column j of matrix B
158 __m256d vAB[4];
159 vAB[0] = vA[0]*_mm256_permute4x64_pd(vbj,0x00);
160 vAB[1] = vA[1]*_mm256_permute4x64_pd(vbj,0x55); // 0b01010101
161 vAB[2] = vA[2]*_mm256_permute4x64_pd(vbj,0xAA); // 0b10101010
162 vAB[3] = vA[3]*_mm256_permute4x64_pd(vbj,0xFF); // 0b10101010
163
164 __m256d vz = vAB[0] + vAB[1] + vAB[2] + vAB[3];
165
166 // Store result in column j of matrix B
167 _mm256_storeu_pd(B+4*j,vz);
168 }
169}
170
180void mat4_vfms(double *C, double const *A, double const *B)
181{
182 // load left hand matrix
183 __m256d vA[4];
184 vA[0] = _mm256_loadu_pd(A+ 0);
185 vA[1] = _mm256_loadu_pd(A+ 4);
186 vA[2] = _mm256_loadu_pd(A+ 8);
187 vA[3] = _mm256_loadu_pd(A+12);
188
189 for(int j=0;j<4;j++)
190 {
191 // load column j of B matrix
192 __m256d vbj = _mm256_loadu_pd(B+4*j);
193
194 // multiply matrix A with column j of matrix B
195 __m256d vAB[4];
196 vAB[0] = vA[0]*_mm256_permute4x64_pd(vbj,0x00);
197 vAB[1] = vA[1]*_mm256_permute4x64_pd(vbj,0x55); // 0b01010101
198 vAB[2] = vA[2]*_mm256_permute4x64_pd(vbj,0xAA); // 0b10101010
199 vAB[3] = vA[3]*_mm256_permute4x64_pd(vbj,0xFF); // 0b10101010
200
201 __m256d vz = vAB[0] + vAB[1] + vAB[2] + vAB[3];
202
203 // Subtract from column j of matrix C
204 vz = _mm256_loadu_pd(C+4*j) - vz;
205
206 // Store result in column j of matrix C
207 _mm256_storeu_pd(C+4*j,vz);
208 }
209}
210
211
222void mat4_inv(double *invA, const double *A)
223{
224 double M[16];
225 for(int k=0;k<16;k++) M[k] = A[k];
226
227 for(int k=0;k<4;k++)
228 {
229 double scale=-1.0/M[5*k];
230 for(int i=0;i<4;i++) M[i+4*k] *= i==k?0:scale; // scale column k
231 for(int j=0;j<4;j++)
232 {
233 if (j==k) continue;
234 for(int i=0;i<4;i++) M[i+4*j] += i==k?0:M[i+4*k]*M[k+4*j]; //sweep
235 }
236 scale=-scale;
237 for(int j=0;j<4;j++) M[k+4*j] *= scale; // scale row k
238 M[5*k] = scale;
239 }
240
241 for(int k=0;k<16;k++) invA[k] = M[k];
242}
243
244
255void mat4_vinv(double *invA, double const *A)
256{
257 __m256d col0, col1, col2, col3;
258 __m256d row0, row1, row2, row3;
259 __m256d tmp1, det;
260
261 // load columns
262 col0 = _mm256_load_pd(A+ 0);
263 col1 = _mm256_load_pd(A+ 4);
264 col2 = _mm256_load_pd(A+ 8);
265 col3 = _mm256_load_pd(A+12);
266
267 tmp1 = _mm256_unpacklo_pd(col0,col1);
268 row2 = _mm256_unpacklo_pd(col2,col3);
269
270 row0 = _mm256_permute2f128_pd(tmp1, row2, 0x20);
271 row2 = _mm256_permute2f128_pd(row2, tmp1, 0x13);
272
273 tmp1 = _mm256_unpackhi_pd(col0,col1);
274 row1 = _mm256_unpackhi_pd(col2,col3);
275
276 row3 = _mm256_permute2f128_pd(tmp1, row1, 0x13);
277 row1 = _mm256_permute2f128_pd(row1, tmp1, 0x20);
278 // ---------------------------------------------
279 tmp1 = _mm256_mul_pd(row2, row3);
280 tmp1 = _mm256_permute_pd(tmp1, 0x05);
281
282 col0 = _mm256_mul_pd(row1, tmp1);
283 col1 = _mm256_mul_pd(row0, tmp1);
284
285 tmp1 = _mm256_permute4x64_pd(tmp1,0x4E);
286
287 col0 = _mm256_fmsub_pd(row1,tmp1,col0);
288 col1 = _mm256_fmsub_pd(row0,tmp1,col1);
289 col1 = _mm256_permute4x64_pd(col1,0x4E);
290 // -----------------------------------------------
291 tmp1 = _mm256_mul_pd(row1, row2);
292 tmp1 = _mm256_permute_pd(tmp1, 0x05);
293
294 col0 = _mm256_fmadd_pd(row3,tmp1,col0);
295 col3 = _mm256_mul_pd(row0, tmp1);
296
297 tmp1 = _mm256_permute4x64_pd(tmp1,0x4E);
298
299 col0 = _mm256_fnmadd_pd(row3,tmp1,col0);
300 col3 = _mm256_fmsub_pd(row0,tmp1,col3);
301 col3 = _mm256_permute4x64_pd(col3,0x4E);
302 // -----------------------------------------------
303 tmp1 = _mm256_mul_pd(_mm256_permute4x64_pd(row1, 0x4E), row3);
304 tmp1 = _mm256_permute_pd(tmp1, 0x05);
305 row2 = _mm256_permute4x64_pd(row2, 0x4E);
306
307 col0 = _mm256_fmadd_pd(row2,tmp1,col0);
308 col2 = _mm256_mul_pd(row0, tmp1);
309
310 tmp1 = _mm256_permute4x64_pd(tmp1,0x4E);
311
312 col0 = _mm256_fnmadd_pd(row2,tmp1,col0);
313 col2 = _mm256_fmsub_pd(row0,tmp1,col2);
314 col2 = _mm256_permute4x64_pd(col2,0x4E);
315 // -----------------------------------------------
316 tmp1 = _mm256_mul_pd(row0, row1);
317 tmp1 = _mm256_permute_pd(tmp1, 0x05);
318
319 col2 = _mm256_fmadd_pd(row3,tmp1,col2);
320 col3 = _mm256_fmsub_pd(row2,tmp1,col3);
321
322 tmp1 = _mm256_permute4x64_pd(tmp1,0x4E);
323
324 col2 = _mm256_fmsub_pd(row3,tmp1,col2);
325 col3 = _mm256_fnmadd_pd(row2,tmp1,col3);
326 // -----------------------------------------------
327 tmp1 = _mm256_mul_pd(row0, row3);
328 tmp1 = _mm256_permute_pd(tmp1, 0x05);
329
330 col1 = _mm256_fnmadd_pd(row2,tmp1,col1);
331 col2 = _mm256_fmadd_pd(row1,tmp1,col2);
332
333 tmp1 = _mm256_permute4x64_pd(tmp1,0x4E);
334
335 col1 = _mm256_fmadd_pd(row2,tmp1,col1);
336 col2 = _mm256_fnmadd_pd(row1,tmp1,col2);
337 // -----------------------------------------------
338 tmp1 = _mm256_mul_pd(row0, row2);
339 tmp1 = _mm256_permute_pd(tmp1, 0x05);
340
341 col1 = _mm256_fmadd_pd(row3,tmp1,col1);
342 col3 = _mm256_fnmadd_pd(row1,tmp1,col3);
343
344 tmp1 = _mm256_permute4x64_pd(tmp1,0x4E);
345
346 col1 = _mm256_fnmadd_pd(row3,tmp1,col1);
347 col3 = _mm256_fmadd_pd(row1,tmp1,col3);
348// -----------------------------------------------
349 det = _mm256_mul_pd(row0, col0);
350 det = _mm256_add_pd(_mm256_permute4x64_pd(det, 0x4E), det);
351 det = _mm256_add_pd(_mm256_permute_pd(det, 0x05), det);
352 det = _mm256_permute4x64_pd(det,0x00);
353 det = _mm256_div_pd(_mm256_set1_pd(1.0),det);
354
355 _mm256_store_pd(invA+ 0,_mm256_mul_pd(det, col0));
356 _mm256_store_pd(invA+ 4,_mm256_mul_pd(det, col1));
357 _mm256_store_pd(invA+ 8,_mm256_mul_pd(det, col2));
358 _mm256_store_pd(invA+12,_mm256_mul_pd(det, col3));
359
360}
361
372void mat4_inv2(float *A)
373{
374 __m128 col0, col1, col2, col3;
375 __m128 row0, row1, row2, row3;
376 __m128 tmp1, det;
377
378 // load columns
379 col0 = _mm_load_ps(A+ 0);
380 col1 = _mm_load_ps(A+ 4);
381 col2 = _mm_load_ps(A+ 8);
382 col3 = _mm_load_ps(A+12);
383
384 tmp1 = _mm_unpacklo_ps(col0,col1);
385 row1 = _mm_unpacklo_ps(col2,col3);
386
387 row0 = _mm_shuffle_ps(tmp1, row1, 0x44);
388 row1 = _mm_shuffle_ps(row1, tmp1, 0xEE);
389
390 tmp1 = _mm_unpackhi_ps(col0,col1);
391 row3 = _mm_unpackhi_ps(col2,col3);
392
393 row2 = _mm_shuffle_ps(tmp1, row3, 0x44);
394 row3 = _mm_shuffle_ps(row3, tmp1, 0xEE);
395 // ---------------------------------------------
396 tmp1 = _mm_mul_ps(row2, row3);
397 tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
398
399 col0 = _mm_mul_ps(row1, tmp1);
400 col1 = _mm_mul_ps(row0, tmp1);
401
402 tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
403
404 col0 = _mm_sub_ps(_mm_mul_ps(row1, tmp1), col0);
405 col1 = _mm_sub_ps(_mm_mul_ps(row0, tmp1), col1);
406 col1 = _mm_shuffle_ps(col1, col1, 0x4E);
407 // -----------------------------------------------
408 tmp1 = _mm_mul_ps(row1, row2);
409 tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
410
411 col0 = _mm_add_ps(_mm_mul_ps(row3, tmp1), col0);
412 col3 = _mm_mul_ps(row0, tmp1);
413
414 tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
415
416 col0 = _mm_sub_ps(col0, _mm_mul_ps(row3, tmp1));
417 col3 = _mm_sub_ps(_mm_mul_ps(row0, tmp1), col3);
418 col3 = _mm_shuffle_ps(col3, col3, 0x4E);
419 // -----------------------------------------------
420 tmp1 = _mm_mul_ps(_mm_shuffle_ps(row1, row1, 0x4E), row3);
421 tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
422 row2 = _mm_shuffle_ps(row2, row2, 0x4E);
423
424 col0 = _mm_add_ps(_mm_mul_ps(row2, tmp1), col0);
425 col2 = _mm_mul_ps(row0, tmp1);
426
427 tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
428
429 col0 = _mm_sub_ps(col0, _mm_mul_ps(row2, tmp1));
430 col2 = _mm_sub_ps(_mm_mul_ps(row0, tmp1), col2);
431 col2 = _mm_shuffle_ps(col2, col2, 0x4E);
432 // -----------------------------------------------
433 tmp1 = _mm_mul_ps(row0, row1);
434 tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
435
436 col2 = _mm_add_ps(_mm_mul_ps(row3, tmp1), col2);
437 col3 = _mm_sub_ps(_mm_mul_ps(row2, tmp1), col3);
438
439 tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
440
441 col2 = _mm_sub_ps(_mm_mul_ps(row3, tmp1), col2);
442 col3 = _mm_sub_ps(col3, _mm_mul_ps(row2, tmp1));
443 // -----------------------------------------------
444 tmp1 = _mm_mul_ps(row0, row3);
445 tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
446
447 col1 = _mm_sub_ps(col1, _mm_mul_ps(row2, tmp1));
448 col2 = _mm_add_ps(_mm_mul_ps(row1, tmp1), col2);
449
450 tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
451
452 col1 = _mm_add_ps(_mm_mul_ps(row2, tmp1), col1);
453 col2 = _mm_sub_ps(col2, _mm_mul_ps(row1, tmp1));
454 // -----------------------------------------------
455 tmp1 = _mm_mul_ps(row0, row2);
456 tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
457
458 col1 = _mm_add_ps(_mm_mul_ps(row3, tmp1), col1);
459 col3 = _mm_sub_ps(col3, _mm_mul_ps(row1, tmp1));
460
461 tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
462
463 col1 = _mm_sub_ps(col1, _mm_mul_ps(row3, tmp1));
464 col3 = _mm_add_ps(_mm_mul_ps(row1, tmp1), col3);
465// -----------------------------------------------
466 det = _mm_mul_ps(row0, col0);
467 det = _mm_add_ps(_mm_shuffle_ps(det, det, 0x4E), det);
468 det = _mm_add_ss(_mm_shuffle_ps(det, det, 0xB1), det);
469 tmp1 = _mm_rcp_ss(det);
470
471 det = _mm_sub_ss(_mm_add_ss(tmp1, tmp1), _mm_mul_ss(det, _mm_mul_ss(tmp1, tmp1)));
472 det = _mm_shuffle_ps(det, det, 0x00);
473
474 _mm_store_ps(A+ 0,_mm_mul_ps(det, col0));
475 _mm_store_ps(A+ 4,_mm_mul_ps(det, col1));
476 _mm_store_ps(A+ 8,_mm_mul_ps(det, col2));
477 _mm_store_ps(A+12,_mm_mul_ps(det, col3));
478}
479
480#ifdef __cplusplus
481}
482#endif
mat4_inv
void mat4_inv(double *invA, const double *A)
Matrix inverse for 4x4 matrix.
Definition: matvec.h:222
mat4_lmul
void mat4_lmul(double const *A, double *B)
In-place left matrix-matrix multiplication for 4x4 matrices.
Definition: matvec.h:143
mat2_inv
void mat2_inv(double *invA, double const *A)
Matrix inverse for 2x2 matrix.
Definition: matvec.h:87
mat2_lmul
void mat2_lmul(double const *A, double *B)
In-place left matrix-matrix multiplication for 2x2 matrices.
Definition: matvec.h:54
mat4_vfms
void mat4_vfms(double *C, double const *A, double const *B)
Fused multiply-subtract for 4x4 matrices.
Definition: matvec.h:180
mat4_vinv
void mat4_vinv(double *invA, double const *A)
Matrix inverse for 4x4 matrix.
Definition: matvec.h:255
mat4_rmul
void mat4_rmul(double *A, double const *B)
In-place right matrix-matrix multiplication for 4x4 matrices.
Definition: matvec.h:107
mat2_vfms
void mat2_vfms(double *C, double const *A, double const *B)
Fused multiply-subtract for 2x2 matrices.
Definition: matvec.h:71
mat2_rmul
void mat2_rmul(double *A, double const *B)
In-place right matrix-matrix multiplication for 2x2 matrices.
Definition: matvec.h:38
mat4_inv2
void mat4_inv2(float *A)
Matrix inverse for 4x4 matrix.
Definition: matvec.h:372