cell_forces.cpp

Go to the documentation of this file.

/**
 * \file cell_forces.cpp
 * \brief Calculate cell forces
 * \example cell_forces.cpp
 
\tableofcontents
 
\section cell_potential Implementation with forces potential
 
In the following code, we solve the problem of identification of forces which
result in given displacements on some surface inside the body.
 
In the experiment, a body is covered on top by transparent gel layer, on the
interface between movements of markers (bids) is observed. Strain in the body is
generated by cells living on the top layer. Here we looking for forces generated
by those cells.
 
Following implementing when needed could be easily extended to curved surfaces
arbitrary located in the body.
 
\ref mofem_citation
 
\htmlonly
<a href="https://doi.org/10.5281/zenodo.439392"><img
src="https://zenodo.org/badge/DOI/10.5281/zenodo.439392.svg" alt="DOI"></a> <a
href="https://doi.org/10.5281/zenodo.439395"><img
src="https://zenodo.org/badge/DOI/10.5281/zenodo.439395.svg" alt="DOI"></a>
\endhtmlonly
 
\subsection cell_local Local curl-free formulation
 
This problem can be mathematically described by minimisation problem with the
constraints, as follows
\f[
\left\{
\begin{array}{l}
\textrm{min}\,J(u,\rho) = \frac{1}{2} (u-u_d,S(u-u_d))_{S_u} +
\frac{\epsilon_\rho}{2}(\rho,\rho)_{S_\rho} \\
\textrm{div}[\sigma(u)] = 0 \quad \textrm{in}\,V \\
n\cdot\sigma(u) = \rho \quad \textrm{on}\,S_\rho \\
\end{array}
\right.
\f]
Applying standard procedure, i.e. Lagrange multiplier method, we get
\f[
\left\{
\begin{array}{l}
\textrm{min}\,J(u,\rho,\Upsilon) =
\frac{1}{2} (u,S(u-u_d))_{S_u} + \frac{\epsilon_\rho}{2}(\rho,\rho)_{S_\rho} +
(\Upsilon,\textrm{div}[\sigma])_V  \\ n\cdot\sigma = \rho \quad
\textrm{on}\,S_\rho \end{array} \right. \f] and applying differentiation by
parts \f[ J(u,\rho,\Upsilon) = \frac{1}{2} (u-u_d,S(u-u_d))_{S_u}
+\frac{\epsilon_\rho}{2}(\rho,\rho)_{S_\rho}
+(\textrm{grad}[\Upsilon],\sigma)_V - (\Upsilon,n\cdot\sigma)_{S_\rho}
\f]
and using second constraint, i.e. static constrain
\f[
J(u,\rho,\Upsilon) =
\frac{1}{2} (u-u_d,S(u-u_d))_{S_u}
+\frac{\epsilon_\rho}{2}(\rho,\rho)_{S_\rho}
+(\textrm{grad}[\Upsilon],\sigma)_V - (\Upsilon,\rho)_{S_\rho}.
\f]
Note that force is enforced in week sense.
 
Notting that
\f[
\sigma =  \mathcal{A} \left( \textrm{grad}[u] \right)^{s}
\f]
and using symmetry of operator \f$\mathcal{A}\f$,
\f[
(\textrm{grad}[\Upsilon],\sigma)_V =
(\textrm{grad}[\Upsilon],\mathcal{A} \textrm{grad}[u] )_V =
(\mathcal{A}^{*}\textrm{grad}[\Upsilon],\textrm{grad}[u] )_V =
(\Sigma,\textrm{grad}[u] )_V
\f]
we finally get
\f[
J(u,\rho,\Upsilon) =
\frac{1}{2} (u-u_d,S(u-u_d))_{S_u}
+\frac{\epsilon_\rho}{2}(\rho,\rho)_{S_\rho}
+(\Sigma,\textrm{grad}[u])_V - (\Upsilon,\rho)_{S_\rho}
\f]
 
Calculating derivatives, we can get Euler equations in following form
\f[
\left\{
\begin{array}{ll}
(Su,\delta u)_{S_u} + (\Sigma,\textrm{grad}[\delta u])_V &= 0 \\
(\sigma,\textrm{grad}[\delta \Upsilon])_V-(\rho,\delta \Upsilon)_{S_\rho} &= 0
\\
(\epsilon_\rho \rho,\delta \rho)_{S_\rho}-(\Upsilon,\delta \rho)_{S_\rho} &= 0
\end{array}
\right.
\f]
and applying finite element discretization, above equations are expressed
by linear algebraic equations suitable for computer calculations
\f[
\left[
\begin{array}{ccc}
S & K_{u \Upsilon} & 0 \\
K_{\Upsilon u} & 0 & -B^\textrm{T} \\
0 & -B & D
\end{array}
\right]
\left[
\begin{array}{c}
u \\ \Upsilon \\ \rho
\end{array}
\right]
=
\left[
\begin{array}{c}
S u_d \\ 0 \\ 0
\end{array}
\right]
\f]
and finally swapping first two rows, we finally get
\f[
\left[
\begin{array}{ccc}
K & 0 & -B^\textrm{T} \\
S & K & 0 \\
0 & -B & D
\end{array}
\right]
\left[
\begin{array}{c}
u \\ \Upsilon \\ \rho
\end{array}
\right]
=
\left[
\begin{array}{c}
0 \\ S u_d \\ 0
\end{array}
\right]
\f]
where
\f[
S=\epsilon_u^{-1} I\\
\epsilon_\rho^\textrm{abs} =  \epsilon_\rho^\text{ref}\epsilon_u^{-1}
\f]
 
Displacement field is \f$\mathbf{u}\f$ and \f$\Upsilon\f$, force field on top
surface is \f$\boldsymbol\rho\f$.
 
We not yet explored nature of cell forces. Since the cells are attached to the
body surface and anything else, the body as results those forces can not be
subjected to rigid body motion.
 
We assume that forces are generated by 2d objects, as results only tangential
forces can be produced by those forces if the surface is planar. For non-planar
surfaces addition force normal to the surface is present, although the magnitude
of this force could be calculated purely from geometrical considerations, thus
additional physical equations are not needed.
 
Assuming that straight and very short pre-stressed fibres generate cell forces;
a force field is curl-free. Utilising that  forces can be expressed by potential
field as follows
\f[
\rho = \frac{\partial \Phi_\rho}{\partial \mathbf{x}}
\f]
where \f$\Phi\f$ is force potential field.
 
\subsubsection cell_local_approx Approximation
 
For this problem \f$H^(\Omega)\f$ function space is required for \f$u\f$,
\f$\Upsilon\f$ and \f$\rho\f$. Note that \f$\rho\f$ is given by derivative of
potential field \f$\Phi\f$ which derivatives gives potential forces.
 
\subsection cell_nonlocal Nonlocal, i.e. weak curl-free
 
In this variant generalisation of the local model to a weakly enforced force
curl-free cell force field is considered. This generalisation is a consequence
of the observation that cell has some small but finite size. Moreover, a cell
has a complex pre-stressed structure which can transfer shear forces.
 
We define model like before, however this time we add additional term,
\f[
 \left\{
\begin{array}{l}
\textrm{min}\,J(u,\rho) =
\frac{1}{2} (u-u_d,S(u-u_d))_{S_u} +
\frac{\epsilon_\rho}{2}(\rho,\rho-l^2\textrm{curl}^s
[\textrm{curl}^s[\rho]])_{S_\rho} \\
\textrm{div}[\sigma(u)] = 0 \quad \textrm{in}\,V \\
n\cdot\sigma = \rho
\end{array}
\right.
\f]
and reformulating this we have
\f[
\left\{
\begin{array}{l}
\textrm{min}\,J(u,\rho) =
\frac{1}{2} (u-u_d,S(u-u_d))_{S_u} + \frac{\epsilon_\rho}{2}(\rho,\rho)_{S_\rho}
+\frac{\epsilon_l^{-1}}{2}(\textrm{curl}^s[\rho],\textrm{curl}^s[\rho])_{S_\rho}
\\
\textrm{div}[\sigma(u)] = 0 \quad \textrm{in}\,V \\
n\cdot\sigma = \rho
\end{array}
\right.
\f]
where parameter \f$\epsilon_l\f$ controls curl-free term, if this parameter
approach zero, this formulation converge to local variant presented above.
 
Applying the same procedure like before, set of Euler equations is derived
\f[
\left\{
\begin{array}{l}
(Su,\delta u)_{S_u} + (\Sigma,\textrm{grad}[\delta u])_V= 0 \\
(\sigma,\textrm{grad}[\delta \Upsilon])_V-(\rho,\delta \Upsilon)_{S_\rho} = 0 \\
(\epsilon_\rho \rho,\delta
\rho)_{S_\rho}+\epsilon_l^{-1}(\textrm{curl}^s[\rho],\textrm{curl}^s[\delta
\rho])_{S_\rho}-(\Upsilon,\delta \rho)_{S_\rho} = 0 \end{array} \right. \f] That
give following system of equations \f[ \left[ \begin{array}{ccc}
S & K_{u \Upsilon} & 0 \\
K_{\Upsilon u} & 0 & -B^\textrm{T} \\
0 & -B & D
\end{array}
\right]
\left[
\begin{array}{c}
u \\ \Upsilon \\ \rho
\end{array}
\right]
=
\left[
\begin{array}{c}
S u_d \\ 0 \\ 0
\end{array}
\right]
\f]
and swapping the first two rows we get
\f[
\left[
\begin{array}{ccc}
K & 0 & -B^\textrm{T} \\
S & K & 0 \\
0 & -B & D
\end{array}
\right]
\left[
\begin{array}{c}
u \\ \Upsilon \\ \rho
\end{array}
\right]
=
\left[
\begin{array}{c}
0 \\ S u_d \\ 0
\end{array}
\right]
\f]
 
\subsubsection cell_nonlocal_approx Approximation
 
For this problem \f$H^(\Omega)\f$ function space is required for \f$u\f$,
\f$\Upsilon\f$. Note that force field need to be in
\f$H(\textrm{curl},S_\rho)\f$ space.
 
\section cell_solution Field split pre-conditioner
 
In this implementation, the block structure of the matrix is utilised. The \e
PCFIELDSPLIT
(see
<http://www.mcs.anl.gov/petsc/petsc-current/docs/manualpages/PC/PCFIELDSPLIT.html>)
is utilised. Matrix is stored using nested matrix format \e MATNEST
(see
)<http://www.mcs.anl.gov/petsc/petsc-current/docs/manualpages/Mat/MatCreateNest.html>).
 
The sub-netsetd matrix is created containing two upper-left blocks,
\f[
X = \left[
\begin{array}{cc}
K & 0  \\
S & K
\end{array}
\right]
\f]
this netsed matrix is blocked in
\f[
A= \left[
\begin{array}{cc}
X & V  \\
H & D
\end{array}
\right]
\f]
where
\f[
V= \left[
\begin{array}{c}
-B^\textrm{T} \\
0
\end{array}
\right]
\f]
and
\f[
H= \left[
\begin{array}{cc}
0 & -B^\textrm{T}
\end{array}
\right]
\f]
 
The system associated with X matrix is solved using \e PCFIELDSPLIT and
multiplicative relaxation scheme. In following implementation sub-matrix, K is
factorised only once.  The system related to matrix \f$A\f$ composed form
matrices \f$X,D,V,H\f$ is solved using Schur complement. Note that here we
exploit MoFEM capability to create sub-problems in easy way.
 
\section cell_running_code Running code
 
Approximations of displacements
\code
./map_disp \
-my_data_x ./examples/data_x.csv \
-my_data_y ./examples/data_y.csv \
-my_file ./examples/mesh_cell_force_map.cub \
-ksp_type cg -pc_type lu -pc_factor_mat_solver_package mumps -ksp_monitor \
-lambda 0.01 -my_order 3 -scale 0.05 -cube_size 1 -my_nparts 4
\endcode
 
If only one layer is specified, another thin polymer layer could be created
automatically using prism elements,
\code
./map_disp_prism \
-my_thinckness 0.01 \
-my_data_x ./examples/data_x.csv \
-my_data_y ./examples/data_y.csv \
-my_file ./examples/mesh_for_prisms.cub \
-ksp_type cg -pc_type lu -pc_factor_mat_solver_package mumps -ksp_monitor \
-lambda 0.01 -my_order 3 -scale 0.05 -cube_size 1 -my_nparts 4
\endcode
 
Config file
\include users_modules/cell_engineering/examples/block_config.in
 
Calculating forces
\code
mpirun -np 4 ./cell_forces \
-my_file analysis_mesh.h5m \
-my_order 1 -my_order_force 2 -my_max_post_proc_ref_level 0 \
-my_block_config ./examples/block_config.in \
-ksp_type fgmres -ksp_monitor \
-fieldsplit_1_ksp_type fgmres \
-fieldsplit_1_pc_type lu \
-fieldsplit_1_pc_factor_mat_solver_package mumps \
-fieldsplit_1_ksp_max_it 100 \
-fieldsplit_1_ksp_monitor  \
-fieldsplit_0_ksp_type gmres \
-fieldsplit_0_ksp_max_it 25 \
-fieldsplit_0_fieldsplit_0_ksp_type preonly \
-fieldsplit_0_fieldsplit_0_pc_type lu \
-fieldsplit_0_fieldsplit_0_pc_factor_mat_solver_package mumps \
-fieldsplit_0_fieldsplit_1_ksp_type preonly \
-fieldsplit_0_fieldsplit_1_pc_type lu \
-fieldsplit_0_fieldsplit_1_pc_factor_mat_solver_package mumps \
-ksp_atol 1e-6 -ksp_rtol 0 -my_eps_u 1e-4 -my_curl 1
\endcode
 
As results of above we get
\image html cell_engineering_forces_example.gif "Example: results" width=600px
 
\section cell_install Installation with docker
 
- First install docker as per the instructions here:
<https://docs.docker.com/installation/#installation>
- Install cell user module: \e docker \e pull \e likask/cell_engineering
 
\todo Improve documentation
 
\todo Generalization to curved surfaces. That should not be difficult adding
metric tensors and taking into account curvature.
 
\todo Instead of elastic material we can use GEL model developed in other
module. That  will allow to take into account drying and other rheological
effects.
 
\todo For nonlinear material instead of KSP solver we can add SNES solver
 
\todo Physical equations for cell mechanics could be added directly to minimized
functional or by constrains.
 
*/
 
