[Core] Explicit all input vectors in addDForce

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

@@ -35,14 +35,21 @@ BaseForceField::BaseForceField()
 {
 }
 
+void BaseForceField::addDForce(const MechanicalParams* mparams, MultiVecDerivId dfId)
+{
+    assert(mparams);
+    this->addDForce(mparams, dfId, mparams->dx(), mparams->x(), mparams->v());
+}
+
 void BaseForceField::addMBKdx(const MechanicalParams* mparams, MultiVecDerivId dfId)
 {
     const auto kFactor = sofa::core::mechanicalparams::kFactorIncludingRayleighDamping(mparams,rayleighStiffness.getValue());
     const auto bFactor = sofa::core::mechanicalparams::bFactor(mparams);
 
     if (kFactor != 0.0 || bFactor != 0.0)
     {
-        addDForce(mparams, dfId);
+        assert(mparams);
+        addDForce(mparams, dfId, mparams->dx(), mparams->x(), mparams->v());
     }
 }
 

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

@@ -96,17 +96,26 @@ class SOFA_CORE_API BaseForceField : public virtual StateAccessor
     ///
     /// If the ForceField can be represented as a matrix, this method computes
     /// \f[
-    ///                    df += kFactor K dx + bFactor B dx
+    ///                    df += kFactor K(x) dx + bFactor B(v) dx
     /// \f]
     /// where K is the stiffness matrix (associated with forces which derive from a potential),
     /// and B is the damping matrix (associated with viscous forces).
     ///
     /// \param mparams
-    /// - \a mparams->mFactor() is the  coefficient for mass contributions (i.e. second-order derivatives term in the ODE)
-    /// - \a mparams->kFactor() is the coefficient for stiffness contributions (i.e. DOFs term in the ODE)
-    /// - \a mparams->readDx() input vector
-    /// \param dfId the output vector
-    virtual void addDForce(const MechanicalParams* mparams, MultiVecDerivId dfId )=0;
+    /// - \a mparams->mFactor() is the  coefficient for mass contributions (i.e. second-order
+    /// derivatives term in the ODE)
+    /// - \a mparams->kFactor() is the coefficient for stiffness contributions (i.e. DOFs term in
+    /// the ODE)
+    /// \param dfId the cotangent output vector
+    /// \param dxId the tangent input vector
+    /// \param xId the position input vector
+    /// \param vId the velocity input vector
+    virtual void addDForce(const MechanicalParams* mparams,
+                           MultiVecDerivId dfId, ConstMultiVecDerivId dxId,
+                           ConstMultiVecCoordId xId, ConstMultiVecDerivId vId) = 0;
+
+    SOFA_ATTRIBUTE_DEPRECATED__ADDDFORCE_OVERLOAD()
+    virtual void addDForce(const MechanicalParams* mparams, MultiVecDerivId dfId) final;
 
     /// \brief Accumulate the contribution of M, B, and/or K matrices multiplied
     /// by the dx vector with the given coefficients.

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

@@ -99,15 +99,22 @@ class ForceField : public BaseForceField, public virtual SingleStateAccessor<TDa
     /// This method retrieves the force and dx vector from the MechanicalState
     /// and call the internal addDForce(VecDeriv&,const VecDeriv&,SReal,SReal)
     /// method implemented by the component.
-    void addDForce(const MechanicalParams* mparams, MultiVecDerivId dfId ) override;
+    void addDForce(const MechanicalParams* mparams, MultiVecDerivId dfId, ConstMultiVecDerivId dxId,
+                   ConstMultiVecCoordId xId, ConstMultiVecDerivId vId) override;
 
     /// Internal addDForce
     /// Overloaded function, usually called from the generic addDForce version.
     /// This addDForce version directly gives access to df and dx vectors through its parameters.
     /// @param mparams
     /// @param df Output vector to fill, result of \f$ kFactor K dx + bFactor B dx \f$
     /// @param dx Input vector used to compute \f$ df = kFactor K dx + bFactor B dx \f$
-    virtual void addDForce(const MechanicalParams* mparams, DataVecDeriv& df, const DataVecDeriv& dx ) = 0;
+    ///
+    ///
+    ///              ********************     WARNING     ********************
+    ///              This overload is deprecated!!! Use `doAddDForce` instead.
+    ///
+    SOFA_ATTRIBUTE_DEPRECATED__ADDDFORCE_DERIVED()
+    virtual void addDForce(const MechanicalParams*, DataVecDeriv&, const DataVecDeriv&) {}
 
     /// Get the potential energy associated to this ForceField.
     ///
@@ -228,6 +235,23 @@ class ForceField : public BaseForceField, public virtual SingleStateAccessor<TDa
         return name;
     }
 
