diff --git a/src/Base/Rotation.cpp b/src/Base/Rotation.cpp index 6c378ecf71..9cf9815f0a 100644 --- a/src/Base/Rotation.cpp +++ b/src/Base/Rotation.cpp @@ -398,11 +398,19 @@ bool Rotation::isSame(const Rotation& q) const bool Rotation::isSame(const Rotation& q, double tol) const { - Vector3d v(0,0,1); - return std::fabs(multVec(v).GetAngle(q.multVec(v))) < tol; + // This follows the implementation of Coin3d where the norm + // (x1-y1)**2 + ... + (x4-y4)**2 is computed. + // This term can be simplified to + // 2 - 2*(x1*y1 + ... + x4*y4) so that for the equality we have to check + // 1 - tol/2 <= x1*y1 + ... + x4*y4 + // Because a quaternion (x1,x2,x3,x4) is equal to (-x1,-x2,-x3,-x4) we use the + // absolute value of the scalar product + double dot = q.quat[0]*quat[0]+q.quat[1]*quat[1]+q.quat[2]*quat[2]+q.quat[3]*quat[3]; + return fabs(dot) >= 1.0 - tol/2; } -Vector3d Rotation::multVec(const Vector3d & src) const { +Vector3d Rotation::multVec(const Vector3d & src) const +{ Vector3d dst; multVec(src,dst); return dst; diff --git a/src/Base/RotationPy.xml b/src/Base/RotationPy.xml index 4106d55762..a33cde5035 100644 --- a/src/Base/RotationPy.xml +++ b/src/Base/RotationPy.xml @@ -50,8 +50,9 @@ - isSame(Rotation) - Checks if the two quaternions perform the same rotation + isSame(Rotation, [tolerance=0]) + Checks if the two quaternions perform the same rotation. + Optionally, a tolerance value greater than zero can be passed. diff --git a/src/Base/RotationPyImp.cpp b/src/Base/RotationPyImp.cpp index 06c358507a..8611010ddd 100644 --- a/src/Base/RotationPyImp.cpp +++ b/src/Base/RotationPyImp.cpp @@ -273,11 +273,12 @@ PyObject* RotationPy::toEuler(PyObject * args) PyObject* RotationPy::isSame(PyObject *args) { PyObject *rot; - if (!PyArg_ParseTuple(args, "O!", &(RotationPy::Type), &rot)) + double tol = 0.0; + if (!PyArg_ParseTuple(args, "O!|d", &(RotationPy::Type), &rot, &tol)) return NULL; Base::Rotation rot1 = * getRotationPtr(); Base::Rotation rot2 = * static_cast(rot)->getRotationPtr(); - bool same = rot1.isSame(rot2); + bool same = tol > 0.0 ? rot1.isSame(rot2, tol) : rot1.isSame(rot2); return Py_BuildValue("O", (same ? Py_True : Py_False)); }