Header.hpp

Go to the documentation of this file.

/* 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/>. */
 
#ifndef __HEADER_HPP__
#define __HEADER_HPP__
 
// declaration
struct CommonData {
 
  MoFEM::Interface &mField;
  Tag thSensitivity;
  Tag thDens;
 
  Vec massVec;
  Vec changeVec;
 
  boost::shared_ptr<VectorDouble> dataRhotGaussPtr;
  boost::shared_ptr<MatrixDouble> dispGradAtGaussPts;
 
  MatrixDouble D;
  VectorDouble sTrain;
  vector<VectorDouble> sTress;
  double volumeFrac;
  MatrixDouble cOmplianceMat;
  VectorDouble sEnsitivityVec;
  VectorDouble rhoVecAtTag;
  MatrixDouble anisLambdaMat;
 
  double edgeLength;
  double lAmbda;
  double mU;
  double lAmbda0;
  double rHo;
  double yOung;
  double pOisson;
  int penalty;
  double mOve;
  double fIlter_radius;
  double cHange;
  // for approx
  double cUrrent_mass; 
  double L_mid; // langange multiplier
  bool uPdate_density;
 
  CommonData(MoFEM::Interface &m_field) : mField(m_field) {
 
    dataRhotGaussPtr = boost::shared_ptr<VectorDouble>(new VectorDouble());
    dispGradAtGaussPts = boost::shared_ptr<MatrixDouble>(new MatrixDouble());
    volumeFrac = 0.5;
    yOung = 10;
    fIlter_radius = 0;
    penalty = 3;
    cHange = 0;
    edgeLength = 1.;
    lAmbda0 = 0.;
    cUrrent_mass = 0;
    L_mid = 0; // langange multiplier
    uPdate_density = false;
    
    anisLambdaMat.resize(3,3,false);
    anisLambdaMat.clear();
 
    ierr = createTags(m_field.get_moab());
  }
  MoFEMErrorCode getParameters() {
    MoFEMFunctionBegin;
    ierr = PetscOptionsBegin(PETSC_COMM_WORLD, "", "Topo Config", "none");
    CHKERR(ierr);
 
    CHKERR PetscOptionsInt("-penalty", "default penalty ", "", penalty, &penalty,
                           PETSC_NULL);
 
    CHKERR PetscOptionsScalar("-young", "default yOung modulus", "", yOung, &yOung,
                              PETSC_NULL);
 
    CHKERR PetscOptionsScalar("-poisson", "poisson ratio", "", 0.1, &pOisson,
                              PETSC_NULL);
 
    CHKERR PetscOptionsScalar("-move", "max move", "", 0.1, &mOve, PETSC_NULL);
    // CHKERR PetscOptionsScalar("-lambda", "lambda smooth", "", lAmbda0, &lAmbda0, PETSC_NULL);
 
    double lambda_coef[3] = {0.,0.,0.};
    int nmax = 3;
    PetscBool is_lambda_defined = PETSC_FALSE;
    PetscOptionsGetRealArray(NULL, "", "-lambda", lambda_coef,
                             &nmax, &is_lambda_defined);
 
    switch (nmax) {
 
    case 0:
    case 1:
      anisLambdaMat(0, 0) = anisLambdaMat(1, 1) = anisLambdaMat(2, 2) =
          lambda_coef[0];
      break;
    case 3:
      anisLambdaMat(0, 0) = lambda_coef[0];
      anisLambdaMat(1, 1) = lambda_coef[1];
      anisLambdaMat(2, 2) = lambda_coef[2];
      break;
    default:
 
      SETERRQ(PETSC_COMM_WORLD, MOFEM_DATA_INCONSISTENCY,
              "Wrong number of lambda parameters, should be 1 or 3, data "
              "inconsistency");
    }
 
    CHKERR PetscOptionsScalar("-volume_frac", "volume frac", "", volumeFrac,
                              &volumeFrac, PETSC_NULL);
 
    // CHKERR PetscOptionsScalar("-filter_r", "default filter radius", "", 0,
    //                           &fIlter_radius, PETSC_NULL);
    ierr = PetscOptionsEnd();
    CHKERRG(ierr);
 
    lAmbda = (yOung * pOisson) / ((1. + pOisson) * (1. - 2. * pOisson));
    mU = yOung / (2. * (1. + pOisson));
 
    const double coefficient = yOung / ((1 + pOisson) * (1 - 2 * pOisson));
    D.resize(36, 1, false);
    D.clear();
 
    FTensor::Ddg<FTensor::PackPtr<double *, 1>, 3, 3> t_D(MAT_TO_DDG(&D));
 
    FTensor::Index<'i', 3> i;
    FTensor::Index<'j', 3> j;
    FTensor::Index<'k', 3> k;
    FTensor::Index<'l', 3> l;
 
    t_D(i, j, k, l) = 0.;
 
    t_D(0, 0, 0, 0) = 1 - pOisson;
    t_D(1, 1, 1, 1) = 1 - pOisson;
    t_D(2, 2, 2, 2) = 1 - pOisson;
 
    t_D(0, 1, 0, 1) = 0.5 * (1 - 2 * pOisson);
    t_D(0, 2, 0, 2) = 0.5 * (1 - 2 * pOisson);
    t_D(1, 2, 1, 2) = 0.5 * (1 - 2 * pOisson);
 
    t_D(0, 0, 1, 1) = pOisson;
    t_D(1, 1, 0, 0) = pOisson;
    t_D(0, 0, 2, 2) = pOisson;
    t_D(2, 2, 0, 0) = pOisson;
    t_D(1, 1, 2, 2) = pOisson;
    t_D(2, 2, 1, 1) = pOisson;
 
    t_D(i, j, k, l) *= coefficient;
 
    MoFEMFunctionReturn(0);
  }
 
 
  MoFEMErrorCode getSensitivityFromTag(const EntityHandle fe_ent, const int nb_gauss_pts) {
   MoFEMFunctionBegin;
    double *tag_data;
    int tag_size;
    CHKERR mField.get_moab().tag_get_by_ptr(
        thSensitivity, &fe_ent, 1, (const void **)&tag_data, &tag_size);
 
    if (tag_size == 1) {
      sEnsitivityVec.resize(nb_gauss_pts , false);
      sEnsitivityVec.clear();
      void const *tag_data[] = {&*sEnsitivityVec.data().begin()};
       const int tag_size = sEnsitivityVec.data().size();
      CHKERR mField.get_moab().tag_set_by_ptr(thSensitivity, &fe_ent, 1,
                                              tag_data, &tag_size);
    } else if (tag_size != nb_gauss_pts) {
      SETERRQ(PETSC_COMM_SELF, MOFEM_DATA_INCONSISTENCY,
              "Wrong size of the tag, data inconsistency");
    } else {
        VectorAdaptor tag_vec = VectorAdaptor(
          tag_size, ublas::shallow_array_adaptor<double>(tag_size, tag_data));
      sEnsitivityVec = tag_vec;
    }
     MoFEMFunctionReturn(0);
  }
 
 
  MoFEMErrorCode setSensitivityToTag(const EntityHandle fe_ent, const double sens) {
    MoFEMFunctionBegin;
    void const *tag_data[] = {&*sEnsitivityVec.data().begin()};
    const int tag_size = sEnsitivityVec.data().size();
    CHKERR mField.get_moab().tag_set_by_ptr(thSensitivity, &fe_ent, 1,
                                            tag_data, &tag_size);
    MoFEMFunctionReturn(0);
  }
 
