Remodeling.cpp

Integration points are taken from OOFEM http://www.oofem.org/resources/doc/oofemrefman/microplanematerial_8C_source.html

Integration points are taken from OOFEM http://www.oofem.org/resources/doc/oofemrefman/microplanematerial_8C_source.html

/**
 * \file Remodeling.cpp
 * \example Remodeling.cpp
 *
 * \brief Integration points are taken from OOFEM
 * <http://www.oofem.org/resources/doc/oofemrefman/microplanematerial_8C_source.html>
 *
 */
 
/*
 * 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 <boost/program_options.hpp>
using namespace std;
namespace po = boost::program_options;
 
#include <BasicFiniteElements.hpp>
using namespace MoFEM;
#include <ElasticMaterials.hpp>
#include <Remodeling.hpp>
 
#ifndef WITH_ADOL_C
#error "MoFEM need to be compiled with ADOL-C"
#endif
 
#define TENSOR1_VEC_PTR(VEC) &VEC[0], &VEC[1], &VEC[2]
 
#define SYMMETRIC_TENSOR4_MAT_PTR(MAT)                                         \
  &MAT(0, 0), &MAT(0, 1), &MAT(0, 2), &MAT(0, 3), &MAT(0, 4), &MAT(0, 5),      \
      &MAT(1, 0), &MAT(1, 1), &MAT(1, 2), &MAT(1, 3), &MAT(1, 4), &MAT(1, 5),  \
      &MAT(2, 0), &MAT(2, 1), &MAT(2, 2), &MAT(2, 3), &MAT(2, 4), &MAT(2, 5),  \
      &MAT(3, 0), &MAT(3, 1), &MAT(3, 2), &MAT(3, 3), &MAT(3, 4), &MAT(3, 5),  \
      &MAT(4, 0), &MAT(4, 1), &MAT(4, 2), &MAT(4, 3), &MAT(4, 4), &MAT(4, 5),  \
      &MAT(5, 0), &MAT(5, 1), &MAT(5, 2), &MAT(5, 3), &MAT(5, 4), &MAT(5, 5)
 
#define TENSOR4_MAT_PTR(MAT) &MAT(0, 0), MAT.size2()
 
#define TENSOR2_MAT_PTR(MAT)                                                   \
  &MAT(0, 0), &MAT(1, 0), &MAT(2, 0), &MAT(3, 0), &MAT(4, 0), &MAT(5, 0),      \
      &MAT(6, 0), &MAT(7, 0), &MAT(8, 0)
 
#define SYMMETRIC_TENSOR2_MAT_PTR(MAT)                                         \
  &MAT(0, 0), &MAT(0, 1), &MAT(0, 2), &MAT(0, 3), &MAT(0, 4), &MAT(0, 5)
 
#define SYMMETRIC_TENSOR2_VEC_PTR(VEC)                                         \
  &VEC[0], &VEC[1], &VEC[2], &VEC[3], &VEC[4], &VEC[5]
 
namespace BoneRemodeling {
 
MoFEMErrorCode Remodeling::CommonData::getParameters() {
 
  PetscBool flg_config = PETSC_FALSE;
  is_atom_testing = PETSC_FALSE;
  with_adol_c = PETSC_FALSE;
  char my_config_file_name[255];
  double young_modulus = 5.0e+9; // Set here some typical value for bone 5 GPa
  double poisson_ratio = 0.3;    // Set here some typical value for bone
  c = 4.0e-5;       // Set here some typical value for bone. days/ m2 // this
                    // parameters governs how fast we can achieve equilibrium
  m = 3.25;         // Set some parameter typical for bone
  n = 2.25;         // Set Some parameter typical for bone
  rHo_ref = 1000;   // Set Some parameter typical for bone. kg/m3
  pSi_ref = 2.75e4; // Set Some parameter typical for bone. energy / volume unit
                    // J/m3 = Pa
  rHo_max = rHo_ref + 0.5 * rHo_ref;
  rHo_min = rHo_ref - 0.5 * rHo_ref;
  b = 0;
  R0 = 0.0; // Set Some parameter typical for bone.
  cUrrent_psi = 0.0;
  cUrrent_mass = 0.0;
  nOremodellingBlock = false;
  less_post_proc = PETSC_FALSE;
 
  MoFEMFunctionBegin;
  CHKERR PetscOptionsBegin(PETSC_COMM_WORLD, "", "Bone remodeling parameters",
                           "none");
 
  // config file
  CHKERR PetscOptionsString("-my_config", "configuration file name", "",
                            "my_config.in", my_config_file_name, 255,
                            &flg_config);
 
  if (flg_config) {
    CHKERR PetscPrintf(PETSC_COMM_WORLD, "Config file: %s loaded.\n",
                       my_config_file_name);
    try {
      ifstream ini_file(my_config_file_name);
      if (!ini_file.is_open()) {
        SETERRQ(PETSC_COMM_SELF, 1, "*** -my_config does not exist ***");
      }
      po::variables_map vm;
      po::options_description config_file_options;
      // std::cout << my_config_file_name << std::endl;
 
      config_file_options.add_options()(
          "young_modulus", po::value<double>(&young_modulus)->default_value(1));
      config_file_options.add_options()(
          "poisson_ratio", po::value<double>(&poisson_ratio)->default_value(1));
      config_file_options.add_options()(
          "c", po::value<double>(&c)->default_value(1));
      config_file_options.add_options()(
          "m", po::value<double>(&m)->default_value(1));
      config_file_options.add_options()(
          "n", po::value<double>(&n)->default_value(1));
      config_file_options.add_options()(
          "rHo_ref", po::value<double>(&rHo_ref)->default_value(1));
      config_file_options.add_options()(
          "pSi_ref", po::value<double>(&pSi_ref)->default_value(1));
      config_file_options.add_options()(
          "R0", po::value<double>(&R0)->default_value(1));
 
      po::parsed_options parsed =
          parse_config_file(ini_file, config_file_options, true);
      store(parsed, vm);
      po::notify(vm);
 
    } 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());
    }
  }
 
  CHKERR PetscOptionsScalar("-young_modulus", "get young modulus", "",
                            young_modulus, &young_modulus, PETSC_NULL);
 
  CHKERR PetscOptionsScalar("-poisson_ratio", "get poisson_ratio", "",
                            poisson_ratio, &poisson_ratio, PETSC_NULL);
 
  CHKERR PetscOptionsScalar("-c", "density evolution (growth) velocity [d/m^2]",
                            "", c, &c, PETSC_NULL);
 
  CHKERR PetscOptionsScalar("-m", "algorithmic exponent", "", m, &m,
                            PETSC_NULL);
 
  CHKERR PetscOptionsScalar("-n", "porosity exponent", "", n, &n, PETSC_NULL);
 
  CHKERR PetscOptionsInt("-b", "bell function exponent", "", b, &b, PETSC_NULL);
 
  CHKERR PetscOptionsScalar("-rho_ref", "reference bone density", "", rHo_ref,
                            &rHo_ref, PETSC_NULL);
 
  CHKERR PetscOptionsScalar("-rho_max", "reference bone density", "", rHo_max,
                            &rHo_max, PETSC_NULL);
  CHKERR PetscOptionsScalar("-rho_min", "reference bone density", "", rHo_min,
                            &rHo_min, PETSC_NULL);
 
  CHKERR PetscOptionsScalar("-psi_ref", "reference energy density", "", pSi_ref,
                            &pSi_ref, PETSC_NULL);
 
  CHKERR PetscOptionsScalar("-r0", "mass source", "", R0, &R0, PETSC_NULL);
 
  CHKERR PetscOptionsBool("-my_is_atom_test",
                          "is used with testing, exit with error when diverged",
                          "", is_atom_testing, &is_atom_testing, PETSC_NULL);
 
  CHKERR PetscOptionsBool("-less_post_proc",
                          "is used to reduce output file size", "",
                          less_post_proc, &less_post_proc, PETSC_NULL);
 
  CHKERR PetscOptionsBool(
      "-with_adolc",
      "calculate the material tangent with automatic differentiation", "",
      with_adol_c, &with_adol_c, PETSC_NULL);
 
  lambda = LAMBDA(young_modulus, poisson_ratio);
  mu = MU(young_modulus, poisson_ratio);
 
  CHKERR PetscPrintf(PETSC_COMM_WORLD,
                     "Young's modulus E[Pa]:                %4.3g\n",
                     young_modulus);
  CHKERR PetscPrintf(PETSC_COMM_WORLD,
                     "Poisson ratio nu[-]                   %4.3g\n",
                     poisson_ratio);
  CHKERR PetscPrintf(PETSC_COMM_WORLD,
                     "Lame coefficient lambda[Pa]:          %4.3g\n", lambda);
  CHKERR PetscPrintf(PETSC_COMM_WORLD,
                     "Lame coefficient mu[Pa]:              %4.3g\n", mu);
  CHKERR PetscPrintf(PETSC_COMM_WORLD,
                     "Density growth velocity [d/m2]        %4.3g\n", c);
  CHKERR PetscPrintf(PETSC_COMM_WORLD,
                     "Algorithmic exponent m[-]:            %4.3g\n", m);
  CHKERR PetscPrintf(PETSC_COMM_WORLD,
                     "Porosity exponent n[-]:               %4.3g\n", n);
  CHKERR PetscPrintf(PETSC_COMM_WORLD,
                     "Reference density rHo_ref[kg/m3]:     %4.3g\n", rHo_ref);
  CHKERR PetscPrintf(PETSC_COMM_WORLD,
                     "Reference energy pSi_ref[Pa]:         %4.3g\n", pSi_ref);
  CHKERR PetscPrintf(PETSC_COMM_WORLD,
                     "Mass conduction R0[kg/m3s]:           %4.3g\n", R0);
  CHKERR PetscPrintf(PETSC_COMM_WORLD,
                     "Bell function coefficent b[-]:        %4.3g\n", b);
  if (b != 0) {
    CHKERR PetscPrintf(PETSC_COMM_WORLD,
                       "Max density rHo_max[kg/m3]:         %4.3g\n", rHo_max);
    CHKERR PetscPrintf(PETSC_COMM_WORLD,
                       "Min density rHo_min[kg/m3]:         %4.3g\n", rHo_min);
  }
 
  ierr = PetscOptionsEnd();
  CHKERRQ(ierr);
 
  MoFEMFunctionReturn(0);
}
 
inline double Remodeling::CommonData::getCFromDensity(const double &rho) {
  double mid_rho = 0.5 * (rHo_max + rHo_min);
  double var_h = (rho - mid_rho) / (rHo_max - mid_rho);
 
  return 1 / (1 + (b != 0) * pow(var_h, 2 * b));
}
 
inline double Remodeling::CommonData::getCFromDensityDiff(const double &rho) {
  double mid_rho = 0.5 * (rHo_max + rHo_min);
  double var_h = (rho - mid_rho) / (rHo_max - mid_rho);
  return (b != 0) * (-2) * b * pow(var_h, 2 * b - 1) /
         ((rHo_max - mid_rho) * pow(pow(var_h, 2 * b) + 1, 2));
}
 
OpGetRhoTimeDirevative::OpGetRhoTimeDirevative(
    Remodeling::CommonData &common_data)
    : VolumeElementForcesAndSourcesCore::UserDataOperator(
          "RHO", UserDataOperator::OPROW),
      commonData(common_data) {}
 
MoFEMErrorCode
OpGetRhoTimeDirevative::doWork(int side, EntityType type,
                               DataForcesAndSourcesCore::EntData &data) {
 
  MoFEMFunctionBegin;
 
  const int nb_gauss_pts = data.getN().size1();
  if (!nb_gauss_pts)
    MoFEMFunctionReturnHot(0);
  const int nb_base_functions = data.getN().size2();
 
  auto base_function = data.getFTensor0N();
  commonData.data.vecRhoDt.resize(nb_gauss_pts, false);
  if (type == MBVERTEX) {
    commonData.data.vecRhoDt.clear();
  }
 
  int nb_dofs = data.getIndices().size();
  if (nb_dofs == 0)
    MoFEMFunctionReturnHot(0);
  double *array;
  CHKERR VecGetArray(getFEMethod()->ts_u_t, &array);
  auto drho_dt = getFTensor0FromVec(commonData.data.vecRhoDt);
 
  for (int gg = 0; gg < nb_gauss_pts; gg++) {
    int bb = 0;
    for (; bb < nb_dofs; bb++) {
      const double field_data = array[data.getLocalIndices()[bb]];
      drho_dt += field_data * base_function;
      ++base_function;
    }
    // It is possible to have more base functions than dofs
    for (; bb != nb_base_functions; bb++)
      ++base_function;
    ++drho_dt;
  }
 
  CHKERR VecRestoreArray(getFEMethod()->ts_u_t, &array);
 
  MoFEMFunctionReturn(0);
}
 
OpCalculateStress::OpCalculateStress(Remodeling::CommonData &common_data)
    : VolumeElementForcesAndSourcesCore::UserDataOperator(
          "DISPLACEMENTS", UserDataOperator::OPROW),
      commonData(common_data) {
  // Set that this operator is executed only for vertices on element
  doVertices = true;
  doEdges = false;
  doQuads = false;
  doTris = false;
  doTets = false;
  doPrisms = false;
}
 
MoFEMErrorCode
OpCalculateStress::doWork(int side, EntityType type,
                          DataForcesAndSourcesCore::EntData &data) {
 
  MoFEMFunctionBegin;
 
  if (type != MBVERTEX)
    MoFEMFunctionReturnHot(9);
 
  if (!commonData.data.gradDispPtr) {
    SETERRQ(PETSC_COMM_SELF, MOFEM_DATA_INCONSISTENCY,
            "Gradient at integration points of displacement is not calculated");
  }
  auto grad = getFTensor2FromMat<3, 3>(*commonData.data.gradDispPtr);
 
  const int nb_gauss_pts = commonData.data.gradDispPtr->size2();
  if (!commonData.data.rhoPtr) {
    SETERRQ(PETSC_COMM_SELF, MOFEM_DATA_INCONSISTENCY,
            "Density at integration points is not calculated");
  }
 
  auto rho = getFTensor0FromVec(*commonData.data.rhoPtr);
 
  commonData.data.vecPsi.resize(nb_gauss_pts, false);
  auto psi = getFTensor0FromVec(commonData.data.vecPsi);
  commonData.data.vecR.resize(nb_gauss_pts, false);
  auto R = getFTensor0FromVec(commonData.data.vecR);
 
  commonData.data.matInvF.resize(9, nb_gauss_pts, false);
  auto invF = getFTensor2FromMat<3, 3>(commonData.data.matInvF);
  commonData.data.vecDetF.resize(nb_gauss_pts, false);
  auto det_f = getFTensor0FromVec(commonData.data.vecDetF);
  MatrixDouble &matS = commonData.data.matS;
  matS.resize(nb_gauss_pts, 6, false);
  FTensor::Tensor2_symmetric<double *, 3> S(SYMMETRIC_TENSOR2_MAT_PTR(matS), 6);
  commonData.data.matP.resize(9, nb_gauss_pts, false);
  auto P = getFTensor2FromMat<3, 3>(commonData.data.matP);
  MatrixDouble &matF = commonData.data.matGradientOfDeformation;
  matF.resize(9, nb_gauss_pts, false);
  FTensor::Tensor2<double *, 3, 3> F(TENSOR2_MAT_PTR(matF));
 
  const double c = commonData.c;
  const double n = commonData.n;
  const double m = commonData.m;
  const double rho_ref = commonData.rHo_ref;
  const double psi_ref = commonData.pSi_ref;
  const double lambda = commonData.lambda;
  const double mu = commonData.mu;
  FTensor::Index<'i', 3> i;
  FTensor::Index<'I', 3> I;
  FTensor::Index<'J', 3> J;
 
  for (int gg = 0; gg != nb_gauss_pts; gg++) {
 
    // Get deformation gradient
    F(i, I) = grad(i, I);
    F(0, 0) += 1;
    F(1, 1) += 1;
    F(2, 2) += 1;
 
    CHKERR determinantTensor3by3(F, det_f);
    CHKERR invertTensor3by3(F, det_f, invF);
 
    const double log_det = log(det_f);
    psi = F(i, J) * F(i, J) - 3 - 2 * log_det;
    psi *= 0.5 * mu;
    psi += 0.5 * lambda * log_det * log_det;
 
    const double coef = lambda * log_det - mu;
    P(i, J) = mu * F(i, J) + coef * invF(J, i);
    S(i, I) = invF(i, J) ^ P(J, I);
 
    const double kp = commonData.getCFromDensity(rho);
    R = c * kp * (pow(rho / rho_ref, n - m) * psi - psi_ref);
 
    ++det_f;
    ++invF;
    ++grad;
    ++rho;
    ++psi;
    ++S;
    ++P;
    ++R;
    ++F;
  }
 
  MoFEMFunctionReturn(0);
}
 
OpCalculateStressTangentWithAdolc::OpCalculateStressTangentWithAdolc(
    Remodeling::CommonData &common_data)
    : VolumeElementForcesAndSourcesCore::UserDataOperator(
          "DISPLACEMENTS", UserDataOperator::OPROW),
      commonData(common_data) {
  // Set that this operator is executed only for vertices on element
  doVertices = true;
  doEdges = false;
  doQuads = false;
  doTris = false;
  doTets = false;
  doPrisms = false;
}
 
MoFEMErrorCode OpCalculateStressTangentWithAdolc::doWork(
    int side, EntityType type, DataForcesAndSourcesCore::EntData &data) {
 
  MoFEMFunctionBegin;
 
  if (type != MBVERTEX)
    MoFEMFunctionReturnHot(0);
 
  auto grad = getFTensor2FromMat<3, 3>(*commonData.data.gradDispPtr);
  const int nb_gauss_pts = commonData.data.gradDispPtr->size2();
 
  vecC.resize(6, false);
  FTensor::Tensor2_symmetric<double *, 3> C(SYMMETRIC_TENSOR2_VEC_PTR(vecC));
  MatrixDouble &dS_dC = commonData.data.matMaterialTangent;
  dS_dC.resize(6, 6 * nb_gauss_pts, false);
  dS_dC.clear();
  FTensor::Ddg<double *, 3, 3> D1(SYMMETRIC_TENSOR4_MAT_PTR(dS_dC), 6);
 
  MatrixDouble &matS = commonData.data.matS;
  matS.resize(nb_gauss_pts, 6, false);
  FTensor::Tensor2_symmetric<double *, 3> S(SYMMETRIC_TENSOR2_MAT_PTR(matS), 6);
  MatrixDouble &dP_dF = commonData.data.matPushedMaterialTangent;
  dP_dF.resize(81, nb_gauss_pts, false);
  MatrixDouble &matF = commonData.data.matGradientOfDeformation;
  matF.resize(9, nb_gauss_pts, false);
  FTensor::Tensor2<double *, 3, 3> F(TENSOR2_MAT_PTR(matF));
  FTensor::Tensor4<double *, 3, 3, 3, 3> D2(TENSOR4_MAT_PTR(dP_dF));
  commonData.data.matP.resize(9, nb_gauss_pts, false);
  auto P = getFTensor2FromMat<3, 3>(commonData.data.matP);
  commonData.data.vecPsi.resize(nb_gauss_pts, false);
  auto psi = getFTensor0FromVec(commonData.data.vecPsi);
 
  FTensor::Index<'i', 3> i;
  FTensor::Index<'k', 3> k;
  FTensor::Index<'I', 3> I;
  FTensor::Index<'J', 3> J;
  FTensor::Index<'K', 3> K;
  FTensor::Index<'L', 3> L;
 
  for (int gg = 0; gg != nb_gauss_pts; gg++) {
 
    // Get deformation gradient
    F(i, I) = grad(i, I);
    F(0, 0) += 1;
    F(1, 1) += 1;
    F(2, 2) += 1;
 
    // Calculate Green defamation Tensor0
    // ^ that means multiplication of Tensor 2 which result in symmetric Tensor
    // rank 2.
    C(I, J) = F(i, I) ^ F(i, J);
 
    // Calculate free energy
    CHKERR recordFreeEnergy_dC<FTensor::Tensor0<FTensor::PackPtr<double *, 1>>,
                               FTensor::Tensor2_symmetric<double *, 3>>(
        commonData, C, psi);
 
    int r_g = ::gradient(commonData.tAg, 6, &vecC[0], &matS(gg, 0));
    if (r_g < 0) {
      // That means that energy function is not smooth and derivative
      // can not be calculated,
      SETERRQ(PETSC_COMM_SELF, MOFEM_OPERATION_UNSUCCESSFUL,
              "ADOL-C function evaluation with error");
    }
 
    // Scale energy density and 2nd Piola Stress
    S(0, 0) *= 2;
    S(1, 1) *= 2;
    S(2, 2) *= 2;
 
    // Calculate 1st Piola-Stress tensor
    P(i, J) = F(i, I) * S(I, J);
 
    const int is = 6 * gg;
    double *hessian_ptr[6] = {&dS_dC(0, is), &dS_dC(1, is), &dS_dC(2, is),
                              &dS_dC(3, is), &dS_dC(4, is), &dS_dC(5, is)};
    int r_h = ::hessian(commonData.tAg, 6, &vecC[0], hessian_ptr);
    if (r_h < 0) {
      // That means that energy function is not smooth and derivative
      // can not be calculated,
      SETERRQ(PETSC_COMM_SELF, MOFEM_OPERATION_UNSUCCESSFUL,
              "ADOL-C function evaluation with error");
    }
 
    // FIXME: TODO: Here tensor 4 with two minor symmetries is used, since
    // \psi_{,ijkl} = \psi_{,klij} = \psi_{,lkji} = ... there is additional
    // major symmetry. That is 21 independent components not 36 how is now
    // implemented using Tenso4_ddg.
    //
    // That way we have to fill off-diagonal part for dS_dC. In future that
    // need to be fixed by implementation of Tenso4 with additional major
    // symmetries. Pleas consider doing such implementation by yourself. You
    // have support of MoFEM developing team to guide you through this process.
    //
    // This deteriorate efficiency.
    //
    // TODO: Implement matrix with more symmetries.
    //
    for (int ii = 0; ii < 6; ii++) {
      for (int jj = 0; jj < ii; jj++) {
        dS_dC(jj, is + ii) = dS_dC(ii, is + jj);
      }
    }
 
    // As result that symmetry of deformation gradient C is used, terms which
    // have mixed indices are two time to big, and terms which has two times
    // midxed index are four times to big. Those terms are not multiplied.
    D1(0, 0, 0, 0) *= 4;
    D1(1, 1, 1, 1) *= 4;
    D1(2, 2, 2, 2) *= 4;
 
    D1(0, 0, 1, 1) *= 4;
    D1(1, 1, 0, 0) *= 4;
    D1(1, 1, 2, 2) *= 4;
    D1(2, 2, 1, 1) *= 4;
    D1(0, 0, 2, 2) *= 4;
    D1(2, 2, 0, 0) *= 4;
 
    D1(0, 0, 0, 1) *= 2;
    D1(0, 0, 0, 2) *= 2;
    D1(0, 0, 1, 2) *= 2;
    D1(0, 1, 0, 0) *= 2;
    D1(0, 2, 0, 0) *= 2;
    D1(1, 2, 0, 0) *= 2;
 
    D1(1, 1, 0, 1) *= 2;
    D1(1, 1, 0, 2) *= 2;
    D1(1, 1, 1, 2) *= 2;
    D1(0, 1, 1, 1) *= 2;
    D1(0, 2, 1, 1) *= 2;
    D1(1, 2, 1, 1) *= 2;
 
    D1(2, 2, 0, 1) *= 2;
    D1(2, 2, 0, 2) *= 2;
    D1(2, 2, 1, 2) *= 2;
    D1(0, 1, 2, 2) *= 2;
    D1(0, 2, 2, 2) *= 2;
    D1(1, 2, 2, 2) *= 2;
 
    D2(i, J, k, L) = (F(i, I) * (D1(I, J, K, L) * F(k, K)));
    // D2(i, J, k, L) += I(i, k) * S(J, L);
 
    ++grad;
    ++S;
    ++F;
    ++D1;
    ++D2;
    ++P;
    ++psi;
  }
 
  MoFEMFunctionReturn(0);
}
 
OpPostProcStress::OpPostProcStress(moab::Interface &post_proc_mesh,
                                   std::vector<EntityHandle> &map_gauss_pts,
                                   Remodeling::CommonData &common_data)
    : VolumeElementForcesAndSourcesCore::UserDataOperator(
          "DISPLACEMENTS", UserDataOperator::OPROW),
      commonData(common_data), postProcMesh(post_proc_mesh),
      mapGaussPts(map_gauss_pts) {
  // Set that this operator is executed only for vertices on element
  doVertices = true;
  doEdges = false;
  doQuads = false;
  doTris = false;
  doTets = false;
  doPrisms = false;
}
 
MoFEMErrorCode
OpPostProcStress::doWork(int side, EntityType type,
                         DataForcesAndSourcesCore::EntData &data) {
 
  MoFEMFunctionBegin;
 
  if (type != MBVERTEX)
    MoFEMFunctionReturnHot(9);
  double def_VAL[9];
  bzero(def_VAL, 9 * sizeof(double));
 
  Tag th_piola2;
  CHKERR postProcMesh.tag_get_handle("Piola_Stress2", 9, MB_TYPE_DOUBLE,
                                     th_piola2, MB_TAG_CREAT | MB_TAG_SPARSE,
                                     def_VAL);
 
  Tag th_psi;
  CHKERR postProcMesh.tag_get_handle("psi", 1, MB_TYPE_DOUBLE, th_psi,
                                     MB_TAG_CREAT | MB_TAG_SPARSE, def_VAL);
 
  Tag th_rho;
  CHKERR postProcMesh.tag_get_handle("RHO", 1, MB_TYPE_DOUBLE, th_rho,
                                     MB_TAG_CREAT | MB_TAG_SPARSE, def_VAL);
 
  Tag th_piola1;
  Tag principal;
  Tag th_prin_stress_vect1, th_prin_stress_vect2, th_prin_stress_vect3;
  Tag th_R;
  Tag th_grad_rho;
  if (!commonData.less_post_proc) {
 
    CHKERR postProcMesh.tag_get_handle("Piola_Stress1", 9, MB_TYPE_DOUBLE,
                                       th_piola1, MB_TAG_CREAT | MB_TAG_SPARSE,
                                       def_VAL);
 
    CHKERR postProcMesh.tag_get_handle("Principal_stress", 3, MB_TYPE_DOUBLE,
                                       principal, MB_TAG_CREAT | MB_TAG_SPARSE,
                                       def_VAL);
 
    CHKERR postProcMesh.tag_get_handle("Principal_stress_S1", 3, MB_TYPE_DOUBLE,
                                       th_prin_stress_vect1,
                                       MB_TAG_CREAT | MB_TAG_SPARSE, def_VAL);
    CHKERR postProcMesh.tag_get_handle("Principal_stress_S2", 3, MB_TYPE_DOUBLE,
                                       th_prin_stress_vect2,
                                       MB_TAG_CREAT | MB_TAG_SPARSE, def_VAL);
    CHKERR postProcMesh.tag_get_handle("Principal_stress_S3", 3, MB_TYPE_DOUBLE,
                                       th_prin_stress_vect3,
                                       MB_TAG_CREAT | MB_TAG_SPARSE, def_VAL);
 
    CHKERR postProcMesh.tag_get_handle("R", 1, MB_TYPE_DOUBLE, th_R,
                                       MB_TAG_CREAT | MB_TAG_SPARSE, def_VAL);
 
    CHKERR postProcMesh.tag_get_handle("grad_rho", 3, MB_TYPE_DOUBLE,
                                       th_grad_rho,
                                       MB_TAG_CREAT | MB_TAG_SPARSE, def_VAL);
  }
 
  const int nb_gauss_pts = commonData.data.gradDispPtr->size2();
  MatrixDouble &matS = commonData.data.matS;
  FTensor::Tensor2_symmetric<double *, 3> S(SYMMETRIC_TENSOR2_MAT_PTR(matS), 6);
  auto P = getFTensor2FromMat<3, 3>(commonData.data.matP);
  auto psi = getFTensor0FromVec(commonData.data.vecPsi);
  auto R = getFTensor0FromVec(commonData.data.vecR);
 
  auto rho = getFTensor0FromVec(*commonData.data.rhoPtr);
  auto grad_rho = getFTensor1FromMat<3>(*commonData.data.gradRhoPtr);
 
  MatrixDouble3by3 stress(3, 3);
  VectorDouble3 eigen_values(3);
  MatrixDouble3by3 eigen_vectors(3, 3);
 
  for (int gg = 0; gg != nb_gauss_pts; gg++) {
 
    double val = psi;
    CHKERR postProcMesh.tag_set_data(th_psi, &mapGaussPts[gg], 1, &val);
    val = rho;
    CHKERR postProcMesh.tag_set_data(th_rho, &mapGaussPts[gg], 1, &val);
 
    // S is symmetric tensor, need to map it to non-symmetric tensor to save it
    // on moab tag to be viable in paraview.
    for (int ii = 0; ii != 3; ii++) {
      for (int jj = 0; jj != 3; jj++) {
        stress(ii, jj) = S(ii, jj);
      }
    }
    CHKERR postProcMesh.tag_set_data(th_piola2, &mapGaussPts[gg], 1,
                                     &stress(0, 0));
 
    if (!commonData.less_post_proc) {
 
      for (int ii = 0; ii != 3; ii++) {
        for (int jj = 0; jj != 3; jj++) {
          stress(ii, jj) = P(ii, jj);
        }
      }
      CHKERR postProcMesh.tag_set_data(th_piola1, &mapGaussPts[gg], 1,
                                       &stress(0, 0));
      val = R;
      CHKERR postProcMesh.tag_set_data(th_R, &mapGaussPts[gg], 1, &val);
      const double t2[] = {grad_rho(0), grad_rho(1), grad_rho(2)};
      CHKERR postProcMesh.tag_set_data(th_grad_rho, &mapGaussPts[gg], 1, t2);
 
      // Add calculation of eigen values, i.e. prinple stresses.
      noalias(eigen_vectors) = stress;
      VectorDouble3 prin_stress_vect1(3);
      VectorDouble3 prin_stress_vect2(3);
      VectorDouble3 prin_stress_vect3(3);
      // LAPACK - eigenvalues and vectors. Applied twice for initial creates
      // memory space
      int n = 3, lda = 3, info, lwork = -1;
      double wkopt;
      info = lapack_dsyev('V', 'U', n, &(eigen_vectors.data()[0]), lda,
                          &(eigen_values.data()[0]), &wkopt, lwork);
      if (info != 0)
        SETERRQ1(PETSC_COMM_SELF, 1,
                 "something is wrong with lapack_dsyev info = %d", info);
      lwork = (int)wkopt;
      double work[lwork];
      info = lapack_dsyev('V', 'U', n, &(eigen_vectors.data()[0]), lda,
                          &(eigen_values.data()[0]), work, lwork);
      if (info != 0)
        SETERRQ1(PETSC_COMM_SELF, 1,
                 "something is wrong with lapack_dsyev info = %d", info);
 
      for (int ii = 0; ii < 3; ii++) {
        prin_stress_vect1[ii] = eigen_vectors(0, ii);
        prin_stress_vect2[ii] = eigen_vectors(1, ii);
        prin_stress_vect3[ii] = eigen_vectors(2, ii);
      }
 
      CHKERR postProcMesh.tag_set_data(principal, &mapGaussPts[gg], 1,
                                       &eigen_values[0]);
      CHKERR postProcMesh.tag_set_data(th_prin_stress_vect1, &mapGaussPts[gg],
                                       1, &*prin_stress_vect1.begin());
      CHKERR postProcMesh.tag_set_data(th_prin_stress_vect2, &mapGaussPts[gg],
                                       1, &*prin_stress_vect2.begin());
      CHKERR postProcMesh.tag_set_data(th_prin_stress_vect3, &mapGaussPts[gg],
                                       1, &*prin_stress_vect3.begin());
    }
 
    ++S;
    ++P;
    ++psi;
    ++R;
    ++rho;
    ++grad_rho;
  }
 
  MoFEMFunctionReturn(0);
}
 
OpCalculateStressTangent::OpCalculateStressTangent(
    Remodeling::CommonData &common_data)
    : VolumeElementForcesAndSourcesCore::UserDataOperator(
          "DISPLACEMENTS", UserDataOperator::OPROW),
      commonData(common_data) {
  // Set that this operator is executed only for vertices on element
  doVertices = true;
  doEdges = false;
  doQuads = false;
  doTris = false;
  doTets = false;
  doPrisms = false;
}
 
MoFEMErrorCode
OpCalculateStressTangent::doWork(int side, EntityType type,
                                DataForcesAndSourcesCore::EntData &data) {
 
  MoFEMFunctionBegin;
 
  if (type != MBVERTEX)
    MoFEMFunctionReturnHot(0);
 
  auto grad = getFTensor2FromMat<3, 3>(*commonData.data.gradDispPtr);
  const int nb_gauss_pts = commonData.data.gradDispPtr->size2();
 
  commonData.data.matInvF.resize(9, nb_gauss_pts, false);
  auto invF = getFTensor2FromMat<3, 3>(commonData.data.matInvF);
  commonData.data.vecDetF.resize(nb_gauss_pts, false);
  auto det_f = getFTensor0FromVec(commonData.data.vecDetF);
 
  MatrixDouble &dP_dF = commonData.data.matPushedMaterialTangent;
  dP_dF.resize(81, nb_gauss_pts, false);
  MatrixDouble &matF = commonData.data.matGradientOfDeformation;
  matF.resize(9, nb_gauss_pts, false);
  FTensor::Tensor2<double *, 3, 3> F(TENSOR2_MAT_PTR(matF));
  FTensor::Tensor4<double *, 3, 3, 3, 3> D2(TENSOR4_MAT_PTR(dP_dF));
  commonData.data.matP.resize(9, nb_gauss_pts, false);
  auto P = getFTensor2FromMat<3, 3>(commonData.data.matP);
  commonData.data.vecPsi.resize(nb_gauss_pts, false);
  auto psi = getFTensor0FromVec(commonData.data.vecPsi);
  FTensor::Index<'i', 3> i;
  FTensor::Index<'k', 3> k;
  FTensor::Index<'I', 3> I;
  FTensor::Index<'J', 3> J;
  FTensor::Index<'K', 3> K;
  FTensor::Index<'L', 3> L;
  const double lambda = commonData.lambda;
  const double mu = commonData.mu;
 
  for (int gg = 0; gg != nb_gauss_pts; gg++) {
 
    // Get deformation gradient
    F(i, I) = grad(i, I);
    F(0, 0) += 1;
    F(1, 1) += 1;
    F(2, 2) += 1;
 
    CHKERR determinantTensor3by3(F, det_f);
    CHKERR invertTensor3by3(F, det_f, invF);
 
    const double log_det = log(det_f);
    psi = F(i, J) * F(i, J) - 3 - 2 * log_det;
    psi *= 0.5 * mu;
    psi += 0.5 * lambda * log_det * log_det;
 
    const double var = lambda * log_det - mu;
    P(i, J) = mu * F(i, J) + var * invF(J, i);
 
    const double coef = mu - lambda * log_det;
    // TODO: This can be improved by utilising the symmetries of the D2 tensor
 
    D2(i, J, k, L) =
        invF(J, i) * (invF(L, k) * lambda) + invF(L, i) * (invF(J, k) * coef);
 
    D2(0, 0, 0, 0) += mu;
    D2(0, 1, 0, 1) += mu;
    D2(0, 2, 0, 2) += mu;
    D2(1, 0, 1, 0) += mu;
    D2(1, 1, 1, 1) += mu;
    D2(1, 2, 1, 2) += mu;
    D2(2, 0, 2, 0) += mu;
    D2(2, 1, 2, 1) += mu;
    D2(2, 2, 2, 2) += mu;
 
    ++det_f;
    ++invF;
    ++grad;
    ++F;
    ++D2;
    ++P;
    ++psi;
  }
 
  MoFEMFunctionReturn(0);
}
 
OpAssmbleStressRhs::OpAssmbleStressRhs(Remodeling::CommonData &common_data)
    : VolumeElementForcesAndSourcesCore::UserDataOperator(
          "DISPLACEMENTS", UserDataOperator::OPROW),
      commonData(common_data) {}
 
MoFEMErrorCode
OpAssmbleStressRhs::doWork(int side, EntityType type,
                           DataForcesAndSourcesCore::EntData &data) {
 
  MoFEMFunctionBegin;
 
  const int nb_dofs = data.getIndices().size();
  if (!nb_dofs)
    MoFEMFunctionReturnHot(0);
  nF.resize(nb_dofs, false);
  nF.clear();
 
  const int nb_gauss_pts = data.getN().size1();
  const int nb_base_functions = data.getN().size2();
  auto diff_base_functions = data.getFTensor1DiffN<3>();
  auto P = getFTensor2FromMat<3, 3>(commonData.data.matP);
  auto rho = getFTensor0FromVec(*commonData.data.rhoPtr);
 
  // MatrixDouble &matS = commonData.data.matS;
  // MatrixDouble &matF = commonData.data.matGradientOfDeformation;
  // FTensor::Tensor2<double*,3,3> F(TENSOR2_MAT_PTR(matF));
  // FTensor::Tensor2_symmetric<double*,3> S(SYMMETRIC_TENSOR2_MAT_PTR(matS),6);
 
  const double n = commonData.n;
  const double rho_ref = commonData.rHo_ref;
 
  FTensor::Index<'I', 3> I;
  FTensor::Index<'J', 3> J;
  FTensor::Index<'i', 3> i;
 
  for (int gg = 0; gg != nb_gauss_pts; gg++) {
 
    // Get volume and integration weight
    double w = getVolume() * getGaussPts()(3, gg);
    double a = w * pow(rho / rho_ref, n);
 
    int bb = 0;
    for (; bb != nb_dofs / 3; bb++) {
      double *ptr = &nF[3 * bb];
      FTensor::Tensor1<double *, 3> t1(ptr, &ptr[1], &ptr[2]);
      t1(i) += a * P(i, I) * diff_base_functions(I);
      // t1(i) += 0.5*a*diff_base_functions(J)*(F(i,I)*S(I,J));
      // t1(i) += 0.5*a*diff_base_functions(I)*(F(i,J)*S(I,J));
      ++diff_base_functions;
    }
    // Could be that we base more base functions than needed to approx. disp.
    // field.
    for (; bb != nb_base_functions; bb++) {
      ++diff_base_functions;
    }
    ++P;
    // ++S;
    // ++F;
    ++rho;
  }
 
  // Assemble internal force vector for time solver (TS)
  CHKERR VecSetValues(
      getFEMethod()->ts_F, /// Get the right hand side vector for time solver
      nb_dofs, &*data.getIndices().begin(), &*nF.begin(), ADD_VALUES);
 
  MoFEMFunctionReturn(0);
}
 
OpAssmbleRhoRhs::OpAssmbleRhoRhs(Remodeling::CommonData &common_data)
    : VolumeElementForcesAndSourcesCore::UserDataOperator(
          "RHO", UserDataOperator::OPROW),
      commonData(common_data) {}
 
MoFEMErrorCode
OpAssmbleRhoRhs::doWork(int side, EntityType type,
                        DataForcesAndSourcesCore::EntData &data) {
 
  MoFEMFunctionBegin;
 
  const int nb_dofs = data.getIndices().size();
  if (!nb_dofs)
    MoFEMFunctionReturnHot(0);
  nF.resize(nb_dofs, false);
  nF.clear();
 
  const int nb_gauss_pts = data.getN().size1();
  const int nb_base_functions = data.getN().size2();
  auto base_functions = data.getFTensor0N();
  auto diff_base_functions = data.getFTensor1DiffN<3>();
  if (!commonData.data.rhoPtr) {
    SETERRQ(PETSC_COMM_SELF, MOFEM_DATA_INCONSISTENCY, "rhoPtsr is null");
  }
  auto rho = getFTensor0FromVec(*commonData.data.rhoPtr);
  if (!commonData.data.gradRhoPtr) {
    SETERRQ(PETSC_COMM_SELF, MOFEM_DATA_INCONSISTENCY, "gradRhoPtr is null");
  }
  auto grad_rho = getFTensor1FromMat<3>(*commonData.data.gradRhoPtr);
  auto R = getFTensor0FromVec(commonData.data.vecR);
 
  if (commonData.data.vecRhoDt.size() != nb_gauss_pts) {
    commonData.data.vecRhoDt.resize(nb_gauss_pts, false);
    commonData.data.vecRhoDt.clear();
    // SETERRQ(PETSC_COMM_SELF,MOFEM_DATA_INCONSISTENCY,"Time derivative of
    // density at integration not calculated");
  }
  auto drho_dt = getFTensor0FromVec(commonData.data.vecRhoDt);
 
  const double R0 = commonData.R0;
 
  FTensor::Index<'I', 3> I;
 
  for (int gg = 0; gg != nb_gauss_pts; gg++) {
 
    // Get volume and integration weight
    double w = getVolume() * getGaussPts()(3, gg);
 
    int bb = 0;
    for (; bb != nb_dofs; bb++) {
      nF[bb] += w * base_functions * drho_dt; // Density rate term
      nF[bb] -= w * base_functions * R;       // Source term
      nF[bb] -= w * R0 * diff_base_functions(I) * grad_rho(I); // Gradient term
      ++base_functions;
      ++diff_base_functions;
    }
    // Could be that we base more base functions than needed to approx. density
    // field.
    for (; bb != nb_base_functions; bb++) {
      ++base_functions;
      ++diff_base_functions;
    }
 
    // Increment quantities for integration pts.
    ++rho;
    ++grad_rho;
    ++drho_dt;
    ++R;
  }
 
  // Assemble internal force vector for time solver (TS)
  CHKERR VecSetValues(
      getFEMethod()->ts_F, /// Get the right hand side vector for time solver
      nb_dofs, &*data.getIndices().begin(), &*nF.begin(), ADD_VALUES);
 
  MoFEMFunctionReturn(0);
}
 
OpAssmbleRhoLhs_dRho::OpAssmbleRhoLhs_dRho(Remodeling::CommonData &common_data)
    : VolumeElementForcesAndSourcesCore::UserDataOperator(
          "RHO", "RHO", UserDataOperator::OPROWCOL),
      commonData(common_data) {
  sYmm = true;
}
 
MoFEMErrorCode
OpAssmbleRhoLhs_dRho::doWork(int row_side, int col_side, EntityType row_type,
                             EntityType col_type,
                             DataForcesAndSourcesCore::EntData &row_data,
                             DataForcesAndSourcesCore::EntData &col_data) {
 
  MoFEMFunctionBegin;
 
  const int row_nb_dofs = row_data.getIndices().size();
  if (!row_nb_dofs)
    MoFEMFunctionReturnHot(0);
  const int col_nb_dofs = col_data.getIndices().size();
  if (!col_nb_dofs)
    MoFEMFunctionReturnHot(0);
 
  // Set size can clear local tangent matrix
  locK_rho_rho.resize(row_nb_dofs, col_nb_dofs, false);
  locK_rho_rho.clear();
 
  const int row_nb_gauss_pts = row_data.getN().size1();
  if (!row_nb_gauss_pts)
    MoFEMFunctionReturnHot(0);
  const int row_nb_base_functions = row_data.getN().size2();
 
  const double ts_a = getFEMethod()->ts_a;
  const double R0 = commonData.R0;
  const double c = commonData.c;
  const double n = commonData.n;
  const double m = commonData.m;
  const double rho_ref = commonData.rHo_ref;
  const double psi_ref = commonData.pSi_ref;
 
  FTensor::Index<'I', 3> I;
 
  auto rho = getFTensor0FromVec(*commonData.data.rhoPtr);
  auto psi = getFTensor0FromVec(commonData.data.vecPsi);
  auto row_base_functions = row_data.getFTensor0N();
  auto row_diff_base_functions = row_data.getFTensor1DiffN<3>();
 
  for (int gg = 0; gg != row_nb_gauss_pts; gg++) {
 
    // Get volume and integration weight
    double w = getVolume() * getGaussPts()(3, gg);
 
    const double kp = commonData.getCFromDensity(rho);
    const double kp_diff = commonData.getCFromDensityDiff(rho);
    const double a = c * kp * ((n - m) / rho) * pow(rho / rho_ref, n - m) * psi;
    const double a_diff =
        c * kp_diff * (pow(rho / rho_ref, n - m) * psi - psi_ref);
 
    int row_bb = 0;
    for (; row_bb != row_nb_dofs; row_bb++) {
      auto col_base_functions = col_data.getFTensor0N(gg, 0);
      auto col_diff_base_functions = col_data.getFTensor1DiffN<3>(gg, 0);
      for (int col_bb = 0; col_bb != col_nb_dofs; col_bb++) {
        locK_rho_rho(row_bb, col_bb) +=
            ts_a * w * row_base_functions * col_base_functions;
        locK_rho_rho(row_bb, col_bb) -=
            w * R0 * row_diff_base_functions(I) * col_diff_base_functions(I);
        locK_rho_rho(row_bb, col_bb) -=
            a * w * row_base_functions * col_base_functions;
        locK_rho_rho(row_bb, col_bb) -=
            a_diff * w * row_base_functions * col_base_functions;
 
        ++col_base_functions;
        ++col_diff_base_functions;
      }
      ++row_base_functions;
      ++row_diff_base_functions;
    }
    for (; row_bb != row_nb_base_functions; row_bb++) {
      ++row_base_functions;
      ++row_diff_base_functions;
    }
    ++psi;
    ++rho;
  }
 
  // cerr << locK_rho_rho << endl;
 
  CHKERR MatSetValues(getFEMethod()->ts_B, row_nb_dofs,
                      &*row_data.getIndices().begin(), col_nb_dofs,
                      &*col_data.getIndices().begin(), &locK_rho_rho(0, 0),
                      ADD_VALUES);
 
  // is symmetric
  if (row_side != col_side || row_type != col_type) {
    transLocK_rho_rho.resize(col_nb_dofs, row_nb_dofs, false);
    noalias(transLocK_rho_rho) = trans(locK_rho_rho);
    CHKERR MatSetValues(getFEMethod()->ts_B, col_nb_dofs,
                        &*col_data.getIndices().begin(), row_nb_dofs,
                        &*row_data.getIndices().begin(),
                        &transLocK_rho_rho(0, 0), ADD_VALUES);
  }
 
  MoFEMFunctionReturn(0);
}
 
OpAssmbleRhoLhs_dF::OpAssmbleRhoLhs_dF(Remodeling::CommonData &common_data)
    : VolumeElementForcesAndSourcesCore::UserDataOperator(
          "RHO", "DISPLACEMENTS", UserDataOperator::OPROWCOL),
      commonData(common_data) {
  sYmm = false;
}
 
MoFEMErrorCode
OpAssmbleRhoLhs_dF::doWork(int row_side, int col_side, EntityType row_type,
                           EntityType col_type,
                           DataForcesAndSourcesCore::EntData &row_data,
                           DataForcesAndSourcesCore::EntData &col_data) {
 
  MoFEMFunctionBegin;
 
  const int row_nb_dofs = row_data.getIndices().size();
  if (!row_nb_dofs)
    MoFEMFunctionReturnHot(0);
  const int col_nb_dofs = col_data.getIndices().size();
  if (!col_nb_dofs)
    MoFEMFunctionReturnHot(0);
 
  // Set size can clear local tangent matrix
  locK_rho_F.resize(row_nb_dofs, col_nb_dofs, false);
  locK_rho_F.clear();
 
  const int row_nb_gauss_pts = row_data.getN().size1();
  const int col_nb_base_functions = col_data.getN().size2();
 
  const double c = commonData.c;
  const double n = commonData.n;
  const double m = commonData.m;
  const double rho_ref = commonData.rHo_ref;
 
  FTensor::Index<'I', 3> I;
  FTensor::Index<'i', 3> i;
 
  auto rho = getFTensor0FromVec(*commonData.data.rhoPtr);
  auto P = getFTensor2FromMat<3, 3>(commonData.data.matP);
 
  FTensor::Tensor1<double, 3> t1;
  auto col_diff_base_functions = col_data.getFTensor1DiffN<3>();
 
  for (int gg = 0; gg != row_nb_gauss_pts; gg++) {
 
    // Get volume and integration weight
    double w = getVolume() * getGaussPts()(3, gg);
    const double kp = commonData.getCFromDensity(rho);
    const double a = w * c * kp * pow(rho / rho_ref, n - m);
 
    int col_bb = 0;
    for (; col_bb != col_nb_dofs / 3; col_bb++) {
      t1(i) = a * P(i, I) * col_diff_base_functions(I);
      auto row_base_functions = row_data.getFTensor0N(gg, 0);
      for (int row_bb = 0; row_bb != row_nb_dofs; row_bb++) {
        FTensor::Tensor1<double *, 3> k(&locK_rho_F(row_bb, 3 * col_bb + 0),
                                        &locK_rho_F(row_bb, 3 * col_bb + 1),
                                        &locK_rho_F(row_bb, 3 * col_bb + 2));
        k(i) -= row_base_functions * t1(i);
        ++row_base_functions;
      }
      ++col_diff_base_functions;
    }
    for (; col_bb != col_nb_base_functions; col_bb++) {
      ++col_diff_base_functions;
    }
 
    ++P;
    ++rho;
  }
 
  CHKERR MatSetValues(getFEMethod()->ts_B, row_nb_dofs,
                      &*row_data.getIndices().begin(), col_nb_dofs,
                      &*col_data.getIndices().begin(), &locK_rho_F(0, 0),
                      ADD_VALUES);
 
  MoFEMFunctionReturn(0);
}
template <bool ADOLC>
OpAssmbleStressLhs_dF<ADOLC>::OpAssmbleStressLhs_dF(
    Remodeling::CommonData &common_data)
    : VolumeElementForcesAndSourcesCore::UserDataOperator(
          "DISPLACEMENTS", "DISPLACEMENTS", UserDataOperator::OPROWCOL),
      commonData(common_data) {
  sYmm = true;
}
template <bool ADOLC>
MoFEMErrorCode OpAssmbleStressLhs_dF<ADOLC>::doWork(
    int row_side, int col_side, EntityType row_type, EntityType col_type,
    DataForcesAndSourcesCore::EntData &row_data,
    DataForcesAndSourcesCore::EntData &col_data) {
 
  MoFEMFunctionBegin;
 
  const int row_nb_dofs = row_data.getIndices().size();
  if (!row_nb_dofs)
    MoFEMFunctionReturnHot(0);
  const int col_nb_dofs = col_data.getIndices().size();
  if (!col_nb_dofs)
    MoFEMFunctionReturnHot(0);
 
  const bool diagonal_block = (row_type == col_type) && (row_side == col_side);
 
  // Set size can clear local tangent matrix
  locK_P_F.resize(row_nb_dofs, col_nb_dofs, false);
  locK_P_F.clear();
 
  const int row_nb_gauss_pts = row_data.getN().size1();
  const int row_nb_base_functions = row_data.getN().size2();
 
  MatrixDouble &matS = commonData.data.matS;
  MatrixDouble &dP_dF = commonData.data.matPushedMaterialTangent;
  // MatrixDouble &dS_dC = commonData.data.matMaterialTangent;
  // MatrixDouble &matF = commonData.data.matGradientOfDeformation;
 
  double t0;
 
  const double n = commonData.n;
  const double rho_ref = commonData.rHo_ref;
 
  FTensor::Index<'i', 3> i;
  FTensor::Index<'j', 3> j;
  FTensor::Index<'k', 3> k;
  FTensor::Index<'I', 3> I;
  FTensor::Index<'J', 3> J;
  FTensor::Index<'K', 3> K;
  FTensor::Index<'L', 3> L;
 
  auto row_diff_base_functions = row_data.getFTensor1DiffN<3>();
  FTensor::Tensor4<double *, 3, 3, 3, 3> D2(TENSOR4_MAT_PTR(dP_dF));
  FTensor::Tensor2_symmetric<double *, 3> S(SYMMETRIC_TENSOR2_MAT_PTR(matS), 6);
  auto rho = getFTensor0FromVec(*commonData.data.rhoPtr);
 
  // FTensor::Tensor4_ddg<double*,3,3> D1(SYMMETRIC_TENSOR4_MAT_PTR(dS_dC),6);
  // FTensor::Tensor2<double*,3,3> F(TENSOR2_MAT_PTR(matF));
 
  for (int gg = 0; gg != row_nb_gauss_pts; gg++) {
 
    // Get volume and integration weight
    double w = getVolume() * getGaussPts()(3, gg);
    const double a = w * pow(rho / rho_ref, n);
 
    int row_bb = 0;
    for (; row_bb != row_nb_dofs / 3; row_bb++) {
 
      auto col_diff_base_functions = col_data.getFTensor1DiffN<3>(gg, 0);
 
      const int final_bb = diagonal_block ? row_bb + 1 : col_nb_dofs / 3;
      int col_bb = 0;
      for (; col_bb != final_bb; col_bb++) {
 
        FTensor::Tensor2<double *, 3, 3> t_assemble(
            &locK_P_F(3 * row_bb + 0, 3 * col_bb + 0),
            &locK_P_F(3 * row_bb + 0, 3 * col_bb + 1),
            &locK_P_F(3 * row_bb + 0, 3 * col_bb + 2),
            &locK_P_F(3 * row_bb + 1, 3 * col_bb + 0),
            &locK_P_F(3 * row_bb + 1, 3 * col_bb + 1),
            &locK_P_F(3 * row_bb + 1, 3 * col_bb + 2),
            &locK_P_F(3 * row_bb + 2, 3 * col_bb + 0),
            &locK_P_F(3 * row_bb + 2, 3 * col_bb + 1),
            &locK_P_F(3 * row_bb + 2, 3 * col_bb + 2));
 
        diffDiff(J, L) =
            a * row_diff_base_functions(J) * col_diff_base_functions(L);
 
        // TODO: FIXME: Tensor D2 has major symmetry, we do not have such tensor
        // implemented at the moment. Using tensor with major symmetry will
        // improve code efficiency.
        t_assemble(i, k) += diffDiff(J, L) * D2(i, J, k, L);
 
        if (ADOLC) {
          // Stress stiffening part (only with adolc since it is using dS / dC
          // derrivative. Check OpCalculateStressTangentWithAdolc operator)
          t0 = diffDiff(J, L) * S(J, L);
          t_assemble(0, 0) += t0;
          t_assemble(1, 1) += t0;
          t_assemble(2, 2) += t0;
        }
 
        // Next base function for column
        ++col_diff_base_functions;
      }
 
      ++row_diff_base_functions;
    }
    for (; row_bb != row_nb_base_functions; row_bb++) {
      ++row_diff_base_functions;
    }
 
    // ++D1;
    ++D2;
    ++S;
    // ++F;
    ++rho;
  }
 
  // Copy of symmetric part for assemble
  if (diagonal_block) {
    for (int row_bb = 0; row_bb != row_nb_dofs / 3; row_bb++) {
      int col_bb = 0;
      for (; col_bb != row_bb + 1; col_bb++) {
        FTensor::Tensor2<double *, 3, 3> t_assemble(
            &locK_P_F(3 * row_bb + 0, 3 * col_bb + 0),
            &locK_P_F(3 * row_bb + 0, 3 * col_bb + 1),
            &locK_P_F(3 * row_bb + 0, 3 * col_bb + 2),
            &locK_P_F(3 * row_bb + 1, 3 * col_bb + 0),
            &locK_P_F(3 * row_bb + 1, 3 * col_bb + 1),
            &locK_P_F(3 * row_bb + 1, 3 * col_bb + 2),
            &locK_P_F(3 * row_bb + 2, 3 * col_bb + 0),
            &locK_P_F(3 * row_bb + 2, 3 * col_bb + 1),
            &locK_P_F(3 * row_bb + 2, 3 * col_bb + 2));
        FTensor::Tensor2<double *, 3, 3> t_off_side(
            &locK_P_F(3 * col_bb + 0, 3 * row_bb + 0),
            &locK_P_F(3 * col_bb + 0, 3 * row_bb + 1),
            &locK_P_F(3 * col_bb + 0, 3 * row_bb + 2),
            &locK_P_F(3 * col_bb + 1, 3 * row_bb + 0),
            &locK_P_F(3 * col_bb + 1, 3 * row_bb + 1),
            &locK_P_F(3 * col_bb + 1, 3 * row_bb + 2),
            &locK_P_F(3 * col_bb + 2, 3 * row_bb + 0),
            &locK_P_F(3 * col_bb + 2, 3 * row_bb + 1),
            &locK_P_F(3 * col_bb + 2, 3 * row_bb + 2));
        t_off_side(i, k) = t_assemble(k, i);
      }
    }
  }
 
  const int *row_ind = &*row_data.getIndices().begin();
  const int *col_ind = &*col_data.getIndices().begin();
  CHKERR MatSetValues(getFEMethod()->ts_B, row_nb_dofs, row_ind, col_nb_dofs,
                      col_ind, &locK_P_F(0, 0), ADD_VALUES);
 
  if (row_type != col_type || row_side != col_side) {
    transLocK_P_F.resize(col_nb_dofs, row_nb_dofs, false);
    noalias(transLocK_P_F) = trans(locK_P_F);
    CHKERR MatSetValues(getFEMethod()->ts_B, col_nb_dofs, col_ind, row_nb_dofs,
                        row_ind, &transLocK_P_F(0, 0), ADD_VALUES);
  }
 
  MoFEMFunctionReturn(0);
}
 
OpAssmbleStressLhs_dRho::OpAssmbleStressLhs_dRho(
    Remodeling::CommonData &common_data)
    : VolumeElementForcesAndSourcesCore::UserDataOperator(
          "DISPLACEMENTS", "RHO", UserDataOperator::OPROWCOL),
      commonData(common_data) {
  sYmm = false; // This is off-diagonal (upper-left), so no symmetric
}
 
MoFEMErrorCode
OpAssmbleStressLhs_dRho::doWork(int row_side, int col_side, EntityType row_type,
                                EntityType col_type,
                                DataForcesAndSourcesCore::EntData &row_data,
                                DataForcesAndSourcesCore::EntData &col_data) {
 
  MoFEMFunctionBegin;
 
  const int row_nb_dofs = row_data.getIndices().size();
  if (!row_nb_dofs)
    MoFEMFunctionReturnHot(0);
  const int col_nb_dofs = col_data.getIndices().size();
  if (!col_nb_dofs)
    MoFEMFunctionReturnHot(0);
 
  // Set size can clear local tangent matrix
  locK_P_RHO.resize(row_nb_dofs, col_nb_dofs, false);
  locK_P_RHO.clear();
 
  FTensor::Tensor1<double, 3> t1;
 
  const double n = commonData.n;
  const double rho_ref = commonData.rHo_ref;
 
  FTensor::Index<'I', 3> I;
  FTensor::Index<'i', 3> i;
 
  auto P = getFTensor2FromMat<3, 3>(commonData.data.matP);
  auto rho = getFTensor0FromVec(*commonData.data.rhoPtr);
 
  const int row_nb_gauss_pts = row_data.getN().size1();
  const int row_nb_base_functions = row_data.getN().size2();
  auto row_diff_base_functions = row_data.getFTensor1DiffN<3>();
 
  for (int gg = 0; gg != row_nb_gauss_pts; gg++) {
 
    // Get volume and integration weight
    double w = getVolume() * getGaussPts()(3, gg);
    double a = w * (n / rho) * pow(rho / rho_ref, n);
 
    int row_bb = 0;
    for (; row_bb != row_nb_dofs / 3; row_bb++) {
      t1(i) = a * row_diff_base_functions(I) * P(i, I);
      auto base_functions = col_data.getFTensor0N(gg, 0);
      for (int col_bb = 0; col_bb != col_nb_dofs; col_bb++) {
        // if(base_functions!=col_data.getN(gg)[col_bb]) {
        //   cerr << "Error" << endl;
        // }
        FTensor::Tensor1<double *, 3> k(&locK_P_RHO(3 * row_bb + 0, col_bb),
                                        &locK_P_RHO(3 * row_bb + 1, col_bb),
                                        &locK_P_RHO(3 * row_bb + 2, col_bb));
        k(i) += t1(i) * base_functions;
        ++base_functions;
      }
      ++row_diff_base_functions;
    }
    for (; row_bb != row_nb_base_functions; row_bb++) {
      ++row_diff_base_functions;
    }
 
    ++P;
    ++rho;
  }
 
  CHKERR MatSetValues(getFEMethod()->ts_B, row_nb_dofs,
                      &*row_data.getIndices().begin(), col_nb_dofs,
                      &*col_data.getIndices().begin(), &locK_P_RHO(0, 0),
                      ADD_VALUES);
 
  MoFEMFunctionReturn(0);
}
 
MonitorPostProc::MonitorPostProc(MoFEM::Interface &m_field,
                                 Remodeling::CommonData &common_data)
    : mField(m_field), commonData(common_data), postProc(m_field),
      postProcElastic(m_field),
      // densityMaps(m_field),
      iNit(false) {}
 
MoFEMErrorCode MonitorPostProc::preProcess() {
  MoFEMFunctionBeginHot;
  MoFEMFunctionReturnHot(0);
}
 
MoFEMErrorCode MonitorPostProc::operator()() {
  MoFEMFunctionBeginHot;
  MoFEMFunctionReturnHot(0);
}
 
MoFEMErrorCode MonitorPostProc::postProcess() {
  MoFEMFunctionBegin;
 
  PetscBool mass_postproc = PETSC_FALSE;
  PetscBool equilibrium_flg = PETSC_FALSE;
  PetscBool analysis_mesh_flg = PETSC_FALSE;
  rate = 1;
 
  CHKERR PetscOptionsBegin(PETSC_COMM_WORLD, "", "Bone remodeling post-process",
                           "none");
 
  CHKERR PetscOptionsBool(
      "-mass_postproc",
      "is used for testing, calculates mass and energy at each time step", "",
      mass_postproc, &mass_postproc, PETSC_NULL);
 
  CHKERR PetscOptionsBool("-analysis_mesh",
                          "saves analysis mesh at each time step", "",
                          analysis_mesh_flg, &analysis_mesh_flg, PETSC_NULL);
 
  CHKERR PetscOptionsScalar(
      "-equilibrium_stop_rate",
      "is used to stop calculations when equilibium state is achieved", "",
      rate, &rate, &equilibrium_flg);
  ierr = PetscOptionsEnd();
  CHKERRQ(ierr);
 
  if (!iNit) {
 
    CHKERR PetscOptionsBegin(PETSC_COMM_WORLD, "",
                             "Bone remodeling post-process", "none");
 
    pRT = 1;
    CHKERR PetscOptionsInt("-my_output_prt",
                           "frequncy how often results are dumped on hard disk",
                           "", 1, &pRT, NULL);
    ierr = PetscOptionsEnd();
    CHKERRQ(ierr);
 
    CHKERR postProc.generateReferenceElementMesh();
    CHKERR postProc.addFieldValuesPostProc("DISPLACEMENTS");
    CHKERR postProc.addFieldValuesPostProc("MESH_NODE_POSITIONS");
    if (!commonData.less_post_proc) {
      CHKERR postProc.addFieldValuesGradientPostProc("DISPLACEMENTS");
    }
    // CHKERR postProc.addFieldValuesPostProc("RHO");
    // CHKERR postProc.addFieldValuesGradientPostProc("RHO");
 
    {
      auto &list_of_operators = postProc.getOpPtrVector();
 
      list_of_operators.push_back(
          new OpCalculateScalarFieldValues("RHO", commonData.data.rhoPtr));
      list_of_operators.push_back(new OpCalculateScalarFieldGradient<3>(
          "RHO", commonData.data.gradRhoPtr));
      // Get displacement gradient at integration points
      list_of_operators.push_back(new OpCalculateVectorFieldGradient<3, 3>(
          "DISPLACEMENTS", commonData.data.gradDispPtr));
      list_of_operators.push_back(new OpCalculateStress(commonData));
      list_of_operators.push_back(new OpPostProcStress(
          postProc.postProcMesh, postProc.mapGaussPts, commonData));
    }
 
    CHKERR postProcElastic.generateReferenceElementMesh();
    CHKERR postProcElastic.addFieldValuesPostProc("DISPLACEMENTS");
    CHKERR postProcElastic.addFieldValuesPostProc("MESH_NODE_POSITIONS");
    CHKERR postProcElastic.addFieldValuesGradientPostProc("DISPLACEMENTS");
    std::map<int, NonlinearElasticElement::BlockData>::iterator sit =
        commonData.elasticPtr->setOfBlocks.begin();
    for (; sit != commonData.elasticPtr->setOfBlocks.end(); sit++) {
      postProcElastic.getOpPtrVector().push_back(new PostProcStress(
          postProcElastic.postProcMesh, postProcElastic.mapGaussPts,
          "DISPLACEMENTS", sit->second, postProcElastic.commonData));
    }
 
    iNit = true;
  }
 
  if (mass_postproc) {
    Vec mass_vec;
    Vec energ_vec;
    int mass_vec_ghost[] = {0};
    CHKERR VecCreateGhost(PETSC_COMM_WORLD, (!mField.get_comm_rank()) ? 1 : 0,
                          1, 1, mass_vec_ghost, &mass_vec);
 
    CHKERR VecZeroEntries(mass_vec);
    CHKERR VecDuplicate(mass_vec, &energ_vec);
 
    Remodeling::Fe energyCalc(mField);
    CHKERR addHOOpsVol("MESH_NODE_POSITIONS", energyCalc, true, false, false,
                       true);
    energyCalc.getOpPtrVector().push_back(
        new OpCalculateScalarFieldValues("RHO", commonData.data.rhoPtr));
    energyCalc.getOpPtrVector().push_back(new OpCalculateScalarFieldGradient<3>(
        "RHO", commonData.data.gradRhoPtr));
    energyCalc.getOpPtrVector().push_back(
        new OpCalculateVectorFieldGradient<3, 3>("DISPLACEMENTS",
                                                 commonData.data.gradDispPtr));
    energyCalc.getOpPtrVector().push_back(new OpCalculateStress(commonData));
    energyCalc.getOpPtrVector().push_back(
        new OpMassAndEnergyCalculation("RHO", commonData, energ_vec, mass_vec));
    CHKERR DMoFEMLoopFiniteElements(commonData.dm, "FE_REMODELLING",
                                    &energyCalc);
 
    CHKERR VecAssemblyBegin(mass_vec);
    CHKERR VecAssemblyEnd(mass_vec);
    double mass_sum;
    CHKERR VecSum(mass_vec, &mass_sum);
 
    CHKERR VecAssemblyBegin(energ_vec);
    CHKERR VecAssemblyEnd(energ_vec);
    double energ_sum;
    CHKERR VecSum(energ_vec, &energ_sum);
 
    CHKERR PetscPrintf(PETSC_COMM_WORLD,
                       "Time: %g Mass: %6.5g Elastic energy: %6.5g \n", ts_t,
                       mass_sum, energ_sum);
    CHKERR VecDestroy(&mass_vec);
    CHKERR VecDestroy(&energ_vec);
 
    if (equilibrium_flg) {
      double equilibrium_rate =
          fabs(energ_sum - commonData.cUrrent_psi) / energ_sum;
      double equilibrium_mass_rate =
          fabs(mass_sum - commonData.cUrrent_mass) / mass_sum;
      if (equilibrium_rate < rate) {
        CHKERR PetscPrintf(
            PETSC_COMM_WORLD,
            "Energy equilibrium state is achieved! Difference = %0.6g %%. \n",
            equilibrium_rate * 100);
      }
      if (equilibrium_mass_rate < rate) {
        CHKERR PetscPrintf(
            PETSC_COMM_WORLD,
            "Mass equilibrium state is achieved! Difference = %0.6g %%. \n",
            equilibrium_mass_rate * 100);
      }
      commonData.cUrrent_psi = energ_sum;
      commonData.cUrrent_mass = mass_sum;
    }
  }
 
  int step;
#if PETSC_VERSION_LE(3, 8, 0)
  CHKERR TSGetTimeStepNumber(ts, &step);
#else
  CHKERR TSGetStepNumber(ts, &step);
#endif
 
  if ((step) % pRT == 0) {
 
    ostringstream sss;
    sss << "out_" << step << ".h5m";
    CHKERR DMoFEMLoopFiniteElements(commonData.dm, "FE_REMODELLING", &postProc);
    CHKERR postProc.postProcMesh.write_file(sss.str().c_str(), "MOAB",
                                            "PARALLEL=WRITE_PART");
    if (analysis_mesh_flg) {
      ostringstream ttt;
      ttt << "analysis_mesh_" << step << ".h5m";
      CHKERR mField.get_moab().write_file(ttt.str().c_str(), "MOAB",
                                          "PARALLEL=WRITE_PART");
    }
 
    if (commonData.nOremodellingBlock) {
      CHKERR DMoFEMLoopFiniteElements(commonData.dm, "ELASTIC",
                                      &postProcElastic);
      ostringstream ss;
      ss << "out_elastic_" << step << ".h5m";
      CHKERR postProcElastic.postProcMesh.write_file(ss.str().c_str(), "MOAB",
                                                     "PARALLEL=WRITE_PART");
    }
  }
  MoFEMFunctionReturn(0);
}
 
MoFEMErrorCode Remodeling::getParameters() {
 
  MoFEMFunctionBegin;
  CHKERR PetscOptionsBegin(PETSC_COMM_WORLD, "", "Bone remodeling", "none");
 
  commonData.oRder = 2;
  CHKERR PetscOptionsInt("-my_order", "default approximation order", "", 2,
                         &commonData.oRder, PETSC_NULL);
 
  ierr = PetscOptionsEnd();
  CHKERRQ(ierr);
 
  CHKERR mField.get_moab().get_entities_by_type(0, MBTET, commonData.tEts_all);
  for (_IT_CUBITMESHSETS_BY_SET_TYPE_FOR_LOOP_(mField, BLOCKSET, it)) {
    string name = it->getName();
    if (name.compare(0, 14, "NO_REMODELLING") == 0) {
      commonData.nOremodellingBlock = true;
      EntityHandle meshset = it->getMeshset();
      CHKERR this->mField.get_moab().get_entities_by_type(
          meshset, MBTET, commonData.tEts_block, true);
      commonData.tEts_all =
          subtract(commonData.tEts_all, commonData.tEts_block);
    }
  }
 
  MoFEMFunctionReturn(0);
}
 
MoFEMErrorCode Remodeling::addFields() {
 
  MoFEMFunctionBegin;
 
  // Seed all mesh entities to MoFEM database, those entities can be potentially
  // used as finite elements or as entities which carry some approximation
  // field.
  commonData.bitLevel.set(0);
  CHKERR mField.getInterface<BitRefManager>()->setBitRefLevelByDim(
      0, 3, commonData.bitLevel);
  int order = commonData.oRder;
 
  // Add displacement field
  CHKERR mField.add_field("DISPLACEMENTS", H1,
                          /*AINSWORTH_LOBATTO_BASE*/ AINSWORTH_LEGENDRE_BASE, 3,
                          MB_TAG_SPARSE, MF_ZERO);
  // Add field representing ho-geometry
  CHKERR mField.add_field("MESH_NODE_POSITIONS", H1, AINSWORTH_LEGENDRE_BASE, 3,
                          MB_TAG_SPARSE, MF_ZERO);
 
  // Check if density is available, if not add density field.
  bool add_rho_field = false;
  if (!mField.check_field("RHO")) {
    CHKERR mField.add_field("RHO", H1, AINSWORTH_LEGENDRE_BASE, 1,
                            MB_TAG_SPARSE, MF_ZERO);
    // FIXME
    CHKERR mField.add_ents_to_field_by_type(commonData.tEts_all, MBTET, "RHO");
    // CHKERR mField.add_ents_to_field_by_type(0,MBTET,"RHO");
    CHKERR mField.synchronise_field_entities("RHO");
    add_rho_field = true;
 
    CHKERR mField.set_field_order(0, MBVERTEX, "RHO", 1);
    CHKERR mField.set_field_order(0, MBEDGE, "RHO", order - 1);
    CHKERR mField.set_field_order(0, MBTRI, "RHO", order - 1);
    CHKERR mField.set_field_order(0, MBTET, "RHO", order - 1);
  }
 
  // Add entities to field
  CHKERR mField.add_ents_to_field_by_type(0, MBTET, "DISPLACEMENTS");
  CHKERR mField.add_ents_to_field_by_type(0, MBTET, "MESH_NODE_POSITIONS");
 
  // Set approximation order to entities
  CHKERR mField.set_field_order(0, MBVERTEX, "DISPLACEMENTS", 1);
  CHKERR mField.set_field_order(0, MBEDGE, "DISPLACEMENTS", order);
  CHKERR mField.set_field_order(0, MBTRI, "DISPLACEMENTS", order);
  CHKERR mField.set_field_order(0, MBTET, "DISPLACEMENTS", order);
 
  // Assumes that geometry is approximated using 2nd order polynomials.
 
  // Apply 2nd order only on skin
  {
    // Skinner skin(&mField.get_moab());
    // Range faces,tets;
    // CHKERR mField.get_moab().get_entities_by_type(0,MBTET,tets);
    // CHKERR skin.find_skin(0,tets,false,faces);
    // Range edges;
    // CHKERR mField.get_moab().get_adjacencies(
    // faces,1,false,edges,moab::Interface::UNION
    // );
    CHKERR mField.set_field_order(0, MBEDGE, "MESH_NODE_POSITIONS", 2);
  }
 
  CHKERR mField.set_field_order(0, MBVERTEX, "MESH_NODE_POSITIONS", 1);
 
  // Build fields
  CHKERR mField.build_fields();
 
  // If order was not set from CT scans set homogenous order equal to
  // reference bone density
  if (add_rho_field) {
    CHKERR mField.getInterface<FieldBlas>()->setField(commonData.rHo_ref,
                                                      MBVERTEX, "RHO");
    // const DofEntity_multiIndex *dofs_ptr;
    // CHKERR mField.get_dofs(&dofs_ptr);
    // for(_IT_GET_DOFS_FIELD_BY_NAME_FOR_LOOP_(mField,"RHO",dit)) {
    //   cerr << (*dit)->getFieldData() << endl;
    // }
  }
 
  // Project geometry given on 10-node tets on ho-geometry
  Projection10NodeCoordsOnField ent_method_material(mField,
                                                    "MESH_NODE_POSITIONS");
  CHKERR mField.loop_dofs("MESH_NODE_POSITIONS", ent_method_material);
 
  MoFEMFunctionReturn(0);
}
 
