matrix functions

most matrix functions are converted over, no benefit to converting over the project/ortho because they would be scalar anyway but may need to move them regardless.

Engine/source/math/isa/avx/avx_intrinsics.h

@@ -1,5 +1,5 @@
 #pragma once
-#include <immintrin.h> // AVX/AVX2 intrinsics
+#include <immintrin.h>  // AVX
 
 namespace
 {
@@ -26,11 +26,58 @@ namespace
 
    inline f32x4 v_mask_xyz() { return _mm_blend_ps(_mm_set1_ps(0.0f), _mm_set1_ps(1.0f), 0b0111); }
 
+   inline f32x4 v_swizzle_mask(f32x4 v1, const int x, const int y, const int z, const int w)
+   {
+      __m128i idx = _mm_set_epi32(w, z, y, x);
+      return _mm_permutevar_ps(v1, idx);
+   }
+
+   inline f32x4 v_swizzle_singular_mask(f32x4 v1, const int x)
+   {
+      __m128i idx = _mm_set1_epi32(x);
+      return _mm_permutevar_ps(v1, idx);
+   }
+
+   inline f32x4 v_swizzle_lo(f32x4 v1)
+   {
+      return _mm_moveldup_ps(v1);
+   }
+
+   inline f32x4 v_swizzle_hi(f32x4 v1)
+   {
+      return _mm_movehdup_ps(v1);
+   }
+
    inline f32x4 v_preserve_w(f32x4 newv, f32x4 original)
    {
       return _mm_blend_ps(newv, original, 0b1000);
    }
 
+   //------------------------------------------------------
+   // Shuffle helpers
+   //------------------------------------------------------
+
+   inline f32x4 v_shuffle_mask(f32x4 v1, f32x4 v2, const int x, const int y, const int z, const int w)
+   {
+      f32x4 lo = v1;
+      f32x4 hi = v2;
+
+      f32x4 combined_lo = _mm_permutevar_ps(lo, _mm_set_epi32(y, y, x, x));
+      f32x4 combined_hi = _mm_permutevar_ps(hi, _mm_set_epi32(w, w, z, z));
+
+      return _mm_movelh_ps(combined_lo, combined_hi);
+   }
+
+   inline f32x4 v_shuffle_hilo(f32x4 v1, f32x4 v2)
+   {
+      return _mm_movelh_ps(v1, v2);
+   }
+
+   inline f32x4 v_shuffle_lohi(f32x4 v1, f32x4 v2)
+   {
+      return _mm_movehl_ps(v2, v1);
+   }
+
    //------------------------------------------------------
    // Float3 helpers (safe loading into 4 lanes)
    //------------------------------------------------------
@@ -54,6 +101,16 @@ namespace
       dst[2] = tmp[2];
    }
 
+   inline f32x4 v_set(float x, float y, float z, float w)
+   {
+      return _mm_set_ps(w, z, y, x);
+   }
+
+   inline f32x4 v_insert_w(f32x4 v, f32x4 w)
+   {
+      return _mm_insert_ps(v, w, 0x30);
+   }
+
    //------------------------------------------------------
    // Simple Arithmatic
    //------------------------------------------------------
@@ -70,7 +127,7 @@ namespace
    // Element-wise subtract
    inline f32x4 v_sub(f32x4 a, f32x4 b) { return _mm_sub_ps(a, b); }
 
-   //------------------------------------------------------
+   //------------------------------------------------------ 
    // Fast recip
    //------------------------------------------------------
 
@@ -137,4 +194,195 @@ namespace
       __m128 sum = _mm_hadd_ps(a, a);
       return _mm_hadd_ps(sum, sum);
    }
