Merge branch 'streamlineInitOfScene' into 'master'

Streamline initialization of scene and its elements

See merge request !204

Base/DynamicalModels/ObjectModels/imstkDynamicalModel.h

@@ -119,6 +119,11 @@ public:
     ///
     virtual void updatePhysicsGeometry(){};
 
+    ///
+    /// \brief Initialize the dynamical model
+    ///
+    virtual bool initialize() = 0;
+
 protected:
 
     DynamicalModelType m_type; ///> Mathematical model type

Base/DynamicalModels/ObjectModels/imstkFEMDeformableBodyModel.cpp

@@ -89,28 +89,55 @@ FEMDeformableBodyModel::configure(const std::string& configFileName)
     m_forceModelConfiguration = std::make_shared<ForceModelConfig>(configFileName);
 }
 
-void
-FEMDeformableBodyModel::initialize(std::shared_ptr<VolumetricMesh> physicsMesh)
+bool
+FEMDeformableBodyModel::initialize()
 {
-    this->setModelGeometry(physicsMesh);
-
     // prerequisite of for successfully initializing
     if (!m_forceModelGeometry || !m_forceModelConfiguration)
     {
         LOG(WARNING) << "DeformableBodyModel::initialize: Physics mesh or force model configuration not set yet!";
-        return;
+        return false;
     }
 
+    auto physicsMesh = std::dynamic_pointer_cast<imstk::VolumetricMesh>(this->getModelGeometry());
     m_vegaPhysicsMesh = physicsMesh->getAttachedVegaMesh();
 
-    this->initializeForceModel();
-    this->initializeMassMatrix();
-    this->initializeDampingMatrix();
-    this->initializeTangentStiffness();
+    if (!this->initializeForceModel())
+    {
+        return false;
+    }
+    ;
+    if (!this->initializeMassMatrix())
+    {
+        return false;
+    }
+    ;
+    if (!this->initializeDampingMatrix())
+    {
+        return false;
+    }
+    ;
+    if (!this->initializeTangentStiffness())
+    {
+        return false;
+    }
+    ;
     this->loadInitialStates();
-    this->loadBoundaryConditions();
-    this->initializeGravityForce();
-    this->initializeExplicitExternalForces();
+    if (!this->loadBoundaryConditions())
+    {
+        return false;
+    }
+    ;
+    if (!this->initializeGravityForce())
+    {
+        return false;
+    }
+    ;
+    if (!this->initializeExplicitExternalForces())
+    {
+        return false;
+    }
+    ;
 
     m_Feff.resize(m_numDOF);
     m_Finternal.resize(m_numDOF);
@@ -119,18 +146,25 @@ FEMDeformableBodyModel::initialize(std::shared_ptr<VolumetricMesh> physicsMesh)
     m_Fcontact.setConstant(0.0);
     m_qSol.resize(m_numDOF);
     m_qSol.setConstant(0.0);
+
+    return true;
 }
 
 void
 FEMDeformableBodyModel::loadInitialStates()
 {
+    if (m_numDOF == 0)
+    {
+        LOG(WARNING) << "FEMDeformableBodyModel::loadInitialStates() - Num. of degree of freedom is zero!";
+    }
+
     // For now the initial states are set to zero
     m_initialState = std::make_shared<kinematicState>(m_numDOF);
     m_previousState = std::make_shared<kinematicState>(m_numDOF);
     m_currentState = std::make_shared<kinematicState>(m_numDOF);
 }
 
