add scripting interfaces

Signed-off-by: Louise Poubel <louise@openrobotics.org>
gazebosim · Sep 3, 2021 · 1233218 · 1233218
1 parent 5ce790a
commit 1233218
Show file tree

Hide file tree

Showing 15 changed files with 171 additions and 20 deletions.
diff --git a/src/Vector2_TEST.cc b/src/Vector2_TEST.cc
@@ -457,6 +457,8 @@ TEST(Vector2Test, NaN)
 
   auto nanVecF = math::Vector2f::NaN;
   EXPECT_FALSE(nanVecF.IsFinite());
+  EXPECT_TRUE(math::isnan(nanVecF.X()));
+  EXPECT_TRUE(math::isnan(nanVecF.Y()));
 
   nanVecF.Correct();
   EXPECT_EQ(math::Vector2f::Zero, nanVecF);

diff --git a/src/Vector3_TEST.cc b/src/Vector3_TEST.cc
@@ -144,26 +144,6 @@ TEST(Vector3dTest, Vector3d)
   EXPECT_EQ(math::Vector3d::UnitZ, math::Vector3d(0, 0, 1));
   EXPECT_EQ(math::Vector3f::UnitZ, math::Vector3f(0, 0, 1));
   EXPECT_EQ(math::Vector3i::UnitZ, math::Vector3i(0, 0, 1));
-
-  auto nanVec = math::Vector3d::NaN;
-  EXPECT_FALSE(nanVec.IsFinite());
-  EXPECT_TRUE(math::isnan(nanVec.X()));
-  EXPECT_TRUE(math::isnan(nanVec.Y()));
-  EXPECT_TRUE(math::isnan(nanVec.Z()));
-
-  nanVec.Correct();
-  EXPECT_EQ(math::Vector3d::Zero, nanVec);
-  EXPECT_TRUE(nanVec.IsFinite());
-
-  auto nanVecF = math::Vector3f::NaN;
-  EXPECT_FALSE(nanVecF.IsFinite());
-  EXPECT_TRUE(math::isnan(nanVecF.X()));
-  EXPECT_TRUE(math::isnan(nanVecF.Y()));
-  EXPECT_TRUE(math::isnan(nanVecF.Z()));
-
-  nanVecF.Correct();
-  EXPECT_EQ(math::Vector3f::Zero, nanVecF);
-  EXPECT_TRUE(nanVecF.IsFinite());
 }
 
 /////////////////////////////////////////////////
@@ -501,3 +481,26 @@ TEST(Vector3dTest, NoException)
   EXPECT_NO_THROW(ss << vInf);
 }
 
+/////////////////////////////////////////////////
+TEST(Vector3dTest, NaN)
+{
+  auto nanVec = math::Vector3d::NaN;
+  EXPECT_FALSE(nanVec.IsFinite());
+  EXPECT_TRUE(math::isnan(nanVec.X()));
+  EXPECT_TRUE(math::isnan(nanVec.Y()));
+  EXPECT_TRUE(math::isnan(nanVec.Z()));
+
+  nanVec.Correct();
+  EXPECT_EQ(math::Vector3d::Zero, nanVec);
+  EXPECT_TRUE(nanVec.IsFinite());
+
+  auto nanVecF = math::Vector3f::NaN;
+  EXPECT_FALSE(nanVecF.IsFinite());
+  EXPECT_TRUE(math::isnan(nanVecF.X()));
+  EXPECT_TRUE(math::isnan(nanVecF.Y()));
+  EXPECT_TRUE(math::isnan(nanVecF.Z()));
+
+  nanVecF.Correct();
+  EXPECT_EQ(math::Vector3f::Zero, nanVecF);
+  EXPECT_TRUE(nanVecF.IsFinite());
+}
diff --git a/src/Vector4_TEST.cc b/src/Vector4_TEST.cc
@@ -459,6 +459,10 @@ TEST(Vector4Test, NaN)
 
   auto nanVecF = math::Vector4f::NaN;
   EXPECT_FALSE(nanVecF.IsFinite());
