(kdl) add eps argument to Norm functions

eps is used to determine when to set norm to zero
orocos · May 4, 2020 · a6fbd38 · a6fbd38 · jbohren-hbr · Jun 21, 2021
1 parent f23632f
commit a6fbd38
Show file tree

Hide file tree

Showing 6 changed files with 14 additions and 13 deletions.
diff --git a/orocos_kdl/src/frameacc.hpp b/orocos_kdl/src/frameacc.hpp
@@ -76,7 +76,7 @@ class VectorAcc
     IMETHOD VectorAcc& operator -= (const VectorAcc& arg);
     IMETHOD static VectorAcc Zero();
     IMETHOD void ReverseSign();
-    IMETHOD doubleAcc Norm();
+    IMETHOD doubleAcc Norm(double eps=epsilon);
     IMETHOD friend VectorAcc operator + (const VectorAcc& r1,const VectorAcc& r2);
     IMETHOD friend VectorAcc operator - (const VectorAcc& r1,const VectorAcc& r2);
     IMETHOD friend VectorAcc operator + (const Vector& r1,const VectorAcc& r2);

diff --git a/orocos_kdl/src/frameacc.inl b/orocos_kdl/src/frameacc.inl
@@ -126,9 +126,9 @@ void VectorAcc::ReverseSign() {
     dv.ReverseSign();
 }
 
-doubleAcc VectorAcc::Norm() {
+doubleAcc VectorAcc::Norm(double eps) {
     doubleAcc res;
-    res.t  = p.Norm();
+    res.t  = p.Norm(eps);
     res.d  = dot(p,v)/res.t;
     res.dd = (dot(p,dv)+dot(v,v)-res.d*res.d)/res.t;
     return res;

diff --git a/orocos_kdl/src/frames.cpp b/orocos_kdl/src/frames.cpp
@@ -85,13 +85,13 @@ namespace KDL {
                      );
     }
 
-    double Vector2::Norm() const
+    double Vector2::Norm(double eps) const
     {
         double tmp1 = fabs(data[0]);
         double tmp2 = fabs(data[1]);
 
-        if (tmp1 == 0.0 && tmp2 == 0.0)
-            return 0.0;
+        if (tmp1 < eps && tmp2 < eps)
+            return 0;
 
         if (tmp1 > tmp2) {
             return tmp1*sqrt(1+sqr(data[1]/data[0]));
@@ -115,7 +115,7 @@ namespace KDL {
 
 
     // do some effort not to lose precision
-    double Vector::Norm() const
+    double Vector::Norm(double eps) const
     {
         double tmp1;
         double tmp2;
@@ -124,7 +124,7 @@ namespace KDL {
         if (tmp1 >= tmp2) {
             tmp2=fabs(data[2]);
             if (tmp1 >= tmp2) {
-                if (tmp1 == 0) {
+                if (tmp1 < eps) {
                     // only to everything exactly zero case, all other are handled correctly
                     return 0;
                 }

diff --git a/orocos_kdl/src/frames.hpp b/orocos_kdl/src/frames.hpp
@@ -237,7 +237,7 @@ class Vector
      double Normalize(double eps=epsilon);
 
      //!    @return the norm of the vector
-     double Norm() const;
+     double Norm(double eps=epsilon) const;
 
 
 
@@ -1015,7 +1015,7 @@ class Vector2
      double Normalize(double eps=epsilon);
 
      //!  @return the norm of the vector
-     double Norm() const;
+     double Norm(double eps=epsilon) const;
 
      //! projects v in its XY plane, and sets *this to these values
      inline void Set3DXY(const Vector& v);

diff --git a/orocos_kdl/src/framevel.hpp b/orocos_kdl/src/framevel.hpp
@@ -104,7 +104,7 @@ class VectorVel
     IMETHOD VectorVel& operator -= (const VectorVel& arg);
     IMETHOD static VectorVel Zero();
     IMETHOD void ReverseSign();
-    IMETHOD doubleVel Norm() const;
+    IMETHOD doubleVel Norm(double eps=epsilon) const;
     IMETHOD friend VectorVel operator + (const VectorVel& r1,const VectorVel& r2);
     IMETHOD friend VectorVel operator - (const VectorVel& r1,const VectorVel& r2);
     IMETHOD friend VectorVel operator + (const Vector& r1,const VectorVel& r2);

diff --git a/orocos_kdl/src/framevel.inl b/orocos_kdl/src/framevel.inl
@@ -350,9 +350,10 @@ void VectorVel::ReverseSign() {
     p.ReverseSign();
     v.ReverseSign();
 }
-doubleVel VectorVel::Norm() const {
+doubleVel VectorVel::Norm(double eps) const {
     double n = p.Norm();
-    return doubleVel(n,n ? dot(p,v)/n : 0); // Setting norm of p to 0 in case norm of v is 0
+    return doubleVel((n < eps) ? 0 : n,
+                     (n < eps) ? 0 : dot(p,v)/n); // Setting norm of p to 0 in case norm of v is 0
 }
 
 bool Equal(const VectorVel& r1,const VectorVel& r2,double eps) {