diff --git a/Sofa/Component/Mass/CMakeLists.txt b/Sofa/Component/Mass/CMakeLists.txt index a6dc70ba3e5..d43240fb14d 100644 --- a/Sofa/Component/Mass/CMakeLists.txt +++ b/Sofa/Component/Mass/CMakeLists.txt @@ -9,9 +9,12 @@ set(HEADER_FILES ${SOFACOMPONENTMASS_SOURCE_DIR}/AddMToMatrixFunctor.h ${SOFACOMPONENTMASS_SOURCE_DIR}/DiagonalMass.h ${SOFACOMPONENTMASS_SOURCE_DIR}/DiagonalMass.inl + ${SOFACOMPONENTMASS_SOURCE_DIR}/ElementFEMMass.h + ${SOFACOMPONENTMASS_SOURCE_DIR}/ElementFEMMass.inl ${SOFACOMPONENTMASS_SOURCE_DIR}/MassType.h ${SOFACOMPONENTMASS_SOURCE_DIR}/MeshMatrixMass.h ${SOFACOMPONENTMASS_SOURCE_DIR}/MeshMatrixMass.inl + ${SOFACOMPONENTMASS_SOURCE_DIR}/NodalMassDensity.h ${SOFACOMPONENTMASS_SOURCE_DIR}/RigidMassType.h ${SOFACOMPONENTMASS_SOURCE_DIR}/UniformMass.h ${SOFACOMPONENTMASS_SOURCE_DIR}/UniformMass.inl @@ -21,14 +24,18 @@ set(HEADER_FILES set(SOURCE_FILES ${SOFACOMPONENTMASS_SOURCE_DIR}/init.cpp ${SOFACOMPONENTMASS_SOURCE_DIR}/DiagonalMass.cpp + ${SOFACOMPONENTMASS_SOURCE_DIR}/ElementFEMMass.cpp ${SOFACOMPONENTMASS_SOURCE_DIR}/MeshMatrixMass.cpp + ${SOFACOMPONENTMASS_SOURCE_DIR}/NodalMassDensity.cpp ${SOFACOMPONENTMASS_SOURCE_DIR}/UniformMass.cpp ) +sofa_find_package(Sofa.FEM REQUIRED) sofa_find_package(Sofa.Simulation.Core REQUIRED) sofa_find_package(Sofa.Component.Topology.Container.Dynamic REQUIRED) add_library(${PROJECT_NAME} SHARED ${HEADER_FILES} ${SOURCE_FILES}) +target_link_libraries(${PROJECT_NAME} PUBLIC Sofa.FEM) target_link_libraries(${PROJECT_NAME} PUBLIC Sofa.Simulation.Core) target_link_libraries(${PROJECT_NAME} PUBLIC Sofa.Component.Topology.Container.Dynamic) diff --git a/Sofa/Component/Mass/Sofa.Component.MassConfig.cmake.in b/Sofa/Component/Mass/Sofa.Component.MassConfig.cmake.in index 5ebd01a7eb5..c36a7aff2d1 100644 --- a/Sofa/Component/Mass/Sofa.Component.MassConfig.cmake.in +++ b/Sofa/Component/Mass/Sofa.Component.MassConfig.cmake.in @@ -4,6 +4,7 @@ @PACKAGE_INIT@ find_package(Sofa.Config QUIET REQUIRED) +sofa_find_package(Sofa.FEM QUIET REQUIRED) sofa_find_package(Sofa.Simulation.Core QUIET REQUIRED) sofa_find_package(Sofa.Component.Topology.Container.Dynamic QUIET REQUIRED) diff --git a/Sofa/Component/Mass/Testing/src/sofa/component/mass/testing/MassTestCreation.h b/Sofa/Component/Mass/Testing/src/sofa/component/mass/testing/MassTestCreation.h index 7dce8da651f..8f21cfe49fb 100644 --- a/Sofa/Component/Mass/Testing/src/sofa/component/mass/testing/MassTestCreation.h +++ b/Sofa/Component/Mass/Testing/src/sofa/component/mass/testing/MassTestCreation.h @@ -183,7 +183,7 @@ struct Mass_test : public sofa::testing::BaseSimulationTest, public sofa::testin // v * Mv should be the 2 * kinetic energy EXPECT_LT(std::abs(vMv - 2 * ke), (SReal)(m_errorMax * this->epsilon())) - << "Kinetic energy inconsistent with addMDx"; + << "Kinetic energy inconsistent with addMDx (vMv = " << vMv << ", kinetic energy = " << (Real)ke << ")"; } /** * @brief Given positions and velocities, checks mass methods. diff --git a/Sofa/Component/Mass/src/sofa/component/mass/ElementFEMMass.cpp b/Sofa/Component/Mass/src/sofa/component/mass/ElementFEMMass.cpp new file mode 100644 index 00000000000..7f4a5825806 --- /dev/null +++ b/Sofa/Component/Mass/src/sofa/component/mass/ElementFEMMass.cpp @@ -0,0 +1,64 @@ +/****************************************************************************** +* 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_MASS_ELEMENTFEMMASS_CPP +#include +#include +#include +#include + +namespace sofa::component::mass +{ + +template class SOFA_COMPONENT_MASS_API ElementFEMMass; +template class SOFA_COMPONENT_MASS_API ElementFEMMass; +template class SOFA_COMPONENT_MASS_API ElementFEMMass; +template class SOFA_COMPONENT_MASS_API ElementFEMMass; +template class SOFA_COMPONENT_MASS_API ElementFEMMass; +template class SOFA_COMPONENT_MASS_API ElementFEMMass; +template class SOFA_COMPONENT_MASS_API ElementFEMMass; +template class SOFA_COMPONENT_MASS_API ElementFEMMass; +template class SOFA_COMPONENT_MASS_API ElementFEMMass; + +void registerFEMMass(sofa::core::ObjectFactory* factory) +{ + factory->registerObjects(sofa::core::ObjectRegistrationData("Finite-element mass (inertia and body force) defined on edges") + .add< ElementFEMMass >() + .add< ElementFEMMass >() + .add< ElementFEMMass >(true)); + + factory->registerObjects(sofa::core::ObjectRegistrationData("Finite-element mass (inertia and body force) defined on triangles") + .add< ElementFEMMass >() + .add< ElementFEMMass >(true)); + + factory->registerObjects(sofa::core::ObjectRegistrationData("Finite-element mass (inertia and body force) defined on quads") + .add< ElementFEMMass >() + .add< ElementFEMMass >(true)); + + factory->registerObjects(sofa::core::ObjectRegistrationData("Finite-element mass (inertia and body force) defined on tetrahedra") + .add< ElementFEMMass >(true)); + + factory->registerObjects(sofa::core::ObjectRegistrationData("Finite-element mass (inertia and body force) defined on hexahedra") + .add< ElementFEMMass >(true)); + +} + +} diff --git a/Sofa/Component/Mass/src/sofa/component/mass/ElementFEMMass.h b/Sofa/Component/Mass/src/sofa/component/mass/ElementFEMMass.h new file mode 100644 index 00000000000..61c1f9d820b --- /dev/null +++ b/Sofa/Component/Mass/src/sofa/component/mass/ElementFEMMass.h @@ -0,0 +1,282 @@ +/****************************************************************************** +* 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 +#include + +#if !defined(SOFA_COMPONENT_MASS_ELEMENTFEMMASS_CPP) +#include +#endif + +namespace sofa::component::mass +{ + +/** + * @class ElementFEMMass + * @brief Computes and stores the mass matrix for a finite element model. + * + * This class calculates the mass matrix for a given set of finite elements based on a nodal mass density field. + * The mass matrix is computed using numerical integration (quadrature) over the element domain. + * The mass density is interpolated from the nodes using the element's shape functions. + * + * Mathematically, the mass matrix M is defined as: + * \f$ M_{ij} = \int_{\Omega} \rho(\mathbf{x}) N_i(\mathbf{x}) N_j(\mathbf{x}) d\Omega \f$ + * where \f$ \rho(\mathbf{x}) \f$ is the mass density and \f$ N_i(\mathbf{x}) \f$ are the shape functions. + * + * The resulting matrix is a block-diagonal-like matrix where each block is a scalar multiple of the identity, + * representing the contribution to each degree of freedom. This allows for efficient storage and computation. + * + * @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 ElementFEMMass : + public core::behavior::Mass, + public virtual sofa::core::behavior::TopologyAccessor +{ +public: + using DataTypes = TDataTypes; + using ElementType = TElementType; + SOFA_CLASS2(SOFA_TEMPLATE2(ElementFEMMass, DataTypes, ElementType), + core::behavior::Mass, + sofa::core::behavior::TopologyAccessor); + +protected: + using FiniteElement = sofa::fem::FiniteElement; + + static constexpr sofa::Size spatial_dimensions = DataTypes::spatial_dimensions; + static constexpr sofa::Size NumberOfNodesInElement = ElementType::NumberOfNodes; + static constexpr sofa::Size NumberOfDofsInElement = NumberOfNodesInElement * spatial_dimensions; + static constexpr sofa::Size TopologicalDimension = FiniteElement::TopologicalDimension; + + using ElementMassMatrix = sofa::type::Mat>; + + using NodalMassDensity = ::sofa::component::mass::NodalMassDensity>; + using GlobalMassMatrixType = sofa::linearalgebra::CompressedRowSparseMatrixMechanical>; + +public: + + /** + * @brief Gets the class name according to the provided template parameters. + * + * For example, `ElementFEMMass` will return "EdgeFEMMass". + * + * @return A string representing the class name. + */ + static const std::string GetCustomClassName() + { + return std::string(sofa::geometry::elementTypeToString(ElementType::Element_type)) + "FEMMass"; + } + + /** + * @brief Gets the template name based on the data types. + * @return A string representing the template name (e.g., "Vec3d"). + */ + static const std::string GetCustomTemplateName() { return DataTypes::Name(); } + + /** + * @brief Link to the nodal mass density component. + * + * This component provides the mass density at each node of the mesh. + * It must be present in the context for the mass to be calculated correctly. + */ + sofa::SingleLink l_nodalMassDensity; + + /** + * @brief Initializes the component. + * + * This method performs several initialization steps: + * 1. Initializes the topology accessor. + * 2. Initializes the base Mass class. + * 3. Validates that a valid nodal mass density is linked. + * 4. Computes and assembles the global mass matrix. + */ + void init() final; + + /** + * @brief Indicates whether the mass matrix is diagonal. + * @return Always returns false, as FEM mass matrices are generally not diagonal (unless lumped). + */ + bool isDiagonal() const override { return false; } + + /** + * @brief Adds the gravity force (f = M * g) to the force vector. + * + * This method computes the product of the mass matrix and the gravity vector, + * adding the result to the force vector `f`. + * + * @param mparams Mechanical parameters for the computation. + * @param f The force vector to which the gravity force will be added. + * @param x The current positions (unused in this implementation). + * @param v The current velocities (unused in this implementation). + */ + void addForce(const core::MechanicalParams* mparams, + sofa::DataVecDeriv_t& f, + const sofa::DataVecCoord_t& x, + const sofa::DataVecDeriv_t& v) override; + + /** + * @brief Accumulates the mass matrix into a global matrix accumulator. + * + * This is used during the assembly of the system matrix for implicit solvers. + * It maps the stored sparse mass matrix into the global system matrix. + * + * @param matrices The accumulator used to build the global system matrix. + */ + void buildMassMatrix(sofa::core::behavior::MassMatrixAccumulator* matrices) override; + + using Inherit1::addMDx; + /** + * @brief Adds the product of the mass matrix and a vector to another vector (f += M * dx * factor). + * + * This is used for computing accelerations or in iterative solvers. + * + * @param mparams Mechanical parameters for the computation. + * @param f The result vector to which the product is added. + * @param dx The vector to be multiplied by the mass matrix. + * @param factor A scaling factor for the product. + */ + void addMDx(const core::MechanicalParams* mparams, DataVecDeriv_t& f, const DataVecDeriv_t& dx, SReal factor) override; + + using Inherit1::accFromF; + /** + * @brief Supposed to compute $ a = M^{-1} f $, but triggers an error in this implementation. + * + * @param mparams Mechanical parameters for the computation. + * @param a The result vector of $M^{-1} f$. + * @param f The vector to be multiplied by the inverse mass matrix. + */ + void accFromF(const core::MechanicalParams* mparams, DataVecDeriv_t& a, const DataVecDeriv_t& f) override; + + + using Inherit1::getKineticEnergy; + /** + * @brief Computes the total kinetic energy of the system. + * + * The kinetic energy $T$ is computed as: + * \f$ T = \frac{1}{2} \mathbf{v}^T \mathbf{M} \mathbf{v} \f$ + * where \f$ \mathbf{M} \f$ is the global mass matrix and \f$ \mathbf{v} \f$ is the velocity vector. + * + * @param mparams Mechanical parameters for the computation. + * @param v The current velocity vector. + * @return The computed kinetic energy. + */ + SReal getKineticEnergy(const core::MechanicalParams* mparams, + const DataVecDeriv_t& v) const override; + + using Inherit1::getPotentialEnergy; + /** + * @brief Computes the total gravitational potential energy of the system. + * + * The potential energy $V$ is computed as: + * \f$ V = - \mathbf{x}^T \mathbf{M} \mathbf{g} \f$ + * where \f$ \mathbf{M} \f$ is the global mass matrix, \f$ \mathbf{x} \f$ is the position vector, + * and \f$ \mathbf{g} \f$ is the gravity vector. + * + * @param mparams Mechanical parameters for the computation. + * @param x The current position vector. + * @return The computed gravitational potential energy. + */ + SReal getPotentialEnergy( + const core::MechanicalParams* mparams, + const DataVecCoord_t& x) const override; + +protected: + + /** + * @brief Default constructor. + */ + ElementFEMMass(); + + /** + * @brief Performs the internal calculation and assembly of the mass matrix. + * + * This method iterates over all elements, performs numerical integration to find + * element-level mass matrices, and then assembles them into the global sparse matrix `m_globalMassMatrix`. + */ + void elementFEMMass_init(); + + /** + * @brief Computes the mass matrix for each finite element. + * + * This method iterates over all elements provided by the topology and performs + * numerical integration (quadrature) to compute the local mass matrix for each element. + * The mass density is interpolated from the nodal values using the element's shape functions. + * + * @param elements The sequence of elements to process. + * @param[out] elementMassMatrices A vector to be populated with the computed local mass matrices. + */ + void calculateElementMassMatrix(const auto& elements, sofa::type::vector &elementMassMatrices); + + /** + * @brief Assembles the global mass matrix from individual element mass matrices. + * + * This method takes the precomputed local mass matrices and distributes their contributions + * into the global sparse mass matrix `m_globalMassMatrix`. It handles the mapping from + * local element node indices to global degree-of-freedom indices. + * + * @param elements The sequence of elements corresponding to the provided mass matrices. + * @param elementMassMatrices The precomputed local mass matrices for each element. + */ + void initializeGlobalMassMatrix(const auto& elements, const sofa::type::vector& elementMassMatrices); + + /** + * @brief Ensures that a valid NodalMassDensity component is linked. + * + * If no link is set, it attempts to find a suitable component in the current context. + * If no component is found, it marks the component state as invalid. + */ + void validateNodalMassDensity(); + + /** + * @brief Represents the global mass matrix of the system. + * + * This matrix is stored in a compressed sparse row format. + * Since the mass contribution for each node is isotropic (affects all spatial dimensions + * identically), this matrix stores only the scalar scaling factor for each identity block + * of size `spatial_dimensions * spatial_dimensions`. + * + * For example, in 3D, a node's mass contribution is a 3x3 diagonal matrix `m * I`. + * Only `m` is stored here. + */ + GlobalMassMatrixType m_globalMassMatrix; +}; + +#if !defined(SOFA_COMPONENT_MASS_ELEMENTFEMMASS_CPP) +template class SOFA_COMPONENT_MASS_API ElementFEMMass; +template class SOFA_COMPONENT_MASS_API ElementFEMMass; +template class SOFA_COMPONENT_MASS_API ElementFEMMass; +template class SOFA_COMPONENT_MASS_API ElementFEMMass; +template class SOFA_COMPONENT_MASS_API ElementFEMMass; +template class SOFA_COMPONENT_MASS_API ElementFEMMass; +template class SOFA_COMPONENT_MASS_API ElementFEMMass; +template class SOFA_COMPONENT_MASS_API ElementFEMMass; +template class SOFA_COMPONENT_MASS_API ElementFEMMass; +#endif + +} // namespace sofa::component::mass diff --git a/Sofa/Component/Mass/src/sofa/component/mass/ElementFEMMass.inl b/Sofa/Component/Mass/src/sofa/component/mass/ElementFEMMass.inl new file mode 100644 index 00000000000..f074bcab895 --- /dev/null +++ b/Sofa/Component/Mass/src/sofa/component/mass/ElementFEMMass.inl @@ -0,0 +1,328 @@ +/****************************************************************************** +* 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 + +namespace sofa::component::mass +{ + +template +ElementFEMMass::ElementFEMMass() + : l_nodalMassDensity(initLink("nodalMassDensity", "Link to nodal mass density")) +{ +} + + +template +void ElementFEMMass::init() +{ + TopologyAccessor::init(); + + if (!this->isComponentStateInvalid()) + { + core::behavior::Mass::init(); + } + + if (!this->isComponentStateInvalid()) + { + validateNodalMassDensity(); + } + + if (!this->isComponentStateInvalid()) + { + elementFEMMass_init(); + } +} + +template +void ElementFEMMass::validateNodalMassDensity() +{ + if (l_nodalMassDensity.empty()) + { + msg_info() << "Link to a nodal mass density should be set to ensure right behavior. First " + "component found in current context will be used."; + l_nodalMassDensity.set(this->getContext()->template get()); + } + + if (l_nodalMassDensity == nullptr) + { + msg_error() << "No nodal mass density component found at path: " << this->l_nodalMassDensity.getLinkedPath() + << ", nor in current context: " << this->getContext()->name << "."; + this->d_componentState.setValue(sofa::core::objectmodel::ComponentState::Invalid); + } +} + + +template +void ElementFEMMass::elementFEMMass_init() +{ + const auto& elements = FiniteElement::getElementSequence(*this->l_topology); + sofa::type::vector elementMassMatrices; + + //1. compute element mass matrix + calculateElementMassMatrix(elements, elementMassMatrices); + + // 2. convert element matrices to dof matrices and store them for later use + initializeGlobalMassMatrix(elements, elementMassMatrices); +} + +template +void ElementFEMMass::calculateElementMassMatrix( + const auto& elements, sofa::type::vector &elementMassMatrices) +{ + const auto nbElements = elements.size(); + elementMassMatrices.resize(nbElements); + + sofa::helper::ReadAccessor nodalMassDensityAccessor { l_nodalMassDensity->d_property }; + auto restPositionAccessor = this->mstate->readRestPositions(); + + SCOPED_TIMER("elementMassMatrix"); + helper::IotaView indices{static_cast(0ul), nbElements}; + std::for_each( + indices.begin(), indices.end(), + [&](const auto elementId) + { + const auto& element = elements[elementId]; + auto& elementMassMatrix = elementMassMatrices[elementId]; + + std::array, NumberOfNodesInElement> nodeDensityInElement; + for (sofa::Size i = 0; i < NumberOfNodesInElement; ++i) + { + nodeDensityInElement[i] = + l_nodalMassDensity->getNodeProperty(element[i], nodalMassDensityAccessor); + } + + std::array, NumberOfNodesInElement> nodeCoordinatesInElement; + for (sofa::Size i = 0; i < NumberOfNodesInElement; ++i) + { + nodeCoordinatesInElement[i] = restPositionAccessor[element[i]]; + } + + for (const auto& [quadraturePoint, weight] : FiniteElement::quadraturePoints()) + { + // gradient of shape functions in the reference element evaluated at the quadrature + // point + const sofa::type::Mat> + dN_dq_ref = FiniteElement::gradientShapeFunctions(quadraturePoint); + + // jacobian of the mapping from the reference space to the physical space, evaluated + // at the quadrature point + sofa::type::Mat> + jacobian = FiniteElement::Helper::jacobianFromReferenceToPhysical( + nodeCoordinatesInElement, dN_dq_ref); + + const auto detJ = sofa::type::absGeneralizedDeterminant(jacobian); + + // shape functions in the reference element evaluated at the quadrature point + const auto N = FiniteElement::shapeFunctions(quadraturePoint); + + const auto density = + FiniteElement::Helper::evaluateValueInElement(nodeDensityInElement, N); + + const auto NT_N = sofa::type::dyad(N, N); + + elementMassMatrix += (weight * density * detJ) * NT_N; + } + }); +} + +template +void ElementFEMMass::initializeGlobalMassMatrix( + const auto& elements, const sofa::type::vector& elementMassMatrices) +{ + SCOPED_TIMER("elementMassMatrix"); + + const auto nbElements = elements.size(); + + m_globalMassMatrix.clear(); + + const auto matrixSize = this->mstate->getSize(); + m_globalMassMatrix.resize(matrixSize, matrixSize); + + helper::IotaView indices{static_cast(0ul), nbElements}; + std::for_each(indices.begin(), indices.end(), + [&](const auto elementId) + { + const auto& element = elements[elementId]; + auto& elementMassMatrix = elementMassMatrices[elementId]; + + for (sofa::Size i = 0; i < NumberOfNodesInElement; ++i) + { + const auto node_i = element[i]; + for (sofa::Size j = 0; j < NumberOfNodesInElement; ++j) + { + const auto node_j = element[j]; + m_globalMassMatrix.add(node_i, node_j, elementMassMatrix(i, j)); + } + } + }); + + m_globalMassMatrix.compress(); +} + +template +void ElementFEMMass::addForce(const 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); + + auto forceAccessor = sofa::helper::getWriteAccessor(f); + + const auto g = getContext()->getGravity(); + Deriv_t theGravity; + DataTypes::set( theGravity, g[0], g[1], g[2] ); + + for (std::size_t xi = 0; xi < m_globalMassMatrix.rowIndex.size(); ++xi) + { + const auto rowId = m_globalMassMatrix.rowIndex[xi]; + typename GlobalMassMatrixType::Range rowRange(m_globalMassMatrix.rowBegin[xi], m_globalMassMatrix.rowBegin[xi + 1]); + for (typename GlobalMassMatrixType::Index xj = rowRange.begin(); xj < rowRange.end(); ++xj) + { + const auto& value = m_globalMassMatrix.colsValue[xj]; + + const auto force = value * theGravity; + forceAccessor[rowId] += force; + } + } +} + +template +void ElementFEMMass::buildMassMatrix( + sofa::core::behavior::MassMatrixAccumulator* matrices) +{ + for (std::size_t xi = 0; xi < m_globalMassMatrix.rowIndex.size(); ++xi) + { + const auto rowId = m_globalMassMatrix.rowIndex[xi]; + typename GlobalMassMatrixType::Range rowRange(m_globalMassMatrix.rowBegin[xi], m_globalMassMatrix.rowBegin[xi + 1]); + for (typename GlobalMassMatrixType::Index xj = rowRange.begin(); xj < rowRange.end(); ++xj) + { + const auto columnId = m_globalMassMatrix.colsIndex[xj]; + const auto& value = m_globalMassMatrix.colsValue[xj]; + + for (typename GlobalMassMatrixType::Index d = 0; d < spatial_dimensions; ++d) + { + matrices->add(rowId * spatial_dimensions + d, columnId * spatial_dimensions + d, value); + } + } + } +} + +template +void ElementFEMMass::addMDx(const core::MechanicalParams* mparams, + DataVecDeriv_t& f, + const DataVecDeriv_t& dx, + SReal factor) +{ + SOFA_UNUSED(mparams); + + auto result = sofa::helper::getWriteAccessor(f); + const auto dxAccessor = sofa::helper::getReadAccessor(dx); + + for (std::size_t xi = 0; xi < m_globalMassMatrix.rowIndex.size(); ++xi) + { + const auto rowId = m_globalMassMatrix.rowIndex[xi]; + typename GlobalMassMatrixType::Range rowRange(m_globalMassMatrix.rowBegin[xi], m_globalMassMatrix.rowBegin[xi + 1]); + for (typename GlobalMassMatrixType::Index xj = rowRange.begin(); xj < rowRange.end(); ++xj) + { + const auto columnId = m_globalMassMatrix.colsIndex[xj]; + const auto& value = m_globalMassMatrix.colsValue[xj]; + + result[rowId] += (factor * value) * dxAccessor[columnId]; + } + } +} + +template +void ElementFEMMass::accFromF(const core::MechanicalParams* mparams, + DataVecDeriv_t& a, + const DataVecDeriv_t& f) +{ + SOFA_UNUSED(mparams); + SOFA_UNUSED(a); + SOFA_UNUSED(f); + + msg_error() << "the method 'accFromF' can't be used with this component as this SPARSE mass matrix can't be inversed easily."; +} + +template +SReal ElementFEMMass::getKineticEnergy( + const core::MechanicalParams* mparams, + const DataVecDeriv_t& v) const +{ + SOFA_UNUSED(mparams); + + SReal kineticEnergy = 0.0; + auto vAccessor = sofa::helper::getReadAccessor(v); + + for (std::size_t xi = 0; xi < m_globalMassMatrix.rowIndex.size(); ++xi) + { + const auto rowId = m_globalMassMatrix.rowIndex[xi]; + typename GlobalMassMatrixType::Range rowRange(m_globalMassMatrix.rowBegin[xi], m_globalMassMatrix.rowBegin[xi + 1]); + for (typename GlobalMassMatrixType::Index xj = rowRange.begin(); xj < rowRange.end(); ++xj) + { + const auto columnId = m_globalMassMatrix.colsIndex[xj]; + const auto& value = m_globalMassMatrix.colsValue[xj]; + + kineticEnergy += sofa::type::dot(vAccessor[rowId], vAccessor[columnId] * value); + } + } + + return kineticEnergy / 2_sreal; +} + +template +SReal ElementFEMMass::getPotentialEnergy( + const core::MechanicalParams* mparams, + const DataVecCoord_t& x) const +{ + SOFA_UNUSED(mparams); + + SReal potentialEnergy = 0.0; + auto xAccessor = sofa::helper::getReadAccessor(x); + + const auto g = getContext()->getGravity(); + Deriv_t theGravity; + DataTypes::set( theGravity, g[0], g[1], g[2] ); + + for (std::size_t xi = 0; xi < m_globalMassMatrix.rowIndex.size(); ++xi) + { + const auto rowId = m_globalMassMatrix.rowIndex[xi]; + typename GlobalMassMatrixType::Range rowRange(m_globalMassMatrix.rowBegin[xi], m_globalMassMatrix.rowBegin[xi + 1]); + for (typename GlobalMassMatrixType::Index xj = rowRange.begin(); xj < rowRange.end(); ++xj) + { + const auto& value = m_globalMassMatrix.colsValue[xj]; + + potentialEnergy -= sofa::type::dot(xAccessor[rowId], theGravity * value); + } + } + + return potentialEnergy; +} + +} // namespace sofa::component::mass diff --git a/Sofa/Component/Mass/src/sofa/component/mass/NodalMassDensity.cpp b/Sofa/Component/Mass/src/sofa/component/mass/NodalMassDensity.cpp new file mode 100644 index 00000000000..cb5f531971c --- /dev/null +++ b/Sofa/Component/Mass/src/sofa/component/mass/NodalMassDensity.cpp @@ -0,0 +1,37 @@ +/****************************************************************************** +* 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 * +******************************************************************************/ +#include +#include + +namespace sofa::component::mass +{ + +template class SOFA_COMPONENT_MASS_API NodalMassDensity; + +void registerNodalMassDensity(sofa::core::ObjectFactory* factory) +{ + factory->registerObjects(core::ObjectRegistrationData("Definition of a nodal mass density (one value per dof).") + .add< NodalMassDensity >() + ); +} + +} diff --git a/Sofa/Component/Mass/src/sofa/component/mass/NodalMassDensity.h b/Sofa/Component/Mass/src/sofa/component/mass/NodalMassDensity.h new file mode 100644 index 00000000000..925f9c4b4b9 --- /dev/null +++ b/Sofa/Component/Mass/src/sofa/component/mass/NodalMassDensity.h @@ -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 * +******************************************************************************/ +#pragma once + +#include +#include +#include + +namespace sofa::component::mass +{ + +template +class NodalMassDensity : public sofa::core::BaseNodalProperty +{ +public: + SOFA_CLASS(NodalMassDensity, sofa::core::BaseNodalProperty); + + template + static bool canCreate(T* obj, sofa::core::objectmodel::BaseContext* context, sofa::core::objectmodel::BaseObjectDescription* arg) + { + if (const auto* state = context->getMechanicalState()) + { + static const auto scalarType = defaulttype::DataTypeInfo::name(); + if (state->getScalarType() == scalarType) + return true; + arg->logError("The mechanical state does not have a scalar type of '" + scalarType + "'"); + return false; + } + return true; + } + +private: + + static constexpr Scalar defaultMassDensity = static_cast(1.); + + NodalMassDensity() : sofa::core::BaseNodalProperty(defaultMassDensity) {} +}; + +} diff --git a/Sofa/Component/Mass/src/sofa/component/mass/init.cpp b/Sofa/Component/Mass/src/sofa/component/mass/init.cpp index 14eb994286d..200d7b42f08 100644 --- a/Sofa/Component/Mass/src/sofa/component/mass/init.cpp +++ b/Sofa/Component/Mass/src/sofa/component/mass/init.cpp @@ -29,6 +29,8 @@ namespace sofa::component::mass extern void registerDiagonalMass(sofa::core::ObjectFactory* factory); extern void registerMeshMatrixMass(sofa::core::ObjectFactory* factory); extern void registerUniformMass(sofa::core::ObjectFactory* factory); +extern void registerNodalMassDensity(sofa::core::ObjectFactory* factory); +extern void registerFEMMass(sofa::core::ObjectFactory* factory); extern "C" { SOFA_EXPORT_DYNAMIC_LIBRARY void initExternalModule(); @@ -57,6 +59,8 @@ void registerObjects(sofa::core::ObjectFactory* factory) registerDiagonalMass(factory); registerMeshMatrixMass(factory); registerUniformMass(factory); + registerNodalMassDensity(factory); + registerFEMMass(factory); } void init() diff --git a/Sofa/Component/Mass/tests/CMakeLists.txt b/Sofa/Component/Mass/tests/CMakeLists.txt index 89d4cd08629..d80b20f5111 100644 --- a/Sofa/Component/Mass/tests/CMakeLists.txt +++ b/Sofa/Component/Mass/tests/CMakeLists.txt @@ -5,6 +5,7 @@ project(Sofa.Component.Mass_test) set(SOURCE_FILES DiagonalMass_test.cpp UniformMass_test.cpp + MassTestCreation[ElementFEMMass].cpp MassTestCreation[MeshMatrixMass].cpp MassTestCreation[UniformMass].cpp ) diff --git a/Sofa/Component/Mass/tests/MassTestCreation[ElementFEMMass].cpp b/Sofa/Component/Mass/tests/MassTestCreation[ElementFEMMass].cpp new file mode 100644 index 00000000000..9a7c75487dc --- /dev/null +++ b/Sofa/Component/Mass/tests/MassTestCreation[ElementFEMMass].cpp @@ -0,0 +1,109 @@ +/****************************************************************************** +* 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 General Public License as published by the Free * +* Software Foundation; either version 2 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 General Public License for * +* more details. * +* * +* You should have received a copy of the GNU 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 * +******************************************************************************/ +#include +#include +#include +#include +#include + +namespace sofa::component::mass::testing +{ + +using MeshTopology = sofa::component::topology::container::constant::MeshTopology; + + +/*************************************************************************************************** + * ElementFEMMass + **************************************************************************************************/ + +template +struct ElementMass_template_test : public Mass_test> +{ + using DataTypes = typename MassParam::DataTypes; + + using VecCoord = sofa::VecCoord_t; + using VecDeriv = sofa::VecDeriv_t; + + + ElementMass_template_test() + { + this->m_testAccFromF = false; + this->m_testAddMToMatrix = false; + this->m_errorMax = 1e3_sreal; + + auto topology = sofa::core::objectmodel::New(); + this->m_node->addObject(topology); + + topology->addEdge(0, 1); + topology->addTriangle(0, 1, 2); + topology->addQuad(0, 1, 2, 3); + topology->addTetra(0, 1, 2, 3); + topology->addHexa(0, 1, 2, 3, 4, 5, 6, 7); + + auto nodalDensity = sofa::core::objectmodel::New>>(); + this->m_node->addObject(nodalDensity); + } + + void run() + { + VecCoord x(8); + VecDeriv v(8); + + sofa::testing::LinearCongruentialRandomGenerator lcg(96547); + + for (std::size_t i = 0; i < 8; ++i) + { + DataTypes::set(x[i], lcg.generateInRange(-10., 10.), lcg.generateInRange(-10., 10.), lcg.generateInRange(-10., 10.)); + DataTypes::set(v[i], lcg.generateInRange(-10., 10.), lcg.generateInRange(-10., 10.), lcg.generateInRange(-10., 10.)); + } + + this->run_test(x, v); + } +}; + +template +struct MassParam +{ + using DataTypes = TDataTypes; + using ElementType = TElementType; +}; + +typedef ::testing::Types< + MassParam, + MassParam, + MassParam, + MassParam, + MassParam, + MassParam, + MassParam, + MassParam, + MassParam +> ElementMassDataTypes; + +TYPED_TEST_SUITE(ElementMass_template_test, ElementMassDataTypes); + +TYPED_TEST(ElementMass_template_test, test) +{ + this->run(); +} + +} // namespace sofa::component::mass::testing diff --git a/Sofa/Component/StateContainer/src/sofa/component/statecontainer/MechanicalObject.h b/Sofa/Component/StateContainer/src/sofa/component/statecontainer/MechanicalObject.h index 7c193cd4407..03ae874b42e 100644 --- a/Sofa/Component/StateContainer/src/sofa/component/statecontainer/MechanicalObject.h +++ b/Sofa/Component/StateContainer/src/sofa/component/statecontainer/MechanicalObject.h @@ -78,6 +78,8 @@ class MechanicalObject : public sofa::core::behavior::MechanicalState protected: virtual ~MechanicalObject(); public: + std::string getScalarType() const override { return sofa::defaulttype::DataTypeInfo::name(); } + void parse ( core::objectmodel::BaseObjectDescription* arg ) override; Data< VecCoord > x; ///< position coordinates of the degrees of freedom diff --git a/Sofa/framework/Core/CMakeLists.txt b/Sofa/framework/Core/CMakeLists.txt index bd3abe288dc..5d049ec19bf 100644 --- a/Sofa/framework/Core/CMakeLists.txt +++ b/Sofa/framework/Core/CMakeLists.txt @@ -24,6 +24,7 @@ set(HEADER_FILES ${SRC_ROOT}/AccumulationVecId.inl ${SRC_ROOT}/BaseMatrixAccumulatorComponent.h ${SRC_ROOT}/BaseLocalMappingMatrix.h + ${SRC_ROOT}/BaseNodalProperty.h ${SRC_ROOT}/BaseMapping.h ${SRC_ROOT}/BaseState.h ${SRC_ROOT}/BehaviorModel.h diff --git a/Sofa/framework/Core/src/sofa/core/BaseNodalProperty.h b/Sofa/framework/Core/src/sofa/core/BaseNodalProperty.h new file mode 100644 index 00000000000..d8929c67f96 --- /dev/null +++ b/Sofa/framework/Core/src/sofa/core/BaseNodalProperty.h @@ -0,0 +1,67 @@ +/****************************************************************************** +* 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 + +namespace sofa::core +{ + +template +class BaseNodalProperty : public virtual sofa::core::objectmodel::BaseComponent +{ +public: + SOFA_CLASS(BaseNodalProperty, sofa::core::objectmodel::BaseComponent); + + BaseNodalProperty() = delete; + + Data > d_property; + + const T& getNodeProperty(sofa::Index i, sofa::helper::ReadAccessor>>& property) const + { + if (property.size() > i) + { + return property[i]; + } + if (!property.empty()) + { + return property->back(); + } + return m_defaultProperty; + } + + const T& getNodeProperty(sofa::Index i) const + { + sofa::helper::ReadAccessor property { d_property }; + return getNodeProperty(i, property); + } + +protected: + explicit BaseNodalProperty(const T defaultProperty) + : d_property(initData(&d_property, {defaultProperty}, "property", "Nodal property")) + , m_defaultProperty(defaultProperty) + {} + + T m_defaultProperty {}; +}; + +} diff --git a/Sofa/framework/Core/src/sofa/core/BaseState.h b/Sofa/framework/Core/src/sofa/core/BaseState.h index ecaa165fb82..7b798ab71eb 100644 --- a/Sofa/framework/Core/src/sofa/core/BaseState.h +++ b/Sofa/framework/Core/src/sofa/core/BaseState.h @@ -46,6 +46,8 @@ class SOFA_CORE_API BaseState : public virtual objectmodel::BaseComponent BaseState(const BaseState& n) = delete; BaseState& operator=(const BaseState& n) = delete; public: + virtual std::string getScalarType() const { return {}; } + /// Current size of all stored vectors virtual Size getSize() const = 0; diff --git a/Sofa/framework/Core/src/sofa/core/behavior/Mass.inl b/Sofa/framework/Core/src/sofa/core/behavior/Mass.inl index ddfc9869433..78041353ccc 100644 --- a/Sofa/framework/Core/src/sofa/core/behavior/Mass.inl +++ b/Sofa/framework/Core/src/sofa/core/behavior/Mass.inl @@ -94,7 +94,9 @@ template void Mass::addMBKdx(const MechanicalParams* mparams, MultiVecDerivId dfId) { this->ForceField::addMBKdx(mparams, dfId); - if (mparams->mFactorIncludingRayleighDamping(rayleighMass.getValue()) != 0.0) + if (mparams && + mparams->mFactorIncludingRayleighDamping(rayleighMass.getValue()) != 0.0 && + this->mstate) { addMDx(mparams, *dfId[this->mstate.get()].write(), *mparams->readDx(this->mstate.get()), mparams->mFactorIncludingRayleighDamping(rayleighMass.getValue())); diff --git a/Sofa/framework/DefaultType/src/sofa/defaulttype/TemplatesAliases.cpp b/Sofa/framework/DefaultType/src/sofa/defaulttype/TemplatesAliases.cpp index 1bba32984c9..2b0a868184f 100644 --- a/Sofa/framework/DefaultType/src/sofa/defaulttype/TemplatesAliases.cpp +++ b/Sofa/framework/DefaultType/src/sofa/defaulttype/TemplatesAliases.cpp @@ -108,6 +108,7 @@ RegisterTemplateAlias::RegisterTemplateAlias(const std::string& alias, const std /// The following types are the generic 'precision' +static RegisterTemplateAlias SRealAlias("SReal", sofa::defaulttype::DataTypeName::name()); static RegisterTemplateAlias Vec1Alias("Vec1", sofa::defaulttype::Vec1Types::Name()); static RegisterTemplateAlias Vec2Alias("Vec2", sofa::defaulttype::Vec2Types::Name()); static RegisterTemplateAlias Vec3Alias("Vec3", sofa::defaulttype::Vec3Types::Name()); diff --git a/Sofa/framework/FEM/src/sofa/fem/FiniteElement.h b/Sofa/framework/FEM/src/sofa/fem/FiniteElement.h index 41a60a04d04..a490fddd6a3 100644 --- a/Sofa/framework/FEM/src/sofa/fem/FiniteElement.h +++ b/Sofa/framework/FEM/src/sofa/fem/FiniteElement.h @@ -40,7 +40,8 @@ struct FiniteElement; using ReferenceCoord = sofa::type::Vec;\ using ShapeFunctionType = std::function;\ using QuadraturePoint = ReferenceCoord; \ - using QuadraturePointAndWeight = std::pair + using QuadraturePointAndWeight = std::pair;\ + using Helper = FiniteElementHelper template @@ -80,6 +81,14 @@ struct FiniteElementHelper return jacobian; } + template + static constexpr auto evaluateValueInElement( + const std::array& valuesAtNodes, + const sofa::type::Vec& shapeFunctions) + { + return std::inner_product(valuesAtNodes.begin(), valuesAtNodes.end(), shapeFunctions.begin(), T(0)); + } + }; } diff --git a/Sofa/framework/FEM/src/sofa/fem/FiniteElement[Edge].h b/Sofa/framework/FEM/src/sofa/fem/FiniteElement[Edge].h index 6568f50dca0..fbffb5dbfed 100644 --- a/Sofa/framework/FEM/src/sofa/fem/FiniteElement[Edge].h +++ b/Sofa/framework/FEM/src/sofa/fem/FiniteElement[Edge].h @@ -41,6 +41,14 @@ struct FiniteElement return topology.getEdges(); } + static constexpr sofa::type::Vec shapeFunctions(const sofa::type::Vec& q) + { + return { + static_cast(0.5) * (static_cast(1) - q[0]), + static_cast(0.5) * (static_cast(1) + q[0]) + }; + } + static constexpr sofa::type::Mat gradientShapeFunctions(const sofa::type::Vec& q) { SOFA_UNUSED(q); diff --git a/Sofa/framework/FEM/src/sofa/fem/FiniteElement[Hexahedron].h b/Sofa/framework/FEM/src/sofa/fem/FiniteElement[Hexahedron].h index ce399e84e68..38339624e47 100644 --- a/Sofa/framework/FEM/src/sofa/fem/FiniteElement[Hexahedron].h +++ b/Sofa/framework/FEM/src/sofa/fem/FiniteElement[Hexahedron].h @@ -64,6 +64,20 @@ struct FiniteElement return topology.getHexahedra(); } + static constexpr sofa::type::Vec shapeFunctions(const sofa::type::Vec& q) + { + return { + -static_cast(0.125) * (q[0] - 1) * (q[1] - 1) * (q[2] - 1), + static_cast(0.125) * (q[0] + 1) * (q[1] - 1) * (q[2] - 1), + -static_cast(0.125) * (q[0] + 1) * (q[1] + 1) * (q[2] - 1), + static_cast(0.125) * (q[0] - 1) * (q[1] + 1) * (q[2] - 1), + static_cast(0.125) * (q[0] - 1) * (q[1] - 1) * (q[2] + 1), + -static_cast(0.125) * (q[0] + 1) * (q[1] - 1) * (q[2] + 1), + static_cast(0.125) * (q[0] + 1) * (q[1] + 1) * (q[2] + 1), + -static_cast(0.125) * (q[0] - 1) * (q[1] + 1) * (q[2] + 1), + }; + } + static constexpr sofa::type::Mat gradientShapeFunctions(const sofa::type::Vec& q) { const auto [x, y, z] = q; diff --git a/Sofa/framework/FEM/src/sofa/fem/FiniteElement[Quad].h b/Sofa/framework/FEM/src/sofa/fem/FiniteElement[Quad].h index 7690e7a62bc..4e37585eace 100644 --- a/Sofa/framework/FEM/src/sofa/fem/FiniteElement[Quad].h +++ b/Sofa/framework/FEM/src/sofa/fem/FiniteElement[Quad].h @@ -47,6 +47,16 @@ struct FiniteElement return topology.getQuads(); } + static constexpr sofa::type::Vec shapeFunctions(const sofa::type::Vec& q) + { + return { + static_cast(0.25) * (q[0] - 1) * (q[1] - 1), + -static_cast(0.25) * (q[0] + 1) * (q[1] - 1), + static_cast(0.25) * (q[0] + 1) * (q[1] + 1), + -static_cast(0.25) * (q[0] - 1) * (q[1] + 1) + }; + } + static constexpr sofa::type::Mat gradientShapeFunctions(const sofa::type::Vec& q) { return { diff --git a/Sofa/framework/FEM/src/sofa/fem/FiniteElement[Tetrahedron].h b/Sofa/framework/FEM/src/sofa/fem/FiniteElement[Tetrahedron].h index b5a5bc5d798..ac14bb9c59b 100644 --- a/Sofa/framework/FEM/src/sofa/fem/FiniteElement[Tetrahedron].h +++ b/Sofa/framework/FEM/src/sofa/fem/FiniteElement[Tetrahedron].h @@ -47,6 +47,16 @@ struct FiniteElement return topology.getTetrahedra(); } + static constexpr sofa::type::Vec shapeFunctions(const sofa::type::Vec& q) + { + return { + static_cast(1) - q[0] - q[1] - q[2], + q[0], + q[1], + q[2] + }; + } + static constexpr sofa::type::Mat gradientShapeFunctions(const sofa::type::Vec& q) { SOFA_UNUSED(q); diff --git a/Sofa/framework/FEM/src/sofa/fem/FiniteElement[Triangle].h b/Sofa/framework/FEM/src/sofa/fem/FiniteElement[Triangle].h index cfad013012d..0d9659040de 100644 --- a/Sofa/framework/FEM/src/sofa/fem/FiniteElement[Triangle].h +++ b/Sofa/framework/FEM/src/sofa/fem/FiniteElement[Triangle].h @@ -45,6 +45,15 @@ struct FiniteElement return topology.getTriangles(); } + static constexpr sofa::type::Vec shapeFunctions(const sofa::type::Vec& q) + { + return { + static_cast(1) - q[0] - q[1], + q[0], + q[1] + }; + } + static constexpr sofa::type::Mat gradientShapeFunctions(const sofa::type::Vec& q) { SOFA_UNUSED(q); diff --git a/examples/Validation/cantilever_beam/CantileverBeam_ElementFEMForceField.xml b/examples/Validation/cantilever_beam/CantileverBeam_ElementFEMForceField.xml index e3d96366ae5..552b1b55e69 100644 --- a/examples/Validation/cantilever_beam/CantileverBeam_ElementFEMForceField.xml +++ b/examples/Validation/cantilever_beam/CantileverBeam_ElementFEMForceField.xml @@ -30,7 +30,8 @@ - + +