BUG: Sets displacement to the mesh vertices

Sets displacement to the mesh vertices as sum of original positions and the current displacements

Base/Geometry/Mesh/imstkMesh.cpp

@@ -80,6 +80,7 @@ void
 Mesh::setInitialVerticesPositions(const std::vector<Vec3d>& vertices)
 {
     m_initialVerticesPositions = vertices;
+    m_verticesDisplacements= vertices;
 }
 
 const std::vector<Vec3d>&
@@ -123,14 +124,20 @@ Mesh::setVerticesDisplacements(const std::vector<Vec3d>& diff)
     m_verticesDisplacements = diff;
 }
 
-void Mesh::setVerticesDisplacements(const Vectord& u)
+void
+Mesh::setVerticesDisplacements(const Vectord& u)
 {
     size_t dofId = 0;
-    for (auto vDisp : m_verticesDisplacements)
+    for (auto &vDisp : m_verticesDisplacements)
     {
         vDisp = Vec3d(u(dofId), u(dofId + 1), u(dofId + 2));
         dofId += 3;
     }
+
+    for (auto i = 0; i < m_verticesPositions.size(); i++)
+    {
+        m_verticesPositions[i] = m_initialVerticesPositions[i] + m_verticesDisplacements[i];
+    }
 }
 
 const std::vector<Vec3d>&