  MoFEMErrorCode getDensityFromTag(const EntityHandle fe_ent, const int nb_gauss_pts) {
  MoFEMFunctionBegin;
    double *tag_data;
    int tag_size;
    CHKERR mField.get_moab().tag_get_by_ptr(
        thDens, &fe_ent, 1, (const void **)&tag_data, &tag_size);
 
    if (tag_size == 1) {
      rhoVecAtTag.resize(nb_gauss_pts , false);
      rhoVecAtTag.clear();
      void const *tag_data[] = {&*rhoVecAtTag.data().begin()};
       const int tag_size = rhoVecAtTag.data().size();
      CHKERR mField.get_moab().tag_set_by_ptr(thDens, &fe_ent, 1,
                                              tag_data, &tag_size);
    } else if (tag_size != nb_gauss_pts) {
      SETERRQ(PETSC_COMM_SELF, MOFEM_DATA_INCONSISTENCY,
              "Wrong size of the tag, data inconsistency");
    } else {
        VectorAdaptor tag_vec = VectorAdaptor(
          tag_size, ublas::shallow_array_adaptor<double>(tag_size, tag_data));
      rhoVecAtTag = tag_vec;
    }
     MoFEMFunctionReturn(0);
  }
 
  MoFEMErrorCode setDensityToTag(const EntityHandle fe_ent, const double rho) {
    MoFEMFunctionBegin;
    void const *tag_data[] = {&*rhoVecAtTag.data().begin()};
    const int tag_size = rhoVecAtTag.data().size();
    CHKERR mField.get_moab().tag_set_by_ptr(thDens, &fe_ent, 1,
                                            tag_data, &tag_size);
    MoFEMFunctionReturn(0);
  }
 
  MoFEMErrorCode createTags(moab::Interface &moab) {
    MoFEMFunctionBegin;
    std::vector<double> def_val(9, 0);
    CHKERR mField.get_moab().tag_get_handle(
        "SENSITIVITY", 1, MB_TYPE_DOUBLE, thSensitivity,
        MB_TAG_CREAT | MB_TAG_VARLEN | MB_TAG_SPARSE, &def_val[0]);
    CHKERR mField.get_moab().tag_get_handle(
        "RHO", 1, MB_TYPE_DOUBLE, thDens,
        MB_TAG_CREAT | MB_TAG_VARLEN | MB_TAG_SPARSE, &def_val[0]);
    MoFEMFunctionReturn(0);
  }
 
  MoFEMErrorCode build_main_dm(DM &dm, DM &sub_dm_elastic, DM &sub_dm_rho,
                          MoFEM::Interface &m_field, BitRefLevel &bit_level0,
                          PetscBool is_partitioned) {
    MoFEMFunctionBegin;
    DMType dm_name = "DM_TOPO";
    // Register DM problem
    CHKERR DMRegister_MoFEM(dm_name);
    CHKERR DMCreate(PETSC_COMM_WORLD, &dm);
    CHKERR DMSetType(dm, dm_name);
    CHKERR DMMoFEMCreateMoFEM(dm, &m_field, dm_name, bit_level0);
    CHKERR DMSetFromOptions(dm);
    CHKERR DMMoFEMAddElement(dm, "ELASTIC");
    CHKERR DMMoFEMAddElement(dm, "RHO_APPROX");
    if (m_field.check_finite_element("PRESSURE_FE"))
      CHKERR DMMoFEMAddElement(dm, "PRESSURE_FE");
    if (m_field.check_finite_element("FORCE_FE"))
      CHKERR DMMoFEMAddElement(dm, "FORCE_FE");
    CHKERR DMMoFEMSetIsPartitioned(dm, is_partitioned);
    m_field.getInterface<ProblemsManager>()->buildProblemFromFields = PETSC_TRUE;
    CHKERR DMSetUp(dm);
    m_field.getInterface<ProblemsManager>()->buildProblemFromFields = PETSC_FALSE;
 
    
    MoFEMFunctionReturn(0);
  }
 
