Create abs, absDot and Corerct functions for Vector2d

- Abs() => Get the absolute value of the vector - AbsDot() => The absolute dot product - Correct() => Corrects any nan values Create unit test for all functions. Did all checks and tests. These functions intend to solve issue gazebosim#71 Signed-off-by: Felipe Ximenes <[email protected]>
ximenesfel · Jul 28, 2020 · 959633c · 959633c
1 parent d50d094
commit 959633c
Show file tree

Hide file tree

Showing 2 changed files with 64 additions and 0 deletions.
diff --git a/include/ignition/math/Vector2.hh b/include/ignition/math/Vector2.hh
@@ -134,6 +134,39 @@ namespace ignition
         return (this->data[0] * _v[0]) + (this->data[1] * _v[1]);
       }
 
+      /// \brief Get the absolute value of the vector
+      /// \return a vector with positive elements
+      public: Vector2 Abs() const
+      {
+        return Vector2(std::abs(this->data[0]),
+                       std::abs(this->data[1]));
+      }
+
+      /// \brief Return the absolute dot product of this vector and
+      /// another vector. This is similar to the Dot function, except the
+      /// absolute value of each component of the vector is used.
+      ///
+      /// result = abs(x1 * x2) + abs(y1 * y2)
+      ///
+      /// \param[in] _v the vector
+      /// \return The absolute dot product
+      public: T AbsDot(const Vector2<T> &_v) const
+      {
+        return std::abs(this->data[0] * _v[0]) +
+               std::abs(this->data[1] * _v[1]);
+      }
+
+       /// \brief Corrects any nan values
+      public: inline void Correct()
+      {
+        // std::isfinite works with floating point values,
+        // need to explicit cast to avoid ambiguity in vc++.
+        if (!std::isfinite(static_cast<double>(this->data[0])))
+          this->data[0] = 0;
+        if (!std::isfinite(static_cast<double>(this->data[1])))
+          this->data[1] = 0;
+      }
+
       /// \brief Assignment operator
       /// \param[in] _v a value for x and y element
       /// \return this

diff --git a/src/Vector2_TEST.cc b/src/Vector2_TEST.cc
@@ -48,6 +48,10 @@ TEST(Vector2Test, Vector2)
   v.Set(4, 5);
   EXPECT_TRUE(v == math::Vector2d(4, 5));
 
+  // operator GetAbs
+  v.Set(-1, -2);
+  EXPECT_TRUE(v.Abs() == math::Vector2d(1, 2));
+
   // ::operator=
   v = math::Vector2d(6, 7);
   EXPECT_TRUE(v == math::Vector2d(6, 7));
@@ -189,6 +193,33 @@ TEST(Vector2Test, Dot)
   EXPECT_DOUBLE_EQ(v.Dot(math::Vector2d(0, 1)), 2.0);
 }
 
+//////////////////////////////////////////////
+TEST(Vector2Test, Correct)
+{
+  math::Vector2d vec1(0, NAN);
+  math::Vector2d vec2(INFINITY, -1);
+  math::Vector2d vec3(10, -2);
+
+  vec1.Correct();
+  vec2.Correct();
+  vec3.Correct();
+
+  EXPECT_EQ(vec1, math::Vector2d(0, 0));
+  EXPECT_EQ(vec2, math::Vector2d(0, -1));
+  EXPECT_EQ(vec3, math::Vector2d(10, -2));
+}
+
+/////////////////////////////////////////////////
+TEST(Vector2Test, AbsDot)
+{
+  math::Vector2d v(1, -2);
+
+  EXPECT_DOUBLE_EQ(v.AbsDot(math::Vector2d(3, 4)), 11.0);
+  EXPECT_DOUBLE_EQ(v.AbsDot(math::Vector2d(0, 0)), 0.0);
+  EXPECT_DOUBLE_EQ(v.AbsDot(math::Vector2d(1, 0)), 1.0);
+  EXPECT_DOUBLE_EQ(v.AbsDot(math::Vector2d(0, 1)), 2.0);
+}
+
 /////////////////////////////////////////////////
 TEST(Vector2Test, OperatorStreamOut)
 {