diff --git a/.gitignore b/.gitignore
index bb2df7a..b99062c 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,4 +1,5 @@
 cmake-*/
+build/
 .idea/
 output/
 *.und/
diff --git a/cmake/recipes/eigen.cmake b/cmake/recipes/eigen.cmake
index 9b7729e..8e33f24 100644
--- a/cmake/recipes/eigen.cmake
+++ b/cmake/recipes/eigen.cmake
@@ -25,8 +25,7 @@ else()
     FetchContent_Declare(
         eigen
         GIT_REPOSITORY https://gitlab.com/libeigen/eigen.git
-#        GIT_TAG tags/3.3.7
-        GIT_TAG tags/3.4.0
+        GIT_TAG tags/3.3.7
         GIT_SHALLOW TRUE
     )
     FetchContent_GetProperties(eigen)
diff --git a/static_sim/Material.cpp b/static_sim/Material.cpp
index d6fe140..edf7df2 100755
--- a/static_sim/Material.cpp
+++ b/static_sim/Material.cpp
@@ -7,155 +7,203 @@
 
 #include "Material.hpp"
 
-namespace ssim{
-
-void Material::computeN_tet(const Eigen::RowVector3d &P, const Eigen::Matrix<double, 4, 3> &X,
-                            Eigen::Matrix<double, 3, 12> &N){
-
-    Eigen::Matrix<double, 4, 4> H;
-    H.col(0).setOnes();
-    H(Eigen::all, Eigen::seq(1, Eigen::last)) = X;
-    double V6 = H.determinant();
-
-    Eigen::Matrix<int, 4, 3> index;
-    index << 1, 2, 3,
-            0, 2, 3,
-            0, 1, 3,
-            0, 1, 2;
-
-    Eigen::RowVector4d a;
-    Eigen::RowVector4d b;
-    Eigen::RowVector4d c;
-    Eigen::RowVector4d d;
-
-    a(0) = H(index.row(0), index.row(0)).determinant();
-    a(1) = -H(index.row(1), index.row(0)).determinant();
-    a(2) = H(index.row(2), index.row(0)).determinant();
-    a(3) = -H(index.row(3), index.row(0)).determinant();
-
-    b(0) = -H(index.row(0), index.row(1)).determinant();
-    b(1) = H(index.row(1), index.row(1)).determinant();
-    b(2) = -H(index.row(2), index.row(1)).determinant();
-    b(3) = H(index.row(3), index.row(1)).determinant();
-
-    c(0) = H(index.row(0), index.row(2)).determinant();
-    c(1) = -H(index.row(1), index.row(2)).determinant();
-    c(2) = H(index.row(2), index.row(2)).determinant();
-    c(3) = -H(index.row(3), index.row(2)).determinant();
-
-    d(0) = -H(index.row(0), index.row(3)).determinant();
-    d(1) = H(index.row(1), index.row(3)).determinant();
-    d(2) = -H(index.row(2), index.row(3)).determinant();
-    d(3) = H(index.row(3), index.row(3)).determinant();
-
-    Eigen::RowVector4d NN;
-    NN(0) = (a(0) + b(0) * P(0) + c(0) * P(1) + d(0) * P(2)) / V6;
-    NN(1) = (a(1) + b(1) * P(0) + c(1) * P(1) + d(1) * P(2)) / V6;
-    NN(2) = (a(2) + b(2) * P(0) + c(2) * P(1) + d(2) * P(2)) / V6;
-    NN(3) = (a(3) + b(3) * P(0) + c(3) * P(1) + d(3) * P(2)) / V6;
-
-    N.setZero();
-    N(0, Eigen::seq(0, Eigen::last, 3)) = NN;
-    N(1, Eigen::seq(1, Eigen::last, 3)) = NN;
-    N(2, Eigen::seq(2, Eigen::last, 3)) = NN;
-
-}
-
-void Material::computeB_tet(const Eigen::Matrix<double, 4, 3> &X, Eigen::Matrix<double, 6, 12> &B){
-
-    Eigen::Matrix<double, 4, 4> H;
-    H.col(0).setOnes();
-    H(Eigen::all, Eigen::seq(1, Eigen::last)) = X;
-    double V6 = H.determinant();
-
-    Eigen::Matrix<int, 4, 3> index;
-    index << 1, 2, 3,
-            0, 2, 3,
-            0, 1, 3,
-            0, 1, 2;
-
-    Eigen::RowVector4d b;
-    Eigen::RowVector4d c;
-    Eigen::RowVector4d d;
-
-    b(0) = -H(index.row(0), index.row(1)).determinant();
-    b(1) = H(index.row(1), index.row(1)).determinant();
-    b(2) = -H(index.row(2), index.row(1)).determinant();
-    b(3) = H(index.row(3), index.row(1)).determinant();
-
-    c(0) = H(index.row(0), index.row(2)).determinant();
-    c(1) = -H(index.row(1), index.row(2)).determinant();
-    c(2) = H(index.row(2), index.row(2)).determinant();
-    c(3) = -H(index.row(3), index.row(2)).determinant();
-
-    d(0) = -H(index.row(0), index.row(3)).determinant();
-    d(1) = H(index.row(1), index.row(3)).determinant();
-    d(2) = -H(index.row(2), index.row(3)).determinant();
-    d(3) = H(index.row(3), index.row(3)).determinant();
-
-    B.setZero();
-    B(0, Eigen::seq(0, Eigen::last, 3)) = b;
-    B(1, Eigen::seq(1, Eigen::last, 3)) = c;
-    B(2, Eigen::seq(2, Eigen::last, 3)) = d;
-    B(3, Eigen::seq(0, Eigen::last, 3)) = c;
-    B(3, Eigen::seq(1, Eigen::last, 3)) = b;
-    B(4, Eigen::seq(1, Eigen::last, 3)) = d;
-    B(4, Eigen::seq(2, Eigen::last, 3)) = c;
-    B(5, Eigen::seq(0, Eigen::last, 3)) = d;
-    B(5, Eigen::seq(2, Eigen::last, 3)) = b;
-    B /= V6;
-
-}
-
-
-void Material::computeKe_tet(const Eigen::Matrix<double, 4, 3> &X, const Eigen::Matrix<double, 6, 6> &D,
-                             Eigen::Matrix<double, 12, 12> &Ke, double &Vol){
-
-    Eigen::Matrix<double, 4, 4> H;
-    H.col(0).setOnes();
-    H(Eigen::all, Eigen::seq(1, Eigen::last)) = X;
-    double V6 = H.determinant();
-    Vol = V6 / 6.0;
-
-    Eigen::Matrix<int, 4, 3> index;
-    index << 1, 2, 3,
-             0, 2, 3,
-             0, 1, 3,
-             0, 1, 2;
-
-    Eigen::RowVector4d b;
-    Eigen::RowVector4d c;
-    Eigen::RowVector4d d;
-
-    b(0) = -H(index.row(0), index.row(1)).determinant();
-    b(1) = H(index.row(1), index.row(1)).determinant();
-    b(2) = -H(index.row(2), index.row(1)).determinant();
-    b(3) = H(index.row(3), index.row(1)).determinant();
-
-    c(0) = H(index.row(0), index.row(2)).determinant();
-    c(1) = -H(index.row(1), index.row(2)).determinant();
-    c(2) = H(index.row(2), index.row(2)).determinant();
-    c(3) = -H(index.row(3), index.row(2)).determinant();
-
-    d(0) = -H(index.row(0), index.row(3)).determinant();
-    d(1) = H(index.row(1), index.row(3)).determinant();
-    d(2) = -H(index.row(2), index.row(3)).determinant();
-    d(3) = H(index.row(3), index.row(3)).determinant();
-
-    Eigen::Matrix<double, 6, 12> B;
-    B.setZero();
-    B(0, Eigen::seq(0, Eigen::last, 3)) = b;
-    B(1, Eigen::seq(1, Eigen::last, 3)) = c;
-    B(2, Eigen::seq(2, Eigen::last, 3)) = d;
-    B(3, Eigen::seq(0, Eigen::last, 3)) = c;
-    B(3, Eigen::seq(1, Eigen::last, 3)) = b;
-    B(4, Eigen::seq(1, Eigen::last, 3)) = d;
-    B(4, Eigen::seq(2, Eigen::last, 3)) = c;
-    B(5, Eigen::seq(0, Eigen::last, 3)) = d;
-    B(5, Eigen::seq(2, Eigen::last, 3)) = b;
-    B /= V6;
-
-    Ke = Vol * (B.transpose() * D * B);
-}
+namespace ssim {
+
+    void Material::computeN_tet(const Eigen::RowVector3d &P, const Eigen::Matrix<double, 4, 3> &X,
+                                Eigen::Matrix<double, 3, 12> &N) {
+
+        Eigen::Matrix<double, 4, 4> H;
+        H.col(0).setOnes();
+        H.rightCols<3>() = X;
+        double V6 = H.determinant();
+
+        Eigen::Matrix<int, 4, 3> index;
+        index << 1, 2, 3,
+                0, 2, 3,
+                0, 1, 3,
+                0, 1, 2;
+
+        Eigen::RowVector4d a;
+        Eigen::RowVector4d b;
+        Eigen::RowVector4d c;
+        Eigen::RowVector4d d;
+
+        auto SubMatrix = [](const Eigen::Matrix4d &original,
+                            const Eigen::Vector3i &rowIdx,
+                            const Eigen::Vector3i &colIdx) {
+            Eigen::Matrix3d ret;
+            for (int i = 0; i < 3; ++i) {
+                int rowI = rowIdx(i);
+                for (int j = 0; j < 3; ++j) {
+                    int colI = colIdx(j);
+                    ret(i, j) = original(rowI, colI);
+                }
+            }
+            return ret;
+        };
+
+        a(0) = SubMatrix(H, index.row(0), index.row(0)).determinant();
+        a(1) = -SubMatrix(H, index.row(1), index.row(0)).determinant();
+        a(2) = SubMatrix(H, index.row(2), index.row(0)).determinant();
+        a(3) = -SubMatrix(H, index.row(3), index.row(0)).determinant();
+
+        b(0) = -SubMatrix(H, index.row(0), index.row(1)).determinant();
+        b(1) = SubMatrix(H, index.row(1), index.row(1)).determinant();
+        b(2) = -SubMatrix(H, index.row(2), index.row(1)).determinant();
+        b(3) = SubMatrix(H, index.row(3), index.row(1)).determinant();
+
+        c(0) = SubMatrix(H, index.row(0), index.row(2)).determinant();
+        c(1) = -SubMatrix(H, index.row(1), index.row(2)).determinant();
+        c(2) = SubMatrix(H, index.row(2), index.row(2)).determinant();
+        c(3) = -SubMatrix(H, index.row(3), index.row(2)).determinant();
+
+        d(0) = -SubMatrix(H, index.row(0), index.row(3)).determinant();
+        d(1) = SubMatrix(H, index.row(1), index.row(3)).determinant();
+        d(2) = -SubMatrix(H, index.row(2), index.row(3)).determinant();
+        d(3) = SubMatrix(H, index.row(3), index.row(3)).determinant();
+
+        Eigen::RowVector4d NN;
+        NN(0) = (a(0) + b(0) * P(0) + c(0) * P(1) + d(0) * P(2)) / V6;
+        NN(1) = (a(1) + b(1) * P(0) + c(1) * P(1) + d(1) * P(2)) / V6;
+        NN(2) = (a(2) + b(2) * P(0) + c(2) * P(1) + d(2) * P(2)) / V6;
+        NN(3) = (a(3) + b(3) * P(0) + c(3) * P(1) + d(3) * P(2)) / V6;
+
+        N.setZero();
+        for (int i = 0; i < 4; ++i) {
+            N(0, i * 3) = NN(i);
+            N(1, i * 3 + 1) = NN(i);
+            N(2, i * 3 + 2) = NN(i);
+        }
+
+    }
+
+    void Material::computeB_tet(const Eigen::Matrix<double, 4, 3> &X, Eigen::Matrix<double, 6, 12> &B) {
+
+        Eigen::Matrix<double, 4, 4> H;
+        H.col(0).setOnes();
+        H.rightCols<3>() = X;
+        double V6 = H.determinant();
+
+        Eigen::Matrix<int, 4, 3> index;
+        index << 1, 2, 3,
+                0, 2, 3,
+                0, 1, 3,
+                0, 1, 2;
+
+        Eigen::RowVector4d b;
+        Eigen::RowVector4d c;
+        Eigen::RowVector4d d;
+
+        auto SubMatrix = [](const Eigen::Matrix4d &original,
+                            const Eigen::Vector3i &rowIdx,
+                            const Eigen::Vector3i &colIdx) {
+            Eigen::Matrix3d ret;
+            for (int i = 0; i < 3; ++i) {
+                int rowI = rowIdx(i);
+                for (int j = 0; j < 3; ++j) {
+                    int colI = colIdx(j);
+                    ret(i, j) = original(rowI, colI);
+                }
+            }
+            return ret;
+        };
+
+        b(0) = -SubMatrix(H, index.row(0), index.row(1)).determinant();
+        b(1) = SubMatrix(H, index.row(1), index.row(1)).determinant();
+        b(2) = -SubMatrix(H, index.row(2), index.row(1)).determinant();
+        b(3) = SubMatrix(H, index.row(3), index.row(1)).determinant();
+
+        c(0) = SubMatrix(H, index.row(0), index.row(2)).determinant();
+        c(1) = -SubMatrix(H, index.row(1), index.row(2)).determinant();
+        c(2) = SubMatrix(H, index.row(2), index.row(2)).determinant();
+        c(3) = -SubMatrix(H, index.row(3), index.row(2)).determinant();
+
+        d(0) = -SubMatrix(H, index.row(0), index.row(3)).determinant();
+        d(1) = SubMatrix(H, index.row(1), index.row(3)).determinant();
+        d(2) = -SubMatrix(H, index.row(2), index.row(3)).determinant();
+        d(3) = SubMatrix(H, index.row(3), index.row(3)).determinant();
+
+        B.setZero();
+        for (int i = 0; i < 4; ++i) {
+            B(0, 3 * i) = b(i);
+            B(1, 3 * i + 1) = c(i);
+            B(2, 3 * i + 2) = d(i);
+            B(3, 3 * i) = c(i);
+            B(3, 3 * i + 1) = b(i);
+            B(4, 3 * i + 1) = d(i);
+            B(4, 3 * i + 2) = c(i);
+            B(5, 3 * i) = d(i);
+            B(5, 3 * i + 2) = b(i);
+        }
+        B /= V6;
+
+    }
+
+
+    void Material::computeKe_tet(const Eigen::Matrix<double, 4, 3> &X, const Eigen::Matrix<double, 6, 6> &D,
+                                 Eigen::Matrix<double, 12, 12> &Ke, double &Vol) {
+
+        Eigen::Matrix<double, 4, 4> H;
+        H.col(0).setOnes();
+        H.rightCols<3>() = X;
+        double V6 = H.determinant();
+        Vol = V6 / 6.0;
+
+        Eigen::Matrix<int, 4, 3> index;
+        index << 1, 2, 3,
+                0, 2, 3,
+                0, 1, 3,
+                0, 1, 2;
+
+        Eigen::RowVector4d b;
+        Eigen::RowVector4d c;
+        Eigen::RowVector4d d;
+
+        auto SubMatrix = [](const Eigen::Matrix4d &original,
+                            const Eigen::Vector3i &rowIdx,
+                            const Eigen::Vector3i &colIdx) {
+            Eigen::Matrix3d ret;
+            for (int i = 0; i < 3; ++i) {
+                int rowI = rowIdx(i);
+                for (int j = 0; j < 3; ++j) {
+                    int colI = colIdx(j);
+                    ret(i, j) = original(rowI, colI);
+                }
+            }
+            return ret;
+        };
+
+        b(0) = -SubMatrix(H, index.row(0), index.row(1)).determinant();
+        b(1) = SubMatrix(H, index.row(1), index.row(1)).determinant();
+        b(2) = -SubMatrix(H, index.row(2), index.row(1)).determinant();
+        b(3) = SubMatrix(H, index.row(3), index.row(1)).determinant();
+
+        c(0) = SubMatrix(H, index.row(0), index.row(2)).determinant();
+        c(1) = -SubMatrix(H, index.row(1), index.row(2)).determinant();
+        c(2) = SubMatrix(H, index.row(2), index.row(2)).determinant();
+        c(3) = -SubMatrix(H, index.row(3), index.row(2)).determinant();
+
+        d(0) = -SubMatrix(H, index.row(0), index.row(3)).determinant();
+        d(1) = SubMatrix(H, index.row(1), index.row(3)).determinant();
+        d(2) = -SubMatrix(H, index.row(2), index.row(3)).determinant();
+        d(3) = SubMatrix(H, index.row(3), index.row(3)).determinant();
+
+        Eigen::Matrix<double, 6, 12> B;
+        B.setZero();
+        for (int i = 0; i < 4; ++i) {
+            B(0, 3 * i) = b(i);
+            B(1, 3 * i + 1) = c(i);
+            B(2, 3 * i + 2) = d(i);
+            B(3, 3 * i) = c(i);
+            B(3, 3 * i + 1) = b(i);
+            B(4, 3 * i + 1) = d(i);
+            B(4, 3 * i + 2) = c(i);
+            B(5, 3 * i) = d(i);
+            B(5, 3 * i + 2) = b(i);
+        }
+        B /= V6;
+
+        Ke = Vol * (B.transpose() * D * B);
+    }
 
 } // namespace SIM
\ No newline at end of file
diff --git a/static_sim/StaticSim.cpp b/static_sim/StaticSim.cpp
index 20e3303..0952ad3 100644
--- a/static_sim/StaticSim.cpp
+++ b/static_sim/StaticSim.cpp
@@ -96,8 +96,8 @@ namespace ssim {
             int cnt = 0;
             for (const auto &item: vFLoc[vI]) {
                 int eleI = item.first;
-                const auto &U_eleI = U(eDof[eleI]);
-                Eigen::Matrix<double, 4, 3> X = TV(TT.row(eleI), Eigen::all);
+                const auto &U_eleI = Utils::SubVector(U, eDof[eleI]);
+                Eigen::Matrix<double, 4, 3> X = Utils::SubMatrix(TV, TT.row(eleI), Eigen::Vector3i(0, 1, 2));
 
                 Eigen::Matrix<double, 6, 12> B;
                 Material::computeB_tet(X, B);
@@ -126,8 +126,8 @@ namespace ssim {
         Qstress.resize(nQ, 6);
         for (int qI = 0; qI < nQ; ++qI) {
             int tI = tetI(qI);
-            const auto &U_tI = U(eDof[tI]);
-            Eigen::Matrix<double, 4, 3> X = TV(TT.row(tI), Eigen::all);
+            const auto &U_tI = Utils::SubVector(U, eDof[tI]);
+            Eigen::Matrix<double, 4, 3> X = Utils::SubMatrix(TV, TT.row(tI), Eigen::Vector3i(0, 1, 2));
 
             Eigen::Matrix<double, 3, 12> N;
             Eigen::Matrix<double, 6, 12> B;
@@ -171,9 +171,11 @@ namespace ssim {
 #endif
                           {
                               Eigen::VectorXi TT_I = TT.row(eI);
-                              eDof[eI](Eigen::seq(0, Eigen::last, 3)) = TT_I * 3;
-                              eDof[eI](Eigen::seq(1, Eigen::last, 3)) = (TT_I * 3).array() + 1;
-                              eDof[eI](Eigen::seq(2, Eigen::last, 3)) = (TT_I * 3).array() + 2;
+                              for (int i_ = 0; i_ < 4; ++i_) {
+                                  eDof[eI](3 * i_) = TT_I(i_) * 3;
+                                  eDof[eI](3 * i_ + 1) = TT_I(i_) * 3 + 1;
+                                  eDof[eI](3 * i_ + 2) = TT_I(i_) * 3 + 2;
+                              }
                           }
 #ifdef USE_TBB
         );
@@ -184,8 +186,9 @@ namespace ssim {
         vNeighbor.resize(nN);
         for (int eI = 0; eI < nEle; ++eI) {
             const Eigen::Matrix<int, 1, 4> &eleVInd = TT.row(eI);
-            for (const auto &nI: eleVInd) {
-                vNeighbor[nI].insert(eleVInd.begin(), eleVInd.end());
+            std::vector<int> eleVInd_vec{eleVInd(0), eleVInd(1), eleVInd(2), eleVInd(3)};
+            for (const auto &nI: eleVInd_vec) {
+                vNeighbor[nI].insert(eleVInd_vec.begin(), eleVInd_vec.end());
             }
         }
         for (int nI = 0; nI < nN; ++nI) { // remove itself
@@ -215,7 +218,7 @@ namespace ssim {
 #endif
                           {
                               // eleKe
-                              Eigen::Matrix<double, 4, 3> X = TV(TT.row(eI), Eigen::all);
+                              Eigen::Matrix<double, 4, 3> X = Utils::SubMatrix(TV, TT.row(eI), Eigen::Vector3i(0, 1, 2));
                               Eigen::Matrix<double, 12, 12> eleKe_I;
                               double vol;
                               Material::computeKe_tet(X, D, eleKe_I, vol);
@@ -257,7 +260,7 @@ namespace ssim {
 
         compliance_ = load.dot(U);
         spdlog::info("compliance C = {:e}", compliance_);
-        TV1 = TV + U.reshaped(3, nN).transpose();
+        // TV1 = TV + U.reshaped(3, nN).transpose();
         // Utils::writeTetVTK(outputPath + "deformed.vtk", TV1, TT);
 
 
@@ -352,7 +355,8 @@ namespace ssim {
         spdlog::debug("#DBC nodes: {}, #NBC particles: {}", nDBC, nNBC);
 
         // ensure (DBC intersect NBC) = (empty)
-        for (const int &nI: DBC_nI) {
+        for (int i_ = 0; i_ < DBC_nI.size(); ++i_) {
+            int nI = DBC_nI(i_);
             load.segment<DIM_>(nI * DIM_).setZero();
         }
     }
diff --git a/static_sim/StaticSim.h b/static_sim/StaticSim.h
index d670219..4d1f705 100644
--- a/static_sim/StaticSim.h
+++ b/static_sim/StaticSim.h
@@ -118,7 +118,7 @@ namespace ssim {
 
         int eleNodeNum;
         int eleDofNum;
-        std::vector<Eigen::Vector<int, 12>> eDof;
+        std::vector<Eigen::VectorXi> eDof;
 
         // owned features
         Eigen::VectorXd load; // load of each dof
diff --git a/static_sim/Utils.cpp b/static_sim/Utils.cpp
index cf56dd5..8012cc4 100755
--- a/static_sim/Utils.cpp
+++ b/static_sim/Utils.cpp
@@ -318,11 +318,32 @@ void Utils::elasticMatrix(double YM, double PR, Eigen::Matrix<double, 6, 6> &D)
     D *= YM / (1.0 + PR) / (1.0 - 2.0 * PR);
 }
 
-double Utils::vonStress(const Eigen::Vector<double, 6> &stress) {
+double Utils::vonStress(const Eigen::VectorXd &stress) {
     return sqrt(0.5 * (pow(stress(0) - stress(1), 2.0) +
                        pow(stress(1) - stress(2), 2.0) +
                        pow(stress(2) - stress(0), 2.0))
-                + 3.0 * stress({3, 4, 5}).squaredNorm());
+                + 3.0 * Utils::SubVector(stress, Eigen::Vector3i(3, 4, 5)).squaredNorm());
+}
+
+Eigen::MatrixXd Utils::SubMatrix(const Eigen::MatrixXd &original,
+                                 const Eigen::VectorXi &rowIdx,
+                                 const Eigen::VectorXi &colIdx) {
+    Eigen::MatrixXd ret(rowIdx.size(), colIdx.size());
+    for (int i = 0; i < rowIdx.size(); ++i) {
+        for (int j = 0; j < colIdx.size(); ++j) {
+            ret(i, j) = original(rowIdx(i), colIdx(j));
+        }
+    }
+    return ret;
+}
+
+Eigen::VectorXd Utils::SubVector(const Eigen::VectorXd &original,
+                                 const Eigen::VectorXi &index) {
+    Eigen::VectorXd ret(index.size());
+    for (int i = 0; i < index.size(); ++i) {
+        ret(i) = original(index(i));
+    }
+    return ret;
 }
 
 } // namespace SIM
\ No newline at end of file
diff --git a/static_sim/Utils.hpp b/static_sim/Utils.hpp
index a3b48ee..2334b13 100755
--- a/static_sim/Utils.hpp
+++ b/static_sim/Utils.hpp
@@ -88,7 +88,7 @@ static void writeOBJ(const std::string &path, const Eigen::MatrixXd &V, const Ei
 
 static void elasticMatrix(double YM, double PR, Eigen::Matrix<double, 6, 6> &D);
 
-static double vonStress(const Eigen::Vector<double, 6> &stress);
+static double vonStress(const Eigen::VectorXd &stress);
 
 template <int dim>
 static void addBlockToMatrix(const Eigen::MatrixXd& block,
@@ -128,6 +128,13 @@ static void addBlockToMatrix(const Eigen::MatrixXd& block,
 
 }
 
+static Eigen::MatrixXd SubMatrix(const Eigen::MatrixXd &original,
+                                 const Eigen::VectorXi &rowIdx,
+                                 const Eigen::VectorXi &colIdx);
+
+static Eigen::VectorXd SubVector(const Eigen::VectorXd &original,
+                                 const Eigen::VectorXi &index);
+
 };
 
 } // namespace SIM