From 1a7ae9fcf604ff575059aed4548d51de1480c0ec Mon Sep 17 00:00:00 2001
From: Lucas Fernando Andrade Costa
 <44685107+luccosta@users.noreply.github.com>
Date: Fri, 7 Aug 2020 13:55:28 -0300
Subject: [PATCH] Round, Rounded, Correct, Distance(x, y, z, w) and operator<
 addition to Vector 4 (#146)

* Round, Rounded, Correct, Distance(x, y, z, w) and operator< addition to Vector4

Signed-off-by: Lucas Fernando <lucas.costa@ee.ufcg.edu.br>

* Correct test

Signed-off-by: Lucas Fernando <lucas.costa@ee.ufcg.edu.br>

Co-authored-by: Louise Poubel <louise@openrobotics.org>
---
 include/ignition/math/Vector4.hh | 54 ++++++++++++++++++++++++++++++++
 src/Vector4_TEST.cc              | 20 ++++++++++++
 2 files changed, 74 insertions(+)

diff --git a/include/ignition/math/Vector4.hh b/include/ignition/math/Vector4.hh
index 0568fc436..3ad31c7d7 100644
--- 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
index dc65da4ea..f0e867e1b 100644
--- 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());