diff options
| author | Campbell Barton <ideasman42@gmail.com> | 2019-04-17 07:17:24 +0300 |
|---|---|---|
| committer | Campbell Barton <ideasman42@gmail.com> | 2019-04-17 07:21:24 +0300 |
| commit | e12c08e8d170b7ca40f204a5b0423c23a9fbc2c1 (patch) | |
| tree | 8cf3453d12edb177a218ef8009357518ec6cab6a /source/blender/blenlib/intern/math_matrix.c | |
| parent | b3dabc200a4b0399ec6b81f2ff2730d07b44fcaa (diff) | |
ClangFormat: apply to source, most of intern
Apply clang format as proposed in T53211.
For details on usage and instructions for migrating branches
without conflicts, see:
https://wiki.blender.org/wiki/Tools/ClangFormat
Diffstat (limited to 'source/blender/blenlib/intern/math_matrix.c')
| -rw-r--r-- | source/blender/blenlib/intern/math_matrix.c | 3566 |
1 files changed, 1799 insertions, 1767 deletions
diff --git a/source/blender/blenlib/intern/math_matrix.c b/source/blender/blenlib/intern/math_matrix.c index 3da69e92227..71ea1ce1bc9 100644 --- a/source/blender/blenlib/intern/math_matrix.c +++ b/source/blender/blenlib/intern/math_matrix.c @@ -23,7 +23,6 @@ * \ingroup bli */ - #include <assert.h> #include "BLI_math.h" @@ -35,1067 +34,1099 @@ void zero_m2(float m[2][2]) { - memset(m, 0, sizeof(float[2][2])); + memset(m, 0, sizeof(float[2][2])); } void zero_m3(float m[3][3]) { - memset(m, 0, sizeof(float[3][3])); + memset(m, 0, sizeof(float[3][3])); } void zero_m4(float m[4][4]) { - memset(m, 0, sizeof(float[4][4])); + memset(m, 0, sizeof(float[4][4])); } void unit_m2(float m[2][2]) { - m[0][0] = m[1][1] = 1.0f; - m[0][1] = 0.0f; - m[1][0] = 0.0f; + m[0][0] = m[1][1] = 1.0f; + m[0][1] = 0.0f; + m[1][0] = 0.0f; } void unit_m3(float m[3][3]) { - m[0][0] = m[1][1] = m[2][2] = 1.0f; - m[0][1] = m[0][2] = 0.0f; - m[1][0] = m[1][2] = 0.0f; - m[2][0] = m[2][1] = 0.0f; + m[0][0] = m[1][1] = m[2][2] = 1.0f; + m[0][1] = m[0][2] = 0.0f; + m[1][0] = m[1][2] = 0.0f; + m[2][0] = m[2][1] = 0.0f; } void unit_m4(float m[4][4]) { - m[0][0] = m[1][1] = m[2][2] = m[3][3] = 1.0f; - m[0][1] = m[0][2] = m[0][3] = 0.0f; - m[1][0] = m[1][2] = m[1][3] = 0.0f; - m[2][0] = m[2][1] = m[2][3] = 0.0f; - m[3][0] = m[3][1] = m[3][2] = 0.0f; + m[0][0] = m[1][1] = m[2][2] = m[3][3] = 1.0f; + m[0][1] = m[0][2] = m[0][3] = 0.0f; + m[1][0] = m[1][2] = m[1][3] = 0.0f; + m[2][0] = m[2][1] = m[2][3] = 0.0f; + m[3][0] = m[3][1] = m[3][2] = 0.0f; } void copy_m2_m2(float m1[2][2], const float m2[2][2]) { - memcpy(m1, m2, sizeof(float[2][2])); + memcpy(m1, m2, sizeof(float[2][2])); } void copy_m3_m3(float m1[3][3], const float m2[3][3]) { - /* destination comes first: */ - memcpy(m1, m2, sizeof(float[3][3])); + /* destination comes first: */ + memcpy(m1, m2, sizeof(float[3][3])); } void copy_m4_m4(float m1[4][4], const float m2[4][4]) { - memcpy(m1, m2, sizeof(float[4][4])); + memcpy(m1, m2, sizeof(float[4][4])); } void copy_m3_m4(float m1[3][3], const float m2[4][4]) { - m1[0][0] = m2[0][0]; - m1[0][1] = m2[0][1]; - m1[0][2] = m2[0][2]; + m1[0][0] = m2[0][0]; + m1[0][1] = m2[0][1]; + m1[0][2] = m2[0][2]; - m1[1][0] = m2[1][0]; - m1[1][1] = m2[1][1]; - m1[1][2] = m2[1][2]; + m1[1][0] = m2[1][0]; + m1[1][1] = m2[1][1]; + m1[1][2] = m2[1][2]; - m1[2][0] = m2[2][0]; - m1[2][1] = m2[2][1]; - m1[2][2] = m2[2][2]; + m1[2][0] = m2[2][0]; + m1[2][1] = m2[2][1]; + m1[2][2] = m2[2][2]; } void copy_m4_m3(float m1[4][4], const float m2[3][3]) /* no clear */ { - m1[0][0] = m2[0][0]; - m1[0][1] = m2[0][1]; - m1[0][2] = m2[0][2]; - - m1[1][0] = m2[1][0]; - m1[1][1] = m2[1][1]; - m1[1][2] = m2[1][2]; + m1[0][0] = m2[0][0]; + m1[0][1] = m2[0][1]; + m1[0][2] = m2[0][2]; - m1[2][0] = m2[2][0]; - m1[2][1] = m2[2][1]; - m1[2][2] = m2[2][2]; + m1[1][0] = m2[1][0]; + m1[1][1] = m2[1][1]; + m1[1][2] = m2[1][2]; - /* Reevan's Bugfix */ - m1[0][3] = 0.0f; - m1[1][3] = 0.0f; - m1[2][3] = 0.0f; + m1[2][0] = m2[2][0]; + m1[2][1] = m2[2][1]; + m1[2][2] = m2[2][2]; - m1[3][0] = 0.0f; - m1[3][1] = 0.0f; - m1[3][2] = 0.0f; - m1[3][3] = 1.0f; + /* Reevan's Bugfix */ + m1[0][3] = 0.0f; + m1[1][3] = 0.0f; + m1[2][3] = 0.0f; + m1[3][0] = 0.0f; + m1[3][1] = 0.0f; + m1[3][2] = 0.0f; + m1[3][3] = 1.0f; } void copy_m3_m3d(float R[3][3], const double A[3][3]) { - /* Keep it stupid simple for better data flow in CPU. */ - R[0][0] = (float)A[0][0]; - R[0][1] = (float)A[0][1]; - R[0][2] = (float)A[0][2]; + /* Keep it stupid simple for better data flow in CPU. */ + R[0][0] = (float)A[0][0]; + R[0][1] = (float)A[0][1]; + R[0][2] = (float)A[0][2]; - R[1][0] = (float)A[1][0]; - R[1][1] = (float)A[1][1]; - R[1][2] = (float)A[1][2]; + R[1][0] = (float)A[1][0]; + R[1][1] = (float)A[1][1]; + R[1][2] = (float)A[1][2]; - R[2][0] = (float)A[2][0]; - R[2][1] = (float)A[2][1]; - R[2][2] = (float)A[2][2]; + R[2][0] = (float)A[2][0]; + R[2][1] = (float)A[2][1]; + R[2][2] = (float)A[2][2]; } void swap_m3m3(float m1[3][3], float m2[3][3]) { - float t; - int i, j; + float t; + int i, j; - for (i = 0; i < 3; i++) { - for (j = 0; j < 3; j++) { - t = m1[i][j]; - m1[i][j] = m2[i][j]; - m2[i][j] = t; - } - } + for (i = 0; i < 3; i++) { + for (j = 0; j < 3; j++) { + t = m1[i][j]; + m1[i][j] = m2[i][j]; + m2[i][j] = t; + } + } } void swap_m4m4(float m1[4][4], float m2[4][4]) { - float t; - int i, j; + float t; + int i, j; - for (i = 0; i < 4; i++) { - for (j = 0; j < 4; j++) { - t = m1[i][j]; - m1[i][j] = m2[i][j]; - m2[i][j] = t; - } - } + for (i = 0; i < 4; i++) { + for (j = 0; j < 4; j++) { + t = m1[i][j]; + m1[i][j] = m2[i][j]; + m2[i][j] = t; + } + } } /******************************** Arithmetic *********************************/ void mul_m4_m4m4(float R[4][4], const float A[4][4], const float B[4][4]) { - if (A == R) { - mul_m4_m4_post(R, B); - } - else if (B == R) { - mul_m4_m4_pre(R, A); - } - else { - mul_m4_m4m4_uniq(R, A, B); - } + if (A == R) { + mul_m4_m4_post(R, B); + } + else if (B == R) { + mul_m4_m4_pre(R, A); + } + else { + mul_m4_m4m4_uniq(R, A, B); + } } void mul_m4_m4m4_uniq(float R[4][4], const float A[4][4], const float B[4][4]) { - BLI_assert(R != A && R != B); + BLI_assert(R != A && R != B); - /* matrix product: R[j][k] = A[j][i] . B[i][k] */ + /* matrix product: R[j][k] = A[j][i] . B[i][k] */ #ifdef __SSE2__ - __m128 A0 = _mm_loadu_ps(A[0]); - __m128 A1 = _mm_loadu_ps(A[1]); - __m128 A2 = _mm_loadu_ps(A[2]); - __m128 A3 = _mm_loadu_ps(A[3]); - - for (int i = 0; i < 4; i++) { - __m128 B0 = _mm_set1_ps(B[i][0]); - __m128 B1 = _mm_set1_ps(B[i][1]); - __m128 B2 = _mm_set1_ps(B[i][2]); - __m128 B3 = _mm_set1_ps(B[i][3]); - - __m128 sum = _mm_add_ps( - _mm_add_ps(_mm_mul_ps(B0, A0), _mm_mul_ps(B1, A1)), - _mm_add_ps(_mm_mul_ps(B2, A2), _mm_mul_ps(B3, A3))); - - _mm_storeu_ps(R[i], sum); - } + __m128 A0 = _mm_loadu_ps(A[0]); + __m128 A1 = _mm_loadu_ps(A[1]); + __m128 A2 = _mm_loadu_ps(A[2]); + __m128 A3 = _mm_loadu_ps(A[3]); + + for (int i = 0; i < 4; i++) { + __m128 B0 = _mm_set1_ps(B[i][0]); + __m128 B1 = _mm_set1_ps(B[i][1]); + __m128 B2 = _mm_set1_ps(B[i][2]); + __m128 B3 = _mm_set1_ps(B[i][3]); + + __m128 sum = _mm_add_ps(_mm_add_ps(_mm_mul_ps(B0, A0), _mm_mul_ps(B1, A1)), + _mm_add_ps(_mm_mul_ps(B2, A2), _mm_mul_ps(B3, A3))); + + _mm_storeu_ps(R[i], sum); + } #else - R[0][0] = B[0][0] * A[0][0] + B[0][1] * A[1][0] + B[0][2] * A[2][0] + B[0][3] * A[3][0]; - R[0][1] = B[0][0] * A[0][1] + B[0][1] * A[1][1] + B[0][2] * A[2][1] + B[0][3] * A[3][1]; - R[0][2] = B[0][0] * A[0][2] + B[0][1] * A[1][2] + B[0][2] * A[2][2] + B[0][3] * A[3][2]; - R[0][3] = B[0][0] * A[0][3] + B[0][1] * A[1][3] + B[0][2] * A[2][3] + B[0][3] * A[3][3]; - - R[1][0] = B[1][0] * A[0][0] + B[1][1] * A[1][0] + B[1][2] * A[2][0] + B[1][3] * A[3][0]; - R[1][1] = B[1][0] * A[0][1] + B[1][1] * A[1][1] + B[1][2] * A[2][1] + B[1][3] * A[3][1]; - R[1][2] = B[1][0] * A[0][2] + B[1][1] * A[1][2] + B[1][2] * A[2][2] + B[1][3] * A[3][2]; - R[1][3] = B[1][0] * A[0][3] + B[1][1] * A[1][3] + B[1][2] * A[2][3] + B[1][3] * A[3][3]; - - R[2][0] = B[2][0] * A[0][0] + B[2][1] * A[1][0] + B[2][2] * A[2][0] + B[2][3] * A[3][0]; - R[2][1] = B[2][0] * A[0][1] + B[2][1] * A[1][1] + B[2][2] * A[2][1] + B[2][3] * A[3][1]; - R[2][2] = B[2][0] * A[0][2] + B[2][1] * A[1][2] + B[2][2] * A[2][2] + B[2][3] * A[3][2]; - R[2][3] = B[2][0] * A[0][3] + B[2][1] * A[1][3] + B[2][2] * A[2][3] + B[2][3] * A[3][3]; - - R[3][0] = B[3][0] * A[0][0] + B[3][1] * A[1][0] + B[3][2] * A[2][0] + B[3][3] * A[3][0]; - R[3][1] = B[3][0] * A[0][1] + B[3][1] * A[1][1] + B[3][2] * A[2][1] + B[3][3] * A[3][1]; - R[3][2] = B[3][0] * A[0][2] + B[3][1] * A[1][2] + B[3][2] * A[2][2] + B[3][3] * A[3][2]; - R[3][3] = B[3][0] * A[0][3] + B[3][1] * A[1][3] + B[3][2] * A[2][3] + B[3][3] * A[3][3]; + R[0][0] = B[0][0] * A[0][0] + B[0][1] * A[1][0] + B[0][2] * A[2][0] + B[0][3] * A[3][0]; + R[0][1] = B[0][0] * A[0][1] + B[0][1] * A[1][1] + B[0][2] * A[2][1] + B[0][3] * A[3][1]; + R[0][2] = B[0][0] * A[0][2] + B[0][1] * A[1][2] + B[0][2] * A[2][2] + B[0][3] * A[3][2]; + R[0][3] = B[0][0] * A[0][3] + B[0][1] * A[1][3] + B[0][2] * A[2][3] + B[0][3] * A[3][3]; + + R[1][0] = B[1][0] * A[0][0] + B[1][1] * A[1][0] + B[1][2] * A[2][0] + B[1][3] * A[3][0]; + R[1][1] = B[1][0] * A[0][1] + B[1][1] * A[1][1] + B[1][2] * A[2][1] + B[1][3] * A[3][1]; + R[1][2] = B[1][0] * A[0][2] + B[1][1] * A[1][2] + B[1][2] * A[2][2] + B[1][3] * A[3][2]; + R[1][3] = B[1][0] * A[0][3] + B[1][1] * A[1][3] + B[1][2] * A[2][3] + B[1][3] * A[3][3]; + + R[2][0] = B[2][0] * A[0][0] + B[2][1] * A[1][0] + B[2][2] * A[2][0] + B[2][3] * A[3][0]; + R[2][1] = B[2][0] * A[0][1] + B[2][1] * A[1][1] + B[2][2] * A[2][1] + B[2][3] * A[3][1]; + R[2][2] = B[2][0] * A[0][2] + B[2][1] * A[1][2] + B[2][2] * A[2][2] + B[2][3] * A[3][2]; + R[2][3] = B[2][0] * A[0][3] + B[2][1] * A[1][3] + B[2][2] * A[2][3] + B[2][3] * A[3][3]; + + R[3][0] = B[3][0] * A[0][0] + B[3][1] * A[1][0] + B[3][2] * A[2][0] + B[3][3] * A[3][0]; + R[3][1] = B[3][0] * A[0][1] + B[3][1] * A[1][1] + B[3][2] * A[2][1] + B[3][3] * A[3][1]; + R[3][2] = B[3][0] * A[0][2] + B[3][1] * A[1][2] + B[3][2] * A[2][2] + B[3][3] * A[3][2]; + R[3][3] = B[3][0] * A[0][3] + B[3][1] * A[1][3] + B[3][2] * A[2][3] + B[3][3] * A[3][3]; #endif } void mul_m4_m4_pre(float R[4][4], const float A[4][4]) { - BLI_assert(A != R); - float B[4][4]; - copy_m4_m4(B, R); - mul_m4_m4m4_uniq(R, A, B); + BLI_assert(A != R); + float B[4][4]; + copy_m4_m4(B, R); + mul_m4_m4m4_uniq(R, A, B); } void mul_m4_m4_post(float R[4][4], const float B[4][4]) { - BLI_assert(B != R); - float A[4][4]; - copy_m4_m4(A, R); - mul_m4_m4m4_uniq(R, A, B); + BLI_assert(B != R); + float A[4][4]; + copy_m4_m4(A, R); + mul_m4_m4m4_uniq(R, A, B); } void mul_m3_m3m3(float R[3][3], const float A[3][3], const float B[3][3]) { - if (A == R) { - mul_m3_m3_post(R, B); - } - else if (B == R) { - mul_m3_m3_pre(R, A); - } - else { - mul_m3_m3m3_uniq(R, A, B); - } + if (A == R) { + mul_m3_m3_post(R, B); + } + else if (B == R) { + mul_m3_m3_pre(R, A); + } + else { + mul_m3_m3m3_uniq(R, A, B); + } } void mul_m3_m3_pre(float R[3][3], const float A[3][3]) { - BLI_assert(A != R); - float B[3][3]; - copy_m3_m3(B, R); - mul_m3_m3m3_uniq(R, A, B); + BLI_assert(A != R); + float B[3][3]; + copy_m3_m3(B, R); + mul_m3_m3m3_uniq(R, A, B); } void mul_m3_m3_post(float R[3][3], const float B[3][3]) { - BLI_assert(B != R); - float A[3][3]; - copy_m3_m3(A, R); - mul_m3_m3m3_uniq(R, A, B); + BLI_assert(B != R); + float A[3][3]; + copy_m3_m3(A, R); + mul_m3_m3m3_uniq(R, A, B); } void mul_m3_m3m3_uniq(float R[3][3], const float A[3][3], const float B[3][3]) { - BLI_assert(R != A && R != B); + BLI_assert(R != A && R != B); - R[0][0] = B[0][0] * A[0][0] + B[0][1] * A[1][0] + B[0][2] * A[2][0]; - R[0][1] = B[0][0] * A[0][1] + B[0][1] * A[1][1] + B[0][2] * A[2][1]; - R[0][2] = B[0][0] * A[0][2] + B[0][1] * A[1][2] + B[0][2] * A[2][2]; + R[0][0] = B[0][0] * A[0][0] + B[0][1] * A[1][0] + B[0][2] * A[2][0]; + R[0][1] = B[0][0] * A[0][1] + B[0][1] * A[1][1] + B[0][2] * A[2][1]; + R[0][2] = B[0][0] * A[0][2] + B[0][1] * A[1][2] + B[0][2] * A[2][2]; - R[1][0] = B[1][0] * A[0][0] + B[1][1] * A[1][0] + B[1][2] * A[2][0]; - R[1][1] = B[1][0] * A[0][1] + B[1][1] * A[1][1] + B[1][2] * A[2][1]; - R[1][2] = B[1][0] * A[0][2] + B[1][1] * A[1][2] + B[1][2] * A[2][2]; + R[1][0] = B[1][0] * A[0][0] + B[1][1] * A[1][0] + B[1][2] * A[2][0]; + R[1][1] = B[1][0] * A[0][1] + B[1][1] * A[1][1] + B[1][2] * A[2][1]; + R[1][2] = B[1][0] * A[0][2] + B[1][1] * A[1][2] + B[1][2] * A[2][2]; - R[2][0] = B[2][0] * A[0][0] + B[2][1] * A[1][0] + B[2][2] * A[2][0]; - R[2][1] = B[2][0] * A[0][1] + B[2][1] * A[1][1] + B[2][2] * A[2][1]; - R[2][2] = B[2][0] * A[0][2] + B[2][1] * A[1][2] + B[2][2] * A[2][2]; + R[2][0] = B[2][0] * A[0][0] + B[2][1] * A[1][0] + B[2][2] * A[2][0]; + R[2][1] = B[2][0] * A[0][1] + B[2][1] * A[1][1] + B[2][2] * A[2][1]; + R[2][2] = B[2][0] * A[0][2] + B[2][1] * A[1][2] + B[2][2] * A[2][2]; } void mul_m4_m4m3(float m1[4][4], const float m3_[4][4], const float m2_[3][3]) { - float m2[3][3], m3[4][4]; + float m2[3][3], m3[4][4]; - /* copy so it works when m1 is the same pointer as m2 or m3 */ - /* TODO: avoid copying when matrices are different */ - copy_m3_m3(m2, m2_); - copy_m4_m4(m3, m3_); + /* copy so it works when m1 is the same pointer as m2 or m3 */ + /* TODO: avoid copying when matrices are different */ + copy_m3_m3(m2, m2_); + copy_m4_m4(m3, m3_); - m1[0][0] = m2[0][0] * m3[0][0] + m2[0][1] * m3[1][0] + m2[0][2] * m3[2][0]; - m1[0][1] = m2[0][0] * m3[0][1] + m2[0][1] * m3[1][1] + m2[0][2] * m3[2][1]; - m1[0][2] = m2[0][0] * m3[0][2] + m2[0][1] * m3[1][2] + m2[0][2] * m3[2][2]; - m1[1][0] = m2[1][0] * m3[0][0] + m2[1][1] * m3[1][0] + m2[1][2] * m3[2][0]; - m1[1][1] = m2[1][0] * m3[0][1] + m2[1][1] * m3[1][1] + m2[1][2] * m3[2][1]; - m1[1][2] = m2[1][0] * m3[0][2] + m2[1][1] * m3[1][2] + m2[1][2] * m3[2][2]; - m1[2][0] = m2[2][0] * m3[0][0] + m2[2][1] * m3[1][0] + m2[2][2] * m3[2][0]; - m1[2][1] = m2[2][0] * m3[0][1] + m2[2][1] * m3[1][1] + m2[2][2] * m3[2][1]; - m1[2][2] = m2[2][0] * m3[0][2] + m2[2][1] * m3[1][2] + m2[2][2] * m3[2][2]; + m1[0][0] = m2[0][0] * m3[0][0] + m2[0][1] * m3[1][0] + m2[0][2] * m3[2][0]; + m1[0][1] = m2[0][0] * m3[0][1] + m2[0][1] * m3[1][1] + m2[0][2] * m3[2][1]; + m1[0][2] = m2[0][0] * m3[0][2] + m2[0][1] * m3[1][2] + m2[0][2] * m3[2][2]; + m1[1][0] = m2[1][0] * m3[0][0] + m2[1][1] * m3[1][0] + m2[1][2] * m3[2][0]; + m1[1][1] = m2[1][0] * m3[0][1] + m2[1][1] * m3[1][1] + m2[1][2] * m3[2][1]; + m1[1][2] = m2[1][0] * m3[0][2] + m2[1][1] * m3[1][2] + m2[1][2] * m3[2][2]; + m1[2][0] = m2[2][0] * m3[0][0] + m2[2][1] * m3[1][0] + m2[2][2] * m3[2][0]; + m1[2][1] = m2[2][0] * m3[0][1] + m2[2][1] * m3[1][1] + m2[2][2] * m3[2][1]; + m1[2][2] = m2[2][0] * m3[0][2] + m2[2][1] * m3[1][2] + m2[2][2] * m3[2][2]; } /* m1 = m2 * m3, ignore the elements on the 4th row/column of m2 */ void mul_m3_m3m4(float m1[3][3], const float m3_[3][3], const float m2_[4][4]) { - float m2[4][4], m3[3][3]; + float m2[4][4], m3[3][3]; - /* copy so it works when m1 is the same pointer as m2 or m3 */ - /* TODO: avoid copying when matrices are different */ - copy_m4_m4(m2, m2_); - copy_m3_m3(m3, m3_); + /* copy so it works when m1 is the same pointer as m2 or m3 */ + /* TODO: avoid copying when matrices are different */ + copy_m4_m4(m2, m2_); + copy_m3_m3(m3, m3_); - /* m1[i][j] = m2[i][k] * m3[k][j] */ - m1[0][0] = m2[0][0] * m3[0][0] + m2[0][1] * m3[1][0] + m2[0][2] * m3[2][0]; - m1[0][1] = m2[0][0] * m3[0][1] + m2[0][1] * m3[1][1] + m2[0][2] * m3[2][1]; - m1[0][2] = m2[0][0] * m3[0][2] + m2[0][1] * m3[1][2] + m2[0][2] * m3[2][2]; + /* m1[i][j] = m2[i][k] * m3[k][j] */ + m1[0][0] = m2[0][0] * m3[0][0] + m2[0][1] * m3[1][0] + m2[0][2] * m3[2][0]; + m1[0][1] = m2[0][0] * m3[0][1] + m2[0][1] * m3[1][1] + m2[0][2] * m3[2][1]; + m1[0][2] = m2[0][0] * m3[0][2] + m2[0][1] * m3[1][2] + m2[0][2] * m3[2][2]; - m1[1][0] = m2[1][0] * m3[0][0] + m2[1][1] * m3[1][0] + m2[1][2] * m3[2][0]; - m1[1][1] = m2[1][0] * m3[0][1] + m2[1][1] * m3[1][1] + m2[1][2] * m3[2][1]; - m1[1][2] = m2[1][0] * m3[0][2] + m2[1][1] * m3[1][2] + m2[1][2] * m3[2][2]; + m1[1][0] = m2[1][0] * m3[0][0] + m2[1][1] * m3[1][0] + m2[1][2] * m3[2][0]; + m1[1][1] = m2[1][0] * m3[0][1] + m2[1][1] * m3[1][1] + m2[1][2] * m3[2][1]; + m1[1][2] = m2[1][0] * m3[0][2] + m2[1][1] * m3[1][2] + m2[1][2] * m3[2][2]; - m1[2][0] = m2[2][0] * m3[0][0] + m2[2][1] * m3[1][0] + m2[2][2] * m3[2][0]; - m1[2][1] = m2[2][0] * m3[0][1] + m2[2][1] * m3[1][1] + m2[2][2] * m3[2][1]; - m1[2][2] = m2[2][0] * m3[0][2] + m2[2][1] * m3[1][2] + m2[2][2] * m3[2][2]; + m1[2][0] = m2[2][0] * m3[0][0] + m2[2][1] * m3[1][0] + m2[2][2] * m3[2][0]; + m1[2][1] = m2[2][0] * m3[0][1] + m2[2][1] * m3[1][1] + m2[2][2] * m3[2][1]; + m1[2][2] = m2[2][0] * m3[0][2] + m2[2][1] * m3[1][2] + m2[2][2] * m3[2][2]; } /* m1 = m2 * m3, ignore the elements on the 4th row/column of m3 */ void mul_m3_m4m3(float m1[3][3], const float m3_[4][4], const float m2_[3][3]) { - float m2[3][3], m3[4][4]; + float m2[3][3], m3[4][4]; - /* copy so it works when m1 is the same pointer as m2 or m3 */ - /* TODO: avoid copying when matrices are different */ - copy_m3_m3(m2, m2_); - copy_m4_m4(m3, m3_); + /* copy so it works when m1 is the same pointer as m2 or m3 */ + /* TODO: avoid copying when matrices are different */ + copy_m3_m3(m2, m2_); + copy_m4_m4(m3, m3_); - /* m1[i][j] = m2[i][k] * m3[k][j] */ - m1[0][0] = m2[0][0] * m3[0][0] + m2[0][1] * m3[1][0] + m2[0][2] * m3[2][0]; - m1[0][1] = m2[0][0] * m3[0][1] + m2[0][1] * m3[1][1] + m2[0][2] * m3[2][1]; - m1[0][2] = m2[0][0] * m3[0][2] + m2[0][1] * m3[1][2] + m2[0][2] * m3[2][2]; + /* m1[i][j] = m2[i][k] * m3[k][j] */ + m1[0][0] = m2[0][0] * m3[0][0] + m2[0][1] * m3[1][0] + m2[0][2] * m3[2][0]; + m1[0][1] = m2[0][0] * m3[0][1] + m2[0][1] * m3[1][1] + m2[0][2] * m3[2][1]; + m1[0][2] = m2[0][0] * m3[0][2] + m2[0][1] * m3[1][2] + m2[0][2] * m3[2][2]; - m1[1][0] = m2[1][0] * m3[0][0] + m2[1][1] * m3[1][0] + m2[1][2] * m3[2][0]; - m1[1][1] = m2[1][0] * m3[0][1] + m2[1][1] * m3[1][1] + m2[1][2] * m3[2][1]; - m1[1][2] = m2[1][0] * m3[0][2] + m2[1][1] * m3[1][2] + m2[1][2] * m3[2][2]; + m1[1][0] = m2[1][0] * m3[0][0] + m2[1][1] * m3[1][0] + m2[1][2] * m3[2][0]; + m1[1][1] = m2[1][0] * m3[0][1] + m2[1][1] * m3[1][1] + m2[1][2] * m3[2][1]; + m1[1][2] = m2[1][0] * m3[0][2] + m2[1][1] * m3[1][2] + m2[1][2] * m3[2][2]; - m1[2][0] = m2[2][0] * m3[0][0] + m2[2][1] * m3[1][0] + m2[2][2] * m3[2][0]; - m1[2][1] = m2[2][0] * m3[0][1] + m2[2][1] * m3[1][1] + m2[2][2] * m3[2][1]; - m1[2][2] = m2[2][0] * m3[0][2] + m2[2][1] * m3[1][2] + m2[2][2] * m3[2][2]; + m1[2][0] = m2[2][0] * m3[0][0] + m2[2][1] * m3[1][0] + m2[2][2] * m3[2][0]; + m1[2][1] = m2[2][0] * m3[0][1] + m2[2][1] * m3[1][1] + m2[2][2] * m3[2][1]; + m1[2][2] = m2[2][0] * m3[0][2] + m2[2][1] * m3[1][2] + m2[2][2] * m3[2][2]; } void mul_m4_m3m4(float m1[4][4], const float m3_[3][3], const float m2_[4][4]) { - float m2[4][4], m3[3][3]; + float m2[4][4], m3[3][3]; - /* copy so it works when m1 is the same pointer as m2 or m3 */ - /* TODO: avoid copying when matrices are different */ - copy_m4_m4(m2, m2_); - copy_m3_m3(m3, m3_); + /* copy so it works when m1 is the same pointer as m2 or m3 */ + /* TODO: avoid copying when matrices are different */ + copy_m4_m4(m2, m2_); + copy_m3_m3(m3, m3_); - m1[0][0] = m2[0][0] * m3[0][0] + m2[0][1] * m3[1][0] + m2[0][2] * m3[2][0]; - m1[0][1] = m2[0][0] * m3[0][1] + m2[0][1] * m3[1][1] + m2[0][2] * m3[2][1]; - m1[0][2] = m2[0][0] * m3[0][2] + m2[0][1] * m3[1][2] + m2[0][2] * m3[2][2]; - m1[1][0] = m2[1][0] * m3[0][0] + m2[1][1] * m3[1][0] + m2[1][2] * m3[2][0]; - m1[1][1] = m2[1][0] * m3[0][1] + m2[1][1] * m3[1][1] + m2[1][2] * m3[2][1]; - m1[1][2] = m2[1][0] * m3[0][2] + m2[1][1] * m3[1][2] + m2[1][2] * m3[2][2]; - m1[2][0] = m2[2][0] * m3[0][0] + m2[2][1] * m3[1][0] + m2[2][2] * m3[2][0]; - m1[2][1] = m2[2][0] * m3[0][1] + m2[2][1] * m3[1][1] + m2[2][2] * m3[2][1]; - m1[2][2] = m2[2][0] * m3[0][2] + m2[2][1] * m3[1][2] + m2[2][2] * m3[2][2]; + m1[0][0] = m2[0][0] * m3[0][0] + m2[0][1] * m3[1][0] + m2[0][2] * m3[2][0]; + m1[0][1] = m2[0][0] * m3[0][1] + m2[0][1] * m3[1][1] + m2[0][2] * m3[2][1]; + m1[0][2] = m2[0][0] * m3[0][2] + m2[0][1] * m3[1][2] + m2[0][2] * m3[2][2]; + m1[1][0] = m2[1][0] * m3[0][0] + m2[1][1] * m3[1][0] + m2[1][2] * m3[2][0]; + m1[1][1] = m2[1][0] * m3[0][1] + m2[1][1] * m3[1][1] + m2[1][2] * m3[2][1]; + m1[1][2] = m2[1][0] * m3[0][2] + m2[1][1] * m3[1][2] + m2[1][2] * m3[2][2]; + m1[2][0] = m2[2][0] * m3[0][0] + m2[2][1] * m3[1][0] + m2[2][2] * m3[2][0]; + m1[2][1] = m2[2][0] * m3[0][1] + m2[2][1] * m3[1][1] + m2[2][2] * m3[2][1]; + m1[2][2] = m2[2][0] * m3[0][2] + m2[2][1] * m3[1][2] + m2[2][2] * m3[2][2]; } void mul_m3_m4m4(float m1[3][3], const float m3[4][4], const float m2[4][4]) { - m1[0][0] = m2[0][0] * m3[0][0] + m2[0][1] * m3[1][0] + m2[0][2] * m3[2][0]; - m1[0][1] = m2[0][0] * m3[0][1] + m2[0][1] * m3[1][1] + m2[0][2] * m3[2][1]; - m1[0][2] = m2[0][0] * m3[0][2] + m2[0][1] * m3[1][2] + m2[0][2] * m3[2][2]; - m1[1][0] = m2[1][0] * m3[0][0] + m2[1][1] * m3[1][0] + m2[1][2] * m3[2][0]; - m1[1][1] = m2[1][0] * m3[0][1] + m2[1][1] * m3[1][1] + m2[1][2] * m3[2][1]; - m1[1][2] = m2[1][0] * m3[0][2] + m2[1][1] * m3[1][2] + m2[1][2] * m3[2][2]; - m1[2][0] = m2[2][0] * m3[0][0] + m2[2][1] * m3[1][0] + m2[2][2] * m3[2][0]; - m1[2][1] = m2[2][0] * m3[0][1] + m2[2][1] * m3[1][1] + m2[2][2] * m3[2][1]; - m1[2][2] = m2[2][0] * m3[0][2] + m2[2][1] * m3[1][2] + m2[2][2] * m3[2][2]; + m1[0][0] = m2[0][0] * m3[0][0] + m2[0][1] * m3[1][0] + m2[0][2] * m3[2][0]; + m1[0][1] = m2[0][0] * m3[0][1] + m2[0][1] * m3[1][1] + m2[0][2] * m3[2][1]; + m1[0][2] = m2[0][0] * m3[0][2] + m2[0][1] * m3[1][2] + m2[0][2] * m3[2][2]; + m1[1][0] = m2[1][0] * m3[0][0] + m2[1][1] * m3[1][0] + m2[1][2] * m3[2][0]; + m1[1][1] = m2[1][0] * m3[0][1] + m2[1][1] * m3[1][1] + m2[1][2] * m3[2][1]; + m1[1][2] = m2[1][0] * m3[0][2] + m2[1][1] * m3[1][2] + m2[1][2] * m3[2][2]; + m1[2][0] = m2[2][0] * m3[0][0] + m2[2][1] * m3[1][0] + m2[2][2] * m3[2][0]; + m1[2][1] = m2[2][0] * m3[0][1] + m2[2][1] * m3[1][1] + m2[2][2] * m3[2][1]; + m1[2][2] = m2[2][0] * m3[0][2] + m2[2][1] * m3[1][2] + m2[2][2] * m3[2][2]; } /** \name Macro helpers for: mul_m3_series * \{ */ -void _va_mul_m3_series_3( - float r[3][3], - const float m1[3][3], const float m2[3][3]) -{ - mul_m3_m3m3(r, m1, m2); -} -void _va_mul_m3_series_4( - float r[3][3], - const float m1[3][3], const float m2[3][3], const float m3[3][3]) -{ - mul_m3_m3m3(r, m1, m2); - mul_m3_m3m3(r, r, m3); -} -void _va_mul_m3_series_5( - float r[3][3], - const float m1[3][3], const float m2[3][3], const float m3[3][3], const float m4[3][3]) -{ - mul_m3_m3m3(r, m1, m2); - mul_m3_m3m3(r, r, m3); - mul_m3_m3m3(r, r, m4); -} -void _va_mul_m3_series_6( - float r[3][3], - const float m1[3][3], const float m2[3][3], const float m3[3][3], const float m4[3][3], - const float m5[3][3]) -{ - mul_m3_m3m3(r, m1, m2); - mul_m3_m3m3(r, r, m3); - mul_m3_m3m3(r, r, m4); - mul_m3_m3m3(r, r, m5); -} -void _va_mul_m3_series_7( - float r[3][3], - const float m1[3][3], const float m2[3][3], const float m3[3][3], const float m4[3][3], - const float m5[3][3], const float m6[3][3]) -{ - mul_m3_m3m3(r, m1, m2); - mul_m3_m3m3(r, r, m3); - mul_m3_m3m3(r, r, m4); - mul_m3_m3m3(r, r, m5); - mul_m3_m3m3(r, r, m6); -} -void _va_mul_m3_series_8( - float r[3][3], - const float m1[3][3], const float m2[3][3], const float m3[3][3], const float m4[3][3], - const float m5[3][3], const float m6[3][3], const float m7[3][3]) -{ - mul_m3_m3m3(r, m1, m2); - mul_m3_m3m3(r, r, m3); - mul_m3_m3m3(r, r, m4); - mul_m3_m3m3(r, r, m5); - mul_m3_m3m3(r, r, m6); - mul_m3_m3m3(r, r, m7); -} -void _va_mul_m3_series_9( - float r[3][3], - const float m1[3][3], const float m2[3][3], const float m3[3][3], const float m4[3][3], - const float m5[3][3], const float m6[3][3], const float m7[3][3], const float m8[3][3]) -{ - mul_m3_m3m3(r, m1, m2); - mul_m3_m3m3(r, r, m3); - mul_m3_m3m3(r, r, m4); - mul_m3_m3m3(r, r, m5); - mul_m3_m3m3(r, r, m6); - mul_m3_m3m3(r, r, m7); - mul_m3_m3m3(r, r, m8); +void _va_mul_m3_series_3(float r[3][3], const float m1[3][3], const float m2[3][3]) +{ + mul_m3_m3m3(r, m1, m2); +} +void _va_mul_m3_series_4(float r[3][3], + const float m1[3][3], + const float m2[3][3], + const float m3[3][3]) +{ + mul_m3_m3m3(r, m1, m2); + mul_m3_m3m3(r, r, m3); +} +void _va_mul_m3_series_5(float r[3][3], + const float m1[3][3], + const float m2[3][3], + const float m3[3][3], + const float m4[3][3]) +{ + mul_m3_m3m3(r, m1, m2); + mul_m3_m3m3(r, r, m3); + mul_m3_m3m3(r, r, m4); +} +void _va_mul_m3_series_6(float r[3][3], + const float m1[3][3], + const float m2[3][3], + const float m3[3][3], + const float m4[3][3], + const float m5[3][3]) +{ + mul_m3_m3m3(r, m1, m2); + mul_m3_m3m3(r, r, m3); + mul_m3_m3m3(r, r, m4); + mul_m3_m3m3(r, r, m5); +} +void _va_mul_m3_series_7(float r[3][3], + const float m1[3][3], + const float m2[3][3], + const float m3[3][3], + const float m4[3][3], + const float m5[3][3], + const float m6[3][3]) +{ + mul_m3_m3m3(r, m1, m2); + mul_m3_m3m3(r, r, m3); + mul_m3_m3m3(r, r, m4); + mul_m3_m3m3(r, r, m5); + mul_m3_m3m3(r, r, m6); +} +void _va_mul_m3_series_8(float r[3][3], + const float m1[3][3], + const float m2[3][3], + const float m3[3][3], + const float m4[3][3], + const float m5[3][3], + const float m6[3][3], + const float m7[3][3]) +{ + mul_m3_m3m3(r, m1, m2); + mul_m3_m3m3(r, r, m3); + mul_m3_m3m3(r, r, m4); + mul_m3_m3m3(r, r, m5); + mul_m3_m3m3(r, r, m6); + mul_m3_m3m3(r, r, m7); +} +void _va_mul_m3_series_9(float r[3][3], + const float m1[3][3], + const float m2[3][3], + const float m3[3][3], + const float m4[3][3], + const float m5[3][3], + const float m6[3][3], + const float m7[3][3], + const float m8[3][3]) +{ + mul_m3_m3m3(r, m1, m2); + mul_m3_m3m3(r, r, m3); + mul_m3_m3m3(r, r, m4); + mul_m3_m3m3(r, r, m5); + mul_m3_m3m3(r, r, m6); + mul_m3_m3m3(r, r, m7); + mul_m3_m3m3(r, r, m8); } /** \} */ /** \name Macro helpers for: mul_m4_series * \{ */ -void _va_mul_m4_series_3( - float r[4][4], - const float m1[4][4], const float m2[4][4]) -{ - mul_m4_m4m4(r, m1, m2); -} -void _va_mul_m4_series_4( - float r[4][4], - const float m1[4][4], const float m2[4][4], const float m3[4][4]) -{ - mul_m4_m4m4(r, m1, m2); - mul_m4_m4m4(r, r, m3); -} -void _va_mul_m4_series_5( - float r[4][4], - const float m1[4][4], const float m2[4][4], const float m3[4][4], const float m4[4][4]) -{ - mul_m4_m4m4(r, m1, m2); - mul_m4_m4m4(r, r, m3); - mul_m4_m4m4(r, r, m4); -} -void _va_mul_m4_series_6( - float r[4][4], - const float m1[4][4], const float m2[4][4], const float m3[4][4], const float m4[4][4], - const float m5[4][4]) -{ - mul_m4_m4m4(r, m1, m2); - mul_m4_m4m4(r, r, m3); - mul_m4_m4m4(r, r, m4); - mul_m4_m4m4(r, r, m5); -} -void _va_mul_m4_series_7( - float r[4][4], - const float m1[4][4], const float m2[4][4], const float m3[4][4], const float m4[4][4], - const float m5[4][4], const float m6[4][4]) -{ - mul_m4_m4m4(r, m1, m2); - mul_m4_m4m4(r, r, m3); - mul_m4_m4m4(r, r, m4); - mul_m4_m4m4(r, r, m5); - mul_m4_m4m4(r, r, m6); -} -void _va_mul_m4_series_8( - float r[4][4], - const float m1[4][4], const float m2[4][4], const float m3[4][4], const float m4[4][4], - const float m5[4][4], const float m6[4][4], const float m7[4][4]) -{ - mul_m4_m4m4(r, m1, m2); - mul_m4_m4m4(r, r, m3); - mul_m4_m4m4(r, r, m4); - mul_m4_m4m4(r, r, m5); - mul_m4_m4m4(r, r, m6); - mul_m4_m4m4(r, r, m7); -} -void _va_mul_m4_series_9( - float r[4][4], - const float m1[4][4], const float m2[4][4], const float m3[4][4], const float m4[4][4], - const float m5[4][4], const float m6[4][4], const float m7[4][4], const float m8[4][4]) -{ - mul_m4_m4m4(r, m1, m2); - mul_m4_m4m4(r, r, m3); - mul_m4_m4m4(r, r, m4); - mul_m4_m4m4(r, r, m5); - mul_m4_m4m4(r, r, m6); - mul_m4_m4m4(r, r, m7); - mul_m4_m4m4(r, r, m8); +void _va_mul_m4_series_3(float r[4][4], const float m1[4][4], const float m2[4][4]) +{ + mul_m4_m4m4(r, m1, m2); +} +void _va_mul_m4_series_4(float r[4][4], + const float m1[4][4], + const float m2[4][4], + const float m3[4][4]) +{ + mul_m4_m4m4(r, m1, m2); + mul_m4_m4m4(r, r, m3); +} +void _va_mul_m4_series_5(float r[4][4], + const float m1[4][4], + const float m2[4][4], + const float m3[4][4], + const float m4[4][4]) +{ + mul_m4_m4m4(r, m1, m2); + mul_m4_m4m4(r, r, m3); + mul_m4_m4m4(r, r, m4); +} +void _va_mul_m4_series_6(float r[4][4], + const float m1[4][4], + const float m2[4][4], + const float m3[4][4], + const float m4[4][4], + const float m5[4][4]) +{ + mul_m4_m4m4(r, m1, m2); + mul_m4_m4m4(r, r, m3); + mul_m4_m4m4(r, r, m4); + mul_m4_m4m4(r, r, m5); +} +void _va_mul_m4_series_7(float r[4][4], + const float m1[4][4], + const float m2[4][4], + const float m3[4][4], + const float m4[4][4], + const float m5[4][4], + const float m6[4][4]) +{ + mul_m4_m4m4(r, m1, m2); + mul_m4_m4m4(r, r, m3); + mul_m4_m4m4(r, r, m4); + mul_m4_m4m4(r, r, m5); + mul_m4_m4m4(r, r, m6); +} +void _va_mul_m4_series_8(float r[4][4], + const float m1[4][4], + const float m2[4][4], + const float m3[4][4], + const float m4[4][4], + const float m5[4][4], + const float m6[4][4], + const float m7[4][4]) +{ + mul_m4_m4m4(r, m1, m2); + mul_m4_m4m4(r, r, m3); + mul_m4_m4m4(r, r, m4); + mul_m4_m4m4(r, r, m5); + mul_m4_m4m4(r, r, m6); + mul_m4_m4m4(r, r, m7); +} +void _va_mul_m4_series_9(float r[4][4], + const float m1[4][4], + const float m2[4][4], + const float m3[4][4], + const float m4[4][4], + const float m5[4][4], + const float m6[4][4], + const float m7[4][4], + const float m8[4][4]) +{ + mul_m4_m4m4(r, m1, m2); + mul_m4_m4m4(r, r, m3); + mul_m4_m4m4(r, r, m4); + mul_m4_m4m4(r, r, m5); + mul_m4_m4m4(r, r, m6); + mul_m4_m4m4(r, r, m7); + mul_m4_m4m4(r, r, m8); } /** \} */ void mul_v2_m3v2(float r[2], const float m[3][3], const float v[2]) { - float temp[3], warped[3]; + float temp[3], warped[3]; - copy_v2_v2(temp, v); - temp[2] = 1.0f; + copy_v2_v2(temp, v); + temp[2] = 1.0f; - mul_v3_m3v3(warped, m, temp); + mul_v3_m3v3(warped, m, temp); - r[0] = warped[0] / warped[2]; - r[1] = warped[1] / warped[2]; + r[0] = warped[0] / warped[2]; + r[1] = warped[1] / warped[2]; } void mul_m3_v2(const float m[3][3], float r[2]) { - mul_v2_m3v2(r, m, r); + mul_v2_m3v2(r, m, r); } void mul_m4_v3(const float mat[4][4], float vec[3]) { - const float x = vec[0]; - const float y = vec[1]; + const float x = vec[0]; + const float y = vec[1]; - vec[0] = x * mat[0][0] + y * mat[1][0] + mat[2][0] * vec[2] + mat[3][0]; - vec[1] = x * mat[0][1] + y * mat[1][1] + mat[2][1] * vec[2] + mat[3][1]; - vec[2] = x * mat[0][2] + y * mat[1][2] + mat[2][2] * vec[2] + mat[3][2]; + vec[0] = x * mat[0][0] + y * mat[1][0] + mat[2][0] * vec[2] + mat[3][0]; + vec[1] = x * mat[0][1] + y * mat[1][1] + mat[2][1] * vec[2] + mat[3][1]; + vec[2] = x * mat[0][2] + y * mat[1][2] + mat[2][2] * vec[2] + mat[3][2]; } void mul_v3_m4v3(float r[3], const float mat[4][4], const float vec[3]) { - const float x = vec[0]; - const float y = vec[1]; + const float x = vec[0]; + const float y = vec[1]; - r[0] = x * mat[0][0] + y * mat[1][0] + mat[2][0] * vec[2] + mat[3][0]; - r[1] = x * mat[0][1] + y * mat[1][1] + mat[2][1] * vec[2] + mat[3][1]; - r[2] = x * mat[0][2] + y * mat[1][2] + mat[2][2] * vec[2] + mat[3][2]; + r[0] = x * mat[0][0] + y * mat[1][0] + mat[2][0] * vec[2] + mat[3][0]; + r[1] = x * mat[0][1] + y * mat[1][1] + mat[2][1] * vec[2] + mat[3][1]; + r[2] = x * mat[0][2] + y * mat[1][2] + mat[2][2] * vec[2] + mat[3][2]; } void mul_v2_m4v3(float r[2], const float mat[4][4], const float vec[3]) { - const float x = vec[0]; + const float x = vec[0]; - r[0] = x * mat[0][0] + vec[1] * mat[1][0] + mat[2][0] * vec[2] + mat[3][0]; - r[1] = x * mat[0][1] + vec[1] * mat[1][1] + mat[2][1] * vec[2] + mat[3][1]; + r[0] = x * mat[0][0] + vec[1] * mat[1][0] + mat[2][0] * vec[2] + mat[3][0]; + r[1] = x * mat[0][1] + vec[1] * mat[1][1] + mat[2][1] * vec[2] + mat[3][1]; } void mul_v2_m2v2(float r[2], const float mat[2][2], const float vec[2]) { - const float x = vec[0]; + const float x = vec[0]; - r[0] = mat[0][0] * x + mat[1][0] * vec[1]; - r[1] = mat[0][1] * x + mat[1][1] * vec[1]; + r[0] = mat[0][0] * x + mat[1][0] * vec[1]; + r[1] = mat[0][1] * x + mat[1][1] * vec[1]; } void mul_m2v2(const float mat[2][2], float vec[2]) { - mul_v2_m2v2(vec, mat, vec); + mul_v2_m2v2(vec, mat, vec); } /* same as mul_m4_v3() but doesnt apply translation component */ void mul_mat3_m4_v3(const float mat[4][4], float vec[3]) { - const float x = vec[0]; - const float y = vec[1]; + const float x = vec[0]; + const float y = vec[1]; - vec[0] = x * mat[0][0] + y * mat[1][0] + mat[2][0] * vec[2]; - vec[1] = x * mat[0][1] + y * mat[1][1] + mat[2][1] * vec[2]; - vec[2] = x * mat[0][2] + y * mat[1][2] + mat[2][2] * vec[2]; + vec[0] = x * mat[0][0] + y * mat[1][0] + mat[2][0] * vec[2]; + vec[1] = x * mat[0][1] + y * mat[1][1] + mat[2][1] * vec[2]; + vec[2] = x * mat[0][2] + y * mat[1][2] + mat[2][2] * vec[2]; } void mul_v3_mat3_m4v3(float r[3], const float mat[4][4], const float vec[3]) { - const float x = vec[0]; - const float y = vec[1]; + const float x = vec[0]; + const float y = vec[1]; - r[0] = x * mat[0][0] + y * mat[1][0] + mat[2][0] * vec[2]; - r[1] = x * mat[0][1] + y * mat[1][1] + mat[2][1] * vec[2]; - r[2] = x * mat[0][2] + y * mat[1][2] + mat[2][2] * vec[2]; + r[0] = x * mat[0][0] + y * mat[1][0] + mat[2][0] * vec[2]; + r[1] = x * mat[0][1] + y * mat[1][1] + mat[2][1] * vec[2]; + r[2] = x * mat[0][2] + y * mat[1][2] + mat[2][2] * vec[2]; } void mul_project_m4_v3(const float mat[4][4], float vec[3]) { - /* absolute value to not flip the frustum upside down behind the camera */ - const float w = fabsf(mul_project_m4_v3_zfac(mat, vec)); - mul_m4_v3(mat, vec); + /* absolute value to not flip the frustum upside down behind the camera */ + const float w = fabsf(mul_project_m4_v3_zfac(mat, vec)); + mul_m4_v3(mat, vec); - vec[0] /= w; - vec[1] /= w; - vec[2] /= w; + vec[0] /= w; + vec[1] /= w; + vec[2] /= w; } void mul_v3_project_m4_v3(float r[3], const float mat[4][4], const float vec[3]) { - const float w = fabsf(mul_project_m4_v3_zfac(mat, vec)); - mul_v3_m4v3(r, mat, vec); + const float w = fabsf(mul_project_m4_v3_zfac(mat, vec)); + mul_v3_m4v3(r, mat, vec); - r[0] /= w; - r[1] /= w; - r[2] /= w; + r[0] /= w; + r[1] /= w; + r[2] /= w; } void mul_v2_project_m4_v3(float r[2], const float mat[4][4], const float vec[3]) { - const float w = fabsf(mul_project_m4_v3_zfac(mat, vec)); - mul_v2_m4v3(r, mat, vec); + const float w = fabsf(mul_project_m4_v3_zfac(mat, vec)); + mul_v2_m4v3(r, mat, vec); - r[0] /= w; - r[1] /= w; + r[0] /= w; + r[1] /= w; } void mul_v4_m4v4(float r[4], const float mat[4][4], const float v[4]) { - const float x = v[0]; - const float y = v[1]; - const float z = v[2]; + const float x = v[0]; + const float y = v[1]; + const float z = v[2]; - r[0] = x * mat[0][0] + y * mat[1][0] + z * mat[2][0] + mat[3][0] * v[3]; - r[1] = x * mat[0][1] + y * mat[1][1] + z * mat[2][1] + mat[3][1] * v[3]; - r[2] = x * mat[0][2] + y * mat[1][2] + z * mat[2][2] + mat[3][2] * v[3]; - r[3] = x * mat[0][3] + y * mat[1][3] + z * mat[2][3] + mat[3][3] * v[3]; + r[0] = x * mat[0][0] + y * mat[1][0] + z * mat[2][0] + mat[3][0] * v[3]; + r[1] = x * mat[0][1] + y * mat[1][1] + z * mat[2][1] + mat[3][1] * v[3]; + r[2] = x * mat[0][2] + y * mat[1][2] + z * mat[2][2] + mat[3][2] * v[3]; + r[3] = x * mat[0][3] + y * mat[1][3] + z * mat[2][3] + mat[3][3] * v[3]; } void mul_m4_v4(const float mat[4][4], float r[4]) { - mul_v4_m4v4(r, mat, r); + mul_v4_m4v4(r, mat, r); } void mul_v4d_m4v4d(double r[4], const float mat[4][4], const double v[4]) { - const double x = v[0]; - const double y = v[1]; - const double z = v[2]; + const double x = v[0]; + const double y = v[1]; + const double z = v[2]; - r[0] = x * (double)mat[0][0] + y * (double)mat[1][0] + z * (double)mat[2][0] + (double)mat[3][0] * v[3]; - r[1] = x * (double)mat[0][1] + y * (double)mat[1][1] + z * (double)mat[2][1] + (double)mat[3][1] * v[3]; - r[2] = x * (double)mat[0][2] + y * (double)mat[1][2] + z * (double)mat[2][2] + (double)mat[3][2] * v[3]; - r[3] = x * (double)mat[0][3] + y * (double)mat[1][3] + z * (double)mat[2][3] + (double)mat[3][3] * v[3]; + r[0] = x * (double)mat[0][0] + y * (double)mat[1][0] + z * (double)mat[2][0] + + (double)mat[3][0] * v[3]; + r[1] = x * (double)mat[0][1] + y * (double)mat[1][1] + z * (double)mat[2][1] + + (double)mat[3][1] * v[3]; + r[2] = x * (double)mat[0][2] + y * (double)mat[1][2] + z * (double)mat[2][2] + + (double)mat[3][2] * v[3]; + r[3] = x * (double)mat[0][3] + y * (double)mat[1][3] + z * (double)mat[2][3] + + (double)mat[3][3] * v[3]; } void mul_m4_v4d(const float mat[4][4], double r[4]) { - mul_v4d_m4v4d(r, mat, r); + mul_v4d_m4v4d(r, mat, r); } void mul_v4_m4v3(float r[4], const float M[4][4], const float v[3]) { - /* v has implicit w = 1.0f */ - r[0] = v[0] * M[0][0] + v[1] * M[1][0] + M[2][0] * v[2] + M[3][0]; - r[1] = v[0] * M[0][1] + v[1] * M[1][1] + M[2][1] * v[2] + M[3][1]; - r[2] = v[0] * M[0][2] + v[1] * M[1][2] + M[2][2] * v[2] + M[3][2]; - r[3] = v[0] * M[0][3] + v[1] * M[1][3] + M[2][3] * v[2] + M[3][3]; + /* v has implicit w = 1.0f */ + r[0] = v[0] * M[0][0] + v[1] * M[1][0] + M[2][0] * v[2] + M[3][0]; + r[1] = v[0] * M[0][1] + v[1] * M[1][1] + M[2][1] * v[2] + M[3][1]; + r[2] = v[0] * M[0][2] + v[1] * M[1][2] + M[2][2] * v[2] + M[3][2]; + r[3] = v[0] * M[0][3] + v[1] * M[1][3] + M[2][3] * v[2] + M[3][3]; } void mul_v3_m3v3(float r[3], const float M[3][3], const float a[3]) { - float t[3]; - copy_v3_v3(t, a); + float t[3]; + copy_v3_v3(t, a); - r[0] = M[0][0] * t[0] + M[1][0] * t[1] + M[2][0] * t[2]; - r[1] = M[0][1] * t[0] + M[1][1] * t[1] + M[2][1] * t[2]; - r[2] = M[0][2] * t[0] + M[1][2] * t[1] + M[2][2] * t[2]; + r[0] = M[0][0] * t[0] + M[1][0] * t[1] + M[2][0] * t[2]; + r[1] = M[0][1] * t[0] + M[1][1] * t[1] + M[2][1] * t[2]; + r[2] = M[0][2] * t[0] + M[1][2] * t[1] + M[2][2] * t[2]; } void mul_v3_m3v3_db(double r[3], const double M[3][3], const double a[3]) { - double t[3]; - copy_v3_v3_db(t, a); + double t[3]; + copy_v3_v3_db(t, a); - r[0] = M[0][0] * t[0] + M[1][0] * t[1] + M[2][0] * t[2]; - r[1] = M[0][1] * t[0] + M[1][1] * t[1] + M[2][1] * t[2]; - r[2] = M[0][2] * t[0] + M[1][2] * t[1] + M[2][2] * t[2]; + r[0] = M[0][0] * t[0] + M[1][0] * t[1] + M[2][0] * t[2]; + r[1] = M[0][1] * t[0] + M[1][1] * t[1] + M[2][1] * t[2]; + r[2] = M[0][2] * t[0] + M[1][2] * t[1] + M[2][2] * t[2]; } void mul_v2_m3v3(float r[2], const float M[3][3], const float a[3]) { - float t[3]; - copy_v3_v3(t, a); + float t[3]; + copy_v3_v3(t, a); - r[0] = M[0][0] * t[0] + M[1][0] * t[1] + M[2][0] * t[2]; - r[1] = M[0][1] * t[0] + M[1][1] * t[1] + M[2][1] * t[2]; + r[0] = M[0][0] * t[0] + M[1][0] * t[1] + M[2][0] * t[2]; + r[1] = M[0][1] * t[0] + M[1][1] * t[1] + M[2][1] * t[2]; } void mul_m3_v3(const float M[3][3], float r[3]) { - mul_v3_m3v3(r, M, (const float[3]){UNPACK3(r)}); + mul_v3_m3v3(r, M, (const float[3]){UNPACK3(r)}); } void mul_m3_v3_db(const double M[3][3], double r[3]) { - mul_v3_m3v3_db(r, M, (const double[3]){UNPACK3(r)}); + mul_v3_m3v3_db(r, M, (const double[3]){UNPACK3(r)}); } void mul_transposed_m3_v3(const float mat[3][3], float vec[3]) { - const float x = vec[0]; - const float y = vec[1]; + const float x = vec[0]; + const float y = vec[1]; - vec[0] = x * mat[0][0] + y * mat[0][1] + mat[0][2] * vec[2]; - vec[1] = x * mat[1][0] + y * mat[1][1] + mat[1][2] * vec[2]; - vec[2] = x * mat[2][0] + y * mat[2][1] + mat[2][2] * vec[2]; + vec[0] = x * mat[0][0] + y * mat[0][1] + mat[0][2] * vec[2]; + vec[1] = x * mat[1][0] + y * mat[1][1] + mat[1][2] * vec[2]; + vec[2] = x * mat[2][0] + y * mat[2][1] + mat[2][2] * vec[2]; } void mul_transposed_mat3_m4_v3(const float mat[4][4], float vec[3]) { - const float x = vec[0]; - const float y = vec[1]; + const float x = vec[0]; + const float y = vec[1]; - vec[0] = x * mat[0][0] + y * mat[0][1] + mat[0][2] * vec[2]; - vec[1] = x * mat[1][0] + y * mat[1][1] + mat[1][2] * vec[2]; - vec[2] = x * mat[2][0] + y * mat[2][1] + mat[2][2] * vec[2]; + vec[0] = x * mat[0][0] + y * mat[0][1] + mat[0][2] * vec[2]; + vec[1] = x * mat[1][0] + y * mat[1][1] + mat[1][2] * vec[2]; + vec[2] = x * mat[2][0] + y * mat[2][1] + mat[2][2] * vec[2]; } void mul_m3_fl(float m[3][3], float f) { - int i, j; + int i, j; - for (i = 0; i < 3; i++) { - for (j = 0; j < 3; j++) { - m[i][j] *= f; - } - } + for (i = 0; i < 3; i++) { + for (j = 0; j < 3; j++) { + m[i][j] *= f; + } + } } void mul_m4_fl(float m[4][4], float f) { - int i, j; + int i, j; - for (i = 0; i < 4; i++) { - for (j = 0; j < 4; j++) { - m[i][j] *= f; - } - } + for (i = 0; i < 4; i++) { + for (j = 0; j < 4; j++) { + m[i][j] *= f; + } + } } void mul_mat3_m4_fl(float m[4][4], float f) { - int i, j; + int i, j; - for (i = 0; i < 3; i++) { - for (j = 0; j < 3; j++) { - m[i][j] *= f; - } - } + for (i = 0; i < 3; i++) { + for (j = 0; j < 3; j++) { + m[i][j] *= f; + } + } } void negate_m3(float m[3][3]) { - int i, j; + int i, j; - for (i = 0; i < 3; i++) { - for (j = 0; j < 3; j++) { - m[i][j] *= -1.0f; - } - } + for (i = 0; i < 3; i++) { + for (j = 0; j < 3; j++) { + m[i][j] *= -1.0f; + } + } } void negate_mat3_m4(float m[4][4]) { - int i, j; + int i, j; - for (i = 0; i < 3; i++) { - for (j = 0; j < 3; j++) { - m[i][j] *= -1.0f; - } - } + for (i = 0; i < 3; i++) { + for (j = 0; j < 3; j++) { + m[i][j] *= -1.0f; + } + } } void negate_m4(float m[4][4]) { - int i, j; + int i, j; - for (i = 0; i < 4; i++) { - for (j = 0; j < 4; j++) { - m[i][j] *= -1.0f; - } - } + for (i = 0; i < 4; i++) { + for (j = 0; j < 4; j++) { + m[i][j] *= -1.0f; + } + } } void mul_m3_v3_double(const float mat[3][3], double vec[3]) { - const double x = vec[0]; - const double y = vec[1]; + const double x = vec[0]; + const double y = vec[1]; - vec[0] = x * (double)mat[0][0] + y * (double)mat[1][0] + (double)mat[2][0] * vec[2]; - vec[1] = x * (double)mat[0][1] + y * (double)mat[1][1] + (double)mat[2][1] * vec[2]; - vec[2] = x * (double)mat[0][2] + y * (double)mat[1][2] + (double)mat[2][2] * vec[2]; + vec[0] = x * (double)mat[0][0] + y * (double)mat[1][0] + (double)mat[2][0] * vec[2]; + vec[1] = x * (double)mat[0][1] + y * (double)mat[1][1] + (double)mat[2][1] * vec[2]; + vec[2] = x * (double)mat[0][2] + y * (double)mat[1][2] + (double)mat[2][2] * vec[2]; } void add_m3_m3m3(float m1[3][3], const float m2[3][3], const float m3[3][3]) { - int i, j; + int i, j; - for (i = 0; i < 3; i++) { - for (j = 0; j < 3; j++) { - m1[i][j] = m2[i][j] + m3[i][j]; - } - } + for (i = 0; i < 3; i++) { + for (j = 0; j < 3; j++) { + m1[i][j] = m2[i][j] + m3[i][j]; + } + } } void add_m4_m4m4(float m1[4][4], const float m2[4][4], const float m3[4][4]) { - int i, j; + int i, j; - for (i = 0; i < 4; i++) { - for (j = 0; j < 4; j++) { - m1[i][j] = m2[i][j] + m3[i][j]; - } - } + for (i = 0; i < 4; i++) { + for (j = 0; j < 4; j++) { + m1[i][j] = m2[i][j] + m3[i][j]; + } + } } void madd_m3_m3m3fl(float m1[3][3], const float m2[3][3], const float m3[3][3], const float f) { - int i, j; + int i, j; - for (i = 0; i < 3; i++) { - for (j = 0; j < 3; j++) { - m1[i][j] = m2[i][j] + m3[i][j] * f; - } - } + for (i = 0; i < 3; i++) { + for (j = 0; j < 3; j++) { + m1[i][j] = m2[i][j] + m3[i][j] * f; + } + } } void madd_m4_m4m4fl(float m1[4][4], const float m2[4][4], const float m3[4][4], const float f) { - int i, j; + int i, j; - for (i = 0; i < 4; i++) { - for (j = 0; j < 4; j++) { - m1[i][j] = m2[i][j] + m3[i][j] * f; - } - } + for (i = 0; i < 4; i++) { + for (j = 0; j < 4; j++) { + m1[i][j] = m2[i][j] + m3[i][j] * f; + } + } } void sub_m3_m3m3(float m1[3][3], const float m2[3][3], const float m3[3][3]) { - int i, j; + int i, j; - for (i = 0; i < 3; i++) { - for (j = 0; j < 3; j++) { - m1[i][j] = m2[i][j] - m3[i][j]; - } - } + for (i = 0; i < 3; i++) { + for (j = 0; j < 3; j++) { + m1[i][j] = m2[i][j] - m3[i][j]; + } + } } void sub_m4_m4m4(float m1[4][4], const float m2[4][4], const float m3[4][4]) { - int i, j; + int i, j; - for (i = 0; i < 4; i++) { - for (j = 0; j < 4; j++) { - m1[i][j] = m2[i][j] - m3[i][j]; - } - } + for (i = 0; i < 4; i++) { + for (j = 0; j < 4; j++) { + m1[i][j] = m2[i][j] - m3[i][j]; + } + } } float determinant_m3_array(const 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])); + 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])); } bool invert_m3_ex(float m[3][3], const float epsilon) { - float tmp[3][3]; - const bool success = invert_m3_m3_ex(tmp, m, epsilon); + float tmp[3][3]; + const bool success = invert_m3_m3_ex(tmp, m, epsilon); - copy_m3_m3(m, tmp); - return success; + copy_m3_m3(m, tmp); + return success; } bool invert_m3_m3_ex(float m1[3][3], const float m2[3][3], const float epsilon) { - float det; - int a, b; - bool success; + float det; + int a, b; + bool success; - BLI_assert(epsilon >= 0.0f); + BLI_assert(epsilon >= 0.0f); - /* calc adjoint */ - adjoint_m3_m3(m1, m2); + /* calc adjoint */ + adjoint_m3_m3(m1, m2); - /* then determinant old matrix! */ - det = determinant_m3_array(m2); + /* then determinant old matrix! */ + det = determinant_m3_array(m2); - success = (fabsf(det) > epsilon); + 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; + 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; } bool invert_m3(float m[3][3]) { - float tmp[3][3]; - const bool success = invert_m3_m3(tmp, m); + float tmp[3][3]; + const bool success = invert_m3_m3(tmp, m); - copy_m3_m3(m, tmp); - return success; + copy_m3_m3(m, tmp); + return success; } bool invert_m3_m3(float m1[3][3], const float m2[3][3]) { - float det; - int a, b; - bool success; + float det; + int a, b; + bool success; - /* calc adjoint */ - adjoint_m3_m3(m1, m2); + /* calc adjoint */ + adjoint_m3_m3(m1, m2); - /* then determinant old matrix! */ - det = determinant_m3_array(m2); + /* then determinant old matrix! */ + det = determinant_m3_array(m2); - success = (det != 0.0f); + success = (det != 0.0f); - 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; - } - } - } + 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; + return success; } bool invert_m4(float m[4][4]) { - float tmp[4][4]; - const bool success = invert_m4_m4(tmp, m); + float tmp[4][4]; + const bool success = invert_m4_m4(tmp, m); - copy_m4_m4(m, tmp); - return success; + copy_m4_m4(m, tmp); + return success; } bool invert_m4_m4(float inverse[4][4], const float mat[4][4]) { - /* Use optimized matrix inverse from Eigen, since performance - * impact of this function is significant in complex rigs. */ - return EIG_invert_m4_m4(inverse, mat); + /* Use optimized matrix inverse from Eigen, since performance + * impact of this function is significant in complex rigs. */ + return EIG_invert_m4_m4(inverse, mat); } /****************************** Linear Algebra *******************************/ void transpose_m3(float mat[3][3]) { - float t; + float t; - t = mat[0][1]; - mat[0][1] = mat[1][0]; - mat[1][0] = t; - t = mat[0][2]; - mat[0][2] = mat[2][0]; - mat[2][0] = t; - t = mat[1][2]; - mat[1][2] = mat[2][1]; - mat[2][1] = t; + t = mat[0][1]; + mat[0][1] = mat[1][0]; + mat[1][0] = t; + t = mat[0][2]; + mat[0][2] = mat[2][0]; + mat[2][0] = t; + t = mat[1][2]; + mat[1][2] = mat[2][1]; + mat[2][1] = t; } void transpose_m3_m3(float rmat[3][3], const float mat[3][3]) { - BLI_assert(rmat != mat); + BLI_assert(rmat != mat); - rmat[0][0] = mat[0][0]; - rmat[0][1] = mat[1][0]; - rmat[0][2] = mat[2][0]; - rmat[1][0] = mat[0][1]; - rmat[1][1] = mat[1][1]; - rmat[1][2] = mat[2][1]; - rmat[2][0] = mat[0][2]; - rmat[2][1] = mat[1][2]; - rmat[2][2] = mat[2][2]; + rmat[0][0] = mat[0][0]; + rmat[0][1] = mat[1][0]; + rmat[0][2] = mat[2][0]; + rmat[1][0] = mat[0][1]; + rmat[1][1] = mat[1][1]; + rmat[1][2] = mat[2][1]; + rmat[2][0] = mat[0][2]; + rmat[2][1] = mat[1][2]; + rmat[2][2] = mat[2][2]; } /* seems obscure but in-fact a common operation */ void transpose_m3_m4(float rmat[3][3], const float mat[4][4]) { - BLI_assert(&rmat[0][0] != &mat[0][0]); + BLI_assert(&rmat[0][0] != &mat[0][0]); - rmat[0][0] = mat[0][0]; - rmat[0][1] = mat[1][0]; - rmat[0][2] = mat[2][0]; - rmat[1][0] = mat[0][1]; - rmat[1][1] = mat[1][1]; - rmat[1][2] = mat[2][1]; - rmat[2][0] = mat[0][2]; - rmat[2][1] = mat[1][2]; - rmat[2][2] = mat[2][2]; + rmat[0][0] = mat[0][0]; + rmat[0][1] = mat[1][0]; + rmat[0][2] = mat[2][0]; + rmat[1][0] = mat[0][1]; + rmat[1][1] = mat[1][1]; + rmat[1][2] = mat[2][1]; + rmat[2][0] = mat[0][2]; + rmat[2][1] = mat[1][2]; + rmat[2][2] = mat[2][2]; } void transpose_m4(float mat[4][4]) { - float t; + float t; - t = mat[0][1]; - mat[0][1] = mat[1][0]; - mat[1][0] = t; - t = mat[0][2]; - mat[0][2] = mat[2][0]; - mat[2][0] = t; - t = mat[0][3]; - mat[0][3] = mat[3][0]; - mat[3][0] = t; + t = mat[0][1]; + mat[0][1] = mat[1][0]; + mat[1][0] = t; + t = mat[0][2]; + mat[0][2] = mat[2][0]; + mat[2][0] = t; + t = mat[0][3]; + mat[0][3] = mat[3][0]; + mat[3][0] = t; - t = mat[1][2]; - mat[1][2] = mat[2][1]; - mat[2][1] = t; - t = mat[1][3]; - mat[1][3] = mat[3][1]; - mat[3][1] = t; + t = mat[1][2]; + mat[1][2] = mat[2][1]; + mat[2][1] = t; + t = mat[1][3]; + mat[1][3] = mat[3][1]; + mat[3][1] = t; - t = mat[2][3]; - mat[2][3] = mat[3][2]; - mat[3][2] = t; + t = mat[2][3]; + mat[2][3] = mat[3][2]; + mat[3][2] = t; } void transpose_m4_m4(float rmat[4][4], const float mat[4][4]) { - BLI_assert(rmat != mat); - - rmat[0][0] = mat[0][0]; - rmat[0][1] = mat[1][0]; - rmat[0][2] = mat[2][0]; - rmat[0][3] = mat[3][0]; - rmat[1][0] = mat[0][1]; - rmat[1][1] = mat[1][1]; - rmat[1][2] = mat[2][1]; - rmat[1][3] = mat[3][1]; - rmat[2][0] = mat[0][2]; - rmat[2][1] = mat[1][2]; - rmat[2][2] = mat[2][2]; - rmat[2][3] = mat[3][2]; - rmat[3][0] = mat[0][3]; - rmat[3][1] = mat[1][3]; - rmat[3][2] = mat[2][3]; - rmat[3][3] = mat[3][3]; + BLI_assert(rmat != mat); + + rmat[0][0] = mat[0][0]; + rmat[0][1] = mat[1][0]; + rmat[0][2] = mat[2][0]; + rmat[0][3] = mat[3][0]; + rmat[1][0] = mat[0][1]; + rmat[1][1] = mat[1][1]; + rmat[1][2] = mat[2][1]; + rmat[1][3] = mat[3][1]; + rmat[2][0] = mat[0][2]; + rmat[2][1] = mat[1][2]; + rmat[2][2] = mat[2][2]; + rmat[2][3] = mat[3][2]; + rmat[3][0] = mat[0][3]; + rmat[3][1] = mat[1][3]; + rmat[3][2] = mat[2][3]; + rmat[3][3] = mat[3][3]; } /* TODO: return bool */ int compare_m4m4(const float mat1[4][4], const float mat2[4][4], float limit) { - if (compare_v4v4(mat1[0], mat2[0], limit)) { - if (compare_v4v4(mat1[1], mat2[1], limit)) { - if (compare_v4v4(mat1[2], mat2[2], limit)) { - if (compare_v4v4(mat1[3], mat2[3], limit)) { - return 1; - } - } - } - } - return 0; + if (compare_v4v4(mat1[0], mat2[0], limit)) { + if (compare_v4v4(mat1[1], mat2[1], limit)) { + if (compare_v4v4(mat1[2], mat2[2], limit)) { + if (compare_v4v4(mat1[3], mat2[3], limit)) { + return 1; + } + } + } + } + return 0; } /** @@ -1105,86 +1136,86 @@ int compare_m4m4(const float mat1[4][4], const float mat2[4][4], float limit) */ void orthogonalize_m3(float mat[3][3], int axis) { - float size[3]; - mat3_to_size(size, mat); - normalize_v3(mat[axis]); - switch (axis) { - case 0: - if (dot_v3v3(mat[0], mat[1]) < 1) { - cross_v3_v3v3(mat[2], mat[0], mat[1]); - normalize_v3(mat[2]); - cross_v3_v3v3(mat[1], mat[2], mat[0]); - } - else if (dot_v3v3(mat[0], mat[2]) < 1) { - cross_v3_v3v3(mat[1], mat[2], mat[0]); - normalize_v3(mat[1]); - cross_v3_v3v3(mat[2], mat[0], mat[1]); - } - else { - float vec[3]; - - vec[0] = mat[0][1]; - vec[1] = mat[0][2]; - vec[2] = mat[0][0]; - - cross_v3_v3v3(mat[2], mat[0], vec); - normalize_v3(mat[2]); - cross_v3_v3v3(mat[1], mat[2], mat[0]); - } - break; - case 1: - if (dot_v3v3(mat[1], mat[0]) < 1) { - cross_v3_v3v3(mat[2], mat[0], mat[1]); - normalize_v3(mat[2]); - cross_v3_v3v3(mat[0], mat[1], mat[2]); - } - else if (dot_v3v3(mat[0], mat[2]) < 1) { - cross_v3_v3v3(mat[0], mat[1], mat[2]); - normalize_v3(mat[0]); - cross_v3_v3v3(mat[2], mat[0], mat[1]); - } - else { - float vec[3]; - - vec[0] = mat[1][1]; - vec[1] = mat[1][2]; - vec[2] = mat[1][0]; - - cross_v3_v3v3(mat[0], mat[1], vec); - normalize_v3(mat[0]); - cross_v3_v3v3(mat[2], mat[0], mat[1]); - } - break; - case 2: - if (dot_v3v3(mat[2], mat[0]) < 1) { - cross_v3_v3v3(mat[1], mat[2], mat[0]); - normalize_v3(mat[1]); - cross_v3_v3v3(mat[0], mat[1], mat[2]); - } - else if (dot_v3v3(mat[2], mat[1]) < 1) { - cross_v3_v3v3(mat[0], mat[1], mat[2]); - normalize_v3(mat[0]); - cross_v3_v3v3(mat[1], mat[2], mat[0]); - } - else { - float vec[3]; - - vec[0] = mat[2][1]; - vec[1] = mat[2][2]; - vec[2] = mat[2][0]; - - cross_v3_v3v3(mat[0], vec, mat[2]); - normalize_v3(mat[0]); - cross_v3_v3v3(mat[1], mat[2], mat[0]); - } - break; - default: - BLI_assert(0); - break; - } - mul_v3_fl(mat[0], size[0]); - mul_v3_fl(mat[1], size[1]); - mul_v3_fl(mat[2], size[2]); + float size[3]; + mat3_to_size(size, mat); + normalize_v3(mat[axis]); + switch (axis) { + case 0: + if (dot_v3v3(mat[0], mat[1]) < 1) { + cross_v3_v3v3(mat[2], mat[0], mat[1]); + normalize_v3(mat[2]); + cross_v3_v3v3(mat[1], mat[2], mat[0]); + } + else if (dot_v3v3(mat[0], mat[2]) < 1) { + cross_v3_v3v3(mat[1], mat[2], mat[0]); + normalize_v3(mat[1]); + cross_v3_v3v3(mat[2], mat[0], mat[1]); + } + else { + float vec[3]; + + vec[0] = mat[0][1]; + vec[1] = mat[0][2]; + vec[2] = mat[0][0]; + + cross_v3_v3v3(mat[2], mat[0], vec); + normalize_v3(mat[2]); + cross_v3_v3v3(mat[1], mat[2], mat[0]); + } + break; + case 1: + if (dot_v3v3(mat[1], mat[0]) < 1) { + cross_v3_v3v3(mat[2], mat[0], mat[1]); + normalize_v3(mat[2]); + cross_v3_v3v3(mat[0], mat[1], mat[2]); + } + else if (dot_v3v3(mat[0], mat[2]) < 1) { + cross_v3_v3v3(mat[0], mat[1], mat[2]); + normalize_v3(mat[0]); + cross_v3_v3v3(mat[2], mat[0], mat[1]); + } + else { + float vec[3]; + + vec[0] = mat[1][1]; + vec[1] = mat[1][2]; + vec[2] = mat[1][0]; + + cross_v3_v3v3(mat[0], mat[1], vec); + normalize_v3(mat[0]); + cross_v3_v3v3(mat[2], mat[0], mat[1]); + } + break; + case 2: + if (dot_v3v3(mat[2], mat[0]) < 1) { + cross_v3_v3v3(mat[1], mat[2], mat[0]); + normalize_v3(mat[1]); + cross_v3_v3v3(mat[0], mat[1], mat[2]); + } + else if (dot_v3v3(mat[2], mat[1]) < 1) { + cross_v3_v3v3(mat[0], mat[1], mat[2]); + normalize_v3(mat[0]); + cross_v3_v3v3(mat[1], mat[2], mat[0]); + } + else { + float vec[3]; + + vec[0] = mat[2][1]; + vec[1] = mat[2][2]; + vec[2] = mat[2][0]; + + cross_v3_v3v3(mat[0], vec, mat[2]); + normalize_v3(mat[0]); + cross_v3_v3v3(mat[1], mat[2], mat[0]); + } + break; + default: + BLI_assert(0); + break; + } + mul_v3_fl(mat[0], size[0]); + mul_v3_fl(mat[1], size[1]); + mul_v3_fl(mat[2], size[2]); } /** @@ -1194,428 +1225,420 @@ void orthogonalize_m3(float mat[3][3], int axis) */ void orthogonalize_m4(float mat[4][4], int axis) { - float size[3]; - mat4_to_size(size, mat); - normalize_v3(mat[axis]); - switch (axis) { - case 0: - if (dot_v3v3(mat[0], mat[1]) < 1) { - cross_v3_v3v3(mat[2], mat[0], mat[1]); - normalize_v3(mat[2]); - cross_v3_v3v3(mat[1], mat[2], mat[0]); - } - else if (dot_v3v3(mat[0], mat[2]) < 1) { - cross_v3_v3v3(mat[1], mat[2], mat[0]); - normalize_v3(mat[1]); - cross_v3_v3v3(mat[2], mat[0], mat[1]); - } - else { - float vec[3]; - - vec[0] = mat[0][1]; - vec[1] = mat[0][2]; - vec[2] = mat[0][0]; - - cross_v3_v3v3(mat[2], mat[0], vec); - normalize_v3(mat[2]); - cross_v3_v3v3(mat[1], mat[2], mat[0]); - } - break; - case 1: - if (dot_v3v3(mat[1], mat[0]) < 1) { - cross_v3_v3v3(mat[2], mat[0], mat[1]); - normalize_v3(mat[2]); - cross_v3_v3v3(mat[0], mat[1], mat[2]); - } - else if (dot_v3v3(mat[0], mat[2]) < 1) { - cross_v3_v3v3(mat[0], mat[1], mat[2]); - normalize_v3(mat[0]); - cross_v3_v3v3(mat[2], mat[0], mat[1]); - } - else { - float vec[3]; - - vec[0] = mat[1][1]; - vec[1] = mat[1][2]; - vec[2] = mat[1][0]; - - cross_v3_v3v3(mat[0], mat[1], vec); - normalize_v3(mat[0]); - cross_v3_v3v3(mat[2], mat[0], mat[1]); - } - break; - case 2: - if (dot_v3v3(mat[2], mat[0]) < 1) { - cross_v3_v3v3(mat[1], mat[2], mat[0]); - normalize_v3(mat[1]); - cross_v3_v3v3(mat[0], mat[1], mat[2]); - } - else if (dot_v3v3(mat[2], mat[1]) < 1) { - cross_v3_v3v3(mat[0], mat[1], mat[2]); - normalize_v3(mat[0]); - cross_v3_v3v3(mat[1], mat[2], mat[0]); - } - else { - float vec[3]; - - vec[0] = mat[2][1]; - vec[1] = mat[2][2]; - vec[2] = mat[2][0]; - - cross_v3_v3v3(mat[0], vec, mat[2]); - normalize_v3(mat[0]); - cross_v3_v3v3(mat[1], mat[2], mat[0]); - } - break; - default: - BLI_assert(0); - break; - } - mul_v3_fl(mat[0], size[0]); - mul_v3_fl(mat[1], size[1]); - mul_v3_fl(mat[2], size[2]); + float size[3]; + mat4_to_size(size, mat); + normalize_v3(mat[axis]); + switch (axis) { + case 0: + if (dot_v3v3(mat[0], mat[1]) < 1) { + cross_v3_v3v3(mat[2], mat[0], mat[1]); + normalize_v3(mat[2]); + cross_v3_v3v3(mat[1], mat[2], mat[0]); + } + else if (dot_v3v3(mat[0], mat[2]) < 1) { + cross_v3_v3v3(mat[1], mat[2], mat[0]); + normalize_v3(mat[1]); + cross_v3_v3v3(mat[2], mat[0], mat[1]); + } + else { + float vec[3]; + + vec[0] = mat[0][1]; + vec[1] = mat[0][2]; + vec[2] = mat[0][0]; + + cross_v3_v3v3(mat[2], mat[0], vec); + normalize_v3(mat[2]); + cross_v3_v3v3(mat[1], mat[2], mat[0]); + } + break; + case 1: + if (dot_v3v3(mat[1], mat[0]) < 1) { + cross_v3_v3v3(mat[2], mat[0], mat[1]); + normalize_v3(mat[2]); + cross_v3_v3v3(mat[0], mat[1], mat[2]); + } + else if (dot_v3v3(mat[0], mat[2]) < 1) { + cross_v3_v3v3(mat[0], mat[1], mat[2]); + normalize_v3(mat[0]); + cross_v3_v3v3(mat[2], mat[0], mat[1]); + } + else { + float vec[3]; + + vec[0] = mat[1][1]; + vec[1] = mat[1][2]; + vec[2] = mat[1][0]; + + cross_v3_v3v3(mat[0], mat[1], vec); + normalize_v3(mat[0]); + cross_v3_v3v3(mat[2], mat[0], mat[1]); + } + break; + case 2: + if (dot_v3v3(mat[2], mat[0]) < 1) { + cross_v3_v3v3(mat[1], mat[2], mat[0]); + normalize_v3(mat[1]); + cross_v3_v3v3(mat[0], mat[1], mat[2]); + } + else if (dot_v3v3(mat[2], mat[1]) < 1) { + cross_v3_v3v3(mat[0], mat[1], mat[2]); + normalize_v3(mat[0]); + cross_v3_v3v3(mat[1], mat[2], mat[0]); + } + else { + float vec[3]; + + vec[0] = mat[2][1]; + vec[1] = mat[2][2]; + vec[2] = mat[2][0]; + + cross_v3_v3v3(mat[0], vec, mat[2]); + normalize_v3(mat[0]); + cross_v3_v3v3(mat[1], mat[2], mat[0]); + } + break; + default: + BLI_assert(0); + break; + } + mul_v3_fl(mat[0], size[0]); + mul_v3_fl(mat[1], size[1]); + mul_v3_fl(mat[2], size[2]); } bool is_orthogonal_m3(const float m[3][3]) { - int i, j; + int i, j; - for (i = 0; i < 3; i++) { - for (j = 0; j < i; j++) { - if (fabsf(dot_v3v3(m[i], m[j])) > 1e-5f) { - return false; - } - } - } + for (i = 0; i < 3; i++) { + for (j = 0; j < i; j++) { + if (fabsf(dot_v3v3(m[i], m[j])) > 1e-5f) { + return false; + } + } + } - return true; + return true; } bool is_orthogonal_m4(const float m[4][4]) { - int i, j; - - for (i = 0; i < 4; i++) { - for (j = 0; j < i; j++) { - if (fabsf(dot_v4v4(m[i], m[j])) > 1e-5f) { - return false; - } - } + int i, j; - } + for (i = 0; i < 4; i++) { + for (j = 0; j < i; j++) { + if (fabsf(dot_v4v4(m[i], m[j])) > 1e-5f) { + return false; + } + } + } - return true; + return true; } bool is_orthonormal_m3(const float m[3][3]) { - if (is_orthogonal_m3(m)) { - int i; + if (is_orthogonal_m3(m)) { + int i; - for (i = 0; i < 3; i++) { - if (fabsf(dot_v3v3(m[i], m[i]) - 1) > 1e-5f) { - return false; - } - } + for (i = 0; i < 3; i++) { + if (fabsf(dot_v3v3(m[i], m[i]) - 1) > 1e-5f) { + return false; + } + } - return true; - } + return true; + } - return false; + return false; } bool is_orthonormal_m4(const float m[4][4]) { - if (is_orthogonal_m4(m)) { - int i; + if (is_orthogonal_m4(m)) { + int i; - for (i = 0; i < 4; i++) - if (fabsf(dot_v4v4(m[i], m[i]) - 1) > 1e-5f) { - return false; - } + for (i = 0; i < 4; i++) + if (fabsf(dot_v4v4(m[i], m[i]) - 1) > 1e-5f) { + return false; + } - return true; - } + return true; + } - return false; + return false; } bool is_uniform_scaled_m3(const float m[3][3]) { - const float eps = 1e-7f; - float t[3][3]; - float l1, l2, l3, l4, l5, l6; + const float eps = 1e-7f; + float t[3][3]; + float l1, l2, l3, l4, l5, l6; - transpose_m3_m3(t, m); + transpose_m3_m3(t, m); - l1 = len_squared_v3(m[0]); - l2 = len_squared_v3(m[1]); - l3 = len_squared_v3(m[2]); + l1 = len_squared_v3(m[0]); + l2 = len_squared_v3(m[1]); + l3 = len_squared_v3(m[2]); - l4 = len_squared_v3(t[0]); - l5 = len_squared_v3(t[1]); - l6 = len_squared_v3(t[2]); + l4 = len_squared_v3(t[0]); + l5 = len_squared_v3(t[1]); + l6 = len_squared_v3(t[2]); - if (fabsf(l2 - l1) <= eps && - fabsf(l3 - l1) <= eps && - fabsf(l4 - l1) <= eps && - fabsf(l5 - l1) <= eps && - fabsf(l6 - l1) <= eps) - { - return true; - } + if (fabsf(l2 - l1) <= eps && fabsf(l3 - l1) <= eps && fabsf(l4 - l1) <= eps && + fabsf(l5 - l1) <= eps && fabsf(l6 - l1) <= eps) { + return true; + } - return false; + return false; } bool is_uniform_scaled_m4(const float m[4][4]) { - float t[3][3]; - copy_m3_m4(t, m); - return is_uniform_scaled_m3(t); + float t[3][3]; + copy_m3_m4(t, m); + return is_uniform_scaled_m3(t); } void normalize_m3_ex(float mat[3][3], float r_scale[3]) { - int i; - for (i = 0; i < 3; i++) { - r_scale[i] = normalize_v3(mat[i]); - } + int i; + for (i = 0; i < 3; i++) { + r_scale[i] = normalize_v3(mat[i]); + } } void normalize_m3(float mat[3][3]) { - int i; - for (i = 0; i < 3; i++) { - normalize_v3(mat[i]); - } + int i; + for (i = 0; i < 3; i++) { + normalize_v3(mat[i]); + } } void normalize_m3_m3_ex(float rmat[3][3], const float mat[3][3], float r_scale[3]) { - int i; - for (i = 0; i < 3; i++) { - r_scale[i] = normalize_v3_v3(rmat[i], mat[i]); - } + int i; + for (i = 0; i < 3; i++) { + r_scale[i] = normalize_v3_v3(rmat[i], mat[i]); + } } void normalize_m3_m3(float rmat[3][3], const float mat[3][3]) { - int i; - for (i = 0; i < 3; i++) { - normalize_v3_v3(rmat[i], mat[i]); - } + int i; + for (i = 0; i < 3; i++) { + normalize_v3_v3(rmat[i], mat[i]); + } } void normalize_m4_ex(float mat[4][4], float r_scale[3]) { - int i; - for (i = 0; i < 3; i++) { - r_scale[i] = normalize_v3(mat[i]); - if (r_scale[i] != 0.0f) { - mat[i][3] /= r_scale[i]; - } - } + int i; + for (i = 0; i < 3; i++) { + r_scale[i] = normalize_v3(mat[i]); + if (r_scale[i] != 0.0f) { + mat[i][3] /= r_scale[i]; + } + } } void normalize_m4(float mat[4][4]) { - int i; - for (i = 0; i < 3; i++) { - float len = normalize_v3(mat[i]); - if (len != 0.0f) { - mat[i][3] /= len; - } - } + int i; + for (i = 0; i < 3; i++) { + float len = normalize_v3(mat[i]); + if (len != 0.0f) { + mat[i][3] /= len; + } + } } void normalize_m4_m4_ex(float rmat[4][4], const float mat[4][4], float r_scale[3]) { - int i; - for (i = 0; i < 3; i++) { - r_scale[i] = normalize_v3_v3(rmat[i], mat[i]); - rmat[i][3] = (r_scale[i] != 0.0f) ? (mat[i][3] / r_scale[i]) : mat[i][3]; - } - copy_v4_v4(rmat[3], mat[3]); + int i; + for (i = 0; i < 3; i++) { + r_scale[i] = normalize_v3_v3(rmat[i], mat[i]); + rmat[i][3] = (r_scale[i] != 0.0f) ? (mat[i][3] / r_scale[i]) : mat[i][3]; + } + copy_v4_v4(rmat[3], mat[3]); } void normalize_m4_m4(float rmat[4][4], const float mat[4][4]) { - int i; - for (i = 0; i < 3; i++) { - float len = normalize_v3_v3(rmat[i], mat[i]); - rmat[i][3] = (len != 0.0f) ? (mat[i][3] / len) : mat[i][3]; - } - copy_v4_v4(rmat[3], mat[3]); + int i; + for (i = 0; i < 3; i++) { + float len = normalize_v3_v3(rmat[i], mat[i]); + rmat[i][3] = (len != 0.0f) ? (mat[i][3] / len) : mat[i][3]; + } + copy_v4_v4(rmat[3], mat[3]); } void adjoint_m2_m2(float m1[2][2], const float m[2][2]) { - BLI_assert(m1 != m); - m1[0][0] = m[1][1]; - m1[0][1] = -m[0][1]; - m1[1][0] = -m[1][0]; - m1[1][1] = m[0][0]; + BLI_assert(m1 != m); + m1[0][0] = m[1][1]; + m1[0][1] = -m[0][1]; + m1[1][0] = -m[1][0]; + m1[1][1] = m[0][0]; } void adjoint_m3_m3(float m1[3][3], const float m[3][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]; + 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]; - m1[1][0] = -m[1][0] * m[2][2] + m[1][2] * m[2][0]; - m1[1][1] = m[0][0] * m[2][2] - m[0][2] * m[2][0]; - m1[1][2] = -m[0][0] * m[1][2] + m[0][2] * m[1][0]; + m1[1][0] = -m[1][0] * m[2][2] + m[1][2] * m[2][0]; + m1[1][1] = m[0][0] * m[2][2] - m[0][2] * m[2][0]; + m1[1][2] = -m[0][0] * m[1][2] + m[0][2] * m[1][0]; - m1[2][0] = m[1][0] * m[2][1] - m[1][1] * m[2][0]; - m1[2][1] = -m[0][0] * m[2][1] + m[0][1] * m[2][0]; - m1[2][2] = m[0][0] * m[1][1] - m[0][1] * m[1][0]; + m1[2][0] = m[1][0] * m[2][1] - m[1][1] * m[2][0]; + m1[2][1] = -m[0][0] * m[2][1] + m[0][1] * m[2][0]; + m1[2][2] = m[0][0] * m[1][1] - m[0][1] * m[1][0]; } void adjoint_m4_m4(float out[4][4], const float in[4][4]) /* out = ADJ(in) */ { - float a1, a2, a3, a4, b1, b2, b3, b4; - float c1, c2, c3, c4, d1, d2, d3, d4; + float a1, a2, a3, a4, b1, b2, b3, b4; + float c1, c2, c3, c4, d1, d2, d3, d4; - a1 = in[0][0]; - b1 = in[0][1]; - c1 = in[0][2]; - d1 = in[0][3]; + a1 = in[0][0]; + b1 = in[0][1]; + c1 = in[0][2]; + d1 = in[0][3]; - a2 = in[1][0]; - b2 = in[1][1]; - c2 = in[1][2]; - d2 = in[1][3]; + a2 = in[1][0]; + b2 = in[1][1]; + c2 = in[1][2]; + d2 = in[1][3]; - a3 = in[2][0]; - b3 = in[2][1]; - c3 = in[2][2]; - d3 = in[2][3]; + a3 = in[2][0]; + b3 = in[2][1]; + c3 = in[2][2]; + d3 = in[2][3]; - a4 = in[3][0]; - b4 = in[3][1]; - c4 = in[3][2]; - d4 = in[3][3]; + a4 = in[3][0]; + b4 = in[3][1]; + c4 = in[3][2]; + d4 = in[3][3]; + out[0][0] = determinant_m3(b2, b3, b4, c2, c3, c4, d2, d3, d4); + out[1][0] = -determinant_m3(a2, a3, a4, c2, c3, c4, d2, d3, d4); + out[2][0] = determinant_m3(a2, a3, a4, b2, b3, b4, d2, d3, d4); + out[3][0] = -determinant_m3(a2, a3, a4, b2, b3, b4, c2, c3, c4); - out[0][0] = determinant_m3(b2, b3, b4, c2, c3, c4, d2, d3, d4); - out[1][0] = -determinant_m3(a2, a3, a4, c2, c3, c4, d2, d3, d4); - out[2][0] = determinant_m3(a2, a3, a4, b2, b3, b4, d2, d3, d4); - out[3][0] = -determinant_m3(a2, a3, a4, b2, b3, b4, c2, c3, c4); + out[0][1] = -determinant_m3(b1, b3, b4, c1, c3, c4, d1, d3, d4); + out[1][1] = determinant_m3(a1, a3, a4, c1, c3, c4, d1, d3, d4); + out[2][1] = -determinant_m3(a1, a3, a4, b1, b3, b4, d1, d3, d4); + out[3][1] = determinant_m3(a1, a3, a4, b1, b3, b4, c1, c3, c4); - out[0][1] = -determinant_m3(b1, b3, b4, c1, c3, c4, d1, d3, d4); - out[1][1] = determinant_m3(a1, a3, a4, c1, c3, c4, d1, d3, d4); - out[2][1] = -determinant_m3(a1, a3, a4, b1, b3, b4, d1, d3, d4); - out[3][1] = determinant_m3(a1, a3, a4, b1, b3, b4, c1, c3, c4); + out[0][2] = determinant_m3(b1, b2, b4, c1, c2, c4, d1, d2, d4); + out[1][2] = -determinant_m3(a1, a2, a4, c1, c2, c4, d1, d2, d4); + out[2][2] = determinant_m3(a1, a2, a4, b1, b2, b4, d1, d2, d4); + out[3][2] = -determinant_m3(a1, a2, a4, b1, b2, b4, c1, c2, c4); - out[0][2] = determinant_m3(b1, b2, b4, c1, c2, c4, d1, d2, d4); - out[1][2] = -determinant_m3(a1, a2, a4, c1, c2, c4, d1, d2, d4); - out[2][2] = determinant_m3(a1, a2, a4, b1, b2, b4, d1, d2, d4); - out[3][2] = -determinant_m3(a1, a2, a4, b1, b2, b4, c1, c2, c4); - - out[0][3] = -determinant_m3(b1, b2, b3, c1, c2, c3, d1, d2, d3); - out[1][3] = determinant_m3(a1, a2, a3, c1, c2, c3, d1, d2, d3); - out[2][3] = -determinant_m3(a1, a2, a3, b1, b2, b3, d1, d2, d3); - out[3][3] = determinant_m3(a1, a2, a3, b1, b2, b3, c1, c2, c3); + out[0][3] = -determinant_m3(b1, b2, b3, c1, c2, c3, d1, d2, d3); + out[1][3] = determinant_m3(a1, a2, a3, c1, c2, c3, d1, d2, d3); + out[2][3] = -determinant_m3(a1, a2, a3, b1, b2, b3, d1, d2, d3); + out[3][3] = determinant_m3(a1, a2, a3, b1, b2, b3, c1, c2, c3); } float determinant_m2(float a, float b, float c, float d) { - return a * d - b * c; + return a * d - b * c; } -float determinant_m3(float a1, float a2, float a3, - float b1, float b2, float b3, - float c1, float c2, float c3) +float determinant_m3( + float a1, float a2, float a3, float b1, float b2, float b3, float c1, float c2, float c3) { - float ans; + float ans; - ans = (a1 * determinant_m2(b2, b3, c2, c3) - - b1 * determinant_m2(a2, a3, c2, c3) + - c1 * determinant_m2(a2, a3, b2, b3)); + ans = (a1 * determinant_m2(b2, b3, c2, c3) - b1 * determinant_m2(a2, a3, c2, c3) + + c1 * determinant_m2(a2, a3, b2, b3)); - return ans; + return ans; } float determinant_m4(const float m[4][4]) { - float ans; - float a1, a2, a3, a4, b1, b2, b3, b4, c1, c2, c3, c4, d1, d2, d3, d4; + float ans; + float a1, a2, a3, a4, b1, b2, b3, b4, c1, c2, c3, c4, d1, d2, d3, d4; - a1 = m[0][0]; - b1 = m[0][1]; - c1 = m[0][2]; - d1 = m[0][3]; + a1 = m[0][0]; + b1 = m[0][1]; + c1 = m[0][2]; + d1 = m[0][3]; - a2 = m[1][0]; - b2 = m[1][1]; - c2 = m[1][2]; - d2 = m[1][3]; + a2 = m[1][0]; + b2 = m[1][1]; + c2 = m[1][2]; + d2 = m[1][3]; - a3 = m[2][0]; - b3 = m[2][1]; - c3 = m[2][2]; - d3 = m[2][3]; + a3 = m[2][0]; + b3 = m[2][1]; + c3 = m[2][2]; + d3 = m[2][3]; - a4 = m[3][0]; - b4 = m[3][1]; - c4 = m[3][2]; - d4 = m[3][3]; + a4 = m[3][0]; + b4 = m[3][1]; + c4 = m[3][2]; + d4 = m[3][3]; - ans = (a1 * determinant_m3(b2, b3, b4, c2, c3, c4, d2, d3, d4) - - b1 * determinant_m3(a2, a3, a4, c2, c3, c4, d2, d3, d4) + - c1 * determinant_m3(a2, a3, a4, b2, b3, b4, d2, d3, d4) - - d1 * determinant_m3(a2, a3, a4, b2, b3, b4, c2, c3, c4)); + ans = (a1 * determinant_m3(b2, b3, b4, c2, c3, c4, d2, d3, d4) - + b1 * determinant_m3(a2, a3, a4, c2, c3, c4, d2, d3, d4) + + c1 * determinant_m3(a2, a3, a4, b2, b3, b4, d2, d3, d4) - + d1 * determinant_m3(a2, a3, a4, b2, b3, b4, c2, c3, c4)); - return ans; + return ans; } /****************************** Transformations ******************************/ void size_to_mat3(float mat[3][3], const float size[3]) { - mat[0][0] = size[0]; - mat[0][1] = 0.0f; - mat[0][2] = 0.0f; - mat[1][1] = size[1]; - mat[1][0] = 0.0f; - mat[1][2] = 0.0f; - mat[2][2] = size[2]; - mat[2][1] = 0.0f; - mat[2][0] = 0.0f; + mat[0][0] = size[0]; + mat[0][1] = 0.0f; + mat[0][2] = 0.0f; + mat[1][1] = size[1]; + mat[1][0] = 0.0f; + mat[1][2] = 0.0f; + mat[2][2] = size[2]; + mat[2][1] = 0.0f; + mat[2][0] = 0.0f; } void size_to_mat4(float mat[4][4], const float size[3]) { - mat[0][0] = size[0]; - mat[0][1] = 0.0f; - mat[0][2] = 0.0f; - mat[0][3] = 0.0f; - mat[1][0] = 0.0f; - mat[1][1] = size[1]; - mat[1][2] = 0.0f; - mat[1][3] = 0.0f; - mat[2][0] = 0.0f; - mat[2][1] = 0.0f; - mat[2][2] = size[2]; - mat[2][3] = 0.0f; - mat[3][0] = 0.0f; - mat[3][1] = 0.0f; - mat[3][2] = 0.0f; - mat[3][3] = 1.0f; + mat[0][0] = size[0]; + mat[0][1] = 0.0f; + mat[0][2] = 0.0f; + mat[0][3] = 0.0f; + mat[1][0] = 0.0f; + mat[1][1] = size[1]; + mat[1][2] = 0.0f; + mat[1][3] = 0.0f; + mat[2][0] = 0.0f; + mat[2][1] = 0.0f; + mat[2][2] = size[2]; + mat[2][3] = 0.0f; + mat[3][0] = 0.0f; + mat[3][1] = 0.0f; + mat[3][2] = 0.0f; + mat[3][3] = 1.0f; } void mat3_to_size(float size[3], const float mat[3][3]) { - size[0] = len_v3(mat[0]); - size[1] = len_v3(mat[1]); - size[2] = len_v3(mat[2]); + size[0] = len_v3(mat[0]); + size[1] = len_v3(mat[1]); + size[2] = len_v3(mat[2]); } void mat4_to_size(float size[3], const float mat[4][4]) { - size[0] = len_v3(mat[0]); - size[1] = len_v3(mat[1]); - size[2] = len_v3(mat[2]); + size[0] = len_v3(mat[0]); + size[1] = len_v3(mat[1]); + size[2] = len_v3(mat[2]); } /* this gets the average scale of a matrix, only use when your scaling @@ -1623,77 +1646,77 @@ void mat4_to_size(float size[3], const float mat[4][4]) * and curve radius */ float mat3_to_scale(const float mat[3][3]) { - /* unit length vector */ - float unit_vec[3]; - copy_v3_fl(unit_vec, (float)M_SQRT1_3); - mul_m3_v3(mat, unit_vec); - return len_v3(unit_vec); + /* unit length vector */ + float unit_vec[3]; + copy_v3_fl(unit_vec, (float)M_SQRT1_3); + mul_m3_v3(mat, unit_vec); + return len_v3(unit_vec); } float mat4_to_scale(const float mat[4][4]) { - /* unit length vector */ - float unit_vec[3]; - copy_v3_fl(unit_vec, (float)M_SQRT1_3); - mul_mat3_m4_v3(mat, unit_vec); - return len_v3(unit_vec); + /* unit length vector */ + float unit_vec[3]; + copy_v3_fl(unit_vec, (float)M_SQRT1_3); + mul_mat3_m4_v3(mat, unit_vec); + return len_v3(unit_vec); } /** Return 2D scale (in XY plane) of given mat4. */ float mat4_to_xy_scale(const float M[4][4]) { - /* unit length vector in xy plane */ - float unit_vec[3] = {(float)M_SQRT1_2, (float)M_SQRT1_2, 0.0f}; - mul_mat3_m4_v3(M, unit_vec); - return len_v3(unit_vec); + /* unit length vector in xy plane */ + float unit_vec[3] = {(float)M_SQRT1_2, (float)M_SQRT1_2, 0.0f}; + mul_mat3_m4_v3(M, unit_vec); + return len_v3(unit_vec); } void mat3_to_rot_size(float rot[3][3], float size[3], const float mat3[3][3]) { - /* keep rot as a 3x3 matrix, the caller can convert into a quat or euler */ - size[0] = normalize_v3_v3(rot[0], mat3[0]); - size[1] = normalize_v3_v3(rot[1], mat3[1]); - size[2] = normalize_v3_v3(rot[2], mat3[2]); - if (UNLIKELY(is_negative_m3(rot))) { - negate_m3(rot); - negate_v3(size); - } + /* keep rot as a 3x3 matrix, the caller can convert into a quat or euler */ + size[0] = normalize_v3_v3(rot[0], mat3[0]); + size[1] = normalize_v3_v3(rot[1], mat3[1]); + size[2] = normalize_v3_v3(rot[2], mat3[2]); + if (UNLIKELY(is_negative_m3(rot))) { + negate_m3(rot); + negate_v3(size); + } } void mat4_to_loc_rot_size(float loc[3], float rot[3][3], float size[3], const float wmat[4][4]) { - float mat3[3][3]; /* wmat -> 3x3 */ + float mat3[3][3]; /* wmat -> 3x3 */ - copy_m3_m4(mat3, wmat); - mat3_to_rot_size(rot, size, mat3); + copy_m3_m4(mat3, wmat); + mat3_to_rot_size(rot, size, mat3); - /* location */ - copy_v3_v3(loc, wmat[3]); + /* location */ + copy_v3_v3(loc, wmat[3]); } void mat4_to_loc_quat(float loc[3], float quat[4], const float wmat[4][4]) { - float mat3[3][3]; - float mat3_n[3][3]; /* normalized mat3 */ + float mat3[3][3]; + float mat3_n[3][3]; /* normalized mat3 */ - copy_m3_m4(mat3, wmat); - normalize_m3_m3(mat3_n, mat3); + copy_m3_m4(mat3, wmat); + normalize_m3_m3(mat3_n, mat3); - /* so scale doesn't interfere with rotation [#24291] */ - /* note: this is a workaround for negative matrix not working for rotation conversion, FIXME */ - if (is_negative_m3(mat3)) { - negate_m3(mat3_n); - } + /* so scale doesn't interfere with rotation [#24291] */ + /* note: this is a workaround for negative matrix not working for rotation conversion, FIXME */ + if (is_negative_m3(mat3)) { + negate_m3(mat3_n); + } - mat3_normalized_to_quat(quat, mat3_n); - copy_v3_v3(loc, wmat[3]); + mat3_normalized_to_quat(quat, mat3_n); + copy_v3_v3(loc, wmat[3]); } void mat4_decompose(float loc[3], float quat[4], float size[3], const float wmat[4][4]) { - float rot[3][3]; - mat4_to_loc_rot_size(loc, rot, size, wmat); - mat3_normalized_to_quat(quat, rot); + float rot[3][3]; + mat4_to_loc_rot_size(loc, rot, size, wmat); + mat3_normalized_to_quat(quat, rot); } /** @@ -1708,46 +1731,46 @@ void mat4_decompose(float loc[3], float quat[4], float size[3], const float wmat #ifndef MATH_STANDALONE void mat3_polar_decompose(const float mat3[3][3], float r_U[3][3], float r_P[3][3]) { - /* From svd decomposition (M = WSV*), we have: - * U = WV* - * P = VSV* - */ - float W[3][3], S[3][3], V[3][3], Vt[3][3]; - float sval[3]; + /* From svd decomposition (M = WSV*), we have: + * U = WV* + * P = VSV* + */ + float W[3][3], S[3][3], V[3][3], Vt[3][3]; + float sval[3]; - BLI_svd_m3(mat3, W, sval, V); + BLI_svd_m3(mat3, W, sval, V); - size_to_mat3(S, sval); + size_to_mat3(S, sval); - transpose_m3_m3(Vt, V); - mul_m3_m3m3(r_U, W, Vt); - mul_m3_series(r_P, V, S, Vt); + transpose_m3_m3(Vt, V); + mul_m3_m3m3(r_U, W, Vt); + mul_m3_series(r_P, V, S, Vt); } #endif void scale_m3_fl(float m[3][3], float scale) { - m[0][0] = m[1][1] = m[2][2] = scale; - m[0][1] = m[0][2] = 0.0; - m[1][0] = m[1][2] = 0.0; - m[2][0] = m[2][1] = 0.0; + m[0][0] = m[1][1] = m[2][2] = scale; + m[0][1] = m[0][2] = 0.0; + m[1][0] = m[1][2] = 0.0; + m[2][0] = m[2][1] = 0.0; } void scale_m4_fl(float m[4][4], float scale) { - m[0][0] = m[1][1] = m[2][2] = scale; - m[3][3] = 1.0; - m[0][1] = m[0][2] = m[0][3] = 0.0; - m[1][0] = m[1][2] = m[1][3] = 0.0; - m[2][0] = m[2][1] = m[2][3] = 0.0; - m[3][0] = m[3][1] = m[3][2] = 0.0; + m[0][0] = m[1][1] = m[2][2] = scale; + m[3][3] = 1.0; + m[0][1] = m[0][2] = m[0][3] = 0.0; + m[1][0] = m[1][2] = m[1][3] = 0.0; + m[2][0] = m[2][1] = m[2][3] = 0.0; + m[3][0] = m[3][1] = m[3][2] = 0.0; } void translate_m4(float mat[4][4], float Tx, float Ty, float Tz) { - mat[3][0] += (Tx * mat[0][0] + Ty * mat[1][0] + Tz * mat[2][0]); - mat[3][1] += (Tx * mat[0][1] + Ty * mat[1][1] + Tz * mat[2][1]); - mat[3][2] += (Tx * mat[0][2] + Ty * mat[1][2] + Tz * mat[2][2]); + mat[3][0] += (Tx * mat[0][0] + Ty * mat[1][0] + Tz * mat[2][0]); + mat[3][1] += (Tx * mat[0][1] + Ty * mat[1][1] + Tz * mat[2][1]); + mat[3][2] += (Tx * mat[0][2] + Ty * mat[1][2] + Tz * mat[2][2]); } /* TODO: enum for axis? */ @@ -1760,39 +1783,39 @@ void translate_m4(float mat[4][4], float Tx, float Ty, float Tz) */ void rotate_m4(float mat[4][4], const char axis, const float angle) { - const float angle_cos = cosf(angle); - const float angle_sin = sinf(angle); - - assert(axis >= 'X' && axis <= 'Z'); - - switch (axis) { - case 'X': - for (int col = 0; col < 4; col++) { - float temp = angle_cos * mat[1][col] + angle_sin * mat[2][col]; - mat[2][col] = -angle_sin * mat[1][col] + angle_cos * mat[2][col]; - mat[1][col] = temp; - } - break; - - case 'Y': - for (int col = 0; col < 4; col++) { - float temp = angle_cos * mat[0][col] - angle_sin * mat[2][col]; - mat[2][col] = angle_sin * mat[0][col] + angle_cos * mat[2][col]; - mat[0][col] = temp; - } - break; - - case 'Z': - for (int col = 0; col < 4; col++) { - float temp = angle_cos * mat[0][col] + angle_sin * mat[1][col]; - mat[1][col] = -angle_sin * mat[0][col] + angle_cos * mat[1][col]; - mat[0][col] = temp; - } - break; - default: - BLI_assert(0); - break; - } + const float angle_cos = cosf(angle); + const float angle_sin = sinf(angle); + + assert(axis >= 'X' && axis <= 'Z'); + + switch (axis) { + case 'X': + for (int col = 0; col < 4; col++) { + float temp = angle_cos * mat[1][col] + angle_sin * mat[2][col]; + mat[2][col] = -angle_sin * mat[1][col] + angle_cos * mat[2][col]; + mat[1][col] = temp; + } + break; + + case 'Y': + for (int col = 0; col < 4; col++) { + float temp = angle_cos * mat[0][col] - angle_sin * mat[2][col]; + mat[2][col] = angle_sin * mat[0][col] + angle_cos * mat[2][col]; + mat[0][col] = temp; + } + break; + + case 'Z': + for (int col = 0; col < 4; col++) { + float temp = angle_cos * mat[0][col] + angle_sin * mat[1][col]; + mat[1][col] = -angle_sin * mat[0][col] + angle_cos * mat[1][col]; + mat[0][col] = temp; + } + break; + default: + BLI_assert(0); + break; + } } /** @@ -1805,61 +1828,67 @@ void rotate_m4(float mat[4][4], const char axis, const float angle) */ void transform_pivot_set_m4(float mat[4][4], const float pivot[3]) { - float tmat[4][4]; + float tmat[4][4]; - unit_m4(tmat); + unit_m4(tmat); - copy_v3_v3(tmat[3], pivot); - mul_m4_m4m4(mat, tmat, mat); + copy_v3_v3(tmat[3], pivot); + mul_m4_m4m4(mat, tmat, mat); - /* invert the matrix */ - negate_v3(tmat[3]); - mul_m4_m4m4(mat, mat, tmat); + /* invert the matrix */ + negate_v3(tmat[3]); + mul_m4_m4m4(mat, mat, tmat); } -void blend_m3_m3m3(float out[3][3], const float dst[3][3], const float src[3][3], const float srcweight) +void blend_m3_m3m3(float out[3][3], + const float dst[3][3], + const float src[3][3], + const float srcweight) { - float srot[3][3], drot[3][3]; - float squat[4], dquat[4], fquat[4]; - float sscale[3], dscale[3], fsize[3]; - float rmat[3][3], smat[3][3]; + float srot[3][3], drot[3][3]; + float squat[4], dquat[4], fquat[4]; + float sscale[3], dscale[3], fsize[3]; + float rmat[3][3], smat[3][3]; - mat3_to_rot_size(drot, dscale, dst); - mat3_to_rot_size(srot, sscale, src); + mat3_to_rot_size(drot, dscale, dst); + mat3_to_rot_size(srot, sscale, src); - mat3_normalized_to_quat(dquat, drot); - mat3_normalized_to_quat(squat, srot); + mat3_normalized_to_quat(dquat, drot); + mat3_normalized_to_quat(squat, srot); - /* do blending */ - interp_qt_qtqt(fquat, dquat, squat, srcweight); - interp_v3_v3v3(fsize, dscale, sscale, srcweight); + /* do blending */ + interp_qt_qtqt(fquat, dquat, squat, srcweight); + interp_v3_v3v3(fsize, dscale, sscale, srcweight); - /* compose new matrix */ - quat_to_mat3(rmat, fquat); - size_to_mat3(smat, fsize); - mul_m3_m3m3(out, rmat, smat); + /* compose new matrix */ + quat_to_mat3(rmat, fquat); + size_to_mat3(smat, fsize); + mul_m3_m3m3(out, rmat, smat); } -void blend_m4_m4m4(float out[4][4], const float dst[4][4], const float src[4][4], const float srcweight) +void blend_m4_m4m4(float out[4][4], + const float dst[4][4], + const float src[4][4], + const float srcweight) { - float sloc[3], dloc[3], floc[3]; - float srot[3][3], drot[3][3]; - float squat[4], dquat[4], fquat[4]; - float sscale[3], dscale[3], fsize[3]; + float sloc[3], dloc[3], floc[3]; + float srot[3][3], drot[3][3]; + float squat[4], dquat[4], fquat[4]; + float sscale[3], dscale[3], fsize[3]; - mat4_to_loc_rot_size(dloc, drot, dscale, dst); - mat4_to_loc_rot_size(sloc, srot, sscale, src); + mat4_to_loc_rot_size(dloc, drot, dscale, dst); + mat4_to_loc_rot_size(sloc, srot, sscale, src); - mat3_normalized_to_quat(dquat, drot); - mat3_normalized_to_quat(squat, srot); + mat3_normalized_to_quat(dquat, drot); + mat3_normalized_to_quat(squat, srot); - /* do blending */ - interp_v3_v3v3(floc, dloc, sloc, srcweight); - interp_qt_qtqt(fquat, dquat, squat, srcweight); - interp_v3_v3v3(fsize, dscale, sscale, srcweight); + /* do blending */ + interp_v3_v3v3(floc, dloc, sloc, srcweight); + interp_qt_qtqt(fquat, dquat, squat, srcweight); + interp_v3_v3v3(fsize, dscale, sscale, srcweight); - /* compose new matrix */ - loc_quat_size_to_mat4(out, floc, fquat, fsize); + /* compose new matrix */ + loc_quat_size_to_mat4(out, floc, fquat, fsize); } /* for builds without Eigen */ @@ -1880,31 +1909,31 @@ void blend_m4_m4m4(float out[4][4], const float dst[4][4], const float src[4][4] */ void interp_m3_m3m3(float R[3][3], const float A[3][3], const float B[3][3], const float t) { - /* 'Rotation' component ('U' part of polar decomposition, - * the closest orthogonal matrix to M3 rot/scale - * transformation matrix), spherically interpolated. */ - float U_A[3][3], U_B[3][3], U[3][3]; - float quat_A[4], quat_B[4], quat[4]; - /* 'Scaling' component ('P' part of polar decomposition, i.e. scaling in U-defined space), - * linearly interpolated. */ - float P_A[3][3], P_B[3][3], P[3][3]; + /* 'Rotation' component ('U' part of polar decomposition, + * the closest orthogonal matrix to M3 rot/scale + * transformation matrix), spherically interpolated. */ + float U_A[3][3], U_B[3][3], U[3][3]; + float quat_A[4], quat_B[4], quat[4]; + /* 'Scaling' component ('P' part of polar decomposition, i.e. scaling in U-defined space), + * linearly interpolated. */ + float P_A[3][3], P_B[3][3], P[3][3]; - int i; + int i; - mat3_polar_decompose(A, U_A, P_A); - mat3_polar_decompose(B, U_B, P_B); + mat3_polar_decompose(A, U_A, P_A); + mat3_polar_decompose(B, U_B, P_B); - mat3_to_quat(quat_A, U_A); - mat3_to_quat(quat_B, U_B); - interp_qt_qtqt(quat, quat_A, quat_B, t); - quat_to_mat3(U, quat); + mat3_to_quat(quat_A, U_A); + mat3_to_quat(quat_B, U_B); + interp_qt_qtqt(quat, quat_A, quat_B, t); + quat_to_mat3(U, quat); - for (i = 0; i < 3; i++) { - interp_v3_v3v3(P[i], P_A[i], P_B[i], t); - } + for (i = 0; i < 3; i++) { + interp_v3_v3v3(P[i], P_A[i], P_B[i], t); + } - /* And we reconstruct rot/scale matrix from interpolated polar components */ - mul_m3_m3m3(R, U, P); + /* And we reconstruct rot/scale matrix from interpolated polar components */ + mul_m3_m3m3(R, U, P); } /** @@ -1917,168 +1946,173 @@ void interp_m3_m3m3(float R[3][3], const float A[3][3], const float B[3][3], con */ void interp_m4_m4m4(float R[4][4], const float A[4][4], const float B[4][4], const float t) { - float A3[3][3], B3[3][3], R3[3][3]; + float A3[3][3], B3[3][3], R3[3][3]; - /* Location component, linearly interpolated. */ - float loc_A[3], loc_B[3], loc[3]; + /* Location component, linearly interpolated. */ + float loc_A[3], loc_B[3], loc[3]; - copy_v3_v3(loc_A, A[3]); - copy_v3_v3(loc_B, B[3]); - interp_v3_v3v3(loc, loc_A, loc_B, t); + copy_v3_v3(loc_A, A[3]); + copy_v3_v3(loc_B, B[3]); + interp_v3_v3v3(loc, loc_A, loc_B, t); - copy_m3_m4(A3, A); - copy_m3_m4(B3, B); + copy_m3_m4(A3, A); + copy_m3_m4(B3, B); - interp_m3_m3m3(R3, A3, B3, t); + interp_m3_m3m3(R3, A3, B3, t); - copy_m4_m3(R, R3); - copy_v3_v3(R[3], loc); + copy_m4_m3(R, R3); + copy_v3_v3(R[3], loc); } -#endif /* MATH_STANDALONE */ +#endif /* MATH_STANDALONE */ bool is_negative_m3(const float mat[3][3]) { - float vec[3]; - cross_v3_v3v3(vec, mat[0], mat[1]); - return (dot_v3v3(vec, mat[2]) < 0.0f); + float vec[3]; + cross_v3_v3v3(vec, mat[0], mat[1]); + return (dot_v3v3(vec, mat[2]) < 0.0f); } bool is_negative_m4(const float mat[4][4]) { - float vec[3]; - cross_v3_v3v3(vec, mat[0], mat[1]); - return (dot_v3v3(vec, mat[2]) < 0.0f); + float vec[3]; + cross_v3_v3v3(vec, mat[0], mat[1]); + return (dot_v3v3(vec, mat[2]) < 0.0f); } bool is_zero_m3(const float mat[3][3]) { - return (is_zero_v3(mat[0]) && - is_zero_v3(mat[1]) && - is_zero_v3(mat[2])); + return (is_zero_v3(mat[0]) && is_zero_v3(mat[1]) && is_zero_v3(mat[2])); } bool is_zero_m4(const float mat[4][4]) { - return (is_zero_v4(mat[0]) && - is_zero_v4(mat[1]) && - is_zero_v4(mat[2]) && - is_zero_v4(mat[3])); + return (is_zero_v4(mat[0]) && is_zero_v4(mat[1]) && is_zero_v4(mat[2]) && is_zero_v4(mat[3])); } bool equals_m3m3(const float mat1[3][3], const float mat2[3][3]) { - return (equals_v3v3(mat1[0], mat2[0]) && - equals_v3v3(mat1[1], mat2[1]) && - equals_v3v3(mat1[2], mat2[2])); + return (equals_v3v3(mat1[0], mat2[0]) && equals_v3v3(mat1[1], mat2[1]) && + equals_v3v3(mat1[2], mat2[2])); } bool equals_m4m4(const float mat1[4][4], const float mat2[4][4]) { - return (equals_v4v4(mat1[0], mat2[0]) && - equals_v4v4(mat1[1], mat2[1]) && - equals_v4v4(mat1[2], mat2[2]) && - equals_v4v4(mat1[3], mat2[3])); + return (equals_v4v4(mat1[0], mat2[0]) && equals_v4v4(mat1[1], mat2[1]) && + equals_v4v4(mat1[2], mat2[2]) && equals_v4v4(mat1[3], mat2[3])); } /* make a 4x4 matrix out of 3 transform components */ /* matrices are made in the order: scale * rot * loc */ /* TODO: need to have a version that allows for rotation order... */ -void loc_eul_size_to_mat4(float mat[4][4], const float loc[3], const float eul[3], const float size[3]) +void loc_eul_size_to_mat4(float mat[4][4], + const float loc[3], + const float eul[3], + const float size[3]) { - float rmat[3][3], smat[3][3], tmat[3][3]; + float rmat[3][3], smat[3][3], tmat[3][3]; - /* initialize new matrix */ - unit_m4(mat); + /* initialize new matrix */ + unit_m4(mat); - /* make rotation + scaling part */ - eul_to_mat3(rmat, eul); - size_to_mat3(smat, size); - mul_m3_m3m3(tmat, rmat, smat); + /* make rotation + scaling part */ + eul_to_mat3(rmat, eul); + size_to_mat3(smat, size); + mul_m3_m3m3(tmat, rmat, smat); - /* copy rot/scale part to output matrix*/ - copy_m4_m3(mat, tmat); + /* copy rot/scale part to output matrix*/ + copy_m4_m3(mat, tmat); - /* copy location to matrix */ - mat[3][0] = loc[0]; - mat[3][1] = loc[1]; - mat[3][2] = loc[2]; + /* copy location to matrix */ + mat[3][0] = loc[0]; + mat[3][1] = loc[1]; + mat[3][2] = loc[2]; } /* make a 4x4 matrix out of 3 transform components */ /* matrices are made in the order: scale * rot * loc */ -void loc_eulO_size_to_mat4(float mat[4][4], const float loc[3], const float eul[3], const float size[3], const short rotOrder) +void loc_eulO_size_to_mat4(float mat[4][4], + const float loc[3], + const float eul[3], + const float size[3], + const short rotOrder) { - float rmat[3][3], smat[3][3], tmat[3][3]; + float rmat[3][3], smat[3][3], tmat[3][3]; - /* initialize new matrix */ - unit_m4(mat); + /* initialize new matrix */ + unit_m4(mat); - /* make rotation + scaling part */ - eulO_to_mat3(rmat, eul, rotOrder); - size_to_mat3(smat, size); - mul_m3_m3m3(tmat, rmat, smat); + /* make rotation + scaling part */ + eulO_to_mat3(rmat, eul, rotOrder); + size_to_mat3(smat, size); + mul_m3_m3m3(tmat, rmat, smat); - /* copy rot/scale part to output matrix*/ - copy_m4_m3(mat, tmat); + /* copy rot/scale part to output matrix*/ + copy_m4_m3(mat, tmat); - /* copy location to matrix */ - mat[3][0] = loc[0]; - mat[3][1] = loc[1]; - mat[3][2] = loc[2]; + /* copy location to matrix */ + mat[3][0] = loc[0]; + mat[3][1] = loc[1]; + mat[3][2] = loc[2]; } - /* make a 4x4 matrix out of 3 transform components */ /* matrices are made in the order: scale * rot * loc */ -void loc_quat_size_to_mat4(float mat[4][4], const float loc[3], const float quat[4], const float size[3]) +void loc_quat_size_to_mat4(float mat[4][4], + const float loc[3], + const float quat[4], + const float size[3]) { - float rmat[3][3], smat[3][3], tmat[3][3]; + float rmat[3][3], smat[3][3], tmat[3][3]; - /* initialize new matrix */ - unit_m4(mat); + /* initialize new matrix */ + unit_m4(mat); - /* make rotation + scaling part */ - quat_to_mat3(rmat, quat); - size_to_mat3(smat, size); - mul_m3_m3m3(tmat, rmat, smat); + /* make rotation + scaling part */ + quat_to_mat3(rmat, quat); + size_to_mat3(smat, size); + mul_m3_m3m3(tmat, rmat, smat); - /* copy rot/scale part to output matrix*/ - copy_m4_m3(mat, tmat); + /* copy rot/scale part to output matrix*/ + copy_m4_m3(mat, tmat); - /* copy location to matrix */ - mat[3][0] = loc[0]; - mat[3][1] = loc[1]; - mat[3][2] = loc[2]; + /* copy location to matrix */ + mat[3][0] = loc[0]; + mat[3][1] = loc[1]; + mat[3][2] = loc[2]; } -void loc_axisangle_size_to_mat4(float mat[4][4], const float loc[3], const float axis[3], const float angle, const float size[3]) +void loc_axisangle_size_to_mat4(float mat[4][4], + const float loc[3], + const float axis[3], + const float angle, + const float size[3]) { - float q[4]; - axis_angle_to_quat(q, axis, angle); - loc_quat_size_to_mat4(mat, loc, q, size); + float q[4]; + axis_angle_to_quat(q, axis, angle); + loc_quat_size_to_mat4(mat, loc, q, size); } /*********************************** Other ***********************************/ void print_m3(const char *str, const float m[3][3]) { - printf("%s\n", str); - printf("%f %f %f\n", m[0][0], m[1][0], m[2][0]); - printf("%f %f %f\n", m[0][1], m[1][1], m[2][1]); - printf("%f %f %f\n", m[0][2], m[1][2], m[2][2]); - printf("\n"); + printf("%s\n", str); + printf("%f %f %f\n", m[0][0], m[1][0], m[2][0]); + printf("%f %f %f\n", m[0][1], m[1][1], m[2][1]); + printf("%f %f %f\n", m[0][2], m[1][2], m[2][2]); + printf("\n"); } void print_m4(const char *str, const float m[4][4]) { - printf("%s\n", str); - printf("%f %f %f %f\n", m[0][0], m[1][0], m[2][0], m[3][0]); - printf("%f %f %f %f\n", m[0][1], m[1][1], m[2][1], m[3][1]); - printf("%f %f %f %f\n", m[0][2], m[1][2], m[2][2], m[3][2]); - printf("%f %f %f %f\n", m[0][3], m[1][3], m[2][3], m[3][3]); - printf("\n"); + printf("%s\n", str); + printf("%f %f %f %f\n", m[0][0], m[1][0], m[2][0], m[3][0]); + printf("%f %f %f %f\n", m[0][1], m[1][1], m[2][1], m[3][1]); + printf("%f %f %f %f\n", m[0][2], m[1][2], m[2][2], m[3][2]); + printf("%f %f %f %f\n", m[0][3], m[1][3], m[2][3], m[3][3]); + printf("\n"); } /*********************************** SVD ************************************ @@ -2093,501 +2127,495 @@ void print_m4(const char *str, const float m[4][4]) void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4]) { - float A[4][4]; - float work1[4], work2[4]; - int m = 4; - int n = 4; - int maxiter = 200; - int nu = min_ii(m, n); - - float *work = work1; - float *e = work2; - float eps; - - 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. */ - - int nct = min_ii(m - 1, n); - int nrt = max_ii(0, min_ii(n - 2, m)); - - copy_m4_m4(A, A_); - zero_m4(U); - zero_v4(s); - - for (k = 0; k < max_ii(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. */ - s[k] = 0; - for (i = k; i < m; i++) { - s[k] = hypotf(s[k], A[i][k]); - } - if (s[k] != 0.0f) { - float invsk; - if (A[k][k] < 0.0f) { - s[k] = -s[k]; - } - invsk = 1.0f / s[k]; - for (i = k; i < m; i++) { - A[i][k] *= invsk; - } - A[k][k] += 1.0f; - } - s[k] = -s[k]; - } - for (j = k + 1; j < n; j++) { - if ((k < nct) && (s[k] != 0.0f)) { - - /* Apply the transformation. */ - - float t = 0; - for (i = k; i < m; i++) { - t += A[i][k] * A[i][j]; - } - t = -t / A[k][k]; - for (i = k; i < m; i++) { - A[i][j] += t * A[i][k]; - } - } - - /* 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. */ - - 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. */ - e[k] = 0; - for (i = k + 1; i < n; i++) { - e[k] = hypotf(e[k], e[i]); - } - if (e[k] != 0.0f) { - float invek; - if (e[k + 1] < 0.0f) { - e[k] = -e[k]; - } - invek = 1.0f / e[k]; - for (i = k + 1; i < n; i++) { - e[i] *= invek; - } - e[k + 1] += 1.0f; - } - e[k] = -e[k]; - if ((k + 1 < m) & (e[k] != 0.0f)) { - float invek1; - - /* Apply the transformation. */ - - for (i = k + 1; i < m; i++) { - work[i] = 0.0f; - } - for (j = k + 1; j < n; j++) { - for (i = k + 1; i < m; i++) { - work[i] += e[j] * A[i][j]; - } - } - invek1 = 1.0f / e[k + 1]; - for (j = k + 1; j < n; j++) { - float t = -e[j] * invek1; - for (i = k + 1; i < m; i++) { - A[i][j] += t * work[i]; - } - } - } - - /* 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. */ - - p = min_ii(n, m + 1); - if (nct < n) { - s[nct] = A[nct][nct]; - } - if (m < p) { - s[p - 1] = 0.0f; - } - if (nrt + 1 < p) { - e[nrt] = A[nrt][p - 1]; - } - e[p - 1] = 0.0f; - - /* If required, generate U. */ - - for (j = nct; j < nu; j++) { - for (i = 0; i < m; i++) { - U[i][j] = 0.0f; - } - U[j][j] = 1.0f; - } - for (k = nct - 1; k >= 0; k--) { - if (s[k] != 0.0f) { - for (j = k + 1; j < nu; j++) { - float t = 0; - for (i = k; i < m; i++) { - t += U[i][k] * U[i][j]; - } - t = -t / U[k][k]; - for (i = k; i < m; i++) { - U[i][j] += t * U[i][k]; - } - } - for (i = k; i < m; i++) { - U[i][k] = -U[i][k]; - } - U[k][k] = 1.0f + U[k][k]; - for (i = 0; i < k - 1; i++) { - U[i][k] = 0.0f; - } - } - else { - for (i = 0; i < m; i++) { - U[i][k] = 0.0f; - } - U[k][k] = 1.0f; - } - } - - /* If required, generate V. */ - - for (k = n - 1; k >= 0; k--) { - if ((k < nrt) & (e[k] != 0.0f)) { - for (j = k + 1; j < nu; j++) { - float t = 0; - for (i = k + 1; i < n; i++) { - t += V[i][k] * V[i][j]; - } - t = -t / V[k + 1][k]; - for (i = k + 1; i < n; i++) { - V[i][j] += t * V[i][k]; - } - } - } - for (i = 0; i < n; i++) { - V[i][k] = 0.0f; - } - V[k][k] = 1.0f; - } - - /* Main iteration loop for the singular values. */ - - pp = p - 1; - iter = 0; - eps = powf(2.0f, -52.0f); - while (p > 0) { - int kase = 0; - - /* 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). */ - - for (k = p - 2; k >= -1; k--) { - if (k == -1) { - break; - } - if (fabsf(e[k]) <= eps * (fabsf(s[k]) + fabsf(s[k + 1]))) { - e[k] = 0.0f; - break; - } - } - if (k == p - 2) { - kase = 4; - } - else { - int ks; - for (ks = p - 1; ks >= k; ks--) { - float t; - if (ks == k) { - break; - } - t = (ks != p ? fabsf(e[ks]) : 0.f) + - (ks != k + 1 ? fabsf(e[ks - 1]) : 0.0f); - if (fabsf(s[ks]) <= eps * t) { - s[ks] = 0.0f; - break; - } - } - if (ks == k) { - kase = 3; - } - else if (ks == p - 1) { - kase = 1; - } - else { - kase = 2; - k = ks; - } - } - k++; - - /* Perform the task indicated by kase. */ - - switch (kase) { - - /* Deflate negligible s(p). */ - - case 1: - { - float f = e[p - 2]; - e[p - 2] = 0.0f; - for (j = p - 2; j >= k; j--) { - float t = hypotf(s[j], f); - float invt = 1.0f / t; - float cs = s[j] * invt; - float sn = f * invt; - s[j] = t; - if (j != k) { - f = -sn * e[j - 1]; - e[j - 1] = cs * e[j - 1]; - } - - for (i = 0; i < n; i++) { - t = cs * V[i][j] + sn * V[i][p - 1]; - V[i][p - 1] = -sn * V[i][j] + cs * V[i][p - 1]; - V[i][j] = t; - } - } - break; - } - - /* Split at negligible s(k). */ - - case 2: - { - float f = e[k - 1]; - e[k - 1] = 0.0f; - for (j = k; j < p; j++) { - float t = hypotf(s[j], f); - float invt = 1.0f / t; - float cs = s[j] * invt; - float sn = f * invt; - s[j] = t; - f = -sn * e[j]; - e[j] = cs * e[j]; - - for (i = 0; i < m; i++) { - t = cs * U[i][j] + sn * U[i][k - 1]; - U[i][k - 1] = -sn * U[i][j] + cs * U[i][k - 1]; - U[i][j] = t; - } - } - break; - } - - /* Perform one qr step. */ - - case 3: - { - - /* Calculate the shift. */ - - 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; - float sp = s[p - 1] * invscale; - float spm1 = s[p - 2] * invscale; - float epm1 = e[p - 2] * invscale; - float sk = s[k] * invscale; - float ek = e[k] * invscale; - float b = ((spm1 + sp) * (spm1 - sp) + epm1 * epm1) * 0.5f; - float c = (sp * epm1) * (sp * epm1); - float shift = 0.0f; - float f, g; - if ((b != 0.0f) || (c != 0.0f)) { - shift = sqrtf(b * b + c); - if (b < 0.0f) { - shift = -shift; - } - shift = c / (b + shift); - } - f = (sk + sp) * (sk - sp) + shift; - g = sk * ek; - - /* Chase zeros. */ - - for (j = k; j < p - 1; j++) { - float t = hypotf(f, g); - /* division by zero checks added to avoid NaN (brecht) */ - float cs = (t == 0.0f) ? 0.0f : f / t; - float sn = (t == 0.0f) ? 0.0f : g / t; - if (j != k) { - e[j - 1] = t; - } - f = cs * s[j] + sn * e[j]; - e[j] = cs * e[j] - sn * s[j]; - g = sn * s[j + 1]; - s[j + 1] = cs * s[j + 1]; - - for (i = 0; i < n; i++) { - t = cs * V[i][j] + sn * V[i][j + 1]; - V[i][j + 1] = -sn * V[i][j] + cs * V[i][j + 1]; - V[i][j] = t; - } - - t = hypotf(f, g); - /* division by zero checks added to avoid NaN (brecht) */ - cs = (t == 0.0f) ? 0.0f : f / t; - sn = (t == 0.0f) ? 0.0f : g / t; - s[j] = t; - f = cs * e[j] + sn * s[j + 1]; - s[j + 1] = -sn * e[j] + cs * s[j + 1]; - g = sn * e[j + 1]; - e[j + 1] = cs * e[j + 1]; - if (j < m - 1) { - for (i = 0; i < m; i++) { - t = cs * U[i][j] + sn * U[i][j + 1]; - U[i][j + 1] = -sn * U[i][j] + cs * U[i][j + 1]; - U[i][j] = t; - } - } - } - e[p - 2] = f; - iter = iter + 1; - break; - } - /* Convergence. */ - - case 4: - { - - /* Make the singular values positive. */ - - if (s[k] <= 0.0f) { - s[k] = (s[k] < 0.0f ? -s[k] : 0.0f); - - for (i = 0; i <= pp; i++) { - V[i][k] = -V[i][k]; - } - } - - /* Order the singular values. */ - - while (k < pp) { - float t; - if (s[k] >= s[k + 1]) { - break; - } - t = s[k]; - s[k] = s[k + 1]; - s[k + 1] = t; - if (k < n - 1) { - for (i = 0; i < n; i++) { - t = V[i][k + 1]; - V[i][k + 1] = V[i][k]; - V[i][k] = t; - } - } - if (k < m - 1) { - for (i = 0; i < m; i++) { - t = U[i][k + 1]; - U[i][k + 1] = U[i][k]; - U[i][k] = t; - } - } - k++; - } - iter = 0; - p--; - break; - } - } - } + float A[4][4]; + float work1[4], work2[4]; + int m = 4; + int n = 4; + int maxiter = 200; + int nu = min_ii(m, n); + + float *work = work1; + float *e = work2; + float eps; + + 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. */ + + int nct = min_ii(m - 1, n); + int nrt = max_ii(0, min_ii(n - 2, m)); + + copy_m4_m4(A, A_); + zero_m4(U); + zero_v4(s); + + for (k = 0; k < max_ii(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. */ + s[k] = 0; + for (i = k; i < m; i++) { + s[k] = hypotf(s[k], A[i][k]); + } + if (s[k] != 0.0f) { + float invsk; + if (A[k][k] < 0.0f) { + s[k] = -s[k]; + } + invsk = 1.0f / s[k]; + for (i = k; i < m; i++) { + A[i][k] *= invsk; + } + A[k][k] += 1.0f; + } + s[k] = -s[k]; + } + for (j = k + 1; j < n; j++) { + if ((k < nct) && (s[k] != 0.0f)) { + + /* Apply the transformation. */ + + float t = 0; + for (i = k; i < m; i++) { + t += A[i][k] * A[i][j]; + } + t = -t / A[k][k]; + for (i = k; i < m; i++) { + A[i][j] += t * A[i][k]; + } + } + + /* 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. */ + + 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. */ + e[k] = 0; + for (i = k + 1; i < n; i++) { + e[k] = hypotf(e[k], e[i]); + } + if (e[k] != 0.0f) { + float invek; + if (e[k + 1] < 0.0f) { + e[k] = -e[k]; + } + invek = 1.0f / e[k]; + for (i = k + 1; i < n; i++) { + e[i] *= invek; + } + e[k + 1] += 1.0f; + } + e[k] = -e[k]; + if ((k + 1 < m) & (e[k] != 0.0f)) { + float invek1; + + /* Apply the transformation. */ + + for (i = k + 1; i < m; i++) { + work[i] = 0.0f; + } + for (j = k + 1; j < n; j++) { + for (i = k + 1; i < m; i++) { + work[i] += e[j] * A[i][j]; + } + } + invek1 = 1.0f / e[k + 1]; + for (j = k + 1; j < n; j++) { + float t = -e[j] * invek1; + for (i = k + 1; i < m; i++) { + A[i][j] += t * work[i]; + } + } + } + + /* 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. */ + + p = min_ii(n, m + 1); + if (nct < n) { + s[nct] = A[nct][nct]; + } + if (m < p) { + s[p - 1] = 0.0f; + } + if (nrt + 1 < p) { + e[nrt] = A[nrt][p - 1]; + } + e[p - 1] = 0.0f; + + /* If required, generate U. */ + + for (j = nct; j < nu; j++) { + for (i = 0; i < m; i++) { + U[i][j] = 0.0f; + } + U[j][j] = 1.0f; + } + for (k = nct - 1; k >= 0; k--) { + if (s[k] != 0.0f) { + for (j = k + 1; j < nu; j++) { + float t = 0; + for (i = k; i < m; i++) { + t += U[i][k] * U[i][j]; + } + t = -t / U[k][k]; + for (i = k; i < m; i++) { + U[i][j] += t * U[i][k]; + } + } + for (i = k; i < m; i++) { + U[i][k] = -U[i][k]; + } + U[k][k] = 1.0f + U[k][k]; + for (i = 0; i < k - 1; i++) { + U[i][k] = 0.0f; + } + } + else { + for (i = 0; i < m; i++) { + U[i][k] = 0.0f; + } + U[k][k] = 1.0f; + } + } + + /* If required, generate V. */ + + for (k = n - 1; k >= 0; k--) { + if ((k < nrt) & (e[k] != 0.0f)) { + for (j = k + 1; j < nu; j++) { + float t = 0; + for (i = k + 1; i < n; i++) { + t += V[i][k] * V[i][j]; + } + t = -t / V[k + 1][k]; + for (i = k + 1; i < n; i++) { + V[i][j] += t * V[i][k]; + } + } + } + for (i = 0; i < n; i++) { + V[i][k] = 0.0f; + } + V[k][k] = 1.0f; + } + + /* Main iteration loop for the singular values. */ + + pp = p - 1; + iter = 0; + eps = powf(2.0f, -52.0f); + while (p > 0) { + int kase = 0; + + /* 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). */ + + for (k = p - 2; k >= -1; k--) { + if (k == -1) { + break; + } + if (fabsf(e[k]) <= eps * (fabsf(s[k]) + fabsf(s[k + 1]))) { + e[k] = 0.0f; + break; + } + } + if (k == p - 2) { + kase = 4; + } + else { + int ks; + for (ks = p - 1; ks >= k; ks--) { + float t; + if (ks == k) { + break; + } + t = (ks != p ? fabsf(e[ks]) : 0.f) + (ks != k + 1 ? fabsf(e[ks - 1]) : 0.0f); + if (fabsf(s[ks]) <= eps * t) { + s[ks] = 0.0f; + break; + } + } + if (ks == k) { + kase = 3; + } + else if (ks == p - 1) { + kase = 1; + } + else { + kase = 2; + k = ks; + } + } + k++; + + /* Perform the task indicated by kase. */ + + switch (kase) { + + /* Deflate negligible s(p). */ + + case 1: { + float f = e[p - 2]; + e[p - 2] = 0.0f; + for (j = p - 2; j >= k; j--) { + float t = hypotf(s[j], f); + float invt = 1.0f / t; + float cs = s[j] * invt; + float sn = f * invt; + s[j] = t; + if (j != k) { + f = -sn * e[j - 1]; + e[j - 1] = cs * e[j - 1]; + } + + for (i = 0; i < n; i++) { + t = cs * V[i][j] + sn * V[i][p - 1]; + V[i][p - 1] = -sn * V[i][j] + cs * V[i][p - 1]; + V[i][j] = t; + } + } + break; + } + + /* Split at negligible s(k). */ + + case 2: { + float f = e[k - 1]; + e[k - 1] = 0.0f; + for (j = k; j < p; j++) { + float t = hypotf(s[j], f); + float invt = 1.0f / t; + float cs = s[j] * invt; + float sn = f * invt; + s[j] = t; + f = -sn * e[j]; + e[j] = cs * e[j]; + + for (i = 0; i < m; i++) { + t = cs * U[i][j] + sn * U[i][k - 1]; + U[i][k - 1] = -sn * U[i][j] + cs * U[i][k - 1]; + U[i][j] = t; + } + } + break; + } + + /* Perform one qr step. */ + + case 3: { + + /* Calculate the shift. */ + + 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; + float sp = s[p - 1] * invscale; + float spm1 = s[p - 2] * invscale; + float epm1 = e[p - 2] * invscale; + float sk = s[k] * invscale; + float ek = e[k] * invscale; + float b = ((spm1 + sp) * (spm1 - sp) + epm1 * epm1) * 0.5f; + float c = (sp * epm1) * (sp * epm1); + float shift = 0.0f; + float f, g; + if ((b != 0.0f) || (c != 0.0f)) { + shift = sqrtf(b * b + c); + if (b < 0.0f) { + shift = -shift; + } + shift = c / (b + shift); + } + f = (sk + sp) * (sk - sp) + shift; + g = sk * ek; + + /* Chase zeros. */ + + for (j = k; j < p - 1; j++) { + float t = hypotf(f, g); + /* division by zero checks added to avoid NaN (brecht) */ + float cs = (t == 0.0f) ? 0.0f : f / t; + float sn = (t == 0.0f) ? 0.0f : g / t; + if (j != k) { + e[j - 1] = t; + } + f = cs * s[j] + sn * e[j]; + e[j] = cs * e[j] - sn * s[j]; + g = sn * s[j + 1]; + s[j + 1] = cs * s[j + 1]; + + for (i = 0; i < n; i++) { + t = cs * V[i][j] + sn * V[i][j + 1]; + V[i][j + 1] = -sn * V[i][j] + cs * V[i][j + 1]; + V[i][j] = t; + } + + t = hypotf(f, g); + /* division by zero checks added to avoid NaN (brecht) */ + cs = (t == 0.0f) ? 0.0f : f / t; + sn = (t == 0.0f) ? 0.0f : g / t; + s[j] = t; + f = cs * e[j] + sn * s[j + 1]; + s[j + 1] = -sn * e[j] + cs * s[j + 1]; + g = sn * e[j + 1]; + e[j + 1] = cs * e[j + 1]; + if (j < m - 1) { + for (i = 0; i < m; i++) { + t = cs * U[i][j] + sn * U[i][j + 1]; + U[i][j + 1] = -sn * U[i][j] + cs * U[i][j + 1]; + U[i][j] = t; + } + } + } + e[p - 2] = f; + iter = iter + 1; + break; + } + /* Convergence. */ + + case 4: { + + /* Make the singular values positive. */ + + if (s[k] <= 0.0f) { + s[k] = (s[k] < 0.0f ? -s[k] : 0.0f); + + for (i = 0; i <= pp; i++) { + V[i][k] = -V[i][k]; + } + } + + /* Order the singular values. */ + + while (k < pp) { + float t; + if (s[k] >= s[k + 1]) { + break; + } + t = s[k]; + s[k] = s[k + 1]; + s[k + 1] = t; + if (k < n - 1) { + for (i = 0; i < n; i++) { + t = V[i][k + 1]; + V[i][k + 1] = V[i][k]; + V[i][k] = t; + } + } + if (k < m - 1) { + for (i = 0; i < m; i++) { + t = U[i][k + 1]; + U[i][k + 1] = U[i][k]; + U[i][k] = t; + } + } + k++; + } + iter = 0; + p--; + break; + } + } + } } void pseudoinverse_m4_m4(float Ainv[4][4], const float A_[4][4], float epsilon) { - /* compute Moore-Penrose pseudo inverse of matrix, singular values - * below epsilon are ignored for stability (truncated SVD) */ - float A[4][4], V[4][4], W[4], Wm[4][4], U[4][4]; - int i; + /* compute Moore-Penrose pseudo inverse of matrix, singular values + * below epsilon are ignored for stability (truncated SVD) */ + float A[4][4], V[4][4], W[4], Wm[4][4], U[4][4]; + int i; - transpose_m4_m4(A, A_); - svd_m4(V, W, U, A); - transpose_m4(U); - transpose_m4(V); + transpose_m4_m4(A, A_); + svd_m4(V, W, U, A); + transpose_m4(U); + transpose_m4(V); - zero_m4(Wm); - for (i = 0; i < 4; i++) { - Wm[i][i] = (W[i] < epsilon) ? 0.0f : 1.0f / W[i]; - } + zero_m4(Wm); + for (i = 0; i < 4; i++) { + Wm[i][i] = (W[i] < epsilon) ? 0.0f : 1.0f / W[i]; + } - transpose_m4(V); + transpose_m4(V); - mul_m4_series(Ainv, U, Wm, V); + mul_m4_series(Ainv, U, Wm, V); } void pseudoinverse_m3_m3(float Ainv[3][3], const float A[3][3], float epsilon) { - /* try regular inverse when possible, otherwise fall back to slow svd */ - if (!invert_m3_m3(Ainv, A)) { - float tmp[4][4], tmpinv[4][4]; + /* try regular inverse when possible, otherwise fall back to slow svd */ + if (!invert_m3_m3(Ainv, A)) { + float tmp[4][4], tmpinv[4][4]; - copy_m4_m3(tmp, A); - pseudoinverse_m4_m4(tmpinv, tmp, epsilon); - copy_m3_m4(Ainv, tmpinv); - } + copy_m4_m3(tmp, A); + pseudoinverse_m4_m4(tmpinv, tmp, epsilon); + copy_m3_m4(Ainv, tmpinv); + } } bool has_zero_axis_m4(const float matrix[4][4]) { - return len_squared_v3(matrix[0]) < FLT_EPSILON || - len_squared_v3(matrix[1]) < FLT_EPSILON || - len_squared_v3(matrix[2]) < FLT_EPSILON; + return len_squared_v3(matrix[0]) < FLT_EPSILON || len_squared_v3(matrix[1]) < FLT_EPSILON || + len_squared_v3(matrix[2]) < FLT_EPSILON; } void invert_m4_m4_safe(float Ainv[4][4], const float A[4][4]) { - if (!invert_m4_m4(Ainv, A)) { - float Atemp[4][4]; + if (!invert_m4_m4(Ainv, A)) { + float Atemp[4][4]; - copy_m4_m4(Atemp, A); + copy_m4_m4(Atemp, A); - /* Matrix is degenerate (e.g. 0 scale on some axis), ideally we should - * never be in this situation, but try to invert it anyway with tweak. - */ - Atemp[0][0] += 1e-8f; - Atemp[1][1] += 1e-8f; - Atemp[2][2] += 1e-8f; + /* Matrix is degenerate (e.g. 0 scale on some axis), ideally we should + * never be in this situation, but try to invert it anyway with tweak. + */ + Atemp[0][0] += 1e-8f; + Atemp[1][1] += 1e-8f; + Atemp[2][2] += 1e-8f; - if (!invert_m4_m4(Ainv, Atemp)) { - unit_m4(Ainv); - } - } + if (!invert_m4_m4(Ainv, Atemp)) { + unit_m4(Ainv); + } + } } /** @@ -2617,12 +2645,14 @@ void invert_m4_m4_safe(float Ainv[4][4], const float A[4][4]) * this defines a transform matrix TM such that (x', y', z') = TM * (x, y, z) * where (x', y', z') are the coordinates of P' in target space such that it keeps (X, Y, Z) coordinates in global space. */ -void BLI_space_transform_from_matrices(SpaceTransform *data, const float local[4][4], const float target[4][4]) +void BLI_space_transform_from_matrices(SpaceTransform *data, + const float local[4][4], + const float target[4][4]) { - float itarget[4][4]; - invert_m4_m4(itarget, target); - mul_m4_m4m4(data->local2target, itarget, local); - invert_m4_m4(data->target2local, data->local2target); + float itarget[4][4]; + invert_m4_m4(itarget, target); + mul_m4_m4m4(data->local2target, itarget, local); + invert_m4_m4(data->target2local, data->local2target); } /** @@ -2635,32 +2665,34 @@ void BLI_space_transform_from_matrices(SpaceTransform *data, const float local[4 * this defines a transform matrix TM such that (X', Y', Z') = TM * (X, Y, Z) * where (X', Y', Z') are the coordinates of p' in global space such that it keeps (x, y, z) coordinates in target space. */ -void BLI_space_transform_global_from_matrices(SpaceTransform *data, const float local[4][4], const float target[4][4]) +void BLI_space_transform_global_from_matrices(SpaceTransform *data, + const float local[4][4], + const float target[4][4]) { - float ilocal[4][4]; - invert_m4_m4(ilocal, local); - mul_m4_m4m4(data->local2target, target, ilocal); - invert_m4_m4(data->target2local, data->local2target); + float ilocal[4][4]; + invert_m4_m4(ilocal, local); + mul_m4_m4m4(data->local2target, target, ilocal); + invert_m4_m4(data->target2local, data->local2target); } void BLI_space_transform_apply(const SpaceTransform *data, float co[3]) { - mul_v3_m4v3(co, ((SpaceTransform *)data)->local2target, co); + mul_v3_m4v3(co, ((SpaceTransform *)data)->local2target, co); } void BLI_space_transform_invert(const SpaceTransform *data, float co[3]) { - mul_v3_m4v3(co, ((SpaceTransform *)data)->target2local, co); + mul_v3_m4v3(co, ((SpaceTransform *)data)->target2local, co); } void BLI_space_transform_apply_normal(const SpaceTransform *data, float no[3]) { - mul_mat3_m4_v3(((SpaceTransform *)data)->local2target, no); - normalize_v3(no); + mul_mat3_m4_v3(((SpaceTransform *)data)->local2target, no); + normalize_v3(no); } void BLI_space_transform_invert_normal(const SpaceTransform *data, float no[3]) { - mul_mat3_m4_v3(((SpaceTransform *)data)->target2local, no); - normalize_v3(no); + mul_mat3_m4_v3(((SpaceTransform *)data)->target2local, no); + normalize_v3(no); } |