diff --git a/VirtualRobot/math/Helpers.cpp b/VirtualRobot/math/Helpers.cpp
index 6ee4ca92209380f12b71f7235d5bea130cbe8122..59a264a174b0e406535c37205729ecc128b994cc 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 7bf8bb5a0741b73eec4b79c0ac9af8f6c3d13294..e02bfc0914e87d0aea86ce2ec5c306a150da6479 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.