DensityMaps.hpp

Go to the documentation of this file.

/** \file DensityMaps.hpp
* \ingroup bone_remodelling
 
* \brief Operator can be used with any volume element to calculate sum of
* volumes of all volumes in the set
 
*/
 
/*
* 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 __DENSITY_MAPS_HPP__
#define __DENSITY_MAPS_HPP__
 
namespace BoneRemodeling {
  /**
   * \brief Create KDTree on surface of the mesh and calculate distance.
   * \ingroup bone_remodelling
   */
  struct SurfaceKDTree {
 
    MoFEM::Interface &mField;
    EntityHandle kdTreeRootMeshset;
    AdaptiveKDTree kdTree;
 
 
    SurfaceKDTree(MoFEM::Interface &m_field):
    mField(m_field),
    kdTree(&m_field.get_moab()) {
    }
 
 
 
 
    Range sKin; //< Skin mesh, i.e. mesh on surface
 
    /**
     * \brief Take a skin from a mesh.
     *
     * @param  volume Volume elements in the mesh
     * @return        Error code
     */
    PetscErrorCode takeASkin(Range &volume) {
      PetscFunctionBegin;
      Skinner skin(&mField.get_moab());
      rval = skin.find_skin(0,volume,false,sKin); CHKERRQ_MOAB(rval);
      // Only for debugging
      // EntityHandle meshset;
      // rval = mField.get_moab().create_meshset(MESHSET_SET,meshset);
      // rval = mField.get_moab().add_entities(meshset,sKin); CHKERRQ(rval);
      // rval = mField.get_moab().write_file("skin.vtk","VTK","",&meshset,1); CHKERRQ(rval);
      PetscFunctionReturn(0);
    }
 
    /**
     * \brief Build tree
     *
     * @return Error code
     */
    PetscErrorCode buildTree() {
      PetscFunctionBegin;
      rval = kdTree.build_tree(sKin,&kdTreeRootMeshset); CHKERRQ_MOAB(rval);
      PetscFunctionReturn(0);
    }
 
    /**
     * \brief Find point on surface which is closet to given point coordinates
     * @param  x  1st coordinate of point
     * @param  y  2nd coordinate of point
     * @param  z  3rd coordinate of point
     * @param  dx coordinate of distance vector
     * @param  dy coordinate of distance vector
     * @param  z  coordinate of distance vector
     * @return    Error code
     */
    PetscErrorCode findClosestPointToTheSurface(
      const double x,
      const double y,
      const double z,
      double &dx,
      double &dy,
      double &dz
    ) {
      PetscFunctionBegin;
      const double coords[] = {x,y,z};
      double closest_point[3];
      EntityHandle triangle;
      rval = kdTree.closest_triangle(
        kdTreeRootMeshset,
        coords,
        closest_point,
        triangle
      ); CHKERRQ_MOAB(rval);
      cblas_daxpy(3,-1,coords,1,closest_point,1);
      dx = closest_point[0];
      dy = closest_point[1];
      dz = closest_point[2];
      PetscFunctionReturn(0);
    }
 
    /**
     * \brief Set distance to vertices on mesh
     *
     * @param  field_name  Name of field for distances
     * @return            Error Code
     */
    PetscErrorCode setDistanceFromSurface(const std::string field_name) {
      PetscFunctionBegin;
      auto field_ents = mField.get_field_ents();
      auto field_bit_number = mField.get_field_bit_number(field_name);
      auto it = field_ents->lower_bound(
          FieldEntity::getLoBitNumberUId(field_bit_number));
      auto hi_it = field_ents->upper_bound(
          FieldEntity::getHiBitNumberUId(field_bit_number));
      // Get values at nodes first
      for(;it!=hi_it;it++) {
        EntityType type = it->get()->getEntType();
        if(type!=MBVERTEX) {
          continue;
        }
        EntityHandle ent = it->get()->getEnt();
        double coords[3];
        rval = mField.get_moab().get_coords(&ent,1,coords); CHKERRQ_MOAB(rval);
        double delta[3];
        ierr = findClosestPointToTheSurface(
          coords[0],coords[1],coords[2],delta[0],delta[1],delta[2]
        ); CHKERRQ(ierr);
        // Get vector of DOFs on vertex
        VectorAdaptor dofs = it->get()->getEntFieldData();
        // Set values
        dofs[0] = cblas_dnrm2(3,delta,1);
      }
      // Get values at edges and project those values on approximation base
      it = field_ents->lower_bound(
          FieldEntity::getLoBitNumberUId(field_bit_number));
      for(;it!=hi_it;it++) {
        EntityType type = it->get()->getEntType();
        if(type!=MBEDGE) {
          continue;
        }
        const EntityHandle *conn;
        int num_ndodes;
        EntityHandle ent = it->get()->getEnt();
        rval = mField.get_moab().get_connectivity(ent,conn,num_ndodes,true); CHKERRQ_MOAB(rval);
        double coords[3*num_ndodes];
        rval = mField.get_moab().get_coords(conn,num_ndodes,coords); CHKERRQ_MOAB(rval);
        cblas_daxpy(3,1,&coords[3],1,coords,1);
        cblas_dscal(3,0.5,coords,1);
        double delta[3];
        ierr = findClosestPointToTheSurface(
          coords[0],coords[1],coords[2],delta[0],delta[1],delta[2]
        ); CHKERRQ(ierr);
        
        auto conn_it0 = field_ents->find(
            FieldEntity::getLoLocalEntityBitNumber(field_bit_number, conn[0]));
        auto conn_it1 = field_ents->find(
            FieldEntity::getLoLocalEntityBitNumber(field_bit_number, conn[1]));
 
        if(conn_it0==field_ents->end()) {
          SETERRQ(PETSC_COMM_SELF,MOFEM_DATA_INCONSISTENCY,"entity not found");
        }
        if (conn_it1 == field_ents->end()) {
          SETERRQ(PETSC_COMM_SELF,MOFEM_DATA_INCONSISTENCY,"entity not found");
        }
 
        VectorAdaptor vec_conn0 = conn_it0->get()->getEntFieldData();
        VectorAdaptor vec_conn1 = conn_it1->get()->getEntFieldData();
        // cerr << vec_conn0 << " " << vec_conn1 << endl;
        double ave_delta = 0.5*(vec_conn0[0]+vec_conn1[0]);
        double edge_shape_function_val = 0.25;
        if(it->get()->getApproxBase()==AINSWORTH_LEGENDRE_BASE) {
        } else if(it->get()->getApproxBase()==AINSWORTH_LOBATTO_BASE) {
          edge_shape_function_val *= LOBATTO_PHI0(0);
        } else {
          SETERRQ(PETSC_COMM_SELF,MOFEM_NOT_IMPLEMENTED,"Base not implemented");
        }
        // Get vector of DOFs on vertex
        VectorAdaptor dofs = it->get()->getEntFieldData();
        // cerr << mid_val_dx << " " << ave_dx << endl;
        // Set values
        dofs[0] = (cblas_dnrm2(3,delta,1)-ave_delta)/edge_shape_function_val;
      }
      PetscFunctionReturn(0);
    }
 
  };
 
  #ifdef WITH_METAIO
 
  /** \brief Load data from MetaImage file, translate grayscale values to densities.
   * \ingroup bone_remodelling
   */
  struct DataFromMetaIO {
 
    MetaImage &metaFile;
    DataFromMetaIO(MetaImage &metaFile):
    metaFile(metaFile) {
    }
 
 
    double sIgma;
    double cUbe_size;
    int fIlter;
    double sCale;
    double dIst_tol;
    double tHreshold;
    double paramElastic[2];
    double paramDensity[2];
    double lAmbda;
 
    PetscErrorCode fromOptions() {
      PetscFunctionBegin;
      ierr = PetscOptionsBegin(PETSC_COMM_WORLD,"","Get parameters for DataFromMetaIO","none"); CHKERRQ(ierr);
 
      PetscBool flg = PETSC_TRUE;
 
      sIgma = 1;
      ierr = PetscOptionsScalar(
        "-density_map_sigma",
        "get sigma for gauss weighted average","",
        sIgma,&sIgma,PETSC_NULL
      ); CHKERRQ(ierr);
 
      cUbe_size = 0;
      ierr = PetscOptionsScalar(
        "-cube_size",
        "get cube size for the averaging","",
        0,&cUbe_size,PETSC_NULL
      ); CHKERRQ(ierr);
 
      dIst_tol = 0;
      ierr = PetscOptionsScalar(
        "-dist_tol",
        "get distance tolerance from mesh surface","",
        0,&dIst_tol,PETSC_NULL
      ); CHKERRQ(ierr);
 
      sCale = 1;
      ierr = PetscOptionsScalar(
        "-scale",
        "scale the distance between voxels (eg. from mm to m)","",
        sCale,&sCale,PETSC_NULL
      ); CHKERRQ(ierr);
 
      tHreshold = -1024;
      ierr = PetscOptionsScalar(
        "-threshold",
        "minimum value of ct threshold","",
        tHreshold,&tHreshold,PETSC_NULL
      ); CHKERRQ(ierr);
 
      fIlter = 0;
      ierr = PetscOptionsInt(
        "-filter",
        "number of the filter","",
        fIlter,&fIlter,PETSC_NULL
      ); CHKERRQ(ierr);
 
      lAmbda = 1;
      ierr = PetscOptionsScalar(
        "-lambda",
        "get lambda coefficient for smoothing","",
        lAmbda,&lAmbda,PETSC_NULL
      ); CHKERRQ(ierr);
 
      paramElastic[0] = 0;
      paramElastic[1] = 1;
      int nmax = 2;
      ierr = PetscOptionsGetScalarArray(
        PETSC_NULL,"-param_elastic",paramElastic,&nmax,&flg
      ); CHKERRQ(ierr);
 
      paramDensity[0] = 1;
      paramDensity[1] = 0;
      int n_max = 2;
      ierr = PetscOptionsGetScalarArray(PETSC_NULL,"-param_density",paramDensity,&n_max,&flg
  ); CHKERRQ(ierr);
 
  ierr = PetscOptionsEnd(); CHKERRQ(ierr);
 
  PetscFunctionReturn(0);
}
 
    /**
    * returns vertors of data for given index of the point
    * @param  vec_idx reference vector of indices
    * @param  vec_data  reference vector of data (colors)
    * @return         error code
    */
    PetscErrorCode getDataForGiveIndex(
      const vector<int> &vec_idx,
      vector<double> &vec_data
    ) {
      PetscFunctionBegin;
      int size_of_vec_idx = vec_idx.size();
      vec_data.resize(size_of_vec_idx);
 
      int ii = 0;
      for(vector<int>::const_iterator vit = vec_idx.begin();vit!=vec_idx.end();vit++) {
        vec_data[ii] = metaFile.ElementData(*vit);
        // if (vec_data[ii] < 500) SETERRQ(PETSC_COMM_SELF,MOFEM_DATA_INCONSISTENCY,"wrong value (lower than 1000)");
        // cout << "INDEX "<< *vit << " Data " << vec_data[ii] << endl;
        ii++;
      } //cout << "-----------------------" << endl;
      PetscFunctionReturn(0);
    }
 
    /**
    * returns vertors of coordinates and indices for given coordinate
    * @param  x       coordinate of point x
    * @param  y       coordinate of point y
    * @param  z       coordinate of point z
    * @param  dx      dimension of the block in x direction
    * @param  dy      dimension of the block in y direction
    * @param  dz      dimension of the block in z direction
    * @param  vec_ix  reference vector of x coordinates
    * @param  vec_iy  reference vector of y coordinates
    * @param  vec_iz  reference vector of z coordinates
    * @param  vec_idx reference vector of indices
    * @return         error code
    */
    PetscErrorCode getInidcesForGivenCoordinate(
      const double x,const double y,const double z,     ///< center of the cube
      const double dx,const double dy,const double dz,  ///< size of the cube
      vector<int> &vec_ix,                    ///<   vectors of coord points inside cube
      vector<int> &vec_iy,                    ///<
      vector<int> &vec_iz,                    ///<
      vector<int> &vec_idx,                   ///<   indices of inside cube with dimensions dx dy dz
      vector<double> &vec_dist                ///<   vector of squared distances between center of the cube and points
    ) {
      PetscFunctionBegin;
 
      const double *elemet_size = metaFile.ElementSize();
      const int *dim_size = metaFile.DimSize();
 
      int nx = ceil(dx/ (elemet_size[0] * sCale));                   // number of points within given size of block - dx
      int ny = ceil(dy/ (elemet_size[1] * sCale));                   // number of points within given size of block - dy
      int nz = ceil(dz/ (elemet_size[2] * sCale));                   // number of points within given size of block - dz
 
      //const double *offset = metaFile.Offset();           // coords before translation (offset)
      double x_befor_offset = x;
      double y_befor_offset = y;
      double z_befor_offset = z;
      // double x_befor_offset = x - metaFile.Offset()[0];  // probably its not necessary
      // double y_befor_offset = y - metaFile.Offset()[1];
      // double z_befor_offset = z - metaFile.Offset()[2];
      // center of the cube
      int ix  = ceil(x_befor_offset/ (elemet_size[0] * sCale));     // original coords/indices (before multiplying by elem size)
      int iy  = ceil(y_befor_offset/ (elemet_size[1] * sCale));
      int iz  = ceil(z_befor_offset/ (elemet_size[2] * sCale));
 
      vec_ix.resize(2*nx);                                // setting sizes of the vectors equal to number of points
      vec_iy.resize(2*ny);
      vec_iz.resize(2*nz);
 
      int ii = 0;
      for(int i = 0;i<2*nx;i++) {                         // vectors of coordinates of points within given cube
        int idx = ix-nx+i;
        if(idx >= dim_size[0] || idx < 0) continue;       // erasing indices beyond the border
        vec_ix[ii++] = idx;
      }
      vec_ix.resize(ii);
      ii = 0;
      for(int i = 0;i<2*ny;i++) {
        int idx = iy-ny+i;
        if(idx >= dim_size[1] || idx < 0) continue;       // erasing indices beyond the border
        vec_iy[ii++] = idx;
      }
      vec_iy.resize(ii);
      ii = 0;
      for(int i = 0;i<2*nz;i++) {
        int idx = iz-nz+i;
        if(idx >= dim_size[2] || idx < 0) continue;       // erasing indices beyond the border
        vec_iz[ii++] = idx;
      }
      vec_iz.resize(ii);
 
 
      if (dx <= 0) {     //
        vec_ix.resize(1);
        vec_ix.push_back(round(x_befor_offset/ (elemet_size[0] * sCale)));
      }
      if (dy <= 0) {
        vec_iy.resize(1);
        vec_iy.push_back(round(y_befor_offset/ (elemet_size[1] * sCale)));
      }
      if (dz <= 0) {
        vec_iz.resize(1);
        vec_iz.push_back(round(z_befor_offset/ (elemet_size[2] * sCale)));
      }
 
      // vec_idx.resize(8*nx*ny*nz);
      // vec_dist.resize(8*nx*ny*nz);
      vec_idx.resize(vec_iz.size() * vec_ix.size() * vec_iy.size());
      vec_dist.resize(vec_iz.size() * vec_ix.size() * vec_iy.size());
      int dm_size0_dim_size1 = dim_size[0] * dim_size[1];
      // calculating the vector of global indices and vector of distances from center of the cube
 
      if (fIlter == 0) {
 
        // vector<double> dy2(vec_iy.size());
        // {
        //   int ii = 0;
        //   for(vector<int>::iterator it_iy = vec_iy.begin(); it_iy != vec_iy.end();it_iy++) {
        //     double dyy = (*it_iy*elemet_size[1] - y_befor_offset);
        //     dy2[ii++] =  dyy * dyy;
        //   }
        // }
        // vector<double> dz2(vec_iz.size());
        // {
        //   int ii = 0;
        //   for(vector<int>::iterator it_iz = vec_iz.begin(); it_iz != vec_iz.end();it_iz++) {
        //     double dzz = (*it_iz*elemet_size[2] - z_befor_offset);
        //     dz2[ii++] = dzz * dzz;
        //   }
        // }
 
        ii = 0;
        for(vector<int>::iterator it_iz = vec_iz.begin(); it_iz != vec_iz.end();it_iz++) {
          double a = (*it_iz) * dm_size0_dim_size1 ;
          double dz2 = (*it_iz*(elemet_size[2]*sCale) - z_befor_offset) * (*it_iz*(elemet_size[2]*sCale) - z_befor_offset);
          for(vector<int>::iterator it_iy = vec_iy.begin(); it_iy != vec_iy.end();it_iy++) {
            double b = (*it_iy) * dim_size[0];
            double dy2 = (*it_iy*(elemet_size[1]*sCale) - y_befor_offset) * (*it_iy*(elemet_size[1]*sCale) - y_befor_offset);
            for(vector<int>::iterator it_ix = vec_ix.begin(); it_ix != vec_ix.end();it_ix++) {
              vec_idx[ii] =  a + b + (*it_ix);
              vec_dist[ii] =  ( dz2 + dy2 + (*it_ix*(elemet_size[0]*sCale) - x_befor_offset)
               * (*it_ix*(elemet_size[0]*sCale) - x_befor_offset) );
              ii++;
              // cout << "Ix Iy Iz" << vec_idx[ii] << " " << (*it_ix) << " " << (*it_iy) << " " << (*it_iz) << endl;
              // cout << "Z " << *it_iz << "-" <<  vec_idx[ii] / (dim_size[0] * dim_size[1]) << " ";
              // cout << "Y " << *it_iy << "-" << (vec_idx[ii] - (*it_iz) * (dim_size[0] * dim_size[1])) / dim_size[0] << " ";
              // cout << "X " << *it_ix << "-" << vec_idx[ii] - (*it_iz) * dim_size[0] * dim_size[1] - (*it_iy) * dim_size[0] << " " << endl;
              // cout << "POINT "<< ii << " GLOBAL  " << vec_idx[ii] <<
              // " DATA "<< metaFile.ElementData(vec_idx[ii]) << " DISTANCE  " << vec_dist[ii] << endl;
 
 
              // if we require more indices than mhd file has ----> ERROR
              // if(vec_idx[ii] >= metaFile.Quantity()) {
              //   SETERRQ(PETSC_COMM_SELF,MOFEM_DATA_INCONSISTENCY,"wrong index of voxels (> Quantity)");
              // }
 
 
            }
          }
        }
 
      }
      else {
 
        ii = 0;
        for(vector<int>::iterator it_iz = vec_iz.begin(); it_iz != vec_iz.end();it_iz++) {
          double a = (*it_iz) * dm_size0_dim_size1 ;
          for(vector<int>::iterator it_iy = vec_iy.begin(); it_iy != vec_iy.end();it_iy++) {
            double b = (*it_iy) * dim_size[0];
            for(vector<int>::iterator it_ix = vec_ix.begin(); it_ix != vec_ix.end();it_ix++) {
              vec_idx[ii++] =  a + b + (*it_ix);
            }
          }
        }
      }
      // cout << ix << " " << " " << iy << " " << iz <<endl;
      //cout << "--------------------------------------------------------" << endl;
      PetscFunctionReturn(0);
    }
 
    /**
     * \brief Averages data within given vector
     *
     * @param  vector  data from metaFile
     * @return            averaged data
     */
    double getAverage(const vector<double> &vec_data) {
      PetscFunctionBegin;
      double data = 0;
      int i=0;
      for(vector<double>::const_iterator vec_it = vec_data.begin();vec_it!=vec_data.end();vec_it++) {
        if (*vec_it < tHreshold) continue;
        data += *vec_it;
        i++;
      }
      if (i == 0) i = 1;    // to avoid division by zero
      data /= i;
      //cout <<"data "<< data << " SIZE " << vec_data.size() << endl;
      return data;
    }
 
    /**
     * \brief Transform data to densities
     *
     * @param  a  array of parameters a,b for linear relation of grayscale - density, a * HU + b = density
     * @param  vec_data  data from metaFile
     * @param  vec_density  output vector with density
     * @return            vector of densities
     */
    PetscErrorCode mapDensityLinear(
      double *paramDensity,
      const vector<double> &vec_data,
      vector<double> &vec_density
    ) {
      PetscFunctionBegin;
      vec_density.resize(vec_data.size());
      int ii = 0;
      for(vector<double>::const_iterator vec_it = vec_data.begin();vec_it!=vec_data.end();vec_it++) {
        vec_density[ii++] = fmax(paramDensity[0] * (*vec_it) + paramDensity[1],0); //       // linear density mapping: param_a and param_b
        //cout << "density " << fmax(a * (*vec_it) + b,0) << endl;
        //cerr << "tHreshold :" << tHreshold << " fIlter: " << fIlter << " dist toler" << dIst_tol << endl;
      }
      PetscFunctionReturn(0);
    }
    PetscErrorCode mapDensityLinear(
      const vector<double> &vec_data,
      vector<double> &vec_density
    ) {
      PetscFunctionBegin;
      ierr = mapDensityLinear(paramDensity,vec_data,vec_density); CHKERRQ(ierr);
      PetscFunctionReturn(0);
    }
    /**
     * \brief Transform densities to Youngs modulus
     *
     * @param  a,b  parameter for relation of density - density, b * density ^ a = E
     * @param  rho   density
     * @return            youngs modulus
     */
     double getElasticMod(                               // function transforms density data to Young modulus
       double *param,                            ///< parameters a,b for density-elasticity relation
       double rho                                              ///< density
     ) {
       return param[1] * pow(rho , param[0]); //         ///< power density mapping: param_a and param_b
     }
 
     double getElasticMod(double rho) {
       return getElasticMod(paramElastic,rho); //
     }
     /** \brief function returns median of data from given vector
     *
     * @param  vec_density  vector with densities
     * @return   median value
    */
    double medianFilter(const vector<double> &vec_density) {
      vector<double> vec_copy;                                  ///< copy of the vector to allow sorting data
      vec_copy.resize(vec_density.size());
      int i = 0;
      for(vector<double>::const_iterator vec_it = vec_density.begin();vec_it!=vec_density.end();vec_it++){
        if (*vec_it < tHreshold) continue;                     ///< applying threshold
        vec_copy[i++] = *vec_it;
        //cout << *vec_it << "-";
      }
      vec_copy.resize(i);
 
      //cout << " " << endl;
      sort(vec_copy.begin(), vec_copy.end());                  ///< sorting vector
      //cout << " " << endl;
      if (vec_copy.size() % 2 == 0) {
        return (vec_copy[vec_copy.size() / 2 - 1] + vec_copy[vec_copy.size() / 2]) / 2.0;
      } else {
        return vec_copy[vec_copy.size() / 2];
      }
      return 0;
    }
 
    vector<double> kErnel;
 
    /**
     * \brief function smooths values in a given vector depending on distance from center (vec_dist)
     *
     */
    double mapGaussSmooth(
      const double sigma,                                         ///< sigma parameter for gausssian smoothing
      const vector<double> &vec_density,                          ///< vector with non-smoothed densities
      const vector<double> &vec_dist                              ///< vector with squared distances form center of the cubes
      //  vector<double> &vec_density_smooth                        ///< vector with the smoothed densities
    ) {
      //vec_density_smooth.resize(vec_density.size());
      kErnel.resize(vec_density.size());
      //calculating kernel
      int i = 0;
      double sum = 0;
      const double k = 2 * sigma*sigma;
      const double m = (1 / sqrt( M_PI * k));
      for(int ii = 0; ii < vec_dist.size(); ii++) {
        if (vec_density[ii] < tHreshold) continue;
        kErnel[i] = m * exp(- (vec_dist[ii]) / k); // distance is already squared
        sum += kErnel[i++];
        //cout <<"KERNEL"<< kernel[i] << endl;;
      }
      kErnel.resize(i);
      ///< normalisation of kernel and smooting values
      int ii = 0;
      double smooth_data = 0;
      for(vector<double>::const_iterator vec_itr = vec_density.begin();vec_itr!=vec_density.end();vec_itr++) {
        if (*vec_itr < tHreshold) continue;
        kErnel[ii] /= sum;
        //cout << kernel[ii] << endl;
        smooth_data += (*vec_itr) * kErnel[ii++];
      }
      if (sum == 0) return 0;    ///< avoid division by zero  (ad exception) this should not happen /FIXME
      return smooth_data;
    }
 
    double mapGaussSmooth(
      const vector<double> &vec_density,                          ///< vector with non-smoothed densities
      const vector<double> &vec_dist                              ///< vector with distances form center of the cubes
    ) {
      return mapGaussSmooth(sIgma,vec_density,vec_dist);
    }
 
  };
 
  #endif // WITH_METAIO
 
  struct DensityMapFe: public MoFEM::VolumeElementForcesAndSourcesCore {
    DensityMapFe(MoFEM::Interface &m_field):
    MoFEM::VolumeElementForcesAndSourcesCore(m_field) {}
    /** \brief Set integrate rule
    */
    int getRule(int order) { return 2*order+1; };              // getRule(int order)  2*oRder;
  };
 
  /** \brief Calculate volume of the model.
  * \ingroup bone_remodelling
  */
  struct OpVolumeCalculation: public MoFEM::VolumeElementForcesAndSourcesCore::UserDataOperator {
 
    Vec volumeVec;
    OpVolumeCalculation(const string &field_name,Vec volume_vec):
    MoFEM::VolumeElementForcesAndSourcesCore::UserDataOperator(field_name,UserDataOperator::OPROW),
    volumeVec(volume_vec) {
    }
 
 
    PetscErrorCode doWork(
      int row_side,EntityType row_type,DataForcesAndSourcesCore::EntData &row_data
    ) {
      PetscFunctionBegin;
 
      //do it only once, no need to repeat this for edges,faces or tets
      if(row_type != MBTET) PetscFunctionReturn(0);
      double vol = getVolume();
      ierr = VecSetValue(volumeVec,0,vol,ADD_VALUES); CHKERRQ(ierr);
 
      PetscFunctionReturn(0);
    }
 
  };
  /**
   * \brief Data structure for storing global and local matrix K and vector f.
   *
   \f[
   Kq=\mathbf{f}
   \f]
   *
   */
  struct CommonData {
 
    VectorDouble rhoAtGaussPts; ///< Values of density at integration Pts
    VectorDouble locF; ///< Local element rhs vector
    MatrixDouble locA; ///< Local element matrix
    MatrixDouble transLocA;
 
    Mat globA; ///< Global matrix
    Vec globF; ///< Global vector
 
    boost::shared_ptr<VectorDouble> distanceValue;
    boost::shared_ptr<MatrixDouble> distanceGradient;
 
  };
 
  /** \brief Assemble LHS matrix K.
  * \ingroup bone_remodelling
  *
  \f[
  K=\int_{V}\phi^{T}\phi\,dV
  \f]
  */
  struct OpCalculateLhs: public MoFEM::VolumeElementForcesAndSourcesCore::UserDataOperator {
 
    CommonData &commonData;
    DataFromMetaIO &dataFromMetaIo;
 
    OpCalculateLhs(
      const std::string &row_field,const std::string &col_field,CommonData &common_data,DataFromMetaIO &data_from_meta_io
    ):
    MoFEM::VolumeElementForcesAndSourcesCore::UserDataOperator(
      row_field,col_field,UserDataOperator::OPROWCOL
    ),
    commonData(common_data),
    dataFromMetaIo(data_from_meta_io) {
      sYmm = true;
    }
 
    PetscErrorCode doWork(
      int row_side,int col_side,
      EntityType row_type,EntityType col_type,
      DataForcesAndSourcesCore::EntData &row_data,
      DataForcesAndSourcesCore::EntData &col_data
    ) {
 
      PetscFunctionBegin;
 
      int nb_rows = row_data.getIndices().size(); // number of rows
      int nb_cols = col_data.getIndices().size(); // number of columns
      if(nb_rows==0) PetscFunctionReturn(0);
      if(nb_cols==0) PetscFunctionReturn(0);
 
      commonData.locA.resize(nb_rows,nb_cols,false);
      commonData.locA.clear();
      int nb_gauss_pts = row_data.getN().size1();
      for(int gg = 0;gg<nb_gauss_pts;gg++) {
        double vol = getVolume()*getGaussPts()(3,gg);
        VectorAdaptor row_base_function = row_data.getN(gg);
        VectorAdaptor col_base_function = col_data.getN(gg);
        commonData.locA += 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);
 
        //const double lambda = 1;
        commonData.locA += vol*dataFromMetaIo.lAmbda*dataFromMetaIo.sCale*dataFromMetaIo.sCale*prod(
        row_diff_base_function,trans(col_diff_base_function));
      }
      ierr = MatSetValues(
        commonData.globA,
        row_data.getIndices().size(),
        &row_data.getIndices()[0],
        col_data.getIndices().size(),
        &col_data.getIndices()[0],
        &commonData.locA(0,0),
        ADD_VALUES
      ); CHKERRQ(ierr);
 
      // Assemble off symmetric side
      if(row_side!=col_side || row_type!=col_type) {
        commonData.transLocA.resize(commonData.locA.size2(),commonData.locA.size1(),false);
        noalias(commonData.transLocA) = trans(commonData.locA);
        ierr = MatSetValues(
          commonData.globA,
          col_data.getIndices().size(),
          &col_data.getIndices()[0],
          row_data.getIndices().size(),
          &row_data.getIndices()[0],
          &commonData.transLocA(0,0),
          ADD_VALUES
        ); CHKERRQ(ierr);
      }
 
      PetscFunctionReturn(0);
    }
 
  };
 
  #ifdef WITH_METAIO
 
  /** \brief Assemble local vector containing density data.
  * \ingroup bone_remodelling
  */
  struct OpCalulatefRhoAtGaussPts: public MoFEM::VolumeElementForcesAndSourcesCore::UserDataOperator {
 
    CommonData &commonData;
    DataFromMetaIO &dataFromMetaIo;
    SurfaceKDTree &surfaceDistance;
 
    OpCalulatefRhoAtGaussPts(
      const string &row_field,
      CommonData &common_data,
      DataFromMetaIO &data_from_meta_io,
      SurfaceKDTree &surface_distance
    ):
    MoFEM::VolumeElementForcesAndSourcesCore::UserDataOperator(
      row_field,row_field,UserDataOperator::OPROW
    ),
    commonData(common_data),
    dataFromMetaIo(data_from_meta_io),
    surfaceDistance(surface_distance) {
    }
 
 
    PetscErrorCode doWork(
      int row_side,EntityType row_type,DataForcesAndSourcesCore::EntData &row_data
    ) {
      PetscFunctionBegin;
 
      if(row_type!=MBVERTEX) PetscFunctionReturn(0);
 
      int nb_rows = row_data.getIndices().size();
      if(nb_rows == 0) PetscFunctionReturn(0);
 
      ////////////////////////////////////////////////////////////////////////////////////////////
      vector<int> vecIx;                           ///< reference vectors of coordinates
      vector<int> vecIy;
      vector<int> vecIz;
      vector<int> vecIdx;                          ///< reference vector of indices
 
      vector<double> vecData;                      ///< reference vector of data (colors)
      vector<double> vecDensity;                   ///< vector of the densities
      vector<double> vecDist;                      ///< vector of reference distances of the points
      ///////////////////////////////////////////////////////////////////////////////////////////////////
 
      auto t0_dist = getFTensor0FromVec(*commonData.distanceValue);
      auto t1_grad = getFTensor1FromMat<3>(*commonData.distanceGradient);
      FTensor::Index<'i',3> i;
 
      FTensor::Tensor1<double*,3> t1_xyz(
        &getCoordsAtGaussPts()(0,0),
        &getCoordsAtGaussPts()(0,1),
        &getCoordsAtGaussPts()(0,2),
        3
      );
 
      FTensor::Tensor1<double,3> t1_dx(
        dataFromMetaIo.metaFile.ElementSize()[0] * dataFromMetaIo.sCale,
        dataFromMetaIo.metaFile.ElementSize()[1] * dataFromMetaIo.sCale,
        dataFromMetaIo.metaFile.ElementSize()[2] * dataFromMetaIo.sCale
      );
      t1_dx(i) *= dataFromMetaIo.cUbe_size;
      double cube_size = sqrt(t1_dx(i)*t1_dx(i))*dataFromMetaIo.dIst_tol;
 
      const int nb_gauss_pts = row_data.getN().size1();
      commonData.rhoAtGaussPts.resize(nb_gauss_pts,false);
      for(int gg = 0;gg<nb_gauss_pts;gg++) {
 
        if(t0_dist<cube_size) {
          const double nrm2_grad = sqrt(t1_grad(i)*t1_grad(i));
          FTensor::Tensor1<double,3> t1_delta;
          t1_delta(i) = t1_grad(i)/nrm2_grad;
          t1_xyz(i) -= (t0_dist-cube_size)*t1_delta(i);
        }
 
 
        // //double cube_size = sqrt(dx*dx+dy*dy+dz*dz)*dataFromMetaIo.dIst_tol; //FIXME
        // double xc_dm = (2 * dataFromMetaIo.cUbe_size - 1) * dataFromMetaIo.metaFile.ElementSize()[0];
        // double yc_dm = (2 * dataFromMetaIo.cUbe_size - 1) * dataFromMetaIo.metaFile.ElementSize()[1];
        // double zc_dm = (2 * dataFromMetaIo.cUbe_size - 1) * dataFromMetaIo.metaFile.ElementSize()[2];
        // double cube_size = 0.5 * sqrt(xc_dm * xc_dm + yc_dm * yc_dm + zc_dm * zc_dm) * dataFromMetaIo.dIst_tol; // real cube size /2
        //
        // if(dist<cube_size) {
        //   // Shift cube from boundary
        //   x += (dist-cube_size)*delta_x/dist;
        //   y += (dist-cube_size)*delta_y/dist;
        //   z += (dist-cube_size)*delta_z/dist;
        // }
 
        //Calculate density like in OpMassCalculation%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        ierr = dataFromMetaIo.getInidcesForGivenCoordinate(
          t1_xyz(0),t1_xyz(1),t1_xyz(2),
          t1_dx(0),t1_dx(1),t1_dx(2),
          vecIx,vecIy,vecIz,vecIdx, vecDist
        ); CHKERRQ(ierr);
 
        ierr = dataFromMetaIo.getDataForGiveIndex(vecIdx,vecData); CHKERRQ(ierr);
        ierr = dataFromMetaIo.mapDensityLinear(vecData,vecDensity); CHKERRQ(ierr);
 
        double data = 0.0;
        switch (dataFromMetaIo.fIlter) {
          case 0:
          data = dataFromMetaIo.mapGaussSmooth(vecDensity,vecDist);
          break;
          case 1:
          data = dataFromMetaIo.getAverage(vecDensity);
          break;
          case 2:
          data = dataFromMetaIo.medianFilter(vecDensity);
 
          default:
          data = dataFromMetaIo.getAverage(vecDensity);
        }
 
        commonData.rhoAtGaussPts[gg] = data;//pow(x,2)*y; //data; // not multiply this by vol
 
        ++t1_xyz;
        ++t0_dist;
        ++t1_grad;
 
      }
 
      PetscFunctionReturn(0);
    }
 
 
  };
 
  /**
  * \brief Assemble RHS vector f.
  * \ingroup bone_remodelling
  *
  \f[
   \mathbf{f}_{e}=\int_{V_{e}}\phi q^{\ast}\,dV_{e}
   \f]
  */
  struct OpAssmbleRhs: public MoFEM::VolumeElementForcesAndSourcesCore::UserDataOperator {
 
    CommonData &commonData;
 
    OpAssmbleRhs(
      const string &row_field,
      CommonData &common_data
    ):
    MoFEM::VolumeElementForcesAndSourcesCore::UserDataOperator(
      row_field,row_field,UserDataOperator::OPROW
    ),
    commonData(common_data) {
    }
 
 
    PetscErrorCode doWork(
      int row_side,EntityType row_type,DataForcesAndSourcesCore::EntData &row_data
    ) {
      PetscFunctionBegin;
 
      int nb_rows = row_data.getIndices().size();
      if(nb_rows == 0) PetscFunctionReturn(0);
      commonData.locF.resize(nb_rows,false);
      commonData.locF.clear();
 
      int nb_gauss_pts = row_data.getN().size1();
      for(int gg = 0;gg<nb_gauss_pts;gg++) {
 
        double vol = getVolume()*getGaussPts()(3,gg);
        VectorAdaptor base_function = row_data.getN(gg);
        commonData.locF += vol*commonData.rhoAtGaussPts[gg]*base_function;
 
      }
 
      ierr = VecSetValues(
        commonData.globF,
        row_data.getIndices().size(),
        &row_data.getIndices()[0],
        &commonData.locF[0],
        ADD_VALUES
      ); CHKERRQ(ierr);
 
      PetscFunctionReturn(0);
    }
 
  };
 
  /**
  * \brief Calculate mass before approximation.
  * \ingroup bone_remodelling
  *
  \f[
  M=\int_{V}\rho\,dV
  \f]
  */
  struct OpMassCalculation: public MoFEM::VolumeElementForcesAndSourcesCore::UserDataOperator {
 
    Vec massVec;
    DataFromMetaIO &dataFromMetaIo;
 
    OpMassCalculation(
      const string &field_name,Vec mass_vec,DataFromMetaIO &data_from_meta_io
    ):
    MoFEM::VolumeElementForcesAndSourcesCore::UserDataOperator(field_name,UserDataOperator::OPROW),
    massVec(mass_vec),
    dataFromMetaIo(data_from_meta_io) {
    }
 
    vector<int> vecIx;                           ///< reference vectors of coordinates
    vector<int> vecIy;
    vector<int> vecIz;
    vector<int> vecIdx;                          ///< reference vector of indices
 
    vector<double> vecData;                      ///< reference vector of data (colors)
    vector<double> vecDensity;                   ///< vector of the densities
    vector<double> vecDist;                      ///< vector of reference distances of the points
 
 
    PetscErrorCode doWork(
      int row_side,EntityType row_type,DataForcesAndSourcesCore::EntData &row_data
    ) {
      PetscFunctionBegin;
 
      //do it only once, no need to repeat this for edges,faces or tets
      if(row_type != MBVERTEX) PetscFunctionReturn(0);
      //double dx, dy, dz;  dx = dy = dz = dataFromMetaIo.cUbe_size * dataFromMetaIo.metaFile.ElementSize()[0];
 
      double dx = dataFromMetaIo.cUbe_size * dataFromMetaIo.metaFile.ElementSize()[0] * dataFromMetaIo.sCale;
      double dy = dataFromMetaIo.cUbe_size * dataFromMetaIo.metaFile.ElementSize()[1] * dataFromMetaIo.sCale;
      double dz = dataFromMetaIo.cUbe_size * dataFromMetaIo.metaFile.ElementSize()[2] * dataFromMetaIo.sCale;
      //cout << " DX DY DZ " << dx << " " <<  dy << " " << dz << endl;
      int nb_gauss_pts = row_data.getN().size1();
      for(int gg = 0;gg<nb_gauss_pts;gg++) {
 
        double vol = getVolume()*getGaussPts()(3,gg);
        double x = getCoordsAtGaussPts()(gg,0);
        double y = getCoordsAtGaussPts()(gg,1);
        double z = getCoordsAtGaussPts()(gg,2);
        //cout <<"i: " << gg << " x " << x << " y " << y << " z " << z <<endl;
 
        ierr = dataFromMetaIo.getInidcesForGivenCoordinate(
          x,y,z,
          dx,dy,dz,
          vecIx,vecIy,vecIz,vecIdx, vecDist
        ); CHKERRQ(ierr);
 
        ierr = dataFromMetaIo.getDataForGiveIndex(vecIdx,vecData); CHKERRQ(ierr);
        ierr = dataFromMetaIo.mapDensityLinear(vecData,vecDensity); CHKERRQ(ierr);
 
        //double data = dataFromMetaIo.mapGaussSmooth(vecDensity,vecDist);
        //double data = dataFromMetaIo.medianFilter(vecDensity);
        //double data = dataFromMetaIo.getAverage(vecDensity);
 
        double data = 0.0;
        switch (dataFromMetaIo.fIlter)
        {
          case 0:
          data = dataFromMetaIo.mapGaussSmooth(vecDensity,vecDist);
          break;
          case 1:
          data = dataFromMetaIo.getAverage(vecDensity);
          break;
          case 2:
          data = dataFromMetaIo.medianFilter(vecDensity);
 
          default:
          data = dataFromMetaIo.getAverage(vecDensity);
        }
 
 
        double rho = data * vol;
        ierr = VecSetValue(massVec,0,rho,ADD_VALUES); CHKERRQ(ierr);
 
      }
 
      PetscFunctionReturn(0);
    }
 
  };
 
  /**
  * \brief Calculate mass after approximation.
  * \ingroup bone_remodelling
  *
  \f[
  M=\int_{V}\rho N\,dV
  \f]
  */
  struct OpMassCalculationFromApprox: public MoFEM::VolumeElementForcesAndSourcesCore::UserDataOperator {
 
    Vec massVec_approx;
 
 
    OpMassCalculationFromApprox(
      const string &field_name,Vec mass_vec_approx
    ):
    MoFEM::VolumeElementForcesAndSourcesCore::UserDataOperator(field_name,UserDataOperator::OPROW),
    massVec_approx(mass_vec_approx){
    }
 
 
    PetscErrorCode doWork(
      int row_side,EntityType row_type,DataForcesAndSourcesCore::EntData &row_data
    ) {
      PetscFunctionBegin;
 
      int nb_rows = row_data.getIndices().size();
      if(!nb_rows) {
        PetscFunctionReturn(0);
      }
 
      int nb_gauss_pts = row_data.getN().size1();
 
      for(int gg = 0;gg<nb_gauss_pts;gg++) {
 
        double vol = getVolume()*getGaussPts()(3,gg);
 
        VectorAdaptor shape_function = row_data.getN(gg,nb_rows);
        double rho = 0;
        rho = inner_prod(shape_function,row_data.getFieldData());
        //cout << rho << " " << shape_function << " " << row_data.getFieldData() << endl;
 
        double mass = rho * vol;
        ierr = VecSetValue(massVec_approx,0,mass,ADD_VALUES); CHKERRQ(ierr);
 
      }
      //cout << endl;
      PetscFunctionReturn(0);
    }
 
  };
  #endif // WITH_METAIO
 
 
}
 
#endif //__DENSITY_MAPS_HPP__
 
/***************************************************************************//**
* \defgroup bone_remodelling Bone remodeling
* \ingroup user_modules
*
* \brief Bone remodeling module
*
******************************************************************************/