diff --git a/src/Base/MatrixPy.xml b/src/Base/MatrixPy.xml index c91da2aa2d..23b5b15e4c 100644 --- a/src/Base/MatrixPy.xml +++ b/src/Base/MatrixPy.xml @@ -110,9 +110,10 @@ Compute the determinant of the matrix -isOrthogonal() -> Float +isOrthogonal([Float]) -> Float Checks if the matrix is orthogonal, i.e. M * M^T = k*I and returns the multiple of the identity matrix. If it's not orthogonal 0 is returned. +As argument you can set a tolerance which by default is 1.0e-6. diff --git a/src/Base/MatrixPyImp.cpp b/src/Base/MatrixPyImp.cpp index 9186f15167..b4f37b89ae 100644 --- a/src/Base/MatrixPyImp.cpp +++ b/src/Base/MatrixPyImp.cpp @@ -24,6 +24,7 @@ #include "PreCompiled.h" #include +#include #include "Base/Matrix.h" // inclusion of the generated files (generated out of MatrixPy.xml) @@ -415,32 +416,34 @@ PyObject* MatrixPy::submatrix(PyObject * args) PyObject* MatrixPy::isOrthogonal(PyObject * args) { - if (!PyArg_ParseTuple(args, "")) + double eps=1.0e-06; + if (!PyArg_ParseTuple(args, "|d",&eps)) return 0; const Base::Matrix4D& mat = *getMatrixPtr(); Base::Matrix4D trp = mat; trp.transpose(); trp = trp * mat; + bool ok = true; double mult = trp[0][0]; - for (int i=0; (i<4) && (mult!=0.0); i++) { - for (int j=0; (j<4) && (mult!=0.0); j++) { + for (int i=0; i<4 && ok; i++) { + for (int j=0; j<4 && ok; j++) { if (i != j) { - if (trp[i][j] != 0.0) { - mult = 0.0; + if (fabs(trp[i][j]) > eps) { + ok = false; break; } } - else { // the diagonal - if (trp[i][j] != mult) { - mult = 0.0; + else { // the main diagonal + if (fabs(trp[i][j]-mult) > eps) { + ok = false; break; } } } } - return Py::new_reference_to(Py::Float(mult)); + return Py::new_reference_to(Py::Float(ok ? mult : 0.0)); } PyObject* MatrixPy::transposed(PyObject * args)