From 7be422a2e8acc66eeff1678d4f86bf7d22ea7394 Mon Sep 17 00:00:00 2001
From: Alex Bilger <alexbilger0@gmail.com>
Date: Fri, 20 Oct 2023 10:54:53 +0200
Subject: [PATCH 1/2] [LinearAlgebra] Speedup accumulation on BTDMatrix

---
 .../src/sofa/linearalgebra/BTDMatrix.h        | 36 +++++++++++++++++++
 1 file changed, 36 insertions(+)

diff --git a/Sofa/framework/LinearAlgebra/src/sofa/linearalgebra/BTDMatrix.h b/Sofa/framework/LinearAlgebra/src/sofa/linearalgebra/BTDMatrix.h
index 9c8a852618e..0df5a32c333 100644
--- a/Sofa/framework/LinearAlgebra/src/sofa/linearalgebra/BTDMatrix.h
+++ b/Sofa/framework/LinearAlgebra/src/sofa/linearalgebra/BTDMatrix.h
@@ -185,6 +185,12 @@ class BTDMatrix : public linearalgebra::BaseMatrix
 
     void add(Index i, Index j, double v) override;
 
+    /**
+     * Accumulation specialized on contributions of the same size than the blocks.
+     */
+    template <std::size_t M = N, std::enable_if_t<(M > 3), int> = 0>
+    void add(Index row, Index col, const type::Mat<BSIZE, BSIZE, Real>& v);
+
     void clear(Index i, Index j) override;
 
     void clearRow(Index i) override;
@@ -226,6 +232,36 @@ class BTDMatrix : public linearalgebra::BaseMatrix
     }
 };
 
+
+template <std::size_t N, typename T>
+template <std::size_t M, std::enable_if_t<(M > 3), int>>
+void BTDMatrix<N, T>::add(Index row, Index col,
+    const type::Mat<BSIZE, BSIZE, Real>& v)
+{
+    if (row % BSIZE == 0 && col % BSIZE == 0)
+    {
+        const Index bi = row / BSIZE;
+        const Index bj = col / BSIZE;
+        const Index bindex = bj - bi + 1;
+        if (bindex >= 3)
+        {
+            return;
+        }
+        data[bi * 3 + bindex] += v;
+    }
+    else
+    {
+        for (sofa::Index i = 0; i < BSIZE; ++i)
+        {
+            for (sofa::Index j = 0; j < BSIZE; ++j)
+            {
+                this->add(row + i, row + j, v(i, j));
+            }
+        }
+    }
+}
+
+
 #if !defined(SOFA_LINEARALGEBRA_BTDMATRIX_CPP)
 extern template class SOFA_LINEARALGEBRA_API linearalgebra::BTDMatrix<6, SReal>;
 #endif

From 51ef90ab2b6db05bd173783f46d981780f6cec44 Mon Sep 17 00:00:00 2001
From: Alex Bilger <alexbilger0@gmail.com>
Date: Fri, 20 Oct 2023 11:13:48 +0200
Subject: [PATCH 2/2] Fix

---
 Sofa/framework/LinearAlgebra/src/sofa/linearalgebra/BTDMatrix.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Sofa/framework/LinearAlgebra/src/sofa/linearalgebra/BTDMatrix.h b/Sofa/framework/LinearAlgebra/src/sofa/linearalgebra/BTDMatrix.h
index 0df5a32c333..35910298f30 100644
--- a/Sofa/framework/LinearAlgebra/src/sofa/linearalgebra/BTDMatrix.h
+++ b/Sofa/framework/LinearAlgebra/src/sofa/linearalgebra/BTDMatrix.h
@@ -255,7 +255,7 @@ void BTDMatrix<N, T>::add(Index row, Index col,
         {
             for (sofa::Index j = 0; j < BSIZE; ++j)
             {
-                this->add(row + i, row + j, v(i, j));
+                this->add(row + i, col + j, v(i, j));
             }
         }
     }