MoFEMErrorCode Remodeling::addElements() {
  MoFEMFunctionBegin;
 
  CHKERR mField.add_finite_element("FE_REMODELLING", MF_ZERO);
  CHKERR mField.modify_finite_element_add_field_row("FE_REMODELLING",
                                                    "DISPLACEMENTS");
  CHKERR mField.modify_finite_element_add_field_col("FE_REMODELLING",
                                                    "DISPLACEMENTS");
  CHKERR mField.modify_finite_element_add_field_data("FE_REMODELLING",
                                                     "DISPLACEMENTS");
  CHKERR mField.modify_finite_element_add_field_data("FE_REMODELLING", "RHO");
  CHKERR mField.modify_finite_element_add_field_data("FE_REMODELLING",
                                                     "MESH_NODE_POSITIONS");
  CHKERR mField.modify_finite_element_add_field_row("FE_REMODELLING", "RHO");
  CHKERR mField.modify_finite_element_add_field_col("FE_REMODELLING", "RHO");
  CHKERR mField.add_ents_to_finite_element_by_type(commonData.tEts_all, MBTET,
                                                   "FE_REMODELLING");
 
  CHKERR mField.add_finite_element("ELASTIC");
  CHKERR mField.modify_finite_element_add_field_row("ELASTIC", "DISPLACEMENTS");
  CHKERR mField.modify_finite_element_add_field_col("ELASTIC", "DISPLACEMENTS");
  CHKERR mField.modify_finite_element_add_field_data("ELASTIC",
                                                     "DISPLACEMENTS");
  CHKERR mField.modify_finite_element_add_field_data("ELASTIC",
                                                     "MESH_NODE_POSITIONS");
 
  if (commonData.nOremodellingBlock) {
    CHKERR mField.add_ents_to_finite_element_by_type(commonData.tEts_block,
                                                     MBTET, "ELASTIC");
  }
  
  commonData.elasticMaterialsPtr =
      boost::shared_ptr<ElasticMaterials>(new ElasticMaterials(mField));
  commonData.elasticPtr = boost::shared_ptr<NonlinearElasticElement>(
      new NonlinearElasticElement(mField, 10));
  CHKERR commonData.elasticMaterialsPtr->setBlocks(
      commonData.elasticPtr->setOfBlocks);
  CHKERR commonData.elasticPtr->addElement("ELASTIC", "DISPLACEMENTS");
  CHKERR commonData.elasticPtr->setOperators(
      "DISPLACEMENTS", "MESH_NODE_POSITIONS", false, true);
 
  CHKERR mField.build_finite_elements();
  CHKERR mField.build_adjacencies(commonData.bitLevel);
 
  // Allocate memory for density and gradient of displacements at integration
  // points
  commonData.data.rhoPtr = boost::shared_ptr<VectorDouble>(new VectorDouble());
  commonData.data.gradRhoPtr =
      boost::shared_ptr<MatrixDouble>(new MatrixDouble());
  commonData.data.gradDispPtr =
      boost::shared_ptr<MatrixDouble>(new MatrixDouble());
 
  // Add operators Rhs
  CHKERR addHOOpsVol("MESH_NODE_POSITIONS", *(commonData.feRhs), true, false,
                     false, false);
  auto &list_of_operators_rhs = commonData.feRhs->getOpPtrVector();
  // Get density at integration points
  list_of_operators_rhs.push_back(
      new OpCalculateScalarFieldValues("RHO", commonData.data.rhoPtr));
  list_of_operators_rhs.push_back(
      new OpCalculateScalarFieldGradient<3>("RHO", commonData.data.gradRhoPtr));
  list_of_operators_rhs.push_back(new OpGetRhoTimeDirevative(commonData));
  // Get displacement gradient at integration points
  list_of_operators_rhs.push_back(new OpCalculateVectorFieldGradient<3, 3>(
      "DISPLACEMENTS", commonData.data.gradDispPtr));
  list_of_operators_rhs.push_back(new OpCalculateStress(commonData));
  list_of_operators_rhs.push_back(new OpAssmbleStressRhs(commonData));
  list_of_operators_rhs.push_back(new OpAssmbleRhoRhs(commonData));
 
  // Add operators Lhs
  CHKERR addHOOpsVol("MESH_NODE_POSITIONS", *(commonData.feLhs), true, false,
                     false, false);
  auto &list_of_operators_lhs = commonData.feLhs->getOpPtrVector();
  // Get density at integration points
  list_of_operators_lhs.push_back(
      new OpCalculateScalarFieldValues("RHO", commonData.data.rhoPtr));
  list_of_operators_rhs.push_back(
      new OpCalculateScalarFieldGradient<3>("RHO", commonData.data.gradRhoPtr));
  // Get displacement gradient at integration points
  list_of_operators_lhs.push_back(new OpCalculateVectorFieldGradient<3, 3>(
      "DISPLACEMENTS", commonData.data.gradDispPtr));
  if (commonData.with_adol_c) {
    list_of_operators_lhs.push_back(
        new OpCalculateStressTangentWithAdolc(commonData));
    list_of_operators_lhs.push_back(
        new OpAssmbleStressLhs_dF<true>(commonData));
  } else {
    list_of_operators_lhs.push_back(new OpCalculateStressTangent(commonData));
    list_of_operators_lhs.push_back(
        new OpAssmbleStressLhs_dF<false>(commonData));
  }
  list_of_operators_lhs.push_back(new OpAssmbleRhoLhs_dRho(commonData));
  list_of_operators_lhs.push_back(new OpAssmbleRhoLhs_dF(commonData));
  list_of_operators_lhs.push_back(new OpAssmbleStressLhs_dRho(commonData));
 
  MoFEMFunctionReturn(0);
}
 
