diff --git a/.github/workflows/nix.yml b/.github/workflows/nix.yml new file mode 100644 index 00000000000..a1bff8fcec3 --- /dev/null +++ b/.github/workflows/nix.yml @@ -0,0 +1,21 @@ +name: "CI - Nix" + +on: + push: + +jobs: + nix: + runs-on: "${{ matrix.os }}-latest" + if: ${{ github.repository_owner == 'sofa-framework' }} + strategy: + matrix: + os: [ubuntu, macos] + steps: + - uses: actions/checkout@v4 + - uses: cachix/install-nix-action@v27 + # TODO: the "sofa" account on cachix does not exist yet + #- uses: cachix/cachix-action@v15 + #with: + #name: sofa + #authToken: '${{ secrets.CACHIX_AUTH_TOKEN }}' + - run: nix build -L diff --git a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/BilateralLagrangianConstraint.cpp b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/BilateralLagrangianConstraint.cpp index e6932abfdae..5f694d58dd0 100644 --- a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/BilateralLagrangianConstraint.cpp +++ b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/BilateralLagrangianConstraint.cpp @@ -37,33 +37,37 @@ class BilateralLagrangianConstraintSpecialization { public: + template + using SubsetIndices = typename BilateralLagrangianConstraint::SubsetIndices; + + template static void bwdInit(BilateralLagrangianConstraint& self) { if (!self.d_keepOrientDiff.getValue()) return; - helper::WriteAccessor::VecDeriv > > wrest = self.d_restVector; + auto wrest = sofa::helper::getWriteAccessor(self.d_restVector); - if (wrest.size() > 0) { - msg_warning("BilateralLagrangianConstraintSpecialization") << "keepOrientationDifference is activated, rest_vector will be ignored! " ; + if (!wrest.empty()) { + msg_warning(&self) << "keepOrientationDifference is activated, rest_vector will be ignored! " ; wrest.resize(0); } - const typename BilateralLagrangianConstraint::SubsetIndices& m1Indices = self.d_m1.getValue(); - const typename BilateralLagrangianConstraint::SubsetIndices& m2Indices = self.d_m2.getValue(); + const SubsetIndices& m1Indices = self.d_m1.getValue(); + const SubsetIndices& m2Indices = self.d_m2.getValue(); const unsigned minp = std::min(m1Indices.size(),m2Indices.size()); - const typename BilateralLagrangianConstraint::DataVecCoord &d_x1 = *self.mstate1->read(core::ConstVecCoordId::position()); - const typename BilateralLagrangianConstraint::DataVecCoord &d_x2 = *self.mstate2->read(core::ConstVecCoordId::position()); + const DataVecCoord_t &d_x1 = *self.mstate1->read(core::ConstVecCoordId::position()); + const DataVecCoord_t &d_x2 = *self.mstate2->read(core::ConstVecCoordId::position()); - const typename BilateralLagrangianConstraint::VecCoord &x1 = d_x1.getValue(); - const typename BilateralLagrangianConstraint::VecCoord &x2 = d_x2.getValue(); + const VecCoord_t &x1 = d_x1.getValue(); + const VecCoord_t &x2 = d_x2.getValue(); for (unsigned pid=0; pid::Coord P = x1[m1Indices[pid]]; - const typename BilateralLagrangianConstraint::Coord Q = x2[m2Indices[pid]]; + const Coord_t P = x1[m1Indices[pid]]; + const Coord_t Q = x2[m2Indices[pid]]; type::Quat qP, qQ, dQP; qP = P.getOrientation(); @@ -73,7 +77,7 @@ class BilateralLagrangianConstraintSpecialization dQP = qP.quatDiff(qQ, qP); dQP.normalize(); - typename BilateralLagrangianConstraint::Coord df; + Coord_t df; df.getCenter() = Q.getCenter() - P.getCenter(); df.getOrientation() = dQP; self.initialDifference.push_back(df); @@ -86,12 +90,12 @@ class BilateralLagrangianConstraintSpecialization static void getConstraintResolution(BilateralLagrangianConstraint& self, const ConstraintParams* cParams, std::vector& resTab, - unsigned int& offset, double tolerance) + unsigned int& offset, SReal tolerance) { SOFA_UNUSED(cParams); - const unsigned minp=std::min(self.d_m1.getValue().size(), - self.d_m2.getValue().size()); - for (unsigned pid=0; pid template static void buildConstraintMatrix(BilateralLagrangianConstraint& self, const ConstraintParams* cParams, - typename BilateralLagrangianConstraint::DataMatrixDeriv &c1_d, - typename BilateralLagrangianConstraint::DataMatrixDeriv &c2_d, + DataMatrixDeriv_t &c1_d, + DataMatrixDeriv_t &c2_d, unsigned int &constraintId, - const typename BilateralLagrangianConstraint::DataVecCoord &/*x1*/, - const typename BilateralLagrangianConstraint::DataVecCoord &/*x2*/) + const DataVecCoord_t &/*x1*/, + const DataVecCoord_t &/*x2*/) { SOFA_UNUSED(cParams) ; - const typename BilateralLagrangianConstraint::SubsetIndices& m1Indices = self.d_m1.getValue(); - const typename BilateralLagrangianConstraint::SubsetIndices& m2Indices = self.d_m2.getValue(); + const SubsetIndices& m1Indices = self.d_m1.getValue(); + const SubsetIndices& m2Indices = self.d_m2.getValue(); unsigned minp = std::min(m1Indices.size(),m2Indices.size()); self.cid.resize(minp); - typename BilateralLagrangianConstraint::MatrixDeriv &c1 = *c1_d.beginEdit(); - typename BilateralLagrangianConstraint::MatrixDeriv &c2 = *c2_d.beginEdit(); + auto c1 = sofa::helper::getWriteAccessor(c1_d); + auto c2 = sofa::helper::getWriteAccessor(c2_d); - const Vec<3, typename BilateralLagrangianConstraint::Real> cx(1,0,0), cy(0,1,0), cz(0,0,1); - const Vec<3, typename BilateralLagrangianConstraint::Real> vZero(0,0,0); + static constexpr Vec<3, Real_t> cx(1,0,0), cy(0,1,0), cz(0,0,1); + static constexpr Vec<3, Real_t> vZero(0,0,0); - for (unsigned pid=0; pid constraintId += 6; //Apply constraint for position - typename BilateralLagrangianConstraint::MatrixDerivRowIterator c1_it = c1.writeLine(self.cid[pid]); - c1_it.addCol(tm1, typename BilateralLagrangianConstraint::Deriv(-cx, vZero)); + auto c1_it = c1->writeLine(self.cid[pid]); + c1_it.addCol(tm1, Deriv_t(-cx, vZero)); - typename BilateralLagrangianConstraint::MatrixDerivRowIterator c2_it = c2.writeLine(self.cid[pid]); - c2_it.addCol(tm2, typename BilateralLagrangianConstraint::Deriv(cx, vZero)); + auto c2_it = c2->writeLine(self.cid[pid]); + c2_it.addCol(tm2, Deriv_t(cx, vZero)); - c1_it = c1.writeLine(self.cid[pid] + 1); - c1_it.setCol(tm1, typename BilateralLagrangianConstraint::Deriv(-cy, vZero)); + c1_it = c1->writeLine(self.cid[pid] + 1); + c1_it.setCol(tm1, Deriv_t(-cy, vZero)); - c2_it = c2.writeLine(self.cid[pid] + 1); - c2_it.setCol(tm2, typename BilateralLagrangianConstraint::Deriv(cy, vZero)); + c2_it = c2->writeLine(self.cid[pid] + 1); + c2_it.setCol(tm2, Deriv_t(cy, vZero)); - c1_it = c1.writeLine(self.cid[pid] + 2); - c1_it.setCol(tm1, typename BilateralLagrangianConstraint::Deriv(-cz, vZero)); + c1_it = c1->writeLine(self.cid[pid] + 2); + c1_it.setCol(tm1, Deriv_t(-cz, vZero)); - c2_it = c2.writeLine(self.cid[pid] + 2); - c2_it.setCol(tm2, typename BilateralLagrangianConstraint::Deriv(cz, vZero)); + c2_it = c2->writeLine(self.cid[pid] + 2); + c2_it.setCol(tm2, Deriv_t(cz, vZero)); //Apply constraint for orientation - c1_it = c1.writeLine(self.cid[pid] + 3); - c1_it.setCol(tm1, typename BilateralLagrangianConstraint::Deriv(vZero, -cx)); + c1_it = c1->writeLine(self.cid[pid] + 3); + c1_it.setCol(tm1, Deriv_t(vZero, -cx)); - c2_it = c2.writeLine(self.cid[pid] + 3); - c2_it.setCol(tm2, typename BilateralLagrangianConstraint::Deriv(vZero, cx)); + c2_it = c2->writeLine(self.cid[pid] + 3); + c2_it.setCol(tm2, Deriv_t(vZero, cx)); - c1_it = c1.writeLine(self.cid[pid] + 4); - c1_it.setCol(tm1, typename BilateralLagrangianConstraint::Deriv(vZero, -cy)); + c1_it = c1->writeLine(self.cid[pid] + 4); + c1_it.setCol(tm1, Deriv_t(vZero, -cy)); - c2_it = c2.writeLine(self.cid[pid] + 4); - c2_it.setCol(tm2, typename BilateralLagrangianConstraint::Deriv(vZero, cy)); + c2_it = c2->writeLine(self.cid[pid] + 4); + c2_it.setCol(tm2, Deriv_t(vZero, cy)); - c1_it = c1.writeLine(self.cid[pid] + 5); - c1_it.setCol(tm1, typename BilateralLagrangianConstraint::Deriv(vZero, -cz)); + c1_it = c1->writeLine(self.cid[pid] + 5); + c1_it.setCol(tm1, Deriv_t(vZero, -cz)); - c2_it = c2.writeLine(self.cid[pid] + 5); - c2_it.setCol(tm2, typename BilateralLagrangianConstraint::Deriv(vZero, cz)); + c2_it = c2->writeLine(self.cid[pid] + 5); + c2_it.setCol(tm2, Deriv_t(vZero, cz)); } - - c1_d.endEdit(); - c2_d.endEdit(); } @@ -190,41 +191,45 @@ class BilateralLagrangianConstraintSpecialization const typename BilateralLagrangianConstraint::SubsetIndices& m2Indices = self.d_m2.getValue(); unsigned min = std::min(m1Indices.size(), m2Indices.size()); - const typename BilateralLagrangianConstraint::VecDeriv& restVector = self.d_restVector.getValue(); - self.dfree.resize(min); + const VecDeriv_t& restVector = self.d_restVector.getValue(); + self.m_violation.resize(min); - const typename BilateralLagrangianConstraint::VecCoord &x1 = d_x1.getValue(); - const typename BilateralLagrangianConstraint::VecCoord &x2 = d_x2.getValue(); + const VecCoord_t &x1 = d_x1.getValue(); + const VecCoord_t &x2 = d_x2.getValue(); - for (unsigned pid=0; pid::Coord dof1 = x1[m1Indices[pid]]; - //typename BilateralLagrangianConstraint::Coord dof2 = x2[m2Indices[pid]]; - typename BilateralLagrangianConstraint::Coord dof1; + Coord_t dof1; - if (self.d_keepOrientDiff.getValue()) { - const typename BilateralLagrangianConstraint::Coord dof1c = x1[m1Indices[pid]]; + if (self.d_keepOrientDiff.getValue()) + { + const Coord_t dof1c = x1[m1Indices[pid]]; - typename BilateralLagrangianConstraint::Coord corr=self.initialDifference[pid]; - type::Quat df = corr.getOrientation(); - type::Quat o1 = dof1c.getOrientation(); - type::Quat ro1 = o1 * df; + Coord_t corr = self.initialDifference[pid]; + type::Quat> df = corr.getOrientation(); + type::Quat> o1 = dof1c.getOrientation(); + type::Quat> ro1 = o1 * df; - dof1.getCenter() = dof1c.getCenter() + corr.getCenter(); - dof1.getOrientation() = ro1; - } else - dof1 = x1[m1Indices[pid]]; + dof1.getCenter() = dof1c.getCenter() + corr.getCenter(); + dof1.getOrientation() = ro1; + } + else + { + dof1 = x1[m1Indices[pid]]; + } - const typename BilateralLagrangianConstraint::Coord dof2 = x2[m2Indices[pid]]; + const Coord_t dof2 = x2[m2Indices[pid]]; - getVCenter(self.dfree[pid]) = dof2.getCenter() - dof1.getCenter(); - getVOrientation(self.dfree[pid]) = dof1.rotate(self.q.angularDisplacement(dof2.getOrientation() , + getVCenter(self.m_violation[pid]) = dof2.getCenter() - dof1.getCenter(); + getVOrientation(self.m_violation[pid]) = dof1.rotate(self.q.angularDisplacement(dof2.getOrientation() , dof1.getOrientation())); // angularDisplacement compute the rotation vector btw the two quaternions if (pid < restVector.size()) - self.dfree[pid] -= restVector[pid]; + self.m_violation[pid] -= restVector[pid]; - for (unsigned int i=0 ; iset(self.cid[pid]+i, self.dfree[pid][i]); + for (unsigned int i = 0; i < self.m_violation[pid].size(); i++) + { + v->set(self.cid[pid]+i, self.m_violation[pid][i]); + } } } @@ -235,9 +240,9 @@ class BilateralLagrangianConstraintSpecialization typename MyClass::Real /*contactDistance*/, int m1, int m2, typename MyClass::Coord /*Pfree*/, typename MyClass::Coord /*Qfree*/, long /*id*/, typename MyClass::PersistentID /*localid*/) { - helper::WriteAccessor::SubsetIndices > > wm1 = self.d_m1; - helper::WriteAccessor::SubsetIndices > > wm2 = self.d_m2; - helper::WriteAccessor > wrest = self.d_restVector; + sofa::helper::WriteAccessor>> wm1 = self.d_m1; + sofa::helper::WriteAccessor>> wm2 = self.d_m2; + auto wrest = sofa::helper::WriteAccessor(self.d_restVector); wm1.push_back(m1); wm2.push_back(m2); @@ -249,25 +254,30 @@ class BilateralLagrangianConstraintSpecialization }; +using RigidBilateralLagrangianConstraint = BilateralLagrangianConstraintSpecialization; + template<> SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API -void BilateralLagrangianConstraint::init(){ +void BilateralLagrangianConstraint::init() +{ unspecializedInit() ; } template<> SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API -void BilateralLagrangianConstraint::bwdInit() { - BilateralLagrangianConstraintSpecialization::bwdInit(*this); +void BilateralLagrangianConstraint::bwdInit() +{ + RigidBilateralLagrangianConstraint::bwdInit(*this); } template<> SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API -void BilateralLagrangianConstraint::getConstraintResolution(const ConstraintParams* cParams, - std::vector& resTab, - unsigned int& offset) +void BilateralLagrangianConstraint::getConstraintResolution( + const ConstraintParams* cParams, + std::vector& resTab, + unsigned int& offset) { - BilateralLagrangianConstraintSpecialization::getConstraintResolution(*this, - cParams, resTab, offset, - d_numericalTolerance.getValue()) ; + RigidBilateralLagrangianConstraint::getConstraintResolution(*this, + cParams, resTab, offset, + d_numericalTolerance.getValue()); } template <> SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API @@ -277,9 +287,9 @@ void BilateralLagrangianConstraint::buildConstraintMatrix(const Con unsigned int &constraintId, const DataVecCoord &x1, const DataVecCoord &x2) { - BilateralLagrangianConstraintSpecialization::buildConstraintMatrix(*this, - cParams, c1_d, c2_d, constraintId, - x1, x2) ; + RigidBilateralLagrangianConstraint::buildConstraintMatrix(*this, + cParams, c1_d, c2_d, constraintId, + x1, x2); } @@ -289,9 +299,9 @@ void BilateralLagrangianConstraint::getConstraintViolation(const Co const DataVecCoord &d_x1, const DataVecCoord &d_x2, const DataVecDeriv &v1, const DataVecDeriv &v2) { - BilateralLagrangianConstraintSpecialization::getConstraintViolation(*this, - cParams, v, d_x1, d_x2, - v1, v2) ; + RigidBilateralLagrangianConstraint::getConstraintViolation(*this, + cParams, v, d_x1, d_x2, + v1, v2); } @@ -312,9 +322,10 @@ void BilateralLagrangianConstraint::addContact(Deriv n Coord Pfree, Coord Qfree, long id, PersistentID localid) { - BilateralLagrangianConstraintSpecialization::addContact(*this, - norm, P, Q, contactDistance, m1, m2, Pfree, Qfree, - id, localid) ; + RigidBilateralLagrangianConstraint::addContact(*this, + norm, P, Q, contactDistance, + m1, m2, Pfree, Qfree, + id, localid); } void registerBilateralLagrangianConstraint(sofa::core::ObjectFactory* factory) diff --git a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/BilateralLagrangianConstraint.h b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/BilateralLagrangianConstraint.h index f986ad3321b..abc001ded95 100644 --- a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/BilateralLagrangianConstraint.h +++ b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/BilateralLagrangianConstraint.h @@ -45,7 +45,6 @@ namespace sofa::component::constraint::lagrangian::model using sofa::core::behavior::BaseConstraint ; using sofa::core::behavior::ConstraintResolution ; using sofa::core::behavior::PairInteractionConstraint ; -using sofa::core::objectmodel::Data ; using sofa::core::ConstraintParams ; using sofa::core::ConstVecCoordId; @@ -93,10 +92,11 @@ class BilateralLagrangianConstraint : public PairInteractionConstraint dfree; + sofa::type::vector m_violation; Quat q; std::vector cid; + SOFA_ATTRIBUTE_DEPRECATED__RENAME_DATA_IN_CONSTRAINT_LAGRANGIAN_MODEL() sofa::core::objectmodel::RenamedData > m1; @@ -114,7 +114,7 @@ class BilateralLagrangianConstraint : public PairInteractionConstraint d_restVector; ///< Relative position to maintain between attached points (optional) VecCoord initialDifference; - Data d_numericalTolerance; ///< a real value specifying the tolerance during the constraint solving. (default=0.0001 + Data d_numericalTolerance; ///< a real value specifying the tolerance during the constraint solving. (default=0.0001 Data d_activate; ///< control constraint activation (true by default) Data d_keepOrientDiff; ///< keep the initial difference in orientation (only for rigids) @@ -128,7 +128,8 @@ class BilateralLagrangianConstraint : public PairInteractionConstraint& resTab, - unsigned int& offset) override; + std::vector& resTab, + unsigned int& offset) override; void handleEvent(sofa::core::objectmodel::Event *event) override; diff --git a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/BilateralLagrangianConstraint.inl b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/BilateralLagrangianConstraint.inl index ad18a6ed7f7..a486df45ecd 100644 --- a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/BilateralLagrangianConstraint.inl +++ b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/BilateralLagrangianConstraint.inl @@ -44,8 +44,8 @@ BilateralLagrangianConstraint::BilateralLagrangianConstraint(Mechanic , d_m1(initData(&d_m1, "first_point","index of the constraint on the first model (object1)")) , d_m2(initData(&d_m2, "second_point","index of the constraint on the second model (object2)")) , d_restVector(initData(&d_restVector, "rest_vector","Relative position to maintain between attached points (optional)")) - , d_numericalTolerance(initData(&d_numericalTolerance, 0.0001, "numericalTolerance", - "a real value specifying the tolerance during the constraint solving. (optional, default=0.0001)") ) + , d_numericalTolerance(initData(&d_numericalTolerance, 1e-4_sreal, "numericalTolerance", + "a real value specifying the tolerance during the constraint solving.") ) , d_activate( initData(&d_activate, true, "activate", "control constraint activation (true by default)")) , d_keepOrientDiff(initData(&d_keepOrientDiff, false, "keepOrientationDifference", "keep the initial difference in orientation (only for rigids)")) , l_topology1(initLink("topology1", "link to the first topology container")) @@ -81,6 +81,11 @@ void BilateralLagrangianConstraint::unspecializedInit() assert(this->mstate1); assert(this->mstate2); + if (!this->mstate1 || !this->mstate2) + { + this->d_componentState.setValue(core::objectmodel::ComponentState::Invalid); + } + prevForces.clear(); } @@ -120,8 +125,10 @@ void BilateralLagrangianConstraint::reinit() template -void BilateralLagrangianConstraint::buildConstraintMatrix(const ConstraintParams*, DataMatrixDeriv &c1_d, DataMatrixDeriv &c2_d, unsigned int &constraintId - , const DataVecCoord &/*x1*/, const DataVecCoord &/*x2*/) +void BilateralLagrangianConstraint::buildConstraintMatrix( + const ConstraintParams*, DataMatrixDeriv& c1_d, DataMatrixDeriv& c2_d, + unsigned int& constraintId, + const DataVecCoord&/*x1*/, const DataVecCoord&/*x2*/) { if (!d_activate.getValue()) return; @@ -133,12 +140,12 @@ void BilateralLagrangianConstraint::buildConstraintMatrix(const Const const SubsetIndices& m1Indices = d_m1.getValue(); const SubsetIndices& m2Indices = d_m2.getValue(); - MatrixDeriv &c1 = *c1_d.beginEdit(); - MatrixDeriv &c2 = *c2_d.beginEdit(); + auto c1 = sofa::helper::getWriteAccessor(c1_d); + auto c2 = sofa::helper::getWriteAccessor(c2_d); cid.resize(minp); - for (unsigned pid=0; pid::buildConstraintMatrix(const Const cid[pid] = constraintId; constraintId += 3; - MatrixDerivRowIterator c1_it = c1.writeLine(cid[pid]); + MatrixDerivRowIterator c1_it = c1->writeLine(cid[pid]); c1_it.addCol(tm1, -cx); - MatrixDerivRowIterator c2_it = c2.writeLine(cid[pid]); + MatrixDerivRowIterator c2_it = c2->writeLine(cid[pid]); c2_it.addCol(tm2, cx); - c1_it = c1.writeLine(cid[pid] + 1); + c1_it = c1->writeLine(cid[pid] + 1); c1_it.setCol(tm1, -cy); - c2_it = c2.writeLine(cid[pid] + 1); + c2_it = c2->writeLine(cid[pid] + 1); c2_it.setCol(tm2, cy); - c1_it = c1.writeLine(cid[pid] + 2); + c1_it = c1->writeLine(cid[pid] + 2); c1_it.setCol(tm1, -cz); - c2_it = c2.writeLine(cid[pid] + 2); + c2_it = c2->writeLine(cid[pid] + 2); c2_it.setCol(tm2, cz); } - - c1_d.endEdit(); - c2_d.endEdit(); } template void BilateralLagrangianConstraint::getConstraintViolation(const ConstraintParams* cParams, - BaseVector *v, - const DataVecCoord &d_x1, const DataVecCoord &d_x2 - , const DataVecDeriv & d_v1, const DataVecDeriv & d_v2) + BaseVector* v, + const DataVecCoord& d_x1, const DataVecCoord& d_x2, + const DataVecDeriv& d_v1, const DataVecDeriv& d_v2) { if (!d_activate.getValue()) return; @@ -196,18 +200,20 @@ void BilateralLagrangianConstraint::getConstraintViolation(const Cons const VecCoord &x1 = d_x1.getValue(); const VecCoord &x2 = d_x2.getValue(); - dfree.resize(minp); + m_violation.resize(minp); - for (unsigned pid=0; pidset(cid[pid] , dfree[pid][0]); - v->set(cid[pid]+1, dfree[pid][1]); - v->set(cid[pid]+2, dfree[pid][2]); + v->set(cid[pid] , m_violation[pid][0]); + v->set(cid[pid]+1, m_violation[pid][1]); + v->set(cid[pid]+2, m_violation[pid][2]); } } @@ -235,7 +241,7 @@ void BilateralLagrangianConstraint::getVelocityViolation(BaseVector * auto pos2 = this->getMState2()->readPositions(); const SReal dt = this->getContext()->getDt(); - const SReal invDt = SReal(1.0) / dt; + const SReal invDt = 1_sreal / dt; for (unsigned pid=0; pidgaussSeidel(0, this); break; } - // UnbuiltGaussSeidel - case 1: { + case ResolutionMethod("UnbuiltGaussSeidel"): { SCOPED_TIMER_VARNAME(unbuiltGaussSeidelTimer, "ConstraintsUnbuiltGaussSeidel"); current_cp->unbuiltGaussSeidel(0, this); break; } - // NonsmoothNonlinearConjugateGradient - case 2: { + case ResolutionMethod("NonsmoothNonlinearConjugateGradient"): { current_cp->NNCG(this, d_newtonIterations.getValue()); break; } diff --git a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintSolver.h b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintSolver.h index 7d752048930..cd3c770fd97 100644 --- a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintSolver.h +++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintSolver.h @@ -33,6 +33,8 @@ #include #include +#include + namespace sofa::component::constraint::lagrangian::solver { @@ -60,7 +62,13 @@ class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API GenericConstraintSolver : ConstraintProblem* getConstraintProblem() override; void lockConstraintProblem(sofa::core::objectmodel::BaseObject* from, ConstraintProblem* p1, ConstraintProblem* p2 = nullptr) override; - Data< sofa::helper::OptionsGroup > d_resolutionMethod; ///< Method used to solve the constraint problem, among: "ProjectedGaussSeidel", "UnbuiltGaussSeidel" or "for NonsmoothNonlinearConjugateGradient" + MAKE_SELECTABLE_ITEMS(ResolutionMethod, + sofa::helper::Item{"ProjectedGaussSeidel", "Projected Gauss-Seidel"}, + sofa::helper::Item{"UnbuiltGaussSeidel", "Gauss-Seidel where the matrix is not assembled"}, + sofa::helper::Item{"NonsmoothNonlinearConjugateGradient", "Non-smooth non-linear conjugate gradient"} + ); + + Data< ResolutionMethod > d_resolutionMethod; ///< Method used to solve the constraint problem, among: "ProjectedGaussSeidel", "UnbuiltGaussSeidel" or "for NonsmoothNonlinearConjugateGradient" SOFA_ATTRIBUTE_DEPRECATED__RENAME_DATA_IN_CONSTRAINT_LAGRANGIAN_SOLVER() sofa::core::objectmodel::RenamedData maxIt; diff --git a/Sofa/Component/LinearSolver/Iterative/CMakeLists.txt b/Sofa/Component/LinearSolver/Iterative/CMakeLists.txt index e94666809ec..50514787466 100644 --- a/Sofa/Component/LinearSolver/Iterative/CMakeLists.txt +++ b/Sofa/Component/LinearSolver/Iterative/CMakeLists.txt @@ -18,6 +18,8 @@ set(HEADER_FILES ${SOFACOMPONENTLINEARSOLVERITERATIVE_SOURCE_DIR}/MinResLinearSolver.inl ${SOFACOMPONENTLINEARSOLVERITERATIVE_SOURCE_DIR}/ShewchukPCGLinearSolver.h ${SOFACOMPONENTLINEARSOLVERITERATIVE_SOURCE_DIR}/ShewchukPCGLinearSolver.inl + ${SOFACOMPONENTLINEARSOLVERITERATIVE_SOURCE_DIR}/PCGLinearSolver.h + ${SOFACOMPONENTLINEARSOLVERITERATIVE_SOURCE_DIR}/PCGLinearSolver.inl ) set(SOURCE_FILES @@ -29,7 +31,7 @@ set(SOURCE_FILES ${SOFACOMPONENTLINEARSOLVERITERATIVE_SOURCE_DIR}/MatrixLinearSolver.cpp ${SOFACOMPONENTLINEARSOLVERITERATIVE_SOURCE_DIR}/MatrixLinearSystem[GraphScattered].cpp ${SOFACOMPONENTLINEARSOLVERITERATIVE_SOURCE_DIR}/MinResLinearSolver.cpp - ${SOFACOMPONENTLINEARSOLVERITERATIVE_SOURCE_DIR}/ShewchukPCGLinearSolver.cpp + ${SOFACOMPONENTLINEARSOLVERITERATIVE_SOURCE_DIR}/PCGLinearSolver.cpp ) sofa_find_package(Sofa.Simulation.Core REQUIRED) diff --git a/Sofa/Component/LinearSolver/Iterative/src/sofa/component/linearsolver/iterative/GraphScatteredTypes.cpp b/Sofa/Component/LinearSolver/Iterative/src/sofa/component/linearsolver/iterative/GraphScatteredTypes.cpp index 51dacd5f02d..f9aa5f3a656 100644 --- a/Sofa/Component/LinearSolver/Iterative/src/sofa/component/linearsolver/iterative/GraphScatteredTypes.cpp +++ b/Sofa/Component/LinearSolver/Iterative/src/sofa/component/linearsolver/iterative/GraphScatteredTypes.cpp @@ -34,7 +34,10 @@ void GraphScatteredMatrix::apply(GraphScatteredVector& res, GraphScatteredVector { // matrix-vector product through visitors parent->propagateDxAndResetDf(x,res); - parent->addMBKdx(res,parent->mparams.mFactor(),parent->mparams.bFactor(),parent->mparams.kFactor(), false); // df = (m M + b B + k K) dx + parent->addMBKdx(res, + core::MatricesFactors::M(parent->mparams.mFactor()), + core::MatricesFactors::B(parent->mparams.bFactor()), + core::MatricesFactors::K(parent->mparams.kFactor()), false); // df = (m M + b B + k K) dx // filter the product to take the constraints into account // diff --git a/Sofa/Component/LinearSolver/Iterative/src/sofa/component/linearsolver/iterative/ShewchukPCGLinearSolver.cpp b/Sofa/Component/LinearSolver/Iterative/src/sofa/component/linearsolver/iterative/PCGLinearSolver.cpp similarity index 82% rename from Sofa/Component/LinearSolver/Iterative/src/sofa/component/linearsolver/iterative/ShewchukPCGLinearSolver.cpp rename to Sofa/Component/LinearSolver/Iterative/src/sofa/component/linearsolver/iterative/PCGLinearSolver.cpp index 5cd240c90db..72f4df45bed 100644 --- a/Sofa/Component/LinearSolver/Iterative/src/sofa/component/linearsolver/iterative/ShewchukPCGLinearSolver.cpp +++ b/Sofa/Component/LinearSolver/Iterative/src/sofa/component/linearsolver/iterative/PCGLinearSolver.cpp @@ -19,8 +19,8 @@ * * * Contact information: contact@sofa-framework.org * ******************************************************************************/ -#define SOFA_COMPONENT_LINEARSOLVER_ITERATIVE_SHEWCHUKPCGLINEARSOLVER_CPP -#include +#define SOFA_COMPONENT_LINEARSOLVER_ITERATIVE_PCGLINEARSOLVER_CPP +#include #include #include #include @@ -28,12 +28,12 @@ namespace sofa::component::linearsolver::iterative { -void registerShewchukPCGLinearSolver(sofa::core::ObjectFactory* factory) +void registerPCGLinearSolver(sofa::core::ObjectFactory* factory) { factory->registerObjects(core::ObjectRegistrationData("Linear system solver using the Shewchuk conjugate gradient iterative algorithm.") - .add< ShewchukPCGLinearSolver >()); + .add< PCGLinearSolver >()); } -template class SOFA_COMPONENT_LINEARSOLVER_ITERATIVE_API ShewchukPCGLinearSolver; +template class SOFA_COMPONENT_LINEARSOLVER_ITERATIVE_API PCGLinearSolver; } // namespace sofa::component::linearsolver::iterative diff --git a/Sofa/Component/LinearSolver/Iterative/src/sofa/component/linearsolver/iterative/PCGLinearSolver.h b/Sofa/Component/LinearSolver/Iterative/src/sofa/component/linearsolver/iterative/PCGLinearSolver.h new file mode 100644 index 00000000000..1506256becd --- /dev/null +++ b/Sofa/Component/LinearSolver/Iterative/src/sofa/component/linearsolver/iterative/PCGLinearSolver.h @@ -0,0 +1,123 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once +#include + +#include +#include +#include + +#include + +#include + +namespace sofa::component::linearsolver::iterative +{ + +/// Linear system solver using the conjugate gradient iterative algorithm +template +class PCGLinearSolver : public sofa::component::linearsolver::MatrixLinearSolver +{ +public: + SOFA_CLASS(SOFA_TEMPLATE2(PCGLinearSolver,TMatrix,TVector),SOFA_TEMPLATE2(sofa::component::linearsolver::MatrixLinearSolver,TMatrix,TVector)); + + typedef TMatrix Matrix; + typedef TVector Vector; + typedef sofa::component::linearsolver::MatrixLinearSolver Inherit; + + SOFA_ATTRIBUTE_DEPRECATED__RENAME_DATA_IN_LINEARSOLVER_ITERATIVE() + sofa::core::objectmodel::RenamedData f_maxIter; + + SOFA_ATTRIBUTE_DEPRECATED__RENAME_DATA_IN_LINEARSOLVER_ITERATIVE() + sofa::core::objectmodel::RenamedData f_tolerance; + + SOFA_ATTRIBUTE_DEPRECATED__RENAME_DATA_IN_LINEARSOLVER_ITERATIVE() + sofa::core::objectmodel::RenamedData f_use_precond; + + SOFA_ATTRIBUTE_DEPRECATED__RENAME_DATA_IN_LINEARSOLVER_ITERATIVE() + sofa::core::objectmodel::RenamedData f_update_step; + + SOFA_ATTRIBUTE_DEPRECATED__RENAME_DATA_IN_LINEARSOLVER_ITERATIVE() + sofa::core::objectmodel::RenamedData f_build_precond; + + SOFA_ATTRIBUTE_DEPRECATED__RENAME_DATA_IN_LINEARSOLVER_ITERATIVE() + sofa::core::objectmodel::RenamedData > > f_graph; + + + Data d_maxIter; ///< Maximum number of iterations after which the iterative descent of the Conjugate Gradient must stop + Data d_tolerance; ///< Desired accuracy of the Conjugate Gradient solution evaluating: |r|²/|b|² (ratio of current residual norm over initial residual norm) + Data d_use_precond; ///< Use a preconditioner + SingleLink l_preconditioner; ///< Link towards the linear solver used to precondition the conjugate gradient + Data d_update_step; ///< Number of steps before the next refresh of preconditioners + Data d_build_precond; ///< Build the preconditioners, if false build the preconditioner only at the initial step + Data > > d_graph; ///< Graph of residuals at each iteration + +protected: + PCGLinearSolver(); + +public: + void solve (Matrix& M, Vector& x, Vector& b) override; + void init() override; + void setSystemMBKMatrix(const core::MechanicalParams* mparams) override; + +private : + unsigned next_refresh_step; + bool first; + int newton_iter; + +protected: + /// This method is separated from the rest to be able to use custom/optimized versions depending on the types of vectors. + /// It computes: p = p*beta + r + inline void cgstep_beta(Vector& p, Vector& r, double beta); + /// This method is separated from the rest to be able to use custom/optimized versions depending on the types of vectors. + /// It computes: x += p*alpha, r -= q*alpha + inline void cgstep_alpha(Vector& x,Vector& p,double alpha); + + void handleEvent(sofa::core::objectmodel::Event* event) override; + + +}; + +template +inline void PCGLinearSolver::cgstep_beta(Vector& p, Vector& r, double beta) +{ + p *= beta; + p += r; //z; +} + +template +inline void PCGLinearSolver::cgstep_alpha(Vector& x,Vector& p,double alpha) +{ + x.peq(p,alpha); // x = x + alpha p +} + +template<> +inline void PCGLinearSolver::cgstep_beta(Vector& p, Vector& r, double beta); + +template<> +inline void PCGLinearSolver::cgstep_alpha(Vector& x,Vector& p,double alpha); + +#if !defined(SOFA_COMPONENT_LINEARSOLVER_ITERATIVE_PCGLINEARSOLVER_CPP) +template class SOFA_COMPONENT_LINEARSOLVER_ITERATIVE_API PCGLinearSolver; +#endif // !defined(SOFA_COMPONENT_LINEARSOLVER_ITERATIVE_PCGLINEARSOLVER_CPP) + +} // namespace sofa::component::linearsolver::iterative diff --git a/Sofa/Component/LinearSolver/Iterative/src/sofa/component/linearsolver/iterative/PCGLinearSolver.inl b/Sofa/Component/LinearSolver/Iterative/src/sofa/component/linearsolver/iterative/PCGLinearSolver.inl new file mode 100644 index 00000000000..ff9da2bb46e --- /dev/null +++ b/Sofa/Component/LinearSolver/Iterative/src/sofa/component/linearsolver/iterative/PCGLinearSolver.inl @@ -0,0 +1,247 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once +#include + +#include +#include +#include +#include +#include +#include + +#include + +namespace sofa::component::linearsolver::iterative +{ + +template +PCGLinearSolver::PCGLinearSolver() + : d_maxIter(initData(&d_maxIter, (unsigned)25, "iterations", "Maximum number of iterations after which the iterative descent of the Conjugate Gradient must stop") ) + , d_tolerance(initData(&d_tolerance, 1e-5, "tolerance", "Desired accuracy of the Conjugate Gradient solution evaluating: |r|²/|b|² (ratio of current residual norm over initial residual norm)") ) + , d_use_precond(initData(&d_use_precond, true, "use_precond", "Use a preconditioner") ) + , l_preconditioner(initLink("preconditioner", "Link towards the linear solver used to precondition the conjugate gradient")) + , d_update_step(initData(&d_update_step, (unsigned)1, "update_step", "Number of steps before the next refresh of preconditioners") ) + , d_build_precond(initData(&d_build_precond, true, "build_precond", "Build the preconditioners, if false build the preconditioner only at the initial step") ) + , d_graph(initData(&d_graph, "graph", "Graph of residuals at each iteration") ) + , next_refresh_step(0) + , newton_iter(0) +{ + d_graph.setWidget("graph"); + first = true; + this->f_listening.setValue(true); + + f_maxIter.setOriginalData(&d_maxIter); + f_tolerance.setOriginalData(&d_tolerance); + f_use_precond.setOriginalData(&d_use_precond); + f_update_step.setOriginalData(&d_update_step); + f_build_precond.setOriginalData(&d_build_precond); + f_graph.setOriginalData(&d_graph); + +} + +template +void PCGLinearSolver::init() +{ + Inherit1::init(); + + // Find linear solvers + if (l_preconditioner.empty()) + { + msg_info() << "Link \"preconditioner\" to the desired linear solver should be set to precondition the conjugate gradient."; + } + else + { + if (l_preconditioner.get() == nullptr) + { + msg_error() << "No preconditioner found at path: " << l_preconditioner.getLinkedPath(); + sofa::core::objectmodel::BaseObject::d_componentState.setValue(sofa::core::objectmodel::ComponentState::Invalid); + return; + } + else + { + if (l_preconditioner.get()->getTemplateName() == "GraphScattered") + { + msg_error() << "Can not use the preconditioner " << l_preconditioner.get()->getName() << " because it is templated on GraphScatteredType"; + sofa::core::objectmodel::BaseObject::d_componentState.setValue(sofa::core::objectmodel::ComponentState::Invalid); + return; + } + else + { + msg_info() << "Preconditioner path used: '" << l_preconditioner.getLinkedPath() << "'"; + } + } + } + + first = true; + sofa::core::objectmodel::BaseObject::d_componentState.setValue(sofa::core::objectmodel::ComponentState::Valid); +} + +template +void PCGLinearSolver::setSystemMBKMatrix(const core::MechanicalParams* mparams) +{ + sofa::helper::AdvancedTimer::valSet("PCG::buildMBK", 1); + + { + SCOPED_TIMER("PCG::setSystemMBKMatrix"); + Inherit::setSystemMBKMatrix(mparams); + } + + if (l_preconditioner.get()==nullptr) return; + + if (first) //We initialize all the preconditioners for the first step + { + l_preconditioner.get()->setSystemMBKMatrix(mparams); + first = false; + next_refresh_step = 1; + } + else if (d_build_precond.getValue()) + { + sofa::helper::AdvancedTimer::valSet("PCG::PrecondBuildMBK", 1); + SCOPED_TIMER_VARNAME(mbkTimer, "PCG::PrecondSetSystemMBKMatrix"); + + if (d_update_step.getValue() > 0) + { + if (next_refresh_step >= d_update_step.getValue()) + { + l_preconditioner.get()->setSystemMBKMatrix(mparams); + next_refresh_step=1; + } + else + { + next_refresh_step++; + } + } + } + + l_preconditioner.get()->updateSystemMatrix(); +} + +template<> +inline void PCGLinearSolver::cgstep_beta(Vector& p, Vector& r, double beta) +{ + p.eq(r,p,beta); // p = p*beta + r +} + +template<> +inline void PCGLinearSolver::cgstep_alpha(Vector& x, Vector& p, double alpha) +{ + x.peq(p,alpha); // x = x + alpha p +} + +template +void PCGLinearSolver::handleEvent(sofa::core::objectmodel::Event* event) { + /// this event shoul be launch before the addKToMatrix + if (sofa::simulation::AnimateBeginEvent::checkEventType(event)) + { + newton_iter = 0; + std::map < std::string, sofa::type::vector >& graph = * d_graph.beginEdit(); + graph.clear(); + } +} + + +template +void PCGLinearSolver::solve (Matrix& M, Vector& x, Vector& b) +{ + SCOPED_TIMER_VARNAME(solveTimer, "PCGLinearSolver::solve"); + + std::map < std::string, sofa::type::vector >& graph = * d_graph.beginEdit(); +// sofa::type::vector& graph_error = graph["Error"]; + + newton_iter++; + char name[256]; + sprintf(name,"Error %d",newton_iter); + sofa::type::vector& graph_error = graph[std::string(name)]; + + const core::ExecParams* params = core::execparams::defaultInstance(); + typename Inherit::TempVectorContainer vtmp(this, params, M, x, b); + Vector& r = *vtmp.createTempVector(); + Vector& w = *vtmp.createTempVector(); + Vector& s = *vtmp.createTempVector(); + + const bool apply_precond = l_preconditioner.get()!=nullptr && d_use_precond.getValue(); + + const double b_norm = b.dot(b); + const double tol = d_tolerance.getValue() * b_norm; + + r = M * x; + cgstep_beta(r,b,-1);// r = -1 * r + b = b - (M * x) + + if (apply_precond) + { + SCOPED_TIMER_VARNAME(applyPrecondTimer, "PCGLinearSolver::apply Precond"); + l_preconditioner.get()->setSystemLHVector(w); + l_preconditioner.get()->setSystemRHVector(r); + l_preconditioner.get()->solveSystem(); + } + else + { + w = r; + } + + double r_norm = r.dot(w); + graph_error.push_back(r_norm/b_norm); + + unsigned iter=1; + while ((iter <= d_maxIter.getValue()) && (r_norm > tol)) + { + s = M * w; + const double dtq = w.dot(s); + double alpha = r_norm / dtq; + + cgstep_alpha(x,w,alpha);//for(int i=0; isetSystemLHVector(s); + l_preconditioner.get()->setSystemRHVector(r); + l_preconditioner.get()->solveSystem(); + } + else + { + s = r; + } + + const double deltaOld = r_norm; + r_norm = r.dot(s); + graph_error.push_back(r_norm/b_norm); + + double beta = r_norm / deltaOld; + + cgstep_beta(w,s,beta);//for (int i=0; i -#include -#include -#include +#include -#include - -#include +SOFA_HEADER_DEPRECATED("v24.12", "v25.12", "sofa/component/linearsolver/iterative/PCGLinearSolver.h") namespace sofa::component::linearsolver::iterative { - -/// Linear system solver using the conjugate gradient iterative algorithm -template -class ShewchukPCGLinearSolver : public sofa::component::linearsolver::MatrixLinearSolver -{ -public: - SOFA_CLASS(SOFA_TEMPLATE2(ShewchukPCGLinearSolver,TMatrix,TVector),SOFA_TEMPLATE2(sofa::component::linearsolver::MatrixLinearSolver,TMatrix,TVector)); - - typedef TMatrix Matrix; - typedef TVector Vector; - typedef sofa::component::linearsolver::MatrixLinearSolver Inherit; - - SOFA_ATTRIBUTE_DEPRECATED__RENAME_DATA_IN_LINEARSOLVER_ITERATIVE() - sofa::core::objectmodel::RenamedData f_maxIter; - - SOFA_ATTRIBUTE_DEPRECATED__RENAME_DATA_IN_LINEARSOLVER_ITERATIVE() - sofa::core::objectmodel::RenamedData f_tolerance; - - SOFA_ATTRIBUTE_DEPRECATED__RENAME_DATA_IN_LINEARSOLVER_ITERATIVE() - sofa::core::objectmodel::RenamedData f_use_precond; - - SOFA_ATTRIBUTE_DEPRECATED__RENAME_DATA_IN_LINEARSOLVER_ITERATIVE() - sofa::core::objectmodel::RenamedData f_update_step; - - SOFA_ATTRIBUTE_DEPRECATED__RENAME_DATA_IN_LINEARSOLVER_ITERATIVE() - sofa::core::objectmodel::RenamedData f_build_precond; - - SOFA_ATTRIBUTE_DEPRECATED__RENAME_DATA_IN_LINEARSOLVER_ITERATIVE() - sofa::core::objectmodel::RenamedData > > f_graph; - - - Data d_maxIter; ///< Maximum number of iterations after which the iterative descent of the Conjugate Gradient must stop - Data d_tolerance; ///< Desired accuracy of the Conjugate Gradient solution evaluating: |r|²/|b|² (ratio of current residual norm over initial residual norm) - Data d_use_precond; ///< Use a preconditioner - SingleLink l_preconditioner; ///< Link towards the linear solver used to precondition the conjugate gradient - Data d_update_step; ///< Number of steps before the next refresh of preconditioners - Data d_build_precond; ///< Build the preconditioners, if false build the preconditioner only at the initial step - Data > > d_graph; ///< Graph of residuals at each iteration - -protected: - ShewchukPCGLinearSolver(); - -public: - void solve (Matrix& M, Vector& x, Vector& b) override; - void init() override; - void setSystemMBKMatrix(const core::MechanicalParams* mparams) override; - -private : - unsigned next_refresh_step; - bool first; - int newton_iter; - -protected: - /// This method is separated from the rest to be able to use custom/optimized versions depending on the types of vectors. - /// It computes: p = p*beta + r - inline void cgstep_beta(Vector& p, Vector& r, double beta); - /// This method is separated from the rest to be able to use custom/optimized versions depending on the types of vectors. - /// It computes: x += p*alpha, r -= q*alpha - inline void cgstep_alpha(Vector& x,Vector& p,double alpha); - - void handleEvent(sofa::core::objectmodel::Event* event) override; - - -}; - -template -inline void ShewchukPCGLinearSolver::cgstep_beta(Vector& p, Vector& r, double beta) -{ - p *= beta; - p += r; //z; -} - template -inline void ShewchukPCGLinearSolver::cgstep_alpha(Vector& x,Vector& p,double alpha) -{ - x.peq(p,alpha); // x = x + alpha p +using ShewchukPCGLinearSolver SOFA_ATTRIBUTE_DEPRECATED("v24.12", "v25.12", "ShewchukPCGLinearSolver has been renamed to PCGLinearSolver") = PCGLinearSolver; } - -template<> -inline void ShewchukPCGLinearSolver::cgstep_beta(Vector& p, Vector& r, double beta); - -template<> -inline void ShewchukPCGLinearSolver::cgstep_alpha(Vector& x,Vector& p,double alpha); - -#if !defined(SOFA_COMPONENT_LINEARSOLVER_ITERATIVE_SHEWCHUKPCGLINEARSOLVER_CPP) -template class SOFA_COMPONENT_LINEARSOLVER_ITERATIVE_API ShewchukPCGLinearSolver; -#endif // !defined(SOFA_COMPONENT_LINEARSOLVER_ITERATIVE_SHEWCHUKPCGLINEARSOLVER_CPP) - -} // namespace sofa::component::linearsolver::iterative diff --git a/Sofa/Component/LinearSolver/Iterative/src/sofa/component/linearsolver/iterative/ShewchukPCGLinearSolver.inl b/Sofa/Component/LinearSolver/Iterative/src/sofa/component/linearsolver/iterative/ShewchukPCGLinearSolver.inl index 68eb09ae9d0..8345764ae6c 100644 --- a/Sofa/Component/LinearSolver/Iterative/src/sofa/component/linearsolver/iterative/ShewchukPCGLinearSolver.inl +++ b/Sofa/Component/LinearSolver/Iterative/src/sofa/component/linearsolver/iterative/ShewchukPCGLinearSolver.inl @@ -20,228 +20,7 @@ * Contact information: contact@sofa-framework.org * ******************************************************************************/ #pragma once -#include -#include -#include -#include -#include -#include -#include +#include -#include - -namespace sofa::component::linearsolver::iterative -{ - -template -ShewchukPCGLinearSolver::ShewchukPCGLinearSolver() - : d_maxIter(initData(&d_maxIter, (unsigned)25, "iterations", "Maximum number of iterations after which the iterative descent of the Conjugate Gradient must stop") ) - , d_tolerance(initData(&d_tolerance, 1e-5, "tolerance", "Desired accuracy of the Conjugate Gradient solution evaluating: |r|²/|b|² (ratio of current residual norm over initial residual norm)") ) - , d_use_precond(initData(&d_use_precond, true, "use_precond", "Use a preconditioner") ) - , l_preconditioner(initLink("preconditioner", "Link towards the linear solver used to precondition the conjugate gradient")) - , d_update_step(initData(&d_update_step, (unsigned)1, "update_step", "Number of steps before the next refresh of preconditioners") ) - , d_build_precond(initData(&d_build_precond, true, "build_precond", "Build the preconditioners, if false build the preconditioner only at the initial step") ) - , d_graph(initData(&d_graph, "graph", "Graph of residuals at each iteration") ) - , next_refresh_step(0) - , newton_iter(0) -{ - d_graph.setWidget("graph"); - first = true; - this->f_listening.setValue(true); - - f_maxIter.setOriginalData(&d_maxIter); - f_tolerance.setOriginalData(&d_tolerance); - f_use_precond.setOriginalData(&d_use_precond); - f_update_step.setOriginalData(&d_update_step); - f_build_precond.setOriginalData(&d_build_precond); - f_graph.setOriginalData(&d_graph); - -} - -template -void ShewchukPCGLinearSolver::init() -{ - Inherit1::init(); - - // Find linear solvers - if (l_preconditioner.empty()) - { - msg_info() << "Link \"preconditioner\" to the desired linear solver should be set to precondition the conjugate gradient."; - } - else - { - if (l_preconditioner.get() == nullptr) - { - msg_error() << "No preconditioner found at path: " << l_preconditioner.getLinkedPath(); - sofa::core::objectmodel::BaseObject::d_componentState.setValue(sofa::core::objectmodel::ComponentState::Invalid); - return; - } - else - { - if (l_preconditioner.get()->getTemplateName() == "GraphScattered") - { - msg_error() << "Can not use the preconditioner " << l_preconditioner.get()->getName() << " because it is templated on GraphScatteredType"; - sofa::core::objectmodel::BaseObject::d_componentState.setValue(sofa::core::objectmodel::ComponentState::Invalid); - return; - } - else - { - msg_info() << "Preconditioner path used: '" << l_preconditioner.getLinkedPath() << "'"; - } - } - } - - first = true; - sofa::core::objectmodel::BaseObject::d_componentState.setValue(sofa::core::objectmodel::ComponentState::Valid); -} - -template -void ShewchukPCGLinearSolver::setSystemMBKMatrix(const core::MechanicalParams* mparams) -{ - sofa::helper::AdvancedTimer::valSet("PCG::buildMBK", 1); - - { - SCOPED_TIMER("PCG::setSystemMBKMatrix"); - Inherit::setSystemMBKMatrix(mparams); - } - - if (l_preconditioner.get()==nullptr) return; - - if (first) //We initialize all the preconditioners for the first step - { - l_preconditioner.get()->setSystemMBKMatrix(mparams); - first = false; - next_refresh_step = 1; - } - else if (d_build_precond.getValue()) - { - sofa::helper::AdvancedTimer::valSet("PCG::PrecondBuildMBK", 1); - SCOPED_TIMER_VARNAME(mbkTimer, "PCG::PrecondSetSystemMBKMatrix"); - - if (d_update_step.getValue() > 0) - { - if (next_refresh_step >= d_update_step.getValue()) - { - l_preconditioner.get()->setSystemMBKMatrix(mparams); - next_refresh_step=1; - } - else - { - next_refresh_step++; - } - } - } - - l_preconditioner.get()->updateSystemMatrix(); -} - -template<> -inline void ShewchukPCGLinearSolver::cgstep_beta(Vector& p, Vector& r, double beta) -{ - p.eq(r,p,beta); // p = p*beta + r -} - -template<> -inline void ShewchukPCGLinearSolver::cgstep_alpha(Vector& x, Vector& p, double alpha) -{ - x.peq(p,alpha); // x = x + alpha p -} - -template -void ShewchukPCGLinearSolver::handleEvent(sofa::core::objectmodel::Event* event) { - /// this event shoul be launch before the addKToMatrix - if (sofa::simulation::AnimateBeginEvent::checkEventType(event)) - { - newton_iter = 0; - std::map < std::string, sofa::type::vector >& graph = * d_graph.beginEdit(); - graph.clear(); - } -} - - -template -void ShewchukPCGLinearSolver::solve (Matrix& M, Vector& x, Vector& b) -{ - SCOPED_TIMER_VARNAME(solveTimer, "PCGLinearSolver::solve"); - - std::map < std::string, sofa::type::vector >& graph = * d_graph.beginEdit(); -// sofa::type::vector& graph_error = graph["Error"]; - - newton_iter++; - char name[256]; - sprintf(name,"Error %d",newton_iter); - sofa::type::vector& graph_error = graph[std::string(name)]; - - const core::ExecParams* params = core::execparams::defaultInstance(); - typename Inherit::TempVectorContainer vtmp(this, params, M, x, b); - Vector& r = *vtmp.createTempVector(); - Vector& w = *vtmp.createTempVector(); - Vector& s = *vtmp.createTempVector(); - - const bool apply_precond = l_preconditioner.get()!=nullptr && d_use_precond.getValue(); - - const double b_norm = b.dot(b); - const double tol = d_tolerance.getValue() * b_norm; - - r = M * x; - cgstep_beta(r,b,-1);// r = -1 * r + b = b - (M * x) - - if (apply_precond) - { - SCOPED_TIMER_VARNAME(applyPrecondTimer, "PCGLinearSolver::apply Precond"); - l_preconditioner.get()->setSystemLHVector(w); - l_preconditioner.get()->setSystemRHVector(r); - l_preconditioner.get()->solveSystem(); - } - else - { - w = r; - } - - double r_norm = r.dot(w); - graph_error.push_back(r_norm/b_norm); - - unsigned iter=1; - while ((iter <= d_maxIter.getValue()) && (r_norm > tol)) - { - s = M * w; - const double dtq = w.dot(s); - double alpha = r_norm / dtq; - - cgstep_alpha(x,w,alpha);//for(int i=0; isetSystemLHVector(s); - l_preconditioner.get()->setSystemRHVector(r); - l_preconditioner.get()->solveSystem(); - } - else - { - s = r; - } - - const double deltaOld = r_norm; - r_norm = r.dot(s); - graph_error.push_back(r_norm/b_norm); - - double beta = r_norm / deltaOld; - - cgstep_beta(w,s,beta);//for (int i=0; i class IdentityMultiMapping : public LinearMultiMapping { diff --git a/Sofa/Component/Mapping/Linear/src/sofa/component/mapping/linear/SubsetMultiMapping.h b/Sofa/Component/Mapping/Linear/src/sofa/component/mapping/linear/SubsetMultiMapping.h index 066888c34e5..75fe2fe440c 100644 --- a/Sofa/Component/Mapping/Linear/src/sofa/component/mapping/linear/SubsetMultiMapping.h +++ b/Sofa/Component/Mapping/Linear/src/sofa/component/mapping/linear/SubsetMultiMapping.h @@ -34,10 +34,6 @@ namespace sofa::component::mapping::linear { -/** - * @class SubsetMapping - * @brief Compute a subset of input points - */ template class SubsetMultiMapping : public LinearMultiMapping { @@ -48,28 +44,6 @@ class SubsetMultiMapping : public LinearMultiMapping typedef TIn In; typedef TOut Out; - typedef typename Out::VecCoord OutVecCoord; - typedef typename Out::VecDeriv OutVecDeriv; - typedef typename Out::Coord OutCoord; - typedef typename Out::Deriv OutDeriv; - typedef typename Out::MatrixDeriv OutMatrixDeriv; - typedef typename In::VecCoord InVecCoord; - typedef typename In::VecDeriv InVecDeriv; - typedef typename In::Coord InCoord; - typedef typename In::Deriv InDeriv; - typedef typename In::MatrixDeriv InMatrixDeriv; - typedef typename InCoord::value_type Real; - typedef typename type::vector vecConstInVecCoord; - typedef typename type::vector vecOutVecCoord; - - typedef Data InDataVecCoord; - typedef Data InDataVecDeriv; - typedef Data InDataMatrixDeriv; - - typedef Data OutDataVecCoord; - typedef Data OutDataVecDeriv; - typedef Data OutDataMatrixDeriv; - /// Correspondence array typedef core::topology::BaseMeshTopology::SetIndex IndexArray; @@ -81,12 +55,12 @@ class SubsetMultiMapping : public LinearMultiMapping void addPoint(int fromModel, int index); // usual Mapping API - void apply(const core::MechanicalParams* mparams, const type::vector& dataVecOutPos, const type::vector& dataVecInPos) override; - void applyJ(const core::MechanicalParams* mparams, const type::vector& dataVecOutVel, const type::vector& dataVecInVel) override; - void applyJT(const core::MechanicalParams* mparams, const type::vector& dataVecOutForce, const type::vector& dataVecInForce) override; + void apply(const core::MechanicalParams* mparams, const type::vector*>& dataVecOutPos, const type::vector*>& dataVecInPos) override; + void applyJ(const core::MechanicalParams* mparams, const type::vector*>& dataVecOutVel, const type::vector*>& dataVecInVel) override; + void applyJT(const core::MechanicalParams* mparams, const type::vector*>& dataVecOutForce, const type::vector*>& dataVecInForce) override; void applyDJT(const core::MechanicalParams* /*mparams*/, core::MultiVecDerivId /*inForce*/, core::ConstMultiVecDerivId /*outForce*/) override {} - void applyJT( const core::ConstraintParams* cparams, const type::vector< InDataMatrixDeriv* >& dataMatOutConst, const type::vector< const OutDataMatrixDeriv* >& dataMatInConst ) override; + void applyJT( const core::ConstraintParams* cparams, const type::vector< DataMatrixDeriv_t* >& dataMatOutConst, const type::vector< const DataMatrixDeriv_t* >& dataMatInConst ) override; /// Experimental API used to handle multimappings in matrix assembly. Returns pointers to matrices associated with parent states, consistently with getFrom(). virtual const type::vector* getJs() override; @@ -99,7 +73,7 @@ class SubsetMultiMapping : public LinearMultiMapping protected : SubsetMultiMapping(); - virtual ~SubsetMultiMapping(); + virtual ~SubsetMultiMapping() override; type::vector baseMatrices; ///< Jacobian of the mapping, in a vector @@ -112,7 +86,6 @@ extern template class SOFA_COMPONENT_MAPPING_LINEAR_API SubsetMultiMapping< defa extern template class SOFA_COMPONENT_MAPPING_LINEAR_API SubsetMultiMapping< defaulttype::Vec1Types, defaulttype::Vec1Types >; extern template class SOFA_COMPONENT_MAPPING_LINEAR_API SubsetMultiMapping< defaulttype::Rigid3Types, defaulttype::Rigid3Types >; extern template class SOFA_COMPONENT_MAPPING_LINEAR_API SubsetMultiMapping< defaulttype::Rigid3Types, defaulttype::Vec3Types >; - #endif } // namespace sofa::component::mapping::linear diff --git a/Sofa/Component/Mapping/Linear/src/sofa/component/mapping/linear/SubsetMultiMapping.inl b/Sofa/Component/Mapping/Linear/src/sofa/component/mapping/linear/SubsetMultiMapping.inl index 261b0f853fd..62e1d9dd422 100644 --- a/Sofa/Component/Mapping/Linear/src/sofa/component/mapping/linear/SubsetMultiMapping.inl +++ b/Sofa/Component/Mapping/Linear/src/sofa/component/mapping/linear/SubsetMultiMapping.inl @@ -43,25 +43,29 @@ void SubsetMultiMapping::init() static constexpr auto Nout = TOut::deriv_total_size; - for( unsigned i=0; i Jacobian; baseMatrices.resize( this->getFrom().size() ); type::vector jacobians( this->getFrom().size() ); - for(unsigned i=0; i; jacobians[i]->resize(Nout*indexPairSize,Nin*this->fromModels[i]->getSize() ); // each jacobian has the same number of rows } + const auto& indexPairsValue = d_indexPairs.getValue(); + // fill the Jacobians - for(unsigned i=0; i::init() } } // finalize the Jacobians - for(unsigned i=0; icompress(); + for (linearalgebra::BaseMatrix* baseMat : baseMatrices) + { + baseMat->compress(); + } } template SubsetMultiMapping::SubsetMultiMapping() - : Inherit() - , d_indexPairs(initData(&d_indexPairs, type::vector(), "indexPairs", "list of couples (parent index + index in the parent)")) + : d_indexPairs(initData(&d_indexPairs, type::vector(), "indexPairs", + "list of couples (parent index + index in the parent)")) { - indexPairs.setOriginalData(&d_indexPairs); + indexPairs.setOriginalData(&d_indexPairs); } template SubsetMultiMapping::~SubsetMultiMapping() { - for(unsigned i=0; i* SubsetMultiMapping void SubsetMultiMapping::addPoint( const core::BaseState* from, int index) { - // find the index of the parent state unsigned i; - for(i=0; ifromModels.size(); i++) - if(this->fromModels.get(i)==from ) + for (i = 0; i < this->fromModels.size(); i++) + { + if (this->fromModels.get(i) == from) + { break; - if(i==this->fromModels.size()) + } + } + if (i == this->fromModels.size()) { - msg_error() << "SubsetMultiMapping::addPoint, parent " << from->getName() << " not found !"; + msg_error() << "addPoint, parent " << from->getName() << " not found !"; assert(0); } @@ -119,56 +128,52 @@ template void SubsetMultiMapping::addPoint( int from, int index) { assert((size_t)from < this->fromModels.size()); - type::vector& indexPairsVector = *d_indexPairs.beginEdit(); + auto indexPairsVector = sofa::helper::getWriteAccessor(d_indexPairs); indexPairsVector.push_back(from); indexPairsVector.push_back(index); - d_indexPairs.endEdit(); } - -template -void SubsetMultiMapping::apply(const core::MechanicalParams* mparams, const type::vector& dataVecOutPos, const type::vector& dataVecInPos) +template +void apply_impl( + const type::vector& dataVecOut, + const type::vector& dataVecIn, + const type::vector& indexPairs) { - SOFA_UNUSED(mparams); - - OutVecCoord& out = *(dataVecOutPos[0]->beginEdit()); + auto out = sofa::helper::getWriteOnlyAccessor(*dataVecOut[0]); - for(unsigned i=0; iendEdit(); + const DataVecIn* inPosPtr = dataVecIn[stateId]; + const typename DataVecIn::value_type& inPos = inPosPtr->getValue(); + core::eq( out[i], inPos[coordId] ); + } } template -void SubsetMultiMapping::applyJ(const core::MechanicalParams* mparams, const type::vector& dataVecOutVel, const type::vector& dataVecInVel) +void SubsetMultiMapping::apply(const core::MechanicalParams* mparams, const type::vector*>& dataVecOutPos, const type::vector*>& dataVecInPos) { SOFA_UNUSED(mparams); + apply_impl( dataVecOutPos, dataVecInPos, d_indexPairs.getValue()); +} - OutVecDeriv& out = *(dataVecOutVel[0]->beginEdit()); - - for(unsigned i=0; iendEdit(); +template +void SubsetMultiMapping::applyJ(const core::MechanicalParams* mparams, const type::vector*>& dataVecOutVel, const type::vector*>& dataVecInVel) +{ + SOFA_UNUSED(mparams); + apply_impl( dataVecOutVel, dataVecInVel, d_indexPairs.getValue()); } template -void SubsetMultiMapping::applyJT( const core::ConstraintParams* /*cparams*/, const type::vector< InDataMatrixDeriv* >& dOut, const type::vector< const OutDataMatrixDeriv* >& dIn) +void SubsetMultiMapping::applyJT( const core::ConstraintParams* /*cparams*/, const type::vector< DataMatrixDeriv_t* >& dOut, const type::vector< const DataMatrixDeriv_t* >& dIn) { type::vector indexP = d_indexPairs.getValue(); // hypothesis: one child only: - const OutMatrixDeriv& in = dIn[0]->getValue(); + const MatrixDeriv_t& in = dIn[0]->getValue(); if (dOut.size() != this->fromModels.size()) { @@ -176,13 +181,13 @@ void SubsetMultiMapping::applyJT( const core::ConstraintParams* /*cpa return; } - typename OutMatrixDeriv::RowConstIterator rowItEnd = in.end(); + typename MatrixDeriv_t::RowConstIterator rowItEnd = in.end(); // loop on the constraints defined on the child of the mapping - for (typename OutMatrixDeriv::RowConstIterator rowIt = in.begin(); rowIt != rowItEnd; ++rowIt) + for (typename MatrixDeriv_t::RowConstIterator rowIt = in.begin(); rowIt != rowItEnd; ++rowIt) { - typename OutMatrixDeriv::ColConstIterator colIt = rowIt.begin(); - typename OutMatrixDeriv::ColConstIterator colItEnd = rowIt.end(); + typename MatrixDeriv_t::ColConstIterator colIt = rowIt.begin(); + typename MatrixDeriv_t::ColConstIterator colItEnd = rowIt.end(); // A constraint can be shared by several nodes, @@ -194,38 +199,40 @@ void SubsetMultiMapping::applyJT( const core::ConstraintParams* /*cpa { unsigned int index_parent = indexP[colIt.index() * 2]; // 0 or 1 (for now...) // writeLine provide an iterator on the line... if this line does not exist, the line is created: - typename InMatrixDeriv::RowIterator o = dOut[index_parent]->beginEdit()->writeLine(rowIt.index()); + typename MatrixDeriv_t::RowIterator o = dOut[index_parent]->beginEdit()->writeLine(rowIt.index()); dOut[index_parent]->endEdit(); // for each col of the constraint direction, it adds a col in the corresponding parent's constraint direction if(d_indexPairs.getValue()[colIt.index() * 2 + 1] < (unsigned int)this->fromModels[index_parent]->getSize()) { - InDeriv tmp; core::eq( tmp, colIt.val() ); + Deriv_t tmp; + core::eq( tmp, colIt.val() ); o.addCol(indexP[colIt.index() * 2 + 1], tmp); } ++colIt; } - - - } } template -void SubsetMultiMapping::applyJT(const core::MechanicalParams* mparams, const type::vector& dataVecOutForce, const type::vector& dataVecInForce) +void SubsetMultiMapping::applyJT(const core::MechanicalParams* mparams, const type::vector*>& dataVecOutForce, const type::vector*>& dataVecInForce) { SOFA_UNUSED(mparams); - const OutDataVecDeriv* cderData = dataVecInForce[0]; - const OutVecDeriv& cder = cderData->getValue(); + const DataVecDeriv_t* cderData = dataVecInForce[0]; + const VecDeriv_t& cder = cderData->getValue(); + + const auto& indexPairsValue = d_indexPairs.getValue(); - for(unsigned i=0; i* inDerivPtr = dataVecOutForce[stateId]; + auto inDeriv = sofa::helper::getWriteAccessor(*inDerivPtr); + core::peq(inDeriv[coordId], cder[i] ); } } diff --git a/Sofa/Component/Mapping/Linear/tests/SubsetMultiMapping_test.cpp b/Sofa/Component/Mapping/Linear/tests/SubsetMultiMapping_test.cpp index b70504770a6..b9fec71addd 100644 --- a/Sofa/Component/Mapping/Linear/tests/SubsetMultiMapping_test.cpp +++ b/Sofa/Component/Mapping/Linear/tests/SubsetMultiMapping_test.cpp @@ -21,75 +21,39 @@ ******************************************************************************/ #include #include -//#include +#include #include #include namespace sofa { -namespace { -using namespace core; -using namespace component; -using type::Vec; -using type::Mat; -using type::vector; +using component::mapping::linear::SubsetMultiMapping; - -/** Test suite for SubsetMultiMapping. - */ +/** + * Test suite for SubsetMultiMapping. + */ template struct SubsetMultiMappingTest : public MultiMapping_test<_SubsetMultiMapping> { + using In = typename _SubsetMultiMapping::In; + using Out = typename _SubsetMultiMapping::Out; - typedef _SubsetMultiMapping SubsetMultiMapping; - - typedef typename SubsetMultiMapping::In InDataTypes; - typedef typename InDataTypes::VecCoord InVecCoord; - typedef typename InDataTypes::VecDeriv InVecDeriv; - typedef typename InDataTypes::Coord InCoord; - typedef typename InDataTypes::Deriv InDeriv; - typedef statecontainer::MechanicalObject InMechanicalObject; - typedef typename InMechanicalObject::ReadVecCoord ReadInVecCoord; - typedef typename InMechanicalObject::WriteVecCoord WriteInVecCoord; - typedef typename InMechanicalObject::WriteVecDeriv WriteInVecDeriv; - typedef typename InDataTypes::Real InReal; - typedef Mat RotationMatrix; - - - typedef typename SubsetMultiMapping::Out OutDataTypes; - typedef typename OutDataTypes::VecCoord OutVecCoord; - typedef typename OutDataTypes::VecDeriv OutVecDeriv; - typedef typename OutDataTypes::Coord OutCoord; - typedef typename OutDataTypes::Deriv OutDeriv; - typedef statecontainer::MechanicalObject OutMechanicalObject; - typedef typename OutMechanicalObject::WriteVecCoord WriteOutVecCoord; - typedef typename OutMechanicalObject::WriteVecDeriv WriteOutVecDeriv; - typedef typename OutMechanicalObject::ReadVecCoord ReadOutVecCoord; - typedef typename OutMechanicalObject::ReadVecDeriv ReadOutVecDeriv; - - void SetUp() override - { - } - - /** @name Test_Cases - For each of these cases, we can test if the mapping work - */ - ///@{ - /** Two parent particles, two children - */ + /** + * Two parent particles, two children + */ bool test_two_parents_one_child() { - const int NP = 2; + static constexpr int NP = 2; this->setupScene(NP); // NP parents, 1 child - SubsetMultiMapping* smm = static_cast( this->mapping ); + _SubsetMultiMapping* smm = static_cast<_SubsetMultiMapping*>( this->mapping ); // parent positions - vector< InVecCoord > incoords(NP); - for( int i=0; i > incoords(NP); + for (int i = 0; i < NP; i++) { incoords[i].resize(1); - InDataTypes::set( incoords[i][0], i+1.,-2., 3. ); + In::set( incoords[i][0], i+1.,-2., 3. ); } // subset @@ -97,34 +61,28 @@ struct SubsetMultiMappingTest : public MultiMapping_test<_SubsetMultiMapping> smm->addPoint(smm->getMechFrom()[1],0); // parent, index in parent // expected child positions - OutVecCoord outcoords(2); - OutDataTypes::set( outcoords[0], 1. , -2., 3. ); - OutDataTypes::set( outcoords[1], 1+1., -2., 3. ); + VecCoord_t outcoords(2); + Out::set( outcoords[0], 1. , -2., 3. ); + Out::set( outcoords[1], 1+1., -2., 3. ); return this->runTest(incoords,outcoords); } - - - ///@} - - }; - -// Define the list of types to instantiate. We do not necessarily need to test all combinations. using ::testing::Types; typedef Types< -mapping::linear::SubsetMultiMapping, -mapping::linear::SubsetMultiMapping -> DataTypes; // the types to instantiate. + SubsetMultiMapping, + SubsetMultiMapping, + SubsetMultiMapping, + SubsetMultiMapping, + SubsetMultiMapping +> DataTypes; -// Test suite for all the instantiations TYPED_TEST_SUITE(SubsetMultiMappingTest, DataTypes); -// first test case + TYPED_TEST( SubsetMultiMappingTest , two_parents_one_child ) { ASSERT_TRUE(this->test_two_parents_one_child()); } -} // namespace } // namespace sofa diff --git a/Sofa/Component/Mapping/NonLinear/tests/RigidMapping_test.cpp b/Sofa/Component/Mapping/NonLinear/tests/RigidMapping_test.cpp index 46a53cb53ef..630add17ad0 100644 --- a/Sofa/Component/Mapping/NonLinear/tests/RigidMapping_test.cpp +++ b/Sofa/Component/Mapping/NonLinear/tests/RigidMapping_test.cpp @@ -90,7 +90,7 @@ struct RigidMappingTest : public sofa::mapping_test::Mapping_test<_RigidMapping> if constexpr ( InDataTypes::spatial_dimensions != 3 ) { // RigidMapping::getK is not yet implemented for 2D rigids - this->flags &= ~Inherit::TEST_getK; + this->setTestExecution(Inherit::TEST_getK, false); } } diff --git a/Sofa/Component/Mapping/Testing/src/sofa/component/mapping/testing/MappingTestCreation.h b/Sofa/Component/Mapping/Testing/src/sofa/component/mapping/testing/MappingTestCreation.h index 3a430a8deed..075524fe651 100644 --- a/Sofa/Component/Mapping/Testing/src/sofa/component/mapping/testing/MappingTestCreation.h +++ b/Sofa/Component/Mapping/Testing/src/sofa/component/mapping/testing/MappingTestCreation.h @@ -22,6 +22,7 @@ #pragma once #include +#include #include using sofa::testing::BaseSimulationTest; @@ -30,7 +31,6 @@ using sofa::testing::NumericTest; #include -#include #include #include #include @@ -38,16 +38,17 @@ using sofa::testing::NumericTest; #include -namespace sofa::mapping_test { +namespace sofa::mapping_test +{ /** @brief Base class for the Mapping tests, with helpers to automatically test applyJ, applyJT, applyDJT and getJs using finite differences. Specific test cases can be created using a derived class instantiated on the mapping class to test, - and calling function runTest( const InVecCoord& parentInit, - const OutVecCoord& childInit, - const InVecCoord parentNew, - const OutVecCoord expectedChildNew); + and calling function runTest( const VecCoord_t& parentInit, + const VecCoord_t& childInit, + const VecCoord_t parentNew, + const VecCoord_t expectedChildNew); This function compares the actual output positions with the expected ones, then automatically tests the methods related to @@ -65,41 +66,16 @@ namespace sofa::mapping_test { @author François Faure @date 2013 */ - template< class _Mapping> struct Mapping_test: public BaseSimulationTest, NumericTest { - typedef _Mapping Mapping; - typedef typename Mapping::In In; - typedef component::statecontainer::MechanicalObject InDOFs; - typedef typename InDOFs::Real Real; - typedef typename InDOFs::Coord InCoord; - typedef typename InDOFs::Deriv InDeriv; - typedef typename InDOFs::VecCoord InVecCoord; - typedef typename InDOFs::VecDeriv InVecDeriv; - typedef typename InDOFs::ReadVecCoord ReadInVecCoord; - typedef typename InDOFs::WriteVecCoord WriteInVecCoord; - typedef typename InDOFs::ReadVecDeriv ReadInVecDeriv; - typedef typename InDOFs::WriteVecDeriv WriteInVecDeriv; - typedef typename InDOFs::MatrixDeriv InMatrixDeriv; - typedef Data InDataVecCoord; - typedef Data InDataVecDeriv; - typedef Data InDataMatrixDeriv; - - typedef typename Mapping::Out Out; - typedef component::statecontainer::MechanicalObject OutDOFs; - typedef typename OutDOFs::Coord OutCoord; - typedef typename OutDOFs::Deriv OutDeriv; - typedef typename OutDOFs::VecCoord OutVecCoord; - typedef typename OutDOFs::VecDeriv OutVecDeriv; - typedef typename OutDOFs::ReadVecCoord ReadOutVecCoord; - typedef typename OutDOFs::WriteVecCoord WriteOutVecCoord; - typedef typename OutDOFs::ReadVecDeriv ReadOutVecDeriv; - typedef typename OutDOFs::WriteVecDeriv WriteOutVecDeriv; - typedef typename OutDOFs::MatrixDeriv OutMatrixDeriv; - typedef Data OutDataVecCoord; - typedef Data OutDataVecDeriv; - typedef Data OutDataMatrixDeriv; + using Mapping = _Mapping; + + using In = typename Mapping::In; + using InDOFs = component::statecontainer::MechanicalObject; + + using Out = typename Mapping::Out; + using OutDOFs = component::statecontainer::MechanicalObject; typedef linearalgebra::EigenSparseMatrix EigenSparseMatrix; @@ -107,10 +83,9 @@ struct Mapping_test: public BaseSimulationTest, NumericTest deltaRange; ///< The minimum and maximum magnitudes of the change of each scalar value of the small displacement is perturbation * numeric_limits::epsilon. This epsilon is 1.19209e-07 for float and 2.22045e-16 for double. - Real errorMax; ///< The test is successful if the (infinite norm of the) difference is less than errorMax * numeric_limits::epsilon - Real errorFactorDJ; ///< The test for geometric stiffness is successful if the (infinite norm of the) difference is less than errorFactorDJ * errorMax * numeric_limits::epsilon + std::pair, Real_t> deltaRange; ///< The minimum and maximum magnitudes of the change of each scalar value of the small displacement is perturbation * numeric_limits::epsilon. This epsilon is 1.19209e-07 for float and 2.22045e-16 for double. + Real_t errorMax; ///< The test is successful if the (infinite norm of the) difference is less than errorMax * numeric_limits::epsilon + Real_t errorFactorDJ; ///< The test for geometric stiffness is successful if the (infinite norm of the) difference is less than errorFactorDJ * errorMax * numeric_limits::epsilon static constexpr unsigned char TEST_getJs = 1; ///< testing getJs used in assembly API @@ -122,6 +97,25 @@ struct Mapping_test: public BaseSimulationTest, NumericTestcreateNewGraph("root"); + root = simpleapi::createRootNode(simulation::getSimulation(), "root"); inDofs = core::objectmodel::New(); root->addObject(inDofs); @@ -149,16 +141,15 @@ struct Mapping_test: public BaseSimulationTest, NumericTestcreateNewGraph("root"); + root = simpleapi::createRootNode(simulation::getSimulation(), "root"); root = sofa::simulation::node::load(fileName.c_str(), false); // InDofs @@ -180,34 +171,8 @@ struct Mapping_test: public BaseSimulationTest, NumericTest& parentInit, + const VecCoord_t& childInit, + const VecCoord_t& parentNew, + const VecCoord_t& expectedChildNew) { - if( deltaRange.second / errorMax <= sofa::testing::g_minDeltaErrorRatio ) - ADD_FAILURE() << "The comparison threshold is too large for the finite difference delta"; - - if( !(flags & TEST_getJs) ) msg_warning("MappingTest") << "getJs is not tested"; - if( !(flags & TEST_getK) ) msg_warning("MappingTest") << "getK is not tested"; - if( !(flags & TEST_applyJT_matrix) ) msg_warning("MappingTest") << "applyJT on matrices is not tested"; - if( !(flags & TEST_applyDJT) ) msg_warning("MappingTest") << "applyDJT is not tested"; + checkComparisonThreshold(); + warnMissingTests(); + const auto errorThreshold = this->epsilon() * errorMax; - const Real errorThreshold = this->epsilon()*errorMax; + using EigenSparseMatrix = linearalgebra::EigenSparseMatrix; - typedef linearalgebra::EigenSparseMatrix EigenSparseMatrix; core::MechanicalParams mparams; mparams.setKFactor(1.0); mparams.setSupportOnlySymmetricMatrix(false); - inDofs->resize(parentInit.size()); - WriteInVecCoord xin = inDofs->writePositions(); - sofa::testing::copyToData(xin,parentInit); // xin = parentInit - - outDofs->resize(childInit.size()); - WriteOutVecCoord xout = outDofs->writePositions(); - sofa::testing::copyToData(xout,childInit); - - /// Init based on parentInit - sofa::simulation::node::initRoot(root.get()); - - /// Updated to parentNew - sofa::testing::copyToData(xin,parentNew); - mapping->apply(&mparams, core::VecCoordId::position(), core::VecCoordId::position()); - mapping->applyJ(&mparams, core::VecDerivId::velocity(), core::VecDerivId::velocity()); /// test apply: check if the child positions are the expected ones - bool succeed=true; + bool succeed = testMappingPositionVelocity( + parentInit, childInit, parentNew, expectedChildNew, errorThreshold, + mparams); - if (expectedChildNew.size() != xout.size()) { - ADD_FAILURE() << "Size of output dofs is wrong: " << xout.size() << " expected: " << expectedChildNew.size(); - succeed = false; - } - for( unsigned i=0; iisSmall( difference(xout[i],expectedChildNew[i]).norm(), errorMax ) ) { - ADD_FAILURE() << "Position of mapped particle " << i << " is wrong: \n" << xout[i] <<"\nexpected: \n" << expectedChildNew[i] - << "\ndifference should be less than " << errorThreshold << " (" << difference(xout[i],expectedChildNew[i]).norm() << ")" << std::endl; - succeed = false; - } - } - - /// test applyJ and everything related to Jacobians - size_t Np = inDofs->getSize(), Nc=outDofs->getSize(); - - InVecCoord xp(Np),xp1(Np); - InVecDeriv vp(Np),fp(Np); - InVecDeriv dfp_a(Np); - InVecDeriv dfp_b(Np); - InVecDeriv fp2(Np); - OutVecCoord xc(Nc),xc1(Nc); - OutVecDeriv vc(Nc),fc(Nc); + const std::size_t sizeIn = inDofs->getSize(); + const std::size_t sizeOut = outDofs->getSize(); // get position data - sofa::testing::copyFromData( xp, inDofs->readPositions() ); - sofa::testing::copyFromData( xc, outDofs->readPositions() ); // positions and have already been propagated + VecCoord_t positionOut = outDofs->readPositions().ref(); // set random child forces and propagate them to the parent - for( unsigned i=0; iwriteForces(); - sofa::testing::copyToData( fin, fp2 ); // reset parent forces before accumulating child forces + VecDeriv_t forceOut = generateRandomVecDeriv(sizeOut, 0.1, 1.); - WriteOutVecDeriv fout = outDofs->writeForces(); - sofa::testing::copyToData( fout, fc ); - mapping->applyJT( &mparams, core::VecDerivId::force(), core::VecDerivId::force() ); - sofa::testing::copyFromData( fp, inDofs->readForces() ); + VecDeriv_t forceIn; + computeForceInFromForceOut(mparams, forceIn, forceOut); // set small parent velocities and use them to update the child - for( unsigned i=0; iepsilon() * deltaRange.first, this->epsilon() * deltaRange.second ); - } + const VecDeriv_t velocityIn = generateRandomVecDeriv(sizeIn, + this->epsilon() * deltaRange.first, + this->epsilon() * deltaRange.second); - for( unsigned i=0; i perturbedPositionIn = computePerturbedPositions(sizeIn, velocityIn); - // propagate small velocity - WriteInVecDeriv vin = inDofs->writeVelocities(); - sofa::testing::copyToData( vin, vp ); - mapping->applyJ( &mparams, core::VecDerivId::velocity(), core::VecDerivId::velocity() ); - ReadOutVecDeriv vout = outDofs->readVelocities(); - sofa::testing::copyFromData( vc, vout); + VecDeriv_t velocityOut; + computeVelocityOutFromVelocityIn(mparams, velocityOut, velocityIn); // apply geometric stiffness inDofs->vRealloc( &mparams, core::VecDerivId::dx() ); // dx is not allocated by default - WriteInVecDeriv dxin = inDofs->writeDx(); - sofa::testing::copyToData( dxin, vp ); - dfp_a.fill( InDeriv() ); - dfp_b.fill( InDeriv() ); - sofa::testing::copyToData( fin, dfp_a ); - sofa::testing::copyToData( fin, dfp_b ); - - //calling applyDJT without updateK before - mapping->applyDJT( &mparams, core::VecDerivId::force(), core::VecDerivId::force() ); - sofa::testing::copyFromData( dfp_a, inDofs->readForces() ); // fp + df due to geometric stiffness - sofa::testing::copyToData( fin, fp2 ); //reset forces + inDofs->writeDx().wref() = velocityIn; - //calling applyDJT after updateK - mapping->updateK( &mparams, core::ConstVecDerivId::force() ); // updating stiffness matrix for the current state and force - mapping->applyDJT( &mparams, core::VecDerivId::force(), core::VecDerivId::force() ); - sofa::testing::copyFromData( dfp_b, inDofs->readForces() ); // fp + df due to geometric stiffness + const VecDeriv_t dfp_withoutUpdateK = applyDJT(mparams, false); + const VecDeriv_t dfp_withUpdateK = applyDJT(mparams, true); // Jacobian will be obsolete after applying new positions - if( flags & TEST_getJs ) + if( isTestExecuted(TEST_getJs) ) { EigenSparseMatrix* J = this->getMatrix(mapping->getJs()); - // cout<<"J = "<< endl << *J << endl; - OutVecDeriv Jv(Nc); - J->mult(Jv,vp); - - // ================ test applyJT() - InVecDeriv jfc( (long)Np,InDeriv()); - J->addMultTranspose(jfc,fc); - if( this->vectorMaxDiff(jfc,fp)>errorThreshold ){ - succeed = false; - ADD_FAILURE() << "applyJT test failed, difference should be less than " << errorThreshold << " (" << this->vectorMaxDiff(jfc,fp) << ")" << std::endl - << "jfc = " << jfc << std::endl<<" fp = " << fp << std::endl; - } - // ================ test getJs() - // check that J.vp = vc - if(this->vectorMaxDiff(Jv,vc)>errorThreshold ){ - succeed = false; - std::cout<<"vp = " << vp << std::endl; - std::cout<<"Jvp = " << Jv << std::endl; - std::cout<<"vc = " << vc << std::endl; - ADD_FAILURE() << "getJs() test failed"<vectorMaxAbs(difference(dxc,vc))<<" should be less than " << errorThreshold << std::endl - << "position change = " << dxc << std::endl - << "velocity = " << vc << std::endl; + if( isTestExecuted(TEST_getK)) + { + succeed &= testGetK(sizeIn, velocityIn, forceChange, errorThreshold); } + if( isTestExecuted(TEST_buildGeometricStiffnessMatrix) ) + { + succeed &= testBuildGeometricStiffnessMatrix(sizeIn, velocityIn, forceChange, errorThreshold); + } + if(!succeed) + { + ADD_FAILURE() << "Failed Seed number = " << this->seed << std::endl; + } + return succeed; + } - // update parent force based on the same child forces - fp2.fill( InDeriv() ); - sofa::testing::copyToData( fin, fp2 ); // reset parent forces before accumulating child forces - sofa::testing::copyToData( fout, preTreatment(fc) ); - mapping->applyJT( &mparams, core::VecDerivId::force(), core::VecDerivId::force() ); - sofa::testing::copyFromData( fp2, inDofs->readForces() ); - InVecDeriv fp12(Np); - for(unsigned i=0; i& parent, + const VecCoord_t expectedChild) + { + VecCoord_t childInit( expectedChild.size() ); // start with null child + return runTest( parent, childInit, parent, expectedChild ); + } + + ~Mapping_test() override + { + if (root != nullptr) + { + sofa::simulation::node::unload(root); } + } + +protected: + - // ================ test applyDJT() - if( flags & TEST_applyDJT ) + /** Returns OutCoord substraction a-b */ + virtual Deriv_t difference( const Coord_t& a, const Coord_t& b ) + { + return Out::coordDifference(a,b); + } + + virtual VecDeriv_t difference( const VecDeriv_t& a, const VecDeriv_t& b ) + { + if (a.size() != b.size()) { - //we test the function applyDJT in two different contexts because - //the implementation usually depends on the context - if (this->vectorMaxDiff(dfp_a,fp12) > errorThreshold * errorFactorDJ) - { - succeed = false; - ADD_FAILURE() << "applyDJT (no call to updateK) test failed" << std::endl - << "dfp = " << dfp_a << std::endl - << "fp2-fp = " << fp12 << std::endl - << "error threshold = " << errorThreshold * errorFactorDJ << std::endl; - } - if (this->vectorMaxDiff(dfp_b, fp12) > errorThreshold * errorFactorDJ) - { - succeed = false; - ADD_FAILURE() << "applyDJT (after call to updateK) test failed" << std::endl - << "dfp = " << dfp_b << std::endl - << "fp2-fp = " << fp12 << std::endl - << "error threshold = " << errorThreshold * errorFactorDJ << std::endl; - } + ADD_FAILURE() << "VecDeriv_t have different sizes"; + return {}; } + VecDeriv_t c; + c.reserve(a.size()); + std::transform(a.begin(), a.end(), b.begin(), std::back_inserter(c), std::minus()); - // ================ test getK() - if( flags & TEST_getK ) + return c; + } + + /** Possible child force pre-treatment, does nothing by default + */ + virtual VecDeriv_t preTreatment( const VecDeriv_t& f ) + { + return f; + } + + + void checkComparisonThreshold() + { + if( deltaRange.second / errorMax <= sofa::testing::g_minDeltaErrorRatio ) { - InVecDeriv Kv(Np); + ADD_FAILURE() << "The comparison threshold is too large for the finite difference delta"; + } + } - const linearalgebra::BaseMatrix* bk = mapping->getK(); + void warnMissingTests() const + { + msg_warning_when(!isTestExecuted(TEST_getJs), "MappingTest") << "getJs is not tested"; + msg_warning_when(!isTestExecuted(TEST_getK) , "MappingTest") << "getK is not tested"; + msg_warning_when(!isTestExecuted(TEST_applyJT_matrix) , "MappingTest") << "applyJT on matrices is not tested"; + msg_warning_when(!isTestExecuted(TEST_applyDJT) , "MappingTest") << "applyDJT is not tested"; + msg_warning_when(!isTestExecuted(TEST_buildGeometricStiffnessMatrix) , "MappingTest") << "buildGeometricStiffnessMatrix is not tested"; + } - // K can be null or empty for linear mappings - // still performing the test with a null Kv vector to check if the mapping is really linear + bool testMappingPositionVelocity(const VecCoord_t& parentInit, + const VecCoord_t& childInit, + const VecCoord_t& parentNew, + const VecCoord_t& expectedChildNew, + const Real_t errorThreshold, + core::MechanicalParams mparams) + { + helper::WriteAccessor positionAccessorIn = inDofs->writePositions(); + positionAccessorIn.wref() = parentInit; - if( bk != nullptr ){ + helper::WriteAccessor positionAccessorOut = outDofs->writePositions(); + positionAccessorOut.wref() = childInit; - typedef linearalgebra::EigenSparseMatrix EigenSparseKMatrix; - const EigenSparseKMatrix* K = dynamic_cast(bk); - if( K == nullptr ){ - succeed = false; - ADD_FAILURE() << "getK returns a matrix of non-EigenSparseMatrix type"; - // TODO perform a slow conversion with a big warning rather than a failure? - } + /// Init based on parentInit + sofa::simulation::node::initRoot(root.get()); - if( K->compressedMatrix.nonZeros() ) K->mult(Kv,vp); - } + /// Updated to parentNew + positionAccessorIn.wref() = parentNew; + mapping->apply(&mparams, core::VecCoordId::position(), core::VecCoordId::position()); + mapping->applyJ(&mparams, core::VecDerivId::velocity(), core::VecDerivId::velocity()); - // check that K.vp = dfp - if(this->vectorMaxDiff(Kv,fp12)>errorThreshold*errorFactorDJ ){ - succeed = false; - ADD_FAILURE() << "K test failed, difference should be less than " << errorThreshold*errorFactorDJ << std::endl - << "Kv = " << Kv << std::endl - << "dfp = " << fp12 << std::endl; - } - } + bool succeed = true; - if( flags & TEST_buildGeometricStiffnessMatrix ) + if (expectedChildNew.size() != positionAccessorOut.size()) { - core::GeometricStiffnessMatrix testGeometricStiffness; - - struct GeometricStiffnessAccumulator : core::MappingMatrixAccumulator - { - void add(sofa::SignedIndex row, sofa::SignedIndex col, float value) override - { - assembledMatrix.add(row, col, value); - } - void add(sofa::SignedIndex row, sofa::SignedIndex col, double value) override - { - assembledMatrix.add(row, col, value); - } - - linearalgebra::EigenSparseMatrix assembledMatrix; - } accumulator; - - testGeometricStiffness.setMatrixAccumulator(&accumulator, mapping->getFromModel(), mapping->getFromModel()); - - accumulator.assembledMatrix.resize(mapping->getFromModel()->getSize() * In::deriv_total_size, mapping->getFromModel()->getSize() * In::deriv_total_size); - mapping->buildGeometricStiffnessMatrix(&testGeometricStiffness); - accumulator.assembledMatrix.compress(); - - InVecDeriv Kv(Np); - if( accumulator.assembledMatrix.compressedMatrix.nonZeros() ) - { - accumulator.assembledMatrix.mult(Kv,vp); - } + ADD_FAILURE() << "Size of output dofs is wrong: " << positionAccessorOut.size() << " expected: " << expectedChildNew.size(); + succeed = false; + } - // check that K.vp = dfp - if (this->vectorMaxDiff(Kv,fp12) > errorThreshold*errorFactorDJ ) + for (unsigned i = 0; i < positionAccessorOut.size(); ++i) + { + if (!this->isSmall(difference(positionAccessorOut[i], expectedChildNew[i]).norm(), errorMax)) { + ADD_FAILURE() << "Position of mapped particle " << i << " is wrong: \n" << positionAccessorOut[i] <<"\nexpected: \n" << expectedChildNew[i] + << "\ndifference should be less than " << errorThreshold << " (" << difference(positionAccessorOut[i],expectedChildNew[i]).norm() << ")" << std::endl; succeed = false; - ADD_FAILURE() << "buildGeometricStiffnessMatrix test failed, difference should be less than " << errorThreshold*errorFactorDJ << std::endl - << "Kv = " << Kv << std::endl - << "dfp = " << fp12 << std::endl; } } - if(!succeed) - { ADD_FAILURE() << "Failed Seed number = " << this->seed << std::endl;} return succeed; } + template + VecDeriv_t generateRandomVecDeriv(const std::size_t size, const Real_t minMagnitude, const Real_t maxMagnitude) + { + VecDeriv_t randomForce; + randomForce.reserve(size); + for (std::size_t i = 0; i < size; i++) + { + randomForce.push_back(DataTypes::randomDeriv(minMagnitude, maxMagnitude)); + } + return randomForce; + } - /** Test the mapping using the given values and small changes. - * Return true in case of success, if all errors are below maxError*epsilon. - * The mapping is initialized using the first parameter, - * @warning this version supposes the mapping initialization does not depend on child positions - * otherwise, use the runTest functions with 4 parameters - * the child position is computed from parent position and compared with the expected one. - * Additionally, the Jacobian-related methods are tested using finite differences. - * - *\param parent parent position - *\param expectedChild expected position of the child corresponding to the parent position - */ - virtual bool runTest( const InVecCoord& parent, - const OutVecCoord expectedChild) + void computeForceInFromForceOut(core::MechanicalParams mparams, VecDeriv_t& forceIn, const VecDeriv_t& forceOut) { - OutVecCoord childInit( expectedChild.size() ); // start with null child - return runTest( parent, childInit, parent, expectedChild ); + inDofs->writeForces()->fill(Deriv_t()); // reset parent forces before accumulating child forces + + outDofs->writeForces().wref() = forceOut; + mapping->applyJT( &mparams, core::VecDerivId::force(), core::VecDerivId::force() ); + forceIn = inDofs->readForces().ref(); } - ~Mapping_test() override + void computeVelocityOutFromVelocityIn(core::MechanicalParams mparams, VecDeriv_t& velocityOut, const VecDeriv_t& velocityIn) { - if (root!=nullptr) - sofa::simulation::node::unload(root); + inDofs->writeVelocities().wref() = velocityIn; + mapping->applyJ( &mparams, core::VecDerivId::velocity(), core::VecDerivId::velocity() ); + velocityOut = outDofs->readVelocities().ref(); + } + + const VecDeriv_t& applyDJT(core::MechanicalParams mparams, bool updateK) + { + inDofs->writeForces()->fill(Deriv_t()); //reset force + + if (updateK) + { + mapping->updateK( &mparams, core::ConstVecDerivId::force() ); // updating stiffness matrix for the current state and force + } + mapping->applyDJT( &mparams, core::VecDerivId::force(), core::VecDerivId::force() ); + return inDofs->readForces().ref(); + } + + [[nodiscard]] bool checkApplyDJT(const VecDeriv_t& dfp, const VecDeriv_t& fp12, Real_t errorThreshold, bool updateK) + { + if (this->vectorMaxDiff(dfp, fp12) > errorThreshold * errorFactorDJ) + { + const std::string updateKString = updateK ? "after call to updateK" : "no call to updateK"; + ADD_FAILURE() << "applyDJT (" << updateKString << ") test failed" << std::endl + << "dfp = " << dfp << std::endl + << "fp2-fp = " << fp12 << std::endl + << "error threshold = " << errorThreshold * errorFactorDJ << std::endl; + return false; + } + return true; + } + + VecCoord_t computePerturbedPositions(const std::size_t sizeIn, const VecDeriv_t velocityIn) + { + VecCoord_t perturbedPositionIn; + perturbedPositionIn.reserve(sizeIn); + const helper::ReadAccessor positionIn = inDofs->readPositions(); + for (std::size_t i = 0; i < sizeIn; ++i) + { + perturbedPositionIn.push_back(positionIn[i] + velocityIn[i]); + } + return perturbedPositionIn; } -protected: /// Get one EigenSparseMatrix out of a list. Error if not one single matrix in the list. template static EigenSparseMatrixType* getMatrix(const type::vector* matrices) { - if( !matrices ){ + if( !matrices ) + { ADD_FAILURE()<< "Matrix list is nullptr (API for assembly is not implemented)"; } - if( matrices->size() != 1 ){ - ADD_FAILURE()<< "Matrix list should have size == 1 in simple mappings"; + + if( matrices->empty() ) + { + ADD_FAILURE()<< "Matrix list is empty"; + return nullptr; + } + + if( matrices->size() != 1 ) + { + ADD_FAILURE()<< "Matrix list should have size == 1 in simple mappings (current size = " << matrices->size() << ")"; } EigenSparseMatrixType* ei = dynamic_cast((*matrices)[0] ); - if( ei == nullptr ){ + if( ei == nullptr ) + { ADD_FAILURE() << "getJs returns a matrix of non-EigenSparseMatrix type"; // TODO perform a slow conversion with a big warning rather than a failure? } return ei; } + bool checkJacobianMatrixTranspose( + EigenSparseMatrix* jacobianMatrix, + const VecDeriv_t& forceOut, + const VecDeriv_t& expectedForceIn, + Real_t errorThreshold) + { + VecDeriv_t computedForceIn(expectedForceIn.size(), Deriv_t()); + + //computedForceIn += J^T * forceOut + jacobianMatrix->addMultTranspose(computedForceIn, forceOut); + const auto diff = this->vectorMaxDiff(computedForceIn, expectedForceIn); + if (diff > errorThreshold) + { + ADD_FAILURE() << + "getJs is not consistent with applyJT, difference should be " + "less than " << errorThreshold << " (" << diff << ")" << std::endl + << "computedForceIn = " << computedForceIn << std::endl + << "expectedForceIn = " << expectedForceIn << std::endl; + return false; + } + return true; + } + + bool checkJacobianMatrix( + EigenSparseMatrix* jacobianMatrix, + const VecDeriv_t& velocityIn, + const VecDeriv_t& expectedVelocityOut, + Real_t errorThreshold) + { + VecDeriv_t computedVelocityOut(expectedVelocityOut.size()); + + jacobianMatrix->mult(computedVelocityOut, velocityIn); + + const auto diff = this->vectorMaxDiff(computedVelocityOut, expectedVelocityOut); + if (diff > errorThreshold) + { + ADD_FAILURE() << + "getJs is not consistent with applyJ, difference should be " + "less than " << errorThreshold << " (" << diff << ")" << std::endl + << "velocityIn = " << velocityIn << std::endl + << "computedVelocityOut = " << computedVelocityOut << std::endl + << "expectedVelocityOut = " << expectedVelocityOut << std::endl; + return false; + } + + return true; + } + + bool testApplyJonPosition( + core::MechanicalParams mparams, + const VecCoord_t& positionOut, + const VecDeriv_t& expectedVelocityOut, + Real_t errorThreshold + ) + { + mapping->apply ( &mparams, core::VecCoordId::position(), core::VecCoordId::position() ); + const VecCoord_t& positionOut1 = outDofs->readPositions(); + + const auto sizeOut = positionOut.size(); + VecDeriv_t dxOut(sizeOut); + for (unsigned i = 0; i < sizeOut; i++) + { + dxOut[i] = difference(positionOut1[i], positionOut[i]); + } + + const auto diff = this->vectorMaxAbs(difference(dxOut, expectedVelocityOut)); + if (diff > errorThreshold) + { + ADD_FAILURE() << "applyJ test failed: the difference between child " + "position change and child velocity (dt=1) " << diff << + " should be less than " << errorThreshold << std::endl + << "position change = " << dxOut << std::endl + << "expectedVelocityOut = " << expectedVelocityOut << std::endl; + return false; + } + + return true; + } + + VecDeriv_t computeForceChange(core::MechanicalParams mparams, const std::size_t sizeIn, VecDeriv_t forceOut, VecDeriv_t forceIn) + { + VecDeriv_t forceChange; + forceChange.reserve(sizeIn); + + // apply has been called, therefore parent force must be updated + // based on the same child forces + VecDeriv_t forceIn2; + computeForceInFromForceOut(mparams, forceIn2, preTreatment(forceOut)); + + for (unsigned i = 0; i < sizeIn; ++i) + { + forceChange.push_back(forceIn2[i] - forceIn[i]); + } + + return forceChange; + } + + bool testGetK(const std::size_t& sizeIn, + const VecDeriv_t& velocityIn, + const VecDeriv_t& forceChange, + Real_t errorThreshold) + { + VecDeriv_t Kv(sizeIn); + + const linearalgebra::BaseMatrix* bk = mapping->getK(); + + // K can be null or empty for linear mappings + // still performing the test with a null Kv vector to check if the mapping is really linear + + if( bk != nullptr ) + { + typedef linearalgebra::EigenSparseMatrix EigenSparseKMatrix; + const EigenSparseKMatrix* K = dynamic_cast(bk); + if( K == nullptr ) + { + ADD_FAILURE() << "getK returns a matrix of non-EigenSparseMatrix type"; + // TODO perform a slow conversion with a big warning rather than a failure? + return false; + } + + if( K->compressedMatrix.nonZeros() ) + { + K->mult(Kv,velocityIn); + } + } + + // check that K.vp = dfp + if (this->vectorMaxDiff(Kv, forceChange) > errorThreshold * errorFactorDJ) + { + ADD_FAILURE() << "K test failed, difference should be less than " << errorThreshold*errorFactorDJ << std::endl + << "Kv = " << Kv << std::endl + << "dfp = " << forceChange << std::endl; + return false; + } + + return true; + } + + bool testBuildGeometricStiffnessMatrix( + std::size_t sizeIn, + const VecDeriv_t& velocityIn, + const VecDeriv_t& forceChange, + Real_t errorThreshold + ) + { + core::GeometricStiffnessMatrix testGeometricStiffness; + + struct GeometricStiffnessAccumulator : core::MappingMatrixAccumulator + { + void add(sofa::SignedIndex row, sofa::SignedIndex col, float value) override + { + assembledMatrix.add(row, col, value); + } + void add(sofa::SignedIndex row, sofa::SignedIndex col, double value) override + { + assembledMatrix.add(row, col, value); + } + + linearalgebra::EigenSparseMatrix assembledMatrix; + } accumulator; + + testGeometricStiffness.setMatrixAccumulator(&accumulator, mapping->getFromModel(), mapping->getFromModel()); + + accumulator.assembledMatrix.resize(mapping->getFromModel()->getSize() * In::deriv_total_size, mapping->getFromModel()->getSize() * In::deriv_total_size); + mapping->buildGeometricStiffnessMatrix(&testGeometricStiffness); + accumulator.assembledMatrix.compress(); + + VecDeriv_t Kv(sizeIn); + if( accumulator.assembledMatrix.compressedMatrix.nonZeros() ) + { + accumulator.assembledMatrix.mult(Kv,velocityIn); + } + + // check that K.vp = dfp + if (this->vectorMaxDiff(Kv,forceChange) > errorThreshold*errorFactorDJ ) + { + ADD_FAILURE() << "buildGeometricStiffnessMatrix test failed, difference should be less than " << errorThreshold*errorFactorDJ << std::endl + << "Kv = " << Kv << std::endl + << "dfp = " << forceChange << std::endl; + return false; + } + + return true; + } }; diff --git a/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/EulerImplicitSolver.cpp b/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/EulerImplicitSolver.cpp index e89ed41245e..fd44c92cb7c 100644 --- a/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/EulerImplicitSolver.cpp +++ b/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/EulerImplicitSolver.cpp @@ -145,8 +145,9 @@ void EulerImplicitSolver::solve(const core::ExecParams* params, SReal dt, sofa:: msg_info() << "EulerImplicitSolver, f = " << f; // add the change of force due to stiffness + Rayleigh damping - mop.addMBKv(b, -d_rayleighMass.getValue(), 0, - h + d_rayleighStiffness.getValue()); // b = f + ( rm M + (h+rs) K ) v + mop.addMBKv(b, core::MatricesFactors::M(-d_rayleighMass.getValue()), + core::MatricesFactors::B(0), + core::MatricesFactors::K(h + d_rayleighStiffness.getValue())); // b = f + ( rm M + (h+rs) K ) v // integration over a time step b.teq(h * @@ -162,19 +163,10 @@ void EulerImplicitSolver::solve(const core::ExecParams* params, SReal dt, sofa:: { SCOPED_TIMER("setSystemMBKMatrix"); - SReal mFact, kFact, bFact; - if (firstOrder) - { - mFact = 1; - bFact = 0; - kFact = -h * tr; - } - else - { - mFact = 1 + tr * h * d_rayleighMass.getValue(); - bFact = -tr * h; - kFact = -tr * h * (h + d_rayleighStiffness.getValue()); - } + const core::MatricesFactors::M mFact (firstOrder ? 1 : 1 + tr * h * d_rayleighMass.getValue()); + const core::MatricesFactors::B bFact (firstOrder ? 0 : -tr * h); + const core::MatricesFactors::K kFact (firstOrder ? -h * tr : -tr * h * (h + d_rayleighStiffness.getValue())); + mop.setSystemMBKMatrix(mFact, bFact, kFact, l_linearSolver.get()); #ifdef SOFA_DUMP_VISITOR_INFO diff --git a/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/NewmarkImplicitSolver.cpp b/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/NewmarkImplicitSolver.cpp index 34e6f35298c..cf35ceb506e 100644 --- a/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/NewmarkImplicitSolver.cpp +++ b/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/NewmarkImplicitSolver.cpp @@ -113,11 +113,15 @@ void NewmarkImplicitSolver::solve(const core::ExecParams* params, SReal dt, sofa mop.propagateDx(a); // b += (-h (1-\gamma)(r_M M + r_K K) - h^2/2 (1-2\beta) K ) a - mop.addMBKdx(b, -h*(1-gamma)*rM, h*(1-gamma), h*(1-gamma)*rK + h*h*(1-2*beta)/2.0,true,true); + mop.addMBKdx(b, + core::MatricesFactors::M(-h*(1-gamma)*rM), + core::MatricesFactors::B(h*(1-gamma)), + core::MatricesFactors::K(h*(1-gamma)*rK + h*h*(1-2*beta)/2.0), + true,true); } // b += -h K v - mop.addMBKv(b, -rM, 1,rK+h); + mop.addMBKv(b, core::MatricesFactors::M(-rM), core::MatricesFactors::B(1), core::MatricesFactors::K(rK+h)); msg_info() << "b = " << b; mop.projectResponse(b); // b is projected to the constrained space @@ -126,9 +130,9 @@ void NewmarkImplicitSolver::solve(const core::ExecParams* params, SReal dt, sofa // 3. Solve system of equations on a_{t+h} - const SReal mFact = 1 + h * gamma * rM; - const SReal bFact = (-h) * gamma; - const SReal kFact = -h * h * beta - h * rK * gamma; + const core::MatricesFactors::M mFact ( 1 + h * gamma * rM); + const core::MatricesFactors::B bFact ( (-h) * gamma); + const core::MatricesFactors::K kFact ( -h * h * beta - h * rK * gamma); mop.setSystemMBKMatrix(mFact, bFact, kFact, l_linearSolver.get()); l_linearSolver->setSystemLHVector(aResult); diff --git a/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/StaticSolver.cpp b/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/StaticSolver.cpp index 48d6750452f..61f3eba93a2 100644 --- a/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/StaticSolver.cpp +++ b/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/StaticSolver.cpp @@ -45,29 +45,29 @@ StaticSolver::StaticSolver() : d_newton_iterations(initData(&d_newton_iterations, (unsigned) 1, "newton_iterations", - "Number of Netwon iterations between each load increments (normally, one load increment per simulation time-step.")) + "Number of Newton iterations between each load increments (normally, one load increment per simulation time-step.")) , d_absolute_correction_tolerance_threshold(initData(&d_absolute_correction_tolerance_threshold, 1e-5_sreal, "absolute_correction_tolerance_threshold", - "Convergence criterion of the norm |du| under which the Netwon iterations stop")) + "Convergence criterion of the norm |du| under which the Newton iterations stop")) , d_relative_correction_tolerance_threshold(initData(&d_relative_correction_tolerance_threshold, 1e-5_sreal, "relative_correction_tolerance_threshold", - "Convergence criterion regarding the ratio |du| / |U| under which the Netwon iterations stop")) + "Convergence criterion regarding the ratio |du| / |U| under which the Newton iterations stop")) , d_absolute_residual_tolerance_threshold( initData(&d_absolute_residual_tolerance_threshold, 1e-5_sreal, "absolute_residual_tolerance_threshold", - "Convergence criterion of the norm |R| under which the Netwon iterations stop." + "Convergence criterion of the norm |R| under which the Newton iterations stop." "Use a negative value to disable this criterion")) , d_relative_residual_tolerance_threshold( initData(&d_relative_residual_tolerance_threshold, 1e-5_sreal, "relative_residual_tolerance_threshold", - "Convergence criterion regarding the ratio |R|/|R0| under which the Netwon iterations stop." + "Convergence criterion regarding the ratio |R|/|R0| under which the Newton iterations stop." "Use a negative value to disable this criterion")) , d_should_diverge_when_residual_is_growing( initData(&d_should_diverge_when_residual_is_growing, false, "should_diverge_when_residual_is_growing", - "Boolean stopping Netwon iterations when the residual is greater than the one from the previous iteration")) + "Boolean stopping Newton iterations when the residual is greater than the one from the previous iteration")) {} void StaticSolver::solve(const sofa::core::ExecParams* params, SReal dt, sofa::core::MultiVecCoordId xResult, sofa::core::MultiVecDerivId vResult) @@ -212,7 +212,10 @@ void StaticSolver::solve(const sofa::core::ExecParams* params, SReal dt, sofa::c // is called on every BaseProjectiveConstraintSet objects. An example of such constraint set is the // FixedProjectiveConstraint. In this case, it will set to 0 every column (_, i) and row (i, _) of the assembled // matrix for the ith degree of freedom. - mop.setSystemMBKMatrix(0, 0, -1, l_linearSolver.get()); + mop.setSystemMBKMatrix( + core::MatricesFactors::M(0), + core::MatricesFactors::B(0), + core::MatricesFactors::K(-1), l_linearSolver.get()); } // Part II. Solve the unknown increment. diff --git a/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/VariationalSymplecticSolver.cpp b/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/VariationalSymplecticSolver.cpp index 173b2523a89..782466879b9 100644 --- a/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/VariationalSymplecticSolver.cpp +++ b/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/VariationalSymplecticSolver.cpp @@ -215,7 +215,9 @@ void VariationalSymplecticSolver::solve(const core::ExecParams* params, SReal dt // corresponds to do b+=-K*res, where res=res(i-1)=q(k,i-1)-q(k,0) mop.propagateDx(res); - mop.addMBKdx(b,0,0,-1.0); + mop.addMBKdx(b,core::MatricesFactors::M(0), + core::MatricesFactors::B(0), + core::MatricesFactors::K(-1.0)); mop.projectResponse(b); @@ -224,9 +226,9 @@ void VariationalSymplecticSolver::solve(const core::ExecParams* params, SReal dt // add left term : matrix=-K+4/h^(2)M, but with dampings rK and rM { SCOPED_TIMER("MBKBuild"); - const SReal mFact = 4.0 / (h * h) + 4 * rM / h; - const SReal kFact = -1.0 - 4 * rK / h; - mop.setSystemMBKMatrix(mFact, 0, kFact, l_linearSolver.get()); + const core::MatricesFactors::M mFact ( 4.0 / (h * h) + 4 * rM / h ); + const core::MatricesFactors::K kFact ( -1.0 - 4 * rK / h ); + mop.setSystemMBKMatrix(mFact, core::MatricesFactors::B(0), kFact, l_linearSolver.get()); } { @@ -312,7 +314,7 @@ void VariationalSymplecticSolver::solve(const core::ExecParams* params, SReal dt MultiVecDeriv b(&vop); b.clear(); // Mass matrix - mop.setSystemMBKMatrix(1, 0, 0, l_linearSolver.get()); + mop.setSystemMBKMatrix(core::MatricesFactors::M(1), core::MatricesFactors::B(0), core::MatricesFactors::K(0), l_linearSolver.get()); // resolution of matrix*b=newp l_linearSolver->setSystemLHVector(b); diff --git a/Sofa/Component/ODESolver/Forward/src/sofa/component/odesolver/forward/EulerExplicitSolver.cpp b/Sofa/Component/ODESolver/Forward/src/sofa/component/odesolver/forward/EulerExplicitSolver.cpp index a1a0ecc83b2..b9ccad03a69 100644 --- a/Sofa/Component/ODESolver/Forward/src/sofa/component/odesolver/forward/EulerExplicitSolver.cpp +++ b/Sofa/Component/ODESolver/Forward/src/sofa/component/odesolver/forward/EulerExplicitSolver.cpp @@ -328,7 +328,10 @@ void EulerExplicitSolver::assembleSystemMatrix(sofa::simulation::common::Mechani // is called on every BaseProjectiveConstraintSet objects. An example of such constraint set is the // FixedProjectiveConstraint. In this case, it will set to 0 every column (_, i) and row (i, _) of the assembled // matrix for the ith degree of freedom. - mop->setSystemMBKMatrix(1, 0, 0, l_linearSolver.get()); + mop->setSystemMBKMatrix( + core::MatricesFactors::M(1), + core::MatricesFactors::B(0), + core::MatricesFactors::K(0), l_linearSolver.get()); } void EulerExplicitSolver::solveSystem(core::MultiVecDerivId solution, core::MultiVecDerivId rhs) const diff --git a/Sofa/GL/Component/Rendering3D/src/sofa/gl/component/rendering3d/DataDisplay.cpp b/Sofa/GL/Component/Rendering3D/src/sofa/gl/component/rendering3d/DataDisplay.cpp index 2249b18d5e1..d126f8ac906 100644 --- a/Sofa/GL/Component/Rendering3D/src/sofa/gl/component/rendering3d/DataDisplay.cpp +++ b/Sofa/GL/Component/Rendering3D/src/sofa/gl/component/rendering3d/DataDisplay.cpp @@ -56,8 +56,8 @@ DataDisplay::DataDisplay() , state(nullptr) , m_topology(nullptr) , l_topology(initLink("topology", "link to the topology container")) - , oldMin(0) - , oldMax(0) + , m_oldMin(std::numeric_limits::max()) + , m_oldMax(std::numeric_limits::lowest()) { this->addAlias(&f_triangleData,"cellData"); // backward compatibility d_currentMin.setReadOnly(true); @@ -141,9 +141,8 @@ void DataDisplay::doDrawVisual(const core::visual::VisualParams* vparams) } // Range for points - Real min ; - Real max ; - min = max = 0; + Real min = std::numeric_limits::max(); + Real max = std::numeric_limits::lowest(); if (bDrawPointData) { VecPointData::const_iterator i = ptData.begin(); min = *i; @@ -212,12 +211,12 @@ void DataDisplay::doDrawVisual(const core::visual::VisualParams* vparams) } - if (max > oldMax) oldMax = max; - if (min < oldMin) oldMin = min; + if (max > m_oldMax) m_oldMax = max; + if (min < m_oldMin) m_oldMin = min; if (f_maximalRange.getValue()) { - max = oldMax; - min = oldMin; + max = m_oldMax; + min = m_oldMin; } d_currentMin.setValue(min); d_currentMax.setValue(max); diff --git a/Sofa/GL/Component/Rendering3D/src/sofa/gl/component/rendering3d/DataDisplay.h b/Sofa/GL/Component/Rendering3D/src/sofa/gl/component/rendering3d/DataDisplay.h index 91491ef5d43..91bb25b1acb 100644 --- a/Sofa/GL/Component/Rendering3D/src/sofa/gl/component/rendering3d/DataDisplay.h +++ b/Sofa/GL/Component/Rendering3D/src/sofa/gl/component/rendering3d/DataDisplay.h @@ -66,8 +66,6 @@ class SOFA_GL_COMPONENT_RENDERING3D_API DataDisplay : public core::visual::Visua /// Link to be set to the topology container in the component graph. SingleLink l_topology; - Real oldMin, oldMax; - void init() override; void doDrawVisual(const core::visual::VisualParams* vparams) override; void doUpdateVisual(const core::visual::VisualParams* vparams) override; @@ -80,6 +78,9 @@ class SOFA_GL_COMPONENT_RENDERING3D_API DataDisplay : public core::visual::Visua type::vector m_normals; DataDisplay(); + + Real m_oldMin, m_oldMax; + }; } // namespace sofa::gl::component::rendering3d diff --git a/Sofa/GL/Component/Rendering3D/src/sofa/gl/component/rendering3d/OglSceneFrame.cpp b/Sofa/GL/Component/Rendering3D/src/sofa/gl/component/rendering3d/OglSceneFrame.cpp index 41d46622f1a..f2bba6ceb04 100644 --- a/Sofa/GL/Component/Rendering3D/src/sofa/gl/component/rendering3d/OglSceneFrame.cpp +++ b/Sofa/GL/Component/Rendering3D/src/sofa/gl/component/rendering3d/OglSceneFrame.cpp @@ -32,20 +32,15 @@ int OglSceneFrameClass = core::RegisterObject("Display a frame at the corner of using namespace sofa::defaulttype; +static constexpr OglSceneFrame::Alignment defaultAlignment("BottomRight"); +static constexpr OglSceneFrame::Style defaultStyle("Cylinders"); + OglSceneFrame::OglSceneFrame() : d_drawFrame(initData(&d_drawFrame, true, "draw", "Display the frame or not")) - , d_style(initData(&d_style, "style", "Style of the frame")) - , d_alignment(initData(&d_alignment, "alignment", "Alignment of the frame in the view")) + , d_style(initData(&d_style, defaultStyle, "style", ("Style of the frame\n" + Style::dataDescription()).c_str())) + , d_alignment(initData(&d_alignment, defaultAlignment, "alignment", ("Alignment of the frame in the view\n" + Alignment::dataDescription()).c_str())) , d_viewportSize(initData(&d_viewportSize, 150, "viewportSize", "Size of the viewport where the frame is rendered")) -{ - sofa::helper::OptionsGroup styleOptions{"Arrows", "Cylinders", "CubeCones"}; - styleOptions.setSelectedItem(1); - d_style.setValue(styleOptions); - - sofa::helper::OptionsGroup alignmentOptions{"BottomLeft", "BottomRight", "TopRight", "TopLeft"}; - alignmentOptions.setSelectedItem(1); - d_alignment.setValue(alignmentOptions); -} +{} void OglSceneFrame::drawArrows(const core::visual::VisualParams* vparams) { @@ -112,22 +107,22 @@ void OglSceneFrame::doDrawVisual(const core::visual::VisualParams* vparams) const auto viewportSize = d_viewportSize.getValue(); - switch(d_alignment.getValue().getSelectedId()) + switch(d_alignment.getValue()) { - case 0: //BottomLeft + case Alignment("BottomLeft"): default: glViewport(0,0,viewportSize,viewportSize); glScissor(0,0,viewportSize,viewportSize); break; - case 1: //BottomRight + case Alignment("BottomRight"): glViewport(viewport[2]-viewportSize,0,viewportSize,viewportSize); glScissor(viewport[2]-viewportSize,0,viewportSize,viewportSize); break; - case 2: //TopRight + case Alignment("TopRight"): glViewport(viewport[2]-viewportSize,viewport[3]-viewportSize,viewportSize,viewportSize); glScissor(viewport[2]-viewportSize,viewport[3]-viewportSize,viewportSize,viewportSize); break; - case 3: //TopLeft + case Alignment("TopLeft"): glViewport(0,viewport[3]-viewportSize,viewportSize,viewportSize); glScissor(0,viewport[3]-viewportSize,viewportSize,viewportSize); break; @@ -157,18 +152,18 @@ void OglSceneFrame::doDrawVisual(const core::visual::VisualParams* vparams) vparams->drawTool()->disableLighting(); - switch (d_style.getValue().getSelectedId()) + switch (d_style.getValue()) { - case 0: + case Style("Arrows"): default: drawArrows(vparams); break; - case 1: + case Style("Cylinders"): drawCylinders(vparams); break; - case 2: + case Style("CubeCones"): drawCubeCones(vparams); break; } diff --git a/Sofa/GL/Component/Rendering3D/src/sofa/gl/component/rendering3d/OglSceneFrame.h b/Sofa/GL/Component/Rendering3D/src/sofa/gl/component/rendering3d/OglSceneFrame.h index c6cdd2288bb..d834c9584d1 100644 --- a/Sofa/GL/Component/Rendering3D/src/sofa/gl/component/rendering3d/OglSceneFrame.h +++ b/Sofa/GL/Component/Rendering3D/src/sofa/gl/component/rendering3d/OglSceneFrame.h @@ -25,6 +25,8 @@ #include #include #include +#include + namespace sofa::gl::component::rendering3d { @@ -38,8 +40,23 @@ class SOFA_GL_COMPONENT_RENDERING3D_API OglSceneFrame : public core::visual::Vis typedef core::visual::VisualParams::Viewport Viewport; Data d_drawFrame; ///< Display the frame or not - Data d_style; ///< Style of the frame - Data d_alignment; ///< Alignment of the frame in the view + + MAKE_SELECTABLE_ITEMS(Style, + sofa::helper::Item{"Arrows", "The frame is composed of arrows"}, + sofa::helper::Item{"Cylinders", "The frame is composed of cylinders"}, + sofa::helper::Item{"CubeCones", "The frame is composed of cubes and cones"}, + ); + + Data