fruit/libguf
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add 3x3 matrix inversion

Browse Source
This commit is contained in:
jun 2025-03-08 11:25:57 +01:00
parent ae104919e0
commit c860a6ccfe
1 changed files with 63 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

81
src/guf_linalg.h
View File

@ -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)
{