-void
+bool
 FEMDeformableBodyModel::loadBoundaryConditions()
 {
     auto fileName = m_forceModelConfiguration->getStringOptionsMap().at("fixedDOFFilename");
@@ -138,7 +172,7 @@ FEMDeformableBodyModel::loadBoundaryConditions()
     if (fileName.empty())
     {
         LOG(WARNING) << "DeformableBodyModel::loadBoundaryConditions: The external boundary conditions file name is empty";
-        return;
+        return false;
     }
     else
     {
@@ -147,7 +181,6 @@ FEMDeformableBodyModel::loadBoundaryConditions()
         if (file.peek() == std::ifstream::traits_type::eof())
         {
             LOG(INFO) << "DeformableBodyModel::loadBoundaryConditions: The external boundary conditions file is empty";
-            return;
         }
 
         if (file.is_open())
@@ -165,6 +198,7 @@ FEMDeformableBodyModel::loadBoundaryConditions()
                 {
                     LOG(WARNING) << "FEMDeformableBodyModel::loadBoundaryConditions(): " <<
                         "The boundary condition node id provided is greater than number of nodes and hence excluded!!";
+                    return false;
                 }
             }
 
@@ -174,11 +208,13 @@ FEMDeformableBodyModel::loadBoundaryConditions()
         else
         {
             LOG(WARNING) << "DeformableBodyModel::loadBoundaryConditions: Could not open boundary conditions file!";
+            return false;
         }
     }
+    return true;
 }
 
-void
+bool
 FEMDeformableBodyModel::initializeForceModel()
 {
     const double g = m_forceModelConfiguration->getFloatsOptionsMap().at("gravity");
@@ -215,16 +251,19 @@ FEMDeformableBodyModel::initializeForceModel()
 
     default:
         LOG(WARNING) << "DeformableBodyModel::initializeForceModel: Unknown force model type";
+        return false;
     } //switch
+
+    return true;
 }
 
-void
+bool
 FEMDeformableBodyModel::initializeMassMatrix(const bool saveToDisk /*= false*/)
 {
     if (!m_forceModelGeometry)
     {
         LOG(WARNING) << "DeformableBodyModel::initializeMassMatrix Force model geometry not set!";
-        return;
+        return false;
     }
 
     vega::SparseMatrix *vegaMatrix;
@@ -235,9 +274,11 @@ FEMDeformableBodyModel::initializeMassMatrix(const bool saveToDisk /*= false*/)
     this->initializeEigenMatrixFromVegaMatrix(*vegaMatrix, m_M);
 
     // TODO: Add option to save mass matrix to file
+
+    return true;
 }
 
-void
+bool
 FEMDeformableBodyModel::initializeDampingMatrix()
 {
     auto dampingLaplacianCoefficient = m_forceModelConfiguration->getFloatsOptionsMap().at("dampingLaplacianCoefficient");
@@ -248,13 +289,13 @@ FEMDeformableBodyModel::initializeDampingMatrix()
 
     if (!m_damped)
     {
-        return;
+        return false;
     }
 
-    if (dampingLaplacianCoefficient <= 0.0)
+    if (dampingLaplacianCoefficient < 0.0)
     {
         LOG(WARNING) << "DeformableBodyModel::initializeDampingMatrix Damping coefficient is negative!";
-        return;
+        return false;
     }
 
     auto imstkVolMesh = std::static_pointer_cast<VolumetricMesh>(m_forceModelGeometry);
@@ -265,7 +306,7 @@ FEMDeformableBodyModel::initializeDampingMatrix()
     if (!meshGraph)
     {
         LOG(WARNING) << "DeformableBodyModel::initializeDampingMatrix: Mesh graph not avaliable!";
-        return;
+        return false;
     }
 
     vega::SparseMatrix *matrix;
@@ -274,7 +315,7 @@ FEMDeformableBodyModel::initializeDampingMatrix()
     if (!matrix)
     {
         LOG(WARNING) << "DeformableBodyModel::initializeDampingMatrix: Mesh Laplacian not avaliable!";
-        return;
+        return false;
     }
 
     matrix->ScalarMultiply(dampingLaplacianCoefficient);
@@ -282,15 +323,17 @@ FEMDeformableBodyModel::initializeDampingMatrix()
     m_vegaDampingMatrix.reset(matrix);
 
     this->initializeEigenMatrixFromVegaMatrix(*matrix, m_C);
+
+    return true;
 }
 
