From 2f26867286fa62987e004433d4d0c97ab1200cfe Mon Sep 17 00:00:00 2001
From: Simon Ottenhaus <simon.ottenhaus@kit.edu>
Date: Fri, 17 May 2019 17:47:47 +0200
Subject: [PATCH] added helper functions
---
VirtualRobot/math/Helpers.cpp | 21 +++++++++++++++++++++
VirtualRobot/math/Helpers.h | 8 ++++++++
2 files changed, 29 insertions(+)
diff --git a/VirtualRobot/math/Helpers.cpp b/VirtualRobot/math/Helpers.cpp
index 6ee4ca922..59a264a17 100644
--- a/VirtualRobot/math/Helpers.cpp
+++ b/VirtualRobot/math/Helpers.cpp
@@ -346,6 +346,27 @@ Eigen::VectorXf Helpers::LimitVectorLength(const Eigen::VectorXf& vec, const Eig
return vec / scale;
}
+Eigen::AngleAxisf Helpers::GetAngleAxisFromTo(const Eigen::Matrix3f& start, const Eigen::Matrix3f& target)
+{
+ return Eigen::AngleAxisf(target * start.inverse());
+}
+
+Eigen::Vector3f Helpers::GetRotationVector(const Eigen::Matrix3f& start, const Eigen::Matrix3f& target)
+{
+ Eigen::AngleAxisf aa = GetAngleAxisFromTo(start, target);
+ return aa.axis() * aa.angle();
+}
+
+Eigen::Matrix3f Helpers::RotationVectorToOrientation(const Eigen::Vector3f& rotation)
+{
+ if(rotation.squaredNorm() == 0)
+ {
+ return Eigen::Matrix3f::Identity();
+ }
+ Eigen::AngleAxisf aa(rotation.norm(), rotation.normalized());
+ return aa.toRotationMatrix();
+}
+
float Helpers::rad2deg(float rad)
{
return rad * (180.0f / M_PI_F);
diff --git a/VirtualRobot/math/Helpers.h b/VirtualRobot/math/Helpers.h
index 7bf8bb5a0..e02bfc091 100644
--- a/VirtualRobot/math/Helpers.h
+++ b/VirtualRobot/math/Helpers.h
@@ -157,6 +157,14 @@ namespace math
static float Distance(const Eigen::Matrix4f& a, const Eigen::Matrix4f& b, float rad2mmFactor);
static Eigen::VectorXf LimitVectorLength(const Eigen::VectorXf& vec, const Eigen::VectorXf& maxLen);
+
+
+ // Rotation vectors:
+
+ static Eigen::AngleAxisf GetAngleAxisFromTo(const Eigen::Matrix3f& start, const Eigen::Matrix3f& target);
+ static Eigen::Vector3f GetRotationVector(const Eigen::Matrix3f& start, const Eigen::Matrix3f& target);
+ static Eigen::Matrix3f RotationVectorToOrientation(const Eigen::Vector3f& rotation);
+
/// Convert a value from radian to degree.
--
GitLab