  MoFEMErrorCode build_sub_dms(DM &dm, DM &sub_dm_elastic, DM &sub_dm_rho,
                               MoFEM::Interface &m_field,
                               BitRefLevel &bit_level0,
                               PetscBool is_partitioned) {
 
    MoFEMFunctionBegin;
    DMType dm_name_sub_elast = "DM_ELASTIC";
    CHKERR DMRegister_MoFEM(dm_name_sub_elast);
    CHKERR DMCreate(PETSC_COMM_WORLD, &sub_dm_elastic);
    CHKERR DMSetType(sub_dm_elastic, dm_name_sub_elast);
    // m_field.getInterface<ProblemsManager>()->buildProblemFromFields =
    // PETSC_TRUE;
    CHKERR DMMoFEMCreateSubDM(sub_dm_elastic, dm, "ELASTIC_PROB");
    CHKERR DMMoFEMSetDestroyProblem(sub_dm_elastic, PETSC_TRUE);
    CHKERR DMMoFEMAddSubFieldRow(sub_dm_elastic, "DISPLACEMENTS");
    CHKERR DMMoFEMAddSubFieldCol(sub_dm_elastic, "DISPLACEMENTS");
    CHKERR DMMoFEMAddElement(sub_dm_elastic, "ELASTIC");
    if (m_field.check_finite_element("PRESSURE_FE"))
      CHKERR DMMoFEMAddElement(sub_dm_elastic, "PRESSURE_FE");
    if (m_field.check_finite_element("FORCE_FE"))
      CHKERR DMMoFEMAddElement(sub_dm_elastic, "FORCE_FE");
 
    CHKERR DMMoFEMSetSquareProblem(sub_dm_elastic, PETSC_TRUE);
    CHKERR DMMoFEMSetIsPartitioned(sub_dm_elastic, is_partitioned);
    CHKERR DMSetUp(sub_dm_elastic);
 
    DMType dm_name_sub_rho = "DM_DENSITY";
    CHKERR DMRegister_MoFEM(dm_name_sub_rho);
    CHKERR DMCreate(PETSC_COMM_WORLD, &sub_dm_rho);
    CHKERR DMSetType(sub_dm_rho, dm_name_sub_rho);
    // m_field.getInterface<ProblemsManager>()->buildProblemFromFields =
    // PETSC_TRUE;
    CHKERR DMMoFEMCreateSubDM(sub_dm_rho, dm, "DENSITY_PROB");
    CHKERR DMMoFEMSetDestroyProblem(sub_dm_rho, PETSC_TRUE);
    CHKERR DMMoFEMAddSubFieldRow(sub_dm_rho, "RHO");
    CHKERR DMMoFEMAddSubFieldCol(sub_dm_rho, "RHO");
    CHKERR DMMoFEMAddElement(sub_dm_rho, "RHO_APPROX");
    CHKERR DMMoFEMSetSquareProblem(sub_dm_rho, PETSC_TRUE);
    CHKERR DMMoFEMSetIsPartitioned(sub_dm_rho, is_partitioned);
    CHKERR DMSetUp(sub_dm_rho);
    MoFEMFunctionReturn(0);
  }
 
