diff --git a/src/Base/Placement.cpp b/src/Base/Placement.cpp
index 50e261c982..6cf542d5cc 100644
--- a/src/Base/Placement.cpp
+++ b/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) &&
diff --git a/src/Base/Placement.h b/src/Base/Placement.h
index 7184046079..f4c5b9d76d 100644
--- a/src/Base/Placement.h
+++ b/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);
diff --git a/src/Base/PlacementPy.xml b/src/Base/PlacementPy.xml
index 58560ff021..83c188c9b6 100644
--- a/src/Base/PlacementPy.xml
+++ b/src/Base/PlacementPy.xml
@@ -144,9 +144,11 @@ t : float\n Parameter of the path. t=0 returns this placement, t=1 returns `p
- isIdentity() -> bool\n
+ isIdentity([tol=0.0]) -> bool\n
Returns True if the placement has no displacement and no rotation.
-Matrix representation is the 4D identity matrix.
+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.
diff --git a/src/Base/PlacementPyImp.cpp b/src/Base/PlacementPyImp.cpp
index 9de81175b3..fa6d77031c 100644
--- a/src/Base/PlacementPyImp.cpp
+++ b/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));
}
diff --git a/src/Base/Rotation.cpp b/src/Base/Rotation.cpp
index 550c3519bc..ef1d491c9e 100644
--- a/src/Base/Rotation.cpp
+++ b/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 &&
diff --git a/src/Base/Rotation.h b/src/Base/Rotation.h
index 990469e788..55b05ab41d 100644
--- a/src/Base/Rotation.h
+++ b/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 */
diff --git a/src/Base/RotationPy.xml b/src/Base/RotationPy.xml
index 120bd69221..5afd250195 100644
--- a/src/Base/RotationPy.xml
+++ b/src/Base/RotationPy.xml
@@ -160,8 +160,10 @@ Returns True if all values in the quaternion representation are zero.
- isIdentity() -> bool\n
-Returns True if the rotation equals the 4D identity matrix.
+ 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.
diff --git a/src/Base/RotationPyImp.cpp b/src/Base/RotationPyImp.cpp
index 32b200fe1e..ece14131a7 100644
--- a/src/Base/RotationPyImp.cpp
+++ b/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));
}