diff --git a/Source/DynamicalModels/ObjectModels/imstkReducedStVKBodyModel.cpp b/Source/DynamicalModels/ObjectModels/imstkReducedStVKBodyModel.cpp index 08f44745179276bf02d1f013f95c9045b62e05ab..26fa5f05d9752db528933007b12e8708ca33ffda 100644 --- a/Source/DynamicalModels/ObjectModels/imstkReducedStVKBodyModel.cpp +++ b/Source/DynamicalModels/ObjectModels/imstkReducedStVKBodyModel.cpp @@ -51,7 +51,7 @@ ReducedStVK::ReducedStVK() : DynamicalModel(DynamicalModelType::ElastoDynamics) { // m_fixedNodeIds.reserve(1000); - m_validGeometryTypes = { Geometry::Type::TetrahedralMesh, Geometry::Type::HexahedralMesh }; + m_validGeometryTypes = { Geometry::Type::TetrahedralMesh, Geometry::Type::HexahedralMesh }; m_solveNode = m_taskGraph->addFunction("FEMModel_Solve", [&]() { getSolver()->solve(); }); } @@ -92,7 +92,7 @@ ReducedStVK::configure(const std::string& configFileName) // Parse the configuration file CHECK(vegaConfigFileOptions.parseOptions(configFileName.data()) == 0) - << "ForceModelConfig::parseConfig - Unable to load the configuration file"; + << "ForceModelConfig::parseConfig - Unable to load the configuration file"; // get the root directory of the boundary file name // std::string rootDir; @@ -160,8 +160,8 @@ ReducedStVK::initialize() { // prerequisite of for successfully initializing CHECK(m_geometry != nullptr && m_config != nullptr) - << "DeformableBodyModel::initialize: Physics mesh or force model configuration not set " - "yet!"; + << "DeformableBodyModel::initialize: Physics mesh or force model configuration not set " + "yet!"; // Setup default solver if model wasn't assigned one if (m_solver == nullptr) @@ -185,13 +185,13 @@ ReducedStVK::initialize() this->readModalMatrix(m_config->m_modesFileName); this->loadInitialStates(); - auto physicsMesh = std::dynamic_pointer_cast<imstk::VolumetricMesh>(this->getModelGeometry()); + auto physicsMesh = std::dynamic_pointer_cast<imstk::VolumetricMesh>(this->getModelGeometry()); m_vegaPhysicsMesh = VegaMeshIO::convertVolumetricMeshToVegaMesh(physicsMesh); - CHECK(m_numDOF == m_vegaPhysicsMesh->getNumVertices()*3); + CHECK(m_numDOF == m_vegaPhysicsMesh->getNumVertices() * 3); - if (!this->initializeForceModel() || !this->initializeMassMatrix() || - !this->initializeTangentStiffness() || !this->initializeDampingMatrix() || - !this->initializeGravityForce() || !this->initializeExplicitExternalForces()) + if (!this->initializeForceModel() || !this->initializeMassMatrix() + || !this->initializeTangentStiffness() || !this->initializeDampingMatrix() + || !this->initializeGravityForce() || !this->initializeExplicitExternalForces()) { return false; } @@ -221,7 +221,7 @@ ReducedStVK::readModalMatrix(const std::string& fname) std::vector<float> Ufloat; int m, n; vega::ReadMatrixFromDisk_(fname.c_str(), m, n, Ufloat); - m_numDOF = m; + m_numDOF = m; m_numDOFReduced = n; std::vector<double> Udouble(Ufloat.size()); @@ -231,7 +231,7 @@ ReducedStVK::readModalMatrix(const std::string& fname) } m_modalMatrix = - std::make_shared<vega::ModalMatrix>(m_numDOF / 3, m_numDOFReduced, Udouble.data()); + std::make_shared<vega::ModalMatrix>(m_numDOF / 3, m_numDOFReduced, Udouble.data()); return; } @@ -256,7 +256,7 @@ ReducedStVK::loadInitialStates() bool ReducedStVK::initializeForceModel() { - const double g = m_config->m_gravity; + const double g = m_config->m_gravity; const bool isGravityPresent = (g > 0) ? true : false; // m_numDOFReduced = m_config->r; @@ -272,7 +272,7 @@ bool ReducedStVK::initializeMassMatrix() { CHECK(m_geometry != nullptr) - << "DeformableBodyModel::initializeMassMatrix Force model geometry not set!"; + << "DeformableBodyModel::initializeMassMatrix Force model geometry not set!"; this->m_massMatrix.resize(m_numDOFReduced * m_numDOFReduced, 0.0); // set M to identity @@ -308,8 +308,8 @@ bool ReducedStVK::initializeTangentStiffness() { CHECK(m_forceModel != nullptr) - << "DeformableBodyModel::initializeTangentStiffness: Tangent stiffness cannot be " - "initialized without force model"; + << "DeformableBodyModel::initializeTangentStiffness: Tangent stiffness cannot be " + "initialized without force model"; this->m_stiffnessMatrix.resize(m_numDOFReduced * m_numDOFReduced, 0.0); m_forceModel->GetTangentStiffnessMatrix(m_initialStateReduced->getQ().data(), @@ -350,30 +350,30 @@ ReducedStVK::computeImplicitSystemRHS(kinematicState& stateAtT, switch (updateType) { - case StateUpdateType::DeltaVelocity: - - m_Feff = m_K * -(uPrev - u + v * dT); - - if (m_damped) - { - m_Feff -= m_C * v; - } - - m_forceModel->GetInternalForce(u.data(), m_Finternal.data()); - m_Feff -= m_Finternal; - this->project(m_FexplicitExternal, m_FexplicitExternalReduced); - m_Feff += m_FexplicitExternalReduced; - // the reduced gravity has alredy been initialized in initializeGravityForce - // this->project(m_Fgravity, m_FgravityReduced); - m_Feff += m_FgravityReduced; - this->project(m_Fcontact, m_FcontactReduced); - m_Feff += m_FcontactReduced; - m_Feff *= dT; - m_Feff += m_M * (vPrev - v); - - break; - default: - LOG(WARNING) << "ReducedStVK::computeImplicitSystemRHS: Update type not supported"; + case StateUpdateType::DeltaVelocity: + + m_Feff = m_K * -(uPrev - u + v * dT); + + if (m_damped) + { + m_Feff -= m_C * v; + } + + m_forceModel->GetInternalForce(u.data(), m_Finternal.data()); + m_Feff -= m_Finternal; + this->project(m_FexplicitExternal, m_FexplicitExternalReduced); + m_Feff += m_FexplicitExternalReduced; + // the reduced gravity has alredy been initialized in initializeGravityForce + // this->project(m_Fgravity, m_FgravityReduced); + m_Feff += m_FgravityReduced; + this->project(m_Fcontact, m_FcontactReduced); + m_Feff += m_FcontactReduced; + m_Feff *= dT; + m_Feff += m_M * (vPrev - v); + + break; + default: + LOG(WARNING) << "ReducedStVK::computeImplicitSystemRHS: Update type not supported"; } } @@ -395,29 +395,29 @@ ReducedStVK::computeSemiImplicitSystemRHS(kinematicState& stateAtT, switch (updateType) { - case StateUpdateType::DeltaVelocity: + case StateUpdateType::DeltaVelocity: - m_Feff = m_K * (vPrev * -dT); + m_Feff = m_K * (vPrev * -dT); - if (m_damped) - { - m_Feff -= m_C * vPrev; - } + if (m_damped) + { + m_Feff -= m_C * vPrev; + } - m_forceModel->GetInternalForce(u.data(), m_Finternal.data()); - m_Feff -= m_Finternal; - this->project(m_FexplicitExternal, m_FexplicitExternalReduced); - m_Feff += m_FexplicitExternalReduced; - this->project(m_Fgravity, m_FgravityReduced); - m_Feff += m_FgravityReduced; - this->project(m_Fcontact, m_FcontactReduced); - m_Feff += m_FcontactReduced; - m_Feff *= dT; + m_forceModel->GetInternalForce(u.data(), m_Finternal.data()); + m_Feff -= m_Finternal; + this->project(m_FexplicitExternal, m_FexplicitExternalReduced); + m_Feff += m_FexplicitExternalReduced; + this->project(m_Fgravity, m_FgravityReduced); + m_Feff += m_FgravityReduced; + this->project(m_Fcontact, m_FcontactReduced); + m_Feff += m_FcontactReduced; + m_Feff *= dT; - break; + break; - default: - LOG(FATAL) << "ReducedStVK::computeSemiImplicitSystemRHS: Update type not supported"; + default: + LOG(FATAL) << "ReducedStVK::computeSemiImplicitSystemRHS: Update type not supported"; } } @@ -430,30 +430,29 @@ ReducedStVK::computeImplicitSystemLHS(const kinematicState& stateAtT, switch (updateType) { - case StateUpdateType::DeltaVelocity: + case StateUpdateType::DeltaVelocity: - stateAtT; // supress warning (state is not used in this update type hence can be + stateAtT; // supress warning (state is not used in this update type hence can be // ignored) - this->updateMassMatrix(); - m_forceModel->GetTangentStiffnessMatrix(newState.getQ().data(), + this->updateMassMatrix(); + m_forceModel->GetTangentStiffnessMatrix(newState.getQ().data(), m_stiffnessMatrix.data()); - this->initializeEigenMatrixFromStdVector(m_stiffnessMatrix, m_K); - this->updateDampingMatrix(); + this->initializeEigenMatrixFromStdVector(m_stiffnessMatrix, m_K); + this->updateDampingMatrix(); - m_Keff = m_M; - if (m_damped) - { - m_Keff += dT * m_C; - } - m_Keff += (dT * dT) * m_K; + m_Keff = m_M; + if (m_damped) + { + m_Keff += dT * m_C; + } + m_Keff += (dT * dT) * m_K; - break; + break; - default: - LOG(FATAL) << "ReducedStVK::computeImplicitSystemLHS: Update type not supported"; + default: + LOG(FATAL) << "ReducedStVK::computeImplicitSystemLHS: Update type not supported"; } - } bool @@ -470,7 +469,10 @@ ReducedStVK::initializeExplicitExternalForces() void ReducedStVK::updateDampingMatrix() { - if (!m_damped) return; + if (!m_damped) + { + return; + } const auto& dampingStiffnessCoefficient = m_config->m_dampingStiffnessCoefficient; const auto& dampingMassCoefficient = m_config->m_dampingMassCoefficient; @@ -546,21 +548,21 @@ ReducedStVK::updateBodyIntermediateStates(const Vectord& solution, const StateUp switch (updateType) { - case StateUpdateType::DeltaVelocity: - m_currentStateReduced->setV(v + solution); - m_currentStateReduced->setU(uPrev + dT * v); + case StateUpdateType::DeltaVelocity: + m_currentStateReduced->setV(v + solution); + m_currentStateReduced->setU(uPrev + dT * v); - break; + break; - case StateUpdateType::Velocity: - m_currentStateReduced->setV(solution); - m_currentStateReduced->setU(uPrev + dT * v); + case StateUpdateType::Velocity: + m_currentStateReduced->setV(solution); + m_currentStateReduced->setU(uPrev + dT * v); - break; + break; - default: - LOG(FATAL) << "DeformableBodyModel::updateBodyIntermediateStates: Unknown state update " - "type"; + default: + LOG(FATAL) << "DeformableBodyModel::updateBodyIntermediateStates: Unknown state update " + "type"; } prolongate(*m_currentStateReduced, *m_currentState); m_qSolReduced = m_currentStateReduced->getQ(); @@ -574,13 +576,13 @@ ReducedStVK::getFunction() // Function to evaluate the nonlinear objective function given the current state return [&, this](const Vectord& q, const bool semiImplicit) -> const Vectord& { - (semiImplicit) ? this->computeSemiImplicitSystemRHS(*m_previousStateReduced, + (semiImplicit) ? this->computeSemiImplicitSystemRHS(*m_previousStateReduced, *m_currentStateReduced, m_updateType) : this->computeImplicitSystemRHS(*m_previousStateReduced, *m_currentStateReduced, m_updateType); - return m_Feff; + return m_Feff; }; #pragma warning(pop) @@ -593,10 +595,10 @@ ReducedStVK::getFunctionGradient() #pragma warning(disable : 4100) // Gradient of the nonlinear objective function given the current state return [&, this](const Vectord& q) -> const Matrixd& { - this->computeImplicitSystemLHS(*m_previousStateReduced, + this->computeImplicitSystemLHS(*m_previousStateReduced, *m_currentStateReduced, m_updateType); - return m_Keff; + return m_Keff; }; #pragma warning(pop) @@ -607,7 +609,7 @@ ReducedStVK::getUpdateFunction() { // Function to evaluate the nonlinear objective function given the current state return [&, this](const Vectord& q, const bool fullyImplicit) -> void { - (fullyImplicit) ? this->updateBodyIntermediateStates(q, m_updateType) + (fullyImplicit) ? this->updateBodyIntermediateStates(q, m_updateType) : this->updateBodyStates(q, m_updateType); }; } @@ -688,5 +690,4 @@ ReducedStVK::initGraphEdges(std::shared_ptr<TaskNode> source, std::shared_ptr<Ta m_taskGraph->addEdge(source, m_solveNode); m_taskGraph->addEdge(m_solveNode, sink); } - } // namespace imstk diff --git a/Source/DynamicalModels/ObjectModels/imstkReducedStVKBodyModel.h b/Source/DynamicalModels/ObjectModels/imstkReducedStVKBodyModel.h index ea3fa62510d27cb084926e502fa5c52719249209..b8f55955b2676b86608c8d7bf861c99eedc82a9a 100644 --- a/Source/DynamicalModels/ObjectModels/imstkReducedStVKBodyModel.h +++ b/Source/DynamicalModels/ObjectModels/imstkReducedStVKBodyModel.h @@ -53,14 +53,15 @@ struct ReducedStVKConfig double m_dampingStiffnessCoefficient = 0.01; double m_dampingLaplacianCoefficient = 0.0; double m_deformationCompliance = 1.0; - double m_gravity = 9.81; + double m_gravity = 9.81; int m_numberOfThreads = 4; }; -class ReducedStVK : public DynamicalModel<FeDeformBodyState> { - using kinematicState = FeDeformBodyState; - // using Matrixd = Eigen::MatrixXd; +class ReducedStVK : public DynamicalModel<FeDeformBodyState> +{ +using kinematicState = FeDeformBodyState; +// using Matrixd = Eigen::MatrixXd; public: /// /// \brief Constructor @@ -140,7 +141,7 @@ public: /// \brief Initialize the Eigen matrix with data inside vega sparse matrix /// static void initializeEigenMatrixFromStdVector(const std::vector<double>& v, - Matrixd& eigenMatrix); + Matrixd& eigenMatrix); /// /// \brief Compute the RHS of the resulting linear system @@ -222,8 +223,7 @@ public: /// /// \brief Returns the unknown vectors /// - Vectord& - getUnknownVec() + Vectord& getUnknownVec() { return m_qSol; } @@ -231,21 +231,19 @@ public: /// /// \brief Set/Get the update type /// - void - setUpdateType(const StateUpdateType& updateType) + void setUpdateType(const StateUpdateType& updateType) { m_updateType = updateType; } - const StateUpdateType& - getUpdateType() const + + const StateUpdateType& getUpdateType() const { return m_updateType; } /// \brief Returns the unknown vectors /// - std::vector<std::size_t>& - getFixNodeIds() + std::vector<std::size_t>& getFixNodeIds() { return m_fixedNodeIds; } @@ -268,20 +266,17 @@ public: /// /// \brief Set the fixed BC implementation state /// - void - enableFixedBC() + void enableFixedBC() { LOG(WARNING) << "Boundary conditions are not allowed to change"; m_implementFixedBC = true; }; - void - disableFixedBC() + void disableFixedBC() { // LOG(FATAL, "Boundary conditions are not allowed to change"); m_implementFixedBC = false; }; - bool - isFixedBCImplemented() const + bool isFixedBCImplemented() const { return m_implementFixedBC; }; @@ -290,7 +285,7 @@ public: /// \brief prolongate reduced vector into full space, ie, u = U * uReduced /// void prolongate(const Vectord& uReduced, Vectord& u) const; - + /// /// \brief prolongate reduced state into full space /// @@ -319,46 +314,46 @@ protected: protected: std::shared_ptr<SolverBase> m_solver = nullptr; - std::shared_ptr<vega::StVKReducedInternalForces> m_internalForceModel; ///> Mathematical model for intenal forces - std::shared_ptr<vega::ReducedStVKForceModel> m_forceModel; - std::shared_ptr<TimeIntegrator> m_timeIntegrator; ///> Time integrator - std::shared_ptr<NonLinearSystem<Matrixd>> m_nonLinearSystem; ///> Nonlinear system resulting from TI and force model + std::shared_ptr<vega::StVKReducedInternalForces> m_internalForceModel; ///> Mathematical model for intenal forces + std::shared_ptr<vega::ReducedStVKForceModel> m_forceModel; + std::shared_ptr<TimeIntegrator> m_timeIntegrator; ///> Time integrator + std::shared_ptr<NonLinearSystem<Matrixd>> m_nonLinearSystem; ///> Nonlinear system resulting from TI and force model std::shared_ptr<vega::ModalMatrix> m_modalMatrix; std::shared_ptr<ReducedStVKConfig> m_config; /// Matrices typical to a elastodynamics and 2nd order analogous systems - Matrixd m_M; ///> Mass matrix - Matrixd m_C; ///> Damping coefficient matrix - Matrixd m_K; ///> Tangent (derivative of internal force w.r.t displacements) stiffness matrix - Matrixd m_Keff; ///> Effective stiffness matrix (dependent on internal force model and time - /// integrator) + Matrixd m_M; ///> Mass matrix + Matrixd m_C; ///> Damping coefficient matrix + Matrixd m_K; ///> Tangent (derivative of internal force w.r.t displacements) stiffness matrix + Matrixd m_Keff; ///> Effective stiffness matrix (dependent on internal force model and time + /// integrator) // full-space vectors - Vectord m_Fcontact; ///> Vector of contact forces - Vectord m_Fgravity; ///> Vector of gravity forces - Vectord m_FexplicitExternal; ///> Vector of explicitly defined external forces - Vectord m_qSol; ///> Vector to maintain solution at each iteration of nonlinear solver + Vectord m_Fcontact; ///> Vector of contact forces + Vectord m_Fgravity; ///> Vector of gravity forces + Vectord m_FexplicitExternal; ///> Vector of explicitly defined external forces + Vectord m_qSol; ///> Vector to maintain solution at each iteration of nonlinear solver // reduced-space vectors - Vectord m_Feff; ///> Vector of effective forces - Vectord m_Finternal; ///> Vector of internal forces - Vectord m_qSolReduced; ///> Vector to maintain solution at each iteration of nonlinear solver + Vectord m_Feff; ///> Vector of effective forces + Vectord m_Finternal; ///> Vector of internal forces + Vectord m_qSolReduced; ///> Vector to maintain solution at each iteration of nonlinear solver Vectord m_FcontactReduced; - Vectord m_FgravityReduced; ///> Vector of gravity forces - Vectord m_FexplicitExternalReduced; ///> Vector of explicitly defined external forces - size_t m_numDOFReduced; + Vectord m_FgravityReduced; ///> Vector of gravity forces + Vectord m_FexplicitExternalReduced; ///> Vector of explicitly defined external forces + size_t m_numDOFReduced; - std::shared_ptr<vega::VolumetricMesh> m_vegaPhysicsMesh; ///> Mesh used for Physics - std::vector<double> m_massMatrix; ///> Vega mass matrix - std::vector<double> m_stiffnessMatrix; ///> Vega Tangent stiffness matrix - std::vector<double> m_dampingMatrix; ///> Vega Laplacian damping matrix + std::shared_ptr<vega::VolumetricMesh> m_vegaPhysicsMesh; ///> Mesh used for Physics + std::vector<double> m_massMatrix; ///> Vega mass matrix + std::vector<double> m_stiffnessMatrix; ///> Vega Tangent stiffness matrix + std::vector<double> m_dampingMatrix; ///> Vega Laplacian damping matrix - std::vector<std::size_t> m_fixedNodeIds; ///> Nodal IDs of the nodes that are fixed + std::vector<std::size_t> m_fixedNodeIds; ///> Nodal IDs of the nodes that are fixed - StateUpdateType m_updateType = StateUpdateType::DeltaVelocity; ///> Update type of the model + StateUpdateType m_updateType = StateUpdateType::DeltaVelocity; ///> Update type of the model - bool m_damped = false; ///> Viscous or structurally damped system + bool m_damped = false; ///> Viscous or structurally damped system // 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 @@ -371,5 +366,4 @@ protected: private: std::shared_ptr<TaskNode> m_solveNode = nullptr; }; - } // namespace imstk \ No newline at end of file diff --git a/Source/Scene/imstkObjectInteractionFactory.cpp b/Source/Scene/imstkObjectInteractionFactory.cpp index 7a120051a7c3b5c0487bdd5f42c9d00039ef8124..eaba788f7d8d03002a21f191d9e6d3d32c89133b 100644 --- a/Source/Scene/imstkObjectInteractionFactory.cpp +++ b/Source/Scene/imstkObjectInteractionFactory.cpp @@ -38,14 +38,15 @@ namespace imstk { // Cast type check template<typename ObjectType> -static bool isType(std::shared_ptr<SceneObject> obj) +static bool +isType(std::shared_ptr<SceneObject> obj) { return std::dynamic_pointer_cast<ObjectType>(obj) != nullptr; } std::shared_ptr<ObjectInteractionPair> makeObjectInteractionPair(std::shared_ptr<CollidingObject> obj1, std::shared_ptr<CollidingObject> obj2, - InteractionType intType, CollisionDetection::Type cdType) + InteractionType intType, CollisionDetection::Type cdType) { std::shared_ptr<ObjectInteractionPair> results = nullptr; if (intType == InteractionType::PbdObjToPbdObjCollision && isType<PbdObject>(obj1) && isType<PbdObject>(obj2)) diff --git a/Source/Scene/imstkScene.h b/Source/Scene/imstkScene.h index ea017f44cf1f7e285b18dbb69f5404b29a3debec..a7556617acbfc5162bcfc6dba8db1f6b47127cae 100644 --- a/Source/Scene/imstkScene.h +++ b/Source/Scene/imstkScene.h @@ -282,8 +282,7 @@ protected: NamedMap<Light> m_lightsMap; std::shared_ptr<IBLProbe> m_globalIBLProbe = nullptr; std::shared_ptr<Camera> m_camera = std::make_shared<Camera>(); - // std::shared_ptr<CollisionGraph> m_collisionGraph = std::make_shared<CollisionGraph>(); - std::shared_ptr<CollisionGraph> m_collisionGraph = nullptr; + std::shared_ptr<CollisionGraph> m_collisionGraph = nullptr; std::vector<std::shared_ptr<SceneObjectControllerBase>> m_objectControllers; ///> List of object controllers std::vector<std::shared_ptr<CameraController>> m_cameraControllers; ///> List of camera controllers std::unordered_map<std::string, std::thread> m_threadMap; ///> diff --git a/Source/SceneEntities/Objects/imstkReducedFeDeformableObject.cpp b/Source/SceneEntities/Objects/imstkReducedFeDeformableObject.cpp index 9e4a8d0d56b257d874df916f1889331935857272..93830c7de4616392e5f9418cdf44f89189c2bd96 100644 --- a/Source/SceneEntities/Objects/imstkReducedFeDeformableObject.cpp +++ b/Source/SceneEntities/Objects/imstkReducedFeDeformableObject.cpp @@ -85,4 +85,3 @@ ReducedFeDeformableObject::getPrevAccelerations() const return m_defModel->getPreviousState()->getQDotDot(); } } // imstk - diff --git a/Source/SceneEntities/Objects/imstkReducedFeDeformableObject.h b/Source/SceneEntities/Objects/imstkReducedFeDeformableObject.h index d83e00c37d288622b9e9780e0767eded9cfc56c0..5f577e2e31e7c0ac806082921422e22baa44a3be 100644 --- a/Source/SceneEntities/Objects/imstkReducedFeDeformableObject.h +++ b/Source/SceneEntities/Objects/imstkReducedFeDeformableObject.h @@ -109,4 +109,3 @@ protected: std::shared_ptr<ReducedStVK> m_defModel = nullptr; }; } // imstk -