+  EXPECT_TRUE(math::isnan(nanVecF.X()));
+  EXPECT_TRUE(math::isnan(nanVecF.Y()));
+  EXPECT_TRUE(math::isnan(nanVecF.Z()));
+  EXPECT_TRUE(math::isnan(nanVecF.W()));
 
   nanVecF.Correct();
   EXPECT_EQ(math::Vector4f::Zero, nanVecF);

diff --git a/src/python/Vector2.i b/src/python/Vector2.i
@@ -37,6 +37,7 @@ namespace ignition
       %rename("%(uppercase)s", %$isstatic, %$isvariable) "";
       public: static const Vector2 Zero;
       public: static const Vector2 One;
+      public: static const Vector2 NaN;
 
       public: Vector2();
       public: Vector2(const T &_x, const T &_y);

diff --git a/src/python/Vector2_TEST.py b/src/python/Vector2_TEST.py
@@ -15,6 +15,7 @@
 import unittest
 import math
 from ignition.math import Vector2d
+from ignition.math import Vector2f
 
 
 class TestVector2(unittest.TestCase):
@@ -295,6 +296,24 @@ def test_lenght(self):
         self.assertAlmostEqual(v.length(), 5)
         self.assertAlmostEqual(v.squared_length(), 25)
 
+    def test_nan(self):
+        nanVec = Vector2d.NAN
+        self.assertFalse(nanVec.is_finite())
+        self.assertTrue(math.isnan(nanVec.x()))
+        self.assertTrue(math.isnan(nanVec.y()))
+
+        nanVec.correct()
+        self.assertEqual(Vector2d.ZERO, nanVec)
+        self.assertTrue(nanVec.is_finite())
+
+        nanVecF = Vector2f.NAN
+        self.assertFalse(nanVecF.is_finite())
+        self.assertTrue(math.isnan(nanVecF.x()))
+        self.assertTrue(math.isnan(nanVecF.y()))
+
+        nanVecF.correct()
+        self.assertEqual(Vector2f.ZERO, nanVecF)
+        self.assertTrue(nanVecF.is_finite())
 
 if __name__ == '__main__':
     unittest.main()
diff --git a/src/python/Vector3.i b/src/python/Vector3.i
@@ -43,6 +43,7 @@ namespace ignition
       public: static const Vector3 UnitY;
       %rename(UNIT_Z) UnitZ;
       public: static const Vector3 UnitZ;
+      public: static const Vector3 NaN;
       public: Vector3();
       public: Vector3(const T &_x, const T &_y, const T &_z);
       public: Vector3(const Vector3<T> &_v);

diff --git a/src/python/Vector3_TEST.py b/src/python/Vector3_TEST.py
@@ -15,6 +15,7 @@
 import unittest
 import math
 from ignition.math import Vector3d
+from ignition.math import Vector3f
 
 
 class TestVector3(unittest.TestCase):
@@ -350,6 +351,26 @@ def test_finite(self):
 
         self.assertTrue(vec1.is_finite())
 
+    def test_nan(self):
+        nanVec = Vector3d.NAN
+        self.assertFalse(nanVec.is_finite())
+        self.assertTrue(math.isnan(nanVec.x()))
+        self.assertTrue(math.isnan(nanVec.y()))
+        self.assertTrue(math.isnan(nanVec.z()))
+
+        nanVec.correct()
+        self.assertEqual(Vector3d.ZERO, nanVec)
+        self.assertTrue(nanVec.is_finite())
+
+        nanVecF = Vector3f.NAN
+        self.assertFalse(nanVecF.is_finite())
+        self.assertTrue(math.isnan(nanVecF.x()))
+        self.assertTrue(math.isnan(nanVecF.y()))
+        self.assertTrue(math.isnan(nanVecF.z()))
+
+        nanVecF.correct()
+        self.assertEqual(Vector3f.ZERO, nanVecF)
+        self.assertTrue(nanVecF.is_finite())
 
 if __name__ == '__main__':
     unittest.main()
diff --git a/src/python/Vector4.i b/src/python/Vector4.i
@@ -37,6 +37,7 @@ namespace ignition
       %rename("%(uppercase)s", %$isstatic, %$isvariable) "";
       public: static const Vector4 Zero;
       public: static const Vector4 One;
