diff --git a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/StopperLagrangianConstraint.h b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/StopperLagrangianConstraint.h index b4d4d8cc6c3..fc0a4bb8cab 100644 --- a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/StopperLagrangianConstraint.h +++ b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/StopperLagrangianConstraint.h @@ -26,6 +26,7 @@ #include #include #include +#include namespace sofa::component::constraint::lagrangian::model { @@ -84,17 +85,22 @@ class StopperLagrangianConstraint : public core::behavior::LagrangianConstraint< typedef core::objectmodel::Data DataVecDeriv; typedef core::objectmodel::Data DataMatrixDeriv; + using Inherit::getMState; + using Inherit::isComponentStateInvalid; + using Inherit::d_componentState; + using Inherit::d_constraintIndex; protected: - Data d_index; ///< index of the stop constraint + sofa::core::objectmodel::lifecycle::DeprecatedData d_index {this, "v26.12", "v27.12", "d_index", "You should now use indices to specify the set of dof indices to constraint"}; + Data> d_indices; ///< index of the stop constraint Data d_min; ///< minimum value accepted Data d_max; ///< maximum value accepted - + sofa::type::vector m_cid; StopperLagrangianConstraint(MechanicalState* object = nullptr); - virtual ~StopperLagrangianConstraint() {} + void init() override; virtual type::vector getConstraintIdentifiers() override final { @@ -103,21 +109,16 @@ class StopperLagrangianConstraint : public core::behavior::LagrangianConstraint< ids.push_back("Unilateral"); return ids; } - virtual type::vector getStopperIdentifiers(){ return {}; } - - public: void buildConstraintMatrix(const core::ConstraintParams* cParams, DataMatrixDeriv &c_d, unsigned int &cIndex, const DataVecCoord &x) override; void getConstraintViolation(const core::ConstraintParams* cParams, linearalgebra::BaseVector *resV, const DataVecCoord &x, const DataVecDeriv &v) override; - void getConstraintResolution(const core::ConstraintParams *, std::vector& resTab, unsigned int& offset) override; }; #if !defined(SOFA_COMPONENT_CONSTRAINTSET_STOPPERLAGRANGIANCONSTRAINT_CPP) extern template class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API StopperLagrangianConstraint; - #endif } //namespace sofa::component::constraint::lagrangian::model diff --git a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/StopperLagrangianConstraint.inl b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/StopperLagrangianConstraint.inl index abfec26f2e2..f67f2477d57 100644 --- a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/StopperLagrangianConstraint.inl +++ b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/StopperLagrangianConstraint.inl @@ -31,33 +31,82 @@ namespace sofa::component::constraint::lagrangian::model template StopperLagrangianConstraint::StopperLagrangianConstraint(MechanicalState* object) : Inherit(object) - , d_index(initData(&d_index, 0, "index", "index of the stop constraint")) + , d_indices(initData(&d_indices, "indices", "indices of the stop constraint")) , d_min(initData(&d_min, -100.0_sreal, "min", "minimum value accepted")) , d_max(initData(&d_max, 100.0_sreal, "max", "maximum value accepted")) { } +template +void StopperLagrangianConstraint::init() +{ + Inherit::init(); + if(!getMState()) + { + msg_error() << "There is no mstate to constraint"; + d_componentState = sofa::core::objectmodel::ComponentState::Invalid; + return; + } + + auto x = getMState()->readPositions(); + if(!d_indices.isSet()) + { + auto indices = sofa::helper::getWriteOnlyAccessor(d_indices); + indices.resize(x.size()); + for(sofa::Index i=0;i void StopperLagrangianConstraint::buildConstraintMatrix(const core::ConstraintParams* /*cParams*/, DataMatrixDeriv &c_d, unsigned int &cIndex, const DataVecCoord &/*x*/) { - auto c = sofa::helper::getWriteAccessor(c_d); + if(isComponentStateInvalid()) + return; + + auto c = sofa::helper::getWriteOnlyAccessor(c_d); + auto indices = sofa::helper::getReadAccessor(d_indices); - MatrixDerivRowIterator c_it = c->writeLine(cIndex++); - c_it.setCol(d_index.getValue(), Coord(1)); + m_cid.resize(indices.size()); + for(unsigned i=0; iwriteLine(m_cid[i]); + c_it.setCol(indices[i], Coord(1)); + cIndex++; + } } template -void StopperLagrangianConstraint::getConstraintViolation(const core::ConstraintParams* /*cParams*/, linearalgebra::BaseVector *resV, const DataVecCoord &x, const DataVecDeriv &/*v*/) +void StopperLagrangianConstraint::getConstraintViolation(const core::ConstraintParams* /*cParams*/, linearalgebra::BaseVector *resV, const DataVecCoord& x_, const DataVecDeriv &/*v*/) { - const auto constraintIndex = this->d_constraintIndex.getValue(); - resV->set(constraintIndex, x.getValue()[d_index.getValue()][0]); + if(isComponentStateInvalid()) + return; + + auto x = x_.getValue(); + auto indices = sofa::helper::getReadAccessor(d_indices); + + for(unsigned int i =0 ; iset(m_cid[i], x[indices[i]][0]); + } } template void StopperLagrangianConstraint::getConstraintResolution(const core::ConstraintParams *, std::vector& resTab, unsigned int& offset) { - for(int i=0; i<1; i++) - resTab[offset++] = new StopperLagrangianConstraintResolution1Dof(d_min.getValue(), d_max.getValue()); + if(isComponentStateInvalid()) + return; + + auto min = d_min.getValue(); + auto max = d_max.getValue(); + auto indices = sofa::helper::getReadAccessor(d_indices); + for(auto _ : indices) + resTab[offset++] = new StopperLagrangianConstraintResolution1Dof(min, max); } } //namespace sofa::component::constraint::lagrangian::model