From f039789d0b91cd535cfeb5af88a3ba555370b982 Mon Sep 17 00:00:00 2001 From: wmayer Date: Tue, 3 Jan 2023 10:54:47 +0100 Subject: [PATCH] Base: add overloaded method isIdentity() to Rotation and Placement that accepts a tolerance --- src/Base/Placement.cpp | 5 ++++ src/Base/Placement.h | 1 + src/Base/PlacementPy.xml | 6 ++-- src/Base/PlacementPyImp.cpp | 6 ++-- src/Base/Rotation.cpp | 59 ++++++++++++++++++++----------------- src/Base/Rotation.h | 5 ++-- src/Base/RotationPy.xml | 6 ++-- src/Base/RotationPyImp.cpp | 6 ++-- 8 files changed, 57 insertions(+), 37 deletions(-) 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)); }