[Constraint.Projective] Implement applyConstraint from new matrix ass…

…embly API (#4309) * [Constraint.Projective] Implement applyConstraint in AttachConstraint) * [Constraint.Projective] Implement applyConstraint in PartialLinearMovementConstraint * [Constraint.Projective] Implement applyConstraint in ProjectToPointConstraint * Fix compilation * revert rename Data --------- Co-authored-by: Paul Baksic <paul.baksic@outlook.fr>
sofa-framework · Dec 20, 2023 · 384077f · 384077f
1 parent 026a501
commit 384077f
Show file tree

Hide file tree

Showing 9 changed files with 81 additions and 3 deletions.
diff --git a/...mponent/Constraint/Projective/src/sofa/component/constraint/projective/AttachConstraint.h b/...mponent/Constraint/Projective/src/sofa/component/constraint/projective/AttachConstraint.h
@@ -29,4 +29,4 @@ namespace sofa::component::constraint::projective
 {
 template <class T>
 using AttachConstraint SOFA_ATTRIBUTE_DEPRECATED("v24.06 ", "v25.06", "AttachConstraint has been renamed to AttachProjectiveConstraint") = AttachProjectiveConstraint<T>;
-}
+}
diff --git a/...nstraint/Projective/src/sofa/component/constraint/projective/AttachProjectiveConstraint.h b/...nstraint/Projective/src/sofa/component/constraint/projective/AttachProjectiveConstraint.h
@@ -93,6 +93,7 @@ class AttachProjectiveConstraint : public core::behavior::PairInteractionProject
 
     /// 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;
+    void applyConstraint(sofa::core::behavior::ZeroDirichletCondition* matrix) override;
 
     virtual void reinitIfChanged();
 

diff --git a/...traint/Projective/src/sofa/component/constraint/projective/AttachProjectiveConstraint.inl b/...traint/Projective/src/sofa/component/constraint/projective/AttachProjectiveConstraint.inl
@@ -545,6 +545,49 @@ void AttachProjectiveConstraint<DataTypes>::applyConstraint(const core::Mechanic
     }
 }
 
+template <class DataTypes>
+void AttachProjectiveConstraint<DataTypes>::applyConstraint(sofa::core::behavior::ZeroDirichletCondition* matrix)
+{
+    if (f_twoWay.getValue())
+        return;
+
+    reinitIfChanged();
+
+    static constexpr unsigned int N = Deriv::size();
+    const SetIndexArray& indices = f_indices2.getValue();
+    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();
+
+    for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it, ++i)
+    {
+        if (!clamp && i < activeFlags.size() && !activeFlags[i])
+            continue;
+
+        auto index = (*it);
+
+        if (NCLast != NC && (i >= activeFlags.size() || !activeFlags[i + 1]))
+        {
+            // Reset Fixed Row and Col
+            for (unsigned int c = 0; c < NCLast; ++c)
+            {
+                matrix->discardRowCol(N * index + c, N * index + c);
+            }
+        }
+        else
+        {
+            // Reset Fixed Row and Col
+            for (unsigned int c = 0; c < NC; ++c)
+            {
+                matrix->discardRowCol(N * index + c, N * index + c);
+            }
+        }
+
+        ++i;
+    }
+}
+
 template<class DataTypes>
 const typename DataTypes::Real AttachProjectiveConstraint<DataTypes>::getConstraintFactor(const int index) {
     return d_constraintFactor.getValue().size() ? d_constraintFactor.getValue()[index] : 1;

diff --git a/...int/Projective/src/sofa/component/constraint/projective/PartialLinearMovementConstraint.h b/...int/Projective/src/sofa/component/constraint/projective/PartialLinearMovementConstraint.h
@@ -29,4 +29,4 @@ namespace sofa::component::constraint::projective
 {
 template <class T>
 using PartialLinearMovementConstraint SOFA_ATTRIBUTE_DEPRECATED("v24.06 ", "v25.06", "PartialLinearMovementConstraint has been renamed to PartialLinearMovementProjectiveConstraint") = PartialLinearMovementProjectiveConstraint<T>;
-}
+}
diff --git a/...tive/src/sofa/component/constraint/projective/PartialLinearMovementProjectiveConstraint.h b/...tive/src/sofa/component/constraint/projective/PartialLinearMovementProjectiveConstraint.h
@@ -139,6 +139,7 @@ public :
 
     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*) override;
 

diff --git a/...ve/src/sofa/component/constraint/projective/PartialLinearMovementProjectiveConstraint.inl b/...ve/src/sofa/component/constraint/projective/PartialLinearMovementProjectiveConstraint.inl
@@ -457,6 +457,25 @@ void PartialLinearMovementProjectiveConstraint<DataTypes>::applyConstraint(const
     }
 }
 
+
+template <class DataTypes>
+void PartialLinearMovementProjectiveConstraint<DataTypes>::applyConstraint(sofa::core::behavior::ZeroDirichletCondition* matrix)
+{
+    static constexpr unsigned int N = Deriv::size();
+    const SetIndexArray& indices = m_indices.getValue();
+    const VecBool& movedDirection = movedDirections.getValue();
+
+    for (const auto index : indices)
+    {
+        for (unsigned int c = 0; c < N; ++c)
+        {
+            if (movedDirection[c]) {
+                matrix->discardRowCol(N * index + c, N * index + c);
+            }
+        }
+    }
+}
+
 //display the path the constrained dofs will go through
 template <class DataTypes>
 void PartialLinearMovementProjectiveConstraint<DataTypes>::draw(const core::visual::VisualParams* vparams)

diff --git a/...onstraint/Projective/src/sofa/component/constraint/projective/PointProjectiveConstraint.h b/...onstraint/Projective/src/sofa/component/constraint/projective/PointProjectiveConstraint.h
@@ -104,6 +104,7 @@ class PointProjectiveConstraint : public core::behavior::ProjectiveConstraintSet
 
     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;
 
     /** 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.

diff --git a/...straint/Projective/src/sofa/component/constraint/projective/PointProjectiveConstraint.inl b/...straint/Projective/src/sofa/component/constraint/projective/PointProjectiveConstraint.inl
@@ -265,7 +265,20 @@ void PointProjectiveConstraint<DataTypes>::applyConstraint(const core::Mechanica
     }
 }
 
+template <class DataTypes>
+void PointProjectiveConstraint<DataTypes>::applyConstraint(sofa::core::behavior::ZeroDirichletCondition* matrix)
+{
+    static constexpr unsigned int N = Deriv::size();
+    const SetIndexArray& indices = f_indices.getValue();
 
+    for (const auto index : indices)
+    {
+        for (unsigned int c = 0; c < N; ++c)
+        {
+            matrix->discardRowCol(N * index + c, N * index + c);
+        }
+    }
+}
 
 
 template <class DataTypes>

diff --git a/...Constraint/Projective/src/sofa/component/constraint/projective/ProjectToPointConstraint.h b/...Constraint/Projective/src/sofa/component/constraint/projective/ProjectToPointConstraint.h
@@ -29,4 +29,4 @@ namespace sofa::component::constraint::projective
 {
 template <class T>
 using ProjectToPointConstraint SOFA_ATTRIBUTE_DEPRECATED("v24.06 ", "v25.06", "ProjectToPointConstraint has been renamed to PointProjectiveConstraint") = PointProjectiveConstraint<T>;
-}
+}