diff --git a/MathLib/LinAlg/Dense/DenseTools.cpp b/MathLib/LinAlg/Dense/DenseTools.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..dc43fbcaada852b65ebca256b27c3b3d0f7f011a
--- /dev/null
+++ b/MathLib/LinAlg/Dense/DenseTools.cpp
@@ -0,0 +1,62 @@
+/**
+ * \file
+ * \author Norihiro Watanabe
+ * \date   2013-04-16
+ * \brief  Implementation of dense matrix and vector utility functions.
+ *
+ * \copyright
+ * Copyright (c) 2013, OpenGeoSys Community (http://www.opengeosys.org)
+ *            Distributed under a Modified BSD License.
+ *              See accompanying file LICENSE.txt or
+ *              http://www.opengeosys.org/project/license
+ *
+ */
+
+#include "DenseTools.h"
+
+namespace MathLib
+{
+
+void applyKnownSolution(DenseMatrix<double> &eqsA, DenseVector<double> &eqsRHS, std::size_t k, double x)
+{
+    const size_t n_rows = eqsA.getNRows();
+    const size_t n_cols = eqsA.getNCols();
+    //A(k, j) = 0.
+    for (size_t j=0; j<n_cols; j++)
+        eqsA(k, j) = .0;
+    //A(k, k) = 1,
+    eqsA(k,k) = 1.0;
+    //b_i -= A(i,k)*val, i!=k
+    for (size_t i=0; i<n_rows; ++i)
+    {
+      if (i==k) continue;
+      double v_i_k = .0;
+      for (size_t j=0; j<n_cols; ++j)
+      {
+        if (j==k) {
+          v_i_k = eqsA(i, k);
+          eqsA(i, k) = .0;
+          break;
+        } else if (j>k) {
+          break;
+        }
+      }
+      eqsRHS[i] -= v_i_k*x;
+    }
+    //b_k = val
+    eqsRHS[k] = x;
+}
+
+void applyKnownSolution(DenseMatrix<double> &A, DenseVector<double> &b, const std::vector<std::size_t> &_vec_knownX_id, const std::vector<double> &_vec_knownX_x)
+{
+    const std::size_t n_bc = _vec_knownX_id.size();
+    for (std::size_t i_bc=0; i_bc<n_bc; i_bc++) {
+        applyKnownSolution(A, b, _vec_knownX_id[i_bc], _vec_knownX_x[i_bc]);
+    }
+}
+
+
+} // MathLib
+
+
+
diff --git a/MathLib/LinAlg/Dense/DenseTools.h b/MathLib/LinAlg/Dense/DenseTools.h
new file mode 100644
index 0000000000000000000000000000000000000000..d8d04743f41ccfdc6d93266aa8d4a425551c1304
--- /dev/null
+++ b/MathLib/LinAlg/Dense/DenseTools.h
@@ -0,0 +1,54 @@
+/**
+ * \file
+ * \author Norihiro Watanabe
+ * \date   2013-04-16
+ * \brief  Declaration of dense matrix and vector utility functions.
+ *
+ * \copyright
+ * Copyright (c) 2013, OpenGeoSys Community (http://www.opengeosys.org)
+ *            Distributed under a Modified BSD License.
+ *              See accompanying file LICENSE.txt or
+ *              http://www.opengeosys.org/project/license
+ *
+ */
+
+#ifndef DENSETOOLS_H_
+#define DENSETOOLS_H_
+
+#include <vector>
+#include "DenseMatrix.h"
+#include "DenseVector.h"
+
+namespace MathLib
+{
+
+/**
+ * apply known solutions to a system of linear equations
+ *
+ * This function introduces the given constrain by diagonalizing a coefficient matrix.
+ * Symmetricity of the matrix is preserved.
+ *
+ * @param A                 Coefficient matrix
+ * @param b                 RHS vector
+ * @param _vec_knownX_id    a vector of known solution entry IDs
+ * @param _vec_knownX_x     a vector of known solutions
+ */
+void applyKnownSolution(DenseMatrix<double> &A, DenseVector<double> &b, const std::vector<std::size_t> &_vec_knownX_id, const std::vector<double> &_vec_knownX_x);
+
+/**
+ * apply known solutions to a system of linear equations
+ *
+ * This function introduces the given constrain by diagonalizing a coefficient matrix.
+ * Symmetricity of the matrix is preserved.
+ *
+ * @param A         Coefficient matrix
+ * @param b         RHS vector
+ * @param row_id    a known solution entry ID
+ * @param x         a known solution
+ */
+void applyKnownSolution(DenseMatrix<double> &A, DenseVector<double> &b, std::size_t row_id, double x);
+
+} // MathLib
+
+#endif //DENSETOOLS_H_
+