Added limitTo function

source/RobotAPI/libraries/core/math/MathUtils.h

@@ -51,6 +51,12 @@ namespace armarx
                 return Eigen::Vector3f(LimitMinMax(min(0), max(0), value(0)), LimitMinMax(min(1), max(1), value(1)), LimitMinMax(min(2), max(2), value(2)));
             }
 
+            static double LimitTo(double value, double absThreshold)
+            {
+                int sign = value >= 0 ? 1 : -1;
+                return sign * std::min<double>(fabs(value), absThreshold);
+            }
+
             static bool CheckMinMax(int min, int max, int value)
             {
                 return value >= min && value <= max;