Add requested templates

sofa-framework · May 8, 2024 · feb52c5 · feb52c5
1 parent 3163d52
commit feb52c5
Show file tree

Hide file tree

Showing 4 changed files with 121 additions and 2 deletions.
diff --git a/...angian/Model/src/sofa/component/constraint/lagrangian/model/FixedLagrangianConstraint.cpp b/...angian/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<DataMatrixDeriv> &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<Vec6Types>::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<Vec3Types>::doBuildConstraintLine( helper::WriteAccessor<DataMatrixDeriv> &c, unsigned int lineNumber)
+void FixedLagrangianConstraint<Vec6Types>::doGetSingleConstraintResolution(std::vector<core::behavior::ConstraintResolution*>& resTab, unsigned int& offset, unsigned int lineNumber)
+{
+    resTab[offset] = new BilateralConstraintResolutionNDof(6);
+    offset += 6;
+}
+
+// Vec3 specialization
+template <>
+void FixedLagrangianConstraint< Vec3Types >::doBuildConstraintLine( helper::WriteAccessor<DataMatrixDeriv> &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<Vec3Types>::doGetSingleConstraintResolution(std::
 }
 
 
+// Vec2 specialization
+template <>
+void FixedLagrangianConstraint< Vec2Types >::doBuildConstraintLine( helper::WriteAccessor<DataMatrixDeriv> &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<Vec2Types>::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<Vec2Types>::doGetSingleConstraintResolution(std::vector<core::behavior::ConstraintResolution*>& resTab, unsigned int& offset, unsigned int lineNumber)
+{
+    resTab[offset] = new BilateralConstraintResolutionNDof(2);
+    offset += 2;
+}
+
+// Vec1 specialization
+template <>
+void FixedLagrangianConstraint< Vec1Types >::doBuildConstraintLine( helper::WriteAccessor<DataMatrixDeriv> &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<Vec1Types>::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<Vec1Types>::doGetSingleConstraintResolution(std::vector<core::behavior::ConstraintResolution*>& resTab, unsigned int& offset, unsigned int lineNumber)
+{
+    resTab[offset] = new BilateralConstraintResolutionNDof(1);
+    offset += 1;
+}
+
 // Rigid3 specialization
 template<>
 void FixedLagrangianConstraint<Rigid3Types>::doBuildConstraintLine( helper::WriteAccessor<DataMatrixDeriv> &c, unsigned int lineNumber)
@@ -129,10 +237,16 @@ void FixedLagrangianConstraint<Rigid3Types>::doGetSingleConstraintResolution(std
 
 
 int FixedLagrangianConstraintClass = core::RegisterObject("Lagrangian-based fixation of DOFs of the model")
+        .add< FixedLagrangianConstraint<Vec6Types> >()
         .add< FixedLagrangianConstraint<Vec3Types> >()
+        .add< FixedLagrangianConstraint<Vec2Types> >()
+        .add< FixedLagrangianConstraint<Vec1Types> >()
         .add< FixedLagrangianConstraint<Rigid3Types> >();
 
+template class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API FixedLagrangianConstraint<Vec6Types>;
 template class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API FixedLagrangianConstraint<Vec3Types>;
+template class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API FixedLagrangianConstraint<Vec2Types>;
+template class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API FixedLagrangianConstraint<Vec1Types>;
 template class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API FixedLagrangianConstraint<Rigid3Types>;
 
 

diff --git a/...grangian/Model/src/sofa/component/constraint/lagrangian/model/FixedLagrangianConstraint.h b/...grangian/Model/src/sofa/component/constraint/lagrangian/model/FixedLagrangianConstraint.h
@@ -86,7 +86,10 @@ class FixedLagrangianConstraint : public core::behavior::Constraint<DataTypes>
 };
 
 #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
@@ -26,6 +26,7 @@ namespace sofa::core::behavior
 {
 
 using namespace sofa::defaulttype;
+template class SOFA_CORE_API Constraint<Vec6Types>;
 template class SOFA_CORE_API Constraint<Vec3Types>;
 template class SOFA_CORE_API Constraint<Vec2Types>;
 template class SOFA_CORE_API Constraint<Vec1Types>;

diff --git 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<DataTypes>
 };
 
 #if !defined(SOFA_CORE_BEHAVIOR_CONSTRAINT_CPP)
+extern template class SOFA_CORE_API Constraint<defaulttype::Vec6Types>;
 extern template class SOFA_CORE_API Constraint<defaulttype::Vec3Types>;
 extern template class SOFA_CORE_API Constraint<defaulttype::Vec2Types>;
 extern template class SOFA_CORE_API Constraint<defaulttype::Vec1Types>;