+protected:
+
+    // all vectors involved in the addDForce operation
+    struct AddDForceVectors
+    {
+        DataVecDeriv& df;
+        const DataVecDeriv& dx;
+        const DataVecCoord& x;
+        const DataVecDeriv& v;
+    };
+
+    // Computes df += kFactor K(x) dx + bFactor B(v) dx
+    // with:
+    // K the derivative of the forces wrt the position
+    // B the derivative of the forces wrt the velocity
+    virtual void doAddDForce(const MechanicalParams* mparams, const AddDForceVectors& vectors);
+
 };
 
 #if !defined(SOFA_CORE_BEHAVIOR_FORCEFIELD_CPP)

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

@@ -51,7 +51,8 @@ void ForceField<DataTypes>::addForce(const MechanicalParams* mparams, MultiVecDe
 }
 
 template<class DataTypes>
-void ForceField<DataTypes>::addDForce(const MechanicalParams* mparams, MultiVecDerivId dfId )
+void ForceField<DataTypes>::addDForce(const MechanicalParams* mparams, MultiVecDerivId dfId,
+    ConstMultiVecDerivId dxId, ConstMultiVecCoordId xId, ConstMultiVecDerivId vId)
 {
     if (mparams && this->mstate)
     {
@@ -60,7 +61,19 @@ void ForceField<DataTypes>::addDForce(const MechanicalParams* mparams, MultiVecD
         mparams->setKFactorUsed(false);
 #endif
 
-        addDForce(mparams, *dfId[this->mstate.get()].write(), *mparams->readDx(this->mstate.get()));
+        Data<VecDeriv>* df = dfId[this->mstate.get()].write(); assert(df);
+        const Data<VecDeriv>* dx = dxId[this->mstate.get()].read(); assert(dx);
+        const Data<VecCoord>* x = xId[this->mstate.get()].read(); assert(x);
+        const Data<VecDeriv>* v = vId[this->mstate.get()].read(); assert(v);
+
+        const AddDForceVectors vectors {
+            .df = *df,
+            .dx = *dx,
+            .x = *x,
+            .v = *v
+        };
+
+        doAddDForce(mparams, vectors);
 
 #ifndef NDEBUG
         if (!mparams->getKFactorUsed())
@@ -76,7 +89,14 @@ void ForceField<DataTypes>::addDForce(const MechanicalParams* mparams, MultiVecD
     }
 }
 
-template<class DataTypes>
+template <class TDataTypes>
+void ForceField<TDataTypes>::doAddDForce(const MechanicalParams* mparams, const AddDForceVectors& vectors)
+{
+    // compatibility with legacy `addDForce`.
+    addDForce(mparams, vectors.df, vectors.dx);
+}
+
+template <class DataTypes>
 SReal ForceField<DataTypes>::getPotentialEnergy(const MechanicalParams* mparams) const
 {
     if (this->mstate)

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

@@ -90,12 +90,9 @@ class MixedInteractionForceField : public BaseInteractionForceField, public Pair
     /// explicitly (i.e. using its value at the beginning of the timestep).
     ///
     /// If the ForceField can be represented as a matrix, this method computes
-    /// $ df += kFactor K dx + bFactor B dx $
-    ///
-    /// This method retrieves the force and dx vector from the two MechanicalState
-    /// and call the internal addDForce(VecDeriv1&,VecDeriv2&,const VecDeriv1&,const VecDeriv2&,SReal,SReal)
-    /// method implemented by the component.
-    void addDForce(const MechanicalParams* mparams, MultiVecDerivId dfId ) override;
+    /// $ df += kFactor K(x) dx + bFactor B(v) dx $
+    void addDForce(const MechanicalParams* mparams, MultiVecDerivId dfId, ConstMultiVecDerivId dxId,
+        ConstMultiVecCoordId xId, ConstMultiVecDerivId vId) override;
 
 
     /// Get the potential energy associated to this ForceField.
@@ -132,7 +129,12 @@ class MixedInteractionForceField : public BaseInteractionForceField, public Pair
     ///
     /// This method must be implemented by the component, and is usually called
     /// by the generic MixedInteractionForceField::addDForce() method.
-
+    ///
+    ///
+    ///              ********************     WARNING     ********************
+    ///              This overload is deprecated!!! Use `doAddDForce` instead.
+    ///
+    SOFA_ATTRIBUTE_DEPRECATED__ADDDFORCE_DERIVED()
     virtual void addDForce(const MechanicalParams* mparams, DataVecDeriv1& df1, DataVecDeriv2& df2, const DataVecDeriv1& dx1, const DataVecDeriv2& dx2)=0;
 
     /// Get the potential energy associated to this ForceField.
@@ -155,6 +157,27 @@ class MixedInteractionForceField : public BaseInteractionForceField, public Pair
 
     BaseMechanicalState* getMechModel1() override { return Inherit2::getMechModel1(); }
     BaseMechanicalState* getMechModel2() override { return Inherit2::getMechModel2(); }
+
+protected:
+
+    // all vectors involved in the addDForce operation
+    template<class DataTypes>
+    struct AddDForceVectors
+    {
+        sofa::DataVecDeriv_t<DataTypes>& df;
+        const sofa::DataVecDeriv_t<DataTypes>& dx;
+        const sofa::DataVecCoord_t<DataTypes>& x;
+        const sofa::DataVecDeriv_t<DataTypes>& v;
+    };
+
+    // Computes df += kFactor K(x) dx + bFactor B(v) dx
+    // with:
+    // K the derivative of the forces wrt the position
+    // B the derivative of the forces wrt the velocity
+    virtual void doAddDForce(const MechanicalParams* mparams,
+        const AddDForceVectors<DataTypes1>& vectors1,
+        const AddDForceVectors<DataTypes2>& vectors2);
+
 };
 
 #if !defined(SOFA_CORE_BEHAVIOR_MIXEDINTERACTIONFORCEFIELD_CPP)

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

@@ -58,19 +58,50 @@ void MixedInteractionForceField<DataTypes1, DataTypes2>::addForce(const Mechanic
 }
 
 template<class DataTypes1, class DataTypes2>
-void MixedInteractionForceField<DataTypes1, DataTypes2>::addDForce(const MechanicalParams* mparams, MultiVecDerivId dfId )
+void MixedInteractionForceField<DataTypes1, DataTypes2>::addDForce(const MechanicalParams* mparams,
+    MultiVecDerivId dfId, ConstMultiVecDerivId dxId, ConstMultiVecCoordId xId, ConstMultiVecDerivId vId)
 {
     if (this->mstate1 && this->mstate2)
     {
         auto state1 = this->mstate1.get();
         auto state2 = this->mstate2.get();
-        addDForce( mparams, 
-                   *dfId[state1].write()    , *dfId[state2].write()   ,
-                   *mparams->readDx(state1) , *mparams->readDx(state2) );
+
+        Data<VecDeriv1>* df1 = dfId[state1].write(); assert(df1);
+        Data<VecDeriv2>* df2 = dfId[state2].write(); assert(df2);
+
+        const Data<VecDeriv1>* dx1 = dxId[state1].read(); assert(dx1);
+        const Data<VecDeriv2>* dx2 = dxId[state2].read(); assert(dx2);
+
+        const Data<VecCoord1>* x1 = xId[state1].read(); assert(x1);
+        const Data<VecCoord2>* x2 = xId[state2].read(); assert(x2);
+
+        const Data<VecDeriv1>* v1 = vId[state1].read(); assert(v1);
+        const Data<VecDeriv2>* v2 = vId[state2].read(); assert(v2);
+
+        const AddDForceVectors<DataTypes1> vectors1 {
+            .df = *df1,
+            .dx = *dx1,
+            .x = *x1,
+            .v = *v1
+        };
+
+        const AddDForceVectors<DataTypes2> vectors2 {
+            .df = *df2,
+            .dx = *dx2,
+            .x = *x2,
+            .v = *v2
+        };
+
+        doAddDForce(mparams, vectors1, vectors2);
     }
 }
 
-
+template <class TDataTypes1, class TDataTypes2>
+void MixedInteractionForceField<TDataTypes1, TDataTypes2>::doAddDForce(
+    const MechanicalParams* mparams, const AddDForceVectors<DataTypes1>& vectors1,
+    const AddDForceVectors<DataTypes2>& vectors2)
+{
+}
 
 template<class DataTypes1, class DataTypes2>
 SReal MixedInteractionForceField<DataTypes1, DataTypes2>::getPotentialEnergy(const MechanicalParams* mparams) const

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

@@ -103,12 +103,13 @@ class PairInteractionForceField : public BaseInteractionForceField, public PairS
     /// explicitly (i.e. using its value at the beginning of the timestep).
     ///
     /// If the ForceField can be represented as a matrix, this method computes
-    /// $ df += kFactor K dx + bFactor B dx $
+    /// $ df += kFactor K(x) dx + bFactor B(v) dx $
     ///
     /// This method retrieves the force and dx vector from the two MechanicalState
     /// and call the internal addDForce(VecDeriv&,VecDeriv&,const VecDeriv&,const VecDeriv&,SReal,SReal)
     /// method implemented by the component.
-    void addDForce(const MechanicalParams* mparams, MultiVecDerivId dfId ) override;
+    void addDForce(const MechanicalParams* mparams, MultiVecDerivId dfId, ConstMultiVecDerivId dxId,
+        ConstMultiVecCoordId xId, ConstMultiVecDerivId vId) override;
 
     /// Compute the force derivative given a small displacement from the
     /// position and velocity used in the previous call to addForce().
@@ -123,10 +124,12 @@ class PairInteractionForceField : public BaseInteractionForceField, public PairS
     /// This method must be implemented by the component, and is usually called
     /// by the generic PairInteractionForceField::addDForce() method.
     ///
-    /// To support old components that implement the deprecated addForce method
-    /// without scalar coefficients, it defaults to using a temporaty vector to
-    /// compute $ K dx $ and then manually scaling all values by kFactor.
-
+    ///
+    ///
+    ///              ********************     WARNING     ********************
+    ///              This overload is deprecated!!! Use `doAddDForce` instead.
+    ///
+    SOFA_ATTRIBUTE_DEPRECATED__ADDDFORCE_DERIVED()
     virtual void addDForce(const MechanicalParams* mparams, DataVecDeriv& df1, DataVecDeriv& df2, const DataVecDeriv& dx1, const DataVecDeriv& dx2)=0;
 
 
@@ -218,6 +221,24 @@ class PairInteractionForceField : public BaseInteractionForceField, public PairS
         return name;
     }
 
+protected:
+
+    // all vectors involved in the addDForce operation
+    struct AddDForceVectors
+    {
+        DataVecDeriv& df;
+        const DataVecDeriv& dx;
+        const DataVecCoord& x;
+        const DataVecDeriv& v;
+    };
+
+    // Computes df += kFactor K(x) dx + bFactor B(v) dx
+    // with:
+    // K the derivative of the forces wrt the position
+    // B the derivative of the forces wrt the velocity
+    virtual void doAddDForce(const MechanicalParams* mparams,
+        const AddDForceVectors& vectors1, const AddDForceVectors& vectors2);
+
 };
 
 #if !defined(SOFA_CORE_BEHAVIOR_PAIRINTERACTIONFORCEFIELD_CPP)

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

