Base: add overloaded method isIdentity() to Rotation and Placement that accepts a tolerance

src/Base/Placement.cpp

@@ -97,6 +97,11 @@ bool Placement::isIdentity() const
     return none;
 }
 
+bool Placement::isIdentity(double tol) const
+{
+    return isSame(Placement(), tol);
+}
+
 bool Placement::isSame(const Placement& p) const
 {
     return this->_rot.isSame(p._rot) &&

src/Base/Placement.h

@@ -62,6 +62,7 @@ public:
     void setRotation(const Rotation& Rot) {_rot = Rot;}
 
     bool isIdentity() const;
+    bool isIdentity(double tol) const;
     void invert();
     Placement inverse() const;
     void move(const Vector3d& MovVec);

src/Base/PlacementPy.xml

@@ -144,9 +144,11 @@ t : float\n    Parameter of the path. t=0 returns this placement, t=1 returns `p
         </Methode>
         <Methode Name="isIdentity" Const="true">
             <Documentation>
-                <UserDocu>isIdentity() -> bool\n
+                <UserDocu>isIdentity([tol=0.0]) -> bool\n
 Returns True if the placement has no displacement and no rotation.
-Matrix representation is the 4D identity matrix.</UserDocu>
+Matrix representation is the 4D identity matrix.
+tol : float\n    Tolerance used to check for identity.
+    If tol is negative or zero, no tolerance is used.</UserDocu>
             </Documentation>
         </Methode>
         <Methode Name="isSame" Const="true">

src/Base/PlacementPyImp.cpp

@@ -275,9 +275,11 @@ PyObject* PlacementPy::slerp(PyObject* args)
 
 PyObject* PlacementPy::isIdentity(PyObject *args)
 {
-    if (!PyArg_ParseTuple(args, ""))
+    double tol = 0.0;
+    if (!PyArg_ParseTuple(args, "|d", &tol))
         return nullptr;
-    bool none = getPlacementPtr()->isIdentity();
+    bool none = tol > 0 ? getPlacementPtr()->isIdentity(tol)
+                        : getPlacementPtr()->isIdentity();
     return Py_BuildValue("O", (none ? Py_True : Py_False));
 }
 

src/Base/Rotation.cpp

@@ -430,33 +430,6 @@ bool Rotation::operator!=(const Rotation & q) const
     return !(*this == q);
 }
 
-bool Rotation::isSame(const Rotation& q) const
-{
-    if ((this->quat[0] == q.quat[0] &&
-         this->quat[1] == q.quat[1] &&
-         this->quat[2] == q.quat[2] &&
-         this->quat[3] == q.quat[3]) ||
-        (this->quat[0] == -q.quat[0] &&
-         this->quat[1] == -q.quat[1] &&
-         this->quat[2] == -q.quat[2] &&
-         this->quat[3] == -q.quat[3]))
-        return true;
-    return false;
-}
-
-bool Rotation::isSame(const Rotation& q, double tol) const
-{
-    // 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 dst;
@@ -758,6 +731,33 @@ void Rotation::getYawPitchRoll(double& y, double& p, double& r) const
     r = (r/D_PI)*180;
 }
 
+bool Rotation::isSame(const Rotation& q) const
+{
+    if ((this->quat[0] == q.quat[0] &&
+         this->quat[1] == q.quat[1] &&
+         this->quat[2] == q.quat[2] &&
+         this->quat[3] == q.quat[3]) ||
+        (this->quat[0] == -q.quat[0] &&
+         this->quat[1] == -q.quat[1] &&
+         this->quat[2] == -q.quat[2] &&
+         this->quat[3] == -q.quat[3]))
+        return true;
+    return false;
+}
+
+bool Rotation::isSame(const Rotation& q, double tol) const
+{
+    // 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;
+}
+
 bool Rotation::isIdentity() const
 {
     return ((this->quat[0] == 0.0  &&
@@ -767,6 +767,11 @@ bool Rotation::isIdentity() const
              this->quat[3] == -1.0));
 }
 
+bool Rotation::isIdentity(double tol) const
+{
+    return isSame(Rotation(), tol);
+}
+
 bool Rotation::isNull() const
 {
     return (this->quat[0] == 0.0 &&

src/Base/Rotation.h

@@ -114,7 +114,10 @@ public:
     void getEulerAngles(EulerSequence seq, double &alpha, double &beta, double &gamma) const;
     void setEulerAngles(EulerSequence seq, double alpha, double beta, double gamma);
     bool isIdentity() const;
+    bool isIdentity(double tol) const;
     bool isNull() const;
+    bool isSame(const Rotation&) const;
+    bool isSame(const Rotation&, double tol) const;
     //@}
 
     /** Invert rotations. */
@@ -141,8 +144,6 @@ public:
     void multVec(const Vector3f & src, Vector3f & dst) const;
     Vector3f multVec(const Vector3f & src) const;
     void scaleAngle(const double scaleFactor);
-    bool isSame(const Rotation&) const;
-    bool isSame(const Rotation&, double tol) const;
     //@}
 
     /** Specialty constructors */

src/Base/RotationPy.xml

@@ -160,8 +160,10 @@ Returns True if all values in the quaternion representation are zero.</UserDocu>
         </Methode>
         <Methode Name="isIdentity" Const="true">
             <Documentation>
-                <UserDocu>isIdentity() -> bool\n
-Returns True if the rotation equals the 4D identity matrix.</UserDocu>
+                <UserDocu>isIdentity(tol=0) -> bool\n
+Returns True if the rotation equals the 4D identity matrix.
+tol : float\n    Tolerance used to check for identity.
+    If tol is negative or zero, no tolerance is used.</UserDocu>
             </Documentation>
         </Methode>
         <Attribute Name="Q" ReadOnly="false">

src/Base/RotationPyImp.cpp

@@ -377,9 +377,11 @@ PyObject* RotationPy::isSame(PyObject *args)
 
 PyObject* RotationPy::isIdentity(PyObject *args)
 {
-    if (!PyArg_ParseTuple(args, ""))
+    double tol = 0.0;
+    if (!PyArg_ParseTuple(args, "|d", &tol))
         return nullptr;
-    bool null = getRotationPtr()->isIdentity();
+    bool null = tol > 0.0 ? getRotationPtr()->isIdentity(tol)
+                          : getRotationPtr()->isIdentity();
     return Py_BuildValue("O", (null ? Py_True : Py_False));
 }