Merge branch 'newton-semiimplicit' into 'master'

Limit maxIterations to 1 for semi-implicit stepping

See merge request !567

Source/DynamicalModels/InternalForceModel/imstkInternalForceModel.cpp

@@ -44,4 +44,11 @@ InternalForceModel::updateValuesFromMatrix(std::shared_ptr<vega::SparseMatrix> v
         offset += rowLengths[row];
     }
 }
+
+void
+InternalForceModel::getForceAndMatrix(const Vectord& u, Vectord& internalForce, SparseMatrixd& tangentStiffnessMatrix)
+{
+    this->getInternalForce(u, internalForce);
+    this->getTangentStiffnessMatrix(u, tangentStiffnessMatrix);
+}
 } // imstk

Source/DynamicalModels/InternalForceModel/imstkInternalForceModel.h

@@ -72,8 +72,9 @@ public:
 
     ///
     /// \brief Compute both internal force \p internalForce and stiffness matrix \p tangentStiffnessMatrix at state \u
+    /// \note This default implementation just call \p getInternalForce and \p getTangentStiffnessMatrix, but a more efficient implementation can be rewrite this.
     ///
-    virtual void getForceAndMatrix(const Vectord& u, Vectord& internalForce, SparseMatrixd& tangentStiffnessMatrix) = 0;
+    virtual void getForceAndMatrix(const Vectord& u, Vectord& internalForce, SparseMatrixd& tangentStiffnessMatrix);
 
     ///
     /// \brief Update the values of the Eigen sparse matrix given the linearized array of data from the Vega matrix

Source/DynamicalModels/InternalForceModel/imstkLinearFEMForceModel.h

@@ -84,15 +84,6 @@ public:
         *tangentStiffnessMatrix = new vega::SparseMatrix(*m_stiffnessMatrix.get());
     }
 
-    ///
-    /// \brief Get the tangent stiffness matrix and internal force
-    ///
-    inline void getForceAndMatrix(const Vectord& u, Vectord& internalForce, SparseMatrixd& tangentStiffnessMatrix) override
-    {
-        getInternalForce(u, internalForce);
-        getTangentStiffnessMatrix(u, tangentStiffnessMatrix);
-    }
-
     ///
     /// \brief Set the tangent stiffness matrix
     ///

Source/DynamicalModels/InternalForceModel/imstkStVKForceModel.h

@@ -74,15 +74,6 @@ public:
         InternalForceModel::updateValuesFromMatrix(m_vegaTangentStiffnessMatrix, tangentStiffnessMatrix.valuePtr());
     }
 
-    ///
-    /// \brief Set the tangent stiffness matrix and internal force
-    ///
-    inline void getForceAndMatrix(const Vectord& u, Vectord& internalForce, SparseMatrixd& tangentStiffnessMatrix) override
-    {
-        getInternalForce(u, internalForce);
-        getTangentStiffnessMatrix(u, tangentStiffnessMatrix);
-    }
-
     ///
     /// \brief Speficy tangent stiffness matrix
     ///

Source/DynamicalModels/ObjectModels/imstkFEMDeformableBodyModel.cpp