-void
+bool
 FEMDeformableBodyModel::initializeTangentStiffness()
 {
     if (!m_internalForceModel)
     {
         LOG(WARNING) << "DeformableBodyModel::initializeTangentStiffness: Tangent stiffness cannot be initialized without force model";
-        return;
+        return false;
     }
 
     vega::SparseMatrix *matrix;
@@ -299,13 +342,13 @@ FEMDeformableBodyModel::initializeTangentStiffness()
     if (!matrix)
     {
         LOG(WARNING) << "DeformableBodyModel::initializeTangentStiffness - Tangent stiffness matrix topology not avaliable!";
-        return;
+        return false;
     }
 
     if (!m_vegaMassMatrix)
     {
         LOG(WARNING) << "DeformableBodyModel::initializeTangentStiffness - Vega mass matrix doesn't exist!";
-        return;
+        return false;
     }
 
     matrix->BuildSubMatrixIndices(*m_vegaMassMatrix.get());
@@ -329,9 +372,11 @@ FEMDeformableBodyModel::initializeTangentStiffness()
     }
 
     m_internalForceModel->setTangentStiffness(m_vegaTangentStiffnessMatrix);
+
+    return true;
 }
 
-void
+bool
 FEMDeformableBodyModel::initializeGravityForce()
 {
     m_Fgravity.resize(m_numDOF);
@@ -339,6 +384,8 @@ FEMDeformableBodyModel::initializeGravityForce()
     const double gravity = m_forceModelConfiguration->getFloatsOptionsMap().at("gravity");
 
     m_vegaPhysicsMesh->computeGravity(m_Fgravity.data(), gravity);
+
+    return true;
 }
 
 void
@@ -448,11 +495,13 @@ FEMDeformableBodyModel::computeImplicitSystemLHS(const kinematicState& stateAtT,
     }
 }
 
-void
+bool
 FEMDeformableBodyModel::initializeExplicitExternalForces()
 {
     m_FexplicitExternal.resize(m_numDOF);
     m_FexplicitExternal.setZero();
+
+    return true;
 }
 
 void

Base/DynamicalModels/ObjectModels/imstkFEMDeformableBodyModel.h

@@ -100,7 +100,7 @@ public:
     ///
     /// \brief Initialize the deformable body model
     ///
-    void initialize(std::shared_ptr<VolumetricMesh> physicsMesh);
+    bool initialize() override;
 
     ///
     /// \brief Load the initial conditions of the deformable object
@@ -110,37 +110,37 @@ public:
     ///
     /// \brief Load the boundary conditions from external file
     ///
-    void loadBoundaryConditions();
+    bool loadBoundaryConditions();
 
     ///
     /// \brief Initialize the force model
     ///
-    void initializeForceModel();
+    bool initializeForceModel();
 
     ///
     /// \brief Initialize the mass matrix from the mesh
     ///
-    void initializeMassMatrix(const bool saveToDisk = false);
+    bool initializeMassMatrix(const bool saveToDisk = false);
 
     ///
     /// \brief Initialize the damping (combines structural and viscous damping) matrix
     ///
-    void initializeDampingMatrix();
+    bool initializeDampingMatrix();
 
     ///
     /// \brief Initialize the tangent stiffness matrix
     ///
-    void initializeTangentStiffness();
+    bool initializeTangentStiffness();
 
     ///
     /// \brief Initialize the gravity force
     ///
-    void initializeGravityForce();
+    bool initializeGravityForce();
 
     ///
     /// \brief Initialize explicit external forces
     ///
-    void initializeExplicitExternalForces();
+    bool initializeExplicitExternalForces();
 
     ///
     /// \brief Initialize the Eigen matrix with data inside vega sparse matrix

Base/DynamicalModels/ObjectModels/imstkPbdModel.cpp

@@ -37,9 +37,6 @@ namespace imstk
 PbdModel::PbdModel() :
     DynamicalModel(DynamicalModelType::positionBasedDynamics)
 {
-    m_initialState = std::make_shared<PbdState>();
-    m_previousState = std::make_shared<PbdState>();
-    m_currentState = std::make_shared<PbdState>();
 }
 
 void
@@ -48,11 +45,60 @@ PbdModel::setModelGeometry(std::shared_ptr<PointSet> m)
     m_mesh = m;
 }
 
