Zgragselus

Matrix 4x4 inversion

Feb 21st, 2026
112
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 3.40 KB | None | 0 0
  1. #ifdef MAT4_USE_SSE
  2.         __m128 f1 = _mm_sub_ps(_mm_mul_ps(_mm_shuffle_ps(mat.m3, mat.m2, 0xAA),
  3.             _mm_shuffle_ps(_mm_shuffle_ps(mat.m4, mat.m3, 0xFF), _mm_shuffle_ps(mat.m4, mat.m3, 0xFF), 0x80)),
  4.             _mm_mul_ps(_mm_shuffle_ps(_mm_shuffle_ps(mat.m4, mat.m3, 0xAA), _mm_shuffle_ps(mat.m4, mat.m3, 0xAA), 0x80),
  5.                 _mm_shuffle_ps(mat.m3, mat.m2, 0xFF)));
  6.  
  7.         __m128 f2 = _mm_sub_ps(_mm_mul_ps(_mm_shuffle_ps(mat.m3, mat.m2, 0x55),
  8.             _mm_shuffle_ps(_mm_shuffle_ps(mat.m4, mat.m3, 0xFF), _mm_shuffle_ps(mat.m4, mat.m3, 0xFF), 0x80)),
  9.             _mm_mul_ps(_mm_shuffle_ps(_mm_shuffle_ps(mat.m4, mat.m3, 0x55), _mm_shuffle_ps(mat.m4, mat.m3, 0x55), 0x80),
  10.                 _mm_shuffle_ps(mat.m3, mat.m2, 0xFF)));
  11.  
  12.         __m128 f3 = _mm_sub_ps(_mm_mul_ps(_mm_shuffle_ps(mat.m3, mat.m2, 0x55),
  13.             _mm_shuffle_ps(_mm_shuffle_ps(mat.m4, mat.m3, 0xAA), _mm_shuffle_ps(mat.m4, mat.m3, 0xAA), 0x80)),
  14.             _mm_mul_ps(_mm_shuffle_ps(_mm_shuffle_ps(mat.m4, mat.m3, 0x55), _mm_shuffle_ps(mat.m4, mat.m3, 0x55), 0x80),
  15.                 _mm_shuffle_ps(mat.m3, mat.m2, 0xAA)));
  16.  
  17.         __m128 f4 = _mm_sub_ps(_mm_mul_ps(_mm_shuffle_ps(mat.m3, mat.m2, 0x00),
  18.             _mm_shuffle_ps(_mm_shuffle_ps(mat.m4, mat.m3, 0xFF), _mm_shuffle_ps(mat.m4, mat.m3, 0xFF), 0x80)),
  19.             _mm_mul_ps(_mm_shuffle_ps(_mm_shuffle_ps(mat.m4, mat.m3, 0x00), _mm_shuffle_ps(mat.m4, mat.m3, 0x00), 0x80),
  20.                 _mm_shuffle_ps(mat.m3, mat.m2, 0xFF)));
  21.  
  22.         __m128 f5 = _mm_sub_ps(_mm_mul_ps(_mm_shuffle_ps(mat.m3, mat.m2, 0x00),
  23.             _mm_shuffle_ps(_mm_shuffle_ps(mat.m4, mat.m3, 0xAA), _mm_shuffle_ps(mat.m4, mat.m3, 0xAA), 0x80)),
  24.             _mm_mul_ps(_mm_shuffle_ps(_mm_shuffle_ps(mat.m4, mat.m3, 0x00), _mm_shuffle_ps(mat.m4, mat.m3, 0x00), 0x80),
  25.                 _mm_shuffle_ps(mat.m3, mat.m2, 0xAA)));
  26.  
  27.         __m128 f6 = _mm_sub_ps(_mm_mul_ps(_mm_shuffle_ps(mat.m3, mat.m2, 0x00),
  28.             _mm_shuffle_ps(_mm_shuffle_ps(mat.m4, mat.m3, 0x55), _mm_shuffle_ps(mat.m4, mat.m3, 0x55), 0x80)),
  29.             _mm_mul_ps(_mm_shuffle_ps(_mm_shuffle_ps(mat.m4, mat.m3, 0x00), _mm_shuffle_ps(mat.m4, mat.m3, 0x00), 0x80),
  30.                 _mm_shuffle_ps(mat.m3, mat.m2, 0x55)));
  31.  
  32.         __m128 v1 = _mm_shuffle_ps(_mm_shuffle_ps(mat.m2, mat.m1, 0x00), _mm_shuffle_ps(mat.m2, mat.m1, 0x00), 0xA8);
  33.         __m128 v2 = _mm_shuffle_ps(_mm_shuffle_ps(mat.m2, mat.m1, 0x55), _mm_shuffle_ps(mat.m2, mat.m1, 0x55), 0xA8);
  34.         __m128 v3 = _mm_shuffle_ps(_mm_shuffle_ps(mat.m2, mat.m1, 0xAA), _mm_shuffle_ps(mat.m2, mat.m1, 0xAA), 0xA8);
  35.         __m128 v4 = _mm_shuffle_ps(_mm_shuffle_ps(mat.m2, mat.m1, 0xFF), _mm_shuffle_ps(mat.m2, mat.m1, 0xFF), 0xA8);
  36.         __m128 s1 = _mm_set_ps(-0.0f, 0.0f, -0.0f, 0.0f);
  37.         __m128 s2 = _mm_set_ps(0.0f, -0.0f, 0.0f, -0.0f);
  38.         __m128 i1 = _mm_xor_ps(s1, _mm_add_ps(_mm_sub_ps(_mm_mul_ps(v2, f1),
  39.             _mm_mul_ps(v3, f2)),
  40.             _mm_mul_ps(v4, f3)));
  41.         __m128 i2 = _mm_xor_ps(s2, _mm_add_ps(_mm_sub_ps(_mm_mul_ps(v1, f1),
  42.             _mm_mul_ps(v3, f4)),
  43.             _mm_mul_ps(v4, f5)));
  44.         __m128 i3 = _mm_xor_ps(s1, _mm_add_ps(_mm_sub_ps(_mm_mul_ps(v1, f2),
  45.             _mm_mul_ps(v2, f4)),
  46.             _mm_mul_ps(v4, f6)));
  47.         __m128 i4 = _mm_xor_ps(s2, _mm_add_ps(_mm_sub_ps(_mm_mul_ps(v1, f3),
  48.             _mm_mul_ps(v2, f5)),
  49.             _mm_mul_ps(v3, f6)));
  50.         __m128 d = _mm_mul_ps(mat.m1, _mm_movelh_ps(_mm_unpacklo_ps(i1, i2), _mm_unpacklo_ps(i3, i4)));
  51.         d = _mm_add_ps(d, _mm_shuffle_ps(d, d, 0x4E));
  52.         d = _mm_add_ps(d, _mm_shuffle_ps(d, d, 0x11));
  53.         d = _mm_div_ps(_mm_set1_ps(1.0f), d);
  54.         return mat4(float4(_mm_mul_ps(i1, d)),
  55.             float4(_mm_mul_ps(i2, d)),
  56.             float4(_mm_mul_ps(i3, d)),
  57.             float4(_mm_mul_ps(i4, d)));
  58. #else
Advertisement
Add Comment
Please, Sign In to add comment