diff options
Diffstat (limited to 'source/blender/blenlib/intern/math_matrix.c')
| -rw-r--r-- | source/blender/blenlib/intern/math_matrix.c | 170 |
1 files changed, 112 insertions, 58 deletions
diff --git a/source/blender/blenlib/intern/math_matrix.c b/source/blender/blenlib/intern/math_matrix.c index 4a828e2c2e1..188ca5d5ee7 100644 --- a/source/blender/blenlib/intern/math_matrix.c +++ b/source/blender/blenlib/intern/math_matrix.c @@ -553,6 +553,51 @@ void sub_m4_m4m4(float m1[][4], float m2[][4], float m3[][4]) m1[i][j] = m2[i][j] - m3[i][j]; } +/* why not make this a standard part of the API? */ +static float determinant_m3_local(float m[3][3]) +{ + return (m[0][0] * (m[1][1] * m[2][2] - m[1][2] * m[2][1]) - + m[1][0] * (m[0][1] * m[2][2] - m[0][2] * m[2][1]) + + m[2][0] * (m[0][1] * m[1][2] - m[0][2] * m[1][1])); +} + +int invert_m3_ex(float m[3][3], const float epsilon) +{ + float tmp[3][3]; + int success; + + success = invert_m3_m3_ex(tmp, m, epsilon); + copy_m3_m3(m, tmp); + + return success; +} + +int invert_m3_m3_ex(float m1[3][3], float m2[3][3], const float epsilon) +{ + float det; + int a, b, success; + + BLI_assert(epsilon >= 0.0f); + + /* calc adjoint */ + adjoint_m3_m3(m1, m2); + + /* then determinant old matrix! */ + det = determinant_m3_local(m2); + + success = (fabsf(det) > epsilon); + + if (LIKELY(det != 0.0f)) { + det = 1.0f / det; + for (a = 0; a < 3; a++) { + for (b = 0; b < 3; b++) { + m1[a][b] *= det; + } + } + } + return success; +} + int invert_m3(float m[3][3]) { float tmp[3][3]; @@ -573,17 +618,16 @@ int invert_m3_m3(float m1[3][3], float m2[3][3]) adjoint_m3_m3(m1, m2); /* then determinant old matrix! */ - det = (m2[0][0] * (m2[1][1] * m2[2][2] - m2[1][2] * m2[2][1]) - - m2[1][0] * (m2[0][1] * m2[2][2] - m2[0][2] * m2[2][1]) + - m2[2][0] * (m2[0][1] * m2[1][2] - m2[0][2] * m2[1][1])); + det = determinant_m3_local(m2); - success = (det != 0); + success = (det != 0.0f); - if (det == 0) det = 1; - det = 1 / det; - for (a = 0; a < 3; a++) { - for (b = 0; b < 3; b++) { - m1[a][b] *= det; + if (LIKELY(det != 0.0f)) { + det = 1.0f / det; + for (a = 0; a < 3; a++) { + for (b = 0; b < 3; b++) { + m1[a][b] *= det; + } } } @@ -652,15 +696,15 @@ int invert_m4_m4(float inverse[4][4], float mat[4][4]) if (temp == 0) return 0; /* No non-zero pivot */ for (k = 0; k < 4; k++) { - tempmat[i][k] = (float)(tempmat[i][k] / temp); - inverse[i][k] = (float)(inverse[i][k] / temp); + tempmat[i][k] = (float)((double)tempmat[i][k] / temp); + inverse[i][k] = (float)((double)inverse[i][k] / temp); } for (j = 0; j < 4; j++) { if (j != i) { temp = tempmat[j][i]; for (k = 0; k < 4; k++) { - tempmat[j][k] -= (float)(tempmat[i][k] * temp); - inverse[j][k] -= (float)(inverse[i][k] * temp); + tempmat[j][k] -= (float)((double)tempmat[i][k] * temp); + inverse[j][k] -= (float)((double)inverse[i][k] * temp); } } } @@ -994,8 +1038,18 @@ void normalize_m4_m4(float rmat[][4], float mat[][4]) if (len != 0.0f) rmat[2][3] = mat[2][3] / len; } +void adjoint_m2_m2(float m1[][2], float m[][2]) +{ + BLI_assert(m1 != m); + m1[0][0] = m[1][1]; + m1[0][1] = -m[1][0]; + m1[1][0] = -m[0][1]; + m1[1][1] = m[0][0]; +} + void adjoint_m3_m3(float m1[][3], float m[][3]) { + BLI_assert(m1 != m); m1[0][0] = m[1][1] * m[2][2] - m[1][2] * m[2][1]; m1[0][1] = -m[0][1] * m[2][2] + m[0][2] * m[2][1]; m1[0][2] = m[0][1] * m[1][2] - m[0][2] * m[1][1]; @@ -1512,7 +1566,7 @@ void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4]) int m = 4; int n = 4; int maxiter = 200; - int nu = minf(m, n); + int nu = min_ff(m, n); float *work = work1; float *e = work2; @@ -1520,22 +1574,22 @@ void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4]) int i = 0, j = 0, k = 0, p, pp, iter; - // Reduce A to bidiagonal form, storing the diagonal elements - // in s and the super-diagonal elements in e. + /* Reduce A to bidiagonal form, storing the diagonal elements + * in s and the super-diagonal elements in e. */ - int nct = minf(m - 1, n); - int nrt = maxf(0, minf(n - 2, m)); + int nct = min_ff(m - 1, n); + int nrt = max_ff(0, min_ff(n - 2, m)); copy_m4_m4(A, A_); zero_m4(U); zero_v4(s); - for (k = 0; k < maxf(nct, nrt); k++) { + for (k = 0; k < max_ff(nct, nrt); k++) { if (k < nct) { - // Compute the transformation for the k-th column and - // place the k-th diagonal in s[k]. - // Compute 2-norm of k-th column without under/overflow. + /* Compute the transformation for the k-th column and + * place the k-th diagonal in s[k]. + * Compute 2-norm of k-th column without under/overflow. */ s[k] = 0; for (i = k; i < m; i++) { s[k] = hypotf(s[k], A[i][k]); @@ -1556,7 +1610,7 @@ void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4]) for (j = k + 1; j < n; j++) { if ((k < nct) && (s[k] != 0.0f)) { - // Apply the transformation. + /* Apply the transformation. */ float t = 0; for (i = k; i < m; i++) { @@ -1568,24 +1622,24 @@ void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4]) } } - // Place the k-th row of A into e for the - // subsequent calculation of the row transformation. + /* Place the k-th row of A into e for the */ + /* subsequent calculation of the row transformation. */ e[j] = A[k][j]; } if (k < nct) { - // Place the transformation in U for subsequent back - // multiplication. + /* Place the transformation in U for subsequent back + * multiplication. */ for (i = k; i < m; i++) U[i][k] = A[i][k]; } if (k < nrt) { - // Compute the k-th row transformation and place the - // k-th super-diagonal in e[k]. - // Compute 2-norm without under/overflow. + /* Compute the k-th row transformation and place the + * k-th super-diagonal in e[k]. + * Compute 2-norm without under/overflow. */ e[k] = 0; for (i = k + 1; i < n; i++) { e[k] = hypotf(e[k], e[i]); @@ -1605,7 +1659,7 @@ void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4]) if ((k + 1 < m) & (e[k] != 0.0f)) { float invek1; - // Apply the transformation. + /* Apply the transformation. */ for (i = k + 1; i < m; i++) { work[i] = 0.0f; @@ -1624,17 +1678,17 @@ void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4]) } } - // Place the transformation in V for subsequent - // back multiplication. + /* Place the transformation in V for subsequent + * back multiplication. */ for (i = k + 1; i < n; i++) V[i][k] = e[i]; } } - // Set up the final bidiagonal matrix or order p. + /* Set up the final bidiagonal matrix or order p. */ - p = minf(n, m + 1); + p = min_ff(n, m + 1); if (nct < n) { s[nct] = A[nct][nct]; } @@ -1646,7 +1700,7 @@ void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4]) } e[p - 1] = 0.0f; - // If required, generate U. + /* If required, generate U. */ for (j = nct; j < nu; j++) { for (i = 0; i < m; i++) { @@ -1682,7 +1736,7 @@ void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4]) } } - // If required, generate V. + /* If required, generate V. */ for (k = n - 1; k >= 0; k--) { if ((k < nrt) & (e[k] != 0.0f)) { @@ -1703,7 +1757,7 @@ void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4]) V[k][k] = 1.0f; } - // Main iteration loop for the singular values. + /* Main iteration loop for the singular values. */ pp = p - 1; iter = 0; @@ -1711,20 +1765,20 @@ void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4]) while (p > 0) { int kase = 0; - // Test for maximum iterations to avoid infinite loop + /* Test for maximum iterations to avoid infinite loop */ if (maxiter == 0) break; maxiter--; - // This section of the program inspects for - // negligible elements in the s and e arrays. On - // completion the variables kase and k are set as follows. - - // kase = 1 if s(p) and e[k - 1] are negligible and k<p - // kase = 2 if s(k) is negligible and k<p - // kase = 3 if e[k - 1] is negligible, k<p, and - // s(k), ..., s(p) are not negligible (qr step). - // kase = 4 if e(p - 1) is negligible (convergence). + /* This section of the program inspects for + * negligible elements in the s and e arrays. On + * completion the variables kase and k are set as follows. + * + * kase = 1 if s(p) and e[k - 1] are negligible and k<p + * kase = 2 if s(k) is negligible and k<p + * kase = 3 if e[k - 1] is negligible, k<p, and + * s(k), ..., s(p) are not negligible (qr step). + * kase = 4 if e(p - 1) is negligible (convergence). */ for (k = p - 2; k >= -1; k--) { if (k == -1) { @@ -1765,11 +1819,11 @@ void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4]) } k++; - // Perform the task indicated by kase. + /* Perform the task indicated by kase. */ switch (kase) { - // Deflate negligible s(p). + /* Deflate negligible s(p). */ case 1: { @@ -1795,7 +1849,7 @@ void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4]) break; } - // Split at negligible s(k). + /* Split at negligible s(k). */ case 2: { @@ -1819,14 +1873,14 @@ void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4]) break; } - // Perform one qr step. + /* Perform one qr step. */ case 3: { - // Calculate the shift. + /* Calculate the shift. */ - float scale = maxf(maxf(maxf(maxf( + float scale = max_ff(max_ff(max_ff(max_ff( fabsf(s[p - 1]), fabsf(s[p - 2])), fabsf(e[p - 2])), fabsf(s[k])), fabsf(e[k])); float invscale = 1.0f / scale; @@ -1849,7 +1903,7 @@ void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4]) f = (sk + sp) * (sk - sp) + shift; g = sk * ek; - // Chase zeros. + /* Chase zeros. */ for (j = k; j < p - 1; j++) { float t = hypotf(f, g); @@ -1891,12 +1945,12 @@ void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4]) iter = iter + 1; break; } - // Convergence. + /* Convergence. */ case 4: { - // Make the singular values positive. + /* Make the singular values positive. */ if (s[k] <= 0.0f) { s[k] = (s[k] < 0.0f ? -s[k] : 0.0f); @@ -1905,7 +1959,7 @@ void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4]) V[i][k] = -V[i][k]; } - // Order the singular values. + /* Order the singular values. */ while (k < pp) { float t; |