  MoFEMErrorCode getBoundBox(const double material_coords[3],
                             std::vector<double> &coords, double &boxSize) {
    MoFEMFunctionBeginHot;
    FTensor::Tensor1<double, 3> t_material_coords(
        material_coords[0], material_coords[1], material_coords[2]);
    FTensor::Tensor1<double *, 3> t_coords(&coords[0], &coords[1], &coords[2],
                                           3);
    FTensor::Index<'i', 3> i;
    double max_xyz[] = {0, 0, 0};
    for (int bb = 0; bb != coords.size() / 3; bb++) {
      t_coords(i) -= t_material_coords(i);
      for (int dd = 0; dd != 3; dd++) {
        max_xyz[dd] = (max_xyz[dd] < fabs(t_coords(dd))) ? fabs(t_coords(dd))
                                                         : max_xyz[dd];
      }
      ++t_coords;
    }
    //take the max of the box 
    boxSize = (max_xyz[0] > max_xyz[1]) ? max_xyz[0] : max_xyz[1];
    boxSize = (boxSize > max_xyz[2]) ? boxSize : max_xyz[2];
    MoFEMFunctionReturnHot(0);
  }
};
 
 
struct OpAssembleK
    : public VolumeElementForcesAndSourcesCore::UserDataOperator {
 
  MatrixDouble rowB;
  MatrixDouble colB;
  MatrixDouble CB;
  MatrixDouble K, transK;
 
  CommonData &commonData;
 
  OpAssembleK(CommonData &common_data, bool symm = true)
      : VolumeElementForcesAndSourcesCore::UserDataOperator(
            "DISPLACEMENTS", "DISPLACEMENTS", OPROWCOL, symm),
        commonData(common_data) {}
 
  /**
   * \brief Do calculations for give operator
   * @param  row_side row side number (local number) of entity on element
   * @param  col_side column side number (local number) of entity on element
   * @param  row_type type of row entity MBVERTEX, MBEDGE, MBTRI or MBTET
   * @param  col_type type of column entity MBVERTEX, MBEDGE, MBTRI or MBTET
   * @param  row_data data for row
   * @param  col_data data for column
   * @return          error code
   */
  MoFEMErrorCode doWork(int row_side, int col_side, EntityType row_type,
                        EntityType col_type,
                        DataForcesAndSourcesCore::EntData &row_data,
                        DataForcesAndSourcesCore::EntData &col_data) {
    MoFEMFunctionBegin;
    // get number of dofs on row
    nbRows = row_data.getIndices().size();
    // if no dofs on row, exit that work, nothing to do here
    if (!nbRows)
      MoFEMFunctionReturnHot(0);
    // get number of dofs on column
    nbCols = col_data.getIndices().size();
    // if no dofs on Columbia, exit nothing to do here
    if (!nbCols)
      MoFEMFunctionReturnHot(0);
    // get number of integration points
    nbIntegrationPts = getGaussPts().size2();
    // chekk if entity block is on matrix diagonal
    if (row_side == col_side && row_type == col_type) { // ASK
      isDiag = true;                                    // yes, it is
    } else {
      isDiag = false;
    }
 
    // integrate local matrix for entity block
    CHKERR iNtegrate(row_data, col_data);
    // asseble local matrix
    CHKERR aSsemble(row_data, col_data);
 
    MoFEMFunctionReturn(0);
  }
 
protected:
  ///< error code
 
  int nbRows;           ///< number of dofs on rows
  int nbCols;           ///< number if dof on column
  int nbIntegrationPts; ///< number of integration points
  bool isDiag;          ///< true if this block is on diagonal
 
  /**
   * \brief Integrate grad-grad operator
   * @param  row_data row data (consist base functions on row entity)
   * @param  col_data column data (consist base functions on column entity)
   * @return          error code
   */
  virtual MoFEMErrorCode
  iNtegrate(DataForcesAndSourcesCore::EntData &row_data,
            DataForcesAndSourcesCore::EntData &col_data) {
    MoFEMFunctionBegin;
 
    int nb_dofs_row = row_data.getFieldData().size();
    if (nb_dofs_row == 0)
      MoFEMFunctionReturnHot(0);
    int nb_dofs_col = col_data.getFieldData().size();
    if (nb_dofs_col == 0)
      MoFEMFunctionReturnHot(0);
 
    K.resize(nb_dofs_row, nb_dofs_col, false);
    K.clear();
    auto max = [](double a, double b) { return a > b ? a : b; };
    auto min = [](double a, double b) { return a < b ? a : b; };
    auto rho = getFTensor0FromVec(*commonData.dataRhotGaussPtr);
 
          auto get_tensor2 = [](MatrixDouble &m, const int r, const int c) {
        return FTensor::Tensor2<FTensor::PackPtr<double *, 3>, 3, 3>(
            &m(r + 0, c + 0), &m(r + 0, c + 1), &m(r + 0, c + 2),
            &m(r + 1, c + 0), &m(r + 1, c + 1), &m(r + 1, c + 2),
            &m(r + 2, c + 0), &m(r + 2, c + 1), &m(r + 2, c + 2));
      };
 
      FTensor::Index<'i', 3> i;
      FTensor::Index<'j', 3> j;
      FTensor::Index<'k', 3> k;
      FTensor::Index<'l', 3> l;
 
      // get element volume
      double vol = getVolume();
 
      // get intergrayion weights
      auto t_w = getFTensor0IntegrationWeight();
 
      auto t_row_diff_base = row_data.getFTensor1DiffN<3>();
      const int row_nb_base_fun = row_data.getN().size2();
 
      FTensor::Ddg<FTensor::PackPtr<double *, 1>, 3, 3> t_D(
          MAT_TO_DDG(&commonData.D));
 
      for (int gg = 0; gg != nbIntegrationPts; gg++) {
 
        const double coeff = pow(rho, commonData.penalty);
        double a = t_w * vol;
        if (getHoGaussPtsDetJac().size()) {
          a *= getHoGaussPtsDetJac()[gg];
        }
        a *= coeff;
        int rr = 0;
        for (; rr != nbRows / 3; ++rr) {
          auto t_m = get_tensor2(K, 3 * rr, 0);
          auto t_col_diff_base = col_data.getFTensor1DiffN<3>(gg, 0);
          FTensor::Christof<double, 3, 3> t_rowD;
          t_rowD(l, j, k) = t_D(i, j, k, l) * (a * t_row_diff_base(i));
          for (int cc = 0; cc != nbCols / 3; ++cc) {
            t_m(i, j) += t_rowD(i, j, k) * t_col_diff_base(k);
            ++t_col_diff_base;
            ++t_m;
          }
          ++t_row_diff_base;
        }
        for (; rr != row_nb_base_fun; ++rr)
          ++t_row_diff_base;
        ++t_w;
        ++rho;
      }
      MoFEMFunctionReturn(0);
  }
 
  /**
   * \brief Assemble local entity block matrix
   * @param  row_data row data (consist base functions on row entity)
   * @param  col_data column data (consist base functions on column entity)
   * @return          error code
   */
  virtual MoFEMErrorCode aSsemble(DataForcesAndSourcesCore::EntData &row_data,
                                  DataForcesAndSourcesCore::EntData &col_data) {
    MoFEMFunctionBegin;
 
    int nb_dofs_row = row_data.getFieldData().size();
    if (nb_dofs_row == 0)
      MoFEMFunctionReturnHot(0);
    int nb_dofs_col = col_data.getFieldData().size();
    if (nb_dofs_col == 0)
      MoFEMFunctionReturnHot(0);
 
    // get pointer to first global index on row
    const int *row_indices = &*row_data.getIndices().data().begin();
    // get pointer to first global index on column
    const int *col_indices = &*col_data.getIndices().data().begin();
    Mat B = getFEMethod()->ksp_B != PETSC_NULL ? getFEMethod()->ksp_B
                                               : getFEMethod()->snes_B;
 
    CHKERR MatSetValues(B, nbRows, row_indices, nbCols, col_indices,
                        &*K.data().begin(), ADD_VALUES);
    if (!isDiag && sYmm) {
      K = trans(K);
      CHKERR MatSetValues(B, nbCols, col_indices, nbRows, row_indices,
                          &*K.data().begin(), ADD_VALUES);
    }
    MoFEMFunctionReturn(0);
  }
};
 
 
 
