diff --git a/Sofa/Component/Collision/Response/Contact/src/sofa/component/collision/response/contact/FrictionContact.h b/Sofa/Component/Collision/Response/Contact/src/sofa/component/collision/response/contact/FrictionContact.h index abfe6878d00..3e71d5e0a69 100644 --- a/Sofa/Component/Collision/Response/Contact/src/sofa/component/collision/response/contact/FrictionContact.h +++ b/Sofa/Component/Collision/Response/Contact/src/sofa/component/collision/response/contact/FrictionContact.h @@ -26,7 +26,7 @@ #include #include #include -#include +#include #include #include @@ -60,7 +60,7 @@ class FrictionContact : public core::collision::Contact, public ContactIdentifie mapper::ContactMapper mapper1; mapper::ContactMapper mapper2; - constraint::lagrangian::model::UnilateralInteractionConstraint::SPtr m_constraint; + constraint::lagrangian::model::UnilateralInteractionLagrangianConstraint::SPtr m_constraint; core::objectmodel::BaseContext* parent; Data mu; ///< friction coefficient (0 for frictionless contacts) diff --git a/Sofa/Component/Collision/Response/Contact/src/sofa/component/collision/response/contact/FrictionContact.inl b/Sofa/Component/Collision/Response/Contact/src/sofa/component/collision/response/contact/FrictionContact.inl index d753e9ed690..6da62dd345b 100644 --- a/Sofa/Component/Collision/Response/Contact/src/sofa/component/collision/response/contact/FrictionContact.inl +++ b/Sofa/Component/Collision/Response/Contact/src/sofa/component/collision/response/contact/FrictionContact.inl @@ -144,7 +144,7 @@ void FrictionContact::activ { mmodel2 = mapper2.createMapping(getName().c_str()); } - m_constraint = sofa::core::objectmodel::New >(mmodel1, mmodel2); + m_constraint = sofa::core::objectmodel::New >(mmodel1, mmodel2); m_constraint->setName( getName() ); setInteractionTags(mmodel1, mmodel2); m_constraint->setCustomTolerance( tol.getValue() ); diff --git a/Sofa/Component/Collision/Response/Contact/src/sofa/component/collision/response/contact/StickContactConstraint.h b/Sofa/Component/Collision/Response/Contact/src/sofa/component/collision/response/contact/StickContactConstraint.h index f612ad49d70..381a414bdcc 100644 --- a/Sofa/Component/Collision/Response/Contact/src/sofa/component/collision/response/contact/StickContactConstraint.h +++ b/Sofa/Component/Collision/Response/Contact/src/sofa/component/collision/response/contact/StickContactConstraint.h @@ -24,7 +24,7 @@ #include #include -#include +#include #include #include #include @@ -58,7 +58,7 @@ class StickContactConstraint : public core::collision::Contact, public ContactId mapper::ContactMapper mapper1; mapper::ContactMapper mapper2; - constraint::lagrangian::model::BilateralInteractionConstraint::SPtr m_constraint; + constraint::lagrangian::model::BilateralInteractionLagrangianConstraint::SPtr m_constraint; core::objectmodel::BaseContext* parent; std::vector< sofa::core::collision::DetectionOutput* > contacts; diff --git a/Sofa/Component/Collision/Response/Contact/src/sofa/component/collision/response/contact/StickContactConstraint.inl b/Sofa/Component/Collision/Response/Contact/src/sofa/component/collision/response/contact/StickContactConstraint.inl index f3af1c76aca..307053696ff 100644 --- a/Sofa/Component/Collision/Response/Contact/src/sofa/component/collision/response/contact/StickContactConstraint.inl +++ b/Sofa/Component/Collision/Response/Contact/src/sofa/component/collision/response/contact/StickContactConstraint.inl @@ -128,7 +128,7 @@ void StickContactConstraint::activateMappers( msg_info() << "Creating StickContactConstraint bilateral constraints"; MechanicalState1* mstate1 = mapper1.createMapping(mapper::GenerateStringID::generate().c_str()); MechanicalState2* mstate2 = mapper2.createMapping(mapper::GenerateStringID::generate().c_str()); - m_constraint = sofa::core::objectmodel::New >(mstate1, mstate2); + m_constraint = sofa::core::objectmodel::New >(mstate1, mstate2); m_constraint->setName( getName() ); } diff --git a/Sofa/Component/Constraint/Lagrangian/Model/CMakeLists.txt b/Sofa/Component/Constraint/Lagrangian/Model/CMakeLists.txt index 147e6931b3e..168cf437499 100644 --- a/Sofa/Component/Constraint/Lagrangian/Model/CMakeLists.txt +++ b/Sofa/Component/Constraint/Lagrangian/Model/CMakeLists.txt @@ -15,17 +15,30 @@ set(HEADER_FILES ${SOFACOMPONENTCONSTRAINTLAGRANGIANMODEL_SOURCE_DIR}/StopperConstraint.inl ${SOFACOMPONENTCONSTRAINTLAGRANGIANMODEL_SOURCE_DIR}/UniformConstraint.h ${SOFACOMPONENTCONSTRAINTLAGRANGIANMODEL_SOURCE_DIR}/UniformConstraint.inl + ${SOFACOMPONENTCONSTRAINTLAGRANGIANMODEL_SOURCE_DIR}/UnilateralConstraintResolution.h ${SOFACOMPONENTCONSTRAINTLAGRANGIANMODEL_SOURCE_DIR}/UnilateralInteractionConstraint.h ${SOFACOMPONENTCONSTRAINTLAGRANGIANMODEL_SOURCE_DIR}/UnilateralInteractionConstraint.inl + + ${SOFACOMPONENTCONSTRAINTLAGRANGIANMODEL_SOURCE_DIR}/BilateralInteractionLagrangianConstraint.h + ${SOFACOMPONENTCONSTRAINTLAGRANGIANMODEL_SOURCE_DIR}/BilateralInteractionLagrangianConstraint.inl + ${SOFACOMPONENTCONSTRAINTLAGRANGIANMODEL_SOURCE_DIR}/SlidingLagrangianConstraint.h + ${SOFACOMPONENTCONSTRAINTLAGRANGIANMODEL_SOURCE_DIR}/SlidingLagrangianConstraint.inl + ${SOFACOMPONENTCONSTRAINTLAGRANGIANMODEL_SOURCE_DIR}/StopperLagrangianConstraint.h + ${SOFACOMPONENTCONSTRAINTLAGRANGIANMODEL_SOURCE_DIR}/StopperLagrangianConstraint.inl + ${SOFACOMPONENTCONSTRAINTLAGRANGIANMODEL_SOURCE_DIR}/UniformLagrangianConstraint.h + ${SOFACOMPONENTCONSTRAINTLAGRANGIANMODEL_SOURCE_DIR}/UniformLagrangianConstraint.inl + ${SOFACOMPONENTCONSTRAINTLAGRANGIANMODEL_SOURCE_DIR}/UnilateralInteractionLagrangianConstraint.h + ${SOFACOMPONENTCONSTRAINTLAGRANGIANMODEL_SOURCE_DIR}/UnilateralInteractionLagrangianConstraint.inl ) set(SOURCE_FILES ${SOFACOMPONENTCONSTRAINTLAGRANGIANMODEL_SOURCE_DIR}/init.cpp - ${SOFACOMPONENTCONSTRAINTLAGRANGIANMODEL_SOURCE_DIR}/BilateralInteractionConstraint.cpp - ${SOFACOMPONENTCONSTRAINTLAGRANGIANMODEL_SOURCE_DIR}/SlidingConstraint.cpp - ${SOFACOMPONENTCONSTRAINTLAGRANGIANMODEL_SOURCE_DIR}/StopperConstraint.cpp - ${SOFACOMPONENTCONSTRAINTLAGRANGIANMODEL_SOURCE_DIR}/UniformConstraint.cpp - ${SOFACOMPONENTCONSTRAINTLAGRANGIANMODEL_SOURCE_DIR}/UnilateralInteractionConstraint.cpp + ${SOFACOMPONENTCONSTRAINTLAGRANGIANMODEL_SOURCE_DIR}/BilateralInteractionLagrangianConstraint.cpp + ${SOFACOMPONENTCONSTRAINTLAGRANGIANMODEL_SOURCE_DIR}/SlidingLagrangianConstraint.cpp + ${SOFACOMPONENTCONSTRAINTLAGRANGIANMODEL_SOURCE_DIR}/StopperLagrangianConstraint.cpp + ${SOFACOMPONENTCONSTRAINTLAGRANGIANMODEL_SOURCE_DIR}/UniformLagrangianConstraint.cpp + ${SOFACOMPONENTCONSTRAINTLAGRANGIANMODEL_SOURCE_DIR}/UnilateralInteractionLagrangianConstraint.cpp + ) sofa_find_package(Sofa.Simulation.Core REQUIRED) diff --git a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/BilateralInteractionConstraint.h b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/BilateralInteractionConstraint.h index af6f155bde2..ef03dd20454 100644 --- a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/BilateralInteractionConstraint.h +++ b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/BilateralInteractionConstraint.h @@ -20,176 +20,13 @@ * Contact information: contact@sofa-framework.org * ******************************************************************************/ #pragma once -#include -#include -#include -#include +#include -#include -#include -#include -#include - -#include - -#include +SOFA_DEPRECATED_HEADER("v24.06", "v25.06", "sofa/component/constraint/lagrangian/model/BilateralInteractionLagrangianConstraint.h") namespace sofa::component::constraint::lagrangian::model { - -/// These 'using' are in a per-file namespace so they will not leak -/// and polluate the standard namespace. -using sofa::core::behavior::BaseConstraint ; -using sofa::core::behavior::ConstraintResolution ; -using sofa::core::behavior::PairInteractionConstraint ; -using sofa::core::objectmodel::Data ; -using sofa::core::ConstraintParams ; -using sofa::core::ConstVecCoordId; - -using sofa::linearalgebra::BaseVector ; -using sofa::type::Vec3d; -using sofa::type::Quat ; - -using sofa::defaulttype::Rigid3Types ; -using sofa::defaulttype::Vec3Types ; - - template -class BilateralInteractionConstraintSpecialization {}; - - -template -class BilateralInteractionConstraint : public PairInteractionConstraint -{ -public: - SOFA_CLASS(SOFA_TEMPLATE(BilateralInteractionConstraint,DataTypes), - SOFA_TEMPLATE(PairInteractionConstraint,DataTypes)); - - /// That any templates variation of BilateralInteractionConstraintSpecialization are friend. - template - friend class BilateralInteractionConstraintSpecialization ; - - typedef PairInteractionConstraint Inherit; - - typedef typename DataTypes::VecCoord VecCoord; - typedef typename DataTypes::VecDeriv VecDeriv; - typedef typename DataTypes::MatrixDeriv MatrixDeriv; - typedef typename DataTypes::Coord Coord; - typedef typename DataTypes::Deriv Deriv; - typedef typename Coord::value_type Real; - typedef typename DataTypes::MatrixDeriv::RowIterator MatrixDerivRowIterator; - - typedef core::behavior::MechanicalState MechanicalState; - typedef BaseConstraint::PersistentID PersistentID; - - typedef Data DataVecCoord; - typedef Data DataVecDeriv; - typedef Data DataMatrixDeriv; - - using SubsetIndices = type::vector; - using DataSubsetIndices = sofa::core::topology::TopologySubsetIndices; - -protected: - std::vector dfree; - Quat q; - - std::vector cid; - - DataSubsetIndices m1; ///< index of the constraint on the first model - DataSubsetIndices m2; ///< index of the constraint on the second model - Data restVector; ///< Relative position to maintain between attached points (optional) - VecCoord initialDifference; - - Data d_numericalTolerance; ///< a real value specifying the tolerance during the constraint solving. (default=0.0001 - Data d_activate; ///< bool to control constraint activation - Data keepOrientDiff; ///< keep the initial difference in orientation (only for rigids) - - - SingleLink, sofa::core::topology::BaseMeshTopology, BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_topology1; ///< Link to be set to the first topology container in order to support topological changes - SingleLink, sofa::core::topology::BaseMeshTopology, BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_topology2; ///< Link to be set to the second topology container in order to support topological changes - - std::vector prevForces; - - SOFA_ATTRIBUTE_DISABLED__BILATERALINTERACTIONCONSTRAINTDATA("Data 'activateAtIteration' has been removed, please use the Data d_activate instead and an engine or a script to change the behavior at the right step (see PR #3327).") - sofa::core::objectmodel::lifecycle::RemovedData activateAtIteration{this, "v22.12", "v23.06", "activateAtIteration", "use the boolean data 'activate' instead and an engine or a script to change the behavior at the right step (see PR #3327)."}; - - SOFA_ATTRIBUTE_DISABLED__BILATERALINTERACTIONCONSTRAINTDATA("Data 'merge' has been removed. Its behavior was unused, undocumented, untested, and unclear (see PR #3328).") - sofa::core::objectmodel::lifecycle::RemovedData merge{this, "v22.12", "v23.06", "merge", "Its behavior was unused, undocumented, untested, and unclear (see PR #3328), please report to sofa-dev if you want the feature back."}; - - SOFA_ATTRIBUTE_DISABLED__BILATERALINTERACTIONCONSTRAINTDATA("Data 'derivative' has been removed. Its behavior was unused, undocumented, untested, and unclear (see PR #3328).") - sofa::core::objectmodel::lifecycle::RemovedData derivative{this, "v22.12", "v23.06", "derivative", "Its behavior was unused, undocumented, untested, and unclear (see PR #3328), please report to sofa-dev if you want the feature back."}; - - - BilateralInteractionConstraint(MechanicalState* object1, MechanicalState* object2) ; - BilateralInteractionConstraint(MechanicalState* object) ; - BilateralInteractionConstraint(); - - virtual ~BilateralInteractionConstraint(){} -public: - void init() override; - - void bwdInit() override {} - - void reinit() override; - - void buildConstraintMatrix(const ConstraintParams* cParams, - DataMatrixDeriv &c1, DataMatrixDeriv &c2, - unsigned int &cIndex, - const DataVecCoord &x1, const DataVecCoord &x2) override; - - void getConstraintViolation(const ConstraintParams* cParams, - BaseVector *v, - const DataVecCoord &x1, const DataVecCoord &x2, - const DataVecDeriv &v1, const DataVecDeriv &v2) override; - - void getVelocityViolation(BaseVector *v, - const DataVecCoord &x1, const DataVecCoord &x2, - const DataVecDeriv &v1, const DataVecDeriv &v2); - - void getConstraintResolution(const ConstraintParams* cParams, - std::vector& resTab, - unsigned int& offset) override; - - void handleEvent(sofa::core::objectmodel::Event *event) override; - - void draw(const core::visual::VisualParams* vparams) override; - - void clear(int reserve = 0) ; - - virtual void addContact(Deriv norm, Coord P, Coord Q, Real contactDistance, - int m1, int m2, Coord Pfree, Coord Qfree, - long id=0, PersistentID localid=0); - - void addContact(Deriv norm, Coord P, Coord Q, Real contactDistance, - int m1, int m2, long id=0, PersistentID localid=0) ; - - void addContact(Deriv norm, Real contactDistance, int m1, int m2, - long id=0, PersistentID localid=0) ; - - /// Method to remove a contact using point @param indices and id of buffer: @sa m1 (resp. @sa 2m) if @param objectId is equal to 0 (resp. to 1) - void removeContact(int objectId, SubsetIndices indices); - - virtual type::vector getBilateralInteractionIdentifiers() {return {};} - - virtual type::vector getPairInteractionIdentifiers() override final - { - type::vector ids = getBilateralInteractionIdentifiers(); - ids.push_back("Bilateral"); - return ids; - } - -private: - void unspecializedInit() ; - - /// Method to get the index position of a @param point Id inside @sa m1 or @sa m2) depending of the value passed in @param cIndices. Return InvalidID if not found. - Index indexOfElemConstraint(const SubsetIndices& cIndices, Index Id); -}; - - -#if !defined(SOFA_COMPONENT_CONSTRAINTSET_BILATERALINTERACTIONCONSTRAINT_CPP) -extern template class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API BilateralInteractionConstraint< Vec3Types >; -extern template class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API BilateralInteractionConstraint< Rigid3Types >; -#endif - -} // namespace sofa::component::constraint::lagrangian::model +using BilateralInteractionConstraint SOFA_ATTRIBUTE_DEPRECATED("v24.06 ", "v25.06", "BilateralInteractionConstraint has been renamed to BilateralInteractionLagrangianConstraint") = BilateralInteractionLagrangianConstraint; +} diff --git a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/BilateralInteractionConstraint.inl b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/BilateralInteractionConstraint.inl index 0aeec985ed2..67fcf165710 100644 --- a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/BilateralInteractionConstraint.inl +++ b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/BilateralInteractionConstraint.inl @@ -20,414 +20,7 @@ * Contact information: contact@sofa-framework.org * ******************************************************************************/ #pragma once -#include -#include -#include -#include -#include -#include -#include -#include // for std::min +#include -namespace sofa::component::constraint::lagrangian::model -{ - -using sofa::core::objectmodel::KeypressedEvent ; -using sofa::core::objectmodel::Event ; -using sofa::helper::WriteAccessor ; -using sofa::type::Vec; - -template -BilateralInteractionConstraint::BilateralInteractionConstraint(MechanicalState* object1, MechanicalState* object2) - : Inherit(object1, object2) - , m1(initData(&m1, "first_point","index of the constraint on the first model")) - , m2(initData(&m2, "second_point","index of the constraint on the second model")) - , restVector(initData(&restVector, "rest_vector","Relative position to maintain between attached points (optional)")) - , d_numericalTolerance(initData(&d_numericalTolerance, 0.0001, "numericalTolerance", - "a real value specifying the tolerance during the constraint solving. (optional, default=0.0001)") ) - , d_activate( initData(&d_activate, true, "activate", "control constraint activation (true by default)")) - , keepOrientDiff(initData(&keepOrientDiff,false, "keepOrientationDifference", "keep the initial difference in orientation (only for rigids)")) - , l_topology1(initLink("topology1", "link to the first topology container")) - , l_topology2(initLink("topology2", "link to the second topology container")) -{ - this->f_listening.setValue(true); -} - -template -BilateralInteractionConstraint::BilateralInteractionConstraint(MechanicalState* object) - : BilateralInteractionConstraint(object, object) -{ -} - -template -BilateralInteractionConstraint::BilateralInteractionConstraint() - : BilateralInteractionConstraint(nullptr, nullptr) -{ -} - -template -void BilateralInteractionConstraint::unspecializedInit() -{ - /// Do general check of validity for inputs - Inherit1::init(); - - /// Using assert means that the previous lines have check that there is two valid mechanical state. - assert(this->mstate1); - assert(this->mstate2); - - prevForces.clear(); -} - -template -void BilateralInteractionConstraint::init() -{ - unspecializedInit(); - - if (sofa::core::topology::BaseMeshTopology* _topology1 = l_topology1.get()) - { - m1.createTopologyHandler(_topology1); - m1.addTopologyEventCallBack(core::topology::TopologyChangeType::POINTSREMOVED, - [this](const core::topology::TopologyChange* change) - { - const auto* pointsRemoved = static_cast(change); - removeContact(0, pointsRemoved->getArray()); - }); - } - - if (sofa::core::topology::BaseMeshTopology* _topology2 = l_topology2.get()) - { - m2.createTopologyHandler(_topology2); - m2.addTopologyEventCallBack(core::topology::TopologyChangeType::POINTSREMOVED, - [this](const core::topology::TopologyChange* change) - { - const auto* pointsRemoved = static_cast(change); - removeContact(1, pointsRemoved->getArray()); - }); - } -} - -template -void BilateralInteractionConstraint::reinit() -{ - prevForces.clear(); -} - - -template -void BilateralInteractionConstraint::buildConstraintMatrix(const ConstraintParams*, DataMatrixDeriv &c1_d, DataMatrixDeriv &c2_d, unsigned int &constraintId - , const DataVecCoord &/*x1*/, const DataVecCoord &/*x2*/) -{ - if (!d_activate.getValue()) - return; - - const unsigned minp = std::min(m1.getValue().size(), m2.getValue().size()); - if (minp == 0) - return; - - const SubsetIndices& m1Indices = m1.getValue(); - const SubsetIndices& m2Indices = m2.getValue(); - - MatrixDeriv &c1 = *c1_d.beginEdit(); - MatrixDeriv &c2 = *c2_d.beginEdit(); - - cid.resize(minp); - - for (unsigned pid=0; pid cx(1,0,0), cy(0,1,0), cz(0,0,1); - - cid[pid] = constraintId; - constraintId += 3; - - MatrixDerivRowIterator c1_it = c1.writeLine(cid[pid]); - c1_it.addCol(tm1, -cx); - - MatrixDerivRowIterator c2_it = c2.writeLine(cid[pid]); - c2_it.addCol(tm2, cx); - - c1_it = c1.writeLine(cid[pid] + 1); - c1_it.setCol(tm1, -cy); - - c2_it = c2.writeLine(cid[pid] + 1); - c2_it.setCol(tm2, cy); - - c1_it = c1.writeLine(cid[pid] + 2); - c1_it.setCol(tm1, -cz); - - c2_it = c2.writeLine(cid[pid] + 2); - c2_it.setCol(tm2, cz); - } - - c1_d.endEdit(); - c2_d.endEdit(); -} - - -template -void BilateralInteractionConstraint::getConstraintViolation(const ConstraintParams* cParams, - BaseVector *v, - const DataVecCoord &d_x1, const DataVecCoord &d_x2 - , const DataVecDeriv & d_v1, const DataVecDeriv & d_v2) -{ - if (!d_activate.getValue()) return; - - const SubsetIndices& m1Indices = m1.getValue(); - const SubsetIndices& m2Indices = m2.getValue(); - - unsigned minp = std::min(m1Indices.size(), m2Indices.size()); - - const VecDeriv& restVector = this->restVector.getValue(); - - if (cParams->constOrder() == sofa::core::ConstraintOrder::VEL) - { - getVelocityViolation(v, d_x1, d_x2, d_v1, d_v2); - return; - } - - const VecCoord &x1 = d_x1.getValue(); - const VecCoord &x2 = d_x2.getValue(); - - dfree.resize(minp); - - for (unsigned pid=0; pidset(cid[pid] , dfree[pid][0]); - v->set(cid[pid]+1, dfree[pid][1]); - v->set(cid[pid]+2, dfree[pid][2]); - } -} - - -template -void BilateralInteractionConstraint::getVelocityViolation(BaseVector *v, - const DataVecCoord &d_x1, - const DataVecCoord &d_x2, - const DataVecDeriv &d_v1, - const DataVecDeriv &d_v2) -{ - const SubsetIndices& m1Indices = m1.getValue(); - const SubsetIndices& m2Indices = m2.getValue(); - - SOFA_UNUSED(d_x1); - SOFA_UNUSED(d_x2); - - const VecCoord &v1 = d_v1.getValue(); - const VecCoord &v2 = d_v2.getValue(); - - const unsigned minp = std::min(m1Indices.size(), m2Indices.size()); - const VecDeriv& restVector = this->restVector.getValue(); - - auto pos1 = this->getMState1()->readPositions(); - auto pos2 = this->getMState2()->readPositions(); - - const SReal dt = this->getContext()->getDt(); - const SReal invDt = SReal(1.0) / dt; - - for (unsigned pid=0; pidset(cid[pid] , dVfree[0] + dPos[0] ); - v->set(cid[pid]+1, dVfree[1] + dPos[1] ); - v->set(cid[pid]+2, dVfree[2] + dPos[2] ); - } -} - - -template -void BilateralInteractionConstraint::getConstraintResolution(const ConstraintParams* cParams, - std::vector& resTab, - unsigned int& offset) -{ - SOFA_UNUSED(cParams); - const unsigned minp=std::min(m1.getValue().size(),m2.getValue().size()); - - prevForces.resize(minp); - for (unsigned pid=0; pid -void BilateralInteractionConstraint::addContact(Deriv /*norm*/, Coord P, Coord Q, - Real /*contactDistance*/, int m1, int m2, - Coord /*Pfree*/, Coord /*Qfree*/, - long /*id*/, PersistentID /*localid*/) -{ - WriteAccessor > wm1 = this->m1; - WriteAccessor > wm2 = this->m2; - WriteAccessor > wrest = this->restVector; - wm1.push_back(m1); - wm2.push_back(m2); - wrest.push_back(Q-P); -} - - -template -void BilateralInteractionConstraint::addContact(Deriv norm, Coord P, Coord Q, Real - contactDistance, int m1, int m2, - long id, PersistentID localid) -{ - addContact(norm, P, Q, contactDistance, m1, m2, - this->getMState2()->read(ConstVecCoordId::freePosition())->getValue()[m2], - this->getMState1()->read(ConstVecCoordId::freePosition())->getValue()[m1], - id, localid); -} - -template -void BilateralInteractionConstraint::addContact(Deriv norm, Real contactDistance, - int m1, int m2, long id, PersistentID localid) -{ - addContact(norm, - this->getMState2()->read(ConstVecCoordId::position())->getValue()[m2], - this->getMState1()->read(ConstVecCoordId::position())->getValue()[m1], - contactDistance, m1, m2, - this->getMState2()->read(ConstVecCoordId::freePosition())->getValue()[m2], - this->getMState1()->read(ConstVecCoordId::freePosition())->getValue()[m1], - id, localid); -} - - -template -void BilateralInteractionConstraint::removeContact(int objectId, SubsetIndices indices) -{ - WriteAccessor > m1Indices = this->m1; - WriteAccessor > m2Indices = this->m2; - WriteAccessor > wrest = this->restVector; - - const SubsetIndices& cIndices1 = m1.getValue(); - const SubsetIndices& cIndices2 = m2.getValue(); - - for (sofa::Size i = 0; i < indices.size(); ++i) - { - const Index elemId = indices[i]; - Index posId = sofa::InvalidID; - - if (objectId == 0) - posId = indexOfElemConstraint(cIndices1, elemId); - else if (objectId == 1) - posId = indexOfElemConstraint(cIndices2, elemId); - - if (posId != sofa::InvalidID) - { - if (wrest.size() == m1Indices.size()) - wrest.erase(wrest.begin() + posId); - - m1Indices.erase(m1Indices.begin() + posId); - m2Indices.erase(m2Indices.begin() + posId); - } - } - -} - - -template -void BilateralInteractionConstraint::clear(int reserve) -{ - WriteAccessor > wm1 = this->m1; - WriteAccessor > wm2 = this->m2; - WriteAccessor > wrest = this->restVector; - wm1.clear(); - wm2.clear(); - wrest.clear(); - if (reserve) - { - wm1.reserve(reserve); - wm2.reserve(reserve); - wrest.reserve(reserve); - } -} - - -template -Index BilateralInteractionConstraint::indexOfElemConstraint(const SubsetIndices& cIndices, Index Id) -{ - const auto it = std::find(cIndices.begin(), cIndices.end(), Id); - - if (it != cIndices.end()) - return Index(std::distance(cIndices.begin(), it)); - else - return sofa::InvalidID; -} - - -template -void BilateralInteractionConstraint::draw(const core::visual::VisualParams* vparams) -{ - if (!vparams->displayFlags().getShowInteractionForceFields()) return; - - const auto stateLifeCycle = vparams->drawTool()->makeStateLifeCycle(); - vparams->drawTool()->disableLighting(); - - constexpr sofa::type::RGBAColor colorActive = sofa::type::RGBAColor::magenta(); - constexpr sofa::type::RGBAColor colorNotActive = sofa::type::RGBAColor::green(); - std::vector< sofa::type::Vec3 > vertices; - - const unsigned minp = std::min(m1.getValue().size(),m2.getValue().size()); - auto positionsM1 = sofa::helper::getReadAccessor(*this->mstate1->read(ConstVecCoordId::position())); - auto positionsM2 = sofa::helper::getReadAccessor(*this->mstate2->read(ConstVecCoordId::position())); - const auto indicesM1 = sofa::helper::getReadAccessor(m1); - const auto indicesM2 = sofa::helper::getReadAccessor(m2); - - for (unsigned i=0; idrawTool()->drawPoints(vertices, 10, (d_activate.getValue()) ? colorActive : colorNotActive); - - -} - -//TODO(dmarchal): implementing keyboard interaction behavior directly in a component is not a valid -//design for a component. Interaction should be defered to an independent Component implemented in the SofaInteraction -//a second possibility is to implement this behavir using script. -template -void BilateralInteractionConstraint::handleEvent(Event *event) -{ - if (KeypressedEvent::checkEventType(event)) - { - const KeypressedEvent *ev = static_cast(event); - switch(ev->getKey()) - { - - case 'A': - case 'a': - if (d_activate.getValue()) - { - msg_info() << "Unactivating constraint"; - d_activate.setValue(false); - } - else - { - msg_info() << "Activating constraint"; - d_activate.setValue(true); - } - - break; - } - } - -} - - -} //namespace sofa::component::constraint::lagrangian::model +SOFA_DEPRECATED_HEADER("v24.06", "v25.06", "sofa/component/constraint/lagrangian/model/BilateralInteractionLagrangianConstraint.inl") diff --git a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/BilateralInteractionConstraint.cpp b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/BilateralInteractionLagrangianConstraint.cpp similarity index 59% rename from Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/BilateralInteractionConstraint.cpp rename to Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/BilateralInteractionLagrangianConstraint.cpp index 64743d5addb..94f1bfad358 100644 --- a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/BilateralInteractionConstraint.cpp +++ b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/BilateralInteractionLagrangianConstraint.cpp @@ -19,9 +19,9 @@ * * * Contact information: contact@sofa-framework.org * ******************************************************************************/ -#define SOFA_COMPONENT_CONSTRAINTSET_BILATERALINTERACTIONCONSTRAINT_CPP +#define SOFA_COMPONENT_CONSTRAINTSET_BILATERALINTERACTIONLAGRANGIANCONSTRAINT_CPP -#include +#include #include #include @@ -33,37 +33,37 @@ class RigidImpl {}; template<> -class BilateralInteractionConstraintSpecialization +class BilateralInteractionLagrangianConstraintSpecialization { public: template - static void bwdInit(BilateralInteractionConstraint& self) { + static void bwdInit(BilateralInteractionLagrangianConstraint& self) { if (!self.keepOrientDiff.getValue()) return; - helper::WriteAccessor::VecDeriv > > wrest = self.restVector; + helper::WriteAccessor::VecDeriv > > wrest = self.restVector; if (wrest.size() > 0) { - msg_warning("BilateralInteractionConstraintSpecialization") << "keepOrientationDifference is activated, rest_vector will be ignored! " ; + msg_warning("BilateralInteractionLagrangianConstraintSpecialization") << "keepOrientationDifference is activated, rest_vector will be ignored! " ; wrest.resize(0); } - const typename BilateralInteractionConstraint::SubsetIndices& m1Indices = self.m1.getValue(); - const typename BilateralInteractionConstraint::SubsetIndices& m2Indices = self.m2.getValue(); + const typename BilateralInteractionLagrangianConstraint::SubsetIndices& m1Indices = self.m1.getValue(); + const typename BilateralInteractionLagrangianConstraint::SubsetIndices& m2Indices = self.m2.getValue(); const unsigned minp = std::min(m1Indices.size(),m2Indices.size()); - const typename BilateralInteractionConstraint::DataVecCoord &d_x1 = *self.mstate1->read(core::ConstVecCoordId::position()); - const typename BilateralInteractionConstraint::DataVecCoord &d_x2 = *self.mstate2->read(core::ConstVecCoordId::position()); + const typename BilateralInteractionLagrangianConstraint::DataVecCoord &d_x1 = *self.mstate1->read(core::ConstVecCoordId::position()); + const typename BilateralInteractionLagrangianConstraint::DataVecCoord &d_x2 = *self.mstate2->read(core::ConstVecCoordId::position()); - const typename BilateralInteractionConstraint::VecCoord &x1 = d_x1.getValue(); - const typename BilateralInteractionConstraint::VecCoord &x2 = d_x2.getValue(); + const typename BilateralInteractionLagrangianConstraint::VecCoord &x1 = d_x1.getValue(); + const typename BilateralInteractionLagrangianConstraint::VecCoord &x2 = d_x2.getValue(); for (unsigned pid=0; pid::Coord P = x1[m1Indices[pid]]; - const typename BilateralInteractionConstraint::Coord Q = x2[m2Indices[pid]]; + const typename BilateralInteractionLagrangianConstraint::Coord P = x1[m1Indices[pid]]; + const typename BilateralInteractionLagrangianConstraint::Coord Q = x2[m2Indices[pid]]; type::Quat qP, qQ, dQP; qP = P.getOrientation(); @@ -73,7 +73,7 @@ class BilateralInteractionConstraintSpecialization dQP = qP.quatDiff(qQ, qP); dQP.normalize(); - typename BilateralInteractionConstraint::Coord df; + typename BilateralInteractionLagrangianConstraint::Coord df; df.getCenter() = Q.getCenter() - P.getCenter(); df.getOrientation() = dQP; self.initialDifference.push_back(df); @@ -83,7 +83,7 @@ class BilateralInteractionConstraintSpecialization template - static void getConstraintResolution(BilateralInteractionConstraint& self, + static void getConstraintResolution(BilateralInteractionLagrangianConstraint& self, const ConstraintParams* cParams, std::vector& resTab, unsigned int& offset, double tolerance) @@ -104,26 +104,26 @@ class BilateralInteractionConstraintSpecialization template - static void buildConstraintMatrix(BilateralInteractionConstraint& self, + static void buildConstraintMatrix(BilateralInteractionLagrangianConstraint& self, const ConstraintParams* cParams, - typename BilateralInteractionConstraint::DataMatrixDeriv &c1_d, - typename BilateralInteractionConstraint::DataMatrixDeriv &c2_d, + typename BilateralInteractionLagrangianConstraint::DataMatrixDeriv &c1_d, + typename BilateralInteractionLagrangianConstraint::DataMatrixDeriv &c2_d, unsigned int &constraintId, - const typename BilateralInteractionConstraint::DataVecCoord &/*x1*/, - const typename BilateralInteractionConstraint::DataVecCoord &/*x2*/) + const typename BilateralInteractionLagrangianConstraint::DataVecCoord &/*x1*/, + const typename BilateralInteractionLagrangianConstraint::DataVecCoord &/*x2*/) { SOFA_UNUSED(cParams) ; - const typename BilateralInteractionConstraint::SubsetIndices& m1Indices = self.m1.getValue(); - const typename BilateralInteractionConstraint::SubsetIndices& m2Indices = self.m2.getValue(); + const typename BilateralInteractionLagrangianConstraint::SubsetIndices& m1Indices = self.m1.getValue(); + const typename BilateralInteractionLagrangianConstraint::SubsetIndices& m2Indices = self.m2.getValue(); unsigned minp = std::min(m1Indices.size(),m2Indices.size()); self.cid.resize(minp); - typename BilateralInteractionConstraint::MatrixDeriv &c1 = *c1_d.beginEdit(); - typename BilateralInteractionConstraint::MatrixDeriv &c2 = *c2_d.beginEdit(); + typename BilateralInteractionLagrangianConstraint::MatrixDeriv &c1 = *c1_d.beginEdit(); + typename BilateralInteractionLagrangianConstraint::MatrixDeriv &c2 = *c2_d.beginEdit(); - const Vec<3, typename BilateralInteractionConstraint::Real> cx(1,0,0), cy(0,1,0), cz(0,0,1); - const Vec<3, typename BilateralInteractionConstraint::Real> vZero(0,0,0); + const Vec<3, typename BilateralInteractionLagrangianConstraint::Real> cx(1,0,0), cy(0,1,0), cz(0,0,1); + const Vec<3, typename BilateralInteractionLagrangianConstraint::Real> vZero(0,0,0); for (unsigned pid=0; pid constraintId += 6; //Apply constraint for position - typename BilateralInteractionConstraint::MatrixDerivRowIterator c1_it = c1.writeLine(self.cid[pid]); - c1_it.addCol(tm1, typename BilateralInteractionConstraint::Deriv(-cx, vZero)); + typename BilateralInteractionLagrangianConstraint::MatrixDerivRowIterator c1_it = c1.writeLine(self.cid[pid]); + c1_it.addCol(tm1, typename BilateralInteractionLagrangianConstraint::Deriv(-cx, vZero)); - typename BilateralInteractionConstraint::MatrixDerivRowIterator c2_it = c2.writeLine(self.cid[pid]); - c2_it.addCol(tm2, typename BilateralInteractionConstraint::Deriv(cx, vZero)); + typename BilateralInteractionLagrangianConstraint::MatrixDerivRowIterator c2_it = c2.writeLine(self.cid[pid]); + c2_it.addCol(tm2, typename BilateralInteractionLagrangianConstraint::Deriv(cx, vZero)); c1_it = c1.writeLine(self.cid[pid] + 1); - c1_it.setCol(tm1, typename BilateralInteractionConstraint::Deriv(-cy, vZero)); + c1_it.setCol(tm1, typename BilateralInteractionLagrangianConstraint::Deriv(-cy, vZero)); c2_it = c2.writeLine(self.cid[pid] + 1); - c2_it.setCol(tm2, typename BilateralInteractionConstraint::Deriv(cy, vZero)); + c2_it.setCol(tm2, typename BilateralInteractionLagrangianConstraint::Deriv(cy, vZero)); c1_it = c1.writeLine(self.cid[pid] + 2); - c1_it.setCol(tm1, typename BilateralInteractionConstraint::Deriv(-cz, vZero)); + c1_it.setCol(tm1, typename BilateralInteractionLagrangianConstraint::Deriv(-cz, vZero)); c2_it = c2.writeLine(self.cid[pid] + 2); - c2_it.setCol(tm2, typename BilateralInteractionConstraint::Deriv(cz, vZero)); + c2_it.setCol(tm2, typename BilateralInteractionLagrangianConstraint::Deriv(cz, vZero)); //Apply constraint for orientation c1_it = c1.writeLine(self.cid[pid] + 3); - c1_it.setCol(tm1, typename BilateralInteractionConstraint::Deriv(vZero, -cx)); + c1_it.setCol(tm1, typename BilateralInteractionLagrangianConstraint::Deriv(vZero, -cx)); c2_it = c2.writeLine(self.cid[pid] + 3); - c2_it.setCol(tm2, typename BilateralInteractionConstraint::Deriv(vZero, cx)); + c2_it.setCol(tm2, typename BilateralInteractionLagrangianConstraint::Deriv(vZero, cx)); c1_it = c1.writeLine(self.cid[pid] + 4); - c1_it.setCol(tm1, typename BilateralInteractionConstraint::Deriv(vZero, -cy)); + c1_it.setCol(tm1, typename BilateralInteractionLagrangianConstraint::Deriv(vZero, -cy)); c2_it = c2.writeLine(self.cid[pid] + 4); - c2_it.setCol(tm2, typename BilateralInteractionConstraint::Deriv(vZero, cy)); + c2_it.setCol(tm2, typename BilateralInteractionLagrangianConstraint::Deriv(vZero, cy)); c1_it = c1.writeLine(self.cid[pid] + 5); - c1_it.setCol(tm1, typename BilateralInteractionConstraint::Deriv(vZero, -cz)); + c1_it.setCol(tm1, typename BilateralInteractionLagrangianConstraint::Deriv(vZero, -cz)); c2_it = c2.writeLine(self.cid[pid] + 5); - c2_it.setCol(tm2, typename BilateralInteractionConstraint::Deriv(vZero, cz)); + c2_it.setCol(tm2, typename BilateralInteractionLagrangianConstraint::Deriv(vZero, cz)); } c1_d.endEdit(); @@ -178,34 +178,34 @@ class BilateralInteractionConstraintSpecialization template - static void getConstraintViolation(BilateralInteractionConstraint& self, + static void getConstraintViolation(BilateralInteractionLagrangianConstraint& self, const ConstraintParams* /*cParams*/, BaseVector *v, - const typename BilateralInteractionConstraint::DataVecCoord &d_x1, - const typename BilateralInteractionConstraint::DataVecCoord &d_x2, - const typename BilateralInteractionConstraint::DataVecDeriv &/*v1*/, - const typename BilateralInteractionConstraint::DataVecDeriv &/*v2*/) + const typename BilateralInteractionLagrangianConstraint::DataVecCoord &d_x1, + const typename BilateralInteractionLagrangianConstraint::DataVecCoord &d_x2, + const typename BilateralInteractionLagrangianConstraint::DataVecDeriv &/*v1*/, + const typename BilateralInteractionLagrangianConstraint::DataVecDeriv &/*v2*/) { - const typename BilateralInteractionConstraint::SubsetIndices& m1Indices = self.m1.getValue(); - const typename BilateralInteractionConstraint::SubsetIndices& m2Indices = self.m2.getValue(); + const typename BilateralInteractionLagrangianConstraint::SubsetIndices& m1Indices = self.m1.getValue(); + const typename BilateralInteractionLagrangianConstraint::SubsetIndices& m2Indices = self.m2.getValue(); unsigned min = std::min(m1Indices.size(), m2Indices.size()); - const typename BilateralInteractionConstraint::VecDeriv& restVector = self.restVector.getValue(); + const typename BilateralInteractionLagrangianConstraint::VecDeriv& restVector = self.restVector.getValue(); self.dfree.resize(min); - const typename BilateralInteractionConstraint::VecCoord &x1 = d_x1.getValue(); - const typename BilateralInteractionConstraint::VecCoord &x2 = d_x2.getValue(); + const typename BilateralInteractionLagrangianConstraint::VecCoord &x1 = d_x1.getValue(); + const typename BilateralInteractionLagrangianConstraint::VecCoord &x2 = d_x2.getValue(); for (unsigned pid=0; pid::Coord dof1 = x1[m1Indices[pid]]; - //typename BilateralInteractionConstraint::Coord dof2 = x2[m2Indices[pid]]; - typename BilateralInteractionConstraint::Coord dof1; + //typename BilateralInteractionLagrangianConstraint::Coord dof1 = x1[m1Indices[pid]]; + //typename BilateralInteractionLagrangianConstraint::Coord dof2 = x2[m2Indices[pid]]; + typename BilateralInteractionLagrangianConstraint::Coord dof1; if (self.keepOrientDiff.getValue()) { - const typename BilateralInteractionConstraint::Coord dof1c = x1[m1Indices[pid]]; + const typename BilateralInteractionLagrangianConstraint::Coord dof1c = x1[m1Indices[pid]]; - typename BilateralInteractionConstraint::Coord corr=self.initialDifference[pid]; + typename BilateralInteractionLagrangianConstraint::Coord corr=self.initialDifference[pid]; type::Quat df = corr.getOrientation(); type::Quat o1 = dof1c.getOrientation(); type::Quat ro1 = o1 * df; @@ -215,7 +215,7 @@ class BilateralInteractionConstraintSpecialization } else dof1 = x1[m1Indices[pid]]; - const typename BilateralInteractionConstraint::Coord dof2 = x2[m2Indices[pid]]; + const typename BilateralInteractionLagrangianConstraint::Coord dof2 = x2[m2Indices[pid]]; getVCenter(self.dfree[pid]) = dof2.getCenter() - dof1.getCenter(); getVOrientation(self.dfree[pid]) = dof1.rotate(self.q.angularDisplacement(dof2.getOrientation() , @@ -229,14 +229,14 @@ class BilateralInteractionConstraintSpecialization } - template > - static void addContact(BilateralInteractionConstraint& self, typename MyClass::Deriv /*norm*/, + template > + static void addContact(BilateralInteractionLagrangianConstraint& self, typename MyClass::Deriv /*norm*/, typename MyClass::Coord P, typename MyClass::Coord Q, typename MyClass::Real /*contactDistance*/, int m1, int m2, typename MyClass::Coord /*Pfree*/, typename MyClass::Coord /*Qfree*/, long /*id*/, typename MyClass::PersistentID /*localid*/) { - helper::WriteAccessor::SubsetIndices > > wm1 = self.m1; - helper::WriteAccessor::SubsetIndices > > wm2 = self.m2; + helper::WriteAccessor::SubsetIndices > > wm1 = self.m1; + helper::WriteAccessor::SubsetIndices > > wm2 = self.m2; helper::WriteAccessor > wrest = self.restVector; wm1.push_back(m1); wm2.push_back(m2); @@ -251,52 +251,52 @@ class BilateralInteractionConstraintSpecialization template<> SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API -void BilateralInteractionConstraint::init(){ +void BilateralInteractionLagrangianConstraint::init(){ unspecializedInit() ; } template<> SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API -void BilateralInteractionConstraint::bwdInit() { - BilateralInteractionConstraintSpecialization::bwdInit(*this); +void BilateralInteractionLagrangianConstraint::bwdInit() { + BilateralInteractionLagrangianConstraintSpecialization::bwdInit(*this); } template<> SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API -void BilateralInteractionConstraint::getConstraintResolution(const ConstraintParams* cParams, +void BilateralInteractionLagrangianConstraint::getConstraintResolution(const ConstraintParams* cParams, std::vector& resTab, unsigned int& offset) { - BilateralInteractionConstraintSpecialization::getConstraintResolution(*this, + BilateralInteractionLagrangianConstraintSpecialization::getConstraintResolution(*this, cParams, resTab, offset, d_numericalTolerance.getValue()) ; } template <> SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API -void BilateralInteractionConstraint::buildConstraintMatrix(const ConstraintParams* cParams, +void BilateralInteractionLagrangianConstraint::buildConstraintMatrix(const ConstraintParams* cParams, DataMatrixDeriv &c1_d, DataMatrixDeriv &c2_d, unsigned int &constraintId, const DataVecCoord &x1, const DataVecCoord &x2) { - BilateralInteractionConstraintSpecialization::buildConstraintMatrix(*this, + BilateralInteractionLagrangianConstraintSpecialization::buildConstraintMatrix(*this, cParams, c1_d, c2_d, constraintId, x1, x2) ; } template <> SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API -void BilateralInteractionConstraint::getConstraintViolation(const ConstraintParams* cParams, +void BilateralInteractionLagrangianConstraint::getConstraintViolation(const ConstraintParams* cParams, BaseVector *v, const DataVecCoord &d_x1, const DataVecCoord &d_x2, const DataVecDeriv &v1, const DataVecDeriv &v2) { - BilateralInteractionConstraintSpecialization::getConstraintViolation(*this, + BilateralInteractionLagrangianConstraintSpecialization::getConstraintViolation(*this, cParams, v, d_x1, d_x2, v1, v2) ; } template <> SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API -void BilateralInteractionConstraint::getVelocityViolation(BaseVector * /*v*/, +void BilateralInteractionLagrangianConstraint::getVelocityViolation(BaseVector * /*v*/, const DataVecCoord &/*x1*/, const DataVecCoord &/*x2*/, const DataVecDeriv &/*v1*/, @@ -306,25 +306,25 @@ void BilateralInteractionConstraint::getVelocityViolation(BaseVecto } template<> SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API -void BilateralInteractionConstraint::addContact(Deriv norm, +void BilateralInteractionLagrangianConstraint::addContact(Deriv norm, Coord P, Coord Q, Real contactDistance, int m1, int m2, Coord Pfree, Coord Qfree, long id, PersistentID localid) { - BilateralInteractionConstraintSpecialization::addContact(*this, + BilateralInteractionLagrangianConstraintSpecialization::addContact(*this, norm, P, Q, contactDistance, m1, m2, Pfree, Qfree, id, localid) ; } -int BilateralInteractionConstraintClass = core::RegisterObject("BilateralInteractionConstraint defining an holonomic equality constraint (attachment)") - .add< BilateralInteractionConstraint >() - .add< BilateralInteractionConstraint >() +int BilateralInteractionLagrangianConstraintClass = core::RegisterObject("BilateralInteractionLagrangianConstraint defining an holonomic equality constraint (attachment)") + .add< BilateralInteractionLagrangianConstraint >() + .add< BilateralInteractionLagrangianConstraint >() ; -template class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API BilateralInteractionConstraint; -template class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API BilateralInteractionConstraint; +template class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API BilateralInteractionLagrangianConstraint; +template class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API BilateralInteractionLagrangianConstraint; } //namespace sofa::component::constraint::lagrangian::model diff --git a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/BilateralInteractionLagrangianConstraint.h b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/BilateralInteractionLagrangianConstraint.h new file mode 100644 index 00000000000..a9e87512df0 --- /dev/null +++ b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/BilateralInteractionLagrangianConstraint.h @@ -0,0 +1,195 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once +#include + +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include + +namespace sofa::component::constraint::lagrangian::model +{ + +/// These 'using' are in a per-file namespace so they will not leak +/// and polluate the standard namespace. +using sofa::core::behavior::BaseConstraint ; +using sofa::core::behavior::ConstraintResolution ; +using sofa::core::behavior::PairInteractionConstraint ; +using sofa::core::objectmodel::Data ; +using sofa::core::ConstraintParams ; +using sofa::core::ConstVecCoordId; + +using sofa::linearalgebra::BaseVector ; +using sofa::type::Vec3d; +using sofa::type::Quat ; + +using sofa::defaulttype::Rigid3Types ; +using sofa::defaulttype::Vec3Types ; + + +template +class BilateralInteractionLagrangianConstraintSpecialization {}; + + +template +class BilateralInteractionLagrangianConstraint : public PairInteractionConstraint +{ +public: + SOFA_CLASS(SOFA_TEMPLATE(BilateralInteractionLagrangianConstraint,DataTypes), + SOFA_TEMPLATE(PairInteractionConstraint,DataTypes)); + + /// That any templates variation of BilateralInteractionLagrangianConstraintSpecialization are friend. + template + friend class BilateralInteractionLagrangianConstraintSpecialization ; + + typedef PairInteractionConstraint Inherit; + + typedef typename DataTypes::VecCoord VecCoord; + typedef typename DataTypes::VecDeriv VecDeriv; + typedef typename DataTypes::MatrixDeriv MatrixDeriv; + typedef typename DataTypes::Coord Coord; + typedef typename DataTypes::Deriv Deriv; + typedef typename Coord::value_type Real; + typedef typename DataTypes::MatrixDeriv::RowIterator MatrixDerivRowIterator; + + typedef core::behavior::MechanicalState MechanicalState; + typedef BaseConstraint::PersistentID PersistentID; + + typedef Data DataVecCoord; + typedef Data DataVecDeriv; + typedef Data DataMatrixDeriv; + + using SubsetIndices = type::vector; + using DataSubsetIndices = sofa::core::topology::TopologySubsetIndices; + +protected: + std::vector dfree; + Quat q; + + std::vector cid; + + DataSubsetIndices m1; ///< index of the constraint on the first model + DataSubsetIndices m2; ///< index of the constraint on the second model + Data restVector; ///< Relative position to maintain between attached points (optional) + VecCoord initialDifference; + + Data d_numericalTolerance; ///< a real value specifying the tolerance during the constraint solving. (default=0.0001 + Data d_activate; ///< bool to control constraint activation + Data keepOrientDiff; ///< keep the initial difference in orientation (only for rigids) + + + SingleLink, sofa::core::topology::BaseMeshTopology, BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_topology1; ///< Link to be set to the first topology container in order to support topological changes + SingleLink, sofa::core::topology::BaseMeshTopology, BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_topology2; ///< Link to be set to the second topology container in order to support topological changes + + std::vector prevForces; + + SOFA_ATTRIBUTE_DISABLED__BILATERALINTERACTIONCONSTRAINTDATA("Data 'activateAtIteration' has been removed, please use the Data d_activate instead and an engine or a script to change the behavior at the right step (see PR #3327).") + sofa::core::objectmodel::lifecycle::RemovedData activateAtIteration{this, "v22.12", "v23.06", "activateAtIteration", "use the boolean data 'activate' instead and an engine or a script to change the behavior at the right step (see PR #3327)."}; + + SOFA_ATTRIBUTE_DISABLED__BILATERALINTERACTIONCONSTRAINTDATA("Data 'merge' has been removed. Its behavior was unused, undocumented, untested, and unclear (see PR #3328).") + sofa::core::objectmodel::lifecycle::RemovedData merge{this, "v22.12", "v23.06", "merge", "Its behavior was unused, undocumented, untested, and unclear (see PR #3328), please report to sofa-dev if you want the feature back."}; + + SOFA_ATTRIBUTE_DISABLED__BILATERALINTERACTIONCONSTRAINTDATA("Data 'derivative' has been removed. Its behavior was unused, undocumented, untested, and unclear (see PR #3328).") + sofa::core::objectmodel::lifecycle::RemovedData derivative{this, "v22.12", "v23.06", "derivative", "Its behavior was unused, undocumented, untested, and unclear (see PR #3328), please report to sofa-dev if you want the feature back."}; + + + BilateralInteractionLagrangianConstraint(MechanicalState* object1, MechanicalState* object2) ; + BilateralInteractionLagrangianConstraint(MechanicalState* object) ; + BilateralInteractionLagrangianConstraint(); + + virtual ~BilateralInteractionLagrangianConstraint(){} +public: + void init() override; + + void bwdInit() override {} + + void reinit() override; + + void buildConstraintMatrix(const ConstraintParams* cParams, + DataMatrixDeriv &c1, DataMatrixDeriv &c2, + unsigned int &cIndex, + const DataVecCoord &x1, const DataVecCoord &x2) override; + + void getConstraintViolation(const ConstraintParams* cParams, + BaseVector *v, + const DataVecCoord &x1, const DataVecCoord &x2, + const DataVecDeriv &v1, const DataVecDeriv &v2) override; + + void getVelocityViolation(BaseVector *v, + const DataVecCoord &x1, const DataVecCoord &x2, + const DataVecDeriv &v1, const DataVecDeriv &v2); + + void getConstraintResolution(const ConstraintParams* cParams, + std::vector& resTab, + unsigned int& offset) override; + + void handleEvent(sofa::core::objectmodel::Event *event) override; + + void draw(const core::visual::VisualParams* vparams) override; + + void clear(int reserve = 0) ; + + virtual void addContact(Deriv norm, Coord P, Coord Q, Real contactDistance, + int m1, int m2, Coord Pfree, Coord Qfree, + long id=0, PersistentID localid=0); + + void addContact(Deriv norm, Coord P, Coord Q, Real contactDistance, + int m1, int m2, long id=0, PersistentID localid=0) ; + + void addContact(Deriv norm, Real contactDistance, int m1, int m2, + long id=0, PersistentID localid=0) ; + + /// Method to remove a contact using point @param indices and id of buffer: @sa m1 (resp. @sa 2m) if @param objectId is equal to 0 (resp. to 1) + void removeContact(int objectId, SubsetIndices indices); + + virtual type::vector getBilateralInteractionIdentifiers() {return {};} + + virtual type::vector getPairInteractionIdentifiers() override final + { + type::vector ids = getBilateralInteractionIdentifiers(); + ids.push_back("Bilateral"); + return ids; + } + +private: + void unspecializedInit() ; + + /// Method to get the index position of a @param point Id inside @sa m1 or @sa m2) depending of the value passed in @param cIndices. Return InvalidID if not found. + Index indexOfElemConstraint(const SubsetIndices& cIndices, Index Id); +}; + + +#if !defined(SOFA_COMPONENT_CONSTRAINTSET_BILATERALINTERACTIONLAGRANGIANCONSTRAINT_CPP) +extern template class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API BilateralInteractionLagrangianConstraint< Vec3Types >; +extern template class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API BilateralInteractionLagrangianConstraint< Rigid3Types >; +#endif + +} // namespace sofa::component::constraint::lagrangian::model diff --git a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/BilateralInteractionLagrangianConstraint.inl b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/BilateralInteractionLagrangianConstraint.inl new file mode 100644 index 00000000000..3d0f88b221b --- /dev/null +++ b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/BilateralInteractionLagrangianConstraint.inl @@ -0,0 +1,433 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once +#include +#include +#include + +#include +#include +#include +#include +#include // for std::min + +namespace sofa::component::constraint::lagrangian::model +{ + +using sofa::core::objectmodel::KeypressedEvent ; +using sofa::core::objectmodel::Event ; +using sofa::helper::WriteAccessor ; +using sofa::type::Vec; + +template +BilateralInteractionLagrangianConstraint::BilateralInteractionLagrangianConstraint(MechanicalState* object1, MechanicalState* object2) + : Inherit(object1, object2) + , m1(initData(&m1, "first_point","index of the constraint on the first model")) + , m2(initData(&m2, "second_point","index of the constraint on the second model")) + , restVector(initData(&restVector, "rest_vector","Relative position to maintain between attached points (optional)")) + , d_numericalTolerance(initData(&d_numericalTolerance, 0.0001, "numericalTolerance", + "a real value specifying the tolerance during the constraint solving. (optional, default=0.0001)") ) + , d_activate( initData(&d_activate, true, "activate", "control constraint activation (true by default)")) + , keepOrientDiff(initData(&keepOrientDiff,false, "keepOrientationDifference", "keep the initial difference in orientation (only for rigids)")) + , l_topology1(initLink("topology1", "link to the first topology container")) + , l_topology2(initLink("topology2", "link to the second topology container")) +{ + this->f_listening.setValue(true); +} + +template +BilateralInteractionLagrangianConstraint::BilateralInteractionLagrangianConstraint(MechanicalState* object) + : BilateralInteractionLagrangianConstraint(object, object) +{ +} + +template +BilateralInteractionLagrangianConstraint::BilateralInteractionLagrangianConstraint() + : BilateralInteractionLagrangianConstraint(nullptr, nullptr) +{ +} + +template +void BilateralInteractionLagrangianConstraint::unspecializedInit() +{ + /// Do general check of validity for inputs + Inherit1::init(); + + /// Using assert means that the previous lines have check that there is two valid mechanical state. + assert(this->mstate1); + assert(this->mstate2); + + prevForces.clear(); +} + +template +void BilateralInteractionLagrangianConstraint::init() +{ + unspecializedInit(); + + if (sofa::core::topology::BaseMeshTopology* _topology1 = l_topology1.get()) + { + m1.createTopologyHandler(_topology1); + m1.addTopologyEventCallBack(core::topology::TopologyChangeType::POINTSREMOVED, + [this](const core::topology::TopologyChange* change) + { + const auto* pointsRemoved = static_cast(change); + removeContact(0, pointsRemoved->getArray()); + }); + } + + if (sofa::core::topology::BaseMeshTopology* _topology2 = l_topology2.get()) + { + m2.createTopologyHandler(_topology2); + m2.addTopologyEventCallBack(core::topology::TopologyChangeType::POINTSREMOVED, + [this](const core::topology::TopologyChange* change) + { + const auto* pointsRemoved = static_cast(change); + removeContact(1, pointsRemoved->getArray()); + }); + } +} + +template +void BilateralInteractionLagrangianConstraint::reinit() +{ + prevForces.clear(); +} + + +template +void BilateralInteractionLagrangianConstraint::buildConstraintMatrix(const ConstraintParams*, DataMatrixDeriv &c1_d, DataMatrixDeriv &c2_d, unsigned int &constraintId + , const DataVecCoord &/*x1*/, const DataVecCoord &/*x2*/) +{ + if (!d_activate.getValue()) + return; + + const unsigned minp = std::min(m1.getValue().size(), m2.getValue().size()); + if (minp == 0) + return; + + const SubsetIndices& m1Indices = m1.getValue(); + const SubsetIndices& m2Indices = m2.getValue(); + + MatrixDeriv &c1 = *c1_d.beginEdit(); + MatrixDeriv &c2 = *c2_d.beginEdit(); + + cid.resize(minp); + + for (unsigned pid=0; pid cx(1,0,0), cy(0,1,0), cz(0,0,1); + + cid[pid] = constraintId; + constraintId += 3; + + MatrixDerivRowIterator c1_it = c1.writeLine(cid[pid]); + c1_it.addCol(tm1, -cx); + + MatrixDerivRowIterator c2_it = c2.writeLine(cid[pid]); + c2_it.addCol(tm2, cx); + + c1_it = c1.writeLine(cid[pid] + 1); + c1_it.setCol(tm1, -cy); + + c2_it = c2.writeLine(cid[pid] + 1); + c2_it.setCol(tm2, cy); + + c1_it = c1.writeLine(cid[pid] + 2); + c1_it.setCol(tm1, -cz); + + c2_it = c2.writeLine(cid[pid] + 2); + c2_it.setCol(tm2, cz); + } + + c1_d.endEdit(); + c2_d.endEdit(); +} + + +template +void BilateralInteractionLagrangianConstraint::getConstraintViolation(const ConstraintParams* cParams, + BaseVector *v, + const DataVecCoord &d_x1, const DataVecCoord &d_x2 + , const DataVecDeriv & d_v1, const DataVecDeriv & d_v2) +{ + if (!d_activate.getValue()) return; + + const SubsetIndices& m1Indices = m1.getValue(); + const SubsetIndices& m2Indices = m2.getValue(); + + unsigned minp = std::min(m1Indices.size(), m2Indices.size()); + + const VecDeriv& restVector = this->restVector.getValue(); + + if (cParams->constOrder() == sofa::core::ConstraintOrder::VEL) + { + getVelocityViolation(v, d_x1, d_x2, d_v1, d_v2); + return; + } + + const VecCoord &x1 = d_x1.getValue(); + const VecCoord &x2 = d_x2.getValue(); + + dfree.resize(minp); + + for (unsigned pid=0; pidset(cid[pid] , dfree[pid][0]); + v->set(cid[pid]+1, dfree[pid][1]); + v->set(cid[pid]+2, dfree[pid][2]); + } +} + + +template +void BilateralInteractionLagrangianConstraint::getVelocityViolation(BaseVector *v, + const DataVecCoord &d_x1, + const DataVecCoord &d_x2, + const DataVecDeriv &d_v1, + const DataVecDeriv &d_v2) +{ + const SubsetIndices& m1Indices = m1.getValue(); + const SubsetIndices& m2Indices = m2.getValue(); + + SOFA_UNUSED(d_x1); + SOFA_UNUSED(d_x2); + + const VecCoord &v1 = d_v1.getValue(); + const VecCoord &v2 = d_v2.getValue(); + + const unsigned minp = std::min(m1Indices.size(), m2Indices.size()); + const VecDeriv& restVector = this->restVector.getValue(); + + auto pos1 = this->getMState1()->readPositions(); + auto pos2 = this->getMState2()->readPositions(); + + const SReal dt = this->getContext()->getDt(); + const SReal invDt = SReal(1.0) / dt; + + for (unsigned pid=0; pidset(cid[pid] , dVfree[0] + dPos[0] ); + v->set(cid[pid]+1, dVfree[1] + dPos[1] ); + v->set(cid[pid]+2, dVfree[2] + dPos[2] ); + } +} + + +template +void BilateralInteractionLagrangianConstraint::getConstraintResolution(const ConstraintParams* cParams, + std::vector& resTab, + unsigned int& offset) +{ + SOFA_UNUSED(cParams); + const unsigned minp=std::min(m1.getValue().size(),m2.getValue().size()); + + prevForces.resize(minp); + for (unsigned pid=0; pid +void BilateralInteractionLagrangianConstraint::addContact(Deriv /*norm*/, Coord P, Coord Q, + Real /*contactDistance*/, int m1, int m2, + Coord /*Pfree*/, Coord /*Qfree*/, + long /*id*/, PersistentID /*localid*/) +{ + WriteAccessor > wm1 = this->m1; + WriteAccessor > wm2 = this->m2; + WriteAccessor > wrest = this->restVector; + wm1.push_back(m1); + wm2.push_back(m2); + wrest.push_back(Q-P); +} + + +template +void BilateralInteractionLagrangianConstraint::addContact(Deriv norm, Coord P, Coord Q, Real + contactDistance, int m1, int m2, + long id, PersistentID localid) +{ + addContact(norm, P, Q, contactDistance, m1, m2, + this->getMState2()->read(ConstVecCoordId::freePosition())->getValue()[m2], + this->getMState1()->read(ConstVecCoordId::freePosition())->getValue()[m1], + id, localid); +} + +template +void BilateralInteractionLagrangianConstraint::addContact(Deriv norm, Real contactDistance, + int m1, int m2, long id, PersistentID localid) +{ + addContact(norm, + this->getMState2()->read(ConstVecCoordId::position())->getValue()[m2], + this->getMState1()->read(ConstVecCoordId::position())->getValue()[m1], + contactDistance, m1, m2, + this->getMState2()->read(ConstVecCoordId::freePosition())->getValue()[m2], + this->getMState1()->read(ConstVecCoordId::freePosition())->getValue()[m1], + id, localid); +} + + +template +void BilateralInteractionLagrangianConstraint::removeContact(int objectId, SubsetIndices indices) +{ + WriteAccessor > m1Indices = this->m1; + WriteAccessor > m2Indices = this->m2; + WriteAccessor > wrest = this->restVector; + + const SubsetIndices& cIndices1 = m1.getValue(); + const SubsetIndices& cIndices2 = m2.getValue(); + + for (sofa::Size i = 0; i < indices.size(); ++i) + { + const Index elemId = indices[i]; + Index posId = sofa::InvalidID; + + if (objectId == 0) + posId = indexOfElemConstraint(cIndices1, elemId); + else if (objectId == 1) + posId = indexOfElemConstraint(cIndices2, elemId); + + if (posId != sofa::InvalidID) + { + if (wrest.size() == m1Indices.size()) + wrest.erase(wrest.begin() + posId); + + m1Indices.erase(m1Indices.begin() + posId); + m2Indices.erase(m2Indices.begin() + posId); + } + } + +} + + +template +void BilateralInteractionLagrangianConstraint::clear(int reserve) +{ + WriteAccessor > wm1 = this->m1; + WriteAccessor > wm2 = this->m2; + WriteAccessor > wrest = this->restVector; + wm1.clear(); + wm2.clear(); + wrest.clear(); + if (reserve) + { + wm1.reserve(reserve); + wm2.reserve(reserve); + wrest.reserve(reserve); + } +} + + +template +Index BilateralInteractionLagrangianConstraint::indexOfElemConstraint(const SubsetIndices& cIndices, Index Id) +{ + const auto it = std::find(cIndices.begin(), cIndices.end(), Id); + + if (it != cIndices.end()) + return Index(std::distance(cIndices.begin(), it)); + else + return sofa::InvalidID; +} + + +template +void BilateralInteractionLagrangianConstraint::draw(const core::visual::VisualParams* vparams) +{ + if (!vparams->displayFlags().getShowInteractionForceFields()) return; + + const auto stateLifeCycle = vparams->drawTool()->makeStateLifeCycle(); + vparams->drawTool()->disableLighting(); + + constexpr sofa::type::RGBAColor colorActive = sofa::type::RGBAColor::magenta(); + constexpr sofa::type::RGBAColor colorNotActive = sofa::type::RGBAColor::green(); + std::vector< sofa::type::Vec3 > vertices; + + const unsigned minp = std::min(m1.getValue().size(),m2.getValue().size()); + auto positionsM1 = sofa::helper::getReadAccessor(*this->mstate1->read(ConstVecCoordId::position())); + auto positionsM2 = sofa::helper::getReadAccessor(*this->mstate2->read(ConstVecCoordId::position())); + const auto indicesM1 = sofa::helper::getReadAccessor(m1); + const auto indicesM2 = sofa::helper::getReadAccessor(m2); + + for (unsigned i=0; idrawTool()->drawPoints(vertices, 10, (d_activate.getValue()) ? colorActive : colorNotActive); + + +} + +//TODO(dmarchal): implementing keyboard interaction behavior directly in a component is not a valid +//design for a component. Interaction should be defered to an independent Component implemented in the SofaInteraction +//a second possibility is to implement this behavir using script. +template +void BilateralInteractionLagrangianConstraint::handleEvent(Event *event) +{ + if (KeypressedEvent::checkEventType(event)) + { + const KeypressedEvent *ev = static_cast(event); + switch(ev->getKey()) + { + + case 'A': + case 'a': + if (d_activate.getValue()) + { + msg_info() << "Unactivating constraint"; + d_activate.setValue(false); + } + else + { + msg_info() << "Activating constraint"; + d_activate.setValue(true); + } + + break; + } + } + +} + + +} //namespace sofa::component::constraint::lagrangian::model diff --git a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/SlidingConstraint.h b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/SlidingConstraint.h index 19e2a124a90..b78a9991843 100644 --- a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/SlidingConstraint.h +++ b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/SlidingConstraint.h @@ -20,94 +20,13 @@ * Contact information: contact@sofa-framework.org * ******************************************************************************/ #pragma once -#include -#include -#include -#include +#include -namespace sofa::component::constraint::lagrangian::model -{ +SOFA_DEPRECATED_HEADER("v24.06", "v25.06", "sofa/component/constraint/lagrangian/model/SlidingLagrangianConstraint.h") -using sofa::core::ConstraintParams; - -template -class SlidingConstraint : public core::behavior::PairInteractionConstraint +namespace sofa::component::constraint::lagrangian::model { -public: - SOFA_CLASS(SOFA_TEMPLATE(SlidingConstraint,DataTypes), SOFA_TEMPLATE(core::behavior::PairInteractionConstraint,DataTypes)); - - typedef typename DataTypes::VecCoord VecCoord; - typedef typename DataTypes::VecDeriv VecDeriv; - typedef typename DataTypes::MatrixDeriv MatrixDeriv; - typedef typename DataTypes::MatrixDeriv::RowIterator MatrixDerivRowIterator; - typedef typename DataTypes::Coord Coord; - typedef typename DataTypes::Deriv Deriv; - typedef typename Coord::value_type Real; - typedef typename core::behavior::MechanicalState MechanicalState; - typedef typename core::behavior::PairInteractionConstraint Inherit; - - typedef core::objectmodel::Data DataVecCoord; - typedef core::objectmodel::Data DataVecDeriv; - typedef core::objectmodel::Data DataMatrixDeriv; - -protected: - - Data d_m1; ///< index of the spliding point on the first model - Data d_m2a; ///< index of one end of the sliding axis - Data d_m2b; ///< index of the other end of the sliding axis - Data d_force; ///< interaction force - - Real m_dist; // constraint violation - Real m_thirdConstraint; // 0 if A getSlidingIdentifiers() { return {}; } - - virtual type::vector getPairInteractionIdentifiers() override final - { - type::vector ids = getSlidingIdentifiers(); - ids.push_back("Sliding"); - return ids; - } - - -public: - void init() override; - - void buildConstraintMatrix(const core::ConstraintParams* cParams, DataMatrixDeriv &c1, DataMatrixDeriv &c2, unsigned int &cIndex - , const DataVecCoord &x1, const DataVecCoord &x2) override; - - void getConstraintViolation(const core::ConstraintParams* cParams, linearalgebra::BaseVector *v, const DataVecCoord &x1, const DataVecCoord &x2 - , const DataVecDeriv &v1, const DataVecDeriv &v2) override; - - void getConstraintResolution(const core::ConstraintParams*, - std::vector& resTab, - unsigned int& offset) override; - void storeLambda(const ConstraintParams* cParams, sofa::core::MultiVecDerivId res, const sofa::linearalgebra::BaseVector* lambda) override; - - void draw(const core::visual::VisualParams* vparams) override; - -private: - // storage of force - Deriv m_dirAxe, m_dirProj, m_dirOrtho; - - - -}; - -#if !defined(SOFA_COMPONENT_CONSTRAINTSET_SLIDINGCONSTRAINT_CPP) -extern template class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API SlidingConstraint< defaulttype::Vec3Types >; - -#endif - -} //namespace sofa::component::constraint::lagrangian::model +template +using SlidingConstraint SOFA_ATTRIBUTE_DEPRECATED("v24.06 ", "v25.06", "SlidingConstraint has been renamed to SlidingLagrangianConstraint") = SlidingLagrangianConstraint; +} diff --git a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/SlidingConstraint.inl b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/SlidingConstraint.inl index e656c12cc7c..777bbaf511e 100644 --- a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/SlidingConstraint.inl +++ b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/SlidingConstraint.inl @@ -20,207 +20,7 @@ * Contact information: contact@sofa-framework.org * ******************************************************************************/ #pragma once -#include -#include -#include -#include -#include -#include -namespace sofa::component::constraint::lagrangian::model -{ -template -SlidingConstraint::SlidingConstraint() - : SlidingConstraint(nullptr, nullptr) -{ -} +#include -template -SlidingConstraint::SlidingConstraint(MechanicalState* object) - : SlidingConstraint(object, object) -{ -} - -template -SlidingConstraint::SlidingConstraint(MechanicalState* object1, MechanicalState* object2) - : Inherit(object1, object2) - , d_m1(initData(&d_m1, 0, "sliding_point","index of the spliding point on the first model")) - , d_m2a(initData(&d_m2a, 0, "axis_1","index of one end of the sliding axis")) - , d_m2b(initData(&d_m2b, 0, "axis_2","index of the other end of the sliding axis")) - , d_force(initData(&d_force,"force","force (impulse) used to solve the constraint")) -{ -} - -template -void SlidingConstraint::init() -{ - assert(this->mstate1); - assert(this->mstate2); - - m_thirdConstraint = 0; -} - - -template -void SlidingConstraint::buildConstraintMatrix(const core::ConstraintParams*, DataMatrixDeriv &c1_d, DataMatrixDeriv &c2_d, unsigned int &cIndex - , const DataVecCoord &x1, const DataVecCoord &x2) -{ - int tm1 = d_m1.getValue(); - int tm2a = d_m2a.getValue(); - int tm2b = d_m2b.getValue(); - - MatrixDeriv &c1 = *c1_d.beginEdit(); - MatrixDeriv &c2 = *c2_d.beginEdit(); - - const Coord P = x1.getValue()[tm1]; - const Coord A = x2.getValue()[tm2a]; - const Coord B = x2.getValue()[tm2b]; - - // the axis - m_dirAxe = B - A; - const Real ab = m_dirAxe.norm(); - m_dirAxe.normalize(); - - // projection of the point on the axis - Real r = (P-A) * m_dirAxe; - Real r2 = r / ab; - const Deriv proj = A + m_dirAxe * r; - - // We move the constraint point onto the projection - m_dirProj = P - proj; - m_dist = m_dirProj.norm(); // constraint violation - m_dirProj.normalize(); // direction of the constraint - - m_dirOrtho = cross(m_dirProj, m_dirAxe); - m_dirOrtho.normalize(); - - m_cid = cIndex; - cIndex += 2; - - MatrixDerivRowIterator c1_it = c1.writeLine(m_cid); - c1_it.addCol(tm1, m_dirProj); - - MatrixDerivRowIterator c2_it = c2.writeLine(m_cid); - c2_it.addCol(tm2a, -m_dirProj * (1-r2)); - c2_it.addCol(tm2b, -m_dirProj * r2); - - c1_it = c1.writeLine(m_cid + 1); - c1_it.setCol(tm1, m_dirOrtho); - - c2_it = c2.writeLine(m_cid + 1); - c2_it.addCol(tm2a, -m_dirOrtho * (1-r2)); - c2_it.addCol(tm2b, -m_dirOrtho * r2); - - m_thirdConstraint = 0; - - if (r < 0) - { - m_thirdConstraint = r; - cIndex++; - - c1_it = c1.writeLine(m_cid + 2); - c1_it.setCol(tm1, m_dirAxe); - - c2_it = c2.writeLine(m_cid + 2); - c2_it.addCol(tm2a, -m_dirAxe); - } - else if (r > ab) - { - m_thirdConstraint = r - ab; - cIndex++; - - c1_it = c1.writeLine(m_cid + 2); - c1_it.setCol(tm1, -m_dirAxe); - - c2_it = c2.writeLine(m_cid + 2); - c2_it.addCol(tm2b, m_dirAxe); - } - - c1_d.endEdit(); - c2_d.endEdit(); -} - - -template -void SlidingConstraint::getConstraintViolation(const core::ConstraintParams *, linearalgebra::BaseVector *v, const DataVecCoord &, const DataVecCoord & - , const DataVecDeriv &, const DataVecDeriv &) -{ - v->set(m_cid, m_dist); - v->set(m_cid+1, 0.0); - - if(m_thirdConstraint) - { - if(m_thirdConstraint>0) - v->set(m_cid+2, -m_thirdConstraint); - else - v->set(m_cid+2, m_thirdConstraint); - } -} - - -template -void SlidingConstraint::getConstraintResolution(const ConstraintParams*, - std::vector& resTab, - unsigned int& offset) -{ - resTab[offset++] = new BilateralConstraintResolution(); - resTab[offset++] = new BilateralConstraintResolution(); - - if(m_thirdConstraint) - resTab[offset++] = new UnilateralConstraintResolution(); -} - - -template -void SlidingConstraint::storeLambda(const ConstraintParams* /*cParams*/, sofa::core::MultiVecDerivId /*res*/, const sofa::linearalgebra::BaseVector* lambda) -{ - Real lamb1,lamb2, lamb3; - - lamb1 = lambda->element(m_cid); - lamb2 = lambda->element(m_cid+1); - - if(m_thirdConstraint) - { - lamb3 = lambda->element(m_cid+2); - d_force.setValue( m_dirProj* lamb1 + m_dirOrtho * lamb2 + m_dirAxe * lamb3); - } - else - { - d_force.setValue( m_dirProj* lamb1 + m_dirOrtho * lamb2 ); - } -} - -template -void SlidingConstraint::draw(const core::visual::VisualParams* vparams) -{ - if (!vparams->displayFlags().getShowInteractionForceFields()) - return; - - const auto stateLifeCycle = vparams->drawTool()->makeStateLifeCycle(); - - vparams->drawTool()->disableLighting(); - - sofa::type::RGBAColor color; - - if(m_thirdConstraint<0) - color = sofa::type::RGBAColor::yellow(); - else if(m_thirdConstraint>0) - color = sofa::type::RGBAColor::green(); - else - color = sofa::type::RGBAColor::magenta(); - - std::vector vertices; - vertices.push_back(DataTypes::getCPos((this->mstate1->read(core::ConstVecCoordId::position())->getValue())[d_m1.getValue()])); - - vparams->drawTool()->drawPoints(vertices, 10, color); - vertices.clear(); - - color = sofa::type::RGBAColor::blue(); - vertices.push_back(DataTypes::getCPos((this->mstate2->read(core::ConstVecCoordId::position())->getValue())[d_m2a.getValue()])); - vertices.push_back(DataTypes::getCPos((this->mstate2->read(core::ConstVecCoordId::position())->getValue())[d_m2b.getValue()])); - vparams->drawTool()->drawLines(vertices, 1, color); - - -} - -} //namespace sofa::component::constraint::lagrangian::model +SOFA_DEPRECATED_HEADER("v24.06", "v25.06", "sofa/component/constraint/lagrangian/model/SlidingLagrangianConstraint.inl") diff --git a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/SlidingConstraint.cpp b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/SlidingLagrangianConstraint.cpp similarity index 85% rename from Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/SlidingConstraint.cpp rename to Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/SlidingLagrangianConstraint.cpp index a3e1d474ae9..86f42c1935b 100644 --- a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/SlidingConstraint.cpp +++ b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/SlidingLagrangianConstraint.cpp @@ -19,9 +19,9 @@ * * * Contact information: contact@sofa-framework.org * ******************************************************************************/ -#define SOFA_COMPONENT_CONSTRAINTSET_SLIDINGCONSTRAINT_CPP +#define SOFA_COMPONENT_CONSTRAINTSET_SLIDINGLAGRANGIANCONSTRAINT_CPP -#include +#include #include #include @@ -32,10 +32,10 @@ namespace sofa::component::constraint::lagrangian::model using namespace sofa::defaulttype; using namespace sofa::helper; -int SlidingConstraintClass = core::RegisterObject("TODO-SlidingConstraint") - .add< SlidingConstraint >(true); +int SlidingLagrangianConstraintClass = core::RegisterObject("TODO-SlidingLagrangianConstraint") + .add< SlidingLagrangianConstraint >(true); -template class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API SlidingConstraint; +template class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API SlidingLagrangianConstraint; } //namespace sofa::component::constraint::lagrangian::model diff --git a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/SlidingLagrangianConstraint.h b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/SlidingLagrangianConstraint.h new file mode 100644 index 00000000000..81015097548 --- /dev/null +++ b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/SlidingLagrangianConstraint.h @@ -0,0 +1,113 @@ +/****************************************************************************** +* 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 + +namespace sofa::component::constraint::lagrangian::model +{ + +using sofa::core::ConstraintParams; + +template +class SlidingLagrangianConstraint : public core::behavior::PairInteractionConstraint +{ +public: + SOFA_CLASS(SOFA_TEMPLATE(SlidingLagrangianConstraint,DataTypes), SOFA_TEMPLATE(core::behavior::PairInteractionConstraint,DataTypes)); + + typedef typename DataTypes::VecCoord VecCoord; + typedef typename DataTypes::VecDeriv VecDeriv; + typedef typename DataTypes::MatrixDeriv MatrixDeriv; + typedef typename DataTypes::MatrixDeriv::RowIterator MatrixDerivRowIterator; + typedef typename DataTypes::Coord Coord; + typedef typename DataTypes::Deriv Deriv; + typedef typename Coord::value_type Real; + typedef typename core::behavior::MechanicalState MechanicalState; + typedef typename core::behavior::PairInteractionConstraint Inherit; + + typedef core::objectmodel::Data DataVecCoord; + typedef core::objectmodel::Data DataVecDeriv; + typedef core::objectmodel::Data DataMatrixDeriv; + +protected: + + Data d_m1; ///< index of the spliding point on the first model + Data d_m2a; ///< index of one end of the sliding axis + Data d_m2b; ///< index of the other end of the sliding axis + Data d_force; ///< interaction force + + Real m_dist; // constraint violation + Real m_thirdConstraint; // 0 if A getSlidingIdentifiers() { return {}; } + + virtual type::vector getPairInteractionIdentifiers() override final + { + type::vector ids = getSlidingIdentifiers(); + ids.push_back("Sliding"); + return ids; + } + + +public: + void init() override; + + void buildConstraintMatrix(const core::ConstraintParams* cParams, DataMatrixDeriv &c1, DataMatrixDeriv &c2, unsigned int &cIndex + , const DataVecCoord &x1, const DataVecCoord &x2) override; + + void getConstraintViolation(const core::ConstraintParams* cParams, linearalgebra::BaseVector *v, const DataVecCoord &x1, const DataVecCoord &x2 + , const DataVecDeriv &v1, const DataVecDeriv &v2) override; + + void getConstraintResolution(const core::ConstraintParams*, + std::vector& resTab, + unsigned int& offset) override; + void storeLambda(const ConstraintParams* cParams, sofa::core::MultiVecDerivId res, const sofa::linearalgebra::BaseVector* lambda) override; + + void draw(const core::visual::VisualParams* vparams) override; + +private: + // storage of force + Deriv m_dirAxe, m_dirProj, m_dirOrtho; + + + +}; + +#if !defined(SOFA_COMPONENT_CONSTRAINTSET_SLIDINGLAGRANGIANCONSTRAINT_CPP) +extern template class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API SlidingLagrangianConstraint< defaulttype::Vec3Types >; + +#endif + +} //namespace sofa::component::constraint::lagrangian::model diff --git a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/SlidingLagrangianConstraint.inl b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/SlidingLagrangianConstraint.inl new file mode 100644 index 00000000000..959515f874a --- /dev/null +++ b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/SlidingLagrangianConstraint.inl @@ -0,0 +1,226 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once +#include +#include +#include +#include +#include +#include +namespace sofa::component::constraint::lagrangian::model +{ + +template +SlidingLagrangianConstraint::SlidingLagrangianConstraint() + : SlidingLagrangianConstraint(nullptr, nullptr) +{ +} + +template +SlidingLagrangianConstraint::SlidingLagrangianConstraint(MechanicalState* object) + : SlidingLagrangianConstraint(object, object) +{ +} + +template +SlidingLagrangianConstraint::SlidingLagrangianConstraint(MechanicalState* object1, MechanicalState* object2) + : Inherit(object1, object2) + , d_m1(initData(&d_m1, 0, "sliding_point","index of the spliding point on the first model")) + , d_m2a(initData(&d_m2a, 0, "axis_1","index of one end of the sliding axis")) + , d_m2b(initData(&d_m2b, 0, "axis_2","index of the other end of the sliding axis")) + , d_force(initData(&d_force,"force","force (impulse) used to solve the constraint")) +{ +} + +template +void SlidingLagrangianConstraint::init() +{ + assert(this->mstate1); + assert(this->mstate2); + + m_thirdConstraint = 0; +} + + +template +void SlidingLagrangianConstraint::buildConstraintMatrix(const core::ConstraintParams*, DataMatrixDeriv &c1_d, DataMatrixDeriv &c2_d, unsigned int &cIndex + , const DataVecCoord &x1, const DataVecCoord &x2) +{ + int tm1 = d_m1.getValue(); + int tm2a = d_m2a.getValue(); + int tm2b = d_m2b.getValue(); + + MatrixDeriv &c1 = *c1_d.beginEdit(); + MatrixDeriv &c2 = *c2_d.beginEdit(); + + const Coord P = x1.getValue()[tm1]; + const Coord A = x2.getValue()[tm2a]; + const Coord B = x2.getValue()[tm2b]; + + // the axis + m_dirAxe = B - A; + const Real ab = m_dirAxe.norm(); + m_dirAxe.normalize(); + + // projection of the point on the axis + Real r = (P-A) * m_dirAxe; + Real r2 = r / ab; + const Deriv proj = A + m_dirAxe * r; + + // We move the constraint point onto the projection + m_dirProj = P - proj; + m_dist = m_dirProj.norm(); // constraint violation + m_dirProj.normalize(); // direction of the constraint + + m_dirOrtho = cross(m_dirProj, m_dirAxe); + m_dirOrtho.normalize(); + + m_cid = cIndex; + cIndex += 2; + + MatrixDerivRowIterator c1_it = c1.writeLine(m_cid); + c1_it.addCol(tm1, m_dirProj); + + MatrixDerivRowIterator c2_it = c2.writeLine(m_cid); + c2_it.addCol(tm2a, -m_dirProj * (1-r2)); + c2_it.addCol(tm2b, -m_dirProj * r2); + + c1_it = c1.writeLine(m_cid + 1); + c1_it.setCol(tm1, m_dirOrtho); + + c2_it = c2.writeLine(m_cid + 1); + c2_it.addCol(tm2a, -m_dirOrtho * (1-r2)); + c2_it.addCol(tm2b, -m_dirOrtho * r2); + + m_thirdConstraint = 0; + + if (r < 0) + { + m_thirdConstraint = r; + cIndex++; + + c1_it = c1.writeLine(m_cid + 2); + c1_it.setCol(tm1, m_dirAxe); + + c2_it = c2.writeLine(m_cid + 2); + c2_it.addCol(tm2a, -m_dirAxe); + } + else if (r > ab) + { + m_thirdConstraint = r - ab; + cIndex++; + + c1_it = c1.writeLine(m_cid + 2); + c1_it.setCol(tm1, -m_dirAxe); + + c2_it = c2.writeLine(m_cid + 2); + c2_it.addCol(tm2b, m_dirAxe); + } + + c1_d.endEdit(); + c2_d.endEdit(); +} + + +template +void SlidingLagrangianConstraint::getConstraintViolation(const core::ConstraintParams *, linearalgebra::BaseVector *v, const DataVecCoord &, const DataVecCoord & + , const DataVecDeriv &, const DataVecDeriv &) +{ + v->set(m_cid, m_dist); + v->set(m_cid+1, 0.0); + + if(m_thirdConstraint) + { + if(m_thirdConstraint>0) + v->set(m_cid+2, -m_thirdConstraint); + else + v->set(m_cid+2, m_thirdConstraint); + } +} + + +template +void SlidingLagrangianConstraint::getConstraintResolution(const ConstraintParams*, + std::vector& resTab, + unsigned int& offset) +{ + resTab[offset++] = new BilateralConstraintResolution(); + resTab[offset++] = new BilateralConstraintResolution(); + + if(m_thirdConstraint) + resTab[offset++] = new UnilateralConstraintResolution(); +} + + +template +void SlidingLagrangianConstraint::storeLambda(const ConstraintParams* /*cParams*/, sofa::core::MultiVecDerivId /*res*/, const sofa::linearalgebra::BaseVector* lambda) +{ + Real lamb1,lamb2, lamb3; + + lamb1 = lambda->element(m_cid); + lamb2 = lambda->element(m_cid+1); + + if(m_thirdConstraint) + { + lamb3 = lambda->element(m_cid+2); + d_force.setValue( m_dirProj* lamb1 + m_dirOrtho * lamb2 + m_dirAxe * lamb3); + } + else + { + d_force.setValue( m_dirProj* lamb1 + m_dirOrtho * lamb2 ); + } +} + +template +void SlidingLagrangianConstraint::draw(const core::visual::VisualParams* vparams) +{ + if (!vparams->displayFlags().getShowInteractionForceFields()) + return; + + const auto stateLifeCycle = vparams->drawTool()->makeStateLifeCycle(); + + vparams->drawTool()->disableLighting(); + + sofa::type::RGBAColor color; + + if(m_thirdConstraint<0) + color = sofa::type::RGBAColor::yellow(); + else if(m_thirdConstraint>0) + color = sofa::type::RGBAColor::green(); + else + color = sofa::type::RGBAColor::magenta(); + + std::vector vertices; + vertices.push_back(DataTypes::getCPos((this->mstate1->read(core::ConstVecCoordId::position())->getValue())[d_m1.getValue()])); + + vparams->drawTool()->drawPoints(vertices, 10, color); + vertices.clear(); + + color = sofa::type::RGBAColor::blue(); + vertices.push_back(DataTypes::getCPos((this->mstate2->read(core::ConstVecCoordId::position())->getValue())[d_m2a.getValue()])); + vertices.push_back(DataTypes::getCPos((this->mstate2->read(core::ConstVecCoordId::position())->getValue())[d_m2b.getValue()])); + vparams->drawTool()->drawLines(vertices, 1, color); + + +} + +} //namespace sofa::component::constraint::lagrangian::model diff --git a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/StopperConstraint.h b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/StopperConstraint.h index 42cdea0cfcf..f73e0306abe 100644 --- a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/StopperConstraint.h +++ b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/StopperConstraint.h @@ -20,108 +20,14 @@ * Contact information: contact@sofa-framework.org * ******************************************************************************/ #pragma once -#include -#include -#include -#include -#include +#include -namespace sofa::component::constraint::lagrangian::model -{ - -class StopperConstraintResolution1Dof : public core::behavior::ConstraintResolution -{ -protected: - double _invW, _w, _min, _max ; - -public: - - StopperConstraintResolution1Dof(const double &min, const double &max) - : core::behavior::ConstraintResolution(1) - , _min(min) - , _max(max) - { - } - - void init(int line, SReal** w, SReal*force) override - { - _w = w[line][line]; - _invW = 1.0/_w; - force[line ] = 0.0; - } +SOFA_DEPRECATED_HEADER("v24.06", "v25.06", "sofa/component/constraint/lagrangian/model/StopperLagrangianConstraint.h") - void resolution(int line, SReal** /*w*/, SReal* d, SReal* force, SReal*) override - { - const double dfree = d[line] - _w * force[line]; - if (dfree > _max) - force[line] = (_max - dfree) * _invW; - else if (dfree < _min) - force[line] = (_min - dfree) * _invW; - else - force[line] = 0; - } -}; - -template< class DataTypes > -class StopperConstraint : public core::behavior::Constraint +namespace sofa::component::constraint::lagrangian::model { -public: - SOFA_CLASS(SOFA_TEMPLATE(StopperConstraint,DataTypes), SOFA_TEMPLATE(core::behavior::Constraint,DataTypes)); - - 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 typename Coord::value_type Real; - typedef typename core::behavior::MechanicalState MechanicalState; - typedef typename core::behavior::Constraint Inherit; - - typedef typename DataTypes::MatrixDeriv::RowIterator MatrixDerivRowIterator; - typedef core::objectmodel::Data DataVecCoord; - typedef core::objectmodel::Data DataVecDeriv; - typedef core::objectmodel::Data DataMatrixDeriv; - -protected: - - unsigned int cid; - - Data index; ///< index of the stop constraint - Data min; ///< minimum value accepted - Data max; ///< maximum value accepted - - - - StopperConstraint(MechanicalState* object = nullptr); - - virtual ~StopperConstraint() {} - - - virtual type::vector getConstraintIdentifiers() override final - { - type::vector ids = getStopperIdentifiers(); - ids.push_back("Stopper"); - ids.push_back("Unilateral"); - return ids; - } - - virtual type::vector getStopperIdentifiers(){ return {}; } - - - -public: - void init() override; - void buildConstraintMatrix(const core::ConstraintParams* cParams, DataMatrixDeriv &c_d, unsigned int &cIndex, const DataVecCoord &x) override; - void getConstraintViolation(const core::ConstraintParams* cParams, linearalgebra::BaseVector *resV, const DataVecCoord &x, const DataVecDeriv &v) override; - - void getConstraintResolution(const core::ConstraintParams *, std::vector& resTab, unsigned int& offset) override; -}; - -#if !defined(SOFA_COMPONENT_CONSTRAINTSET_STOPPERCONSTRAINT_CPP) -extern template class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API StopperConstraint; - -#endif - -} //namespace sofa::component::constraint::lagrangian::model +template +using StopperConstraint SOFA_ATTRIBUTE_DEPRECATED("v24.06 ", "v25.06", "StopperConstraint has been renamed to StopperLagrangianConstraint") = StopperLagrangianConstraint; +} diff --git a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/StopperConstraint.inl b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/StopperConstraint.inl index 6f1e04be10f..9c4215fb990 100644 --- a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/StopperConstraint.inl +++ b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/StopperConstraint.inl @@ -20,62 +20,7 @@ * Contact information: contact@sofa-framework.org * ******************************************************************************/ #pragma once -#include -#include -#include +#include -namespace sofa::component::constraint::lagrangian::model -{ - -template -StopperConstraint::StopperConstraint(MechanicalState* object) - : Inherit(object) - , index(initData(&index, 0, "index", "index of the stop constraint")) - , min(initData(&min, -100.0_sreal, "min", "minimum value accepted")) - , max(initData(&max, 100.0_sreal, "max", "maximum value accepted")) -{ -} - -template -void StopperConstraint::init() -{ - this->mstate = dynamic_cast(this->getContext()->getMechanicalState()); - assert(this->mstate); - - helper::WriteAccessor > xData = *this->mstate->write(core::VecCoordId::position()); - VecCoord& x = xData.wref(); - if (x[index.getValue()].x() < min.getValue()) - x[index.getValue()].x() = (Real) min.getValue(); - if (x[index.getValue()].x() > max.getValue()) - x[index.getValue()].x() = (Real) max.getValue(); -} - -template -void StopperConstraint::buildConstraintMatrix(const core::ConstraintParams* /*cParams*/, DataMatrixDeriv &c_d, unsigned int &cIndex, const DataVecCoord &/*x*/) -{ - cid = cIndex; - - MatrixDeriv& c = *c_d.beginEdit(); - - MatrixDerivRowIterator c_it = c.writeLine(cid); - c_it.setCol(index.getValue(), Coord(1)); - - cIndex++; - c_d.endEdit(); -} - -template -void StopperConstraint::getConstraintViolation(const core::ConstraintParams* /*cParams*/, linearalgebra::BaseVector *resV, const DataVecCoord &x, const DataVecDeriv &/*v*/) -{ - resV->set(cid, x.getValue()[index.getValue()][0]); -} - -template -void StopperConstraint::getConstraintResolution(const core::ConstraintParams *, std::vector& resTab, unsigned int& offset) -{ - for(int i=0; i<1; i++) - resTab[offset++] = new StopperConstraintResolution1Dof(min.getValue(), max.getValue()); -} - -} //namespace sofa::component::constraint::lagrangian::model +SOFA_DEPRECATED_HEADER("v24.06", "v25.06", "sofa/component/constraint/lagrangian/model/StopperLagrangianConstraint.inl") diff --git a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/StopperConstraint.cpp b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/StopperLagrangianConstraint.cpp similarity index 85% rename from Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/StopperConstraint.cpp rename to Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/StopperLagrangianConstraint.cpp index a56e09d0713..431a80e8428 100644 --- a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/StopperConstraint.cpp +++ b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/StopperLagrangianConstraint.cpp @@ -19,8 +19,8 @@ * * * Contact information: contact@sofa-framework.org * ******************************************************************************/ -#define SOFA_COMPONENT_CONSTRAINTSET_STOPPERCONSTRAINT_CPP -#include +#define SOFA_COMPONENT_CONSTRAINTSET_STOPPERLAGRANGIANCONSTRAINT_CPP +#include #include #include @@ -31,12 +31,12 @@ namespace sofa::component::constraint::lagrangian::model using namespace sofa::defaulttype; using namespace sofa::helper; -int StopperConstraintClass = core::RegisterObject("TODO-StopperConstraint") - .add< StopperConstraint >() +int StopperLagrangianConstraintClass = core::RegisterObject("TODO-StopperLagrangianConstraint") + .add< StopperLagrangianConstraint >() ; -template class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API StopperConstraint; +template class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API StopperLagrangianConstraint; } //namespace sofa::component::constraint::lagrangian::model 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 new file mode 100644 index 00000000000..e9f6d463985 --- /dev/null +++ b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/StopperLagrangianConstraint.h @@ -0,0 +1,127 @@ +/****************************************************************************** +* 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::component::constraint::lagrangian::model +{ + +class StopperLagrangianConstraintResolution1Dof : public core::behavior::ConstraintResolution +{ +protected: + double _invW, _w, _min, _max ; + +public: + + StopperLagrangianConstraintResolution1Dof(const double &min, const double &max) + : core::behavior::ConstraintResolution(1) + , _min(min) + , _max(max) + { + } + + void init(int line, SReal** w, SReal*force) override + { + _w = w[line][line]; + _invW = 1.0/_w; + force[line ] = 0.0; + } + + void resolution(int line, SReal** /*w*/, SReal* d, SReal* force, SReal*) override + { + const double dfree = d[line] - _w * force[line]; + + if (dfree > _max) + force[line] = (_max - dfree) * _invW; + else if (dfree < _min) + force[line] = (_min - dfree) * _invW; + else + force[line] = 0; + } +}; + +template< class DataTypes > +class StopperLagrangianConstraint : public core::behavior::Constraint +{ +public: + SOFA_CLASS(SOFA_TEMPLATE(StopperLagrangianConstraint,DataTypes), SOFA_TEMPLATE(core::behavior::Constraint,DataTypes)); + + 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 typename Coord::value_type Real; + typedef typename core::behavior::MechanicalState MechanicalState; + typedef typename core::behavior::Constraint Inherit; + + typedef typename DataTypes::MatrixDeriv::RowIterator MatrixDerivRowIterator; + typedef core::objectmodel::Data DataVecCoord; + typedef core::objectmodel::Data DataVecDeriv; + typedef core::objectmodel::Data DataMatrixDeriv; + +protected: + + unsigned int cid; + + Data index; ///< index of the stop constraint + Data min; ///< minimum value accepted + Data max; ///< maximum value accepted + + + + StopperLagrangianConstraint(MechanicalState* object = nullptr); + + virtual ~StopperLagrangianConstraint() {} + + + virtual type::vector getConstraintIdentifiers() override final + { + type::vector ids = getStopperIdentifiers(); + ids.push_back("Stopper"); + ids.push_back("Unilateral"); + return ids; + } + + virtual type::vector getStopperIdentifiers(){ return {}; } + + + +public: + void init() override; + void buildConstraintMatrix(const core::ConstraintParams* cParams, DataMatrixDeriv &c_d, unsigned int &cIndex, const DataVecCoord &x) override; + void getConstraintViolation(const core::ConstraintParams* cParams, linearalgebra::BaseVector *resV, const DataVecCoord &x, const DataVecDeriv &v) override; + + void getConstraintResolution(const core::ConstraintParams *, std::vector& resTab, unsigned int& offset) override; +}; + +#if !defined(SOFA_COMPONENT_CONSTRAINTSET_STOPPERLAGRANGIANCONSTRAINT_CPP) +extern template class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API StopperLagrangianConstraint; + +#endif + +} //namespace sofa::component::constraint::lagrangian::model diff --git a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/StopperLagrangianConstraint.inl b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/StopperLagrangianConstraint.inl new file mode 100644 index 00000000000..7265ab54804 --- /dev/null +++ b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/StopperLagrangianConstraint.inl @@ -0,0 +1,81 @@ +/****************************************************************************** +* 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::constraint::lagrangian::model +{ + +template +StopperLagrangianConstraint::StopperLagrangianConstraint(MechanicalState* object) + : Inherit(object) + , index(initData(&index, 0, "index", "index of the stop constraint")) + , min(initData(&min, -100.0_sreal, "min", "minimum value accepted")) + , max(initData(&max, 100.0_sreal, "max", "maximum value accepted")) +{ +} + +template +void StopperLagrangianConstraint::init() +{ + this->mstate = dynamic_cast(this->getContext()->getMechanicalState()); + assert(this->mstate); + + helper::WriteAccessor > xData = *this->mstate->write(core::VecCoordId::position()); + VecCoord& x = xData.wref(); + if (x[index.getValue()].x() < min.getValue()) + x[index.getValue()].x() = (Real) min.getValue(); + if (x[index.getValue()].x() > max.getValue()) + x[index.getValue()].x() = (Real) max.getValue(); +} + +template +void StopperLagrangianConstraint::buildConstraintMatrix(const core::ConstraintParams* /*cParams*/, DataMatrixDeriv &c_d, unsigned int &cIndex, const DataVecCoord &/*x*/) +{ + cid = cIndex; + + MatrixDeriv& c = *c_d.beginEdit(); + + MatrixDerivRowIterator c_it = c.writeLine(cid); + c_it.setCol(index.getValue(), Coord(1)); + + cIndex++; + c_d.endEdit(); +} + +template +void StopperLagrangianConstraint::getConstraintViolation(const core::ConstraintParams* /*cParams*/, linearalgebra::BaseVector *resV, const DataVecCoord &x, const DataVecDeriv &/*v*/) +{ + resV->set(cid, x.getValue()[index.getValue()][0]); +} + +template +void StopperLagrangianConstraint::getConstraintResolution(const core::ConstraintParams *, std::vector& resTab, unsigned int& offset) +{ + for(int i=0; i<1; i++) + resTab[offset++] = new StopperLagrangianConstraintResolution1Dof(min.getValue(), max.getValue()); +} + +} //namespace sofa::component::constraint::lagrangian::model diff --git a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/UniformConstraint.h b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/UniformConstraint.h index e53399c5b9a..23e3d697dec 100644 --- a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/UniformConstraint.h +++ b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/UniformConstraint.h @@ -20,58 +20,13 @@ * Contact information: contact@sofa-framework.org * ******************************************************************************/ #pragma once -#include -#include +#include -namespace sofa::component::constraint::lagrangian::model -{ +SOFA_DEPRECATED_HEADER("v24.06", "v25.06", "sofa/component/constraint/lagrangian/model/UniformLagrangianConstraint.h") -template < class DataTypes > -class UniformConstraint : public sofa::core::behavior::Constraint< DataTypes > +namespace sofa::component::constraint::lagrangian::model { -public: - SOFA_CLASS(SOFA_TEMPLATE(UniformConstraint, DataTypes), SOFA_TEMPLATE(sofa::core::behavior::Constraint, DataTypes)); - - typedef typename DataTypes::VecCoord VecCoord; - typedef typename DataTypes::VecDeriv VecDeriv; - typedef typename DataTypes::MatrixDeriv MatrixDeriv; - typedef typename DataTypes::Coord Coord; - typedef typename DataTypes::Deriv Deriv; - typedef typename DataTypes::Real Real; - - typedef sofa::Data DataVecCoord; - typedef sofa::Data DataVecDeriv; - typedef sofa::Data DataMatrixDeriv; - - void buildConstraintMatrix(const sofa::core::ConstraintParams* cParams, DataMatrixDeriv & c, unsigned int &cIndex, const DataVecCoord &x) override; - - void getConstraintViolation(const sofa::core::ConstraintParams* cParams, sofa::linearalgebra::BaseVector *resV, const DataVecCoord &x, const DataVecDeriv &v) override; - - void getConstraintResolution(const sofa::core::ConstraintParams* cParams, std::vector& crVector, unsigned int& offset) override; - - sofa::Data d_iterative; ///< Iterate over the bilateral constraints, otherwise a block factorisation is computed. - sofa::Data d_constraintRestPos; ///< if false, constrains the pos to be zero / if true constraint the current position to stay at rest position -protected: - - unsigned int m_constraintIndex; - - UniformConstraint(); - - virtual type::vector getConstraintIdentifiers() override final - { - type::vector ids = getStopperIdentifiers(); - ids.push_back("Uniform"); - ids.push_back("Bilateral"); - return ids; - } - - virtual type::vector getStopperIdentifiers(){ return {}; } - -}; - -#if !defined(SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_UNIFORMCONSTRAINT_CPP) - extern template class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API UniformConstraint; -#endif - -} // namespace sofa::component::constraint::lagrangian::model +template +using UniformConstraint SOFA_ATTRIBUTE_DEPRECATED("v24.06 ", "v25.06", "UniformConstraint has been renamed to UniformLagrangianConstraint") = UniformLagrangianConstraint; +} \ No newline at end of file diff --git a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/UniformConstraint.inl b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/UniformConstraint.inl index b34a9377b97..991bb0accc8 100644 --- a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/UniformConstraint.inl +++ b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/UniformConstraint.inl @@ -19,119 +19,8 @@ * * * Contact information: contact@sofa-framework.org * ******************************************************************************/ - #pragma once -#include - -#include -#include -#include - -namespace sofa::component::constraint::lagrangian::model -{ - -template< class DataTypes > -UniformConstraint::UniformConstraint() - :d_iterative(initData(&d_iterative, true, "iterative", "Iterate over the bilateral constraints, otherwise a block factorisation is computed.")) - ,d_constraintRestPos(initData(&d_constraintRestPos, false, "constrainToRestPos", "if false, constrains the pos to be zero / if true constraint the current position to stay at rest position")) - ,m_constraintIndex(0) -{ - -} - -template< class DataTypes > -void UniformConstraint::buildConstraintMatrix(const sofa::core::ConstraintParams* cParams, DataMatrixDeriv & c, unsigned int &cIndex, const DataVecCoord &x) -{ - SOFA_UNUSED(cParams); - - const auto N = Deriv::size(); // MatrixDeriv is a container of Deriv types. - - auto& jacobian = sofa::helper::getWriteAccessor(c).wref(); - auto xVec = sofa::helper::getReadAccessor(x); - - m_constraintIndex = cIndex; // we should not have to remember this, it should be available through the API directly. - - for (std::size_t i = 0; i < xVec.size(); ++i) - { - for (std::size_t j = 0; j < N; ++j) - { - auto row = jacobian.writeLine(N*i + j + m_constraintIndex); - Deriv d; - d[j] = Real(1); - row.setCol(i, d); - ++cIndex; - } - } -} - -template -void computeViolation(DstV& resV, unsigned int constraintIndex, const - Free& free, size_t N, std::function f) -{ - for (std::size_t i = 0; i < free.size(); ++i) - { - for (std::size_t j = 0; j < N; ++j) - { - resV->set(constraintIndex + i*N + j, f(i, j) ); - } - } -} - -template< class DataTypes > -void UniformConstraint::getConstraintViolation(const sofa::core::ConstraintParams* cParams, sofa::linearalgebra::BaseVector *resV, const DataVecCoord &x, const DataVecDeriv &v) -{ - auto xfree = sofa::helper::getReadAccessor(x); - auto vfree = sofa::helper::getReadAccessor(v); - const SReal dt = this->getContext()->getDt(); - const SReal invDt = 1.0 / dt; - - auto pos = this->getMState()->readPositions(); - auto restPos = this->getMState()->readRestPositions(); - - if (cParams->constOrder() == sofa::core::ConstraintOrder::VEL) - { - if (d_constraintRestPos.getValue()){ - computeViolation(resV, m_constraintIndex, vfree, Deriv::size(),[&invDt,&pos,&vfree,&restPos](int i, int j) - { return vfree[i][j] + invDt *(pos[i][j]-restPos[i][j]); }); - } - else { - computeViolation(resV, m_constraintIndex, vfree, Deriv::size(),[&invDt,&pos,&vfree](int i, int j) - { return vfree[i][j] + invDt *pos[i][j]; }); - } - } - else - { - if( d_constraintRestPos.getValue() ) - computeViolation(resV, m_constraintIndex, xfree, Coord::size(), - [&xfree,&restPos](int i, int j){ return xfree[i][j] - restPos[i][j]; }); - else - computeViolation(resV, m_constraintIndex, xfree, Coord::size(),[&xfree](int i, int j){ return xfree[i][j]; }); - } -} - -template< class DataTypes > -void UniformConstraint::getConstraintResolution(const sofa::core::ConstraintParams* cParams, std::vector& crVector, unsigned int& offset) -{ - SOFA_UNUSED(cParams); - if (d_iterative.getValue()) - { - for (std::size_t i = 0; i < this->getMState()->getSize(); ++i) - { - for (std::size_t j = 0; j < Deriv::size(); ++j) - { - auto* cr = new sofa::component::constraint::lagrangian::model::BilateralConstraintResolution(); - crVector[offset++] = cr; - } - } - } - else - { - const std::size_t nbLines = this->getMState()->getSize() * Deriv::size(); - auto* cr = new sofa::component::constraint::lagrangian::model::BilateralConstraintResolutionNDof(nbLines); - crVector[offset] = cr; - offset += nbLines; - } -} +#include -} // namespace sofa::component::constraint::lagrangian::model +SOFA_DEPRECATED_HEADER("v24.06", "v25.06", "sofa/component/constraint/lagrangian/model/UniformLagrangianConstraint.inl") diff --git a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/UniformConstraint.cpp b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/UniformLagrangianConstraint.cpp similarity index 82% rename from Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/UniformConstraint.cpp rename to Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/UniformLagrangianConstraint.cpp index 60f11dac072..eb993968042 100644 --- a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/UniformConstraint.cpp +++ b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/UniformLagrangianConstraint.cpp @@ -19,8 +19,8 @@ * * * Contact information: contact@sofa-framework.org * ******************************************************************************/ -#define SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_UNIFORMCONSTRAINT_CPP -#include +#define SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_UNIFORMLAGRANGIANCONSTRAINT_CPP +#include #include #include @@ -28,11 +28,11 @@ namespace sofa::component::constraint::lagrangian::model { -int UniformConstraintClass = sofa::core::RegisterObject("A constraint equation applied on all dofs.") -.add< UniformConstraint >() +int UniformLagrangianConstraintClass = sofa::core::RegisterObject("A constraint equation applied on all dofs.") +.add< UniformLagrangianConstraint >() ; -template class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API UniformConstraint; +template class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API UniformLagrangianConstraint; } // namespace sofa::component::constraint::lagrangian::model 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 new file mode 100644 index 00000000000..7576ad73b2f --- /dev/null +++ b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/UniformLagrangianConstraint.h @@ -0,0 +1,78 @@ +/****************************************************************************** +* 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::component::constraint::lagrangian::model +{ + +template < class DataTypes > +class UniformLagrangianConstraint : public sofa::core::behavior::Constraint< DataTypes > +{ +public: + SOFA_CLASS(SOFA_TEMPLATE(UniformLagrangianConstraint, DataTypes), SOFA_TEMPLATE(sofa::core::behavior::Constraint, DataTypes)); + + typedef typename DataTypes::VecCoord VecCoord; + typedef typename DataTypes::VecDeriv VecDeriv; + typedef typename DataTypes::MatrixDeriv MatrixDeriv; + typedef typename DataTypes::Coord Coord; + typedef typename DataTypes::Deriv Deriv; + typedef typename DataTypes::Real Real; + + typedef sofa::Data DataVecCoord; + typedef sofa::Data DataVecDeriv; + typedef sofa::Data DataMatrixDeriv; + + void buildConstraintMatrix(const sofa::core::ConstraintParams* cParams, DataMatrixDeriv & c, unsigned int &cIndex, const DataVecCoord &x) override; + + void getConstraintViolation(const sofa::core::ConstraintParams* cParams, sofa::linearalgebra::BaseVector *resV, const DataVecCoord &x, const DataVecDeriv &v) override; + + void getConstraintResolution(const sofa::core::ConstraintParams* cParams, std::vector& crVector, unsigned int& offset) override; + + sofa::Data d_iterative; ///< Iterate over the bilateral constraints, otherwise a block factorisation is computed. + sofa::Data d_constraintRestPos; ///< if false, constrains the pos to be zero / if true constraint the current position to stay at rest position +protected: + + unsigned int m_constraintIndex; + + UniformLagrangianConstraint(); + + virtual type::vector getConstraintIdentifiers() override final + { + type::vector ids = getStopperIdentifiers(); + ids.push_back("Uniform"); + ids.push_back("Bilateral"); + return ids; + } + + virtual type::vector getStopperIdentifiers(){ return {}; } + +}; + +#if !defined(SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_UNIFORMLAGRANGIANCONSTRAINT_CPP) + extern template class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API UniformLagrangianConstraint; +#endif + + +} // namespace sofa::component::constraint::lagrangian::model diff --git a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/UniformLagrangianConstraint.inl b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/UniformLagrangianConstraint.inl new file mode 100644 index 00000000000..707797df0c4 --- /dev/null +++ b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/UniformLagrangianConstraint.inl @@ -0,0 +1,137 @@ +/****************************************************************************** +* 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 + +namespace sofa::component::constraint::lagrangian::model +{ + +template< class DataTypes > +UniformLagrangianConstraint::UniformLagrangianConstraint() + :d_iterative(initData(&d_iterative, true, "iterative", "Iterate over the bilateral constraints, otherwise a block factorisation is computed.")) + ,d_constraintRestPos(initData(&d_constraintRestPos, false, "constrainToRestPos", "if false, constrains the pos to be zero / if true constraint the current position to stay at rest position")) + ,m_constraintIndex(0) +{ + +} + +template< class DataTypes > +void UniformLagrangianConstraint::buildConstraintMatrix(const sofa::core::ConstraintParams* cParams, DataMatrixDeriv & c, unsigned int &cIndex, const DataVecCoord &x) +{ + SOFA_UNUSED(cParams); + + const auto N = Deriv::size(); // MatrixDeriv is a container of Deriv types. + + auto& jacobian = sofa::helper::getWriteAccessor(c).wref(); + auto xVec = sofa::helper::getReadAccessor(x); + + m_constraintIndex = cIndex; // we should not have to remember this, it should be available through the API directly. + + for (std::size_t i = 0; i < xVec.size(); ++i) + { + for (std::size_t j = 0; j < N; ++j) + { + auto row = jacobian.writeLine(N*i + j + m_constraintIndex); + Deriv d; + d[j] = Real(1); + row.setCol(i, d); + ++cIndex; + } + } +} + +template +void computeViolation(DstV& resV, unsigned int constraintIndex, const + Free& free, size_t N, std::function f) +{ + for (std::size_t i = 0; i < free.size(); ++i) + { + for (std::size_t j = 0; j < N; ++j) + { + resV->set(constraintIndex + i*N + j, f(i, j) ); + } + } +} + +template< class DataTypes > +void UniformLagrangianConstraint::getConstraintViolation(const sofa::core::ConstraintParams* cParams, sofa::linearalgebra::BaseVector *resV, const DataVecCoord &x, const DataVecDeriv &v) +{ + auto xfree = sofa::helper::getReadAccessor(x); + auto vfree = sofa::helper::getReadAccessor(v); + const SReal dt = this->getContext()->getDt(); + const SReal invDt = 1.0 / dt; + + auto pos = this->getMState()->readPositions(); + auto restPos = this->getMState()->readRestPositions(); + + if (cParams->constOrder() == sofa::core::ConstraintOrder::VEL) + { + if (d_constraintRestPos.getValue()){ + computeViolation(resV, m_constraintIndex, vfree, Deriv::size(),[&invDt,&pos,&vfree,&restPos](int i, int j) + { return vfree[i][j] + invDt *(pos[i][j]-restPos[i][j]); }); + } + else { + computeViolation(resV, m_constraintIndex, vfree, Deriv::size(),[&invDt,&pos,&vfree](int i, int j) + { return vfree[i][j] + invDt *pos[i][j]; }); + } + } + else + { + if( d_constraintRestPos.getValue() ) + computeViolation(resV, m_constraintIndex, xfree, Coord::size(), + [&xfree,&restPos](int i, int j){ return xfree[i][j] - restPos[i][j]; }); + else + computeViolation(resV, m_constraintIndex, xfree, Coord::size(),[&xfree](int i, int j){ return xfree[i][j]; }); + } +} + +template< class DataTypes > +void UniformLagrangianConstraint::getConstraintResolution(const sofa::core::ConstraintParams* cParams, std::vector& crVector, unsigned int& offset) +{ + SOFA_UNUSED(cParams); + + if (d_iterative.getValue()) + { + for (std::size_t i = 0; i < this->getMState()->getSize(); ++i) + { + for (std::size_t j = 0; j < Deriv::size(); ++j) + { + auto* cr = new sofa::component::constraint::lagrangian::model::BilateralConstraintResolution(); + crVector[offset++] = cr; + } + } + } + else + { + const std::size_t nbLines = this->getMState()->getSize() * Deriv::size(); + auto* cr = new sofa::component::constraint::lagrangian::model::BilateralConstraintResolutionNDof(nbLines); + crVector[offset] = cr; + offset += nbLines; + } +} + +} // namespace sofa::component::constraint::lagrangian::model diff --git a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/UnilateralConstraintResolution.h b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/UnilateralConstraintResolution.h new file mode 100644 index 00000000000..3cf9f7847e5 --- /dev/null +++ b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/UnilateralConstraintResolution.h @@ -0,0 +1,100 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once +#include + +#include +#include +#include +#include +#include + + +namespace sofa::component::constraint::lagrangian::model +{ + +class UnilateralConstraintResolution : public core::behavior::ConstraintResolution +{ + public: + UnilateralConstraintResolution() : core::behavior::ConstraintResolution(1) {} + + void resolution(int line, SReal** w, SReal* d, SReal* force, SReal* dfree) override + { + SOFA_UNUSED(dfree); + force[line] -= d[line] / w[line][line]; + if (force[line] < 0) force[line] = 0.0; + } +}; + +// A little experiment on how to best save the forces for the hot start. +// TODO : save as a map (index of the contact <-> force) +class PreviousForcesContainer +{ + public: + PreviousForcesContainer() : resetFlag(true) {} + SReal popForce() + { + resetFlag = true; + if (forces.empty()) return 0; + const SReal f = forces.front(); + forces.pop_front(); + return f; + } + + void pushForce(SReal f) + { + if (resetFlag) + { + forces.clear(); + resetFlag = false; + } + + forces.push_back(f); + } + + protected: + std::deque forces; + bool resetFlag; // We delete all forces that were not read +}; + +class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API UnilateralConstraintResolutionWithFriction + : public core::behavior::ConstraintResolution +{ + public: + UnilateralConstraintResolutionWithFriction(SReal mu, PreviousForcesContainer* prev = nullptr, + bool* active = nullptr) + : core::behavior::ConstraintResolution(3), _mu(mu), _prev(prev), _active(active) + { + } + + void init(int line, SReal** w, SReal* force) override; + void resolution(int line, SReal** w, SReal* d, SReal* force, SReal* dFree) override; + void store(int line, SReal* force, bool /*convergence*/) override; + + protected: + SReal _mu; + SReal _W[6]; + PreviousForcesContainer* _prev; + bool* _active; // Will set this after the resolution +}; + +} \ No newline at end of file diff --git a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/UnilateralInteractionConstraint.h b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/UnilateralInteractionConstraint.h index 254103f59fe..c7738de22e1 100644 --- a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/UnilateralInteractionConstraint.h +++ b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/UnilateralInteractionConstraint.h @@ -20,210 +20,13 @@ * Contact information: contact@sofa-framework.org * ******************************************************************************/ #pragma once -#include -#include -#include -#include -#include -#include -#include -#include +#include +SOFA_DEPRECATED_HEADER("v24.06", "v25.06", "sofa/component/constraint/lagrangian/model/UnilateralInteractionLagrangianConstraint.h") namespace sofa::component::constraint::lagrangian::model { -class UnilateralConstraintResolution : public core::behavior::ConstraintResolution -{ -public: - - UnilateralConstraintResolution() : core::behavior::ConstraintResolution(1) - { - - } - - void resolution(int line, SReal** w, SReal* d, SReal* force, SReal *dfree) override - { - SOFA_UNUSED(dfree); - force[line] -= d[line] / w[line][line]; - if(force[line] < 0) - force[line] = 0.0; - } -}; - -// A little experiment on how to best save the forces for the hot start. -// TODO : save as a map (index of the contact <-> force) -class PreviousForcesContainer -{ -public: - PreviousForcesContainer() : resetFlag(true) {} - SReal popForce() - { - resetFlag = true; - if(forces.empty()) return 0; - const SReal f = forces.front(); - forces.pop_front(); - return f; - } - - void pushForce(SReal f) - { - if(resetFlag) - { - forces.clear(); - resetFlag = false; - } - - forces.push_back(f); - } - -protected: - std::deque forces; - bool resetFlag; // We delete all forces that were not read -}; - -class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API UnilateralConstraintResolutionWithFriction : public core::behavior::ConstraintResolution -{ -public: - UnilateralConstraintResolutionWithFriction(SReal mu, PreviousForcesContainer* prev = nullptr, bool* active = nullptr) - :core::behavior::ConstraintResolution(3) - , _mu(mu) - , _prev(prev) - , _active(active) - { - } - - void init(int line, SReal** w, SReal* force) override; - void resolution(int line, SReal** w, SReal* d, SReal* force, SReal *dFree) override; - void store(int line, SReal* force, bool /*convergence*/) override; - -protected: - SReal _mu; - SReal _W[6]; - PreviousForcesContainer* _prev; - bool* _active; // Will set this after the resolution -}; - - -template -class UnilateralInteractionConstraint : public core::behavior::PairInteractionConstraint -{ -public: - SOFA_CLASS(SOFA_TEMPLATE(UnilateralInteractionConstraint,DataTypes), SOFA_TEMPLATE(core::behavior::PairInteractionConstraint,DataTypes)); - - typedef typename DataTypes::VecCoord VecCoord; - typedef typename DataTypes::VecDeriv VecDeriv; - typedef typename DataTypes::MatrixDeriv MatrixDeriv; - typedef typename DataTypes::MatrixDeriv::RowConstIterator MatrixDerivRowConstIterator; - typedef typename DataTypes::MatrixDeriv::ColConstIterator MatrixDerivColConstIterator; - typedef typename DataTypes::MatrixDeriv::RowIterator MatrixDerivRowIterator; - typedef typename DataTypes::MatrixDeriv::ColIterator MatrixDerivColIterator; - typedef typename DataTypes::Coord Coord; - typedef typename DataTypes::Deriv Deriv; - typedef typename Coord::value_type Real; - typedef typename core::behavior::MechanicalState MechanicalState; - - typedef core::behavior::BaseConstraint::ConstraintBlockInfo ConstraintBlockInfo; - typedef core::behavior::BaseConstraint::PersistentID PersistentID; - typedef core::behavior::BaseConstraint::ConstCoord ConstCoord; - - typedef core::behavior::BaseConstraint::VecConstraintBlockInfo VecConstraintBlockInfo; - typedef core::behavior::BaseConstraint::VecPersistentID VecPersistentID; - typedef core::behavior::BaseConstraint::VecConstCoord VecConstCoord; - typedef core::behavior::BaseConstraint::VecConstDeriv VecConstDeriv; - typedef core::behavior::BaseConstraint::VecConstArea VecConstArea; - - typedef core::objectmodel::Data DataVecCoord; - typedef core::objectmodel::Data DataVecDeriv; - typedef core::objectmodel::Data DataMatrixDeriv; - - typedef typename core::behavior::PairInteractionConstraint Inherit; - -protected: - - struct Contact - { - int m1, m2; ///< the two extremities of the spring: masses m1 and m2 - Deriv norm; ///< contact normal, from m1 to m2 - Deriv t; ///< added for friction - Deriv s; ///< added for friction - Real contactDistance; - - unsigned int id; - long contactId; - PersistentID localId; - SReal mu; ///< angle for friction - - Coord P, Q; - - mutable Real dfree; - }; - - sofa::type::vector contacts; - Real epsilon; - bool yetIntegrated; - SReal customTolerance; - - PreviousForcesContainer prevForces; - bool* contactsStatus; - - /// Computes constraint violation in position and stores it into resolution global vector - /// - /// @param v Global resolution vector - virtual void getPositionViolation(linearalgebra::BaseVector *v); - - ///Computes constraint violation in velocity and stores it into resolution global vector - /// - /// @param v Global resolution vector - virtual void getVelocityViolation(linearalgebra::BaseVector *v); - -public: - - unsigned int constraintId; -protected: - - virtual type::vector getUnilateralInteractionIdentifiers() {return {};} - - virtual type::vector getPairInteractionIdentifiers() override final - { - type::vector ids = getUnilateralInteractionIdentifiers(); - ids.push_back("Unilateral"); - return ids; - } - - - UnilateralInteractionConstraint(MechanicalState* object1=nullptr, MechanicalState* object2=nullptr); - virtual ~UnilateralInteractionConstraint(); - -public: - void setCustomTolerance(SReal tol) { customTolerance = tol; } - - void clear(int reserve = 0); - - virtual void addContact(SReal mu, Deriv norm, Coord P, Coord Q, Real contactDistance, int m1, int m2, Coord Pfree, Coord Qfree, long id=0, PersistentID localid=0); - - void addContact(SReal mu, Deriv norm, Coord P, Coord Q, Real contactDistance, int m1, int m2, long id=0, PersistentID localid=0); - void addContact(SReal mu, Deriv norm, Real contactDistance, int m1, int m2, long id=0, PersistentID localid=0); - - void buildConstraintMatrix(const core::ConstraintParams* cParams, DataMatrixDeriv &c1, DataMatrixDeriv &c2, unsigned int &cIndex - , const DataVecCoord &x1, const DataVecCoord &x2) override; - - void getConstraintViolation(const core::ConstraintParams* cParams, linearalgebra::BaseVector *v, const DataVecCoord &x1, const DataVecCoord &x2 - , const DataVecDeriv &v1, const DataVecDeriv &v2) override; - - - void getConstraintInfo(const core::ConstraintParams* cParams, VecConstraintBlockInfo& blocks, VecPersistentID& ids, VecConstCoord& positions, VecConstDeriv& directions, VecConstArea& areas) override; - - void getConstraintResolution(const core::ConstraintParams *,std::vector& resTab, unsigned int& offset) override; - bool isActive() const override; - - void draw(const core::visual::VisualParams* vparams) override; -}; - - -#if !defined(SOFA_COMPONENT_CONSTRAINTSET_UNILATERALINTERACTIONCONSTRAINT_CPP) -extern template class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API UnilateralInteractionConstraint; - -#endif - -} //namespace sofa::component::constraint::lagrangian::model +template +using UnilateralInteractionConstraint SOFA_ATTRIBUTE_DEPRECATED("v24.06 ", "v25.06", "UnilateralInteractionConstraint has been renamed to UnilateralInteractionLagrangianConstraint") = UnilateralInteractionLagrangianConstraint; +} \ No newline at end of file diff --git a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/UnilateralInteractionConstraint.inl b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/UnilateralInteractionConstraint.inl index 7cd50ba7b9b..1b2dee107de 100644 --- a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/UnilateralInteractionConstraint.inl +++ b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/UnilateralInteractionConstraint.inl @@ -20,402 +20,7 @@ * Contact information: contact@sofa-framework.org * ******************************************************************************/ #pragma once -#include -#include -#include -#include -#include -namespace sofa::component::constraint::lagrangian::model -{ +#include -template -UnilateralInteractionConstraint::UnilateralInteractionConstraint(MechanicalState* object1, MechanicalState* object2) - : Inherit(object1, object2) - , epsilon(Real(0.001)) - , yetIntegrated(false) - , customTolerance(0.0) - , contactsStatus(nullptr) -{ -} - -template -UnilateralInteractionConstraint::~UnilateralInteractionConstraint() -{ - if(contactsStatus) - delete[] contactsStatus; -} - -template -void UnilateralInteractionConstraint::clear(int reserve) -{ - contacts.clear(); - if (reserve) - contacts.reserve(reserve); -} - -template -void UnilateralInteractionConstraint::addContact(SReal mu, Deriv norm, Coord P, Coord Q, Real contactDistance, int m1, int m2, long id, PersistentID localid) -{ - addContact(mu, norm, P, Q, contactDistance, m1, m2, - this->getMState2()->read(core::ConstVecCoordId::freePosition())->getValue()[m2], - this->getMState1()->read(core::ConstVecCoordId::freePosition())->getValue()[m1], - id, localid); -} - -template -void UnilateralInteractionConstraint::addContact(SReal mu, Deriv norm, Real contactDistance, int m1, int m2, long id, PersistentID localid) -{ - addContact(mu, norm, - this->getMState2()->read(core::ConstVecCoordId::position())->getValue()[m2], - this->getMState1()->read(core::ConstVecCoordId::position())->getValue()[m1], - contactDistance, m1, m2, - this->getMState2()->read(core::ConstVecCoordId::freePosition())->getValue()[m2], - this->getMState1()->read(core::ConstVecCoordId::freePosition())->getValue()[m1], - id, localid); -} - -template -void UnilateralInteractionConstraint::addContact(SReal mu, Deriv norm, Coord P, Coord Q, Real contactDistance, int m1, int m2, Coord /*Pfree*/, Coord /*Qfree*/, long id, PersistentID localid) -{ - contacts.resize(contacts.size() + 1); - Contact &c = contacts.back(); - - c.P = P; - c.Q = Q; - c.m1 = m1; - c.m2 = m2; - c.norm = norm; - c.t = Deriv(norm.z(), norm.x(), norm.y()); - c.s = cross(norm, c.t); - c.s = c.s / c.s.norm(); - c.t = cross((-norm), c.s); - c.mu = mu; - c.contactId = id; - c.localId = localid; - c.contactDistance = contactDistance; -} - - -template -void UnilateralInteractionConstraint::buildConstraintMatrix(const core::ConstraintParams *, DataMatrixDeriv &c1_d, DataMatrixDeriv &c2_d, unsigned int &contactId - , const DataVecCoord &, const DataVecCoord &) -{ - assert(this->mstate1); - assert(this->mstate2); - - if (this->mstate1 == this->mstate2) - { - MatrixDeriv& c1 = *c1_d.beginEdit(); - - for (unsigned int i = 0; i < contacts.size(); i++) - { - Contact& c = contacts[i]; - - c.id = contactId++; - - MatrixDerivRowIterator c1_it = c1.writeLine(c.id); - - c1_it.addCol(c.m1, -c.norm); - c1_it.addCol(c.m2, c.norm); - - if (c.mu > 0.0) - { - c1_it = c1.writeLine(c.id + 1); - c1_it.setCol(c.m1, -c.t); - c1_it.setCol(c.m2, c.t); - - c1_it = c1.writeLine(c.id + 2); - c1_it.setCol(c.m1, -c.s); - c1_it.setCol(c.m2, c.s); - - contactId += 2; - } - } - - c1_d.endEdit(); - } - else - { - MatrixDeriv& c1 = *c1_d.beginEdit(); - MatrixDeriv& c2 = *c2_d.beginEdit(); - - for (unsigned int i = 0; i < contacts.size(); i++) - { - Contact& c = contacts[i]; - - c.id = contactId++; - - MatrixDerivRowIterator c1_it = c1.writeLine(c.id); - c1_it.addCol(c.m1, -c.norm); - - MatrixDerivRowIterator c2_it = c2.writeLine(c.id); - c2_it.addCol(c.m2, c.norm); - - if (c.mu > 0.0) - { - c1_it = c1.writeLine(c.id + 1); - c1_it.setCol(c.m1, -c.t); - - c1_it = c1.writeLine(c.id + 2); - c1_it.setCol(c.m1, -c.s); - - c2_it = c2.writeLine(c.id + 1); - c2_it.setCol(c.m2, c.t); - - c2_it = c2.writeLine(c.id + 2); - c2_it.setCol(c.m2, c.s); - - contactId += 2; - } - } - - c1_d.endEdit(); - c2_d.endEdit(); - } -} - - -template -void UnilateralInteractionConstraint::getPositionViolation(linearalgebra::BaseVector *v) -{ - const VecCoord &PfreeVec = this->getMState2()->read(core::ConstVecCoordId::freePosition())->getValue(); - const VecCoord &QfreeVec = this->getMState1()->read(core::ConstVecCoordId::freePosition())->getValue(); - - Real dfree = (Real)0.0; - Real dfree_t = (Real)0.0; - Real dfree_s = (Real)0.0; - - const unsigned int cSize = contacts.size(); - - for (unsigned int i = 0; i < cSize; i++) - { - const Contact& c = contacts[i]; - - // Compute dfree, dfree_t and d_free_s - - const Coord &Pfree = PfreeVec[c.m2]; - const Coord &Qfree = QfreeVec[c.m1]; - - const Coord PPfree = Pfree - c.P; - const Coord QQfree = Qfree - c.Q; - - const Real ref_dist = PPfree.norm() + QQfree.norm(); - - dfree = dot(Pfree - Qfree, c.norm) - c.contactDistance; - const Real delta = dot(c.P - c.Q, c.norm) - c.contactDistance; - - if ((helper::rabs(delta) < 0.00001 * ref_dist) && (helper::rabs(dfree) < 0.00001 * ref_dist)) - { - dfree_t = dot(PPfree, c.t) - dot(QQfree, c.t); - dfree_s = dot(PPfree, c.s) - dot(QQfree, c.s); - } - else if (helper::rabs(delta - dfree) > 0.001 * delta) - { - const Real dt = delta / (delta - dfree); - - if (dt > 0.0 && dt < 1.0) - { - const Coord Pt = c.P * (1 - dt) + Pfree * dt; - const Coord Qt = c.Q * (1 - dt) + Qfree * dt; - const Coord PtPfree = Pfree - Pt; - const Coord QtQfree = Qfree - Qt; - - dfree_t = dot(PtPfree, c.t) - dot(QtQfree, c.t); - dfree_s = dot(PtPfree, c.s) - dot(QtQfree, c.s); - } - else if (dfree < 0.0) - { - dfree_t = dot(PPfree, c.t) - dot(QQfree, c.t); - dfree_s = dot(PPfree, c.s) - dot(QQfree, c.s); - } - else - { - dfree_t = 0; - dfree_s = 0; - } - } - else - { - dfree_t = dot(PPfree, c.t) - dot(QQfree, c.t); - dfree_s = dot(PPfree, c.s) - dot(QQfree, c.s); - } - - // Sets dfree in global violation vector - - v->set(c.id, dfree); - - c.dfree = dfree; // PJ : For isActive() method. Don't know if it's still usefull. - - if (c.mu > 0.0) - { - v->set(c.id + 1, dfree_t); - v->set(c.id + 2, dfree_s); - } - } -} - - -template -void UnilateralInteractionConstraint::getVelocityViolation(linearalgebra::BaseVector *v) -{ - auto P = this->getMState2()->readPositions(); - auto Q = this->getMState1()->readPositions(); - - const SReal dt = this->getContext()->getDt(); - const SReal invDt = SReal(1.0) / dt; - - const VecDeriv &PvfreeVec = this->getMState2()->read(core::ConstVecDerivId::freeVelocity())->getValue(); - const VecDeriv &QvfreeVec = this->getMState1()->read(core::ConstVecDerivId::freeVelocity())->getValue(); - - const unsigned int cSize = contacts.size(); - - for (unsigned int i = 0; i < cSize; i++) - { - const Contact& c = contacts[i]; - - const Deriv QP_invDt = (P[c.m2] - Q[c.m1])*invDt; - const Deriv QP_vfree = PvfreeVec[c.m2] - QvfreeVec[c.m1]; - const Deriv dFreeVec = QP_vfree + QP_invDt; - - v->set(c.id, dot(dFreeVec, c.norm) - c.contactDistance*invDt ); // dvfree = 1/dt * [ dot ( P - Q, n) - contactDist ] + dot(v_P - v_Q , n ) ] - - if (c.mu > 0.0) - { - v->set(c.id + 1, dot(QP_vfree, c.t)); // dfree_t - v->set(c.id + 2, dot(QP_vfree, c.s)); // dfree_s - } - } -} - - -template -void UnilateralInteractionConstraint::getConstraintViolation(const core::ConstraintParams *cparams, linearalgebra::BaseVector *v, const DataVecCoord &, const DataVecCoord & - , const DataVecDeriv &, const DataVecDeriv &) -{ - switch (cparams->constOrder()) - { - case core::ConstraintOrder::POS_AND_VEL : - case core::ConstraintOrder::POS : - getPositionViolation(v); - break; - - case core::ConstraintOrder::ACC : - case core::ConstraintOrder::VEL : - getVelocityViolation(v); - break; - - default : - msg_error() << "UnilateralInteractionConstraint doesn't implement " << cparams->getName() << " constraint violation\n"; - break; - } -} - - -template -void UnilateralInteractionConstraint::getConstraintInfo(const core::ConstraintParams*, VecConstraintBlockInfo& blocks, VecPersistentID& ids, VecConstCoord& /*positions*/, VecConstDeriv& directions, VecConstArea& /*areas*/) -{ - if (contacts.empty()) return; - const bool friction = (contacts[0].mu > 0.0); /// @todo: can there be both friction-less and friction contacts in the same UnilateralInteractionConstraint ??? - ConstraintBlockInfo info; - info.parent = this; - info.const0 = contacts[0].id; - info.nbLines = friction ? 3 : 1; - info.hasId = true; - info.offsetId = ids.size(); - info.hasDirection = true; - info.offsetDirection = directions.size(); - info.nbGroups = contacts.size(); - - for (unsigned int i=0; i -void UnilateralInteractionConstraint::getConstraintResolution(const core::ConstraintParams *, std::vector& resTab, unsigned int& offset) -{ - if(contactsStatus) - { - delete[] contactsStatus; - contactsStatus = nullptr; - } - - if (contacts.size() > 0) - { - contactsStatus = new bool[contacts.size()]; - memset(contactsStatus, 0, sizeof(bool)*contacts.size()); - } - - for(unsigned int i=0; i 0.0) - { - UnilateralConstraintResolutionWithFriction* ucrwf = new UnilateralConstraintResolutionWithFriction(c.mu, nullptr, &contactsStatus[i]); - ucrwf->setTolerance(customTolerance); - resTab[offset] = ucrwf; - - // TODO : cette méthode de stockage des forces peu mal fonctionner avec 2 threads quand on utilise l'haptique - offset += 3; - } - else - resTab[offset++] = new UnilateralConstraintResolution(); - } -} - -template -bool UnilateralInteractionConstraint::isActive() const -{ - for(unsigned int i = 0; i < contacts.size(); i++) - if(contacts[i].dfree < 0) - return true; - - return false; -} - -template -void UnilateralInteractionConstraint::draw(const core::visual::VisualParams* vparams) -{ - if (!vparams->displayFlags().getShowInteractionForceFields()) return; - - const auto stateLifeCycle = vparams->drawTool()->makeStateLifeCycle(); - vparams->drawTool()->disableLighting(); - - std::vector redVertices; - std::vector otherVertices; - std::vector otherColors; - - for (unsigned int i=0; idrawTool()->drawLines(otherVertices, 3, otherColors); - - - - -} - -} //namespace sofa::component::constraint::lagrangian::model +SOFA_DEPRECATED_HEADER("v24.06", "v25.06", "sofa/component/constraint/lagrangian/model/UnilateralInteractionLagrangianConstraint.inl") diff --git a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/UnilateralInteractionConstraint.cpp b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/UnilateralInteractionLagrangianConstraint.cpp similarity index 91% rename from Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/UnilateralInteractionConstraint.cpp rename to Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/UnilateralInteractionLagrangianConstraint.cpp index 731d7ff2372..6e44411758f 100644 --- a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/UnilateralInteractionConstraint.cpp +++ b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/UnilateralInteractionLagrangianConstraint.cpp @@ -19,8 +19,8 @@ * * * Contact information: contact@sofa-framework.org * ******************************************************************************/ -#define SOFA_COMPONENT_CONSTRAINTSET_UNILATERALINTERACTIONCONSTRAINT_CPP -#include +#define SOFA_COMPONENT_CONSTRAINTSET_UNILATERALINTERACTIONLAGRANGIANCONSTRAINT_CPP +#include #include #include @@ -31,13 +31,13 @@ using namespace sofa::defaulttype; using namespace sofa::helper; //TODO(dmarchal) What does this TODO mean ? -int UnilateralInteractionConstraintClass = core::RegisterObject("TODO-UnilateralInteractionConstraint") - .add< UnilateralInteractionConstraint >() +int UnilateralInteractionLagrangianConstraintClass = core::RegisterObject("TODO-UnilateralInteractionLagrangianConstraint") + .add< UnilateralInteractionLagrangianConstraint >() ; -template class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API UnilateralInteractionConstraint; +template class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API UnilateralInteractionLagrangianConstraint; diff --git a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/UnilateralInteractionLagrangianConstraint.h b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/UnilateralInteractionLagrangianConstraint.h new file mode 100644 index 00000000000..d777b53b4fb --- /dev/null +++ b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/UnilateralInteractionLagrangianConstraint.h @@ -0,0 +1,158 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace sofa::component::constraint::lagrangian::model +{ + +template +class UnilateralInteractionLagrangianConstraint : public core::behavior::PairInteractionConstraint +{ +public: + SOFA_CLASS(SOFA_TEMPLATE(UnilateralInteractionLagrangianConstraint,DataTypes), SOFA_TEMPLATE(core::behavior::PairInteractionConstraint,DataTypes)); + + typedef typename DataTypes::VecCoord VecCoord; + typedef typename DataTypes::VecDeriv VecDeriv; + typedef typename DataTypes::MatrixDeriv MatrixDeriv; + typedef typename DataTypes::MatrixDeriv::RowConstIterator MatrixDerivRowConstIterator; + typedef typename DataTypes::MatrixDeriv::ColConstIterator MatrixDerivColConstIterator; + typedef typename DataTypes::MatrixDeriv::RowIterator MatrixDerivRowIterator; + typedef typename DataTypes::MatrixDeriv::ColIterator MatrixDerivColIterator; + typedef typename DataTypes::Coord Coord; + typedef typename DataTypes::Deriv Deriv; + typedef typename Coord::value_type Real; + typedef typename core::behavior::MechanicalState MechanicalState; + + typedef core::behavior::BaseConstraint::ConstraintBlockInfo ConstraintBlockInfo; + typedef core::behavior::BaseConstraint::PersistentID PersistentID; + typedef core::behavior::BaseConstraint::ConstCoord ConstCoord; + + typedef core::behavior::BaseConstraint::VecConstraintBlockInfo VecConstraintBlockInfo; + typedef core::behavior::BaseConstraint::VecPersistentID VecPersistentID; + typedef core::behavior::BaseConstraint::VecConstCoord VecConstCoord; + typedef core::behavior::BaseConstraint::VecConstDeriv VecConstDeriv; + typedef core::behavior::BaseConstraint::VecConstArea VecConstArea; + + typedef core::objectmodel::Data DataVecCoord; + typedef core::objectmodel::Data DataVecDeriv; + typedef core::objectmodel::Data DataMatrixDeriv; + + typedef typename core::behavior::PairInteractionConstraint Inherit; + +protected: + + struct Contact + { + int m1, m2; ///< the two extremities of the spring: masses m1 and m2 + Deriv norm; ///< contact normal, from m1 to m2 + Deriv t; ///< added for friction + Deriv s; ///< added for friction + Real contactDistance; + + unsigned int id; + long contactId; + PersistentID localId; + SReal mu; ///< angle for friction + + Coord P, Q; + + mutable Real dfree; + }; + + sofa::type::vector contacts; + Real epsilon; + bool yetIntegrated; + SReal customTolerance; + + PreviousForcesContainer prevForces; + bool* contactsStatus; + + /// Computes constraint violation in position and stores it into resolution global vector + /// + /// @param v Global resolution vector + virtual void getPositionViolation(linearalgebra::BaseVector *v); + + ///Computes constraint violation in velocity and stores it into resolution global vector + /// + /// @param v Global resolution vector + virtual void getVelocityViolation(linearalgebra::BaseVector *v); + +public: + + unsigned int constraintId; +protected: + + virtual type::vector getUnilateralInteractionIdentifiers() {return {};} + + virtual type::vector getPairInteractionIdentifiers() override final + { + type::vector ids = getUnilateralInteractionIdentifiers(); + ids.push_back("Unilateral"); + return ids; + } + + + UnilateralInteractionLagrangianConstraint(MechanicalState* object1=nullptr, MechanicalState* object2=nullptr); + virtual ~UnilateralInteractionLagrangianConstraint(); + +public: + void setCustomTolerance(SReal tol) { customTolerance = tol; } + + void clear(int reserve = 0); + + virtual void addContact(SReal mu, Deriv norm, Coord P, Coord Q, Real contactDistance, int m1, int m2, Coord Pfree, Coord Qfree, long id=0, PersistentID localid=0); + + void addContact(SReal mu, Deriv norm, Coord P, Coord Q, Real contactDistance, int m1, int m2, long id=0, PersistentID localid=0); + void addContact(SReal mu, Deriv norm, Real contactDistance, int m1, int m2, long id=0, PersistentID localid=0); + + void buildConstraintMatrix(const core::ConstraintParams* cParams, DataMatrixDeriv &c1, DataMatrixDeriv &c2, unsigned int &cIndex + , const DataVecCoord &x1, const DataVecCoord &x2) override; + + void getConstraintViolation(const core::ConstraintParams* cParams, linearalgebra::BaseVector *v, const DataVecCoord &x1, const DataVecCoord &x2 + , const DataVecDeriv &v1, const DataVecDeriv &v2) override; + + + void getConstraintInfo(const core::ConstraintParams* cParams, VecConstraintBlockInfo& blocks, VecPersistentID& ids, VecConstCoord& positions, VecConstDeriv& directions, VecConstArea& areas) override; + + void getConstraintResolution(const core::ConstraintParams *,std::vector& resTab, unsigned int& offset) override; + bool isActive() const override; + + void draw(const core::visual::VisualParams* vparams) override; +}; + + +#if !defined(SOFA_COMPONENT_CONSTRAINTSET_UNILATERALINTERACTIONLAGRANGIANCONSTRAINT_CPP) +extern template class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API UnilateralInteractionLagrangianConstraint; +#endif + + +} //namespace sofa::component::constraint::lagrangian::model diff --git a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/UnilateralInteractionLagrangianConstraint.inl b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/UnilateralInteractionLagrangianConstraint.inl new file mode 100644 index 00000000000..4b65ac27673 --- /dev/null +++ b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/UnilateralInteractionLagrangianConstraint.inl @@ -0,0 +1,421 @@ +/****************************************************************************** +* 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::component::constraint::lagrangian::model +{ + +template +UnilateralInteractionLagrangianConstraint::UnilateralInteractionLagrangianConstraint(MechanicalState* object1, MechanicalState* object2) + : Inherit(object1, object2) + , epsilon(Real(0.001)) + , yetIntegrated(false) + , customTolerance(0.0) + , contactsStatus(nullptr) +{ +} + +template +UnilateralInteractionLagrangianConstraint::~UnilateralInteractionLagrangianConstraint() +{ + if(contactsStatus) + delete[] contactsStatus; +} + +template +void UnilateralInteractionLagrangianConstraint::clear(int reserve) +{ + contacts.clear(); + if (reserve) + contacts.reserve(reserve); +} + +template +void UnilateralInteractionLagrangianConstraint::addContact(SReal mu, Deriv norm, Coord P, Coord Q, Real contactDistance, int m1, int m2, long id, PersistentID localid) +{ + addContact(mu, norm, P, Q, contactDistance, m1, m2, + this->getMState2()->read(core::ConstVecCoordId::freePosition())->getValue()[m2], + this->getMState1()->read(core::ConstVecCoordId::freePosition())->getValue()[m1], + id, localid); +} + +template +void UnilateralInteractionLagrangianConstraint::addContact(SReal mu, Deriv norm, Real contactDistance, int m1, int m2, long id, PersistentID localid) +{ + addContact(mu, norm, + this->getMState2()->read(core::ConstVecCoordId::position())->getValue()[m2], + this->getMState1()->read(core::ConstVecCoordId::position())->getValue()[m1], + contactDistance, m1, m2, + this->getMState2()->read(core::ConstVecCoordId::freePosition())->getValue()[m2], + this->getMState1()->read(core::ConstVecCoordId::freePosition())->getValue()[m1], + id, localid); +} + +template +void UnilateralInteractionLagrangianConstraint::addContact(SReal mu, Deriv norm, Coord P, Coord Q, Real contactDistance, int m1, int m2, Coord /*Pfree*/, Coord /*Qfree*/, long id, PersistentID localid) +{ + contacts.resize(contacts.size() + 1); + Contact &c = contacts.back(); + + c.P = P; + c.Q = Q; + c.m1 = m1; + c.m2 = m2; + c.norm = norm; + c.t = Deriv(norm.z(), norm.x(), norm.y()); + c.s = cross(norm, c.t); + c.s = c.s / c.s.norm(); + c.t = cross((-norm), c.s); + c.mu = mu; + c.contactId = id; + c.localId = localid; + c.contactDistance = contactDistance; +} + + +template +void UnilateralInteractionLagrangianConstraint::buildConstraintMatrix(const core::ConstraintParams *, DataMatrixDeriv &c1_d, DataMatrixDeriv &c2_d, unsigned int &contactId + , const DataVecCoord &, const DataVecCoord &) +{ + assert(this->mstate1); + assert(this->mstate2); + + if (this->mstate1 == this->mstate2) + { + MatrixDeriv& c1 = *c1_d.beginEdit(); + + for (unsigned int i = 0; i < contacts.size(); i++) + { + Contact& c = contacts[i]; + + c.id = contactId++; + + MatrixDerivRowIterator c1_it = c1.writeLine(c.id); + + c1_it.addCol(c.m1, -c.norm); + c1_it.addCol(c.m2, c.norm); + + if (c.mu > 0.0) + { + c1_it = c1.writeLine(c.id + 1); + c1_it.setCol(c.m1, -c.t); + c1_it.setCol(c.m2, c.t); + + c1_it = c1.writeLine(c.id + 2); + c1_it.setCol(c.m1, -c.s); + c1_it.setCol(c.m2, c.s); + + contactId += 2; + } + } + + c1_d.endEdit(); + } + else + { + MatrixDeriv& c1 = *c1_d.beginEdit(); + MatrixDeriv& c2 = *c2_d.beginEdit(); + + for (unsigned int i = 0; i < contacts.size(); i++) + { + Contact& c = contacts[i]; + + c.id = contactId++; + + MatrixDerivRowIterator c1_it = c1.writeLine(c.id); + c1_it.addCol(c.m1, -c.norm); + + MatrixDerivRowIterator c2_it = c2.writeLine(c.id); + c2_it.addCol(c.m2, c.norm); + + if (c.mu > 0.0) + { + c1_it = c1.writeLine(c.id + 1); + c1_it.setCol(c.m1, -c.t); + + c1_it = c1.writeLine(c.id + 2); + c1_it.setCol(c.m1, -c.s); + + c2_it = c2.writeLine(c.id + 1); + c2_it.setCol(c.m2, c.t); + + c2_it = c2.writeLine(c.id + 2); + c2_it.setCol(c.m2, c.s); + + contactId += 2; + } + } + + c1_d.endEdit(); + c2_d.endEdit(); + } +} + + +template +void UnilateralInteractionLagrangianConstraint::getPositionViolation(linearalgebra::BaseVector *v) +{ + const VecCoord &PfreeVec = this->getMState2()->read(core::ConstVecCoordId::freePosition())->getValue(); + const VecCoord &QfreeVec = this->getMState1()->read(core::ConstVecCoordId::freePosition())->getValue(); + + Real dfree = (Real)0.0; + Real dfree_t = (Real)0.0; + Real dfree_s = (Real)0.0; + + const unsigned int cSize = contacts.size(); + + for (unsigned int i = 0; i < cSize; i++) + { + const Contact& c = contacts[i]; + + // Compute dfree, dfree_t and d_free_s + + const Coord &Pfree = PfreeVec[c.m2]; + const Coord &Qfree = QfreeVec[c.m1]; + + const Coord PPfree = Pfree - c.P; + const Coord QQfree = Qfree - c.Q; + + const Real ref_dist = PPfree.norm() + QQfree.norm(); + + dfree = dot(Pfree - Qfree, c.norm) - c.contactDistance; + const Real delta = dot(c.P - c.Q, c.norm) - c.contactDistance; + + if ((helper::rabs(delta) < 0.00001 * ref_dist) && (helper::rabs(dfree) < 0.00001 * ref_dist)) + { + dfree_t = dot(PPfree, c.t) - dot(QQfree, c.t); + dfree_s = dot(PPfree, c.s) - dot(QQfree, c.s); + } + else if (helper::rabs(delta - dfree) > 0.001 * delta) + { + const Real dt = delta / (delta - dfree); + + if (dt > 0.0 && dt < 1.0) + { + const Coord Pt = c.P * (1 - dt) + Pfree * dt; + const Coord Qt = c.Q * (1 - dt) + Qfree * dt; + const Coord PtPfree = Pfree - Pt; + const Coord QtQfree = Qfree - Qt; + + dfree_t = dot(PtPfree, c.t) - dot(QtQfree, c.t); + dfree_s = dot(PtPfree, c.s) - dot(QtQfree, c.s); + } + else if (dfree < 0.0) + { + dfree_t = dot(PPfree, c.t) - dot(QQfree, c.t); + dfree_s = dot(PPfree, c.s) - dot(QQfree, c.s); + } + else + { + dfree_t = 0; + dfree_s = 0; + } + } + else + { + dfree_t = dot(PPfree, c.t) - dot(QQfree, c.t); + dfree_s = dot(PPfree, c.s) - dot(QQfree, c.s); + } + + // Sets dfree in global violation vector + + v->set(c.id, dfree); + + c.dfree = dfree; // PJ : For isActive() method. Don't know if it's still usefull. + + if (c.mu > 0.0) + { + v->set(c.id + 1, dfree_t); + v->set(c.id + 2, dfree_s); + } + } +} + + +template +void UnilateralInteractionLagrangianConstraint::getVelocityViolation(linearalgebra::BaseVector *v) +{ + auto P = this->getMState2()->readPositions(); + auto Q = this->getMState1()->readPositions(); + + const SReal dt = this->getContext()->getDt(); + const SReal invDt = SReal(1.0) / dt; + + const VecDeriv &PvfreeVec = this->getMState2()->read(core::ConstVecDerivId::freeVelocity())->getValue(); + const VecDeriv &QvfreeVec = this->getMState1()->read(core::ConstVecDerivId::freeVelocity())->getValue(); + + const unsigned int cSize = contacts.size(); + + for (unsigned int i = 0; i < cSize; i++) + { + const Contact& c = contacts[i]; + + const Deriv QP_invDt = (P[c.m2] - Q[c.m1])*invDt; + const Deriv QP_vfree = PvfreeVec[c.m2] - QvfreeVec[c.m1]; + const Deriv dFreeVec = QP_vfree + QP_invDt; + + v->set(c.id, dot(dFreeVec, c.norm) - c.contactDistance*invDt ); // dvfree = 1/dt * [ dot ( P - Q, n) - contactDist ] + dot(v_P - v_Q , n ) ] + + if (c.mu > 0.0) + { + v->set(c.id + 1, dot(QP_vfree, c.t)); // dfree_t + v->set(c.id + 2, dot(QP_vfree, c.s)); // dfree_s + } + } +} + + +template +void UnilateralInteractionLagrangianConstraint::getConstraintViolation(const core::ConstraintParams *cparams, linearalgebra::BaseVector *v, const DataVecCoord &, const DataVecCoord & + , const DataVecDeriv &, const DataVecDeriv &) +{ + switch (cparams->constOrder()) + { + case core::ConstraintOrder::POS_AND_VEL : + case core::ConstraintOrder::POS : + getPositionViolation(v); + break; + + case core::ConstraintOrder::ACC : + case core::ConstraintOrder::VEL : + getVelocityViolation(v); + break; + + default : + msg_error() << "UnilateralInteractionLagrangianConstraint doesn't implement " << cparams->getName() << " constraint violation\n"; + break; + } +} + + +template +void UnilateralInteractionLagrangianConstraint::getConstraintInfo(const core::ConstraintParams*, VecConstraintBlockInfo& blocks, VecPersistentID& ids, VecConstCoord& /*positions*/, VecConstDeriv& directions, VecConstArea& /*areas*/) +{ + if (contacts.empty()) return; + const bool friction = (contacts[0].mu > 0.0); /// @todo: can there be both friction-less and friction contacts in the same UnilateralInteractionLagrangianConstraint ??? + ConstraintBlockInfo info; + info.parent = this; + info.const0 = contacts[0].id; + info.nbLines = friction ? 3 : 1; + info.hasId = true; + info.offsetId = ids.size(); + info.hasDirection = true; + info.offsetDirection = directions.size(); + info.nbGroups = contacts.size(); + + for (unsigned int i=0; i +void UnilateralInteractionLagrangianConstraint::getConstraintResolution(const core::ConstraintParams *, std::vector& resTab, unsigned int& offset) +{ + if(contactsStatus) + { + delete[] contactsStatus; + contactsStatus = nullptr; + } + + if (contacts.size() > 0) + { + contactsStatus = new bool[contacts.size()]; + memset(contactsStatus, 0, sizeof(bool)*contacts.size()); + } + + for(unsigned int i=0; i 0.0) + { + UnilateralConstraintResolutionWithFriction* ucrwf = new UnilateralConstraintResolutionWithFriction(c.mu, nullptr, &contactsStatus[i]); + ucrwf->setTolerance(customTolerance); + resTab[offset] = ucrwf; + + // TODO : cette méthode de stockage des forces peu mal fonctionner avec 2 threads quand on utilise l'haptique + offset += 3; + } + else + resTab[offset++] = new UnilateralConstraintResolution(); + } +} + +template +bool UnilateralInteractionLagrangianConstraint::isActive() const +{ + for(unsigned int i = 0; i < contacts.size(); i++) + if(contacts[i].dfree < 0) + return true; + + return false; +} + +template +void UnilateralInteractionLagrangianConstraint::draw(const core::visual::VisualParams* vparams) +{ + if (!vparams->displayFlags().getShowInteractionForceFields()) return; + + const auto stateLifeCycle = vparams->drawTool()->makeStateLifeCycle(); + vparams->drawTool()->disableLighting(); + + std::vector redVertices; + std::vector otherVertices; + std::vector otherColors; + + for (unsigned int i=0; idrawTool()->drawLines(otherVertices, 3, otherColors); + + + + +} + +} //namespace sofa::component::constraint::lagrangian::model diff --git a/Sofa/Component/Constraint/Lagrangian/Model/tests/BilateralInteractionConstraint_test.cpp b/Sofa/Component/Constraint/Lagrangian/Model/tests/BilateralInteractionLagrangianConstraint_test.cpp similarity index 85% rename from Sofa/Component/Constraint/Lagrangian/Model/tests/BilateralInteractionConstraint_test.cpp rename to Sofa/Component/Constraint/Lagrangian/Model/tests/BilateralInteractionLagrangianConstraint_test.cpp index 49925d5a621..a00f9d760d1 100644 --- a/Sofa/Component/Constraint/Lagrangian/Model/tests/BilateralInteractionConstraint_test.cpp +++ b/Sofa/Component/Constraint/Lagrangian/Model/tests/BilateralInteractionLagrangianConstraint_test.cpp @@ -23,7 +23,7 @@ #include #include #include -#include +#include #include #include using sofa::core::execparams::defaultInstance; @@ -50,7 +50,7 @@ using namespace component; using namespace defaulttype; template -struct BilateralInteractionConstraint_test : public NumericTest<> +struct BilateralInteractionLangrangianConstraint_test : public NumericTest<> { typedef _DataTypes DataTypes; typedef typename DataTypes::VecCoord VecCoord; @@ -59,7 +59,7 @@ struct BilateralInteractionConstraint_test : public NumericTest<> typedef typename DataTypes::Deriv Deriv; typedef typename DataTypes::CPos CPos; typedef typename Coord::value_type Real; - typedef constraintset::BilateralInteractionConstraint BilateralInteractionConstraint; + typedef constraintset::BilateralInteractionLangrangianConstraint BilateralInteractionLangrangianConstraint; typedef component::topology::PointSetTopologyContainer PointSetTopologyContainer; typedef container::MechanicalObject MechanicalObject; @@ -103,7 +103,7 @@ struct BilateralInteractionConstraint_test : public NumericTest<> " \n" " \n" " \n" - " \n" + " \n" " \n" ; Node::SPtr root = SceneLoaderXML::loadFromMemory ("testscene", @@ -111,7 +111,7 @@ struct BilateralInteractionConstraint_test : public NumericTest<> scene.str().size()) ; root->init(sofa::core::execparams::defaultInstance()) ; - BilateralInteractionConstraint* constraint = root->getTreeObject() ; + BilateralInteractionLangrangianConstraint* constraint = root->getTreeObject() ; EXPECT_TRUE( constraint != nullptr ) ; EXPECT_TRUE( constraint->findData("first_point") != nullptr ) ; @@ -135,7 +135,7 @@ struct BilateralInteractionConstraint_test : public NumericTest<> std::stringstream scene; scene << " \n" " \n" - " \n" + " \n" " \n" ; Node::SPtr root = SceneLoaderXML::loadFromMemory ("testscene", @@ -150,7 +150,7 @@ struct BilateralInteractionConstraint_test : public NumericTest<> }; template<> -void BilateralInteractionConstraint_test::checkRigid3fFixForBackwardCompatibility(){ +void BilateralInteractionLangrangianConstraint_test::checkRigid3fFixForBackwardCompatibility(){ EXPECT_MSG_EMIT(Warning) ; /// I'm using '\n' so that the XML parser correctly report the line number @@ -160,7 +160,7 @@ void BilateralInteractionConstraint_test::checkRigid3fFixForBackwa " \n" " \n" " \n" - " \n" + " \n" " \n" ; Node::SPtr root = SceneLoaderXML::loadFromMemory ("testscene", @@ -172,12 +172,12 @@ void BilateralInteractionConstraint_test::checkRigid3fFixForBackwa template<> -void BilateralInteractionConstraint_test::init_Vec3Setup() +void BilateralInteractionLangrangianConstraint_test::init_Vec3Setup() { /// Load the scene //TODO(dmarchal): This general load should be updated... there is no reason to load // a scene independently of the data template to use. - std::string sceneName = "BilateralInteractionConstraint.scn"; + std::string sceneName = "BilateralInteractionLangrangianConstraint.scn"; std::string fileName = std::string(SOFATEST_SCENES_DIR) + "/" + sceneName; root = sofa::simulation::getSimulation()->load(fileName.c_str()).get(); @@ -199,7 +199,7 @@ void BilateralInteractionConstraint_test::init_Vec3Setup() } template<> -bool BilateralInteractionConstraint_test::test_Vec3ConstrainedPositions() +bool BilateralInteractionLangrangianConstraint_test::test_Vec3ConstrainedPositions() { std::vector meca; root->get(&meca,std::string("mecaConstraint"),root->SearchDown); @@ -240,27 +240,27 @@ typedef Types DataTypes; // the types to instanciate. // Test suite for all the instanciations -TYPED_TEST_SUITE(BilateralInteractionConstraint_test, DataTypes); +TYPED_TEST_SUITE(BilateralInteractionLangrangianConstraint_test, DataTypes); //TODO(dmarchal): Needs a serious refactor !!! -TYPED_TEST( BilateralInteractionConstraint_test , checkVec3ConstrainedPositions ) +TYPED_TEST( BilateralInteractionLangrangianConstraint_test , checkVec3ConstrainedPositions ) { this->init_Vec3Setup(); ASSERT_TRUE( this->test_Vec3ConstrainedPositions() ); } -TYPED_TEST( BilateralInteractionConstraint_test , attributesTests ) +TYPED_TEST( BilateralInteractionLangrangianConstraint_test , attributesTests ) { ASSERT_NO_THROW( this->attributesTests() ); } -TYPED_TEST( BilateralInteractionConstraint_test , checkMstateRequiredAssumption ) +TYPED_TEST( BilateralInteractionLangrangianConstraint_test , checkMstateRequiredAssumption ) { ASSERT_NO_THROW( this->checkMstateRequiredAssumption() ); } -TYPED_TEST( BilateralInteractionConstraint_test , checkRigid3fFixForBackwardCompatibility) +TYPED_TEST( BilateralInteractionLangrangianConstraint_test , checkRigid3fFixForBackwardCompatibility) { ASSERT_NO_THROW( this->checkRigid3fFixForBackwardCompatibility() ); } diff --git a/Sofa/Component/Constraint/Lagrangian/Model/tests/scenes_test/BilateralInteractionConstraint.scn b/Sofa/Component/Constraint/Lagrangian/Model/tests/scenes_test/BilateralInteractionConstraint.scn index 6bcda80a448..d2cb769cbb7 100644 --- a/Sofa/Component/Constraint/Lagrangian/Model/tests/scenes_test/BilateralInteractionConstraint.scn +++ b/Sofa/Component/Constraint/Lagrangian/Model/tests/scenes_test/BilateralInteractionConstraint.scn @@ -33,5 +33,5 @@ - + diff --git a/Sofa/Component/Constraint/Projective/CMakeLists.txt b/Sofa/Component/Constraint/Projective/CMakeLists.txt index fcb8797553f..a6f2a49cda8 100644 --- a/Sofa/Component/Constraint/Projective/CMakeLists.txt +++ b/Sofa/Component/Constraint/Projective/CMakeLists.txt @@ -45,30 +45,72 @@ set(HEADER_FILES ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/ProjectToPointConstraint.inl ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/ProjectDirectionConstraint.h ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/ProjectDirectionConstraint.inl + + ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/AffineMovementProjectiveConstraint.h + ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/AffineMovementProjectiveConstraint.inl + ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/AttachProjectiveConstraint.h + ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/AttachProjectiveConstraint.inl + ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/FixedProjectiveConstraint.h + ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/FixedProjectiveConstraint.inl + ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/FixedPlaneProjectiveConstraint.h + ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/FixedPlaneProjectiveConstraint.inl + ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/FixedRotationProjectiveConstraint.h + ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/FixedRotationProjectiveConstraint.inl + ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/FixedTranslationProjectiveConstraint.h + ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/FixedTranslationProjectiveConstraint.inl + ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/HermiteSplineProjectiveConstraint.h + ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/HermiteSplineProjectiveConstraint.inl + ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/LinearMovementProjectiveConstraint.h + ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/LinearMovementProjectiveConstraint.inl + ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/LinearVelocityProjectiveConstraint.h + ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/LinearVelocityProjectiveConstraint.inl + ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/OscillatorProjectiveConstraint.h + ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/OscillatorProjectiveConstraint.inl + ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/ParabolicProjectiveConstraint.h + ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/ParabolicProjectiveConstraint.inl + ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/PartialFixedProjectiveConstraint.h + ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/PartialFixedProjectiveConstraint.inl + ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/PartialLinearMovementProjectiveConstraint.h + ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/PartialLinearMovementProjectiveConstraint.inl + ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/PatchTestMovementProjectiveConstraint.h + ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/PatchTestMovementProjectiveConstraint.inl + ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/PositionBasedDynamicsProjectiveConstraint.h + ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/PositionBasedDynamicsProjectiveConstraint.inl + ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/SkeletalMotionProjectiveConstraint.h + ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/SkeletalMotionProjectiveConstraint.inl + ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/LineProjectiveConstraint.h + ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/LineProjectiveConstraint.inl + ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/PlaneProjectiveConstraint.h + ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/PlaneProjectiveConstraint.inl + ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/PointProjectiveConstraint.h + ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/PointProjectiveConstraint.inl + ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/DirectionProjectiveConstraint.h + ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/DirectionProjectiveConstraint.inl + ) set(SOURCE_FILES ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/init.cpp - ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/AffineMovementConstraint.cpp - ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/AttachConstraint.cpp - ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/FixedConstraint.cpp - ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/FixedPlaneConstraint.cpp - ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/FixedRotationConstraint.cpp - ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/FixedTranslationConstraint.cpp - ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/HermiteSplineConstraint.cpp - ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/LinearMovementConstraint.cpp - ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/LinearVelocityConstraint.cpp - ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/OscillatorConstraint.cpp - ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/ParabolicConstraint.cpp - ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/PartialFixedConstraint.cpp - ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/PartialLinearMovementConstraint.cpp - ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/PatchTestMovementConstraint.cpp - ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/PositionBasedDynamicsConstraint.cpp - ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/SkeletalMotionConstraint.cpp - ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/ProjectToLineConstraint.cpp - ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/ProjectToPlaneConstraint.cpp - ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/ProjectToPointConstraint.cpp - ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/ProjectDirectionConstraint.cpp + ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/AffineMovementProjectiveConstraint.cpp + ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/AttachProjectiveConstraint.cpp + ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/FixedProjectiveConstraint.cpp + ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/FixedPlaneProjectiveConstraint.cpp + ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/FixedRotationProjectiveConstraint.cpp + ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/FixedTranslationProjectiveConstraint.cpp + ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/HermiteSplineProjectiveConstraint.cpp + ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/LinearMovementProjectiveConstraint.cpp + ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/LinearVelocityProjectiveConstraint.cpp + ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/OscillatorProjectiveConstraint.cpp + ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/ParabolicProjectiveConstraint.cpp + ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/PartialFixedProjectiveConstraint.cpp + ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/PartialLinearMovementProjectiveConstraint.cpp + ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/PatchTestMovementProjectiveConstraint.cpp + ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/PositionBasedDynamicsProjectiveConstraint.cpp + ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/SkeletalMotionProjectiveConstraint.cpp + ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/LineProjectiveConstraint.cpp + ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/PlaneProjectiveConstraint.cpp + ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/PointProjectiveConstraint.cpp + ${SOFACOMPONENTCONSTRAINTPROJECTIVE_SOURCE_DIR}/DirectionProjectiveConstraint.cpp ) sofa_find_package(Sofa.Simulation.Core REQUIRED) diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/AffineMovementConstraint.h b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/AffineMovementConstraint.h index 74f2cae555c..d17e1e9364d 100644 --- a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/AffineMovementConstraint.h +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/AffineMovementConstraint.h @@ -20,146 +20,13 @@ * Contact information: contact@sofa-framework.org * ******************************************************************************/ #pragma once -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include -namespace sofa::component::constraint::projective -{ - -template -class AffineMovementConstraintInternalData -{ -}; +SOFA_DEPRECATED_HEADER("v24.06", "v25.06", "sofa/component/constraint/projective/AffineMovementConstraint.h") -/** - Impose a motion to all the boundary points of a mesh. The motion of the 4 corners are given in the data m_cornerMovements and the movements of the edge points are computed by linear interpolation. -*/ -template -class AffineMovementConstraint : public core::behavior::ProjectiveConstraintSet +namespace sofa::component::constraint::projective { -public: - SOFA_CLASS(SOFA_TEMPLATE(AffineMovementConstraint,TDataTypes), - SOFA_TEMPLATE(sofa::core::behavior::ProjectiveConstraintSet, TDataTypes)); - - using Index = sofa::Index; - typedef TDataTypes DataTypes; - typedef typename DataTypes::VecCoord VecCoord; - typedef typename DataTypes::VecDeriv VecDeriv; - typedef typename DataTypes::Coord Coord; - typedef typename DataTypes::CPos CPos; - typedef typename DataTypes::Deriv Deriv; - typedef typename DataTypes::Real Real; - typedef Data DataVecCoord; - typedef Data DataVecDeriv; - typedef type::vector SetIndexArray; - typedef sofa::core::topology::TopologySubsetIndices SetIndex; - typedef type::Quat Quat; - typedef type::Vec3 Vec3; - - typedef typename DataTypes::MatrixDeriv MatrixDeriv; - typedef core::objectmodel::Data DataMatrixDeriv; - - static const auto CoordSize = Coord::total_size; - typedef type::Mat<3,3,Real> RotationMatrix; - -protected: - AffineMovementConstraintInternalData *data; - friend class AffineMovementConstraintInternalData; - -public : - /// indices of the DOFs of the mesh - SetIndex m_meshIndices; - /// indices of the DOFs the constraint is applied to - SetIndex m_indices; - /// data begin time when the constraint is applied - Data m_beginConstraintTime; - /// data end time when the constraint is applied - Data m_endConstraintTime; - /// Rotation Matrix of affine transformation - Data m_rotation; - /// Quaternion of affine transformation (for rigid) - Data m_quaternion; - /// Translation Matrix of affine transformation - Data m_translation; - /// Draw constrained points - Data m_drawConstrainedPoints; - /// initial constrained DOFs position - VecCoord x0; - /// final constrained DOFs position - VecCoord xf; - /// initial mesh DOFs position - VecCoord meshPointsX0; - /// final mesh DOFs position - VecCoord meshPointsXf; - - /// Link to be set to the topology container in the component graph. - SingleLink, sofa::core::topology::BaseMeshTopology, BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_topology; - -protected: - AffineMovementConstraint(); - - virtual ~AffineMovementConstraint(); - -public: - //Add or clear constraints - void clearConstraints(); - void addConstraint(Index index); - void removeConstraint(Index index); - - /// -- Constraint interface - void init() override; - - /// Cancel the possible forces - void projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& resData) override; - /// Cancel the possible velocities - void projectVelocity(const core::MechanicalParams* mparams, DataVecDeriv& vData) override; - /// Apply the computed movements to the border mesh points between beginConstraintTime and endConstraintTime - void projectPosition(const core::MechanicalParams* mparams, DataVecCoord& xData) override; - - void projectJacobianMatrix(const core::MechanicalParams* /*mparams*/, DataMatrixDeriv& /* cData */) override - { - msg_error() << "projectJacobianMatrix not implemented"; - } - - /// Compute the theoretical final positions - void getFinalPositions (VecCoord& finalPos, DataVecCoord& xData); - - // Implement projectMatrix for assembled solver of compliant - void projectMatrix( sofa::linearalgebra::BaseMatrix* /*M*/, unsigned /*offset*/ ) override; - - /// Draw the constrained points (= border mesh points) - void draw(const core::visual::VisualParams* vparams) override; - -protected: - - void projectResponseImpl(VecDeriv& dx); - -private: - - /// Initialize initial positions - void initializeInitialPositions (const SetIndexArray & indices, DataVecCoord& xData, VecCoord& x0); - - /// Initialize final positions - void initializeFinalPositions (const SetIndexArray & indices, DataVecCoord& xData, VecCoord& x0 , VecCoord& xf); - - /// Apply affine transform - void transform(const SetIndexArray & indices, VecCoord& x0 , VecCoord& xf); -}; - -#if !defined(SOFABOUNDARYCONDITION_AFFINEMOVEMENTCONSTRAINT_CPP) -extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API AffineMovementConstraint; -extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API AffineMovementConstraint; -#endif //SOFABOUNDARYCONDITION_AFFINEMOVEMENTCONSTRAINT_CPP - -} // namespace sofa::component::constraint::projective +template +using AffineMovementConstraint SOFA_ATTRIBUTE_DEPRECATED("v24.06 ", "v25.06", "AffineMovementConstraint has been renamed to AffineMovementProjectiveConstraint") = AffineMovementProjectiveConstraint; +} diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/AffineMovementConstraint.inl b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/AffineMovementConstraint.inl index b6434701c86..5230ec8026b 100644 --- a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/AffineMovementConstraint.inl +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/AffineMovementConstraint.inl @@ -21,290 +21,6 @@ ******************************************************************************/ #pragma once -#include -#include -#include -#include -#include -#include +#include -#include - -namespace sofa::component::constraint::projective -{ - -template -AffineMovementConstraint::AffineMovementConstraint() - : core::behavior::ProjectiveConstraintSet(nullptr) - , data(new AffineMovementConstraintInternalData) - , m_meshIndices( initData(&m_meshIndices,"meshIndices","Indices of the mesh") ) - , m_indices( initData(&m_indices,"indices","Indices of the constrained points") ) - , m_beginConstraintTime( initData(&m_beginConstraintTime,"beginConstraintTime","Begin time of the bilinear constraint") ) - , m_endConstraintTime( initData(&m_endConstraintTime,"endConstraintTime","End time of the bilinear constraint") ) - , m_rotation( initData(&m_rotation,"rotation","rotation applied to border points") ) - , m_quaternion( initData(&m_quaternion,"quaternion","quaternion applied to border points") ) - , m_translation( initData(&m_translation,"translation","translation applied to border points") ) - , m_drawConstrainedPoints( initData(&m_drawConstrainedPoints,"drawConstrainedPoints","draw constrained points") ) - , l_topology(initLink("topology", "link to the topology container")) -{ - if(!m_beginConstraintTime.isSet()) - m_beginConstraintTime = 0; - if(!m_endConstraintTime.isSet()) - m_endConstraintTime = 20; -} - - - -template -AffineMovementConstraint::~AffineMovementConstraint() -{ - -} - -template -void AffineMovementConstraint::clearConstraints() -{ - m_indices.beginEdit()->clear(); - m_indices.endEdit(); -} - -template -void AffineMovementConstraint::addConstraint(Index index) -{ - m_indices.beginEdit()->push_back(index); - m_indices.endEdit(); -} - -template -void AffineMovementConstraint::removeConstraint(Index index) -{ - removeValue(*m_indices.beginEdit(),index); - m_indices.endEdit(); -} - -// -- Constraint interface - - -template -void AffineMovementConstraint::init() -{ - this->core::behavior::ProjectiveConstraintSet::init(); - - if (l_topology.empty()) - { - msg_info() << "link to Topology container should be set to ensure right behavior. First Topology found in current context will be used."; - l_topology.set(this->getContext()->getMeshTopologyLink()); - } - - if (sofa::core::topology::BaseMeshTopology* _topology = l_topology.get()) - { - msg_info() << "Topology path used: '" << l_topology.getLinkedPath() << "'"; - - // Initialize topological changes support - m_indices.createTopologyHandler(_topology); - } - else - { - msg_info() << "No topology component found at path: " << l_topology.getLinkedPath() << ", nor in current context: " << this->getContext()->name; - } - - const SetIndexArray & indices = m_indices.getValue(); - - auto maxIndex=this->mstate->getSize(); - for (Size i=0; i= maxIndex) - { - msg_error() << "Index " << index << " not valid!"; - removeConstraint(index); - } - } - -} - -template -void AffineMovementConstraint::projectResponseImpl(VecDeriv& dx) -{ - const SetIndexArray & indices = m_indices.getValue(); - for (size_t i = 0; i< indices.size(); ++i) - { - dx[indices[i]]=Deriv(); - } -} - -template -void AffineMovementConstraint::projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& resData) -{ - SOFA_UNUSED(mparams); - helper::WriteAccessor res = resData; - projectResponseImpl(res.wref()); -} - - - -template -void AffineMovementConstraint::projectVelocity(const core::MechanicalParams* mparams, DataVecDeriv& vData) -{ - SOFA_UNUSED(mparams); - helper::WriteAccessor res = vData; - projectResponseImpl(res.wref()); -} - -template -void AffineMovementConstraint::projectPosition(const core::MechanicalParams* /*mparams*/, DataVecCoord& xData) -{ - helper::WriteAccessor x = xData; - const SetIndexArray & indices = m_indices.getValue(); - - // Time - SReal beginTime = m_beginConstraintTime.getValue(); - SReal endTime = m_endConstraintTime.getValue(); - SReal totalTime = endTime - beginTime; - - //initialize initial mesh Dofs positions, if it's not done - if(meshPointsX0.size()==0) - this->initializeInitialPositions(m_meshIndices.getValue(),xData,meshPointsX0); - - //initialize final mesh Dofs positions, if it's not done - if(meshPointsXf.size()==0) - this->initializeFinalPositions(m_meshIndices.getValue(),xData, meshPointsX0, meshPointsXf); - - //initialize initial constrained Dofs positions, if it's not done - if(x0.size() == 0) - this->initializeInitialPositions(indices,xData,x0); - - //initialize final constrained Dofs positions, if it's not done - if (xf.size() == 0) - this->initializeFinalPositions(indices,xData,x0,xf); - // Update the intermediate Dofs positions computed by linear interpolation - SReal time = sofa::core::objectmodel::basecontext::getTime(this->getContext()->getRootContext()); - if( time > beginTime && time <= endTime && totalTime > 0) - { - for (auto index : indices) - { - DataTypes::setCPos( x[index], - ((DataTypes::getCPos(xf[index])-DataTypes::getCPos(x0[index]))*time + - (DataTypes::getCPos(x0[index])*endTime - DataTypes::getCPos(xf[index])*beginTime))/totalTime); - } - } - else if (time > endTime) - { - for (auto index : indices) - { - x[index] = xf[index]; - } - } -} - -template -void AffineMovementConstraint::projectMatrix( sofa::linearalgebra::BaseMatrix* M, unsigned /*offset*/ ) -{ - // clears the rows and columns associated with constrained particles - const unsigned blockSize = DataTypes::deriv_total_size; - - for (const auto id : m_indices.getValue()) - { - M->clearRowsCols( id * blockSize, (id+1) * blockSize ); - } -} - -template -void AffineMovementConstraint::getFinalPositions( VecCoord& finalPos,DataVecCoord& xData) -{ - // Indices of mesh points - const SetIndexArray & meshIndices = m_meshIndices.getValue(); - - // Initialize final positions - if(meshPointsXf.size()==0) - {this->initializeFinalPositions(meshIndices,xData,meshPointsX0,meshPointsXf);} - - // Set final positions - finalPos.resize(meshIndices.size()); - for (size_t i=0; i < meshIndices.size() ; ++i) - { - finalPos[meshIndices[i]] = meshPointsXf[meshIndices[i]]; - } -} - -template -void AffineMovementConstraint::initializeInitialPositions (const SetIndexArray & indices, DataVecCoord& xData, VecCoord& x0) -{ - helper::WriteAccessor x = xData; - - x0.resize(x.size()); - for (size_t i=0; i < indices.size() ; ++i) - { - x0[indices[i]] = x[indices[i]]; - } -} - - -template <> -void AffineMovementConstraint::transform(const SetIndexArray & indices, - defaulttype::Rigid3Types::VecCoord& x0, - defaulttype::Rigid3Types::VecCoord& xf) -{ - // Get quaternion and translation values - RotationMatrix rotationMat(0); - const Quat quat = m_quaternion.getValue(); - quat.toMatrix(rotationMat); - const Vec3 translation = m_translation.getValue(); - - // Apply transformation - for (size_t i=0; i < indices.size() ; ++i) - { - // Translation - xf[indices[i]].getCenter() = rotationMat*(x0[indices[i]].getCenter()) + translation; - // Rotation - xf[indices[i]].getOrientation() = (quat)+x0[indices[i]].getOrientation(); - } -} - -template -void AffineMovementConstraint::transform(const SetIndexArray & indices, VecCoord& x0, VecCoord& xf) -{ - Vec3 translation = m_translation.getValue(); - - for (size_t i=0; i < indices.size() ; ++i) - { - DataTypes::setCPos(xf[indices[i]], (m_rotation.getValue())*DataTypes::getCPos(x0[indices[i]]) + translation); - } -} - - -template -void AffineMovementConstraint::initializeFinalPositions (const SetIndexArray & indices, DataVecCoord& xData, VecCoord& x0, VecCoord& xf) -{ - helper::WriteAccessor x = xData; - - xf.resize(x.size()); - - // if the positions were not initialized - if(x0.size() == 0) - this->initializeInitialPositions(indices,xData,x0); - - transform(indices,x0,xf); -} - -template -void AffineMovementConstraint::draw(const core::visual::VisualParams* vparams) -{ - const SetIndexArray & indices = m_indices.getValue(); - const VecCoord& x = this->mstate->read(core::ConstVecCoordId::position())->getValue(); - Vec3 point; - - if(m_drawConstrainedPoints.getValue()) - { - std::vector< Vec3 > points; - for( auto& index : indices ) - { - point = DataTypes::getCPos(x[index]); - points.push_back(point); - } - constexpr sofa::type::RGBAColor color(1,0.5,0.5,1); - vparams->drawTool()->drawPoints(points, 10, color); - } -} - -} // namespace sofa::component::constraint::projective +SOFA_DEPRECATED_HEADER("v24.06", "v25.06", "sofa/component/constraint/projective/AffineMovementConstraint.inl") diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/AffineMovementConstraint.cpp b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/AffineMovementProjectiveConstraint.cpp similarity index 78% rename from Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/AffineMovementConstraint.cpp rename to Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/AffineMovementProjectiveConstraint.cpp index 433bad30dff..1e4887d01e4 100644 --- a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/AffineMovementConstraint.cpp +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/AffineMovementProjectiveConstraint.cpp @@ -19,24 +19,25 @@ * * * Contact information: contact@sofa-framework.org * ******************************************************************************/ -#define SOFABOUNDARYCONDITION_AFFINEMOVEMENTCONSTRAINT_CPP +#define SOFABOUNDARYCONDITION_AFFINEMOVEMENTPROJECTIVECONSTRAINT_CPP #include #include #include #include -#include +#include namespace sofa::component::constraint::projective { -int AffineMovementConstraintRegister = core::RegisterObject("Constraint the movement by a rigid transform.") - .add< AffineMovementConstraint >() - .add< AffineMovementConstraint >() +int AffineMovementProjectiveConstraintRegister = core::RegisterObject("Constraint the movement by a rigid transform.") + .add< AffineMovementProjectiveConstraint >() + .add< AffineMovementProjectiveConstraint >() + .addAlias("AffineMovementConstraint") ; -template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API AffineMovementConstraint; -template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API AffineMovementConstraint; +template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API AffineMovementProjectiveConstraint; +template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API AffineMovementProjectiveConstraint; } // namespace sofa::component::constraint::projective diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/AffineMovementProjectiveConstraint.h b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/AffineMovementProjectiveConstraint.h new file mode 100644 index 00000000000..74ca2823e84 --- /dev/null +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/AffineMovementProjectiveConstraint.h @@ -0,0 +1,165 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace sofa::component::constraint::projective +{ + +template +class AffineMovementProjectiveConstraintInternalData +{ +}; + +/** + Impose a motion to all the boundary points of a mesh. The motion of the 4 corners are given in the data m_cornerMovements and the movements of the edge points are computed by linear interpolation. +*/ +template +class AffineMovementProjectiveConstraint : public core::behavior::ProjectiveConstraintSet +{ +public: + SOFA_CLASS(SOFA_TEMPLATE(AffineMovementProjectiveConstraint,TDataTypes), + SOFA_TEMPLATE(sofa::core::behavior::ProjectiveConstraintSet, TDataTypes)); + + using Index = sofa::Index; + typedef TDataTypes DataTypes; + typedef typename DataTypes::VecCoord VecCoord; + typedef typename DataTypes::VecDeriv VecDeriv; + typedef typename DataTypes::Coord Coord; + typedef typename DataTypes::CPos CPos; + typedef typename DataTypes::Deriv Deriv; + typedef typename DataTypes::Real Real; + typedef Data DataVecCoord; + typedef Data DataVecDeriv; + typedef type::vector SetIndexArray; + typedef sofa::core::topology::TopologySubsetIndices SetIndex; + typedef type::Quat Quat; + typedef type::Vec3 Vec3; + + typedef typename DataTypes::MatrixDeriv MatrixDeriv; + typedef core::objectmodel::Data DataMatrixDeriv; + + static const auto CoordSize = Coord::total_size; + typedef type::Mat<3,3,Real> RotationMatrix; + +protected: + AffineMovementProjectiveConstraintInternalData *data; + friend class AffineMovementProjectiveConstraintInternalData; + +public : + /// indices of the DOFs of the mesh + SetIndex m_meshIndices; + /// indices of the DOFs the constraint is applied to + SetIndex m_indices; + /// data begin time when the constraint is applied + Data m_beginConstraintTime; + /// data end time when the constraint is applied + Data m_endConstraintTime; + /// Rotation Matrix of affine transformation + Data m_rotation; + /// Quaternion of affine transformation (for rigid) + Data m_quaternion; + /// Translation Matrix of affine transformation + Data m_translation; + /// Draw constrained points + Data m_drawConstrainedPoints; + /// initial constrained DOFs position + VecCoord x0; + /// final constrained DOFs position + VecCoord xf; + /// initial mesh DOFs position + VecCoord meshPointsX0; + /// final mesh DOFs position + VecCoord meshPointsXf; + + /// Link to be set to the topology container in the component graph. + SingleLink, sofa::core::topology::BaseMeshTopology, BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_topology; + +protected: + AffineMovementProjectiveConstraint(); + + virtual ~AffineMovementProjectiveConstraint(); + +public: + //Add or clear constraints + void clearConstraints(); + void addConstraint(Index index); + void removeConstraint(Index index); + + /// -- Constraint interface + void init() override; + + /// Cancel the possible forces + void projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& resData) override; + /// Cancel the possible velocities + void projectVelocity(const core::MechanicalParams* mparams, DataVecDeriv& vData) override; + /// Apply the computed movements to the border mesh points between beginConstraintTime and endConstraintTime + void projectPosition(const core::MechanicalParams* mparams, DataVecCoord& xData) override; + + void projectJacobianMatrix(const core::MechanicalParams* /*mparams*/, DataMatrixDeriv& /* cData */) override + { + msg_error() << "projectJacobianMatrix not implemented"; + } + + /// Compute the theoretical final positions + void getFinalPositions (VecCoord& finalPos, DataVecCoord& xData); + + // Implement projectMatrix for assembled solver of compliant + void projectMatrix( sofa::linearalgebra::BaseMatrix* /*M*/, unsigned /*offset*/ ) override; + + /// Draw the constrained points (= border mesh points) + void draw(const core::visual::VisualParams* vparams) override; + +protected: + + void projectResponseImpl(VecDeriv& dx); + +private: + + /// Initialize initial positions + void initializeInitialPositions (const SetIndexArray & indices, DataVecCoord& xData, VecCoord& x0); + + /// Initialize final positions + void initializeFinalPositions (const SetIndexArray & indices, DataVecCoord& xData, VecCoord& x0 , VecCoord& xf); + + /// Apply affine transform + void transform(const SetIndexArray & indices, VecCoord& x0 , VecCoord& xf); +}; + +#if !defined(SOFABOUNDARYCONDITION_AFFINEMOVEMENTPROJECTIVECONSTRAINT_CPP) +extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API AffineMovementProjectiveConstraint; +extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API AffineMovementProjectiveConstraint; +#endif //SOFABOUNDARYCONDITION_AFFINEMOVEMENTPROJECTIVECONSTRAINT_CPP + +} // namespace sofa::component::constraint::projective diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/AffineMovementProjectiveConstraint.inl b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/AffineMovementProjectiveConstraint.inl new file mode 100644 index 00000000000..ee7a56d83ac --- /dev/null +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/AffineMovementProjectiveConstraint.inl @@ -0,0 +1,310 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once + +#include +#include +#include +#include +#include +#include + +#include + +namespace sofa::component::constraint::projective +{ + +template +AffineMovementProjectiveConstraint::AffineMovementProjectiveConstraint() + : core::behavior::ProjectiveConstraintSet(nullptr) + , data(new AffineMovementProjectiveConstraintInternalData) + , m_meshIndices( initData(&m_meshIndices,"meshIndices","Indices of the mesh") ) + , m_indices( initData(&m_indices,"indices","Indices of the constrained points") ) + , m_beginConstraintTime( initData(&m_beginConstraintTime,"beginConstraintTime","Begin time of the bilinear constraint") ) + , m_endConstraintTime( initData(&m_endConstraintTime,"endConstraintTime","End time of the bilinear constraint") ) + , m_rotation( initData(&m_rotation,"rotation","rotation applied to border points") ) + , m_quaternion( initData(&m_quaternion,"quaternion","quaternion applied to border points") ) + , m_translation( initData(&m_translation,"translation","translation applied to border points") ) + , m_drawConstrainedPoints( initData(&m_drawConstrainedPoints,"drawConstrainedPoints","draw constrained points") ) + , l_topology(initLink("topology", "link to the topology container")) +{ + if(!m_beginConstraintTime.isSet()) + m_beginConstraintTime = 0; + if(!m_endConstraintTime.isSet()) + m_endConstraintTime = 20; +} + + + +template +AffineMovementProjectiveConstraint::~AffineMovementProjectiveConstraint() +{ + +} + +template +void AffineMovementProjectiveConstraint::clearConstraints() +{ + m_indices.beginEdit()->clear(); + m_indices.endEdit(); +} + +template +void AffineMovementProjectiveConstraint::addConstraint(Index index) +{ + m_indices.beginEdit()->push_back(index); + m_indices.endEdit(); +} + +template +void AffineMovementProjectiveConstraint::removeConstraint(Index index) +{ + removeValue(*m_indices.beginEdit(),index); + m_indices.endEdit(); +} + +// -- Constraint interface + + +template +void AffineMovementProjectiveConstraint::init() +{ + this->core::behavior::ProjectiveConstraintSet::init(); + + if (l_topology.empty()) + { + msg_info() << "link to Topology container should be set to ensure right behavior. First Topology found in current context will be used."; + l_topology.set(this->getContext()->getMeshTopologyLink()); + } + + if (sofa::core::topology::BaseMeshTopology* _topology = l_topology.get()) + { + msg_info() << "Topology path used: '" << l_topology.getLinkedPath() << "'"; + + // Initialize topological changes support + m_indices.createTopologyHandler(_topology); + } + else + { + msg_info() << "No topology component found at path: " << l_topology.getLinkedPath() << ", nor in current context: " << this->getContext()->name; + } + + const SetIndexArray & indices = m_indices.getValue(); + + auto maxIndex=this->mstate->getSize(); + for (Size i=0; i= maxIndex) + { + msg_error() << "Index " << index << " not valid!"; + removeConstraint(index); + } + } + +} + +template +void AffineMovementProjectiveConstraint::projectResponseImpl(VecDeriv& dx) +{ + const SetIndexArray & indices = m_indices.getValue(); + for (size_t i = 0; i< indices.size(); ++i) + { + dx[indices[i]]=Deriv(); + } +} + +template +void AffineMovementProjectiveConstraint::projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& resData) +{ + SOFA_UNUSED(mparams); + helper::WriteAccessor res = resData; + projectResponseImpl(res.wref()); +} + + + +template +void AffineMovementProjectiveConstraint::projectVelocity(const core::MechanicalParams* mparams, DataVecDeriv& vData) +{ + SOFA_UNUSED(mparams); + helper::WriteAccessor res = vData; + projectResponseImpl(res.wref()); +} + +template +void AffineMovementProjectiveConstraint::projectPosition(const core::MechanicalParams* /*mparams*/, DataVecCoord& xData) +{ + helper::WriteAccessor x = xData; + const SetIndexArray & indices = m_indices.getValue(); + + // Time + SReal beginTime = m_beginConstraintTime.getValue(); + SReal endTime = m_endConstraintTime.getValue(); + SReal totalTime = endTime - beginTime; + + //initialize initial mesh Dofs positions, if it's not done + if(meshPointsX0.size()==0) + this->initializeInitialPositions(m_meshIndices.getValue(),xData,meshPointsX0); + + //initialize final mesh Dofs positions, if it's not done + if(meshPointsXf.size()==0) + this->initializeFinalPositions(m_meshIndices.getValue(),xData, meshPointsX0, meshPointsXf); + + //initialize initial constrained Dofs positions, if it's not done + if(x0.size() == 0) + this->initializeInitialPositions(indices,xData,x0); + + //initialize final constrained Dofs positions, if it's not done + if (xf.size() == 0) + this->initializeFinalPositions(indices,xData,x0,xf); + // Update the intermediate Dofs positions computed by linear interpolation + SReal time = sofa::core::objectmodel::basecontext::getTime(this->getContext()->getRootContext()); + if( time > beginTime && time <= endTime && totalTime > 0) + { + for (auto index : indices) + { + DataTypes::setCPos( x[index], + ((DataTypes::getCPos(xf[index])-DataTypes::getCPos(x0[index]))*time + + (DataTypes::getCPos(x0[index])*endTime - DataTypes::getCPos(xf[index])*beginTime))/totalTime); + } + } + else if (time > endTime) + { + for (auto index : indices) + { + x[index] = xf[index]; + } + } +} + +template +void AffineMovementProjectiveConstraint::projectMatrix( sofa::linearalgebra::BaseMatrix* M, unsigned /*offset*/ ) +{ + // clears the rows and columns associated with constrained particles + const unsigned blockSize = DataTypes::deriv_total_size; + + for (const auto id : m_indices.getValue()) + { + M->clearRowsCols( id * blockSize, (id+1) * blockSize ); + } +} + +template +void AffineMovementProjectiveConstraint::getFinalPositions( VecCoord& finalPos,DataVecCoord& xData) +{ + // Indices of mesh points + const SetIndexArray & meshIndices = m_meshIndices.getValue(); + + // Initialize final positions + if(meshPointsXf.size()==0) + {this->initializeFinalPositions(meshIndices,xData,meshPointsX0,meshPointsXf);} + + // Set final positions + finalPos.resize(meshIndices.size()); + for (size_t i=0; i < meshIndices.size() ; ++i) + { + finalPos[meshIndices[i]] = meshPointsXf[meshIndices[i]]; + } +} + +template +void AffineMovementProjectiveConstraint::initializeInitialPositions (const SetIndexArray & indices, DataVecCoord& xData, VecCoord& x0) +{ + helper::WriteAccessor x = xData; + + x0.resize(x.size()); + for (size_t i=0; i < indices.size() ; ++i) + { + x0[indices[i]] = x[indices[i]]; + } +} + + +template <> +void AffineMovementProjectiveConstraint::transform(const SetIndexArray & indices, + defaulttype::Rigid3Types::VecCoord& x0, + defaulttype::Rigid3Types::VecCoord& xf) +{ + // Get quaternion and translation values + RotationMatrix rotationMat(0); + const Quat quat = m_quaternion.getValue(); + quat.toMatrix(rotationMat); + const Vec3 translation = m_translation.getValue(); + + // Apply transformation + for (size_t i=0; i < indices.size() ; ++i) + { + // Translation + xf[indices[i]].getCenter() = rotationMat*(x0[indices[i]].getCenter()) + translation; + // Rotation + xf[indices[i]].getOrientation() = (quat)+x0[indices[i]].getOrientation(); + } +} + +template +void AffineMovementProjectiveConstraint::transform(const SetIndexArray & indices, VecCoord& x0, VecCoord& xf) +{ + Vec3 translation = m_translation.getValue(); + + for (size_t i=0; i < indices.size() ; ++i) + { + DataTypes::setCPos(xf[indices[i]], (m_rotation.getValue())*DataTypes::getCPos(x0[indices[i]]) + translation); + } +} + + +template +void AffineMovementProjectiveConstraint::initializeFinalPositions (const SetIndexArray & indices, DataVecCoord& xData, VecCoord& x0, VecCoord& xf) +{ + helper::WriteAccessor x = xData; + + xf.resize(x.size()); + + // if the positions were not initialized + if(x0.size() == 0) + this->initializeInitialPositions(indices,xData,x0); + + transform(indices,x0,xf); +} + +template +void AffineMovementProjectiveConstraint::draw(const core::visual::VisualParams* vparams) +{ + const SetIndexArray & indices = m_indices.getValue(); + const VecCoord& x = this->mstate->read(core::ConstVecCoordId::position())->getValue(); + Vec3 point; + + if(m_drawConstrainedPoints.getValue()) + { + std::vector< Vec3 > points; + for( auto& index : indices ) + { + point = DataTypes::getCPos(x[index]); + points.push_back(point); + } + constexpr sofa::type::RGBAColor color(1,0.5,0.5,1); + vparams->drawTool()->drawPoints(points, 10, color); + } +} + +} // namespace sofa::component::constraint::projective diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/AttachConstraint.h b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/AttachConstraint.h index 14eff971abb..692f1178c2d 100644 --- a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/AttachConstraint.h +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/AttachConstraint.h @@ -20,105 +20,13 @@ * Contact information: contact@sofa-framework.org * ******************************************************************************/ #pragma once -#include -#include -#include +#include -namespace sofa::component::constraint::projective -{ +SOFA_DEPRECATED_HEADER("v24.06", "v25.06", "sofa/component/constraint/projective/AttachProjectiveConstraint.h") -/** Attach given pair of particles, projecting the positions of the second particles to the first ones. -*/ -template -class AttachConstraint : public core::behavior::PairInteractionProjectiveConstraintSet +namespace sofa::component::constraint::projective { -public: - SOFA_CLASS(SOFA_TEMPLATE(AttachConstraint,DataTypes),SOFA_TEMPLATE(sofa::core::behavior::PairInteractionProjectiveConstraintSet,DataTypes)); - - typedef typename DataTypes::VecCoord VecCoord; - typedef typename DataTypes::VecDeriv VecDeriv; - typedef typename DataTypes::Coord Coord; - typedef typename DataTypes::Deriv Deriv; - typedef typename DataTypes::Real Real; - - typedef core::objectmodel::Data DataVecCoord; - typedef core::objectmodel::Data DataVecDeriv; - - typedef type::vector SetIndexArray; - typedef sofa::core::topology::TopologySubsetIndices SetIndex; - -public: - SetIndex f_indices1; ///< Indices of the source points on the first model - SetIndex f_indices2; ///< Indices of the fixed points on the second model - Data f_twoWay; ///< true if forces should be projected back from model2 to model1 - Data f_freeRotations; ///< true to keep rotations free (only used for Rigid DOFs) - Data f_lastFreeRotation; ///< true to keep rotation of the last attached point free (only used for Rigid DOFs) - Data f_restRotations; ///< true to use rest rotations local offsets (only used for Rigid DOFs) - Data f_lastPos; ///< position at which the attach constraint should become inactive - Data f_lastDir; ///< direction from lastPos at which the attach coustraint should become inactive - Data f_clamp; ///< true to clamp particles at lastPos instead of freeing them. - Data f_minDistance; ///< the constraint become inactive if the distance between the points attached is bigger than minDistance. - Data< Real > d_positionFactor; ///< IN: Factor applied to projection of position - Data< Real > d_velocityFactor; ///< IN: Factor applied to projection of velocity - Data< Real > d_responseFactor; ///< IN: Factor applied to projection of force/acceleration - Data< type::vector > d_constraintFactor; ///< Constraint factor per pair of points constrained. 0 -> the constraint is released. 1 -> the constraint is fully constrained - - type::vector activeFlags; - type::vector constraintReleased; - type::vector lastDist; - type::vector> restRotations; - -protected: - AttachConstraint(); - AttachConstraint(core::behavior::MechanicalState *mm1, core::behavior::MechanicalState *mm2); - ~AttachConstraint() override; - -public: - - /// Inherited from Base - void init() override; - void reinit() override; - void draw(const core::visual::VisualParams* vparams) override; - - /// Inherited from Constraint - void projectJacobianMatrix(const core::MechanicalParams* mparams, core::MultiMatrixDerivId cId) override; - void projectResponse(const core::MechanicalParams *mparams, DataVecDeriv& dx1, DataVecDeriv& dx2) override; - void projectVelocity(const core::MechanicalParams *mparams, DataVecDeriv& v1, DataVecDeriv& v2) override; - void projectPosition(const core::MechanicalParams *mparams, DataVecCoord& x1, DataVecCoord& x2) override; - - /// Project the global Mechanical Matrix to constrained space using offset parameter - void applyConstraint(const core::MechanicalParams *mparams, - const sofa::core::behavior::MultiMatrixAccessor* matrix) override; - - /// Project the global Mechanical Vector to constrained space using offset parameter - void applyConstraint(const core::MechanicalParams *mparams, linearalgebra::BaseVector* vector, const sofa::core::behavior::MultiMatrixAccessor* matrix) override; - - virtual void reinitIfChanged(); - - template - static std::string templateName(const T* ptr= nullptr) { - return core::behavior::PairInteractionProjectiveConstraintSet::templateName(ptr); - } - -protected : - const Real getConstraintFactor(const int index); - void doProjectPosition(Coord& x1, Coord& x2, bool freeRotations, unsigned index, Real positionFactor); - void doProjectVelocity(Deriv& x1, Deriv& x2, bool freeRotations, unsigned index, Real velocityFactor); - void doProjectResponse(Deriv& dx1, Deriv& dx2, bool freeRotations, bool twoway, unsigned index, Real responseFactor); - - void calcRestRotations(); - static unsigned int DerivConstrainedSize(bool freeRotations); - -}; - - -#if !defined(SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_ATTACHCONSTRAINT_CPP) -extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API AttachConstraint; -extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API AttachConstraint; -extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API AttachConstraint; -extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API AttachConstraint; -extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API AttachConstraint; -#endif - -} // namespace sofa::component::constraint::projective +template +using AttachConstraint SOFA_ATTRIBUTE_DEPRECATED("v24.06 ", "v25.06", "AttachConstraint has been renamed to AttachProjectiveConstraint") = AttachProjectiveConstraint; +} \ No newline at end of file diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/AttachConstraint.inl b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/AttachConstraint.inl index a77b223d41d..ed5521f875d 100644 --- a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/AttachConstraint.inl +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/AttachConstraint.inl @@ -20,624 +20,7 @@ * Contact information: contact@sofa-framework.org * ******************************************************************************/ #pragma once -#include -#include -#include -#include -#include -#include -namespace sofa::component::constraint::projective -{ +#include -using sofa::simulation::Node ; - -template<> -inline void AttachConstraint::doProjectPosition(Coord& x1, Coord& x2, bool freeRotations, unsigned index, Real positionFactor) -{ - SOFA_UNUSED(positionFactor); - // do nothing if distance between x2 & x1 is bigger than f_minDistance - if (f_minDistance.getValue() != -1.0 && - (x2.getCenter() - x1.getCenter()).norm() > f_minDistance.getValue()) - { - constraintReleased[index] = true; - return; - } - constraintReleased[index] = false; - - x2.getCenter() = x1.getCenter(); - if (!freeRotations) - { - if (!restRotations.empty()) - { - if (index+1 >= lastDist.size() || activeFlags[index+1]) - x2.getOrientation() = x1.getOrientation()*restRotations[index]; - else - { - // gradually set the velocity along the direction axis - const Real fact = -lastDist[index] / (lastDist[index+1]-lastDist[index]); - const sofa::type::Vec3 axis(restRotations[index][0], restRotations[index][1], restRotations[index][2]); - const Real angle = acos(restRotations[index][3])*2; - x2.getOrientation() = x1.getOrientation()*sofa::type::Quat(axis,angle*fact); - } - } - else - x2.getOrientation() = x1.getOrientation(); - } -} - - -template<> -inline void AttachConstraint::doProjectPosition(Coord& x1, Coord& x2, bool freeRotations, unsigned index, Real positionFactor) -{ - SOFA_UNUSED(positionFactor); - // do nothing if distance between x2 & x1 is bigger than f_minDistance - if (f_minDistance.getValue() != -1 && - (x2.getCenter() - x1.getCenter()).norm() > f_minDistance.getValue()) - { - constraintReleased[index] = true; - return; - } - constraintReleased[index] = false; - - x2.getCenter() = x1.getCenter(); - if (!freeRotations) - x2.getOrientation() = x1.getOrientation(); -} - - -template<> -inline void AttachConstraint::doProjectVelocity(Deriv& x1, Deriv& x2, bool freeRotations, unsigned index, Real velocityFactor) -{ - SOFA_UNUSED(velocityFactor); - // do nothing if distance between x2 & x1 is bigger than f_minDistance - if (constraintReleased[index]) - return; - - getVCenter( x2) = getVCenter(x1); - if (!freeRotations) - getVOrientation(x2) = getVOrientation(x1); -} - - -template<> -inline void AttachConstraint::doProjectVelocity(Deriv& x1, Deriv& x2, bool freeRotations, unsigned index, Real velocityFactor) -{ - SOFA_UNUSED(velocityFactor); - // do nothing if distance between x2 & x1 is bigger than f_minDistance - if (constraintReleased[index]) return; - - getVCenter(x2) = getVCenter(x1); - if (!freeRotations) - getVOrientation(x2) = getVOrientation(x1); -} - -template<> -inline void AttachConstraint::doProjectResponse(Deriv& dx1, Deriv& dx2, bool freeRotations, bool twoway, unsigned index, Real responseFactor) -{ - SOFA_UNUSED(responseFactor); - // do nothing if distance between x2 & x1 is bigger than f_minDistance - if (constraintReleased[index]) return; - - if (!twoway) - { - if (!freeRotations) - dx2 = Deriv(); - else - getVCenter(dx2).clear(); - } - else - { - if (!freeRotations) - { - dx1 += dx2; - dx2 = dx1; - } - else - { - getVCenter(dx1) += getVCenter(dx2); - getVCenter(dx2) = getVCenter(dx1); - } - } -} - - -template<> -inline void AttachConstraint::doProjectResponse(Deriv& dx1, Deriv& dx2, bool freeRotations, bool twoway, unsigned index, Real responseFactor) -{ - SOFA_UNUSED(responseFactor); - // do nothing if distance between x2 & x1 is bigger than f_minDistance - if (constraintReleased[index]) return; - - if (!twoway) - { - if (!freeRotations) - dx2 = Deriv(); - else - getVCenter(dx2).clear(); - } - else - { - if (!freeRotations) - { - dx1 += dx2; - dx2 = dx1; - } - else - { - getVCenter(dx1) += getVCenter(dx2); - getVCenter(dx2) = getVCenter(dx1); - } - } -} - -template -inline unsigned int AttachConstraint::DerivConstrainedSize(bool freeRotations) -{ - if (std::is_same::value || std::is_same::value) { - if (freeRotations) - return Deriv::spatial_dimensions; - else - return Deriv::total_size; - } - else { - SOFA_UNUSED(freeRotations); - return Deriv::size(); - } -} - -// Could be simplified with default values for mm1 and mm2, but that way we are assured that either both or neither of mm1/mm2 are set. -template -AttachConstraint::AttachConstraint() - : AttachConstraint::AttachConstraint(nullptr, nullptr) -{ -} - -template -AttachConstraint::AttachConstraint(core::behavior::MechanicalState *mm1, core::behavior::MechanicalState *mm2) - : core::behavior::PairInteractionProjectiveConstraintSet(mm1,mm2) - , f_indices1( initData(&f_indices1,"indices1","Indices of the source points on the first model") ) - , f_indices2( initData(&f_indices2,"indices2","Indices of the fixed points on the second model") ) - , f_twoWay( initData(&f_twoWay,false,"twoWay", "true if forces should be projected back from model2 to model1") ) - , f_freeRotations( initData(&f_freeRotations,false,"freeRotations", "true to keep rotations free (only used for Rigid DOFs)") ) - , f_lastFreeRotation( initData(&f_lastFreeRotation,false,"lastFreeRotation", "true to keep rotation of the last attached point free (only used for Rigid DOFs)") ) - , f_restRotations( initData(&f_restRotations,false,"restRotations", "true to use rest rotations local offsets (only used for Rigid DOFs)") ) - , f_lastPos( initData(&f_lastPos,"lastPos", "position at which the attach constraint should become inactive") ) - , f_lastDir( initData(&f_lastDir,"lastDir", "direction from lastPos at which the attach coustraint should become inactive") ) - , f_clamp( initData(&f_clamp, false,"clamp", "true to clamp particles at lastPos instead of freeing them.") ) - , f_minDistance( initData(&f_minDistance, static_cast(-1),"minDistance", "the constraint become inactive if the distance between the points attached is bigger than minDistance.") ) - , d_positionFactor(initData(&d_positionFactor, static_cast(1.0), "positionFactor", "IN: Factor applied to projection of position")) - , d_velocityFactor(initData(&d_velocityFactor, static_cast(1.0), "velocityFactor", "IN: Factor applied to projection of velocity")) - , d_responseFactor(initData(&d_responseFactor, static_cast(1.0), "responseFactor", "IN: Factor applied to projection of force/acceleration")) - , d_constraintFactor( initData(&d_constraintFactor,"constraintFactor","Constraint factor per pair of points constrained. 0 -> the constraint is released. 1 -> the constraint is fully constrained") ) -{ - -} - -template -AttachConstraint::~AttachConstraint() -{ -} - -template -void AttachConstraint::init() -{ - this->core::behavior::PairInteractionProjectiveConstraintSet::init(); - reinit(); -} - -template -void AttachConstraint::reinit() -{ - // Check coherency of size between indices vectors 1 and 2 - if(f_indices1.getValue().size() != f_indices2.getValue().size()) - { - msg_warning() << "Size mismatch between indices1 and indices2 (" - << f_indices1.getValue().size() << " != " << f_indices2.getValue().size() << ")."; - } - - // Set to the correct length if dynamic, else check coherency. - if(d_constraintFactor.getValue().size()) - { - helper::ReadAccessor>> constraintFactor = d_constraintFactor; - if(constraintFactor.size() != f_indices2.getValue().size()) - { - msg_warning() << "Size of vector constraintFactor, do not fit number of indices attached (" << constraintFactor.size() << " != " << f_indices2.getValue().size() << ")."; - } - else - { - for (unsigned int j=0; j 1.0) || (constraintFactor[j] < 0.0)) - { - msg_warning() << "Value of vector constraintFactor at indice "< 1.0e-10) { - lastDist.resize(f_indices2.getValue().size()); - } - - if (f_restRotations.getValue()) - calcRestRotations(); - this->d_componentState.setValue(sofa::core::objectmodel::ComponentState::Valid); -} - -template -void AttachConstraint::reinitIfChanged() { - if((f_indices1.getParent() || f_indices2.getParent()) && constraintReleased.size() != f_indices2.getValue().size()) - { - reinit(); - } -} - -template -void AttachConstraint::calcRestRotations() -{ -} - -template <> -void AttachConstraint::calcRestRotations(); - -template -void AttachConstraint::projectJacobianMatrix(const core::MechanicalParams* mparams, core::MultiMatrixDerivId cId) -{ - SOFA_UNUSED(mparams); - SOFA_UNUSED(cId); -} - -template -void AttachConstraint::projectPosition(const core::MechanicalParams * mparams, DataVecCoord& res1_d, DataVecCoord& res2_d) -{ - SOFA_UNUSED(mparams); - const SetIndexArray & indices1 = f_indices1.getValue(); - const SetIndexArray & indices2 = f_indices2.getValue(); - const bool freeRotations = f_freeRotations.getValue(); - const bool lastFreeRotation = f_lastFreeRotation.getValue(); - const bool last = (f_lastDir.isSet() && f_lastDir.getValue().norm() > 1.0e-10); - const bool clamp = f_clamp.getValue(); - - VecCoord &res1 = *res1_d.beginEdit(); - VecCoord &res2 = *res2_d.beginEdit(); - - // update active flags - reinitIfChanged(); - - for (unsigned int i=0; i p3d; - DataTypes::get(p3d[0],p3d[1],p3d[2],p); - lastDist[i] = (Real)( (p3d-f_lastPos.getValue())*f_lastDir.getValue()); - if (lastDist[i] > 0.0) - { - if (clamp) - { - msg_info_when(activeFlags[i]) << "AttachConstraint: point " - <> positionFactor = d_positionFactor; - for (unsigned int i=0; i=activeFlags.size() || !activeFlags[i+1])), i, positionFactor); - } - else if (clamp) - { - DataTypes::set(p,f_lastPos.getValue()[0],f_lastPos.getValue()[1],f_lastPos.getValue()[2]); - - msg_info() << "AttachConstraint: x2["< -void AttachConstraint::projectVelocity(const core::MechanicalParams * mparams, DataVecDeriv& res1_d, DataVecDeriv& res2_d) -{ - SOFA_UNUSED(mparams); - VecDeriv &res1 = *res1_d.beginEdit(); - VecDeriv &res2 = *res2_d.beginEdit(); - - const SetIndexArray & indices1 = f_indices1.getValue(); - const SetIndexArray & indices2 = f_indices2.getValue(); - const bool freeRotations = f_freeRotations.getValue(); - const bool lastFreeRotation = f_lastFreeRotation.getValue(); - const bool clamp = f_clamp.getValue(); - - reinitIfChanged(); - helper::ReadAccessor> velocityFactor = d_velocityFactor; - for (unsigned int i=0; i=activeFlags.size() || !activeFlags[i+1])), i, velocityFactor); - } - else if (clamp) - { - msg_info() << "AttachConstraint: v2["< -void AttachConstraint::projectResponse(const core::MechanicalParams * mparams, DataVecDeriv& res1_d, DataVecDeriv& res2_d) -{ - SOFA_UNUSED(mparams); - VecDeriv &res1 = *res1_d.beginEdit(); - VecDeriv &res2 = *res2_d.beginEdit(); - - const SetIndexArray & indices1 = f_indices1.getValue(); - const SetIndexArray & indices2 = f_indices2.getValue(); - const bool twoway = f_twoWay.getValue(); - const bool freeRotations = f_freeRotations.getValue(); - const bool lastFreeRotation = f_lastFreeRotation.getValue(); - const bool clamp = f_clamp.getValue(); - - reinitIfChanged(); - helper::ReadAccessor> responseFactor = d_responseFactor; - for (unsigned int i=0; i=activeFlags.size() || !activeFlags[i+1])), twoway, i, responseFactor); - - msg_info() << " final r2["< -void AttachConstraint::applyConstraint(const core::MechanicalParams * mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) -{ - SOFA_UNUSED(mparams); - if (f_twoWay.getValue()) - return; - - const sofa::core::behavior::MultiMatrixAccessor::MatrixRef r = matrix->getMatrix(this->mstate2); - if (!r) - return; - - sofa::linearalgebra::BaseMatrix *mat = r.matrix; - const unsigned int offset = r.offset; - - const SetIndexArray & indices = f_indices2.getValue(); - const unsigned int N = Deriv::size(); - const unsigned int NC = DerivConstrainedSize(f_freeRotations.getValue()); - const unsigned int NCLast = DerivConstrainedSize(f_lastFreeRotation.getValue()); - unsigned int i=0; - const bool clamp = f_clamp.getValue(); - - reinitIfChanged(); - - for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it, ++i) - { - if (!clamp && i < activeFlags.size() && !activeFlags[i]) - continue; - - msg_info() << "AttachConstraint: apply in matrix column/row "<<(*it); - - if (NCLast != NC && (i>=activeFlags.size() || !activeFlags[i+1])) - { - // Reset Fixed Row and Col - for (unsigned int c=0; cclearRowCol(offset + N * (*it) + c); - // Set Fixed Vertex - for (unsigned int c=0; cset(offset + N * (*it) + c, offset + N * (*it) + c, 1.0); - } - else - { - // Reset Fixed Row and Col - for (unsigned int c=0; cclearRowCol(offset + N * (*it) + c); - // Set Fixed Vertex - for (unsigned int c=0; cset(offset + N * (*it) + c, offset + N * (*it) + c, 1.0); - } - } -} - - -template -void AttachConstraint::applyConstraint(const core::MechanicalParams * mparams, linearalgebra::BaseVector* vect, const sofa::core::behavior::MultiMatrixAccessor* matrix) -{ - SOFA_UNUSED(mparams); - if (f_twoWay.getValue()) - return; - - const int o = matrix->getGlobalOffset(this->mstate2); - if (o < 0) - return; - - unsigned int offset = (unsigned int)o; - - msg_info() << "applyConstraint in Vector with offset = " << offset ; - - const SetIndexArray & indices = f_indices2.getValue(); - const unsigned int N = Deriv::size(); - const unsigned int NC = DerivConstrainedSize(f_freeRotations.getValue()); - const unsigned int NCLast = DerivConstrainedSize(f_lastFreeRotation.getValue()); - unsigned int i = 0; - const bool clamp = f_clamp.getValue(); - - reinitIfChanged(); - - for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it, ++i) - { - if (!clamp && i < activeFlags.size() && !activeFlags[i]) - continue; - - if (NCLast != NC && (i>=activeFlags.size() || !activeFlags[i+1])) - { - for (unsigned int c=0; cclear(offset + N * (*it) + c); - } - else - { - for (unsigned int c=0; cclear(offset + N * (*it) + c); - } - } -} - -template -const typename DataTypes::Real AttachConstraint::getConstraintFactor(const int index) { - return d_constraintFactor.getValue().size() ? d_constraintFactor.getValue()[index] : 1; -} - -template -void AttachConstraint::doProjectPosition(Coord& x1, Coord& x2, bool freeRotations, unsigned index, Real positionFactor) -{ - SOFA_UNUSED(freeRotations); - // do nothing if distance between x2 & x1 is bigger than f_minDistance - if (f_minDistance.getValue() != -1 && - (x2 - x1).norm() > f_minDistance.getValue()) - { - constraintReleased[index] = true; - return; - } - constraintReleased[index] = false; - - Deriv corr = (x2-x1)*(0.5*positionFactor*getConstraintFactor(index)); - - x1 += corr; - x2 -= corr; -} - -template -void AttachConstraint::doProjectVelocity(Deriv &x1, Deriv &x2, bool freeRotations, unsigned index, Real velocityFactor) -{ - SOFA_UNUSED(freeRotations); - // do nothing if distance between x2 & x1 is bigger than f_minDistance - if (constraintReleased[index]) return; - - Deriv corr = (x2-x1)*(0.5*velocityFactor*getConstraintFactor(index)); - - x1 += corr; - x2 -= corr; -} - -template -void AttachConstraint::doProjectResponse(Deriv& dx1, Deriv& dx2, bool freeRotations, bool twoway, unsigned index, Real responseFactor) -{ - SOFA_UNUSED(freeRotations); - // do nothing if distance between x2 & x1 is bigger than f_minDistance - if (constraintReleased[index]) return; - - if (!twoway) - { - dx2 = Deriv(); - } - else - { - Deriv corr = (dx2-dx1)*0.5*responseFactor*getConstraintFactor(index); - dx1 += corr; - dx2 -= corr; - } -} - - -template -void AttachConstraint::draw(const core::visual::VisualParams* vparams) -{ - if (!vparams->displayFlags().getShowBehaviorModels()) - return; - - const auto stateLifeCycle = vparams->drawTool()->makeStateLifeCycle(); - vparams->drawTool()->disableLighting(); - - const SetIndexArray & indices1 = f_indices1.getValue(); - const SetIndexArray & indices2 = f_indices2.getValue(); - const VecCoord& x1 = this->mstate1->read(core::ConstVecCoordId::position())->getValue(); - const VecCoord& x2 = this->mstate2->read(core::ConstVecCoordId::position())->getValue(); - - constexpr sofa::type::RGBAColor color1(1,0.5,0.5,1); - std::vector vertices; - - for (unsigned int i=0; i i && !activeFlags[i]) - continue; - vertices.push_back(sofa::type::Vec3(x2[indices2[i]][0],x2[indices2[i]][1],x2[indices2[i]][2])); - } - vparams->drawTool()->drawPoints(vertices,10,color1); - vertices.clear(); - - constexpr sofa::type::RGBAColor color2(1,0.5,0.5,1); - for (unsigned int i=0; i i && !activeFlags[i]) - continue; - vertices.push_back(sofa::type::Vec3(x1[indices1[i]][0],x1[indices1[i]][1],x1[indices1[i]][2])); - vertices.push_back(sofa::type::Vec3(x2[indices2[i]][0],x2[indices2[i]][1],x2[indices2[i]][2])); - } - vparams->drawTool()->drawLines(vertices,1,color2); - -} - -} // namespace sofa::component::constraint::projective +SOFA_DEPRECATED_HEADER("v24.06", "v25.06", "sofa/component/constraint/projective/AttachProjectiveConstraint.inl") diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/AttachConstraint.cpp b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/AttachProjectiveConstraint.cpp similarity index 70% rename from Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/AttachConstraint.cpp rename to Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/AttachProjectiveConstraint.cpp index 8642434e285..a4b0cf94002 100644 --- a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/AttachConstraint.cpp +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/AttachProjectiveConstraint.cpp @@ -19,8 +19,8 @@ * * * Contact information: contact@sofa-framework.org * ******************************************************************************/ -#define SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_ATTACHCONSTRAINT_CPP -#include +#define SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_ATTACHPROJECTIVECONSTRAINT_CPP +#include #include namespace sofa::component::constraint::projective @@ -30,16 +30,17 @@ using namespace sofa::type; using namespace sofa::defaulttype; using namespace sofa::helper; -int AttachConstraintClass = core::RegisterObject("Attach given pair of particles, projecting the positions of the second particles to the first ones") - .add< AttachConstraint >() - .add< AttachConstraint >() - .add< AttachConstraint >() - .add< AttachConstraint >() - .add< AttachConstraint >() +int AttachProjectiveConstraintClass = core::RegisterObject("Attach given pair of particles, projecting the positions of the second particles to the first ones") + .add< AttachProjectiveConstraint >() + .add< AttachProjectiveConstraint >() + .add< AttachProjectiveConstraint >() + .add< AttachProjectiveConstraint >() + .add< AttachProjectiveConstraint >() + .addAlias("AttachConstraint") ; template <> SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API -void AttachConstraint::calcRestRotations() +void AttachProjectiveConstraint::calcRestRotations() { const SetIndexArray & indices2 = f_indices2.getValue(); const VecCoord& x0 = this->mstate2->read(core::ConstVecCoordId::restPosition())->getValue(); @@ -61,11 +62,11 @@ void AttachConstraint::calcRestRotations() } } -template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API AttachConstraint; -template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API AttachConstraint; -template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API AttachConstraint; -template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API AttachConstraint; -template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API AttachConstraint; +template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API AttachProjectiveConstraint; +template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API AttachProjectiveConstraint; +template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API AttachProjectiveConstraint; +template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API AttachProjectiveConstraint; +template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API AttachProjectiveConstraint; diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/AttachProjectiveConstraint.h b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/AttachProjectiveConstraint.h new file mode 100644 index 00000000000..37ed886dc4b --- /dev/null +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/AttachProjectiveConstraint.h @@ -0,0 +1,124 @@ +/****************************************************************************** +* 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::constraint::projective +{ + +/** Attach given pair of particles, projecting the positions of the second particles to the first ones. +*/ +template +class AttachProjectiveConstraint : public core::behavior::PairInteractionProjectiveConstraintSet +{ +public: + SOFA_CLASS(SOFA_TEMPLATE(AttachProjectiveConstraint,DataTypes),SOFA_TEMPLATE(sofa::core::behavior::PairInteractionProjectiveConstraintSet,DataTypes)); + + typedef typename DataTypes::VecCoord VecCoord; + typedef typename DataTypes::VecDeriv VecDeriv; + typedef typename DataTypes::Coord Coord; + typedef typename DataTypes::Deriv Deriv; + typedef typename DataTypes::Real Real; + + typedef core::objectmodel::Data DataVecCoord; + typedef core::objectmodel::Data DataVecDeriv; + + typedef type::vector SetIndexArray; + typedef sofa::core::topology::TopologySubsetIndices SetIndex; + +public: + SetIndex f_indices1; ///< Indices of the source points on the first model + SetIndex f_indices2; ///< Indices of the fixed points on the second model + Data f_twoWay; ///< true if forces should be projected back from model2 to model1 + Data f_freeRotations; ///< true to keep rotations free (only used for Rigid DOFs) + Data f_lastFreeRotation; ///< true to keep rotation of the last attached point free (only used for Rigid DOFs) + Data f_restRotations; ///< true to use rest rotations local offsets (only used for Rigid DOFs) + Data f_lastPos; ///< position at which the attach constraint should become inactive + Data f_lastDir; ///< direction from lastPos at which the attach coustraint should become inactive + Data f_clamp; ///< true to clamp particles at lastPos instead of freeing them. + Data f_minDistance; ///< the constraint become inactive if the distance between the points attached is bigger than minDistance. + Data< Real > d_positionFactor; ///< IN: Factor applied to projection of position + Data< Real > d_velocityFactor; ///< IN: Factor applied to projection of velocity + Data< Real > d_responseFactor; ///< IN: Factor applied to projection of force/acceleration + Data< type::vector > d_constraintFactor; ///< Constraint factor per pair of points constrained. 0 -> the constraint is released. 1 -> the constraint is fully constrained + + type::vector activeFlags; + type::vector constraintReleased; + type::vector lastDist; + type::vector> restRotations; + +protected: + AttachProjectiveConstraint(); + AttachProjectiveConstraint(core::behavior::MechanicalState *mm1, core::behavior::MechanicalState *mm2); + ~AttachProjectiveConstraint() override; + +public: + + /// Inherited from Base + void init() override; + void reinit() override; + void draw(const core::visual::VisualParams* vparams) override; + + /// Inherited from Constraint + void projectJacobianMatrix(const core::MechanicalParams* mparams, core::MultiMatrixDerivId cId) override; + void projectResponse(const core::MechanicalParams *mparams, DataVecDeriv& dx1, DataVecDeriv& dx2) override; + void projectVelocity(const core::MechanicalParams *mparams, DataVecDeriv& v1, DataVecDeriv& v2) override; + void projectPosition(const core::MechanicalParams *mparams, DataVecCoord& x1, DataVecCoord& x2) override; + + /// Project the global Mechanical Matrix to constrained space using offset parameter + void applyConstraint(const core::MechanicalParams *mparams, + const sofa::core::behavior::MultiMatrixAccessor* matrix) override; + + /// Project the global Mechanical Vector to constrained space using offset parameter + void applyConstraint(const core::MechanicalParams *mparams, linearalgebra::BaseVector* vector, const sofa::core::behavior::MultiMatrixAccessor* matrix) override; + + virtual void reinitIfChanged(); + + template + static std::string templateName(const T* ptr= nullptr) { + return core::behavior::PairInteractionProjectiveConstraintSet::templateName(ptr); + } + +protected : + const Real getConstraintFactor(const int index); + void doProjectPosition(Coord& x1, Coord& x2, bool freeRotations, unsigned index, Real positionFactor); + void doProjectVelocity(Deriv& x1, Deriv& x2, bool freeRotations, unsigned index, Real velocityFactor); + void doProjectResponse(Deriv& dx1, Deriv& dx2, bool freeRotations, bool twoway, unsigned index, Real responseFactor); + + void calcRestRotations(); + static unsigned int DerivConstrainedSize(bool freeRotations); + +}; + + +#if !defined(SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_ATTACHPROJECTIVECONSTRAINT_CPP) +extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API AttachProjectiveConstraint; +extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API AttachProjectiveConstraint; +extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API AttachProjectiveConstraint; +extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API AttachProjectiveConstraint; +extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API AttachProjectiveConstraint; +#endif + +} // namespace sofa::component::constraint::projective diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/AttachProjectiveConstraint.inl b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/AttachProjectiveConstraint.inl new file mode 100644 index 00000000000..e7bd429434f --- /dev/null +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/AttachProjectiveConstraint.inl @@ -0,0 +1,643 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once +#include +#include +#include +#include +#include +#include + +namespace sofa::component::constraint::projective +{ + +using sofa::simulation::Node ; + +template<> +inline void AttachProjectiveConstraint::doProjectPosition(Coord& x1, Coord& x2, bool freeRotations, unsigned index, Real positionFactor) +{ + SOFA_UNUSED(positionFactor); + // do nothing if distance between x2 & x1 is bigger than f_minDistance + if (f_minDistance.getValue() != -1.0 && + (x2.getCenter() - x1.getCenter()).norm() > f_minDistance.getValue()) + { + constraintReleased[index] = true; + return; + } + constraintReleased[index] = false; + + x2.getCenter() = x1.getCenter(); + if (!freeRotations) + { + if (!restRotations.empty()) + { + if (index+1 >= lastDist.size() || activeFlags[index+1]) + x2.getOrientation() = x1.getOrientation()*restRotations[index]; + else + { + // gradually set the velocity along the direction axis + const Real fact = -lastDist[index] / (lastDist[index+1]-lastDist[index]); + const sofa::type::Vec3 axis(restRotations[index][0], restRotations[index][1], restRotations[index][2]); + const Real angle = acos(restRotations[index][3])*2; + x2.getOrientation() = x1.getOrientation()*sofa::type::Quat(axis,angle*fact); + } + } + else + x2.getOrientation() = x1.getOrientation(); + } +} + + +template<> +inline void AttachProjectiveConstraint::doProjectPosition(Coord& x1, Coord& x2, bool freeRotations, unsigned index, Real positionFactor) +{ + SOFA_UNUSED(positionFactor); + // do nothing if distance between x2 & x1 is bigger than f_minDistance + if (f_minDistance.getValue() != -1 && + (x2.getCenter() - x1.getCenter()).norm() > f_minDistance.getValue()) + { + constraintReleased[index] = true; + return; + } + constraintReleased[index] = false; + + x2.getCenter() = x1.getCenter(); + if (!freeRotations) + x2.getOrientation() = x1.getOrientation(); +} + + +template<> +inline void AttachProjectiveConstraint::doProjectVelocity(Deriv& x1, Deriv& x2, bool freeRotations, unsigned index, Real velocityFactor) +{ + SOFA_UNUSED(velocityFactor); + // do nothing if distance between x2 & x1 is bigger than f_minDistance + if (constraintReleased[index]) + return; + + getVCenter( x2) = getVCenter(x1); + if (!freeRotations) + getVOrientation(x2) = getVOrientation(x1); +} + + +template<> +inline void AttachProjectiveConstraint::doProjectVelocity(Deriv& x1, Deriv& x2, bool freeRotations, unsigned index, Real velocityFactor) +{ + SOFA_UNUSED(velocityFactor); + // do nothing if distance between x2 & x1 is bigger than f_minDistance + if (constraintReleased[index]) return; + + getVCenter(x2) = getVCenter(x1); + if (!freeRotations) + getVOrientation(x2) = getVOrientation(x1); +} + +template<> +inline void AttachProjectiveConstraint::doProjectResponse(Deriv& dx1, Deriv& dx2, bool freeRotations, bool twoway, unsigned index, Real responseFactor) +{ + SOFA_UNUSED(responseFactor); + // do nothing if distance between x2 & x1 is bigger than f_minDistance + if (constraintReleased[index]) return; + + if (!twoway) + { + if (!freeRotations) + dx2 = Deriv(); + else + getVCenter(dx2).clear(); + } + else + { + if (!freeRotations) + { + dx1 += dx2; + dx2 = dx1; + } + else + { + getVCenter(dx1) += getVCenter(dx2); + getVCenter(dx2) = getVCenter(dx1); + } + } +} + + +template<> +inline void AttachProjectiveConstraint::doProjectResponse(Deriv& dx1, Deriv& dx2, bool freeRotations, bool twoway, unsigned index, Real responseFactor) +{ + SOFA_UNUSED(responseFactor); + // do nothing if distance between x2 & x1 is bigger than f_minDistance + if (constraintReleased[index]) return; + + if (!twoway) + { + if (!freeRotations) + dx2 = Deriv(); + else + getVCenter(dx2).clear(); + } + else + { + if (!freeRotations) + { + dx1 += dx2; + dx2 = dx1; + } + else + { + getVCenter(dx1) += getVCenter(dx2); + getVCenter(dx2) = getVCenter(dx1); + } + } +} + +template +inline unsigned int AttachProjectiveConstraint::DerivConstrainedSize(bool freeRotations) +{ + if (std::is_same::value || std::is_same::value) { + if (freeRotations) + return Deriv::spatial_dimensions; + else + return Deriv::total_size; + } + else { + SOFA_UNUSED(freeRotations); + return Deriv::size(); + } +} + +// Could be simplified with default values for mm1 and mm2, but that way we are assured that either both or neither of mm1/mm2 are set. +template +AttachProjectiveConstraint::AttachProjectiveConstraint() + : AttachProjectiveConstraint::AttachProjectiveConstraint(nullptr, nullptr) +{ +} + +template +AttachProjectiveConstraint::AttachProjectiveConstraint(core::behavior::MechanicalState *mm1, core::behavior::MechanicalState *mm2) + : core::behavior::PairInteractionProjectiveConstraintSet(mm1,mm2) + , f_indices1( initData(&f_indices1,"indices1","Indices of the source points on the first model") ) + , f_indices2( initData(&f_indices2,"indices2","Indices of the fixed points on the second model") ) + , f_twoWay( initData(&f_twoWay,false,"twoWay", "true if forces should be projected back from model2 to model1") ) + , f_freeRotations( initData(&f_freeRotations,false,"freeRotations", "true to keep rotations free (only used for Rigid DOFs)") ) + , f_lastFreeRotation( initData(&f_lastFreeRotation,false,"lastFreeRotation", "true to keep rotation of the last attached point free (only used for Rigid DOFs)") ) + , f_restRotations( initData(&f_restRotations,false,"restRotations", "true to use rest rotations local offsets (only used for Rigid DOFs)") ) + , f_lastPos( initData(&f_lastPos,"lastPos", "position at which the attach constraint should become inactive") ) + , f_lastDir( initData(&f_lastDir,"lastDir", "direction from lastPos at which the attach coustraint should become inactive") ) + , f_clamp( initData(&f_clamp, false,"clamp", "true to clamp particles at lastPos instead of freeing them.") ) + , f_minDistance( initData(&f_minDistance, static_cast(-1),"minDistance", "the constraint become inactive if the distance between the points attached is bigger than minDistance.") ) + , d_positionFactor(initData(&d_positionFactor, static_cast(1.0), "positionFactor", "IN: Factor applied to projection of position")) + , d_velocityFactor(initData(&d_velocityFactor, static_cast(1.0), "velocityFactor", "IN: Factor applied to projection of velocity")) + , d_responseFactor(initData(&d_responseFactor, static_cast(1.0), "responseFactor", "IN: Factor applied to projection of force/acceleration")) + , d_constraintFactor( initData(&d_constraintFactor,"constraintFactor","Constraint factor per pair of points constrained. 0 -> the constraint is released. 1 -> the constraint is fully constrained") ) +{ + +} + +template +AttachProjectiveConstraint::~AttachProjectiveConstraint() +{ +} + +template +void AttachProjectiveConstraint::init() +{ + this->core::behavior::PairInteractionProjectiveConstraintSet::init(); + reinit(); +} + +template +void AttachProjectiveConstraint::reinit() +{ + // Check coherency of size between indices vectors 1 and 2 + if(f_indices1.getValue().size() != f_indices2.getValue().size()) + { + msg_warning() << "Size mismatch between indices1 and indices2 (" + << f_indices1.getValue().size() << " != " << f_indices2.getValue().size() << ")."; + } + + // Set to the correct length if dynamic, else check coherency. + if(d_constraintFactor.getValue().size()) + { + helper::ReadAccessor>> constraintFactor = d_constraintFactor; + if(constraintFactor.size() != f_indices2.getValue().size()) + { + msg_warning() << "Size of vector constraintFactor, do not fit number of indices attached (" << constraintFactor.size() << " != " << f_indices2.getValue().size() << ")."; + } + else + { + for (unsigned int j=0; j 1.0) || (constraintFactor[j] < 0.0)) + { + msg_warning() << "Value of vector constraintFactor at indice "< 1.0e-10) { + lastDist.resize(f_indices2.getValue().size()); + } + + if (f_restRotations.getValue()) + calcRestRotations(); + this->d_componentState.setValue(sofa::core::objectmodel::ComponentState::Valid); +} + +template +void AttachProjectiveConstraint::reinitIfChanged() { + if((f_indices1.getParent() || f_indices2.getParent()) && constraintReleased.size() != f_indices2.getValue().size()) + { + reinit(); + } +} + +template +void AttachProjectiveConstraint::calcRestRotations() +{ +} + +template <> +void AttachProjectiveConstraint::calcRestRotations(); + +template +void AttachProjectiveConstraint::projectJacobianMatrix(const core::MechanicalParams* mparams, core::MultiMatrixDerivId cId) +{ + SOFA_UNUSED(mparams); + SOFA_UNUSED(cId); +} + +template +void AttachProjectiveConstraint::projectPosition(const core::MechanicalParams * mparams, DataVecCoord& res1_d, DataVecCoord& res2_d) +{ + SOFA_UNUSED(mparams); + const SetIndexArray & indices1 = f_indices1.getValue(); + const SetIndexArray & indices2 = f_indices2.getValue(); + const bool freeRotations = f_freeRotations.getValue(); + const bool lastFreeRotation = f_lastFreeRotation.getValue(); + const bool last = (f_lastDir.isSet() && f_lastDir.getValue().norm() > 1.0e-10); + const bool clamp = f_clamp.getValue(); + + VecCoord &res1 = *res1_d.beginEdit(); + VecCoord &res2 = *res2_d.beginEdit(); + + // update active flags + reinitIfChanged(); + + for (unsigned int i=0; i p3d; + DataTypes::get(p3d[0],p3d[1],p3d[2],p); + lastDist[i] = (Real)( (p3d-f_lastPos.getValue())*f_lastDir.getValue()); + if (lastDist[i] > 0.0) + { + if (clamp) + { + msg_info_when(activeFlags[i]) << "AttachProjectiveConstraint: point " + <> positionFactor = d_positionFactor; + for (unsigned int i=0; i=activeFlags.size() || !activeFlags[i+1])), i, positionFactor); + } + else if (clamp) + { + DataTypes::set(p,f_lastPos.getValue()[0],f_lastPos.getValue()[1],f_lastPos.getValue()[2]); + + msg_info() << "AttachProjectiveConstraint: x2["< +void AttachProjectiveConstraint::projectVelocity(const core::MechanicalParams * mparams, DataVecDeriv& res1_d, DataVecDeriv& res2_d) +{ + SOFA_UNUSED(mparams); + VecDeriv &res1 = *res1_d.beginEdit(); + VecDeriv &res2 = *res2_d.beginEdit(); + + const SetIndexArray & indices1 = f_indices1.getValue(); + const SetIndexArray & indices2 = f_indices2.getValue(); + const bool freeRotations = f_freeRotations.getValue(); + const bool lastFreeRotation = f_lastFreeRotation.getValue(); + const bool clamp = f_clamp.getValue(); + + reinitIfChanged(); + helper::ReadAccessor> velocityFactor = d_velocityFactor; + for (unsigned int i=0; i=activeFlags.size() || !activeFlags[i+1])), i, velocityFactor); + } + else if (clamp) + { + msg_info() << "AttachProjectiveConstraint: v2["< +void AttachProjectiveConstraint::projectResponse(const core::MechanicalParams * mparams, DataVecDeriv& res1_d, DataVecDeriv& res2_d) +{ + SOFA_UNUSED(mparams); + VecDeriv &res1 = *res1_d.beginEdit(); + VecDeriv &res2 = *res2_d.beginEdit(); + + const SetIndexArray & indices1 = f_indices1.getValue(); + const SetIndexArray & indices2 = f_indices2.getValue(); + const bool twoway = f_twoWay.getValue(); + const bool freeRotations = f_freeRotations.getValue(); + const bool lastFreeRotation = f_lastFreeRotation.getValue(); + const bool clamp = f_clamp.getValue(); + + reinitIfChanged(); + helper::ReadAccessor> responseFactor = d_responseFactor; + for (unsigned int i=0; i=activeFlags.size() || !activeFlags[i+1])), twoway, i, responseFactor); + + msg_info() << " final r2["< +void AttachProjectiveConstraint::applyConstraint(const core::MechanicalParams * mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) +{ + SOFA_UNUSED(mparams); + if (f_twoWay.getValue()) + return; + + const sofa::core::behavior::MultiMatrixAccessor::MatrixRef r = matrix->getMatrix(this->mstate2); + if (!r) + return; + + sofa::linearalgebra::BaseMatrix *mat = r.matrix; + const unsigned int offset = r.offset; + + const SetIndexArray & indices = f_indices2.getValue(); + const unsigned int N = Deriv::size(); + const unsigned int NC = DerivConstrainedSize(f_freeRotations.getValue()); + const unsigned int NCLast = DerivConstrainedSize(f_lastFreeRotation.getValue()); + unsigned int i=0; + const bool clamp = f_clamp.getValue(); + + reinitIfChanged(); + + for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it, ++i) + { + if (!clamp && i < activeFlags.size() && !activeFlags[i]) + continue; + + msg_info() << "AttachProjectiveConstraint: apply in matrix column/row "<<(*it); + + if (NCLast != NC && (i>=activeFlags.size() || !activeFlags[i+1])) + { + // Reset Fixed Row and Col + for (unsigned int c=0; cclearRowCol(offset + N * (*it) + c); + // Set Fixed Vertex + for (unsigned int c=0; cset(offset + N * (*it) + c, offset + N * (*it) + c, 1.0); + } + else + { + // Reset Fixed Row and Col + for (unsigned int c=0; cclearRowCol(offset + N * (*it) + c); + // Set Fixed Vertex + for (unsigned int c=0; cset(offset + N * (*it) + c, offset + N * (*it) + c, 1.0); + } + } +} + + +template +void AttachProjectiveConstraint::applyConstraint(const core::MechanicalParams * mparams, linearalgebra::BaseVector* vect, const sofa::core::behavior::MultiMatrixAccessor* matrix) +{ + SOFA_UNUSED(mparams); + if (f_twoWay.getValue()) + return; + + const int o = matrix->getGlobalOffset(this->mstate2); + if (o < 0) + return; + + unsigned int offset = (unsigned int)o; + + msg_info() << "applyConstraint in Vector with offset = " << offset ; + + const SetIndexArray & indices = f_indices2.getValue(); + const unsigned int N = Deriv::size(); + const unsigned int NC = DerivConstrainedSize(f_freeRotations.getValue()); + const unsigned int NCLast = DerivConstrainedSize(f_lastFreeRotation.getValue()); + unsigned int i = 0; + const bool clamp = f_clamp.getValue(); + + reinitIfChanged(); + + for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it, ++i) + { + if (!clamp && i < activeFlags.size() && !activeFlags[i]) + continue; + + if (NCLast != NC && (i>=activeFlags.size() || !activeFlags[i+1])) + { + for (unsigned int c=0; cclear(offset + N * (*it) + c); + } + else + { + for (unsigned int c=0; cclear(offset + N * (*it) + c); + } + } +} + +template +const typename DataTypes::Real AttachProjectiveConstraint::getConstraintFactor(const int index) { + return d_constraintFactor.getValue().size() ? d_constraintFactor.getValue()[index] : 1; +} + +template +void AttachProjectiveConstraint::doProjectPosition(Coord& x1, Coord& x2, bool freeRotations, unsigned index, Real positionFactor) +{ + SOFA_UNUSED(freeRotations); + // do nothing if distance between x2 & x1 is bigger than f_minDistance + if (f_minDistance.getValue() != -1 && + (x2 - x1).norm() > f_minDistance.getValue()) + { + constraintReleased[index] = true; + return; + } + constraintReleased[index] = false; + + Deriv corr = (x2-x1)*(0.5*positionFactor*getConstraintFactor(index)); + + x1 += corr; + x2 -= corr; +} + +template +void AttachProjectiveConstraint::doProjectVelocity(Deriv &x1, Deriv &x2, bool freeRotations, unsigned index, Real velocityFactor) +{ + SOFA_UNUSED(freeRotations); + // do nothing if distance between x2 & x1 is bigger than f_minDistance + if (constraintReleased[index]) return; + + Deriv corr = (x2-x1)*(0.5*velocityFactor*getConstraintFactor(index)); + + x1 += corr; + x2 -= corr; +} + +template +void AttachProjectiveConstraint::doProjectResponse(Deriv& dx1, Deriv& dx2, bool freeRotations, bool twoway, unsigned index, Real responseFactor) +{ + SOFA_UNUSED(freeRotations); + // do nothing if distance between x2 & x1 is bigger than f_minDistance + if (constraintReleased[index]) return; + + if (!twoway) + { + dx2 = Deriv(); + } + else + { + Deriv corr = (dx2-dx1)*0.5*responseFactor*getConstraintFactor(index); + dx1 += corr; + dx2 -= corr; + } +} + + +template +void AttachProjectiveConstraint::draw(const core::visual::VisualParams* vparams) +{ + if (!vparams->displayFlags().getShowBehaviorModels()) + return; + + const auto stateLifeCycle = vparams->drawTool()->makeStateLifeCycle(); + vparams->drawTool()->disableLighting(); + + const SetIndexArray & indices1 = f_indices1.getValue(); + const SetIndexArray & indices2 = f_indices2.getValue(); + const VecCoord& x1 = this->mstate1->read(core::ConstVecCoordId::position())->getValue(); + const VecCoord& x2 = this->mstate2->read(core::ConstVecCoordId::position())->getValue(); + + constexpr sofa::type::RGBAColor color1(1,0.5,0.5,1); + std::vector vertices; + + for (unsigned int i=0; i i && !activeFlags[i]) + continue; + vertices.push_back(sofa::type::Vec3(x2[indices2[i]][0],x2[indices2[i]][1],x2[indices2[i]][2])); + } + vparams->drawTool()->drawPoints(vertices,10,color1); + vertices.clear(); + + constexpr sofa::type::RGBAColor color2(1,0.5,0.5,1); + for (unsigned int i=0; i i && !activeFlags[i]) + continue; + vertices.push_back(sofa::type::Vec3(x1[indices1[i]][0],x1[indices1[i]][1],x1[indices1[i]][2])); + vertices.push_back(sofa::type::Vec3(x2[indices2[i]][0],x2[indices2[i]][1],x2[indices2[i]][2])); + } + vparams->drawTool()->drawLines(vertices,1,color2); + +} + +} // namespace sofa::component::constraint::projective diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/ProjectDirectionConstraint.cpp b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/DirectionProjectiveConstraint.cpp similarity index 75% rename from Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/ProjectDirectionConstraint.cpp rename to Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/DirectionProjectiveConstraint.cpp index 9d956a1949d..a4f22a624ef 100644 --- a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/ProjectDirectionConstraint.cpp +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/DirectionProjectiveConstraint.cpp @@ -19,8 +19,8 @@ * * * Contact information: contact@sofa-framework.org * ******************************************************************************/ -#define SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_ProjectDirectionConstraint_CPP -#include +#define SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_DirectionProjectiveConstraint_CPP +#include #include #include @@ -32,14 +32,14 @@ using namespace sofa::defaulttype; using namespace sofa::helper; -int ProjectDirectionConstraintClass = core::RegisterObject("Attach given particles to their initial positions") - .add< ProjectDirectionConstraint >() - .add< ProjectDirectionConstraint >() - +int DirectionProjectiveConstraintClass = core::RegisterObject("Attach given particles to their initial positions") + .add< DirectionProjectiveConstraint >() + .add< DirectionProjectiveConstraint >() + .addAlias("ProjectDirectionConstraint") ; -template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API ProjectDirectionConstraint; -template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API ProjectDirectionConstraint; +template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API DirectionProjectiveConstraint; +template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API DirectionProjectiveConstraint; } // namespace sofa::component::constraint::projective diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/DirectionProjectiveConstraint.h b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/DirectionProjectiveConstraint.h new file mode 100644 index 00000000000..29c85fc63e0 --- /dev/null +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/DirectionProjectiveConstraint.h @@ -0,0 +1,133 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once +#include + +#include +#include +#include +#include +#include +#include +#include +//#include +#include +#include +#include +#include +#include + +namespace sofa::component::constraint::projective +{ + +/// This class can be overridden if needed for additionnal storage within template specializations. +template +class DirectionProjectiveConstraintInternalData +{ + +}; + +/** Project particles to an affine straight line going through the particle original position. +*/ +template +class DirectionProjectiveConstraint : public core::behavior::ProjectiveConstraintSet +{ +public: + SOFA_CLASS(SOFA_TEMPLATE(DirectionProjectiveConstraint,DataTypes),SOFA_TEMPLATE(sofa::core::behavior::ProjectiveConstraintSet, DataTypes)); + + using Index = sofa::Index; + typedef typename DataTypes::VecCoord VecCoord; + typedef typename DataTypes::VecDeriv VecDeriv; + typedef typename DataTypes::MatrixDeriv MatrixDeriv; + typedef typename DataTypes::Coord Coord; + typedef typename DataTypes::Deriv Deriv; + typedef typename DataTypes::CPos CPos; + typedef typename MatrixDeriv::RowIterator MatrixDerivRowIterator; + typedef typename MatrixDeriv::RowType MatrixDerivRowType; + typedef Data DataVecCoord; + typedef Data DataVecDeriv; + typedef Data DataMatrixDeriv; + typedef type::vector Indices; + SOFA_ATTRIBUTE_REPLACED__TYPEMEMBER(Vector3, sofa::type::Vec3); + typedef sofa::core::topology::TopologySubsetIndices IndexSubsetData; + typedef linearalgebra::EigenBaseSparseMatrix BaseSparseMatrix; + typedef linearalgebra::EigenSparseMatrix SparseMatrix; + typedef typename SparseMatrix::Block Block; ///< projection matrix of a particle displacement to the plane + enum {bsize=SparseMatrix::Nin}; ///< size of a block + + +protected: + DirectionProjectiveConstraint(); + + virtual ~DirectionProjectiveConstraint(); + +public: + IndexSubsetData f_indices; ///< the particles to project + Data f_drawSize; ///< The size of the square used to display the constrained particles + Data f_direction; ///< The direction of the line. Will be normalized by init() + + /// Link to be set to the topology container in the component graph. + SingleLink, sofa::core::topology::BaseMeshTopology, BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_topology; + +protected: + DirectionProjectiveConstraintInternalData* data; + friend class DirectionProjectiveConstraintInternalData; + + type::vector m_origin; + + +public: + void clearConstraints(); + void addConstraint(Index index); + void removeConstraint(Index index); + + // -- Constraint interface + void init() override; + void reinit() override; + + void projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& resData) override; + void projectVelocity(const core::MechanicalParams* mparams, DataVecDeriv& vData) override; + void projectPosition(const core::MechanicalParams* mparams, DataVecCoord& xData) override; + void projectJacobianMatrix(const core::MechanicalParams* mparams, DataMatrixDeriv& cData) override; + + void applyConstraint(const core::MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) override; + void applyConstraint(const core::MechanicalParams* mparams, linearalgebra::BaseVector* vector, const sofa::core::behavior::MultiMatrixAccessor* matrix) override; + + /// Project the given matrix (Experimental API, see the spec in sofa::core::behavior::BaseProjectiveConstraintSet). + void projectMatrix( sofa::linearalgebra::BaseMatrix* /*M*/, unsigned /*offset*/ ) override; + + + void draw(const core::visual::VisualParams* vparams) override; + +protected : + + SparseMatrix jacobian; ///< projection matrix in local state + SparseMatrix J; ///< auxiliary variable +}; + + +#if !defined(SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_DirectionProjectiveConstraint_CPP) +extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API DirectionProjectiveConstraint; +extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API DirectionProjectiveConstraint; +#endif + +} // namespace sofa::component::constraint::projective diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/DirectionProjectiveConstraint.inl b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/DirectionProjectiveConstraint.inl new file mode 100644 index 00000000000..77d50d83146 --- /dev/null +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/DirectionProjectiveConstraint.inl @@ -0,0 +1,277 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once + +#include +#include +#include +#include +#include +#include +#include + + +namespace sofa::component::constraint::projective +{ + +template +DirectionProjectiveConstraint::DirectionProjectiveConstraint() + : core::behavior::ProjectiveConstraintSet(nullptr) + , f_indices( initData(&f_indices,"indices","Indices of the fixed points") ) + , f_drawSize( initData(&f_drawSize,(SReal)0.0,"drawSize","0 -> point based rendering, >0 -> radius of spheres") ) + , f_direction( initData(&f_direction,CPos(),"direction","Direction of the line")) + , l_topology(initLink("topology", "link to the topology container")) + , data(new DirectionProjectiveConstraintInternalData()) +{ + f_indices.beginEdit()->push_back(0); + f_indices.endEdit(); +} + + +template +DirectionProjectiveConstraint::~DirectionProjectiveConstraint() +{ + delete data; +} + +template +void DirectionProjectiveConstraint::clearConstraints() +{ + f_indices.beginEdit()->clear(); + f_indices.endEdit(); +} + +template +void DirectionProjectiveConstraint::addConstraint(Index index) +{ + f_indices.beginEdit()->push_back(index); + f_indices.endEdit(); +} + +template +void DirectionProjectiveConstraint::removeConstraint(Index index) +{ + sofa::type::removeValue(*f_indices.beginEdit(),index); + f_indices.endEdit(); +} + +// -- Constraint interface + + +template +void DirectionProjectiveConstraint::init() +{ + this->core::behavior::ProjectiveConstraintSet::init(); + + if (l_topology.empty()) + { + msg_info() << "link to Topology container should be set to ensure right behavior. First Topology found in current context will be used."; + l_topology.set(this->getContext()->getMeshTopologyLink()); + } + + + if (sofa::core::topology::BaseMeshTopology* _topology = l_topology.get()) + { + msg_info() << "Topology path used: '" << l_topology.getLinkedPath() << "'"; + + // Initialize topological changes support + f_indices.createTopologyHandler(_topology); + } + else + { + msg_info() << "No topology component found at path: " << l_topology.getLinkedPath() << ", nor in current context: " << this->getContext()->name; + } + + const Indices & indices = f_indices.getValue(); + + const Index maxIndex=this->mstate->getSize(); + for (unsigned int i=0; i= maxIndex) + { + msg_error() << "Index " << index << " not valid!"; + removeConstraint(index); + } + } + + reinit(); +} + +template +void DirectionProjectiveConstraint::reinit() +{ + // normalize the normal vector + CPos n = f_direction.getValue(); + if( n.norm()==0 ) + n[1]=0; + else n *= 1/n.norm(); + f_direction.setValue(n); + + // create the matrix blocks corresponding to the projection to the line: nn^t or to the identity + Block bProjection; + for(unsigned i=0; imstate->getSize(); + const unsigned blockSize = DataTypes::deriv_total_size; + jacobian.resize( numBlocks*blockSize,numBlocks*blockSize ); + + // fill the jacobian in ascending order + Indices::const_iterator it = tmp.begin(); + unsigned i = 0; + while( i < numBlocks ) + { + if( it != tmp.end() && i==*it ) // constrained particle: set diagonal to projection block, and the cursor to the next constraint + { + jacobian.insertBackBlock(i,i,bProjection); + ++it; + } + else // unconstrained particle: set diagonal to identity block + { + jacobian.insertBackBlock(i,i,Block::Identity()); + } + i++; + } + jacobian.compress(); + + const VecCoord& x = this->mstate->read(core::ConstVecCoordId::position())->getValue(); + const Indices &indices = f_indices.getValue(); + for (const auto id : indices) + { + m_origin.push_back(DataTypes::getCPos(x[id])); + } + +} + +template +void DirectionProjectiveConstraint::projectMatrix( sofa::linearalgebra::BaseMatrix* M, unsigned offset ) +{ + J.copy(jacobian, M->colSize(), offset); // projection matrix for an assembled state + BaseSparseMatrix* E = dynamic_cast(M); + assert(E); + E->compressedMatrix = J.compressedMatrix * E->compressedMatrix * J.compressedMatrix; +} + + + +template +void DirectionProjectiveConstraint::projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& resData) +{ + SOFA_UNUSED(mparams); + + helper::WriteAccessor res ( resData ); + jacobian.mult(res.wref(),res.ref()); +} + +template +void DirectionProjectiveConstraint::projectJacobianMatrix(const core::MechanicalParams* /*mparams*/ , DataMatrixDeriv& /*cData*/) +{ + msg_error() << "projectJacobianMatrix(const core::MechanicalParams*, DataMatrixDeriv& ) is not implemented"; +} + +template +void DirectionProjectiveConstraint::projectVelocity(const core::MechanicalParams* mparams, DataVecDeriv& vData) +{ + projectResponse(mparams,vData); +} + +template +void DirectionProjectiveConstraint::projectPosition(const core::MechanicalParams* /*mparams*/ , DataVecCoord& xData) +{ + VecCoord& x = *xData.beginEdit(); + + const CPos& n = f_direction.getValue(); + + const Indices& indices = f_indices.getValue(); + for(unsigned i=0; i +void DirectionProjectiveConstraint::applyConstraint(const core::MechanicalParams* /*mparams*/, const sofa::core::behavior::MultiMatrixAccessor* /*matrix*/) +{ + msg_error() << "applyConstraint is not implemented"; +} + +template +void DirectionProjectiveConstraint::applyConstraint(const core::MechanicalParams* /*mparams*/, linearalgebra::BaseVector* /*vector*/, const sofa::core::behavior::MultiMatrixAccessor* /*matrix*/) +{ + dmsg_error() << "DirectionProjectiveConstraint::applyConstraint(const core::MechanicalParams* mparams, linearalgebra::BaseVector* vector, const sofa::core::behavior::MultiMatrixAccessor* matrix) is not implemented"; +} + + + + +template +void DirectionProjectiveConstraint::draw(const core::visual::VisualParams* vparams) +{ + if (!vparams->displayFlags().getShowBehaviorModels()) return; + if (!this->isActive()) return; + const VecCoord& x = this->mstate->read(core::ConstVecCoordId::position())->getValue(); + + const auto stateLifeCycle = vparams->drawTool()->makeStateLifeCycle(); + + const Indices & indices = f_indices.getValue(); + + if( f_drawSize.getValue() == 0) // old classical drawing by points + { + std::vector< sofa::type::Vec3 > points; + sofa::type::Vec3 point; + for (unsigned int index : indices) + { + point = DataTypes::getCPos(x[index]); + points.push_back(point); + } + vparams->drawTool()->drawPoints(points, 10, sofa::type::RGBAColor(1,0.5,0.5,1)); + } + else // new drawing by spheres + { + std::vector< sofa::type::Vec3 > points; + sofa::type::Vec3 point; + for (unsigned int index : indices) + { + point = DataTypes::getCPos(x[index]); + points.push_back(point); + } + vparams->drawTool()->drawSpheres(points, (float)f_drawSize.getValue(), sofa::type::RGBAColor(1.0f,0.35f,0.35f,1.0f)); + } + + +} + +} // namespace sofa::component::constraint::projective diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedConstraint.h b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedConstraint.h index 7c83fb40cb8..1ddf32be894 100644 --- a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedConstraint.h +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedConstraint.h @@ -20,119 +20,13 @@ * Contact information: contact@sofa-framework.org * ******************************************************************************/ #pragma once -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include -namespace sofa::component::constraint::projective -{ - -/// This class can be overridden if needed for additionnal storage within template specializations. -template -class FixedConstraintInternalData -{ +SOFA_DEPRECATED_HEADER("v24.06", "v25.06", "sofa/component/constraint/projective/FixedProjectiveConstraint.h") -}; - - -/** Maintain a constant velocity. - * If the particle is initially fixed then it is attached to its initial position. - * Otherwise it keeps on drifting. - * For maintaining particles fixed in any case, @sa ProjectToPointConstraint -*/ -template -class FixedConstraint : public core::behavior::ProjectiveConstraintSet +namespace sofa::component::constraint::projective { -public: - SOFA_CLASS(SOFA_TEMPLATE(FixedConstraint,DataTypes),SOFA_TEMPLATE(sofa::core::behavior::ProjectiveConstraintSet, DataTypes)); - - using Index = sofa::Index; - typedef typename DataTypes::VecCoord VecCoord; - typedef typename DataTypes::VecDeriv VecDeriv; - typedef typename DataTypes::MatrixDeriv MatrixDeriv; - typedef typename DataTypes::Coord Coord; - typedef typename DataTypes::Deriv Deriv; - typedef typename MatrixDeriv::RowIterator MatrixDerivRowIterator; - typedef typename MatrixDeriv::RowType MatrixDerivRowType; - typedef Data DataVecCoord; - typedef Data DataVecDeriv; - typedef Data DataMatrixDeriv; - typedef type::vector SetIndexArray; - typedef sofa::core::topology::TopologySubsetIndices SetIndex; - typedef sofa::core::topology::Point Point; - - SOFA_ATTRIBUTE_REPLACED__TYPEMEMBER(Vec3, sofa::type::Vec3); -protected: - FixedConstraint(); - - virtual ~FixedConstraint(); - -public: - SetIndex d_indices; - Data d_fixAll; ///< filter all the DOF to implement a fixed object - Data d_showObject; ///< draw or not the fixed constraints - Data d_drawSize; ///< 0 -> point based rendering, >0 -> radius of spheres - Data d_projectVelocity; ///< activate project velocity to set velocity to zero - - /// Link to be set to the topology container in the component graph. - SingleLink, sofa::core::topology::BaseMeshTopology, BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_topology; -protected: - FixedConstraintInternalData* data; - friend class FixedConstraintInternalData; - - -public: - void clearConstraints(); - void addConstraint(Index index); - void removeConstraint(Index index); - - // -- Constraint interface - void init() override; - void reinit() override; - - void projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& resData) override; - void projectVelocity(const core::MechanicalParams* mparams, DataVecDeriv& vData) override; - void projectPosition(const core::MechanicalParams* mparams, DataVecCoord& xData) override; - void projectJacobianMatrix(const core::MechanicalParams* mparams, DataMatrixDeriv& cData) override; - - - void applyConstraint(const core::MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) override; - void applyConstraint(const core::MechanicalParams* mparams, linearalgebra::BaseVector* vect, const sofa::core::behavior::MultiMatrixAccessor* matrix) override; - - /** Project the given matrix (Experimental API). - See doc in base parent class - */ - void projectMatrix( sofa::linearalgebra::BaseMatrix* /*M*/, unsigned /*offset*/ ) override; - - void applyConstraint(sofa::core::behavior::ZeroDirichletCondition* matrix) override; - - void draw(const core::visual::VisualParams* vparams) override; - - bool fixAllDOFs() const { return d_fixAll.getValue(); } - -protected : - /// Function check values of given indices - void checkIndices(); - -}; - -#if !defined(SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_FIXEDCONSTRAINT_CPP) -extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API FixedConstraint; -extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API FixedConstraint; -extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API FixedConstraint; -extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API FixedConstraint; -extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API FixedConstraint; -extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API FixedConstraint; - -#endif - -} // namespace sofa::component::constraint::projective +template +using FixedConstraint SOFA_ATTRIBUTE_DEPRECATED("v24.06 ", "v25.06", "FixedConstraint has been renamed to FixedProjectiveConstraint") = FixedProjectiveConstraint; +} \ No newline at end of file diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedConstraint.inl b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedConstraint.inl index 23fcc61269d..4244fcd3b94 100644 --- a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedConstraint.inl +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedConstraint.inl @@ -21,406 +21,6 @@ ******************************************************************************/ #pragma once -#include -#include -#include -#include -#include +#include -using sofa::core::objectmodel::ComponentState; - - -namespace sofa::component::constraint::projective -{ - -template -FixedConstraint::FixedConstraint() - : core::behavior::ProjectiveConstraintSet(nullptr) - , d_indices( initData(&d_indices,"indices","Indices of the fixed points") ) - , d_fixAll( initData(&d_fixAll,false,"fixAll","filter all the DOF to implement a fixed object") ) - , d_showObject(initData(&d_showObject,true,"showObject","draw or not the fixed constraints")) - , d_drawSize( initData(&d_drawSize,(SReal)0.0,"drawSize","0 -> point based rendering, >0 -> radius of spheres") ) - , d_projectVelocity( initData(&d_projectVelocity,false,"activate_projectVelocity","activate project velocity to set velocity") ) - , l_topology(initLink("topology", "link to the topology container")) - , data(new FixedConstraintInternalData()) -{ - // default to indice 0 - d_indices.beginEdit()->push_back(0); - d_indices.endEdit(); - - this->addUpdateCallback("updateIndices", { &d_indices}, [this](const core::DataTracker& t) - { - SOFA_UNUSED(t); - checkIndices(); - return sofa::core::objectmodel::ComponentState::Valid; - }, {}); -} - - -template -FixedConstraint::~FixedConstraint() -{ - delete data; -} - -template -void FixedConstraint::clearConstraints() -{ - d_indices.beginEdit()->clear(); - d_indices.endEdit(); -} - -template -void FixedConstraint::addConstraint(Index index) -{ - d_indices.beginEdit()->push_back(index); - d_indices.endEdit(); -} - -template -void FixedConstraint::removeConstraint(Index index) -{ - sofa::type::removeValue(*d_indices.beginEdit(),index); - d_indices.endEdit(); -} - -// -- Constraint interface - - -template -void FixedConstraint::init() -{ - this->d_componentState.setValue(ComponentState::Invalid); - this->core::behavior::ProjectiveConstraintSet::init(); - - if (!this->mstate.get()) - { - msg_warning() << "Missing mstate, cannot initialize the component."; - return; - } - - if (l_topology.empty()) - { - msg_info() << "link to Topology container should be set to ensure right behavior. First Topology found in current context will be used."; - l_topology.set(this->getContext()->getMeshTopologyLink()); - } - - if (sofa::core::topology::BaseMeshTopology* _topology = l_topology.get()) - { - msg_info() << "Topology path used: '" << l_topology.getLinkedPath() << "'"; - - // Initialize topological changes support - d_indices.createTopologyHandler(_topology); - } - else - { - msg_info() << "Can not find the topology, won't be able to handle topological changes"; - } - - this->checkIndices(); - this->d_componentState.setValue(ComponentState::Valid); -} - -template -void FixedConstraint::reinit() -{ - this->checkIndices(); -} - -template -void FixedConstraint::checkIndices() -{ - // Check value of given indices - Index maxIndex=this->mstate->getSize(); - - const SetIndexArray & indices = d_indices.getValue(); - SetIndexArray invalidIndices; - for (unsigned int i=0; i= maxIndex) - { - msg_warning() << "Index " << index << " not valid, should be [0,"<< maxIndex <<"]. Constraint will be removed."; - invalidIndices.push_back(index); - } - } - - // if invalid indices, sort them and remove in decreasing order as removeConstraint perform a swap and pop_back. - if (!invalidIndices.empty()) - { - std::sort( invalidIndices.begin(), invalidIndices.end(), std::greater() ); - const int max = invalidIndices.size()-1; - for (int i=max; i>= 0; i--) - { - removeConstraint(invalidIndices[i]); - } - } -} - -template -void FixedConstraint::projectMatrix( sofa::linearalgebra::BaseMatrix* M, unsigned offset ) -{ - static const unsigned blockSize = DataTypes::deriv_total_size; - - if( d_fixAll.getValue() ) - { - const unsigned size = this->mstate->getSize(); - for( unsigned i=0; iclearRowsCols( offset + i * blockSize, offset + (i+1) * (blockSize) ); - } - } - else - { - // clears the rows and columns associated with fixed particles - for (const auto id : d_indices.getValue()) - { - M->clearRowsCols( offset + id * blockSize, offset + (id+1) * blockSize ); - } - } -} - - -template -void FixedConstraint::projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& resData) -{ - SOFA_UNUSED(mparams); - - helper::WriteAccessor res (resData ); - const SetIndexArray & indices = d_indices.getValue(); - - if( d_fixAll.getValue() ) - { - // fix everything - typename VecDeriv::iterator it; - for( it = res.begin(); it != res.end(); ++it ) - { - *it = Deriv(); - } - } - else - { - for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) - { - res[*it] = Deriv(); - } - } -} - -template -void FixedConstraint::projectJacobianMatrix(const core::MechanicalParams* mparams, DataMatrixDeriv& cData) -{ - SOFA_UNUSED(mparams); - - helper::WriteAccessor c (cData ); - - if( d_fixAll.getValue() ) - { - // fix everything - c->clear(); - } - else - { - const SetIndexArray& indices = d_indices.getValue(); - for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) - { - c->clearColBlock(*it); - } - } -} - -// projectVelocity applies the same changes on velocity vector as projectResponse on position vector : -// Each fixed point received a null velocity vector. -// When a new fixed point is added while its velocity vector is already null, projectVelocity is not usefull. -// But when a new fixed point is added while its velocity vector is not null, it's necessary to fix it to null or -// to set the projectVelocity option to True. If not, the fixed point is going to drift. -template -void FixedConstraint::projectVelocity(const core::MechanicalParams* mparams, DataVecDeriv& vData) -{ - SOFA_UNUSED(mparams); - - if(!d_projectVelocity.getValue()) return; - - helper::WriteAccessor res (vData ); - - if ( d_fixAll.getValue() ) // fix everyting - { - for(Size i=0; id_indices.getValue(); - for(Index ind : indices) - { - res[ind] = Deriv(); - } - } -} - - -template -void FixedConstraint::projectPosition(const core::MechanicalParams* /*mparams*/, DataVecCoord& /*xData*/) -{ - -} - -// Matrix Integration interface -template -void FixedConstraint::applyConstraint(const core::MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) -{ - SOFA_UNUSED(mparams); - if(const core::behavior::MultiMatrixAccessor::MatrixRef r = matrix->getMatrix(this->mstate.get())) - { - const unsigned int N = Deriv::size(); - - if( d_fixAll.getValue() ) - { - const unsigned size = this->mstate->getSize(); - for(unsigned int i=0; iclearRowCol(r.offset + N * i + c); - // Set Fixed Vertex - for (unsigned int c=0; cset(r.offset + N * i + c, r.offset + N * i + c, 1.0); - } - } - else - { - const SetIndexArray & indices = d_indices.getValue(); - - for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) - { - // Reset Fixed Row and Col - for (unsigned int c=0; cclearRowCol(r.offset + N * (*it) + c); - // Set Fixed Vertex - for (unsigned int c=0; cset(r.offset + N * (*it) + c, r.offset + N * (*it) + c, 1.0); - } - } - } -} - -template -void FixedConstraint::applyConstraint(const core::MechanicalParams* mparams, linearalgebra::BaseVector* vect, const sofa::core::behavior::MultiMatrixAccessor* matrix) -{ - SOFA_UNUSED(mparams); - const int o = matrix->getGlobalOffset(this->mstate.get()); - if (o >= 0) - { - const unsigned int offset = (unsigned int)o; - const unsigned int N = Deriv::size(); - - if( d_fixAll.getValue() ) - { - for(sofa::Size i=0; i < (sofa::Size) vect->size(); i++ ) - { - for (unsigned int c=0; cclear(offset + N * i + c); - } - } - else - { - const SetIndexArray & indices = d_indices.getValue(); - for (const auto & index : indices) - { - for (unsigned int c=0; cclear(offset + N * index + c); - } - } - } -} - -template -void FixedConstraint::applyConstraint(sofa::core::behavior::ZeroDirichletCondition* matrix) -{ - static constexpr unsigned int N = Deriv::size(); - - if( d_fixAll.getValue() ) - { - const sofa::Size size = this->mstate->getMatrixSize(); - for(sofa::Index i = 0; i < size; ++i) - { - matrix->discardRowCol(i, i); - } - } - else - { - const SetIndexArray & indices = d_indices.getValue(); - - for (const auto index : indices) - { - for (unsigned int c = 0; c < N; ++c) - { - matrix->discardRowCol(N * index + c, N * index + c); - } - } - } -} - -template -void FixedConstraint::draw(const core::visual::VisualParams* vparams) -{ - if (this->d_componentState.getValue() != ComponentState::Valid) return; - if (!vparams->displayFlags().getShowBehaviorModels()) return; - if (!d_showObject.getValue()) return; - if (!this->isActive()) return; - - const auto stateLifeCycle = vparams->drawTool()->makeStateLifeCycle(); - - const VecCoord& x = this->mstate->read(core::ConstVecCoordId::position())->getValue(); - const SetIndexArray & indices = d_indices.getValue(); - - if( d_drawSize.getValue() == 0) // old classical drawing by points - { - std::vector< sofa::type::Vec3 > points; - sofa::type::Vec3 point; - - if (d_fixAll.getValue()) - { - for (unsigned i = 0; i < x.size(); i++) - { - point = DataTypes::getCPos(x[i]); - points.push_back(point); - } - } - else - { - for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) - { - point = DataTypes::getCPos(x[*it]); - points.push_back(point); - } - } - vparams->drawTool()->drawPoints(points, 10, sofa::type::RGBAColor(1,0.5,0.5,1)); - } - else // new drawing by spheres - { - vparams->drawTool()->setLightingEnabled(true); - - std::vector< sofa::type::Vec3 > points; - sofa::type::Vec3 point; - if( d_fixAll.getValue()==true ) - for (unsigned i=0; idrawTool()->drawSpheres(points, (float)d_drawSize.getValue(), sofa::type::RGBAColor(1.0f,0.35f,0.35f,1.0f)); - } - - -} - -} // namespace sofa::component::constraint::projective +SOFA_DEPRECATED_HEADER("v24.06", "v25.06", "sofa/component/constraint/projective/FixedProjectiveConstraint.inl") diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedPlaneConstraint.h b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedPlaneConstraint.h index d6b7e177dd0..8848ba55069 100644 --- a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedPlaneConstraint.h +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedPlaneConstraint.h @@ -20,117 +20,13 @@ * Contact information: contact@sofa-framework.org * ******************************************************************************/ #pragma once -#include -#include +#include -#include -#include -#include -#include +SOFA_DEPRECATED_HEADER("v24.06", "v25.06", "sofa/component/constraint/projective/FixedPlaneProjectiveConstraint.h") namespace sofa::component::constraint::projective { - -using sofa::linearalgebra::BaseVector; -using sofa::core::MechanicalParams; -using sofa::core::visual::VisualParams; -using sofa::core::topology::BaseMeshTopology; -using sofa::core::behavior::MultiMatrixAccessor; -using sofa::core::behavior::ProjectiveConstraintSet; - -/// This class can be overriden if needed for additionnal storage within template specilizations. -template -class FixedPlaneConstraintInternalData -{ -}; - -template -class FixedPlaneConstraint : public ProjectiveConstraintSet -{ -public: - SOFA_CLASS(SOFA_TEMPLATE(FixedPlaneConstraint,DataTypes), - SOFA_TEMPLATE(ProjectiveConstraintSet, DataTypes)); - - using Index = sofa::Index; - typedef typename DataTypes::VecCoord VecCoord; - typedef typename DataTypes::VecDeriv VecDeriv; - typedef typename DataTypes::MatrixDeriv MatrixDeriv; - typedef typename DataTypes::Coord Coord; - typedef typename DataTypes::Deriv Deriv; - typedef typename Coord::value_type Real; - typedef typename MatrixDeriv::RowIterator MatrixDerivRowIterator; - typedef typename MatrixDeriv::RowType MatrixDerivRowType; - typedef Data DataVecCoord; - typedef Data DataVecDeriv; - typedef Data DataMatrixDeriv; - typedef type::vector SetIndexArray; - typedef core::topology::TopologySubsetIndices SetIndex; -public: - Data d_direction; ///< direction on which the constraint applied - Data d_dmin; ///< coordinates min of the plane for the vertex selection - Data d_dmax; ///< coordinates max of the plane for the vertex selection - SetIndex d_indices; ///< the set of vertex indices - - /// Link to be set to the topology container in the component graph. - SingleLink, sofa::core::topology::BaseMeshTopology, BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_topology; - - - /// inherited from the BaseObject interface - void init() override; - void draw(const VisualParams* vparams) override; - - /// -- Constraint interface - void projectResponse(const MechanicalParams* mparams, DataVecDeriv& resData) override; - void projectVelocity(const MechanicalParams* mparams, DataVecDeriv& vData) override; - void projectPosition(const MechanicalParams* mparams, DataVecCoord& xData) override; - - /// Implement projectMatrix for assembled solver of compliant - void projectMatrix( sofa::linearalgebra::BaseMatrix* M, unsigned offset) override; - void projectJacobianMatrix(const MechanicalParams* mparams, DataMatrixDeriv& cData) override; - - /// Implement applyConstraint for direct solvers - void applyConstraint(const MechanicalParams* mparams, - const MultiMatrixAccessor* matrix) override; - - void applyConstraint(const MechanicalParams* mparams, BaseVector* vect, - const MultiMatrixAccessor* matrix) override; - - void applyConstraint(sofa::core::behavior::ZeroDirichletCondition* matrix) override; - - void setDirection (Coord dir); - void selectVerticesAlongPlane(); - void setDminAndDmax(const Real _dmin,const Real _dmax); - - void addConstraint(Index index); - void removeConstraint(Index index); - -protected: - FixedPlaneConstraint(); - ~FixedPlaneConstraint(); - - FixedPlaneConstraintInternalData data; - friend class FixedPlaneConstraintInternalData; - - /// whether vertices should be selected from 2 parallel planes - bool m_selectVerticesFromPlanes {false}; - - ////////////////////////// Inherited attributes //////////////////////////// - /// https://gcc.gnu.org/onlinedocs/gcc/Name-lookup.html - /// Bring inherited attributes and function in the current lookup context. - /// otherwise any access to the base::attribute would require - /// the "this->" approach. - using ProjectiveConstraintSet::mstate; - using ProjectiveConstraintSet::getContext; - - bool isPointInPlane(const Coord& p) const ; -}; - -#if !defined(SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_FIXEDPLANECONSTRAINT_CPP) -extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API FixedPlaneConstraint; -extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API FixedPlaneConstraint; -extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API FixedPlaneConstraint; - -#endif - -} // namespace sofa::component::constraint::projective +template +using FixedPlaneConstraint SOFA_ATTRIBUTE_DEPRECATED("v24.06 ", "v25.06", "FixedPlaneConstraint has been renamed to FixedPlaneProjectiveConstraint") = FixedPlaneProjectiveConstraint; +} \ No newline at end of file diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedPlaneConstraint.inl b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedPlaneConstraint.inl index 9c8dad30165..34acfcbe022 100644 --- a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedPlaneConstraint.inl +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedPlaneConstraint.inl @@ -21,297 +21,6 @@ ******************************************************************************/ #pragma once -#include -#include -#include -#include -#include -#include -#include -#include +#include -namespace sofa::component::constraint::projective -{ - -using sofa::helper::WriteAccessor; -using sofa::type::Vec; - -/////////////////////////// DEFINITION OF FixedPlaneConstraint ///////////////////////////////////// -template -FixedPlaneConstraint::FixedPlaneConstraint() - : d_direction( initData(&d_direction,"direction","normal direction of the plane")) - , d_dmin( initData(&d_dmin,(Real)0,"dmin","Minimum plane distance from the origin")) - , d_dmax( initData(&d_dmax,(Real)0,"dmax","Maximum plane distance from the origin") ) - , d_indices( initData(&d_indices,"indices","Indices of the fixed points")) - , l_topology(initLink("topology", "link to the topology container")) -{ - m_selectVerticesFromPlanes=false; -} - -template -FixedPlaneConstraint::~FixedPlaneConstraint() -{ - -} - -/// Matrix Integration interface -template -void FixedPlaneConstraint::applyConstraint(const MechanicalParams* mparams, const MultiMatrixAccessor* matrix) -{ - SOFA_UNUSED(mparams); - if(const MultiMatrixAccessor::MatrixRef r = matrix->getMatrix(mstate.get())) - { - /// Implement plane constraint only when the direction is along the coordinates directions - // TODO : generalize projection to any direction - - const unsigned int N = Deriv::size(); - Coord dir=d_direction.getValue(); - for (auto& index : d_indices.getValue()) - { - /// Reset Fixed Row and Col - for (unsigned int c=0; cclearRowCol(r.offset + N * index + c); - /// Set Fixed Vertex - for (unsigned int c=0; cset(r.offset + N * index + c, r.offset + N * index + c, 1.0); - } - } -} - -template -void FixedPlaneConstraint::applyConstraint(const MechanicalParams* mparams, - BaseVector* vect, - const MultiMatrixAccessor* matrix) -{ - SOFA_UNUSED(mparams); - const int o = matrix->getGlobalOffset(mstate.get()); - if (o >= 0) - { - const unsigned int offset = (unsigned int)o; - /// Implement plane constraint only when the direction is along the coordinates directions - // TODO : generalize projection to any direction - Coord dir=d_direction.getValue(); - - const unsigned int N = Deriv::size(); - - for (auto& index : d_indices.getValue()) - { - for (unsigned int c=0; cclear(offset + N * index + c); - } - } -} - -template -void FixedPlaneConstraint::applyConstraint( - sofa::core::behavior::ZeroDirichletCondition* matrix) -{ - static constexpr unsigned int N = Deriv::size(); - const Coord dir = d_direction.getValue(); - for (auto& index : d_indices.getValue()) - { - for (unsigned int c=0; cdiscardRowCol(N * index + c, N * index + c); - } - } - } -} - -template -void FixedPlaneConstraint::addConstraint(Index index) -{ - d_indices.beginEdit()->push_back(index); - d_indices.endEdit(); -} - -template -void FixedPlaneConstraint::removeConstraint(Index index) -{ - sofa::type::removeValue(*d_indices.beginEdit(),(unsigned int)index); - d_indices.endEdit(); -} - - -/// This function are there to provide kind of type translation to the vector one so we can -/// implement the algorithm as is the different objects where of similar type. -/// this solution is not really satisfactory but for the moment it does the job. -/// A better solution would that all the used types are following the same interface which -/// requires to touch core sofa classes. -sofa::type::Vec3d& getVec(sofa::defaulttype::Rigid3dTypes::Deriv& i) { return i.getVCenter(); } -sofa::type::Vec3d& getVec(sofa::defaulttype::Rigid3dTypes::Coord& i) { return i.getCenter(); } -const sofa::type::Vec3d& getVec(const sofa::defaulttype::Rigid3dTypes::Deriv& i) { return i.getVCenter(); } -const sofa::type::Vec3d& getVec(const sofa::defaulttype::Rigid3dTypes::Coord& i) { return i.getCenter(); } -sofa::type::Vec3d& getVec(sofa::defaulttype::Vec3dTypes::Deriv& i) { return i; } -const sofa::type::Vec3d& getVec(const sofa::defaulttype::Vec3dTypes::Deriv& i) { return i; } -sofa::type::Vec6d& getVec(sofa::defaulttype::Vec6dTypes::Deriv& i) { return i; } -const sofa::type::Vec6d& getVec(const sofa::defaulttype::Vec6dTypes::Deriv& i) { return i; } - -sofa::type::Vec3f& getVec(sofa::defaulttype::Rigid3fTypes::Deriv& i) { return i.getVCenter(); } -sofa::type::Vec3f& getVec(sofa::defaulttype::Rigid3fTypes::Coord& i) { return i.getCenter(); } -const sofa::type::Vec3f& getVec(const sofa::defaulttype::Rigid3fTypes::Deriv& i) { return i.getVCenter(); } -const sofa::type::Vec3f& getVec(const sofa::defaulttype::Rigid3fTypes::Coord& i) { return i.getCenter(); } -sofa::type::Vec3f& getVec(sofa::defaulttype::Vec3fTypes::Deriv& i) { return i; } -const sofa::type::Vec3f& getVec(const sofa::defaulttype::Vec3fTypes::Deriv& i) { return i; } -sofa::type::Vec6f& getVec(sofa::defaulttype::Vec6fTypes::Deriv& i) { return i; } -const sofa::type::Vec6f& getVec(const sofa::defaulttype::Vec6fTypes::Deriv& i) { return i; } - -template -void FixedPlaneConstraint::projectResponse(const MechanicalParams* mparams, DataVecDeriv& resData) -{ - SOFA_UNUSED(mparams); - WriteAccessor res = resData; - - const auto& indices = d_indices.getValue(); - const auto& dir = getVec(d_direction.getValue()); - auto& dx = res.wref(); - for (const auto& i : indices) - { - /// only constraint one projection of the displacement to be zero - auto val = getVec(dx[i]); - val = val - (dir * dot(val, dir)); - DataTypes::setDPos(dx[i], val); - } -} - -/// project dx to constrained space (dx models a velocity) -template -void FixedPlaneConstraint::projectVelocity(const MechanicalParams* /*mparams*/, DataVecDeriv& /*vData*/) -{ - -} - -/// project x to constrained space (x models a position) -template -void FixedPlaneConstraint::projectPosition(const MechanicalParams* /*mparams*/, DataVecCoord& /*xData*/) -{ - -} - -template -void FixedPlaneConstraint::projectMatrix( sofa::linearalgebra::BaseMatrix* M, unsigned /*offset*/ ) -{ - /// clears the rows and columns associated with constrained particles - const unsigned blockSize = DataTypes::deriv_total_size; - - for(auto& index : d_indices.getValue()) - { - M->clearRowsCols((index) * blockSize,(index+1) * (blockSize) ); - } -} - -template -void FixedPlaneConstraint::projectJacobianMatrix(const MechanicalParams* mparams, DataMatrixDeriv& cData) -{ - SOFA_UNUSED(mparams); - WriteAccessor c = cData; - - const auto& indices = d_indices.getValue(); - const auto& dir = getVec(d_direction.getValue()); - auto& dx = c.wref(); - - auto itRow = dx.begin(); - auto itRowEnd = dx.end(); - while (itRow != itRowEnd) - { - for (auto colIt = itRow.begin(); colIt != itRow.end(); colIt++) - { - if (std::find(indices.begin(), indices.end(), colIt.index()) != indices.end()) - { - auto val = getVec(colIt.val()); - Deriv r(type::NOINIT); - DataTypes::setDPos(r, -(dir * dot(val, dir))); - dx.writeLine(itRow.index()).addCol(colIt.index(), r); - } - } - } -} - -template -void FixedPlaneConstraint::setDirection(Coord dir) -{ - if (dir.norm2()>0) - { - d_direction.setValue(dir); - } -} - -template -void FixedPlaneConstraint::selectVerticesAlongPlane() -{ - const VecCoord& x = mstate->read(core::ConstVecCoordId::position())->getValue(); - for(unsigned int i=0; i -void FixedPlaneConstraint::init() -{ - ProjectiveConstraintSet::init(); - - if (l_topology.empty()) - { - msg_info() << "link to Topology container should be set to ensure right behavior. First Topology found in current context will be used."; - l_topology.set(this->getContext()->getMeshTopologyLink()); - } - - if (sofa::core::topology::BaseMeshTopology* _topology = l_topology.get()) - { - msg_info() << "Topology path used: '" << l_topology.getLinkedPath() << "'"; - - // Initialize topological changes support - d_indices.createTopologyHandler(_topology); - } - else - { - msg_info() << "No topology component found at path: " << l_topology.getLinkedPath() << ", nor in current context: " << this->getContext()->name; - } - - /// test that dmin or dmax are different from zero - if (d_dmin.getValue()!=d_dmax.getValue()) - m_selectVerticesFromPlanes=true; - - if (m_selectVerticesFromPlanes) - selectVerticesAlongPlane(); -} - -template -void FixedPlaneConstraint::setDminAndDmax(const Real _dmin,const Real _dmax) -{ - d_dmin=_dmin; - d_dmax=_dmax; - m_selectVerticesFromPlanes=true; -} - -template -void FixedPlaneConstraint::draw(const VisualParams* vparams) -{ - if (!vparams->displayFlags().getShowBehaviorModels()) return; - const VecCoord& x = mstate->read(core::ConstVecCoordId::position())->getValue(); - vparams->drawTool()->disableLighting(); - - type::vector points; - for(auto& index : d_indices.getValue()) - { - points.push_back({x[index][0], x[index][1], x[index][2]}); - } - - vparams->drawTool()->drawPoints(points, 10, sofa::type::RGBAColor{1,1.0,0.5,1}); -} - -template -bool FixedPlaneConstraint::isPointInPlane(const Coord& p) const -{ - const Real d = getVec(p) * getVec(d_direction.getValue()); - return d > d_dmin.getValue() && d < d_dmax.getValue(); -} - -} // namespace sofa::component::constraint::projective +SOFA_DEPRECATED_HEADER("v24.06", "v25.06", "sofa/component/constraint/projective/FixedPlaneProjectiveConstraint.inl") diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedPlaneConstraint.cpp b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedPlaneProjectiveConstraint.cpp similarity index 77% rename from Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedPlaneConstraint.cpp rename to Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedPlaneProjectiveConstraint.cpp index 725d791f612..14dd5857dad 100644 --- a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedPlaneConstraint.cpp +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedPlaneProjectiveConstraint.cpp @@ -19,8 +19,8 @@ * * * Contact information: contact@sofa-framework.org * ******************************************************************************/ -#define SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_FIXEDPLANECONSTRAINT_CPP -#include +#define SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_FIXEDPLANEPROJECTIVECONSTRAINT_CPP +#include #include #include #include @@ -32,15 +32,16 @@ using namespace sofa::defaulttype; using namespace sofa::helper; -int FixedPlaneConstraintClass = core::RegisterObject("Project particles on a given plane") - .add< FixedPlaneConstraint >() - .add< FixedPlaneConstraint >() - .add< FixedPlaneConstraint >() +int FixedPlaneProjectiveConstraintClass = core::RegisterObject("Project particles on a given plane") + .add< FixedPlaneProjectiveConstraint >() + .add< FixedPlaneProjectiveConstraint >() + .add< FixedPlaneProjectiveConstraint >() + .addAlias("FixedPlaneConstraint") ; -template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API FixedPlaneConstraint; -template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API FixedPlaneConstraint; -template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API FixedPlaneConstraint; +template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API FixedPlaneProjectiveConstraint; +template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API FixedPlaneProjectiveConstraint; +template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API FixedPlaneProjectiveConstraint; } // namespace sofa::component::constraint::projective diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedPlaneProjectiveConstraint.h b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedPlaneProjectiveConstraint.h new file mode 100644 index 00000000000..6885db83f9e --- /dev/null +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedPlaneProjectiveConstraint.h @@ -0,0 +1,135 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once +#include + +#include + +#include +#include +#include +#include + +namespace sofa::component::constraint::projective +{ + +using sofa::linearalgebra::BaseVector; +using sofa::core::MechanicalParams; +using sofa::core::visual::VisualParams; +using sofa::core::topology::BaseMeshTopology; +using sofa::core::behavior::MultiMatrixAccessor; +using sofa::core::behavior::ProjectiveConstraintSet; + +/// This class can be overriden if needed for additionnal storage within template specilizations. +template +class FixedPlaneProjectiveConstraintInternalData +{ +}; + +template +class FixedPlaneProjectiveConstraint : public ProjectiveConstraintSet +{ +public: + SOFA_CLASS(SOFA_TEMPLATE(FixedPlaneProjectiveConstraint,DataTypes), + SOFA_TEMPLATE(ProjectiveConstraintSet, DataTypes)); + + using Index = sofa::Index; + typedef typename DataTypes::VecCoord VecCoord; + typedef typename DataTypes::VecDeriv VecDeriv; + typedef typename DataTypes::MatrixDeriv MatrixDeriv; + typedef typename DataTypes::Coord Coord; + typedef typename DataTypes::Deriv Deriv; + typedef typename Coord::value_type Real; + typedef typename MatrixDeriv::RowIterator MatrixDerivRowIterator; + typedef typename MatrixDeriv::RowType MatrixDerivRowType; + typedef Data DataVecCoord; + typedef Data DataVecDeriv; + typedef Data DataMatrixDeriv; + typedef type::vector SetIndexArray; + typedef core::topology::TopologySubsetIndices SetIndex; +public: + Data d_direction; ///< direction on which the constraint applied + Data d_dmin; ///< coordinates min of the plane for the vertex selection + Data d_dmax; ///< coordinates max of the plane for the vertex selection + SetIndex d_indices; ///< the set of vertex indices + + /// Link to be set to the topology container in the component graph. + SingleLink, sofa::core::topology::BaseMeshTopology, BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_topology; + + + /// inherited from the BaseObject interface + void init() override; + void draw(const VisualParams* vparams) override; + + /// -- Constraint interface + void projectResponse(const MechanicalParams* mparams, DataVecDeriv& resData) override; + void projectVelocity(const MechanicalParams* mparams, DataVecDeriv& vData) override; + void projectPosition(const MechanicalParams* mparams, DataVecCoord& xData) override; + + /// Implement projectMatrix for assembled solver of compliant + void projectMatrix( sofa::linearalgebra::BaseMatrix* M, unsigned offset) override; + void projectJacobianMatrix(const MechanicalParams* mparams, DataMatrixDeriv& cData) override; + + /// Implement applyConstraint for direct solvers + void applyConstraint(const MechanicalParams* mparams, + const MultiMatrixAccessor* matrix) override; + + void applyConstraint(const MechanicalParams* mparams, BaseVector* vect, + const MultiMatrixAccessor* matrix) override; + + void applyConstraint(sofa::core::behavior::ZeroDirichletCondition* matrix) override; + + void setDirection (Coord dir); + void selectVerticesAlongPlane(); + void setDminAndDmax(const Real _dmin,const Real _dmax); + + void addConstraint(Index index); + void removeConstraint(Index index); + +protected: + FixedPlaneProjectiveConstraint(); + ~FixedPlaneProjectiveConstraint(); + + FixedPlaneProjectiveConstraintInternalData data; + friend class FixedPlaneProjectiveConstraintInternalData; + + /// whether vertices should be selected from 2 parallel planes + bool m_selectVerticesFromPlanes {false}; + + ////////////////////////// Inherited attributes //////////////////////////// + /// https://gcc.gnu.org/onlinedocs/gcc/Name-lookup.html + /// Bring inherited attributes and function in the current lookup context. + /// otherwise any access to the base::attribute would require + /// the "this->" approach. + using ProjectiveConstraintSet::mstate; + using ProjectiveConstraintSet::getContext; + + bool isPointInPlane(const Coord& p) const ; +}; + +#if !defined(SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_FIXEDPLANEPROJECTIVECONSTRAINT_CPP) +extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API FixedPlaneProjectiveConstraint; +extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API FixedPlaneProjectiveConstraint; +extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API FixedPlaneProjectiveConstraint; +#endif + +} // namespace sofa::component::constraint::projective diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedPlaneProjectiveConstraint.inl b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedPlaneProjectiveConstraint.inl new file mode 100644 index 00000000000..49b29b03583 --- /dev/null +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedPlaneProjectiveConstraint.inl @@ -0,0 +1,317 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace sofa::component::constraint::projective +{ + +using sofa::helper::WriteAccessor; +using sofa::type::Vec; + +/////////////////////////// DEFINITION OF FixedPlaneProjectiveConstraint ///////////////////////////////////// +template +FixedPlaneProjectiveConstraint::FixedPlaneProjectiveConstraint() + : d_direction( initData(&d_direction,"direction","normal direction of the plane")) + , d_dmin( initData(&d_dmin,(Real)0,"dmin","Minimum plane distance from the origin")) + , d_dmax( initData(&d_dmax,(Real)0,"dmax","Maximum plane distance from the origin") ) + , d_indices( initData(&d_indices,"indices","Indices of the fixed points")) + , l_topology(initLink("topology", "link to the topology container")) +{ + m_selectVerticesFromPlanes=false; +} + +template +FixedPlaneProjectiveConstraint::~FixedPlaneProjectiveConstraint() +{ + +} + +/// Matrix Integration interface +template +void FixedPlaneProjectiveConstraint::applyConstraint(const MechanicalParams* mparams, const MultiMatrixAccessor* matrix) +{ + SOFA_UNUSED(mparams); + if(const MultiMatrixAccessor::MatrixRef r = matrix->getMatrix(mstate.get())) + { + /// Implement plane constraint only when the direction is along the coordinates directions + // TODO : generalize projection to any direction + + const unsigned int N = Deriv::size(); + Coord dir=d_direction.getValue(); + for (auto& index : d_indices.getValue()) + { + /// Reset Fixed Row and Col + for (unsigned int c=0; cclearRowCol(r.offset + N * index + c); + /// Set Fixed Vertex + for (unsigned int c=0; cset(r.offset + N * index + c, r.offset + N * index + c, 1.0); + } + } +} + +template +void FixedPlaneProjectiveConstraint::applyConstraint(const MechanicalParams* mparams, + BaseVector* vect, + const MultiMatrixAccessor* matrix) +{ + SOFA_UNUSED(mparams); + const int o = matrix->getGlobalOffset(mstate.get()); + if (o >= 0) + { + const unsigned int offset = (unsigned int)o; + /// Implement plane constraint only when the direction is along the coordinates directions + // TODO : generalize projection to any direction + Coord dir=d_direction.getValue(); + + const unsigned int N = Deriv::size(); + + for (auto& index : d_indices.getValue()) + { + for (unsigned int c=0; cclear(offset + N * index + c); + } + } +} + +template +void FixedPlaneProjectiveConstraint::applyConstraint( + sofa::core::behavior::ZeroDirichletCondition* matrix) +{ + static constexpr unsigned int N = Deriv::size(); + const Coord dir = d_direction.getValue(); + for (auto& index : d_indices.getValue()) + { + for (unsigned int c=0; cdiscardRowCol(N * index + c, N * index + c); + } + } + } +} + +template +void FixedPlaneProjectiveConstraint::addConstraint(Index index) +{ + d_indices.beginEdit()->push_back(index); + d_indices.endEdit(); +} + +template +void FixedPlaneProjectiveConstraint::removeConstraint(Index index) +{ + sofa::type::removeValue(*d_indices.beginEdit(),(unsigned int)index); + d_indices.endEdit(); +} + + +/// This function are there to provide kind of type translation to the vector one so we can +/// implement the algorithm as is the different objects where of similar type. +/// this solution is not really satisfactory but for the moment it does the job. +/// A better solution would that all the used types are following the same interface which +/// requires to touch core sofa classes. +sofa::type::Vec3d& getVec(sofa::defaulttype::Rigid3dTypes::Deriv& i) { return i.getVCenter(); } +sofa::type::Vec3d& getVec(sofa::defaulttype::Rigid3dTypes::Coord& i) { return i.getCenter(); } +const sofa::type::Vec3d& getVec(const sofa::defaulttype::Rigid3dTypes::Deriv& i) { return i.getVCenter(); } +const sofa::type::Vec3d& getVec(const sofa::defaulttype::Rigid3dTypes::Coord& i) { return i.getCenter(); } +sofa::type::Vec3d& getVec(sofa::defaulttype::Vec3dTypes::Deriv& i) { return i; } +const sofa::type::Vec3d& getVec(const sofa::defaulttype::Vec3dTypes::Deriv& i) { return i; } +sofa::type::Vec6d& getVec(sofa::defaulttype::Vec6dTypes::Deriv& i) { return i; } +const sofa::type::Vec6d& getVec(const sofa::defaulttype::Vec6dTypes::Deriv& i) { return i; } + +sofa::type::Vec3f& getVec(sofa::defaulttype::Rigid3fTypes::Deriv& i) { return i.getVCenter(); } +sofa::type::Vec3f& getVec(sofa::defaulttype::Rigid3fTypes::Coord& i) { return i.getCenter(); } +const sofa::type::Vec3f& getVec(const sofa::defaulttype::Rigid3fTypes::Deriv& i) { return i.getVCenter(); } +const sofa::type::Vec3f& getVec(const sofa::defaulttype::Rigid3fTypes::Coord& i) { return i.getCenter(); } +sofa::type::Vec3f& getVec(sofa::defaulttype::Vec3fTypes::Deriv& i) { return i; } +const sofa::type::Vec3f& getVec(const sofa::defaulttype::Vec3fTypes::Deriv& i) { return i; } +sofa::type::Vec6f& getVec(sofa::defaulttype::Vec6fTypes::Deriv& i) { return i; } +const sofa::type::Vec6f& getVec(const sofa::defaulttype::Vec6fTypes::Deriv& i) { return i; } + +template +void FixedPlaneProjectiveConstraint::projectResponse(const MechanicalParams* mparams, DataVecDeriv& resData) +{ + SOFA_UNUSED(mparams); + WriteAccessor res = resData; + + const auto& indices = d_indices.getValue(); + const auto& dir = getVec(d_direction.getValue()); + auto& dx = res.wref(); + for (const auto& i : indices) + { + /// only constraint one projection of the displacement to be zero + auto val = getVec(dx[i]); + val = val - (dir * dot(val, dir)); + DataTypes::setDPos(dx[i], val); + } +} + +/// project dx to constrained space (dx models a velocity) +template +void FixedPlaneProjectiveConstraint::projectVelocity(const MechanicalParams* /*mparams*/, DataVecDeriv& /*vData*/) +{ + +} + +/// project x to constrained space (x models a position) +template +void FixedPlaneProjectiveConstraint::projectPosition(const MechanicalParams* /*mparams*/, DataVecCoord& /*xData*/) +{ + +} + +template +void FixedPlaneProjectiveConstraint::projectMatrix( sofa::linearalgebra::BaseMatrix* M, unsigned /*offset*/ ) +{ + /// clears the rows and columns associated with constrained particles + const unsigned blockSize = DataTypes::deriv_total_size; + + for(auto& index : d_indices.getValue()) + { + M->clearRowsCols((index) * blockSize,(index+1) * (blockSize) ); + } +} + +template +void FixedPlaneProjectiveConstraint::projectJacobianMatrix(const MechanicalParams* mparams, DataMatrixDeriv& cData) +{ + SOFA_UNUSED(mparams); + WriteAccessor c = cData; + + const auto& indices = d_indices.getValue(); + const auto& dir = getVec(d_direction.getValue()); + auto& dx = c.wref(); + + auto itRow = dx.begin(); + auto itRowEnd = dx.end(); + while (itRow != itRowEnd) + { + for (auto colIt = itRow.begin(); colIt != itRow.end(); colIt++) + { + if (std::find(indices.begin(), indices.end(), colIt.index()) != indices.end()) + { + auto val = getVec(colIt.val()); + Deriv r(type::NOINIT); + DataTypes::setDPos(r, -(dir * dot(val, dir))); + dx.writeLine(itRow.index()).addCol(colIt.index(), r); + } + } + } +} + +template +void FixedPlaneProjectiveConstraint::setDirection(Coord dir) +{ + if (dir.norm2()>0) + { + d_direction.setValue(dir); + } +} + +template +void FixedPlaneProjectiveConstraint::selectVerticesAlongPlane() +{ + const VecCoord& x = mstate->read(core::ConstVecCoordId::position())->getValue(); + for(unsigned int i=0; i +void FixedPlaneProjectiveConstraint::init() +{ + ProjectiveConstraintSet::init(); + + if (l_topology.empty()) + { + msg_info() << "link to Topology container should be set to ensure right behavior. First Topology found in current context will be used."; + l_topology.set(this->getContext()->getMeshTopologyLink()); + } + + if (sofa::core::topology::BaseMeshTopology* _topology = l_topology.get()) + { + msg_info() << "Topology path used: '" << l_topology.getLinkedPath() << "'"; + + // Initialize topological changes support + d_indices.createTopologyHandler(_topology); + } + else + { + msg_info() << "No topology component found at path: " << l_topology.getLinkedPath() << ", nor in current context: " << this->getContext()->name; + } + + /// test that dmin or dmax are different from zero + if (d_dmin.getValue()!=d_dmax.getValue()) + m_selectVerticesFromPlanes=true; + + if (m_selectVerticesFromPlanes) + selectVerticesAlongPlane(); +} + +template +void FixedPlaneProjectiveConstraint::setDminAndDmax(const Real _dmin,const Real _dmax) +{ + d_dmin=_dmin; + d_dmax=_dmax; + m_selectVerticesFromPlanes=true; +} + +template +void FixedPlaneProjectiveConstraint::draw(const VisualParams* vparams) +{ + if (!vparams->displayFlags().getShowBehaviorModels()) return; + const VecCoord& x = mstate->read(core::ConstVecCoordId::position())->getValue(); + vparams->drawTool()->disableLighting(); + + type::vector points; + for(auto& index : d_indices.getValue()) + { + points.push_back({x[index][0], x[index][1], x[index][2]}); + } + + vparams->drawTool()->drawPoints(points, 10, sofa::type::RGBAColor{1,1.0,0.5,1}); +} + +template +bool FixedPlaneProjectiveConstraint::isPointInPlane(const Coord& p) const +{ + const Real d = getVec(p) * getVec(d_direction.getValue()); + return d > d_dmin.getValue() && d < d_dmax.getValue(); +} + +} // namespace sofa::component::constraint::projective diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedConstraint.cpp b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedProjectiveConstraint.cpp similarity index 73% rename from Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedConstraint.cpp rename to Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedProjectiveConstraint.cpp index 42021e2cdbc..15aeffb660c 100644 --- a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedConstraint.cpp +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedProjectiveConstraint.cpp @@ -19,8 +19,8 @@ * * * Contact information: contact@sofa-framework.org * ******************************************************************************/ -#define SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_FIXEDCONSTRAINT_CPP -#include +#define SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_FIXEDPROJECTIVECONSTRAINT_CPP +#include #include #include @@ -32,37 +32,37 @@ using namespace sofa::defaulttype; using namespace sofa::helper; -int FixedConstraintClass = core::RegisterObject("Attach given particles to their initial positions") - .add< FixedConstraint >() - .add< FixedConstraint >() - .add< FixedConstraint >() - .add< FixedConstraint >() - .add< FixedConstraint >() - .add< FixedConstraint >() - +int FixedProjectiveConstraintClass = core::RegisterObject("Attach given particles to their initial positions") + .add< FixedProjectiveConstraint >() + .add< FixedProjectiveConstraint >() + .add< FixedProjectiveConstraint >() + .add< FixedProjectiveConstraint >() + .add< FixedProjectiveConstraint >() + .add< FixedProjectiveConstraint >() + .addAlias("FixedConstraint") ; // methods specilizations declaration template <> SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API -void FixedConstraint::draw(const core::visual::VisualParams* vparams); +void FixedProjectiveConstraint::draw(const core::visual::VisualParams* vparams); template <> SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API -void FixedConstraint::draw(const core::visual::VisualParams* vparams); +void FixedProjectiveConstraint::draw(const core::visual::VisualParams* vparams); -template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API FixedConstraint; -template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API FixedConstraint; -template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API FixedConstraint; -template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API FixedConstraint; -template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API FixedConstraint; -template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API FixedConstraint; +template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API FixedProjectiveConstraint; +template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API FixedProjectiveConstraint; +template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API FixedProjectiveConstraint; +template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API FixedProjectiveConstraint; +template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API FixedProjectiveConstraint; +template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API FixedProjectiveConstraint; // methods specilizations definition template <> -void FixedConstraint::draw(const core::visual::VisualParams* vparams) +void FixedProjectiveConstraint::draw(const core::visual::VisualParams* vparams) { if (this->d_componentState.getValue() != ComponentState::Valid) return; if (!d_showObject.getValue()) return; @@ -104,7 +104,7 @@ void FixedConstraint::draw(const core::visual::VisualParams* vparam } template <> -void FixedConstraint::draw(const core::visual::VisualParams* vparams) +void FixedProjectiveConstraint::draw(const core::visual::VisualParams* vparams) { if (this->d_componentState.getValue() != ComponentState::Valid) return; if (!d_showObject.getValue()) return; diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedProjectiveConstraint.h b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedProjectiveConstraint.h new file mode 100644 index 00000000000..d2fad673643 --- /dev/null +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedProjectiveConstraint.h @@ -0,0 +1,137 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace sofa::component::constraint::projective +{ + +/// This class can be overridden if needed for additionnal storage within template specializations. +template +class FixedProjectiveConstraintInternalData +{ + +}; + + +/** Maintain a constant velocity. + * If the particle is initially fixed then it is attached to its initial position. + * Otherwise it keeps on drifting. + * For maintaining particles fixed in any case, @sa PointProjectiveConstraint +*/ +template +class FixedProjectiveConstraint : public core::behavior::ProjectiveConstraintSet +{ +public: + SOFA_CLASS(SOFA_TEMPLATE(FixedProjectiveConstraint,DataTypes),SOFA_TEMPLATE(sofa::core::behavior::ProjectiveConstraintSet, DataTypes)); + + using Index = sofa::Index; + typedef typename DataTypes::VecCoord VecCoord; + typedef typename DataTypes::VecDeriv VecDeriv; + typedef typename DataTypes::MatrixDeriv MatrixDeriv; + typedef typename DataTypes::Coord Coord; + typedef typename DataTypes::Deriv Deriv; + typedef typename MatrixDeriv::RowIterator MatrixDerivRowIterator; + typedef typename MatrixDeriv::RowType MatrixDerivRowType; + typedef Data DataVecCoord; + typedef Data DataVecDeriv; + typedef Data DataMatrixDeriv; + typedef type::vector SetIndexArray; + typedef sofa::core::topology::TopologySubsetIndices SetIndex; + typedef sofa::core::topology::Point Point; + + SOFA_ATTRIBUTE_REPLACED__TYPEMEMBER(Vec3, sofa::type::Vec3); +protected: + FixedProjectiveConstraint(); + + virtual ~FixedProjectiveConstraint(); + +public: + SetIndex d_indices; + Data d_fixAll; ///< filter all the DOF to implement a fixed object + Data d_showObject; ///< draw or not the fixed constraints + Data d_drawSize; ///< 0 -> point based rendering, >0 -> radius of spheres + Data d_projectVelocity; ///< activate project velocity to set velocity to zero + + /// Link to be set to the topology container in the component graph. + SingleLink, sofa::core::topology::BaseMeshTopology, BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_topology; +protected: + FixedProjectiveConstraintInternalData* data; + friend class FixedProjectiveConstraintInternalData; + + +public: + void clearConstraints(); + void addConstraint(Index index); + void removeConstraint(Index index); + + // -- Constraint interface + void init() override; + void reinit() override; + + void projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& resData) override; + void projectVelocity(const core::MechanicalParams* mparams, DataVecDeriv& vData) override; + void projectPosition(const core::MechanicalParams* mparams, DataVecCoord& xData) override; + void projectJacobianMatrix(const core::MechanicalParams* mparams, DataMatrixDeriv& cData) override; + + + void applyConstraint(const core::MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) override; + void applyConstraint(const core::MechanicalParams* mparams, linearalgebra::BaseVector* vect, const sofa::core::behavior::MultiMatrixAccessor* matrix) override; + + /** Project the given matrix (Experimental API). + See doc in base parent class + */ + void projectMatrix( sofa::linearalgebra::BaseMatrix* /*M*/, unsigned /*offset*/ ) override; + + void applyConstraint(sofa::core::behavior::ZeroDirichletCondition* matrix) override; + + void draw(const core::visual::VisualParams* vparams) override; + + bool fixAllDOFs() const { return d_fixAll.getValue(); } + +protected : + /// Function check values of given indices + void checkIndices(); + +}; + +#if !defined(SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_FIXEDPROJECTIVECONSTRAINT_CPP) +extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API FixedProjectiveConstraint; +extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API FixedProjectiveConstraint; +extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API FixedProjectiveConstraint; +extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API FixedProjectiveConstraint; +extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API FixedProjectiveConstraint; +extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API FixedProjectiveConstraint; +#endif + +} // namespace sofa::component::constraint::projective diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedProjectiveConstraint.inl b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedProjectiveConstraint.inl new file mode 100644 index 00000000000..78bd3636d2e --- /dev/null +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedProjectiveConstraint.inl @@ -0,0 +1,426 @@ +/****************************************************************************** +* 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 + +using sofa::core::objectmodel::ComponentState; + + +namespace sofa::component::constraint::projective +{ + +template +FixedProjectiveConstraint::FixedProjectiveConstraint() + : core::behavior::ProjectiveConstraintSet(nullptr) + , d_indices( initData(&d_indices,"indices","Indices of the fixed points") ) + , d_fixAll( initData(&d_fixAll,false,"fixAll","filter all the DOF to implement a fixed object") ) + , d_showObject(initData(&d_showObject,true,"showObject","draw or not the fixed constraints")) + , d_drawSize( initData(&d_drawSize,(SReal)0.0,"drawSize","0 -> point based rendering, >0 -> radius of spheres") ) + , d_projectVelocity( initData(&d_projectVelocity,false,"activate_projectVelocity","activate project velocity to set velocity") ) + , l_topology(initLink("topology", "link to the topology container")) + , data(new FixedProjectiveConstraintInternalData()) +{ + // default to indice 0 + d_indices.beginEdit()->push_back(0); + d_indices.endEdit(); + + this->addUpdateCallback("updateIndices", { &d_indices}, [this](const core::DataTracker& t) + { + SOFA_UNUSED(t); + checkIndices(); + return sofa::core::objectmodel::ComponentState::Valid; + }, {}); +} + + +template +FixedProjectiveConstraint::~FixedProjectiveConstraint() +{ + delete data; +} + +template +void FixedProjectiveConstraint::clearConstraints() +{ + d_indices.beginEdit()->clear(); + d_indices.endEdit(); +} + +template +void FixedProjectiveConstraint::addConstraint(Index index) +{ + d_indices.beginEdit()->push_back(index); + d_indices.endEdit(); +} + +template +void FixedProjectiveConstraint::removeConstraint(Index index) +{ + sofa::type::removeValue(*d_indices.beginEdit(),index); + d_indices.endEdit(); +} + +// -- Constraint interface + + +template +void FixedProjectiveConstraint::init() +{ + this->d_componentState.setValue(ComponentState::Invalid); + this->core::behavior::ProjectiveConstraintSet::init(); + + if (!this->mstate.get()) + { + msg_warning() << "Missing mstate, cannot initialize the component."; + return; + } + + if (l_topology.empty()) + { + msg_info() << "link to Topology container should be set to ensure right behavior. First Topology found in current context will be used."; + l_topology.set(this->getContext()->getMeshTopologyLink()); + } + + if (sofa::core::topology::BaseMeshTopology* _topology = l_topology.get()) + { + msg_info() << "Topology path used: '" << l_topology.getLinkedPath() << "'"; + + // Initialize topological changes support + d_indices.createTopologyHandler(_topology); + } + else + { + msg_info() << "Can not find the topology, won't be able to handle topological changes"; + } + + this->checkIndices(); + this->d_componentState.setValue(ComponentState::Valid); +} + +template +void FixedProjectiveConstraint::reinit() +{ + this->checkIndices(); +} + +template +void FixedProjectiveConstraint::checkIndices() +{ + // Check value of given indices + Index maxIndex=this->mstate->getSize(); + + const SetIndexArray & indices = d_indices.getValue(); + SetIndexArray invalidIndices; + for (unsigned int i=0; i= maxIndex) + { + msg_warning() << "Index " << index << " not valid, should be [0,"<< maxIndex <<"]. Constraint will be removed."; + invalidIndices.push_back(index); + } + } + + // if invalid indices, sort them and remove in decreasing order as removeConstraint perform a swap and pop_back. + if (!invalidIndices.empty()) + { + std::sort( invalidIndices.begin(), invalidIndices.end(), std::greater() ); + const int max = invalidIndices.size()-1; + for (int i=max; i>= 0; i--) + { + removeConstraint(invalidIndices[i]); + } + } +} + +template +void FixedProjectiveConstraint::projectMatrix( sofa::linearalgebra::BaseMatrix* M, unsigned offset ) +{ + static const unsigned blockSize = DataTypes::deriv_total_size; + + if( d_fixAll.getValue() ) + { + const unsigned size = this->mstate->getSize(); + for( unsigned i=0; iclearRowsCols( offset + i * blockSize, offset + (i+1) * (blockSize) ); + } + } + else + { + // clears the rows and columns associated with fixed particles + for (const auto id : d_indices.getValue()) + { + M->clearRowsCols( offset + id * blockSize, offset + (id+1) * blockSize ); + } + } +} + + +template +void FixedProjectiveConstraint::projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& resData) +{ + SOFA_UNUSED(mparams); + + helper::WriteAccessor res (resData ); + const SetIndexArray & indices = d_indices.getValue(); + + if( d_fixAll.getValue() ) + { + // fix everything + typename VecDeriv::iterator it; + for( it = res.begin(); it != res.end(); ++it ) + { + *it = Deriv(); + } + } + else + { + for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) + { + res[*it] = Deriv(); + } + } +} + +template +void FixedProjectiveConstraint::projectJacobianMatrix(const core::MechanicalParams* mparams, DataMatrixDeriv& cData) +{ + SOFA_UNUSED(mparams); + + helper::WriteAccessor c (cData ); + + if( d_fixAll.getValue() ) + { + // fix everything + c->clear(); + } + else + { + const SetIndexArray& indices = d_indices.getValue(); + for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) + { + c->clearColBlock(*it); + } + } +} + +// projectVelocity applies the same changes on velocity vector as projectResponse on position vector : +// Each fixed point received a null velocity vector. +// When a new fixed point is added while its velocity vector is already null, projectVelocity is not usefull. +// But when a new fixed point is added while its velocity vector is not null, it's necessary to fix it to null or +// to set the projectVelocity option to True. If not, the fixed point is going to drift. +template +void FixedProjectiveConstraint::projectVelocity(const core::MechanicalParams* mparams, DataVecDeriv& vData) +{ + SOFA_UNUSED(mparams); + + if(!d_projectVelocity.getValue()) return; + + helper::WriteAccessor res (vData ); + + if ( d_fixAll.getValue() ) // fix everyting + { + for(Size i=0; id_indices.getValue(); + for(Index ind : indices) + { + res[ind] = Deriv(); + } + } +} + + +template +void FixedProjectiveConstraint::projectPosition(const core::MechanicalParams* /*mparams*/, DataVecCoord& /*xData*/) +{ + +} + +// Matrix Integration interface +template +void FixedProjectiveConstraint::applyConstraint(const core::MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) +{ + SOFA_UNUSED(mparams); + if(const core::behavior::MultiMatrixAccessor::MatrixRef r = matrix->getMatrix(this->mstate.get())) + { + const unsigned int N = Deriv::size(); + + if( d_fixAll.getValue() ) + { + const unsigned size = this->mstate->getSize(); + for(unsigned int i=0; iclearRowCol(r.offset + N * i + c); + // Set Fixed Vertex + for (unsigned int c=0; cset(r.offset + N * i + c, r.offset + N * i + c, 1.0); + } + } + else + { + const SetIndexArray & indices = d_indices.getValue(); + + for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) + { + // Reset Fixed Row and Col + for (unsigned int c=0; cclearRowCol(r.offset + N * (*it) + c); + // Set Fixed Vertex + for (unsigned int c=0; cset(r.offset + N * (*it) + c, r.offset + N * (*it) + c, 1.0); + } + } + } +} + +template +void FixedProjectiveConstraint::applyConstraint(const core::MechanicalParams* mparams, linearalgebra::BaseVector* vect, const sofa::core::behavior::MultiMatrixAccessor* matrix) +{ + SOFA_UNUSED(mparams); + const int o = matrix->getGlobalOffset(this->mstate.get()); + if (o >= 0) + { + const unsigned int offset = (unsigned int)o; + const unsigned int N = Deriv::size(); + + if( d_fixAll.getValue() ) + { + for(sofa::Size i=0; i < (sofa::Size) vect->size(); i++ ) + { + for (unsigned int c=0; cclear(offset + N * i + c); + } + } + else + { + const SetIndexArray & indices = d_indices.getValue(); + for (const auto & index : indices) + { + for (unsigned int c=0; cclear(offset + N * index + c); + } + } + } +} + +template +void FixedProjectiveConstraint::applyConstraint(sofa::core::behavior::ZeroDirichletCondition* matrix) +{ + static constexpr unsigned int N = Deriv::size(); + + if( d_fixAll.getValue() ) + { + const sofa::Size size = this->mstate->getMatrixSize(); + for(sofa::Index i = 0; i < size; ++i) + { + matrix->discardRowCol(i, i); + } + } + else + { + const SetIndexArray & indices = d_indices.getValue(); + + for (const auto index : indices) + { + for (unsigned int c = 0; c < N; ++c) + { + matrix->discardRowCol(N * index + c, N * index + c); + } + } + } +} + +template +void FixedProjectiveConstraint::draw(const core::visual::VisualParams* vparams) +{ + if (this->d_componentState.getValue() != ComponentState::Valid) return; + if (!vparams->displayFlags().getShowBehaviorModels()) return; + if (!d_showObject.getValue()) return; + if (!this->isActive()) return; + + const auto stateLifeCycle = vparams->drawTool()->makeStateLifeCycle(); + + const VecCoord& x = this->mstate->read(core::ConstVecCoordId::position())->getValue(); + const SetIndexArray & indices = d_indices.getValue(); + + if( d_drawSize.getValue() == 0) // old classical drawing by points + { + std::vector< sofa::type::Vec3 > points; + sofa::type::Vec3 point; + + if (d_fixAll.getValue()) + { + for (unsigned i = 0; i < x.size(); i++) + { + point = DataTypes::getCPos(x[i]); + points.push_back(point); + } + } + else + { + for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) + { + point = DataTypes::getCPos(x[*it]); + points.push_back(point); + } + } + vparams->drawTool()->drawPoints(points, 10, sofa::type::RGBAColor(1,0.5,0.5,1)); + } + else // new drawing by spheres + { + vparams->drawTool()->setLightingEnabled(true); + + std::vector< sofa::type::Vec3 > points; + sofa::type::Vec3 point; + if( d_fixAll.getValue()==true ) + for (unsigned i=0; idrawTool()->drawSpheres(points, (float)d_drawSize.getValue(), sofa::type::RGBAColor(1.0f,0.35f,0.35f,1.0f)); + } + + +} + +} // namespace sofa::component::constraint::projective diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedRotationConstraint.h b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedRotationConstraint.h index cd66b14734a..511c63a88d4 100644 --- a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedRotationConstraint.h +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedRotationConstraint.h @@ -20,61 +20,13 @@ * Contact information: contact@sofa-framework.org * ******************************************************************************/ #pragma once -#include -#include -#include +#include -namespace sofa::component::constraint::projective -{ +SOFA_DEPRECATED_HEADER("v24.06", "v25.06", "sofa/component/constraint/projective/FixedRotationProjectiveConstraint.h") -/** - * Prevents rotation around X or Y or Z axis - */ -template -class FixedRotationConstraint : public core::behavior::ProjectiveConstraintSet +namespace sofa::component::constraint::projective { -public: - SOFA_CLASS(SOFA_TEMPLATE(FixedRotationConstraint,DataTypes),SOFA_TEMPLATE(sofa::core::behavior::ProjectiveConstraintSet, DataTypes)); - - typedef typename DataTypes::VecCoord VecCoord; - typedef typename DataTypes::VecDeriv VecDeriv; - typedef typename DataTypes::MatrixDeriv MatrixDeriv; - typedef typename DataTypes::MatrixDeriv::RowType MatrixDerivRowType; - typedef typename DataTypes::Coord Coord; - typedef typename DataTypes::Deriv Deriv; - typedef typename DataTypes::Real Real; - typedef Data DataVecCoord; - typedef Data DataVecDeriv; - typedef Data DataMatrixDeriv; - typedef type::Vec<3,Real> Vec3; - - -protected: - FixedRotationConstraint(); - ~FixedRotationConstraint() override; -public: - void init() override; - - void projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& dx) override; - void projectVelocity(const core::MechanicalParams* mparams, DataVecDeriv& dx) override; - void projectPosition(const core::MechanicalParams* mparams, DataVecCoord& x) override; - void projectJacobianMatrix(const core::MechanicalParams* mparams, DataMatrixDeriv& c) override; - - void draw(const core::visual::VisualParams* vparams) override; - - -protected : - Data< bool > FixedXRotation; ///< Prevent Rotation around X axis - Data< bool > FixedYRotation; ///< Prevent Rotation around Y axis - Data< bool > FixedZRotation; ///< Prevent Rotation around Z axis - type::vector> previousOrientation; -}; - - -#if !defined(SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_FIXEDROTATIONCONSTRAINT_CPP) -extern template class FixedRotationConstraint; - -#endif - -} // namespace sofa::component::constraint::projective +template +using FixedRotationConstraint SOFA_ATTRIBUTE_DEPRECATED("v24.06 ", "v25.06", "FixedRotationConstraint has been renamed to FixedRotationProjectiveConstraint") = FixedRotationProjectiveConstraint; +} \ No newline at end of file diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedRotationConstraint.inl b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedRotationConstraint.inl index 0f1a6731a8d..0ecc0e7851c 100644 --- a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedRotationConstraint.inl +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedRotationConstraint.inl @@ -21,122 +21,6 @@ ******************************************************************************/ #pragma once -#include -#include -#include +#include - -namespace sofa::component::constraint::projective -{ - - -template -FixedRotationConstraint::FixedRotationConstraint() - : core::behavior::ProjectiveConstraintSet(nullptr), - FixedXRotation( initData( &FixedXRotation, false, "FixedXRotation", "Prevent Rotation around X axis")), - FixedYRotation( initData( &FixedYRotation, false, "FixedYRotation", "Prevent Rotation around Y axis")), - FixedZRotation( initData( &FixedZRotation, false, "FixedZRotation", "Prevent Rotation around Z axis")) -{ -} - - -template -FixedRotationConstraint::~FixedRotationConstraint() -{ -} - - -template -void FixedRotationConstraint::init() -{ - this->core::behavior::ProjectiveConstraintSet::init(); - - // Retrieves mechanical state - VecCoord x = this->mstate->read(core::ConstVecCoordId::position())->getValue(); - - // Stores initial orientation for each vertex - previousOrientation.resize(x.size()); - for (unsigned int i=0; i -void FixedRotationConstraint::projectResponse(const core::MechanicalParams* /*mparams*/, DataVecDeriv& /*res*/) -{ - -} - -template -void FixedRotationConstraint::projectJacobianMatrix(const core::MechanicalParams* /*mparams*/, DataMatrixDeriv& /*res*/) -{ - -} - -template -void FixedRotationConstraint::projectVelocity(const core::MechanicalParams* /*mparams*/, DataVecDeriv& /*dx*/) -{ - -} - -template -void FixedRotationConstraint::projectPosition(const core::MechanicalParams* /*mparams*/, DataVecCoord& xData) -{ - helper::WriteAccessor x = xData; - for (unsigned int i = 0; i < x.size(); ++i) - { - // Current orientations - const sofa::type::Quat& Q = x[i].getOrientation(); - // Previous orientations - const sofa::type::Quat& Q_prev = previousOrientation[i]; - - auto project = [](const Vec3 a, const Vec3 b) -> Vec3 { - return (a * b) * b; - }; - auto decompose_ts = [&](const sofa::type::Quat q, const Vec3 twistAxis) { - Vec3 vec3_part(q[0], q[1], q[2]); - Vec3 projected = project(vec3_part, twistAxis); - sofa::type::Quat twist(projected[0], projected[1], projected[2], q[3]); - // Singularity : A perpendicular angle would give you quaternion (0, 0, 0, 0) - if (std::none_of(twist.ptr(), twist.ptr() + 4, [](SReal x) {return x != 0.; })) { - twist = sofa::type::Quat::identity(); - } - twist.normalize(); - sofa::type::Quat swing = q * twist.inverse(); - swing.normalize(); - return std::make_pair(twist, swing); - }; - const Vec3 vx(1, 0, 0), vy(0, 1, 0), vz(0, 0, 1); - - sofa::type::Quat Q_remaining = Q; - sofa::type::Quat Qp_remaining = Q_prev; - sofa::type::Quat to_keep = sofa::type::Quat::identity(); - - auto remove_rotation = [&](const Vec3 axis) { - Q_remaining = decompose_ts(Q_remaining, axis).second; - sofa::type::Quat twist; - std::tie(twist, Qp_remaining) = decompose_ts(Qp_remaining, axis); - to_keep = twist * to_keep; - }; - - if (FixedXRotation.getValue() == true){ - remove_rotation(vx); - } - if (FixedYRotation.getValue() == true){ - remove_rotation(vy); - } - if (FixedZRotation.getValue() == true){ - remove_rotation(vz); - } - x[i].getOrientation() = Q_remaining * to_keep; - } -} - - -template -void FixedRotationConstraint::draw(const core::visual::VisualParams* ) -{ -} - -} // namespace sofa::component::constraint::projective +SOFA_DEPRECATED_HEADER("v24.06", "v25.06", "sofa/component/constraint/projective/FixedRotationProjectiveConstraint.inl") diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedRotationConstraint.cpp b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedRotationProjectiveConstraint.cpp similarity index 80% rename from Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedRotationConstraint.cpp rename to Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedRotationProjectiveConstraint.cpp index 3a47b6bbd69..1844831172d 100644 --- a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedRotationConstraint.cpp +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedRotationProjectiveConstraint.cpp @@ -19,8 +19,8 @@ * * * Contact information: contact@sofa-framework.org * ******************************************************************************/ -#define SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_FIXEDROTATIONCONSTRAINT_CPP -#include +#define SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_FIXEDROTATIONPROJECTIVECONSTRAINT_CPP +#include #include #include @@ -31,10 +31,12 @@ namespace sofa::component::constraint::projective using namespace sofa::defaulttype; -int FixedRotationConstraintClass = core::RegisterObject("Prevents rotation around x or/and y or/and z axis") - .add< FixedRotationConstraint >() +int FixedRotationProjectiveConstraintClass = core::RegisterObject("Prevents rotation around x or/and y or/and z axis") + .add< FixedRotationProjectiveConstraint >() + .addAlias("FixedRotationConstraint") + ; -template class FixedRotationConstraint; +template class FixedRotationProjectiveConstraint; } // namespace sofa::component::constraint::projective diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedRotationProjectiveConstraint.h b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedRotationProjectiveConstraint.h new file mode 100644 index 00000000000..fa9800c22fe --- /dev/null +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedRotationProjectiveConstraint.h @@ -0,0 +1,79 @@ +/****************************************************************************** +* 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::constraint::projective +{ + +/** + * Prevents rotation around X or Y or Z axis + */ +template +class FixedRotationProjectiveConstraint : public core::behavior::ProjectiveConstraintSet +{ +public: + SOFA_CLASS(SOFA_TEMPLATE(FixedRotationProjectiveConstraint,DataTypes),SOFA_TEMPLATE(sofa::core::behavior::ProjectiveConstraintSet, DataTypes)); + + typedef typename DataTypes::VecCoord VecCoord; + typedef typename DataTypes::VecDeriv VecDeriv; + typedef typename DataTypes::MatrixDeriv MatrixDeriv; + typedef typename DataTypes::MatrixDeriv::RowType MatrixDerivRowType; + typedef typename DataTypes::Coord Coord; + typedef typename DataTypes::Deriv Deriv; + typedef typename DataTypes::Real Real; + typedef Data DataVecCoord; + typedef Data DataVecDeriv; + typedef Data DataMatrixDeriv; + typedef type::Vec<3,Real> Vec3; + + +protected: + FixedRotationProjectiveConstraint(); + ~FixedRotationProjectiveConstraint() override; +public: + void init() override; + + void projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& dx) override; + void projectVelocity(const core::MechanicalParams* mparams, DataVecDeriv& dx) override; + void projectPosition(const core::MechanicalParams* mparams, DataVecCoord& x) override; + void projectJacobianMatrix(const core::MechanicalParams* mparams, DataMatrixDeriv& c) override; + + void draw(const core::visual::VisualParams* vparams) override; + + +protected : + Data< bool > FixedXRotation; ///< Prevent Rotation around X axis + Data< bool > FixedYRotation; ///< Prevent Rotation around Y axis + Data< bool > FixedZRotation; ///< Prevent Rotation around Z axis + type::vector> previousOrientation; +}; + + +#if !defined(SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_FIXEDROTATIONPROJECTIVECONSTRAINT_CPP) +extern template class FixedRotationProjectiveConstraint; +#endif + +} // namespace sofa::component::constraint::projective diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedRotationProjectiveConstraint.inl b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedRotationProjectiveConstraint.inl new file mode 100644 index 00000000000..d900e468c9c --- /dev/null +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedRotationProjectiveConstraint.inl @@ -0,0 +1,142 @@ +/****************************************************************************** +* 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::constraint::projective +{ + + +template +FixedRotationProjectiveConstraint::FixedRotationProjectiveConstraint() + : core::behavior::ProjectiveConstraintSet(nullptr), + FixedXRotation( initData( &FixedXRotation, false, "FixedXRotation", "Prevent Rotation around X axis")), + FixedYRotation( initData( &FixedYRotation, false, "FixedYRotation", "Prevent Rotation around Y axis")), + FixedZRotation( initData( &FixedZRotation, false, "FixedZRotation", "Prevent Rotation around Z axis")) +{ +} + + +template +FixedRotationProjectiveConstraint::~FixedRotationProjectiveConstraint() +{ +} + + +template +void FixedRotationProjectiveConstraint::init() +{ + this->core::behavior::ProjectiveConstraintSet::init(); + + // Retrieves mechanical state + VecCoord x = this->mstate->read(core::ConstVecCoordId::position())->getValue(); + + // Stores initial orientation for each vertex + previousOrientation.resize(x.size()); + for (unsigned int i=0; i +void FixedRotationProjectiveConstraint::projectResponse(const core::MechanicalParams* /*mparams*/, DataVecDeriv& /*res*/) +{ + +} + +template +void FixedRotationProjectiveConstraint::projectJacobianMatrix(const core::MechanicalParams* /*mparams*/, DataMatrixDeriv& /*res*/) +{ + +} + +template +void FixedRotationProjectiveConstraint::projectVelocity(const core::MechanicalParams* /*mparams*/, DataVecDeriv& /*dx*/) +{ + +} + +template +void FixedRotationProjectiveConstraint::projectPosition(const core::MechanicalParams* /*mparams*/, DataVecCoord& xData) +{ + helper::WriteAccessor x = xData; + for (unsigned int i = 0; i < x.size(); ++i) + { + // Current orientations + const sofa::type::Quat& Q = x[i].getOrientation(); + // Previous orientations + const sofa::type::Quat& Q_prev = previousOrientation[i]; + + auto project = [](const Vec3 a, const Vec3 b) -> Vec3 { + return (a * b) * b; + }; + auto decompose_ts = [&](const sofa::type::Quat q, const Vec3 twistAxis) { + Vec3 vec3_part(q[0], q[1], q[2]); + Vec3 projected = project(vec3_part, twistAxis); + sofa::type::Quat twist(projected[0], projected[1], projected[2], q[3]); + // Singularity : A perpendicular angle would give you quaternion (0, 0, 0, 0) + if (std::none_of(twist.ptr(), twist.ptr() + 4, [](SReal x) {return x != 0.; })) { + twist = sofa::type::Quat::identity(); + } + twist.normalize(); + sofa::type::Quat swing = q * twist.inverse(); + swing.normalize(); + return std::make_pair(twist, swing); + }; + const Vec3 vx(1, 0, 0), vy(0, 1, 0), vz(0, 0, 1); + + sofa::type::Quat Q_remaining = Q; + sofa::type::Quat Qp_remaining = Q_prev; + sofa::type::Quat to_keep = sofa::type::Quat::identity(); + + auto remove_rotation = [&](const Vec3 axis) { + Q_remaining = decompose_ts(Q_remaining, axis).second; + sofa::type::Quat twist; + std::tie(twist, Qp_remaining) = decompose_ts(Qp_remaining, axis); + to_keep = twist * to_keep; + }; + + if (FixedXRotation.getValue() == true){ + remove_rotation(vx); + } + if (FixedYRotation.getValue() == true){ + remove_rotation(vy); + } + if (FixedZRotation.getValue() == true){ + remove_rotation(vz); + } + x[i].getOrientation() = Q_remaining * to_keep; + } +} + + +template +void FixedRotationProjectiveConstraint::draw(const core::visual::VisualParams* ) +{ +} + +} // namespace sofa::component::constraint::projective diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedTranslationConstraint.h b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedTranslationConstraint.h index 29360c58629..47203f733ee 100644 --- a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedTranslationConstraint.h +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedTranslationConstraint.h @@ -20,87 +20,13 @@ * Contact information: contact@sofa-framework.org * ******************************************************************************/ #pragma once -#include -#include -#include -#include +#include -namespace sofa::component::constraint::projective -{ +SOFA_DEPRECATED_HEADER("v24.06", "v25.06", "sofa/component/constraint/projective/FixedTranslationProjectiveConstraint.h") -/// This class can be overridden if needed for additionnal storage within template specializations. -template -class FixedTranslationConstraintInternalData -{ -}; - -/** Attach given particles to their initial positions. -*/ -template -class FixedTranslationConstraint : public core::behavior::ProjectiveConstraintSet +namespace sofa::component::constraint::projective { -public: - SOFA_CLASS(SOFA_TEMPLATE(FixedTranslationConstraint,DataTypes),SOFA_TEMPLATE(sofa::core::behavior::ProjectiveConstraintSet, DataTypes)); - - using Index = sofa::Index; - typedef typename DataTypes::VecCoord VecCoord; - typedef typename DataTypes::VecDeriv VecDeriv; - typedef typename DataTypes::MatrixDeriv MatrixDeriv; - typedef typename DataTypes::Coord Coord; - typedef typename DataTypes::Deriv Deriv; - typedef typename MatrixDeriv::RowType MatrixDerivRowType; - typedef typename MatrixDeriv::RowIterator MatrixDerivRowIterator; - typedef Data DataVecCoord; - typedef Data DataVecDeriv; - typedef Data DataMatrixDeriv; - typedef type::vector SetIndexArray; - typedef sofa::core::topology::TopologySubsetIndices SetIndex; -protected: - FixedTranslationConstraintInternalData data; - friend class FixedTranslationConstraintInternalData; - -public: - SetIndex f_indices; ///< Indices of the fixed points - Data f_fixAll; ///< filter all the DOF to implement a fixed object - Data _drawSize; ///< 0 -> point based rendering, >0 -> radius of spheres - SetIndex f_coordinates; ///< Coordinates of the fixed points - - /// Link to be set to the topology container in the component graph. - SingleLink, sofa::core::topology::BaseMeshTopology, BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_topology; -protected: - FixedTranslationConstraint(); - - virtual ~FixedTranslationConstraint(); -public: - // methods to add/remove some indices - void clearIndices(); - void addIndex(Index index); - void removeIndex(Index index); - - // -- Constraint interface - void init() override; - - void projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& resData) override; - void projectVelocity(const core::MechanicalParams* mparams, DataVecDeriv& vData) override; - void projectPosition(const core::MechanicalParams* mparams, DataVecCoord& xData) override; - void projectJacobianMatrix(const core::MechanicalParams* mparams, DataMatrixDeriv& cData) override; - - - void draw(const core::visual::VisualParams* vparams) override; - -protected: - template - void projectResponseT(DataDeriv& dx, - const std::function& clear); - -}; - -#if !defined(SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_FIXEDTRANSLATIONCONSTRAINT_CPP) -extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API FixedTranslationConstraint; -extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API FixedTranslationConstraint; -extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API FixedTranslationConstraint; - -#endif - -} // namespace sofa::component::constraint::projective +template +using FixedTranslationConstraint SOFA_ATTRIBUTE_DEPRECATED("v24.06 ", "v25.06", "FixedTranslationConstraint has been renamed to FixedTranslationProjectiveConstraint") = FixedTranslationProjectiveConstraint; +} \ No newline at end of file diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedTranslationConstraint.inl b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedTranslationConstraint.inl index 9c734a4b1c4..7b1b96bad54 100644 --- a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedTranslationConstraint.inl +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedTranslationConstraint.inl @@ -21,188 +21,6 @@ ******************************************************************************/ #pragma once -#include -#include -#include -#include -#include +#include -namespace sofa::component::constraint::projective -{ - -template< class DataTypes> -FixedTranslationConstraint::FixedTranslationConstraint() - : core::behavior::ProjectiveConstraintSet(nullptr) - , f_indices( initData(&f_indices,"indices","Indices of the fixed points") ) - , f_fixAll( initData(&f_fixAll,false,"fixAll","filter all the DOF to implement a fixed object") ) - , _drawSize( initData(&_drawSize,(SReal)0.0,"drawSize","0 -> point based rendering, >0 -> radius of spheres") ) - , f_coordinates( initData(&f_coordinates,"coordinates","Coordinates of the fixed points") ) - , l_topology(initLink("topology", "link to the topology container")) -{ - // default to indice 0 - f_indices.beginEdit()->push_back(0); - f_indices.endEdit(); -} - - -template -FixedTranslationConstraint::~FixedTranslationConstraint() -{ - -} - -template -void FixedTranslationConstraint::clearIndices() -{ - f_indices.beginEdit()->clear(); - f_indices.endEdit(); -} - -template -void FixedTranslationConstraint::addIndex(Index index) -{ - f_indices.beginEdit()->push_back(index); - f_indices.endEdit(); -} - -template -void FixedTranslationConstraint::removeIndex(Index index) -{ - sofa::type::removeValue(*f_indices.beginEdit(),index); - f_indices.endEdit(); -} - -// -- Constraint interface -template -void FixedTranslationConstraint::init() -{ - this->core::behavior::ProjectiveConstraintSet::init(); - - if (l_topology.empty()) - { - msg_info() << "link to Topology container should be set to ensure right behavior. First Topology found in current context will be used."; - l_topology.set(this->getContext()->getMeshTopologyLink()); - } - - if (sofa::core::topology::BaseMeshTopology* _topology = l_topology.get()) - { - msg_info() << "Topology path used: '" << l_topology.getLinkedPath() << "'"; - - // Initialize topological changes support - f_indices.createTopologyHandler(_topology); - f_coordinates.createTopologyHandler(_topology); - } - else - { - msg_info() << "No topology component found at path: " << l_topology.getLinkedPath() << ", nor in current context: " << this->getContext()->name; - } -} - - -template -static inline void clearPos(defaulttype::RigidDeriv& v) -{ - getVCenter(v).clear(); -} - -template -static inline void clearPos(type::Vec<6,T>& v) -{ - for (unsigned int i=0; i<3; ++i) - v[i] = 0; -} - -template template -void FixedTranslationConstraint::projectResponseT(DataDeriv& dx, - const std::function& clear) -{ - if (f_fixAll.getValue()) - { - for (std::size_t i = 0; i < dx.size(); i++) - { - clear(dx, i); - } - } - else - { - const SetIndexArray & indices = f_indices.getValue(); - for (const auto index : indices) - { - clear(dx, index); - } - } -} - -template -void FixedTranslationConstraint::projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& resData) -{ - SOFA_UNUSED(mparams); - helper::WriteAccessor res = resData; - projectResponseT(res.wref(), [](auto& dx, const unsigned int index) {dx[index].clear(); }); -} - -template -void FixedTranslationConstraint::projectVelocity(const core::MechanicalParams* /*mparams*/, DataVecDeriv& /*vData*/) -{ - -} - -template -void FixedTranslationConstraint::projectPosition(const core::MechanicalParams* /*mparams*/, DataVecCoord& /*xData*/) -{ - -} - -template -void FixedTranslationConstraint::projectJacobianMatrix(const core::MechanicalParams* mparams, DataMatrixDeriv& cData) -{ - SOFA_UNUSED(mparams); - helper::WriteAccessor c = cData; - projectResponseT(c.wref(), [](MatrixDeriv& res, const unsigned int index) { res.clearColBlock(index); }); -} - - -template -void FixedTranslationConstraint::draw(const core::visual::VisualParams* vparams) -{ - const SetIndexArray & indices = f_indices.getValue(); - if (!vparams->displayFlags().getShowBehaviorModels()) - return; - const VecCoord& x = this->mstate->read(core::ConstVecCoordId::position())->getValue(); - - const auto stateLifeCycle = vparams->drawTool()->makeStateLifeCycle(); - vparams->drawTool()->disableLighting(); - - std::vector vertices; - constexpr sofa::type::RGBAColor color(1, 0.5, 0.5, 1); - - if (f_fixAll.getValue() == true) - { - for (unsigned i = 0; i < x.size(); i++) - { - sofa::type::Vec3 v; - const typename DataTypes::CPos& cpos = DataTypes::getCPos(x[i]); - for(Size j=0 ; jdrawTool()->drawPoints(vertices, 10, color); - - -} - -} // namespace sofa::component::constraint::projective +SOFA_DEPRECATED_HEADER("v24.06", "v25.06", "sofa/component/constraint/projective/FixedTranslationProjectiveConstraint.inl") diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedTranslationConstraint.cpp b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedTranslationProjectiveConstraint.cpp similarity index 74% rename from Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedTranslationConstraint.cpp rename to Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedTranslationProjectiveConstraint.cpp index c4acd3e16f1..0336e9a6f8d 100644 --- a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedTranslationConstraint.cpp +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedTranslationProjectiveConstraint.cpp @@ -19,8 +19,8 @@ * * * Contact information: contact@sofa-framework.org * ******************************************************************************/ -#define SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_FIXEDTRANSLATIONCONSTRAINT_CPP -#include +#define SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_FIXEDTRANSLATIONPROJECTIVECONSTRAINT_CPP +#include #include #include @@ -31,16 +31,16 @@ namespace sofa::component::constraint::projective using namespace sofa::defaulttype; using namespace sofa::helper; -int FixedTranslationConstraintClass = core::RegisterObject("Attach given rigids to their initial positions but they still can have rotations") - .add< FixedTranslationConstraint >() - .add< FixedTranslationConstraint >() - .add< FixedTranslationConstraint >() +int FixedTranslationProjectiveConstraintClass = core::RegisterObject("Attach given rigids to their initial positions but they still can have rotations") + .add< FixedTranslationProjectiveConstraint >() + .add< FixedTranslationProjectiveConstraint >() + .add< FixedTranslationProjectiveConstraint >() + .addAlias("FixedTranslationConstraint") + ; - ; - -template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API FixedTranslationConstraint; -template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API FixedTranslationConstraint; -template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API FixedTranslationConstraint; +template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API FixedTranslationProjectiveConstraint; +template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API FixedTranslationProjectiveConstraint; +template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API FixedTranslationProjectiveConstraint; } // namespace sofa::component::constraint::projective diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedTranslationProjectiveConstraint.h b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedTranslationProjectiveConstraint.h new file mode 100644 index 00000000000..4290a2c6d13 --- /dev/null +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedTranslationProjectiveConstraint.h @@ -0,0 +1,105 @@ +/****************************************************************************** +* 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 + +namespace sofa::component::constraint::projective +{ + +/// This class can be overridden if needed for additionnal storage within template specializations. +template +class FixedTranslationProjectiveConstraintInternalData +{ +}; + +/** Attach given particles to their initial positions. +*/ +template +class FixedTranslationProjectiveConstraint : public core::behavior::ProjectiveConstraintSet +{ +public: + SOFA_CLASS(SOFA_TEMPLATE(FixedTranslationProjectiveConstraint,DataTypes),SOFA_TEMPLATE(sofa::core::behavior::ProjectiveConstraintSet, DataTypes)); + + using Index = sofa::Index; + typedef typename DataTypes::VecCoord VecCoord; + typedef typename DataTypes::VecDeriv VecDeriv; + typedef typename DataTypes::MatrixDeriv MatrixDeriv; + typedef typename DataTypes::Coord Coord; + typedef typename DataTypes::Deriv Deriv; + typedef typename MatrixDeriv::RowType MatrixDerivRowType; + typedef typename MatrixDeriv::RowIterator MatrixDerivRowIterator; + typedef Data DataVecCoord; + typedef Data DataVecDeriv; + typedef Data DataMatrixDeriv; + typedef type::vector SetIndexArray; + typedef sofa::core::topology::TopologySubsetIndices SetIndex; +protected: + FixedTranslationProjectiveConstraintInternalData data; + friend class FixedTranslationProjectiveConstraintInternalData; + +public: + SetIndex f_indices; ///< Indices of the fixed points + Data f_fixAll; ///< filter all the DOF to implement a fixed object + Data _drawSize; ///< 0 -> point based rendering, >0 -> radius of spheres + SetIndex f_coordinates; ///< Coordinates of the fixed points + + /// Link to be set to the topology container in the component graph. + SingleLink, sofa::core::topology::BaseMeshTopology, BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_topology; +protected: + FixedTranslationProjectiveConstraint(); + + virtual ~FixedTranslationProjectiveConstraint(); +public: + // methods to add/remove some indices + void clearIndices(); + void addIndex(Index index); + void removeIndex(Index index); + + // -- Constraint interface + void init() override; + + void projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& resData) override; + void projectVelocity(const core::MechanicalParams* mparams, DataVecDeriv& vData) override; + void projectPosition(const core::MechanicalParams* mparams, DataVecCoord& xData) override; + void projectJacobianMatrix(const core::MechanicalParams* mparams, DataMatrixDeriv& cData) override; + + + void draw(const core::visual::VisualParams* vparams) override; + +protected: + template + void projectResponseT(DataDeriv& dx, + const std::function& clear); + +}; + +#if !defined(SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_FIXEDTRANSLATIONPROJECTIVECONSTRAINT_CPP) +extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API FixedTranslationProjectiveConstraint; +extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API FixedTranslationProjectiveConstraint; +extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API FixedTranslationProjectiveConstraint; +#endif + +} // namespace sofa::component::constraint::projective diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedTranslationProjectiveConstraint.inl b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedTranslationProjectiveConstraint.inl new file mode 100644 index 00000000000..d1f7408b8c8 --- /dev/null +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/FixedTranslationProjectiveConstraint.inl @@ -0,0 +1,208 @@ +/****************************************************************************** +* 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::component::constraint::projective +{ + +template< class DataTypes> +FixedTranslationProjectiveConstraint::FixedTranslationProjectiveConstraint() + : core::behavior::ProjectiveConstraintSet(nullptr) + , f_indices( initData(&f_indices,"indices","Indices of the fixed points") ) + , f_fixAll( initData(&f_fixAll,false,"fixAll","filter all the DOF to implement a fixed object") ) + , _drawSize( initData(&_drawSize,(SReal)0.0,"drawSize","0 -> point based rendering, >0 -> radius of spheres") ) + , f_coordinates( initData(&f_coordinates,"coordinates","Coordinates of the fixed points") ) + , l_topology(initLink("topology", "link to the topology container")) +{ + // default to indice 0 + f_indices.beginEdit()->push_back(0); + f_indices.endEdit(); +} + + +template +FixedTranslationProjectiveConstraint::~FixedTranslationProjectiveConstraint() +{ + +} + +template +void FixedTranslationProjectiveConstraint::clearIndices() +{ + f_indices.beginEdit()->clear(); + f_indices.endEdit(); +} + +template +void FixedTranslationProjectiveConstraint::addIndex(Index index) +{ + f_indices.beginEdit()->push_back(index); + f_indices.endEdit(); +} + +template +void FixedTranslationProjectiveConstraint::removeIndex(Index index) +{ + sofa::type::removeValue(*f_indices.beginEdit(),index); + f_indices.endEdit(); +} + +// -- Constraint interface +template +void FixedTranslationProjectiveConstraint::init() +{ + this->core::behavior::ProjectiveConstraintSet::init(); + + if (l_topology.empty()) + { + msg_info() << "link to Topology container should be set to ensure right behavior. First Topology found in current context will be used."; + l_topology.set(this->getContext()->getMeshTopologyLink()); + } + + if (sofa::core::topology::BaseMeshTopology* _topology = l_topology.get()) + { + msg_info() << "Topology path used: '" << l_topology.getLinkedPath() << "'"; + + // Initialize topological changes support + f_indices.createTopologyHandler(_topology); + f_coordinates.createTopologyHandler(_topology); + } + else + { + msg_info() << "No topology component found at path: " << l_topology.getLinkedPath() << ", nor in current context: " << this->getContext()->name; + } +} + + +template +static inline void clearPos(defaulttype::RigidDeriv& v) +{ + getVCenter(v).clear(); +} + +template +static inline void clearPos(type::Vec<6,T>& v) +{ + for (unsigned int i=0; i<3; ++i) + v[i] = 0; +} + +template template +void FixedTranslationProjectiveConstraint::projectResponseT(DataDeriv& dx, + const std::function& clear) +{ + if (f_fixAll.getValue()) + { + for (std::size_t i = 0; i < dx.size(); i++) + { + clear(dx, i); + } + } + else + { + const SetIndexArray & indices = f_indices.getValue(); + for (const auto index : indices) + { + clear(dx, index); + } + } +} + +template +void FixedTranslationProjectiveConstraint::projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& resData) +{ + SOFA_UNUSED(mparams); + helper::WriteAccessor res = resData; + projectResponseT(res.wref(), [](auto& dx, const unsigned int index) {dx[index].clear(); }); +} + +template +void FixedTranslationProjectiveConstraint::projectVelocity(const core::MechanicalParams* /*mparams*/, DataVecDeriv& /*vData*/) +{ + +} + +template +void FixedTranslationProjectiveConstraint::projectPosition(const core::MechanicalParams* /*mparams*/, DataVecCoord& /*xData*/) +{ + +} + +template +void FixedTranslationProjectiveConstraint::projectJacobianMatrix(const core::MechanicalParams* mparams, DataMatrixDeriv& cData) +{ + SOFA_UNUSED(mparams); + helper::WriteAccessor c = cData; + projectResponseT(c.wref(), [](MatrixDeriv& res, const unsigned int index) { res.clearColBlock(index); }); +} + + +template +void FixedTranslationProjectiveConstraint::draw(const core::visual::VisualParams* vparams) +{ + const SetIndexArray & indices = f_indices.getValue(); + if (!vparams->displayFlags().getShowBehaviorModels()) + return; + const VecCoord& x = this->mstate->read(core::ConstVecCoordId::position())->getValue(); + + const auto stateLifeCycle = vparams->drawTool()->makeStateLifeCycle(); + vparams->drawTool()->disableLighting(); + + std::vector vertices; + constexpr sofa::type::RGBAColor color(1, 0.5, 0.5, 1); + + if (f_fixAll.getValue() == true) + { + for (unsigned i = 0; i < x.size(); i++) + { + sofa::type::Vec3 v; + const typename DataTypes::CPos& cpos = DataTypes::getCPos(x[i]); + for(Size j=0 ; jdrawTool()->drawPoints(vertices, 10, color); + + +} + +} // namespace sofa::component::constraint::projective diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/HermiteSplineConstraint.h b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/HermiteSplineConstraint.h index 858a5ab5a6c..9d2f358ef52 100644 --- a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/HermiteSplineConstraint.h +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/HermiteSplineConstraint.h @@ -20,115 +20,13 @@ * Contact information: contact@sofa-framework.org * ******************************************************************************/ #pragma once -#include -#include -#include -#include -#include +#include -namespace sofa::component::constraint::projective -{ +SOFA_DEPRECATED_HEADER("v24.06", "v25.06", "sofa/component/constraint/projective/HermiteSplineProjectiveConstraint.h") -/** - Impose a trajectory to given Dofs following a Hermite cubic spline constraint. - Control parameters are : - - begin and end points - - derivates at this points - - acceleration curve on the trajectory - */ -template -class HermiteSplineConstraint : public core::behavior::ProjectiveConstraintSet +namespace sofa::component::constraint::projective { -public: - SOFA_CLASS(SOFA_TEMPLATE(HermiteSplineConstraint,DataTypes),SOFA_TEMPLATE(sofa::core::behavior::ProjectiveConstraintSet, DataTypes)); - - typedef typename DataTypes::VecCoord VecCoord; - typedef typename DataTypes::VecDeriv VecDeriv; - typedef typename DataTypes::MatrixDeriv MatrixDeriv; - typedef typename DataTypes::Coord Coord; - typedef typename DataTypes::Deriv Deriv; - typedef typename Coord::value_type Real; - typedef typename MatrixDeriv::RowIterator MatrixDerivRowIterator; - typedef typename MatrixDeriv::RowType MatrixDerivRowType; - typedef Data DataVecCoord; - typedef Data DataVecDeriv; - typedef Data DataMatrixDeriv; - typedef type::vector SetIndexArray; - typedef sofa::core::topology::TopologySubsetIndices SetIndex; - typedef typename type::Vec<3, Real> Vec3R; - typedef typename type::Vec<2, Real> Vec2R; - typedef typename type::Quat QuatR; - -public: - ///indices of the DOFs constraints - SetIndex m_indices; - - /// the time steps defining the duration of the constraint - Data m_tBegin; - Data m_tEnd; ///< End Time of the motion - - /// control parameters : - /// first control point - Data m_x0; - /// first derivated control point - Data m_dx0; - /// second control point - Data m_x1; - /// second derivated control point - Data m_dx1; - /// acceleration parameters : the accaleration curve is itself a hermite spline, with first point at (0,0) and second at (1,1) - /// and derivated on this points are : - Data m_sx0; - Data m_sx1; ///< second interpolation vector - - /// Link to be set to the topology container in the component graph. - SingleLink, sofa::core::topology::BaseMeshTopology, BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_topology; - -protected: - explicit HermiteSplineConstraint(core::behavior::MechanicalState* mstate = nullptr); - - ~HermiteSplineConstraint(); -public: - void clearConstraints(); - void addConstraint(unsigned index ); - - void setBeginTime(const Real &t) {m_tBegin.setValue(t);} - void setEndTime(const Real &t) {m_tEnd.setValue(t);} - - Real getBeginTime() {return m_tBegin.getValue();} - Real getEndTime() {return m_tEnd.getValue();} - - void computeHermiteCoefs( const Real u, Real &H00, Real &H10, Real &H01, Real &H11); - void computeDerivateHermiteCoefs( const Real u, Real &dH00, Real &dH10, Real &dH01, Real &dH11); - - /// -- Constraint interface - void init() override; - void reinit() override; - - - void projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& resData) override; - void projectVelocity(const core::MechanicalParams* mparams, DataVecDeriv& vData) override; - void projectPosition(const core::MechanicalParams* mparams, DataVecCoord& xData) override; - void projectJacobianMatrix(const core::MechanicalParams* mparams, DataMatrixDeriv& cData) override; - - void draw(const core::visual::VisualParams* vparams) override; - -protected: - template - void projectResponseT(DataDeriv& dx, - const std::function& clear); - -}; - -template <> -void SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API HermiteSplineConstraint::init(); - - -#if !defined(SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_HERMITESPLINECONSTRAINT_CPP) -extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API HermiteSplineConstraint; -extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API HermiteSplineConstraint; - -#endif - -} // namespace sofa::component::constraint::projective +template +using HermiteSplineConstraint SOFA_ATTRIBUTE_DEPRECATED("v24.06 ", "v25.06", "HermiteSplineConstraint has been renamed to HermiteSplineProjectiveConstraint") = HermiteSplineProjectiveConstraint; +} \ No newline at end of file diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/HermiteSplineConstraint.inl b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/HermiteSplineConstraint.inl index 6fc823bf0c1..6064b936335 100644 --- a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/HermiteSplineConstraint.inl +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/HermiteSplineConstraint.inl @@ -21,247 +21,6 @@ ******************************************************************************/ #pragma once -#include -#include -#include +#include -namespace sofa::component::constraint::projective -{ - -template -HermiteSplineConstraint::HermiteSplineConstraint(core::behavior::MechanicalState* mstate) - : core::behavior::ProjectiveConstraintSet(mstate) - , m_indices( initData(&m_indices,"indices","Indices of the constrained points") ) - , m_tBegin(initData(&m_tBegin,"BeginTime","Begin Time of the motion") ) - , m_tEnd(initData(&m_tEnd,"EndTime","End Time of the motion") ) - , m_x0(initData(&m_x0,"X0","first control point") ) - , m_dx0(initData(&m_dx0,"dX0","first control tangente") ) - , m_x1(initData(&m_x1,"X1","second control point") ) - , m_dx1(initData(&m_dx1,"dX1","sceond control tangente") ) - , m_sx0(initData(&m_sx0,"SX0","first interpolation vector") ) - , m_sx1(initData(&m_sx1,"SX1","second interpolation vector") ) - , l_topology(initLink("topology", "link to the topology container")) -{ -} - -template -HermiteSplineConstraint::~HermiteSplineConstraint() -{ -} - -template -void HermiteSplineConstraint::clearConstraints() -{ - m_indices.beginEdit()->clear(); - m_indices.endEdit(); -} - -template -void HermiteSplineConstraint::addConstraint(unsigned index) -{ - m_indices.beginEdit()->push_back(index); - m_indices.endEdit(); -} - - -template -void HermiteSplineConstraint::init() -{ - this->core::behavior::ProjectiveConstraintSet::init(); - - if (l_topology.empty()) - { - msg_info() << "link to Topology container should be set to ensure right behavior. First Topology found in current context will be used."; - l_topology.set(this->getContext()->getMeshTopologyLink()); - } - - if (sofa::core::topology::BaseMeshTopology* _topology = l_topology.get()) - { - msg_info() << "Topology path used: '" << l_topology.getLinkedPath() << "'"; - - // Initialize functions and parameters for topology data and handler - m_indices.createTopologyHandler(_topology); - } - else - { - msg_info() << "No topology component found at path: " << l_topology.getLinkedPath() << ", nor in current context: " << this->getContext()->name; - } -} - -template -void HermiteSplineConstraint::reinit() -{ - init(); -} - - -template -void HermiteSplineConstraint::computeHermiteCoefs( const Real u, Real &H00, Real &H10, Real &H01, Real &H11) -{ - //-- time interpolation --> acceleration is itself computed from hemite - Real u2 = u*u; - Real u3 = u*u*u; - //Real uH00 = 2*u3 -3*u2 +1 ; //hermite coefs - Real uH10 = u3 -2*u2 +u; - Real uH01 = -2*u3 + 3*u2; - Real uH11 = u3 -u2; - Vec2R pu = m_sx0.getValue()*uH10 + Vec2R(1,1)*uH01 + m_sx1.getValue()*uH11; - Real su = pu.y(); - - Real su2 = su*su; - Real su3 = su*su*su; - H00 = 2*su3 -3*su2 +1 ; - H10 = su3 -2*su2 +su; - H01 = -2*su3 + 3*su2; - H11 = su3 -su2; -} - -template -void HermiteSplineConstraint::computeDerivateHermiteCoefs( const Real u, Real &dH00, Real &dH10, Real &dH01, Real &dH11) -{ - //-- time interpolation --> acceleration is itself computed from hemite - Real u2 = u*u; - Real u3 = u*u*u; - Real uH10 = u3 -2*u2 +u; - Real uH01 = -2*u3 + 3*u2; - Real uH11 = u3 -u2; - Vec2R pu = m_sx0.getValue()*uH10 + Vec2R(1,1)*uH01 + m_sx1.getValue()*uH11; - Real su = pu.y(); - - Real su2 = su*su; - dH00 = 6*su2 -6*su ; - dH10 = 3*su2 -4*su +1; - dH01 = -6*su2 + 6*su; - dH11 = 3*su2 -2*su; -} - - -template template -void HermiteSplineConstraint::projectResponseT(DataDeriv& dx, - const std::function& clear) -{ - Real t = (Real) this->getContext()->getTime(); - if ( t >= m_tBegin.getValue() && t <= m_tEnd.getValue()) - { - const SetIndexArray & indices = m_indices.getValue(); - for(SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) - clear(dx, *it); - } -} - -template -void HermiteSplineConstraint::projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& resData) -{ - SOFA_UNUSED(mparams); - helper::WriteAccessor res = resData; - projectResponseT(res.wref(), [](auto& dx, const unsigned int index) {dx[index].clear();}); -} - -template -void HermiteSplineConstraint::projectVelocity(const core::MechanicalParams* /*mparams*/, DataVecDeriv& vData) -{ - helper::WriteAccessor dx = vData; - Real t = (Real) this->getContext()->getTime(); - - if ( t >= m_tBegin.getValue() && t <= m_tEnd.getValue() ) - { - Real DT = m_tEnd.getValue() - m_tBegin.getValue(); - const SetIndexArray & indices = m_indices.getValue(); - - t -= m_tBegin.getValue(); - Real u = t/DT; - - Real dH00, dH10, dH01, dH11; - computeDerivateHermiteCoefs( u, dH00, dH10, dH01, dH11); - - for(SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) - { - dx[*it] = m_x0.getValue()*dH00 + m_dx0.getValue()*dH10 + m_x1.getValue()*dH01 + m_dx1.getValue()*dH11; - } - } -} - -template -void HermiteSplineConstraint::projectPosition(const core::MechanicalParams* /*mparams*/, DataVecCoord& xData) -{ - helper::WriteAccessor x = xData; - Real t = (Real) this->getContext()->getTime(); - - if ( t >= m_tBegin.getValue() && t <= m_tEnd.getValue() ) - { - Real DT = m_tEnd.getValue() - m_tBegin.getValue(); - const SetIndexArray & indices = m_indices.getValue(); - - t -= m_tBegin.getValue(); - Real u = t/DT; - - Real H00, H10, H01, H11; - computeHermiteCoefs( u, H00, H10, H01, H11); - - for(SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) - { - x[*it] = m_x0.getValue()*H00 + m_dx0.getValue()*H10 + m_x1.getValue()*H01 + m_dx1.getValue()*H11; - } - } -} - -template -void HermiteSplineConstraint::projectJacobianMatrix(const core::MechanicalParams* mparams, DataMatrixDeriv& cData) -{ - SOFA_UNUSED(mparams); - helper::WriteAccessor c = cData; - - projectResponseT(c.wref(), [](MatrixDeriv& res, const unsigned int index) { res.clearColBlock(index); }); -} - -template -void HermiteSplineConstraint::draw(const core::visual::VisualParams* vparams) -{ - if (!vparams->displayFlags().getShowBehaviorModels()) return; - - Real dt = (Real) this->getContext()->getDt(); - Real DT = m_tEnd.getValue() - m_tBegin.getValue(); - - const auto stateLifeCycle = vparams->drawTool()->makeStateLifeCycle(); - vparams->drawTool()->disableLighting(); - - std::vector vertices; - constexpr sofa::type::RGBAColor color(1, 0.5, 0.5, 1); - - const Vec3R& mx0 = m_x0.getValue(); - const Vec3R& mx1 = m_x0.getValue(); - const Vec3R& mdx0 = m_dx0.getValue(); - const Vec3R& mdx1 = m_dx1.getValue(); - - for (Real t=0.0 ; t< DT ; t+= dt) - { - Real u = t/DT; - - Real H00, H10, H01, H11; - computeHermiteCoefs( u, H00, H10, H01, H11); - - Vec3R p = mx0*H00 + mdx0*H10 + mx1*H01 + mdx1*H11; - - sofa::type::Vec3 v(p[0], p[1],p[2]); - vertices.push_back(v); - } - vparams->drawTool()->drawLineStrip(vertices, 2, color); - - vertices.clear(); - vertices.push_back(sofa::type::Vec3(mx0[0], mx0[1], mx0[2])); - vertices.push_back(sofa::type::Vec3(mx1[0], mx1[1], mx1[2])); - - vparams->drawTool()->drawPoints(vertices, 5.0, sofa::type::RGBAColor::red()); - - //display control tangents - vertices.clear(); - vertices.push_back(mx0); - vertices.push_back(mx0 + mdx0*0.1); - vertices.push_back(mx1); - vertices.push_back(mx1 + mdx1*0.1); - - vparams->drawTool()->drawLines(vertices, 1.0, sofa::type::RGBAColor::red()); - -} - -} // namespace sofa::component::constraint::projective +SOFA_DEPRECATED_HEADER("v24.06", "v25.06", "sofa/component/constraint/projective/HermiteSplineProjectiveConstraint.inl") diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/HermiteSplineConstraint.cpp b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/HermiteSplineProjectiveConstraint.cpp similarity index 76% rename from Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/HermiteSplineConstraint.cpp rename to Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/HermiteSplineProjectiveConstraint.cpp index 1d6ec21d8bb..6dd49852e21 100644 --- a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/HermiteSplineConstraint.cpp +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/HermiteSplineProjectiveConstraint.cpp @@ -19,8 +19,8 @@ * * * Contact information: contact@sofa-framework.org * ******************************************************************************/ -#define SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_HERMITESPLINECONSTRAINT_CPP -#include +#define SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_HERMITESPLINEPROJECTIVECONSTRAINT_CPP +#include #include #include #include @@ -28,18 +28,19 @@ namespace sofa::component::constraint::projective { -int HermiteSplineConstraintClass = core::RegisterObject("Apply a hermite cubic spline trajectory to given points") - .add< HermiteSplineConstraint >() - .add< HermiteSplineConstraint >() +int HermiteSplineProjectiveConstraintClass = core::RegisterObject("Apply a hermite cubic spline trajectory to given points") + .add< HermiteSplineProjectiveConstraint >() + .add< HermiteSplineProjectiveConstraint >() + .addAlias("HermiteSplineConstraint") ; template <> -void HermiteSplineConstraint::init() +void HermiteSplineProjectiveConstraint::init() { this->core::behavior::ProjectiveConstraintSet::init(); } -template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API HermiteSplineConstraint; -template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API HermiteSplineConstraint; +template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API HermiteSplineProjectiveConstraint; +template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API HermiteSplineProjectiveConstraint; } // namespace sofa::component::constraint::projective diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/HermiteSplineProjectiveConstraint.h b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/HermiteSplineProjectiveConstraint.h new file mode 100644 index 00000000000..1777e0bda2f --- /dev/null +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/HermiteSplineProjectiveConstraint.h @@ -0,0 +1,134 @@ +/****************************************************************************** +* 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::component::constraint::projective +{ + +/** + Impose a trajectory to given Dofs following a Hermite cubic spline constraint. + Control parameters are : + - begin and end points + - derivates at this points + - acceleration curve on the trajectory + */ +template +class HermiteSplineProjectiveConstraint : public core::behavior::ProjectiveConstraintSet +{ +public: + SOFA_CLASS(SOFA_TEMPLATE(HermiteSplineProjectiveConstraint,DataTypes),SOFA_TEMPLATE(sofa::core::behavior::ProjectiveConstraintSet, DataTypes)); + + typedef typename DataTypes::VecCoord VecCoord; + typedef typename DataTypes::VecDeriv VecDeriv; + typedef typename DataTypes::MatrixDeriv MatrixDeriv; + typedef typename DataTypes::Coord Coord; + typedef typename DataTypes::Deriv Deriv; + typedef typename Coord::value_type Real; + typedef typename MatrixDeriv::RowIterator MatrixDerivRowIterator; + typedef typename MatrixDeriv::RowType MatrixDerivRowType; + typedef Data DataVecCoord; + typedef Data DataVecDeriv; + typedef Data DataMatrixDeriv; + typedef type::vector SetIndexArray; + typedef sofa::core::topology::TopologySubsetIndices SetIndex; + typedef typename type::Vec<3, Real> Vec3R; + typedef typename type::Vec<2, Real> Vec2R; + typedef typename type::Quat QuatR; + +public: + ///indices of the DOFs constraints + SetIndex m_indices; + + /// the time steps defining the duration of the constraint + Data m_tBegin; + Data m_tEnd; ///< End Time of the motion + + /// control parameters : + /// first control point + Data m_x0; + /// first derivated control point + Data m_dx0; + /// second control point + Data m_x1; + /// second derivated control point + Data m_dx1; + /// acceleration parameters : the accaleration curve is itself a hermite spline, with first point at (0,0) and second at (1,1) + /// and derivated on this points are : + Data m_sx0; + Data m_sx1; ///< second interpolation vector + + /// Link to be set to the topology container in the component graph. + SingleLink, sofa::core::topology::BaseMeshTopology, BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_topology; + +protected: + explicit HermiteSplineProjectiveConstraint(core::behavior::MechanicalState* mstate = nullptr); + + ~HermiteSplineProjectiveConstraint(); +public: + void clearConstraints(); + void addConstraint(unsigned index ); + + void setBeginTime(const Real &t) {m_tBegin.setValue(t);} + void setEndTime(const Real &t) {m_tEnd.setValue(t);} + + Real getBeginTime() {return m_tBegin.getValue();} + Real getEndTime() {return m_tEnd.getValue();} + + void computeHermiteCoefs( const Real u, Real &H00, Real &H10, Real &H01, Real &H11); + void computeDerivateHermiteCoefs( const Real u, Real &dH00, Real &dH10, Real &dH01, Real &dH11); + + /// -- Constraint interface + void init() override; + void reinit() override; + + + void projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& resData) override; + void projectVelocity(const core::MechanicalParams* mparams, DataVecDeriv& vData) override; + void projectPosition(const core::MechanicalParams* mparams, DataVecCoord& xData) override; + void projectJacobianMatrix(const core::MechanicalParams* mparams, DataMatrixDeriv& cData) override; + + void draw(const core::visual::VisualParams* vparams) override; + +protected: + template + void projectResponseT(DataDeriv& dx, + const std::function& clear); + +}; + +template <> +void SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API HermiteSplineProjectiveConstraint::init(); + + +#if !defined(SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_HERMITESPLINEPROJECTIVECONSTRAINT_CPP) +extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API HermiteSplineProjectiveConstraint; +extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API HermiteSplineProjectiveConstraint; + +#endif + +} // namespace sofa::component::constraint::projective diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/HermiteSplineProjectiveConstraint.inl b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/HermiteSplineProjectiveConstraint.inl new file mode 100644 index 00000000000..eeef7815197 --- /dev/null +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/HermiteSplineProjectiveConstraint.inl @@ -0,0 +1,267 @@ +/****************************************************************************** +* 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::constraint::projective +{ + +template +HermiteSplineProjectiveConstraint::HermiteSplineProjectiveConstraint(core::behavior::MechanicalState* mstate) + : core::behavior::ProjectiveConstraintSet(mstate) + , m_indices( initData(&m_indices,"indices","Indices of the constrained points") ) + , m_tBegin(initData(&m_tBegin,"BeginTime","Begin Time of the motion") ) + , m_tEnd(initData(&m_tEnd,"EndTime","End Time of the motion") ) + , m_x0(initData(&m_x0,"X0","first control point") ) + , m_dx0(initData(&m_dx0,"dX0","first control tangente") ) + , m_x1(initData(&m_x1,"X1","second control point") ) + , m_dx1(initData(&m_dx1,"dX1","sceond control tangente") ) + , m_sx0(initData(&m_sx0,"SX0","first interpolation vector") ) + , m_sx1(initData(&m_sx1,"SX1","second interpolation vector") ) + , l_topology(initLink("topology", "link to the topology container")) +{ +} + +template +HermiteSplineProjectiveConstraint::~HermiteSplineProjectiveConstraint() +{ +} + +template +void HermiteSplineProjectiveConstraint::clearConstraints() +{ + m_indices.beginEdit()->clear(); + m_indices.endEdit(); +} + +template +void HermiteSplineProjectiveConstraint::addConstraint(unsigned index) +{ + m_indices.beginEdit()->push_back(index); + m_indices.endEdit(); +} + + +template +void HermiteSplineProjectiveConstraint::init() +{ + this->core::behavior::ProjectiveConstraintSet::init(); + + if (l_topology.empty()) + { + msg_info() << "link to Topology container should be set to ensure right behavior. First Topology found in current context will be used."; + l_topology.set(this->getContext()->getMeshTopologyLink()); + } + + if (sofa::core::topology::BaseMeshTopology* _topology = l_topology.get()) + { + msg_info() << "Topology path used: '" << l_topology.getLinkedPath() << "'"; + + // Initialize functions and parameters for topology data and handler + m_indices.createTopologyHandler(_topology); + } + else + { + msg_info() << "No topology component found at path: " << l_topology.getLinkedPath() << ", nor in current context: " << this->getContext()->name; + } +} + +template +void HermiteSplineProjectiveConstraint::reinit() +{ + init(); +} + + +template +void HermiteSplineProjectiveConstraint::computeHermiteCoefs( const Real u, Real &H00, Real &H10, Real &H01, Real &H11) +{ + //-- time interpolation --> acceleration is itself computed from hemite + Real u2 = u*u; + Real u3 = u*u*u; + //Real uH00 = 2*u3 -3*u2 +1 ; //hermite coefs + Real uH10 = u3 -2*u2 +u; + Real uH01 = -2*u3 + 3*u2; + Real uH11 = u3 -u2; + Vec2R pu = m_sx0.getValue()*uH10 + Vec2R(1,1)*uH01 + m_sx1.getValue()*uH11; + Real su = pu.y(); + + Real su2 = su*su; + Real su3 = su*su*su; + H00 = 2*su3 -3*su2 +1 ; + H10 = su3 -2*su2 +su; + H01 = -2*su3 + 3*su2; + H11 = su3 -su2; +} + +template +void HermiteSplineProjectiveConstraint::computeDerivateHermiteCoefs( const Real u, Real &dH00, Real &dH10, Real &dH01, Real &dH11) +{ + //-- time interpolation --> acceleration is itself computed from hemite + Real u2 = u*u; + Real u3 = u*u*u; + Real uH10 = u3 -2*u2 +u; + Real uH01 = -2*u3 + 3*u2; + Real uH11 = u3 -u2; + Vec2R pu = m_sx0.getValue()*uH10 + Vec2R(1,1)*uH01 + m_sx1.getValue()*uH11; + Real su = pu.y(); + + Real su2 = su*su; + dH00 = 6*su2 -6*su ; + dH10 = 3*su2 -4*su +1; + dH01 = -6*su2 + 6*su; + dH11 = 3*su2 -2*su; +} + + +template template +void HermiteSplineProjectiveConstraint::projectResponseT(DataDeriv& dx, + const std::function& clear) +{ + Real t = (Real) this->getContext()->getTime(); + if ( t >= m_tBegin.getValue() && t <= m_tEnd.getValue()) + { + const SetIndexArray & indices = m_indices.getValue(); + for(SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) + clear(dx, *it); + } +} + +template +void HermiteSplineProjectiveConstraint::projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& resData) +{ + SOFA_UNUSED(mparams); + helper::WriteAccessor res = resData; + projectResponseT(res.wref(), [](auto& dx, const unsigned int index) {dx[index].clear();}); +} + +template +void HermiteSplineProjectiveConstraint::projectVelocity(const core::MechanicalParams* /*mparams*/, DataVecDeriv& vData) +{ + helper::WriteAccessor dx = vData; + Real t = (Real) this->getContext()->getTime(); + + if ( t >= m_tBegin.getValue() && t <= m_tEnd.getValue() ) + { + Real DT = m_tEnd.getValue() - m_tBegin.getValue(); + const SetIndexArray & indices = m_indices.getValue(); + + t -= m_tBegin.getValue(); + Real u = t/DT; + + Real dH00, dH10, dH01, dH11; + computeDerivateHermiteCoefs( u, dH00, dH10, dH01, dH11); + + for(SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) + { + dx[*it] = m_x0.getValue()*dH00 + m_dx0.getValue()*dH10 + m_x1.getValue()*dH01 + m_dx1.getValue()*dH11; + } + } +} + +template +void HermiteSplineProjectiveConstraint::projectPosition(const core::MechanicalParams* /*mparams*/, DataVecCoord& xData) +{ + helper::WriteAccessor x = xData; + Real t = (Real) this->getContext()->getTime(); + + if ( t >= m_tBegin.getValue() && t <= m_tEnd.getValue() ) + { + Real DT = m_tEnd.getValue() - m_tBegin.getValue(); + const SetIndexArray & indices = m_indices.getValue(); + + t -= m_tBegin.getValue(); + Real u = t/DT; + + Real H00, H10, H01, H11; + computeHermiteCoefs( u, H00, H10, H01, H11); + + for(SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) + { + x[*it] = m_x0.getValue()*H00 + m_dx0.getValue()*H10 + m_x1.getValue()*H01 + m_dx1.getValue()*H11; + } + } +} + +template +void HermiteSplineProjectiveConstraint::projectJacobianMatrix(const core::MechanicalParams* mparams, DataMatrixDeriv& cData) +{ + SOFA_UNUSED(mparams); + helper::WriteAccessor c = cData; + + projectResponseT(c.wref(), [](MatrixDeriv& res, const unsigned int index) { res.clearColBlock(index); }); +} + +template +void HermiteSplineProjectiveConstraint::draw(const core::visual::VisualParams* vparams) +{ + if (!vparams->displayFlags().getShowBehaviorModels()) return; + + Real dt = (Real) this->getContext()->getDt(); + Real DT = m_tEnd.getValue() - m_tBegin.getValue(); + + const auto stateLifeCycle = vparams->drawTool()->makeStateLifeCycle(); + vparams->drawTool()->disableLighting(); + + std::vector vertices; + constexpr sofa::type::RGBAColor color(1, 0.5, 0.5, 1); + + const Vec3R& mx0 = m_x0.getValue(); + const Vec3R& mx1 = m_x0.getValue(); + const Vec3R& mdx0 = m_dx0.getValue(); + const Vec3R& mdx1 = m_dx1.getValue(); + + for (Real t=0.0 ; t< DT ; t+= dt) + { + Real u = t/DT; + + Real H00, H10, H01, H11; + computeHermiteCoefs( u, H00, H10, H01, H11); + + Vec3R p = mx0*H00 + mdx0*H10 + mx1*H01 + mdx1*H11; + + sofa::type::Vec3 v(p[0], p[1],p[2]); + vertices.push_back(v); + } + vparams->drawTool()->drawLineStrip(vertices, 2, color); + + vertices.clear(); + vertices.push_back(sofa::type::Vec3(mx0[0], mx0[1], mx0[2])); + vertices.push_back(sofa::type::Vec3(mx1[0], mx1[1], mx1[2])); + + vparams->drawTool()->drawPoints(vertices, 5.0, sofa::type::RGBAColor::red()); + + //display control tangents + vertices.clear(); + vertices.push_back(mx0); + vertices.push_back(mx0 + mdx0*0.1); + vertices.push_back(mx1); + vertices.push_back(mx1 + mdx1*0.1); + + vparams->drawTool()->drawLines(vertices, 1.0, sofa::type::RGBAColor::red()); + +} + +} // namespace sofa::component::constraint::projective diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/ProjectToPlaneConstraint.cpp b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/LineProjectiveConstraint.cpp similarity index 78% rename from Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/ProjectToPlaneConstraint.cpp rename to Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/LineProjectiveConstraint.cpp index 8d584ac88e0..4ac4c9552ec 100644 --- a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/ProjectToPlaneConstraint.cpp +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/LineProjectiveConstraint.cpp @@ -19,8 +19,8 @@ * * * Contact information: contact@sofa-framework.org * ******************************************************************************/ -#define SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_ProjectToPlaneConstraint_CPP -#include +#define SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_LineProjectiveConstraint_CPP +#include #include namespace sofa::component::constraint::projective @@ -30,14 +30,13 @@ using namespace sofa::defaulttype; using namespace sofa::helper; -int ProjectToPlaneConstraintClass = core::RegisterObject("Attach given particles to their initial positions") - .add< ProjectToPlaneConstraint >() - .add< ProjectToPlaneConstraint >() - +int LineProjectiveConstraintClass = core::RegisterObject("Attach given particles to their initial positions") + .add< LineProjectiveConstraint >() + .add< LineProjectiveConstraint >() + .addAlias("ProjectToLineConstraint") ; -template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API ProjectToPlaneConstraint; -template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API ProjectToPlaneConstraint; +template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API LineProjectiveConstraint; +template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API LineProjectiveConstraint; } // namespace sofa::component::constraint::projective - diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/LineProjectiveConstraint.h b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/LineProjectiveConstraint.h new file mode 100644 index 00000000000..5a907598823 --- /dev/null +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/LineProjectiveConstraint.h @@ -0,0 +1,137 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace sofa::component::constraint::projective +{ + +/// This class can be overridden if needed for additionnal storage within template specializations. +template +class LineProjectiveConstraintInternalData +{ + +}; + +/** Project particles to an affine straight line. + @author Francois Faure, 2012 + @todo Optimized versions for lines parallel to the main directions +*/ +template +class LineProjectiveConstraint : public core::behavior::ProjectiveConstraintSet +{ +public: + SOFA_CLASS(SOFA_TEMPLATE(LineProjectiveConstraint,DataTypes),SOFA_TEMPLATE(sofa::core::behavior::ProjectiveConstraintSet, DataTypes)); + + using Index = sofa::Index; + typedef typename DataTypes::VecCoord VecCoord; + typedef typename DataTypes::VecDeriv VecDeriv; + typedef typename DataTypes::MatrixDeriv MatrixDeriv; + typedef typename DataTypes::Coord Coord; + typedef typename DataTypes::Deriv Deriv; + typedef typename DataTypes::CPos CPos; + typedef typename MatrixDeriv::RowIterator MatrixDerivRowIterator; + typedef typename MatrixDeriv::RowType MatrixDerivRowType; + typedef Data DataVecCoord; + typedef Data DataVecDeriv; + typedef Data DataMatrixDeriv; + SOFA_ATTRIBUTE_REPLACED__TYPEMEMBER(Vector3, sofa::type::Vec3); + typedef type::vector Indices; + typedef sofa::core::topology::TopologySubsetIndices IndexSubsetData; + typedef linearalgebra::EigenBaseSparseMatrix BaseSparseMatrix; + typedef linearalgebra::EigenSparseMatrix SparseMatrix; + typedef typename SparseMatrix::Block Block; ///< projection matrix of a particle displacement to the plane + enum {bsize=SparseMatrix::Nin}; ///< size of a block + + +protected: + LineProjectiveConstraint(); + + virtual ~LineProjectiveConstraint(); + +public: + IndexSubsetData f_indices; ///< the particles to project + Data f_drawSize; ///< The size of the square used to display the constrained particles + Data f_origin; ///< A point on the line + Data f_direction; ///< The direction of the line. Will be normalized by init() + + /// Link to be set to the topology container in the component graph. + SingleLink, sofa::core::topology::BaseMeshTopology, BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_topology; + +protected: + LineProjectiveConstraintInternalData* data; + friend class LineProjectiveConstraintInternalData; + + +public: + void clearConstraints(); + void addConstraint(Index index); + void removeConstraint(Index index); + + // -- Constraint interface + void init() override; + void reinit() override; + + void projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& resData) override; + void projectVelocity(const core::MechanicalParams* mparams, DataVecDeriv& vData) override; + void projectPosition(const core::MechanicalParams* mparams, DataVecCoord& xData) override; + void projectJacobianMatrix(const core::MechanicalParams* mparams, DataMatrixDeriv& cData) override; + + void applyConstraint(const core::MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) override; + void applyConstraint(const core::MechanicalParams* mparams, linearalgebra::BaseVector* vector, const sofa::core::behavior::MultiMatrixAccessor* matrix) override; + + /// Project the given matrix (Experimental API, see the spec in sofa::core::behavior::BaseProjectiveConstraintSet). + void projectMatrix( sofa::linearalgebra::BaseMatrix* /*M*/, unsigned /*offset*/ ) override; + + + void draw(const core::visual::VisualParams* vparams) override; + +protected : + + SparseMatrix jacobian; ///< projection matrix in local state + SparseMatrix J; ///< auxiliary variable + + /// Resize/update Jacobian matrix according to the linked mechanical state and the direction + void updateJacobian(); + +}; + + +#if !defined(SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_LineProjectiveConstraint_CPP) +extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API LineProjectiveConstraint; +extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API LineProjectiveConstraint; +#endif + +} // namespace sofa::component::constraint::projective diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/LineProjectiveConstraint.inl b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/LineProjectiveConstraint.inl new file mode 100644 index 00000000000..2111e6be0f1 --- /dev/null +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/LineProjectiveConstraint.inl @@ -0,0 +1,286 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace sofa::component::constraint::projective +{ + +template +LineProjectiveConstraint::LineProjectiveConstraint() + : core::behavior::ProjectiveConstraintSet(nullptr) + , f_indices( initData(&f_indices,"indices","Indices of the fixed points") ) + , f_drawSize( initData(&f_drawSize,(SReal)0.0,"drawSize","0 -> point based rendering, >0 -> radius of spheres") ) + , f_origin( initData(&f_origin,CPos(),"origin","A point in the line")) + , f_direction( initData(&f_direction,CPos(),"direction","Direction of the line")) + , l_topology(initLink("topology", "link to the topology container")) + , data(new LineProjectiveConstraintInternalData()) +{ + f_indices.beginEdit()->push_back(0); + f_indices.endEdit(); +} + + +template +LineProjectiveConstraint::~LineProjectiveConstraint() +{ + delete data; +} + +template +void LineProjectiveConstraint::clearConstraints() +{ + f_indices.beginEdit()->clear(); + f_indices.endEdit(); +} + +template +void LineProjectiveConstraint::addConstraint(Index index) +{ + f_indices.beginEdit()->push_back(index); + f_indices.endEdit(); +} + +template +void LineProjectiveConstraint::removeConstraint(Index index) +{ + sofa::type::removeValue(*f_indices.beginEdit(),index); + f_indices.endEdit(); +} + +// -- Constraint interface + + +template +void LineProjectiveConstraint::init() +{ + this->core::behavior::ProjectiveConstraintSet::init(); + + if (l_topology.empty()) + { + msg_info() << "link to Topology container should be set to ensure right behavior. First Topology found in current context will be used."; + l_topology.set(this->getContext()->getMeshTopologyLink()); + } + + if (sofa::core::topology::BaseMeshTopology* _topology = l_topology.get()) + { + msg_info() << "Topology path used: '" << l_topology.getLinkedPath() << "'"; + + // Initialize topological changes support + f_indices.createTopologyHandler(_topology); + } + else + { + msg_info() << "No topology component found at path: " << l_topology.getLinkedPath() << ", nor in current context: " << this->getContext()->name; + } + + const Indices & indices = f_indices.getValue(); + + const Index maxIndex=this->mstate->getSize(); + for (unsigned int i=0; i= maxIndex) + { + msg_error() << "Index " << index << " not valid!"; + removeConstraint(index); + } + } + + updateJacobian(); +} + +template +void LineProjectiveConstraint::reinit() +{ + updateJacobian(); +} + +template +void LineProjectiveConstraint::updateJacobian() +{ + // normalize the normal vector + CPos n = f_direction.getValue(); + if( n.norm()==0 ) + n[0]=1; // arbritary normal vector + else n *= 1/n.norm(); + f_direction.setValue(n); + + // create the matrix blocks corresponding to the projection to the line: nn^t or to the identity + Block bProjection; + for(unsigned i=0; imstate->getSize(); + const unsigned blockSize = DataTypes::deriv_total_size; + jacobian.resize( numBlocks*blockSize,numBlocks*blockSize ); + + // fill the jacobian in ascending order + Indices::const_iterator it = tmp.begin(); + unsigned i = 0; + while( i < numBlocks ) // (FF) do not stop after the last constrained particle, for the remainder of the diagonal would be null, while it must be identity. + { + if( it!=tmp.end() && i==*it ) // constrained particle: set diagonal to projection block, and the cursor to the next constraint + { + jacobian.insertBackBlock(i,i,bProjection); + ++it; + } + else // unconstrained particle: set diagonal to identity block + { + jacobian.insertBackBlock(i,i,Block::Identity()); + } + i++; + } + jacobian.compress(); +} + +template +void LineProjectiveConstraint::projectMatrix( sofa::linearalgebra::BaseMatrix* M, unsigned offset ) +{ + J.copy(jacobian, M->colSize(), offset); // projection matrix for an assembled state + BaseSparseMatrix* E = dynamic_cast(M); + assert(E); + E->compressedMatrix = J.compressedMatrix * E->compressedMatrix * J.compressedMatrix; +} + + + +template +void LineProjectiveConstraint::projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& resData) +{ + SOFA_UNUSED(mparams); + + helper::WriteAccessor res(resData); + + using Size = decltype(jacobian.colSize()); + if( (jacobian.colSize() / Size(DataTypes::deriv_total_size)) != Size(res.size())) + { + updateJacobian(); + } + + jacobian.mult(res.wref(),res.ref()); +} + +template +void LineProjectiveConstraint::projectJacobianMatrix(const core::MechanicalParams* /*mparams*/ , DataMatrixDeriv& /*cData*/) +{ + msg_error() << "projectJacobianMatrix(const core::MechanicalParams*, DataMatrixDeriv& ) is not implemented"; +} + +template +void LineProjectiveConstraint::projectVelocity(const core::MechanicalParams* mparams, DataVecDeriv& vData) +{ + projectResponse(mparams,vData); +} + +template +void LineProjectiveConstraint::projectPosition(const core::MechanicalParams* /*mparams*/ , DataVecCoord& xData) +{ + VecCoord& x = *xData.beginEdit(); + + const CPos& n = f_direction.getValue(); + const CPos& o = f_origin.getValue(); + + const Indices& indices = f_indices.getValue(); + for(unsigned i=0; i +void LineProjectiveConstraint::applyConstraint(const core::MechanicalParams* /*mparams*/, const sofa::core::behavior::MultiMatrixAccessor* /*matrix*/) +{ + msg_error() << "applyConstraint is not implemented "; +} + +template +void LineProjectiveConstraint::applyConstraint(const core::MechanicalParams* /*mparams*/, linearalgebra::BaseVector* /*vector*/, const sofa::core::behavior::MultiMatrixAccessor* /*matrix*/) +{ + msg_error() << "LineProjectiveConstraint::applyConstraint(const core::MechanicalParams* mparams, linearalgebra::BaseVector* vector, const sofa::core::behavior::MultiMatrixAccessor* matrix) is not implemented "; +} + + + + +template +void LineProjectiveConstraint::draw(const core::visual::VisualParams* vparams) +{ + if (!vparams->displayFlags().getShowBehaviorModels()) return; + if (!this->isActive()) return; + const VecCoord& x = this->mstate->read(core::ConstVecCoordId::position())->getValue(); + + const auto stateLifeCycle = vparams->drawTool()->makeStateLifeCycle(); + + const Indices & indices = f_indices.getValue(); + + if( f_drawSize.getValue() == 0) // old classical drawing by points + { + std::vector< sofa::type::Vec3 > points; + sofa::type::Vec3 point; + + for (Indices::const_iterator it = indices.begin(); + it != indices.end(); + ++it) + { + point = DataTypes::getCPos(x[*it]); + points.push_back(point); + } + vparams->drawTool()->drawPoints(points, 10, sofa::type::RGBAColor(1,0.5,0.5,1)); + } + else // new drawing by spheres + { + std::vector< sofa::type::Vec3 > points; + sofa::type::Vec3 point; + for (unsigned int index : indices) + { + point = DataTypes::getCPos(x[index]); + points.push_back(point); + } + vparams->drawTool()->drawSpheres(points, (float)f_drawSize.getValue(), sofa::type::RGBAColor(1.0f,0.35f,0.35f,1.0f)); + } + + +} + +} // namespace sofa::component::constraint::projective diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/LinearMovementConstraint.h b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/LinearMovementConstraint.h index 4df6f686718..f06082811df 100644 --- a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/LinearMovementConstraint.h +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/LinearMovementConstraint.h @@ -20,149 +20,13 @@ * Contact information: contact@sofa-framework.org * ******************************************************************************/ #pragma once -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include -namespace sofa::component::constraint::projective -{ - -template -class LinearMovementConstraintInternalData -{ -}; +SOFA_DEPRECATED_HEADER("v24.06", "v25.06", "sofa/component/constraint/projective/LinearMovementProjectiveConstraint.h") -/** impose a motion to given DOFs (translation and rotation) - The motion between 2 key times is linearly interpolated - Rigid version doesn't handle Topology change. -*/ -template -class LinearMovementConstraint : public core::behavior::ProjectiveConstraintSet +namespace sofa::component::constraint::projective { -public: - SOFA_CLASS(SOFA_TEMPLATE(LinearMovementConstraint,TDataTypes),SOFA_TEMPLATE(sofa::core::behavior::ProjectiveConstraintSet, TDataTypes)); - - using Index = sofa::Index; - typedef TDataTypes DataTypes; - typedef typename DataTypes::VecCoord VecCoord; - typedef typename DataTypes::VecDeriv VecDeriv; - typedef typename DataTypes::MatrixDeriv MatrixDeriv; - typedef typename DataTypes::Coord Coord; - typedef typename DataTypes::Deriv Deriv; - typedef typename DataTypes::Real Real; - typedef typename MatrixDeriv::RowIterator MatrixDerivRowIterator; - typedef typename MatrixDeriv::RowType MatrixDerivRowType; - typedef Data DataVecCoord; - typedef Data DataVecDeriv; - typedef Data DataMatrixDeriv; - typedef type::vector SetIndexArray; - typedef sofa::core::topology::TopologySubsetIndices SetIndex; - -protected: - LinearMovementConstraintInternalData *data; - friend class LinearMovementConstraintInternalData; - -public : - /// indices of the DOFs the constraint is applied to - SetIndex m_indices; - /// the key frames when the motion is defined by the user - Data > m_keyTimes; - /// the motions corresponding to the key frames - Data m_keyMovements; - - /// indicates whether movements are relative to the dof or absolute - Data< bool > d_relativeMovements; - - /// attributes to precise display - /// if showMovement is true we display the expected movement - /// otherwise we show which are the fixed dofs - Data< bool > showMovement; - - - /// the key times surrounding the current simulation time (for interpolation) - Real prevT, nextT; - ///the motions corresponding to the surrouding key times - Deriv prevM, nextM; - ///initial constrained DOFs position - VecCoord x0; - - /// Link to be set to the topology container in the component graph. - SingleLink, sofa::core::topology::BaseMeshTopology, BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_topology; - -protected: - LinearMovementConstraint(); - ~LinearMovementConstraint() override; - -public: - ///methods to add/remove some indices, keyTimes, keyMovement - void clearIndices(); - void addIndex(Index index); - void removeIndex(Index index); - void clearKeyMovements(); - - ///@brief Add a new key movement - /// @param time : the simulation time you want to set a movement (in sec) - /// @param movement : the corresponding motion - /// for instance, addKeyMovement(1.0, Deriv(5,0,0) ) will set a translation of 5 in x direction a time 1.0s - /// - void addKeyMovement(Real time, Deriv movement); - - /// -- Constraint interface - void init() override; - void reset() override; - - void projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& resData) override; - void projectVelocity(const core::MechanicalParams* mparams, DataVecDeriv& vData) override; - void projectPosition(const core::MechanicalParams* mparams, DataVecCoord& xData) override; - void projectJacobianMatrix(const core::MechanicalParams* mparams, DataMatrixDeriv& cData) override; - - void projectMatrix( sofa::linearalgebra::BaseMatrix* /*M*/, unsigned /*offset*/ ) override; - - void applyConstraint(const core::MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) override; - void applyConstraint(const core::MechanicalParams* mparams, linearalgebra::BaseVector* vector, const sofa::core::behavior::MultiMatrixAccessor* matrix) override; - void applyConstraint(sofa::core::behavior::ZeroDirichletCondition* matrix) override; - - void draw(const core::visual::VisualParams* vparams) override; - -protected: - template - void projectResponseT(DataDeriv& dx, - const std::function& clear); - - template - void interpolatePosition(Real cT, typename std::enable_if >::value, VecCoord>::type& x); - template - void interpolatePosition(Real cT, typename std::enable_if >::value, VecCoord>::type& x); - -private: - /// to keep the time corresponding to the key times - Real currentTime; - - /// to know if we found the key times - bool finished; - - /// find previous and next time keys - void findKeyTimes(); -}; - - -#if !defined(SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_LINEARMOVEMENTCONSTRAINT_CPP) -extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API LinearMovementConstraint; -extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API LinearMovementConstraint; -extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API LinearMovementConstraint; -extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API LinearMovementConstraint; -extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API LinearMovementConstraint; -#endif - -} // namespace sofa::component::constraint::projective +template +using LinearMovementConstraint SOFA_ATTRIBUTE_DEPRECATED("v24.06 ", "v25.06", "LinearMovementConstraint has been renamed to LinearMovementProjectiveConstraint") = LinearMovementProjectiveConstraint; +} \ No newline at end of file diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/LinearMovementConstraint.inl b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/LinearMovementConstraint.inl index d1700e27943..1a8538c38a0 100644 --- a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/LinearMovementConstraint.inl +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/LinearMovementConstraint.inl @@ -21,455 +21,6 @@ ******************************************************************************/ #pragma once -#include -#include -#include -#include -#include -#include -#include -#include +#include -namespace sofa::component::constraint::projective -{ - -template -LinearMovementConstraint::LinearMovementConstraint() - : core::behavior::ProjectiveConstraintSet(nullptr) - , data(new LinearMovementConstraintInternalData) - , m_indices( initData(&m_indices,"indices","Indices of the constrained points") ) - , m_keyTimes( initData(&m_keyTimes,"keyTimes","key times for the movements") ) - , m_keyMovements( initData(&m_keyMovements,"movements","movements corresponding to the key times") ) - , d_relativeMovements( initData(&d_relativeMovements, bool(true), "relativeMovements", "If true, movements are relative to first position, absolute otherwise") ) - , showMovement( initData(&showMovement, bool(false), "showMovement", "Visualization of the movement to be applied to constrained dofs.")) - , l_topology(initLink("topology", "link to the topology container")) - , finished(false) -{ - // default to indice 0 - m_indices.beginEdit()->push_back(0); - m_indices.endEdit(); - - //default valueEvent to 0 - m_keyTimes.beginEdit()->push_back( 0.0 ); - m_keyTimes.endEdit(); - m_keyMovements.beginEdit()->push_back( Deriv() ); - m_keyMovements.endEdit(); -} - - - -template -LinearMovementConstraint::~LinearMovementConstraint() -{ - -} - -template -void LinearMovementConstraint::clearIndices() -{ - m_indices.beginEdit()->clear(); - m_indices.endEdit(); -} - -template -void LinearMovementConstraint::addIndex(Index index) -{ - m_indices.beginEdit()->push_back(index); - m_indices.endEdit(); -} - -template -void LinearMovementConstraint::removeIndex(Index index) -{ - sofa::type::removeValue(*m_indices.beginEdit(),index); - m_indices.endEdit(); -} - -template -void LinearMovementConstraint::clearKeyMovements() -{ - m_keyTimes.beginEdit()->clear(); - m_keyTimes.endEdit(); - m_keyMovements.beginEdit()->clear(); - m_keyMovements.endEdit(); -} - -template -void LinearMovementConstraint::addKeyMovement(Real time, Deriv movement) -{ - m_keyTimes.beginEdit()->push_back( time ); - m_keyTimes.endEdit(); - m_keyMovements.beginEdit()->push_back( movement ); - m_keyMovements.endEdit(); -} - -// -- Constraint interface -template -void LinearMovementConstraint::init() -{ - this->core::behavior::ProjectiveConstraintSet::init(); - - if (l_topology.empty()) - { - msg_info() << "link to Topology container should be set to ensure right behavior. First Topology found in current context will be used."; - l_topology.set(this->getContext()->getMeshTopologyLink()); - } - - if (sofa::core::topology::BaseMeshTopology* _topology = l_topology.get()) - { - msg_info() << "Topology path used: '" << l_topology.getLinkedPath() << "'"; - - // Initialize topological changes support - m_indices.createTopologyHandler(_topology); - } - else - { - msg_info() << "No topology component found at path: " << l_topology.getLinkedPath() << ", nor in current context: " << this->getContext()->name; - } - - x0.resize(0); - nextM = prevM = Deriv(); - - currentTime = -1.0; - finished = false; -} - - -template -void LinearMovementConstraint::reset() -{ - nextT = prevT = 0.0; - nextM = prevM = Deriv(); - - currentTime = -1.0; - finished = false; -} - - -template -template -void LinearMovementConstraint::projectResponseT(DataDeriv& dx, - const std::function& clear) -{ - Real cT = static_cast(this->getContext()->getTime()); - if ((cT != currentTime) || !finished) - { - findKeyTimes(); - } - - if (finished && nextT != prevT) - { - const SetIndexArray & indices = m_indices.getValue(); - - //set the motion to the Dofs - for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) - { - clear(dx, *it); - } - } -} - -template -void LinearMovementConstraint::projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& resData) -{ - SOFA_UNUSED(mparams); - helper::WriteAccessor res = resData; - projectResponseT(res.wref(), [](auto& dx, const unsigned int index) {dx[index].clear(); }); -} - -template -void LinearMovementConstraint::projectVelocity(const core::MechanicalParams* /*mparams*/, DataVecDeriv& vData) -{ - helper::WriteAccessor dx = vData; - Real cT = static_cast(this->getContext()->getTime()); - if ((cT != currentTime) || !finished) - { - findKeyTimes(); - } - - if (finished && nextT != prevT) - { - const SetIndexArray & indices = m_indices.getValue(); - - //set the motion to the Dofs - for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) - { - dx[*it] = (nextM - prevM)*(1.0 / (nextT - prevT)); - } - } -} - - -template -void LinearMovementConstraint::projectPosition(const core::MechanicalParams* /*mparams*/, DataVecCoord& xData) -{ - helper::WriteAccessor x = xData; - Real cT = static_cast(this->getContext()->getTime()); - - //initialize initial Dofs positions, if it's not done - if (x0.size() == 0) - { - const SetIndexArray & indices = m_indices.getValue(); - x0.resize(x.size()); - for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) - { - x0[*it] = x[*it]; - } - } - - if ((cT != currentTime) || !finished) - { - findKeyTimes(); - } - - //if we found 2 keyTimes, we have to interpolate a velocity (linear interpolation) - if(finished && nextT != prevT) - { - interpolatePosition(cT, x.wref()); - } -} - -template -template -void LinearMovementConstraint::interpolatePosition(Real cT, typename std::enable_if >::value, VecCoord>::type& x) -{ - const SetIndexArray & indices = m_indices.getValue(); - - Real dt = (cT - prevT) / (nextT - prevT); - Deriv m = prevM + (nextM-prevM)*dt; - - //set the motion to the Dofs - if (d_relativeMovements.getValue()) - { - for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) - { - x[*it] = x0[*it] + m ; - } - } - else - { - for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) - { - x[*it] = m ; - } - } -} - -template -template -void LinearMovementConstraint::interpolatePosition(Real cT, typename std::enable_if >::value, VecCoord>::type& x) -{ - const SetIndexArray & indices = m_indices.getValue(); - - Real dt = (cT - prevT) / (nextT - prevT); - Deriv m = prevM + (nextM-prevM)*dt; - type::Quat prevOrientation = type::Quat::createQuaterFromEuler(getVOrientation(prevM)); - type::Quat nextOrientation = type::Quat::createQuaterFromEuler(getVOrientation(nextM)); - - //set the motion to the Dofs - if (d_relativeMovements.getValue()) - { - for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) - { - x[*it].getCenter() = x0[*it].getCenter() + getVCenter(m) ; - x[*it].getOrientation() = x0[*it].getOrientation() * prevOrientation.slerp2(nextOrientation, dt); - } - } - else - { - for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) - { - x[*it].getCenter() = getVCenter(m) ; - x[*it].getOrientation() = prevOrientation.slerp2(nextOrientation, dt); - } - } -} - -template -void LinearMovementConstraint::projectJacobianMatrix(const core::MechanicalParams* mparams, DataMatrixDeriv& cData) -{ - SOFA_UNUSED(mparams); - helper::WriteAccessor c = cData; - - projectResponseT(c.wref(), [](MatrixDeriv& res, const unsigned int index) { res.clearColBlock(index); }); -} - -template -void LinearMovementConstraint::findKeyTimes() -{ - Real cT = static_cast(this->getContext()->getTime()); - finished = false; - - if(m_keyTimes.getValue().size() != 0 && cT >= *m_keyTimes.getValue().begin() && cT <= *m_keyTimes.getValue().rbegin()) - { - nextT = *m_keyTimes.getValue().begin(); - prevT = nextT; - - typename type::vector::const_iterator it_t = m_keyTimes.getValue().begin(); - typename VecDeriv::const_iterator it_m = m_keyMovements.getValue().begin(); - - //WARNING : we consider that the key-events are in chronological order - //here we search between which keyTimes we are, to know which are the motion to interpolate - while( it_t != m_keyTimes.getValue().end() && !finished) - { - if( *it_t <= cT) - { - prevT = *it_t; - prevM = *it_m; - } - else - { - nextT = *it_t; - nextM = *it_m; - finished = true; - } - ++it_t; - ++it_m; - } - } -} - - -template -void LinearMovementConstraint::projectMatrix( sofa::linearalgebra::BaseMatrix* M, unsigned offset ) -{ - static const unsigned blockSize = DataTypes::deriv_total_size; - - // clears the rows and columns associated with fixed particles - for (const auto id : m_indices.getValue()) - { - M->clearRowsCols( offset + id * blockSize, offset + (id+1) * blockSize ); - } - -} - - -// Matrix Integration interface -template -void LinearMovementConstraint::applyConstraint(const core::MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) -{ - SOFA_UNUSED(mparams); - if(const core::behavior::MultiMatrixAccessor::MatrixRef r = matrix->getMatrix(this->mstate.get())) - { - const unsigned int N = Deriv::size(); - const SetIndexArray & indices = m_indices.getValue(); - - for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) - { - // Reset Fixed Row and Col - for (unsigned int c=0; cclearRowCol(r.offset + N * (*it) + c); - // Set Fixed Vertex - for (unsigned int c=0; cset(r.offset + N * (*it) + c, r.offset + N * (*it) + c, 1.0); - } - } -} - -template -void LinearMovementConstraint::applyConstraint(const core::MechanicalParams* mparams, linearalgebra::BaseVector* vector, const sofa::core::behavior::MultiMatrixAccessor* matrix) -{ - SOFA_UNUSED(mparams); - const int o = matrix->getGlobalOffset(this->mstate.get()); - if (o >= 0) - { - const unsigned int offset = (unsigned int)o; - const unsigned int N = Deriv::size(); - - const SetIndexArray & indices = m_indices.getValue(); - for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) - { - for (unsigned int c=0; cclear(offset + N * (*it) + c); - } - } -} - -template -void LinearMovementConstraint::applyConstraint( - sofa::core::behavior::ZeroDirichletCondition* matrix) -{ - constexpr unsigned int N = Deriv::size(); - - for (const auto index : m_indices.getValue()) - { - for (unsigned int c = 0; c < N; ++c) - { - matrix->discardRowCol(N * index + c, N * index + c); - } - } -} - -//display the path the constrained dofs will go through -template -void LinearMovementConstraint::draw(const core::visual::VisualParams* vparams) -{ - if (!vparams->displayFlags().getShowBehaviorModels() || m_keyTimes.getValue().size() == 0) - return; - - const auto stateLifeCycle = vparams->drawTool()->makeStateLifeCycle(); - constexpr sofa::type::RGBAColor color(1, 0.5, 0.5, 1); - - if (showMovement.getValue()) - { - vparams->drawTool()->disableLighting(); - - std::vector vertices; - - constexpr auto minDimensions = std::min(DataTypes::spatial_dimensions, 3u); - - const SetIndexArray & indices = m_indices.getValue(); - const VecDeriv& keyMovements = m_keyMovements.getValue(); - if (d_relativeMovements.getValue()) - { - for (unsigned int i = 0; i < m_keyMovements.getValue().size() - 1; i++) - { - for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) - { - const auto& tmp0 = DataTypes::getCPos(x0[*it]) + DataTypes::getDPos(keyMovements[i]); - const auto& tmp1 = DataTypes::getCPos(x0[*it]) + DataTypes::getDPos(keyMovements[i + 1]); - sofa::type::Vec3 v0, v1; - std::copy_n(tmp0.begin(), minDimensions, v0.begin()); - std::copy_n(tmp1.begin(), minDimensions, v1.begin()); - vertices.push_back(v0); - vertices.push_back(v1); - } - } - } - else - { - for (unsigned int i = 0; i < keyMovements.size() - 1; i++) - { - for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) - { - const auto& tmp0 = DataTypes::getDPos(keyMovements[i]); - const auto& tmp1 = DataTypes::getDPos(keyMovements[i + 1]); - sofa::type::Vec3 v0, v1; - std::copy_n(tmp0.begin(), minDimensions, v0.begin()); - std::copy_n(tmp1.begin(), minDimensions, v1.begin()); - vertices.push_back(v0); - vertices.push_back(v1); - } - } - } - vparams->drawTool()->drawLines(vertices, 10, color); - } - else - { - const VecCoord& x = this->mstate->read(core::ConstVecCoordId::position())->getValue(); - - sofa::type::vector points; - type::Vec3 point; - const SetIndexArray & indices = m_indices.getValue(); - for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) - { - point = DataTypes::getCPos(x[*it]); - points.push_back(point); - } - vparams->drawTool()->drawPoints(points, 10, color); - } - - -} - -} // namespace sofa::component::constraint::projective +SOFA_DEPRECATED_HEADER("v24.06", "v25.06", "sofa/component/constraint/projective/LinearMovementProjectiveConstraint.inl") diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/LinearMovementConstraint.cpp b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/LinearMovementProjectiveConstraint.cpp similarity index 70% rename from Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/LinearMovementConstraint.cpp rename to Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/LinearMovementProjectiveConstraint.cpp index 3c47dc2dbce..7feda4bdfb7 100644 --- a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/LinearMovementConstraint.cpp +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/LinearMovementProjectiveConstraint.cpp @@ -19,8 +19,8 @@ * * * Contact information: contact@sofa-framework.org * ******************************************************************************/ -#define SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_LINEARMOVEMENTCONSTRAINT_CPP -#include +#define SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_LINEARMOVEMENTPROJECTIVECONSTRAINT_CPP +#include #include #include #include @@ -29,7 +29,7 @@ namespace sofa::component::constraint::projective { template <> SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API -void LinearMovementConstraint::init() +void LinearMovementProjectiveConstraint::init() { this->core::behavior::ProjectiveConstraintSet::init(); @@ -42,18 +42,20 @@ void LinearMovementConstraint::init() //declaration of the class, for the factory -int LinearMovementConstraintClass = core::RegisterObject("translate given particles") - .add< LinearMovementConstraint >() - .add< LinearMovementConstraint >() - .add< LinearMovementConstraint >() - .add< LinearMovementConstraint >() - .add< LinearMovementConstraint >(); - - -template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API LinearMovementConstraint; -template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API LinearMovementConstraint; -template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API LinearMovementConstraint; -template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API LinearMovementConstraint; -template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API LinearMovementConstraint; +int LinearMovementProjectiveConstraintClass = core::RegisterObject("translate given particles") + .add< LinearMovementProjectiveConstraint >() + .add< LinearMovementProjectiveConstraint >() + .add< LinearMovementProjectiveConstraint >() + .add< LinearMovementProjectiveConstraint >() + .add< LinearMovementProjectiveConstraint >() + .addAlias("LinearMovementConstraint") + ; + + +template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API LinearMovementProjectiveConstraint; +template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API LinearMovementProjectiveConstraint; +template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API LinearMovementProjectiveConstraint; +template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API LinearMovementProjectiveConstraint; +template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API LinearMovementProjectiveConstraint; } // namespace sofa::component::constraint::projective diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/LinearMovementProjectiveConstraint.h b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/LinearMovementProjectiveConstraint.h new file mode 100644 index 00000000000..b5013c890a0 --- /dev/null +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/LinearMovementProjectiveConstraint.h @@ -0,0 +1,168 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace sofa::component::constraint::projective +{ + +template +class LinearMovementProjectiveConstraintInternalData +{ +}; + +/** impose a motion to given DOFs (translation and rotation) + The motion between 2 key times is linearly interpolated + Rigid version doesn't handle Topology change. +*/ +template +class LinearMovementProjectiveConstraint : public core::behavior::ProjectiveConstraintSet +{ +public: + SOFA_CLASS(SOFA_TEMPLATE(LinearMovementProjectiveConstraint,TDataTypes),SOFA_TEMPLATE(sofa::core::behavior::ProjectiveConstraintSet, TDataTypes)); + + using Index = sofa::Index; + typedef TDataTypes DataTypes; + typedef typename DataTypes::VecCoord VecCoord; + typedef typename DataTypes::VecDeriv VecDeriv; + typedef typename DataTypes::MatrixDeriv MatrixDeriv; + typedef typename DataTypes::Coord Coord; + typedef typename DataTypes::Deriv Deriv; + typedef typename DataTypes::Real Real; + typedef typename MatrixDeriv::RowIterator MatrixDerivRowIterator; + typedef typename MatrixDeriv::RowType MatrixDerivRowType; + typedef Data DataVecCoord; + typedef Data DataVecDeriv; + typedef Data DataMatrixDeriv; + typedef type::vector SetIndexArray; + typedef sofa::core::topology::TopologySubsetIndices SetIndex; + +protected: + LinearMovementProjectiveConstraintInternalData *data; + friend class LinearMovementProjectiveConstraintInternalData; + +public : + /// indices of the DOFs the constraint is applied to + SetIndex m_indices; + /// the key frames when the motion is defined by the user + Data > m_keyTimes; + /// the motions corresponding to the key frames + Data m_keyMovements; + + /// indicates whether movements are relative to the dof or absolute + Data< bool > d_relativeMovements; + + /// attributes to precise display + /// if showMovement is true we display the expected movement + /// otherwise we show which are the fixed dofs + Data< bool > showMovement; + + + /// the key times surrounding the current simulation time (for interpolation) + Real prevT, nextT; + ///the motions corresponding to the surrouding key times + Deriv prevM, nextM; + ///initial constrained DOFs position + VecCoord x0; + + /// Link to be set to the topology container in the component graph. + SingleLink, sofa::core::topology::BaseMeshTopology, BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_topology; + +protected: + LinearMovementProjectiveConstraint(); + ~LinearMovementProjectiveConstraint() override; + +public: + ///methods to add/remove some indices, keyTimes, keyMovement + void clearIndices(); + void addIndex(Index index); + void removeIndex(Index index); + void clearKeyMovements(); + + ///@brief Add a new key movement + /// @param time : the simulation time you want to set a movement (in sec) + /// @param movement : the corresponding motion + /// for instance, addKeyMovement(1.0, Deriv(5,0,0) ) will set a translation of 5 in x direction a time 1.0s + /// + void addKeyMovement(Real time, Deriv movement); + + /// -- Constraint interface + void init() override; + void reset() override; + + void projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& resData) override; + void projectVelocity(const core::MechanicalParams* mparams, DataVecDeriv& vData) override; + void projectPosition(const core::MechanicalParams* mparams, DataVecCoord& xData) override; + void projectJacobianMatrix(const core::MechanicalParams* mparams, DataMatrixDeriv& cData) override; + + void projectMatrix( sofa::linearalgebra::BaseMatrix* /*M*/, unsigned /*offset*/ ) override; + + void applyConstraint(const core::MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) override; + void applyConstraint(const core::MechanicalParams* mparams, linearalgebra::BaseVector* vector, const sofa::core::behavior::MultiMatrixAccessor* matrix) override; + void applyConstraint(sofa::core::behavior::ZeroDirichletCondition* matrix) override; + + void draw(const core::visual::VisualParams* vparams) override; + +protected: + template + void projectResponseT(DataDeriv& dx, + const std::function& clear); + + template + void interpolatePosition(Real cT, typename std::enable_if >::value, VecCoord>::type& x); + template + void interpolatePosition(Real cT, typename std::enable_if >::value, VecCoord>::type& x); + +private: + /// to keep the time corresponding to the key times + Real currentTime; + + /// to know if we found the key times + bool finished; + + /// find previous and next time keys + void findKeyTimes(); +}; + + +#if !defined(SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_LINEARMOVEMENTPROJECTIVECONSTRAINT_CPP) +extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API LinearMovementProjectiveConstraint; +extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API LinearMovementProjectiveConstraint; +extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API LinearMovementProjectiveConstraint; +extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API LinearMovementProjectiveConstraint; +extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API LinearMovementProjectiveConstraint; +#endif + +} // namespace sofa::component::constraint::projective diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/LinearMovementProjectiveConstraint.inl b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/LinearMovementProjectiveConstraint.inl new file mode 100644 index 00000000000..a5b6f81616b --- /dev/null +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/LinearMovementProjectiveConstraint.inl @@ -0,0 +1,475 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace sofa::component::constraint::projective +{ + +template +LinearMovementProjectiveConstraint::LinearMovementProjectiveConstraint() + : core::behavior::ProjectiveConstraintSet(nullptr) + , data(new LinearMovementProjectiveConstraintInternalData) + , m_indices( initData(&m_indices,"indices","Indices of the constrained points") ) + , m_keyTimes( initData(&m_keyTimes,"keyTimes","key times for the movements") ) + , m_keyMovements( initData(&m_keyMovements,"movements","movements corresponding to the key times") ) + , d_relativeMovements( initData(&d_relativeMovements, bool(true), "relativeMovements", "If true, movements are relative to first position, absolute otherwise") ) + , showMovement( initData(&showMovement, bool(false), "showMovement", "Visualization of the movement to be applied to constrained dofs.")) + , l_topology(initLink("topology", "link to the topology container")) + , finished(false) +{ + // default to indice 0 + m_indices.beginEdit()->push_back(0); + m_indices.endEdit(); + + //default valueEvent to 0 + m_keyTimes.beginEdit()->push_back( 0.0 ); + m_keyTimes.endEdit(); + m_keyMovements.beginEdit()->push_back( Deriv() ); + m_keyMovements.endEdit(); +} + + + +template +LinearMovementProjectiveConstraint::~LinearMovementProjectiveConstraint() +{ + +} + +template +void LinearMovementProjectiveConstraint::clearIndices() +{ + m_indices.beginEdit()->clear(); + m_indices.endEdit(); +} + +template +void LinearMovementProjectiveConstraint::addIndex(Index index) +{ + m_indices.beginEdit()->push_back(index); + m_indices.endEdit(); +} + +template +void LinearMovementProjectiveConstraint::removeIndex(Index index) +{ + sofa::type::removeValue(*m_indices.beginEdit(),index); + m_indices.endEdit(); +} + +template +void LinearMovementProjectiveConstraint::clearKeyMovements() +{ + m_keyTimes.beginEdit()->clear(); + m_keyTimes.endEdit(); + m_keyMovements.beginEdit()->clear(); + m_keyMovements.endEdit(); +} + +template +void LinearMovementProjectiveConstraint::addKeyMovement(Real time, Deriv movement) +{ + m_keyTimes.beginEdit()->push_back( time ); + m_keyTimes.endEdit(); + m_keyMovements.beginEdit()->push_back( movement ); + m_keyMovements.endEdit(); +} + +// -- Constraint interface +template +void LinearMovementProjectiveConstraint::init() +{ + this->core::behavior::ProjectiveConstraintSet::init(); + + if (l_topology.empty()) + { + msg_info() << "link to Topology container should be set to ensure right behavior. First Topology found in current context will be used."; + l_topology.set(this->getContext()->getMeshTopologyLink()); + } + + if (sofa::core::topology::BaseMeshTopology* _topology = l_topology.get()) + { + msg_info() << "Topology path used: '" << l_topology.getLinkedPath() << "'"; + + // Initialize topological changes support + m_indices.createTopologyHandler(_topology); + } + else + { + msg_info() << "No topology component found at path: " << l_topology.getLinkedPath() << ", nor in current context: " << this->getContext()->name; + } + + x0.resize(0); + nextM = prevM = Deriv(); + + currentTime = -1.0; + finished = false; +} + + +template +void LinearMovementProjectiveConstraint::reset() +{ + nextT = prevT = 0.0; + nextM = prevM = Deriv(); + + currentTime = -1.0; + finished = false; +} + + +template +template +void LinearMovementProjectiveConstraint::projectResponseT(DataDeriv& dx, + const std::function& clear) +{ + Real cT = static_cast(this->getContext()->getTime()); + if ((cT != currentTime) || !finished) + { + findKeyTimes(); + } + + if (finished && nextT != prevT) + { + const SetIndexArray & indices = m_indices.getValue(); + + //set the motion to the Dofs + for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) + { + clear(dx, *it); + } + } +} + +template +void LinearMovementProjectiveConstraint::projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& resData) +{ + SOFA_UNUSED(mparams); + helper::WriteAccessor res = resData; + projectResponseT(res.wref(), [](auto& dx, const unsigned int index) {dx[index].clear(); }); +} + +template +void LinearMovementProjectiveConstraint::projectVelocity(const core::MechanicalParams* /*mparams*/, DataVecDeriv& vData) +{ + helper::WriteAccessor dx = vData; + Real cT = static_cast(this->getContext()->getTime()); + if ((cT != currentTime) || !finished) + { + findKeyTimes(); + } + + if (finished && nextT != prevT) + { + const SetIndexArray & indices = m_indices.getValue(); + + //set the motion to the Dofs + for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) + { + dx[*it] = (nextM - prevM)*(1.0 / (nextT - prevT)); + } + } +} + + +template +void LinearMovementProjectiveConstraint::projectPosition(const core::MechanicalParams* /*mparams*/, DataVecCoord& xData) +{ + helper::WriteAccessor x = xData; + Real cT = static_cast(this->getContext()->getTime()); + + //initialize initial Dofs positions, if it's not done + if (x0.size() == 0) + { + const SetIndexArray & indices = m_indices.getValue(); + x0.resize(x.size()); + for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) + { + x0[*it] = x[*it]; + } + } + + if ((cT != currentTime) || !finished) + { + findKeyTimes(); + } + + //if we found 2 keyTimes, we have to interpolate a velocity (linear interpolation) + if(finished && nextT != prevT) + { + interpolatePosition(cT, x.wref()); + } +} + +template +template +void LinearMovementProjectiveConstraint::interpolatePosition(Real cT, typename std::enable_if >::value, VecCoord>::type& x) +{ + const SetIndexArray & indices = m_indices.getValue(); + + Real dt = (cT - prevT) / (nextT - prevT); + Deriv m = prevM + (nextM-prevM)*dt; + + //set the motion to the Dofs + if (d_relativeMovements.getValue()) + { + for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) + { + x[*it] = x0[*it] + m ; + } + } + else + { + for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) + { + x[*it] = m ; + } + } +} + +template +template +void LinearMovementProjectiveConstraint::interpolatePosition(Real cT, typename std::enable_if >::value, VecCoord>::type& x) +{ + const SetIndexArray & indices = m_indices.getValue(); + + Real dt = (cT - prevT) / (nextT - prevT); + Deriv m = prevM + (nextM-prevM)*dt; + type::Quat prevOrientation = type::Quat::createQuaterFromEuler(getVOrientation(prevM)); + type::Quat nextOrientation = type::Quat::createQuaterFromEuler(getVOrientation(nextM)); + + //set the motion to the Dofs + if (d_relativeMovements.getValue()) + { + for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) + { + x[*it].getCenter() = x0[*it].getCenter() + getVCenter(m) ; + x[*it].getOrientation() = x0[*it].getOrientation() * prevOrientation.slerp2(nextOrientation, dt); + } + } + else + { + for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) + { + x[*it].getCenter() = getVCenter(m) ; + x[*it].getOrientation() = prevOrientation.slerp2(nextOrientation, dt); + } + } +} + +template +void LinearMovementProjectiveConstraint::projectJacobianMatrix(const core::MechanicalParams* mparams, DataMatrixDeriv& cData) +{ + SOFA_UNUSED(mparams); + helper::WriteAccessor c = cData; + + projectResponseT(c.wref(), [](MatrixDeriv& res, const unsigned int index) { res.clearColBlock(index); }); +} + +template +void LinearMovementProjectiveConstraint::findKeyTimes() +{ + Real cT = static_cast(this->getContext()->getTime()); + finished = false; + + if(m_keyTimes.getValue().size() != 0 && cT >= *m_keyTimes.getValue().begin() && cT <= *m_keyTimes.getValue().rbegin()) + { + nextT = *m_keyTimes.getValue().begin(); + prevT = nextT; + + typename type::vector::const_iterator it_t = m_keyTimes.getValue().begin(); + typename VecDeriv::const_iterator it_m = m_keyMovements.getValue().begin(); + + //WARNING : we consider that the key-events are in chronological order + //here we search between which keyTimes we are, to know which are the motion to interpolate + while( it_t != m_keyTimes.getValue().end() && !finished) + { + if( *it_t <= cT) + { + prevT = *it_t; + prevM = *it_m; + } + else + { + nextT = *it_t; + nextM = *it_m; + finished = true; + } + ++it_t; + ++it_m; + } + } +} + + +template +void LinearMovementProjectiveConstraint::projectMatrix( sofa::linearalgebra::BaseMatrix* M, unsigned offset ) +{ + static const unsigned blockSize = DataTypes::deriv_total_size; + + // clears the rows and columns associated with fixed particles + for (const auto id : m_indices.getValue()) + { + M->clearRowsCols( offset + id * blockSize, offset + (id+1) * blockSize ); + } + +} + + +// Matrix Integration interface +template +void LinearMovementProjectiveConstraint::applyConstraint(const core::MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) +{ + SOFA_UNUSED(mparams); + if(const core::behavior::MultiMatrixAccessor::MatrixRef r = matrix->getMatrix(this->mstate.get())) + { + const unsigned int N = Deriv::size(); + const SetIndexArray & indices = m_indices.getValue(); + + for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) + { + // Reset Fixed Row and Col + for (unsigned int c=0; cclearRowCol(r.offset + N * (*it) + c); + // Set Fixed Vertex + for (unsigned int c=0; cset(r.offset + N * (*it) + c, r.offset + N * (*it) + c, 1.0); + } + } +} + +template +void LinearMovementProjectiveConstraint::applyConstraint(const core::MechanicalParams* mparams, linearalgebra::BaseVector* vector, const sofa::core::behavior::MultiMatrixAccessor* matrix) +{ + SOFA_UNUSED(mparams); + const int o = matrix->getGlobalOffset(this->mstate.get()); + if (o >= 0) + { + const unsigned int offset = (unsigned int)o; + const unsigned int N = Deriv::size(); + + const SetIndexArray & indices = m_indices.getValue(); + for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) + { + for (unsigned int c=0; cclear(offset + N * (*it) + c); + } + } +} + +template +void LinearMovementProjectiveConstraint::applyConstraint( + sofa::core::behavior::ZeroDirichletCondition* matrix) +{ + constexpr unsigned int N = Deriv::size(); + + for (const auto index : m_indices.getValue()) + { + for (unsigned int c = 0; c < N; ++c) + { + matrix->discardRowCol(N * index + c, N * index + c); + } + } +} + +//display the path the constrained dofs will go through +template +void LinearMovementProjectiveConstraint::draw(const core::visual::VisualParams* vparams) +{ + if (!vparams->displayFlags().getShowBehaviorModels() || m_keyTimes.getValue().size() == 0) + return; + + const auto stateLifeCycle = vparams->drawTool()->makeStateLifeCycle(); + constexpr sofa::type::RGBAColor color(1, 0.5, 0.5, 1); + + if (showMovement.getValue()) + { + vparams->drawTool()->disableLighting(); + + std::vector vertices; + + constexpr auto minDimensions = std::min(DataTypes::spatial_dimensions, 3u); + + const SetIndexArray & indices = m_indices.getValue(); + const VecDeriv& keyMovements = m_keyMovements.getValue(); + if (d_relativeMovements.getValue()) + { + for (unsigned int i = 0; i < m_keyMovements.getValue().size() - 1; i++) + { + for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) + { + const auto& tmp0 = DataTypes::getCPos(x0[*it]) + DataTypes::getDPos(keyMovements[i]); + const auto& tmp1 = DataTypes::getCPos(x0[*it]) + DataTypes::getDPos(keyMovements[i + 1]); + sofa::type::Vec3 v0, v1; + std::copy_n(tmp0.begin(), minDimensions, v0.begin()); + std::copy_n(tmp1.begin(), minDimensions, v1.begin()); + vertices.push_back(v0); + vertices.push_back(v1); + } + } + } + else + { + for (unsigned int i = 0; i < keyMovements.size() - 1; i++) + { + for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) + { + const auto& tmp0 = DataTypes::getDPos(keyMovements[i]); + const auto& tmp1 = DataTypes::getDPos(keyMovements[i + 1]); + sofa::type::Vec3 v0, v1; + std::copy_n(tmp0.begin(), minDimensions, v0.begin()); + std::copy_n(tmp1.begin(), minDimensions, v1.begin()); + vertices.push_back(v0); + vertices.push_back(v1); + } + } + } + vparams->drawTool()->drawLines(vertices, 10, color); + } + else + { + const VecCoord& x = this->mstate->read(core::ConstVecCoordId::position())->getValue(); + + sofa::type::vector points; + type::Vec3 point; + const SetIndexArray & indices = m_indices.getValue(); + for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) + { + point = DataTypes::getCPos(x[*it]); + points.push_back(point); + } + vparams->drawTool()->drawPoints(points, 10, color); + } + + +} + +} // namespace sofa::component::constraint::projective diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/LinearVelocityConstraint.h b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/LinearVelocityConstraint.h index e347423b33b..297a56d6f2c 100644 --- a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/LinearVelocityConstraint.h +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/LinearVelocityConstraint.h @@ -20,116 +20,13 @@ * Contact information: contact@sofa-framework.org * ******************************************************************************/ #pragma once -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace sofa::component::constraint::projective -{ +#include +SOFA_DEPRECATED_HEADER("v24.06", "v25.06", "sofa/component/constraint/projective/LinearVelocityProjectiveConstraint.h") -/** impose a motion to given DOFs (translation and rotation) - The motion between 2 key times is linearly interpolated -*/ -template -class LinearVelocityConstraint : public core::behavior::ProjectiveConstraintSet +namespace sofa::component::constraint::projective { -public: - SOFA_CLASS(SOFA_TEMPLATE(LinearVelocityConstraint,TDataTypes),SOFA_TEMPLATE(core::behavior::ProjectiveConstraintSet,TDataTypes)); - - using Index = sofa::Index; - typedef TDataTypes DataTypes; - typedef typename DataTypes::VecCoord VecCoord; - typedef typename DataTypes::VecDeriv VecDeriv; - typedef typename DataTypes::MatrixDeriv MatrixDeriv; - typedef typename DataTypes::Coord Coord; - typedef typename DataTypes::Deriv Deriv; - typedef typename DataTypes::Real Real; - typedef typename MatrixDeriv::RowType MatrixDerivRowType; - typedef Data DataVecCoord; - typedef Data DataVecDeriv; - typedef Data DataMatrixDeriv; - typedef type::vector SetIndexArray; - typedef sofa::core::topology::TopologySubsetIndices SetIndex; - -public : - /// indices of the DOFs the constraint is applied to - SetIndex d_indices; - /// the key frames when the motion is defined by the user - Data > d_keyTimes; - /// the motions corresponding to the key frames - Data d_keyVelocities; - /// the coordinates on which to applay velocities - SetIndex d_coordinates; - - /// the key times surrounding the current simulation time (for interpolation) - Real prevT, nextT; - ///the velocities corresponding to the surrouding key times - Deriv prevV, nextV; - ///position at the initial step for constrained DOFs position - VecCoord x0; - ///position at the previous step for constrained DOFs position - VecCoord xP; - - /// Link to be set to the topology container in the component graph. - SingleLink, sofa::core::topology::BaseMeshTopology, BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_topology; - -protected: - LinearVelocityConstraint(); - - virtual ~LinearVelocityConstraint(); -public: - ///methods to add/remove some indices, keyTimes, keyVelocity - void clearIndices(); - void addIndex(Index index); - void removeIndex(Index index); - void clearKeyVelocities(); - /**add a new key movement - @param time : the simulation time you want to set a movement (in sec) - @param movement : the corresponding motion - for instance, addKeyMovement(1.0, Deriv(5,0,0) ) will set a translation of 5 in x direction a time 1.0s - **/ - void addKeyVelocity(Real time, Deriv movement); - - - /// -- Constraint interface - void init() override; - void reset() override; - void projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& resData) override; - void projectVelocity(const core::MechanicalParams* mparams, DataVecDeriv& vData) override; - void projectPosition(const core::MechanicalParams* mparams, DataVecCoord& xData) override; - void projectJacobianMatrix(const core::MechanicalParams* mparams, DataMatrixDeriv& cData) override; - - void draw(const core::visual::VisualParams* vparams) override; - -private: - - /// to keep the time corresponding to the key times - Real currentTime; - - /// to know if we found the key times - bool finished; - - /// find previous and next time keys - void findKeyTimes(); -}; - -#if !defined(SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_LINEARVELOCITYCONSTRAINT_CPP) -extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API LinearVelocityConstraint; -extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API LinearVelocityConstraint; -extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API LinearVelocityConstraint; -extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API LinearVelocityConstraint; -extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API LinearVelocityConstraint; - -#endif - -} // namespace sofa::component::constraint::projective +template +using LinearVelocityConstraint SOFA_ATTRIBUTE_DEPRECATED("v24.06 ", "v25.06", "LinearVelocityConstraint has been renamed to LinearVelocityProjectiveConstraint") = LinearVelocityProjectiveConstraint; +} \ No newline at end of file diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/LinearVelocityConstraint.inl b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/LinearVelocityConstraint.inl index 9457ccfcb94..4e9baec9e9f 100644 --- a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/LinearVelocityConstraint.inl +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/LinearVelocityConstraint.inl @@ -21,328 +21,6 @@ ******************************************************************************/ #pragma once -#include -#include -#include -#include -#include -#include -#include +#include - -namespace sofa::component::constraint::projective -{ - -template -LinearVelocityConstraint::LinearVelocityConstraint() - : core::behavior::ProjectiveConstraintSet(nullptr) - , d_indices( initData(&d_indices,"indices","Indices of the constrained points") ) - , d_keyTimes( initData(&d_keyTimes,"keyTimes","key times for the movements") ) - , d_keyVelocities( initData(&d_keyVelocities,"velocities","velocities corresponding to the key times") ) - , d_coordinates( initData(&d_coordinates, "coordinates", "coordinates on which to apply velocities") ) - , l_topology(initLink("topology", "link to the topology container")) - , finished(false) -{ - d_indices.beginEdit()->push_back(0); - d_indices.endEdit(); - - d_keyTimes.beginEdit()->push_back( 0.0 ); - d_keyTimes.endEdit(); - d_keyVelocities.beginEdit()->push_back( Deriv() ); - d_keyVelocities.endEdit(); -} - - -template -LinearVelocityConstraint::~LinearVelocityConstraint() -{ - -} - -template -void LinearVelocityConstraint::clearIndices() -{ - d_indices.beginEdit()->clear(); - d_indices.endEdit(); -} - -template -void LinearVelocityConstraint::addIndex(Index index) -{ - d_indices.beginEdit()->push_back(index); - d_indices.endEdit(); -} - -template -void LinearVelocityConstraint::removeIndex(Index index) -{ - sofa::type::removeValue(*d_indices.beginEdit(),index); - d_indices.endEdit(); -} - -template -void LinearVelocityConstraint::clearKeyVelocities() -{ - d_keyTimes.beginEdit()->clear(); - d_keyTimes.endEdit(); - d_keyVelocities.beginEdit()->clear(); - d_keyVelocities.endEdit(); -} - -template -void LinearVelocityConstraint::addKeyVelocity(Real time, Deriv movement) -{ - d_keyTimes.beginEdit()->push_back( time ); - d_keyTimes.endEdit(); - d_keyVelocities.beginEdit()->push_back( movement ); - d_keyVelocities.endEdit(); -} - -// -- Constraint interface - - -template -void LinearVelocityConstraint::init() -{ - this->core::behavior::ProjectiveConstraintSet::init(); - - if (l_topology.empty()) - { - msg_info() << "link to Topology container should be set to ensure right behavior. First Topology found in current context will be used."; - l_topology.set(this->getContext()->getMeshTopologyLink()); - } - - if (sofa::core::topology::BaseMeshTopology* _topology = l_topology.get()) - { - msg_info() << "Topology path used: '" << l_topology.getLinkedPath() << "'"; - - // Initialize topological changes support - d_indices.createTopologyHandler(_topology); - d_coordinates.createTopologyHandler(_topology); - } - else - { - msg_info() << "No topology component found at path: " << l_topology.getLinkedPath() << ", nor in current context: " << this->getContext()->name; - } - - x0.resize(0); - xP.resize(0); - nextV = prevV = Deriv(); - - currentTime = -1.0; - finished = false; -} - -template -void LinearVelocityConstraint::reset() -{ - nextT = prevT = 0.0; - nextV = prevV = Deriv(); - - currentTime = -1.0; - finished = false; -} - - -template -void LinearVelocityConstraint::projectResponse(const core::MechanicalParams* /*mparams*/, DataVecDeriv& resData) -{ - helper::WriteAccessor res = resData; - VecDeriv& dx = res.wref(); - - Real cT = (Real) this->getContext()->getTime(); - if ((cT != currentTime) || !finished) - { - findKeyTimes(); - } - - if (finished && nextT != prevT) - { - const SetIndexArray & indices = d_indices.getValue(); - - //set the motion to the Dofs - for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) - { - dx[*it] = Deriv(); - } - } - -} - -template -void LinearVelocityConstraint::projectVelocity(const core::MechanicalParams* /*mparams*/, DataVecDeriv& vData) -{ - helper::WriteAccessor dx = vData; - Real cT = (Real) this->getContext()->getTime(); - - if ((cT != currentTime) || !finished) - { - findKeyTimes(); - } - - if (finished && nextT != prevT) - { - //if we found 2 keyTimes, we have to interpolate a velocity (linear interpolation) - Deriv v = ((nextV - prevV)*((cT - prevT)/(nextT - prevT))) + prevV; - - const SetIndexArray & indices = d_indices.getValue(); - const SetIndexArray & coordinates = d_coordinates.getValue(); - - if (coordinates.size() == 0) - { - //set the motion to the Dofs - for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) - { - dx[*it] = v; - } - } - else - { - for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) - { - for(SetIndexArray::const_iterator itInd = coordinates.begin(); itInd != coordinates.end(); ++itInd) - { - dx[*it][*itInd] = v[*itInd]; - } - } - } - } -} - - -template -void LinearVelocityConstraint::projectPosition(const core::MechanicalParams* /*mparams*/, DataVecCoord& xData) -{ - helper::WriteAccessor x = xData; - - //initialize initial Dofs positions, if it's not done - if (x0.size() == 0) - { - const SetIndexArray & indices = d_indices.getValue(); - x0.resize( x.size() ); - xP.resize( x.size() ); - for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) - { - x0[*it] = x[*it]; - xP[*it] = x0[*it]; - } - } - - Real cT = (Real) this->getContext()->getTime(); - - if ((cT != currentTime) || !finished) - { - findKeyTimes(); - } - - - Real dTsimu = (Real) this->getContext()->getDt(); - - - if(finished) - { - Real dt = (cT - prevT) / (nextT - prevT); - Deriv m = (nextV-prevV)*dt + prevV; - - const SetIndexArray & indices = d_indices.getValue(); - const SetIndexArray & coordinates = d_coordinates.getValue(); - - if (coordinates.size() == 0) - { - //set the motion to the Dofs - for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) - { - x[*it] = xP[*it] + m*dTsimu; - xP[*it] = x[*it]; - } - } - else - { - for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) - { - for(SetIndexArray::const_iterator itInd = coordinates.begin(); itInd != coordinates.end(); ++itInd) - { - x[*it][*itInd] = xP[*it][*itInd] + m[*itInd]*dTsimu; - xP[*it] = x[*it]; - } - } - } - } -} - -template -void LinearVelocityConstraint::findKeyTimes() -{ - Real cT = (Real) this->getContext()->getTime(); - finished = false; - - if(d_keyTimes.getValue().size() != 0 && cT >= *d_keyTimes.getValue().begin() && cT <= *d_keyTimes.getValue().rbegin()) - { - nextT = *d_keyTimes.getValue().begin(); - prevT = nextT; - - typename type::vector::const_iterator it_t = d_keyTimes.getValue().begin(); - typename VecDeriv::const_iterator it_v = d_keyVelocities.getValue().begin(); - - //WARNING : we consider that the key-events are in chronological order - //here we search between which keyTimes we are, to know which are the motion to interpolate - while( it_t != d_keyTimes.getValue().end() && !finished) - { - if( *it_t <= cT) - { - prevT = *it_t; - prevV = *it_v; - } - else - { - nextT = *it_t; - nextV = *it_v; - finished = true; - } - ++it_t; - ++it_v; - } - } -}// LinearVelocityConstraint::findKeyTimes - -template -void LinearVelocityConstraint::projectJacobianMatrix(const core::MechanicalParams* /*mparams*/, DataMatrixDeriv& /*cData*/) -{ - -} - -//display the path the constrained dofs will go through -template -void LinearVelocityConstraint::draw(const core::visual::VisualParams* vparams) -{ - if (!vparams->displayFlags().getShowBehaviorModels() || d_keyTimes.getValue().size() == 0 ) return; - const auto stateLifeCycle = vparams->drawTool()->makeStateLifeCycle(); - - vparams->drawTool()->disableLighting(); - - std::vector vertices; - constexpr sofa::type::RGBAColor color(1, 0.5, 0.5, 1); - const VecDeriv& keyVelocities = d_keyVelocities.getValue(); - const SetIndexArray & indices = d_indices.getValue(); - for (unsigned int i=0 ; idrawTool()->drawLines(vertices, 1.0, color); - - -} - -} // namespace sofa::component::constraint::projective +SOFA_DEPRECATED_HEADER("v24.06", "v25.06", "sofa/component/constraint/projective/LinearVelocityProjectiveConstraint.inl") diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/LinearVelocityConstraint.cpp b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/LinearVelocityProjectiveConstraint.cpp similarity index 73% rename from Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/LinearVelocityConstraint.cpp rename to Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/LinearVelocityProjectiveConstraint.cpp index 163a066f18c..fa21e76c8ae 100644 --- a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/LinearVelocityConstraint.cpp +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/LinearVelocityProjectiveConstraint.cpp @@ -19,8 +19,8 @@ * * * Contact information: contact@sofa-framework.org * ******************************************************************************/ -#define SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_LINEARVELOCITYCONSTRAINT_CPP -#include +#define SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_LINEARVELOCITYPROJECTIVECONSTRAINT_CPP +#include #include #include #include @@ -32,19 +32,19 @@ using namespace sofa::defaulttype; using namespace sofa::helper; //declaration of the class, for the factory -int LinearVelocityConstraintClass = core::RegisterObject("apply velocity to given particles") - .add< LinearVelocityConstraint >() - .add< LinearVelocityConstraint >() - .add< LinearVelocityConstraint >() - .add< LinearVelocityConstraint >() - .add< LinearVelocityConstraint >() - +int LinearVelocityProjectiveConstraintClass = core::RegisterObject("apply velocity to given particles") + .add< LinearVelocityProjectiveConstraint >() + .add< LinearVelocityProjectiveConstraint >() + .add< LinearVelocityProjectiveConstraint >() + .add< LinearVelocityProjectiveConstraint >() + .add< LinearVelocityProjectiveConstraint >() + .addAlias("LinearVelocityConstraint") ; -template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API LinearVelocityConstraint; -template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API LinearVelocityConstraint; -template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API LinearVelocityConstraint; -template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API LinearVelocityConstraint; -template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API LinearVelocityConstraint; +template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API LinearVelocityProjectiveConstraint; +template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API LinearVelocityProjectiveConstraint; +template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API LinearVelocityProjectiveConstraint; +template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API LinearVelocityProjectiveConstraint; +template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API LinearVelocityProjectiveConstraint; } // namespace sofa::component::constraint::projective diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/LinearVelocityProjectiveConstraint.h b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/LinearVelocityProjectiveConstraint.h new file mode 100644 index 00000000000..74c67b83997 --- /dev/null +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/LinearVelocityProjectiveConstraint.h @@ -0,0 +1,135 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace sofa::component::constraint::projective +{ + + +/** impose a motion to given DOFs (translation and rotation) + The motion between 2 key times is linearly interpolated +*/ +template +class LinearVelocityProjectiveConstraint : public core::behavior::ProjectiveConstraintSet +{ +public: + SOFA_CLASS(SOFA_TEMPLATE(LinearVelocityProjectiveConstraint,TDataTypes),SOFA_TEMPLATE(core::behavior::ProjectiveConstraintSet,TDataTypes)); + + using Index = sofa::Index; + typedef TDataTypes DataTypes; + typedef typename DataTypes::VecCoord VecCoord; + typedef typename DataTypes::VecDeriv VecDeriv; + typedef typename DataTypes::MatrixDeriv MatrixDeriv; + typedef typename DataTypes::Coord Coord; + typedef typename DataTypes::Deriv Deriv; + typedef typename DataTypes::Real Real; + typedef typename MatrixDeriv::RowType MatrixDerivRowType; + typedef Data DataVecCoord; + typedef Data DataVecDeriv; + typedef Data DataMatrixDeriv; + typedef type::vector SetIndexArray; + typedef sofa::core::topology::TopologySubsetIndices SetIndex; + +public : + /// indices of the DOFs the constraint is applied to + SetIndex d_indices; + /// the key frames when the motion is defined by the user + Data > d_keyTimes; + /// the motions corresponding to the key frames + Data d_keyVelocities; + /// the coordinates on which to applay velocities + SetIndex d_coordinates; + + /// the key times surrounding the current simulation time (for interpolation) + Real prevT, nextT; + ///the velocities corresponding to the surrouding key times + Deriv prevV, nextV; + ///position at the initial step for constrained DOFs position + VecCoord x0; + ///position at the previous step for constrained DOFs position + VecCoord xP; + + /// Link to be set to the topology container in the component graph. + SingleLink, sofa::core::topology::BaseMeshTopology, BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_topology; + +protected: + LinearVelocityProjectiveConstraint(); + + virtual ~LinearVelocityProjectiveConstraint(); +public: + ///methods to add/remove some indices, keyTimes, keyVelocity + void clearIndices(); + void addIndex(Index index); + void removeIndex(Index index); + void clearKeyVelocities(); + /**add a new key movement + @param time : the simulation time you want to set a movement (in sec) + @param movement : the corresponding motion + for instance, addKeyMovement(1.0, Deriv(5,0,0) ) will set a translation of 5 in x direction a time 1.0s + **/ + void addKeyVelocity(Real time, Deriv movement); + + + /// -- Constraint interface + void init() override; + void reset() override; + void projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& resData) override; + void projectVelocity(const core::MechanicalParams* mparams, DataVecDeriv& vData) override; + void projectPosition(const core::MechanicalParams* mparams, DataVecCoord& xData) override; + void projectJacobianMatrix(const core::MechanicalParams* mparams, DataMatrixDeriv& cData) override; + + void draw(const core::visual::VisualParams* vparams) override; + +private: + + /// to keep the time corresponding to the key times + Real currentTime; + + /// to know if we found the key times + bool finished; + + /// find previous and next time keys + void findKeyTimes(); +}; + +#if !defined(SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_LINEARVELOCITYPROJECTIVECONSTRAINT_CPP) +extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API LinearVelocityProjectiveConstraint; +extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API LinearVelocityProjectiveConstraint; +extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API LinearVelocityProjectiveConstraint; +extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API LinearVelocityProjectiveConstraint; +extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API LinearVelocityProjectiveConstraint; + +#endif + +} // namespace sofa::component::constraint::projective diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/LinearVelocityProjectiveConstraint.inl b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/LinearVelocityProjectiveConstraint.inl new file mode 100644 index 00000000000..80ff4e51937 --- /dev/null +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/LinearVelocityProjectiveConstraint.inl @@ -0,0 +1,348 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once + +#include +#include +#include +#include +#include +#include +#include + + +namespace sofa::component::constraint::projective +{ + +template +LinearVelocityProjectiveConstraint::LinearVelocityProjectiveConstraint() + : core::behavior::ProjectiveConstraintSet(nullptr) + , d_indices( initData(&d_indices,"indices","Indices of the constrained points") ) + , d_keyTimes( initData(&d_keyTimes,"keyTimes","key times for the movements") ) + , d_keyVelocities( initData(&d_keyVelocities,"velocities","velocities corresponding to the key times") ) + , d_coordinates( initData(&d_coordinates, "coordinates", "coordinates on which to apply velocities") ) + , l_topology(initLink("topology", "link to the topology container")) + , finished(false) +{ + d_indices.beginEdit()->push_back(0); + d_indices.endEdit(); + + d_keyTimes.beginEdit()->push_back( 0.0 ); + d_keyTimes.endEdit(); + d_keyVelocities.beginEdit()->push_back( Deriv() ); + d_keyVelocities.endEdit(); +} + + +template +LinearVelocityProjectiveConstraint::~LinearVelocityProjectiveConstraint() +{ + +} + +template +void LinearVelocityProjectiveConstraint::clearIndices() +{ + d_indices.beginEdit()->clear(); + d_indices.endEdit(); +} + +template +void LinearVelocityProjectiveConstraint::addIndex(Index index) +{ + d_indices.beginEdit()->push_back(index); + d_indices.endEdit(); +} + +template +void LinearVelocityProjectiveConstraint::removeIndex(Index index) +{ + sofa::type::removeValue(*d_indices.beginEdit(),index); + d_indices.endEdit(); +} + +template +void LinearVelocityProjectiveConstraint::clearKeyVelocities() +{ + d_keyTimes.beginEdit()->clear(); + d_keyTimes.endEdit(); + d_keyVelocities.beginEdit()->clear(); + d_keyVelocities.endEdit(); +} + +template +void LinearVelocityProjectiveConstraint::addKeyVelocity(Real time, Deriv movement) +{ + d_keyTimes.beginEdit()->push_back( time ); + d_keyTimes.endEdit(); + d_keyVelocities.beginEdit()->push_back( movement ); + d_keyVelocities.endEdit(); +} + +// -- Constraint interface + + +template +void LinearVelocityProjectiveConstraint::init() +{ + this->core::behavior::ProjectiveConstraintSet::init(); + + if (l_topology.empty()) + { + msg_info() << "link to Topology container should be set to ensure right behavior. First Topology found in current context will be used."; + l_topology.set(this->getContext()->getMeshTopologyLink()); + } + + if (sofa::core::topology::BaseMeshTopology* _topology = l_topology.get()) + { + msg_info() << "Topology path used: '" << l_topology.getLinkedPath() << "'"; + + // Initialize topological changes support + d_indices.createTopologyHandler(_topology); + d_coordinates.createTopologyHandler(_topology); + } + else + { + msg_info() << "No topology component found at path: " << l_topology.getLinkedPath() << ", nor in current context: " << this->getContext()->name; + } + + x0.resize(0); + xP.resize(0); + nextV = prevV = Deriv(); + + currentTime = -1.0; + finished = false; +} + +template +void LinearVelocityProjectiveConstraint::reset() +{ + nextT = prevT = 0.0; + nextV = prevV = Deriv(); + + currentTime = -1.0; + finished = false; +} + + +template +void LinearVelocityProjectiveConstraint::projectResponse(const core::MechanicalParams* /*mparams*/, DataVecDeriv& resData) +{ + helper::WriteAccessor res = resData; + VecDeriv& dx = res.wref(); + + Real cT = (Real) this->getContext()->getTime(); + if ((cT != currentTime) || !finished) + { + findKeyTimes(); + } + + if (finished && nextT != prevT) + { + const SetIndexArray & indices = d_indices.getValue(); + + //set the motion to the Dofs + for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) + { + dx[*it] = Deriv(); + } + } + +} + +template +void LinearVelocityProjectiveConstraint::projectVelocity(const core::MechanicalParams* /*mparams*/, DataVecDeriv& vData) +{ + helper::WriteAccessor dx = vData; + Real cT = (Real) this->getContext()->getTime(); + + if ((cT != currentTime) || !finished) + { + findKeyTimes(); + } + + if (finished && nextT != prevT) + { + //if we found 2 keyTimes, we have to interpolate a velocity (linear interpolation) + Deriv v = ((nextV - prevV)*((cT - prevT)/(nextT - prevT))) + prevV; + + const SetIndexArray & indices = d_indices.getValue(); + const SetIndexArray & coordinates = d_coordinates.getValue(); + + if (coordinates.size() == 0) + { + //set the motion to the Dofs + for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) + { + dx[*it] = v; + } + } + else + { + for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) + { + for(SetIndexArray::const_iterator itInd = coordinates.begin(); itInd != coordinates.end(); ++itInd) + { + dx[*it][*itInd] = v[*itInd]; + } + } + } + } +} + + +template +void LinearVelocityProjectiveConstraint::projectPosition(const core::MechanicalParams* /*mparams*/, DataVecCoord& xData) +{ + helper::WriteAccessor x = xData; + + //initialize initial Dofs positions, if it's not done + if (x0.size() == 0) + { + const SetIndexArray & indices = d_indices.getValue(); + x0.resize( x.size() ); + xP.resize( x.size() ); + for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) + { + x0[*it] = x[*it]; + xP[*it] = x0[*it]; + } + } + + Real cT = (Real) this->getContext()->getTime(); + + if ((cT != currentTime) || !finished) + { + findKeyTimes(); + } + + + Real dTsimu = (Real) this->getContext()->getDt(); + + + if(finished) + { + Real dt = (cT - prevT) / (nextT - prevT); + Deriv m = (nextV-prevV)*dt + prevV; + + const SetIndexArray & indices = d_indices.getValue(); + const SetIndexArray & coordinates = d_coordinates.getValue(); + + if (coordinates.size() == 0) + { + //set the motion to the Dofs + for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) + { + x[*it] = xP[*it] + m*dTsimu; + xP[*it] = x[*it]; + } + } + else + { + for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) + { + for(SetIndexArray::const_iterator itInd = coordinates.begin(); itInd != coordinates.end(); ++itInd) + { + x[*it][*itInd] = xP[*it][*itInd] + m[*itInd]*dTsimu; + xP[*it] = x[*it]; + } + } + } + } +} + +template +void LinearVelocityProjectiveConstraint::findKeyTimes() +{ + Real cT = (Real) this->getContext()->getTime(); + finished = false; + + if(d_keyTimes.getValue().size() != 0 && cT >= *d_keyTimes.getValue().begin() && cT <= *d_keyTimes.getValue().rbegin()) + { + nextT = *d_keyTimes.getValue().begin(); + prevT = nextT; + + typename type::vector::const_iterator it_t = d_keyTimes.getValue().begin(); + typename VecDeriv::const_iterator it_v = d_keyVelocities.getValue().begin(); + + //WARNING : we consider that the key-events are in chronological order + //here we search between which keyTimes we are, to know which are the motion to interpolate + while( it_t != d_keyTimes.getValue().end() && !finished) + { + if( *it_t <= cT) + { + prevT = *it_t; + prevV = *it_v; + } + else + { + nextT = *it_t; + nextV = *it_v; + finished = true; + } + ++it_t; + ++it_v; + } + } +}// LinearVelocityProjectiveConstraint::findKeyTimes + +template +void LinearVelocityProjectiveConstraint::projectJacobianMatrix(const core::MechanicalParams* /*mparams*/, DataMatrixDeriv& /*cData*/) +{ + +} + +//display the path the constrained dofs will go through +template +void LinearVelocityProjectiveConstraint::draw(const core::visual::VisualParams* vparams) +{ + if (!vparams->displayFlags().getShowBehaviorModels() || d_keyTimes.getValue().size() == 0 ) return; + const auto stateLifeCycle = vparams->drawTool()->makeStateLifeCycle(); + + vparams->drawTool()->disableLighting(); + + std::vector vertices; + constexpr sofa::type::RGBAColor color(1, 0.5, 0.5, 1); + const VecDeriv& keyVelocities = d_keyVelocities.getValue(); + const SetIndexArray & indices = d_indices.getValue(); + for (unsigned int i=0 ; idrawTool()->drawLines(vertices, 1.0, color); + + +} + +} // namespace sofa::component::constraint::projective diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/OscillatorConstraint.h b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/OscillatorConstraint.h index ab2389a61da..9bbd88c7ce1 100644 --- a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/OscillatorConstraint.h +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/OscillatorConstraint.h @@ -20,93 +20,13 @@ * Contact information: contact@sofa-framework.org * ******************************************************************************/ #pragma once -#include -#include -#include -#include +#include +SOFA_DEPRECATED_HEADER("v24.06", "v25.06", "sofa/component/constraint/projective/OscillatorProjectiveConstraint.h") namespace sofa::component::constraint::projective { - -/** - * Apply sinusoidal trajectories to particles. - * Defined as \f$ x = x_m A \sin ( \omega t + \phi )\f$ - * where \f$ x_m, A , \omega t , \phi \f$ are the mean value, the amplitude, the pulsation and the phase, respectively. - */ -template -class OscillatorConstraint : public core::behavior::ProjectiveConstraintSet -{ -public: - SOFA_CLASS(SOFA_TEMPLATE(OscillatorConstraint,TDataTypes),SOFA_TEMPLATE(core::behavior::ProjectiveConstraintSet,TDataTypes)); - - typedef TDataTypes DataTypes; - typedef typename DataTypes::VecCoord VecCoord; - typedef typename DataTypes::VecDeriv VecDeriv; - typedef typename DataTypes::MatrixDeriv MatrixDeriv; - typedef typename DataTypes::Coord Coord; - typedef typename DataTypes::Deriv Deriv; - typedef typename DataTypes::Real Real; - typedef typename MatrixDeriv::RowIterator MatrixDerivRowIterator; - typedef typename MatrixDeriv::RowType MatrixDerivRowType; - typedef Data DataVecCoord; - typedef Data DataVecDeriv; - typedef Data DataMatrixDeriv; - -protected: - struct Oscillator - { - unsigned int index; - Coord mean; - Deriv amplitude; - Real pulsation; - Real phase; - - Oscillator(); - Oscillator(unsigned int i, const Coord& m, const Deriv& a, - const Real& w, const Real& p); - - inline friend std::istream& operator >>(std::istream& in, Oscillator& o) - { - in >> o.index >> o.mean >> o.amplitude >> o.pulsation >> o.phase; - return in; - } - - inline friend std::ostream& operator <<(std::ostream& out, const Oscillator& o) - { - out << o.index << " " << o.mean << " " << o.amplitude << " " - << o.pulsation << " " << o.phase << "\n"; - return out; - } - }; - - Data< type::vector< Oscillator > > constraints; ///< constrained particles - - -public: - explicit OscillatorConstraint(core::behavior::MechanicalState* mstate=nullptr); - ~OscillatorConstraint() override ; - - OscillatorConstraint* addConstraint(unsigned index, - const Coord& mean, const Deriv& amplitude, - Real pulsation, Real phase); - - void projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& resData) override; - void projectVelocity(const core::MechanicalParams* mparams, DataVecDeriv& vData) override; - void projectPosition(const core::MechanicalParams* mparams, DataVecCoord& xData) override; - void projectJacobianMatrix(const core::MechanicalParams* mparams, DataMatrixDeriv& cData) override; - -protected: - template - void projectResponseT(DataDeriv& dx, - const std::function& clear); -}; - - -#if !defined(SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_OSCILLATORCONSTRAINT_CPP) -extern template class OscillatorConstraint; -extern template class OscillatorConstraint; -#endif - -} // namespace sofa::component::constraint::projective +template +using OscillatorConstraint SOFA_ATTRIBUTE_DEPRECATED("v24.06 ", "v25.06", "OscillatorConstraint has been renamed to OscillatorProjectiveConstraint") = OscillatorProjectiveConstraint; +} \ No newline at end of file diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/OscillatorConstraint.inl b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/OscillatorConstraint.inl index 831e3468cfb..72eca9675a6 100644 --- a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/OscillatorConstraint.inl +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/OscillatorConstraint.inl @@ -21,107 +21,6 @@ ******************************************************************************/ #pragma once -#include -#include -#include +#include -namespace sofa::component::constraint::projective -{ - -template -OscillatorConstraint::OscillatorConstraint(core::behavior::MechanicalState* mstate) - : core::behavior::ProjectiveConstraintSet(mstate) - , constraints(initData(&constraints,"oscillators","Define a sequence of oscillating particules: \n[index, Mean(x,y,z), amplitude(x,y,z), pulsation, phase]")) -{ -} - -template -OscillatorConstraint::~OscillatorConstraint() -{ -} - -template -OscillatorConstraint* OscillatorConstraint::addConstraint(unsigned index, const Coord& mean, const Deriv& amplitude, Real pulsation, Real phase) -{ - this->constraints.beginEdit()->push_back( Oscillator(index,mean,amplitude,pulsation,phase) ); - return this; -} - - -template template -void OscillatorConstraint::projectResponseT(DataDeriv& res, - const std::function& clear) -{ - const auto& oscillators = constraints.getValue(); - - for (unsigned i = 0; i < oscillators.size(); ++i) - { - const unsigned& index = oscillators[i].index; - clear(res, index); - } -} - -template -void OscillatorConstraint::projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& resData) -{ - SOFA_UNUSED(mparams); - helper::WriteAccessor res = resData; - projectResponseT(res.wref(),[](VecDeriv& res, const unsigned int index) { res[index].clear(); }); -} - -template -void OscillatorConstraint::projectVelocity(const core::MechanicalParams* /*mparams*/, DataVecDeriv& vData) -{ - helper::WriteAccessor v = vData; - const type::vector& oscillators = constraints.getValue(); - Real t = (Real) this->getContext()->getTime(); - for (unsigned i = 0; i < oscillators.size(); ++i) - { - const unsigned& index = oscillators[i].index; - const Deriv& a = oscillators[i].amplitude; - const Real& w = oscillators[i].pulsation; - const Real& p = oscillators[i].phase; - - v[index] = a * w * cos(w * t + p); - } -} - -template -void OscillatorConstraint::projectPosition(const core::MechanicalParams* /*mparams*/, DataVecCoord& xData) -{ - helper::WriteAccessor x = xData; - const type::vector &oscillators = constraints.getValue(); - Real t = (Real) this->getContext()->getTime(); - for (unsigned i = 0; i < oscillators.size(); ++i) - { - const unsigned& index = oscillators[i].index; - const Coord& m = oscillators[i].mean; - const Deriv& a = oscillators[i].amplitude; - const Real& w = oscillators[i].pulsation; - const Real& p = oscillators[i].phase; - - x[index] = m + a * sin(w * t + p); - } -} - -template -void OscillatorConstraint::projectJacobianMatrix(const core::MechanicalParams* mparams, DataMatrixDeriv& cData) -{ - SOFA_UNUSED(mparams); - helper::WriteAccessor c = cData; - projectResponseT(c.wref(), [](MatrixDeriv& res, const unsigned int index) { res.clearColBlock(index); }); -} - -template -OscillatorConstraint::Oscillator::Oscillator(): index(0) -{ -} - -template -OscillatorConstraint::Oscillator::Oscillator(unsigned int i, const Coord& m, const Deriv& a, - const Real& w, const Real& p) : - index(i), mean(m), amplitude(a), pulsation(w), phase(p) -{ -} - -} // namespace sofa::component::constraint::projective +SOFA_DEPRECATED_HEADER("v24.06", "v25.06", "sofa/component/constraint/projective/OscillatorProjectiveConstraint.inl") diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/OscillatorConstraint.cpp b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/OscillatorProjectiveConstraint.cpp similarity index 77% rename from Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/OscillatorConstraint.cpp rename to Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/OscillatorProjectiveConstraint.cpp index 8edb65b5447..9b5d0b9e3c3 100644 --- a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/OscillatorConstraint.cpp +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/OscillatorProjectiveConstraint.cpp @@ -19,8 +19,8 @@ * * * Contact information: contact@sofa-framework.org * ******************************************************************************/ -#define SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_OSCILLATORCONSTRAINT_CPP -#include +#define SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_OSCILLATORPROJECTIVECONSTRAINT_CPP +#include #include #include #include @@ -31,11 +31,13 @@ namespace sofa::component::constraint::projective using namespace sofa::defaulttype; using namespace sofa::helper; -int OscillatorConstraintClass = core::RegisterObject("Apply a sinusoidal trajectory to given points") - .add< OscillatorConstraint >() - .add< OscillatorConstraint >(); +int OscillatorProjectiveConstraintClass = core::RegisterObject("Apply a sinusoidal trajectory to given points") + .add< OscillatorProjectiveConstraint >() + .add< OscillatorProjectiveConstraint >() + .addAlias("OscillatorConstraint") + ; -template class OscillatorConstraint; -template class OscillatorConstraint; +template class OscillatorProjectiveConstraint; +template class OscillatorProjectiveConstraint; } // namespace sofa::component::constraint::projective diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/OscillatorProjectiveConstraint.h b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/OscillatorProjectiveConstraint.h new file mode 100644 index 00000000000..dd97f5d8b45 --- /dev/null +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/OscillatorProjectiveConstraint.h @@ -0,0 +1,112 @@ +/****************************************************************************** +* 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 + + +namespace sofa::component::constraint::projective +{ + +/** + * Apply sinusoidal trajectories to particles. + * Defined as \f$ x = x_m A \sin ( \omega t + \phi )\f$ + * where \f$ x_m, A , \omega t , \phi \f$ are the mean value, the amplitude, the pulsation and the phase, respectively. + */ +template +class OscillatorProjectiveConstraint : public core::behavior::ProjectiveConstraintSet +{ +public: + SOFA_CLASS(SOFA_TEMPLATE(OscillatorProjectiveConstraint,TDataTypes),SOFA_TEMPLATE(core::behavior::ProjectiveConstraintSet,TDataTypes)); + + typedef TDataTypes DataTypes; + typedef typename DataTypes::VecCoord VecCoord; + typedef typename DataTypes::VecDeriv VecDeriv; + typedef typename DataTypes::MatrixDeriv MatrixDeriv; + typedef typename DataTypes::Coord Coord; + typedef typename DataTypes::Deriv Deriv; + typedef typename DataTypes::Real Real; + typedef typename MatrixDeriv::RowIterator MatrixDerivRowIterator; + typedef typename MatrixDeriv::RowType MatrixDerivRowType; + typedef Data DataVecCoord; + typedef Data DataVecDeriv; + typedef Data DataMatrixDeriv; + +protected: + struct Oscillator + { + unsigned int index; + Coord mean; + Deriv amplitude; + Real pulsation; + Real phase; + + Oscillator(); + Oscillator(unsigned int i, const Coord& m, const Deriv& a, + const Real& w, const Real& p); + + inline friend std::istream& operator >>(std::istream& in, Oscillator& o) + { + in >> o.index >> o.mean >> o.amplitude >> o.pulsation >> o.phase; + return in; + } + + inline friend std::ostream& operator <<(std::ostream& out, const Oscillator& o) + { + out << o.index << " " << o.mean << " " << o.amplitude << " " + << o.pulsation << " " << o.phase << "\n"; + return out; + } + }; + + Data< type::vector< Oscillator > > constraints; ///< constrained particles + + +public: + explicit OscillatorProjectiveConstraint(core::behavior::MechanicalState* mstate=nullptr); + ~OscillatorProjectiveConstraint() override ; + + OscillatorProjectiveConstraint* addConstraint(unsigned index, + const Coord& mean, const Deriv& amplitude, + Real pulsation, Real phase); + + void projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& resData) override; + void projectVelocity(const core::MechanicalParams* mparams, DataVecDeriv& vData) override; + void projectPosition(const core::MechanicalParams* mparams, DataVecCoord& xData) override; + void projectJacobianMatrix(const core::MechanicalParams* mparams, DataMatrixDeriv& cData) override; + +protected: + template + void projectResponseT(DataDeriv& dx, + const std::function& clear); +}; + + +#if !defined(SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_OSCILLATORPROJECTIVECONSTRAINT_CPP) +extern template class OscillatorProjectiveConstraint; +extern template class OscillatorProjectiveConstraint; +#endif + +} // namespace sofa::component::constraint::projective diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/OscillatorProjectiveConstraint.inl b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/OscillatorProjectiveConstraint.inl new file mode 100644 index 00000000000..0f089b2eea1 --- /dev/null +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/OscillatorProjectiveConstraint.inl @@ -0,0 +1,127 @@ +/****************************************************************************** +* 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::constraint::projective +{ + +template +OscillatorProjectiveConstraint::OscillatorProjectiveConstraint(core::behavior::MechanicalState* mstate) + : core::behavior::ProjectiveConstraintSet(mstate) + , constraints(initData(&constraints,"oscillators","Define a sequence of oscillating particules: \n[index, Mean(x,y,z), amplitude(x,y,z), pulsation, phase]")) +{ +} + +template +OscillatorProjectiveConstraint::~OscillatorProjectiveConstraint() +{ +} + +template +OscillatorProjectiveConstraint* OscillatorProjectiveConstraint::addConstraint(unsigned index, const Coord& mean, const Deriv& amplitude, Real pulsation, Real phase) +{ + this->constraints.beginEdit()->push_back( Oscillator(index,mean,amplitude,pulsation,phase) ); + return this; +} + + +template template +void OscillatorProjectiveConstraint::projectResponseT(DataDeriv& res, + const std::function& clear) +{ + const auto& oscillators = constraints.getValue(); + + for (unsigned i = 0; i < oscillators.size(); ++i) + { + const unsigned& index = oscillators[i].index; + clear(res, index); + } +} + +template +void OscillatorProjectiveConstraint::projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& resData) +{ + SOFA_UNUSED(mparams); + helper::WriteAccessor res = resData; + projectResponseT(res.wref(),[](VecDeriv& res, const unsigned int index) { res[index].clear(); }); +} + +template +void OscillatorProjectiveConstraint::projectVelocity(const core::MechanicalParams* /*mparams*/, DataVecDeriv& vData) +{ + helper::WriteAccessor v = vData; + const type::vector& oscillators = constraints.getValue(); + Real t = (Real) this->getContext()->getTime(); + for (unsigned i = 0; i < oscillators.size(); ++i) + { + const unsigned& index = oscillators[i].index; + const Deriv& a = oscillators[i].amplitude; + const Real& w = oscillators[i].pulsation; + const Real& p = oscillators[i].phase; + + v[index] = a * w * cos(w * t + p); + } +} + +template +void OscillatorProjectiveConstraint::projectPosition(const core::MechanicalParams* /*mparams*/, DataVecCoord& xData) +{ + helper::WriteAccessor x = xData; + const type::vector &oscillators = constraints.getValue(); + Real t = (Real) this->getContext()->getTime(); + for (unsigned i = 0; i < oscillators.size(); ++i) + { + const unsigned& index = oscillators[i].index; + const Coord& m = oscillators[i].mean; + const Deriv& a = oscillators[i].amplitude; + const Real& w = oscillators[i].pulsation; + const Real& p = oscillators[i].phase; + + x[index] = m + a * sin(w * t + p); + } +} + +template +void OscillatorProjectiveConstraint::projectJacobianMatrix(const core::MechanicalParams* mparams, DataMatrixDeriv& cData) +{ + SOFA_UNUSED(mparams); + helper::WriteAccessor c = cData; + projectResponseT(c.wref(), [](MatrixDeriv& res, const unsigned int index) { res.clearColBlock(index); }); +} + +template +OscillatorProjectiveConstraint::Oscillator::Oscillator(): index(0) +{ +} + +template +OscillatorProjectiveConstraint::Oscillator::Oscillator(unsigned int i, const Coord& m, const Deriv& a, + const Real& w, const Real& p) : + index(i), mean(m), amplitude(a), pulsation(w), phase(p) +{ +} + +} // namespace sofa::component::constraint::projective diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/ParabolicConstraint.h b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/ParabolicConstraint.h index 4b6b1bc7c75..9dcfe542353 100644 --- a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/ParabolicConstraint.h +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/ParabolicConstraint.h @@ -20,108 +20,13 @@ * Contact information: contact@sofa-framework.org * ******************************************************************************/ #pragma once -#include -#include -#include -#include -#include +#include +SOFA_DEPRECATED_HEADER("v24.06", "v25.06", "sofa/component/constraint/projective/ParabolicProjectiveConstraint.h") namespace sofa::component::constraint::projective { - -/** Apply a parabolic trajectory to particles going through 3 points specified by the user. - The DOFs set in the "indices" list follow the computed parabol from "tBegin" to "tEnd". - */ -template -class ParabolicConstraint : public core::behavior::ProjectiveConstraintSet -{ -public: - SOFA_CLASS(SOFA_TEMPLATE(ParabolicConstraint,DataTypes),SOFA_TEMPLATE(core::behavior::ProjectiveConstraintSet,DataTypes)); - - typedef typename DataTypes::VecCoord VecCoord; - typedef typename DataTypes::VecDeriv VecDeriv; - typedef typename DataTypes::MatrixDeriv MatrixDeriv; - typedef typename DataTypes::Coord Coord; - typedef typename DataTypes::Deriv Deriv; - typedef typename DataTypes::Real Real; - typedef typename MatrixDeriv::RowIterator MatrixDerivRowIterator; - typedef typename MatrixDeriv::RowType MatrixDerivRowType; - typedef Data DataVecCoord; - typedef Data DataVecDeriv; - typedef Data DataMatrixDeriv; - typedef type::vector SetIndexArray; - typedef sofa::core::topology::TopologySubsetIndices SetIndex; - typedef type::Vec<3, Real> Vec3R; - typedef type::Quat QuatR; - -protected: - ///indices of the DOFs constraints - SetIndex m_indices; - - /// the three points defining the parabol - Data m_P1; - Data m_P2; ///< second point of the parabol - Data m_P3; ///< third point of the parabol - - /// the time steps defining the velocity of the movement - Data m_tBegin; - Data m_tEnd; ///< End Time of the motion - - /// Link to be set to the topology container in the component graph. - SingleLink, sofa::core::topology::BaseMeshTopology, BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_topology; - - /// the 3 points projected in the parabol plan - Vec3R m_locP1; - Vec3R m_locP2; - Vec3R m_locP3; - - /// the quaternion doing the projection - QuatR m_projection; - - explicit ParabolicConstraint(core::behavior::MechanicalState* mstate = nullptr); - - ~ParabolicConstraint(); -public: - void addConstraint(unsigned index ); - - void setP1(const Vec3R &p) {m_P1.setValue(p);} - void setP2(const Vec3R &p) {m_P2.setValue(p);} - void setP3(const Vec3R &p) {m_P3.setValue(p);} - - void setBeginTime(const Real &t) {m_tBegin.setValue(t);} - void setEndTime(const Real &t) {m_tEnd.setValue(t);} - - Vec3R getP1() {return m_P1.getValue();} - Vec3R getP2() {return m_P2.getValue();} - Vec3R getP3() {return m_P3.getValue();} - - Real getBeginTime() {return m_tBegin.getValue();} - Real getEndTime() {return m_tEnd.getValue();} - - /// -- Constraint interface - void init() override; - void reinit() override; - - void projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& resData) override; - void projectVelocity(const core::MechanicalParams* mparams, DataVecDeriv& vData) override; - void projectPosition(const core::MechanicalParams* mparams, DataVecCoord& xData) override; - void projectJacobianMatrix(const core::MechanicalParams* mparams, DataMatrixDeriv& cData) override; - - void draw(const core::visual::VisualParams* vparams) override; - -protected: - template - void projectResponseT(DataDeriv& dx, - const std::function& clear); -}; - - -#if !defined(SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_PARABOLICCONSTRAINT_CPP) -extern template class ParabolicConstraint; -extern template class ParabolicConstraint; - -#endif - -} // namespace sofa::component::constraint::projective +template +using ParabolicConstraint SOFA_ATTRIBUTE_DEPRECATED("v24.06 ", "v25.06", "ParabolicConstraint has been renamed to ParabolicProjectiveConstraint") = ParabolicProjectiveConstraint; +} \ No newline at end of file diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/ParabolicConstraint.inl b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/ParabolicConstraint.inl index e8b6489cc9c..7c6fa4b59a2 100644 --- a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/ParabolicConstraint.inl +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/ParabolicConstraint.inl @@ -21,243 +21,6 @@ ******************************************************************************/ #pragma once -#include -#include -#include +#include -namespace sofa::component::constraint::projective -{ - -template -ParabolicConstraint::ParabolicConstraint(core::behavior::MechanicalState* mstate) - : core::behavior::ProjectiveConstraintSet(mstate) - , m_indices( initData(&m_indices,"indices","Indices of the constrained points") ) - , m_P1(initData(&m_P1,"P1","first point of the parabol") ) - , m_P2(initData(&m_P2,"P2","second point of the parabol") ) - , m_P3(initData(&m_P3,"P3","third point of the parabol") ) - , m_tBegin(initData(&m_tBegin,"BeginTime","Begin Time of the motion") ) - , m_tEnd(initData(&m_tEnd,"EndTime","End Time of the motion") ) - , l_topology(initLink("topology", "link to the topology container")) -{ -} - -template -ParabolicConstraint::~ParabolicConstraint() -{ -} - -template -void ParabolicConstraint::addConstraint(unsigned index) -{ - m_indices.beginEdit()->push_back(index); - m_indices.endEdit(); -} - - -template -void ParabolicConstraint::init() -{ - this->core::behavior::ProjectiveConstraintSet::init(); - - if (l_topology.empty()) - { - msg_info() << "link to Topology container should be set to ensure right behavior. First Topology found in current context will be used."; - l_topology.set(this->getContext()->getMeshTopologyLink()); - } - - if (sofa::core::topology::BaseMeshTopology* _topology = l_topology.get()) - { - msg_info() << "Topology path used: '" << l_topology.getLinkedPath() << "'"; - - // Initialize functions and parameters for topology data and handler - m_indices.createTopologyHandler(_topology); - } - else - { - msg_info() << "No topology component found at path: " << l_topology.getLinkedPath() << ", nor in current context: " << this->getContext()->name; - } - - Vec3R P1 = m_P1.getValue(); - Vec3R P2 = m_P2.getValue(); - Vec3R P3 = m_P3.getValue(); - - //compute the projection to go in the parabol plan, - //such as P1 is the origin, P1P3 vector is the x axis, and P1P2 is in the xy plan - //by the way the computation of the parabol equation is much easier - if(P1 != P2 && P1 != P3 && P2 != P3) - { - - Vec3R P1P2 = P2 - P1; - Vec3R P1P3 = P3 - P1; - - Vec3R ax = P1P3; - Vec3R az = cross(P1P3, P1P2); - Vec3R ay = cross(az, ax); - ax.normalize(); - ay.normalize(); - az.normalize(); - - - type::Mat<3,3,Real> Mrot(ax, ay, az); - type::Mat<3,3,Real> Mrot2; - Mrot2.transpose(Mrot); - m_projection.fromMatrix(Mrot2); - m_projection.normalize(); - - m_locP1 = Vec3R(); - m_locP2 = m_projection.inverseRotate(P1P2); - m_locP3 = m_projection.inverseRotate(P1P3); - } -} - -template -void ParabolicConstraint::reinit() -{ - init(); -} - - -template -template -void ParabolicConstraint::projectResponseT(DataDeriv& dx, - const std::function& clear) -{ - Real t = (Real) this->getContext()->getTime(); - if ( t >= m_tBegin.getValue() && t <= m_tEnd.getValue()) - { - const SetIndexArray & indices = m_indices.getValue(); - for(SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) - clear(dx, *it); - } -} - -template -void ParabolicConstraint::projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& resData) -{ - SOFA_UNUSED(mparams); - helper::WriteAccessor res = resData; - projectResponseT(res.wref(),[](VecDeriv& dx, const unsigned int index) { dx[index].clear(); }); -} - -template -void ParabolicConstraint::projectVelocity(const core::MechanicalParams* /*mparams*/, DataVecDeriv& vData) -{ - helper::WriteAccessor dx = vData; - Real t = (Real) this->getContext()->getTime(); - Real dt = (Real) this->getContext()->getDt(); - - if ( t >= m_tBegin.getValue() && t <= m_tEnd.getValue() ) - { - Real relativeTime = (t - m_tBegin.getValue() ) / (m_tEnd.getValue() - m_tBegin.getValue()); - const SetIndexArray & indices = m_indices.getValue(); - - for(SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) - { - //compute velocity by doing v = dx/dt - Real pxP = m_locP3.x()*relativeTime; - Real pyP = (- m_locP2.y() / (m_locP3.x()*m_locP2.x() - m_locP2.x()*m_locP2.x())) * (pxP *pxP) + ( (m_locP3.x()*m_locP2.y()) / (m_locP3.x()*m_locP2.x() - m_locP2.x()*m_locP2.x())) * pxP; - relativeTime = (t+dt - m_tBegin.getValue() ) / (m_tEnd.getValue() - m_tBegin.getValue()); - Real pxN = m_locP3.x()*relativeTime; - Real pyN = (- m_locP2.y() / (m_locP3.x()*m_locP2.x() - m_locP2.x()*m_locP2.x())) * (pxN *pxN) + ( (m_locP3.x()*m_locP2.y()) / (m_locP3.x()*m_locP2.x() - m_locP2.x()*m_locP2.x())) * pxN; - - Vec3R locVel = Vec3R( (pxN-pxP)/dt, (pyN-pyP)/dt, 0.0); - - Vec3R worldVel = m_projection.rotate(locVel); - - dx[*it] = worldVel; - } - } -} - -template -void ParabolicConstraint::projectPosition(const core::MechanicalParams* /*mparams*/, DataVecCoord& xData) -{ - helper::WriteAccessor x = xData; - Real t = (Real) this->getContext()->getTime(); - - if ( t >= m_tBegin.getValue() && t <= m_tEnd.getValue() ) - { - Real relativeTime = (t - m_tBegin.getValue() ) / (m_tEnd.getValue() - m_tBegin.getValue()); - const SetIndexArray & indices = m_indices.getValue(); - - for(SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) - { - //compute position from the equation of the parabol : Y = -y2/(x3*x2-x2�) * X� + (x3*y2)/(x3*x2-x2�) * X - //with P1:(0,0,0), P2:(x2,y2,z2), P3:(x3,y3,z3) , projected in parabol plan - Real px = m_locP3.x()*relativeTime; - Real py = (- m_locP2.y() / (m_locP3.x()*m_locP2.x() - m_locP2.x()*m_locP2.x())) * (px *px) + ( (m_locP3.x()*m_locP2.y()) / (m_locP3.x()*m_locP2.x() - m_locP2.x()*m_locP2.x())) * px; - Vec3R locPos( px , py, 0.0); - - //projection to world coordinates - Vec3R worldPos = m_P1.getValue() + m_projection.rotate(locPos); - - x[*it] = worldPos; - } - } -} - -template -void ParabolicConstraint::projectJacobianMatrix(const core::MechanicalParams* mparams, DataMatrixDeriv& cData) -{ - SOFA_UNUSED(mparams); - helper::WriteAccessor c = cData; - - projectResponseT(c.wref(), [](MatrixDeriv& res, const unsigned int index) { res.clearColBlock(index); }); -} - - -template -void ParabolicConstraint::draw(const core::visual::VisualParams* vparams) -{ - const auto stateLifeCycle = vparams->drawTool()->makeStateLifeCycle(); - - if (!vparams->displayFlags().getShowBehaviorModels()) return; - - Real dt = (Real) this->getContext()->getDt(); - Real t = m_tEnd.getValue() - m_tBegin.getValue(); - Real nbStep = t/dt; - - vparams->drawTool()->disableLighting(); - constexpr sofa::type::RGBAColor color(1, 0.5, 0.5, 1); - std::vector vertices; - - for (unsigned int i=0 ; i< nbStep ; i++) - { - //draw lines between each step of the parabolic trajectory - //so, the smaller is dt, the finer is the parabol - Real relativeTime = i/nbStep; - Real px = m_locP3.x()*relativeTime; - Real py = (- m_locP2.y() / (m_locP3.x()*m_locP2.x() - m_locP2.x()*m_locP2.x())) * (px *px) + ( (m_locP3.x()*m_locP2.y()) / (m_locP3.x()*m_locP2.x() - m_locP2.x()*m_locP2.x())) * px; - Vec3R locPos( px , py, 0.0); - Vec3R worldPos = m_P1.getValue() + m_projection.rotate(locPos); - - vertices.push_back(sofa::type::Vec3(worldPos[0],worldPos[1],worldPos[2])); - - relativeTime = (i+1)/nbStep; - px = m_locP3.x()*relativeTime; - py = (- m_locP2.y() / (m_locP3.x()*m_locP2.x() - m_locP2.x()*m_locP2.x())) * (px *px) + ( (m_locP3.x()*m_locP2.y()) / (m_locP3.x()*m_locP2.x() - m_locP2.x()*m_locP2.x())) * px; - locPos = Vec3R( px , py, 0.0); - worldPos = m_P1.getValue() + m_projection.rotate(locPos); - - - vertices.push_back(sofa::type::Vec3(worldPos[0],worldPos[1],worldPos[2])); - - } - vparams->drawTool()->drawLines(vertices, 1.0, color); - vertices.clear(); - - //draw points for the 3 control points - const Vec3R& mp1 = m_P1.getValue(); - const Vec3R& mp2 = m_P2.getValue(); - const Vec3R& mp3 = m_P3.getValue(); - vertices.push_back(sofa::type::Vec3(mp1[0],mp1[1],mp1[2])); - vertices.push_back(sofa::type::Vec3(mp2[0],mp2[1],mp2[2])); - vertices.push_back(sofa::type::Vec3(mp3[0],mp3[1],mp3[2])); - - vparams->drawTool()->drawPoints(vertices, 5.0, color); - - - -} - -} // namespace sofa::component::constraint::projective +SOFA_DEPRECATED_HEADER("v24.06", "v25.06", "sofa/component/constraint/projective/ParabolicProjectiveConstraint.inl") diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/ParabolicConstraint.cpp b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/ParabolicProjectiveConstraint.cpp similarity index 75% rename from Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/ParabolicConstraint.cpp rename to Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/ParabolicProjectiveConstraint.cpp index 15239ed4f36..76f60aa6ee6 100644 --- a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/ParabolicConstraint.cpp +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/ParabolicProjectiveConstraint.cpp @@ -19,8 +19,8 @@ * * * Contact information: contact@sofa-framework.org * ******************************************************************************/ -#define SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_PARABOLICCONSTRAINT_CPP -#include +#define SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_PARABOLICPROJECTIVECONSTRAINT_CPP +#include #include #include #include @@ -28,11 +28,13 @@ namespace sofa::component::constraint::projective { -int ParabolicConstraintClass = core::RegisterObject("Apply a parabolic trajectory to given points") - .add< ParabolicConstraint >() - .add< ParabolicConstraint >(); +int ParabolicProjectiveConstraintClass = core::RegisterObject("Apply a parabolic trajectory to given points") + .add< ParabolicProjectiveConstraint >() + .add< ParabolicProjectiveConstraint >() + .addAlias("ParabolicConstraint") + ; -template class ParabolicConstraint; -template class ParabolicConstraint; +template class ParabolicProjectiveConstraint; +template class ParabolicProjectiveConstraint; } // namespace sofa::component::constraint::projective diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/ParabolicProjectiveConstraint.h b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/ParabolicProjectiveConstraint.h new file mode 100644 index 00000000000..422c9cb020b --- /dev/null +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/ParabolicProjectiveConstraint.h @@ -0,0 +1,126 @@ +/****************************************************************************** +* 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::component::constraint::projective +{ + +/** Apply a parabolic trajectory to particles going through 3 points specified by the user. + The DOFs set in the "indices" list follow the computed parabol from "tBegin" to "tEnd". + */ +template +class ParabolicProjectiveConstraint : public core::behavior::ProjectiveConstraintSet +{ +public: + SOFA_CLASS(SOFA_TEMPLATE(ParabolicProjectiveConstraint,DataTypes),SOFA_TEMPLATE(core::behavior::ProjectiveConstraintSet,DataTypes)); + + typedef typename DataTypes::VecCoord VecCoord; + typedef typename DataTypes::VecDeriv VecDeriv; + typedef typename DataTypes::MatrixDeriv MatrixDeriv; + typedef typename DataTypes::Coord Coord; + typedef typename DataTypes::Deriv Deriv; + typedef typename DataTypes::Real Real; + typedef typename MatrixDeriv::RowIterator MatrixDerivRowIterator; + typedef typename MatrixDeriv::RowType MatrixDerivRowType; + typedef Data DataVecCoord; + typedef Data DataVecDeriv; + typedef Data DataMatrixDeriv; + typedef type::vector SetIndexArray; + typedef sofa::core::topology::TopologySubsetIndices SetIndex; + typedef type::Vec<3, Real> Vec3R; + typedef type::Quat QuatR; + +protected: + ///indices of the DOFs constraints + SetIndex m_indices; + + /// the three points defining the parabol + Data m_P1; + Data m_P2; ///< second point of the parabol + Data m_P3; ///< third point of the parabol + + /// the time steps defining the velocity of the movement + Data m_tBegin; + Data m_tEnd; ///< End Time of the motion + + /// Link to be set to the topology container in the component graph. + SingleLink, sofa::core::topology::BaseMeshTopology, BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_topology; + + /// the 3 points projected in the parabol plan + Vec3R m_locP1; + Vec3R m_locP2; + Vec3R m_locP3; + + /// the quaternion doing the projection + QuatR m_projection; + + explicit ParabolicProjectiveConstraint(core::behavior::MechanicalState* mstate = nullptr); + + ~ParabolicProjectiveConstraint(); +public: + void addConstraint(unsigned index ); + + void setP1(const Vec3R &p) {m_P1.setValue(p);} + void setP2(const Vec3R &p) {m_P2.setValue(p);} + void setP3(const Vec3R &p) {m_P3.setValue(p);} + + void setBeginTime(const Real &t) {m_tBegin.setValue(t);} + void setEndTime(const Real &t) {m_tEnd.setValue(t);} + + Vec3R getP1() {return m_P1.getValue();} + Vec3R getP2() {return m_P2.getValue();} + Vec3R getP3() {return m_P3.getValue();} + + Real getBeginTime() {return m_tBegin.getValue();} + Real getEndTime() {return m_tEnd.getValue();} + + /// -- Constraint interface + void init() override; + void reinit() override; + + void projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& resData) override; + void projectVelocity(const core::MechanicalParams* mparams, DataVecDeriv& vData) override; + void projectPosition(const core::MechanicalParams* mparams, DataVecCoord& xData) override; + void projectJacobianMatrix(const core::MechanicalParams* mparams, DataMatrixDeriv& cData) override; + + void draw(const core::visual::VisualParams* vparams) override; + +protected: + template + void projectResponseT(DataDeriv& dx, + const std::function& clear); +}; + + +#if !defined(SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_PARABOLICPROJECTIVECONSTRAINT_CPP) +extern template class ParabolicProjectiveConstraint; +extern template class ParabolicProjectiveConstraint; +#endif + +} // namespace sofa::component::constraint::projective diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/ParabolicProjectiveConstraint.inl b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/ParabolicProjectiveConstraint.inl new file mode 100644 index 00000000000..09a34cc7b9b --- /dev/null +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/ParabolicProjectiveConstraint.inl @@ -0,0 +1,263 @@ +/****************************************************************************** +* 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::constraint::projective +{ + +template +ParabolicProjectiveConstraint::ParabolicProjectiveConstraint(core::behavior::MechanicalState* mstate) + : core::behavior::ProjectiveConstraintSet(mstate) + , m_indices( initData(&m_indices,"indices","Indices of the constrained points") ) + , m_P1(initData(&m_P1,"P1","first point of the parabol") ) + , m_P2(initData(&m_P2,"P2","second point of the parabol") ) + , m_P3(initData(&m_P3,"P3","third point of the parabol") ) + , m_tBegin(initData(&m_tBegin,"BeginTime","Begin Time of the motion") ) + , m_tEnd(initData(&m_tEnd,"EndTime","End Time of the motion") ) + , l_topology(initLink("topology", "link to the topology container")) +{ +} + +template +ParabolicProjectiveConstraint::~ParabolicProjectiveConstraint() +{ +} + +template +void ParabolicProjectiveConstraint::addConstraint(unsigned index) +{ + m_indices.beginEdit()->push_back(index); + m_indices.endEdit(); +} + + +template +void ParabolicProjectiveConstraint::init() +{ + this->core::behavior::ProjectiveConstraintSet::init(); + + if (l_topology.empty()) + { + msg_info() << "link to Topology container should be set to ensure right behavior. First Topology found in current context will be used."; + l_topology.set(this->getContext()->getMeshTopologyLink()); + } + + if (sofa::core::topology::BaseMeshTopology* _topology = l_topology.get()) + { + msg_info() << "Topology path used: '" << l_topology.getLinkedPath() << "'"; + + // Initialize functions and parameters for topology data and handler + m_indices.createTopologyHandler(_topology); + } + else + { + msg_info() << "No topology component found at path: " << l_topology.getLinkedPath() << ", nor in current context: " << this->getContext()->name; + } + + Vec3R P1 = m_P1.getValue(); + Vec3R P2 = m_P2.getValue(); + Vec3R P3 = m_P3.getValue(); + + //compute the projection to go in the parabol plan, + //such as P1 is the origin, P1P3 vector is the x axis, and P1P2 is in the xy plan + //by the way the computation of the parabol equation is much easier + if(P1 != P2 && P1 != P3 && P2 != P3) + { + + Vec3R P1P2 = P2 - P1; + Vec3R P1P3 = P3 - P1; + + Vec3R ax = P1P3; + Vec3R az = cross(P1P3, P1P2); + Vec3R ay = cross(az, ax); + ax.normalize(); + ay.normalize(); + az.normalize(); + + + type::Mat<3,3,Real> Mrot(ax, ay, az); + type::Mat<3,3,Real> Mrot2; + Mrot2.transpose(Mrot); + m_projection.fromMatrix(Mrot2); + m_projection.normalize(); + + m_locP1 = Vec3R(); + m_locP2 = m_projection.inverseRotate(P1P2); + m_locP3 = m_projection.inverseRotate(P1P3); + } +} + +template +void ParabolicProjectiveConstraint::reinit() +{ + init(); +} + + +template +template +void ParabolicProjectiveConstraint::projectResponseT(DataDeriv& dx, + const std::function& clear) +{ + Real t = (Real) this->getContext()->getTime(); + if ( t >= m_tBegin.getValue() && t <= m_tEnd.getValue()) + { + const SetIndexArray & indices = m_indices.getValue(); + for(SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) + clear(dx, *it); + } +} + +template +void ParabolicProjectiveConstraint::projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& resData) +{ + SOFA_UNUSED(mparams); + helper::WriteAccessor res = resData; + projectResponseT(res.wref(),[](VecDeriv& dx, const unsigned int index) { dx[index].clear(); }); +} + +template +void ParabolicProjectiveConstraint::projectVelocity(const core::MechanicalParams* /*mparams*/, DataVecDeriv& vData) +{ + helper::WriteAccessor dx = vData; + Real t = (Real) this->getContext()->getTime(); + Real dt = (Real) this->getContext()->getDt(); + + if ( t >= m_tBegin.getValue() && t <= m_tEnd.getValue() ) + { + Real relativeTime = (t - m_tBegin.getValue() ) / (m_tEnd.getValue() - m_tBegin.getValue()); + const SetIndexArray & indices = m_indices.getValue(); + + for(SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) + { + //compute velocity by doing v = dx/dt + Real pxP = m_locP3.x()*relativeTime; + Real pyP = (- m_locP2.y() / (m_locP3.x()*m_locP2.x() - m_locP2.x()*m_locP2.x())) * (pxP *pxP) + ( (m_locP3.x()*m_locP2.y()) / (m_locP3.x()*m_locP2.x() - m_locP2.x()*m_locP2.x())) * pxP; + relativeTime = (t+dt - m_tBegin.getValue() ) / (m_tEnd.getValue() - m_tBegin.getValue()); + Real pxN = m_locP3.x()*relativeTime; + Real pyN = (- m_locP2.y() / (m_locP3.x()*m_locP2.x() - m_locP2.x()*m_locP2.x())) * (pxN *pxN) + ( (m_locP3.x()*m_locP2.y()) / (m_locP3.x()*m_locP2.x() - m_locP2.x()*m_locP2.x())) * pxN; + + Vec3R locVel = Vec3R( (pxN-pxP)/dt, (pyN-pyP)/dt, 0.0); + + Vec3R worldVel = m_projection.rotate(locVel); + + dx[*it] = worldVel; + } + } +} + +template +void ParabolicProjectiveConstraint::projectPosition(const core::MechanicalParams* /*mparams*/, DataVecCoord& xData) +{ + helper::WriteAccessor x = xData; + Real t = (Real) this->getContext()->getTime(); + + if ( t >= m_tBegin.getValue() && t <= m_tEnd.getValue() ) + { + Real relativeTime = (t - m_tBegin.getValue() ) / (m_tEnd.getValue() - m_tBegin.getValue()); + const SetIndexArray & indices = m_indices.getValue(); + + for(SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) + { + //compute position from the equation of the parabol : Y = -y2/(x3*x2-x2�) * X� + (x3*y2)/(x3*x2-x2�) * X + //with P1:(0,0,0), P2:(x2,y2,z2), P3:(x3,y3,z3) , projected in parabol plan + Real px = m_locP3.x()*relativeTime; + Real py = (- m_locP2.y() / (m_locP3.x()*m_locP2.x() - m_locP2.x()*m_locP2.x())) * (px *px) + ( (m_locP3.x()*m_locP2.y()) / (m_locP3.x()*m_locP2.x() - m_locP2.x()*m_locP2.x())) * px; + Vec3R locPos( px , py, 0.0); + + //projection to world coordinates + Vec3R worldPos = m_P1.getValue() + m_projection.rotate(locPos); + + x[*it] = worldPos; + } + } +} + +template +void ParabolicProjectiveConstraint::projectJacobianMatrix(const core::MechanicalParams* mparams, DataMatrixDeriv& cData) +{ + SOFA_UNUSED(mparams); + helper::WriteAccessor c = cData; + + projectResponseT(c.wref(), [](MatrixDeriv& res, const unsigned int index) { res.clearColBlock(index); }); +} + + +template +void ParabolicProjectiveConstraint::draw(const core::visual::VisualParams* vparams) +{ + const auto stateLifeCycle = vparams->drawTool()->makeStateLifeCycle(); + + if (!vparams->displayFlags().getShowBehaviorModels()) return; + + Real dt = (Real) this->getContext()->getDt(); + Real t = m_tEnd.getValue() - m_tBegin.getValue(); + Real nbStep = t/dt; + + vparams->drawTool()->disableLighting(); + constexpr sofa::type::RGBAColor color(1, 0.5, 0.5, 1); + std::vector vertices; + + for (unsigned int i=0 ; i< nbStep ; i++) + { + //draw lines between each step of the parabolic trajectory + //so, the smaller is dt, the finer is the parabol + Real relativeTime = i/nbStep; + Real px = m_locP3.x()*relativeTime; + Real py = (- m_locP2.y() / (m_locP3.x()*m_locP2.x() - m_locP2.x()*m_locP2.x())) * (px *px) + ( (m_locP3.x()*m_locP2.y()) / (m_locP3.x()*m_locP2.x() - m_locP2.x()*m_locP2.x())) * px; + Vec3R locPos( px , py, 0.0); + Vec3R worldPos = m_P1.getValue() + m_projection.rotate(locPos); + + vertices.push_back(sofa::type::Vec3(worldPos[0],worldPos[1],worldPos[2])); + + relativeTime = (i+1)/nbStep; + px = m_locP3.x()*relativeTime; + py = (- m_locP2.y() / (m_locP3.x()*m_locP2.x() - m_locP2.x()*m_locP2.x())) * (px *px) + ( (m_locP3.x()*m_locP2.y()) / (m_locP3.x()*m_locP2.x() - m_locP2.x()*m_locP2.x())) * px; + locPos = Vec3R( px , py, 0.0); + worldPos = m_P1.getValue() + m_projection.rotate(locPos); + + + vertices.push_back(sofa::type::Vec3(worldPos[0],worldPos[1],worldPos[2])); + + } + vparams->drawTool()->drawLines(vertices, 1.0, color); + vertices.clear(); + + //draw points for the 3 control points + const Vec3R& mp1 = m_P1.getValue(); + const Vec3R& mp2 = m_P2.getValue(); + const Vec3R& mp3 = m_P3.getValue(); + vertices.push_back(sofa::type::Vec3(mp1[0],mp1[1],mp1[2])); + vertices.push_back(sofa::type::Vec3(mp2[0],mp2[1],mp2[2])); + vertices.push_back(sofa::type::Vec3(mp3[0],mp3[1],mp3[2])); + + vparams->drawTool()->drawPoints(vertices, 5.0, color); + + + +} + +} // namespace sofa::component::constraint::projective diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PartialFixedConstraint.h b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PartialFixedConstraint.h index b54bdc2b611..b5d76e4379f 100644 --- a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PartialFixedConstraint.h +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PartialFixedConstraint.h @@ -20,87 +20,13 @@ * Contact information: contact@sofa-framework.org * ******************************************************************************/ #pragma once -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include -namespace sofa::component::constraint::projective -{ +SOFA_DEPRECATED_HEADER("v24.06", "v25.06", "sofa/component/constraint/projective/PartialFixedProjectiveConstraint.h") -/** - * Attach given particles to their initial positions, in some directions only. - * The fixed and free directioons are the same for all the particles, defined in the fixedDirections attribute. - **/ -template -class PartialFixedConstraint : public FixedConstraint +namespace sofa::component::constraint::projective { -public: - SOFA_CLASS(SOFA_TEMPLATE(PartialFixedConstraint,DataTypes),SOFA_TEMPLATE(FixedConstraint, DataTypes)); - - typedef FixedConstraint Inherited; - typedef typename DataTypes::VecCoord VecCoord; - typedef typename DataTypes::VecDeriv VecDeriv; - typedef typename DataTypes::MatrixDeriv MatrixDeriv; - typedef typename DataTypes::Coord Coord; - typedef typename DataTypes::Deriv Deriv; - typedef typename DataTypes::Real Real; - typedef typename MatrixDeriv::RowIterator MatrixDerivRowIterator; - typedef typename MatrixDeriv::RowType MatrixDerivRowType; - typedef Data DataVecCoord; - typedef Data DataVecDeriv; - typedef Data DataMatrixDeriv; - typedef type::vector SetIndexArray; - typedef sofa::core::topology::TopologySubsetIndices SetIndex; - -public: - enum { NumDimensions = Deriv::total_size }; - typedef sofa::type::fixed_array VecBool; - Data d_fixedDirections; ///< Defines the directions in which the particles are fixed: true (or 1) for fixed, false (or 0) for free. - Data d_projectVelocity; ///< activate project velocity to set velocity to zero - -protected: - PartialFixedConstraint(); - virtual ~PartialFixedConstraint(); - -public: - - // -- Constraint interface - void reinit() override; - - void projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& resData) override; - void projectVelocity(const core::MechanicalParams* mparams, DataVecDeriv& vData) override; - void projectJacobianMatrix(const core::MechanicalParams* mparams, DataMatrixDeriv& cData) override; - - void applyConstraint(const core::MechanicalParams* mparams, linearalgebra::BaseVector* vector, const sofa::core::behavior::MultiMatrixAccessor* matrix) override; - void applyConstraint(const core::MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) override; - - void projectMatrix( sofa::linearalgebra::BaseMatrix* /*M*/, unsigned /*offset*/ ) override; - - void applyConstraint(sofa::core::behavior::ZeroDirichletCondition* matrix) override; - -protected: - template - void projectResponseT(DataDeriv& dx, - const std::function& clear); -}; - -#if !defined(SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_PARTIALFIXEDCONSTRAINT_CPP) -extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PartialFixedConstraint; -extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PartialFixedConstraint; -extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PartialFixedConstraint; -extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PartialFixedConstraint; -extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PartialFixedConstraint; -extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PartialFixedConstraint; - -#endif - -} // namespace sofa::component::constraint::projective +template +using PartialFixedConstraint SOFA_ATTRIBUTE_DEPRECATED("v24.06 ", "v25.06", "PartialFixedConstraint has been renamed to PartialFixedProjectiveConstraint") = PartialFixedProjectiveConstraint; +} \ No newline at end of file diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PartialFixedConstraint.inl b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PartialFixedConstraint.inl index 1e0a8bb8b9f..1fd9f96ae7b 100644 --- a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PartialFixedConstraint.inl +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PartialFixedConstraint.inl @@ -21,323 +21,6 @@ ******************************************************************************/ #pragma once -#include -#include -#include -#include -#include -#include +#include - -namespace sofa::component::constraint::projective -{ - -template -PartialFixedConstraint::PartialFixedConstraint() - : d_fixedDirections( initData(&d_fixedDirections,"fixedDirections","for each direction, 1 if fixed, 0 if free") ) - , d_projectVelocity(initData(&d_projectVelocity, false, "projectVelocity", "project velocity to ensure no drift of the fixed point")) -{ - VecBool blockedDirection; - for( unsigned i=0; i -PartialFixedConstraint::~PartialFixedConstraint() -{ - //Parent class FixedConstraint already destruct : pointHandler and data -} - -template -void PartialFixedConstraint::reinit() -{ - this->Inherited::reinit(); -} - - -template -template -void PartialFixedConstraint::projectResponseT(DataDeriv& res, - const std::function& clear) -{ - const VecBool& blockedDirection = d_fixedDirections.getValue(); - - if (this->d_fixAll.getValue() == true) - { - // fix everything - for( std::size_t i=0; id_indices.getValue(); - for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) - { - clear(res, *it, blockedDirection); - } - } -} - -template -void PartialFixedConstraint::projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& resData) -{ - SOFA_UNUSED(mparams); - helper::WriteAccessor res = resData; - projectResponseT(res.wref(), - [](VecDeriv& dx, const unsigned int index, const VecBool& b) - { - for (std::size_t j = 0; j < b.size(); j++) if (b[j]) dx[index][j] = 0.0; - } - ); -} - -// projectVelocity applies the same changes on velocity vector as projectResponse on position vector : -// Each fixed point received a null velocity vector. -// When a new fixed point is added while its velocity vector is already null, projectVelocity is not usefull. -// But when a new fixed point is added while its velocity vector is not null, it's necessary to fix it to null or -// to set the projectVelocity option to True. If not, the fixed point is going to drift. -template -void PartialFixedConstraint::projectVelocity(const core::MechanicalParams* mparams, DataVecDeriv& vData) -{ - SOFA_UNUSED(mparams); - - if(!d_projectVelocity.getValue()) return; - - const VecBool& blockedDirection = d_fixedDirections.getValue(); - helper::WriteAccessor res = vData; - - if ( this->d_fixAll.getValue() ) - { - // fix everyting - for (Size i = 0; i < res.size(); i++) - { - for (unsigned int c = 0; c < NumDimensions; ++c) - { - if (blockedDirection[c]) res[i][c] = 0; - } - } - } - else - { - const SetIndexArray & indices = this->d_indices.getValue(); - for(Index ind : indices) - { - for (unsigned int c = 0; c < NumDimensions; ++c) - { - if (blockedDirection[c]) - res[ind][c] = 0; - } - } - } -} - - -template -void PartialFixedConstraint::projectJacobianMatrix(const core::MechanicalParams* mparams, DataMatrixDeriv& cData) -{ - SOFA_UNUSED(mparams); - helper::WriteAccessor c = cData; - - projectResponseT(c.wref(), - [](MatrixDeriv& res, const unsigned int index, const VecBool& btype) - { - auto itRow = res.begin(); - auto itRowEnd = res.end(); - - while (itRow != itRowEnd) - { - for (auto colIt = itRow.begin(); colIt != itRow.end(); colIt++) - { - if (index == (unsigned int)colIt.index()) - { - Deriv b = colIt.val(); - for (unsigned int j = 0; j < btype.size(); j++) - if (btype[j]) b[j] = 0.0; - res.writeLine(itRow.index()).setCol(colIt.index(), b); - } - } - ++itRow; - } - }); -} - -template -void PartialFixedConstraint::applyConstraint(const core::MechanicalParams* mparams, linearalgebra::BaseVector* vector, const sofa::core::behavior::MultiMatrixAccessor* matrix) -{ - SOFA_UNUSED(mparams); - const int o = matrix->getGlobalOffset(this->mstate.get()); - if (o >= 0) - { - const unsigned int offset = (unsigned int)o; - const unsigned int N = Deriv::size(); - - const VecBool& blockedDirection = d_fixedDirections.getValue(); - - if( this->d_fixAll.getValue() ) - { - for(sofa::Index i=0; i<(sofa::Size) vector->size(); i++ ) - { - for (unsigned int c = 0; c < N; ++c) - { - if (blockedDirection[c]) - { - vector->clear(offset + N * i + c); - } - } - } - } - else - { - const SetIndexArray & indices = this->d_indices.getValue(); - for (const unsigned int index : indices) - { - for (unsigned int c = 0; c < N; ++c) - { - if (blockedDirection[c]) - { - vector->clear(offset + N * index + c); - } - } - } - } - } -} - -template -void PartialFixedConstraint::applyConstraint(const core::MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) -{ - SOFA_UNUSED(mparams); - if(const core::behavior::MultiMatrixAccessor::MatrixRef r = matrix->getMatrix(this->mstate.get())) - { - const unsigned int N = Deriv::size(); - const VecBool& blockedDirection = d_fixedDirections.getValue(); - const SetIndexArray & indices = this->d_indices.getValue(); - - if( this->d_fixAll.getValue() ) - { - const unsigned size = this->mstate->getSize(); - for(unsigned int i=0; iclearRowCol(r.offset + N * i + c); - } - } - // Set Fixed Vertex - for (unsigned int c=0; cset(r.offset + N * i + c, r.offset + N * i + c, 1.0); - } - } - } - } - else - { - for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) - { - // Reset Fixed Row and Col - for (unsigned int c=0; cclearRowCol(r.offset + N * (*it) + c); - } - } - // Set Fixed Vertex - for (unsigned int c=0; cset(r.offset + N * (*it) + c, r.offset + N * (*it) + c, 1.0); - } - } - } - } - } -} - -template -void PartialFixedConstraint::projectMatrix( sofa::linearalgebra::BaseMatrix* M, unsigned offset ) -{ - static const unsigned blockSize = DataTypes::deriv_total_size; - - const VecBool& blockedDirection = d_fixedDirections.getValue(); - - if( this->d_fixAll.getValue() ) - { - const unsigned size = this->mstate->getSize(); - for( unsigned i=0; iclearRowCol( offset + i * blockSize + c ); - } - } - } - } - else - { - const SetIndexArray & indices = this->d_indices.getValue(); - for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) - { - for (unsigned int c = 0; c < blockSize; ++c) - { - if (blockedDirection[c]) - { - M->clearRowCol( offset + (*it) * blockSize + c); - } - } - } - } -} - -template -void PartialFixedConstraint::applyConstraint( - sofa::core::behavior::ZeroDirichletCondition* matrix) -{ - static constexpr unsigned int N = Deriv::size(); - const VecBool& blockedDirection = d_fixedDirections.getValue(); - - if( this->d_fixAll.getValue() ) - { - const sofa::Size size = this->mstate->getSize(); - - for(sofa::Index i = 0; i < size; ++i) - { - for (unsigned int c=0; cdiscardRowCol(N * i + c, N * i + c); - } - } - } - } - else - { - const SetIndexArray & indices = this->d_indices.getValue(); - - for (const auto index : indices) - { - for (unsigned int c = 0; c < N; ++c) - { - if (blockedDirection[c]) - { - matrix->discardRowCol(N * index + c, N * index + c); - } - } - } - } -} -} // namespace sofa::component::constraint::projective +SOFA_DEPRECATED_HEADER("v24.06", "v25.06", "sofa/component/constraint/projective/PartialFixedProjectiveConstraint.inl") diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PartialFixedConstraint.cpp b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PartialFixedProjectiveConstraint.cpp similarity index 71% rename from Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PartialFixedConstraint.cpp rename to Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PartialFixedProjectiveConstraint.cpp index d28aae72e24..663c048c8d2 100644 --- a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PartialFixedConstraint.cpp +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PartialFixedProjectiveConstraint.cpp @@ -19,8 +19,8 @@ * * * Contact information: contact@sofa-framework.org * ******************************************************************************/ -#define SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_PARTIALFIXEDCONSTRAINT_CPP -#include +#define SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_PARTIALFIXEDPROJECTIVECONSTRAINT_CPP +#include #include #include @@ -31,21 +31,22 @@ using namespace sofa::defaulttype; using namespace sofa::helper; -int PartialFixedConstraintClass = core::RegisterObject("Attach given particles to their initial positions") - .add< PartialFixedConstraint >() - .add< PartialFixedConstraint >() - .add< PartialFixedConstraint >() - .add< PartialFixedConstraint >() - .add< PartialFixedConstraint >() - .add< PartialFixedConstraint >() +int PartialFixedProjectiveConstraintClass = core::RegisterObject("Attach given particles to their initial positions") + .add< PartialFixedProjectiveConstraint >() + .add< PartialFixedProjectiveConstraint >() + .add< PartialFixedProjectiveConstraint >() + .add< PartialFixedProjectiveConstraint >() + .add< PartialFixedProjectiveConstraint >() + .add< PartialFixedProjectiveConstraint >() + .addAlias("PartialFixedConstraint") ; -template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PartialFixedConstraint; -template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PartialFixedConstraint; -template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PartialFixedConstraint; -template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PartialFixedConstraint; -template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PartialFixedConstraint; -template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PartialFixedConstraint; +template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PartialFixedProjectiveConstraint; +template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PartialFixedProjectiveConstraint; +template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PartialFixedProjectiveConstraint; +template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PartialFixedProjectiveConstraint; +template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PartialFixedProjectiveConstraint; +template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PartialFixedProjectiveConstraint; } // namespace sofa::component::constraint::projective diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PartialFixedProjectiveConstraint.h b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PartialFixedProjectiveConstraint.h new file mode 100644 index 00000000000..89f347b303d --- /dev/null +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PartialFixedProjectiveConstraint.h @@ -0,0 +1,105 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace sofa::component::constraint::projective +{ + +/** + * Attach given particles to their initial positions, in some directions only. + * The fixed and free directioons are the same for all the particles, defined in the fixedDirections attribute. + **/ +template +class PartialFixedProjectiveConstraint : public FixedProjectiveConstraint +{ +public: + SOFA_CLASS(SOFA_TEMPLATE(PartialFixedProjectiveConstraint,DataTypes),SOFA_TEMPLATE(FixedProjectiveConstraint, DataTypes)); + + typedef FixedProjectiveConstraint Inherited; + typedef typename DataTypes::VecCoord VecCoord; + typedef typename DataTypes::VecDeriv VecDeriv; + typedef typename DataTypes::MatrixDeriv MatrixDeriv; + typedef typename DataTypes::Coord Coord; + typedef typename DataTypes::Deriv Deriv; + typedef typename DataTypes::Real Real; + typedef typename MatrixDeriv::RowIterator MatrixDerivRowIterator; + typedef typename MatrixDeriv::RowType MatrixDerivRowType; + typedef Data DataVecCoord; + typedef Data DataVecDeriv; + typedef Data DataMatrixDeriv; + typedef type::vector SetIndexArray; + typedef sofa::core::topology::TopologySubsetIndices SetIndex; + +public: + enum { NumDimensions = Deriv::total_size }; + typedef sofa::type::fixed_array VecBool; + Data d_fixedDirections; ///< Defines the directions in which the particles are fixed: true (or 1) for fixed, false (or 0) for free. + Data d_projectVelocity; ///< activate project velocity to set velocity to zero + +protected: + PartialFixedProjectiveConstraint(); + virtual ~PartialFixedProjectiveConstraint(); + +public: + + // -- Constraint interface + void reinit() override; + + void projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& resData) override; + void projectVelocity(const core::MechanicalParams* mparams, DataVecDeriv& vData) override; + void projectJacobianMatrix(const core::MechanicalParams* mparams, DataMatrixDeriv& cData) override; + + void applyConstraint(const core::MechanicalParams* mparams, linearalgebra::BaseVector* vector, const sofa::core::behavior::MultiMatrixAccessor* matrix) override; + void applyConstraint(const core::MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) override; + + void projectMatrix( sofa::linearalgebra::BaseMatrix* /*M*/, unsigned /*offset*/ ) override; + + void applyConstraint(sofa::core::behavior::ZeroDirichletCondition* matrix) override; + +protected: + template + void projectResponseT(DataDeriv& dx, + const std::function& clear); +}; + +#if !defined(SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_PARTIALFIXEDPROJECTIVECONSTRAINT_CPP) +extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PartialFixedProjectiveConstraint; +extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PartialFixedProjectiveConstraint; +extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PartialFixedProjectiveConstraint; +extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PartialFixedProjectiveConstraint; +extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PartialFixedProjectiveConstraint; +extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PartialFixedProjectiveConstraint; +#endif + +} // namespace sofa::component::constraint::projective diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PartialFixedProjectiveConstraint.inl b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PartialFixedProjectiveConstraint.inl new file mode 100644 index 00000000000..3c9b090d771 --- /dev/null +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PartialFixedProjectiveConstraint.inl @@ -0,0 +1,343 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once + +#include +#include +#include +#include +#include +#include + + +namespace sofa::component::constraint::projective +{ + +template +PartialFixedProjectiveConstraint::PartialFixedProjectiveConstraint() + : d_fixedDirections( initData(&d_fixedDirections,"fixedDirections","for each direction, 1 if fixed, 0 if free") ) + , d_projectVelocity(initData(&d_projectVelocity, false, "projectVelocity", "project velocity to ensure no drift of the fixed point")) +{ + VecBool blockedDirection; + for( unsigned i=0; i +PartialFixedProjectiveConstraint::~PartialFixedProjectiveConstraint() +{ + //Parent class FixedConstraint already destruct : pointHandler and data +} + +template +void PartialFixedProjectiveConstraint::reinit() +{ + this->Inherited::reinit(); +} + + +template +template +void PartialFixedProjectiveConstraint::projectResponseT(DataDeriv& res, + const std::function& clear) +{ + const VecBool& blockedDirection = d_fixedDirections.getValue(); + + if (this->d_fixAll.getValue() == true) + { + // fix everything + for( std::size_t i=0; id_indices.getValue(); + for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) + { + clear(res, *it, blockedDirection); + } + } +} + +template +void PartialFixedProjectiveConstraint::projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& resData) +{ + SOFA_UNUSED(mparams); + helper::WriteAccessor res = resData; + projectResponseT(res.wref(), + [](VecDeriv& dx, const unsigned int index, const VecBool& b) + { + for (std::size_t j = 0; j < b.size(); j++) if (b[j]) dx[index][j] = 0.0; + } + ); +} + +// projectVelocity applies the same changes on velocity vector as projectResponse on position vector : +// Each fixed point received a null velocity vector. +// When a new fixed point is added while its velocity vector is already null, projectVelocity is not usefull. +// But when a new fixed point is added while its velocity vector is not null, it's necessary to fix it to null or +// to set the projectVelocity option to True. If not, the fixed point is going to drift. +template +void PartialFixedProjectiveConstraint::projectVelocity(const core::MechanicalParams* mparams, DataVecDeriv& vData) +{ + SOFA_UNUSED(mparams); + + if(!d_projectVelocity.getValue()) return; + + const VecBool& blockedDirection = d_fixedDirections.getValue(); + helper::WriteAccessor res = vData; + + if ( this->d_fixAll.getValue() ) + { + // fix everyting + for (Size i = 0; i < res.size(); i++) + { + for (unsigned int c = 0; c < NumDimensions; ++c) + { + if (blockedDirection[c]) res[i][c] = 0; + } + } + } + else + { + const SetIndexArray & indices = this->d_indices.getValue(); + for(Index ind : indices) + { + for (unsigned int c = 0; c < NumDimensions; ++c) + { + if (blockedDirection[c]) + res[ind][c] = 0; + } + } + } +} + + +template +void PartialFixedProjectiveConstraint::projectJacobianMatrix(const core::MechanicalParams* mparams, DataMatrixDeriv& cData) +{ + SOFA_UNUSED(mparams); + helper::WriteAccessor c = cData; + + projectResponseT(c.wref(), + [](MatrixDeriv& res, const unsigned int index, const VecBool& btype) + { + auto itRow = res.begin(); + auto itRowEnd = res.end(); + + while (itRow != itRowEnd) + { + for (auto colIt = itRow.begin(); colIt != itRow.end(); colIt++) + { + if (index == (unsigned int)colIt.index()) + { + Deriv b = colIt.val(); + for (unsigned int j = 0; j < btype.size(); j++) + if (btype[j]) b[j] = 0.0; + res.writeLine(itRow.index()).setCol(colIt.index(), b); + } + } + ++itRow; + } + }); +} + +template +void PartialFixedProjectiveConstraint::applyConstraint(const core::MechanicalParams* mparams, linearalgebra::BaseVector* vector, const sofa::core::behavior::MultiMatrixAccessor* matrix) +{ + SOFA_UNUSED(mparams); + const int o = matrix->getGlobalOffset(this->mstate.get()); + if (o >= 0) + { + const unsigned int offset = (unsigned int)o; + const unsigned int N = Deriv::size(); + + const VecBool& blockedDirection = d_fixedDirections.getValue(); + + if( this->d_fixAll.getValue() ) + { + for(sofa::Index i=0; i<(sofa::Size) vector->size(); i++ ) + { + for (unsigned int c = 0; c < N; ++c) + { + if (blockedDirection[c]) + { + vector->clear(offset + N * i + c); + } + } + } + } + else + { + const SetIndexArray & indices = this->d_indices.getValue(); + for (const unsigned int index : indices) + { + for (unsigned int c = 0; c < N; ++c) + { + if (blockedDirection[c]) + { + vector->clear(offset + N * index + c); + } + } + } + } + } +} + +template +void PartialFixedProjectiveConstraint::applyConstraint(const core::MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) +{ + SOFA_UNUSED(mparams); + if(const core::behavior::MultiMatrixAccessor::MatrixRef r = matrix->getMatrix(this->mstate.get())) + { + const unsigned int N = Deriv::size(); + const VecBool& blockedDirection = d_fixedDirections.getValue(); + const SetIndexArray & indices = this->d_indices.getValue(); + + if( this->d_fixAll.getValue() ) + { + const unsigned size = this->mstate->getSize(); + for(unsigned int i=0; iclearRowCol(r.offset + N * i + c); + } + } + // Set Fixed Vertex + for (unsigned int c=0; cset(r.offset + N * i + c, r.offset + N * i + c, 1.0); + } + } + } + } + else + { + for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) + { + // Reset Fixed Row and Col + for (unsigned int c=0; cclearRowCol(r.offset + N * (*it) + c); + } + } + // Set Fixed Vertex + for (unsigned int c=0; cset(r.offset + N * (*it) + c, r.offset + N * (*it) + c, 1.0); + } + } + } + } + } +} + +template +void PartialFixedProjectiveConstraint::projectMatrix( sofa::linearalgebra::BaseMatrix* M, unsigned offset ) +{ + static const unsigned blockSize = DataTypes::deriv_total_size; + + const VecBool& blockedDirection = d_fixedDirections.getValue(); + + if( this->d_fixAll.getValue() ) + { + const unsigned size = this->mstate->getSize(); + for( unsigned i=0; iclearRowCol( offset + i * blockSize + c ); + } + } + } + } + else + { + const SetIndexArray & indices = this->d_indices.getValue(); + for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) + { + for (unsigned int c = 0; c < blockSize; ++c) + { + if (blockedDirection[c]) + { + M->clearRowCol( offset + (*it) * blockSize + c); + } + } + } + } +} + +template +void PartialFixedProjectiveConstraint::applyConstraint( + sofa::core::behavior::ZeroDirichletCondition* matrix) +{ + static constexpr unsigned int N = Deriv::size(); + const VecBool& blockedDirection = d_fixedDirections.getValue(); + + if( this->d_fixAll.getValue() ) + { + const sofa::Size size = this->mstate->getSize(); + + for(sofa::Index i = 0; i < size; ++i) + { + for (unsigned int c=0; cdiscardRowCol(N * i + c, N * i + c); + } + } + } + } + else + { + const SetIndexArray & indices = this->d_indices.getValue(); + + for (const auto index : indices) + { + for (unsigned int c = 0; c < N; ++c) + { + if (blockedDirection[c]) + { + matrix->discardRowCol(N * index + c, N * index + c); + } + } + } + } +} +} // namespace sofa::component::constraint::projective diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PartialLinearMovementConstraint.h b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PartialLinearMovementConstraint.h index 072548a5030..f3eb83d8868 100644 --- a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PartialLinearMovementConstraint.h +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PartialLinearMovementConstraint.h @@ -20,157 +20,13 @@ * Contact information: contact@sofa-framework.org * ******************************************************************************/ #pragma once -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include -namespace sofa::component::constraint::projective -{ - -template -class PartialLinearMovementConstraintInternalData -{ -}; +SOFA_DEPRECATED_HEADER("v24.06", "v25.06", "sofa/component/constraint/projective/PartialLinearMovementProjectiveConstraint.h") -/** impose a motion to given DOFs (translation and rotation) in some directions only. - The moved and free directioons are the same for all the particles, defined in the movedDirections attribute. - The motion between 2 key times is linearly interpolated -*/ -template -class PartialLinearMovementConstraint : public core::behavior::ProjectiveConstraintSet +namespace sofa::component::constraint::projective { -public: - SOFA_CLASS(SOFA_TEMPLATE(PartialLinearMovementConstraint,TDataTypes),SOFA_TEMPLATE(sofa::core::behavior::ProjectiveConstraintSet, TDataTypes)); - - using Index = sofa::Index; - typedef TDataTypes DataTypes; - typedef typename DataTypes::VecCoord VecCoord; - typedef typename DataTypes::VecDeriv VecDeriv; - typedef typename DataTypes::MatrixDeriv MatrixDeriv; - typedef typename DataTypes::Coord Coord; - typedef typename DataTypes::Deriv Deriv; - typedef typename DataTypes::Real Real; - typedef typename MatrixDeriv::RowIterator MatrixDerivRowIterator; - typedef typename MatrixDeriv::RowType MatrixDerivRowType; - typedef Data DataVecCoord; - typedef Data DataVecDeriv; - typedef Data DataMatrixDeriv; - typedef type::vector SetIndexArray; - typedef sofa::core::topology::TopologySubsetIndices SetIndex; - -protected: - PartialLinearMovementConstraintInternalData *data; - friend class PartialLinearMovementConstraintInternalData; - -public : - /// indices of the DOFs the constraint is applied to - SetIndex m_indices; - /// the key frames when the motion is defined by the user - core::objectmodel::Data > m_keyTimes; - /// the motions corresponding to the key frames - core::objectmodel::Data m_keyMovements; - - /// attributes to precise display - /// if showMovement is true we display the expected movement - /// otherwise we show which are the fixed dofs - core::objectmodel::Data< bool > showMovement; - - /// the key times surrounding the current simulation time (for interpolation) - Real prevT, nextT; - ///the motions corresponding to the surrouding key times - Deriv prevM, nextM; - ///initial constrained DOFs position - VecCoord x0; - - core::objectmodel::Data linearMovementBetweenNodesInIndices; ///< Take into account the linear movement between the constrained points - core::objectmodel::Data mainIndice; ///< The main indice node in the list of constrained nodes, it defines how to apply the linear movement between this constrained nodes - core::objectmodel::Data minDepIndice; ///< The indice node in the list of constrained nodes, which is imposed the minimum displacment - core::objectmodel::Data maxDepIndice; ///< The indice node in the list of constrained nodes, which is imposed the maximum displacment - core::objectmodel::Data > m_imposedDisplacmentOnMacroNodes; ///< imposed displacement at u1 u2 u3 u4 for 2d case - ///< and u1 u2 u3 u4 u5 u6 u7 u8 for 3d case - Data X0; ///< Size of specimen in X-direction - Data Y0; ///< Size of specimen in Y-direction - Data Z0; ///< Size of specimen in Z-direction - - enum { NumDimensions = Deriv::total_size }; - typedef sofa::type::fixed_array VecBool; - core::objectmodel::Data movedDirections; ///< Defines the directions in which the particles are moved: true (or 1) for fixed, false (or 0) for free. - - /// Link to be set to the topology container in the component graph. - SingleLink, sofa::core::topology::BaseMeshTopology, BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_topology; - -protected: - PartialLinearMovementConstraint(); - ~PartialLinearMovementConstraint() override; - -public: - ///methods to add/remove some indices, keyTimes, keyMovement - void clearIndices(); - void addIndex(Index index); - void removeIndex(Index index); - void clearKeyMovements(); - - ///@brief Add a new key movement - ///@param time : the simulation time you want to set a movement (in sec) - ///@param movement : the corresponding motion - ///for instance, addKeyMovement(1.0, Deriv(5,0,0) ) will set a translation of 5 in x direction a time 1.0s - void addKeyMovement(Real time, Deriv movement); - - - /// -- Constraint interface - void init() override; - void reset() override; - - void projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& resData) override; - void projectVelocity(const core::MechanicalParams* mparams, DataVecDeriv& vData) override; - void projectPosition(const core::MechanicalParams* mparams, DataVecCoord& xData) override; - void projectJacobianMatrix(const core::MechanicalParams* mparams, DataMatrixDeriv& cData) override; - - void applyConstraint(const core::MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) override; - void applyConstraint(const core::MechanicalParams* mparams, linearalgebra::BaseVector* vector, const sofa::core::behavior::MultiMatrixAccessor* matrix) override; - - void draw(const core::visual::VisualParams*) override; - -protected: - template - void projectResponseT(DataDeriv& dx, - const std::function& clear); - - template - void interpolatePosition(Real cT, typename std::enable_if >::value, VecCoord>::type& x); - template - void interpolatePosition(Real cT, typename std::enable_if >::value, VecCoord>::type& x); - -private: - - /// to keep the time corresponding to the key times - Real currentTime; - - /// to know if we found the key times - bool finished; - - /// find previous and next time keys - void findKeyTimes(); -}; - - -#if !defined(SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_PARTIALLINEARMOVEMENTCONSTRAINT_CPP) -extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PartialLinearMovementConstraint; -extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PartialLinearMovementConstraint; -extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PartialLinearMovementConstraint; -extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PartialLinearMovementConstraint; -extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PartialLinearMovementConstraint; -#endif - -} // namespace sofa::component::constraint::projective +template +using PartialLinearMovementConstraint SOFA_ATTRIBUTE_DEPRECATED("v24.06 ", "v25.06", "PartialLinearMovementConstraint has been renamed to PartialLinearMovementProjectiveConstraint") = PartialLinearMovementProjectiveConstraint; +} \ No newline at end of file diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PartialLinearMovementConstraint.inl b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PartialLinearMovementConstraint.inl index 08bd3e7df9d..4d212e34cae 100644 --- a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PartialLinearMovementConstraint.inl +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PartialLinearMovementConstraint.inl @@ -21,487 +21,6 @@ ******************************************************************************/ #pragma once -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include - -namespace sofa::component::constraint::projective -{ - -template -PartialLinearMovementConstraint::PartialLinearMovementConstraint() - : core::behavior::ProjectiveConstraintSet(nullptr) - , data(new PartialLinearMovementConstraintInternalData) - , m_indices( initData(&m_indices,"indices","Indices of the constrained points") ) - , m_keyTimes( initData(&m_keyTimes,"keyTimes","key times for the movements") ) - , m_keyMovements( initData(&m_keyMovements,"movements","movements corresponding to the key times") ) - , showMovement( initData(&showMovement, (bool)false, "showMovement", "Visualization of the movement to be applied to constrained dofs.")) - , linearMovementBetweenNodesInIndices( initData(&linearMovementBetweenNodesInIndices, (bool)false, "linearMovementBetweenNodesInIndices", "Take into account the linear movement between the constrained points")) - , mainIndice( initData(&mainIndice, "mainIndice", "The main indice node in the list of constrained nodes, it defines how to apply the linear movement between this constrained nodes ")) - , minDepIndice( initData(&minDepIndice, "minDepIndice", "The indice node in the list of constrained nodes, which is imposed the minimum displacment ")) - , maxDepIndice( initData(&maxDepIndice, "maxDepIndice", "The indice node in the list of constrained nodes, which is imposed the maximum displacment ")) - , m_imposedDisplacmentOnMacroNodes( initData(&m_imposedDisplacmentOnMacroNodes,"imposedDisplacmentOnMacroNodes","The imposed displacment on macro nodes") ) - , X0 ( initData ( &X0, Real(0.0),"X0","Size of specimen in X-direction" ) ) - , Y0 ( initData ( &Y0, Real(0.0),"Y0","Size of specimen in Y-direction" ) ) - , Z0 ( initData ( &Z0, Real(0.0),"Z0","Size of specimen in Z-direction" ) ) - , movedDirections( initData(&movedDirections,"movedDirections","for each direction, 1 if moved, 0 if free") ) - , l_topology(initLink("topology", "link to the topology container")) - , finished(false) -{ - // default to indice 0 - m_indices.beginEdit()->push_back(0); - m_indices.endEdit(); - - //default valueEvent to 0 - m_keyTimes.beginEdit()->push_back( 0.0 ); - m_keyTimes.endEdit(); - m_keyMovements.beginEdit()->push_back( Deriv() ); - m_keyMovements.endEdit(); - VecBool movedDirection; - for( unsigned i=0; i -PartialLinearMovementConstraint::~PartialLinearMovementConstraint() -{ - -} - -template -void PartialLinearMovementConstraint::clearIndices() -{ - m_indices.beginEdit()->clear(); - m_indices.endEdit(); -} - -template -void PartialLinearMovementConstraint::addIndex(Index index) -{ - m_indices.beginEdit()->push_back(index); - m_indices.endEdit(); -} - -template -void PartialLinearMovementConstraint::removeIndex(Index index) -{ - sofa::type::removeValue(*m_indices.beginEdit(),index); - m_indices.endEdit(); -} - -template -void PartialLinearMovementConstraint::clearKeyMovements() -{ - m_keyTimes.beginEdit()->clear(); - m_keyTimes.endEdit(); - m_keyMovements.beginEdit()->clear(); - m_keyMovements.endEdit(); -} - -template -void PartialLinearMovementConstraint::addKeyMovement(Real time, Deriv movement) -{ - m_keyTimes.beginEdit()->push_back( time ); - m_keyTimes.endEdit(); - m_keyMovements.beginEdit()->push_back( movement ); - m_keyMovements.endEdit(); -} - -// -- Constraint interface - - -template -void PartialLinearMovementConstraint::init() -{ - this->core::behavior::ProjectiveConstraintSet::init(); - - if (l_topology.empty()) - { - msg_info() << "link to Topology container should be set to ensure right behavior. First Topology found in current context will be used."; - l_topology.set(this->getContext()->getMeshTopologyLink()); - } - - if (sofa::core::topology::BaseMeshTopology* _topology = l_topology.get()) - { - msg_info() << "Topology path used: '" << l_topology.getLinkedPath() << "'"; - - // Initialize topological changes support - m_indices.createTopologyHandler(_topology); - } - else - { - msg_info() << "No topology component found at path: " << l_topology.getLinkedPath() << ", nor in current context: " << this->getContext()->name; - } - - x0.resize(0); - nextM = prevM = Deriv(); - - currentTime = -1.0; - finished = false; -} - - -template -void PartialLinearMovementConstraint::reset() -{ - nextT = prevT = 0.0; - nextM = prevM = Deriv(); - - currentTime = -1.0; - finished = false; -} - - -template -template -void PartialLinearMovementConstraint::projectResponseT(DataDeriv& dx, - const std::function& clear) -{ - Real cT = (Real) this->getContext()->getTime(); - VecBool movedDirection = movedDirections.getValue(); - if ((cT != currentTime) || !finished) - { - findKeyTimes(); - } - - if (finished && nextT != prevT) - { - const SetIndexArray & indices = m_indices.getValue(); - - //set the motion to the Dofs - for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) - { - clear(dx, *it, movedDirection); - } - } -} - -template -void PartialLinearMovementConstraint::projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& resData) -{ - SOFA_UNUSED(mparams); - helper::WriteAccessor res = resData; - projectResponseT(res.wref(), [](VecDeriv& dx, const unsigned int index, const VecBool& b) - { for (unsigned j = 0; j < b.size(); j++) if (b[j]) dx[index][j] = 0.0; }); -} - -template -void PartialLinearMovementConstraint::projectVelocity(const core::MechanicalParams* /*mparams*/, DataVecDeriv& vData) -{ - helper::WriteAccessor dx = vData; - Real cT = (Real) this->getContext()->getTime(); - if ((cT != currentTime) || !finished) - { - findKeyTimes(); - } - - if (finished && nextT != prevT) - { - const SetIndexArray & indices = m_indices.getValue(); - - //set the motion to the Dofs - for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) - { - dx[*it] = (nextM - prevM)*(1.0 / (nextT - prevT)); - } - } -} - - -template -void PartialLinearMovementConstraint::projectPosition(const core::MechanicalParams* /*mparams*/, DataVecCoord& xData) -{ - helper::WriteAccessor x = xData; - Real cT = (Real) this->getContext()->getTime(); - - //initialize initial Dofs positions, if it's not done - if (x0.size() == 0) - { - const SetIndexArray & indices = m_indices.getValue(); - x0.resize(x.size()); - for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) - { - x0[*it] = x[*it]; - } - } - - if ((cT != currentTime) || !finished) - { - findKeyTimes(); - } - - //if we found 2 keyTimes, we have to interpolate a velocity (linear interpolation) - if(finished && nextT != prevT) - { - interpolatePosition(cT, x.wref()); - } -} - -template -template -void PartialLinearMovementConstraint::interpolatePosition(Real cT, typename std::enable_if >::value, VecCoord>::type& x) -{ - const SetIndexArray & indices = m_indices.getValue(); - Real dt = (cT - prevT) / (nextT - prevT); - Deriv m = prevM + (nextM-prevM)*dt; - VecBool movedDirection = movedDirections.getValue(); - //set the motion to the Dofs - if(linearMovementBetweenNodesInIndices.getValue()) - { - - const type::vector &imposedDisplacmentOnMacroNodes = this->m_imposedDisplacmentOnMacroNodes.getValue(); - Real a = X0.getValue(); - Real b = Y0.getValue(); - Real c = Z0.getValue(); - bool case2d=false; - if((a==0.0)||(b==0.0)||(c==0.0)) case2d=true; - if(a==0.0) {a=b; b=c;} - if(b==0.0) {b=c;} - - for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) - { - for( unsigned j=0; j< NumDimensions; j++) - { - if(movedDirection[j]) - { - if(case2d) - { - x[*it][j] = x0[*it][j] + ((Real)1.0/(a*b))*((a-x0[*it][0])*(b-x0[*it][1])*imposedDisplacmentOnMacroNodes[0]+ ///< N1 - x0[*it][0]*(b-x0[*it][1])*imposedDisplacmentOnMacroNodes[1]+ ///< N2 - x0[*it][0]*x0[*it][1]*imposedDisplacmentOnMacroNodes[2]+ ///< N3 - (a-x0[*it][0])*x0[*it][1]*imposedDisplacmentOnMacroNodes[3])*m[j]; ///< N4 - // 4|----------------|3 - // | | - // | | - // | | - // 1|----------------|2 - } - else ///< case3d - { - // |Y - // 5---------8 - // /| /| - // / | / | - // 6--|------7 | - // | |/ | | - // | 1------|--4--->X - // | / | / - // |/ |/ - // 2---------3 - // Z/ - // - - x[*it][j] = x0[*it][j] + ((Real)1.0/(a*b*c))*( - (a-x0[*it][0])*(b-x0[*it][1])*(c-x0[*it][2])*imposedDisplacmentOnMacroNodes[0]+ ///< N1 - (a-x0[*it][0])*(b-x0[*it][1])*x0[*it][2]*imposedDisplacmentOnMacroNodes[1]+ ///< N2 - x0[*it][0]*(b-x0[*it][1])*x0[*it][2]*imposedDisplacmentOnMacroNodes[2]+ ///< N3 - x0[*it][0]*(b-x0[*it][1])*(c-x0[*it][2])*imposedDisplacmentOnMacroNodes[3]+ ///< N4 - (a-x0[*it][0])*x0[*it][1]*(c-x0[*it][2])*imposedDisplacmentOnMacroNodes[4]+ ///< N5 - (a-x0[*it][0])*x0[*it][1]*x0[*it][2]*imposedDisplacmentOnMacroNodes[5]+ ///< N6 - x0[*it][0]*x0[*it][1]*x0[*it][2]*imposedDisplacmentOnMacroNodes[6]+ ///< N7 - x0[*it][0]*x0[*it][1]*(c-x0[*it][2])*imposedDisplacmentOnMacroNodes[7] ///< N8 - - )*m[j]; - - } - } - } - } - - } - else - { - for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) - { - for( unsigned j=0; j< NumDimensions; j++) - if(movedDirection[j]) x[*it][j] = x0[*it][j] + m[j] ; - } - } - -} - -template -template -void PartialLinearMovementConstraint::interpolatePosition(Real cT, typename std::enable_if >::value, VecCoord>::type& x) -{ - const SetIndexArray & indices = m_indices.getValue(); - - Real dt = (cT - prevT) / (nextT - prevT); - Deriv m = prevM + (nextM-prevM)*dt; - type::Quat prevOrientation = type::Quat::createQuaterFromEuler(getVOrientation(prevM)); - type::Quat nextOrientation = type::Quat::createQuaterFromEuler(getVOrientation(nextM)); - - //set the motion to the Dofs - for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) - { - x[*it].getCenter() = x0[*it].getCenter() + getVCenter(m) ; - x[*it].getOrientation() = x0[*it].getOrientation() * prevOrientation.slerp2(nextOrientation, dt); - } -} - -template -void PartialLinearMovementConstraint::projectJacobianMatrix(const core::MechanicalParams* mparams, DataMatrixDeriv& cData) -{ - SOFA_UNUSED(mparams); - helper::WriteAccessor c = cData; - projectResponseT(c.wref(), - [](MatrixDeriv& res, const unsigned int index, const VecBool& btype) - { - auto itRow = res.begin(); - auto itRowEnd = res.end(); - - while (itRow != itRowEnd) - { - for (auto colIt = itRow.begin(); colIt != itRow.end(); colIt++) - { - if (index == (unsigned int)colIt.index()) - { - Deriv b = colIt.val(); - for (unsigned int j = 0; j < btype.size(); j++) if (btype[j]) b[j] = 0.0; - res.writeLine(itRow.index()).setCol(colIt.index(), b); - } - } - } - }); -} - -template -void PartialLinearMovementConstraint::findKeyTimes() -{ - Real cT = (Real) this->getContext()->getTime(); - finished = false; - - if(m_keyTimes.getValue().size() != 0 && cT >= *m_keyTimes.getValue().begin() && cT <= *m_keyTimes.getValue().rbegin()) - { - nextT = *m_keyTimes.getValue().begin(); - prevT = nextT; - - typename type::vector::const_iterator it_t = m_keyTimes.getValue().begin(); - typename VecDeriv::const_iterator it_m = m_keyMovements.getValue().begin(); - - //WARNING : we consider that the key-events are in chronological order - //here we search between which keyTimes we are, to know which are the motion to interpolate - while( it_t != m_keyTimes.getValue().end() && !finished) - { - if( *it_t <= cT) - { - prevT = *it_t; - prevM = *it_m; - } - else - { - nextT = *it_t; - nextM = *it_m; - finished = true; - } - ++it_t; - ++it_m; - } - } -} - -// Matrix Integration interface -template -void PartialLinearMovementConstraint::applyConstraint(const core::MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) -{ - SOFA_UNUSED(mparams); - const SetIndexArray & indices = m_indices.getValue(); - - if (core::behavior::MultiMatrixAccessor::MatrixRef r = matrix->getMatrix(this->mstate.get())) - { - VecBool movedDirection = movedDirections.getValue(); - for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) - { - // Reset Fixed Row and Col - for (unsigned int c=0; cclearRowCol(r.offset + NumDimensions * (*it) + c); - } - // Set Fixed Vertex - for (unsigned int c=0; cset(r.offset + NumDimensions * (*it) + c, r.offset + NumDimensions * (*it) + c, 1.0); - } - } - } -} - -template -void PartialLinearMovementConstraint::applyConstraint(const core::MechanicalParams* mparams, linearalgebra::BaseVector* vector, const sofa::core::behavior::MultiMatrixAccessor* matrix) -{ - SOFA_UNUSED(mparams); - const int o = matrix->getGlobalOffset(this->mstate.get()); - if (o >= 0) { - unsigned int offset = (unsigned int)o; - VecBool movedDirection = movedDirections.getValue(); - const SetIndexArray & indices = m_indices.getValue(); - for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) - { - for (unsigned int c = 0; c < NumDimensions; ++c) - { - if (movedDirection[c]) - { - vector->clear(offset + NumDimensions * (*it) + c); - } - } - } - } -} - -//display the path the constrained dofs will go through -template -void PartialLinearMovementConstraint::draw(const core::visual::VisualParams* vparams) -{ - const auto stateLifeCycle = vparams->drawTool()->makeStateLifeCycle(); - - if (!vparams->displayFlags().getShowBehaviorModels() || m_keyTimes.getValue().size() == 0) - return; - - sofa::type::vector vertices; - constexpr sofa::type::RGBAColor color(1, 0.5, 0.5, 1); - - if (showMovement.getValue()) - { - vparams->drawTool()->disableLighting(); - - const SetIndexArray & indices = m_indices.getValue(); - const VecDeriv& keyMovements = m_keyMovements.getValue(); - for (unsigned int i = 0; i < keyMovements.size() - 1; i++) - { - for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) - { - const type::Vec3 v0 { DataTypes::getCPos(x0[*it]) + DataTypes::getDPos(keyMovements[i]) }; - const type::Vec3 v1 { DataTypes::getCPos(x0[*it]) + DataTypes::getDPos(keyMovements[i + 1]) }; - - vertices.push_back(v0); - vertices.push_back(v1); - } - } - vparams->drawTool()->drawLines(vertices, 1, color); - } - else - { - const VecCoord& x = this->mstate->read(core::ConstVecCoordId::position())->getValue(); - - type::Vec3 point; - const SetIndexArray & indices = m_indices.getValue(); - for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) - { - point = DataTypes::getCPos(x[*it]); - vertices.push_back(point); - } - vparams->drawTool()->drawPoints(vertices, 10, color); - } - - -} -} // namespace sofa::component::constraint::projective +SOFA_DEPRECATED_HEADER("v24.06", "v25.06", "sofa/component/constraint/projective/PartialLinearMovementProjectiveConstraint.inl") diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PartialLinearMovementConstraint.cpp b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PartialLinearMovementProjectiveConstraint.cpp similarity index 72% rename from Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PartialLinearMovementConstraint.cpp rename to Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PartialLinearMovementProjectiveConstraint.cpp index 694c399bc9c..3cb3e883968 100644 --- a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PartialLinearMovementConstraint.cpp +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PartialLinearMovementProjectiveConstraint.cpp @@ -19,8 +19,8 @@ * * * Contact information: contact@sofa-framework.org * ******************************************************************************/ -#define SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_PARTIALLINEARMOVEMENTCONSTRAINT_CPP -#include +#define SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_PARTIALLINEARMOVEMENTPROJECTIVECONSTRAINT_CPP +#include #include #include #include @@ -29,19 +29,20 @@ namespace sofa::component::constraint::projective { //declaration of the class, for the factory -int PartialLinearMovementConstraintClass = core::RegisterObject("translate given particles") - .add< PartialLinearMovementConstraint >() - .add< PartialLinearMovementConstraint >() - .add< PartialLinearMovementConstraint >() - .add< PartialLinearMovementConstraint >() - .add< PartialLinearMovementConstraint >() +int PartialLinearMovementProjectiveConstraintClass = core::RegisterObject("translate given particles") + .add< PartialLinearMovementProjectiveConstraint >() + .add< PartialLinearMovementProjectiveConstraint >() + .add< PartialLinearMovementProjectiveConstraint >() + .add< PartialLinearMovementProjectiveConstraint >() + .add< PartialLinearMovementProjectiveConstraint >() + .addAlias("PartialLinearMovementConstraint") ; -template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PartialLinearMovementConstraint; -template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PartialLinearMovementConstraint; -template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PartialLinearMovementConstraint; -template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PartialLinearMovementConstraint; -template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PartialLinearMovementConstraint; +template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PartialLinearMovementProjectiveConstraint; +template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PartialLinearMovementProjectiveConstraint; +template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PartialLinearMovementProjectiveConstraint; +template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PartialLinearMovementProjectiveConstraint; +template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PartialLinearMovementProjectiveConstraint; } // namespace sofa::component::constraint::projective diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PartialLinearMovementProjectiveConstraint.h b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PartialLinearMovementProjectiveConstraint.h new file mode 100644 index 00000000000..336f0bf2ce6 --- /dev/null +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PartialLinearMovementProjectiveConstraint.h @@ -0,0 +1,176 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace sofa::component::constraint::projective +{ + +template +class PartialLinearMovementProjectiveConstraintInternalData +{ +}; + +/** impose a motion to given DOFs (translation and rotation) in some directions only. + The moved and free directioons are the same for all the particles, defined in the movedDirections attribute. + The motion between 2 key times is linearly interpolated +*/ +template +class PartialLinearMovementProjectiveConstraint : public core::behavior::ProjectiveConstraintSet +{ +public: + SOFA_CLASS(SOFA_TEMPLATE(PartialLinearMovementProjectiveConstraint,TDataTypes),SOFA_TEMPLATE(sofa::core::behavior::ProjectiveConstraintSet, TDataTypes)); + + using Index = sofa::Index; + typedef TDataTypes DataTypes; + typedef typename DataTypes::VecCoord VecCoord; + typedef typename DataTypes::VecDeriv VecDeriv; + typedef typename DataTypes::MatrixDeriv MatrixDeriv; + typedef typename DataTypes::Coord Coord; + typedef typename DataTypes::Deriv Deriv; + typedef typename DataTypes::Real Real; + typedef typename MatrixDeriv::RowIterator MatrixDerivRowIterator; + typedef typename MatrixDeriv::RowType MatrixDerivRowType; + typedef Data DataVecCoord; + typedef Data DataVecDeriv; + typedef Data DataMatrixDeriv; + typedef type::vector SetIndexArray; + typedef sofa::core::topology::TopologySubsetIndices SetIndex; + +protected: + PartialLinearMovementProjectiveConstraintInternalData *data; + friend class PartialLinearMovementProjectiveConstraintInternalData; + +public : + /// indices of the DOFs the constraint is applied to + SetIndex m_indices; + /// the key frames when the motion is defined by the user + core::objectmodel::Data > m_keyTimes; + /// the motions corresponding to the key frames + core::objectmodel::Data m_keyMovements; + + /// attributes to precise display + /// if showMovement is true we display the expected movement + /// otherwise we show which are the fixed dofs + core::objectmodel::Data< bool > showMovement; + + /// the key times surrounding the current simulation time (for interpolation) + Real prevT, nextT; + ///the motions corresponding to the surrouding key times + Deriv prevM, nextM; + ///initial constrained DOFs position + VecCoord x0; + + core::objectmodel::Data linearMovementBetweenNodesInIndices; ///< Take into account the linear movement between the constrained points + core::objectmodel::Data mainIndice; ///< The main indice node in the list of constrained nodes, it defines how to apply the linear movement between this constrained nodes + core::objectmodel::Data minDepIndice; ///< The indice node in the list of constrained nodes, which is imposed the minimum displacment + core::objectmodel::Data maxDepIndice; ///< The indice node in the list of constrained nodes, which is imposed the maximum displacment + core::objectmodel::Data > m_imposedDisplacmentOnMacroNodes; ///< imposed displacement at u1 u2 u3 u4 for 2d case + ///< and u1 u2 u3 u4 u5 u6 u7 u8 for 3d case + Data X0; ///< Size of specimen in X-direction + Data Y0; ///< Size of specimen in Y-direction + Data Z0; ///< Size of specimen in Z-direction + + enum { NumDimensions = Deriv::total_size }; + typedef sofa::type::fixed_array VecBool; + core::objectmodel::Data movedDirections; ///< Defines the directions in which the particles are moved: true (or 1) for fixed, false (or 0) for free. + + /// Link to be set to the topology container in the component graph. + SingleLink, sofa::core::topology::BaseMeshTopology, BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_topology; + +protected: + PartialLinearMovementProjectiveConstraint(); + ~PartialLinearMovementProjectiveConstraint() override; + +public: + ///methods to add/remove some indices, keyTimes, keyMovement + void clearIndices(); + void addIndex(Index index); + void removeIndex(Index index); + void clearKeyMovements(); + + ///@brief Add a new key movement + ///@param time : the simulation time you want to set a movement (in sec) + ///@param movement : the corresponding motion + ///for instance, addKeyMovement(1.0, Deriv(5,0,0) ) will set a translation of 5 in x direction a time 1.0s + void addKeyMovement(Real time, Deriv movement); + + + /// -- Constraint interface + void init() override; + void reset() override; + + void projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& resData) override; + void projectVelocity(const core::MechanicalParams* mparams, DataVecDeriv& vData) override; + void projectPosition(const core::MechanicalParams* mparams, DataVecCoord& xData) override; + void projectJacobianMatrix(const core::MechanicalParams* mparams, DataMatrixDeriv& cData) override; + + void applyConstraint(const core::MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) override; + void applyConstraint(const core::MechanicalParams* mparams, linearalgebra::BaseVector* vector, const sofa::core::behavior::MultiMatrixAccessor* matrix) override; + + void draw(const core::visual::VisualParams*) override; + +protected: + template + void projectResponseT(DataDeriv& dx, + const std::function& clear); + + template + void interpolatePosition(Real cT, typename std::enable_if >::value, VecCoord>::type& x); + template + void interpolatePosition(Real cT, typename std::enable_if >::value, VecCoord>::type& x); + +private: + + /// to keep the time corresponding to the key times + Real currentTime; + + /// to know if we found the key times + bool finished; + + /// find previous and next time keys + void findKeyTimes(); +}; + + +#if !defined(SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_PARTIALLINEARMOVEMENTPROJECTIVECONSTRAINT_CPP) +extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PartialLinearMovementProjectiveConstraint; +extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PartialLinearMovementProjectiveConstraint; +extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PartialLinearMovementProjectiveConstraint; +extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PartialLinearMovementProjectiveConstraint; +extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PartialLinearMovementProjectiveConstraint; +#endif + +} // namespace sofa::component::constraint::projective diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PartialLinearMovementProjectiveConstraint.inl b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PartialLinearMovementProjectiveConstraint.inl new file mode 100644 index 00000000000..af02bb262d2 --- /dev/null +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PartialLinearMovementProjectiveConstraint.inl @@ -0,0 +1,507 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +namespace sofa::component::constraint::projective +{ + +template +PartialLinearMovementProjectiveConstraint::PartialLinearMovementProjectiveConstraint() + : core::behavior::ProjectiveConstraintSet(nullptr) + , data(new PartialLinearMovementProjectiveConstraintInternalData) + , m_indices( initData(&m_indices,"indices","Indices of the constrained points") ) + , m_keyTimes( initData(&m_keyTimes,"keyTimes","key times for the movements") ) + , m_keyMovements( initData(&m_keyMovements,"movements","movements corresponding to the key times") ) + , showMovement( initData(&showMovement, (bool)false, "showMovement", "Visualization of the movement to be applied to constrained dofs.")) + , linearMovementBetweenNodesInIndices( initData(&linearMovementBetweenNodesInIndices, (bool)false, "linearMovementBetweenNodesInIndices", "Take into account the linear movement between the constrained points")) + , mainIndice( initData(&mainIndice, "mainIndice", "The main indice node in the list of constrained nodes, it defines how to apply the linear movement between this constrained nodes ")) + , minDepIndice( initData(&minDepIndice, "minDepIndice", "The indice node in the list of constrained nodes, which is imposed the minimum displacment ")) + , maxDepIndice( initData(&maxDepIndice, "maxDepIndice", "The indice node in the list of constrained nodes, which is imposed the maximum displacment ")) + , m_imposedDisplacmentOnMacroNodes( initData(&m_imposedDisplacmentOnMacroNodes,"imposedDisplacmentOnMacroNodes","The imposed displacment on macro nodes") ) + , X0 ( initData ( &X0, Real(0.0),"X0","Size of specimen in X-direction" ) ) + , Y0 ( initData ( &Y0, Real(0.0),"Y0","Size of specimen in Y-direction" ) ) + , Z0 ( initData ( &Z0, Real(0.0),"Z0","Size of specimen in Z-direction" ) ) + , movedDirections( initData(&movedDirections,"movedDirections","for each direction, 1 if moved, 0 if free") ) + , l_topology(initLink("topology", "link to the topology container")) + , finished(false) +{ + // default to indice 0 + m_indices.beginEdit()->push_back(0); + m_indices.endEdit(); + + //default valueEvent to 0 + m_keyTimes.beginEdit()->push_back( 0.0 ); + m_keyTimes.endEdit(); + m_keyMovements.beginEdit()->push_back( Deriv() ); + m_keyMovements.endEdit(); + VecBool movedDirection; + for( unsigned i=0; i +PartialLinearMovementProjectiveConstraint::~PartialLinearMovementProjectiveConstraint() +{ + +} + +template +void PartialLinearMovementProjectiveConstraint::clearIndices() +{ + m_indices.beginEdit()->clear(); + m_indices.endEdit(); +} + +template +void PartialLinearMovementProjectiveConstraint::addIndex(Index index) +{ + m_indices.beginEdit()->push_back(index); + m_indices.endEdit(); +} + +template +void PartialLinearMovementProjectiveConstraint::removeIndex(Index index) +{ + sofa::type::removeValue(*m_indices.beginEdit(),index); + m_indices.endEdit(); +} + +template +void PartialLinearMovementProjectiveConstraint::clearKeyMovements() +{ + m_keyTimes.beginEdit()->clear(); + m_keyTimes.endEdit(); + m_keyMovements.beginEdit()->clear(); + m_keyMovements.endEdit(); +} + +template +void PartialLinearMovementProjectiveConstraint::addKeyMovement(Real time, Deriv movement) +{ + m_keyTimes.beginEdit()->push_back( time ); + m_keyTimes.endEdit(); + m_keyMovements.beginEdit()->push_back( movement ); + m_keyMovements.endEdit(); +} + +// -- Constraint interface + + +template +void PartialLinearMovementProjectiveConstraint::init() +{ + this->core::behavior::ProjectiveConstraintSet::init(); + + if (l_topology.empty()) + { + msg_info() << "link to Topology container should be set to ensure right behavior. First Topology found in current context will be used."; + l_topology.set(this->getContext()->getMeshTopologyLink()); + } + + if (sofa::core::topology::BaseMeshTopology* _topology = l_topology.get()) + { + msg_info() << "Topology path used: '" << l_topology.getLinkedPath() << "'"; + + // Initialize topological changes support + m_indices.createTopologyHandler(_topology); + } + else + { + msg_info() << "No topology component found at path: " << l_topology.getLinkedPath() << ", nor in current context: " << this->getContext()->name; + } + + x0.resize(0); + nextM = prevM = Deriv(); + + currentTime = -1.0; + finished = false; +} + + +template +void PartialLinearMovementProjectiveConstraint::reset() +{ + nextT = prevT = 0.0; + nextM = prevM = Deriv(); + + currentTime = -1.0; + finished = false; +} + + +template +template +void PartialLinearMovementProjectiveConstraint::projectResponseT(DataDeriv& dx, + const std::function& clear) +{ + Real cT = (Real) this->getContext()->getTime(); + VecBool movedDirection = movedDirections.getValue(); + if ((cT != currentTime) || !finished) + { + findKeyTimes(); + } + + if (finished && nextT != prevT) + { + const SetIndexArray & indices = m_indices.getValue(); + + //set the motion to the Dofs + for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) + { + clear(dx, *it, movedDirection); + } + } +} + +template +void PartialLinearMovementProjectiveConstraint::projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& resData) +{ + SOFA_UNUSED(mparams); + helper::WriteAccessor res = resData; + projectResponseT(res.wref(), [](VecDeriv& dx, const unsigned int index, const VecBool& b) + { for (unsigned j = 0; j < b.size(); j++) if (b[j]) dx[index][j] = 0.0; }); +} + +template +void PartialLinearMovementProjectiveConstraint::projectVelocity(const core::MechanicalParams* /*mparams*/, DataVecDeriv& vData) +{ + helper::WriteAccessor dx = vData; + Real cT = (Real) this->getContext()->getTime(); + if ((cT != currentTime) || !finished) + { + findKeyTimes(); + } + + if (finished && nextT != prevT) + { + const SetIndexArray & indices = m_indices.getValue(); + + //set the motion to the Dofs + for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) + { + dx[*it] = (nextM - prevM)*(1.0 / (nextT - prevT)); + } + } +} + + +template +void PartialLinearMovementProjectiveConstraint::projectPosition(const core::MechanicalParams* /*mparams*/, DataVecCoord& xData) +{ + helper::WriteAccessor x = xData; + Real cT = (Real) this->getContext()->getTime(); + + //initialize initial Dofs positions, if it's not done + if (x0.size() == 0) + { + const SetIndexArray & indices = m_indices.getValue(); + x0.resize(x.size()); + for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) + { + x0[*it] = x[*it]; + } + } + + if ((cT != currentTime) || !finished) + { + findKeyTimes(); + } + + //if we found 2 keyTimes, we have to interpolate a velocity (linear interpolation) + if(finished && nextT != prevT) + { + interpolatePosition(cT, x.wref()); + } +} + +template +template +void PartialLinearMovementProjectiveConstraint::interpolatePosition(Real cT, typename std::enable_if >::value, VecCoord>::type& x) +{ + const SetIndexArray & indices = m_indices.getValue(); + Real dt = (cT - prevT) / (nextT - prevT); + Deriv m = prevM + (nextM-prevM)*dt; + VecBool movedDirection = movedDirections.getValue(); + //set the motion to the Dofs + if(linearMovementBetweenNodesInIndices.getValue()) + { + + const type::vector &imposedDisplacmentOnMacroNodes = this->m_imposedDisplacmentOnMacroNodes.getValue(); + Real a = X0.getValue(); + Real b = Y0.getValue(); + Real c = Z0.getValue(); + bool case2d=false; + if((a==0.0)||(b==0.0)||(c==0.0)) case2d=true; + if(a==0.0) {a=b; b=c;} + if(b==0.0) {b=c;} + + for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) + { + for( unsigned j=0; j< NumDimensions; j++) + { + if(movedDirection[j]) + { + if(case2d) + { + x[*it][j] = x0[*it][j] + ((Real)1.0/(a*b))*((a-x0[*it][0])*(b-x0[*it][1])*imposedDisplacmentOnMacroNodes[0]+ ///< N1 + x0[*it][0]*(b-x0[*it][1])*imposedDisplacmentOnMacroNodes[1]+ ///< N2 + x0[*it][0]*x0[*it][1]*imposedDisplacmentOnMacroNodes[2]+ ///< N3 + (a-x0[*it][0])*x0[*it][1]*imposedDisplacmentOnMacroNodes[3])*m[j]; ///< N4 + // 4|----------------|3 + // | | + // | | + // | | + // 1|----------------|2 + } + else ///< case3d + { + // |Y + // 5---------8 + // /| /| + // / | / | + // 6--|------7 | + // | |/ | | + // | 1------|--4--->X + // | / | / + // |/ |/ + // 2---------3 + // Z/ + // + + x[*it][j] = x0[*it][j] + ((Real)1.0/(a*b*c))*( + (a-x0[*it][0])*(b-x0[*it][1])*(c-x0[*it][2])*imposedDisplacmentOnMacroNodes[0]+ ///< N1 + (a-x0[*it][0])*(b-x0[*it][1])*x0[*it][2]*imposedDisplacmentOnMacroNodes[1]+ ///< N2 + x0[*it][0]*(b-x0[*it][1])*x0[*it][2]*imposedDisplacmentOnMacroNodes[2]+ ///< N3 + x0[*it][0]*(b-x0[*it][1])*(c-x0[*it][2])*imposedDisplacmentOnMacroNodes[3]+ ///< N4 + (a-x0[*it][0])*x0[*it][1]*(c-x0[*it][2])*imposedDisplacmentOnMacroNodes[4]+ ///< N5 + (a-x0[*it][0])*x0[*it][1]*x0[*it][2]*imposedDisplacmentOnMacroNodes[5]+ ///< N6 + x0[*it][0]*x0[*it][1]*x0[*it][2]*imposedDisplacmentOnMacroNodes[6]+ ///< N7 + x0[*it][0]*x0[*it][1]*(c-x0[*it][2])*imposedDisplacmentOnMacroNodes[7] ///< N8 + + )*m[j]; + + } + } + } + } + + } + else + { + for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) + { + for( unsigned j=0; j< NumDimensions; j++) + if(movedDirection[j]) x[*it][j] = x0[*it][j] + m[j] ; + } + } + +} + +template +template +void PartialLinearMovementProjectiveConstraint::interpolatePosition(Real cT, typename std::enable_if >::value, VecCoord>::type& x) +{ + const SetIndexArray & indices = m_indices.getValue(); + + Real dt = (cT - prevT) / (nextT - prevT); + Deriv m = prevM + (nextM-prevM)*dt; + type::Quat prevOrientation = type::Quat::createQuaterFromEuler(getVOrientation(prevM)); + type::Quat nextOrientation = type::Quat::createQuaterFromEuler(getVOrientation(nextM)); + + //set the motion to the Dofs + for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) + { + x[*it].getCenter() = x0[*it].getCenter() + getVCenter(m) ; + x[*it].getOrientation() = x0[*it].getOrientation() * prevOrientation.slerp2(nextOrientation, dt); + } +} + +template +void PartialLinearMovementProjectiveConstraint::projectJacobianMatrix(const core::MechanicalParams* mparams, DataMatrixDeriv& cData) +{ + SOFA_UNUSED(mparams); + helper::WriteAccessor c = cData; + projectResponseT(c.wref(), + [](MatrixDeriv& res, const unsigned int index, const VecBool& btype) + { + auto itRow = res.begin(); + auto itRowEnd = res.end(); + + while (itRow != itRowEnd) + { + for (auto colIt = itRow.begin(); colIt != itRow.end(); colIt++) + { + if (index == (unsigned int)colIt.index()) + { + Deriv b = colIt.val(); + for (unsigned int j = 0; j < btype.size(); j++) if (btype[j]) b[j] = 0.0; + res.writeLine(itRow.index()).setCol(colIt.index(), b); + } + } + } + }); +} + +template +void PartialLinearMovementProjectiveConstraint::findKeyTimes() +{ + Real cT = (Real) this->getContext()->getTime(); + finished = false; + + if(m_keyTimes.getValue().size() != 0 && cT >= *m_keyTimes.getValue().begin() && cT <= *m_keyTimes.getValue().rbegin()) + { + nextT = *m_keyTimes.getValue().begin(); + prevT = nextT; + + typename type::vector::const_iterator it_t = m_keyTimes.getValue().begin(); + typename VecDeriv::const_iterator it_m = m_keyMovements.getValue().begin(); + + //WARNING : we consider that the key-events are in chronological order + //here we search between which keyTimes we are, to know which are the motion to interpolate + while( it_t != m_keyTimes.getValue().end() && !finished) + { + if( *it_t <= cT) + { + prevT = *it_t; + prevM = *it_m; + } + else + { + nextT = *it_t; + nextM = *it_m; + finished = true; + } + ++it_t; + ++it_m; + } + } +} + +// Matrix Integration interface +template +void PartialLinearMovementProjectiveConstraint::applyConstraint(const core::MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) +{ + SOFA_UNUSED(mparams); + const SetIndexArray & indices = m_indices.getValue(); + + if (core::behavior::MultiMatrixAccessor::MatrixRef r = matrix->getMatrix(this->mstate.get())) + { + VecBool movedDirection = movedDirections.getValue(); + for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) + { + // Reset Fixed Row and Col + for (unsigned int c=0; cclearRowCol(r.offset + NumDimensions * (*it) + c); + } + // Set Fixed Vertex + for (unsigned int c=0; cset(r.offset + NumDimensions * (*it) + c, r.offset + NumDimensions * (*it) + c, 1.0); + } + } + } +} + +template +void PartialLinearMovementProjectiveConstraint::applyConstraint(const core::MechanicalParams* mparams, linearalgebra::BaseVector* vector, const sofa::core::behavior::MultiMatrixAccessor* matrix) +{ + SOFA_UNUSED(mparams); + const int o = matrix->getGlobalOffset(this->mstate.get()); + if (o >= 0) { + unsigned int offset = (unsigned int)o; + VecBool movedDirection = movedDirections.getValue(); + const SetIndexArray & indices = m_indices.getValue(); + for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) + { + for (unsigned int c = 0; c < NumDimensions; ++c) + { + if (movedDirection[c]) + { + vector->clear(offset + NumDimensions * (*it) + c); + } + } + } + } +} + +//display the path the constrained dofs will go through +template +void PartialLinearMovementProjectiveConstraint::draw(const core::visual::VisualParams* vparams) +{ + const auto stateLifeCycle = vparams->drawTool()->makeStateLifeCycle(); + + if (!vparams->displayFlags().getShowBehaviorModels() || m_keyTimes.getValue().size() == 0) + return; + + sofa::type::vector vertices; + constexpr sofa::type::RGBAColor color(1, 0.5, 0.5, 1); + + if (showMovement.getValue()) + { + vparams->drawTool()->disableLighting(); + + const SetIndexArray & indices = m_indices.getValue(); + const VecDeriv& keyMovements = m_keyMovements.getValue(); + for (unsigned int i = 0; i < keyMovements.size() - 1; i++) + { + for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) + { + const type::Vec3 v0 { DataTypes::getCPos(x0[*it]) + DataTypes::getDPos(keyMovements[i]) }; + const type::Vec3 v1 { DataTypes::getCPos(x0[*it]) + DataTypes::getDPos(keyMovements[i + 1]) }; + + vertices.push_back(v0); + vertices.push_back(v1); + } + } + vparams->drawTool()->drawLines(vertices, 1, color); + } + else + { + const VecCoord& x = this->mstate->read(core::ConstVecCoordId::position())->getValue(); + + type::Vec3 point; + const SetIndexArray & indices = m_indices.getValue(); + for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) + { + point = DataTypes::getCPos(x[*it]); + vertices.push_back(point); + } + vparams->drawTool()->drawPoints(vertices, 10, color); + } + + +} +} // namespace sofa::component::constraint::projective diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PatchTestMovementConstraint.h b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PatchTestMovementConstraint.h index 2a53250282d..3857c8c889c 100644 --- a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PatchTestMovementConstraint.h +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PatchTestMovementConstraint.h @@ -20,146 +20,13 @@ * Contact information: contact@sofa-framework.org * ******************************************************************************/ #pragma once -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include -#include -#include +SOFA_DEPRECATED_HEADER("v24.06", "v25.06", "sofa/component/constraint/projective/PatchTestMovementProjectiveConstraint.h") namespace sofa::component::constraint::projective { - -template -class PatchTestMovementConstraintInternalData -{ -}; - -/** - Impose a motion to all the boundary points of a mesh. The motion of the 4 corners are given in the data d_cornerMovements and the movements of the edge points are computed by linear interpolation. -*/ -template -class PatchTestMovementConstraint : public core::behavior::ProjectiveConstraintSet -{ -public: - SOFA_CLASS(SOFA_TEMPLATE(PatchTestMovementConstraint,TDataTypes),SOFA_TEMPLATE(sofa::core::behavior::ProjectiveConstraintSet, TDataTypes)); - - using Index = sofa::Index; - typedef TDataTypes DataTypes; - typedef typename DataTypes::VecCoord VecCoord; - typedef typename DataTypes::VecDeriv VecDeriv; - typedef typename DataTypes::Coord Coord; - typedef typename DataTypes::Deriv Deriv; - typedef typename DataTypes::Real Real; - typedef Data DataVecCoord; - typedef Data DataVecDeriv; - typedef type::vector SetIndexArray; - typedef sofa::core::topology::TopologySubsetIndices SetIndex; - - static constexpr unsigned int CoordSize = Coord::total_size; - - typedef typename DataTypes::MatrixDeriv MatrixDeriv; - typedef core::objectmodel::Data DataMatrixDeriv; - -protected: - PatchTestMovementConstraintInternalData *data; - friend class PatchTestMovementConstraintInternalData; - -public : - /// indices of the DOFs of the mesh - SetIndex d_meshIndices; - /// indices of the DOFs the constraint is applied to - SetIndex d_indices; - /// data begin time when the constraint is applied - Data d_beginConstraintTime; - /// data end time when the constraint is applied - Data d_endConstraintTime; - /// coordinates of the DOFs the constraint is applied to - Data d_constrainedPoints; - /// the movements of the corner points (this is the difference between initial and final positions of the 4 corners) - Data d_cornerMovements; - /// the coordinates of the corner points - Data d_cornerPoints; - /// Draw constrained points - Data d_drawConstrainedPoints; - /// initial constrained DOFs position - VecCoord x0; - /// final constrained DOFs position - VecCoord xf; - /// initial mesh DOFs position - VecCoord meshPointsX0; - /// final mesh DOFs position - VecCoord meshPointsXf; - - /// Link to be set to the topology container in the component graph. - SingleLink, sofa::core::topology::BaseMeshTopology, BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_topology; - -protected: - PatchTestMovementConstraint(); - - virtual ~PatchTestMovementConstraint(); - -public: - //Add or clear constraints - void clearConstraints(); - void addConstraint(Index index); - void removeConstraint(Index index); - - /// -- Constraint interface - void init() override; - - /// Cancel the possible forces - void projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& resData) override; - /// Cancel the possible velocities - void projectVelocity(const core::MechanicalParams* mparams, DataVecDeriv& vData) override; - /// Apply the computed movements to the border mesh points between beginConstraintTime and endConstraintTime - void projectPosition(const core::MechanicalParams* mparams, DataVecCoord& xData) override; - // Implement projectMatrix for assembled solver of compliant - void projectMatrix( sofa::linearalgebra::BaseMatrix* /*M*/, unsigned /*offset*/ ) override; - - void projectJacobianMatrix(const core::MechanicalParams* /*mparams*/, DataMatrixDeriv& /* cData */) override - { - msg_error() <<"projectJacobianMatrix not implemented"; - } - - /// Compute the theoretical final positions - void getFinalPositions (VecCoord& finalPos, DataVecCoord& xData); - - /// Draw the constrained points (= border mesh points) - void draw(const core::visual::VisualParams* vparams) override; - -protected: - - void projectResponseImpl(VecDeriv& dx); - -private: - - /// Find the corners of the grid mesh - void findCornerPoints(); - - /// Compute the displacement of each mesh point by linear interpolation with the displacement of corner points - void computeInterpolatedDisplacement (int pointIndice,const DataVecCoord& xData, Deriv& displacement); - - /// Initialize initial positions - void initializeInitialPositions (const SetIndexArray & indices, DataVecCoord& xData, VecCoord& x0); - - /// Initialize final positions - void initializeFinalPositions (const SetIndexArray & indices, DataVecCoord& xData, VecCoord& x0 , VecCoord& xf); -}; - - -#if !defined(SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_PATCHTESTMOVEMENTCONSTRAINT_CPP) -extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PatchTestMovementConstraint; -extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PatchTestMovementConstraint; - -#endif - -} // namespace sofa::component::constraint::projective +template +using PatchTestMovementConstraint SOFA_ATTRIBUTE_DEPRECATED("v24.06 ", "v25.06", "PatchTestMovementConstraint has been renamed to PatchTestMovementProjectiveConstraint") = PatchTestMovementProjectiveConstraint; +} \ No newline at end of file diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PatchTestMovementConstraint.inl b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PatchTestMovementConstraint.inl index be21d19e187..7a3a2a49a06 100644 --- a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PatchTestMovementConstraint.inl +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PatchTestMovementConstraint.inl @@ -21,429 +21,6 @@ ******************************************************************************/ #pragma once -#include -#include -#include -#include -#include -#include -#include -#include +#include -namespace sofa::component::constraint::projective -{ - -template -PatchTestMovementConstraint::PatchTestMovementConstraint() - : core::behavior::ProjectiveConstraintSet(nullptr) - , data(new PatchTestMovementConstraintInternalData) - , d_meshIndices( initData(&d_meshIndices,"meshIndices","Indices of the mesh") ) - , d_indices( initData(&d_indices,"indices","Indices of the constrained points") ) - , d_beginConstraintTime( initData(&d_beginConstraintTime,"beginConstraintTime","Begin time of the bilinear constraint") ) - , d_endConstraintTime( initData(&d_endConstraintTime,"endConstraintTime","End time of the bilinear constraint") ) - , d_constrainedPoints( initData(&d_constrainedPoints,"constrainedPoints","Coordinates of the constrained points") ) - , d_cornerMovements( initData(&d_cornerMovements,"cornerMovements","movements of the corners of the grid") ) - , d_cornerPoints( initData(&d_cornerPoints,"cornerPoints","corner points for computing constraint") ) - , d_drawConstrainedPoints( initData(&d_drawConstrainedPoints,"drawConstrainedPoints","draw constrained points") ) - , l_topology(initLink("topology", "link to the topology container")) -{ - if(!d_beginConstraintTime.isSet()) - d_beginConstraintTime = 0; - if(!d_endConstraintTime.isSet()) - d_endConstraintTime = 20; -} - - - -template -PatchTestMovementConstraint::~PatchTestMovementConstraint() -{ - -} - -template -void PatchTestMovementConstraint::clearConstraints() -{ - d_indices.beginEdit()->clear(); - d_indices.endEdit(); -} - -template -void PatchTestMovementConstraint::addConstraint(Index index) -{ - d_indices.beginEdit()->push_back(index); - d_indices.endEdit(); -} - -template -void PatchTestMovementConstraint::removeConstraint(Index index) -{ - sofa::type::removeValue(*d_indices.beginEdit(),index); - d_indices.endEdit(); -} - -// -- Constraint interface - - -template -void PatchTestMovementConstraint::init() -{ - this->core::behavior::ProjectiveConstraintSet::init(); - - if (l_topology.empty()) - { - msg_info() << "link to Topology container should be set to ensure right behavior. First Topology found in current context will be used."; - l_topology.set(this->getContext()->getMeshTopologyLink()); - } - - if (sofa::core::topology::BaseMeshTopology* _topology = l_topology.get()) - { - msg_info() << "Topology path used: '" << l_topology.getLinkedPath() << "'"; - - // Initialize topological changes support - d_indices.createTopologyHandler(_topology); - } - else - { - msg_info() << "No topology component found at path: " << l_topology.getLinkedPath() << ", nor in current context: " << this->getContext()->name; - } - - const SetIndexArray & indices = d_indices.getValue(); - - const Index maxIndex=this->mstate->getSize(); - for (unsigned int i=0; i= maxIndex) - { - msg_error() <<"Index " << index << " not valid!"; - removeConstraint(index); - } - } - - // Find the 4 corners of the grid topology - this->findCornerPoints(); -} - -template -void PatchTestMovementConstraint::findCornerPoints() -{ - Coord corner0, corner1, corner2, corner3,corner4,corner5,corner6,corner7; - // Write accessor - helper::WriteAccessor< Data > cornerPositions = d_cornerPoints; - helper::WriteAccessor< Data > constrainedPoints = d_constrainedPoints; - bool isMeshin3D = false; - - if (!constrainedPoints.empty()) - { - const Coord& point = constrainedPoints[0]; - - // Search if the constrained points are in the same plane - for(Size i = 0; i < constrainedPoints.size() ; i++) - { - if(CoordSize > 2 && constrainedPoints[i][2]!=point[2]) - { - isMeshin3D = true; - } - } - } - - if(constrainedPoints.size() > 0) - { - corner0 = constrainedPoints[0]; - corner1 = constrainedPoints[0]; - corner2 = constrainedPoints[0]; - corner3 = constrainedPoints[0]; - corner4 = constrainedPoints[0]; - corner5 = constrainedPoints[0]; - corner6 = constrainedPoints[0]; - corner7 = constrainedPoints[0]; - - for (size_t i = 0; i < constrainedPoints.size() ; i++) - { - if(constrainedPoints[i][0] < corner0[0] || constrainedPoints[i][1] < corner0[1] || ( CoordSize>2 && constrainedPoints[i][2] < corner0[2] ) ) - { - corner0 = constrainedPoints[i]; - } - - if(constrainedPoints[i][0] > corner2[0] || constrainedPoints[i][1] > corner2[1] || ( CoordSize>2 && constrainedPoints[i][2] < corner2[2] ) ) - { - corner2 = constrainedPoints[i]; - } - - if(constrainedPoints[i][1] < corner1[1] || constrainedPoints[i][0] > corner1[0] || ( CoordSize>2 && constrainedPoints[i][2] < corner1[2] )) - { - corner1 = constrainedPoints[i]; - } - - if(constrainedPoints[i][0] < corner3[0] || constrainedPoints[i][1] > corner3[1] || ( CoordSize>2 && constrainedPoints[i][2] < corner3[2] )) - { - corner3 = constrainedPoints[i]; - } - - if(isMeshin3D && (constrainedPoints[i][0] < corner4[0] || constrainedPoints[i][1] < corner4[1] || (CoordSize>2 && constrainedPoints[i][2] > corner0[2] )) ) - { - corner4 = constrainedPoints[i]; - } - - if(isMeshin3D && (constrainedPoints[i][0] > corner6[0] || constrainedPoints[i][1] > corner6[1] || (CoordSize>2 && constrainedPoints[i][2] > corner2[2] )) ) - { - corner6 = constrainedPoints[i]; - } - - if(isMeshin3D && (constrainedPoints[i][1] < corner5[1] || constrainedPoints[i][0] > corner5[0] || (CoordSize>2 && constrainedPoints[i][2] > corner5[2] )) ) - { - corner5 = constrainedPoints[i]; - } - - else if(isMeshin3D && (constrainedPoints[i][0] < corner7[0] || constrainedPoints[i][1] > corner7[1] || (CoordSize>2 && constrainedPoints[i][2] > corner7[2] )) ) - { - corner7 = constrainedPoints[i]; - } - } - - cornerPositions.push_back(corner0); - cornerPositions.push_back(corner1); - cornerPositions.push_back(corner2); - cornerPositions.push_back(corner3); - - // 3D - if(isMeshin3D) - { - cornerPositions.push_back(corner4); - cornerPositions.push_back(corner5); - cornerPositions.push_back(corner6); - cornerPositions.push_back(corner7); - } - } -} - -template -void PatchTestMovementConstraint::projectResponseImpl(VecDeriv& dx) -{ - const SetIndexArray & indices = d_indices.getValue(); - for (size_t i = 0; i< indices.size(); ++i) - { - dx[indices[i]]=Deriv(); - } -} - -template -void PatchTestMovementConstraint::projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& resData) -{ - SOFA_UNUSED(mparams); - helper::WriteAccessor res = resData; - projectResponseImpl(res.wref()); -} - -template -void PatchTestMovementConstraint::projectVelocity(const core::MechanicalParams* mparams, DataVecDeriv& vData) -{ - SOFA_UNUSED(mparams); - helper::WriteAccessor res = vData; - projectResponseImpl(res.wref()); -} - -template -void PatchTestMovementConstraint::projectPosition(const core::MechanicalParams* /*mparams*/, DataVecCoord& xData) -{ - const sofa::simulation::Node::SPtr root = down_cast( this->getContext()->getRootContext() ); - helper::WriteAccessor x = xData; - const SetIndexArray & indices = d_indices.getValue(); - - // Time - double beginTime = d_beginConstraintTime.getValue(); - double endTime = d_endConstraintTime.getValue(); - double totalTime = endTime - beginTime; - - //initialize initial mesh Dofs positions, if it's not done - if(meshPointsX0.size()==0) - this->initializeInitialPositions(d_meshIndices.getValue(),xData,meshPointsX0); - - //initialize final mesh Dofs positions, if it's not done - if(meshPointsXf.size()==0) - this->initializeFinalPositions(d_meshIndices.getValue(),xData, meshPointsX0, meshPointsXf); - - //initialize initial constrained Dofs positions, if it's not done - if(x0.size() == 0) - this->initializeInitialPositions(indices,xData,x0); - - //initialize final constrained Dofs positions, if it's not done - if (xf.size() == 0) - this->initializeFinalPositions(indices,xData,x0,xf); - - // Update the intermediate Dofs positions computed by linear interpolation - double time = root->getTime(); - if( time > beginTime && time <= endTime && totalTime > 0) - { - for (size_t i = 0; i< indices.size(); ++i) - { - x[indices[i]] = ((xf[indices[i]]-x0[indices[i]])*time + (x0[indices[i]]*endTime - xf[indices[i]]*beginTime))/totalTime; - } - } - else if (time > endTime) - { - for (size_t i = 0; i< indices.size(); ++i) - { - x[indices[i]] = xf[indices[i]]; - } - } -} - -template -void PatchTestMovementConstraint::projectMatrix( sofa::linearalgebra::BaseMatrix* M, unsigned offset ) -{ - // clears the rows and columns associated with constrained particles - const unsigned blockSize = DataTypes::deriv_total_size; - - for (const auto id : d_indices.getValue()) - { - M->clearRowsCols( offset + id * blockSize, offset + (id+1) * blockSize ); - } - -} - -template -void PatchTestMovementConstraint::getFinalPositions( VecCoord& finalPos,DataVecCoord& xData) -{ - // Indices of mesh points - const SetIndexArray & meshIndices = d_meshIndices.getValue(); - - // Initialize final positions - if(meshPointsXf.size()==0) - {this->initializeFinalPositions(meshIndices,xData,meshPointsX0,meshPointsXf);} - - // Set final positions - finalPos.resize(meshIndices.size()); - for (size_t i=0; i < meshIndices.size() ; ++i) - { - finalPos[meshIndices[i]] = meshPointsXf[meshIndices[i]]; - } -} - -template -void PatchTestMovementConstraint::initializeInitialPositions (const SetIndexArray & indices, DataVecCoord& xData, VecCoord& x0) -{ - helper::WriteAccessor x = xData; - - x0.resize(x.size()); - for (size_t i=0; i < indices.size() ; ++i) - { - x0[indices[i]] = x[indices[i]]; - } - -} - -template -void PatchTestMovementConstraint::initializeFinalPositions (const SetIndexArray & indices, DataVecCoord& xData, VecCoord& x0, VecCoord& xf) -{ - Deriv displacement; - helper::WriteAccessor x = xData; - - xf.resize(x.size()); - - // if the positions were not initialized - if(x0.size() == 0) - this->initializeInitialPositions(indices,xData,x0); - - for (size_t i=0; i < indices.size() ; ++i) - { - this->computeInterpolatedDisplacement(indices[i],xData,displacement); - xf[indices[i]] = x0[indices[i]] + displacement ; - } - -} - -template -void PatchTestMovementConstraint::computeInterpolatedDisplacement(int pointIndice,const DataVecCoord& xData, Deriv& displacement) -{ - // For each mesh point compute the associated displacement - - // The 3 barycentric coefficients along x, y and z axis - Real alpha, beta, gamma; - - // Corner points - const VecCoord& cornerPoints = d_cornerPoints.getValue(); - if(cornerPoints.size()==0) - this->findCornerPoints(); - - if(cornerPoints.size() == 4) - { - Coord corner0 = cornerPoints[0]; - Coord corner1 = cornerPoints[1]; - Coord corner3 = cornerPoints[3]; - - // Coord of the point - helper::ReadAccessor x = xData; - Coord point = x[pointIndice]; - - // Compute alpha = barycentric coefficient along the x axis - alpha = fabs(point[0]-corner0[0])/fabs(corner1[0]-corner0[0]); - - // Compute beta = barycentric coefficient along the y axis - beta = fabs(point[1]-corner0[1])/fabs(corner3[1]-corner0[1]); - - // cornerMovements - const VecDeriv& cornerMovements = d_cornerMovements.getValue(); - - // Compute displacement by linear interpolation - displacement = cornerMovements[0]*(1-alpha)*(1-beta) + cornerMovements[1]*alpha*(1-beta)+ cornerMovements[2]*alpha*beta+cornerMovements[3]*(1-alpha)*beta; - } - - else if(cornerPoints.size() == 8) - { - Coord corner0 = cornerPoints[0]; - Coord corner1 = cornerPoints[1]; - Coord corner3 = cornerPoints[3]; - Coord corner4 = cornerPoints[4]; - - // Coord of the point - helper::ReadAccessor x = xData; - Coord point = x[pointIndice]; - - // Compute alpha = barycentric coefficient along the x axis - alpha = fabs(point[0]-corner0[0])/fabs(corner1[0]-corner0[0]); - - // Compute beta = barycentric coefficient along the y axis - beta = fabs(point[1]-corner0[1])/fabs(corner3[1]-corner0[1]); - - // Compute gamma = barycentric coefficient along the z axis - if( CoordSize>2 ) - gamma = fabs(point[2]-corner0[2])/fabs(corner4[2]-corner0[2]); // 3D - else - gamma = 0; // 2D - - // cornerMovements - const VecDeriv& cornerMovements = d_cornerMovements.getValue(); - - // Compute displacement by linear interpolation - displacement = (cornerMovements[0]*(1-alpha)*(1-beta) + cornerMovements[1]*alpha*(1-beta)+ cornerMovements[2]*alpha*beta+cornerMovements[3]*(1-alpha)*beta) * (1-gamma) - + (cornerMovements[4]*(1-alpha)*(1-beta) + cornerMovements[5]*alpha*(1-beta)+ cornerMovements[6]*alpha*beta+cornerMovements[7]*(1-alpha)*beta) * gamma; - } - else - { - msg_info() << "error don't find the corner points" ; - } - -} - -template -void PatchTestMovementConstraint::draw(const core::visual::VisualParams* vparams) -{ - const SetIndexArray & indices = d_indices.getValue(); - const VecCoord& x = this->mstate->read(core::ConstVecCoordId::position())->getValue(); - type::Vec3 point; - - if(d_drawConstrainedPoints.getValue()) - { - std::vector< type::Vec3 > points; - for (unsigned int index : indices) - { - point = DataTypes::getCPos(x[index]); - points.push_back(point); - } - vparams->drawTool()->drawPoints(points, 10, sofa::type::RGBAColor(1,0.5,0.5,1)); - } -} - -} // namespace sofa::component::constraint::projective +SOFA_DEPRECATED_HEADER("v24.06", "v25.06", "sofa/component/constraint/projective/PatchTestMovementProjectiveConstraint.inl") diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PatchTestMovementConstraint.cpp b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PatchTestMovementProjectiveConstraint.cpp similarity index 82% rename from Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PatchTestMovementConstraint.cpp rename to Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PatchTestMovementProjectiveConstraint.cpp index beb420617e0..4206b03ac40 100644 --- a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PatchTestMovementConstraint.cpp +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PatchTestMovementProjectiveConstraint.cpp @@ -19,8 +19,8 @@ * * * Contact information: contact@sofa-framework.org * ******************************************************************************/ -#define SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_PATCHTESTMOVEMENTCONSTRAINT_CPP -#include +#define SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_PATCHTESTMOVEMENTPROJECTIVECONSTRAINT_CPP +#include #include #include #include @@ -29,14 +29,14 @@ namespace sofa::component::constraint::projective { //declaration of the class, for the factory -int PatchTestMovementConstraintClass = core::RegisterObject("bilinear constraint") - .add< PatchTestMovementConstraint >() - .add< PatchTestMovementConstraint >() - +int PatchTestMovementProjectiveConstraintClass = core::RegisterObject("bilinear constraint") + .add< PatchTestMovementProjectiveConstraint >() + .add< PatchTestMovementProjectiveConstraint >() + .addAlias("PatchTestMovementConstraint") ; -template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PatchTestMovementConstraint; -template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PatchTestMovementConstraint; +template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PatchTestMovementProjectiveConstraint; +template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PatchTestMovementProjectiveConstraint; } // namespace sofa::component::constraint::projective diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PatchTestMovementProjectiveConstraint.h b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PatchTestMovementProjectiveConstraint.h new file mode 100644 index 00000000000..5cabd8d460f --- /dev/null +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PatchTestMovementProjectiveConstraint.h @@ -0,0 +1,165 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace sofa::component::constraint::projective +{ + +template +class PatchTestMovementProjectiveConstraintInternalData +{ +}; + +/** + Impose a motion to all the boundary points of a mesh. The motion of the 4 corners are given in the data d_cornerMovements and the movements of the edge points are computed by linear interpolation. +*/ +template +class PatchTestMovementProjectiveConstraint : public core::behavior::ProjectiveConstraintSet +{ +public: + SOFA_CLASS(SOFA_TEMPLATE(PatchTestMovementProjectiveConstraint,TDataTypes),SOFA_TEMPLATE(sofa::core::behavior::ProjectiveConstraintSet, TDataTypes)); + + using Index = sofa::Index; + typedef TDataTypes DataTypes; + typedef typename DataTypes::VecCoord VecCoord; + typedef typename DataTypes::VecDeriv VecDeriv; + typedef typename DataTypes::Coord Coord; + typedef typename DataTypes::Deriv Deriv; + typedef typename DataTypes::Real Real; + typedef Data DataVecCoord; + typedef Data DataVecDeriv; + typedef type::vector SetIndexArray; + typedef sofa::core::topology::TopologySubsetIndices SetIndex; + + static constexpr unsigned int CoordSize = Coord::total_size; + + typedef typename DataTypes::MatrixDeriv MatrixDeriv; + typedef core::objectmodel::Data DataMatrixDeriv; + +protected: + PatchTestMovementProjectiveConstraintInternalData *data; + friend class PatchTestMovementProjectiveConstraintInternalData; + +public : + /// indices of the DOFs of the mesh + SetIndex d_meshIndices; + /// indices of the DOFs the constraint is applied to + SetIndex d_indices; + /// data begin time when the constraint is applied + Data d_beginConstraintTime; + /// data end time when the constraint is applied + Data d_endConstraintTime; + /// coordinates of the DOFs the constraint is applied to + Data d_constrainedPoints; + /// the movements of the corner points (this is the difference between initial and final positions of the 4 corners) + Data d_cornerMovements; + /// the coordinates of the corner points + Data d_cornerPoints; + /// Draw constrained points + Data d_drawConstrainedPoints; + /// initial constrained DOFs position + VecCoord x0; + /// final constrained DOFs position + VecCoord xf; + /// initial mesh DOFs position + VecCoord meshPointsX0; + /// final mesh DOFs position + VecCoord meshPointsXf; + + /// Link to be set to the topology container in the component graph. + SingleLink, sofa::core::topology::BaseMeshTopology, BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_topology; + +protected: + PatchTestMovementProjectiveConstraint(); + + virtual ~PatchTestMovementProjectiveConstraint(); + +public: + //Add or clear constraints + void clearConstraints(); + void addConstraint(Index index); + void removeConstraint(Index index); + + /// -- Constraint interface + void init() override; + + /// Cancel the possible forces + void projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& resData) override; + /// Cancel the possible velocities + void projectVelocity(const core::MechanicalParams* mparams, DataVecDeriv& vData) override; + /// Apply the computed movements to the border mesh points between beginConstraintTime and endConstraintTime + void projectPosition(const core::MechanicalParams* mparams, DataVecCoord& xData) override; + // Implement projectMatrix for assembled solver of compliant + void projectMatrix( sofa::linearalgebra::BaseMatrix* /*M*/, unsigned /*offset*/ ) override; + + void projectJacobianMatrix(const core::MechanicalParams* /*mparams*/, DataMatrixDeriv& /* cData */) override + { + msg_error() <<"projectJacobianMatrix not implemented"; + } + + /// Compute the theoretical final positions + void getFinalPositions (VecCoord& finalPos, DataVecCoord& xData); + + /// Draw the constrained points (= border mesh points) + void draw(const core::visual::VisualParams* vparams) override; + +protected: + + void projectResponseImpl(VecDeriv& dx); + +private: + + /// Find the corners of the grid mesh + void findCornerPoints(); + + /// Compute the displacement of each mesh point by linear interpolation with the displacement of corner points + void computeInterpolatedDisplacement (int pointIndice,const DataVecCoord& xData, Deriv& displacement); + + /// Initialize initial positions + void initializeInitialPositions (const SetIndexArray & indices, DataVecCoord& xData, VecCoord& x0); + + /// Initialize final positions + void initializeFinalPositions (const SetIndexArray & indices, DataVecCoord& xData, VecCoord& x0 , VecCoord& xf); +}; + + +#if !defined(SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_PATCHTESTMOVEMENTPROJECTIVECONSTRAINT_CPP) +extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PatchTestMovementProjectiveConstraint; +extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PatchTestMovementProjectiveConstraint; +#endif + + +} // namespace sofa::component::constraint::projective diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PatchTestMovementProjectiveConstraint.inl b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PatchTestMovementProjectiveConstraint.inl new file mode 100644 index 00000000000..b93be91ea5c --- /dev/null +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PatchTestMovementProjectiveConstraint.inl @@ -0,0 +1,449 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace sofa::component::constraint::projective +{ + +template +PatchTestMovementProjectiveConstraint::PatchTestMovementProjectiveConstraint() + : core::behavior::ProjectiveConstraintSet(nullptr) + , data(new PatchTestMovementProjectiveConstraintInternalData) + , d_meshIndices( initData(&d_meshIndices,"meshIndices","Indices of the mesh") ) + , d_indices( initData(&d_indices,"indices","Indices of the constrained points") ) + , d_beginConstraintTime( initData(&d_beginConstraintTime,"beginConstraintTime","Begin time of the bilinear constraint") ) + , d_endConstraintTime( initData(&d_endConstraintTime,"endConstraintTime","End time of the bilinear constraint") ) + , d_constrainedPoints( initData(&d_constrainedPoints,"constrainedPoints","Coordinates of the constrained points") ) + , d_cornerMovements( initData(&d_cornerMovements,"cornerMovements","movements of the corners of the grid") ) + , d_cornerPoints( initData(&d_cornerPoints,"cornerPoints","corner points for computing constraint") ) + , d_drawConstrainedPoints( initData(&d_drawConstrainedPoints,"drawConstrainedPoints","draw constrained points") ) + , l_topology(initLink("topology", "link to the topology container")) +{ + if(!d_beginConstraintTime.isSet()) + d_beginConstraintTime = 0; + if(!d_endConstraintTime.isSet()) + d_endConstraintTime = 20; +} + + + +template +PatchTestMovementProjectiveConstraint::~PatchTestMovementProjectiveConstraint() +{ + +} + +template +void PatchTestMovementProjectiveConstraint::clearConstraints() +{ + d_indices.beginEdit()->clear(); + d_indices.endEdit(); +} + +template +void PatchTestMovementProjectiveConstraint::addConstraint(Index index) +{ + d_indices.beginEdit()->push_back(index); + d_indices.endEdit(); +} + +template +void PatchTestMovementProjectiveConstraint::removeConstraint(Index index) +{ + sofa::type::removeValue(*d_indices.beginEdit(),index); + d_indices.endEdit(); +} + +// -- Constraint interface + + +template +void PatchTestMovementProjectiveConstraint::init() +{ + this->core::behavior::ProjectiveConstraintSet::init(); + + if (l_topology.empty()) + { + msg_info() << "link to Topology container should be set to ensure right behavior. First Topology found in current context will be used."; + l_topology.set(this->getContext()->getMeshTopologyLink()); + } + + if (sofa::core::topology::BaseMeshTopology* _topology = l_topology.get()) + { + msg_info() << "Topology path used: '" << l_topology.getLinkedPath() << "'"; + + // Initialize topological changes support + d_indices.createTopologyHandler(_topology); + } + else + { + msg_info() << "No topology component found at path: " << l_topology.getLinkedPath() << ", nor in current context: " << this->getContext()->name; + } + + const SetIndexArray & indices = d_indices.getValue(); + + const Index maxIndex=this->mstate->getSize(); + for (unsigned int i=0; i= maxIndex) + { + msg_error() <<"Index " << index << " not valid!"; + removeConstraint(index); + } + } + + // Find the 4 corners of the grid topology + this->findCornerPoints(); +} + +template +void PatchTestMovementProjectiveConstraint::findCornerPoints() +{ + Coord corner0, corner1, corner2, corner3,corner4,corner5,corner6,corner7; + // Write accessor + helper::WriteAccessor< Data > cornerPositions = d_cornerPoints; + helper::WriteAccessor< Data > constrainedPoints = d_constrainedPoints; + bool isMeshin3D = false; + + if (!constrainedPoints.empty()) + { + const Coord& point = constrainedPoints[0]; + + // Search if the constrained points are in the same plane + for(Size i = 0; i < constrainedPoints.size() ; i++) + { + if(CoordSize > 2 && constrainedPoints[i][2]!=point[2]) + { + isMeshin3D = true; + } + } + } + + if(constrainedPoints.size() > 0) + { + corner0 = constrainedPoints[0]; + corner1 = constrainedPoints[0]; + corner2 = constrainedPoints[0]; + corner3 = constrainedPoints[0]; + corner4 = constrainedPoints[0]; + corner5 = constrainedPoints[0]; + corner6 = constrainedPoints[0]; + corner7 = constrainedPoints[0]; + + for (size_t i = 0; i < constrainedPoints.size() ; i++) + { + if(constrainedPoints[i][0] < corner0[0] || constrainedPoints[i][1] < corner0[1] || ( CoordSize>2 && constrainedPoints[i][2] < corner0[2] ) ) + { + corner0 = constrainedPoints[i]; + } + + if(constrainedPoints[i][0] > corner2[0] || constrainedPoints[i][1] > corner2[1] || ( CoordSize>2 && constrainedPoints[i][2] < corner2[2] ) ) + { + corner2 = constrainedPoints[i]; + } + + if(constrainedPoints[i][1] < corner1[1] || constrainedPoints[i][0] > corner1[0] || ( CoordSize>2 && constrainedPoints[i][2] < corner1[2] )) + { + corner1 = constrainedPoints[i]; + } + + if(constrainedPoints[i][0] < corner3[0] || constrainedPoints[i][1] > corner3[1] || ( CoordSize>2 && constrainedPoints[i][2] < corner3[2] )) + { + corner3 = constrainedPoints[i]; + } + + if(isMeshin3D && (constrainedPoints[i][0] < corner4[0] || constrainedPoints[i][1] < corner4[1] || (CoordSize>2 && constrainedPoints[i][2] > corner0[2] )) ) + { + corner4 = constrainedPoints[i]; + } + + if(isMeshin3D && (constrainedPoints[i][0] > corner6[0] || constrainedPoints[i][1] > corner6[1] || (CoordSize>2 && constrainedPoints[i][2] > corner2[2] )) ) + { + corner6 = constrainedPoints[i]; + } + + if(isMeshin3D && (constrainedPoints[i][1] < corner5[1] || constrainedPoints[i][0] > corner5[0] || (CoordSize>2 && constrainedPoints[i][2] > corner5[2] )) ) + { + corner5 = constrainedPoints[i]; + } + + else if(isMeshin3D && (constrainedPoints[i][0] < corner7[0] || constrainedPoints[i][1] > corner7[1] || (CoordSize>2 && constrainedPoints[i][2] > corner7[2] )) ) + { + corner7 = constrainedPoints[i]; + } + } + + cornerPositions.push_back(corner0); + cornerPositions.push_back(corner1); + cornerPositions.push_back(corner2); + cornerPositions.push_back(corner3); + + // 3D + if(isMeshin3D) + { + cornerPositions.push_back(corner4); + cornerPositions.push_back(corner5); + cornerPositions.push_back(corner6); + cornerPositions.push_back(corner7); + } + } +} + +template +void PatchTestMovementProjectiveConstraint::projectResponseImpl(VecDeriv& dx) +{ + const SetIndexArray & indices = d_indices.getValue(); + for (size_t i = 0; i< indices.size(); ++i) + { + dx[indices[i]]=Deriv(); + } +} + +template +void PatchTestMovementProjectiveConstraint::projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& resData) +{ + SOFA_UNUSED(mparams); + helper::WriteAccessor res = resData; + projectResponseImpl(res.wref()); +} + +template +void PatchTestMovementProjectiveConstraint::projectVelocity(const core::MechanicalParams* mparams, DataVecDeriv& vData) +{ + SOFA_UNUSED(mparams); + helper::WriteAccessor res = vData; + projectResponseImpl(res.wref()); +} + +template +void PatchTestMovementProjectiveConstraint::projectPosition(const core::MechanicalParams* /*mparams*/, DataVecCoord& xData) +{ + const sofa::simulation::Node::SPtr root = down_cast( this->getContext()->getRootContext() ); + helper::WriteAccessor x = xData; + const SetIndexArray & indices = d_indices.getValue(); + + // Time + double beginTime = d_beginConstraintTime.getValue(); + double endTime = d_endConstraintTime.getValue(); + double totalTime = endTime - beginTime; + + //initialize initial mesh Dofs positions, if it's not done + if(meshPointsX0.size()==0) + this->initializeInitialPositions(d_meshIndices.getValue(),xData,meshPointsX0); + + //initialize final mesh Dofs positions, if it's not done + if(meshPointsXf.size()==0) + this->initializeFinalPositions(d_meshIndices.getValue(),xData, meshPointsX0, meshPointsXf); + + //initialize initial constrained Dofs positions, if it's not done + if(x0.size() == 0) + this->initializeInitialPositions(indices,xData,x0); + + //initialize final constrained Dofs positions, if it's not done + if (xf.size() == 0) + this->initializeFinalPositions(indices,xData,x0,xf); + + // Update the intermediate Dofs positions computed by linear interpolation + double time = root->getTime(); + if( time > beginTime && time <= endTime && totalTime > 0) + { + for (size_t i = 0; i< indices.size(); ++i) + { + x[indices[i]] = ((xf[indices[i]]-x0[indices[i]])*time + (x0[indices[i]]*endTime - xf[indices[i]]*beginTime))/totalTime; + } + } + else if (time > endTime) + { + for (size_t i = 0; i< indices.size(); ++i) + { + x[indices[i]] = xf[indices[i]]; + } + } +} + +template +void PatchTestMovementProjectiveConstraint::projectMatrix( sofa::linearalgebra::BaseMatrix* M, unsigned offset ) +{ + // clears the rows and columns associated with constrained particles + const unsigned blockSize = DataTypes::deriv_total_size; + + for (const auto id : d_indices.getValue()) + { + M->clearRowsCols( offset + id * blockSize, offset + (id+1) * blockSize ); + } + +} + +template +void PatchTestMovementProjectiveConstraint::getFinalPositions( VecCoord& finalPos,DataVecCoord& xData) +{ + // Indices of mesh points + const SetIndexArray & meshIndices = d_meshIndices.getValue(); + + // Initialize final positions + if(meshPointsXf.size()==0) + {this->initializeFinalPositions(meshIndices,xData,meshPointsX0,meshPointsXf);} + + // Set final positions + finalPos.resize(meshIndices.size()); + for (size_t i=0; i < meshIndices.size() ; ++i) + { + finalPos[meshIndices[i]] = meshPointsXf[meshIndices[i]]; + } +} + +template +void PatchTestMovementProjectiveConstraint::initializeInitialPositions (const SetIndexArray & indices, DataVecCoord& xData, VecCoord& x0) +{ + helper::WriteAccessor x = xData; + + x0.resize(x.size()); + for (size_t i=0; i < indices.size() ; ++i) + { + x0[indices[i]] = x[indices[i]]; + } + +} + +template +void PatchTestMovementProjectiveConstraint::initializeFinalPositions (const SetIndexArray & indices, DataVecCoord& xData, VecCoord& x0, VecCoord& xf) +{ + Deriv displacement; + helper::WriteAccessor x = xData; + + xf.resize(x.size()); + + // if the positions were not initialized + if(x0.size() == 0) + this->initializeInitialPositions(indices,xData,x0); + + for (size_t i=0; i < indices.size() ; ++i) + { + this->computeInterpolatedDisplacement(indices[i],xData,displacement); + xf[indices[i]] = x0[indices[i]] + displacement ; + } + +} + +template +void PatchTestMovementProjectiveConstraint::computeInterpolatedDisplacement(int pointIndice,const DataVecCoord& xData, Deriv& displacement) +{ + // For each mesh point compute the associated displacement + + // The 3 barycentric coefficients along x, y and z axis + Real alpha, beta, gamma; + + // Corner points + const VecCoord& cornerPoints = d_cornerPoints.getValue(); + if(cornerPoints.size()==0) + this->findCornerPoints(); + + if(cornerPoints.size() == 4) + { + Coord corner0 = cornerPoints[0]; + Coord corner1 = cornerPoints[1]; + Coord corner3 = cornerPoints[3]; + + // Coord of the point + helper::ReadAccessor x = xData; + Coord point = x[pointIndice]; + + // Compute alpha = barycentric coefficient along the x axis + alpha = fabs(point[0]-corner0[0])/fabs(corner1[0]-corner0[0]); + + // Compute beta = barycentric coefficient along the y axis + beta = fabs(point[1]-corner0[1])/fabs(corner3[1]-corner0[1]); + + // cornerMovements + const VecDeriv& cornerMovements = d_cornerMovements.getValue(); + + // Compute displacement by linear interpolation + displacement = cornerMovements[0]*(1-alpha)*(1-beta) + cornerMovements[1]*alpha*(1-beta)+ cornerMovements[2]*alpha*beta+cornerMovements[3]*(1-alpha)*beta; + } + + else if(cornerPoints.size() == 8) + { + Coord corner0 = cornerPoints[0]; + Coord corner1 = cornerPoints[1]; + Coord corner3 = cornerPoints[3]; + Coord corner4 = cornerPoints[4]; + + // Coord of the point + helper::ReadAccessor x = xData; + Coord point = x[pointIndice]; + + // Compute alpha = barycentric coefficient along the x axis + alpha = fabs(point[0]-corner0[0])/fabs(corner1[0]-corner0[0]); + + // Compute beta = barycentric coefficient along the y axis + beta = fabs(point[1]-corner0[1])/fabs(corner3[1]-corner0[1]); + + // Compute gamma = barycentric coefficient along the z axis + if( CoordSize>2 ) + gamma = fabs(point[2]-corner0[2])/fabs(corner4[2]-corner0[2]); // 3D + else + gamma = 0; // 2D + + // cornerMovements + const VecDeriv& cornerMovements = d_cornerMovements.getValue(); + + // Compute displacement by linear interpolation + displacement = (cornerMovements[0]*(1-alpha)*(1-beta) + cornerMovements[1]*alpha*(1-beta)+ cornerMovements[2]*alpha*beta+cornerMovements[3]*(1-alpha)*beta) * (1-gamma) + + (cornerMovements[4]*(1-alpha)*(1-beta) + cornerMovements[5]*alpha*(1-beta)+ cornerMovements[6]*alpha*beta+cornerMovements[7]*(1-alpha)*beta) * gamma; + } + else + { + msg_info() << "error don't find the corner points" ; + } + +} + +template +void PatchTestMovementProjectiveConstraint::draw(const core::visual::VisualParams* vparams) +{ + const SetIndexArray & indices = d_indices.getValue(); + const VecCoord& x = this->mstate->read(core::ConstVecCoordId::position())->getValue(); + type::Vec3 point; + + if(d_drawConstrainedPoints.getValue()) + { + std::vector< type::Vec3 > points; + for (unsigned int index : indices) + { + point = DataTypes::getCPos(x[index]); + points.push_back(point); + } + vparams->drawTool()->drawPoints(points, 10, sofa::type::RGBAColor(1,0.5,0.5,1)); + } +} + +} // namespace sofa::component::constraint::projective diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/ProjectToLineConstraint.cpp b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PlaneProjectiveConstraint.cpp similarity index 75% rename from Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/ProjectToLineConstraint.cpp rename to Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PlaneProjectiveConstraint.cpp index 5c701444225..11092578661 100644 --- a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/ProjectToLineConstraint.cpp +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PlaneProjectiveConstraint.cpp @@ -19,8 +19,8 @@ * * * Contact information: contact@sofa-framework.org * ******************************************************************************/ -#define SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_ProjectToLineConstraint_CPP -#include +#define SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_PlaneProjectiveConstraint_CPP +#include #include namespace sofa::component::constraint::projective @@ -30,13 +30,14 @@ using namespace sofa::defaulttype; using namespace sofa::helper; -int ProjectToLineConstraintClass = core::RegisterObject("Attach given particles to their initial positions") - .add< ProjectToLineConstraint >() - .add< ProjectToLineConstraint >() - +int PlaneProjectiveConstraintClass = core::RegisterObject("Attach given particles to their initial positions") + .add< PlaneProjectiveConstraint >() + .add< PlaneProjectiveConstraint >() + .addAlias("ProjectToPlaneConstraint") ; -template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API ProjectToLineConstraint; -template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API ProjectToLineConstraint; +template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PlaneProjectiveConstraint; +template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PlaneProjectiveConstraint; } // namespace sofa::component::constraint::projective + diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PlaneProjectiveConstraint.h b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PlaneProjectiveConstraint.h new file mode 100644 index 00000000000..13a325f7d8c --- /dev/null +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PlaneProjectiveConstraint.h @@ -0,0 +1,137 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace sofa::component::constraint::projective +{ + +/// This class can be overridden if needed for additionnal storage within template specializations. +template +class PlaneProjectiveConstraintInternalData +{ + +}; + +/** Project particles to an affine plane. + @author Francois Faure, 2012 + @todo Optimized versions for planes parallel to the main directions +*/ +template +class PlaneProjectiveConstraint : public core::behavior::ProjectiveConstraintSet +{ +public: + SOFA_CLASS(SOFA_TEMPLATE(PlaneProjectiveConstraint,DataTypes),SOFA_TEMPLATE(sofa::core::behavior::ProjectiveConstraintSet, DataTypes)); + + using Index = sofa::Index; + typedef typename DataTypes::VecCoord VecCoord; + typedef typename DataTypes::VecDeriv VecDeriv; + typedef typename DataTypes::MatrixDeriv MatrixDeriv; + typedef typename DataTypes::Coord Coord; + typedef typename DataTypes::Deriv Deriv; + typedef typename DataTypes::CPos CPos; + typedef typename MatrixDeriv::RowIterator MatrixDerivRowIterator; + typedef typename MatrixDeriv::RowType MatrixDerivRowType; + typedef Data DataVecCoord; + typedef Data DataVecDeriv; + typedef Data DataMatrixDeriv; + typedef type::vector Indices; + typedef sofa::core::topology::TopologySubsetIndices IndexSubsetData; + typedef linearalgebra::EigenBaseSparseMatrix BaseSparseMatrix; + typedef linearalgebra::EigenSparseMatrix SparseMatrix; + typedef typename SparseMatrix::Block Block; ///< projection matrix of a particle displacement to the plane + enum {bsize=SparseMatrix::Nin}; ///< size of a block + + SOFA_ATTRIBUTE_REPLACED__TYPEMEMBER(Vector3, sofa::type::Vec3); + +protected: + PlaneProjectiveConstraint(); + + virtual ~PlaneProjectiveConstraint(); + +public: + IndexSubsetData f_indices; ///< the particles to project + Data f_origin; ///< A point in the plane + Data f_normal; ///< The normal to the plane. Will be normalized by init(). + Data f_drawSize; ///< The size of the display of the constrained particles + + /// Link to be set to the topology container in the component graph. + SingleLink, sofa::core::topology::BaseMeshTopology, BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_topology; + +protected: + PlaneProjectiveConstraintInternalData* data; + friend class PlaneProjectiveConstraintInternalData; + + +public: + void clearConstraints(); + void addConstraint(Index index); + void removeConstraint(Index index); + + // -- Constraint interface + void init() override; + void reinit() override; + + void projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& resData) override; + void projectVelocity(const core::MechanicalParams* mparams, DataVecDeriv& vData) override; + void projectPosition(const core::MechanicalParams* mparams, DataVecCoord& xData) override; + void projectJacobianMatrix(const core::MechanicalParams* mparams, DataMatrixDeriv& cData) override; + + void applyConstraint(const core::MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) override; + void applyConstraint(const core::MechanicalParams* mparams, linearalgebra::BaseVector* vector, const sofa::core::behavior::MultiMatrixAccessor* matrix) override; + + /** Project the given matrix (Experimental API). + Replace M with PMP, where P is the projection matrix corresponding to the projectResponse method, shifted by the given offset, i.e. P is the identity matrix with a block on the diagonal replaced by the projection matrix. + */ + void projectMatrix( sofa::linearalgebra::BaseMatrix* /*M*/, unsigned /*offset*/ ) override; + + + void draw(const core::visual::VisualParams* vparams) override; + +protected : + + SparseMatrix jacobian; ///< projection matrix in local state + SparseMatrix J; ///< auxiliary variable +}; + + +#if !defined(SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_PlaneProjectiveConstraint_CPP) +extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PlaneProjectiveConstraint; +extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PlaneProjectiveConstraint; + +#endif + +} // namespace sofa::component::constraint::projective + diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PlaneProjectiveConstraint.inl b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PlaneProjectiveConstraint.inl new file mode 100644 index 00000000000..9ba0cae1e0d --- /dev/null +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PlaneProjectiveConstraint.inl @@ -0,0 +1,276 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace sofa::component::constraint::projective +{ + +template +PlaneProjectiveConstraint::PlaneProjectiveConstraint() + : core::behavior::ProjectiveConstraintSet(nullptr) + , f_indices( initData(&f_indices,"indices","Indices of the fixed points") ) + , f_origin( initData(&f_origin,CPos(),"origin","A point in the plane")) + , f_normal( initData(&f_normal,CPos(),"normal","Normal vector to the plane")) + , f_drawSize( initData(&f_drawSize,(SReal)0.0,"drawSize","0 -> point based rendering, >0 -> radius of spheres") ) + , l_topology(initLink("topology", "link to the topology container")) + , data(new PlaneProjectiveConstraintInternalData()) +{ + f_indices.beginEdit()->push_back(0); + f_indices.endEdit(); +} + + +template +PlaneProjectiveConstraint::~PlaneProjectiveConstraint() +{ + delete data; +} + +template +void PlaneProjectiveConstraint::clearConstraints() +{ + f_indices.beginEdit()->clear(); + f_indices.endEdit(); +} + +template +void PlaneProjectiveConstraint::addConstraint(Index index) +{ + f_indices.beginEdit()->push_back(index); + f_indices.endEdit(); +} + +template +void PlaneProjectiveConstraint::removeConstraint(Index index) +{ + sofa::type::removeValue(*f_indices.beginEdit(),index); + f_indices.endEdit(); +} + +// -- Constraint interface + + +template +void PlaneProjectiveConstraint::init() +{ + this->core::behavior::ProjectiveConstraintSet::init(); + + if (l_topology.empty()) + { + msg_info() << "link to Topology container should be set to ensure right behavior. First Topology found in current context will be used."; + l_topology.set(this->getContext()->getMeshTopologyLink()); + } + + if (sofa::core::topology::BaseMeshTopology* _topology = l_topology.get()) + { + msg_info() << "Topology path used: '" << l_topology.getLinkedPath() << "'"; + + // Initialize topological changes support + f_indices.createTopologyHandler(_topology); + } + else + { + msg_info() << "No topology component found at path: " << l_topology.getLinkedPath() << ", nor in current context: " << this->getContext()->name; + } + + const Indices & indices = f_indices.getValue(); + + const Index maxIndex=this->mstate->getSize(); + for (unsigned int i=0; i= maxIndex) + { + msg_error() << "Index " << index << " not valid!"; + removeConstraint(index); + } + } + + reinit(); + +} + +template +void PlaneProjectiveConstraint::reinit() +{ + + // normalize the normal vector + CPos n = f_normal.getValue(); + if( n.norm()==0 ) + n[1]=0; + else n *= 1/n.norm(); + f_normal.setValue(n); + + // create the matrix blocks corresponding to the projection to the plane: I-nn^t or to the identity + Block bProjection; + for(unsigned i=0; imstate->getSize(); + const unsigned blockSize = DataTypes::deriv_total_size; + jacobian.resize( numBlocks*blockSize,numBlocks*blockSize ); + + // fill the jacobian in ascending order + unsigned i=0; + Indices::const_iterator it = tmp.begin(); + while( i +void PlaneProjectiveConstraint::projectMatrix( sofa::linearalgebra::BaseMatrix* M, unsigned offset ) +{ + J.copy(jacobian, M->colSize(), offset); // projection matrix for an assembled state + BaseSparseMatrix* E = dynamic_cast(M); + assert(E); + E->compressedMatrix = J.compressedMatrix * E->compressedMatrix * J.compressedMatrix; +} + + + +template +void PlaneProjectiveConstraint::projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& resData) +{ + SOFA_UNUSED(mparams); + + helper::WriteAccessor res ( resData ); + jacobian.mult(res.wref(),res.ref()); +} + +template +void PlaneProjectiveConstraint::projectJacobianMatrix(const core::MechanicalParams* /*mparams*/ , DataMatrixDeriv& /*cData*/) +{ + msg_error() << "projectJacobianMatrix(const core::MechanicalParams*, DataMatrixDeriv& ) is not implemented"; +} + +template +void PlaneProjectiveConstraint::projectVelocity(const core::MechanicalParams* mparams, DataVecDeriv& vData) +{ + projectResponse(mparams,vData); +} + +template +void PlaneProjectiveConstraint::projectPosition(const core::MechanicalParams* /*mparams*/ , DataVecCoord& xData) +{ + VecCoord& x = *xData.beginEdit(); + + const CPos& n = f_normal.getValue(); + const CPos& o = f_origin.getValue(); + + const Indices& indices = f_indices.getValue(); + for(unsigned i=0; i +void PlaneProjectiveConstraint::applyConstraint(const core::MechanicalParams* /*mparams*/, const sofa::core::behavior::MultiMatrixAccessor* /*matrix*/) +{ + msg_error() << "applyConstraint is not implemented "; +} + +template +void PlaneProjectiveConstraint::applyConstraint(const core::MechanicalParams* /*mparams*/, linearalgebra::BaseVector* /*vector*/, const sofa::core::behavior::MultiMatrixAccessor* /*matrix*/) +{ + msg_error() << "PlaneProjectiveConstraint::applyConstraint(const core::MechanicalParams* mparams, linearalgebra::BaseVector* vector, const sofa::core::behavior::MultiMatrixAccessor* matrix) is not implemented "; +} + +template +void PlaneProjectiveConstraint::draw(const core::visual::VisualParams* vparams) +{ + if (!vparams->displayFlags().getShowBehaviorModels()) return; + if (!this->isActive()) return; + const VecCoord& x = this->mstate->read(core::ConstVecCoordId::position())->getValue(); + + const auto stateLifeCycle = vparams->drawTool()->makeStateLifeCycle(); + + const Indices & indices = f_indices.getValue(); + + if( f_drawSize.getValue() == 0) // old classical drawing by points + { + std::vector< sofa::type::Vec3 > points; + sofa::type::Vec3 point; + for (unsigned int index : indices) + { + point = DataTypes::getCPos(x[index]); + points.push_back(point); + } + vparams->drawTool()->drawPoints(points, 10, sofa::type::RGBAColor(1,0.5,0.5,1)); + } + else // new drawing by spheres + { + std::vector< sofa::type::Vec3 > points; + sofa::type::Vec3 point; + + for (unsigned int index : indices) + { + point = DataTypes::getCPos(x[index]); + points.push_back(point); + } + vparams->drawTool()->drawSpheres(points, (float)f_drawSize.getValue(), sofa::type::RGBAColor(1.0f,0.35f,0.35f,1.0f)); + } + + +} + +} // namespace sofa::component::constraint::projective diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/ProjectToPointConstraint.cpp b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PointProjectiveConstraint.cpp similarity index 68% rename from Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/ProjectToPointConstraint.cpp rename to Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PointProjectiveConstraint.cpp index e76a0ffe931..2622f32391d 100644 --- a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/ProjectToPointConstraint.cpp +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PointProjectiveConstraint.cpp @@ -19,8 +19,8 @@ * * * Contact information: contact@sofa-framework.org * ******************************************************************************/ -#define SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_ProjectToPointConstraint_CPP -#include +#define SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_PointProjectiveConstraint_CPP +#include #include #include @@ -32,17 +32,17 @@ using namespace sofa::defaulttype; using namespace sofa::helper; -int ProjectToPointConstraintClass = core::RegisterObject("Project particles to a point") - .add< ProjectToPointConstraint >() - .add< ProjectToPointConstraint >() - .add< ProjectToPointConstraint >() - .add< ProjectToPointConstraint >() - +int PointProjectiveConstraintClass = core::RegisterObject("Project particles to a point") + .add< PointProjectiveConstraint >() + .add< PointProjectiveConstraint >() + .add< PointProjectiveConstraint >() + .add< PointProjectiveConstraint >() + .addAlias("ProjectToPointConstraint") ; -template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API ProjectToPointConstraint; -template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API ProjectToPointConstraint; -template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API ProjectToPointConstraint; -template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API ProjectToPointConstraint; +template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PointProjectiveConstraint; +template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PointProjectiveConstraint; +template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PointProjectiveConstraint; +template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PointProjectiveConstraint; } // namespace sofa::component::constraint::projective diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PointProjectiveConstraint.h b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PointProjectiveConstraint.h new file mode 100644 index 00000000000..2055e7fe236 --- /dev/null +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PointProjectiveConstraint.h @@ -0,0 +1,130 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace sofa::component::constraint::projective +{ + +/// This class can be overridden if needed for additionnal storage within template specializations. +template +class PointProjectiveConstraintInternalData +{ + +}; + +/** Attach given particles to their initial positions. + * Contrary to FixedConstraint, this one stops the particles even if they have a non-null initial velocity. + * @sa FixedConstraint +*/ +template +class PointProjectiveConstraint : public core::behavior::ProjectiveConstraintSet +{ +public: + SOFA_CLASS(SOFA_TEMPLATE(PointProjectiveConstraint,DataTypes),SOFA_TEMPLATE(sofa::core::behavior::ProjectiveConstraintSet, DataTypes)); + + using Index = sofa::Index; + typedef typename DataTypes::VecCoord VecCoord; + typedef typename DataTypes::VecDeriv VecDeriv; + typedef typename DataTypes::MatrixDeriv MatrixDeriv; + typedef typename DataTypes::Coord Coord; + typedef typename DataTypes::Deriv Deriv; + typedef typename MatrixDeriv::RowIterator MatrixDerivRowIterator; + typedef typename MatrixDeriv::RowType MatrixDerivRowType; + typedef Data DataVecCoord; + typedef Data DataVecDeriv; + typedef Data DataMatrixDeriv; + typedef type::vector SetIndexArray; + typedef sofa::core::topology::TopologySubsetIndices SetIndex; + + SOFA_ATTRIBUTE_REPLACED__TYPEMEMBER(Vector3, sofa::type::Vec3); + +protected: + PointProjectiveConstraint(); + + virtual ~PointProjectiveConstraint(); + +public: + SetIndex f_indices; ///< the indices of the points to project to the target + Data f_point; ///< the target of the projection + Data f_fixAll; ///< to project all the points, rather than those listed in f_indices + Data f_drawSize; ///< 0 -> point based rendering, >0 -> radius of spheres + + /// Link to be set to the topology container in the component graph. + SingleLink, sofa::core::topology::BaseMeshTopology, BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_topology; + +protected: + PointProjectiveConstraintInternalData* data; + friend class PointProjectiveConstraintInternalData; + + +public: + void clearConstraints(); + void addConstraint(Index index); + void removeConstraint(Index index); + + // -- Constraint interface + void init() override; + void reinit() override; + + void projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& resData) override; + void projectVelocity(const core::MechanicalParams* mparams, DataVecDeriv& vData) override; + void projectPosition(const core::MechanicalParams* mparams, DataVecCoord& xData) override; + void projectJacobianMatrix(const core::MechanicalParams* mparams, DataMatrixDeriv& cData) override; + + void applyConstraint(const core::MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) override; + void applyConstraint(const core::MechanicalParams* mparams, linearalgebra::BaseVector* vector, const sofa::core::behavior::MultiMatrixAccessor* matrix) override; + + /** Project the given matrix (Experimental API). + Replace M with PMP, where P is the projection matrix corresponding to the projectResponse method, shifted by the given offset, i.e. P is the identity matrix with a block on the diagonal replaced by the projection matrix. + */ + void projectMatrix( sofa::linearalgebra::BaseMatrix* /*M*/, unsigned /*offset*/ ) override; + + + void draw(const core::visual::VisualParams* vparams) override; + + bool fixAllDOFs() const { return f_fixAll.getValue(); } + +protected : + +}; + + +#if !defined(SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_PointProjectiveConstraint_CPP) +extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PointProjectiveConstraint; +extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PointProjectiveConstraint; +extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PointProjectiveConstraint; +extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PointProjectiveConstraint; +#endif + +} // namespace sofa::component::constraint::projective diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PointProjectiveConstraint.inl b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PointProjectiveConstraint.inl new file mode 100644 index 00000000000..b50b5aedd38 --- /dev/null +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PointProjectiveConstraint.inl @@ -0,0 +1,324 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + + +namespace sofa::component::constraint::projective +{ + +template +PointProjectiveConstraint::PointProjectiveConstraint() + : core::behavior::ProjectiveConstraintSet(nullptr) + , f_indices( initData(&f_indices,"indices","Indices of the points to project") ) + , f_point( initData(&f_point,"point","Target of the projection") ) + , f_fixAll( initData(&f_fixAll,false,"fixAll","filter all the DOF to implement a fixed object") ) + , f_drawSize( initData(&f_drawSize,(SReal)0.0,"drawSize","0 -> point based rendering, >0 -> radius of spheres") ) + , l_topology(initLink("topology", "link to the topology container")) + , data(new PointProjectiveConstraintInternalData()) +{ + f_indices.beginEdit()->push_back(0); + f_indices.endEdit(); +} + + +template +PointProjectiveConstraint::~PointProjectiveConstraint() +{ + delete data; +} + +template +void PointProjectiveConstraint::clearConstraints() +{ + f_indices.beginEdit()->clear(); + f_indices.endEdit(); +} + +template +void PointProjectiveConstraint::addConstraint(Index index) +{ + f_indices.beginEdit()->push_back(index); + f_indices.endEdit(); +} + +template +void PointProjectiveConstraint::removeConstraint(Index index) +{ + sofa::type::removeValue(*f_indices.beginEdit(),index); + f_indices.endEdit(); +} + +// -- Constraint interface + + +template +void PointProjectiveConstraint::init() +{ + this->core::behavior::ProjectiveConstraintSet::init(); + + if (l_topology.empty()) + { + msg_info() << "link to Topology container should be set to ensure right behavior. First Topology found in current context will be used."; + l_topology.set(this->getContext()->getMeshTopologyLink()); + } + + if (sofa::core::topology::BaseMeshTopology* _topology = l_topology.get()) + { + msg_info() << "Topology path used: '" << l_topology.getLinkedPath() << "'"; + + // Initialize topological changes support + f_indices.createTopologyHandler(_topology); + } + else + { + msg_info() << "No topology component found at path: " << l_topology.getLinkedPath() << ", nor in current context: " << this->getContext()->name; + } + + const SetIndexArray & indices = f_indices.getValue(); + + std::stringstream sstream; + const Index maxIndex=this->mstate->getSize(); + for (unsigned int i=0; i= maxIndex) + { + sstream << "Index " << index << " not valid!\n"; + removeConstraint(index); + } + } + msg_error_when(!sstream.str().empty()) << sstream.str(); + + reinit(); +} + +template +void PointProjectiveConstraint::reinit() +{ + + // get the indices sorted + SetIndexArray tmp = f_indices.getValue(); + std::sort(tmp.begin(),tmp.end()); +} + +template +void PointProjectiveConstraint::projectMatrix( sofa::linearalgebra::BaseMatrix* M, unsigned offset ) +{ + const unsigned blockSize = DataTypes::deriv_total_size; + + // clears the rows and columns associated with fixed particles + for (const auto id : f_indices.getValue()) + { + M->clearRowsCols( offset + id * blockSize, offset + (id+1) * blockSize ); + } +} + +template +void PointProjectiveConstraint::projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& resData) +{ + SOFA_UNUSED(mparams); + + helper::WriteAccessor res ( resData ); + const SetIndexArray & indices = f_indices.getValue(); + if( f_fixAll.getValue() ) + { + // fix everything + typename VecDeriv::iterator it; + for( it = res.begin(); it != res.end(); ++it ) + { + *it = Deriv(); + } + } + else + { + for (SetIndexArray::const_iterator it = indices.begin(); + it != indices.end(); + ++it) + { + res[*it] = Deriv(); + } + } +} + +template +void PointProjectiveConstraint::projectJacobianMatrix(const core::MechanicalParams* mparams, DataMatrixDeriv& cData) +{ + SOFA_UNUSED(mparams); + + helper::WriteAccessor c ( cData ); + + if( f_fixAll.getValue() ) + { + // fix everything + c->clear(); + } + else + { + const SetIndexArray& indices = f_indices.getValue(); + for (SetIndexArray::const_iterator it = indices.begin(); + it != indices.end(); + ++it) + { + c->clearColBlock(*it); + } + } +} + +template +void PointProjectiveConstraint::projectVelocity(const core::MechanicalParams* mparams , DataVecDeriv& vData) +{ + projectResponse(mparams, vData); +} + +template +void PointProjectiveConstraint::projectPosition(const core::MechanicalParams* mparams, DataVecCoord& xData) +{ + SOFA_UNUSED(mparams); + + helper::WriteAccessor res ( xData ); + const SetIndexArray & indices = f_indices.getValue(); + if( f_fixAll.getValue() ) + { + // fix everything + typename VecCoord::iterator it; + for( it = res.begin(); it != res.end(); ++it ) + { + *it = f_point.getValue(); + } + } + else + { + for (SetIndexArray::const_iterator it = indices.begin(); + it != indices.end(); + ++it) + { + res[*it] = f_point.getValue(); + } + } +} + +template +void PointProjectiveConstraint::applyConstraint(const core::MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) +{ + SOFA_UNUSED(mparams); + if(const core::behavior::MultiMatrixAccessor::MatrixRef r = matrix->getMatrix(this->mstate.get())) + { + const unsigned int N = Deriv::size(); + const SetIndexArray & indices = f_indices.getValue(); + + for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) + { + // Reset Fixed Row and Col + for (unsigned int c=0; cclearRowCol(r.offset + N * (*it) + c); + // Set Fixed Vertex + for (unsigned int c=0; cset(r.offset + N * (*it) + c, r.offset + N * (*it) + c, 1.0); + } + } +} + +template +void PointProjectiveConstraint::applyConstraint(const core::MechanicalParams* mparams, linearalgebra::BaseVector* vector, const sofa::core::behavior::MultiMatrixAccessor* matrix) +{ + SOFA_UNUSED(mparams); + const int o = matrix->getGlobalOffset(this->mstate.get()); + if (o >= 0) + { + const unsigned int offset = (unsigned int)o; + const unsigned int N = Deriv::size(); + + const SetIndexArray & indices = f_indices.getValue(); + for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) + { + for (unsigned int c=0; cclear(offset + N * (*it) + c); + } + } +} + + + + +template +void PointProjectiveConstraint::draw(const core::visual::VisualParams* vparams) +{ + if (!vparams->displayFlags().getShowBehaviorModels()) return; + if (!this->isActive()) return; + const VecCoord& x = this->mstate->read(core::ConstVecCoordId::position())->getValue(); + const SetIndexArray & indices = f_indices.getValue(); + + const auto stateLifeCycle = vparams->drawTool()->makeStateLifeCycle(); + + if( f_drawSize.getValue() == 0) // old classical drawing by points + { + std::vector< sofa::type::Vec3 > points; + sofa::type::Vec3 point; + if( f_fixAll.getValue() ) + for (unsigned i=0; idrawTool()->drawPoints(points, 10, sofa::type::RGBAColor(1,0.5,0.5,1)); + } + else // new drawing by spheres + { + std::vector< sofa::type::Vec3 > points; + sofa::type::Vec3 point; + if(f_fixAll.getValue()) + for (unsigned i=0; idrawTool()->drawSpheres(points, (float)f_drawSize.getValue(), sofa::type::RGBAColor(1.0f,0.35f,0.35f,1.0f)); + } + + +} + +} // namespace sofa::component::constraint::projective + + + diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PositionBasedDynamicsConstraint.h b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PositionBasedDynamicsConstraint.h index d861e5a7a72..f27bc01bb64 100644 --- a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PositionBasedDynamicsConstraint.h +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PositionBasedDynamicsConstraint.h @@ -20,95 +20,13 @@ * Contact information: contact@sofa-framework.org * ******************************************************************************/ #pragma once -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include -namespace sofa::component::constraint::projective -{ - -/// This class can be overridden if needed for additionnal storage within template specializations. -template -class PositionBasedDynamicsConstraintInternalData -{ -}; - -/** Position-based dynamics as described in [Muller06]: -input: target positions X -output : x(t) <- x(t) + stiffness.( X - x(t) ) - v(t) <- [ x(t) - x(t-1) ] / dt = v(t) + stiffness.( X - x(t) ) /dt - -*/ +SOFA_DEPRECATED_HEADER("v24.06", "v25.06", "sofa/component/constraint/projective/PositionBasedDynamicsProjectiveConstraint.h") -template -class PositionBasedDynamicsConstraint : public core::behavior::ProjectiveConstraintSet +namespace sofa::component::constraint::projective { -public: - SOFA_CLASS(SOFA_TEMPLATE(PositionBasedDynamicsConstraint,DataTypes),SOFA_TEMPLATE(sofa::core::behavior::ProjectiveConstraintSet, DataTypes)); - - typedef typename DataTypes::VecCoord VecCoord; - typedef typename DataTypes::VecDeriv VecDeriv; - typedef typename DataTypes::MatrixDeriv MatrixDeriv; - typedef typename DataTypes::Coord Coord; - typedef typename DataTypes::Deriv Deriv; - typedef typename DataTypes::Real Real; - typedef typename MatrixDeriv::RowIterator MatrixDerivRowIterator; - typedef typename MatrixDeriv::RowType MatrixDerivRowType; - typedef Data DataVecCoord; - typedef Data DataVecDeriv; - typedef Data DataMatrixDeriv; - -protected: - PositionBasedDynamicsConstraintInternalData data; - friend class PositionBasedDynamicsConstraintInternalData; - -public: - Data< Real > stiffness; ///< Blending between current pos and target pos. - Data< VecCoord > position; ///< Target positions. - - Data < VecDeriv > velocity; ///< Velocities. - Data < VecCoord > old_position; ///< Old positions. - - PositionBasedDynamicsConstraint(); - - virtual ~PositionBasedDynamicsConstraint(); - - // -- Constraint interface - void init() override; - void reset() override; - - void projectResponse(const core::MechanicalParams* , DataVecDeriv& ) override {} - void projectVelocity(const core::MechanicalParams* mparams, DataVecDeriv& vData) override; - void projectPosition(const core::MechanicalParams* mparams, DataVecCoord& xData) override; - void projectJacobianMatrix(const core::MechanicalParams* mparams, DataMatrixDeriv& cData) override; - - // Handle topological changes - void handleTopologyChange() override; - -protected : - - - -}; - - -#if !defined(SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_POSITIONBASEDDYNAMICSCONSTRAINT_CPP) -extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PositionBasedDynamicsConstraint; -extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PositionBasedDynamicsConstraint; -extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PositionBasedDynamicsConstraint; -extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PositionBasedDynamicsConstraint; -extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PositionBasedDynamicsConstraint; -//extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PositionBasedDynamicsConstraint; - -#endif - -} // namespace sofa::component::constraint::projective +template +using PositionBasedDynamicsConstraint SOFA_ATTRIBUTE_DEPRECATED("v24.06 ", "v25.06", "PositionBasedDynamicsConstraint has been renamed to PositionBasedDynamicsProjectiveConstraint") = PositionBasedDynamicsProjectiveConstraint; +} \ No newline at end of file diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PositionBasedDynamicsConstraint.inl b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PositionBasedDynamicsConstraint.inl index 80117815e5d..35018c27864 100644 --- a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PositionBasedDynamicsConstraint.inl +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PositionBasedDynamicsConstraint.inl @@ -21,118 +21,6 @@ ******************************************************************************/ #pragma once -#include -#include -#include -#include +#include -namespace sofa::component::constraint::projective -{ - - -template -PositionBasedDynamicsConstraint::PositionBasedDynamicsConstraint() - : core::behavior::ProjectiveConstraintSet(nullptr) - , stiffness(initData(&stiffness,(Real)1.0,"stiffness","Blending between current pos and target pos.")) - , position(initData(&position,"position","Target positions.")) - , velocity(initData(&velocity,"velocity","Velocities.")) - , old_position(initData(&old_position,"old_position","Old positions.")) -{ -} - - -// Handle topological changes -template void PositionBasedDynamicsConstraint::handleTopologyChange() -{ - this->reinit(); -} - -template -PositionBasedDynamicsConstraint::~PositionBasedDynamicsConstraint() -{ -} - - -// -- Constraint interface - - -template -void PositionBasedDynamicsConstraint::init() -{ - this->core::behavior::ProjectiveConstraintSet::init(); - if ((int)position.getValue().size() != (int)this->mstate->getSize()) msg_error() << "Invalid target position vector size." ; -} - - -template -void PositionBasedDynamicsConstraint::reset() -{ - this->core::behavior::ProjectiveConstraintSet::reset(); - - helper::WriteAccessor vel ( velocity ); - std::fill(vel.begin(),vel.end(),Deriv()); - - helper::WriteAccessor old_pos ( old_position ); - const VecCoord& x = this->mstate->read(core::ConstVecCoordId::position())->getValue(); - old_pos.resize(x.size()); - std::copy(x.begin(),x.end(),old_pos.begin()); -} - - - -template -void PositionBasedDynamicsConstraint::projectJacobianMatrix(const core::MechanicalParams* mparams, DataMatrixDeriv& cData) -{ - SOFA_UNUSED(mparams); - helper::WriteAccessor c ( cData ); -} - - - -template -void PositionBasedDynamicsConstraint::projectVelocity(const core::MechanicalParams* mparams, DataVecDeriv& vData) -{ - SOFA_UNUSED(mparams); - - helper::WriteAccessor res (vData ); - helper::ReadAccessor vel ( velocity ); - - if (vel.size() != res.size()) { msg_error() << "Invalid target position vector size." ; return; } - std::copy(vel.begin(),vel.end(),res.begin()); -} - -template -void PositionBasedDynamicsConstraint::projectPosition(const core::MechanicalParams* mparams, DataVecCoord& xData) -{ - SOFA_UNUSED(mparams); - - helper::WriteAccessor res ( xData ); - helper::WriteAccessor vel ( velocity ); - helper::WriteAccessor old_pos ( old_position ); - helper::ReadAccessor tpos = position ; - if (tpos.size() != res.size()) { msg_error() << "Invalid target position vector size." ; return; } - - Real dt = (Real)this->getContext()->getDt(); - if(!dt) return; - Real invdt=(Real)(1./dt); - - vel.resize(res.size()); - - if(old_pos.size() != res.size()) { - old_pos.resize(res.size()); - std::copy(res.begin(),res.end(),old_pos.begin()); - } - - for( size_t i=0; i -void PositionBasedDynamicsConstraint::projectPosition(const core::MechanicalParams* mparams, DataVecCoord& xData); - -} // namespace sofa::component::constraint::projective +SOFA_DEPRECATED_HEADER("v24.06", "v25.06", "sofa/component/constraint/projective/PositionBasedDynamicsProjectiveConstraint.inl") diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PositionBasedDynamicsConstraint.cpp b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PositionBasedDynamicsProjectiveConstraint.cpp similarity index 79% rename from Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PositionBasedDynamicsConstraint.cpp rename to Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PositionBasedDynamicsProjectiveConstraint.cpp index 8861bb04b35..c8c2c4a7b77 100644 --- a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PositionBasedDynamicsConstraint.cpp +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PositionBasedDynamicsProjectiveConstraint.cpp @@ -19,8 +19,8 @@ * * * Contact information: contact@sofa-framework.org * ******************************************************************************/ -#define SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_POSITIONBASEDDYNAMICSCONSTRAINT_CPP -#include +#define SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_POSITIONBASEDDYNAMICSPROJECTIVECONSTRAINT_CPP +#include #include #include @@ -35,30 +35,30 @@ using namespace sofa::defaulttype; using namespace sofa::helper; -int PositionBasedDynamicsConstraintClass = core::RegisterObject("Position-based dynamics") +int PositionBasedDynamicsProjectiveConstraintClass = core::RegisterObject("Position-based dynamics") - .add< PositionBasedDynamicsConstraint >(true) - .add< PositionBasedDynamicsConstraint >() - .add< PositionBasedDynamicsConstraint >() - .add< PositionBasedDynamicsConstraint >() - .add< PositionBasedDynamicsConstraint >() -//.add< PositionBasedDynamicsConstraint >() + .add< PositionBasedDynamicsProjectiveConstraint >(true) + .add< PositionBasedDynamicsProjectiveConstraint >() + .add< PositionBasedDynamicsProjectiveConstraint >() + .add< PositionBasedDynamicsProjectiveConstraint >() + .add< PositionBasedDynamicsProjectiveConstraint >() + .addAlias("PositionBasedDynamicsConstraint") ; -template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PositionBasedDynamicsConstraint; -template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PositionBasedDynamicsConstraint; -template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PositionBasedDynamicsConstraint; -template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PositionBasedDynamicsConstraint; -template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PositionBasedDynamicsConstraint; -//template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PositionBasedDynamicsConstraint; +template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PositionBasedDynamicsProjectiveConstraint; +template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PositionBasedDynamicsProjectiveConstraint; +template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PositionBasedDynamicsProjectiveConstraint; +template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PositionBasedDynamicsProjectiveConstraint; +template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PositionBasedDynamicsProjectiveConstraint; +//template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PositionBasedDynamicsProjectiveConstraint; // specialization for rigids template <> -void PositionBasedDynamicsConstraint::projectPosition(const core::MechanicalParams* mparams, DataVecCoord& xData) +void PositionBasedDynamicsProjectiveConstraint::projectPosition(const core::MechanicalParams* mparams, DataVecCoord& xData) { SOFA_UNUSED(mparams); diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PositionBasedDynamicsProjectiveConstraint.h b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PositionBasedDynamicsProjectiveConstraint.h new file mode 100644 index 00000000000..ad225656f55 --- /dev/null +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PositionBasedDynamicsProjectiveConstraint.h @@ -0,0 +1,114 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace sofa::component::constraint::projective +{ + +/// This class can be overridden if needed for additionnal storage within template specializations. +template +class PositionBasedDynamicsProjectiveConstraintInternalData +{ +}; + +/** Position-based dynamics as described in [Muller06]: +input: target positions X +output : x(t) <- x(t) + stiffness.( X - x(t) ) + v(t) <- [ x(t) - x(t-1) ] / dt = v(t) + stiffness.( X - x(t) ) /dt + +*/ + +template +class PositionBasedDynamicsProjectiveConstraint : public core::behavior::ProjectiveConstraintSet +{ +public: + SOFA_CLASS(SOFA_TEMPLATE(PositionBasedDynamicsProjectiveConstraint,DataTypes),SOFA_TEMPLATE(sofa::core::behavior::ProjectiveConstraintSet, DataTypes)); + + typedef typename DataTypes::VecCoord VecCoord; + typedef typename DataTypes::VecDeriv VecDeriv; + typedef typename DataTypes::MatrixDeriv MatrixDeriv; + typedef typename DataTypes::Coord Coord; + typedef typename DataTypes::Deriv Deriv; + typedef typename DataTypes::Real Real; + typedef typename MatrixDeriv::RowIterator MatrixDerivRowIterator; + typedef typename MatrixDeriv::RowType MatrixDerivRowType; + typedef Data DataVecCoord; + typedef Data DataVecDeriv; + typedef Data DataMatrixDeriv; + +protected: + PositionBasedDynamicsProjectiveConstraintInternalData data; + friend class PositionBasedDynamicsProjectiveConstraintInternalData; + +public: + Data< Real > stiffness; ///< Blending between current pos and target pos. + Data< VecCoord > position; ///< Target positions. + + Data < VecDeriv > velocity; ///< Velocities. + Data < VecCoord > old_position; ///< Old positions. + + PositionBasedDynamicsProjectiveConstraint(); + + virtual ~PositionBasedDynamicsProjectiveConstraint(); + + // -- Constraint interface + void init() override; + void reset() override; + + void projectResponse(const core::MechanicalParams* , DataVecDeriv& ) override {} + void projectVelocity(const core::MechanicalParams* mparams, DataVecDeriv& vData) override; + void projectPosition(const core::MechanicalParams* mparams, DataVecCoord& xData) override; + void projectJacobianMatrix(const core::MechanicalParams* mparams, DataMatrixDeriv& cData) override; + + // Handle topological changes + void handleTopologyChange() override; + +protected : + + + +}; + + +#if !defined(SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_POSITIONBASEDDYNAMICSPROJECTIVECONSTRAINT_CPP) +extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PositionBasedDynamicsProjectiveConstraint; +extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PositionBasedDynamicsProjectiveConstraint; +extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PositionBasedDynamicsProjectiveConstraint; +extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PositionBasedDynamicsProjectiveConstraint; +extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PositionBasedDynamicsProjectiveConstraint; +//extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API PositionBasedDynamicsProjectiveConstraint; +#endif + + +} // namespace sofa::component::constraint::projective diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PositionBasedDynamicsProjectiveConstraint.inl b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PositionBasedDynamicsProjectiveConstraint.inl new file mode 100644 index 00000000000..473b6ceafa1 --- /dev/null +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/PositionBasedDynamicsProjectiveConstraint.inl @@ -0,0 +1,138 @@ +/****************************************************************************** +* 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 + +namespace sofa::component::constraint::projective +{ + + +template +PositionBasedDynamicsProjectiveConstraint::PositionBasedDynamicsProjectiveConstraint() + : core::behavior::ProjectiveConstraintSet(nullptr) + , stiffness(initData(&stiffness,(Real)1.0,"stiffness","Blending between current pos and target pos.")) + , position(initData(&position,"position","Target positions.")) + , velocity(initData(&velocity,"velocity","Velocities.")) + , old_position(initData(&old_position,"old_position","Old positions.")) +{ +} + + +// Handle topological changes +template void PositionBasedDynamicsProjectiveConstraint::handleTopologyChange() +{ + this->reinit(); +} + +template +PositionBasedDynamicsProjectiveConstraint::~PositionBasedDynamicsProjectiveConstraint() +{ +} + + +// -- Constraint interface + + +template +void PositionBasedDynamicsProjectiveConstraint::init() +{ + this->core::behavior::ProjectiveConstraintSet::init(); + if ((int)position.getValue().size() != (int)this->mstate->getSize()) msg_error() << "Invalid target position vector size." ; +} + + +template +void PositionBasedDynamicsProjectiveConstraint::reset() +{ + this->core::behavior::ProjectiveConstraintSet::reset(); + + helper::WriteAccessor vel ( velocity ); + std::fill(vel.begin(),vel.end(),Deriv()); + + helper::WriteAccessor old_pos ( old_position ); + const VecCoord& x = this->mstate->read(core::ConstVecCoordId::position())->getValue(); + old_pos.resize(x.size()); + std::copy(x.begin(),x.end(),old_pos.begin()); +} + + + +template +void PositionBasedDynamicsProjectiveConstraint::projectJacobianMatrix(const core::MechanicalParams* mparams, DataMatrixDeriv& cData) +{ + SOFA_UNUSED(mparams); + helper::WriteAccessor c ( cData ); +} + + + +template +void PositionBasedDynamicsProjectiveConstraint::projectVelocity(const core::MechanicalParams* mparams, DataVecDeriv& vData) +{ + SOFA_UNUSED(mparams); + + helper::WriteAccessor res (vData ); + helper::ReadAccessor vel ( velocity ); + + if (vel.size() != res.size()) { msg_error() << "Invalid target position vector size." ; return; } + std::copy(vel.begin(),vel.end(),res.begin()); +} + +template +void PositionBasedDynamicsProjectiveConstraint::projectPosition(const core::MechanicalParams* mparams, DataVecCoord& xData) +{ + SOFA_UNUSED(mparams); + + helper::WriteAccessor res ( xData ); + helper::WriteAccessor vel ( velocity ); + helper::WriteAccessor old_pos ( old_position ); + helper::ReadAccessor tpos = position ; + if (tpos.size() != res.size()) { msg_error() << "Invalid target position vector size." ; return; } + + Real dt = (Real)this->getContext()->getDt(); + if(!dt) return; + Real invdt=(Real)(1./dt); + + vel.resize(res.size()); + + if(old_pos.size() != res.size()) { + old_pos.resize(res.size()); + std::copy(res.begin(),res.end(),old_pos.begin()); + } + + for( size_t i=0; i +void PositionBasedDynamicsProjectiveConstraint::projectPosition(const core::MechanicalParams* mparams, DataVecCoord& xData); + +} // namespace sofa::component::constraint::projective diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/ProjectDirectionConstraint.h b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/ProjectDirectionConstraint.h index 73b8f78525c..53470fae7fd 100644 --- a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/ProjectDirectionConstraint.h +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/ProjectDirectionConstraint.h @@ -20,115 +20,13 @@ * Contact information: contact@sofa-framework.org * ******************************************************************************/ #pragma once -#include -#include -#include -#include -#include -#include -#include -#include -//#include -#include -#include -#include -#include -#include +#include -namespace sofa::component::constraint::projective -{ - -/// This class can be overridden if needed for additionnal storage within template specializations. -template -class ProjectDirectionConstraintInternalData -{ +SOFA_DEPRECATED_HEADER("v24.06", "v25.06", "sofa/component/constraint/projective/DirectionProjectiveConstraint.h") -}; - -/** Project particles to an affine straight line going through the particle original position. -*/ -template -class ProjectDirectionConstraint : public core::behavior::ProjectiveConstraintSet +namespace sofa::component::constraint::projective { -public: - SOFA_CLASS(SOFA_TEMPLATE(ProjectDirectionConstraint,DataTypes),SOFA_TEMPLATE(sofa::core::behavior::ProjectiveConstraintSet, DataTypes)); - - using Index = sofa::Index; - typedef typename DataTypes::VecCoord VecCoord; - typedef typename DataTypes::VecDeriv VecDeriv; - typedef typename DataTypes::MatrixDeriv MatrixDeriv; - typedef typename DataTypes::Coord Coord; - typedef typename DataTypes::Deriv Deriv; - typedef typename DataTypes::CPos CPos; - typedef typename MatrixDeriv::RowIterator MatrixDerivRowIterator; - typedef typename MatrixDeriv::RowType MatrixDerivRowType; - typedef Data DataVecCoord; - typedef Data DataVecDeriv; - typedef Data DataMatrixDeriv; - typedef type::vector Indices; - SOFA_ATTRIBUTE_REPLACED__TYPEMEMBER(Vector3, sofa::type::Vec3); - typedef sofa::core::topology::TopologySubsetIndices IndexSubsetData; - typedef linearalgebra::EigenBaseSparseMatrix BaseSparseMatrix; - typedef linearalgebra::EigenSparseMatrix SparseMatrix; - typedef typename SparseMatrix::Block Block; ///< projection matrix of a particle displacement to the plane - enum {bsize=SparseMatrix::Nin}; ///< size of a block - - -protected: - ProjectDirectionConstraint(); - - virtual ~ProjectDirectionConstraint(); - -public: - IndexSubsetData f_indices; ///< the particles to project - Data f_drawSize; ///< The size of the square used to display the constrained particles - Data f_direction; ///< The direction of the line. Will be normalized by init() - - /// Link to be set to the topology container in the component graph. - SingleLink, sofa::core::topology::BaseMeshTopology, BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_topology; - -protected: - ProjectDirectionConstraintInternalData* data; - friend class ProjectDirectionConstraintInternalData; - - type::vector m_origin; - - -public: - void clearConstraints(); - void addConstraint(Index index); - void removeConstraint(Index index); - - // -- Constraint interface - void init() override; - void reinit() override; - - void projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& resData) override; - void projectVelocity(const core::MechanicalParams* mparams, DataVecDeriv& vData) override; - void projectPosition(const core::MechanicalParams* mparams, DataVecCoord& xData) override; - void projectJacobianMatrix(const core::MechanicalParams* mparams, DataMatrixDeriv& cData) override; - - void applyConstraint(const core::MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) override; - void applyConstraint(const core::MechanicalParams* mparams, linearalgebra::BaseVector* vector, const sofa::core::behavior::MultiMatrixAccessor* matrix) override; - - /// Project the given matrix (Experimental API, see the spec in sofa::core::behavior::BaseProjectiveConstraintSet). - void projectMatrix( sofa::linearalgebra::BaseMatrix* /*M*/, unsigned /*offset*/ ) override; - - - void draw(const core::visual::VisualParams* vparams) override; - -protected : - - SparseMatrix jacobian; ///< projection matrix in local state - SparseMatrix J; ///< auxiliary variable -}; - - -#if !defined(SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_ProjectDirectionConstraint_CPP) -extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API ProjectDirectionConstraint; -extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API ProjectDirectionConstraint; - -#endif - -} // namespace sofa::component::constraint::projective +template +using ProjectDirectionConstraint SOFA_ATTRIBUTE_DEPRECATED("v24.06 ", "v25.06", "ProjectDirectionConstraint has been renamed to DirectionProjectiveConstraint") = DirectionProjectiveConstraint; +} \ No newline at end of file diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/ProjectDirectionConstraint.inl b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/ProjectDirectionConstraint.inl index 1d63aa29002..8f9d63e5751 100644 --- a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/ProjectDirectionConstraint.inl +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/ProjectDirectionConstraint.inl @@ -21,257 +21,6 @@ ******************************************************************************/ #pragma once -#include -#include -#include -#include -#include -#include -#include +#include - -namespace sofa::component::constraint::projective -{ - -template -ProjectDirectionConstraint::ProjectDirectionConstraint() - : core::behavior::ProjectiveConstraintSet(nullptr) - , f_indices( initData(&f_indices,"indices","Indices of the fixed points") ) - , f_drawSize( initData(&f_drawSize,(SReal)0.0,"drawSize","0 -> point based rendering, >0 -> radius of spheres") ) - , f_direction( initData(&f_direction,CPos(),"direction","Direction of the line")) - , l_topology(initLink("topology", "link to the topology container")) - , data(new ProjectDirectionConstraintInternalData()) -{ - f_indices.beginEdit()->push_back(0); - f_indices.endEdit(); -} - - -template -ProjectDirectionConstraint::~ProjectDirectionConstraint() -{ - delete data; -} - -template -void ProjectDirectionConstraint::clearConstraints() -{ - f_indices.beginEdit()->clear(); - f_indices.endEdit(); -} - -template -void ProjectDirectionConstraint::addConstraint(Index index) -{ - f_indices.beginEdit()->push_back(index); - f_indices.endEdit(); -} - -template -void ProjectDirectionConstraint::removeConstraint(Index index) -{ - sofa::type::removeValue(*f_indices.beginEdit(),index); - f_indices.endEdit(); -} - -// -- Constraint interface - - -template -void ProjectDirectionConstraint::init() -{ - this->core::behavior::ProjectiveConstraintSet::init(); - - if (l_topology.empty()) - { - msg_info() << "link to Topology container should be set to ensure right behavior. First Topology found in current context will be used."; - l_topology.set(this->getContext()->getMeshTopologyLink()); - } - - - if (sofa::core::topology::BaseMeshTopology* _topology = l_topology.get()) - { - msg_info() << "Topology path used: '" << l_topology.getLinkedPath() << "'"; - - // Initialize topological changes support - f_indices.createTopologyHandler(_topology); - } - else - { - msg_info() << "No topology component found at path: " << l_topology.getLinkedPath() << ", nor in current context: " << this->getContext()->name; - } - - const Indices & indices = f_indices.getValue(); - - const Index maxIndex=this->mstate->getSize(); - for (unsigned int i=0; i= maxIndex) - { - msg_error() << "Index " << index << " not valid!"; - removeConstraint(index); - } - } - - reinit(); -} - -template -void ProjectDirectionConstraint::reinit() -{ - // normalize the normal vector - CPos n = f_direction.getValue(); - if( n.norm()==0 ) - n[1]=0; - else n *= 1/n.norm(); - f_direction.setValue(n); - - // create the matrix blocks corresponding to the projection to the line: nn^t or to the identity - Block bProjection; - for(unsigned i=0; imstate->getSize(); - const unsigned blockSize = DataTypes::deriv_total_size; - jacobian.resize( numBlocks*blockSize,numBlocks*blockSize ); - - // fill the jacobian in ascending order - Indices::const_iterator it = tmp.begin(); - unsigned i = 0; - while( i < numBlocks ) - { - if( it != tmp.end() && i==*it ) // constrained particle: set diagonal to projection block, and the cursor to the next constraint - { - jacobian.insertBackBlock(i,i,bProjection); - ++it; - } - else // unconstrained particle: set diagonal to identity block - { - jacobian.insertBackBlock(i,i,Block::Identity()); - } - i++; - } - jacobian.compress(); - - const VecCoord& x = this->mstate->read(core::ConstVecCoordId::position())->getValue(); - const Indices &indices = f_indices.getValue(); - for (const auto id : indices) - { - m_origin.push_back(DataTypes::getCPos(x[id])); - } - -} - -template -void ProjectDirectionConstraint::projectMatrix( sofa::linearalgebra::BaseMatrix* M, unsigned offset ) -{ - J.copy(jacobian, M->colSize(), offset); // projection matrix for an assembled state - BaseSparseMatrix* E = dynamic_cast(M); - assert(E); - E->compressedMatrix = J.compressedMatrix * E->compressedMatrix * J.compressedMatrix; -} - - - -template -void ProjectDirectionConstraint::projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& resData) -{ - SOFA_UNUSED(mparams); - - helper::WriteAccessor res ( resData ); - jacobian.mult(res.wref(),res.ref()); -} - -template -void ProjectDirectionConstraint::projectJacobianMatrix(const core::MechanicalParams* /*mparams*/ , DataMatrixDeriv& /*cData*/) -{ - msg_error() << "projectJacobianMatrix(const core::MechanicalParams*, DataMatrixDeriv& ) is not implemented"; -} - -template -void ProjectDirectionConstraint::projectVelocity(const core::MechanicalParams* mparams, DataVecDeriv& vData) -{ - projectResponse(mparams,vData); -} - -template -void ProjectDirectionConstraint::projectPosition(const core::MechanicalParams* /*mparams*/ , DataVecCoord& xData) -{ - VecCoord& x = *xData.beginEdit(); - - const CPos& n = f_direction.getValue(); - - const Indices& indices = f_indices.getValue(); - for(unsigned i=0; i -void ProjectDirectionConstraint::applyConstraint(const core::MechanicalParams* /*mparams*/, const sofa::core::behavior::MultiMatrixAccessor* /*matrix*/) -{ - msg_error() << "applyConstraint is not implemented"; -} - -template -void ProjectDirectionConstraint::applyConstraint(const core::MechanicalParams* /*mparams*/, linearalgebra::BaseVector* /*vector*/, const sofa::core::behavior::MultiMatrixAccessor* /*matrix*/) -{ - dmsg_error() << "ProjectDirectionConstraint::applyConstraint(const core::MechanicalParams* mparams, linearalgebra::BaseVector* vector, const sofa::core::behavior::MultiMatrixAccessor* matrix) is not implemented"; -} - - - - -template -void ProjectDirectionConstraint::draw(const core::visual::VisualParams* vparams) -{ - if (!vparams->displayFlags().getShowBehaviorModels()) return; - if (!this->isActive()) return; - const VecCoord& x = this->mstate->read(core::ConstVecCoordId::position())->getValue(); - - const auto stateLifeCycle = vparams->drawTool()->makeStateLifeCycle(); - - const Indices & indices = f_indices.getValue(); - - if( f_drawSize.getValue() == 0) // old classical drawing by points - { - std::vector< sofa::type::Vec3 > points; - sofa::type::Vec3 point; - for (unsigned int index : indices) - { - point = DataTypes::getCPos(x[index]); - points.push_back(point); - } - vparams->drawTool()->drawPoints(points, 10, sofa::type::RGBAColor(1,0.5,0.5,1)); - } - else // new drawing by spheres - { - std::vector< sofa::type::Vec3 > points; - sofa::type::Vec3 point; - for (unsigned int index : indices) - { - point = DataTypes::getCPos(x[index]); - points.push_back(point); - } - vparams->drawTool()->drawSpheres(points, (float)f_drawSize.getValue(), sofa::type::RGBAColor(1.0f,0.35f,0.35f,1.0f)); - } - - -} - -} // namespace sofa::component::constraint::projective +SOFA_DEPRECATED_HEADER("v24.06", "v25.06", "sofa/component/constraint/projective/DirectionProjectiveConstraint.inl") diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/ProjectToLineConstraint.h b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/ProjectToLineConstraint.h index 9e30c5687d7..1824578181a 100644 --- a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/ProjectToLineConstraint.h +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/ProjectToLineConstraint.h @@ -20,119 +20,13 @@ * Contact information: contact@sofa-framework.org * ******************************************************************************/ #pragma once -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include -namespace sofa::component::constraint::projective -{ - -/// This class can be overridden if needed for additionnal storage within template specializations. -template -class ProjectToLineConstraintInternalData -{ +SOFA_DEPRECATED_HEADER("v24.06", "v25.06", "sofa/component/constraint/projective/LineProjectiveConstraint.h") -}; - -/** Project particles to an affine straight line. - @author Francois Faure, 2012 - @todo Optimized versions for lines parallel to the main directions -*/ -template -class ProjectToLineConstraint : public core::behavior::ProjectiveConstraintSet +namespace sofa::component::constraint::projective { -public: - SOFA_CLASS(SOFA_TEMPLATE(ProjectToLineConstraint,DataTypes),SOFA_TEMPLATE(sofa::core::behavior::ProjectiveConstraintSet, DataTypes)); - - using Index = sofa::Index; - typedef typename DataTypes::VecCoord VecCoord; - typedef typename DataTypes::VecDeriv VecDeriv; - typedef typename DataTypes::MatrixDeriv MatrixDeriv; - typedef typename DataTypes::Coord Coord; - typedef typename DataTypes::Deriv Deriv; - typedef typename DataTypes::CPos CPos; - typedef typename MatrixDeriv::RowIterator MatrixDerivRowIterator; - typedef typename MatrixDeriv::RowType MatrixDerivRowType; - typedef Data DataVecCoord; - typedef Data DataVecDeriv; - typedef Data DataMatrixDeriv; - SOFA_ATTRIBUTE_REPLACED__TYPEMEMBER(Vector3, sofa::type::Vec3); - typedef type::vector Indices; - typedef sofa::core::topology::TopologySubsetIndices IndexSubsetData; - typedef linearalgebra::EigenBaseSparseMatrix BaseSparseMatrix; - typedef linearalgebra::EigenSparseMatrix SparseMatrix; - typedef typename SparseMatrix::Block Block; ///< projection matrix of a particle displacement to the plane - enum {bsize=SparseMatrix::Nin}; ///< size of a block - - -protected: - ProjectToLineConstraint(); - - virtual ~ProjectToLineConstraint(); - -public: - IndexSubsetData f_indices; ///< the particles to project - Data f_drawSize; ///< The size of the square used to display the constrained particles - Data f_origin; ///< A point on the line - Data f_direction; ///< The direction of the line. Will be normalized by init() - - /// Link to be set to the topology container in the component graph. - SingleLink, sofa::core::topology::BaseMeshTopology, BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_topology; - -protected: - ProjectToLineConstraintInternalData* data; - friend class ProjectToLineConstraintInternalData; - - -public: - void clearConstraints(); - void addConstraint(Index index); - void removeConstraint(Index index); - - // -- Constraint interface - void init() override; - void reinit() override; - - void projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& resData) override; - void projectVelocity(const core::MechanicalParams* mparams, DataVecDeriv& vData) override; - void projectPosition(const core::MechanicalParams* mparams, DataVecCoord& xData) override; - void projectJacobianMatrix(const core::MechanicalParams* mparams, DataMatrixDeriv& cData) override; - - void applyConstraint(const core::MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) override; - void applyConstraint(const core::MechanicalParams* mparams, linearalgebra::BaseVector* vector, const sofa::core::behavior::MultiMatrixAccessor* matrix) override; - - /// Project the given matrix (Experimental API, see the spec in sofa::core::behavior::BaseProjectiveConstraintSet). - void projectMatrix( sofa::linearalgebra::BaseMatrix* /*M*/, unsigned /*offset*/ ) override; - - - void draw(const core::visual::VisualParams* vparams) override; - -protected : - - SparseMatrix jacobian; ///< projection matrix in local state - SparseMatrix J; ///< auxiliary variable - - /// Resize/update Jacobian matrix according to the linked mechanical state and the direction - void updateJacobian(); - -}; - - -#if !defined(SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_ProjectToLineConstraint_CPP) -extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API ProjectToLineConstraint; -extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API ProjectToLineConstraint; - -#endif - -} // namespace sofa::component::constraint::projective +template +using ProjectToLineConstraint SOFA_ATTRIBUTE_DEPRECATED("v24.06 ", "v25.06", "ProjectToLineConstraint has been renamed to LineProjectiveConstraint") = LineProjectiveConstraint; +} \ No newline at end of file diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/ProjectToLineConstraint.inl b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/ProjectToLineConstraint.inl index 7abe1ce1c5e..3b15b549d19 100644 --- a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/ProjectToLineConstraint.inl +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/ProjectToLineConstraint.inl @@ -21,266 +21,6 @@ ******************************************************************************/ #pragma once -#include -#include -#include -#include -#include -#include -#include +#include -namespace sofa::component::constraint::projective -{ - -template -ProjectToLineConstraint::ProjectToLineConstraint() - : core::behavior::ProjectiveConstraintSet(nullptr) - , f_indices( initData(&f_indices,"indices","Indices of the fixed points") ) - , f_drawSize( initData(&f_drawSize,(SReal)0.0,"drawSize","0 -> point based rendering, >0 -> radius of spheres") ) - , f_origin( initData(&f_origin,CPos(),"origin","A point in the line")) - , f_direction( initData(&f_direction,CPos(),"direction","Direction of the line")) - , l_topology(initLink("topology", "link to the topology container")) - , data(new ProjectToLineConstraintInternalData()) -{ - f_indices.beginEdit()->push_back(0); - f_indices.endEdit(); -} - - -template -ProjectToLineConstraint::~ProjectToLineConstraint() -{ - delete data; -} - -template -void ProjectToLineConstraint::clearConstraints() -{ - f_indices.beginEdit()->clear(); - f_indices.endEdit(); -} - -template -void ProjectToLineConstraint::addConstraint(Index index) -{ - f_indices.beginEdit()->push_back(index); - f_indices.endEdit(); -} - -template -void ProjectToLineConstraint::removeConstraint(Index index) -{ - sofa::type::removeValue(*f_indices.beginEdit(),index); - f_indices.endEdit(); -} - -// -- Constraint interface - - -template -void ProjectToLineConstraint::init() -{ - this->core::behavior::ProjectiveConstraintSet::init(); - - if (l_topology.empty()) - { - msg_info() << "link to Topology container should be set to ensure right behavior. First Topology found in current context will be used."; - l_topology.set(this->getContext()->getMeshTopologyLink()); - } - - if (sofa::core::topology::BaseMeshTopology* _topology = l_topology.get()) - { - msg_info() << "Topology path used: '" << l_topology.getLinkedPath() << "'"; - - // Initialize topological changes support - f_indices.createTopologyHandler(_topology); - } - else - { - msg_info() << "No topology component found at path: " << l_topology.getLinkedPath() << ", nor in current context: " << this->getContext()->name; - } - - const Indices & indices = f_indices.getValue(); - - const Index maxIndex=this->mstate->getSize(); - for (unsigned int i=0; i= maxIndex) - { - msg_error() << "Index " << index << " not valid!"; - removeConstraint(index); - } - } - - updateJacobian(); -} - -template -void ProjectToLineConstraint::reinit() -{ - updateJacobian(); -} - -template -void ProjectToLineConstraint::updateJacobian() -{ - // normalize the normal vector - CPos n = f_direction.getValue(); - if( n.norm()==0 ) - n[0]=1; // arbritary normal vector - else n *= 1/n.norm(); - f_direction.setValue(n); - - // create the matrix blocks corresponding to the projection to the line: nn^t or to the identity - Block bProjection; - for(unsigned i=0; imstate->getSize(); - const unsigned blockSize = DataTypes::deriv_total_size; - jacobian.resize( numBlocks*blockSize,numBlocks*blockSize ); - - // fill the jacobian in ascending order - Indices::const_iterator it = tmp.begin(); - unsigned i = 0; - while( i < numBlocks ) // (FF) do not stop after the last constrained particle, for the remainder of the diagonal would be null, while it must be identity. - { - if( it!=tmp.end() && i==*it ) // constrained particle: set diagonal to projection block, and the cursor to the next constraint - { - jacobian.insertBackBlock(i,i,bProjection); - ++it; - } - else // unconstrained particle: set diagonal to identity block - { - jacobian.insertBackBlock(i,i,Block::Identity()); - } - i++; - } - jacobian.compress(); -} - -template -void ProjectToLineConstraint::projectMatrix( sofa::linearalgebra::BaseMatrix* M, unsigned offset ) -{ - J.copy(jacobian, M->colSize(), offset); // projection matrix for an assembled state - BaseSparseMatrix* E = dynamic_cast(M); - assert(E); - E->compressedMatrix = J.compressedMatrix * E->compressedMatrix * J.compressedMatrix; -} - - - -template -void ProjectToLineConstraint::projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& resData) -{ - SOFA_UNUSED(mparams); - - helper::WriteAccessor res(resData); - - using Size = decltype(jacobian.colSize()); - if( (jacobian.colSize() / Size(DataTypes::deriv_total_size)) != Size(res.size())) - { - updateJacobian(); - } - - jacobian.mult(res.wref(),res.ref()); -} - -template -void ProjectToLineConstraint::projectJacobianMatrix(const core::MechanicalParams* /*mparams*/ , DataMatrixDeriv& /*cData*/) -{ - msg_error() << "projectJacobianMatrix(const core::MechanicalParams*, DataMatrixDeriv& ) is not implemented"; -} - -template -void ProjectToLineConstraint::projectVelocity(const core::MechanicalParams* mparams, DataVecDeriv& vData) -{ - projectResponse(mparams,vData); -} - -template -void ProjectToLineConstraint::projectPosition(const core::MechanicalParams* /*mparams*/ , DataVecCoord& xData) -{ - VecCoord& x = *xData.beginEdit(); - - const CPos& n = f_direction.getValue(); - const CPos& o = f_origin.getValue(); - - const Indices& indices = f_indices.getValue(); - for(unsigned i=0; i -void ProjectToLineConstraint::applyConstraint(const core::MechanicalParams* /*mparams*/, const sofa::core::behavior::MultiMatrixAccessor* /*matrix*/) -{ - msg_error() << "applyConstraint is not implemented "; -} - -template -void ProjectToLineConstraint::applyConstraint(const core::MechanicalParams* /*mparams*/, linearalgebra::BaseVector* /*vector*/, const sofa::core::behavior::MultiMatrixAccessor* /*matrix*/) -{ - msg_error() << "ProjectToLineConstraint::applyConstraint(const core::MechanicalParams* mparams, linearalgebra::BaseVector* vector, const sofa::core::behavior::MultiMatrixAccessor* matrix) is not implemented "; -} - - - - -template -void ProjectToLineConstraint::draw(const core::visual::VisualParams* vparams) -{ - if (!vparams->displayFlags().getShowBehaviorModels()) return; - if (!this->isActive()) return; - const VecCoord& x = this->mstate->read(core::ConstVecCoordId::position())->getValue(); - - const auto stateLifeCycle = vparams->drawTool()->makeStateLifeCycle(); - - const Indices & indices = f_indices.getValue(); - - if( f_drawSize.getValue() == 0) // old classical drawing by points - { - std::vector< sofa::type::Vec3 > points; - sofa::type::Vec3 point; - - for (Indices::const_iterator it = indices.begin(); - it != indices.end(); - ++it) - { - point = DataTypes::getCPos(x[*it]); - points.push_back(point); - } - vparams->drawTool()->drawPoints(points, 10, sofa::type::RGBAColor(1,0.5,0.5,1)); - } - else // new drawing by spheres - { - std::vector< sofa::type::Vec3 > points; - sofa::type::Vec3 point; - for (unsigned int index : indices) - { - point = DataTypes::getCPos(x[index]); - points.push_back(point); - } - vparams->drawTool()->drawSpheres(points, (float)f_drawSize.getValue(), sofa::type::RGBAColor(1.0f,0.35f,0.35f,1.0f)); - } - - -} - -} // namespace sofa::component::constraint::projective +SOFA_DEPRECATED_HEADER("v24.06", "v25.06", "sofa/component/constraint/projective/LineProjectiveConstraint.inl") diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/ProjectToPlaneConstraint.h b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/ProjectToPlaneConstraint.h index 925bf007d65..d7fb513705e 100644 --- a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/ProjectToPlaneConstraint.h +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/ProjectToPlaneConstraint.h @@ -20,118 +20,13 @@ * Contact information: contact@sofa-framework.org * ******************************************************************************/ #pragma once -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include -namespace sofa::component::constraint::projective -{ - -/// This class can be overridden if needed for additionnal storage within template specializations. -template -class ProjectToPlaneConstraintInternalData -{ +SOFA_DEPRECATED_HEADER("v24.06", "v25.06", "sofa/component/constraint/projective/PlaneProjectiveConstraint.h") -}; - -/** Project particles to an affine plane. - @author Francois Faure, 2012 - @todo Optimized versions for planes parallel to the main directions -*/ -template -class ProjectToPlaneConstraint : public core::behavior::ProjectiveConstraintSet +namespace sofa::component::constraint::projective { -public: - SOFA_CLASS(SOFA_TEMPLATE(ProjectToPlaneConstraint,DataTypes),SOFA_TEMPLATE(sofa::core::behavior::ProjectiveConstraintSet, DataTypes)); - - using Index = sofa::Index; - typedef typename DataTypes::VecCoord VecCoord; - typedef typename DataTypes::VecDeriv VecDeriv; - typedef typename DataTypes::MatrixDeriv MatrixDeriv; - typedef typename DataTypes::Coord Coord; - typedef typename DataTypes::Deriv Deriv; - typedef typename DataTypes::CPos CPos; - typedef typename MatrixDeriv::RowIterator MatrixDerivRowIterator; - typedef typename MatrixDeriv::RowType MatrixDerivRowType; - typedef Data DataVecCoord; - typedef Data DataVecDeriv; - typedef Data DataMatrixDeriv; - typedef type::vector Indices; - typedef sofa::core::topology::TopologySubsetIndices IndexSubsetData; - typedef linearalgebra::EigenBaseSparseMatrix BaseSparseMatrix; - typedef linearalgebra::EigenSparseMatrix SparseMatrix; - typedef typename SparseMatrix::Block Block; ///< projection matrix of a particle displacement to the plane - enum {bsize=SparseMatrix::Nin}; ///< size of a block - - SOFA_ATTRIBUTE_REPLACED__TYPEMEMBER(Vector3, sofa::type::Vec3); - -protected: - ProjectToPlaneConstraint(); - - virtual ~ProjectToPlaneConstraint(); - -public: - IndexSubsetData f_indices; ///< the particles to project - Data f_origin; ///< A point in the plane - Data f_normal; ///< The normal to the plane. Will be normalized by init(). - Data f_drawSize; ///< The size of the display of the constrained particles - - /// Link to be set to the topology container in the component graph. - SingleLink, sofa::core::topology::BaseMeshTopology, BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_topology; - -protected: - ProjectToPlaneConstraintInternalData* data; - friend class ProjectToPlaneConstraintInternalData; - - -public: - void clearConstraints(); - void addConstraint(Index index); - void removeConstraint(Index index); - - // -- Constraint interface - void init() override; - void reinit() override; - - void projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& resData) override; - void projectVelocity(const core::MechanicalParams* mparams, DataVecDeriv& vData) override; - void projectPosition(const core::MechanicalParams* mparams, DataVecCoord& xData) override; - void projectJacobianMatrix(const core::MechanicalParams* mparams, DataMatrixDeriv& cData) override; - - void applyConstraint(const core::MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) override; - void applyConstraint(const core::MechanicalParams* mparams, linearalgebra::BaseVector* vector, const sofa::core::behavior::MultiMatrixAccessor* matrix) override; - - /** Project the given matrix (Experimental API). - Replace M with PMP, where P is the projection matrix corresponding to the projectResponse method, shifted by the given offset, i.e. P is the identity matrix with a block on the diagonal replaced by the projection matrix. - */ - void projectMatrix( sofa::linearalgebra::BaseMatrix* /*M*/, unsigned /*offset*/ ) override; - - - void draw(const core::visual::VisualParams* vparams) override; - -protected : - - SparseMatrix jacobian; ///< projection matrix in local state - SparseMatrix J; ///< auxiliary variable -}; - - -#if !defined(SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_ProjectToPlaneConstraint_CPP) -extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API ProjectToPlaneConstraint; -extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API ProjectToPlaneConstraint; - -#endif - -} // namespace sofa::component::constraint::projective - +template +using ProjectToPlaneConstraint SOFA_ATTRIBUTE_DEPRECATED("v24.06 ", "v25.06", "ProjectToPlaneConstraint has been renamed to PlaneProjectiveConstraint") = PlaneProjectiveConstraint; +} \ No newline at end of file diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/ProjectToPlaneConstraint.inl b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/ProjectToPlaneConstraint.inl index 6e7fd9be4f2..b003eab4b41 100644 --- a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/ProjectToPlaneConstraint.inl +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/ProjectToPlaneConstraint.inl @@ -21,256 +21,6 @@ ******************************************************************************/ #pragma once -#include -#include -#include -#include -#include -#include -#include +#include -namespace sofa::component::constraint::projective -{ - -template -ProjectToPlaneConstraint::ProjectToPlaneConstraint() - : core::behavior::ProjectiveConstraintSet(nullptr) - , f_indices( initData(&f_indices,"indices","Indices of the fixed points") ) - , f_origin( initData(&f_origin,CPos(),"origin","A point in the plane")) - , f_normal( initData(&f_normal,CPos(),"normal","Normal vector to the plane")) - , f_drawSize( initData(&f_drawSize,(SReal)0.0,"drawSize","0 -> point based rendering, >0 -> radius of spheres") ) - , l_topology(initLink("topology", "link to the topology container")) - , data(new ProjectToPlaneConstraintInternalData()) -{ - f_indices.beginEdit()->push_back(0); - f_indices.endEdit(); -} - - -template -ProjectToPlaneConstraint::~ProjectToPlaneConstraint() -{ - delete data; -} - -template -void ProjectToPlaneConstraint::clearConstraints() -{ - f_indices.beginEdit()->clear(); - f_indices.endEdit(); -} - -template -void ProjectToPlaneConstraint::addConstraint(Index index) -{ - f_indices.beginEdit()->push_back(index); - f_indices.endEdit(); -} - -template -void ProjectToPlaneConstraint::removeConstraint(Index index) -{ - sofa::type::removeValue(*f_indices.beginEdit(),index); - f_indices.endEdit(); -} - -// -- Constraint interface - - -template -void ProjectToPlaneConstraint::init() -{ - this->core::behavior::ProjectiveConstraintSet::init(); - - if (l_topology.empty()) - { - msg_info() << "link to Topology container should be set to ensure right behavior. First Topology found in current context will be used."; - l_topology.set(this->getContext()->getMeshTopologyLink()); - } - - if (sofa::core::topology::BaseMeshTopology* _topology = l_topology.get()) - { - msg_info() << "Topology path used: '" << l_topology.getLinkedPath() << "'"; - - // Initialize topological changes support - f_indices.createTopologyHandler(_topology); - } - else - { - msg_info() << "No topology component found at path: " << l_topology.getLinkedPath() << ", nor in current context: " << this->getContext()->name; - } - - const Indices & indices = f_indices.getValue(); - - const Index maxIndex=this->mstate->getSize(); - for (unsigned int i=0; i= maxIndex) - { - msg_error() << "Index " << index << " not valid!"; - removeConstraint(index); - } - } - - reinit(); - -} - -template -void ProjectToPlaneConstraint::reinit() -{ - - // normalize the normal vector - CPos n = f_normal.getValue(); - if( n.norm()==0 ) - n[1]=0; - else n *= 1/n.norm(); - f_normal.setValue(n); - - // create the matrix blocks corresponding to the projection to the plane: I-nn^t or to the identity - Block bProjection; - for(unsigned i=0; imstate->getSize(); - const unsigned blockSize = DataTypes::deriv_total_size; - jacobian.resize( numBlocks*blockSize,numBlocks*blockSize ); - - // fill the jacobian in ascending order - unsigned i=0; - Indices::const_iterator it = tmp.begin(); - while( i -void ProjectToPlaneConstraint::projectMatrix( sofa::linearalgebra::BaseMatrix* M, unsigned offset ) -{ - J.copy(jacobian, M->colSize(), offset); // projection matrix for an assembled state - BaseSparseMatrix* E = dynamic_cast(M); - assert(E); - E->compressedMatrix = J.compressedMatrix * E->compressedMatrix * J.compressedMatrix; -} - - - -template -void ProjectToPlaneConstraint::projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& resData) -{ - SOFA_UNUSED(mparams); - - helper::WriteAccessor res ( resData ); - jacobian.mult(res.wref(),res.ref()); -} - -template -void ProjectToPlaneConstraint::projectJacobianMatrix(const core::MechanicalParams* /*mparams*/ , DataMatrixDeriv& /*cData*/) -{ - msg_error() << "projectJacobianMatrix(const core::MechanicalParams*, DataMatrixDeriv& ) is not implemented"; -} - -template -void ProjectToPlaneConstraint::projectVelocity(const core::MechanicalParams* mparams, DataVecDeriv& vData) -{ - projectResponse(mparams,vData); -} - -template -void ProjectToPlaneConstraint::projectPosition(const core::MechanicalParams* /*mparams*/ , DataVecCoord& xData) -{ - VecCoord& x = *xData.beginEdit(); - - const CPos& n = f_normal.getValue(); - const CPos& o = f_origin.getValue(); - - const Indices& indices = f_indices.getValue(); - for(unsigned i=0; i -void ProjectToPlaneConstraint::applyConstraint(const core::MechanicalParams* /*mparams*/, const sofa::core::behavior::MultiMatrixAccessor* /*matrix*/) -{ - msg_error() << "applyConstraint is not implemented "; -} - -template -void ProjectToPlaneConstraint::applyConstraint(const core::MechanicalParams* /*mparams*/, linearalgebra::BaseVector* /*vector*/, const sofa::core::behavior::MultiMatrixAccessor* /*matrix*/) -{ - msg_error() << "ProjectToPlaneConstraint::applyConstraint(const core::MechanicalParams* mparams, linearalgebra::BaseVector* vector, const sofa::core::behavior::MultiMatrixAccessor* matrix) is not implemented "; -} - -template -void ProjectToPlaneConstraint::draw(const core::visual::VisualParams* vparams) -{ - if (!vparams->displayFlags().getShowBehaviorModels()) return; - if (!this->isActive()) return; - const VecCoord& x = this->mstate->read(core::ConstVecCoordId::position())->getValue(); - - const auto stateLifeCycle = vparams->drawTool()->makeStateLifeCycle(); - - const Indices & indices = f_indices.getValue(); - - if( f_drawSize.getValue() == 0) // old classical drawing by points - { - std::vector< sofa::type::Vec3 > points; - sofa::type::Vec3 point; - for (unsigned int index : indices) - { - point = DataTypes::getCPos(x[index]); - points.push_back(point); - } - vparams->drawTool()->drawPoints(points, 10, sofa::type::RGBAColor(1,0.5,0.5,1)); - } - else // new drawing by spheres - { - std::vector< sofa::type::Vec3 > points; - sofa::type::Vec3 point; - - for (unsigned int index : indices) - { - point = DataTypes::getCPos(x[index]); - points.push_back(point); - } - vparams->drawTool()->drawSpheres(points, (float)f_drawSize.getValue(), sofa::type::RGBAColor(1.0f,0.35f,0.35f,1.0f)); - } - - -} - -} // namespace sofa::component::constraint::projective +SOFA_DEPRECATED_HEADER("v24.06", "v25.06", "sofa/component/constraint/projective/PlaneProjectiveConstraint.inl") diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/ProjectToPointConstraint.h b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/ProjectToPointConstraint.h index 91a50f2b7c0..c5902f37823 100644 --- a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/ProjectToPointConstraint.h +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/ProjectToPointConstraint.h @@ -20,112 +20,13 @@ * Contact information: contact@sofa-framework.org * ******************************************************************************/ #pragma once -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include -namespace sofa::component::constraint::projective -{ - -/// This class can be overridden if needed for additionnal storage within template specializations. -template -class ProjectToPointConstraintInternalData -{ +SOFA_DEPRECATED_HEADER("v24.06", "v25.06", "sofa/component/constraint/projective/PointProjectiveConstraint.h") -}; - -/** Attach given particles to their initial positions. - * Contrary to FixedConstraint, this one stops the particles even if they have a non-null initial velocity. - * @sa FixedConstraint -*/ -template -class ProjectToPointConstraint : public core::behavior::ProjectiveConstraintSet +namespace sofa::component::constraint::projective { -public: - SOFA_CLASS(SOFA_TEMPLATE(ProjectToPointConstraint,DataTypes),SOFA_TEMPLATE(sofa::core::behavior::ProjectiveConstraintSet, DataTypes)); - - using Index = sofa::Index; - typedef typename DataTypes::VecCoord VecCoord; - typedef typename DataTypes::VecDeriv VecDeriv; - typedef typename DataTypes::MatrixDeriv MatrixDeriv; - typedef typename DataTypes::Coord Coord; - typedef typename DataTypes::Deriv Deriv; - typedef typename MatrixDeriv::RowIterator MatrixDerivRowIterator; - typedef typename MatrixDeriv::RowType MatrixDerivRowType; - typedef Data DataVecCoord; - typedef Data DataVecDeriv; - typedef Data DataMatrixDeriv; - typedef type::vector SetIndexArray; - typedef sofa::core::topology::TopologySubsetIndices SetIndex; - - SOFA_ATTRIBUTE_REPLACED__TYPEMEMBER(Vector3, sofa::type::Vec3); - -protected: - ProjectToPointConstraint(); - - virtual ~ProjectToPointConstraint(); - -public: - SetIndex f_indices; ///< the indices of the points to project to the target - Data f_point; ///< the target of the projection - Data f_fixAll; ///< to project all the points, rather than those listed in f_indices - Data f_drawSize; ///< 0 -> point based rendering, >0 -> radius of spheres - - /// Link to be set to the topology container in the component graph. - SingleLink, sofa::core::topology::BaseMeshTopology, BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_topology; - -protected: - ProjectToPointConstraintInternalData* data; - friend class ProjectToPointConstraintInternalData; - - -public: - void clearConstraints(); - void addConstraint(Index index); - void removeConstraint(Index index); - - // -- Constraint interface - void init() override; - void reinit() override; - - void projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& resData) override; - void projectVelocity(const core::MechanicalParams* mparams, DataVecDeriv& vData) override; - void projectPosition(const core::MechanicalParams* mparams, DataVecCoord& xData) override; - void projectJacobianMatrix(const core::MechanicalParams* mparams, DataMatrixDeriv& cData) override; - - void applyConstraint(const core::MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) override; - void applyConstraint(const core::MechanicalParams* mparams, linearalgebra::BaseVector* vector, const sofa::core::behavior::MultiMatrixAccessor* matrix) override; - - /** Project the given matrix (Experimental API). - Replace M with PMP, where P is the projection matrix corresponding to the projectResponse method, shifted by the given offset, i.e. P is the identity matrix with a block on the diagonal replaced by the projection matrix. - */ - void projectMatrix( sofa::linearalgebra::BaseMatrix* /*M*/, unsigned /*offset*/ ) override; - - - void draw(const core::visual::VisualParams* vparams) override; - - bool fixAllDOFs() const { return f_fixAll.getValue(); } - -protected : - -}; - - -#if !defined(SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_ProjectToPointConstraint_CPP) -extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API ProjectToPointConstraint; -extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API ProjectToPointConstraint; -extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API ProjectToPointConstraint; -extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API ProjectToPointConstraint; - -#endif - -} // namespace sofa::component::constraint::projective +template +using ProjectToPointConstraint SOFA_ATTRIBUTE_DEPRECATED("v24.06 ", "v25.06", "ProjectToPointConstraint has been renamed to PointProjectiveConstraint") = PointProjectiveConstraint; +} \ No newline at end of file diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/ProjectToPointConstraint.inl b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/ProjectToPointConstraint.inl index 8bfd98db63a..70213c8c05f 100644 --- a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/ProjectToPointConstraint.inl +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/ProjectToPointConstraint.inl @@ -21,304 +21,6 @@ ******************************************************************************/ #pragma once -#include -#include -#include -#include -#include -#include -#include -#include - - -namespace sofa::component::constraint::projective -{ - -template -ProjectToPointConstraint::ProjectToPointConstraint() - : core::behavior::ProjectiveConstraintSet(nullptr) - , f_indices( initData(&f_indices,"indices","Indices of the points to project") ) - , f_point( initData(&f_point,"point","Target of the projection") ) - , f_fixAll( initData(&f_fixAll,false,"fixAll","filter all the DOF to implement a fixed object") ) - , f_drawSize( initData(&f_drawSize,(SReal)0.0,"drawSize","0 -> point based rendering, >0 -> radius of spheres") ) - , l_topology(initLink("topology", "link to the topology container")) - , data(new ProjectToPointConstraintInternalData()) -{ - f_indices.beginEdit()->push_back(0); - f_indices.endEdit(); -} - - -template -ProjectToPointConstraint::~ProjectToPointConstraint() -{ - delete data; -} - -template -void ProjectToPointConstraint::clearConstraints() -{ - f_indices.beginEdit()->clear(); - f_indices.endEdit(); -} - -template -void ProjectToPointConstraint::addConstraint(Index index) -{ - f_indices.beginEdit()->push_back(index); - f_indices.endEdit(); -} - -template -void ProjectToPointConstraint::removeConstraint(Index index) -{ - sofa::type::removeValue(*f_indices.beginEdit(),index); - f_indices.endEdit(); -} - -// -- Constraint interface - - -template -void ProjectToPointConstraint::init() -{ - this->core::behavior::ProjectiveConstraintSet::init(); - - if (l_topology.empty()) - { - msg_info() << "link to Topology container should be set to ensure right behavior. First Topology found in current context will be used."; - l_topology.set(this->getContext()->getMeshTopologyLink()); - } - - if (sofa::core::topology::BaseMeshTopology* _topology = l_topology.get()) - { - msg_info() << "Topology path used: '" << l_topology.getLinkedPath() << "'"; - - // Initialize topological changes support - f_indices.createTopologyHandler(_topology); - } - else - { - msg_info() << "No topology component found at path: " << l_topology.getLinkedPath() << ", nor in current context: " << this->getContext()->name; - } - - const SetIndexArray & indices = f_indices.getValue(); - - std::stringstream sstream; - const Index maxIndex=this->mstate->getSize(); - for (unsigned int i=0; i= maxIndex) - { - sstream << "Index " << index << " not valid!\n"; - removeConstraint(index); - } - } - msg_error_when(!sstream.str().empty()) << sstream.str(); - - reinit(); -} - -template -void ProjectToPointConstraint::reinit() -{ - - // get the indices sorted - SetIndexArray tmp = f_indices.getValue(); - std::sort(tmp.begin(),tmp.end()); -} - -template -void ProjectToPointConstraint::projectMatrix( sofa::linearalgebra::BaseMatrix* M, unsigned offset ) -{ - const unsigned blockSize = DataTypes::deriv_total_size; - - // clears the rows and columns associated with fixed particles - for (const auto id : f_indices.getValue()) - { - M->clearRowsCols( offset + id * blockSize, offset + (id+1) * blockSize ); - } -} - -template -void ProjectToPointConstraint::projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& resData) -{ - SOFA_UNUSED(mparams); - - helper::WriteAccessor res ( resData ); - const SetIndexArray & indices = f_indices.getValue(); - if( f_fixAll.getValue() ) - { - // fix everything - typename VecDeriv::iterator it; - for( it = res.begin(); it != res.end(); ++it ) - { - *it = Deriv(); - } - } - else - { - for (SetIndexArray::const_iterator it = indices.begin(); - it != indices.end(); - ++it) - { - res[*it] = Deriv(); - } - } -} - -template -void ProjectToPointConstraint::projectJacobianMatrix(const core::MechanicalParams* mparams, DataMatrixDeriv& cData) -{ - SOFA_UNUSED(mparams); - - helper::WriteAccessor c ( cData ); - - if( f_fixAll.getValue() ) - { - // fix everything - c->clear(); - } - else - { - const SetIndexArray& indices = f_indices.getValue(); - for (SetIndexArray::const_iterator it = indices.begin(); - it != indices.end(); - ++it) - { - c->clearColBlock(*it); - } - } -} - -template -void ProjectToPointConstraint::projectVelocity(const core::MechanicalParams* mparams , DataVecDeriv& vData) -{ - projectResponse(mparams, vData); -} - -template -void ProjectToPointConstraint::projectPosition(const core::MechanicalParams* mparams, DataVecCoord& xData) -{ - SOFA_UNUSED(mparams); - - helper::WriteAccessor res ( xData ); - const SetIndexArray & indices = f_indices.getValue(); - if( f_fixAll.getValue() ) - { - // fix everything - typename VecCoord::iterator it; - for( it = res.begin(); it != res.end(); ++it ) - { - *it = f_point.getValue(); - } - } - else - { - for (SetIndexArray::const_iterator it = indices.begin(); - it != indices.end(); - ++it) - { - res[*it] = f_point.getValue(); - } - } -} - -template -void ProjectToPointConstraint::applyConstraint(const core::MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) -{ - SOFA_UNUSED(mparams); - if(const core::behavior::MultiMatrixAccessor::MatrixRef r = matrix->getMatrix(this->mstate.get())) - { - const unsigned int N = Deriv::size(); - const SetIndexArray & indices = f_indices.getValue(); - - for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) - { - // Reset Fixed Row and Col - for (unsigned int c=0; cclearRowCol(r.offset + N * (*it) + c); - // Set Fixed Vertex - for (unsigned int c=0; cset(r.offset + N * (*it) + c, r.offset + N * (*it) + c, 1.0); - } - } -} - -template -void ProjectToPointConstraint::applyConstraint(const core::MechanicalParams* mparams, linearalgebra::BaseVector* vector, const sofa::core::behavior::MultiMatrixAccessor* matrix) -{ - SOFA_UNUSED(mparams); - const int o = matrix->getGlobalOffset(this->mstate.get()); - if (o >= 0) - { - const unsigned int offset = (unsigned int)o; - const unsigned int N = Deriv::size(); - - const SetIndexArray & indices = f_indices.getValue(); - for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) - { - for (unsigned int c=0; cclear(offset + N * (*it) + c); - } - } -} - - - - -template -void ProjectToPointConstraint::draw(const core::visual::VisualParams* vparams) -{ - if (!vparams->displayFlags().getShowBehaviorModels()) return; - if (!this->isActive()) return; - const VecCoord& x = this->mstate->read(core::ConstVecCoordId::position())->getValue(); - const SetIndexArray & indices = f_indices.getValue(); - - const auto stateLifeCycle = vparams->drawTool()->makeStateLifeCycle(); - - if( f_drawSize.getValue() == 0) // old classical drawing by points - { - std::vector< sofa::type::Vec3 > points; - sofa::type::Vec3 point; - if( f_fixAll.getValue() ) - for (unsigned i=0; idrawTool()->drawPoints(points, 10, sofa::type::RGBAColor(1,0.5,0.5,1)); - } - else // new drawing by spheres - { - std::vector< sofa::type::Vec3 > points; - sofa::type::Vec3 point; - if(f_fixAll.getValue()) - for (unsigned i=0; idrawTool()->drawSpheres(points, (float)f_drawSize.getValue(), sofa::type::RGBAColor(1.0f,0.35f,0.35f,1.0f)); - } - - -} - -} // namespace sofa::component::constraint::projective - - +#include +SOFA_DEPRECATED_HEADER("v24.06", "v25.06", "sofa/component/constraint/projective/PointProjectiveConstraint.inl") diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/SkeletalMotionConstraint.h b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/SkeletalMotionConstraint.h index e383eee8dad..9a5b8fc1fca 100644 --- a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/SkeletalMotionConstraint.h +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/SkeletalMotionConstraint.h @@ -20,229 +20,13 @@ * Contact information: contact@sofa-framework.org * ******************************************************************************/ #pragma once -#include -#include -#include -#include -#include +#include -namespace sofa::component::constraint::projective -{ - -// a joint of the skeletal hierarchy, it participates in the skeletal animation chain and may be animated -template -struct SkeletonJoint; - -// joints index to export in the MechanicalObject (in order to use them for skinning for instance) -typedef int SkeletonBone; - -// impose a specific motion (translation and rotation) for each DOFs of a MechanicalObject -template -class SkeletalMotionConstraint : public core::behavior::ProjectiveConstraintSet -{ -public: - SOFA_CLASS(SOFA_TEMPLATE(SkeletalMotionConstraint,TDataTypes),SOFA_TEMPLATE(sofa::core::behavior::ProjectiveConstraintSet, TDataTypes)); - typedef TDataTypes DataTypes; - typedef sofa::core::behavior::ProjectiveConstraintSet TProjectiveConstraintSet; - typedef sofa::core::behavior::MechanicalState MechanicalState; - typedef typename DataTypes::VecCoord VecCoord; - typedef typename DataTypes::VecDeriv VecDeriv; - typedef typename DataTypes::MatrixDeriv MatrixDeriv; - typedef typename DataTypes::Coord Coord; - typedef typename DataTypes::Deriv Deriv; - typedef typename DataTypes::Real Real; - typedef typename MatrixDeriv::RowIterator MatrixDerivRowIterator; - typedef typename MatrixDeriv::RowType MatrixDerivRowType; - typedef Data DataVecCoord; - typedef Data DataVecDeriv; - typedef Data DataMatrixDeriv; - -protected: - SkeletalMotionConstraint(); - - virtual ~SkeletalMotionConstraint(); - -public: - - void init() override; - void reset() override; - - float getAnimationSpeed() const {return animationSpeed.getValue();} - void setAnimationSpeed(float speed) {animationSpeed.setValue(speed);} - - void findKeyTimes(Real ct); - - void projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& resData) override; - void projectVelocity(const core::MechanicalParams* /*mparams*/, DataVecDeriv& vData) override; - void projectPosition(const core::MechanicalParams* /*mparams*/, DataVecCoord& xData) override; - void projectJacobianMatrix(const core::MechanicalParams* mparams, DataMatrixDeriv& cData) override; - - using core::behavior::ProjectiveConstraintSet::applyConstraint; - void applyConstraint(const core::MechanicalParams* mparams, linearalgebra::BaseVector* vector, const sofa::core::behavior::MultiMatrixAccessor* matrix) override; - void applyConstraint(const core::MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) override; - - void projectMatrix( sofa::linearalgebra::BaseMatrix* M, unsigned offset ) override; +SOFA_DEPRECATED_HEADER("v24.06", "v25.06", "sofa/component/constraint/projective/SkeletalMotionProjectiveConstraint.h") - void draw(const core::visual::VisualParams* vparams) override; - - template - void localToGlobal(typename std::enable_if >::value, VecCoord>::type& x); - - void setSkeletalMotion(const type::vector >& skeletonJoints, const type::vector& skeletonBones); - - void addChannel(unsigned int jointIndex , Coord channel, double time); - -protected: - template - void projectResponseT(DataDeriv& dx, - const std::function& clear); - - template - void interpolatePosition(Real cT, typename std::enable_if >::value, VecCoord>::type& x); - -protected: - // every nodes needed in the animation chain - Data > > skeletonJoints; ///< skeleton joints - // mesh skeleton bones which need to be updated according to the animated nodes, we use them to fill the mechanical object - Data > skeletonBones; ///< skeleton bones - - // control how fast the animation is played since animation time is not simulation time - Data animationSpeed; ///< animation speed - - /// is the projective constraint activated? - Data active; - -private: - /// the key times surrounding the current simulation time (for interpolation) - Real prevT, nextT; - - /// the global position of the bones at time t+dt used for the velocities computation - VecCoord nextPositions; - - /// to know if we found the key times - bool finished; - -}; - -template -struct SkeletonJoint +namespace sofa::component::constraint::projective { - friend class SkeletalMotionConstraint; - - typedef typename DataTypes::Coord Coord; - - SkeletonJoint() - : mParentIndex(-1) - , mChannels() - , mTimes() - , mPreviousMotionTime(0) - , mNextMotionTime(0) - {} - - virtual ~SkeletonJoint(){} - - void addChannel(Coord channel, double time) - { - mChannels.push_back(channel); - mTimes.push_back(time); - } - - inline friend std::ostream& operator << (std::ostream& out, const SkeletonJoint& skeletonJoint) - { - out << "Parent" << " " << skeletonJoint.mParentIndex << " "; - out << "Channels" << " " << skeletonJoint.mChannels.size() << " " << skeletonJoint.mChannels << " "; - out << "Times" << " " << skeletonJoint.mTimes.size() << " " << skeletonJoint.mTimes << " "; - out << "PreviousMotion" << " " << skeletonJoint.mPreviousMotion << " "; - out << "PreviousMotionTime" << " " << skeletonJoint.mPreviousMotionTime << " "; - out << "NextMotion" << " " << skeletonJoint.mNextMotion << " "; - out << "NextMotionTime" << " " << skeletonJoint.mNextMotionTime << " "; - out << "LocalRigid" << " " << skeletonJoint.mLocalRigid; - - return out; - } - - inline friend std::istream& operator >> (std::istream& in, SkeletonJoint& skeletonJoint) - { - std::string tmp; - - in >> tmp >> skeletonJoint.mParentIndex; - - size_t numChannel; - in >> tmp >> numChannel; - skeletonJoint.mChannels.resize(numChannel); - Coord channel; - for(Size i = 0; i < numChannel; ++i) - { - in >> channel; - skeletonJoint.mChannels[i] = channel; - } - - size_t numTime; - in >> tmp >> numTime; - skeletonJoint.mTimes.resize(numTime); - double time; - for(Size i = 0; i < numTime; ++i) - { - in >> time; - skeletonJoint.mTimes[i] = time; - } - - in >> tmp >> skeletonJoint.mPreviousMotion; - in >> tmp >> skeletonJoint.mNextMotion; - in >> tmp >> skeletonJoint.mLocalRigid; - - return in; - } - - // parent joint, set to -1 if root, you must set this value - int mParentIndex; - - // set the joint rest position, you must set this value - void setRestPosition(const Coord& restPosition) - { - mPreviousMotion = restPosition; - mNextMotion = restPosition; - mLocalRigid = restPosition; - } - - // following data are useful for animation only, you must fill those vectors if this joint is animated - - // each channel represents a local transformation at a given time in the animation - type::vector mChannels; - - // times corresponding to each animation channel, the channel mChannels[i] must be played at the time contained in mTimes[i] - type::vector mTimes; - -private: - - // following data are used internally to compute the final joint transformation at a specific time using interpolation - - // previous joint motion - Coord mPreviousMotion; - - // next joint motion - Coord mNextMotion; - - // time position for previous joint motion - double mPreviousMotionTime; - - // time position for next joint motion - double mNextMotionTime; - - // this rigid represent the animated node at a specific time relatively to its parent, it may be an interpolation between two channels - // we need to store the current rigid in order to compute the final world position of its rigid children - Coord mLocalRigid; - - // mCurrentRigid in the world coordinate - Coord mWorldRigid; -}; - -#if defined(WIN32) && !defined(SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_SKELETALMOTIONCONSTRAINT_CPP) -extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API SkeletalMotionConstraint; - -#endif - - -} // namespace sofa::component::constraint::projective - +template +using SkeletalMotionConstraint SOFA_ATTRIBUTE_DEPRECATED("v24.06 ", "v25.06", "SkeletalMotionConstraint has been renamed to SkeletalMotionProjectiveConstraint") = SkeletalMotionProjectiveConstraint; +} \ No newline at end of file diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/SkeletalMotionConstraint.inl b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/SkeletalMotionConstraint.inl index 47194a6e86f..2682541f635 100644 --- a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/SkeletalMotionConstraint.inl +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/SkeletalMotionConstraint.inl @@ -21,402 +21,6 @@ ******************************************************************************/ #pragma once -#include -#include -#include -#include -#include -#include -#include -#include +#include -namespace sofa::component::constraint::projective -{ - -template -SkeletalMotionConstraint::SkeletalMotionConstraint() : - sofa::core::behavior::ProjectiveConstraintSet() - , skeletonJoints(initData(&skeletonJoints, "joints", "skeleton joints")) - , skeletonBones(initData(&skeletonBones, "bones", "skeleton bones")) - , animationSpeed(initData(&animationSpeed, 1.0f, "animationSpeed", "animation speed")) - , active(initData(&active, true, "active", "is the constraint active?")) - , finished(false) -{ -} - -template -SkeletalMotionConstraint::~SkeletalMotionConstraint() -{ -} - -template -void SkeletalMotionConstraint::init() -{ - nextPositions.resize(skeletonBones.getValue().size()); - sofa::core::behavior::ProjectiveConstraintSet::init(); -} - -template -void SkeletalMotionConstraint::reset() -{ - sofa::core::behavior::ProjectiveConstraintSet::reset(); -} - -template -void SkeletalMotionConstraint::findKeyTimes(Real ct) -{ - //Note: works only if the times are sorted - - finished = false; - - for(unsigned int i = 0; i < skeletonJoints.getValue().size(); ++i) - { - SkeletonJoint& skeletonJoint = (*skeletonJoints.beginEdit())[i]; - - - if(skeletonJoint.mChannels.empty() || skeletonJoint.mChannels.size() != skeletonJoint.mTimes.size()) - continue; - - for(unsigned int j = 0; j < skeletonJoint.mTimes.size(); ++j) - { - Real keyTime = (Real) skeletonJoint.mTimes[j]; - if(keyTime <= ct) - { - { - skeletonJoint.mPreviousMotionTime = keyTime; - const defaulttype::RigidCoord<3, Real>& motion = skeletonJoint.mChannels[j]; - skeletonJoint.mPreviousMotion = motion; - } - if(prevT < keyTime) - prevT = keyTime; - } - else - { - { - skeletonJoint.mNextMotionTime = keyTime; - const defaulttype::RigidCoord<3, Real>& motion = skeletonJoint.mChannels[j]; - skeletonJoint.mNextMotion = motion; - } - if(nextT > keyTime) - nextT = keyTime; - - finished = true; - break; - } - } - skeletonJoints.endEdit(); - } -} - -template template -void SkeletalMotionConstraint::projectResponseT(DataDeriv& res, - const std::function& clear) -{ - if( !active.getValue() ) return; - - for (unsigned int i = 0; i < res.size(); ++i) - clear(res, i); -} - -template -void SkeletalMotionConstraint::projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& resData) -{ - SOFA_UNUSED(mparams); - if( !active.getValue() ) return; - - helper::WriteAccessor res = resData; - projectResponseT(res.wref(), [](VecDeriv& res, const unsigned int index) { res[index].clear(); }); -} - -template -void SkeletalMotionConstraint::projectVelocity(const core::MechanicalParams* /*mparams*/, DataVecDeriv& vData) -{ - if( !active.getValue() ) return; - - helper::WriteAccessor dx = vData; - helper::ReadAccessor x = ((MechanicalState*)this->getContext()->getMechanicalState())->readPositions(); - Real cT = (Real) this->getContext()->getTime() * animationSpeed.getValue(); - Real dt = (Real) this->getContext()->getDt(); - - if(0.0 != cT) - { - findKeyTimes(cT+dt); - if(finished) - { - // compute the position of the bones at cT + dt - this->interpolatePosition(cT+dt, nextPositions); - // compute the velocity using finite difference - for (unsigned i=0; i -void SkeletalMotionConstraint::projectPosition(const core::MechanicalParams* /*mparams*/, DataVecCoord& xData) -{ - if( !active.getValue() ) return; - - helper::WriteAccessor x = xData; - Real cT = (Real) this->getContext()->getTime() * animationSpeed.getValue(); - - if(0.0 != cT) - { - findKeyTimes(cT); - - // if we found 2 keyTimes, we have to interpolate a velocity (linear interpolation) - interpolatePosition(cT, x.wref()); - } -} - -template -template -void SkeletalMotionConstraint::interpolatePosition(Real cT, typename std::enable_if >::value, VecCoord>::type& x) -{ - // set the motion to the SkeletonJoint corresponding rigid - - if(finished) - for(unsigned int i = 0; i < skeletonJoints.getValue().size(); ++i) - { - - SkeletonJoint& skeletonJoint = (*skeletonJoints.beginEdit())[i]; - if( skeletonJoint.mPreviousMotionTime != skeletonJoint.mNextMotionTime) - { - Real dt = (Real)((cT - skeletonJoint.mPreviousMotionTime) / (skeletonJoint.mNextMotionTime - skeletonJoint.mPreviousMotionTime)); - - const type::vector >& channels = skeletonJoint.mChannels; - - if(channels.empty()) - continue; - - skeletonJoint.mLocalRigid.getCenter() = skeletonJoint.mPreviousMotion.getCenter() + (skeletonJoint.mNextMotion.getCenter() - skeletonJoint.mPreviousMotion.getCenter()) * dt; - skeletonJoint.mLocalRigid.getOrientation().slerp(skeletonJoint.mPreviousMotion.getOrientation(), skeletonJoint.mNextMotion.getOrientation(), (float) dt, true); - - skeletonJoints.endEdit(); - } - else - { - const type::vector >& channels = skeletonJoint.mChannels; - - if(channels.empty()) - continue; - - skeletonJoint.mLocalRigid.getCenter() = skeletonJoint.mNextMotion.getCenter(); - skeletonJoint.mLocalRigid.getOrientation() = skeletonJoint.mNextMotion.getOrientation(); - - skeletonJoints.endEdit(); - } - } - else - { - for(unsigned int i = 0; i < skeletonJoints.getValue().size(); ++i) - { - SkeletonJoint& skeletonJoint = (*skeletonJoints.beginEdit())[i]; - - const type::vector >& channels = skeletonJoint.mChannels; - - if(channels.empty()) - continue; - - skeletonJoint.mLocalRigid.getCenter() = skeletonJoint.mNextMotion.getCenter(); - skeletonJoint.mLocalRigid.getOrientation() = skeletonJoint.mNextMotion.getOrientation(); - - skeletonJoints.endEdit(); - } - } - - // apply the final transformation from skeletonBones to dofs here - localToGlobal(x); -} - -template -void SkeletalMotionConstraint::projectJacobianMatrix(const core::MechanicalParams* mparams, DataMatrixDeriv& cData) -{ - SOFA_UNUSED(mparams); - if( !active.getValue() ) return; - - helper::WriteAccessor c = cData; - - projectResponseT(c.wref(), [](MatrixDeriv& res, const unsigned int index) { res.clearColBlock(index); }); -} - -template -template -void SkeletalMotionConstraint::localToGlobal(typename std::enable_if >::value, VecCoord>::type& x) -{ - for(unsigned int i = 0; i < skeletonJoints.getValue().size(); ++i) - { - SkeletonJoint& skeletonJoint = (*skeletonJoints.beginEdit())[i]; - - defaulttype::RigidCoord< 3, Real> worldRigid = skeletonJoint.mLocalRigid; - - // break if the parent joint is the root - for(int parentIndex = skeletonJoint.mParentIndex; -1 != parentIndex; parentIndex = skeletonJoints.getValue()[parentIndex].mParentIndex) - { - defaulttype::RigidCoord< 3, Real> parentLocalRigid = skeletonJoints.getValue()[parentIndex].mLocalRigid; - worldRigid = parentLocalRigid.mult(worldRigid); - } - - skeletonJoint.mWorldRigid = worldRigid; - - skeletonJoints.endEdit(); - } - - for(unsigned int i = 0; i < skeletonBones.getValue().size(); ++i) - x[i] = skeletonJoints.getValue()[skeletonBones.getValue()[i]].mWorldRigid; -} - -template -void SkeletalMotionConstraint::setSkeletalMotion(const type::vector >& skeletonJoints, const type::vector& skeletonBones) -{ - this->skeletonJoints.setValue(skeletonJoints); - this->skeletonBones.setValue(skeletonBones); - this->init(); -} - -template -void SkeletalMotionConstraint::addChannel(unsigned int jointIndex , Coord channel, double time) -{ - (*skeletonJoints.beginEdit())[jointIndex].addChannel(channel, time); - skeletonJoints.endEdit(); -} - -// Matrix Integration interface -template -void SkeletalMotionConstraint::applyConstraint(const core::MechanicalParams* /*mparams*/, const sofa::core::behavior::MultiMatrixAccessor* /*matrix*/) -{ - if( !active.getValue() ) return; - - /*const unsigned int N = Deriv::size(); - const SetIndexArray & indices = m_indices.getValue(); - - for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) - { - // Reset Fixed Row and Col - for (unsigned int c=0;cclearRowCol(offset + N * (*it) + c); - // Set Fixed Vertex - for (unsigned int c=0;cset(offset + N * (*it) + c, offset + N * (*it) + c, 1.0); - }*/ -} - -template -void SkeletalMotionConstraint::applyConstraint(const core::MechanicalParams* /*mparams*/, linearalgebra::BaseVector* /*vector*/, const sofa::core::behavior::MultiMatrixAccessor* /*matrix*/) -{ - if( !active.getValue() ) return; - - /*const unsigned int N = Deriv::size(); - - const SetIndexArray & indices = m_indices.getValue(); - for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) - { - for (unsigned int c=0;cclear(offset + N * (*it) + c); - }*/ -} - -template -void SkeletalMotionConstraint::projectMatrix( sofa::linearalgebra::BaseMatrix* M, unsigned offset ) -{ - const unsigned blockSize = DataTypes::deriv_total_size; - const unsigned size = this->mstate->getSize(); - for( unsigned i=0; iclearRowsCols( offset + i * blockSize, offset + (i+1) * (blockSize) ); - } -} - -// display the paths the constrained dofs will go through -template -void SkeletalMotionConstraint::draw(const core::visual::VisualParams* vparams) -{ - if( !active.getValue() ) return; - - if (!vparams->displayFlags().getShowBehaviorModels()) - return; - - sofa::type::vector points; - sofa::type::vector linesX; - sofa::type::vector linesY; - sofa::type::vector linesZ; - sofa::type::vector colorFalloff; - - type::Vec3 point; - type::Vec3 line; - - // draw joints (not bones we draw them differently later) - { - for(unsigned int i = 0; i < skeletonJoints.getValue().size(); ++i) - { - defaulttype::RigidCoord< 3, Real> jointWorldRigid = skeletonJoints.getValue()[i].mWorldRigid; - - unsigned int j; - for(j = 0; j < skeletonBones.getValue().size(); ++j) - if((int)i == skeletonBones.getValue()[j]) - break; - - if(skeletonBones.getValue().size() != j) - continue; - - point = DataTypes::getCPos(jointWorldRigid); - points.push_back(point); - - linesX.push_back(point); - line = point + DataTypes::getCRot(jointWorldRigid).rotate(type::Vec3f(0.1f, 0.0f, 0.0f)); - linesX.push_back(line); - - linesY.push_back(point); - line = point + DataTypes::getCRot(jointWorldRigid).rotate(type::Vec3f(0.0f, 0.1f, 0.0f)); - linesY.push_back(line); - - linesZ.push_back(point); - line = point + DataTypes::getCRot(jointWorldRigid).rotate(type::Vec3f(0.0f, 0.0f, 0.1f)); - linesZ.push_back(line); - } - vparams->drawTool()->drawPoints(points, 10, sofa::type::RGBAColor (1.0f , 0.5f , 0.5f , 1.0f)); - vparams->drawTool()->drawLines (linesX, 2, sofa::type::RGBAColor (0.75f, 0.0f , 0.0f , 1.0f)); - vparams->drawTool()->drawLines (linesY, 2, sofa::type::RGBAColor (0.0f , 0.75f, 0.0f , 1.0f)); - vparams->drawTool()->drawLines (linesZ, 2, sofa::type::RGBAColor (0.0f , 0.0f , 0.75f, 1.0f)); - } - - points.clear(); - linesX.clear(); - linesY.clear(); - linesZ.clear(); - - // draw bones now - { - for(unsigned int i = 0; i < skeletonBones.getValue().size(); ++i) - { - defaulttype::RigidCoord< 3, Real> boneWorldRigid = skeletonJoints.getValue()[skeletonBones.getValue()[i]].mWorldRigid; - - point = DataTypes::getCPos(boneWorldRigid); - points.push_back(point); - - linesX.push_back(point); - line = point + DataTypes::getCRot(boneWorldRigid).rotate(type::Vec3f(0.1f, 0.0f, 0.0f)); - linesX.push_back(line); - - linesY.push_back(point); - line = point + DataTypes::getCRot(boneWorldRigid).rotate(type::Vec3f(0.0f, 0.1f, 0.0f)); - linesY.push_back(line); - - linesZ.push_back(point); - line = point + DataTypes::getCRot(boneWorldRigid).rotate(type::Vec3f(0.0f, 0.0f, 0.1f)); - linesZ.push_back(line); - } - vparams->drawTool()->drawPoints(points, 10, sofa::type::RGBAColor (1.0f, 0.5f, 0.5f, 1.0f)); - vparams->drawTool()->drawLines (linesX, 2 , sofa::type::RGBAColor (1.0f, 0.0f, 0.0f, 1.0f)); - vparams->drawTool()->drawLines (linesY, 2 , sofa::type::RGBAColor (0.0f, 1.0f, 0.0f, 1.0f)); - vparams->drawTool()->drawLines (linesZ, 2 , sofa::type::RGBAColor (0.0f, 0.0f, 1.0f, 1.0f)); - } -} - -} // namespace sofa::component::constraint::projective +SOFA_DEPRECATED_HEADER("v24.06", "v25.06", "sofa/component/constraint/projective/SkeletalMotionProjectiveConstraint.inl") diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/SkeletalMotionConstraint.cpp b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/SkeletalMotionProjectiveConstraint.cpp similarity index 81% rename from Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/SkeletalMotionConstraint.cpp rename to Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/SkeletalMotionProjectiveConstraint.cpp index e8010c918f6..3fb3bf05de1 100644 --- a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/SkeletalMotionConstraint.cpp +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/SkeletalMotionProjectiveConstraint.cpp @@ -19,8 +19,8 @@ * * * Contact information: contact@sofa-framework.org * ******************************************************************************/ -#define SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_SKELETALMOTIONCONSTRAINT_CPP -#include +#define SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_SKELETALMOTIONPROJECTIVECONSTRAINT_CPP +#include #include #include @@ -28,11 +28,11 @@ namespace sofa::component::constraint::projective { //declaration of the class, for the factory -int SkeletalMotionConstraintClass = core::RegisterObject("animate a skeleton") - .add< SkeletalMotionConstraint >() - +int SkeletalMotionProjectiveConstraintClass = core::RegisterObject("animate a skeleton") + .add< SkeletalMotionProjectiveConstraint >() + .addAlias("SkeletalMotionConstraint") ; -template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API SkeletalMotionConstraint; +template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API SkeletalMotionProjectiveConstraint; } // namespace sofa::component::constraint::projective diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/SkeletalMotionProjectiveConstraint.h b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/SkeletalMotionProjectiveConstraint.h new file mode 100644 index 00000000000..b47595e588e --- /dev/null +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/SkeletalMotionProjectiveConstraint.h @@ -0,0 +1,247 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once +#include + +#include +#include +#include +#include + +namespace sofa::component::constraint::projective +{ + +// a joint of the skeletal hierarchy, it participates in the skeletal animation chain and may be animated +template +struct SkeletonJoint; + +// joints index to export in the MechanicalObject (in order to use them for skinning for instance) +typedef int SkeletonBone; + +// impose a specific motion (translation and rotation) for each DOFs of a MechanicalObject +template +class SkeletalMotionProjectiveConstraint : public core::behavior::ProjectiveConstraintSet +{ +public: + SOFA_CLASS(SOFA_TEMPLATE(SkeletalMotionProjectiveConstraint,TDataTypes),SOFA_TEMPLATE(sofa::core::behavior::ProjectiveConstraintSet, TDataTypes)); + typedef TDataTypes DataTypes; + typedef sofa::core::behavior::ProjectiveConstraintSet TProjectiveConstraintSet; + typedef sofa::core::behavior::MechanicalState MechanicalState; + typedef typename DataTypes::VecCoord VecCoord; + typedef typename DataTypes::VecDeriv VecDeriv; + typedef typename DataTypes::MatrixDeriv MatrixDeriv; + typedef typename DataTypes::Coord Coord; + typedef typename DataTypes::Deriv Deriv; + typedef typename DataTypes::Real Real; + typedef typename MatrixDeriv::RowIterator MatrixDerivRowIterator; + typedef typename MatrixDeriv::RowType MatrixDerivRowType; + typedef Data DataVecCoord; + typedef Data DataVecDeriv; + typedef Data DataMatrixDeriv; + +protected: + SkeletalMotionProjectiveConstraint(); + + virtual ~SkeletalMotionProjectiveConstraint(); + +public: + + void init() override; + void reset() override; + + float getAnimationSpeed() const {return animationSpeed.getValue();} + void setAnimationSpeed(float speed) {animationSpeed.setValue(speed);} + + void findKeyTimes(Real ct); + + void projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& resData) override; + void projectVelocity(const core::MechanicalParams* /*mparams*/, DataVecDeriv& vData) override; + void projectPosition(const core::MechanicalParams* /*mparams*/, DataVecCoord& xData) override; + void projectJacobianMatrix(const core::MechanicalParams* mparams, DataMatrixDeriv& cData) override; + + using core::behavior::ProjectiveConstraintSet::applyConstraint; + void applyConstraint(const core::MechanicalParams* mparams, linearalgebra::BaseVector* vector, const sofa::core::behavior::MultiMatrixAccessor* matrix) override; + void applyConstraint(const core::MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) override; + + void projectMatrix( sofa::linearalgebra::BaseMatrix* M, unsigned offset ) override; + + void draw(const core::visual::VisualParams* vparams) override; + + template + void localToGlobal(typename std::enable_if >::value, VecCoord>::type& x); + + void setSkeletalMotion(const type::vector >& skeletonJoints, const type::vector& skeletonBones); + + void addChannel(unsigned int jointIndex , Coord channel, double time); + +protected: + template + void projectResponseT(DataDeriv& dx, + const std::function& clear); + + template + void interpolatePosition(Real cT, typename std::enable_if >::value, VecCoord>::type& x); + +protected: + // every nodes needed in the animation chain + Data > > skeletonJoints; ///< skeleton joints + // mesh skeleton bones which need to be updated according to the animated nodes, we use them to fill the mechanical object + Data > skeletonBones; ///< skeleton bones + + // control how fast the animation is played since animation time is not simulation time + Data animationSpeed; ///< animation speed + + /// is the projective constraint activated? + Data active; + +private: + /// the key times surrounding the current simulation time (for interpolation) + Real prevT, nextT; + + /// the global position of the bones at time t+dt used for the velocities computation + VecCoord nextPositions; + + /// to know if we found the key times + bool finished; + +}; + +template +struct SkeletonJoint +{ + friend class SkeletalMotionProjectiveConstraint; + + typedef typename DataTypes::Coord Coord; + + SkeletonJoint() + : mParentIndex(-1) + , mChannels() + , mTimes() + , mPreviousMotionTime(0) + , mNextMotionTime(0) + {} + + virtual ~SkeletonJoint(){} + + void addChannel(Coord channel, double time) + { + mChannels.push_back(channel); + mTimes.push_back(time); + } + + inline friend std::ostream& operator << (std::ostream& out, const SkeletonJoint& skeletonJoint) + { + out << "Parent" << " " << skeletonJoint.mParentIndex << " "; + out << "Channels" << " " << skeletonJoint.mChannels.size() << " " << skeletonJoint.mChannels << " "; + out << "Times" << " " << skeletonJoint.mTimes.size() << " " << skeletonJoint.mTimes << " "; + out << "PreviousMotion" << " " << skeletonJoint.mPreviousMotion << " "; + out << "PreviousMotionTime" << " " << skeletonJoint.mPreviousMotionTime << " "; + out << "NextMotion" << " " << skeletonJoint.mNextMotion << " "; + out << "NextMotionTime" << " " << skeletonJoint.mNextMotionTime << " "; + out << "LocalRigid" << " " << skeletonJoint.mLocalRigid; + + return out; + } + + inline friend std::istream& operator >> (std::istream& in, SkeletonJoint& skeletonJoint) + { + std::string tmp; + + in >> tmp >> skeletonJoint.mParentIndex; + + size_t numChannel; + in >> tmp >> numChannel; + skeletonJoint.mChannels.resize(numChannel); + Coord channel; + for(Size i = 0; i < numChannel; ++i) + { + in >> channel; + skeletonJoint.mChannels[i] = channel; + } + + size_t numTime; + in >> tmp >> numTime; + skeletonJoint.mTimes.resize(numTime); + double time; + for(Size i = 0; i < numTime; ++i) + { + in >> time; + skeletonJoint.mTimes[i] = time; + } + + in >> tmp >> skeletonJoint.mPreviousMotion; + in >> tmp >> skeletonJoint.mNextMotion; + in >> tmp >> skeletonJoint.mLocalRigid; + + return in; + } + + // parent joint, set to -1 if root, you must set this value + int mParentIndex; + + // set the joint rest position, you must set this value + void setRestPosition(const Coord& restPosition) + { + mPreviousMotion = restPosition; + mNextMotion = restPosition; + mLocalRigid = restPosition; + } + + // following data are useful for animation only, you must fill those vectors if this joint is animated + + // each channel represents a local transformation at a given time in the animation + type::vector mChannels; + + // times corresponding to each animation channel, the channel mChannels[i] must be played at the time contained in mTimes[i] + type::vector mTimes; + +private: + + // following data are used internally to compute the final joint transformation at a specific time using interpolation + + // previous joint motion + Coord mPreviousMotion; + + // next joint motion + Coord mNextMotion; + + // time position for previous joint motion + double mPreviousMotionTime; + + // time position for next joint motion + double mNextMotionTime; + + // this rigid represent the animated node at a specific time relatively to its parent, it may be an interpolation between two channels + // we need to store the current rigid in order to compute the final world position of its rigid children + Coord mLocalRigid; + + // mCurrentRigid in the world coordinate + Coord mWorldRigid; +}; + +#if defined(WIN32) && !defined(SOFA_COMPONENT_PROJECTIVECONSTRAINTSET_SKELETALMOTIONPROJECTIVECONSTRAINT_CPP) +extern template class SOFA_COMPONENT_CONSTRAINT_PROJECTIVE_API SkeletalMotionProjectiveConstraint; + +#endif + +} // namespace sofa::component::constraint::projective + diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/SkeletalMotionProjectiveConstraint.inl b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/SkeletalMotionProjectiveConstraint.inl new file mode 100644 index 00000000000..7c06a73fd18 --- /dev/null +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/SkeletalMotionProjectiveConstraint.inl @@ -0,0 +1,422 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace sofa::component::constraint::projective +{ + +template +SkeletalMotionProjectiveConstraint::SkeletalMotionProjectiveConstraint() : + sofa::core::behavior::ProjectiveConstraintSet() + , skeletonJoints(initData(&skeletonJoints, "joints", "skeleton joints")) + , skeletonBones(initData(&skeletonBones, "bones", "skeleton bones")) + , animationSpeed(initData(&animationSpeed, 1.0f, "animationSpeed", "animation speed")) + , active(initData(&active, true, "active", "is the constraint active?")) + , finished(false) +{ +} + +template +SkeletalMotionProjectiveConstraint::~SkeletalMotionProjectiveConstraint() +{ +} + +template +void SkeletalMotionProjectiveConstraint::init() +{ + nextPositions.resize(skeletonBones.getValue().size()); + sofa::core::behavior::ProjectiveConstraintSet::init(); +} + +template +void SkeletalMotionProjectiveConstraint::reset() +{ + sofa::core::behavior::ProjectiveConstraintSet::reset(); +} + +template +void SkeletalMotionProjectiveConstraint::findKeyTimes(Real ct) +{ + //Note: works only if the times are sorted + + finished = false; + + for(unsigned int i = 0; i < skeletonJoints.getValue().size(); ++i) + { + SkeletonJoint& skeletonJoint = (*skeletonJoints.beginEdit())[i]; + + + if(skeletonJoint.mChannels.empty() || skeletonJoint.mChannels.size() != skeletonJoint.mTimes.size()) + continue; + + for(unsigned int j = 0; j < skeletonJoint.mTimes.size(); ++j) + { + Real keyTime = (Real) skeletonJoint.mTimes[j]; + if(keyTime <= ct) + { + { + skeletonJoint.mPreviousMotionTime = keyTime; + const defaulttype::RigidCoord<3, Real>& motion = skeletonJoint.mChannels[j]; + skeletonJoint.mPreviousMotion = motion; + } + if(prevT < keyTime) + prevT = keyTime; + } + else + { + { + skeletonJoint.mNextMotionTime = keyTime; + const defaulttype::RigidCoord<3, Real>& motion = skeletonJoint.mChannels[j]; + skeletonJoint.mNextMotion = motion; + } + if(nextT > keyTime) + nextT = keyTime; + + finished = true; + break; + } + } + skeletonJoints.endEdit(); + } +} + +template template +void SkeletalMotionProjectiveConstraint::projectResponseT(DataDeriv& res, + const std::function& clear) +{ + if( !active.getValue() ) return; + + for (unsigned int i = 0; i < res.size(); ++i) + clear(res, i); +} + +template +void SkeletalMotionProjectiveConstraint::projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& resData) +{ + SOFA_UNUSED(mparams); + if( !active.getValue() ) return; + + helper::WriteAccessor res = resData; + projectResponseT(res.wref(), [](VecDeriv& res, const unsigned int index) { res[index].clear(); }); +} + +template +void SkeletalMotionProjectiveConstraint::projectVelocity(const core::MechanicalParams* /*mparams*/, DataVecDeriv& vData) +{ + if( !active.getValue() ) return; + + helper::WriteAccessor dx = vData; + helper::ReadAccessor x = ((MechanicalState*)this->getContext()->getMechanicalState())->readPositions(); + Real cT = (Real) this->getContext()->getTime() * animationSpeed.getValue(); + Real dt = (Real) this->getContext()->getDt(); + + if(0.0 != cT) + { + findKeyTimes(cT+dt); + if(finished) + { + // compute the position of the bones at cT + dt + this->interpolatePosition(cT+dt, nextPositions); + // compute the velocity using finite difference + for (unsigned i=0; i +void SkeletalMotionProjectiveConstraint::projectPosition(const core::MechanicalParams* /*mparams*/, DataVecCoord& xData) +{ + if( !active.getValue() ) return; + + helper::WriteAccessor x = xData; + Real cT = (Real) this->getContext()->getTime() * animationSpeed.getValue(); + + if(0.0 != cT) + { + findKeyTimes(cT); + + // if we found 2 keyTimes, we have to interpolate a velocity (linear interpolation) + interpolatePosition(cT, x.wref()); + } +} + +template +template +void SkeletalMotionProjectiveConstraint::interpolatePosition(Real cT, typename std::enable_if >::value, VecCoord>::type& x) +{ + // set the motion to the SkeletonJoint corresponding rigid + + if(finished) + for(unsigned int i = 0; i < skeletonJoints.getValue().size(); ++i) + { + + SkeletonJoint& skeletonJoint = (*skeletonJoints.beginEdit())[i]; + if( skeletonJoint.mPreviousMotionTime != skeletonJoint.mNextMotionTime) + { + Real dt = (Real)((cT - skeletonJoint.mPreviousMotionTime) / (skeletonJoint.mNextMotionTime - skeletonJoint.mPreviousMotionTime)); + + const type::vector >& channels = skeletonJoint.mChannels; + + if(channels.empty()) + continue; + + skeletonJoint.mLocalRigid.getCenter() = skeletonJoint.mPreviousMotion.getCenter() + (skeletonJoint.mNextMotion.getCenter() - skeletonJoint.mPreviousMotion.getCenter()) * dt; + skeletonJoint.mLocalRigid.getOrientation().slerp(skeletonJoint.mPreviousMotion.getOrientation(), skeletonJoint.mNextMotion.getOrientation(), (float) dt, true); + + skeletonJoints.endEdit(); + } + else + { + const type::vector >& channels = skeletonJoint.mChannels; + + if(channels.empty()) + continue; + + skeletonJoint.mLocalRigid.getCenter() = skeletonJoint.mNextMotion.getCenter(); + skeletonJoint.mLocalRigid.getOrientation() = skeletonJoint.mNextMotion.getOrientation(); + + skeletonJoints.endEdit(); + } + } + else + { + for(unsigned int i = 0; i < skeletonJoints.getValue().size(); ++i) + { + SkeletonJoint& skeletonJoint = (*skeletonJoints.beginEdit())[i]; + + const type::vector >& channels = skeletonJoint.mChannels; + + if(channels.empty()) + continue; + + skeletonJoint.mLocalRigid.getCenter() = skeletonJoint.mNextMotion.getCenter(); + skeletonJoint.mLocalRigid.getOrientation() = skeletonJoint.mNextMotion.getOrientation(); + + skeletonJoints.endEdit(); + } + } + + // apply the final transformation from skeletonBones to dofs here + localToGlobal(x); +} + +template +void SkeletalMotionProjectiveConstraint::projectJacobianMatrix(const core::MechanicalParams* mparams, DataMatrixDeriv& cData) +{ + SOFA_UNUSED(mparams); + if( !active.getValue() ) return; + + helper::WriteAccessor c = cData; + + projectResponseT(c.wref(), [](MatrixDeriv& res, const unsigned int index) { res.clearColBlock(index); }); +} + +template +template +void SkeletalMotionProjectiveConstraint::localToGlobal(typename std::enable_if >::value, VecCoord>::type& x) +{ + for(unsigned int i = 0; i < skeletonJoints.getValue().size(); ++i) + { + SkeletonJoint& skeletonJoint = (*skeletonJoints.beginEdit())[i]; + + defaulttype::RigidCoord< 3, Real> worldRigid = skeletonJoint.mLocalRigid; + + // break if the parent joint is the root + for(int parentIndex = skeletonJoint.mParentIndex; -1 != parentIndex; parentIndex = skeletonJoints.getValue()[parentIndex].mParentIndex) + { + defaulttype::RigidCoord< 3, Real> parentLocalRigid = skeletonJoints.getValue()[parentIndex].mLocalRigid; + worldRigid = parentLocalRigid.mult(worldRigid); + } + + skeletonJoint.mWorldRigid = worldRigid; + + skeletonJoints.endEdit(); + } + + for(unsigned int i = 0; i < skeletonBones.getValue().size(); ++i) + x[i] = skeletonJoints.getValue()[skeletonBones.getValue()[i]].mWorldRigid; +} + +template +void SkeletalMotionProjectiveConstraint::setSkeletalMotion(const type::vector >& skeletonJoints, const type::vector& skeletonBones) +{ + this->skeletonJoints.setValue(skeletonJoints); + this->skeletonBones.setValue(skeletonBones); + this->init(); +} + +template +void SkeletalMotionProjectiveConstraint::addChannel(unsigned int jointIndex , Coord channel, double time) +{ + (*skeletonJoints.beginEdit())[jointIndex].addChannel(channel, time); + skeletonJoints.endEdit(); +} + +// Matrix Integration interface +template +void SkeletalMotionProjectiveConstraint::applyConstraint(const core::MechanicalParams* /*mparams*/, const sofa::core::behavior::MultiMatrixAccessor* /*matrix*/) +{ + if( !active.getValue() ) return; + + /*const unsigned int N = Deriv::size(); + const SetIndexArray & indices = m_indices.getValue(); + + for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) + { + // Reset Fixed Row and Col + for (unsigned int c=0;cclearRowCol(offset + N * (*it) + c); + // Set Fixed Vertex + for (unsigned int c=0;cset(offset + N * (*it) + c, offset + N * (*it) + c, 1.0); + }*/ +} + +template +void SkeletalMotionProjectiveConstraint::applyConstraint(const core::MechanicalParams* /*mparams*/, linearalgebra::BaseVector* /*vector*/, const sofa::core::behavior::MultiMatrixAccessor* /*matrix*/) +{ + if( !active.getValue() ) return; + + /*const unsigned int N = Deriv::size(); + + const SetIndexArray & indices = m_indices.getValue(); + for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) + { + for (unsigned int c=0;cclear(offset + N * (*it) + c); + }*/ +} + +template +void SkeletalMotionProjectiveConstraint::projectMatrix( sofa::linearalgebra::BaseMatrix* M, unsigned offset ) +{ + const unsigned blockSize = DataTypes::deriv_total_size; + const unsigned size = this->mstate->getSize(); + for( unsigned i=0; iclearRowsCols( offset + i * blockSize, offset + (i+1) * (blockSize) ); + } +} + +// display the paths the constrained dofs will go through +template +void SkeletalMotionProjectiveConstraint::draw(const core::visual::VisualParams* vparams) +{ + if( !active.getValue() ) return; + + if (!vparams->displayFlags().getShowBehaviorModels()) + return; + + sofa::type::vector points; + sofa::type::vector linesX; + sofa::type::vector linesY; + sofa::type::vector linesZ; + sofa::type::vector colorFalloff; + + type::Vec3 point; + type::Vec3 line; + + // draw joints (not bones we draw them differently later) + { + for(unsigned int i = 0; i < skeletonJoints.getValue().size(); ++i) + { + defaulttype::RigidCoord< 3, Real> jointWorldRigid = skeletonJoints.getValue()[i].mWorldRigid; + + unsigned int j; + for(j = 0; j < skeletonBones.getValue().size(); ++j) + if((int)i == skeletonBones.getValue()[j]) + break; + + if(skeletonBones.getValue().size() != j) + continue; + + point = DataTypes::getCPos(jointWorldRigid); + points.push_back(point); + + linesX.push_back(point); + line = point + DataTypes::getCRot(jointWorldRigid).rotate(type::Vec3f(0.1f, 0.0f, 0.0f)); + linesX.push_back(line); + + linesY.push_back(point); + line = point + DataTypes::getCRot(jointWorldRigid).rotate(type::Vec3f(0.0f, 0.1f, 0.0f)); + linesY.push_back(line); + + linesZ.push_back(point); + line = point + DataTypes::getCRot(jointWorldRigid).rotate(type::Vec3f(0.0f, 0.0f, 0.1f)); + linesZ.push_back(line); + } + vparams->drawTool()->drawPoints(points, 10, sofa::type::RGBAColor (1.0f , 0.5f , 0.5f , 1.0f)); + vparams->drawTool()->drawLines (linesX, 2, sofa::type::RGBAColor (0.75f, 0.0f , 0.0f , 1.0f)); + vparams->drawTool()->drawLines (linesY, 2, sofa::type::RGBAColor (0.0f , 0.75f, 0.0f , 1.0f)); + vparams->drawTool()->drawLines (linesZ, 2, sofa::type::RGBAColor (0.0f , 0.0f , 0.75f, 1.0f)); + } + + points.clear(); + linesX.clear(); + linesY.clear(); + linesZ.clear(); + + // draw bones now + { + for(unsigned int i = 0; i < skeletonBones.getValue().size(); ++i) + { + defaulttype::RigidCoord< 3, Real> boneWorldRigid = skeletonJoints.getValue()[skeletonBones.getValue()[i]].mWorldRigid; + + point = DataTypes::getCPos(boneWorldRigid); + points.push_back(point); + + linesX.push_back(point); + line = point + DataTypes::getCRot(boneWorldRigid).rotate(type::Vec3f(0.1f, 0.0f, 0.0f)); + linesX.push_back(line); + + linesY.push_back(point); + line = point + DataTypes::getCRot(boneWorldRigid).rotate(type::Vec3f(0.0f, 0.1f, 0.0f)); + linesY.push_back(line); + + linesZ.push_back(point); + line = point + DataTypes::getCRot(boneWorldRigid).rotate(type::Vec3f(0.0f, 0.0f, 0.1f)); + linesZ.push_back(line); + } + vparams->drawTool()->drawPoints(points, 10, sofa::type::RGBAColor (1.0f, 0.5f, 0.5f, 1.0f)); + vparams->drawTool()->drawLines (linesX, 2 , sofa::type::RGBAColor (1.0f, 0.0f, 0.0f, 1.0f)); + vparams->drawTool()->drawLines (linesY, 2 , sofa::type::RGBAColor (0.0f, 1.0f, 0.0f, 1.0f)); + vparams->drawTool()->drawLines (linesZ, 2 , sofa::type::RGBAColor (0.0f, 0.0f, 1.0f, 1.0f)); + } +} + +} // namespace sofa::component::constraint::projective diff --git a/Sofa/Component/Constraint/Projective/tests/AffineMovementConstraint_test.cpp b/Sofa/Component/Constraint/Projective/tests/AffineMovementProjectiveConstraint_test.cpp similarity index 95% rename from Sofa/Component/Constraint/Projective/tests/AffineMovementConstraint_test.cpp rename to Sofa/Component/Constraint/Projective/tests/AffineMovementProjectiveConstraint_test.cpp index 82379cb5145..a1d845afc32 100644 --- a/Sofa/Component/Constraint/Projective/tests/AffineMovementConstraint_test.cpp +++ b/Sofa/Component/Constraint/Projective/tests/AffineMovementProjectiveConstraint_test.cpp @@ -27,7 +27,7 @@ using sofa::testing::NumericTest; #include #include #include -#include +#include #include #include #include @@ -42,7 +42,7 @@ namespace sofa { namespace { template -struct AffineMovementConstraint_test : public BaseSimulationTest, NumericTest +struct AffineMovementProjectiveConstraint_test : public BaseSimulationTest, NumericTest { typedef _DataTypes DataTypes; typedef typename DataTypes::Coord Coord; @@ -197,9 +197,9 @@ typedef Types< > DataTypes; // the types to instanciate. // Test suite for all the instanciations -TYPED_TEST_SUITE(AffineMovementConstraint_test, DataTypes); +TYPED_TEST_SUITE(AffineMovementProjectiveConstraint_test, DataTypes); // first test case -TYPED_TEST( AffineMovementConstraint_test , testValue ) +TYPED_TEST( AffineMovementProjectiveConstraint_test , testValue ) { EXPECT_MSG_NOEMIT(Error) ; ASSERT_TRUE( this->projectPosition(5e-6,5e-5)); diff --git a/Sofa/Component/Constraint/Projective/tests/CMakeLists.txt b/Sofa/Component/Constraint/Projective/tests/CMakeLists.txt index 042f7537a21..7e271d937b6 100644 --- a/Sofa/Component/Constraint/Projective/tests/CMakeLists.txt +++ b/Sofa/Component/Constraint/Projective/tests/CMakeLists.txt @@ -3,14 +3,14 @@ cmake_minimum_required(VERSION 3.12) project(Sofa.Component.Constraint.Projective_test) set(SOURCE_FILES - AffineMovementConstraint_test.cpp - FixedConstraint_test.cpp - FixedPlaneConstraint_test.cpp - PartialFixedConstraint_test.cpp - ProjectDirectionConstraint_test.cpp - ProjectToLineConstraint_test.cpp - ProjectToPlaneConstraint_test.cpp - ProjectToPointConstraint_test.cpp + AffineMovementProjectiveConstraint_test.cpp + FixedProjectiveConstraint_test.cpp + FixedPlaneProjectiveConstraint_test.cpp + PartialFixedProjectiveConstraint_test.cpp + DirectionProjectiveConstraint_test.cpp + LineProjectiveConstraint_test.cpp + PlaneProjectiveConstraint_test.cpp + PointProjectiveConstraint_test.cpp ) add_executable(${PROJECT_NAME} ${SOURCE_FILES}) diff --git a/Sofa/Component/Constraint/Projective/tests/ProjectDirectionConstraint_test.cpp b/Sofa/Component/Constraint/Projective/tests/DirectionProjectiveConstraint_test.cpp similarity index 92% rename from Sofa/Component/Constraint/Projective/tests/ProjectDirectionConstraint_test.cpp rename to Sofa/Component/Constraint/Projective/tests/DirectionProjectiveConstraint_test.cpp index 3a60bde1807..697184f6f30 100644 --- a/Sofa/Component/Constraint/Projective/tests/ProjectDirectionConstraint_test.cpp +++ b/Sofa/Component/Constraint/Projective/tests/DirectionProjectiveConstraint_test.cpp @@ -27,7 +27,7 @@ using sofa::testing::NumericTest; #include #include #include -#include +#include #include #include #include @@ -41,11 +41,11 @@ using namespace component; using namespace defaulttype; using namespace core::objectmodel; -/** Test suite for ProjectDirectionConstraint. +/** Test suite for DirectionProjectiveConstraint. The test cases are defined in the #Test_Cases member group. */ template -struct ProjectDirectionConstraint_test : public BaseSimulationTest, NumericTest +struct DirectionProjectiveConstraint_test : public BaseSimulationTest, NumericTest { typedef _DataTypes DataTypes; typedef typename DataTypes::VecCoord VecCoord; @@ -54,8 +54,8 @@ struct ProjectDirectionConstraint_test : public BaseSimulationTest, NumericTest< typedef typename DataTypes::Deriv Deriv; typedef typename DataTypes::CPos CPos; typedef typename Coord::value_type Real; - typedef constraint::projective::ProjectDirectionConstraint ProjectDirectionConstraint; - typedef typename ProjectDirectionConstraint::Indices Indices; + typedef constraint::projective::DirectionProjectiveConstraint DirectionProjectiveConstraint; + typedef typename DirectionProjectiveConstraint::Indices Indices; typedef component::topology::container::dynamic::PointSetTopologyContainer PointSetTopologyContainer; typedef statecontainer::MechanicalObject MechanicalObject; @@ -65,7 +65,7 @@ struct ProjectDirectionConstraint_test : public BaseSimulationTest, NumericTest< unsigned numNodes; ///< number of particles used for the test Indices indices; ///< indices of the nodes to project CPos direction; ///< direction to project to - typename ProjectDirectionConstraint::SPtr projection; + typename DirectionProjectiveConstraint::SPtr projection; typename MechanicalObject::SPtr dofs; /// Create the context for the tests. @@ -85,7 +85,7 @@ struct ProjectDirectionConstraint_test : public BaseSimulationTest, NumericTest< root->addObject(dofs); // Project direction constraint - projection = New(); + projection = New(); root->addObject(projection); /// Set the values @@ -236,10 +236,10 @@ typedef Types< > DataTypes; // the types to instanciate. // Test suite for all the instanciations -TYPED_TEST_SUITE(ProjectDirectionConstraint_test, DataTypes); +TYPED_TEST_SUITE(DirectionProjectiveConstraint_test, DataTypes); // first test case -TYPED_TEST( ProjectDirectionConstraint_test , oneConstrainedParticle ) +TYPED_TEST( DirectionProjectiveConstraint_test , oneConstrainedParticle ) { EXPECT_MSG_NOEMIT(Error) ; this->init_oneConstrainedParticle(); @@ -247,7 +247,7 @@ TYPED_TEST( ProjectDirectionConstraint_test , oneConstrainedParticle ) ASSERT_TRUE( this->test_projectVelocity() ); } // second test case -TYPED_TEST( ProjectDirectionConstraint_test , allParticlesConstrained ) +TYPED_TEST( DirectionProjectiveConstraint_test , allParticlesConstrained ) { EXPECT_MSG_NOEMIT(Error) ; this->init_allParticlesConstrained(); diff --git a/Sofa/Component/Constraint/Projective/tests/FixedPlaneConstraint_test.cpp b/Sofa/Component/Constraint/Projective/tests/FixedPlaneProjectiveConstraint_test.cpp similarity index 89% rename from Sofa/Component/Constraint/Projective/tests/FixedPlaneConstraint_test.cpp rename to Sofa/Component/Constraint/Projective/tests/FixedPlaneProjectiveConstraint_test.cpp index 5ed1c9ca855..6537e800394 100644 --- a/Sofa/Component/Constraint/Projective/tests/FixedPlaneConstraint_test.cpp +++ b/Sofa/Component/Constraint/Projective/tests/FixedPlaneProjectiveConstraint_test.cpp @@ -23,7 +23,7 @@ #include using sofa::testing::BaseSimulationTest; -#include +#include #include #include #include @@ -46,12 +46,12 @@ void createUniformMass(simulation::Node::SPtr node, component::statecontainer::M } template -struct FixedPlaneConstraint_test : public BaseSimulationTest +struct FixedPlaneProjectiveConstraint_test : public BaseSimulationTest { typedef _DataTypes DataTypes; typedef typename DataTypes::Real Real; - typedef component::constraint::projective::FixedPlaneConstraint FixedPlaneConstraint; + typedef component::constraint::projective::FixedPlaneProjectiveConstraint FixedPlaneProjectiveConstraint; typedef component::mechanicalload::ConstantForceField ForceField; typedef component::statecontainer::MechanicalObject MechanicalObject; @@ -89,7 +89,7 @@ struct FixedPlaneConstraint_test : public BaseSimulationTest typename ForceField::SPtr forceField = addNew(node); forceField->setForce( 0, force ); - typename FixedPlaneConstraint::SPtr constraint = addNew(node); + typename FixedPlaneProjectiveConstraint::SPtr constraint = addNew(node); constraint->d_indices.setValue({0}); if(constraint->d_indices.getValue().empty()) { @@ -134,22 +134,22 @@ typedef Types< > DataTypes; // the types to instanciate. // Test suite for all the instanciations -TYPED_TEST_SUITE(FixedPlaneConstraint_test, DataTypes); +TYPED_TEST_SUITE(FixedPlaneProjectiveConstraint_test, DataTypes); // test cases -TYPED_TEST( FixedPlaneConstraint_test , testContraintExplicit ) +TYPED_TEST( FixedPlaneProjectiveConstraint_test , testContraintExplicit ) { EXPECT_MSG_NOEMIT(Error) ; EXPECT_TRUE( this->test(1e-8, std::string("Explicit")) ); } -TYPED_TEST( FixedPlaneConstraint_test , testConstraintImplicitWithCG ) +TYPED_TEST( FixedPlaneProjectiveConstraint_test , testConstraintImplicitWithCG ) { EXPECT_MSG_NOEMIT(Error) ; EXPECT_TRUE( this->test(1e-8, std::string("Implicit")) ); } -TYPED_TEST( FixedPlaneConstraint_test , testConstraintImplicitWithSparseLDL ) +TYPED_TEST( FixedPlaneProjectiveConstraint_test , testConstraintImplicitWithSparseLDL ) { EXPECT_MSG_NOEMIT(Error) ; EXPECT_TRUE( this->test(1e-8, std::string("Implicit_SparseLDL")) ); diff --git a/Sofa/Component/Constraint/Projective/tests/FixedConstraint_test.cpp b/Sofa/Component/Constraint/Projective/tests/FixedProjectiveConstraint_test.cpp similarity index 92% rename from Sofa/Component/Constraint/Projective/tests/FixedConstraint_test.cpp rename to Sofa/Component/Constraint/Projective/tests/FixedProjectiveConstraint_test.cpp index e755e35fdd6..e717e5d6b82 100644 --- a/Sofa/Component/Constraint/Projective/tests/FixedConstraint_test.cpp +++ b/Sofa/Component/Constraint/Projective/tests/FixedProjectiveConstraint_test.cpp @@ -20,7 +20,7 @@ * Contact information: contact@sofa-framework.org * ******************************************************************************/ -#include +#include #include #include #include @@ -49,10 +49,10 @@ void createUniformMass(simulation::Node::SPtr node, component::statecontainer::M } template -struct FixedConstraint_test : public BaseTest +struct FixedProjectiveConstraint_test : public BaseTest { typedef _DataTypes DataTypes; - typedef component::constraint::projective::FixedConstraint FixedConstraint; + typedef component::constraint::projective::FixedProjectiveConstraint FixedProjectiveConstraint; typedef component::mechanicalload::ConstantForceField ForceField; typedef component::statecontainer::MechanicalObject MechanicalObject; @@ -106,7 +106,7 @@ struct FixedConstraint_test : public BaseTest forceField->setForce( 1, force ); /// Let's fix the particle's movement for particle number 1 (the second one). - typename FixedConstraint::SPtr cst = sofa::core::objectmodel::New(); + typename FixedProjectiveConstraint::SPtr cst = sofa::core::objectmodel::New(); cst->findData("indices")->read("1"); node->addObject(cst); @@ -197,7 +197,7 @@ struct FixedConstraint_test : public BaseTest } /// Add fixconstraint - typename FixedConstraint::SPtr cst = sofa::core::objectmodel::New(); + typename FixedProjectiveConstraint::SPtr cst = sofa::core::objectmodel::New(); type::vector indices = { 0, 1, 2 }; cst->d_indices.setValue(indices); node->addObject(cst); @@ -247,27 +247,27 @@ typedef Types< > DataTypes; // the types to instanciate. // Test suite for all the instanciations -TYPED_TEST_SUITE(FixedConstraint_test, DataTypes); +TYPED_TEST_SUITE(FixedProjectiveConstraint_test, DataTypes); // first test case -TYPED_TEST( FixedConstraint_test , testValueImplicitWithCG ) +TYPED_TEST( FixedProjectiveConstraint_test , testValueImplicitWithCG ) { EXPECT_MSG_NOEMIT(Error) ; EXPECT_TRUE( this->test(1e-8,std::string("Implicit")) ); } -TYPED_TEST( FixedConstraint_test , testValueExplicit ) +TYPED_TEST( FixedProjectiveConstraint_test , testValueExplicit ) { EXPECT_MSG_NOEMIT(Error) ; EXPECT_TRUE( this->test(1e-8, std::string("Explicit")) ); } -TYPED_TEST( FixedConstraint_test , testValueImplicitWithSparseLDL ) +TYPED_TEST( FixedProjectiveConstraint_test , testValueImplicitWithSparseLDL ) { EXPECT_MSG_NOEMIT(Error) ; EXPECT_TRUE( this->test(1e-8, std::string("Implicit_SparseLDL")) ); } -TYPED_TEST(FixedConstraint_test, testTopologicalChanges) +TYPED_TEST(FixedProjectiveConstraint_test, testTopologicalChanges) { EXPECT_MSG_NOEMIT(Error); EXPECT_TRUE(this->testTopologicalChanges()); diff --git a/Sofa/Component/Constraint/Projective/tests/ProjectToLineConstraint_test.cpp b/Sofa/Component/Constraint/Projective/tests/LineProjectiveConstraint_test.cpp similarity index 92% rename from Sofa/Component/Constraint/Projective/tests/ProjectToLineConstraint_test.cpp rename to Sofa/Component/Constraint/Projective/tests/LineProjectiveConstraint_test.cpp index 8308103367a..bda02f7bf43 100644 --- a/Sofa/Component/Constraint/Projective/tests/ProjectToLineConstraint_test.cpp +++ b/Sofa/Component/Constraint/Projective/tests/LineProjectiveConstraint_test.cpp @@ -27,7 +27,7 @@ using sofa::testing::NumericTest; #include #include #include -#include +#include #include #include #include @@ -39,11 +39,11 @@ using namespace defaulttype; -/** Test suite for ProjectToLineConstraint. +/** Test suite for LineProjectiveConstraint. The test cases are defined in the #Test_Cases member group. */ template -struct ProjectToLineConstraint_test : public BaseSimulationTest, NumericTest +struct LineProjectiveConstraint_test : public BaseSimulationTest, NumericTest { typedef _DataTypes DataTypes; typedef typename DataTypes::VecCoord VecCoord; @@ -52,8 +52,8 @@ struct ProjectToLineConstraint_test : public BaseSimulationTest, NumericTest ProjectToLineConstraint; - typedef typename ProjectToLineConstraint::Indices Indices; + typedef constraint::projective::LineProjectiveConstraint LineProjectiveConstraint; + typedef typename LineProjectiveConstraint::Indices Indices; typedef component::topology::container::dynamic::PointSetTopologyContainer PointSetTopologyContainer; typedef statecontainer::MechanicalObject MechanicalObject; @@ -64,7 +64,7 @@ struct ProjectToLineConstraint_test : public BaseSimulationTest, NumericTest(); root->addObject(dofs); - projection = core::objectmodel::New(); + projection = core::objectmodel::New(); root->addObject(projection); /// Set the values @@ -221,9 +221,9 @@ typedef Types< > DataTypes; // the types to instanciate. // Test suite for all the instanciations -TYPED_TEST_SUITE(ProjectToLineConstraint_test, DataTypes); +TYPED_TEST_SUITE(LineProjectiveConstraint_test, DataTypes); // first test case -TYPED_TEST( ProjectToLineConstraint_test , oneConstrainedParticle ) +TYPED_TEST( LineProjectiveConstraint_test , oneConstrainedParticle ) { EXPECT_MSG_NOEMIT(Error) ; this->init_oneConstrainedParticle(); @@ -231,7 +231,7 @@ TYPED_TEST( ProjectToLineConstraint_test , oneConstrainedParticle ) ASSERT_TRUE( this->test_projectVelocity() ); } // next test case -TYPED_TEST( ProjectToLineConstraint_test , allParticlesConstrained ) +TYPED_TEST( LineProjectiveConstraint_test , allParticlesConstrained ) { EXPECT_MSG_NOEMIT(Error) ; this->init_allParticlesConstrained(); diff --git a/Sofa/Component/Constraint/Projective/tests/PartialFixedConstraint_test.cpp b/Sofa/Component/Constraint/Projective/tests/PartialFixedProjectiveConstraint_test.cpp similarity index 88% rename from Sofa/Component/Constraint/Projective/tests/PartialFixedConstraint_test.cpp rename to Sofa/Component/Constraint/Projective/tests/PartialFixedProjectiveConstraint_test.cpp index a7b462c0f5e..3ca05c51679 100644 --- a/Sofa/Component/Constraint/Projective/tests/PartialFixedConstraint_test.cpp +++ b/Sofa/Component/Constraint/Projective/tests/PartialFixedProjectiveConstraint_test.cpp @@ -22,7 +22,7 @@ #include using sofa::testing::BaseSimulationTest; -#include +#include #include #include #include @@ -45,12 +45,12 @@ void createUniformMass(simulation::Node::SPtr node, component::statecontainer::M } template -struct PartialFixedConstraint_test : public BaseSimulationTest +struct PartialFixedProjectiveConstraint_test : public BaseSimulationTest { typedef _DataTypes DataTypes; typedef typename DataTypes::Real Real; - typedef component::constraint::projective::PartialFixedConstraint PartialFixedConstraint; + typedef component::constraint::projective::PartialFixedProjectiveConstraint PartialFixedProjectiveConstraint; typedef component::mechanicalload::ConstantForceField ForceField; typedef component::statecontainer::MechanicalObject MechanicalObject; @@ -92,7 +92,7 @@ struct PartialFixedConstraint_test : public BaseSimulationTest typename ForceField::SPtr forceField = addNew(node); forceField->setForce( 0, force ); - typename PartialFixedConstraint::SPtr constraint = addNew(node); + typename PartialFixedProjectiveConstraint::SPtr constraint = addNew(node); // Init simulation sofa::simulation::node::initRoot(root.get()); @@ -134,22 +134,22 @@ typedef Types< > DataTypes; // the types to instanciate. // Test suite for all the instanciations -TYPED_TEST_SUITE(PartialFixedConstraint_test, DataTypes); +TYPED_TEST_SUITE(PartialFixedProjectiveConstraint_test, DataTypes); // test cases -TYPED_TEST( PartialFixedConstraint_test , testContraintExplicit ) +TYPED_TEST( PartialFixedProjectiveConstraint_test , testContraintExplicit ) { EXPECT_MSG_NOEMIT(Error) ; EXPECT_TRUE( this->test(1e-8, std::string("Explicit")) ); } -TYPED_TEST( PartialFixedConstraint_test , testContraintImplicitWithCG ) +TYPED_TEST( PartialFixedProjectiveConstraint_test , testContraintImplicitWithCG ) { EXPECT_MSG_NOEMIT(Error) ; EXPECT_TRUE( this->test(1e-8, std::string("Implicit")) ); } -TYPED_TEST( PartialFixedConstraint_test , testContraintImplicitWithSparseLDL ) +TYPED_TEST( PartialFixedProjectiveConstraint_test , testContraintImplicitWithSparseLDL ) { EXPECT_MSG_NOEMIT(Error) ; EXPECT_TRUE( this->test(1e-8, std::string("Implicit_SparseLDL")) ); diff --git a/Sofa/Component/Constraint/Projective/tests/ProjectToPlaneConstraint_test.cpp b/Sofa/Component/Constraint/Projective/tests/PlaneProjectiveConstraint_test.cpp similarity index 92% rename from Sofa/Component/Constraint/Projective/tests/ProjectToPlaneConstraint_test.cpp rename to Sofa/Component/Constraint/Projective/tests/PlaneProjectiveConstraint_test.cpp index 76c5e46ae25..c09830aed0c 100644 --- a/Sofa/Component/Constraint/Projective/tests/ProjectToPlaneConstraint_test.cpp +++ b/Sofa/Component/Constraint/Projective/tests/PlaneProjectiveConstraint_test.cpp @@ -27,7 +27,7 @@ using sofa::testing::NumericTest; #include #include #include -#include +#include #include #include #include @@ -40,11 +40,11 @@ using namespace defaulttype; -/** Test suite for ProjectToPlaneConstraint. +/** Test suite for PlaneProjectiveConstraint. The test cases are defined in the #Test_Cases member group. */ template -struct ProjectToPlaneConstraint_test : public BaseSimulationTest, NumericTest +struct PlaneProjectiveConstraint_test : public BaseSimulationTest, NumericTest { typedef _DataTypes DataTypes; typedef typename DataTypes::VecCoord VecCoord; @@ -53,8 +53,8 @@ struct ProjectToPlaneConstraint_test : public BaseSimulationTest, NumericTest ProjectToPlaneConstraint; - typedef typename ProjectToPlaneConstraint::Indices Indices; + typedef constraint::projective::PlaneProjectiveConstraint PlaneProjectiveConstraint; + typedef typename PlaneProjectiveConstraint::Indices Indices; typedef component::topology::container::dynamic::PointSetTopologyContainer PointSetTopologyContainer; typedef statecontainer::MechanicalObject MechanicalObject; @@ -65,7 +65,7 @@ struct ProjectToPlaneConstraint_test : public BaseSimulationTest, NumericTest(); root->addObject(dofs); - projection = core::objectmodel::New(); + projection = core::objectmodel::New(); root->addObject(projection); /// Set the values @@ -220,9 +220,9 @@ typedef Types< > DataTypes; // the types to instanciate. // Test suite for all the instanciations -TYPED_TEST_SUITE(ProjectToPlaneConstraint_test, DataTypes); +TYPED_TEST_SUITE(PlaneProjectiveConstraint_test, DataTypes); // first test case -TYPED_TEST( ProjectToPlaneConstraint_test , oneConstrainedParticle ) +TYPED_TEST( PlaneProjectiveConstraint_test , oneConstrainedParticle ) { EXPECT_MSG_NOEMIT(Error) ; this->init_oneConstrainedParticle(); @@ -230,7 +230,7 @@ TYPED_TEST( ProjectToPlaneConstraint_test , oneConstrainedParticle ) ASSERT_TRUE( this->test_projectVelocity() ); } // next test case -TYPED_TEST( ProjectToPlaneConstraint_test , allParticlesConstrained ) +TYPED_TEST( PlaneProjectiveConstraint_test , allParticlesConstrained ) { EXPECT_MSG_NOEMIT(Error) ; this->init_allParticlesConstrained(); diff --git a/Sofa/Component/Constraint/Projective/tests/ProjectToPointConstraint_test.cpp b/Sofa/Component/Constraint/Projective/tests/PointProjectiveConstraint_test.cpp similarity index 92% rename from Sofa/Component/Constraint/Projective/tests/ProjectToPointConstraint_test.cpp rename to Sofa/Component/Constraint/Projective/tests/PointProjectiveConstraint_test.cpp index 1b33c49fe5f..fdcbc6ddfa5 100644 --- a/Sofa/Component/Constraint/Projective/tests/ProjectToPointConstraint_test.cpp +++ b/Sofa/Component/Constraint/Projective/tests/PointProjectiveConstraint_test.cpp @@ -27,7 +27,7 @@ using sofa::testing::NumericTest; #include #include #include -#include +#include #include #include #include @@ -41,11 +41,11 @@ using namespace defaulttype; using sofa::core::objectmodel::New; -/** Test suite for ProjectToPointConstraint. +/** Test suite for PointProjectiveConstraint. The test cases are defined in the #Test_Cases member group. */ template -struct ProjectToPointConstraint_test : public BaseSimulationTest, NumericTest +struct PointProjectiveConstraint_test : public BaseSimulationTest, NumericTest { typedef _DataTypes DataTypes; typedef typename DataTypes::VecCoord VecCoord; @@ -54,8 +54,8 @@ struct ProjectToPointConstraint_test : public BaseSimulationTest, NumericTest ProjectToPointConstraint; - typedef typename ProjectToPointConstraint::SetIndexArray Indices; + typedef constraint::projective::PointProjectiveConstraint PointProjectiveConstraint; + typedef typename PointProjectiveConstraint::SetIndexArray Indices; typedef component::topology::container::dynamic::PointSetTopologyContainer PointSetTopologyContainer; typedef statecontainer::MechanicalObject MechanicalObject; @@ -65,7 +65,7 @@ struct ProjectToPointConstraint_test : public BaseSimulationTest, NumericTestaddObject(dofs); // Project to point constraint - projection = New(); + projection = New(); root->addObject(projection); /// Set the values @@ -224,9 +224,9 @@ typedef Types< > DataTypes; // the types to instanciate. // Test suite for all the instanciations -TYPED_TEST_SUITE(ProjectToPointConstraint_test, DataTypes); +TYPED_TEST_SUITE(PointProjectiveConstraint_test, DataTypes); // first test case -TYPED_TEST( ProjectToPointConstraint_test , oneConstrainedParticle ) +TYPED_TEST( PointProjectiveConstraint_test , oneConstrainedParticle ) { EXPECT_MSG_NOEMIT(Error) ; this->init_oneConstrainedParticle(); @@ -234,7 +234,7 @@ TYPED_TEST( ProjectToPointConstraint_test , oneConstrainedParticle ) ASSERT_TRUE( this->test_projectVelocity() ); } // next test case -TYPED_TEST( ProjectToPointConstraint_test , allParticlesConstrained ) +TYPED_TEST( PointProjectiveConstraint_test , allParticlesConstrained ) { EXPECT_MSG_NOEMIT(Error) ; this->init_allParticlesConstrained(); diff --git a/Sofa/Component/Diffusion/tests/scenes/TetrahedronDiffusionFEMForceField.scn b/Sofa/Component/Diffusion/tests/scenes/TetrahedronDiffusionFEMForceField.scn index b54a164645c..c5a6db9c33b 100644 --- a/Sofa/Component/Diffusion/tests/scenes/TetrahedronDiffusionFEMForceField.scn +++ b/Sofa/Component/Diffusion/tests/scenes/TetrahedronDiffusionFEMForceField.scn @@ -1,6 +1,6 @@ - + @@ -36,8 +36,8 @@ - - + + diff --git a/Sofa/Component/Mapping/NonLinear/tests/SquareDistanceMapping_test.cpp b/Sofa/Component/Mapping/NonLinear/tests/SquareDistanceMapping_test.cpp index cf5d2724d67..8472a6f1a3c 100644 --- a/Sofa/Component/Mapping/NonLinear/tests/SquareDistanceMapping_test.cpp +++ b/Sofa/Component/Mapping/NonLinear/tests/SquareDistanceMapping_test.cpp @@ -168,7 +168,7 @@ struct SquareDistanceMappingCompare_test : NumericTest {{"position", "@../loader.position"}, {"edges", "@../loader.edges"}, {"name", "topology"}}); simpleapi::createObject(node, "MechanicalObject", {{"name", "defoDOF"}, {"template", "Vec3"}}); simpleapi::createObject(node, "EdgeSetGeometryAlgorithms"); - simpleapi::createObject(node, "FixedConstraint", {{"indices", "0"}}); + simpleapi::createObject(node, "FixedProjectiveConstraint", {{"indices", "0"}}); simpleapi::createObject(node, "DiagonalMass", {{"totalMass", "1e-2"}}); } diff --git a/Sofa/Component/MechanicalLoad/tests/SkeletalMotionConstraint_test.cpp b/Sofa/Component/MechanicalLoad/tests/SkeletalMotionConstraint_test.cpp index eb5c9790beb..f6a846d532b 100644 --- a/Sofa/Component/MechanicalLoad/tests/SkeletalMotionConstraint_test.cpp +++ b/Sofa/Component/MechanicalLoad/tests/SkeletalMotionConstraint_test.cpp @@ -27,7 +27,7 @@ using sofa::testing::NumericTest; #include #include #include -#include +#include #include #include #include @@ -44,7 +44,7 @@ using namespace defaulttype; The test cases are defined in the #Test_Cases member group. */ template -struct SkeletalMotionConstraint_test : public BaseSimulationTest, NumericTest +struct SkeletalMotionProjectiveConstraint_test : public BaseSimulationTest, NumericTest { typedef _DataTypes DataTypes; typedef typename DataTypes::VecCoord VecCoord; @@ -55,7 +55,7 @@ struct SkeletalMotionConstraint_test : public BaseSimulationTest, NumericTest SkeletalMotionConstraint; + typedef constraint::projective::SkeletalMotionProjectiveConstraint SkeletalMotionProjectiveConstraint; typedef constraint::projective::SkeletonJoint SkeletonJoint; typedef statecontainer::MechanicalObject MechanicalObject; @@ -63,7 +63,7 @@ struct SkeletalMotionConstraint_test : public BaseSimulationTest, NumericTest joints; ///< skeletal joint - typename SkeletalMotionConstraint::SPtr projection; + typename SkeletalMotionProjectiveConstraint::SPtr projection; typename MechanicalObject::SPtr dofs; /// Create the context for the tests. @@ -77,7 +77,7 @@ struct SkeletalMotionConstraint_test : public BaseSimulationTest, NumericTest(); root->addObject(dofs); - projection = core::objectmodel::New(); + projection = core::objectmodel::New(); root->addObject(projection); @@ -184,9 +184,9 @@ typedef Types< > DataTypes; // the types to instanciate. // Test suite for all the instanciations -TYPED_TEST_SUITE(SkeletalMotionConstraint_test, DataTypes); +TYPED_TEST_SUITE(SkeletalMotionProjectiveConstraint_test, DataTypes); // first test case -TYPED_TEST( SkeletalMotionConstraint_test , twoConstrainedBones ) +TYPED_TEST( SkeletalMotionProjectiveConstraint_test , twoConstrainedBones ) { EXPECT_MSG_NOEMIT(Error) ; this->init_2bones(); diff --git a/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/StaticSolver.cpp b/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/StaticSolver.cpp index 04679365b1a..e999ad4621f 100644 --- a/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/StaticSolver.cpp +++ b/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/StaticSolver.cpp @@ -174,7 +174,7 @@ void StaticSolver::solve(const sofa::core::ExecParams* params, SReal dt, sofa::c // Step 2 Projective constraints // Calls the "projectResponse" method of every BaseProjectiveConstraintSet objects found in the - // current context tree. An example of such constraint set is the FixedConstraint. In this case, + // current context tree. An example of such constraint set is the FixedProjectiveConstraint. In this case, // it will set to 0 every row (i, _) of the right-hand side (force) vector for the ith degree of // freedom. mop.projectResponse(force); @@ -218,7 +218,7 @@ void StaticSolver::solve(const sofa::core::ExecParams* params, SReal dt, sofa::c // B. For LinearSolver using other type of matrices (FullMatrix, SparseMatrix, CompressedRowSparseMatrix), // the "addMBKToMatrix" method is called on each BaseForceField objects and the "applyConstraint" method // is called on every BaseProjectiveConstraintSet objects. An example of such constraint set is the - // FixedConstraint. In this case, it will set to 0 every column (_, i) and row (i, _) of the assembled + // FixedProjectiveConstraint. In this case, it will set to 0 every column (_, i) and row (i, _) of the assembled // matrix for the ith degree of freedom. matrix.setSystemMBKMatrix(MechanicalMatrix::K * -1.0); } diff --git a/Sofa/Component/ODESolver/Backward/tests/EulerImplicitSolverStatic_test.cpp b/Sofa/Component/ODESolver/Backward/tests/EulerImplicitSolverStatic_test.cpp index 226deeb18ca..6e35c3245b1 100644 --- a/Sofa/Component/ODESolver/Backward/tests/EulerImplicitSolverStatic_test.cpp +++ b/Sofa/Component/ODESolver/Backward/tests/EulerImplicitSolverStatic_test.cpp @@ -131,7 +131,7 @@ struct EulerImplicit_test_2_particles_to_equilibrium : public BaseSimulationTest 0.1 // damping ratio ); - simpleapi::createObject(string, "FixedConstraint", { + simpleapi::createObject(string, "FixedProjectiveConstraint", { { "indices", "0"} }); diff --git a/Sofa/Component/ODESolver/Backward/tests/StaticSolver_test.cpp b/Sofa/Component/ODESolver/Backward/tests/StaticSolver_test.cpp index 512f0f2a4fe..b13f1a66af3 100644 --- a/Sofa/Component/ODESolver/Backward/tests/StaticSolver_test.cpp +++ b/Sofa/Component/ODESolver/Backward/tests/StaticSolver_test.cpp @@ -71,7 +71,7 @@ class StaticSolverTest : public sofa::testing::BaseTest }); createObject(root, "BoxROI", {{"name", "top_roi"}, {"box", "-7.5 -7.5 -0.9 7.5 7.5 0.1"}}); - createObject(root, "FixedConstraint", {{"indices", "@top_roi.indices"}}); + createObject(root, "FixedProjectiveConstraint", {{"indices", "@top_roi.indices"}}); createObject(root, "BoxROI", {{"name", "base_roi"}, {"box", "-7.5 -7.5 79.9 7.5 7.5 80.1"}}); createObject(root, "SurfacePressureForceField", {{"pressure", "100"}, {"mainDirection", "0 -1 0"}, {"triangleIndices", "@base_roi.trianglesInROI"}}); diff --git a/Sofa/Component/ODESolver/Backward/tests/scenes/EulerImplicitSpringDynamicTest.xml b/Sofa/Component/ODESolver/Backward/tests/scenes/EulerImplicitSpringDynamicTest.xml index 9d80018d423..6164322ee8d 100644 --- a/Sofa/Component/ODESolver/Backward/tests/scenes/EulerImplicitSpringDynamicTest.xml +++ b/Sofa/Component/ODESolver/Backward/tests/scenes/EulerImplicitSpringDynamicTest.xml @@ -1,6 +1,6 @@ - + @@ -14,7 +14,7 @@ - + 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 59a9e6d7f90..40f671c2032 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 @@ -297,7 +297,7 @@ void EulerExplicitSolver::projectResponse(sofa::simulation::common::MechanicalOp SCOPED_TIMER("projectResponse"); // Calls the "projectResponse" method of every BaseProjectiveConstraintSet objects found in the - // current context tree. An example of such constraint set is the FixedConstraint. In this case, + // current context tree. An example of such constraint set is the FixedProjectiveConstraint. In this case, // it will set to 0 every row (i, _) of the input vector for the ith degree of freedom. mop->projectResponse(vecId); } @@ -326,7 +326,7 @@ void EulerExplicitSolver::assembleSystemMatrix(core::behavior::MultiMatrix - + @@ -27,7 +27,7 @@ - + diff --git a/Sofa/Component/SolidMechanics/FEM/HyperElastic/tests/scenes/TetrahedronHyperelasticityFEMForceField_test.scn b/Sofa/Component/SolidMechanics/FEM/HyperElastic/tests/scenes/TetrahedronHyperelasticityFEMForceField_test.scn index 275d2734611..cfcca0d0492 100644 --- a/Sofa/Component/SolidMechanics/FEM/HyperElastic/tests/scenes/TetrahedronHyperelasticityFEMForceField_test.scn +++ b/Sofa/Component/SolidMechanics/FEM/HyperElastic/tests/scenes/TetrahedronHyperelasticityFEMForceField_test.scn @@ -1,7 +1,7 @@ - + @@ -24,7 +24,7 @@ - + diff --git a/Sofa/Component/SolidMechanics/simutests/AffinePatch_test.cpp b/Sofa/Component/SolidMechanics/simutests/AffinePatch_test.cpp index 90c87619417..b0779c4062b 100644 --- a/Sofa/Component/SolidMechanics/simutests/AffinePatch_test.cpp +++ b/Sofa/Component/SolidMechanics/simutests/AffinePatch_test.cpp @@ -30,7 +30,7 @@ #include // Including constraint, force and mass -#include +#include #include #include #include @@ -61,7 +61,7 @@ struct AffinePatch_sofa_test : public sofa::testing::BaseSimulationTest, sofa::t typedef typename DataTypes::VecDeriv VecDeriv; typedef typename DataTypes::Deriv Deriv; typedef typename DataTypes::Real Real; - typedef constraint::projective::AffineMovementConstraint AffineMovementConstraint; + typedef constraint::projective::AffineMovementProjectiveConstraint AffineMovementProjectiveConstraint; typedef statecontainer::MechanicalObject MechanicalObject; typedef typename component::solidmechanics::spring::MeshSpringForceField MeshSpringForceField; typedef typename component::solidmechanics::fem::elastic::TetrahedronFEMForceField TetraForceField; diff --git a/Sofa/Component/SolidMechanics/simutests/LinearElasticity_test.cpp b/Sofa/Component/SolidMechanics/simutests/LinearElasticity_test.cpp index 197433adacf..b9d34e25cc2 100644 --- a/Sofa/Component/SolidMechanics/simutests/LinearElasticity_test.cpp +++ b/Sofa/Component/SolidMechanics/simutests/LinearElasticity_test.cpp @@ -39,9 +39,9 @@ #include #include #include -#include -#include -#include +#include +#include +#include #include namespace sofa { @@ -136,13 +136,13 @@ CylinderTractionStruct createCylinderTractionScene( typename BoxRoi::SPtr boxRoi1 = modeling::addNew(root,"boxRoiFix"); boxRoi1->d_alignedBoxes.setValue(vecBox); boxRoi1->d_strict.setValue(false); - // FixedConstraint - typename component::constraint::projective::FixedConstraint::SPtr fc= - modeling::addNew >(root); + // FixedProjectiveConstraint + typename component::constraint::projective::FixedProjectiveConstraint::SPtr fc= + modeling::addNew >(root); sofa::modeling::setDataLink(&boxRoi1->d_indices,&fc->d_indices); - // FixedPlaneConstraint - typename component::constraint::projective::FixedPlaneConstraint::SPtr fpc= - modeling::addNew >(root); + // FixedPlaneProjectiveConstraint + typename component::constraint::projective::FixedPlaneProjectiveConstraint::SPtr fpc= + modeling::addNew >(root); fpc->d_dmin= -0.01; fpc->d_dmax= 0.01; fpc->d_direction=Coord(0,0,1); @@ -158,9 +158,9 @@ CylinderTractionStruct createCylinderTractionScene( modeling::addNew >(root); tractionStruct.forceField=tpff; sofa::modeling::setDataLink(&boxRoi2->d_triangleIndices,&tpff->triangleList); - // ProjectToLineConstraint - typename component::constraint::projective::ProjectToLineConstraint::SPtr ptlc= - modeling::addNew >(root); + // LineProjectiveConstraint + typename component::constraint::projective::LineProjectiveConstraint::SPtr ptlc= + modeling::addNew >(root); ptlc->f_direction=Coord(1,0,0); ptlc->f_origin=Coord(0,0,0); sofa::type::vector vArray; diff --git a/Sofa/Component/Topology/Testing/src/sofa/component/topology/testing/RegularGridNodeCreation.h b/Sofa/Component/Topology/Testing/src/sofa/component/topology/testing/RegularGridNodeCreation.h index d4b22e9e5c6..6a6799a8d6e 100644 --- a/Sofa/Component/Topology/Testing/src/sofa/component/topology/testing/RegularGridNodeCreation.h +++ b/Sofa/Component/Topology/Testing/src/sofa/component/topology/testing/RegularGridNodeCreation.h @@ -40,7 +40,7 @@ template struct PatchTestStruct { simulation::Node::SPtr SquareNode; - typename component::constraint::projective::AffineMovementConstraint::SPtr affineConstraint; + typename component::constraint::projective::AffineMovementProjectiveConstraint::SPtr affineConstraint; typename component::statecontainer::MechanicalObject::SPtr dofs; }; @@ -66,7 +66,7 @@ PatchTestStruct createRegularGridScene( typedef component::topology::container::grid::RegularGridTopology RegularGridTopology; typedef typename component::engine::select::BoxROI BoxRoi; typedef typename sofa::component::engine::select::PairBoxROI PairBoxRoi; - typedef typename component::constraint::projective::AffineMovementConstraint AffineMovementConstraint; + typedef typename component::constraint::projective::AffineMovementProjectiveConstraint AffineMovementProjectiveConstraint; typedef component::linearsolver::iterative::CGLinearSolver CGLinearSolver; // Root node @@ -114,7 +114,7 @@ PatchTestStruct createRegularGridScene( pairBoxRoi->includedBox.setValue(includedBox); //Affine constraint - patchStruct.affineConstraint = modeling::addNew(SquareNode,"affineConstraint"); + patchStruct.affineConstraint = modeling::addNew(SquareNode,"affineConstraint"); modeling::setDataLink(&boxRoi->d_indices,&patchStruct.affineConstraint->m_meshIndices); modeling::setDataLink(&pairBoxRoi->f_indices,& patchStruct.affineConstraint->m_indices); diff --git a/Sofa/GUI/Component/src/sofa/gui/component/performer/ConstraintAttachBodyPerformer.h b/Sofa/GUI/Component/src/sofa/gui/component/performer/ConstraintAttachBodyPerformer.h index 76657bbf82d..40547d5856a 100644 --- a/Sofa/GUI/Component/src/sofa/gui/component/performer/ConstraintAttachBodyPerformer.h +++ b/Sofa/GUI/Component/src/sofa/gui/component/performer/ConstraintAttachBodyPerformer.h @@ -25,7 +25,7 @@ #include #include #include -#include +#include #include @@ -57,7 +57,7 @@ class ConstraintAttachBodyPerformer: public TInteractionPerformer typedef typename DataTypes::VecCoord VecCoord; typedef sofa::component::collision::response::mapper::BaseContactMapper< DataTypes > MouseContactMapper; typedef sofa::core::behavior::MechanicalState< DataTypes > MouseContainer; -// typedef sofa::component::constraint::lagrangian::model::BilateralInteractionConstraint< DataTypes > MouseConstraint; +// typedef sofa::component::constraint::lagrangian::model::BilateralInteractionLagrangianConstraint< DataTypes > MouseConstraint; // typedef sofa::core::behavior::BaseForceField MouseForceField; @@ -91,7 +91,7 @@ class ConstraintAttachBodyPerformer: public TInteractionPerformer virtual bool start_partial(const BodyPicked& picked); MouseContactMapper *mapper; - sofa::component::constraint::lagrangian::model::BilateralInteractionConstraint::SPtr m_constraint; + sofa::component::constraint::lagrangian::model::BilateralInteractionLagrangianConstraint::SPtr m_constraint; core::visual::DisplayFlags flags; diff --git a/Sofa/GUI/Component/src/sofa/gui/component/performer/ConstraintAttachBodyPerformer.inl b/Sofa/GUI/Component/src/sofa/gui/component/performer/ConstraintAttachBodyPerformer.inl index 1c42a3fbdae..bce589b32d1 100644 --- a/Sofa/GUI/Component/src/sofa/gui/component/performer/ConstraintAttachBodyPerformer.inl +++ b/Sofa/GUI/Component/src/sofa/gui/component/performer/ConstraintAttachBodyPerformer.inl @@ -172,10 +172,10 @@ bool ConstraintAttachBodyPerformer::start_partial(const BodyPicked& p type::Vec3d point1; type::Vec3d point2; - using sofa::component::constraint::lagrangian::model::BilateralInteractionConstraint; + using sofa::component::constraint::lagrangian::model::BilateralInteractionLagrangianConstraint; - m_constraint = sofa::core::objectmodel::New >(mstate1, mstate2); - BilateralInteractionConstraint< DataTypes >* bconstraint = static_cast< BilateralInteractionConstraint< sofa::defaulttype::Vec3Types >* >(m_constraint.get()); + m_constraint = sofa::core::objectmodel::New >(mstate1, mstate2); + BilateralInteractionLagrangianConstraint< DataTypes >* bconstraint = static_cast< BilateralInteractionLagrangianConstraint< sofa::defaulttype::Vec3Types >* >(m_constraint.get()); bconstraint->setName("Constraint-Mouse-Contact"); type::Vec3d normal = point1-point2; diff --git a/Sofa/GUI/Component/src/sofa/gui/component/performer/FixParticlePerformer.inl b/Sofa/GUI/Component/src/sofa/gui/component/performer/FixParticlePerformer.inl index b96e142bbbd..a52c148ef2f 100644 --- a/Sofa/GUI/Component/src/sofa/gui/component/performer/FixParticlePerformer.inl +++ b/Sofa/GUI/Component/src/sofa/gui/component/performer/FixParticlePerformer.inl @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include @@ -62,7 +62,7 @@ void FixParticlePerformer::start() //Fix all the points - typename sofa::component::constraint::projective::FixedConstraint::SPtr fixFixation = sofa::core::objectmodel::New< sofa::component::constraint::projective::FixedConstraint >(); + typename sofa::component::constraint::projective::FixedProjectiveConstraint::SPtr fixFixation = sofa::core::objectmodel::New< sofa::component::constraint::projective::FixedProjectiveConstraint >(); fixFixation->d_fixAll.setValue(true); nodeFixation->addObject(fixFixation); diff --git a/Sofa/GUI/Component/src/sofa/gui/component/performer/SuturePointPerformer.h b/Sofa/GUI/Component/src/sofa/gui/component/performer/SuturePointPerformer.h index b12adce22fb..4b93b261499 100644 --- a/Sofa/GUI/Component/src/sofa/gui/component/performer/SuturePointPerformer.h +++ b/Sofa/GUI/Component/src/sofa/gui/component/performer/SuturePointPerformer.h @@ -51,7 +51,7 @@ class SOFA_GUI_COMPONENT_API SuturePointPerformer: public TInteractionPerformer< typedef typename DataTypes::Real Real; typedef sofa::component::solidmechanics::spring::LinearSpring Spring; typedef sofa::component::solidmechanics::spring::StiffSpringForceField SpringObjectType; - typedef sofa::component::constraint::projective::FixedConstraint FixObjectType; + typedef sofa::component::constraint::projective::FixedProjectiveConstraint FixObjectType; SuturePointPerformer(BaseMouseInteractor *i); ~SuturePointPerformer(); diff --git a/Sofa/framework/Core/src/sofa/core/ObjectFactory.cpp b/Sofa/framework/Core/src/sofa/core/ObjectFactory.cpp index 412011f4879..172e3c98442 100644 --- a/Sofa/framework/Core/src/sofa/core/ObjectFactory.cpp +++ b/Sofa/framework/Core/src/sofa/core/ObjectFactory.cpp @@ -206,14 +206,20 @@ objectmodel::BaseObject::SPtr ObjectFactory::createObject(objectmodel::BaseConte using sofa::helper::lifecycle::ComponentChange; using sofa::helper::lifecycle::uncreatableComponents; + using sofa::helper::lifecycle::movedComponents; if(it == registry.end()) { arg->logError("The object '" + classname + "' is not in the factory."); auto uuncreatableComponent = uncreatableComponents.find(classname); + auto movedComponent = movedComponents.find(classname); if( uuncreatableComponent != uncreatableComponents.end() ) { arg->logError( uuncreatableComponent->second.getMessage() ); } + else if (movedComponent != movedComponents.end()) + { + arg->logError( movedComponent->second.getMessage() ); + } else { std::vector possibleNames; diff --git a/Sofa/framework/Helper/src/sofa/helper/ComponentChange.cpp b/Sofa/framework/Helper/src/sofa/helper/ComponentChange.cpp index a3cb1196bed..5a0880c3747 100644 --- a/Sofa/framework/Helper/src/sofa/helper/ComponentChange.cpp +++ b/Sofa/framework/Helper/src/sofa/helper/ComponentChange.cpp @@ -32,7 +32,7 @@ const std::map > deprecatedComponents = { {"RigidRigidMapping", Deprecated("v23.06", "v23.12", "You can use the component RigidMapping with template='Rigid3,Rigid3' instead.")}, }; -const std::map > uncreatableComponents = { +const std::map > movedComponents = { // SofaValidation was pluginized in #1302 {"CompareState", Pluginized("v20.06", "SofaValidation")}, {"CompareTopology", Pluginized("v20.06", "SofaValidation")}, @@ -66,107 +66,6 @@ const std::map > uncreatableComponents { "LMConstraintSolver", Pluginized("v20.12", "LMConstraint") }, { "LMConstraintDirectSolver", Pluginized("v20.12", "LMConstraint") }, - /***********************/ - // REMOVED SINCE v23.06 - - { "OglGrid", Removed("v22.12", "v23.06")}, - { "OglLineAxis", Removed("v22.12", "v23.06")}, - - /***********************/ - // REMOVED SINCE v22.06 - - {"PointConstraint", Removed("v21.12", "v22.06")}, - - /***********************/ - // REMOVED SINCE v21.12 - - { "LMDNewProximityIntersection", Removed("v21.12", "v21.12") }, - { "LocalMinDistanceFilter", Removed("v21.12", "v21.12") }, - { "LineLocalMinDistanceFilter", Removed("v21.12", "v21.12") }, - { "PointLocalMinDistanceFilter", Removed("v21.12", "v21.12") }, - { "TriangleLocalMinDistanceFilter", Removed("v21.12", "v21.12") }, - - /***********************/ - // REMOVED SINCE v21.06 - - {"LengthContainer", Removed("v21.06", "v21.06")}, - {"PoissonContainer", Removed("v21.06", "v21.06")}, - {"RadiusContainer", Removed("v21.06", "v21.06")}, - {"StiffnessContainer", Removed("v21.06", "v21.06")}, - - /***********************/ - // REMOVED SINCE v20.12 - - { "DynamicSparseGridTopologyAlgorithms", Removed("v20.12", "v20.12") }, - { "HexahedronSetTopologyAlgorithms", Removed("v20.12", "v20.12") }, - { "TetrahedronSetTopologyAlgorithms", Removed("v20.12", "v20.12") }, - { "QuadSetTopologyAlgorithms", Removed("v20.12", "v20.12") }, - { "TriangleSetTopologyAlgorithms", Removed("v20.12", "v20.12") }, - { "EdgeSetTopologyAlgorithms", Removed("v20.12", "v20.12") }, - { "PointSetTopologyAlgorithms", Removed("v20.12", "v20.12") }, - - /***********************/ - // REMOVED SINCE v20.06 - - {"Euler", Removed("v19.12", "v20.06")}, - {"EulerExplicit", Removed("v19.12", "v20.06")}, - {"ExplicitEuler", Removed("v19.12", "v20.06")}, - {"EulerSolver", Removed("v19.12", "v20.06")}, - {"ExplicitEulerSolver", Removed("v19.12", "v20.06")}, - - {"Capsule", Removed("v19.12", "v20.06")}, - {"CapsuleModel", Removed("v19.12", "v20.06")}, - {"TCapsuleModel", Removed("v19.12", "v20.06")}, - - {"Cube", Removed("v19.12", "v20.06")}, - {"CubeModel", Removed("v19.12", "v20.06")}, - - {"CudaPoint", Removed("v19.12", "v20.06")}, - {"CudaPointModel", Removed("v19.12", "v20.06")}, - - {"Cylinder", Removed("v19.12", "v20.06")}, - {"CylinderModel", Removed("v19.12", "v20.06")}, - - {"Line", Removed("v19.12", "v20.06")}, - {"TLineModel", Removed("v19.12", "v20.06")}, - {"LineMeshModel", Removed("v19.12", "v20.06")}, - {"LineSetModel", Removed("v19.12", "v20.06")}, - {"LineMesh", Removed("v19.12", "v20.06")}, - {"LineSet", Removed("v19.12", "v20.06")}, - {"LineModel", Removed("v19.12", "v20.06")}, - - {"OBB", Removed("v19.12", "v20.06")}, - {"OBBModel", Removed("v19.12", "v20.06")}, - {"TOBBModel", Removed("v19.12", "v20.06")}, - - {"Point", Removed("v19.12", "v20.06")}, - {"TPointModel", Removed("v19.12", "v20.06")}, - {"PointModel", Removed("v19.12", "v20.06")}, - {"PointMesh", Removed("v19.12", "v20.06")}, - {"PointSet", Removed("v19.12", "v20.06")}, - - {"Ray", Removed("v19.12", "v20.06")}, - {"RayModel", Removed("v19.12", "v20.06")}, - - {"RigidCapsule", Removed("v19.12", "v20.06")}, - {"RigidCapsuleModel", Removed("v19.12", "v20.06")}, - {"RigidCapsuleCollisionModel", Removed("v19.12", "v20.06")}, - - {"Sphere", Removed("v19.12", "v20.06")}, - {"SphereModel", Removed("v19.12", "v20.06")}, - {"TSphereModel", Removed("v19.12", "v20.06")}, - - {"Tetrahedron", Removed("v19.12", "v20.06")}, - {"TetrahedronModel", Removed("v19.12", "v20.06")}, - - {"Triangle", Removed("v19.12", "v20.06")}, - {"TriangleSet", Removed("v19.12", "v20.06")}, - {"TriangleMesh", Removed("v19.12", "v20.06")}, - {"TriangleSetModel", Removed("v19.12", "v20.06")}, - {"TriangleMeshModel", Removed("v19.12", "v20.06")}, - {"TriangleModel", Removed("v19.12", "v20.06")}, - {"TTriangleModel", Removed("v19.12", "v20.06")}, - /***********************/ // MOVED SINCE v21.06 { "SpatialGridPointModel", Moved("v21.06", "SofaMiscCollision", "SofaSphFluid") }, @@ -715,13 +614,148 @@ const std::map > uncreatableComponents { "CompareState", Moved("v22.06", "SofaValidation", "Sofa.Component.Playback") }, { "CompareTopology", Moved("v22.06", "SofaValidation", "Sofa.Component.Playback") }, + + // Moved to CSparseSolvers + { "SparseCholeskySolver", Moved("v23.12", "Sofa.Component.LinearSolver.Direct", "CSparseSolvers") }, + + { "SparseLUSolver", Moved("v23.12", "Sofa.Component.LinearSolver.Direct", "CSparseSolvers") } + +}; + +const std::map > uncreatableComponents = { + + /***********************/ + // REMOVED SINCE v23.06 + + { "OglGrid", Removed("v22.12", "v23.06")}, + { "OglLineAxis", Removed("v22.12", "v23.06")}, + + /***********************/ + // REMOVED SINCE v22.06 + + {"PointConstraint", Removed("v21.12", "v22.06")}, + + /***********************/ + // REMOVED SINCE v21.12 + + { "LMDNewProximityIntersection", Removed("v21.12", "v21.12") }, + { "LocalMinDistanceFilter", Removed("v21.12", "v21.12") }, + { "LineLocalMinDistanceFilter", Removed("v21.12", "v21.12") }, + { "PointLocalMinDistanceFilter", Removed("v21.12", "v21.12") }, + { "TriangleLocalMinDistanceFilter", Removed("v21.12", "v21.12") }, + + /***********************/ + // REMOVED SINCE v21.06 + + {"LengthContainer", Removed("v21.06", "v21.06")}, + {"PoissonContainer", Removed("v21.06", "v21.06")}, + {"RadiusContainer", Removed("v21.06", "v21.06")}, + {"StiffnessContainer", Removed("v21.06", "v21.06")}, + + /***********************/ + // REMOVED SINCE v20.12 + + { "DynamicSparseGridTopologyAlgorithms", Removed("v20.12", "v20.12") }, + { "HexahedronSetTopologyAlgorithms", Removed("v20.12", "v20.12") }, + { "TetrahedronSetTopologyAlgorithms", Removed("v20.12", "v20.12") }, + { "QuadSetTopologyAlgorithms", Removed("v20.12", "v20.12") }, + { "TriangleSetTopologyAlgorithms", Removed("v20.12", "v20.12") }, + { "EdgeSetTopologyAlgorithms", Removed("v20.12", "v20.12") }, + { "PointSetTopologyAlgorithms", Removed("v20.12", "v20.12") }, + + /***********************/ + // REMOVED SINCE v20.06 + + {"Euler", Removed("v19.12", "v20.06")}, + {"EulerExplicit", Removed("v19.12", "v20.06")}, + {"ExplicitEuler", Removed("v19.12", "v20.06")}, + {"EulerSolver", Removed("v19.12", "v20.06")}, + {"ExplicitEulerSolver", Removed("v19.12", "v20.06")}, + + {"Capsule", Removed("v19.12", "v20.06")}, + {"CapsuleModel", Removed("v19.12", "v20.06")}, + {"TCapsuleModel", Removed("v19.12", "v20.06")}, + + {"Cube", Removed("v19.12", "v20.06")}, + {"CubeModel", Removed("v19.12", "v20.06")}, + + {"CudaPoint", Removed("v19.12", "v20.06")}, + {"CudaPointModel", Removed("v19.12", "v20.06")}, + + {"Cylinder", Removed("v19.12", "v20.06")}, + {"CylinderModel", Removed("v19.12", "v20.06")}, + + {"Line", Removed("v19.12", "v20.06")}, + {"TLineModel", Removed("v19.12", "v20.06")}, + {"LineMeshModel", Removed("v19.12", "v20.06")}, + {"LineSetModel", Removed("v19.12", "v20.06")}, + {"LineMesh", Removed("v19.12", "v20.06")}, + {"LineSet", Removed("v19.12", "v20.06")}, + {"LineModel", Removed("v19.12", "v20.06")}, + + {"OBB", Removed("v19.12", "v20.06")}, + {"OBBModel", Removed("v19.12", "v20.06")}, + {"TOBBModel", Removed("v19.12", "v20.06")}, + + {"Point", Removed("v19.12", "v20.06")}, + {"TPointModel", Removed("v19.12", "v20.06")}, + {"PointModel", Removed("v19.12", "v20.06")}, + {"PointMesh", Removed("v19.12", "v20.06")}, + {"PointSet", Removed("v19.12", "v20.06")}, + + {"Ray", Removed("v19.12", "v20.06")}, + {"RayModel", Removed("v19.12", "v20.06")}, + + {"RigidCapsule", Removed("v19.12", "v20.06")}, + {"RigidCapsuleModel", Removed("v19.12", "v20.06")}, + {"RigidCapsuleCollisionModel", Removed("v19.12", "v20.06")}, + + {"Sphere", Removed("v19.12", "v20.06")}, + {"SphereModel", Removed("v19.12", "v20.06")}, + {"TSphereModel", Removed("v19.12", "v20.06")}, + + {"Tetrahedron", Removed("v19.12", "v20.06")}, + {"TetrahedronModel", Removed("v19.12", "v20.06")}, + + {"Triangle", Removed("v19.12", "v20.06")}, + {"TriangleSet", Removed("v19.12", "v20.06")}, + {"TriangleMesh", Removed("v19.12", "v20.06")}, + {"TriangleSetModel", Removed("v19.12", "v20.06")}, + {"TriangleMeshModel", Removed("v19.12", "v20.06")}, + {"TriangleModel", Removed("v19.12", "v20.06")}, + {"TTriangleModel", Removed("v19.12", "v20.06")}, + // Removed in #4040, deprecated in #2777 { "MechanicalMatrixMapper", Removed("v23.06", "v23.12") }, { "MappingGeometricStiffnessForceField", Removed("v23.06", "v23.12") }, - // Moved to CSparseSolvers - { "SparseCholeskySolver", Moved("v23.12", "Sofa.Component.LinearSolver.Direct", "CSparseSolvers") }, - { "SparseLUSolver", Moved("v23.12", "Sofa.Component.LinearSolver.Direct", "CSparseSolvers") }, + // Change Constraint naming #4302 + {"AffineMovementConstraint", Renamed("v24.06","v25.06","AffineMovementProjectiveConstraint")}, + {"AttachConstraint", Renamed("v24.06","v25.06","AttachProjectiveConstraint")}, + {"FixedConstraint", Renamed("v24.06","v25.06","FixedProjectiveConstraint")}, + {"FixedPlaneConstraint", Renamed("v24.06","v25.06","FixedPlaneProjectiveConstraint")}, + {"FixedRotationConstraint", Renamed("v24.06","v25.06","FixedRotationProjectiveConstraint")}, + {"FixedTranslationConstraint", Renamed("v24.06","v25.06","FixedTranslationProjectiveConstraint")}, + {"HermiteSplineConstraint", Renamed("v24.06","v25.06","HermiteSplineProjectiveConstraint")}, + {"LinearMovementConstraint", Renamed("v24.06","v25.06","LinearMovementProjectiveConstraint")}, + {"LinearVelocityConstraint", Renamed("v24.06","v25.06","LinearVelocityProjectiveConstraint")}, + {"OscillatorConstraint", Renamed("v24.06","v25.06","OscillatorProjectiveConstraint")}, + {"ParabolicConstraint", Renamed("v24.06","v25.06","ParabolicProjectiveConstraint")}, + {"PartialFixedConstraint", Renamed("v24.06","v25.06","PartialFixedProjectiveConstraint")}, + {"PartialLinearMovementConstraint", Renamed("v24.06","v25.06","PartialLinearMovementProjectiveConstraint")}, + {"PatchTestMovementConstraint", Renamed("v24.06","v25.06","PatchTestMovementProjectiveConstraint")}, + {"PositionBasedDynamicsConstraint", Renamed("v24.06","v25.06","PositionBasedDynamicsProjectiveConstraint")}, + {"SkeletalMotionConstraint", Renamed("v24.06","v25.06","SkeletalMotionProjectiveConstraint")}, + {"ProjectToLineConstraint", Renamed("v24.06","v25.06","LineProjectiveConstraint")}, + {"ProjectToPlaneConstraint", Renamed("v24.06","v25.06","PlaneProjectiveConstraint")}, + {"ProjectToPointConstraint", Renamed("v24.06","v25.06","PointProjectiveConstraint")}, + {"ProjectDirectionConstraint", Renamed("v24.06","v25.06","DirectionProjectiveConstraint")}, + {"BilateralInteractionConstraint", Renamed("v24.06","v25.06","BilateralInteractionLagrangianConstraint")}, + {"SlidingConstraint", Renamed("v24.06","v25.06","SlidingLagrangianConstraint")}, + {"StopperConstraint", Renamed("v24.06","v25.06","StopperLagrangianConstraint")}, + {"UniformConstraint", Renamed("v24.06","v25.06","UniformLagrangianConstraint")}, + {"UnilateralInteractionConstraint", Renamed("v24.06","v25.06","UnilateralInteractionLagrangianConstraint")} + }; } // namespace sofa::helper::lifecycle diff --git a/Sofa/framework/Helper/src/sofa/helper/ComponentChange.h b/Sofa/framework/Helper/src/sofa/helper/ComponentChange.h index 732065b8bc9..40c94c23b19 100644 --- a/Sofa/framework/Helper/src/sofa/helper/ComponentChange.h +++ b/Sofa/framework/Helper/src/sofa/helper/ComponentChange.h @@ -111,7 +111,22 @@ class SOFA_HELPER_API Moved : public ComponentChange } }; +class SOFA_HELPER_API Renamed : public ComponentChange +{ +public: + Renamed(const std::string& sinceVersion, const std::string& untilVersion, const std::string& newName) + { + std::stringstream output; + output << "This component has been RENAMED to " << newName << " since SOFA " << sinceVersion + << ", and this alias will be removed in SOFA " << untilVersion << "." + << " To continue using this component after SOFA "<< untilVersion <<" you will need to update your scene "; + m_message = output.str(); + m_changeVersion = untilVersion; + } +}; + extern SOFA_HELPER_API const std::map< std::string, Deprecated, std::less<> > deprecatedComponents; +extern SOFA_HELPER_API const std::map< std::string, ComponentChange, std::less<> > movedComponents; extern SOFA_HELPER_API const std::map< std::string, ComponentChange, std::less<> > uncreatableComponents; } // namespace sofa::helper::lifecycle diff --git a/applications/plugins/ExternalBehaviorModel/example/MultiMapping.py b/applications/plugins/ExternalBehaviorModel/example/MultiMapping.py index 40912650f48..c3503363497 100644 --- a/applications/plugins/ExternalBehaviorModel/example/MultiMapping.py +++ b/applications/plugins/ExternalBehaviorModel/example/MultiMapping.py @@ -14,7 +14,7 @@ def createGraph(self,node): rigidNode = self.rootNode.createChild('rigid') rigidNode.createObject('MechanicalObject', template='Rigid', name='dofs', position="0 0 0 0 0 0 1", showObject='1', showObjectScale='0.8') rigidNode.createObject('UniformMass', template='Rigid', totalMass="1") - rigidNode.createObject('PartialFixedConstraint',indices="0",fixedDirections="1 1 1 0 0 0") + rigidNode.createObject('PartialFixedProjectiveConstraint',indices="0",fixedDirections="1 1 1 0 0 0") rigidCollisionNode = rigidNode.createChild('rigid') rigidCollisionNode.createObject('MeshOBJLoader', filename="mesh/cube.obj", name="loader") diff --git a/applications/plugins/Sensable/examples/Carving.scn b/applications/plugins/Sensable/examples/Carving.scn index f14b61c400f..0faf21331dc 100644 --- a/applications/plugins/Sensable/examples/Carving.scn +++ b/applications/plugins/Sensable/examples/Carving.scn @@ -21,7 +21,7 @@ - + diff --git a/applications/plugins/Sensable/examples/Deformable-Method1.scn b/applications/plugins/Sensable/examples/Deformable-Method1.scn index 39152d3c235..493310549a7 100644 --- a/applications/plugins/Sensable/examples/Deformable-Method1.scn +++ b/applications/plugins/Sensable/examples/Deformable-Method1.scn @@ -17,7 +17,7 @@ - + diff --git a/applications/plugins/SofaAssimp/SceneColladaLoader.cpp b/applications/plugins/SofaAssimp/SceneColladaLoader.cpp index c13aae99da0..2021ecae599 100644 --- a/applications/plugins/SofaAssimp/SceneColladaLoader.cpp +++ b/applications/plugins/SofaAssimp/SceneColladaLoader.cpp @@ -34,7 +34,7 @@ #include #include #include -#include +#include #include #include #include @@ -331,18 +331,18 @@ bool SceneColladaLoader::readDAE (std::ifstream &/*file*/, const char* /*filenam } } - // generating a SkeletalMotionConstraint and filling up its properties - SkeletalMotionConstraint::SPtr currentSkeletalMotionConstraint = sofa::core::objectmodel::New >(); + // generating a SkeletalMotionProjectiveConstraint and filling up its properties + SkeletalMotionProjectiveConstraint::SPtr currentSkeletalMotionProjectiveConstraint = sofa::core::objectmodel::New >(); { - // adding the generated SkeletalMotionConstraint to its parent Node - currentSubNode->addObject(currentSkeletalMotionConstraint); + // adding the generated SkeletalMotionProjectiveConstraint to its parent Node + currentSubNode->addObject(currentSkeletalMotionProjectiveConstraint); std::stringstream nameStream(meshName); if(meshName.empty()) nameStream << componentIndex++; - currentSkeletalMotionConstraint->setName(nameStream.str()); + currentSkeletalMotionProjectiveConstraint->setName(nameStream.str()); - currentSkeletalMotionConstraint->setAnimationSpeed(animationSpeed.getValue()); + currentSkeletalMotionProjectiveConstraint->setAnimationSpeed(animationSpeed.getValue()); aiNode* parentAiNode = NULL; if(parentNodeInfo) @@ -351,7 +351,7 @@ bool SceneColladaLoader::readDAE (std::ifstream &/*file*/, const char* /*filenam type::vector > skeletonJoints; type::vector skeletonBones; fillSkeletalInfo(currentAiScene, parentAiNode, currentAiNode, currentTransformation, currentAiMesh, skeletonJoints, skeletonBones); - currentSkeletalMotionConstraint->setSkeletalMotion(skeletonJoints, skeletonBones); + currentSkeletalMotionProjectiveConstraint->setSkeletalMotion(skeletonJoints, skeletonBones); } } else diff --git a/applications/plugins/SofaAssimp/SceneColladaLoader.h b/applications/plugins/SofaAssimp/SceneColladaLoader.h index 117a5ae5f6f..869e8519399 100644 --- a/applications/plugins/SofaAssimp/SceneColladaLoader.h +++ b/applications/plugins/SofaAssimp/SceneColladaLoader.h @@ -26,7 +26,7 @@ #include #include #include -#include +#include #include // C++ importer interface #include // Output data structure @@ -130,7 +130,7 @@ class SOFA_ASSIMP_API SceneColladaLoader : public sofa::core::loader::SceneLoade private: - // build the joints and bones array used in the SkeletalMotionConstraint + // build the joints and bones array used in the SkeletalMotionProjectiveConstraint bool fillSkeletalInfo(const aiScene* scene, aiNode* meshParentNode, aiNode* meshNode, aiMatrix4x4 meshTransformation, aiMesh* mesh, type::vector >& skeletonJoints, type::vector& skeletonBones) const; // clean the scene graph of its empty and useless intermediary nodes diff --git a/applications/plugins/SofaAssimp/doc/index.html b/applications/plugins/SofaAssimp/doc/index.html index 3b7b3d369a2..7c553b03b3b 100644 --- a/applications/plugins/SofaAssimp/doc/index.html +++ b/applications/plugins/SofaAssimp/doc/index.html @@ -131,11 +131,11 @@

Examples



      As you can see in Sofa Modeler, the scene to load a collada file is very simple. We have the SceneColladaLoader to load it and, important thing, we also have an EulerSolver -which will be used by the potential SkeletalMotionConstraints to "play" the animation updating positions and velocities. +which will be used by the potential SkeletalMotionProjectiveConstraints to "play" the animation updating positions and velocities. When you launch runSofa with this scene, a new node will be added which contains the whole collada scene. The name of this node is the name you gave to the SceneColladaLoader plus the string "_scene". On the right picture, the loader generated two child nodes call "mesh 0" which contains a mesh without skinning (the sphere), and "mesh 1" which contains a mesh with skinning and bones animation directly from the collada file. -The SkeletalMotionConstraint automatically interpolates between animation frames, play with the animation time step to slown down or speed up the animation. +The SkeletalMotionProjectiveConstraint automatically interpolates between animation frames, play with the animation time step to slown down or speed up the animation.

As you can see on the other example below, the woman's hair does not fit her head very well because its coordinates system belongs to a dummy object not supported for the moment. diff --git a/applications/plugins/SofaCUDA/CMakeLists.txt b/applications/plugins/SofaCUDA/CMakeLists.txt index 18d32a9d0c5..25aded79541 100644 --- a/applications/plugins/SofaCUDA/CMakeLists.txt +++ b/applications/plugins/SofaCUDA/CMakeLists.txt @@ -88,10 +88,10 @@ set(HEADER_FILES ${SOFACUDA_SOURCE_DIR}/component/collision/geometry/CudaTriangleModel.h ### Constraints - ${SOFACUDA_SOURCE_DIR}/component/constraint/projective/CudaFixedConstraint.h - ${SOFACUDA_SOURCE_DIR}/component/constraint/projective/CudaFixedConstraint.inl - ${SOFACUDA_SOURCE_DIR}/component/constraint/projective/CudaLinearMovementConstraint.h - ${SOFACUDA_SOURCE_DIR}/component/constraint/projective/CudaLinearMovementConstraint.inl + ${SOFACUDA_SOURCE_DIR}/component/constraint/projective/CudaFixedProjectiveConstraint.h + ${SOFACUDA_SOURCE_DIR}/component/constraint/projective/CudaFixedProjectiveConstraint.inl + ${SOFACUDA_SOURCE_DIR}/component/constraint/projective/CudaLinearMovementProjectiveConstraint.h + ${SOFACUDA_SOURCE_DIR}/component/constraint/projective/CudaLinearMovementProjectiveConstraint.inl ${SOFACUDA_SOURCE_DIR}/component/collision/response/contact/CudaPenalityContactForceField.h ${SOFACUDA_SOURCE_DIR}/component/collision/response/contact/CudaPenalityContactForceField.inl @@ -155,11 +155,11 @@ set(SOURCE_FILES ${SOFACUDA_SOURCE_DIR}/component/collision/geometry/CudaTriangleModel.cpp ### Constraints - ${SOFACUDA_SOURCE_DIR}/component/constraint/lagrangian/model/CudaBilateralInteractionConstraint.cpp - ${SOFACUDA_SOURCE_DIR}/component/constraint/projective/CudaFixedConstraint.cpp - ${SOFACUDA_SOURCE_DIR}/component/constraint/projective/CudaFixedTranslationConstraint.cpp - ${SOFACUDA_SOURCE_DIR}/component/constraint/projective/CudaLinearMovementConstraint.cpp - ${SOFACUDA_SOURCE_DIR}/component/constraint/projective/CudaLinearVelocityConstraint.cpp + ${SOFACUDA_SOURCE_DIR}/component/constraint/lagrangian/model/CudaBilateralInteractionLagrangianConstraint.cpp + ${SOFACUDA_SOURCE_DIR}/component/constraint/projective/CudaFixedProjectiveConstraint.cpp + ${SOFACUDA_SOURCE_DIR}/component/constraint/projective/CudaFixedTranslationProjectiveConstraint.cpp + ${SOFACUDA_SOURCE_DIR}/component/constraint/projective/CudaLinearMovementProjectiveConstraint.cpp + ${SOFACUDA_SOURCE_DIR}/component/constraint/projective/CudaLinearVelocityProjectiveConstraint.cpp ${SOFACUDA_SOURCE_DIR}/component/mapping/linear/CudaBeamLinearMapping.cpp ${SOFACUDA_SOURCE_DIR}/component/engine/select/CudaBoxROI.cpp @@ -214,8 +214,8 @@ set(CUDA_SOURCES ### Collisions ### Constraints - ${SOFACUDA_SOURCE_DIR}/component/constraint/projective/CudaFixedConstraint.cu - ${SOFACUDA_SOURCE_DIR}/component/constraint/projective/CudaLinearMovementConstraint.cu + ${SOFACUDA_SOURCE_DIR}/component/constraint/projective/CudaFixedProjectiveConstraint.cu + ${SOFACUDA_SOURCE_DIR}/component/constraint/projective/CudaLinearMovementProjectiveConstraint.cu ) diff --git a/applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/lagrangian/model/CudaBilateralInteractionConstraint.cpp b/applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/lagrangian/model/CudaBilateralInteractionLagrangianConstraint.cpp similarity index 72% rename from applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/lagrangian/model/CudaBilateralInteractionConstraint.cpp rename to applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/lagrangian/model/CudaBilateralInteractionLagrangianConstraint.cpp index b0486fe7930..6904cb6b1f6 100644 --- a/applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/lagrangian/model/CudaBilateralInteractionConstraint.cpp +++ b/applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/lagrangian/model/CudaBilateralInteractionLagrangianConstraint.cpp @@ -21,18 +21,18 @@ ******************************************************************************/ #include #include -#include +#include #include namespace sofa::component::constraint::lagrangian::model { -template class SOFA_GPU_CUDA_API BilateralInteractionConstraint; -template class SOFA_GPU_CUDA_API BilateralInteractionConstraint; +template class SOFA_GPU_CUDA_API BilateralInteractionLagrangianConstraint; +template class SOFA_GPU_CUDA_API BilateralInteractionLagrangianConstraint; #ifdef SOFA_GPU_CUDA_DOUBLE -template class SOFA_GPU_CUDA_API BilateralInteractionConstraint; -template class SOFA_GPU_CUDA_API BilateralInteractionConstraint; +template class SOFA_GPU_CUDA_API BilateralInteractionLagrangianConstraint; +template class SOFA_GPU_CUDA_API BilateralInteractionLagrangianConstraint; #endif // SOFA_GPU_CUDA_DOUBLE } //namespace sofa::component::constraint::lagrangian::model @@ -42,12 +42,12 @@ namespace sofa::gpu::cuda using namespace sofa::component::constraint::lagrangian::model; -int BilateralInteractionConstraintCudaClass = core::RegisterObject("Supports GPU-side computations using CUDA") - .add< BilateralInteractionConstraint >() - .add< BilateralInteractionConstraint >() +int BilateralInteractionLagrangianConstraintCudaClass = core::RegisterObject("Supports GPU-side computations using CUDA") + .add< BilateralInteractionLagrangianConstraint >() + .add< BilateralInteractionLagrangianConstraint >() #ifdef SOFA_GPU_CUDA_DOUBLE - .add< BilateralInteractionConstraint >() - .add< BilateralInteractionConstraint >() + .add< BilateralInteractionLagrangianConstraint >() + .add< BilateralInteractionLagrangianConstraint >() #endif // SOFA_GPU_CUDA_DOUBLE ; } // namespace sofa::gpu::cuda diff --git a/applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/projective/CudaFixedConstraint.cu b/applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/projective/CudaFixedConstraint.cu deleted file mode 100644 index b193ef8ef77..00000000000 --- a/applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/projective/CudaFixedConstraint.cu +++ /dev/null @@ -1,223 +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 -#include "cuda.h" -#include - -#if defined(__cplusplus) && CUDA_VERSION < 2000 -namespace sofa -{ -namespace gpu -{ -namespace cuda -{ -#endif - -extern "C" -{ - void FixedConstraintCuda1f_projectResponseContiguous(unsigned int size, void* dx); - void FixedConstraintCuda1f_projectResponseIndexed(unsigned int size, const void* indices, void* dx); - void FixedConstraintCuda3f_projectResponseContiguous(unsigned int size, void* dx); - void FixedConstraintCuda3f_projectResponseIndexed(unsigned int size, const void* indices, void* dx); - void FixedConstraintCuda3f1_projectResponseContiguous(unsigned int size, void* dx); - void FixedConstraintCuda3f1_projectResponseIndexed(unsigned int size, const void* indices, void* dx); - void FixedConstraintCudaRigid3f_projectResponseContiguous(unsigned int size, void* dx); - void FixedConstraintCudaRigid3f_projectResponseIndexed(unsigned int size, const void* indices, void* dx); - -#ifdef SOFA_GPU_CUDA_DOUBLE - - void FixedConstraintCuda3d_projectResponseContiguous(unsigned int size, void* dx); - void FixedConstraintCuda3d_projectResponseIndexed(unsigned int size, const void* indices, void* dx); - void FixedConstraintCuda3d1_projectResponseContiguous(unsigned int size, void* dx); - void FixedConstraintCuda3d1_projectResponseIndexed(unsigned int size, const void* indices, void* dx); - void FixedConstraintCudaRigid3d_projectResponseContiguous(unsigned int size, void* dx); - void FixedConstraintCudaRigid3d_projectResponseIndexed(unsigned int size, const void* indices, void* dx); - -#endif // SOFA_GPU_CUDA_DOUBLE -} - -////////////////////// -// GPU-side methods // -////////////////////// - -template -__global__ void FixedConstraintCuda1t_projectResponseContiguous_kernel(int size, real* dx) -{ - int index = blockIdx.x * BSIZE+threadIdx.x; - if (index < size) - dx[index] = 0.0f; -} - -template -__global__ void FixedConstraintCuda3t_projectResponseContiguous_kernel(int size, CudaVec3* dx) -{ - int index = blockIdx.x * BSIZE+threadIdx.x; - if (index < size) - dx[index] = CudaVec3::make(0.0f,0.0f,0.0f); -} - -template -__global__ void FixedConstraintCuda3t1_projectResponseContiguous_kernel(int size, CudaVec4* dx) -{ - int index = blockIdx.x * BSIZE+threadIdx.x; - if (index < size) - dx[index] = CudaVec4::make(0.0f,0.0f,0.0f,0.0f); -} - -template -__global__ void FixedConstraintCudaRigid3t_projectResponseContiguous_kernel(int size, CudaRigidDeriv3* dx) -{ - int index = blockIdx.x * BSIZE+threadIdx.x; - if (index < size) - dx[index] = CudaRigidDeriv3::make(0.0f,0.0f,0.0f,0.0f,0.0f,0.0f); -} - -template -__global__ void FixedConstraintCuda1t_projectResponseIndexed_kernel(int size, const int* indices, real* dx) -{ - int index = blockIdx.x * BSIZE+threadIdx.x; - if (index < size) - dx[indices[index]] = 0.0f; -} - -template -__global__ void FixedConstraintCuda3t_projectResponseIndexed_kernel(int size, const int* indices, CudaVec3* dx) -{ - int index = blockIdx.x * BSIZE+threadIdx.x; - if (index < size) - dx[indices[index]] = CudaVec3::make(0.0f,0.0f,0.0f); -} - -template -__global__ void FixedConstraintCuda3t1_projectResponseIndexed_kernel(int size, const int* indices, CudaVec4* dx) -{ - int index = blockIdx.x * BSIZE+threadIdx.x; - if (index < size) - dx[indices[index]] = CudaVec4::make(0.0f,0.0f,0.0f,0.0f); -} - -template -__global__ void FixedConstraintCudaRigid3t_projectResponseIndexed_kernel(int size, const int* indices, CudaRigidDeriv3* dx) -{ - int index = blockIdx.x * BSIZE+threadIdx.x; - if (index < size) - dx[indices[index]] = CudaRigidDeriv3::make(0.0f,0.0f,0.0f,0.0f,0.0f,0.0f); -} - -////////////////////// -// CPU-side methods // -////////////////////// - -void FixedConstraintCuda1f_projectResponseContiguous(unsigned int size, void* dx) -{ - cudaMemset(dx, 0, size*sizeof(float)); -} - -void FixedConstraintCuda3f_projectResponseContiguous(unsigned int size, void* dx) -{ - cudaMemset(dx, 0, size*3*sizeof(float)); -} - -void FixedConstraintCuda3f1_projectResponseContiguous(unsigned int size, void* dx) -{ - cudaMemset(dx, 0, size*4*sizeof(float)); -} - -void FixedConstraintCudaRigid3f_projectResponseContiguous(unsigned int size, void* dx) -{ - cudaMemset(dx, 0, size*6*sizeof(float)); -} - -void FixedConstraintCuda1f_projectResponseIndexed(unsigned int size, const void* indices, void* dx) -{ - dim3 threads(BSIZE,1); - dim3 grid((size+BSIZE-1)/BSIZE,1); - {FixedConstraintCuda1t_projectResponseIndexed_kernel<<< grid, threads >>>(size, (const int*)indices, (float*)dx); mycudaDebugError("FixedConstraintCuda1t_projectResponseIndexed_kernel");} -} - -void FixedConstraintCuda3f_projectResponseIndexed(unsigned int size, const void* indices, void* dx) -{ - dim3 threads(BSIZE,1); - dim3 grid((size+BSIZE-1)/BSIZE,1); - {FixedConstraintCuda3t_projectResponseIndexed_kernel<<< grid, threads >>>(size, (const int*)indices, (CudaVec3*)dx); mycudaDebugError("FixedConstraintCuda3t_projectResponseIndexed_kernel");} -} - -void FixedConstraintCuda3f1_projectResponseIndexed(unsigned int size, const void* indices, void* dx) -{ - dim3 threads(BSIZE,1); - dim3 grid((size+BSIZE-1)/BSIZE,1); - {FixedConstraintCuda3t1_projectResponseIndexed_kernel<<< grid, threads >>>(size, (const int*)indices, (CudaVec4*)dx); mycudaDebugError("FixedConstraintCuda3t1_projectResponseIndexed_kernel");} -} - -void FixedConstraintCudaRigid3f_projectResponseIndexed(unsigned int size, const void* indices, void* dx) -{ - dim3 threads(BSIZE,1); - dim3 grid((size+BSIZE-1)/BSIZE,1); - {FixedConstraintCudaRigid3t_projectResponseIndexed_kernel<<< grid, threads >>>(size, (const int*)indices, (CudaRigidDeriv3*)dx); mycudaDebugError("FixedConstraintCudaRigid3t_projectResponseIndexed_kernel");} -} - -#ifdef SOFA_GPU_CUDA_DOUBLE - -void FixedConstraintCuda3d_projectResponseContiguous(unsigned int size, void* dx) -{ - cudaMemset(dx, 0, size*3*sizeof(double)); -} - -void FixedConstraintCuda3d1_projectResponseContiguous(unsigned int size, void* dx) -{ - cudaMemset(dx, 0, size*4*sizeof(double)); -} - -void FixedConstraintCudaRigid3d_projectResponseContiguous(unsigned int size, void* dx) -{ - cudaMemset(dx, 0, size*6*sizeof(double)); -} - -void FixedConstraintCuda3d_projectResponseIndexed(unsigned int size, const void* indices, void* dx) -{ - dim3 threads(BSIZE,1); - dim3 grid((size+BSIZE-1)/BSIZE,1); - {FixedConstraintCuda3t_projectResponseIndexed_kernel<<< grid, threads >>>(size, (const int*)indices, (CudaVec3*)dx); mycudaDebugError("FixedConstraintCuda3t_projectResponseIndexed_kernel");} -} - -void FixedConstraintCuda3d1_projectResponseIndexed(unsigned int size, const void* indices, void* dx) -{ - dim3 threads(BSIZE,1); - dim3 grid((size+BSIZE-1)/BSIZE,1); - {FixedConstraintCuda3t1_projectResponseIndexed_kernel<<< grid, threads >>>(size, (const int*)indices, (CudaVec4*)dx); mycudaDebugError("FixedConstraintCuda3t1_projectResponseIndexed_kernel");} -} - -void FixedConstraintCudaRigid3d_projectResponseIndexed(unsigned int size, const void* indices, void* dx) -{ - dim3 threads(BSIZE,1); - dim3 grid((size+BSIZE-1)/BSIZE,1); - {FixedConstraintCudaRigid3t_projectResponseIndexed_kernel<<< grid, threads >>>(size, (const int*)indices, (CudaRigidDeriv3*)dx); mycudaDebugError("FixedConstraintCudaRigid3t_projectResponseIndexed_kernel");} -} - -#endif // SOFA_GPU_CUDA_DOUBLE - -#if defined(__cplusplus) && CUDA_VERSION < 2000 -} // namespace cuda -} // namespace gpu -} // namespace sofa -#endif diff --git a/applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/projective/CudaFixedConstraint.h b/applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/projective/CudaFixedConstraint.h index 15e27ce1a2a..f92c6941f62 100644 --- a/applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/projective/CudaFixedConstraint.h +++ b/applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/projective/CudaFixedConstraint.h @@ -21,91 +21,6 @@ ******************************************************************************/ #pragma once -#include -#include +#include -namespace sofa::component::constraint::projective -{ - -template -class FixedConstraintInternalData< gpu::cuda::CudaVectorTypes > -{ -public: - using Index = sofa::Index; - typedef FixedConstraintInternalData< gpu::cuda::CudaVectorTypes > Data; - typedef gpu::cuda::CudaVectorTypes DataTypes; - typedef FixedConstraint Main; - typedef typename DataTypes::VecDeriv VecDeriv; - typedef typename DataTypes::Deriv Deriv; - typedef typename DataTypes::Real Real; - typedef typename Main::SetIndex SetIndex; - typedef typename Main::SetIndexArray SetIndexArray; - - // min/max fixed indices for contiguous constraints - Index minIndex; - Index maxIndex; - // vector of indices for general case - gpu::cuda::CudaVector cudaIndices; - - - static void init(Main* m); - - static void addConstraint(Main* m, Index index); - - static void removeConstraint(Main* m, Index index); - - static void projectResponse(Main* m, VecDeriv& dx); -}; - -template -class FixedConstraintInternalData< gpu::cuda::CudaRigidTypes > -{ -public: - using Index = sofa::Index; - typedef FixedConstraintInternalData< gpu::cuda::CudaRigidTypes > Data; - typedef gpu::cuda::CudaRigidTypes DataTypes; - typedef FixedConstraint Main; - typedef typename DataTypes::VecDeriv VecDeriv; - typedef typename DataTypes::Deriv Deriv; - typedef typename DataTypes::Real Real; - typedef typename Main::SetIndex SetIndex; - typedef typename Main::SetIndexArray SetIndexArray; - - // min/max fixed indices for contiguous constraints - Index minIndex; - Index maxIndex; - // vector of indices for general case - gpu::cuda::CudaVector cudaIndices; - - - static void init(Main* m); - - static void addConstraint(Main* m, Index index); - - static void removeConstraint(Main* m, Index index); - - static void projectResponse(Main* m, VecDeriv& dx); -}; - -// I know using macros is bad design but this is the only way not to repeat the code for all CUDA types -#define CudaFixedConstraint_DeclMethods(T) \ - template<> void FixedConstraint< T >::init(); \ - template<> void FixedConstraint< T >::addConstraint(Index index); \ - template<> void FixedConstraint< T >::removeConstraint(Index index); \ - template<> void FixedConstraint< T >::projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& resData); - -CudaFixedConstraint_DeclMethods(gpu::cuda::CudaVec3fTypes); -CudaFixedConstraint_DeclMethods(gpu::cuda::CudaVec3f1Types); -CudaFixedConstraint_DeclMethods(gpu::cuda::CudaRigid3fTypes); - -#ifdef SOFA_GPU_CUDA_DOUBLE - -CudaFixedConstraint_DeclMethods(gpu::cuda::CudaVec3dTypes); -CudaFixedConstraint_DeclMethods(gpu::cuda::CudaVec3d1Types); -CudaFixedConstraint_DeclMethods(gpu::cuda::CudaRigid3dTypes); - -#endif // SOFA_GPU_CUDA_DOUBLE - -#undef CudaFixedConstraint_DeclMethods - -} // namespace sofa::component::constraint::projective +SOFA_DEPRECATED_HEADER("v23.12", "v24.12", "SofaCUDA/component/constraint/projective/CudaFixedProjectiveConstraint.h") diff --git a/applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/projective/CudaFixedConstraint.inl b/applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/projective/CudaFixedConstraint.inl index 1054b5a3784..95ad8d25985 100644 --- a/applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/projective/CudaFixedConstraint.inl +++ b/applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/projective/CudaFixedConstraint.inl @@ -21,433 +21,6 @@ ******************************************************************************/ #pragma once -#include -#include +#include -namespace sofa::gpu::cuda -{ - -extern "C" -{ - void FixedConstraintCuda1f_projectResponseContiguous(unsigned int size, void* dx); - void FixedConstraintCuda1f_projectResponseIndexed(unsigned int size, const void* indices, void* dx); - void FixedConstraintCuda3f_projectResponseContiguous(unsigned int size, void* dx); - void FixedConstraintCuda3f_projectResponseIndexed(unsigned int size, const void* indices, void* dx); - void FixedConstraintCuda3f1_projectResponseContiguous(unsigned int size, void* dx); - void FixedConstraintCuda3f1_projectResponseIndexed(unsigned int size, const void* indices, void* dx); - void FixedConstraintCudaRigid3f_projectResponseContiguous(unsigned int size, void* dx); - void FixedConstraintCudaRigid3f_projectResponseIndexed(unsigned int size, const void* indices, void* dx); - -#ifdef SOFA_GPU_CUDA_DOUBLE - - void FixedConstraintCuda3d_projectResponseContiguous(unsigned int size, void* dx); - void FixedConstraintCuda3d_projectResponseIndexed(unsigned int size, const void* indices, void* dx); - void FixedConstraintCuda3d1_projectResponseContiguous(unsigned int size, void* dx); - void FixedConstraintCuda3d1_projectResponseIndexed(unsigned int size, const void* indices, void* dx); - void FixedConstraintCudaRigid3d_projectResponseContiguous(unsigned int size, void* dx); - void FixedConstraintCudaRigid3d_projectResponseIndexed(unsigned int size, const void* indices, void* dx); - -#endif // SOFA_GPU_CUDA_DOUBLE -} - -} // namespace sofa::gpu::cuda - -namespace sofa::component::constraint::projective -{ - -using namespace gpu::cuda; - -template -void FixedConstraintInternalData< gpu::cuda::CudaVectorTypes >::init(Main* m) -{ - Data& data = *m->data; - data.minIndex = sofa::InvalidID; - data.maxIndex = sofa::InvalidID; - data.cudaIndices.clear(); - m->core::behavior::template ProjectiveConstraintSet::init(); - const SetIndexArray& indices = m->d_indices.getValue(); - if (!indices.empty()) - { - // put indices in a set to sort them and remove duplicates - std::set sortedIndices; - for (typename SetIndex::const_iterator it = indices.begin(); it!=indices.end(); ++it) - sortedIndices.insert(*it); - // check if the indices are contiguous - if (*sortedIndices.begin() + (int)sortedIndices.size()-1 == *sortedIndices.rbegin()) - { - data.minIndex = *sortedIndices.begin(); - data.maxIndex = *sortedIndices.rbegin(); - msg_info("CudaFixedConstraint") << "init: " << sortedIndices.size() << " contiguous fixed indices, " << data.minIndex << " - " << data.maxIndex; - } - else - { - msg_info("CudaFixedConstraint") << "init: " << sortedIndices.size() << " non-contiguous fixed indices"; - data.cudaIndices.reserve(sortedIndices.size()); - for (std::set::const_iterator it = sortedIndices.begin(); it!=sortedIndices.end(); ++it) - data.cudaIndices.push_back(*it); - } - } - - m->d_componentState.setValue(ComponentState::Valid); -} - -template -void FixedConstraintInternalData< gpu::cuda::CudaVectorTypes >::addConstraint(Main* m, Index index) -{ - Data& data = *m->data; - //std::cout << "CudaFixedConstraint::addConstraint("<d_indices.beginEdit()->push_back(index); - m->d_indices.endEdit(); - if (data.cudaIndices.empty()) - { - if (data.minIndex == sofa::InvalidID) - { - //std::cout << "CudaFixedConstraint: single index "<= data.minIndex && index <= data.maxIndex) - { - // point already fixed - } - else if (data.minIndex == index + 1) - { - data.minIndex = index; - //std::cout << "CudaFixedConstraint: new min index "< -void FixedConstraintInternalData< gpu::cuda::CudaVectorTypes >::removeConstraint(Main* m, Index index) -{ - Data& data = *m->data; - removeValue(*m->d_indices.beginEdit(),index); - m->d_indices.endEdit(); - if (data.cudaIndices.empty()) - { - if (data.minIndex <= index && index <= data.maxIndex) - { - if (data.minIndex == index) - { - if (data.maxIndex == index) - { - // empty set - data.minIndex = sofa::InvalidID; - data.maxIndex = sofa::InvalidID; - } - else - ++data.minIndex; - } - else if (data.maxIndex == index) - --data.maxIndex; - else - { - data.cudaIndices.reserve(data.maxIndex-data.minIndex); - for (int i=data.minIndex; i -void FixedConstraintInternalData< gpu::cuda::CudaRigidTypes >::init(Main* m) -{ - Data& data = *m->data; - data.minIndex = sofa::InvalidID; - data.maxIndex = sofa::InvalidID; - data.cudaIndices.clear(); - m->core::behavior::template ProjectiveConstraintSet::init(); - const SetIndexArray& indices = m->d_indices.getValue(); - if (!indices.empty()) - { - // put indices in a set to sort them and remove duplicates - std::set sortedIndices; - for (typename SetIndex::const_iterator it = indices.begin(); it!=indices.end(); ++it) - sortedIndices.insert(*it); - // check if the indices are contiguous - if (*sortedIndices.begin() + (int)sortedIndices.size()-1 == *sortedIndices.rbegin()) - { - data.minIndex = *sortedIndices.begin(); - data.maxIndex = *sortedIndices.rbegin(); - msg_info("CudaFixedConstraint") << "init: " << sortedIndices.size() << " contiguous fixed indices, " << data.minIndex << " - " << data.maxIndex; - } - else - { - msg_info("CudaFixedConstraint") << "init: " << sortedIndices.size() << " non-contiguous fixed indices"; - data.cudaIndices.reserve(sortedIndices.size()); - for (std::set::const_iterator it = sortedIndices.begin(); it!=sortedIndices.end(); ++it) - data.cudaIndices.push_back(*it); - } - } - - m->d_componentState.setValue(ComponentState::Valid); -} - -template -void FixedConstraintInternalData< gpu::cuda::CudaRigidTypes >::addConstraint(Main* m, Index index) -{ - Data& data = *m->data; - //std::cout << "CudaFixedConstraint::addConstraint("<d_indices.beginEdit()->push_back(index); - m->d_indices.endEdit(); - if (data.cudaIndices.empty()) - { - if (data.minIndex == sofa::InvalidID) - { - //std::cout << "CudaFixedConstraint: single index "<= data.minIndex && index <= data.maxIndex) - { - // point already fixed - } - else if (data.minIndex == index+1) - { - data.minIndex = index; - //std::cout << "CudaFixedConstraint: new min index "< -void FixedConstraintInternalData< gpu::cuda::CudaRigidTypes >::removeConstraint(Main* m, Index index) -{ - Data& data = *m->data; - removeValue(*m->d_indices.beginEdit(),index); - m->d_indices.endEdit(); - if (data.cudaIndices.empty()) - { - if (data.minIndex <= index && index <= data.maxIndex) - { - if (data.minIndex == index) - { - if (data.maxIndex == index) - { - // empty set - data.minIndex = sofa::InvalidID; - data.maxIndex = sofa::InvalidID; - } - else - ++data.minIndex; - } - else if (data.maxIndex == index) - --data.maxIndex; - else - { - data.cudaIndices.reserve(data.maxIndex-data.minIndex); - for (int i=data.minIndex; i -void FixedConstraintInternalData::projectResponse(Main* m, VecDeriv& dx) -{ - const Data& data = *m->data; - if (m->d_fixAll.getValue()) - FixedConstraintCuda1f_projectResponseContiguous(dx.size(), ((float*)dx.deviceWrite())); - else if (data.minIndex >= 0) - FixedConstraintCuda1f_projectResponseContiguous(data.maxIndex-data.minIndex+1, ((float*)dx.deviceWrite())+data.minIndex); - else - FixedConstraintCuda1f_projectResponseIndexed(data.cudaIndices.size(), data.cudaIndices.deviceRead(), dx.deviceWrite()); -} - - -template <> -void FixedConstraintInternalData::projectResponse(Main* m, VecDeriv& dx) -{ - const Data& data = *m->data; - if (m->d_fixAll.getValue()) - FixedConstraintCuda3f_projectResponseContiguous(dx.size(), ((float*)dx.deviceWrite())); - else if (data.minIndex != sofa::InvalidID) - FixedConstraintCuda3f_projectResponseContiguous(data.maxIndex - data.minIndex + 1, ((float*)dx.deviceWrite()) + 3 * data.minIndex); - else - FixedConstraintCuda3f_projectResponseIndexed(data.cudaIndices.size(), data.cudaIndices.deviceRead(), dx.deviceWrite()); -} - - -template <> -void FixedConstraintInternalData::projectResponse(Main* m, VecDeriv& dx) -{ - const Data& data = *m->data; - if (m->d_fixAll.getValue()) - FixedConstraintCuda3f1_projectResponseContiguous(dx.size(), ((float*)dx.deviceWrite())); - else if (data.minIndex != sofa::InvalidID) - FixedConstraintCuda3f1_projectResponseContiguous(data.maxIndex-data.minIndex+1, ((float*)dx.deviceWrite())+4*data.minIndex); - else - FixedConstraintCuda3f1_projectResponseIndexed(data.cudaIndices.size(), data.cudaIndices.deviceRead(), dx.deviceWrite()); -} - -template <> -void FixedConstraintInternalData::projectResponse(Main* m, VecDeriv& dx) -{ - const Data& data = *m->data; - if (m->d_fixAll.getValue()) - FixedConstraintCudaRigid3f_projectResponseContiguous(dx.size(), ((float*)dx.deviceWrite())); - else if (data.minIndex != sofa::InvalidID) - FixedConstraintCudaRigid3f_projectResponseContiguous(data.maxIndex-data.minIndex+1, ((float*)dx.deviceWrite())+6*data.minIndex); - else - FixedConstraintCudaRigid3f_projectResponseIndexed(data.cudaIndices.size(), data.cudaIndices.deviceRead(), dx.deviceWrite()); -} - - - -#ifdef SOFA_GPU_CUDA_DOUBLE - -// // Handle topological changes -// template <> -// void FixedConstraint::handleTopologyChange() { -// // std::list::const_iterator itBegin=topology->firstChange(); -// // std::list::const_iterator itEnd=topology->lastChange(); -// // -// // d_indices.beginEdit()->handleTopologyEvents(itBegin,itEnd,this->getMState()->getSize()); -// //printf("WARNING handleTopologyChange not implemented\n"); -// } - -template <> -void FixedConstraintInternalData::projectResponse(Main* m, VecDeriv& dx) -{ - Data& data = *m->data; - if (m->d_fixAll.getValue()) - FixedConstraintCuda3d_projectResponseContiguous(dx.size(), ((double*)dx.deviceWrite())); - else if (data.minIndex >= 0) - FixedConstraintCuda3d_projectResponseContiguous(data.maxIndex-data.minIndex+1, ((double*)dx.deviceWrite())+3*data.minIndex); - else - FixedConstraintCuda3d_projectResponseIndexed(data.cudaIndices.size(), data.cudaIndices.deviceRead(), dx.deviceWrite()); -} - -template <> -void FixedConstraintInternalData::projectResponse(Main* m, VecDeriv& dx) -{ - Data& data = *m->data; - if (m->d_fixAll.getValue()) - FixedConstraintCuda3d1_projectResponseContiguous(dx.size(), ((double*)dx.deviceWrite())); - else if (data.minIndex >= 0) - FixedConstraintCuda3d1_projectResponseContiguous(data.maxIndex-data.minIndex+1, ((double*)dx.deviceWrite())+4*data.minIndex); - else - FixedConstraintCuda3d1_projectResponseIndexed(data.cudaIndices.size(), data.cudaIndices.deviceRead(), dx.deviceWrite()); -} - -template <> -void FixedConstraintInternalData::projectResponse(Main* m, VecDeriv& dx) -{ - Data& data = *m->data; - if (m->d_fixAll.getValue()) - FixedConstraintCudaRigid3d_projectResponseContiguous(dx.size(), ((double*)dx.deviceWrite())); - else if (data.minIndex >= 0) - FixedConstraintCudaRigid3d_projectResponseContiguous(data.maxIndex-data.minIndex+1, ((double*)dx.deviceWrite())+6*data.minIndex); - else - FixedConstraintCudaRigid3d_projectResponseIndexed(data.cudaIndices.size(), data.cudaIndices.deviceRead(), dx.deviceWrite()); -} - -#endif // SOFA_GPU_CUDA_DOUBLE - -// I know using macros is bad design but this is the only way not to repeat the code for all CUDA types -#define CudaFixedConstraint_ImplMethods(T) \ - template<> void FixedConstraint< T >::init() \ - { data->init(this); } \ - template<> void FixedConstraint< T >::addConstraint(Index index) \ - { data->addConstraint(this, index); } \ - template<> void FixedConstraint< T >::removeConstraint(Index index) \ - { data->removeConstraint(this, index); } \ - template<> void FixedConstraint< T >::projectResponse(const core::MechanicalParams* /* mparams */, DataVecDeriv& d_resData) \ - { \ - VecDeriv &resData = *d_resData.beginEdit(); \ - data->projectResponse(this, resData); \ - d_resData.endEdit(); \ - } - -CudaFixedConstraint_ImplMethods(gpu::cuda::CudaVec3fTypes); -CudaFixedConstraint_ImplMethods(gpu::cuda::CudaVec3f1Types); -CudaFixedConstraint_ImplMethods(gpu::cuda::CudaRigid3fTypes); - -#ifdef SOFA_GPU_CUDA_DOUBLE - -CudaFixedConstraint_ImplMethods(gpu::cuda::CudaVec3dTypes); -CudaFixedConstraint_ImplMethods(gpu::cuda::CudaVec3d1Types); -CudaFixedConstraint_ImplMethods(gpu::cuda::CudaRigid3dTypes); - -#endif // SOFA_GPU_CUDA_DOUBLE - -#undef CudaFixedConstraint_ImplMethods - -} // namespace sofa::component::constraint::projective +SOFA_DEPRECATED_HEADER("v23.12", "v24.12", "SofaCUDA/component/constraint/projective/CudaFixedProjectiveConstraint.inl") diff --git a/applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/projective/CudaFixedConstraint.cpp b/applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/projective/CudaFixedProjectiveConstraint.cpp similarity index 54% rename from applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/projective/CudaFixedConstraint.cpp rename to applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/projective/CudaFixedProjectiveConstraint.cpp index eb3455d6c04..0384749ab2c 100644 --- a/applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/projective/CudaFixedConstraint.cpp +++ b/applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/projective/CudaFixedProjectiveConstraint.cpp @@ -20,7 +20,7 @@ * Contact information: contact@sofa-framework.org * ******************************************************************************/ #include -#include +#include #include #include #include @@ -29,17 +29,17 @@ namespace sofa::component::constraint::projective { -template class SOFA_GPU_CUDA_API FixedConstraint; +template class SOFA_GPU_CUDA_API FixedProjectiveConstraint; -template class SOFA_GPU_CUDA_API FixedConstraint; -template class SOFA_GPU_CUDA_API FixedConstraint; -template class SOFA_GPU_CUDA_API FixedConstraint; -template class SOFA_GPU_CUDA_API FixedConstraint; +template class SOFA_GPU_CUDA_API FixedProjectiveConstraint; +template class SOFA_GPU_CUDA_API FixedProjectiveConstraint; +template class SOFA_GPU_CUDA_API FixedProjectiveConstraint; +template class SOFA_GPU_CUDA_API FixedProjectiveConstraint; #ifdef SOFA_GPU_CUDA_DOUBLE -template class SOFA_GPU_CUDA_API FixedConstraint; -template class SOFA_GPU_CUDA_API FixedConstraint; -template class SOFA_GPU_CUDA_API FixedConstraint; -template class SOFA_GPU_CUDA_API FixedConstraint; +template class SOFA_GPU_CUDA_API FixedProjectiveConstraint; +template class SOFA_GPU_CUDA_API FixedProjectiveConstraint; +template class SOFA_GPU_CUDA_API FixedProjectiveConstraint; +template class SOFA_GPU_CUDA_API FixedProjectiveConstraint; #endif // SOFA_GPU_CUDA_DOUBLE } // namespace sofa::component::constraint::projective @@ -47,17 +47,17 @@ template class SOFA_GPU_CUDA_API FixedConstraint; namespace sofa::gpu::cuda { -int FixedConstraintCudaClass = core::RegisterObject("Supports GPU-side computations using CUDA") - .add< component::constraint::projective::FixedConstraint >() - .add< component::constraint::projective::FixedConstraint >() - .add< component::constraint::projective::FixedConstraint >() - .add< component::constraint::projective::FixedConstraint >() - .add< component::constraint::projective::FixedConstraint >() +int FixedProjectiveConstraintCudaClass = core::RegisterObject("Supports GPU-side computations using CUDA") + .add< component::constraint::projective::FixedProjectiveConstraint >() + .add< component::constraint::projective::FixedProjectiveConstraint >() + .add< component::constraint::projective::FixedProjectiveConstraint >() + .add< component::constraint::projective::FixedProjectiveConstraint >() + .add< component::constraint::projective::FixedProjectiveConstraint >() #ifdef SOFA_GPU_CUDA_DOUBLE - .add< component::constraint::projective::FixedConstraint >() - .add< component::constraint::projective::FixedConstraint >() - .add< component::constraint::projective::FixedConstraint >() - .add< component::constraint::projective::FixedConstraint >() + .add< component::constraint::projective::FixedProjectiveConstraint >() + .add< component::constraint::projective::FixedProjectiveConstraint >() + .add< component::constraint::projective::FixedProjectiveConstraint >() + .add< component::constraint::projective::FixedProjectiveConstraint >() #endif // SOFA_GPU_CUDA_DOUBLE ; diff --git a/applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/projective/CudaFixedProjectiveConstraint.cu b/applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/projective/CudaFixedProjectiveConstraint.cu new file mode 100644 index 00000000000..24a7dd330c2 --- /dev/null +++ b/applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/projective/CudaFixedProjectiveConstraint.cu @@ -0,0 +1,223 @@ +/****************************************************************************** +* 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 +#include "cuda.h" +#include + +#if defined(__cplusplus) && CUDA_VERSION < 2000 +namespace sofa +{ +namespace gpu +{ +namespace cuda +{ +#endif + +extern "C" +{ + void FixedProjectiveConstraintCuda1f_projectResponseContiguous(unsigned int size, void* dx); + void FixedProjectiveConstraintCuda1f_projectResponseIndexed(unsigned int size, const void* indices, void* dx); + void FixedProjectiveConstraintCuda3f_projectResponseContiguous(unsigned int size, void* dx); + void FixedProjectiveConstraintCuda3f_projectResponseIndexed(unsigned int size, const void* indices, void* dx); + void FixedProjectiveConstraintCuda3f1_projectResponseContiguous(unsigned int size, void* dx); + void FixedProjectiveConstraintCuda3f1_projectResponseIndexed(unsigned int size, const void* indices, void* dx); + void FixedProjectiveConstraintCudaRigid3f_projectResponseContiguous(unsigned int size, void* dx); + void FixedProjectiveConstraintCudaRigid3f_projectResponseIndexed(unsigned int size, const void* indices, void* dx); + +#ifdef SOFA_GPU_CUDA_DOUBLE + + void FixedProjectiveConstraintCuda3d_projectResponseContiguous(unsigned int size, void* dx); + void FixedProjectiveConstraintCuda3d_projectResponseIndexed(unsigned int size, const void* indices, void* dx); + void FixedProjectiveConstraintCuda3d1_projectResponseContiguous(unsigned int size, void* dx); + void FixedProjectiveConstraintCuda3d1_projectResponseIndexed(unsigned int size, const void* indices, void* dx); + void FixedProjectiveConstraintCudaRigid3d_projectResponseContiguous(unsigned int size, void* dx); + void FixedProjectiveConstraintCudaRigid3d_projectResponseIndexed(unsigned int size, const void* indices, void* dx); + +#endif // SOFA_GPU_CUDA_DOUBLE +} + +////////////////////// +// GPU-side methods // +////////////////////// + +template +__global__ void FixedProjectiveConstraintCuda1t_projectResponseContiguous_kernel(int size, real* dx) +{ + int index = blockIdx.x * BSIZE+threadIdx.x; + if (index < size) + dx[index] = 0.0f; +} + +template +__global__ void FixedProjectiveConstraintCuda3t_projectResponseContiguous_kernel(int size, CudaVec3* dx) +{ + int index = blockIdx.x * BSIZE+threadIdx.x; + if (index < size) + dx[index] = CudaVec3::make(0.0f,0.0f,0.0f); +} + +template +__global__ void FixedProjectiveConstraintCuda3t1_projectResponseContiguous_kernel(int size, CudaVec4* dx) +{ + int index = blockIdx.x * BSIZE+threadIdx.x; + if (index < size) + dx[index] = CudaVec4::make(0.0f,0.0f,0.0f,0.0f); +} + +template +__global__ void FixedProjectiveConstraintCudaRigid3t_projectResponseContiguous_kernel(int size, CudaRigidDeriv3* dx) +{ + int index = blockIdx.x * BSIZE+threadIdx.x; + if (index < size) + dx[index] = CudaRigidDeriv3::make(0.0f,0.0f,0.0f,0.0f,0.0f,0.0f); +} + +template +__global__ void FixedProjectiveConstraintCuda1t_projectResponseIndexed_kernel(int size, const int* indices, real* dx) +{ + int index = blockIdx.x * BSIZE+threadIdx.x; + if (index < size) + dx[indices[index]] = 0.0f; +} + +template +__global__ void FixedProjectiveConstraintCuda3t_projectResponseIndexed_kernel(int size, const int* indices, CudaVec3* dx) +{ + int index = blockIdx.x * BSIZE+threadIdx.x; + if (index < size) + dx[indices[index]] = CudaVec3::make(0.0f,0.0f,0.0f); +} + +template +__global__ void FixedProjectiveConstraintCuda3t1_projectResponseIndexed_kernel(int size, const int* indices, CudaVec4* dx) +{ + int index = blockIdx.x * BSIZE+threadIdx.x; + if (index < size) + dx[indices[index]] = CudaVec4::make(0.0f,0.0f,0.0f,0.0f); +} + +template +__global__ void FixedProjectiveConstraintCudaRigid3t_projectResponseIndexed_kernel(int size, const int* indices, CudaRigidDeriv3* dx) +{ + int index = blockIdx.x * BSIZE+threadIdx.x; + if (index < size) + dx[indices[index]] = CudaRigidDeriv3::make(0.0f,0.0f,0.0f,0.0f,0.0f,0.0f); +} + +////////////////////// +// CPU-side methods // +////////////////////// + +void FixedProjectiveConstraintCuda1f_projectResponseContiguous(unsigned int size, void* dx) +{ + cudaMemset(dx, 0, size*sizeof(float)); +} + +void FixedProjectiveConstraintCuda3f_projectResponseContiguous(unsigned int size, void* dx) +{ + cudaMemset(dx, 0, size*3*sizeof(float)); +} + +void FixedProjectiveConstraintCuda3f1_projectResponseContiguous(unsigned int size, void* dx) +{ + cudaMemset(dx, 0, size*4*sizeof(float)); +} + +void FixedProjectiveConstraintCudaRigid3f_projectResponseContiguous(unsigned int size, void* dx) +{ + cudaMemset(dx, 0, size*6*sizeof(float)); +} + +void FixedProjectiveConstraintCuda1f_projectResponseIndexed(unsigned int size, const void* indices, void* dx) +{ + dim3 threads(BSIZE,1); + dim3 grid((size+BSIZE-1)/BSIZE,1); + {FixedProjectiveConstraintCuda1t_projectResponseIndexed_kernel<<< grid, threads >>>(size, (const int*)indices, (float*)dx); mycudaDebugError("FixedProjectiveConstraintCuda1t_projectResponseIndexed_kernel");} +} + +void FixedProjectiveConstraintCuda3f_projectResponseIndexed(unsigned int size, const void* indices, void* dx) +{ + dim3 threads(BSIZE,1); + dim3 grid((size+BSIZE-1)/BSIZE,1); + {FixedProjectiveConstraintCuda3t_projectResponseIndexed_kernel<<< grid, threads >>>(size, (const int*)indices, (CudaVec3*)dx); mycudaDebugError("FixedProjectiveConstraintCuda3t_projectResponseIndexed_kernel");} +} + +void FixedProjectiveConstraintCuda3f1_projectResponseIndexed(unsigned int size, const void* indices, void* dx) +{ + dim3 threads(BSIZE,1); + dim3 grid((size+BSIZE-1)/BSIZE,1); + {FixedProjectiveConstraintCuda3t1_projectResponseIndexed_kernel<<< grid, threads >>>(size, (const int*)indices, (CudaVec4*)dx); mycudaDebugError("FixedProjectiveConstraintCuda3t1_projectResponseIndexed_kernel");} +} + +void FixedProjectiveConstraintCudaRigid3f_projectResponseIndexed(unsigned int size, const void* indices, void* dx) +{ + dim3 threads(BSIZE,1); + dim3 grid((size+BSIZE-1)/BSIZE,1); + {FixedProjectiveConstraintCudaRigid3t_projectResponseIndexed_kernel<<< grid, threads >>>(size, (const int*)indices, (CudaRigidDeriv3*)dx); mycudaDebugError("FixedProjectiveConstraintCudaRigid3t_projectResponseIndexed_kernel");} +} + +#ifdef SOFA_GPU_CUDA_DOUBLE + +void FixedProjectiveConstraintCuda3d_projectResponseContiguous(unsigned int size, void* dx) +{ + cudaMemset(dx, 0, size*3*sizeof(double)); +} + +void FixedProjectiveConstraintCuda3d1_projectResponseContiguous(unsigned int size, void* dx) +{ + cudaMemset(dx, 0, size*4*sizeof(double)); +} + +void FixedProjectiveConstraintCudaRigid3d_projectResponseContiguous(unsigned int size, void* dx) +{ + cudaMemset(dx, 0, size*6*sizeof(double)); +} + +void FixedProjectiveConstraintCuda3d_projectResponseIndexed(unsigned int size, const void* indices, void* dx) +{ + dim3 threads(BSIZE,1); + dim3 grid((size+BSIZE-1)/BSIZE,1); + {FixedProjectiveConstraintCuda3t_projectResponseIndexed_kernel<<< grid, threads >>>(size, (const int*)indices, (CudaVec3*)dx); mycudaDebugError("FixedProjectiveConstraintCuda3t_projectResponseIndexed_kernel");} +} + +void FixedProjectiveConstraintCuda3d1_projectResponseIndexed(unsigned int size, const void* indices, void* dx) +{ + dim3 threads(BSIZE,1); + dim3 grid((size+BSIZE-1)/BSIZE,1); + {FixedProjectiveConstraintCuda3t1_projectResponseIndexed_kernel<<< grid, threads >>>(size, (const int*)indices, (CudaVec4*)dx); mycudaDebugError("FixedProjectiveConstraintCuda3t1_projectResponseIndexed_kernel");} +} + +void FixedProjectiveConstraintCudaRigid3d_projectResponseIndexed(unsigned int size, const void* indices, void* dx) +{ + dim3 threads(BSIZE,1); + dim3 grid((size+BSIZE-1)/BSIZE,1); + {FixedProjectiveConstraintCudaRigid3t_projectResponseIndexed_kernel<<< grid, threads >>>(size, (const int*)indices, (CudaRigidDeriv3*)dx); mycudaDebugError("FixedProjectiveConstraintCudaRigid3t_projectResponseIndexed_kernel");} +} + +#endif // SOFA_GPU_CUDA_DOUBLE + +#if defined(__cplusplus) && CUDA_VERSION < 2000 +} // namespace cuda +} // namespace gpu +} // namespace sofa +#endif diff --git a/applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/projective/CudaFixedProjectiveConstraint.h b/applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/projective/CudaFixedProjectiveConstraint.h new file mode 100644 index 00000000000..1c4d1bf8b08 --- /dev/null +++ b/applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/projective/CudaFixedProjectiveConstraint.h @@ -0,0 +1,112 @@ +/****************************************************************************** +* 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::component::constraint::projective +{ + +template +class FixedProjectiveConstraintInternalData< gpu::cuda::CudaVectorTypes > +{ +public: + using Index = sofa::Index; + typedef FixedProjectiveConstraintInternalData< gpu::cuda::CudaVectorTypes > Data; + typedef gpu::cuda::CudaVectorTypes DataTypes; + typedef FixedProjectiveConstraint Main; + typedef typename DataTypes::VecDeriv VecDeriv; + typedef typename DataTypes::Deriv Deriv; + typedef typename DataTypes::Real Real; + typedef typename Main::SetIndex SetIndex; + typedef typename Main::SetIndexArray SetIndexArray; + + // min/max fixed indices for contiguous constraints + Index minIndex; + Index maxIndex; + // vector of indices for general case + gpu::cuda::CudaVector cudaIndices; + + + static void init(Main* m); + + static void addConstraint(Main* m, Index index); + + static void removeConstraint(Main* m, Index index); + + static void projectResponse(Main* m, VecDeriv& dx); +}; + +template +class FixedProjectiveConstraintInternalData< gpu::cuda::CudaRigidTypes > +{ +public: + using Index = sofa::Index; + typedef FixedProjectiveConstraintInternalData< gpu::cuda::CudaRigidTypes > Data; + typedef gpu::cuda::CudaRigidTypes DataTypes; + typedef FixedProjectiveConstraint Main; + typedef typename DataTypes::VecDeriv VecDeriv; + typedef typename DataTypes::Deriv Deriv; + typedef typename DataTypes::Real Real; + typedef typename Main::SetIndex SetIndex; + typedef typename Main::SetIndexArray SetIndexArray; + + // min/max fixed indices for contiguous constraints + Index minIndex; + Index maxIndex; + // vector of indices for general case + gpu::cuda::CudaVector cudaIndices; + + + static void init(Main* m); + + static void addConstraint(Main* m, Index index); + + static void removeConstraint(Main* m, Index index); + + static void projectResponse(Main* m, VecDeriv& dx); +}; + +// I know using macros is bad design but this is the only way not to repeat the code for all CUDA types +#define CudaFixedProjectiveConstraint_DeclMethods(T) \ + template<> void FixedProjectiveConstraint< T >::init(); \ + template<> void FixedProjectiveConstraint< T >::addConstraint(Index index); \ + template<> void FixedProjectiveConstraint< T >::removeConstraint(Index index); \ + template<> void FixedProjectiveConstraint< T >::projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& resData); + +CudaFixedProjectiveConstraint_DeclMethods(gpu::cuda::CudaVec3fTypes); +CudaFixedProjectiveConstraint_DeclMethods(gpu::cuda::CudaVec3f1Types); +CudaFixedProjectiveConstraint_DeclMethods(gpu::cuda::CudaRigid3fTypes); + +#ifdef SOFA_GPU_CUDA_DOUBLE + +CudaFixedProjectiveConstraint_DeclMethods(gpu::cuda::CudaVec3dTypes); +CudaFixedProjectiveConstraint_DeclMethods(gpu::cuda::CudaVec3d1Types); +CudaFixedProjectiveConstraint_DeclMethods(gpu::cuda::CudaRigid3dTypes); + +#endif // SOFA_GPU_CUDA_DOUBLE + +#undef CudaFixedProjectiveConstraint_DeclMethods + + +} // namespace sofa::component::constraint::projective diff --git a/applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/projective/CudaFixedProjectiveConstraint.inl b/applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/projective/CudaFixedProjectiveConstraint.inl new file mode 100644 index 00000000000..6d3ce62aa71 --- /dev/null +++ b/applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/projective/CudaFixedProjectiveConstraint.inl @@ -0,0 +1,453 @@ +/****************************************************************************** +* 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::gpu::cuda +{ + +extern "C" +{ + void FixedProjectiveConstraintCuda1f_projectResponseContiguous(unsigned int size, void* dx); + void FixedProjectiveConstraintCuda1f_projectResponseIndexed(unsigned int size, const void* indices, void* dx); + void FixedProjectiveConstraintCuda3f_projectResponseContiguous(unsigned int size, void* dx); + void FixedProjectiveConstraintCuda3f_projectResponseIndexed(unsigned int size, const void* indices, void* dx); + void FixedProjectiveConstraintCuda3f1_projectResponseContiguous(unsigned int size, void* dx); + void FixedProjectiveConstraintCuda3f1_projectResponseIndexed(unsigned int size, const void* indices, void* dx); + void FixedProjectiveConstraintCudaRigid3f_projectResponseContiguous(unsigned int size, void* dx); + void FixedProjectiveConstraintCudaRigid3f_projectResponseIndexed(unsigned int size, const void* indices, void* dx); + +#ifdef SOFA_GPU_CUDA_DOUBLE + + void FixedProjectiveConstraintCuda3d_projectResponseContiguous(unsigned int size, void* dx); + void FixedProjectiveConstraintCuda3d_projectResponseIndexed(unsigned int size, const void* indices, void* dx); + void FixedProjectiveConstraintCuda3d1_projectResponseContiguous(unsigned int size, void* dx); + void FixedProjectiveConstraintCuda3d1_projectResponseIndexed(unsigned int size, const void* indices, void* dx); + void FixedProjectiveConstraintCudaRigid3d_projectResponseContiguous(unsigned int size, void* dx); + void FixedProjectiveConstraintCudaRigid3d_projectResponseIndexed(unsigned int size, const void* indices, void* dx); + +#endif // SOFA_GPU_CUDA_DOUBLE +} + +} // namespace sofa::gpu::cuda + +namespace sofa::component::constraint::projective +{ + +using namespace gpu::cuda; + +template +void FixedProjectiveConstraintInternalData< gpu::cuda::CudaVectorTypes >::init(Main* m) +{ + Data& data = *m->data; + data.minIndex = sofa::InvalidID; + data.maxIndex = sofa::InvalidID; + data.cudaIndices.clear(); + m->core::behavior::template ProjectiveConstraintSet::init(); + const SetIndexArray& indices = m->d_indices.getValue(); + if (!indices.empty()) + { + // put indices in a set to sort them and remove duplicates + std::set sortedIndices; + for (typename SetIndex::const_iterator it = indices.begin(); it!=indices.end(); ++it) + sortedIndices.insert(*it); + // check if the indices are contiguous + if (*sortedIndices.begin() + (int)sortedIndices.size()-1 == *sortedIndices.rbegin()) + { + data.minIndex = *sortedIndices.begin(); + data.maxIndex = *sortedIndices.rbegin(); + msg_info("CudaFixedProjectiveConstraint") << "init: " << sortedIndices.size() << " contiguous fixed indices, " << data.minIndex << " - " << data.maxIndex; + } + else + { + msg_info("CudaFixedProjectiveConstraint") << "init: " << sortedIndices.size() << " non-contiguous fixed indices"; + data.cudaIndices.reserve(sortedIndices.size()); + for (std::set::const_iterator it = sortedIndices.begin(); it!=sortedIndices.end(); ++it) + data.cudaIndices.push_back(*it); + } + } + + m->d_componentState.setValue(ComponentState::Valid); +} + +template +void FixedProjectiveConstraintInternalData< gpu::cuda::CudaVectorTypes >::addConstraint(Main* m, Index index) +{ + Data& data = *m->data; + //std::cout << "CudaFixedProjectiveConstraint::addConstraint("<d_indices.beginEdit()->push_back(index); + m->d_indices.endEdit(); + if (data.cudaIndices.empty()) + { + if (data.minIndex == sofa::InvalidID) + { + //std::cout << "CudaFixedProjectiveConstraint: single index "<= data.minIndex && index <= data.maxIndex) + { + // point already fixed + } + else if (data.minIndex == index + 1) + { + data.minIndex = index; + //std::cout << "CudaFixedProjectiveConstraint: new min index "< +void FixedProjectiveConstraintInternalData< gpu::cuda::CudaVectorTypes >::removeConstraint(Main* m, Index index) +{ + Data& data = *m->data; + removeValue(*m->d_indices.beginEdit(),index); + m->d_indices.endEdit(); + if (data.cudaIndices.empty()) + { + if (data.minIndex <= index && index <= data.maxIndex) + { + if (data.minIndex == index) + { + if (data.maxIndex == index) + { + // empty set + data.minIndex = sofa::InvalidID; + data.maxIndex = sofa::InvalidID; + } + else + ++data.minIndex; + } + else if (data.maxIndex == index) + --data.maxIndex; + else + { + data.cudaIndices.reserve(data.maxIndex-data.minIndex); + for (int i=data.minIndex; i +void FixedProjectiveConstraintInternalData< gpu::cuda::CudaRigidTypes >::init(Main* m) +{ + Data& data = *m->data; + data.minIndex = sofa::InvalidID; + data.maxIndex = sofa::InvalidID; + data.cudaIndices.clear(); + m->core::behavior::template ProjectiveConstraintSet::init(); + const SetIndexArray& indices = m->d_indices.getValue(); + if (!indices.empty()) + { + // put indices in a set to sort them and remove duplicates + std::set sortedIndices; + for (typename SetIndex::const_iterator it = indices.begin(); it!=indices.end(); ++it) + sortedIndices.insert(*it); + // check if the indices are contiguous + if (*sortedIndices.begin() + (int)sortedIndices.size()-1 == *sortedIndices.rbegin()) + { + data.minIndex = *sortedIndices.begin(); + data.maxIndex = *sortedIndices.rbegin(); + msg_info("CudaFixedProjectiveConstraint") << "init: " << sortedIndices.size() << " contiguous fixed indices, " << data.minIndex << " - " << data.maxIndex; + } + else + { + msg_info("CudaFixedProjectiveConstraint") << "init: " << sortedIndices.size() << " non-contiguous fixed indices"; + data.cudaIndices.reserve(sortedIndices.size()); + for (std::set::const_iterator it = sortedIndices.begin(); it!=sortedIndices.end(); ++it) + data.cudaIndices.push_back(*it); + } + } + + m->d_componentState.setValue(ComponentState::Valid); +} + +template +void FixedProjectiveConstraintInternalData< gpu::cuda::CudaRigidTypes >::addConstraint(Main* m, Index index) +{ + Data& data = *m->data; + //std::cout << "CudaFixedProjectiveConstraint::addConstraint("<d_indices.beginEdit()->push_back(index); + m->d_indices.endEdit(); + if (data.cudaIndices.empty()) + { + if (data.minIndex == sofa::InvalidID) + { + //std::cout << "CudaFixedProjectiveConstraint: single index "<= data.minIndex && index <= data.maxIndex) + { + // point already fixed + } + else if (data.minIndex == index+1) + { + data.minIndex = index; + //std::cout << "CudaFixedProjectiveConstraint: new min index "< +void FixedProjectiveConstraintInternalData< gpu::cuda::CudaRigidTypes >::removeConstraint(Main* m, Index index) +{ + Data& data = *m->data; + removeValue(*m->d_indices.beginEdit(),index); + m->d_indices.endEdit(); + if (data.cudaIndices.empty()) + { + if (data.minIndex <= index && index <= data.maxIndex) + { + if (data.minIndex == index) + { + if (data.maxIndex == index) + { + // empty set + data.minIndex = sofa::InvalidID; + data.maxIndex = sofa::InvalidID; + } + else + ++data.minIndex; + } + else if (data.maxIndex == index) + --data.maxIndex; + else + { + data.cudaIndices.reserve(data.maxIndex-data.minIndex); + for (int i=data.minIndex; i +void FixedProjectiveConstraintInternalData::projectResponse(Main* m, VecDeriv& dx) +{ + const Data& data = *m->data; + if (m->d_fixAll.getValue()) + FixedProjectiveConstraintCuda1f_projectResponseContiguous(dx.size(), ((float*)dx.deviceWrite())); + else if (data.minIndex >= 0) + FixedProjectiveConstraintCuda1f_projectResponseContiguous(data.maxIndex-data.minIndex+1, ((float*)dx.deviceWrite())+data.minIndex); + else + FixedProjectiveConstraintCuda1f_projectResponseIndexed(data.cudaIndices.size(), data.cudaIndices.deviceRead(), dx.deviceWrite()); +} + + +template <> +void FixedProjectiveConstraintInternalData::projectResponse(Main* m, VecDeriv& dx) +{ + const Data& data = *m->data; + if (m->d_fixAll.getValue()) + FixedProjectiveConstraintCuda3f_projectResponseContiguous(dx.size(), ((float*)dx.deviceWrite())); + else if (data.minIndex != sofa::InvalidID) + FixedProjectiveConstraintCuda3f_projectResponseContiguous(data.maxIndex - data.minIndex + 1, ((float*)dx.deviceWrite()) + 3 * data.minIndex); + else + FixedProjectiveConstraintCuda3f_projectResponseIndexed(data.cudaIndices.size(), data.cudaIndices.deviceRead(), dx.deviceWrite()); +} + + +template <> +void FixedProjectiveConstraintInternalData::projectResponse(Main* m, VecDeriv& dx) +{ + const Data& data = *m->data; + if (m->d_fixAll.getValue()) + FixedProjectiveConstraintCuda3f1_projectResponseContiguous(dx.size(), ((float*)dx.deviceWrite())); + else if (data.minIndex != sofa::InvalidID) + FixedProjectiveConstraintCuda3f1_projectResponseContiguous(data.maxIndex-data.minIndex+1, ((float*)dx.deviceWrite())+4*data.minIndex); + else + FixedProjectiveConstraintCuda3f1_projectResponseIndexed(data.cudaIndices.size(), data.cudaIndices.deviceRead(), dx.deviceWrite()); +} + +template <> +void FixedProjectiveConstraintInternalData::projectResponse(Main* m, VecDeriv& dx) +{ + const Data& data = *m->data; + if (m->d_fixAll.getValue()) + FixedProjectiveConstraintCudaRigid3f_projectResponseContiguous(dx.size(), ((float*)dx.deviceWrite())); + else if (data.minIndex != sofa::InvalidID) + FixedProjectiveConstraintCudaRigid3f_projectResponseContiguous(data.maxIndex-data.minIndex+1, ((float*)dx.deviceWrite())+6*data.minIndex); + else + FixedProjectiveConstraintCudaRigid3f_projectResponseIndexed(data.cudaIndices.size(), data.cudaIndices.deviceRead(), dx.deviceWrite()); +} + + + +#ifdef SOFA_GPU_CUDA_DOUBLE + +// // Handle topological changes +// template <> +// void FixedProjectiveConstraint::handleTopologyChange() { +// // std::list::const_iterator itBegin=topology->firstChange(); +// // std::list::const_iterator itEnd=topology->lastChange(); +// // +// // d_indices.beginEdit()->handleTopologyEvents(itBegin,itEnd,this->getMState()->getSize()); +// //printf("WARNING handleTopologyChange not implemented\n"); +// } + +template <> +void FixedProjectiveConstraintInternalData::projectResponse(Main* m, VecDeriv& dx) +{ + Data& data = *m->data; + if (m->d_fixAll.getValue()) + FixedProjectiveConstraintCuda3d_projectResponseContiguous(dx.size(), ((double*)dx.deviceWrite())); + else if (data.minIndex >= 0) + FixedProjectiveConstraintCuda3d_projectResponseContiguous(data.maxIndex-data.minIndex+1, ((double*)dx.deviceWrite())+3*data.minIndex); + else + FixedProjectiveConstraintCuda3d_projectResponseIndexed(data.cudaIndices.size(), data.cudaIndices.deviceRead(), dx.deviceWrite()); +} + +template <> +void FixedProjectiveConstraintInternalData::projectResponse(Main* m, VecDeriv& dx) +{ + Data& data = *m->data; + if (m->d_fixAll.getValue()) + FixedProjectiveConstraintCuda3d1_projectResponseContiguous(dx.size(), ((double*)dx.deviceWrite())); + else if (data.minIndex >= 0) + FixedProjectiveConstraintCuda3d1_projectResponseContiguous(data.maxIndex-data.minIndex+1, ((double*)dx.deviceWrite())+4*data.minIndex); + else + FixedProjectiveConstraintCuda3d1_projectResponseIndexed(data.cudaIndices.size(), data.cudaIndices.deviceRead(), dx.deviceWrite()); +} + +template <> +void FixedProjectiveConstraintInternalData::projectResponse(Main* m, VecDeriv& dx) +{ + Data& data = *m->data; + if (m->d_fixAll.getValue()) + FixedProjectiveConstraintCudaRigid3d_projectResponseContiguous(dx.size(), ((double*)dx.deviceWrite())); + else if (data.minIndex >= 0) + FixedProjectiveConstraintCudaRigid3d_projectResponseContiguous(data.maxIndex-data.minIndex+1, ((double*)dx.deviceWrite())+6*data.minIndex); + else + FixedProjectiveConstraintCudaRigid3d_projectResponseIndexed(data.cudaIndices.size(), data.cudaIndices.deviceRead(), dx.deviceWrite()); +} + +#endif // SOFA_GPU_CUDA_DOUBLE + +// I know using macros is bad design but this is the only way not to repeat the code for all CUDA types +#define CudaFixedProjectiveConstraint_ImplMethods(T) \ + template<> void FixedProjectiveConstraint< T >::init() \ + { data->init(this); } \ + template<> void FixedProjectiveConstraint< T >::addConstraint(Index index) \ + { data->addConstraint(this, index); } \ + template<> void FixedProjectiveConstraint< T >::removeConstraint(Index index) \ + { data->removeConstraint(this, index); } \ + template<> void FixedProjectiveConstraint< T >::projectResponse(const core::MechanicalParams* /* mparams */, DataVecDeriv& d_resData) \ + { \ + VecDeriv &resData = *d_resData.beginEdit(); \ + data->projectResponse(this, resData); \ + d_resData.endEdit(); \ + } + +CudaFixedProjectiveConstraint_ImplMethods(gpu::cuda::CudaVec3fTypes); +CudaFixedProjectiveConstraint_ImplMethods(gpu::cuda::CudaVec3f1Types); +CudaFixedProjectiveConstraint_ImplMethods(gpu::cuda::CudaRigid3fTypes); + +#ifdef SOFA_GPU_CUDA_DOUBLE + +CudaFixedProjectiveConstraint_ImplMethods(gpu::cuda::CudaVec3dTypes); +CudaFixedProjectiveConstraint_ImplMethods(gpu::cuda::CudaVec3d1Types); +CudaFixedProjectiveConstraint_ImplMethods(gpu::cuda::CudaRigid3dTypes); + +#endif // SOFA_GPU_CUDA_DOUBLE + +#undef CudaFixedProjectiveConstraint_ImplMethods + +} // namespace sofa::component::constraint::projective diff --git a/applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/projective/CudaFixedTranslationConstraint.cpp b/applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/projective/CudaFixedTranslationProjectiveConstraint.cpp similarity index 70% rename from applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/projective/CudaFixedTranslationConstraint.cpp rename to applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/projective/CudaFixedTranslationProjectiveConstraint.cpp index 7d9d18db439..64ebf9fecac 100644 --- a/applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/projective/CudaFixedTranslationConstraint.cpp +++ b/applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/projective/CudaFixedTranslationProjectiveConstraint.cpp @@ -21,7 +21,7 @@ ******************************************************************************/ #include #include -#include +#include #include @@ -32,15 +32,15 @@ namespace sofa::component::constraint::projective { template<> -void FixedTranslationConstraint::draw(const core::visual::VisualParams* vparams); +void FixedTranslationProjectiveConstraint::draw(const core::visual::VisualParams* vparams); #ifdef SOFA_GPU_CUDA_DOUBLE template<> -void FixedTranslationConstraint::draw(const core::visual::VisualParams* vparams); +void FixedTranslationProjectiveConstraint::draw(const core::visual::VisualParams* vparams); #endif // SOFA_GPU_CUDA_DOUBLE template <> -void component::constraint::projective::FixedTranslationConstraint::draw(const core::visual::VisualParams* vparams) +void component::constraint::projective::FixedTranslationProjectiveConstraint::draw(const core::visual::VisualParams* vparams) { #if SOFACUDA_HAVE_SOFA_GL == 1 const SetIndexArray & indices = f_indices.getValue(); @@ -71,7 +71,7 @@ void component::constraint::projective::FixedTranslationConstraint -void component::constraint::projective::FixedTranslationConstraint::draw(const core::visual::VisualParams* vparams) +void component::constraint::projective::FixedTranslationProjectiveConstraint::draw(const core::visual::VisualParams* vparams) { const SetIndexArray & indices = f_indices.getValue(); if (!vparams->displayFlags().getShowBehaviorModels()) @@ -100,11 +100,11 @@ void component::constraint::projective::FixedTranslationConstraint; -template class SOFA_GPU_CUDA_API FixedTranslationConstraint; +template class SOFA_GPU_CUDA_API FixedTranslationProjectiveConstraint; +template class SOFA_GPU_CUDA_API FixedTranslationProjectiveConstraint; #ifdef SOFA_GPU_CUDA_DOUBLE -template class SOFA_GPU_CUDA_API FixedTranslationConstraint; -template class SOFA_GPU_CUDA_API FixedTranslationConstraint; +template class SOFA_GPU_CUDA_API FixedTranslationProjectiveConstraint; +template class SOFA_GPU_CUDA_API FixedTranslationProjectiveConstraint; #endif // SOFA_GPU_CUDA_DOUBLE }// namespace sofa::component::constraint::projective @@ -113,16 +113,16 @@ namespace sofa::gpu::cuda { -int FixedTranslationConstraintCudaClass = core::RegisterObject("Supports GPU-side computations using CUDA") -// .add< component::constraint::projective::FixedTranslationConstraint >() -// .add< component::constraint::projective::FixedTranslationConstraint >() - .add< component::constraint::projective::FixedTranslationConstraint >() - .add< component::constraint::projective::FixedTranslationConstraint >() +int FixedTranslationProjectiveConstraintCudaClass = core::RegisterObject("Supports GPU-side computations using CUDA") +// .add< component::constraint::projective::FixedTranslationProjectiveConstraint >() +// .add< component::constraint::projective::FixedTranslationProjectiveConstraint >() + .add< component::constraint::projective::FixedTranslationProjectiveConstraint >() + .add< component::constraint::projective::FixedTranslationProjectiveConstraint >() #ifdef SOFA_GPU_CUDA_DOUBLE -// .add< component::constraint::projective::FixedTranslationConstraint >() -// .add< component::constraint::projective::FixedTranslationConstraint >() - .add< component::constraint::projective::FixedTranslationConstraint >() - .add< component::constraint::projective::FixedTranslationConstraint >() +// .add< component::constraint::projective::FixedTranslationProjectiveConstraint >() +// .add< component::constraint::projective::FixedTranslationProjectiveConstraint >() + .add< component::constraint::projective::FixedTranslationProjectiveConstraint >() + .add< component::constraint::projective::FixedTranslationProjectiveConstraint >() #endif // SOFA_GPU_CUDA_DOUBLE ; diff --git a/applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/projective/CudaLinearMovementConstraint.cu b/applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/projective/CudaLinearMovementConstraint.cu deleted file mode 100644 index 2deca0a26ad..00000000000 --- a/applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/projective/CudaLinearMovementConstraint.cu +++ /dev/null @@ -1,208 +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 -#include "cuda.h" -#include - -#if defined(__cplusplus) && CUDA_VERSION < 2000 -namespace sofa -{ -namespace gpu -{ -namespace cuda -{ -#endif - -extern "C" -{ - - void LinearMovementConstraintCudaVec6f_projectResponseIndexed(unsigned size, const void* indices, void* dx); - void LinearMovementConstraintCudaVec6f_projectPositionIndexed(unsigned size, const void* indices, const void* dir, const void* x0, void* x); - void LinearMovementConstraintCudaVec6f_projectVelocityIndexed(unsigned size, const void* indices, const void* dir, void* dx); - void LinearMovementConstraintCudaRigid3f_projectResponseIndexed(unsigned size, const void* indices, void* dx); - void LinearMovementConstraintCudaRigid3f_projectPositionIndexed(unsigned size, const void* indices, const void* dir, const void* x0, void* x); - void LinearMovementConstraintCudaRigid3f_projectVelocityIndexed(unsigned size, const void* indices, const void* dir, void* dx); - -#ifdef SOFA_GPU_CUDA_DOUBLE - void LinearMovementConstraintCudaVec6d_projectResponseIndexed(unsigned size, const void* indices, void* dx); - void LinearMovementConstraintCudaVec6d_projectPositionIndexed(unsigned size, const void* indices, const void* dir, const void* x0, void* x); - void LinearMovementConstraintCudaVec6d_projectVelocityIndexed(unsigned size, const void* indices, const void* dir, void* dx); - void LinearMovementConstraintCudaRigid3d_projectResponseIndexed(unsigned size, const void* indices, void* dx); - void LinearMovementConstraintCudaRigid3d_projectPositionIndexed(unsigned size, const void* indices, const void* dir, const void* x0, void* x); - void LinearMovementConstraintCudaRigid3d_projectVelocityIndexed(unsigned size, const void* indices, const void* dir, void* dx); -#endif // SOFA_GPU_CUDA_DOUBLE - -}// extern "C" - -////////////////////// -// GPU-side methods // -////////////////////// - -template -__global__ void LinearMovementConstraintCudaVec6t_projectPositionIndexed_kernel(unsigned size, const int* indices, real dirX, real dirY, real dirZ, real dirU, real dirV, real dirW, const CudaVec6* x0, CudaVec6* x) -{ - int index = blockIdx.x * BSIZE+threadIdx.x; - - CudaVec6 m = CudaVec6::make(dirX, dirY, dirZ, dirU, dirV, dirW); - if (index < size) - { - x[indices[index]] = x0[index]; - x[indices[index]] += m; - } -}// projectPositionIndexed_kernel - -template -__global__ void LinearMovementConstraintCudaRigid3t_projectResponseIndexed_kernel(unsigned size, const int* indices, CudaRigidDeriv3* dx) -{ - int index = blockIdx.x * BSIZE+threadIdx.x; - - if (index < size) - { - dx[indices[index]] = CudaRigidDeriv3::make(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); - } -}// kernel Indexed Cuda3t1 - -template -__global__ void LinearMovementConstraintCudaRigid3t_projectPositionIndexed_kernel(unsigned size, const int* indices, real dirX, real dirY, real dirZ, real dirU, real dirV, real dirW, const CudaRigidCoord3* x0, CudaRigidCoord3* x) -{ - int index = blockIdx.x * BSIZE+threadIdx.x; - - CudaRigidCoord3 m = CudaRigidCoord3::make(dirX, dirY, dirZ, dirU, dirV, dirW, 0.0); - if (index < size) - { - x[indices[index]] = x0[index]; - x[indices[index]] += m; - } -}// projectPositionIndexed_kernel - - -template -__global__ void LinearMovementConstraintCudaRigid3t_projectVelocityIndexed_kernel(unsigned size, const int* indices, real velX, real velY, real velZ, real velU, real velV, real velW, CudaRigidDeriv3* dx) -{ - int index = blockIdx.x * BSIZE+threadIdx.x; - - CudaRigidDeriv3 vel = CudaRigidDeriv3::make(velX, velY, velZ, velU, velV, velW); - if (index < size) - { - dx[indices[index]] = vel; - } -}// projectVelocityIndexed_kernel - -////////////////////// -// CPU-side methods // -////////////////////// -void LinearMovementConstraintCudaVec6f_projectResponseIndexed(unsigned size, const void* indices, void* dx) -{ - dim3 threads(BSIZE, 1); - dim3 grid((size+BSIZE-1)/BSIZE,1); - {LinearMovementConstraintCudaRigid3t_projectResponseIndexed_kernel<<< grid, threads >>>(size, (const int*)indices, (CudaRigidDeriv3*)dx); mycudaDebugError("LinearMovementConstraintCudaRigid3t_projectResponseIndexed_kernel");} -} - -void LinearMovementConstraintCudaVec6f_projectPositionIndexed(unsigned size, const void* indices, const void* dir, const void* x0, void* x) -{ - dim3 threads(BSIZE, 1); - dim3 grid((size+BSIZE-1)/BSIZE,1); - LinearMovementConstraintCudaVec6t_projectPositionIndexed_kernel<<< grid, threads >>>(size, (const int*)indices, - ((float*)dir)[0], ((float*)dir)[1], ((float*)dir)[2], ((float*)dir)[3], ((float*)dir)[4], ((float*)dir)[5], - (const CudaVec6*) x0, (CudaVec6*)x); -} - -void LinearMovementConstraintCudaVec6f_projectVelocityIndexed(unsigned size, const void* indices, const void* dir, void* dx) -{ - dim3 threads(BSIZE, 1); - dim3 grid((size+BSIZE-1)/BSIZE,1); - {LinearMovementConstraintCudaRigid3t_projectVelocityIndexed_kernel<<< grid, threads >>>(size, (const int*)indices, ((float*)dir)[0], ((float*)dir)[1], ((float*)dir)[2], ((float*)dir)[3], ((float*)dir)[4], ((float*)dir)[5], (CudaRigidDeriv3*)dx); mycudaDebugError("LinearMovementConstraintCudaRigid3t_projectVelocityIndexed_kernel");} -} -void LinearMovementConstraintCudaRigid3f_projectResponseIndexed(unsigned size, const void* indices, void* dx) -{ - dim3 threads(BSIZE, 1); - dim3 grid((size+BSIZE-1)/BSIZE,1); - {LinearMovementConstraintCudaRigid3t_projectResponseIndexed_kernel<<< grid, threads >>>(size, (const int*)indices, (CudaRigidDeriv3*)dx); mycudaDebugError("LinearMovementConstraintCudaRigid3t_projectResponseIndexed_kernel");} -} - -void LinearMovementConstraintCudaRigid3f_projectPositionIndexed(unsigned size, const void* indices, const void* dir, const void* x0, void* x) -{ - dim3 threads(BSIZE, 1); - dim3 grid((size+BSIZE-1)/BSIZE,1); - {LinearMovementConstraintCudaRigid3t_projectPositionIndexed_kernel<<< grid, threads >>>(size, (const int*)indices, ((float*)dir)[0], ((float*)dir)[1], ((float*)dir)[2], ((float*)dir)[3], ((float*)dir)[4], ((float*)dir)[5], (const CudaRigidCoord3*) x0, (CudaRigidCoord3*)x); mycudaDebugError("LinearMovementConstraintCudaRigid3t_projectPositionIndexed_kernel");} -} - -void LinearMovementConstraintCudaRigid3f_projectVelocityIndexed(unsigned size, const void* indices, const void* dir, void* dx) -{ - dim3 threads(BSIZE, 1); - dim3 grid((size+BSIZE-1)/BSIZE,1); - {LinearMovementConstraintCudaRigid3t_projectVelocityIndexed_kernel<<< grid, threads >>>(size, (const int*)indices, ((float*)dir)[0], ((float*)dir)[1], ((float*)dir)[2], ((float*)dir)[3], ((float*)dir)[4], ((float*)dir)[5], (CudaRigidDeriv3*)dx); mycudaDebugError("LinearMovementConstraintCudaRigid3t_projectVelocityIndexed_kernel");} -} - -#ifdef SOFA_GPU_CUDA_DOUBLE -void LinearMovementConstraintCudaVec6d_projectResponseIndexed(unsigned size, const void* indices, void* dx) -{ - dim3 threads(BSIZE, 1); - dim3 grid((size+BSIZE-1)/BSIZE,1); - {LinearMovementConstraintCudaRigid3t_projectResponseIndexed_kernel<<< grid, threads >>>(size, (const int*)indices, (CudaRigidDeriv3*)dx); mycudaDebugError("LinearMovementConstraintCudaRigid3t_projectResponseIndexed_kernel");} -} - -void LinearMovementConstraintCudaVec6d_projectPositionIndexed(unsigned size, const void* indices, const void* dir, const void* x0, void* x) -{ - dim3 threads(BSIZE, 1); - dim3 grid((size+BSIZE-1)/BSIZE,1); - LinearMovementConstraintCudaVec6t_projectPositionIndexed_kernel<<< grid, threads >>>(size, (const int*)indices, - ((double*)dir)[0], ((double*)dir)[1], ((double*)dir)[2], ((double*)dir)[3], ((double*)dir)[4], ((double*)dir)[5], - (const CudaVec6*) x0, (CudaVec6*)x); -} - -void LinearMovementConstraintCudaVec6d_projectVelocityIndexed(unsigned size, const void* indices, const void* dir, void* dx) -{ - dim3 threads(BSIZE, 1); - dim3 grid((size+BSIZE-1)/BSIZE,1); - {LinearMovementConstraintCudaRigid3t_projectVelocityIndexed_kernel<<< grid, threads >>>(size, (const int*)indices, ((double*)dir)[0], ((double*)dir)[1], ((double*)dir)[2], ((double*)dir)[3], ((double*)dir)[4], ((double*)dir)[5], (CudaRigidDeriv3*)dx); mycudaDebugError("LinearMovementConstraintCudaRigid3t_projectVelocityIndexed_kernel");} -} - -void LinearMovementConstraintCudaRigid3d_projectResponseIndexed(unsigned size, const void* indices, void* dx) -{ - dim3 threads(BSIZE, 1); - dim3 grid((size+BSIZE-1)/BSIZE,1); - {LinearMovementConstraintCudaRigid3t_projectResponseIndexed_kernel<<< grid, threads >>>(size, (const int*)indices, (CudaRigidDeriv3*)dx); mycudaDebugError("LinearMovementConstraintCudaRigid3t_projectResponseIndexed_kernel");} -} - -void LinearMovementConstraintCudaRigid3d_projectPositionIndexed(unsigned size, const void* indices, const void* dir, const void* x0, void* x) -{ - dim3 threads(BSIZE, 1); - dim3 grid((size+BSIZE-1)/BSIZE,1); - {LinearMovementConstraintCudaRigid3t_projectPositionIndexed_kernel<<< grid, threads >>>(size, (const int*)indices, ((double*)dir)[0], ((double*)dir)[1], ((double*)dir)[2], ((double*)dir)[3], ((double*)dir)[4], ((double*)dir)[5], (const CudaRigidCoord3*) x0, (CudaRigidCoord3*)x); mycudaDebugError("LinearMovementConstraintCudaRigid3t_projectPositionIndexed_kernel");} -} - -void LinearMovementConstraintCudaRigid3d_projectVelocityIndexed(unsigned size, const void* indices, const void* dir, void* dx) -{ - dim3 threads(BSIZE, 1); - dim3 grid((size+BSIZE-1)/BSIZE,1); - {LinearMovementConstraintCudaRigid3t_projectVelocityIndexed_kernel<<< grid, threads >>>(size, (const int*)indices, ((double*)dir)[0], ((double*)dir)[1], ((double*)dir)[2], ((double*)dir)[3], ((double*)dir)[4], ((double*)dir)[5], (CudaRigidDeriv3*)dx); mycudaDebugError("LinearMovementConstraintCudaRigid3t_projectVelocityIndexed_kernel");} -} - -#endif // SOFA_GPU_CUDA_DOUBLE - -#if defined(__cplusplus) && CUDA_VERSION < 2000 -} // namespace cuda -} // namespace gpu -} // namespace sofa -#endif diff --git a/applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/projective/CudaLinearMovementConstraint.h b/applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/projective/CudaLinearMovementConstraint.h index fcfa31f5a2f..a5251686676 100644 --- a/applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/projective/CudaLinearMovementConstraint.h +++ b/applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/projective/CudaLinearMovementConstraint.h @@ -21,190 +21,6 @@ ******************************************************************************/ #pragma once -#include -#include +#include -namespace sofa::gpu::cuda -{ - -template -class CudaKernelsLinearMovementConstraint; - -}// namespace sofa::gpu::cuda - -namespace sofa::component::constraint::projective -{ - -template -class LinearMovementConstraintInternalData< gpu::cuda::CudaVectorTypes > -{ -public: - using Index = sofa::Index; - typedef LinearMovementConstraintInternalData< gpu::cuda::CudaVectorTypes > Data; - typedef gpu::cuda::CudaVectorTypes DataTypes; - typedef LinearMovementConstraint Main; - typedef typename DataTypes::VecCoord VecCoord; - typedef typename DataTypes::VecDeriv VecDeriv; - typedef typename DataTypes::MatrixDeriv MatrixDeriv; - typedef typename DataTypes::MatrixDeriv::RowType MatrixDerivRowType; - typedef typename DataTypes::Deriv Deriv; - typedef typename DataTypes::Real Real; - typedef typename DataTypes::Coord Coord; - typedef type::vector SetIndexArray; - typedef sofa::core::topology::PointSubsetData< SetIndexArray > SetIndex; - - typedef sofa::core::objectmodel::Data DataVecDeriv; - typedef sofa::core::objectmodel::Data DataMatrixDeriv; - - typedef gpu::cuda::CudaKernelsLinearMovementConstraint Kernels; - - // vector of indices for general case - gpu::cuda::CudaVector indices; - - // initial positions - gpu::cuda::CudaVector x0; - - int size; - - static void init(Main* m, VecCoord& x); - - static void addIndex(Main* m, Index index); - - static void removeIndex(Main* m, Index index); - - static void projectResponse(Main* m, VecDeriv& dx); - static void projectPosition(Main* m, VecCoord& x); - static void projectVelocity(Main* m, VecDeriv& dx); - -}; - - -template -class LinearMovementConstraintInternalData< gpu::cuda::CudaRigidTypes > -{ -public: - using Index = sofa::Index; - typedef LinearMovementConstraintInternalData< gpu::cuda::CudaRigidTypes > Data; - typedef gpu::cuda::CudaRigidTypes DataTypes; - typedef LinearMovementConstraint Main; - typedef typename DataTypes::VecCoord VecCoord; - typedef typename DataTypes::VecDeriv VecDeriv; - typedef typename DataTypes::MatrixDeriv MatrixDeriv; - typedef typename DataTypes::MatrixDeriv::RowType MatrixDerivRowType; - typedef typename DataTypes::Deriv Deriv; - typedef typename DataTypes::Real Real; - typedef typename DataTypes::Coord Coord; - typedef type::vector SetIndexArray; - typedef sofa::core::topology::PointSubsetData< SetIndexArray > SetIndex; - - typedef sofa::core::objectmodel::Data DataVecDeriv; - typedef sofa::core::objectmodel::Data DataMatrixDeriv; - - typedef gpu::cuda::CudaKernelsLinearMovementConstraint Kernels; - - // vector of indices for general case - gpu::cuda::CudaVector indices; - - // initial positions - gpu::cuda::CudaVector x0; - - int size; - - static void init(Main* m, VecCoord& x); - - static void addIndex(Main* m, Index index); - - static void removeIndex(Main* m, Index index); - - static void projectResponse(Main* m, VecDeriv& dx); - static void projectPosition(Main* m, VecCoord& x); - static void projectVelocity(Main* m, VecDeriv& dx); - -}; - - -// template<> -// void LinearMovementConstraint< gpu::cuda::CudaRigid3fTypes >::init(); - -template<> -void LinearMovementConstraint< gpu::cuda::CudaVec6fTypes >::addIndex(Index index); - -template<> -void LinearMovementConstraint< gpu::cuda::CudaVec6fTypes >::removeIndex(Index index); - -template<> -void LinearMovementConstraint< gpu::cuda::CudaVec6fTypes >::projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& dx); - -template<> -void LinearMovementConstraint< gpu::cuda::CudaVec6fTypes >::projectJacobianMatrix(const core::MechanicalParams* mparams, DataMatrixDeriv& dx); - -template<> -void LinearMovementConstraint< gpu::cuda::CudaVec6fTypes >::projectPosition(const core::MechanicalParams* mparams, DataVecCoord& x); - -template<> -void LinearMovementConstraint< gpu::cuda::CudaVec6fTypes >::projectVelocity(const core::MechanicalParams* mparams, DataVecDeriv& dx); - -template<> -void LinearMovementConstraint< gpu::cuda::CudaRigid3fTypes >::addIndex(Index index); - -template<> -void LinearMovementConstraint< gpu::cuda::CudaRigid3fTypes >::removeIndex(Index index); - -template<> -void LinearMovementConstraint< gpu::cuda::CudaRigid3fTypes >::projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& dx); - -template<> -void LinearMovementConstraint< gpu::cuda::CudaRigid3fTypes >::projectJacobianMatrix(const core::MechanicalParams* mparams, DataMatrixDeriv& dx); - -template<> -void LinearMovementConstraint< gpu::cuda::CudaRigid3fTypes >::projectPosition(const core::MechanicalParams* mparams, DataVecCoord& x); - -template<> -void LinearMovementConstraint< gpu::cuda::CudaRigid3fTypes >::projectVelocity(const core::MechanicalParams* mparams, DataVecDeriv& dx); - -#ifdef SOFA_GPU_CUDA_DOUBLE - -// template<> -// void LinearMovementConstraint< gpu::cuda::CudaRigid3dTypes >::init(); - -template<> -void LinearMovementConstraint< gpu::cuda::CudaVec6dTypes >::addIndex(Index index); - -template<> -void LinearMovementConstraint< gpu::cuda::CudaVec6dTypes >::removeIndex(Index index); - -template<> -void LinearMovementConstraint< gpu::cuda::CudaVec6dTypes >::projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& dx); - -template<> -void LinearMovementConstraint< gpu::cuda::CudaVec6dTypes >::projectJacobianMatrix(const core::MechanicalParams* mparams, DataMatrixDeriv& dx); - -template<> -void LinearMovementConstraint< gpu::cuda::CudaVec6dTypes >::projectPosition(const core::MechanicalParams* mparams, DataVecCoord& x); - -template<> -void LinearMovementConstraint< gpu::cuda::CudaVec6dTypes >::projectVelocity(const core::MechanicalParams* mparams, DataVecDeriv& dx); - -template<> -void LinearMovementConstraint< gpu::cuda::CudaRigid3dTypes >::addIndex(Index index); - -template<> -void LinearMovementConstraint< gpu::cuda::CudaRigid3dTypes >::removeIndex(Index index); - -template<> -void LinearMovementConstraint< gpu::cuda::CudaRigid3dTypes >::projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& dx); - -template<> -void LinearMovementConstraint< gpu::cuda::CudaRigid3dTypes >::projectJacobianMatrix(const core::MechanicalParams* mparams, DataMatrixDeriv& dx); - -template<> -void LinearMovementConstraint< gpu::cuda::CudaRigid3dTypes >::projectPosition(const core::MechanicalParams* mparams, DataVecCoord& x); - -template<> -void LinearMovementConstraint< gpu::cuda::CudaRigid3dTypes >::projectVelocity(const core::MechanicalParams* mparams, DataVecDeriv& dx); - -#endif // SOFA_GPU_CUDA_DOUBLE - - - -} // namespace sofa::component::constraint::projective +SOFA_DEPRECATED_HEADER("v23.12", "v24.12", "SofaCUDA/component/constraint/projective/CudaLinearMovementProjectiveConstraint.h") diff --git a/applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/projective/CudaLinearMovementConstraint.inl b/applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/projective/CudaLinearMovementConstraint.inl index a9abcd99c24..1771a40013b 100644 --- a/applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/projective/CudaLinearMovementConstraint.inl +++ b/applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/projective/CudaLinearMovementConstraint.inl @@ -21,576 +21,6 @@ ******************************************************************************/ #pragma once -#include -#include +#include -namespace sofa::gpu::cuda -{ - -extern "C" -{ - - void LinearMovementConstraintCudaVec6f_projectResponseIndexed(unsigned size, const void* indices, void* dx); - void LinearMovementConstraintCudaVec6f_projectPositionIndexed(unsigned size, const void* indices, const void* dir, const void* x0, void* x); - void LinearMovementConstraintCudaVec6f_projectVelocityIndexed(unsigned size, const void* indices, const void* dir, void* dx); - void LinearMovementConstraintCudaRigid3f_projectResponseIndexed(unsigned size, const void* indices, void* dx); - void LinearMovementConstraintCudaRigid3f_projectPositionIndexed(unsigned size, const void* indices, const void* dir, const void* x0, void* x); - void LinearMovementConstraintCudaRigid3f_projectVelocityIndexed(unsigned size, const void* indices, const void* dir, void* dx); - -#ifdef SOFA_GPU_CUDA_DOUBLE - void LinearMovementConstraintCudaVec6d_projectResponseIndexed(unsigned size, const void* indices, void* dx); - void LinearMovementConstraintCudaVec6d_projectPositionIndexed(unsigned size, const void* indices, const void* dir, const void* x0, void* x); - void LinearMovementConstraintCudaVec6d_projectVelocityIndexed(unsigned size, const void* indices, const void* dir, void* dx); - void LinearMovementConstraintCudaRigid3d_projectResponseIndexed(unsigned size, const void* indices, void* dx); - void LinearMovementConstraintCudaRigid3d_projectPositionIndexed(unsigned size, const void* indices, const void* dir, const void* x0, void* x); - void LinearMovementConstraintCudaRigid3d_projectVelocityIndexed(unsigned size, const void* indices, const void* dir, void* dx); -#endif // SOFA_GPU_CUDA_DOUBLE - -}// extern "C" - -template<> -class CudaKernelsLinearMovementConstraint< CudaVec6fTypes > -{ -public: - static void projectResponse(unsigned size, const void* indices, void* dx) - { - LinearMovementConstraintCudaVec6f_projectResponseIndexed(size, indices, dx); - } - static void projectPosition(unsigned size, const void* indices, const void* dir, const void* x0, void* x) - { - LinearMovementConstraintCudaVec6f_projectPositionIndexed(size, indices, dir, x0, x); - } - static void projectVelocity(unsigned size, const void* indices, const void* dir, void* dx) - { - LinearMovementConstraintCudaVec6f_projectVelocityIndexed(size, indices, dir, dx); - } -};// CudaKernelsLinearMovementConstraint - -template<> -class CudaKernelsLinearMovementConstraint< CudaRigid3fTypes > -{ -public: - static void projectResponse(unsigned size, const void* indices, void* dx) - { - LinearMovementConstraintCudaRigid3f_projectResponseIndexed(size, indices, dx); - } - static void projectPosition(unsigned size, const void* indices, const void* dir, const void* x0, void* x) - { - LinearMovementConstraintCudaRigid3f_projectPositionIndexed(size, indices, dir, x0, x); - } - static void projectVelocity(unsigned size, const void* indices, const void* dir, void* dx) - { - LinearMovementConstraintCudaRigid3f_projectVelocityIndexed(size, indices, dir, dx); - } -};// CudaKernelsLinearMovementConstraint - -#ifdef SOFA_GPU_CUDA_DOUBLE - -template<> -class CudaKernelsLinearMovementConstraint< CudaVec6dTypes > -{ -public: - static void projectResponse(unsigned size, const void* indices, void* dx) - { - LinearMovementConstraintCudaVec6d_projectResponseIndexed(size, indices, dx); - } - static void projectPosition(unsigned size, const void* indices, const void* dir, const void* x0, void* x) - { - LinearMovementConstraintCudaVec6d_projectPositionIndexed(size, indices, dir, x0, x); - } - static void projectVelocity(unsigned size, const void* indices, const void* dir, void* dx) - { - LinearMovementConstraintCudaVec6d_projectVelocityIndexed(size, indices, dir, dx); - } -};// CudaKernelsLinearMovementConstraint - -template<> -class CudaKernelsLinearMovementConstraint< CudaRigid3dTypes > -{ -public: - static void projectResponse(unsigned size, const void* indices, void* dx) - { - LinearMovementConstraintCudaRigid3d_projectResponseIndexed(size, indices, dx); - } - static void projectPosition(unsigned size, const void* indices, const void* dir, const void* x0, void* x) - { - LinearMovementConstraintCudaRigid3d_projectPositionIndexed(size, indices, dir, x0, x); - } - static void projectVelocity(unsigned size, const void* indices, const void* dir, void* dx) - { - LinearMovementConstraintCudaRigid3d_projectVelocityIndexed(size, indices, dir, dx); - } -};// CudaKernelsLinearMovementConstraint - -#endif // SOFA_GPU_CUDA_DOUBLE - -} // namespace sofa::gpu::cuda - -namespace sofa::component::constraint::projective -{ - -///////////////////////////////////// -// CudaVectorTypes specializations -///////////////////////////////////// -template -void LinearMovementConstraintInternalData< gpu::cuda::CudaVectorTypes >::addIndex(Main* m, Index index) -{ - Data& data = *m->data; - - m->m_indices.beginEdit()->push_back(index); - m->m_indices.endEdit(); - - data.indices.push_back(index); - // TODO : then it becomes non-consistent and also in the main version !!! -// data.x0.push_back(); - -}// LinearMovementConstraintInternalData::addIndex - -template -void LinearMovementConstraintInternalData< gpu::cuda::CudaVectorTypes >::removeIndex(Main* m, Index index) -{ - // Data& data = m->data; - - removeValue(*m->m_indices.beginEdit(),index); - m->m_indices.endEdit(); - - // removeValue(data.indices, index); - // TODO : then it becomes non-consistent and also in the main version !!! -// data.x0.push_back(); - -}// LinearMovementConstraintInternalData::removeIndex - -template -void LinearMovementConstraintInternalData< gpu::cuda::CudaVectorTypes >::init(Main* m, VecCoord& x) -{ - Data& data = *m->data; - const SetIndexArray & indices = m->m_indices.getValue(); -// m->x0.resize( indices.size() ); -// for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) -// m->x0[*it] = x[*it]; - - // gpu part - data.size = indices.size(); - data.indices.resize(data.size); - unsigned index =0; - for (typename SetIndex::const_iterator it = indices.begin(); it != indices.end(); it++) - { - data.indices[index] = *it; - index++; - } - - data.x0.resize(data.size); - index = 0; - for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) - { - data.x0[index] = x[*it]; - index++; - } -}// LinearMovementConstraintInternalData::init - -template -void LinearMovementConstraintInternalData< gpu::cuda::CudaVectorTypes >::projectResponse(Main* m, VecDeriv& dx) -{ - Data& data = *m->data; - - Real cT = (Real) m->getContext()->getTime(); - - if ((cT != m->currentTime) || !m->finished) - { - m->findKeyTimes(); - } - - if (m->finished && m->nextT != m->prevT) - { - Kernels::projectResponse( - data.size, - data.indices.deviceRead(), - dx.deviceWrite() - ); - } -}// LinearMovementConstraintInternalData::projectResponse - -template -void LinearMovementConstraintInternalData< gpu::cuda::CudaVectorTypes >::projectPosition(Main* m, VecCoord& x) -{ - Data& data = *m->data; - - Real cT = (Real) m->getContext()->getTime(); - - // initialize initial Dofs positions, if it's not done - if (data.x0.size() == 0) - { - data.init(m, x); - } - - if ((cT != m->currentTime) || !m->finished) - { - m->findKeyTimes(); - } - - if (m->finished && m->nextT != m->prevT) - { - Real dt = (cT - m->prevT) / (m->nextT - m->prevT); - Deriv depl = m->prevM + (m->nextM-m->prevM)*dt; - - Kernels::projectPosition( - data.size, - data.indices.deviceRead(), - depl.ptr(), - data.x0.deviceRead(), - x.deviceWrite() - ); - } -}// LinearMovementConstraintInternalData::projectPosition - -template -void LinearMovementConstraintInternalData< gpu::cuda::CudaVectorTypes >::projectVelocity(Main* m, VecDeriv& dx) -{ - Data& data = *m->data; - - Real cT = (Real) m->getContext()->getTime(); - - if ((cT != m->currentTime) || !m->finished) - m->findKeyTimes(); - - if (m->finished && m->nextT != m->prevT) - { - Deriv dv = (m->nextM - m->prevM)*(1.0/(m->nextT - m->prevT)); - - Kernels::projectVelocity( - data.size, - data.indices.deviceRead(), - dv.ptr(), - dx.deviceWrite() - ); - } -}// LinearMovementConstraintInternalData::projectVelocity - - -///////////////////////////////////// -// CudaRigidTypes specializations -///////////////////////////////////// - -template -void LinearMovementConstraintInternalData< gpu::cuda::CudaRigidTypes >::addIndex(Main* m, Index index) -{ - Data& data = *m->data; - - m->m_indices.beginEdit()->push_back(index); - m->m_indices.endEdit(); - - data.indices.push_back(index); - // TODO : then it becomes non-consistent and also in the main version !!! -// data.x0.push_back(); - -}// LinearMovementConstraintInternalData::addIndex - -template -void LinearMovementConstraintInternalData< gpu::cuda::CudaRigidTypes >::removeIndex(Main* m, Index index) -{ - // Data& data = m->data; - - removeValue(*m->m_indices.beginEdit(),index); - m->m_indices.endEdit(); - - // removeValue(data.indices, index); - // TODO : then it becomes non-consistent and also in the main version !!! -// data.x0.push_back(); - -}// LinearMovementConstraintInternalData::removeIndex - -template -void LinearMovementConstraintInternalData< gpu::cuda::CudaRigidTypes >::init(Main* m, VecCoord& x) -{ - Data& data = *m->data; - const SetIndexArray & indices = m->m_indices.getValue(); -// m->x0.resize( indices.size() ); -// for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) -// m->x0[*it] = x[*it]; - - // gpu part - data.size = indices.size(); - data.indices.resize(data.size); - unsigned index =0; - for (typename SetIndex::const_iterator it = indices.begin(); it != indices.end(); it++) - { - data.indices[index] = *it; - index++; - } - - data.x0.resize(data.size); - index = 0; - for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) - { - data.x0[index] = x[*it]; - index++; - } -}// LinearMovementConstraintInternalData::init - -template -void LinearMovementConstraintInternalData< gpu::cuda::CudaRigidTypes >::projectResponse(Main* m, VecDeriv& dx) -{ - Data& data = *m->data; - - Real cT = (Real) m->getContext()->getTime(); - - if ((cT != m->currentTime) || !m->finished) - { - m->findKeyTimes(); - } - - if (m->finished && m->nextT != m->prevT) - { - Kernels::projectResponse( - data.size, - data.indices.deviceRead(), - dx.deviceWrite() - ); - } -}// LinearMovementConstraintInternalData::projectResponse - -template -void LinearMovementConstraintInternalData< gpu::cuda::CudaRigidTypes >::projectPosition(Main* m, VecCoord& x) -{ - Data& data = *m->data; - - Real cT = (Real) m->getContext()->getTime(); - - // initialize initial Dofs positions, if it's not done - if (data.x0.size() == 0) - { - data.init(m, x); - } - - if ((cT != m->currentTime) || !m->finished) - { - m->findKeyTimes(); - } - - if (m->finished && m->nextT != m->prevT) - { - Real dt = (cT - m->prevT) / (m->nextT - m->prevT); - Deriv depl = m->prevM + (m->nextM-m->prevM)*dt; - - Kernels::projectPosition( - data.size, - data.indices.deviceRead(), - depl.ptr(), - data.x0.deviceRead(), - x.deviceWrite() - ); - } -}// LinearMovementConstraintInternalData::projectPosition - -template -void LinearMovementConstraintInternalData< gpu::cuda::CudaRigidTypes >::projectVelocity(Main* m, VecDeriv& dx) -{ - Data& data = *m->data; - - Real cT = (Real) m->getContext()->getTime(); - - if ((cT != m->currentTime) || !m->finished) - m->findKeyTimes(); - - if (m->finished && m->nextT != m->prevT) - { - Deriv dv = (m->nextM - m->prevM)*(1.0/(m->nextT - m->prevT)); - - Kernels::projectVelocity( - data.size, - data.indices.deviceRead(), - dv.ptr(), - dx.deviceWrite() - ); - } -}// LinearMovementConstraintInternalData::projectVelocity - - -////////////////////////////// -// Specializations -///////////////////////////// - -// template<> -// void LinearMovementConstraint< gpu::cuda::CudaRigid3fTypes >::init() -// { -// data.init(this); -// } - -template<> -void LinearMovementConstraint< gpu::cuda::CudaVec6fTypes >::addIndex(Index index) -{ - data->addIndex(this, index); -} - -template<> -void LinearMovementConstraint< gpu::cuda::CudaVec6fTypes >::removeIndex(Index index) -{ - data->removeIndex(this, index); -} - -template<> -void LinearMovementConstraint< gpu::cuda::CudaVec6fTypes >::projectResponse(const core::MechanicalParams* /* mparams */, DataVecDeriv& dx) -{ - VecDeriv& _dx = *dx.beginEdit(); - data->projectResponse(this, _dx); - dx.endEdit(); -} - -template<> -void LinearMovementConstraint< gpu::cuda::CudaVec6fTypes >::projectJacobianMatrix(const core::MechanicalParams* /* mparams */, DataMatrixDeriv& /*dx*/) -{ - /* data.projectResponseT(this, index); */ -} - -template<> -void LinearMovementConstraint< gpu::cuda::CudaVec6fTypes >::projectPosition(const core::MechanicalParams* /* mparams */, DataVecCoord& x) -{ - VecCoord& _x = *x.beginEdit(); - data->projectPosition(this, _x); - x.endEdit(); -} - -template<> -void LinearMovementConstraint< gpu::cuda::CudaVec6fTypes >::projectVelocity(const core::MechanicalParams* /* mparams */, DataVecDeriv& dx) -{ - VecDeriv& _dx = *dx.beginEdit(); - data->projectVelocity(this, _dx); - dx.endEdit(); -} - -template<> -void LinearMovementConstraint< gpu::cuda::CudaRigid3fTypes >::addIndex(Index index) -{ - data->addIndex(this, index); -} - -template<> -void LinearMovementConstraint< gpu::cuda::CudaRigid3fTypes >::removeIndex(Index index) -{ - data->removeIndex(this, index); -} - -template<> -void LinearMovementConstraint< gpu::cuda::CudaRigid3fTypes >::projectResponse(const core::MechanicalParams* /* mparams */, DataVecDeriv& dx) -{ - VecDeriv& _dx = *dx.beginEdit(); - data->projectResponse(this, _dx); - dx.endEdit(); -} - -template<> -void LinearMovementConstraint< gpu::cuda::CudaRigid3fTypes >::projectJacobianMatrix(const core::MechanicalParams* /* mparams */, DataMatrixDeriv& /*dx*/) -{ - /* data.projectResponseT(this, index); */ -} - -template<> -void LinearMovementConstraint< gpu::cuda::CudaRigid3fTypes >::projectPosition(const core::MechanicalParams* /* mparams */, DataVecCoord& x) -{ - VecCoord& _x = *x.beginEdit(); - data->projectPosition(this, _x); - x.endEdit(); -} - -template<> -void LinearMovementConstraint< gpu::cuda::CudaRigid3fTypes >::projectVelocity(const core::MechanicalParams* /* mparams */, DataVecDeriv& dx) -{ - VecDeriv& _dx = *dx.beginEdit(); - data->projectVelocity(this, _dx); - dx.endEdit(); -} - -#ifdef SOFA_GPU_CUDA_DOUBLE -// template<> -// void LinearMovementConstraint< gpu::cuda::CudaRigid3dTypes >::init() -// { -// data->init(this); -// } - -template<> -void LinearMovementConstraint< gpu::cuda::CudaVec6dTypes >::addIndex(Index index) -{ - data->addIndex(this, index); -} - -template<> -void LinearMovementConstraint< gpu::cuda::CudaVec6dTypes >::removeIndex(Index index) -{ - data->removeIndex(this, index); -} - -template<> -void LinearMovementConstraint< gpu::cuda::CudaVec6dTypes >::projectResponse(const core::MechanicalParams* /* mparams */, DataVecDeriv& dx) -{ - VecDeriv& _dx = *dx.beginEdit(); - data->projectResponse(this, _dx); - dx.endEdit(); -} - -template<> -void LinearMovementConstraint< gpu::cuda::CudaVec6dTypes >::projectJacobianMatrix(const core::MechanicalParams* /* mparams */, DataMatrixDeriv& /*dx*/) -{ - /* data.projectResponseT(this, index); */ -} - -template<> -void LinearMovementConstraint< gpu::cuda::CudaVec6dTypes >::projectPosition(const core::MechanicalParams* /* mparams */, DataVecCoord& x) -{ - VecCoord& _x = *x.beginEdit(); - data->projectPosition(this, _x); - x.endEdit(); -} - -template<> -void LinearMovementConstraint< gpu::cuda::CudaVec6dTypes >::projectVelocity(const core::MechanicalParams* /* mparams */, DataVecDeriv& dx) -{ - VecDeriv& _dx = *dx.beginEdit(); - data->projectVelocity(this, _dx); - dx.endEdit(); -} - -template<> -void LinearMovementConstraint< gpu::cuda::CudaRigid3dTypes >::addIndex(Index index) -{ - data->addIndex(this, index); -} - -template<> -void LinearMovementConstraint< gpu::cuda::CudaRigid3dTypes >::removeIndex(Index index) -{ - data->removeIndex(this, index); -} - -template<> -void LinearMovementConstraint< gpu::cuda::CudaRigid3dTypes >::projectResponse(const core::MechanicalParams* /* mparams */, DataVecDeriv& dx) -{ - VecDeriv& _dx = *dx.beginEdit(); - data->projectResponse(this, _dx); - dx.endEdit(); -} - -template<> -void LinearMovementConstraint< gpu::cuda::CudaRigid3dTypes >::projectJacobianMatrix(const core::MechanicalParams* /* mparams */, DataMatrixDeriv& /*dx*/) -{ - /* data.projectResponseT(this, index); */ -} - -template<> -void LinearMovementConstraint< gpu::cuda::CudaRigid3dTypes >::projectPosition(const core::MechanicalParams* /* mparams */, DataVecCoord& x) -{ - VecCoord& _x = *x.beginEdit(); - data->projectPosition(this, _x); - x.endEdit(); -} - -template<> -void LinearMovementConstraint< gpu::cuda::CudaRigid3dTypes >::projectVelocity(const core::MechanicalParams* /* mparams */, DataVecDeriv& dx) -{ - VecDeriv& _dx = *dx.beginEdit(); - data->projectVelocity(this, _dx); - dx.endEdit(); -} - -#endif // SOFA_GPU_CUDA_DOUBLE - -} // namespace sofa::component::constraint::projective +SOFA_DEPRECATED_HEADER("v23.12", "v24.12", "SofaCUDA/component/constraint/projective/CudaLinearMovementProjectiveConstraint.inl") diff --git a/applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/projective/CudaLinearMovementConstraint.cpp b/applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/projective/CudaLinearMovementProjectiveConstraint.cpp similarity index 72% rename from applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/projective/CudaLinearMovementConstraint.cpp rename to applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/projective/CudaLinearMovementProjectiveConstraint.cpp index 6968e2ddfed..2ef5054b202 100644 --- a/applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/projective/CudaLinearMovementConstraint.cpp +++ b/applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/projective/CudaLinearMovementProjectiveConstraint.cpp @@ -20,7 +20,7 @@ * Contact information: contact@sofa-framework.org * ******************************************************************************/ #include -#include +#include #include #include @@ -41,16 +41,16 @@ namespace sofa::gpu::cuda { -int LinearMovementConstraintCudaClass = core::RegisterObject("Supports GPU-side computations using CUDA") -// .add< component::constraint::projective::LinearMovementConstraint >() -// .add< component::constraint::projective::LinearMovementConstraint >() - .add< component::constraint::projective::LinearMovementConstraint >() - .add< component::constraint::projective::LinearMovementConstraint >() +int LinearMovementProjectiveConstraintCudaClass = core::RegisterObject("Supports GPU-side computations using CUDA") +// .add< component::constraint::projective::LinearMovementProjectiveConstraint >() +// .add< component::constraint::projective::LinearMovementProjectiveConstraint >() + .add< component::constraint::projective::LinearMovementProjectiveConstraint >() + .add< component::constraint::projective::LinearMovementProjectiveConstraint >() #ifdef SOFA_GPU_CUDA_DOUBLE -// .add< component::constraint::projective::LinearMovementConstraint >() -// .add< component::constraint::projective::LinearMovementConstraint >() - .add< component::constraint::projective::LinearMovementConstraint >() - .add< component::constraint::projective::LinearMovementConstraint >() +// .add< component::constraint::projective::LinearMovementProjectiveConstraint >() +// .add< component::constraint::projective::LinearMovementProjectiveConstraint >() + .add< component::constraint::projective::LinearMovementProjectiveConstraint >() + .add< component::constraint::projective::LinearMovementProjectiveConstraint >() #endif // SOFA_GPU_CUDA_DOUBLE ; diff --git a/applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/projective/CudaLinearMovementProjectiveConstraint.cu b/applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/projective/CudaLinearMovementProjectiveConstraint.cu new file mode 100644 index 00000000000..d315362b0de --- /dev/null +++ b/applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/projective/CudaLinearMovementProjectiveConstraint.cu @@ -0,0 +1,208 @@ +/****************************************************************************** +* 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 +#include "cuda.h" +#include + +#if defined(__cplusplus) && CUDA_VERSION < 2000 +namespace sofa +{ +namespace gpu +{ +namespace cuda +{ +#endif + +extern "C" +{ + + void LinearMovementProjectiveConstraintCudaVec6f_projectResponseIndexed(unsigned size, const void* indices, void* dx); + void LinearMovementProjectiveConstraintCudaVec6f_projectPositionIndexed(unsigned size, const void* indices, const void* dir, const void* x0, void* x); + void LinearMovementProjectiveConstraintCudaVec6f_projectVelocityIndexed(unsigned size, const void* indices, const void* dir, void* dx); + void LinearMovementProjectiveConstraintCudaRigid3f_projectResponseIndexed(unsigned size, const void* indices, void* dx); + void LinearMovementProjectiveConstraintCudaRigid3f_projectPositionIndexed(unsigned size, const void* indices, const void* dir, const void* x0, void* x); + void LinearMovementProjectiveConstraintCudaRigid3f_projectVelocityIndexed(unsigned size, const void* indices, const void* dir, void* dx); + +#ifdef SOFA_GPU_CUDA_DOUBLE + void LinearMovementProjectiveConstraintCudaVec6d_projectResponseIndexed(unsigned size, const void* indices, void* dx); + void LinearMovementProjectiveConstraintCudaVec6d_projectPositionIndexed(unsigned size, const void* indices, const void* dir, const void* x0, void* x); + void LinearMovementProjectiveConstraintCudaVec6d_projectVelocityIndexed(unsigned size, const void* indices, const void* dir, void* dx); + void LinearMovementProjectiveConstraintCudaRigid3d_projectResponseIndexed(unsigned size, const void* indices, void* dx); + void LinearMovementProjectiveConstraintCudaRigid3d_projectPositionIndexed(unsigned size, const void* indices, const void* dir, const void* x0, void* x); + void LinearMovementProjectiveConstraintCudaRigid3d_projectVelocityIndexed(unsigned size, const void* indices, const void* dir, void* dx); +#endif // SOFA_GPU_CUDA_DOUBLE + +}// extern "C" + +////////////////////// +// GPU-side methods // +////////////////////// + +template +__global__ void LinearMovementProjectiveConstraintCudaVec6t_projectPositionIndexed_kernel(unsigned size, const int* indices, real dirX, real dirY, real dirZ, real dirU, real dirV, real dirW, const CudaVec6* x0, CudaVec6* x) +{ + int index = blockIdx.x * BSIZE+threadIdx.x; + + CudaVec6 m = CudaVec6::make(dirX, dirY, dirZ, dirU, dirV, dirW); + if (index < size) + { + x[indices[index]] = x0[index]; + x[indices[index]] += m; + } +}// projectPositionIndexed_kernel + +template +__global__ void LinearMovementProjectiveConstraintCudaRigid3t_projectResponseIndexed_kernel(unsigned size, const int* indices, CudaRigidDeriv3* dx) +{ + int index = blockIdx.x * BSIZE+threadIdx.x; + + if (index < size) + { + dx[indices[index]] = CudaRigidDeriv3::make(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + } +}// kernel Indexed Cuda3t1 + +template +__global__ void LinearMovementProjectiveConstraintCudaRigid3t_projectPositionIndexed_kernel(unsigned size, const int* indices, real dirX, real dirY, real dirZ, real dirU, real dirV, real dirW, const CudaRigidCoord3* x0, CudaRigidCoord3* x) +{ + int index = blockIdx.x * BSIZE+threadIdx.x; + + CudaRigidCoord3 m = CudaRigidCoord3::make(dirX, dirY, dirZ, dirU, dirV, dirW, 0.0); + if (index < size) + { + x[indices[index]] = x0[index]; + x[indices[index]] += m; + } +}// projectPositionIndexed_kernel + + +template +__global__ void LinearMovementProjectiveConstraintCudaRigid3t_projectVelocityIndexed_kernel(unsigned size, const int* indices, real velX, real velY, real velZ, real velU, real velV, real velW, CudaRigidDeriv3* dx) +{ + int index = blockIdx.x * BSIZE+threadIdx.x; + + CudaRigidDeriv3 vel = CudaRigidDeriv3::make(velX, velY, velZ, velU, velV, velW); + if (index < size) + { + dx[indices[index]] = vel; + } +}// projectVelocityIndexed_kernel + +////////////////////// +// CPU-side methods // +////////////////////// +void LinearMovementProjectiveConstraintCudaVec6f_projectResponseIndexed(unsigned size, const void* indices, void* dx) +{ + dim3 threads(BSIZE, 1); + dim3 grid((size+BSIZE-1)/BSIZE,1); + {LinearMovementProjectiveConstraintCudaRigid3t_projectResponseIndexed_kernel<<< grid, threads >>>(size, (const int*)indices, (CudaRigidDeriv3*)dx); mycudaDebugError("LinearMovementProjectiveConstraintCudaRigid3t_projectResponseIndexed_kernel");} +} + +void LinearMovementProjectiveConstraintCudaVec6f_projectPositionIndexed(unsigned size, const void* indices, const void* dir, const void* x0, void* x) +{ + dim3 threads(BSIZE, 1); + dim3 grid((size+BSIZE-1)/BSIZE,1); + LinearMovementProjectiveConstraintCudaVec6t_projectPositionIndexed_kernel<<< grid, threads >>>(size, (const int*)indices, + ((float*)dir)[0], ((float*)dir)[1], ((float*)dir)[2], ((float*)dir)[3], ((float*)dir)[4], ((float*)dir)[5], + (const CudaVec6*) x0, (CudaVec6*)x); +} + +void LinearMovementProjectiveConstraintCudaVec6f_projectVelocityIndexed(unsigned size, const void* indices, const void* dir, void* dx) +{ + dim3 threads(BSIZE, 1); + dim3 grid((size+BSIZE-1)/BSIZE,1); + {LinearMovementProjectiveConstraintCudaRigid3t_projectVelocityIndexed_kernel<<< grid, threads >>>(size, (const int*)indices, ((float*)dir)[0], ((float*)dir)[1], ((float*)dir)[2], ((float*)dir)[3], ((float*)dir)[4], ((float*)dir)[5], (CudaRigidDeriv3*)dx); mycudaDebugError("LinearMovementProjectiveConstraintCudaRigid3t_projectVelocityIndexed_kernel");} +} +void LinearMovementProjectiveConstraintCudaRigid3f_projectResponseIndexed(unsigned size, const void* indices, void* dx) +{ + dim3 threads(BSIZE, 1); + dim3 grid((size+BSIZE-1)/BSIZE,1); + {LinearMovementProjectiveConstraintCudaRigid3t_projectResponseIndexed_kernel<<< grid, threads >>>(size, (const int*)indices, (CudaRigidDeriv3*)dx); mycudaDebugError("LinearMovementProjectiveConstraintCudaRigid3t_projectResponseIndexed_kernel");} +} + +void LinearMovementProjectiveConstraintCudaRigid3f_projectPositionIndexed(unsigned size, const void* indices, const void* dir, const void* x0, void* x) +{ + dim3 threads(BSIZE, 1); + dim3 grid((size+BSIZE-1)/BSIZE,1); + {LinearMovementProjectiveConstraintCudaRigid3t_projectPositionIndexed_kernel<<< grid, threads >>>(size, (const int*)indices, ((float*)dir)[0], ((float*)dir)[1], ((float*)dir)[2], ((float*)dir)[3], ((float*)dir)[4], ((float*)dir)[5], (const CudaRigidCoord3*) x0, (CudaRigidCoord3*)x); mycudaDebugError("LinearMovementProjectiveConstraintCudaRigid3t_projectPositionIndexed_kernel");} +} + +void LinearMovementProjectiveConstraintCudaRigid3f_projectVelocityIndexed(unsigned size, const void* indices, const void* dir, void* dx) +{ + dim3 threads(BSIZE, 1); + dim3 grid((size+BSIZE-1)/BSIZE,1); + {LinearMovementProjectiveConstraintCudaRigid3t_projectVelocityIndexed_kernel<<< grid, threads >>>(size, (const int*)indices, ((float*)dir)[0], ((float*)dir)[1], ((float*)dir)[2], ((float*)dir)[3], ((float*)dir)[4], ((float*)dir)[5], (CudaRigidDeriv3*)dx); mycudaDebugError("LinearMovementProjectiveConstraintCudaRigid3t_projectVelocityIndexed_kernel");} +} + +#ifdef SOFA_GPU_CUDA_DOUBLE +void LinearMovementProjectiveConstraintCudaVec6d_projectResponseIndexed(unsigned size, const void* indices, void* dx) +{ + dim3 threads(BSIZE, 1); + dim3 grid((size+BSIZE-1)/BSIZE,1); + {LinearMovementProjectiveConstraintCudaRigid3t_projectResponseIndexed_kernel<<< grid, threads >>>(size, (const int*)indices, (CudaRigidDeriv3*)dx); mycudaDebugError("LinearMovementProjectiveConstraintCudaRigid3t_projectResponseIndexed_kernel");} +} + +void LinearMovementProjectiveConstraintCudaVec6d_projectPositionIndexed(unsigned size, const void* indices, const void* dir, const void* x0, void* x) +{ + dim3 threads(BSIZE, 1); + dim3 grid((size+BSIZE-1)/BSIZE,1); + LinearMovementProjectiveConstraintCudaVec6t_projectPositionIndexed_kernel<<< grid, threads >>>(size, (const int*)indices, + ((double*)dir)[0], ((double*)dir)[1], ((double*)dir)[2], ((double*)dir)[3], ((double*)dir)[4], ((double*)dir)[5], + (const CudaVec6*) x0, (CudaVec6*)x); +} + +void LinearMovementProjectiveConstraintCudaVec6d_projectVelocityIndexed(unsigned size, const void* indices, const void* dir, void* dx) +{ + dim3 threads(BSIZE, 1); + dim3 grid((size+BSIZE-1)/BSIZE,1); + {LinearMovementProjectiveConstraintCudaRigid3t_projectVelocityIndexed_kernel<<< grid, threads >>>(size, (const int*)indices, ((double*)dir)[0], ((double*)dir)[1], ((double*)dir)[2], ((double*)dir)[3], ((double*)dir)[4], ((double*)dir)[5], (CudaRigidDeriv3*)dx); mycudaDebugError("LinearMovementProjectiveConstraintCudaRigid3t_projectVelocityIndexed_kernel");} +} + +void LinearMovementProjectiveConstraintCudaRigid3d_projectResponseIndexed(unsigned size, const void* indices, void* dx) +{ + dim3 threads(BSIZE, 1); + dim3 grid((size+BSIZE-1)/BSIZE,1); + {LinearMovementProjectiveConstraintCudaRigid3t_projectResponseIndexed_kernel<<< grid, threads >>>(size, (const int*)indices, (CudaRigidDeriv3*)dx); mycudaDebugError("LinearMovementProjectiveConstraintCudaRigid3t_projectResponseIndexed_kernel");} +} + +void LinearMovementProjectiveConstraintCudaRigid3d_projectPositionIndexed(unsigned size, const void* indices, const void* dir, const void* x0, void* x) +{ + dim3 threads(BSIZE, 1); + dim3 grid((size+BSIZE-1)/BSIZE,1); + {LinearMovementProjectiveConstraintCudaRigid3t_projectPositionIndexed_kernel<<< grid, threads >>>(size, (const int*)indices, ((double*)dir)[0], ((double*)dir)[1], ((double*)dir)[2], ((double*)dir)[3], ((double*)dir)[4], ((double*)dir)[5], (const CudaRigidCoord3*) x0, (CudaRigidCoord3*)x); mycudaDebugError("LinearMovementProjectiveConstraintCudaRigid3t_projectPositionIndexed_kernel");} +} + +void LinearMovementProjectiveConstraintCudaRigid3d_projectVelocityIndexed(unsigned size, const void* indices, const void* dir, void* dx) +{ + dim3 threads(BSIZE, 1); + dim3 grid((size+BSIZE-1)/BSIZE,1); + {LinearMovementProjectiveConstraintCudaRigid3t_projectVelocityIndexed_kernel<<< grid, threads >>>(size, (const int*)indices, ((double*)dir)[0], ((double*)dir)[1], ((double*)dir)[2], ((double*)dir)[3], ((double*)dir)[4], ((double*)dir)[5], (CudaRigidDeriv3*)dx); mycudaDebugError("LinearMovementProjectiveConstraintCudaRigid3t_projectVelocityIndexed_kernel");} +} + +#endif // SOFA_GPU_CUDA_DOUBLE + +#if defined(__cplusplus) && CUDA_VERSION < 2000 +} // namespace cuda +} // namespace gpu +} // namespace sofa +#endif diff --git a/applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/projective/CudaLinearMovementProjectiveConstraint.h b/applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/projective/CudaLinearMovementProjectiveConstraint.h new file mode 100644 index 00000000000..be287995b87 --- /dev/null +++ b/applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/projective/CudaLinearMovementProjectiveConstraint.h @@ -0,0 +1,210 @@ +/****************************************************************************** +* 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::gpu::cuda +{ + +template +class CudaKernelsLinearMovementProjectiveConstraint; + +}// namespace sofa::gpu::cuda + +namespace sofa::component::constraint::projective +{ + +template +class LinearMovementProjectiveConstraintInternalData< gpu::cuda::CudaVectorTypes > +{ +public: + using Index = sofa::Index; + typedef LinearMovementProjectiveConstraintInternalData< gpu::cuda::CudaVectorTypes > Data; + typedef gpu::cuda::CudaVectorTypes DataTypes; + typedef LinearMovementProjectiveConstraint Main; + typedef typename DataTypes::VecCoord VecCoord; + typedef typename DataTypes::VecDeriv VecDeriv; + typedef typename DataTypes::MatrixDeriv MatrixDeriv; + typedef typename DataTypes::MatrixDeriv::RowType MatrixDerivRowType; + typedef typename DataTypes::Deriv Deriv; + typedef typename DataTypes::Real Real; + typedef typename DataTypes::Coord Coord; + typedef type::vector SetIndexArray; + typedef sofa::core::topology::PointSubsetData< SetIndexArray > SetIndex; + + typedef sofa::core::objectmodel::Data DataVecDeriv; + typedef sofa::core::objectmodel::Data DataMatrixDeriv; + + typedef gpu::cuda::CudaKernelsLinearMovementProjectiveConstraint Kernels; + + // vector of indices for general case + gpu::cuda::CudaVector indices; + + // initial positions + gpu::cuda::CudaVector x0; + + int size; + + static void init(Main* m, VecCoord& x); + + static void addIndex(Main* m, Index index); + + static void removeIndex(Main* m, Index index); + + static void projectResponse(Main* m, VecDeriv& dx); + static void projectPosition(Main* m, VecCoord& x); + static void projectVelocity(Main* m, VecDeriv& dx); + +}; + + +template +class LinearMovementProjectiveConstraintInternalData< gpu::cuda::CudaRigidTypes > +{ +public: + using Index = sofa::Index; + typedef LinearMovementProjectiveConstraintInternalData< gpu::cuda::CudaRigidTypes > Data; + typedef gpu::cuda::CudaRigidTypes DataTypes; + typedef LinearMovementProjectiveConstraint Main; + typedef typename DataTypes::VecCoord VecCoord; + typedef typename DataTypes::VecDeriv VecDeriv; + typedef typename DataTypes::MatrixDeriv MatrixDeriv; + typedef typename DataTypes::MatrixDeriv::RowType MatrixDerivRowType; + typedef typename DataTypes::Deriv Deriv; + typedef typename DataTypes::Real Real; + typedef typename DataTypes::Coord Coord; + typedef type::vector SetIndexArray; + typedef sofa::core::topology::PointSubsetData< SetIndexArray > SetIndex; + + typedef sofa::core::objectmodel::Data DataVecDeriv; + typedef sofa::core::objectmodel::Data DataMatrixDeriv; + + typedef gpu::cuda::CudaKernelsLinearMovementProjectiveConstraint Kernels; + + // vector of indices for general case + gpu::cuda::CudaVector indices; + + // initial positions + gpu::cuda::CudaVector x0; + + int size; + + static void init(Main* m, VecCoord& x); + + static void addIndex(Main* m, Index index); + + static void removeIndex(Main* m, Index index); + + static void projectResponse(Main* m, VecDeriv& dx); + static void projectPosition(Main* m, VecCoord& x); + static void projectVelocity(Main* m, VecDeriv& dx); + +}; + + +// template<> +// void LinearMovementProjectiveConstraint< gpu::cuda::CudaRigid3fTypes >::init(); + +template<> +void LinearMovementProjectiveConstraint< gpu::cuda::CudaVec6fTypes >::addIndex(Index index); + +template<> +void LinearMovementProjectiveConstraint< gpu::cuda::CudaVec6fTypes >::removeIndex(Index index); + +template<> +void LinearMovementProjectiveConstraint< gpu::cuda::CudaVec6fTypes >::projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& dx); + +template<> +void LinearMovementProjectiveConstraint< gpu::cuda::CudaVec6fTypes >::projectJacobianMatrix(const core::MechanicalParams* mparams, DataMatrixDeriv& dx); + +template<> +void LinearMovementProjectiveConstraint< gpu::cuda::CudaVec6fTypes >::projectPosition(const core::MechanicalParams* mparams, DataVecCoord& x); + +template<> +void LinearMovementProjectiveConstraint< gpu::cuda::CudaVec6fTypes >::projectVelocity(const core::MechanicalParams* mparams, DataVecDeriv& dx); + +template<> +void LinearMovementProjectiveConstraint< gpu::cuda::CudaRigid3fTypes >::addIndex(Index index); + +template<> +void LinearMovementProjectiveConstraint< gpu::cuda::CudaRigid3fTypes >::removeIndex(Index index); + +template<> +void LinearMovementProjectiveConstraint< gpu::cuda::CudaRigid3fTypes >::projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& dx); + +template<> +void LinearMovementProjectiveConstraint< gpu::cuda::CudaRigid3fTypes >::projectJacobianMatrix(const core::MechanicalParams* mparams, DataMatrixDeriv& dx); + +template<> +void LinearMovementProjectiveConstraint< gpu::cuda::CudaRigid3fTypes >::projectPosition(const core::MechanicalParams* mparams, DataVecCoord& x); + +template<> +void LinearMovementProjectiveConstraint< gpu::cuda::CudaRigid3fTypes >::projectVelocity(const core::MechanicalParams* mparams, DataVecDeriv& dx); + +#ifdef SOFA_GPU_CUDA_DOUBLE + +// template<> +// void LinearMovementProjectiveConstraint< gpu::cuda::CudaRigid3dTypes >::init(); + +template<> +void LinearMovementProjectiveConstraint< gpu::cuda::CudaVec6dTypes >::addIndex(Index index); + +template<> +void LinearMovementProjectiveConstraint< gpu::cuda::CudaVec6dTypes >::removeIndex(Index index); + +template<> +void LinearMovementProjectiveConstraint< gpu::cuda::CudaVec6dTypes >::projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& dx); + +template<> +void LinearMovementProjectiveConstraint< gpu::cuda::CudaVec6dTypes >::projectJacobianMatrix(const core::MechanicalParams* mparams, DataMatrixDeriv& dx); + +template<> +void LinearMovementProjectiveConstraint< gpu::cuda::CudaVec6dTypes >::projectPosition(const core::MechanicalParams* mparams, DataVecCoord& x); + +template<> +void LinearMovementProjectiveConstraint< gpu::cuda::CudaVec6dTypes >::projectVelocity(const core::MechanicalParams* mparams, DataVecDeriv& dx); + +template<> +void LinearMovementProjectiveConstraint< gpu::cuda::CudaRigid3dTypes >::addIndex(Index index); + +template<> +void LinearMovementProjectiveConstraint< gpu::cuda::CudaRigid3dTypes >::removeIndex(Index index); + +template<> +void LinearMovementProjectiveConstraint< gpu::cuda::CudaRigid3dTypes >::projectResponse(const core::MechanicalParams* mparams, DataVecDeriv& dx); + +template<> +void LinearMovementProjectiveConstraint< gpu::cuda::CudaRigid3dTypes >::projectJacobianMatrix(const core::MechanicalParams* mparams, DataMatrixDeriv& dx); + +template<> +void LinearMovementProjectiveConstraint< gpu::cuda::CudaRigid3dTypes >::projectPosition(const core::MechanicalParams* mparams, DataVecCoord& x); + +template<> +void LinearMovementProjectiveConstraint< gpu::cuda::CudaRigid3dTypes >::projectVelocity(const core::MechanicalParams* mparams, DataVecDeriv& dx); + +#endif // SOFA_GPU_CUDA_DOUBLE + + + +} // namespace sofa::component::constraint::projective diff --git a/applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/projective/CudaLinearMovementProjectiveConstraint.inl b/applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/projective/CudaLinearMovementProjectiveConstraint.inl new file mode 100644 index 00000000000..e3f00a0f5d7 --- /dev/null +++ b/applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/projective/CudaLinearMovementProjectiveConstraint.inl @@ -0,0 +1,596 @@ +/****************************************************************************** +* 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::gpu::cuda +{ + +extern "C" +{ + + void LinearMovementProjectiveConstraintCudaVec6f_projectResponseIndexed(unsigned size, const void* indices, void* dx); + void LinearMovementProjectiveConstraintCudaVec6f_projectPositionIndexed(unsigned size, const void* indices, const void* dir, const void* x0, void* x); + void LinearMovementProjectiveConstraintCudaVec6f_projectVelocityIndexed(unsigned size, const void* indices, const void* dir, void* dx); + void LinearMovementProjectiveConstraintCudaRigid3f_projectResponseIndexed(unsigned size, const void* indices, void* dx); + void LinearMovementProjectiveConstraintCudaRigid3f_projectPositionIndexed(unsigned size, const void* indices, const void* dir, const void* x0, void* x); + void LinearMovementProjectiveConstraintCudaRigid3f_projectVelocityIndexed(unsigned size, const void* indices, const void* dir, void* dx); + +#ifdef SOFA_GPU_CUDA_DOUBLE + void LinearMovementProjectiveConstraintCudaVec6d_projectResponseIndexed(unsigned size, const void* indices, void* dx); + void LinearMovementProjectiveConstraintCudaVec6d_projectPositionIndexed(unsigned size, const void* indices, const void* dir, const void* x0, void* x); + void LinearMovementProjectiveConstraintCudaVec6d_projectVelocityIndexed(unsigned size, const void* indices, const void* dir, void* dx); + void LinearMovementProjectiveConstraintCudaRigid3d_projectResponseIndexed(unsigned size, const void* indices, void* dx); + void LinearMovementProjectiveConstraintCudaRigid3d_projectPositionIndexed(unsigned size, const void* indices, const void* dir, const void* x0, void* x); + void LinearMovementProjectiveConstraintCudaRigid3d_projectVelocityIndexed(unsigned size, const void* indices, const void* dir, void* dx); +#endif // SOFA_GPU_CUDA_DOUBLE + +}// extern "C" + +template<> +class CudaKernelsLinearMovementProjectiveConstraint< CudaVec6fTypes > +{ +public: + static void projectResponse(unsigned size, const void* indices, void* dx) + { + LinearMovementProjectiveConstraintCudaVec6f_projectResponseIndexed(size, indices, dx); + } + static void projectPosition(unsigned size, const void* indices, const void* dir, const void* x0, void* x) + { + LinearMovementProjectiveConstraintCudaVec6f_projectPositionIndexed(size, indices, dir, x0, x); + } + static void projectVelocity(unsigned size, const void* indices, const void* dir, void* dx) + { + LinearMovementProjectiveConstraintCudaVec6f_projectVelocityIndexed(size, indices, dir, dx); + } +};// CudaKernelsLinearMovementProjectiveConstraint + +template<> +class CudaKernelsLinearMovementProjectiveConstraint< CudaRigid3fTypes > +{ +public: + static void projectResponse(unsigned size, const void* indices, void* dx) + { + LinearMovementProjectiveConstraintCudaRigid3f_projectResponseIndexed(size, indices, dx); + } + static void projectPosition(unsigned size, const void* indices, const void* dir, const void* x0, void* x) + { + LinearMovementProjectiveConstraintCudaRigid3f_projectPositionIndexed(size, indices, dir, x0, x); + } + static void projectVelocity(unsigned size, const void* indices, const void* dir, void* dx) + { + LinearMovementProjectiveConstraintCudaRigid3f_projectVelocityIndexed(size, indices, dir, dx); + } +};// CudaKernelsLinearMovementProjectiveConstraint + +#ifdef SOFA_GPU_CUDA_DOUBLE + +template<> +class CudaKernelsLinearMovementProjectiveConstraint< CudaVec6dTypes > +{ +public: + static void projectResponse(unsigned size, const void* indices, void* dx) + { + LinearMovementProjectiveConstraintCudaVec6d_projectResponseIndexed(size, indices, dx); + } + static void projectPosition(unsigned size, const void* indices, const void* dir, const void* x0, void* x) + { + LinearMovementProjectiveConstraintCudaVec6d_projectPositionIndexed(size, indices, dir, x0, x); + } + static void projectVelocity(unsigned size, const void* indices, const void* dir, void* dx) + { + LinearMovementProjectiveConstraintCudaVec6d_projectVelocityIndexed(size, indices, dir, dx); + } +};// CudaKernelsLinearMovementProjectiveConstraint + +template<> +class CudaKernelsLinearMovementProjectiveConstraint< CudaRigid3dTypes > +{ +public: + static void projectResponse(unsigned size, const void* indices, void* dx) + { + LinearMovementProjectiveConstraintCudaRigid3d_projectResponseIndexed(size, indices, dx); + } + static void projectPosition(unsigned size, const void* indices, const void* dir, const void* x0, void* x) + { + LinearMovementProjectiveConstraintCudaRigid3d_projectPositionIndexed(size, indices, dir, x0, x); + } + static void projectVelocity(unsigned size, const void* indices, const void* dir, void* dx) + { + LinearMovementProjectiveConstraintCudaRigid3d_projectVelocityIndexed(size, indices, dir, dx); + } +};// CudaKernelsLinearMovementProjectiveConstraint + +#endif // SOFA_GPU_CUDA_DOUBLE + +} // namespace sofa::gpu::cuda + +namespace sofa::component::constraint::projective +{ + +///////////////////////////////////// +// CudaVectorTypes specializations +///////////////////////////////////// +template +void LinearMovementProjectiveConstraintInternalData< gpu::cuda::CudaVectorTypes >::addIndex(Main* m, Index index) +{ + Data& data = *m->data; + + m->m_indices.beginEdit()->push_back(index); + m->m_indices.endEdit(); + + data.indices.push_back(index); + // TODO : then it becomes non-consistent and also in the main version !!! +// data.x0.push_back(); + +}// LinearMovementProjectiveConstraintInternalData::addIndex + +template +void LinearMovementProjectiveConstraintInternalData< gpu::cuda::CudaVectorTypes >::removeIndex(Main* m, Index index) +{ + // Data& data = m->data; + + removeValue(*m->m_indices.beginEdit(),index); + m->m_indices.endEdit(); + + // removeValue(data.indices, index); + // TODO : then it becomes non-consistent and also in the main version !!! +// data.x0.push_back(); + +}// LinearMovementProjectiveConstraintInternalData::removeIndex + +template +void LinearMovementProjectiveConstraintInternalData< gpu::cuda::CudaVectorTypes >::init(Main* m, VecCoord& x) +{ + Data& data = *m->data; + const SetIndexArray & indices = m->m_indices.getValue(); +// m->x0.resize( indices.size() ); +// for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) +// m->x0[*it] = x[*it]; + + // gpu part + data.size = indices.size(); + data.indices.resize(data.size); + unsigned index =0; + for (typename SetIndex::const_iterator it = indices.begin(); it != indices.end(); it++) + { + data.indices[index] = *it; + index++; + } + + data.x0.resize(data.size); + index = 0; + for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) + { + data.x0[index] = x[*it]; + index++; + } +}// LinearMovementProjectiveConstraintInternalData::init + +template +void LinearMovementProjectiveConstraintInternalData< gpu::cuda::CudaVectorTypes >::projectResponse(Main* m, VecDeriv& dx) +{ + Data& data = *m->data; + + Real cT = (Real) m->getContext()->getTime(); + + if ((cT != m->currentTime) || !m->finished) + { + m->findKeyTimes(); + } + + if (m->finished && m->nextT != m->prevT) + { + Kernels::projectResponse( + data.size, + data.indices.deviceRead(), + dx.deviceWrite() + ); + } +}// LinearMovementProjectiveConstraintInternalData::projectResponse + +template +void LinearMovementProjectiveConstraintInternalData< gpu::cuda::CudaVectorTypes >::projectPosition(Main* m, VecCoord& x) +{ + Data& data = *m->data; + + Real cT = (Real) m->getContext()->getTime(); + + // initialize initial Dofs positions, if it's not done + if (data.x0.size() == 0) + { + data.init(m, x); + } + + if ((cT != m->currentTime) || !m->finished) + { + m->findKeyTimes(); + } + + if (m->finished && m->nextT != m->prevT) + { + Real dt = (cT - m->prevT) / (m->nextT - m->prevT); + Deriv depl = m->prevM + (m->nextM-m->prevM)*dt; + + Kernels::projectPosition( + data.size, + data.indices.deviceRead(), + depl.ptr(), + data.x0.deviceRead(), + x.deviceWrite() + ); + } +}// LinearMovementProjectiveConstraintInternalData::projectPosition + +template +void LinearMovementProjectiveConstraintInternalData< gpu::cuda::CudaVectorTypes >::projectVelocity(Main* m, VecDeriv& dx) +{ + Data& data = *m->data; + + Real cT = (Real) m->getContext()->getTime(); + + if ((cT != m->currentTime) || !m->finished) + m->findKeyTimes(); + + if (m->finished && m->nextT != m->prevT) + { + Deriv dv = (m->nextM - m->prevM)*(1.0/(m->nextT - m->prevT)); + + Kernels::projectVelocity( + data.size, + data.indices.deviceRead(), + dv.ptr(), + dx.deviceWrite() + ); + } +}// LinearMovementProjectiveConstraintInternalData::projectVelocity + + +///////////////////////////////////// +// CudaRigidTypes specializations +///////////////////////////////////// + +template +void LinearMovementProjectiveConstraintInternalData< gpu::cuda::CudaRigidTypes >::addIndex(Main* m, Index index) +{ + Data& data = *m->data; + + m->m_indices.beginEdit()->push_back(index); + m->m_indices.endEdit(); + + data.indices.push_back(index); + // TODO : then it becomes non-consistent and also in the main version !!! +// data.x0.push_back(); + +}// LinearMovementProjectiveConstraintInternalData::addIndex + +template +void LinearMovementProjectiveConstraintInternalData< gpu::cuda::CudaRigidTypes >::removeIndex(Main* m, Index index) +{ + // Data& data = m->data; + + removeValue(*m->m_indices.beginEdit(),index); + m->m_indices.endEdit(); + + // removeValue(data.indices, index); + // TODO : then it becomes non-consistent and also in the main version !!! +// data.x0.push_back(); + +}// LinearMovementProjectiveConstraintInternalData::removeIndex + +template +void LinearMovementProjectiveConstraintInternalData< gpu::cuda::CudaRigidTypes >::init(Main* m, VecCoord& x) +{ + Data& data = *m->data; + const SetIndexArray & indices = m->m_indices.getValue(); +// m->x0.resize( indices.size() ); +// for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) +// m->x0[*it] = x[*it]; + + // gpu part + data.size = indices.size(); + data.indices.resize(data.size); + unsigned index =0; + for (typename SetIndex::const_iterator it = indices.begin(); it != indices.end(); it++) + { + data.indices[index] = *it; + index++; + } + + data.x0.resize(data.size); + index = 0; + for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) + { + data.x0[index] = x[*it]; + index++; + } +}// LinearMovementProjectiveConstraintInternalData::init + +template +void LinearMovementProjectiveConstraintInternalData< gpu::cuda::CudaRigidTypes >::projectResponse(Main* m, VecDeriv& dx) +{ + Data& data = *m->data; + + Real cT = (Real) m->getContext()->getTime(); + + if ((cT != m->currentTime) || !m->finished) + { + m->findKeyTimes(); + } + + if (m->finished && m->nextT != m->prevT) + { + Kernels::projectResponse( + data.size, + data.indices.deviceRead(), + dx.deviceWrite() + ); + } +}// LinearMovementProjectiveConstraintInternalData::projectResponse + +template +void LinearMovementProjectiveConstraintInternalData< gpu::cuda::CudaRigidTypes >::projectPosition(Main* m, VecCoord& x) +{ + Data& data = *m->data; + + Real cT = (Real) m->getContext()->getTime(); + + // initialize initial Dofs positions, if it's not done + if (data.x0.size() == 0) + { + data.init(m, x); + } + + if ((cT != m->currentTime) || !m->finished) + { + m->findKeyTimes(); + } + + if (m->finished && m->nextT != m->prevT) + { + Real dt = (cT - m->prevT) / (m->nextT - m->prevT); + Deriv depl = m->prevM + (m->nextM-m->prevM)*dt; + + Kernels::projectPosition( + data.size, + data.indices.deviceRead(), + depl.ptr(), + data.x0.deviceRead(), + x.deviceWrite() + ); + } +}// LinearMovementProjectiveConstraintInternalData::projectPosition + +template +void LinearMovementProjectiveConstraintInternalData< gpu::cuda::CudaRigidTypes >::projectVelocity(Main* m, VecDeriv& dx) +{ + Data& data = *m->data; + + Real cT = (Real) m->getContext()->getTime(); + + if ((cT != m->currentTime) || !m->finished) + m->findKeyTimes(); + + if (m->finished && m->nextT != m->prevT) + { + Deriv dv = (m->nextM - m->prevM)*(1.0/(m->nextT - m->prevT)); + + Kernels::projectVelocity( + data.size, + data.indices.deviceRead(), + dv.ptr(), + dx.deviceWrite() + ); + } +}// LinearMovementProjectiveConstraintInternalData::projectVelocity + + +////////////////////////////// +// Specializations +///////////////////////////// + +// template<> +// void LinearMovementProjectiveConstraint< gpu::cuda::CudaRigid3fTypes >::init() +// { +// data.init(this); +// } + +template<> +void LinearMovementProjectiveConstraint< gpu::cuda::CudaVec6fTypes >::addIndex(Index index) +{ + data->addIndex(this, index); +} + +template<> +void LinearMovementProjectiveConstraint< gpu::cuda::CudaVec6fTypes >::removeIndex(Index index) +{ + data->removeIndex(this, index); +} + +template<> +void LinearMovementProjectiveConstraint< gpu::cuda::CudaVec6fTypes >::projectResponse(const core::MechanicalParams* /* mparams */, DataVecDeriv& dx) +{ + VecDeriv& _dx = *dx.beginEdit(); + data->projectResponse(this, _dx); + dx.endEdit(); +} + +template<> +void LinearMovementProjectiveConstraint< gpu::cuda::CudaVec6fTypes >::projectJacobianMatrix(const core::MechanicalParams* /* mparams */, DataMatrixDeriv& /*dx*/) +{ + /* data.projectResponseT(this, index); */ +} + +template<> +void LinearMovementProjectiveConstraint< gpu::cuda::CudaVec6fTypes >::projectPosition(const core::MechanicalParams* /* mparams */, DataVecCoord& x) +{ + VecCoord& _x = *x.beginEdit(); + data->projectPosition(this, _x); + x.endEdit(); +} + +template<> +void LinearMovementProjectiveConstraint< gpu::cuda::CudaVec6fTypes >::projectVelocity(const core::MechanicalParams* /* mparams */, DataVecDeriv& dx) +{ + VecDeriv& _dx = *dx.beginEdit(); + data->projectVelocity(this, _dx); + dx.endEdit(); +} + +template<> +void LinearMovementProjectiveConstraint< gpu::cuda::CudaRigid3fTypes >::addIndex(Index index) +{ + data->addIndex(this, index); +} + +template<> +void LinearMovementProjectiveConstraint< gpu::cuda::CudaRigid3fTypes >::removeIndex(Index index) +{ + data->removeIndex(this, index); +} + +template<> +void LinearMovementProjectiveConstraint< gpu::cuda::CudaRigid3fTypes >::projectResponse(const core::MechanicalParams* /* mparams */, DataVecDeriv& dx) +{ + VecDeriv& _dx = *dx.beginEdit(); + data->projectResponse(this, _dx); + dx.endEdit(); +} + +template<> +void LinearMovementProjectiveConstraint< gpu::cuda::CudaRigid3fTypes >::projectJacobianMatrix(const core::MechanicalParams* /* mparams */, DataMatrixDeriv& /*dx*/) +{ + /* data.projectResponseT(this, index); */ +} + +template<> +void LinearMovementProjectiveConstraint< gpu::cuda::CudaRigid3fTypes >::projectPosition(const core::MechanicalParams* /* mparams */, DataVecCoord& x) +{ + VecCoord& _x = *x.beginEdit(); + data->projectPosition(this, _x); + x.endEdit(); +} + +template<> +void LinearMovementProjectiveConstraint< gpu::cuda::CudaRigid3fTypes >::projectVelocity(const core::MechanicalParams* /* mparams */, DataVecDeriv& dx) +{ + VecDeriv& _dx = *dx.beginEdit(); + data->projectVelocity(this, _dx); + dx.endEdit(); +} + +#ifdef SOFA_GPU_CUDA_DOUBLE +// template<> +// void LinearMovementProjectiveConstraint< gpu::cuda::CudaRigid3dTypes >::init() +// { +// data->init(this); +// } + +template<> +void LinearMovementProjectiveConstraint< gpu::cuda::CudaVec6dTypes >::addIndex(Index index) +{ + data->addIndex(this, index); +} + +template<> +void LinearMovementProjectiveConstraint< gpu::cuda::CudaVec6dTypes >::removeIndex(Index index) +{ + data->removeIndex(this, index); +} + +template<> +void LinearMovementProjectiveConstraint< gpu::cuda::CudaVec6dTypes >::projectResponse(const core::MechanicalParams* /* mparams */, DataVecDeriv& dx) +{ + VecDeriv& _dx = *dx.beginEdit(); + data->projectResponse(this, _dx); + dx.endEdit(); +} + +template<> +void LinearMovementProjectiveConstraint< gpu::cuda::CudaVec6dTypes >::projectJacobianMatrix(const core::MechanicalParams* /* mparams */, DataMatrixDeriv& /*dx*/) +{ + /* data.projectResponseT(this, index); */ +} + +template<> +void LinearMovementProjectiveConstraint< gpu::cuda::CudaVec6dTypes >::projectPosition(const core::MechanicalParams* /* mparams */, DataVecCoord& x) +{ + VecCoord& _x = *x.beginEdit(); + data->projectPosition(this, _x); + x.endEdit(); +} + +template<> +void LinearMovementProjectiveConstraint< gpu::cuda::CudaVec6dTypes >::projectVelocity(const core::MechanicalParams* /* mparams */, DataVecDeriv& dx) +{ + VecDeriv& _dx = *dx.beginEdit(); + data->projectVelocity(this, _dx); + dx.endEdit(); +} + +template<> +void LinearMovementProjectiveConstraint< gpu::cuda::CudaRigid3dTypes >::addIndex(Index index) +{ + data->addIndex(this, index); +} + +template<> +void LinearMovementProjectiveConstraint< gpu::cuda::CudaRigid3dTypes >::removeIndex(Index index) +{ + data->removeIndex(this, index); +} + +template<> +void LinearMovementProjectiveConstraint< gpu::cuda::CudaRigid3dTypes >::projectResponse(const core::MechanicalParams* /* mparams */, DataVecDeriv& dx) +{ + VecDeriv& _dx = *dx.beginEdit(); + data->projectResponse(this, _dx); + dx.endEdit(); +} + +template<> +void LinearMovementProjectiveConstraint< gpu::cuda::CudaRigid3dTypes >::projectJacobianMatrix(const core::MechanicalParams* /* mparams */, DataMatrixDeriv& /*dx*/) +{ + /* data.projectResponseT(this, index); */ +} + +template<> +void LinearMovementProjectiveConstraint< gpu::cuda::CudaRigid3dTypes >::projectPosition(const core::MechanicalParams* /* mparams */, DataVecCoord& x) +{ + VecCoord& _x = *x.beginEdit(); + data->projectPosition(this, _x); + x.endEdit(); +} + +template<> +void LinearMovementProjectiveConstraint< gpu::cuda::CudaRigid3dTypes >::projectVelocity(const core::MechanicalParams* /* mparams */, DataVecDeriv& dx) +{ + VecDeriv& _dx = *dx.beginEdit(); + data->projectVelocity(this, _dx); + dx.endEdit(); +} + +#endif // SOFA_GPU_CUDA_DOUBLE + +} // namespace sofa::component::constraint::projective diff --git a/applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/projective/CudaLinearVelocityConstraint.cpp b/applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/projective/CudaLinearVelocityProjectiveConstraint.cpp similarity index 75% rename from applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/projective/CudaLinearVelocityConstraint.cpp rename to applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/projective/CudaLinearVelocityProjectiveConstraint.cpp index 57c4996f06d..c720af9db74 100644 --- a/applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/projective/CudaLinearVelocityConstraint.cpp +++ b/applications/plugins/SofaCUDA/src/SofaCUDA/component/constraint/projective/CudaLinearVelocityProjectiveConstraint.cpp @@ -20,7 +20,7 @@ * Contact information: contact@sofa-framework.org * ******************************************************************************/ #include -#include +#include #include @@ -28,11 +28,11 @@ namespace sofa::component::constraint::projective { -template class SOFA_GPU_CUDA_API LinearVelocityConstraint; -template class SOFA_GPU_CUDA_API LinearVelocityConstraint; +template class SOFA_GPU_CUDA_API LinearVelocityProjectiveConstraint; +template class SOFA_GPU_CUDA_API LinearVelocityProjectiveConstraint; #ifdef SOFA_GPU_CUDA_DOUBLE -template class SOFA_GPU_CUDA_API LinearVelocityConstraint; -template class SOFA_GPU_CUDA_API LinearVelocityConstraint; +template class SOFA_GPU_CUDA_API LinearVelocityProjectiveConstraint; +template class SOFA_GPU_CUDA_API LinearVelocityProjectiveConstraint; #endif // SOFA_GPU_CUDA_DOUBLE }// namespace sofa::component::constraint::projective @@ -40,12 +40,12 @@ template class SOFA_GPU_CUDA_API LinearVelocityConstraint >() - .add< sofa::component::constraint::projective::LinearVelocityConstraint >() +int LinearVelocityProjectiveConstraintCudaClass = core::RegisterObject("Supports GPU-side computations using CUDA") + .add< sofa::component::constraint::projective::LinearVelocityProjectiveConstraint >() + .add< sofa::component::constraint::projective::LinearVelocityProjectiveConstraint >() #ifdef SOFA_GPU_CUDA_DOUBLE - .add< sofa::component::constraint::projective::LinearVelocityConstraint >() - .add< sofa::component::constraint::projective::LinearVelocityConstraint >() + .add< sofa::component::constraint::projective::LinearVelocityProjectiveConstraint >() + .add< sofa::component::constraint::projective::LinearVelocityProjectiveConstraint >() #endif // SOFA_GPU_CUDA_DOUBLE ; diff --git a/applications/plugins/SofaCarving/examples/CarvingTool.scn b/applications/plugins/SofaCarving/examples/CarvingTool.scn index 90f4e9fb695..56cebdcab64 100644 --- a/applications/plugins/SofaCarving/examples/CarvingTool.scn +++ b/applications/plugins/SofaCarving/examples/CarvingTool.scn @@ -5,7 +5,7 @@ - + @@ -66,7 +66,7 @@ - diff --git a/applications/plugins/SofaMatrix/examples/ComplianceMatrixExporter.scn b/applications/plugins/SofaMatrix/examples/ComplianceMatrixExporter.scn index 4949de49328..245bc21c6ce 100644 --- a/applications/plugins/SofaMatrix/examples/ComplianceMatrixExporter.scn +++ b/applications/plugins/SofaMatrix/examples/ComplianceMatrixExporter.scn @@ -1,7 +1,7 @@ - + @@ -35,7 +35,7 @@ - + diff --git a/applications/plugins/SofaMatrix/examples/ComplianceMatrixImage.scn b/applications/plugins/SofaMatrix/examples/ComplianceMatrixImage.scn index d443ea93089..b131e803661 100644 --- a/applications/plugins/SofaMatrix/examples/ComplianceMatrixImage.scn +++ b/applications/plugins/SofaMatrix/examples/ComplianceMatrixImage.scn @@ -1,7 +1,7 @@ - + @@ -35,7 +35,7 @@ - + diff --git a/applications/plugins/SofaTest/Elasticity_test.h b/applications/plugins/SofaTest/Elasticity_test.h index fbe0c742828..1dd3d658302 100644 --- a/applications/plugins/SofaTest/Elasticity_test.h +++ b/applications/plugins/SofaTest/Elasticity_test.h @@ -31,7 +31,7 @@ #include #include -#include +#include #include namespace sofa { @@ -41,7 +41,7 @@ template struct PatchTestStruct { simulation::Node::SPtr SquareNode; - typename component::constraint::projective::AffineMovementConstraint::SPtr affineConstraint; + typename component::constraint::projective::AffineMovementProjectiveConstraint::SPtr affineConstraint; typename component::statecontainer::MechanicalObject::SPtr dofs; }; diff --git a/applications/plugins/SofaTest/Elasticity_test.inl b/applications/plugins/SofaTest/Elasticity_test.inl index f32feeac540..3433abc20af 100644 --- a/applications/plugins/SofaTest/Elasticity_test.inl +++ b/applications/plugins/SofaTest/Elasticity_test.inl @@ -36,10 +36,10 @@ // Constraint -#include +#include #include -#include -#include +#include +#include // ForceField #include @@ -98,7 +98,7 @@ Elasticity_test::createRegularGridScene( typedef component::topology::container::grid::RegularGridTopology RegularGridTopology; typedef typename component::engine::select::BoxROI BoxRoi; typedef typename sofa::component::engine::select::PairBoxROI PairBoxRoi; - typedef typename sofa::component::constraint::projective::AffineMovementConstraint AffineMovementConstraint; + typedef typename sofa::component::constraint::projective::AffineMovementProjectiveConstraint AffineMovementProjectiveConstraint; typedef component::linearsolver::iterative::CGLinearSolver CGLinearSolver; // Root node @@ -144,7 +144,7 @@ Elasticity_test::createRegularGridScene( pairBoxRoi->includedBox.setValue(includedBox); //Affine constraint - patchStruct.affineConstraint = modeling::addNew(SquareNode,"affineConstraint"); + patchStruct.affineConstraint = modeling::addNew(SquareNode,"affineConstraint"); modeling::setDataLink(&boxRoi->d_indices,&patchStruct.affineConstraint->m_meshIndices); modeling::setDataLink(&pairBoxRoi->f_indices,& patchStruct.affineConstraint->m_indices); @@ -223,9 +223,9 @@ CylinderTractionStruct Elasticity_test::createCylinderTra typename component::constraint::projective::FixedConstraint::SPtr fc= modeling::addNew >(root); sofa::modeling::setDataLink(&boxRoi1->d_indices,&fc->d_indices); - // FixedPlaneConstraint - typename component::constraint::projective::FixedPlaneConstraint::SPtr fpc= - modeling::addNew >(root); + // FixedPlaneProjectiveConstraint + typename component::constraint::projective::FixedPlaneProjectiveConstraint::SPtr fpc= + modeling::addNew >(root); fpc->d_dmin= -0.01; fpc->d_dmax= 0.01; fpc->d_direction=Coord(0,0,1); @@ -241,9 +241,9 @@ CylinderTractionStruct Elasticity_test::createCylinderTra modeling::addNew >(root); tractionStruct.forceField=tpff; sofa::modeling::setDataLink(&boxRoi2->d_triangleIndices,&tpff->triangleList); - // ProjectToLineConstraint - typename component::constraint::projective::ProjectToLineConstraint::SPtr ptlc= - modeling::addNew >(root); + // LineProjectiveConstraint + typename component::constraint::projective::LineProjectiveConstraint::SPtr ptlc= + modeling::addNew >(root); ptlc->f_direction=Coord(1,0,0); ptlc->f_origin=Coord(0,0,0); sofa::type::vector vArray; diff --git a/applications/plugins/SofaTest/InitPlugin_test.h b/applications/plugins/SofaTest/InitPlugin_test.h index f79c0bdeb81..140827304d8 100644 --- a/applications/plugins/SofaTest/InitPlugin_test.h +++ b/applications/plugins/SofaTest/InitPlugin_test.h @@ -95,7 +95,7 @@ Other solver tests are available in Compliant_test: AssembledSolver_test and Dam -if constrained particle have the expected position. -if unconstrained particle have not changed. -Some projective constraint tests are available in SofaTest_test: ProjectToLineConstraint and ProjectToPlaneConstraint. +Some projective constraint tests are available in SofaTest_test: LineProjectiveConstraint and ProjectToPlaneConstraint. - Engine test: To test engine you set input values and check if the ouput values correspond to the expected ones. The test Engine_test tests if the update method is called only if necessary. To test this a minimal engine TestEngine was created with a counter in its update method. diff --git a/applications/plugins/SofaTest/SofaTest_test/scenes/damping.py b/applications/plugins/SofaTest/SofaTest_test/scenes/damping.py index 3b5bd86d9c8..b5c763922e7 100644 --- a/applications/plugins/SofaTest/SofaTest_test/scenes/damping.py +++ b/applications/plugins/SofaTest/SofaTest_test/scenes/damping.py @@ -120,7 +120,7 @@ def createScene(node): angularNode.createObject('MechanicalObject', template="Rigid", name="dofs", position="0 0 0 0 0 0 1", velocity="0 0 0 "+str(INITIAL_VELOCITY)+" 0 0") angularNode.createObject('UniformMass', filename=rigidFilename) angularNode.createObject('UniformVelocityDampingForceField', dampingCoefficient=DAMPING_COEF) - #angularNode.createObject('PartialFixedConstraint', indices='0', fixedDirections="1 1 1 0 1 1") + #angularNode.createObject('PartialFixedProjectiveConstraint', indices='0', fixedDirections="1 1 1 0 1 1") # translation damping test @@ -130,7 +130,7 @@ def createScene(node): translationNode.createObject('MechanicalObject', template="Rigid", name="dofs", position="0 0 0 0 0 0 1", velocity=str(INITIAL_VELOCITY)+" 0 0 0 0 0") translationNode.createObject('UniformMass', filename=rigidFilename) translationNode.createObject('UniformVelocityDampingForceField', dampingCoefficient=DAMPING_COEF) - #translationNode.createObject('PartialFixedConstraint', indices='0', fixedDirections="0 1 1 1 1 1") + #translationNode.createObject('PartialFixedProjectiveConstraint', indices='0', fixedDirections="0 1 1 1 1 1") return node diff --git a/applications/projects/SceneChecking/src/SceneChecking/SceneCheckUsingAlias.cpp b/applications/projects/SceneChecking/src/SceneChecking/SceneCheckUsingAlias.cpp index dcdde083ad0..12c3208029d 100644 --- a/applications/projects/SceneChecking/src/SceneChecking/SceneCheckUsingAlias.cpp +++ b/applications/projects/SceneChecking/src/SceneChecking/SceneCheckUsingAlias.cpp @@ -91,7 +91,7 @@ void SceneCheckUsingAlias::doPrintSummary() auto searchAlias = uncreatableComponents.find(unique_alias); if( searchAlias != uncreatableComponents.end() ) { - usingAliasesWarning << " This alias will be REMOVED at the SOFA release " << searchAlias->second.getVersion() << ", please update your scenes."; + usingAliasesWarning << " " << searchAlias->second.getMessage(); } if(unique_alias != unique_aliases.back()) usingAliasesWarning << msgendl; diff --git a/examples/Benchmark/Accuracy/TriangleFEMForceField_compare.scn b/examples/Benchmark/Accuracy/TriangleFEMForceField_compare.scn index acff67b6931..9d690bed376 100644 --- a/examples/Benchmark/Accuracy/TriangleFEMForceField_compare.scn +++ b/examples/Benchmark/Accuracy/TriangleFEMForceField_compare.scn @@ -1,7 +1,7 @@ - + @@ -30,7 +30,7 @@ - + @@ -59,7 +59,7 @@ - + @@ -88,7 +88,7 @@ - + diff --git a/examples/Benchmark/Accuracy/cylinder_PhantomSolution.scn b/examples/Benchmark/Accuracy/cylinder_PhantomSolution.scn index c17a4fb2b37..85132630579 100644 --- a/examples/Benchmark/Accuracy/cylinder_PhantomSolution.scn +++ b/examples/Benchmark/Accuracy/cylinder_PhantomSolution.scn @@ -5,7 +5,7 @@ - + @@ -29,7 +29,7 @@ - + diff --git a/examples/Benchmark/Performance/Bar16-fem-implicit-CudaVec3f.pscn b/examples/Benchmark/Performance/Bar16-fem-implicit-CudaVec3f.pscn index 1b2601cdefb..c25c4b47529 100644 --- a/examples/Benchmark/Performance/Bar16-fem-implicit-CudaVec3f.pscn +++ b/examples/Benchmark/Performance/Bar16-fem-implicit-CudaVec3f.pscn @@ -15,7 +15,7 @@ /> --> '."\n"; ?> - + diff --git a/examples/Benchmark/Performance/Bar16-fem-implicit-Vec3d.pscn b/examples/Benchmark/Performance/Bar16-fem-implicit-Vec3d.pscn index d8df5a99135..32580e91d45 100644 --- a/examples/Benchmark/Performance/Bar16-fem-implicit-Vec3d.pscn +++ b/examples/Benchmark/Performance/Bar16-fem-implicit-Vec3d.pscn @@ -2,7 +2,7 @@ - + @@ -27,7 +27,7 @@ /> --> '."\n"; ?> - + diff --git a/examples/Benchmark/Performance/Bar16-fem-implicit-Vec3f.pscn b/examples/Benchmark/Performance/Bar16-fem-implicit-Vec3f.pscn index f911693ab22..70b4cc28ff4 100644 --- a/examples/Benchmark/Performance/Bar16-fem-implicit-Vec3f.pscn +++ b/examples/Benchmark/Performance/Bar16-fem-implicit-Vec3f.pscn @@ -15,7 +15,7 @@ /> --> '."\n"; ?> - + diff --git a/examples/Benchmark/Performance/Bar16-spring-rk4-CudaVec3f.pscn b/examples/Benchmark/Performance/Bar16-spring-rk4-CudaVec3f.pscn index 08e39fab926..c1f3e1d33d5 100644 --- a/examples/Benchmark/Performance/Bar16-spring-rk4-CudaVec3f.pscn +++ b/examples/Benchmark/Performance/Bar16-spring-rk4-CudaVec3f.pscn @@ -15,7 +15,7 @@ /> --> '."\n"; ?> - + diff --git a/examples/Benchmark/Performance/Bar16-spring-rk4-Vec3d.pscn b/examples/Benchmark/Performance/Bar16-spring-rk4-Vec3d.pscn index 8529cfce5cc..0ed5c7c7531 100644 --- a/examples/Benchmark/Performance/Bar16-spring-rk4-Vec3d.pscn +++ b/examples/Benchmark/Performance/Bar16-spring-rk4-Vec3d.pscn @@ -2,7 +2,7 @@ - + @@ -27,7 +27,7 @@ /> --> '."\n"; ?> - + diff --git a/examples/Benchmark/Performance/Bar16-spring-rk4-Vec3f.pscn b/examples/Benchmark/Performance/Bar16-spring-rk4-Vec3f.pscn index e1f658a0562..b467e0639b5 100644 --- a/examples/Benchmark/Performance/Bar16-spring-rk4-Vec3f.pscn +++ b/examples/Benchmark/Performance/Bar16-spring-rk4-Vec3f.pscn @@ -15,7 +15,7 @@ /> --> '."\n"; ?> - + diff --git a/examples/Benchmark/Performance/MatrixAssembly/MatrixAssembly_assembledCG.scn b/examples/Benchmark/Performance/MatrixAssembly/MatrixAssembly_assembledCG.scn index ccd8c09cf70..41bab20a46f 100644 --- a/examples/Benchmark/Performance/MatrixAssembly/MatrixAssembly_assembledCG.scn +++ b/examples/Benchmark/Performance/MatrixAssembly/MatrixAssembly_assembledCG.scn @@ -10,7 +10,7 @@ The differences are in the way the global system matrix is built and solved: --> - + @@ -39,7 +39,7 @@ The differences are in the way the global system matrix is built and solved: - + diff --git a/examples/Benchmark/Performance/MatrixAssembly/MatrixAssembly_assembledCG_blocs.scn b/examples/Benchmark/Performance/MatrixAssembly/MatrixAssembly_assembledCG_blocs.scn index d82a67aabbf..01f2c6d497c 100644 --- a/examples/Benchmark/Performance/MatrixAssembly/MatrixAssembly_assembledCG_blocs.scn +++ b/examples/Benchmark/Performance/MatrixAssembly/MatrixAssembly_assembledCG_blocs.scn @@ -10,7 +10,7 @@ The differences are in the way the global system matrix is built and solved: --> - + @@ -38,7 +38,7 @@ The differences are in the way the global system matrix is built and solved: - + diff --git a/examples/Benchmark/Performance/MatrixAssembly/MatrixAssembly_direct.scn b/examples/Benchmark/Performance/MatrixAssembly/MatrixAssembly_direct.scn index 02276f5d64c..e9d27faa57b 100644 --- a/examples/Benchmark/Performance/MatrixAssembly/MatrixAssembly_direct.scn +++ b/examples/Benchmark/Performance/MatrixAssembly/MatrixAssembly_direct.scn @@ -10,7 +10,7 @@ The differences are in the way the global system matrix is built and solved: --> - + @@ -40,7 +40,7 @@ The differences are in the way the global system matrix is built and solved: - + diff --git a/examples/Benchmark/Performance/MatrixAssembly/MatrixAssembly_direct_blocs.scn b/examples/Benchmark/Performance/MatrixAssembly/MatrixAssembly_direct_blocs.scn index 98675483b53..3a1e7a22ab2 100644 --- a/examples/Benchmark/Performance/MatrixAssembly/MatrixAssembly_direct_blocs.scn +++ b/examples/Benchmark/Performance/MatrixAssembly/MatrixAssembly_direct_blocs.scn @@ -10,7 +10,7 @@ The differences are in the way the global system matrix is built and solved: --> - + @@ -39,7 +39,7 @@ The differences are in the way the global system matrix is built and solved: - + diff --git a/examples/Benchmark/Performance/MatrixAssembly/MatrixAssembly_matrixfreeCG.scn b/examples/Benchmark/Performance/MatrixAssembly/MatrixAssembly_matrixfreeCG.scn index 3d13ce2db0d..e5f0867fd30 100644 --- a/examples/Benchmark/Performance/MatrixAssembly/MatrixAssembly_matrixfreeCG.scn +++ b/examples/Benchmark/Performance/MatrixAssembly/MatrixAssembly_matrixfreeCG.scn @@ -10,7 +10,7 @@ The differences are in the way the global system matrix is built and solved: --> - + @@ -40,7 +40,7 @@ The differences are in the way the global system matrix is built and solved: - + diff --git a/examples/Benchmark/TopologicalChanges/AffineMovementConstraint__RemovingMeshTest.scn b/examples/Benchmark/TopologicalChanges/AffineMovementConstraint__RemovingMeshTest.scn index 483706da240..281c96ff109 100644 --- a/examples/Benchmark/TopologicalChanges/AffineMovementConstraint__RemovingMeshTest.scn +++ b/examples/Benchmark/TopologicalChanges/AffineMovementConstraint__RemovingMeshTest.scn @@ -4,7 +4,7 @@ - + @@ -30,7 +30,7 @@ - + diff --git a/examples/Benchmark/TopologicalChanges/FixedConstraint_RemovingMeshTest.scn b/examples/Benchmark/TopologicalChanges/FixedConstraint_RemovingMeshTest.scn index c0406f6cca1..84fa89acc83 100644 --- a/examples/Benchmark/TopologicalChanges/FixedConstraint_RemovingMeshTest.scn +++ b/examples/Benchmark/TopologicalChanges/FixedConstraint_RemovingMeshTest.scn @@ -4,7 +4,7 @@ - + @@ -30,7 +30,7 @@ - + diff --git a/examples/Benchmark/TopologicalChanges/FixedPlaneConstraint_RemovingMeshTest.scn b/examples/Benchmark/TopologicalChanges/FixedPlaneConstraint_RemovingMeshTest.scn index 411c3c7bb3f..66d04efb6cc 100644 --- a/examples/Benchmark/TopologicalChanges/FixedPlaneConstraint_RemovingMeshTest.scn +++ b/examples/Benchmark/TopologicalChanges/FixedPlaneConstraint_RemovingMeshTest.scn @@ -4,7 +4,7 @@ - + @@ -30,7 +30,7 @@ - + diff --git a/examples/Benchmark/TopologicalChanges/LinearMovementConstraint_RemovingMeshTest.scn b/examples/Benchmark/TopologicalChanges/LinearMovementConstraint_RemovingMeshTest.scn index 9f30bb75e82..662cb14f052 100644 --- a/examples/Benchmark/TopologicalChanges/LinearMovementConstraint_RemovingMeshTest.scn +++ b/examples/Benchmark/TopologicalChanges/LinearMovementConstraint_RemovingMeshTest.scn @@ -4,7 +4,7 @@ - + @@ -30,7 +30,7 @@ - - + @@ -35,7 +35,7 @@ - + diff --git a/examples/Benchmark/TopologicalChanges/ProjectToLineConstraint_RemovingMeshTest.scn b/examples/Benchmark/TopologicalChanges/ProjectToLineConstraint_RemovingMeshTest.scn index 6a59053485f..82a7db960a9 100644 --- a/examples/Benchmark/TopologicalChanges/ProjectToLineConstraint_RemovingMeshTest.scn +++ b/examples/Benchmark/TopologicalChanges/ProjectToLineConstraint_RemovingMeshTest.scn @@ -4,7 +4,7 @@ - + @@ -30,7 +30,7 @@ - + diff --git a/examples/Benchmark/TopologicalChanges/ProjectToPlaneConstraint_RemovingMeshTest.scn b/examples/Benchmark/TopologicalChanges/ProjectToPlaneConstraint_RemovingMeshTest.scn index d4d097c6af2..2171a27bc6b 100644 --- a/examples/Benchmark/TopologicalChanges/ProjectToPlaneConstraint_RemovingMeshTest.scn +++ b/examples/Benchmark/TopologicalChanges/ProjectToPlaneConstraint_RemovingMeshTest.scn @@ -4,7 +4,7 @@ - + @@ -30,7 +30,7 @@ - + diff --git a/examples/Benchmark/TopologicalChanges/ProjectToPointConstraint_RemovingMeshTest.scn b/examples/Benchmark/TopologicalChanges/ProjectToPointConstraint_RemovingMeshTest.scn index f25820f316b..0fc9b098e3b 100644 --- a/examples/Benchmark/TopologicalChanges/ProjectToPointConstraint_RemovingMeshTest.scn +++ b/examples/Benchmark/TopologicalChanges/ProjectToPointConstraint_RemovingMeshTest.scn @@ -4,7 +4,7 @@ - + @@ -30,7 +30,7 @@ - + diff --git a/examples/Component/Constraint/Lagrangian/BilateralInteractionConstraint_NNCG.scn b/examples/Component/Constraint/Lagrangian/BilateralInteractionConstraint_NNCG.scn index 1ba14adfb2e..7ca3e60cdda 100644 --- a/examples/Component/Constraint/Lagrangian/BilateralInteractionConstraint_NNCG.scn +++ b/examples/Component/Constraint/Lagrangian/BilateralInteractionConstraint_NNCG.scn @@ -7,7 +7,7 @@ - + @@ -72,7 +72,7 @@ - + @@ -98,7 +98,7 @@ - + @@ -124,7 +124,7 @@ - + @@ -150,5 +150,5 @@ - + diff --git a/examples/Component/Constraint/Lagrangian/BilateralInteractionConstraint_PGS.scn b/examples/Component/Constraint/Lagrangian/BilateralInteractionConstraint_PGS.scn index 043835a1768..5553e710327 100644 --- a/examples/Component/Constraint/Lagrangian/BilateralInteractionConstraint_PGS.scn +++ b/examples/Component/Constraint/Lagrangian/BilateralInteractionConstraint_PGS.scn @@ -1,5 +1,5 @@ - + @@ -7,7 +7,7 @@ - + @@ -72,7 +72,7 @@ - + @@ -98,7 +98,7 @@ - + @@ -124,7 +124,7 @@ - + @@ -150,5 +150,5 @@ - + diff --git a/examples/Component/Constraint/Lagrangian/BilateralInteractionConstraint_Rigid.scn b/examples/Component/Constraint/Lagrangian/BilateralInteractionConstraint_Rigid.scn index 0a387c66ad4..4d6cd05e366 100644 --- a/examples/Component/Constraint/Lagrangian/BilateralInteractionConstraint_Rigid.scn +++ b/examples/Component/Constraint/Lagrangian/BilateralInteractionConstraint_Rigid.scn @@ -1,10 +1,10 @@ - + - + @@ -44,5 +44,5 @@ - + diff --git a/examples/Component/Constraint/Lagrangian/BilateralInteractionConstraint_UGS.scn b/examples/Component/Constraint/Lagrangian/BilateralInteractionConstraint_UGS.scn index 80c1002e8e1..b65ea20ca98 100644 --- a/examples/Component/Constraint/Lagrangian/BilateralInteractionConstraint_UGS.scn +++ b/examples/Component/Constraint/Lagrangian/BilateralInteractionConstraint_UGS.scn @@ -1,5 +1,5 @@ - + @@ -7,7 +7,7 @@ - + @@ -72,7 +72,7 @@ - + @@ -98,7 +98,7 @@ - + @@ -124,7 +124,7 @@ - + @@ -150,5 +150,5 @@ - + diff --git a/examples/Component/Constraint/Lagrangian/InextensiblePendulum.scn b/examples/Component/Constraint/Lagrangian/InextensiblePendulum.scn index ab6bdf1cb57..52d7979148e 100644 --- a/examples/Component/Constraint/Lagrangian/InextensiblePendulum.scn +++ b/examples/Component/Constraint/Lagrangian/InextensiblePendulum.scn @@ -5,7 +5,7 @@ - + @@ -48,7 +48,7 @@ - + diff --git a/examples/Component/Constraint/Lagrangian/SlidingConstraint.scn b/examples/Component/Constraint/Lagrangian/SlidingConstraint.scn index 615a001036d..0adb51d70a3 100644 --- a/examples/Component/Constraint/Lagrangian/SlidingConstraint.scn +++ b/examples/Component/Constraint/Lagrangian/SlidingConstraint.scn @@ -6,7 +6,7 @@ - + @@ -53,7 +53,7 @@ - + @@ -81,5 +81,5 @@ - + diff --git a/examples/Component/Constraint/Projective/AffineMovementConstraint.scn b/examples/Component/Constraint/Projective/AffineMovementConstraint.scn index 5e87538455e..cf9dee066e4 100644 --- a/examples/Component/Constraint/Projective/AffineMovementConstraint.scn +++ b/examples/Component/Constraint/Projective/AffineMovementConstraint.scn @@ -1,6 +1,6 @@ - + @@ -21,6 +21,6 @@ - + diff --git a/examples/Component/Constraint/Projective/AffineMovementConstraint3D.scn b/examples/Component/Constraint/Projective/AffineMovementConstraint3D.scn index 6fe3cfe9ee2..9559ae20198 100644 --- a/examples/Component/Constraint/Projective/AffineMovementConstraint3D.scn +++ b/examples/Component/Constraint/Projective/AffineMovementConstraint3D.scn @@ -1,6 +1,6 @@ - + @@ -21,7 +21,7 @@ - + diff --git a/examples/Component/Constraint/Projective/AttachConstraint.scn b/examples/Component/Constraint/Projective/AttachConstraint.scn index e419879213c..222d1833721 100644 --- a/examples/Component/Constraint/Projective/AttachConstraint.scn +++ b/examples/Component/Constraint/Projective/AttachConstraint.scn @@ -1,5 +1,5 @@ - + @@ -50,8 +50,8 @@ - - + + @@ -80,7 +80,7 @@ - - + + diff --git a/examples/Component/Constraint/Projective/AttachConstraintMatrix.scn b/examples/Component/Constraint/Projective/AttachConstraintMatrix.scn index ec7ae3cf09a..e6b835075a0 100644 --- a/examples/Component/Constraint/Projective/AttachConstraintMatrix.scn +++ b/examples/Component/Constraint/Projective/AttachConstraintMatrix.scn @@ -1,5 +1,5 @@ - + @@ -37,8 +37,8 @@ --> - - + + @@ -66,7 +66,7 @@ --> - - + + diff --git a/examples/Component/Constraint/Projective/BilinearConstraint.scn b/examples/Component/Constraint/Projective/BilinearConstraint.scn index 3886aca51c9..86cf8836f0d 100644 --- a/examples/Component/Constraint/Projective/BilinearConstraint.scn +++ b/examples/Component/Constraint/Projective/BilinearConstraint.scn @@ -1,6 +1,6 @@ - + @@ -21,7 +21,7 @@ - diff --git a/examples/Component/Constraint/Projective/BilinearConstraint3D.scn b/examples/Component/Constraint/Projective/BilinearConstraint3D.scn index ba7c8b1a8c6..790ebfd1aa1 100644 --- a/examples/Component/Constraint/Projective/BilinearConstraint3D.scn +++ b/examples/Component/Constraint/Projective/BilinearConstraint3D.scn @@ -1,6 +1,6 @@ - + @@ -21,7 +21,7 @@ - - + @@ -31,7 +31,7 @@ - + diff --git a/examples/Component/Constraint/Projective/FixedRotationConstraint.scn b/examples/Component/Constraint/Projective/FixedRotationConstraint.scn index 9a1d330bced..bf7d63aa34c 100644 --- a/examples/Component/Constraint/Projective/FixedRotationConstraint.scn +++ b/examples/Component/Constraint/Projective/FixedRotationConstraint.scn @@ -2,7 +2,7 @@ - + @@ -24,7 +24,7 @@ - + diff --git a/examples/Component/Constraint/Projective/HermiteSplineConstraint.scn b/examples/Component/Constraint/Projective/HermiteSplineConstraint.scn index 0e9cebd5f00..94c103f3313 100644 --- a/examples/Component/Constraint/Projective/HermiteSplineConstraint.scn +++ b/examples/Component/Constraint/Projective/HermiteSplineConstraint.scn @@ -1,5 +1,5 @@ - + @@ -41,7 +41,7 @@ - + @@ -72,7 +72,7 @@ - + diff --git a/examples/Component/Constraint/Projective/LinearMovementConstraint.scn b/examples/Component/Constraint/Projective/LinearMovementConstraint.scn index 765e24d0691..a0c44039f02 100644 --- a/examples/Component/Constraint/Projective/LinearMovementConstraint.scn +++ b/examples/Component/Constraint/Projective/LinearMovementConstraint.scn @@ -1,7 +1,7 @@ - + @@ -15,7 +15,7 @@ - - - - + @@ -35,7 +35,7 @@ - + diff --git a/examples/Component/Constraint/Projective/OscillatorConstraint_rigid.scn b/examples/Component/Constraint/Projective/OscillatorConstraint_rigid.scn index 29e63ffcf16..ebf9b35dcdd 100644 --- a/examples/Component/Constraint/Projective/OscillatorConstraint_rigid.scn +++ b/examples/Component/Constraint/Projective/OscillatorConstraint_rigid.scn @@ -1,5 +1,5 @@ - + @@ -11,13 +11,13 @@ - + - + diff --git a/examples/Component/Constraint/Projective/ParabolicConstraint.scn b/examples/Component/Constraint/Projective/ParabolicConstraint.scn index 9ad9caf4787..3ca3981defe 100644 --- a/examples/Component/Constraint/Projective/ParabolicConstraint.scn +++ b/examples/Component/Constraint/Projective/ParabolicConstraint.scn @@ -3,7 +3,7 @@ - + @@ -26,7 +26,7 @@ - + diff --git a/examples/Component/Constraint/Projective/PartialFixedConstraint.scn b/examples/Component/Constraint/Projective/PartialFixedConstraint.scn index 9e235784ff4..2d58937d7d5 100644 --- a/examples/Component/Constraint/Projective/PartialFixedConstraint.scn +++ b/examples/Component/Constraint/Projective/PartialFixedConstraint.scn @@ -3,7 +3,7 @@ - + @@ -30,7 +30,7 @@ - + diff --git a/examples/Component/Constraint/Projective/PatchTestConstraint.scn b/examples/Component/Constraint/Projective/PatchTestConstraint.scn index 4c1b2ad4c17..14a0cde03f6 100644 --- a/examples/Component/Constraint/Projective/PatchTestConstraint.scn +++ b/examples/Component/Constraint/Projective/PatchTestConstraint.scn @@ -1,6 +1,6 @@ - + @@ -21,7 +21,7 @@ - - + @@ -28,7 +28,7 @@ - + diff --git a/examples/Component/Constraint/Projective/ProjectToLineConstraint.scn b/examples/Component/Constraint/Projective/ProjectToLineConstraint.scn index ecfa2892dd8..6b7fecb9a70 100644 --- a/examples/Component/Constraint/Projective/ProjectToLineConstraint.scn +++ b/examples/Component/Constraint/Projective/ProjectToLineConstraint.scn @@ -1,6 +1,6 @@ - + @@ -28,7 +28,7 @@ - + diff --git a/examples/Component/Constraint/Projective/ProjectToPlaneConstraint.scn b/examples/Component/Constraint/Projective/ProjectToPlaneConstraint.scn index 8c46fd43d58..c9c99fc9716 100644 --- a/examples/Component/Constraint/Projective/ProjectToPlaneConstraint.scn +++ b/examples/Component/Constraint/Projective/ProjectToPlaneConstraint.scn @@ -1,6 +1,6 @@ - + @@ -34,7 +34,7 @@ - + diff --git a/examples/Component/Constraint/Projective/ProjectToPointConstraint.scn b/examples/Component/Constraint/Projective/ProjectToPointConstraint.scn index ebef7961b34..d6289699af8 100644 --- a/examples/Component/Constraint/Projective/ProjectToPointConstraint.scn +++ b/examples/Component/Constraint/Projective/ProjectToPointConstraint.scn @@ -1,6 +1,6 @@ - + @@ -28,7 +28,7 @@ - + diff --git a/examples/Component/Diffusion/TetrahedronDiffusionFEMForceField.scn b/examples/Component/Diffusion/TetrahedronDiffusionFEMForceField.scn index b3b91f47d5a..6d8f39a209e 100644 --- a/examples/Component/Diffusion/TetrahedronDiffusionFEMForceField.scn +++ b/examples/Component/Diffusion/TetrahedronDiffusionFEMForceField.scn @@ -1,5 +1,5 @@ - + @@ -35,8 +35,8 @@ - - + + diff --git a/examples/Component/Engine/Analyze/ShapeMatching.scn b/examples/Component/Engine/Analyze/ShapeMatching.scn index 963419e8bb6..79d6cc58032 100644 --- a/examples/Component/Engine/Analyze/ShapeMatching.scn +++ b/examples/Component/Engine/Analyze/ShapeMatching.scn @@ -4,7 +4,7 @@ - + @@ -33,7 +33,7 @@ - + diff --git a/examples/Component/Engine/Generate/GenerateCylinder.scn b/examples/Component/Engine/Generate/GenerateCylinder.scn index d2eefb6e655..0811e77902f 100644 --- a/examples/Component/Engine/Generate/GenerateCylinder.scn +++ b/examples/Component/Engine/Generate/GenerateCylinder.scn @@ -1,6 +1,6 @@ - + @@ -22,9 +22,9 @@ - + - + diff --git a/examples/Component/Engine/Generate/GenerateGrid.scn b/examples/Component/Engine/Generate/GenerateGrid.scn index 5e3a62f1069..716d48c1cd6 100644 --- a/examples/Component/Engine/Generate/GenerateGrid.scn +++ b/examples/Component/Engine/Generate/GenerateGrid.scn @@ -1,6 +1,6 @@ - + @@ -22,9 +22,9 @@ - + - + @@ -37,9 +37,9 @@ - + - + diff --git a/examples/Component/Engine/Select/BoxROI.scn b/examples/Component/Engine/Select/BoxROI.scn index 446af45803e..5faea6b844d 100644 --- a/examples/Component/Engine/Select/BoxROI.scn +++ b/examples/Component/Engine/Select/BoxROI.scn @@ -4,7 +4,7 @@ - + diff --git a/examples/Component/Engine/Select/MeshSplittingEngine.scn b/examples/Component/Engine/Select/MeshSplittingEngine.scn index 8016eba6c8d..48e86e8323c 100644 --- a/examples/Component/Engine/Select/MeshSplittingEngine.scn +++ b/examples/Component/Engine/Select/MeshSplittingEngine.scn @@ -2,7 +2,7 @@ - + diff --git a/examples/Component/Engine/Select/NearestPointROI.scn b/examples/Component/Engine/Select/NearestPointROI.scn index d0db2545113..6eef96cbb89 100644 --- a/examples/Component/Engine/Select/NearestPointROI.scn +++ b/examples/Component/Engine/Select/NearestPointROI.scn @@ -2,9 +2,9 @@ - + - + @@ -34,7 +34,7 @@ - + @@ -50,7 +50,7 @@ - + @@ -58,8 +58,8 @@ - - + + - + diff --git a/examples/Component/Engine/Select/SphereROI.scn b/examples/Component/Engine/Select/SphereROI.scn index 6b12d91a9f5..41f7d5eafa5 100644 --- a/examples/Component/Engine/Select/SphereROI.scn +++ b/examples/Component/Engine/Select/SphereROI.scn @@ -3,7 +3,7 @@ - + diff --git a/examples/Component/Engine/Select/SubsetTopology.scn b/examples/Component/Engine/Select/SubsetTopology.scn index 21e3ebf839d..5368d557aef 100644 --- a/examples/Component/Engine/Select/SubsetTopology.scn +++ b/examples/Component/Engine/Select/SubsetTopology.scn @@ -3,7 +3,7 @@ - + diff --git a/examples/Component/Engine/Select/SubsetTopology_withtetrahedra.scn b/examples/Component/Engine/Select/SubsetTopology_withtetrahedra.scn index c2ad7fcabc4..e89f7fcc39f 100644 --- a/examples/Component/Engine/Select/SubsetTopology_withtetrahedra.scn +++ b/examples/Component/Engine/Select/SubsetTopology_withtetrahedra.scn @@ -3,7 +3,7 @@ - + @@ -33,7 +33,7 @@ - + diff --git a/examples/Component/Engine/Transform/DisplacementMatrixEngine.scn b/examples/Component/Engine/Transform/DisplacementMatrixEngine.scn index 3ad1146f2b3..bda26fff218 100644 --- a/examples/Component/Engine/Transform/DisplacementMatrixEngine.scn +++ b/examples/Component/Engine/Transform/DisplacementMatrixEngine.scn @@ -1,6 +1,6 @@ - + @@ -13,7 +13,7 @@ - + diff --git a/examples/Component/IO/Mesh/GIDMeshLoader.scn b/examples/Component/IO/Mesh/GIDMeshLoader.scn index 67d832f36aa..30cc187f067 100644 --- a/examples/Component/IO/Mesh/GIDMeshLoader.scn +++ b/examples/Component/IO/Mesh/GIDMeshLoader.scn @@ -1,6 +1,6 @@ - + @@ -21,7 +21,7 @@ - + diff --git a/examples/Component/IO/Mesh/GridMeshCreator.scn b/examples/Component/IO/Mesh/GridMeshCreator.scn index 57ad5cb3d65..f7fc089de77 100644 --- a/examples/Component/IO/Mesh/GridMeshCreator.scn +++ b/examples/Component/IO/Mesh/GridMeshCreator.scn @@ -1,6 +1,6 @@ - + diff --git a/examples/Component/IO/Mesh/LiverUseNewLoaders.scn b/examples/Component/IO/Mesh/LiverUseNewLoaders.scn index feb56b80452..33fc1206e06 100644 --- a/examples/Component/IO/Mesh/LiverUseNewLoaders.scn +++ b/examples/Component/IO/Mesh/LiverUseNewLoaders.scn @@ -4,7 +4,7 @@ - + @@ -33,7 +33,7 @@ - + diff --git a/examples/Component/IO/Mesh/MeshGmshLoader.scn b/examples/Component/IO/Mesh/MeshGmshLoader.scn index 0acff14cbf5..4158c668ac3 100644 --- a/examples/Component/IO/Mesh/MeshGmshLoader.scn +++ b/examples/Component/IO/Mesh/MeshGmshLoader.scn @@ -5,7 +5,7 @@ - + @@ -34,7 +34,7 @@ - + @@ -52,7 +52,7 @@ - + diff --git a/examples/Component/Mapping/NonLinear/RigidMapping-basic.scn b/examples/Component/Mapping/NonLinear/RigidMapping-basic.scn index 7744bd8aec9..1288a343fcd 100644 --- a/examples/Component/Mapping/NonLinear/RigidMapping-basic.scn +++ b/examples/Component/Mapping/NonLinear/RigidMapping-basic.scn @@ -1,6 +1,6 @@ - + @@ -15,7 +15,7 @@ - + diff --git a/examples/Component/Mapping/NonLinear/RigidMapping2d-basic.scn b/examples/Component/Mapping/NonLinear/RigidMapping2d-basic.scn index 5f19f6a606d..f80e9ca2cc0 100644 --- a/examples/Component/Mapping/NonLinear/RigidMapping2d-basic.scn +++ b/examples/Component/Mapping/NonLinear/RigidMapping2d-basic.scn @@ -1,6 +1,6 @@ - + @@ -16,7 +16,7 @@ - + diff --git a/examples/Component/Mapping/NonLinear/RigidRigidMapping-basic.scn b/examples/Component/Mapping/NonLinear/RigidRigidMapping-basic.scn index 6027d7de902..cc1d920970c 100644 --- a/examples/Component/Mapping/NonLinear/RigidRigidMapping-basic.scn +++ b/examples/Component/Mapping/NonLinear/RigidRigidMapping-basic.scn @@ -1,6 +1,6 @@ - + @@ -14,7 +14,7 @@ - + diff --git a/examples/Component/Mass/DiagonalMass.scn b/examples/Component/Mass/DiagonalMass.scn index ac24b41552c..f83e61ebcb5 100644 --- a/examples/Component/Mass/DiagonalMass.scn +++ b/examples/Component/Mass/DiagonalMass.scn @@ -4,7 +4,7 @@ - + @@ -34,7 +34,7 @@ - + diff --git a/examples/Component/Mass/MeshMatrixMass.scn b/examples/Component/Mass/MeshMatrixMass.scn index b8abec1d314..a837db2e000 100644 --- a/examples/Component/Mass/MeshMatrixMass.scn +++ b/examples/Component/Mass/MeshMatrixMass.scn @@ -4,7 +4,7 @@ - + @@ -36,7 +36,7 @@ - + diff --git a/examples/Component/Mass/UniformMass.scn b/examples/Component/Mass/UniformMass.scn index c85e272e18a..7de3a469e35 100644 --- a/examples/Component/Mass/UniformMass.scn +++ b/examples/Component/Mass/UniformMass.scn @@ -4,7 +4,7 @@ - + @@ -34,7 +34,7 @@ - + diff --git a/examples/Component/MechanicalLoad/InteractionEllipsoidForceField.scn b/examples/Component/MechanicalLoad/InteractionEllipsoidForceField.scn index 697072de215..6f689334ae7 100644 --- a/examples/Component/MechanicalLoad/InteractionEllipsoidForceField.scn +++ b/examples/Component/MechanicalLoad/InteractionEllipsoidForceField.scn @@ -3,7 +3,7 @@ - + @@ -28,7 +28,7 @@ - + diff --git a/examples/Component/MechanicalLoad/TrianglePressureForceField.scn b/examples/Component/MechanicalLoad/TrianglePressureForceField.scn index 80d90907be9..28efd74f9b2 100644 --- a/examples/Component/MechanicalLoad/TrianglePressureForceField.scn +++ b/examples/Component/MechanicalLoad/TrianglePressureForceField.scn @@ -4,7 +4,7 @@ - + @@ -32,7 +32,7 @@ - + diff --git a/examples/Component/SolidMechanics/FEM/TopoMap_cylinder3d.scn b/examples/Component/SolidMechanics/FEM/TopoMap_cylinder3d.scn index 334b284bed6..5885d3e06be 100644 --- a/examples/Component/SolidMechanics/FEM/TopoMap_cylinder3d.scn +++ b/examples/Component/SolidMechanics/FEM/TopoMap_cylinder3d.scn @@ -4,7 +4,7 @@ - + @@ -34,7 +34,7 @@ - + diff --git a/examples/Component/SolidMechanics/Spring/angularSpringForceField.scn b/examples/Component/SolidMechanics/Spring/angularSpringForceField.scn index 2054f6972a8..6befcf8641f 100644 --- a/examples/Component/SolidMechanics/Spring/angularSpringForceField.scn +++ b/examples/Component/SolidMechanics/Spring/angularSpringForceField.scn @@ -2,7 +2,7 @@ - + @@ -21,7 +21,7 @@ - + - + @@ -26,7 +26,7 @@ - + diff --git a/examples/Component/Topology/Container/Dynamic/RemovingBilateralInteractionConstraint.scn b/examples/Component/Topology/Container/Dynamic/RemovingBilateralInteractionConstraint.scn index 99379c74861..ba9d315d81f 100644 --- a/examples/Component/Topology/Container/Dynamic/RemovingBilateralInteractionConstraint.scn +++ b/examples/Component/Topology/Container/Dynamic/RemovingBilateralInteractionConstraint.scn @@ -3,7 +3,7 @@ - + @@ -69,7 +69,7 @@ - diff --git a/examples/Component/Topology/Container/Dynamic/RemovingTetra2TriangleProcess_performanceTest.scn b/examples/Component/Topology/Container/Dynamic/RemovingTetra2TriangleProcess_performanceTest.scn index a2f7b98388e..827911681f4 100644 --- a/examples/Component/Topology/Container/Dynamic/RemovingTetra2TriangleProcess_performanceTest.scn +++ b/examples/Component/Topology/Container/Dynamic/RemovingTetra2TriangleProcess_performanceTest.scn @@ -4,7 +4,7 @@ - + @@ -38,7 +38,7 @@ - + diff --git a/examples/Component/Topology/Container/Dynamic/TetrahedronForceFieldTopologyChangeHandling.scn b/examples/Component/Topology/Container/Dynamic/TetrahedronForceFieldTopologyChangeHandling.scn index 5964665426c..f39abb2049c 100644 --- a/examples/Component/Topology/Container/Dynamic/TetrahedronForceFieldTopologyChangeHandling.scn +++ b/examples/Component/Topology/Container/Dynamic/TetrahedronForceFieldTopologyChangeHandling.scn @@ -3,7 +3,7 @@ - + @@ -31,7 +31,7 @@ - + @@ -48,7 +48,7 @@ - + diff --git a/examples/Component/Topology/Mapping/Tetra2TriangleTopologicalMapping.scn b/examples/Component/Topology/Mapping/Tetra2TriangleTopologicalMapping.scn index a3c10c569b0..6b56e0beacb 100644 --- a/examples/Component/Topology/Mapping/Tetra2TriangleTopologicalMapping.scn +++ b/examples/Component/Topology/Mapping/Tetra2TriangleTopologicalMapping.scn @@ -4,7 +4,7 @@ - + @@ -33,7 +33,7 @@ - + diff --git a/examples/Component/Topology/Mapping/Tetra2TriangleTopologicalMapping_NoInitialTriangle_option.scn b/examples/Component/Topology/Mapping/Tetra2TriangleTopologicalMapping_NoInitialTriangle_option.scn index 22a8eea5978..ed957f9a363 100644 --- a/examples/Component/Topology/Mapping/Tetra2TriangleTopologicalMapping_NoInitialTriangle_option.scn +++ b/examples/Component/Topology/Mapping/Tetra2TriangleTopologicalMapping_NoInitialTriangle_option.scn @@ -4,7 +4,7 @@ - + @@ -32,7 +32,7 @@ - + diff --git a/examples/Component/Topology/Mapping/Tetra2TriangleTopologicalMapping_with_TetrahedronModel.scn b/examples/Component/Topology/Mapping/Tetra2TriangleTopologicalMapping_with_TetrahedronModel.scn index 6a54be5dd80..91f8769f1c6 100644 --- a/examples/Component/Topology/Mapping/Tetra2TriangleTopologicalMapping_with_TetrahedronModel.scn +++ b/examples/Component/Topology/Mapping/Tetra2TriangleTopologicalMapping_with_TetrahedronModel.scn @@ -3,7 +3,7 @@ - + @@ -34,7 +34,7 @@ - + diff --git a/examples/Component/Visual/LinearBlendSkinningGPU.scn b/examples/Component/Visual/LinearBlendSkinningGPU.scn index 8b2f2a3a17b..78f55310d59 100644 --- a/examples/Component/Visual/LinearBlendSkinningGPU.scn +++ b/examples/Component/Visual/LinearBlendSkinningGPU.scn @@ -1,6 +1,6 @@ - + @@ -13,7 +13,7 @@ - + diff --git a/examples/Demos/TriangleSurfaceCutting.scn b/examples/Demos/TriangleSurfaceCutting.scn index ff1ac8f95c0..6606ffde44a 100644 --- a/examples/Demos/TriangleSurfaceCutting.scn +++ b/examples/Demos/TriangleSurfaceCutting.scn @@ -4,7 +4,7 @@ - + @@ -33,7 +33,7 @@ - + diff --git a/examples/Demos/include_test.scn b/examples/Demos/include_test.scn index e87c364c0a6..2abaeecc312 100644 --- a/examples/Demos/include_test.scn +++ b/examples/Demos/include_test.scn @@ -1,6 +1,6 @@ - + diff --git a/examples/Demos/include_test.xml b/examples/Demos/include_test.xml index b9d0af04bd9..630c537c627 100644 --- a/examples/Demos/include_test.xml +++ b/examples/Demos/include_test.xml @@ -3,7 +3,7 @@ - + diff --git a/examples/Demos/liver.scn b/examples/Demos/liver.scn index f9a61bcd922..97cbaccc4cd 100644 --- a/examples/Demos/liver.scn +++ b/examples/Demos/liver.scn @@ -4,7 +4,7 @@ - + @@ -33,7 +33,7 @@ - + diff --git a/examples/Demos/liverConfiguration.scn b/examples/Demos/liverConfiguration.scn index 79bb8040f47..320b1c9cd4a 100644 --- a/examples/Demos/liverConfiguration.scn +++ b/examples/Demos/liverConfiguration.scn @@ -4,7 +4,7 @@ - + @@ -49,7 +49,7 @@ - + diff --git a/examples/Demos/rigidifiedSectionsInBeam.scn b/examples/Demos/rigidifiedSectionsInBeam.scn index 6997800c1fd..1556d02f278 100644 --- a/examples/Demos/rigidifiedSectionsInBeam.scn +++ b/examples/Demos/rigidifiedSectionsInBeam.scn @@ -15,7 +15,7 @@ The distances do not vary over time for the rigidified beam, whereas we can obse - + @@ -41,7 +41,7 @@ The distances do not vary over time for the rigidified beam, whereas we can obse - + @@ -76,7 +76,7 @@ The distances do not vary over time for the rigidified beam, whereas we can obse - + diff --git a/examples/Demos/simpleBoundaryConditions.scn b/examples/Demos/simpleBoundaryConditions.scn index 4a2f8a3c4b5..003ba8a71e6 100644 --- a/examples/Demos/simpleBoundaryConditions.scn +++ b/examples/Demos/simpleBoundaryConditions.scn @@ -1,7 +1,7 @@ - + @@ -27,15 +27,15 @@ - + - + - + diff --git a/examples/Demos/simpleSphere.scn b/examples/Demos/simpleSphere.scn index ff25c96b2b5..3f368f5dc1e 100644 --- a/examples/Demos/simpleSphere.scn +++ b/examples/Demos/simpleSphere.scn @@ -4,7 +4,7 @@ - + @@ -32,7 +32,7 @@ - + diff --git a/examples/Demos/skybox.scn b/examples/Demos/skybox.scn index 71debbb798b..6291ef1f642 100644 --- a/examples/Demos/skybox.scn +++ b/examples/Demos/skybox.scn @@ -2,7 +2,7 @@ - + @@ -46,7 +46,7 @@ - + diff --git a/examples/Objects/BoxConstraint.xml b/examples/Objects/BoxConstraint.xml index c0e071328df..c738f9b2ee3 100644 --- a/examples/Objects/BoxConstraint.xml +++ b/examples/Objects/BoxConstraint.xml @@ -1,4 +1,4 @@ - + diff --git a/examples/Objects/InstrumentCoil.xml b/examples/Objects/InstrumentCoil.xml index 487295942a2..8fe3e9883a9 100644 --- a/examples/Objects/InstrumentCoil.xml +++ b/examples/Objects/InstrumentCoil.xml @@ -12,7 +12,7 @@ - + diff --git a/examples/Objects/PlaneConstraint.xml b/examples/Objects/PlaneConstraint.xml index 38c58c77d7d..3584c1a658f 100644 --- a/examples/Objects/PlaneConstraint.xml +++ b/examples/Objects/PlaneConstraint.xml @@ -1,4 +1,4 @@ - + diff --git a/examples/Tutorials/StepByStep/TopologicalMapping/2_TopoMapping.html b/examples/Tutorials/StepByStep/TopologicalMapping/2_TopoMapping.html index 6f85f2bb9fc..75bde4f22ea 100644 --- a/examples/Tutorials/StepByStep/TopologicalMapping/2_TopoMapping.html +++ b/examples/Tutorials/StepByStep/TopologicalMapping/2_TopoMapping.html @@ -31,7 +31,7 @@

Add a DiagonalMass component. In the Property 2/2 tab, change the mass density to "2".

-

Add a FixedPlaneConstraint component. In the Property 2/2 tab, change the normal direction of the plane to (0 0 1), the minimum plane distance from the origin to "-0.1" and the maximum plane distance from the origin to "0.1".

+

Add a FixedPlaneProjectiveConstraint component. In the Property 2/2 tab, change the normal direction of the plane to (0 0 1), the minimum plane distance from the origin to "-0.1" and the maximum plane distance from the origin to "0.1".

Add a FixedConstraint component.

diff --git a/examples/Tutorials/StepByStep/TopologicalMapping/2_TopoMapping.scn b/examples/Tutorials/StepByStep/TopologicalMapping/2_TopoMapping.scn index 62835d77397..8b2b9ec44de 100644 --- a/examples/Tutorials/StepByStep/TopologicalMapping/2_TopoMapping.scn +++ b/examples/Tutorials/StepByStep/TopologicalMapping/2_TopoMapping.scn @@ -2,7 +2,7 @@ - + @@ -29,7 +29,7 @@ - + diff --git a/examples/Tutorials/StepByStep/TopologicalMapping/3_TopoMapping.scn b/examples/Tutorials/StepByStep/TopologicalMapping/3_TopoMapping.scn index 8ceefb5a0a0..c09c77d9c8e 100644 --- a/examples/Tutorials/StepByStep/TopologicalMapping/3_TopoMapping.scn +++ b/examples/Tutorials/StepByStep/TopologicalMapping/3_TopoMapping.scn @@ -2,7 +2,7 @@ - + @@ -31,7 +31,7 @@ - + diff --git a/examples/Tutorials/StepByStep/TopologicalMapping/4_TopoMapping.scn b/examples/Tutorials/StepByStep/TopologicalMapping/4_TopoMapping.scn index 08159c9da90..33ba1964c7e 100644 --- a/examples/Tutorials/StepByStep/TopologicalMapping/4_TopoMapping.scn +++ b/examples/Tutorials/StepByStep/TopologicalMapping/4_TopoMapping.scn @@ -3,7 +3,7 @@ - + @@ -33,7 +33,7 @@ - + diff --git a/examples/Tutorials/Topologies/TopologyTetra2TriangleTopologicalMapping.scn b/examples/Tutorials/Topologies/TopologyTetra2TriangleTopologicalMapping.scn index f278c7c6683..f3f5a631066 100644 --- a/examples/Tutorials/Topologies/TopologyTetra2TriangleTopologicalMapping.scn +++ b/examples/Tutorials/Topologies/TopologyTetra2TriangleTopologicalMapping.scn @@ -5,7 +5,7 @@ - + @@ -37,7 +37,7 @@ - +