@@ -61,20 +61,54 @@ void PairInteractionForceField<DataTypes>::addForce(const MechanicalParams* mpar
 }
 
 template<class DataTypes>
-void PairInteractionForceField<DataTypes>::addDForce(const MechanicalParams* mparams, MultiVecDerivId dfId )
+void PairInteractionForceField<DataTypes>::addDForce(const MechanicalParams* mparams, MultiVecDerivId dfId,
+    ConstMultiVecDerivId dxId, ConstMultiVecCoordId xId, ConstMultiVecDerivId vId)
 {
     auto state1 = this->mstate1.get();
-    auto state2 = this->mstate2.get();    
+    auto state2 = this->mstate2.get();
     if (state1 && state2)
     {
-        addDForce(
-            mparams, *dfId[state1].write(), *dfId[state2].write(),
-            *mparams->readDx(state1), *mparams->readDx(state2));
+        Data<VecDeriv>* df1 = dfId[state1].write(); assert(df1);
+        Data<VecDeriv>* df2 = dfId[state2].write(); assert(df2);
+
+        const Data<VecDeriv>* dx1 = dxId[state1].read(); assert(dx1);
+        const Data<VecDeriv>* dx2 = dxId[state2].read(); assert(dx2);
+
+        const Data<VecCoord>* x1 = xId[state1].read(); assert(x1);
+        const Data<VecCoord>* x2 = xId[state2].read(); assert(x2);
+
+        const Data<VecDeriv>* v1 = vId[state1].read(); assert(v1);
+        const Data<VecDeriv>* v2 = vId[state2].read(); assert(v2);
+
+        const AddDForceVectors vectors1 {
+            .df = *df1,
+            .dx = *dx1,
+            .x = *x1,
+            .v = *v1
+        };
+
+        const AddDForceVectors vectors2 {
+            .df = *df2,
+            .dx = *dx2,
+            .x = *x2,
+            .v = *v2
+        };
+
+        doAddDForce(mparams, vectors1, vectors2);
     }
     else
         msg_error() << "PairInteractionForceField<DataTypes>::addDForce(const MechanicalParams* /*mparams*/, MultiVecDerivId /*fId*/ ), mstate missing";
 }
 
+template <class TDataTypes>
+void PairInteractionForceField<TDataTypes>::doAddDForce(const MechanicalParams* mparams,
+                                                        const AddDForceVectors& vectors1,
+                                                        const AddDForceVectors& vectors2)
+{
+    // compatibility with legacy `addDForce`.
+    addDForce(mparams, vectors1.df, vectors2.df, vectors1.dx, vectors2.dx);
+}
+
 template<class DataTypes>
 SReal PairInteractionForceField<DataTypes>::getPotentialEnergy(const MechanicalParams* mparams) const
 {