MoFEMErrorCode Remodeling::addMomentumFluxes() {
 
  MoFEMFunctionBegin;
  // Add Neumann forces elements
  CHKERR MetaNeummanForces::addNeumannBCElements(mField, "DISPLACEMENTS");
  CHKERR MetaNodalForces::addElement(mField, "DISPLACEMENTS");
  CHKERR MetaEdgeForces::addElement(mField, "DISPLACEMENTS");
 
  // forces and pressures on surface
  CHKERR MetaNeummanForces::setMomentumFluxOperators(
      mField, commonData.neumannForces, PETSC_NULL, "DISPLACEMENTS");
  // noadl forces
  CHKERR MetaNodalForces::setOperators(mField, commonData.nodalForces,
                                       PETSC_NULL, "DISPLACEMENTS");
  // edge forces
  CHKERR MetaEdgeForces::setOperators(mField, commonData.edgeForces, PETSC_NULL,
                                      "DISPLACEMENTS");
 
  for (boost::ptr_map<string, NeummanForcesSurface>::iterator mit =
           commonData.neumannForces.begin();
       mit != commonData.neumannForces.end(); mit++) {
    mit->second->methodsOp.push_back(
        new TimeForceScale("-my_load_history", false));
  }
  for (boost::ptr_map<string, NodalForce>::iterator mit =
           commonData.nodalForces.begin();
       mit != commonData.nodalForces.end(); mit++) {
    mit->second->methodsOp.push_back(
        new TimeForceScale("-my_load_history", false));
  }
  for (boost::ptr_map<string, EdgeForce>::iterator mit =
           commonData.edgeForces.begin();
       mit != commonData.edgeForces.end(); mit++) {
    mit->second->methodsOp.push_back(
        new TimeForceScale("-my_load_history", false));
  }
 
  MoFEMFunctionReturn(0);
}
 
