Round, Rounded, Correct, Distance(x, y, z, w) and operator< addition …

…to Vector 4 (gazebosim#146) * Round, Rounded, Correct, Distance(x, y, z, w) and operator< addition to Vector4 Signed-off-by: Lucas Fernando <[email protected]> * Correct test Signed-off-by: Lucas Fernando <[email protected]> Co-authored-by: Louise Poubel <[email protected]>
traversaro · Aug 7, 2020 · 1a7ae9f · 1a7ae9f
1 parent c43f478
commit 1a7ae9f
Show file tree

Hide file tree

Showing 2 changed files with 74 additions and 0 deletions.
diff --git a/include/ignition/math/Vector4.hh b/include/ignition/math/Vector4.hh
@@ -84,6 +84,17 @@ namespace ignition
                     (this->data[3]-_pt[3])*(this->data[3]-_pt[3]));
       }
 
+      /// \brief Calc distance to the given point
+      /// \param[in] _x value along x
+      /// \param[in] _y value along y
+      /// \param[in] _z value along z
+      /// \param[in] _w value along w
+      /// \return the distance
+      public: T Distance(T _x, T _y, T _z, T _w) const
+      {
+        return this->Distance(Vector4(_x, _y, _z, _w));
+      }
+
       /// \brief Returns the length (magnitude) of the vector
       /// \return The length
       public: T Length() const
@@ -101,6 +112,39 @@ namespace ignition
              + std::pow(this->data[3], 2);
       }
 
+      /// \brief Round to near whole number.
+      public: void Round()
+      {
+        this->data[0] = nearbyint(this->data[0]);
+        this->data[1] = nearbyint(this->data[1]);
+        this->data[2] = nearbyint(this->data[2]);
+        this->data[3] = nearbyint(this->data[3]);
+      }
+
+      /// \brief Get a rounded version of this vector
+      /// \return a rounded vector
+      public: Vector4 Rounded() const
+      {
+        Vector4<T> result = *this;
+        result.Round();
+        return result;
+      }
+
+      /// \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;
+        if (!std::isfinite(static_cast<double>(this->data[2])))
+          this->data[2] = 0;
+        if (!std::isfinite(static_cast<double>(this->data[3])))
+          this->data[3] = 0;
+      }
+
       /// \brief Normalize the vector length
       public: void Normalize()
       {
@@ -639,6 +683,16 @@ namespace ignition
         this->data[3] = _v;
       }
 
+      /// \brief Less than operator.
+      /// \param[in] _pt Vector to compare.
+      /// \return True if this vector's X(), Y(), Z() or W() value is less
+      /// than the given vector's corresponding values.
+      public: bool operator<(const Vector4<T> &_pt) const
+      {
+        return this->data[0] < _pt[0] || this->data[1] < _pt[1] ||
+               this->data[2] < _pt[2] || this->data[3] < _pt[3];
+      }
+
       /// \brief Stream insertion operator
       /// \param[in] _out output stream
       /// \param[in] _pt Vector4 to output

diff --git a/src/Vector4_TEST.cc b/src/Vector4_TEST.cc
@@ -43,8 +43,10 @@ TEST(Vector4dTest, Vector4d)
   math::Vector4d v(v1);
   EXPECT_EQ(v, v1);
 
+  // ::Distance
   EXPECT_TRUE(math::equal(v.Distance(
           math::Vector4d(0, 0, 0, 0)), 5.4772, 1e-3));
+  EXPECT_TRUE(math::equal(v.Distance(1, 2, 3, 4), 0.0));
 
   // ::Length()
   v.Set(1, 2, 3, 4);
@@ -53,7 +55,21 @@ TEST(Vector4dTest, Vector4d)
   // ::SquaredLength()
   EXPECT_TRUE(math::equal(v.SquaredLength(), 30.0));
 
+  // ::Rounded
+  v.Set(1.23, 2.34, 3.55, 8.49);
+  EXPECT_TRUE(v.Rounded() == math::Vector4d(1, 2, 4, 8));
+
+  // ::Round
+  v.Round();
+  EXPECT_TRUE(v == math::Vector4d(1, 2, 4, 8));
+
+  // ::Correct
+  v.Set(1, 1, std::nan("1"), 1);
+  v.Correct();
+  EXPECT_TRUE(v == math::Vector4d(1, 1, 0, 1));
+
   // ::Normalize
+  v.Set(1, 2, 3, 4);
   v.Normalize();
   EXPECT_EQ(v, math::Vector4d(0.182574, 0.365148, 0.547723, 0.730297));
 
@@ -145,6 +161,10 @@ TEST(Vector4dTest, Vector4d)
   // ::operator != vector4
   EXPECT_NE(v, math::Vector4d());
 
+  // ::operator < vector4
+  v.Set(1, 2, 3, 4);
+  EXPECT_TRUE(v < math::Vector4d(4, 3, 2, 1));
+
   // ::IsFinite
   EXPECT_TRUE(v.IsFinite());