From 1a33ac02ca861da81bf846eaa3a1d38560b79b49 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Thu, 13 Aug 2026 09:43:14 +0200 Subject: [PATCH 1/4] Add potential energy computation for the LinearSmallStrainFEMForceField --- .../elastic/LinearSmallStrainFEMForceField.h | 6 ++ .../LinearSmallStrainFEMForceField.inl | 55 +++++++++++++++---- 2 files changed, 49 insertions(+), 12 deletions(-) diff --git a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/LinearSmallStrainFEMForceField.h b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/LinearSmallStrainFEMForceField.h index 899edf4c084..f80ab957d0b 100644 --- a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/LinearSmallStrainFEMForceField.h +++ b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/LinearSmallStrainFEMForceField.h @@ -52,6 +52,12 @@ class LinearSmallStrainFEMForceField : using StrainDisplacement = typename trait::StrainDisplacement; using ElementGradient = typename trait::ElementGradient; + /// 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: void init() override; 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..1494e96d8a6 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 @@ -40,6 +40,26 @@ void LinearSmallStrainFEMForceField::init() } +template +auto LinearSmallStrainFEMForceField::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; +} + template void LinearSmallStrainFEMForceField::computeElementsForces( const sofa::simulation::Range& range, @@ -53,19 +73,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 = computeElementDisplacement( + elements[elementId], nodePositions, restPositionAccessor.ref()); elementForces[elementId] = stiffnessMatrix * displacement; } @@ -145,7 +156,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 = 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 From d6e97b5c9b67812a338852e8acfc09871dd4d3ad Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Tue, 18 Aug 2026 16:49:58 +0200 Subject: [PATCH 2/4] Add potential energy for the CorotationalFEMForceField --- .../fem/elastic/CorotationalFEMForceField.h | 7 ++ .../fem/elastic/CorotationalFEMForceField.inl | 66 ++++++++++++++++--- 2 files changed, 63 insertions(+), 10 deletions(-) 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..fcd51f19b6d 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; + /// Displacement of the element nodes in the element's own rotated frame. Returns a 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..767f433f0a3 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,40 @@ 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); + + // The element rotations are those of the last force computation: they are filled while the forces + // are computed, so there are none to read before the first one has run. + 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 std::array, trait::NumberOfNodesInElement> elementNodesCoordinates = + extractNodesVectorFromGlobalVector(element, positionAccessor.ref()); + const std::array, trait::NumberOfNodesInElement> restElementNodesCoordinates = + extractNodesVectorFromGlobalVector(element, restPositionAccessor.ref()); + + const auto displacement = computeElementLocalDisplacement( + elementNodesCoordinates, restElementNodesCoordinates, m_rotations[elementId]); + + // the element stiffness matrix is the quadratic form of the strain energy: 1/2 d^T K d, taken + // on the co-rotated displacement, so a rigid motion of the element contributes nothing + energy += displacement * (elementStiffness[elementId] * displacement); + } + + return static_cast(0.5 * energy); } template From 1d81874f6ef45e46de3a09a000e9e85452f5c72c Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Wed, 19 Aug 2026 09:28:46 +0200 Subject: [PATCH 3/4] Use auto --- .../fem/elastic/CorotationalFEMForceField.h | 2 +- .../fem/elastic/CorotationalFEMForceField.inl | 11 +++-------- 2 files changed, 4 insertions(+), 9 deletions(-) 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 fcd51f19b6d..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 @@ -162,7 +162,7 @@ class CorotationalFEMForceField : sofa::type::vector m_rotations; sofa::type::vector m_initialRotationsTransposed; - /// Displacement of the element nodes in the element's own rotated frame. Returns a flat vector. + /// 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, 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 767f433f0a3..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 @@ -225,8 +225,6 @@ SReal CorotationalFEMForceField::getPotentialEnergy( const auto& elements = trait::FiniteElement::getElementSequence(*this->l_topology); const auto elementStiffness = sofa::helper::getReadAccessor(this->d_elementStiffness); - // The element rotations are those of the last force computation: they are filled while the forces - // are computed, so there are none to read before the first one has run. if (m_rotations.size() < elements.size()) return 0; @@ -239,16 +237,13 @@ SReal CorotationalFEMForceField::getPotentialEnergy( { const auto& element = elements[elementId]; - const std::array, trait::NumberOfNodesInElement> elementNodesCoordinates = - extractNodesVectorFromGlobalVector(element, positionAccessor.ref()); - const std::array, trait::NumberOfNodesInElement> restElementNodesCoordinates = - extractNodesVectorFromGlobalVector(element, restPositionAccessor.ref()); + const auto elementNodesCoordinates = extractNodesVectorFromGlobalVector(element, positionAccessor.ref()); + const auto restElementNodesCoordinates = extractNodesVectorFromGlobalVector(element, restPositionAccessor.ref()); const auto displacement = computeElementLocalDisplacement( elementNodesCoordinates, restElementNodesCoordinates, m_rotations[elementId]); - // the element stiffness matrix is the quadratic form of the strain energy: 1/2 d^T K d, taken - // on the co-rotated displacement, so a rigid motion of the element contributes nothing + // Quadratic form of strain energy: 1/2 d^T K d energy += displacement * (elementStiffness[elementId] * displacement); } From e546455c7cb0c846cd95fc0be2e009d905520d7b Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Wed, 19 Aug 2026 10:09:13 +0200 Subject: [PATCH 4/4] Factor out displacement compute helper --- .../elastic/BaseElementLinearFEMForceField.h | 7 ++++++ .../BaseElementLinearFEMForceField.inl | 20 ++++++++++++++++ .../elastic/LinearSmallStrainFEMForceField.h | 6 ----- .../LinearSmallStrainFEMForceField.inl | 24 ++----------------- 4 files changed, 29 insertions(+), 28 deletions(-) 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/LinearSmallStrainFEMForceField.h b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/LinearSmallStrainFEMForceField.h index f80ab957d0b..899edf4c084 100644 --- a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/LinearSmallStrainFEMForceField.h +++ b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/LinearSmallStrainFEMForceField.h @@ -52,12 +52,6 @@ class LinearSmallStrainFEMForceField : using StrainDisplacement = typename trait::StrainDisplacement; using ElementGradient = typename trait::ElementGradient; - /// 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: void init() override; 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 1494e96d8a6..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 @@ -40,26 +40,6 @@ void LinearSmallStrainFEMForceField::init() } -template -auto LinearSmallStrainFEMForceField::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; -} - template void LinearSmallStrainFEMForceField::computeElementsForces( const sofa::simulation::Range& range, @@ -75,7 +55,7 @@ void LinearSmallStrainFEMForceField::computeElementsForc { const auto& stiffnessMatrix = elementStiffness[elementId]; - const auto displacement = computeElementDisplacement( + const auto displacement = this->computeElementDisplacement( elements[elementId], nodePositions, restPositionAccessor.ref()); elementForces[elementId] = stiffnessMatrix * displacement; @@ -169,7 +149,7 @@ SReal LinearSmallStrainFEMForceField::getPotentialEnergy for (std::size_t elementId = 0; elementId < elements.size(); ++elementId) { - const auto displacement = computeElementDisplacement( + 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