diff --git a/Sofa/Component/SolidMechanics/FEM/Elastic/CMakeLists.txt b/Sofa/Component/SolidMechanics/FEM/Elastic/CMakeLists.txt index fddd6033a0d..829fcc8475a 100644 --- a/Sofa/Component/SolidMechanics/FEM/Elastic/CMakeLists.txt +++ b/Sofa/Component/SolidMechanics/FEM/Elastic/CMakeLists.txt @@ -15,6 +15,8 @@ set(HEADER_FILES ${SOFACOMPONENTSOLIDMECHANICSFEMELASTIC_SOURCE_DIR}/BeamFEMForceField.inl ${SOFACOMPONENTSOLIDMECHANICSFEMELASTIC_SOURCE_DIR}/CorotationalFEMForceField.h ${SOFACOMPONENTSOLIDMECHANICSFEMELASTIC_SOURCE_DIR}/CorotationalFEMForceField.inl + ${SOFACOMPONENTSOLIDMECHANICSFEMELASTIC_SOURCE_DIR}/FEMSourceTerm.h + ${SOFACOMPONENTSOLIDMECHANICSFEMELASTIC_SOURCE_DIR}/FEMSourceTerm.inl ${SOFACOMPONENTSOLIDMECHANICSFEMELASTIC_SOURCE_DIR}/LinearSmallStrainFEMForceField.h ${SOFACOMPONENTSOLIDMECHANICSFEMELASTIC_SOURCE_DIR}/LinearSmallStrainFEMForceField.inl ${SOFACOMPONENTSOLIDMECHANICSFEMELASTIC_SOURCE_DIR}/FastTetrahedralCorotationalForceField.h @@ -69,6 +71,7 @@ set(SOURCE_FILES ${SOFACOMPONENTSOLIDMECHANICSFEMELASTIC_SOURCE_DIR}/BaseLinearElasticityFEMForceField.cpp ${SOFACOMPONENTSOLIDMECHANICSFEMELASTIC_SOURCE_DIR}/BeamFEMForceField.cpp ${SOFACOMPONENTSOLIDMECHANICSFEMELASTIC_SOURCE_DIR}/CorotationalFEMForceField.cpp + ${SOFACOMPONENTSOLIDMECHANICSFEMELASTIC_SOURCE_DIR}/FEMSourceTerm.cpp ${SOFACOMPONENTSOLIDMECHANICSFEMELASTIC_SOURCE_DIR}/LinearSmallStrainFEMForceField.cpp ${SOFACOMPONENTSOLIDMECHANICSFEMELASTIC_SOURCE_DIR}/FastTetrahedralCorotationalForceField.cpp ${SOFACOMPONENTSOLIDMECHANICSFEMELASTIC_SOURCE_DIR}/FEMForceField.cpp diff --git a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/FEMSourceTerm.cpp b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/FEMSourceTerm.cpp new file mode 100644 index 00000000000..31935713335 --- /dev/null +++ b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/FEMSourceTerm.cpp @@ -0,0 +1,58 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#define SOFA_COMPONENT_SOLIDMECHANICS_FEM_ELASTIC_FEM_SOURCE_TERM_CPP + +#include + +#include +#include +#include + +namespace sofa::component::solidmechanics::fem::elastic +{ + +void registerFEMSourceTerm(sofa::core::ObjectFactory* factory) +{ + factory->registerObjects(sofa::core::ObjectRegistrationData("Consistent nodal load of a source term, integrated with the finite-element quadrature from a nodal source field") + .add< FEMSourceTerm >() + .add< FEMSourceTerm >() + .add< FEMSourceTerm >() + .add< FEMSourceTerm >() + .add< FEMSourceTerm >() + .add< FEMSourceTerm >() + .add< FEMSourceTerm >() + .add< FEMSourceTerm >() + .add< FEMSourceTerm >() + ); +} + +template class SOFA_COMPONENT_SOLIDMECHANICS_FEM_ELASTIC_API FEMSourceTerm; +template class SOFA_COMPONENT_SOLIDMECHANICS_FEM_ELASTIC_API FEMSourceTerm; +template class SOFA_COMPONENT_SOLIDMECHANICS_FEM_ELASTIC_API FEMSourceTerm; +template class SOFA_COMPONENT_SOLIDMECHANICS_FEM_ELASTIC_API FEMSourceTerm; +template class SOFA_COMPONENT_SOLIDMECHANICS_FEM_ELASTIC_API FEMSourceTerm; +template class SOFA_COMPONENT_SOLIDMECHANICS_FEM_ELASTIC_API FEMSourceTerm; +template class SOFA_COMPONENT_SOLIDMECHANICS_FEM_ELASTIC_API FEMSourceTerm; +template class SOFA_COMPONENT_SOLIDMECHANICS_FEM_ELASTIC_API FEMSourceTerm; +template class SOFA_COMPONENT_SOLIDMECHANICS_FEM_ELASTIC_API FEMSourceTerm; + +} // namespace sofa::component::solidmechanics::fem::elastic diff --git a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/FEMSourceTerm.h b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/FEMSourceTerm.h new file mode 100644 index 00000000000..724881a4bd2 --- /dev/null +++ b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/FEMSourceTerm.h @@ -0,0 +1,169 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once + +#include +#include +#include +#include +#include + +#if !defined(SOFA_COMPONENT_SOLIDMECHANICS_FEM_ELASTIC_FEM_SOURCE_TERM_CPP) +#include +#endif + +namespace sofa::component::solidmechanics::fem::elastic +{ + +/** + * @class FEMSourceTerm + * @brief Computes nodal source terms by integrating a given source density field stored at the nodes. + * + * This class assembles and stores a geometric matrix M using a quadrature rule over the element domain. + * The matrix is then used to multiply the source density b to compute a nodal source term F that + * contributes to the RHS of the weak form through the addForce function. + * + * @tparam TDataTypes The data types used for positions, velocities, etc. (e.g., Vec3Types). + * @tparam TElementType The type of finite element (e.g., sofa::geometry::Tetrahedron). + */ +template +class FEMSourceTerm : + public sofa::core::behavior::ForceField, + public virtual sofa::core::behavior::TopologyAccessor +{ +public: + using DataTypes = TDataTypes; + using ElementType = TElementType; + SOFA_CLASS2(SOFA_TEMPLATE2(FEMSourceTerm, DataTypes, ElementType), + sofa::core::behavior::ForceField, + sofa::core::behavior::TopologyAccessor); + +protected: + using FiniteElement = sofa::fem::FiniteElement; + + static constexpr sofa::Size NumberOfNodesInElement = ElementType::NumberOfNodes; + + using ElementMatrix = sofa::type::Mat>; + using GlobalMatrix = sofa::linearalgebra::CompressedRowSparseMatrixMechanical>; + +public: + + /** + * @brief Initializes the component. + * + * This method performs several initialization steps: + * 1. Initializes the base force field. + * 2. Initializes the topology accessor. + * 3. Resizes the nodal source density. + * 4. Assembles the global matrix M. + */ + void init() override; + + /** + * @brief Adds the force (f = M * b) to the RHS vector. + * + * This method computes the product of the geometric matrix and the source density vector, + * adding the result to the force vector `f`. + * + * @param mparams Mechanical parameters for the computation. + * @param f The force vector to which the source term will be added. + * @param x The current positions (unused in this implementation). + * @param v The current velocities (unused in this implementation). + */ + void addForce( + const sofa::core::MechanicalParams* mparams, + sofa::DataVecDeriv_t& f, + const sofa::DataVecCoord_t& x, + const sofa::DataVecDeriv_t& v) override; + + /** + * @brief A no-op as the load is prescribed on the rest configuration + */ + void addDForce(const sofa::core::MechanicalParams* mparams, + sofa::DataVecDeriv_t& df, + const sofa::DataVecDeriv_t& dx) override; + + /** + * @brief A no-op as the load is prescribed on the rest configuration + */ + void buildStiffnessMatrix(sofa::core::behavior::StiffnessMatrix* matrix) override; + + using sofa::core::behavior::ForceField::getPotentialEnergy; + /** + * @brief Not implemented, returns 0. + */ + SReal getPotentialEnergy(const sofa::core::MechanicalParams* mparams, + const sofa::DataVecCoord_t& x) const override; + + /** + * @brief Source term (per unit volume) sampled at each node. + */ + sofa::Data > d_nodalSourceDensity; + +protected: + + /** + * @brief Default constructor. + */ + FEMSourceTerm(); + + /** + * @brief Resizes the source density to the size of the mechanical state + */ + void resizeNodalSourceDensity(const std::size_t size); + + /** + * @brief Assembles and stores the geometry-only matrix \f$ M_{ij} = \int_{\Omega} N_i N_j \, d\Omega \f$ over each element on the rest configuration. + */ + void assembleGlobalMatrix(); + + /** + * @brief Computes the geometry-only matrix of each element. + */ + void calculateElementMatrix(const auto& elements, sofa::type::vector& elementMatrices); + + /** + * @brief Scatters the element matrices into the global matrix. + */ + void initializeGlobalMatrix(const auto& elements, const sofa::type::vector& elementMatrices); + + /** + * @brief Geometry-only matrix \f$ M_{ij} = \int_{\Omega} N_i N_j \, d\Omega \f$ of the system. + * + * Stored in compressed sparse row format. Assembled once in init on the rest configuration. + */ + GlobalMatrix m_globalMatrix; +}; + +#if !defined(SOFA_COMPONENT_SOLIDMECHANICS_FEM_ELASTIC_FEM_SOURCE_TERM_CPP) +extern template class SOFA_COMPONENT_SOLIDMECHANICS_FEM_ELASTIC_API FEMSourceTerm; +extern template class SOFA_COMPONENT_SOLIDMECHANICS_FEM_ELASTIC_API FEMSourceTerm; +extern template class SOFA_COMPONENT_SOLIDMECHANICS_FEM_ELASTIC_API FEMSourceTerm; +extern template class SOFA_COMPONENT_SOLIDMECHANICS_FEM_ELASTIC_API FEMSourceTerm; +extern template class SOFA_COMPONENT_SOLIDMECHANICS_FEM_ELASTIC_API FEMSourceTerm; +extern template class SOFA_COMPONENT_SOLIDMECHANICS_FEM_ELASTIC_API FEMSourceTerm; +extern template class SOFA_COMPONENT_SOLIDMECHANICS_FEM_ELASTIC_API FEMSourceTerm; +extern template class SOFA_COMPONENT_SOLIDMECHANICS_FEM_ELASTIC_API FEMSourceTerm; +extern template class SOFA_COMPONENT_SOLIDMECHANICS_FEM_ELASTIC_API FEMSourceTerm; +#endif + +} // namespace sofa::component::solidmechanics::fem::elastic diff --git a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/FEMSourceTerm.inl b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/FEMSourceTerm.inl new file mode 100644 index 00000000000..528e0079750 --- /dev/null +++ b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/FEMSourceTerm.inl @@ -0,0 +1,197 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once +#include +#include + +namespace sofa::component::solidmechanics::fem::elastic +{ + +template +FEMSourceTerm::FEMSourceTerm() + : d_nodalSourceDensity(initData(&d_nodalSourceDensity, "nodalSourceDensity", + "Source term (per unit volume) sampled at each node. Interpolated inside the " + "element with the shape functions and integrated on the reference configuration.")) +{ +} + +template +void FEMSourceTerm::init() +{ + sofa::core::behavior::ForceField::init(); + + if (!this->isComponentStateInvalid()) + { + sofa::core::behavior::TopologyAccessor::init(); + } + + if (!this->isComponentStateInvalid() && this->mstate) + { + this->resizeNodalSourceDensity(this->mstate->getSize()); + } + + if (!this->isComponentStateInvalid() && this->l_topology && this->mstate) + { + this->assembleGlobalMatrix(); + } + + if (!this->isComponentStateInvalid()) + { + this->d_componentState.setValue(sofa::core::objectmodel::ComponentState::Valid); + } +} + +template +void FEMSourceTerm::resizeNodalSourceDensity(const std::size_t size) +{ + sofa::helper::WriteAccessor nodalSourceDensity = sofa::helper::getWriteAccessor(d_nodalSourceDensity); + + if (nodalSourceDensity.size() < size) + { + nodalSourceDensity.resize(size, sofa::Deriv_t{}); + } +} + +template +void FEMSourceTerm::assembleGlobalMatrix() +{ + const auto& elements = FiniteElement::getElementSequence(*this->l_topology); + sofa::type::vector elementMatrices; + + // 1. compute the geometry-only matrix of each element + calculateElementMatrix(elements, elementMatrices); + + // 2. scatter the element matrices into the global matrix + initializeGlobalMatrix(elements, elementMatrices); +} + +template +void FEMSourceTerm::calculateElementMatrix( + const auto& elements, sofa::type::vector& elementMatrices) +{ + const auto restPositionsAccessor = this->mstate->readRestPositions(); + elementMatrices.resize(elements.size()); + + for (std::size_t elementId = 0; elementId < elements.size(); ++elementId) + { + const auto& element = elements[elementId]; + auto& elementMatrix = elementMatrices[elementId]; + + const std::array, NumberOfNodesInElement> elementNodesRestCoordinates = + extractNodesVectorFromGlobalVector(element, restPositionsAccessor.ref()); + + // M_ij = integral of N_i N_j dV, evaluated on the rest configuration (geometry only). + for (const auto& [quadraturePoint, weight] : FiniteElement::quadraturePoints()) + { + const auto N = FiniteElement::shapeFunctions(quadraturePoint); + const auto dN_dq_ref = FiniteElement::gradientShapeFunctions(quadraturePoint); + + const auto jacobian = FiniteElement::Helper::jacobianFromReferenceToPhysical( + elementNodesRestCoordinates, dN_dq_ref); + const auto detJ = sofa::type::absGeneralizedDeterminant(jacobian); + + const auto NT_N = sofa::type::dyad(N, N); + + elementMatrix += (weight * detJ) * NT_N; + } + } +} + +template +void FEMSourceTerm::initializeGlobalMatrix( + const auto& elements, const sofa::type::vector& elementMatrices) +{ + m_globalMatrix.clear(); + const auto size = this->mstate->getSize(); + m_globalMatrix.resize(size, size); + + for (std::size_t elementId = 0; elementId < elements.size(); ++elementId) + { + const auto& element = elements[elementId]; + const auto& elementMatrix = elementMatrices[elementId]; + + for (sofa::Size i = 0; i < NumberOfNodesInElement; ++i) + { + for (sofa::Size j = 0; j < NumberOfNodesInElement; ++j) + { + m_globalMatrix.add(element[i], element[j], elementMatrix(i, j)); + } + } + } + + m_globalMatrix.compress(); +} + +template +void FEMSourceTerm::addForce(const sofa::core::MechanicalParams* mparams, + sofa::DataVecDeriv_t& f, + const sofa::DataVecCoord_t& x, + const sofa::DataVecDeriv_t& v) +{ + SOFA_UNUSED(mparams); + SOFA_UNUSED(x); + SOFA_UNUSED(v); + + const sofa::helper::ReadAccessor nodalSourceDensity = sofa::helper::getReadAccessor(d_nodalSourceDensity); + auto forceAccessor = sofa::helper::getWriteAccessor(f); + + // f_i = sum_j M_ij b_j : apply the global matrix to the nodal source density. + for (std::size_t xi = 0; xi < m_globalMatrix.rowIndex.size(); ++xi) + { + const auto rowId = m_globalMatrix.rowIndex[xi]; + typename GlobalMatrix::Range rowRange(m_globalMatrix.rowBegin[xi], m_globalMatrix.rowBegin[xi + 1]); + for (typename GlobalMatrix::Index xj = rowRange.begin(); xj < rowRange.end(); ++xj) + { + const auto columnId = m_globalMatrix.colsIndex[xj]; + const auto& value = m_globalMatrix.colsValue[xj]; + + forceAccessor[rowId] += nodalSourceDensity[columnId] * value; + } + } +} + +template +void FEMSourceTerm::addDForce(const sofa::core::MechanicalParams* mparams, + sofa::DataVecDeriv_t& df, + const sofa::DataVecDeriv_t& dx) +{ + SOFA_UNUSED(mparams); + SOFA_UNUSED(df); + SOFA_UNUSED(dx); +} + +template +void FEMSourceTerm::buildStiffnessMatrix(sofa::core::behavior::StiffnessMatrix* matrix) +{ + SOFA_UNUSED(matrix); +} + +template +SReal FEMSourceTerm::getPotentialEnergy(const sofa::core::MechanicalParams* mparams, + const sofa::DataVecCoord_t& x) const +{ + SOFA_UNUSED(mparams); + SOFA_UNUSED(x); + return 0.0; +} + +} // namespace sofa::component::solidmechanics::fem::elastic diff --git a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/init.cpp b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/init.cpp index 2c37e45f155..95e84fb5d1b 100644 --- a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/init.cpp +++ b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/init.cpp @@ -28,6 +28,7 @@ namespace sofa::component::solidmechanics::fem::elastic extern void registerBeamFEMForceField(sofa::core::ObjectFactory* factory); extern void registerCorotationalFEMForceField(sofa::core::ObjectFactory* factory); +extern void registerFEMSourceTerm(sofa::core::ObjectFactory* factory); extern void registerFastTetrahedralCorotationalForceField(sofa::core::ObjectFactory* factory); extern void registerHexahedralFEMForceField(sofa::core::ObjectFactory* factory); extern void registerHexahedralFEMForceFieldAndMass(sofa::core::ObjectFactory* factory); @@ -68,6 +69,7 @@ void registerObjects(sofa::core::ObjectFactory* factory) { registerBeamFEMForceField(factory); registerCorotationalFEMForceField(factory); + registerFEMSourceTerm(factory); registerFastTetrahedralCorotationalForceField(factory); registerHexahedralFEMForceField(factory); registerHexahedralFEMForceFieldAndMass(factory);