From feb52c5ca85073f6028c6a2ef10966be92bb8f56 Mon Sep 17 00:00:00 2001 From: bakpaul Date: Mon, 6 May 2024 17:44:51 +0200 Subject: [PATCH] Add requested templates --- .../model/FixedLagrangianConstraint.cpp | 118 +++++++++++++++++- .../model/FixedLagrangianConstraint.h | 3 + .../src/sofa/core/behavior/Constraint.cpp | 1 + .../Core/src/sofa/core/behavior/Constraint.h | 1 + 4 files changed, 121 insertions(+), 2 deletions(-) diff --git a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/FixedLagrangianConstraint.cpp b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/FixedLagrangianConstraint.cpp index 9450db9111a..69b16e126d7 100644 --- a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/FixedLagrangianConstraint.cpp +++ b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/FixedLagrangianConstraint.cpp @@ -32,10 +32,59 @@ namespace sofa::component::constraint::lagrangian::model using namespace sofa::defaulttype; using namespace sofa::helper; +// Vec6 specialization +template <> +void FixedLagrangianConstraint< Vec6Types >::doBuildConstraintLine( helper::WriteAccessor &c, unsigned int lineNumber) +{ + constexpr Coord c0(1,0,0,0,0,0), c1(0,1,0,0,0,0), c2(0,0,1,0,0,0), c3(0,0,0,1,0,0), c4(0,0,0,0,1,0), c5(0,0,0,0,0,1); + const unsigned dofIdx = d_fixAll.getValue() ? lineNumber : d_indices.getValue()[lineNumber]; + + MatrixDerivRowIterator c_it = c->writeLine(m_cid[lineNumber]); + c_it.setCol(dofIdx, c0); + + c_it = c->writeLine(m_cid[lineNumber] + 1); + c_it.setCol(dofIdx, c1); + + c_it = c->writeLine(m_cid[lineNumber] + 2); + c_it.setCol(dofIdx, c2); + + c_it = c->writeLine(m_cid[lineNumber] + 3); + c_it.setCol(dofIdx, c3); + + c_it = c->writeLine(m_cid[lineNumber] + 4); + c_it.setCol(dofIdx, c4); + + c_it = c->writeLine(m_cid[lineNumber] + 5); + c_it.setCol(dofIdx, c5); + + +} + +template<> +void FixedLagrangianConstraint::doGetSingleConstraintViolation(linearalgebra::BaseVector *resV, const DataVecCoord * freePos, const DataVecCoord * restPos,unsigned int lineNumber) +{ + unsigned dofIdx = d_fixAll.getValue() ? lineNumber : d_indices.getValue()[lineNumber]; + const Coord dfree = freePos->getValue()[dofIdx] - restPos->getValue()[dofIdx]; + + resV->set(m_cid[lineNumber] , dfree[0]); + resV->set(m_cid[lineNumber]+1, dfree[1]); + resV->set(m_cid[lineNumber]+2, dfree[2]); + resV->set(m_cid[lineNumber]+3, dfree[3]); + resV->set(m_cid[lineNumber]+4, dfree[4]); + resV->set(m_cid[lineNumber]+5, dfree[5]); + +} -// Vec3 specialization template<> -void FixedLagrangianConstraint::doBuildConstraintLine( helper::WriteAccessor &c, unsigned int lineNumber) +void FixedLagrangianConstraint::doGetSingleConstraintResolution(std::vector& resTab, unsigned int& offset, unsigned int lineNumber) +{ + resTab[offset] = new BilateralConstraintResolutionNDof(6); + offset += 6; +} + +// Vec3 specialization +template <> +void FixedLagrangianConstraint< Vec3Types >::doBuildConstraintLine( helper::WriteAccessor &c, unsigned int lineNumber) { constexpr Coord cx(1,0,0), cy(0,1,0), cz(0,0,1); const unsigned dofIdx = d_fixAll.getValue() ? lineNumber : d_indices.getValue()[lineNumber]; @@ -71,6 +120,65 @@ void FixedLagrangianConstraint::doGetSingleConstraintResolution(std:: } +// Vec2 specialization +template <> +void FixedLagrangianConstraint< Vec2Types >::doBuildConstraintLine( helper::WriteAccessor &c, unsigned int lineNumber) +{ + constexpr Coord cx(1, 0), cy(0, 1); + const unsigned dofIdx = d_fixAll.getValue() ? lineNumber : d_indices.getValue()[lineNumber]; + + MatrixDerivRowIterator c_it = c->writeLine(m_cid[lineNumber]); + c_it.setCol(dofIdx, cx); + + c_it = c->writeLine(m_cid[lineNumber] + 1); + c_it.setCol(dofIdx, cy); +} + +template<> +void FixedLagrangianConstraint::doGetSingleConstraintViolation(linearalgebra::BaseVector *resV, const DataVecCoord * freePos, const DataVecCoord * restPos,unsigned int lineNumber) +{ + unsigned dofIdx = d_fixAll.getValue() ? lineNumber : d_indices.getValue()[lineNumber]; + const Coord dfree = freePos->getValue()[dofIdx] - restPos->getValue()[dofIdx]; + + resV->set(m_cid[lineNumber] , dfree[0]); + resV->set(m_cid[lineNumber]+1, dfree[1]); + +} + +template<> +void FixedLagrangianConstraint::doGetSingleConstraintResolution(std::vector& resTab, unsigned int& offset, unsigned int lineNumber) +{ + resTab[offset] = new BilateralConstraintResolutionNDof(2); + offset += 2; +} + +// Vec1 specialization +template <> +void FixedLagrangianConstraint< Vec1Types >::doBuildConstraintLine( helper::WriteAccessor &c, unsigned int lineNumber) +{ + constexpr Coord cx(1); + const unsigned dofIdx = d_fixAll.getValue() ? lineNumber : d_indices.getValue()[lineNumber]; + + MatrixDerivRowIterator c_it = c->writeLine(m_cid[lineNumber]); + c_it.setCol(dofIdx, cx); +} + +template<> +void FixedLagrangianConstraint::doGetSingleConstraintViolation(linearalgebra::BaseVector *resV, const DataVecCoord * freePos, const DataVecCoord * restPos,unsigned int lineNumber) +{ + unsigned dofIdx = d_fixAll.getValue() ? lineNumber : d_indices.getValue()[lineNumber]; + const Coord dfree = freePos->getValue()[dofIdx] - restPos->getValue()[dofIdx]; + + resV->set(m_cid[lineNumber] , dfree[0]); +} + +template<> +void FixedLagrangianConstraint::doGetSingleConstraintResolution(std::vector& resTab, unsigned int& offset, unsigned int lineNumber) +{ + resTab[offset] = new BilateralConstraintResolutionNDof(1); + offset += 1; +} + // Rigid3 specialization template<> void FixedLagrangianConstraint::doBuildConstraintLine( helper::WriteAccessor &c, unsigned int lineNumber) @@ -129,10 +237,16 @@ void FixedLagrangianConstraint::doGetSingleConstraintResolution(std int FixedLagrangianConstraintClass = core::RegisterObject("Lagrangian-based fixation of DOFs of the model") + .add< FixedLagrangianConstraint >() .add< FixedLagrangianConstraint >() + .add< FixedLagrangianConstraint >() + .add< FixedLagrangianConstraint >() .add< FixedLagrangianConstraint >(); +template class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API FixedLagrangianConstraint; template class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API FixedLagrangianConstraint; +template class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API FixedLagrangianConstraint; +template class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API FixedLagrangianConstraint; template class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API FixedLagrangianConstraint; diff --git a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/FixedLagrangianConstraint.h b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/FixedLagrangianConstraint.h index 710d8d7dc50..37c78811980 100644 --- a/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/FixedLagrangianConstraint.h +++ b/Sofa/Component/Constraint/Lagrangian/Model/src/sofa/component/constraint/lagrangian/model/FixedLagrangianConstraint.h @@ -86,7 +86,10 @@ class FixedLagrangianConstraint : public core::behavior::Constraint }; #if !defined(SOFA_COMPONENT_CONSTRAINTSET_FIXEDLAGRANGIANCONSTRAINT_CPP) +extern template class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API FixedLagrangianConstraint< defaulttype::Vec6Types >; extern template class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API FixedLagrangianConstraint< defaulttype::Vec3Types >; +extern template class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API FixedLagrangianConstraint< defaulttype::Vec2Types >; +extern template class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API FixedLagrangianConstraint< defaulttype::Vec1Types >; extern template class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API FixedLagrangianConstraint< defaulttype::Rigid3Types >; #endif diff --git a/Sofa/framework/Core/src/sofa/core/behavior/Constraint.cpp b/Sofa/framework/Core/src/sofa/core/behavior/Constraint.cpp index b41e61444de..af0cef4e3f0 100644 --- a/Sofa/framework/Core/src/sofa/core/behavior/Constraint.cpp +++ b/Sofa/framework/Core/src/sofa/core/behavior/Constraint.cpp @@ -26,6 +26,7 @@ namespace sofa::core::behavior { using namespace sofa::defaulttype; +template class SOFA_CORE_API Constraint; template class SOFA_CORE_API Constraint; template class SOFA_CORE_API Constraint; template class SOFA_CORE_API Constraint; diff --git a/Sofa/framework/Core/src/sofa/core/behavior/Constraint.h b/Sofa/framework/Core/src/sofa/core/behavior/Constraint.h index dd3dbb44609..283a5ec2da7 100644 --- a/Sofa/framework/Core/src/sofa/core/behavior/Constraint.h +++ b/Sofa/framework/Core/src/sofa/core/behavior/Constraint.h @@ -134,6 +134,7 @@ class Constraint : public BaseConstraint, public SingleStateAccessor }; #if !defined(SOFA_CORE_BEHAVIOR_CONSTRAINT_CPP) +extern template class SOFA_CORE_API Constraint; extern template class SOFA_CORE_API Constraint; extern template class SOFA_CORE_API Constraint; extern template class SOFA_CORE_API Constraint;