From c860a6ccfee0d7b734a325e033c02a2fed08b2a5 Mon Sep 17 00:00:00 2001 From: jun <83899451+zeichensystem@users.noreply.github.com> Date: Sat, 8 Mar 2025 11:25:57 +0100 Subject: [PATCH] Add 3x3 matrix inversion --- src/guf_linalg.h | 81 +++++++++++++++++++++++++++++++++++++----------- 1 file changed, 63 insertions(+), 18 deletions(-) diff --git a/src/guf_linalg.h b/src/guf_linalg.h index 991fbd7..c51ccdb 100644 --- a/src/guf_linalg.h +++ b/src/guf_linalg.h @@ -82,7 +82,8 @@ GUF_LINALG_KWRDS void guf_mat4x4_mul(const guf_mat4x4 *a, const guf_mat4x4 *b, guf_mat4x4 * restrict result); #endif - GUF_LINALG_KWRDS bool guf_mat4x4_inverted(const guf_mat4x4 *m, guf_mat4x4 *inverted); + GUF_LINALG_KWRDS bool guf_mat4x4_inverted(const guf_mat4x4 *m, guf_mat4x4 *inverted); // Using Gaussian elemination/row-reduction. + GUF_LINALG_KWRDS bool guf_mat3x3_inverted(const guf_mat3x3 *m, guf_mat3x3 *inverted); // Using a faster approach from wikipedia I don't get. #ifdef __cplusplus // C++ has no restrict keyword like C99... @@ -490,14 +491,15 @@ GUF_LINALG_KWRDS bool guf_mat4x4_inverted(const guf_mat4x4 *m, guf_mat4x4 *inver guf_mat4x4_init_identity(inverted); // This matrix starts as the identity matrix and will be transfomed into the inverse. guf_mat4x4 rref = *m; // This initial matrix will be transformed into the identity matrix if m is invertible. - const float ASSERT_EPS = 1e-6f; + const float ASSERT_EPS = 1e-3f; + const float EPS = 1e-18f; for (int row_start = 0; row_start < 3; ++row_start) { // 1.) Try to bring into row echelon form. int max_pivot_row = -1; float max_pivot_abs = -INFINITY; - for (int row = row_start; row < 4; ++row) { // Find the largest pivot element in the current column + for (int row = row_start; row < 4; ++row) { // Find max pivot element in the current column ("Partial pivoting") const float candidate = fabsf(rref.data[row][row_start]); - if (candidate != 0 && candidate > max_pivot_abs) { // TODO: was "!guf_nearly_zero_f32(candidate, EPS)" + if (!guf_nearly_zero_f32(candidate, EPS) && candidate > max_pivot_abs) { max_pivot_abs = candidate; max_pivot_row = row; } @@ -512,31 +514,30 @@ GUF_LINALG_KWRDS bool guf_mat4x4_inverted(const guf_mat4x4 *m, guf_mat4x4 *inver // Swap the found pivot-row with the row at row_start. guf_mat4x4_swap_rows(&rref, row_start, max_pivot_row); guf_mat4x4_swap_rows(inverted, row_start, max_pivot_row); - const int pivot_row = row_start; - const int pivot_col = row_start; - const float pivot_val = rref.data[pivot_row][pivot_col]; - GUF_ASSERT(pivot_val != 0); + const int pivot_idx = row_start; // Same for pivot_col and pivot_row (pivot is the diagonal element) + const float pivot_val = rref.data[pivot_idx][pivot_idx]; GUF_ASSERT(!isnan(pivot_val)); + GUF_ASSERT(!guf_nearly_zero_f32(pivot_val, EPS)); - for (int row = pivot_row + 1; row < 4; ++row) { // Make all values below the pivot element zero. - // We want to turn rref.data[row][pivot_col] into zero. - // pivot_val * scale = rref.data[row][pivot_col] <=> scale = rref.data[row][pivot_col] / pivot_val - const float scale = rref.data[row][pivot_col] / pivot_val; // TODO: Not sure if it's more accurate to not pre-calculate the scale (like in commit dac1d159b1cfa7d6cd71236d693b2cab6218ba51 (Fri Mar 7 15:37:55 2025 +0100)) + for (int row = pivot_idx + 1; row < 4; ++row) { // Make all values below the pivot element zero. + // We want to turn rref.data[row][pivot_idx] into zero. + // pivot_val * scale = rref.data[row][pivot_idx] <=> scale = rref.data[row][pivot_idx] / pivot_val + const float scale = rref.data[row][pivot_idx] / pivot_val; // TODO: Not sure if it's more accurate to not pre-calculate the scale (like in commit dac1d159b1cfa7d6cd71236d693b2cab6218ba51 (Fri Mar 7 15:37:55 2025 +0100)) for (int col = 0; col < 4; ++col) { - if (col >= pivot_col) { - rref.data[row][col] -= rref.data[pivot_row][col] * scale; // Subtract the scaled pivot_row from row + if (col >= pivot_idx) { + rref.data[row][col] -= rref.data[pivot_idx][col] * scale; // Subtract the scaled pivot-row from row } else { - GUF_ASSERT(rref.data[pivot_row][col] == 0); + GUF_ASSERT(rref.data[pivot_idx][col] == 0); GUF_ASSERT(rref.data[row][col] == 0); } - inverted->data[row][col] -= inverted->data[pivot_row][col] * scale; + inverted->data[row][col] -= inverted->data[pivot_idx][col] * scale; } - rref.data[row][pivot_col] = 0; // -= rref.data[pivot_row][pivot_col] * rref.data[row][pivot_col] / pivot_val + rref.data[row][pivot_idx] = 0; // -= rref.data[pivot_idx][pivot_idx] * rref.data[row][pivot_idx] / pivot_val } } for (int i = 3; i >= 0; --i) { // 2.) Try to bring into *reduced* row echelon form. - if (rref.data[i][i] == 0) { // TODO: was "guf_nearly_zero_f32(rref.data[i][i], EPS)" + if (rref.data[i][i] == 0) { return false; } for (int col = 0; col < 4; ++col) { // Scale row_i to make elem[i][i] == 1 @@ -558,6 +559,50 @@ GUF_LINALG_KWRDS bool guf_mat4x4_inverted(const guf_mat4x4 *m, guf_mat4x4 *inver return true; } +GUF_LINALG_KWRDS bool guf_mat3x3_inverted(const guf_mat3x3 *m, guf_mat3x3 *inverted) +{ + // TODO: Test this. + GUF_ASSERT(m && inverted); + const float EPS = 1e-12f; // TODO: Not sure about this EPS. + + // cf. https://en.wikipedia.org/wiki/Determinant (last-retrieved 2025-03-08) + const float det = m->data[0][0] * m->data[1][1] * m->data[2][2] + + m->data[0][1] * m->data[1][2] * m->data[2][0] + + m->data[0][2] * m->data[1][0] * m->data[2][1] - + m->data[0][1] * m->data[1][0] * m->data[2][2] - + m->data[0][0] * m->data[1][2] * m->data[2][1]; + + if (guf_nearly_zero_f32(det, EPS)) { // m is only invertible if the determinant != 0 + return false; + } + + // cf. https://en.wikipedia.org/wiki/Invertible_matrix#Inversion_of_3_%C3%97_3_matrices (last-retrieved 2025-03-08) + // (I don't understand this method of matrix inversion, but it looks fast...) + const float A = (m->data[1][1] * m->data[2][2] - m->data[1][2] * m->data[2][1]); + const float B = -(m->data[1][0] * m->data[2][2] - m->data[1][2] * m->data[2][0]); + const float C = (m->data[1][0] * m->data[2][1] - m->data[1][1] * m->data[2][0]); + const float D = -(m->data[0][1] * m->data[2][2] - m->data[0][2] * m->data[2][1]); + const float E = (m->data[0][0] * m->data[2][2] - m->data[0][2] * m->data[2][0]); + const float F = -(m->data[0][0] * m->data[2][1] - m->data[0][1] * m->data[2][0]); + const float G = (m->data[0][1] * m->data[1][2] - m->data[0][2] * m->data[1][1]); + const float H = -(m->data[0][0] * m->data[1][2] - m->data[0][2] * m->data[1][0]); + const float I = (m->data[0][0] * m->data[1][1] - m->data[0][1] * m->data[1][0]); + + inverted->data[0][0] = A / det; + inverted->data[1][0] = B / det; + inverted->data[2][0] = C / det; + + inverted->data[0][1] = D / det; + inverted->data[1][1] = E / det; + inverted->data[2][1] = F / det; + + inverted->data[0][2] = G / det; + inverted->data[1][2] = H / det; + inverted->data[2][2] = I / det; + + return true; +} + // Assumes the last row of mat is [0, 0, 0, 1] and v->w implicitly is 1 (affine transformation) GUF_LINALG_KWRDS guf_vec3 guf_vec3_transformed(const guf_mat4x4 *mat, const guf_vec3 *v) {