From 1e96b51374f7a4b1f6c0c534f1371c9bb025ef77 Mon Sep 17 00:00:00 2001
From: Arne Morten Kvarving <arne.morten.kvarving@sintef.no>
Date: Thu, 14 Sep 2023 11:57:50 +0200
Subject: [PATCH] added: extract a sub-block of a matrix

---
 src/LinAlg/Test/TestMatrix.C | 20 ++++++++++++++++++++
 src/LinAlg/matrix.h          | 18 ++++++++++++++++++
 2 files changed, 38 insertions(+)

diff --git a/src/LinAlg/Test/TestMatrix.C b/src/LinAlg/Test/TestMatrix.C
index 41216469..cb1c76e7 100644
--- a/src/LinAlg/Test/TestMatrix.C
+++ b/src/LinAlg/Test/TestMatrix.C
@@ -38,6 +38,26 @@ TEST(TestMatrix, AddBlock)
 }
 
 
+TEST(TestMatrix, ExtractBlock)
+{
+  utl::matrix<int> a(3,3), b(2,2);
+
+  std::iota(a.begin(), a.end(), 1);
+
+  a.extractBlock(b,1,1);
+  EXPECT_EQ(b(1,1), 1);
+  EXPECT_EQ(b(2,1), 2);
+  EXPECT_EQ(b(1,2), 4);
+  EXPECT_EQ(b(2,2), 5);
+
+  a.extractBlock(b,2,2,true);
+  EXPECT_EQ(b(1,1), 1+5);
+  EXPECT_EQ(b(2,1), 2+6);
+  EXPECT_EQ(b(1,2), 4+8);
+  EXPECT_EQ(b(2,2), 5+9);
+}
+
+
 TEST(TestMatrix, AddRows)
 {
   utl::matrix<int> a(3,5);
diff --git a/src/LinAlg/matrix.h b/src/LinAlg/matrix.h
index 71989da3..a8480a95 100644
--- a/src/LinAlg/matrix.h
+++ b/src/LinAlg/matrix.h
@@ -565,6 +565,24 @@ namespace utl //! General utility classes and functions.
       }
     }
 
+    //! \brief Extract a block of the matrix to another matrix.
+    void extractBlock(matrix<T>& block, size_t r, size_t c,
+                      bool addTo = false,
+                      bool transposed = false) const
+    {
+      size_t nr = transposed ? block.cols() : block.rows();
+      size_t nc = transposed ? block.rows() : block.cols();
+      for (size_t i = 1; i <= nr && i+r-1 <= nrow; i++)
+      {
+        size_t ip = i+r-2 + nrow*(c-1);
+        for (size_t j = 1; j <= nc && j+c-1 <= ncol; j++, ip += nrow)
+          if (addTo)
+            (transposed ? block(j,i) : block(i,j)) += this->elem[ip];
+          else
+            (transposed ? block(j,i) : block(i,j)) = this->elem[ip];
+      }
+    }
+
     //! \brief Create a diagonal matrix.
     matrix<T>& diag(T d, size_t dim = 0)
     {