added Transform function for Line

VirtualRobot/math/Line.cpp

@@ -141,6 +141,11 @@ Line Line::FromPoses(const Eigen::Matrix4f &p1, const Eigen::Matrix4f &p2)
     return FromPoints(p1.block<3,1>(0,3), p2.block<3,1>(0,3));
 }
 
+Line Line::Transform(const Eigen::Matrix4f &pose)
+{
+    return Line(pos + pose.block<3,1>(0,3), pose.block<3, 3>(0, 0) * dir);
+}
+
 
 
 

VirtualRobot/math/Line.h

@@ -48,6 +48,8 @@ namespace math
         static Line FromPoints(Eigen::Vector3f p1, Eigen::Vector3f p2);
         static Line FromPoses(const Eigen::Matrix4f& p1, const Eigen::Matrix4f& p2);
 
+        Line Transform(const Eigen::Matrix4f& pose);
+
     private:
         Eigen::Vector3f pos;
         Eigen::Vector3f dir;