From caeaedde5204b07be1b598beafb14ea0c2afd385 Mon Sep 17 00:00:00 2001 From: marauder2k7 Date: Wed, 4 Mar 2026 09:56:29 +0000 Subject: [PATCH] add inverse to fix tests by adding an explicit inverse to function copy 3rd row into dst it seems original was not touching this at all. --- Engine/source/math/impl/mat44_impl.inl | 46 +++++++++++++++++++++- Engine/source/math/impl/math_c.cpp | 35 ++++++++++++++++ Engine/source/math/isa/avx/mat44.cpp | 1 + Engine/source/math/isa/avx2/mat44.cpp | 1 + Engine/source/math/isa/neon/mat44.cpp | 1 + Engine/source/math/isa/sse2/mat44.cpp | 1 + Engine/source/math/isa/sse41/mat44.cpp | 1 + Engine/source/math/mMatrix.h | 3 +- Engine/source/math/public/mat44_dispatch.h | 1 + 9 files changed, 87 insertions(+), 3 deletions(-) diff --git a/Engine/source/math/impl/mat44_impl.inl b/Engine/source/math/impl/mat44_impl.inl index c27daeb45..41bb4bd0d 100644 --- a/Engine/source/math/impl/mat44_impl.inl +++ b/Engine/source/math/impl/mat44_impl.inl @@ -85,7 +85,7 @@ namespace math_backend::mat44 // Store inverse 3x3 (transpose of cofactor matrix) mTemp = m_transpose(mTemp); - mTemp.r3 = v_set(0, 0, 0, 1.0f); + mTemp.r3 = ma.r3; // ---- Translation ---- @@ -106,6 +106,50 @@ namespace math_backend::mat44 m[11] = v_extract0(v_swizzle_singular_mask(result, 2)); } + // Matrix Inverse + inline void mat44_inverse_to_impl(const float* m, float* dst) + { + f32x4x4 ma = m_load(m); + + // Compute cofactors using cross products + f32x4x4 mTemp; + mTemp.r0 = v_cross(ma.r1, ma.r2); + mTemp.r1 = v_cross(ma.r2, ma.r0); + mTemp.r2 = v_cross(ma.r0, ma.r1); + + // Determinant = dot(ma.r0, c0) + f32x4 det = v_dot3(ma.r0, mTemp.r0); + f32x4 invDet = v_rcp_nr(det); + + // Scale cofactors + mTemp.r0 = v_mul(mTemp.r0, invDet); + mTemp.r1 = v_mul(mTemp.r1, invDet); + mTemp.r2 = v_mul(mTemp.r2, invDet); + + // Store inverse 3x3 (transpose of cofactor matrix) + + mTemp = m_transpose(mTemp); + mTemp.r3 = ma.r3; + + // ---- Translation ---- + + // Load original translation + f32x4 T = v_set(dst[3], dst[7], dst[11], 0.0f); + + // Compute -(Tx*ma.r0 + Ty*ma.r1 + Tz*ma.r2) + f32x4 result = v_mul(ma.r0, v_swizzle_singular_mask(T, 0)); + result = v_add(result, v_mul(ma.r1, v_swizzle_singular_mask(T, 1))); + result = v_add(result, v_mul(ma.r2, v_swizzle_singular_mask(T, 2))); + result = v_mul(result, v_set1(-1.0f)); + + m_store(dst, mTemp); + + // Store translation + dst[3] = v_extract0(result); + dst[7] = v_extract0(v_swizzle_singular_mask(result, 1)); + dst[11] = v_extract0(v_swizzle_singular_mask(result, 2)); + } + // Matrix Affine Inverse inline void mat44_affine_inverse_impl(float* m) { diff --git a/Engine/source/math/impl/math_c.cpp b/Engine/source/math/impl/math_c.cpp index 01640ca1f..ac6358b60 100644 --- a/Engine/source/math/impl/math_c.cpp +++ b/Engine/source/math/impl/math_c.cpp @@ -263,6 +263,41 @@ namespace math_backend::mat44::dispatch }; + gMat44.inverse_to = [](const float* m, float* d) { + // using Cramers Rule find the Inverse + // Minv = (1/det(M)) * adjoint(M) + F32 det = gMat44.determinant(m); + AssertFatal(det != 0.0f, "MatrixF::inverse: non-singular matrix, no inverse."); + + F32 invDet = 1.0f / det; + + d[0] = (m[5] * m[10] - m[6] * m[9]) * invDet; + d[1] = (m[9] * m[2] - m[10] * m[1]) * invDet; + d[2] = (m[1] * m[6] - m[2] * m[5]) * invDet; + + d[4] = (m[6] * m[8] - m[4] * m[10]) * invDet; + d[5] = (m[10] * m[0] - m[8] * m[2]) * invDet; + d[6] = (m[2] * m[4] - m[0] * m[6]) * invDet; + + d[8] = (m[4] * m[9] - m[5] * m[8]) * invDet; + d[9] = (m[8] * m[1] - m[9] * m[0]) * invDet; + d[10] = (m[0] * m[5] - m[1] * m[4]) * invDet; + + // invert the translation + F32 temp[6]; + temp[0] = -m[3]; + temp[1] = -m[7]; + temp[2] = -m[11]; + gMat44.mul_vec3(d, temp, &temp[3]); + d[3] = temp[3]; + d[7] = temp[4]; + d[11] = temp[5]; + d[12] = m[12]; + d[13] = m[13]; + d[14] = m[14]; + d[15] = m[15]; + }; + gMat44.affine_inverse = [](float* a) { F32 temp[16]; dMemcpy(temp, a, 16 * sizeof(F32)); diff --git a/Engine/source/math/isa/avx/mat44.cpp b/Engine/source/math/isa/avx/mat44.cpp index 6634b652c..50d64b6a2 100644 --- a/Engine/source/math/isa/avx/mat44.cpp +++ b/Engine/source/math/isa/avx/mat44.cpp @@ -9,6 +9,7 @@ namespace math_backend::mat44::dispatch { gMat44.transpose = mat44_transpose_impl; gMat44.inverse = mat44_inverse_impl; + gMat44.inverse_to = mat44_inverse_to_impl; gMat44.affine_inverse = mat44_affine_inverse_impl; gMat44.mul_mat44 = mat44_mul_mat44_impl; gMat44.mul_pos3 = mat44_mul_pos3_impl; diff --git a/Engine/source/math/isa/avx2/mat44.cpp b/Engine/source/math/isa/avx2/mat44.cpp index 11e842174..e6b891e9e 100644 --- a/Engine/source/math/isa/avx2/mat44.cpp +++ b/Engine/source/math/isa/avx2/mat44.cpp @@ -9,6 +9,7 @@ namespace math_backend::mat44::dispatch { gMat44.transpose = mat44_transpose_impl; gMat44.inverse = mat44_inverse_impl; + gMat44.inverse_to = mat44_inverse_to_impl; gMat44.affine_inverse = mat44_affine_inverse_impl; gMat44.mul_mat44 = mat44_mul_mat44_impl; gMat44.mul_pos3 = mat44_mul_pos3_impl; diff --git a/Engine/source/math/isa/neon/mat44.cpp b/Engine/source/math/isa/neon/mat44.cpp index 579cfe097..cfeebb3bf 100644 --- a/Engine/source/math/isa/neon/mat44.cpp +++ b/Engine/source/math/isa/neon/mat44.cpp @@ -9,6 +9,7 @@ namespace math_backend::mat44::dispatch { gMat44.transpose = mat44_transpose_impl; gMat44.inverse = mat44_inverse_impl; + gMat44.inverse_to = mat44_inverse_to_impl; gMat44.affine_inverse = mat44_affine_inverse_impl; gMat44.mul_mat44 = mat44_mul_mat44_impl; gMat44.mul_pos3 = mat44_mul_pos3_impl; diff --git a/Engine/source/math/isa/sse2/mat44.cpp b/Engine/source/math/isa/sse2/mat44.cpp index 6738ee391..2a84c029f 100644 --- a/Engine/source/math/isa/sse2/mat44.cpp +++ b/Engine/source/math/isa/sse2/mat44.cpp @@ -9,6 +9,7 @@ namespace math_backend::mat44::dispatch { gMat44.transpose = mat44_transpose_impl; gMat44.inverse = mat44_inverse_impl; + gMat44.inverse_to = mat44_inverse_to_impl; gMat44.affine_inverse = mat44_affine_inverse_impl; gMat44.mul_mat44 = mat44_mul_mat44_impl; gMat44.mul_pos3 = mat44_mul_pos3_impl; diff --git a/Engine/source/math/isa/sse41/mat44.cpp b/Engine/source/math/isa/sse41/mat44.cpp index 94bc5e4b2..489d495b8 100644 --- a/Engine/source/math/isa/sse41/mat44.cpp +++ b/Engine/source/math/isa/sse41/mat44.cpp @@ -9,6 +9,7 @@ namespace math_backend::mat44::dispatch { gMat44.transpose = mat44_transpose_impl; gMat44.inverse = mat44_inverse_impl; + gMat44.inverse_to = mat44_inverse_to_impl; gMat44.affine_inverse = mat44_affine_inverse_impl; gMat44.mul_mat44 = mat44_mul_mat44_impl; gMat44.mul_pos3 = mat44_mul_pos3_impl; diff --git a/Engine/source/math/mMatrix.h b/Engine/source/math/mMatrix.h index a670ab3ba..92bea5fad 100644 --- a/Engine/source/math/mMatrix.h +++ b/Engine/source/math/mMatrix.h @@ -381,8 +381,7 @@ inline MatrixF& MatrixF::inverse() inline void MatrixF::invertTo( MatrixF *out ) { - out = this; - GetMat44().inverse(*out); + GetMat44().inverse_to(*this, *out); } inline MatrixF& MatrixF::affineInverse() diff --git a/Engine/source/math/public/mat44_dispatch.h b/Engine/source/math/public/mat44_dispatch.h index b1b78ef13..2c4342aee 100644 --- a/Engine/source/math/public/mat44_dispatch.h +++ b/Engine/source/math/public/mat44_dispatch.h @@ -9,6 +9,7 @@ namespace math_backend::mat44::dispatch { void (*transpose)(float*) = nullptr; void (*inverse)(float*) = nullptr; + void (*inverse_to)(const float*, float*) = nullptr; void (*affine_inverse)(float*) = nullptr; void (*normalize)(float*) = nullptr; void (*mul_mat44)(const float* a, const float* b, float* r) = nullptr;