+bool
+PbdModel::configure(const int nCons, ...)
+{
+    va_list args;
+    va_start(args, nCons);
+    for (int i = 0; i < nCons; ++i)
+    {
+        m_constraintConfig.push_back(std::string(va_arg(args, char*)));
+    }
+
+    m_uniformMassValue = va_arg(args, double);
+
+    if (nCons > 0)
+    {
+        char *sgrav = va_arg(args, char*);
+        std::stringstream gStream(sgrav);
+        float x, y, z;
+        gStream >> x;
+        gStream >> y;
+        gStream >> z;
+        Vec3d g(x,y,z);
+        this->setGravity(g);
+
+        this->setTimeStep(va_arg(args, double));
+
+        char *s = va_arg(args, char*);
+        if (strlen(s) > 0)
+        {
+            std::stringstream stream(s);
+            size_t n;
+            while (stream >> n)
+            {
+                m_fixedNodeIds.push_back(n-1);
+            }
+        }
+        this->setMaxNumIterations(va_arg(args, int));
+    }
+    this->setProximity(va_arg(args, double));
+    this->setContactStiffness(va_arg(args, double));
+    this->setNumDegreeOfFreedom(this->getModelGeometry()->getNumVertices() * 3);
+    va_end(args);
+
+    return true;
+}
+
 bool
 PbdModel::initialize()
 {
     if (m_mesh)
     {
+        m_initialState = std::make_shared<PbdState>();
+        m_previousState = std::make_shared<PbdState>();
+        m_currentState = std::make_shared<PbdState>();
+
         bool option[3] = { 1, 0, 0 };
         m_initialState->initialize(m_mesh, option);
         m_previousState->initialize(m_mesh, option);
@@ -64,19 +110,139 @@ PbdModel::initialize()
         m_currentState->setPositions(m_mesh->getVertexPositions());
 
         auto nP = m_mesh->getNumVertices();
-        m_invMass.resize(nP, 0);
+
         m_mass.resize(nP, 0);
+        this->setUniformMass(m_uniformMassValue);
 
-        return true;
+        m_invMass.resize(nP, 0);
+        for (auto i :m_fixedNodeIds)
+        {
+            setFixedPoint(i);
+        }
     }
     else
     {
         LOG(WARNING) << "Model geometry is not yet set! Cannot initialize without model geometry.";
         return false;
     }
+
+    auto nCons = m_constraintConfig.size();
+    for (int i = 0; i < nCons; ++i)
+    {
+        auto s = m_constraintConfig.at(i).c_str();
+        int len = 0;
+        while (s[len] != ' ' && s[len] != '\0')
+        {
+            ++len;
+        }
+
+        if (strncmp("FEM", &s[0], len) == 0)
+        {
+            int pos = len + 1;
+            len = 0;
+            while (s[pos + len] != ' ' && s[pos + len] != '\0')
+            {
+                ++len;
+            }
+
+            if (strncmp("Corotation", &s[pos], len) == 0)
+            {
+                if (!this->initializeFEMConstraints(PbdFEMConstraint::MaterialType::Corotation))
+                {
+                    return false;
+                }
+                ;
+            }
+            else if (strncmp("NeoHookean", &s[pos], len) == 0)
+            {
+                if (!this->initializeFEMConstraints(PbdFEMConstraint::MaterialType::NeoHookean))
+                {
+                    return false;
+                }
+                ;
+            }
+            else if (strncmp("Stvk", &s[pos], len) == 0)
+            {
+                if (!this->initializeFEMConstraints(PbdFEMConstraint::MaterialType::StVK))
+                {
+                    return false;
+                }
+                ;
+            }
+            else
+            { // default
+                if (!this->initializeFEMConstraints(PbdFEMConstraint::MaterialType::StVK))
+                {
+                    return false;
+                }
+                ;
+            }
+
+            float YoungModulus, PoissonRatio;
+            sscanf(&s[pos + len + 1], "%f %f", &YoungModulus, &PoissonRatio);
+            this->computeLameConstants(YoungModulus, PoissonRatio);
+        }
+        else if (strncmp("Volume", &s[0], len) == 0)
+        {
+            float stiffness;
+            sscanf(&s[len + 1], "%f", &stiffness);
+            if (!this->initializeVolumeConstraints(stiffness))
+            {
+                return false;
+            }
+            ;
+        }
+        else if (strncmp("Distance", &s[0], len) == 0)
+        {
+            float stiffness;
+            sscanf(&s[len + 1], "%f", &stiffness);
+            if (!this->initializeDistanceConstraints(stiffness))
+            {
+                return false;
+            }
+            ;
+        }
+        else if (strncmp("Area", &s[0], len) == 0)
+        {
+            float stiffness;
+            sscanf(&s[len + 1], "%f", &stiffness);
+            if (!this->initializeAreaConstraints(stiffness))
+            {
+                return false;
+            }
+            ;
+        }
+        else if (strncmp("Dihedral", &s[0], len) == 0)
+        {
+            float stiffness;
+            sscanf(&s[len + 1], "%f", &stiffness);
+            if (!this->initializeDihedralConstraints(stiffness))
+            {
+                return false;
+            }
+            ;
+        }
+        else if (strncmp("ConstantDensity", &s[0], len) == 0)
+        {
+            float stiffness;
+            sscanf(&s[len + 1], "%f", &stiffness);
+            if (!this->initializeConstantDensityConstraint(stiffness))
+            {
+                return false;
+            }
+            ;
+        }
+        else
+        {
+            LOG(WARNING) << "PbdModel::initialize() - Type of PBD constraints not identified!";
+            return false;
+        }
+    }
+    return true;
 }
 
