[Bf-blender-cvs] [e44c49d] master: Py Mathutils: add `invert_safe()` and `inverted_safe()` to `Matrix`.
Bastien Montagne
noreply at git.blender.org
Sat Sep 6 15:01:06 CEST 2014
Commit: e44c49d89d67038657cdcbd373e64b9a17b6c993
Author: Bastien Montagne
Date: Sat Sep 6 14:54:08 2014 +0200
Branches: master
https://developer.blender.org/rBe44c49d89d67038657cdcbd373e64b9a17b6c993
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!
===================================================================
M source/blender/python/mathutils/mathutils_Matrix.c
M source/blender/python/mathutils/mathutils_Matrix.h
M tests/python/bl_pyapi_mathutils.py
===================================================================
diff --git a/source/blender/python/mathutils/mathutils_Matrix.c b/source/blender/python/mathutils/mathutils_Matrix.c
index 09db282..675234f 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:: <http://en.wikipedia.org/wiki/Inverse_matrix>\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 c7fb23d..f94af9e 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 7354f55..85232e4 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),
More information about the Bf-blender-cvs
mailing list