From d3d20a1f7e55ede2b974c0f8202f6c3ef22d939c Mon Sep 17 00:00:00 2001
From: Alex Bilger <alxbilger@users.noreply.github.com>
Date: Thu, 25 Jan 2024 11:09:16 +0100
Subject: [PATCH] [StateContainer] Accelerate copy of MatrixDeriv for CRS
 matrices (#4443)

* [LinearAlgebra] Factorize value filtering

No more template specialization

* Compatibility with all CompressedRowSparseMatrixGeneric

including CompressedRowSparseMatrixConstraint

* [StateContainer] Accelerate copy of MatrixDeriv for CRS matrices

* set the sizes again because in some cases they are changed in copyToBaseMatrix

* add comment

* Fix when rowBegin is empty
---
 .../linearsystem/MatrixLinearSystem.inl       | 12 ++++++--
 .../statecontainer/MechanicalObject.inl       | 29 ++++++++++++++-----
 .../CompressedRowSparseMatrixMechanical.h     |  2 +-
 3 files changed, 32 insertions(+), 11 deletions(-)

diff --git a/Sofa/Component/LinearSystem/src/sofa/component/linearsystem/MatrixLinearSystem.inl b/Sofa/Component/LinearSystem/src/sofa/component/linearsystem/MatrixLinearSystem.inl
index 18231191cb6..074c5b85614 100644
--- a/Sofa/Component/LinearSystem/src/sofa/component/linearsystem/MatrixLinearSystem.inl
+++ b/Sofa/Component/LinearSystem/src/sofa/component/linearsystem/MatrixLinearSystem.inl
@@ -1036,10 +1036,11 @@ auto MatrixLinearSystem<TMatrix, TVector>::computeJacobiansFrom(BaseMechanicalSt
         return jacobians;
     }
 
-    MechanicalResetConstraintVisitor(&cparams).execute(this->getSolveContext());
-
     auto mappingJacobianId = sofa::core::MatrixDerivId::mappingJacobian();
 
+    // this clears the matrix identified by mappingJacobian() among others
+    MechanicalResetConstraintVisitor(&cparams).execute(this->getSolveContext());
+
     // optimisation to build only the relevent entries of the jacobian matrices
     // The relevent entries are the ones that have a influence on the result
     // of the product J^T * K * J.
@@ -1073,6 +1074,13 @@ auto MatrixLinearSystem<TMatrix, TVector>::computeJacobiansFrom(BaseMechanicalSt
         J->resize(mstate->getMatrixSize(), input->getMatrixSize());
         unsigned int offset {};
         input->copyToBaseMatrix(J.get(), mappingJacobianId, offset);
+
+        //set the sizes again because in some cases they are changed in copyToBaseMatrix
+        J->nCol = input->getMatrixSize();
+        J->nRow = mstate->getMatrixSize();
+        J->nBlockCol = J->nCol;
+        J->nBlockRow = J->nRow;
+
         J->fullRows();
     }
 
diff --git a/Sofa/Component/StateContainer/src/sofa/component/statecontainer/MechanicalObject.inl b/Sofa/Component/StateContainer/src/sofa/component/statecontainer/MechanicalObject.inl
index fc0ed06fb5b..4c9f98935e5 100644
--- a/Sofa/Component/StateContainer/src/sofa/component/statecontainer/MechanicalObject.inl
+++ b/Sofa/Component/StateContainer/src/sofa/component/statecontainer/MechanicalObject.inl
@@ -40,6 +40,8 @@
 
 #include <algorithm>
 #include <cassert>
+#include <sofa/linearalgebra/CompressedRowSparseMatrixMechanical.h>
+
 
 namespace
 {
@@ -855,17 +857,28 @@ void MechanicalObject<DataTypes>::copyToBaseMatrix(linearalgebra::BaseMatrix* de
     {
         const MatrixDeriv& matrix = matrixData->getValue();
 
-        for (MatrixDerivRowConstIterator rowIt = matrix.begin(); rowIt != matrix.end(); ++rowIt)
+        if (auto* crs = dynamic_cast<linearalgebra::CompressedRowSparseMatrixMechanical<Real, sofa::linearalgebra::CRSMechanicalPolicy>*>(dest))
+        {
+            // This is more performant compared to the generic case
+            // The structure of the matrix is the same compared to the generic
+            // case, but dest sizes may be modified compared to the generic case
+            crs->copyNonZeros(matrix);
+        }
+        else //generic case
         {
-            const int cid = rowIt.index();
-            for (MatrixDerivColConstIterator colIt = rowIt.begin(); colIt != rowIt.end(); ++colIt)
+            //no modification of the size
+            for (MatrixDerivRowConstIterator rowIt = matrix.begin(); rowIt != matrix.end(); ++rowIt)
             {
-                const unsigned int dof = colIt.index();
-                const Deriv n = colIt.val();
-
-                for (unsigned int r = 0; r < Deriv::size(); ++r)
+                const int cid = rowIt.index();
+                for (MatrixDerivColConstIterator colIt = rowIt.begin(); colIt != rowIt.end(); ++colIt)
                 {
-                    dest->add(cid, offset + dof * Deriv::size() + r, n[r]);
+                    const unsigned int dof = colIt.index();
+                    const Deriv n = colIt.val();
+
+                    for (unsigned int r = 0; r < Deriv::size(); ++r)
+                    {
+                        dest->add(cid, offset + dof * Deriv::size() + r, n[r]);
+                    }
                 }
             }
         }
diff --git a/Sofa/framework/LinearAlgebra/src/sofa/linearalgebra/CompressedRowSparseMatrixMechanical.h b/Sofa/framework/LinearAlgebra/src/sofa/linearalgebra/CompressedRowSparseMatrixMechanical.h
index e53298da336..6c55b59cb20 100644
--- a/Sofa/framework/LinearAlgebra/src/sofa/linearalgebra/CompressedRowSparseMatrixMechanical.h
+++ b/Sofa/framework/LinearAlgebra/src/sofa/linearalgebra/CompressedRowSparseMatrixMechanical.h
@@ -579,7 +579,7 @@ class CompressedRowSparseMatrixMechanical final // final is used to allow the co
                 }
             }
 
-            if (!keepEmptyRows && this->rowBegin.back() == vid) // row was empty
+            if (!keepEmptyRows && !this->rowBegin.empty() && this->rowBegin.back() == vid) // row was empty
             {
                 this->rowIndex.pop_back();
                 this->rowBegin.pop_back();