-void PbdModel::computeLameConstants(const double& E, const double nu)
+void
+PbdModel::computeLameConstants(const double E, const double nu)
 {
     m_mu = E/(2*(1+nu));
     m_lambda = E*nu/((1-2*nu)*(1+nu));
@@ -108,7 +274,7 @@ PbdModel::initializeFEMConstraints(PbdFEMConstraint::MaterialType type)
 }
 
 bool
-PbdModel::initializeVolumeConstraints(const double& stiffness)
+PbdModel::initializeVolumeConstraints(const double stiffness)
 {
     // Check if constraint type matches the mesh type
     if (m_mesh->getType() != Geometry::Type::TetrahedralMesh)
@@ -133,7 +299,7 @@ PbdModel::initializeVolumeConstraints(const double& stiffness)
 }
 
 bool
-PbdModel::initializeDistanceConstraints(const double& stiffness)
+PbdModel::initializeDistanceConstraints(const double stiffness)
 {
     if (m_mesh->getType() == Geometry::Type::TetrahedralMesh)
     {
@@ -256,7 +422,7 @@ PbdModel::initializeDistanceConstraints(const double& stiffness)
 }
 
 bool
-PbdModel::initializeAreaConstraints(const double& stiffness)
+PbdModel::initializeAreaConstraints(const double stiffness)
 {
     // check if constraint type matches the mesh type
     if (m_mesh->getType() != Geometry::Type::SurfaceMesh)
@@ -281,7 +447,7 @@ PbdModel::initializeAreaConstraints(const double& stiffness)
 }
 
 bool
-PbdModel::initializeDihedralConstraints(const double& stiffness)
+PbdModel::initializeDihedralConstraints(const double stiffness)
 {
     if (m_mesh->getType() != Geometry::Type::SurfaceMesh)
     {
@@ -303,7 +469,10 @@ PbdModel::initializeDihedralConstraints(const double& stiffness)
         onering[tri[2]].push_back(k);
     }
 
-    std::vector<std::vector<bool>> E(triMesh->getNumVertices(), std::vector<bool>(triMesh->getNumVertices(), 1));
+    std::vector<std::vector<bool>> E(triMesh->getNumVertices(),
+                                     std::vector<bool>(triMesh->getNumVertices(),
+                                     1));
+
     for (size_t k = 0; k < elements.size(); ++k)
     {
         auto& tri = elements[k];
@@ -397,11 +566,14 @@ PbdModel::initializeDihedralConstraints(const double& stiffness)
 }
 
 bool
-PbdModel::initializeConstantDensityConstraint(const double& stiffness)
+PbdModel::initializeConstantDensityConstraint(const double stiffness)
 {
     // check if constraint type matches the mesh type
-    if (m_mesh->getType() != Geometry::Type::SurfaceMesh && m_mesh->getType() != Geometry::Type::TetrahedralMesh && m_mesh->getType() != Geometry::Type::LineMesh &&
-        m_mesh->getType() != Geometry::Type::