+
+   //------------------------------------------------------
+   // Matrix type (row-major 4x4)
+   //------------------------------------------------------
+
+   struct f32x4x4
+   {
+      f32x4 r0;
+      f32x4 r1;
+      f32x4 r2;
+      f32x4 r3;
+   };
+
+   //------------------------------------------------------
+   // Matrix Load / Store
+   //------------------------------------------------------
+
+   inline f32x4x4 m_load(const float* m) // expects 16 floats (row-major)
+   {
+      f32x4x4 out;
+      out.r0 = v_load(m + 0);
+      out.r1 = v_load(m + 4);
+      out.r2 = v_load(m + 8);
+      out.r3 = v_load(m + 12);
+      return out;
+   }
+
+   inline void m_store(float* dst, const f32x4x4& m)
+   {
+      v_store(dst + 0, m.r0);
+      v_store(dst + 4, m.r1);
+      v_store(dst + 8, m.r2);
+      v_store(dst + 12, m.r3);
+   }
+
+   inline f32x4x4 m_identity()
+   {
+      f32x4x4 m;
+      m.r0 = _mm_set_ps(0, 0, 0, 1);
+      m.r1 = _mm_set_ps(0, 0, 1, 0);
+      m.r2 = _mm_set_ps(0, 1, 0, 0);
+      m.r3 = _mm_set_ps(1, 0, 0, 0);
+      return m;
+   }
+
+   inline f32x4x4 m_zero()
+   {
+      f32x4 z = v_zero();
+      return { z, z, z, z };
+   }
+
+   //------------------------------------------------------
+   // Matrix × Vector
+   //------------------------------------------------------
+
+   inline f32x4 m_mul_vec4(const f32x4x4& m, f32x4 v)
+   {
+      f32x4 x = v_dot4(m.r0, v);
+      f32x4 y = v_dot4(m.r1, v);
+      f32x4 z = v_dot4(m.r2, v);
+      f32x4 w = v_dot4(m.r3, v);
+
+      // combine to a vector
+      return _mm_set_ps(_mm_cvtss_f32(w),
+         _mm_cvtss_f32(z),
+         _mm_cvtss_f32(y),
+         _mm_cvtss_f32(x));
+   }
+
+   inline f32x4 m_mul_vec3(const f32x4x4& m, f32x4 v)
+   {
+      f32x4 x = v_dot3(m.r0, v);
+      f32x4 y = v_dot3(m.r1, v);
+      f32x4 z = v_dot3(m.r2, v);
+
+      // combine to a vector
+      return _mm_set_ps(0.0f,
+         _mm_cvtss_f32(z),
+         _mm_cvtss_f32(y),
+         _mm_cvtss_f32(x));
+   }
+
+   //------------------------------------------------------
+   // Matrix × Matrix
+   //------------------------------------------------------
+
+   inline f32x4x4 m_mul(const f32x4x4& a, const f32x4x4& b)
+   {
+      // Transpose B once for dot products
+      f32x4 b0 = b.r0;
+      f32x4 b1 = b.r1;
+      f32x4 b2 = b.r2;
+      f32x4 b3 = b.r3;
+
+      // Transpose (SSE2)
+      _MM_TRANSPOSE4_PS(b0, b1, b2, b3);
+
+      f32x4x4 C;
+
+      auto mul_row = [&](f32x4 arow)
+      {
+         f32x4 x = v_dot4(arow, b0);
+         f32x4 y = v_dot4(arow, b1);
+         f32x4 z = v_dot4(arow, b2);
+         f32x4 w = v_dot4(arow, b3);
+
+         f32x4 xy = _mm_unpacklo_ps(x, y);
+         f32x4 zw = _mm_unpacklo_ps(z, w);
+         return _mm_movelh_ps(xy, zw);
+      };
+
+      C.r0 = mul_row(a.r0);
+      C.r1 = mul_row(a.r1);
+      C.r2 = mul_row(a.r2);
+      C.r3 = mul_row(a.r3);
+
+      return C;
+   }
+
+   //------------------------------------------------------
+   // Transpose
+   //------------------------------------------------------
+
+   inline f32x4x4 m_transpose(const f32x4x4& m)
+   {
+      f32x4 r0 = m.r0;
+      f32x4 r1 = m.r1;
+      f32x4 r2 = m.r2;
+      f32x4 r3 = m.r3;
+
+      _MM_TRANSPOSE4_PS(r0, r1, r2, r3);
+
+      return { r0, r1, r2, r3 };
+   }
+
+   inline f32x4x4 m_inverse_rigid(const f32x4x4& m)
+   {
+      f32x4x4 t = m_transpose(m);
+
+      // Extract translation
+      f32x4 T = v_set(
+         _mm_cvtss_f32(_mm_shuffle_ps(m.r0, m.r0, _MM_SHUFFLE(3, 3, 3, 3))),
+         _mm_cvtss_f32(_mm_shuffle_ps(m.r1, m.r1, _MM_SHUFFLE(3, 3, 3, 3))),
+         _mm_cvtss_f32(_mm_shuffle_ps(m.r2, m.r2, _MM_SHUFFLE(3, 3, 3, 3))),
+         0.0f
+      );
+
+      f32x4 newT = m_mul_vec4(t, T);
+      newT = _mm_sub_ps(v_zero(), newT);
+
+      t.r0 = v_preserve_w(t.r0, newT);
+      t.r1 = v_preserve_w(t.r1, newT);
+      t.r2 = v_preserve_w(t.r2, newT);
+      t.r3 = v_set(0, 0, 0, 1);
+
+      return t;
+   }
+
+   inline f32x4 m_determinant(const f32x4x4& m)
+   {
+      f32x4 a = m.r0;
+      f32x4 b = m.r1;
+      f32x4 c = m.r2;
+      f32x4 d = m.r3;
+
+      f32x4 c0 = v_cross(c, d);
+      f32x4 c1 = v_cross(d, b);
+      f32x4 c2 = v_cross(b, c);
+
+      f32x4 term0 = _mm_mul_ps(a, c0);
+      f32x4 term1 = _mm_mul_ps(a, c1);
+      f32x4 term2 = _mm_mul_ps(a, c2);
+
+      f32x4 det = _mm_add_ps(term0, _mm_add_ps(term1, term2));
+
+      return v_hadd4(det);
+   }
+
+   inline f32x4 m_determinant_affine(const f32x4x4& m)
+   {
+      f32x4 r0 = m.r0;
+      f32x4 r1 = m.r1;
+      f32x4 r2 = m.r2;
+
+      r0 = _mm_and_ps(r0, v_mask_xyz());
+      r1 = _mm_and_ps(r1, v_mask_xyz());
+      r2 = _mm_and_ps(r2, v_mask_xyz());
+
+      f32x4 c0 = v_cross(r1, r2);
+      return v_dot3(r0, c0); // splatted determinant
+   }
 }

Engine/source/math/isa/avx/mat44.cpp

@@ -0,0 +1,22 @@
+#include "avx_intrinsics.h"
+#include "mat44_dispatch.h"
+
+#include "mat44_impl.inl"
+
+namespace math_backend::mat44::dispatch
+{
+   void install_avx()
+   {
+      gMat44.transpose        = mat44_transpose_impl;
+      gMat44.inverse          = mat44_inverse_impl;
+      gMat44.affine_inverse   = mat44_affine_inverse_impl;
+      gMat44.mul_mat44        = mat44_mul_mat44_impl;
+      gMat44.mul_pos3         = mat44_mul_pos3_impl;
+      gMat44.mul_vec3         = mat44_mul_vec3_impl;
+      gMat44.mul_float4       = mat44_mul_float4_impl;
+      gMat44.scale            = mat44_scale_impl;
+      gMat44.get_scale        = mat44_get_scale_impl;
+      gMat44.normalize        = mat44_normalize_impl;
+      gMat44.determinant      = mat44_get_determinant;
+   }
+}