Skip to content

[FEM] Implement getPotentialEnergy for Linear & Corotational FEMForceField #6228

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
th-skam wants to merge 4 commits into
base: master
Choose a base branch
from
Open

[FEM] Implement getPotentialEnergy for Linear & Corotational FEMForceField #6228

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
1a33ac0
Add potential energy computation for the LinearSmallStrainFEMForceField
th-skam Aug 13, 2026
d6e97b5
Add potential energy for the CorotationalFEMForceField
th-skam Aug 18, 2026
1d81874
Use auto
th-skam Aug 19, 2026
e546455
Factor out displacement compute helper
th-skam Aug 19, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
7 changes: 7 additions & 0 deletions ...EM/Elastic/src/sofa/component/solidmechanics/fem/elastic/BaseElementLinearFEMForceField.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -53,6 +53,7 @@ class BaseElementLinearFEMForceField : public sofa::component::solidmechanics::f
using trait = sofa::component::solidmechanics::fem::elastic::trait<DataTypes, ElementType>;
using ElementHessian = typename trait::ElementHessian;
using StrainDisplacement = typename trait::StrainDisplacement;
using ElementDisplacement = typename trait::ElementDisplacement;
using Real = typename trait::Real;

protected:
Expand All @@ -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<DataTypes>& nodePositions,
const sofa::VecCoord_t<DataTypes>& nodeRestPositions) const;

public:

/**
Expand Down
20 changes: 20 additions & 0 deletions .../Elastic/src/sofa/component/solidmechanics/fem/elastic/BaseElementLinearFEMForceField.inl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -103,4 +103,24 @@ void BaseElementLinearFEMForceField<DataTypes, ElementType>::precomputeElementSt
});
}

template <class DataTypes, class ElementType>
auto BaseElementLinearFEMForceField<DataTypes, ElementType>::computeElementDisplacement(
const typename trait::TopologyElement& element,
const sofa::VecCoord_t<DataTypes>& nodePositions,
const sofa::VecCoord_t<DataTypes>& 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;
}

}
7 changes: 7 additions & 0 deletions ...ics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/CorotationalFEMForceField.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -124,6 +124,7 @@ class CorotationalFEMForceField :
private:
using trait = sofa::component::solidmechanics::fem::elastic::trait<DataTypes, ElementType>;
using ElementGradient = typename trait::ElementGradient;
using ElementDisplacement = typename trait::ElementDisplacement;
using RotationMatrix = sofa::type::Mat<trait::spatial_dimensions, trait::spatial_dimensions, sofa::Real_t<DataTypes>>;


Expand Down Expand Up @@ -161,6 +162,12 @@ class CorotationalFEMForceField :
sofa::type::vector<RotationMatrix> m_rotations;
sofa::type::vector<RotationMatrix> m_initialRotationsTransposed;

/// Local displacement of the element nodes in the element's own rotated frame. Flat vector.
ElementDisplacement computeElementLocalDisplacement(
const std::array<sofa::Coord_t<DataTypes>, trait::NumberOfNodesInElement>& nodes,
const std::array<sofa::Coord_t<DataTypes>, trait::NumberOfNodesInElement>& restNodes,
const RotationMatrix& rotation) const;

sofa::Coord_t<DataTypes> translation(const std::array<sofa::Coord_t<DataTypes>, trait::NumberOfNodesInElement>& nodes) const;
static sofa::Coord_t<DataTypes> computeCentroid(const std::array<sofa::Coord_t<DataTypes>, trait::NumberOfNodesInElement>& nodes);

Expand Down
61 changes: 51 additions & 10 deletions ...s/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/CorotationalFEMForceField.inl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -78,6 +78,26 @@ void CorotationalFEMForceField<DataTypes, ElementType>::beforeElementForce(
m_rotations.resize(elements.size(), RotationMatrix::Identity());
}

template <class DataTypes, class ElementType>
auto CorotationalFEMForceField<DataTypes, ElementType>::computeElementLocalDisplacement(
const std::array<sofa::Coord_t<DataTypes>, trait::NumberOfNodesInElement>& nodes,
const std::array<sofa::Coord_t<DataTypes>, 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 <class DataTypes, class ElementType>
void CorotationalFEMForceField<DataTypes, ElementType>::computeElementsForces(
const sofa::simulation::Range<std::size_t>& range, const sofa::core::MechanicalParams* mparams,
Expand All @@ -102,15 +122,8 @@ void CorotationalFEMForceField<DataTypes, ElementType>::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];

Expand Down Expand Up @@ -206,7 +219,35 @@ SReal CorotationalFEMForceField<DataTypes, ElementType>::getPotentialEnergy(
const sofa::core::MechanicalParams*,
const sofa::DataVecCoord_t<DataTypes>& 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<DataTypes> 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<SReal>(0.5 * energy);
}

template <class DataTypes, class ElementType>
Expand Down
35 changes: 23 additions & 12 deletions .../Elastic/src/sofa/component/solidmechanics/fem/elastic/LinearSmallStrainFEMForceField.inl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -53,19 +53,10 @@ void LinearSmallStrainFEMForceField<DataTypes, ElementType>::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;
}
Expand Down Expand Up @@ -145,7 +136,27 @@ SReal LinearSmallStrainFEMForceField<DataTypes, ElementType>::getPotentialEnergy
const sofa::core::MechanicalParams*,
const sofa::DataVecCoord_t<DataTypes>& 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<DataTypes> 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<SReal>(0.5 * energy);
}

template <class DataTypes, class ElementType>
Expand Down
Loading