public SparseBlockMatrix getSearchDirectionMatrix(Vector uk, Vector qk) {
    SparseBlockMatrix BM = new SparseBlockMatrix(3, 3);
    SparseMatrix M = this.getM();
    SparseMatrix A = this.getA(qk);
    SparseMatrix AT = A.copy().trans();
    SparseMatrix C = this.getC(uk);
    SparseMatrix CT = C.copy().trans();
    SparseMatrix R = this.getBR();

    SparseMatrix BM13 = new SparseMatrixRowMajor(M.getRowDim(), R.getColDim());
    SparseMatrix BM22 = new SparseMatrixRowMajor(A.getRowDim(), A.getColDim());
    SparseMatrix BM31 = new SparseMatrixRowMajor(R.getRowDim(), M.getColDim());

    BM.setBlock(1, 1, M);
    BM.setBlock(1, 2, AT);
    BM.setBlock(1, 3, BM13);

    BM.setBlock(2, 1, A);
    BM.setBlock(2, 2, BM22);
    BM.setBlock(2, 3, C);

    BM.setBlock(3, 1, BM31);
    BM.setBlock(3, 2, CT);
    BM.setBlock(3, 3, R);

    return BM;
  }
  /**
   * Return a new BolckMatrix object that share the same sub matrix objects
   *
   * @param BM
   * @param col1
   * @param col2
   * @return
   */
  public SparseBlockMatrix changeBlockColumn(SparseBlockMatrix BM, int col1, int col2) {
    int blockRow = BM.getRowBlockDim();
    int blockCol = BM.getColBlockDim();

    SparseBlockMatrix newBM = new SparseBlockMatrix(blockRow, blockCol);
    for (int i = 1; i <= blockRow; i++) {
      for (int j = 1; j <= blockCol; j++) {
        newBM.setBlock(i, j, BM.getBlock(i, j));
      }
    }

    for (int i = 1; i <= BM.getRowBlockDim(); i++) {
      newBM.setBlock(i, col1, BM.getBlock(i, col2));
      newBM.setBlock(i, col2, BM.getBlock(i, col1));
    }
    return newBM;
  }
 protected void setDirichlet(
     SparseBlockMatrix BM, SparseBlockVector BV, int matIndex, double value) {
   int row = matIndex;
   int col = matIndex;
   BM.set(row, col, 1.0);
   BV.set(row, value);
   for (int r = 1; r <= BM.getRowDim(); r++) {
     if (r != row) {
       BV.add(r, -BM.get(r, col) * value);
       BM.set(r, col, 0.0);
     }
   }
   for (int c = 1; c <= BM.getColDim(); c++) {
     if (c != col) {
       BM.set(row, c, 0.0);
     }
   }
 }