@@ -168,7 +168,8 @@ FEMDeformableBodyModel::initialize()
     if (m_solver == nullptr)
     {
         // Create a nonlinear system
-        auto nlSystem = std::make_shared<NonLinearSystem<SparseMatrixd>>(getFunction(), getFunctionGradient());
+        // auto nlSystem = std::make_shared<NonLinearSystem<SparseMatrixd>>(getFunction(), getFunctionGradient());
+        auto nlSystem = std::make_shared<NonLinearSystem<SparseMatrixd>>(getFunction(), getFunctionGradient(), getFunctionAndGradient());
 
         nlSystem->setUnknownVector(getUnknownVec());
         nlSystem->setUpdateFunction(getUpdateFunction());
@@ -455,13 +456,13 @@ FEMDeformableBodyModel::computeImplicitSystemRHS(kinematicState&       stateAtT,
     const auto& v     = newState.getQDot();
 
     // Do checks if there are uninitialized matrices
-    m_internalForceModel->getTangentStiffnessMatrix(u, m_K);
     const double dT = m_timeIntegrator->getTimestepSize();
 
     switch (updateType)
     {
     case StateUpdateType::DeltaVelocity:
 
+        m_internalForceModel->getTangentStiffnessMatrix(u, m_K);
         m_Feff = m_K * -(uPrev - u + v * dT);
 
         if (m_damped)
@@ -550,6 +551,97 @@ FEMDeformableBodyModel::computeImplicitSystemLHS(const kinematicState& imstkNotU
     }
 }
 
+void
+FEMDeformableBodyModel::computeSemiImplicitSystemRHSAndLHS(kinematicState&       stateAtT,
+                                                           kinematicState&       newState,
+                                                           const StateUpdateType updateType)
+{
+    const auto&  vPrev = stateAtT.getQDot();
+    const double dT    = m_timeIntegrator->getTimestepSize();
+
+    switch (updateType)
+    {
+    case StateUpdateType::DeltaVelocity:
+        // LHS
+        this->updateMassMatrix();
+        m_internalForceModel->getForceAndMatrix(newState.getQ(), m_Finternal, m_K);
+        this->updateDampingMatrix();
+
+        m_Keff = m_M;
+        if (m_damped)
+        {
+            m_Keff += dT * m_C;
+        }
+        m_Keff += (dT * dT) * m_K;
+
+        // RHS
+        m_Feff = m_K * (vPrev * -dT);
+
+        if (m_damped)
+        {
+            m_Feff -= m_C * vPrev;
+        }
+
+        m_Feff -= m_Finternal;
+        m_Feff += m_FexplicitExternal;
+        m_Feff += m_Fgravity;
+        m_Feff += m_Fcontact;
+        m_Feff *= dT;
+
+        break;
+
+    default:
+        LOG(FATAL) << "Update type not supported";
+    }
+}
+
+void
+FEMDeformableBodyModel::computeImplicitSystemRHSAndLHS(kinematicState&       stateAtT,
+                                                       kinematicState&       newState,
+                                                       const StateUpdateType updateType)
+{
+    const auto&  uPrev = stateAtT.getQ();
+    const auto&  vPrev = stateAtT.getQDot();
+    auto&        u     = newState.getQ();
+    const auto&  v     = newState.getQDot();
+    const double dT    = m_timeIntegrator->getTimestepSize();
+
+    switch (updateType)
+    {
+    case StateUpdateType::DeltaVelocity:
+        // LHS
+        this->updateMassMatrix();
+        m_internalForceModel->getForceAndMatrix(u, m_Finternal, m_K);
+        this->updateDampingMatrix();
+
+        m_Keff = m_M;
+        if (m_damped)
+        {
+            m_Keff += dT * m_C;
+        }
+        m_Keff += (dT * dT) * m_K;
+
+        // RHS
+        m_Feff = m_K * -(uPrev - u + v * dT);
+
+        if (m_damped)
+        {
+            m_Feff -= m_C * v;
+        }
+
+        m_Feff -= m_Finternal;
+        m_Feff += m_FexplicitExternal;
+        m_Feff += m_Fgravity;
+        m_Feff += m_Fcontact;
+        m_Feff *= dT;
+        m_Feff += m_M * (vPrev - v);
+        break;
+
+    default:
+        LOG(FATAL) << "Update type not supported";
+    }
+}
+
 bool
 FEMDeformableBodyModel::initializeExplicitExternalForces()
 {
@@ -740,6 +832,33 @@ FEMDeformableBodyModel::getFunctionGradient()
 #endif
 }
 
+NonLinearSystem<SparseMatrixd>::VectorMatrixFunctionType
+FEMDeformableBodyModel::getFunctionAndGradient()
+{
+#ifdef WIN32
+#pragma warning( push )
+#pragma warning( disable : 4100 )
+#endif
+
+    // Function to evaluate the nonlinear objective function given the current state
+    // return [&, this](const Vectord& q, const bool semiImplicit) -> NonLinearSolver<SparseMatrixd>::VecMatPair
+    return [&, this](const Vectord& q, const bool semiImplicit)
+           {
+               (semiImplicit) ?
+               this->computeSemiImplicitSystemRHSAndLHS(*m_previousState.get(), *m_currentState.get(), m_updateType) :
+               this->computeImplicitSystemRHSAndLHS(*m_previousState.get(), *m_currentState.get(), m_updateType);
+               if (this->m_implementFixedBC)
+               {
+                   applyBoundaryConditions(m_Feff);
+                   applyBoundaryConditions(m_Keff);
+               }
+               return std::make_pair(&m_Feff, &m_Keff);
+           };
+#ifdef WIN32
+#pragma warning( pop )
+#endif
+}
+
 NonLinearSystem<SparseMatrixd>::UpdateFunctionType
 FEMDeformableBodyModel::getUpdateFunction()
 {

Source/DynamicalModels/ObjectModels/imstkFEMDeformableBodyModel.h

@@ -175,6 +175,16 @@ public:
     ///
     void computeImplicitSystemLHS(const kinematicState& prevState, kinematicState& newState, const StateUpdateType updateType);
 
+    ///
+    /// \brief Compute the RHS and LHS of the resulting linear system using semi-implicit scheme
+    ///
+    void computeSemiImplicitSystemRHSAndLHS(kinematicState& prevState, kinematicState& newState, const StateUpdateType updateType);
+
+    ///
+    /// \brief Compute the RHS and LHS of the resulting linear system using fully-implicit scheme
+    ///
+    void computeImplicitSystemRHSAndLHS(kinematicState& prevState, kinematicState& newState, const StateUpdateType updateType);
+
     ///
     /// \brief Update damping Matrix
     ///
@@ -226,6 +236,12 @@ public:
     ///
     System::MatrixFunctionType getFunctionGradient();
 
+    ///
+    /// \brief Returns the "function" that evaluates the nonlinear function and its gradient
+    /// function given the state vector
+    ///
+    System::VectorMatrixFunctionType getFunctionAndGradient();
+
     ///
     /// \brief Get the contact force vector
     ///

Source/Solvers/imstkNewtonSolver.cpp

@@ -72,13 +72,16 @@ NewtonSolver<SystemMatrix>::solveGivenState(Vectord& x)
 
     Vectord dx = x;
 
-    for (size_t i = 0; i < m_maxIterations; ++i)
+    const int maxIters = this->m_isSemiImplicit ? 1 : m_maxIterations;
+
+    for (size_t i = 0; i < maxIters; ++i)
     {
         if (fnorm < stopTolerance)
         {
             return;
         }
         this->updateJacobian(x);
+
         m_linearSolver->solve(dx);
         this->m_updateIterate(-dx, x);
 
@@ -123,6 +126,7 @@ NewtonSolver<SystemMatrix>::solve()
             error0 = error;
         }
 
+        // std::cout << "Num. of Newton Iterations: " << iterNum << "\tError ratio: " << error/error0 << ", " << error << " " << error0 << std::endl;
         if (error / error0 < epsilon && iterNum > 0)
         {
             // std::cout << "Num. of Newton Iterations: " << iterNum << "\tError ratio: " << error/error0 << ", " << error << " " << error0 << std::endl;
@@ -152,15 +156,24 @@ NewtonSolver<SystemMatrix>::updateJacobian(const Vectord& x)
         return -1;
     }
 
-    auto& A = this->m_nonLinearSystem->m_dF(x);
+    // auto& A = this->m_nonLinearSystem->m_dF(x);
+    // if (A.innerSize() == 0)
+    // {
+    //     LOG(WARNING) << "NewtonMethod::updateJacobian - Size of matrix is 0!";
+    //     return -1;
+    // }
+    //
+    // auto& b = this->m_nonLinearSystem->m_F(x, this->m_isSemiImplicit);
+
+    const auto& vecAndMat = this->m_nonLinearSystem->m_F_dF(x, this->m_isSemiImplicit);
+    auto&       b = *vecAndMat.first;
+    auto&       A = *vecAndMat.second;
     if (A.innerSize() == 0)
     {
         LOG(WARNING) << "NewtonMethod::updateJacobian - Size of matrix is 0!";
         return -1;
     }
 
-    auto& b = this->m_nonLinearSystem->m_F(x, this->m_isSemiImplicit);
-
     auto linearSystem = std::make_shared<typename LinearSolverType::LinearSystemType>(A, b);
     //linearSystem->setLinearProjectors(this->m_nonLinearSystem->getLinearProjectors()); /// \todo Left for near future reference. Clear in future.
     m_linearSolver->setSystem(linearSystem);

Source/Solvers/imstkNonLinearSystem.cpp

@@ -26,6 +26,15 @@ namespace imstk
 {
 template<typename Matrix>
 NonLinearSystem<Matrix>::NonLinearSystem(const VectorFunctionType& F, const MatrixFunctionType& dF) : m_F(F), m_dF(dF)
+{
+    this->m_F_dF = [this](const Vectord& x, const bool semiImplicit)
+                   {
+                       return std::make_pair(&(this->m_F(x, semiImplicit)), &(this->m_dF(x)));
+                   };
+}
+
+template<typename Matrix>
+NonLinearSystem<Matrix>::NonLinearSystem(const VectorFunctionType& F, const MatrixFunctionType& dF, const VectorMatrixFunctionType& F_dF) : m_F(F), m_dF(dF), m_F_dF(F_dF)
 {
 }
 

Source/Solvers/imstkNonLinearSystem.h

@@ -22,6 +22,7 @@
 #pragma once
 
 #include "imstkMath.h"
+#include <utility>
 
 namespace imstk
 {
@@ -36,7 +37,10 @@ class NonLinearSystem
 public:
     using VectorFunctionType = std::function<const Vectord& (const Vectord&, const bool)>;
     using MatrixFunctionType = std::function<const Matrix& (const Vectord&)>;
-    using UpdateFunctionType = std::function<void (const Vectord&, const bool)>;
+    // using VecMatPair = std::pair<const Vectord&, const Matrix&>;
+    using VecMatPair = std::pair<const Vectord*, const Matrix*>;
+    using VectorMatrixFunctionType    = std::function<VecMatPair(const Vectord&, const bool)>;
+    using UpdateFunctionType          = std::function<void (const Vectord&, const bool)>;
     using UpdatePrevStateFunctionType = std::function<void ()>;
 
 public:
@@ -45,6 +49,7 @@ public:
     ///
     NonLinearSystem() {};
     NonLinearSystem(const VectorFunctionType& F, const MatrixFunctionType& dF);
+    NonLinearSystem(const VectorFunctionType& F, const MatrixFunctionType& dF, const VectorMatrixFunctionType& F_dF);
 
     virtual ~NonLinearSystem() {};
 
@@ -129,8 +134,9 @@ public:
     }*/
 
 public:
-    VectorFunctionType m_F;  ///> Nonlinear function
-    MatrixFunctionType m_dF; ///> Gradient of the Nonlinear function with respect to the unknown vector
+    VectorFunctionType       m_F;  ///> Nonlinear function
+    MatrixFunctionType       m_dF; ///> Gradient of the Nonlinear function with respect to the unknown vector
+    VectorMatrixFunctionType m_F_dF;
     Vectord* m_unknown = nullptr;
 
     UpdateFunctionType m_FUpdate;