Add TriMeshModel::rotate

VirtualRobot/Visualization/TriMeshModel.cpp

@@ -530,6 +530,20 @@ namespace VirtualRobot
         return areas;
     }
 
+    void TriMeshModel::rotate(const Eigen::Matrix3f& mx)
+    {
+        for (auto& vec3f : normals)
+        {
+            const Eigen::Vector3f old = vec3f;
+            vec3f = mx * old;
+        }
+        for (auto& vec3f : vertices)
+        {
+            const Eigen::Vector3f old = vec3f;
+            vec3f = mx * old;
+        }
+    }
+
     void TriMeshModel::fattenShrink(float offset, bool updateNormals)
     {
         size_t i;

VirtualRobot/Visualization/TriMeshModel.h

@@ -122,6 +122,7 @@ namespace VirtualRobot
         /// Get the areas of all faces.
         std::vector<float> getFaceAreas() const;
 
+        void rotate(const Eigen::Matrix3f& mx);
 
         // Overwrite all colors
         void setColor(VisualizationFactory::Color color);