From e44c49d89d67038657cdcbd373e64b9a17b6c993 Mon Sep 17 00:00:00 2001 From: Bastien Montagne Date: Sat, 6 Sep 2014 14:54:08 +0200 Subject: Py Mathutils: add `invert_safe()` and `inverted_safe()` to `Matrix`. Those two mimic our BLI invert_m4_m4_safe - they add a small offset to diagonal values, in case org matrix is degenerated, and if still non-invertible, return identity matrix. Org patch by me, final enhanced version by ideasman42, many thanks! --- source/blender/python/mathutils/mathutils_Matrix.c | 215 ++++++++++++++++----- source/blender/python/mathutils/mathutils_Matrix.h | 1 + tests/python/bl_pyapi_mathutils.py | 23 +++ 3 files changed, 191 insertions(+), 48 deletions(-) diff --git a/source/blender/python/mathutils/mathutils_Matrix.c b/source/blender/python/mathutils/mathutils_Matrix.c index 09db282d956..675234fe3e4 100644 --- a/source/blender/python/mathutils/mathutils_Matrix.c +++ b/source/blender/python/mathutils/mathutils_Matrix.c @@ -910,56 +910,141 @@ static float matrix_determinant_internal(const MatrixObject *self) } } +static void adjoint_matrix_n(float *mat_dst, const float *mat_src, const unsigned short dim) +{ + /* calculate the classical adjoint */ + switch (dim) { + case 2: + { + adjoint_m2_m2((float (*)[2])mat_dst, (float (*)[2])mat_src); + break; + } + case 3: + { + adjoint_m3_m3((float (*)[3])mat_dst, (float (*)[3])mat_src); + break; + } + case 4: + { + adjoint_m4_m4((float (*)[4])mat_dst, (float (*)[4])mat_src); + break; + } + default: + BLI_assert(0); + } +} + +static void matrix_invert_with_det_n_internal(float *mat_dst, const float *mat_src, const float det, const unsigned short dim) +{ + float mat[MATRIX_MAX_DIM * MATRIX_MAX_DIM]; + unsigned short i, j, k; + + BLI_assert(det != 0.0f); + + adjoint_matrix_n(mat, mat_src, dim); + + /* divide by determinant & set values */ + k = 0; + for (i = 0; i < dim; i++) { /* num_col */ + for (j = 0; j < dim; j++) { /* num_row */ + mat_dst[MATRIX_ITEM_INDEX_NUMROW(dim, j, i)] = mat[k++] / det; + } + } +} + /** - * \param r_mat can be from ``self->matrix`` or not. */ + * \param r_mat can be from ``self->matrix`` or not. + */ static bool matrix_invert_internal(const MatrixObject *self, float *r_mat) { float det; - + BLI_assert(self->num_col == self->num_row); det = matrix_determinant_internal(self); if (det != 0.0f) { - float mat[MATRIX_MAX_DIM * MATRIX_MAX_DIM]; - int x, y, z; + matrix_invert_with_det_n_internal(r_mat, self->matrix, det, self->num_col); + return true; + } + else { + return false; + } +} + +/** + * Similar to ``matrix_invert_internal`` but should never error. + * \param r_mat can be from ``self->matrix`` or not. + */ +static void matrix_invert_safe_internal(const MatrixObject *self, float *r_mat) +{ + float det; + float *in_mat = self->matrix; + BLI_assert(self->num_col == self->num_row); + det = matrix_determinant_internal(self); + + if (det == 0.0f) { + const float eps = PSEUDOINVERSE_EPSILON; + + /* We will copy self->matrix into r_mat (if needed), and modify it in place to add diagonal epsilon. */ + in_mat = r_mat; - /* calculate the classical adjoint */ switch (self->num_col) { case 2: { - adjoint_m2_m2((float (*)[2])mat, (float (*)[2])self->matrix); + float (*mat)[2] = (float (*)[2])in_mat; + + if (in_mat != self->matrix) { + copy_m2_m2(mat, (float (*)[2])self->matrix); + } + mat[0][0] += eps; + mat[1][1] += eps; + + if (UNLIKELY((det = determinant_m2(mat[0][0], mat[0][1], mat[1][0], mat[1][1])) == 0.0f)) { + unit_m2(mat); + det = 1.0f; + } break; } case 3: { - adjoint_m3_m3((float (*)[3])mat, (float (*)[3])self->matrix); + float (*mat)[3] = (float (*)[3])in_mat; + + if (in_mat != self->matrix) { + copy_m3_m3(mat, (float (*)[3])self->matrix); + } + mat[0][0] += eps; + mat[1][1] += eps; + mat[2][2] += eps; + + if (UNLIKELY((det = determinant_m3_array(mat)) == 0.0f)) { + unit_m3(mat); + det = 1.0f; + } break; } case 4: { - adjoint_m4_m4((float (*)[4])mat, (float (*)[4])self->matrix); + float (*mat)[4] = (float (*)[4])in_mat; + + if (in_mat != self->matrix) { + copy_m4_m4(mat, (float (*)[4])self->matrix); + } + mat[0][0] += eps; + mat[1][1] += eps; + mat[2][2] += eps; + mat[3][3] += eps; + + if (UNLIKELY(det = determinant_m4(mat)) == 0.0f) { + unit_m4(mat); + det = 1.0f; + } break; } default: BLI_assert(0); } - /* divide by determinate */ - for (x = 0; x < (self->num_col * self->num_row); x++) { - mat[x] /= det; - } - /* set values */ - z = 0; - for (x = 0; x < self->num_col; x++) { - for (y = 0; y < self->num_row; y++) { - r_mat[MATRIX_ITEM_INDEX(self, y, x)] = mat[z]; - z++; - } - } - - return true; - } - else { - return false; } + + matrix_invert_with_det_n_internal(r_mat, in_mat, det, self->num_col); } @@ -1398,6 +1483,56 @@ static PyObject *Matrix_inverted_noargs(MatrixObject *self) Py_RETURN_NONE; } +PyDoc_STRVAR(Matrix_invert_safe_doc, +".. method:: invert_safe()\n" +"\n" +" Set the matrix to its inverse, will never error.\n" +" If degenerated (e.g. zero scale on an axis), add some epsilon to its diagonal, to get an invertible one.\n" +" If tweaked matrix is still degenerated, set to the identity matrix instead.\n" +"\n" +" .. seealso:: \n" +); +static PyObject *Matrix_invert_safe(MatrixObject *self) +{ + if (BaseMath_ReadCallback(self) == -1) + return NULL; + + if (matrix_invert_is_compat(self) == false) { + return NULL; + } + + matrix_invert_safe_internal(self, self->matrix); + + (void)BaseMath_WriteCallback(self); + Py_RETURN_NONE; +} + +PyDoc_STRVAR(Matrix_inverted_safe_doc, +".. method:: inverted_safe()\n" +"\n" +" Return an inverted copy of the matrix, will never error.\n" +" If degenerated (e.g. zero scale on an axis), add some epsilon to its diagonal, to get an invertible one.\n" +" If tweaked matrix is still degenerated, return the identity matrix instead.\n" +"\n" +" :return: the inverted matrix.\n" +" :rtype: :class:`Matrix`\n" +); +static PyObject *Matrix_inverted_safe(MatrixObject *self) +{ + float mat[MATRIX_MAX_DIM * MATRIX_MAX_DIM]; + + if (BaseMath_ReadCallback(self) == -1) + return NULL; + + if (matrix_invert_is_compat(self) == false) { + return NULL; + } + + matrix_invert_safe_internal(self, mat); + + return Matrix_copy_notest(self, mat); +} + /*---------------------------matrix.adjugate() ---------------------*/ PyDoc_STRVAR(Matrix_adjugate_doc, ".. method:: adjugate()\n" @@ -1421,35 +1556,17 @@ static PyObject *Matrix_adjugate(MatrixObject *self) } /* calculate the classical adjoint */ - switch (self->num_col) { - case 2: - { - float mat[2][2]; - adjoint_m2_m2(mat, (float (*)[2])self->matrix); - copy_v4_v4((float *)self->matrix, (float *)mat); - break; - } - case 3: - { - float mat[3][3]; - adjoint_m3_m3(mat, (float (*)[3])self->matrix); - copy_m3_m3((float (*)[3])self->matrix, mat); - break; - } - case 4: - { - float mat[4][4]; - adjoint_m4_m4(mat, (float (*)[4])self->matrix); - copy_m4_m4((float (*)[4])self->matrix, mat); - break; - } - default: + if (self->num_col <= 4) { + adjoint_matrix_n(self->matrix, self->matrix, self->num_col); + } + else { PyErr_Format(PyExc_ValueError, "Matrix adjugate(d): size (%d) unsupported", (int)self->num_col); return NULL; } + (void)BaseMath_WriteCallback(self); Py_RETURN_NONE; } @@ -2556,6 +2673,8 @@ static struct PyMethodDef Matrix_methods[] = { {"normalized", (PyCFunction) Matrix_normalized, METH_NOARGS, Matrix_normalized_doc}, {"invert", (PyCFunction) Matrix_invert, METH_VARARGS, Matrix_invert_doc}, {"inverted", (PyCFunction) Matrix_inverted, METH_VARARGS, Matrix_inverted_doc}, + {"invert_safe", (PyCFunction) Matrix_invert_safe, METH_NOARGS, Matrix_invert_safe_doc}, + {"inverted_safe", (PyCFunction) Matrix_inverted_safe, METH_NOARGS, Matrix_inverted_safe_doc}, {"adjugate", (PyCFunction) Matrix_adjugate, METH_NOARGS, Matrix_adjugate_doc}, {"adjugated", (PyCFunction) Matrix_adjugated, METH_NOARGS, Matrix_adjugated_doc}, {"to_3x3", (PyCFunction) Matrix_to_3x3, METH_NOARGS, Matrix_to_3x3_doc}, diff --git a/source/blender/python/mathutils/mathutils_Matrix.h b/source/blender/python/mathutils/mathutils_Matrix.h index c7fb23d8776..f94af9e540e 100644 --- a/source/blender/python/mathutils/mathutils_Matrix.h +++ b/source/blender/python/mathutils/mathutils_Matrix.h @@ -41,6 +41,7 @@ extern PyTypeObject matrix_access_Type; # define MATRIX_ITEM_ASSERT(_mat, _row, _col) (void)0 #endif +#define MATRIX_ITEM_INDEX_NUMROW(_totrow, _row, _col) ((_totrow * (_col)) + (_row)) #define MATRIX_ITEM_INDEX(_mat, _row, _col) (MATRIX_ITEM_ASSERT(_mat, _row, _col),(((_mat)->num_row * (_col)) + (_row))) #define MATRIX_ITEM_PTR( _mat, _row, _col) ((_mat)->matrix + MATRIX_ITEM_INDEX(_mat, _row, _col)) #define MATRIX_ITEM( _mat, _row, _col) ((_mat)->matrix [MATRIX_ITEM_INDEX(_mat, _row, _col)]) diff --git a/tests/python/bl_pyapi_mathutils.py b/tests/python/bl_pyapi_mathutils.py index 7354f559b95..85232e465d7 100644 --- a/tests/python/bl_pyapi_mathutils.py +++ b/tests/python/bl_pyapi_mathutils.py @@ -162,6 +162,29 @@ class MatrixTesting(unittest.TestCase): self.assertEqual(mat.inverted(), inv_mat) + def test_matrix_inverse_safe(self): + mat = Matrix(((1, 4, 0, -1), + (2, -1, 0, -2), + (0, 3, 0, 3), + (-2, 9, 0, 0))) + + # Warning, if we change epsilon in py api we have to update this!!! + epsilon = 1e-8 + inv_mat_safe = mat.copy() + inv_mat_safe[0][0] += epsilon + inv_mat_safe[1][1] += epsilon + inv_mat_safe[2][2] += epsilon + inv_mat_safe[3][3] += epsilon + inv_mat_safe.invert() + ''' + inv_mat_safe = Matrix(((1.0, -0.5, 0.0, -0.5), + (0.222222, -0.111111, -0.0, 0.0), + (-333333344.0, 316666656.0, 100000000.0, 150000000.0), + (0.888888, -0.9444444, 0.0, -0.5))) + ''' + + self.assertEqual(mat.inverted_safe(), inv_mat_safe) + def test_matrix_mult(self): mat = Matrix(((1, 4, 0, -1), (2, -1, 2, -2), -- cgit v1.2.3