+      public: static const Vector4 NaN;
       public: Vector4();
       public: Vector4(const T &_x, const T &_y, const T &_z, const T &_w);
       public: Vector4(const Vector4<T> &_v);
@@ -58,6 +59,7 @@ namespace ignition
       public: bool operator==(const Vector4<T> &_v) const;
       public: bool Equal(const Vector4 &_v, const T &_tol) const;
       public: bool IsFinite() const;
+      public: inline void Correct();
       public: inline T X() const;
       public: inline T Y() const;
       public: inline T Z() const;

diff --git a/src/python/Vector4_TEST.py b/src/python/Vector4_TEST.py
@@ -16,6 +16,7 @@
 import unittest
 import math
 from ignition.math import Vector4d
+from ignition.math import Vector4f
 
 
 class TestVector4(unittest.TestCase):
@@ -256,6 +257,28 @@ def test_finite(self):
         vec1 = Vector4d(0.1, 0.2, 0.3, 0.4)
         self.assertTrue(vec1.is_finite())
 
+    def test_nan(self):
+        nanVec = Vector4d.NAN
+        self.assertFalse(nanVec.is_finite())
+        self.assertTrue(math.isnan(nanVec.x()))
+        self.assertTrue(math.isnan(nanVec.y()))
+        self.assertTrue(math.isnan(nanVec.z()))
+        self.assertTrue(math.isnan(nanVec.w()))
+
+        nanVec.correct()
+        self.assertEqual(Vector4d.ZERO, nanVec)
+        self.assertTrue(nanVec.is_finite())
+
+        nanVecF = Vector4f.NAN
+        self.assertFalse(nanVecF.is_finite())
+        self.assertTrue(math.isnan(nanVecF.x()))
+        self.assertTrue(math.isnan(nanVecF.y()))
+        self.assertTrue(math.isnan(nanVecF.z()))
+        self.assertTrue(math.isnan(nanVecF.w()))
+
+        nanVecF.correct()
+        self.assertEqual(Vector4f.ZERO, nanVecF)
+        self.assertTrue(nanVecF.is_finite())
 
 if __name__ == '__main__':
     unittest.main()
