[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;
 }
@@ -266,17 +262,17 @@ type::Vector6 DiagonalMass<Vec3Types, GeometricalTypes>::getMomentumVec3Impl( co
 
 
 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

@@ -303,16 +303,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::Vector6 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;
 
@@ -351,8 +349,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) ;
@@ -377,9 +375,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.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::Vector6 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

@@ -2060,25 +2060,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;
 }
 
 
@@ -2114,20 +2107,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;
 }
@@ -2141,30 +2131,6 @@ type::Vector6 MeshMatrixMass<DataTypes, GeometricalTypes>::getMomentum ( const c
 }
 
 
-
-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)
 {

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

@@ -375,45 +375,20 @@ Vector6 UniformMass<Vec3Types>::getMomentumVec3DImpl ( const MechanicalParams*,
 
 template <class VecTypes>
 template <class T>
-SReal UniformMass<VecTypes>::getPotentialEnergyRigidImpl(const core::MechanicalParams* mparams,
-                                                                    const DataVecCoord& p_x) const
+SReal UniformMass<VecTypes>::getGravitationalPotentialEnergyRigidImpl(const core::MechanicalParams* mparams,
+                                                                    const DataVecCoord& p_x, const Deriv& g) const
 {
     SOFA_UNUSED(mparams) ;
     SReal e = 0;
     ReadAccessor< DataVecCoord > x = p_x;
     ReadAccessor<Data<SetIndexArray > > indices = d_indices;
 
-    typename Coord::Pos g ( getContext()->getGravity() );
     for (unsigned int i=0; i<indices.size(); i++)
-        e -= g*d_vertexMass.getValue().mass*x[indices[i]].getCenter();
+        e -= getVCenter(g) * d_vertexMass.getValue().mass * x[indices[i]].getCenter();
 
     return e;
 }
 
-template <class VecTypes>
-template <class T>
-void UniformMass<VecTypes>::addMDxToVectorVecImpl(linearalgebra::BaseVector *resVect,
-                                                     const VecDeriv* dx,
-                                                     SReal mFact,
-                                                     unsigned int& offset)
-{
-    unsigned int derivDim = (unsigned)Deriv::size();
-    double m = d_vertexMass.getValue();
-
-    ReadAccessor<Data<SetIndexArray > > indices = d_indices;
-
-    const SReal* g = getContext()->getGravity().ptr();
-
-    for (unsigned int i=0; i<indices.size(); i++)
-        for (unsigned int j=0; j<derivDim; j++)
-        {
-            if (dx != nullptr)
-                resVect->add(offset + indices[i] * derivDim + j, mFact * m * g[j] * (*dx)[indices[i]][0]);
-            else
-                resVect->add(offset + indices[i] * derivDim + j, mFact * m * g[j]);
-        }
-}
-
 
 template<> SOFA_COMPONENT_MASS_API
 void UniformMass<Rigid3Types>::constructor_message()
@@ -455,17 +430,17 @@ void UniformMass<Rigid2Types>::draw(const VisualParams* vparams)
 }
 
 template <> SOFA_COMPONENT_MASS_API
-SReal UniformMass<Rigid3Types>::getPotentialEnergy( const MechanicalParams* params,
-                                                                 const DataVecCoord& d_x ) const
+SReal UniformMass<Rigid3Types>::getGravitationalPotentialEnergy( const MechanicalParams* params,
+                                                                 const DataVecCoord& d_x, const Deriv& g ) const
 {
-    return getPotentialEnergyRigidImpl<Rigid3Types>(params, d_x) ;
+    return getGravitationalPotentialEnergyRigidImpl<Rigid3Types>(params, d_x, g) ;
 }
 
 template <> SOFA_COMPONENT_MASS_API
-SReal UniformMass<Rigid2Types>::getPotentialEnergy( const MechanicalParams* params,
-                                                                 const DataVecCoord& vx ) const
+SReal UniformMass<Rigid2Types>::getGravitationalPotentialEnergy( const MechanicalParams* params,
+                                                                 const DataVecCoord& vx, const Deriv& g ) const
 {
-    return getPotentialEnergyRigidImpl<Rigid2Types>(params, vx) ;
+    return getGravitationalPotentialEnergyRigidImpl<Rigid2Types>(params, vx, g) ;
 }
 
 template <> SOFA_COMPONENT_MASS_API
@@ -474,15 +449,6 @@ void UniformMass<Vec6Types>::draw(const core::visual::VisualParams* vparams)
     drawVec6Impl<Vec6Types>(vparams) ;
 }
 
-template <> SOFA_COMPONENT_MASS_API
-void UniformMass<Vec3Types>::addMDxToVector(linearalgebra::BaseVector *resVect,
-                                                     const VecDeriv* dx,
-                                                     SReal mFact,
-                                                     unsigned int& offset)
-{
-    addMDxToVectorVecImpl<Vec3Types>(resVect, dx,mFact,offset) ;
-}
-
 template <> SOFA_COMPONENT_MASS_API
 Vector6 UniformMass<Vec3Types>::getMomentum ( const MechanicalParams* params,
                                                        const DataVecCoord& d_x,