MoFEMErrorCode Remodeling::buildDM() {
 
  MoFEMFunctionBegin;
  commonData.dm_name = "DM_REMODELING";
  // Register DM problem
  CHKERR DMRegister_MoFEM(commonData.dm_name);
  CHKERR DMCreate(PETSC_COMM_WORLD, &commonData.dm);
  CHKERR DMSetType(commonData.dm, commonData.dm_name);
  CHKERR DMMoFEMSetIsPartitioned(commonData.dm, PETSC_TRUE);
  // Create DM instance
  CHKERR DMMoFEMCreateMoFEM(commonData.dm, &mField, commonData.dm_name,
                            commonData.bitLevel);
  // Configure DM form line command options (DM itself, solvers,
  // pre-conditioners, ... )
  CHKERR DMSetFromOptions(commonData.dm);
  // Add elements to dm (only one here)
  CHKERR DMMoFEMAddElement(commonData.dm, "FE_REMODELLING");
  CHKERR DMMoFEMAddElement(commonData.dm, "ELASTIC");
 
  if (mField.check_finite_element("FORCE_FE")) {
    CHKERR DMMoFEMAddElement(commonData.dm, "FORCE_FE");
  }
  if (mField.check_finite_element("PRESSURE_FE")) {
    CHKERR DMMoFEMAddElement(commonData.dm, "PRESSURE_FE");
  }
  mField.getInterface<ProblemsManager>()->buildProblemFromFields = PETSC_TRUE;
  CHKERR DMSetUp(commonData.dm);
 
  MoFEMFunctionReturn(0);
}
 
