Merge pull request #12 from SofaDefrost/cleaning_componentState

[component] added component state management to prevent segfault

component/constraint/CableConstraint.h

@@ -148,6 +148,7 @@ class CableConstraint : public CableModel<DataTypes>
     using CableModel<DataTypes>::d_maxDispVariation ;
     using CableModel<DataTypes>::d_force ;
     using CableModel<DataTypes>::d_displacement ;
+    using CableModel<DataTypes>::m_componentstate ;
     ///////////////////////////////////////////////////////////////////////////
 
 protected:

component/constraint/CableConstraint.inl

@@ -44,6 +44,7 @@ namespace component
 namespace constraintset
 {
 
+using sofa::core::objectmodel::ComponentState;
 using sofa::helper::WriteAccessor;
 
 template<class DataTypes>
@@ -110,23 +111,28 @@ void CableConstraint<DataTypes>::reinit()
 template<class DataTypes>
 void CableConstraint<DataTypes>::internalInit()
 {
-    // check for errors in the initialization
     if(d_value.getValue().size()==0)
     {
-        WriteAccessor<Data<vector<Real>>> inputValue = d_value;
-        inputValue.resize(1,0.);
+        WriteAccessor<Data<vector<Real>>> value = d_value;
+        value.resize(1,0.);
     }
 
+    // check for errors in the initialization
     if(d_value.getValue().size()<d_valueIndex.getValue())
-        serr<<"bad size of inputValue ="<< d_value.getValue().size()<<"  or wrong value for inputIndex = "<<d_valueIndex.getValue()<<sendl;
-
+    {
+        msg_warning() << "Bad size for data value (size="<< d_value.getValue().size()<<"), or wrong value for data valueIndex (valueIndex="<<d_valueIndex.getValue()<<"). Set default valueIndex=0.";
+        d_valueIndex.setValue(0);
+    }
 }
 
 template<class DataTypes>
 void CableConstraint<DataTypes>::getConstraintResolution(const ConstraintParams* cParam,
                                                          std::vector<ConstraintResolution*>& resTab,
                                                          unsigned int& offset)
 {
+    if(m_componentstate != ComponentState::Valid)
+            return ;
+
     SOFA_UNUSED(cParam);
 
     double imposed_value=d_value.getValue()[d_valueIndex.getValue()];

component/constraint/PartialRigidificationConstraint.inl

@@ -88,58 +88,40 @@ void PartialRigidificationConstraint<DataTypes>::buildConstraintMatrix(const Con
 {
     SOFA_UNUSED(cParams);
 
-    std::cout<<"step0"<<std::endl;
     const VecCoord X = x.getValue();
     MatrixDeriv& matrix = *cMatrix.beginEdit();
     m_cid = cIndex;
-    std::cout<<"step1"<<std::endl;
     const Vec3 cx(1, 0, 0), cy(0, 1, 0), cz(0, 0, 1), vZero(0, 0, 0);
-  //  const Vec3 p0p1 = X[1].getCenter() - X[0].getCenter();
-
-//	const Deriv::Rot tau0p1cx = p0p1.cross(-cx);
-//	const Deriv::Rot tau0p1cy = p0p1.cross(-cy);
-//	const Deriv::Rot tau0p1cz = p0p1.cross(-cz);
-
-    std::cout<<"step2"<<std::endl;
 
     MatrixDerivRowIterator cit = matrix.writeLine(cIndex);
     cit.addCol(0, Deriv(cx, vZero));
-//	cit.addCol(0, Deriv(cx, tau0p1cx));
 
     cit = matrix.writeLine(cIndex+1);
     cit.setCol(0, Deriv(cy, vZero));
-//	cit.addCol(0, Deriv(cy, tau0p1cy));
 
     cit = matrix.writeLine(cIndex+2);
     cit.setCol(0, Deriv(cz, vZero));
-//	cit.addCol(0, Deriv(cz, tau0p1cz));
 
     cit = matrix.writeLine(cIndex+3);
     cit.setCol(0, Deriv(vZero, cx));
-//    cit.addCol(0, Deriv(cx.cross(p0p1), vZero));
 
     cit = matrix.writeLine(cIndex+4);
     cit.setCol(0, Deriv(vZero, cy));
-//    cit.addCol(0, Deriv(cy.cross(p0p1), vZero));
 
     cit = matrix.writeLine(cIndex+5);
     cit.setCol(0, Deriv(vZero, cz));
-//    cit.addCol(0, Deriv(cz.cross(p0p1), vZero));
-
-      std::cout<<"DEBUG/// state"<<mstate->getName()<<std::endl;
 
-      // Indicates the size of the constraint block
-      cIndex +=6;
+    // Indicates the size of the constraint block
+    cIndex +=6;
 }
 
 template<class DataTypes>
 void PartialRigidificationConstraint<DataTypes>::getConstraintResolution(std::vector<ConstraintResolution*>& resTab,
                                                                          unsigned int& offset)
 {
-//	for(size_t i = 0 ; i < 6 ; ++i)
     resTab[offset] = new PartialRigidificationConstraintResolution6Dof();
 
-    // indicates the size of the block on which the constraint resoluation works
+    // Indicates the size of the block on which the constraint resoluation works
     offset += 6;
 }
 

component/constraint/SurfacePressureConstraint.h

@@ -149,6 +149,7 @@ class SOFA_SOFTROBOTS_API SurfacePressureConstraint : public SurfacePressureMode
     using SurfacePressureModel<DataTypes>::m_displayedValue ;
     using SurfacePressureModel<DataTypes>::m_columnId;
     using SurfacePressureModel<DataTypes>::m_visualization;
+    using SoftRobotsConstraint<DataTypes>::m_componentstate;
     ////////////////////////////////////////////////////////////////////////////
 
 private:

component/constraint/SurfacePressureConstraint.inl

@@ -47,6 +47,7 @@ namespace constraintset
 namespace _surfacepressureconstraint_
 {
 
+using sofa::core::objectmodel::ComponentState ;
 using sofa::helper::WriteAccessor ;
 using sofa::defaulttype::Vec3d;
 using sofa::helper::vector ;
@@ -144,15 +145,18 @@ void SurfacePressureConstraint<DataTypes>::reset()
 template<class DataTypes>
 void SurfacePressureConstraint<DataTypes>::initData()
 {
-    // check for errors in the initialization
     if(d_value.getValue().size()==0)
     {
-        WriteAccessor<Data<vector<Real>>> inputValue = d_value;
-        inputValue.resize(1,0.);
+        WriteAccessor<Data<vector<Real>>> value = d_value;
+        value.resize(1,0.);
     }
 
+    // check for errors in the initialization
     if(d_value.getValue().size()<d_valueIndex.getValue())
-        msg_error(this) <<"bad size of inputValue ="<< d_value.getValue().size()<<"  or wrong value for inputIndex = "<<d_valueIndex.getValue();
+    {
+        msg_warning() <<"Bad size for data value (size="<< d_value.getValue().size()<<"), or wrong value for data valueIndex (valueIndex="<<d_valueIndex.getValue()<<"). Set default valueIndex=0.";
+        d_valueIndex.setValue(0);
+    }
     else
         m_displayedValue = d_value.getValue()[d_valueIndex.getValue()];
 }
@@ -162,6 +166,9 @@ template<class DataTypes>
 void SurfacePressureConstraint<DataTypes>::getConstraintResolution(std::vector<ConstraintResolution*>& resTab,
                                                                    unsigned int& offset)
 {
+    if(m_componentstate != ComponentState::Valid)
+            return ;
+
     double imposed_value = d_value.getValue()[d_valueIndex.getValue()];
     m_displayedValue = d_value.getValue()[d_valueIndex.getValue()];
 

component/constraint/UnilateralPlaneConstraint.h

@@ -145,6 +145,7 @@ class SOFA_SOFTROBOTS_API UnilateralPlaneConstraint : public Constraint<DataType
     /// otherwise any access to the base::attribute would require
     /// the "this->" approach.
     using Constraint<DataTypes>::mstate ;
+    using Constraint<DataTypes>::m_componentstate ;
     ////////////////////////////////////////////////////////////////////////////
 };
 

component/constraint/UnilateralPlaneConstraint.inl

@@ -46,6 +46,7 @@ namespace component
 namespace constraintset
 {
 
+using sofa::core::objectmodel::ComponentState;
 using sofa::core::VecCoordId;
 using sofa::core::ConstVecCoordId ;
 using sofa::helper::WriteAccessor ;
@@ -91,20 +92,28 @@ UnilateralPlaneConstraint<DataTypes>::~UnilateralPlaneConstraint()
 template<class DataTypes>
 void UnilateralPlaneConstraint<DataTypes>::init()
 {
+    m_componentstate = ComponentState::Invalid;
     Inherit::init();
 
     if(mstate == nullptr)
-        msg_error(this) << "There is no mechanical state associated with this node. "
+    {
+        msg_error(this) << "There is no mechanical state associated with this node. Component deactivated."
                            "To remove this error message, fix your scene possibly by "
                            "adding a MechanicalObject." ;
+        return;
+    }
 
     checkIndicesRegardingState();
+    m_componentstate = ComponentState::Valid;
 }
 
 
 template<class DataTypes>
 void UnilateralPlaneConstraint<DataTypes>::reinit()
 {
+    if(m_componentstate != ComponentState::Valid)
+            return ;
+
     checkIndicesRegardingState();
 }
 
@@ -130,6 +139,9 @@ void UnilateralPlaneConstraint<DataTypes>::buildConstraintMatrix(const core::Con
                                                                   unsigned int &cIndex,
                                                                  const DataVecCoord &x)
 {
+    if(m_componentstate != ComponentState::Valid)
+            return ;
+
     SOFA_UNUSED(cParams);
     SOFA_UNUSED(x);
 
@@ -164,6 +176,9 @@ void UnilateralPlaneConstraint<DataTypes>::getConstraintViolation(const core::Co
                                                                    const DataVecCoord &xfree,
                                                                    const DataVecDeriv &vfree)
 {
+    if(m_componentstate != ComponentState::Valid)
+            return ;
+
     SOFA_UNUSED(cParams);
     SOFA_UNUSED(vfree);
 
@@ -187,6 +202,9 @@ void UnilateralPlaneConstraint<DataTypes>::getConstraintResolution(const Constra
                                                                    std::vector<core::behavior::ConstraintResolution*>& resTab,
                                                                    unsigned int& offset)
 {
+    if(m_componentstate != ComponentState::Valid)
+            return ;
+
     SOFA_UNUSED(cParams);
 
     resTab[offset] = new UnilateralPlaneConstraintResolution(1);
@@ -197,6 +215,8 @@ void UnilateralPlaneConstraint<DataTypes>::getConstraintResolution(const Constra
 template<class DataTypes>
 void UnilateralPlaneConstraint<DataTypes>::draw(const VisualParams* vparams)
 {
+    if(m_componentstate != ComponentState::Valid)
+            return ;
 
     if (!vparams->displayFlags().getShowCollisionModels())
         return;

component/constraint/model/CableModel.h

@@ -135,16 +135,6 @@ class SOFA_SOFTROBOTS_API CableModel : virtual public SoftRobotsConstraint<DataT
 
     SReal getCableLength(const VecCoord &positions);
 
-private:
-    void internalInit();
-
-    void checkIndicesRegardingState();
-    void initActuatedPoints();
-
-    void drawPullPoint(const VisualParams* vparams);
-    void drawPoints(const VisualParams* vparams);
-    void drawLinesBetweenPoints(const VisualParams* vparams);
-
     ////////////////////////// Inherited attributes ////////////////////////////
     /// https://gcc.gnu.org/onlinedocs/gcc/Name-lookup.html
     /// Bring m_state in the current lookup context.
@@ -157,7 +147,18 @@ class SOFA_SOFTROBOTS_API CableModel : virtual public SoftRobotsConstraint<DataT
     using SoftRobotsConstraint<DataTypes>::m_deltaMax ;
     using SoftRobotsConstraint<DataTypes>::m_lambdaMax ;
     using SoftRobotsConstraint<DataTypes>::addAlias ;
+    using SoftRobotsConstraint<DataTypes>::m_componentstate ;
     ////////////////////////////////////////////////////////////////////////////
+
+private:
+    void internalInit();
+
+    void checkIndicesRegardingState();
+    void initActuatedPoints();
+
+    void drawPullPoint(const VisualParams* vparams);
+    void drawPoints(const VisualParams* vparams);
+    void drawLinesBetweenPoints(const VisualParams* vparams);
 };
 
 // Declares template as extern to avoid the code generation of the template for

component/constraint/model/CableModel.inl

@@ -48,6 +48,7 @@ namespace component
 namespace constraintset
 {
 
+using sofa::core::objectmodel::ComponentState;
 using sofa::core::visual::VisualParams;
 using sofa::defaulttype::BaseVector;
 using sofa::helper::ReadAccessor;
@@ -200,6 +201,7 @@ CableModel<DataTypes>::~CableModel()
 template<class DataTypes>
 void CableModel<DataTypes>::init()
 {
+    m_componentstate = ComponentState::Invalid;
     SoftRobotsConstraint<DataTypes>::init();
 
     if(d_indexDeprecated.isSet())
@@ -208,22 +210,32 @@ void CableModel<DataTypes>::init()
                              "'index' should be replaced by the field 'indices'." ;
 
     if(m_state==nullptr)
+    {
         msg_error(this) << "There is no mechanical state associated with this node. "
                            "The object is then deactivated. "
                            "To remove this error message, fix your scene possibly by "
                            "adding a MechanicalObject." ;
+        return;
+    }
 
     if(m_state->read(VecCoordId::position())==nullptr)
+    {
         msg_error(this) << "There is no position vector in the MechanicalState. "
                              "The object is deactivated. " ;
+        return;
+    }
 
     internalInit();
+    m_componentstate = ComponentState::Valid;
 }
 
 
 template<class DataTypes>
 void CableModel<DataTypes>::bwdInit()
 {
+    if(m_componentstate != ComponentState::Valid)
+            return ;
+
     // The initial length of the cable is computed in bwdInit so the mapping (if there is any)
     // will be considered
     VecCoord positions = (*m_state->read(VecCoordId::position())).getValue();
@@ -239,13 +251,19 @@ void CableModel<DataTypes>::bwdInit()
 template<class DataTypes>
 void CableModel<DataTypes>::reinit()
 {
+    if(m_componentstate != ComponentState::Valid)
+            return ;
+
     internalInit();
 }
 
 
 template<class DataTypes>
 void CableModel<DataTypes>::reset()
 {
+    if(m_componentstate != ComponentState::Valid)
+        return ;
+
     VecCoord positions = (*m_state->read(VecCoordId::position())).getValue();
     Real cableLength = getCableLength(positions);
     d_cableLength.setValue(cableLength);
@@ -333,6 +351,9 @@ SReal CableModel<DataTypes>::getCableLength(const VecCoord &positions)
 template<class DataTypes>
 void CableModel<DataTypes>::buildConstraintMatrix(const ConstraintParams* cParams, DataMatrixDeriv &cMatrix, unsigned int &cIndex, const DataVecCoord &x)
 {
+    if(m_componentstate != ComponentState::Valid)
+            return ;
+
     SOFA_UNUSED(cParams);
 
     m_columnIndex = cIndex;
@@ -437,6 +458,9 @@ void CableModel<DataTypes>::buildConstraintMatrix(const ConstraintParams* cParam
 template<class DataTypes>
 void CableModel<DataTypes>::getConstraintViolation(const ConstraintParams* cParams, BaseVector *resV, const DataVecCoord &xfree, const DataVecDeriv &vfree)
 {
+    if(m_componentstate != ComponentState::Valid)
+            return ;
+
     SOFA_UNUSED(cParams);
     SOFA_UNUSED(vfree);
     const VecCoord& positions =  xfree.getValue();
@@ -467,6 +491,9 @@ void CableModel<DataTypes>::getConstraintViolation(const ConstraintParams* cPara
 template<class DataTypes>
 void CableModel<DataTypes>::draw(const VisualParams* vparams)
 {
+    if(m_componentstate != ComponentState::Valid)
+            return ;
+
     if (!vparams->displayFlags().getShowInteractionForceFields()) return;
 
     drawLinesBetweenPoints(vparams);