diff --git a/Base/Geometry/Mesh/imstkMesh.cpp b/Base/Geometry/Mesh/imstkMesh.cpp
index 0024fa94a641e347af28b51eb5c748113797ffe5..1ddec4f7df111c6771296f9a9f49b212b70bed3e 100644
--- a/Base/Geometry/Mesh/imstkMesh.cpp
+++ b/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>&