struct OpCalculateSensitivities
    : public MoFEM::VolumeElementForcesAndSourcesCore::UserDataOperator {
  CommonData &commonData;
  // double &volumeVec;
  OpCalculateSensitivities(CommonData &common_data)
      : VolumeElementForcesAndSourcesCore::UserDataOperator(
            "RHO", UserDataOperator::OPROW),
        commonData(common_data) {}
 
  MoFEMErrorCode doWork(int side, EntityType type,
                        DataForcesAndSourcesCore::EntData &data) {
    MoFEMFunctionBegin;
    if (type != MBVERTEX)
      MoFEMFunctionReturnHot(0);
    int nb_dofs = data.getFieldData().size();
    if (nb_dofs == 0) {
      MoFEMFunctionReturnHot(0);
    }
    const double p = commonData.penalty;
    const EntityHandle fe_ent = getFEEntityHandle();
    const int nb_gauss_pts = getGaussPts().size2();
 
    FTensor::Index<'i', 3> i;
    FTensor::Index<'j', 3> j;
 
    auto rho = getFTensor0FromVec(*commonData.dataRhotGaussPtr);
    commonData.rhoVecAtTag.resize(nb_gauss_pts);
    commonData.rhoVecAtTag.clear();
    auto rho_tag = getFTensor0FromVec(commonData.rhoVecAtTag);
    auto grad = getFTensor2FromMat<3, 3>(*commonData.dispGradAtGaussPts);
 
    FTensor::Tensor2_symmetric<double, 3> strain;
    const double lambda = commonData.lAmbda;
    const double mu = commonData.mU;
 
    commonData.getSensitivityFromTag(fe_ent, nb_gauss_pts);
    commonData.getDensityFromTag(fe_ent, nb_gauss_pts);
    auto t_sensitivity = getFTensor0FromVec(commonData.sEnsitivityVec);
 
    double newSensitivity = 0;
    for (int gg = 0; gg != nb_gauss_pts; ++gg) {
 
      double vol = getVolume() * getGaussPts()(3, gg);
      strain(i, j) = 0.5 * (grad(i, j) || grad(j, i));
      const double trace = strain(i, i);
 
      double energy =
          0.5 * trace * trace * lambda + mu * strain(i, j) * strain(i, j);
      rho = rho > 1. ? 1. : (rho < 0.001 ? 0.001 : rho); //FIXME: use lobatto quadrature to avoid this
      t_sensitivity = (-p) * pow(rho, p - 1) * energy * vol; //
      // if(rho < 0.001 || rho > 1.00001)
      //   SETERRQ(PETSC_COMM_WORLD, MOFEM_DATA_INCONSISTENCY,
      //           "rho is not within boundaries, that should not happen");
      if (t_sensitivity > 0) //FIXME:
        SETERRQ(
            PETSC_COMM_WORLD, MOFEM_DATA_INCONSISTENCY,
            "t_sensitivity is not within boundaries, that should not happen");
      rho_tag = rho;
 
      ++t_sensitivity;
      ++rho;
      ++grad;
      ++rho_tag;
    }
 
    commonData.setDensityToTag(fe_ent, nb_gauss_pts);
    commonData.setSensitivityToTag(fe_ent, nb_gauss_pts);
  
    MoFEMFunctionReturn(0);
  }
};
 
 
template <bool UPDATE = true>
struct OpUpdateDensity
    : public MoFEM::VolumeElementForcesAndSourcesCore::UserDataOperator {
  CommonData &commonData;
  OpUpdateDensity(CommonData &common_data)
      : VolumeElementForcesAndSourcesCore::UserDataOperator(
            "RHO", UserDataOperator::OPROW),
        commonData(common_data) {}
 
  MoFEMErrorCode doWork(int side, EntityType type,
                        DataForcesAndSourcesCore::EntData &data) {
    MoFEMFunctionBegin;
    if (type != MBVERTEX)
      MoFEMFunctionReturnHot(0);
    int nb_dofs = data.getFieldData().size();
    if (nb_dofs == 0) {
      MoFEMFunctionReturnHot(0);
    }
 
    int nb_gauss_pts = data.getN().size1();
 
    EntityHandle fe_ent = getFEEntityHandle();
    commonData.getDensityFromTag(fe_ent, nb_gauss_pts);
    const int &p = commonData.penalty;
    auto rho_old = getFTensor0FromVec(commonData.rhoVecAtTag);
    auto rho_new = getFTensor0FromVec(*commonData.dataRhotGaussPtr);
    const double &mv = commonData.mOve;
    commonData.getSensitivityFromTag(fe_ent, nb_gauss_pts);
    auto t_sens = getFTensor0FromVec(commonData.sEnsitivityVec);
 
    auto max = [](double a, double b) { return a > b ? a : b; };
    auto min = [](double a, double b) { return a < b ? a : b; };
 
 
    double change = 0;
    for (int gg = 0; gg != nb_gauss_pts; gg++) {
 
      VectorAdaptor shape_function = data.getN(gg, nb_dofs);
 
      const double vol = getVolume() * getGaussPts()(3,gg);
      const double sensitivity = t_sens;
      // double rho = rho_old > 1.0 ? 1. : (rho_old < 0.001 ? 0.001 : rho_old);
      double rho = rho_old ;
 
      const double &x = rho;
 
     //FROM ZEGARD PAPER
      const double den = commonData.L_mid * vol; 
      const double xnew = rho * sqrt(-sensitivity/den);
      
      rho_new = max(0.001, min(1.0, max(x - mv, min(1.0, min(x + mv, xnew)))));
      if (rho_new < 0.001 || rho_new > 1.00001)
        SETERRQ(PETSC_COMM_WORLD, MOFEM_DATA_INCONSISTENCY,
                "rho_new is not within boundaries, that should not happen");
      if (rho_old < 0.001 || rho_old > 1.00001)
        SETERRQ(PETSC_COMM_WORLD, MOFEM_DATA_INCONSISTENCY,
                "rho_new is not within boundaries, that should not happen");
      const double &check = rho_new;
      if (!std::isnormal(check))
        SETERRQ(PETSC_COMM_WORLD, MOFEM_DATA_INCONSISTENCY,
                "rho_new is not normal, that should not happen"); //FIXME:
      if (UPDATE)
        change += vol * fabs(rho_old - rho_new);
 
      ++t_sens;
      ++rho_new;
      ++rho_old;
    }
    if (UPDATE)
      CHKERR VecSetValue(commonData.changeVec,0,change,ADD_VALUES);
 
    MoFEMFunctionReturn(0);
  }
};
 
 
struct OpVolumeCalculation
    : public MoFEM::VolumeElementForcesAndSourcesCore::UserDataOperator {
 
  Vec &volumeVec;
  OpVolumeCalculation(Vec &volume_vec)
      : MoFEM::VolumeElementForcesAndSourcesCore::UserDataOperator(
            "RHO", UserDataOperator::OPROW),
        volumeVec(volume_vec) {}
 
  MoFEMErrorCode doWork(int row_side, EntityType row_type,
                        DataForcesAndSourcesCore::EntData &row_data) {
    MoFEMFunctionBegin;
    if (row_type != MBVERTEX)
      MoFEMFunctionReturnHot(0);
    double vol = getVolume();
    CHKERR VecSetValue(volumeVec, 0, vol, ADD_VALUES);
 
    MoFEMFunctionReturn(0);
  }
};
 
