[SofaBaseMechanics] Fix UniformMass topology changes handling mode.

SofaKernel/modules/SofaBaseMechanics/SofaBaseMechanics_test/UniformMass_test.cpp

@@ -113,7 +113,6 @@ struct UniformMassTest :  public BaseTest
         EXPECT_TRUE( m_mass->findData("showInitialCenterOfGravity") != nullptr ) ;
 
         EXPECT_TRUE( m_mass->findData("indices") != nullptr ) ;
-        EXPECT_TRUE( m_mass->findData("handleTopologicalChanges") != nullptr ) ;
         EXPECT_TRUE( m_mass->findData("preserveTotalMass") != nullptr ) ;
 
         EXPECT_TRUE( m_mass->findData("compute_mapping_inertia") != nullptr ) ;

SofaKernel/modules/SofaBaseMechanics/src/SofaBaseMechanics/UniformMass.cpp

@@ -209,7 +209,7 @@ void UniformMass<RigidTypes, MassType>::drawRigid2DImpl(const VisualParams* vpar
         return;
 
     const VecCoord& x =mstate->read(core::ConstVecCoordId::position())->getValue();
-    ReadAccessor<Data<vector<int> > > indices = d_indices;
+    ReadAccessor<Data<SetIndexArray > > indices = d_indices;
     type::Vec3d len;
 
     len[0] = len[1] = sqrt(d_vertexMass.getValue().inertiaMatrix);
@@ -232,7 +232,7 @@ void UniformMass<RigidTypes, MassType>::drawRigid3DImpl(const VisualParams* vpar
         return;
 
     const VecCoord& x =mstate->read(core::ConstVecCoordId::position())->getValue();
-    ReadAccessor<Data<vector<int> > > indices = d_indices;
+    ReadAccessor<Data<SetIndexArray > > indices = d_indices;
     typename RigidTypes::Vec3 gravityCenter;
     type::Vec3d len;
 
@@ -283,7 +283,7 @@ void UniformMass<Vec6Types, MassType>::drawVec6Impl(const core::visual::VisualPa
         return;
     const VecCoord& x =mstate->read(core::ConstVecCoordId::position())->getValue();
     const VecCoord& x0 = mstate->read(core::ConstVecCoordId::restPosition())->getValue();
-    ReadAccessor<Data<vector<int> > > indices = d_indices;
+    ReadAccessor<Data<SetIndexArray > > indices = d_indices;
 
     Mat3x3d R; R.identity();
 
@@ -327,7 +327,7 @@ Vector6 UniformMass<RigidTypes,MassType>::getMomentumRigid3DImpl( const Mechanic
 {
     ReadAccessor<DataVecDeriv> v = d_v;
     ReadAccessor<DataVecCoord> x = d_x;
-    ReadAccessor<Data<vector<int> > > indices = d_indices;
+    ReadAccessor<Data<SetIndexArray > > indices = d_indices;
 
     Real m = d_vertexMass.getValue().mass;
     const typename MassType::Mat3x3& I = d_vertexMass.getValue().inertiaMassMatrix;
@@ -354,7 +354,7 @@ Vector6 UniformMass<Vec3Types, MassType>::getMomentumVec3DImpl ( const Mechanica
 {
     ReadAccessor<DataVecDeriv> v = d_v;
     ReadAccessor<DataVecCoord> x = d_x;
-    ReadAccessor<Data<vector<int> > > indices = d_indices;
+    ReadAccessor<Data<SetIndexArray > > indices = d_indices;
 
     const MassType& m = d_vertexMass.getValue();
     type::Vec6d momentum;
@@ -379,7 +379,7 @@ SReal UniformMass<VecTypes, MassType>::getPotentialEnergyRigidImpl(const core::M
     SOFA_UNUSED(mparams) ;
     SReal e = 0;
     ReadAccessor< DataVecCoord > x = p_x;
-    ReadAccessor<Data<vector<int> > > indices = d_indices;
+    ReadAccessor<Data<SetIndexArray > > indices = d_indices;
 
     typename Coord::Pos g ( getContext()->getGravity() );
     for (unsigned int i=0; i<indices.size(); i++)
@@ -398,7 +398,7 @@ void UniformMass<VecTypes, MassType>::addMDxToVectorVecImpl(defaulttype::BaseVec
     unsigned int derivDim = (unsigned)Deriv::size();
     double m = d_vertexMass.getValue();
 
-    ReadAccessor<Data<vector<int> > > indices = d_indices;
+    ReadAccessor<Data<SetIndexArray > > indices = d_indices;
 
     const SReal* g = getContext()->getGravity().ptr();
 

SofaKernel/modules/SofaBaseMechanics/src/SofaBaseMechanics/UniformMass.h

@@ -28,6 +28,7 @@
 #include <sofa/defaulttype/BaseVector.h>
 #include <sofa/core/objectmodel/DataFileName.h>
 #include <sofa/core/topology/BaseMeshTopology.h>
+#include <SofaBaseTopology/TopologySubsetIndices.h>
 
 namespace sofa::component::mass
 {
@@ -47,6 +48,8 @@ class UniformMass : public core::behavior::Mass<DataTypes>
     typedef typename Coord::value_type Real;
     typedef core::objectmodel::Data<VecCoord> DataVecCoord;
     typedef core::objectmodel::Data<VecDeriv> DataVecDeriv;
+    typedef type::vector<Index> SetIndexArray;
+    typedef sofa::component::topology::TopologySubsetIndices DataSetIndex;
     typedef TMassType MassType;
 
     Data<MassType> d_vertexMass;   ///< single value defining the mass of each particle
@@ -65,9 +68,7 @@ class UniformMass : public core::behavior::Mass<DataTypes>
     /// indices outside of this range are discarded (useful for parallelization
     /// using mesh partitionning)
     Data< type::Vec<2,int> > d_localRange;
-    Data< type::vector<int> >     d_indices; ///< optional local DOF indices. Any computation involving only indices outside of this list are discarded
-
-    Data<bool> d_handleTopologicalChanges; ///< The mass and totalMass are recomputed on particles add/remove.
+    DataSetIndex     d_indices; ///< optional local DOF indices. Any computation involving only indices outside of this list are discarded
     Data<bool> d_preserveTotalMass; ///< Prevent totalMass from decreasing when removing particles.
 
     ////////////////////////// Inherited attributes ////////////////////////////
@@ -123,8 +124,6 @@ class UniformMass : public core::behavior::Mass<DataTypes>
     virtual void initFromTotalMass();
     /// @}
 
-    void handleTopologyChange() override;
-
     void addMDx(const core::MechanicalParams* mparams, DataVecDeriv& f, const DataVecDeriv& dx, SReal factor) override;
     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;
@@ -148,6 +147,7 @@ class UniformMass : public core::behavior::Mass<DataTypes>
 
 
 private:
+    void updateMassOnResize(sofa::Size newSize);
 
     template<class T>
     void drawRigid3DImpl(const core::visual::VisualParams* vparams) ;

SofaKernel/modules/SofaBaseMechanics/src/SofaBaseMechanics/UniformMass.inl

@@ -23,13 +23,13 @@
 #include <SofaBaseMechanics/UniformMass.h>
 #include <sofa/core/fwd.h>
 #include <sofa/core/visual/VisualParams.h>
-#include <sofa/core/topology/Topology.h>
 #include <sofa/core/objectmodel/Context.h>
 #include <sofa/helper/accessor.h>
 #include <sofa/defaulttype/DataTypeInfo.h>
 #include <SofaBaseMechanics/AddMToMatrixFunctor.h>
 #include <sofa/core/behavior/MultiMatrixAccessor.h>
-#include <sofa/core/topology/TopologyChange.h>
+#include <sofa/core/topology/BaseTopology.h>
+#include <SofaBaseTopology/TopologyData.inl>
 #include <sofa/core/MechanicalParams.h>
 
 namespace sofa::component::mass
@@ -72,7 +72,6 @@ UniformMass<DataTypes, MassType>::UniformMass()
                                                                                    "Any computation involving only indices outside of this range \n"
                                                                                    "are discarded (useful for parallelization using mesh partitionning)" ) )
     , d_indices ( initData ( &d_indices, "indices", "optional local DOF indices. Any computation involving only indices outside of this list are discarded" ) )
-    , d_handleTopologicalChanges ( initData ( &d_handleTopologicalChanges, false, "handleTopologicalChanges", "The mass and totalMass are recomputed on particles add/remove." ) )
     , d_preserveTotalMass( initData ( &d_preserveTotalMass, false, "preserveTotalMass", "Prevent totalMass from decreasing when removing particles."))
     , l_topology(initLink("topology", "link to the topology container"))
 {
@@ -133,8 +132,8 @@ void UniformMass<DataTypes, MassType>::initDefaultImpl()
 
     Mass<DataTypes>::init();
 
-    WriteAccessor<Data<vector<int> > > indices = d_indices;
-
+    WriteAccessor<Data<SetIndexArray > > indices = d_indices;
+    
     if(mstate==nullptr)
     {
         msg_warning(this) << "Missing mechanical state. \n"
@@ -167,6 +166,22 @@ void UniformMass<DataTypes, MassType>::initDefaultImpl()
             indices.push_back(i);
     }
 
+    // check if mass should use topology
+    if (l_topology.empty())
+    {
+        l_topology.set(this->getContext()->getMeshTopologyLink());
+    }
+
+    BaseMeshTopology* meshTopology = l_topology.get();
+    if (meshTopology != nullptr && dynamic_cast<sofa::core::topology::TopologyContainer*>(meshTopology) != nullptr)
+    {
+        msg_info() << "Topology path used: '" << l_topology.getLinkedPath() << "'";
+
+        d_indices.createTopologyHandler(meshTopology);
+        d_indices.addTopologyEventCallBack(sofa::core::topology::TopologyChangeType::ENDING_EVENT, [this](const core::topology::TopologyChange* eventTopo) {
+            updateMassOnResize(d_indices.getValue().size());
+        });
+    }
 
     //If user defines the vertexMass, use this as the mass
     if (d_vertexMass.isSet())
@@ -261,7 +276,7 @@ void UniformMass<DataTypes, MassType>::doUpdateInternal()
 template <class DataTypes, class MassType>
 bool UniformMass<DataTypes, MassType>::checkVertexMass()
 {
-    if(d_vertexMass.getValue() <= 0.0 )
+    if(d_vertexMass.getValue() < 0.0 )
     {
         msg_warning(this) << "vertexMass data can not have a negative value. \n"
                              "To remove this warning, you need to set one single, non-zero and positive value to the vertexMass data";
@@ -290,7 +305,7 @@ void UniformMass<DataTypes, MassType>::initFromVertexMass()
 template <class DataTypes, class MassType>
 bool UniformMass<DataTypes, MassType>::checkTotalMass()
 {
-    if(d_totalMass.getValue() <= 0.0)
+    if(d_totalMass.getValue() < 0.0)
     {
         msg_warning(this) << "totalMass data can not have a negative value. \n"
                              "To remove this warning, you need to set a non-zero positive value to the totalMass data";
@@ -338,106 +353,23 @@ void UniformMass<DataTypes, MassType>::initFromTotalMass()
 
 
 template <class DataTypes, class MassType>
-void UniformMass<DataTypes, MassType>::handleTopologyChange()
+void UniformMass<DataTypes, MassType>::updateMassOnResize(sofa::Size newSize)
 {
-    if (l_topology.empty())
+    if (newSize == 0)
     {
-        msg_info() << "link to Topology container should be set to ensure right behavior. First Topology found in current context will be used.";
-        l_topology.set(this->getContext()->getMeshTopologyLink());
+        d_vertexMass.setValue(static_cast<MassType>(0.0));
+        d_totalMass.setValue(Real(0));
+        return;
     }
 
-    BaseMeshTopology *meshTopology = l_topology.get();
-    msg_info() << "Topology path used: '" << l_topology.getLinkedPath() << "'";
-
-    if ( meshTopology != nullptr && mstate->getSize()>0 )
+    if (d_preserveTotalMass.getValue())
     {
-        list< const TopologyChange * >::const_iterator it = meshTopology->beginChange();
-        list< const TopologyChange * >::const_iterator itEnd = meshTopology->endChange();
-
-        while ( it != itEnd )
-        {
-            switch ( ( *it )->getChangeType() )
-            {
-            // POINTS ADDED -----------------
-            case core::topology::POINTSADDED:
-                if ( d_handleTopologicalChanges.getValue())
-                {
-                    WriteAccessor<Data<vector<int> > > indices = d_indices;
-                    size_t sizeIndices = indices.size();
-                    const auto& pointsAdded = ( static_cast< const sofa::core::topology::PointsAdded * >( *it ) )->getIndexArray();
-                    size_t nbPointsAdded = pointsAdded.size();
-
-                    for(size_t i=0; i<nbPointsAdded; i++)
-                    {
-                        indices.push_back(int(pointsAdded[i]));
-                    }
-
-                    size_t newNbPoints = sizeIndices+nbPointsAdded;
-
-                    if (d_preserveTotalMass.getValue())
-                    {
-                        Real newVertexMass = d_totalMass.getValue() / Real(newNbPoints);
-                        d_vertexMass.setValue( static_cast< MassType >( newVertexMass ) );
-                    }
-                    else
-                    {
-                        d_totalMass.setValue (Real(newNbPoints) * Real(d_vertexMass.getValue()) );
-                    }
-                    this->cleanTracker();
-                }
-                break;
-
-            // POINTS REMOVED -----------------
-            case core::topology::POINTSREMOVED:
-                if ( d_handleTopologicalChanges.getValue())
-                {
-                    WriteAccessor<Data<vector<int> > > indices = d_indices;
-                    size_t sizeIndices = indices.size();
-                    const auto& pointsRemoved = ( static_cast< const sofa::core::topology::PointsRemoved * >( *it ) )->getArray();
-                    size_t nbPointsRemoved = pointsRemoved.size();
-
-                    size_t count=0;
-                    for(size_t i=0; i<nbPointsRemoved; i++)
-                    {
-                        for(size_t j=0; j<sizeIndices; j++)
-                        {
-                            if(indices[j] == int(pointsRemoved[i]))
-                            {
-                                count++;
-                            }
-                            else
-                                indices[i] = indices[i+count];
-                        }
-                    }
-
-                    size_t newNbPoints = sizeIndices-nbPointsRemoved;
-                    indices.resize(newNbPoints);
-
-                    if(newNbPoints<=0)
-                    {
-                        msg_warning() << "All points removed";
-                        return;
-                    }
-
-                    if (d_preserveTotalMass.getValue())
-                    {
-                        Real newVertexMass = d_totalMass.getValue() / Real(newNbPoints);
-                        d_vertexMass.setValue( static_cast< MassType >( newVertexMass ) );
-                    }
-                    else
-                    {
-                        d_totalMass.setValue (Real(newNbPoints) * Real(d_vertexMass.getValue()) );
-                    }
-                    this->cleanTracker();
-                }
-                break;
-
-            default:
-                break;
-            }
-
-            ++it;
-        }
+        Real newVertexMass = d_totalMass.getValue() / Real(newSize);
+        d_vertexMass.setValue(static_cast<MassType>(newVertexMass));
+    }
+    else
+    {
+        d_totalMass.setValue(Real(newSize) * Real(d_vertexMass.getValue()));
     }
 }
 
@@ -452,7 +384,7 @@ void UniformMass<DataTypes, MassType>::addMDx ( const core::MechanicalParams*,
     helper::WriteAccessor<DataVecDeriv> res = vres;
     helper::ReadAccessor<DataVecDeriv> dx = vdx;
 
-    WriteAccessor<Data<vector<int> > > indices = d_indices;
+    WriteAccessor<Data<SetIndexArray > > indices = d_indices;
 
     MassType m = d_vertexMass.getValue();
     if ( factor != 1.0 )
@@ -471,7 +403,7 @@ void UniformMass<DataTypes, MassType>::accFromF ( const core::MechanicalParams*,
     WriteOnlyAccessor<DataVecDeriv> a = va;
     ReadAccessor<DataVecDeriv> f = vf;
 
-    ReadAccessor<Data<vector<int> > > indices = d_indices;
+    ReadAccessor<Data<SetIndexArray > > indices = d_indices;
 
     MassType m = d_vertexMass.getValue();
     for ( unsigned int i=0; i<indices.size(); i++ )
@@ -537,7 +469,7 @@ void UniformMass<DataTypes, MassType>::addForce ( const core::MechanicalParams*,
 
 
 
-    ReadAccessor<Data<vector<int> > > indices = d_indices;
+    ReadAccessor<Data<SetIndexArray > > indices = d_indices;
 
     // add weight and inertia force
     if (this->m_separateGravity.getValue()) for ( unsigned int i=0; i<indices.size(); i++ )
@@ -556,7 +488,7 @@ SReal UniformMass<DataTypes, MassType>::getKineticEnergy ( const MechanicalParam
     SOFA_UNUSED(params);
 
     ReadAccessor<DataVecDeriv> v = d_v;
-    ReadAccessor<Data<vector<int> > > indices = d_indices;
+    ReadAccessor<Data<SetIndexArray > > indices = d_indices;
 
     SReal e = 0;
     const MassType& m = d_vertexMass.getValue();
@@ -573,7 +505,7 @@ SReal UniformMass<DataTypes, MassType>::getPotentialEnergy ( const MechanicalPar
 {
     SOFA_UNUSED(params);
     ReadAccessor<DataVecCoord> x = d_x;
-    ReadAccessor<Data<vector<int> > > indices = d_indices;
+    ReadAccessor<Data<SetIndexArray > > indices = d_indices;
 
     SReal e = 0;
     const MassType& m = d_vertexMass.getValue();
@@ -623,7 +555,7 @@ void UniformMass<DataTypes, MassType>::addMToMatrix (const MechanicalParams *mpa
 
     Real mFactor = Real(sofa::core::mechanicalparams::mFactorIncludingRayleighDamping(mparams, this->rayleighMass.getValue()));
 
-    ReadAccessor<Data<vector<int> > > indices = d_indices;
+    ReadAccessor<Data<SetIndexArray > > indices = d_indices;
     for ( unsigned int i=0; i<indices.size(); i++ )
         calc ( r.matrix, m, int(r.offset + N*indices[i]), mFactor);
 }
@@ -660,8 +592,11 @@ void UniformMass<DataTypes, MassType>::draw(const VisualParams* vparams)
     if (!mstate.get())
         return;
 
+    if (!d_showCenterOfGravity.getValue())
+        return;
+
     ReadAccessor<VecCoord> x = mstate->read(ConstVecCoordId::position())->getValue();
-    ReadAccessor<Data<vector<int> > > indices = d_indices;
+    ReadAccessor<Data<SetIndexArray > > indices = d_indices;
 
     Coord gravityCenter;
     std::vector<  sofa::type::Vector3 > points;
@@ -670,14 +605,13 @@ void UniformMass<DataTypes, MassType>::draw(const VisualParams* vparams)
         sofa::type::Vector3 p;
         p = DataTypes::getCPos(x[indices[i]]);
 
-        points.push_back ( p );
+        points.push_back ( p );        
         gravityCenter += x[indices[i]];
     }
-
-
-    if ( d_showCenterOfGravity.getValue() )
+    vparams->drawTool()->drawSpheres(points, 0.01, sofa::type::RGBAColor::yellow());
+
     {
-        gravityCenter /= x.size();
+        gravityCenter /= indices.size();
         const sofa::type::RGBAColor color = sofa::type::RGBAColor::yellow();
 
         Real axisSize = d_showAxisSize.getValue();