diff --git a/src/ruby/Vector2.i b/src/ruby/Vector2.i
@@ -35,6 +35,7 @@ namespace ignition
     {
       public: static const Vector2 Zero;
       public: static const Vector2 One;
+      public: static const Vector2 NaN;
 
       public: Vector2();
       public: Vector2(const T &_x, const T &_y);

diff --git a/src/ruby/Vector2_TEST.rb b/src/ruby/Vector2_TEST.rb
@@ -257,6 +257,28 @@ def test_length
     assert((v.SquaredLength() - 17.65).abs < 1e-8,
            "Squared length of v should be near 17.65")
   end
+
+  def test_nan
+    nanVec = Ignition::Math::Vector2d.NaN
+    assert(!nanVec.IsFinite(),
+           "NaN vector shouldn't be finite")
+    assert(nanVec.X().nan?, "X should be NaN")
+    assert(nanVec.Y().nan?, "Y should be NaN")
+
+    nanVec.Correct()
+    assert(Ignition::Math::Vector2d.Zero == nanVec,
+           "Corrected vector equals zero")
+
+    nanVecF = Ignition::Math::Vector2f.NaN
+    assert(!nanVecF.IsFinite(),
+           "NaN vector shouldn't be finite")
+    assert(nanVecF.X().nan?, "X should be NaN")
+    assert(nanVecF.Y().nan?, "Y should be NaN")
+
+    nanVecF.Correct()
+    assert(Ignition::Math::Vector2f.Zero == nanVecF,
+           "Corrected vector equals zero")
+  end
 end
 
 exit Test::Unit::UI::Console::TestRunner.run(Vector2_TEST).passed? ? 0 : -1
diff --git a/src/ruby/Vector3.i b/src/ruby/Vector3.i
@@ -38,6 +38,7 @@ namespace ignition
       public: static const Vector3 UnitX;
       public: static const Vector3 UnitY;
       public: static const Vector3 UnitZ;
+      public: static const Vector3 NaN;
       public: Vector3();
       public: Vector3(const T &_x, const T &_y, const T &_z);
       public: Vector3(const Vector3<T> &_v);

diff --git a/src/ruby/Vector3_TEST.rb b/src/ruby/Vector3_TEST.rb
@@ -425,6 +425,29 @@ def test_finite
     assert(vec1.IsFinite(), "Vector3 vec1 should be be finite")
   end
 
+  def test_nan
+    nanVec = Ignition::Math::Vector3d.NaN
+    assert(!nanVec.IsFinite(),
+           "NaN vector shouldn't be finite")
+    assert(nanVec.X().nan?, "X should be NaN")
+    assert(nanVec.Y().nan?, "Y should be NaN")
+    assert(nanVec.Z().nan?, "Z should be NaN")
+
+    nanVec.Correct()
+    assert(Ignition::Math::Vector3d.Zero == nanVec,
+           "Corrected vector equals zero")
+
+    nanVecF = Ignition::Math::Vector3f.NaN
+    assert(!nanVecF.IsFinite(),
+           "NaN vector shouldn't be finite")
+    assert(nanVecF.X().nan?, "X should be NaN")
+    assert(nanVecF.Y().nan?, "Y should be NaN")
+    assert(nanVecF.Z().nan?, "Z should be NaN")
+
+    nanVecF.Correct()
+    assert(Ignition::Math::Vector3f.Zero == nanVecF,
+           "Corrected vector equals zero")
+  end
 end
 
 exit Test::Unit::UI::Console::TestRunner.run(Vector3_TEST).passed? ? 0 : -1
diff --git a/src/ruby/Vector4.i b/src/ruby/Vector4.i
@@ -35,6 +35,7 @@ namespace ignition
     {
       public: static const Vector4 Zero;
       public: static const Vector4 One;
+      public: static const Vector4 NaN;
       public: Vector4();
       public: Vector4(const T &_x, const T &_y, const T &_z, const T &_w);
       public: Vector4(const Vector4<T> &_v);
@@ -56,6 +57,7 @@ namespace ignition
       public: bool operator==(const Vector4<T> &_v) const;
       public: bool Equal(const Vector4 &_v, const T &_tol) const;
       public: bool IsFinite() const;
+      public: inline void Correct();
       public: inline T X() const;
       public: inline T Y() const;
       public: inline T Z() const;

diff --git a/src/ruby/Vector4_TEST.rb b/src/ruby/Vector4_TEST.rb
@@ -295,6 +295,32 @@ def test_finite
 
     assert(vec1.IsFinite(), "Vector4 vec1 should be be finite")
   end
+
+  def test_nan
+    nanVec = Ignition::Math::Vector4d.NaN
+    assert(!nanVec.IsFinite(),
+           "NaN vector shouldn't be finite")
+    assert(nanVec.X().nan?, "X should be NaN")
+    assert(nanVec.Y().nan?, "Y should be NaN")
+    assert(nanVec.Z().nan?, "Z should be NaN")
+    assert(nanVec.W().nan?, "W should be NaN")
+
+    nanVec.Correct()
+    assert(Ignition::Math::Vector4d.Zero == nanVec,
+           "Corrected vector equals zero")
+
+    nanVecF = Ignition::Math::Vector4f.NaN
+    assert(!nanVecF.IsFinite(),
+           "NaN vector shouldn't be finite")
+    assert(nanVecF.X().nan?, "X should be NaN")
+    assert(nanVecF.Y().nan?, "Y should be NaN")
+    assert(nanVecF.Z().nan?, "Z should be NaN")
+    assert(nanVecF.W().nan?, "W should be NaN")
+
+    nanVecF.Correct()
+    assert(Ignition::Math::Vector4f.Zero == nanVecF,
+           "Corrected vector equals zero")
+  end
 end
 
 exit Test::Unit::UI::Console::TestRunner.run(Vector4_TEST).passed? ? 0 : -1