MoFEMErrorCode Remodeling::solveDM() {
 
  MoFEMFunctionBegin;
 
  Vec F = commonData.F;
  Vec D = commonData.D;
  Mat A = commonData.A;
  DM dm = commonData.dm;
  TS ts = commonData.ts;
 
  CHKERR TSSetIFunction(ts, F, PETSC_NULL, PETSC_NULL);
  CHKERR TSSetIJacobian(ts, A, A, PETSC_NULL, PETSC_NULL);
  double ftime = 1;
#if PETSC_VERSION_GE(3, 8, 0)
  CHKERR TSSetMaxTime(ts, ftime);
#endif
  CHKERR TSSetFromOptions(ts);
  CHKERR TSSetDM(ts, dm);
 
  SNES snes;
  CHKERR TSGetSNES(ts, &snes);
  KSP ksp;
  CHKERR SNESGetKSP(snes, &ksp);
  PC pc;
  CHKERR KSPGetPC(ksp, &pc);
  PetscBool is_pcfs = PETSC_FALSE;
  PetscObjectTypeCompare((PetscObject)pc, PCFIELDSPLIT, &is_pcfs);
 
  // Set up FIELDSPLIT
  // Only is user set -pc_type fieldsplit
  if (is_pcfs == PETSC_TRUE) {
    IS is_disp, is_rho;
    const MoFEM::Problem *problem_ptr;
    CHKERR DMMoFEMGetProblemPtr(dm, &problem_ptr);
    CHKERR mField.getInterface<ISManager>()->isCreateProblemFieldAndRank(
        problem_ptr->getName(), ROW, "DISPLACEMENTS", 0, 3, &is_disp);
    CHKERR mField.getInterface<ISManager>()->isCreateProblemFieldAndRank(
        problem_ptr->getName(), ROW, "RHO", 0, 1, &is_rho);
    CHKERR ISSort(is_disp);
    CHKERR ISSort(is_rho);
    CHKERR PCFieldSplitSetIS(pc, NULL, is_disp);
    CHKERR PCFieldSplitSetIS(pc, NULL, is_rho);
    CHKERR ISDestroy(&is_disp);
    CHKERR ISDestroy(&is_rho);
  }
 
  // Monitor
  MonitorPostProc post_proc(mField, commonData);
  TsCtx *ts_ctx;
  DMMoFEMGetTsCtx(dm, &ts_ctx);
  {
    ts_ctx->getPostProcessMonitor().push_back(&post_proc);
    CHKERR TSMonitorSet(ts, TsMonitorSet, ts_ctx, PETSC_NULL);
  }
 
#if PETSC_VERSION_GE(3, 7, 0)
  CHKERR TSSetExactFinalTime(ts, TS_EXACTFINALTIME_STEPOVER);
#endif
  CHKERR TSSolve(ts, D);
  CHKERR TSGetTime(ts, &ftime);
  PetscInt steps, snesfails, rejects, nonlinits, linits;
#if PETSC_VERSION_LE(3, 8, 0)
  CHKERR TSGetTimeStepNumber(ts, &steps);
#else
  CHKERR TSGetStepNumber(ts, &steps);
#endif
 
  CHKERR TSGetSNESFailures(ts, &snesfails);
  CHKERR TSGetStepRejections(ts, &rejects);
  CHKERR TSGetSNESIterations(ts, &nonlinits);
  CHKERR TSGetKSPIterations(ts, &linits);
  PetscPrintf(PETSC_COMM_WORLD,
              "steps %D (%D rejected, %D SNES fails), ftime %g, nonlinits %D, "
              "linits %D\n",
              steps, rejects, snesfails, ftime, nonlinits, linits);
 
  if (commonData.is_atom_testing) {
    if (commonData.cUrrent_psi < 1.67 || commonData.cUrrent_psi > 1.68)
      SETERRQ(PETSC_COMM_SELF, MOFEM_ATOM_TEST_INVALID, "atom test diverged");
  }
 
  MoFEMFunctionReturn(0);
}
 
MoFEMErrorCode OpMassAndEnergyCalculation::doWork(
    int row_side, EntityType row_type,
    DataForcesAndSourcesCore::EntData &row_data) {
  MoFEMFunctionBegin;
 
  if (row_type != MBVERTEX)
    MoFEMFunctionReturnHot(0);
 
  const int nb_gauss_pts = row_data.getN().size1();
  // commonData.data.vecPsi.resize(nb_gauss_pts,false);
  auto rho = getFTensor0FromVec(*commonData.data.rhoPtr);
  auto psi = getFTensor0FromVec(commonData.data.vecPsi);
 
  double energy = 0;
  double mass = 0;
  for (int gg = 0; gg < nb_gauss_pts; gg++) {
 
    double vol = getVolume() * getGaussPts()(3, gg);
    energy += vol * psi;
    mass += vol * rho;
 
    ++psi;
    ++rho;
  }
 
  CHKERR VecSetValue(energVec, 0, energy, ADD_VALUES);
  CHKERR VecSetValue(massVec, 0, mass, ADD_VALUES);
  MoFEMFunctionReturn(0);
}
 
} // namespace BoneRemodeling