added a version of eigen4f2rpy returning the result

added a version of rpy2eigen4f returning the result
added a version of posrpy2eigen4f returning the result
added a version of getTranslation using output parameters
added a version of quat2eigen4f(Quaternion) using output parameters
added a version of quat2eigen4f(float,float,float) using output parameters
added a version of eigen4f2quat using output parameters

VirtualRobot/MathTools.cpp

@@ -233,7 +233,12 @@ namespace VirtualRobot
         storeRPY(2) = x[5];
     }
 
-
+    Eigen::Vector3f VIRTUAL_ROBOT_IMPORT_EXPORT MathTools::eigen4f2rpy(const Eigen::Matrix4f& m)
+    {
+        Eigen::Vector3f storeRPY;
+        eigen4f2rpy(m, storeRPY);
+        return storeRPY;
+    }
 
     void MathTools::rpy2eigen4f(float r, float p, float y, Eigen::Matrix4f& m)
     {
@@ -265,7 +270,13 @@ namespace VirtualRobot
         m(3, 1) = 0;
         m(3, 2) = 0;
         m(3, 3) = 1.0f;
+    }
 
+    Eigen::Matrix4f MathTools::rpy2eigen4f(float r, float p, float y)
+    {
+        Eigen::Matrix4f m;
+        rpy2eigen4f(r,p,y,m);
+        return m;
     }
 
 
@@ -290,12 +301,23 @@ namespace VirtualRobot
         posrpy2eigen4f(x, m);
     }
 
+    Eigen::Matrix4f VIRTUAL_ROBOT_IMPORT_EXPORT MathTools::posrpy2eigen4f(const Eigen::Vector3f& pos, const Eigen::Vector3f& rpy)
+    {
+        Eigen::Matrix4f m;
+        posrpy2eigen4f(pos, rpy,m );
+        return m;
+    }
 
     Eigen::Vector3f MathTools::getTranslation(const Eigen::Matrix4f& m)
     {
         return m.block(0, 3, 3, 1);
     }
 
+    void VIRTUAL_ROBOT_IMPORT_EXPORT MathTools::getTranslation(const Eigen::Matrix4f& m, Eigen::Vector3f& v)
+    {
+        v = getTranslation(m);
+    }
+
     float MathTools::getDistancePointPlane(const Eigen::Vector3f& point, const Plane& plane)
     {
         return plane.n.dot(point - plane.p);
@@ -646,6 +668,11 @@ namespace VirtualRobot
         return quat2eigen4f(q.x, q.y, q.z, q.w);
     }
 
+    void VIRTUAL_ROBOT_IMPORT_EXPORT MathTools::quat2eigen4f(const Quaternion q, Eigen::Matrix4f& m)
+    {
+        m = quat2eigen4f(q);
+    }
+
 
     Eigen::Matrix4f MathTools::quat2eigen4f(float x, float y, float z, float w)
     {
@@ -679,6 +706,11 @@ namespace VirtualRobot
         //m(3,3) = w*w + x*x + y*y + z*z;*/
     }
 
+    void VIRTUAL_ROBOT_IMPORT_EXPORT MathTools::quat2eigen4f(float x, float y, float z, float w, Eigen::Matrix4f& m)
+    {
+        m = quat2eigen4f(x,y,z,w);
+    }
+
     std::string MathTools::getTransformXMLString(const Eigen::Matrix3f& m, int tabs, bool skipMatrixTag)
     {
         std::string t;
@@ -1386,6 +1418,11 @@ namespace VirtualRobot
         return res;
     }
 
+    void VIRTUAL_ROBOT_IMPORT_EXPORT MathTools::eigen4f2quat(const Eigen::Matrix4f& m, Quaternion& q)
+    {
+        q = eigen4f2quat(m);
+    }
+
     MathTools::Quaternion VIRTUAL_ROBOT_IMPORT_EXPORT MathTools::axisangle2quat(const Eigen::Vector3f& axis, float angle)
     {
         Eigen::Matrix4f m = axisangle2eigen4f(axis, angle);

VirtualRobot/MathTools.h

@@ -111,8 +111,11 @@ namespace VirtualRobot
             The translation is set to zero.
         */
         void VIRTUAL_ROBOT_IMPORT_EXPORT rpy2eigen4f(float r, float p, float y, Eigen::Matrix4f& m);
+        Eigen::Matrix4f VIRTUAL_ROBOT_IMPORT_EXPORT rpy2eigen4f(float r, float p, float y);
+
         void VIRTUAL_ROBOT_IMPORT_EXPORT posrpy2eigen4f(const float x[6], Eigen::Matrix4f& m);
         void VIRTUAL_ROBOT_IMPORT_EXPORT posrpy2eigen4f(const Eigen::Vector3f& pos, const Eigen::Vector3f& rpy, Eigen::Matrix4f& m);
+        Eigen::Matrix4f VIRTUAL_ROBOT_IMPORT_EXPORT posrpy2eigen4f(const Eigen::Vector3f& pos, const Eigen::Vector3f& rpy);
 
         /*!
             Convert homogeneous matrix to translation and rpy rotation.
@@ -121,6 +124,7 @@ namespace VirtualRobot
         */
         void VIRTUAL_ROBOT_IMPORT_EXPORT eigen4f2rpy(const Eigen::Matrix4f& m, float x[6]);
         void VIRTUAL_ROBOT_IMPORT_EXPORT eigen4f2rpy(const Eigen::Matrix4f& m, Eigen::Vector3f& storeRPY);
+        Eigen::Vector3f VIRTUAL_ROBOT_IMPORT_EXPORT eigen4f2rpy(const Eigen::Matrix4f& m);
 
         /*!
             Convert quaternion values to a 3x3 rotation matrix and store it to the rotational component of the result.
@@ -129,10 +133,14 @@ namespace VirtualRobot
         */
         Eigen::Matrix4f VIRTUAL_ROBOT_IMPORT_EXPORT quat2eigen4f(float x, float y, float z, float w);
         Eigen::Matrix4f VIRTUAL_ROBOT_IMPORT_EXPORT quat2eigen4f(const Quaternion q);
+        void VIRTUAL_ROBOT_IMPORT_EXPORT quat2eigen4f(float x, float y, float z, float w, Eigen::Matrix4f& m);
+        void VIRTUAL_ROBOT_IMPORT_EXPORT quat2eigen4f(const Quaternion q, Eigen::Matrix4f& m);
 
         Quaternion VIRTUAL_ROBOT_IMPORT_EXPORT eigen4f2quat(const Eigen::Matrix4f& m);
+        void VIRTUAL_ROBOT_IMPORT_EXPORT eigen4f2quat(const Eigen::Matrix4f& m, Quaternion& q);
 
         Eigen::Vector3f VIRTUAL_ROBOT_IMPORT_EXPORT getTranslation(const Eigen::Matrix4f& m);
+        void VIRTUAL_ROBOT_IMPORT_EXPORT getTranslation(const Eigen::Matrix4f& m, Eigen::Vector3f &v);
 
         void VIRTUAL_ROBOT_IMPORT_EXPORT eigen4f2axisangle(const Eigen::Matrix4f& m, Eigen::Vector3f& storeAxis, float& storeAngle);
         Eigen::Matrix4f VIRTUAL_ROBOT_IMPORT_EXPORT axisangle2eigen4f(const Eigen::Vector3f& axis, float angle);