Sum normalized functions (gazebosim#140)

Signed-off-by: Lucas Fernando <[email protected]>
Co-authored-by: Michael Carroll <[email protected]>

include/ignition/math/Vector4.hh

@@ -115,6 +115,15 @@ namespace ignition
         }
       }
 
+      /// \brief Return a normalized vector
+      /// \return unit length vector
+      public: Vector4 Normalized() const
+      {
+        Vector4<T> result = *this;
+        result.Normalize();
+        return result;
+      }
+
       /// \brief Return the dot product of this vector and another vector
       /// \param[in] _v the vector
       /// \return the dot product
@@ -201,6 +210,13 @@ namespace ignition
         return *std::min_element(this->data, this->data+4);
       }
 
+      /// \brief Return the sum of the values
+      /// \return the sum
+      public: T Sum() const
+      {
+        return this->data[0] + this->data[1] + this->data[2] + this->data[3];
+      }
+
       /// \brief Assignment operator
       /// \param[in] _v the vector
       /// \return a reference to this vector

src/Vector4_TEST.cc

@@ -57,6 +57,11 @@ TEST(Vector4dTest, Vector4d)
   v.Normalize();
   EXPECT_EQ(v, math::Vector4d(0.182574, 0.365148, 0.547723, 0.730297));
 
+  // ::Normalized
+  v.Set(1, 2, 3, 4);
+  EXPECT_EQ(v.Normalized(),
+            math::Vector4d(0.182574, 0.365148, 0.547723, 0.730297));
+
   // ::Set
   v.Set(2, 4, 6, 8);
   EXPECT_EQ(v, math::Vector4d(2, 4, 6, 8));
@@ -210,6 +215,16 @@ TEST(Vector2Test, EqualTolerance)
   EXPECT_TRUE(math::Vector4d::Zero.Equal(math::Vector4d::One, 1.1));
 }
 
+/////////////////////////////////////////////////
+TEST(Vector4dTest, Sum)
+{
+  math::Vector4d vec1(1.5, 2.5, 3.5, -4.5);
+
+  EXPECT_TRUE(math::equal(math::Vector4d::Zero.Sum(), 0.0, 1e-6));
+  EXPECT_TRUE(math::equal(math::Vector4d::One.Sum(), 4.0, 1e-6));
+  EXPECT_TRUE(math::equal(vec1.Sum(), 3.0, 1e-6));
+}
+
 /////////////////////////////////////////////////
 TEST(Vector4dTest, Add)
 {