diff --git a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/BaseElementLinearFEMForceField.h b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/BaseElementLinearFEMForceField.h index efbc9dda6db..384f092584a 100644 --- a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/BaseElementLinearFEMForceField.h +++ b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/BaseElementLinearFEMForceField.h @@ -53,6 +53,7 @@ class BaseElementLinearFEMForceField : public sofa::component::solidmechanics::f using trait = sofa::component::solidmechanics::fem::elastic::trait; using ElementHessian = typename trait::ElementHessian; using StrainDisplacement = typename trait::StrainDisplacement; + using ElementDisplacement = typename trait::ElementDisplacement; using Real = typename trait::Real; protected: @@ -64,6 +65,12 @@ class BaseElementLinearFEMForceField : public sofa::component::solidmechanics::f */ void precomputeElementStiffness(); + /// Displacement of the element nodes relative to their rest position. Returns a flat vector. + ElementDisplacement computeElementDisplacement( + const typename trait::TopologyElement& element, + const sofa::VecCoord_t& nodePositions, + const sofa::VecCoord_t& nodeRestPositions) const; + public: /** diff --git a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/BaseElementLinearFEMForceField.inl b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/BaseElementLinearFEMForceField.inl index e2191f8485d..e67f0e9928e 100644 --- a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/BaseElementLinearFEMForceField.inl +++ b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/BaseElementLinearFEMForceField.inl @@ -103,4 +103,24 @@ void BaseElementLinearFEMForceField::precomputeElementSt }); } +template +auto BaseElementLinearFEMForceField::computeElementDisplacement( + const typename trait::TopologyElement& element, + const sofa::VecCoord_t& nodePositions, + const sofa::VecCoord_t& nodeRestPositions) const -> ElementDisplacement +{ + ElementDisplacement displacement{ sofa::type::NOINIT }; + + for (sofa::Size j = 0; j < trait::NumberOfNodesInElement; ++j) + { + const auto nodeId = element[j]; + for (sofa::Size dim = 0; dim < trait::spatial_dimensions; ++dim) + { + displacement[j * trait::spatial_dimensions + dim] = nodePositions[nodeId][dim] - nodeRestPositions[nodeId][dim]; + } + } + + return displacement; +} + } diff --git a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/CorotationalFEMForceField.h b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/CorotationalFEMForceField.h index a74f6d0b002..170e2e0b460 100644 --- a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/CorotationalFEMForceField.h +++ b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/CorotationalFEMForceField.h @@ -124,6 +124,7 @@ class CorotationalFEMForceField : private: using trait = sofa::component::solidmechanics::fem::elastic::trait; using ElementGradient = typename trait::ElementGradient; + using ElementDisplacement = typename trait::ElementDisplacement; using RotationMatrix = sofa::type::Mat>; @@ -161,6 +162,12 @@ class CorotationalFEMForceField : sofa::type::vector m_rotations; sofa::type::vector m_initialRotationsTransposed; + /// Local displacement of the element nodes in the element's own rotated frame. Flat vector. + ElementDisplacement computeElementLocalDisplacement( + const std::array, trait::NumberOfNodesInElement>& nodes, + const std::array, trait::NumberOfNodesInElement>& restNodes, + const RotationMatrix& rotation) const; + sofa::Coord_t translation(const std::array, trait::NumberOfNodesInElement>& nodes) const; static sofa::Coord_t computeCentroid(const std::array, trait::NumberOfNodesInElement>& nodes); diff --git a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/CorotationalFEMForceField.inl b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/CorotationalFEMForceField.inl index 2e9dbe20c50..8e3fa64dc1d 100644 --- a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/CorotationalFEMForceField.inl +++ b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/CorotationalFEMForceField.inl @@ -78,6 +78,26 @@ void CorotationalFEMForceField::beforeElementForce( m_rotations.resize(elements.size(), RotationMatrix::Identity()); } +template +auto CorotationalFEMForceField::computeElementLocalDisplacement( + const std::array, trait::NumberOfNodesInElement>& nodes, + const std::array, trait::NumberOfNodesInElement>& restNodes, + const RotationMatrix& rotation) const -> ElementDisplacement +{ + const auto t = translation(nodes); + const auto t0 = translation(restNodes); + + ElementDisplacement displacement{ sofa::type::NOINIT }; + + for (sofa::Size j = 0; j < trait::NumberOfNodesInElement; ++j) + { + displacement.setsub(j * trait::spatial_dimensions, + rotation.multTranspose(nodes[j] - t) - (restNodes[j] - t0)); + } + + return displacement; +} + template void CorotationalFEMForceField::computeElementsForces( const sofa::simulation::Range& range, const sofa::core::MechanicalParams* mparams, @@ -102,15 +122,8 @@ void CorotationalFEMForceField::computeElementsForces( m_rotationMethods.computeRotation(elementRotation, elementInitialRotationTransposed, elementNodesCoordinates, restElementNodesCoordinates); - const auto t = translation(elementNodesCoordinates); - const auto t0 = translation(restElementNodesCoordinates); - - typename trait::ElementDisplacement displacement(sofa::type::NOINIT); - for (sofa::Size j = 0; j < trait::NumberOfNodesInElement; ++j) - { - displacement.setsub(j * DIM, - elementRotation.multTranspose(elementNodesCoordinates[j] - t) - (restElementNodesCoordinates[j] - t0)); - } + const auto displacement = computeElementLocalDisplacement( + elementNodesCoordinates, restElementNodesCoordinates, elementRotation); const auto& stiffnessMatrix = elementStiffness[elementId]; @@ -206,7 +219,35 @@ SReal CorotationalFEMForceField::getPotentialEnergy( const sofa::core::MechanicalParams*, const sofa::DataVecCoord_t& x) const { - return 0; + if (this->isComponentStateInvalid()) + return 0; + + const auto& elements = trait::FiniteElement::getElementSequence(*this->l_topology); + const auto elementStiffness = sofa::helper::getReadAccessor(this->d_elementStiffness); + + if (m_rotations.size() < elements.size()) + return 0; + + const auto positionAccessor = sofa::helper::getReadAccessor(x); + const auto restPositionAccessor = this->mstate->readRestPositions(); + + sofa::Real_t energy {}; + + for (std::size_t elementId = 0; elementId < elements.size(); ++elementId) + { + const auto& element = elements[elementId]; + + const auto elementNodesCoordinates = extractNodesVectorFromGlobalVector(element, positionAccessor.ref()); + const auto restElementNodesCoordinates = extractNodesVectorFromGlobalVector(element, restPositionAccessor.ref()); + + const auto displacement = computeElementLocalDisplacement( + elementNodesCoordinates, restElementNodesCoordinates, m_rotations[elementId]); + + // Quadratic form of strain energy: 1/2 d^T K d + energy += displacement * (elementStiffness[elementId] * displacement); + } + + return static_cast(0.5 * energy); } template diff --git a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/LinearSmallStrainFEMForceField.inl b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/LinearSmallStrainFEMForceField.inl index 6fea5667a99..7c9ba473847 100644 --- a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/LinearSmallStrainFEMForceField.inl +++ b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/LinearSmallStrainFEMForceField.inl @@ -53,19 +53,10 @@ void LinearSmallStrainFEMForceField::computeElementsForc for (std::size_t elementId = range.start; elementId < range.end; ++elementId) { - const auto& element = elements[elementId]; const auto& stiffnessMatrix = elementStiffness[elementId]; - typename trait::ElementDisplacement displacement{ sofa::type::NOINIT }; - - for (sofa::Size j = 0; j < trait::NumberOfNodesInElement; ++j) - { - const auto nodeId = element[j]; - for (sofa::Size dim = 0; dim < trait::spatial_dimensions; ++dim) - { - displacement[j * trait::spatial_dimensions + dim] = nodePositions[nodeId][dim] - restPositionAccessor[nodeId][dim]; - } - } + const auto displacement = this->computeElementDisplacement( + elements[elementId], nodePositions, restPositionAccessor.ref()); elementForces[elementId] = stiffnessMatrix * displacement; } @@ -145,7 +136,27 @@ SReal LinearSmallStrainFEMForceField::getPotentialEnergy const sofa::core::MechanicalParams*, const sofa::DataVecCoord_t& x) const { - return 0; + if (this->isComponentStateInvalid()) + return 0; + + const auto& elements = trait::FiniteElement::getElementSequence(*this->l_topology); + const auto elementStiffness = sofa::helper::getReadAccessor(this->d_elementStiffness); + + const auto positionAccessor = sofa::helper::getReadAccessor(x); + const auto restPositionAccessor = this->mstate->readRestPositions(); + + sofa::Real_t energy {}; + + for (std::size_t elementId = 0; elementId < elements.size(); ++elementId) + { + const auto displacement = this->computeElementDisplacement( + elements[elementId], positionAccessor.ref(), restPositionAccessor.ref()); + + // the element stiffness matrix is the quadratic form of the strain energy: 1/2 d^T K d + energy += displacement * (elementStiffness[elementId] * displacement); + } + + return static_cast(0.5 * energy); } template