From ae31e4ff6becbe9249969f4054ec6d111ce31836 Mon Sep 17 00:00:00 2001
From: Simon Ottenhaus <simon.ottenhaus@kit.edu>
Date: Wed, 12 Sep 2018 15:11:02 +0200
Subject: [PATCH] LimitVectorLength
---
VirtualRobot/math/Helpers.cpp | 18 ++++++++++++++++++
VirtualRobot/math/Helpers.h | 1 +
2 files changed, 19 insertions(+)
diff --git a/VirtualRobot/math/Helpers.cpp b/VirtualRobot/math/Helpers.cpp
index 46a12f1ca..5e9d4bfce 100644
--- a/VirtualRobot/math/Helpers.cpp
+++ b/VirtualRobot/math/Helpers.cpp
@@ -270,3 +270,21 @@ Eigen::Matrix3f Helpers::GetOrientation(const Eigen::Matrix4f& pose)
return pose.block<3, 3>(0, 0);
}
+Eigen::VectorXf Helpers::LimitVectorLength(const Eigen::VectorXf& vec, const Eigen::VectorXf& maxLen)
+{
+ if(maxLen.rows() != 1 && maxLen.rows() != vec.rows())
+ {
+ throw std::invalid_argument("maxLen.rows != 1 and != maxLen.rows");
+ }
+ float scale = 1;
+ for(int i = 0; i < vec.rows(); i++)
+ {
+ int j = maxLen.rows() == 1 ? 0 : i;
+ if(std::abs(vec(i)) > maxLen(j) && maxLen(j) >= 0)
+ {
+ scale = std::min(scale, maxLen(j) / std::abs(vec(i)));
+ }
+ }
+ return vec / scale;
+}
+
diff --git a/VirtualRobot/math/Helpers.h b/VirtualRobot/math/Helpers.h
index e0f3d45c5..4e30135dd 100644
--- a/VirtualRobot/math/Helpers.h
+++ b/VirtualRobot/math/Helpers.h
@@ -65,6 +65,7 @@ namespace math
static float Distance(const Eigen::Matrix4f& a, const Eigen::Matrix4f& b, float rad2mmFactor);
static Eigen::Vector3f GetPosition(const Eigen::Matrix4f& pose);
static Eigen::Matrix3f GetOrientation(const Eigen::Matrix4f& pose);
+ static Eigen::VectorXf LimitVectorLength(const Eigen::VectorXf& vec, const Eigen::VectorXf& maxLen);
private:
};
--
GitLab