diff --git a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/FixedLagrangianConstraint.h b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/FixedLagrangianConstraint.h index 03944cdd4ba..22f083180c3 100644 --- a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/FixedLagrangianConstraint.h +++ b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/FixedLagrangianConstraint.h @@ -22,7 +22,7 @@ #pragma once #include -#include +#include #include #include #include @@ -32,10 +32,10 @@ namespace sofa::component::constraint::lagrangian::model { template< class DataTypes > -class FixedLagrangianConstraint : public core::behavior::Constraint +class FixedLagrangianConstraint : public core::behavior::LagrangianConstraint { public: - SOFA_CLASS(SOFA_TEMPLATE(FixedLagrangianConstraint,DataTypes), SOFA_TEMPLATE(core::behavior::Constraint,DataTypes)); + SOFA_CLASS(SOFA_TEMPLATE(FixedLagrangianConstraint,DataTypes), SOFA_TEMPLATE(core::behavior::LagrangianConstraint,DataTypes)); typedef typename DataTypes::VecCoord VecCoord; typedef typename DataTypes::VecDeriv VecDeriv; @@ -43,7 +43,7 @@ class FixedLagrangianConstraint : public core::behavior::Constraint typedef typename DataTypes::Deriv Deriv; typedef typename DataTypes::MatrixDeriv MatrixDeriv; typedef typename core::behavior::MechanicalState MechanicalState; - typedef typename core::behavior::Constraint Inherit; + typedef typename core::behavior::LagrangianConstraint Inherit; typedef typename DataTypes::MatrixDeriv::RowIterator MatrixDerivRowIterator; typedef core::objectmodel::Data DataVecCoord; diff --git a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/StopperLagrangianConstraint.h b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/StopperLagrangianConstraint.h index 4d04c866b9c..41f86c73f22 100644 --- a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/StopperLagrangianConstraint.h +++ b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/StopperLagrangianConstraint.h @@ -22,7 +22,7 @@ #pragma once #include -#include +#include #include #include #include @@ -65,10 +65,10 @@ class StopperLagrangianConstraintResolution1Dof : public core::behavior::Constra }; template< class DataTypes > -class StopperLagrangianConstraint : public core::behavior::Constraint +class StopperLagrangianConstraint : public core::behavior::LagrangianConstraint { public: - SOFA_CLASS(SOFA_TEMPLATE(StopperLagrangianConstraint,DataTypes), SOFA_TEMPLATE(core::behavior::Constraint,DataTypes)); + SOFA_CLASS(SOFA_TEMPLATE(StopperLagrangianConstraint,DataTypes), SOFA_TEMPLATE(core::behavior::LagrangianConstraint,DataTypes)); typedef typename DataTypes::VecCoord VecCoord; typedef typename DataTypes::VecDeriv VecDeriv; @@ -77,7 +77,7 @@ class StopperLagrangianConstraint : public core::behavior::Constraint typedef typename DataTypes::MatrixDeriv MatrixDeriv; typedef typename Coord::value_type Real; typedef typename core::behavior::MechanicalState MechanicalState; - typedef typename core::behavior::Constraint Inherit; + typedef typename core::behavior::LagrangianConstraint Inherit; typedef typename DataTypes::MatrixDeriv::RowIterator MatrixDerivRowIterator; typedef core::objectmodel::Data DataVecCoord; diff --git a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/UniformLagrangianConstraint.h b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/UniformLagrangianConstraint.h index ffdaf656bc6..905ead566c4 100644 --- a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/UniformLagrangianConstraint.h +++ b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/UniformLagrangianConstraint.h @@ -22,16 +22,16 @@ #pragma once #include -#include +#include namespace sofa::component::constraint::lagrangian::model { template < class DataTypes > -class UniformLagrangianConstraint : public sofa::core::behavior::Constraint< DataTypes > +class UniformLagrangianConstraint : public sofa::core::behavior::LagrangianConstraint< DataTypes > { public: - SOFA_CLASS(SOFA_TEMPLATE(UniformLagrangianConstraint, DataTypes), SOFA_TEMPLATE(sofa::core::behavior::Constraint, DataTypes)); + SOFA_CLASS(SOFA_TEMPLATE(UniformLagrangianConstraint, DataTypes), SOFA_TEMPLATE(sofa::core::behavior::LagrangianConstraint, DataTypes)); typedef typename DataTypes::VecCoord VecCoord; typedef typename DataTypes::VecDeriv VecDeriv; diff --git a/Sofa/framework/Core/CMakeLists.txt b/Sofa/framework/Core/CMakeLists.txt index 5948823e627..a712ab08e63 100644 --- a/Sofa/framework/Core/CMakeLists.txt +++ b/Sofa/framework/Core/CMakeLists.txt @@ -69,8 +69,8 @@ set(HEADER_FILES ${SRC_ROOT}/behavior/BaseProjectiveConstraintSet.h ${SRC_ROOT}/behavior/BaseRotationFinder.h ${SRC_ROOT}/behavior/BaseVectorOperations.h - ${SRC_ROOT}/behavior/BlockType.h ${SRC_ROOT}/behavior/BlocMatrixWriter.h + ${SRC_ROOT}/behavior/BlockType.h ${SRC_ROOT}/behavior/Constraint.h ${SRC_ROOT}/behavior/Constraint.inl ${SRC_ROOT}/behavior/ConstraintCorrection.h @@ -80,11 +80,13 @@ set(HEADER_FILES ${SRC_ROOT}/behavior/DefaultMultiMatrixAccessor.h ${SRC_ROOT}/behavior/ForceField.h ${SRC_ROOT}/behavior/ForceField.inl + ${SRC_ROOT}/behavior/LagrangianConstraint.h + ${SRC_ROOT}/behavior/LagrangianConstraint.inl ${SRC_ROOT}/behavior/LinearSolver.h ${SRC_ROOT}/behavior/LinearSolverAccessor.h - ${SRC_ROOT}/behavior/MatrixAPICompatibility.h ${SRC_ROOT}/behavior/Mass.h ${SRC_ROOT}/behavior/Mass.inl + ${SRC_ROOT}/behavior/MatrixAPICompatibility.h ${SRC_ROOT}/behavior/MechanicalMatrix.h ${SRC_ROOT}/behavior/MechanicalState.h ${SRC_ROOT}/behavior/MechanicalState.inl @@ -106,10 +108,10 @@ set(HEADER_FILES ${SRC_ROOT}/behavior/ProjectiveConstraintSet.h ${SRC_ROOT}/behavior/ProjectiveConstraintSet.inl ${SRC_ROOT}/behavior/RotationFinder.h - ${SRC_ROOT}/behavior/StateAccessor.h ${SRC_ROOT}/behavior/SingleMatrixAccessor.h ${SRC_ROOT}/behavior/SingleStateAccessor.h ${SRC_ROOT}/behavior/SingleStateAccessor.inl + ${SRC_ROOT}/behavior/StateAccessor.h ${SRC_ROOT}/behavior/fwd.h ${SRC_ROOT}/collision/BroadPhaseDetection.h ${SRC_ROOT}/collision/CollisionAlgorithm.h @@ -246,12 +248,12 @@ set(SOURCE_FILES ${SRC_ROOT}/behavior/BaseMechanicalState.cpp ${SRC_ROOT}/behavior/BaseOrderingMethod.cpp ${SRC_ROOT}/behavior/BaseProjectiveConstraintSet.cpp - ${SRC_ROOT}/behavior/Constraint.cpp ${SRC_ROOT}/behavior/ConstraintCorrection.cpp ${SRC_ROOT}/behavior/ConstraintResolution.cpp ${SRC_ROOT}/behavior/ConstraintSolver.cpp ${SRC_ROOT}/behavior/DefaultMultiMatrixAccessor.cpp ${SRC_ROOT}/behavior/ForceField.cpp + ${SRC_ROOT}/behavior/LagrangianConstraint.cpp ${SRC_ROOT}/behavior/LinearSolver.cpp ${SRC_ROOT}/behavior/LinearSolverAccessor.cpp ${SRC_ROOT}/behavior/Mass.cpp diff --git a/Sofa/framework/Core/src/sofa/core/behavior/BaseLagrangianConstraint.h b/Sofa/framework/Core/src/sofa/core/behavior/BaseLagrangianConstraint.h index 49eeb0cea1b..813d472031c 100644 --- a/Sofa/framework/Core/src/sofa/core/behavior/BaseLagrangianConstraint.h +++ b/Sofa/framework/Core/src/sofa/core/behavior/BaseLagrangianConstraint.h @@ -35,7 +35,7 @@ namespace sofa::core::behavior * given its current position and velocity. * * Constraints can be internal to a given body (attached to one MechanicalState, - * see the Constraint class), or link several bodies together (such as contacts, + * see the LagrangianConstraint class), or link several bodies together (such as contacts, * see the InteractionConstraint class). * */ diff --git a/Sofa/framework/Core/src/sofa/core/behavior/BaseMechanicalState.h b/Sofa/framework/Core/src/sofa/core/behavior/BaseMechanicalState.h index 9344498e63f..462bb36afc9 100644 --- a/Sofa/framework/Core/src/sofa/core/behavior/BaseMechanicalState.h +++ b/Sofa/framework/Core/src/sofa/core/behavior/BaseMechanicalState.h @@ -36,7 +36,7 @@ namespace sofa::core::behavior * * This class only contains the data of the body and not any of its * active computations, which are handled by the Mass, ForceField, and - * Constraint components. + * LagrangianConstraint components. * * Two types of vectors are used : * \li \code VecCoord \endcode : containing positions. diff --git a/Sofa/framework/Core/src/sofa/core/behavior/Constraint.h b/Sofa/framework/Core/src/sofa/core/behavior/Constraint.h index 8a4e5b8c722..3641d740e98 100644 --- a/Sofa/framework/Core/src/sofa/core/behavior/Constraint.h +++ b/Sofa/framework/Core/src/sofa/core/behavior/Constraint.h @@ -21,127 +21,12 @@ ******************************************************************************/ #pragma once -#include -#include -#include +#include -#include -#include +SOFA_HEADER_DEPRECATED("v25.12", "v26.12", "sofa/core/behavior/LagrangianConstraint.h") namespace sofa::core::behavior { - -/** - * \brief Component computing constraints within a simulated body. - * - * This class define the abstract API common to constraints using a given type - * of DOFs. - * A Constraint computes constraints applied to one simulated body given its - * current position and velocity. - * - */ template -class Constraint : public BaseLagrangianConstraint, public SingleStateAccessor -{ -public: - SOFA_CLASS2(SOFA_TEMPLATE(Constraint, DataTypes), BaseLagrangianConstraint, SOFA_TEMPLATE(SingleStateAccessor, DataTypes)); - - typedef typename DataTypes::Real Real; - typedef typename DataTypes::VecCoord VecCoord; - typedef typename DataTypes::VecDeriv VecDeriv; - typedef typename DataTypes::Coord Coord; - typedef typename DataTypes::Deriv Deriv; - typedef typename DataTypes::MatrixDeriv MatrixDeriv; - - typedef core::objectmodel::Data DataVecCoord; - typedef core::objectmodel::Data DataVecDeriv; - typedef core::objectmodel::Data DataMatrixDeriv; -protected: - Constraint(MechanicalState *mm = nullptr); - - ~Constraint() override; - - virtual void init() override; -public: - Data endTime; ///< The constraint stops acting after the given value. Use a negative value for infinite constraints - virtual bool isActive() const; ///< if false, the constraint does nothing - - using BaseConstraintSet::getConstraintViolation; - - /// Construct the Constraint violations vector of each constraint - /// - /// \param v is the result vector that contains the whole constraints violations - /// \param cParams defines the state vectors to use for positions and velocities. Also defines the order of the constraint (POS, VEL, ACC) - void getConstraintViolation(const ConstraintParams* cParams, linearalgebra::BaseVector *v) override; - - /// Construct the Constraint violations vector of each constraint - /// - /// \param resV is the result vector that contains the whole constraints violations - /// \param x is the position vector used to compute constraint position violation - /// \param v is the velocity vector used to compute constraint velocity violation - /// \param cParams defines the state vectors to use for positions and velocities. Also defines the order of the constraint (POS, VEL, ACC) - /// - /// This is the method that should be implemented by the component - virtual void getConstraintViolation(const ConstraintParams* cParams, linearalgebra::BaseVector *resV, const DataVecCoord &x, const DataVecDeriv &v) = 0; - - - /// Construct the Jacobian Matrix - /// - /// \param cId is the result constraint sparse matrix Id - /// \param cIndex is the index of the next constraint equation: when building the constraint matrix, you have to use this index, and then update it - /// \param cParams defines the state vectors to use for positions and velocities. Also defines the order of the constraint (POS, VEL, ACC) - void buildConstraintMatrix(const ConstraintParams* cParams, MultiMatrixDerivId cId, unsigned int &cIndex) override; - - /// Construct the Jacobian Matrix - /// - /// \param c is the result constraint sparse matrix - /// \param cIndex is the index of the next constraint equation: when building the constraint matrix, you have to use this index, and then update it - /// \param x is the position vector used for constraint equation computation - /// \param cParams defines the state vectors to use for positions and velocities. Also defines the order of the constraint (POS, VEL, ACC) - /// - /// This is the method that should be implemented by the component - virtual void buildConstraintMatrix(const ConstraintParams* cParams, DataMatrixDeriv & c, unsigned int &cIndex, const DataVecCoord &x) = 0; - - - void storeLambda(const ConstraintParams* cParams, MultiVecDerivId res, const sofa::linearalgebra::BaseVector* lambda) override; - - /// 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) - { - if (dynamic_cast*>(context->getMechanicalState()) == nullptr) { - arg->logError("No mechanical state with the datatype '" + std::string(DataTypes::Name()) + "' found in the context node."); - return false; - } - - return BaseObject::canCreate(obj, context, arg); - } - - virtual type::vector getBaseConstraintIdentifiers() override final - { - type::vector ids = getConstraintIdentifiers(); - ids.push_back("Constraint"); - return ids; - } - -protected: - - virtual type::vector getConstraintIdentifiers(){ return {}; } - - -private: - void storeLambda(const ConstraintParams* cParams, Data& resId, const Data& jacobian, const sofa::linearalgebra::BaseVector* lambda); -}; - -#if !defined(SOFA_CORE_BEHAVIOR_CONSTRAINT_CPP) -extern template class SOFA_CORE_API Constraint; -extern template class SOFA_CORE_API Constraint; -extern template class SOFA_CORE_API Constraint; -extern template class SOFA_CORE_API Constraint; -extern template class SOFA_CORE_API Constraint; -extern template class SOFA_CORE_API Constraint; - - -#endif -} // namespace sofa::core::behavior +using Constraint SOFA_ATTRIBUTE_DEPRECATED("v25.12", "v26.12", "Constraint has been renamed to LagrangianConstraint") = LagrangianConstraint; +} diff --git a/Sofa/framework/Core/src/sofa/core/behavior/Constraint.inl b/Sofa/framework/Core/src/sofa/core/behavior/Constraint.inl index e80274dd83a..8cd5117155b 100644 --- a/Sofa/framework/Core/src/sofa/core/behavior/Constraint.inl +++ b/Sofa/framework/Core/src/sofa/core/behavior/Constraint.inl @@ -21,75 +21,6 @@ ******************************************************************************/ #pragma once -#include -#include +#include -namespace sofa::core::behavior -{ - -template -Constraint::Constraint(MechanicalState *mm) - : Inherit1(), Inherit2(mm) - , endTime( initData(&endTime,(Real)-1,"endTime","The constraint stops acting after the given value.\nUse a negative value for infinite constraints") ) -{ -} - -template -Constraint::~Constraint() -{ -} - - -template -bool Constraint::isActive() const -{ - if( endTime.getValue()<0 ) return true; - return endTime.getValue()>getContext()->getTime(); -} - -template -void Constraint::init() -{ - Inherit1::init(); - Inherit2::init(); -} - -template -void Constraint::getConstraintViolation(const ConstraintParams* cParams, linearalgebra::BaseVector *v) -{ - if (cParams) - { - getConstraintViolation(cParams, v, *cParams->readX(this->mstate.get()), *cParams->readV(this->mstate.get())); - } -} - - -template -void Constraint::buildConstraintMatrix(const ConstraintParams* cParams, MultiMatrixDerivId cId, unsigned int &cIndex) -{ - if (cParams) - { - buildConstraintMatrix(cParams, *cId[this->mstate.get()].write(), cIndex, *cParams->readX(this->mstate.get())); - } -} - - -template -void Constraint::storeLambda(const ConstraintParams* cParams, MultiVecDerivId res, const sofa::linearalgebra::BaseVector* lambda) -{ - if (cParams) - { - storeLambda(cParams, *res[this->mstate.get()].write(), *cParams->readJ(this->mstate.get()), lambda); - } -} - -template -void Constraint::storeLambda(const ConstraintParams*, Data& result, const Data& jacobian, const sofa::linearalgebra::BaseVector* lambda) -{ - auto res = sofa::helper::getWriteAccessor(result); - const MatrixDeriv& j = jacobian.getValue(); - j.multTransposeBaseVector(res, lambda ); // lambda is a vector of scalar value so block size is one. -} - - -} // namespace sofa::core::behavior +SOFA_HEADER_DEPRECATED("v25.12", "v26.12", "sofa/core/behavior/LagrangianConstraint.inl") diff --git a/Sofa/framework/Core/src/sofa/core/behavior/Constraint.cpp b/Sofa/framework/Core/src/sofa/core/behavior/LagrangianConstraint.cpp similarity index 83% rename from Sofa/framework/Core/src/sofa/core/behavior/Constraint.cpp rename to Sofa/framework/Core/src/sofa/core/behavior/LagrangianConstraint.cpp index af0cef4e3f0..0341a1ce428 100644 --- a/Sofa/framework/Core/src/sofa/core/behavior/Constraint.cpp +++ b/Sofa/framework/Core/src/sofa/core/behavior/LagrangianConstraint.cpp @@ -26,12 +26,12 @@ namespace sofa::core::behavior { using namespace sofa::defaulttype; -template class SOFA_CORE_API Constraint; -template class SOFA_CORE_API Constraint; -template class SOFA_CORE_API Constraint; -template class SOFA_CORE_API Constraint; -template class SOFA_CORE_API Constraint; -template class SOFA_CORE_API Constraint; +template class SOFA_CORE_API LagrangianConstraint; +template class SOFA_CORE_API LagrangianConstraint; +template class SOFA_CORE_API LagrangianConstraint; +template class SOFA_CORE_API LagrangianConstraint; +template class SOFA_CORE_API LagrangianConstraint; +template class SOFA_CORE_API LagrangianConstraint; } // namespace sofa::core::behavior diff --git a/Sofa/framework/Core/src/sofa/core/behavior/LagrangianConstraint.h b/Sofa/framework/Core/src/sofa/core/behavior/LagrangianConstraint.h new file mode 100644 index 00000000000..52aba30d714 --- /dev/null +++ b/Sofa/framework/Core/src/sofa/core/behavior/LagrangianConstraint.h @@ -0,0 +1,147 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once + +#include +#include +#include + +#include +#include + +namespace sofa::core::behavior +{ + +/** + * \brief Component computing constraints within a simulated body. + * + * This class defines the abstract API common to constraints using a given type + * of DOFs. + * A LagrangianConstraint computes constraints applied to one simulated body given its + * current position and velocity. + * + */ +template +class LagrangianConstraint : public BaseLagrangianConstraint, public SingleStateAccessor +{ +public: + SOFA_CLASS2(SOFA_TEMPLATE(LagrangianConstraint, DataTypes), BaseLagrangianConstraint, SOFA_TEMPLATE(SingleStateAccessor, DataTypes)); + + typedef typename DataTypes::Real Real; + typedef typename DataTypes::VecCoord VecCoord; + typedef typename DataTypes::VecDeriv VecDeriv; + typedef typename DataTypes::Coord Coord; + typedef typename DataTypes::Deriv Deriv; + typedef typename DataTypes::MatrixDeriv MatrixDeriv; + + typedef core::objectmodel::Data DataVecCoord; + typedef core::objectmodel::Data DataVecDeriv; + typedef core::objectmodel::Data DataMatrixDeriv; +protected: + LagrangianConstraint(MechanicalState *mm = nullptr); + + ~LagrangianConstraint() override; + + virtual void init() override; +public: + Data endTime; ///< The constraint stops acting after the given value. Use a negative value for infinite constraints + virtual bool isActive() const; ///< if false, the constraint does nothing + + using BaseConstraintSet::getConstraintViolation; + + /// Construct the Constraint violations vector of each constraint + /// + /// \param v is the result vector that contains the whole constraints violations + /// \param cParams defines the state vectors to use for positions and velocities. Also defines the order of the constraint (POS, VEL, ACC) + void getConstraintViolation(const ConstraintParams* cParams, linearalgebra::BaseVector *v) override; + + /// Construct the Constraint violations vector of each constraint + /// + /// \param resV is the result vector that contains the whole constraints violations + /// \param x is the position vector used to compute constraint position violation + /// \param v is the velocity vector used to compute constraint velocity violation + /// \param cParams defines the state vectors to use for positions and velocities. Also defines the order of the constraint (POS, VEL, ACC) + /// + /// This is the method that should be implemented by the component + virtual void getConstraintViolation(const ConstraintParams* cParams, linearalgebra::BaseVector *resV, const DataVecCoord &x, const DataVecDeriv &v) = 0; + + + /// Construct the Jacobian Matrix + /// + /// \param cId is the result constraint sparse matrix Id + /// \param cIndex is the index of the next constraint equation: when building the constraint matrix, you have to use this index, and then update it + /// \param cParams defines the state vectors to use for positions and velocities. Also defines the order of the constraint (POS, VEL, ACC) + void buildConstraintMatrix(const ConstraintParams* cParams, MultiMatrixDerivId cId, unsigned int &cIndex) override; + + /// Construct the Jacobian Matrix + /// + /// \param c is the result constraint sparse matrix + /// \param cIndex is the index of the next constraint equation: when building the constraint matrix, you have to use this index, and then update it + /// \param x is the position vector used for constraint equation computation + /// \param cParams defines the state vectors to use for positions and velocities. Also defines the order of the constraint (POS, VEL, ACC) + /// + /// This is the method that should be implemented by the component + virtual void buildConstraintMatrix(const ConstraintParams* cParams, DataMatrixDeriv & c, unsigned int &cIndex, const DataVecCoord &x) = 0; + + + void storeLambda(const ConstraintParams* cParams, MultiVecDerivId res, const sofa::linearalgebra::BaseVector* lambda) override; + + /// 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) + { + if (dynamic_cast*>(context->getMechanicalState()) == nullptr) { + arg->logError("No mechanical state with the datatype '" + std::string(DataTypes::Name()) + "' found in the context node."); + return false; + } + + return BaseObject::canCreate(obj, context, arg); + } + + virtual type::vector getBaseConstraintIdentifiers() override final + { + type::vector ids = getConstraintIdentifiers(); + ids.push_back("LagrangianConstraint"); + return ids; + } + +protected: + + virtual type::vector getConstraintIdentifiers(){ return {}; } + + +private: + void storeLambda(const ConstraintParams* cParams, Data& resId, const Data& jacobian, const sofa::linearalgebra::BaseVector* lambda); +}; + +#if !defined(SOFA_CORE_BEHAVIOR_CONSTRAINT_CPP) +extern template class SOFA_CORE_API LagrangianConstraint; +extern template class SOFA_CORE_API LagrangianConstraint; +extern template class SOFA_CORE_API LagrangianConstraint; +extern template class SOFA_CORE_API LagrangianConstraint; +extern template class SOFA_CORE_API LagrangianConstraint; +extern template class SOFA_CORE_API LagrangianConstraint; + + +#endif +} // namespace sofa::core::behavior diff --git a/Sofa/framework/Core/src/sofa/core/behavior/LagrangianConstraint.inl b/Sofa/framework/Core/src/sofa/core/behavior/LagrangianConstraint.inl new file mode 100644 index 00000000000..f4cfd00a8e6 --- /dev/null +++ b/Sofa/framework/Core/src/sofa/core/behavior/LagrangianConstraint.inl @@ -0,0 +1,95 @@ +/****************************************************************************** +* 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 + +namespace sofa::core::behavior +{ + +template +LagrangianConstraint::LagrangianConstraint(MechanicalState *mm) + : Inherit1(), Inherit2(mm) + , endTime( initData(&endTime,(Real)-1,"endTime","The constraint stops acting after the given value.\nUse a negative value for infinite constraints") ) +{ +} + +template +LagrangianConstraint::~LagrangianConstraint() +{ +} + + +template +bool LagrangianConstraint::isActive() const +{ + if( endTime.getValue()<0 ) return true; + return endTime.getValue()>getContext()->getTime(); +} + +template +void LagrangianConstraint::init() +{ + Inherit1::init(); + Inherit2::init(); +} + +template +void LagrangianConstraint::getConstraintViolation(const ConstraintParams* cParams, linearalgebra::BaseVector *v) +{ + if (cParams) + { + getConstraintViolation(cParams, v, *cParams->readX(this->mstate.get()), *cParams->readV(this->mstate.get())); + } +} + + +template +void LagrangianConstraint::buildConstraintMatrix(const ConstraintParams* cParams, MultiMatrixDerivId cId, unsigned int &cIndex) +{ + if (cParams) + { + buildConstraintMatrix(cParams, *cId[this->mstate.get()].write(), cIndex, *cParams->readX(this->mstate.get())); + } +} + + +template +void LagrangianConstraint::storeLambda(const ConstraintParams* cParams, MultiVecDerivId res, const sofa::linearalgebra::BaseVector* lambda) +{ + if (cParams) + { + storeLambda(cParams, *res[this->mstate.get()].write(), *cParams->readJ(this->mstate.get()), lambda); + } +} + +template +void LagrangianConstraint::storeLambda(const ConstraintParams*, Data& result, const Data& jacobian, const sofa::linearalgebra::BaseVector* lambda) +{ + auto res = sofa::helper::getWriteAccessor(result); + const MatrixDeriv& j = jacobian.getValue(); + j.multTransposeBaseVector(res, lambda ); // lambda is a vector of scalar value so block size is one. +} + + +} // namespace sofa::core::behavior