Merge pull request #122 from pdima/vector2_norm_fix

Fixed Vector2::Norm()
orocos · Mar 13, 2018 · 6fe130a · 6fe130a
2 parents f806a66 + bbbca30
commit 6fe130a
Show file tree

Hide file tree

Showing 3 changed files with 21 additions and 3 deletions.
diff --git a/orocos_kdl/src/frames.cpp b/orocos_kdl/src/frames.cpp
@@ -87,10 +87,16 @@ namespace KDL {
 
     double Vector2::Norm() const
     {
-        if (fabs(data[0]) > fabs(data[1]) ) {
-            return data[0]*sqrt(1+sqr(data[1]/data[0]));
+        double tmp1 = fabs(data[0]);
+        double tmp2 = fabs(data[1]);
+
+        if (tmp1 == 0.0 && tmp2 == 0.0)
+            return 0.0;
+
+        if (tmp1 > tmp2) {
+            return tmp1*sqrt(1+sqr(data[1]/data[0]));
         } else {
-            return data[1]*sqrt(1+sqr(data[0]/data[1]));
+            return tmp2*sqrt(1+sqr(data[0]/data[1]));
         }
     }
     // makes v a unitvector and returns the norm of v.

diff --git a/orocos_kdl/tests/framestest.cpp b/orocos_kdl/tests/framestest.cpp
@@ -40,6 +40,16 @@ void FramesTest::TestVector() {
 	CPPUNIT_ASSERT_EQUAL(nu2.Norm(),10.0);
 }
 
+void FramesTest::TestVector2DNorm() {
+    Vector2 nu(0, 0);
+	CPPUNIT_ASSERT_EQUAL(nu.Norm(), 0.0);
+
+	CPPUNIT_ASSERT_EQUAL(Vector2(1, 0).Norm(), 1.0);
+	CPPUNIT_ASSERT_EQUAL(Vector2(0, 1).Norm(), 1.0);
+	CPPUNIT_ASSERT_EQUAL(Vector2(-1, 0).Norm(), 1.0);
+	CPPUNIT_ASSERT_EQUAL(Vector2(0, -1).Norm(), 1.0);
+}
+
 void FramesTest::TestTwist2(Twist& t) {
 	Twist t2(Vector(16,-3,5),Vector(-4,2,1));
 

diff --git a/orocos_kdl/tests/framestest.hpp b/orocos_kdl/tests/framestest.hpp
@@ -11,6 +11,7 @@ class FramesTest : public CppUnit::TestFixture
 {
     CPPUNIT_TEST_SUITE( FramesTest);
     CPPUNIT_TEST(TestVector);
+    CPPUNIT_TEST(TestVector2DNorm);
     CPPUNIT_TEST(TestTwist);
     CPPUNIT_TEST(TestWrench);
     CPPUNIT_TEST(TestRotation);
@@ -27,6 +28,7 @@ class FramesTest : public CppUnit::TestFixture
     void tearDown();
 
     void TestVector();
+    void TestVector2DNorm();
     void TestTwist();
     void TestWrench();
     void TestRotation();