Add function to transform 2D position

git-svn-id: http://svn.code.sf.net/p/simox/code/trunk@686 042f3d55-54a8-47e9-b7fb-15903f145c44

VirtualRobot/MathTools.cpp

@@ -676,11 +676,16 @@ std::string MathTools::getTransformXMLString(const Eigen::Matrix3f &m, const std
 
 Eigen::Vector3f MathTools::transformPosition( const Eigen::Vector3f &pos, const Eigen::Matrix4f &m )
 {
-	Eigen::Matrix4f t;
-	t.setIdentity();
-	t.block(0,3,3,1)=pos;
+	Eigen::Vector4f t(pos.x(), pos.y(), pos.z(), 1);
 	t = m * t;
-	return t.block(0,3,3,1);
+	return t.head(3);
+}
+
+Eigen::Vector2f MathTools::transformPosition( const Eigen::Vector2f &pos, const Eigen::Matrix4f &m )
+{
+	Eigen::Vector4f t(pos.x(), pos.y(), 0, 1);
+	t = m * t;
+	return t.head(2);
 }
 
 bool VIRTUAL_ROBOT_IMPORT_EXPORT MathTools::onNormalPointingSide( const Eigen::Vector3f &point, const Plane &p )

VirtualRobot/MathTools.h

@@ -513,6 +513,12 @@ namespace MathTools
 	*/
 	Eigen::Vector3f VIRTUAL_ROBOT_IMPORT_EXPORT transformPosition(const Eigen::Vector3f &pos, const Eigen::Matrix4f &m);
 
+	/*!
+		This method can be used to multiply a 2d position with a matrix
+		result = m * pos
+	*/
+	Eigen::Vector2f VIRTUAL_ROBOT_IMPORT_EXPORT transformPosition(const Eigen::Vector2f &pos, const Eigen::Matrix4f &m);
+
 	//! Get a random point inside the triangle that is spanned by v1, v2 and v3
 	Eigen::Vector3f VIRTUAL_ROBOT_IMPORT_EXPORT randomPointInTriangle(const Eigen::Vector3f &v1, const Eigen::Vector3f &v2, const Eigen::Vector3f &v3);