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));
}