fix implementation of Rotation::isSame(const Rotation& q, double tol) and extend Python bindings

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;

src/Base/RotationPy.xml

@@ -50,8 +50,9 @@
         <Methode Name="isSame">
             <Documentation>
                 <UserDocu>
-                    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.
                 </UserDocu>
             </Documentation>
         </Methode>

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<RotationPy*>(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));
 }