Commit c9f002ec authored by Sreekanth Arikatla's avatar Sreekanth Arikatla
Browse files

Merge branch 'newton-semiimplicit' into 'master'

Limit maxIterations to 1 for semi-implicit stepping

See merge request iMSTK/iMSTK!567
parents c5a752c4 e9f92930
......@@ -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
......@@ -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
......
......@@ -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
///
......
......@@ -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
///
......
......@@ -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()
{
......
......@@ -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
///
......
......@@ -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);
......
......@@ -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)
{
}
......
......@@ -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;
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment