[Core] Explicit x vector in getPotentialEnergy

Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/HexahedronFEMForceFieldAndMass.h

@@ -97,14 +97,9 @@ class HexahedronFEMForceFieldAndMass : virtual public core::behavior::Mass<DataT
         return 0.0;
     }
 
-    SReal getPotentialEnergy(const core::MechanicalParams* /*mparams*/, const DataVecCoord&  /* x */) const override
-    {
-        msg_warning() << "Method getPotentialEnergy not implemented yet.";
-        return 0.0;
-    }
-
-    SReal getPotentialEnergy(const core::MechanicalParams* /*mparams*/) const override
+    SReal getPotentialEnergy(const core::MechanicalParams* /*mparams*/, const DataVecCoord& x) const override
     {
+        SOFA_UNUSED(x);
         msg_warning() << "Method getPotentialEnergy not implemented yet.";
         return 0.0;
     }

Sofa/Component/SolidMechanics/TensorMass/src/sofa/component/solidmechanics/tensormass/TetrahedralTensorMassForceField.h

@@ -109,16 +109,11 @@ class TetrahedralTensorMassForceField : public core::behavior::ForceField<DataTy
     void addDForce(const core::MechanicalParams* mparams, DataVecDeriv& d_df, const DataVecDeriv& d_dx) override;
     void buildStiffnessMatrix(sofa::core::behavior::StiffnessMatrix* matrix) override;
     void buildDampingMatrix(core::behavior::DampingMatrix* /*matrix*/) final;
-    SReal getPotentialEnergy(const core::MechanicalParams* /*mparams*/, const DataVecCoord&  /* x */) const override
-    {
-        msg_warning() << "Method getPotentialEnergy not implemented yet.";
-        return 0.0;
-    }
 
     virtual Real getLambda() const { return lambda;}
     virtual Real getMu() const { return mu;}
 
