diff --git a/Base/ForceModel/imstkFEElasticityForceModel.h b/Base/ForceModel/imstkFEElasticityForceModel.h
index 9944df44387c57d48973217a6822dd5f3dabc138..c4834a58b088036c6225e77eaeaf7a1ff07c0056 100644
--- a/Base/ForceModel/imstkFEElasticityForceModel.h
+++ b/Base/ForceModel/imstkFEElasticityForceModel.h
@@ -24,6 +24,7 @@
#include "g3log/g3log.hpp"
+#include "imstkInternalForceModel.h"
#include "imstkMath.h"
// Vega
@@ -45,7 +46,7 @@ public:
///
/// \brief Constructor
///
- FEElasticityForceModel(const TimeIntegrator::Type type);
+ FEElasticityForceModel();
///
/// \brief Destructor
@@ -55,9 +56,9 @@ public:
void getInternalForce(Vectord& u, Vectord& internalForce) override
{
m_feElasticForceModel->GetInternalForce(u.data(), internalForce.data());
- };
+ }
- void getTangentStiffnessMatrix(Vectord& u, std::shared_ptr<SparseMatrixd> tangentStiffnessMatrix) override
+ void getTangentStiffnessMatrix(Vectord& u, SparseMatrixd& tangentStiffnessMatrix) override
{
m_feElasticForceModel->GetTangentStiffnessMatrixTopology(u.data(), );
}
diff --git a/Base/SceneElements/Objects/imstkDeformableBodyModel.cpp b/Base/SceneElements/Objects/imstkDeformableBodyModel.cpp
index e13709c5f7c7214a7c2e4d9e726afc798c249434..1c3fd16dca39743d1019536e3eb250f1123945f6 100644
--- a/Base/SceneElements/Objects/imstkDeformableBodyModel.cpp
+++ b/Base/SceneElements/Objects/imstkDeformableBodyModel.cpp
@@ -19,27 +19,402 @@
=========================================================================*/
+#include <fstream>
+
#include "imstkDeformableBodyModel.h"
+#include "generateMassMatrix.h"
namespace imstk
{
-void DeformableBodyModel::configure(const std::string& configFileName)
+DeformableBodyModel::DeformableBodyModel() : DynamicalModel(DynamicalModel::Type::elastoDynamics), m_damped(false){}
+
+void
+DeformableBodyModel::setForceModelConfiguration(std::shared_ptr<ForceModelConfig> fmConfig)
+{
+ m_forceModelConfiguration = fmConfig;
+}
+
+std::shared_ptr<imstk::ForceModelConfig>
+DeformableBodyModel::getForceModelConfiguration() const
+{
+ return m_forceModelConfiguration;
+}
+
+void
+DeformableBodyModel::setInternalForceModel(std::shared_ptr<InternalForceModel> fm)
+{
+ m_internalForceModel = fm;
+}
+
+std::shared_ptr<imstk::InternalForceModel>
+DeformableBodyModel::getInternalForceModel() const
+{
+ return m_internalForceModel;
+}
+
+void
+DeformableBodyModel::setTimeIntegrator(std::shared_ptr<TimeIntegrator> timeIntegrator)
+{
+ m_timeIntegrator = timeIntegrator;
+}
+
+std::shared_ptr<imstk::TimeIntegrator>
+DeformableBodyModel::getTimeIntegrator() const
+{
+ return m_timeIntegrator;
+}
+
+void
+DeformableBodyModel::setModelGeometry(std::shared_ptr<Geometry> geometry)
+{
+ m_forceModelGeometry = geometry;
+}
+
+std::shared_ptr<imstk::Geometry>
+DeformableBodyModel::getModelGeometry()
+{
+ return m_forceModelGeometry;
+}
+
+void
+DeformableBodyModel::configure(const std::string& configFileName)
{
m_forceModelConfiguration = std::make_shared<ForceModelConfig>(configFileName);
}
-void DeformableBodyModel::initializeForceModel()
+bool
+DeformableBodyModel::loadBoundaryConditions()
+{
+ auto fileName = m_forceModelConfiguration->getStringOptionsMap().at("fixedDOFFilename");
+
+ if (fileName.empty())
+ {
+ LOG(WARNING) << "DeformableBodyModel::loadBoundaryConditions: The external boundary conditions file name is empty";
+ return false;
+ }
+ else
+ {
+ std::ifstream file(fileName.data());
+
+ if (file.is_open())
+ {
+ size_t index;
+ while (!file.eof())
+ {
+ file >> index;
+ m_fixedNodeIds.emplace_back(index);
+ }
+ }
+ else
+ {
+ LOG(WARNING) << "DeformableBodyModel::loadBoundaryConditions: Could not open external file with boundary conditions";
+ return false;
+ }
+
+ file.close();
+ return true;
+ }
+}
+
+void
+DeformableBodyModel::initializeForceModel()
+{
+ auto imstkVolMesh = std::static_pointer_cast<VolumetricMesh>(m_forceModelGeometry);
+ std::shared_ptr<vega::VolumetricMesh> vegaMesh = VegaMeshReader::getVegaVolumeMeshFromVolumeMesh(imstkVolMesh);
+
+ switch (m_forceModelConfiguration->getForceModelType())
+ {
+ case ForceModelType::StVK:
+
+ this->m_internalForceModel = std::make_shared<StVKForceModel>(vegaMesh);
+ break;
+
+ case ForceModelType::Linear:
+
+ this->m_internalForceModel = std::make_shared<LinearFEMForceModel>(vegaMesh);
+ break;
+
+ case ForceModelType::Corotational:
+
+ this->m_internalForceModel = std::make_shared<CorotationalFEMForceModel>(vegaMesh);
+ break;
+
+ case ForceModelType::Invertible:
+
+ this->m_internalForceModel = std::make_shared<IsotropicHyperelasticFEForceModel>(vegaMesh, -MAX_D);
+ break;
+
+ default:
+ LOG(WARNING) << "DeformableBodyModel::initializeForceModel: Unknown force model type";
+
+ } //switch
+}
+
+void
+DeformableBodyModel::initializeMassMatrix(const bool saveToDisk /*= false*/)
+{
+ if (!this->m_forceModelGeometry)
+ {
+ LOG(INFO) << "DeformableBodyModel::initializeMassMatrix Force model geometry not set!";
+ return;
+ }
+
+ vega::SparseMatrix *vegaMatrix;
+ auto imstkVolMesh = std::static_pointer_cast<VolumetricMesh>(m_forceModelGeometry);
+ std::shared_ptr<vega::VolumetricMesh> vegaMesh = VegaMeshReader::getVegaVolumeMeshFromVolumeMesh(imstkVolMesh);
+ vega::GenerateMassMatrix::computeMassMatrix(vegaMesh.get(), &vegaMatrix, true);
+
+ auto rowLengths = vegaMatrix->GetRowLengths();
+ auto nonZeroValues = vegaMatrix->GetEntries();
+ auto columnIndices = vegaMatrix->GetColumnIndices();
+
+ std::vector<Eigen::Triplet<double>> triplets;
+ triplets.reserve(vegaMatrix->GetNumEntries());
+ for (int i = 0, end = vegaMatrix->GetNumRows(); i < end; ++i)
+ {
+ for (int k = 0, k_end = rowLengths[i]; k < k_end; ++k)
+ {
+ triplets.emplace_back(i, columnIndices[i][k], nonZeroValues[i][k]);
+ }
+ }
+ m_M.resize(vegaMatrix->GetNumRows(), vegaMatrix->GetNumColumns());
+ m_M.setFromTriplets(std::begin(triplets), std::end(triplets));
+ m_M.makeCompressed();
+
+ /*this->vegaMassMatrix.reset(vegaMatrix);
+
+ if (saveToDisk)
+ {
+ char name[] = "ComputedMassMatrix.mass";
+ this->vegaMassMatrix->Save(name);
+ }*/
+}
+
+void
+DeformableBodyModel::initializeDampingMatrix()
+{
+ auto dampingLaplacianCoefficient =
+ this->m_forceModelConfiguration->getFloatsOptionsMap().at("dampingLaplacianCoefficient");
+
+ auto dampingMassCoefficient =
+ this->m_forceModelConfiguration->getFloatsOptionsMap().at("dampingMassCoefficient");
+
+ if (dampingLaplacianCoefficient == 0.0 || dampingMassCoefficient == 0.0)
+ {
+ m_damped = false;
+ return;
+ }
+ else
+ {
+ m_damped = true;
+ }
+
+ if (dampingLaplacianCoefficient <= 0.0)
+ {
+ LOG(INFO) << "DeformableBodyModel::initializeDampingMatrix Damping coefficient is negative!";
+ return;
+ }
+
+ auto imstkVolMesh = std::static_pointer_cast<VolumetricMesh>(m_forceModelGeometry);
+ std::shared_ptr<vega::VolumetricMesh> vegaMesh = VegaMeshReader::getVegaVolumeMeshFromVolumeMesh(imstkVolMesh);
+
+ auto meshGraph = std::make_shared<vega::Graph>(*vega::GenerateMeshGraph::Generate(this->m_forceModelGeometry.get()));
+
+ if (!meshGraph)
+ {
+ // TODO: log this
+ return;
+ }
+
+ vega::SparseMatrix *matrix;
+ meshGraph->GetLaplacian(&matrix, 1);
+
+ if (!matrix)
+ {
+ // TODO: log this
+ return;
+ }
+
+ matrix->ScalarMultiply(dampingLaplacianCoefficient);
+
+ auto rowLengths = matrix->GetRowLengths();
+ auto nonZeroValues = matrix->GetEntries();
+ auto columnIndices = matrix->GetColumnIndices();
+
+ std::vector<Eigen::Triplet<double>> triplets;
+ triplets.reserve(matrix->GetNumEntries());
+ for (int i = 0, end = matrix->GetNumRows(); i < end; ++i)
+ {
+ for (int k = 0, k_end = rowLengths[i]; k < k_end; ++k)
+ {
+ triplets.emplace_back(i, columnIndices[i][k], nonZeroValues[i][k]);
+ }
+ }
+
+ m_C.resize(matrix->GetNumRows(), matrix->GetNumColumns());
+ m_C.setFromTriplets(std::begin(triplets), std::end(triplets));
+ m_C.makeCompressed();
+
+ //this->dampingMatrix.reset(matrix);
+}
+
+void
+DeformableBodyModel::initializeTangentStiffness()
{
+ if (!m_internalForceModel)
+ {
+ LOG(WARNING) << "Tangent stiffness cannot be initialized without force model";
+ return;
+ }
+
+ vega::SparseMatrix *matrix;
+ m_internalForceModel->GetTangentStiffnessMatrixTopology(&matrix);
+
+ if (!matrix)
+ {
+ // TODO: log this
+ return;
+ }
+
+ if (!this->vegaMassMatrix)
+ {
+ // TODO: log this
+ return;
+ }
+
+ matrix->BuildSubMatrixIndices(*this->vegaMassMatrix.get());
+
+ if (this->dampingMatrix)
+ {
+ matrix->BuildSubMatrixIndices(*this->dampingMatrix.get(), 1);
+ }
+
+ auto rowLengths = matrix->GetRowLengths();
+ auto columnIndices = matrix->GetColumnIndices();
+ std::vector<Eigen::Triplet<double>> triplets;
+ triplets.reserve(matrix->GetNumEntries());
+ for (int i = 0, end = matrix->GetNumRows(); i < end; ++i)
+ {
+ for (int k = 0, k_end = rowLengths[i]; k < k_end; ++k)
+ {
+ triplets.emplace_back(i, columnIndices[i][k], 0.001);
+ }
+ }
+ m_K.resize(matrix->GetNumRows(),
+ matrix->GetNumColumns());
+ m_K.setFromTriplets(std::begin(triplets), std::end(triplets));
+ m_K.makeCompressed();
+
+ this->vegaTangentStiffnessMatrix.reset(matrix);
+
+ const auto &dampingStiffnessCoefficient =
+ m_forceModelConfiguration->getFloatsOptionsMap().at("dampingStiffnessCoefficient");
+
+ const auto &dampingMassCoefficient =
+ m_forceModelConfiguration->getFloatsOptionsMap().at("dampingMassCoefficient");
+
+ // Initialize the Raleigh damping matrix
+ m_C = dampingMassCoefficient*m_M + m_K*dampingStiffnessCoefficient;
}
-void DeformableBodyModel::initializeGravity()
+void
+DeformableBodyModel::initializeGravity()
{
m_gravityForce.resize(m_numDOF);
m_gravityForce.setZero();
- float gravityStrength = m_forceModelConfiguration->getFloatsOptionsMap().at("gravity");
- this->volumetricMesh->computeGravity(this->gravity, this->gravityForce);
+ double gravity = m_forceModelConfiguration->getFloatsOptionsMap().at("gravity");
+
+ auto vegaMesh = VegaMeshReader::getVegaVolumeMeshFromVolumeMesh(std::static_pointer_cast<VolumetricMesh>(m_forceModelGeometry));
+ vegaMesh->computeGravity(m_gravityForce.data(), gravity);
+}
+
+void
+DeformableBodyModel::computeImplicitSystemRHS(const kinematicState& state,
+ const kinematicState& newState)
+{
+ // Do checks if there are uninitialized matrices
+
+ /*auto &M = this->evalMass(newState);
+ auto &K = this->evalDFx(newState);
+ auto &f = this->evalRHS(newState);*/
+
+ m_internalForceModel->getTangentStiffnessMatrix(newState.getQ(), m_K);
+
+ double timeStep = m_timeIntegrator->getTimestepSize();
+
+ m_Feff = m_M * (newState.getQDot() - state.getQDot()) / timeStep;
+ m_Feff -= m_K * (newState.getQ() - state.getQ() - newState.getQDot() * timeStep);
+ m_Feff += m_explicitExternalForce;
+ m_Feff += m_gravityForce;
+
+ if (m_damped)
+ {
+ m_Feff -= timeStep*this->Damping(newState)*newState.getQDot();
+ }
+ //state.applyBoundaryConditions(this->rhs);
+}
+
+void
+DeformableBodyModel::computeImplicitSystemLHS(const kinematicState& state,
+ const kinematicState& newState)
+{
+ // Do checks if there are uninitialized matrices
+
+
+ /*auto &M = this->evalMass(newState);
+ auto &K = this->evalDFx(newState);
+ auto &C = this->evalDFv(newState);*/
+
+ m_internalForceModel->getTangentStiffnessMatrix(newState.getQ(), m_K);
+
+ double timeStep = m_timeIntegrator->getTimestepSize();
+
+ m_Keff = (1.0 / timeStep) * m_M;
+ if (m_damped)
+ {
+ const auto &dampingStiffnessCoefficient =
+ m_forceModelConfiguration->getFloatsOptionsMap().at("dampingStiffnessCoefficient");
+
+ const auto &dampingMassCoefficient =
+ m_forceModelConfiguration->getFloatsOptionsMap().at("dampingMassCoefficient");
+
+ m_C = dampingMassCoefficient*m_M + m_K*dampingStiffnessCoefficient;
+ m_Keff += m_C;
+ }
+ m_Keff += timeStep * m_K;
+
+ //previousState.applyBoundaryConditions(this->systemMatrix);
+}
+
+void
+DeformableBodyModel::initializeExplicitExternalForces()
+{
+ m_explicitExternalForce.resize(m_numDOF);
+ m_explicitExternalForce.setZero();
+}
+
+NonLinearSystem::VectorFunctionType&
+DeformableBodyModel::getFunction(const Vectord& q)
+{
+ // Function to evaluate the nonlinear objective function.
+ return [&, this](const Vectord &) -> const Vectord&
+ {
+ computeImplicitSystemRHS(state, newState);
+ return m_Feff;
+ };
+}
+
+NonLinearSystem::MatrixFunctionType&
+DeformableBodyModel::getFunctionGradient(const Vectord& q)
+{
+ // Gradient of the nonlinear objective function.
+ return [&, this](const Vectord &) -> const SparseMatrixd&
+ {
+ computeImplicitSystemLHS(state, newState);
+ return m_Keff;
+ };
}
}
diff --git a/Base/SceneElements/Objects/imstkDeformableBodyModel.h b/Base/SceneElements/Objects/imstkDeformableBodyModel.h
index 06926cb4417db7236eebda9b5081dd002630a79b..f5308593df054ae8484098f18b6e0ecd3c8f7d09 100644
--- a/Base/SceneElements/Objects/imstkDeformableBodyModel.h
+++ b/Base/SceneElements/Objects/imstkDeformableBodyModel.h
@@ -24,14 +24,29 @@
#include <memory>
+#include "Eigen/Sparse"
+
#include "imstkGeometry.h"
#include "imstkDynamicalModel.h"
#include "imstkTimeIntegrator.h"
#include "imstkInternalForceModel.h"
#include "imstkForceModelConfig.h"
-#include "imstkKinematicState.h"
+#include "imstkProblemState.h"
+#include "imstkNonlinearSystem.h"
+#include "imstkVegaMeshReader.h"
+
+//force models
+#include "imstkStVKForceModel.h"
+#include "imstkLinearFEMForceModel.h"
+#include "imstkCorotationalFEMForceModel.h"
+#include "imstkIsotropicHyperelasticFEMForceModel.h"
+#include "imstkVegaMeshReader.h"
-namespace imstk {
+//vega
+#include "sparseMatrix.h"
+
+namespace imstk
+{
///
/// \class DeformableBodyModel
@@ -51,11 +66,6 @@ public:
///
virtual ~DeformableBodyModel() = default;
- ///
- /// \brief Set the geometry on which the force model acts
- ///
- void setForceModelGeometry(std::shared_ptr<Geometry> fmGeometry);
-
///
/// \brief Set/Get force model configuration
///
@@ -63,10 +73,10 @@ public:
std::shared_ptr<ForceModelConfig> getForceModelConfiguration() const;
///
- /// \brief Set/Get force model
+ /// \brief Set/Get internal force model
///
- void setForceModel(std::shared_ptr<InternalForceModel> fm);
- std::shared_ptr<InternalForceModel> getForceModel() const;
+ void setInternalForceModel(std::shared_ptr<InternalForceModel> fm);
+ std::shared_ptr<InternalForceModel> getInternalForceModel() const;
///
/// \brief Set/Get time integrator
@@ -75,15 +85,26 @@ public:
std::shared_ptr<TimeIntegrator> getTimeIntegrator() const;
///
- /// \brief Returns the tangent linear system for a given state
+ /// \brief Set/Get the geometry used by force model
///
- void getLinearSystem();
+ void setModelGeometry(std::shared_ptr<Geometry> geometry);
+ std::shared_ptr<Geometry> getModelGeometry();
+
+ ///
+ /// \brief Returns the tangent linear system given curent state
+ ///
+ void getTangent(Vectord& q);
///
/// \brief Configure the force model from external file
///
void configure(const std::string& configFileName);
+ ///
+ /// \brief Initialize the deformable body model
+ ///
+ void initialize();
+
///
/// \brief Load the boundary conditions from external file
///
@@ -114,31 +135,59 @@ public:
///
void initializeGravity();
+ ///
+ /// \brief Compute the RHS of the resulting linear system
+ ///
+ void computeImplicitSystemRHS(const kinematicState& prevState,
+ const kinematicState& newState);
+
+ ///
+ /// \brief Compute the LHS of the resulting linear system
+ ///
+ void computeImplicitSystemLHS(const kinematicState& prevState,
+ const kinematicState& newState);
///
/// \brief Initialize explicit external forces
///
void initializeExplicitExternalForces();
-protected:
+ ///
+ /// \brief Returns the "function" that evaluates the nonlinear function given
+ /// the state vector
+ ///
+ NonLinearSystem::VectorFunctionType& getFunction(const Vectord& q);
- std::shared_ptr<ForceModelConfig> m_forceModelConfiguration; ///> Store the configuration here
+ ///
+ /// \brief Returns the "function" that evaluates the gradient of the nonlinear
+ /// function given the state vector
+ ///
+ NonLinearSystem::MatrixFunctionType& getFunctionGradient(const Vectord& q);
+protected:
std::shared_ptr<InternalForceModel> m_internalForceModel; ///> Mathematical model for intenal forces
std::shared_ptr<TimeIntegrator> m_timeIntegrator; ///> Time integrator
- std::shared_ptr<Geometry> m_forceModelGeometry; ///> Geometry used by force model
+ std::shared_ptr<ForceModelConfig> m_forceModelConfiguration; ///> Store the configuration here
+ std::shared_ptr<Geometry> m_forceModelGeometry; ///> Geometry used by force model
+
+ bool m_damped;
/// Matrices typical to a elastodynamics and 2nd order analogous systems
- std::shared_ptr<SparseMatrixd> m_M; ///> Mass matrix
- std::shared_ptr<SparseMatrixd> m_C; ///> Damping coefficient matrix
- std::shared_ptr<SparseMatrixd> m_K; ///> Tangent (derivative of internal force w.r.t displacements) stiffness matrix
- std::shared_ptr<SparseMatrixd> m_Keff; ///> Effective stiffness matrix (dependent on internal force model and time integrator)
+ SparseMatrixd m_M; ///> Mass matrix
+ SparseMatrixd m_C; ///> Damping coefficient matrix
+ SparseMatrixd m_K; ///> Tangent (derivative of internal force w.r.t displacements) stiffness matrix
+ SparseMatrixd m_Keff; ///> Effective stiffness matrix (dependent on internal force model and time integrator)
+
+ Vectord m_Feff; ///> Vector of gravity forces
// External field forces
Vectord m_gravityForce; ///> Vector of gravity forces
// Explicit external forces
Vectord m_explicitExternalForce; ///> Vector of explicitly defined external forces
+
+ // Dirichlet boundary conditions
+ std::vector<std::size_t> m_fixedNodeIds;
};
} // imstk