struct OpMassCalculation
    : public MoFEM::VolumeElementForcesAndSourcesCore::UserDataOperator {
 
  Vec massVec;
  CommonData &commonData;
 
  OpMassCalculation(const string &field_name, CommonData &common_data,
                    Vec mass_vec)
      : MoFEM::VolumeElementForcesAndSourcesCore::UserDataOperator(
            field_name, UserDataOperator::OPROW),
        commonData(common_data), massVec(mass_vec) {}
 
  MoFEMErrorCode doWork(int row_side, EntityType row_type,
                        DataForcesAndSourcesCore::EntData &row_data) {
    MoFEMFunctionBegin;
    if (row_type != MBVERTEX)
      MoFEMFunctionReturnHot(0);
    int nb_gauss_pts = row_data.getN().size1();
    auto rho = getFTensor0FromVec(*commonData.dataRhotGaussPtr);
    double mass = 0;
    for (int gg = 0; gg < nb_gauss_pts; gg++) {
 
      double vol = getVolume() * getGaussPts()(3, gg);
      if (getHoGaussPtsDetJac().size() > 0) {
        vol *= getHoGaussPtsDetJac()[gg]; ///< higher order geometry
      }
      mass += rho * vol;
      // commonData.cUrrent_mass += rho * vol;
      ++rho;
    }
      CHKERR VecSetValue(commonData.massVec,0,mass,ADD_VALUES);
    MoFEMFunctionReturn(0);
  }
};
 
