diff --git a/src/guf_linalg.h b/src/guf_linalg.h index b07b5f5..991fbd7 100644 --- a/src/guf_linalg.h +++ b/src/guf_linalg.h @@ -490,76 +490,71 @@ 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 EPS = 1e-6f; - const float EPS_ASSERT = 0.05f; + const float ASSERT_EPS = 1e-6f; for (int row_start = 0; row_start < 3; ++row_start) { // 1.) Try to bring into row echelon form. - // Find pivot, i.e. the element with row in range [row_start, 3] which has - // the the largest value in the current_column (current_column = row_start) - int first_nonzero_row = -1; - float largest = -INFINITY; - for (int row = row_start; row < 4; ++row) { + 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 const float candidate = fabsf(rref.data[row][row_start]); - if (!guf_nearly_zero_f32(candidate, EPS) && candidate > largest) { - largest = candidate; - first_nonzero_row = row; + if (candidate != 0 && candidate > max_pivot_abs) { // TODO: was "!guf_nearly_zero_f32(candidate, EPS)" + max_pivot_abs = candidate; + max_pivot_row = row; } } - if (first_nonzero_row == -1) { - GUF_ASSERT(largest == -INFINITY); - GUF_ASSERT(!guf_mat4x4_is_identity(&rref, EPS)); + if (max_pivot_row == -1) { // If all potential pivots were zero, m is not invertible. + GUF_ASSERT(max_pivot_abs == -INFINITY); + GUF_ASSERT(!guf_mat4x4_is_identity(&rref, ASSERT_EPS)); return false; } - GUF_ASSERT(largest > 0); + GUF_ASSERT(max_pivot_abs > 0); // Swap the found pivot-row with the row at row_start. - guf_mat4x4_swap_rows(&rref, row_start, first_nonzero_row); - guf_mat4x4_swap_rows(inverted, row_start, first_nonzero_row); + 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(!guf_nearly_zero_f32(pivot_val, EPS)); + GUF_ASSERT(pivot_val != 0); + GUF_ASSERT(!isnan(pivot_val)); for (int row = pivot_row + 1; row < 4; ++row) { // Make all values below the pivot element zero. - const float val = rref.data[row][pivot_col]; // val: the current value we want to turn into zero. - if (guf_nearly_zero_f32(val, EPS)) { - rref.data[row][pivot_col] = 0; - continue; - } + // 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 col = 0; col < 4; ++col) { - // pivot_val * scale = -val <=> scale = -val / pivot_val if (col >= pivot_col) { - rref.data[row][col] += rref.data[pivot_row][col] * -val / pivot_val; // Scale pivot_row and add to row + rref.data[row][col] -= rref.data[pivot_row][col] * scale; // Subtract the scaled pivot_row from row } else { GUF_ASSERT(rref.data[pivot_row][col] == 0); GUF_ASSERT(rref.data[row][col] == 0); } - inverted->data[row][col] += inverted->data[pivot_row][col] * -val / pivot_val; + inverted->data[row][col] -= inverted->data[pivot_row][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_col] = 0; // -= rref.data[pivot_row][pivot_col] * rref.data[row][pivot_col] / pivot_val } } for (int i = 3; i >= 0; --i) { // 2.) Try to bring into *reduced* row echelon form. - if (guf_nearly_zero_f32(rref.data[i][i], EPS)) { + if (rref.data[i][i] == 0) { // TODO: was "guf_nearly_zero_f32(rref.data[i][i], EPS)" return false; } - for (int col = 0; col < 4; ++col) { // Scale row_i to make elem[i][i] 1 - inverted->data[i][col] *= 1.f / rref.data[i][i]; + for (int col = 0; col < 4; ++col) { // Scale row_i to make elem[i][i] == 1 + inverted->data[i][col] /= rref.data[i][i]; GUF_ASSERT((col != i) ? rref.data[i][col] == 0 : rref.data[i][col] != 0); } - rref.data[i][i] = 1; // *= 1.f / rref.data[i][i] (== 1) + rref.data[i][i] = 1; // rref.data[i][i] / rref.data[i][i] == 1 for (int row = i - 1; row >= 0; --row) { // Make all elements in column above rref.data[i][i] zero (by adding a scaled row_i to row). for (int col = 0; col < 4; ++col) { - inverted->data[row][col] += inverted->data[i][col] * -rref.data[row][i]; + inverted->data[row][col] -= inverted->data[i][col] * rref.data[row][i]; } - rref.data[row][i] = 0; // += rref.data[i][i] * -rref.data[row][i] (== 0) + rref.data[row][i] = 0; // -= rref.data[i][i] * rref.data[row][i] (== 0) } } - GUF_ASSERT(guf_mat4x4_is_identity(&rref, EPS)); - (void)EPS_ASSERT; + GUF_ASSERT(guf_mat4x4_is_identity(&rref, ASSERT_EPS)); + (void)ASSERT_EPS; return true; } @@ -820,12 +815,12 @@ GUF_LINALG_KWRDS void guf_mat4x4_print_with_precision(const guf_mat4x4 *m, FILE } } } - const int whole_digits = max_digits; + const int whole_digits = max_digits + 1; num_frac_digits = GUF_CLAMP(num_frac_digits, 0, 32); for (int row = 0; row < 4; ++row) { for (int col = 0; col < 4; ++col) { - fprintf(outfile, "% *.*f ", whole_digits, num_frac_digits, (double)m->data[row][col]); + fprintf(outfile, "% *.*f ", whole_digits + num_frac_digits + 2, num_frac_digits, (double)m->data[row][col]); } fputc('\n', outfile); } diff --git a/src/test/example.c b/src/test/example.c index f81ef31..8fe9193 100644 --- a/src/test/example.c +++ b/src/test/example.c @@ -256,6 +256,8 @@ int main(void) guf_mat4x4_init_from_quaternion(&mat, q); guf_mat4x4_set_trans(&mat, (guf_vec3){42.1234f, -512.2f, 3.1415926f}); + mat.data[2][0] *= 100000.4f; + const guf_mat4x4 mat_cpy = mat; printf("Matrix:\n"); @@ -269,17 +271,16 @@ int main(void) printf("Inverse:\n"); guf_mat4x4_print_with_precision(&mat_inv, stdout, 8); GUF_ASSERT(guf_mat4x4_inverted(&mat_inv, &mat)); - GUF_ASSERT(guf_mat4x4_nearly_equal(&mat, &mat_cpy, 1e-3f, 1e-4f)); + GUF_ASSERT(guf_mat4x4_nearly_equal(&mat, &mat_cpy, 1e-4f, 1e-5f)); } - mat = (guf_mat4x4) {.data = { {1, 1.3f, 1, 1}, {2, 2.6f, 2, 2}, {0, 0, 2, 4}, {0, 0, 0, 1} }}; - printf("Matrix:\n"); + printf("Matrix 2:\n"); guf_mat4x4_print_with_precision(&mat, stdout, 8); invertible = guf_mat4x4_inverted(&mat, &mat_inv); if (!invertible) { diff --git a/todo.txt b/todo.txt index 6ee6102..a7b5500 100644 --- a/todo.txt +++ b/todo.txt @@ -2,6 +2,8 @@ - no guf_init.h +- linalg: float precision question += elem * -val / pivot_val vs elem * (-val / pivot_val) + - unicode normalisation - track allocs for test (implement alloc tracker) - handle right-to-left text properly