mirror of
https://github.com/TorqueGameEngines/Torque3D.git
synced 2026-07-12 15:14:35 +00:00
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.
This commit is contained in:
marauder2k7
parent
67f12311d4
commit
bc3145bc55
21 changed files with 1881 additions and 136 deletions
|
|
@ -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
|
||||
}
|
||||
}
|
||||
|
|
|
|||
22
Engine/source/math/isa/avx/mat44.cpp
Normal file
22
Engine/source/math/isa/avx/mat44.cpp
Normal file
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
|
@ -1,5 +1,5 @@
|
|||
#pragma once
|
||||
#include <immintrin.h> // AVX/AVX2 intrinsics
|
||||
#include <immintrin.h> // AVX2
|
||||
|
||||
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, int x, int y, int z, 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
|
||||
}
|
||||
}
|
||||
|
|
|
|||
22
Engine/source/math/isa/avx2/mat44.cpp
Normal file
22
Engine/source/math/isa/avx2/mat44.cpp
Normal file
|
|
@ -0,0 +1,22 @@
|
|||
#include "avx2_intrinsics.h"
|
||||
#include "mat44_dispatch.h"
|
||||
|
||||
#include "mat44_impl.inl"
|
||||
|
||||
namespace math_backend::mat44::dispatch
|
||||
{
|
||||
void install_avx2()
|
||||
{
|
||||
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;
|
||||
}
|
||||
}
|
||||
22
Engine/source/math/isa/sse2/mat44.cpp
Normal file
22
Engine/source/math/isa/sse2/mat44.cpp
Normal file
|
|
@ -0,0 +1,22 @@
|
|||
#include "sse2_intrinsics.h"
|
||||
#include "mat44_dispatch.h"
|
||||
|
||||
#include "mat44_impl.inl"
|
||||
|
||||
namespace math_backend::mat44::dispatch
|
||||
{
|
||||
void install_sse2()
|
||||
{
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
|
@ -7,7 +7,7 @@ namespace
|
|||
typedef __m128 f32x4;
|
||||
|
||||
//------------------------------------------------------
|
||||
// Load / Store
|
||||
// Vector/Point Load / Store
|
||||
//------------------------------------------------------
|
||||
|
||||
// Load 4 floats from memory into a SIMD register
|
||||
|
|
@ -35,6 +35,17 @@ namespace
|
|||
return _mm_set_ps(1.0f, p[2], p[1], p[0]);
|
||||
}
|
||||
|
||||
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)
|
||||
{
|
||||
f32x4 mask = _mm_castsi128_ps(_mm_set_epi32(-1, 0, 0, 0));
|
||||
return _mm_or_ps( _mm_and_ps(mask, w), _mm_andnot_ps(mask, v));
|
||||
}
|
||||
|
||||
inline void v_store3(float* dst, f32x4 v)
|
||||
{
|
||||
alignas(16) float tmp[4]; // temp storage
|
||||
|
|
@ -50,12 +61,70 @@ namespace
|
|||
|
||||
inline f32x4 v_mask_xyz() { return _mm_castsi128_ps(_mm_set_epi32(0, -1, -1, -1)); }
|
||||
|
||||
inline f32x4 v_swizzle_mask(f32x4 v, int x, int y, int z, int w)
|
||||
{
|
||||
alignas(16) float tmp[4];
|
||||
_mm_store_ps(tmp, v);
|
||||
|
||||
return _mm_set_ps(
|
||||
tmp[w],
|
||||
tmp[z],
|
||||
tmp[y],
|
||||
tmp[x]
|
||||
);
|
||||
}
|
||||
|
||||
inline f32x4 v_swizzle_singular_mask(f32x4 v, int x)
|
||||
{
|
||||
alignas(16) float tmp[4];
|
||||
_mm_store_ps(tmp, v);
|
||||
return _mm_set1_ps(tmp[x]);
|
||||
}
|
||||
|
||||
inline f32x4 v_swizzle_lo(f32x4 v1)
|
||||
{
|
||||
return _mm_shuffle_ps(v1, v1, _MM_SHUFFLE(2,2,0,0));
|
||||
}
|
||||
|
||||
inline f32x4 v_swizzle_hi(f32x4 v1)
|
||||
{
|
||||
return _mm_shuffle_ps(v1, v1, _MM_SHUFFLE(3, 3, 1, 1));
|
||||
}
|
||||
|
||||
inline f32x4 v_preserve_w(f32x4 newv, f32x4 original)
|
||||
{
|
||||
f32x4 mask = _mm_castsi128_ps(_mm_set_epi32(-1, 0, 0, 0));
|
||||
f32x4 mask = _mm_castsi128_ps(_mm_set_epi32(-1, 0, 0, 0)); // lane3 = 1
|
||||
return _mm_or_ps(_mm_and_ps(mask, original), _mm_andnot_ps(mask, newv));
|
||||
}
|
||||
|
||||
//------------------------------------------------------
|
||||
// Shuffle helpers
|
||||
//------------------------------------------------------
|
||||
|
||||
inline f32x4 v_shuffle_mask(f32x4 v1, f32x4 v2, int x, int y, int z, int w)
|
||||
{
|
||||
alignas(16) float a[4], b[4];
|
||||
_mm_store_ps(a, v1);
|
||||
_mm_store_ps(b, v2);
|
||||
|
||||
return _mm_set_ps(
|
||||
b[w],
|
||||
b[z],
|
||||
a[y],
|
||||
a[x]
|
||||
);
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
|
||||
//------------------------------------------------------
|
||||
// Simple Arithmatic
|
||||
|
|
@ -73,6 +142,13 @@ namespace
|
|||
// Element-wise subtract
|
||||
inline f32x4 v_sub(f32x4 a, f32x4 b) { return _mm_sub_ps(a, b); }
|
||||
|
||||
// Absolute value.
|
||||
inline f32x4 v_abs(f32x4 v)
|
||||
{
|
||||
const __m128 mask = _mm_castsi128_ps(_mm_set1_epi32(0x7fffffff));
|
||||
return _mm_and_ps(v, mask);
|
||||
}
|
||||
|
||||
//------------------------------------------------------
|
||||
// Fast recip
|
||||
//------------------------------------------------------
|
||||
|
|
@ -153,4 +229,292 @@ namespace
|
|||
shuf = _mm_shuffle_ps(sums, sums, _MM_SHUFFLE(1, 0, 3, 2));
|
||||
return _mm_add_ps(sums, shuf);
|
||||
}
|
||||
|
||||
//------------------------------------------------------
|
||||
// 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);
|
||||
|
||||
// Pack lowest lane of each into a vector
|
||||
f32x4 xy = _mm_unpacklo_ps(x, y);
|
||||
f32x4 zw = _mm_unpacklo_ps(z, w);
|
||||
return _mm_movelh_ps(xy, zw);
|
||||
}
|
||||
|
||||
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 void m_set3x3_transpose(float* dst, f32x4 c0, f32x4 c1, f32x4 c2)
|
||||
{
|
||||
dst[0] = v_extract0(c0); // c0.x
|
||||
dst[1] = v_extract0(c1); // c1.x
|
||||
dst[2] = v_extract0(c2); // c2.x
|
||||
|
||||
dst[4] = _mm_cvtss_f32(_mm_shuffle_ps(c0, c0, _MM_SHUFFLE(3, 3, 3, 1))); // c0.y
|
||||
dst[5] = _mm_cvtss_f32(_mm_shuffle_ps(c1, c1, _MM_SHUFFLE(3, 3, 3, 1))); // c1.y
|
||||
dst[6] = _mm_cvtss_f32(_mm_shuffle_ps(c2, c2, _MM_SHUFFLE(3, 3, 3, 1))); // c2.y
|
||||
|
||||
dst[8] = _mm_cvtss_f32(_mm_shuffle_ps(c0, c0, _MM_SHUFFLE(3, 3, 3, 2))); // c0.z
|
||||
dst[9] = _mm_cvtss_f32(_mm_shuffle_ps(c1, c1, _MM_SHUFFLE(3, 3, 3, 2))); // c1.z
|
||||
dst[10] = _mm_cvtss_f32(_mm_shuffle_ps(c2, c2, _MM_SHUFFLE(3, 3, 3, 2))); // c2.z
|
||||
}
|
||||
|
||||
inline f32x4x4 m_inverse(const f32x4x4& m)
|
||||
{
|
||||
f32x4 a = m.r0;
|
||||
f32x4 b = m.r1;
|
||||
f32x4 c = m.r2;
|
||||
f32x4 d = m.r3;
|
||||
|
||||
f32x4 c0 = v_cross(b, c);
|
||||
f32x4 c1 = v_cross(c, d);
|
||||
f32x4 c2 = v_cross(d, a);
|
||||
f32x4 c3 = v_cross(a, b);
|
||||
|
||||
f32x4 det = v_dot4(a, c1);
|
||||
f32x4 invDet = v_rcp_nr(det);
|
||||
|
||||
f32x4x4 adj;
|
||||
adj.r0 = _mm_mul_ps(c1, invDet);
|
||||
adj.r1 = _mm_mul_ps(c2, invDet);
|
||||
adj.r2 = _mm_mul_ps(c3, invDet);
|
||||
adj.r3 = _mm_mul_ps(c0, invDet);
|
||||
|
||||
return m_transpose(adj);
|
||||
}
|
||||
|
||||
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 f32x4x4 m_inverse_affine(const f32x4x4& m)
|
||||
{
|
||||
// Extract upper 3x3 rows
|
||||
f32x4 r0 = m.r0;
|
||||
f32x4 r1 = m.r1;
|
||||
f32x4 r2 = m.r2;
|
||||
|
||||
// Zero translation
|
||||
r0 = _mm_and_ps(r0, v_mask_xyz());
|
||||
r1 = _mm_and_ps(r1, v_mask_xyz());
|
||||
r2 = _mm_and_ps(r2, v_mask_xyz());
|
||||
|
||||
// Compute cofactors via cross products
|
||||
f32x4 c0 = v_cross(r1, r2);
|
||||
f32x4 c1 = v_cross(r2, r0);
|
||||
f32x4 c2 = v_cross(r0, r1);
|
||||
|
||||
// Determinant
|
||||
f32x4 det = v_dot3(r0, c0);
|
||||
|
||||
// Reciprocal determinant
|
||||
f32x4 invDet = v_rcp_nr(det);
|
||||
|
||||
// Inverse 3x3 (transpose of cofactor matrix)
|
||||
f32x4 i0 = _mm_mul_ps(c0, invDet);
|
||||
f32x4 i1 = _mm_mul_ps(c1, invDet);
|
||||
f32x4 i2 = _mm_mul_ps(c2, invDet);
|
||||
|
||||
// Transpose 3x3
|
||||
f32x4 t0 = i0;
|
||||
f32x4 t1 = i1;
|
||||
f32x4 t2 = i2;
|
||||
f32x4 t3 = _mm_setzero_ps();
|
||||
|
||||
_MM_TRANSPOSE4_PS(t0, t1, t2, t3);
|
||||
|
||||
// Extract translation
|
||||
alignas(16) float tmp[4];
|
||||
tmp[0] = _mm_cvtss_f32(_mm_shuffle_ps(m.r0, m.r0, _MM_SHUFFLE(3, 3, 3, 3)));
|
||||
tmp[1] = _mm_cvtss_f32(_mm_shuffle_ps(m.r1, m.r1, _MM_SHUFFLE(3, 3, 3, 3)));
|
||||
tmp[2] = _mm_cvtss_f32(_mm_shuffle_ps(m.r2, m.r2, _MM_SHUFFLE(3, 3, 3, 3)));
|
||||
tmp[3] = 0.0f;
|
||||
|
||||
f32x4 T = v_load(tmp);
|
||||
|
||||
// New translation = -R' * T
|
||||
f32x4 newT = m_mul_vec4({ t0, t1, t2, t3 }, T);
|
||||
newT = _mm_sub_ps(v_zero(), newT);
|
||||
|
||||
// Assemble final matrix
|
||||
f32x4x4 out;
|
||||
out.r0 = v_preserve_w(t0, newT);
|
||||
out.r1 = v_preserve_w(t1, newT);
|
||||
out.r2 = v_preserve_w(t2, newT);
|
||||
out.r3 = _mm_set_ps(1, 0, 0, 0);
|
||||
|
||||
return out;
|
||||
}
|
||||
|
||||
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
|
||||
}
|
||||
}
|
||||
|
|
|
|||
22
Engine/source/math/isa/sse41/mat44.cpp
Normal file
22
Engine/source/math/isa/sse41/mat44.cpp
Normal file
|
|
@ -0,0 +1,22 @@
|
|||
#include "sse41_intrinsics.h"
|
||||
#include "mat44_dispatch.h"
|
||||
|
||||
#include "mat44_impl.inl"
|
||||
|
||||
namespace math_backend::mat44::dispatch
|
||||
{
|
||||
void install_sse41()
|
||||
{
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
|
@ -26,11 +26,70 @@ 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 v, int x, int y, int z, int w)
|
||||
{
|
||||
alignas(16) float tmp[4];
|
||||
_mm_store_ps(tmp, v);
|
||||
|
||||
return _mm_set_ps(
|
||||
tmp[w],
|
||||
tmp[z],
|
||||
tmp[y],
|
||||
tmp[x]
|
||||
);
|
||||
}
|
||||
|
||||
inline f32x4 v_swizzle_singular_mask(f32x4 v, int x)
|
||||
{
|
||||
alignas(16) float tmp[4];
|
||||
_mm_store_ps(tmp, v);
|
||||
return _mm_set1_ps(tmp[x]);
|
||||
}
|
||||
|
||||
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, int x, int y, int z, int w)
|
||||
{
|
||||
alignas(16) float a[4], b[4];
|
||||
_mm_store_ps(a, v1);
|
||||
_mm_store_ps(b, v2);
|
||||
|
||||
return _mm_set_ps(
|
||||
b[w],
|
||||
b[z],
|
||||
a[y],
|
||||
a[x]
|
||||
);
|
||||
}
|
||||
|
||||
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 +113,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 +139,7 @@ namespace
|
|||
// Element-wise subtract
|
||||
inline f32x4 v_sub(f32x4 a, f32x4 b) { return _mm_sub_ps(a, b); }
|
||||
|
||||
//------------------------------------------------------
|
||||
//------------------------------------------------------
|
||||
// Fast recip
|
||||
//------------------------------------------------------
|
||||
|
||||
|
|
@ -104,13 +173,13 @@ namespace
|
|||
// full dot4
|
||||
inline f32x4 v_dot4(f32x4 a, f32x4 b)
|
||||
{
|
||||
return _mm_dp_ps(a, b, 0xF1); // f32x4, 4 lanes into lane 1
|
||||
return _mm_dp_ps(a, b, 0xFF); // f32x4, 4 lanes into lane 1
|
||||
}
|
||||
|
||||
// dot3 (ignores w)
|
||||
inline f32x4 v_dot3(f32x4 a, f32x4 b)
|
||||
{
|
||||
return _mm_dp_ps(a, b, 0x71); // f32x4, 3 last lanes into lane 1
|
||||
return _mm_dp_ps(a, b, 0x7F); // f32x4, 3 last lanes into lane 1
|
||||
}
|
||||
|
||||
// cross product xyz only.
|
||||
|
|
@ -137,4 +206,293 @@ 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)
|
||||
{
|
||||
_mm_storeu_ps(dst + 0, m.r0);
|
||||
_mm_storeu_ps(dst + 4, m.r1);
|
||||
_mm_storeu_ps(dst + 8, m.r2);
|
||||
_mm_storeu_ps(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 void m_set3x3_transpose(float* dst, f32x4 c0, f32x4 c1, f32x4 c2)
|
||||
{
|
||||
dst[0] = v_extract0(c0); // c0.x
|
||||
dst[1] = v_extract0(c1); // c1.x
|
||||
dst[2] = v_extract0(c2); // c2.x
|
||||
|
||||
dst[4] = _mm_cvtss_f32(_mm_shuffle_ps(c0, c0, _MM_SHUFFLE(3, 3, 3, 1))); // c0.y
|
||||
dst[5] = _mm_cvtss_f32(_mm_shuffle_ps(c1, c1, _MM_SHUFFLE(3, 3, 3, 1))); // c1.y
|
||||
dst[6] = _mm_cvtss_f32(_mm_shuffle_ps(c2, c2, _MM_SHUFFLE(3, 3, 3, 1))); // c2.y
|
||||
|
||||
dst[8] = _mm_cvtss_f32(_mm_shuffle_ps(c0, c0, _MM_SHUFFLE(3, 3, 3, 2))); // c0.z
|
||||
dst[9] = _mm_cvtss_f32(_mm_shuffle_ps(c1, c1, _MM_SHUFFLE(3, 3, 3, 2))); // c1.z
|
||||
dst[10] = _mm_cvtss_f32(_mm_shuffle_ps(c2, c2, _MM_SHUFFLE(3, 3, 3, 2))); // c2.z
|
||||
}
|
||||
|
||||
inline f32x4x4 m_inverse(const f32x4x4& m)
|
||||
{
|
||||
f32x4 a = m.r0;
|
||||
f32x4 b = m.r1;
|
||||
f32x4 c = m.r2;
|
||||
f32x4 d = m.r3;
|
||||
|
||||
f32x4 c0 = v_cross(b, c);
|
||||
f32x4 c1 = v_cross(c, d);
|
||||
f32x4 c2 = v_cross(d, a);
|
||||
f32x4 c3 = v_cross(a, b);
|
||||
|
||||
f32x4 det = v_dot4(a, c1);
|
||||
f32x4 invDet = v_rcp_nr(det);
|
||||
|
||||
f32x4x4 adj;
|
||||
adj.r0 = _mm_mul_ps(c1, invDet);
|
||||
adj.r1 = _mm_mul_ps(c2, invDet);
|
||||
adj.r2 = _mm_mul_ps(c3, invDet);
|
||||
adj.r3 = _mm_mul_ps(c0, invDet);
|
||||
|
||||
return m_transpose(adj);
|
||||
}
|
||||
|
||||
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 f32x4x4 m_inverse_affine(const f32x4x4& m)
|
||||
{
|
||||
// Extract upper 3x3 rows
|
||||
f32x4 r0 = m.r0;
|
||||
f32x4 r1 = m.r1;
|
||||
f32x4 r2 = m.r2;
|
||||
|
||||
// Zero translation
|
||||
r0 = _mm_and_ps(r0, v_mask_xyz());
|
||||
r1 = _mm_and_ps(r1, v_mask_xyz());
|
||||
r2 = _mm_and_ps(r2, v_mask_xyz());
|
||||
|
||||
// Compute cofactors via cross products
|
||||
f32x4 c0 = v_cross(r1, r2);
|
||||
f32x4 c1 = v_cross(r2, r0);
|
||||
f32x4 c2 = v_cross(r0, r1);
|
||||
|
||||
// Determinant
|
||||
f32x4 det = v_dot3(r0, c0);
|
||||
|
||||
// Reciprocal determinant
|
||||
f32x4 invDet = v_rcp_nr(det);
|
||||
|
||||
// Inverse 3x3 (transpose of cofactor matrix)
|
||||
f32x4 i0 = _mm_mul_ps(c0, invDet);
|
||||
f32x4 i1 = _mm_mul_ps(c1, invDet);
|
||||
f32x4 i2 = _mm_mul_ps(c2, invDet);
|
||||
|
||||
// Transpose 3x3
|
||||
f32x4 t0 = i0;
|
||||
f32x4 t1 = i1;
|
||||
f32x4 t2 = i2;
|
||||
f32x4 t3 = _mm_setzero_ps();
|
||||
|
||||
_MM_TRANSPOSE4_PS(t0, t1, t2, t3);
|
||||
|
||||
// Extract translation
|
||||
alignas(16) float tmp[4];
|
||||
tmp[0] = _mm_cvtss_f32(_mm_shuffle_ps(m.r0, m.r0, _MM_SHUFFLE(3, 3, 3, 3)));
|
||||
tmp[1] = _mm_cvtss_f32(_mm_shuffle_ps(m.r1, m.r1, _MM_SHUFFLE(3, 3, 3, 3)));
|
||||
tmp[2] = _mm_cvtss_f32(_mm_shuffle_ps(m.r2, m.r2, _MM_SHUFFLE(3, 3, 3, 3)));
|
||||
tmp[3] = 0.0f;
|
||||
|
||||
f32x4 T = v_load(tmp);
|
||||
|
||||
// New translation = -R' * T
|
||||
f32x4 newT = m_mul_vec4({ t0, t1, t2, t3 }, T);
|
||||
newT = _mm_sub_ps(v_zero(), newT);
|
||||
|
||||
// Assemble final matrix
|
||||
f32x4x4 out;
|
||||
out.r0 = v_preserve_w(t0, newT);
|
||||
out.r1 = v_preserve_w(t1, newT);
|
||||
out.r2 = v_preserve_w(t2, newT);
|
||||
out.r3 = _mm_set_ps(1, 0, 0, 0);
|
||||
|
||||
return out;
|
||||
}
|
||||
|
||||
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
|
||||
}
|
||||
}
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue