[All] Propose a new gravity API

Sofa/Component/Mass/src/sofa/component/mass/DiagonalMass.cpp

@@ -39,21 +39,17 @@ using namespace sofa::defaulttype;
 
 template <class RigidTypes, class GeometricalTypes>
 template <class T>
-SReal DiagonalMass<RigidTypes, GeometricalTypes>::getPotentialEnergyRigidImpl( const MechanicalParams* mparams,
-                                                                        const DataVecCoord& x) const
+SReal DiagonalMass<RigidTypes, GeometricalTypes>::getGravitationalPotentialEnergyRigidImpl( const MechanicalParams* mparams,
+                                                                        const DataVecCoord& x, const Deriv& g) const
 {
     SOFA_UNUSED(mparams) ;
     SReal e = 0;
     const MassVector &masses= d_vertexMass.getValue();
     const VecCoord& _x = x.getValue();
 
-    // gravity
-    Vec3d g ( this->getContext()->getGravity() );
-    Deriv theGravity;
-    RigidTypes::set( theGravity, g[0], g[1], g[2]);
     for (unsigned int i=0; i<_x.size(); i++)
     {
-        e -= getVCenter(theGravity) * masses[i].mass * _x[i].getCenter();
+        e -= getVCenter(g) * masses[i].mass * _x[i].getCenter();
     }
     return e;
 }
@@ -65,7 +61,7 @@ void DiagonalMass<RigidTypes, GeometricalTypes>::drawRigid3dImpl(const VisualPar
 {
     const MassVector &masses= d_vertexMass.getValue();
     if (!vparams->displayFlags().getShowBehaviorModels()) return;
-    const VecCoord& x =mstate->read(core::ConstVecCoordId::position())->getValue();
+    const VecCoord& x =this->mstate->read(core::ConstVecCoordId::position())->getValue();
 
     if(masses.size() != x.size()) return;
 
@@ -113,7 +109,7 @@ void DiagonalMass<RigidTypes, GeometricalTypes>::drawRigid2dImpl(const VisualPar
 {
     const MassVector &masses= d_vertexMass.getValue();
     if (!vparams->displayFlags().getShowBehaviorModels()) return;
-    const VecCoord& x =mstate->read(core::ConstVecCoordId::position())->getValue();
+    const VecCoord& x =this->mstate->read(core::ConstVecCoordId::position())->getValue();
     for (unsigned int i=0; i<x.size(); i++)
     {
         if (masses[i].mass == 0) continue;
@@ -266,17 +262,17 @@ type::Vec6 DiagonalMass<Vec3Types, GeometricalTypes>::getMomentumVec3Impl( const
 
 
 template <>
-SReal DiagonalMass<Rigid3Types>::getPotentialEnergy( const MechanicalParams* mparams,
-                                                                   const DataVecCoord& x) const
+SReal DiagonalMass<Rigid3Types>::getGravitationalPotentialEnergy( const MechanicalParams* mparams,
+                                                                   const DataVecCoord& x, const Deriv& g) const
 {
-    return getPotentialEnergyRigidImpl<Rigid3Types>(mparams, x) ;
+    return getGravitationalPotentialEnergyRigidImpl<Rigid3Types>(mparams, x, g) ;
 }
 
 template <>
-SReal DiagonalMass<Rigid2Types>::getPotentialEnergy( const MechanicalParams* mparams,
-                                                                   const DataVecCoord& x) const
+SReal DiagonalMass<Rigid2Types>::getGravitationalPotentialEnergy( const MechanicalParams* mparams,
+                                                                   const DataVecCoord& x, const Deriv& g) const
 {
-    return getPotentialEnergyRigidImpl<Rigid2Types>(mparams, x) ;
+    return getGravitationalPotentialEnergyRigidImpl<Rigid2Types>(mparams, x, g) ;
 }
 
 template <>

Sofa/Component/Mass/src/sofa/component/mass/DiagonalMass.h

@@ -124,7 +124,6 @@ class DiagonalMass : public core::behavior::Mass<DataTypes>
     /// Bring inherited attributes and function in the current lookup context.
     /// otherwise any access to the base::attribute would require
     /// the "this->" approach.
-    using core::behavior::ForceField<DataTypes>::mstate ;
     using core::objectmodel::BaseObject::getContext;
     ////////////////////////////////////////////////////////////////////////////
 
@@ -303,16 +302,14 @@ class DiagonalMass : public core::behavior::Mass<DataTypes>
 
     void accFromF(const core::MechanicalParams* mparams, DataVecDeriv& a, const DataVecDeriv& f) override;
 
-    void addForce(const core::MechanicalParams* mparams, DataVecDeriv& f, const DataVecCoord& x, const DataVecDeriv& v) override;
+    void addGravitationalForce(const core::MechanicalParams* mparams, DataVecDeriv& f, const DataVecCoord& x, const DataVecDeriv& v, const Deriv& gravity) override; ///< Mg force in a lumped gravity field
 
-    SReal getKineticEnergy(const core::MechanicalParams* mparams, const DataVecDeriv& v) const override;  ///< vMv/2 using dof->getV() override
+    SReal getGravitationalPotentialEnergy(const core::MechanicalParams* mparams, const DataVecCoord& x, const Deriv& gravity) const override;   ///< Mgx potential in a uniform gravity field, null at origin
 
-    SReal getPotentialEnergy(const core::MechanicalParams* mparams, const DataVecCoord& x) const override;   ///< Mgx potential in a uniform gravity field, null at origin
+    SReal getKineticEnergy(const core::MechanicalParams* mparams, const DataVecDeriv& v) const override;  ///< vMv/2 using dof->getV() override
 
     type::Vec6 getMomentum(const core::MechanicalParams* mparams, const DataVecCoord& x, const DataVecDeriv& v) const override;  ///< (Mv,cross(x,Mv)+Iw) override
 
-    void addGravityToV(const core::MechanicalParams* mparams, DataVecDeriv& d_v) override;
-
     /// Add Mass contribution to global Matrix assembling
     void addMToMatrix(const core::MechanicalParams *mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) override;
 
@@ -337,8 +334,8 @@ class DiagonalMass : public core::behavior::Mass<DataTypes>
 
 private:
     template <class T>
-    SReal getPotentialEnergyRigidImpl( const core::MechanicalParams* mparams,
-                                       const DataVecCoord& x) const ;
+    SReal getGravitationalPotentialEnergyRigidImpl( const core::MechanicalParams* mparams,
+                                       const DataVecCoord& x, const Deriv& g) const ;
 
     template <class T>
     void drawRigid3dImpl(const core::visual::VisualParams* vparams) ;
@@ -363,9 +360,9 @@ class DiagonalMass : public core::behavior::Mass<DataTypes>
 
 // Specialization for rigids
 template <>
-SReal DiagonalMass<defaulttype::Rigid3Types>::getPotentialEnergy( const core::MechanicalParams* mparams, const DataVecCoord& x) const;
+SReal DiagonalMass<defaulttype::Rigid3Types>::getGravitationalPotentialEnergy( const core::MechanicalParams* mparams, const DataVecCoord& x, const Deriv& g) const;
 template <>
-SReal DiagonalMass<defaulttype::Rigid2Types>::getPotentialEnergy( const core::MechanicalParams* mparams, const DataVecCoord& x) const;
+SReal DiagonalMass<defaulttype::Rigid2Types>::getGravitationalPotentialEnergy( const core::MechanicalParams* mparams, const DataVecCoord& x, const Deriv& g) const;
 template <>
 void DiagonalMass<defaulttype::Rigid3Types>::draw(const core::visual::VisualParams* vparams);
 template <>

Sofa/Component/Mass/src/sofa/component/mass/DiagonalMass.inl

@@ -589,19 +589,17 @@ SReal DiagonalMass<DataTypes, GeometricalTypes>::getKineticEnergy( const core::M
 }
 
 template <class DataTypes, class GeometricalTypes>
-SReal DiagonalMass<DataTypes, GeometricalTypes>::getPotentialEnergy( const core::MechanicalParams* /*mparams*/, const DataVecCoord& x ) const
+SReal DiagonalMass<DataTypes, GeometricalTypes>::getGravitationalPotentialEnergy( const core::MechanicalParams* mparams, const DataVecCoord& x, const Deriv& gravity ) const
 {
+    SOFA_UNUSED(mparams);
 
     const MassVector &masses= d_vertexMass.getValue();
     helper::ReadAccessor< DataVecCoord > _x = x;
     SReal e = 0;
-    // gravity
-    type::Vec3d g ( this->getContext()->getGravity() );
-    Deriv theGravity;
-    DataTypes::set ( theGravity, g[0], g[1], g[2]);
+
     for (unsigned int i=0; i<masses.size(); i++)
     {
-        e -= type::dot(theGravity, _x[i]) * masses[i];
+        e -= type::dot(gravity, _x[i]) * masses[i];
     }
     return e;
 }
@@ -1369,48 +1367,20 @@ void DiagonalMass<DataTypes, GeometricalTypes>::doUpdateInternal()
     printMass();
 }
 
-
-template <class DataTypes, class GeometricalTypes>
-void DiagonalMass<DataTypes, GeometricalTypes>::addGravityToV(const core::MechanicalParams* mparams, DataVecDeriv& d_v)
-{
-    if(mparams)
-    {
-        VecDeriv& v = *d_v.beginEdit();
-        // gravity
-        sofa::type::Vec3d g ( this->getContext()->getGravity() );
-        Deriv theGravity;
-        DataTypes::set ( theGravity, g[0], g[1], g[2]);
-        Deriv hg = theGravity * typename DataTypes::Real(sofa::core::mechanicalparams::dt(mparams));
-
-        for (unsigned int i=0; i<v.size(); i++)
-        {
-            v[i] += hg;
-        }
-        d_v.endEdit();
-    }
-}
-
-
 template <class DataTypes, class GeometricalTypes>
-void DiagonalMass<DataTypes, GeometricalTypes>::addForce(const core::MechanicalParams* /*mparams*/, DataVecDeriv& f, const DataVecCoord& , const DataVecDeriv& )
+void DiagonalMass<DataTypes, GeometricalTypes>::addGravitationalForce(const core::MechanicalParams* mparams, DataVecDeriv& f, const DataVecCoord& x, const DataVecDeriv& v, const Deriv& gravity)
 {
-    //if gravity was added separately (in solver's "solve" method), then nothing to do here
-    if(this->m_separateGravity.getValue())
-        return;
+    SOFA_UNUSED(mparams);
+    SOFA_UNUSED(x);
+    SOFA_UNUSED(v);
 
     const MassVector &masses= d_vertexMass.getValue();
     helper::WriteOnlyAccessor< DataVecDeriv > _f = f;
 
-    // gravity
-    sofa::type::Vec3d g ( this->getContext()->getGravity() );
-    Deriv theGravity;
-    DataTypes::set ( theGravity, g[0], g[1], g[2]);
-
-
     // add weight and inertia force
     for (unsigned int i=0; i<masses.size(); i++)
     {
-        _f[i] += theGravity*masses[i];
+        _f[i] += gravity*masses[i];
     }
 }
 

Sofa/Component/Mass/src/sofa/component/mass/MeshMatrixMass.cpp

@@ -43,9 +43,9 @@ Vec6 MeshMatrixMass<Vec3Types>::getMomentum ( const core::MechanicalParams*, con
     for( unsigned int i=0 ; i<v.size() ; i++ )
     {
         Deriv linearMomentum = v[i] * vertexMass[i];
-        for( Index j=0 ; j<DataTypes::spatial_dimensions ; ++j ) momentum[j] += linearMomentum[j];
+        for( Index j=0 ; j<Vec3Types::spatial_dimensions ; ++j ) momentum[j] += linearMomentum[j];
         Deriv angularMomentum = cross( x[i], linearMomentum );
-        for( Index j=0 ; j<DataTypes::spatial_dimensions ; ++j ) momentum[3+j] += angularMomentum[j];
+        for( Index j=0 ; j<Vec3Types::spatial_dimensions ; ++j ) momentum[3+j] += angularMomentum[j];
     }
 
     const auto& edges = l_topology->getEdges();
@@ -58,14 +58,14 @@ Vec6 MeshMatrixMass<Vec3Types>::getMomentum ( const core::MechanicalParams*, con
         const MassType m = edgeMass[i] * static_cast<MassType>(0.5);
 
         Deriv linearMomentum = v[v0] * m;
-        for( Index j=0 ; j<DataTypes::spatial_dimensions ; ++j ) momentum[j] += linearMomentum[j];
+        for( Index j=0 ; j<Vec3Types::spatial_dimensions ; ++j ) momentum[j] += linearMomentum[j];
         Deriv angularMomentum = cross( x[v0], linearMomentum );
-        for( Index j=0 ; j<DataTypes::spatial_dimensions ; ++j ) momentum[3+j] += angularMomentum[j];
+        for( Index j=0 ; j<Vec3Types::spatial_dimensions ; ++j ) momentum[3+j] += angularMomentum[j];
 
         linearMomentum = v[v1] * m;
-        for( Index j=0 ; j<DataTypes::spatial_dimensions ; ++j ) momentum[j] += linearMomentum[j];
+        for( Index j=0 ; j<Vec3Types::spatial_dimensions ; ++j ) momentum[j] += linearMomentum[j];
         angularMomentum = cross( x[v1], linearMomentum );
-        for( Index j=0 ; j<DataTypes::spatial_dimensions ; ++j ) momentum[3+j] += angularMomentum[j];
+        for( Index j=0 ; j<Vec3Types::spatial_dimensions ; ++j ) momentum[3+j] += angularMomentum[j];
     }
 
     return momentum;

Sofa/Component/Mass/src/sofa/component/mass/MeshMatrixMass.h

@@ -204,16 +204,14 @@ class MeshMatrixMass : public core::behavior::Mass<DataTypes>
 
     void accFromF(const core::MechanicalParams*, DataVecDeriv& a, const DataVecDeriv& f) override; // This function can't be used as it use M^-1
 
-    void addForce(const core::MechanicalParams*, DataVecDeriv& f, const DataVecCoord& x, const DataVecDeriv& v) override;
+    void addGravitationalForce(const core::MechanicalParams* mparams, DataVecDeriv& f, const DataVecCoord& x, const DataVecDeriv& v, const Deriv& gravity) override; ///< Mg force in a homogeneous gravity field
 
-    SReal getKineticEnergy(const core::MechanicalParams*, const DataVecDeriv& v) const override;  ///< vMv/2 using dof->getV() override
+    SReal getGravitationalPotentialEnergy(const core::MechanicalParams*, const DataVecCoord& x, const Deriv& gravity) const override;   ///< Mgx potential in a uniform gravity field, null at origin
 
-    SReal getPotentialEnergy(const core::MechanicalParams*, const DataVecCoord& x) const override;   ///< Mgx potential in a uniform gravity field, null at origin
+    SReal getKineticEnergy(const core::MechanicalParams*, const DataVecDeriv& v) const override;  ///< vMv/2 using dof->getV() override
 
     type::Vec6 getMomentum(const core::MechanicalParams* mparams, const DataVecCoord& x, const DataVecDeriv& v) const override;  ///< (Mv,cross(x,Mv)) override
 
-    void addGravityToV(const core::MechanicalParams* mparams, DataVecDeriv& d_v) override;
-
     bool isDiagonal() const override { return isLumped(); }
 
 

Sofa/Component/Mass/src/sofa/component/mass/MeshMatrixMass.inl

@@ -2076,25 +2076,18 @@ void MeshMatrixMass<DataTypes, GeometricalTypes>::accFromF(const core::Mechanica
 
 
 template <class DataTypes, class GeometricalTypes>
-void MeshMatrixMass<DataTypes, GeometricalTypes>::addForce(const core::MechanicalParams*, DataVecDeriv& vf, const DataVecCoord& , const DataVecDeriv& )
+void MeshMatrixMass<DataTypes, GeometricalTypes>::addGravitationalForce(const core::MechanicalParams* mparams, DataVecDeriv& vf, const DataVecCoord& x, const DataVecDeriv& v, const Deriv& gravity )
 {
-
-    //if gravity was added separately (in solver's "solve" method), then nothing to do here
-    if(this->m_separateGravity.getValue())
-        return ;
+    SOFA_UNUSED(mparams);
+    SOFA_UNUSED(x);
+    SOFA_UNUSED(v);
 
     helper::WriteAccessor< DataVecDeriv > f = vf;
-
     const auto &vertexMass= d_vertexMass.getValue();
 
-    // gravity
-    type::Vec3d g ( this->getContext()->getGravity() );
-    Deriv theGravity;
-    DataTypes::set ( theGravity, g[0], g[1], g[2]);
-
     // add weight and inertia force
     for (unsigned int i=0; i<f.size(); ++i)
-        f[i] += theGravity * vertexMass[i] * m_massLumpingCoeff;
+        f[i] += gravity * vertexMass[i] * m_massLumpingCoeff;
 }
 
 
@@ -2130,20 +2123,17 @@ SReal MeshMatrixMass<DataTypes, GeometricalTypes>::getKineticEnergy( const core:
 
 
 template <class DataTypes, class GeometricalTypes>
-SReal MeshMatrixMass<DataTypes, GeometricalTypes>::getPotentialEnergy( const core::MechanicalParams*, const DataVecCoord& vx) const
+SReal MeshMatrixMass<DataTypes, GeometricalTypes>::getGravitationalPotentialEnergy( const core::MechanicalParams* mparams, const DataVecCoord& vx, const Deriv& gravity) const
 {
+    SOFA_UNUSED(mparams);
+
     const auto &vertexMass= d_vertexMass.getValue();
 
     helper::ReadAccessor< DataVecCoord > x = vx;
-
     SReal e = 0;
-    // gravity
-    type::Vec3d g ( this->getContext()->getGravity() );
-    Deriv theGravity;
-    DataTypes::set ( theGravity, g[0], g[1], g[2]);
 
     for (unsigned int i=0; i<x.size(); i++)
-        e -= dot(theGravity,x[i])*vertexMass[i] * m_massLumpingCoeff;
+        e -= dot(gravity,x[i])*vertexMass[i] * m_massLumpingCoeff;
 
     return e;
 }
@@ -2157,30 +2147,6 @@ type::Vec6 MeshMatrixMass<DataTypes, GeometricalTypes>::getMomentum ( const core
 }
 
 
-
-template <class DataTypes, class GeometricalTypes>
-void MeshMatrixMass<DataTypes, GeometricalTypes>::addGravityToV(const core::MechanicalParams* mparams, DataVecDeriv& d_v)
-{
-    if(this->mstate && mparams)
-    {
-        VecDeriv& v = *d_v.beginEdit();
-
-        // gravity
-        type::Vec3d g ( this->getContext()->getGravity() );
-        Deriv theGravity;
-        DataTypes::set ( theGravity, g[0], g[1], g[2]);
-        Deriv hg = theGravity * (typename DataTypes::Real(sofa::core::mechanicalparams::dt(mparams)));
-
-        for (unsigned int i=0; i<v.size(); i++)
-            v[i] += hg;
-        d_v.endEdit();
-    }
-
-}
-
-
-
-
 template <class DataTypes, class GeometricalTypes>
 void MeshMatrixMass<DataTypes, GeometricalTypes>::addMToMatrix(const core::MechanicalParams *mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix)
 {