add Equal tolerance method to Quaternion (#196)

Signed-off-by: Ashton Larkin <ashton@openrobotics.org>
gazebosim · Mar 10, 2021 · 05e5232 · 05e5232
1 parent 19f9372
commit 05e5232
Show file tree

Hide file tree

Showing 2 changed files with 37 additions and 8 deletions.
diff --git a/include/ignition/math/Quaternion.hh b/include/ignition/math/Quaternion.hh
@@ -689,26 +689,33 @@ namespace ignition
         return _v + uv + uuv;
       }
 
+      /// \brief Equality test with tolerance.
+      /// \param[in] _qt Quaternion<T> for comparison
+      /// \param[in] _tol equality tolerance
+      /// \return true if the elements of the quaternions are equal
+      /// within the tolerance specified by _tol, false otherwise
+      public: bool Equal(const Quaternion<T> &_qt, const T &_tol) const
+      {
+        return equal(this->qx, _qt.qx, _tol) &&
+               equal(this->qy, _qt.qy, _tol) &&
+               equal(this->qz, _qt.qz, _tol) &&
+               equal(this->qw, _qt.qw, _tol);
+      }
+
       /// \brief Equal to operator
       /// \param[in] _qt Quaternion<T> for comparison
       /// \return True if equal
       public: bool operator==(const Quaternion<T> &_qt) const
       {
-        return equal(this->qx, _qt.qx, static_cast<T>(0.001)) &&
-               equal(this->qy, _qt.qy, static_cast<T>(0.001)) &&
-               equal(this->qz, _qt.qz, static_cast<T>(0.001)) &&
-               equal(this->qw, _qt.qw, static_cast<T>(0.001));
+        return this->Equal(_qt, static_cast<T>(0.001));
       }
 
       /// \brief Not equal to operator
       /// \param[in] _qt Quaternion<T> for comparison
       /// \return True if not equal
       public: bool operator!=(const Quaternion<T> &_qt) const
       {
-        return !equal(this->qx, _qt.qx, static_cast<T>(0.001)) ||
-               !equal(this->qy, _qt.qy, static_cast<T>(0.001)) ||
-               !equal(this->qz, _qt.qz, static_cast<T>(0.001)) ||
-               !equal(this->qw, _qt.qw, static_cast<T>(0.001));
+        return !(*this == _qt);
       }
 
       /// \brief Unary minus operator

diff --git a/src/Quaternion_TEST.cc b/src/Quaternion_TEST.cc
@@ -131,6 +131,28 @@ TEST(QuaternionTest, ConstructAxisAngle)
   EXPECT_TRUE(q == q1);
 }
 
+/////////////////////////////////////////////////
+TEST(QuaternionTest, Equal)
+{
+  // doubles
+  math::Quaterniond q(1, 2, 3, 4);
+  math::Quaterniond q2(1.01, 2.015, 3.002, 4.007);
+  EXPECT_TRUE(q.Equal(q2, 0.02));
+  EXPECT_FALSE(q.Equal(q2, 0.01));
+
+  // floats
+  math::Quaternionf q3(1, 2, 3, 4);
+  math::Quaternionf q4(1.05f, 2.1f, 3.03f, 4.04f);
+  EXPECT_TRUE(q3.Equal(q4, 0.2f));
+  EXPECT_FALSE(q3.Equal(q4, 0.04f));
+
+  // ints
+  math::Quaternioni q5(3, 5, -1, 9);
+  math::Quaternioni q6(3, 6, 1, 12);
+  EXPECT_TRUE(q5.Equal(q6, 3));
+  EXPECT_FALSE(q5.Equal(q6, 2));
+}
+
 /////////////////////////////////////////////////
 TEST(QuaternionTest, Identity)
 {