diff options
Diffstat (limited to 'source/blender/blenlib')
-rw-r--r-- | source/blender/blenlib/BLI_math_matrix.h | 3 | ||||
-rw-r--r-- | source/blender/blenlib/BLI_quadric.h | 2 | ||||
-rw-r--r-- | source/blender/blenlib/intern/math_matrix.c | 62 | ||||
-rw-r--r-- | source/blender/blenlib/intern/quadric.c | 9 |
4 files changed, 59 insertions, 17 deletions
diff --git a/source/blender/blenlib/BLI_math_matrix.h b/source/blender/blenlib/BLI_math_matrix.h index 9e34631d460..0783a7981ea 100644 --- a/source/blender/blenlib/BLI_math_matrix.h +++ b/source/blender/blenlib/BLI_math_matrix.h @@ -98,6 +98,9 @@ void mul_m3_fl(float R[3][3], float f); void mul_m4_fl(float R[4][4], float f); void mul_mat3_m4_fl(float R[4][4], float f); +int invert_m3_ex(float m[3][3], const float epsilon); +int invert_m3_m3_ex(float m1[3][3], float m2[3][3], const float epsilon); + int invert_m3(float R[3][3]); int invert_m3_m3(float R[3][3], float A[3][3]); int invert_m4(float R[4][4]); diff --git a/source/blender/blenlib/BLI_quadric.h b/source/blender/blenlib/BLI_quadric.h index aec11ec2b44..e71a6473852 100644 --- a/source/blender/blenlib/BLI_quadric.h +++ b/source/blender/blenlib/BLI_quadric.h @@ -51,6 +51,6 @@ void BLI_quadric_mul(Quadric *a, const float scalar); /* solve */ float BLI_quadric_evaluate(const Quadric *q, const float v[3]); -int BLI_quadric_optimize(const Quadric *q, float v[3]); +int BLI_quadric_optimize(const Quadric *q, float v[3], const float epsilon); #endif /* __BLI_QUADRIC_H__ */ diff --git a/source/blender/blenlib/intern/math_matrix.c b/source/blender/blenlib/intern/math_matrix.c index f622a5ace35..38214f9c6b0 100644 --- a/source/blender/blenlib/intern/math_matrix.c +++ b/source/blender/blenlib/intern/math_matrix.c @@ -504,6 +504,51 @@ void sub_m4_m4m4(float m1[][4], float m2[][4], float m3[][4]) m1[i][j] = m2[i][j] - m3[i][j]; } +/* why not make this a standard part of the API? */ +static float determinant_m3_local(float m[3][3]) +{ + return (m[0][0] * (m[1][1] * m[2][2] - m[1][2] * m[2][1]) - + m[1][0] * (m[0][1] * m[2][2] - m[0][2] * m[2][1]) + + m[2][0] * (m[0][1] * m[1][2] - m[0][2] * m[1][1])); +} + +int invert_m3_ex(float m[3][3], const float epsilon) +{ + float tmp[3][3]; + int success; + + success = invert_m3_m3_ex(tmp, m, epsilon); + copy_m3_m3(m, tmp); + + return success; +} + +int invert_m3_m3_ex(float m1[3][3], float m2[3][3], const float epsilon) +{ + float det; + int a, b, success; + + BLI_assert(epsilon >= 0.0f); + + /* calc adjoint */ + adjoint_m3_m3(m1, m2); + + /* then determinant old matrix! */ + det = determinant_m3_local(m2); + + success = (fabsf(det) > epsilon); + + if (LIKELY(det != 0.0f)) { + det = 1.0f / det; + for (a = 0; a < 3; a++) { + for (b = 0; b < 3; b++) { + m1[a][b] *= det; + } + } + } + return success; +} + int invert_m3(float m[3][3]) { float tmp[3][3]; @@ -524,17 +569,16 @@ int invert_m3_m3(float m1[3][3], float m2[3][3]) adjoint_m3_m3(m1, m2); /* then determinant old matrix! */ - det = (m2[0][0] * (m2[1][1] * m2[2][2] - m2[1][2] * m2[2][1]) - - m2[1][0] * (m2[0][1] * m2[2][2] - m2[0][2] * m2[2][1]) + - m2[2][0] * (m2[0][1] * m2[1][2] - m2[0][2] * m2[1][1])); + det = determinant_m3_local(m2); - success = (det != 0); + success = (det != 0.0f); - if (det == 0) det = 1; - det = 1 / det; - for (a = 0; a < 3; a++) { - for (b = 0; b < 3; b++) { - m1[a][b] *= det; + if (LIKELY(det != 0.0f)) { + det = 1.0f / det; + for (a = 0; a < 3; a++) { + for (b = 0; b < 3; b++) { + m1[a][b] *= det; + } } } diff --git a/source/blender/blenlib/intern/quadric.c b/source/blender/blenlib/intern/quadric.c index bb39cb61e78..b06534d282a 100644 --- a/source/blender/blenlib/intern/quadric.c +++ b/source/blender/blenlib/intern/quadric.c @@ -107,18 +107,13 @@ float BLI_quadric_evaluate(const Quadric *q, const float v[3]) q->d2); } -int BLI_quadric_optimize(const Quadric *q, float v[3]) +int BLI_quadric_optimize(const Quadric *q, float v[3], const float epsilon) { float m[3][3]; - float det; BLI_quadric_to_tensor_m3(q, m); - det = determinant_m3(m[0][0], m[0][1], m[0][2], - m[1][0], m[1][1], m[1][2], - m[2][0], m[2][1], m[2][2]); - if (fabsf(det) > FLT_EPSILON) { - invert_m3(m); + if (invert_m3_ex(m, epsilon)) { BLI_quadric_to_vector_v3(q, v); mul_m3_v3(m, v); negate_v3(v); |