/* This file is part of MoFEM.
 
 * MoFEM is free software: you can redistribute it and/or modify it under
 * the terms of the GNU Lesser General Public License as published by the
 * Free Software Foundation, either version 3 of the License, or (at your
 * option) any later version.
 *
 * MoFEM is distributed in the hope that it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public
 * License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with MoFEM. If not, see <http://www.gnu.org/licenses/>. */
 
#include <BasicFiniteElements.hpp>
using namespace MoFEM;
#include <Hooke.hpp>
#include <CellForces.hpp>
 
static char help[] = "-my_block_config set block data\n"
                     "\n";
 
namespace CellEngineering {
 
struct BlockOptionData {
  int oRder;
  double yOung;
  double pOisson;
  BlockOptionData() : oRder(-1), yOung(-1), pOisson(-2) {}
};
 
} // namespace CellEngineering
 
using namespace boost::numeric;
using namespace CellEngineering;
 
#include <boost/program_options.hpp>
using namespace std;
namespace po = boost::program_options;
 
static int debug = 0;
 
int main(int argc, char *argv[]) {
 
  MoFEM::Core::Initialize(&argc, &argv, (char *)0, help);
 
  try {
 
    moab::Core mb_instance;
    moab::Interface &moab = mb_instance;
 
    PetscBool flg_block_config, flg_file;
    char mesh_file_name[255];
    char block_config_file[255];
    PetscBool flg_order_force;
    PetscInt order = 2;
    PetscInt order_force = 2;
    PetscBool flg_eps_u, flg_eps_rho, flg_eps_l;
    double eps_u = 1e-6;
    double eps_rho = 1e-3;
    double eps_l = 0;
    PetscBool is_curl = PETSC_TRUE;
    CHKERR PetscOptionsBegin(PETSC_COMM_WORLD, "", "Elastic Config", "none");
    CHKERR PetscOptionsString("-my_file", "mesh file name", "", "mesh.h5m",
                              mesh_file_name, 255, &flg_file);
    CHKERR PetscOptionsInt("-my_order", "default approximation order", "",
                           order, &order, PETSC_NULL);
    CHKERR PetscOptionsInt(
        "-my_order_force",
        "default approximation order for force approximation", "", order_force,
        &order_force, &flg_order_force);
    CHKERR PetscOptionsString("-my_block_config", "elastic configure file name",
                              "", "block_conf.in", block_config_file, 255,
                              &flg_block_config);
    CHKERR PetscOptionsReal("-my_eps_rho", "foce optimality parameter ", "",
                            eps_rho, &eps_rho, &flg_eps_rho);
    CHKERR PetscOptionsReal("-my_eps_u", "displacement optimality parameter ",
                            "", eps_u, &eps_u, &flg_eps_u);
    CHKERR PetscOptionsReal("-my_eps_l", "displacement optimality parameter ",
                            "", eps_l, &eps_l, &flg_eps_l);
    CHKERR PetscOptionsBool("-my_curl", "use Hcurl space to approximate forces",
                            "", is_curl, &is_curl, NULL);
    ierr = PetscOptionsEnd();
    CHKERRQ(ierr);
 
    // Reade parameters from line command
    if (flg_file != PETSC_TRUE) {
      SETERRQ(PETSC_COMM_SELF, MOFEM_INVALID_DATA,
              "*** ERROR -my_file (MESH FILE NEEDED)");
    }
 
    ParallelComm *pcomm = ParallelComm::get_pcomm(&moab, MYPCOMM_INDEX);
    if (pcomm == NULL)
      pcomm = new ParallelComm(&moab, PETSC_COMM_WORLD);
 
    // Read mesh to MOAB
    const char *option;
    option = "PARALLEL=READ_PART;"
             "PARALLEL_RESOLVE_SHARED_ENTS;"
             "PARTITION=PARALLEL_PARTITION;";
    // option = "";
    CHKERR moab.load_file(mesh_file_name, 0, option);
 
    // Create MoFEM (Joseph) database
    MoFEM::Core core(moab);
    MoFEM::Interface &m_field = core;
 
    MeshsetsManager *mmanager_ptr;
    CHKERR m_field.query_interface(mmanager_ptr);
    // print bcs
    CHKERR mmanager_ptr->printDisplacementSet();
    CHKERR mmanager_ptr->printForceSet();
    // print block sets with materials
    CHKERR mmanager_ptr->printMaterialsSet();
 
    // stl::bitset see for more details
    BitRefLevel bit_level0;
    bit_level0.set(0);
    {
      Range ents3d;
      CHKERR moab.get_entities_by_dimension(0, 3, ents3d);
      CHKERR m_field.seed_ref_level(ents3d, bit_level0, false);
    }
 
    // set app. order
 
    std::vector<Range> setOrderToEnts(10);
 
    // configure blocks by parsing config file
    // it allow to set approximation order for each block independently
    Range set_order_ents;
    std::map<int, BlockOptionData> block_data;
    if (flg_block_config) {
      double read_eps_u, read_eps_rho, read_eps_l;
      try {
        ifstream ini_file(block_config_file);
        if (!ini_file.is_open()) {
          SETERRQ(PETSC_COMM_SELF, 1,
                  "*** -my_block_config does not exist ***");
        }
        // std::cerr << block_config_file << std::endl;
        po::variables_map vm;
        po::options_description config_file_options;
        config_file_options.add_options()(
            "eps_u", po::value<double>(&read_eps_u)->default_value(-1))(
            "eps_rho", po::value<double>(&read_eps_rho)->default_value(-1))(
            "eps_l", po::value<double>(&read_eps_l)->default_value(-1));
        for (_IT_CUBITMESHSETS_BY_SET_TYPE_FOR_LOOP_(m_field, BLOCKSET, it)) {
          std::ostringstream str_order;
          str_order << "block_" << it->getMeshsetId() << ".displacement_order";
          config_file_options.add_options()(
              str_order.str().c_str(),
              po::value<int>(&block_data[it->getMeshsetId()].oRder)
                  ->default_value(order));
          std::ostringstream str_cond;
          str_cond << "block_" << it->getMeshsetId() << ".young_modulus";
          config_file_options.add_options()(
              str_cond.str().c_str(),
              po::value<double>(&block_data[it->getMeshsetId()].yOung)
                  ->default_value(-1));
          std::ostringstream str_capa;
          str_capa << "block_" << it->getMeshsetId() << ".poisson_ratio";
          config_file_options.add_options()(
              str_capa.str().c_str(),
              po::value<double>(&block_data[it->getMeshsetId()].pOisson)
                  ->default_value(-2));
        }
        po::parsed_options parsed =
            parse_config_file(ini_file, config_file_options, true);
        store(parsed, vm);
        po::notify(vm);
        for (_IT_CUBITMESHSETS_BY_SET_TYPE_FOR_LOOP_(m_field, BLOCKSET, it)) {
          if (block_data[it->getMeshsetId()].oRder == -1)
            continue;
          if (block_data[it->getMeshsetId()].oRder == order)
            continue;
          PetscPrintf(PETSC_COMM_WORLD, "Set block %d order to %d\n",
                      it->getMeshsetId(), block_data[it->getMeshsetId()].oRder);
          Range block_ents;
          CHKERR moab.get_entities_by_handle(it->getMeshset(), block_ents,
                                             true);
          // block_ents = block_ents.subset_by_type(MBTET);
          Range nodes;
          CHKERR moab.get_connectivity(block_ents, nodes, true);
          Range ents_to_set_order, ents3d;
          CHKERR moab.get_adjacencies(nodes, 3, false, ents3d,
                                      moab::Interface::UNION);
          CHKERR moab.get_adjacencies(ents3d, 2, false, ents_to_set_order,
                                      moab::Interface::UNION);
          CHKERR moab.get_adjacencies(ents3d, 1, false, ents_to_set_order,
                                      moab::Interface::UNION);
          ents_to_set_order = subtract(
              ents_to_set_order, ents_to_set_order.subset_by_type(MBQUAD));
          ents_to_set_order = subtract(
              ents_to_set_order, ents_to_set_order.subset_by_type(MBPRISM));
          set_order_ents.merge(ents3d);
          set_order_ents.merge(ents_to_set_order);
          setOrderToEnts[block_data[it->getMeshsetId()].oRder].merge(
              set_order_ents);
        }
        CHKERR m_field.synchronise_entities(set_order_ents, 0);
        std::vector<std::string> additional_parameters;
        additional_parameters =
            collect_unrecognized(parsed.options, po::include_positional);
        for (std::vector<std::string>::iterator vit =
                 additional_parameters.begin();
             vit != additional_parameters.end(); vit++) {
          CHKERR PetscPrintf(PETSC_COMM_WORLD,
                             "** WARNING Unrecognized option %s\n",
                             vit->c_str());
        }
      } catch (const std::exception &ex) {
        std::ostringstream ss;
        ss << ex.what() << std::endl;
        SETERRQ(PETSC_COMM_SELF, MOFEM_STD_EXCEPTION_THROW, ss.str().c_str());
      }
      if (read_eps_u > 0) {
        eps_u = read_eps_u;
      };
      if (read_eps_rho > 0) {
        eps_rho = read_eps_rho;
      }
      if (read_eps_l > 0) {
        eps_l = read_eps_l;
      }
    }
 
    PetscPrintf(PETSC_COMM_WORLD, "epsU = %6.4e epsRho = %6.4e\n", eps_u,
                eps_rho);
 
    // Fields
    CHKERR m_field.add_field("U", H1, AINSWORTH_LEGENDRE_BASE, 3, MB_TAG_SPARSE,
                             MF_ZERO);
    CHKERR m_field.add_field("UPSILON", H1, AINSWORTH_LEGENDRE_BASE, 3,
                             MB_TAG_SPARSE, MF_ZERO);
    if (is_curl) {
      CHKERR m_field.add_field("RHO", HCURL, AINSWORTH_LEGENDRE_BASE, 1);
    } else {
      CHKERR m_field.add_field("RHO", H1, AINSWORTH_LEGENDRE_BASE, 1);
    }
 
    // add entitities (by tets) to the field
    CHKERR m_field.add_ents_to_field_by_type(0, MBTET, "U");
    CHKERR m_field.add_ents_to_field_by_type(0, MBTET, "UPSILON");
    CHKERR m_field.add_ents_to_field_by_type(0, MBPRISM, "U");
    CHKERR m_field.add_ents_to_field_by_type(0, MBPRISM, "UPSILON");
 
    CHKERR m_field.synchronise_field_entities("U");
    CHKERR m_field.synchronise_field_entities("UPSILON");
    CHKERR m_field.synchronise_field_entities("RHO");
 
    Range vertex_to_fix;
    Range edges_to_fix;
    Range ents_1st_layer;
    // If problem is partitioned meshset culd not exist on this part
    if (mmanager_ptr->checkMeshset(202, SIDESET)) {
      CHKERR mmanager_ptr->getEntitiesByDimension(202, SIDESET, 2,
                                                  ents_1st_layer, true);
      CHKERR mmanager_ptr->getEntitiesByDimension(202, SIDESET, 0,
                                                  vertex_to_fix, false);
      CHKERR mmanager_ptr->getEntitiesByDimension(202, SIDESET, 1, edges_to_fix,
                                                  false);
      if (vertex_to_fix.size() != 1 && !vertex_to_fix.empty()) {
        SETERRQ1(PETSC_COMM_WORLD, MOFEM_DATA_INCONSISTENCY,
                 "Should be one vertex only, but is %d", vertex_to_fix.size());
      }
    }
    CHKERR m_field.synchronise_entities(ents_1st_layer, 0);
    CHKERR m_field.add_ents_to_field_by_type(
        ents_1st_layer.subset_by_type(MBTRI), MBTRI, "RHO");
    Range ents_2nd_layer;
    // If problem is partitioned meshset culd not exist on this part
    if (mmanager_ptr->checkMeshset(101, SIDESET)) {
      CHKERR mmanager_ptr->getEntitiesByDimension(101, SIDESET, 2,
                                                  ents_2nd_layer, true);
    }
    CHKERR m_field.synchronise_entities(ents_2nd_layer, 0);
 
    for (int oo = 2; oo != setOrderToEnts.size(); oo++) {
      if (setOrderToEnts[oo].size() > 0) {
        CHKERR m_field.synchronise_entities(setOrderToEnts[oo], 0);
        CHKERR m_field.set_field_order(setOrderToEnts[oo], "U", oo);
        CHKERR m_field.set_field_order(setOrderToEnts[oo], "UPSILON", oo);
      }
    }
 
    const int through_thickness_order = 2;
    {
      Range ents3d;
      CHKERR moab.get_entities_by_dimension(0, 3, ents3d);
      Range ents;
      CHKERR moab.get_adjacencies(ents3d, 2, false, ents,
                                  moab::Interface::UNION);
      CHKERR moab.get_adjacencies(ents3d, 1, false, ents,
                                  moab::Interface::UNION);
 
      Range prisms;
      CHKERR moab.get_entities_by_type(0, MBPRISM, prisms);
      {
        Range quads;
        CHKERR moab.get_adjacencies(prisms, 2, false, quads,
                                    moab::Interface::UNION);
        Range prism_tris;
        prism_tris = quads.subset_by_type(MBTRI);
        quads = subtract(quads, prism_tris);
        Range quads_edges;
        CHKERR moab.get_adjacencies(quads, 1, false, quads_edges,
                                    moab::Interface::UNION);
        Range prism_tris_edges;
        CHKERR moab.get_adjacencies(prism_tris, 1, false, prism_tris_edges,
                                    moab::Interface::UNION);
        quads_edges = subtract(quads_edges, prism_tris_edges);
        prisms.merge(quads);
        prisms.merge(quads_edges);
      }
 
      ents.merge(ents3d);
      ents = subtract(ents, set_order_ents);
      ents = subtract(ents, prisms);
 
      CHKERR m_field.synchronise_entities(ents, 0);
      CHKERR m_field.synchronise_entities(prisms, 0);
 
      CHKERR m_field.set_field_order(ents, "U", order);
      CHKERR m_field.set_field_order(ents, "UPSILON", order);
      // approx. order through thickness to 2
      CHKERR m_field.set_field_order(prisms, "U", through_thickness_order);
      CHKERR m_field.set_field_order(prisms, "UPSILON",
                                     through_thickness_order);
    }
    CHKERR m_field.set_field_order(0, MBVERTEX, "U", 1);
    CHKERR m_field.set_field_order(0, MBVERTEX, "UPSILON", 1);
 
    if (is_curl) {
      CHKERR m_field.set_field_order(0, MBTRI, "RHO", order_force);
      CHKERR m_field.set_field_order(0, MBEDGE, "RHO", order_force);
    } else {
      CHKERR m_field.set_field_order(0, MBTRI, "RHO", order_force);
      CHKERR m_field.set_field_order(0, MBEDGE, "RHO", order_force);
      CHKERR m_field.set_field_order(0, MBVERTEX, "RHO", 1);
    }
 
    // Add elastic element
    boost::shared_ptr<Hooke<adouble>> hooke_adouble_ptr(new Hooke<adouble>());
    boost::shared_ptr<Hooke<double>> hooke_double_ptr(new Hooke<double>());
    NonlinearElasticElement elastic(m_field, 2);
    CHKERR elastic.setBlocks(hooke_double_ptr, hooke_adouble_ptr);
    CHKERR elastic.addElement("ELASTIC", "U");
    CHKERR elastic.setOperators("U", "MESH_NODE_POSITIONS", false, true);
    // Add prisms
    CellEngineering::FatPrism fat_prism_rhs(m_field);
    CellEngineering::FatPrism fat_prism_lhs(m_field);
    {
      CHKERR m_field.add_finite_element("ELASTIC_PRISM", MF_ZERO);
      CHKERR m_field.modify_finite_element_add_field_row("ELASTIC_PRISM", "U");
      CHKERR m_field.modify_finite_element_add_field_col("ELASTIC_PRISM", "U");
      CHKERR m_field.modify_finite_element_add_field_data("ELASTIC_PRISM", "U");
      Range ents_2nd_layer;
      CHKERR mmanager_ptr->getEntitiesByDimension(2, BLOCKSET, 3,
                                                  elastic.setOfBlocks[2].tEts);
      CHKERR m_field.add_ents_to_finite_element_by_type(
          elastic.setOfBlocks[2].tEts, MBPRISM, "ELASTIC_PRISM");
      MatrixDouble inv_jac;
      // right hand side operators
      fat_prism_rhs.getOpPtrVector().push_back(
          new MoFEM::OpCalculateInvJacForFatPrism(inv_jac));
      fat_prism_rhs.getOpPtrVector().push_back(
          new MoFEM::OpSetInvJacH1ForFatPrism(inv_jac));
      fat_prism_rhs.getOpPtrVector().push_back(
          new NonlinearElasticElement::OpGetCommonDataAtGaussPts(
              "U", elastic.commonData));
      fat_prism_rhs.getOpPtrVector().push_back(
          new NonlinearElasticElement::OpJacobianPiolaKirchhoffStress(
              "U", elastic.setOfBlocks[2], elastic.commonData, 2, false, false,
              true));
      fat_prism_rhs.getOpPtrVector().push_back(
          new NonlinearElasticElement::OpRhsPiolaKirchhoff(
              "U", elastic.setOfBlocks[2], elastic.commonData));
      // Left hand side operators
      fat_prism_lhs.getOpPtrVector().push_back(
          new MoFEM::OpCalculateInvJacForFatPrism(inv_jac));
      fat_prism_lhs.getOpPtrVector().push_back(
          new MoFEM::OpSetInvJacH1ForFatPrism(inv_jac));
      fat_prism_lhs.getOpPtrVector().push_back(
          new NonlinearElasticElement::OpGetCommonDataAtGaussPts(
              "U", elastic.commonData));
      fat_prism_lhs.getOpPtrVector().push_back(
          new NonlinearElasticElement::OpJacobianPiolaKirchhoffStress(
              "U", elastic.setOfBlocks[2], elastic.commonData, 2, true, false,
              true));
      fat_prism_lhs.getOpPtrVector().push_back(
          new NonlinearElasticElement::OpLhsPiolaKirchhoff_dx(
              "U", "U", elastic.setOfBlocks[2], elastic.commonData));
    }
 
    // Update material parameters
    for (_IT_CUBITMESHSETS_BY_SET_TYPE_FOR_LOOP_(m_field, BLOCKSET, it)) {
      int id = it->getMeshsetId();
      if (block_data[id].yOung > 0) {
        elastic.setOfBlocks[id].E = block_data[id].yOung;
        CHKERR PetscPrintf(PETSC_COMM_WORLD,
                           "Block %d set Young modulus %3.4g\n", id,
                           elastic.setOfBlocks[id].E);
      }
      if (block_data[id].pOisson >= -1) {
        elastic.setOfBlocks[id].PoissonRatio = block_data[id].pOisson;
        CHKERR PetscPrintf(PETSC_COMM_WORLD,
                           "Block %d set Poisson ratio %3.4g\n", id,
                           elastic.setOfBlocks[id].PoissonRatio);
      }
    }
 
    // build field
    CHKERR m_field.build_fields();
 
    // Control elements
    CHKERR m_field.add_finite_element("KUPSUPS");
    CHKERR m_field.modify_finite_element_add_field_row("KUPSUPS", "UPSILON");
    CHKERR m_field.modify_finite_element_add_field_col("KUPSUPS", "UPSILON");
    CHKERR m_field.modify_finite_element_add_field_data("KUPSUPS", "UPSILON");
    CHKERR m_field.add_ents_to_finite_element_by_type(0, MBTET, "KUPSUPS");
    CHKERR m_field.add_ents_to_finite_element_by_type(0, MBPRISM, "KUPSUPS");
 
    CHKERR m_field.add_finite_element("DISPLACEMENTS_PENALTY");
    CHKERR m_field.modify_finite_element_add_field_row("DISPLACEMENTS_PENALTY",
                                                       "UPSILON");
    CHKERR m_field.modify_finite_element_add_field_col("DISPLACEMENTS_PENALTY",
                                                       "U");
    CHKERR m_field.modify_finite_element_add_field_data("DISPLACEMENTS_PENALTY",
                                                        "UPSILON");
    CHKERR m_field.modify_finite_element_add_field_data("DISPLACEMENTS_PENALTY",
                                                        "U");
    CHKERR m_field.modify_finite_element_add_field_data("DISPLACEMENTS_PENALTY",
                                                        "DISP_X");
    CHKERR m_field.modify_finite_element_add_field_data("DISPLACEMENTS_PENALTY",
                                                        "DISP_Y");
    CHKERR m_field.add_ents_to_finite_element_by_type(ents_2nd_layer, MBTRI,
                                                      "DISPLACEMENTS_PENALTY");
 
    // Add element to calculate residual on 1st layer
    CHKERR m_field.add_finite_element("BT");
    CHKERR m_field.modify_finite_element_add_field_row("BT", "U");
    CHKERR m_field.modify_finite_element_add_field_row("BT", "UPSILON");
    CHKERR m_field.modify_finite_element_add_field_col("BT", "RHO");
    CHKERR m_field.modify_finite_element_add_field_data("BT", "U");
    CHKERR m_field.modify_finite_element_add_field_data("BT", "UPSILON");
    CHKERR m_field.modify_finite_element_add_field_data("BT", "RHO");
    CHKERR m_field.add_ents_to_finite_element_by_type(ents_1st_layer, MBTRI,
                                                      "BT");
 
    CHKERR m_field.add_finite_element("B");
    CHKERR m_field.modify_finite_element_add_field_row("B", "RHO");
    CHKERR m_field.modify_finite_element_add_field_col("B", "U");
    CHKERR m_field.modify_finite_element_add_field_col("B", "UPSILON");
    CHKERR m_field.modify_finite_element_add_field_data("B", "U");
    CHKERR m_field.modify_finite_element_add_field_data("B", "UPSILON");
    CHKERR m_field.modify_finite_element_add_field_data("B", "RHO");
    CHKERR m_field.add_ents_to_finite_element_by_type(ents_1st_layer, MBTRI,
                                                      "B");
 
    // Add element to calculate residual on 1st layer
    CHKERR m_field.add_finite_element("D");
    CHKERR m_field.modify_finite_element_add_field_row("D", "RHO");
    CHKERR m_field.modify_finite_element_add_field_col("D", "RHO");
    CHKERR m_field.modify_finite_element_add_field_data("D", "RHO");
    CHKERR m_field.add_ents_to_finite_element_by_type(ents_1st_layer, MBTRI,
                                                      "D");
 
    // build finite elemnts
    CHKERR m_field.build_finite_elements();
    // build adjacencies
    CHKERR m_field.build_adjacencies(bit_level0);
 
    // Register MOFEM DM
    DMType dm_name = "MOFEM";
    CHKERR DMRegister_MoFEM(dm_name);
 
    DM dm_control;
    {
      // Craete dm_control instance
      CHKERR DMCreate(PETSC_COMM_WORLD, &dm_control);
      CHKERR DMSetType(dm_control, dm_name);
      // set dm_control data structure which created mofem datastructures
      CHKERR DMMoFEMCreateMoFEM(dm_control, &m_field, "CONTROL_PROB",
                                bit_level0);
      CHKERR DMSetFromOptions(dm_control);
      CHKERR DMMoFEMSetSquareProblem(dm_control, PETSC_TRUE);
      CHKERR DMMoFEMSetIsPartitioned(dm_control, PETSC_TRUE);
      // add elements to dm_control
      CHKERR DMMoFEMAddElement(dm_control, "ELASTIC");
      CHKERR DMMoFEMAddElement(dm_control, "ELASTIC_PRISM");
      CHKERR DMMoFEMAddElement(dm_control, "KUPSUPS");
      CHKERR DMMoFEMAddElement(dm_control, "DISPLACEMENTS_PENALTY");
      CHKERR DMMoFEMAddElement(dm_control, "B");
      CHKERR DMMoFEMAddElement(dm_control, "BT");
      CHKERR DMMoFEMAddElement(dm_control, "D");
      CHKERR DMSetUp(dm_control);
    }
 
    MatrixDouble inv_jac;
    CellEngineering::CommonData common_data;
 
    ublas::matrix<Mat> nested_matrices(2, 2);
    ublas::vector<IS> nested_is_rows(2);
    ublas::vector<IS> nested_is_cols(2);
    for (int i = 0; i != 2; i++) {
      nested_is_rows[i] = PETSC_NULL;
      nested_is_cols[i] = PETSC_NULL;
      for (int j = 0; j != 2; j++) {
        nested_matrices(i, j) = PETSC_NULL;
      }
    }
 
    ublas::matrix<Mat> sub_nested_matrices(2, 2);
    ublas::vector<IS> sub_nested_is_rows(2);
    ublas::vector<IS> sub_nested_is_cols(2);
    for (int i = 0; i != 2; i++) {
      sub_nested_is_rows[i] = PETSC_NULL;
      sub_nested_is_cols[i] = PETSC_NULL;
      for (int j = 0; j != 2; j++) {
        sub_nested_matrices(i, j) = PETSC_NULL;
      }
    }
 
    DM dm_sub_volume_control;
    {
      CHKERR DMCreate(PETSC_COMM_WORLD, &dm_sub_volume_control);
      CHKERR DMSetType(dm_sub_volume_control, dm_name);
      // set dm_sub_volume_control data structure which created mofem data
      // structures
      CHKERR DMMoFEMCreateSubDM(dm_sub_volume_control, dm_control,
                                "SUB_CONTROL_PROB");
      CHKERR DMMoFEMSetSquareProblem(dm_sub_volume_control, PETSC_TRUE);
      CHKERR DMMoFEMAddSubFieldRow(dm_sub_volume_control, "U");
      CHKERR DMMoFEMAddSubFieldRow(dm_sub_volume_control, "UPSILON");
      CHKERR DMMoFEMAddSubFieldCol(dm_sub_volume_control, "U");
      CHKERR DMMoFEMAddSubFieldCol(dm_sub_volume_control, "UPSILON");
      // add elements to dm_sub_volume_control
      CHKERR DMSetUp(dm_sub_volume_control);
 
      const Problem *prb_ptr;
      CHKERR m_field.get_problem("SUB_CONTROL_PROB", &prb_ptr);
      boost::shared_ptr<Problem::SubProblemData> sub_data =
          prb_ptr->getSubData();
 
      CHKERR sub_data->getRowIs(&nested_is_rows[0]);
      CHKERR sub_data->getColIs(&nested_is_cols[0]);
      // That will be filled at the end
      nested_matrices(0, 0) = PETSC_NULL;
    }
 
    {
      DM dm_sub_sub_elastic;
 
      CHKERR DMCreate(PETSC_COMM_WORLD, &dm_sub_sub_elastic);
      CHKERR DMSetType(dm_sub_sub_elastic, dm_name);
      // set dm_sub_sub_elastic data structure which created mofem data
      // structures
      CHKERR DMMoFEMCreateSubDM(dm_sub_sub_elastic, dm_sub_volume_control,
                                "ELASTIC_PROB");
      CHKERR DMMoFEMSetSquareProblem(dm_sub_sub_elastic, PETSC_TRUE);
      CHKERR DMMoFEMAddElement(dm_sub_sub_elastic, "ELASTIC");
      CHKERR DMMoFEMAddElement(dm_sub_sub_elastic, "ELASTIC_PRISM");
      CHKERR DMMoFEMAddSubFieldRow(dm_sub_sub_elastic, "U");
      CHKERR DMMoFEMAddSubFieldCol(dm_sub_sub_elastic, "U");
      // add elements to dm_sub_sub_elastic
      CHKERR DMSetUp(dm_sub_sub_elastic);
 
      Mat Kuu;
      Vec Du, Fu;
      CHKERR DMCreateMatrix(dm_sub_sub_elastic, &Kuu);
      CHKERR DMCreateGlobalVector(dm_sub_sub_elastic, &Du);
      CHKERR DMCreateGlobalVector(dm_sub_sub_elastic, &Fu);
      CHKERR MatZeroEntries(Kuu);
      CHKERR VecZeroEntries(Du);
      CHKERR VecZeroEntries(Fu);
      CHKERR DMoFEMMeshToLocalVector(dm_sub_sub_elastic, Du, INSERT_VALUES,
                                     SCATTER_REVERSE);
 
      // Manage  Dirichlet bC
      boost::shared_ptr<FEMethod> dirihlet_bc_ptr;
      dirihlet_bc_ptr = boost::shared_ptr<FEMethod>(
          new DirichletDisplacementBc(m_field, "U", Kuu, Du, Fu));
      dirihlet_bc_ptr->snes_ctx = FEMethod::CTX_SNESNONE;
      dirihlet_bc_ptr->ts_ctx = FEMethod::CTX_TSNONE;
      // preproc
      CHKERR DMoFEMPreProcessFiniteElements(dm_sub_sub_elastic,
                                            dirihlet_bc_ptr.get());
      // internal force vector (to take into account  Dirichlet boundary
      // conditions)
      elastic.getLoopFeRhs().snes_f = Fu;
      fat_prism_rhs.snes_f = Fu;
      CHKERR DMoFEMLoopFiniteElements(dm_sub_sub_elastic, "ELASTIC",
                                      &elastic.getLoopFeRhs());
      CHKERR DMoFEMLoopFiniteElements(dm_sub_sub_elastic, "ELASTIC_PRISM",
                                      &fat_prism_rhs);
      // elastic element matrix
      elastic.getLoopFeLhs().snes_B = Kuu;
      fat_prism_lhs.snes_B = Kuu;
      CHKERR DMoFEMLoopFiniteElements(dm_sub_sub_elastic, "ELASTIC",
                                      &elastic.getLoopFeLhs());
      CHKERR DMoFEMLoopFiniteElements(dm_sub_sub_elastic, "ELASTIC_PRISM",
                                      &fat_prism_lhs);
      // postproc
      CHKERR DMoFEMPostProcessFiniteElements(dm_sub_sub_elastic,
                                             dirihlet_bc_ptr.get());
      CHKERR VecGhostUpdateBegin(Fu, ADD_VALUES, SCATTER_REVERSE);
      CHKERR VecGhostUpdateEnd(Fu, ADD_VALUES, SCATTER_REVERSE);
      CHKERR VecAssemblyBegin(Fu);
      CHKERR VecAssemblyEnd(Fu);
      CHKERR VecScale(Fu, -1);
      CHKERR VecDestroy(&Du);
      CHKERR VecDestroy(&Fu);
 
      const Problem *prb_ptr;
      CHKERR m_field.get_problem("ELASTIC_PROB", &prb_ptr);
      boost::shared_ptr<Problem::SubProblemData> sub_data =
          prb_ptr->getSubData();
 
      CHKERR sub_data->getRowIs(&sub_nested_is_rows[0]);
      CHKERR sub_data->getColIs(&sub_nested_is_cols[0]);
      sub_nested_matrices(0, 0) = Kuu;
      IS isUpsilon;
      CHKERR m_field.query_interface<ISManager>()
          ->isCreateFromProblemFieldToOtherProblemField(
              "ELASTIC_PROB", "U", ROW, "SUB_CONTROL_PROB", "UPSILON", ROW,
              PETSC_NULL, &isUpsilon);
      sub_nested_is_rows[1] = isUpsilon;
      sub_nested_is_cols[1] = isUpsilon;
      sub_nested_matrices(1, 1) = Kuu;
      PetscObjectReference((PetscObject)Kuu);
      PetscObjectReference((PetscObject)isUpsilon);
 
      // Matrix View
      if (debug) {
        cerr << "Kuu" << endl;
        MatView(Kuu, PETSC_VIEWER_DRAW_WORLD);
        std::string wait;
        std::cin >> wait;
      }
 
      CHKERR DMDestroy(&dm_sub_sub_elastic);
    }
 
    {
      DM dm_sub_disp_penalty;
 
      // Craete dm_control instance
      CHKERR DMCreate(PETSC_COMM_WORLD, &dm_sub_disp_penalty);
      CHKERR DMSetType(dm_sub_disp_penalty, dm_name);
      // set dm_sub_disp_penalty data structure which created mofem
      // datastructures
      CHKERR DMMoFEMCreateSubDM(dm_sub_disp_penalty, dm_sub_volume_control,
                                "S_PROB");
      CHKERR DMMoFEMSetSquareProblem(dm_sub_disp_penalty, PETSC_FALSE);
      CHKERR DMMoFEMAddSubFieldRow(dm_sub_disp_penalty, "UPSILON");
      CHKERR DMMoFEMAddSubFieldCol(dm_sub_disp_penalty, "U");
      // add elements to dm_sub_disp_penalty
      CHKERR DMMoFEMAddElement(dm_sub_disp_penalty, "DISPLACEMENTS_PENALTY");
      CHKERR DMSetUp(dm_sub_disp_penalty);
 
      Mat S;
      CHKERR DMCreateMatrix(dm_sub_disp_penalty, &S);
      CHKERR MatZeroEntries(S);
 
      CellEngineering::FaceElement face_element(m_field);
      face_element.getOpPtrVector().push_back(new OpCellS(S, eps_u));
      CHKERR DMoFEMLoopFiniteElements(dm_sub_disp_penalty,
                                      "DISPLACEMENTS_PENALTY", &face_element);
      CHKERR MatAssemblyBegin(S, MAT_FLUSH_ASSEMBLY);
      CHKERR MatAssemblyEnd(S, MAT_FLUSH_ASSEMBLY);
 
      // // Matrix View
      if (debug) {
        cerr << "S" << endl;
        MatView(S, PETSC_VIEWER_DRAW_WORLD);
        std::string wait;
        std::cin >> wait;
      }
 
      const Problem *problem_ptr;
      CHKERR m_field.get_problem("S_PROB", &problem_ptr);
      boost::shared_ptr<Problem::SubProblemData> sub_data =
          problem_ptr->getSubData();
      // CHKERR sub_data->getRowIs(&sub_nested_is_rows[1]);
      // CHKERR sub_data->getColIs(&sub_nested_is_cols[0]);
      sub_nested_matrices(1, 0) = S;
 
      CHKERR DMDestroy(&dm_sub_disp_penalty);
    }
 
    // Calculate penalty matrix
    {
      DM dm_sub_force_penalty;
 
      // Craete dm_control instance
      CHKERR DMCreate(PETSC_COMM_WORLD, &dm_sub_force_penalty);
      CHKERR DMSetType(dm_sub_force_penalty, dm_name);
      // set dm_sub_force_penalty data structure which created mofem
      // datastructures
      CHKERR DMMoFEMCreateSubDM(dm_sub_force_penalty, dm_control, "D_PROB");
      CHKERR DMMoFEMSetSquareProblem(dm_sub_force_penalty, PETSC_TRUE);
      CHKERR DMMoFEMAddSubFieldRow(dm_sub_force_penalty, "RHO");
      CHKERR DMMoFEMAddSubFieldCol(dm_sub_force_penalty, "RHO");
      // add elements to dm_sub_force_penalty
      CHKERR DMMoFEMAddElement(dm_sub_force_penalty, "D");
      CHKERR DMSetUp(dm_sub_force_penalty);
 
      Mat D;
      CHKERR DMCreateMatrix(dm_sub_force_penalty, &D);
      CHKERR MatZeroEntries(D);
 
      {
        CellEngineering::FaceElement face_d_matrix(m_field);
        face_d_matrix.getOpPtrVector().push_back(
            new OpCalculateInvJacForFace(inv_jac));
        if (is_curl) {
          face_d_matrix.getOpPtrVector().push_back(
              new OpSetInvJacHcurlFace(inv_jac));
          face_d_matrix.getOpPtrVector().push_back(
              new OpCellCurlD(D, eps_rho / eps_u, eps_l * eps_u));
        } else {
          face_d_matrix.getOpPtrVector().push_back(
              new OpSetInvJacH1ForFace(inv_jac));
          face_d_matrix.getOpPtrVector().push_back(
              new OpCellPotentialD(D, eps_rho / eps_u));
        }
        CHKERR DMoFEMLoopFiniteElements(dm_sub_force_penalty, "D",
                                        &face_d_matrix);
      }
      CHKERR MatAssemblyBegin(D, MAT_FINAL_ASSEMBLY);
      CHKERR MatAssemblyEnd(D, MAT_FINAL_ASSEMBLY);
 
      const Problem *problem_ptr;
      CHKERR m_field.get_problem("D_PROB", &problem_ptr);
 
      // Zero rows, force field is given by gradients of potential field, so one
      // of the values has to be fixed like for rigid body motion.
      if (is_curl == PETSC_FALSE) {
        int nb_dofs_to_fix = 0;
        int index_to_fix = 0;
        if (!vertex_to_fix.empty()) {
          boost::shared_ptr<NumeredDofEntity> dof_ptr;
          CHKERR problem_ptr->getDofByNameEntAndEntDofIdx(
              m_field.get_field_bit_number("RHO"), vertex_to_fix[0], 0, ROW,
              dof_ptr);
          if (dof_ptr) {
            if (dof_ptr->getPart() == m_field.get_comm_rank()) {
              nb_dofs_to_fix = 1;
              index_to_fix = dof_ptr->getPetscGlobalDofIdx();
              cerr << *dof_ptr << endl;
            }
          }
        }
        CHKERR MatZeroRowsColumns(D, nb_dofs_to_fix, &index_to_fix,
                                  eps_rho / eps_u, PETSC_NULL, PETSC_NULL);
      } else {
        std::vector<int> dofs_to_fix;
        for (auto p_eit = edges_to_fix.pair_begin();
             p_eit != edges_to_fix.pair_end(); ++p_eit) {
          auto bit_number = m_field.get_field_bit_number("RHO");
          auto row_dofs = problem_ptr->numeredRowDofsPtr;
          auto lo = row_dofs->lower_bound(
              FieldEntity::getLoLocalEntityBitNumber(bit_number, p_eit->first));
          auto hi =
              row_dofs->lower_bound(FieldEntity::getHiLocalEntityBitNumber(
                  bit_number, p_eit->second));
          for (; lo != hi; ++lo)
            dofs_to_fix.push_back((*lo)->getPetscGlobalDofIdx());
        }
        CHKERR MatZeroRowsColumns(D, dofs_to_fix.size(), &*dofs_to_fix.begin(),
                                  eps_rho / eps_u, PETSC_NULL, PETSC_NULL);
      }
 
      // Matrix View
      if (debug) {
        cerr << "D" << endl;
        MatView(D, PETSC_VIEWER_DRAW_WORLD);
        std::string wait;
        std::cin >> wait;
      }
 
      boost::shared_ptr<Problem::SubProblemData> sub_data =
          problem_ptr->getSubData();
      CHKERR sub_data->getRowIs(&nested_is_rows[1]);
      CHKERR sub_data->getColIs(&nested_is_cols[1]);
      nested_matrices(1, 1) = D;
 
      CHKERR DMDestroy(&dm_sub_force_penalty);
    }
 
    {
      DM dm_sub_force;
 
      // Craete dm_control instance
      CHKERR DMCreate(PETSC_COMM_WORLD, &dm_sub_force);
      CHKERR DMSetType(dm_sub_force, dm_name);
      // set dm_sub_force data structure which created mofem data structures
      CHKERR DMMoFEMCreateSubDM(dm_sub_force, dm_control, "FORCES_PROB");
      CHKERR DMMoFEMSetSquareProblem(dm_sub_force, PETSC_FALSE);
      CHKERR DMMoFEMAddSubFieldRow(dm_sub_force, "RHO");
      CHKERR DMMoFEMAddSubFieldCol(dm_sub_force, "U");
      CHKERR DMMoFEMAddSubFieldCol(dm_sub_force, "UPSILON");
      // add elements to dm_sub_force
      CHKERR DMMoFEMAddElement(dm_sub_force, "B");
      CHKERR DMSetUp(dm_sub_force);
 
      Mat UB, UPSILONB;
      CHKERR DMCreateMatrix(dm_sub_force, &UB);
      CHKERR MatZeroEntries(UB);
      // note be will be transposed in place latter on
      CHKERR DMCreateMatrix(dm_sub_force, &UPSILONB);
      CHKERR MatZeroEntries(UPSILONB);
      {
        CellEngineering::FaceElement face_b_matrices(m_field);
        face_b_matrices.getOpPtrVector().push_back(
            new OpCalculateInvJacForFace(inv_jac));
        if (is_curl) {
          face_b_matrices.getOpPtrVector().push_back(
              new OpSetInvJacH1ForFace(inv_jac));
          face_b_matrices.getOpPtrVector().push_back(
              new OpSetInvJacHcurlFace(inv_jac));
          face_b_matrices.getOpPtrVector().push_back(new OpCellCurlB(UB, "U"));
          face_b_matrices.getOpPtrVector().push_back(
              new OpCellCurlB(UPSILONB, "UPSILON"));
        } else {
          face_b_matrices.getOpPtrVector().push_back(
              new OpSetInvJacH1ForFace(inv_jac));
          face_b_matrices.getOpPtrVector().push_back(
              new OpCellPotentialB(UB, "U"));
          face_b_matrices.getOpPtrVector().push_back(
              new OpCellPotentialB(UPSILONB, "UPSILON"));
        }
        CHKERR DMoFEMLoopFiniteElements(dm_sub_force, "B", &face_b_matrices);
      }
      CHKERR MatAssemblyBegin(UB, MAT_FINAL_ASSEMBLY);
      CHKERR MatAssemblyBegin(UPSILONB, MAT_FINAL_ASSEMBLY);
      CHKERR MatAssemblyEnd(UB, MAT_FINAL_ASSEMBLY);
      CHKERR MatAssemblyEnd(UPSILONB, MAT_FINAL_ASSEMBLY);
 
      const Problem *problem_ptr;
      CHKERR m_field.get_problem("FORCES_PROB", &problem_ptr);
 
      // Zero rows, force field is given by gradients of potential field, so one
      // of the values has to be fixed like for rigid body motion.
      if (is_curl == PETSC_FALSE) {
        int nb_dofs_to_fix = 0;
        int index_to_fix = 0;
        if (!vertex_to_fix.empty()) {
          boost::shared_ptr<NumeredDofEntity> dof_ptr;
          CHKERR problem_ptr->getDofByNameEntAndEntDofIdx(
              m_field.get_field_bit_number("RHO"), vertex_to_fix[0], 0, ROW,
              dof_ptr);
          if (dof_ptr) {
            if (dof_ptr->getPart() == m_field.get_comm_rank()) {
              nb_dofs_to_fix = 1;
              index_to_fix = dof_ptr->getPetscGlobalDofIdx();
              cerr << *dof_ptr << endl;
            }
          }
        }
        CHKERR MatZeroRows(UB, nb_dofs_to_fix, &index_to_fix, 0, PETSC_NULL,
                           PETSC_NULL);
        CHKERR MatZeroRows(UPSILONB, nb_dofs_to_fix, &index_to_fix, 0,
                           PETSC_NULL, PETSC_NULL);
      } else {
        std::vector<int> dofs_to_fix;
        for (auto p_eit = edges_to_fix.pair_begin();
             p_eit != edges_to_fix.pair_end(); ++p_eit) {
          auto bit_number = m_field.get_field_bit_number("RHO");
          auto row_dofs = problem_ptr->numeredRowDofsPtr;
          auto lo = row_dofs->lower_bound(
              FieldEntity::getLoLocalEntityBitNumber(bit_number, p_eit->first));
          auto hi =
              row_dofs->lower_bound(FieldEntity::getHiLocalEntityBitNumber(
                  bit_number, p_eit->second));
          for (; lo != hi; ++lo)
            dofs_to_fix.push_back((*lo)->getPetscGlobalDofIdx());
        }
        CHKERR MatZeroRows(UB, dofs_to_fix.size(), &*dofs_to_fix.begin(), 0,
                           PETSC_NULL, PETSC_NULL);
        CHKERR MatZeroRows(UPSILONB, dofs_to_fix.size(), &*dofs_to_fix.begin(),
                           0, PETSC_NULL, PETSC_NULL);
      }
 
      Mat UBT;
      CHKERR MatTranspose(UB, MAT_INITIAL_MATRIX, &UBT);
      CHKERR MatDestroy(&UB);
 
      // Matrix View
      if (debug) {
        cerr << "UBT" << endl;
        MatView(UBT, PETSC_VIEWER_DRAW_WORLD);
        std::string wait;
        std::cin >> wait;
      }
 
      boost::shared_ptr<Problem::SubProblemData> sub_data =
          problem_ptr->getSubData();
      // CHKERR sub_data->getColIs(&nested_is_rows[0]);
      // CHKERR sub_data->getRowIs(&nested_is_cols[1]);
      nested_matrices(0, 1) = UBT;
 
      if (debug) {
        cerr << "UPSILONB" << endl;
        MatView(UPSILONB, PETSC_VIEWER_DRAW_WORLD);
        std::string wait;
        std::cin >> wait;
      }
 
      // CHKERR sub_data->getRowIs(&nested_is_rows[1]);
      // CHKERR sub_data->getColIs(&nested_is_cols[0]);
      nested_matrices(1, 0) = UPSILONB;
 
      CHKERR DMDestroy(&dm_sub_force);
    }
 
    Mat SubA;
    CHKERR MatCreateNest(PETSC_COMM_WORLD, 2, &sub_nested_is_rows[0], 2,
                         &sub_nested_is_cols[0], &sub_nested_matrices(0, 0),
                         &SubA);
    nested_matrices(0, 0) = SubA;
 
    CHKERR MatAssemblyBegin(SubA, MAT_FINAL_ASSEMBLY);
    CHKERR MatAssemblyEnd(SubA, MAT_FINAL_ASSEMBLY);
 
    if (debug) {
      cerr << "Nested SubA" << endl;
      MatView(SubA, PETSC_VIEWER_STDOUT_WORLD);
    }
 
    Mat A;
    CHKERR MatCreateNest(PETSC_COMM_WORLD, 2, &nested_is_rows[0], 2,
                         &nested_is_cols[0], &nested_matrices(0, 0), &A);
 
    CHKERR MatAssemblyBegin(A, MAT_FINAL_ASSEMBLY);
    CHKERR MatAssemblyEnd(A, MAT_FINAL_ASSEMBLY);
 
    if (debug) {
      cerr << "Nested A" << endl;
      MatView(A, PETSC_VIEWER_STDOUT_WORLD);
    }
 
    Vec D, F;
    CHKERR DMCreateGlobalVector(dm_control, &D);
    CHKERR DMCreateGlobalVector(dm_control, &F);
 
    // Asseble the right hand side vector
    {
      CellEngineering::FaceElement face_element(m_field);
      face_element.getOpPtrVector().push_back(new OpGetDispX(common_data));
      face_element.getOpPtrVector().push_back(new OpGetDispY(common_data));
      face_element.getOpPtrVector().push_back(
          new OpCell_g(F, eps_u, common_data));
      CHKERR DMoFEMLoopFiniteElements(dm_control, "DISPLACEMENTS_PENALTY",
                                      &face_element);
      CHKERR VecGhostUpdateBegin(F, ADD_VALUES, SCATTER_REVERSE);
      CHKERR VecGhostUpdateEnd(F, ADD_VALUES, SCATTER_REVERSE);
      CHKERR VecAssemblyBegin(F);
      CHKERR VecAssemblyEnd(F);
    }
 
    KSP solver;
    // KSP solver;
    {
      CHKERR KSPCreate(PETSC_COMM_WORLD, &solver);
      CHKERR KSPSetDM(solver, dm_control);
      CHKERR KSPSetFromOptions(solver);
      CHKERR KSPSetOperators(solver, A, A);
      CHKERR KSPSetDMActive(solver, PETSC_FALSE);
      CHKERR KSPSetInitialGuessKnoll(solver, PETSC_FALSE);
      CHKERR KSPSetInitialGuessNonzero(solver, PETSC_FALSE);
      PC pc;
      CHKERR KSPGetPC(solver, &pc);
      CHKERR PCSetType(pc, PCFIELDSPLIT);
      PetscBool is_pcfs = PETSC_FALSE;
      PetscObjectTypeCompare((PetscObject)pc, PCFIELDSPLIT, &is_pcfs);
      if (is_pcfs) {
        CHKERR PCSetOperators(pc, A, A);
        CHKERR PCFieldSplitSetIS(pc, NULL, nested_is_rows[0]);
        CHKERR PCFieldSplitSetIS(pc, NULL, nested_is_rows[1]);
        CHKERR PCFieldSplitSetType(pc, PC_COMPOSITE_SCHUR);
        CHKERR PCSetUp(pc);
        KSP *sub_ksp;
        PetscInt n;
        CHKERR PCFieldSplitGetSubKSP(pc, &n, &sub_ksp);
        {
          PC sub_pc_0;
          CHKERR KSPGetPC(sub_ksp[0], &sub_pc_0);
          CHKERR PCSetOperators(sub_pc_0, SubA, SubA);
          CHKERR PCSetType(sub_pc_0, PCFIELDSPLIT);
          CHKERR PCFieldSplitSetIS(sub_pc_0, NULL, sub_nested_is_rows[0]);
          CHKERR PCFieldSplitSetIS(sub_pc_0, NULL, sub_nested_is_rows[1]);
          CHKERR PCFieldSplitSetType(sub_pc_0, PC_COMPOSITE_MULTIPLICATIVE);
          // CHKERR
          // PCFieldSplitSetSchurFactType(sub_pc_0,PC_FIELDSPLIT_SCHUR_FACT_LOWER);
          // CHKERR PCFieldSplitSetType(sub_pc_0,PC_COMPOSITE_SCHUR);
          CHKERR PCSetUp(sub_pc_0);
        }
      } else {
        SETERRQ(PETSC_COMM_WORLD, MOFEM_DATA_INCONSISTENCY,
                "Only works with pre-conditioner PCFIELDSPLIT");
      }
      CHKERR KSPSetUp(solver);
    }
 
    // Solve system of equations
    CHKERR KSPSolve(solver, F, D);
 
    CHKERR VecGhostUpdateBegin(D, INSERT_VALUES, SCATTER_FORWARD);
    CHKERR VecGhostUpdateEnd(D, INSERT_VALUES, SCATTER_FORWARD);
    CHKERR DMoFEMMeshToLocalVector(dm_control, D, INSERT_VALUES,
                                   SCATTER_REVERSE);
 
    if (debug) {
      CHKERR VecView(D, PETSC_VIEWER_DRAW_WORLD);
      std::string wait;
      std::cin >> wait;
    }
 
    // Clean sub matrices and sub indices
    for (int i = 0; i != 2; i++) {
      if (sub_nested_is_rows[i]) {
        CHKERR ISDestroy(&sub_nested_is_rows[i]);
      }
      if (sub_nested_is_cols[i]) {
        CHKERR ISDestroy(&sub_nested_is_cols[i]);
      }
      for (int j = 0; j != 2; j++) {
        if (sub_nested_matrices(i, j)) {
          CHKERR MatDestroy(&sub_nested_matrices(i, j));
        }
      }
    }
    for (int i = 0; i != 2; i++) {
      if (nested_is_rows[i]) {
        CHKERR ISDestroy(&nested_is_rows[i]);
      }
      if (nested_is_cols[i]) {
        CHKERR ISDestroy(&nested_is_cols[i]);
      }
      for (int j = 0; j != 2; j++) {
        if (nested_matrices(i, j)) {
          CHKERR MatDestroy(&nested_matrices(i, j));
        }
      }
    }
 
    CHKERR MatDestroy(&SubA);
    CHKERR MatDestroy(&A);
    CHKERR VecDestroy(&D);
    CHKERR VecDestroy(&F);
 
    CHKERR DMDestroy(&dm_sub_volume_control);
 
    PostProcVolumeOnRefinedMesh post_proc(m_field);
    {
      CHKERR post_proc.generateReferenceElementMesh();
      CHKERR post_proc.addFieldValuesPostProc("U");
      CHKERR post_proc.addFieldValuesGradientPostProc("U");
      // add postpocessing for sresses
      post_proc.getOpPtrVector().push_back(new PostProcHookStress(
          m_field, post_proc.postProcMesh, post_proc.mapGaussPts, "U",
          post_proc.commonData, &elastic.setOfBlocks));
      CHKERR DMoFEMLoopFiniteElements(dm_control, "ELASTIC", &post_proc);
      CHKERR post_proc.writeFile("out.h5m");
      elastic.getLoopFeEnergy().snes_ctx = SnesMethod::CTX_SNESNONE;
      elastic.getLoopFeEnergy().eNergy = 0;
      CHKERR DMoFEMLoopFiniteElements(dm_control, "ELASTIC",
                                      &elastic.getLoopFeEnergy());
      PetscPrintf(PETSC_COMM_WORLD, "Elastic energy %6.4e\n",
                  elastic.getLoopFeEnergy().eNergy);
    }
 
    {
      PostProcFaceOnRefinedMesh post_proc_face(m_field);
      CHKERR post_proc_face.generateReferenceElementMesh();
      post_proc_face.getOpPtrVector().push_back(
          new OpCalculateInvJacForFace(inv_jac));
      if (is_curl) {
        post_proc_face.getOpPtrVector().push_back(
            new OpSetInvJacHcurlFace(inv_jac));
        post_proc_face.getOpPtrVector().push_back(
            new OpVirtualCurlRho("RHO", common_data));
      } else {
        post_proc_face.getOpPtrVector().push_back(
            new OpSetInvJacH1ForFace(inv_jac));
        post_proc_face.getOpPtrVector().push_back(
            new OpVirtualPotentialRho("RHO", common_data));
      }
      post_proc_face.getOpPtrVector().push_back(
          new PostProcTraction(m_field, post_proc_face.postProcMesh,
                               post_proc_face.mapGaussPts, common_data));
      CHKERR DMoFEMLoopFiniteElements(dm_control, "D", &post_proc_face);
      CHKERR post_proc_face.writeFile("out_tractions.h5m");
    }
 
    CHKERR DMDestroy(&dm_control);
 
  }
  CATCH_ERRORS;
 
  MoFEM::Core::Finalize();
  return 0;
}