diff --git a/StgSharp.Mathematics/Numeric/MatrixCompute.cs b/StgSharp.Mathematics/Numeric/MatrixCompute.cs index 132b110a..d378d0df 100644 --- a/StgSharp.Mathematics/Numeric/MatrixCompute.cs +++ b/StgSharp.Mathematics/Numeric/MatrixCompute.cs @@ -27,7 +27,6 @@ // ----------------------------------------------------------------------- using StgSharp.HighPerformance; using StgSharp.Internal; -using StgSharp.Mathematics.Linear; using System; using System.Collections.Generic; using System.Linq; diff --git a/StgSharp.Native/sn-src/load_intrinsic.x86.c b/StgSharp.Native/sn-src/load_intrinsic.x86.c index a1d70eb0..cc453dd0 100644 --- a/StgSharp.Native/sn-src/load_intrinsic.x86.c +++ b/StgSharp.Native/sn-src/load_intrinsic.x86.c @@ -54,7 +54,7 @@ void load_intrinsic_sse(sn_intrinsic *context, SIMDID id) context->mat[F32].clear_panel = PNL_PROC(float, sse, , clear); context->mat[F32].ker_fma = KER_PROC(float, sse, , fma); context->mat[F32].ker_tile_fma = KERTILE_PROC(float, sse, , fma); - context->mat[F32].pivot = pivot_float; + context->mat[F32].pivot = NULL; context->mat[F32].store_panel = NULL; context->mat[F64].buffer_add = BUF_PROC(double, sse, , add); @@ -93,7 +93,7 @@ void load_intrinsic_avx(sn_intrinsic *context, SIMDID id) KER_PROC(float, avx, _fma, fma); context->mat[F32].ker_tile_fma = is_fma ? KERTILE_PROC(float, avx, , fma) : KERTILE_PROC(float, avx, _fma, fma); - context->mat[F32].pivot = pivot_float; + context->mat[F32].pivot = NULL; context->mat[F32].store_panel = STORE_PANEL(avx, float); context->mat[F64].buffer_add = BUF_PROC(double, avx, , add); @@ -128,7 +128,7 @@ void load_intrinsic_512(sn_intrinsic *context, SIMDID id) context->mat[F32].clear_panel = PNL_PROC(float, 512, , clear); context->mat[F32].ker_fma = KER_PROC(float, 512, , fma); context->mat[F32].ker_tile_fma = KERTILE_PROC(float, 512, , fma); - context->mat[F32].pivot = pivot_float; + context->mat[F32].pivot = NULL; context->mat[F32].store_panel = STORE_PANEL(512, float); context->mat[F64].buffer_add = BUF_PROC(double, 512, , add); diff --git a/StgSharp.Native/sn-src/m.solve.x86.c b/StgSharp.Native/sn-src/m.solve.x86.c index 2c481ca0..e6ed05a5 100644 --- a/StgSharp.Native/sn-src/m.solve.x86.c +++ b/StgSharp.Native/sn-src/m.solve.x86.c @@ -3,6 +3,90 @@ #if SN_IS_ARCH(SN_ARCH_X86_64) #include "sn_intrinsic.h" +#include + +DECLARE_KER_PROC_LEFT_RIGHT_ANS_SCALAR(float, sse, , try_plu) +{ + /* Aliases */ + MAT_KERNEL(float) *upper = left; /* U */ + MAT_KERNEL(float) *lower = right; /* L (unit diag) */ + MAT_KERNEL(float) *source = ans; /* A */ + unsigned char *pivot = (unsigned char *)&scalar->data[0]; /* length = 4 */ + int *success = (int *)&scalar->data[1]; + + /* Init permutation and outputs */ + pivot[0] = 0; + pivot[1] = 1; + pivot[2] = 2; + pivot[3] = 3; + *success = 1; + + /* Working copy of A in column-major a[col][row], load via SIMD view */ + MAT_KERNEL(float) a; + const __m128 z = _mm_setzero_ps(); + for (int c = 0; c < 4; ++c) { + __m128 col = _mm_load_ps((float *)&source->f32_x[c]); + _mm_storeu_ps((float *)&a.f32_x[c], col); + _mm_store_ps((float *)&upper->f32_x[c], z); + _mm_store_ps((float *)&lower->f32_x[c], z); + } + + const float eps = 1e-12f; + + for (int k = 0; k < 4; ++k) { + /* Pivot search on column k, rows k..3 */ + int p = k; + float maxabs = fabsf(a.m[k][p]); + for (int r = k + 1; r < 4; ++r) { + float v = fabsf(a.m[k][r]); + if (v > maxabs) { + maxabs = v; + p = r; + } + } + + if (maxabs < eps) { + *success = 0; + /* leave outputs zeroed */ + return; + } + + /* Swap rows if needed */ + if (p != k) { + for (int c = 0; c < 4; ++c) { + float tmp = a.m[c][k]; + a.m[c][k] = a.m[c][p]; + a.m[c][p] = tmp; + } + unsigned char tmpi = pivot[k]; + pivot[k] = pivot[p]; + pivot[p] = tmpi; + } + + float pivot_val = a.m[k][k]; + + /* Eliminate below pivot: store multipliers in L column k */ + for (int r = k + 1; r < 4; ++r) { + float factor = a.m[k][r] / pivot_val; + lower->m[k][r] = factor; + for (int c = k; c < 4; ++c) { + a.m[c][r] -= factor * a.m[c][k]; + } + } + + /* Set U row k (columns k..3) and L diagonal */ + lower->m[k][k] = 1.0f; + for (int c = k; c < 4; ++c) { + upper->m[c][k] = a.m[c][k]; + } + } + + /* Ensure remaining diagonal ones for L */ + for (int d = 0; d < 4; ++d) { + if (lower->m[d][d] == 0.0f) + lower->m[d][d] = 1.0f; + } +} DECLARE_KER_PROC_RIGHT_ANS(float, sse, , quality) {