imstkFemDeformableBodyModel.h

/*
** This file is part of the Interactive Medical Simulation Toolkit (iMSTK)
** iMSTK is distributed under the Apache License, Version 2.0.
** See accompanying NOTICE for details.
*/

#pragma once

#include "imstkDynamicalModel.h"
#include "imstkInternalForceModelTypes.h"
#include "imstkVectorizedState.h"
#include "imstkNonLinearSystem.h"

#include <sparseMatrix.h>

namespace vega
{
class VolumetricMesh;
} // namespace vega

namespace imstk
{
class InternalForceModel;
class TimeIntegrator;
class SolverBase;
class VegaMeshIO;
template<typename T, int N> class VecDataArray;

struct FemModelConfig
{
    FeMethodType m_femMethod = FeMethodType::Invertible;
    HyperElasticMaterialType m_hyperElasticMaterialType = HyperElasticMaterialType::StVK;

    // file names (remove from here?)
    std::string m_fixedDOFFilename;
    std::vector<std::size_t> m_fixedNodeIds;

    double m_dampingMassCoefficient      = 0.1;
    double m_dampingStiffnessCoefficient = 0.01;
    double m_dampingLaplacianCoefficient = 0.0;
    double m_deformationCompliance       = 1.0;
    double m_compressionResistance       = 500.0;
    double m_inversionThreshold = -std::numeric_limits<double>::max();
    double m_gravity = 9.81;
};

class FemDeformableBodyModel : public DynamicalModel<FeDeformBodyState>
{
public:
    using kinematicState = FeDeformBodyState;
    using System = NonLinearSystem<SparseMatrixd>;

public:
    FemDeformableBodyModel();
    ~FemDeformableBodyModel() override;

public:
    void configure(const std::string& configFileName);
    void configure(std::shared_ptr<FemModelConfig> config = std::make_shared<FemModelConfig>());

    bool initialize() override;

    void setForceModelConfiguration(std::shared_ptr<FemModelConfig> fmConfig) { this->m_FEModelConfig = fmConfig; }
    std::shared_ptr<FemModelConfig> getForceModelConfiguration() const { return m_FEModelConfig; }

    void setInternalForceModel(std::shared_ptr<InternalForceModel> fm) { m_internalForceModel = fm; }
    std::shared_ptr<InternalForceModel> getInternalForceModel() const { return m_internalForceModel; }

    void setTimeIntegrator(std::shared_ptr<TimeIntegrator> timeIntegrator) { this->m_timeIntegrator = timeIntegrator; }
    std::shared_ptr<TimeIntegrator> getTimeIntegrator() const { return m_timeIntegrator; }

    void loadInitialStates();

    bool loadBoundaryConditions();

    bool initializeForceModel();

    bool initializeMassMatrix();

    bool initializeDampingMatrix();

    bool initializeTangentStiffness();

    bool initializeGravityForce();

    bool initializeExplicitExternalForces();

    static void initializeEigenMatrixFromVegaMatrix(const vega::SparseMatrix& vegaMatrix, SparseMatrixd& eigenMatrix);

    void computeImplicitSystemRHS(kinematicState& prevState, kinematicState& newState, const StateUpdateType updateType);

    void computeSemiImplicitSystemRHS(kinematicState& stateAtT, kinematicState& newState, const StateUpdateType updateType);

    void computeImplicitSystemLHS(const kinematicState& prevState, kinematicState& newState, const StateUpdateType updateType);

    void computeSemiImplicitSystemRHSAndLHS(kinematicState& prevState, kinematicState& newState, const StateUpdateType updateType);

    void computeImplicitSystemRHSAndLHS(kinematicState& prevState, kinematicState& newState, const StateUpdateType updateType);

    void updateDampingMatrix();

    void applyBoundaryConditions(SparseMatrixd& M, const bool withCompliance = false) const;
    void applyBoundaryConditions(Vectord& x) const;

    void updateMassMatrix();

    void updatePhysicsGeometry() override;

    void updateBodyStates(const Vectord& solution, const StateUpdateType updateType);
    void updateBodyIntermediateStates(const Vectord& solution, const StateUpdateType updateType);

    void updateBodyPreviousStates();

    System::VectorFunctionType getFunction();

    System::UpdateFunctionType getUpdateFunction();
    System::UpdatePrevStateFunctionType getUpdatePrevStateFunction();

    System::MatrixFunctionType getFunctionGradient();

    System::VectorMatrixFunctionType getFunctionAndGradient();

    Vectord& getContactForce() { return m_Fcontact; }

    Vectord& getUnknownVec() { return m_qSol; }

    void setUpdateType(const StateUpdateType& updateType) { m_updateType = updateType; }
    const StateUpdateType& getUpdateType() const { return m_updateType; }

    std::vector<std::size_t>& getFixNodeIds() { return m_fixedNodeIds; }

    virtual void setTimeStep(const double timeStep);

    virtual double getTimeStep() const;

    void setFixedSizeTimeStepping();

    void enableFixedBC() { m_implementFixedBC = true; };
    void disableFixedBC() { m_implementFixedBC = false; };
    bool isFixedBCImplemented() const { return m_implementFixedBC; };

    std::shared_ptr<TaskNode> getSolveNode() const { return m_solveNode; }

    std::shared_ptr<SolverBase> getSolver() const { return m_solver; }
    void setSolver(std::shared_ptr<SolverBase> solver) { this->m_solver = solver; }

protected:
    void initGraphEdges(std::shared_ptr<TaskNode> source, std::shared_ptr<TaskNode> sink) override;

    std::shared_ptr<SolverBase> m_solver = nullptr;
    std::shared_ptr<InternalForceModel> m_internalForceModel = nullptr;          
    std::shared_ptr<TimeIntegrator>     m_timeIntegrator     = nullptr;          
    std::shared_ptr<NonLinearSystem<SparseMatrixd>> m_nonLinearSystem = nullptr; 

    std::shared_ptr<FemModelConfig> m_FEModelConfig = nullptr;

    SparseMatrixd m_M;                                                            
    SparseMatrixd m_C;                                                            
    SparseMatrixd m_K;                                                            
    SparseMatrixd m_Keff;                                                         

    Vectord m_Finternal;                                                          
    Vectord m_Feff;                                                               
    Vectord m_Fcontact;                                                           
    Vectord m_Fgravity;                                                           
    Vectord m_FexplicitExternal;                                                  
    Vectord m_qSol;                                                               

    std::shared_ptr<vega::VolumetricMesh> m_vegaPhysicsMesh = nullptr;            
    std::shared_ptr<vega::SparseMatrix>   m_vegaMassMatrix  = nullptr;            
    std::shared_ptr<vega::SparseMatrix>   m_vegaTangentStiffnessMatrix = nullptr; 
    std::shared_ptr<vega::SparseMatrix>   m_vegaDampingMatrix = nullptr;          

    std::vector<std::size_t> m_fixedNodeIds;                                      

    StateUpdateType m_updateType = StateUpdateType::DeltaVelocity;                

    bool m_damped = false;                                                        

    // 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;

private:
    std::shared_ptr<TaskNode> m_solveNode = nullptr;
};
} // namespace imstk