diff --git a/Sofa/Component/Mass/src/sofa/component/mass/DiagonalMass.h b/Sofa/Component/Mass/src/sofa/component/mass/DiagonalMass.h index 87f15b99a2e..025149bf9f2 100644 --- a/Sofa/Component/Mass/src/sofa/component/mass/DiagonalMass.h +++ b/Sofa/Component/Mass/src/sofa/component/mass/DiagonalMass.h @@ -310,7 +310,7 @@ class DiagonalMass : public core::behavior::Mass void addGravityToV(const core::MechanicalParams* mparams, DataVecDeriv& d_v) override; /// Add Mass contribution to global Matrix assembling - void addMToMatrix(const core::MechanicalParams *mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) override; + void addMToMatrix(sofa::linearalgebra::BaseMatrix * mat, SReal mFact, unsigned int &offset) override; void buildMassMatrix(sofa::core::behavior::MassMatrixAccumulator* matrices) override; void buildStiffnessMatrix(core::behavior::StiffnessMatrix* /* matrix */) override {} void buildDampingMatrix(core::behavior::DampingMatrix* /* matrices */) override {} diff --git a/Sofa/Component/Mass/src/sofa/component/mass/DiagonalMass.inl b/Sofa/Component/Mass/src/sofa/component/mass/DiagonalMass.inl index 452714ace0e..806c58ccb5a 100644 --- a/Sofa/Component/Mass/src/sofa/component/mass/DiagonalMass.inl +++ b/Sofa/Component/Mass/src/sofa/component/mass/DiagonalMass.inl @@ -616,16 +616,14 @@ DiagonalMass::getMomentum ( const core::MechanicalP } template -void DiagonalMass::addMToMatrix(const core::MechanicalParams *mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) +void DiagonalMass::addMToMatrix(sofa::linearalgebra::BaseMatrix * mat, SReal mFact, unsigned int &offset) { const MassVector &masses= d_vertexMass.getValue(); static constexpr auto N = Deriv::total_size; AddMToMatrixFunctor calc; - sofa::core::behavior::MultiMatrixAccessor::MatrixRef r = matrix->getMatrix(this->mstate); - const Real mFactor = Real(sofa::core::mechanicalparams::mFactorIncludingRayleighDamping(mparams, this->rayleighMass.getValue())); for (unsigned int i = 0; i < masses.size(); i++) { - calc(r.matrix, masses[i], r.offset + N * i, mFactor); + calc(mat, masses[i], offset + N * i, mFact); } } diff --git a/Sofa/Component/Mass/src/sofa/component/mass/MeshMatrixMass.h b/Sofa/Component/Mass/src/sofa/component/mass/MeshMatrixMass.h index 195efd7a95c..bddcc0ce966 100644 --- a/Sofa/Component/Mass/src/sofa/component/mass/MeshMatrixMass.h +++ b/Sofa/Component/Mass/src/sofa/component/mass/MeshMatrixMass.h @@ -219,7 +219,7 @@ class MeshMatrixMass : public core::behavior::Mass /// Add Mass contribution to global Matrix assembling - void addMToMatrix(const core::MechanicalParams *mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) override; + void addMToMatrix(sofa::linearalgebra::BaseMatrix * mat, SReal mFact, unsigned int &offset) override; void buildMassMatrix(sofa::core::behavior::MassMatrixAccumulator* matrices) override; void buildStiffnessMatrix(core::behavior::StiffnessMatrix* /* matrix */) override {} void buildDampingMatrix(core::behavior::DampingMatrix* /* matrices */) override {} diff --git a/Sofa/Component/Mass/src/sofa/component/mass/MeshMatrixMass.inl b/Sofa/Component/Mass/src/sofa/component/mass/MeshMatrixMass.inl index 3162394f316..8f0336e38f0 100644 --- a/Sofa/Component/Mass/src/sofa/component/mass/MeshMatrixMass.inl +++ b/Sofa/Component/Mass/src/sofa/component/mass/MeshMatrixMass.inl @@ -2190,7 +2190,7 @@ void MeshMatrixMass::addGravityToV(const core::Mech template -void MeshMatrixMass::addMToMatrix(const core::MechanicalParams *mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) +void MeshMatrixMass::addMToMatrix(sofa::linearalgebra::BaseMatrix * mat, SReal mFact, unsigned int &offset) { const auto &vertexMass= d_vertexMass.getValue(); const auto &edgeMass= d_edgeMass.getValue(); @@ -2200,9 +2200,6 @@ void MeshMatrixMass::addMToMatrix(const core::Mecha static constexpr auto N = Deriv::total_size; AddMToMatrixFunctor calc; - sofa::core::behavior::MultiMatrixAccessor::MatrixRef r = matrix->getMatrix(this->mstate); - sofa::linearalgebra::BaseMatrix* mat = r.matrix; - const Real mFactor = Real(sofa::core::mechanicalparams::mFactorIncludingRayleighDamping(mparams, this->rayleighMass.getValue())); if((mat->colSize()) != (linearalgebra::BaseMatrix::Index)(l_topology->getNbPoints()*N) || (mat->rowSize()) != (linearalgebra::BaseMatrix::Index)(l_topology->getNbPoints()*N)) { @@ -2217,7 +2214,7 @@ void MeshMatrixMass::addMToMatrix(const core::Mecha unsigned int i {}; for (const auto& v : vertexMass) { - calc(r.matrix, v * m_massLumpingCoeff, r.offset + N * i, mFactor); + calc(mat, v * m_massLumpingCoeff, offset + N * i, mFact); massTotal += v * m_massLumpingCoeff; ++i; } @@ -2239,7 +2236,7 @@ void MeshMatrixMass::addMToMatrix(const core::Mecha unsigned int i {}; for (const auto& v : vertexMass) { - calc(r.matrix, v, r.offset + N * i, mFactor); + calc(mat, v, offset + N * i, mFact); massTotal += v; ++i; } @@ -2250,8 +2247,8 @@ void MeshMatrixMass::addMToMatrix(const core::Mecha v0 = edges[j][0]; v1 = edges[j][1]; - calc(r.matrix, edgeMass[j], r.offset + N*v0, r.offset + N*v1, mFactor); - calc(r.matrix, edgeMass[j], r.offset + N*v1, r.offset + N*v0, mFactor); + calc(mat, edgeMass[j], offset + N*v0, offset + N*v1, mFact); + calc(mat, edgeMass[j], offset + N*v1, offset + N*v0, mFact); massTotal += 2 * edgeMass[j]; } diff --git a/Sofa/Component/Mass/src/sofa/component/mass/UniformMass.h b/Sofa/Component/Mass/src/sofa/component/mass/UniformMass.h index 6b0e22a0e8d..eb323c52470 100644 --- a/Sofa/Component/Mass/src/sofa/component/mass/UniformMass.h +++ b/Sofa/Component/Mass/src/sofa/component/mass/UniformMass.h @@ -142,7 +142,7 @@ class UniformMass : public core::behavior::Mass void addGravityToV(const core::MechanicalParams* mparams, DataVecDeriv& d_v) override; - void addMToMatrix(const core::MechanicalParams *mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) override; /// Add Mass contribution to global Matrix assembling + void addMToMatrix(sofa::linearalgebra::BaseMatrix * mat, SReal mFact, unsigned int &offset) override; /// Add Mass contribution to global Matrix assembling void buildMassMatrix(sofa::core::behavior::MassMatrixAccumulator* matrices) override; void buildStiffnessMatrix(core::behavior::StiffnessMatrix* /* matrix */) override {} void buildDampingMatrix(core::behavior::DampingMatrix* /* matrices */) override {} diff --git a/Sofa/Component/Mass/src/sofa/component/mass/UniformMass.inl b/Sofa/Component/Mass/src/sofa/component/mass/UniformMass.inl index f94c2381f6c..3c24e46c56b 100644 --- a/Sofa/Component/Mass/src/sofa/component/mass/UniformMass.inl +++ b/Sofa/Component/Mass/src/sofa/component/mass/UniformMass.inl @@ -555,22 +555,18 @@ UniformMass::getMomentum ( const core::MechanicalParams* params, /// Add Mass contribution to global Matrix assembling template -void UniformMass::addMToMatrix (const MechanicalParams *mparams, - const MultiMatrixAccessor* matrix) +void UniformMass::addMToMatrix (sofa::linearalgebra::BaseMatrix * mat, SReal mFact, unsigned int &offset) { const MassType& m = d_vertexMass.getValue(); static constexpr auto N = Deriv::total_size; AddMToMatrixFunctor calc; - MultiMatrixAccessor::MatrixRef r = matrix->getMatrix(mstate); - - const Real mFactor = Real(sofa::core::mechanicalparams::mFactorIncludingRayleighDamping(mparams, this->rayleighMass.getValue())); const ReadAccessor > indices = d_indices; for (auto id : *indices) { - calc ( r.matrix, m, int(r.offset + N * id), mFactor); + calc ( mat, m, int(offset + N * id), mFact); } } diff --git a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/BeamFEMForceField.h b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/BeamFEMForceField.h index 31d649061df..8f703b67b4c 100644 --- a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/BeamFEMForceField.h +++ b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/BeamFEMForceField.h @@ -178,7 +178,7 @@ class BeamFEMForceField : public ForceField virtual void reinitBeam(Index i); void addForce(const MechanicalParams* mparams, DataVecDeriv & dataF, const DataVecCoord & dataX , const DataVecDeriv & dataV ) override; void addDForce(const MechanicalParams* mparams, DataVecDeriv& datadF , const DataVecDeriv& datadX ) override; - void addKToMatrix(const MechanicalParams* mparams, const MultiMatrixAccessor* matrix ) override; + void addKToMatrix(sofa::linearalgebra::BaseMatrix * matrix, SReal kFact, unsigned int &offset) override; void buildStiffnessMatrix(core::behavior::StiffnessMatrix* matrix) override; void buildDampingMatrix(core::behavior::DampingMatrix* /*matrix*/) final; SReal getPotentialEnergy(const MechanicalParams* mparams, const DataVecCoord& x) const override; diff --git a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/BeamFEMForceField.inl b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/BeamFEMForceField.inl index a4006e09c11..7abbcb71598 100644 --- a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/BeamFEMForceField.inl +++ b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/BeamFEMForceField.inl @@ -496,115 +496,104 @@ void BeamFEMForceField::applyStiffnessLarge(VecDeriv& df, const VecDe } template -void BeamFEMForceField::addKToMatrix(const sofa::core::MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix ) +void BeamFEMForceField::addKToMatrix(sofa::linearalgebra::BaseMatrix * matrix, SReal kFact, unsigned int &offset) { - sofa::core::behavior::MultiMatrixAccessor::MatrixRef r = matrix->getMatrix(this->mstate); - Real k = (Real)sofa::core::mechanicalparams::kFactorIncludingRayleighDamping(mparams, this->rayleighStiffness.getValue()); - linearalgebra::BaseMatrix* mat = r.matrix; - if (!m_indexedElements) return; - if (r) + if (m_partialListSegment) { - const unsigned int &offset = r.offset; - if (m_partialListSegment) + for (unsigned int i : d_listSegment.getValue()) { + const auto& [a, b] = (*m_indexedElements)[i].array(); - for (unsigned int i : d_listSegment.getValue()) - { - const auto& [a, b] = (*m_indexedElements)[i].array(); - - type::Quat& q = beamQuat(i); - q.normalize(); - Transformation R,Rt; - q.toMatrix(R); - Rt.transpose(R); - const StiffnessMatrix& K0 = d_beamsData.getValue()[i]._k_loc; - StiffnessMatrix K; - for (int x1=0; x1<12; x1+=3) - for (int y1=0; y1<12; y1+=3) + type::Quat& q = beamQuat(i); + q.normalize(); + Transformation R,Rt; + q.toMatrix(R); + Rt.transpose(R); + const StiffnessMatrix& K0 = d_beamsData.getValue()[i]._k_loc; + StiffnessMatrix K; + for (int x1=0; x1<12; x1+=3) + for (int y1=0; y1<12; y1+=3) + { + type::Mat<3,3,Real> m; + K0.getsub(x1,y1, m); + m = R*m*Rt; + K.setsub(x1,y1, m); + } + int index[12]; + for (int x1=0; x1<6; x1++) + index[x1] = offset+a*6+x1; + for (int x1=0; x1<6; x1++) + index[6+x1] = offset+b*6+x1; + for (int x1=0; x1<12; ++x1) + for (int y1=0; y1<12; ++y1) + matrix->add(index[x1], index[y1], - K(x1,y1)*kFact); + + } + + } + else + { + unsigned int i {}; + for(auto it = m_indexedElements->begin() ; it != m_indexedElements->end() ; ++it, ++i) + { + const auto& [a, b] = it->array(); + + type::Quat& q = beamQuat(i); + q.normalize(); + Transformation R,Rt; + q.toMatrix(R); + Rt.transpose(R); + const StiffnessMatrix& K0 = d_beamsData.getValue()[i]._k_loc; + StiffnessMatrix K; + const bool exploitSymmetry = d_useSymmetricAssembly.getValue(); + + if (exploitSymmetry) { + for (int x1=0; x1<12; x1+=3) { + for (int y1=x1; y1<12; y1+=3) { type::Mat<3,3,Real> m; K0.getsub(x1,y1, m); m = R*m*Rt; - K.setsub(x1,y1, m); - } - int index[12]; - for (int x1=0; x1<6; x1++) - index[x1] = offset+a*6+x1; - for (int x1=0; x1<6; x1++) - index[6+x1] = offset+b*6+x1; - for (int x1=0; x1<12; ++x1) - for (int y1=0; y1<12; ++y1) - mat->add(index[x1], index[y1], - K(x1,y1)*k); - - } - - } - else - { - unsigned int i {}; - for(auto it = m_indexedElements->begin() ; it != m_indexedElements->end() ; ++it, ++i) - { - const auto& [a, b] = it->array(); - - type::Quat& q = beamQuat(i); - q.normalize(); - Transformation R,Rt; - q.toMatrix(R); - Rt.transpose(R); - const StiffnessMatrix& K0 = d_beamsData.getValue()[i]._k_loc; - StiffnessMatrix K; - const bool exploitSymmetry = d_useSymmetricAssembly.getValue(); - - if (exploitSymmetry) { - for (int x1=0; x1<12; x1+=3) { - for (int y1=x1; y1<12; y1+=3) - { - type::Mat<3,3,Real> m; - K0.getsub(x1,y1, m); - m = R*m*Rt; + for (int i=0; i<3; i++) + for (int j=0; j<3; j++) { + K.elems[i+x1][j+y1] += m[i][j]; + K.elems[j+y1][i+x1] += m[i][j]; + } + if (x1 == y1) for (int i=0; i<3; i++) - for (int j=0; j<3; j++) { - K.elems[i+x1][j+y1] += m[i][j]; - K.elems[j+y1][i+x1] += m[i][j]; - } - if (x1 == y1) - for (int i=0; i<3; i++) - for (int j=0; j<3; j++) - K.elems[i+x1][j+y1] *= SReal(0.5); - - } + for (int j=0; j<3; j++) + K.elems[i+x1][j+y1] *= SReal(0.5); + } - } else { - for (int x1=0; x1<12; x1+=3) { - for (int y1=0; y1<12; y1+=3) - { - type::Mat<3,3,Real> m; - K0.getsub(x1,y1, m); - m = R*m*Rt; - K.setsub(x1,y1, m); - } + } + } else { + for (int x1=0; x1<12; x1+=3) { + for (int y1=0; y1<12; y1+=3) + { + type::Mat<3,3,Real> m; + K0.getsub(x1,y1, m); + m = R*m*Rt; + K.setsub(x1,y1, m); } } + } - int index[12]; - for (int x1=0; x1<6; x1++) - index[x1] = offset+a*6+x1; - for (int x1=0; x1<6; x1++) - index[6+x1] = offset+b*6+x1; - for (int x1=0; x1<12; ++x1) - for (int y1=0; y1<12; ++y1) - mat->add(index[x1], index[y1], - K(x1,y1)*k); + int index[12]; + for (int x1=0; x1<6; x1++) + index[x1] = offset+a*6+x1; + for (int x1=0; x1<6; x1++) + index[6+x1] = offset+b*6+x1; + for (int x1=0; x1<12; ++x1) + for (int y1=0; y1<12; ++y1) + matrix->add(index[x1], index[y1], - K(x1,y1)*kFact); - } } - } - } template diff --git a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/HexahedralFEMForceField.h b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/HexahedralFEMForceField.h index c19d144af6b..48ade06d120 100644 --- a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/HexahedralFEMForceField.h +++ b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/HexahedralFEMForceField.h @@ -152,7 +152,7 @@ class HexahedralFEMForceField : virtual public BaseLinearElasticityFEMForceField return 0.0; } - void addKToMatrix(const core::MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) override; + void addKToMatrix(sofa::linearalgebra::BaseMatrix * matrix, SReal kFact, unsigned int &offset) override; void buildStiffnessMatrix(core::behavior::StiffnessMatrix* matrix) override; void buildDampingMatrix(core::behavior::DampingMatrix* /*matrix*/) final; diff --git a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/HexahedralFEMForceField.inl b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/HexahedralFEMForceField.inl index 3b1574cfdcc..af1fe47009e 100644 --- a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/HexahedralFEMForceField.inl +++ b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/HexahedralFEMForceField.inl @@ -589,15 +589,13 @@ void HexahedralFEMForceField::accumulateForcePolar(WDataRefVecDeriv& ///////////////////////////////////////////////// template -void HexahedralFEMForceField::addKToMatrix(const core::MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) +void HexahedralFEMForceField::addKToMatrix(sofa::linearalgebra::BaseMatrix * matrix, SReal kFact, unsigned int &offset) { // Build Matrix Block for this ForceField int i,j,n1, n2; Index node1, node2; - sofa::core::behavior::MultiMatrixAccessor::MatrixRef r = matrix->getMatrix(this->mstate); - const Real kFactor = (Real)sofa::core::mechanicalparams::kFactorIncludingRayleighDamping(mparams, this->rayleighStiffness.getValue()); const type::vector::HexahedronInformation>& hexahedronInf = d_hexahedronInfo.getValue(); for(Size e=0 ; e<_topology->getNbHexahedra() ; ++e) @@ -618,7 +616,7 @@ void HexahedralFEMForceField::addKToMatrix(const core::MechanicalPara Coord(Ke[3*n1+2][3*n2+0],Ke[3*n1+2][3*n2+1],Ke[3*n1+2][3*n2+2])) ) * hexahedronInf[e].rotation; for(i=0; i<3; i++) for (j=0; j<3; j++) - r.matrix->add(r.offset+3*node1+i, r.offset+3*node2+j, - tmp[i][j]*kFactor); + matrix->add(offset+3*node1+i, offset+3*node2+j, - tmp[i][j]*kFact); } } } diff --git a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/HexahedralFEMForceFieldAndMass.h b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/HexahedralFEMForceFieldAndMass.h index 940f5c4650b..c34a11b663b 100644 --- a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/HexahedralFEMForceFieldAndMass.h +++ b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/HexahedralFEMForceFieldAndMass.h @@ -70,14 +70,14 @@ class HexahedralFEMForceFieldAndMass : virtual public sofa::core::behavior::Mass void addMDx(const core::MechanicalParams* mparams, DataVecDeriv& f, const DataVecDeriv& dx, SReal factor) override; ///// WARNING this method only add diagonal elements in the given matrix ! - void addMToMatrix(const core::MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) override; + void addMToMatrix(sofa::linearalgebra::BaseMatrix * mat, SReal mFact, unsigned int &offset) override; bool isDiagonal() const override { return d_useLumpedMass.getValue(); } using HexahedralFEMForceFieldT::addKToMatrix; using MassT::addKToMatrix; ///// WARNING this method only add diagonal elements in the given matrix ! - void addKToMatrix(const core::MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) override; + void addKToMatrix(sofa::linearalgebra::BaseMatrix * matrix, SReal kFact, unsigned int &offset) override; ///// WARNING this method only add diagonal elements in the given matrix ! void addMBKToMatrix(const core::MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) override; @@ -86,6 +86,8 @@ class HexahedralFEMForceFieldAndMass : virtual public sofa::core::behavior::Mass void addForce(const core::MechanicalParams* mparams, DataVecDeriv& f, const DataVecCoord& x, const DataVecDeriv& v) override; + using HexahedralFEMForceFieldT::getPotentialEnergy; + using MassT::getPotentialEnergy; SReal getPotentialEnergy(const core::MechanicalParams* /*mparams*/, const DataVecCoord& /* x */) const override { msg_warning() << "Method getPotentialEnergy not implemented yet."; diff --git a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/HexahedralFEMForceFieldAndMass.inl b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/HexahedralFEMForceFieldAndMass.inl index 592dc384f00..6a5ab2e0a27 100644 --- a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/HexahedralFEMForceFieldAndMass.inl +++ b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/HexahedralFEMForceFieldAndMass.inl @@ -261,7 +261,7 @@ void HexahedralFEMForceFieldAndMass::addMDx(const core::MechanicalPar } template -void HexahedralFEMForceFieldAndMass::addMToMatrix(const core::MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) +void HexahedralFEMForceFieldAndMass::addMToMatrix(sofa::linearalgebra::BaseMatrix * mat, SReal mFact, unsigned int &offset) { // Build Matrix Block for this ForceField int i, j, n1, n2; @@ -269,13 +269,10 @@ void HexahedralFEMForceFieldAndMass::addMToMatrix(const core::Mechani const VecElement& hexahedra = this->_topology->getHexahedra(); - sofa::core::behavior::MultiMatrixAccessor::MatrixRef r = matrix->getMatrix(this->mstate); - for(unsigned int e=0; erayleighMass.getValue()); // find index of node 1 for (n1=0; n1<8; n1++) { @@ -292,7 +289,7 @@ void HexahedralFEMForceFieldAndMass::addMToMatrix(const core::Mechani Coord(Me[3*n1+2][3*n2+0],Me[3*n1+2][3*n2+1],Me[3*n1+2][3*n2+2])); for(i=0; i<3; i++) for (j=0; j<3; j++) - r.matrix->add(r.offset+3*node1+i, r.offset+3*node2+j, tmp[i][j]*mFactor); + mat->add(offset+3*node1+i, offset+3*node2+j, tmp[i][j]*mFact); } } } @@ -300,7 +297,7 @@ void HexahedralFEMForceFieldAndMass::addMToMatrix(const core::Mechani ///// WARNING this method only add diagonal elements in the given matrix ! template -void HexahedralFEMForceFieldAndMass::addKToMatrix(const core::MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) +void HexahedralFEMForceFieldAndMass::addKToMatrix(sofa::linearalgebra::BaseMatrix * matrix, SReal kFact, unsigned int &offset) { // Build Matrix Block for this ForceField int i,j,n1, n2, e; @@ -311,14 +308,11 @@ void HexahedralFEMForceFieldAndMass::addKToMatrix(const core::Mechani Index node1, node2; const VecElement& hexahedra = this->_topology->getHexahedra(); - sofa::core::behavior::MultiMatrixAccessor::MatrixRef r = matrix->getMatrix(this->mstate); - for(it = this->d_hexahedronInfo.getValue().begin(), e=0 ; it != this->d_hexahedronInfo.getValue().end() ; ++it,++e) { const Element hexa = hexahedra[e]; const ElementStiffness &Ke = it->stiffness; - Real kFactor = (Real)sofa::core::mechanicalparams::kFactorIncludingRayleighDamping(mparams, this->rayleighStiffness.getValue()); // find index of node 1 for (n1=0; n1<8; n1++) { @@ -335,7 +329,7 @@ void HexahedralFEMForceFieldAndMass::addKToMatrix(const core::Mechani Coord(Ke[3*n1+2][3*n2+0],Ke[3*n1+2][3*n2+1],Ke[3*n1+2][3*n2+2])) ) * it->rotation; for(i=0; i<3; i++) for (j=0; j<3; j++) - r.matrix->add(r.offset+3*node1+i, r.offset+3*node2+j, - tmp[i][j]*kFactor); + matrix->add(offset+3*node1+i, offset+3*node2+j, - tmp[i][j]*kFact); } } } diff --git a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/HexahedronFEMForceField.h b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/HexahedronFEMForceField.h index d603aba35ce..345e52f9b84 100644 --- a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/HexahedronFEMForceField.h +++ b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/HexahedronFEMForceField.h @@ -160,18 +160,14 @@ class HexahedronFEMForceField : virtual public BaseLinearElasticityFEMForceField void addDForce (const core::MechanicalParams* mparams, DataVecDeriv& df, const DataVecDeriv& dx) override; - SReal getPotentialEnergy(const core::MechanicalParams* /*mparams*/, - const DataVecCoord& /* x */) const override; - - // getPotentialEnergy is implemented for polar method - SReal getPotentialEnergy(const core::MechanicalParams*) const override; + SReal getPotentialEnergy(const core::MechanicalParams* mparams, const DataVecCoord& x) const override; const Transformation& getElementRotation(const sofa::Index elemidx); void getNodeRotation(Transformation& R, sofa::Index nodeIdx) ; void getRotations(linearalgebra::BaseMatrix * rotations,int offset = 0) override ; - void addKToMatrix(const core::MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) override; + void addKToMatrix(sofa::linearalgebra::BaseMatrix * matrix, SReal kFact, unsigned int &offset) override; void buildStiffnessMatrix(core::behavior::StiffnessMatrix* matrix) override; void buildDampingMatrix(core::behavior::DampingMatrix* /* matrices */) override {} diff --git a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/HexahedronFEMForceField.inl b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/HexahedronFEMForceField.inl index 816e48d3ceb..7c564fbb4b2 100644 --- a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/HexahedronFEMForceField.inl +++ b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/HexahedronFEMForceField.inl @@ -992,14 +992,6 @@ void HexahedronFEMForceField::getNodeRotation(Transformation& R, unsi R = Rmoy; } -template -SReal HexahedronFEMForceField::getPotentialEnergy(const core::MechanicalParams* /*mparams*/, - const DataVecCoord& /* x */) const -{ - msg_warning() << "Method getPotentialEnergy not implemented yet."; - return 0.0; -} - template void HexahedronFEMForceField::getRotations(linearalgebra::BaseMatrix * rotations,int offset) { @@ -1104,8 +1096,11 @@ void HexahedronFEMForceField::accumulateForcePolar( WDataRefVecDeriv } template -inline SReal HexahedronFEMForceField::getPotentialEnergy(const core::MechanicalParams*) const +inline SReal HexahedronFEMForceField::getPotentialEnergy(const core::MechanicalParams* mparams, const DataVecCoord& x) const { + SOFA_UNUSED(mparams); + SOFA_UNUSED(x); + return m_potentialEnergy; } @@ -1133,13 +1128,10 @@ inline void HexahedronFEMForceField::setMethod(int val) template -void HexahedronFEMForceField::addKToMatrix(const core::MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) +void HexahedronFEMForceField::addKToMatrix(sofa::linearalgebra::BaseMatrix * matrix, SReal kFact, unsigned int &offset) { // Build Matrix Block for this ForceField - sofa::core::behavior::MultiMatrixAccessor::MatrixRef r = matrix->getMatrix(this->mstate); - const Real kFactor = (Real)sofa::core::mechanicalparams::kFactorIncludingRayleighDamping(mparams, this->rayleighStiffness.getValue()); - sofa::Index e { 0 }; //index of the element in the topology const auto& stiffnesses = d_elementStiffnesses.getValue(); @@ -1165,7 +1157,7 @@ void HexahedronFEMForceField::addKToMatrix(const core::MechanicalPara Coord(Ke[3*n1+1][3*n2+0],Ke[3*n1+1][3*n2+1],Ke[3*n1+1][3*n2+2]), Coord(Ke[3*n1+2][3*n2+0],Ke[3*n1+2][3*n2+1],Ke[3*n1+2][3*n2+2])) ) * Rot; - r.matrix->add( r.offset + 3 * node1, r.offset + 3 * node2, tmp * (-kFactor)); + matrix->add( offset + 3 * node1, offset + 3 * node2, tmp * (-kFact)); } } } diff --git a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/HexahedronFEMForceFieldAndMass.h b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/HexahedronFEMForceFieldAndMass.h index b02346a4526..d6e8b1531a1 100644 --- a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/HexahedronFEMForceFieldAndMass.h +++ b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/HexahedronFEMForceFieldAndMass.h @@ -73,15 +73,15 @@ class HexahedronFEMForceFieldAndMass : virtual public core::behavior::Mass::addKToMatrix; - void addKToMatrix(const core::MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) override + void addKToMatrix(sofa::linearalgebra::BaseMatrix * matrix, SReal kFact, unsigned int &offset) override { - HexahedronFEMForceFieldT::addKToMatrix(mparams, matrix); + HexahedronFEMForceFieldT::addKToMatrix(matrix, kFact, offset); } void buildStiffnessMatrix(core::behavior::StiffnessMatrix*) override; diff --git a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/HexahedronFEMForceFieldAndMass.inl b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/HexahedronFEMForceFieldAndMass.inl index def75891881..8f073f94451 100644 --- a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/HexahedronFEMForceFieldAndMass.inl +++ b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/HexahedronFEMForceFieldAndMass.inl @@ -216,7 +216,7 @@ void HexahedronFEMForceFieldAndMass::addMDx(const core::MechanicalPar template -void HexahedronFEMForceFieldAndMass::addMToMatrix(const core::MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) +void HexahedronFEMForceFieldAndMass::addMToMatrix(sofa::linearalgebra::BaseMatrix * mat, SReal mFact, unsigned int &offset) { // Build Matrix Block for this ForceField int i,j,n1, n2, e; @@ -225,13 +225,10 @@ void HexahedronFEMForceFieldAndMass::addMToMatrix(const core::Mechani int node1, node2; - sofa::core::behavior::MultiMatrixAccessor::MatrixRef r = matrix->getMatrix(this->mstate); - for(it = this->getIndexedElements()->begin(), e=0 ; it != this->getIndexedElements()->end() ; ++it,++e) { const ElementMass &Me = d_elementMasses.getValue()[e]; - Real mFactor = (Real)sofa::core::mechanicalparams::mFactorIncludingRayleighDamping(mparams, this->rayleighMass.getValue()); // find index of node 1 for (n1=0; n1<8; n1++) { @@ -247,7 +244,7 @@ void HexahedronFEMForceFieldAndMass::addMToMatrix(const core::Mechani Coord(Me[3*n1+2][3*n2+0],Me[3*n1+2][3*n2+1],Me[3*n1+2][3*n2+2])); for(i=0; i<3; i++) for (j=0; j<3; j++) - r.matrix->add(r.offset+3*node1+i, r.offset+3*node2+j, tmp[i][j]*mFactor); + mat->add(offset+3*node1+i, offset+3*node2+j, tmp[i][j]*mFact); } } } diff --git a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/TriangularFEMForceFieldOptim.h b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/TriangularFEMForceFieldOptim.h index 1c4df87598c..6da99129b2c 100644 --- a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/TriangularFEMForceFieldOptim.h +++ b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/TriangularFEMForceFieldOptim.h @@ -106,7 +106,7 @@ class TriangularFEMForceFieldOptim : public core::behavior::ForceField::addDForce(const core::MechanicalPa // -------------------------------------------------------------------------------------- template -void TriangularFEMForceFieldOptim::addKToMatrix(const core::MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) +void TriangularFEMForceFieldOptim::addKToMatrix(sofa::linearalgebra::BaseMatrix * matrix, SReal kFact, unsigned int &offset) { - sofa::core::behavior::MultiMatrixAccessor::MatrixRef r = matrix->getMatrix(this->mstate); - sofa::helper::ReadAccessor< core::objectmodel::Data< VecTriangleState > > triState = d_triangleState; sofa::helper::ReadAccessor< core::objectmodel::Data< VecTriangleInfo > > triInfo = d_triangleInfo; - const Real kFactor = (Real)sofa::core::mechanicalparams::kFactorIncludingRayleighDamping(mparams, this->rayleighStiffness.getValue()); const unsigned int nbTriangles = m_topology->getNbTriangles(); const VecElement& triangles = m_topology->getTriangles(); const Real gamma = this->gamma; @@ -430,7 +427,7 @@ void TriangularFEMForceFieldOptim::addKToMatrix(const core::Mechanica const TriangleInfo& ti = triInfo[i]; const TriangleState& ts = triState[i]; sofa::type::MatNoInit<3,4,Real> KJt; - const Real factor = -kFactor * ti.ss_factor; + const Real factor = -kFact * ti.ss_factor; const Real fG = factor * gamma; const Real fGM = factor * (gamma+mu); const Real fM_2 = factor * (0.5f*mu); @@ -467,21 +464,21 @@ void TriangularFEMForceFieldOptim::addKToMatrix(const core::Mechanica Transformation frame = ts.frame; - r.matrix->add(r.offset + S * t[0], r.offset + S * t[0], frame.multTranspose(JKJt00*frame)); + matrix->add(offset + S * t[0], offset + S * t[0], frame.multTranspose(JKJt00*frame)); const MatBloc M01 = frame.multTranspose(JKJt01*frame); - r.matrix->add(r.offset + S * t[0], r.offset + S * t[1], M01); - r.matrix->add(r.offset + S * t[1], r.offset + S * t[0], M01.transposed()); + matrix->add(offset + S * t[0], offset + S * t[1], M01); + matrix->add(offset + S * t[1], offset + S * t[0], M01.transposed()); const MatBloc M02 = frame.multTranspose(JKJt02*frame); - r.matrix->add(r.offset + S * t[0], r.offset + S * t[2], M02); - r.matrix->add(r.offset + S * t[2], r.offset + S * t[0], M02.transposed()); - r.matrix->add(r.offset + S * t[1], r.offset + S * t[1], frame.multTranspose(JKJt11*frame)); + matrix->add(offset + S * t[0], offset + S * t[2], M02); + matrix->add(offset + S * t[2], offset + S * t[0], M02.transposed()); + matrix->add(offset + S * t[1], offset + S * t[1], frame.multTranspose(JKJt11*frame)); const MatBloc M12 = frame.multTranspose(JKJt12*frame); - r.matrix->add(r.offset + S * t[1], r.offset + S * t[2], M12); - r.matrix->add(r.offset + S * t[2], r.offset + S * t[1], M12.transposed()); - r.matrix->add(r.offset + S * t[2], r.offset + S * t[2], frame.multTranspose(JKJt22*frame)); + matrix->add(offset + S * t[1], offset + S * t[2], M12); + matrix->add(offset + S * t[2], offset + S * t[1], M12.transposed()); + matrix->add(offset + S * t[2], offset + S * t[2], frame.multTranspose(JKJt22*frame)); } } diff --git a/Sofa/Component/SolidMechanics/Spring/src/sofa/component/solidmechanics/spring/FastTriangularBendingSprings.h b/Sofa/Component/SolidMechanics/Spring/src/sofa/component/solidmechanics/spring/FastTriangularBendingSprings.h index f89a0514d21..f29a3fc16f2 100644 --- a/Sofa/Component/SolidMechanics/Spring/src/sofa/component/solidmechanics/spring/FastTriangularBendingSprings.h +++ b/Sofa/Component/SolidMechanics/Spring/src/sofa/component/solidmechanics/spring/FastTriangularBendingSprings.h @@ -145,7 +145,7 @@ class FastTriangularBendingSprings : public core::behavior::ForceField< _DataTyp #endif /// Stiffness matrix assembly - void addStiffness( sofa::linearalgebra::BaseMatrix *bm, unsigned int offset, SReal scale, core::behavior::ForceField< _DataTypes>* ff ) const; + void addStiffness( sofa::linearalgebra::BaseMatrix *bm, unsigned int offset, SReal scale, FastTriangularBendingSprings< _DataTypes>* ff ) const; /// Compliant stiffness matrix assembly void getStiffness( StiffnessMatrix &K ) const; /// replace a vertex index with another one @@ -165,6 +165,7 @@ class FastTriangularBendingSprings : public core::behavior::ForceField< _DataTyp return in; } }; + friend EdgeSpring; /// The list of edge springs, one for each edge between two triangles sofa::core::topology::EdgeData > d_edgeSprings; diff --git a/Sofa/Component/SolidMechanics/Spring/src/sofa/component/solidmechanics/spring/FastTriangularBendingSprings.inl b/Sofa/Component/SolidMechanics/Spring/src/sofa/component/solidmechanics/spring/FastTriangularBendingSprings.inl index 5532d529d59..37232292690 100644 --- a/Sofa/Component/SolidMechanics/Spring/src/sofa/component/solidmechanics/spring/FastTriangularBendingSprings.inl +++ b/Sofa/Component/SolidMechanics/Spring/src/sofa/component/solidmechanics/spring/FastTriangularBendingSprings.inl @@ -536,7 +536,7 @@ typename FastTriangularBendingSprings<_DataTypes>::Real FastTriangularBendingSp } template -void FastTriangularBendingSprings<_DataTypes>::EdgeSpring::addStiffness( sofa::linearalgebra::BaseMatrix *bm, unsigned int offset, SReal scale, core::behavior::ForceField< _DataTypes>* ff ) const +void FastTriangularBendingSprings<_DataTypes>::EdgeSpring::addStiffness( sofa::linearalgebra::BaseMatrix *bm, unsigned int offset, SReal scale, FastTriangularBendingSprings< _DataTypes>* ff ) const { StiffnessMatrix K; getStiffness( K ); diff --git a/Sofa/framework/Core/src/sofa/core/behavior/Constraint.h b/Sofa/framework/Core/src/sofa/core/behavior/Constraint.h index 62ed6dc2294..9fe74c9c232 100644 --- a/Sofa/framework/Core/src/sofa/core/behavior/Constraint.h +++ b/Sofa/framework/Core/src/sofa/core/behavior/Constraint.h @@ -71,38 +71,16 @@ class Constraint : public BaseConstraint, public SingleStateAccessor /// /// \param v is the result vector that contains the whole constraints violations /// \param cParams defines the state vectors to use for positions and velocities. Also defines the order of the constraint (POS, VEL, ACC) - void getConstraintViolation(const ConstraintParams* cParams, linearalgebra::BaseVector *v) override; - - /// Construct the Constraint violations vector of each constraint - /// - /// \param resV is the result vector that contains the whole constraints violations - /// \param x is the position vector used to compute contraint position violation - /// \param v is the velocity vector used to compute contraint velocity violation - /// \param cParams defines the state vectors to use for positions and velocities. Also defines the order of the constraint (POS, VEL, ACC) - /// - /// This is the method that should be implemented by the component - virtual void getConstraintViolation(const ConstraintParams* cParams, linearalgebra::BaseVector *resV, const DataVecCoord &x, const DataVecDeriv &v) = 0; - + void getConstraintViolation(const ConstraintParams* cParams, linearalgebra::BaseVector *v) final; /// Construct the Jacobian Matrix /// /// \param cId is the result constraint sparse matrix Id /// \param cIndex is the index of the next constraint equation: when building the constraint matrix, you have to use this index, and then update it /// \param cParams defines the state vectors to use for positions and velocities. Also defines the order of the constraint (POS, VEL, ACC) - void buildConstraintMatrix(const ConstraintParams* cParams, MultiMatrixDerivId cId, unsigned int &cIndex) override; - - /// Construct the Jacobian Matrix - /// - /// \param c is the result constraint sparse matrix - /// \param cIndex is the index of the next constraint equation: when building the constraint matrix, you have to use this index, and then update it - /// \param x is the position vector used for contraint equation computation - /// \param cParams defines the state vectors to use for positions and velocities. Also defines the order of the constraint (POS, VEL, ACC) - /// - /// This is the method that should be implemented by the component - virtual void buildConstraintMatrix(const ConstraintParams* cParams, DataMatrixDeriv & c, unsigned int &cIndex, const DataVecCoord &x) = 0; - + void buildConstraintMatrix(const ConstraintParams* cParams, MultiMatrixDerivId cId, unsigned int &cIndex) final; - void storeLambda(const ConstraintParams* cParams, MultiVecDerivId res, const sofa::linearalgebra::BaseVector* lambda) override; + void storeLambda(const ConstraintParams* cParams, MultiVecDerivId res, const sofa::linearalgebra::BaseVector* lambda) final; /// Pre-construction check method called by ObjectFactory. /// Check that DataTypes matches the MechanicalState. @@ -126,11 +104,30 @@ class Constraint : public BaseConstraint, public SingleStateAccessor protected: - virtual type::vector getConstraintIdentifiers(){ return {}; } + /// Construct the Constraint violations vector of each constraint + /// + /// \param resV is the result vector that contains the whole constraints violations + /// \param x is the position vector used to compute contraint position violation + /// \param v is the velocity vector used to compute contraint velocity violation + /// \param cParams defines the state vectors to use for positions and velocities. Also defines the order of the constraint (POS, VEL, ACC) + /// + /// This is the method that should be implemented by the component + virtual void getConstraintViolation(const ConstraintParams* cParams, linearalgebra::BaseVector *resV, const DataVecCoord &x, const DataVecDeriv &v) = 0; + + /// Construct the Jacobian Matrix + /// + /// \param c is the result constraint sparse matrix + /// \param cIndex is the index of the next constraint equation: when building the constraint matrix, you have to use this index, and then update it + /// \param x is the position vector used for contraint equation computation + /// \param cParams defines the state vectors to use for positions and velocities. Also defines the order of the constraint (POS, VEL, ACC) + /// + /// This is the method that should be implemented by the component + virtual void buildConstraintMatrix(const ConstraintParams* cParams, DataMatrixDeriv & c, unsigned int &cIndex, const DataVecCoord &x) = 0; + virtual void storeLambda(const ConstraintParams* cParams, Data& resId, const Data& jacobian, const sofa::linearalgebra::BaseVector* lambda); + + virtual type::vector getConstraintIdentifiers(){ return {}; } -private: - void storeLambda(const ConstraintParams* cParams, Data& resId, const Data& jacobian, const sofa::linearalgebra::BaseVector* lambda); }; #if !defined(SOFA_CORE_BEHAVIOR_CONSTRAINT_CPP) diff --git a/Sofa/framework/Core/src/sofa/core/behavior/Constraint.inl b/Sofa/framework/Core/src/sofa/core/behavior/Constraint.inl index e80274dd83a..6d600b32ce4 100644 --- a/Sofa/framework/Core/src/sofa/core/behavior/Constraint.inl +++ b/Sofa/framework/Core/src/sofa/core/behavior/Constraint.inl @@ -57,6 +57,11 @@ void Constraint::init() template void Constraint::getConstraintViolation(const ConstraintParams* cParams, linearalgebra::BaseVector *v) { + if (this->isComponentStateInvalid()) + { + return; + } + if (cParams) { getConstraintViolation(cParams, v, *cParams->readX(this->mstate.get()), *cParams->readV(this->mstate.get())); @@ -67,6 +72,11 @@ void Constraint::getConstraintViolation(const ConstraintParams* cPara template void Constraint::buildConstraintMatrix(const ConstraintParams* cParams, MultiMatrixDerivId cId, unsigned int &cIndex) { + if (this->isComponentStateInvalid()) + { + return; + } + if (cParams) { buildConstraintMatrix(cParams, *cId[this->mstate.get()].write(), cIndex, *cParams->readX(this->mstate.get())); @@ -77,6 +87,11 @@ void Constraint::buildConstraintMatrix(const ConstraintParams* cParam template void Constraint::storeLambda(const ConstraintParams* cParams, MultiVecDerivId res, const sofa::linearalgebra::BaseVector* lambda) { + if (this->isComponentStateInvalid()) + { + return; + } + if (cParams) { storeLambda(cParams, *res[this->mstate.get()].write(), *cParams->readJ(this->mstate.get()), lambda); diff --git a/Sofa/framework/Core/src/sofa/core/behavior/ConstraintCorrection.h b/Sofa/framework/Core/src/sofa/core/behavior/ConstraintCorrection.h index cdf18249e99..5c1e128bd76 100644 --- a/Sofa/framework/Core/src/sofa/core/behavior/ConstraintCorrection.h +++ b/Sofa/framework/Core/src/sofa/core/behavior/ConstraintCorrection.h @@ -73,7 +73,6 @@ class ConstraintCorrection : public BaseConstraintCorrection MultiLink< ConstraintCorrection, core::behavior::ConstraintSolver, BaseLink::FLAG_NONE > l_constraintsolvers; public: - /// Compute the motion coming from the contact space lambda /// dx = A^-1 x J^t x lambda @@ -88,16 +87,10 @@ class ConstraintCorrection : public BaseConstraintCorrection /// @param cparams the ConstraintParams relative to the constraint solver /// @param dx the VecId where to store the corrective motion /// @param lambda is the constraint space force vector - void computeMotionCorrectionFromLambda(const core::ConstraintParams* cparams, core::MultiVecDerivId dx, const linearalgebra::BaseVector * lambda) override; - - - /// Compute the corrective motion coming from the motion space force - - /// @param cparams the ConstraintParams relative to the constraint solver - /// @param dx the VecId where to store the corrective motion - /// @param f is the VecId where the motion space force : f = J^t x lambda - virtual void computeMotionCorrection(const core::ConstraintParams* cparams, core::MultiVecDerivId dx, core::MultiVecDerivId f) = 0; - + void computeMotionCorrectionFromLambda( + const core::ConstraintParams* cparams, + core::MultiVecDerivId dx, + const linearalgebra::BaseVector * lambda) final; /// Compute motion correction from the constraint resolution (LCP) calculated force /// @@ -106,9 +99,12 @@ class ConstraintCorrection : public BaseConstraintCorrection /// @param v is the velocity result VecId /// @param dx if the corrective motion result VecId /// @param f is the motion space force vector - void applyMotionCorrection(const core::ConstraintParams * cparams, core::MultiVecCoordId x, core::MultiVecDerivId v, core::MultiVecDerivId dx, core::ConstMultiVecDerivId correction) override; - - virtual void applyMotionCorrection(const core::ConstraintParams * cparams, Data< VecCoord > &x, Data< VecDeriv > &v, Data< VecDeriv > &dx, const Data< VecDeriv > & correction) = 0; + void applyMotionCorrection( + const core::ConstraintParams * cparams, + core::MultiVecCoordId x, + core::MultiVecDerivId v, + core::MultiVecDerivId dx, + core::ConstMultiVecDerivId correction) final; /// Compute position correction from the constraint resolution (LCP) calculated force /// @@ -116,9 +112,11 @@ class ConstraintCorrection : public BaseConstraintCorrection /// @param x is the position result VecId /// @param dx id the corrective position result VecId /// @param f is the motion space force vector - void applyPositionCorrection(const core::ConstraintParams * cparams, core::MultiVecCoordId x, core::MultiVecDerivId dx, core::ConstMultiVecDerivId correction) override; - - virtual void applyPositionCorrection(const core::ConstraintParams * cparams, Data< VecCoord > &x, Data& dx, const Data< VecDeriv > &correction) = 0; + void applyPositionCorrection( + const core::ConstraintParams * cparams, + core::MultiVecCoordId x, + core::MultiVecDerivId dx, + core::ConstMultiVecDerivId correction) final; /// Compute velocity correction from the constraint resolution (LCP) calculated force /// @@ -126,16 +124,21 @@ class ConstraintCorrection : public BaseConstraintCorrection /// @param v is the velocity result VecId /// @param dv is the corrective velocity result VecId /// @param f is the motion space force vector - void applyVelocityCorrection(const core::ConstraintParams * cparams, core::MultiVecDerivId v, core::MultiVecDerivId dv, core::ConstMultiVecDerivId correction) override; - - virtual void applyVelocityCorrection(const core::ConstraintParams * cparams, Data< VecDeriv > &v, Data& dv , const Data< VecDeriv > &correction) = 0; + void applyVelocityCorrection( + const core::ConstraintParams * cparams, + core::MultiVecDerivId v, + core::MultiVecDerivId dv, + core::ConstMultiVecDerivId correction) final; /// Apply predictive constraint force /// /// @param cparams /// @param f is the motion space force vector /// @param lambda is the constraint space force vector - void applyPredictiveConstraintForce(const core::ConstraintParams * cparams, core::MultiVecDerivId f, const linearalgebra::BaseVector *lambda) override; + void applyPredictiveConstraintForce( + const core::ConstraintParams * cparams, + core::MultiVecDerivId f, + const linearalgebra::BaseVector *lambda) final; /// Pre-construction check method called by ObjectFactory. @@ -161,6 +164,21 @@ class ConstraintCorrection : public BaseConstraintCorrection } protected: + + /// Compute the corrective motion coming from the motion space force + /// @param cparams the ConstraintParams relative to the constraint solver + /// @param dx the VecId where to store the corrective motion + /// @param f is the VecId where the motion space force : f = J^t x lambda + virtual void computeMotionCorrection( + const core::ConstraintParams* cparams, + core::MultiVecDerivId dx, + core::MultiVecDerivId f) = 0; + + virtual void applyMotionCorrection(const core::ConstraintParams * cparams, Data< VecCoord > &x, Data< VecDeriv > &v, Data< VecDeriv > &dx, const Data< VecDeriv > & correction) = 0; + virtual void applyPositionCorrection(const core::ConstraintParams * cparams, Data< VecCoord > &x, Data& dx, const Data< VecDeriv > &correction) = 0; + virtual void applyVelocityCorrection(const core::ConstraintParams * cparams, Data< VecDeriv > &v, Data& dv , const Data< VecDeriv > &correction) = 0; + + MechanicalState *mstate; private: diff --git a/Sofa/framework/Core/src/sofa/core/behavior/ConstraintCorrection.inl b/Sofa/framework/Core/src/sofa/core/behavior/ConstraintCorrection.inl index 0a375b07d29..8597ea0d780 100644 --- a/Sofa/framework/Core/src/sofa/core/behavior/ConstraintCorrection.inl +++ b/Sofa/framework/Core/src/sofa/core/behavior/ConstraintCorrection.inl @@ -78,6 +78,11 @@ void ConstraintCorrection< DataTypes >::computeMotionCorrectionFromLambda(const template< class DataTypes > void ConstraintCorrection< DataTypes >::applyMotionCorrection(const core::ConstraintParams *cparams, core::MultiVecCoordId x, core::MultiVecDerivId v, core::MultiVecDerivId dx, core::ConstMultiVecDerivId correction) { + if (isComponentStateInvalid()) + { + return; + } + if (mstate) { Data< VecCoord > *x_d = x[mstate].write(); @@ -96,6 +101,11 @@ void ConstraintCorrection< DataTypes >::applyMotionCorrection(const core::Constr template< class DataTypes > void ConstraintCorrection< DataTypes >::applyPositionCorrection(const core::ConstraintParams *cparams, core::MultiVecCoordId x, core::MultiVecDerivId dx, core::ConstMultiVecDerivId correction) { + if (isComponentStateInvalid()) + { + return; + } + if (mstate) { Data< VecCoord > *x_d = x[mstate].write(); @@ -113,6 +123,11 @@ void ConstraintCorrection< DataTypes >::applyPositionCorrection(const core::Cons template< class DataTypes > void ConstraintCorrection< DataTypes >::applyVelocityCorrection(const core::ConstraintParams *cparams, core::MultiVecDerivId v, core::MultiVecDerivId dv, core::ConstMultiVecDerivId correction) { + if (isComponentStateInvalid()) + { + return; + } + if (mstate) { Data< VecDeriv >* v_d = v[mstate].write(); @@ -133,12 +148,17 @@ void ConstraintCorrection< DataTypes >::applyPredictiveConstraintForce(const cor if (mstate) { addConstraintForceInMotionSpace(cparams, f, cparams->j(), lambda); - } + } } template< class DataTypes > void ConstraintCorrection< DataTypes >::addConstraintForceInMotionSpace(const core::ConstraintParams* cparams, core::MultiVecDerivId f, core::ConstMultiMatrixDerivId j, const linearalgebra::BaseVector * lambda) { + if (isComponentStateInvalid()) + { + return; + } + if (mstate) { Data< VecDeriv > *f_d = f[mstate].write(); diff --git a/Sofa/framework/Core/src/sofa/core/behavior/ForceField.h b/Sofa/framework/Core/src/sofa/core/behavior/ForceField.h index 3b9caa987e6..1207d4d1684 100644 --- a/Sofa/framework/Core/src/sofa/core/behavior/ForceField.h +++ b/Sofa/framework/Core/src/sofa/core/behavior/ForceField.h @@ -74,17 +74,7 @@ class ForceField : public BaseForceField, public SingleStateAccessor /// This method retrieves the force, x and v vector from the MechanicalState /// and call the internal addForce(const MechanicalParams*, DataVecDeriv&,const DataVecCoord&,const DataVecDeriv&) /// method implemented by the component. - void addForce(const MechanicalParams* mparams, MultiVecDerivId fId ) override; - - /// Given the current position and velocity states, update the current force - /// vector by computing and adding the forces associated with this - /// ForceField. - /// - /// If the ForceField can be represented as a matrix, this method computes - /// \f$ f += B v + K x \f$ - /// - /// This is the method that should be implemented by the component - virtual void addForce(const MechanicalParams* /*mparams*/, DataVecDeriv& f, const DataVecCoord& x, const DataVecDeriv& v) = 0; + void addForce(const MechanicalParams* mparams, MultiVecDerivId fId ) final; /// Compute the force derivative given a small displacement from the /// position and velocity used in the previous call to addForce(). @@ -99,15 +89,7 @@ class ForceField : public BaseForceField, public SingleStateAccessor /// This method retrieves the force and dx vector from the MechanicalState /// and call the internal addDForce(VecDeriv&,const VecDeriv&,SReal,SReal) /// method implemented by the component. - void addDForce(const MechanicalParams* mparams, MultiVecDerivId dfId ) override; - - /// Internal addDForce - /// Overloaded function, usually called from the generic addDForce version. - /// This addDForce version directly gives access to df and dx vectors through its parameters. - /// @param mparams - /// @param df Output vector to fill, result of \f$ kFactor K dx + bFactor B dx \f$ - /// @param dx Input vector used to compute \f$ df = kFactor K dx + bFactor B dx \f$ - virtual void addDForce(const MechanicalParams* mparams, DataVecDeriv& df, const DataVecDeriv& dx ) = 0; + void addDForce(const MechanicalParams* mparams, MultiVecDerivId dfId ) final; //This is required to tell the compiler addClambda is legitimately overloaded, //and it does not hide the one from BaseForceField. @@ -126,9 +108,6 @@ class ForceField : public BaseForceField, public SingleStateAccessor /// by the generic ForceField::getPotentialEnergy(const MechanicalParams* mparams) method. SReal getPotentialEnergy(const MechanicalParams* mparams) const override; - virtual SReal getPotentialEnergy(const MechanicalParams* /*mparams*/, const DataVecCoord& x) const = 0; - - /// @} /// @name Matrix operations @@ -136,52 +115,8 @@ class ForceField : public BaseForceField, public SingleStateAccessor void addKToMatrix(const MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix ) override; - /// Internal addKToMatrix - /// Overloaded function, usually called from the generic addKToMatrix version. - /// This addKToMatrix version directly gives access to the matrix to fill, the stiffness factor and - /// the offset used to identify where the force field must add its contributions in the matrix. - /// @param matrix the global stiffness matrix in which the force field adds its contribution. The matrix is global, - /// i.e. different objects can add their contribution into the same large matrix. Each object adds its contribution - /// to a different section of the matrix. That is why, an offset is used to identify where in the matrix the force - /// field must start adding its contribution. - /// @param kFact stiffness factor that needs to be multiplied to each matrix entry. - /// @param offset Starting index of the submatrix to fill in the global matrix. - virtual void addKToMatrix(sofa::linearalgebra::BaseMatrix * matrix, SReal kFact, unsigned int &offset); - - void addBToMatrix(const MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) override; - - /** Accumulate an element matrix to a global assembly matrix. This is a helper for addKToMatrix, to accumulate each (square) element matrix in the (square) assembled matrix. - \param bm the global assembly matrix - \param offset start index of the local DOFs within the global matrix - \param nodeIndex indices of the nodes of the element within the local nodes, as stored in the topology - \param em element matrix, typically a stiffness, damping, mass, or weighted sum thereof - \param scale weight applied to the matrix, typically ±params->kfactor() for a stiffness matrix - */ - template - void addToMatrix(sofa::linearalgebra::BaseMatrix* bm, unsigned offset, const IndexArray& nodeIndex, const ElementMat& em, SReal scale ) - { - constexpr auto S = DataTypes::deriv_total_size; // size of node blocks - for (unsigned n1=0; n1add( ROW,COLUMN, em[row][column]* scale ); - } - } - } - } - } + void addBToMatrix(const MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) final; - virtual void addBToMatrix(sofa::linearalgebra::BaseMatrix * matrix, SReal bFact, unsigned int &offset); /// @} /// Pre-construction check method called by ObjectFactory. @@ -221,6 +156,76 @@ class ForceField : public BaseForceField, public SingleStateAccessor return name; } + +protected: + + /// Given the current position and velocity states, update the current force + /// vector by computing and adding the forces associated with this + /// ForceField. + /// + /// If the ForceField can be represented as a matrix, this method computes + /// \f$ f += B v + K x \f$ + /// + /// This is the method that should be implemented by the component + virtual void addForce(const MechanicalParams* /*mparams*/, DataVecDeriv& f, const DataVecCoord& x, const DataVecDeriv& v) = 0; + + /// Internal addDForce + /// Overloaded function, usually called from the generic addDForce version. + /// This addDForce version directly gives access to df and dx vectors through its parameters. + /// @param mparams + /// @param df Output vector to fill, result of \f$ kFactor K dx + bFactor B dx \f$ + /// @param dx Input vector used to compute \f$ df = kFactor K dx + bFactor B dx \f$ + virtual void addDForce(const MechanicalParams* mparams, DataVecDeriv& df, const DataVecDeriv& dx ) = 0; + + virtual SReal getPotentialEnergy(const MechanicalParams* /*mparams*/, const DataVecCoord& x) const = 0; + + /// Internal addKToMatrix + /// Overloaded function, usually called from the generic addKToMatrix version. + /// This addKToMatrix version directly gives access to the matrix to fill, the stiffness factor and + /// the offset used to identify where the force field must add its contributions in the matrix. + /// @param matrix the global stiffness matrix in which the force field adds its contribution. The matrix is global, + /// i.e. different objects can add their contribution into the same large matrix. Each object adds its contribution + /// to a different section of the matrix. That is why, an offset is used to identify where in the matrix the force + /// field must start adding its contribution. + /// @param kFact stiffness factor that needs to be multiplied to each matrix entry. + /// @param offset Starting index of the submatrix to fill in the global matrix. + virtual void addKToMatrix(sofa::linearalgebra::BaseMatrix * matrix, SReal kFact, unsigned int &offset); + + virtual void addBToMatrix(sofa::linearalgebra::BaseMatrix * matrix, SReal bFact, unsigned int &offset); + + + /** + * Accumulate an element matrix to a global assembly matrix. This is a helper for addKToMatrix, to accumulate each (square) element matrix in the (square) assembled matrix. + * \param bm the global assembly matrix + * \param offset start index of the local DOFs within the global matrix + * \param nodeIndex indices of the nodes of the element within the local nodes, as stored in the topology + * \param em element matrix, typically a stiffness, damping, mass, or weighted sum thereof + * \param scale weight applied to the matrix, typically ±params->kfactor() for a stiffness matrix + */ + template + void addToMatrix(sofa::linearalgebra::BaseMatrix* bm, unsigned offset, const IndexArray& nodeIndex, const ElementMat& em, SReal scale ) + { + constexpr auto S = DataTypes::deriv_total_size; // size of node blocks + for (unsigned n1=0; n1add( ROW,COLUMN, em[row][column]* scale ); + } + } + } + } + } + }; #if !defined(SOFA_CORE_BEHAVIOR_FORCEFIELD_CPP) diff --git a/Sofa/framework/Core/src/sofa/core/behavior/ForceField.inl b/Sofa/framework/Core/src/sofa/core/behavior/ForceField.inl index d84e1e33b8b..edb0fa9121c 100644 --- a/Sofa/framework/Core/src/sofa/core/behavior/ForceField.inl +++ b/Sofa/framework/Core/src/sofa/core/behavior/ForceField.inl @@ -43,6 +43,11 @@ ForceField::~ForceField() = default; template void ForceField::addForce(const MechanicalParams* mparams, MultiVecDerivId fId ) { + if (isComponentStateInvalid()) + { + return; + } + auto mstate = this->mstate.get(); if (mparams && mstate) { @@ -53,6 +58,11 @@ void ForceField::addForce(const MechanicalParams* mparams, MultiVecDe template void ForceField::addDForce(const MechanicalParams* mparams, MultiVecDerivId dfId ) { + if (isComponentStateInvalid()) + { + return; + } + if (mparams && this->mstate) { @@ -80,6 +90,11 @@ void ForceField::addClambda(const MechanicalParams* /*mparams*/, Data template SReal ForceField::getPotentialEnergy(const MechanicalParams* mparams) const { + if (isComponentStateInvalid()) + { + return 0; + } + if (this->mstate) return getPotentialEnergy(mparams, *mparams->readX(this->mstate.get())); return 0; @@ -88,6 +103,11 @@ SReal ForceField::getPotentialEnergy(const MechanicalParams* mparams) template void ForceField::addKToMatrix(const MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix ) { + if (isComponentStateInvalid()) + { + return; + } + if (this->mstate) { sofa::core::behavior::MultiMatrixAccessor::MatrixRef r = matrix->getMatrix(this->mstate); @@ -114,6 +134,11 @@ void ForceField::addKToMatrix(sofa::linearalgebra::BaseMatrix * /*mat template void ForceField::addBToMatrix(const MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) { + if (isComponentStateInvalid()) + { + return; + } + if (this->mstate) { sofa::core::behavior::MultiMatrixAccessor::MatrixRef r = matrix->getMatrix(this->mstate); diff --git a/Sofa/framework/Core/src/sofa/core/behavior/Mass.h b/Sofa/framework/Core/src/sofa/core/behavior/Mass.h index 1dec7f93119..b35d0a2d4f3 100644 --- a/Sofa/framework/Core/src/sofa/core/behavior/Mass.h +++ b/Sofa/framework/Core/src/sofa/core/behavior/Mass.h @@ -64,21 +64,13 @@ class Mass : virtual public ForceField, public BaseMass /// /// This method retrieves the force and dx vector and call the internal /// addMDx(const MechanicalParams*, DataVecDeriv&, const DataVecDeriv&, SReal) method implemented by the component. - void addMDx(const MechanicalParams* mparams, MultiVecDerivId fid, SReal factor) override; - - virtual void addMDx(const MechanicalParams* mparams, DataVecDeriv& f, const DataVecDeriv& dx, SReal factor); + void addMDx(const MechanicalParams* mparams, MultiVecDerivId fid, SReal factor) final; /// $ dx = M^-1 f $ /// /// This method retrieves the force and dx vector and call the internal /// accFromF(VecDeriv&,const VecDeriv&) method implemented by the component. - void accFromF(const MechanicalParams* mparams, MultiVecDerivId aid) override; - - virtual void accFromF(const MechanicalParams* mparams, DataVecDeriv& a, const DataVecDeriv& f); - - - /// Mass forces (gravity) often have null derivative - void addDForce(const MechanicalParams* /*mparams*/, DataVecDeriv & /*df*/, const DataVecDeriv & /*dx*/ ) override; + void accFromF(const MechanicalParams* mparams, MultiVecDerivId aid) final; /// Accumulate the contribution of M, B, and/or K matrices multiplied /// by the dx vector with the given coefficients. @@ -90,21 +82,19 @@ class Mass : virtual public ForceField, public BaseMass /// \param mFact coefficient for mass contributions (i.e. second-order derivatives term in the ODE) /// \param bFact coefficient for damping contributions (i.e. first derivatives term in the ODE) /// \param kFact coefficient for stiffness contributions (i.e. DOFs term in the ODE) - void addMBKdx(const MechanicalParams* mparams, MultiVecDerivId dfId) override; + void addMBKdx(const MechanicalParams* mparams, MultiVecDerivId dfId) final; /// $ e = 1/2 v^t M v $ /// /// This method retrieves the velocity vector and call the internal /// getKineticEnergy(const MechanicalParams*, const DataVecDeriv&) method implemented by the component. SReal getKineticEnergy( const MechanicalParams* mparams) const override; - virtual SReal getKineticEnergy( const MechanicalParams* mparams, const DataVecDeriv& v) const; /// $ e = M g x $ /// /// This method retrieves the positions vector and call the internal /// getPotentialEnergy(const MechanicalParams*, const VecCoord&) method implemented by the component. SReal getPotentialEnergy( const MechanicalParams* mparams) const override; - SReal getPotentialEnergy( const MechanicalParams* mparams, const DataVecCoord& x ) const override; /// $ m = ( Mv, cross(x,Mv)+Iw ) $ @@ -112,22 +102,14 @@ class Mass : virtual public ForceField, public BaseMass /// /// This method retrieves the positions and velocity vectors and call the internal /// getMomentum(const MechanicalParams*, const VecCoord&, const VecDeriv&) method implemented by the component. - type::Vec6 getMomentum( const MechanicalParams* mparams ) const override; - virtual type::Vec6 getMomentum( const MechanicalParams* , const DataVecCoord& , const DataVecDeriv& ) const; - - + type::Vec6 getMomentum( const MechanicalParams* mparams ) const final; /// @} /// @name Matrix operations /// @{ - void addKToMatrix(sofa::linearalgebra::BaseMatrix * /*matrix*/, SReal /*kFact*/, unsigned int &/*offset*/) override {} - void addBToMatrix(sofa::linearalgebra::BaseMatrix * /*matrix*/, SReal /*bFact*/, unsigned int &/*offset*/) override {} - - void addMToMatrix(const MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) override; - virtual void addMToMatrix(sofa::linearalgebra::BaseMatrix * matrix, SReal mFact, unsigned int &offset); - + void addMToMatrix(const MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) final; /// Compute the system matrix corresponding to m M + b B + k K /// @@ -155,6 +137,24 @@ class Mass : virtual public ForceField, public BaseMass void getElementMass(sofa::Index index, linearalgebra::BaseMatrix *m) const override; protected: + + virtual void addMDx(const MechanicalParams* mparams, DataVecDeriv& f, const DataVecDeriv& dx, SReal factor); + + virtual void accFromF(const MechanicalParams* mparams, DataVecDeriv& a, const DataVecDeriv& f); + + /// Mass forces (gravity) often have null derivative + void addDForce(const MechanicalParams* /*mparams*/, DataVecDeriv & /*df*/, const DataVecDeriv & /*dx*/ ) override; + + virtual SReal getKineticEnergy( const MechanicalParams* mparams, const DataVecDeriv& v) const; + SReal getPotentialEnergy( const MechanicalParams* mparams, const DataVecCoord& x ) const override; + + virtual type::Vec6 getMomentum( const MechanicalParams* , const DataVecCoord& , const DataVecDeriv& ) const; + + void addKToMatrix(sofa::linearalgebra::BaseMatrix * /*matrix*/, SReal /*kFact*/, unsigned int &/*offset*/) override {} + void addBToMatrix(sofa::linearalgebra::BaseMatrix * /*matrix*/, SReal /*bFact*/, unsigned int &/*offset*/) override {} + virtual void addMToMatrix(sofa::linearalgebra::BaseMatrix * matrix, SReal mFact, unsigned int &offset); + + /// stream to export Kinematic, Potential and Mechanical Energy to gnuplot files std::unique_ptr m_gnuplotFileEnergy; diff --git a/Sofa/framework/Core/src/sofa/core/behavior/Mass.inl b/Sofa/framework/Core/src/sofa/core/behavior/Mass.inl index 40d733d9c8b..56cb1de4d50 100644 --- a/Sofa/framework/Core/src/sofa/core/behavior/Mass.inl +++ b/Sofa/framework/Core/src/sofa/core/behavior/Mass.inl @@ -47,6 +47,11 @@ Mass::~Mass() template void Mass::addMDx(const MechanicalParams* mparams, MultiVecDerivId fid, SReal factor) { + if (isComponentStateInvalid()) + { + return; + } + if (mparams) { auto mstate = this->mstate.get(); @@ -64,6 +69,11 @@ void Mass::addMDx(const MechanicalParams* /*mparams*/, DataVecDeriv& template void Mass::accFromF(const MechanicalParams* mparams, MultiVecDerivId aid) { + if (isComponentStateInvalid()) + { + return; + } + if(mparams) { auto mstate = this->mstate.get(); @@ -97,6 +107,11 @@ void Mass::addDForce(const MechanicalParams* template void Mass::addMBKdx(const MechanicalParams* mparams, MultiVecDerivId dfId) { + if (isComponentStateInvalid()) + { + return; + } + this->ForceField::addMBKdx(mparams, dfId); if (mparams->mFactorIncludingRayleighDamping(rayleighMass.getValue()) != 0.0) { @@ -108,6 +123,11 @@ void Mass::addMBKdx(const MechanicalParams* mparams, MultiVecDerivId template SReal Mass::getKineticEnergy(const MechanicalParams* mparams) const { + if (isComponentStateInvalid()) + { + return 0; + } + if (this->mstate) return getKineticEnergy(mparams /* PARAMS FIRST */, *mparams->readV(this->mstate.get())); return 0.0; @@ -124,6 +144,11 @@ SReal Mass::getKineticEnergy(const MechanicalParams* /*mparams*/, con template SReal Mass::getPotentialEnergy(const MechanicalParams* mparams) const { + if (isComponentStateInvalid()) + { + return 0; + } + if (this->mstate) return getPotentialEnergy(mparams /* PARAMS FIRST */, *mparams->readX(this->mstate.get())); return 0.0; @@ -140,6 +165,11 @@ SReal Mass::getPotentialEnergy(const MechanicalParams* /*mparams*/, c template type::Vec6 Mass::getMomentum( const MechanicalParams* mparams ) const { + if (isComponentStateInvalid()) + { + return {}; + } + auto state = this->mstate.get(); if (state) return getMomentum(mparams, *mparams->readX(state), *mparams->readV(state)); @@ -158,6 +188,11 @@ type::Vec6 Mass::getMomentum( const MechanicalParams* /*mparams*/, co template void Mass::addMToMatrix(const MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) { + if (isComponentStateInvalid()) + { + return; + } + sofa::core::behavior::MultiMatrixAccessor::MatrixRef r = matrix->getMatrix(this->mstate); if (r) addMToMatrix(r.matrix, mparams->mFactorIncludingRayleighDamping(rayleighMass.getValue()), r.offset); @@ -176,6 +211,11 @@ void Mass::addMToMatrix(sofa::linearalgebra::BaseMatrix * /*mat*/, SR template void Mass::addMBKToMatrix(const MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) { + if (isComponentStateInvalid()) + { + return; + } + this->ForceField::addMBKToMatrix(mparams, matrix); if (mparams->mFactorIncludingRayleighDamping(rayleighMass.getValue()) != 0.0) addMToMatrix(mparams, matrix); @@ -184,6 +224,11 @@ void Mass::addMBKToMatrix(const MechanicalParams* mparams, const sofa template void Mass::addGravityToV(const MechanicalParams* mparams, MultiVecDerivId vid) { + if (isComponentStateInvalid()) + { + return; + } + if(this->mstate) { DataVecDeriv& v = *vid[this->mstate.get()].write(); diff --git a/Sofa/framework/Core/src/sofa/core/behavior/MixedInteractionConstraint.h b/Sofa/framework/Core/src/sofa/core/behavior/MixedInteractionConstraint.h index b36a5d6ef91..e1d48739b5c 100644 --- a/Sofa/framework/Core/src/sofa/core/behavior/MixedInteractionConstraint.h +++ b/Sofa/framework/Core/src/sofa/core/behavior/MixedInteractionConstraint.h @@ -83,7 +83,30 @@ class MixedInteractionConstraint : public BaseInteractionConstraint, public Pair /// /// \param v is the result vector that contains the whole constraints violations /// \param cParams defines the state vectors to use for positions and velocities. Also defines the order of the constraint (POS, VEL, ACC) - void getConstraintViolation(const ConstraintParams* cParams, linearalgebra::BaseVector *v) override; + void getConstraintViolation(const ConstraintParams* cParams, linearalgebra::BaseVector *v) final; + + /// Construct the Jacobian Matrix + /// + /// \param cId is the result constraint sparse matrix Id + /// \param cIndex is the index of the next constraint equation: when building the constraint matrix, you have to use this index, and then update it + /// \param cParams defines the state vectors to use for positions and velocities. Also defines the order of the constraint (POS, VEL, ACC) + void buildConstraintMatrix(const ConstraintParams* cParams, MultiMatrixDerivId cId, unsigned int &cIndex) final; + + /// Construction method called by ObjectFactory. + template + static typename T::SPtr create(T* p0, core::objectmodel::BaseContext* context, core::objectmodel::BaseObjectDescription* arg) + { + typename T::SPtr obj = core::behavior::BaseInteractionConstraint::create(p0, context, arg); + + if (arg) + { + obj->parse(arg); + } + + return obj; + } + +protected: /// Construct the Constraint violations vector of each constraint /// @@ -96,13 +119,6 @@ class MixedInteractionConstraint : public BaseInteractionConstraint, public Pair virtual void getConstraintViolation(const ConstraintParams* cParams, linearalgebra::BaseVector *v, const DataVecCoord1 &x1, const DataVecCoord2 &x2 , const DataVecDeriv1 &v1, const DataVecDeriv2 &v2) = 0; - /// Construct the Jacobian Matrix - /// - /// \param cId is the result constraint sparse matrix Id - /// \param cIndex is the index of the next constraint equation: when building the constraint matrix, you have to use this index, and then update it - /// \param cParams defines the state vectors to use for positions and velocities. Also defines the order of the constraint (POS, VEL, ACC) - void buildConstraintMatrix(const ConstraintParams* cParams, MultiMatrixDerivId cId, unsigned int &cIndex) override; - /// Construct the Jacobian Matrix /// /// \param c1 and c2 are the results constraint sparse matrix @@ -114,20 +130,6 @@ class MixedInteractionConstraint : public BaseInteractionConstraint, public Pair virtual void buildConstraintMatrix(const ConstraintParams* cParams, DataMatrixDeriv1 &c1, DataMatrixDeriv2 &c2, unsigned int &cIndex , const DataVecCoord1 &x1, const DataVecCoord2 &x2) = 0; - - /// Construction method called by ObjectFactory. - template - static typename T::SPtr create(T* p0, core::objectmodel::BaseContext* context, core::objectmodel::BaseObjectDescription* arg) - { - typename T::SPtr obj = core::behavior::BaseInteractionConstraint::create(p0, context, arg); - - if (arg) - { - obj->parse(arg); - } - - return obj; - } }; #if !defined(SOFA_CORE_BEHAVIOR_MIXEDINTERACTIONCONSTRAINT_CPP) diff --git a/Sofa/framework/Core/src/sofa/core/behavior/MixedInteractionConstraint.inl b/Sofa/framework/Core/src/sofa/core/behavior/MixedInteractionConstraint.inl index b30ac9047a2..e2f103b76e8 100644 --- a/Sofa/framework/Core/src/sofa/core/behavior/MixedInteractionConstraint.inl +++ b/Sofa/framework/Core/src/sofa/core/behavior/MixedInteractionConstraint.inl @@ -49,6 +49,11 @@ bool MixedInteractionConstraint::isActive() const template void MixedInteractionConstraint::getConstraintViolation(const ConstraintParams* cParams, linearalgebra::BaseVector *v) { + if (this->isComponentStateInvalid()) + { + return; + } + if (cParams) { getConstraintViolation(cParams, v, *cParams->readX(this->mstate1.get()), *cParams->readX(this->mstate2.get()), @@ -59,6 +64,11 @@ void MixedInteractionConstraint::getConstraintViolation( template void MixedInteractionConstraint::buildConstraintMatrix(const ConstraintParams* cParams, MultiMatrixDerivId cId, unsigned int &cIndex) { + if (this->isComponentStateInvalid()) + { + return; + } + if (cParams) { buildConstraintMatrix(cParams, *cId[this->mstate1.get()].write(), *cId[this->mstate2.get()].write(), cIndex, diff --git a/Sofa/framework/Core/src/sofa/core/behavior/MixedInteractionForceField.h b/Sofa/framework/Core/src/sofa/core/behavior/MixedInteractionForceField.h index e9c0e65950f..2d178ef3886 100644 --- a/Sofa/framework/Core/src/sofa/core/behavior/MixedInteractionForceField.h +++ b/Sofa/framework/Core/src/sofa/core/behavior/MixedInteractionForceField.h @@ -80,7 +80,7 @@ class MixedInteractionForceField : public BaseInteractionForceField, public Pair /// This method retrieves the force, x and v vector from the two MechanicalState /// and call the internal addForce(VecDeriv&,VecDeriv&,const VecCoord&,const VecCoord&,const VecDeriv&,const VecDeriv&) /// method implemented by the component. - void addForce(const MechanicalParams* mparams, MultiVecDerivId fId ) override; + void addForce(const MechanicalParams* mparams, MultiVecDerivId fId ) final; /// Compute the force derivative given a small displacement from the /// position and velocity used in the previous call to addForce(). @@ -95,7 +95,7 @@ class MixedInteractionForceField : public BaseInteractionForceField, public Pair /// This method retrieves the force and dx vector from the two MechanicalState /// and call the internal addDForce(VecDeriv1&,VecDeriv2&,const VecDeriv1&,const VecDeriv2&,SReal,SReal) /// method implemented by the component. - void addDForce(const MechanicalParams* mparams, MultiVecDerivId dfId ) override; + void addDForce(const MechanicalParams* mparams, MultiVecDerivId dfId ) final; /// Get the potential energy associated to this ForceField. @@ -108,6 +108,20 @@ class MixedInteractionForceField : public BaseInteractionForceField, public Pair /// the component. SReal getPotentialEnergy(const MechanicalParams* mparams) const override; + template + static std::string shortName(const T* ptr = nullptr, objectmodel::BaseObjectDescription* arg = nullptr) + { + std::string name = Inherit1::shortName(ptr, arg); + sofa::helper::replaceAll(name, "InteractionForceField", "IFF"); + sofa::helper::replaceAll(name, "ForceField", "FF"); + return name; + } + + using Inherit2::getMechModel1; + using Inherit2::getMechModel2; + +protected: + /// Given the current position and velocity states, update the current force /// vector by computing and adding the forces associated with this /// ForceField. @@ -117,7 +131,6 @@ class MixedInteractionForceField : public BaseInteractionForceField, public Pair /// /// This method must be implemented by the component, and is usually called /// by the generic ForceField::addForce() method. - virtual void addForce(const MechanicalParams* mparams, DataVecDeriv1& f1, DataVecDeriv2& f2, const DataVecCoord1& x1, const DataVecCoord2& x2, const DataVecDeriv1& v1, const DataVecDeriv2& v2)=0; /// Compute the force derivative given a small displacement from the @@ -132,7 +145,6 @@ class MixedInteractionForceField : public BaseInteractionForceField, public Pair /// /// This method must be implemented by the component, and is usually called /// by the generic MixedInteractionForceField::addDForce() method. - virtual void addDForce(const MechanicalParams* mparams, DataVecDeriv1& df1, DataVecDeriv2& df2, const DataVecDeriv1& dx1, const DataVecDeriv2& dx2)=0; /// Get the potential energy associated to this ForceField. @@ -144,17 +156,6 @@ class MixedInteractionForceField : public BaseInteractionForceField, public Pair /// by the generic MixedInteractionForceField::getPotentialEnergy() method. virtual SReal getPotentialEnergy(const MechanicalParams* mparams, const DataVecCoord1& x1, const DataVecCoord2& x2) const =0; - template - static std::string shortName(const T* ptr = nullptr, objectmodel::BaseObjectDescription* arg = nullptr) - { - std::string name = Inherit1::shortName(ptr, arg); - sofa::helper::replaceAll(name, "InteractionForceField", "IFF"); - sofa::helper::replaceAll(name, "ForceField", "FF"); - return name; - } - - using Inherit2::getMechModel1; - using Inherit2::getMechModel2; }; #if !defined(SOFA_CORE_BEHAVIOR_MIXEDINTERACTIONFORCEFIELD_CPP) diff --git a/Sofa/framework/Core/src/sofa/core/behavior/MixedInteractionForceField.inl b/Sofa/framework/Core/src/sofa/core/behavior/MixedInteractionForceField.inl index 8cbd487f143..6adfa69963b 100644 --- a/Sofa/framework/Core/src/sofa/core/behavior/MixedInteractionForceField.inl +++ b/Sofa/framework/Core/src/sofa/core/behavior/MixedInteractionForceField.inl @@ -45,7 +45,11 @@ MixedInteractionForceField::~MixedInteractionForceField( template void MixedInteractionForceField::addForce(const MechanicalParams* mparams, MultiVecDerivId fId ) { - + if (this->isComponentStateInvalid()) + { + return; + } + if (this->mstate1 && this->mstate2) { auto state1 = this->mstate1.get(); @@ -60,6 +64,11 @@ void MixedInteractionForceField::addForce(const Mechanic template void MixedInteractionForceField::addDForce(const MechanicalParams* mparams, MultiVecDerivId dfId ) { + if (this->isComponentStateInvalid()) + { + return; + } + if (this->mstate1 && this->mstate2) { auto state1 = this->mstate1.get(); @@ -75,6 +84,11 @@ void MixedInteractionForceField::addDForce(const Mechani template SReal MixedInteractionForceField::getPotentialEnergy(const MechanicalParams* mparams) const { + if (this->isComponentStateInvalid()) + { + return 0; + } + if (this->mstate1 && this->mstate2) return getPotentialEnergy(mparams, *mparams->readX(this->mstate1.get()),*mparams->readX(this->mstate2.get())); else return 0; diff --git a/Sofa/framework/Core/src/sofa/core/behavior/PairInteractionConstraint.h b/Sofa/framework/Core/src/sofa/core/behavior/PairInteractionConstraint.h index 78aee67f699..909bbea1c33 100644 --- a/Sofa/framework/Core/src/sofa/core/behavior/PairInteractionConstraint.h +++ b/Sofa/framework/Core/src/sofa/core/behavior/PairInteractionConstraint.h @@ -66,39 +66,16 @@ class PairInteractionConstraint : public BaseInteractionConstraint, public PairS /// /// \param v is the result vector that contains the whole constraints violations /// \param cParams defines the state vectors to use for positions and velocities. Also defines the order of the constraint (POS, VEL, ACC) - void getConstraintViolation(const ConstraintParams* cParams, linearalgebra::BaseVector *v) override; - - /// Construct the Constraint violations vector of each constraint - /// - /// \param v is the result vector that contains the whole constraints violations - /// \param x1 and x2 are the position vectors used to compute contraint position violation - /// \param v1 and v2 are the velocity vectors used to compute contraint velocity violation - /// \param cParams defines the state vectors to use for positions and velocities. Also defines the order of the constraint (POS, VEL, ACC) - /// - /// This is the method that should be implemented by the component - virtual void getConstraintViolation(const ConstraintParams* cParams, linearalgebra::BaseVector *v, const DataVecCoord &x1, const DataVecCoord &x2 - , const DataVecDeriv &v1, const DataVecDeriv &v2) = 0; + void getConstraintViolation(const ConstraintParams* cParams, linearalgebra::BaseVector *v) final; /// Construct the Jacobian Matrix /// /// \param cId is the result constraint sparse matrix Id /// \param cIndex is the index of the next constraint equation: when building the constraint matrix, you have to use this index, and then update it /// \param cParams defines the state vectors to use for positions and velocities. Also defines the order of the constraint (POS, VEL, ACC) - void buildConstraintMatrix(const ConstraintParams* cParams, MultiMatrixDerivId cId, unsigned int &cIndex) override; - - /// Construct the Jacobian Matrix - /// - /// \param c1 and c2 are the results constraint sparse matrix - /// \param cIndex is the index of the next constraint equation: when building the constraint matrix, you have to use this index, and then update it - /// \param x1 and x2 are the position vectors used for contraint equation computation - /// \param cParams defines the state vectors to use for positions and velocities. Also defines the order of the constraint (POS, VEL, ACC) - /// - /// This is the method that should be implemented by the component - virtual void buildConstraintMatrix(const ConstraintParams* cParams, DataMatrixDeriv &c1, DataMatrixDeriv &c2, unsigned int &cIndex - , const DataVecCoord &x1, const DataVecCoord &x2) = 0; - + void buildConstraintMatrix(const ConstraintParams* cParams, MultiMatrixDerivId cId, unsigned int &cIndex) final; - void storeLambda(const ConstraintParams* cParams, MultiVecDerivId res, const sofa::linearalgebra::BaseVector* lambda) override; + void storeLambda(const ConstraintParams* cParams, MultiVecDerivId res, const sofa::linearalgebra::BaseVector* lambda) final; /// Pre-construction check method called by ObjectFactory. /// Check that DataTypes matches the MechanicalState. @@ -157,14 +134,36 @@ class PairInteractionConstraint : public BaseInteractionConstraint, public PairS protected: - virtual type::vector getInteractionIdentifiers() override final + /// Construct the Constraint violations vector of each constraint + /// + /// \param v is the result vector that contains the whole constraints violations + /// \param x1 and x2 are the position vectors used to compute contraint position violation + /// \param v1 and v2 are the velocity vectors used to compute contraint velocity violation + /// \param cParams defines the state vectors to use for positions and velocities. Also defines the order of the constraint (POS, VEL, ACC) + /// + /// This is the method that should be implemented by the component + virtual void getConstraintViolation(const ConstraintParams* cParams, linearalgebra::BaseVector *v, const DataVecCoord &x1, const DataVecCoord &x2 + , const DataVecDeriv &v1, const DataVecDeriv &v2) = 0; + + /// Construct the Jacobian Matrix + /// + /// \param c1 and c2 are the results constraint sparse matrix + /// \param cIndex is the index of the next constraint equation: when building the constraint matrix, you have to use this index, and then update it + /// \param x1 and x2 are the position vectors used for contraint equation computation + /// \param cParams defines the state vectors to use for positions and velocities. Also defines the order of the constraint (POS, VEL, ACC) + /// + /// This is the method that should be implemented by the component + virtual void buildConstraintMatrix(const ConstraintParams* cParams, DataMatrixDeriv &c1, DataMatrixDeriv &c2, unsigned int &cIndex + , const DataVecCoord &x1, const DataVecCoord &x2) = 0; + + virtual type::vector getInteractionIdentifiers() override final { type::vector ids = getPairInteractionIdentifiers(); ids.push_back("Pair"); return ids; } - virtual type::vector getPairInteractionIdentifiers(){ return {}; } + virtual type::vector getPairInteractionIdentifiers(){ return {}; } void storeLambda(const ConstraintParams* cParams, Data& res1, Data& res2, const Data& j1, const Data& j2, const sofa::linearalgebra::BaseVector* lambda); diff --git a/Sofa/framework/Core/src/sofa/core/behavior/PairInteractionConstraint.inl b/Sofa/framework/Core/src/sofa/core/behavior/PairInteractionConstraint.inl index 204b7678e46..ffc2d52c00e 100644 --- a/Sofa/framework/Core/src/sofa/core/behavior/PairInteractionConstraint.inl +++ b/Sofa/framework/Core/src/sofa/core/behavior/PairInteractionConstraint.inl @@ -52,6 +52,11 @@ bool PairInteractionConstraint::isActive() const template void PairInteractionConstraint::getConstraintViolation(const ConstraintParams* cParams, linearalgebra::BaseVector *v) { + if (isComponentStateInvalid()) + { + return; + } + if (cParams) { getConstraintViolation(cParams, v, *cParams->readX(this->mstate1.get()), *cParams->readX(this->mstate2.get()), *cParams->readV(this->mstate1.get()), *cParams->readV(this->mstate2.get())); @@ -62,6 +67,11 @@ void PairInteractionConstraint::getConstraintViolation(const Constrai template void PairInteractionConstraint::buildConstraintMatrix(const ConstraintParams* cParams, MultiMatrixDerivId cId, unsigned int &cIndex) { + if (isComponentStateInvalid()) + { + return; + } + if (cParams) { buildConstraintMatrix(cParams, *cId[this->mstate1.get()].write(), *cId[this->mstate2.get()].write(), cIndex, *cParams->readX(this->mstate1.get()), *cParams->readX(this->mstate2.get())); @@ -71,6 +81,11 @@ void PairInteractionConstraint::buildConstraintMatrix(const Constrain template void PairInteractionConstraint::storeLambda(const ConstraintParams* cParams, MultiVecDerivId res, const sofa::linearalgebra::BaseVector* lambda) { + if (isComponentStateInvalid()) + { + return; + } + if (cParams) { storeLambda(cParams, *res[this->mstate1.get()].write(), *res[this->mstate2.get()].write(), *cParams->readJ(this->mstate1.get()), *cParams->readJ(this->mstate2.get()), lambda); diff --git a/Sofa/framework/Core/src/sofa/core/behavior/PairInteractionForceField.h b/Sofa/framework/Core/src/sofa/core/behavior/PairInteractionForceField.h index 063ecb8dada..9c9c2d4d64f 100644 --- a/Sofa/framework/Core/src/sofa/core/behavior/PairInteractionForceField.h +++ b/Sofa/framework/Core/src/sofa/core/behavior/PairInteractionForceField.h @@ -80,20 +80,7 @@ class PairInteractionForceField : public BaseInteractionForceField, public PairS /// This method retrieves the force, x and v vector from the two MechanicalState /// and call the internal addForce(VecDeriv&,VecDeriv&,const VecCoord&,const VecCoord&,const VecDeriv&,const VecDeriv&) /// method implemented by the component. - void addForce(const MechanicalParams* mparams, MultiVecDerivId fId ) override; - - /// Given the current position and velocity states, update the current force - /// vector by computing and adding the forces associated with this - /// ForceField. - /// - /// If the ForceField can be represented as a matrix, this method computes - /// $ f += B v + K x $ - /// - /// This method must be implemented by the component, and is usually called - /// by the generic ForceField::addForce() method. - - virtual void addForce(const MechanicalParams* mparams, DataVecDeriv& f1, DataVecDeriv& f2, const DataVecCoord& x1, const DataVecCoord& x2, const DataVecDeriv& v1, const DataVecDeriv& v2 )=0; - + void addForce(const MechanicalParams* mparams, MultiVecDerivId fId ) final; /// Compute the force derivative given a small displacement from the /// position and velocity used in the previous call to addForce(). @@ -108,27 +95,7 @@ class PairInteractionForceField : public BaseInteractionForceField, public PairS /// This method retrieves the force and dx vector from the two MechanicalState /// and call the internal addDForce(VecDeriv&,VecDeriv&,const VecDeriv&,const VecDeriv&,SReal,SReal) /// method implemented by the component. - void addDForce(const MechanicalParams* mparams, MultiVecDerivId dfId ) override; - - /// Compute the force derivative given a small displacement from the - /// position and velocity used in the previous call to addForce(). - /// - /// The derivative should be directly derived from the computations - /// done by addForce. Any forces neglected in addDForce will be integrated - /// explicitly (i.e. using its value at the beginning of the timestep). - /// - /// If the ForceField can be represented as a matrix, this method computes - /// $ df += kFactor K dx + bFactor B dx $ - /// - /// This method must be implemented by the component, and is usually called - /// by the generic PairInteractionForceField::addDForce() method. - /// - /// To support old components that implement the deprecated addForce method - /// without scalar coefficients, it defaults to using a temporaty vector to - /// compute $ K dx $ and then manually scaling all values by kFactor. - - virtual void addDForce(const MechanicalParams* mparams, DataVecDeriv& df1, DataVecDeriv& df2, const DataVecDeriv& dx1, const DataVecDeriv& dx2)=0; - + void addDForce(const MechanicalParams* mparams, MultiVecDerivId dfId ) final; /// Get the potential energy associated to this ForceField. /// @@ -140,21 +107,6 @@ class PairInteractionForceField : public BaseInteractionForceField, public PairS /// the component. SReal getPotentialEnergy(const MechanicalParams* mparams) const override; - /// Get the potential energy associated to this ForceField. - /// - /// Used to extimate the total energy of the system by some - /// post-stabilization techniques. - /// - /// This method must be implemented by the component, and is usually called - /// by the generic ForceField::getPotentialEnergy() method. - - virtual SReal getPotentialEnergy(const MechanicalParams* mparams, const DataVecCoord& x1, const DataVecCoord& x2) const=0; - - - - - - /// @} /// Pre-construction check method called by ObjectFactory. @@ -218,6 +170,46 @@ class PairInteractionForceField : public BaseInteractionForceField, public PairS return name; } +protected: + + /// Given the current position and velocity states, update the current force + /// vector by computing and adding the forces associated with this + /// ForceField. + /// + /// If the ForceField can be represented as a matrix, this method computes + /// $ f += B v + K x $ + /// + /// This method must be implemented by the component, and is usually called + /// by the generic ForceField::addForce() method. + virtual void addForce(const MechanicalParams* mparams, DataVecDeriv& f1, DataVecDeriv& f2, const DataVecCoord& x1, const DataVecCoord& x2, const DataVecDeriv& v1, const DataVecDeriv& v2 )=0; + + /// Compute the force derivative given a small displacement from the + /// position and velocity used in the previous call to addForce(). + /// + /// The derivative should be directly derived from the computations + /// done by addForce. Any forces neglected in addDForce will be integrated + /// explicitly (i.e. using its value at the beginning of the timestep). + /// + /// If the ForceField can be represented as a matrix, this method computes + /// $ df += kFactor K dx + bFactor B dx $ + /// + /// This method must be implemented by the component, and is usually called + /// by the generic PairInteractionForceField::addDForce() method. + /// + /// To support old components that implement the deprecated addForce method + /// without scalar coefficients, it defaults to using a temporaty vector to + /// compute $ K dx $ and then manually scaling all values by kFactor. + virtual void addDForce(const MechanicalParams* mparams, DataVecDeriv& df1, DataVecDeriv& df2, const DataVecDeriv& dx1, const DataVecDeriv& dx2)=0; + + /// Get the potential energy associated to this ForceField. + /// + /// Used to extimate the total energy of the system by some + /// post-stabilization techniques. + /// + /// This method must be implemented by the component, and is usually called + /// by the generic ForceField::getPotentialEnergy() method. + virtual SReal getPotentialEnergy(const MechanicalParams* mparams, const DataVecCoord& x1, const DataVecCoord& x2) const=0; + }; #if !defined(SOFA_CORE_BEHAVIOR_PAIRINTERACTIONFORCEFIELD_CPP) diff --git a/Sofa/framework/Core/src/sofa/core/behavior/PairInteractionForceField.inl b/Sofa/framework/Core/src/sofa/core/behavior/PairInteractionForceField.inl index 61e666c2e87..81ca9931f2a 100644 --- a/Sofa/framework/Core/src/sofa/core/behavior/PairInteractionForceField.inl +++ b/Sofa/framework/Core/src/sofa/core/behavior/PairInteractionForceField.inl @@ -48,6 +48,11 @@ PairInteractionForceField::~PairInteractionForceField() template void PairInteractionForceField::addForce(const MechanicalParams* mparams, MultiVecDerivId fId ) { + if (isComponentStateInvalid()) + { + return; + } + auto state1 = this->mstate1.get(); auto state2 = this->mstate2.get(); if (state1 && state2) @@ -63,6 +68,11 @@ void PairInteractionForceField::addForce(const MechanicalParams* mpar template void PairInteractionForceField::addDForce(const MechanicalParams* mparams, MultiVecDerivId dfId ) { + if (isComponentStateInvalid()) + { + return; + } + auto state1 = this->mstate1.get(); auto state2 = this->mstate2.get(); if (state1 && state2) @@ -78,6 +88,11 @@ void PairInteractionForceField::addDForce(const MechanicalParams* mpa template SReal PairInteractionForceField::getPotentialEnergy(const MechanicalParams* mparams) const { + if (isComponentStateInvalid()) + { + return 0; + } + auto state1 = this->mstate1.get(); auto state2 = this->mstate2.get(); if (state1 && state2) diff --git a/Sofa/framework/Core/src/sofa/core/behavior/PairInteractionProjectiveConstraintSet.h b/Sofa/framework/Core/src/sofa/core/behavior/PairInteractionProjectiveConstraintSet.h index 3547b05f9da..fa1f805f209 100644 --- a/Sofa/framework/Core/src/sofa/core/behavior/PairInteractionProjectiveConstraintSet.h +++ b/Sofa/framework/Core/src/sofa/core/behavior/PairInteractionProjectiveConstraintSet.h @@ -71,7 +71,7 @@ class PairInteractionProjectiveConstraintSet : public BaseInteractionProjectiveC /// This method retrieves the dx vector from the MechanicalState and call /// the internal projectResponse(VecDeriv&,VecDeriv&) method implemented by /// the component. - void projectResponse(const MechanicalParams* mparams, MultiVecDerivId dxId) override; + void projectResponse(const MechanicalParams* mparams, MultiVecDerivId dxId) final; /// Project the L matrix of the Lagrange Multiplier equation system. /// @@ -85,23 +85,14 @@ class PairInteractionProjectiveConstraintSet : public BaseInteractionProjectiveC /// This method retrieves the v vector from the MechanicalState and call /// the internal projectVelocity(VecDeriv&,VecDeriv&) method implemented by /// the component. - void projectVelocity(const MechanicalParams* mparams, MultiVecDerivId vId) override; + void projectVelocity(const MechanicalParams* mparams, MultiVecDerivId vId) final; /// Project x to constrained space (x models a position). /// /// This method retrieves the x vector from the MechanicalState and call /// the internal projectPosition(VecCoord&,VecCoord&) method implemented by /// the component. - void projectPosition(const MechanicalParams* mparams, MultiVecCoordId xId) override; - - /// Project dx to constrained space (dx models an acceleration). - virtual void projectResponse(const MechanicalParams* /*mparams*/, DataVecDeriv& dx1, DataVecDeriv& dx2) = 0; - - /// Project v to constrained space (v models a velocity). - virtual void projectVelocity(const MechanicalParams* /*mparams*/, DataVecDeriv& v1, DataVecDeriv& v2) = 0; - - /// Project x to constrained space (x models a position). - virtual void projectPosition(const MechanicalParams* /*mparams*/, DataVecCoord& x1, DataVecCoord& x2) = 0; + void projectPosition(const MechanicalParams* mparams, MultiVecCoordId xId) final; /// @} @@ -171,6 +162,18 @@ class PairInteractionProjectiveConstraintSet : public BaseInteractionProjectiveC using Inherit2::getMechModel1; using Inherit2::getMechModel2; + +protected: + + /// Project dx to constrained space (dx models an acceleration). + virtual void projectResponse(const MechanicalParams* /*mparams*/, DataVecDeriv& dx1, DataVecDeriv& dx2) = 0; + + /// Project v to constrained space (v models a velocity). + virtual void projectVelocity(const MechanicalParams* /*mparams*/, DataVecDeriv& v1, DataVecDeriv& v2) = 0; + + /// Project x to constrained space (x models a position). + virtual void projectPosition(const MechanicalParams* /*mparams*/, DataVecCoord& x1, DataVecCoord& x2) = 0; + }; #if !defined(SOFA_CORE_BEHAVIOR_PAIRINTERACTIONPROJECTIVECONSTRAINTSET_CPP) diff --git a/Sofa/framework/Core/src/sofa/core/behavior/PairInteractionProjectiveConstraintSet.inl b/Sofa/framework/Core/src/sofa/core/behavior/PairInteractionProjectiveConstraintSet.inl index 6ff41998817..85640344a05 100644 --- a/Sofa/framework/Core/src/sofa/core/behavior/PairInteractionProjectiveConstraintSet.inl +++ b/Sofa/framework/Core/src/sofa/core/behavior/PairInteractionProjectiveConstraintSet.inl @@ -61,6 +61,11 @@ void PairInteractionProjectiveConstraintSet::projectJacobianMatrix(co template void PairInteractionProjectiveConstraintSet::projectResponse(const MechanicalParams* mparams, MultiVecDerivId dxId) { + if (isComponentStateInvalid()) + { + return; + } + if( !isActive() ) return; if (this->mstate1 && this->mstate2) { @@ -71,6 +76,11 @@ void PairInteractionProjectiveConstraintSet::projectResponse(const Me template void PairInteractionProjectiveConstraintSet::projectVelocity(const MechanicalParams* mparams, MultiVecDerivId vId) { + if (isComponentStateInvalid()) + { + return; + } + if( !isActive() ) return; if (this->mstate1 && this->mstate2) { @@ -81,6 +91,11 @@ void PairInteractionProjectiveConstraintSet::projectVelocity(const Me template void PairInteractionProjectiveConstraintSet::projectPosition(const MechanicalParams* mparams, MultiVecCoordId xId) { + if (isComponentStateInvalid()) + { + return; + } + if( !isActive() ) return; if (this->mstate1 && this->mstate2) { diff --git a/Sofa/framework/Core/src/sofa/core/behavior/ProjectiveConstraintSet.h b/Sofa/framework/Core/src/sofa/core/behavior/ProjectiveConstraintSet.h index 6374406aa49..fc354d159ea 100644 --- a/Sofa/framework/Core/src/sofa/core/behavior/ProjectiveConstraintSet.h +++ b/Sofa/framework/Core/src/sofa/core/behavior/ProjectiveConstraintSet.h @@ -80,52 +80,28 @@ class ProjectiveConstraintSet : public BaseProjectiveConstraintSet, public Singl /// This method retrieves the dxId vector from the MechanicalState and call /// the internal projectResponse(VecDeriv&) method implemented by /// the component. - void projectResponse(const MechanicalParams* mparams, MultiVecDerivId dxId) override; + void projectResponse(const MechanicalParams* mparams, MultiVecDerivId dxId) final; /// Project the L matrix of the Lagrange Multiplier equation system. /// /// This method retrieves the lines of the Jacobian Matrix from the MechanicalState and call /// the internal projectResponse(MatrixDeriv&) method implemented by /// the component. - void projectJacobianMatrix(const MechanicalParams* mparams, MultiMatrixDerivId cId) override; + void projectJacobianMatrix(const MechanicalParams* mparams, MultiMatrixDerivId cId) final; /// Project v to constrained space (v models a velocity). /// /// This method retrieves the vId vector from the MechanicalState and call /// the internal projectVelocity(VecDeriv&) method implemented by /// the component. - void projectVelocity(const MechanicalParams* mparams, MultiVecDerivId vId) override; + void projectVelocity(const MechanicalParams* mparams, MultiVecDerivId vId) final; /// Project x to constrained space (x models a position). /// /// This method retrieves the xId vector from the MechanicalState and call /// the internal projectPosition(VecCoord&) method implemented by /// the component. - void projectPosition(const MechanicalParams* mparams, MultiVecCoordId xId) override; - - - - /// Project dx to constrained space (dx models an acceleration). - /// - /// This method must be implemented by the component, and is usually called - /// by the generic ProjectiveConstraintSet::projectResponse() method. - virtual void projectResponse(const MechanicalParams* mparams, DataVecDeriv& dx) = 0; - - /// Project v to constrained space (v models a velocity). - /// - /// This method must be implemented by the component, and is usually called - /// by the generic ProjectiveConstraintSet::projectVelocity() method. - virtual void projectVelocity(const MechanicalParams* mparams, DataVecDeriv& v) = 0; - /// Project x to constrained space (x models a position). - /// - /// This method must be implemented by the component, and is usually called - /// by the generic ProjectiveConstraintSet::projectPosition() method. - virtual void projectPosition(const MechanicalParams* mparams, DataVecCoord& x) = 0; - - /// Project c to constrained space (c models a constraint). - /// - /// This method must be implemented by the component to handle Lagrange Multiplier based constraint - virtual void projectJacobianMatrix(const MechanicalParams* /*mparams*/, DataMatrixDeriv& cData) = 0; + void projectPosition(const MechanicalParams* mparams, MultiVecCoordId xId) final; /// @} @@ -155,6 +131,33 @@ class ProjectiveConstraintSet : public BaseProjectiveConstraintSet, public Singl return BaseObject::canCreate(obj, context, arg); } + + +protected: + + /// Project dx to constrained space (dx models an acceleration). + /// + /// This method must be implemented by the component, and is usually called + /// by the generic ProjectiveConstraintSet::projectResponse() method. + virtual void projectResponse(const MechanicalParams* mparams, DataVecDeriv& dx) = 0; + + /// Project v to constrained space (v models a velocity). + /// + /// This method must be implemented by the component, and is usually called + /// by the generic ProjectiveConstraintSet::projectVelocity() method. + virtual void projectVelocity(const MechanicalParams* mparams, DataVecDeriv& v) = 0; + /// Project x to constrained space (x models a position). + /// + /// This method must be implemented by the component, and is usually called + /// by the generic ProjectiveConstraintSet::projectPosition() method. + virtual void projectPosition(const MechanicalParams* mparams, DataVecCoord& x) = 0; + + /// Project c to constrained space (c models a constraint). + /// + /// This method must be implemented by the component to handle Lagrange Multiplier based constraint + virtual void projectJacobianMatrix(const MechanicalParams* /*mparams*/, DataMatrixDeriv& cData) = 0; + + }; #if !defined(SOFA_CORE_BEHAVIOR_PROJECTIVECONSTRAINTSET_CPP) diff --git a/Sofa/framework/Core/src/sofa/core/behavior/ProjectiveConstraintSet.inl b/Sofa/framework/Core/src/sofa/core/behavior/ProjectiveConstraintSet.inl index c2b446d01cb..9a342cab2df 100644 --- a/Sofa/framework/Core/src/sofa/core/behavior/ProjectiveConstraintSet.inl +++ b/Sofa/framework/Core/src/sofa/core/behavior/ProjectiveConstraintSet.inl @@ -50,6 +50,11 @@ bool ProjectiveConstraintSet::isActive() const template void ProjectiveConstraintSet::projectJacobianMatrix(const MechanicalParams* mparams, MultiMatrixDerivId cId) { + if (isComponentStateInvalid()) + { + return; + } + if (!isActive()) return; @@ -64,11 +69,16 @@ void ProjectiveConstraintSet::projectJacobianMatrix(const MechanicalP template void ProjectiveConstraintSet::projectResponse(const MechanicalParams* mparams, MultiVecDerivId dxId) { + if (isComponentStateInvalid()) + { + return; + } + if (!isActive()) return; if (this->mstate) { - projectResponse(mparams, *dxId[this->mstate.get()].write()); + projectResponse(mparams, *dxId[this->mstate.get()].write()); } msg_error_when(!this->mstate) << "ProjectiveConstraintSet::projectResponse(const MechanicalParams* mparams, MultiVecDerivId dxId), no this->mstate for " << this->getName(); } @@ -76,13 +86,17 @@ void ProjectiveConstraintSet::projectResponse(const MechanicalParams* template void ProjectiveConstraintSet::projectVelocity(const MechanicalParams* mparams, MultiVecDerivId vId) { + if (isComponentStateInvalid()) + { + return; + } + if (!isActive()) return; if (this->mstate) { - - projectVelocity(mparams, *vId[this->mstate.get()].write()); + projectVelocity(mparams, *vId[this->mstate.get()].write()); } msg_error_when(!this->mstate) << "ProjectiveConstraintSet::projectVelocity(const MechanicalParams* mparams, MultiVecDerivId dxId), no this->mstate for " << this->getName(); } @@ -90,13 +104,17 @@ void ProjectiveConstraintSet::projectVelocity(const MechanicalParams* template void ProjectiveConstraintSet::projectPosition(const MechanicalParams* mparams, MultiVecCoordId xId) { + if (isComponentStateInvalid()) + { + return; + } + if (!isActive()) return; if (this->mstate) { - - projectPosition(mparams, *xId[this->mstate.get()].write()); + projectPosition(mparams, *xId[this->mstate.get()].write()); } } diff --git a/Sofa/framework/Core/src/sofa/core/objectmodel/Base.h b/Sofa/framework/Core/src/sofa/core/objectmodel/Base.h index 5aa1709efbf..d307440b383 100644 --- a/Sofa/framework/Core/src/sofa/core/objectmodel/Base.h +++ b/Sofa/framework/Core/src/sofa/core/objectmodel/Base.h @@ -369,6 +369,7 @@ class SOFA_CORE_API Base ComponentState getComponentState() const { return d_componentState.getValue() ; } bool isComponentStateValid() const { return d_componentState.getValue() == ComponentState::Valid; } + bool isComponentStateInvalid() const { return d_componentState.getValue() == ComponentState::Invalid; } ///@}