struct OpAssembleRhoLhs
    : public MoFEM::VolumeElementForcesAndSourcesCore::UserDataOperator {
 
  CommonData &commonData;
  MatrixDouble locK;
  MatrixDouble transK;
 
  OpAssembleRhoLhs(const std::string &row_field, const std::string &col_field,
                   CommonData &common_data)
      : MoFEM::VolumeElementForcesAndSourcesCore::UserDataOperator(
            row_field, col_field, UserDataOperator::OPROWCOL),
        commonData(common_data) {
    sYmm = true;
  }
 
  MoFEMErrorCode doWork(int row_side, int col_side, EntityType row_type,
                        EntityType col_type,
                        DataForcesAndSourcesCore::EntData &row_data,
                        DataForcesAndSourcesCore::EntData &col_data) {
 
    MoFEMFunctionBegin;
 
    int nb_rows = row_data.getIndices().size(); // number of rows
    int nb_cols = col_data.getIndices().size(); // number of columns
    if (nb_rows == 0)
      MoFEMFunctionReturnHot(0);
    if (nb_cols == 0)
      MoFEMFunctionReturnHot(0);
 
    locK.resize(nb_rows, nb_cols, false);
    locK.clear();
 
    // double lsquared = commonData.lAmbda0 * commonData.lAmbda0 *
    //                         commonData.edgeLength * commonData.edgeLength;
    MatrixDouble anisotropic_filter = commonData.anisLambdaMat;
    anisotropic_filter *= commonData.edgeLength;
    // anisotropic_filter.resize(3,3);
    // anisotropic_filter.clear();
    // anisotropic_filter(0,0) = sqrt(lsquared);
    // anisotropic_filter(1,1) = sqrt(lsquared);
    // anisotropic_filter(2,2) = 1e2 * commonData.edgeLength;
 
    // lsquared = commonData.lAmbda0; //FIXME:
    int nb_gauss_pts = row_data.getN().size1();
    for (int gg = 0; gg < nb_gauss_pts; gg++) {
      double vol = getVolume() * getGaussPts()(3, gg);
      if (getHoGaussPtsDetJac().size() > 0) {
        vol *= getHoGaussPtsDetJac()[gg]; ///< higher order geometry
      }
      VectorAdaptor row_base_function = row_data.getN(gg);
      VectorAdaptor col_base_function = col_data.getN(gg);
      locK += vol * outer_prod(row_base_function, col_base_function);
 
      MatrixAdaptor row_diff_base_function = row_data.getDiffN(gg);
      MatrixAdaptor col_diff_base_function = col_data.getDiffN(gg);
 
    row_diff_base_function = prod(row_diff_base_function, anisotropic_filter);
    col_diff_base_function = prod(col_diff_base_function, anisotropic_filter);
 
    // locK += vol * lsquared * //FIXME:
    locK += vol * prod(row_diff_base_function, trans(col_diff_base_function));
    }
    CHKERR MatSetValues(getFEMethod()->ksp_B, row_data.getIndices().size(),
                        &row_data.getIndices()[0], col_data.getIndices().size(),
                        &col_data.getIndices()[0], &locK(0, 0), ADD_VALUES);
 
    // Assemble off symmetric side
    if (row_side != col_side || row_type != col_type) {
      transK.resize(locK.size2(), locK.size1(), false);
      noalias(transK) = trans(locK);
      CHKERR MatSetValues(getFEMethod()->ksp_B, col_data.getIndices().size(),
                          &col_data.getIndices()[0],
                          row_data.getIndices().size(),
                          &row_data.getIndices()[0], &transK(0, 0), ADD_VALUES);
    }
 
    MoFEMFunctionReturn(0);
  }
 
  };
 
  struct OpAssembleDensityRhs
      : public MoFEM::VolumeElementForcesAndSourcesCore::UserDataOperator {
 
    CommonData &commonData;
    VectorDouble nF;
    OpAssembleDensityRhs(const string &row_field, CommonData &common_data)
        : MoFEM::VolumeElementForcesAndSourcesCore::UserDataOperator(
              row_field, row_field, UserDataOperator::OPROW),
          commonData(common_data) {}
 
    MoFEMErrorCode doWork(int row_side, EntityType row_type,
                          DataForcesAndSourcesCore::EntData &row_data) {
      MoFEMFunctionBegin;
 
      int nb_rows = row_data.getIndices().size();
      if (nb_rows == 0)
        MoFEMFunctionReturnHot(0);
      nF.resize(nb_rows, false);
      nF.clear();
 
      auto rho = getFTensor0FromVec(*commonData.dataRhotGaussPtr);
      // auto rho_old = getFTensor0FromVec(commonData.rhoVecAtTag);
      int nb_gauss_pts = row_data.getN().size1();
      
      for (int gg = 0; gg < nb_gauss_pts; gg++) {
 
        double vol = getVolume() * getGaussPts()(3, gg);
        if (getHoGaussPtsDetJac().size() > 0) {
          vol *= getHoGaussPtsDetJac()[gg]; ///< higher order geometry
        }
          VectorAdaptor base_function = row_data.getN(gg);
 
          nF += vol * rho * base_function;
 
          if (rho > 1.000001 || rho < 0.001) // for debug only
            SETERRQ(PETSC_COMM_SELF, MOFEM_DATA_INCONSISTENCY, //FIXME:
                    "wrong rho. data inconsistency");
 
          ++rho;
      }
 
      CHKERR VecSetValues(getFEMethod()->ksp_f, row_data.getIndices().size(),
                          &row_data.getIndices()[0], &nF[0], ADD_VALUES);
 
      MoFEMFunctionReturn(0);
    }
  };
 
struct OpPostProcCompliance
    : public MoFEM::VolumeElementForcesAndSourcesCore::UserDataOperator {
  moab::Interface &postProcMesh;
  std::vector<EntityHandle> &mapGaussPts;
  CommonData &commonData;
 
  OpPostProcCompliance(moab::Interface &post_proc_mesh,
                       std::vector<EntityHandle> &map_gauss_pts,
                       CommonData &common_data)
      : VolumeElementForcesAndSourcesCore::UserDataOperator(
            "DISPLACEMENTS", UserDataOperator::OPROW),
        postProcMesh(post_proc_mesh),mapGaussPts(map_gauss_pts),commonData(common_data) {}
 
  MoFEMErrorCode doWork(int side, EntityType type,
                        DataForcesAndSourcesCore::EntData &data) {
    MoFEMFunctionBegin;
    if (type != MBVERTEX)
      MoFEMFunctionReturnHot(0);
    double def_VAL[9];
    bzero(def_VAL, 9 * sizeof(double));
    FTensor::Tensor2_symmetric<double, 3> strain;
    FTensor::Index<'i', 3> i;
    FTensor::Index<'j', 3> j;
    Tag th_psi;
 
    const double p = commonData.penalty;
    const double lambda = commonData.lAmbda;
    const double mu = commonData.mU;
 
    CHKERR postProcMesh.tag_get_handle("compliance", 1, MB_TYPE_DOUBLE, th_psi,
                                       MB_TAG_CREAT | MB_TAG_SPARSE, def_VAL);
    auto rho = getFTensor0FromVec(*commonData.dataRhotGaussPtr);
    auto grad = getFTensor2FromMat<3, 3>(*commonData.dispGradAtGaussPts);
 
    const int nb_gauss_pts = mapGaussPts.size();
 
    for (int gg = 0; gg != nb_gauss_pts; gg++) {
      
      strain(i, j) = 0.5 * (grad(i, j) || grad(j, i));
      const double trace = grad(i, i);
      const double energy =
      // const double energy = pow(rho, p) * 
          0.5 * lambda * trace * trace + mu * strain(i, j) * strain(i, j);
 
      CHKERR postProcMesh.tag_set_data(th_psi, &mapGaussPts[gg], 1, &energy);
 
      ++rho;
      ++grad;
    }
 
    MoFEMFunctionReturn(0);
  }
};
 
