added limitTo function for vectors

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

@@ -57,6 +57,16 @@ namespace armarx
                 return sign * std::min<double>(fabs(value), absThreshold);
             }
 
+            static Eigen::Vector3f LimitTo(Eigen::Vector3f val, float maxNorm)
+            {
+                float norm = val.norm();
+                if (norm > maxNorm)
+                {
+                    return val / norm * maxNorm;
+                }
+                return val;
+            }
+
             static bool CheckMinMax(int min, int max, int value)
             {
                 return value >= min && value <= max;