-    SReal getPotentialEnergy(const core::MechanicalParams* mparams) const override;
+    SReal getPotentialEnergy(const core::MechanicalParams* mparams, const DataVecCoord& x) const override;
     void setYoungModulus(const Real modulus)
     {
         d_youngModulus.setValue(modulus);

Sofa/Component/SolidMechanics/TensorMass/src/sofa/component/solidmechanics/tensormass/TetrahedralTensorMassForceField.inl

@@ -224,7 +224,7 @@ void TetrahedralTensorMassForceField<DataTypes>::applyTetrahedronDestruction(con
 }
 
 
-template <class DataTypes> 
+template <class DataTypes>
 TetrahedralTensorMassForceField<DataTypes>::TetrahedralTensorMassForceField()
     : _initialPoints(0)
     , updateMatrix(true)
@@ -238,12 +238,12 @@ TetrahedralTensorMassForceField<DataTypes>::TetrahedralTensorMassForceField()
 {
 }
 
-template <class DataTypes> 
+template <class DataTypes>
 TetrahedralTensorMassForceField<DataTypes>::~TetrahedralTensorMassForceField()
 {
 }
 
-template <class DataTypes> void 
+template <class DataTypes> void
 TetrahedralTensorMassForceField<DataTypes>::init()
 {
     this->Inherited::init();
@@ -338,12 +338,10 @@ void TetrahedralTensorMassForceField<DataTypes>::initNeighbourhoodPoints() {}
 
 
 template <class DataTypes>
-SReal  TetrahedralTensorMassForceField<DataTypes>::getPotentialEnergy(const core::MechanicalParams* /* mparams */) const
+SReal TetrahedralTensorMassForceField<DataTypes>::getPotentialEnergy(const core::MechanicalParams* /* mparams */, const DataVecCoord& x) const
 {
     SCOPED_TIMER("getPotentialEnergy");
 
-    const VecCoord& x = this->mstate->read(core::vec_id::read_access::position)->getValue();
-
     SReal energy=0;
 
     unsigned int v0,v1;
@@ -355,25 +353,23 @@ SReal  TetrahedralTensorMassForceField<DataTypes>::getPotentialEnergy(const core
     Deriv force,dp;
     Deriv dp0,dp1;
 
-    for(int i=0; i<nbEdges; i++ )
+    const auto xAccessor = sofa::helper::getReadAccessor(x);
+
+    for (int i = 0; i < nbEdges; i++)
     {
-        einfo=&edgeInf[i];
-        v0=m_topology->getEdge(i)[0];
-        v1=m_topology->getEdge(i)[1];
-        dp0=x[v0]-_initialPoints[v0];
-        dp1=x[v1]-_initialPoints[v1];
-        dp = dp1-dp0;
-        force=einfo->DfDx*dp;
-        energy+=dot(force,dp1);
-        force=einfo->DfDx.multTranspose(dp);
-        energy-=dot(force,dp0);
+        einfo = &edgeInf[i];
+        v0 = m_topology->getEdge(i)[0];
+        v1 = m_topology->getEdge(i)[1];
+        dp0 = xAccessor[v0] - _initialPoints[v0];
+        dp1 = xAccessor[v1] - _initialPoints[v1];
+        dp = dp1 - dp0;
+        force = einfo->DfDx * dp;
+        energy += dot(force, dp1);
+        force = einfo->DfDx.multTranspose(dp);
+        energy -= dot(force, dp0);
     }
 
-    energy/=-2.0;
-
-    msg_info() << "energy="<<energy ;
-
-    return(energy);
+    return -energy / 2_sreal;
 }
 
 template <class DataTypes>

Sofa/framework/Core/src/sofa/core/behavior/BaseForceField.cpp

@@ -46,6 +46,11 @@ void BaseForceField::addMBKdx(const MechanicalParams* mparams, MultiVecDerivId d
     }
 }
 
+SReal BaseForceField::getPotentialEnergy(const MechanicalParams* mparams) const
+{
+    return this->getPotentialEnergy(mparams, mparams->x());
+}
+
 void BaseForceField::addBToMatrix(const MechanicalParams* /*mparams*/, const sofa::core::behavior::MultiMatrixAccessor* /*matrix*/)
 {
 }

Sofa/framework/Core/src/sofa/core/behavior/BaseForceField.h

@@ -134,10 +134,14 @@ class SOFA_CORE_API BaseForceField : public virtual StateAccessor
 
     /// \brief Get the potential energy associated to this ForceField during the
     /// last call of addForce( const MechanicalParams* mparams );
-    ///
-    /// Used to estimate the total energy of the system by some
-    /// post-stabilization techniques.
-    virtual SReal getPotentialEnergy( const MechanicalParams* mparams = mechanicalparams::defaultInstance() ) const=0;
+    /// \note Used to estimate the total energy of the system by some post-stabilization techniques.
+    /// \param mparams
+    /// \param xId The potential energy is evaluated on this vector of generalized coordinates
+    virtual SReal getPotentialEnergy(const MechanicalParams* mparams,
+                                     ConstMultiVecCoordId xId) const = 0;
+
+    SOFA_ATTRIBUTE_DEPRECATED__GETPOTENTIALENERGY_OVERLOAD()
+    virtual SReal getPotentialEnergy( const MechanicalParams* mparams = mechanicalparams::defaultInstance() ) const final;
     /// @}
 
 

Sofa/framework/Core/src/sofa/core/behavior/BaseMass.cpp

@@ -49,6 +49,11 @@ bool BaseMass::removeInNode( objectmodel::BaseNode* node )
     return true;
 }
 
+SReal BaseMass::getPotentialEnergy(const MechanicalParams* mparams) const
+{
+    return this->getPotentialEnergy(mparams, mparams->x());
+}
+
 void BaseMass::buildMassMatrix(sofa::core::behavior::MassMatrixAccumulator* matrices)
 {
     static std::set<BaseMass*> hasEmittedWarning;

Sofa/framework/Core/src/sofa/core/behavior/BaseMass.h

@@ -72,7 +72,10 @@ class SOFA_CORE_API BaseMass : public virtual StateAccessor
     /// vMv/2
     virtual SReal getKineticEnergy(const MechanicalParams* mparams = mechanicalparams::defaultInstance()) const = 0;
     /// Mgx
-    virtual SReal getPotentialEnergy(const MechanicalParams* mparams = mechanicalparams::defaultInstance()) const = 0;
+    virtual SReal getPotentialEnergy(const MechanicalParams* mparams, ConstMultiVecCoordId xId) const = 0;
+
+    SOFA_ATTRIBUTE_DEPRECATED__GETPOTENTIALENERGY_OVERLOAD()
+    virtual SReal getPotentialEnergy(const MechanicalParams* mparams = mechanicalparams::defaultInstance()) const final;
 
     /// (Mv,xMv+Iw) (linear and angular momenta against world origin)
     virtual type::Vec6 getMomentum(const MechanicalParams* mparams = mechanicalparams::defaultInstance()) const = 0;

Sofa/framework/Core/src/sofa/core/behavior/ForceField.h

@@ -116,10 +116,10 @@ class ForceField : public BaseForceField, public virtual SingleStateAccessor<TDa
     ///
     /// This method must be implemented by the component, and is usually called
     /// by the generic ForceField::getPotentialEnergy(const MechanicalParams* mparams) method.
-    SReal getPotentialEnergy(const MechanicalParams* mparams) const override;
+    SReal getPotentialEnergy(const MechanicalParams* mparams, ConstMultiVecCoordId xId) const override;
 
     virtual SReal getPotentialEnergy(const MechanicalParams* /*mparams*/, const DataVecCoord& x) const = 0;
-
+    using BaseForceField::getPotentialEnergy;
 
     /// @}
 

Sofa/framework/Core/src/sofa/core/behavior/ForceField.inl

@@ -77,10 +77,13 @@ void ForceField<DataTypes>::addDForce(const MechanicalParams* mparams, MultiVecD
 }
 
 template<class DataTypes>
-SReal ForceField<DataTypes>::getPotentialEnergy(const MechanicalParams* mparams) const
+SReal ForceField<DataTypes>::getPotentialEnergy(const MechanicalParams* mparams, ConstMultiVecCoordId xId) const
 {
     if (this->mstate)
-        return getPotentialEnergy(mparams, *mparams->readX(this->mstate.get()));
+    {
+        const DataVecCoord* x = xId[this->mstate.get()].read(); assert(x);
+        return getPotentialEnergy(mparams, *x);
+    }
     return 0;
 }
 

Sofa/framework/Core/src/sofa/core/behavior/Mass.h

@@ -99,12 +99,9 @@ class Mass : virtual public ForceField<DataTypes>, public BaseMass
     SReal getKineticEnergy( const MechanicalParams* mparams) const override;
     virtual SReal getKineticEnergy( const MechanicalParams* mparams, const DataVecDeriv& v) const;
 
-    ///                         $ e = M g x $
-    ///
-    /// This method retrieves the positions vector and call the internal
-    /// getPotentialEnergy(const MechanicalParams*, const VecCoord&) method implemented by the component.
-    SReal getPotentialEnergy( const MechanicalParams* mparams) const override;
+    SReal getPotentialEnergy( const MechanicalParams* mparams, ConstMultiVecCoordId x) const override { return ForceField<DataTypes>::getPotentialEnergy(mparams, x); }
     SReal getPotentialEnergy( const MechanicalParams* mparams, const DataVecCoord& x  ) const override;
+    using BaseForceField::getPotentialEnergy;
 
 
     ///    $ m = ( Mv, cross(x,Mv)+Iw ) $

Sofa/framework/Core/src/sofa/core/behavior/Mass.inl

@@ -118,15 +118,6 @@ SReal Mass<DataTypes>::getKineticEnergy(const MechanicalParams* /*mparams*/, con
     return 0.0;
 }
 
-
-template<class DataTypes>
-SReal Mass<DataTypes>::getPotentialEnergy(const MechanicalParams* mparams) const
-{
-    if (this->mstate)
-        return getPotentialEnergy(mparams /* PARAMS FIRST */, *mparams->readX(this->mstate.get()));
-    return 0.0;
-}
-
 template<class DataTypes>
 SReal Mass<DataTypes>::getPotentialEnergy(const MechanicalParams* /*mparams*/, const DataVecCoord& /*x*/) const
 {
@@ -217,10 +208,14 @@ void Mass<DataTypes>::exportGnuplot(const MechanicalParams* mparams, SReal time)
 {
     if (m_gnuplotFileEnergy!=nullptr)
     {
-        (*m_gnuplotFileEnergy) << time <<"\t"<< this->getKineticEnergy(mparams)
-                               <<"\t"<< this->getPotentialEnergy(mparams)
-                              <<"\t"<< this->getPotentialEnergy(mparams)
-                                +this->getKineticEnergy(mparams)<< std::endl;
+        assert(mparams);
+        auto xId = mparams->x();
+        const auto kineticEnergy = this->getKineticEnergy(mparams);
+        const auto potentialEnergy = this->getPotentialEnergy(mparams, xId);
+        (*m_gnuplotFileEnergy) << time <<"\t"<< kineticEnergy
+                               <<"\t"<< potentialEnergy
+                              <<"\t"<< potentialEnergy
+                                +kineticEnergy<< std::endl;
     }
 }
 

Sofa/framework/Core/src/sofa/core/behavior/MixedInteractionForceField.h

@@ -106,7 +106,7 @@ class MixedInteractionForceField : public BaseInteractionForceField, public Pair
     /// This method retrieves the x vector from the MechanicalState and call
     /// the internal getPotentialEnergy(const VecCoord&,const VecCoord&) method implemented by
     /// the component.
-    SReal getPotentialEnergy(const MechanicalParams* mparams) const override;
+    SReal getPotentialEnergy(const MechanicalParams* mparams, ConstMultiVecCoordId xId) const override;
 
     /// Given the current position and velocity states, update the current force
     /// vector by computing and adding the forces associated with this

Sofa/framework/Core/src/sofa/core/behavior/MixedInteractionForceField.inl

@@ -73,10 +73,14 @@ void MixedInteractionForceField<DataTypes1, DataTypes2>::addDForce(const Mechani
 
 
 template<class DataTypes1, class DataTypes2>
-SReal MixedInteractionForceField<DataTypes1, DataTypes2>::getPotentialEnergy(const MechanicalParams* mparams) const
+SReal MixedInteractionForceField<DataTypes1, DataTypes2>::getPotentialEnergy(const MechanicalParams* mparams, ConstMultiVecCoordId xId) const
 {
     if (this->mstate1 && this->mstate2)
-        return getPotentialEnergy(mparams, *mparams->readX(this->mstate1.get()),*mparams->readX(this->mstate2.get()));
+    {
+        const DataVecCoord1* x1 = xId[this->mstate1.get()].read(); assert(x1);
+        const DataVecCoord2* x2 = xId[this->mstate2.get()].read(); assert(x2);
+        return getPotentialEnergy(mparams, *x1, *x2);
+    }
     else return 0;
 }
 

Sofa/framework/Core/src/sofa/core/behavior/PairInteractionForceField.h

@@ -138,7 +138,8 @@ class PairInteractionForceField : public BaseInteractionForceField, public PairS
     /// This method retrieves the x vector from the MechanicalState and call
     /// the internal getPotentialEnergy(const VecCoord&,const VecCoord&) method implemented by
     /// the component.
-    SReal getPotentialEnergy(const MechanicalParams* mparams) const override;
+    SReal getPotentialEnergy(const MechanicalParams* mparams, ConstMultiVecCoordId xId) const override;
+    using BaseForceField::getPotentialEnergy;
 
     /// Get the potential energy associated to this ForceField.
     ///

Sofa/framework/Core/src/sofa/core/behavior/PairInteractionForceField.inl

@@ -76,12 +76,16 @@ void PairInteractionForceField<DataTypes>::addDForce(const MechanicalParams* mpa
 }
 
 template<class DataTypes>
-SReal PairInteractionForceField<DataTypes>::getPotentialEnergy(const MechanicalParams* mparams) const
+SReal PairInteractionForceField<DataTypes>::getPotentialEnergy(const MechanicalParams* mparams, ConstMultiVecCoordId xId) const
 {
     auto state1 = this->mstate1.get();
     auto state2 = this->mstate2.get();
     if (state1 && state2)
-        return getPotentialEnergy(mparams, *mparams->readX(state1),*mparams->readX(state2));
+    {
+        const DataVecCoord* x1 = xId[state1].read(); assert(x1);
+        const DataVecCoord* x2 = xId[state2].read(); assert(x2);
+        return getPotentialEnergy(mparams, *x1, *x2);
+    }
     else return 0.0;
 }
 

Sofa/framework/Core/src/sofa/core/config.h.in

@@ -133,4 +133,11 @@ SOFA_ATTRIBUTE_DEPRECATED("v26.06", "v29.06", "Use toBaseComponent instead.")
 #else
 #define SOFA_CORE_DEPRECATED_REMOVE_CONTEXT() \
 SOFA_ATTRIBUTE_DISABLED("v26.12", "v27.12", "Use BaseContext instead of Context.")
-#endif
+#endif
+
+#ifdef SOFA_BUILD_SOFA_CORE
+#define SOFA_ATTRIBUTE_DEPRECATED__GETPOTENTIALENERGY_OVERLOAD()
+#else
+#define SOFA_ATTRIBUTE_DEPRECATED__GETPOTENTIALENERGY_OVERLOAD() \
+    SOFA_ATTRIBUTE_DEPRECATED("v26.12", "v27.06", "getPotentialEnergy must be called by providing the position vector.")
+#endif

Sofa/framework/Simulation/Core/src/sofa/simulation/MechanicalOperations.cpp

@@ -194,16 +194,22 @@ void MechanicalOperations::projectPosition(core::MultiVecCoordId x, SReal time)
     executeVisitor( MechanicalProjectPositionVisitor(&mparams, time, x) );
 }
 
-/// Apply projective constraints to the given velocity vector
-void MechanicalOperations::computeEnergy(SReal &kineticEnergy, SReal &potentialEnergy)
+void MechanicalOperations::computeEnergy(core::ConstMultiVecCoordId xId,
+    SReal& kineticEnergy, SReal& potentialEnergy)
 {
     kineticEnergy = 0;
     potentialEnergy = 0;
-    MechanicalComputeEnergyVisitor energyVisitor(&mparams);
+    MechanicalComputeEnergyVisitor energyVisitor(&mparams, xId);
     executeVisitor(&energyVisitor);
     kineticEnergy = energyVisitor.getKineticEnergy();
     potentialEnergy = energyVisitor.getPotentialEnergy();
 }
+
+void MechanicalOperations::computeEnergy(SReal &kineticEnergy, SReal &potentialEnergy)
+{
+    computeEnergy(mparams.x(), kineticEnergy, potentialEnergy);
+}
+
 /// Apply projective constraints to the given velocity vector
 void MechanicalOperations::projectVelocity(core::MultiVecDerivId v, SReal time)
 {

Sofa/framework/Simulation/Core/src/sofa/simulation/MechanicalOperations.h

@@ -77,6 +77,8 @@ class SOFA_SIMULATION_CORE_API MechanicalOperations
     void integrateVelocity(core::MultiVecDerivId res, core::ConstMultiVecCoordId x, core::ConstMultiVecDerivId v, SReal dt); ///< res = x + v.dt
     void accFromF(core::MultiVecDerivId a, core::ConstMultiVecDerivId f); ///< a = M^-1 . f
     /// Compute Energy
+    void computeEnergy(core::ConstMultiVecCoordId xId, SReal &kineticEnergy, SReal &potentialEnergy);
+    SOFA_ATTRIBUTE_DEPRECATED__COMPUTEENERGY_OVERLOAD()
     void computeEnergy(SReal &kineticEnergy, SReal &potentialEnergy);
     /// Compute the current force (given the latest propagated position and velocity)
     void computeForce(core::MultiVecDerivId result, bool clear = true, bool accumulate = true);

Sofa/framework/Simulation/Core/src/sofa/simulation/config.h.in

@@ -221,3 +221,17 @@
 #define SOFA_ATTRIBUTE_DEPRECATED__MECHANICALOPERATIONS_PRINTWITHELAPSEDTIME() \
     SOFA_ATTRIBUTE_DISABLED("v26.12", "v27.06", "This method is unused.")
 #endif
+
+#ifdef SOFA_BUILD_SOFA_SIMULATION_CORE
+#define SOFA_ATTRIBUTE_DEPRECATED__MECHANICALCOMPUTEENERGYVISITOR_CONSTRUCTOR_OVERLOAD()
+#else
+#define SOFA_ATTRIBUTE_DEPRECATED__MECHANICALCOMPUTEENERGYVISITOR_CONSTRUCTOR_OVERLOAD() \
+    SOFA_ATTRIBUTE_DISABLED("v26.12", "v27.06", "Constructor must be used by providing the x vector.")
+#endif
+
+#ifdef SOFA_BUILD_SOFA_SIMULATION_CORE
+#define SOFA_ATTRIBUTE_DEPRECATED__COMPUTEENERGY_OVERLOAD()
+#else
+#define SOFA_ATTRIBUTE_DEPRECATED__COMPUTEENERGY_OVERLOAD() \
+    SOFA_ATTRIBUTE_DISABLED("v26.12", "v27.06", "computeEnergy must be used by providing the x vector.")
+#endif

Sofa/framework/Simulation/Core/src/sofa/simulation/mechanicalvisitor/MechanicalComputeEnergyVisitor.cpp

@@ -29,11 +29,16 @@ namespace sofa::simulation::mechanicalvisitor
 
 MechanicalComputeEnergyVisitor::MechanicalComputeEnergyVisitor(const sofa::core::MechanicalParams* mparams)
     : sofa::simulation::MechanicalVisitor(mparams)
-    , m_kineticEnergy(0.)
-    , m_potentialEnergy(0.)
 {
+    assert(mparams);
+    m_xId = mparams->x();
 }
 
+MechanicalComputeEnergyVisitor::MechanicalComputeEnergyVisitor(
+    const sofa::core::MechanicalParams* mparams, core::ConstMultiVecCoordId xId)
+    : sofa::simulation::MechanicalVisitor(mparams)
+    , m_xId(xId)
+{}
 
 MechanicalComputeEnergyVisitor::~MechanicalComputeEnergyVisitor()
 {
@@ -59,7 +64,7 @@ Visitor::Result MechanicalComputeEnergyVisitor::fwdMass(simulation::Node* /*node
 /// Process the BaseForceField
 Visitor::Result MechanicalComputeEnergyVisitor::fwdForceField(simulation::Node* /*node*/, sofa::core::behavior::BaseForceField* f)
 {
-    m_potentialEnergy += (SReal)f->getPotentialEnergy();
+    m_potentialEnergy += (SReal)f->getPotentialEnergy(mparams, m_xId);
     return RESULT_CONTINUE;
 }