struct DirichletDensityBc : public DirichletDisplacementBc {
 
  DirichletDensityBc(MoFEM::Interface &m_field, const std::string &field_name,
                     Mat aij, Vec x, Vec f)
      : DirichletDisplacementBc(m_field, field_name, aij, x, f) {}
 
  DirichletDensityBc(MoFEM::Interface &m_field, const std::string &field_name)
      : DirichletDisplacementBc(m_field, field_name) {}
 
  MoFEMErrorCode iNitalize() {
    MoFEMFunctionBegin;
    if (mapZeroRows.empty() || !methodsOp.empty()) {
      ParallelComm *pcomm =
          ParallelComm::get_pcomm(&mField.get_moab(), MYPCOMM_INDEX);
 
      for (_IT_CUBITMESHSETS_BY_SET_TYPE_FOR_LOOP_(mField, BLOCKSET, it)) {
        string name = it->getName();
        if (name.compare(0, 7, "DENSITY") == 0) {
          // if (name.compare(0, 7, "fdsfdsfsd") == 0) {
 
          VectorDouble scaled_values(1);
          scaled_values[0] = 1;
 
          for (int dim = 0; dim < 3; dim++) {
            Range ents;
            CHKERR it->getMeshsetIdEntitiesByDimension(mField.get_moab(), dim,
                                                       ents, true);
            if (dim > 1) {
              Range _edges;
              CHKERR mField.get_moab().get_adjacencies(ents, 1, false, _edges,
                                                       moab::Interface::UNION);
              ents.insert(_edges.begin(), _edges.end());
            }
            if (dim > 0) {
              Range _nodes;
              CHKERR mField.get_moab().get_connectivity(ents, _nodes, true);
              ents.insert(_nodes.begin(), _nodes.end());
            }
            auto &dofs_by_uid = problemPtr->getNumeredRowDofs()->get<Unique_mi_tag>();
            for (auto eit = ents.pair_begin(); eit != ents.pair_end(); ++eit) {
              auto bit_number = mField.get_field_bit_number(fieldName);
              auto lo_dit = dofs_by_uid.lower_bound(
                  DofEntity::getLoFieldEntityUId(bit_number, eit->first));
              auto hi_dit = dofs_by_uid.upper_bound(
                  DofEntity::getHiFieldEntityUId(bit_number, eit->second));
              for (; lo_dit != hi_dit; ++lo_dit) {
                auto &dof = *lo_dit;
                if (dof->getEntType() == MBVERTEX) {
                  mapZeroRows[dof->getPetscGlobalDofIdx()] = scaled_values[0];
                } else {
                  mapZeroRows[dof->getPetscGlobalDofIdx()] = 0;
                }
              }
            }
            // for (Range::iterator eit = ents.begin(); eit != ents.end(); eit++) {
            //   for (_IT_NUMEREDDOF_ROW_BY_NAME_ENT_PART_FOR_LOOP_(
            //            problemPtr, fieldName, *eit, pcomm->rank(), dof_ptr)) {
            //     NumeredDofEntity *dof = dof_ptr->get();
            //     if (dof->getEntType() == MBVERTEX) {
            //       mapZeroRows[dof->getPetscGlobalDofIdx()] = scaled_values[0];
            //       // dof->getFieldData() = scaled_values[0];
            //     } else {
            //       mapZeroRows[dof->getPetscGlobalDofIdx()] = 0;
            //       // dof->getFieldData() = 0;
            //     }
            //   }
            //  }
            // }
          }
        }
      }
      dofsIndices.resize(mapZeroRows.size());
      dofsValues.resize(mapZeroRows.size());
      int ii = 0;
      std::map<DofIdx, FieldData>::iterator mit = mapZeroRows.begin();
      for (; mit != mapZeroRows.end(); mit++, ii++) {
        dofsIndices[ii] = mit->first;
        dofsValues[ii] = mit->second;
      }
    }
    MoFEMFunctionReturn(0);
  }
 
  MoFEMErrorCode postProcess() {
    MoFEMFunctionBegin;
 
    switch (ts_ctx) {
    case CTX_TSSETIFUNCTION: {
      snes_ctx = CTX_SNESSETFUNCTION;
      snes_x = ts_u;
      snes_f = ts_F;
      break;
    }
    case CTX_TSSETIJACOBIAN: {
      snes_ctx = CTX_SNESSETJACOBIAN;
      snes_B = ts_B;
      break;
    }
    default:
      break;
    }
 
    if (snes_B) {
      CHKERR MatAssemblyBegin(snes_B, MAT_FINAL_ASSEMBLY);
      CHKERR MatAssemblyEnd(snes_B, MAT_FINAL_ASSEMBLY);
      CHKERR MatZeroRowsColumns(snes_B, dofsIndices.size(),
                                dofsIndices.empty() ? PETSC_NULL
                                                    : &dofsIndices[0],
                                dIag, PETSC_NULL, PETSC_NULL);
    }
    if (snes_f) {
      CHKERR VecAssemblyBegin(snes_f);
      CHKERR VecAssemblyEnd(snes_f);
      for (std::vector<int>::iterator vit = dofsIndices.begin();
           vit != dofsIndices.end(); vit++) {
        CHKERR VecSetValue(snes_f, *vit, 0., INSERT_VALUES); 
      }
      CHKERR VecAssemblyBegin(snes_f);
      CHKERR VecAssemblyEnd(snes_f);
    }
 
    MoFEMFunctionReturn(0);
  }
};
 
#endif //__HEADER_HPP__