added transform function to Plane

VirtualRobot/math/Plane.cpp

@@ -101,6 +101,7 @@ Plane Plane::FromNormal(Eigen::Vector3f pos, Eigen::Vector3f normal)
     return Plane(pos, d1, d2);
 }
 
-
-
-
+math::Plane math::Plane::Transform(const Eigen::Matrix4f &transform)
+{
+    return Plane((transform * Eigen::Vector4f(pos(0), pos(1), pos(2), 1)).block<3,1>(0,0), transform.block<3,3>(0,0) * dir1, transform.block<3,3>(0,0) * dir2);
+}

VirtualRobot/math/Plane.h

@@ -44,7 +44,7 @@ public:
     Eigen::Vector3f Dir1(){return dir1;}
     Eigen::Vector3f Dir2(){return dir2;}
 
-
+    Plane Transform(const Eigen::Matrix4f &transform);
     Eigen::Vector3f GetPoint(float u, float v) override;
     Eigen::Vector3f GetDdu(float u, float v) override;
     Eigen::Vector3f GetDdv(float u, float v) override;