BUG: Fix the default fixed boundary conditioition implementation

Fix the default fixed boundary conditioition implementation by row and column nullification

Source/DynamicalModels/ObjectModels/imstkFEMDeformableBodyModel.cpp

@@ -165,7 +165,7 @@ FEMDeformableBodyModel::loadBoundaryConditions()
             size_t index;
             auto maxAllowed = m_vegaPhysicsMesh->getNumVertices();
             while (!file.eof())
-            {
+            {                
                 file >> index;
                 if (index < maxAllowed)
                 {
@@ -514,23 +514,26 @@ FEMDeformableBodyModel::applyBoundaryConditions(SparseMatrixd &M, const bool wit
     // Set column and row to zero.
     for (auto & index : m_fixedNodeIds)
     {
-        auto idx = static_cast<SparseMatrixd::Index>(index);
+        auto nodeIdx = static_cast<SparseMatrixd::Index>(index)*3;
 
-        for (int k = 0; k < M.outerSize(); ++k)
+        for (auto idx = nodeIdx; idx < nodeIdx + 3; idx++)
         {
-            for (SparseMatrixd::InnerIterator i(M, k); i; ++i)
+            for (long long k = 0; k < M.outerSize(); ++k)
             {
-                if (i.row() == idx || i.col() == idx)
-                {
-                    i.valueRef() = 0.0;
-                }
-
-                if (i.row() == idx && i.col() == idx)
+                for (SparseMatrixd::InnerIterator i(M, k); i; ++i)
                 {
-                    i.valueRef() = compliance;
+                    if (i.row() == idx || i.col() == idx)
+                    {
+                        i.valueRef() = 0.0;
+                    }
+
+                    if (i.row() == idx && i.col() == idx)
+                    {
+                        i.valueRef() = compliance;
+                    }
                 }
             }
-        }
+        }        
     }
 }
 
@@ -539,7 +542,8 @@ FEMDeformableBodyModel::applyBoundaryConditions(Vectord &x) const
 {
     for (auto & index : m_fixedNodeIds)
     {
-        x(index) = 0.0;
+        const auto _3Index = 3 * index;
+        x(_3Index) = x(_3Index + 1) = x(_3Index + 2) = 0.0;
     }
 }
 
@@ -610,7 +614,10 @@ FEMDeformableBodyModel::getFunction()
                (semiImplicit) ?
                this->computeSemiImplicitSystemRHS(*m_previousState.get(), *m_currentState.get(), m_updateType) :
                this->computeImplicitSystemRHS(*m_previousState.get(), *m_currentState.get(), m_updateType);
-
+               if (this->m_implementFixedBC)
+               {
+                   applyBoundaryConditions(m_Feff);
+               }
                return m_Feff;
            };
 }
@@ -621,8 +628,12 @@ FEMDeformableBodyModel::getFunctionGradient()
     // Gradient of the nonlinear objective function given the current state
     return [&, this](const Vectord &q)->const SparseMatrixd &
            {
-               this->computeImplicitSystemLHS(*m_previousState.get(), *m_currentState.get(), m_updateType);
+                this->computeImplicitSystemLHS(*m_previousState.get(), *m_currentState.get(), m_updateType);
 
+                if(this->m_implementFixedBC)
+                {
+                    applyBoundaryConditions(m_Keff);
+                }               
                return m_Keff;
            };
 }

Source/DynamicalModels/ObjectModels/imstkFEMDeformableBodyModel.h

@@ -248,6 +248,13 @@ public:
     ///
     void setFixedSizeTimeStepping();
 
+    ///
+    /// \brief Set the fixed BC implementation state
+    ///
+    void enableFixedBC() { m_implementFixedBC = true; };
+    void disableFixedBC() { m_implementFixedBC = false; };
+    bool isFixedBCImplemented() const { return m_implementFixedBC; };
+
 protected:
     std::shared_ptr<InternalForceModel> m_internalForceModel;       ///> Mathematical model for intenal forces
     std::shared_ptr<TimeIntegrator>     m_timeIntegrator;           ///> Time integrator
@@ -278,6 +285,10 @@ protected:
     stateUpdateType m_updateType = stateUpdateType::deltaVelocity;        ///> Update type of the model
 
     bool m_damped = false;                                                ///> Viscous or structurally damped system
+
+    // If this is true, the tangent stiffness and force vector will be modified to
+    // accommodate (the rows and columns will be nullified) the fixed boundary conditions
+    bool m_implementFixedBC = true;
 };
 } // imstk