diff --git a/Sofa/Component/Mass/src/sofa/component/mass/DiagonalMass.cpp b/Sofa/Component/Mass/src/sofa/component/mass/DiagonalMass.cpp index 7d159a6ef7a..3c3704d2a35 100644 --- a/Sofa/Component/Mass/src/sofa/component/mass/DiagonalMass.cpp +++ b/Sofa/Component/Mass/src/sofa/component/mass/DiagonalMass.cpp @@ -39,21 +39,17 @@ using namespace sofa::defaulttype; template template -SReal DiagonalMass::getPotentialEnergyRigidImpl( const MechanicalParams* mparams, - const DataVecCoord& x) const +SReal DiagonalMass::getGravitationalPotentialEnergyRigidImpl( const MechanicalParams* mparams, + const DataVecCoord& x, const Deriv& g) const { SOFA_UNUSED(mparams) ; SReal e = 0; const MassVector &masses= d_vertexMass.getValue(); const VecCoord& _x = x.getValue(); - // gravity - Vec3d g ( this->getContext()->getGravity() ); - Deriv theGravity; - RigidTypes::set( theGravity, g[0], g[1], g[2]); for (unsigned int i=0; i<_x.size(); i++) { - e -= getVCenter(theGravity) * masses[i].mass * _x[i].getCenter(); + e -= getVCenter(g) * masses[i].mass * _x[i].getCenter(); } return e; } @@ -65,7 +61,7 @@ void DiagonalMass::drawRigid3dImpl(const VisualPar { const MassVector &masses= d_vertexMass.getValue(); if (!vparams->displayFlags().getShowBehaviorModels()) return; - const VecCoord& x =mstate->read(core::ConstVecCoordId::position())->getValue(); + const VecCoord& x =this->mstate->read(core::ConstVecCoordId::position())->getValue(); if(masses.size() != x.size()) return; @@ -113,7 +109,7 @@ void DiagonalMass::drawRigid2dImpl(const VisualPar { const MassVector &masses= d_vertexMass.getValue(); if (!vparams->displayFlags().getShowBehaviorModels()) return; - const VecCoord& x =mstate->read(core::ConstVecCoordId::position())->getValue(); + const VecCoord& x =this->mstate->read(core::ConstVecCoordId::position())->getValue(); for (unsigned int i=0; i::getMomentumVec3Impl( const template <> -SReal DiagonalMass::getPotentialEnergy( const MechanicalParams* mparams, - const DataVecCoord& x) const +SReal DiagonalMass::getGravitationalPotentialEnergy( const MechanicalParams* mparams, + const DataVecCoord& x, const Deriv& g) const { - return getPotentialEnergyRigidImpl(mparams, x) ; + return getGravitationalPotentialEnergyRigidImpl(mparams, x, g) ; } template <> -SReal DiagonalMass::getPotentialEnergy( const MechanicalParams* mparams, - const DataVecCoord& x) const +SReal DiagonalMass::getGravitationalPotentialEnergy( const MechanicalParams* mparams, + const DataVecCoord& x, const Deriv& g) const { - return getPotentialEnergyRigidImpl(mparams, x) ; + return getGravitationalPotentialEnergyRigidImpl(mparams, x, g) ; } template <> diff --git a/Sofa/Component/Mass/src/sofa/component/mass/DiagonalMass.h b/Sofa/Component/Mass/src/sofa/component/mass/DiagonalMass.h index ab6b1052c81..25ab23e90cd 100644 --- a/Sofa/Component/Mass/src/sofa/component/mass/DiagonalMass.h +++ b/Sofa/Component/Mass/src/sofa/component/mass/DiagonalMass.h @@ -124,7 +124,6 @@ class DiagonalMass : public core::behavior::Mass /// Bring inherited attributes and function in the current lookup context. /// otherwise any access to the base::attribute would require /// the "this->" approach. - using core::behavior::ForceField::mstate ; using core::objectmodel::BaseObject::getContext; //////////////////////////////////////////////////////////////////////////// @@ -303,16 +302,14 @@ class DiagonalMass : public core::behavior::Mass void accFromF(const core::MechanicalParams* mparams, DataVecDeriv& a, const DataVecDeriv& f) override; - void addForce(const core::MechanicalParams* mparams, DataVecDeriv& f, const DataVecCoord& x, const DataVecDeriv& v) override; + void addGravitationalForce(const core::MechanicalParams* mparams, DataVecDeriv& f, const DataVecCoord& x, const DataVecDeriv& v, const Deriv& gravity) override; ///< Mg force in a lumped gravity field - SReal getKineticEnergy(const core::MechanicalParams* mparams, const DataVecDeriv& v) const override; ///< vMv/2 using dof->getV() override + SReal getGravitationalPotentialEnergy(const core::MechanicalParams* mparams, const DataVecCoord& x, const Deriv& gravity) const override; ///< Mgx potential in a uniform gravity field, null at origin - SReal getPotentialEnergy(const core::MechanicalParams* mparams, const DataVecCoord& x) const override; ///< Mgx potential in a uniform gravity field, null at origin + SReal getKineticEnergy(const core::MechanicalParams* mparams, const DataVecDeriv& v) const override; ///< vMv/2 using dof->getV() override type::Vec6 getMomentum(const core::MechanicalParams* mparams, const DataVecCoord& x, const DataVecDeriv& v) const override; ///< (Mv,cross(x,Mv)+Iw) override - 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; @@ -337,8 +334,8 @@ class DiagonalMass : public core::behavior::Mass private: template - SReal getPotentialEnergyRigidImpl( const core::MechanicalParams* mparams, - const DataVecCoord& x) const ; + SReal getGravitationalPotentialEnergyRigidImpl( const core::MechanicalParams* mparams, + const DataVecCoord& x, const Deriv& g) const ; template void drawRigid3dImpl(const core::visual::VisualParams* vparams) ; @@ -363,9 +360,9 @@ class DiagonalMass : public core::behavior::Mass // Specialization for rigids template <> -SReal DiagonalMass::getPotentialEnergy( const core::MechanicalParams* mparams, const DataVecCoord& x) const; +SReal DiagonalMass::getGravitationalPotentialEnergy( const core::MechanicalParams* mparams, const DataVecCoord& x, const Deriv& g) const; template <> -SReal DiagonalMass::getPotentialEnergy( const core::MechanicalParams* mparams, const DataVecCoord& x) const; +SReal DiagonalMass::getGravitationalPotentialEnergy( const core::MechanicalParams* mparams, const DataVecCoord& x, const Deriv& g) const; template <> void DiagonalMass::draw(const core::visual::VisualParams* vparams); template <> diff --git a/Sofa/Component/Mass/src/sofa/component/mass/DiagonalMass.inl b/Sofa/Component/Mass/src/sofa/component/mass/DiagonalMass.inl index 822106d511b..d934d465016 100644 --- a/Sofa/Component/Mass/src/sofa/component/mass/DiagonalMass.inl +++ b/Sofa/Component/Mass/src/sofa/component/mass/DiagonalMass.inl @@ -589,19 +589,17 @@ SReal DiagonalMass::getKineticEnergy( const core::M } template -SReal DiagonalMass::getPotentialEnergy( const core::MechanicalParams* /*mparams*/, const DataVecCoord& x ) const +SReal DiagonalMass::getGravitationalPotentialEnergy( const core::MechanicalParams* mparams, const DataVecCoord& x, const Deriv& gravity ) const { + SOFA_UNUSED(mparams); const MassVector &masses= d_vertexMass.getValue(); helper::ReadAccessor< DataVecCoord > _x = x; SReal e = 0; - // gravity - type::Vec3d g ( this->getContext()->getGravity() ); - Deriv theGravity; - DataTypes::set ( theGravity, g[0], g[1], g[2]); + for (unsigned int i=0; i::doUpdateInternal() printMass(); } - -template -void DiagonalMass::addGravityToV(const core::MechanicalParams* mparams, DataVecDeriv& d_v) -{ - if(mparams) - { - VecDeriv& v = *d_v.beginEdit(); - // gravity - sofa::type::Vec3d g ( this->getContext()->getGravity() ); - Deriv theGravity; - DataTypes::set ( theGravity, g[0], g[1], g[2]); - Deriv hg = theGravity * typename DataTypes::Real(sofa::core::mechanicalparams::dt(mparams)); - - for (unsigned int i=0; i -void DiagonalMass::addForce(const core::MechanicalParams* /*mparams*/, DataVecDeriv& f, const DataVecCoord& , const DataVecDeriv& ) +void DiagonalMass::addGravitationalForce(const core::MechanicalParams* mparams, DataVecDeriv& f, const DataVecCoord& x, const DataVecDeriv& v, const Deriv& gravity) { - //if gravity was added separately (in solver's "solve" method), then nothing to do here - if(this->m_separateGravity.getValue()) - return; + SOFA_UNUSED(mparams); + SOFA_UNUSED(x); + SOFA_UNUSED(v); const MassVector &masses= d_vertexMass.getValue(); helper::WriteOnlyAccessor< DataVecDeriv > _f = f; - // gravity - sofa::type::Vec3d g ( this->getContext()->getGravity() ); - Deriv theGravity; - DataTypes::set ( theGravity, g[0], g[1], g[2]); - - // add weight and inertia force for (unsigned int i=0; i::getMomentum ( const core::MechanicalParams*, con for( unsigned int i=0 ; igetEdges(); @@ -58,14 +58,14 @@ Vec6 MeshMatrixMass::getMomentum ( const core::MechanicalParams*, con const MassType m = edgeMass[i] * static_cast(0.5); Deriv linearMomentum = v[v0] * m; - for( Index j=0 ; j void accFromF(const core::MechanicalParams*, DataVecDeriv& a, const DataVecDeriv& f) override; // This function can't be used as it use M^-1 - void addForce(const core::MechanicalParams*, DataVecDeriv& f, const DataVecCoord& x, const DataVecDeriv& v) override; + void addGravitationalForce(const core::MechanicalParams* mparams, DataVecDeriv& f, const DataVecCoord& x, const DataVecDeriv& v, const Deriv& gravity) override; ///< Mg force in a homogeneous gravity field - SReal getKineticEnergy(const core::MechanicalParams*, const DataVecDeriv& v) const override; ///< vMv/2 using dof->getV() override + SReal getGravitationalPotentialEnergy(const core::MechanicalParams*, const DataVecCoord& x, const Deriv& gravity) const override; ///< Mgx potential in a uniform gravity field, null at origin - SReal getPotentialEnergy(const core::MechanicalParams*, const DataVecCoord& x) const override; ///< Mgx potential in a uniform gravity field, null at origin + SReal getKineticEnergy(const core::MechanicalParams*, const DataVecDeriv& v) const override; ///< vMv/2 using dof->getV() override type::Vec6 getMomentum(const core::MechanicalParams* mparams, const DataVecCoord& x, const DataVecDeriv& v) const override; ///< (Mv,cross(x,Mv)) override - void addGravityToV(const core::MechanicalParams* mparams, DataVecDeriv& d_v) override; - bool isDiagonal() const override { return isLumped(); } diff --git a/Sofa/Component/Mass/src/sofa/component/mass/MeshMatrixMass.inl b/Sofa/Component/Mass/src/sofa/component/mass/MeshMatrixMass.inl index 9ce8c5c2443..b4cce60466a 100644 --- a/Sofa/Component/Mass/src/sofa/component/mass/MeshMatrixMass.inl +++ b/Sofa/Component/Mass/src/sofa/component/mass/MeshMatrixMass.inl @@ -2076,25 +2076,18 @@ void MeshMatrixMass::accFromF(const core::Mechanica template -void MeshMatrixMass::addForce(const core::MechanicalParams*, DataVecDeriv& vf, const DataVecCoord& , const DataVecDeriv& ) +void MeshMatrixMass::addGravitationalForce(const core::MechanicalParams* mparams, DataVecDeriv& vf, const DataVecCoord& x, const DataVecDeriv& v, const Deriv& gravity ) { - - //if gravity was added separately (in solver's "solve" method), then nothing to do here - if(this->m_separateGravity.getValue()) - return ; + SOFA_UNUSED(mparams); + SOFA_UNUSED(x); + SOFA_UNUSED(v); helper::WriteAccessor< DataVecDeriv > f = vf; - const auto &vertexMass= d_vertexMass.getValue(); - // gravity - type::Vec3d g ( this->getContext()->getGravity() ); - Deriv theGravity; - DataTypes::set ( theGravity, g[0], g[1], g[2]); - // add weight and inertia force for (unsigned int i=0; i::getKineticEnergy( const core: template -SReal MeshMatrixMass::getPotentialEnergy( const core::MechanicalParams*, const DataVecCoord& vx) const +SReal MeshMatrixMass::getGravitationalPotentialEnergy( const core::MechanicalParams* mparams, const DataVecCoord& vx, const Deriv& gravity) const { + SOFA_UNUSED(mparams); + const auto &vertexMass= d_vertexMass.getValue(); helper::ReadAccessor< DataVecCoord > x = vx; - SReal e = 0; - // gravity - type::Vec3d g ( this->getContext()->getGravity() ); - Deriv theGravity; - DataTypes::set ( theGravity, g[0], g[1], g[2]); for (unsigned int i=0; i::getMomentum ( const core } - -template -void MeshMatrixMass::addGravityToV(const core::MechanicalParams* mparams, DataVecDeriv& d_v) -{ - if(this->mstate && mparams) - { - VecDeriv& v = *d_v.beginEdit(); - - // gravity - type::Vec3d g ( this->getContext()->getGravity() ); - Deriv theGravity; - DataTypes::set ( theGravity, g[0], g[1], g[2]); - Deriv hg = theGravity * (typename DataTypes::Real(sofa::core::mechanicalparams::dt(mparams))); - - for (unsigned int i=0; i void MeshMatrixMass::addMToMatrix(const core::MechanicalParams *mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) { diff --git a/Sofa/Component/Mass/src/sofa/component/mass/UniformMass.cpp b/Sofa/Component/Mass/src/sofa/component/mass/UniformMass.cpp index 76fabb8aa00..76c844e9a83 100644 --- a/Sofa/Component/Mass/src/sofa/component/mass/UniformMass.cpp +++ b/Sofa/Component/Mass/src/sofa/component/mass/UniformMass.cpp @@ -197,7 +197,7 @@ void UniformMass::loadFromFileRigidImpl(const string& filename) } setMass(m); } - else if (d_totalMass.getValue()>0 && mstate!=nullptr) d_vertexMass.setValue((Real)d_totalMass.getValue() / mstate->getSize()); + else if (d_totalMass.getValue()>0 && this->mstate!=nullptr) d_vertexMass.setValue((Real)d_totalMass.getValue() / this->mstate->getSize()); } @@ -208,7 +208,7 @@ void UniformMass::drawRigid2DImpl(const VisualParams* vparams) if (!vparams->displayFlags().getShowBehaviorModels()) return; - const VecCoord& x =mstate->read(core::ConstVecCoordId::position())->getValue(); + const VecCoord& x =this->mstate->read(core::ConstVecCoordId::position())->getValue(); ReadAccessor > indices = d_indices; type::Vec3d len; @@ -231,7 +231,7 @@ void UniformMass::drawRigid3DImpl(const VisualParams* vparams) if (!vparams->displayFlags().getShowBehaviorModels()) return; - const VecCoord& x =mstate->read(core::ConstVecCoordId::position())->getValue(); + const VecCoord& x =this->mstate->read(core::ConstVecCoordId::position())->getValue(); ReadAccessor > indices = d_indices; typename RigidTypes::Vec3 gravityCenter; type::Vec3d len; @@ -261,7 +261,7 @@ void UniformMass::drawRigid3DImpl(const VisualParams* vparams) if (d_showInitialCenterOfGravity.getValue()) { - const VecCoord& x0 = mstate->read(core::ConstVecCoordId::restPosition())->getValue(); + const VecCoord& x0 = this->mstate->read(core::ConstVecCoordId::restPosition())->getValue(); for (unsigned int i=0; idrawTool()->drawFrame(x0[indices[i]].getCenter(), x0[indices[i]].getOrientation(), len*d_showAxisSize.getValue()); @@ -281,8 +281,8 @@ void UniformMass::drawVec6Impl(const core::visual::VisualParams* vpar { if (!vparams->displayFlags().getShowBehaviorModels()) return; - const VecCoord& x =mstate->read(core::ConstVecCoordId::position())->getValue(); - const VecCoord& x0 = mstate->read(core::ConstVecCoordId::restPosition())->getValue(); + const VecCoord& x =this->mstate->read(core::ConstVecCoordId::position())->getValue(); + const VecCoord& x0 = this->mstate->read(core::ConstVecCoordId::restPosition())->getValue(); ReadAccessor > indices = d_indices; Mat3x3d R; R.identity(); @@ -375,45 +375,20 @@ Vec6 UniformMass::getMomentumVec3DImpl ( const MechanicalParams*, template template -SReal UniformMass::getPotentialEnergyRigidImpl(const core::MechanicalParams* mparams, - const DataVecCoord& p_x) const +SReal UniformMass::getGravitationalPotentialEnergyRigidImpl(const core::MechanicalParams* mparams, + const DataVecCoord& p_x, const Deriv& g) const { SOFA_UNUSED(mparams) ; SReal e = 0; ReadAccessor< DataVecCoord > x = p_x; ReadAccessor > indices = d_indices; - typename Coord::Pos g ( getContext()->getGravity() ); for (unsigned int i=0; i -template -void UniformMass::addMDxToVectorVecImpl(linearalgebra::BaseVector *resVect, - const VecDeriv* dx, - SReal mFact, - unsigned int& offset) -{ - unsigned int derivDim = (unsigned)Deriv::size(); - double m = d_vertexMass.getValue(); - - ReadAccessor > indices = d_indices; - - const SReal* g = getContext()->getGravity().ptr(); - - for (unsigned int i=0; iadd(offset + indices[i] * derivDim + j, mFact * m * g[j] * (*dx)[indices[i]][0]); - else - resVect->add(offset + indices[i] * derivDim + j, mFact * m * g[j]); - } -} - template<> SOFA_COMPONENT_MASS_API void UniformMass::constructor_message() @@ -455,17 +430,17 @@ void UniformMass::draw(const VisualParams* vparams) } template <> SOFA_COMPONENT_MASS_API -SReal UniformMass::getPotentialEnergy( const MechanicalParams* params, - const DataVecCoord& d_x ) const +SReal UniformMass::getGravitationalPotentialEnergy( const MechanicalParams* params, + const DataVecCoord& d_x, const Deriv& g ) const { - return getPotentialEnergyRigidImpl(params, d_x) ; + return getGravitationalPotentialEnergyRigidImpl(params, d_x, g) ; } template <> SOFA_COMPONENT_MASS_API -SReal UniformMass::getPotentialEnergy( const MechanicalParams* params, - const DataVecCoord& vx ) const +SReal UniformMass::getGravitationalPotentialEnergy( const MechanicalParams* params, + const DataVecCoord& vx, const Deriv& g ) const { - return getPotentialEnergyRigidImpl(params, vx) ; + return getGravitationalPotentialEnergyRigidImpl(params, vx, g) ; } template <> SOFA_COMPONENT_MASS_API @@ -474,15 +449,6 @@ void UniformMass::draw(const core::visual::VisualParams* vparams) drawVec6Impl(vparams) ; } -template <> SOFA_COMPONENT_MASS_API -void UniformMass::addMDxToVector(linearalgebra::BaseVector *resVect, - const VecDeriv* dx, - SReal mFact, - unsigned int& offset) -{ - addMDxToVectorVecImpl(resVect, dx,mFact,offset) ; -} - template <> SOFA_COMPONENT_MASS_API Vec6 UniformMass::getMomentum ( const MechanicalParams* params, const DataVecCoord& d_x, diff --git a/Sofa/Component/Mass/src/sofa/component/mass/UniformMass.h b/Sofa/Component/Mass/src/sofa/component/mass/UniformMass.h index 6dae712e814..001811b0ff4 100644 --- a/Sofa/Component/Mass/src/sofa/component/mass/UniformMass.h +++ b/Sofa/Component/Mass/src/sofa/component/mass/UniformMass.h @@ -83,7 +83,6 @@ class UniformMass : public core::behavior::Mass /// Bring inherited attributes and function in the current lookup context. /// otherwise any access to the base::attribute would require /// the "this->" approach. - using core::behavior::ForceField::mstate ; using core::objectmodel::BaseObject::getContext; //////////////////////////////////////////////////////////////////////////// @@ -132,15 +131,15 @@ class UniformMass : public core::behavior::Mass void addMDx(const core::MechanicalParams* mparams, DataVecDeriv& f, const DataVecDeriv& dx, SReal factor) override; void accFromF(const core::MechanicalParams* mparams, DataVecDeriv& a, const DataVecDeriv& f) override; - void addForce(const core::MechanicalParams* mparams, DataVecDeriv& f, const DataVecCoord& x, const DataVecDeriv& v) override; + + void addGravitationalForce(const core::MechanicalParams* mparams, DataVecDeriv& f, const DataVecCoord& x, const DataVecDeriv& v, const Deriv& gravity) override; ///< Mg force in a uniform gravity field + SReal getGravitationalPotentialEnergy(const core::MechanicalParams* mparams, const DataVecCoord& x, const Deriv& gravity) const override; ///< Mgx potential in a uniform gravity field, null at origin SReal getKineticEnergy(const core::MechanicalParams* mparams, const DataVecDeriv& d_v) const override; ///< vMv/2 using dof->getV() override - SReal getPotentialEnergy(const core::MechanicalParams* mparams, const DataVecCoord& x) const override; ///< Mgx potential in a uniform gravity field, null at origin type::Vec6 getMomentum(const core::MechanicalParams* mparams, const DataVecCoord& x, const DataVecDeriv& v) const override; ///< (Mv,cross(x,Mv)+Iw) override - void addMDxToVector(linearalgebra::BaseVector *resVect, const VecDeriv *dx, SReal mFact, unsigned int& offset); - - void addGravityToV(const core::MechanicalParams* mparams, DataVecDeriv& d_v) override; + SOFA_ATTRIBUTE_DISABLED("v22.12 (PR#2988)", "v23.12", "Removing the addMDxToVector function related to the gravity") + void addMDxToVector(linearalgebra::BaseVector *resVect, const VecDeriv *dx, SReal mFact, unsigned int& offset) = delete; void addMToMatrix(const core::MechanicalParams *mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) override; /// Add Mass contribution to global Matrix assembling @@ -170,8 +169,8 @@ class UniformMass : public core::behavior::Mass void drawVec6Impl(const core::visual::VisualParams* vparams) ; template - SReal getPotentialEnergyRigidImpl(const core::MechanicalParams* mparams, - const DataVecCoord& x) const; ///< Mgx potential in a uniform gravity field, null at origin + SReal getGravitationalPotentialEnergyRigidImpl(const core::MechanicalParams* mparams, + const DataVecCoord& x, const Deriv& g) const; ///< Mgx potential in a uniform gravity field, null at origin @@ -187,12 +186,6 @@ class UniformMass : public core::behavior::Mass template void loadFromFileRigidImpl(const std::string& filename) ; - template - void addMDxToVectorVecImpl(linearalgebra::BaseVector *resVect, - const VecDeriv* dx, - SReal mFact, - unsigned int& offset); - }; // Specialization for rigids @@ -205,9 +198,9 @@ void UniformMass::draw(const core::visual::VisualParam template <> void UniformMass::draw(const core::visual::VisualParams* vparams); template <> -SReal UniformMass::getPotentialEnergy ( const core::MechanicalParams*, const DataVecCoord& x ) const; +SReal UniformMass::getGravitationalPotentialEnergy ( const core::MechanicalParams*, const DataVecCoord& x, const Deriv& g ) const; template <> -SReal UniformMass::getPotentialEnergy ( const core::MechanicalParams*, const DataVecCoord& x ) const; +SReal UniformMass::getGravitationalPotentialEnergy ( const core::MechanicalParams*, const DataVecCoord& x, const Deriv& g ) const; template <> void UniformMass::draw(const core::visual::VisualParams* vparams); diff --git a/Sofa/Component/Mass/src/sofa/component/mass/UniformMass.inl b/Sofa/Component/Mass/src/sofa/component/mass/UniformMass.inl index 2be5c83e440..907773a54ce 100644 --- a/Sofa/Component/Mass/src/sofa/component/mass/UniformMass.inl +++ b/Sofa/Component/Mass/src/sofa/component/mass/UniformMass.inl @@ -135,12 +135,10 @@ void UniformMass::initDefaultImpl() WriteAccessor > indices = d_indices; - if(mstate==nullptr) + if(this->mstate==nullptr) { - msg_warning(this) << "Missing mechanical state. \n" - "UniformMass need to be used with an object also having a MechanicalState. \n" - "To remove this warning: add a to the parent node of the one \n" - " containing this "; + msg_warning(this) << "Missing mechanical state. UniformMass need to be used with an object also having a MechanicalState." << msgendl + << "To remove this warning: add a MechanicalObject to the parent node of the one containing this UniformMass"; return; } @@ -152,7 +150,7 @@ void UniformMass::initDefaultImpl() //If localRange is set, update indices if (d_localRange.getValue()[0] >= 0 && d_localRange.getValue()[1] > 0 - && d_localRange.getValue()[1] + 1 < int(mstate->getSize())) + && d_localRange.getValue()[1] + 1 < int(this->mstate->getSize())) { indices.clear(); for(int i=d_localRange.getValue()[0]; i<=d_localRange.getValue()[1]; i++) @@ -163,7 +161,7 @@ void UniformMass::initDefaultImpl() if(indices.size()==0) { indices.clear(); - for(int i=0; igetSize()); i++) + for(int i=0; imstate->getSize()); i++) indices.push_back(i); } @@ -427,63 +425,19 @@ void UniformMass::accFromF ( const core::MechanicalParams*, a[i] = f[i] / m; } - template -void UniformMass::addMDxToVector ( BaseVector * resVect, - const VecDeriv* dx, - SReal mFact, - unsigned int& offset ) +void UniformMass::addGravitationalForce ( const core::MechanicalParams* mparams, DataVecDeriv& vf, const DataVecCoord& x, const DataVecDeriv& v, const Deriv& gravity) { - SOFA_UNUSED(resVect); - SOFA_UNUSED(dx); - SOFA_UNUSED(mFact); - SOFA_UNUSED(offset); -} - - -template -void UniformMass::addGravityToV(const MechanicalParams* mparams, - DataVecDeriv& d_v) -{ - if (mparams) - { - VecDeriv& v = *d_v.beginEdit(); - - const SReal* g = getContext()->getGravity().ptr(); - Deriv theGravity; - DataTypes::set ( theGravity, g[0], g[1], g[2] ); - Deriv hg = theGravity * Real(sofa::core::mechanicalparams::dt(mparams)); - - dmsg_info()<< " addGravityToV hg = "< -void UniformMass::addForce ( const core::MechanicalParams*, DataVecDeriv& vf, const DataVecCoord& /*x*/, const DataVecDeriv& /*v*/ ) -{ - //if gravity was added separately (in solver's "solve" method), then nothing to do here - if ( this->m_separateGravity.getValue() ) - return; - - helper::WriteAccessor f = vf; + SOFA_UNUSED(mparams); + SOFA_UNUSED(x); + SOFA_UNUSED(v); // weight - const SReal* g = getContext()->getGravity().ptr(); - Deriv theGravity; - DataTypes::set - ( theGravity, g[0], g[1], g[2] ); const MassType& m = d_vertexMass.getValue(); - Deriv mg = theGravity * m; - - dmsg_info() <<" addForce, mg = "< -namespace sofa::component::contextobject -{ - -using namespace sofa::type; -using namespace core::behavior; +#include +#include -Gravity::Gravity() - : f_gravity( initData(&f_gravity,Vec3(0,0,0),"gravity","Gravity in the world coordinate system") ) +namespace sofa::component::mechanicalload { -} -void Gravity::apply() -{ - getContext()->setGravity( f_gravity.getValue() ); -} +using namespace sofa::defaulttype; -int GravityClass = core::RegisterObject("Gravity in world coordinates") - .add< Gravity >() +int GravityForceFieldClass = core::RegisterObject("Gravity as an external force in the world coordinate system") + .add< GravityForceField >() + .add< GravityForceField >() + .add< GravityForceField >() + .add< GravityForceField >() + .add< GravityForceField >() + .add< GravityForceField >() ; -} // namespace sofa::component::contextobject +template class SOFA_CORE_API GravityForceField; +template class SOFA_CORE_API GravityForceField; +template class SOFA_CORE_API GravityForceField; +template class SOFA_CORE_API GravityForceField; +template class SOFA_CORE_API GravityForceField; +template class SOFA_CORE_API GravityForceField; + +} // namespace sofa::component::mechanicalload diff --git a/Sofa/Component/MechanicalLoad/src/sofa/component/mechanicalload/GravityForceField.h b/Sofa/Component/MechanicalLoad/src/sofa/component/mechanicalload/GravityForceField.h new file mode 100644 index 00000000000..a33d3a4cc38 --- /dev/null +++ b/Sofa/Component/MechanicalLoad/src/sofa/component/mechanicalload/GravityForceField.h @@ -0,0 +1,91 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once + +#include +#include +#include + +namespace sofa::component::mechanicalload +{ + +/// ForceField applying the external force due to the gravitational acceleration. +/// This class requires a link towards a Mass to compute the space integration with the mass density. +template +class GravityForceField : public core::behavior::ForceField +{ +public: + SOFA_CLASS(SOFA_TEMPLATE(GravityForceField, DataTypes), SOFA_TEMPLATE(core::behavior::ForceField, DataTypes)); + + typedef core::behavior::ForceField Inherit; + typedef typename DataTypes::VecCoord VecCoord; + typedef typename DataTypes::VecDeriv VecDeriv; + typedef typename DataTypes::Coord Coord; + typedef typename DataTypes::Deriv Deriv; + typedef typename DataTypes::DPos DPos; + typedef typename Coord::value_type Real; + typedef core::objectmodel::Data DataVecCoord; + typedef core::objectmodel::Data DataVecDeriv; + + Data< DPos > d_gravitationalAcceleration; ///< Value corresponding to the gravitational acceleration + Data< type::Vec3 > d_gravity; ///< Vector3 which can be linked to the gravity data of Node + SingleLink, sofa::core::behavior::Mass, BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_mass; ///< Link to be set to the mass in the component graph + + /// Init function + void init() override; + + /// Add the external forces due to the gravitational acceleration. + void addForce (const core::MechanicalParams* params, DataVecDeriv& f, const DataVecCoord& x, const DataVecDeriv& v) override; + + /// Gravity force has null variation + void addDForce(const core::MechanicalParams* mparams, DataVecDeriv& d_df , const DataVecDeriv& d_dx) override; + + /// Gravity force has null variation + void addKToMatrix(sofa::linearalgebra::BaseMatrix *mat, SReal k, unsigned int &offset) override; + + SReal getPotentialEnergy(const core::MechanicalParams* params, const DataVecCoord& x) const override; + + /// Set the gravitational acceleration + void setGravitationalAcceleration(const DPos grav); + + /// Check the norm of the gravity + void checkGravityNorm(); + + /// Update gravitational acceleration from the gravity defined in root node + void setGravityFromRootNode(); + +protected: + GravityForceField(); + bool m_isNormNull = false; +}; + + +#if !defined(SOFA_COMPONENT_FORCEFIELD_GRAVITYFORCEFIELD_CPP) +extern template class SOFA_CORE_API GravityForceField; +extern template class SOFA_CORE_API GravityForceField; +extern template class SOFA_CORE_API GravityForceField; +extern template class SOFA_CORE_API GravityForceField; +extern template class SOFA_CORE_API GravityForceField; +extern template class SOFA_CORE_API GravityForceField; +#endif + +} // namespace sofa::component::mechanicalload diff --git a/Sofa/Component/MechanicalLoad/src/sofa/component/mechanicalload/GravityForceField.inl b/Sofa/Component/MechanicalLoad/src/sofa/component/mechanicalload/GravityForceField.inl new file mode 100644 index 00000000000..35b7a19520a --- /dev/null +++ b/Sofa/Component/MechanicalLoad/src/sofa/component/mechanicalload/GravityForceField.inl @@ -0,0 +1,188 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once + +#include "GravityForceField.h" +#include + +namespace sofa::component::mechanicalload +{ + + +using namespace sofa::type; + + +template +GravityForceField::GravityForceField() + : d_gravitationalAcceleration(initData(&d_gravitationalAcceleration, "gravitationalAcceleration", "Value corresponding to the gravitational acceleration")) + , d_gravity(initData(&d_gravity, "gravity", "Vector3 which can be linked to the gravity data of Node")) + , l_mass(initLink("mass", "link to the mass")) +{ + // To avoid confusion, the data "d_gravity" used for the automatic creation of GravityForceField when using the Node gravity is hidden to the user + d_gravity.setDisplayed(false); + + this->addUpdateCallback("connnectToGravity", { &d_gravity}, [this](const core::DataTracker& t) + { + SOFA_UNUSED(t); + + setGravityFromRootNode(); + checkGravityNorm(); + + return sofa::core::objectmodel::ComponentState::Valid; + }, {}); +} + + +template +void GravityForceField::init() +{ + this->d_componentState.setValue(sofa::core::objectmodel::ComponentState::Invalid); + + if (l_mass.empty()) + { + msg_info() << "link to the mass should be set to ensure right behavior. First mass found in current context will be used."; + sofa::core::behavior::Mass* p_mass; + this->getContext()->get(p_mass); + l_mass.set(p_mass); + } + + // Check if several GravityForceField in the current node + std::vector*> gravityFFVector; + this->getContext()->template get(&gravityFFVector, core::objectmodel::BaseContext::Local); + if(gravityFFVector.size()>1) + { + msg_warning() << "Several gravities seem to be defined in node " << this->getContext()->getName(); + } + + // Link to the mass component + if (sofa::core::behavior::Mass* _mass = l_mass.get()) + { + msg_info() << "Mass path used: '" << l_mass.getLinkedPath() << "'"; + } + else + { + msg_error() << "No Mass component with template "<< this->getTemplateName() <<" found in current context: " << this->getContext()->name << ", nor any valid link to a Mass was given. No gravity will be applied."; + return; + } + + // Check if norm is null + checkGravityNorm(); + + // init from ForceField + Inherit::init(); + + // if all init passes, component is valid + this->d_componentState.setValue(sofa::core::objectmodel::ComponentState::Valid); +} + + +template +void GravityForceField::setGravityFromRootNode() +{ + const Vec3& gravityRootNode = d_gravity.getValue(); + auto gravity = sofa::helper::getWriteAccessor(d_gravitationalAcceleration); + for(Size i=0; ( i +void GravityForceField::setGravitationalAcceleration(const DPos grav) +{ + d_gravitationalAcceleration.setValue(grav); +} + + +template +void GravityForceField::checkGravityNorm() +{ + const DPos & gravity = d_gravitationalAcceleration.getValue(); + const Real norm = gravity.norm(); + + if(norm == 0.0) + { + m_isNormNull = true; + msg_info() << "Gravitational acceleration is null"; + } + else + { + m_isNormNull = false; + } +} + + +template +void GravityForceField::addForce(const core::MechanicalParams* params, DataVecDeriv& f, const DataVecCoord& x1, const DataVecDeriv& v1) +{ + if(this->d_componentState.getValue() == core::objectmodel::ComponentState::Invalid) + return; + + sofa::core::behavior::Mass* _mass = l_mass.get(); + Deriv gravity; + DataTypes::setDPos(gravity, d_gravitationalAcceleration.getValue()); + + if(m_isNormNull) + return; + + _mass->addGravitationalForce(params,f,x1,v1,gravity); +} + + +template +SReal GravityForceField::getPotentialEnergy(const core::MechanicalParams* params, const DataVecCoord& x) const +{ + sofa::core::behavior::Mass* _mass = l_mass.get(); + Deriv gravity; + DataTypes::setDPos(gravity, d_gravitationalAcceleration.getValue()); + + if(_mass && !m_isNormNull) + return _mass->getGravitationalPotentialEnergy(params, x, gravity); + else + return 0.0; +} + + +template +void GravityForceField::addDForce(const core::MechanicalParams* mparams, DataVecDeriv& d_df , const DataVecDeriv& d_dx) +{ + // Derivative of a constant gravity field is null, no need to compute addKToMatrix nor addDForce + SOFA_UNUSED(mparams); + SOFA_UNUSED(d_df); + SOFA_UNUSED(d_dx); + mparams->setKFactorUsed(true); +} + + +template +void GravityForceField::addKToMatrix(sofa::linearalgebra::BaseMatrix * mat, SReal k, unsigned int & offset) +{ + // Derivative of a constant gravity field is null, no need to compute addKToMatrix nor addDForce + SOFA_UNUSED(mat); + SOFA_UNUSED(k); + SOFA_UNUSED(offset); +} + + + +} // namespace sofa::component::mechanicalload diff --git a/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/EulerImplicitSolver.cpp b/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/EulerImplicitSolver.cpp index 60f1f03db35..4ea34d3acae 100644 --- a/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/EulerImplicitSolver.cpp +++ b/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/EulerImplicitSolver.cpp @@ -272,8 +272,6 @@ void EulerImplicitSolver::solve(const core::ExecParams* params, SReal dt, sofa:: } #endif - mop.addSeparateGravity(dt, newVel); // v += dt*g . Used if mass wants to add G separately from the other forces to v - if (f_velocityDamping.getValue()!=0.0) newVel *= exp(-h*f_velocityDamping.getValue()); diff --git a/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/NewmarkImplicitSolver.cpp b/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/NewmarkImplicitSolver.cpp index 0d4b9818b8f..9bb1e3f763e 100644 --- a/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/NewmarkImplicitSolver.cpp +++ b/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/NewmarkImplicitSolver.cpp @@ -165,7 +165,6 @@ void NewmarkImplicitSolver::solve(const core::ExecParams* params, SReal dt, sofa #endif - mop.addSeparateGravity(dt, newVel); // v += dt*g . Used if mass wants to add G separately from the other forces to v. if (d_velocityDamping.getValue()!=0.0) newVel *= exp(-h*d_velocityDamping.getValue()); diff --git a/Sofa/Component/ODESolver/Forward/src/sofa/component/odesolver/forward/CentralDifferenceSolver.cpp b/Sofa/Component/ODESolver/Forward/src/sofa/component/odesolver/forward/CentralDifferenceSolver.cpp index bc1c3fd6542..80a6844c8c9 100644 --- a/Sofa/Component/ODESolver/Forward/src/sofa/component/odesolver/forward/CentralDifferenceSolver.cpp +++ b/Sofa/Component/ODESolver/Forward/src/sofa/component/odesolver/forward/CentralDifferenceSolver.cpp @@ -87,8 +87,6 @@ void CentralDifferenceSolver::solve(const core::ExecParams* params, SReal dt, so const SReal r = f_rayleighMass.getValue(); - mop.addSeparateGravity(dt); // v += dt*g . Used if mass wants to added G separately from the other forces to v. - //projectVelocity(vel); // initial velocities are projected to the constrained space // compute the current force diff --git a/Sofa/Component/ODESolver/Forward/src/sofa/component/odesolver/forward/EulerSolver.cpp b/Sofa/Component/ODESolver/Forward/src/sofa/component/odesolver/forward/EulerSolver.cpp index 643c38cb12a..b1933a9baf2 100644 --- a/Sofa/Component/ODESolver/Forward/src/sofa/component/odesolver/forward/EulerSolver.cpp +++ b/Sofa/Component/ODESolver/Forward/src/sofa/component/odesolver/forward/EulerSolver.cpp @@ -73,7 +73,6 @@ void EulerExplicitSolver::solve(const core::ExecParams* params, acc.realloc(&vop, !d_threadSafeVisitor.getValue(), true); - addSeparateGravity(&mop, dt, vResult); computeForce(&mop, f); SReal nbNonDiagonalMasses = 0; @@ -260,16 +259,6 @@ void EulerExplicitSolver::parse(sofa::core::objectmodel::BaseObjectDescription* } } -void EulerExplicitSolver::addSeparateGravity(sofa::simulation::common::MechanicalOperations* mop, SReal dt, core::MultiVecDerivId v) -{ - sofa::helper::ScopedAdvancedTimer timer("addSeparateGravity"); - - /// Calls the "addGravityToV" method of every BaseMass objects found in the current - /// context tree, if the BaseMass object has the m_separateGravity flag set to true. - /// The method "addGravityToV" usually performs v += dt * g - mop->addSeparateGravity(dt, v); -} - void EulerExplicitSolver::computeForce(sofa::simulation::common::MechanicalOperations* mop, core::MultiVecDerivId f) { sofa::helper::ScopedAdvancedTimer timer("ComputeForce"); diff --git a/Sofa/Component/ODESolver/Forward/src/sofa/component/odesolver/forward/EulerSolver.h b/Sofa/Component/ODESolver/Forward/src/sofa/component/odesolver/forward/EulerSolver.h index 6e371eab84f..58b05b30d0c 100644 --- a/Sofa/Component/ODESolver/Forward/src/sofa/component/odesolver/forward/EulerSolver.h +++ b/Sofa/Component/ODESolver/Forward/src/sofa/component/odesolver/forward/EulerSolver.h @@ -107,10 +107,6 @@ class SOFA_COMPONENT_ODESOLVER_FORWARD_API EulerExplicitSolver : public sofa::co const sofa::core::behavior::MultiVecDeriv& acc, SReal dt) const; - /// Gravity times time step size is added to the velocity for some masses - /// v += g * dt - static void addSeparateGravity(sofa::simulation::common::MechanicalOperations* mop, SReal dt, core::MultiVecDerivId v); - /// Assemble the force vector (right-hand side of the equation) static void computeForce(sofa::simulation::common::MechanicalOperations* mop, core::MultiVecDerivId f); diff --git a/Sofa/Component/ODESolver/Forward/src/sofa/component/odesolver/forward/RungeKutta2Solver.cpp b/Sofa/Component/ODESolver/Forward/src/sofa/component/odesolver/forward/RungeKutta2Solver.cpp index 630e39b069f..7633f15b905 100644 --- a/Sofa/Component/ODESolver/Forward/src/sofa/component/odesolver/forward/RungeKutta2Solver.cpp +++ b/Sofa/Component/ODESolver/Forward/src/sofa/component/odesolver/forward/RungeKutta2Solver.cpp @@ -57,8 +57,6 @@ void RungeKutta2Solver::solve(const core::ExecParams* params, SReal dt, sofa::co SReal startTime = this->getTime(); - mop.addSeparateGravity(dt); // v += dt*g . Used if mass wants to added G separately from the other forces to v. - // Compute state derivative. vel is the derivative of pos mop.computeAcc (startTime, acc, pos, vel); // acc is the derivative of vel diff --git a/Sofa/Component/ODESolver/Forward/src/sofa/component/odesolver/forward/RungeKutta4Solver.cpp b/Sofa/Component/ODESolver/Forward/src/sofa/component/odesolver/forward/RungeKutta4Solver.cpp index 16b72a64bcf..b41242d71ed 100644 --- a/Sofa/Component/ODESolver/Forward/src/sofa/component/odesolver/forward/RungeKutta4Solver.cpp +++ b/Sofa/Component/ODESolver/Forward/src/sofa/component/odesolver/forward/RungeKutta4Solver.cpp @@ -69,8 +69,6 @@ void RungeKutta4Solver::solve(const core::ExecParams* params, SReal dt, sofa::co SReal startTime = this->getTime(); - mop.addSeparateGravity(dt); // v += dt*g . Used if mass wants to added G separately from the other forces to v. - //First step dmsg_info() << "RK4 Step 1"; 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 60583d0f130..fd644ba9917 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 @@ -33,20 +33,23 @@ namespace sofa::component::solidmechanics::fem::elastic /** Compute Finite Element forces based on hexahedral elements including continuum mass matrices */ -template -class HexahedralFEMForceFieldAndMass : virtual public sofa::core::behavior::Mass, virtual public HexahedralFEMForceField +template +class HexahedralFEMForceFieldAndMass : virtual public sofa::core::behavior::Mass, virtual public HexahedralFEMForceField { public: - SOFA_CLASS2(SOFA_TEMPLATE(HexahedralFEMForceFieldAndMass,DataTypes), SOFA_TEMPLATE(sofa::core::behavior::Mass,DataTypes), SOFA_TEMPLATE(HexahedralFEMForceField,DataTypes)); - typedef HexahedralFEMForceField HexahedralFEMForceFieldT; - typedef sofa::core::behavior::Mass MassT; + typedef HexahedralFEMForceField HexahedralFEMForceFieldT; + typedef sofa::core::behavior::Mass MassT; + typedef typename HexahedralFEMForceFieldT::DataTypes HexaFEMTDataTypes; + typedef typename MassT::DataTypes MassTDataTypes; - typedef typename DataTypes::VecCoord VecCoord; - typedef typename DataTypes::VecDeriv VecDeriv; + SOFA_CLASS2(SOFA_TEMPLATE(HexahedralFEMForceFieldAndMass,TDataTypes), SOFA_TEMPLATE(sofa::core::behavior::Mass,MassTDataTypes), SOFA_TEMPLATE(HexahedralFEMForceField,HexaFEMTDataTypes)); + + typedef typename TDataTypes::VecCoord VecCoord; + typedef typename TDataTypes::VecDeriv VecDeriv; typedef VecCoord Vector; - typedef typename DataTypes::Coord Coord; - typedef typename DataTypes::Deriv Deriv; + typedef typename TDataTypes::Coord Coord; + typedef typename TDataTypes::Deriv Deriv; typedef typename Coord::value_type Real; typedef core::objectmodel::Data DataVecDeriv; typedef core::objectmodel::Data DataVecCoord; @@ -75,7 +78,7 @@ class HexahedralFEMForceFieldAndMass : virtual public sofa::core::behavior::Mass bool isDiagonal() const override { return _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; @@ -84,23 +87,27 @@ class HexahedralFEMForceFieldAndMass : virtual public sofa::core::behavior::Mass void accFromF(const core::MechanicalParams* mparams, DataVecDeriv& a, const DataVecDeriv& f) override; - void addForce(const core::MechanicalParams* mparams, DataVecDeriv& f, const DataVecCoord& x, const DataVecDeriv& v) override; + void addGravitationalForce(const core::MechanicalParams* mparams, DataVecDeriv& f, const DataVecCoord& x, const DataVecDeriv& v, const Deriv& gravity) override; - SReal getPotentialEnergy(const core::MechanicalParams* /*mparams*/, const DataVecCoord& /* x */) const override + SReal getGravitationalPotentialEnergy(const core::MechanicalParams* mparams, const DataVecCoord& x, const Deriv& gravity) const override { - msg_warning() << "Method getPotentialEnergy not implemented yet."; + SOFA_UNUSED(mparams); + SOFA_UNUSED(x); + SOFA_UNUSED(gravity); + msg_warning() << "Method getGravitationalPotentialEnergy not implemented yet."; return 0.0; } SReal getKineticEnergy(const core::MechanicalParams* /* mparams */, const DataVecDeriv& /*v*/) const override ///< vMv/2 using dof->getV() override { - msg_error() << "HexahedralFEMForceFieldAndMass::getKineticEnergy not yet implemented"; + msg_error() << "HexahedralFEMForceFieldAndMass::getKineticEnergy not yet implemented"; return 0; } - void addDForce(const core::MechanicalParams* mparams, DataVecDeriv& df, const DataVecDeriv& dx) override; + SReal getPotentialEnergy( const core::MechanicalParams* mparams, const DataVecCoord& x ) const override; - void addGravityToV(const core::MechanicalParams* mparams, DataVecDeriv& d_v) override; + void addForce(const core::MechanicalParams* mparams, DataVecDeriv& f,const DataVecCoord& x, const DataVecDeriv& v) override; + void addDForce(const core::MechanicalParams* mparams, DataVecDeriv& df, const DataVecDeriv& dx) override; void draw(const core::visual::VisualParams* vparams) override; @@ -109,8 +116,7 @@ class HexahedralFEMForceFieldAndMass : virtual public sofa::core::behavior::Mass void setDensity(Real d) {_density.setValue( d );} Real getDensity() {return _density.getValue();} - - + using HexahedralFEMForceFieldT::canCreate; protected: virtual void computeElementMasses( ); ///< compute the mass matrices @@ -121,6 +127,9 @@ class HexahedralFEMForceFieldAndMass : virtual public sofa::core::behavior::Mass void computeLumpedMasses(); + bool insertInNode( core::objectmodel::BaseNode* node ) override { return HexahedralFEMForceFieldT::insertInNode(node); } + bool removeInNode( core::objectmodel::BaseNode* node ) override { return HexahedralFEMForceFieldT::removeInNode(node); } + protected: Data _density; ///< density == volumetric mass in english (kg.m-3) Data _useLumpedMass; ///< Does it use lumped masses? 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 282521fd8e5..fc558d4ff87 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 @@ -131,7 +131,7 @@ void HexahedralFEMForceFieldAndMass::computeLumpedMasses( ) template void HexahedralFEMForceFieldAndMass::computeElementMasses( ) { - const VecCoord& initialPoints = this->mstate->read(core::ConstVecCoordId::restPosition())->getValue(); + const VecCoord& initialPoints = HexahedralFEMForceFieldT::mstate->read(core::ConstVecCoordId::restPosition())->getValue(); const VecElement& hexahedra = this->_topology->getHexahedra(); @@ -261,7 +261,7 @@ void HexahedralFEMForceFieldAndMass::addMToMatrix(const core::Mechani const VecElement& hexahedra = this->_topology->getHexahedra(); - sofa::core::behavior::MultiMatrixAccessor::MatrixRef r = matrix->getMatrix(this->mstate); + sofa::core::behavior::MultiMatrixAccessor::MatrixRef r = matrix->getMatrix(HexahedralFEMForceFieldT::mstate); for(unsigned int e=0; e::addKToMatrix(const core::Mechani Index node1, node2; const VecElement& hexahedra = this->_topology->getHexahedra(); - sofa::core::behavior::MultiMatrixAccessor::MatrixRef r = matrix->getMatrix(this->mstate); + sofa::core::behavior::MultiMatrixAccessor::MatrixRef r = matrix->getMatrix(HexahedralFEMForceFieldT::mstate); for(it = this->hexahedronInfo.getValue().begin(), e=0 ; it != this->hexahedronInfo.getValue().end() ; ++it,++e) { @@ -350,7 +350,7 @@ void HexahedralFEMForceFieldAndMass::addMBKToMatrix (const core::Mech typename type::vector::const_iterator it; - sofa::core::behavior::MultiMatrixAccessor::MatrixRef r = matrix->getMatrix(this->mstate); + sofa::core::behavior::MultiMatrixAccessor::MatrixRef r = matrix->getMatrix(HexahedralFEMForceFieldT::mstate); unsigned int e = 0; for ( it = this->hexahedronInfo.getValue().begin() ; it != this->hexahedronInfo.getValue().end() ; ++it, ++e ) @@ -402,47 +402,35 @@ void HexahedralFEMForceFieldAndMass::accFromF(const core::MechanicalP template -void HexahedralFEMForceFieldAndMass::addGravityToV(const core::MechanicalParams* mparams, DataVecDeriv& d_v) +void HexahedralFEMForceFieldAndMass::addGravitationalForce(const core::MechanicalParams* mparams, DataVecDeriv& f, const DataVecCoord& x, const DataVecDeriv& v, const Deriv& gravity) { - if(mparams) + SOFA_UNUSED(mparams); + SOFA_UNUSED(x); + SOFA_UNUSED(v); + helper::WriteAccessor< DataVecDeriv > _f = f; + for (unsigned int i=0; i<_particleMasses.getValue().size(); i++) { - VecDeriv& v = *d_v.beginEdit(); - - SReal _dt = sofa::core::mechanicalparams::dt(mparams); - - for (unsigned int i=0; i<_particleMasses.getValue().size(); i++) - { - v[i] +=this->getContext()->getGravity()*_dt; - } - d_v.beginEdit(); + _f[i] += gravity * _particleMasses.getValue()[i]; } } - template -void HexahedralFEMForceFieldAndMass::addForce(const core::MechanicalParams* mparams, DataVecDeriv& f, const DataVecCoord& x, const DataVecDeriv& v) +void HexahedralFEMForceFieldAndMass::addForce(const core::MechanicalParams* mparams, DataVecDeriv& f,const DataVecCoord& x, const DataVecDeriv& v) { - HexahedralFEMForceFieldT::addForce(mparams, f,x,v); - - //if gravity was added separately (in solver's "solve" method), then nothing to do here - if (this->m_separateGravity.getValue()) - return; - - helper::WriteAccessor< DataVecDeriv > _f = f; - for (unsigned int i=0; i<_particleMasses.getValue().size(); i++) - { - _f[i] += this->getContext()->getGravity()*_particleMasses.getValue()[i]; - } + HexahedralFEMForceFieldT::addForce(mparams, f, x, v); } - - template void HexahedralFEMForceFieldAndMass::addDForce(const core::MechanicalParams* mparams, DataVecDeriv& df, const DataVecDeriv& dx) { HexahedralFEMForceFieldT::addDForce(mparams, df, dx); } +template +SReal HexahedralFEMForceFieldAndMass::getPotentialEnergy(const core::MechanicalParams* mparams, const DataVecCoord& x) const +{ + return HexahedralFEMForceFieldT::getPotentialEnergy(mparams, x); +} template SReal HexahedralFEMForceFieldAndMass::getElementMass(sofa::Index /*index*/) const @@ -459,7 +447,8 @@ void HexahedralFEMForceFieldAndMass::draw(const core::visual::VisualP if (!vparams->displayFlags().getShowBehaviorModels()) return; - const VecCoord& x = this->mstate->read(core::ConstVecCoordId::position())->getValue(); + + const VecCoord& x = HexahedralFEMForceFieldT::mstate->read(core::ConstVecCoordId::position())->getValue(); std::vector pos; pos.reserve(x.size()); 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 f5418ba1512..99b1b7e1741 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 @@ -31,21 +31,23 @@ namespace sofa::component::solidmechanics::fem::elastic /** Compute Finite Element forces based on hexahedral elements including continuum mass matrices */ -template -class HexahedronFEMForceFieldAndMass : virtual public core::behavior::Mass, virtual public HexahedronFEMForceField +template +class HexahedronFEMForceFieldAndMass : virtual public core::behavior::Mass, virtual public HexahedronFEMForceField { public: - SOFA_CLASS2(SOFA_TEMPLATE(HexahedronFEMForceFieldAndMass,DataTypes), SOFA_TEMPLATE(sofa::core::behavior::Mass,DataTypes), SOFA_TEMPLATE(HexahedronFEMForceField,DataTypes)); - - typedef HexahedronFEMForceField HexahedronFEMForceFieldT; - typedef sofa::core::behavior::Mass MassT; - - typedef typename DataTypes::Real Real ; - typedef typename DataTypes::Coord Coord ; - typedef typename DataTypes::Deriv Deriv ; - typedef typename DataTypes::VecCoord VecCoord ; - typedef typename DataTypes::VecDeriv VecDeriv ; - typedef typename DataTypes::VecReal VecReal ; + typedef HexahedronFEMForceField HexahedronFEMForceFieldT; + typedef sofa::core::behavior::Mass MassT; + typedef typename HexahedronFEMForceFieldT::DataTypes HexaFEMTDataTypes; + typedef typename MassT::DataTypes MassTDataTypes; + + SOFA_CLASS2(SOFA_TEMPLATE(HexahedronFEMForceFieldAndMass,TDataTypes), SOFA_TEMPLATE(sofa::core::behavior::Mass,MassTDataTypes), SOFA_TEMPLATE(HexahedronFEMForceField,HexaFEMTDataTypes)); + + typedef typename TDataTypes::Real Real ; + typedef typename TDataTypes::Coord Coord ; + typedef typename TDataTypes::Deriv Deriv ; + typedef typename TDataTypes::VecCoord VecCoord ; + typedef typename TDataTypes::VecDeriv VecDeriv ; + typedef typename TDataTypes::VecReal VecReal ; typedef VecCoord Vector; typedef core::objectmodel::Data DataVecDeriv; @@ -78,7 +80,7 @@ class HexahedronFEMForceFieldAndMass : virtual public core::behavior::Mass::addKToMatrix; + void addKToMatrix(const core::MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) override { HexahedronFEMForceFieldT::addKToMatrix(mparams, matrix); @@ -86,7 +88,7 @@ class HexahedronFEMForceFieldAndMass : virtual public core::behavior::MassgetV() override { @@ -94,22 +96,20 @@ class HexahedronFEMForceFieldAndMass : virtual public core::behavior::Mass _lumpedMasses; ///< masses per particle computed by lumping mass matrices - }; #if !defined(SOFA_COMPONENT_FORCEFIELD_HEXAHEDRONFEMFORCEFIELDANDMASS_CPP) 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 e4f5125788b..5fb9718d11f 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 @@ -222,7 +222,7 @@ void HexahedronFEMForceFieldAndMass::addMToMatrix(const core::Mechani int node1, node2; - sofa::core::behavior::MultiMatrixAccessor::MatrixRef r = matrix->getMatrix(this->mstate); + sofa::core::behavior::MultiMatrixAccessor::MatrixRef r = matrix->getMatrix(HexahedronFEMForceFieldT::mstate); for(it = this->getIndexedElements()->begin(), e=0 ; it != this->getIndexedElements()->end() ; ++it,++e) { @@ -257,46 +257,39 @@ void HexahedronFEMForceFieldAndMass::accFromF(const core::MechanicalP // need to built the big global mass matrix and to inverse it... } - template -void HexahedronFEMForceFieldAndMass::addGravityToV(const core::MechanicalParams* mparams, DataVecDeriv& d_v) +void HexahedronFEMForceFieldAndMass::addGravitationalForce(const core::MechanicalParams* mparams, DataVecDeriv& f, const DataVecCoord& x, const DataVecDeriv& v, const Deriv& gravity) { - if(mparams) + SOFA_UNUSED(mparams); + SOFA_UNUSED(x); + SOFA_UNUSED(v); + + helper::WriteAccessor< DataVecDeriv > _f = f; + for (unsigned int i=0; i<_particleMasses.size(); i++) { - VecDeriv& v = *d_v.beginEdit(); - SReal dt = sofa::core::mechanicalparams::dt(mparams); - for (unsigned int i=0; i<_particleMasses.size(); i++) - { - v[i] +=this->getContext()->getGravity()*dt; - } - d_v.beginEdit(); + _f[i] += gravity * _particleMasses[i]; } } + template -void HexahedronFEMForceFieldAndMass::addForce (const core::MechanicalParams* mparams, DataVecDeriv& f, const DataVecCoord& x, const DataVecDeriv& v) +void HexahedronFEMForceFieldAndMass::addForce(const core::MechanicalParams* mparams, DataVecDeriv& f,const DataVecCoord& x, const DataVecDeriv& v) { - HexahedronFEMForceFieldT::addForce(mparams, f,x,v); - - //if gravity was added separately (in solver's "solve" method), then nothing to do here - if (this->m_separateGravity.getValue()) - return; - - helper::WriteAccessor< DataVecDeriv > _f = f; - for (unsigned int i=0; i<_particleMasses.size(); i++) - { - _f[i] += this->getContext()->getGravity()*_particleMasses[i]; - } + HexahedronFEMForceFieldT::addForce(mparams, f, x, v); } - template void HexahedronFEMForceFieldAndMass::addDForce(const core::MechanicalParams* mparams, DataVecDeriv& df, const DataVecDeriv& dx) { HexahedronFEMForceFieldT::addDForce(mparams, df, dx); } +template +SReal HexahedronFEMForceFieldAndMass::getPotentialEnergy( const core::MechanicalParams* mparams, const DataVecCoord& x ) const +{ + return HexahedronFEMForceFieldT::getPotentialEnergy(mparams, x); +} template SReal HexahedronFEMForceFieldAndMass::getElementMass(sofa::Index /*index*/) const @@ -312,16 +305,15 @@ void HexahedronFEMForceFieldAndMass::draw(const core::visual::VisualP if (!vparams->displayFlags().getShowBehaviorModels()) return; - const VecCoord& x = this->mstate->read(core::ConstVecCoordId::position())->getValue(); - // since drawTool requires a std::vector we have to convert x in an ugly way + + const VecCoord& x = HexahedronFEMForceFieldT::mstate->read(core::ConstVecCoordId::position())->getValue(); + std::vector pos; - pos.resize(x.size()); - auto posIT = pos.begin(); - typename VecCoord::const_iterator xIT = x.begin(); - for(; posIT != pos.end() ; ++posIT, ++xIT) - { - *posIT = *xIT; - } + pos.reserve(x.size()); + + std::transform(x.begin(), x.end(), std::back_inserter(pos), + [](const auto& e){ return DataTypes::getCPos(e); }); + vparams->drawTool()->drawPoints(pos,2.0f, sofa::type::RGBAColor::white()); } diff --git a/Sofa/Component/SolidMechanics/FEM/NonUniform/src/sofa/component/solidmechanics/fem/nonuniform/HexahedronCompositeFEMForceFieldAndMass.inl b/Sofa/Component/SolidMechanics/FEM/NonUniform/src/sofa/component/solidmechanics/fem/nonuniform/HexahedronCompositeFEMForceFieldAndMass.inl index e72022613a4..af715e9fd07 100644 --- a/Sofa/Component/SolidMechanics/FEM/NonUniform/src/sofa/component/solidmechanics/fem/nonuniform/HexahedronCompositeFEMForceFieldAndMass.inl +++ b/Sofa/Component/SolidMechanics/FEM/NonUniform/src/sofa/component/solidmechanics/fem/nonuniform/HexahedronCompositeFEMForceFieldAndMass.inl @@ -1547,7 +1547,7 @@ template void HexahedronCompositeFEMForceFieldAndMass::draw(const core::visual::VisualParams* vparams) { if (!vparams->displayFlags().getShowForceFields()) return; - if (!this->mstate) return; + if (!HexahedronFEMForceFieldT::mstate) return; if (vparams->displayFlags().getShowWireFrame()) return; @@ -1557,7 +1557,7 @@ void HexahedronCompositeFEMForceFieldAndMass::draw(const core::visual::Visual const auto stateLifeCycle = vparams->drawTool()->makeStateLifeCycle(); - const VecCoord& x = this->mstate->read(core::ConstVecCoordId::position())->getValue(); + const VecCoord& x = HexahedronFEMForceFieldT::mstate->read(core::ConstVecCoordId::position())->getValue(); sofa::type::RGBAColor colour; diff --git a/Sofa/Component/SolidMechanics/FEM/NonUniform/src/sofa/component/solidmechanics/fem/nonuniform/NonUniformHexahedralFEMForceFieldAndMass.h b/Sofa/Component/SolidMechanics/FEM/NonUniform/src/sofa/component/solidmechanics/fem/nonuniform/NonUniformHexahedralFEMForceFieldAndMass.h index 4531e76ecb3..2114e0b7512 100644 --- a/Sofa/Component/SolidMechanics/FEM/NonUniform/src/sofa/component/solidmechanics/fem/nonuniform/NonUniformHexahedralFEMForceFieldAndMass.h +++ b/Sofa/Component/SolidMechanics/FEM/NonUniform/src/sofa/component/solidmechanics/fem/nonuniform/NonUniformHexahedralFEMForceFieldAndMass.h @@ -191,7 +191,6 @@ class NonUniformHexahedralFEMForceFieldAndMass : virtual public component::solid bool matrixIsDirty; ///< Matrix \f$ \alpha M + \beta B + \gamma C \f$ needs to be recomputed type::vector< ElementMass > mbkMatrix; ///< Matrix \f$ \alpha M + \beta B + \gamma C \f$ -protected: virtual void computeCorrection( ElementMass& ) {} ///< Limit the conditioning number of each mbkMatrix as defined by maxConditioning (in derived classes). }; diff --git a/Sofa/Component/SolidMechanics/FEM/NonUniform/src/sofa/component/solidmechanics/fem/nonuniform/NonUniformHexahedralFEMForceFieldAndMass.inl b/Sofa/Component/SolidMechanics/FEM/NonUniform/src/sofa/component/solidmechanics/fem/nonuniform/NonUniformHexahedralFEMForceFieldAndMass.inl index d9465a894fb..05a2ad3d4b1 100644 --- a/Sofa/Component/SolidMechanics/FEM/NonUniform/src/sofa/component/solidmechanics/fem/nonuniform/NonUniformHexahedralFEMForceFieldAndMass.inl +++ b/Sofa/Component/SolidMechanics/FEM/NonUniform/src/sofa/component/solidmechanics/fem/nonuniform/NonUniformHexahedralFEMForceFieldAndMass.inl @@ -91,7 +91,7 @@ void NonUniformHexahedralFEMForceFieldAndMass::reinit() return; } - const VecCoord& X0=this->mstate->read(core::ConstVecCoordId::restPosition())->getValue(); + const VecCoord& X0=HexahedralFEMForceFieldT::mstate->read(core::ConstVecCoordId::restPosition())->getValue(); type::Vec<8,Coord> nodesCoarse; for(int w=0; w<8; ++w) nodesCoarse[w] = (X0)[this->_topology->getHexahedron(0)[w]]; @@ -428,7 +428,7 @@ template void NonUniformHexahedralFEMForceFieldAndMass::initLarge( const int i) { - const VecCoord& X0=this->mstate->read(core::ConstVecCoordId::restPosition())->getValue(); + const VecCoord& X0=HexahedralFEMForceFieldT::mstate->read(core::ConstVecCoordId::restPosition())->getValue(); type::Vec<8,Coord> nodes; for(int w=0; w<8; ++w) @@ -463,7 +463,7 @@ template void NonUniformHexahedralFEMForceFieldAndMass::initPolar( const int i) { - const VecCoord& X0=this->mstate->read(core::ConstVecCoordId::restPosition())->getValue(); + const VecCoord& X0=HexahedralFEMForceFieldT::mstate->read(core::ConstVecCoordId::restPosition())->getValue(); type::Vec<8,Coord> nodes; for(int j=0; j<8; ++j) @@ -934,8 +934,8 @@ void NonUniformHexahedralFEMForceFieldAndMass::addMBKdx(const core::M { Real mFactor=(Real)sofa::core::mechanicalparams::mFactorIncludingRayleighDamping(mparams, this->rayleighMass.getValue()); Real kFactor=(Real)sofa::core::mechanicalparams::kFactorIncludingRayleighDamping(mparams, this->rayleighStiffness.getValue()); - helper::ReadAccessor < DataVecDeriv > dx = *mparams->readDx(this->mstate.get()); - helper::WriteAccessor< DataVecDeriv > df = *dfId[this->mstate.get()].write(); + helper::ReadAccessor < DataVecDeriv > dx = *mparams->readDx(HexahedralFEMForceFieldT::mstate.get()); + helper::WriteAccessor< DataVecDeriv > df = *dfId[HexahedralFEMForceFieldT::mstate.get()].write(); const VecElement& hexahedra = this->_topology->getHexahedra(); const auto& hexahedronInf = this->hexahedronInfo.getValue(); diff --git a/Sofa/Component/SolidMechanics/FEM/NonUniform/src/sofa/component/solidmechanics/fem/nonuniform/NonUniformHexahedronFEMForceFieldAndMass.h b/Sofa/Component/SolidMechanics/FEM/NonUniform/src/sofa/component/solidmechanics/fem/nonuniform/NonUniformHexahedronFEMForceFieldAndMass.h index 7a15ee4bc63..982fbde3242 100644 --- a/Sofa/Component/SolidMechanics/FEM/NonUniform/src/sofa/component/solidmechanics/fem/nonuniform/NonUniformHexahedronFEMForceFieldAndMass.h +++ b/Sofa/Component/SolidMechanics/FEM/NonUniform/src/sofa/component/solidmechanics/fem/nonuniform/NonUniformHexahedronFEMForceFieldAndMass.h @@ -90,8 +90,8 @@ class NonUniformHexahedronFEMForceFieldAndMass : virtual public component::solid void reinit() override { msg_warning() << "Non-uniform mechanical properties can't be updated, changes on mechanical properties (young, poisson, density) are not taken into account."; } void addMDx(const core::MechanicalParams* mparams, DataVecDeriv& f, const DataVecDeriv& dx, SReal factor) override; - void addGravityToV(const core::MechanicalParams* mparams, DataVecDeriv& d_v) override; void addForce(const core::MechanicalParams* mparams, DataVecDeriv& f, const DataVecCoord& x, const DataVecDeriv& v) override; + void addGravitationalForce(const core::MechanicalParams* mparams, DataVecDeriv& f, const DataVecCoord& x, const DataVecDeriv& v, const Deriv& gravity) override; protected: diff --git a/Sofa/Component/SolidMechanics/FEM/NonUniform/src/sofa/component/solidmechanics/fem/nonuniform/NonUniformHexahedronFEMForceFieldAndMass.inl b/Sofa/Component/SolidMechanics/FEM/NonUniform/src/sofa/component/solidmechanics/fem/nonuniform/NonUniformHexahedronFEMForceFieldAndMass.inl index 1b0e0c57e0a..142d875e89e 100644 --- a/Sofa/Component/SolidMechanics/FEM/NonUniform/src/sofa/component/solidmechanics/fem/nonuniform/NonUniformHexahedronFEMForceFieldAndMass.inl +++ b/Sofa/Component/SolidMechanics/FEM/NonUniform/src/sofa/component/solidmechanics/fem/nonuniform/NonUniformHexahedronFEMForceFieldAndMass.inl @@ -44,6 +44,7 @@ void NonUniformHexahedronFEMForceFieldAndMass::init() else this->_alreadyInit=true; + this->core::behavior::Mass::init(); this->core::behavior::ForceField::init(); if (l_topology.empty()) @@ -79,7 +80,7 @@ void NonUniformHexahedronFEMForceFieldAndMass::init() if (this->_initialPoints.getValue().size() == 0) { - const VecCoord& p = this->mstate->read(core::ConstVecCoordId::position())->getValue(); + const VecCoord& p = HexahedronFEMForceFieldT::mstate->read(core::ConstVecCoordId::position())->getValue(); this->_initialPoints.setValue(p); } @@ -278,7 +279,7 @@ void NonUniformHexahedronFEMForceFieldAndMass::computeClassicalMechanicalMatr type::fixed_array nodes; // for (unsigned int k=0;k<8;++k) nodes[k] = this->_sparseGrid->_virtualFinerLevels[level]->getPointPos(points[k]); - for (unsigned int k=0; k<8; ++k) nodes[k] = this->_sparseGrid->_virtualFinerLevels[level]->getPointPos(points[k]).linearProduct(this->mstate->getScale()); + for (unsigned int k=0; k<8; ++k) nodes[k] = this->_sparseGrid->_virtualFinerLevels[level]->getPointPos(points[k]).linearProduct(HexahedronFEMForceFieldT::mstate->getScale()); // //given an elementIndice, find the 8 others from the sparse grid // //compute MaterialStiffness @@ -449,48 +450,23 @@ void NonUniformHexahedronFEMForceFieldAndMass::addMDx(const core::MechanicalP } template -void NonUniformHexahedronFEMForceFieldAndMass::addGravityToV(const core::MechanicalParams* mparams, DataVecDeriv& d_v) +void NonUniformHexahedronFEMForceFieldAndMass::addForce(const core::MechanicalParams* mparams, DataVecDeriv& f, const DataVecCoord& x, const DataVecDeriv& v) { - if(d_useMass.getValue()) - HexahedronFEMForceFieldAndMassT::addGravityToV(mparams, d_v); - else - { - if(mparams) - { - VecDeriv& v = *d_v.beginEdit(); - - const SReal* g = this->getContext()->getGravity().ptr(); - Deriv theGravity; - T::set( theGravity, (Real)g[0], (Real)g[1], (Real)g[2]); - Deriv hg = theGravity * (sofa::core::mechanicalparams::dt(mparams)); - for (unsigned int i=0; i -void NonUniformHexahedronFEMForceFieldAndMass::addForce(const core::MechanicalParams* mparams, DataVecDeriv& f, const DataVecCoord& x, const DataVecDeriv& v) +void NonUniformHexahedronFEMForceFieldAndMass::addGravitationalForce(const core::MechanicalParams* mparams, DataVecDeriv& f, const DataVecCoord& x, const DataVecDeriv& v, const Deriv& gravity) { - if(d_useMass.getValue()) - HexahedronFEMForceFieldAndMassT::addForce(mparams, f,x,v); - else - { - HexahedronFEMForceFieldT::addForce(mparams, f,x,v); + SOFA_UNUSED(mparams); + SOFA_UNUSED(x); + SOFA_UNUSED(v); - helper::WriteAccessor _f = f; + helper::WriteAccessor _f = f; - const SReal* g = this->getContext()->getGravity().ptr(); - Deriv theGravity; - T::set( theGravity, g[0], g[1], g[2]); - - for (unsigned int i=0; i<_f.size(); i++) - { - _f[i] += theGravity * this->_particleMasses[i]; - } + for (unsigned int i=0; i<_f.size(); i++) + { + _f[i] += gravity * this->_particleMasses[i]; } } diff --git a/Sofa/framework/Core/src/sofa/core/behavior/BaseMass.cpp b/Sofa/framework/Core/src/sofa/core/behavior/BaseMass.cpp index 1e627f202f7..3757d34a7ed 100644 --- a/Sofa/framework/Core/src/sofa/core/behavior/BaseMass.cpp +++ b/Sofa/framework/Core/src/sofa/core/behavior/BaseMass.cpp @@ -26,11 +26,26 @@ namespace sofa::core::behavior { BaseMass::BaseMass() - : m_separateGravity (initData(&m_separateGravity , false, "separateGravity", "add separately gravity to velocity computation")) - , rayleighMass (initData(&rayleighMass , 0_sreal, "rayleighMass", "Rayleigh damping - mass matrix coefficient")) + : rayleighMass (initData(&rayleighMass , 0_sreal, "rayleighMass", "Rayleigh damping - mass matrix coefficient")) { } +void BaseMass::addMBKdx(const MechanicalParams* mparams, MultiVecDerivId dfId) +{ + if (sofa::core::mechanicalparams::mFactorIncludingRayleighDamping(mparams,rayleighMass.getValue()) != 0.0) + { + addMDx(mparams, dfId, sofa::core::mechanicalparams::mFactorIncludingRayleighDamping(mparams,rayleighMass.getValue())); + } +} + +void BaseMass::addMBKToMatrix(const MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) +{ + if (sofa::core::mechanicalparams::mFactorIncludingRayleighDamping(mparams,rayleighMass.getValue()) != 0.0 ) + { + addMToMatrix(mparams, matrix); + } +} + bool BaseMass::insertInNode( objectmodel::BaseNode* node ) { node->addMass(this); diff --git a/Sofa/framework/Core/src/sofa/core/behavior/BaseMass.h b/Sofa/framework/Core/src/sofa/core/behavior/BaseMass.h index 16848aadf18..deac8499fe5 100644 --- a/Sofa/framework/Core/src/sofa/core/behavior/BaseMass.h +++ b/Sofa/framework/Core/src/sofa/core/behavior/BaseMass.h @@ -62,24 +62,50 @@ class SOFA_CORE_API BaseMass : public virtual StateAccessor /// f += factor M dx virtual void addMDx(const MechanicalParams* mparams, MultiVecDerivId fid, SReal factor) =0; - /// dx = M^-1 f - virtual void accFromF(const MechanicalParams* mparams, MultiVecDerivId aid) = 0; - /// \brief Perform v += dt*g operation. Used if mass wants to added G separately from the other forces to v. + /// \brief Accumulate the contribution of M, B, and/or K matrices multiplied + /// by the dx vector with the given coefficients. + /// + /// This method computes + /// \f[ + /// df += mFactor M dx + bFactor B dx + kFactor K dx + /// \f] + /// + /// where M is the mass matrix (associated with inertial forces), + /// K is the stiffness matrix (associated with forces which derive from a potential), + /// and B is the damping matrix (associated with viscous forces). + /// + /// Very often, at least one of these matrices is null. + /// In most cases only one of these matrices will be non-null for a given + /// component. For forcefields without mass it simply calls addDForce. /// - /// \param mparams \a sofa::core::mechanicalparams::dt(mparams) is the time step of for temporal discretization. - virtual void addGravityToV(const MechanicalParams* mparams, MultiVecDerivId vid) = 0; + /// \param mparams + /// - \a mparams->readDx() is the input vector + /// - \a mparams->mFactor() is the coefficient for mass contributions (i.e. second-order derivatives term in the ODE) + /// - \a sofa::core::mechanicalparams::bFactor(mparams) is the coefficient for damping contributions (i.e. first derivatives term in the ODE) + /// - \a mparams->kFactor() is the coefficient for stiffness contributions (i.e. DOFs term in the ODE) + /// \param dfId the output vector + virtual void addMBKdx(const MechanicalParams* mparams, MultiVecDerivId dfId); + + + /// dx = M^-1 f + virtual void accFromF(const MechanicalParams* mparams, MultiVecDerivId aid) = 0; /// vMv/2 virtual SReal getKineticEnergy(const MechanicalParams* mparams = mechanicalparams::defaultInstance()) const = 0; - /// Mgx - virtual SReal getPotentialEnergy(const MechanicalParams* mparams = mechanicalparams::defaultInstance()) const = 0; /// (Mv,xMv+Iw) (linear and angular momenta against world origin) virtual type::Vec6 getMomentum(const MechanicalParams* mparams = mechanicalparams::defaultInstance()) const = 0; /// @} + + SOFA_ATTRIBUTE_DISABLED("v22.12 (PR#2988)", "v23.12", "Removing the separate gravity API.") + virtual void addGravityToV(const MechanicalParams* mparams, MultiVecDerivId vid) = delete; + SOFA_ATTRIBUTE_DISABLED("v22.12 (PR#2988)", "v23.12", "Removing the separate gravity API.") + DeprecatedAndRemoved m_separateGravity; + + /// @name Matrix operations /// @{ @@ -90,6 +116,16 @@ class SOFA_CORE_API BaseMass : public virtual StateAccessor /// \param mparams \a mparams->mFactor() is the coefficient for mass contributions (i.e. second-order derivatives term in the ODE) virtual void addMToMatrix(const MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) = 0; + + /// \brief Compute the system matrix corresponding to \f$ m M + b B + k K \f$ + /// + /// \param mparams + /// - \a mparams->mFactor() is the coefficient for mass contributions (i.e. second-order derivatives term in the ODE) + /// - \a sofa::core::mechanicalparams::bFactor(mparams) is the coefficient for damping contributions (i.e. first derivatives term in the ODE) + /// - \a mparams->kFactor() is the coefficient for stiffness contributions (i.e. DOFs term in the ODE) + /// \param matrix the matrix to add the result to + virtual void addMBKToMatrix(const MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix ); + /// @} /// initialization to export kinetic and potential energy to gnuplot files format @@ -102,15 +138,9 @@ class SOFA_CORE_API BaseMass : public virtual StateAccessor virtual SReal getElementMass(sofa::Index index) const =0; /// Get the matrix relative to the DOF at \a index. virtual void getElementMass(sofa::Index index, linearalgebra::BaseMatrix *m) const = 0; - + /// Return whether the mass matrix is diagonal or not virtual bool isDiagonal() const = 0; - /// Member specifying if the gravity is added separately to the DOFs velocities (in solve method), - /// or if is added with the other forces(addForceMethod) - Data m_separateGravity; - - - /** @name Rayleigh Damping (mass contribution) */ /// @{ diff --git a/Sofa/framework/Core/src/sofa/core/behavior/Mass.h b/Sofa/framework/Core/src/sofa/core/behavior/Mass.h index 4cf46065123..d5bde016610 100644 --- a/Sofa/framework/Core/src/sofa/core/behavior/Mass.h +++ b/Sofa/framework/Core/src/sofa/core/behavior/Mass.h @@ -24,6 +24,8 @@ #include #include #include +#include +#include namespace sofa::core::behavior { @@ -37,12 +39,13 @@ namespace sofa::core::behavior * * It is also a ForceField, computing gravity-related forces. */ -template -class Mass : virtual public ForceField, public BaseMass +template +class Mass : public BaseMass, public SingleStateAccessor { public: - SOFA_CLASS2(SOFA_TEMPLATE(Mass,DataTypes), SOFA_TEMPLATE(ForceField,DataTypes), BaseMass); + SOFA_CLASS(SOFA_TEMPLATE(Mass,TDataTypes), BaseMass); + typedef TDataTypes DataTypes; typedef typename DataTypes::VecCoord VecCoord; typedef typename DataTypes::VecDeriv VecDeriv; typedef typename DataTypes::Real Real; @@ -76,21 +79,17 @@ class Mass : virtual public ForceField, public BaseMass 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; - - /// Accumulate the contribution of M, B, and/or K matrices multiplied - /// by the dx vector with the given coefficients. + /// $ f = M g $ /// - /// This method computes - /// $ df += mFactor M dx + bFactor B dx + kFactor K dx $ - /// For masses, it calls both addMdx and addDForce (which is often empty). + /// This method computes the external force due to the gravitational acceleration + /// addGravitationalForce(const MechanicalParams*, DataVecDeriv& f, const DataVecCoord& x, const DataVecDeriv& v) method implemented by the component. + virtual void addGravitationalForce( const MechanicalParams* mparams, DataVecDeriv& f, const DataVecCoord& x, const DataVecDeriv& v, const Deriv& gravity) = 0; + + /// $ e = M g x $ /// - /// \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; + /// This method retrieves the positions vector and call the internal + /// getGravitationalPotentialEnergy(const MechanicalParams*, const VecCoord&) method implemented by the component. + virtual SReal getGravitationalPotentialEnergy( const MechanicalParams* mparams, const DataVecCoord& x, const Deriv& gravity) const = 0; /// $ e = 1/2 v^t M v $ /// @@ -99,14 +98,6 @@ class Mass : virtual public ForceField, public BaseMass 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 ) $ /// linearMomentum = Mv, angularMomentum_particle = cross(x,linearMomentum), angularMomentum_body = cross(x,linearMomentum)+Iw /// @@ -122,20 +113,9 @@ class Mass : virtual public ForceField, public BaseMass /// @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); - /// Compute the system matrix corresponding to m M + b B + k K - /// - /// \param matrix matrix to add the result to - /// \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 addMBKToMatrix(const MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) override; - /// @} /// initialization to export kinetic and potential energy to gnuplot files format @@ -144,11 +124,6 @@ class Mass : virtual public ForceField, public BaseMass /// export kinetic and potential energy state at "time" to a gnuplot file void exportGnuplot(const MechanicalParams* mparams, SReal time) override; - /// perform v += dt*g operation. Used if mass wants to added G separately from the other forces to v. - void addGravityToV(const MechanicalParams* mparams, MultiVecDerivId /*vid*/) override; - virtual void addGravityToV(const MechanicalParams* /* mparams */, DataVecDeriv& /* d_v */); - - /// recover the mass of an element SReal getElementMass(sofa::Index) const override; void getElementMass(sofa::Index index, linearalgebra::BaseMatrix *m) const override; @@ -158,9 +133,79 @@ class Mass : virtual public ForceField, public BaseMass std::ofstream* m_gnuplotFileEnergy; public: - bool insertInNode( objectmodel::BaseNode* node ) override { BaseMass::insertInNode(node); BaseForceField::insertInNode(node); return true; } - bool removeInNode( objectmodel::BaseNode* node ) override { BaseMass::removeInNode(node); BaseForceField::removeInNode(node); return true; } - + /// Pre-construction check method called by ObjectFactory. + /// Check that DataTypes matches the MechanicalState. + template + static bool canCreate(T*& obj, objectmodel::BaseContext* context, objectmodel::BaseObjectDescription* arg) + { + const std::string attributeName {"mstate"}; + std::string mstateLink = arg->getAttribute(attributeName,""); + if (mstateLink.empty()) + { + if (dynamic_cast*>(context->getMechanicalState()) == nullptr) + { + arg->logError("Since the attribute '" + attributeName + "' has not been specified, a mechanical state " + "with the datatype '" + DataTypes::Name() + "' has been searched in the current context, but not found."); + return false; + } + } + else + { + MechanicalState* mstate = nullptr; + context->findLinkDest(mstate, mstateLink, nullptr); + if (!mstate) + { + arg->logError("Data attribute '" + attributeName + "' does not point to a valid mechanical state of datatype '" + std::string(DataTypes::Name()) + "'."); + return false; + } + } + return BaseObject::canCreate(obj, context, arg); + } + + /// Automatic creation of a local GravityForceField if the gravity in the root node is defined + template + static typename T::SPtr create(T*, sofa::core::objectmodel::BaseContext* context, sofa::core::objectmodel::BaseObjectDescription* arg) + { + typename T::SPtr obj = sofa::core::objectmodel::New(); + if (context) context->addObject(obj); + if (arg) obj->parse(arg); + + const sofa::core::objectmodel::BaseContext::Vec3& gravity = context->getRootContext()->getGravity(); + SReal gravityNorm = gravity.norm(); + if(gravityNorm!=0.0) + { + // SOFA_ATTRIBUTE_DISABLED("v22.12 (PR#2988)", "v23.12", "Transition removing gravity and introducing GravityForceField") + // to remove after v23.06 ... + bool savePLog = context->f_printLog.getValue(); + context->f_printLog.setValue(true); + msg_info(context) << "A gravity seem to apply using the gravity in the root node (PR#2988)." << msgendl + << "A GravityForceField is automatically added in the node \"" << context->getName() << "\"."; + context->f_printLog.setValue(savePLog); + // until here + + + const std::string templated = context->getMechanicalState()->getTemplateName(); + std::string gravity_string = "@"+ context->getRootContext()->getPathName()+".gravity"; + + sofa::core::objectmodel::BaseObjectDescription desc("GravityForceField","GravityForceField"); + desc.setAttribute("template", templated); + desc.setAttribute("gravity", gravity_string); + + /// Create the object + BaseObject::SPtr objGravityFF = sofa::core::ObjectFactory::getInstance()->createObject(context, &desc); + if (objGravityFF==nullptr) + { + std::stringstream msg; + msg << "Component '" << desc.getName() << "' of type '" << desc.getAttribute("type","") << "' failed:" << msgendl ; + for (std::vector< std::string >::const_iterator it = desc.getErrors().begin(); it != desc.getErrors().end(); ++it) + msg << " " << *it << msgendl ; + msg_error(context) << msg.str() ; + return nullptr; + } + } + + return obj; + } }; diff --git a/Sofa/framework/Core/src/sofa/core/behavior/Mass.inl b/Sofa/framework/Core/src/sofa/core/behavior/Mass.inl index de59e97b744..1fab9672be7 100644 --- a/Sofa/framework/Core/src/sofa/core/behavior/Mass.inl +++ b/Sofa/framework/Core/src/sofa/core/behavior/Mass.inl @@ -33,7 +33,7 @@ namespace sofa::core::behavior template Mass::Mass(MechanicalState *mm) - : ForceField(mm) + : BaseMass(), SingleStateAccessor(mm) , m_gnuplotFileEnergy(nullptr) { } @@ -77,33 +77,6 @@ void Mass::accFromF(const MechanicalParams* /*mparams*/, DataVecDeriv msg_warning() << "Method accFromF(const MechanicalParams* , DataVecDeriv& , const DataVecDeriv& ) not implemented."; } - -template -void Mass::addDForce(const MechanicalParams* - #ifndef NDEBUG - mparams - #endif - , - DataVecDeriv & /*df*/, const DataVecDeriv & /*dx*/) -{ -#ifndef NDEBUG - // @TODO Remove - // Hack to disable warning message - sofa::core::mechanicalparams::kFactorIncludingRayleighDamping(mparams, this->rayleighStiffness.getValue()); -#endif -} - -template -void Mass::addMBKdx(const MechanicalParams* mparams, MultiVecDerivId dfId) -{ - this->ForceField::addMBKdx(mparams, dfId); - if (mparams->mFactorIncludingRayleighDamping(rayleighMass.getValue()) != 0.0) - { - addMDx(mparams, *dfId[this->mstate.get()].write(), - *mparams->readDx(this->mstate.get()), mparams->mFactorIncludingRayleighDamping(rayleighMass.getValue())); - } -} - template SReal Mass::getKineticEnergy(const MechanicalParams* mparams) const { @@ -119,23 +92,6 @@ SReal Mass::getKineticEnergy(const MechanicalParams* /*mparams*/, con return 0.0; } - -template -SReal Mass::getPotentialEnergy(const MechanicalParams* mparams) const -{ - if (this->mstate) - return getPotentialEnergy(mparams /* PARAMS FIRST */, *mparams->readX(this->mstate.get())); - return 0.0; -} - -template -SReal Mass::getPotentialEnergy(const MechanicalParams* /*mparams*/, const DataVecCoord& /*x*/) const -{ - msg_warning() << "Method getPotentialEnergy( const MechanicalParams*, const DataVecCoord& ) not implemented."; - return 0.0; -} - - template type::Vec6 Mass::getMomentum( const MechanicalParams* mparams ) const { @@ -172,34 +128,6 @@ void Mass::addMToMatrix(sofa::linearalgebra::BaseMatrix * /*mat*/, SR } } -template -void Mass::addMBKToMatrix(const MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) -{ - this->ForceField::addMBKToMatrix(mparams, matrix); - if (mparams->mFactorIncludingRayleighDamping(rayleighMass.getValue()) != 0.0) - addMToMatrix(mparams, matrix); -} - -template -void Mass::addGravityToV(const MechanicalParams* mparams, MultiVecDerivId vid) -{ - if(this->mstate) - { - DataVecDeriv& v = *vid[this->mstate.get()].write(); - addGravityToV(mparams, v); - } -} - -template -void Mass::addGravityToV(const MechanicalParams* /* mparams */, DataVecDeriv& /* d_v */) -{ - static int i=0; - if (i < 10) { - msg_warning() << "Method addGravityToV with Scalar not implemented"; - i++; - } -} - template void Mass::initGnuplot(const std::string path) @@ -218,10 +146,7 @@ void Mass::exportGnuplot(const MechanicalParams* mparams, SReal time) { if (m_gnuplotFileEnergy!=nullptr) { - (*m_gnuplotFileEnergy) << time <<"\t"<< this->getKineticEnergy(mparams) - <<"\t"<< this->getPotentialEnergy(mparams) - <<"\t"<< this->getPotentialEnergy(mparams) - +this->getKineticEnergy(mparams)<< std::endl; + (*m_gnuplotFileEnergy) << time <<"\t"<< this->getKineticEnergy(mparams) << "\t" << 0.0 << "\t" << this->getKineticEnergy(mparams)<< std::endl; } } diff --git a/Sofa/framework/Core/src/sofa/core/behavior/OdeSolver.cpp b/Sofa/framework/Core/src/sofa/core/behavior/OdeSolver.cpp index 27b57c6cf7c..490615376c5 100644 --- a/Sofa/framework/Core/src/sofa/core/behavior/OdeSolver.cpp +++ b/Sofa/framework/Core/src/sofa/core/behavior/OdeSolver.cpp @@ -40,12 +40,6 @@ OdeSolver::~OdeSolver() {} -//const OdeSolver::MechanicalMatrix OdeSolver::M(1,0,0); -//const OdeSolver::MechanicalMatrix OdeSolver::B(0,1,0); -//const OdeSolver::MechanicalMatrix OdeSolver::K(0,0,1); - - - bool OdeSolver::insertInNode( objectmodel::BaseNode* node ) { node->addOdeSolver(this); diff --git a/Sofa/framework/Core/src/sofa/core/objectmodel/Context.cpp b/Sofa/framework/Core/src/sofa/core/objectmodel/Context.cpp index 1bc2b619271..13d86b065ae 100644 --- a/Sofa/framework/Core/src/sofa/core/objectmodel/Context.cpp +++ b/Sofa/framework/Core/src/sofa/core/objectmodel/Context.cpp @@ -26,7 +26,7 @@ namespace sofa::core::objectmodel Context::Context() : is_activated(initData(&is_activated, true, "activated", "To Activate a node")) - , worldGravity_(initData(&worldGravity_, Vec3(SReal(0),SReal(-9.81),SReal(0)),"gravity","Gravity in the world coordinate system")) + , d_gravity(initData(&d_gravity, Vec3(SReal(0.),SReal(0.),SReal(0.)),"gravity","Gravity in the world coordinate system")) , dt_(initData(&dt_,SReal(0.01),"dt","Time step")) , time_(initData(&time_,SReal(0.),"time","Current time")) , animate_(initData(&animate_,false,"animate","Animate the Simulation(applied at initialization only)")) @@ -88,7 +88,7 @@ SReal Context::getTime() const /// Gravity vector in world coordinates const Context::Vec3& Context::getGravity() const { - return worldGravity_.getValue(); + return d_gravity.getValue(); } /// Animation flag @@ -114,7 +114,7 @@ void Context::setTime(SReal t) /// Gravity vector void Context::setGravity(const Vec3& g) { - worldGravity_ .setValue(g); + d_gravity.setValue(g); } /// Animation flag @@ -136,7 +136,7 @@ void Context::copyContext(const Context& c) void Context::copySimulationContext(const Context& c) { - worldGravity_.setValue(c.getGravity()); ///< Gravity IN THE WORLD COORDINATE SYSTEM. + d_gravity.setValue(c.getGravity()); ///< Gravity IN THE WORLD COORDINATE SYSTEM. setDt(c.getDt()); setTime(c.getTime()); setAnimate(c.getAnimate()); diff --git a/Sofa/framework/Core/src/sofa/core/objectmodel/Context.h b/Sofa/framework/Core/src/sofa/core/objectmodel/Context.h index 1b98960ccd5..c37712b5a23 100644 --- a/Sofa/framework/Core/src/sofa/core/objectmodel/Context.h +++ b/Sofa/framework/Core/src/sofa/core/objectmodel/Context.h @@ -36,7 +36,7 @@ class SOFA_CORE_API Context : public BaseContext SOFA_CLASS(Context, BaseContext); Data is_activated; ///< To Activate a node - Data worldGravity_; ///< Gravity IN THE WORLD COORDINATE SYSTEM. + Data d_gravity; ///< Gravity IN THE WORLD COORDINATE SYSTEM. Data dt_; ///< Time step Data time_; ///< Current time Data animate_; ///< Animate the Simulation(applied at initialization only) @@ -97,7 +97,7 @@ class SOFA_CORE_API Context : public BaseContext void setChangeSleepingState(bool val) override; /// Display flags: Gravity - virtual void setDisplayWorldGravity(bool val) { worldGravity_.setDisplayed(val); } + virtual void setDisplayWorldGravity(bool val) { d_gravity.setDisplayed(val); } /// @} diff --git a/Sofa/framework/Helper/src/sofa/helper/ComponentChange.cpp b/Sofa/framework/Helper/src/sofa/helper/ComponentChange.cpp index fcfa1da65f9..fe7d95295b1 100644 --- a/Sofa/framework/Helper/src/sofa/helper/ComponentChange.cpp +++ b/Sofa/framework/Helper/src/sofa/helper/ComponentChange.cpp @@ -93,6 +93,7 @@ const std::map > uncreatableComponents /***********************/ // REMOVED SINCE v22.06 + { "Gravity", Removed("v22.06", "v22.06") }, {"PointConstraint", Removed("v21.12", "v22.06")}, /***********************/ diff --git a/Sofa/framework/Simulation/Core/CMakeLists.txt b/Sofa/framework/Simulation/Core/CMakeLists.txt index e3d4e8bc9c2..ce35047b078 100644 --- a/Sofa/framework/Simulation/Core/CMakeLists.txt +++ b/Sofa/framework/Simulation/Core/CMakeLists.txt @@ -94,7 +94,6 @@ set(HEADER_FILES ${SRC_ROOT}/mechanicalvisitor/MechanicalAddMBKdxVisitor.h ${SRC_ROOT}/mechanicalvisitor/MechanicalAddMBK_ToMatrixVisitor.h ${SRC_ROOT}/mechanicalvisitor/MechanicalAddMDxVisitor.h - ${SRC_ROOT}/mechanicalvisitor/MechanicalAddSeparateGravityVisitor.h ${SRC_ROOT}/mechanicalvisitor/MechanicalApplyConstraintsVisitor.h ${SRC_ROOT}/mechanicalvisitor/MechanicalApplyProjectiveConstraint_ToMatrixVisitor.h ${SRC_ROOT}/mechanicalvisitor/MechanicalBeginIntegrationVisitor.h @@ -227,7 +226,6 @@ set(SOURCE_FILES ${SRC_ROOT}/mechanicalvisitor/MechanicalAddMBKdxVisitor.cpp ${SRC_ROOT}/mechanicalvisitor/MechanicalAddMBK_ToMatrixVisitor.cpp ${SRC_ROOT}/mechanicalvisitor/MechanicalAddMDxVisitor.cpp - ${SRC_ROOT}/mechanicalvisitor/MechanicalAddSeparateGravityVisitor.cpp ${SRC_ROOT}/mechanicalvisitor/MechanicalApplyConstraintsVisitor.cpp ${SRC_ROOT}/mechanicalvisitor/MechanicalApplyProjectiveConstraint_ToMatrixVisitor.cpp ${SRC_ROOT}/mechanicalvisitor/MechanicalBeginIntegrationVisitor.cpp diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/MechanicalOperations.cpp b/Sofa/framework/Simulation/Core/src/sofa/simulation/MechanicalOperations.cpp index f75aa8f2785..7d063b33730 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/MechanicalOperations.cpp +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/MechanicalOperations.cpp @@ -42,7 +42,6 @@ #include #include #include -#include #include #include #include @@ -321,16 +320,6 @@ void MechanicalOperations::addMBKv(core::MultiVecDerivId df, SReal m, SReal b, S mparams.setDx(dx); } - - -/// Add dt*Gravity to the velocity -void MechanicalOperations::addSeparateGravity(SReal dt, core::MultiVecDerivId result) -{ - mparams.setDt(dt); - setV(result); - executeVisitor( MechanicalAddSeparateGravityVisitor(&mparams, result) ); -} - void MechanicalOperations::computeContactForce(core::MultiVecDerivId result) { setF(result); diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/MechanicalOperations.h b/Sofa/framework/Simulation/Core/src/sofa/simulation/MechanicalOperations.h index 9a8d996a4a9..941cf3e6eeb 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/MechanicalOperations.h +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/MechanicalOperations.h @@ -84,8 +84,10 @@ class SOFA_SIMULATION_CORE_API MechanicalOperations void addMBKdx(core::MultiVecDerivId df, SReal m, SReal b, SReal k, bool clear = true, bool accumulate = true); /// accumulate $ df += (m M + b B + k K) velocity $ void addMBKv(core::MultiVecDerivId df, SReal m, SReal b, SReal k, bool clear = true, bool accumulate = true); - /// Add dt*Gravity to the velocity - void addSeparateGravity(SReal dt, core::MultiVecDerivId result = core::VecDerivId::velocity() ); + + + SOFA_ATTRIBUTE_DISABLED("v22.12 (PR#2988)", "v23.12", "Removing the separate gravity API.") + void addSeparateGravity(SReal dt, core::MultiVecDerivId result = core::VecDerivId::velocity() ) = delete; void computeContactForce(core::MultiVecDerivId result); void computeContactDf(core::MultiVecDerivId df); diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/mechanicalvisitor/MechanicalAddMBK_ToMatrixVisitor.cpp b/Sofa/framework/Simulation/Core/src/sofa/simulation/mechanicalvisitor/MechanicalAddMBK_ToMatrixVisitor.cpp index 3b38035c14e..36948603383 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/mechanicalvisitor/MechanicalAddMBK_ToMatrixVisitor.cpp +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/mechanicalvisitor/MechanicalAddMBK_ToMatrixVisitor.cpp @@ -23,6 +23,7 @@ #include #include +#include namespace sofa::simulation::mechanicalvisitor { @@ -40,6 +41,16 @@ MechanicalAddMBK_ToMatrixVisitor::fwdMechanicalState(simulation::Node *, core::b return RESULT_CONTINUE; } +Visitor::Result MechanicalAddMBK_ToMatrixVisitor::fwdMass(simulation::Node *, core::behavior::BaseMass *mass) +{ + if (matrix != nullptr) + { + mass->addMBKToMatrix(this->mparams, matrix); + } + + return RESULT_CONTINUE; +} + Visitor::Result MechanicalAddMBK_ToMatrixVisitor::fwdForceField(simulation::Node *, core::behavior::BaseForceField *ff) { if (matrix != nullptr) @@ -56,4 +67,4 @@ bool MechanicalAddMBK_ToMatrixVisitor::stopAtMechanicalMapping(simulation::Node SOFA_UNUSED(node); return !map->areMatricesMapped(); } -} \ No newline at end of file +} diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/mechanicalvisitor/MechanicalAddMBK_ToMatrixVisitor.h b/Sofa/framework/Simulation/Core/src/sofa/simulation/mechanicalvisitor/MechanicalAddMBK_ToMatrixVisitor.h index d74571c2fa5..c2d14ec90fe 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/mechanicalvisitor/MechanicalAddMBK_ToMatrixVisitor.h +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/mechanicalvisitor/MechanicalAddMBK_ToMatrixVisitor.h @@ -40,8 +40,10 @@ class SOFA_SIMULATION_CORE_API MechanicalAddMBK_ToMatrixVisitor : public Mechani Result fwdMechanicalState(simulation::Node* /*node*/, core::behavior::BaseMechanicalState* /*ms*/) override; + Result fwdMass(simulation::Node* /*node*/, core::behavior::BaseMass* mass) override; + Result fwdForceField(simulation::Node* /*node*/, core::behavior::BaseForceField* ff) override; bool stopAtMechanicalMapping(simulation::Node* node, core::BaseMapping* map) override; }; -} \ No newline at end of file +} diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/mechanicalvisitor/MechanicalAddMBKdxVisitor.cpp b/Sofa/framework/Simulation/Core/src/sofa/simulation/mechanicalvisitor/MechanicalAddMBKdxVisitor.cpp index 91c77e95ecf..70eb2df4f62 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/mechanicalvisitor/MechanicalAddMBKdxVisitor.cpp +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/mechanicalvisitor/MechanicalAddMBKdxVisitor.cpp @@ -23,6 +23,7 @@ #include #include +#include namespace sofa::simulation::mechanicalvisitor { @@ -38,6 +39,12 @@ Visitor::Result MechanicalAddMBKdxVisitor::fwdMappedMechanicalState(simulation:: return RESULT_CONTINUE; } +Visitor::Result MechanicalAddMBKdxVisitor::fwdMass(simulation::Node* /*node*/, core::behavior::BaseMass* mass) +{ + mass->addMBKdx( this->mparams, res); + return RESULT_CONTINUE; +} + Visitor::Result MechanicalAddMBKdxVisitor::fwdForceField(simulation::Node* /*node*/, core::behavior::BaseForceField* ff) { if( !ff->isCompliance.getValue() ) diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/mechanicalvisitor/MechanicalAddMBKdxVisitor.h b/Sofa/framework/Simulation/Core/src/sofa/simulation/mechanicalvisitor/MechanicalAddMBKdxVisitor.h index 32473a6418d..dc74d498a44 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/mechanicalvisitor/MechanicalAddMBKdxVisitor.h +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/mechanicalvisitor/MechanicalAddMBKdxVisitor.h @@ -50,6 +50,7 @@ class SOFA_SIMULATION_CORE_API MechanicalAddMBKdxVisitor : public MechanicalVisi } Result fwdMechanicalState(simulation::Node* /*node*/,sofa::core::behavior::BaseMechanicalState* mm) override; Result fwdMappedMechanicalState(simulation::Node* /*node*/,sofa::core::behavior::BaseMechanicalState* mm) override; + Result fwdMass(simulation::Node* /*node*/,sofa::core::behavior::BaseMass* mass) override; Result fwdForceField(simulation::Node* /*node*/,sofa::core::behavior::BaseForceField* ff) override; void bwdMechanicalMapping(simulation::Node* /*node*/, sofa::core::BaseMapping* map) override; void bwdMechanicalState(simulation::Node* /*node*/,sofa::core::behavior::BaseMechanicalState* mm) override; diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/mechanicalvisitor/MechanicalAddSeparateGravityVisitor.cpp b/Sofa/framework/Simulation/Core/src/sofa/simulation/mechanicalvisitor/MechanicalAddSeparateGravityVisitor.cpp deleted file mode 100644 index 9e02a23654f..00000000000 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/mechanicalvisitor/MechanicalAddSeparateGravityVisitor.cpp +++ /dev/null @@ -1,40 +0,0 @@ -/****************************************************************************** -* SOFA, Simulation Open-Framework Architecture * -* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * -* * -* This program is free software; you can redistribute it and/or modify it * -* under the terms of the GNU Lesser General Public License as published by * -* the Free Software Foundation; either version 2.1 of the License, or (at * -* your option) any later version. * -* * -* This program is distributed in the hope that it will be useful, but WITHOUT * -* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * -* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * -* for more details. * -* * -* You should have received a copy of the GNU Lesser General Public License * -* along with this program. If not, see . * -******************************************************************************* -* Authors: The SOFA Team and external contributors (see Authors.txt) * -* * -* Contact information: contact@sofa-framework.org * -******************************************************************************/ - -#include - -#include - -namespace sofa::simulation::mechanicalvisitor -{ - -Visitor::Result MechanicalAddSeparateGravityVisitor::fwdMass(simulation::Node* /*node*/, core::behavior::BaseMass* mass) -{ - if( mass->m_separateGravity.getValue() ) - { - // - mass->addGravityToV(this->mparams, res); - } - return RESULT_CONTINUE; -} - -} \ No newline at end of file diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/mechanicalvisitor/MechanicalAddSeparateGravityVisitor.h b/Sofa/framework/Simulation/Core/src/sofa/simulation/mechanicalvisitor/MechanicalAddSeparateGravityVisitor.h deleted file mode 100644 index eab1dc99ecc..00000000000 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/mechanicalvisitor/MechanicalAddSeparateGravityVisitor.h +++ /dev/null @@ -1,59 +0,0 @@ -/****************************************************************************** -* SOFA, Simulation Open-Framework Architecture * -* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * -* * -* This program is free software; you can redistribute it and/or modify it * -* under the terms of the GNU Lesser General Public License as published by * -* the Free Software Foundation; either version 2.1 of the License, or (at * -* your option) any later version. * -* * -* This program is distributed in the hope that it will be useful, but WITHOUT * -* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * -* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * -* for more details. * -* * -* You should have received a copy of the GNU Lesser General Public License * -* along with this program. If not, see . * -******************************************************************************* -* Authors: The SOFA Team and external contributors (see Authors.txt) * -* * -* Contact information: contact@sofa-framework.org * -******************************************************************************/ -#pragma once - -#include - -namespace sofa::simulation::mechanicalvisitor -{ - -/** Add dt*mass*Gravity to the velocity -This is called if the mass wants to be added separately to the mm from the other forces -*/ -class SOFA_SIMULATION_CORE_API MechanicalAddSeparateGravityVisitor : public MechanicalVisitor -{ -public: - sofa::core::MultiVecDerivId res; - MechanicalAddSeparateGravityVisitor(const sofa::core::MechanicalParams* m_mparams, - sofa::core::MultiVecDerivId res ) - : MechanicalVisitor(m_mparams) , res(res) - { -#ifdef SOFA_DUMP_VISITOR_INFO - setReadWriteVectors(); -#endif - } - - /// Process the BaseMass - Result fwdMass(simulation::Node* /*node*/,sofa::core::behavior::BaseMass* mass) override; - - /// Return a class name for this visitor - /// Only used for debugging / profiling purposes - const char* getClassName() const override { return "MechanicalAddSeparateGravityVisitor"; } - virtual std::string getInfos() const override { std::string name= "["+res.getName()+"]"; return name; } -#ifdef SOFA_DUMP_VISITOR_INFO - void setReadWriteVectors() override - { - addReadWriteVector(res); - } -#endif -}; -} \ No newline at end of file diff --git a/applications/collections/deprecated/modules/SofaGraphComponent/CMakeLists.txt b/applications/collections/deprecated/modules/SofaGraphComponent/CMakeLists.txt index de243f61391..a21d03ab253 100644 --- a/applications/collections/deprecated/modules/SofaGraphComponent/CMakeLists.txt +++ b/applications/collections/deprecated/modules/SofaGraphComponent/CMakeLists.txt @@ -18,10 +18,6 @@ list(APPEND HEADER_FILES ${SOFAGRAPHCOMPONENT_SRC}/Gravity.h ${SOFAGRAPHCOMPONENT_SRC}/InteractingBehaviorModel.h ) -list(APPEND SOURCE_FILES - ${SOFAGRAPHCOMPONENT_SRC}/Gravity.cpp - ) - add_library(${PROJECT_NAME} SHARED ${HEADER_FILES} ${SOURCE_FILES}) target_link_libraries(${PROJECT_NAME} PUBLIC Sofa.Helper Sofa.Simulation.Core SofaBaseUtils SofaBaseCollision) diff --git a/applications/collections/deprecated/modules/SofaGraphComponent/src/SofaGraphComponent/Gravity.h b/applications/collections/deprecated/modules/SofaGraphComponent/src/SofaGraphComponent/Gravity.h index 90c2111d95c..38471760df8 100644 --- a/applications/collections/deprecated/modules/SofaGraphComponent/src/SofaGraphComponent/Gravity.h +++ b/applications/collections/deprecated/modules/SofaGraphComponent/src/SofaGraphComponent/Gravity.h @@ -35,17 +35,17 @@ namespace sofa::component::contextobject { /** Override the default gravity */ -class SOFA_SOFAGRAPHCOMPONENT_API Gravity : public core::objectmodel::ContextObject +class SOFA_ATTRIBUTE_DISABLED("v22.12 (PR#2988)", "v23.12", "Gravity class has been removed") Gravity : public core::objectmodel::ContextObject { public: SOFA_CLASS(Gravity, core::objectmodel::ContextObject); protected: - Gravity(); + Gravity() = default; public: - Data f_gravity; ///< Gravity in the world coordinate system + SOFA_ATTRIBUTE_DISABLED("v22.12 (PR#2988)", "v23.12", "Gravity class has been removed") + DeprecatedAndRemoved f_gravity; ///< Gravity in the world coordinate system - /// Modify the context of the Node - void apply() override; + void apply() override {}; }; } // namespace sofa::component::contextobject diff --git a/applications/plugins/CMakeLists.txt b/applications/plugins/CMakeLists.txt index f5a94f9287a..5a71ef85227 100644 --- a/applications/plugins/CMakeLists.txt +++ b/applications/plugins/CMakeLists.txt @@ -30,7 +30,7 @@ sofa_add_subdirectory(plugin Flexible Flexible EXTERNAL) # Depends on im sofa_add_subdirectory(plugin Registration Registration EXTERNAL) # Depends on image, SofaPython, SofaGui and SofaDistanceGrid sofa_add_subdirectory(plugin BulletCollisionDetection BulletCollisionDetection) # Depends on Compliant and LMConstraint sofa_add_subdirectory(plugin PreassembledMass PreassembledMass) # Depends on Flexible and Compliant -sofa_add_subdirectory(plugin ExternalBehaviorModel ExternalBehaviorModel OFF WHEN_TO_SHOW "SOFA_ENABLE_LEGACY_HEADERS" VALUE_IF_HIDDEN OFF) +sofa_add_subdirectory(plugin ExternalBehaviorModel ExternalBehaviorModel OFF) sofa_add_subdirectory(plugin InvertibleFVM InvertibleFVM EXTERNAL) sofa_add_subdirectory(plugin MeshSTEPLoader MeshSTEPLoader) sofa_add_subdirectory(plugin PluginExample PluginExample EXTERNAL) diff --git a/applications/plugins/PreassembledMass/PreassembledMass.h b/applications/plugins/PreassembledMass/PreassembledMass.h index 8decc9ca4da..c8a6f58db2e 100644 --- a/applications/plugins/PreassembledMass/PreassembledMass.h +++ b/applications/plugins/PreassembledMass/PreassembledMass.h @@ -96,10 +96,9 @@ class PreassembledMass : public core::behavior::Mass< DataTypes >, public BasePr // -- Mass interface (employed only if d_massOnIndependents==true) void addMDx(const core::MechanicalParams* mparams, DataVecDeriv& f, const DataVecDeriv& dx, double factor); void accFromF(const core::MechanicalParams* mparams, DataVecDeriv& a, const DataVecDeriv& f); - void addForce(const core::MechanicalParams* mparams, DataVecDeriv& f, const DataVecCoord& x, const DataVecDeriv& v); + void addGravitationalForce(const core::MechanicalParams* mparams, DataVecDeriv& f, const DataVecCoord& x, const DataVecDeriv& v, const Deriv& gravity); double getKineticEnergy(const core::MechanicalParams* mparams, const DataVecDeriv& v) const; - double getPotentialEnergy(const core::MechanicalParams* mparams, const DataVecCoord& x) const; - void addGravityToV(const core::MechanicalParams* mparams, DataVecDeriv& d_v); + double getGravitationalPotentialEnergy(const core::MechanicalParams* mparams, const DataVecCoord& x, const Deriv& gravity) const; void addMToMatrix(const core::MechanicalParams *mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix); diff --git a/applications/plugins/PreassembledMass/PreassembledMass.inl b/applications/plugins/PreassembledMass/PreassembledMass.inl index aa24e319fc2..2ca871ebfa5 100644 --- a/applications/plugins/PreassembledMass/PreassembledMass.inl +++ b/applications/plugins/PreassembledMass/PreassembledMass.inl @@ -146,10 +146,10 @@ double PreassembledMass< DataTypes >::getKineticEnergy( const core::MechanicalPa } template < class DataTypes > -double PreassembledMass< DataTypes >::getPotentialEnergy( const core::MechanicalParams* mparams, const DataVecCoord& x ) const +double PreassembledMass< DataTypes >::getGravitationalPotentialEnergy( const core::MechanicalParams* mparams, const DataVecCoord& x, const Deriv& gravity ) const { serr<::getPotentialEnergy( mparams, x ); + return core::behavior::Mass< DataTypes >::getGravitationalPotentialEnergy( mparams, x, gravity ); // const VecCoord& _x = x.getValue(); @@ -170,48 +170,21 @@ double PreassembledMass< DataTypes >::getPotentialEnergy( const core::Mechanical -template < class DataTypes > -void PreassembledMass< DataTypes >::addGravityToV(const core::MechanicalParams* mparams, DataVecDeriv& d_v) -{ - if(mparams) - { - VecDeriv& v = *d_v.beginEdit(); - - // gravity - Vec3 g ( this->getContext()->getGravity() * (sofa::core::mechanicalparams::dt(mparams)) ); - Deriv theGravity; - DataTypes::set ( theGravity, g[0], g[1], g[2]); - Deriv hg = theGravity * (sofa::core::mechanicalparams::dt(mparams)); - - // add weight force - for (unsigned int i=0; i -void PreassembledMass< DataTypes >::addForce(const core::MechanicalParams* /*mparams*/, DataVecDeriv& f, const DataVecCoord& /*x*/, const DataVecDeriv& /*v*/) +void PreassembledMass< DataTypes >::addGravitationalForce(const core::MechanicalParams* mparams, DataVecDeriv& f, const DataVecCoord& x, const DataVecDeriv& v, const Deriv& gravity) { - //if gravity was added separately (in solver's "solve" method), then nothing to do here - if(this->m_separateGravity.getValue()) return; + SOFA_UNUSED(mparams); + SOFA_UNUSED(x); + SOFA_UNUSED(v); VecDeriv& _f = *f.beginEdit(); - // gravity - Vec3 g ( this->getContext()->getGravity() ); -// Deriv theGravity; -// DataTypes::set( theGravity, g[0], g[1], g[2] ); - // add weight -// d_massMatrix.template addMul_by_line( _f, theGravity ); - //TODO optimize this!!! VecDeriv gravities(_f.size()); for(size_t i=0 ; i<_f.size() ; ++i ) - DataTypes::set( gravities[i], g[0], g[1], g[2] ); + gravities[i] = gravity; + d_massMatrix.getValue().addMult( _f, gravities ); f.endEdit(); diff --git a/applications/plugins/SofaCUDA/src/SofaCUDA/component/mass/CudaDiagonalMass.cu b/applications/plugins/SofaCUDA/src/SofaCUDA/component/mass/CudaDiagonalMass.cu index dfe3741c21d..3659b428677 100644 --- a/applications/plugins/SofaCUDA/src/SofaCUDA/component/mass/CudaDiagonalMass.cu +++ b/applications/plugins/SofaCUDA/src/SofaCUDA/component/mass/CudaDiagonalMass.cu @@ -40,8 +40,8 @@ extern "C" void DiagonalMassCuda_accFromFf(unsigned int size, const void * mass, const void* f, void* a); void DiagonalMassCuda_accFromFd(unsigned int size, const void * mass, const void* f, void* a); - void DiagonalMassCuda_addForcef(unsigned int size, const void * mass,const double * g, const void* f); - void DiagonalMassCuda_addForced(unsigned int size, const void * mass,const double * g, const void* f); + void DiagonalMassCuda_addGravitationalForcef(unsigned int size, const void * mass,const double * g, const void* f); + void DiagonalMassCuda_addGravitationalForced(unsigned int size, const void * mass,const double * g, const void* f); } @@ -72,7 +72,7 @@ __global__ void DiagonalMassCuda_accFromF_kernel(int size, const real * inv_mass } template -__global__ void DiagonalMassCuda_addForce_kernel(int size, const real * mass, real g_x, real g_y, real g_z, real* f) +__global__ void DiagonalMassCuda_addGravitationalForce_kernel(int size, const real * mass, real g_x, real g_y, real g_z, real* f) { int index = blockIdx.x * BSIZE+threadIdx.x; int index3 = index * 3; @@ -102,11 +102,11 @@ void DiagonalMassCuda_accFromFf(unsigned int size, const void * mass, const void {DiagonalMassCuda_accFromF_kernel<<< grid, threads >>>(size, (const float *) mass, (const float*)f, (float*)a); mycudaDebugError("DiagonalMassCuda_accFromF_kernel");} } -void DiagonalMassCuda_addForcef(unsigned int size, const void * mass,const double * g, const void* f) +void DiagonalMassCuda_addGravitationalForcef(unsigned int size, const void * mass,const double * g, const void* f) { dim3 threads(BSIZE,1); dim3 grid((size+BSIZE-1)/BSIZE,1); - {DiagonalMassCuda_addForce_kernel<<< grid, threads >>>(size,(const float *) mass, g[0],g[1],g[2], (float*)f); mycudaDebugError("DiagonalMassCuda_addForce_kernel");} + {DiagonalMassCuda_addGravitationalForce_kernel<<< grid, threads >>>(size,(const float *) mass, g[0],g[1],g[2], (float*)f); mycudaDebugError("DiagonalMassCuda_addGravitationalForce_kernel");} } @@ -126,11 +126,11 @@ void DiagonalMassCuda_accFromFd(unsigned int size, const void * mass, const void {DiagonalMassCuda_accFromF_kernel<<< grid, threads >>>(size, (const double *) mass, (const double*)f, (double*)a); mycudaDebugError("DiagonalMassCuda_accFromF_kernel");} } -void DiagonalMassCuda_addForced(unsigned int size, const void * mass,const double * g, const void* f) +void DiagonalMassCuda_addGravitationalForced(unsigned int size, const void * mass,const double * g, const void* f) { dim3 threads(BSIZE,1); dim3 grid((size+BSIZE-1)/BSIZE,1); - {DiagonalMassCuda_addForce_kernel<<< grid, threads >>>(size,(const double *) mass, g[0],g[1],g[2], (double*)f); mycudaDebugError("DiagonalMassCuda_addForce_kernel");} + {DiagonalMassCuda_addGravitationalForce_kernel<<< grid, threads >>>(size,(const double *) mass, g[0],g[1],g[2], (double*)f); mycudaDebugError("DiagonalMassCuda_addGravitationalForce_kernel");} } #endif diff --git a/applications/plugins/SofaCUDA/src/SofaCUDA/component/mass/CudaDiagonalMass.h b/applications/plugins/SofaCUDA/src/SofaCUDA/component/mass/CudaDiagonalMass.h index 62fa38325fc..165a03072b5 100644 --- a/applications/plugins/SofaCUDA/src/SofaCUDA/component/mass/CudaDiagonalMass.h +++ b/applications/plugins/SofaCUDA/src/SofaCUDA/component/mass/CudaDiagonalMass.h @@ -62,7 +62,7 @@ template <> void DiagonalMass::accFromF(const core::MechanicalParams* mparams, DataVecDeriv& d_a, const DataVecDeriv& d_f); template<> -void DiagonalMass::addForce(const core::MechanicalParams* mparams, DataVecDeriv& d_f, const DataVecCoord& d_x, const DataVecDeriv& d_v); +void DiagonalMass::addGravitationalForce(const core::MechanicalParams* mparams, DataVecDeriv& d_f, const DataVecCoord& d_x, const DataVecDeriv& d_v, const Deriv& gravity); #ifdef SOFA_GPU_CUDA_DOUBLE @@ -75,7 +75,7 @@ template <> void DiagonalMass::accFromF(const core::MechanicalParams* mparams, DataVecDeriv& d_a, const DataVecDeriv& d_f); template<> -void DiagonalMass::addForce(const core::MechanicalParams* mparams, DataVecDeriv& d_f, const DataVecCoord& d_x, const DataVecDeriv& d_v); +void DiagonalMass::addGravitationalForce(const core::MechanicalParams* mparams, DataVecDeriv& d_f, const DataVecCoord& d_x, const DataVecDeriv& d_v, const Deriv& gravity); // template<> // bool DiagonalMass::addBBox(double* minBBox, double* maxBBox); diff --git a/applications/plugins/SofaCUDA/src/SofaCUDA/component/mass/CudaDiagonalMass.inl b/applications/plugins/SofaCUDA/src/SofaCUDA/component/mass/CudaDiagonalMass.inl index 00a12755061..83c31cbcaaa 100644 --- a/applications/plugins/SofaCUDA/src/SofaCUDA/component/mass/CudaDiagonalMass.inl +++ b/applications/plugins/SofaCUDA/src/SofaCUDA/component/mass/CudaDiagonalMass.inl @@ -42,8 +42,8 @@ extern "C" void DiagonalMassCuda_accFromFf(unsigned int size, const void * mass, const void* f, void* a); void DiagonalMassCuda_accFromFd(unsigned int size, const void * mass, const void* f, void* a); - void DiagonalMassCuda_addForcef(unsigned int size, const void * mass,const void * g, const void* f); - void DiagonalMassCuda_addForced(unsigned int size, const void * mass,const void * g, const void* f); + void DiagonalMassCuda_addGravitationalForcef(unsigned int size, const void * mass,const void * g, const void* f); + void DiagonalMassCuda_addGravitationalForced(unsigned int size, const void * mass,const void * g, const void* f); } @@ -88,24 +88,12 @@ void DiagonalMass::accFromF(const core::MechanicalParams* /*mpar } template <> -void DiagonalMass::addForce(const core::MechanicalParams* /*mparams*/, DataVecDeriv& d_f, const DataVecCoord& /*d_x*/, const DataVecDeriv& /*d_v*/) +void DiagonalMass::addGravitationalForce(const core::MechanicalParams* /*mparams*/, DataVecDeriv& d_f, const DataVecCoord& /*d_x*/, const DataVecDeriv& /*d_v*/, const Deriv& gravity) { VecDeriv& f = *d_f.beginEdit(); - //const VecCoord& x = d_x.getValue(); - //const VecDeriv& v = d_v.getValue(); - type::Vec3d g ( this->getContext()->getGravity() ); const MassVector &masses= d_vertexMass.getValue(); - DiagonalMassCuda_addForcef(masses.size(),masses.deviceRead(),g.ptr(), f.deviceWrite()); - -// // gravity -// Vec3d g ( this->getContext()->getGravity() ); -// Deriv theGravity; -// DataTypes::set ( theGravity, g[0], g[1], g[2]); -// -// for (unsigned int i=0;i::accFromF(const core::MechanicalParams* /*mpar } template<> -void DiagonalMass::addForce(const core::MechanicalParams* /*mparams*/, DataVecDeriv& d_f, const DataVecCoord& /*d_x*/, const DataVecDeriv& /*d_v*/) +void DiagonalMass::addGravitationalForce(const core::MechanicalParams* /*mparams*/, DataVecDeriv& d_f, const DataVecCoord& /*d_x*/, const DataVecDeriv& /*d_v*/, const Deriv& gravity) { VecDeriv& f = *d_f.beginEdit(); - //const VecCoord& x = d_x.getValue(); - //const VecDeriv& v = d_v.getValue(); - Vec3d g ( this->getContext()->getGravity() ); const MassVector &masses= d_vertexMass.getValue(); - DiagonalMassCuda_addForced(masses.size(),masses.deviceRead(),g.ptr(), f.deviceWrite()); + DiagonalMassCuda_addGravitationalForced(masses.size(),masses.deviceRead(),gravity.ptr(), f.deviceWrite()); d_f.endEdit(); } diff --git a/applications/plugins/SofaCUDA/src/SofaCUDA/component/mass/CudaMeshMatrixMass.cu b/applications/plugins/SofaCUDA/src/SofaCUDA/component/mass/CudaMeshMatrixMass.cu index 4ad614a9b80..5e94c4f47e9 100644 --- a/applications/plugins/SofaCUDA/src/SofaCUDA/component/mass/CudaMeshMatrixMass.cu +++ b/applications/plugins/SofaCUDA/src/SofaCUDA/component/mass/CudaMeshMatrixMass.cu @@ -36,15 +36,15 @@ namespace cuda extern "C" { void MeshMatrixMassCuda_addMDx3f(unsigned int size, float factor, float massLumpingCoeff, const void * vertexMass, const void* dx, void* res); - void MeshMatrixMassCuda_addForce3f(int dim, void * f, const void * vertexMass, const double * gravity, float massLumpingCoeff); + void MeshMatrixMassCuda_addGravitationalForce3f(int dim, void * f, const void * vertexMass, const float * gravity, float massLumpingCoeff); void MeshMatrixMassCuda_accFromF3f(int dim, void * acc, const void * f, const void * vertexMass, float massLumpingCoeff); void MeshMatrixMassCuda_addMDx2f(unsigned int size, float factor, float massLumpingCoeff, const void * vertexMass, const void* dx, void* res); - void MeshMatrixMassCuda_addForce2f(int dim, void * f, const void * vertexMass, const double * gravity, float massLumpingCoeff); + void MeshMatrixMassCuda_addGravitationalForce2f(int dim, void * f, const void * vertexMass, const float * gravity, float massLumpingCoeff); void MeshMatrixMassCuda_accFromF2f(int dim, void * acc, const void * f, const void * vertexMass, float massLumpingCoeff); void MeshMatrixMassCuda_addMDx1f(unsigned int size, float factor, float massLumpingCoeff, const void * vertexMass, const void* dx, void* res); - void MeshMatrixMassCuda_addForce1f(int dim, void * f, const void * vertexMass, const double * gravity, float massLumpingCoeff); + void MeshMatrixMassCuda_addGravitationalForce1f(int dim, void * f, const void * vertexMass, const float * gravity, float massLumpingCoeff); void MeshMatrixMassCuda_accFromF1f(int dim, void * acc, const void * f, const void * vertexMass, float massLumpingCoeff); } @@ -85,7 +85,7 @@ __global__ void MeshMatrixMassCuda_addMDx3f_kernel(real factor, real massLumping } template -__global__ void MeshMatrixMassCuda_addForce3f_kernel(int dim, real * f, const real * vertexMass, real g_x, real g_y, real massLumpingCoeff) +__global__ void MeshMatrixMassCuda_addGravitationalForce3f_kernel(int dim, real * f, const real * vertexMass, real g_x, real g_y, real g_z, real massLumpingCoeff) { int index = blockIdx.x * BSIZE+threadIdx.x; int index2 = index * 3; @@ -93,7 +93,7 @@ __global__ void MeshMatrixMassCuda_addForce3f_kernel(int dim, real * f, const r { f[index2+0] += vertexMass[index] * massLumpingCoeff * g_x; f[index2+1] += vertexMass[index] * massLumpingCoeff * g_y; - f[index2+2] += vertexMass[index] * massLumpingCoeff * g_y; + f[index2+2] += vertexMass[index] * massLumpingCoeff * g_z; } } @@ -145,11 +145,11 @@ void MeshMatrixMassCuda_addMDx3f(unsigned int size, float factor, float massLump {MeshMatrixMassCuda_addMDx3f_kernel<<< grid, threads >>>(factor, massLumpingCoeff, (const float *) vertexMass, (const float *) dx, (float*) res); mycudaDebugError("MeshMatrixMassCuda_addMDx2f_kernel");} } -void MeshMatrixMassCuda_addForce3f(int dim, void * f, const void * vertexMass, const double * g, float massLumpingCoeff) +void MeshMatrixMassCuda_addGravitationalForce3f(int dim, void * f, const void * vertexMass, const float * g, float massLumpingCoeff) { dim3 threads(BSIZE,1); dim3 grid((dim+BSIZE-1)/BSIZE,1); - {MeshMatrixMassCuda_addForce3f_kernel<<< grid, threads >>>(dim, (float *) f, (const float *) vertexMass, g[0], g[1], massLumpingCoeff); mycudaDebugError("MeshMatrixMassCuda_addForce2f_kernel");} + {MeshMatrixMassCuda_addGravitationalForce3f_kernel<<< grid, threads >>>(dim, (float *) f, (const float *) vertexMass, g[0], g[1], g[2], massLumpingCoeff); mycudaDebugError("MeshMatrixMassCuda_addGravitationalForce2f_kernel");} } void MeshMatrixMassCuda_accFromF3f(int dim, void * acc, const void * f, const void * vertexMass, float massLumpingCoeff) @@ -201,7 +201,7 @@ __global__ void MeshMatrixMassCuda_addMDx2f_kernel(real factor, real massLumping } template -__global__ void MeshMatrixMassCuda_addForce2f_kernel(int dim, real * f, const real * vertexMass, real g_x, real g_y, real massLumpingCoeff) +__global__ void MeshMatrixMassCuda_addGravitationalForce2f_kernel(int dim, real * f, const real * vertexMass, real g_x, real g_y, real massLumpingCoeff) { int index = blockIdx.x * BSIZE+threadIdx.x; int index2 = index * 2; @@ -260,11 +260,11 @@ void MeshMatrixMassCuda_addMDx2f(unsigned int size, float factor, float massLump {MeshMatrixMassCuda_addMDx2f_kernel<<< grid, threads >>>(factor, massLumpingCoeff, (const float *) vertexMass, (const float *) dx, (float*) res); mycudaDebugError("MeshMatrixMassCuda_addMDx2f_kernel");} } -void MeshMatrixMassCuda_addForce2f(int dim, void * f, const void * vertexMass, const double * g, float massLumpingCoeff) +void MeshMatrixMassCuda_addGravitationalForce2f(int dim, void * f, const void * vertexMass, const float * g, float massLumpingCoeff) { dim3 threads(BSIZE,1); dim3 grid((dim+BSIZE-1)/BSIZE,1); - {MeshMatrixMassCuda_addForce2f_kernel<<< grid, threads >>>(dim, (float *) f, (const float *) vertexMass, g[0], g[1], massLumpingCoeff); mycudaDebugError("MeshMatrixMassCuda_addForce2f_kernel");} + {MeshMatrixMassCuda_addGravitationalForce2f_kernel<<< grid, threads >>>(dim, (float *) f, (const float *) vertexMass, g[0], g[1], massLumpingCoeff); mycudaDebugError("MeshMatrixMassCuda_addGravitationalForce2f_kernel");} } void MeshMatrixMassCuda_accFromF2f(int dim, void * acc, const void * f, const void * vertexMass, float massLumpingCoeff) @@ -312,7 +312,7 @@ __global__ void MeshMatrixMassCuda_addMDx1f_kernel(real factor, real massLumping } template -__global__ void MeshMatrixMassCuda_addForce1f_kernel(int dim, real * f, const real * vertexMass, real g_x, real g_y, real massLumpingCoeff) +__global__ void MeshMatrixMassCuda_addGravitationalForce1f_kernel(int dim, real * f, const real * vertexMass, real g_x, real massLumpingCoeff) { int index = blockIdx.x * BSIZE+threadIdx.x; if (index < dim) @@ -365,11 +365,11 @@ void MeshMatrixMassCuda_addMDx1f(unsigned int size, float factor, float massLump {MeshMatrixMassCuda_addMDx1f_kernel<<< grid, threads >>>(factor, massLumpingCoeff, (const float *) vertexMass, (const float *) dx, (float*) res); mycudaDebugError("MeshMatrixMassCuda_addMDx2f_kernel");} } -void MeshMatrixMassCuda_addForce1f(int dim, void * f, const void * vertexMass, const double * g, float massLumpingCoeff) +void MeshMatrixMassCuda_addGravitationalForce1f(int dim, void * f, const void * vertexMass, const float * g, float massLumpingCoeff) { dim3 threads(BSIZE,1); dim3 grid((dim+BSIZE-1)/BSIZE,1); - {MeshMatrixMassCuda_addForce1f_kernel<<< grid, threads >>>(dim, (float *) f, (const float *) vertexMass, g[0], g[1], massLumpingCoeff); mycudaDebugError("MeshMatrixMassCuda_addForce2f_kernel");} + {MeshMatrixMassCuda_addGravitationalForce1f_kernel<<< grid, threads >>>(dim, (float *) f, (const float *) vertexMass, g[0], massLumpingCoeff); mycudaDebugError("MeshMatrixMassCuda_addGravitationalForce2f_kernel");} } void MeshMatrixMassCuda_accFromF1f(int dim, void * acc, const void * f, const void * vertexMass, float massLumpingCoeff) diff --git a/applications/plugins/SofaCUDA/src/SofaCUDA/component/mass/CudaMeshMatrixMass.h b/applications/plugins/SofaCUDA/src/SofaCUDA/component/mass/CudaMeshMatrixMass.h index 98c7c3e92c4..b7b311da924 100644 --- a/applications/plugins/SofaCUDA/src/SofaCUDA/component/mass/CudaMeshMatrixMass.h +++ b/applications/plugins/SofaCUDA/src/SofaCUDA/component/mass/CudaMeshMatrixMass.h @@ -50,7 +50,7 @@ template<> void MeshMatrixMass::addMDx(const core::MechanicalParams*, DataVecDeriv& f, const DataVecDeriv& dx, SReal factor); template<> -void MeshMatrixMass::addForce(const core::MechanicalParams*, DataVecDeriv& /*vf*/, const DataVecCoord& /* */, const DataVecDeriv& /* */); +void MeshMatrixMass::addGravitationalForce(const core::MechanicalParams*, DataVecDeriv& /*vf*/, const DataVecCoord& /* */, const DataVecDeriv& /* */, const Deriv& /* */); template<> void MeshMatrixMass::accFromF(const core::MechanicalParams*, DataVecDeriv& a, const DataVecDeriv& f); @@ -73,7 +73,7 @@ template<> void MeshMatrixMass::addMDx(const core::MechanicalParams*, DataVecDeriv& f, const DataVecDeriv& dx, SReal factor); template<> -void MeshMatrixMass::addForce(const core::MechanicalParams*, DataVecDeriv& /*vf*/, const DataVecCoord& /* */, const DataVecDeriv& /* */); +void MeshMatrixMass::addGravitationalForce(const core::MechanicalParams*, DataVecDeriv& /*vf*/, const DataVecCoord& /* */, const DataVecDeriv& /* */, const Deriv& /* */); template<> void MeshMatrixMass::accFromF(const core::MechanicalParams*, DataVecDeriv& a, const DataVecDeriv& f); @@ -96,7 +96,7 @@ template<> void MeshMatrixMass::addMDx(const core::MechanicalParams*, DataVecDeriv& f, const DataVecDeriv& dx, SReal factor); template<> -void MeshMatrixMass::addForce(const core::MechanicalParams*, DataVecDeriv& /*vf*/, const DataVecCoord& /* */, const DataVecDeriv& /* */); +void MeshMatrixMass::addGravitationalForce(const core::MechanicalParams*, DataVecDeriv& /*vf*/, const DataVecCoord& /* */, const DataVecDeriv& /* */, const Deriv& /* */); template<> void MeshMatrixMass::accFromF(const core::MechanicalParams*, DataVecDeriv& a, const DataVecDeriv& f); diff --git a/applications/plugins/SofaCUDA/src/SofaCUDA/component/mass/CudaMeshMatrixMass.inl b/applications/plugins/SofaCUDA/src/SofaCUDA/component/mass/CudaMeshMatrixMass.inl index b806eaa6fd3..66495d2c3f4 100644 --- a/applications/plugins/SofaCUDA/src/SofaCUDA/component/mass/CudaMeshMatrixMass.inl +++ b/applications/plugins/SofaCUDA/src/SofaCUDA/component/mass/CudaMeshMatrixMass.inl @@ -38,15 +38,15 @@ using namespace sofa::gpu::cuda; extern "C" { void MeshMatrixMassCuda_addMDx3f(unsigned int size, float factor, float massLumpingCoeff,const void * vertexMass, const void* dx, void* res); - void MeshMatrixMassCuda_addForce3f(int dim, void * f, const void * vertexMass, const double * gravity, float massLumpingCoeff); + void MeshMatrixMassCuda_addGravitationalForce3f(int dim, void * f, const void * vertexMass, const float * gravity, float massLumpingCoeff); void MeshMatrixMassCuda_accFromF3f(int dim, void * acc, const void * f, const void * vertexMass, float massLumpingCoeff); void MeshMatrixMassCuda_addMDx2f(unsigned int size, float factor, float massLumpingCoeff,const void * vertexMass, const void* dx, void* res); - void MeshMatrixMassCuda_addForce2f(int dim, void * f, const void * vertexMass, const double * gravity, float massLumpingCoeff); + void MeshMatrixMassCuda_addGravitationalForce2f(int dim, void * f, const void * vertexMass, const float * gravity, float massLumpingCoeff); void MeshMatrixMassCuda_accFromF2f(int dim, void * acc, const void * f, const void * vertexMass, float massLumpingCoeff); - void MeshMatrixMassCuda_addMDx1f(unsigned int size, float factor, float massLumpingCoeff,const void * vertexMass, const void* dx, void* res); - void MeshMatrixMassCuda_addForce1f(int dim, void * f, const void * vertexMass, const double * gravity, float massLumpingCoeff); + void MeshMatrixMassCuda_addMDx1fc(unsigned int size, float factor, float massLumpingCoeff,const void * vertexMass, const void* dx, void* res); + void MeshMatrixMassCuda_addGravitationalForce1f(int dim, void * f, const void * vertexMass, const float * gravity, float massLumpingCoeff); void MeshMatrixMassCuda_accFromF1f(int dim, void * acc, const void * f, const void * vertexMass, float massLumpingCoeff); } }// cuda @@ -85,13 +85,12 @@ void MeshMatrixMass::addMDx(const core::MechanicalParams* /*mpar } template<> -void MeshMatrixMass::addForce(const core::MechanicalParams* /*mparams*/, DataVecDeriv& d_f, const DataVecCoord& /* */, const DataVecDeriv& /* */) +void MeshMatrixMass::addGravitationalForce(const core::MechanicalParams* d_f, DataVecDeriv& /*vf*/, const DataVecCoord& /* */, const DataVecDeriv& /* */, const Deriv& gravity) { VecDeriv& f = *d_f.beginEdit(); const CudaVector& vertexMass = data.vMass; - type::Vec3d g ( this->getContext()->getGravity() ); - MeshMatrixMassCuda_addForce3f( vertexMass.size(), f.deviceWrite(), vertexMass.deviceRead(), g.ptr(), (float) m_massLumpingCoeff); + MeshMatrixMassCuda_addGravitationalForce3f( vertexMass.size(), f.deviceWrite(), vertexMass.deviceRead(), gravity.ptr(), (float) m_massLumpingCoeff); d_f.endEdit(); } @@ -134,13 +133,12 @@ void MeshMatrixMass::addMDx(const core::MechanicalParams* /*mpar } template<> -void MeshMatrixMass::addForce(const core::MechanicalParams* /*mparams*/, DataVecDeriv& d_f, const DataVecCoord& /* */, const DataVecDeriv& /* */) +void MeshMatrixMass::addGravitationalForce(const core::MechanicalParams*, DataVecDeriv& d_f, const DataVecCoord& /* */, const DataVecDeriv& /* */, const Deriv& gravity) { VecDeriv& f = *d_f.beginEdit(); const CudaVector& vertexMass = data.vMass; - type::Vec3d g ( this->getContext()->getGravity() ); - MeshMatrixMassCuda_addForce2f( vertexMass.size(), f.deviceWrite(), vertexMass.deviceRead(), g.ptr(), (float) m_massLumpingCoeff); + MeshMatrixMassCuda_addGravitationalForce2f( vertexMass.size(), f.deviceWrite(), vertexMass.deviceRead(), gravity.ptr(), (float) m_massLumpingCoeff); d_f.endEdit(); } @@ -183,13 +181,12 @@ void MeshMatrixMass::addMDx(const core::MechanicalParams* /*mpar } template<> -void MeshMatrixMass::addForce(const core::MechanicalParams* /*mparams*/, DataVecDeriv& d_f, const DataVecCoord& /* */, const DataVecDeriv& /* */) +void MeshMatrixMass::addGravitationalForce(const core::MechanicalParams*, DataVecDeriv& d_f, const DataVecCoord& /* */, const DataVecDeriv& /* */, const Deriv& gravity) { VecDeriv& f = *d_f.beginEdit(); const CudaVector& vertexMass = data.vMass; - type::Vec3d g ( this->getContext()->getGravity() ); - MeshMatrixMassCuda_addForce1f( vertexMass.size(), f.deviceWrite(), vertexMass.deviceRead(), g.ptr(), (float) m_massLumpingCoeff); + MeshMatrixMassCuda_addGravitationalForce1f( vertexMass.size(), f.deviceWrite(), vertexMass.deviceRead(), gravity.ptr(), (float) m_massLumpingCoeff); d_f.endEdit(); } diff --git a/applications/plugins/SofaCUDA/src/SofaCUDA/component/mass/CudaUniformMass.cu b/applications/plugins/SofaCUDA/src/SofaCUDA/component/mass/CudaUniformMass.cu index dbd2ea4347b..815010bfd08 100644 --- a/applications/plugins/SofaCUDA/src/SofaCUDA/component/mass/CudaUniformMass.cu +++ b/applications/plugins/SofaCUDA/src/SofaCUDA/component/mass/CudaUniformMass.cu @@ -39,25 +39,25 @@ extern "C" { void UniformMassCuda3f_addMDx(unsigned int size, float mass, void* res, const void* dx); void UniformMassCuda3f_accFromF(unsigned int size, float mass, void* a, const void* f); - void UniformMassCuda3f_addForce(unsigned int size, const float *mg, void* f); + void UniformMassCuda3f_addGravitationalForce(unsigned int size, const float *mg, void* f); void UniformMassCuda3f1_addMDx(unsigned int size, float mass, void* res, const void* dx); void UniformMassCuda3f1_accFromF(unsigned int size, float mass, void* a, const void* f); - void UniformMassCuda3f1_addForce(unsigned int size, const float *mg, void* f); + void UniformMassCuda3f1_addGravitationalForce(unsigned int size, const float *mg, void* f); void UniformMassCudaRigid3f_addMDx(unsigned int size, float mass, void* res, const void* dx); void UniformMassCudaRigid3f_accFromF(unsigned int size, float mass, void* a, const void* dx); - void UniformMassCudaRigid3f_addForce(unsigned int size, const float* mg, void* f); + void UniformMassCudaRigid3f_addGravitationalForce(unsigned int size, const float* mg, void* f); #ifdef SOFA_GPU_CUDA_DOUBLE void UniformMassCuda3d_addMDx(unsigned int size, double mass, void* res, const void* dx); void UniformMassCuda3d_accFromF(unsigned int size, double mass, void* a, const void* f); - void UniformMassCuda3d_addForce(unsigned int size, const double *mg, void* f); + void UniformMassCuda3d_addGravitationalForce(unsigned int size, const double *mg, void* f); void UniformMassCuda3d1_addMDx(unsigned int size, double mass, void* res, const void* dx); void UniformMassCuda3d1_accFromF(unsigned int size, double mass, void* a, const void* f); - void UniformMassCuda3d1_addForce(unsigned int size, const double *mg, void* f); + void UniformMassCuda3d1_addGravitationalForce(unsigned int size, const double *mg, void* f); #endif // SOFA_GPU_CUDA_DOUBLE @@ -402,21 +402,21 @@ void UniformMassCudaRigid3f_accFromF(unsigned int size, float mass, void* a, con mycudaDebugError("UniformMassCudaRigid3f_accFromF"); } -void UniformMassCuda3f_addForce(unsigned int size, const float *mg, void* f) +void UniformMassCuda3f_addGravitationalForce(unsigned int size, const float *mg, void* f) { dim3 threads(BSIZE,1); dim3 grid((size+BSIZE-1)/BSIZE,1); {UniformMassCuda3t_addForce_kernel<<< grid, threads >>>(size, mg[0], mg[1], mg[2], (float*)f); mycudaDebugError("UniformMassCuda3t_addForce_kernel");} } -void UniformMassCuda3f1_addForce(unsigned int size, const float *mg, void* f) +void UniformMassCuda3f1_addGravitationalForce(unsigned int size, const float *mg, void* f) { dim3 threads(BSIZE,1); dim3 grid((size+BSIZE-1)/BSIZE,1); {UniformMassCuda3t1_addForce_kernel<<< grid, threads >>>(size, mg[0], mg[1], mg[2], (CudaVec4*)f); mycudaDebugError("UniformMassCuda3t1_addForce_kernel");} } -void UniformMassCudaRigid3f_addForce(unsigned int size, const float* mg, void* f) +void UniformMassCudaRigid3f_addGravitationalForce(unsigned int size, const float* mg, void* f) { dim3 threads(BSIZE,1); dim3 grid((size+BSIZE-1)/BSIZE,1); @@ -464,14 +464,14 @@ void UniformMassCuda3d1_accFromF(unsigned int size, double mass, void* a, const //UniformMassCuda1t_accFromF_kernel<<< grid, threads >>>(4*size, 1.0f/mass, (double*)a, (const double*)f); } -void UniformMassCuda3d_addForce(unsigned int size, const double *mg, void* f) +void UniformMassCuda3d_addGravitationalForce(unsigned int size, const double *mg, void* f) { dim3 threads(BSIZE,1); dim3 grid((size+BSIZE-1)/BSIZE,1); {UniformMassCuda3t_addForce_kernel<<< grid, threads >>>(size, mg[0], mg[1], mg[2], (double*)f); mycudaDebugError("UniformMassCuda3t_addForce_kernel");} } -void UniformMassCuda3d1_addForce(unsigned int size, const double *mg, void* f) +void UniformMassCuda3d1_addGravitationalForce(unsigned int size, const double *mg, void* f) { dim3 threads(BSIZE,1); dim3 grid((size+BSIZE-1)/BSIZE,1); diff --git a/applications/plugins/SofaCUDA/src/SofaCUDA/component/mass/CudaUniformMass.inl b/applications/plugins/SofaCUDA/src/SofaCUDA/component/mass/CudaUniformMass.inl index d7ed89a8697..1963f6e2394 100644 --- a/applications/plugins/SofaCUDA/src/SofaCUDA/component/mass/CudaUniformMass.inl +++ b/applications/plugins/SofaCUDA/src/SofaCUDA/component/mass/CudaUniformMass.inl @@ -1,4 +1,4 @@ -/****************************************************************************** +/****************************************************************************** * SOFA, Simulation Open-Framework Architecture * * (c) 2006 INRIA, USTL, UJF, CNRS, MGH * * * @@ -39,25 +39,25 @@ extern "C" { void UniformMassCuda3f_addMDx(unsigned int size, float mass, void* res, const void* dx); void UniformMassCuda3f_accFromF(unsigned int size, float mass, void* a, const void* f); - void UniformMassCuda3f_addForce(unsigned int size, const float *mg, void* f); + void UniformMassCuda3f_addGravitationalForce(unsigned int size, const float *mg, void* f); void UniformMassCuda3f1_addMDx(unsigned int size, float mass, void* res, const void* dx); void UniformMassCuda3f1_accFromF(unsigned int size, float mass, void* a, const void* f); - void UniformMassCuda3f1_addForce(unsigned int size, const float *mg, void* f); + void UniformMassCuda3f1_addGravitationalForce(unsigned int size, const float *mg, void* f); - void UniformMassCudaRigid3f_addMDx(unsigned int size, float mass, void* res, const void* dx); - void UniformMassCudaRigid3f_accFromF(unsigned int size, float mass, void* a, const void* dx); - void UniformMassCudaRigid3f_addForce(unsigned int size, const float* mg, void* f); + void UniformMassCudaRigid3f_addMDx(unsigned int size, float mass, void* res, const void* dx); + void UniformMassCudaRigid3f_accFromF(unsigned int size, float mass, void* a, const void* dx); + void UniformMassCudaRigid3f_addGravitationalForce(unsigned int size, const float* mg, void* f); #ifdef SOFA_GPU_CUDA_DOUBLE void UniformMassCuda3d_addMDx(unsigned int size, double mass, void* res, const void* dx); void UniformMassCuda3d_accFromF(unsigned int size, double mass, void* a, const void* f); - void UniformMassCuda3d_addForce(unsigned int size, const double *mg, void* f); + void UniformMassCuda3d_addGravitationalForce(unsigned int size, const double *mg, void* f); void UniformMassCuda3d1_addMDx(unsigned int size, double mass, void* res, const void* dx); void UniformMassCuda3d1_accFromF(unsigned int size, double mass, void* a, const void* f); - void UniformMassCuda3d1_addForce(unsigned int size, const double *mg, void* f); + void UniformMassCuda3d1_addGravitationalForce(unsigned int size, const double *mg, void* f); #endif // SOFA_GPU_CUDA_DOUBLE } @@ -98,18 +98,13 @@ void UniformMass::accFromF(const core::MechanicalParams* /*mpara } template <> -void UniformMass::addForce(const core::MechanicalParams* /*mparams*/, DataVecDeriv& d_f, const DataVecCoord& /*d_x*/, const DataVecDeriv& /*d_v*/) +void UniformMass::addGravitationalForce(const core::MechanicalParams* /*mparams*/, DataVecDeriv& d_f, const DataVecCoord& /*d_x*/, const DataVecDeriv& /*d_v*/, const Deriv& gravity) { VecDeriv& f = *d_f.beginEdit(); - //const VecCoord& x = d_x.getValue(); - //const VecDeriv& v = d_v.getValue(); // weight - type::Vec3d g ( this->getContext()->getGravity() ); - Deriv theGravity; - DataTypes::set( theGravity, g[0], g[1], g[2]); - Deriv mg = theGravity * d_vertexMass.getValue(); - UniformMassCuda3f_addForce(f.size(), mg.ptr(), f.deviceWrite()); + Deriv mg = gravity * d_vertexMass.getValue(); + UniformMassCuda3f_addGravitationalForce(f.size(), mg.ptr(), f.deviceWrite()); d_f.endEdit(); } @@ -137,18 +132,13 @@ void UniformMass::accFromF(const core::MechanicalParams* /*mpar } template <> -void UniformMass::addForce(const core::MechanicalParams* /*mparams*/, DataVecDeriv& d_f, const DataVecCoord& /*d_x*/, const DataVecDeriv& /*d_v*/) +void UniformMass::addGravitationalForce(const core::MechanicalParams* /*mparams*/, DataVecDeriv& d_f, const DataVecCoord& /*d_x*/, const DataVecDeriv& /*d_v*/, const Deriv& gravity) { VecDeriv& f = *d_f.beginEdit(); - //const VecCoord& x = d_x.getValue(); - //const VecDeriv& v = d_v.getValue(); // weight - type::Vec3d g ( this->getContext()->getGravity() ); - Deriv theGravity; - DataTypes::set( theGravity, g[0], g[1], g[2]); - Deriv mg = theGravity * d_vertexMass.getValue(); - UniformMassCuda3f1_addForce(f.size(), mg.ptr(), f.deviceWrite()); + Deriv mg = gravity * d_vertexMass.getValue(); + UniformMassCuda3f1_addGravitationalForce(f.size(), mg.ptr(), f.deviceWrite()); d_f.endEdit(); } @@ -180,32 +170,28 @@ void UniformMass::accFromF(const core::MechanicalPa } template<> -void UniformMass::addForce(const core::MechanicalParams * /*mparams*/, DataVecDeriv &f, const DataVecCoord& /*x*/, const DataVecDeriv& /*v*/) +void UniformMass::addGravitationalForce(const core::MechanicalParams * /*mparams*/, DataVecDeriv &f, const DataVecCoord& /*x*/, const DataVecDeriv& /*v*/, const Deriv& gravity) { - VecDeriv& _f = *f.beginEdit(); - type::Vec3d g(this->getContext()->getGravity()); float m = d_vertexMass.getValue().mass; - const float mg[] = { (float)(m*g(0)), (float)(m*g(1)), (float)(m*g(2)) }; - UniformMassCudaRigid3f_addForce(_f.size(), mg, _f.deviceWrite()); + const float mg[] = { (float)(m*gravity[0]), (float)(m*gravity[1])), (float)(m*gravity[2]) }; + UniformMassCudaRigid3f_addGravitationalForce(_f.size(), mg, _f.deviceWrite()); f.endEdit(); - } template <> -SReal UniformMass::getPotentialEnergy(const core::MechanicalParams* /*mparams*/, const DataVecCoord& d_x) const +SReal UniformMass::getGravitationalPotentialEnergy(const core::MechanicalParams* /*mparams*/, const DataVecCoord& d_x, const Deriv& gravity) const { const VecCoord& x = d_x.getValue(); SReal e = 0; // gravity - type::Vec3d g ( this->getContext()->getGravity() ); for (unsigned int i=0; i::accFromF(const core::MechanicalParams* /*mpara } template <> -void UniformMass::addForce(const core::MechanicalParams* /*mparams*/, DataVecDeriv& d_f, const DataVecCoord& /*d_x*/, const DataVecDeriv& /*d_v*/) +void UniformMass::addGravitationalForce(const core::MechanicalParams* /*mparams*/, DataVecDeriv& d_f, const DataVecCoord& /*d_x*/, const DataVecDeriv& /*d_v*/, const Deriv gravity) { VecDeriv& f = *d_f.beginEdit(); - //const VecCoord& x = d_x.getValue(); - //const VecDeriv& v = d_v.getValue(); // weight - Vec3d g ( this->getContext()->getGravity() ); - Deriv theGravity; - DataTypes::set( theGravity, g[0], g[1], g[2]); - Deriv mg = theGravity * d_vertexMass.getValue(); - UniformMassCuda3d_addForce(f.size(), mg.ptr(), f.deviceWrite()); + Deriv mg = gravity * d_vertexMass.getValue(); + UniformMassCuda3d_addGravitationalForce(f.size(), mg.ptr(), f.deviceWrite()); d_f.endEdit(); } @@ -330,18 +311,13 @@ void UniformMass::accFromF(const core::MechanicalParams* /*mpar } template <> -void UniformMass::addForce(const core::MechanicalParams* /*mparams*/, DataVecDeriv& d_f, const DataVecCoord& /*d_x*/, const DataVecDeriv& /*d_v*/) +void UniformMass::addGravitationalForce(const core::MechanicalParams* /*mparams*/, DataVecDeriv& d_f, const DataVecCoord& /*d_x*/, const DataVecDeriv& /*d_v*/, const Deriv gravity) { VecDeriv& f = *d_f.beginEdit(); - //const VecCoord& x = d_x.getValue(); - //const VecDeriv& v = d_v.getValue(); // weight - Vec3d g ( this->getContext()->getGravity() ); - Deriv theGravity; - DataTypes::set( theGravity, g[0], g[1], g[2]); - Deriv mg = theGravity * d_vertexMass.getValue(); - UniformMassCuda3d1_addForce(f.size(), mg.ptr(), f.deviceWrite()); + Deriv mg = gravity * d_vertexMass.getValue(); + UniformMassCuda3d1_addGravitationalForce(f.size(), mg.ptr(), f.deviceWrite()); d_f.endEdit(); } diff --git a/examples/Component/SceneUtility/Gravity.scn b/examples/Component/SolidMechanics/FEM/GravityForceField.scn similarity index 97% rename from examples/Component/SceneUtility/Gravity.scn rename to examples/Component/SolidMechanics/FEM/GravityForceField.scn index da33a81be81..e8e46aa88b8 100644 --- a/examples/Component/SceneUtility/Gravity.scn +++ b/examples/Component/SolidMechanics/FEM/GravityForceField.scn @@ -22,10 +22,10 @@ - + @@ -45,10 +45,10 @@ - +